Merge topic branch 'trace-debug' into integration-linux-ux500

7 files changed, 1261 insertions, 0 deletions

diff --git a/arch/arm/Kconfig.debug b/arch/arm/Kconfig.debug
index 85348a09d65..6ca82db8d21 100644
--- a/arch/arm/Kconfig.debug
+++ b/arch/arm/Kconfig.debug
@@ -318,6 +318,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 OC_ETM
 	bool "On-chip ETM and ETB"
 	depends on ARM_AMBA
diff --git a/arch/arm/common/Makefile b/arch/arm/common/Makefile
index 215816f1775..d5b5aa2fe3c 100644
--- a/arch/arm/common/Makefile
+++ b/arch/arm/common/Makefile
@@ -15,3 +15,4 @@ obj-$(CONFIG_ARCH_IXP2000)	+= uengine.o
 obj-$(CONFIG_ARCH_IXP23XX)	+= uengine.o
 obj-$(CONFIG_PCI_HOST_ITE8152)  += it8152.o
 obj-$(CONFIG_ARM_TIMER_SP804)	+= timer-sp.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/include/asm/setup.h b/arch/arm/include/asm/setup.h
index 23ebc0c82a3..ea1384fea05 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/mach-ux500/board-mop500-regulators.c b/arch/arm/mach-ux500/board-mop500-regulators.c
index ffa0d4f9cdc..44fbb68b15a 100644
--- a/arch/arm/mach-ux500/board-mop500-regulators.c
+++ b/arch/arm/mach-ux500/board-mop500-regulators.c
@@ -129,6 +129,7 @@ static struct regulator_consumer_supply ab8500_vaux2_consumers[] = {
 };
 
 static struct regulator_consumer_supply ab8500_vaux3_consumers[] = {
+	REGULATOR_SUPPLY("v-SD-STM", "stm"),
 	/* External MMC slot power */
 	REGULATOR_SUPPLY("vmmc", "sdi0"),
 };
diff --git a/arch/arm/mach-ux500/board-mop500-stm.c b/arch/arm/mach-ux500/board-mop500-stm.c
new file mode 100644
index 00000000000..1bef2a01873
--- /dev/null
+++ b/arch/arm/mach-ux500/board-mop500-stm.c
@@ -0,0 +1,447 @@
+/*
+ * Copyright (C) 2011 ST-Ericsson
+ *
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * Author: Olivier Germain <olivier.germain@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/platform_device.h>
+#include <linux/gpio.h>
+#include <linux/gpio/nomadik.h>
+#include <linux/mfd/dbx500-prcmu.h>
+#include <linux/regulator/consumer.h>
+
+#include <asm/mach-types.h>
+#include <plat/pincfg.h>
+#include <mach/devices.h>
+#include <asm/io.h>
+#include <trace/stm.h>
+#include "pins-db8500.h"
+
+#define HREFV60_SDMMC_EN_GPIO 169
+#define HREFV60_SDMMC_1V8_3V_GPIO 5
+
+#define U8520_SDMMC_EN_GPIO 78
+#define U8520_SDMMC_1V8_3V_GPIO 5
+
+#define STM_DEVICE (&u8500_stm_device.dev)
+#define STM_ERR(msg) dev_err(STM_DEVICE, msg)
+#define STM_WARN(msg) dev_warn(STM_DEVICE, msg)
+
+static struct regulator *regulator_aux3;
+static enum stm_connection_type
+	stm_current_connection = STM_STE_INVALID_CONNECTION;
+
+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_stm_ape_microsd_pins[] = {
+	GPIO23_MS_CLK  | PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP,
+	GPIO24_MS_BS   | PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP,
+	GPIO25_MS_DAT0 | PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP,
+	GPIO26_MS_DAT1 | PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP,
+	GPIO27_MS_DAT2 | PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP,
+	GPIO28_MS_DAT3 | PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP,
+};
+
+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 pin_cfg_t mop500_stm_modem_microsd_pins[] = {
+	GPIO18_GPIO        | PIN_OUTPUT_LOW,
+	GPIO19_GPIO        | PIN_OUTPUT_HIGH,
+	GPIO20_GPIO        | PIN_OUTPUT_HIGH,
+	GPIO22_GPIO        | PIN_INPUT_PULLUP,
+	GPIO23_STMMOD_CLK  | PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP,
+	GPIO24_UARTMOD_RXD | PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP,
+	GPIO25_STMMOD_DAT0 | PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP,
+	GPIO26_STMMOD_DAT1 | PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP,
+	GPIO27_STMMOD_DAT2 | PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP,
+	GPIO28_STMMOD_DAT3 | PIN_SLPM_USE_MUX_SETTINGS_IN_SLEEP,
+};
+
+/* sdi0 (removable MMC/SD/SDIO cards) */
+static pin_cfg_t mop500_sdi0_pins[] = {
+	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,
+};
+
+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)
+		STM_ERR("Failed to disable MIPI60\n");
+	else {
+		retval = nmk_config_pins(ARRAY_AND_SIZE(mop500_ske_pins));
+		if (retval)
+			STM_ERR("Failed to enable SKE gpio\n");
+	}
+	return retval;
+}
+
+static int stm_enable_ape_microsd(void)
+{
+	int retval;
+
+	/*
+	 * Configure STM APE on GPIO23,GPIO28,GPIO27,GPIO26,GPIO25
+	 * On HREF board an external SD buffer exist (ST6G3244ME)
+	 * to perform level conversion from 1.8v to 3.3V on SD card signals
+	 * When STM is redirected on micro SD connector GPIO18,GP19,GPIO20
+	 * are configured in standard GPIO mode and are used to configure
+	 * direction on external SD buffer ST6G3244ME.
+	 */
+
+	retval = nmk_config_pins(ARRAY_AND_SIZE(mop500_stm_ape_microsd_pins));
+	if (retval)
+		STM_ERR("Failed to enable STM APE on MICRO SD\n");
+
+	/* Enable altC1 on GPIO23-28 (STMAPE) */
+	prcmu_enable_stm_ape();
+
+	return retval;
+}
+
+static int stm_disable_ape_microsd(void)
+{
+	int retval;
+
+	/* Disable altC1 on GPIO23-28 (STMAPE) */
+	prcmu_disable_stm_ape();
+
+	/* Reconfigure GPIO for SD */
+	retval = nmk_config_pins_sleep(ARRAY_AND_SIZE(mop500_sdi0_pins));
+	if (retval)
+		STM_ERR("Failed to disable STM APE on MICRO SD "
+		"and to reconfigure GPIO for SD\n");
+
+	return retval;
+}
+
+static int stm_enable_modem_microsd(void)
+{
+	int retval;
+
+	/*
+	 * Configure STM APE on GPIO23,GPIO28,GPIO27,GPIO26,GPIO25
+	 * On HREF board an external SD buffer exist (ST6G3244ME)
+	 * to perform level conversion from 1.8v to 3.3V on SD card
+	 * signals. When STM is redirected on micro SD connector
+	 * GPIO18,GP19,GPIO20 are configured in standard GPIO mode
+	 * and are used to configure direction on external SD buffer
+	 * ST6G3244ME.
+	 */
+
+	retval = nmk_config_pins(ARRAY_AND_SIZE(mop500_stm_modem_microsd_pins));
+	if (retval)
+		STM_ERR("Failed to enable STM MODEM on MICRO SD\n");
+
+	return retval;
+}
+
+static int stm_disable_modem_microsd(void)
+{
+	int retval;
+
+	/* Reconfigure GPIO for SD */
+	retval = nmk_config_pins_sleep(ARRAY_AND_SIZE(mop500_sdi0_pins));
+	if (retval)
+		STM_ERR("Failed to disable STM MODEM on MICRO SD "
+		"and to reconfigure GPIO for SD\n");
+
+	return retval;
+}
+
+/* Enable or disable micro sd card buffers on HREF */
+static void control_level_shifter_for_microsd(int gpio_dir)
+{
+	int gpio[2];
+
+	if (machine_is_hrefv60() || machine_is_u9540()) {
+		gpio[0] = HREFV60_SDMMC_EN_GPIO;
+		gpio[1] = HREFV60_SDMMC_1V8_3V_GPIO;
+	} else if (machine_is_u8520()) {
+		gpio[0] = U8520_SDMMC_EN_GPIO;
+		gpio[1] = U8520_SDMMC_1V8_3V_GPIO;
+	} else	{
+		gpio[0] = MOP500_EGPIO(17);
+		gpio[1] = MOP500_EGPIO(18);
+	}
+
+	/* Select the default 2.9V and enable / disable level shifter */
+	gpio_direction_output(gpio[1], 0);
+	gpio_direction_output(gpio[0], gpio_dir);
+}
+
+/* Enable micro sd card buffers on HREF */
+static int enable_level_shifter_for_microsd(void)
+{
+	control_level_shifter_for_microsd(1);
+	STM_WARN("Level Shifter for SD card connector on.\n");
+	return 0;
+}
+
+/* Disable micro sd card buffers on HREF */
+static int disable_level_shifter_for_microsd(void)
+{
+	control_level_shifter_for_microsd(0);
+	STM_WARN("Level Shifter for SD card connector off.\n");
+	return 0;
+}
+
+/* Enable VAUX3 to power on buffer on STM MICRO SD cable */
+static int enable_vaux3_for_microsd_cable(void)
+{
+	int error;
+
+	regulator_aux3 = regulator_get(&u8500_stm_device.dev, "v-SD-STM");
+
+	if (IS_ERR(regulator_aux3)) {
+		error = PTR_ERR(regulator_aux3);
+		STM_ERR("Failed to get regulator, supply: v-SD-STM\n");
+		return error;
+	}
+
+	error = regulator_enable(regulator_aux3);
+
+	if (error) {
+		STM_ERR("Unable to enable regulator on SD card connector\n");
+		return error;
+	}
+
+	STM_WARN("Regulator on SD card connector power on.\n");
+	return error;
+}
+
+/* Disable VAUX3 to power off buffer on STM MICRO SD cable */
+static int disable_vaux3_for_microsd_cable(void)
+{
+	int error = 0;
+
+	error = regulator_disable(regulator_aux3);
+
+	if (regulator_aux3)
+		regulator_put(regulator_aux3);
+
+	STM_WARN("Regulator for stm on SD card connector power off.\n");
+
+	return error;
+
+}
+
+static int stm_ste_connection(enum stm_connection_type con_type)
+{
+	int retval = -EINVAL;
+
+	/* Check if connection type has been changed */
+	if (con_type == stm_current_connection)
+		return 0;
+
+	if (con_type != STM_DISCONNECT) {
+		/*  Always enable MIPI34 GPIO pins */
+		retval = nmk_config_pins(
+				ARRAY_AND_SIZE(mop500_stm_mipi34_pins));
+		if (retval) {
+			STM_ERR("Failed to enable MIPI34\n");
+			goto stm_ste_connection_error;
+		}
+	}
+
+	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) */
+		prcmu_enable_stm_mod_uart();
+		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) */
+		prcmu_disable_stm_mod_uart();
+		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) */
+		prcmu_enable_stm_mod_uart();
+		/* Enable APE on MIPI60 */
+		retval = nmk_config_pins_sleep(ARRAY_AND_SIZE(mop500_ske_pins));
+		if (retval)
+			STM_ERR("Failed to disable SKE GPIO\n");
+		else {
+			retval = nmk_config_pins(
+					ARRAY_AND_SIZE(mop500_stm_mipi60_pins));
+			if (retval)
+				STM_ERR("Failed to enable MIPI60\n");
+		}
+		break;
+
+	case STM_STE_MODEM_ON_MICROSD:
+		/* Disable APE on micro SD */
+		retval = stm_disable_ape_microsd();
+		/* Enable modem on micro SD */
+		if (!retval)
+			retval = stm_enable_modem_microsd();
+		/* Enable SD card buffer and regulator on href */
+		if (!retval && (stm_current_connection
+			!= STM_STE_APE_ON_MICROSD)) {
+			enable_level_shifter_for_microsd();
+			enable_vaux3_for_microsd_cable();
+		}
+		break;
+
+	case STM_STE_APE_ON_MICROSD:
+		/* Disable modem on micro SD */
+		retval = stm_disable_modem_microsd();
+		/* Enable ape on micro SD */
+		if (!retval)
+			retval = stm_enable_ape_microsd();
+		/* Enable SD card buffer and regulator on href */
+		if (!retval && (stm_current_connection
+			!= STM_STE_MODEM_ON_MICROSD)) {
+			enable_level_shifter_for_microsd();
+			enable_vaux3_for_microsd_cable();
+		}
+		break;
+
+	case STM_DISCONNECT:
+		retval = nmk_config_pins_sleep(
+				ARRAY_AND_SIZE(mop500_stm_mipi34_pins));
+		if (retval)
+			STM_ERR("Failed to disable MIPI34\n");
+
+		retval = stm_ste_disable_ape_on_mipi60();
+		if (retval)
+			STM_ERR("Failed to disable MIPI60\n");
+
+		retval = stm_disable_modem_microsd();
+		if (retval)
+			STM_ERR("Failed to disable modem on microsd\n");
+
+		retval = stm_disable_ape_microsd();
+		if (retval)
+			STM_ERR("Failed to disable ape on microsd\n");
+		break;
+
+	default:
+		STM_ERR("Bad connection type\n");
+		goto stm_ste_connection_error;
+	}
+
+	/* Disable power for microsd */
+	if ((stm_current_connection == STM_STE_MODEM_ON_MICROSD)
+	|| (stm_current_connection == STM_STE_APE_ON_MICROSD)) {
+		if ((con_type != STM_STE_MODEM_ON_MICROSD)
+		&& (con_type != STM_STE_APE_ON_MICROSD)) {
+			disable_vaux3_for_microsd_cable();
+			disable_level_shifter_for_microsd();
+		}
+	}
+
+	stm_current_connection = con_type;
+
+stm_ste_connection_error:
+	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 u8500_stm_device = {
+	.name = "stm",
+	.id = -1,
+	.dev = {
+		.platform_data = &stm_pdata,
+	},
+};
diff --git a/arch/arm/mach-ux500/hwreg.c b/arch/arm/mach-ux500/hwreg.c
new file mode 100644
index 00000000000..585f2a2b7be
--- /dev/null
+++ b/arch/arm/mach-ux500/hwreg.c
@@ -0,0 +1,736 @@
+/*
+ * 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 <linux/uaccess.h>
+#include <linux/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;
+};
+
+static struct hwreg_io_range *hwreg_io_current_map;
+
+/*
+ * HWREG guts: mapping table
+ */
+static struct hwreg_io_range hwreg_u8500_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 struct hwreg_io_range hwreg_u9540_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 = U9540_ASIC_ID_BASE,	.size = SZ_4K},
+
+	{.base = 0, .size = 0, },
+
+};
+
+static void hwreg_io_init(void)
+{
+	int i;
+
+	for (i = 0; hwreg_io_current_map[i].base; ++i) {
+		hwreg_io_current_map[i].addr =
+			ioremap(hwreg_io_current_map[i].base,
+				hwreg_io_current_map[i].size);
+		if (!hwreg_io_current_map[i].addr)
+			printk(KERN_WARNING
+				"%s: ioremap for %d (%08x) failed\n",
+				__func__, i, hwreg_io_current_map[i].base);
+	}
+}
+
+static void hwreg_io_exit(void)
+{
+	int i;
+
+	for (i = 0; hwreg_io_current_map[i].base; ++i)
+		if (hwreg_io_current_map[i].addr)
+			iounmap(hwreg_io_current_map[i].addr);
+}
+
+static void *hwreg_io_ptov(u32 phys)
+{
+	int i;
+
+	for (i = 0; hwreg_io_current_map[i].base; ++i) {
+		u32 base = hwreg_io_current_map[i].base;
+		u32 size = hwreg_io_current_map[i].size;
+		u8 *addr = hwreg_io_current_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_current_map[i].base; ++i) {
+		err = seq_printf(s, "%d: 0x%08X => 0x%08X\n",
+			i, hwreg_io_current_map[i].base,
+			hwreg_io_current_map[i].base +
+			hwreg_io_current_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;
+
+	if (cpu_is_u9540()) {
+		printk(KERN_INFO "hwreg: cpu is U9540\n");
+		hwreg_io_current_map = hwreg_u9540_io_map;
+	} else {
+		printk(KERN_INFO "hwreg: cpu is U8500\n");
+		hwreg_io_current_map = hwreg_u8500_io_map;
+	}
+
+	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");