857 files changed, 45756 insertions, 19097 deletions

diff --git a/drivers/acpi/acpica/Makefile b/drivers/acpi/acpica/Makefile
index 0a1b3435f92..7f1d40797e8 100644
--- a/drivers/acpi/acpica/Makefile
+++ b/drivers/acpi/acpica/Makefile
@@ -158,5 +158,6 @@ acpi-y +=		\
 	utresrc.o	\
 	utstate.o	\
 	utxface.o	\
+	utxfinit.o	\
 	utxferror.o	\
 	utxfmutex.o
diff --git a/drivers/acpi/acpica/achware.h b/drivers/acpi/acpica/achware.h
index 5de4ec72766..d902d31abc6 100644
--- a/drivers/acpi/acpica/achware.h
+++ b/drivers/acpi/acpica/achware.h
@@ -110,8 +110,7 @@ acpi_status acpi_hw_write_port(acpi_io_address address, u32 value, u32 width);
 /*
  * hwgpe - GPE support
  */
-u32 acpi_hw_get_gpe_register_bit(struct acpi_gpe_event_info *gpe_event_info,
-			     struct acpi_gpe_register_info *gpe_register_info);
+u32 acpi_hw_get_gpe_register_bit(struct acpi_gpe_event_info *gpe_event_info);
 
 acpi_status
 acpi_hw_low_set_gpe(struct acpi_gpe_event_info *gpe_event_info, u32 action);
diff --git a/drivers/acpi/acpica/aclocal.h b/drivers/acpi/acpica/aclocal.h
index cc80fe10e8e..c816ee67509 100644
--- a/drivers/acpi/acpica/aclocal.h
+++ b/drivers/acpi/acpica/aclocal.h
@@ -707,15 +707,18 @@ union acpi_parse_value {
 	u8                              disasm_opcode;  /* Subtype used for disassembly */\
 	char                            aml_op_name[16])	/* Op name (debug only) */
 
-#define ACPI_DASM_BUFFER                0x00
-#define ACPI_DASM_RESOURCE              0x01
-#define ACPI_DASM_STRING                0x02
-#define ACPI_DASM_UNICODE               0x03
-#define ACPI_DASM_EISAID                0x04
-#define ACPI_DASM_MATCHOP               0x05
-#define ACPI_DASM_LNOT_PREFIX           0x06
-#define ACPI_DASM_LNOT_SUFFIX           0x07
-#define ACPI_DASM_IGNORE                0x08
+/* Flags for disasm_flags field above */
+
+#define ACPI_DASM_BUFFER                0x00	/* Buffer is a simple data buffer */
+#define ACPI_DASM_RESOURCE              0x01	/* Buffer is a Resource Descriptor */
+#define ACPI_DASM_STRING                0x02	/* Buffer is a ASCII string */
+#define ACPI_DASM_UNICODE               0x03	/* Buffer is a Unicode string */
+#define ACPI_DASM_PLD_METHOD            0x04	/* Buffer is a _PLD method bit-packed buffer */
+#define ACPI_DASM_EISAID                0x05	/* Integer is an EISAID */
+#define ACPI_DASM_MATCHOP               0x06	/* Parent opcode is a Match() operator */
+#define ACPI_DASM_LNOT_PREFIX           0x07	/* Start of a Lnot_equal (etc.) pair of opcodes */
+#define ACPI_DASM_LNOT_SUFFIX           0x08	/* End  of a Lnot_equal (etc.) pair of opcodes */
+#define ACPI_DASM_IGNORE                0x09	/* Not used at this time */
 
 /*
  * Generic operation (for example:  If, While, Store)
@@ -932,6 +935,7 @@ struct acpi_bit_register_info {
 #define ACPI_OSI_WIN_VISTA_SP1          0x09
 #define ACPI_OSI_WIN_VISTA_SP2          0x0A
 #define ACPI_OSI_WIN_7                  0x0B
+#define ACPI_OSI_WIN_8                  0x0C
 
 #define ACPI_ALWAYS_ILLEGAL             0x00
 
@@ -1024,6 +1028,7 @@ struct acpi_port_info {
  ****************************************************************************/
 
 struct acpi_db_method_info {
+	acpi_handle method;
 	acpi_handle main_thread_gate;
 	acpi_handle thread_complete_gate;
 	acpi_thread_id *threads;
diff --git a/drivers/acpi/acpica/acmacros.h b/drivers/acpi/acpica/acmacros.h
index 832b6198652..a7f68c47f51 100644
--- a/drivers/acpi/acpica/acmacros.h
+++ b/drivers/acpi/acpica/acmacros.h
@@ -277,10 +277,33 @@
 
 /* Bitfields within ACPI registers */
 
-#define ACPI_REGISTER_PREPARE_BITS(val, pos, mask)      ((val << pos) & mask)
-#define ACPI_REGISTER_INSERT_VALUE(reg, pos, mask, val)  reg = (reg & (~(mask))) | ACPI_REGISTER_PREPARE_BITS(val, pos, mask)
+#define ACPI_REGISTER_PREPARE_BITS(val, pos, mask) \
+	((val << pos) & mask)
 
-#define ACPI_INSERT_BITS(target, mask, source)          target = ((target & (~(mask))) | (source & mask))
+#define ACPI_REGISTER_INSERT_VALUE(reg, pos, mask, val) \
+	reg = (reg & (~(mask))) | ACPI_REGISTER_PREPARE_BITS(val, pos, mask)
+
+#define ACPI_INSERT_BITS(target, mask, source) \
+	target = ((target & (~(mask))) | (source & mask))
+
+/* Generic bitfield macros and masks */
+
+#define ACPI_GET_BITS(source_ptr, position, mask) \
+	((*source_ptr >> position) & mask)
+
+#define ACPI_SET_BITS(target_ptr, position, mask, value) \
+	(*target_ptr |= ((value & mask) << position))
+
+#define ACPI_1BIT_MASK      0x00000001
+#define ACPI_2BIT_MASK      0x00000003
+#define ACPI_3BIT_MASK      0x00000007
+#define ACPI_4BIT_MASK      0x0000000F
+#define ACPI_5BIT_MASK      0x0000001F
+#define ACPI_6BIT_MASK      0x0000003F
+#define ACPI_7BIT_MASK      0x0000007F
+#define ACPI_8BIT_MASK      0x000000FF
+#define ACPI_16BIT_MASK     0x0000FFFF
+#define ACPI_24BIT_MASK     0x00FFFFFF
 
 /*
  * An object of type struct acpi_namespace_node can appear in some contexts
diff --git a/drivers/acpi/acpica/dswload.c b/drivers/acpi/acpica/dswload.c
index 552aa3a50c8..557510084c7 100644
--- a/drivers/acpi/acpica/dswload.c
+++ b/drivers/acpi/acpica/dswload.c
@@ -230,6 +230,20 @@ acpi_ds_load1_begin_op(struct acpi_walk_state * walk_state,
 			walk_state->scope_info->common.value = ACPI_TYPE_ANY;
 			break;
 
+		case ACPI_TYPE_METHOD:
+
+			/*
+			 * Allow scope change to root during execution of module-level
+			 * code. Root is typed METHOD during this time.
+			 */
+			if ((node == acpi_gbl_root_node) &&
+			    (walk_state->
+			     parse_flags & ACPI_PARSE_MODULE_LEVEL)) {
+				break;
+			}
+
+			/*lint -fallthrough */
+
 		default:
 
 			/* All other types are an error */
diff --git a/drivers/acpi/acpica/dswload2.c b/drivers/acpi/acpica/dswload2.c
index ae714772476..89c0114210c 100644
--- a/drivers/acpi/acpica/dswload2.c
+++ b/drivers/acpi/acpica/dswload2.c
@@ -230,6 +230,20 @@ acpi_ds_load2_begin_op(struct acpi_walk_state *walk_state,
 			walk_state->scope_info->common.value = ACPI_TYPE_ANY;
 			break;
 
+		case ACPI_TYPE_METHOD:
+
+			/*
+			 * Allow scope change to root during execution of module-level
+			 * code. Root is typed METHOD during this time.
+			 */
+			if ((node == acpi_gbl_root_node) &&
+			    (walk_state->
+			     parse_flags & ACPI_PARSE_MODULE_LEVEL)) {
+				break;
+			}
+
+			/*lint -fallthrough */
+
 		default:
 
 			/* All other types are an error */
diff --git a/drivers/acpi/acpica/evgpe.c b/drivers/acpi/acpica/evgpe.c
index afbd5cb391f..ef0193d74b5 100644
--- a/drivers/acpi/acpica/evgpe.c
+++ b/drivers/acpi/acpica/evgpe.c
@@ -80,8 +80,7 @@ acpi_ev_update_gpe_enable_mask(struct acpi_gpe_event_info *gpe_event_info)
 		return_ACPI_STATUS(AE_NOT_EXIST);
 	}
 
-	register_bit = acpi_hw_get_gpe_register_bit(gpe_event_info,
-						gpe_register_info);
+	register_bit = acpi_hw_get_gpe_register_bit(gpe_event_info);
 
 	/* Clear the run bit up front */
 
@@ -379,6 +378,18 @@ u32 acpi_ev_gpe_detect(struct acpi_gpe_xrupt_info * gpe_xrupt_list)
 			 */
 			if (!(gpe_register_info->enable_for_run |
 			      gpe_register_info->enable_for_wake)) {
+				ACPI_DEBUG_PRINT((ACPI_DB_INTERRUPTS,
+						  "Ignore disabled registers for GPE%02X-GPE%02X: "
+						  "RunEnable=%02X, WakeEnable=%02X\n",
+						  gpe_register_info->
+						  base_gpe_number,
+						  gpe_register_info->
+						  base_gpe_number +
+						  (ACPI_GPE_REGISTER_WIDTH - 1),
+						  gpe_register_info->
+						  enable_for_run,
+						  gpe_register_info->
+						  enable_for_wake));
 				continue;
 			}
 
@@ -401,9 +412,14 @@ u32 acpi_ev_gpe_detect(struct acpi_gpe_xrupt_info * gpe_xrupt_list)
 			}
 
 			ACPI_DEBUG_PRINT((ACPI_DB_INTERRUPTS,
-					  "Read GPE Register at GPE%02X: Status=%02X, Enable=%02X\n",
+					  "Read registers for GPE%02X-GPE%02X: Status=%02X, Enable=%02X, "
+					  "RunEnable=%02X, WakeEnable=%02X\n",
 					  gpe_register_info->base_gpe_number,
-					  status_reg, enable_reg));
+					  gpe_register_info->base_gpe_number +
+					  (ACPI_GPE_REGISTER_WIDTH - 1),
+					  status_reg, enable_reg,
+					  gpe_register_info->enable_for_run,
+					  gpe_register_info->enable_for_wake));
 
 			/* Check if there is anything active at all in this register */
 
diff --git a/drivers/acpi/acpica/evxfgpe.c b/drivers/acpi/acpica/evxfgpe.c
index 6affbdb4b88..87c5f233226 100644
--- a/drivers/acpi/acpica/evxfgpe.c
+++ b/drivers/acpi/acpica/evxfgpe.c
@@ -357,8 +357,7 @@ acpi_status acpi_set_gpe_wake_mask(acpi_handle gpe_device, u32 gpe_number, u8 ac
 		goto unlock_and_exit;
 	}
 
-	register_bit =
-	    acpi_hw_get_gpe_register_bit(gpe_event_info, gpe_register_info);
+	register_bit = acpi_hw_get_gpe_register_bit(gpe_event_info);
 
 	/* Perform the action */
 
diff --git a/drivers/acpi/acpica/hwgpe.c b/drivers/acpi/acpica/hwgpe.c
index 25bd28c4ae8..db4076580e2 100644
--- a/drivers/acpi/acpica/hwgpe.c
+++ b/drivers/acpi/acpica/hwgpe.c
@@ -60,7 +60,6 @@ acpi_hw_enable_wakeup_gpe_block(struct acpi_gpe_xrupt_info *gpe_xrupt_info,
  * FUNCTION:	acpi_hw_get_gpe_register_bit
  *
  * PARAMETERS:	gpe_event_info	    - Info block for the GPE
- *		gpe_register_info   - Info block for the GPE register
  *
  * RETURN:	Register mask with a one in the GPE bit position
  *
@@ -69,11 +68,10 @@ acpi_hw_enable_wakeup_gpe_block(struct acpi_gpe_xrupt_info *gpe_xrupt_info,
  *
  ******************************************************************************/
 
-u32 acpi_hw_get_gpe_register_bit(struct acpi_gpe_event_info *gpe_event_info,
-			     struct acpi_gpe_register_info *gpe_register_info)
+u32 acpi_hw_get_gpe_register_bit(struct acpi_gpe_event_info *gpe_event_info)
 {
 	return (u32)1 << (gpe_event_info->gpe_number -
-				gpe_register_info->base_gpe_number);
+		 gpe_event_info->register_info->base_gpe_number);
 }
 
 /******************************************************************************
@@ -115,8 +113,7 @@ acpi_hw_low_set_gpe(struct acpi_gpe_event_info *gpe_event_info, u32 action)
 
 	/* Set or clear just the bit that corresponds to this GPE */
 
-	register_bit = acpi_hw_get_gpe_register_bit(gpe_event_info,
-						gpe_register_info);
+	register_bit = acpi_hw_get_gpe_register_bit(gpe_event_info);
 	switch (action) {
 	case ACPI_GPE_CONDITIONAL_ENABLE:
 
@@ -178,8 +175,7 @@ acpi_status acpi_hw_clear_gpe(struct acpi_gpe_event_info * gpe_event_info)
 	 * Write a one to the appropriate bit in the status register to
 	 * clear this GPE.
 	 */
-	register_bit =
-	    acpi_hw_get_gpe_register_bit(gpe_event_info, gpe_register_info);
+	register_bit = acpi_hw_get_gpe_register_bit(gpe_event_info);
 
 	status = acpi_hw_write(register_bit,
 			       &gpe_register_info->status_address);
@@ -222,8 +218,7 @@ acpi_hw_get_gpe_status(struct acpi_gpe_event_info * gpe_event_info,
 
 	/* Get the register bitmask for this GPE */
 
-	register_bit = acpi_hw_get_gpe_register_bit(gpe_event_info,
-						gpe_register_info);
+	register_bit = acpi_hw_get_gpe_register_bit(gpe_event_info);
 
 	/* GPE currently enabled? (enabled for runtime?) */
 
diff --git a/drivers/acpi/acpica/hwxfsleep.c b/drivers/acpi/acpica/hwxfsleep.c
index 1f165a750ae..0ff1ecea5c3 100644
--- a/drivers/acpi/acpica/hwxfsleep.c
+++ b/drivers/acpi/acpica/hwxfsleep.c
@@ -381,7 +381,6 @@ ACPI_EXPORT_SYMBOL(acpi_enter_sleep_state)
  * FUNCTION:    acpi_leave_sleep_state_prep
  *
  * PARAMETERS:  sleep_state         - Which sleep state we are exiting
- *              flags               - ACPI_EXECUTE_BFS to run optional method
  *
  * RETURN:      Status
  *
diff --git a/drivers/acpi/acpica/nsdump.c b/drivers/acpi/acpica/nsdump.c
index 7ee4e6aeb0a..2526aaf945e 100644
--- a/drivers/acpi/acpica/nsdump.c
+++ b/drivers/acpi/acpica/nsdump.c
@@ -264,7 +264,7 @@ acpi_ns_dump_one_object(acpi_handle obj_handle,
 		switch (type) {
 		case ACPI_TYPE_PROCESSOR:
 
-			acpi_os_printf("ID %X Len %.4X Addr %p\n",
+			acpi_os_printf("ID %02X Len %02X Addr %p\n",
 				       obj_desc->processor.proc_id,
 				       obj_desc->processor.length,
 				       ACPI_CAST_PTR(void,
diff --git a/drivers/acpi/acpica/tbinstal.c b/drivers/acpi/acpica/tbinstal.c
index 74f97d74db1..70f9d787c82 100644
--- a/drivers/acpi/acpica/tbinstal.c
+++ b/drivers/acpi/acpica/tbinstal.c
@@ -350,6 +350,7 @@ struct acpi_table_header *acpi_tb_table_override(struct acpi_table_header
 acpi_status acpi_tb_resize_root_table_list(void)
 {
 	struct acpi_table_desc *tables;
+	u32 table_count;
 
 	ACPI_FUNCTION_TRACE(tb_resize_root_table_list);
 
@@ -363,8 +364,13 @@ acpi_status acpi_tb_resize_root_table_list(void)
 
 	/* Increase the Table Array size */
 
-	tables = ACPI_ALLOCATE_ZEROED(((acpi_size) acpi_gbl_root_table_list.
-				       max_table_count +
+	if (acpi_gbl_root_table_list.flags & ACPI_ROOT_ORIGIN_ALLOCATED) {
+		table_count = acpi_gbl_root_table_list.max_table_count;
+	} else {
+		table_count = acpi_gbl_root_table_list.current_table_count;
+	}
+
+	tables = ACPI_ALLOCATE_ZEROED(((acpi_size) table_count +
 				       ACPI_ROOT_TABLE_SIZE_INCREMENT) *
 				      sizeof(struct acpi_table_desc));
 	if (!tables) {
@@ -377,8 +383,8 @@ acpi_status acpi_tb_resize_root_table_list(void)
 
 	if (acpi_gbl_root_table_list.tables) {
 		ACPI_MEMCPY(tables, acpi_gbl_root_table_list.tables,
-			    (acpi_size) acpi_gbl_root_table_list.
-			    max_table_count * sizeof(struct acpi_table_desc));
+			    (acpi_size) table_count *
+			    sizeof(struct acpi_table_desc));
 
 		if (acpi_gbl_root_table_list.flags & ACPI_ROOT_ORIGIN_ALLOCATED) {
 			ACPI_FREE(acpi_gbl_root_table_list.tables);
@@ -386,9 +392,9 @@ acpi_status acpi_tb_resize_root_table_list(void)
 	}
 
 	acpi_gbl_root_table_list.tables = tables;
-	acpi_gbl_root_table_list.max_table_count +=
-	    ACPI_ROOT_TABLE_SIZE_INCREMENT;
-	acpi_gbl_root_table_list.flags |= (u8)ACPI_ROOT_ORIGIN_ALLOCATED;
+	acpi_gbl_root_table_list.max_table_count =
+	    table_count + ACPI_ROOT_TABLE_SIZE_INCREMENT;
+	acpi_gbl_root_table_list.flags |= ACPI_ROOT_ORIGIN_ALLOCATED;
 
 	return_ACPI_STATUS(AE_OK);
 }
diff --git a/drivers/acpi/acpica/tbxface.c b/drivers/acpi/acpica/tbxface.c
index 29e51bc0138..21101262e47 100644
--- a/drivers/acpi/acpica/tbxface.c
+++ b/drivers/acpi/acpica/tbxface.c
@@ -159,14 +159,12 @@ acpi_initialize_tables(struct acpi_table_desc * initial_table_array,
  * DESCRIPTION: Reallocate Root Table List into dynamic memory. Copies the
  *              root list from the previously provided scratch area. Should
  *              be called once dynamic memory allocation is available in the
- *              kernel
+ *              kernel.
  *
  ******************************************************************************/
 acpi_status acpi_reallocate_root_table(void)
 {
-	struct acpi_table_desc *tables;
-	acpi_size new_size;
-	acpi_size current_size;
+	acpi_status status;
 
 	ACPI_FUNCTION_TRACE(acpi_reallocate_root_table);
 
@@ -178,39 +176,10 @@ acpi_status acpi_reallocate_root_table(void)
 		return_ACPI_STATUS(AE_SUPPORT);
 	}
 
-	/*
-	 * Get the current size of the root table and add the default
-	 * increment to create the new table size.
-	 */
-	current_size = (acpi_size)
-	    acpi_gbl_root_table_list.current_table_count *
-	    sizeof(struct acpi_table_desc);
-
-	new_size = current_size +
-	    (ACPI_ROOT_TABLE_SIZE_INCREMENT * sizeof(struct acpi_table_desc));
-
-	/* Create new array and copy the old array */
-
-	tables = ACPI_ALLOCATE_ZEROED(new_size);
-	if (!tables) {
-		return_ACPI_STATUS(AE_NO_MEMORY);
-	}
+	acpi_gbl_root_table_list.flags |= ACPI_ROOT_ALLOW_RESIZE;
 
-	ACPI_MEMCPY(tables, acpi_gbl_root_table_list.tables, current_size);
-
-	/*
-	 * Update the root table descriptor. The new size will be the current
-	 * number of tables plus the increment, independent of the reserved
-	 * size of the original table list.
-	 */
-	acpi_gbl_root_table_list.tables = tables;
-	acpi_gbl_root_table_list.max_table_count =
-	    acpi_gbl_root_table_list.current_table_count +
-	    ACPI_ROOT_TABLE_SIZE_INCREMENT;
-	acpi_gbl_root_table_list.flags =
-	    ACPI_ROOT_ORIGIN_ALLOCATED | ACPI_ROOT_ALLOW_RESIZE;
-
-	return_ACPI_STATUS(AE_OK);
+	status = acpi_tb_resize_root_table_list();
+	return_ACPI_STATUS(status);
 }
 
 /*******************************************************************************
diff --git a/drivers/acpi/acpica/utosi.c b/drivers/acpi/acpica/utosi.c
index 34ef0bd7e4b..676285d6116 100644
--- a/drivers/acpi/acpica/utosi.c
+++ b/drivers/acpi/acpica/utosi.c
@@ -73,6 +73,7 @@ static struct acpi_interface_info acpi_default_supported_interfaces[] = {
 	{"Windows 2006 SP1", NULL, 0, ACPI_OSI_WIN_VISTA_SP1},	/* Windows Vista SP1 - Added 09/2009 */
 	{"Windows 2006 SP2", NULL, 0, ACPI_OSI_WIN_VISTA_SP2},	/* Windows Vista SP2 - Added 09/2010 */
 	{"Windows 2009", NULL, 0, ACPI_OSI_WIN_7},	/* Windows 7 and Server 2008 R2 - Added 09/2009 */
+	{"Windows 2012", NULL, 0, ACPI_OSI_WIN_8},	/* Windows 8 and Server 2012 - Added 08/2012 */
 
 	/* Feature Group Strings */
 
diff --git a/drivers/acpi/acpica/utxface.c b/drivers/acpi/acpica/utxface.c
index 534179f1177..b09632b4f5b 100644
--- a/drivers/acpi/acpica/utxface.c
+++ b/drivers/acpi/acpica/utxface.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Module Name: utxface - External interfaces for "global" ACPI functions
+ * Module Name: utxface - External interfaces, miscellaneous utility functions
  *
  *****************************************************************************/
 
@@ -53,271 +53,6 @@
 #define _COMPONENT          ACPI_UTILITIES
 ACPI_MODULE_NAME("utxface")
 
-#ifndef ACPI_ASL_COMPILER
-/*******************************************************************************
- *
- * FUNCTION:    acpi_initialize_subsystem
- *
- * PARAMETERS:  None
- *
- * RETURN:      Status
- *
- * DESCRIPTION: Initializes all global variables.  This is the first function
- *              called, so any early initialization belongs here.
- *
- ******************************************************************************/
-acpi_status __init acpi_initialize_subsystem(void)
-{
-	acpi_status status;
-
-	ACPI_FUNCTION_TRACE(acpi_initialize_subsystem);
-
-	acpi_gbl_startup_flags = ACPI_SUBSYSTEM_INITIALIZE;
-	ACPI_DEBUG_EXEC(acpi_ut_init_stack_ptr_trace());
-
-	/* Initialize the OS-Dependent layer */
-
-	status = acpi_os_initialize();
-	if (ACPI_FAILURE(status)) {
-		ACPI_EXCEPTION((AE_INFO, status, "During OSL initialization"));
-		return_ACPI_STATUS(status);
-	}
-
-	/* Initialize all globals used by the subsystem */
-
-	status = acpi_ut_init_globals();
-	if (ACPI_FAILURE(status)) {
-		ACPI_EXCEPTION((AE_INFO, status,
-				"During initialization of globals"));
-		return_ACPI_STATUS(status);
-	}
-
-	/* Create the default mutex objects */
-
-	status = acpi_ut_mutex_initialize();
-	if (ACPI_FAILURE(status)) {
-		ACPI_EXCEPTION((AE_INFO, status,
-				"During Global Mutex creation"));
-		return_ACPI_STATUS(status);
-	}
-
-	/*
-	 * Initialize the namespace manager and
-	 * the root of the namespace tree
-	 */
-	status = acpi_ns_root_initialize();
-	if (ACPI_FAILURE(status)) {
-		ACPI_EXCEPTION((AE_INFO, status,
-				"During Namespace initialization"));
-		return_ACPI_STATUS(status);
-	}
-
-	/* Initialize the global OSI interfaces list with the static names */
-
-	status = acpi_ut_initialize_interfaces();
-	if (ACPI_FAILURE(status)) {
-		ACPI_EXCEPTION((AE_INFO, status,
-				"During OSI interfaces initialization"));
-		return_ACPI_STATUS(status);
-	}
-
-	/* If configured, initialize the AML debugger */
-
-	ACPI_DEBUGGER_EXEC(status = acpi_db_initialize());
-	return_ACPI_STATUS(status);
-}
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_enable_subsystem
- *
- * PARAMETERS:  flags           - Init/enable Options
- *
- * RETURN:      Status
- *
- * DESCRIPTION: Completes the subsystem initialization including hardware.
- *              Puts system into ACPI mode if it isn't already.
- *
- ******************************************************************************/
-acpi_status acpi_enable_subsystem(u32 flags)
-{
-	acpi_status status = AE_OK;
-
-	ACPI_FUNCTION_TRACE(acpi_enable_subsystem);
-
-#if (!ACPI_REDUCED_HARDWARE)
-
-	/* Enable ACPI mode */
-
-	if (!(flags & ACPI_NO_ACPI_ENABLE)) {
-		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
-				  "[Init] Going into ACPI mode\n"));
-
-		acpi_gbl_original_mode = acpi_hw_get_mode();
-
-		status = acpi_enable();
-		if (ACPI_FAILURE(status)) {
-			ACPI_WARNING((AE_INFO, "AcpiEnable failed"));
-			return_ACPI_STATUS(status);
-		}
-	}
-
-	/*
-	 * Obtain a permanent mapping for the FACS. This is required for the
-	 * Global Lock and the Firmware Waking Vector
-	 */
-	status = acpi_tb_initialize_facs();
-	if (ACPI_FAILURE(status)) {
-		ACPI_WARNING((AE_INFO, "Could not map the FACS table"));
-		return_ACPI_STATUS(status);
-	}
-#endif				/* !ACPI_REDUCED_HARDWARE */
-
-	/*
-	 * Install the default op_region handlers. These are installed unless
-	 * other handlers have already been installed via the
-	 * install_address_space_handler interface.
-	 */
-	if (!(flags & ACPI_NO_ADDRESS_SPACE_INIT)) {
-		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
-				  "[Init] Installing default address space handlers\n"));
-
-		status = acpi_ev_install_region_handlers();
-		if (ACPI_FAILURE(status)) {
-			return_ACPI_STATUS(status);
-		}
-	}
-#if (!ACPI_REDUCED_HARDWARE)
-	/*
-	 * Initialize ACPI Event handling (Fixed and General Purpose)
-	 *
-	 * Note1: We must have the hardware and events initialized before we can
-	 * execute any control methods safely. Any control method can require
-	 * ACPI hardware support, so the hardware must be fully initialized before
-	 * any method execution!
-	 *
-	 * Note2: Fixed events are initialized and enabled here. GPEs are
-	 * initialized, but cannot be enabled until after the hardware is
-	 * completely initialized (SCI and global_lock activated)
-	 */
-	if (!(flags & ACPI_NO_EVENT_INIT)) {
-		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
-				  "[Init] Initializing ACPI events\n"));
-
-		status = acpi_ev_initialize_events();
-		if (ACPI_FAILURE(status)) {
-			return_ACPI_STATUS(status);
-		}
-	}
-
-	/*
-	 * Install the SCI handler and Global Lock handler. This completes the
-	 * hardware initialization.
-	 */
-	if (!(flags & ACPI_NO_HANDLER_INIT)) {
-		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
-				  "[Init] Installing SCI/GL handlers\n"));
-
-		status = acpi_ev_install_xrupt_handlers();
-		if (ACPI_FAILURE(status)) {
-			return_ACPI_STATUS(status);
-		}
-	}
-#endif				/* !ACPI_REDUCED_HARDWARE */
-
-	return_ACPI_STATUS(status);
-}
-
-ACPI_EXPORT_SYMBOL(acpi_enable_subsystem)
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_initialize_objects
- *
- * PARAMETERS:  flags           - Init/enable Options
- *
- * RETURN:      Status
- *
- * DESCRIPTION: Completes namespace initialization by initializing device
- *              objects and executing AML code for Regions, buffers, etc.
- *
- ******************************************************************************/
-acpi_status acpi_initialize_objects(u32 flags)
-{
-	acpi_status status = AE_OK;
-
-	ACPI_FUNCTION_TRACE(acpi_initialize_objects);
-
-	/*
-	 * Run all _REG methods
-	 *
-	 * Note: Any objects accessed by the _REG methods will be automatically
-	 * initialized, even if they contain executable AML (see the call to
-	 * acpi_ns_initialize_objects below).
-	 */
-	if (!(flags & ACPI_NO_ADDRESS_SPACE_INIT)) {
-		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
-				  "[Init] Executing _REG OpRegion methods\n"));
-
-		status = acpi_ev_initialize_op_regions();
-		if (ACPI_FAILURE(status)) {
-			return_ACPI_STATUS(status);
-		}
-	}
-
-	/*
-	 * Execute any module-level code that was detected during the table load
-	 * phase. Although illegal since ACPI 2.0, there are many machines that
-	 * contain this type of code. Each block of detected executable AML code
-	 * outside of any control method is wrapped with a temporary control
-	 * method object and placed on a global list. The methods on this list
-	 * are executed below.
-	 */
-	acpi_ns_exec_module_code_list();
-
-	/*
-	 * Initialize the objects that remain uninitialized. This runs the
-	 * executable AML that may be part of the declaration of these objects:
-	 * operation_regions, buffer_fields, Buffers, and Packages.
-	 */
-	if (!(flags & ACPI_NO_OBJECT_INIT)) {
-		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
-				  "[Init] Completing Initialization of ACPI Objects\n"));
-
-		status = acpi_ns_initialize_objects();
-		if (ACPI_FAILURE(status)) {
-			return_ACPI_STATUS(status);
-		}
-	}
-
-	/*
-	 * Initialize all device objects in the namespace. This runs the device
-	 * _STA and _INI methods.
-	 */
-	if (!(flags & ACPI_NO_DEVICE_INIT)) {
-		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
-				  "[Init] Initializing ACPI Devices\n"));
-
-		status = acpi_ns_initialize_devices();
-		if (ACPI_FAILURE(status)) {
-			return_ACPI_STATUS(status);
-		}
-	}
-
-	/*
-	 * Empty the caches (delete the cached objects) on the assumption that
-	 * the table load filled them up more than they will be at runtime --
-	 * thus wasting non-paged memory.
-	 */
-	status = acpi_purge_cached_objects();
-
-	acpi_gbl_startup_flags |= ACPI_INITIALIZED_OK;
-	return_ACPI_STATUS(status);
-}
-
-ACPI_EXPORT_SYMBOL(acpi_initialize_objects)
-
-#endif
 /*******************************************************************************
  *
  * FUNCTION:    acpi_terminate
@@ -683,3 +418,90 @@ acpi_check_address_range(acpi_adr_space_type space_id,
 
 ACPI_EXPORT_SYMBOL(acpi_check_address_range)
 #endif				/* !ACPI_ASL_COMPILER */
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_decode_pld_buffer
+ *
+ * PARAMETERS:  in_buffer           - Buffer returned by _PLD method
+ *              length              - Length of the in_buffer
+ *              return_buffer       - Where the decode buffer is returned
+ *
+ * RETURN:      Status and the decoded _PLD buffer. User must deallocate
+ *              the buffer via ACPI_FREE.
+ *
+ * DESCRIPTION: Decode the bit-packed buffer returned by the _PLD method into
+ *              a local struct that is much more useful to an ACPI driver.
+ *
+ ******************************************************************************/
+acpi_status
+acpi_decode_pld_buffer(u8 *in_buffer,
+		       acpi_size length, struct acpi_pld_info ** return_buffer)
+{
+	struct acpi_pld_info *pld_info;
+	u32 *buffer = ACPI_CAST_PTR(u32, in_buffer);
+	u32 dword;
+
+	/* Parameter validation */
+
+	if (!in_buffer || !return_buffer || (length < 16)) {
+		return (AE_BAD_PARAMETER);
+	}
+
+	pld_info = ACPI_ALLOCATE_ZEROED(sizeof(struct acpi_pld_info));
+	if (!pld_info) {
+		return (AE_NO_MEMORY);
+	}
+
+	/* First 32-bit DWord */
+
+	ACPI_MOVE_32_TO_32(&dword, &buffer[0]);
+	pld_info->revision = ACPI_PLD_GET_REVISION(&dword);
+	pld_info->ignore_color = ACPI_PLD_GET_IGNORE_COLOR(&dword);
+	pld_info->color = ACPI_PLD_GET_COLOR(&dword);
+
+	/* Second 32-bit DWord */
+
+	ACPI_MOVE_32_TO_32(&dword, &buffer[1]);
+	pld_info->width = ACPI_PLD_GET_WIDTH(&dword);
+	pld_info->height = ACPI_PLD_GET_HEIGHT(&dword);
+
+	/* Third 32-bit DWord */
+
+	ACPI_MOVE_32_TO_32(&dword, &buffer[2]);
+	pld_info->user_visible = ACPI_PLD_GET_USER_VISIBLE(&dword);
+	pld_info->dock = ACPI_PLD_GET_DOCK(&dword);
+	pld_info->lid = ACPI_PLD_GET_LID(&dword);
+	pld_info->panel = ACPI_PLD_GET_PANEL(&dword);
+	pld_info->vertical_position = ACPI_PLD_GET_VERTICAL(&dword);
+	pld_info->horizontal_position = ACPI_PLD_GET_HORIZONTAL(&dword);
+	pld_info->shape = ACPI_PLD_GET_SHAPE(&dword);
+	pld_info->group_orientation = ACPI_PLD_GET_ORIENTATION(&dword);
+	pld_info->group_token = ACPI_PLD_GET_TOKEN(&dword);
+	pld_info->group_position = ACPI_PLD_GET_POSITION(&dword);
+	pld_info->bay = ACPI_PLD_GET_BAY(&dword);
+
+	/* Fourth 32-bit DWord */
+
+	ACPI_MOVE_32_TO_32(&dword, &buffer[3]);
+	pld_info->ejectable = ACPI_PLD_GET_EJECTABLE(&dword);
+	pld_info->ospm_eject_required = ACPI_PLD_GET_OSPM_EJECT(&dword);
+	pld_info->cabinet_number = ACPI_PLD_GET_CABINET(&dword);
+	pld_info->card_cage_number = ACPI_PLD_GET_CARD_CAGE(&dword);
+	pld_info->reference = ACPI_PLD_GET_REFERENCE(&dword);
+	pld_info->rotation = ACPI_PLD_GET_ROTATION(&dword);
+	pld_info->order = ACPI_PLD_GET_ORDER(&dword);
+
+	if (length >= ACPI_PLD_BUFFER_SIZE) {
+
+		/* Fifth 32-bit DWord (Revision 2 of _PLD) */
+
+		ACPI_MOVE_32_TO_32(&dword, &buffer[4]);
+		pld_info->vertical_offset = ACPI_PLD_GET_VERT_OFFSET(&dword);
+		pld_info->horizontal_offset = ACPI_PLD_GET_HORIZ_OFFSET(&dword);
+	}
+
+	*return_buffer = pld_info;
+	return (AE_OK);
+}
+
+ACPI_EXPORT_SYMBOL(acpi_decode_pld_buffer)
diff --git a/drivers/acpi/acpica/utxfinit.c b/drivers/acpi/acpica/utxfinit.c
new file mode 100644
index 00000000000..14f523627a5
--- /dev/null
+++ b/drivers/acpi/acpica/utxfinit.c
@@ -0,0 +1,317 @@
+/******************************************************************************
+ *
+ * Module Name: utxfinit - External interfaces for ACPICA initialization
+ *
+ *****************************************************************************/
+
+/*
+ * Copyright (C) 2000 - 2012, Intel Corp.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. Redistributions in binary form must reproduce at minimum a disclaimer
+ *    substantially similar to the "NO WARRANTY" disclaimer below
+ *    ("Disclaimer") and any redistribution must be conditioned upon
+ *    including a substantially similar Disclaimer requirement for further
+ *    binary redistribution.
+ * 3. Neither the names of the above-listed copyright holders nor the names
+ *    of any contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") version 2 as published by the Free
+ * Software Foundation.
+ *
+ * NO WARRANTY
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * HOLDERS OR CONTRIBUTORS BE LIABLE FOR SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
+ * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGES.
+ */
+
+#include <linux/export.h>
+#include <acpi/acpi.h>
+#include "accommon.h"
+#include "acevents.h"
+#include "acnamesp.h"
+#include "acdebug.h"
+#include "actables.h"
+
+#define _COMPONENT          ACPI_UTILITIES
+ACPI_MODULE_NAME("utxfinit")
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_initialize_subsystem
+ *
+ * PARAMETERS:  None
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Initializes all global variables. This is the first function
+ *              called, so any early initialization belongs here.
+ *
+ ******************************************************************************/
+acpi_status acpi_initialize_subsystem(void)
+{
+	acpi_status status;
+
+	ACPI_FUNCTION_TRACE(acpi_initialize_subsystem);
+
+	acpi_gbl_startup_flags = ACPI_SUBSYSTEM_INITIALIZE;
+	ACPI_DEBUG_EXEC(acpi_ut_init_stack_ptr_trace());
+
+	/* Initialize the OS-Dependent layer */
+
+	status = acpi_os_initialize();
+	if (ACPI_FAILURE(status)) {
+		ACPI_EXCEPTION((AE_INFO, status, "During OSL initialization"));
+		return_ACPI_STATUS(status);
+	}
+
+	/* Initialize all globals used by the subsystem */
+
+	status = acpi_ut_init_globals();
+	if (ACPI_FAILURE(status)) {
+		ACPI_EXCEPTION((AE_INFO, status,
+				"During initialization of globals"));
+		return_ACPI_STATUS(status);
+	}
+
+	/* Create the default mutex objects */
+
+	status = acpi_ut_mutex_initialize();
+	if (ACPI_FAILURE(status)) {
+		ACPI_EXCEPTION((AE_INFO, status,
+				"During Global Mutex creation"));
+		return_ACPI_STATUS(status);
+	}
+
+	/*
+	 * Initialize the namespace manager and
+	 * the root of the namespace tree
+	 */
+	status = acpi_ns_root_initialize();
+	if (ACPI_FAILURE(status)) {
+		ACPI_EXCEPTION((AE_INFO, status,
+				"During Namespace initialization"));
+		return_ACPI_STATUS(status);
+	}
+
+	/* Initialize the global OSI interfaces list with the static names */
+
+	status = acpi_ut_initialize_interfaces();
+	if (ACPI_FAILURE(status)) {
+		ACPI_EXCEPTION((AE_INFO, status,
+				"During OSI interfaces initialization"));
+		return_ACPI_STATUS(status);
+	}
+
+	/* If configured, initialize the AML debugger */
+
+	ACPI_DEBUGGER_EXEC(status = acpi_db_initialize());
+	return_ACPI_STATUS(status);
+}
+ACPI_EXPORT_SYMBOL(acpi_initialize_subsystem)
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_enable_subsystem
+ *
+ * PARAMETERS:  flags               - Init/enable Options
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Completes the subsystem initialization including hardware.
+ *              Puts system into ACPI mode if it isn't already.
+ *
+ ******************************************************************************/
+acpi_status acpi_enable_subsystem(u32 flags)
+{
+	acpi_status status = AE_OK;
+
+	ACPI_FUNCTION_TRACE(acpi_enable_subsystem);
+
+#if (!ACPI_REDUCED_HARDWARE)
+
+	/* Enable ACPI mode */
+
+	if (!(flags & ACPI_NO_ACPI_ENABLE)) {
+		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
+				  "[Init] Going into ACPI mode\n"));
+
+		acpi_gbl_original_mode = acpi_hw_get_mode();
+
+		status = acpi_enable();
+		if (ACPI_FAILURE(status)) {
+			ACPI_WARNING((AE_INFO, "AcpiEnable failed"));
+			return_ACPI_STATUS(status);
+		}
+	}
+
+	/*
+	 * Obtain a permanent mapping for the FACS. This is required for the
+	 * Global Lock and the Firmware Waking Vector
+	 */
+	status = acpi_tb_initialize_facs();
+	if (ACPI_FAILURE(status)) {
+		ACPI_WARNING((AE_INFO, "Could not map the FACS table"));
+		return_ACPI_STATUS(status);
+	}
+#endif				/* !ACPI_REDUCED_HARDWARE */
+
+	/*
+	 * Install the default op_region handlers. These are installed unless
+	 * other handlers have already been installed via the
+	 * install_address_space_handler interface.
+	 */
+	if (!(flags & ACPI_NO_ADDRESS_SPACE_INIT)) {
+		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
+				  "[Init] Installing default address space handlers\n"));
+
+		status = acpi_ev_install_region_handlers();
+		if (ACPI_FAILURE(status)) {
+			return_ACPI_STATUS(status);
+		}
+	}
+#if (!ACPI_REDUCED_HARDWARE)
+	/*
+	 * Initialize ACPI Event handling (Fixed and General Purpose)
+	 *
+	 * Note1: We must have the hardware and events initialized before we can
+	 * execute any control methods safely. Any control method can require
+	 * ACPI hardware support, so the hardware must be fully initialized before
+	 * any method execution!
+	 *
+	 * Note2: Fixed events are initialized and enabled here. GPEs are
+	 * initialized, but cannot be enabled until after the hardware is
+	 * completely initialized (SCI and global_lock activated) and the various
+	 * initialization control methods are run (_REG, _STA, _INI) on the
+	 * entire namespace.
+	 */
+	if (!(flags & ACPI_NO_EVENT_INIT)) {
+		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
+				  "[Init] Initializing ACPI events\n"));
+
+		status = acpi_ev_initialize_events();
+		if (ACPI_FAILURE(status)) {
+			return_ACPI_STATUS(status);
+		}
+	}
+
+	/*
+	 * Install the SCI handler and Global Lock handler. This completes the
+	 * hardware initialization.
+	 */
+	if (!(flags & ACPI_NO_HANDLER_INIT)) {
+		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
+				  "[Init] Installing SCI/GL handlers\n"));
+
+		status = acpi_ev_install_xrupt_handlers();
+		if (ACPI_FAILURE(status)) {
+			return_ACPI_STATUS(status);
+		}
+	}
+#endif				/* !ACPI_REDUCED_HARDWARE */
+
+	return_ACPI_STATUS(status);
+}
+ACPI_EXPORT_SYMBOL(acpi_enable_subsystem)
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_initialize_objects
+ *
+ * PARAMETERS:  flags               - Init/enable Options
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Completes namespace initialization by initializing device
+ *              objects and executing AML code for Regions, buffers, etc.
+ *
+ ******************************************************************************/
+acpi_status acpi_initialize_objects(u32 flags)
+{
+	acpi_status status = AE_OK;
+
+	ACPI_FUNCTION_TRACE(acpi_initialize_objects);
+
+	/*
+	 * Run all _REG methods
+	 *
+	 * Note: Any objects accessed by the _REG methods will be automatically
+	 * initialized, even if they contain executable AML (see the call to
+	 * acpi_ns_initialize_objects below).
+	 */
+	if (!(flags & ACPI_NO_ADDRESS_SPACE_INIT)) {
+		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
+				  "[Init] Executing _REG OpRegion methods\n"));
+
+		status = acpi_ev_initialize_op_regions();
+		if (ACPI_FAILURE(status)) {
+			return_ACPI_STATUS(status);
+		}
+	}
+
+	/*
+	 * Execute any module-level code that was detected during the table load
+	 * phase. Although illegal since ACPI 2.0, there are many machines that
+	 * contain this type of code. Each block of detected executable AML code
+	 * outside of any control method is wrapped with a temporary control
+	 * method object and placed on a global list. The methods on this list
+	 * are executed below.
+	 */
+	acpi_ns_exec_module_code_list();
+
+	/*
+	 * Initialize the objects that remain uninitialized. This runs the
+	 * executable AML that may be part of the declaration of these objects:
+	 * operation_regions, buffer_fields, Buffers, and Packages.
+	 */
+	if (!(flags & ACPI_NO_OBJECT_INIT)) {
+		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
+				  "[Init] Completing Initialization of ACPI Objects\n"));
+
+		status = acpi_ns_initialize_objects();
+		if (ACPI_FAILURE(status)) {
+			return_ACPI_STATUS(status);
+		}
+	}
+
+	/*
+	 * Initialize all device objects in the namespace. This runs the device
+	 * _STA and _INI methods.
+	 */
+	if (!(flags & ACPI_NO_DEVICE_INIT)) {
+		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
+				  "[Init] Initializing ACPI Devices\n"));
+
+		status = acpi_ns_initialize_devices();
+		if (ACPI_FAILURE(status)) {
+			return_ACPI_STATUS(status);
+		}
+	}
+
+	/*
+	 * Empty the caches (delete the cached objects) on the assumption that
+	 * the table load filled them up more than they will be at runtime --
+	 * thus wasting non-paged memory.
+	 */
+	status = acpi_purge_cached_objects();
+
+	acpi_gbl_startup_flags |= ACPI_INITIALIZED_OK;
+	return_ACPI_STATUS(status);
+}
+ACPI_EXPORT_SYMBOL(acpi_initialize_objects)
diff --git a/drivers/acpi/bus.c b/drivers/acpi/bus.c
index e0596954290..d59175efc42 100644
--- a/drivers/acpi/bus.c
+++ b/drivers/acpi/bus.c
@@ -994,8 +994,6 @@ static int __init acpi_bus_init(void)
 	status = acpi_ec_ecdt_probe();
 	/* Ignore result. Not having an ECDT is not fatal. */
 
-	acpi_bus_osc_support();
-
 	status = acpi_initialize_objects(ACPI_FULL_INITIALIZATION);
 	if (ACPI_FAILURE(status)) {
 		printk(KERN_ERR PREFIX "Unable to initialize ACPI objects\n");
@@ -1003,6 +1001,12 @@ static int __init acpi_bus_init(void)
 	}
 
 	/*
+	 * _OSC method may exist in module level code,
+	 * so it must be run after ACPI_FULL_INITIALIZATION
+	 */
+	acpi_bus_osc_support();
+
+	/*
 	 * _PDC control method may load dynamic SSDT tables,
 	 * and we need to install the table handler before that.
 	 */
diff --git a/drivers/acpi/button.c b/drivers/acpi/button.c
index 314a3b84bbc..f0d936b65e3 100644
--- a/drivers/acpi/button.c
+++ b/drivers/acpi/button.c
@@ -450,15 +450,4 @@ static int acpi_button_remove(struct acpi_device *device, int type)
 	return 0;
 }
 
-static int __init acpi_button_init(void)
-{
-	return acpi_bus_register_driver(&acpi_button_driver);
-}
-
-static void __exit acpi_button_exit(void)
-{
-	acpi_bus_unregister_driver(&acpi_button_driver);
-}
-
-module_init(acpi_button_init);
-module_exit(acpi_button_exit);
+module_acpi_driver(acpi_button_driver);
diff --git a/drivers/acpi/ec.c b/drivers/acpi/ec.c
index 7edaccce664..a51df968131 100644
--- a/drivers/acpi/ec.c
+++ b/drivers/acpi/ec.c
@@ -71,9 +71,6 @@ enum ec_command {
 #define ACPI_EC_UDELAY_GLK	1000	/* Wait 1ms max. to get global lock */
 #define ACPI_EC_MSI_UDELAY	550	/* Wait 550us for MSI EC */
 
-#define ACPI_EC_STORM_THRESHOLD 8	/* number of false interrupts
-					   per one transaction */
-
 enum {
 	EC_FLAGS_QUERY_PENDING,		/* Query is pending */
 	EC_FLAGS_GPE_STORM,		/* GPE storm detected */
@@ -87,6 +84,15 @@ static unsigned int ec_delay __read_mostly = ACPI_EC_DELAY;
 module_param(ec_delay, uint, 0644);
 MODULE_PARM_DESC(ec_delay, "Timeout(ms) waited until an EC command completes");
 
+/*
+ * If the number of false interrupts per one transaction exceeds
+ * this threshold, will think there is a GPE storm happened and
+ * will disable the GPE for normal transaction.
+ */
+static unsigned int ec_storm_threshold  __read_mostly = 8;
+module_param(ec_storm_threshold, uint, 0644);
+MODULE_PARM_DESC(ec_storm_threshold, "Maxim false GPE numbers not considered as GPE storm");
+
 /* If we find an EC via the ECDT, we need to keep a ptr to its context */
 /* External interfaces use first EC only, so remember */
 typedef int (*acpi_ec_query_func) (void *data);
@@ -319,7 +325,7 @@ static int acpi_ec_transaction(struct acpi_ec *ec, struct transaction *t)
 		msleep(1);
 		/* It is safe to enable the GPE outside of the transaction. */
 		acpi_enable_gpe(NULL, ec->gpe);
-	} else if (t->irq_count > ACPI_EC_STORM_THRESHOLD) {
+	} else if (t->irq_count > ec_storm_threshold) {
 		pr_info(PREFIX "GPE storm detected, "
 			"transactions will use polling mode\n");
 		set_bit(EC_FLAGS_GPE_STORM, &ec->flags);
@@ -924,6 +930,17 @@ static int ec_flag_msi(const struct dmi_system_id *id)
 	return 0;
 }
 
+/*
+ * Clevo M720 notebook actually works ok with IRQ mode, if we lifted
+ * the GPE storm threshold back to 20
+ */
+static int ec_enlarge_storm_threshold(const struct dmi_system_id *id)
+{
+	pr_debug("Setting the EC GPE storm threshold to 20\n");
+	ec_storm_threshold  = 20;
+	return 0;
+}
+
 static struct dmi_system_id __initdata ec_dmi_table[] = {
 	{
 	ec_skip_dsdt_scan, "Compal JFL92", {
@@ -955,10 +972,13 @@ static struct dmi_system_id __initdata ec_dmi_table[] = {
 	{
 	ec_validate_ecdt, "ASUS hardware", {
 	DMI_MATCH(DMI_BOARD_VENDOR, "ASUSTeK Computer Inc.") }, NULL},
+	{
+	ec_enlarge_storm_threshold, "CLEVO hardware", {
+	DMI_MATCH(DMI_SYS_VENDOR, "CLEVO Co."),
+	DMI_MATCH(DMI_PRODUCT_NAME, "M720T/M730T"),}, NULL},
 	{},
 };
 
-
 int __init acpi_ec_ecdt_probe(void)
 {
 	acpi_status status;
diff --git a/drivers/acpi/fan.c b/drivers/acpi/fan.c
index bc36a476f1a..3bd6a54702d 100644
--- a/drivers/acpi/fan.c
+++ b/drivers/acpi/fan.c
@@ -212,24 +212,4 @@ static int acpi_fan_resume(struct device *dev)
 }
 #endif
 
-static int __init acpi_fan_init(void)
-{
-	int result = 0;
-
-	result = acpi_bus_register_driver(&acpi_fan_driver);
-	if (result < 0)
-		return -ENODEV;
-
-	return 0;
-}
-
-static void __exit acpi_fan_exit(void)
-{
-
-	acpi_bus_unregister_driver(&acpi_fan_driver);
-
-	return;
-}
-
-module_init(acpi_fan_init);
-module_exit(acpi_fan_exit);
+module_acpi_driver(acpi_fan_driver);
diff --git a/drivers/acpi/glue.c b/drivers/acpi/glue.c
index 243ee85e4d2..d1a2d74033e 100644
--- a/drivers/acpi/glue.c
+++ b/drivers/acpi/glue.c
@@ -25,6 +25,8 @@
 static LIST_HEAD(bus_type_list);
 static DECLARE_RWSEM(bus_type_sem);
 
+#define PHYSICAL_NODE_STRING "physical_node"
+
 int register_acpi_bus_type(struct acpi_bus_type *type)
 {
 	if (acpi_disabled)
@@ -124,84 +126,119 @@ acpi_handle acpi_get_child(acpi_handle parent, u64 address)
 
 EXPORT_SYMBOL(acpi_get_child);
 
-/* Link ACPI devices with physical devices */
-static void acpi_glue_data_handler(acpi_handle handle,
-				   void *context)
-{
-	/* we provide an empty handler */
-}
-
-/* Note: a success call will increase reference count by one */
-struct device *acpi_get_physical_device(acpi_handle handle)
-{
-	acpi_status status;
-	struct device *dev;
-
-	status = acpi_get_data(handle, acpi_glue_data_handler, (void **)&dev);
-	if (ACPI_SUCCESS(status))
-		return get_device(dev);
-	return NULL;
-}
-
-EXPORT_SYMBOL(acpi_get_physical_device);
-
 static int acpi_bind_one(struct device *dev, acpi_handle handle)
 {
 	struct acpi_device *acpi_dev;
 	acpi_status status;
+	struct acpi_device_physical_node *physical_node;
+	char physical_node_name[sizeof(PHYSICAL_NODE_STRING) + 2];
+	int retval = -EINVAL;
 
 	if (dev->archdata.acpi_handle) {
 		dev_warn(dev, "Drivers changed 'acpi_handle'\n");
 		return -EINVAL;
 	}
+
 	get_device(dev);
-	status = acpi_attach_data(handle, acpi_glue_data_handler, dev);
-	if (ACPI_FAILURE(status)) {
-		put_device(dev);
-		return -EINVAL;
+	status = acpi_bus_get_device(handle, &acpi_dev);
+	if (ACPI_FAILURE(status))
+		goto err;
+
+	physical_node = kzalloc(sizeof(struct acpi_device_physical_node),
+		GFP_KERNEL);
+	if (!physical_node) {
+		retval = -ENOMEM;
+		goto err;
 	}
-	dev->archdata.acpi_handle = handle;
 
-	status = acpi_bus_get_device(handle, &acpi_dev);
-	if (!ACPI_FAILURE(status)) {
-		int ret;
-
-		ret = sysfs_create_link(&dev->kobj, &acpi_dev->dev.kobj,
-				"firmware_node");
-		ret = sysfs_create_link(&acpi_dev->dev.kobj, &dev->kobj,
-				"physical_node");
-		if (acpi_dev->wakeup.flags.valid)
-			device_set_wakeup_capable(dev, true);
+	mutex_lock(&acpi_dev->physical_node_lock);
+	/* allocate physical node id according to physical_node_id_bitmap */
+	physical_node->node_id =
+		find_first_zero_bit(acpi_dev->physical_node_id_bitmap,
+		ACPI_MAX_PHYSICAL_NODE);
+	if (physical_node->node_id >= ACPI_MAX_PHYSICAL_NODE) {
+		retval = -ENOSPC;
+		mutex_unlock(&acpi_dev->physical_node_lock);
+		goto err;
 	}
 
+	set_bit(physical_node->node_id, acpi_dev->physical_node_id_bitmap);
+	physical_node->dev = dev;
+	list_add_tail(&physical_node->node, &acpi_dev->physical_node_list);
+	acpi_dev->physical_node_count++;
+	mutex_unlock(&acpi_dev->physical_node_lock);
+
+	dev->archdata.acpi_handle = handle;
+
+	if (!physical_node->node_id)
+		strcpy(physical_node_name, PHYSICAL_NODE_STRING);
+	else
+		sprintf(physical_node_name,
+			"physical_node%d", physical_node->node_id);
+	retval = sysfs_create_link(&acpi_dev->dev.kobj, &dev->kobj,
+			physical_node_name);
+	retval = sysfs_create_link(&dev->kobj, &acpi_dev->dev.kobj,
+		"firmware_node");
+
+	if (acpi_dev->wakeup.flags.valid)
+		device_set_wakeup_capable(dev, true);
+
 	return 0;
+
+ err:
+	put_device(dev);
+	return retval;
 }
 
 static int acpi_unbind_one(struct device *dev)
 {
+	struct acpi_device_physical_node *entry;
+	struct acpi_device *acpi_dev;
+	acpi_status status;
+	struct list_head *node, *next;
+
 	if (!dev->archdata.acpi_handle)
 		return 0;
-	if (dev == acpi_get_physical_device(dev->archdata.acpi_handle)) {
-		struct acpi_device *acpi_dev;
 
-		/* acpi_get_physical_device increase refcnt by one */
-		put_device(dev);
+	status = acpi_bus_get_device(dev->archdata.acpi_handle,
+		&acpi_dev);
+	if (ACPI_FAILURE(status))
+		goto err;
 
-		if (!acpi_bus_get_device(dev->archdata.acpi_handle,
-					&acpi_dev)) {
-			sysfs_remove_link(&dev->kobj, "firmware_node");
-			sysfs_remove_link(&acpi_dev->dev.kobj, "physical_node");
-		}
+	mutex_lock(&acpi_dev->physical_node_lock);
+	list_for_each_safe(node, next, &acpi_dev->physical_node_list) {
+		char physical_node_name[sizeof(PHYSICAL_NODE_STRING) + 2];
+
+		entry = list_entry(node, struct acpi_device_physical_node,
+			node);
+		if (entry->dev != dev)
+			continue;
+
+		list_del(node);
+		clear_bit(entry->node_id, acpi_dev->physical_node_id_bitmap);
 
-		acpi_detach_data(dev->archdata.acpi_handle,
-				 acpi_glue_data_handler);
+		acpi_dev->physical_node_count--;
+
+		if (!entry->node_id)
+			strcpy(physical_node_name, PHYSICAL_NODE_STRING);
+		else
+			sprintf(physical_node_name,
+				"physical_node%d", entry->node_id);
+
+		sysfs_remove_link(&acpi_dev->dev.kobj, physical_node_name);
+		sysfs_remove_link(&dev->kobj, "firmware_node");
 		dev->archdata.acpi_handle = NULL;
 		/* acpi_bind_one increase refcnt by one */
 		put_device(dev);
-	} else {
-		dev_err(dev, "Oops, 'acpi_handle' corrupt\n");
+		kfree(entry);
 	}
+	mutex_unlock(&acpi_dev->physical_node_lock);
+
 	return 0;
+
+err:
+	dev_err(dev, "Oops, 'acpi_handle' corrupt\n");
+	return -EINVAL;
 }
 
 static int acpi_platform_notify(struct device *dev)
diff --git a/drivers/acpi/hed.c b/drivers/acpi/hed.c
index d0c1967f759..20a0f2c3ca3 100644
--- a/drivers/acpi/hed.c
+++ b/drivers/acpi/hed.c
@@ -86,25 +86,7 @@ static struct acpi_driver acpi_hed_driver = {
 		.notify = acpi_hed_notify,
 	},
 };
-
-static int __init acpi_hed_init(void)
-{
-	if (acpi_disabled)
-		return -ENODEV;
-
-	if (acpi_bus_register_driver(&acpi_hed_driver) < 0)
-		return -ENODEV;
-
-	return 0;
-}
-
-static void __exit acpi_hed_exit(void)
-{
-	acpi_bus_unregister_driver(&acpi_hed_driver);
-}
-
-module_init(acpi_hed_init);
-module_exit(acpi_hed_exit);
+module_acpi_driver(acpi_hed_driver);
 
 ACPI_MODULE_NAME("hed");
 MODULE_AUTHOR("Huang Ying");
diff --git a/drivers/acpi/proc.c b/drivers/acpi/proc.c
index 251c7b6273a..27adb090bb3 100644
--- a/drivers/acpi/proc.c
+++ b/drivers/acpi/proc.c
@@ -302,26 +302,41 @@ acpi_system_wakeup_device_seq_show(struct seq_file *seq, void *offset)
 	list_for_each_safe(node, next, &acpi_wakeup_device_list) {
 		struct acpi_device *dev =
 		    container_of(node, struct acpi_device, wakeup_list);
-		struct device *ldev;
+		struct acpi_device_physical_node *entry;
 
 		if (!dev->wakeup.flags.valid)
 			continue;
 
-		ldev = acpi_get_physical_device(dev->handle);
-		seq_printf(seq, "%s\t  S%d\t%c%-8s  ",
+		seq_printf(seq, "%s\t  S%d\t",
 			   dev->pnp.bus_id,
-			   (u32) dev->wakeup.sleep_state,
-			   dev->wakeup.flags.run_wake ? '*' : ' ',
-			   (device_may_wakeup(&dev->dev)
-			     || (ldev && device_may_wakeup(ldev))) ?
-			       "enabled" : "disabled");
-		if (ldev)
-			seq_printf(seq, "%s:%s",
-				   ldev->bus ? ldev->bus->name : "no-bus",
-				   dev_name(ldev));
-		seq_printf(seq, "\n");
-		put_device(ldev);
-
+			   (u32) dev->wakeup.sleep_state);
+
+		if (!dev->physical_node_count)
+			seq_printf(seq, "%c%-8s\n",
+				dev->wakeup.flags.run_wake ?
+				'*' : ' ', "disabled");
+		else {
+			struct device *ldev;
+			list_for_each_entry(entry, &dev->physical_node_list,
+					node) {
+				ldev = get_device(entry->dev);
+				if (!ldev)
+					continue;
+
+				if (&entry->node !=
+						dev->physical_node_list.next)
+					seq_printf(seq, "\t\t");
+
+				seq_printf(seq, "%c%-8s  %s:%s\n",
+					dev->wakeup.flags.run_wake ? '*' : ' ',
+					(device_may_wakeup(&dev->dev) ||
+					(ldev && device_may_wakeup(ldev))) ?
+					"enabled" : "disabled",
+					ldev->bus ? ldev->bus->name :
+					"no-bus", dev_name(ldev));
+				put_device(ldev);
+			}
+		}
 	}
 	mutex_unlock(&acpi_device_lock);
 	return 0;
@@ -329,12 +344,14 @@ acpi_system_wakeup_device_seq_show(struct seq_file *seq, void *offset)
 
 static void physical_device_enable_wakeup(struct acpi_device *adev)
 {
-	struct device *dev = acpi_get_physical_device(adev->handle);
+	struct acpi_device_physical_node *entry;
 
-	if (dev && device_can_wakeup(dev)) {
-		bool enable = !device_may_wakeup(dev);
-		device_set_wakeup_enable(dev, enable);
-	}
+	list_for_each_entry(entry,
+		&adev->physical_node_list, node)
+		if (entry->dev && device_can_wakeup(entry->dev)) {
+			bool enable = !device_may_wakeup(entry->dev);
+			device_set_wakeup_enable(entry->dev, enable);
+		}
 }
 
 static ssize_t
diff --git a/drivers/acpi/processor_idle.c b/drivers/acpi/processor_idle.c
index 3655ab92381..e8086c72530 100644
--- a/drivers/acpi/processor_idle.c
+++ b/drivers/acpi/processor_idle.c
@@ -1132,7 +1132,7 @@ static int acpi_processor_setup_cpuidle_states(struct acpi_processor *pr)
 int acpi_processor_hotplug(struct acpi_processor *pr)
 {
 	int ret = 0;
-	struct cpuidle_device *dev = per_cpu(acpi_cpuidle_device, pr->id);
+	struct cpuidle_device *dev;
 
 	if (disabled_by_idle_boot_param())
 		return 0;
@@ -1147,6 +1147,7 @@ int acpi_processor_hotplug(struct acpi_processor *pr)
 	if (!pr->flags.power_setup_done)
 		return -ENODEV;
 
+	dev = per_cpu(acpi_cpuidle_device, pr->id);
 	cpuidle_pause_and_lock();
 	cpuidle_disable_device(dev);
 	acpi_processor_get_power_info(pr);
diff --git a/drivers/acpi/sbshc.c b/drivers/acpi/sbshc.c
index f8d2a472795..cf6129a8af7 100644
--- a/drivers/acpi/sbshc.c
+++ b/drivers/acpi/sbshc.c
@@ -310,23 +310,7 @@ static int acpi_smbus_hc_remove(struct acpi_device *device, int type)
 	return 0;
 }
 
-static int __init acpi_smb_hc_init(void)
-{
-	int result;
-
-	result = acpi_bus_register_driver(&acpi_smb_hc_driver);
-	if (result < 0)
-		return -ENODEV;
-	return 0;
-}
-
-static void __exit acpi_smb_hc_exit(void)
-{
-	acpi_bus_unregister_driver(&acpi_smb_hc_driver);
-}
-
-module_init(acpi_smb_hc_init);
-module_exit(acpi_smb_hc_exit);
+module_acpi_driver(acpi_smb_hc_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Alexey Starikovskiy");
diff --git a/drivers/acpi/scan.c b/drivers/acpi/scan.c
index d1ecca2b641..1fcb8678665 100644
--- a/drivers/acpi/scan.c
+++ b/drivers/acpi/scan.c
@@ -10,6 +10,7 @@
 #include <linux/signal.h>
 #include <linux/kthread.h>
 #include <linux/dmi.h>
+#include <linux/nls.h>
 
 #include <acpi/acpi_drivers.h>
 
@@ -232,8 +233,35 @@ end:
 }
 static DEVICE_ATTR(path, 0444, acpi_device_path_show, NULL);
 
+/* sysfs file that shows description text from the ACPI _STR method */
+static ssize_t description_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf) {
+	struct acpi_device *acpi_dev = to_acpi_device(dev);
+	int result;
+
+	if (acpi_dev->pnp.str_obj == NULL)
+		return 0;
+
+	/*
+	 * The _STR object contains a Unicode identifier for a device.
+	 * We need to convert to utf-8 so it can be displayed.
+	 */
+	result = utf16s_to_utf8s(
+		(wchar_t *)acpi_dev->pnp.str_obj->buffer.pointer,
+		acpi_dev->pnp.str_obj->buffer.length,
+		UTF16_LITTLE_ENDIAN, buf,
+		PAGE_SIZE);
+
+	buf[result++] = '\n';
+
+	return result;
+}
+static DEVICE_ATTR(description, 0444, description_show, NULL);
+
 static int acpi_device_setup_files(struct acpi_device *dev)
 {
+	struct acpi_buffer buffer = {ACPI_ALLOCATE_BUFFER, NULL};
 	acpi_status status;
 	acpi_handle temp;
 	int result = 0;
@@ -257,6 +285,21 @@ static int acpi_device_setup_files(struct acpi_device *dev)
 			goto end;
 	}
 
+	/*
+	 * If device has _STR, 'description' file is created
+	 */
+	status = acpi_get_handle(dev->handle, "_STR", &temp);
+	if (ACPI_SUCCESS(status)) {
+		status = acpi_evaluate_object(dev->handle, "_STR",
+					NULL, &buffer);
+		if (ACPI_FAILURE(status))
+			buffer.pointer = NULL;
+		dev->pnp.str_obj = buffer.pointer;
+		result = device_create_file(&dev->dev, &dev_attr_description);
+		if (result)
+			goto end;
+	}
+
         /*
          * If device has _EJ0, 'eject' file is created that is used to trigger
          * hot-removal function from userland.
@@ -274,8 +317,15 @@ static void acpi_device_remove_files(struct acpi_device *dev)
 	acpi_handle temp;
 
 	/*
-	 * If device has _EJ0, 'eject' file is created that is used to trigger
-	 * hot-removal function from userland.
+	 * If device has _STR, remove 'description' file
+	 */
+	status = acpi_get_handle(dev->handle, "_STR", &temp);
+	if (ACPI_SUCCESS(status)) {
+		kfree(dev->pnp.str_obj);
+		device_remove_file(&dev->dev, &dev_attr_description);
+	}
+	/*
+	 * If device has _EJ0, remove 'eject' file.
 	 */
 	status = acpi_get_handle(dev->handle, "_EJ0", &temp);
 	if (ACPI_SUCCESS(status))
@@ -481,6 +531,8 @@ static int acpi_device_register(struct acpi_device *device)
 	INIT_LIST_HEAD(&device->children);
 	INIT_LIST_HEAD(&device->node);
 	INIT_LIST_HEAD(&device->wakeup_list);
+	INIT_LIST_HEAD(&device->physical_node_list);
+	mutex_init(&device->physical_node_lock);
 
 	new_bus_id = kzalloc(sizeof(struct acpi_device_bus_id), GFP_KERNEL);
 	if (!new_bus_id) {
diff --git a/drivers/acpi/tables.c b/drivers/acpi/tables.c
index f336bca7c45..2572d9715bd 100644
--- a/drivers/acpi/tables.c
+++ b/drivers/acpi/tables.c
@@ -240,10 +240,17 @@ acpi_table_parse_entries(char *id,
 	       table_end) {
 		if (entry->type == entry_id
 		    && (!max_entries || count++ < max_entries))
-			if (handler(entry, table_end)) {
-				early_acpi_os_unmap_memory((char *)table_header, tbl_size);
-				return -EINVAL;
-			}
+			if (handler(entry, table_end))
+				goto err;
+
+		/*
+		 * If entry->length is 0, break from this loop to avoid
+		 * infinite loop.
+		 */
+		if (entry->length == 0) {
+			pr_err(PREFIX "[%4.4s:0x%02x] Invalid zero length\n", id, entry_id);
+			goto err;
+		}
 
 		entry = (struct acpi_subtable_header *)
 		    ((unsigned long)entry + entry->length);
@@ -255,6 +262,9 @@ acpi_table_parse_entries(char *id,
 
 	early_acpi_os_unmap_memory((char *)table_header, tbl_size);
 	return count;
+err:
+	early_acpi_os_unmap_memory((char *)table_header, tbl_size);
+	return -EINVAL;
 }
 
 int __init
diff --git a/drivers/acpi/thermal.c b/drivers/acpi/thermal.c
index edda74a4340..804204d4199 100644
--- a/drivers/acpi/thermal.c
+++ b/drivers/acpi/thermal.c
@@ -708,6 +708,40 @@ static int thermal_get_crit_temp(struct thermal_zone_device *thermal,
 		return -EINVAL;
 }
 
+static int thermal_get_trend(struct thermal_zone_device *thermal,
+				int trip, enum thermal_trend *trend)
+{
+	struct acpi_thermal *tz = thermal->devdata;
+	enum thermal_trip_type type;
+	int i;
+
+	if (thermal_get_trip_type(thermal, trip, &type))
+		return -EINVAL;
+
+	if (type == THERMAL_TRIP_ACTIVE) {
+		/* aggressive active cooling */
+		*trend = THERMAL_TREND_RAISING;
+		return 0;
+	}
+
+	/*
+	 * tz->temperature has already been updated by generic thermal layer,
+	 * before this callback being invoked
+	 */
+	i = (tz->trips.passive.tc1 * (tz->temperature - tz->last_temperature))
+		+ (tz->trips.passive.tc2
+		* (tz->temperature - tz->trips.passive.temperature));
+
+	if (i > 0)
+		*trend = THERMAL_TREND_RAISING;
+	else if (i < 0)
+		*trend = THERMAL_TREND_DROPPING;
+	else
+		*trend = THERMAL_TREND_STABLE;
+	return 0;
+}
+
+
 static int thermal_notify(struct thermal_zone_device *thermal, int trip,
 			   enum thermal_trip_type trip_type)
 {
@@ -731,11 +765,9 @@ static int thermal_notify(struct thermal_zone_device *thermal, int trip,
 	return 0;
 }
 
-typedef int (*cb)(struct thermal_zone_device *, int,
-		  struct thermal_cooling_device *);
 static int acpi_thermal_cooling_device_cb(struct thermal_zone_device *thermal,
 					struct thermal_cooling_device *cdev,
-					cb action)
+					bool bind)
 {
 	struct acpi_device *device = cdev->devdata;
 	struct acpi_thermal *tz = thermal->devdata;
@@ -759,11 +791,19 @@ static int acpi_thermal_cooling_device_cb(struct thermal_zone_device *thermal,
 		    i++) {
 			handle = tz->trips.passive.devices.handles[i];
 			status = acpi_bus_get_device(handle, &dev);
-			if (ACPI_SUCCESS(status) && (dev == device)) {
-				result = action(thermal, trip, cdev);
-				if (result)
-					goto failed;
-			}
+			if (ACPI_FAILURE(status) || dev != device)
+				continue;
+			if (bind)
+				result =
+					thermal_zone_bind_cooling_device
+					(thermal, trip, cdev,
+					 THERMAL_NO_LIMIT, THERMAL_NO_LIMIT);
+			else
+				result =
+					thermal_zone_unbind_cooling_device
+					(thermal, trip, cdev);
+			if (result)
+				goto failed;
 		}
 	}
 
@@ -776,11 +816,17 @@ static int acpi_thermal_cooling_device_cb(struct thermal_zone_device *thermal,
 		    j++) {
 			handle = tz->trips.active[i].devices.handles[j];
 			status = acpi_bus_get_device(handle, &dev);
-			if (ACPI_SUCCESS(status) && (dev == device)) {
-				result = action(thermal, trip, cdev);
-				if (result)
-					goto failed;
-			}
+			if (ACPI_FAILURE(status) || dev != device)
+				continue;
+			if (bind)
+				result = thermal_zone_bind_cooling_device
+					(thermal, trip, cdev,
+					 THERMAL_NO_LIMIT, THERMAL_NO_LIMIT);
+			else
+				result = thermal_zone_unbind_cooling_device
+					(thermal, trip, cdev);
+			if (result)
+				goto failed;
 		}
 	}
 
@@ -788,7 +834,14 @@ static int acpi_thermal_cooling_device_cb(struct thermal_zone_device *thermal,
 		handle = tz->devices.handles[i];
 		status = acpi_bus_get_device(handle, &dev);
 		if (ACPI_SUCCESS(status) && (dev == device)) {
-			result = action(thermal, -1, cdev);
+			if (bind)
+				result = thermal_zone_bind_cooling_device
+						(thermal, -1, cdev,
+						 THERMAL_NO_LIMIT,
+						 THERMAL_NO_LIMIT);
+			else
+				result = thermal_zone_unbind_cooling_device
+						(thermal, -1, cdev);
 			if (result)
 				goto failed;
 		}
@@ -802,16 +855,14 @@ static int
 acpi_thermal_bind_cooling_device(struct thermal_zone_device *thermal,
 					struct thermal_cooling_device *cdev)
 {
-	return acpi_thermal_cooling_device_cb(thermal, cdev,
-				thermal_zone_bind_cooling_device);
+	return acpi_thermal_cooling_device_cb(thermal, cdev, true);
 }
 
 static int
 acpi_thermal_unbind_cooling_device(struct thermal_zone_device *thermal,
 					struct thermal_cooling_device *cdev)
 {
-	return acpi_thermal_cooling_device_cb(thermal, cdev,
-				thermal_zone_unbind_cooling_device);
+	return acpi_thermal_cooling_device_cb(thermal, cdev, false);
 }
 
 static const struct thermal_zone_device_ops acpi_thermal_zone_ops = {
@@ -823,6 +874,7 @@ static const struct thermal_zone_device_ops acpi_thermal_zone_ops = {
 	.get_trip_type = thermal_get_trip_type,
 	.get_trip_temp = thermal_get_trip_temp,
 	.get_crit_temp = thermal_get_crit_temp,
+	.get_trend = thermal_get_trend,
 	.notify = thermal_notify,
 };
 
@@ -849,15 +901,12 @@ static int acpi_thermal_register_thermal_zone(struct acpi_thermal *tz)
 		tz->thermal_zone =
 			thermal_zone_device_register("acpitz", trips, 0, tz,
 						     &acpi_thermal_zone_ops,
-						     tz->trips.passive.tc1,
-						     tz->trips.passive.tc2,
 						     tz->trips.passive.tsp*100,
 						     tz->polling_frequency*100);
 	else
 		tz->thermal_zone =
 			thermal_zone_device_register("acpitz", trips, 0, tz,
-						     &acpi_thermal_zone_ops,
-						     0, 0, 0,
+						     &acpi_thermal_zone_ops, 0,
 						     tz->polling_frequency*100);
 	if (IS_ERR(tz->thermal_zone))
 		return -ENODEV;
diff --git a/drivers/acpi/utils.c b/drivers/acpi/utils.c
index 3e87c9c538a..462f7e30036 100644
--- a/drivers/acpi/utils.c
+++ b/drivers/acpi/utils.c
@@ -384,7 +384,7 @@ acpi_evaluate_reference(acpi_handle handle,
 EXPORT_SYMBOL(acpi_evaluate_reference);
 
 acpi_status
-acpi_get_physical_device_location(acpi_handle handle, struct acpi_pld *pld)
+acpi_get_physical_device_location(acpi_handle handle, struct acpi_pld_info **pld)
 {
 	acpi_status status;
 	struct acpi_buffer buffer = { ACPI_ALLOCATE_BUFFER, NULL };
@@ -400,13 +400,16 @@ acpi_get_physical_device_location(acpi_handle handle, struct acpi_pld *pld)
 	if (!output || output->type != ACPI_TYPE_PACKAGE
 	    || !output->package.count
 	    || output->package.elements[0].type != ACPI_TYPE_BUFFER
-	    || output->package.elements[0].buffer.length > sizeof(*pld)) {
+	    || output->package.elements[0].buffer.length < ACPI_PLD_REV1_BUFFER_SIZE) {
 		status = AE_TYPE;
 		goto out;
 	}
 
-	memcpy(pld, output->package.elements[0].buffer.pointer,
-	       output->package.elements[0].buffer.length);
+	status = acpi_decode_pld_buffer(
+			output->package.elements[0].buffer.pointer,
+			output->package.elements[0].buffer.length,
+			pld);
+
 out:
 	kfree(buffer.pointer);
 	return status;
diff --git a/drivers/base/memory.c b/drivers/base/memory.c
index 7dda4f790f0..86c88216a50 100644
--- a/drivers/base/memory.c
+++ b/drivers/base/memory.c
@@ -248,26 +248,23 @@ static bool pages_correctly_reserved(unsigned long start_pfn,
 static int
 memory_block_action(unsigned long phys_index, unsigned long action)
 {
-	unsigned long start_pfn, start_paddr;
+	unsigned long start_pfn;
 	unsigned long nr_pages = PAGES_PER_SECTION * sections_per_block;
 	struct page *first_page;
 	int ret;
 
 	first_page = pfn_to_page(phys_index << PFN_SECTION_SHIFT);
+	start_pfn = page_to_pfn(first_page);
 
 	switch (action) {
 		case MEM_ONLINE:
-			start_pfn = page_to_pfn(first_page);
-
 			if (!pages_correctly_reserved(start_pfn, nr_pages))
 				return -EBUSY;
 
 			ret = online_pages(start_pfn, nr_pages);
 			break;
 		case MEM_OFFLINE:
-			start_paddr = page_to_pfn(first_page) << PAGE_SHIFT;
-			ret = remove_memory(start_paddr,
-					    nr_pages << PAGE_SHIFT);
+			ret = offline_pages(start_pfn, nr_pages);
 			break;
 		default:
 			WARN(1, KERN_WARNING "%s(%ld, %ld) unknown action: "
@@ -278,13 +275,11 @@ memory_block_action(unsigned long phys_index, unsigned long action)
 	return ret;
 }
 
-static int memory_block_change_state(struct memory_block *mem,
+static int __memory_block_change_state(struct memory_block *mem,
 		unsigned long to_state, unsigned long from_state_req)
 {
 	int ret = 0;
 
-	mutex_lock(&mem->state_mutex);
-
 	if (mem->state != from_state_req) {
 		ret = -EINVAL;
 		goto out;
@@ -312,10 +307,20 @@ static int memory_block_change_state(struct memory_block *mem,
 		break;
 	}
 out:
-	mutex_unlock(&mem->state_mutex);
 	return ret;
 }
 
+static int memory_block_change_state(struct memory_block *mem,
+		unsigned long to_state, unsigned long from_state_req)
+{
+	int ret;
+
+	mutex_lock(&mem->state_mutex);
+	ret = __memory_block_change_state(mem, to_state, from_state_req);
+	mutex_unlock(&mem->state_mutex);
+
+	return ret;
+}
 static ssize_t
 store_mem_state(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t count)
@@ -656,6 +661,21 @@ int unregister_memory_section(struct mem_section *section)
 }
 
 /*
+ * offline one memory block. If the memory block has been offlined, do nothing.
+ */
+int offline_memory_block(struct memory_block *mem)
+{
+	int ret = 0;
+
+	mutex_lock(&mem->state_mutex);
+	if (mem->state != MEM_OFFLINE)
+		ret = __memory_block_change_state(mem, MEM_OFFLINE, MEM_ONLINE);
+	mutex_unlock(&mem->state_mutex);
+
+	return ret;
+}
+
+/*
  * Initialize the sysfs support for memory devices...
  */
 int __init memory_dev_init(void)
diff --git a/drivers/block/drbd/drbd_main.c b/drivers/block/drbd/drbd_main.c
index f93a0320e95..f55683ad4ff 100644
--- a/drivers/block/drbd/drbd_main.c
+++ b/drivers/block/drbd/drbd_main.c
@@ -162,23 +162,12 @@ static const struct block_device_operations drbd_ops = {
 	.release = drbd_release,
 };
 
-static void bio_destructor_drbd(struct bio *bio)
-{
-	bio_free(bio, drbd_md_io_bio_set);
-}
-
 struct bio *bio_alloc_drbd(gfp_t gfp_mask)
 {
-	struct bio *bio;
-
 	if (!drbd_md_io_bio_set)
 		return bio_alloc(gfp_mask, 1);
 
-	bio = bio_alloc_bioset(gfp_mask, 1, drbd_md_io_bio_set);
-	if (!bio)
-		return NULL;
-	bio->bi_destructor = bio_destructor_drbd;
-	return bio;
+	return bio_alloc_bioset(gfp_mask, 1, drbd_md_io_bio_set);
 }
 
 #ifdef __CHECKER__
diff --git a/drivers/block/osdblk.c b/drivers/block/osdblk.c
index 87311ebac0d..1bbc681688e 100644
--- a/drivers/block/osdblk.c
+++ b/drivers/block/osdblk.c
@@ -266,11 +266,10 @@ static struct bio *bio_chain_clone(struct bio *old_chain, gfp_t gfpmask)
 	struct bio *tmp, *new_chain = NULL, *tail = NULL;
 
 	while (old_chain) {
-		tmp = bio_kmalloc(gfpmask, old_chain->bi_max_vecs);
+		tmp = bio_clone_kmalloc(old_chain, gfpmask);
 		if (!tmp)
 			goto err_out;
 
-		__bio_clone(tmp, old_chain);
 		tmp->bi_bdev = NULL;
 		gfpmask &= ~__GFP_WAIT;
 		tmp->bi_next = NULL;
diff --git a/drivers/block/pktcdvd.c b/drivers/block/pktcdvd.c
index ba66e4445f4..2e7de7a59bf 100644
--- a/drivers/block/pktcdvd.c
+++ b/drivers/block/pktcdvd.c
@@ -522,38 +522,6 @@ static void pkt_bio_finished(struct pktcdvd_device *pd)
 	}
 }
 
-static void pkt_bio_destructor(struct bio *bio)
-{
-	kfree(bio->bi_io_vec);
-	kfree(bio);
-}
-
-static struct bio *pkt_bio_alloc(int nr_iovecs)
-{
-	struct bio_vec *bvl = NULL;
-	struct bio *bio;
-
-	bio = kmalloc(sizeof(struct bio), GFP_KERNEL);
-	if (!bio)
-		goto no_bio;
-	bio_init(bio);
-
-	bvl = kcalloc(nr_iovecs, sizeof(struct bio_vec), GFP_KERNEL);
-	if (!bvl)
-		goto no_bvl;
-
-	bio->bi_max_vecs = nr_iovecs;
-	bio->bi_io_vec = bvl;
-	bio->bi_destructor = pkt_bio_destructor;
-
-	return bio;
-
- no_bvl:
-	kfree(bio);
- no_bio:
-	return NULL;
-}
-
 /*
  * Allocate a packet_data struct
  */
@@ -567,7 +535,7 @@ static struct packet_data *pkt_alloc_packet_data(int frames)
 		goto no_pkt;
 
 	pkt->frames = frames;
-	pkt->w_bio = pkt_bio_alloc(frames);
+	pkt->w_bio = bio_kmalloc(GFP_KERNEL, frames);
 	if (!pkt->w_bio)
 		goto no_bio;
 
@@ -581,9 +549,10 @@ static struct packet_data *pkt_alloc_packet_data(int frames)
 	bio_list_init(&pkt->orig_bios);
 
 	for (i = 0; i < frames; i++) {
-		struct bio *bio = pkt_bio_alloc(1);
+		struct bio *bio = bio_kmalloc(GFP_KERNEL, 1);
 		if (!bio)
 			goto no_rd_bio;
+
 		pkt->r_bios[i] = bio;
 	}
 
@@ -1111,21 +1080,17 @@ static void pkt_gather_data(struct pktcdvd_device *pd, struct packet_data *pkt)
 	 * Schedule reads for missing parts of the packet.
 	 */
 	for (f = 0; f < pkt->frames; f++) {
-		struct bio_vec *vec;
-
 		int p, offset;
+
 		if (written[f])
 			continue;
+
 		bio = pkt->r_bios[f];
-		vec = bio->bi_io_vec;
-		bio_init(bio);
-		bio->bi_max_vecs = 1;
+		bio_reset(bio);
 		bio->bi_sector = pkt->sector + f * (CD_FRAMESIZE >> 9);
 		bio->bi_bdev = pd->bdev;
 		bio->bi_end_io = pkt_end_io_read;
 		bio->bi_private = pkt;
-		bio->bi_io_vec = vec;
-		bio->bi_destructor = pkt_bio_destructor;
 
 		p = (f * CD_FRAMESIZE) / PAGE_SIZE;
 		offset = (f * CD_FRAMESIZE) % PAGE_SIZE;
@@ -1418,14 +1383,11 @@ static void pkt_start_write(struct pktcdvd_device *pd, struct packet_data *pkt)
 	}
 
 	/* Start the write request */
-	bio_init(pkt->w_bio);
-	pkt->w_bio->bi_max_vecs = PACKET_MAX_SIZE;
+	bio_reset(pkt->w_bio);
 	pkt->w_bio->bi_sector = pkt->sector;
 	pkt->w_bio->bi_bdev = pd->bdev;
 	pkt->w_bio->bi_end_io = pkt_end_io_packet_write;
 	pkt->w_bio->bi_private = pkt;
-	pkt->w_bio->bi_io_vec = bvec;
-	pkt->w_bio->bi_destructor = pkt_bio_destructor;
 	for (f = 0; f < pkt->frames; f++)
 		if (!bio_add_page(pkt->w_bio, bvec[f].bv_page, CD_FRAMESIZE, bvec[f].bv_offset))
 			BUG();
diff --git a/drivers/block/rbd.c b/drivers/block/rbd.c
index 54a55f03115..bb3d9be3b1b 100644
--- a/drivers/block/rbd.c
+++ b/drivers/block/rbd.c
@@ -41,6 +41,8 @@
 
 #include "rbd_types.h"
 
+#define RBD_DEBUG	/* Activate rbd_assert() calls */
+
 /*
  * The basic unit of block I/O is a sector.  It is interpreted in a
  * number of contexts in Linux (blk, bio, genhd), but the default is
@@ -50,16 +52,24 @@
 #define	SECTOR_SHIFT	9
 #define	SECTOR_SIZE	(1ULL << SECTOR_SHIFT)
 
+/* It might be useful to have this defined elsewhere too */
+
+#define	U64_MAX	((u64) (~0ULL))
+
 #define RBD_DRV_NAME "rbd"
 #define RBD_DRV_NAME_LONG "rbd (rados block device)"
 
 #define RBD_MINORS_PER_MAJOR	256		/* max minors per blkdev */
 
 #define RBD_MAX_SNAP_NAME_LEN	32
+#define RBD_MAX_SNAP_COUNT	510	/* allows max snapc to fit in 4KB */
 #define RBD_MAX_OPT_LEN		1024
 
 #define RBD_SNAP_HEAD_NAME	"-"
 
+#define RBD_IMAGE_ID_LEN_MAX	64
+#define RBD_OBJ_PREFIX_LEN_MAX	64
+
 /*
  * An RBD device name will be "rbd#", where the "rbd" comes from
  * RBD_DRV_NAME above, and # is a unique integer identifier.
@@ -69,21 +79,22 @@
 #define DEV_NAME_LEN		32
 #define MAX_INT_FORMAT_WIDTH	((5 * sizeof (int)) / 2 + 1)
 
-#define RBD_NOTIFY_TIMEOUT_DEFAULT 10
+#define RBD_READ_ONLY_DEFAULT		false
 
 /*
  * block device image metadata (in-memory version)
  */
 struct rbd_image_header {
-	u64 image_size;
+	/* These four fields never change for a given rbd image */
 	char *object_prefix;
+	u64 features;
 	__u8 obj_order;
 	__u8 crypt_type;
 	__u8 comp_type;
-	struct ceph_snap_context *snapc;
-	size_t snap_names_len;
-	u32 total_snaps;
 
+	/* The remaining fields need to be updated occasionally */
+	u64 image_size;
+	struct ceph_snap_context *snapc;
 	char *snap_names;
 	u64 *snap_sizes;
 
@@ -91,7 +102,7 @@ struct rbd_image_header {
 };
 
 struct rbd_options {
-	int	notify_timeout;
+	bool	read_only;
 };
 
 /*
@@ -99,7 +110,6 @@ struct rbd_options {
  */
 struct rbd_client {
 	struct ceph_client	*client;
-	struct rbd_options	*rbd_opts;
 	struct kref		kref;
 	struct list_head	node;
 };
@@ -141,6 +151,16 @@ struct rbd_snap {
 	u64			size;
 	struct list_head	node;
 	u64			id;
+	u64			features;
+};
+
+struct rbd_mapping {
+	char                    *snap_name;
+	u64                     snap_id;
+	u64                     size;
+	u64                     features;
+	bool                    snap_exists;
+	bool			read_only;
 };
 
 /*
@@ -151,8 +171,9 @@ struct rbd_device {
 
 	int			major;		/* blkdev assigned major */
 	struct gendisk		*disk;		/* blkdev's gendisk and rq */
-	struct request_queue	*q;
 
+	u32			image_format;	/* Either 1 or 2 */
+	struct rbd_options	rbd_opts;
 	struct rbd_client	*rbd_client;
 
 	char			name[DEV_NAME_LEN]; /* blkdev name, e.g. rbd3 */
@@ -160,6 +181,8 @@ struct rbd_device {
 	spinlock_t		lock;		/* queue lock */
 
 	struct rbd_image_header	header;
+	char			*image_id;
+	size_t			image_id_len;
 	char			*image_name;
 	size_t			image_name_len;
 	char			*header_name;
@@ -171,13 +194,8 @@ struct rbd_device {
 
 	/* protects updating the header */
 	struct rw_semaphore     header_rwsem;
-	/* name of the snapshot this device reads from */
-	char                    *snap_name;
-	/* id of the snapshot this device reads from */
-	u64                     snap_id;	/* current snapshot id */
-	/* whether the snap_id this device reads from still exists */
-	bool                    snap_exists;
-	int                     read_only;
+
+	struct rbd_mapping	mapping;
 
 	struct list_head	node;
 
@@ -196,12 +214,10 @@ static DEFINE_SPINLOCK(rbd_dev_list_lock);
 static LIST_HEAD(rbd_client_list);		/* clients */
 static DEFINE_SPINLOCK(rbd_client_list_lock);
 
-static int __rbd_init_snaps_header(struct rbd_device *rbd_dev);
+static int rbd_dev_snaps_update(struct rbd_device *rbd_dev);
+static int rbd_dev_snaps_register(struct rbd_device *rbd_dev);
+
 static void rbd_dev_release(struct device *dev);
-static ssize_t rbd_snap_add(struct device *dev,
-			    struct device_attribute *attr,
-			    const char *buf,
-			    size_t count);
 static void __rbd_remove_snap_dev(struct rbd_snap *snap);
 
 static ssize_t rbd_add(struct bus_type *bus, const char *buf,
@@ -229,6 +245,18 @@ static struct device rbd_root_dev = {
 	.release =      rbd_root_dev_release,
 };
 
+#ifdef RBD_DEBUG
+#define rbd_assert(expr)						\
+		if (unlikely(!(expr))) {				\
+			printk(KERN_ERR "\nAssertion failure in %s() "	\
+						"at line %d:\n\n"	\
+					"\trbd_assert(%s);\n\n",	\
+					__func__, __LINE__, #expr);	\
+			BUG();						\
+		}
+#else /* !RBD_DEBUG */
+#  define rbd_assert(expr)	((void) 0)
+#endif /* !RBD_DEBUG */
 
 static struct device *rbd_get_dev(struct rbd_device *rbd_dev)
 {
@@ -246,11 +274,11 @@ static int rbd_open(struct block_device *bdev, fmode_t mode)
 {
 	struct rbd_device *rbd_dev = bdev->bd_disk->private_data;
 
-	if ((mode & FMODE_WRITE) && rbd_dev->read_only)
+	if ((mode & FMODE_WRITE) && rbd_dev->mapping.read_only)
 		return -EROFS;
 
 	rbd_get_dev(rbd_dev);
-	set_device_ro(bdev, rbd_dev->read_only);
+	set_device_ro(bdev, rbd_dev->mapping.read_only);
 
 	return 0;
 }
@@ -274,8 +302,7 @@ static const struct block_device_operations rbd_bd_ops = {
  * Initialize an rbd client instance.
  * We own *ceph_opts.
  */
-static struct rbd_client *rbd_client_create(struct ceph_options *ceph_opts,
-					    struct rbd_options *rbd_opts)
+static struct rbd_client *rbd_client_create(struct ceph_options *ceph_opts)
 {
 	struct rbd_client *rbdc;
 	int ret = -ENOMEM;
@@ -299,8 +326,6 @@ static struct rbd_client *rbd_client_create(struct ceph_options *ceph_opts,
 	if (ret < 0)
 		goto out_err;
 
-	rbdc->rbd_opts = rbd_opts;
-
 	spin_lock(&rbd_client_list_lock);
 	list_add_tail(&rbdc->node, &rbd_client_list);
 	spin_unlock(&rbd_client_list_lock);
@@ -322,36 +347,52 @@ out_opt:
 }
 
 /*
- * Find a ceph client with specific addr and configuration.
+ * Find a ceph client with specific addr and configuration.  If
+ * found, bump its reference count.
  */
-static struct rbd_client *__rbd_client_find(struct ceph_options *ceph_opts)
+static struct rbd_client *rbd_client_find(struct ceph_options *ceph_opts)
 {
 	struct rbd_client *client_node;
+	bool found = false;
 
 	if (ceph_opts->flags & CEPH_OPT_NOSHARE)
 		return NULL;
 
-	list_for_each_entry(client_node, &rbd_client_list, node)
-		if (!ceph_compare_options(ceph_opts, client_node->client))
-			return client_node;
-	return NULL;
+	spin_lock(&rbd_client_list_lock);
+	list_for_each_entry(client_node, &rbd_client_list, node) {
+		if (!ceph_compare_options(ceph_opts, client_node->client)) {
+			kref_get(&client_node->kref);
+			found = true;
+			break;
+		}
+	}
+	spin_unlock(&rbd_client_list_lock);
+
+	return found ? client_node : NULL;
 }
 
 /*
  * mount options
  */
 enum {
-	Opt_notify_timeout,
 	Opt_last_int,
 	/* int args above */
 	Opt_last_string,
 	/* string args above */
+	Opt_read_only,
+	Opt_read_write,
+	/* Boolean args above */
+	Opt_last_bool,
 };
 
 static match_table_t rbd_opts_tokens = {
-	{Opt_notify_timeout, "notify_timeout=%d"},
 	/* int args above */
 	/* string args above */
+	{Opt_read_only, "mapping.read_only"},
+	{Opt_read_only, "ro"},		/* Alternate spelling */
+	{Opt_read_write, "read_write"},
+	{Opt_read_write, "rw"},		/* Alternate spelling */
+	/* Boolean args above */
 	{-1, NULL}
 };
 
@@ -376,16 +417,22 @@ static int parse_rbd_opts_token(char *c, void *private)
 	} else if (token > Opt_last_int && token < Opt_last_string) {
 		dout("got string token %d val %s\n", token,
 		     argstr[0].from);
+	} else if (token > Opt_last_string && token < Opt_last_bool) {
+		dout("got Boolean token %d\n", token);
 	} else {
 		dout("got token %d\n", token);
 	}
 
 	switch (token) {
-	case Opt_notify_timeout:
-		rbd_opts->notify_timeout = intval;
+	case Opt_read_only:
+		rbd_opts->read_only = true;
+		break;
+	case Opt_read_write:
+		rbd_opts->read_only = false;
 		break;
 	default:
-		BUG_ON(token);
+		rbd_assert(false);
+		break;
 	}
 	return 0;
 }
@@ -394,48 +441,33 @@ static int parse_rbd_opts_token(char *c, void *private)
  * Get a ceph client with specific addr and configuration, if one does
  * not exist create it.
  */
-static struct rbd_client *rbd_get_client(const char *mon_addr,
-					 size_t mon_addr_len,
-					 char *options)
+static int rbd_get_client(struct rbd_device *rbd_dev, const char *mon_addr,
+				size_t mon_addr_len, char *options)
 {
-	struct rbd_client *rbdc;
+	struct rbd_options *rbd_opts = &rbd_dev->rbd_opts;
 	struct ceph_options *ceph_opts;
-	struct rbd_options *rbd_opts;
-
-	rbd_opts = kzalloc(sizeof(*rbd_opts), GFP_KERNEL);
-	if (!rbd_opts)
-		return ERR_PTR(-ENOMEM);
+	struct rbd_client *rbdc;
 
-	rbd_opts->notify_timeout = RBD_NOTIFY_TIMEOUT_DEFAULT;
+	rbd_opts->read_only = RBD_READ_ONLY_DEFAULT;
 
 	ceph_opts = ceph_parse_options(options, mon_addr,
 					mon_addr + mon_addr_len,
 					parse_rbd_opts_token, rbd_opts);
-	if (IS_ERR(ceph_opts)) {
-		kfree(rbd_opts);
-		return ERR_CAST(ceph_opts);
-	}
+	if (IS_ERR(ceph_opts))
+		return PTR_ERR(ceph_opts);
 
-	spin_lock(&rbd_client_list_lock);
-	rbdc = __rbd_client_find(ceph_opts);
+	rbdc = rbd_client_find(ceph_opts);
 	if (rbdc) {
 		/* using an existing client */
-		kref_get(&rbdc->kref);
-		spin_unlock(&rbd_client_list_lock);
-
 		ceph_destroy_options(ceph_opts);
-		kfree(rbd_opts);
-
-		return rbdc;
+	} else {
+		rbdc = rbd_client_create(ceph_opts);
+		if (IS_ERR(rbdc))
+			return PTR_ERR(rbdc);
 	}
-	spin_unlock(&rbd_client_list_lock);
-
-	rbdc = rbd_client_create(ceph_opts, rbd_opts);
+	rbd_dev->rbd_client = rbdc;
 
-	if (IS_ERR(rbdc))
-		kfree(rbd_opts);
-
-	return rbdc;
+	return 0;
 }
 
 /*
@@ -453,7 +485,6 @@ static void rbd_client_release(struct kref *kref)
 	spin_unlock(&rbd_client_list_lock);
 
 	ceph_destroy_client(rbdc->client);
-	kfree(rbdc->rbd_opts);
 	kfree(rbdc);
 }
 
@@ -479,10 +510,38 @@ static void rbd_coll_release(struct kref *kref)
 	kfree(coll);
 }
 
+static bool rbd_image_format_valid(u32 image_format)
+{
+	return image_format == 1 || image_format == 2;
+}
+
 static bool rbd_dev_ondisk_valid(struct rbd_image_header_ondisk *ondisk)
 {
-	return !memcmp(&ondisk->text,
-			RBD_HEADER_TEXT, sizeof (RBD_HEADER_TEXT));
+	size_t size;
+	u32 snap_count;
+
+	/* The header has to start with the magic rbd header text */
+	if (memcmp(&ondisk->text, RBD_HEADER_TEXT, sizeof (RBD_HEADER_TEXT)))
+		return false;
+
+	/*
+	 * The size of a snapshot header has to fit in a size_t, and
+	 * that limits the number of snapshots.
+	 */
+	snap_count = le32_to_cpu(ondisk->snap_count);
+	size = SIZE_MAX - sizeof (struct ceph_snap_context);
+	if (snap_count > size / sizeof (__le64))
+		return false;
+
+	/*
+	 * Not only that, but the size of the entire the snapshot
+	 * header must also be representable in a size_t.
+	 */
+	size -= snap_count * sizeof (__le64);
+	if ((u64) size < le64_to_cpu(ondisk->snap_names_len))
+		return false;
+
+	return true;
 }
 
 /*
@@ -490,179 +549,203 @@ static bool rbd_dev_ondisk_valid(struct rbd_image_header_ondisk *ondisk)
  * header.
  */
 static int rbd_header_from_disk(struct rbd_image_header *header,
-				 struct rbd_image_header_ondisk *ondisk,
-				 u32 allocated_snaps)
+				 struct rbd_image_header_ondisk *ondisk)
 {
 	u32 snap_count;
+	size_t len;
+	size_t size;
+	u32 i;
 
-	if (!rbd_dev_ondisk_valid(ondisk))
-		return -ENXIO;
+	memset(header, 0, sizeof (*header));
 
 	snap_count = le32_to_cpu(ondisk->snap_count);
-	if (snap_count > (SIZE_MAX - sizeof(struct ceph_snap_context))
-				 / sizeof (u64))
-		return -EINVAL;
-	header->snapc = kmalloc(sizeof(struct ceph_snap_context) +
-				snap_count * sizeof(u64),
-				GFP_KERNEL);
-	if (!header->snapc)
+
+	len = strnlen(ondisk->object_prefix, sizeof (ondisk->object_prefix));
+	header->object_prefix = kmalloc(len + 1, GFP_KERNEL);
+	if (!header->object_prefix)
 		return -ENOMEM;
+	memcpy(header->object_prefix, ondisk->object_prefix, len);
+	header->object_prefix[len] = '\0';
 
 	if (snap_count) {
-		header->snap_names_len = le64_to_cpu(ondisk->snap_names_len);
-		header->snap_names = kmalloc(header->snap_names_len,
-					     GFP_KERNEL);
+		u64 snap_names_len = le64_to_cpu(ondisk->snap_names_len);
+
+		/* Save a copy of the snapshot names */
+
+		if (snap_names_len > (u64) SIZE_MAX)
+			return -EIO;
+		header->snap_names = kmalloc(snap_names_len, GFP_KERNEL);
 		if (!header->snap_names)
-			goto err_snapc;
-		header->snap_sizes = kmalloc(snap_count * sizeof(u64),
-					     GFP_KERNEL);
+			goto out_err;
+		/*
+		 * Note that rbd_dev_v1_header_read() guarantees
+		 * the ondisk buffer we're working with has
+		 * snap_names_len bytes beyond the end of the
+		 * snapshot id array, this memcpy() is safe.
+		 */
+		memcpy(header->snap_names, &ondisk->snaps[snap_count],
+			snap_names_len);
+
+		/* Record each snapshot's size */
+
+		size = snap_count * sizeof (*header->snap_sizes);
+		header->snap_sizes = kmalloc(size, GFP_KERNEL);
 		if (!header->snap_sizes)
-			goto err_names;
+			goto out_err;
+		for (i = 0; i < snap_count; i++)
+			header->snap_sizes[i] =
+				le64_to_cpu(ondisk->snaps[i].image_size);
 	} else {
 		WARN_ON(ondisk->snap_names_len);
-		header->snap_names_len = 0;
 		header->snap_names = NULL;
 		header->snap_sizes = NULL;
 	}
 
-	header->object_prefix = kmalloc(sizeof (ondisk->block_name) + 1,
-					GFP_KERNEL);
-	if (!header->object_prefix)
-		goto err_sizes;
-
-	memcpy(header->object_prefix, ondisk->block_name,
-	       sizeof(ondisk->block_name));
-	header->object_prefix[sizeof (ondisk->block_name)] = '\0';
-
-	header->image_size = le64_to_cpu(ondisk->image_size);
+	header->features = 0;	/* No features support in v1 images */
 	header->obj_order = ondisk->options.order;
 	header->crypt_type = ondisk->options.crypt_type;
 	header->comp_type = ondisk->options.comp_type;
 
+	/* Allocate and fill in the snapshot context */
+
+	header->image_size = le64_to_cpu(ondisk->image_size);
+	size = sizeof (struct ceph_snap_context);
+	size += snap_count * sizeof (header->snapc->snaps[0]);
+	header->snapc = kzalloc(size, GFP_KERNEL);
+	if (!header->snapc)
+		goto out_err;
+
 	atomic_set(&header->snapc->nref, 1);
 	header->snapc->seq = le64_to_cpu(ondisk->snap_seq);
 	header->snapc->num_snaps = snap_count;
-	header->total_snaps = snap_count;
-
-	if (snap_count && allocated_snaps == snap_count) {
-		int i;
-
-		for (i = 0; i < snap_count; i++) {
-			header->snapc->snaps[i] =
-				le64_to_cpu(ondisk->snaps[i].id);
-			header->snap_sizes[i] =
-				le64_to_cpu(ondisk->snaps[i].image_size);
-		}
-
-		/* copy snapshot names */
-		memcpy(header->snap_names, &ondisk->snaps[snap_count],
-			header->snap_names_len);
-	}
+	for (i = 0; i < snap_count; i++)
+		header->snapc->snaps[i] =
+			le64_to_cpu(ondisk->snaps[i].id);
 
 	return 0;
 
-err_sizes:
+out_err:
 	kfree(header->snap_sizes);
 	header->snap_sizes = NULL;
-err_names:
 	kfree(header->snap_names);
 	header->snap_names = NULL;
-err_snapc:
-	kfree(header->snapc);
-	header->snapc = NULL;
+	kfree(header->object_prefix);
+	header->object_prefix = NULL;
 
 	return -ENOMEM;
 }
 
-static int snap_by_name(struct rbd_image_header *header, const char *snap_name,
-			u64 *seq, u64 *size)
+static int snap_by_name(struct rbd_device *rbd_dev, const char *snap_name)
 {
-	int i;
-	char *p = header->snap_names;
 
-	for (i = 0; i < header->total_snaps; i++) {
-		if (!strcmp(snap_name, p)) {
+	struct rbd_snap *snap;
 
-			/* Found it.  Pass back its id and/or size */
+	list_for_each_entry(snap, &rbd_dev->snaps, node) {
+		if (!strcmp(snap_name, snap->name)) {
+			rbd_dev->mapping.snap_id = snap->id;
+			rbd_dev->mapping.size = snap->size;
+			rbd_dev->mapping.features = snap->features;
 
-			if (seq)
-				*seq = header->snapc->snaps[i];
-			if (size)
-				*size = header->snap_sizes[i];
-			return i;
+			return 0;
 		}
-		p += strlen(p) + 1;	/* Skip ahead to the next name */
 	}
+
 	return -ENOENT;
 }
 
-static int rbd_header_set_snap(struct rbd_device *rbd_dev, u64 *size)
+static int rbd_dev_set_mapping(struct rbd_device *rbd_dev, char *snap_name)
 {
 	int ret;
 
-	down_write(&rbd_dev->header_rwsem);
-
-	if (!memcmp(rbd_dev->snap_name, RBD_SNAP_HEAD_NAME,
+	if (!memcmp(snap_name, RBD_SNAP_HEAD_NAME,
 		    sizeof (RBD_SNAP_HEAD_NAME))) {
-		rbd_dev->snap_id = CEPH_NOSNAP;
-		rbd_dev->snap_exists = false;
-		rbd_dev->read_only = 0;
-		if (size)
-			*size = rbd_dev->header.image_size;
+		rbd_dev->mapping.snap_id = CEPH_NOSNAP;
+		rbd_dev->mapping.size = rbd_dev->header.image_size;
+		rbd_dev->mapping.features = rbd_dev->header.features;
+		rbd_dev->mapping.snap_exists = false;
+		rbd_dev->mapping.read_only = rbd_dev->rbd_opts.read_only;
+		ret = 0;
 	} else {
-		u64 snap_id = 0;
-
-		ret = snap_by_name(&rbd_dev->header, rbd_dev->snap_name,
-					&snap_id, size);
+		ret = snap_by_name(rbd_dev, snap_name);
 		if (ret < 0)
 			goto done;
-		rbd_dev->snap_id = snap_id;
-		rbd_dev->snap_exists = true;
-		rbd_dev->read_only = 1;
+		rbd_dev->mapping.snap_exists = true;
+		rbd_dev->mapping.read_only = true;
 	}
-
-	ret = 0;
+	rbd_dev->mapping.snap_name = snap_name;
 done:
-	up_write(&rbd_dev->header_rwsem);
 	return ret;
 }
 
 static void rbd_header_free(struct rbd_image_header *header)
 {
 	kfree(header->object_prefix);
+	header->object_prefix = NULL;
 	kfree(header->snap_sizes);
+	header->snap_sizes = NULL;
 	kfree(header->snap_names);
+	header->snap_names = NULL;
 	ceph_put_snap_context(header->snapc);
+	header->snapc = NULL;
 }
 
-/*
- * get the actual striped segment name, offset and length
- */
-static u64 rbd_get_segment(struct rbd_image_header *header,
-			   const char *object_prefix,
-			   u64 ofs, u64 len,
-			   char *seg_name, u64 *segofs)
+static char *rbd_segment_name(struct rbd_device *rbd_dev, u64 offset)
+{
+	char *name;
+	u64 segment;
+	int ret;
+
+	name = kmalloc(RBD_MAX_SEG_NAME_LEN + 1, GFP_NOIO);
+	if (!name)
+		return NULL;
+	segment = offset >> rbd_dev->header.obj_order;
+	ret = snprintf(name, RBD_MAX_SEG_NAME_LEN, "%s.%012llx",
+			rbd_dev->header.object_prefix, segment);
+	if (ret < 0 || ret >= RBD_MAX_SEG_NAME_LEN) {
+		pr_err("error formatting segment name for #%llu (%d)\n",
+			segment, ret);
+		kfree(name);
+		name = NULL;
+	}
+
+	return name;
+}
+
+static u64 rbd_segment_offset(struct rbd_device *rbd_dev, u64 offset)
 {
-	u64 seg = ofs >> header->obj_order;
+	u64 segment_size = (u64) 1 << rbd_dev->header.obj_order;
 
-	if (seg_name)
-		snprintf(seg_name, RBD_MAX_SEG_NAME_LEN,
-			 "%s.%012llx", object_prefix, seg);
+	return offset & (segment_size - 1);
+}
+
+static u64 rbd_segment_length(struct rbd_device *rbd_dev,
+				u64 offset, u64 length)
+{
+	u64 segment_size = (u64) 1 << rbd_dev->header.obj_order;
 
-	ofs = ofs & ((1 << header->obj_order) - 1);
-	len = min_t(u64, len, (1 << header->obj_order) - ofs);
+	offset &= segment_size - 1;
 
-	if (segofs)
-		*segofs = ofs;
+	rbd_assert(length <= U64_MAX - offset);
+	if (offset + length > segment_size)
+		length = segment_size - offset;
 
-	return len;
+	return length;
 }
 
 static int rbd_get_num_segments(struct rbd_image_header *header,
 				u64 ofs, u64 len)
 {
-	u64 start_seg = ofs >> header->obj_order;
-	u64 end_seg = (ofs + len - 1) >> header->obj_order;
+	u64 start_seg;
+	u64 end_seg;
+
+	if (!len)
+		return 0;
+	if (len - 1 > U64_MAX - ofs)
+		return -ERANGE;
+
+	start_seg = ofs >> header->obj_order;
+	end_seg = (ofs + len - 1) >> header->obj_order;
+
 	return end_seg - start_seg + 1;
 }
 
@@ -724,7 +807,9 @@ static struct bio *bio_chain_clone(struct bio **old, struct bio **next,
 				   struct bio_pair **bp,
 				   int len, gfp_t gfpmask)
 {
-	struct bio *tmp, *old_chain = *old, *new_chain = NULL, *tail = NULL;
+	struct bio *old_chain = *old;
+	struct bio *new_chain = NULL;
+	struct bio *tail;
 	int total = 0;
 
 	if (*bp) {
@@ -733,9 +818,12 @@ static struct bio *bio_chain_clone(struct bio **old, struct bio **next,
 	}
 
 	while (old_chain && (total < len)) {
+		struct bio *tmp;
+
 		tmp = bio_kmalloc(gfpmask, old_chain->bi_max_vecs);
 		if (!tmp)
 			goto err_out;
+		gfpmask &= ~__GFP_WAIT;	/* can't wait after the first */
 
 		if (total + old_chain->bi_size > len) {
 			struct bio_pair *bp;
@@ -763,24 +851,18 @@ static struct bio *bio_chain_clone(struct bio **old, struct bio **next,
 		}
 
 		tmp->bi_bdev = NULL;
-		gfpmask &= ~__GFP_WAIT;
 		tmp->bi_next = NULL;
-
-		if (!new_chain) {
-			new_chain = tail = tmp;
-		} else {
+		if (new_chain)
 			tail->bi_next = tmp;
-			tail = tmp;
-		}
+		else
+			new_chain = tmp;
+		tail = tmp;
 		old_chain = old_chain->bi_next;
 
 		total += tmp->bi_size;
 	}
 
-	BUG_ON(total < len);
-
-	if (tail)
-		tail->bi_next = NULL;
+	rbd_assert(total == len);
 
 	*old = old_chain;
 
@@ -938,8 +1020,9 @@ static int rbd_do_request(struct request *rq,
 	layout->fl_stripe_count = cpu_to_le32(1);
 	layout->fl_object_size = cpu_to_le32(1 << RBD_MAX_OBJ_ORDER);
 	layout->fl_pg_pool = cpu_to_le32(rbd_dev->pool_id);
-	ceph_calc_raw_layout(osdc, layout, snapid, ofs, &len, &bno,
-				req, ops);
+	ret = ceph_calc_raw_layout(osdc, layout, snapid, ofs, &len, &bno,
+				   req, ops);
+	rbd_assert(ret == 0);
 
 	ceph_osdc_build_request(req, ofs, &len,
 				ops,
@@ -1030,8 +1113,8 @@ static int rbd_req_sync_op(struct rbd_device *rbd_dev,
 			   int flags,
 			   struct ceph_osd_req_op *ops,
 			   const char *object_name,
-			   u64 ofs, u64 len,
-			   char *buf,
+			   u64 ofs, u64 inbound_size,
+			   char *inbound,
 			   struct ceph_osd_request **linger_req,
 			   u64 *ver)
 {
@@ -1039,15 +1122,15 @@ static int rbd_req_sync_op(struct rbd_device *rbd_dev,
 	struct page **pages;
 	int num_pages;
 
-	BUG_ON(ops == NULL);
+	rbd_assert(ops != NULL);
 
-	num_pages = calc_pages_for(ofs , len);
+	num_pages = calc_pages_for(ofs, inbound_size);
 	pages = ceph_alloc_page_vector(num_pages, GFP_KERNEL);
 	if (IS_ERR(pages))
 		return PTR_ERR(pages);
 
 	ret = rbd_do_request(NULL, rbd_dev, snapc, snapid,
-			  object_name, ofs, len, NULL,
+			  object_name, ofs, inbound_size, NULL,
 			  pages, num_pages,
 			  flags,
 			  ops,
@@ -1057,8 +1140,8 @@ static int rbd_req_sync_op(struct rbd_device *rbd_dev,
 	if (ret < 0)
 		goto done;
 
-	if ((flags & CEPH_OSD_FLAG_READ) && buf)
-		ret = ceph_copy_from_page_vector(pages, buf, ofs, ret);
+	if ((flags & CEPH_OSD_FLAG_READ) && inbound)
+		ret = ceph_copy_from_page_vector(pages, inbound, ofs, ret);
 
 done:
 	ceph_release_page_vector(pages, num_pages);
@@ -1085,14 +1168,11 @@ static int rbd_do_op(struct request *rq,
 	struct ceph_osd_req_op *ops;
 	u32 payload_len;
 
-	seg_name = kmalloc(RBD_MAX_SEG_NAME_LEN + 1, GFP_NOIO);
+	seg_name = rbd_segment_name(rbd_dev, ofs);
 	if (!seg_name)
 		return -ENOMEM;
-
-	seg_len = rbd_get_segment(&rbd_dev->header,
-				  rbd_dev->header.object_prefix,
-				  ofs, len,
-				  seg_name, &seg_ofs);
+	seg_len = rbd_segment_length(rbd_dev, ofs, len);
+	seg_ofs = rbd_segment_offset(rbd_dev, ofs);
 
 	payload_len = (flags & CEPH_OSD_FLAG_WRITE ? seg_len : 0);
 
@@ -1104,7 +1184,7 @@ static int rbd_do_op(struct request *rq,
 	/* we've taken care of segment sizes earlier when we
 	   cloned the bios. We should never have a segment
 	   truncated at this point */
-	BUG_ON(seg_len < len);
+	rbd_assert(seg_len == len);
 
 	ret = rbd_do_request(rq, rbd_dev, snapc, snapid,
 			     seg_name, seg_ofs, seg_len,
@@ -1306,89 +1386,36 @@ static int rbd_req_sync_unwatch(struct rbd_device *rbd_dev)
 	return ret;
 }
 
-struct rbd_notify_info {
-	struct rbd_device *rbd_dev;
-};
-
-static void rbd_notify_cb(u64 ver, u64 notify_id, u8 opcode, void *data)
-{
-	struct rbd_device *rbd_dev = (struct rbd_device *)data;
-	if (!rbd_dev)
-		return;
-
-	dout("rbd_notify_cb %s notify_id=%llu opcode=%u\n",
-			rbd_dev->header_name, (unsigned long long) notify_id,
-			(unsigned int) opcode);
-}
-
 /*
- * Request sync osd notify
- */
-static int rbd_req_sync_notify(struct rbd_device *rbd_dev)
-{
-	struct ceph_osd_req_op *ops;
-	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
-	struct ceph_osd_event *event;
-	struct rbd_notify_info info;
-	int payload_len = sizeof(u32) + sizeof(u32);
-	int ret;
-
-	ops = rbd_create_rw_ops(1, CEPH_OSD_OP_NOTIFY, payload_len);
-	if (!ops)
-		return -ENOMEM;
-
-	info.rbd_dev = rbd_dev;
-
-	ret = ceph_osdc_create_event(osdc, rbd_notify_cb, 1,
-				     (void *)&info, &event);
-	if (ret < 0)
-		goto fail;
-
-	ops[0].watch.ver = 1;
-	ops[0].watch.flag = 1;
-	ops[0].watch.cookie = event->cookie;
-	ops[0].watch.prot_ver = RADOS_NOTIFY_VER;
-	ops[0].watch.timeout = 12;
-
-	ret = rbd_req_sync_op(rbd_dev, NULL,
-			       CEPH_NOSNAP,
-			       CEPH_OSD_FLAG_WRITE | CEPH_OSD_FLAG_ONDISK,
-			       ops,
-			       rbd_dev->header_name,
-			       0, 0, NULL, NULL, NULL);
-	if (ret < 0)
-		goto fail_event;
-
-	ret = ceph_osdc_wait_event(event, CEPH_OSD_TIMEOUT_DEFAULT);
-	dout("ceph_osdc_wait_event returned %d\n", ret);
-	rbd_destroy_ops(ops);
-	return 0;
-
-fail_event:
-	ceph_osdc_cancel_event(event);
-fail:
-	rbd_destroy_ops(ops);
-	return ret;
-}
-
-/*
- * Request sync osd read
+ * Synchronous osd object method call
  */
 static int rbd_req_sync_exec(struct rbd_device *rbd_dev,
 			     const char *object_name,
 			     const char *class_name,
 			     const char *method_name,
-			     const char *data,
-			     int len,
+			     const char *outbound,
+			     size_t outbound_size,
+			     char *inbound,
+			     size_t inbound_size,
+			     int flags,
 			     u64 *ver)
 {
 	struct ceph_osd_req_op *ops;
 	int class_name_len = strlen(class_name);
 	int method_name_len = strlen(method_name);
+	int payload_size;
 	int ret;
 
-	ops = rbd_create_rw_ops(1, CEPH_OSD_OP_CALL,
-				    class_name_len + method_name_len + len);
+	/*
+	 * Any input parameters required by the method we're calling
+	 * will be sent along with the class and method names as
+	 * part of the message payload.  That data and its size are
+	 * supplied via the indata and indata_len fields (named from
+	 * the perspective of the server side) in the OSD request
+	 * operation.
+	 */
+	payload_size = class_name_len + method_name_len + outbound_size;
+	ops = rbd_create_rw_ops(1, CEPH_OSD_OP_CALL, payload_size);
 	if (!ops)
 		return -ENOMEM;
 
@@ -1397,14 +1424,14 @@ static int rbd_req_sync_exec(struct rbd_device *rbd_dev,
 	ops[0].cls.method_name = method_name;
 	ops[0].cls.method_len = (__u8) method_name_len;
 	ops[0].cls.argc = 0;
-	ops[0].cls.indata = data;
-	ops[0].cls.indata_len = len;
+	ops[0].cls.indata = outbound;
+	ops[0].cls.indata_len = outbound_size;
 
 	ret = rbd_req_sync_op(rbd_dev, NULL,
 			       CEPH_NOSNAP,
-			       CEPH_OSD_FLAG_WRITE | CEPH_OSD_FLAG_ONDISK,
-			       ops,
-			       object_name, 0, 0, NULL, NULL, ver);
+			       flags, ops,
+			       object_name, 0, inbound_size, inbound,
+			       NULL, ver);
 
 	rbd_destroy_ops(ops);
 
@@ -1446,10 +1473,6 @@ static void rbd_rq_fn(struct request_queue *q)
 		struct rbd_req_coll *coll;
 		struct ceph_snap_context *snapc;
 
-		/* peek at request from block layer */
-		if (!rq)
-			break;
-
 		dout("fetched request\n");
 
 		/* filter out block requests we don't understand */
@@ -1464,7 +1487,7 @@ static void rbd_rq_fn(struct request_queue *q)
 		size = blk_rq_bytes(rq);
 		ofs = blk_rq_pos(rq) * SECTOR_SIZE;
 		rq_bio = rq->bio;
-		if (do_write && rbd_dev->read_only) {
+		if (do_write && rbd_dev->mapping.read_only) {
 			__blk_end_request_all(rq, -EROFS);
 			continue;
 		}
@@ -1473,7 +1496,8 @@ static void rbd_rq_fn(struct request_queue *q)
 
 		down_read(&rbd_dev->header_rwsem);
 
-		if (rbd_dev->snap_id != CEPH_NOSNAP && !rbd_dev->snap_exists) {
+		if (rbd_dev->mapping.snap_id != CEPH_NOSNAP &&
+				!rbd_dev->mapping.snap_exists) {
 			up_read(&rbd_dev->header_rwsem);
 			dout("request for non-existent snapshot");
 			spin_lock_irq(q->queue_lock);
@@ -1490,6 +1514,12 @@ static void rbd_rq_fn(struct request_queue *q)
 		     size, (unsigned long long) blk_rq_pos(rq) * SECTOR_SIZE);
 
 		num_segs = rbd_get_num_segments(&rbd_dev->header, ofs, size);
+		if (num_segs <= 0) {
+			spin_lock_irq(q->queue_lock);
+			__blk_end_request_all(rq, num_segs);
+			ceph_put_snap_context(snapc);
+			continue;
+		}
 		coll = rbd_alloc_coll(num_segs);
 		if (!coll) {
 			spin_lock_irq(q->queue_lock);
@@ -1501,10 +1531,7 @@ static void rbd_rq_fn(struct request_queue *q)
 		do {
 			/* a bio clone to be passed down to OSD req */
 			dout("rq->bio->bi_vcnt=%hu\n", rq->bio->bi_vcnt);
-			op_size = rbd_get_segment(&rbd_dev->header,
-						  rbd_dev->header.object_prefix,
-						  ofs, size,
-						  NULL, NULL);
+			op_size = rbd_segment_length(rbd_dev, ofs, size);
 			kref_get(&coll->kref);
 			bio = bio_chain_clone(&rq_bio, &next_bio, &bp,
 					      op_size, GFP_ATOMIC);
@@ -1524,7 +1551,7 @@ static void rbd_rq_fn(struct request_queue *q)
 					      coll, cur_seg);
 			else
 				rbd_req_read(rq, rbd_dev,
-					     rbd_dev->snap_id,
+					     rbd_dev->mapping.snap_id,
 					     ofs,
 					     op_size, bio,
 					     coll, cur_seg);
@@ -1580,8 +1607,6 @@ static void rbd_free_disk(struct rbd_device *rbd_dev)
 	if (!disk)
 		return;
 
-	rbd_header_free(&rbd_dev->header);
-
 	if (disk->flags & GENHD_FL_UP)
 		del_gendisk(disk);
 	if (disk->queue)
@@ -1590,105 +1615,96 @@ static void rbd_free_disk(struct rbd_device *rbd_dev)
 }
 
 /*
- * reload the ondisk the header 
+ * Read the complete header for the given rbd device.
+ *
+ * Returns a pointer to a dynamically-allocated buffer containing
+ * the complete and validated header.  Caller can pass the address
+ * of a variable that will be filled in with the version of the
+ * header object at the time it was read.
+ *
+ * Returns a pointer-coded errno if a failure occurs.
  */
-static int rbd_read_header(struct rbd_device *rbd_dev,
-			   struct rbd_image_header *header)
+static struct rbd_image_header_ondisk *
+rbd_dev_v1_header_read(struct rbd_device *rbd_dev, u64 *version)
 {
-	ssize_t rc;
-	struct rbd_image_header_ondisk *dh;
+	struct rbd_image_header_ondisk *ondisk = NULL;
 	u32 snap_count = 0;
-	u64 ver;
-	size_t len;
+	u64 names_size = 0;
+	u32 want_count;
+	int ret;
 
 	/*
-	 * First reads the fixed-size header to determine the number
-	 * of snapshots, then re-reads it, along with all snapshot
-	 * records as well as their stored names.
+	 * The complete header will include an array of its 64-bit
+	 * snapshot ids, followed by the names of those snapshots as
+	 * a contiguous block of NUL-terminated strings.  Note that
+	 * the number of snapshots could change by the time we read
+	 * it in, in which case we re-read it.
 	 */
-	len = sizeof (*dh);
-	while (1) {
-		dh = kmalloc(len, GFP_KERNEL);
-		if (!dh)
-			return -ENOMEM;
-
-		rc = rbd_req_sync_read(rbd_dev,
-				       CEPH_NOSNAP,
+	do {
+		size_t size;
+
+		kfree(ondisk);
+
+		size = sizeof (*ondisk);
+		size += snap_count * sizeof (struct rbd_image_snap_ondisk);
+		size += names_size;
+		ondisk = kmalloc(size, GFP_KERNEL);
+		if (!ondisk)
+			return ERR_PTR(-ENOMEM);
+
+		ret = rbd_req_sync_read(rbd_dev, CEPH_NOSNAP,
 				       rbd_dev->header_name,
-				       0, len,
-				       (char *)dh, &ver);
-		if (rc < 0)
-			goto out_dh;
-
-		rc = rbd_header_from_disk(header, dh, snap_count);
-		if (rc < 0) {
-			if (rc == -ENXIO)
-				pr_warning("unrecognized header format"
-					   " for image %s\n",
-					   rbd_dev->image_name);
-			goto out_dh;
+				       0, size,
+				       (char *) ondisk, version);
+
+		if (ret < 0)
+			goto out_err;
+		if (WARN_ON((size_t) ret < size)) {
+			ret = -ENXIO;
+			pr_warning("short header read for image %s"
+					" (want %zd got %d)\n",
+				rbd_dev->image_name, size, ret);
+			goto out_err;
+		}
+		if (!rbd_dev_ondisk_valid(ondisk)) {
+			ret = -ENXIO;
+			pr_warning("invalid header for image %s\n",
+				rbd_dev->image_name);
+			goto out_err;
 		}
 
-		if (snap_count == header->total_snaps)
-			break;
+		names_size = le64_to_cpu(ondisk->snap_names_len);
+		want_count = snap_count;
+		snap_count = le32_to_cpu(ondisk->snap_count);
+	} while (snap_count != want_count);
 
-		snap_count = header->total_snaps;
-		len = sizeof (*dh) +
-			snap_count * sizeof(struct rbd_image_snap_ondisk) +
-			header->snap_names_len;
+	return ondisk;
 
-		rbd_header_free(header);
-		kfree(dh);
-	}
-	header->obj_version = ver;
+out_err:
+	kfree(ondisk);
 
-out_dh:
-	kfree(dh);
-	return rc;
+	return ERR_PTR(ret);
 }
 
 /*
- * create a snapshot
+ * reload the ondisk the header
  */
-static int rbd_header_add_snap(struct rbd_device *rbd_dev,
-			       const char *snap_name,
-			       gfp_t gfp_flags)
+static int rbd_read_header(struct rbd_device *rbd_dev,
+			   struct rbd_image_header *header)
 {
-	int name_len = strlen(snap_name);
-	u64 new_snapid;
+	struct rbd_image_header_ondisk *ondisk;
+	u64 ver = 0;
 	int ret;
-	void *data, *p, *e;
-	struct ceph_mon_client *monc;
-
-	/* we should create a snapshot only if we're pointing at the head */
-	if (rbd_dev->snap_id != CEPH_NOSNAP)
-		return -EINVAL;
 
-	monc = &rbd_dev->rbd_client->client->monc;
-	ret = ceph_monc_create_snapid(monc, rbd_dev->pool_id, &new_snapid);
-	dout("created snapid=%llu\n", (unsigned long long) new_snapid);
-	if (ret < 0)
-		return ret;
-
-	data = kmalloc(name_len + 16, gfp_flags);
-	if (!data)
-		return -ENOMEM;
+	ondisk = rbd_dev_v1_header_read(rbd_dev, &ver);
+	if (IS_ERR(ondisk))
+		return PTR_ERR(ondisk);
+	ret = rbd_header_from_disk(header, ondisk);
+	if (ret >= 0)
+		header->obj_version = ver;
+	kfree(ondisk);
 
-	p = data;
-	e = data + name_len + 16;
-
-	ceph_encode_string_safe(&p, e, snap_name, name_len, bad);
-	ceph_encode_64_safe(&p, e, new_snapid, bad);
-
-	ret = rbd_req_sync_exec(rbd_dev, rbd_dev->header_name,
-				"rbd", "snap_add",
-				data, p - data, NULL);
-
-	kfree(data);
-
-	return ret < 0 ? ret : 0;
-bad:
-	return -ERANGE;
+	return ret;
 }
 
 static void __rbd_remove_all_snaps(struct rbd_device *rbd_dev)
@@ -1715,11 +1731,15 @@ static int __rbd_refresh_header(struct rbd_device *rbd_dev, u64 *hver)
 	down_write(&rbd_dev->header_rwsem);
 
 	/* resized? */
-	if (rbd_dev->snap_id == CEPH_NOSNAP) {
+	if (rbd_dev->mapping.snap_id == CEPH_NOSNAP) {
 		sector_t size = (sector_t) h.image_size / SECTOR_SIZE;
 
-		dout("setting size to %llu sectors", (unsigned long long) size);
-		set_capacity(rbd_dev->disk, size);
+		if (size != (sector_t) rbd_dev->mapping.size) {
+			dout("setting size to %llu sectors",
+				(unsigned long long) size);
+			rbd_dev->mapping.size = (u64) size;
+			set_capacity(rbd_dev->disk, size);
+		}
 	}
 
 	/* rbd_dev->header.object_prefix shouldn't change */
@@ -1732,16 +1752,16 @@ static int __rbd_refresh_header(struct rbd_device *rbd_dev, u64 *hver)
 		*hver = h.obj_version;
 	rbd_dev->header.obj_version = h.obj_version;
 	rbd_dev->header.image_size = h.image_size;
-	rbd_dev->header.total_snaps = h.total_snaps;
 	rbd_dev->header.snapc = h.snapc;
 	rbd_dev->header.snap_names = h.snap_names;
-	rbd_dev->header.snap_names_len = h.snap_names_len;
 	rbd_dev->header.snap_sizes = h.snap_sizes;
 	/* Free the extra copy of the object prefix */
 	WARN_ON(strcmp(rbd_dev->header.object_prefix, h.object_prefix));
 	kfree(h.object_prefix);
 
-	ret = __rbd_init_snaps_header(rbd_dev);
+	ret = rbd_dev_snaps_update(rbd_dev);
+	if (!ret)
+		ret = rbd_dev_snaps_register(rbd_dev);
 
 	up_write(&rbd_dev->header_rwsem);
 
@@ -1763,29 +1783,12 @@ static int rbd_init_disk(struct rbd_device *rbd_dev)
 {
 	struct gendisk *disk;
 	struct request_queue *q;
-	int rc;
 	u64 segment_size;
-	u64 total_size = 0;
-
-	/* contact OSD, request size info about the object being mapped */
-	rc = rbd_read_header(rbd_dev, &rbd_dev->header);
-	if (rc)
-		return rc;
-
-	/* no need to lock here, as rbd_dev is not registered yet */
-	rc = __rbd_init_snaps_header(rbd_dev);
-	if (rc)
-		return rc;
-
-	rc = rbd_header_set_snap(rbd_dev, &total_size);
-	if (rc)
-		return rc;
 
 	/* create gendisk info */
-	rc = -ENOMEM;
 	disk = alloc_disk(RBD_MINORS_PER_MAJOR);
 	if (!disk)
-		goto out;
+		return -ENOMEM;
 
 	snprintf(disk->disk_name, sizeof(disk->disk_name), RBD_DRV_NAME "%d",
 		 rbd_dev->dev_id);
@@ -1795,7 +1798,6 @@ static int rbd_init_disk(struct rbd_device *rbd_dev)
 	disk->private_data = rbd_dev;
 
 	/* init rq */
-	rc = -ENOMEM;
 	q = blk_init_queue(rbd_rq_fn, &rbd_dev->lock);
 	if (!q)
 		goto out_disk;
@@ -1816,20 +1818,14 @@ static int rbd_init_disk(struct rbd_device *rbd_dev)
 	q->queuedata = rbd_dev;
 
 	rbd_dev->disk = disk;
-	rbd_dev->q = q;
 
-	/* finally, announce the disk to the world */
-	set_capacity(disk, total_size / SECTOR_SIZE);
-	add_disk(disk);
+	set_capacity(rbd_dev->disk, rbd_dev->mapping.size / SECTOR_SIZE);
 
-	pr_info("%s: added with size 0x%llx\n",
-		disk->disk_name, (unsigned long long)total_size);
 	return 0;
-
 out_disk:
 	put_disk(disk);
-out:
-	return rc;
+
+	return -ENOMEM;
 }
 
 /*
@@ -1854,6 +1850,19 @@ static ssize_t rbd_size_show(struct device *dev,
 	return sprintf(buf, "%llu\n", (unsigned long long) size * SECTOR_SIZE);
 }
 
+/*
+ * Note this shows the features for whatever's mapped, which is not
+ * necessarily the base image.
+ */
+static ssize_t rbd_features_show(struct device *dev,
+			     struct device_attribute *attr, char *buf)
+{
+	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
+
+	return sprintf(buf, "0x%016llx\n",
+			(unsigned long long) rbd_dev->mapping.features);
+}
+
 static ssize_t rbd_major_show(struct device *dev,
 			      struct device_attribute *attr, char *buf)
 {
@@ -1895,13 +1904,25 @@ static ssize_t rbd_name_show(struct device *dev,
 	return sprintf(buf, "%s\n", rbd_dev->image_name);
 }
 
+static ssize_t rbd_image_id_show(struct device *dev,
+			     struct device_attribute *attr, char *buf)
+{
+	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
+
+	return sprintf(buf, "%s\n", rbd_dev->image_id);
+}
+
+/*
+ * Shows the name of the currently-mapped snapshot (or
+ * RBD_SNAP_HEAD_NAME for the base image).
+ */
 static ssize_t rbd_snap_show(struct device *dev,
 			     struct device_attribute *attr,
 			     char *buf)
 {
 	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
 
-	return sprintf(buf, "%s\n", rbd_dev->snap_name);
+	return sprintf(buf, "%s\n", rbd_dev->mapping.snap_name);
 }
 
 static ssize_t rbd_image_refresh(struct device *dev,
@@ -1918,25 +1939,27 @@ static ssize_t rbd_image_refresh(struct device *dev,
 }
 
 static DEVICE_ATTR(size, S_IRUGO, rbd_size_show, NULL);
+static DEVICE_ATTR(features, S_IRUGO, rbd_features_show, NULL);
 static DEVICE_ATTR(major, S_IRUGO, rbd_major_show, NULL);
 static DEVICE_ATTR(client_id, S_IRUGO, rbd_client_id_show, NULL);
 static DEVICE_ATTR(pool, S_IRUGO, rbd_pool_show, NULL);
 static DEVICE_ATTR(pool_id, S_IRUGO, rbd_pool_id_show, NULL);
 static DEVICE_ATTR(name, S_IRUGO, rbd_name_show, NULL);
+static DEVICE_ATTR(image_id, S_IRUGO, rbd_image_id_show, NULL);
 static DEVICE_ATTR(refresh, S_IWUSR, NULL, rbd_image_refresh);
 static DEVICE_ATTR(current_snap, S_IRUGO, rbd_snap_show, NULL);
-static DEVICE_ATTR(create_snap, S_IWUSR, NULL, rbd_snap_add);
 
 static struct attribute *rbd_attrs[] = {
 	&dev_attr_size.attr,
+	&dev_attr_features.attr,
 	&dev_attr_major.attr,
 	&dev_attr_client_id.attr,
 	&dev_attr_pool.attr,
 	&dev_attr_pool_id.attr,
 	&dev_attr_name.attr,
+	&dev_attr_image_id.attr,
 	&dev_attr_current_snap.attr,
 	&dev_attr_refresh.attr,
-	&dev_attr_create_snap.attr,
 	NULL
 };
 
@@ -1982,12 +2005,24 @@ static ssize_t rbd_snap_id_show(struct device *dev,
 	return sprintf(buf, "%llu\n", (unsigned long long)snap->id);
 }
 
+static ssize_t rbd_snap_features_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	struct rbd_snap *snap = container_of(dev, struct rbd_snap, dev);
+
+	return sprintf(buf, "0x%016llx\n",
+			(unsigned long long) snap->features);
+}
+
 static DEVICE_ATTR(snap_size, S_IRUGO, rbd_snap_size_show, NULL);
 static DEVICE_ATTR(snap_id, S_IRUGO, rbd_snap_id_show, NULL);
+static DEVICE_ATTR(snap_features, S_IRUGO, rbd_snap_features_show, NULL);
 
 static struct attribute *rbd_snap_attrs[] = {
 	&dev_attr_snap_size.attr,
 	&dev_attr_snap_id.attr,
+	&dev_attr_snap_features.attr,
 	NULL,
 };
 
@@ -2012,10 +2047,21 @@ static struct device_type rbd_snap_device_type = {
 	.release	= rbd_snap_dev_release,
 };
 
+static bool rbd_snap_registered(struct rbd_snap *snap)
+{
+	bool ret = snap->dev.type == &rbd_snap_device_type;
+	bool reg = device_is_registered(&snap->dev);
+
+	rbd_assert(!ret ^ reg);
+
+	return ret;
+}
+
 static void __rbd_remove_snap_dev(struct rbd_snap *snap)
 {
 	list_del(&snap->node);
-	device_unregister(&snap->dev);
+	if (device_is_registered(&snap->dev))
+		device_unregister(&snap->dev);
 }
 
 static int rbd_register_snap_dev(struct rbd_snap *snap,
@@ -2028,13 +2074,17 @@ static int rbd_register_snap_dev(struct rbd_snap *snap,
 	dev->parent = parent;
 	dev->release = rbd_snap_dev_release;
 	dev_set_name(dev, "snap_%s", snap->name);
+	dout("%s: registering device for snapshot %s\n", __func__, snap->name);
+
 	ret = device_register(dev);
 
 	return ret;
 }
 
 static struct rbd_snap *__rbd_add_snap_dev(struct rbd_device *rbd_dev,
-					      int i, const char *name)
+						const char *snap_name,
+						u64 snap_id, u64 snap_size,
+						u64 snap_features)
 {
 	struct rbd_snap *snap;
 	int ret;
@@ -2044,17 +2094,13 @@ static struct rbd_snap *__rbd_add_snap_dev(struct rbd_device *rbd_dev,
 		return ERR_PTR(-ENOMEM);
 
 	ret = -ENOMEM;
-	snap->name = kstrdup(name, GFP_KERNEL);
+	snap->name = kstrdup(snap_name, GFP_KERNEL);
 	if (!snap->name)
 		goto err;
 
-	snap->size = rbd_dev->header.snap_sizes[i];
-	snap->id = rbd_dev->header.snapc->snaps[i];
-	if (device_is_registered(&rbd_dev->dev)) {
-		ret = rbd_register_snap_dev(snap, &rbd_dev->dev);
-		if (ret < 0)
-			goto err;
-	}
+	snap->id = snap_id;
+	snap->size = snap_size;
+	snap->features = snap_features;
 
 	return snap;
 
@@ -2065,128 +2111,439 @@ err:
 	return ERR_PTR(ret);
 }
 
+static char *rbd_dev_v1_snap_info(struct rbd_device *rbd_dev, u32 which,
+		u64 *snap_size, u64 *snap_features)
+{
+	char *snap_name;
+
+	rbd_assert(which < rbd_dev->header.snapc->num_snaps);
+
+	*snap_size = rbd_dev->header.snap_sizes[which];
+	*snap_features = 0;	/* No features for v1 */
+
+	/* Skip over names until we find the one we are looking for */
+
+	snap_name = rbd_dev->header.snap_names;
+	while (which--)
+		snap_name += strlen(snap_name) + 1;
+
+	return snap_name;
+}
+
 /*
- * search for the previous snap in a null delimited string list
+ * Get the size and object order for an image snapshot, or if
+ * snap_id is CEPH_NOSNAP, gets this information for the base
+ * image.
  */
-const char *rbd_prev_snap_name(const char *name, const char *start)
+static int _rbd_dev_v2_snap_size(struct rbd_device *rbd_dev, u64 snap_id,
+				u8 *order, u64 *snap_size)
 {
-	if (name < start + 2)
-		return NULL;
+	__le64 snapid = cpu_to_le64(snap_id);
+	int ret;
+	struct {
+		u8 order;
+		__le64 size;
+	} __attribute__ ((packed)) size_buf = { 0 };
+
+	ret = rbd_req_sync_exec(rbd_dev, rbd_dev->header_name,
+				"rbd", "get_size",
+				(char *) &snapid, sizeof (snapid),
+				(char *) &size_buf, sizeof (size_buf),
+				CEPH_OSD_FLAG_READ, NULL);
+	dout("%s: rbd_req_sync_exec returned %d\n", __func__, ret);
+	if (ret < 0)
+		return ret;
+
+	*order = size_buf.order;
+	*snap_size = le64_to_cpu(size_buf.size);
+
+	dout("  snap_id 0x%016llx order = %u, snap_size = %llu\n",
+		(unsigned long long) snap_id, (unsigned int) *order,
+		(unsigned long long) *snap_size);
+
+	return 0;
+}
+
+static int rbd_dev_v2_image_size(struct rbd_device *rbd_dev)
+{
+	return _rbd_dev_v2_snap_size(rbd_dev, CEPH_NOSNAP,
+					&rbd_dev->header.obj_order,
+					&rbd_dev->header.image_size);
+}
+
+static int rbd_dev_v2_object_prefix(struct rbd_device *rbd_dev)
+{
+	void *reply_buf;
+	int ret;
+	void *p;
+
+	reply_buf = kzalloc(RBD_OBJ_PREFIX_LEN_MAX, GFP_KERNEL);
+	if (!reply_buf)
+		return -ENOMEM;
+
+	ret = rbd_req_sync_exec(rbd_dev, rbd_dev->header_name,
+				"rbd", "get_object_prefix",
+				NULL, 0,
+				reply_buf, RBD_OBJ_PREFIX_LEN_MAX,
+				CEPH_OSD_FLAG_READ, NULL);
+	dout("%s: rbd_req_sync_exec returned %d\n", __func__, ret);
+	if (ret < 0)
+		goto out;
+
+	p = reply_buf;
+	rbd_dev->header.object_prefix = ceph_extract_encoded_string(&p,
+						p + RBD_OBJ_PREFIX_LEN_MAX,
+						NULL, GFP_NOIO);
+
+	if (IS_ERR(rbd_dev->header.object_prefix)) {
+		ret = PTR_ERR(rbd_dev->header.object_prefix);
+		rbd_dev->header.object_prefix = NULL;
+	} else {
+		dout("  object_prefix = %s\n", rbd_dev->header.object_prefix);
+	}
 
-	name -= 2;
-	while (*name) {
-		if (name == start)
-			return start;
-		name--;
+out:
+	kfree(reply_buf);
+
+	return ret;
+}
+
+static int _rbd_dev_v2_snap_features(struct rbd_device *rbd_dev, u64 snap_id,
+		u64 *snap_features)
+{
+	__le64 snapid = cpu_to_le64(snap_id);
+	struct {
+		__le64 features;
+		__le64 incompat;
+	} features_buf = { 0 };
+	int ret;
+
+	ret = rbd_req_sync_exec(rbd_dev, rbd_dev->header_name,
+				"rbd", "get_features",
+				(char *) &snapid, sizeof (snapid),
+				(char *) &features_buf, sizeof (features_buf),
+				CEPH_OSD_FLAG_READ, NULL);
+	dout("%s: rbd_req_sync_exec returned %d\n", __func__, ret);
+	if (ret < 0)
+		return ret;
+	*snap_features = le64_to_cpu(features_buf.features);
+
+	dout("  snap_id 0x%016llx features = 0x%016llx incompat = 0x%016llx\n",
+		(unsigned long long) snap_id,
+		(unsigned long long) *snap_features,
+		(unsigned long long) le64_to_cpu(features_buf.incompat));
+
+	return 0;
+}
+
+static int rbd_dev_v2_features(struct rbd_device *rbd_dev)
+{
+	return _rbd_dev_v2_snap_features(rbd_dev, CEPH_NOSNAP,
+						&rbd_dev->header.features);
+}
+
+static int rbd_dev_v2_snap_context(struct rbd_device *rbd_dev, u64 *ver)
+{
+	size_t size;
+	int ret;
+	void *reply_buf;
+	void *p;
+	void *end;
+	u64 seq;
+	u32 snap_count;
+	struct ceph_snap_context *snapc;
+	u32 i;
+
+	/*
+	 * We'll need room for the seq value (maximum snapshot id),
+	 * snapshot count, and array of that many snapshot ids.
+	 * For now we have a fixed upper limit on the number we're
+	 * prepared to receive.
+	 */
+	size = sizeof (__le64) + sizeof (__le32) +
+			RBD_MAX_SNAP_COUNT * sizeof (__le64);
+	reply_buf = kzalloc(size, GFP_KERNEL);
+	if (!reply_buf)
+		return -ENOMEM;
+
+	ret = rbd_req_sync_exec(rbd_dev, rbd_dev->header_name,
+				"rbd", "get_snapcontext",
+				NULL, 0,
+				reply_buf, size,
+				CEPH_OSD_FLAG_READ, ver);
+	dout("%s: rbd_req_sync_exec returned %d\n", __func__, ret);
+	if (ret < 0)
+		goto out;
+
+	ret = -ERANGE;
+	p = reply_buf;
+	end = (char *) reply_buf + size;
+	ceph_decode_64_safe(&p, end, seq, out);
+	ceph_decode_32_safe(&p, end, snap_count, out);
+
+	/*
+	 * Make sure the reported number of snapshot ids wouldn't go
+	 * beyond the end of our buffer.  But before checking that,
+	 * make sure the computed size of the snapshot context we
+	 * allocate is representable in a size_t.
+	 */
+	if (snap_count > (SIZE_MAX - sizeof (struct ceph_snap_context))
+				 / sizeof (u64)) {
+		ret = -EINVAL;
+		goto out;
 	}
-	return name + 1;
+	if (!ceph_has_room(&p, end, snap_count * sizeof (__le64)))
+		goto out;
+
+	size = sizeof (struct ceph_snap_context) +
+				snap_count * sizeof (snapc->snaps[0]);
+	snapc = kmalloc(size, GFP_KERNEL);
+	if (!snapc) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	atomic_set(&snapc->nref, 1);
+	snapc->seq = seq;
+	snapc->num_snaps = snap_count;
+	for (i = 0; i < snap_count; i++)
+		snapc->snaps[i] = ceph_decode_64(&p);
+
+	rbd_dev->header.snapc = snapc;
+
+	dout("  snap context seq = %llu, snap_count = %u\n",
+		(unsigned long long) seq, (unsigned int) snap_count);
+
+out:
+	kfree(reply_buf);
+
+	return 0;
 }
 
-/*
- * compare the old list of snapshots that we have to what's in the header
- * and update it accordingly. Note that the header holds the snapshots
- * in a reverse order (from newest to oldest) and we need to go from
- * older to new so that we don't get a duplicate snap name when
- * doing the process (e.g., removed snapshot and recreated a new
- * one with the same name.
- */
-static int __rbd_init_snaps_header(struct rbd_device *rbd_dev)
+static char *rbd_dev_v2_snap_name(struct rbd_device *rbd_dev, u32 which)
 {
-	const char *name, *first_name;
-	int i = rbd_dev->header.total_snaps;
-	struct rbd_snap *snap, *old_snap = NULL;
-	struct list_head *p, *n;
+	size_t size;
+	void *reply_buf;
+	__le64 snap_id;
+	int ret;
+	void *p;
+	void *end;
+	size_t snap_name_len;
+	char *snap_name;
+
+	size = sizeof (__le32) + RBD_MAX_SNAP_NAME_LEN;
+	reply_buf = kmalloc(size, GFP_KERNEL);
+	if (!reply_buf)
+		return ERR_PTR(-ENOMEM);
 
-	first_name = rbd_dev->header.snap_names;
-	name = first_name + rbd_dev->header.snap_names_len;
+	snap_id = cpu_to_le64(rbd_dev->header.snapc->snaps[which]);
+	ret = rbd_req_sync_exec(rbd_dev, rbd_dev->header_name,
+				"rbd", "get_snapshot_name",
+				(char *) &snap_id, sizeof (snap_id),
+				reply_buf, size,
+				CEPH_OSD_FLAG_READ, NULL);
+	dout("%s: rbd_req_sync_exec returned %d\n", __func__, ret);
+	if (ret < 0)
+		goto out;
 
-	list_for_each_prev_safe(p, n, &rbd_dev->snaps) {
-		u64 cur_id;
+	p = reply_buf;
+	end = (char *) reply_buf + size;
+	snap_name_len = 0;
+	snap_name = ceph_extract_encoded_string(&p, end, &snap_name_len,
+				GFP_KERNEL);
+	if (IS_ERR(snap_name)) {
+		ret = PTR_ERR(snap_name);
+		goto out;
+	} else {
+		dout("  snap_id 0x%016llx snap_name = %s\n",
+			(unsigned long long) le64_to_cpu(snap_id), snap_name);
+	}
+	kfree(reply_buf);
 
-		old_snap = list_entry(p, struct rbd_snap, node);
+	return snap_name;
+out:
+	kfree(reply_buf);
 
-		if (i)
-			cur_id = rbd_dev->header.snapc->snaps[i - 1];
+	return ERR_PTR(ret);
+}
 
-		if (!i || old_snap->id < cur_id) {
-			/*
-			 * old_snap->id was skipped, thus was
-			 * removed.  If this rbd_dev is mapped to
-			 * the removed snapshot, record that it no
-			 * longer exists, to prevent further I/O.
-			 */
-			if (rbd_dev->snap_id == old_snap->id)
-				rbd_dev->snap_exists = false;
-			__rbd_remove_snap_dev(old_snap);
-			continue;
-		}
-		if (old_snap->id == cur_id) {
-			/* we have this snapshot already */
-			i--;
-			name = rbd_prev_snap_name(name, first_name);
+static char *rbd_dev_v2_snap_info(struct rbd_device *rbd_dev, u32 which,
+		u64 *snap_size, u64 *snap_features)
+{
+	__le64 snap_id;
+	u8 order;
+	int ret;
+
+	snap_id = rbd_dev->header.snapc->snaps[which];
+	ret = _rbd_dev_v2_snap_size(rbd_dev, snap_id, &order, snap_size);
+	if (ret)
+		return ERR_PTR(ret);
+	ret = _rbd_dev_v2_snap_features(rbd_dev, snap_id, snap_features);
+	if (ret)
+		return ERR_PTR(ret);
+
+	return rbd_dev_v2_snap_name(rbd_dev, which);
+}
+
+static char *rbd_dev_snap_info(struct rbd_device *rbd_dev, u32 which,
+		u64 *snap_size, u64 *snap_features)
+{
+	if (rbd_dev->image_format == 1)
+		return rbd_dev_v1_snap_info(rbd_dev, which,
+					snap_size, snap_features);
+	if (rbd_dev->image_format == 2)
+		return rbd_dev_v2_snap_info(rbd_dev, which,
+					snap_size, snap_features);
+	return ERR_PTR(-EINVAL);
+}
+
+/*
+ * Scan the rbd device's current snapshot list and compare it to the
+ * newly-received snapshot context.  Remove any existing snapshots
+ * not present in the new snapshot context.  Add a new snapshot for
+ * any snaphots in the snapshot context not in the current list.
+ * And verify there are no changes to snapshots we already know
+ * about.
+ *
+ * Assumes the snapshots in the snapshot context are sorted by
+ * snapshot id, highest id first.  (Snapshots in the rbd_dev's list
+ * are also maintained in that order.)
+ */
+static int rbd_dev_snaps_update(struct rbd_device *rbd_dev)
+{
+	struct ceph_snap_context *snapc = rbd_dev->header.snapc;
+	const u32 snap_count = snapc->num_snaps;
+	struct list_head *head = &rbd_dev->snaps;
+	struct list_head *links = head->next;
+	u32 index = 0;
+
+	dout("%s: snap count is %u\n", __func__, (unsigned int) snap_count);
+	while (index < snap_count || links != head) {
+		u64 snap_id;
+		struct rbd_snap *snap;
+		char *snap_name;
+		u64 snap_size = 0;
+		u64 snap_features = 0;
+
+		snap_id = index < snap_count ? snapc->snaps[index]
+					     : CEPH_NOSNAP;
+		snap = links != head ? list_entry(links, struct rbd_snap, node)
+				     : NULL;
+		rbd_assert(!snap || snap->id != CEPH_NOSNAP);
+
+		if (snap_id == CEPH_NOSNAP || (snap && snap->id > snap_id)) {
+			struct list_head *next = links->next;
+
+			/* Existing snapshot not in the new snap context */
+
+			if (rbd_dev->mapping.snap_id == snap->id)
+				rbd_dev->mapping.snap_exists = false;
+			__rbd_remove_snap_dev(snap);
+			dout("%ssnap id %llu has been removed\n",
+				rbd_dev->mapping.snap_id == snap->id ?
+								"mapped " : "",
+				(unsigned long long) snap->id);
+
+			/* Done with this list entry; advance */
+
+			links = next;
 			continue;
 		}
-		for (; i > 0;
-		     i--, name = rbd_prev_snap_name(name, first_name)) {
-			if (!name) {
-				WARN_ON(1);
-				return -EINVAL;
+
+		snap_name = rbd_dev_snap_info(rbd_dev, index,
+					&snap_size, &snap_features);
+		if (IS_ERR(snap_name))
+			return PTR_ERR(snap_name);
+
+		dout("entry %u: snap_id = %llu\n", (unsigned int) snap_count,
+			(unsigned long long) snap_id);
+		if (!snap || (snap_id != CEPH_NOSNAP && snap->id < snap_id)) {
+			struct rbd_snap *new_snap;
+
+			/* We haven't seen this snapshot before */
+
+			new_snap = __rbd_add_snap_dev(rbd_dev, snap_name,
+					snap_id, snap_size, snap_features);
+			if (IS_ERR(new_snap)) {
+				int err = PTR_ERR(new_snap);
+
+				dout("  failed to add dev, error %d\n", err);
+
+				return err;
 			}
-			cur_id = rbd_dev->header.snapc->snaps[i];
-			/* snapshot removal? handle it above */
-			if (cur_id >= old_snap->id)
-				break;
-			/* a new snapshot */
-			snap = __rbd_add_snap_dev(rbd_dev, i - 1, name);
-			if (IS_ERR(snap))
-				return PTR_ERR(snap);
-
-			/* note that we add it backward so using n and not p */
-			list_add(&snap->node, n);
-			p = &snap->node;
+
+			/* New goes before existing, or at end of list */
+
+			dout("  added dev%s\n", snap ? "" : " at end\n");
+			if (snap)
+				list_add_tail(&new_snap->node, &snap->node);
+			else
+				list_add_tail(&new_snap->node, head);
+		} else {
+			/* Already have this one */
+
+			dout("  already present\n");
+
+			rbd_assert(snap->size == snap_size);
+			rbd_assert(!strcmp(snap->name, snap_name));
+			rbd_assert(snap->features == snap_features);
+
+			/* Done with this list entry; advance */
+
+			links = links->next;
 		}
+
+		/* Advance to the next entry in the snapshot context */
+
+		index++;
 	}
-	/* we're done going over the old snap list, just add what's left */
-	for (; i > 0; i--) {
-		name = rbd_prev_snap_name(name, first_name);
-		if (!name) {
-			WARN_ON(1);
-			return -EINVAL;
+	dout("%s: done\n", __func__);
+
+	return 0;
+}
+
+/*
+ * Scan the list of snapshots and register the devices for any that
+ * have not already been registered.
+ */
+static int rbd_dev_snaps_register(struct rbd_device *rbd_dev)
+{
+	struct rbd_snap *snap;
+	int ret = 0;
+
+	dout("%s called\n", __func__);
+	if (WARN_ON(!device_is_registered(&rbd_dev->dev)))
+		return -EIO;
+
+	list_for_each_entry(snap, &rbd_dev->snaps, node) {
+		if (!rbd_snap_registered(snap)) {
+			ret = rbd_register_snap_dev(snap, &rbd_dev->dev);
+			if (ret < 0)
+				break;
 		}
-		snap = __rbd_add_snap_dev(rbd_dev, i - 1, name);
-		if (IS_ERR(snap))
-			return PTR_ERR(snap);
-		list_add(&snap->node, &rbd_dev->snaps);
 	}
+	dout("%s: returning %d\n", __func__, ret);
 
-	return 0;
+	return ret;
 }
 
 static int rbd_bus_add_dev(struct rbd_device *rbd_dev)
 {
-	int ret;
 	struct device *dev;
-	struct rbd_snap *snap;
+	int ret;
 
 	mutex_lock_nested(&ctl_mutex, SINGLE_DEPTH_NESTING);
-	dev = &rbd_dev->dev;
 
+	dev = &rbd_dev->dev;
 	dev->bus = &rbd_bus_type;
 	dev->type = &rbd_device_type;
 	dev->parent = &rbd_root_dev;
 	dev->release = rbd_dev_release;
 	dev_set_name(dev, "%d", rbd_dev->dev_id);
 	ret = device_register(dev);
-	if (ret < 0)
-		goto out;
 
-	list_for_each_entry(snap, &rbd_dev->snaps, node) {
-		ret = rbd_register_snap_dev(snap, &rbd_dev->dev);
-		if (ret < 0)
-			break;
-	}
-out:
 	mutex_unlock(&ctl_mutex);
+
 	return ret;
 }
 
@@ -2211,33 +2568,37 @@ static int rbd_init_watch_dev(struct rbd_device *rbd_dev)
 	return ret;
 }
 
-static atomic64_t rbd_id_max = ATOMIC64_INIT(0);
+static atomic64_t rbd_dev_id_max = ATOMIC64_INIT(0);
 
 /*
  * Get a unique rbd identifier for the given new rbd_dev, and add
  * the rbd_dev to the global list.  The minimum rbd id is 1.
  */
-static void rbd_id_get(struct rbd_device *rbd_dev)
+static void rbd_dev_id_get(struct rbd_device *rbd_dev)
 {
-	rbd_dev->dev_id = atomic64_inc_return(&rbd_id_max);
+	rbd_dev->dev_id = atomic64_inc_return(&rbd_dev_id_max);
 
 	spin_lock(&rbd_dev_list_lock);
 	list_add_tail(&rbd_dev->node, &rbd_dev_list);
 	spin_unlock(&rbd_dev_list_lock);
+	dout("rbd_dev %p given dev id %llu\n", rbd_dev,
+		(unsigned long long) rbd_dev->dev_id);
 }
 
 /*
  * Remove an rbd_dev from the global list, and record that its
  * identifier is no longer in use.
  */
-static void rbd_id_put(struct rbd_device *rbd_dev)
+static void rbd_dev_id_put(struct rbd_device *rbd_dev)
 {
 	struct list_head *tmp;
 	int rbd_id = rbd_dev->dev_id;
 	int max_id;
 
-	BUG_ON(rbd_id < 1);
+	rbd_assert(rbd_id > 0);
 
+	dout("rbd_dev %p released dev id %llu\n", rbd_dev,
+		(unsigned long long) rbd_dev->dev_id);
 	spin_lock(&rbd_dev_list_lock);
 	list_del_init(&rbd_dev->node);
 
@@ -2245,7 +2606,7 @@ static void rbd_id_put(struct rbd_device *rbd_dev)
 	 * If the id being "put" is not the current maximum, there
 	 * is nothing special we need to do.
 	 */
-	if (rbd_id != atomic64_read(&rbd_id_max)) {
+	if (rbd_id != atomic64_read(&rbd_dev_id_max)) {
 		spin_unlock(&rbd_dev_list_lock);
 		return;
 	}
@@ -2266,12 +2627,13 @@ static void rbd_id_put(struct rbd_device *rbd_dev)
 	spin_unlock(&rbd_dev_list_lock);
 
 	/*
-	 * The max id could have been updated by rbd_id_get(), in
+	 * The max id could have been updated by rbd_dev_id_get(), in
 	 * which case it now accurately reflects the new maximum.
 	 * Be careful not to overwrite the maximum value in that
 	 * case.
 	 */
-	atomic64_cmpxchg(&rbd_id_max, rbd_id, max_id);
+	atomic64_cmpxchg(&rbd_dev_id_max, rbd_id, max_id);
+	dout("  max dev id has been reset\n");
 }
 
 /*
@@ -2360,28 +2722,31 @@ static inline char *dup_token(const char **buf, size_t *lenp)
 }
 
 /*
- * This fills in the pool_name, image_name, image_name_len, snap_name,
- * rbd_dev, rbd_md_name, and name fields of the given rbd_dev, based
- * on the list of monitor addresses and other options provided via
- * /sys/bus/rbd/add.
+ * This fills in the pool_name, image_name, image_name_len, rbd_dev,
+ * rbd_md_name, and name fields of the given rbd_dev, based on the
+ * list of monitor addresses and other options provided via
+ * /sys/bus/rbd/add.  Returns a pointer to a dynamically-allocated
+ * copy of the snapshot name to map if successful, or a
+ * pointer-coded error otherwise.
  *
  * Note: rbd_dev is assumed to have been initially zero-filled.
  */
-static int rbd_add_parse_args(struct rbd_device *rbd_dev,
-			      const char *buf,
-			      const char **mon_addrs,
-			      size_t *mon_addrs_size,
-			      char *options,
-			     size_t options_size)
+static char *rbd_add_parse_args(struct rbd_device *rbd_dev,
+				const char *buf,
+				const char **mon_addrs,
+				size_t *mon_addrs_size,
+				char *options,
+				size_t options_size)
 {
 	size_t len;
-	int ret;
+	char *err_ptr = ERR_PTR(-EINVAL);
+	char *snap_name;
 
 	/* The first four tokens are required */
 
 	len = next_token(&buf);
 	if (!len)
-		return -EINVAL;
+		return err_ptr;
 	*mon_addrs_size = len + 1;
 	*mon_addrs = buf;
 
@@ -2389,9 +2754,9 @@ static int rbd_add_parse_args(struct rbd_device *rbd_dev,
 
 	len = copy_token(&buf, options, options_size);
 	if (!len || len >= options_size)
-		return -EINVAL;
+		return err_ptr;
 
-	ret = -ENOMEM;
+	err_ptr = ERR_PTR(-ENOMEM);
 	rbd_dev->pool_name = dup_token(&buf, NULL);
 	if (!rbd_dev->pool_name)
 		goto out_err;
@@ -2400,41 +2765,227 @@ static int rbd_add_parse_args(struct rbd_device *rbd_dev,
 	if (!rbd_dev->image_name)
 		goto out_err;
 
-	/* Create the name of the header object */
+	/* Snapshot name is optional */
+	len = next_token(&buf);
+	if (!len) {
+		buf = RBD_SNAP_HEAD_NAME; /* No snapshot supplied */
+		len = sizeof (RBD_SNAP_HEAD_NAME) - 1;
+	}
+	snap_name = kmalloc(len + 1, GFP_KERNEL);
+	if (!snap_name)
+		goto out_err;
+	memcpy(snap_name, buf, len);
+	*(snap_name + len) = '\0';
 
-	rbd_dev->header_name = kmalloc(rbd_dev->image_name_len
-						+ sizeof (RBD_SUFFIX),
-					GFP_KERNEL);
-	if (!rbd_dev->header_name)
+dout("    SNAP_NAME is <%s>, len is %zd\n", snap_name, len);
+
+	return snap_name;
+
+out_err:
+	kfree(rbd_dev->image_name);
+	rbd_dev->image_name = NULL;
+	rbd_dev->image_name_len = 0;
+	kfree(rbd_dev->pool_name);
+	rbd_dev->pool_name = NULL;
+
+	return err_ptr;
+}
+
+/*
+ * An rbd format 2 image has a unique identifier, distinct from the
+ * name given to it by the user.  Internally, that identifier is
+ * what's used to specify the names of objects related to the image.
+ *
+ * A special "rbd id" object is used to map an rbd image name to its
+ * id.  If that object doesn't exist, then there is no v2 rbd image
+ * with the supplied name.
+ *
+ * This function will record the given rbd_dev's image_id field if
+ * it can be determined, and in that case will return 0.  If any
+ * errors occur a negative errno will be returned and the rbd_dev's
+ * image_id field will be unchanged (and should be NULL).
+ */
+static int rbd_dev_image_id(struct rbd_device *rbd_dev)
+{
+	int ret;
+	size_t size;
+	char *object_name;
+	void *response;
+	void *p;
+
+	/*
+	 * First, see if the format 2 image id file exists, and if
+	 * so, get the image's persistent id from it.
+	 */
+	size = sizeof (RBD_ID_PREFIX) + rbd_dev->image_name_len;
+	object_name = kmalloc(size, GFP_NOIO);
+	if (!object_name)
+		return -ENOMEM;
+	sprintf(object_name, "%s%s", RBD_ID_PREFIX, rbd_dev->image_name);
+	dout("rbd id object name is %s\n", object_name);
+
+	/* Response will be an encoded string, which includes a length */
+
+	size = sizeof (__le32) + RBD_IMAGE_ID_LEN_MAX;
+	response = kzalloc(size, GFP_NOIO);
+	if (!response) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	ret = rbd_req_sync_exec(rbd_dev, object_name,
+				"rbd", "get_id",
+				NULL, 0,
+				response, RBD_IMAGE_ID_LEN_MAX,
+				CEPH_OSD_FLAG_READ, NULL);
+	dout("%s: rbd_req_sync_exec returned %d\n", __func__, ret);
+	if (ret < 0)
+		goto out;
+
+	p = response;
+	rbd_dev->image_id = ceph_extract_encoded_string(&p,
+						p + RBD_IMAGE_ID_LEN_MAX,
+						&rbd_dev->image_id_len,
+						GFP_NOIO);
+	if (IS_ERR(rbd_dev->image_id)) {
+		ret = PTR_ERR(rbd_dev->image_id);
+		rbd_dev->image_id = NULL;
+	} else {
+		dout("image_id is %s\n", rbd_dev->image_id);
+	}
+out:
+	kfree(response);
+	kfree(object_name);
+
+	return ret;
+}
+
+static int rbd_dev_v1_probe(struct rbd_device *rbd_dev)
+{
+	int ret;
+	size_t size;
+
+	/* Version 1 images have no id; empty string is used */
+
+	rbd_dev->image_id = kstrdup("", GFP_KERNEL);
+	if (!rbd_dev->image_id)
+		return -ENOMEM;
+	rbd_dev->image_id_len = 0;
+
+	/* Record the header object name for this rbd image. */
+
+	size = rbd_dev->image_name_len + sizeof (RBD_SUFFIX);
+	rbd_dev->header_name = kmalloc(size, GFP_KERNEL);
+	if (!rbd_dev->header_name) {
+		ret = -ENOMEM;
 		goto out_err;
+	}
 	sprintf(rbd_dev->header_name, "%s%s", rbd_dev->image_name, RBD_SUFFIX);
 
+	/* Populate rbd image metadata */
+
+	ret = rbd_read_header(rbd_dev, &rbd_dev->header);
+	if (ret < 0)
+		goto out_err;
+	rbd_dev->image_format = 1;
+
+	dout("discovered version 1 image, header name is %s\n",
+		rbd_dev->header_name);
+
+	return 0;
+
+out_err:
+	kfree(rbd_dev->header_name);
+	rbd_dev->header_name = NULL;
+	kfree(rbd_dev->image_id);
+	rbd_dev->image_id = NULL;
+
+	return ret;
+}
+
+static int rbd_dev_v2_probe(struct rbd_device *rbd_dev)
+{
+	size_t size;
+	int ret;
+	u64 ver = 0;
+
 	/*
-	 * The snapshot name is optional.  If none is is supplied,
-	 * we use the default value.
+	 * Image id was filled in by the caller.  Record the header
+	 * object name for this rbd image.
 	 */
-	rbd_dev->snap_name = dup_token(&buf, &len);
-	if (!rbd_dev->snap_name)
+	size = sizeof (RBD_HEADER_PREFIX) + rbd_dev->image_id_len;
+	rbd_dev->header_name = kmalloc(size, GFP_KERNEL);
+	if (!rbd_dev->header_name)
+		return -ENOMEM;
+	sprintf(rbd_dev->header_name, "%s%s",
+			RBD_HEADER_PREFIX, rbd_dev->image_id);
+
+	/* Get the size and object order for the image */
+
+	ret = rbd_dev_v2_image_size(rbd_dev);
+	if (ret < 0)
 		goto out_err;
-	if (!len) {
-		/* Replace the empty name with the default */
-		kfree(rbd_dev->snap_name);
-		rbd_dev->snap_name
-			= kmalloc(sizeof (RBD_SNAP_HEAD_NAME), GFP_KERNEL);
-		if (!rbd_dev->snap_name)
-			goto out_err;
 
-		memcpy(rbd_dev->snap_name, RBD_SNAP_HEAD_NAME,
-			sizeof (RBD_SNAP_HEAD_NAME));
-	}
+	/* Get the object prefix (a.k.a. block_name) for the image */
 
-	return 0;
+	ret = rbd_dev_v2_object_prefix(rbd_dev);
+	if (ret < 0)
+		goto out_err;
+
+	/* Get the features for the image */
 
+	ret = rbd_dev_v2_features(rbd_dev);
+	if (ret < 0)
+		goto out_err;
+
+	/* crypto and compression type aren't (yet) supported for v2 images */
+
+	rbd_dev->header.crypt_type = 0;
+	rbd_dev->header.comp_type = 0;
+
+	/* Get the snapshot context, plus the header version */
+
+	ret = rbd_dev_v2_snap_context(rbd_dev, &ver);
+	if (ret)
+		goto out_err;
+	rbd_dev->header.obj_version = ver;
+
+	rbd_dev->image_format = 2;
+
+	dout("discovered version 2 image, header name is %s\n",
+		rbd_dev->header_name);
+
+	return -ENOTSUPP;
 out_err:
 	kfree(rbd_dev->header_name);
-	kfree(rbd_dev->image_name);
-	kfree(rbd_dev->pool_name);
-	rbd_dev->pool_name = NULL;
+	rbd_dev->header_name = NULL;
+	kfree(rbd_dev->header.object_prefix);
+	rbd_dev->header.object_prefix = NULL;
+
+	return ret;
+}
+
+/*
+ * Probe for the existence of the header object for the given rbd
+ * device.  For format 2 images this includes determining the image
+ * id.
+ */
+static int rbd_dev_probe(struct rbd_device *rbd_dev)
+{
+	int ret;
+
+	/*
+	 * Get the id from the image id object.  If it's not a
+	 * format 2 image, we'll get ENOENT back, and we'll assume
+	 * it's a format 1 image.
+	 */
+	ret = rbd_dev_image_id(rbd_dev);
+	if (ret)
+		ret = rbd_dev_v1_probe(rbd_dev);
+	else
+		ret = rbd_dev_v2_probe(rbd_dev);
+	if (ret)
+		dout("probe failed, returning %d\n", ret);
 
 	return ret;
 }
@@ -2449,16 +3000,17 @@ static ssize_t rbd_add(struct bus_type *bus,
 	size_t mon_addrs_size = 0;
 	struct ceph_osd_client *osdc;
 	int rc = -ENOMEM;
+	char *snap_name;
 
 	if (!try_module_get(THIS_MODULE))
 		return -ENODEV;
 
 	options = kmalloc(count, GFP_KERNEL);
 	if (!options)
-		goto err_nomem;
+		goto err_out_mem;
 	rbd_dev = kzalloc(sizeof(*rbd_dev), GFP_KERNEL);
 	if (!rbd_dev)
-		goto err_nomem;
+		goto err_out_mem;
 
 	/* static rbd_device initialization */
 	spin_lock_init(&rbd_dev->lock);
@@ -2466,27 +3018,18 @@ static ssize_t rbd_add(struct bus_type *bus,
 	INIT_LIST_HEAD(&rbd_dev->snaps);
 	init_rwsem(&rbd_dev->header_rwsem);
 
-	/* generate unique id: find highest unique id, add one */
-	rbd_id_get(rbd_dev);
-
-	/* Fill in the device name, now that we have its id. */
-	BUILD_BUG_ON(DEV_NAME_LEN
-			< sizeof (RBD_DRV_NAME) + MAX_INT_FORMAT_WIDTH);
-	sprintf(rbd_dev->name, "%s%d", RBD_DRV_NAME, rbd_dev->dev_id);
-
 	/* parse add command */
-	rc = rbd_add_parse_args(rbd_dev, buf, &mon_addrs, &mon_addrs_size,
-				options, count);
-	if (rc)
-		goto err_put_id;
-
-	rbd_dev->rbd_client = rbd_get_client(mon_addrs, mon_addrs_size - 1,
-						options);
-	if (IS_ERR(rbd_dev->rbd_client)) {
-		rc = PTR_ERR(rbd_dev->rbd_client);
-		goto err_put_id;
+	snap_name = rbd_add_parse_args(rbd_dev, buf,
+				&mon_addrs, &mon_addrs_size, options, count);
+	if (IS_ERR(snap_name)) {
+		rc = PTR_ERR(snap_name);
+		goto err_out_mem;
 	}
 
+	rc = rbd_get_client(rbd_dev, mon_addrs, mon_addrs_size - 1, options);
+	if (rc < 0)
+		goto err_out_args;
+
 	/* pick the pool */
 	osdc = &rbd_dev->rbd_client->client->osdc;
 	rc = ceph_pg_poolid_by_name(osdc->osdmap, rbd_dev->pool_name);
@@ -2494,23 +3037,53 @@ static ssize_t rbd_add(struct bus_type *bus,
 		goto err_out_client;
 	rbd_dev->pool_id = rc;
 
-	/* register our block device */
-	rc = register_blkdev(0, rbd_dev->name);
+	rc = rbd_dev_probe(rbd_dev);
 	if (rc < 0)
 		goto err_out_client;
+	rbd_assert(rbd_image_format_valid(rbd_dev->image_format));
+
+	/* no need to lock here, as rbd_dev is not registered yet */
+	rc = rbd_dev_snaps_update(rbd_dev);
+	if (rc)
+		goto err_out_header;
+
+	rc = rbd_dev_set_mapping(rbd_dev, snap_name);
+	if (rc)
+		goto err_out_header;
+
+	/* generate unique id: find highest unique id, add one */
+	rbd_dev_id_get(rbd_dev);
+
+	/* Fill in the device name, now that we have its id. */
+	BUILD_BUG_ON(DEV_NAME_LEN
+			< sizeof (RBD_DRV_NAME) + MAX_INT_FORMAT_WIDTH);
+	sprintf(rbd_dev->name, "%s%d", RBD_DRV_NAME, rbd_dev->dev_id);
+
+	/* Get our block major device number. */
+
+	rc = register_blkdev(0, rbd_dev->name);
+	if (rc < 0)
+		goto err_out_id;
 	rbd_dev->major = rc;
 
-	rc = rbd_bus_add_dev(rbd_dev);
+	/* Set up the blkdev mapping. */
+
+	rc = rbd_init_disk(rbd_dev);
 	if (rc)
 		goto err_out_blkdev;
 
+	rc = rbd_bus_add_dev(rbd_dev);
+	if (rc)
+		goto err_out_disk;
+
 	/*
 	 * At this point cleanup in the event of an error is the job
 	 * of the sysfs code (initiated by rbd_bus_del_dev()).
-	 *
-	 * Set up and announce blkdev mapping.
 	 */
-	rc = rbd_init_disk(rbd_dev);
+
+	down_write(&rbd_dev->header_rwsem);
+	rc = rbd_dev_snaps_register(rbd_dev);
+	up_write(&rbd_dev->header_rwsem);
 	if (rc)
 		goto err_out_bus;
 
@@ -2518,6 +3091,13 @@ static ssize_t rbd_add(struct bus_type *bus,
 	if (rc)
 		goto err_out_bus;
 
+	/* Everything's ready.  Announce the disk to the world. */
+
+	add_disk(rbd_dev->disk);
+
+	pr_info("%s: added with size 0x%llx\n", rbd_dev->disk->disk_name,
+		(unsigned long long) rbd_dev->mapping.size);
+
 	return count;
 
 err_out_bus:
@@ -2527,19 +3107,23 @@ err_out_bus:
 	kfree(options);
 	return rc;
 
+err_out_disk:
+	rbd_free_disk(rbd_dev);
 err_out_blkdev:
 	unregister_blkdev(rbd_dev->major, rbd_dev->name);
+err_out_id:
+	rbd_dev_id_put(rbd_dev);
+err_out_header:
+	rbd_header_free(&rbd_dev->header);
 err_out_client:
+	kfree(rbd_dev->header_name);
 	rbd_put_client(rbd_dev);
-err_put_id:
-	if (rbd_dev->pool_name) {
-		kfree(rbd_dev->snap_name);
-		kfree(rbd_dev->header_name);
-		kfree(rbd_dev->image_name);
-		kfree(rbd_dev->pool_name);
-	}
-	rbd_id_put(rbd_dev);
-err_nomem:
+	kfree(rbd_dev->image_id);
+err_out_args:
+	kfree(rbd_dev->mapping.snap_name);
+	kfree(rbd_dev->image_name);
+	kfree(rbd_dev->pool_name);
+err_out_mem:
 	kfree(rbd_dev);
 	kfree(options);
 
@@ -2585,12 +3169,16 @@ static void rbd_dev_release(struct device *dev)
 	rbd_free_disk(rbd_dev);
 	unregister_blkdev(rbd_dev->major, rbd_dev->name);
 
+	/* release allocated disk header fields */
+	rbd_header_free(&rbd_dev->header);
+
 	/* done with the id, and with the rbd_dev */
-	kfree(rbd_dev->snap_name);
+	kfree(rbd_dev->mapping.snap_name);
+	kfree(rbd_dev->image_id);
 	kfree(rbd_dev->header_name);
 	kfree(rbd_dev->pool_name);
 	kfree(rbd_dev->image_name);
-	rbd_id_put(rbd_dev);
+	rbd_dev_id_put(rbd_dev);
 	kfree(rbd_dev);
 
 	/* release module ref */
@@ -2628,47 +3216,7 @@ static ssize_t rbd_remove(struct bus_type *bus,
 
 done:
 	mutex_unlock(&ctl_mutex);
-	return ret;
-}
 
-static ssize_t rbd_snap_add(struct device *dev,
-			    struct device_attribute *attr,
-			    const char *buf,
-			    size_t count)
-{
-	struct rbd_device *rbd_dev = dev_to_rbd_dev(dev);
-	int ret;
-	char *name = kmalloc(count + 1, GFP_KERNEL);
-	if (!name)
-		return -ENOMEM;
-
-	snprintf(name, count, "%s", buf);
-
-	mutex_lock_nested(&ctl_mutex, SINGLE_DEPTH_NESTING);
-
-	ret = rbd_header_add_snap(rbd_dev,
-				  name, GFP_KERNEL);
-	if (ret < 0)
-		goto err_unlock;
-
-	ret = __rbd_refresh_header(rbd_dev, NULL);
-	if (ret < 0)
-		goto err_unlock;
-
-	/* shouldn't hold ctl_mutex when notifying.. notify might
-	   trigger a watch callback that would need to get that mutex */
-	mutex_unlock(&ctl_mutex);
-
-	/* make a best effort, don't error if failed */
-	rbd_req_sync_notify(rbd_dev);
-
-	ret = count;
-	kfree(name);
-	return ret;
-
-err_unlock:
-	mutex_unlock(&ctl_mutex);
-	kfree(name);
 	return ret;
 }
 
diff --git a/drivers/block/rbd_types.h b/drivers/block/rbd_types.h
index 0924e9e41a6..cbe77fa105b 100644
--- a/drivers/block/rbd_types.h
+++ b/drivers/block/rbd_types.h
@@ -15,15 +15,30 @@
 
 #include <linux/types.h>
 
+/* For format version 2, rbd image 'foo' consists of objects
+ *   rbd_id.foo		- id of image
+ *   rbd_header.<id>	- image metadata
+ *   rbd_data.<id>.0000000000000000
+ *   rbd_data.<id>.0000000000000001
+ *   ...		- data
+ * Clients do not access header data directly in rbd format 2.
+ */
+
+#define RBD_HEADER_PREFIX      "rbd_header."
+#define RBD_DATA_PREFIX        "rbd_data."
+#define RBD_ID_PREFIX          "rbd_id."
+
 /*
- * rbd image 'foo' consists of objects
- *   foo.rbd      - image metadata
- *   foo.00000000
- *   foo.00000001
- *   ...          - data
+ * For format version 1, rbd image 'foo' consists of objects
+ *   foo.rbd		- image metadata
+ *   rb.<idhi>.<idlo>.00000000
+ *   rb.<idhi>.<idlo>.00000001
+ *   ...		- data
+ * There is no notion of a persistent image id in rbd format 1.
  */
 
 #define RBD_SUFFIX		".rbd"
+
 #define RBD_DIRECTORY           "rbd_directory"
 #define RBD_INFO                "rbd_info"
 
@@ -47,7 +62,7 @@ struct rbd_image_snap_ondisk {
 
 struct rbd_image_header_ondisk {
 	char text[40];
-	char block_name[24];
+	char object_prefix[24];
 	char signature[4];
 	char version[8];
 	struct {
diff --git a/drivers/char/ds1620.c b/drivers/char/ds1620.c
index aab9605f0b4..24ffd8cec51 100644
--- a/drivers/char/ds1620.c
+++ b/drivers/char/ds1620.c
@@ -74,21 +74,21 @@ static inline void netwinder_ds1620_reset(void)
 
 static inline void netwinder_lock(unsigned long *flags)
 {
-	spin_lock_irqsave(&nw_gpio_lock, *flags);
+	raw_spin_lock_irqsave(&nw_gpio_lock, *flags);
 }
 
 static inline void netwinder_unlock(unsigned long *flags)
 {
-	spin_unlock_irqrestore(&nw_gpio_lock, *flags);
+	raw_spin_unlock_irqrestore(&nw_gpio_lock, *flags);
 }
 
 static inline void netwinder_set_fan(int i)
 {
 	unsigned long flags;
 
-	spin_lock_irqsave(&nw_gpio_lock, flags);
+	raw_spin_lock_irqsave(&nw_gpio_lock, flags);
 	nw_gpio_modify_op(GPIO_FAN, i ? GPIO_FAN : 0);
-	spin_unlock_irqrestore(&nw_gpio_lock, flags);
+	raw_spin_unlock_irqrestore(&nw_gpio_lock, flags);
 }
 
 static inline int netwinder_get_fan(void)
diff --git a/drivers/char/mbcs.c b/drivers/char/mbcs.c
index 0c7d340b9ab..f74e892711d 100644
--- a/drivers/char/mbcs.c
+++ b/drivers/char/mbcs.c
@@ -507,7 +507,7 @@ static int mbcs_gscr_mmap(struct file *fp, struct vm_area_struct *vma)
 
 	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
 
-	/* Remap-pfn-range will mark the range VM_IO and VM_RESERVED */
+	/* Remap-pfn-range will mark the range VM_IO */
 	if (remap_pfn_range(vma,
 			    vma->vm_start,
 			    __pa(soft->gscr_addr) >> PAGE_SHIFT,
diff --git a/drivers/char/mem.c b/drivers/char/mem.c
index e5eedfa24c9..0537903c985 100644
--- a/drivers/char/mem.c
+++ b/drivers/char/mem.c
@@ -322,7 +322,7 @@ static int mmap_mem(struct file *file, struct vm_area_struct *vma)
 
 	vma->vm_ops = &mmap_mem_ops;
 
-	/* Remap-pfn-range will mark the range VM_IO and VM_RESERVED */
+	/* Remap-pfn-range will mark the range VM_IO */
 	if (remap_pfn_range(vma,
 			    vma->vm_start,
 			    vma->vm_pgoff,
diff --git a/drivers/char/mspec.c b/drivers/char/mspec.c
index 845f97fd183..e1f60f968fd 100644
--- a/drivers/char/mspec.c
+++ b/drivers/char/mspec.c
@@ -286,7 +286,7 @@ mspec_mmap(struct file *file, struct vm_area_struct *vma,
 	atomic_set(&vdata->refcnt, 1);
 	vma->vm_private_data = vdata;
 
-	vma->vm_flags |= (VM_IO | VM_RESERVED | VM_PFNMAP | VM_DONTEXPAND);
+	vma->vm_flags |= VM_IO | VM_PFNMAP | VM_DONTEXPAND | VM_DONTDUMP;
 	if (vdata->type == MSPEC_FETCHOP || vdata->type == MSPEC_UNCACHED)
 		vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
 	vma->vm_ops = &mspec_vm_ops;
diff --git a/drivers/char/nwflash.c b/drivers/char/nwflash.c
index a0e2f7d7035..e371480d363 100644
--- a/drivers/char/nwflash.c
+++ b/drivers/char/nwflash.c
@@ -583,9 +583,9 @@ static void kick_open(void)
 	 * we want to write a bit pattern XXX1 to Xilinx to enable
 	 * the write gate, which will be open for about the next 2ms.
 	 */
-	spin_lock_irqsave(&nw_gpio_lock, flags);
+	raw_spin_lock_irqsave(&nw_gpio_lock, flags);
 	nw_cpld_modify(CPLD_FLASH_WR_ENABLE, CPLD_FLASH_WR_ENABLE);
-	spin_unlock_irqrestore(&nw_gpio_lock, flags);
+	raw_spin_unlock_irqrestore(&nw_gpio_lock, flags);
 
 	/*
 	 * let the ISA bus to catch on...
diff --git a/drivers/char/raw.c b/drivers/char/raw.c
index 54a3a6d0981..0bb207eaef2 100644
--- a/drivers/char/raw.c
+++ b/drivers/char/raw.c
@@ -285,7 +285,7 @@ static long raw_ctl_compat_ioctl(struct file *file, unsigned int cmd,
 
 static const struct file_operations raw_fops = {
 	.read		= do_sync_read,
-	.aio_read	= generic_file_aio_read,
+	.aio_read	= blkdev_aio_read,
 	.write		= do_sync_write,
 	.aio_write	= blkdev_aio_write,
 	.fsync		= blkdev_fsync,
diff --git a/drivers/char/tpm/tpm.c b/drivers/char/tpm/tpm.c
index f26afdb1a70..93211df52aa 100644
--- a/drivers/char/tpm/tpm.c
+++ b/drivers/char/tpm/tpm.c
@@ -1182,17 +1182,20 @@ ssize_t tpm_write(struct file *file, const char __user *buf,
 		  size_t size, loff_t *off)
 {
 	struct tpm_chip *chip = file->private_data;
-	size_t in_size = size, out_size;
+	size_t in_size = size;
+	ssize_t out_size;
 
 	/* cannot perform a write until the read has cleared
-	   either via tpm_read or a user_read_timer timeout */
-	while (atomic_read(&chip->data_pending) != 0)
-		msleep(TPM_TIMEOUT);
-
-	mutex_lock(&chip->buffer_mutex);
+	   either via tpm_read or a user_read_timer timeout.
+	   This also prevents splitted buffered writes from blocking here.
+	*/
+	if (atomic_read(&chip->data_pending) != 0)
+		return -EBUSY;
 
 	if (in_size > TPM_BUFSIZE)
-		in_size = TPM_BUFSIZE;
+		return -E2BIG;
+
+	mutex_lock(&chip->buffer_mutex);
 
 	if (copy_from_user
 	    (chip->data_buffer, (void __user *) buf, in_size)) {
@@ -1202,6 +1205,10 @@ ssize_t tpm_write(struct file *file, const char __user *buf,
 
 	/* atomic tpm command send and result receive */
 	out_size = tpm_transmit(chip, chip->data_buffer, TPM_BUFSIZE);
+	if (out_size < 0) {
+		mutex_unlock(&chip->buffer_mutex);
+		return out_size;
+	}
 
 	atomic_set(&chip->data_pending, out_size);
 	mutex_unlock(&chip->buffer_mutex);
@@ -1259,6 +1266,7 @@ void tpm_remove_hardware(struct device *dev)
 
 	misc_deregister(&chip->vendor.miscdev);
 	sysfs_remove_group(&dev->kobj, chip->vendor.attr_group);
+	tpm_remove_ppi(&dev->kobj);
 	tpm_bios_log_teardown(chip->bios_dir);
 
 	/* write it this way to be explicit (chip->dev == dev) */
@@ -1476,7 +1484,7 @@ struct tpm_chip *tpm_register_hardware(struct device *dev,
 		goto put_device;
 	}
 
-	if (sys_add_ppi(&dev->kobj)) {
+	if (tpm_add_ppi(&dev->kobj)) {
 		misc_deregister(&chip->vendor.miscdev);
 		goto put_device;
 	}
diff --git a/drivers/char/tpm/tpm.h b/drivers/char/tpm/tpm.h
index 02c266aa2bf..8ef7649a50a 100644
--- a/drivers/char/tpm/tpm.h
+++ b/drivers/char/tpm/tpm.h
@@ -329,10 +329,15 @@ extern int wait_for_tpm_stat(struct tpm_chip *, u8, unsigned long,
 			     wait_queue_head_t *);
 
 #ifdef CONFIG_ACPI
-extern ssize_t sys_add_ppi(struct kobject *parent);
+extern int tpm_add_ppi(struct kobject *);
+extern void tpm_remove_ppi(struct kobject *);
 #else
-static inline ssize_t sys_add_ppi(struct kobject *parent)
+static inline int tpm_add_ppi(struct kobject *parent)
 {
 	return 0;
 }
+
+static inline void tpm_remove_ppi(struct kobject *parent)
+{
+}
 #endif
diff --git a/drivers/char/tpm/tpm_ppi.c b/drivers/char/tpm/tpm_ppi.c
index f27b58cfae9..720ebcf29fd 100644
--- a/drivers/char/tpm/tpm_ppi.c
+++ b/drivers/char/tpm/tpm_ppi.c
@@ -444,18 +444,20 @@ static struct attribute *ppi_attrs[] = {
 	&dev_attr_vs_operations.attr, NULL,
 };
 static struct attribute_group ppi_attr_grp = {
+	.name = "ppi",
 	.attrs = ppi_attrs
 };
 
-ssize_t sys_add_ppi(struct kobject *parent)
+int tpm_add_ppi(struct kobject *parent)
 {
-	struct kobject *ppi;
-	ppi = kobject_create_and_add("ppi", parent);
-	if (sysfs_create_group(ppi, &ppi_attr_grp))
-		return -EFAULT;
-	else
-		return 0;
+	return sysfs_create_group(parent, &ppi_attr_grp);
+}
+EXPORT_SYMBOL_GPL(tpm_add_ppi);
+
+void tpm_remove_ppi(struct kobject *parent)
+{
+	sysfs_remove_group(parent, &ppi_attr_grp);
 }
-EXPORT_SYMBOL_GPL(sys_add_ppi);
+EXPORT_SYMBOL_GPL(tpm_remove_ppi);
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/cpufreq/omap-cpufreq.c b/drivers/cpufreq/omap-cpufreq.c
index 65f8e9a5497..1f3417a8322 100644
--- a/drivers/cpufreq/omap-cpufreq.c
+++ b/drivers/cpufreq/omap-cpufreq.c
@@ -30,20 +30,12 @@
 #include <asm/smp_plat.h>
 #include <asm/cpu.h>
 
-#include <plat/clock.h>
-#include <plat/omap-pm.h>
-#include <plat/common.h>
-#include <plat/omap_device.h>
-
-#include <mach/hardware.h>
-
 /* OPP tolerance in percentage */
 #define	OPP_TOLERANCE	4
 
 static struct cpufreq_frequency_table *freq_table;
 static atomic_t freq_table_users = ATOMIC_INIT(0);
 static struct clk *mpu_clk;
-static char *mpu_clk_name;
 static struct device *mpu_dev;
 static struct regulator *mpu_reg;
 
@@ -108,6 +100,14 @@ static int omap_target(struct cpufreq_policy *policy,
 	}
 
 	freq = freqs.new * 1000;
+	ret = clk_round_rate(mpu_clk, freq);
+	if (IS_ERR_VALUE(ret)) {
+		dev_warn(mpu_dev,
+			 "CPUfreq: Cannot find matching frequency for %lu\n",
+			 freq);
+		return ret;
+	}
+	freq = ret;
 
 	if (mpu_reg) {
 		opp = opp_find_freq_ceil(mpu_dev, &freq);
@@ -172,7 +172,7 @@ static int __cpuinit omap_cpu_init(struct cpufreq_policy *policy)
 {
 	int result = 0;
 
-	mpu_clk = clk_get(NULL, mpu_clk_name);
+	mpu_clk = clk_get(NULL, "cpufreq_ck");
 	if (IS_ERR(mpu_clk))
 		return PTR_ERR(mpu_clk);
 
@@ -253,22 +253,10 @@ static struct cpufreq_driver omap_driver = {
 
 static int __init omap_cpufreq_init(void)
 {
-	if (cpu_is_omap24xx())
-		mpu_clk_name = "virt_prcm_set";
-	else if (cpu_is_omap34xx())
-		mpu_clk_name = "dpll1_ck";
-	else if (cpu_is_omap44xx())
-		mpu_clk_name = "dpll_mpu_ck";
-
-	if (!mpu_clk_name) {
-		pr_err("%s: unsupported Silicon?\n", __func__);
-		return -EINVAL;
-	}
-
-	mpu_dev = omap_device_get_by_hwmod_name("mpu");
-	if (IS_ERR(mpu_dev)) {
+	mpu_dev = get_cpu_device(0);
+	if (!mpu_dev) {
 		pr_warning("%s: unable to get the mpu device\n", __func__);
-		return PTR_ERR(mpu_dev);
+		return -EINVAL;
 	}
 
 	mpu_reg = regulator_get(mpu_dev, "vcc");
diff --git a/drivers/cpuidle/cpuidle.c b/drivers/cpuidle/cpuidle.c
index e28f6ea46f1..7f15b8514a1 100644
--- a/drivers/cpuidle/cpuidle.c
+++ b/drivers/cpuidle/cpuidle.c
@@ -368,7 +368,7 @@ EXPORT_SYMBOL_GPL(cpuidle_enable_device);
  */
 void cpuidle_disable_device(struct cpuidle_device *dev)
 {
-	if (!dev->enabled)
+	if (!dev || !dev->enabled)
 		return;
 	if (!cpuidle_get_driver() || !cpuidle_curr_governor)
 		return;
diff --git a/drivers/dma/Kconfig b/drivers/dma/Kconfig
index d06ea2950dd..677cd6e4e1a 100644
--- a/drivers/dma/Kconfig
+++ b/drivers/dma/Kconfig
@@ -208,6 +208,16 @@ config SIRF_DMA
 	help
 	  Enable support for the CSR SiRFprimaII DMA engine.
 
+config TI_EDMA
+	tristate "TI EDMA support"
+	depends on ARCH_DAVINCI
+	select DMA_ENGINE
+	select DMA_VIRTUAL_CHANNELS
+	default n
+	help
+	  Enable support for the TI EDMA controller. This DMA
+	  engine is found on TI DaVinci and AM33xx parts.
+
 config ARCH_HAS_ASYNC_TX_FIND_CHANNEL
 	bool
 
@@ -292,6 +302,13 @@ config DMA_OMAP
 	select DMA_ENGINE
 	select DMA_VIRTUAL_CHANNELS
 
+config MMP_PDMA
+	bool "MMP PDMA support"
+	depends on (ARCH_MMP || ARCH_PXA)
+	select DMA_ENGINE
+	help
+	  Support the MMP PDMA engine for PXA and MMP platfrom.
+
 config DMA_ENGINE
 	bool
 
diff --git a/drivers/dma/Makefile b/drivers/dma/Makefile
index 4cf6b128ab9..7428feaa870 100644
--- a/drivers/dma/Makefile
+++ b/drivers/dma/Makefile
@@ -23,6 +23,7 @@ obj-$(CONFIG_IMX_DMA) += imx-dma.o
 obj-$(CONFIG_MXS_DMA) += mxs-dma.o
 obj-$(CONFIG_TIMB_DMA) += timb_dma.o
 obj-$(CONFIG_SIRF_DMA) += sirf-dma.o
+obj-$(CONFIG_TI_EDMA) += edma.o
 obj-$(CONFIG_STE_DMA40) += ste_dma40.o ste_dma40_ll.o
 obj-$(CONFIG_TEGRA20_APB_DMA) += tegra20-apb-dma.o
 obj-$(CONFIG_PL330_DMA) += pl330.o
@@ -32,3 +33,4 @@ obj-$(CONFIG_EP93XX_DMA) += ep93xx_dma.o
 obj-$(CONFIG_DMA_SA11X0) += sa11x0-dma.o
 obj-$(CONFIG_MMP_TDMA) += mmp_tdma.o
 obj-$(CONFIG_DMA_OMAP) += omap-dma.o
+obj-$(CONFIG_MMP_PDMA) += mmp_pdma.o
diff --git a/drivers/dma/amba-pl08x.c b/drivers/dma/amba-pl08x.c
index 6fbeebb9486..d1cc5791476 100644
--- a/drivers/dma/amba-pl08x.c
+++ b/drivers/dma/amba-pl08x.c
@@ -1892,6 +1892,7 @@ static int pl08x_probe(struct amba_device *adev, const struct amba_id *id)
 	pl08x->pd = dev_get_platdata(&adev->dev);
 	if (!pl08x->pd) {
 		dev_err(&adev->dev, "no platform data supplied\n");
+		ret = -EINVAL;
 		goto out_no_platdata;
 	}
 
@@ -1943,6 +1944,7 @@ static int pl08x_probe(struct amba_device *adev, const struct amba_id *id)
 		dev_err(&adev->dev, "%s failed to allocate "
 			"physical channel holders\n",
 			__func__);
+		ret = -ENOMEM;
 		goto out_no_phychans;
 	}
 
diff --git a/drivers/dma/at_hdmac.c b/drivers/dma/at_hdmac.c
index 17d6958342e..13a02f4425b 100644
--- a/drivers/dma/at_hdmac.c
+++ b/drivers/dma/at_hdmac.c
@@ -852,12 +852,13 @@ atc_dma_cyclic_fill_desc(struct dma_chan *chan, struct at_desc *desc,
  * @buf_len: total number of bytes for the entire buffer
  * @period_len: number of bytes for each period
  * @direction: transfer direction, to or from device
+ * @flags: tx descriptor status flags
  * @context: transfer context (ignored)
  */
 static struct dma_async_tx_descriptor *
 atc_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t buf_addr, size_t buf_len,
 		size_t period_len, enum dma_transfer_direction direction,
-		void *context)
+		unsigned long flags, void *context)
 {
 	struct at_dma_chan	*atchan = to_at_dma_chan(chan);
 	struct at_dma_slave	*atslave = chan->private;
diff --git a/drivers/dma/dw_dmac.c b/drivers/dma/dw_dmac.c
index d3c5a5a88f1..c4b0eb3cde8 100644
--- a/drivers/dma/dw_dmac.c
+++ b/drivers/dma/dw_dmac.c
@@ -36,12 +36,22 @@
  * which does not support descriptor writeback.
  */
 
+static inline unsigned int dwc_get_dms(struct dw_dma_slave *slave)
+{
+	return slave ? slave->dst_master : 0;
+}
+
+static inline unsigned int dwc_get_sms(struct dw_dma_slave *slave)
+{
+	return slave ? slave->src_master : 1;
+}
+
 #define DWC_DEFAULT_CTLLO(_chan) ({				\
 		struct dw_dma_slave *__slave = (_chan->private);	\
 		struct dw_dma_chan *_dwc = to_dw_dma_chan(_chan);	\
 		struct dma_slave_config	*_sconfig = &_dwc->dma_sconfig;	\
-		int _dms = __slave ? __slave->dst_master : 0;	\
-		int _sms = __slave ? __slave->src_master : 1;	\
+		int _dms = dwc_get_dms(__slave);		\
+		int _sms = dwc_get_sms(__slave);		\
 		u8 _smsize = __slave ? _sconfig->src_maxburst :	\
 			DW_DMA_MSIZE_16;			\
 		u8 _dmsize = __slave ? _sconfig->dst_maxburst :	\
@@ -56,16 +66,6 @@
 	})
 
 /*
- * This is configuration-dependent and usually a funny size like 4095.
- *
- * Note that this is a transfer count, i.e. if we transfer 32-bit
- * words, we can do 16380 bytes per descriptor.
- *
- * This parameter is also system-specific.
- */
-#define DWC_MAX_COUNT	4095U
-
-/*
  * Number of descriptors to allocate for each channel. This should be
  * made configurable somehow; preferably, the clients (at least the
  * ones using slave transfers) should be able to give us a hint.
@@ -177,6 +177,11 @@ static void dwc_initialize(struct dw_dma_chan *dwc)
 
 		cfghi = dws->cfg_hi;
 		cfglo |= dws->cfg_lo & ~DWC_CFGL_CH_PRIOR_MASK;
+	} else {
+		if (dwc->dma_sconfig.direction == DMA_MEM_TO_DEV)
+			cfghi = DWC_CFGH_DST_PER(dwc->dma_sconfig.slave_id);
+		else if (dwc->dma_sconfig.direction == DMA_DEV_TO_MEM)
+			cfghi = DWC_CFGH_SRC_PER(dwc->dma_sconfig.slave_id);
 	}
 
 	channel_writel(dwc, CFG_LO, cfglo);
@@ -206,7 +211,7 @@ static inline unsigned int dwc_fast_fls(unsigned long long v)
 	return 0;
 }
 
-static void dwc_dump_chan_regs(struct dw_dma_chan *dwc)
+static inline void dwc_dump_chan_regs(struct dw_dma_chan *dwc)
 {
 	dev_err(chan2dev(&dwc->chan),
 		"  SAR: 0x%x DAR: 0x%x LLP: 0x%x CTL: 0x%x:%08x\n",
@@ -227,10 +232,29 @@ static inline void dwc_chan_disable(struct dw_dma *dw, struct dw_dma_chan *dwc)
 
 /*----------------------------------------------------------------------*/
 
+/* Perform single block transfer */
+static inline void dwc_do_single_block(struct dw_dma_chan *dwc,
+				       struct dw_desc *desc)
+{
+	struct dw_dma	*dw = to_dw_dma(dwc->chan.device);
+	u32		ctllo;
+
+	/* Software emulation of LLP mode relies on interrupts to continue
+	 * multi block transfer. */
+	ctllo = desc->lli.ctllo | DWC_CTLL_INT_EN;
+
+	channel_writel(dwc, SAR, desc->lli.sar);
+	channel_writel(dwc, DAR, desc->lli.dar);
+	channel_writel(dwc, CTL_LO, ctllo);
+	channel_writel(dwc, CTL_HI, desc->lli.ctlhi);
+	channel_set_bit(dw, CH_EN, dwc->mask);
+}
+
 /* Called with dwc->lock held and bh disabled */
 static void dwc_dostart(struct dw_dma_chan *dwc, struct dw_desc *first)
 {
 	struct dw_dma	*dw = to_dw_dma(dwc->chan.device);
+	unsigned long	was_soft_llp;
 
 	/* ASSERT:  channel is idle */
 	if (dma_readl(dw, CH_EN) & dwc->mask) {
@@ -242,6 +266,26 @@ static void dwc_dostart(struct dw_dma_chan *dwc, struct dw_desc *first)
 		return;
 	}
 
+	if (dwc->nollp) {
+		was_soft_llp = test_and_set_bit(DW_DMA_IS_SOFT_LLP,
+						&dwc->flags);
+		if (was_soft_llp) {
+			dev_err(chan2dev(&dwc->chan),
+				"BUG: Attempted to start new LLP transfer "
+				"inside ongoing one\n");
+			return;
+		}
+
+		dwc_initialize(dwc);
+
+		dwc->tx_list = &first->tx_list;
+		dwc->tx_node_active = first->tx_list.next;
+
+		dwc_do_single_block(dwc, first);
+
+		return;
+	}
+
 	dwc_initialize(dwc);
 
 	channel_writel(dwc, LLP, first->txd.phys);
@@ -553,8 +597,36 @@ static void dw_dma_tasklet(unsigned long data)
 			dwc_handle_cyclic(dw, dwc, status_err, status_xfer);
 		else if (status_err & (1 << i))
 			dwc_handle_error(dw, dwc);
-		else if (status_xfer & (1 << i))
+		else if (status_xfer & (1 << i)) {
+			unsigned long flags;
+
+			spin_lock_irqsave(&dwc->lock, flags);
+			if (test_bit(DW_DMA_IS_SOFT_LLP, &dwc->flags)) {
+				if (dwc->tx_node_active != dwc->tx_list) {
+					struct dw_desc *desc =
+						list_entry(dwc->tx_node_active,
+							   struct dw_desc,
+							   desc_node);
+
+					dma_writel(dw, CLEAR.XFER, dwc->mask);
+
+					/* move pointer to next descriptor */
+					dwc->tx_node_active =
+						dwc->tx_node_active->next;
+
+					dwc_do_single_block(dwc, desc);
+
+					spin_unlock_irqrestore(&dwc->lock, flags);
+					continue;
+				} else {
+					/* we are done here */
+					clear_bit(DW_DMA_IS_SOFT_LLP, &dwc->flags);
+				}
+			}
+			spin_unlock_irqrestore(&dwc->lock, flags);
+
 			dwc_scan_descriptors(dw, dwc);
+		}
 	}
 
 	/*
@@ -636,6 +708,7 @@ dwc_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 		size_t len, unsigned long flags)
 {
 	struct dw_dma_chan	*dwc = to_dw_dma_chan(chan);
+	struct dw_dma_slave	*dws = chan->private;
 	struct dw_desc		*desc;
 	struct dw_desc		*first;
 	struct dw_desc		*prev;
@@ -643,6 +716,7 @@ dwc_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 	size_t			offset;
 	unsigned int		src_width;
 	unsigned int		dst_width;
+	unsigned int		data_width;
 	u32			ctllo;
 
 	dev_vdbg(chan2dev(chan),
@@ -655,7 +729,11 @@ dwc_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 		return NULL;
 	}
 
-	src_width = dst_width = dwc_fast_fls(src | dest | len);
+	data_width = min_t(unsigned int, dwc->dw->data_width[dwc_get_sms(dws)],
+					 dwc->dw->data_width[dwc_get_dms(dws)]);
+
+	src_width = dst_width = min_t(unsigned int, data_width,
+				      dwc_fast_fls(src | dest | len));
 
 	ctllo = DWC_DEFAULT_CTLLO(chan)
 			| DWC_CTLL_DST_WIDTH(dst_width)
@@ -667,7 +745,7 @@ dwc_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 
 	for (offset = 0; offset < len; offset += xfer_count << src_width) {
 		xfer_count = min_t(size_t, (len - offset) >> src_width,
-				DWC_MAX_COUNT);
+					   dwc->block_size);
 
 		desc = dwc_desc_get(dwc);
 		if (!desc)
@@ -725,6 +803,7 @@ dwc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 	dma_addr_t		reg;
 	unsigned int		reg_width;
 	unsigned int		mem_width;
+	unsigned int		data_width;
 	unsigned int		i;
 	struct scatterlist	*sg;
 	size_t			total_len = 0;
@@ -748,6 +827,8 @@ dwc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 		ctllo |= sconfig->device_fc ? DWC_CTLL_FC(DW_DMA_FC_P_M2P) :
 			DWC_CTLL_FC(DW_DMA_FC_D_M2P);
 
+		data_width = dwc->dw->data_width[dwc_get_sms(dws)];
+
 		for_each_sg(sgl, sg, sg_len, i) {
 			struct dw_desc	*desc;
 			u32		len, dlen, mem;
@@ -755,7 +836,8 @@ dwc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 			mem = sg_dma_address(sg);
 			len = sg_dma_len(sg);
 
-			mem_width = dwc_fast_fls(mem | len);
+			mem_width = min_t(unsigned int,
+					  data_width, dwc_fast_fls(mem | len));
 
 slave_sg_todev_fill_desc:
 			desc = dwc_desc_get(dwc);
@@ -768,8 +850,8 @@ slave_sg_todev_fill_desc:
 			desc->lli.sar = mem;
 			desc->lli.dar = reg;
 			desc->lli.ctllo = ctllo | DWC_CTLL_SRC_WIDTH(mem_width);
-			if ((len >> mem_width) > DWC_MAX_COUNT) {
-				dlen = DWC_MAX_COUNT << mem_width;
+			if ((len >> mem_width) > dwc->block_size) {
+				dlen = dwc->block_size << mem_width;
 				mem += dlen;
 				len -= dlen;
 			} else {
@@ -808,6 +890,8 @@ slave_sg_todev_fill_desc:
 		ctllo |= sconfig->device_fc ? DWC_CTLL_FC(DW_DMA_FC_P_P2M) :
 			DWC_CTLL_FC(DW_DMA_FC_D_P2M);
 
+		data_width = dwc->dw->data_width[dwc_get_dms(dws)];
+
 		for_each_sg(sgl, sg, sg_len, i) {
 			struct dw_desc	*desc;
 			u32		len, dlen, mem;
@@ -815,7 +899,8 @@ slave_sg_todev_fill_desc:
 			mem = sg_dma_address(sg);
 			len = sg_dma_len(sg);
 
-			mem_width = dwc_fast_fls(mem | len);
+			mem_width = min_t(unsigned int,
+					  data_width, dwc_fast_fls(mem | len));
 
 slave_sg_fromdev_fill_desc:
 			desc = dwc_desc_get(dwc);
@@ -828,8 +913,8 @@ slave_sg_fromdev_fill_desc:
 			desc->lli.sar = reg;
 			desc->lli.dar = mem;
 			desc->lli.ctllo = ctllo | DWC_CTLL_DST_WIDTH(mem_width);
-			if ((len >> reg_width) > DWC_MAX_COUNT) {
-				dlen = DWC_MAX_COUNT << reg_width;
+			if ((len >> reg_width) > dwc->block_size) {
+				dlen = dwc->block_size << reg_width;
 				mem += dlen;
 				len -= dlen;
 			} else {
@@ -945,6 +1030,8 @@ static int dwc_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 	} else if (cmd == DMA_TERMINATE_ALL) {
 		spin_lock_irqsave(&dwc->lock, flags);
 
+		clear_bit(DW_DMA_IS_SOFT_LLP, &dwc->flags);
+
 		dwc_chan_disable(dw, dwc);
 
 		dwc->paused = false;
@@ -1187,6 +1274,13 @@ struct dw_cyclic_desc *dw_dma_cyclic_prep(struct dma_chan *chan,
 	unsigned long			flags;
 
 	spin_lock_irqsave(&dwc->lock, flags);
+	if (dwc->nollp) {
+		spin_unlock_irqrestore(&dwc->lock, flags);
+		dev_dbg(chan2dev(&dwc->chan),
+				"channel doesn't support LLP transfers\n");
+		return ERR_PTR(-EINVAL);
+	}
+
 	if (!list_empty(&dwc->queue) || !list_empty(&dwc->active_list)) {
 		spin_unlock_irqrestore(&dwc->lock, flags);
 		dev_dbg(chan2dev(&dwc->chan),
@@ -1212,7 +1306,7 @@ struct dw_cyclic_desc *dw_dma_cyclic_prep(struct dma_chan *chan,
 	periods = buf_len / period_len;
 
 	/* Check for too big/unaligned periods and unaligned DMA buffer. */
-	if (period_len > (DWC_MAX_COUNT << reg_width))
+	if (period_len > (dwc->block_size << reg_width))
 		goto out_err;
 	if (unlikely(period_len & ((1 << reg_width) - 1)))
 		goto out_err;
@@ -1374,6 +1468,11 @@ static int __devinit dw_probe(struct platform_device *pdev)
 	struct resource		*io;
 	struct dw_dma		*dw;
 	size_t			size;
+	void __iomem		*regs;
+	bool			autocfg;
+	unsigned int		dw_params;
+	unsigned int		nr_channels;
+	unsigned int		max_blk_size = 0;
 	int			irq;
 	int			err;
 	int			i;
@@ -1390,32 +1489,46 @@ static int __devinit dw_probe(struct platform_device *pdev)
 	if (irq < 0)
 		return irq;
 
-	size = sizeof(struct dw_dma);
-	size += pdata->nr_channels * sizeof(struct dw_dma_chan);
-	dw = kzalloc(size, GFP_KERNEL);
+	regs = devm_request_and_ioremap(&pdev->dev, io);
+	if (!regs)
+		return -EBUSY;
+
+	dw_params = dma_read_byaddr(regs, DW_PARAMS);
+	autocfg = dw_params >> DW_PARAMS_EN & 0x1;
+
+	if (autocfg)
+		nr_channels = (dw_params >> DW_PARAMS_NR_CHAN & 0x7) + 1;
+	else
+		nr_channels = pdata->nr_channels;
+
+	size = sizeof(struct dw_dma) + nr_channels * sizeof(struct dw_dma_chan);
+	dw = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
 	if (!dw)
 		return -ENOMEM;
 
-	if (!request_mem_region(io->start, DW_REGLEN, pdev->dev.driver->name)) {
-		err = -EBUSY;
-		goto err_kfree;
-	}
+	dw->clk = devm_clk_get(&pdev->dev, "hclk");
+	if (IS_ERR(dw->clk))
+		return PTR_ERR(dw->clk);
+	clk_prepare_enable(dw->clk);
 
-	dw->regs = ioremap(io->start, DW_REGLEN);
-	if (!dw->regs) {
-		err = -ENOMEM;
-		goto err_release_r;
-	}
+	dw->regs = regs;
+
+	/* get hardware configuration parameters */
+	if (autocfg) {
+		max_blk_size = dma_readl(dw, MAX_BLK_SIZE);
 
-	dw->clk = clk_get(&pdev->dev, "hclk");
-	if (IS_ERR(dw->clk)) {
-		err = PTR_ERR(dw->clk);
-		goto err_clk;
+		dw->nr_masters = (dw_params >> DW_PARAMS_NR_MASTER & 3) + 1;
+		for (i = 0; i < dw->nr_masters; i++) {
+			dw->data_width[i] =
+				(dw_params >> DW_PARAMS_DATA_WIDTH(i) & 3) + 2;
+		}
+	} else {
+		dw->nr_masters = pdata->nr_masters;
+		memcpy(dw->data_width, pdata->data_width, 4);
 	}
-	clk_prepare_enable(dw->clk);
 
 	/* Calculate all channel mask before DMA setup */
-	dw->all_chan_mask = (1 << pdata->nr_channels) - 1;
+	dw->all_chan_mask = (1 << nr_channels) - 1;
 
 	/* force dma off, just in case */
 	dw_dma_off(dw);
@@ -1423,17 +1536,19 @@ static int __devinit dw_probe(struct platform_device *pdev)
 	/* disable BLOCK interrupts as well */
 	channel_clear_bit(dw, MASK.BLOCK, dw->all_chan_mask);
 
-	err = request_irq(irq, dw_dma_interrupt, 0, "dw_dmac", dw);
+	err = devm_request_irq(&pdev->dev, irq, dw_dma_interrupt, 0,
+			       "dw_dmac", dw);
 	if (err)
-		goto err_irq;
+		return err;
 
 	platform_set_drvdata(pdev, dw);
 
 	tasklet_init(&dw->tasklet, dw_dma_tasklet, (unsigned long)dw);
 
 	INIT_LIST_HEAD(&dw->dma.channels);
-	for (i = 0; i < pdata->nr_channels; i++) {
+	for (i = 0; i < nr_channels; i++) {
 		struct dw_dma_chan	*dwc = &dw->chan[i];
+		int			r = nr_channels - i - 1;
 
 		dwc->chan.device = &dw->dma;
 		dma_cookie_init(&dwc->chan);
@@ -1445,7 +1560,7 @@ static int __devinit dw_probe(struct platform_device *pdev)
 
 		/* 7 is highest priority & 0 is lowest. */
 		if (pdata->chan_priority == CHAN_PRIORITY_ASCENDING)
-			dwc->priority = pdata->nr_channels - i - 1;
+			dwc->priority = r;
 		else
 			dwc->priority = i;
 
@@ -1458,6 +1573,32 @@ static int __devinit dw_probe(struct platform_device *pdev)
 		INIT_LIST_HEAD(&dwc->free_list);
 
 		channel_clear_bit(dw, CH_EN, dwc->mask);
+
+		dwc->dw = dw;
+
+		/* hardware configuration */
+		if (autocfg) {
+			unsigned int dwc_params;
+
+			dwc_params = dma_read_byaddr(regs + r * sizeof(u32),
+						     DWC_PARAMS);
+
+			/* Decode maximum block size for given channel. The
+			 * stored 4 bit value represents blocks from 0x00 for 3
+			 * up to 0x0a for 4095. */
+			dwc->block_size =
+				(4 << ((max_blk_size >> 4 * i) & 0xf)) - 1;
+			dwc->nollp =
+				(dwc_params >> DWC_PARAMS_MBLK_EN & 0x1) == 0;
+		} else {
+			dwc->block_size = pdata->block_size;
+
+			/* Check if channel supports multi block transfer */
+			channel_writel(dwc, LLP, 0xfffffffc);
+			dwc->nollp =
+				(channel_readl(dwc, LLP) & 0xfffffffc) == 0;
+			channel_writel(dwc, LLP, 0);
+		}
 	}
 
 	/* Clear all interrupts on all channels. */
@@ -1486,35 +1627,21 @@ static int __devinit dw_probe(struct platform_device *pdev)
 	dma_writel(dw, CFG, DW_CFG_DMA_EN);
 
 	printk(KERN_INFO "%s: DesignWare DMA Controller, %d channels\n",
-			dev_name(&pdev->dev), pdata->nr_channels);
+			dev_name(&pdev->dev), nr_channels);
 
 	dma_async_device_register(&dw->dma);
 
 	return 0;
-
-err_irq:
-	clk_disable_unprepare(dw->clk);
-	clk_put(dw->clk);
-err_clk:
-	iounmap(dw->regs);
-	dw->regs = NULL;
-err_release_r:
-	release_resource(io);
-err_kfree:
-	kfree(dw);
-	return err;
 }
 
 static int __devexit dw_remove(struct platform_device *pdev)
 {
 	struct dw_dma		*dw = platform_get_drvdata(pdev);
 	struct dw_dma_chan	*dwc, *_dwc;
-	struct resource		*io;
 
 	dw_dma_off(dw);
 	dma_async_device_unregister(&dw->dma);
 
-	free_irq(platform_get_irq(pdev, 0), dw);
 	tasklet_kill(&dw->tasklet);
 
 	list_for_each_entry_safe(dwc, _dwc, &dw->dma.channels,
@@ -1523,17 +1650,6 @@ static int __devexit dw_remove(struct platform_device *pdev)
 		channel_clear_bit(dw, CH_EN, dwc->mask);
 	}
 
-	clk_disable_unprepare(dw->clk);
-	clk_put(dw->clk);
-
-	iounmap(dw->regs);
-	dw->regs = NULL;
-
-	io = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	release_mem_region(io->start, DW_REGLEN);
-
-	kfree(dw);
-
 	return 0;
 }
 
diff --git a/drivers/dma/dw_dmac_regs.h b/drivers/dma/dw_dmac_regs.h
index 50830bee087..ff39fa6cd2b 100644
--- a/drivers/dma/dw_dmac_regs.h
+++ b/drivers/dma/dw_dmac_regs.h
@@ -82,9 +82,39 @@ struct dw_dma_regs {
 	DW_REG(ID);
 	DW_REG(TEST);
 
+	/* reserved */
+	DW_REG(__reserved0);
+	DW_REG(__reserved1);
+
 	/* optional encoded params, 0x3c8..0x3f7 */
+	u32	__reserved;
+
+	/* per-channel configuration registers */
+	u32	DWC_PARAMS[DW_DMA_MAX_NR_CHANNELS];
+	u32	MULTI_BLK_TYPE;
+	u32	MAX_BLK_SIZE;
+
+	/* top-level parameters */
+	u32	DW_PARAMS;
 };
 
+/* To access the registers in early stage of probe */
+#define dma_read_byaddr(addr, name) \
+	readl((addr) + offsetof(struct dw_dma_regs, name))
+
+/* Bitfields in DW_PARAMS */
+#define DW_PARAMS_NR_CHAN	8		/* number of channels */
+#define DW_PARAMS_NR_MASTER	11		/* number of AHB masters */
+#define DW_PARAMS_DATA_WIDTH(n)	(15 + 2 * (n))
+#define DW_PARAMS_DATA_WIDTH1	15		/* master 1 data width */
+#define DW_PARAMS_DATA_WIDTH2	17		/* master 2 data width */
+#define DW_PARAMS_DATA_WIDTH3	19		/* master 3 data width */
+#define DW_PARAMS_DATA_WIDTH4	21		/* master 4 data width */
+#define DW_PARAMS_EN		28		/* encoded parameters */
+
+/* Bitfields in DWC_PARAMS */
+#define DWC_PARAMS_MBLK_EN	11		/* multi block transfer */
+
 /* Bitfields in CTL_LO */
 #define DWC_CTLL_INT_EN		(1 << 0)	/* irqs enabled? */
 #define DWC_CTLL_DST_WIDTH(n)	((n)<<1)	/* bytes per element */
@@ -140,10 +170,9 @@ struct dw_dma_regs {
 /* Bitfields in CFG */
 #define DW_CFG_DMA_EN		(1 << 0)
 
-#define DW_REGLEN		0x400
-
 enum dw_dmac_flags {
 	DW_DMA_IS_CYCLIC = 0,
+	DW_DMA_IS_SOFT_LLP = 1,
 };
 
 struct dw_dma_chan {
@@ -154,6 +183,10 @@ struct dw_dma_chan {
 	bool			paused;
 	bool			initialized;
 
+	/* software emulation of the LLP transfers */
+	struct list_head	*tx_list;
+	struct list_head	*tx_node_active;
+
 	spinlock_t		lock;
 
 	/* these other elements are all protected by lock */
@@ -165,8 +198,15 @@ struct dw_dma_chan {
 
 	unsigned int		descs_allocated;
 
+	/* hardware configuration */
+	unsigned int		block_size;
+	bool			nollp;
+
 	/* configuration passed via DMA_SLAVE_CONFIG */
 	struct dma_slave_config dma_sconfig;
+
+	/* backlink to dw_dma */
+	struct dw_dma		*dw;
 };
 
 static inline struct dw_dma_chan_regs __iomem *
@@ -193,6 +233,10 @@ struct dw_dma {
 
 	u8			all_chan_mask;
 
+	/* hardware configuration */
+	unsigned char		nr_masters;
+	unsigned char		data_width[4];
+
 	struct dw_dma_chan	chan[0];
 };
 
diff --git a/drivers/dma/edma.c b/drivers/dma/edma.c
new file mode 100644
index 00000000000..05aea3ce850
--- /dev/null
+++ b/drivers/dma/edma.c
@@ -0,0 +1,671 @@
+/*
+ * TI EDMA DMA engine driver
+ *
+ * Copyright 2012 Texas Instruments
+ *
+ * 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 version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/dmaengine.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+#include <mach/edma.h>
+
+#include "dmaengine.h"
+#include "virt-dma.h"
+
+/*
+ * This will go away when the private EDMA API is folded
+ * into this driver and the platform device(s) are
+ * instantiated in the arch code. We can only get away
+ * with this simplification because DA8XX may not be built
+ * in the same kernel image with other DaVinci parts. This
+ * avoids having to sprinkle dmaengine driver platform devices
+ * and data throughout all the existing board files.
+ */
+#ifdef CONFIG_ARCH_DAVINCI_DA8XX
+#define EDMA_CTLRS	2
+#define EDMA_CHANS	32
+#else
+#define EDMA_CTLRS	1
+#define EDMA_CHANS	64
+#endif /* CONFIG_ARCH_DAVINCI_DA8XX */
+
+/* Max of 16 segments per channel to conserve PaRAM slots */
+#define MAX_NR_SG		16
+#define EDMA_MAX_SLOTS		MAX_NR_SG
+#define EDMA_DESCRIPTORS	16
+
+struct edma_desc {
+	struct virt_dma_desc		vdesc;
+	struct list_head		node;
+	int				absync;
+	int				pset_nr;
+	struct edmacc_param		pset[0];
+};
+
+struct edma_cc;
+
+struct edma_chan {
+	struct virt_dma_chan		vchan;
+	struct list_head		node;
+	struct edma_desc		*edesc;
+	struct edma_cc			*ecc;
+	int				ch_num;
+	bool				alloced;
+	int				slot[EDMA_MAX_SLOTS];
+	dma_addr_t			addr;
+	int				addr_width;
+	int				maxburst;
+};
+
+struct edma_cc {
+	int				ctlr;
+	struct dma_device		dma_slave;
+	struct edma_chan		slave_chans[EDMA_CHANS];
+	int				num_slave_chans;
+	int				dummy_slot;
+};
+
+static inline struct edma_cc *to_edma_cc(struct dma_device *d)
+{
+	return container_of(d, struct edma_cc, dma_slave);
+}
+
+static inline struct edma_chan *to_edma_chan(struct dma_chan *c)
+{
+	return container_of(c, struct edma_chan, vchan.chan);
+}
+
+static inline struct edma_desc
+*to_edma_desc(struct dma_async_tx_descriptor *tx)
+{
+	return container_of(tx, struct edma_desc, vdesc.tx);
+}
+
+static void edma_desc_free(struct virt_dma_desc *vdesc)
+{
+	kfree(container_of(vdesc, struct edma_desc, vdesc));
+}
+
+/* Dispatch a queued descriptor to the controller (caller holds lock) */
+static void edma_execute(struct edma_chan *echan)
+{
+	struct virt_dma_desc *vdesc = vchan_next_desc(&echan->vchan);
+	struct edma_desc *edesc;
+	int i;
+
+	if (!vdesc) {
+		echan->edesc = NULL;
+		return;
+	}
+
+	list_del(&vdesc->node);
+
+	echan->edesc = edesc = to_edma_desc(&vdesc->tx);
+
+	/* Write descriptor PaRAM set(s) */
+	for (i = 0; i < edesc->pset_nr; i++) {
+		edma_write_slot(echan->slot[i], &edesc->pset[i]);
+		dev_dbg(echan->vchan.chan.device->dev,
+			"\n pset[%d]:\n"
+			"  chnum\t%d\n"
+			"  slot\t%d\n"
+			"  opt\t%08x\n"
+			"  src\t%08x\n"
+			"  dst\t%08x\n"
+			"  abcnt\t%08x\n"
+			"  ccnt\t%08x\n"
+			"  bidx\t%08x\n"
+			"  cidx\t%08x\n"
+			"  lkrld\t%08x\n",
+			i, echan->ch_num, echan->slot[i],
+			edesc->pset[i].opt,
+			edesc->pset[i].src,
+			edesc->pset[i].dst,
+			edesc->pset[i].a_b_cnt,
+			edesc->pset[i].ccnt,
+			edesc->pset[i].src_dst_bidx,
+			edesc->pset[i].src_dst_cidx,
+			edesc->pset[i].link_bcntrld);
+		/* Link to the previous slot if not the last set */
+		if (i != (edesc->pset_nr - 1))
+			edma_link(echan->slot[i], echan->slot[i+1]);
+		/* Final pset links to the dummy pset */
+		else
+			edma_link(echan->slot[i], echan->ecc->dummy_slot);
+	}
+
+	edma_start(echan->ch_num);
+}
+
+static int edma_terminate_all(struct edma_chan *echan)
+{
+	unsigned long flags;
+	LIST_HEAD(head);
+
+	spin_lock_irqsave(&echan->vchan.lock, flags);
+
+	/*
+	 * Stop DMA activity: we assume the callback will not be called
+	 * after edma_dma() returns (even if it does, it will see
+	 * echan->edesc is NULL and exit.)
+	 */
+	if (echan->edesc) {
+		echan->edesc = NULL;
+		edma_stop(echan->ch_num);
+	}
+
+	vchan_get_all_descriptors(&echan->vchan, &head);
+	spin_unlock_irqrestore(&echan->vchan.lock, flags);
+	vchan_dma_desc_free_list(&echan->vchan, &head);
+
+	return 0;
+}
+
+
+static int edma_slave_config(struct edma_chan *echan,
+	struct dma_slave_config *config)
+{
+	if ((config->src_addr_width > DMA_SLAVE_BUSWIDTH_4_BYTES) ||
+	    (config->dst_addr_width > DMA_SLAVE_BUSWIDTH_4_BYTES))
+		return -EINVAL;
+
+	if (config->direction == DMA_MEM_TO_DEV) {
+		if (config->dst_addr)
+			echan->addr = config->dst_addr;
+		if (config->dst_addr_width)
+			echan->addr_width = config->dst_addr_width;
+		if (config->dst_maxburst)
+			echan->maxburst = config->dst_maxburst;
+	} else if (config->direction == DMA_DEV_TO_MEM) {
+		if (config->src_addr)
+			echan->addr = config->src_addr;
+		if (config->src_addr_width)
+			echan->addr_width = config->src_addr_width;
+		if (config->src_maxburst)
+			echan->maxburst = config->src_maxburst;
+	}
+
+	return 0;
+}
+
+static int edma_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
+			unsigned long arg)
+{
+	int ret = 0;
+	struct dma_slave_config *config;
+	struct edma_chan *echan = to_edma_chan(chan);
+
+	switch (cmd) {
+	case DMA_TERMINATE_ALL:
+		edma_terminate_all(echan);
+		break;
+	case DMA_SLAVE_CONFIG:
+		config = (struct dma_slave_config *)arg;
+		ret = edma_slave_config(echan, config);
+		break;
+	default:
+		ret = -ENOSYS;
+	}
+
+	return ret;
+}
+
+static struct dma_async_tx_descriptor *edma_prep_slave_sg(
+	struct dma_chan *chan, struct scatterlist *sgl,
+	unsigned int sg_len, enum dma_transfer_direction direction,
+	unsigned long tx_flags, void *context)
+{
+	struct edma_chan *echan = to_edma_chan(chan);
+	struct device *dev = chan->device->dev;
+	struct edma_desc *edesc;
+	struct scatterlist *sg;
+	int i;
+	int acnt, bcnt, ccnt, src, dst, cidx;
+	int src_bidx, dst_bidx, src_cidx, dst_cidx;
+
+	if (unlikely(!echan || !sgl || !sg_len))
+		return NULL;
+
+	if (echan->addr_width == DMA_SLAVE_BUSWIDTH_UNDEFINED) {
+		dev_err(dev, "Undefined slave buswidth\n");
+		return NULL;
+	}
+
+	if (sg_len > MAX_NR_SG) {
+		dev_err(dev, "Exceeded max SG segments %d > %d\n",
+			sg_len, MAX_NR_SG);
+		return NULL;
+	}
+
+	edesc = kzalloc(sizeof(*edesc) + sg_len *
+		sizeof(edesc->pset[0]), GFP_ATOMIC);
+	if (!edesc) {
+		dev_dbg(dev, "Failed to allocate a descriptor\n");
+		return NULL;
+	}
+
+	edesc->pset_nr = sg_len;
+
+	for_each_sg(sgl, sg, sg_len, i) {
+		/* Allocate a PaRAM slot, if needed */
+		if (echan->slot[i] < 0) {
+			echan->slot[i] =
+				edma_alloc_slot(EDMA_CTLR(echan->ch_num),
+						EDMA_SLOT_ANY);
+			if (echan->slot[i] < 0) {
+				dev_err(dev, "Failed to allocate slot\n");
+				return NULL;
+			}
+		}
+
+		acnt = echan->addr_width;
+
+		/*
+		 * If the maxburst is equal to the fifo width, use
+		 * A-synced transfers. This allows for large contiguous
+		 * buffer transfers using only one PaRAM set.
+		 */
+		if (echan->maxburst == 1) {
+			edesc->absync = false;
+			ccnt = sg_dma_len(sg) / acnt / (SZ_64K - 1);
+			bcnt = sg_dma_len(sg) / acnt - ccnt * (SZ_64K - 1);
+			if (bcnt)
+				ccnt++;
+			else
+				bcnt = SZ_64K - 1;
+			cidx = acnt;
+		/*
+		 * If maxburst is greater than the fifo address_width,
+		 * use AB-synced transfers where A count is the fifo
+		 * address_width and B count is the maxburst. In this
+		 * case, we are limited to transfers of C count frames
+		 * of (address_width * maxburst) where C count is limited
+		 * to SZ_64K-1. This places an upper bound on the length
+		 * of an SG segment that can be handled.
+		 */
+		} else {
+			edesc->absync = true;
+			bcnt = echan->maxburst;
+			ccnt = sg_dma_len(sg) / (acnt * bcnt);
+			if (ccnt > (SZ_64K - 1)) {
+				dev_err(dev, "Exceeded max SG segment size\n");
+				return NULL;
+			}
+			cidx = acnt * bcnt;
+		}
+
+		if (direction == DMA_MEM_TO_DEV) {
+			src = sg_dma_address(sg);
+			dst = echan->addr;
+			src_bidx = acnt;
+			src_cidx = cidx;
+			dst_bidx = 0;
+			dst_cidx = 0;
+		} else {
+			src = echan->addr;
+			dst = sg_dma_address(sg);
+			src_bidx = 0;
+			src_cidx = 0;
+			dst_bidx = acnt;
+			dst_cidx = cidx;
+		}
+
+		edesc->pset[i].opt = EDMA_TCC(EDMA_CHAN_SLOT(echan->ch_num));
+		/* Configure A or AB synchronized transfers */
+		if (edesc->absync)
+			edesc->pset[i].opt |= SYNCDIM;
+		/* If this is the last set, enable completion interrupt flag */
+		if (i == sg_len - 1)
+			edesc->pset[i].opt |= TCINTEN;
+
+		edesc->pset[i].src = src;
+		edesc->pset[i].dst = dst;
+
+		edesc->pset[i].src_dst_bidx = (dst_bidx << 16) | src_bidx;
+		edesc->pset[i].src_dst_cidx = (dst_cidx << 16) | src_cidx;
+
+		edesc->pset[i].a_b_cnt = bcnt << 16 | acnt;
+		edesc->pset[i].ccnt = ccnt;
+		edesc->pset[i].link_bcntrld = 0xffffffff;
+
+	}
+
+	return vchan_tx_prep(&echan->vchan, &edesc->vdesc, tx_flags);
+}
+
+static void edma_callback(unsigned ch_num, u16 ch_status, void *data)
+{
+	struct edma_chan *echan = data;
+	struct device *dev = echan->vchan.chan.device->dev;
+	struct edma_desc *edesc;
+	unsigned long flags;
+
+	/* Stop the channel */
+	edma_stop(echan->ch_num);
+
+	switch (ch_status) {
+	case DMA_COMPLETE:
+		dev_dbg(dev, "transfer complete on channel %d\n", ch_num);
+
+		spin_lock_irqsave(&echan->vchan.lock, flags);
+
+		edesc = echan->edesc;
+		if (edesc) {
+			edma_execute(echan);
+			vchan_cookie_complete(&edesc->vdesc);
+		}
+
+		spin_unlock_irqrestore(&echan->vchan.lock, flags);
+
+		break;
+	case DMA_CC_ERROR:
+		dev_dbg(dev, "transfer error on channel %d\n", ch_num);
+		break;
+	default:
+		break;
+	}
+}
+
+/* Alloc channel resources */
+static int edma_alloc_chan_resources(struct dma_chan *chan)
+{
+	struct edma_chan *echan = to_edma_chan(chan);
+	struct device *dev = chan->device->dev;
+	int ret;
+	int a_ch_num;
+	LIST_HEAD(descs);
+
+	a_ch_num = edma_alloc_channel(echan->ch_num, edma_callback,
+					chan, EVENTQ_DEFAULT);
+
+	if (a_ch_num < 0) {
+		ret = -ENODEV;
+		goto err_no_chan;
+	}
+
+	if (a_ch_num != echan->ch_num) {
+		dev_err(dev, "failed to allocate requested channel %u:%u\n",
+			EDMA_CTLR(echan->ch_num),
+			EDMA_CHAN_SLOT(echan->ch_num));
+		ret = -ENODEV;
+		goto err_wrong_chan;
+	}
+
+	echan->alloced = true;
+	echan->slot[0] = echan->ch_num;
+
+	dev_info(dev, "allocated channel for %u:%u\n",
+		 EDMA_CTLR(echan->ch_num), EDMA_CHAN_SLOT(echan->ch_num));
+
+	return 0;
+
+err_wrong_chan:
+	edma_free_channel(a_ch_num);
+err_no_chan:
+	return ret;
+}
+
+/* Free channel resources */
+static void edma_free_chan_resources(struct dma_chan *chan)
+{
+	struct edma_chan *echan = to_edma_chan(chan);
+	struct device *dev = chan->device->dev;
+	int i;
+
+	/* Terminate transfers */
+	edma_stop(echan->ch_num);
+
+	vchan_free_chan_resources(&echan->vchan);
+
+	/* Free EDMA PaRAM slots */
+	for (i = 1; i < EDMA_MAX_SLOTS; i++) {
+		if (echan->slot[i] >= 0) {
+			edma_free_slot(echan->slot[i]);
+			echan->slot[i] = -1;
+		}
+	}
+
+	/* Free EDMA channel */
+	if (echan->alloced) {
+		edma_free_channel(echan->ch_num);
+		echan->alloced = false;
+	}
+
+	dev_info(dev, "freeing channel for %u\n", echan->ch_num);
+}
+
+/* Send pending descriptor to hardware */
+static void edma_issue_pending(struct dma_chan *chan)
+{
+	struct edma_chan *echan = to_edma_chan(chan);
+	unsigned long flags;
+
+	spin_lock_irqsave(&echan->vchan.lock, flags);
+	if (vchan_issue_pending(&echan->vchan) && !echan->edesc)
+		edma_execute(echan);
+	spin_unlock_irqrestore(&echan->vchan.lock, flags);
+}
+
+static size_t edma_desc_size(struct edma_desc *edesc)
+{
+	int i;
+	size_t size;
+
+	if (edesc->absync)
+		for (size = i = 0; i < edesc->pset_nr; i++)
+			size += (edesc->pset[i].a_b_cnt & 0xffff) *
+				(edesc->pset[i].a_b_cnt >> 16) *
+				 edesc->pset[i].ccnt;
+	else
+		size = (edesc->pset[0].a_b_cnt & 0xffff) *
+			(edesc->pset[0].a_b_cnt >> 16) +
+			(edesc->pset[0].a_b_cnt & 0xffff) *
+			(SZ_64K - 1) * edesc->pset[0].ccnt;
+
+	return size;
+}
+
+/* Check request completion status */
+static enum dma_status edma_tx_status(struct dma_chan *chan,
+				      dma_cookie_t cookie,
+				      struct dma_tx_state *txstate)
+{
+	struct edma_chan *echan = to_edma_chan(chan);
+	struct virt_dma_desc *vdesc;
+	enum dma_status ret;
+	unsigned long flags;
+
+	ret = dma_cookie_status(chan, cookie, txstate);
+	if (ret == DMA_SUCCESS || !txstate)
+		return ret;
+
+	spin_lock_irqsave(&echan->vchan.lock, flags);
+	vdesc = vchan_find_desc(&echan->vchan, cookie);
+	if (vdesc) {
+		txstate->residue = edma_desc_size(to_edma_desc(&vdesc->tx));
+	} else if (echan->edesc && echan->edesc->vdesc.tx.cookie == cookie) {
+		struct edma_desc *edesc = echan->edesc;
+		txstate->residue = edma_desc_size(edesc);
+	} else {
+		txstate->residue = 0;
+	}
+	spin_unlock_irqrestore(&echan->vchan.lock, flags);
+
+	return ret;
+}
+
+static void __init edma_chan_init(struct edma_cc *ecc,
+				  struct dma_device *dma,
+				  struct edma_chan *echans)
+{
+	int i, j;
+
+	for (i = 0; i < EDMA_CHANS; i++) {
+		struct edma_chan *echan = &echans[i];
+		echan->ch_num = EDMA_CTLR_CHAN(ecc->ctlr, i);
+		echan->ecc = ecc;
+		echan->vchan.desc_free = edma_desc_free;
+
+		vchan_init(&echan->vchan, dma);
+
+		INIT_LIST_HEAD(&echan->node);
+		for (j = 0; j < EDMA_MAX_SLOTS; j++)
+			echan->slot[j] = -1;
+	}
+}
+
+static void edma_dma_init(struct edma_cc *ecc, struct dma_device *dma,
+			  struct device *dev)
+{
+	dma->device_prep_slave_sg = edma_prep_slave_sg;
+	dma->device_alloc_chan_resources = edma_alloc_chan_resources;
+	dma->device_free_chan_resources = edma_free_chan_resources;
+	dma->device_issue_pending = edma_issue_pending;
+	dma->device_tx_status = edma_tx_status;
+	dma->device_control = edma_control;
+	dma->dev = dev;
+
+	INIT_LIST_HEAD(&dma->channels);
+}
+
+static int __devinit edma_probe(struct platform_device *pdev)
+{
+	struct edma_cc *ecc;
+	int ret;
+
+	ecc = devm_kzalloc(&pdev->dev, sizeof(*ecc), GFP_KERNEL);
+	if (!ecc) {
+		dev_err(&pdev->dev, "Can't allocate controller\n");
+		return -ENOMEM;
+	}
+
+	ecc->ctlr = pdev->id;
+	ecc->dummy_slot = edma_alloc_slot(ecc->ctlr, EDMA_SLOT_ANY);
+	if (ecc->dummy_slot < 0) {
+		dev_err(&pdev->dev, "Can't allocate PaRAM dummy slot\n");
+		return -EIO;
+	}
+
+	dma_cap_zero(ecc->dma_slave.cap_mask);
+	dma_cap_set(DMA_SLAVE, ecc->dma_slave.cap_mask);
+
+	edma_dma_init(ecc, &ecc->dma_slave, &pdev->dev);
+
+	edma_chan_init(ecc, &ecc->dma_slave, ecc->slave_chans);
+
+	ret = dma_async_device_register(&ecc->dma_slave);
+	if (ret)
+		goto err_reg1;
+
+	platform_set_drvdata(pdev, ecc);
+
+	dev_info(&pdev->dev, "TI EDMA DMA engine driver\n");
+
+	return 0;
+
+err_reg1:
+	edma_free_slot(ecc->dummy_slot);
+	return ret;
+}
+
+static int __devexit edma_remove(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct edma_cc *ecc = dev_get_drvdata(dev);
+
+	dma_async_device_unregister(&ecc->dma_slave);
+	edma_free_slot(ecc->dummy_slot);
+
+	return 0;
+}
+
+static struct platform_driver edma_driver = {
+	.probe		= edma_probe,
+	.remove		= __devexit_p(edma_remove),
+	.driver = {
+		.name = "edma-dma-engine",
+		.owner = THIS_MODULE,
+	},
+};
+
+bool edma_filter_fn(struct dma_chan *chan, void *param)
+{
+	if (chan->device->dev->driver == &edma_driver.driver) {
+		struct edma_chan *echan = to_edma_chan(chan);
+		unsigned ch_req = *(unsigned *)param;
+		return ch_req == echan->ch_num;
+	}
+	return false;
+}
+EXPORT_SYMBOL(edma_filter_fn);
+
+static struct platform_device *pdev0, *pdev1;
+
+static const struct platform_device_info edma_dev_info0 = {
+	.name = "edma-dma-engine",
+	.id = 0,
+	.dma_mask = DMA_BIT_MASK(32),
+};
+
+static const struct platform_device_info edma_dev_info1 = {
+	.name = "edma-dma-engine",
+	.id = 1,
+	.dma_mask = DMA_BIT_MASK(32),
+};
+
+static int edma_init(void)
+{
+	int ret = platform_driver_register(&edma_driver);
+
+	if (ret == 0) {
+		pdev0 = platform_device_register_full(&edma_dev_info0);
+		if (IS_ERR(pdev0)) {
+			platform_driver_unregister(&edma_driver);
+			ret = PTR_ERR(pdev0);
+			goto out;
+		}
+	}
+
+	if (EDMA_CTLRS == 2) {
+		pdev1 = platform_device_register_full(&edma_dev_info1);
+		if (IS_ERR(pdev1)) {
+			platform_driver_unregister(&edma_driver);
+			platform_device_unregister(pdev0);
+			ret = PTR_ERR(pdev1);
+		}
+	}
+
+out:
+	return ret;
+}
+subsys_initcall(edma_init);
+
+static void __exit edma_exit(void)
+{
+	platform_device_unregister(pdev0);
+	if (pdev1)
+		platform_device_unregister(pdev1);
+	platform_driver_unregister(&edma_driver);
+}
+module_exit(edma_exit);
+
+MODULE_AUTHOR("Matt Porter <mporter@ti.com>");
+MODULE_DESCRIPTION("TI EDMA DMA engine driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/dma/ep93xx_dma.c b/drivers/dma/ep93xx_dma.c
index 64256f64425..bcfde400904 100644
--- a/drivers/dma/ep93xx_dma.c
+++ b/drivers/dma/ep93xx_dma.c
@@ -1120,6 +1120,7 @@ fail:
  * @buf_len: length of the buffer (in bytes)
  * @period_len: length of a single period
  * @dir: direction of the operation
+ * @flags: tx descriptor status flags
  * @context: operation context (ignored)
  *
  * Prepares a descriptor for cyclic DMA operation. This means that once the
@@ -1133,7 +1134,8 @@ fail:
 static struct dma_async_tx_descriptor *
 ep93xx_dma_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t dma_addr,
 			   size_t buf_len, size_t period_len,
-			   enum dma_transfer_direction dir, void *context)
+			   enum dma_transfer_direction dir, unsigned long flags,
+			   void *context)
 {
 	struct ep93xx_dma_chan *edmac = to_ep93xx_dma_chan(chan);
 	struct ep93xx_dma_desc *desc, *first;
diff --git a/drivers/dma/imx-dma.c b/drivers/dma/imx-dma.c
index 2a3fab289db..f11b5b2b1a1 100644
--- a/drivers/dma/imx-dma.c
+++ b/drivers/dma/imx-dma.c
@@ -801,7 +801,7 @@ static struct dma_async_tx_descriptor *imxdma_prep_slave_sg(
 static struct dma_async_tx_descriptor *imxdma_prep_dma_cyclic(
 		struct dma_chan *chan, dma_addr_t dma_addr, size_t buf_len,
 		size_t period_len, enum dma_transfer_direction direction,
-		void *context)
+		unsigned long flags, void *context)
 {
 	struct imxdma_channel *imxdmac = to_imxdma_chan(chan);
 	struct imxdma_engine *imxdma = imxdmac->imxdma;
diff --git a/drivers/dma/imx-sdma.c b/drivers/dma/imx-sdma.c
index 1b781d6ac42..c099ca0846f 100644
--- a/drivers/dma/imx-sdma.c
+++ b/drivers/dma/imx-sdma.c
@@ -1012,7 +1012,7 @@ err_out:
 static struct dma_async_tx_descriptor *sdma_prep_dma_cyclic(
 		struct dma_chan *chan, dma_addr_t dma_addr, size_t buf_len,
 		size_t period_len, enum dma_transfer_direction direction,
-		void *context)
+		unsigned long flags, void *context)
 {
 	struct sdma_channel *sdmac = to_sdma_chan(chan);
 	struct sdma_engine *sdma = sdmac->sdma;
diff --git a/drivers/dma/ioat/dma_v2.c b/drivers/dma/ioat/dma_v2.c
index 86895760b59..b9d66785144 100644
--- a/drivers/dma/ioat/dma_v2.c
+++ b/drivers/dma/ioat/dma_v2.c
@@ -434,12 +434,11 @@ static struct ioat_ring_ent *ioat2_alloc_ring_ent(struct dma_chan *chan, gfp_t f
 		return NULL;
 	memset(hw, 0, sizeof(*hw));
 
-	desc = kmem_cache_alloc(ioat2_cache, flags);
+	desc = kmem_cache_zalloc(ioat2_cache, flags);
 	if (!desc) {
 		pci_pool_free(dma->dma_pool, hw, phys);
 		return NULL;
 	}
-	memset(desc, 0, sizeof(*desc));
 
 	dma_async_tx_descriptor_init(&desc->txd, chan);
 	desc->txd.tx_submit = ioat2_tx_submit_unlock;
diff --git a/drivers/dma/ioat/pci.c b/drivers/dma/ioat/pci.c
index 5e3a40f7994..c0573061b45 100644
--- a/drivers/dma/ioat/pci.c
+++ b/drivers/dma/ioat/pci.c
@@ -40,6 +40,17 @@ MODULE_VERSION(IOAT_DMA_VERSION);
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_AUTHOR("Intel Corporation");
 
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB0	0x0e20
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB1	0x0e21
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB2	0x0e22
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB3	0x0e23
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB4	0x0e24
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB5	0x0e25
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB6	0x0e26
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB7	0x0e27
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB8	0x0e2e
+#define PCI_DEVICE_ID_INTEL_IOAT_IVB9	0x0e2f
+
 static struct pci_device_id ioat_pci_tbl[] = {
 	/* I/OAT v1 platforms */
 	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT) },
@@ -83,6 +94,17 @@ static struct pci_device_id ioat_pci_tbl[] = {
 	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_SNB8) },
 	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_SNB9) },
 
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB0) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB1) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB2) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB3) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB4) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB5) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB6) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB7) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB8) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_IOAT_IVB9) },
+
 	{ 0, }
 };
 MODULE_DEVICE_TABLE(pci, ioat_pci_tbl);
diff --git a/drivers/dma/mmp_pdma.c b/drivers/dma/mmp_pdma.c
new file mode 100644
index 00000000000..14da1f403ed
--- /dev/null
+++ b/drivers/dma/mmp_pdma.c
@@ -0,0 +1,875 @@
+/*
+ * Copyright 2012 Marvell International Ltd.
+ *
+ * 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/types.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/slab.h>
+#include <linux/dmaengine.h>
+#include <linux/platform_device.h>
+#include <linux/device.h>
+#include <linux/platform_data/mmp_dma.h>
+#include <linux/dmapool.h>
+#include <linux/of_device.h>
+#include <linux/of.h>
+
+#include "dmaengine.h"
+
+#define DCSR		0x0000
+#define DALGN		0x00a0
+#define DINT		0x00f0
+#define DDADR		0x0200
+#define DSADR		0x0204
+#define DTADR		0x0208
+#define DCMD		0x020c
+
+#define DCSR_RUN	(1 << 31)	/* Run Bit (read / write) */
+#define DCSR_NODESC	(1 << 30)	/* No-Descriptor Fetch (read / write) */
+#define DCSR_STOPIRQEN	(1 << 29)	/* Stop Interrupt Enable (read / write) */
+#define DCSR_REQPEND	(1 << 8)	/* Request Pending (read-only) */
+#define DCSR_STOPSTATE	(1 << 3)	/* Stop State (read-only) */
+#define DCSR_ENDINTR	(1 << 2)	/* End Interrupt (read / write) */
+#define DCSR_STARTINTR	(1 << 1)	/* Start Interrupt (read / write) */
+#define DCSR_BUSERR	(1 << 0)	/* Bus Error Interrupt (read / write) */
+
+#define DCSR_EORIRQEN	(1 << 28)       /* End of Receive Interrupt Enable (R/W) */
+#define DCSR_EORJMPEN	(1 << 27)       /* Jump to next descriptor on EOR */
+#define DCSR_EORSTOPEN	(1 << 26)       /* STOP on an EOR */
+#define DCSR_SETCMPST	(1 << 25)       /* Set Descriptor Compare Status */
+#define DCSR_CLRCMPST	(1 << 24)       /* Clear Descriptor Compare Status */
+#define DCSR_CMPST	(1 << 10)       /* The Descriptor Compare Status */
+#define DCSR_EORINTR	(1 << 9)        /* The end of Receive */
+
+#define DRCMR_MAPVLD	(1 << 7)	/* Map Valid (read / write) */
+#define DRCMR_CHLNUM	0x1f		/* mask for Channel Number (read / write) */
+
+#define DDADR_DESCADDR	0xfffffff0	/* Address of next descriptor (mask) */
+#define DDADR_STOP	(1 << 0)	/* Stop (read / write) */
+
+#define DCMD_INCSRCADDR	(1 << 31)	/* Source Address Increment Setting. */
+#define DCMD_INCTRGADDR	(1 << 30)	/* Target Address Increment Setting. */
+#define DCMD_FLOWSRC	(1 << 29)	/* Flow Control by the source. */
+#define DCMD_FLOWTRG	(1 << 28)	/* Flow Control by the target. */
+#define DCMD_STARTIRQEN	(1 << 22)	/* Start Interrupt Enable */
+#define DCMD_ENDIRQEN	(1 << 21)	/* End Interrupt Enable */
+#define DCMD_ENDIAN	(1 << 18)	/* Device Endian-ness. */
+#define DCMD_BURST8	(1 << 16)	/* 8 byte burst */
+#define DCMD_BURST16	(2 << 16)	/* 16 byte burst */
+#define DCMD_BURST32	(3 << 16)	/* 32 byte burst */
+#define DCMD_WIDTH1	(1 << 14)	/* 1 byte width */
+#define DCMD_WIDTH2	(2 << 14)	/* 2 byte width (HalfWord) */
+#define DCMD_WIDTH4	(3 << 14)	/* 4 byte width (Word) */
+#define DCMD_LENGTH	0x01fff		/* length mask (max = 8K - 1) */
+
+#define PDMA_ALIGNMENT		3
+#define PDMA_MAX_DESC_BYTES	0x1000
+
+struct mmp_pdma_desc_hw {
+	u32 ddadr;	/* Points to the next descriptor + flags */
+	u32 dsadr;	/* DSADR value for the current transfer */
+	u32 dtadr;	/* DTADR value for the current transfer */
+	u32 dcmd;	/* DCMD value for the current transfer */
+} __aligned(32);
+
+struct mmp_pdma_desc_sw {
+	struct mmp_pdma_desc_hw desc;
+	struct list_head node;
+	struct list_head tx_list;
+	struct dma_async_tx_descriptor async_tx;
+};
+
+struct mmp_pdma_phy;
+
+struct mmp_pdma_chan {
+	struct device *dev;
+	struct dma_chan chan;
+	struct dma_async_tx_descriptor desc;
+	struct mmp_pdma_phy *phy;
+	enum dma_transfer_direction dir;
+
+	/* channel's basic info */
+	struct tasklet_struct tasklet;
+	u32 dcmd;
+	u32 drcmr;
+	u32 dev_addr;
+
+	/* list for desc */
+	spinlock_t desc_lock;		/* Descriptor list lock */
+	struct list_head chain_pending;	/* Link descriptors queue for pending */
+	struct list_head chain_running;	/* Link descriptors queue for running */
+	bool idle;			/* channel statue machine */
+
+	struct dma_pool *desc_pool;	/* Descriptors pool */
+};
+
+struct mmp_pdma_phy {
+	int idx;
+	void __iomem *base;
+	struct mmp_pdma_chan *vchan;
+};
+
+struct mmp_pdma_device {
+	int				dma_channels;
+	void __iomem			*base;
+	struct device			*dev;
+	struct dma_device		device;
+	struct mmp_pdma_phy		*phy;
+};
+
+#define tx_to_mmp_pdma_desc(tx) container_of(tx, struct mmp_pdma_desc_sw, async_tx)
+#define to_mmp_pdma_desc(lh) container_of(lh, struct mmp_pdma_desc_sw, node)
+#define to_mmp_pdma_chan(dchan) container_of(dchan, struct mmp_pdma_chan, chan)
+#define to_mmp_pdma_dev(dmadev) container_of(dmadev, struct mmp_pdma_device, device)
+
+static void set_desc(struct mmp_pdma_phy *phy, dma_addr_t addr)
+{
+	u32 reg = (phy->idx << 4) + DDADR;
+
+	writel(addr, phy->base + reg);
+}
+
+static void enable_chan(struct mmp_pdma_phy *phy)
+{
+	u32 reg;
+
+	if (!phy->vchan)
+		return;
+
+	reg = phy->vchan->drcmr;
+	reg = (((reg) < 64) ? 0x0100 : 0x1100) + (((reg) & 0x3f) << 2);
+	writel(DRCMR_MAPVLD | phy->idx, phy->base + reg);
+
+	reg = (phy->idx << 2) + DCSR;
+	writel(readl(phy->base + reg) | DCSR_RUN,
+					phy->base + reg);
+}
+
+static void disable_chan(struct mmp_pdma_phy *phy)
+{
+	u32 reg;
+
+	if (phy) {
+		reg = (phy->idx << 2) + DCSR;
+		writel(readl(phy->base + reg) & ~DCSR_RUN,
+						phy->base + reg);
+	}
+}
+
+static int clear_chan_irq(struct mmp_pdma_phy *phy)
+{
+	u32 dcsr;
+	u32 dint = readl(phy->base + DINT);
+	u32 reg = (phy->idx << 2) + DCSR;
+
+	if (dint & BIT(phy->idx)) {
+		/* clear irq */
+		dcsr = readl(phy->base + reg);
+		writel(dcsr, phy->base + reg);
+		if ((dcsr & DCSR_BUSERR) && (phy->vchan))
+			dev_warn(phy->vchan->dev, "DCSR_BUSERR\n");
+		return 0;
+	}
+	return -EAGAIN;
+}
+
+static irqreturn_t mmp_pdma_chan_handler(int irq, void *dev_id)
+{
+	struct mmp_pdma_phy *phy = dev_id;
+
+	if (clear_chan_irq(phy) == 0) {
+		tasklet_schedule(&phy->vchan->tasklet);
+		return IRQ_HANDLED;
+	} else
+		return IRQ_NONE;
+}
+
+static irqreturn_t mmp_pdma_int_handler(int irq, void *dev_id)
+{
+	struct mmp_pdma_device *pdev = dev_id;
+	struct mmp_pdma_phy *phy;
+	u32 dint = readl(pdev->base + DINT);
+	int i, ret;
+	int irq_num = 0;
+
+	while (dint) {
+		i = __ffs(dint);
+		dint &= (dint - 1);
+		phy = &pdev->phy[i];
+		ret = mmp_pdma_chan_handler(irq, phy);
+		if (ret == IRQ_HANDLED)
+			irq_num++;
+	}
+
+	if (irq_num)
+		return IRQ_HANDLED;
+	else
+		return IRQ_NONE;
+}
+
+/* lookup free phy channel as descending priority */
+static struct mmp_pdma_phy *lookup_phy(struct mmp_pdma_chan *pchan)
+{
+	int prio, i;
+	struct mmp_pdma_device *pdev = to_mmp_pdma_dev(pchan->chan.device);
+	struct mmp_pdma_phy *phy;
+
+	/*
+	 * dma channel priorities
+	 * ch 0 - 3,  16 - 19  <--> (0)
+	 * ch 4 - 7,  20 - 23  <--> (1)
+	 * ch 8 - 11, 24 - 27  <--> (2)
+	 * ch 12 - 15, 28 - 31  <--> (3)
+	 */
+	for (prio = 0; prio <= (((pdev->dma_channels - 1) & 0xf) >> 2); prio++) {
+		for (i = 0; i < pdev->dma_channels; i++) {
+			if (prio != ((i & 0xf) >> 2))
+				continue;
+			phy = &pdev->phy[i];
+			if (!phy->vchan) {
+				phy->vchan = pchan;
+				return phy;
+			}
+		}
+	}
+
+	return NULL;
+}
+
+/* desc->tx_list ==> pending list */
+static void append_pending_queue(struct mmp_pdma_chan *chan,
+					struct mmp_pdma_desc_sw *desc)
+{
+	struct mmp_pdma_desc_sw *tail =
+				to_mmp_pdma_desc(chan->chain_pending.prev);
+
+	if (list_empty(&chan->chain_pending))
+		goto out_splice;
+
+	/* one irq per queue, even appended */
+	tail->desc.ddadr = desc->async_tx.phys;
+	tail->desc.dcmd &= ~DCMD_ENDIRQEN;
+
+	/* softly link to pending list */
+out_splice:
+	list_splice_tail_init(&desc->tx_list, &chan->chain_pending);
+}
+
+/**
+ * start_pending_queue - transfer any pending transactions
+ * pending list ==> running list
+ */
+static void start_pending_queue(struct mmp_pdma_chan *chan)
+{
+	struct mmp_pdma_desc_sw *desc;
+
+	/* still in running, irq will start the pending list */
+	if (!chan->idle) {
+		dev_dbg(chan->dev, "DMA controller still busy\n");
+		return;
+	}
+
+	if (list_empty(&chan->chain_pending)) {
+		/* chance to re-fetch phy channel with higher prio */
+		if (chan->phy) {
+			chan->phy->vchan = NULL;
+			chan->phy = NULL;
+		}
+		dev_dbg(chan->dev, "no pending list\n");
+		return;
+	}
+
+	if (!chan->phy) {
+		chan->phy = lookup_phy(chan);
+		if (!chan->phy) {
+			dev_dbg(chan->dev, "no free dma channel\n");
+			return;
+		}
+	}
+
+	/*
+	 * pending -> running
+	 * reintilize pending list
+	 */
+	desc = list_first_entry(&chan->chain_pending,
+				struct mmp_pdma_desc_sw, node);
+	list_splice_tail_init(&chan->chain_pending, &chan->chain_running);
+
+	/*
+	 * Program the descriptor's address into the DMA controller,
+	 * then start the DMA transaction
+	 */
+	set_desc(chan->phy, desc->async_tx.phys);
+	enable_chan(chan->phy);
+	chan->idle = false;
+}
+
+
+/* desc->tx_list ==> pending list */
+static dma_cookie_t mmp_pdma_tx_submit(struct dma_async_tx_descriptor *tx)
+{
+	struct mmp_pdma_chan *chan = to_mmp_pdma_chan(tx->chan);
+	struct mmp_pdma_desc_sw *desc = tx_to_mmp_pdma_desc(tx);
+	struct mmp_pdma_desc_sw *child;
+	unsigned long flags;
+	dma_cookie_t cookie = -EBUSY;
+
+	spin_lock_irqsave(&chan->desc_lock, flags);
+
+	list_for_each_entry(child, &desc->tx_list, node) {
+		cookie = dma_cookie_assign(&child->async_tx);
+	}
+
+	append_pending_queue(chan, desc);
+
+	spin_unlock_irqrestore(&chan->desc_lock, flags);
+
+	return cookie;
+}
+
+struct mmp_pdma_desc_sw *mmp_pdma_alloc_descriptor(struct mmp_pdma_chan *chan)
+{
+	struct mmp_pdma_desc_sw *desc;
+	dma_addr_t pdesc;
+
+	desc = dma_pool_alloc(chan->desc_pool, GFP_ATOMIC, &pdesc);
+	if (!desc) {
+		dev_err(chan->dev, "out of memory for link descriptor\n");
+		return NULL;
+	}
+
+	memset(desc, 0, sizeof(*desc));
+	INIT_LIST_HEAD(&desc->tx_list);
+	dma_async_tx_descriptor_init(&desc->async_tx, &chan->chan);
+	/* each desc has submit */
+	desc->async_tx.tx_submit = mmp_pdma_tx_submit;
+	desc->async_tx.phys = pdesc;
+
+	return desc;
+}
+
+/**
+ * mmp_pdma_alloc_chan_resources - Allocate resources for DMA channel.
+ *
+ * This function will create a dma pool for descriptor allocation.
+ * Request irq only when channel is requested
+ * Return - The number of allocated descriptors.
+ */
+
+static int mmp_pdma_alloc_chan_resources(struct dma_chan *dchan)
+{
+	struct mmp_pdma_chan *chan = to_mmp_pdma_chan(dchan);
+
+	if (chan->desc_pool)
+		return 1;
+
+	chan->desc_pool =
+		dma_pool_create(dev_name(&dchan->dev->device), chan->dev,
+				  sizeof(struct mmp_pdma_desc_sw),
+				  __alignof__(struct mmp_pdma_desc_sw), 0);
+	if (!chan->desc_pool) {
+		dev_err(chan->dev, "unable to allocate descriptor pool\n");
+		return -ENOMEM;
+	}
+	if (chan->phy) {
+		chan->phy->vchan = NULL;
+		chan->phy = NULL;
+	}
+	chan->idle = true;
+	chan->dev_addr = 0;
+	return 1;
+}
+
+static void mmp_pdma_free_desc_list(struct mmp_pdma_chan *chan,
+				  struct list_head *list)
+{
+	struct mmp_pdma_desc_sw *desc, *_desc;
+
+	list_for_each_entry_safe(desc, _desc, list, node) {
+		list_del(&desc->node);
+		dma_pool_free(chan->desc_pool, desc, desc->async_tx.phys);
+	}
+}
+
+static void mmp_pdma_free_chan_resources(struct dma_chan *dchan)
+{
+	struct mmp_pdma_chan *chan = to_mmp_pdma_chan(dchan);
+	unsigned long flags;
+
+	spin_lock_irqsave(&chan->desc_lock, flags);
+	mmp_pdma_free_desc_list(chan, &chan->chain_pending);
+	mmp_pdma_free_desc_list(chan, &chan->chain_running);
+	spin_unlock_irqrestore(&chan->desc_lock, flags);
+
+	dma_pool_destroy(chan->desc_pool);
+	chan->desc_pool = NULL;
+	chan->idle = true;
+	chan->dev_addr = 0;
+	if (chan->phy) {
+		chan->phy->vchan = NULL;
+		chan->phy = NULL;
+	}
+	return;
+}
+
+static struct dma_async_tx_descriptor *
+mmp_pdma_prep_memcpy(struct dma_chan *dchan,
+	dma_addr_t dma_dst, dma_addr_t dma_src,
+	size_t len, unsigned long flags)
+{
+	struct mmp_pdma_chan *chan;
+	struct mmp_pdma_desc_sw *first = NULL, *prev = NULL, *new;
+	size_t copy = 0;
+
+	if (!dchan)
+		return NULL;
+
+	if (!len)
+		return NULL;
+
+	chan = to_mmp_pdma_chan(dchan);
+
+	if (!chan->dir) {
+		chan->dir = DMA_MEM_TO_MEM;
+		chan->dcmd = DCMD_INCTRGADDR | DCMD_INCSRCADDR;
+		chan->dcmd |= DCMD_BURST32;
+	}
+
+	do {
+		/* Allocate the link descriptor from DMA pool */
+		new = mmp_pdma_alloc_descriptor(chan);
+		if (!new) {
+			dev_err(chan->dev, "no memory for desc\n");
+			goto fail;
+		}
+
+		copy = min_t(size_t, len, PDMA_MAX_DESC_BYTES);
+
+		new->desc.dcmd = chan->dcmd | (DCMD_LENGTH & copy);
+		new->desc.dsadr = dma_src;
+		new->desc.dtadr = dma_dst;
+
+		if (!first)
+			first = new;
+		else
+			prev->desc.ddadr = new->async_tx.phys;
+
+		new->async_tx.cookie = 0;
+		async_tx_ack(&new->async_tx);
+
+		prev = new;
+		len -= copy;
+
+		if (chan->dir == DMA_MEM_TO_DEV) {
+			dma_src += copy;
+		} else if (chan->dir == DMA_DEV_TO_MEM) {
+			dma_dst += copy;
+		} else if (chan->dir == DMA_MEM_TO_MEM) {
+			dma_src += copy;
+			dma_dst += copy;
+		}
+
+		/* Insert the link descriptor to the LD ring */
+		list_add_tail(&new->node, &first->tx_list);
+	} while (len);
+
+	first->async_tx.flags = flags; /* client is in control of this ack */
+	first->async_tx.cookie = -EBUSY;
+
+	/* last desc and fire IRQ */
+	new->desc.ddadr = DDADR_STOP;
+	new->desc.dcmd |= DCMD_ENDIRQEN;
+
+	return &first->async_tx;
+
+fail:
+	if (first)
+		mmp_pdma_free_desc_list(chan, &first->tx_list);
+	return NULL;
+}
+
+static struct dma_async_tx_descriptor *
+mmp_pdma_prep_slave_sg(struct dma_chan *dchan, struct scatterlist *sgl,
+			 unsigned int sg_len, enum dma_transfer_direction dir,
+			 unsigned long flags, void *context)
+{
+	struct mmp_pdma_chan *chan = to_mmp_pdma_chan(dchan);
+	struct mmp_pdma_desc_sw *first = NULL, *prev = NULL, *new = NULL;
+	size_t len, avail;
+	struct scatterlist *sg;
+	dma_addr_t addr;
+	int i;
+
+	if ((sgl == NULL) || (sg_len == 0))
+		return NULL;
+
+	for_each_sg(sgl, sg, sg_len, i) {
+		addr = sg_dma_address(sg);
+		avail = sg_dma_len(sgl);
+
+		do {
+			len = min_t(size_t, avail, PDMA_MAX_DESC_BYTES);
+
+			/* allocate and populate the descriptor */
+			new = mmp_pdma_alloc_descriptor(chan);
+			if (!new) {
+				dev_err(chan->dev, "no memory for desc\n");
+				goto fail;
+			}
+
+			new->desc.dcmd = chan->dcmd | (DCMD_LENGTH & len);
+			if (dir == DMA_MEM_TO_DEV) {
+				new->desc.dsadr = addr;
+				new->desc.dtadr = chan->dev_addr;
+			} else {
+				new->desc.dsadr = chan->dev_addr;
+				new->desc.dtadr = addr;
+			}
+
+			if (!first)
+				first = new;
+			else
+				prev->desc.ddadr = new->async_tx.phys;
+
+			new->async_tx.cookie = 0;
+			async_tx_ack(&new->async_tx);
+			prev = new;
+
+			/* Insert the link descriptor to the LD ring */
+			list_add_tail(&new->node, &first->tx_list);
+
+			/* update metadata */
+			addr += len;
+			avail -= len;
+		} while (avail);
+	}
+
+	first->async_tx.cookie = -EBUSY;
+	first->async_tx.flags = flags;
+
+	/* last desc and fire IRQ */
+	new->desc.ddadr = DDADR_STOP;
+	new->desc.dcmd |= DCMD_ENDIRQEN;
+
+	return &first->async_tx;
+
+fail:
+	if (first)
+		mmp_pdma_free_desc_list(chan, &first->tx_list);
+	return NULL;
+}
+
+static int mmp_pdma_control(struct dma_chan *dchan, enum dma_ctrl_cmd cmd,
+		unsigned long arg)
+{
+	struct mmp_pdma_chan *chan = to_mmp_pdma_chan(dchan);
+	struct dma_slave_config *cfg = (void *)arg;
+	unsigned long flags;
+	int ret = 0;
+	u32 maxburst = 0, addr = 0;
+	enum dma_slave_buswidth width = DMA_SLAVE_BUSWIDTH_UNDEFINED;
+
+	if (!dchan)
+		return -EINVAL;
+
+	switch (cmd) {
+	case DMA_TERMINATE_ALL:
+		disable_chan(chan->phy);
+		if (chan->phy) {
+			chan->phy->vchan = NULL;
+			chan->phy = NULL;
+		}
+		spin_lock_irqsave(&chan->desc_lock, flags);
+		mmp_pdma_free_desc_list(chan, &chan->chain_pending);
+		mmp_pdma_free_desc_list(chan, &chan->chain_running);
+		spin_unlock_irqrestore(&chan->desc_lock, flags);
+		chan->idle = true;
+		break;
+	case DMA_SLAVE_CONFIG:
+		if (cfg->direction == DMA_DEV_TO_MEM) {
+			chan->dcmd = DCMD_INCTRGADDR | DCMD_FLOWSRC;
+			maxburst = cfg->src_maxburst;
+			width = cfg->src_addr_width;
+			addr = cfg->src_addr;
+		} else if (cfg->direction == DMA_MEM_TO_DEV) {
+			chan->dcmd = DCMD_INCSRCADDR | DCMD_FLOWTRG;
+			maxburst = cfg->dst_maxburst;
+			width = cfg->dst_addr_width;
+			addr = cfg->dst_addr;
+		}
+
+		if (width == DMA_SLAVE_BUSWIDTH_1_BYTE)
+			chan->dcmd |= DCMD_WIDTH1;
+		else if (width == DMA_SLAVE_BUSWIDTH_2_BYTES)
+			chan->dcmd |= DCMD_WIDTH2;
+		else if (width == DMA_SLAVE_BUSWIDTH_4_BYTES)
+			chan->dcmd |= DCMD_WIDTH4;
+
+		if (maxburst == 8)
+			chan->dcmd |= DCMD_BURST8;
+		else if (maxburst == 16)
+			chan->dcmd |= DCMD_BURST16;
+		else if (maxburst == 32)
+			chan->dcmd |= DCMD_BURST32;
+
+		if (cfg) {
+			chan->dir = cfg->direction;
+			chan->drcmr = cfg->slave_id;
+		}
+		chan->dev_addr = addr;
+		break;
+	default:
+		return -ENOSYS;
+	}
+
+	return ret;
+}
+
+static enum dma_status mmp_pdma_tx_status(struct dma_chan *dchan,
+			dma_cookie_t cookie, struct dma_tx_state *txstate)
+{
+	struct mmp_pdma_chan *chan = to_mmp_pdma_chan(dchan);
+	enum dma_status ret;
+	unsigned long flags;
+
+	spin_lock_irqsave(&chan->desc_lock, flags);
+	ret = dma_cookie_status(dchan, cookie, txstate);
+	spin_unlock_irqrestore(&chan->desc_lock, flags);
+
+	return ret;
+}
+
+/**
+ * mmp_pdma_issue_pending - Issue the DMA start command
+ * pending list ==> running list
+ */
+static void mmp_pdma_issue_pending(struct dma_chan *dchan)
+{
+	struct mmp_pdma_chan *chan = to_mmp_pdma_chan(dchan);
+	unsigned long flags;
+
+	spin_lock_irqsave(&chan->desc_lock, flags);
+	start_pending_queue(chan);
+	spin_unlock_irqrestore(&chan->desc_lock, flags);
+}
+
+/*
+ * dma_do_tasklet
+ * Do call back
+ * Start pending list
+ */
+static void dma_do_tasklet(unsigned long data)
+{
+	struct mmp_pdma_chan *chan = (struct mmp_pdma_chan *)data;
+	struct mmp_pdma_desc_sw *desc, *_desc;
+	LIST_HEAD(chain_cleanup);
+	unsigned long flags;
+
+	/* submit pending list; callback for each desc; free desc */
+
+	spin_lock_irqsave(&chan->desc_lock, flags);
+
+	/* update the cookie if we have some descriptors to cleanup */
+	if (!list_empty(&chan->chain_running)) {
+		dma_cookie_t cookie;
+
+		desc = to_mmp_pdma_desc(chan->chain_running.prev);
+		cookie = desc->async_tx.cookie;
+		dma_cookie_complete(&desc->async_tx);
+
+		dev_dbg(chan->dev, "completed_cookie=%d\n", cookie);
+	}
+
+	/*
+	 * move the descriptors to a temporary list so we can drop the lock
+	 * during the entire cleanup operation
+	 */
+	list_splice_tail_init(&chan->chain_running, &chain_cleanup);
+
+	/* the hardware is now idle and ready for more */
+	chan->idle = true;
+
+	/* Start any pending transactions automatically */
+	start_pending_queue(chan);
+	spin_unlock_irqrestore(&chan->desc_lock, flags);
+
+	/* Run the callback for each descriptor, in order */
+	list_for_each_entry_safe(desc, _desc, &chain_cleanup, node) {
+		struct dma_async_tx_descriptor *txd = &desc->async_tx;
+
+		/* Remove from the list of transactions */
+		list_del(&desc->node);
+		/* Run the link descriptor callback function */
+		if (txd->callback)
+			txd->callback(txd->callback_param);
+
+		dma_pool_free(chan->desc_pool, desc, txd->phys);
+	}
+}
+
+static int __devexit mmp_pdma_remove(struct platform_device *op)
+{
+	struct mmp_pdma_device *pdev = platform_get_drvdata(op);
+
+	dma_async_device_unregister(&pdev->device);
+	return 0;
+}
+
+static int __devinit mmp_pdma_chan_init(struct mmp_pdma_device *pdev,
+							int idx, int irq)
+{
+	struct mmp_pdma_phy *phy  = &pdev->phy[idx];
+	struct mmp_pdma_chan *chan;
+	int ret;
+
+	chan = devm_kzalloc(pdev->dev,
+			sizeof(struct mmp_pdma_chan), GFP_KERNEL);
+	if (chan == NULL)
+		return -ENOMEM;
+
+	phy->idx = idx;
+	phy->base = pdev->base;
+
+	if (irq) {
+		ret = devm_request_irq(pdev->dev, irq,
+			mmp_pdma_chan_handler, IRQF_DISABLED, "pdma", phy);
+		if (ret) {
+			dev_err(pdev->dev, "channel request irq fail!\n");
+			return ret;
+		}
+	}
+
+	spin_lock_init(&chan->desc_lock);
+	chan->dev = pdev->dev;
+	chan->chan.device = &pdev->device;
+	tasklet_init(&chan->tasklet, dma_do_tasklet, (unsigned long)chan);
+	INIT_LIST_HEAD(&chan->chain_pending);
+	INIT_LIST_HEAD(&chan->chain_running);
+
+	/* register virt channel to dma engine */
+	list_add_tail(&chan->chan.device_node,
+			&pdev->device.channels);
+
+	return 0;
+}
+
+static struct of_device_id mmp_pdma_dt_ids[] = {
+	{ .compatible = "marvell,pdma-1.0", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, mmp_pdma_dt_ids);
+
+static int __devinit mmp_pdma_probe(struct platform_device *op)
+{
+	struct mmp_pdma_device *pdev;
+	const struct of_device_id *of_id;
+	struct mmp_dma_platdata *pdata = dev_get_platdata(&op->dev);
+	struct resource *iores;
+	int i, ret, irq = 0;
+	int dma_channels = 0, irq_num = 0;
+
+	pdev = devm_kzalloc(&op->dev, sizeof(*pdev), GFP_KERNEL);
+	if (!pdev)
+		return -ENOMEM;
+	pdev->dev = &op->dev;
+
+	iores = platform_get_resource(op, IORESOURCE_MEM, 0);
+	if (!iores)
+		return -EINVAL;
+
+	pdev->base = devm_request_and_ioremap(pdev->dev, iores);
+	if (!pdev->base)
+		return -EADDRNOTAVAIL;
+
+	of_id = of_match_device(mmp_pdma_dt_ids, pdev->dev);
+	if (of_id)
+		of_property_read_u32(pdev->dev->of_node,
+				"#dma-channels", &dma_channels);
+	else if (pdata && pdata->dma_channels)
+		dma_channels = pdata->dma_channels;
+	else
+		dma_channels = 32;	/* default 32 channel */
+	pdev->dma_channels = dma_channels;
+
+	for (i = 0; i < dma_channels; i++) {
+		if (platform_get_irq(op, i) > 0)
+			irq_num++;
+	}
+
+	pdev->phy = devm_kzalloc(pdev->dev,
+		dma_channels * sizeof(struct mmp_pdma_chan), GFP_KERNEL);
+	if (pdev->phy == NULL)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&pdev->device.channels);
+
+	if (irq_num != dma_channels) {
+		/* all chan share one irq, demux inside */
+		irq = platform_get_irq(op, 0);
+		ret = devm_request_irq(pdev->dev, irq,
+			mmp_pdma_int_handler, IRQF_DISABLED, "pdma", pdev);
+		if (ret)
+			return ret;
+	}
+
+	for (i = 0; i < dma_channels; i++) {
+		irq = (irq_num != dma_channels) ? 0 : platform_get_irq(op, i);
+		ret = mmp_pdma_chan_init(pdev, i, irq);
+		if (ret)
+			return ret;
+	}
+
+	dma_cap_set(DMA_SLAVE, pdev->device.cap_mask);
+	dma_cap_set(DMA_MEMCPY, pdev->device.cap_mask);
+	dma_cap_set(DMA_SLAVE, pdev->device.cap_mask);
+	pdev->device.dev = &op->dev;
+	pdev->device.device_alloc_chan_resources = mmp_pdma_alloc_chan_resources;
+	pdev->device.device_free_chan_resources = mmp_pdma_free_chan_resources;
+	pdev->device.device_tx_status = mmp_pdma_tx_status;
+	pdev->device.device_prep_dma_memcpy = mmp_pdma_prep_memcpy;
+	pdev->device.device_prep_slave_sg = mmp_pdma_prep_slave_sg;
+	pdev->device.device_issue_pending = mmp_pdma_issue_pending;
+	pdev->device.device_control = mmp_pdma_control;
+	pdev->device.copy_align = PDMA_ALIGNMENT;
+
+	if (pdev->dev->coherent_dma_mask)
+		dma_set_mask(pdev->dev, pdev->dev->coherent_dma_mask);
+	else
+		dma_set_mask(pdev->dev, DMA_BIT_MASK(64));
+
+	ret = dma_async_device_register(&pdev->device);
+	if (ret) {
+		dev_err(pdev->device.dev, "unable to register\n");
+		return ret;
+	}
+
+	dev_info(pdev->device.dev, "initialized\n");
+	return 0;
+}
+
+static const struct platform_device_id mmp_pdma_id_table[] = {
+	{ "mmp-pdma", },
+	{ },
+};
+
+static struct platform_driver mmp_pdma_driver = {
+	.driver		= {
+		.name	= "mmp-pdma",
+		.owner  = THIS_MODULE,
+		.of_match_table = mmp_pdma_dt_ids,
+	},
+	.id_table	= mmp_pdma_id_table,
+	.probe		= mmp_pdma_probe,
+	.remove		= __devexit_p(mmp_pdma_remove),
+};
+
+module_platform_driver(mmp_pdma_driver);
+
+MODULE_DESCRIPTION("MARVELL MMP Periphera DMA Driver");
+MODULE_AUTHOR("Marvell International Ltd.");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/dma/mmp_tdma.c b/drivers/dma/mmp_tdma.c
index 07fa48688ba..f3e8d71bcbc 100644
--- a/drivers/dma/mmp_tdma.c
+++ b/drivers/dma/mmp_tdma.c
@@ -20,6 +20,7 @@
 #include <linux/device.h>
 #include <mach/regs-icu.h>
 #include <linux/platform_data/dma-mmp_tdma.h>
+#include <linux/of_device.h>
 
 #include "dmaengine.h"
 
@@ -127,7 +128,6 @@ struct mmp_tdma_device {
 	void __iomem			*base;
 	struct dma_device		device;
 	struct mmp_tdma_chan		*tdmac[TDMA_CHANNEL_NUM];
-	int				irq;
 };
 
 #define to_mmp_tdma_chan(dchan) container_of(dchan, struct mmp_tdma_chan, chan)
@@ -358,7 +358,7 @@ struct mmp_tdma_desc *mmp_tdma_alloc_descriptor(struct mmp_tdma_chan *tdmac)
 static struct dma_async_tx_descriptor *mmp_tdma_prep_dma_cyclic(
 		struct dma_chan *chan, dma_addr_t dma_addr, size_t buf_len,
 		size_t period_len, enum dma_transfer_direction direction,
-		void *context)
+		unsigned long flags, void *context)
 {
 	struct mmp_tdma_chan *tdmac = to_mmp_tdma_chan(chan);
 	struct mmp_tdma_desc *desc;
@@ -492,7 +492,7 @@ static int __devinit mmp_tdma_chan_init(struct mmp_tdma_device *tdev,
 		return -ENOMEM;
 	}
 	if (irq)
-		tdmac->irq = irq + idx;
+		tdmac->irq = irq;
 	tdmac->dev	   = tdev->dev;
 	tdmac->chan.device = &tdev->device;
 	tdmac->idx	   = idx;
@@ -505,34 +505,43 @@ static int __devinit mmp_tdma_chan_init(struct mmp_tdma_device *tdev,
 	/* add the channel to tdma_chan list */
 	list_add_tail(&tdmac->chan.device_node,
 			&tdev->device.channels);
-
 	return 0;
 }
 
+static struct of_device_id mmp_tdma_dt_ids[] = {
+	{ .compatible = "marvell,adma-1.0", .data = (void *)MMP_AUD_TDMA},
+	{ .compatible = "marvell,pxa910-squ", .data = (void *)PXA910_SQU},
+	{}
+};
+MODULE_DEVICE_TABLE(of, mmp_tdma_dt_ids);
+
 static int __devinit mmp_tdma_probe(struct platform_device *pdev)
 {
-	const struct platform_device_id *id = platform_get_device_id(pdev);
-	enum mmp_tdma_type type = id->driver_data;
+	enum mmp_tdma_type type;
+	const struct of_device_id *of_id;
 	struct mmp_tdma_device *tdev;
 	struct resource *iores;
 	int i, ret;
-	int irq = 0;
+	int irq = 0, irq_num = 0;
 	int chan_num = TDMA_CHANNEL_NUM;
 
+	of_id = of_match_device(mmp_tdma_dt_ids, &pdev->dev);
+	if (of_id)
+		type = (enum mmp_tdma_type) of_id->data;
+	else
+		type = platform_get_device_id(pdev)->driver_data;
+
 	/* always have couple channels */
 	tdev = devm_kzalloc(&pdev->dev, sizeof(*tdev), GFP_KERNEL);
 	if (!tdev)
 		return -ENOMEM;
 
 	tdev->dev = &pdev->dev;
-	iores = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (!iores)
-		return -EINVAL;
 
-	if (resource_size(iores) != chan_num)
-		tdev->irq = iores->start;
-	else
-		irq = iores->start;
+	for (i = 0; i < chan_num; i++) {
+		if (platform_get_irq(pdev, i) > 0)
+			irq_num++;
+	}
 
 	iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!iores)
@@ -542,25 +551,26 @@ static int __devinit mmp_tdma_probe(struct platform_device *pdev)
 	if (!tdev->base)
 		return -EADDRNOTAVAIL;
 
-	if (tdev->irq) {
-		ret = devm_request_irq(&pdev->dev, tdev->irq,
+	INIT_LIST_HEAD(&tdev->device.channels);
+
+	if (irq_num != chan_num) {
+		irq = platform_get_irq(pdev, 0);
+		ret = devm_request_irq(&pdev->dev, irq,
 			mmp_tdma_int_handler, IRQF_DISABLED, "tdma", tdev);
 		if (ret)
 			return ret;
 	}
 
-	dma_cap_set(DMA_SLAVE, tdev->device.cap_mask);
-	dma_cap_set(DMA_CYCLIC, tdev->device.cap_mask);
-
-	INIT_LIST_HEAD(&tdev->device.channels);
-
 	/* initialize channel parameters */
 	for (i = 0; i < chan_num; i++) {
+		irq = (irq_num != chan_num) ? 0 : platform_get_irq(pdev, i);
 		ret = mmp_tdma_chan_init(tdev, i, irq, type);
 		if (ret)
 			return ret;
 	}
 
+	dma_cap_set(DMA_SLAVE, tdev->device.cap_mask);
+	dma_cap_set(DMA_CYCLIC, tdev->device.cap_mask);
 	tdev->device.dev = &pdev->dev;
 	tdev->device.device_alloc_chan_resources =
 					mmp_tdma_alloc_chan_resources;
@@ -595,6 +605,7 @@ static struct platform_driver mmp_tdma_driver = {
 	.driver		= {
 		.name	= "mmp-tdma",
 		.owner  = THIS_MODULE,
+		.of_match_table = mmp_tdma_dt_ids,
 	},
 	.id_table	= mmp_tdma_id_table,
 	.probe		= mmp_tdma_probe,
diff --git a/drivers/dma/mxs-dma.c b/drivers/dma/mxs-dma.c
index 7f41b25805f..9f02e794b12 100644
--- a/drivers/dma/mxs-dma.c
+++ b/drivers/dma/mxs-dma.c
@@ -101,7 +101,8 @@ struct mxs_dma_ccw {
 	u32		pio_words[MXS_PIO_WORDS];
 };
 
-#define NUM_CCW	(int)(PAGE_SIZE / sizeof(struct mxs_dma_ccw))
+#define CCW_BLOCK_SIZE	(4 * PAGE_SIZE)
+#define NUM_CCW	(int)(CCW_BLOCK_SIZE / sizeof(struct mxs_dma_ccw))
 
 struct mxs_dma_chan {
 	struct mxs_dma_engine		*mxs_dma;
@@ -354,14 +355,15 @@ static int mxs_dma_alloc_chan_resources(struct dma_chan *chan)
 
 	mxs_chan->chan_irq = data->chan_irq;
 
-	mxs_chan->ccw = dma_alloc_coherent(mxs_dma->dma_device.dev, PAGE_SIZE,
-				&mxs_chan->ccw_phys, GFP_KERNEL);
+	mxs_chan->ccw = dma_alloc_coherent(mxs_dma->dma_device.dev,
+				CCW_BLOCK_SIZE, &mxs_chan->ccw_phys,
+				GFP_KERNEL);
 	if (!mxs_chan->ccw) {
 		ret = -ENOMEM;
 		goto err_alloc;
 	}
 
-	memset(mxs_chan->ccw, 0, PAGE_SIZE);
+	memset(mxs_chan->ccw, 0, CCW_BLOCK_SIZE);
 
 	if (mxs_chan->chan_irq != NO_IRQ) {
 		ret = request_irq(mxs_chan->chan_irq, mxs_dma_int_handler,
@@ -387,7 +389,7 @@ static int mxs_dma_alloc_chan_resources(struct dma_chan *chan)
 err_clk:
 	free_irq(mxs_chan->chan_irq, mxs_dma);
 err_irq:
-	dma_free_coherent(mxs_dma->dma_device.dev, PAGE_SIZE,
+	dma_free_coherent(mxs_dma->dma_device.dev, CCW_BLOCK_SIZE,
 			mxs_chan->ccw, mxs_chan->ccw_phys);
 err_alloc:
 	return ret;
@@ -402,7 +404,7 @@ static void mxs_dma_free_chan_resources(struct dma_chan *chan)
 
 	free_irq(mxs_chan->chan_irq, mxs_dma);
 
-	dma_free_coherent(mxs_dma->dma_device.dev, PAGE_SIZE,
+	dma_free_coherent(mxs_dma->dma_device.dev, CCW_BLOCK_SIZE,
 			mxs_chan->ccw, mxs_chan->ccw_phys);
 
 	clk_disable_unprepare(mxs_dma->clk);
@@ -531,7 +533,7 @@ err_out:
 static struct dma_async_tx_descriptor *mxs_dma_prep_dma_cyclic(
 		struct dma_chan *chan, dma_addr_t dma_addr, size_t buf_len,
 		size_t period_len, enum dma_transfer_direction direction,
-		void *context)
+		unsigned long flags, void *context)
 {
 	struct mxs_dma_chan *mxs_chan = to_mxs_dma_chan(chan);
 	struct mxs_dma_engine *mxs_dma = mxs_chan->mxs_dma;
diff --git a/drivers/dma/omap-dma.c b/drivers/dma/omap-dma.c
index 2e166277766..bb2d8e7029e 100644
--- a/drivers/dma/omap-dma.c
+++ b/drivers/dma/omap-dma.c
@@ -36,6 +36,7 @@ struct omap_chan {
 	struct dma_slave_config	cfg;
 	unsigned dma_sig;
 	bool cyclic;
+	bool paused;
 
 	int dma_ch;
 	struct omap_desc *desc;
@@ -367,7 +368,8 @@ static struct dma_async_tx_descriptor *omap_dma_prep_slave_sg(
 
 static struct dma_async_tx_descriptor *omap_dma_prep_dma_cyclic(
 	struct dma_chan *chan, dma_addr_t buf_addr, size_t buf_len,
-	size_t period_len, enum dma_transfer_direction dir, void *context)
+	size_t period_len, enum dma_transfer_direction dir, unsigned long flags,
+	void *context)
 {
 	struct omap_chan *c = to_omap_dma_chan(chan);
 	enum dma_slave_buswidth dev_width;
@@ -415,7 +417,10 @@ static struct dma_async_tx_descriptor *omap_dma_prep_dma_cyclic(
 	d->dev_addr = dev_addr;
 	d->fi = burst;
 	d->es = es;
-	d->sync_mode = OMAP_DMA_SYNC_PACKET;
+	if (burst)
+		d->sync_mode = OMAP_DMA_SYNC_PACKET;
+	else
+		d->sync_mode = OMAP_DMA_SYNC_ELEMENT;
 	d->sync_type = sync_type;
 	d->periph_port = OMAP_DMA_PORT_MPUI;
 	d->sg[0].addr = buf_addr;
@@ -426,7 +431,10 @@ static struct dma_async_tx_descriptor *omap_dma_prep_dma_cyclic(
 	if (!c->cyclic) {
 		c->cyclic = true;
 		omap_dma_link_lch(c->dma_ch, c->dma_ch);
-		omap_enable_dma_irq(c->dma_ch, OMAP_DMA_FRAME_IRQ);
+
+		if (flags & DMA_PREP_INTERRUPT)
+			omap_enable_dma_irq(c->dma_ch, OMAP_DMA_FRAME_IRQ);
+
 		omap_disable_dma_irq(c->dma_ch, OMAP_DMA_BLOCK_IRQ);
 	}
 
@@ -435,7 +443,7 @@ static struct dma_async_tx_descriptor *omap_dma_prep_dma_cyclic(
 		omap_set_dma_dest_burst_mode(c->dma_ch, OMAP_DMA_DATA_BURST_16);
 	}
 
-	return vchan_tx_prep(&c->vc, &d->vd, DMA_CTRL_ACK | DMA_PREP_INTERRUPT);
+	return vchan_tx_prep(&c->vc, &d->vd, flags);
 }
 
 static int omap_dma_slave_config(struct omap_chan *c, struct dma_slave_config *cfg)
@@ -469,11 +477,14 @@ static int omap_dma_terminate_all(struct omap_chan *c)
 	 */
 	if (c->desc) {
 		c->desc = NULL;
-		omap_stop_dma(c->dma_ch);
+		/* Avoid stopping the dma twice */
+		if (!c->paused)
+			omap_stop_dma(c->dma_ch);
 	}
 
 	if (c->cyclic) {
 		c->cyclic = false;
+		c->paused = false;
 		omap_dma_unlink_lch(c->dma_ch, c->dma_ch);
 	}
 
@@ -486,14 +497,30 @@ static int omap_dma_terminate_all(struct omap_chan *c)
 
 static int omap_dma_pause(struct omap_chan *c)
 {
-	/* FIXME: not supported by platform private API */
-	return -EINVAL;
+	/* Pause/Resume only allowed with cyclic mode */
+	if (!c->cyclic)
+		return -EINVAL;
+
+	if (!c->paused) {
+		omap_stop_dma(c->dma_ch);
+		c->paused = true;
+	}
+
+	return 0;
 }
 
 static int omap_dma_resume(struct omap_chan *c)
 {
-	/* FIXME: not supported by platform private API */
-	return -EINVAL;
+	/* Pause/Resume only allowed with cyclic mode */
+	if (!c->cyclic)
+		return -EINVAL;
+
+	if (c->paused) {
+		omap_start_dma(c->dma_ch);
+		c->paused = false;
+	}
+
+	return 0;
 }
 
 static int omap_dma_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
diff --git a/drivers/dma/pl330.c b/drivers/dma/pl330.c
index 5d3bbcd279b..665668b6f2b 100644
--- a/drivers/dma/pl330.c
+++ b/drivers/dma/pl330.c
@@ -23,7 +23,6 @@
 #include <linux/dmaengine.h>
 #include <linux/amba/bus.h>
 #include <linux/amba/pl330.h>
-#include <linux/pm_runtime.h>
 #include <linux/scatterlist.h>
 #include <linux/of.h>
 
@@ -586,8 +585,6 @@ struct dma_pl330_dmac {
 
 	/* Peripheral channels connected to this DMAC */
 	struct dma_pl330_chan *peripherals; /* keep at end */
-
-	struct clk *clk;
 };
 
 struct dma_pl330_desc {
@@ -2395,7 +2392,7 @@ static int pl330_alloc_chan_resources(struct dma_chan *chan)
 	pch->pl330_chid = pl330_request_channel(&pdmac->pif);
 	if (!pch->pl330_chid) {
 		spin_unlock_irqrestore(&pch->lock, flags);
-		return 0;
+		return -ENOMEM;
 	}
 
 	tasklet_init(&pch->task, pl330_tasklet, (unsigned long) pch);
@@ -2685,7 +2682,7 @@ static inline int get_burst_len(struct dma_pl330_desc *desc, size_t len)
 static struct dma_async_tx_descriptor *pl330_prep_dma_cyclic(
 		struct dma_chan *chan, dma_addr_t dma_addr, size_t len,
 		size_t period_len, enum dma_transfer_direction direction,
-		void *context)
+		unsigned long flags, void *context)
 {
 	struct dma_pl330_desc *desc;
 	struct dma_pl330_chan *pch = to_pchan(chan);
@@ -2889,29 +2886,17 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
 		goto probe_err1;
 	}
 
-	pdmac->clk = clk_get(&adev->dev, "dma");
-	if (IS_ERR(pdmac->clk)) {
-		dev_err(&adev->dev, "Cannot get operation clock.\n");
-		ret = -EINVAL;
-		goto probe_err2;
-	}
-
 	amba_set_drvdata(adev, pdmac);
 
-#ifndef CONFIG_PM_RUNTIME
-	/* enable dma clk */
-	clk_enable(pdmac->clk);
-#endif
-
 	irq = adev->irq[0];
 	ret = request_irq(irq, pl330_irq_handler, 0,
 			dev_name(&adev->dev), pi);
 	if (ret)
-		goto probe_err3;
+		goto probe_err2;
 
 	ret = pl330_add(pi);
 	if (ret)
-		goto probe_err4;
+		goto probe_err3;
 
 	INIT_LIST_HEAD(&pdmac->desc_pool);
 	spin_lock_init(&pdmac->pool_lock);
@@ -2933,7 +2918,7 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
 	if (!pdmac->peripherals) {
 		ret = -ENOMEM;
 		dev_err(&adev->dev, "unable to allocate pdmac->peripherals\n");
-		goto probe_err5;
+		goto probe_err4;
 	}
 
 	for (i = 0; i < num_chan; i++) {
@@ -2961,6 +2946,7 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
 		if (pi->pcfg.num_peri) {
 			dma_cap_set(DMA_SLAVE, pd->cap_mask);
 			dma_cap_set(DMA_CYCLIC, pd->cap_mask);
+			dma_cap_set(DMA_PRIVATE, pd->cap_mask);
 		}
 	}
 
@@ -2976,7 +2962,7 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
 	ret = dma_async_device_register(pd);
 	if (ret) {
 		dev_err(&adev->dev, "unable to register DMAC\n");
-		goto probe_err5;
+		goto probe_err4;
 	}
 
 	dev_info(&adev->dev,
@@ -2989,15 +2975,10 @@ pl330_probe(struct amba_device *adev, const struct amba_id *id)
 
 	return 0;
 
-probe_err5:
-	pl330_del(pi);
 probe_err4:
-	free_irq(irq, pi);
+	pl330_del(pi);
 probe_err3:
-#ifndef CONFIG_PM_RUNTIME
-	clk_disable(pdmac->clk);
-#endif
-	clk_put(pdmac->clk);
+	free_irq(irq, pi);
 probe_err2:
 	iounmap(pi->base);
 probe_err1:
@@ -3044,10 +3025,6 @@ static int __devexit pl330_remove(struct amba_device *adev)
 	res = &adev->res;
 	release_mem_region(res->start, resource_size(res));
 
-#ifndef CONFIG_PM_RUNTIME
-	clk_disable(pdmac->clk);
-#endif
-
 	kfree(pdmac);
 
 	return 0;
@@ -3063,49 +3040,10 @@ static struct amba_id pl330_ids[] = {
 
 MODULE_DEVICE_TABLE(amba, pl330_ids);
 
-#ifdef CONFIG_PM_RUNTIME
-static int pl330_runtime_suspend(struct device *dev)
-{
-	struct dma_pl330_dmac *pdmac = dev_get_drvdata(dev);
-
-	if (!pdmac) {
-		dev_err(dev, "failed to get dmac\n");
-		return -ENODEV;
-	}
-
-	clk_disable(pdmac->clk);
-
-	return 0;
-}
-
-static int pl330_runtime_resume(struct device *dev)
-{
-	struct dma_pl330_dmac *pdmac = dev_get_drvdata(dev);
-
-	if (!pdmac) {
-		dev_err(dev, "failed to get dmac\n");
-		return -ENODEV;
-	}
-
-	clk_enable(pdmac->clk);
-
-	return 0;
-}
-#else
-#define pl330_runtime_suspend	NULL
-#define pl330_runtime_resume	NULL
-#endif /* CONFIG_PM_RUNTIME */
-
-static const struct dev_pm_ops pl330_pm_ops = {
-	.runtime_suspend = pl330_runtime_suspend,
-	.runtime_resume = pl330_runtime_resume,
-};
-
 static struct amba_driver pl330_driver = {
 	.drv = {
 		.owner = THIS_MODULE,
 		.name = "dma-pl330",
-		.pm = &pl330_pm_ops,
 	},
 	.id_table = pl330_ids,
 	.probe = pl330_probe,
diff --git a/drivers/dma/sa11x0-dma.c b/drivers/dma/sa11x0-dma.c
index f5a73606217..b893159c1ec 100644
--- a/drivers/dma/sa11x0-dma.c
+++ b/drivers/dma/sa11x0-dma.c
@@ -614,7 +614,7 @@ static struct dma_async_tx_descriptor *sa11x0_dma_prep_slave_sg(
 
 static struct dma_async_tx_descriptor *sa11x0_dma_prep_dma_cyclic(
 	struct dma_chan *chan, dma_addr_t addr, size_t size, size_t period,
-	enum dma_transfer_direction dir, void *context)
+	enum dma_transfer_direction dir, unsigned long flags, void *context)
 {
 	struct sa11x0_dma_chan *c = to_sa11x0_dma_chan(chan);
 	struct sa11x0_dma_desc *txd;
diff --git a/drivers/dma/sirf-dma.c b/drivers/dma/sirf-dma.c
index 434ad31174f..64385cde044 100644
--- a/drivers/dma/sirf-dma.c
+++ b/drivers/dma/sirf-dma.c
@@ -489,7 +489,7 @@ err_dir:
 static struct dma_async_tx_descriptor *
 sirfsoc_dma_prep_cyclic(struct dma_chan *chan, dma_addr_t addr,
 	size_t buf_len, size_t period_len,
-	enum dma_transfer_direction direction, void *context)
+	enum dma_transfer_direction direction, unsigned long flags, void *context)
 {
 	struct sirfsoc_dma_chan *schan = dma_chan_to_sirfsoc_dma_chan(chan);
 	struct sirfsoc_dma_desc *sdesc = NULL;
@@ -570,21 +570,19 @@ static int __devinit sirfsoc_dma_probe(struct platform_device *op)
 
 	if (of_property_read_u32(dn, "cell-index", &id)) {
 		dev_err(dev, "Fail to get DMAC index\n");
-		ret = -ENODEV;
-		goto free_mem;
+		return -ENODEV;
 	}
 
 	sdma->irq = irq_of_parse_and_map(dn, 0);
 	if (sdma->irq == NO_IRQ) {
 		dev_err(dev, "Error mapping IRQ!\n");
-		ret = -EINVAL;
-		goto free_mem;
+		return -EINVAL;
 	}
 
 	ret = of_address_to_resource(dn, 0, &res);
 	if (ret) {
 		dev_err(dev, "Error parsing memory region!\n");
-		goto free_mem;
+		goto irq_dispose;
 	}
 
 	regs_start = res.start;
@@ -597,12 +595,11 @@ static int __devinit sirfsoc_dma_probe(struct platform_device *op)
 		goto irq_dispose;
 	}
 
-	ret = devm_request_irq(dev, sdma->irq, &sirfsoc_dma_irq, 0, DRV_NAME,
-		sdma);
+	ret = request_irq(sdma->irq, &sirfsoc_dma_irq, 0, DRV_NAME, sdma);
 	if (ret) {
 		dev_err(dev, "Error requesting IRQ!\n");
 		ret = -EINVAL;
-		goto unmap_mem;
+		goto irq_dispose;
 	}
 
 	dma = &sdma->dma;
@@ -652,13 +649,9 @@ static int __devinit sirfsoc_dma_probe(struct platform_device *op)
 	return 0;
 
 free_irq:
-	devm_free_irq(dev, sdma->irq, sdma);
+	free_irq(sdma->irq, sdma);
 irq_dispose:
 	irq_dispose_mapping(sdma->irq);
-unmap_mem:
-	iounmap(sdma->base);
-free_mem:
-	devm_kfree(dev, sdma);
 	return ret;
 }
 
@@ -668,10 +661,8 @@ static int __devexit sirfsoc_dma_remove(struct platform_device *op)
 	struct sirfsoc_dma *sdma = dev_get_drvdata(dev);
 
 	dma_async_device_unregister(&sdma->dma);
-	devm_free_irq(dev, sdma->irq, sdma);
+	free_irq(sdma->irq, sdma);
 	irq_dispose_mapping(sdma->irq);
-	iounmap(sdma->base);
-	devm_kfree(dev, sdma);
 	return 0;
 }
 
diff --git a/drivers/dma/ste_dma40.c b/drivers/dma/ste_dma40.c
index 000d309602b..ae55091c227 100644
--- a/drivers/dma/ste_dma40.c
+++ b/drivers/dma/ste_dma40.c
@@ -2347,7 +2347,8 @@ static struct dma_async_tx_descriptor *d40_prep_slave_sg(struct dma_chan *chan,
 static struct dma_async_tx_descriptor *
 dma40_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t dma_addr,
 		     size_t buf_len, size_t period_len,
-		     enum dma_transfer_direction direction, void *context)
+		     enum dma_transfer_direction direction, unsigned long flags,
+		     void *context)
 {
 	unsigned int periods = buf_len / period_len;
 	struct dma_async_tx_descriptor *txd;
@@ -2920,19 +2921,23 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 	struct d40_base *base = NULL;
 	int num_log_chans = 0;
 	int num_phy_chans;
+	int clk_ret = -EINVAL;
 	int i;
 	u32 pid;
 	u32 cid;
 	u8 rev;
 
 	clk = clk_get(&pdev->dev, NULL);
-
 	if (IS_ERR(clk)) {
 		d40_err(&pdev->dev, "No matching clock found\n");
 		goto failure;
 	}
 
-	clk_enable(clk);
+	clk_ret = clk_prepare_enable(clk);
+	if (clk_ret) {
+		d40_err(&pdev->dev, "Failed to prepare/enable clock\n");
+		goto failure;
+	}
 
 	/* Get IO for DMAC base address */
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "base");
@@ -3062,10 +3067,10 @@ static struct d40_base * __init d40_hw_detect_init(struct platform_device *pdev)
 	return base;
 
 failure:
-	if (!IS_ERR(clk)) {
-		clk_disable(clk);
+	if (!clk_ret)
+		clk_disable_unprepare(clk);
+	if (!IS_ERR(clk))
 		clk_put(clk);
-	}
 	if (virtbase)
 		iounmap(virtbase);
 	if (res)
diff --git a/drivers/dma/tegra20-apb-dma.c b/drivers/dma/tegra20-apb-dma.c
index 4708467e4d8..528c62dd4b0 100644
--- a/drivers/dma/tegra20-apb-dma.c
+++ b/drivers/dma/tegra20-apb-dma.c
@@ -169,6 +169,7 @@ typedef void (*dma_isr_handler)(struct tegra_dma_channel *tdc,
 /* tegra_dma_channel: Channel specific information */
 struct tegra_dma_channel {
 	struct dma_chan		dma_chan;
+	char			name[30];
 	bool			config_init;
 	int			id;
 	int			irq;
@@ -475,8 +476,7 @@ static void tegra_dma_abort_all(struct tegra_dma_channel *tdc)
 	while (!list_empty(&tdc->pending_sg_req)) {
 		sgreq = list_first_entry(&tdc->pending_sg_req,
 						typeof(*sgreq), node);
-		list_del(&sgreq->node);
-		list_add_tail(&sgreq->node, &tdc->free_sg_req);
+		list_move_tail(&sgreq->node, &tdc->free_sg_req);
 		if (sgreq->last_sg) {
 			dma_desc = sgreq->dma_desc;
 			dma_desc->dma_status = DMA_ERROR;
@@ -570,8 +570,7 @@ static void handle_cont_sngl_cycle_dma_done(struct tegra_dma_channel *tdc,
 
 	/* If not last req then put at end of pending list */
 	if (!list_is_last(&sgreq->node, &tdc->pending_sg_req)) {
-		list_del(&sgreq->node);
-		list_add_tail(&sgreq->node, &tdc->pending_sg_req);
+		list_move_tail(&sgreq->node, &tdc->pending_sg_req);
 		sgreq->configured = false;
 		st = handle_continuous_head_request(tdc, sgreq, to_terminate);
 		if (!st)
@@ -990,7 +989,7 @@ static struct dma_async_tx_descriptor *tegra_dma_prep_slave_sg(
 struct dma_async_tx_descriptor *tegra_dma_prep_dma_cyclic(
 	struct dma_chan *dc, dma_addr_t buf_addr, size_t buf_len,
 	size_t period_len, enum dma_transfer_direction direction,
-	void *context)
+	unsigned long flags, void *context)
 {
 	struct tegra_dma_channel *tdc = to_tegra_dma_chan(dc);
 	struct tegra_dma_desc *dma_desc = NULL;
@@ -1284,7 +1283,6 @@ static int __devinit tegra_dma_probe(struct platform_device *pdev)
 	INIT_LIST_HEAD(&tdma->dma_dev.channels);
 	for (i = 0; i < cdata->nr_channels; i++) {
 		struct tegra_dma_channel *tdc = &tdma->channels[i];
-		char irq_name[30];
 
 		tdc->chan_base_offset = TEGRA_APBDMA_CHANNEL_BASE_ADD_OFFSET +
 					i * TEGRA_APBDMA_CHANNEL_REGISTER_SIZE;
@@ -1296,9 +1294,9 @@ static int __devinit tegra_dma_probe(struct platform_device *pdev)
 			goto err_irq;
 		}
 		tdc->irq = res->start;
-		snprintf(irq_name, sizeof(irq_name), "apbdma.%d", i);
+		snprintf(tdc->name, sizeof(tdc->name), "apbdma.%d", i);
 		ret = devm_request_irq(&pdev->dev, tdc->irq,
-				tegra_dma_isr, 0, irq_name, tdc);
+				tegra_dma_isr, 0, tdc->name, tdc);
 		if (ret) {
 			dev_err(&pdev->dev,
 				"request_irq failed with err %d channel %d\n",
diff --git a/drivers/firewire/core-cdev.c b/drivers/firewire/core-cdev.c
index 2783f69dada..f8d22872d75 100644
--- a/drivers/firewire/core-cdev.c
+++ b/drivers/firewire/core-cdev.c
@@ -473,8 +473,8 @@ static int ioctl_get_info(struct client *client, union ioctl_arg *arg)
 	client->bus_reset_closure = a->bus_reset_closure;
 	if (a->bus_reset != 0) {
 		fill_bus_reset_event(&bus_reset, client);
-		ret = copy_to_user(u64_to_uptr(a->bus_reset),
-				   &bus_reset, sizeof(bus_reset));
+		/* unaligned size of bus_reset is 36 bytes */
+		ret = copy_to_user(u64_to_uptr(a->bus_reset), &bus_reset, 36);
 	}
 	if (ret == 0 && list_empty(&client->link))
 		list_add_tail(&client->link, &client->device->client_list);
diff --git a/drivers/gpio/gpio-stp-xway.c b/drivers/gpio/gpio-stp-xway.c
index e35096bf3cf..8bead0bb645 100644
--- a/drivers/gpio/gpio-stp-xway.c
+++ b/drivers/gpio/gpio-stp-xway.c
@@ -82,7 +82,7 @@ struct xway_stp {
 	struct gpio_chip gc;
 	void __iomem *virt;
 	u32 edge;	/* rising or falling edge triggered shift register */
-	u16 shadow;	/* shadow the shift registers state */
+	u32 shadow;	/* shadow the shift registers state */
 	u8 groups;	/* we can drive 1-3 groups of 8bit each */
 	u8 dsl;		/* the 2 LSBs can be driven by the dsl core */
 	u8 phy1;	/* 3 bits can be driven by phy1 */
diff --git a/drivers/gpu/drm/drm_edid.c b/drivers/gpu/drm/drm_edid.c
index 5dda07cf709..fadcd44ff19 100644
--- a/drivers/gpu/drm/drm_edid.c
+++ b/drivers/gpu/drm/drm_edid.c
@@ -395,13 +395,14 @@ out:
  * \param adapter : i2c device adaptor
  * \return 1 on success
  */
-static bool
+bool
 drm_probe_ddc(struct i2c_adapter *adapter)
 {
 	unsigned char out;
 
 	return (drm_do_probe_ddc_edid(adapter, &out, 0, 1) == 0);
 }
+EXPORT_SYMBOL(drm_probe_ddc);
 
 /**
  * drm_get_edid - get EDID data, if available
diff --git a/drivers/gpu/drm/drm_gem.c b/drivers/gpu/drm/drm_gem.c
index 92177d5aede..24efae464e2 100644
--- a/drivers/gpu/drm/drm_gem.c
+++ b/drivers/gpu/drm/drm_gem.c
@@ -706,7 +706,7 @@ int drm_gem_mmap(struct file *filp, struct vm_area_struct *vma)
 		goto out_unlock;
 	}
 
-	vma->vm_flags |= VM_RESERVED | VM_IO | VM_PFNMAP | VM_DONTEXPAND;
+	vma->vm_flags |= VM_IO | VM_PFNMAP | VM_DONTEXPAND | VM_DONTDUMP;
 	vma->vm_ops = obj->dev->driver->gem_vm_ops;
 	vma->vm_private_data = map->handle;
 	vma->vm_page_prot =  pgprot_writecombine(vm_get_page_prot(vma->vm_flags));
diff --git a/drivers/gpu/drm/drm_vm.c b/drivers/gpu/drm/drm_vm.c
index 23a824e6a22..db7bd292410 100644
--- a/drivers/gpu/drm/drm_vm.c
+++ b/drivers/gpu/drm/drm_vm.c
@@ -514,8 +514,7 @@ static int drm_mmap_dma(struct file *filp, struct vm_area_struct *vma)
 
 	vma->vm_ops = &drm_vm_dma_ops;
 
-	vma->vm_flags |= VM_RESERVED;	/* Don't swap */
-	vma->vm_flags |= VM_DONTEXPAND;
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
 
 	drm_vm_open_locked(dev, vma);
 	return 0;
@@ -643,21 +642,16 @@ int drm_mmap_locked(struct file *filp, struct vm_area_struct *vma)
 	case _DRM_SHM:
 		vma->vm_ops = &drm_vm_shm_ops;
 		vma->vm_private_data = (void *)map;
-		/* Don't let this area swap.  Change when
-		   DRM_KERNEL advisory is supported. */
-		vma->vm_flags |= VM_RESERVED;
 		break;
 	case _DRM_SCATTER_GATHER:
 		vma->vm_ops = &drm_vm_sg_ops;
 		vma->vm_private_data = (void *)map;
-		vma->vm_flags |= VM_RESERVED;
 		vma->vm_page_prot = drm_dma_prot(map->type, vma);
 		break;
 	default:
 		return -EINVAL;	/* This should never happen. */
 	}
-	vma->vm_flags |= VM_RESERVED;	/* Don't swap */
-	vma->vm_flags |= VM_DONTEXPAND;
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
 
 	drm_vm_open_locked(dev, vma);
 	return 0;
diff --git a/drivers/gpu/drm/exynos/exynos_ddc.c b/drivers/gpu/drm/exynos/exynos_ddc.c
index 961a1806a24..37e6ec704e1 100644
--- a/drivers/gpu/drm/exynos/exynos_ddc.c
+++ b/drivers/gpu/drm/exynos/exynos_ddc.c
@@ -26,29 +26,41 @@ static int s5p_ddc_probe(struct i2c_client *client,
 {
 	hdmi_attach_ddc_client(client);
 
-	dev_info(&client->adapter->dev, "attached s5p_ddc "
-		"into i2c adapter successfully\n");
+	dev_info(&client->adapter->dev,
+		"attached %s into i2c adapter successfully\n",
+		client->name);
 
 	return 0;
 }
 
 static int s5p_ddc_remove(struct i2c_client *client)
 {
-	dev_info(&client->adapter->dev, "detached s5p_ddc "
-		"from i2c adapter successfully\n");
+	dev_info(&client->adapter->dev,
+		"detached %s from i2c adapter successfully\n",
+		client->name);
 
 	return 0;
 }
 
 static struct i2c_device_id ddc_idtable[] = {
 	{"s5p_ddc", 0},
+	{"exynos5-hdmiddc", 0},
 	{ },
 };
 
+static struct of_device_id hdmiddc_match_types[] = {
+	{
+		.compatible = "samsung,exynos5-hdmiddc",
+	}, {
+		/* end node */
+	}
+};
+
 struct i2c_driver ddc_driver = {
 	.driver = {
-		.name = "s5p_ddc",
+		.name = "exynos-hdmiddc",
 		.owner = THIS_MODULE,
+		.of_match_table = hdmiddc_match_types,
 	},
 	.id_table	= ddc_idtable,
 	.probe		= s5p_ddc_probe,
diff --git a/drivers/gpu/drm/exynos/exynos_drm_connector.c b/drivers/gpu/drm/exynos/exynos_drm_connector.c
index c2b1b1441ed..18c271862ca 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_connector.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_connector.c
@@ -40,6 +40,7 @@ struct exynos_drm_connector {
 	struct drm_connector	drm_connector;
 	uint32_t		encoder_id;
 	struct exynos_drm_manager *manager;
+	uint32_t		dpms;
 };
 
 /* convert exynos_video_timings to drm_display_mode */
@@ -149,8 +150,12 @@ static int exynos_drm_connector_get_modes(struct drm_connector *connector)
 		count = drm_add_edid_modes(connector, edid);
 		kfree(edid);
 	} else {
-		struct drm_display_mode *mode = drm_mode_create(connector->dev);
 		struct exynos_drm_panel_info *panel;
+		struct drm_display_mode *mode = drm_mode_create(connector->dev);
+		if (!mode) {
+			DRM_ERROR("failed to create a new display mode.\n");
+			return 0;
+		}
 
 		if (display_ops->get_panel)
 			panel = display_ops->get_panel(manager->dev);
@@ -194,8 +199,7 @@ static int exynos_drm_connector_mode_valid(struct drm_connector *connector,
 	return ret;
 }
 
-static struct drm_encoder *exynos_drm_best_encoder(
-						struct drm_connector *connector)
+struct drm_encoder *exynos_drm_best_encoder(struct drm_connector *connector)
 {
 	struct drm_device *dev = connector->dev;
 	struct exynos_drm_connector *exynos_connector =
@@ -224,6 +228,43 @@ static struct drm_connector_helper_funcs exynos_connector_helper_funcs = {
 	.best_encoder	= exynos_drm_best_encoder,
 };
 
+void exynos_drm_display_power(struct drm_connector *connector, int mode)
+{
+	struct drm_encoder *encoder = exynos_drm_best_encoder(connector);
+	struct exynos_drm_connector *exynos_connector;
+	struct exynos_drm_manager *manager = exynos_drm_get_manager(encoder);
+	struct exynos_drm_display_ops *display_ops = manager->display_ops;
+
+	exynos_connector = to_exynos_connector(connector);
+
+	if (exynos_connector->dpms == mode) {
+		DRM_DEBUG_KMS("desired dpms mode is same as previous one.\n");
+		return;
+	}
+
+	if (display_ops && display_ops->power_on)
+		display_ops->power_on(manager->dev, mode);
+
+	exynos_connector->dpms = mode;
+}
+
+static void exynos_drm_connector_dpms(struct drm_connector *connector,
+					int mode)
+{
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	/*
+	 * in case that drm_crtc_helper_set_mode() is called,
+	 * encoder/crtc->funcs->dpms() will be just returned
+	 * because they already were DRM_MODE_DPMS_ON so only
+	 * exynos_drm_display_power() will be called.
+	 */
+	drm_helper_connector_dpms(connector, mode);
+
+	exynos_drm_display_power(connector, mode);
+
+}
+
 static int exynos_drm_connector_fill_modes(struct drm_connector *connector,
 				unsigned int max_width, unsigned int max_height)
 {
@@ -283,7 +324,7 @@ static void exynos_drm_connector_destroy(struct drm_connector *connector)
 }
 
 static struct drm_connector_funcs exynos_connector_funcs = {
-	.dpms		= drm_helper_connector_dpms,
+	.dpms		= exynos_drm_connector_dpms,
 	.fill_modes	= exynos_drm_connector_fill_modes,
 	.detect		= exynos_drm_connector_detect,
 	.destroy	= exynos_drm_connector_destroy,
@@ -332,6 +373,7 @@ struct drm_connector *exynos_drm_connector_create(struct drm_device *dev,
 
 	exynos_connector->encoder_id = encoder->base.id;
 	exynos_connector->manager = manager;
+	exynos_connector->dpms = DRM_MODE_DPMS_OFF;
 	connector->encoder = encoder;
 
 	err = drm_mode_connector_attach_encoder(connector, encoder);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_connector.h b/drivers/gpu/drm/exynos/exynos_drm_connector.h
index 1c7b2b5b579..22f6cc442c3 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_connector.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_connector.h
@@ -31,4 +31,8 @@
 struct drm_connector *exynos_drm_connector_create(struct drm_device *dev,
 						   struct drm_encoder *encoder);
 
+struct drm_encoder *exynos_drm_best_encoder(struct drm_connector *connector);
+
+void exynos_drm_display_power(struct drm_connector *connector, int mode);
+
 #endif
diff --git a/drivers/gpu/drm/exynos/exynos_drm_core.c b/drivers/gpu/drm/exynos/exynos_drm_core.c
index 19bdf0a194e..94026ad76a7 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_core.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_core.c
@@ -34,33 +34,15 @@
 
 static LIST_HEAD(exynos_drm_subdrv_list);
 
-static int exynos_drm_subdrv_probe(struct drm_device *dev,
+static int exynos_drm_create_enc_conn(struct drm_device *dev,
 					struct exynos_drm_subdrv *subdrv)
 {
 	struct drm_encoder *encoder;
 	struct drm_connector *connector;
+	int ret;
 
 	DRM_DEBUG_DRIVER("%s\n", __FILE__);
 
-	if (subdrv->probe) {
-		int ret;
-
-		/*
-		 * this probe callback would be called by sub driver
-		 * after setting of all resources to this sub driver,
-		 * such as clock, irq and register map are done or by load()
-		 * of exynos drm driver.
-		 *
-		 * P.S. note that this driver is considered for modularization.
-		 */
-		ret = subdrv->probe(dev, subdrv->dev);
-		if (ret)
-			return ret;
-	}
-
-	if (!subdrv->manager)
-		return 0;
-
 	subdrv->manager->dev = subdrv->dev;
 
 	/* create and initialize a encoder for this sub driver. */
@@ -78,24 +60,22 @@ static int exynos_drm_subdrv_probe(struct drm_device *dev,
 	connector = exynos_drm_connector_create(dev, encoder);
 	if (!connector) {
 		DRM_ERROR("failed to create connector\n");
-		encoder->funcs->destroy(encoder);
-		return -EFAULT;
+		ret = -EFAULT;
+		goto err_destroy_encoder;
 	}
 
 	subdrv->encoder = encoder;
 	subdrv->connector = connector;
 
 	return 0;
+
+err_destroy_encoder:
+	encoder->funcs->destroy(encoder);
+	return ret;
 }
 
-static void exynos_drm_subdrv_remove(struct drm_device *dev,
-				      struct exynos_drm_subdrv *subdrv)
+static void exynos_drm_destroy_enc_conn(struct exynos_drm_subdrv *subdrv)
 {
-	DRM_DEBUG_DRIVER("%s\n", __FILE__);
-
-	if (subdrv->remove)
-		subdrv->remove(dev);
-
 	if (subdrv->encoder) {
 		struct drm_encoder *encoder = subdrv->encoder;
 		encoder->funcs->destroy(encoder);
@@ -109,9 +89,43 @@ static void exynos_drm_subdrv_remove(struct drm_device *dev,
 	}
 }
 
+static int exynos_drm_subdrv_probe(struct drm_device *dev,
+					struct exynos_drm_subdrv *subdrv)
+{
+	if (subdrv->probe) {
+		int ret;
+
+		subdrv->drm_dev = dev;
+
+		/*
+		 * this probe callback would be called by sub driver
+		 * after setting of all resources to this sub driver,
+		 * such as clock, irq and register map are done or by load()
+		 * of exynos drm driver.
+		 *
+		 * P.S. note that this driver is considered for modularization.
+		 */
+		ret = subdrv->probe(dev, subdrv->dev);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static void exynos_drm_subdrv_remove(struct drm_device *dev,
+				      struct exynos_drm_subdrv *subdrv)
+{
+	DRM_DEBUG_DRIVER("%s\n", __FILE__);
+
+	if (subdrv->remove)
+		subdrv->remove(dev, subdrv->dev);
+}
+
 int exynos_drm_device_register(struct drm_device *dev)
 {
 	struct exynos_drm_subdrv *subdrv, *n;
+	unsigned int fine_cnt = 0;
 	int err;
 
 	DRM_DEBUG_DRIVER("%s\n", __FILE__);
@@ -120,14 +134,36 @@ int exynos_drm_device_register(struct drm_device *dev)
 		return -EINVAL;
 
 	list_for_each_entry_safe(subdrv, n, &exynos_drm_subdrv_list, list) {
-		subdrv->drm_dev = dev;
 		err = exynos_drm_subdrv_probe(dev, subdrv);
 		if (err) {
 			DRM_DEBUG("exynos drm subdrv probe failed.\n");
 			list_del(&subdrv->list);
+			continue;
+		}
+
+		/*
+		 * if manager is null then it means that this sub driver
+		 * doesn't need encoder and connector.
+		 */
+		if (!subdrv->manager) {
+			fine_cnt++;
+			continue;
+		}
+
+		err = exynos_drm_create_enc_conn(dev, subdrv);
+		if (err) {
+			DRM_DEBUG("failed to create encoder and connector.\n");
+			exynos_drm_subdrv_remove(dev, subdrv);
+			list_del(&subdrv->list);
+			continue;
 		}
+
+		fine_cnt++;
 	}
 
+	if (!fine_cnt)
+		return -EINVAL;
+
 	return 0;
 }
 EXPORT_SYMBOL_GPL(exynos_drm_device_register);
@@ -143,8 +179,10 @@ int exynos_drm_device_unregister(struct drm_device *dev)
 		return -EINVAL;
 	}
 
-	list_for_each_entry(subdrv, &exynos_drm_subdrv_list, list)
+	list_for_each_entry(subdrv, &exynos_drm_subdrv_list, list) {
 		exynos_drm_subdrv_remove(dev, subdrv);
+		exynos_drm_destroy_enc_conn(subdrv);
+	}
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/exynos/exynos_drm_crtc.c b/drivers/gpu/drm/exynos/exynos_drm_crtc.c
index df1e34f0f09..fce245f64c4 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_crtc.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_crtc.c
@@ -66,7 +66,6 @@ struct exynos_drm_crtc {
 
 static void exynos_drm_crtc_dpms(struct drm_crtc *crtc, int mode)
 {
-	struct drm_device *dev = crtc->dev;
 	struct exynos_drm_crtc *exynos_crtc = to_exynos_crtc(crtc);
 
 	DRM_DEBUG_KMS("crtc[%d] mode[%d]\n", crtc->base.id, mode);
@@ -76,12 +75,8 @@ static void exynos_drm_crtc_dpms(struct drm_crtc *crtc, int mode)
 		return;
 	}
 
-	mutex_lock(&dev->struct_mutex);
-
 	exynos_drm_fn_encoder(crtc, &mode, exynos_drm_encoder_crtc_dpms);
 	exynos_crtc->dpms = mode;
-
-	mutex_unlock(&dev->struct_mutex);
 }
 
 static void exynos_drm_crtc_prepare(struct drm_crtc *crtc)
@@ -97,6 +92,7 @@ static void exynos_drm_crtc_commit(struct drm_crtc *crtc)
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
+	exynos_drm_crtc_dpms(crtc, DRM_MODE_DPMS_ON);
 	exynos_plane_commit(exynos_crtc->plane);
 	exynos_plane_dpms(exynos_crtc->plane, DRM_MODE_DPMS_ON);
 }
@@ -126,8 +122,6 @@ exynos_drm_crtc_mode_set(struct drm_crtc *crtc, struct drm_display_mode *mode,
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	exynos_drm_crtc_dpms(crtc, DRM_MODE_DPMS_ON);
-
 	/*
 	 * copy the mode data adjusted by mode_fixup() into crtc->mode
 	 * so that hardware can be seet to proper mode.
@@ -161,6 +155,12 @@ static int exynos_drm_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
+	/* when framebuffer changing is requested, crtc's dpms should be on */
+	if (exynos_crtc->dpms > DRM_MODE_DPMS_ON) {
+		DRM_ERROR("failed framebuffer changing request.\n");
+		return -EPERM;
+	}
+
 	crtc_w = crtc->fb->width - x;
 	crtc_h = crtc->fb->height - y;
 
@@ -213,6 +213,12 @@ static int exynos_drm_crtc_page_flip(struct drm_crtc *crtc,
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
+	/* when the page flip is requested, crtc's dpms should be on */
+	if (exynos_crtc->dpms > DRM_MODE_DPMS_ON) {
+		DRM_ERROR("failed page flip request.\n");
+		return -EINVAL;
+	}
+
 	mutex_lock(&dev->struct_mutex);
 
 	if (event) {
diff --git a/drivers/gpu/drm/exynos/exynos_drm_drv.h b/drivers/gpu/drm/exynos/exynos_drm_drv.h
index a4ab98b52dd..a3423103649 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_drv.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_drv.h
@@ -36,6 +36,20 @@
 #define MAX_FB_BUFFER	4
 #define DEFAULT_ZPOS	-1
 
+#define _wait_for(COND, MS) ({ \
+	unsigned long timeout__ = jiffies + msecs_to_jiffies(MS);	\
+	int ret__ = 0;							\
+	while (!(COND)) {						\
+		if (time_after(jiffies, timeout__)) {			\
+			ret__ = -ETIMEDOUT;				\
+			break;						\
+		}							\
+	}								\
+	ret__;								\
+})
+
+#define wait_for(COND, MS) _wait_for(COND, MS)
+
 struct drm_device;
 struct exynos_drm_overlay;
 struct drm_connector;
@@ -60,6 +74,8 @@ enum exynos_drm_output_type {
  * @commit: apply hardware specific overlay data to registers.
  * @enable: enable hardware specific overlay.
  * @disable: disable hardware specific overlay.
+ * @wait_for_vblank: wait for vblank interrupt to make sure that
+ *	hardware overlay is disabled.
  */
 struct exynos_drm_overlay_ops {
 	void (*mode_set)(struct device *subdrv_dev,
@@ -67,6 +83,7 @@ struct exynos_drm_overlay_ops {
 	void (*commit)(struct device *subdrv_dev, int zpos);
 	void (*enable)(struct device *subdrv_dev, int zpos);
 	void (*disable)(struct device *subdrv_dev, int zpos);
+	void (*wait_for_vblank)(struct device *subdrv_dev);
 };
 
 /*
@@ -265,7 +282,7 @@ struct exynos_drm_subdrv {
 	struct exynos_drm_manager *manager;
 
 	int (*probe)(struct drm_device *drm_dev, struct device *dev);
-	void (*remove)(struct drm_device *dev);
+	void (*remove)(struct drm_device *drm_dev, struct device *dev);
 	int (*open)(struct drm_device *drm_dev, struct device *dev,
 			struct drm_file *file);
 	void (*close)(struct drm_device *drm_dev, struct device *dev,
diff --git a/drivers/gpu/drm/exynos/exynos_drm_encoder.c b/drivers/gpu/drm/exynos/exynos_drm_encoder.c
index 39bd8abff3f..e51503fbaf2 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_encoder.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_encoder.c
@@ -31,6 +31,7 @@
 
 #include "exynos_drm_drv.h"
 #include "exynos_drm_encoder.h"
+#include "exynos_drm_connector.h"
 
 #define to_exynos_encoder(x)	container_of(x, struct exynos_drm_encoder,\
 				drm_encoder)
@@ -44,26 +45,23 @@
  * @dpms: store the encoder dpms value.
  */
 struct exynos_drm_encoder {
+	struct drm_crtc			*old_crtc;
 	struct drm_encoder		drm_encoder;
 	struct exynos_drm_manager	*manager;
 	int dpms;
 };
 
-static void exynos_drm_display_power(struct drm_encoder *encoder, int mode)
+static void exynos_drm_connector_power(struct drm_encoder *encoder, int mode)
 {
 	struct drm_device *dev = encoder->dev;
 	struct drm_connector *connector;
-	struct exynos_drm_manager *manager = exynos_drm_get_manager(encoder);
 
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
-		if (connector->encoder == encoder) {
-			struct exynos_drm_display_ops *display_ops =
-							manager->display_ops;
-
+		if (exynos_drm_best_encoder(connector) == encoder) {
 			DRM_DEBUG_KMS("connector[%d] dpms[%d]\n",
 					connector->base.id, mode);
-			if (display_ops && display_ops->power_on)
-				display_ops->power_on(manager->dev, mode);
+
+			exynos_drm_display_power(connector, mode);
 		}
 	}
 }
@@ -88,13 +86,13 @@ static void exynos_drm_encoder_dpms(struct drm_encoder *encoder, int mode)
 	case DRM_MODE_DPMS_ON:
 		if (manager_ops && manager_ops->apply)
 			manager_ops->apply(manager->dev);
-		exynos_drm_display_power(encoder, mode);
+		exynos_drm_connector_power(encoder, mode);
 		exynos_encoder->dpms = mode;
 		break;
 	case DRM_MODE_DPMS_STANDBY:
 	case DRM_MODE_DPMS_SUSPEND:
 	case DRM_MODE_DPMS_OFF:
-		exynos_drm_display_power(encoder, mode);
+		exynos_drm_connector_power(encoder, mode);
 		exynos_encoder->dpms = mode;
 		break;
 	default:
@@ -127,24 +125,74 @@ exynos_drm_encoder_mode_fixup(struct drm_encoder *encoder,
 	return true;
 }
 
+static void disable_plane_to_crtc(struct drm_device *dev,
+						struct drm_crtc *old_crtc,
+						struct drm_crtc *new_crtc)
+{
+	struct drm_plane *plane;
+
+	/*
+	 * if old_crtc isn't same as encoder->crtc then it means that
+	 * user changed crtc id to another one so the plane to old_crtc
+	 * should be disabled and plane->crtc should be set to new_crtc
+	 * (encoder->crtc)
+	 */
+	list_for_each_entry(plane, &dev->mode_config.plane_list, head) {
+		if (plane->crtc == old_crtc) {
+			/*
+			 * do not change below call order.
+			 *
+			 * plane->funcs->disable_plane call checks
+			 * if encoder->crtc is same as plane->crtc and if same
+			 * then overlay_ops->disable callback will be called
+			 * to diasble current hw overlay so plane->crtc should
+			 * have new_crtc because new_crtc was set to
+			 * encoder->crtc in advance.
+			 */
+			plane->crtc = new_crtc;
+			plane->funcs->disable_plane(plane);
+		}
+	}
+}
+
 static void exynos_drm_encoder_mode_set(struct drm_encoder *encoder,
 					 struct drm_display_mode *mode,
 					 struct drm_display_mode *adjusted_mode)
 {
 	struct drm_device *dev = encoder->dev;
 	struct drm_connector *connector;
-	struct exynos_drm_manager *manager = exynos_drm_get_manager(encoder);
-	struct exynos_drm_manager_ops *manager_ops = manager->ops;
+	struct exynos_drm_manager *manager;
+	struct exynos_drm_manager_ops *manager_ops;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	exynos_drm_encoder_dpms(encoder, DRM_MODE_DPMS_ON);
-
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
-		if (connector->encoder == encoder)
+		if (connector->encoder == encoder) {
+			struct exynos_drm_encoder *exynos_encoder;
+
+			exynos_encoder = to_exynos_encoder(encoder);
+
+			if (exynos_encoder->old_crtc != encoder->crtc &&
+					exynos_encoder->old_crtc) {
+
+				/*
+				 * disable a plane to old crtc and change
+				 * crtc of the plane to new one.
+				 */
+				disable_plane_to_crtc(dev,
+						exynos_encoder->old_crtc,
+						encoder->crtc);
+			}
+
+			manager = exynos_drm_get_manager(encoder);
+			manager_ops = manager->ops;
+
 			if (manager_ops && manager_ops->mode_set)
 				manager_ops->mode_set(manager->dev,
 							adjusted_mode);
+
+			exynos_encoder->old_crtc = encoder->crtc;
+		}
 	}
 }
 
@@ -166,12 +214,27 @@ static void exynos_drm_encoder_commit(struct drm_encoder *encoder)
 		manager_ops->commit(manager->dev);
 }
 
+static void exynos_drm_encoder_disable(struct drm_encoder *encoder)
+{
+	struct drm_plane *plane;
+	struct drm_device *dev = encoder->dev;
+
+	exynos_drm_encoder_dpms(encoder, DRM_MODE_DPMS_OFF);
+
+	/* all planes connected to this encoder should be also disabled. */
+	list_for_each_entry(plane, &dev->mode_config.plane_list, head) {
+		if (plane->crtc == encoder->crtc)
+			plane->funcs->disable_plane(plane);
+	}
+}
+
 static struct drm_encoder_helper_funcs exynos_encoder_helper_funcs = {
 	.dpms		= exynos_drm_encoder_dpms,
 	.mode_fixup	= exynos_drm_encoder_mode_fixup,
 	.mode_set	= exynos_drm_encoder_mode_set,
 	.prepare	= exynos_drm_encoder_prepare,
 	.commit		= exynos_drm_encoder_commit,
+	.disable	= exynos_drm_encoder_disable,
 };
 
 static void exynos_drm_encoder_destroy(struct drm_encoder *encoder)
@@ -338,6 +401,19 @@ void exynos_drm_encoder_crtc_dpms(struct drm_encoder *encoder, void *data)
 		manager_ops->dpms(manager->dev, mode);
 
 	/*
+	 * set current mode to new one so that data aren't updated into
+	 * registers by drm_helper_connector_dpms two times.
+	 *
+	 * in case that drm_crtc_helper_set_mode() is called,
+	 * overlay_ops->commit() and manager_ops->commit() callbacks
+	 * can be called two times, first at drm_crtc_helper_set_mode()
+	 * and second at drm_helper_connector_dpms().
+	 * so with this setting, when drm_helper_connector_dpms() is called
+	 * encoder->funcs->dpms() will be ignored.
+	 */
+	exynos_encoder->dpms = mode;
+
+	/*
 	 * if this condition is ok then it means that the crtc is already
 	 * detached from encoder and last function for detaching is properly
 	 * done, so clear pipe from manager to prevent repeated call.
@@ -422,4 +498,14 @@ void exynos_drm_encoder_plane_disable(struct drm_encoder *encoder, void *data)
 
 	if (overlay_ops && overlay_ops->disable)
 		overlay_ops->disable(manager->dev, zpos);
+
+	/*
+	 * wait for vblank interrupt
+	 * - this makes sure that hardware overlay is disabled to avoid
+	 * for the dma accesses to memory after gem buffer was released
+	 * because the setting for disabling the overlay will be updated
+	 * at vsync.
+	 */
+	if (overlay_ops->wait_for_vblank)
+		overlay_ops->wait_for_vblank(manager->dev);
 }
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fb.c b/drivers/gpu/drm/exynos/exynos_drm_fb.c
index 53afcc5f094..4ef4cd3f993 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fb.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fb.c
@@ -41,10 +41,12 @@
  * exynos specific framebuffer structure.
  *
  * @fb: drm framebuffer obejct.
+ * @buf_cnt: a buffer count to drm framebuffer.
  * @exynos_gem_obj: array of exynos specific gem object containing a gem object.
  */
 struct exynos_drm_fb {
 	struct drm_framebuffer		fb;
+	unsigned int			buf_cnt;
 	struct exynos_drm_gem_obj	*exynos_gem_obj[MAX_FB_BUFFER];
 };
 
@@ -101,6 +103,25 @@ static struct drm_framebuffer_funcs exynos_drm_fb_funcs = {
 	.dirty		= exynos_drm_fb_dirty,
 };
 
+void exynos_drm_fb_set_buf_cnt(struct drm_framebuffer *fb,
+						unsigned int cnt)
+{
+	struct exynos_drm_fb *exynos_fb;
+
+	exynos_fb = to_exynos_fb(fb);
+
+	exynos_fb->buf_cnt = cnt;
+}
+
+unsigned int exynos_drm_fb_get_buf_cnt(struct drm_framebuffer *fb)
+{
+	struct exynos_drm_fb *exynos_fb;
+
+	exynos_fb = to_exynos_fb(fb);
+
+	return exynos_fb->buf_cnt;
+}
+
 struct drm_framebuffer *
 exynos_drm_framebuffer_init(struct drm_device *dev,
 			    struct drm_mode_fb_cmd2 *mode_cmd,
@@ -127,6 +148,43 @@ exynos_drm_framebuffer_init(struct drm_device *dev,
 	return &exynos_fb->fb;
 }
 
+static u32 exynos_drm_format_num_buffers(struct drm_mode_fb_cmd2 *mode_cmd)
+{
+	unsigned int cnt = 0;
+
+	if (mode_cmd->pixel_format != DRM_FORMAT_NV12)
+		return drm_format_num_planes(mode_cmd->pixel_format);
+
+	while (cnt != MAX_FB_BUFFER) {
+		if (!mode_cmd->handles[cnt])
+			break;
+		cnt++;
+	}
+
+	/*
+	 * check if NV12 or NV12M.
+	 *
+	 * NV12
+	 * handles[0] = base1, offsets[0] = 0
+	 * handles[1] = base1, offsets[1] = Y_size
+	 *
+	 * NV12M
+	 * handles[0] = base1, offsets[0] = 0
+	 * handles[1] = base2, offsets[1] = 0
+	 */
+	if (cnt == 2) {
+		/*
+		 * in case of NV12 format, offsets[1] is not 0 and
+		 * handles[0] is same as handles[1].
+		 */
+		if (mode_cmd->offsets[1] &&
+			mode_cmd->handles[0] == mode_cmd->handles[1])
+			cnt = 1;
+	}
+
+	return cnt;
+}
+
 static struct drm_framebuffer *
 exynos_user_fb_create(struct drm_device *dev, struct drm_file *file_priv,
 		      struct drm_mode_fb_cmd2 *mode_cmd)
@@ -134,7 +192,6 @@ exynos_user_fb_create(struct drm_device *dev, struct drm_file *file_priv,
 	struct drm_gem_object *obj;
 	struct drm_framebuffer *fb;
 	struct exynos_drm_fb *exynos_fb;
-	int nr;
 	int i;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
@@ -152,9 +209,11 @@ exynos_user_fb_create(struct drm_device *dev, struct drm_file *file_priv,
 	}
 
 	exynos_fb = to_exynos_fb(fb);
-	nr = exynos_drm_format_num_buffers(fb->pixel_format);
+	exynos_fb->buf_cnt = exynos_drm_format_num_buffers(mode_cmd);
+
+	DRM_DEBUG_KMS("buf_cnt = %d\n", exynos_fb->buf_cnt);
 
-	for (i = 1; i < nr; i++) {
+	for (i = 1; i < exynos_fb->buf_cnt; i++) {
 		obj = drm_gem_object_lookup(dev, file_priv,
 				mode_cmd->handles[i]);
 		if (!obj) {
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fb.h b/drivers/gpu/drm/exynos/exynos_drm_fb.h
index 50823756cde..96262e54f76 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fb.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_fb.h
@@ -28,19 +28,6 @@
 #ifndef _EXYNOS_DRM_FB_H_
 #define _EXYNOS_DRM_FB_H
 
-static inline int exynos_drm_format_num_buffers(uint32_t format)
-{
-	switch (format) {
-	case DRM_FORMAT_NV12:
-	case DRM_FORMAT_NV12MT:
-		return 2;
-	case DRM_FORMAT_YUV420:
-		return 3;
-	default:
-		return 1;
-	}
-}
-
 struct drm_framebuffer *
 exynos_drm_framebuffer_init(struct drm_device *dev,
 			    struct drm_mode_fb_cmd2 *mode_cmd,
@@ -52,4 +39,11 @@ struct exynos_drm_gem_buf *exynos_drm_fb_buffer(struct drm_framebuffer *fb,
 
 void exynos_drm_mode_config_init(struct drm_device *dev);
 
+/* set a buffer count to drm framebuffer. */
+void exynos_drm_fb_set_buf_cnt(struct drm_framebuffer *fb,
+						unsigned int cnt);
+
+/* get a buffer count to drm framebuffer. */
+unsigned int exynos_drm_fb_get_buf_cnt(struct drm_framebuffer *fb);
+
 #endif
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fbdev.c b/drivers/gpu/drm/exynos/exynos_drm_fbdev.c
index bd4ff634823..67eb6ba56ed 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fbdev.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fbdev.c
@@ -79,6 +79,9 @@ static int exynos_drm_fbdev_update(struct drm_fb_helper *helper,
 		return -EFAULT;
 	}
 
+	/* buffer count to framebuffer always is 1 at booting time. */
+	exynos_drm_fb_set_buf_cnt(fb, 1);
+
 	offset = fbi->var.xoffset * (fb->bits_per_pixel >> 3);
 	offset += fbi->var.yoffset * fb->pitches[0];
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fimd.c b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
index 58d50e368a5..130a2b510d4 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fimd.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
@@ -19,8 +19,8 @@
 #include <linux/clk.h>
 #include <linux/pm_runtime.h>
 
+#include <video/samsung_fimd.h>
 #include <drm/exynos_drm.h>
-#include <plat/regs-fb-v4.h>
 
 #include "exynos_drm_drv.h"
 #include "exynos_drm_fbdev.h"
@@ -57,6 +57,18 @@
 
 #define get_fimd_context(dev)	platform_get_drvdata(to_platform_device(dev))
 
+struct fimd_driver_data {
+	unsigned int timing_base;
+};
+
+struct fimd_driver_data exynos4_fimd_driver_data = {
+	.timing_base = 0x0,
+};
+
+struct fimd_driver_data exynos5_fimd_driver_data = {
+	.timing_base = 0x20000,
+};
+
 struct fimd_win_data {
 	unsigned int		offset_x;
 	unsigned int		offset_y;
@@ -91,6 +103,13 @@ struct fimd_context {
 	struct exynos_drm_panel_info *panel;
 };
 
+static inline struct fimd_driver_data *drm_fimd_get_driver_data(
+	struct platform_device *pdev)
+{
+	return (struct fimd_driver_data *)
+		platform_get_device_id(pdev)->driver_data;
+}
+
 static bool fimd_display_is_connected(struct device *dev)
 {
 	DRM_DEBUG_KMS("%s\n", __FILE__);
@@ -194,32 +213,35 @@ static void fimd_commit(struct device *dev)
 	struct fimd_context *ctx = get_fimd_context(dev);
 	struct exynos_drm_panel_info *panel = ctx->panel;
 	struct fb_videomode *timing = &panel->timing;
+	struct fimd_driver_data *driver_data;
+	struct platform_device *pdev = to_platform_device(dev);
 	u32 val;
 
+	driver_data = drm_fimd_get_driver_data(pdev);
 	if (ctx->suspended)
 		return;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
 	/* setup polarity values from machine code. */
-	writel(ctx->vidcon1, ctx->regs + VIDCON1);
+	writel(ctx->vidcon1, ctx->regs + driver_data->timing_base + VIDCON1);
 
 	/* setup vertical timing values. */
 	val = VIDTCON0_VBPD(timing->upper_margin - 1) |
 	       VIDTCON0_VFPD(timing->lower_margin - 1) |
 	       VIDTCON0_VSPW(timing->vsync_len - 1);
-	writel(val, ctx->regs + VIDTCON0);
+	writel(val, ctx->regs + driver_data->timing_base + VIDTCON0);
 
 	/* setup horizontal timing values.  */
 	val = VIDTCON1_HBPD(timing->left_margin - 1) |
 	       VIDTCON1_HFPD(timing->right_margin - 1) |
 	       VIDTCON1_HSPW(timing->hsync_len - 1);
-	writel(val, ctx->regs + VIDTCON1);
+	writel(val, ctx->regs + driver_data->timing_base + VIDTCON1);
 
 	/* setup horizontal and vertical display size. */
 	val = VIDTCON2_LINEVAL(timing->yres - 1) |
 	       VIDTCON2_HOZVAL(timing->xres - 1);
-	writel(val, ctx->regs + VIDTCON2);
+	writel(val, ctx->regs + driver_data->timing_base + VIDTCON2);
 
 	/* setup clock source, clock divider, enable dma. */
 	val = ctx->vidcon0;
@@ -570,10 +592,22 @@ static void fimd_win_disable(struct device *dev, int zpos)
 	win_data->enabled = false;
 }
 
+static void fimd_wait_for_vblank(struct device *dev)
+{
+	struct fimd_context *ctx = get_fimd_context(dev);
+	int ret;
+
+	ret = wait_for((__raw_readl(ctx->regs + VIDCON1) &
+					VIDCON1_VSTATUS_VSYNC), 50);
+	if (ret < 0)
+		DRM_DEBUG_KMS("vblank wait timed out.\n");
+}
+
 static struct exynos_drm_overlay_ops fimd_overlay_ops = {
 	.mode_set = fimd_win_mode_set,
 	.commit = fimd_win_commit,
 	.disable = fimd_win_disable,
+	.wait_for_vblank = fimd_wait_for_vblank,
 };
 
 static struct exynos_drm_manager fimd_manager = {
@@ -678,7 +712,7 @@ static int fimd_subdrv_probe(struct drm_device *drm_dev, struct device *dev)
 	return 0;
 }
 
-static void fimd_subdrv_remove(struct drm_device *drm_dev)
+static void fimd_subdrv_remove(struct drm_device *drm_dev, struct device *dev)
 {
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
@@ -747,16 +781,10 @@ static void fimd_clear_win(struct fimd_context *ctx, int win)
 	writel(val, ctx->regs + SHADOWCON);
 }
 
-static int fimd_power_on(struct fimd_context *ctx, bool enable)
+static int fimd_clock(struct fimd_context *ctx, bool enable)
 {
-	struct exynos_drm_subdrv *subdrv = &ctx->subdrv;
-	struct device *dev = subdrv->dev;
-
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	if (enable != false && enable != true)
-		return -EINVAL;
-
 	if (enable) {
 		int ret;
 
@@ -769,18 +797,31 @@ static int fimd_power_on(struct fimd_context *ctx, bool enable)
 			clk_disable(ctx->bus_clk);
 			return ret;
 		}
+	} else {
+		clk_disable(ctx->lcd_clk);
+		clk_disable(ctx->bus_clk);
+	}
+
+	return 0;
+}
+
+static int fimd_activate(struct fimd_context *ctx, bool enable)
+{
+	if (enable) {
+		int ret;
+		struct device *dev = ctx->subdrv.dev;
+
+		ret = fimd_clock(ctx, true);
+		if (ret < 0)
+			return ret;
 
 		ctx->suspended = false;
 
 		/* if vblank was enabled status, enable it again. */
 		if (test_and_clear_bit(0, &ctx->irq_flags))
 			fimd_enable_vblank(dev);
-
-		fimd_apply(dev);
 	} else {
-		clk_disable(ctx->lcd_clk);
-		clk_disable(ctx->bus_clk);
-
+		fimd_clock(ctx, false);
 		ctx->suspended = true;
 	}
 
@@ -930,15 +971,15 @@ static int fimd_suspend(struct device *dev)
 {
 	struct fimd_context *ctx = get_fimd_context(dev);
 
-	if (pm_runtime_suspended(dev))
-		return 0;
-
 	/*
 	 * do not use pm_runtime_suspend(). if pm_runtime_suspend() is
 	 * called here, an error would be returned by that interface
 	 * because the usage_count of pm runtime is more than 1.
 	 */
-	return fimd_power_on(ctx, false);
+	if (!pm_runtime_suspended(dev))
+		return fimd_activate(ctx, false);
+
+	return 0;
 }
 
 static int fimd_resume(struct device *dev)
@@ -950,8 +991,21 @@ static int fimd_resume(struct device *dev)
 	 * of pm runtime would still be 1 so in this case, fimd driver
 	 * should be on directly not drawing on pm runtime interface.
 	 */
-	if (!pm_runtime_suspended(dev))
-		return fimd_power_on(ctx, true);
+	if (pm_runtime_suspended(dev)) {
+		int ret;
+
+		ret = fimd_activate(ctx, true);
+		if (ret < 0)
+			return ret;
+
+		/*
+		 * in case of dpms on(standby), fimd_apply function will
+		 * be called by encoder's dpms callback to update fimd's
+		 * registers but in case of sleep wakeup, it's not.
+		 * so fimd_apply function should be called at here.
+		 */
+		fimd_apply(dev);
+	}
 
 	return 0;
 }
@@ -964,7 +1018,7 @@ static int fimd_runtime_suspend(struct device *dev)
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	return fimd_power_on(ctx, false);
+	return fimd_activate(ctx, false);
 }
 
 static int fimd_runtime_resume(struct device *dev)
@@ -973,10 +1027,22 @@ static int fimd_runtime_resume(struct device *dev)
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	return fimd_power_on(ctx, true);
+	return fimd_activate(ctx, true);
 }
 #endif
 
+static struct platform_device_id fimd_driver_ids[] = {
+	{
+		.name		= "exynos4-fb",
+		.driver_data	= (unsigned long)&exynos4_fimd_driver_data,
+	}, {
+		.name		= "exynos5-fb",
+		.driver_data	= (unsigned long)&exynos5_fimd_driver_data,
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(platform, fimd_driver_ids);
+
 static const struct dev_pm_ops fimd_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(fimd_suspend, fimd_resume)
 	SET_RUNTIME_PM_OPS(fimd_runtime_suspend, fimd_runtime_resume, NULL)
@@ -985,6 +1051,7 @@ static const struct dev_pm_ops fimd_pm_ops = {
 struct platform_driver fimd_driver = {
 	.probe		= fimd_probe,
 	.remove		= __devexit_p(fimd_remove),
+	.id_table       = fimd_driver_ids,
 	.driver		= {
 		.name	= "exynos4-fb",
 		.owner	= THIS_MODULE,
diff --git a/drivers/gpu/drm/exynos/exynos_drm_g2d.c b/drivers/gpu/drm/exynos/exynos_drm_g2d.c
index bc2a2e9be8e..f7aab24ea46 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_g2d.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_g2d.c
@@ -122,6 +122,7 @@ struct g2d_runqueue_node {
 	struct list_head	list;
 	struct list_head	run_cmdlist;
 	struct list_head	event_list;
+	pid_t			pid;
 	struct completion	complete;
 	int			async;
 };
@@ -164,8 +165,7 @@ static int g2d_init_cmdlist(struct g2d_data *g2d)
 		return -ENOMEM;
 	}
 
-	node = kcalloc(G2D_CMDLIST_NUM, G2D_CMDLIST_NUM * sizeof(*node),
-			GFP_KERNEL);
+	node = kcalloc(G2D_CMDLIST_NUM, sizeof(*node), GFP_KERNEL);
 	if (!node) {
 		dev_err(dev, "failed to allocate memory\n");
 		ret = -ENOMEM;
@@ -679,6 +679,7 @@ int exynos_g2d_exec_ioctl(struct drm_device *drm_dev, void *data,
 	}
 
 	mutex_lock(&g2d->runqueue_mutex);
+	runqueue_node->pid = current->pid;
 	list_add_tail(&runqueue_node->list, &g2d->runqueue);
 	if (!g2d->runqueue_node)
 		g2d_exec_runqueue(g2d);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_gem.c b/drivers/gpu/drm/exynos/exynos_drm_gem.c
index fcdbe46914f..d2545560664 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_gem.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_gem.c
@@ -500,7 +500,7 @@ static int exynos_drm_gem_mmap_buffer(struct file *filp,
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	vma->vm_flags |= (VM_IO | VM_RESERVED);
+	vma->vm_flags |= VM_IO | VM_DONTEXPAND | VM_DONTDUMP;
 
 	update_vm_cache_attr(exynos_gem_obj, vma);
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_hdmi.c b/drivers/gpu/drm/exynos/exynos_drm_hdmi.c
index c3d3a5e4f10..c3b9e2b4518 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_hdmi.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_hdmi.c
@@ -29,6 +29,11 @@
 #define get_ctx_from_subdrv(subdrv)	container_of(subdrv,\
 					struct drm_hdmi_context, subdrv);
 
+/* Common hdmi subdrv needs to access the hdmi and mixer though context.
+* These should be initialied by the repective drivers */
+static struct exynos_drm_hdmi_context *hdmi_ctx;
+static struct exynos_drm_hdmi_context *mixer_ctx;
+
 /* these callback points shoud be set by specific drivers. */
 static struct exynos_hdmi_ops *hdmi_ops;
 static struct exynos_mixer_ops *mixer_ops;
@@ -41,6 +46,18 @@ struct drm_hdmi_context {
 	bool	enabled[MIXER_WIN_NR];
 };
 
+void exynos_hdmi_drv_attach(struct exynos_drm_hdmi_context *ctx)
+{
+	if (ctx)
+		hdmi_ctx = ctx;
+}
+
+void exynos_mixer_drv_attach(struct exynos_drm_hdmi_context *ctx)
+{
+	if (ctx)
+		mixer_ctx = ctx;
+}
+
 void exynos_hdmi_ops_register(struct exynos_hdmi_ops *ops)
 {
 	DRM_DEBUG_KMS("%s\n", __FILE__);
@@ -274,10 +291,21 @@ static void drm_mixer_disable(struct device *subdrv_dev, int zpos)
 	ctx->enabled[win] = false;
 }
 
+static void drm_mixer_wait_for_vblank(struct device *subdrv_dev)
+{
+	struct drm_hdmi_context *ctx = to_context(subdrv_dev);
+
+	DRM_DEBUG_KMS("%s\n", __FILE__);
+
+	if (mixer_ops && mixer_ops->wait_for_vblank)
+		mixer_ops->wait_for_vblank(ctx->mixer_ctx->ctx);
+}
+
 static struct exynos_drm_overlay_ops drm_hdmi_overlay_ops = {
 	.mode_set = drm_mixer_mode_set,
 	.commit = drm_mixer_commit,
 	.disable = drm_mixer_disable,
+	.wait_for_vblank = drm_mixer_wait_for_vblank,
 };
 
 static struct exynos_drm_manager hdmi_manager = {
@@ -292,46 +320,30 @@ static int hdmi_subdrv_probe(struct drm_device *drm_dev,
 {
 	struct exynos_drm_subdrv *subdrv = to_subdrv(dev);
 	struct drm_hdmi_context *ctx;
-	struct platform_device *pdev = to_platform_device(dev);
-	struct exynos_drm_common_hdmi_pd *pd;
 
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
-	pd = pdev->dev.platform_data;
-
-	if (!pd) {
-		DRM_DEBUG_KMS("platform data is null.\n");
-		return -EFAULT;
-	}
-
-	if (!pd->hdmi_dev) {
-		DRM_DEBUG_KMS("hdmi device is null.\n");
+	if (!hdmi_ctx) {
+		DRM_ERROR("hdmi context not initialized.\n");
 		return -EFAULT;
 	}
 
-	if (!pd->mixer_dev) {
-		DRM_DEBUG_KMS("mixer device is null.\n");
+	if (!mixer_ctx) {
+		DRM_ERROR("mixer context not initialized.\n");
 		return -EFAULT;
 	}
 
 	ctx = get_ctx_from_subdrv(subdrv);
 
-	ctx->hdmi_ctx = (struct exynos_drm_hdmi_context *)
-				to_context(pd->hdmi_dev);
-	if (!ctx->hdmi_ctx) {
-		DRM_DEBUG_KMS("hdmi context is null.\n");
+	if (!ctx) {
+		DRM_ERROR("no drm hdmi context.\n");
 		return -EFAULT;
 	}
 
-	ctx->hdmi_ctx->drm_dev = drm_dev;
-
-	ctx->mixer_ctx = (struct exynos_drm_hdmi_context *)
-				to_context(pd->mixer_dev);
-	if (!ctx->mixer_ctx) {
-		DRM_DEBUG_KMS("mixer context is null.\n");
-		return -EFAULT;
-	}
+	ctx->hdmi_ctx = hdmi_ctx;
+	ctx->mixer_ctx = mixer_ctx;
 
+	ctx->hdmi_ctx->drm_dev = drm_dev;
 	ctx->mixer_ctx->drm_dev = drm_dev;
 
 	return 0;
diff --git a/drivers/gpu/drm/exynos/exynos_drm_hdmi.h b/drivers/gpu/drm/exynos/exynos_drm_hdmi.h
index a91c42088e4..2da5ffd3a05 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_hdmi.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_hdmi.h
@@ -67,11 +67,14 @@ struct exynos_mixer_ops {
 	void (*dpms)(void *ctx, int mode);
 
 	/* overlay */
+	void (*wait_for_vblank)(void *ctx);
 	void (*win_mode_set)(void *ctx, struct exynos_drm_overlay *overlay);
 	void (*win_commit)(void *ctx, int zpos);
 	void (*win_disable)(void *ctx, int zpos);
 };
 
+void exynos_hdmi_drv_attach(struct exynos_drm_hdmi_context *ctx);
+void exynos_mixer_drv_attach(struct exynos_drm_hdmi_context *ctx);
 void exynos_hdmi_ops_register(struct exynos_hdmi_ops *ops);
 void exynos_mixer_ops_register(struct exynos_mixer_ops *ops);
 #endif
diff --git a/drivers/gpu/drm/exynos/exynos_drm_plane.c b/drivers/gpu/drm/exynos/exynos_drm_plane.c
index 03b472b4301..60b877a388c 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_plane.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_plane.c
@@ -32,6 +32,42 @@ static const uint32_t formats[] = {
 	DRM_FORMAT_NV12MT,
 };
 
+/*
+ * This function is to get X or Y size shown via screen. This needs length and
+ * start position of CRTC.
+ *
+ *      <--- length --->
+ * CRTC ----------------
+ *      ^ start        ^ end
+ *
+ * There are six cases from a to b.
+ *
+ *             <----- SCREEN ----->
+ *             0                 last
+ *   ----------|------------------|----------
+ * CRTCs
+ * a -------
+ *        b -------
+ *        c --------------------------
+ *                 d --------
+ *                           e -------
+ *                                  f -------
+ */
+static int exynos_plane_get_size(int start, unsigned length, unsigned last)
+{
+	int end = start + length;
+	int size = 0;
+
+	if (start <= 0) {
+		if (end > 0)
+			size = min_t(unsigned, end, last);
+	} else if (start <= last) {
+		size = min_t(unsigned, last - start, length);
+	}
+
+	return size;
+}
+
 int exynos_plane_mode_set(struct drm_plane *plane, struct drm_crtc *crtc,
 			  struct drm_framebuffer *fb, int crtc_x, int crtc_y,
 			  unsigned int crtc_w, unsigned int crtc_h,
@@ -47,7 +83,7 @@ int exynos_plane_mode_set(struct drm_plane *plane, struct drm_crtc *crtc,
 
 	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
 
-	nr = exynos_drm_format_num_buffers(fb->pixel_format);
+	nr = exynos_drm_fb_get_buf_cnt(fb);
 	for (i = 0; i < nr; i++) {
 		struct exynos_drm_gem_buf *buffer = exynos_drm_fb_buffer(fb, i);
 
@@ -64,8 +100,24 @@ int exynos_plane_mode_set(struct drm_plane *plane, struct drm_crtc *crtc,
 				(unsigned long)overlay->dma_addr[i]);
 	}
 
-	actual_w = min((unsigned)(crtc->mode.hdisplay - crtc_x), crtc_w);
-	actual_h = min((unsigned)(crtc->mode.vdisplay - crtc_y), crtc_h);
+	actual_w = exynos_plane_get_size(crtc_x, crtc_w, crtc->mode.hdisplay);
+	actual_h = exynos_plane_get_size(crtc_y, crtc_h, crtc->mode.vdisplay);
+
+	if (crtc_x < 0) {
+		if (actual_w)
+			src_x -= crtc_x;
+		else
+			src_x += crtc_w;
+		crtc_x = 0;
+	}
+
+	if (crtc_y < 0) {
+		if (actual_h)
+			src_y -= crtc_y;
+		else
+			src_y += crtc_h;
+		crtc_y = 0;
+	}
 
 	/* set drm framebuffer data. */
 	overlay->fb_x = src_x;
diff --git a/drivers/gpu/drm/exynos/exynos_drm_vidi.c b/drivers/gpu/drm/exynos/exynos_drm_vidi.c
index 8fe431ae537..e4b8a8f741f 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_vidi.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_vidi.c
@@ -56,6 +56,7 @@ struct vidi_context {
 	unsigned int			connected;
 	bool				vblank_on;
 	bool				suspended;
+	bool				direct_vblank;
 	struct work_struct		work;
 	struct mutex			lock;
 };
@@ -224,6 +225,15 @@ static int vidi_enable_vblank(struct device *dev)
 	if (!test_and_set_bit(0, &ctx->irq_flags))
 		ctx->vblank_on = true;
 
+	ctx->direct_vblank = true;
+
+	/*
+	 * in case of page flip request, vidi_finish_pageflip function
+	 * will not be called because direct_vblank is true and then
+	 * that function will be called by overlay_ops->commit callback
+	 */
+	schedule_work(&ctx->work);
+
 	return 0;
 }
 
@@ -425,7 +435,17 @@ static void vidi_fake_vblank_handler(struct work_struct *work)
 	/* refresh rate is about 50Hz. */
 	usleep_range(16000, 20000);
 
-	drm_handle_vblank(subdrv->drm_dev, manager->pipe);
+	mutex_lock(&ctx->lock);
+
+	if (ctx->direct_vblank) {
+		drm_handle_vblank(subdrv->drm_dev, manager->pipe);
+		ctx->direct_vblank = false;
+		mutex_unlock(&ctx->lock);
+		return;
+	}
+
+	mutex_unlock(&ctx->lock);
+
 	vidi_finish_pageflip(subdrv->drm_dev, manager->pipe);
 }
 
@@ -453,7 +473,7 @@ static int vidi_subdrv_probe(struct drm_device *drm_dev, struct device *dev)
 	return 0;
 }
 
-static void vidi_subdrv_remove(struct drm_device *drm_dev)
+static void vidi_subdrv_remove(struct drm_device *drm_dev, struct device *dev)
 {
 	DRM_DEBUG_KMS("%s\n", __FILE__);
 
diff --git a/drivers/gpu/drm/exynos/exynos_hdmi.c b/drivers/gpu/drm/exynos/exynos_hdmi.c
index e1c53956aa2..2c115f8a62a 100644
--- a/drivers/gpu/drm/exynos/exynos_hdmi.c
+++ b/drivers/gpu/drm/exynos/exynos_hdmi.c
@@ -32,6 +32,9 @@
 #include <linux/pm_runtime.h>
 #include <linux/clk.h>
 #include <linux/regulator/consumer.h>
+#include <linux/io.h>
+#include <linux/of_gpio.h>
+#include <plat/gpio-cfg.h>
 
 #include <drm/exynos_drm.h>
 
@@ -40,10 +43,18 @@
 
 #include "exynos_hdmi.h"
 
+#include <linux/gpio.h>
+#include <media/s5p_hdmi.h>
+
 #define MAX_WIDTH		1920
 #define MAX_HEIGHT		1080
 #define get_hdmi_context(dev)	platform_get_drvdata(to_platform_device(dev))
 
+enum hdmi_type {
+	HDMI_TYPE13,
+	HDMI_TYPE14,
+};
+
 struct hdmi_resources {
 	struct clk			*hdmi;
 	struct clk			*sclk_hdmi;
@@ -59,13 +70,12 @@ struct hdmi_context {
 	struct drm_device		*drm_dev;
 	bool				hpd;
 	bool				powered;
-	bool				is_v13;
 	bool				dvi_mode;
 	struct mutex			hdmi_mutex;
 
 	void __iomem			*regs;
-	unsigned int			external_irq;
-	unsigned int			internal_irq;
+	int				external_irq;
+	int				internal_irq;
 
 	struct i2c_client		*ddc_port;
 	struct i2c_client		*hdmiphy_port;
@@ -76,8 +86,9 @@ struct hdmi_context {
 	struct hdmi_resources		res;
 	void				*parent_ctx;
 
-	void				(*cfg_hpd)(bool external);
-	int				(*get_hpd)(void);
+	int				hpd_gpio;
+
+	enum hdmi_type			type;
 };
 
 /* HDMI Version 1.3 */
@@ -1209,7 +1220,7 @@ static void hdmi_v14_regs_dump(struct hdmi_context *hdata, char *prefix)
 
 static void hdmi_regs_dump(struct hdmi_context *hdata, char *prefix)
 {
-	if (hdata->is_v13)
+	if (hdata->type == HDMI_TYPE13)
 		hdmi_v13_regs_dump(hdata, prefix);
 	else
 		hdmi_v14_regs_dump(hdata, prefix);
@@ -1250,7 +1261,7 @@ static int hdmi_v14_conf_index(struct drm_display_mode *mode)
 static int hdmi_conf_index(struct hdmi_context *hdata,
 			   struct drm_display_mode *mode)
 {
-	if (hdata->is_v13)
+	if (hdata->type == HDMI_TYPE13)
 		return hdmi_v13_conf_index(mode);
 
 	return hdmi_v14_conf_index(mode);
@@ -1346,7 +1357,7 @@ static int hdmi_check_timing(void *ctx, void *timing)
 			check_timing->yres, check_timing->refresh,
 			check_timing->vmode);
 
-	if (hdata->is_v13)
+	if (hdata->type == HDMI_TYPE13)
 		return hdmi_v13_check_timing(check_timing);
 	else
 		return hdmi_v14_check_timing(check_timing);
@@ -1412,7 +1423,7 @@ static void hdmi_reg_acr(struct hdmi_context *hdata, u8 *acr)
 	hdmi_reg_writeb(hdata, HDMI_ACR_CTS1, acr[2]);
 	hdmi_reg_writeb(hdata, HDMI_ACR_CTS2, acr[1]);
 
-	if (hdata->is_v13)
+	if (hdata->type == HDMI_TYPE13)
 		hdmi_reg_writeb(hdata, HDMI_V13_ACR_CON, 4);
 	else
 		hdmi_reg_writeb(hdata, HDMI_ACR_CON, 4);
@@ -1516,7 +1527,7 @@ static void hdmi_conf_reset(struct hdmi_context *hdata)
 {
 	u32 reg;
 
-	if (hdata->is_v13)
+	if (hdata->type == HDMI_TYPE13)
 		reg = HDMI_V13_CORE_RSTOUT;
 	else
 		reg = HDMI_CORE_RSTOUT;
@@ -1530,12 +1541,9 @@ static void hdmi_conf_reset(struct hdmi_context *hdata)
 
 static void hdmi_conf_init(struct hdmi_context *hdata)
 {
-	/* enable HPD interrupts */
+	/* disable HPD interrupts */
 	hdmi_reg_writemask(hdata, HDMI_INTC_CON, 0, HDMI_INTC_EN_GLOBAL |
 		HDMI_INTC_EN_HPD_PLUG | HDMI_INTC_EN_HPD_UNPLUG);
-	mdelay(10);
-	hdmi_reg_writemask(hdata, HDMI_INTC_CON, ~0, HDMI_INTC_EN_GLOBAL |
-		HDMI_INTC_EN_HPD_PLUG | HDMI_INTC_EN_HPD_UNPLUG);
 
 	/* choose HDMI mode */
 	hdmi_reg_writemask(hdata, HDMI_MODE_SEL,
@@ -1551,7 +1559,7 @@ static void hdmi_conf_init(struct hdmi_context *hdata)
 				HDMI_VID_PREAMBLE_DIS | HDMI_GUARD_BAND_DIS);
 	}
 
-	if (hdata->is_v13) {
+	if (hdata->type == HDMI_TYPE13) {
 		/* choose bluescreen (fecal) color */
 		hdmi_reg_writeb(hdata, HDMI_V13_BLUE_SCREEN_0, 0x12);
 		hdmi_reg_writeb(hdata, HDMI_V13_BLUE_SCREEN_1, 0x34);
@@ -1833,7 +1841,7 @@ static void hdmi_v14_timing_apply(struct hdmi_context *hdata)
 
 static void hdmi_timing_apply(struct hdmi_context *hdata)
 {
-	if (hdata->is_v13)
+	if (hdata->type == HDMI_TYPE13)
 		hdmi_v13_timing_apply(hdata);
 	else
 		hdmi_v14_timing_apply(hdata);
@@ -1855,7 +1863,7 @@ static void hdmiphy_conf_reset(struct hdmi_context *hdata)
 	if (hdata->hdmiphy_port)
 		i2c_master_send(hdata->hdmiphy_port, buffer, 2);
 
-	if (hdata->is_v13)
+	if (hdata->type == HDMI_TYPE13)
 		reg = HDMI_V13_PHY_RSTOUT;
 	else
 		reg = HDMI_PHY_RSTOUT;
@@ -1882,7 +1890,7 @@ static void hdmiphy_conf_apply(struct hdmi_context *hdata)
 	}
 
 	/* pixel clock */
-	if (hdata->is_v13)
+	if (hdata->type == HDMI_TYPE13)
 		hdmiphy_data = hdmi_v13_confs[hdata->cur_conf].hdmiphy_data;
 	else
 		hdmiphy_data = hdmi_confs[hdata->cur_conf].hdmiphy_data;
@@ -1950,7 +1958,7 @@ static void hdmi_mode_fixup(void *ctx, struct drm_connector *connector,
 
 	drm_mode_set_crtcinfo(adjusted_mode, 0);
 
-	if (hdata->is_v13)
+	if (hdata->type == HDMI_TYPE13)
 		index = hdmi_v13_conf_index(adjusted_mode);
 	else
 		index = hdmi_v14_conf_index(adjusted_mode);
@@ -1964,7 +1972,7 @@ static void hdmi_mode_fixup(void *ctx, struct drm_connector *connector,
 	 * to adjusted_mode.
 	 */
 	list_for_each_entry(m, &connector->modes, head) {
-		if (hdata->is_v13)
+		if (hdata->type == HDMI_TYPE13)
 			index = hdmi_v13_conf_index(m);
 		else
 			index = hdmi_v14_conf_index(m);
@@ -2024,8 +2032,6 @@ static void hdmi_poweron(struct hdmi_context *hdata)
 
 	hdata->powered = true;
 
-	if (hdata->cfg_hpd)
-		hdata->cfg_hpd(true);
 	mutex_unlock(&hdata->hdmi_mutex);
 
 	pm_runtime_get_sync(hdata->dev);
@@ -2061,8 +2067,6 @@ static void hdmi_poweroff(struct hdmi_context *hdata)
 	pm_runtime_put_sync(hdata->dev);
 
 	mutex_lock(&hdata->hdmi_mutex);
-	if (hdata->cfg_hpd)
-		hdata->cfg_hpd(false);
 
 	hdata->powered = false;
 
@@ -2110,17 +2114,13 @@ static irqreturn_t hdmi_external_irq_thread(int irq, void *arg)
 	struct exynos_drm_hdmi_context *ctx = arg;
 	struct hdmi_context *hdata = ctx->ctx;
 
-	if (!hdata->get_hpd)
-		goto out;
-
 	mutex_lock(&hdata->hdmi_mutex);
-	hdata->hpd = hdata->get_hpd();
+	hdata->hpd = gpio_get_value(hdata->hpd_gpio);
 	mutex_unlock(&hdata->hdmi_mutex);
 
 	if (ctx->drm_dev)
 		drm_helper_hpd_irq_event(ctx->drm_dev);
 
-out:
 	return IRQ_HANDLED;
 }
 
@@ -2143,18 +2143,9 @@ static irqreturn_t hdmi_internal_irq_thread(int irq, void *arg)
 			HDMI_INTC_FLAG_HPD_PLUG);
 	}
 
-	mutex_lock(&hdata->hdmi_mutex);
-	hdata->hpd = hdmi_reg_read(hdata, HDMI_HPD_STATUS);
-	if (hdata->powered && hdata->hpd) {
-		mutex_unlock(&hdata->hdmi_mutex);
-		goto out;
-	}
-	mutex_unlock(&hdata->hdmi_mutex);
-
 	if (ctx->drm_dev)
 		drm_helper_hpd_irq_event(ctx->drm_dev);
 
-out:
 	return IRQ_HANDLED;
 }
 
@@ -2262,18 +2253,89 @@ void hdmi_attach_hdmiphy_client(struct i2c_client *hdmiphy)
 		hdmi_hdmiphy = hdmiphy;
 }
 
+#ifdef CONFIG_OF
+static struct s5p_hdmi_platform_data *drm_hdmi_dt_parse_pdata
+					(struct device *dev)
+{
+	struct device_node *np = dev->of_node;
+	struct s5p_hdmi_platform_data *pd;
+	enum of_gpio_flags flags;
+	u32 value;
+
+	pd = devm_kzalloc(dev, sizeof(*pd), GFP_KERNEL);
+	if (!pd) {
+		DRM_ERROR("memory allocation for pdata failed\n");
+		goto err_data;
+	}
+
+	if (!of_find_property(np, "hpd-gpio", &value)) {
+		DRM_ERROR("no hpd gpio property found\n");
+		goto err_data;
+	}
+
+	pd->hpd_gpio = of_get_named_gpio_flags(np, "hpd-gpio", 0, &flags);
+
+	return pd;
+
+err_data:
+	return NULL;
+}
+#else
+static struct s5p_hdmi_platform_data *drm_hdmi_dt_parse_pdata
+					(struct device *dev)
+{
+	return NULL;
+}
+#endif
+
+static struct platform_device_id hdmi_driver_types[] = {
+	{
+		.name		= "s5pv210-hdmi",
+		.driver_data    = HDMI_TYPE13,
+	}, {
+		.name		= "exynos4-hdmi",
+		.driver_data    = HDMI_TYPE13,
+	}, {
+		.name		= "exynos4-hdmi14",
+		.driver_data	= HDMI_TYPE14,
+	}, {
+		.name		= "exynos5-hdmi",
+		.driver_data	= HDMI_TYPE14,
+	}, {
+		/* end node */
+	}
+};
+
+static struct of_device_id hdmi_match_types[] = {
+	{
+		.compatible = "samsung,exynos5-hdmi",
+		.data	= (void	*)HDMI_TYPE14,
+	}, {
+		/* end node */
+	}
+};
+
 static int __devinit hdmi_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	struct exynos_drm_hdmi_context *drm_hdmi_ctx;
 	struct hdmi_context *hdata;
-	struct exynos_drm_hdmi_pdata *pdata;
+	struct s5p_hdmi_platform_data *pdata;
 	struct resource *res;
 	int ret;
 
 	DRM_DEBUG_KMS("[%d]\n", __LINE__);
 
-	pdata = pdev->dev.platform_data;
+	if (pdev->dev.of_node) {
+		pdata = drm_hdmi_dt_parse_pdata(dev);
+		if (IS_ERR(pdata)) {
+			DRM_ERROR("failed to parse dt\n");
+			return PTR_ERR(pdata);
+		}
+	} else {
+		pdata = pdev->dev.platform_data;
+	}
+
 	if (!pdata) {
 		DRM_ERROR("no platform data specified\n");
 		return -EINVAL;
@@ -2300,18 +2362,33 @@ static int __devinit hdmi_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, drm_hdmi_ctx);
 
-	hdata->is_v13 = pdata->is_v13;
-	hdata->cfg_hpd = pdata->cfg_hpd;
-	hdata->get_hpd = pdata->get_hpd;
+	if (dev->of_node) {
+		const struct of_device_id *match;
+		match = of_match_node(of_match_ptr(hdmi_match_types),
+					pdev->dev.of_node);
+		hdata->type = (enum hdmi_type)match->data;
+	} else {
+		hdata->type = (enum hdmi_type)platform_get_device_id
+					(pdev)->driver_data;
+	}
+
+	hdata->hpd_gpio = pdata->hpd_gpio;
 	hdata->dev = dev;
 
 	ret = hdmi_resources_init(hdata);
+
 	if (ret) {
 		ret = -EINVAL;
+		DRM_ERROR("hdmi_resources_init failed\n");
 		goto err_data;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		DRM_ERROR("failed to find registers\n");
+		ret = -ENOENT;
+		goto err_resource;
+	}
 
 	hdata->regs = devm_request_and_ioremap(&pdev->dev, res);
 	if (!hdata->regs) {
@@ -2320,11 +2397,17 @@ static int __devinit hdmi_probe(struct platform_device *pdev)
 		goto err_resource;
 	}
 
+	ret = gpio_request(hdata->hpd_gpio, "HPD");
+	if (ret) {
+		DRM_ERROR("failed to request HPD gpio\n");
+		goto err_resource;
+	}
+
 	/* DDC i2c driver */
 	if (i2c_add_driver(&ddc_driver)) {
 		DRM_ERROR("failed to register ddc i2c driver\n");
 		ret = -ENOENT;
-		goto err_resource;
+		goto err_gpio;
 	}
 
 	hdata->ddc_port = hdmi_ddc;
@@ -2338,32 +2421,31 @@ static int __devinit hdmi_probe(struct platform_device *pdev)
 
 	hdata->hdmiphy_port = hdmi_hdmiphy;
 
-	hdata->external_irq = platform_get_irq_byname(pdev, "external_irq");
+	hdata->external_irq = gpio_to_irq(hdata->hpd_gpio);
 	if (hdata->external_irq < 0) {
-		DRM_ERROR("failed to get platform irq\n");
+		DRM_ERROR("failed to get GPIO external irq\n");
 		ret = hdata->external_irq;
 		goto err_hdmiphy;
 	}
 
-	hdata->internal_irq = platform_get_irq_byname(pdev, "internal_irq");
+	hdata->internal_irq = platform_get_irq(pdev, 0);
 	if (hdata->internal_irq < 0) {
 		DRM_ERROR("failed to get platform internal irq\n");
 		ret = hdata->internal_irq;
 		goto err_hdmiphy;
 	}
 
+	hdata->hpd = gpio_get_value(hdata->hpd_gpio);
+
 	ret = request_threaded_irq(hdata->external_irq, NULL,
 			hdmi_external_irq_thread, IRQF_TRIGGER_RISING |
 			IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
 			"hdmi_external", drm_hdmi_ctx);
 	if (ret) {
-		DRM_ERROR("failed to register hdmi internal interrupt\n");
+		DRM_ERROR("failed to register hdmi external interrupt\n");
 		goto err_hdmiphy;
 	}
 
-	if (hdata->cfg_hpd)
-		hdata->cfg_hpd(false);
-
 	ret = request_threaded_irq(hdata->internal_irq, NULL,
 			hdmi_internal_irq_thread, IRQF_ONESHOT,
 			"hdmi_internal", drm_hdmi_ctx);
@@ -2372,6 +2454,9 @@ static int __devinit hdmi_probe(struct platform_device *pdev)
 		goto err_free_irq;
 	}
 
+	/* Attach HDMI Driver to common hdmi. */
+	exynos_hdmi_drv_attach(drm_hdmi_ctx);
+
 	/* register specific callbacks to common hdmi. */
 	exynos_hdmi_ops_register(&hdmi_ops);
 
@@ -2385,6 +2470,8 @@ err_hdmiphy:
 	i2c_del_driver(&hdmiphy_driver);
 err_ddc:
 	i2c_del_driver(&ddc_driver);
+err_gpio:
+	gpio_free(hdata->hpd_gpio);
 err_resource:
 	hdmi_resources_cleanup(hdata);
 err_data:
@@ -2402,6 +2489,9 @@ static int __devexit hdmi_remove(struct platform_device *pdev)
 	pm_runtime_disable(dev);
 
 	free_irq(hdata->internal_irq, hdata);
+	free_irq(hdata->external_irq, hdata);
+
+	gpio_free(hdata->hpd_gpio);
 
 	hdmi_resources_cleanup(hdata);
 
@@ -2447,9 +2537,11 @@ static SIMPLE_DEV_PM_OPS(hdmi_pm_ops, hdmi_suspend, hdmi_resume);
 struct platform_driver hdmi_driver = {
 	.probe		= hdmi_probe,
 	.remove		= __devexit_p(hdmi_remove),
+	.id_table = hdmi_driver_types,
 	.driver		= {
-		.name	= "exynos4-hdmi",
+		.name	= "exynos-hdmi",
 		.owner	= THIS_MODULE,
 		.pm	= &hdmi_pm_ops,
+		.of_match_table = hdmi_match_types,
 	},
 };
diff --git a/drivers/gpu/drm/exynos/exynos_hdmiphy.c b/drivers/gpu/drm/exynos/exynos_hdmiphy.c
index 0a8162b7de3..27d1720f1bb 100644
--- a/drivers/gpu/drm/exynos/exynos_hdmiphy.c
+++ b/drivers/gpu/drm/exynos/exynos_hdmiphy.c
@@ -42,13 +42,23 @@ static int hdmiphy_remove(struct i2c_client *client)
 
 static const struct i2c_device_id hdmiphy_id[] = {
 	{ "s5p_hdmiphy", 0 },
+	{ "exynos5-hdmiphy", 0 },
 	{ },
 };
 
+static struct of_device_id hdmiphy_match_types[] = {
+	{
+		.compatible = "samsung,exynos5-hdmiphy",
+	}, {
+		/* end node */
+	}
+};
+
 struct i2c_driver hdmiphy_driver = {
 	.driver = {
-		.name	= "s5p-hdmiphy",
+		.name	= "exynos-hdmiphy",
 		.owner	= THIS_MODULE,
+		.of_match_table = hdmiphy_match_types,
 	},
 	.id_table = hdmiphy_id,
 	.probe		= hdmiphy_probe,
diff --git a/drivers/gpu/drm/exynos/exynos_mixer.c b/drivers/gpu/drm/exynos/exynos_mixer.c
index e6098f247a5..614b2e9ac46 100644
--- a/drivers/gpu/drm/exynos/exynos_mixer.c
+++ b/drivers/gpu/drm/exynos/exynos_mixer.c
@@ -73,16 +73,28 @@ struct mixer_resources {
 	struct clk		*sclk_dac;
 };
 
+enum mixer_version_id {
+	MXR_VER_0_0_0_16,
+	MXR_VER_16_0_33_0,
+};
+
 struct mixer_context {
 	struct device		*dev;
 	int			pipe;
 	bool			interlace;
 	bool			powered;
+	bool			vp_enabled;
 	u32			int_en;
 
 	struct mutex		mixer_mutex;
 	struct mixer_resources	mixer_res;
 	struct hdmi_win_data	win_data[MIXER_WIN_NR];
+	enum mixer_version_id	mxr_ver;
+};
+
+struct mixer_drv_data {
+	enum mixer_version_id	version;
+	bool					is_vp_enabled;
 };
 
 static const u8 filter_y_horiz_tap8[] = {
@@ -251,7 +263,8 @@ static void mixer_vsync_set_update(struct mixer_context *ctx, bool enable)
 	mixer_reg_writemask(res, MXR_STATUS, enable ?
 			MXR_STATUS_SYNC_ENABLE : 0, MXR_STATUS_SYNC_ENABLE);
 
-	vp_reg_write(res, VP_SHADOW_UPDATE, enable ?
+	if (ctx->vp_enabled)
+		vp_reg_write(res, VP_SHADOW_UPDATE, enable ?
 			VP_SHADOW_UPDATE_ENABLE : 0);
 }
 
@@ -333,8 +346,11 @@ static void mixer_cfg_layer(struct mixer_context *ctx, int win, bool enable)
 		mixer_reg_writemask(res, MXR_CFG, val, MXR_CFG_GRP1_ENABLE);
 		break;
 	case 2:
-		vp_reg_writemask(res, VP_ENABLE, val, VP_ENABLE_ON);
-		mixer_reg_writemask(res, MXR_CFG, val, MXR_CFG_VP_ENABLE);
+		if (ctx->vp_enabled) {
+			vp_reg_writemask(res, VP_ENABLE, val, VP_ENABLE_ON);
+			mixer_reg_writemask(res, MXR_CFG, val,
+				MXR_CFG_VP_ENABLE);
+		}
 		break;
 	}
 }
@@ -465,6 +481,18 @@ static void vp_video_buffer(struct mixer_context *ctx, int win)
 	vp_regs_dump(ctx);
 }
 
+static void mixer_layer_update(struct mixer_context *ctx)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+	u32 val;
+
+	val = mixer_reg_read(res, MXR_CFG);
+
+	/* allow one update per vsync only */
+	if (!(val & MXR_CFG_LAYER_UPDATE_COUNT_MASK))
+		mixer_reg_writemask(res, MXR_CFG, ~0, MXR_CFG_LAYER_UPDATE);
+}
+
 static void mixer_graph_buffer(struct mixer_context *ctx, int win)
 {
 	struct mixer_resources *res = &ctx->mixer_res;
@@ -545,6 +573,11 @@ static void mixer_graph_buffer(struct mixer_context *ctx, int win)
 	mixer_cfg_scan(ctx, win_data->mode_height);
 	mixer_cfg_rgb_fmt(ctx, win_data->mode_height);
 	mixer_cfg_layer(ctx, win, true);
+
+	/* layer update mandatory for mixer 16.0.33.0 */
+	if (ctx->mxr_ver == MXR_VER_16_0_33_0)
+		mixer_layer_update(ctx);
+
 	mixer_run(ctx);
 
 	mixer_vsync_set_update(ctx, true);
@@ -592,7 +625,8 @@ static void mixer_win_reset(struct mixer_context *ctx)
 	 */
 	val = MXR_LAYER_CFG_GRP1_VAL(3);
 	val |= MXR_LAYER_CFG_GRP0_VAL(2);
-	val |= MXR_LAYER_CFG_VP_VAL(1);
+	if (ctx->vp_enabled)
+		val |= MXR_LAYER_CFG_VP_VAL(1);
 	mixer_reg_write(res, MXR_LAYER_CFG, val);
 
 	/* setting background color */
@@ -615,14 +649,17 @@ static void mixer_win_reset(struct mixer_context *ctx)
 	val = MXR_GRP_CFG_ALPHA_VAL(0);
 	mixer_reg_write(res, MXR_VIDEO_CFG, val);
 
-	/* configuration of Video Processor Registers */
-	vp_win_reset(ctx);
-	vp_default_filter(res);
+	if (ctx->vp_enabled) {
+		/* configuration of Video Processor Registers */
+		vp_win_reset(ctx);
+		vp_default_filter(res);
+	}
 
 	/* disable all layers */
 	mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_GRP0_ENABLE);
 	mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_GRP1_ENABLE);
-	mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_VP_ENABLE);
+	if (ctx->vp_enabled)
+		mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_VP_ENABLE);
 
 	mixer_vsync_set_update(ctx, true);
 	spin_unlock_irqrestore(&res->reg_slock, flags);
@@ -645,8 +682,10 @@ static void mixer_poweron(struct mixer_context *ctx)
 	pm_runtime_get_sync(ctx->dev);
 
 	clk_enable(res->mixer);
-	clk_enable(res->vp);
-	clk_enable(res->sclk_mixer);
+	if (ctx->vp_enabled) {
+		clk_enable(res->vp);
+		clk_enable(res->sclk_mixer);
+	}
 
 	mixer_reg_write(res, MXR_INT_EN, ctx->int_en);
 	mixer_win_reset(ctx);
@@ -666,8 +705,10 @@ static void mixer_poweroff(struct mixer_context *ctx)
 	ctx->int_en = mixer_reg_read(res, MXR_INT_EN);
 
 	clk_disable(res->mixer);
-	clk_disable(res->vp);
-	clk_disable(res->sclk_mixer);
+	if (ctx->vp_enabled) {
+		clk_disable(res->vp);
+		clk_disable(res->sclk_mixer);
+	}
 
 	pm_runtime_put_sync(ctx->dev);
 
@@ -726,6 +767,18 @@ static void mixer_dpms(void *ctx, int mode)
 	}
 }
 
+static void mixer_wait_for_vblank(void *ctx)
+{
+	struct mixer_context *mixer_ctx = ctx;
+	struct mixer_resources *res = &mixer_ctx->mixer_res;
+	int ret;
+
+	ret = wait_for((mixer_reg_read(res, MXR_INT_STATUS) &
+				MXR_INT_STATUS_VSYNC), 50);
+	if (ret < 0)
+		DRM_DEBUG_KMS("vblank wait timed out.\n");
+}
+
 static void mixer_win_mode_set(void *ctx,
 			      struct exynos_drm_overlay *overlay)
 {
@@ -788,7 +841,7 @@ static void mixer_win_commit(void *ctx, int win)
 
 	DRM_DEBUG_KMS("[%d] %s, win: %d\n", __LINE__, __func__, win);
 
-	if (win > 1)
+	if (win > 1 && mixer_ctx->vp_enabled)
 		vp_video_buffer(mixer_ctx, win);
 	else
 		mixer_graph_buffer(mixer_ctx, win);
@@ -818,6 +871,7 @@ static struct exynos_mixer_ops mixer_ops = {
 	.dpms			= mixer_dpms,
 
 	/* overlay */
+	.wait_for_vblank	= mixer_wait_for_vblank,
 	.win_mode_set		= mixer_win_mode_set,
 	.win_commit		= mixer_win_commit,
 	.win_disable		= mixer_win_disable,
@@ -923,39 +977,20 @@ static int __devinit mixer_resources_init(struct exynos_drm_hdmi_context *ctx,
 		ret = -ENODEV;
 		goto fail;
 	}
-	mixer_res->vp = clk_get(dev, "vp");
-	if (IS_ERR_OR_NULL(mixer_res->vp)) {
-		dev_err(dev, "failed to get clock 'vp'\n");
-		ret = -ENODEV;
-		goto fail;
-	}
-	mixer_res->sclk_mixer = clk_get(dev, "sclk_mixer");
-	if (IS_ERR_OR_NULL(mixer_res->sclk_mixer)) {
-		dev_err(dev, "failed to get clock 'sclk_mixer'\n");
-		ret = -ENODEV;
-		goto fail;
-	}
+
 	mixer_res->sclk_hdmi = clk_get(dev, "sclk_hdmi");
 	if (IS_ERR_OR_NULL(mixer_res->sclk_hdmi)) {
 		dev_err(dev, "failed to get clock 'sclk_hdmi'\n");
 		ret = -ENODEV;
 		goto fail;
 	}
-	mixer_res->sclk_dac = clk_get(dev, "sclk_dac");
-	if (IS_ERR_OR_NULL(mixer_res->sclk_dac)) {
-		dev_err(dev, "failed to get clock 'sclk_dac'\n");
-		ret = -ENODEV;
-		goto fail;
-	}
-	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "mxr");
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (res == NULL) {
 		dev_err(dev, "get memory resource failed.\n");
 		ret = -ENXIO;
 		goto fail;
 	}
 
-	clk_set_parent(mixer_res->sclk_mixer, mixer_res->sclk_hdmi);
-
 	mixer_res->mixer_regs = devm_ioremap(&pdev->dev, res->start,
 							resource_size(res));
 	if (mixer_res->mixer_regs == NULL) {
@@ -964,57 +999,126 @@ static int __devinit mixer_resources_init(struct exynos_drm_hdmi_context *ctx,
 		goto fail;
 	}
 
-	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "vp");
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (res == NULL) {
-		dev_err(dev, "get memory resource failed.\n");
+		dev_err(dev, "get interrupt resource failed.\n");
 		ret = -ENXIO;
 		goto fail;
 	}
 
-	mixer_res->vp_regs = devm_ioremap(&pdev->dev, res->start,
-							resource_size(res));
-	if (mixer_res->vp_regs == NULL) {
-		dev_err(dev, "register mapping failed.\n");
-		ret = -ENXIO;
+	ret = devm_request_irq(&pdev->dev, res->start, mixer_irq_handler,
+							0, "drm_mixer", ctx);
+	if (ret) {
+		dev_err(dev, "request interrupt failed.\n");
 		goto fail;
 	}
+	mixer_res->irq = res->start;
+
+	return 0;
 
-	res = platform_get_resource_byname(pdev, IORESOURCE_IRQ, "irq");
+fail:
+	if (!IS_ERR_OR_NULL(mixer_res->sclk_hdmi))
+		clk_put(mixer_res->sclk_hdmi);
+	if (!IS_ERR_OR_NULL(mixer_res->mixer))
+		clk_put(mixer_res->mixer);
+	return ret;
+}
+
+static int __devinit vp_resources_init(struct exynos_drm_hdmi_context *ctx,
+				 struct platform_device *pdev)
+{
+	struct mixer_context *mixer_ctx = ctx->ctx;
+	struct device *dev = &pdev->dev;
+	struct mixer_resources *mixer_res = &mixer_ctx->mixer_res;
+	struct resource *res;
+	int ret;
+
+	mixer_res->vp = clk_get(dev, "vp");
+	if (IS_ERR_OR_NULL(mixer_res->vp)) {
+		dev_err(dev, "failed to get clock 'vp'\n");
+		ret = -ENODEV;
+		goto fail;
+	}
+	mixer_res->sclk_mixer = clk_get(dev, "sclk_mixer");
+	if (IS_ERR_OR_NULL(mixer_res->sclk_mixer)) {
+		dev_err(dev, "failed to get clock 'sclk_mixer'\n");
+		ret = -ENODEV;
+		goto fail;
+	}
+	mixer_res->sclk_dac = clk_get(dev, "sclk_dac");
+	if (IS_ERR_OR_NULL(mixer_res->sclk_dac)) {
+		dev_err(dev, "failed to get clock 'sclk_dac'\n");
+		ret = -ENODEV;
+		goto fail;
+	}
+
+	if (mixer_res->sclk_hdmi)
+		clk_set_parent(mixer_res->sclk_mixer, mixer_res->sclk_hdmi);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
 	if (res == NULL) {
-		dev_err(dev, "get interrupt resource failed.\n");
+		dev_err(dev, "get memory resource failed.\n");
 		ret = -ENXIO;
 		goto fail;
 	}
 
-	ret = devm_request_irq(&pdev->dev, res->start, mixer_irq_handler,
-							0, "drm_mixer", ctx);
-	if (ret) {
-		dev_err(dev, "request interrupt failed.\n");
+	mixer_res->vp_regs = devm_ioremap(&pdev->dev, res->start,
+							resource_size(res));
+	if (mixer_res->vp_regs == NULL) {
+		dev_err(dev, "register mapping failed.\n");
+		ret = -ENXIO;
 		goto fail;
 	}
-	mixer_res->irq = res->start;
 
 	return 0;
 
 fail:
 	if (!IS_ERR_OR_NULL(mixer_res->sclk_dac))
 		clk_put(mixer_res->sclk_dac);
-	if (!IS_ERR_OR_NULL(mixer_res->sclk_hdmi))
-		clk_put(mixer_res->sclk_hdmi);
 	if (!IS_ERR_OR_NULL(mixer_res->sclk_mixer))
 		clk_put(mixer_res->sclk_mixer);
 	if (!IS_ERR_OR_NULL(mixer_res->vp))
 		clk_put(mixer_res->vp);
-	if (!IS_ERR_OR_NULL(mixer_res->mixer))
-		clk_put(mixer_res->mixer);
 	return ret;
 }
 
+static struct mixer_drv_data exynos5_mxr_drv_data = {
+	.version = MXR_VER_16_0_33_0,
+	.is_vp_enabled = 0,
+};
+
+static struct mixer_drv_data exynos4_mxr_drv_data = {
+	.version = MXR_VER_0_0_0_16,
+	.is_vp_enabled = 1,
+};
+
+static struct platform_device_id mixer_driver_types[] = {
+	{
+		.name		= "s5p-mixer",
+		.driver_data	= (unsigned long)&exynos4_mxr_drv_data,
+	}, {
+		.name		= "exynos5-mixer",
+		.driver_data	= (unsigned long)&exynos5_mxr_drv_data,
+	}, {
+		/* end node */
+	}
+};
+
+static struct of_device_id mixer_match_types[] = {
+	{
+		.compatible = "samsung,exynos5-mixer",
+		.data	= &exynos5_mxr_drv_data,
+	}, {
+		/* end node */
+	}
+};
+
 static int __devinit mixer_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	struct exynos_drm_hdmi_context *drm_hdmi_ctx;
 	struct mixer_context *ctx;
+	struct mixer_drv_data *drv;
 	int ret;
 
 	dev_info(dev, "probe start\n");
@@ -1034,15 +1138,41 @@ static int __devinit mixer_probe(struct platform_device *pdev)
 
 	mutex_init(&ctx->mixer_mutex);
 
+	if (dev->of_node) {
+		const struct of_device_id *match;
+		match = of_match_node(of_match_ptr(mixer_match_types),
+							  pdev->dev.of_node);
+		drv = match->data;
+	} else {
+		drv = (struct mixer_drv_data *)
+			platform_get_device_id(pdev)->driver_data;
+	}
+
 	ctx->dev = &pdev->dev;
 	drm_hdmi_ctx->ctx = (void *)ctx;
+	ctx->vp_enabled = drv->is_vp_enabled;
+	ctx->mxr_ver = drv->version;
 
 	platform_set_drvdata(pdev, drm_hdmi_ctx);
 
 	/* acquire resources: regs, irqs, clocks */
 	ret = mixer_resources_init(drm_hdmi_ctx, pdev);
-	if (ret)
+	if (ret) {
+		DRM_ERROR("mixer_resources_init failed\n");
 		goto fail;
+	}
+
+	if (ctx->vp_enabled) {
+		/* acquire vp resources: regs, irqs, clocks */
+		ret = vp_resources_init(drm_hdmi_ctx, pdev);
+		if (ret) {
+			DRM_ERROR("vp_resources_init failed\n");
+			goto fail;
+		}
+	}
+
+	/* attach mixer driver to common hdmi. */
+	exynos_mixer_drv_attach(drm_hdmi_ctx);
 
 	/* register specific callback point to common hdmi. */
 	exynos_mixer_ops_register(&mixer_ops);
@@ -1082,10 +1212,12 @@ static SIMPLE_DEV_PM_OPS(mixer_pm_ops, mixer_suspend, NULL);
 
 struct platform_driver mixer_driver = {
 	.driver = {
-		.name = "s5p-mixer",
+		.name = "exynos-mixer",
 		.owner = THIS_MODULE,
 		.pm = &mixer_pm_ops,
+		.of_match_table = mixer_match_types,
 	},
 	.probe = mixer_probe,
 	.remove = __devexit_p(mixer_remove),
+	.id_table	= mixer_driver_types,
 };
diff --git a/drivers/gpu/drm/exynos/regs-mixer.h b/drivers/gpu/drm/exynos/regs-mixer.h
index fd2f4d14cf6..5d8dbc0301e 100644
--- a/drivers/gpu/drm/exynos/regs-mixer.h
+++ b/drivers/gpu/drm/exynos/regs-mixer.h
@@ -69,6 +69,7 @@
 	(((val) << (low_bit)) & MXR_MASK(high_bit, low_bit))
 
 /* bits for MXR_STATUS */
+#define MXR_STATUS_SOFT_RESET		(1 << 8)
 #define MXR_STATUS_16_BURST		(1 << 7)
 #define MXR_STATUS_BURST_MASK		(1 << 7)
 #define MXR_STATUS_BIG_ENDIAN		(1 << 3)
@@ -77,6 +78,8 @@
 #define MXR_STATUS_REG_RUN		(1 << 0)
 
 /* bits for MXR_CFG */
+#define MXR_CFG_LAYER_UPDATE		(1 << 31)
+#define MXR_CFG_LAYER_UPDATE_COUNT_MASK (3 << 29)
 #define MXR_CFG_RGB601_0_255		(0 << 9)
 #define MXR_CFG_RGB601_16_235		(1 << 9)
 #define MXR_CFG_RGB709_0_255		(2 << 9)
diff --git a/drivers/gpu/drm/gma500/framebuffer.c b/drivers/gpu/drm/gma500/framebuffer.c
index 884ba73ac6c..afded54dbb1 100644
--- a/drivers/gpu/drm/gma500/framebuffer.c
+++ b/drivers/gpu/drm/gma500/framebuffer.c
@@ -178,8 +178,7 @@ static int psbfb_mmap(struct fb_info *info, struct vm_area_struct *vma)
 	 */
 	vma->vm_ops = &psbfb_vm_ops;
 	vma->vm_private_data = (void *)psbfb;
-	vma->vm_flags |= VM_RESERVED | VM_IO |
-					VM_MIXEDMAP | VM_DONTEXPAND;
+	vma->vm_flags |= VM_IO | VM_MIXEDMAP | VM_DONTEXPAND | VM_DONTDUMP;
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index e957f3740f6..19dbdd7dd56 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -1399,10 +1399,16 @@ out:
 	case 0:
 	case -ERESTARTSYS:
 	case -EINTR:
+	case -EBUSY:
+		/*
+		 * EBUSY is ok: this just means that another thread
+		 * already did the job.
+		 */
 		return VM_FAULT_NOPAGE;
 	case -ENOMEM:
 		return VM_FAULT_OOM;
 	default:
+		WARN_ON_ONCE(ret);
 		return VM_FAULT_SIGBUS;
 	}
 }
@@ -3217,10 +3223,6 @@ int i915_gem_set_caching_ioctl(struct drm_device *dev, void *data,
 	enum i915_cache_level level;
 	int ret;
 
-	ret = i915_mutex_lock_interruptible(dev);
-	if (ret)
-		return ret;
-
 	switch (args->caching) {
 	case I915_CACHING_NONE:
 		level = I915_CACHE_NONE;
@@ -3232,6 +3234,10 @@ int i915_gem_set_caching_ioctl(struct drm_device *dev, void *data,
 		return -EINVAL;
 	}
 
+	ret = i915_mutex_lock_interruptible(dev);
+	if (ret)
+		return ret;
+
 	obj = to_intel_bo(drm_gem_object_lookup(dev, file, args->handle));
 	if (&obj->base == NULL) {
 		ret = -ENOENT;
diff --git a/drivers/gpu/drm/i915/i915_gem_context.c b/drivers/gpu/drm/i915/i915_gem_context.c
index 1eb48faf741..05ed42f203d 100644
--- a/drivers/gpu/drm/i915/i915_gem_context.c
+++ b/drivers/gpu/drm/i915/i915_gem_context.c
@@ -328,7 +328,7 @@ mi_set_context(struct intel_ring_buffer *ring,
 	 * itlb_before_ctx_switch.
 	 */
 	if (IS_GEN6(ring->dev) && ring->itlb_before_ctx_switch) {
-		ret = ring->flush(ring, 0, 0);
+		ret = ring->flush(ring, I915_GEM_GPU_DOMAINS, 0);
 		if (ret)
 			return ret;
 	}
diff --git a/drivers/gpu/drm/i915/i915_gem_tiling.c b/drivers/gpu/drm/i915/i915_gem_tiling.c
index 3208650a235..cedbfd7b3df 100644
--- a/drivers/gpu/drm/i915/i915_gem_tiling.c
+++ b/drivers/gpu/drm/i915/i915_gem_tiling.c
@@ -91,7 +91,10 @@ i915_gem_detect_bit_6_swizzle(struct drm_device *dev)
 	uint32_t swizzle_x = I915_BIT_6_SWIZZLE_UNKNOWN;
 	uint32_t swizzle_y = I915_BIT_6_SWIZZLE_UNKNOWN;
 
-	if (INTEL_INFO(dev)->gen >= 6) {
+	if (IS_VALLEYVIEW(dev)) {
+		swizzle_x = I915_BIT_6_SWIZZLE_NONE;
+		swizzle_y = I915_BIT_6_SWIZZLE_NONE;
+	} else if (INTEL_INFO(dev)->gen >= 6) {
 		uint32_t dimm_c0, dimm_c1;
 		dimm_c0 = I915_READ(MAD_DIMM_C0);
 		dimm_c1 = I915_READ(MAD_DIMM_C1);
diff --git a/drivers/gpu/drm/i915/i915_irq.c b/drivers/gpu/drm/i915/i915_irq.c
index 4e9888388c0..32e1bda865b 100644
--- a/drivers/gpu/drm/i915/i915_irq.c
+++ b/drivers/gpu/drm/i915/i915_irq.c
@@ -697,12 +697,12 @@ static irqreturn_t ivybridge_irq_handler(DRM_IRQ_ARGS)
 			intel_opregion_gse_intr(dev);
 
 		for (i = 0; i < 3; i++) {
+			if (de_iir & (DE_PIPEA_VBLANK_IVB << (5 * i)))
+				drm_handle_vblank(dev, i);
 			if (de_iir & (DE_PLANEA_FLIP_DONE_IVB << (5 * i))) {
 				intel_prepare_page_flip(dev, i);
 				intel_finish_page_flip_plane(dev, i);
 			}
-			if (de_iir & (DE_PIPEA_VBLANK_IVB << (5 * i)))
-				drm_handle_vblank(dev, i);
 		}
 
 		/* check event from PCH */
@@ -784,6 +784,12 @@ static irqreturn_t ironlake_irq_handler(DRM_IRQ_ARGS)
 	if (de_iir & DE_GSE)
 		intel_opregion_gse_intr(dev);
 
+	if (de_iir & DE_PIPEA_VBLANK)
+		drm_handle_vblank(dev, 0);
+
+	if (de_iir & DE_PIPEB_VBLANK)
+		drm_handle_vblank(dev, 1);
+
 	if (de_iir & DE_PLANEA_FLIP_DONE) {
 		intel_prepare_page_flip(dev, 0);
 		intel_finish_page_flip_plane(dev, 0);
@@ -794,12 +800,6 @@ static irqreturn_t ironlake_irq_handler(DRM_IRQ_ARGS)
 		intel_finish_page_flip_plane(dev, 1);
 	}
 
-	if (de_iir & DE_PIPEA_VBLANK)
-		drm_handle_vblank(dev, 0);
-
-	if (de_iir & DE_PIPEB_VBLANK)
-		drm_handle_vblank(dev, 1);
-
 	/* check event from PCH */
 	if (de_iir & DE_PCH_EVENT) {
 		if (pch_iir & hotplug_mask)
diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index 7637824c6a7..64c1be0a9cf 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -527,6 +527,9 @@
 # define VS_TIMER_DISPATCH				(1 << 6)
 # define MI_FLUSH_ENABLE				(1 << 12)
 
+#define GEN6_GT_MODE	0x20d0
+#define   GEN6_GT_MODE_HI	(1 << 9)
+
 #define GFX_MODE	0x02520
 #define GFX_MODE_GEN7	0x0229c
 #define RING_MODE_GEN7(ring)	((ring)->mmio_base+0x29c)
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index e3c02655d36..2b6ce9b2674 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -2806,13 +2806,34 @@ static void ironlake_fdi_disable(struct drm_crtc *crtc)
 	udelay(100);
 }
 
+static bool intel_crtc_has_pending_flip(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	unsigned long flags;
+	bool pending;
+
+	if (atomic_read(&dev_priv->mm.wedged))
+		return false;
+
+	spin_lock_irqsave(&dev->event_lock, flags);
+	pending = to_intel_crtc(crtc)->unpin_work != NULL;
+	spin_unlock_irqrestore(&dev->event_lock, flags);
+
+	return pending;
+}
+
 static void intel_crtc_wait_for_pending_flips(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
 
 	if (crtc->fb == NULL)
 		return;
 
+	wait_event(dev_priv->pending_flip_queue,
+		   !intel_crtc_has_pending_flip(crtc));
+
 	mutex_lock(&dev->struct_mutex);
 	intel_finish_fb(crtc->fb);
 	mutex_unlock(&dev->struct_mutex);
@@ -4370,7 +4391,7 @@ static int i9xx_crtc_mode_set(struct drm_crtc *crtc,
 	/* default to 8bpc */
 	pipeconf &= ~(PIPECONF_BPP_MASK | PIPECONF_DITHER_EN);
 	if (is_dp) {
-		if (mode->private_flags & INTEL_MODE_DP_FORCE_6BPC) {
+		if (adjusted_mode->private_flags & INTEL_MODE_DP_FORCE_6BPC) {
 			pipeconf |= PIPECONF_BPP_6 |
 				    PIPECONF_DITHER_EN |
 				    PIPECONF_DITHER_TYPE_SP;
@@ -4802,7 +4823,8 @@ static int ironlake_crtc_mode_set(struct drm_crtc *crtc,
 		target_clock = adjusted_mode->clock;
 
 	/* determine panel color depth */
-	dither = intel_choose_pipe_bpp_dither(crtc, fb, &pipe_bpp, mode);
+	dither = intel_choose_pipe_bpp_dither(crtc, fb, &pipe_bpp,
+					      adjusted_mode);
 	if (is_lvds && dev_priv->lvds_dither)
 		dither = true;
 
@@ -6159,15 +6181,13 @@ static void do_intel_finish_page_flip(struct drm_device *dev,
 	struct intel_unpin_work *work;
 	struct drm_i915_gem_object *obj;
 	struct drm_pending_vblank_event *e;
-	struct timeval tnow, tvbl;
+	struct timeval tvbl;
 	unsigned long flags;
 
 	/* Ignore early vblank irqs */
 	if (intel_crtc == NULL)
 		return;
 
-	do_gettimeofday(&tnow);
-
 	spin_lock_irqsave(&dev->event_lock, flags);
 	work = intel_crtc->unpin_work;
 	if (work == NULL || !work->pending) {
@@ -6181,25 +6201,6 @@ static void do_intel_finish_page_flip(struct drm_device *dev,
 		e = work->event;
 		e->event.sequence = drm_vblank_count_and_time(dev, intel_crtc->pipe, &tvbl);
 
-		/* Called before vblank count and timestamps have
-		 * been updated for the vblank interval of flip
-		 * completion? Need to increment vblank count and
-		 * add one videorefresh duration to returned timestamp
-		 * to account for this. We assume this happened if we
-		 * get called over 0.9 frame durations after the last
-		 * timestamped vblank.
-		 *
-		 * This calculation can not be used with vrefresh rates
-		 * below 5Hz (10Hz to be on the safe side) without
-		 * promoting to 64 integers.
-		 */
-		if (10 * (timeval_to_ns(&tnow) - timeval_to_ns(&tvbl)) >
-		    9 * crtc->framedur_ns) {
-			e->event.sequence++;
-			tvbl = ns_to_timeval(timeval_to_ns(&tvbl) +
-					     crtc->framedur_ns);
-		}
-
 		e->event.tv_sec = tvbl.tv_sec;
 		e->event.tv_usec = tvbl.tv_usec;
 
@@ -6216,9 +6217,8 @@ static void do_intel_finish_page_flip(struct drm_device *dev,
 
 	atomic_clear_mask(1 << intel_crtc->plane,
 			  &obj->pending_flip.counter);
-	if (atomic_read(&obj->pending_flip) == 0)
-		wake_up(&dev_priv->pending_flip_queue);
 
+	wake_up(&dev_priv->pending_flip_queue);
 	schedule_work(&work->work);
 
 	trace_i915_flip_complete(intel_crtc->plane, work->pending_flip_obj);
diff --git a/drivers/gpu/drm/i915/intel_dp.c b/drivers/gpu/drm/i915/intel_dp.c
index 6c8746c030c..d1e8ddb2d6c 100644
--- a/drivers/gpu/drm/i915/intel_dp.c
+++ b/drivers/gpu/drm/i915/intel_dp.c
@@ -36,6 +36,7 @@
 #include <drm/i915_drm.h>
 #include "i915_drv.h"
 
+#define DP_RECEIVER_CAP_SIZE	0xf
 #define DP_LINK_STATUS_SIZE	6
 #define DP_LINK_CHECK_TIMEOUT	(10 * 1000)
 
@@ -1796,8 +1797,7 @@ intel_dp_start_link_train(struct intel_dp *intel_dp)
 			if ((intel_dp->train_set[i] & DP_TRAIN_MAX_SWING_REACHED) == 0)
 				break;
 		if (i == intel_dp->lane_count && voltage_tries == 5) {
-			++loop_tries;
-			if (loop_tries == 5) {
+			if (++loop_tries == 5) {
 				DRM_DEBUG_KMS("too many full retries, give up\n");
 				break;
 			}
@@ -1807,15 +1807,11 @@ intel_dp_start_link_train(struct intel_dp *intel_dp)
 		}
 
 		/* Check to see if we've tried the same voltage 5 times */
-		if ((intel_dp->train_set[0] & DP_TRAIN_VOLTAGE_SWING_MASK) == voltage) {
-			++voltage_tries;
-			if (voltage_tries == 5) {
-				DRM_DEBUG_KMS("too many voltage retries, give up\n");
-				break;
-			}
-		} else
+		if ((intel_dp->train_set[0] & DP_TRAIN_VOLTAGE_SWING_MASK) != voltage) {
+			voltage = intel_dp->train_set[0] & DP_TRAIN_VOLTAGE_SWING_MASK;
 			voltage_tries = 0;
-		voltage = intel_dp->train_set[0] & DP_TRAIN_VOLTAGE_SWING_MASK;
+		} else
+			++voltage_tries;
 
 		/* Compute new intel_dp->train_set as requested by target */
 		intel_get_adjust_train(intel_dp, link_status);
@@ -1963,12 +1959,25 @@ static bool
 intel_dp_get_dpcd(struct intel_dp *intel_dp)
 {
 	if (intel_dp_aux_native_read_retry(intel_dp, 0x000, intel_dp->dpcd,
-					   sizeof(intel_dp->dpcd)) &&
-	    (intel_dp->dpcd[DP_DPCD_REV] != 0)) {
-		return true;
-	}
+					   sizeof(intel_dp->dpcd)) == 0)
+		return false; /* aux transfer failed */
 
-	return false;
+	if (intel_dp->dpcd[DP_DPCD_REV] == 0)
+		return false; /* DPCD not present */
+
+	if (!(intel_dp->dpcd[DP_DOWNSTREAMPORT_PRESENT] &
+	      DP_DWN_STRM_PORT_PRESENT))
+		return true; /* native DP sink */
+
+	if (intel_dp->dpcd[DP_DPCD_REV] == 0x10)
+		return true; /* no per-port downstream info */
+
+	if (intel_dp_aux_native_read_retry(intel_dp, DP_DOWNSTREAM_PORT_0,
+					   intel_dp->downstream_ports,
+					   DP_MAX_DOWNSTREAM_PORTS) == 0)
+		return false; /* downstream port status fetch failed */
+
+	return true;
 }
 
 static void
@@ -2068,11 +2077,43 @@ intel_dp_check_link_status(struct intel_dp *intel_dp)
 	}
 }
 
+/* XXX this is probably wrong for multiple downstream ports */
 static enum drm_connector_status
 intel_dp_detect_dpcd(struct intel_dp *intel_dp)
 {
-	if (intel_dp_get_dpcd(intel_dp))
+	uint8_t *dpcd = intel_dp->dpcd;
+	bool hpd;
+	uint8_t type;
+
+	if (!intel_dp_get_dpcd(intel_dp))
+		return connector_status_disconnected;
+
+	/* if there's no downstream port, we're done */
+	if (!(dpcd[DP_DOWNSTREAMPORT_PRESENT] & DP_DWN_STRM_PORT_PRESENT))
+		return connector_status_connected;
+
+	/* If we're HPD-aware, SINK_COUNT changes dynamically */
+	hpd = !!(intel_dp->downstream_ports[0] & DP_DS_PORT_HPD);
+	if (hpd) {
+		uint8_t reg;
+		if (!intel_dp_aux_native_read_retry(intel_dp, DP_SINK_COUNT,
+						    &reg, 1))
+			return connector_status_unknown;
+		return DP_GET_SINK_COUNT(reg) ? connector_status_connected
+					      : connector_status_disconnected;
+	}
+
+	/* If no HPD, poke DDC gently */
+	if (drm_probe_ddc(&intel_dp->adapter))
 		return connector_status_connected;
+
+	/* Well we tried, say unknown for unreliable port types */
+	type = intel_dp->downstream_ports[0] & DP_DS_PORT_TYPE_MASK;
+	if (type == DP_DS_PORT_TYPE_VGA || type == DP_DS_PORT_TYPE_NON_EDID)
+		return connector_status_unknown;
+
+	/* Anything else is out of spec, warn and ignore */
+	DRM_DEBUG_KMS("Broken DP branch device, ignoring\n");
 	return connector_status_disconnected;
 }
 
diff --git a/drivers/gpu/drm/i915/intel_drv.h b/drivers/gpu/drm/i915/intel_drv.h
index 05cc7c372fc..fe7142502f4 100644
--- a/drivers/gpu/drm/i915/intel_drv.h
+++ b/drivers/gpu/drm/i915/intel_drv.h
@@ -332,6 +332,7 @@ struct intel_hdmi {
 };
 
 #define DP_RECEIVER_CAP_SIZE		0xf
+#define DP_MAX_DOWNSTREAM_PORTS		0x10
 #define DP_LINK_CONFIGURATION_SIZE	9
 
 struct intel_dp {
@@ -346,6 +347,7 @@ struct intel_dp {
 	uint8_t link_bw;
 	uint8_t lane_count;
 	uint8_t dpcd[DP_RECEIVER_CAP_SIZE];
+	uint8_t downstream_ports[DP_MAX_DOWNSTREAM_PORTS];
 	struct i2c_adapter adapter;
 	struct i2c_algo_dp_aux_data algo;
 	bool is_pch_edp;
diff --git a/drivers/gpu/drm/i915/intel_pm.c b/drivers/gpu/drm/i915/intel_pm.c
index d69f8f49beb..b3b4b6cea8b 100644
--- a/drivers/gpu/drm/i915/intel_pm.c
+++ b/drivers/gpu/drm/i915/intel_pm.c
@@ -3474,6 +3474,11 @@ static void gen6_init_clock_gating(struct drm_device *dev)
 			   DISPPLANE_TRICKLE_FEED_DISABLE);
 		intel_flush_display_plane(dev_priv, pipe);
 	}
+
+	/* The default value should be 0x200 according to docs, but the two
+	 * platforms I checked have a 0 for this. (Maybe BIOS overrides?) */
+	I915_WRITE(GEN6_GT_MODE, _MASKED_BIT_DISABLE(0xffff));
+	I915_WRITE(GEN6_GT_MODE, _MASKED_BIT_ENABLE(GEN6_GT_MODE_HI));
 }
 
 static void gen7_setup_fixed_func_scheduler(struct drm_i915_private *dev_priv)
diff --git a/drivers/gpu/drm/nouveau/core/core/parent.c b/drivers/gpu/drm/nouveau/core/core/parent.c
index a1ea034611d..db7c5494310 100644
--- a/drivers/gpu/drm/nouveau/core/core/parent.c
+++ b/drivers/gpu/drm/nouveau/core/core/parent.c
@@ -101,23 +101,6 @@ nouveau_parent_create_(struct nouveau_object *parent,
 	return 0;
 }
 
-int
-_nouveau_parent_ctor(struct nouveau_object *parent,
-		     struct nouveau_object *engine,
-		     struct nouveau_oclass *oclass, void *data, u32 size,
-		     struct nouveau_object **pobject)
-{
-	struct nouveau_parent *object;
-	int ret;
-
-	ret = nouveau_parent_create(parent, engine, oclass, 0, NULL, 0, &object);
-	*pobject = nv_object(object);
-	if (ret)
-		return ret;
-
-	return 0;
-}
-
 void
 nouveau_parent_destroy(struct nouveau_parent *parent)
 {
diff --git a/drivers/gpu/drm/nouveau/core/include/core/parent.h b/drivers/gpu/drm/nouveau/core/include/core/parent.h
index d3aa251a5eb..3c2e940eb0f 100644
--- a/drivers/gpu/drm/nouveau/core/include/core/parent.h
+++ b/drivers/gpu/drm/nouveau/core/include/core/parent.h
@@ -50,9 +50,6 @@ int  nouveau_parent_create_(struct nouveau_object *, struct nouveau_object *,
 			    int size, void **);
 void nouveau_parent_destroy(struct nouveau_parent *);
 
-int  _nouveau_parent_ctor(struct nouveau_object *, struct nouveau_object *,
-			  struct nouveau_oclass *, void *, u32,
-			  struct nouveau_object **);
 void _nouveau_parent_dtor(struct nouveau_object *);
 #define _nouveau_parent_init _nouveau_object_init
 #define _nouveau_parent_fini _nouveau_object_fini
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/timer.h b/drivers/gpu/drm/nouveau/core/include/subdev/timer.h
index 49bff901544..c24ec8ab3db 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/timer.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/timer.h
@@ -26,7 +26,7 @@ void nouveau_timer_alarm(void *, u32 nsec, struct nouveau_alarm *);
 struct nouveau_timer {
 	struct nouveau_subdev base;
 	u64  (*read)(struct nouveau_timer *);
-	void (*alarm)(struct nouveau_timer *, u32 time, struct nouveau_alarm *);
+	void (*alarm)(struct nouveau_timer *, u64 time, struct nouveau_alarm *);
 };
 
 static inline struct nouveau_timer *
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/base.c b/drivers/gpu/drm/nouveau/core/subdev/bios/base.c
index 2fbb6df697c..dcb5c2befc9 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/base.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/base.c
@@ -185,23 +185,22 @@ static void
 nouveau_bios_shadow_acpi(struct nouveau_bios *bios)
 {
 	struct pci_dev *pdev = nv_device(bios)->pdev;
-	int cnt = 65536 / 4096;
-	int ret;
+	int ret, cnt, i;
+	u8  data[3];
 
 	if (!nouveau_acpi_rom_supported(pdev))
 		return;
 
-	bios->data = kmalloc(65536, GFP_KERNEL);
 	bios->size = 0;
-	if (!bios->data)
-		return;
-
-	while (cnt--) {
-		ret = nouveau_acpi_get_bios_chunk(bios->data, bios->size, 4096);
-		if (ret != 4096)
-			return;
+	if (nouveau_acpi_get_bios_chunk(data, 0, 3) == 3)
+		bios->size = data[2] * 512;
 
-		bios->size += 4096;
+	bios->data = kmalloc(bios->size, GFP_KERNEL);
+	for (i = 0; bios->data && i < bios->size; i += cnt) {
+		cnt = min((bios->size - i), (u32)4096);
+		ret = nouveau_acpi_get_bios_chunk(bios->data, i, cnt);
+		if (ret != cnt)
+			break;
 	}
 }
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/clock/nv50.c b/drivers/gpu/drm/nouveau/core/subdev/clock/nv50.c
index fd181fbcedd..f4147f67eda 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/clock/nv50.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/clock/nv50.c
@@ -90,6 +90,7 @@ nv50_clock_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 		return ret;
 
 	priv->base.pll_set = nv50_clock_pll_set;
+	priv->base.pll_calc = nv04_clock_pll_calc;
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/therm/nv50.c b/drivers/gpu/drm/nouveau/core/subdev/therm/nv50.c
index f87a7a3eb4e..9360ddd469e 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/therm/nv50.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/therm/nv50.c
@@ -92,7 +92,7 @@ nv50_fan_pwm_clock(struct nouveau_therm *therm)
 		if (nv_rd32(therm, 0xc040) & 0x800000) {
 			/* Use the HOST clock (100 MHz)
 			* Where does this constant(2.4) comes from? */
-			pwm_clock = (100000000 >> pwm_div) / 10 / 24;
+			pwm_clock = (100000000 >> pwm_div) * 10 / 24;
 		} else {
 			/* Where does this constant(20) comes from? */
 			pwm_clock = (crystal * 1000) >> pwm_div;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/timer/nv04.c b/drivers/gpu/drm/nouveau/core/subdev/timer/nv04.c
index 49976be4d73..c26ca9bef67 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/timer/nv04.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/timer/nv04.c
@@ -85,7 +85,7 @@ nv04_timer_alarm_trigger(struct nouveau_timer *ptimer)
 }
 
 static void
-nv04_timer_alarm(struct nouveau_timer *ptimer, u32 time,
+nv04_timer_alarm(struct nouveau_timer *ptimer, u64 time,
 		 struct nouveau_alarm *alarm)
 {
 	struct nv04_timer_priv *priv = (void *)ptimer;
diff --git a/drivers/gpu/drm/ttm/ttm_bo_vm.c b/drivers/gpu/drm/ttm/ttm_bo_vm.c
index a877813571a..3ba72dbdc4b 100644
--- a/drivers/gpu/drm/ttm/ttm_bo_vm.c
+++ b/drivers/gpu/drm/ttm/ttm_bo_vm.c
@@ -285,7 +285,7 @@ int ttm_bo_mmap(struct file *filp, struct vm_area_struct *vma,
 	 */
 
 	vma->vm_private_data = bo;
-	vma->vm_flags |= VM_RESERVED | VM_IO | VM_MIXEDMAP | VM_DONTEXPAND;
+	vma->vm_flags |= VM_IO | VM_MIXEDMAP | VM_DONTEXPAND | VM_DONTDUMP;
 	return 0;
 out_unref:
 	ttm_bo_unref(&bo);
@@ -300,7 +300,7 @@ int ttm_fbdev_mmap(struct vm_area_struct *vma, struct ttm_buffer_object *bo)
 
 	vma->vm_ops = &ttm_bo_vm_ops;
 	vma->vm_private_data = ttm_bo_reference(bo);
-	vma->vm_flags |= VM_RESERVED | VM_IO | VM_MIXEDMAP | VM_DONTEXPAND;
+	vma->vm_flags |= VM_IO | VM_MIXEDMAP | VM_DONTEXPAND;
 	return 0;
 }
 EXPORT_SYMBOL(ttm_fbdev_mmap);
diff --git a/drivers/gpu/drm/udl/udl_fb.c b/drivers/gpu/drm/udl/udl_fb.c
index 67df842fbb3..69a2b16f42a 100644
--- a/drivers/gpu/drm/udl/udl_fb.c
+++ b/drivers/gpu/drm/udl/udl_fb.c
@@ -243,7 +243,7 @@ static int udl_fb_mmap(struct fb_info *info, struct vm_area_struct *vma)
 			size = 0;
 	}
 
-	vma->vm_flags |= VM_RESERVED;	/* avoid to swap out this VMA */
+	/* VM_IO | VM_DONTEXPAND | VM_DONTDUMP are set by remap_pfn_range() */
 	return 0;
 }
 
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index c74e73b2069..c4633de6446 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -334,16 +334,6 @@ config SENSORS_DA9052_ADC
 	  This driver can also be built as module. If so, the module
 	  will be called da9052-hwmon.
 
-config SENSORS_EXYNOS4_TMU
-	tristate "Temperature sensor on Samsung EXYNOS4"
-	depends on ARCH_EXYNOS4
-	help
-	  If you say yes here you get support for TMU (Thermal Management
-	  Unit) on SAMSUNG EXYNOS4 series of SoC.
-
-	  This driver can also be built as a module. If so, the module
-	  will be called exynos4-tmu.
-
 config SENSORS_I5K_AMB
 	tristate "FB-DIMM AMB temperature sensor on Intel 5000 series chipsets"
 	depends on PCI
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index a62ce17ddbf..8d5fcb5e8e9 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -50,7 +50,6 @@ obj-$(CONFIG_SENSORS_DS1621)	+= ds1621.o
 obj-$(CONFIG_SENSORS_EMC1403)	+= emc1403.o
 obj-$(CONFIG_SENSORS_EMC2103)	+= emc2103.o
 obj-$(CONFIG_SENSORS_EMC6W201)	+= emc6w201.o
-obj-$(CONFIG_SENSORS_EXYNOS4_TMU)	+= exynos4_tmu.o
 obj-$(CONFIG_SENSORS_F71805F)	+= f71805f.o
 obj-$(CONFIG_SENSORS_F71882FG)	+= f71882fg.o
 obj-$(CONFIG_SENSORS_F75375S)	+= f75375s.o
diff --git a/drivers/hwmon/acpi_power_meter.c b/drivers/hwmon/acpi_power_meter.c
index 23ab3c496b0..1672e2a5db4 100644
--- a/drivers/hwmon/acpi_power_meter.c
+++ b/drivers/hwmon/acpi_power_meter.c
@@ -29,6 +29,7 @@
 #include <linux/kdev_t.h>
 #include <linux/sched.h>
 #include <linux/time.h>
+#include <linux/err.h>
 #include <acpi/acpi_drivers.h>
 #include <acpi/acpi_bus.h>
 
diff --git a/drivers/hwmon/adm9240.c b/drivers/hwmon/adm9240.c
index 8b24d1a4a2b..dafa477715e 100644
--- a/drivers/hwmon/adm9240.c
+++ b/drivers/hwmon/adm9240.c
@@ -50,6 +50,7 @@
 #include <linux/hwmon-vid.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
+#include <linux/jiffies.h>
 
 /* Addresses to scan */
 static const unsigned short normal_i2c[] = { 0x2c, 0x2d, 0x2e, 0x2f,
diff --git a/drivers/hwmon/adt7411.c b/drivers/hwmon/adt7411.c
index fe72c69a2d6..517f1856c70 100644
--- a/drivers/hwmon/adt7411.c
+++ b/drivers/hwmon/adt7411.c
@@ -15,7 +15,6 @@
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/err.h>
-#include <linux/delay.h>
 #include <linux/mutex.h>
 #include <linux/jiffies.h>
 #include <linux/i2c.h>
diff --git a/drivers/hwmon/adt7462.c b/drivers/hwmon/adt7462.c
index baee482aedf..98a7d81e25c 100644
--- a/drivers/hwmon/adt7462.c
+++ b/drivers/hwmon/adt7462.c
@@ -26,7 +26,6 @@
 #include <linux/hwmon-sysfs.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
-#include <linux/delay.h>
 #include <linux/log2.h>
 #include <linux/slab.h>
 
diff --git a/drivers/hwmon/adt7475.c b/drivers/hwmon/adt7475.c
index 861c756e953..989e54c3925 100644
--- a/drivers/hwmon/adt7475.c
+++ b/drivers/hwmon/adt7475.c
@@ -20,6 +20,7 @@
 #include <linux/hwmon-sysfs.h>
 #include <linux/hwmon-vid.h>
 #include <linux/err.h>
+#include <linux/jiffies.h>
 
 /* Indexes for the sysfs hooks */
 
diff --git a/drivers/hwmon/applesmc.c b/drivers/hwmon/applesmc.c
index 8f3f6f2c45f..b41baffa20f 100644
--- a/drivers/hwmon/applesmc.c
+++ b/drivers/hwmon/applesmc.c
@@ -43,6 +43,7 @@
 #include <linux/leds.h>
 #include <linux/hwmon.h>
 #include <linux/workqueue.h>
+#include <linux/err.h>
 
 /* data port used by Apple SMC */
 #define APPLESMC_DATA_PORT	0x300
diff --git a/drivers/hwmon/asus_atk0110.c b/drivers/hwmon/asus_atk0110.c
index cccb0e9d45b..56dbcfb3e30 100644
--- a/drivers/hwmon/asus_atk0110.c
+++ b/drivers/hwmon/asus_atk0110.c
@@ -14,6 +14,8 @@
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/dmi.h>
+#include <linux/jiffies.h>
+#include <linux/err.h>
 
 #include <acpi/acpi.h>
 #include <acpi/acpixf.h>
diff --git a/drivers/hwmon/da9052-hwmon.c b/drivers/hwmon/da9052-hwmon.c
index fc65f2d3ec9..b8d01c5f571 100644
--- a/drivers/hwmon/da9052-hwmon.c
+++ b/drivers/hwmon/da9052-hwmon.c
@@ -12,7 +12,6 @@
  *
  */
 
-#include <linux/delay.h>
 #include <linux/err.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
diff --git a/drivers/hwmon/emc1403.c b/drivers/hwmon/emc1403.c
index 68ab94bde3f..142e1cb8dea 100644
--- a/drivers/hwmon/emc1403.c
+++ b/drivers/hwmon/emc1403.c
@@ -33,6 +33,7 @@
 #include <linux/err.h>
 #include <linux/sysfs.h>
 #include <linux/mutex.h>
+#include <linux/jiffies.h>
 
 #define THERMAL_PID_REG		0xfd
 #define THERMAL_SMSC_ID_REG	0xfe
diff --git a/drivers/hwmon/emc6w201.c b/drivers/hwmon/emc6w201.c
index ada12a98a97..a98c917b588 100644
--- a/drivers/hwmon/emc6w201.c
+++ b/drivers/hwmon/emc6w201.c
@@ -18,7 +18,6 @@
  */
 
 #include <linux/module.h>
-#include <linux/delay.h>
 #include <linux/init.h>
 #include <linux/slab.h>
 #include <linux/jiffies.h>
diff --git a/drivers/hwmon/exynos4_tmu.c b/drivers/hwmon/exynos4_tmu.c
deleted file mode 100644
index e912059140c..00000000000
--- a/drivers/hwmon/exynos4_tmu.c
+++ /dev/null
@@ -1,518 +0,0 @@
-/*
- * exynos4_tmu.c - Samsung EXYNOS4 TMU (Thermal Management Unit)
- *
- *  Copyright (C) 2011 Samsung Electronics
- *  Donggeun Kim <dg77.kim@samsung.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.
- *
- * 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 <linux/module.h>
-#include <linux/err.h>
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/platform_device.h>
-#include <linux/interrupt.h>
-#include <linux/clk.h>
-#include <linux/workqueue.h>
-#include <linux/sysfs.h>
-#include <linux/kobject.h>
-#include <linux/io.h>
-#include <linux/mutex.h>
-
-#include <linux/hwmon.h>
-#include <linux/hwmon-sysfs.h>
-
-#include <linux/platform_data/exynos4_tmu.h>
-
-#define EXYNOS4_TMU_REG_TRIMINFO	0x0
-#define EXYNOS4_TMU_REG_CONTROL		0x20
-#define EXYNOS4_TMU_REG_STATUS		0x28
-#define EXYNOS4_TMU_REG_CURRENT_TEMP	0x40
-#define EXYNOS4_TMU_REG_THRESHOLD_TEMP	0x44
-#define EXYNOS4_TMU_REG_TRIG_LEVEL0	0x50
-#define EXYNOS4_TMU_REG_TRIG_LEVEL1	0x54
-#define EXYNOS4_TMU_REG_TRIG_LEVEL2	0x58
-#define EXYNOS4_TMU_REG_TRIG_LEVEL3	0x5C
-#define EXYNOS4_TMU_REG_PAST_TEMP0	0x60
-#define EXYNOS4_TMU_REG_PAST_TEMP1	0x64
-#define EXYNOS4_TMU_REG_PAST_TEMP2	0x68
-#define EXYNOS4_TMU_REG_PAST_TEMP3	0x6C
-#define EXYNOS4_TMU_REG_INTEN		0x70
-#define EXYNOS4_TMU_REG_INTSTAT		0x74
-#define EXYNOS4_TMU_REG_INTCLEAR	0x78
-
-#define EXYNOS4_TMU_GAIN_SHIFT		8
-#define EXYNOS4_TMU_REF_VOLTAGE_SHIFT	24
-
-#define EXYNOS4_TMU_TRIM_TEMP_MASK	0xff
-#define EXYNOS4_TMU_CORE_ON	3
-#define EXYNOS4_TMU_CORE_OFF	2
-#define EXYNOS4_TMU_DEF_CODE_TO_TEMP_OFFSET	50
-#define EXYNOS4_TMU_TRIG_LEVEL0_MASK	0x1
-#define EXYNOS4_TMU_TRIG_LEVEL1_MASK	0x10
-#define EXYNOS4_TMU_TRIG_LEVEL2_MASK	0x100
-#define EXYNOS4_TMU_TRIG_LEVEL3_MASK	0x1000
-#define EXYNOS4_TMU_INTCLEAR_VAL	0x1111
-
-struct exynos4_tmu_data {
-	struct exynos4_tmu_platform_data *pdata;
-	struct device *hwmon_dev;
-	struct resource *mem;
-	void __iomem *base;
-	int irq;
-	struct work_struct irq_work;
-	struct mutex lock;
-	struct clk *clk;
-	u8 temp_error1, temp_error2;
-};
-
-/*
- * TMU treats temperature as a mapped temperature code.
- * The temperature is converted differently depending on the calibration type.
- */
-static int temp_to_code(struct exynos4_tmu_data *data, u8 temp)
-{
-	struct exynos4_tmu_platform_data *pdata = data->pdata;
-	int temp_code;
-
-	/* temp should range between 25 and 125 */
-	if (temp < 25 || temp > 125) {
-		temp_code = -EINVAL;
-		goto out;
-	}
-
-	switch (pdata->cal_type) {
-	case TYPE_TWO_POINT_TRIMMING:
-		temp_code = (temp - 25) *
-		    (data->temp_error2 - data->temp_error1) /
-		    (85 - 25) + data->temp_error1;
-		break;
-	case TYPE_ONE_POINT_TRIMMING:
-		temp_code = temp + data->temp_error1 - 25;
-		break;
-	default:
-		temp_code = temp + EXYNOS4_TMU_DEF_CODE_TO_TEMP_OFFSET;
-		break;
-	}
-out:
-	return temp_code;
-}
-
-/*
- * Calculate a temperature value from a temperature code.
- * The unit of the temperature is degree Celsius.
- */
-static int code_to_temp(struct exynos4_tmu_data *data, u8 temp_code)
-{
-	struct exynos4_tmu_platform_data *pdata = data->pdata;
-	int temp;
-
-	/* temp_code should range between 75 and 175 */
-	if (temp_code < 75 || temp_code > 175) {
-		temp = -ENODATA;
-		goto out;
-	}
-
-	switch (pdata->cal_type) {
-	case TYPE_TWO_POINT_TRIMMING:
-		temp = (temp_code - data->temp_error1) * (85 - 25) /
-		    (data->temp_error2 - data->temp_error1) + 25;
-		break;
-	case TYPE_ONE_POINT_TRIMMING:
-		temp = temp_code - data->temp_error1 + 25;
-		break;
-	default:
-		temp = temp_code - EXYNOS4_TMU_DEF_CODE_TO_TEMP_OFFSET;
-		break;
-	}
-out:
-	return temp;
-}
-
-static int exynos4_tmu_initialize(struct platform_device *pdev)
-{
-	struct exynos4_tmu_data *data = platform_get_drvdata(pdev);
-	struct exynos4_tmu_platform_data *pdata = data->pdata;
-	unsigned int status, trim_info;
-	int ret = 0, threshold_code;
-
-	mutex_lock(&data->lock);
-	clk_enable(data->clk);
-
-	status = readb(data->base + EXYNOS4_TMU_REG_STATUS);
-	if (!status) {
-		ret = -EBUSY;
-		goto out;
-	}
-
-	/* Save trimming info in order to perform calibration */
-	trim_info = readl(data->base + EXYNOS4_TMU_REG_TRIMINFO);
-	data->temp_error1 = trim_info & EXYNOS4_TMU_TRIM_TEMP_MASK;
-	data->temp_error2 = ((trim_info >> 8) & EXYNOS4_TMU_TRIM_TEMP_MASK);
-
-	/* Write temperature code for threshold */
-	threshold_code = temp_to_code(data, pdata->threshold);
-	if (threshold_code < 0) {
-		ret = threshold_code;
-		goto out;
-	}
-	writeb(threshold_code,
-		data->base + EXYNOS4_TMU_REG_THRESHOLD_TEMP);
-
-	writeb(pdata->trigger_levels[0],
-		data->base + EXYNOS4_TMU_REG_TRIG_LEVEL0);
-	writeb(pdata->trigger_levels[1],
-		data->base + EXYNOS4_TMU_REG_TRIG_LEVEL1);
-	writeb(pdata->trigger_levels[2],
-		data->base + EXYNOS4_TMU_REG_TRIG_LEVEL2);
-	writeb(pdata->trigger_levels[3],
-		data->base + EXYNOS4_TMU_REG_TRIG_LEVEL3);
-
-	writel(EXYNOS4_TMU_INTCLEAR_VAL,
-		data->base + EXYNOS4_TMU_REG_INTCLEAR);
-out:
-	clk_disable(data->clk);
-	mutex_unlock(&data->lock);
-
-	return ret;
-}
-
-static void exynos4_tmu_control(struct platform_device *pdev, bool on)
-{
-	struct exynos4_tmu_data *data = platform_get_drvdata(pdev);
-	struct exynos4_tmu_platform_data *pdata = data->pdata;
-	unsigned int con, interrupt_en;
-
-	mutex_lock(&data->lock);
-	clk_enable(data->clk);
-
-	con = pdata->reference_voltage << EXYNOS4_TMU_REF_VOLTAGE_SHIFT |
-		pdata->gain << EXYNOS4_TMU_GAIN_SHIFT;
-	if (on) {
-		con |= EXYNOS4_TMU_CORE_ON;
-		interrupt_en = pdata->trigger_level3_en << 12 |
-			pdata->trigger_level2_en << 8 |
-			pdata->trigger_level1_en << 4 |
-			pdata->trigger_level0_en;
-	} else {
-		con |= EXYNOS4_TMU_CORE_OFF;
-		interrupt_en = 0; /* Disable all interrupts */
-	}
-	writel(interrupt_en, data->base + EXYNOS4_TMU_REG_INTEN);
-	writel(con, data->base + EXYNOS4_TMU_REG_CONTROL);
-
-	clk_disable(data->clk);
-	mutex_unlock(&data->lock);
-}
-
-static int exynos4_tmu_read(struct exynos4_tmu_data *data)
-{
-	u8 temp_code;
-	int temp;
-
-	mutex_lock(&data->lock);
-	clk_enable(data->clk);
-
-	temp_code = readb(data->base + EXYNOS4_TMU_REG_CURRENT_TEMP);
-	temp = code_to_temp(data, temp_code);
-
-	clk_disable(data->clk);
-	mutex_unlock(&data->lock);
-
-	return temp;
-}
-
-static void exynos4_tmu_work(struct work_struct *work)
-{
-	struct exynos4_tmu_data *data = container_of(work,
-			struct exynos4_tmu_data, irq_work);
-
-	mutex_lock(&data->lock);
-	clk_enable(data->clk);
-
-	writel(EXYNOS4_TMU_INTCLEAR_VAL, data->base + EXYNOS4_TMU_REG_INTCLEAR);
-
-	kobject_uevent(&data->hwmon_dev->kobj, KOBJ_CHANGE);
-
-	enable_irq(data->irq);
-
-	clk_disable(data->clk);
-	mutex_unlock(&data->lock);
-}
-
-static irqreturn_t exynos4_tmu_irq(int irq, void *id)
-{
-	struct exynos4_tmu_data *data = id;
-
-	disable_irq_nosync(irq);
-	schedule_work(&data->irq_work);
-
-	return IRQ_HANDLED;
-}
-
-static ssize_t exynos4_tmu_show_name(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	return sprintf(buf, "exynos4-tmu\n");
-}
-
-static ssize_t exynos4_tmu_show_temp(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	struct exynos4_tmu_data *data = dev_get_drvdata(dev);
-	int ret;
-
-	ret = exynos4_tmu_read(data);
-	if (ret < 0)
-		return ret;
-
-	/* convert from degree Celsius to millidegree Celsius */
-	return sprintf(buf, "%d\n", ret * 1000);
-}
-
-static ssize_t exynos4_tmu_show_alarm(struct device *dev,
-		struct device_attribute *devattr, char *buf)
-{
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
-	struct exynos4_tmu_data *data = dev_get_drvdata(dev);
-	struct exynos4_tmu_platform_data *pdata = data->pdata;
-	int temp;
-	unsigned int trigger_level;
-
-	temp = exynos4_tmu_read(data);
-	if (temp < 0)
-		return temp;
-
-	trigger_level = pdata->threshold + pdata->trigger_levels[attr->index];
-
-	return sprintf(buf, "%d\n", !!(temp > trigger_level));
-}
-
-static ssize_t exynos4_tmu_show_level(struct device *dev,
-		struct device_attribute *devattr, char *buf)
-{
-	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
-	struct exynos4_tmu_data *data = dev_get_drvdata(dev);
-	struct exynos4_tmu_platform_data *pdata = data->pdata;
-	unsigned int temp = pdata->threshold +
-			pdata->trigger_levels[attr->index];
-
-	return sprintf(buf, "%u\n", temp * 1000);
-}
-
-static DEVICE_ATTR(name, S_IRUGO, exynos4_tmu_show_name, NULL);
-static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, exynos4_tmu_show_temp, NULL, 0);
-
-static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO,
-		exynos4_tmu_show_alarm, NULL, 1);
-static SENSOR_DEVICE_ATTR(temp1_crit_alarm, S_IRUGO,
-		exynos4_tmu_show_alarm, NULL, 2);
-static SENSOR_DEVICE_ATTR(temp1_emergency_alarm, S_IRUGO,
-		exynos4_tmu_show_alarm, NULL, 3);
-
-static SENSOR_DEVICE_ATTR(temp1_max, S_IRUGO, exynos4_tmu_show_level, NULL, 1);
-static SENSOR_DEVICE_ATTR(temp1_crit, S_IRUGO, exynos4_tmu_show_level, NULL, 2);
-static SENSOR_DEVICE_ATTR(temp1_emergency, S_IRUGO,
-		exynos4_tmu_show_level, NULL, 3);
-
-static struct attribute *exynos4_tmu_attributes[] = {
-	&dev_attr_name.attr,
-	&sensor_dev_attr_temp1_input.dev_attr.attr,
-	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp1_emergency_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp1_max.dev_attr.attr,
-	&sensor_dev_attr_temp1_crit.dev_attr.attr,
-	&sensor_dev_attr_temp1_emergency.dev_attr.attr,
-	NULL,
-};
-
-static const struct attribute_group exynos4_tmu_attr_group = {
-	.attrs = exynos4_tmu_attributes,
-};
-
-static int __devinit exynos4_tmu_probe(struct platform_device *pdev)
-{
-	struct exynos4_tmu_data *data;
-	struct exynos4_tmu_platform_data *pdata = pdev->dev.platform_data;
-	int ret;
-
-	if (!pdata) {
-		dev_err(&pdev->dev, "No platform init data supplied.\n");
-		return -ENODEV;
-	}
-
-	data = kzalloc(sizeof(struct exynos4_tmu_data), GFP_KERNEL);
-	if (!data) {
-		dev_err(&pdev->dev, "Failed to allocate driver structure\n");
-		return -ENOMEM;
-	}
-
-	data->irq = platform_get_irq(pdev, 0);
-	if (data->irq < 0) {
-		ret = data->irq;
-		dev_err(&pdev->dev, "Failed to get platform irq\n");
-		goto err_free;
-	}
-
-	INIT_WORK(&data->irq_work, exynos4_tmu_work);
-
-	data->mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!data->mem) {
-		ret = -ENOENT;
-		dev_err(&pdev->dev, "Failed to get platform resource\n");
-		goto err_free;
-	}
-
-	data->mem = request_mem_region(data->mem->start,
-			resource_size(data->mem), pdev->name);
-	if (!data->mem) {
-		ret = -ENODEV;
-		dev_err(&pdev->dev, "Failed to request memory region\n");
-		goto err_free;
-	}
-
-	data->base = ioremap(data->mem->start, resource_size(data->mem));
-	if (!data->base) {
-		ret = -ENODEV;
-		dev_err(&pdev->dev, "Failed to ioremap memory\n");
-		goto err_mem_region;
-	}
-
-	ret = request_irq(data->irq, exynos4_tmu_irq,
-		IRQF_TRIGGER_RISING,
-		"exynos4-tmu", data);
-	if (ret) {
-		dev_err(&pdev->dev, "Failed to request irq: %d\n", data->irq);
-		goto err_io_remap;
-	}
-
-	data->clk = clk_get(NULL, "tmu_apbif");
-	if (IS_ERR(data->clk)) {
-		ret = PTR_ERR(data->clk);
-		dev_err(&pdev->dev, "Failed to get clock\n");
-		goto err_irq;
-	}
-
-	data->pdata = pdata;
-	platform_set_drvdata(pdev, data);
-	mutex_init(&data->lock);
-
-	ret = exynos4_tmu_initialize(pdev);
-	if (ret) {
-		dev_err(&pdev->dev, "Failed to initialize TMU\n");
-		goto err_clk;
-	}
-
-	ret = sysfs_create_group(&pdev->dev.kobj, &exynos4_tmu_attr_group);
-	if (ret) {
-		dev_err(&pdev->dev, "Failed to create sysfs group\n");
-		goto err_clk;
-	}
-
-	data->hwmon_dev = hwmon_device_register(&pdev->dev);
-	if (IS_ERR(data->hwmon_dev)) {
-		ret = PTR_ERR(data->hwmon_dev);
-		dev_err(&pdev->dev, "Failed to register hwmon device\n");
-		goto err_create_group;
-	}
-
-	exynos4_tmu_control(pdev, true);
-
-	return 0;
-
-err_create_group:
-	sysfs_remove_group(&pdev->dev.kobj, &exynos4_tmu_attr_group);
-err_clk:
-	platform_set_drvdata(pdev, NULL);
-	clk_put(data->clk);
-err_irq:
-	free_irq(data->irq, data);
-err_io_remap:
-	iounmap(data->base);
-err_mem_region:
-	release_mem_region(data->mem->start, resource_size(data->mem));
-err_free:
-	kfree(data);
-
-	return ret;
-}
-
-static int __devexit exynos4_tmu_remove(struct platform_device *pdev)
-{
-	struct exynos4_tmu_data *data = platform_get_drvdata(pdev);
-
-	exynos4_tmu_control(pdev, false);
-
-	hwmon_device_unregister(data->hwmon_dev);
-	sysfs_remove_group(&pdev->dev.kobj, &exynos4_tmu_attr_group);
-
-	clk_put(data->clk);
-
-	free_irq(data->irq, data);
-
-	iounmap(data->base);
-	release_mem_region(data->mem->start, resource_size(data->mem));
-
-	platform_set_drvdata(pdev, NULL);
-
-	kfree(data);
-
-	return 0;
-}
-
-#ifdef CONFIG_PM_SLEEP
-static int exynos4_tmu_suspend(struct device *dev)
-{
-	exynos4_tmu_control(to_platform_device(dev), false);
-
-	return 0;
-}
-
-static int exynos4_tmu_resume(struct device *dev)
-{
-	struct platform_device *pdev = to_platform_device(dev);
-
-	exynos4_tmu_initialize(pdev);
-	exynos4_tmu_control(pdev, true);
-
-	return 0;
-}
-
-static SIMPLE_DEV_PM_OPS(exynos4_tmu_pm,
-			 exynos4_tmu_suspend, exynos4_tmu_resume);
-#define EXYNOS4_TMU_PM	&exynos4_tmu_pm
-#else
-#define EXYNOS4_TMU_PM	NULL
-#endif
-
-static struct platform_driver exynos4_tmu_driver = {
-	.driver = {
-		.name   = "exynos4-tmu",
-		.owner  = THIS_MODULE,
-		.pm     = EXYNOS4_TMU_PM,
-	},
-	.probe = exynos4_tmu_probe,
-	.remove	= __devexit_p(exynos4_tmu_remove),
-};
-
-module_platform_driver(exynos4_tmu_driver);
-
-MODULE_DESCRIPTION("EXYNOS4 TMU Driver");
-MODULE_AUTHOR("Donggeun Kim <dg77.kim@samsung.com>");
-MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:exynos4-tmu");
diff --git a/drivers/hwmon/hih6130.c b/drivers/hwmon/hih6130.c
index e8ee75f5547..9a675efaa78 100644
--- a/drivers/hwmon/hih6130.c
+++ b/drivers/hwmon/hih6130.c
@@ -33,6 +33,7 @@
 #include <linux/mutex.h>
 #include <linux/device.h>
 #include <linux/delay.h>
+#include <linux/jiffies.h>
 
 /**
  * struct hih6130 - HIH-6130 device specific data
diff --git a/drivers/hwmon/i5k_amb.c b/drivers/hwmon/i5k_amb.c
index a18882cc073..46141abaafb 100644
--- a/drivers/hwmon/i5k_amb.c
+++ b/drivers/hwmon/i5k_amb.c
@@ -21,12 +21,10 @@
  */
 
 #include <linux/module.h>
-#include <linux/jiffies.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
-#include <linux/delay.h>
 #include <linux/log2.h>
 #include <linux/pci.h>
 #include <linux/platform_device.h>
diff --git a/drivers/hwmon/ibmaem.c b/drivers/hwmon/ibmaem.c
index 37f17e0d9d5..a14f634248e 100644
--- a/drivers/hwmon/ibmaem.c
+++ b/drivers/hwmon/ibmaem.c
@@ -36,6 +36,7 @@
 #include <linux/platform_device.h>
 #include <linux/math64.h>
 #include <linux/time.h>
+#include <linux/err.h>
 
 #define REFRESH_INTERVAL	(HZ)
 #define IPMI_TIMEOUT		(30 * HZ)
diff --git a/drivers/hwmon/ibmpex.c b/drivers/hwmon/ibmpex.c
index 41dbf8161ed..b622a93ec32 100644
--- a/drivers/hwmon/ibmpex.c
+++ b/drivers/hwmon/ibmpex.c
@@ -26,6 +26,7 @@
 #include <linux/jiffies.h>
 #include <linux/mutex.h>
 #include <linux/slab.h>
+#include <linux/err.h>
 
 #define REFRESH_INTERVAL	(2 * HZ)
 #define DRVNAME			"ibmpex"
diff --git a/drivers/hwmon/ina2xx.c b/drivers/hwmon/ina2xx.c
index 70717d4a5e8..2b726346f8f 100644
--- a/drivers/hwmon/ina2xx.c
+++ b/drivers/hwmon/ina2xx.c
@@ -33,6 +33,7 @@
 #include <linux/i2c.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
+#include <linux/jiffies.h>
 
 #include <linux/platform_data/ina2xx.h>
 
diff --git a/drivers/hwmon/k8temp.c b/drivers/hwmon/k8temp.c
index 49a69c5b3b8..e8c7fb0bbf9 100644
--- a/drivers/hwmon/k8temp.c
+++ b/drivers/hwmon/k8temp.c
@@ -22,7 +22,6 @@
  */
 
 #include <linux/module.h>
-#include <linux/delay.h>
 #include <linux/init.h>
 #include <linux/slab.h>
 #include <linux/jiffies.h>
diff --git a/drivers/hwmon/lineage-pem.c b/drivers/hwmon/lineage-pem.c
index bd75d241543..41df29f59b0 100644
--- a/drivers/hwmon/lineage-pem.c
+++ b/drivers/hwmon/lineage-pem.c
@@ -29,6 +29,7 @@
 #include <linux/i2c.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
+#include <linux/jiffies.h>
 
 /*
  * This driver supports various Lineage Compact Power Line DC/DC and AC/DC
diff --git a/drivers/hwmon/lm92.c b/drivers/hwmon/lm92.c
index 2282d77e83e..71626f3c874 100644
--- a/drivers/hwmon/lm92.c
+++ b/drivers/hwmon/lm92.c
@@ -48,6 +48,7 @@
 #include <linux/hwmon-sysfs.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
+#include <linux/jiffies.h>
 
 /*
  * The LM92 and MAX6635 have 2 two-state pins for address selection,
diff --git a/drivers/hwmon/lm93.c b/drivers/hwmon/lm93.c
index c3d4255ed15..1a003f73e4e 100644
--- a/drivers/hwmon/lm93.c
+++ b/drivers/hwmon/lm93.c
@@ -47,6 +47,7 @@
 #include <linux/hwmon-vid.h>
 #include <linux/err.h>
 #include <linux/delay.h>
+#include <linux/jiffies.h>
 
 /* LM93 REGISTER ADDRESSES */
 
diff --git a/drivers/hwmon/ltc4151.c b/drivers/hwmon/ltc4151.c
index 8496baa08bc..4319a94f549 100644
--- a/drivers/hwmon/ltc4151.c
+++ b/drivers/hwmon/ltc4151.c
@@ -36,6 +36,7 @@
 #include <linux/i2c.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
+#include <linux/jiffies.h>
 
 /* chip registers */
 #define LTC4151_SENSE_H	0x00
diff --git a/drivers/hwmon/ltc4215.c b/drivers/hwmon/ltc4215.c
index 98b3d04f98b..e8876108a6b 100644
--- a/drivers/hwmon/ltc4215.c
+++ b/drivers/hwmon/ltc4215.c
@@ -19,6 +19,7 @@
 #include <linux/i2c.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
+#include <linux/jiffies.h>
 
 /* Here are names of the chip's registers (a.k.a. commands) */
 enum ltc4215_cmd {
diff --git a/drivers/hwmon/ltc4245.c b/drivers/hwmon/ltc4245.c
index 52075914eb0..3653f79dc2d 100644
--- a/drivers/hwmon/ltc4245.c
+++ b/drivers/hwmon/ltc4245.c
@@ -21,6 +21,7 @@
 #include <linux/i2c.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
+#include <linux/jiffies.h>
 #include <linux/i2c/ltc4245.h>
 
 /* Here are names of the chip's registers (a.k.a. commands) */
diff --git a/drivers/hwmon/ltc4261.c b/drivers/hwmon/ltc4261.c
index 77476a575c4..84a2d2872b2 100644
--- a/drivers/hwmon/ltc4261.c
+++ b/drivers/hwmon/ltc4261.c
@@ -33,6 +33,7 @@
 #include <linux/i2c.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
+#include <linux/jiffies.h>
 
 /* chip registers */
 #define LTC4261_STATUS	0x00	/* readonly */
diff --git a/drivers/hwmon/max16065.c b/drivers/hwmon/max16065.c
index 019427d7a5f..e0019c69d1b 100644
--- a/drivers/hwmon/max16065.c
+++ b/drivers/hwmon/max16065.c
@@ -22,7 +22,6 @@
 #include <linux/i2c.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
-#include <linux/delay.h>
 #include <linux/jiffies.h>
 
 enum chips { max16065, max16066, max16067, max16068, max16070, max16071 };
diff --git a/drivers/hwmon/max1619.c b/drivers/hwmon/max1619.c
index 6c11ec21407..445e5d40ac8 100644
--- a/drivers/hwmon/max1619.c
+++ b/drivers/hwmon/max1619.c
@@ -1,7 +1,7 @@
 /*
  * max1619.c - Part of lm_sensors, Linux kernel modules for hardware
  *             monitoring
- * Copyright (C) 2003-2004 Alexey Fisher <fishor@mail.ru>
+ * Copyright (C) 2003-2004 Oleksij Rempel <bug-track@fisher-privat.net>
  *                         Jean Delvare <khali@linux-fr.org>
  *
  * Based on the lm90 driver. The MAX1619 is a sensor chip made by Maxim.
@@ -357,7 +357,7 @@ static struct max1619_data *max1619_update_device(struct device *dev)
 
 module_i2c_driver(max1619_driver);
 
-MODULE_AUTHOR("Alexey Fisher <fishor@mail.ru> and "
+MODULE_AUTHOR("Oleksij Rempel <bug-track@fisher-privat.net> and "
 	"Jean Delvare <khali@linux-fr.org>");
 MODULE_DESCRIPTION("MAX1619 sensor driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/max6642.c b/drivers/hwmon/max6642.c
index bf236c0782b..223461a6d70 100644
--- a/drivers/hwmon/max6642.c
+++ b/drivers/hwmon/max6642.c
@@ -7,7 +7,7 @@
  * Derived from:
  *
  *  Based on the max1619 driver.
- *  Copyright (C) 2003-2004 Alexey Fisher <fishor@mail.ru>
+ *  Copyright (C) 2003-2004 Oleksij Rempel <bug-track@fisher-privat.net>
  *                          Jean Delvare <khali@linux-fr.org>
  *
  * The MAX6642 is a sensor chip made by Maxim.
diff --git a/drivers/hwmon/pmbus/pmbus_core.c b/drivers/hwmon/pmbus/pmbus_core.c
index 29b319db573..7d19b1bb9ce 100644
--- a/drivers/hwmon/pmbus/pmbus_core.c
+++ b/drivers/hwmon/pmbus/pmbus_core.c
@@ -26,7 +26,7 @@
 #include <linux/i2c.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
-#include <linux/delay.h>
+#include <linux/jiffies.h>
 #include <linux/i2c/pmbus.h>
 #include "pmbus.h"
 
diff --git a/drivers/hwmon/s3c-hwmon.c b/drivers/hwmon/s3c-hwmon.c
index fe11b95670b..bcecd025fcc 100644
--- a/drivers/hwmon/s3c-hwmon.c
+++ b/drivers/hwmon/s3c-hwmon.c
@@ -22,7 +22,6 @@
 
 #include <linux/module.h>
 #include <linux/slab.h>
-#include <linux/delay.h>
 #include <linux/io.h>
 #include <linux/init.h>
 #include <linux/err.h>
diff --git a/drivers/hwmon/sht21.c b/drivers/hwmon/sht21.c
index c2565d04cd4..5f67546950b 100644
--- a/drivers/hwmon/sht21.c
+++ b/drivers/hwmon/sht21.c
@@ -29,6 +29,7 @@
 #include <linux/err.h>
 #include <linux/mutex.h>
 #include <linux/device.h>
+#include <linux/jiffies.h>
 
 /* I2C command bytes */
 #define SHT21_TRIG_T_MEASUREMENT_HM  0xe3
diff --git a/drivers/hwmon/smm665.c b/drivers/hwmon/smm665.c
index cbc51fb30db..d9e1b7de78d 100644
--- a/drivers/hwmon/smm665.c
+++ b/drivers/hwmon/smm665.c
@@ -24,6 +24,7 @@
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
 #include <linux/delay.h>
+#include <linux/jiffies.h>
 
 /* Internal reference voltage (VREF, x 1000 */
 #define SMM665_VREF_ADC_X1000	1250
diff --git a/drivers/hwmon/thmc50.c b/drivers/hwmon/thmc50.c
index 080c2637048..3c2c48d904e 100644
--- a/drivers/hwmon/thmc50.c
+++ b/drivers/hwmon/thmc50.c
@@ -28,6 +28,7 @@
 #include <linux/hwmon-sysfs.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
+#include <linux/jiffies.h>
 
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/hwmon/tmp102.c b/drivers/hwmon/tmp102.c
index 4e1ff82c63e..b8777e54190 100644
--- a/drivers/hwmon/tmp102.c
+++ b/drivers/hwmon/tmp102.c
@@ -26,6 +26,7 @@
 #include <linux/err.h>
 #include <linux/mutex.h>
 #include <linux/device.h>
+#include <linux/jiffies.h>
 
 #define	DRIVER_NAME "tmp102"
 
diff --git a/drivers/hwmon/ultra45_env.c b/drivers/hwmon/ultra45_env.c
index c315c59f61f..44136bb6d04 100644
--- a/drivers/hwmon/ultra45_env.c
+++ b/drivers/hwmon/ultra45_env.c
@@ -12,6 +12,7 @@
 #include <linux/io.h>
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
+#include <linux/err.h>
 
 #define DRV_MODULE_VERSION	"0.1"
 
diff --git a/drivers/hwmon/w83791d.c b/drivers/hwmon/w83791d.c
index 93ea81a4bf3..39ab7bcc616 100644
--- a/drivers/hwmon/w83791d.c
+++ b/drivers/hwmon/w83791d.c
@@ -41,6 +41,7 @@
 #include <linux/hwmon-sysfs.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
+#include <linux/jiffies.h>
 
 #define NUMBER_OF_VIN		10
 #define NUMBER_OF_FANIN		5
diff --git a/drivers/hwmon/w83792d.c b/drivers/hwmon/w83792d.c
index 06d6f56d4f6..053645279f3 100644
--- a/drivers/hwmon/w83792d.c
+++ b/drivers/hwmon/w83792d.c
@@ -44,6 +44,7 @@
 #include <linux/err.h>
 #include <linux/mutex.h>
 #include <linux/sysfs.h>
+#include <linux/jiffies.h>
 
 /* Addresses to scan */
 static const unsigned short normal_i2c[] = { 0x2c, 0x2d, 0x2e, 0x2f,
diff --git a/drivers/hwmon/w83793.c b/drivers/hwmon/w83793.c
index 4fc47e06207..99799fd1d91 100644
--- a/drivers/hwmon/w83793.c
+++ b/drivers/hwmon/w83793.c
@@ -46,6 +46,7 @@
 #include <linux/kref.h>
 #include <linux/notifier.h>
 #include <linux/reboot.h>
+#include <linux/jiffies.h>
 
 /* Default values */
 #define WATCHDOG_TIMEOUT 2	/* 2 minute default timeout */
diff --git a/drivers/hwmon/w83795.c b/drivers/hwmon/w83795.c
index b813c646c7c..55a4f489453 100644
--- a/drivers/hwmon/w83795.c
+++ b/drivers/hwmon/w83795.c
@@ -34,7 +34,7 @@
 #include <linux/hwmon-sysfs.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
-#include <linux/delay.h>
+#include <linux/jiffies.h>
 
 /* Addresses to scan */
 static const unsigned short normal_i2c[] = {
diff --git a/drivers/hwmon/w83l786ng.c b/drivers/hwmon/w83l786ng.c
index c99c8a0473c..f0e8286c3c7 100644
--- a/drivers/hwmon/w83l786ng.c
+++ b/drivers/hwmon/w83l786ng.c
@@ -33,6 +33,7 @@
 #include <linux/hwmon-sysfs.h>
 #include <linux/err.h>
 #include <linux/mutex.h>
+#include <linux/jiffies.h>
 
 /* Addresses to scan */
 static const unsigned short normal_i2c[] = { 0x2e, 0x2f, I2C_CLIENT_END };
diff --git a/drivers/i2c/Kconfig b/drivers/i2c/Kconfig
index 5a3bb3d738d..2f8c76becc6 100644
--- a/drivers/i2c/Kconfig
+++ b/drivers/i2c/Kconfig
@@ -4,7 +4,7 @@
 
 menuconfig I2C
 	tristate "I2C support"
-	depends on HAS_IOMEM
+	depends on !S390
 	select RT_MUTEXES
 	---help---
 	  I2C (pronounce: I-squared-C) is a slow serial bus protocol used in
@@ -49,6 +49,7 @@ config I2C_CHARDEV
 
 config I2C_MUX
 	tristate "I2C bus multiplexing support"
+	depends on HAS_IOMEM
 	help
 	  Say Y here if you want the I2C core to support the ability to
 	  handle multiplexed I2C bus topologies, by presenting each
@@ -86,6 +87,19 @@ config I2C_SMBUS
 source drivers/i2c/algos/Kconfig
 source drivers/i2c/busses/Kconfig
 
+config I2C_STUB
+	tristate "I2C/SMBus Test Stub"
+	depends on EXPERIMENTAL && m
+	default 'n'
+	help
+	  This module may be useful to developers of SMBus client drivers,
+	  especially for certain kinds of sensor chips.
+
+	  If you do build this module, be sure to read the notes and warnings
+	  in <file:Documentation/i2c/i2c-stub>.
+
+	  If you don't know what to do here, definitely say N.
+
 config I2C_DEBUG_CORE
 	bool "I2C Core debugging messages"
 	help
@@ -103,6 +117,7 @@ config I2C_DEBUG_ALGO
 
 config I2C_DEBUG_BUS
 	bool "I2C Bus debugging messages"
+	depends on HAS_IOMEM
 	help
 	  Say Y here if you want the I2C bus drivers to produce a bunch of
 	  debug messages to the system log.  Select this if you are having
diff --git a/drivers/i2c/algos/i2c-algo-pca.c b/drivers/i2c/algos/i2c-algo-pca.c
index 6f5f98d69af..f892a424009 100644
--- a/drivers/i2c/algos/i2c-algo-pca.c
+++ b/drivers/i2c/algos/i2c-algo-pca.c
@@ -46,14 +46,19 @@ static int i2c_debug;
 #define pca_set_con(adap, val) pca_outw(adap, I2C_PCA_CON, val)
 #define pca_get_con(adap) pca_inw(adap, I2C_PCA_CON)
 #define pca_wait(adap) adap->wait_for_completion(adap->data)
-#define pca_reset(adap) adap->reset_chip(adap->data)
 
-static void pca9665_reset(void *pd)
+static void pca_reset(struct i2c_algo_pca_data *adap)
 {
-	struct i2c_algo_pca_data *adap = pd;
-	pca_outw(adap, I2C_PCA_INDPTR, I2C_PCA_IPRESET);
-	pca_outw(adap, I2C_PCA_IND, 0xA5);
-	pca_outw(adap, I2C_PCA_IND, 0x5A);
+	if (adap->chip == I2C_PCA_CHIP_9665) {
+		/* Ignore the reset function from the module,
+		 * we can use the parallel bus reset.
+		 */
+		pca_outw(adap, I2C_PCA_INDPTR, I2C_PCA_IPRESET);
+		pca_outw(adap, I2C_PCA_IND, 0xA5);
+		pca_outw(adap, I2C_PCA_IND, 0x5A);
+	} else {
+		adap->reset_chip(adap->data);
+	}
 }
 
 /*
@@ -378,11 +383,12 @@ static unsigned int pca_probe_chip(struct i2c_adapter *adap)
 	pca_outw(pca_data, I2C_PCA_INDPTR, I2C_PCA_IADR);
 	if (pca_inw(pca_data, I2C_PCA_IND) == 0xAA) {
 		printk(KERN_INFO "%s: PCA9665 detected.\n", adap->name);
-		return I2C_PCA_CHIP_9665;
+		pca_data->chip = I2C_PCA_CHIP_9665;
 	} else {
 		printk(KERN_INFO "%s: PCA9564 detected.\n", adap->name);
-		return I2C_PCA_CHIP_9564;
+		pca_data->chip = I2C_PCA_CHIP_9564;
 	}
+	return pca_data->chip;
 }
 
 static int pca_init(struct i2c_adapter *adap)
@@ -456,11 +462,6 @@ static int pca_init(struct i2c_adapter *adap)
 		 */
 		int raise_fall_time;
 
-		/* Ignore the reset function from the module,
-		 * we can use the parallel bus reset
-		 */
-		pca_data->reset_chip = pca9665_reset;
-
 		if (pca_data->i2c_clock > 1265800) {
 			printk(KERN_WARNING "%s: I2C clock speed too high."
 				" Using 1265.8kHz.\n", adap->name);
diff --git a/drivers/i2c/busses/Kconfig b/drivers/i2c/busses/Kconfig
index 42d9fdd63de..65dd599a026 100644
--- a/drivers/i2c/busses/Kconfig
+++ b/drivers/i2c/busses/Kconfig
@@ -3,6 +3,7 @@
 #
 
 menu "I2C Hardware Bus support"
+	depends on HAS_IOMEM
 
 comment "PC SMBus host controller drivers"
 	depends on PCI
@@ -80,6 +81,7 @@ config I2C_I801
 	tristate "Intel 82801 (ICH/PCH)"
 	depends on PCI
 	select CHECK_SIGNATURE if X86 && DMI
+	select GPIOLIB if I2C_MUX
 	help
 	  If you say yes to this option, support will be included for the Intel
 	  801 family of mainboard I2C interfaces.  Specifically, the following
@@ -224,7 +226,7 @@ config I2C_VIA
 	  will be called i2c-via.
 
 config I2C_VIAPRO
-	tristate "VIA VT82C596/82C686/82xx and CX700/VX8xx"
+	tristate "VIA VT82C596/82C686/82xx and CX700/VX8xx/VX900"
 	depends on PCI
 	help
 	  If you say yes to this option, support will be included for the VIA
@@ -240,6 +242,7 @@ config I2C_VIAPRO
 	    CX700
 	    VX800/VX820
 	    VX855/VX875
+	    VX900
 
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-viapro.
@@ -291,18 +294,21 @@ comment "I2C system bus drivers (mostly embedded / system-on-chip)"
 
 config I2C_AT91
 	tristate "Atmel AT91 I2C Two-Wire interface (TWI)"
-	depends on ARCH_AT91 && EXPERIMENTAL && BROKEN
+	depends on ARCH_AT91 && EXPERIMENTAL
 	help
 	  This supports the use of the I2C interface on Atmel AT91
 	  processors.
 
-	  This driver is BROKEN because the controller which it uses
-	  will easily trigger RX overrun and TX underrun errors.  Using
-	  low I2C clock rates may partially work around those issues
-	  on some systems.  Another serious problem is that there is no
-	  documented way to issue repeated START conditions, as needed
+	  A serious problem is that there is no documented way to issue
+	  repeated START conditions for more than two messages, as needed
 	  to support combined I2C messages.  Use the i2c-gpio driver
-	  unless your system can cope with those limitations.
+	  unless your system can cope with this limitation.
+
+	  Caution! at91rm9200, at91sam9261, at91sam9260, at91sam9263 devices
+	  don't have clock stretching in transmission mode. For that reason,
+	  you can encounter underrun issues causing premature stop sendings if
+	  the latency to fill the transmission register is too long. If you
+	  are facing this situation, use the i2c-gpio driver.
 
 config I2C_AU1550
 	tristate "Au1550/Au1200/Au1300 SMBus interface"
@@ -715,6 +721,16 @@ config I2C_XLR
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-xlr.
 
+config I2C_RCAR
+	tristate "Renesas R-Car I2C Controller"
+	depends on ARCH_SHMOBILE && I2C
+	help
+	  If you say yes to this option, support will be included for the
+	  R-Car I2C controller.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called i2c-rcar.
+
 comment "External I2C/SMBus adapter drivers"
 
 config I2C_DIOLAN_U2C
@@ -849,19 +865,6 @@ config I2C_SIBYTE
 	help
 	  Supports the SiByte SOC on-chip I2C interfaces (2 channels).
 
-config I2C_STUB
-	tristate "I2C/SMBus Test Stub"
-	depends on EXPERIMENTAL && m
-	default 'n'
-	help
-	  This module may be useful to developers of SMBus client drivers,
-	  especially for certain kinds of sensor chips.
-
-	  If you do build this module, be sure to read the notes and warnings
-	  in <file:Documentation/i2c/i2c-stub>.
-
-	  If you don't know what to do here, definitely say N.
-
 config SCx200_I2C
 	tristate "NatSemi SCx200 I2C using GPIO pins (DEPRECATED)"
 	depends on SCx200_GPIO
diff --git a/drivers/i2c/busses/Makefile b/drivers/i2c/busses/Makefile
index 37c4182cc98..2d33d62952c 100644
--- a/drivers/i2c/busses/Makefile
+++ b/drivers/i2c/busses/Makefile
@@ -71,6 +71,7 @@ obj-$(CONFIG_I2C_VERSATILE)	+= i2c-versatile.o
 obj-$(CONFIG_I2C_OCTEON)	+= i2c-octeon.o
 obj-$(CONFIG_I2C_XILINX)	+= i2c-xiic.o
 obj-$(CONFIG_I2C_XLR)		+= i2c-xlr.o
+obj-$(CONFIG_I2C_RCAR)		+= i2c-rcar.o
 
 # External I2C/SMBus adapter drivers
 obj-$(CONFIG_I2C_DIOLAN_U2C)	+= i2c-diolan-u2c.o
diff --git a/drivers/i2c/busses/i2c-at91.c b/drivers/i2c/busses/i2c-at91.c
index e24484beef0..aa59a254be2 100644
--- a/drivers/i2c/busses/i2c-at91.c
+++ b/drivers/i2c/busses/i2c-at91.c
@@ -1,315 +1,554 @@
 /*
-    i2c Support for Atmel's AT91 Two-Wire Interface (TWI)
-
-    Copyright (C) 2004 Rick Bronson
-    Converted to 2.6 by Andrew Victor <andrew@sanpeople.com>
-
-    Borrowed heavily from original work by:
-    Copyright (C) 2000 Philip Edelbrock <phil@stimpy.netroedge.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.
-*/
+ *  i2c Support for Atmel's AT91 Two-Wire Interface (TWI)
+ *
+ *  Copyright (C) 2011 Weinmann Medical GmbH
+ *  Author: Nikolaus Voss <n.voss@weinmann.de>
+ *
+ *  Evolved from original work by:
+ *  Copyright (C) 2004 Rick Bronson
+ *  Converted to 2.6 by Andrew Victor <andrew@sanpeople.com>
+ *
+ *  Borrowed heavily from original work by:
+ *  Copyright (C) 2000 Philip Edelbrock <phil@stimpy.netroedge.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.
+ */
 
-#include <linux/module.h>
-#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/completion.h>
 #include <linux/err.h>
-#include <linux/slab.h>
-#include <linux/types.h>
-#include <linux/delay.h>
 #include <linux/i2c.h>
-#include <linux/init.h>
-#include <linux/clk.h>
-#include <linux/platform_device.h>
+#include <linux/interrupt.h>
 #include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_i2c.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#define TWI_CLK_HZ		100000			/* max 400 Kbits/s */
+#define AT91_I2C_TIMEOUT	msecs_to_jiffies(100)	/* transfer timeout */
+
+/* AT91 TWI register definitions */
+#define	AT91_TWI_CR		0x0000	/* Control Register */
+#define	AT91_TWI_START		0x0001	/* Send a Start Condition */
+#define	AT91_TWI_STOP		0x0002	/* Send a Stop Condition */
+#define	AT91_TWI_MSEN		0x0004	/* Master Transfer Enable */
+#define	AT91_TWI_SVDIS		0x0020	/* Slave Transfer Disable */
+#define	AT91_TWI_SWRST		0x0080	/* Software Reset */
+
+#define	AT91_TWI_MMR		0x0004	/* Master Mode Register */
+#define	AT91_TWI_IADRSZ_1	0x0100	/* Internal Device Address Size */
+#define	AT91_TWI_MREAD		0x1000	/* Master Read Direction */
+
+#define	AT91_TWI_IADR		0x000c	/* Internal Address Register */
+
+#define	AT91_TWI_CWGR		0x0010	/* Clock Waveform Generator Reg */
+
+#define	AT91_TWI_SR		0x0020	/* Status Register */
+#define	AT91_TWI_TXCOMP		0x0001	/* Transmission Complete */
+#define	AT91_TWI_RXRDY		0x0002	/* Receive Holding Register Ready */
+#define	AT91_TWI_TXRDY		0x0004	/* Transmit Holding Register Ready */
 
-#include <mach/at91_twi.h>
-#include <mach/board.h>
-#include <mach/cpu.h>
+#define	AT91_TWI_OVRE		0x0040	/* Overrun Error */
+#define	AT91_TWI_UNRE		0x0080	/* Underrun Error */
+#define	AT91_TWI_NACK		0x0100	/* Not Acknowledged */
 
-#define TWI_CLOCK		100000		/* Hz. max 400 Kbits/sec */
+#define	AT91_TWI_IER		0x0024	/* Interrupt Enable Register */
+#define	AT91_TWI_IDR		0x0028	/* Interrupt Disable Register */
+#define	AT91_TWI_IMR		0x002c	/* Interrupt Mask Register */
+#define	AT91_TWI_RHR		0x0030	/* Receive Holding Register */
+#define	AT91_TWI_THR		0x0034	/* Transmit Holding Register */
 
+struct at91_twi_pdata {
+	unsigned	clk_max_div;
+	unsigned	clk_offset;
+	bool		has_unre_flag;
+};
+
+struct at91_twi_dev {
+	struct device		*dev;
+	void __iomem		*base;
+	struct completion	cmd_complete;
+	struct clk		*clk;
+	u8			*buf;
+	size_t			buf_len;
+	struct i2c_msg		*msg;
+	int			irq;
+	unsigned		transfer_status;
+	struct i2c_adapter	adapter;
+	unsigned		twi_cwgr_reg;
+	struct at91_twi_pdata	*pdata;
+};
 
-static struct clk *twi_clk;
-static void __iomem *twi_base;
+static unsigned at91_twi_read(struct at91_twi_dev *dev, unsigned reg)
+{
+	return readl_relaxed(dev->base + reg);
+}
+
+static void at91_twi_write(struct at91_twi_dev *dev, unsigned reg, unsigned val)
+{
+	writel_relaxed(val, dev->base + reg);
+}
 
-#define at91_twi_read(reg)		__raw_readl(twi_base + (reg))
-#define at91_twi_write(reg, val)	__raw_writel((val), twi_base + (reg))
+static void at91_disable_twi_interrupts(struct at91_twi_dev *dev)
+{
+	at91_twi_write(dev, AT91_TWI_IDR,
+		       AT91_TWI_TXCOMP | AT91_TWI_RXRDY | AT91_TWI_TXRDY);
+}
 
+static void at91_init_twi_bus(struct at91_twi_dev *dev)
+{
+	at91_disable_twi_interrupts(dev);
+	at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_SWRST);
+	at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_MSEN);
+	at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_SVDIS);
+	at91_twi_write(dev, AT91_TWI_CWGR, dev->twi_cwgr_reg);
+}
 
 /*
- * Initialize the TWI hardware registers.
+ * Calculate symmetric clock as stated in datasheet:
+ * twi_clk = F_MAIN / (2 * (cdiv * (1 << ckdiv) + offset))
  */
-static void __devinit at91_twi_hwinit(void)
+static void __devinit at91_calc_twi_clock(struct at91_twi_dev *dev, int twi_clk)
 {
-	unsigned long cdiv, ckdiv;
-
-	at91_twi_write(AT91_TWI_IDR, 0xffffffff);	/* Disable all interrupts */
-	at91_twi_write(AT91_TWI_CR, AT91_TWI_SWRST);	/* Reset peripheral */
-	at91_twi_write(AT91_TWI_CR, AT91_TWI_MSEN);	/* Set Master mode */
-
-	/* Calcuate clock dividers */
-	cdiv = (clk_get_rate(twi_clk) / (2 * TWI_CLOCK)) - 3;
-	cdiv = cdiv + 1;	/* round up */
-	ckdiv = 0;
-	while (cdiv > 255) {
-		ckdiv++;
-		cdiv = cdiv >> 1;
+	int ckdiv, cdiv, div;
+	struct at91_twi_pdata *pdata = dev->pdata;
+	int offset = pdata->clk_offset;
+	int max_ckdiv = pdata->clk_max_div;
+
+	div = max(0, (int)DIV_ROUND_UP(clk_get_rate(dev->clk),
+				       2 * twi_clk) - offset);
+	ckdiv = fls(div >> 8);
+	cdiv = div >> ckdiv;
+
+	if (ckdiv > max_ckdiv) {
+		dev_warn(dev->dev, "%d exceeds ckdiv max value which is %d.\n",
+			 ckdiv, max_ckdiv);
+		ckdiv = max_ckdiv;
+		cdiv = 255;
 	}
 
-	if (cpu_is_at91rm9200()) {			/* AT91RM9200 Errata #22 */
-		if (ckdiv > 5) {
-			printk(KERN_ERR "AT91 I2C: Invalid TWI_CLOCK value!\n");
-			ckdiv = 5;
-		}
-	}
+	dev->twi_cwgr_reg = (ckdiv << 16) | (cdiv << 8) | cdiv;
+	dev_dbg(dev->dev, "cdiv %d ckdiv %d\n", cdiv, ckdiv);
+}
 
-	at91_twi_write(AT91_TWI_CWGR, (ckdiv << 16) | (cdiv << 8) | cdiv);
+static void at91_twi_write_next_byte(struct at91_twi_dev *dev)
+{
+	if (dev->buf_len <= 0)
+		return;
+
+	at91_twi_write(dev, AT91_TWI_THR, *dev->buf);
+
+	/* send stop when last byte has been written */
+	if (--dev->buf_len == 0)
+		at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_STOP);
+
+	dev_dbg(dev->dev, "wrote 0x%x, to go %d\n", *dev->buf, dev->buf_len);
+
+	++dev->buf;
 }
 
-/*
- * Poll the i2c status register until the specified bit is set.
- * Returns 0 if timed out (100 msec).
- */
-static short at91_poll_status(unsigned long bit)
+static void at91_twi_read_next_byte(struct at91_twi_dev *dev)
 {
-	int loop_cntr = 10000;
+	if (dev->buf_len <= 0)
+		return;
+
+	*dev->buf = at91_twi_read(dev, AT91_TWI_RHR) & 0xff;
+	--dev->buf_len;
+
+	/* handle I2C_SMBUS_BLOCK_DATA */
+	if (unlikely(dev->msg->flags & I2C_M_RECV_LEN)) {
+		dev->msg->flags &= ~I2C_M_RECV_LEN;
+		dev->buf_len += *dev->buf;
+		dev->msg->len = dev->buf_len + 1;
+		dev_dbg(dev->dev, "received block length %d\n", dev->buf_len);
+	}
+
+	/* send stop if second but last byte has been read */
+	if (dev->buf_len == 1)
+		at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_STOP);
 
-	do {
-		udelay(10);
-	} while (!(at91_twi_read(AT91_TWI_SR) & bit) && (--loop_cntr > 0));
+	dev_dbg(dev->dev, "read 0x%x, to go %d\n", *dev->buf, dev->buf_len);
 
-	return (loop_cntr > 0);
+	++dev->buf;
 }
 
-static int xfer_read(struct i2c_adapter *adap, unsigned char *buf, int length)
+static irqreturn_t atmel_twi_interrupt(int irq, void *dev_id)
 {
-	/* Send Start */
-	at91_twi_write(AT91_TWI_CR, AT91_TWI_START);
-
-	/* Read data */
-	while (length--) {
-		if (!length)	/* need to send Stop before reading last byte */
-			at91_twi_write(AT91_TWI_CR, AT91_TWI_STOP);
-		if (!at91_poll_status(AT91_TWI_RXRDY)) {
-			dev_dbg(&adap->dev, "RXRDY timeout\n");
-			return -ETIMEDOUT;
-		}
-		*buf++ = (at91_twi_read(AT91_TWI_RHR) & 0xff);
+	struct at91_twi_dev *dev = dev_id;
+	const unsigned status = at91_twi_read(dev, AT91_TWI_SR);
+	const unsigned irqstatus = status & at91_twi_read(dev, AT91_TWI_IMR);
+
+	if (!irqstatus)
+		return IRQ_NONE;
+	else if (irqstatus & AT91_TWI_RXRDY)
+		at91_twi_read_next_byte(dev);
+	else if (irqstatus & AT91_TWI_TXRDY)
+		at91_twi_write_next_byte(dev);
+
+	/* catch error flags */
+	dev->transfer_status |= status;
+
+	if (irqstatus & AT91_TWI_TXCOMP) {
+		at91_disable_twi_interrupts(dev);
+		complete(&dev->cmd_complete);
 	}
 
-	return 0;
+	return IRQ_HANDLED;
 }
 
-static int xfer_write(struct i2c_adapter *adap, unsigned char *buf, int length)
+static int at91_do_twi_transfer(struct at91_twi_dev *dev)
 {
-	/* Load first byte into transmitter */
-	at91_twi_write(AT91_TWI_THR, *buf++);
+	int ret;
+	bool has_unre_flag = dev->pdata->has_unre_flag;
 
-	/* Send Start */
-	at91_twi_write(AT91_TWI_CR, AT91_TWI_START);
+	dev_dbg(dev->dev, "transfer: %s %d bytes.\n",
+		(dev->msg->flags & I2C_M_RD) ? "read" : "write", dev->buf_len);
 
-	do {
-		if (!at91_poll_status(AT91_TWI_TXRDY)) {
-			dev_dbg(&adap->dev, "TXRDY timeout\n");
-			return -ETIMEDOUT;
-		}
+	INIT_COMPLETION(dev->cmd_complete);
+	dev->transfer_status = 0;
+	if (dev->msg->flags & I2C_M_RD) {
+		unsigned start_flags = AT91_TWI_START;
 
-		length--;	/* byte was transmitted */
+		if (at91_twi_read(dev, AT91_TWI_SR) & AT91_TWI_RXRDY) {
+			dev_err(dev->dev, "RXRDY still set!");
+			at91_twi_read(dev, AT91_TWI_RHR);
+		}
 
-		if (length > 0)		/* more data to send? */
-			at91_twi_write(AT91_TWI_THR, *buf++);
-	} while (length);
+		/* if only one byte is to be read, immediately stop transfer */
+		if (dev->buf_len <= 1 && !(dev->msg->flags & I2C_M_RECV_LEN))
+			start_flags |= AT91_TWI_STOP;
+		at91_twi_write(dev, AT91_TWI_CR, start_flags);
+		at91_twi_write(dev, AT91_TWI_IER,
+			       AT91_TWI_TXCOMP | AT91_TWI_RXRDY);
+	} else {
+		at91_twi_write_next_byte(dev);
+		at91_twi_write(dev, AT91_TWI_IER,
+			       AT91_TWI_TXCOMP | AT91_TWI_TXRDY);
+	}
 
-	/* Send Stop */
-	at91_twi_write(AT91_TWI_CR, AT91_TWI_STOP);
+	ret = wait_for_completion_interruptible_timeout(&dev->cmd_complete,
+							dev->adapter.timeout);
+	if (ret == 0) {
+		dev_err(dev->dev, "controller timed out\n");
+		at91_init_twi_bus(dev);
+		return -ETIMEDOUT;
+	}
+	if (dev->transfer_status & AT91_TWI_NACK) {
+		dev_dbg(dev->dev, "received nack\n");
+		return -EREMOTEIO;
+	}
+	if (dev->transfer_status & AT91_TWI_OVRE) {
+		dev_err(dev->dev, "overrun while reading\n");
+		return -EIO;
+	}
+	if (has_unre_flag && dev->transfer_status & AT91_TWI_UNRE) {
+		dev_err(dev->dev, "underrun while writing\n");
+		return -EIO;
+	}
+	dev_dbg(dev->dev, "transfer complete\n");
 
 	return 0;
 }
 
-/*
- * Generic i2c master transfer entrypoint.
- *
- * Note: We do not use Atmel's feature of storing the "internal device address".
- * Instead the "internal device address" has to be written using a separate
- * i2c message.
- * http://lists.arm.linux.org.uk/pipermail/linux-arm-kernel/2004-September/024411.html
- */
-static int at91_xfer(struct i2c_adapter *adap, struct i2c_msg *pmsg, int num)
+static int at91_twi_xfer(struct i2c_adapter *adap, struct i2c_msg *msg, int num)
 {
-	int i, ret;
+	struct at91_twi_dev *dev = i2c_get_adapdata(adap);
+	int ret;
+	unsigned int_addr_flag = 0;
+	struct i2c_msg *m_start = msg;
 
 	dev_dbg(&adap->dev, "at91_xfer: processing %d messages:\n", num);
 
-	for (i = 0; i < num; i++) {
-		dev_dbg(&adap->dev, " #%d: %sing %d byte%s %s 0x%02x\n", i,
-			pmsg->flags & I2C_M_RD ? "read" : "writ",
-			pmsg->len, pmsg->len > 1 ? "s" : "",
-			pmsg->flags & I2C_M_RD ? "from" : "to",	pmsg->addr);
-
-		at91_twi_write(AT91_TWI_MMR, (pmsg->addr << 16)
-			| ((pmsg->flags & I2C_M_RD) ? AT91_TWI_MREAD : 0));
-
-		if (pmsg->len && pmsg->buf) {	/* sanity check */
-			if (pmsg->flags & I2C_M_RD)
-				ret = xfer_read(adap, pmsg->buf, pmsg->len);
-			else
-				ret = xfer_write(adap, pmsg->buf, pmsg->len);
-
-			if (ret)
-				return ret;
-
-			/* Wait until transfer is finished */
-			if (!at91_poll_status(AT91_TWI_TXCOMP)) {
-				dev_dbg(&adap->dev, "TXCOMP timeout\n");
-				return -ETIMEDOUT;
-			}
+	/*
+	 * The hardware can handle at most two messages concatenated by a
+	 * repeated start via it's internal address feature.
+	 */
+	if (num > 2) {
+		dev_err(dev->dev,
+			"cannot handle more than two concatenated messages.\n");
+		return 0;
+	} else if (num == 2) {
+		int internal_address = 0;
+		int i;
+
+		if (msg->flags & I2C_M_RD) {
+			dev_err(dev->dev, "first transfer must be write.\n");
+			return -EINVAL;
 		}
-		dev_dbg(&adap->dev, "transfer complete\n");
-		pmsg++;		/* next message */
+		if (msg->len > 3) {
+			dev_err(dev->dev, "first message size must be <= 3.\n");
+			return -EINVAL;
+		}
+
+		/* 1st msg is put into the internal address, start with 2nd */
+		m_start = &msg[1];
+		for (i = 0; i < msg->len; ++i) {
+			const unsigned addr = msg->buf[msg->len - 1 - i];
+
+			internal_address |= addr << (8 * i);
+			int_addr_flag += AT91_TWI_IADRSZ_1;
+		}
+		at91_twi_write(dev, AT91_TWI_IADR, internal_address);
 	}
-	return i;
+
+	at91_twi_write(dev, AT91_TWI_MMR, (m_start->addr << 16) | int_addr_flag
+		       | ((m_start->flags & I2C_M_RD) ? AT91_TWI_MREAD : 0));
+
+	dev->buf_len = m_start->len;
+	dev->buf = m_start->buf;
+	dev->msg = m_start;
+
+	ret = at91_do_twi_transfer(dev);
+
+	return (ret < 0) ? ret : num;
 }
 
-/*
- * Return list of supported functionality.
- */
-static u32 at91_func(struct i2c_adapter *adapter)
+static u32 at91_twi_func(struct i2c_adapter *adapter)
 {
-	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL
+		| I2C_FUNC_SMBUS_READ_BLOCK_DATA;
 }
 
-static struct i2c_algorithm at91_algorithm = {
-	.master_xfer	= at91_xfer,
-	.functionality	= at91_func,
+static struct i2c_algorithm at91_twi_algorithm = {
+	.master_xfer	= at91_twi_xfer,
+	.functionality	= at91_twi_func,
 };
 
-/*
- * Main initialization routine.
- */
-static int __devinit at91_i2c_probe(struct platform_device *pdev)
-{
-	struct i2c_adapter *adapter;
-	struct resource *res;
-	int rc;
+static struct at91_twi_pdata at91rm9200_config = {
+	.clk_max_div = 5,
+	.clk_offset = 3,
+	.has_unre_flag = true,
+};
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res)
-		return -ENXIO;
+static struct at91_twi_pdata at91sam9261_config = {
+	.clk_max_div = 5,
+	.clk_offset = 4,
+	.has_unre_flag = false,
+};
 
-	if (!request_mem_region(res->start, resource_size(res), "at91_i2c"))
-		return -EBUSY;
+static struct at91_twi_pdata at91sam9260_config = {
+	.clk_max_div = 7,
+	.clk_offset = 4,
+	.has_unre_flag = false,
+};
+
+static struct at91_twi_pdata at91sam9g20_config = {
+	.clk_max_div = 7,
+	.clk_offset = 4,
+	.has_unre_flag = false,
+};
+
+static struct at91_twi_pdata at91sam9g10_config = {
+	.clk_max_div = 7,
+	.clk_offset = 4,
+	.has_unre_flag = false,
+};
 
-	twi_base = ioremap(res->start, resource_size(res));
-	if (!twi_base) {
-		rc = -ENOMEM;
-		goto fail0;
+static struct at91_twi_pdata at91sam9x5_config = {
+	.clk_max_div = 7,
+	.clk_offset = 4,
+	.has_unre_flag = false,
+};
+
+static const struct platform_device_id at91_twi_devtypes[] = {
+	{
+		.name = "i2c-at91rm9200",
+		.driver_data = (unsigned long) &at91rm9200_config,
+	}, {
+		.name = "i2c-at91sam9261",
+		.driver_data = (unsigned long) &at91sam9261_config,
+	}, {
+		.name = "i2c-at91sam9260",
+		.driver_data = (unsigned long) &at91sam9260_config,
+	}, {
+		.name = "i2c-at91sam9g20",
+		.driver_data = (unsigned long) &at91sam9g20_config,
+	}, {
+		.name = "i2c-at91sam9g10",
+		.driver_data = (unsigned long) &at91sam9g10_config,
+	}, {
+		/* sentinel */
 	}
+};
 
-	twi_clk = clk_get(NULL, "twi_clk");
-	if (IS_ERR(twi_clk)) {
-		dev_err(&pdev->dev, "no clock defined\n");
-		rc = -ENODEV;
-		goto fail1;
+#if defined(CONFIG_OF)
+static const struct of_device_id atmel_twi_dt_ids[] = {
+	{
+		.compatible = "atmel,at91sam9260-i2c",
+		.data = &at91sam9260_config,
+	} , {
+		.compatible = "atmel,at91sam9g20-i2c",
+		.data = &at91sam9g20_config,
+	} , {
+		.compatible = "atmel,at91sam9g10-i2c",
+		.data = &at91sam9g10_config,
+	}, {
+		.compatible = "atmel,at91sam9x5-i2c",
+		.data = &at91sam9x5_config,
+	}, {
+		/* sentinel */
 	}
+};
+MODULE_DEVICE_TABLE(of, atmel_twi_dt_ids);
+#else
+#define atmel_twi_dt_ids NULL
+#endif
 
-	adapter = kzalloc(sizeof(struct i2c_adapter), GFP_KERNEL);
-	if (adapter == NULL) {
-		dev_err(&pdev->dev, "can't allocate inteface!\n");
-		rc = -ENOMEM;
-		goto fail2;
+static struct at91_twi_pdata * __devinit at91_twi_get_driver_data(
+					struct platform_device *pdev)
+{
+	if (pdev->dev.of_node) {
+		const struct of_device_id *match;
+		match = of_match_node(atmel_twi_dt_ids, pdev->dev.of_node);
+		if (!match)
+			return NULL;
+		return match->data;
 	}
-	snprintf(adapter->name, sizeof(adapter->name), "AT91");
-	adapter->algo = &at91_algorithm;
-	adapter->class = I2C_CLASS_HWMON;
-	adapter->dev.parent = &pdev->dev;
-	/* adapter->id == 0 ... only one TWI controller for now */
+	return (struct at91_twi_pdata *) platform_get_device_id(pdev)->driver_data;
+}
+
+static int __devinit at91_twi_probe(struct platform_device *pdev)
+{
+	struct at91_twi_dev *dev;
+	struct resource *mem;
+	int rc;
+
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+	init_completion(&dev->cmd_complete);
+	dev->dev = &pdev->dev;
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!mem)
+		return -ENODEV;
+
+	dev->pdata = at91_twi_get_driver_data(pdev);
+	if (!dev->pdata)
+		return -ENODEV;
 
-	platform_set_drvdata(pdev, adapter);
+	dev->base = devm_request_and_ioremap(&pdev->dev, mem);
+	if (!dev->base)
+		return -EBUSY;
 
-	clk_enable(twi_clk);		/* enable peripheral clock */
-	at91_twi_hwinit();		/* initialize TWI controller */
+	dev->irq = platform_get_irq(pdev, 0);
+	if (dev->irq < 0)
+		return dev->irq;
 
-	rc = i2c_add_numbered_adapter(adapter);
+	rc = devm_request_irq(&pdev->dev, dev->irq, atmel_twi_interrupt, 0,
+			 dev_name(dev->dev), dev);
 	if (rc) {
-		dev_err(&pdev->dev, "Adapter %s registration failed\n",
-				adapter->name);
-		goto fail3;
+		dev_err(dev->dev, "Cannot get irq %d: %d\n", dev->irq, rc);
+		return rc;
 	}
 
-	dev_info(&pdev->dev, "AT91 i2c bus driver.\n");
-	return 0;
+	platform_set_drvdata(pdev, dev);
 
-fail3:
-	platform_set_drvdata(pdev, NULL);
-	kfree(adapter);
-	clk_disable(twi_clk);
-fail2:
-	clk_put(twi_clk);
-fail1:
-	iounmap(twi_base);
-fail0:
-	release_mem_region(res->start, resource_size(res));
+	dev->clk = devm_clk_get(dev->dev, NULL);
+	if (IS_ERR(dev->clk)) {
+		dev_err(dev->dev, "no clock defined\n");
+		return -ENODEV;
+	}
+	clk_prepare_enable(dev->clk);
+
+	at91_calc_twi_clock(dev, TWI_CLK_HZ);
+	at91_init_twi_bus(dev);
+
+	snprintf(dev->adapter.name, sizeof(dev->adapter.name), "AT91");
+	i2c_set_adapdata(&dev->adapter, dev);
+	dev->adapter.owner = THIS_MODULE;
+	dev->adapter.class = I2C_CLASS_HWMON;
+	dev->adapter.algo = &at91_twi_algorithm;
+	dev->adapter.dev.parent = dev->dev;
+	dev->adapter.nr = pdev->id;
+	dev->adapter.timeout = AT91_I2C_TIMEOUT;
+	dev->adapter.dev.of_node = pdev->dev.of_node;
+
+	rc = i2c_add_numbered_adapter(&dev->adapter);
+	if (rc) {
+		dev_err(dev->dev, "Adapter %s registration failed\n",
+			dev->adapter.name);
+		clk_disable_unprepare(dev->clk);
+		return rc;
+	}
 
-	return rc;
+	of_i2c_register_devices(&dev->adapter);
+
+	dev_info(dev->dev, "AT91 i2c bus driver.\n");
+	return 0;
 }
 
-static int __devexit at91_i2c_remove(struct platform_device *pdev)
+static int __devexit at91_twi_remove(struct platform_device *pdev)
 {
-	struct i2c_adapter *adapter = platform_get_drvdata(pdev);
-	struct resource *res;
+	struct at91_twi_dev *dev = platform_get_drvdata(pdev);
 	int rc;
 
-	rc = i2c_del_adapter(adapter);
-	platform_set_drvdata(pdev, NULL);
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	iounmap(twi_base);
-	release_mem_region(res->start, resource_size(res));
-
-	clk_disable(twi_clk);		/* disable peripheral clock */
-	clk_put(twi_clk);
+	rc = i2c_del_adapter(&dev->adapter);
+	clk_disable_unprepare(dev->clk);
 
 	return rc;
 }
 
 #ifdef CONFIG_PM
 
-/* NOTE: could save a few mA by keeping clock off outside of at91_xfer... */
-
-static int at91_i2c_suspend(struct device *dev)
+static int at91_twi_runtime_suspend(struct device *dev)
 {
-	clk_disable(twi_clk);
+	struct at91_twi_dev *twi_dev = dev_get_drvdata(dev);
+
+	clk_disable(twi_dev->clk);
+
 	return 0;
 }
 
-static int at91_i2c_resume(struct device *dev)
+static int at91_twi_runtime_resume(struct device *dev)
 {
-	return clk_enable(twi_clk);
+	struct at91_twi_dev *twi_dev = dev_get_drvdata(dev);
+
+	return clk_enable(twi_dev->clk);
 }
 
-static SIMPLE_DEV_PM_OPS(at91_i2c_pm, at91_i2c_suspend, at91_i2c_resume);
-#define AT91_I2C_PM	(&at91_i2c_pm)
+static const struct dev_pm_ops at91_twi_pm = {
+	.runtime_suspend	= at91_twi_runtime_suspend,
+	.runtime_resume		= at91_twi_runtime_resume,
+};
 
+#define at91_twi_pm_ops (&at91_twi_pm)
 #else
-#define AT91_I2C_PM	NULL
+#define at91_twi_pm_ops NULL
 #endif
 
-static struct platform_driver at91_i2c_driver = {
-	.probe		= at91_i2c_probe,
-	.remove		= __devexit_p(at91_i2c_remove),
+static struct platform_driver at91_twi_driver = {
+	.probe		= at91_twi_probe,
+	.remove		= __devexit_p(at91_twi_remove),
+	.id_table	= at91_twi_devtypes,
 	.driver		= {
 		.name	= "at91_i2c",
 		.owner	= THIS_MODULE,
-		.pm	= AT91_I2C_PM,
+		.of_match_table = atmel_twi_dt_ids,
+		.pm	= at91_twi_pm_ops,
 	},
 };
 
-module_platform_driver(at91_i2c_driver);
+static int __init at91_twi_init(void)
+{
+	return platform_driver_register(&at91_twi_driver);
+}
+
+static void __exit at91_twi_exit(void)
+{
+	platform_driver_unregister(&at91_twi_driver);
+}
+
+subsys_initcall(at91_twi_init);
+module_exit(at91_twi_exit);
 
-MODULE_AUTHOR("Rick Bronson");
+MODULE_AUTHOR("Nikolaus Voss <n.voss@weinmann.de>");
 MODULE_DESCRIPTION("I2C (TWI) driver for Atmel AT91");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("platform:at91_i2c");
diff --git a/drivers/i2c/busses/i2c-davinci.c b/drivers/i2c/busses/i2c-davinci.c
index 79a2542d8c4..6a0a5531944 100644
--- a/drivers/i2c/busses/i2c-davinci.c
+++ b/drivers/i2c/busses/i2c-davinci.c
@@ -38,6 +38,8 @@
 #include <linux/slab.h>
 #include <linux/cpufreq.h>
 #include <linux/gpio.h>
+#include <linux/of_i2c.h>
+#include <linux/of_device.h>
 
 #include <mach/hardware.h>
 #include <linux/platform_data/i2c-davinci.h>
@@ -114,6 +116,7 @@ struct davinci_i2c_dev {
 	struct completion	xfr_complete;
 	struct notifier_block	freq_transition;
 #endif
+	struct davinci_i2c_platform_data *pdata;
 };
 
 /* default platform data to use if not supplied in the platform_device */
@@ -155,7 +158,7 @@ static void generic_i2c_clock_pulse(unsigned int scl_pin)
 static void i2c_recover_bus(struct davinci_i2c_dev *dev)
 {
 	u32 flag = 0;
-	struct davinci_i2c_platform_data *pdata = dev->dev->platform_data;
+	struct davinci_i2c_platform_data *pdata = dev->pdata;
 
 	dev_err(dev->dev, "initiating i2c bus recovery\n");
 	/* Send NACK to the slave */
@@ -163,8 +166,7 @@ static void i2c_recover_bus(struct davinci_i2c_dev *dev)
 	flag |=  DAVINCI_I2C_MDR_NACK;
 	/* write the data into mode register */
 	davinci_i2c_write_reg(dev, DAVINCI_I2C_MDR_REG, flag);
-	if (pdata)
-		generic_i2c_clock_pulse(pdata->scl_pin);
+	generic_i2c_clock_pulse(pdata->scl_pin);
 	/* Send STOP */
 	flag = davinci_i2c_read_reg(dev, DAVINCI_I2C_MDR_REG);
 	flag |= DAVINCI_I2C_MDR_STP;
@@ -187,7 +189,7 @@ static inline void davinci_i2c_reset_ctrl(struct davinci_i2c_dev *i2c_dev,
 
 static void i2c_davinci_calc_clk_dividers(struct davinci_i2c_dev *dev)
 {
-	struct davinci_i2c_platform_data *pdata = dev->dev->platform_data;
+	struct davinci_i2c_platform_data *pdata = dev->pdata;
 	u16 psc;
 	u32 clk;
 	u32 d;
@@ -235,10 +237,7 @@ static void i2c_davinci_calc_clk_dividers(struct davinci_i2c_dev *dev)
  */
 static int i2c_davinci_init(struct davinci_i2c_dev *dev)
 {
-	struct davinci_i2c_platform_data *pdata = dev->dev->platform_data;
-
-	if (!pdata)
-		pdata = &davinci_i2c_platform_data_default;
+	struct davinci_i2c_platform_data *pdata = dev->pdata;
 
 	/* put I2C into reset */
 	davinci_i2c_reset_ctrl(dev, 0);
@@ -260,6 +259,7 @@ static int i2c_davinci_init(struct davinci_i2c_dev *dev)
 	dev_dbg(dev->dev, "bus_freq = %dkHz, bus_delay = %d\n",
 		pdata->bus_freq, pdata->bus_delay);
 
+
 	/* Take the I2C module out of reset: */
 	davinci_i2c_reset_ctrl(dev, 1);
 
@@ -308,13 +308,11 @@ static int
 i2c_davinci_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg, int stop)
 {
 	struct davinci_i2c_dev *dev = i2c_get_adapdata(adap);
-	struct davinci_i2c_platform_data *pdata = dev->dev->platform_data;
+	struct davinci_i2c_platform_data *pdata = dev->pdata;
 	u32 flag;
 	u16 w;
 	int r;
 
-	if (!pdata)
-		pdata = &davinci_i2c_platform_data_default;
 	/* Introduce a delay, required for some boards (e.g Davinci EVM) */
 	if (pdata->bus_delay)
 		udelay(pdata->bus_delay);
@@ -635,6 +633,12 @@ static struct i2c_algorithm i2c_davinci_algo = {
 	.functionality	= i2c_davinci_func,
 };
 
+static const struct of_device_id davinci_i2c_of_match[] = {
+	{.compatible = "ti,davinci-i2c", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, davinci_i2c_of_match);
+
 static int davinci_i2c_probe(struct platform_device *pdev)
 {
 	struct davinci_i2c_dev *dev;
@@ -674,14 +678,33 @@ static int davinci_i2c_probe(struct platform_device *pdev)
 #endif
 	dev->dev = get_device(&pdev->dev);
 	dev->irq = irq->start;
+	dev->pdata = dev->dev->platform_data;
 	platform_set_drvdata(pdev, dev);
 
+	if (!dev->pdata && pdev->dev.of_node) {
+		u32 prop;
+
+		dev->pdata = devm_kzalloc(&pdev->dev,
+			sizeof(struct davinci_i2c_platform_data), GFP_KERNEL);
+		if (!dev->pdata) {
+			r = -ENOMEM;
+			goto err_free_mem;
+		}
+		memcpy(dev->pdata, &davinci_i2c_platform_data_default,
+			sizeof(struct davinci_i2c_platform_data));
+		if (!of_property_read_u32(pdev->dev.of_node, "clock-frequency",
+			&prop))
+			dev->pdata->bus_freq = prop / 1000;
+	} else if (!dev->pdata) {
+		dev->pdata = &davinci_i2c_platform_data_default;
+	}
+
 	dev->clk = clk_get(&pdev->dev, NULL);
 	if (IS_ERR(dev->clk)) {
 		r = -ENODEV;
 		goto err_free_mem;
 	}
-	clk_enable(dev->clk);
+	clk_prepare_enable(dev->clk);
 
 	dev->base = ioremap(mem->start, resource_size(mem));
 	if (!dev->base) {
@@ -711,6 +734,7 @@ static int davinci_i2c_probe(struct platform_device *pdev)
 	adap->algo = &i2c_davinci_algo;
 	adap->dev.parent = &pdev->dev;
 	adap->timeout = DAVINCI_I2C_TIMEOUT;
+	adap->dev.of_node = pdev->dev.of_node;
 
 	adap->nr = pdev->id;
 	r = i2c_add_numbered_adapter(adap);
@@ -718,6 +742,7 @@ static int davinci_i2c_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failure adding adapter\n");
 		goto err_free_irq;
 	}
+	of_i2c_register_devices(adap);
 
 	return 0;
 
@@ -726,7 +751,7 @@ err_free_irq:
 err_unuse_clocks:
 	iounmap(dev->base);
 err_mem_ioremap:
-	clk_disable(dev->clk);
+	clk_disable_unprepare(dev->clk);
 	clk_put(dev->clk);
 	dev->clk = NULL;
 err_free_mem:
@@ -750,7 +775,7 @@ static int davinci_i2c_remove(struct platform_device *pdev)
 	i2c_del_adapter(&dev->adapter);
 	put_device(&pdev->dev);
 
-	clk_disable(dev->clk);
+	clk_disable_unprepare(dev->clk);
 	clk_put(dev->clk);
 	dev->clk = NULL;
 
@@ -772,7 +797,7 @@ static int davinci_i2c_suspend(struct device *dev)
 
 	/* put I2C into reset */
 	davinci_i2c_reset_ctrl(i2c_dev, 0);
-	clk_disable(i2c_dev->clk);
+	clk_disable_unprepare(i2c_dev->clk);
 
 	return 0;
 }
@@ -782,7 +807,7 @@ static int davinci_i2c_resume(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct davinci_i2c_dev *i2c_dev = platform_get_drvdata(pdev);
 
-	clk_enable(i2c_dev->clk);
+	clk_prepare_enable(i2c_dev->clk);
 	/* take I2C out of reset */
 	davinci_i2c_reset_ctrl(i2c_dev, 1);
 
@@ -809,6 +834,7 @@ static struct platform_driver davinci_i2c_driver = {
 		.name	= "i2c_davinci",
 		.owner	= THIS_MODULE,
 		.pm	= davinci_i2c_pm_ops,
+		.of_match_table = of_match_ptr(davinci_i2c_of_match),
 	},
 };
 
diff --git a/drivers/i2c/busses/i2c-designware-core.c b/drivers/i2c/busses/i2c-designware-core.c
index 7b8ebbefb58..cbba7db9ad5 100644
--- a/drivers/i2c/busses/i2c-designware-core.c
+++ b/drivers/i2c/busses/i2c-designware-core.c
@@ -370,7 +370,7 @@ static void i2c_dw_xfer_init(struct dw_i2c_dev *dev)
  * messages into the tx buffer.  Even if the size of i2c_msg data is
  * longer than the size of the tx buffer, it handles everything.
  */
-void
+static void
 i2c_dw_xfer_msg(struct dw_i2c_dev *dev)
 {
 	struct i2c_msg *msgs = dev->msgs;
diff --git a/drivers/i2c/busses/i2c-i801.c b/drivers/i2c/busses/i2c-i801.c
index 33e9b0c09af..37793156bd9 100644
--- a/drivers/i2c/busses/i2c-i801.c
+++ b/drivers/i2c/busses/i2c-i801.c
@@ -80,6 +80,13 @@
 #include <linux/dmi.h>
 #include <linux/slab.h>
 #include <linux/wait.h>
+#include <linux/err.h>
+
+#if defined CONFIG_I2C_MUX || defined CONFIG_I2C_MUX_MODULE
+#include <linux/gpio.h>
+#include <linux/i2c-mux-gpio.h>
+#include <linux/platform_device.h>
+#endif
 
 /* I801 SMBus address offsets */
 #define SMBHSTSTS(p)	(0 + (p)->smba)
@@ -158,6 +165,15 @@
 #define PCI_DEVICE_ID_INTEL_LYNXPOINT_SMBUS	0x8c22
 #define PCI_DEVICE_ID_INTEL_LYNXPOINT_LP_SMBUS	0x9c22
 
+struct i801_mux_config {
+	char *gpio_chip;
+	unsigned values[3];
+	int n_values;
+	unsigned classes[3];
+	unsigned gpios[2];		/* Relative to gpio_chip->base */
+	int n_gpios;
+};
+
 struct i801_priv {
 	struct i2c_adapter adapter;
 	unsigned long smba;
@@ -175,6 +191,11 @@ struct i801_priv {
 	int count;
 	int len;
 	u8 *data;
+
+#if defined CONFIG_I2C_MUX || defined CONFIG_I2C_MUX_MODULE
+	const struct i801_mux_config *mux_drvdata;
+	struct platform_device *mux_pdev;
+#endif
 };
 
 static struct pci_driver i801_driver;
@@ -900,6 +921,165 @@ static void __init input_apanel_init(void) {}
 static void __devinit i801_probe_optional_slaves(struct i801_priv *priv) {}
 #endif	/* CONFIG_X86 && CONFIG_DMI */
 
+#if defined CONFIG_I2C_MUX || defined CONFIG_I2C_MUX_MODULE
+static struct i801_mux_config i801_mux_config_asus_z8_d12 = {
+	.gpio_chip = "gpio_ich",
+	.values = { 0x02, 0x03 },
+	.n_values = 2,
+	.classes = { I2C_CLASS_SPD, I2C_CLASS_SPD },
+	.gpios = { 52, 53 },
+	.n_gpios = 2,
+};
+
+static struct i801_mux_config i801_mux_config_asus_z8_d18 = {
+	.gpio_chip = "gpio_ich",
+	.values = { 0x02, 0x03, 0x01 },
+	.n_values = 3,
+	.classes = { I2C_CLASS_SPD, I2C_CLASS_SPD, I2C_CLASS_SPD },
+	.gpios = { 52, 53 },
+	.n_gpios = 2,
+};
+
+static struct dmi_system_id __devinitdata mux_dmi_table[] = {
+	{
+		.matches = {
+			DMI_MATCH(DMI_BOARD_VENDOR, "ASUSTeK Computer INC."),
+			DMI_MATCH(DMI_BOARD_NAME, "Z8NA-D6(C)"),
+		},
+		.driver_data = &i801_mux_config_asus_z8_d12,
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_BOARD_VENDOR, "ASUSTeK Computer INC."),
+			DMI_MATCH(DMI_BOARD_NAME, "Z8P(N)E-D12(X)"),
+		},
+		.driver_data = &i801_mux_config_asus_z8_d12,
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_BOARD_VENDOR, "ASUSTeK Computer INC."),
+			DMI_MATCH(DMI_BOARD_NAME, "Z8NH-D12"),
+		},
+		.driver_data = &i801_mux_config_asus_z8_d12,
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_BOARD_VENDOR, "ASUSTeK Computer INC."),
+			DMI_MATCH(DMI_BOARD_NAME, "Z8PH-D12/IFB"),
+		},
+		.driver_data = &i801_mux_config_asus_z8_d12,
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_BOARD_VENDOR, "ASUSTeK Computer INC."),
+			DMI_MATCH(DMI_BOARD_NAME, "Z8NR-D12"),
+		},
+		.driver_data = &i801_mux_config_asus_z8_d12,
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_BOARD_VENDOR, "ASUSTeK Computer INC."),
+			DMI_MATCH(DMI_BOARD_NAME, "Z8P(N)H-D12"),
+		},
+		.driver_data = &i801_mux_config_asus_z8_d12,
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_BOARD_VENDOR, "ASUSTeK Computer INC."),
+			DMI_MATCH(DMI_BOARD_NAME, "Z8PG-D18"),
+		},
+		.driver_data = &i801_mux_config_asus_z8_d18,
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_BOARD_VENDOR, "ASUSTeK Computer INC."),
+			DMI_MATCH(DMI_BOARD_NAME, "Z8PE-D18"),
+		},
+		.driver_data = &i801_mux_config_asus_z8_d18,
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_BOARD_VENDOR, "ASUSTeK Computer INC."),
+			DMI_MATCH(DMI_BOARD_NAME, "Z8PS-D12"),
+		},
+		.driver_data = &i801_mux_config_asus_z8_d12,
+	},
+	{ }
+};
+
+/* Setup multiplexing if needed */
+static int __devinit i801_add_mux(struct i801_priv *priv)
+{
+	struct device *dev = &priv->adapter.dev;
+	const struct i801_mux_config *mux_config;
+	struct i2c_mux_gpio_platform_data gpio_data;
+	int err;
+
+	if (!priv->mux_drvdata)
+		return 0;
+	mux_config = priv->mux_drvdata;
+
+	/* Prepare the platform data */
+	memset(&gpio_data, 0, sizeof(struct i2c_mux_gpio_platform_data));
+	gpio_data.parent = priv->adapter.nr;
+	gpio_data.values = mux_config->values;
+	gpio_data.n_values = mux_config->n_values;
+	gpio_data.classes = mux_config->classes;
+	gpio_data.gpio_chip = mux_config->gpio_chip;
+	gpio_data.gpios = mux_config->gpios;
+	gpio_data.n_gpios = mux_config->n_gpios;
+	gpio_data.idle = I2C_MUX_GPIO_NO_IDLE;
+
+	/* Register the mux device */
+	priv->mux_pdev = platform_device_register_data(dev, "i2c-mux-gpio",
+				PLATFORM_DEVID_AUTO, &gpio_data,
+				sizeof(struct i2c_mux_gpio_platform_data));
+	if (IS_ERR(priv->mux_pdev)) {
+		err = PTR_ERR(priv->mux_pdev);
+		priv->mux_pdev = NULL;
+		dev_err(dev, "Failed to register i2c-mux-gpio device\n");
+		return err;
+	}
+
+	return 0;
+}
+
+static void __devexit i801_del_mux(struct i801_priv *priv)
+{
+	if (priv->mux_pdev)
+		platform_device_unregister(priv->mux_pdev);
+}
+
+static unsigned int __devinit i801_get_adapter_class(struct i801_priv *priv)
+{
+	const struct dmi_system_id *id;
+	const struct i801_mux_config *mux_config;
+	unsigned int class = I2C_CLASS_HWMON | I2C_CLASS_SPD;
+	int i;
+
+	id = dmi_first_match(mux_dmi_table);
+	if (id) {
+		/* Remove from branch classes from trunk */
+		mux_config = id->driver_data;
+		for (i = 0; i < mux_config->n_values; i++)
+			class &= ~mux_config->classes[i];
+
+		/* Remember for later */
+		priv->mux_drvdata = mux_config;
+	}
+
+	return class;
+}
+#else
+static inline int i801_add_mux(struct i801_priv *priv) { return 0; }
+static inline void i801_del_mux(struct i801_priv *priv) { }
+
+static inline unsigned int i801_get_adapter_class(struct i801_priv *priv)
+{
+	return I2C_CLASS_HWMON | I2C_CLASS_SPD;
+}
+#endif
+
 static int __devinit i801_probe(struct pci_dev *dev,
 				const struct pci_device_id *id)
 {
@@ -913,7 +1093,7 @@ static int __devinit i801_probe(struct pci_dev *dev,
 
 	i2c_set_adapdata(&priv->adapter, priv);
 	priv->adapter.owner = THIS_MODULE;
-	priv->adapter.class = I2C_CLASS_HWMON | I2C_CLASS_SPD;
+	priv->adapter.class = i801_get_adapter_class(priv);
 	priv->adapter.algo = &smbus_algorithm;
 
 	priv->pci_dev = dev;
@@ -1033,6 +1213,8 @@ static int __devinit i801_probe(struct pci_dev *dev,
 	}
 
 	i801_probe_optional_slaves(priv);
+	/* We ignore errors - multiplexing is optional */
+	i801_add_mux(priv);
 
 	pci_set_drvdata(dev, priv);
 
@@ -1052,6 +1234,7 @@ static void __devexit i801_remove(struct pci_dev *dev)
 {
 	struct i801_priv *priv = pci_get_drvdata(dev);
 
+	i801_del_mux(priv);
 	i2c_del_adapter(&priv->adapter);
 	pci_write_config_byte(dev, SMBHSTCFG, priv->original_hstcfg);
 
diff --git a/drivers/i2c/busses/i2c-imx.c b/drivers/i2c/busses/i2c-imx.c
index b7907ba7448..2ef162d148c 100644
--- a/drivers/i2c/busses/i2c-imx.c
+++ b/drivers/i2c/busses/i2c-imx.c
@@ -272,9 +272,9 @@ static void __init i2c_imx_set_clk(struct imx_i2c_struct *i2c_imx,
 
 	/* dev_dbg() can't be used, because adapter is not yet registered */
 #ifdef CONFIG_I2C_DEBUG_BUS
-	printk(KERN_DEBUG "I2C: <%s> I2C_CLK=%d, REQ DIV=%d\n",
+	dev_dbg(&i2c_imx->adapter.dev, "<%s> I2C_CLK=%d, REQ DIV=%d\n",
 		__func__, i2c_clk_rate, div);
-	printk(KERN_DEBUG "I2C: <%s> IFDR[IC]=0x%x, REAL DIV=%d\n",
+	dev_dbg(&i2c_imx->adapter.dev, "<%s> IFDR[IC]=0x%x, REAL DIV=%d\n",
 		__func__, i2c_clk_div[i][1], i2c_clk_div[i][0]);
 #endif
 }
@@ -564,7 +564,7 @@ static int __init i2c_imx_probe(struct platform_device *pdev)
 		resource_size(res), res->start);
 	dev_dbg(&i2c_imx->adapter.dev, "adapter name: \"%s\"\n",
 		i2c_imx->adapter.name);
-	dev_dbg(&i2c_imx->adapter.dev, "IMX I2C adapter registered\n");
+	dev_info(&i2c_imx->adapter.dev, "IMX I2C adapter registered\n");
 
 	return 0;   /* Return OK */
 }
diff --git a/drivers/i2c/busses/i2c-mpc.c b/drivers/i2c/busses/i2c-mpc.c
index 57f7703ce2e..ca86430cb4a 100644
--- a/drivers/i2c/busses/i2c-mpc.c
+++ b/drivers/i2c/busses/i2c-mpc.c
@@ -576,7 +576,23 @@ static int mpc_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num)
 			    mpc_write(i2c, pmsg->addr, pmsg->buf, pmsg->len, i);
 		}
 	}
-	mpc_i2c_stop(i2c);
+	mpc_i2c_stop(i2c); /* Initiate STOP */
+	orig_jiffies = jiffies;
+	/* Wait until STOP is seen, allow up to 1 s */
+	while (readb(i2c->base + MPC_I2C_SR) & CSR_MBB) {
+		if (time_after(jiffies, orig_jiffies + HZ)) {
+			u8 status = readb(i2c->base + MPC_I2C_SR);
+
+			dev_dbg(i2c->dev, "timeout\n");
+			if ((status & (CSR_MCF | CSR_MBB | CSR_RXAK)) != 0) {
+				writeb(status & ~CSR_MAL,
+				       i2c->base + MPC_I2C_SR);
+				mpc_i2c_fixup(i2c);
+			}
+			return -EIO;
+		}
+		cond_resched();
+	}
 	return (ret < 0) ? ret : num;
 }
 
diff --git a/drivers/i2c/busses/i2c-mxs.c b/drivers/i2c/busses/i2c-mxs.c
index 51f05b8520e..1f58197062c 100644
--- a/drivers/i2c/busses/i2c-mxs.c
+++ b/drivers/i2c/busses/i2c-mxs.c
@@ -7,8 +7,6 @@
  *
  * Copyright (C) 2009-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  *
- * TODO: add dma-support if platform-support for it is available
- *
  * 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
@@ -31,9 +29,16 @@
 #include <linux/of.h>
 #include <linux/of_device.h>
 #include <linux/of_i2c.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
+#include <linux/fsl/mxs-dma.h>
 
 #define DRIVER_NAME "mxs-i2c"
 
+static bool use_pioqueue;
+module_param(use_pioqueue, bool, 0);
+MODULE_PARM_DESC(use_pioqueue, "Use PIOQUEUE mode for transfer instead of DMA");
+
 #define MXS_I2C_CTRL0		(0x00)
 #define MXS_I2C_CTRL0_SET	(0x04)
 
@@ -146,6 +151,16 @@ struct mxs_i2c_dev {
 	u32 cmd_err;
 	struct i2c_adapter adapter;
 	const struct mxs_i2c_speed_config *speed;
+
+	/* DMA support components */
+	bool				dma_mode;
+	int				dma_channel;
+	struct dma_chan         	*dmach;
+	struct mxs_dma_data		dma_data;
+	uint32_t			pio_data[2];
+	uint32_t			addr_data;
+	struct scatterlist		sg_io[2];
+	bool				dma_read;
 };
 
 static void mxs_i2c_reset(struct mxs_i2c_dev *i2c)
@@ -157,7 +172,11 @@ static void mxs_i2c_reset(struct mxs_i2c_dev *i2c)
 	writel(i2c->speed->timing2, i2c->regs + MXS_I2C_TIMING2);
 
 	writel(MXS_I2C_IRQ_MASK << 8, i2c->regs + MXS_I2C_CTRL1_SET);
-	writel(MXS_I2C_QUEUECTRL_PIO_QUEUE_MODE,
+	if (i2c->dma_mode)
+		writel(MXS_I2C_QUEUECTRL_PIO_QUEUE_MODE,
+			i2c->regs + MXS_I2C_QUEUECTRL_CLR);
+	else
+		writel(MXS_I2C_QUEUECTRL_PIO_QUEUE_MODE,
 			i2c->regs + MXS_I2C_QUEUECTRL_SET);
 }
 
@@ -248,6 +267,150 @@ static int mxs_i2c_finish_read(struct mxs_i2c_dev *i2c, u8 *buf, int len)
 	return 0;
 }
 
+static void mxs_i2c_dma_finish(struct mxs_i2c_dev *i2c)
+{
+	if (i2c->dma_read) {
+		dma_unmap_sg(i2c->dev, &i2c->sg_io[0], 1, DMA_TO_DEVICE);
+		dma_unmap_sg(i2c->dev, &i2c->sg_io[1], 1, DMA_FROM_DEVICE);
+	} else {
+		dma_unmap_sg(i2c->dev, i2c->sg_io, 2, DMA_TO_DEVICE);
+	}
+}
+
+static void mxs_i2c_dma_irq_callback(void *param)
+{
+	struct mxs_i2c_dev *i2c = param;
+
+	complete(&i2c->cmd_complete);
+	mxs_i2c_dma_finish(i2c);
+}
+
+static int mxs_i2c_dma_setup_xfer(struct i2c_adapter *adap,
+			struct i2c_msg *msg, uint32_t flags)
+{
+	struct dma_async_tx_descriptor *desc;
+	struct mxs_i2c_dev *i2c = i2c_get_adapdata(adap);
+
+	if (msg->flags & I2C_M_RD) {
+		i2c->dma_read = 1;
+		i2c->addr_data = (msg->addr << 1) | I2C_SMBUS_READ;
+
+		/*
+		 * SELECT command.
+		 */
+
+		/* Queue the PIO register write transfer. */
+		i2c->pio_data[0] = MXS_CMD_I2C_SELECT;
+		desc = dmaengine_prep_slave_sg(i2c->dmach,
+					(struct scatterlist *)&i2c->pio_data[0],
+					1, DMA_TRANS_NONE, 0);
+		if (!desc) {
+			dev_err(i2c->dev,
+				"Failed to get PIO reg. write descriptor.\n");
+			goto select_init_pio_fail;
+		}
+
+		/* Queue the DMA data transfer. */
+		sg_init_one(&i2c->sg_io[0], &i2c->addr_data, 1);
+		dma_map_sg(i2c->dev, &i2c->sg_io[0], 1, DMA_TO_DEVICE);
+		desc = dmaengine_prep_slave_sg(i2c->dmach, &i2c->sg_io[0], 1,
+					DMA_MEM_TO_DEV,
+					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+		if (!desc) {
+			dev_err(i2c->dev,
+				"Failed to get DMA data write descriptor.\n");
+			goto select_init_dma_fail;
+		}
+
+		/*
+		 * READ command.
+		 */
+
+		/* Queue the PIO register write transfer. */
+		i2c->pio_data[1] = flags | MXS_CMD_I2C_READ |
+				MXS_I2C_CTRL0_XFER_COUNT(msg->len);
+		desc = dmaengine_prep_slave_sg(i2c->dmach,
+					(struct scatterlist *)&i2c->pio_data[1],
+					1, DMA_TRANS_NONE, DMA_PREP_INTERRUPT);
+		if (!desc) {
+			dev_err(i2c->dev,
+				"Failed to get PIO reg. write descriptor.\n");
+			goto select_init_dma_fail;
+		}
+
+		/* Queue the DMA data transfer. */
+		sg_init_one(&i2c->sg_io[1], msg->buf, msg->len);
+		dma_map_sg(i2c->dev, &i2c->sg_io[1], 1, DMA_FROM_DEVICE);
+		desc = dmaengine_prep_slave_sg(i2c->dmach, &i2c->sg_io[1], 1,
+					DMA_DEV_TO_MEM,
+					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+		if (!desc) {
+			dev_err(i2c->dev,
+				"Failed to get DMA data write descriptor.\n");
+			goto read_init_dma_fail;
+		}
+	} else {
+		i2c->dma_read = 0;
+		i2c->addr_data = (msg->addr << 1) | I2C_SMBUS_WRITE;
+
+		/*
+		 * WRITE command.
+		 */
+
+		/* Queue the PIO register write transfer. */
+		i2c->pio_data[0] = flags | MXS_CMD_I2C_WRITE |
+				MXS_I2C_CTRL0_XFER_COUNT(msg->len + 1);
+		desc = dmaengine_prep_slave_sg(i2c->dmach,
+					(struct scatterlist *)&i2c->pio_data[0],
+					1, DMA_TRANS_NONE, 0);
+		if (!desc) {
+			dev_err(i2c->dev,
+				"Failed to get PIO reg. write descriptor.\n");
+			goto write_init_pio_fail;
+		}
+
+		/* Queue the DMA data transfer. */
+		sg_init_table(i2c->sg_io, 2);
+		sg_set_buf(&i2c->sg_io[0], &i2c->addr_data, 1);
+		sg_set_buf(&i2c->sg_io[1], msg->buf, msg->len);
+		dma_map_sg(i2c->dev, i2c->sg_io, 2, DMA_TO_DEVICE);
+		desc = dmaengine_prep_slave_sg(i2c->dmach, i2c->sg_io, 2,
+					DMA_MEM_TO_DEV,
+					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+		if (!desc) {
+			dev_err(i2c->dev,
+				"Failed to get DMA data write descriptor.\n");
+			goto write_init_dma_fail;
+		}
+	}
+
+	/*
+	 * The last descriptor must have this callback,
+	 * to finish the DMA transaction.
+	 */
+	desc->callback = mxs_i2c_dma_irq_callback;
+	desc->callback_param = i2c;
+
+	/* Start the transfer. */
+	dmaengine_submit(desc);
+	dma_async_issue_pending(i2c->dmach);
+	return 0;
+
+/* Read failpath. */
+read_init_dma_fail:
+	dma_unmap_sg(i2c->dev, &i2c->sg_io[1], 1, DMA_FROM_DEVICE);
+select_init_dma_fail:
+	dma_unmap_sg(i2c->dev, &i2c->sg_io[0], 1, DMA_TO_DEVICE);
+select_init_pio_fail:
+	return -EINVAL;
+
+/* Write failpath. */
+write_init_dma_fail:
+	dma_unmap_sg(i2c->dev, i2c->sg_io, 2, DMA_TO_DEVICE);
+write_init_pio_fail:
+	return -EINVAL;
+}
+
 /*
  * Low level master read/write transaction.
  */
@@ -258,6 +421,8 @@ static int mxs_i2c_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg,
 	int ret;
 	int flags;
 
+	flags = stop ? MXS_I2C_CTRL0_POST_SEND_STOP : 0;
+
 	dev_dbg(i2c->dev, "addr: 0x%04x, len: %d, flags: 0x%x, stop: %d\n",
 		msg->addr, msg->len, msg->flags, stop);
 
@@ -267,23 +432,29 @@ static int mxs_i2c_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg,
 	init_completion(&i2c->cmd_complete);
 	i2c->cmd_err = 0;
 
-	flags = stop ? MXS_I2C_CTRL0_POST_SEND_STOP : 0;
-
-	if (msg->flags & I2C_M_RD)
-		mxs_i2c_pioq_setup_read(i2c, msg->addr, msg->len, flags);
-	else
-		mxs_i2c_pioq_setup_write(i2c, msg->addr, msg->buf, msg->len,
-					flags);
+	if (i2c->dma_mode) {
+		ret = mxs_i2c_dma_setup_xfer(adap, msg, flags);
+		if (ret)
+			return ret;
+	} else {
+		if (msg->flags & I2C_M_RD) {
+			mxs_i2c_pioq_setup_read(i2c, msg->addr,
+						msg->len, flags);
+		} else {
+			mxs_i2c_pioq_setup_write(i2c, msg->addr, msg->buf,
+						msg->len, flags);
+		}
 
-	writel(MXS_I2C_QUEUECTRL_QUEUE_RUN,
+		writel(MXS_I2C_QUEUECTRL_QUEUE_RUN,
 			i2c->regs + MXS_I2C_QUEUECTRL_SET);
+	}
 
 	ret = wait_for_completion_timeout(&i2c->cmd_complete,
 						msecs_to_jiffies(1000));
 	if (ret == 0)
 		goto timeout;
 
-	if ((!i2c->cmd_err) && (msg->flags & I2C_M_RD)) {
+	if (!i2c->dma_mode && !i2c->cmd_err && (msg->flags & I2C_M_RD)) {
 		ret = mxs_i2c_finish_read(i2c, msg->buf, msg->len);
 		if (ret)
 			goto timeout;
@@ -301,6 +472,8 @@ static int mxs_i2c_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg,
 
 timeout:
 	dev_dbg(i2c->dev, "Timeout!\n");
+	if (i2c->dma_mode)
+		mxs_i2c_dma_finish(i2c);
 	mxs_i2c_reset(i2c);
 	return -ETIMEDOUT;
 }
@@ -342,11 +515,13 @@ static irqreturn_t mxs_i2c_isr(int this_irq, void *dev_id)
 		/* MXS_I2C_CTRL1_OVERSIZE_XFER_TERM_IRQ is only for slaves */
 		i2c->cmd_err = -EIO;
 
-	is_last_cmd = (readl(i2c->regs + MXS_I2C_QUEUESTAT) &
-		MXS_I2C_QUEUESTAT_WRITE_QUEUE_CNT_MASK) == 0;
+	if (!i2c->dma_mode) {
+		is_last_cmd = (readl(i2c->regs + MXS_I2C_QUEUESTAT) &
+			MXS_I2C_QUEUESTAT_WRITE_QUEUE_CNT_MASK) == 0;
 
-	if (is_last_cmd || i2c->cmd_err)
-		complete(&i2c->cmd_complete);
+		if (is_last_cmd || i2c->cmd_err)
+			complete(&i2c->cmd_complete);
+	}
 
 	writel(stat, i2c->regs + MXS_I2C_CTRL1_CLR);
 
@@ -358,6 +533,21 @@ static const struct i2c_algorithm mxs_i2c_algo = {
 	.functionality = mxs_i2c_func,
 };
 
+static bool mxs_i2c_dma_filter(struct dma_chan *chan, void *param)
+{
+	struct mxs_i2c_dev *i2c = param;
+
+	if (!mxs_dma_is_apbx(chan))
+		return false;
+
+	if (chan->chan_id != i2c->dma_channel)
+		return false;
+
+	chan->private = &i2c->dma_data;
+
+	return true;
+}
+
 static int mxs_i2c_get_ofdata(struct mxs_i2c_dev *i2c)
 {
 	uint32_t speed;
@@ -365,6 +555,26 @@ static int mxs_i2c_get_ofdata(struct mxs_i2c_dev *i2c)
 	struct device_node *node = dev->of_node;
 	int ret;
 
+	/*
+	 * The MXS I2C DMA mode is prefered and enabled by default.
+	 * The PIO mode is still supported, but should be used only
+	 * for debuging purposes etc.
+	 */
+	i2c->dma_mode = !use_pioqueue;
+	if (!i2c->dma_mode)
+		dev_info(dev, "Using PIOQUEUE mode for I2C transfers!\n");
+
+	/*
+	 * TODO: This is a temporary solution and should be changed
+	 * to use generic DMA binding later when the helpers get in.
+	 */
+	ret = of_property_read_u32(node, "fsl,i2c-dma-channel",
+				   &i2c->dma_channel);
+	if (ret) {
+		dev_warn(dev, "Failed to get DMA channel, using PIOQUEUE!\n");
+		i2c->dma_mode = 0;
+	}
+
 	ret = of_property_read_u32(node, "clock-frequency", &speed);
 	if (ret)
 		dev_warn(dev, "No I2C speed selected, using 100kHz\n");
@@ -384,7 +594,8 @@ static int __devinit mxs_i2c_probe(struct platform_device *pdev)
 	struct pinctrl *pinctrl;
 	struct resource *res;
 	resource_size_t res_size;
-	int err, irq;
+	int err, irq, dmairq;
+	dma_cap_mask_t mask;
 
 	pinctrl = devm_pinctrl_get_select_default(dev);
 	if (IS_ERR(pinctrl))
@@ -395,7 +606,10 @@ static int __devinit mxs_i2c_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res)
+	irq = platform_get_irq(pdev, 0);
+	dmairq = platform_get_irq(pdev, 1);
+
+	if (!res || irq < 0 || dmairq < 0)
 		return -ENOENT;
 
 	res_size = resource_size(res);
@@ -406,10 +620,6 @@ static int __devinit mxs_i2c_probe(struct platform_device *pdev)
 	if (!i2c->regs)
 		return -EBUSY;
 
-	irq = platform_get_irq(pdev, 0);
-	if (irq < 0)
-		return irq;
-
 	err = devm_request_irq(dev, irq, mxs_i2c_isr, 0, dev_name(dev), i2c);
 	if (err)
 		return err;
@@ -423,6 +633,18 @@ static int __devinit mxs_i2c_probe(struct platform_device *pdev)
 			return err;
 	}
 
+	/* Setup the DMA */
+	if (i2c->dma_mode) {
+		dma_cap_zero(mask);
+		dma_cap_set(DMA_SLAVE, mask);
+		i2c->dma_data.chan_irq = dmairq;
+		i2c->dmach = dma_request_channel(mask, mxs_i2c_dma_filter, i2c);
+		if (!i2c->dmach) {
+			dev_err(dev, "Failed to request dma\n");
+			return -ENODEV;
+		}
+	}
+
 	platform_set_drvdata(pdev, i2c);
 
 	/* Do reset to enforce correct startup after pinmuxing */
@@ -458,6 +680,9 @@ static int __devexit mxs_i2c_remove(struct platform_device *pdev)
 	if (ret)
 		return -EBUSY;
 
+	if (i2c->dmach)
+		dma_release_channel(i2c->dmach);
+
 	writel(MXS_I2C_CTRL0_SFTRST, i2c->regs + MXS_I2C_CTRL0_SET);
 
 	platform_set_drvdata(pdev, NULL);
diff --git a/drivers/i2c/busses/i2c-nomadik.c b/drivers/i2c/busses/i2c-nomadik.c
index 61b00edacb0..698d7acb0f0 100644
--- a/drivers/i2c/busses/i2c-nomadik.c
+++ b/drivers/i2c/busses/i2c-nomadik.c
@@ -22,9 +22,10 @@
 #include <linux/err.h>
 #include <linux/clk.h>
 #include <linux/io.h>
-#include <linux/regulator/consumer.h>
 #include <linux/pm_runtime.h>
 #include <linux/platform_data/i2c-nomadik.h>
+#include <linux/of.h>
+#include <linux/of_i2c.h>
 
 #define DRIVER_NAME "nmk-i2c"
 
@@ -146,7 +147,6 @@ struct i2c_nmk_client {
  * @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 {
@@ -160,7 +160,6 @@ struct nmk_i2c_dev {
 	int				stop;
 	struct completion		xfer_complete;
 	int				result;
-	struct regulator		*regulator;
 	bool				busy;
 };
 
@@ -643,8 +642,6 @@ static int nmk_i2c_xfer(struct i2c_adapter *i2c_adap,
 
 	dev->busy = true;
 
-	if (dev->regulator)
-		regulator_enable(dev->regulator);
 	pm_runtime_get_sync(&dev->adev->dev);
 
 	clk_enable(dev->clk);
@@ -676,8 +673,6 @@ static int nmk_i2c_xfer(struct i2c_adapter *i2c_adap,
 out:
 	clk_disable(dev->clk);
 	pm_runtime_put_sync(&dev->adev->dev);
-	if (dev->regulator)
-		regulator_disable(dev->regulator);
 
 	dev->busy = false;
 
@@ -920,18 +915,42 @@ static struct nmk_i2c_controller u8500_i2c = {
 	.sm             = I2C_FREQ_MODE_FAST,
 };
 
+static void nmk_i2c_of_probe(struct device_node *np,
+			struct nmk_i2c_controller *pdata)
+{
+	of_property_read_u32(np, "clock-frequency", &pdata->clk_freq);
+
+	/* This driver only supports 'standard' and 'fast' modes of operation. */
+	if (pdata->clk_freq <= 100000)
+		pdata->sm = I2C_FREQ_MODE_STANDARD;
+	else
+		pdata->sm = I2C_FREQ_MODE_FAST;
+}
+
 static atomic_t adapter_id = ATOMIC_INIT(0);
 
 static int nmk_i2c_probe(struct amba_device *adev, const struct amba_id *id)
 {
 	int ret = 0;
 	struct nmk_i2c_controller *pdata = adev->dev.platform_data;
+	struct device_node *np = adev->dev.of_node;
 	struct nmk_i2c_dev	*dev;
 	struct i2c_adapter *adap;
 
-	if (!pdata)
-		/* No i2c configuration found, using the default. */
-		pdata = &u8500_i2c;
+	if (!pdata) {
+		if (np) {
+			pdata = devm_kzalloc(&adev->dev, sizeof(*pdata), GFP_KERNEL);
+			if (!pdata) {
+				ret = -ENOMEM;
+				goto err_no_mem;
+			}
+			/* Provide the default configuration as a base. */
+			memcpy(pdata, &u8500_i2c, sizeof(struct nmk_i2c_controller));
+			nmk_i2c_of_probe(np, pdata);
+		} else
+			/* No i2c configuration found, using the default. */
+			pdata = &u8500_i2c;
+	}
 
 	dev = kzalloc(sizeof(struct nmk_i2c_dev), GFP_KERNEL);
 	if (!dev) {
@@ -957,12 +976,6 @@ static int nmk_i2c_probe(struct amba_device *adev, const struct amba_id *id)
 		goto err_irq;
 	}
 
-	dev->regulator = regulator_get(&adev->dev, "v-i2c");
-	if (IS_ERR(dev->regulator)) {
-		dev_warn(&adev->dev, "could not get i2c regulator\n");
-		dev->regulator = NULL;
-	}
-
 	pm_suspend_ignore_children(&adev->dev, true);
 
 	dev->clk = clk_get(&adev->dev, NULL);
@@ -973,6 +986,7 @@ static int nmk_i2c_probe(struct amba_device *adev, const struct amba_id *id)
 	}
 
 	adap = &dev->adap;
+	adap->dev.of_node = np;
 	adap->dev.parent = &adev->dev;
 	adap->owner	= THIS_MODULE;
 	adap->class	= I2C_CLASS_HWMON | I2C_CLASS_SPD;
@@ -1002,6 +1016,8 @@ static int nmk_i2c_probe(struct amba_device *adev, const struct amba_id *id)
 		goto err_add_adap;
 	}
 
+	of_i2c_register_devices(adap);
+
 	pm_runtime_put(&adev->dev);
 
 	return 0;
@@ -1009,8 +1025,6 @@ static int nmk_i2c_probe(struct amba_device *adev, const struct amba_id *id)
  err_add_adap:
 	clk_put(dev->clk);
  err_no_clk:
-	if (dev->regulator)
-		regulator_put(dev->regulator);
 	free_irq(dev->irq, dev);
  err_irq:
 	iounmap(dev->virtbase);
@@ -1038,8 +1052,6 @@ static int nmk_i2c_remove(struct amba_device *adev)
 	if (res)
 		release_mem_region(res->start, resource_size(res));
 	clk_put(dev->clk);
-	if (dev->regulator)
-		regulator_put(dev->regulator);
 	pm_runtime_disable(&adev->dev);
 	amba_set_drvdata(adev, NULL);
 	kfree(dev);
diff --git a/drivers/i2c/busses/i2c-omap.c b/drivers/i2c/busses/i2c-omap.c
index a0e49f6aaf9..db31eaed6ea 100644
--- a/drivers/i2c/busses/i2c-omap.c
+++ b/drivers/i2c/busses/i2c-omap.c
@@ -43,6 +43,7 @@
 #include <linux/slab.h>
 #include <linux/i2c-omap.h>
 #include <linux/pm_runtime.h>
+#include <linux/pm_qos.h>
 
 /* I2C controller revisions */
 #define OMAP_I2C_OMAP1_REV_2		0x20
@@ -55,6 +56,9 @@
 /* timeout waiting for the controller to respond */
 #define OMAP_I2C_TIMEOUT (msecs_to_jiffies(1000))
 
+/* timeout for pm runtime autosuspend */
+#define OMAP_I2C_PM_TIMEOUT		1000	/* ms */
+
 /* For OMAP3 I2C_IV has changed to I2C_WE (wakeup enable) */
 enum {
 	OMAP_I2C_REV_REG = 0,
@@ -176,15 +180,15 @@ enum {
 #define I2C_OMAP_ERRATA_I462		(1 << 1)
 
 struct omap_i2c_dev {
+	spinlock_t		lock;		/* IRQ synchronization */
 	struct device		*dev;
 	void __iomem		*base;		/* virtual */
 	int			irq;
 	int			reg_shift;      /* bit shift for I2C register addresses */
 	struct completion	cmd_complete;
 	struct resource		*ioarea;
-	u32			latency;	/* maximum mpu wkup latency */
-	void			(*set_mpu_wkup_lat)(struct device *dev,
-						    long latency);
+	u32			latency;	/* maximum MPU wkup latency */
+	struct pm_qos_request	pm_qos_request;
 	u32			speed;		/* Speed of bus in kHz */
 	u32			dtrev;		/* extra revision from DT */
 	u32			flags;
@@ -193,12 +197,14 @@ struct omap_i2c_dev {
 	u8			*regs;
 	size_t			buf_len;
 	struct i2c_adapter	adapter;
+	u8			threshold;
 	u8			fifo_size;	/* use as flag and value
 						 * fifo_size==0 implies no fifo
 						 * if set, should be trsh+1
 						 */
 	u8			rev;
 	unsigned		b_hw:1;		/* bad h/w fixes */
+	unsigned		receiver:1;	/* true when we're in receiver mode */
 	u16			iestate;	/* Saved interrupt register */
 	u16			pscstate;
 	u16			scllstate;
@@ -417,13 +423,6 @@ static int omap_i2c_init(struct omap_i2c_dev *dev)
 	omap_i2c_write_reg(dev, OMAP_I2C_SCLL_REG, scll);
 	omap_i2c_write_reg(dev, OMAP_I2C_SCLH_REG, sclh);
 
-	if (dev->fifo_size) {
-		/* Note: setup required fifo size - 1. RTRSH and XTRSH */
-		buf = (dev->fifo_size - 1) << 8 | OMAP_I2C_BUF_RXFIF_CLR |
-			(dev->fifo_size - 1) | OMAP_I2C_BUF_TXFIF_CLR;
-		omap_i2c_write_reg(dev, OMAP_I2C_BUF_REG, buf);
-	}
-
 	/* Take the I2C module out of reset: */
 	omap_i2c_write_reg(dev, OMAP_I2C_CON_REG, OMAP_I2C_CON_EN);
 
@@ -461,6 +460,43 @@ static int omap_i2c_wait_for_bb(struct omap_i2c_dev *dev)
 	return 0;
 }
 
+static void omap_i2c_resize_fifo(struct omap_i2c_dev *dev, u8 size, bool is_rx)
+{
+	u16		buf;
+
+	if (dev->flags & OMAP_I2C_FLAG_NO_FIFO)
+		return;
+
+	/*
+	 * Set up notification threshold based on message size. We're doing
+	 * this to try and avoid draining feature as much as possible. Whenever
+	 * we have big messages to transfer (bigger than our total fifo size)
+	 * then we might use draining feature to transfer the remaining bytes.
+	 */
+
+	dev->threshold = clamp(size, (u8) 1, dev->fifo_size);
+
+	buf = omap_i2c_read_reg(dev, OMAP_I2C_BUF_REG);
+
+	if (is_rx) {
+		/* Clear RX Threshold */
+		buf &= ~(0x3f << 8);
+		buf |= ((dev->threshold - 1) << 8) | OMAP_I2C_BUF_RXFIF_CLR;
+	} else {
+		/* Clear TX Threshold */
+		buf &= ~0x3f;
+		buf |= (dev->threshold - 1) | OMAP_I2C_BUF_TXFIF_CLR;
+	}
+
+	omap_i2c_write_reg(dev, OMAP_I2C_BUF_REG, buf);
+
+	if (dev->rev < OMAP_I2C_REV_ON_3630_4430)
+		dev->b_hw = 1; /* Enable hardware fixes */
+
+	/* calculate wakeup latency constraint for MPU */
+	dev->latency = (1000000 * dev->threshold) / (1000 * dev->speed / 8);
+}
+
 /*
  * Low level master read/write transaction.
  */
@@ -477,6 +513,9 @@ static int omap_i2c_xfer_msg(struct i2c_adapter *adap,
 	if (msg->len == 0)
 		return -EINVAL;
 
+	dev->receiver = !!(msg->flags & I2C_M_RD);
+	omap_i2c_resize_fifo(dev, msg->len, dev->receiver);
+
 	omap_i2c_write_reg(dev, OMAP_I2C_SA_REG, msg->addr);
 
 	/* REVISIT: Could the STB bit of I2C_CON be used with probing? */
@@ -590,8 +629,16 @@ omap_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
 	if (r < 0)
 		goto out;
 
-	if (dev->set_mpu_wkup_lat != NULL)
-		dev->set_mpu_wkup_lat(dev->dev, dev->latency);
+	/*
+	 * When waiting for completion of a i2c transfer, we need to
+	 * set a wake up latency constraint for the MPU. This is to
+	 * ensure quick enough wakeup from idle, when transfer
+	 * completes.
+	 */
+	if (dev->latency)
+		pm_qos_add_request(&dev->pm_qos_request,
+				   PM_QOS_CPU_DMA_LATENCY,
+				   dev->latency);
 
 	for (i = 0; i < num; i++) {
 		r = omap_i2c_xfer_msg(adap, &msgs[i], (i == (num - 1)));
@@ -599,15 +646,16 @@ omap_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
 			break;
 	}
 
-	if (dev->set_mpu_wkup_lat != NULL)
-		dev->set_mpu_wkup_lat(dev->dev, -1);
+	if (dev->latency)
+		pm_qos_remove_request(&dev->pm_qos_request);
 
 	if (r == 0)
 		r = num;
 
 	omap_i2c_wait_for_bb(dev);
 out:
-	pm_runtime_put(dev->dev);
+	pm_runtime_mark_last_busy(dev->dev);
+	pm_runtime_put_autosuspend(dev->dev);
 	return r;
 }
 
@@ -725,186 +773,252 @@ omap_i2c_omap1_isr(int this_irq, void *dev_id)
  * data to DATA_REG. Otherwise some data bytes can be lost while transferring
  * them from the memory to the I2C interface.
  */
-static int errata_omap3_i462(struct omap_i2c_dev *dev, u16 *stat, int *err)
+static int errata_omap3_i462(struct omap_i2c_dev *dev)
 {
 	unsigned long timeout = 10000;
+	u16 stat;
 
-	while (--timeout && !(*stat & OMAP_I2C_STAT_XUDF)) {
-		if (*stat & (OMAP_I2C_STAT_NACK | OMAP_I2C_STAT_AL)) {
-			omap_i2c_ack_stat(dev, *stat & (OMAP_I2C_STAT_XRDY |
+	do {
+		stat = omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG);
+		if (stat & OMAP_I2C_STAT_XUDF)
+			break;
+
+		if (stat & (OMAP_I2C_STAT_NACK | OMAP_I2C_STAT_AL)) {
+			omap_i2c_ack_stat(dev, (OMAP_I2C_STAT_XRDY |
 							OMAP_I2C_STAT_XDR));
-			return -ETIMEDOUT;
+			if (stat & OMAP_I2C_STAT_NACK) {
+				dev->cmd_err |= OMAP_I2C_STAT_NACK;
+				omap_i2c_ack_stat(dev, OMAP_I2C_STAT_NACK);
+			}
+
+			if (stat & OMAP_I2C_STAT_AL) {
+				dev_err(dev->dev, "Arbitration lost\n");
+				dev->cmd_err |= OMAP_I2C_STAT_AL;
+				omap_i2c_ack_stat(dev, OMAP_I2C_STAT_NACK);
+			}
+
+			return -EIO;
 		}
 
 		cpu_relax();
-		*stat = omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG);
-	}
+	} while (--timeout);
 
 	if (!timeout) {
 		dev_err(dev->dev, "timeout waiting on XUDF bit\n");
 		return 0;
 	}
 
-	*err |= OMAP_I2C_STAT_XUDF;
 	return 0;
 }
 
+static void omap_i2c_receive_data(struct omap_i2c_dev *dev, u8 num_bytes,
+		bool is_rdr)
+{
+	u16		w;
+
+	while (num_bytes--) {
+		w = omap_i2c_read_reg(dev, OMAP_I2C_DATA_REG);
+		*dev->buf++ = w;
+		dev->buf_len--;
+
+		/*
+		 * Data reg in 2430, omap3 and
+		 * omap4 is 8 bit wide
+		 */
+		if (dev->flags & OMAP_I2C_FLAG_16BIT_DATA_REG) {
+			*dev->buf++ = w >> 8;
+			dev->buf_len--;
+		}
+	}
+}
+
+static int omap_i2c_transmit_data(struct omap_i2c_dev *dev, u8 num_bytes,
+		bool is_xdr)
+{
+	u16		w;
+
+	while (num_bytes--) {
+		w = *dev->buf++;
+		dev->buf_len--;
+
+		/*
+		 * Data reg in 2430, omap3 and
+		 * omap4 is 8 bit wide
+		 */
+		if (dev->flags & OMAP_I2C_FLAG_16BIT_DATA_REG) {
+			w |= *dev->buf++ << 8;
+			dev->buf_len--;
+		}
+
+		if (dev->errata & I2C_OMAP_ERRATA_I462) {
+			int ret;
+
+			ret = errata_omap3_i462(dev);
+			if (ret < 0)
+				return ret;
+		}
+
+		omap_i2c_write_reg(dev, OMAP_I2C_DATA_REG, w);
+	}
+
+	return 0;
+}
+
+static irqreturn_t
+omap_i2c_isr(int irq, void *dev_id)
+{
+	struct omap_i2c_dev *dev = dev_id;
+	irqreturn_t ret = IRQ_HANDLED;
+	u16 mask;
+	u16 stat;
+
+	spin_lock(&dev->lock);
+	mask = omap_i2c_read_reg(dev, OMAP_I2C_IE_REG);
+	stat = omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG);
+
+	if (stat & mask)
+		ret = IRQ_WAKE_THREAD;
+
+	spin_unlock(&dev->lock);
+
+	return ret;
+}
+
 static irqreturn_t
-omap_i2c_isr(int this_irq, void *dev_id)
+omap_i2c_isr_thread(int this_irq, void *dev_id)
 {
 	struct omap_i2c_dev *dev = dev_id;
+	unsigned long flags;
 	u16 bits;
-	u16 stat, w;
-	int err, count = 0;
+	u16 stat;
+	int err = 0, count = 0;
+
+	spin_lock_irqsave(&dev->lock, flags);
+	do {
+		bits = omap_i2c_read_reg(dev, OMAP_I2C_IE_REG);
+		stat = omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG);
+		stat &= bits;
+
+		/* If we're in receiver mode, ignore XDR/XRDY */
+		if (dev->receiver)
+			stat &= ~(OMAP_I2C_STAT_XDR | OMAP_I2C_STAT_XRDY);
+		else
+			stat &= ~(OMAP_I2C_STAT_RDR | OMAP_I2C_STAT_RRDY);
 
-	if (pm_runtime_suspended(dev->dev))
-		return IRQ_NONE;
+		if (!stat) {
+			/* my work here is done */
+			goto out;
+		}
 
-	bits = omap_i2c_read_reg(dev, OMAP_I2C_IE_REG);
-	while ((stat = (omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG))) & bits) {
 		dev_dbg(dev->dev, "IRQ (ISR = 0x%04x)\n", stat);
 		if (count++ == 100) {
 			dev_warn(dev->dev, "Too much work in one IRQ\n");
 			break;
 		}
 
-		err = 0;
-complete:
-		/*
-		 * Ack the stat in one go, but [R/X]DR and [R/X]RDY should be
-		 * acked after the data operation is complete.
-		 * Ref: TRM SWPU114Q Figure 18-31
-		 */
-		omap_i2c_write_reg(dev, OMAP_I2C_STAT_REG, stat &
-				~(OMAP_I2C_STAT_RRDY | OMAP_I2C_STAT_RDR |
-				OMAP_I2C_STAT_XRDY | OMAP_I2C_STAT_XDR));
-
-		if (stat & OMAP_I2C_STAT_NACK)
+		if (stat & OMAP_I2C_STAT_NACK) {
 			err |= OMAP_I2C_STAT_NACK;
+			omap_i2c_ack_stat(dev, OMAP_I2C_STAT_NACK);
+			break;
+		}
 
 		if (stat & OMAP_I2C_STAT_AL) {
 			dev_err(dev->dev, "Arbitration lost\n");
 			err |= OMAP_I2C_STAT_AL;
+			omap_i2c_ack_stat(dev, OMAP_I2C_STAT_AL);
+			break;
 		}
+
 		/*
 		 * ProDB0017052: Clear ARDY bit twice
 		 */
 		if (stat & (OMAP_I2C_STAT_ARDY | OMAP_I2C_STAT_NACK |
 					OMAP_I2C_STAT_AL)) {
-			omap_i2c_ack_stat(dev, stat &
-				(OMAP_I2C_STAT_RRDY | OMAP_I2C_STAT_RDR |
-				OMAP_I2C_STAT_XRDY | OMAP_I2C_STAT_XDR |
-				OMAP_I2C_STAT_ARDY));
-			omap_i2c_complete_cmd(dev, err);
-			return IRQ_HANDLED;
+			omap_i2c_ack_stat(dev, (OMAP_I2C_STAT_RRDY |
+						OMAP_I2C_STAT_RDR |
+						OMAP_I2C_STAT_XRDY |
+						OMAP_I2C_STAT_XDR |
+						OMAP_I2C_STAT_ARDY));
+			break;
 		}
-		if (stat & (OMAP_I2C_STAT_RRDY | OMAP_I2C_STAT_RDR)) {
+
+		if (stat & OMAP_I2C_STAT_RDR) {
 			u8 num_bytes = 1;
 
+			if (dev->fifo_size)
+				num_bytes = dev->buf_len;
+
+			omap_i2c_receive_data(dev, num_bytes, true);
+
 			if (dev->errata & I2C_OMAP_ERRATA_I207)
 				i2c_omap_errata_i207(dev, stat);
 
-			if (dev->fifo_size) {
-				if (stat & OMAP_I2C_STAT_RRDY)
-					num_bytes = dev->fifo_size;
-				else    /* read RXSTAT on RDR interrupt */
-					num_bytes = (omap_i2c_read_reg(dev,
-							OMAP_I2C_BUFSTAT_REG)
-							>> 8) & 0x3F;
-			}
-			while (num_bytes) {
-				num_bytes--;
-				w = omap_i2c_read_reg(dev, OMAP_I2C_DATA_REG);
-				if (dev->buf_len) {
-					*dev->buf++ = w;
-					dev->buf_len--;
-					/*
-					 * Data reg in 2430, omap3 and
-					 * omap4 is 8 bit wide
-					 */
-					if (dev->flags &
-						 OMAP_I2C_FLAG_16BIT_DATA_REG) {
-						if (dev->buf_len) {
-							*dev->buf++ = w >> 8;
-							dev->buf_len--;
-						}
-					}
-				} else {
-					if (stat & OMAP_I2C_STAT_RRDY)
-						dev_err(dev->dev,
-							"RRDY IRQ while no data"
-								" requested\n");
-					if (stat & OMAP_I2C_STAT_RDR)
-						dev_err(dev->dev,
-							"RDR IRQ while no data"
-								" requested\n");
-					break;
-				}
-			}
-			omap_i2c_ack_stat(dev,
-				stat & (OMAP_I2C_STAT_RRDY | OMAP_I2C_STAT_RDR));
+			omap_i2c_ack_stat(dev, OMAP_I2C_STAT_RDR);
+			break;
+		}
+
+		if (stat & OMAP_I2C_STAT_RRDY) {
+			u8 num_bytes = 1;
+
+			if (dev->threshold)
+				num_bytes = dev->threshold;
+
+			omap_i2c_receive_data(dev, num_bytes, false);
+			omap_i2c_ack_stat(dev, OMAP_I2C_STAT_RRDY);
 			continue;
 		}
-		if (stat & (OMAP_I2C_STAT_XRDY | OMAP_I2C_STAT_XDR)) {
+
+		if (stat & OMAP_I2C_STAT_XDR) {
 			u8 num_bytes = 1;
-			if (dev->fifo_size) {
-				if (stat & OMAP_I2C_STAT_XRDY)
-					num_bytes = dev->fifo_size;
-				else    /* read TXSTAT on XDR interrupt */
-					num_bytes = omap_i2c_read_reg(dev,
-							OMAP_I2C_BUFSTAT_REG)
-							& 0x3F;
-			}
-			while (num_bytes) {
-				num_bytes--;
-				w = 0;
-				if (dev->buf_len) {
-					w = *dev->buf++;
-					dev->buf_len--;
-					/*
-					 * Data reg in 2430, omap3 and
-					 * omap4 is 8 bit wide
-					 */
-					if (dev->flags &
-						 OMAP_I2C_FLAG_16BIT_DATA_REG) {
-						if (dev->buf_len) {
-							w |= *dev->buf++ << 8;
-							dev->buf_len--;
-						}
-					}
-				} else {
-					if (stat & OMAP_I2C_STAT_XRDY)
-						dev_err(dev->dev,
-							"XRDY IRQ while no "
-							"data to send\n");
-					if (stat & OMAP_I2C_STAT_XDR)
-						dev_err(dev->dev,
-							"XDR IRQ while no "
-							"data to send\n");
-					break;
-				}
-
-				if ((dev->errata & I2C_OMAP_ERRATA_I462) &&
-				    errata_omap3_i462(dev, &stat, &err))
-					goto complete;
-
-				omap_i2c_write_reg(dev, OMAP_I2C_DATA_REG, w);
-			}
-			omap_i2c_ack_stat(dev,
-				stat & (OMAP_I2C_STAT_XRDY | OMAP_I2C_STAT_XDR));
+			int ret;
+
+			if (dev->fifo_size)
+				num_bytes = dev->buf_len;
+
+			ret = omap_i2c_transmit_data(dev, num_bytes, true);
+			if (ret < 0)
+				break;
+
+			omap_i2c_ack_stat(dev, OMAP_I2C_STAT_XDR);
+			break;
+		}
+
+		if (stat & OMAP_I2C_STAT_XRDY) {
+			u8 num_bytes = 1;
+			int ret;
+
+			if (dev->threshold)
+				num_bytes = dev->threshold;
+
+			ret = omap_i2c_transmit_data(dev, num_bytes, false);
+			if (ret < 0)
+				break;
+
+			omap_i2c_ack_stat(dev, OMAP_I2C_STAT_XRDY);
 			continue;
 		}
+
 		if (stat & OMAP_I2C_STAT_ROVR) {
 			dev_err(dev->dev, "Receive overrun\n");
-			dev->cmd_err |= OMAP_I2C_STAT_ROVR;
+			err |= OMAP_I2C_STAT_ROVR;
+			omap_i2c_ack_stat(dev, OMAP_I2C_STAT_ROVR);
+			break;
 		}
+
 		if (stat & OMAP_I2C_STAT_XUDF) {
 			dev_err(dev->dev, "Transmit underflow\n");
-			dev->cmd_err |= OMAP_I2C_STAT_XUDF;
+			err |= OMAP_I2C_STAT_XUDF;
+			omap_i2c_ack_stat(dev, OMAP_I2C_STAT_XUDF);
+			break;
 		}
-	}
+	} while (stat);
+
+	omap_i2c_complete_cmd(dev, err);
+
+out:
+	spin_unlock_irqrestore(&dev->lock, flags);
 
-	return count ? IRQ_HANDLED : IRQ_NONE;
+	return IRQ_HANDLED;
 }
 
 static const struct i2c_algorithm omap_i2c_algo = {
@@ -943,12 +1057,12 @@ omap_i2c_probe(struct platform_device *pdev)
 {
 	struct omap_i2c_dev	*dev;
 	struct i2c_adapter	*adap;
-	struct resource		*mem, *irq, *ioarea;
+	struct resource		*mem;
 	const struct omap_i2c_bus_platform_data *pdata =
 		pdev->dev.platform_data;
 	struct device_node	*node = pdev->dev.of_node;
 	const struct of_device_id *match;
-	irq_handler_t isr;
+	int irq;
 	int r;
 
 	/* NOTE: driver uses the static register mapping */
@@ -957,23 +1071,23 @@ omap_i2c_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "no mem resource?\n");
 		return -ENODEV;
 	}
-	irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (!irq) {
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
 		dev_err(&pdev->dev, "no irq resource?\n");
-		return -ENODEV;
+		return irq;
 	}
 
-	ioarea = request_mem_region(mem->start, resource_size(mem),
-			pdev->name);
-	if (!ioarea) {
-		dev_err(&pdev->dev, "I2C region already claimed\n");
-		return -EBUSY;
+	dev = devm_kzalloc(&pdev->dev, sizeof(struct omap_i2c_dev), GFP_KERNEL);
+	if (!dev) {
+		dev_err(&pdev->dev, "Menory allocation failed\n");
+		return -ENOMEM;
 	}
 
-	dev = kzalloc(sizeof(struct omap_i2c_dev), GFP_KERNEL);
-	if (!dev) {
-		r = -ENOMEM;
-		goto err_release_region;
+	dev->base = devm_request_and_ioremap(&pdev->dev, mem);
+	if (!dev->base) {
+		dev_err(&pdev->dev, "I2C region already claimed\n");
+		return -ENOMEM;
 	}
 
 	match = of_match_device(of_match_ptr(omap_i2c_of_match), &pdev->dev);
@@ -990,17 +1104,13 @@ omap_i2c_probe(struct platform_device *pdev)
 	} else if (pdata != NULL) {
 		dev->speed = pdata->clkrate;
 		dev->flags = pdata->flags;
-		dev->set_mpu_wkup_lat = pdata->set_mpu_wkup_lat;
 		dev->dtrev = pdata->rev;
 	}
 
 	dev->dev = &pdev->dev;
-	dev->irq = irq->start;
-	dev->base = ioremap(mem->start, resource_size(mem));
-	if (!dev->base) {
-		r = -ENOMEM;
-		goto err_free_mem;
-	}
+	dev->irq = irq;
+
+	spin_lock_init(&dev->lock);
 
 	platform_set_drvdata(pdev, dev);
 	init_completion(&dev->cmd_complete);
@@ -1013,6 +1123,9 @@ omap_i2c_probe(struct platform_device *pdev)
 		dev->regs = (u8 *)reg_map_ip_v1;
 
 	pm_runtime_enable(dev->dev);
+	pm_runtime_set_autosuspend_delay(dev->dev, OMAP_I2C_PM_TIMEOUT);
+	pm_runtime_use_autosuspend(dev->dev);
+
 	r = pm_runtime_get_sync(dev->dev);
 	if (IS_ERR_VALUE(r))
 		goto err_free_mem;
@@ -1042,32 +1155,31 @@ omap_i2c_probe(struct platform_device *pdev)
 
 		dev->fifo_size = (dev->fifo_size / 2);
 
-		if (dev->rev >= OMAP_I2C_REV_ON_3630_4430)
-			dev->b_hw = 0; /* Disable hardware fixes */
-		else
+		if (dev->rev < OMAP_I2C_REV_ON_3630_4430)
 			dev->b_hw = 1; /* Enable hardware fixes */
 
 		/* calculate wakeup latency constraint for MPU */
-		if (dev->set_mpu_wkup_lat != NULL)
-			dev->latency = (1000000 * dev->fifo_size) /
-				       (1000 * dev->speed / 8);
+		dev->latency = (1000000 * dev->fifo_size) /
+			       (1000 * dev->speed / 8);
 	}
 
 	/* reset ASAP, clearing any IRQs */
 	omap_i2c_init(dev);
 
-	isr = (dev->rev < OMAP_I2C_OMAP1_REV_2) ? omap_i2c_omap1_isr :
-								   omap_i2c_isr;
-	r = request_irq(dev->irq, isr, IRQF_NO_SUSPEND, pdev->name, dev);
+	if (dev->rev < OMAP_I2C_OMAP1_REV_2)
+		r = devm_request_irq(&pdev->dev, dev->irq, omap_i2c_omap1_isr,
+				IRQF_NO_SUSPEND, pdev->name, dev);
+	else
+		r = devm_request_threaded_irq(&pdev->dev, dev->irq,
+				omap_i2c_isr, omap_i2c_isr_thread,
+				IRQF_NO_SUSPEND | IRQF_ONESHOT,
+				pdev->name, dev);
 
 	if (r) {
 		dev_err(dev->dev, "failure requesting irq %i\n", dev->irq);
 		goto err_unuse_clocks;
 	}
 
-	dev_info(dev->dev, "bus %d rev%d.%d.%d at %d kHz\n", pdev->id,
-		 dev->dtrev, dev->rev >> 4, dev->rev & 0xf, dev->speed);
-
 	adap = &dev->adapter;
 	i2c_set_adapdata(adap, dev);
 	adap->owner = THIS_MODULE;
@@ -1082,27 +1194,25 @@ omap_i2c_probe(struct platform_device *pdev)
 	r = i2c_add_numbered_adapter(adap);
 	if (r) {
 		dev_err(dev->dev, "failure adding adapter\n");
-		goto err_free_irq;
+		goto err_unuse_clocks;
 	}
 
+	dev_info(dev->dev, "bus %d rev%d.%d.%d at %d kHz\n", adap->nr,
+		 dev->dtrev, dev->rev >> 4, dev->rev & 0xf, dev->speed);
+
 	of_i2c_register_devices(adap);
 
-	pm_runtime_put(dev->dev);
+	pm_runtime_mark_last_busy(dev->dev);
+	pm_runtime_put_autosuspend(dev->dev);
 
 	return 0;
 
-err_free_irq:
-	free_irq(dev->irq, dev);
 err_unuse_clocks:
 	omap_i2c_write_reg(dev, OMAP_I2C_CON_REG, 0);
 	pm_runtime_put(dev->dev);
-	iounmap(dev->base);
 	pm_runtime_disable(&pdev->dev);
 err_free_mem:
 	platform_set_drvdata(pdev, NULL);
-	kfree(dev);
-err_release_region:
-	release_mem_region(mem->start, resource_size(mem));
 
 	return r;
 }
@@ -1110,12 +1220,10 @@ err_release_region:
 static int __devexit omap_i2c_remove(struct platform_device *pdev)
 {
 	struct omap_i2c_dev	*dev = platform_get_drvdata(pdev);
-	struct resource		*mem;
 	int ret;
 
 	platform_set_drvdata(pdev, NULL);
 
-	free_irq(dev->irq, dev);
 	i2c_del_adapter(&dev->adapter);
 	ret = pm_runtime_get_sync(&pdev->dev);
 	if (IS_ERR_VALUE(ret))
@@ -1124,10 +1232,6 @@ static int __devexit omap_i2c_remove(struct platform_device *pdev)
 	omap_i2c_write_reg(dev, OMAP_I2C_CON_REG, 0);
 	pm_runtime_put(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
-	iounmap(dev->base);
-	kfree(dev);
-	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	release_mem_region(mem->start, resource_size(mem));
 	return 0;
 }
 
diff --git a/drivers/i2c/busses/i2c-parport.c b/drivers/i2c/busses/i2c-parport.c
index 24565687ac9..81d88786962 100644
--- a/drivers/i2c/busses/i2c-parport.c
+++ b/drivers/i2c/busses/i2c-parport.c
@@ -151,7 +151,7 @@ static const struct i2c_algo_bit_data parport_algo_data = {
 
 /* ----- I2c and parallel port call-back functions and structures --------- */
 
-void i2c_parport_irq(void *data)
+static void i2c_parport_irq(void *data)
 {
 	struct i2c_par *adapter = data;
 	struct i2c_client *ara = adapter->ara;
diff --git a/drivers/i2c/busses/i2c-piix4.c b/drivers/i2c/busses/i2c-piix4.c
index ef511df2c96..8bbd6ece7c4 100644
--- a/drivers/i2c/busses/i2c-piix4.c
+++ b/drivers/i2c/busses/i2c-piix4.c
@@ -37,6 +37,7 @@
 #include <linux/stddef.h>
 #include <linux/ioport.h>
 #include <linux/i2c.h>
+#include <linux/slab.h>
 #include <linux/init.h>
 #include <linux/dmi.h>
 #include <linux/acpi.h>
diff --git a/drivers/i2c/busses/i2c-rcar.c b/drivers/i2c/busses/i2c-rcar.c
new file mode 100644
index 00000000000..f9399d163af
--- /dev/null
+++ b/drivers/i2c/busses/i2c-rcar.c
@@ -0,0 +1,709 @@
+/*
+ *  drivers/i2c/busses/i2c-rcar.c
+ *
+ * Copyright (C) 2012 Renesas Solutions Corp.
+ * Kuninori Morimoto <kuninori.morimoto.gx@renesas.com>
+ *
+ * This file is based on the drivers/i2c/busses/i2c-sh7760.c
+ * (c) 2005-2008 MSC Vertriebsges.m.b.H, Manuel Lauss <mlau@msc-ge.com>
+ *
+ * This file used out-of-tree driver i2c-rcar.c
+ * Copyright (C) 2011-2012 Renesas Electronics Corporation
+ *
+ * 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
+ *
+ * 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 <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/i2c.h>
+#include <linux/i2c/i2c-rcar.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+/* register offsets */
+#define ICSCR	0x00	/* slave ctrl */
+#define ICMCR	0x04	/* master ctrl */
+#define ICSSR	0x08	/* slave status */
+#define ICMSR	0x0C	/* master status */
+#define ICSIER	0x10	/* slave irq enable */
+#define ICMIER	0x14	/* master irq enable */
+#define ICCCR	0x18	/* clock dividers */
+#define ICSAR	0x1C	/* slave address */
+#define ICMAR	0x20	/* master address */
+#define ICRXTX	0x24	/* data port */
+
+/* ICMCR */
+#define MDBS	(1 << 7)	/* non-fifo mode switch */
+#define FSCL	(1 << 6)	/* override SCL pin */
+#define FSDA	(1 << 5)	/* override SDA pin */
+#define OBPC	(1 << 4)	/* override pins */
+#define MIE	(1 << 3)	/* master if enable */
+#define TSBE	(1 << 2)
+#define FSB	(1 << 1)	/* force stop bit */
+#define ESG	(1 << 0)	/* en startbit gen */
+
+/* ICMSR */
+#define MNR	(1 << 6)	/* nack received */
+#define MAL	(1 << 5)	/* arbitration lost */
+#define MST	(1 << 4)	/* sent a stop */
+#define MDE	(1 << 3)
+#define MDT	(1 << 2)
+#define MDR	(1 << 1)
+#define MAT	(1 << 0)	/* slave addr xfer done */
+
+/* ICMIE */
+#define MNRE	(1 << 6)	/* nack irq en */
+#define MALE	(1 << 5)	/* arblos irq en */
+#define MSTE	(1 << 4)	/* stop irq en */
+#define MDEE	(1 << 3)
+#define MDTE	(1 << 2)
+#define MDRE	(1 << 1)
+#define MATE	(1 << 0)	/* address sent irq en */
+
+
+enum {
+	RCAR_BUS_PHASE_ADDR,
+	RCAR_BUS_PHASE_DATA,
+	RCAR_BUS_PHASE_STOP,
+};
+
+enum {
+	RCAR_IRQ_CLOSE,
+	RCAR_IRQ_OPEN_FOR_SEND,
+	RCAR_IRQ_OPEN_FOR_RECV,
+	RCAR_IRQ_OPEN_FOR_STOP,
+};
+
+/*
+ * flags
+ */
+#define ID_LAST_MSG	(1 << 0)
+#define ID_IOERROR	(1 << 1)
+#define ID_DONE		(1 << 2)
+#define ID_ARBLOST	(1 << 3)
+#define ID_NACK		(1 << 4)
+
+struct rcar_i2c_priv {
+	void __iomem *io;
+	struct i2c_adapter adap;
+	struct i2c_msg	*msg;
+
+	spinlock_t lock;
+	wait_queue_head_t wait;
+
+	int pos;
+	int irq;
+	u32 icccr;
+	u32 flags;
+};
+
+#define rcar_i2c_priv_to_dev(p)		((p)->adap.dev.parent)
+#define rcar_i2c_is_recv(p)		((p)->msg->flags & I2C_M_RD)
+
+#define rcar_i2c_flags_set(p, f)	((p)->flags |= (f))
+#define rcar_i2c_flags_has(p, f)	((p)->flags & (f))
+
+#define LOOP_TIMEOUT	1024
+
+/*
+ *		basic functions
+ */
+static void rcar_i2c_write(struct rcar_i2c_priv *priv, int reg, u32 val)
+{
+	writel(val, priv->io + reg);
+}
+
+static u32 rcar_i2c_read(struct rcar_i2c_priv *priv, int reg)
+{
+	return readl(priv->io + reg);
+}
+
+static void rcar_i2c_init(struct rcar_i2c_priv *priv)
+{
+	/*
+	 * reset slave mode.
+	 * slave mode is not used on this driver
+	 */
+	rcar_i2c_write(priv, ICSIER, 0);
+	rcar_i2c_write(priv, ICSAR, 0);
+	rcar_i2c_write(priv, ICSCR, 0);
+	rcar_i2c_write(priv, ICSSR, 0);
+
+	/* reset master mode */
+	rcar_i2c_write(priv, ICMIER, 0);
+	rcar_i2c_write(priv, ICMCR, 0);
+	rcar_i2c_write(priv, ICMSR, 0);
+	rcar_i2c_write(priv, ICMAR, 0);
+}
+
+static void rcar_i2c_irq_mask(struct rcar_i2c_priv *priv, int open)
+{
+	u32 val = MNRE | MALE | MSTE | MATE; /* default */
+
+	switch (open) {
+	case RCAR_IRQ_OPEN_FOR_SEND:
+		val |= MDEE; /* default + send */
+		break;
+	case RCAR_IRQ_OPEN_FOR_RECV:
+		val |= MDRE; /* default + read */
+		break;
+	case RCAR_IRQ_OPEN_FOR_STOP:
+		val = MSTE; /* stop irq only */
+		break;
+	case RCAR_IRQ_CLOSE:
+	default:
+		val = 0; /* all close */
+		break;
+	}
+	rcar_i2c_write(priv, ICMIER, val);
+}
+
+static void rcar_i2c_set_addr(struct rcar_i2c_priv *priv, u32 recv)
+{
+	rcar_i2c_write(priv, ICMAR, (priv->msg->addr << 1) | recv);
+}
+
+/*
+ *		bus control functions
+ */
+static int rcar_i2c_bus_barrier(struct rcar_i2c_priv *priv)
+{
+	int i;
+
+	for (i = 0; i < LOOP_TIMEOUT; i++) {
+		/* make sure that bus is not busy */
+		if (!(rcar_i2c_read(priv, ICMCR) & FSDA))
+			return 0;
+		udelay(1);
+	}
+
+	return -EBUSY;
+}
+
+static void rcar_i2c_bus_phase(struct rcar_i2c_priv *priv, int phase)
+{
+	switch (phase) {
+	case RCAR_BUS_PHASE_ADDR:
+		rcar_i2c_write(priv, ICMCR, MDBS | MIE | ESG);
+		break;
+	case RCAR_BUS_PHASE_DATA:
+		rcar_i2c_write(priv, ICMCR, MDBS | MIE);
+		break;
+	case RCAR_BUS_PHASE_STOP:
+		rcar_i2c_write(priv, ICMCR, MDBS | MIE | FSB);
+		break;
+	}
+}
+
+/*
+ *		clock function
+ */
+static int rcar_i2c_clock_calculate(struct rcar_i2c_priv *priv,
+				    u32 bus_speed,
+				    struct device *dev)
+{
+	struct clk *clkp = clk_get(NULL, "peripheral_clk");
+	u32 scgd, cdf;
+	u32 round, ick;
+	u32 scl;
+
+	if (!clkp) {
+		dev_err(dev, "there is no peripheral_clk\n");
+		return -EIO;
+	}
+
+	/*
+	 * calculate SCL clock
+	 * see
+	 *	ICCCR
+	 *
+	 * ick	= clkp / (1 + CDF)
+	 * SCL	= ick / (20 + SCGD * 8 + F[(ticf + tr + intd) * ick])
+	 *
+	 * ick  : I2C internal clock < 20 MHz
+	 * ticf : I2C SCL falling time  =  35 ns here
+	 * tr   : I2C SCL rising  time  = 200 ns here
+	 * intd : LSI internal delay    =  50 ns here
+	 * clkp : peripheral_clk
+	 * F[]  : integer up-valuation
+	 */
+	for (cdf = 0; cdf < 4; cdf++) {
+		ick = clk_get_rate(clkp) / (1 + cdf);
+		if (ick < 20000000)
+			goto ick_find;
+	}
+	dev_err(dev, "there is no best CDF\n");
+	return -EIO;
+
+ick_find:
+	/*
+	 * it is impossible to calculate large scale
+	 * number on u32. separate it
+	 *
+	 * F[(ticf + tr + intd) * ick]
+	 *  = F[(35 + 200 + 50)ns * ick]
+	 *  = F[285 * ick / 1000000000]
+	 *  = F[(ick / 1000000) * 285 / 1000]
+	 */
+	round = (ick + 500000) / 1000000 * 285;
+	round = (round + 500) / 1000;
+
+	/*
+	 * SCL	= ick / (20 + SCGD * 8 + F[(ticf + tr + intd) * ick])
+	 *
+	 * Calculation result (= SCL) should be less than
+	 * bus_speed for hardware safety
+	 */
+	for (scgd = 0; scgd < 0x40; scgd++) {
+		scl = ick / (20 + (scgd * 8) + round);
+		if (scl <= bus_speed)
+			goto scgd_find;
+	}
+	dev_err(dev, "it is impossible to calculate best SCL\n");
+	return -EIO;
+
+scgd_find:
+	dev_dbg(dev, "clk %d/%d(%lu), round %u, CDF:0x%x, SCGD: 0x%x\n",
+		scl, bus_speed, clk_get_rate(clkp), round, cdf, scgd);
+
+	/*
+	 * keep icccr value
+	 */
+	priv->icccr = (scgd << 2 | cdf);
+
+	return 0;
+}
+
+static void rcar_i2c_clock_start(struct rcar_i2c_priv *priv)
+{
+	rcar_i2c_write(priv, ICCCR, priv->icccr);
+}
+
+/*
+ *		status functions
+ */
+static u32 rcar_i2c_status_get(struct rcar_i2c_priv *priv)
+{
+	return rcar_i2c_read(priv, ICMSR);
+}
+
+#define rcar_i2c_status_clear(priv) rcar_i2c_status_bit_clear(priv, 0xffffffff)
+static void rcar_i2c_status_bit_clear(struct rcar_i2c_priv *priv, u32 bit)
+{
+	rcar_i2c_write(priv, ICMSR, ~bit);
+}
+
+/*
+ *		recv/send functions
+ */
+static int rcar_i2c_recv(struct rcar_i2c_priv *priv)
+{
+	rcar_i2c_set_addr(priv, 1);
+	rcar_i2c_status_clear(priv);
+	rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_ADDR);
+	rcar_i2c_irq_mask(priv, RCAR_IRQ_OPEN_FOR_RECV);
+
+	return 0;
+}
+
+static int rcar_i2c_send(struct rcar_i2c_priv *priv)
+{
+	int ret;
+
+	/*
+	 * It should check bus status when send case
+	 */
+	ret = rcar_i2c_bus_barrier(priv);
+	if (ret < 0)
+		return ret;
+
+	rcar_i2c_set_addr(priv, 0);
+	rcar_i2c_status_clear(priv);
+	rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_ADDR);
+	rcar_i2c_irq_mask(priv, RCAR_IRQ_OPEN_FOR_SEND);
+
+	return 0;
+}
+
+#define rcar_i2c_send_restart(priv) rcar_i2c_status_bit_clear(priv, (MAT | MDE))
+#define rcar_i2c_recv_restart(priv) rcar_i2c_status_bit_clear(priv, (MAT | MDR))
+
+/*
+ *		interrupt functions
+ */
+static int rcar_i2c_irq_send(struct rcar_i2c_priv *priv, u32 msr)
+{
+	struct i2c_msg *msg = priv->msg;
+
+	/*
+	 * FIXME
+	 * sometimes, unknown interrupt happened.
+	 * Do nothing
+	 */
+	if (!(msr & MDE))
+		return 0;
+
+	/*
+	 * If address transfer phase finished,
+	 * goto data phase.
+	 */
+	if (msr & MAT)
+		rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_DATA);
+
+	if (priv->pos < msg->len) {
+		/*
+		 * Prepare next data to ICRXTX register.
+		 * This data will go to _SHIFT_ register.
+		 *
+		 *    *
+		 * [ICRXTX] -> [SHIFT] -> [I2C bus]
+		 */
+		rcar_i2c_write(priv, ICRXTX, msg->buf[priv->pos]);
+		priv->pos++;
+
+	} else {
+		/*
+		 * The last data was pushed to ICRXTX on _PREV_ empty irq.
+		 * It is on _SHIFT_ register, and will sent to I2C bus.
+		 *
+		 *		  *
+		 * [ICRXTX] -> [SHIFT] -> [I2C bus]
+		 */
+
+		if (priv->flags & ID_LAST_MSG)
+			/*
+			 * If current msg is the _LAST_ msg,
+			 * prepare stop condition here.
+			 * ID_DONE will be set on STOP irq.
+			 */
+			rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_STOP);
+		else
+			/*
+			 * If current msg is _NOT_ last msg,
+			 * it doesn't call stop phase.
+			 * thus, there is no STOP irq.
+			 * return ID_DONE here.
+			 */
+			return ID_DONE;
+	}
+
+	rcar_i2c_send_restart(priv);
+
+	return 0;
+}
+
+static int rcar_i2c_irq_recv(struct rcar_i2c_priv *priv, u32 msr)
+{
+	struct i2c_msg *msg = priv->msg;
+
+	/*
+	 * FIXME
+	 * sometimes, unknown interrupt happened.
+	 * Do nothing
+	 */
+	if (!(msr & MDR))
+		return 0;
+
+	if (msr & MAT) {
+		/*
+		 * Address transfer phase finished,
+		 * but, there is no data at this point.
+		 * Do nothing.
+		 */
+	} else if (priv->pos < msg->len) {
+		/*
+		 * get received data
+		 */
+		msg->buf[priv->pos] = rcar_i2c_read(priv, ICRXTX);
+		priv->pos++;
+	}
+
+	/*
+	 * If next received data is the _LAST_,
+	 * go to STOP phase,
+	 * otherwise, go to DATA phase.
+	 */
+	if (priv->pos + 1 >= msg->len)
+		rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_STOP);
+	else
+		rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_DATA);
+
+	rcar_i2c_recv_restart(priv);
+
+	return 0;
+}
+
+static irqreturn_t rcar_i2c_irq(int irq, void *ptr)
+{
+	struct rcar_i2c_priv *priv = ptr;
+	struct device *dev = rcar_i2c_priv_to_dev(priv);
+	u32 msr;
+
+	/*-------------- spin lock -----------------*/
+	spin_lock(&priv->lock);
+
+	msr = rcar_i2c_status_get(priv);
+
+	/*
+	 * Arbitration lost
+	 */
+	if (msr & MAL) {
+		/*
+		 * CAUTION
+		 *
+		 * When arbitration lost, device become _slave_ mode.
+		 */
+		dev_dbg(dev, "Arbitration Lost\n");
+		rcar_i2c_flags_set(priv, (ID_DONE | ID_ARBLOST));
+		goto out;
+	}
+
+	/*
+	 * Stop
+	 */
+	if (msr & MST) {
+		dev_dbg(dev, "Stop\n");
+		rcar_i2c_flags_set(priv, ID_DONE);
+		goto out;
+	}
+
+	/*
+	 * Nack
+	 */
+	if (msr & MNR) {
+		dev_dbg(dev, "Nack\n");
+
+		/* go to stop phase */
+		rcar_i2c_bus_phase(priv, RCAR_BUS_PHASE_STOP);
+		rcar_i2c_irq_mask(priv, RCAR_IRQ_OPEN_FOR_STOP);
+		rcar_i2c_flags_set(priv, ID_NACK);
+		goto out;
+	}
+
+	/*
+	 * recv/send
+	 */
+	if (rcar_i2c_is_recv(priv))
+		rcar_i2c_flags_set(priv, rcar_i2c_irq_recv(priv, msr));
+	else
+		rcar_i2c_flags_set(priv, rcar_i2c_irq_send(priv, msr));
+
+out:
+	if (rcar_i2c_flags_has(priv, ID_DONE)) {
+		rcar_i2c_irq_mask(priv, RCAR_IRQ_CLOSE);
+		rcar_i2c_status_clear(priv);
+		wake_up(&priv->wait);
+	}
+
+	spin_unlock(&priv->lock);
+	/*-------------- spin unlock -----------------*/
+
+	return IRQ_HANDLED;
+}
+
+static int rcar_i2c_master_xfer(struct i2c_adapter *adap,
+				struct i2c_msg *msgs,
+				int num)
+{
+	struct rcar_i2c_priv *priv = i2c_get_adapdata(adap);
+	struct device *dev = rcar_i2c_priv_to_dev(priv);
+	unsigned long flags;
+	int i, ret, timeout;
+
+	pm_runtime_get_sync(dev);
+
+	/*-------------- spin lock -----------------*/
+	spin_lock_irqsave(&priv->lock, flags);
+
+	rcar_i2c_init(priv);
+	rcar_i2c_clock_start(priv);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+	/*-------------- spin unlock -----------------*/
+
+	ret = -EINVAL;
+	for (i = 0; i < num; i++) {
+		/*-------------- spin lock -----------------*/
+		spin_lock_irqsave(&priv->lock, flags);
+
+		/* init each data */
+		priv->msg	= &msgs[i];
+		priv->pos	= 0;
+		priv->flags	= 0;
+		if (priv->msg == &msgs[num - 1])
+			rcar_i2c_flags_set(priv, ID_LAST_MSG);
+
+		/* start send/recv */
+		if (rcar_i2c_is_recv(priv))
+			ret = rcar_i2c_recv(priv);
+		else
+			ret = rcar_i2c_send(priv);
+
+		spin_unlock_irqrestore(&priv->lock, flags);
+		/*-------------- spin unlock -----------------*/
+
+		if (ret < 0)
+			break;
+
+		/*
+		 * wait result
+		 */
+		timeout = wait_event_timeout(priv->wait,
+					     rcar_i2c_flags_has(priv, ID_DONE),
+					     5 * HZ);
+		if (!timeout) {
+			ret = -ETIMEDOUT;
+			break;
+		}
+
+		/*
+		 * error handling
+		 */
+		if (rcar_i2c_flags_has(priv, ID_NACK)) {
+			ret = -EREMOTEIO;
+			break;
+		}
+
+		if (rcar_i2c_flags_has(priv, ID_ARBLOST)) {
+			ret = -EAGAIN;
+			break;
+		}
+
+		if (rcar_i2c_flags_has(priv, ID_IOERROR)) {
+			ret = -EIO;
+			break;
+		}
+
+		ret = i + 1; /* The number of transfer */
+	}
+
+	pm_runtime_put(dev);
+
+	if (ret < 0)
+		dev_err(dev, "error %d : %x\n", ret, priv->flags);
+
+	return ret;
+}
+
+static u32 rcar_i2c_func(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+}
+
+static const struct i2c_algorithm rcar_i2c_algo = {
+	.master_xfer	= rcar_i2c_master_xfer,
+	.functionality	= rcar_i2c_func,
+};
+
+static int __devinit rcar_i2c_probe(struct platform_device *pdev)
+{
+	struct i2c_rcar_platform_data *pdata = pdev->dev.platform_data;
+	struct rcar_i2c_priv *priv;
+	struct i2c_adapter *adap;
+	struct resource *res;
+	struct device *dev = &pdev->dev;
+	u32 bus_speed;
+	int ret;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(dev, "no mmio resources\n");
+		return -ENODEV;
+	}
+
+	priv = devm_kzalloc(dev, sizeof(struct rcar_i2c_priv), GFP_KERNEL);
+	if (!priv) {
+		dev_err(dev, "no mem for private data\n");
+		return -ENOMEM;
+	}
+
+	bus_speed = 100000; /* default 100 kHz */
+	if (pdata && pdata->bus_speed)
+		bus_speed = pdata->bus_speed;
+	ret = rcar_i2c_clock_calculate(priv, bus_speed, dev);
+	if (ret < 0)
+		return ret;
+
+	priv->io = devm_ioremap(dev, res->start, resource_size(res));
+	if (!priv->io) {
+		dev_err(dev, "cannot ioremap\n");
+		return -ENODEV;
+	}
+
+	priv->irq = platform_get_irq(pdev, 0);
+	init_waitqueue_head(&priv->wait);
+	spin_lock_init(&priv->lock);
+
+	adap			= &priv->adap;
+	adap->nr		= pdev->id;
+	adap->algo		= &rcar_i2c_algo;
+	adap->class		= I2C_CLASS_HWMON | I2C_CLASS_SPD;
+	adap->retries		= 3;
+	adap->dev.parent	= dev;
+	i2c_set_adapdata(adap, priv);
+	strlcpy(adap->name, pdev->name, sizeof(adap->name));
+
+	ret = devm_request_irq(dev, priv->irq, rcar_i2c_irq, 0,
+			       dev_name(dev), priv);
+	if (ret < 0) {
+		dev_err(dev, "cannot get irq %d\n", priv->irq);
+		return ret;
+	}
+
+	ret = i2c_add_numbered_adapter(adap);
+	if (ret < 0) {
+		dev_err(dev, "reg adap failed: %d\n", ret);
+		return ret;
+	}
+
+	pm_runtime_enable(dev);
+	platform_set_drvdata(pdev, priv);
+
+	dev_info(dev, "probed\n");
+
+	return 0;
+}
+
+static int __devexit rcar_i2c_remove(struct platform_device *pdev)
+{
+	struct rcar_i2c_priv *priv = platform_get_drvdata(pdev);
+	struct device *dev = &pdev->dev;
+
+	i2c_del_adapter(&priv->adap);
+	pm_runtime_disable(dev);
+
+	return 0;
+}
+
+static struct platform_driver rcar_i2c_drv = {
+	.driver	= {
+		.name	= "i2c-rcar",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= rcar_i2c_probe,
+	.remove		= __devexit_p(rcar_i2c_remove),
+};
+
+module_platform_driver(rcar_i2c_drv);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Renesas R-Car I2C bus driver");
+MODULE_AUTHOR("Kuninori Morimoto <kuninori.morimoto.gx@renesas.com>");
diff --git a/drivers/i2c/busses/i2c-s3c2410.c b/drivers/i2c/busses/i2c-s3c2410.c
index 4d07dea9bca..3e0335f1fc6 100644
--- a/drivers/i2c/busses/i2c-s3c2410.c
+++ b/drivers/i2c/busses/i2c-s3c2410.c
@@ -601,14 +601,14 @@ static int s3c24xx_i2c_xfer(struct i2c_adapter *adap,
 	int ret;
 
 	pm_runtime_get_sync(&adap->dev);
-	clk_enable(i2c->clk);
+	clk_prepare_enable(i2c->clk);
 
 	for (retry = 0; retry < adap->retries; retry++) {
 
 		ret = s3c24xx_i2c_doxfer(i2c, msgs, num);
 
 		if (ret != -EAGAIN) {
-			clk_disable(i2c->clk);
+			clk_disable_unprepare(i2c->clk);
 			pm_runtime_put(&adap->dev);
 			return ret;
 		}
@@ -618,7 +618,7 @@ static int s3c24xx_i2c_xfer(struct i2c_adapter *adap,
 		udelay(100);
 	}
 
-	clk_disable(i2c->clk);
+	clk_disable_unprepare(i2c->clk);
 	pm_runtime_put(&adap->dev);
 	return -EREMOTEIO;
 }
@@ -977,7 +977,7 @@ static int s3c24xx_i2c_probe(struct platform_device *pdev)
 
 	dev_dbg(&pdev->dev, "clock source %p\n", i2c->clk);
 
-	clk_enable(i2c->clk);
+	clk_prepare_enable(i2c->clk);
 
 	/* map the registers */
 
@@ -1065,7 +1065,7 @@ static int s3c24xx_i2c_probe(struct platform_device *pdev)
 	pm_runtime_enable(&i2c->adap.dev);
 
 	dev_info(&pdev->dev, "%s: S3C I2C adapter\n", dev_name(&i2c->adap.dev));
-	clk_disable(i2c->clk);
+	clk_disable_unprepare(i2c->clk);
 	return 0;
 
  err_cpufreq:
@@ -1082,7 +1082,7 @@ static int s3c24xx_i2c_probe(struct platform_device *pdev)
 	kfree(i2c->ioarea);
 
  err_clk:
-	clk_disable(i2c->clk);
+	clk_disable_unprepare(i2c->clk);
 	clk_put(i2c->clk);
 
  err_noclk:
@@ -1106,7 +1106,7 @@ static int s3c24xx_i2c_remove(struct platform_device *pdev)
 	i2c_del_adapter(&i2c->adap);
 	free_irq(i2c->irq, i2c);
 
-	clk_disable(i2c->clk);
+	clk_disable_unprepare(i2c->clk);
 	clk_put(i2c->clk);
 
 	iounmap(i2c->regs);
@@ -1135,9 +1135,9 @@ static int s3c24xx_i2c_resume(struct device *dev)
 	struct s3c24xx_i2c *i2c = platform_get_drvdata(pdev);
 
 	i2c->suspended = 0;
-	clk_enable(i2c->clk);
+	clk_prepare_enable(i2c->clk);
 	s3c24xx_i2c_init(i2c);
-	clk_disable(i2c->clk);
+	clk_disable_unprepare(i2c->clk);
 
 	return 0;
 }
diff --git a/drivers/i2c/busses/i2c-scmi.c b/drivers/i2c/busses/i2c-scmi.c
index 388cbdc96db..6aafa3d88ff 100644
--- a/drivers/i2c/busses/i2c-scmi.c
+++ b/drivers/i2c/busses/i2c-scmi.c
@@ -426,19 +426,7 @@ static struct acpi_driver acpi_smbus_cmi_driver = {
 		.remove = acpi_smbus_cmi_remove,
 	},
 };
-
-static int __init acpi_smbus_cmi_init(void)
-{
-	return acpi_bus_register_driver(&acpi_smbus_cmi_driver);
-}
-
-static void __exit acpi_smbus_cmi_exit(void)
-{
-	acpi_bus_unregister_driver(&acpi_smbus_cmi_driver);
-}
-
-module_init(acpi_smbus_cmi_init);
-module_exit(acpi_smbus_cmi_exit);
+module_acpi_driver(acpi_smbus_cmi_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Crane Cai <crane.cai@amd.com>");
diff --git a/drivers/i2c/busses/i2c-viapro.c b/drivers/i2c/busses/i2c-viapro.c
index 333011c83d5..271c9a2b0fd 100644
--- a/drivers/i2c/busses/i2c-viapro.c
+++ b/drivers/i2c/busses/i2c-viapro.c
@@ -401,6 +401,7 @@ found:
 	case PCI_DEVICE_ID_VIA_CX700:
 	case PCI_DEVICE_ID_VIA_VX800:
 	case PCI_DEVICE_ID_VIA_VX855:
+	case PCI_DEVICE_ID_VIA_VX900:
 	case PCI_DEVICE_ID_VIA_8251:
 	case PCI_DEVICE_ID_VIA_8237:
 	case PCI_DEVICE_ID_VIA_8237A:
@@ -470,6 +471,8 @@ static DEFINE_PCI_DEVICE_TABLE(vt596_ids) = {
 	  .driver_data = SMBBA3 },
 	{ PCI_DEVICE(PCI_VENDOR_ID_VIA, PCI_DEVICE_ID_VIA_VX855),
 	  .driver_data = SMBBA3 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_VIA, PCI_DEVICE_ID_VIA_VX900),
+	  .driver_data = SMBBA3 },
 	{ 0, }
 };
 
diff --git a/drivers/i2c/busses/scx200_acb.c b/drivers/i2c/busses/scx200_acb.c
index 2eacb7784d5..08aab57337d 100644
--- a/drivers/i2c/busses/scx200_acb.c
+++ b/drivers/i2c/busses/scx200_acb.c
@@ -23,6 +23,8 @@
     Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/errno.h>
 #include <linux/kernel.h>
@@ -37,8 +39,6 @@
 
 #include <linux/scx200.h>
 
-#define NAME "scx200_acb"
-
 MODULE_AUTHOR("Christer Weinigel <wingel@nano-system.com>");
 MODULE_DESCRIPTION("NatSemi SCx200 ACCESS.bus Driver");
 MODULE_ALIAS("platform:cs5535-smb");
@@ -398,7 +398,7 @@ static __devinit int scx200_acb_probe(struct scx200_acb_iface *iface)
 	outb(0x70, ACBCTL2);
 
 	if (inb(ACBCTL2) != 0x70) {
-		pr_debug(NAME ": ACBCTL2 readback failed\n");
+		pr_debug("ACBCTL2 readback failed\n");
 		return -ENXIO;
 	}
 
@@ -406,8 +406,7 @@ static __devinit int scx200_acb_probe(struct scx200_acb_iface *iface)
 
 	val = inb(ACBCTL1);
 	if (val) {
-		pr_debug(NAME ": disabled, but ACBCTL1=0x%02x\n",
-			val);
+		pr_debug("disabled, but ACBCTL1=0x%02x\n", val);
 		return -ENXIO;
 	}
 
@@ -417,8 +416,8 @@ static __devinit int scx200_acb_probe(struct scx200_acb_iface *iface)
 
 	val = inb(ACBCTL1);
 	if ((val & ACBCTL1_NMINTE) != ACBCTL1_NMINTE) {
-		pr_debug(NAME ": enabled, but NMINTE won't be set, "
-			 "ACBCTL1=0x%02x\n", val);
+		pr_debug("enabled, but NMINTE won't be set, ACBCTL1=0x%02x\n",
+			 val);
 		return -ENXIO;
 	}
 
@@ -433,7 +432,7 @@ static __devinit struct scx200_acb_iface *scx200_create_iface(const char *text,
 
 	iface = kzalloc(sizeof(*iface), GFP_KERNEL);
 	if (!iface) {
-		printk(KERN_ERR NAME ": can't allocate memory\n");
+		pr_err("can't allocate memory\n");
 		return NULL;
 	}
 
@@ -459,14 +458,14 @@ static int __devinit scx200_acb_create(struct scx200_acb_iface *iface)
 
 	rc = scx200_acb_probe(iface);
 	if (rc) {
-		printk(KERN_WARNING NAME ": probe failed\n");
+		pr_warn("probe failed\n");
 		return rc;
 	}
 
 	scx200_acb_reset(iface);
 
 	if (i2c_add_adapter(adapter) < 0) {
-		printk(KERN_ERR NAME ": failed to register\n");
+		pr_err("failed to register\n");
 		return -ENODEV;
 	}
 
@@ -493,8 +492,7 @@ static struct scx200_acb_iface * __devinit scx200_create_dev(const char *text,
 		return NULL;
 
 	if (!request_region(base, 8, iface->adapter.name)) {
-		printk(KERN_ERR NAME ": can't allocate io 0x%lx-0x%lx\n",
-		       base, base + 8 - 1);
+		pr_err("can't allocate io 0x%lx-0x%lx\n", base, base + 8 - 1);
 		goto errout_free;
 	}
 
@@ -583,7 +581,7 @@ static __init void scx200_scan_isa(void)
 
 static int __init scx200_acb_init(void)
 {
-	pr_debug(NAME ": NatSemi SCx200 ACCESS.bus Driver\n");
+	pr_debug("NatSemi SCx200 ACCESS.bus Driver\n");
 
 	/* First scan for ISA-based devices */
 	scx200_scan_isa();	/* XXX: should we care about errors? */
diff --git a/drivers/i2c/busses/scx200_i2c.c b/drivers/i2c/busses/scx200_i2c.c
index 7ee0d502cea..ae1258b95d6 100644
--- a/drivers/i2c/busses/scx200_i2c.c
+++ b/drivers/i2c/busses/scx200_i2c.c
@@ -21,6 +21,8 @@
    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.		     
 */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/errno.h>
 #include <linux/kernel.h>
@@ -31,8 +33,6 @@
 
 #include <linux/scx200_gpio.h>
 
-#define NAME "scx200_i2c"
-
 MODULE_AUTHOR("Christer Weinigel <wingel@nano-system.com>");
 MODULE_DESCRIPTION("NatSemi SCx200 I2C Driver");
 MODULE_LICENSE("GPL");
@@ -88,17 +88,17 @@ static struct i2c_adapter scx200_i2c_ops = {
 
 static int scx200_i2c_init(void)
 {
-	pr_debug(NAME ": NatSemi SCx200 I2C Driver\n");
+	pr_debug("NatSemi SCx200 I2C Driver\n");
 
 	if (!scx200_gpio_present()) {
-		printk(KERN_ERR NAME ": no SCx200 gpio pins available\n");
+		pr_err("no SCx200 gpio pins available\n");
 		return -ENODEV;
 	}
 
-	pr_debug(NAME ": SCL=GPIO%02u, SDA=GPIO%02u\n", scl, sda);
+	pr_debug("SCL=GPIO%02u, SDA=GPIO%02u\n", scl, sda);
 
 	if (scl == -1 || sda == -1 || scl == sda) {
-		printk(KERN_ERR NAME ": scl and sda must be specified\n");
+		pr_err("scl and sda must be specified\n");
 		return -EINVAL;
 	}
 
@@ -107,8 +107,7 @@ static int scx200_i2c_init(void)
 	scx200_gpio_configure(sda, ~2, 5);
 
 	if (i2c_bit_add_bus(&scx200_i2c_ops) < 0) {
-		printk(KERN_ERR NAME ": adapter %s registration failed\n", 
-		       scx200_i2c_ops.name);
+		pr_err("adapter %s registration failed\n", scx200_i2c_ops.name);
 		return -ENODEV;
 	}
 	
diff --git a/drivers/i2c/i2c-core.c b/drivers/i2c/i2c-core.c
index 2421d95130d..a7edf987a33 100644
--- a/drivers/i2c/i2c-core.c
+++ b/drivers/i2c/i2c-core.c
@@ -1989,12 +1989,22 @@ static s32 i2c_smbus_xfer_emulated(struct i2c_adapter *adapter, u16 addr,
 	unsigned char msgbuf0[I2C_SMBUS_BLOCK_MAX+3];
 	unsigned char msgbuf1[I2C_SMBUS_BLOCK_MAX+2];
 	int num = read_write == I2C_SMBUS_READ ? 2 : 1;
-	struct i2c_msg msg[2] = { { addr, flags, 1, msgbuf0 },
-	                          { addr, flags | I2C_M_RD, 0, msgbuf1 }
-	                        };
 	int i;
 	u8 partial_pec = 0;
 	int status;
+	struct i2c_msg msg[2] = {
+		{
+			.addr = addr,
+			.flags = flags,
+			.len = 1,
+			.buf = msgbuf0,
+		}, {
+			.addr = addr,
+			.flags = flags | I2C_M_RD,
+			.len = 0,
+			.buf = msgbuf1,
+		},
+	};
 
 	msgbuf0[0] = command;
 	switch (size) {
diff --git a/drivers/i2c/i2c-mux.c b/drivers/i2c/i2c-mux.c
index 1038c381aea..d94e0ce7827 100644
--- a/drivers/i2c/i2c-mux.c
+++ b/drivers/i2c/i2c-mux.c
@@ -88,9 +88,23 @@ static u32 i2c_mux_functionality(struct i2c_adapter *adap)
 	return parent->algo->functionality(parent);
 }
 
+/* Return all parent classes, merged */
+static unsigned int i2c_mux_parent_classes(struct i2c_adapter *parent)
+{
+	unsigned int class = 0;
+
+	do {
+		class |= parent->class;
+		parent = i2c_parent_is_i2c_adapter(parent);
+	} while (parent);
+
+	return class;
+}
+
 struct i2c_adapter *i2c_add_mux_adapter(struct i2c_adapter *parent,
 				struct device *mux_dev,
 				void *mux_priv, u32 force_nr, u32 chan_id,
+				unsigned int class,
 				int (*select) (struct i2c_adapter *,
 					       void *, u32),
 				int (*deselect) (struct i2c_adapter *,
@@ -127,6 +141,14 @@ struct i2c_adapter *i2c_add_mux_adapter(struct i2c_adapter *parent,
 	priv->adap.algo_data = priv;
 	priv->adap.dev.parent = &parent->dev;
 
+	/* Sanity check on class */
+	if (i2c_mux_parent_classes(parent) & class)
+		dev_err(&parent->dev,
+			"Segment %d behind mux can't share classes with ancestors\n",
+			chan_id);
+	else
+		priv->adap.class = class;
+
 	/*
 	 * Try to populate the mux adapter's of_node, expands to
 	 * nothing if !CONFIG_OF.
diff --git a/drivers/i2c/i2c-smbus.c b/drivers/i2c/i2c-smbus.c
index df3e0bf31eb..92cdd2323b0 100644
--- a/drivers/i2c/i2c-smbus.c
+++ b/drivers/i2c/i2c-smbus.c
@@ -142,7 +142,8 @@ static int smbalert_probe(struct i2c_client *ara,
 	struct i2c_adapter *adapter = ara->adapter;
 	int res;
 
-	alert = kzalloc(sizeof(struct i2c_smbus_alert), GFP_KERNEL);
+	alert = devm_kzalloc(&ara->dev, sizeof(struct i2c_smbus_alert),
+			     GFP_KERNEL);
 	if (!alert)
 		return -ENOMEM;
 
@@ -154,10 +155,8 @@ static int smbalert_probe(struct i2c_client *ara,
 	if (setup->irq > 0) {
 		res = devm_request_irq(&ara->dev, setup->irq, smbalert_irq,
 				       0, "smbus_alert", alert);
-		if (res) {
-			kfree(alert);
+		if (res)
 			return res;
-		}
 	}
 
 	i2c_set_clientdata(ara, alert);
@@ -167,14 +166,12 @@ static int smbalert_probe(struct i2c_client *ara,
 	return 0;
 }
 
-/* IRQ resource is managed so it is freed automatically */
+/* IRQ and memory resources are managed so they are freed automatically */
 static int smbalert_remove(struct i2c_client *ara)
 {
 	struct i2c_smbus_alert *alert = i2c_get_clientdata(ara);
 
 	cancel_work_sync(&alert->alert);
-
-	kfree(alert);
 	return 0;
 }
 
diff --git a/drivers/i2c/muxes/i2c-mux-gpio.c b/drivers/i2c/muxes/i2c-mux-gpio.c
index 68b1f8ec343..566a6757a33 100644
--- a/drivers/i2c/muxes/i2c-mux-gpio.c
+++ b/drivers/i2c/muxes/i2c-mux-gpio.c
@@ -21,6 +21,7 @@ struct gpiomux {
 	struct i2c_adapter *parent;
 	struct i2c_adapter **adap; /* child busses */
 	struct i2c_mux_gpio_platform_data data;
+	unsigned gpio_base;
 };
 
 static void i2c_mux_gpio_set(const struct gpiomux *mux, unsigned val)
@@ -28,7 +29,8 @@ static void i2c_mux_gpio_set(const struct gpiomux *mux, unsigned val)
 	int i;
 
 	for (i = 0; i < mux->data.n_gpios; i++)
-		gpio_set_value(mux->data.gpios[i], val & (1 << i));
+		gpio_set_value(mux->gpio_base + mux->data.gpios[i],
+			       val & (1 << i));
 }
 
 static int i2c_mux_gpio_select(struct i2c_adapter *adap, void *data, u32 chan)
@@ -49,13 +51,19 @@ static int i2c_mux_gpio_deselect(struct i2c_adapter *adap, void *data, u32 chan)
 	return 0;
 }
 
+static int __devinit match_gpio_chip_by_label(struct gpio_chip *chip,
+					      void *data)
+{
+	return !strcmp(chip->label, data);
+}
+
 static int __devinit i2c_mux_gpio_probe(struct platform_device *pdev)
 {
 	struct gpiomux *mux;
 	struct i2c_mux_gpio_platform_data *pdata;
 	struct i2c_adapter *parent;
 	int (*deselect) (struct i2c_adapter *, void *, u32);
-	unsigned initial_state;
+	unsigned initial_state, gpio_base;
 	int i, ret;
 
 	pdata = pdev->dev.platform_data;
@@ -64,6 +72,23 @@ static int __devinit i2c_mux_gpio_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
+	/*
+	 * If a GPIO chip name is provided, the GPIO pin numbers provided are
+	 * relative to its base GPIO number. Otherwise they are absolute.
+	 */
+	if (pdata->gpio_chip) {
+		struct gpio_chip *gpio;
+
+		gpio = gpiochip_find(pdata->gpio_chip,
+				     match_gpio_chip_by_label);
+		if (!gpio)
+			return -EPROBE_DEFER;
+
+		gpio_base = gpio->base;
+	} else {
+		gpio_base = 0;
+	}
+
 	parent = i2c_get_adapter(pdata->parent);
 	if (!parent) {
 		dev_err(&pdev->dev, "Parent adapter (%d) not found\n",
@@ -71,7 +96,7 @@ static int __devinit i2c_mux_gpio_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	mux = kzalloc(sizeof(*mux), GFP_KERNEL);
+	mux = devm_kzalloc(&pdev->dev, sizeof(*mux), GFP_KERNEL);
 	if (!mux) {
 		ret = -ENOMEM;
 		goto alloc_failed;
@@ -79,11 +104,13 @@ static int __devinit i2c_mux_gpio_probe(struct platform_device *pdev)
 
 	mux->parent = parent;
 	mux->data = *pdata;
-	mux->adap = kzalloc(sizeof(struct i2c_adapter *) * pdata->n_values,
-			    GFP_KERNEL);
+	mux->gpio_base = gpio_base;
+	mux->adap = devm_kzalloc(&pdev->dev,
+				 sizeof(*mux->adap) * pdata->n_values,
+				 GFP_KERNEL);
 	if (!mux->adap) {
 		ret = -ENOMEM;
-		goto alloc_failed2;
+		goto alloc_failed;
 	}
 
 	if (pdata->idle != I2C_MUX_GPIO_NO_IDLE) {
@@ -95,17 +122,19 @@ static int __devinit i2c_mux_gpio_probe(struct platform_device *pdev)
 	}
 
 	for (i = 0; i < pdata->n_gpios; i++) {
-		ret = gpio_request(pdata->gpios[i], "i2c-mux-gpio");
+		ret = gpio_request(gpio_base + pdata->gpios[i], "i2c-mux-gpio");
 		if (ret)
 			goto err_request_gpio;
-		gpio_direction_output(pdata->gpios[i],
+		gpio_direction_output(gpio_base + pdata->gpios[i],
 				      initial_state & (1 << i));
 	}
 
 	for (i = 0; i < pdata->n_values; i++) {
 		u32 nr = pdata->base_nr ? (pdata->base_nr + i) : 0;
+		unsigned int class = pdata->classes ? pdata->classes[i] : 0;
 
-		mux->adap[i] = i2c_add_mux_adapter(parent, &pdev->dev, mux, nr, i,
+		mux->adap[i] = i2c_add_mux_adapter(parent, &pdev->dev, mux, nr,
+						   i, class,
 						   i2c_mux_gpio_select, deselect);
 		if (!mux->adap[i]) {
 			ret = -ENODEV;
@@ -127,10 +156,7 @@ add_adapter_failed:
 	i = pdata->n_gpios;
 err_request_gpio:
 	for (; i > 0; i--)
-		gpio_free(pdata->gpios[i - 1]);
-	kfree(mux->adap);
-alloc_failed2:
-	kfree(mux);
+		gpio_free(gpio_base + pdata->gpios[i - 1]);
 alloc_failed:
 	i2c_put_adapter(parent);
 
@@ -146,12 +172,10 @@ static int __devexit i2c_mux_gpio_remove(struct platform_device *pdev)
 		i2c_del_mux_adapter(mux->adap[i]);
 
 	for (i = 0; i < mux->data.n_gpios; i++)
-		gpio_free(mux->data.gpios[i]);
+		gpio_free(mux->gpio_base + mux->data.gpios[i]);
 
 	platform_set_drvdata(pdev, NULL);
 	i2c_put_adapter(mux->parent);
-	kfree(mux->adap);
-	kfree(mux);
 
 	return 0;
 }
diff --git a/drivers/i2c/muxes/i2c-mux-pca9541.c b/drivers/i2c/muxes/i2c-mux-pca9541.c
index f8f72f39e0b..f3b8f9a6a89 100644
--- a/drivers/i2c/muxes/i2c-mux-pca9541.c
+++ b/drivers/i2c/muxes/i2c-mux-pca9541.c
@@ -354,7 +354,7 @@ static int pca9541_probe(struct i2c_client *client,
 	if (pdata)
 		force = pdata->modes[0].adap_id;
 	data->mux_adap = i2c_add_mux_adapter(adap, &client->dev, client,
-					     force, 0,
+					     force, 0, 0,
 					     pca9541_select_chan,
 					     pca9541_release_chan);
 
diff --git a/drivers/i2c/muxes/i2c-mux-pca954x.c b/drivers/i2c/muxes/i2c-mux-pca954x.c
index f2dfe0d8fcc..8e4387235b6 100644
--- a/drivers/i2c/muxes/i2c-mux-pca954x.c
+++ b/drivers/i2c/muxes/i2c-mux-pca954x.c
@@ -186,7 +186,7 @@ static int pca954x_probe(struct i2c_client *client,
 {
 	struct i2c_adapter *adap = to_i2c_adapter(client->dev.parent);
 	struct pca954x_platform_data *pdata = client->dev.platform_data;
-	int num, force;
+	int num, force, class;
 	struct pca954x *data;
 	int ret = -ENODEV;
 
@@ -216,18 +216,20 @@ static int pca954x_probe(struct i2c_client *client,
 	/* Now create an adapter for each channel */
 	for (num = 0; num < chips[data->type].nchans; num++) {
 		force = 0;			  /* dynamic adap number */
+		class = 0;			  /* no class by default */
 		if (pdata) {
-			if (num < pdata->num_modes)
+			if (num < pdata->num_modes) {
 				/* force static number */
 				force = pdata->modes[num].adap_id;
-			else
+				class = pdata->modes[num].class;
+			} else
 				/* discard unconfigured channels */
 				break;
 		}
 
 		data->virt_adaps[num] =
 			i2c_add_mux_adapter(adap, &client->dev, client,
-				force, num, pca954x_select_chan,
+				force, num, class, pca954x_select_chan,
 				(pdata && pdata->modes[num].deselect_on_exit)
 					? pca954x_deselect_mux : NULL);
 
diff --git a/drivers/i2c/muxes/i2c-mux-pinctrl.c b/drivers/i2c/muxes/i2c-mux-pinctrl.c
index 46a66976347..5f097f309b9 100644
--- a/drivers/i2c/muxes/i2c-mux-pinctrl.c
+++ b/drivers/i2c/muxes/i2c-mux-pinctrl.c
@@ -221,7 +221,7 @@ static int __devinit i2c_mux_pinctrl_probe(struct platform_device *pdev)
 				(mux->pdata->base_bus_num + i) : 0;
 
 		mux->busses[i] = i2c_add_mux_adapter(mux->parent, &pdev->dev,
-						     mux, bus, i,
+						     mux, bus, i, 0,
 						     i2c_mux_pinctrl_select,
 						     deselect);
 		if (!mux->busses[i]) {
diff --git a/drivers/idle/intel_idle.c b/drivers/idle/intel_idle.c
index e8726177d10..b0f6b4c8ee1 100644
--- a/drivers/idle/intel_idle.c
+++ b/drivers/idle/intel_idle.c
@@ -413,6 +413,7 @@ static const struct x86_cpu_id intel_idle_ids[] = {
 	ICPU(0x2a, idle_cpu_snb),
 	ICPU(0x2d, idle_cpu_snb),
 	ICPU(0x3a, idle_cpu_ivb),
+	ICPU(0x3e, idle_cpu_ivb),
 	{}
 };
 MODULE_DEVICE_TABLE(x86cpu, intel_idle_ids);
diff --git a/drivers/infiniband/core/cma.c b/drivers/infiniband/core/cma.c
index 1983adc1924..a7568c34a1a 100644
--- a/drivers/infiniband/core/cma.c
+++ b/drivers/infiniband/core/cma.c
@@ -3498,7 +3498,8 @@ out:
 }
 
 static const struct ibnl_client_cbs cma_cb_table[] = {
-	[RDMA_NL_RDMA_CM_ID_STATS] = { .dump = cma_get_id_stats },
+	[RDMA_NL_RDMA_CM_ID_STATS] = { .dump = cma_get_id_stats,
+				       .module = THIS_MODULE },
 };
 
 static int __init cma_init(void)
diff --git a/drivers/infiniband/core/netlink.c b/drivers/infiniband/core/netlink.c
index fe10a949aef..da06abde9e0 100644
--- a/drivers/infiniband/core/netlink.c
+++ b/drivers/infiniband/core/netlink.c
@@ -154,6 +154,7 @@ static int ibnl_rcv_msg(struct sk_buff *skb, struct nlmsghdr *nlh)
 			{
 				struct netlink_dump_control c = {
 					.dump = client->cb_table[op].dump,
+					.module = client->cb_table[op].module,
 				};
 				return netlink_dump_start(nls, skb, nlh, &c);
 			}
diff --git a/drivers/infiniband/hw/ehca/ehca_uverbs.c b/drivers/infiniband/hw/ehca/ehca_uverbs.c
index 45ee89b65c2..1a1d5d99fcf 100644
--- a/drivers/infiniband/hw/ehca/ehca_uverbs.c
+++ b/drivers/infiniband/hw/ehca/ehca_uverbs.c
@@ -117,7 +117,7 @@ static int ehca_mmap_fw(struct vm_area_struct *vma, struct h_galpas *galpas,
 	physical = galpas->user.fw_handle;
 	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
 	ehca_gen_dbg("vsize=%llx physical=%llx", vsize, physical);
-	/* VM_IO | VM_RESERVED are set by remap_pfn_range() */
+	/* VM_IO | VM_DONTEXPAND | VM_DONTDUMP are set by remap_pfn_range() */
 	ret = remap_4k_pfn(vma, vma->vm_start, physical >> EHCA_PAGESHIFT,
 			   vma->vm_page_prot);
 	if (unlikely(ret)) {
@@ -139,7 +139,7 @@ static int ehca_mmap_queue(struct vm_area_struct *vma, struct ipz_queue *queue,
 	u64 start, ofs;
 	struct page *page;
 
-	vma->vm_flags |= VM_RESERVED;
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
 	start = vma->vm_start;
 	for (ofs = 0; ofs < queue->queue_length; ofs += PAGE_SIZE) {
 		u64 virt_addr = (u64)ipz_qeit_calc(queue, ofs);
diff --git a/drivers/infiniband/hw/ipath/ipath_file_ops.c b/drivers/infiniband/hw/ipath/ipath_file_ops.c
index 736d9edbdbe..3eb7e454849 100644
--- a/drivers/infiniband/hw/ipath/ipath_file_ops.c
+++ b/drivers/infiniband/hw/ipath/ipath_file_ops.c
@@ -1225,7 +1225,7 @@ static int mmap_kvaddr(struct vm_area_struct *vma, u64 pgaddr,
 
 	vma->vm_pgoff = (unsigned long) addr >> PAGE_SHIFT;
 	vma->vm_ops = &ipath_file_vm_ops;
-	vma->vm_flags |= VM_RESERVED | VM_DONTEXPAND;
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
 	ret = 1;
 
 bail:
diff --git a/drivers/infiniband/hw/qib/qib_file_ops.c b/drivers/infiniband/hw/qib/qib_file_ops.c
index faa44cb0807..959a5c4ff81 100644
--- a/drivers/infiniband/hw/qib/qib_file_ops.c
+++ b/drivers/infiniband/hw/qib/qib_file_ops.c
@@ -971,7 +971,7 @@ static int mmap_kvaddr(struct vm_area_struct *vma, u64 pgaddr,
 
 	vma->vm_pgoff = (unsigned long) addr >> PAGE_SHIFT;
 	vma->vm_ops = &qib_file_vm_ops;
-	vma->vm_flags |= VM_RESERVED | VM_DONTEXPAND;
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
 	ret = 1;
 
 bail:
diff --git a/drivers/infiniband/ulp/srpt/ib_srpt.c b/drivers/infiniband/ulp/srpt/ib_srpt.c
index 9e1449f8c6a..cf23c46185b 100644
--- a/drivers/infiniband/ulp/srpt/ib_srpt.c
+++ b/drivers/infiniband/ulp/srpt/ib_srpt.c
@@ -3564,16 +3564,6 @@ static int srpt_get_tcm_cmd_state(struct se_cmd *se_cmd)
 	return srpt_get_cmd_state(ioctx);
 }
 
-static u16 srpt_set_fabric_sense_len(struct se_cmd *cmd, u32 sense_length)
-{
-	return 0;
-}
-
-static u16 srpt_get_fabric_sense_len(void)
-{
-	return 0;
-}
-
 /**
  * srpt_parse_i_port_id() - Parse an initiator port ID.
  * @name: ASCII representation of a 128-bit initiator port ID.
@@ -3953,8 +3943,6 @@ static struct target_core_fabric_ops srpt_template = {
 	.queue_data_in			= srpt_queue_response,
 	.queue_status			= srpt_queue_status,
 	.queue_tm_rsp			= srpt_queue_response,
-	.get_fabric_sense_len		= srpt_get_fabric_sense_len,
-	.set_fabric_sense_len		= srpt_set_fabric_sense_len,
 	/*
 	 * Setup function pointers for generic logic in
 	 * target_core_fabric_configfs.c
diff --git a/drivers/input/evdev.c b/drivers/input/evdev.c
index 118d0300f1f..6ae2ac47c9c 100644
--- a/drivers/input/evdev.c
+++ b/drivers/input/evdev.c
@@ -23,11 +23,11 @@
 #include <linux/input/mt.h>
 #include <linux/major.h>
 #include <linux/device.h>
+#include <linux/cdev.h>
 #include "input-compat.h"
 
 struct evdev {
 	int open;
-	int minor;
 	struct input_handle handle;
 	wait_queue_head_t wait;
 	struct evdev_client __rcu *grab;
@@ -35,6 +35,7 @@ struct evdev {
 	spinlock_t client_lock; /* protects client_list */
 	struct mutex mutex;
 	struct device dev;
+	struct cdev cdev;
 	bool exist;
 };
 
@@ -51,9 +52,6 @@ struct evdev_client {
 	struct input_event buffer[];
 };
 
-static struct evdev *evdev_table[EVDEV_MINORS];
-static DEFINE_MUTEX(evdev_table_mutex);
-
 static void __pass_event(struct evdev_client *client,
 			 const struct input_event *event)
 {
@@ -310,35 +308,16 @@ static unsigned int evdev_compute_buffer_size(struct input_dev *dev)
 
 static int evdev_open(struct inode *inode, struct file *file)
 {
-	struct evdev *evdev;
+	struct evdev *evdev = container_of(inode->i_cdev, struct evdev, cdev);
+	unsigned int bufsize = evdev_compute_buffer_size(evdev->handle.dev);
 	struct evdev_client *client;
-	int i = iminor(inode) - EVDEV_MINOR_BASE;
-	unsigned int bufsize;
 	int error;
 
-	if (i >= EVDEV_MINORS)
-		return -ENODEV;
-
-	error = mutex_lock_interruptible(&evdev_table_mutex);
-	if (error)
-		return error;
-	evdev = evdev_table[i];
-	if (evdev)
-		get_device(&evdev->dev);
-	mutex_unlock(&evdev_table_mutex);
-
-	if (!evdev)
-		return -ENODEV;
-
-	bufsize = evdev_compute_buffer_size(evdev->handle.dev);
-
 	client = kzalloc(sizeof(struct evdev_client) +
 				bufsize * sizeof(struct input_event),
 			 GFP_KERNEL);
-	if (!client) {
-		error = -ENOMEM;
-		goto err_put_evdev;
-	}
+	if (!client)
+		return -ENOMEM;
 
 	client->bufsize = bufsize;
 	spin_lock_init(&client->buffer_lock);
@@ -352,13 +331,12 @@ static int evdev_open(struct inode *inode, struct file *file)
 	file->private_data = client;
 	nonseekable_open(inode, file);
 
+	get_device(&evdev->dev);
 	return 0;
 
  err_free_client:
 	evdev_detach_client(evdev, client);
 	kfree(client);
- err_put_evdev:
-	put_device(&evdev->dev);
 	return error;
 }
 
@@ -942,26 +920,6 @@ static const struct file_operations evdev_fops = {
 	.llseek		= no_llseek,
 };
 
-static int evdev_install_chrdev(struct evdev *evdev)
-{
-	/*
-	 * No need to do any locking here as calls to connect and
-	 * disconnect are serialized by the input core
-	 */
-	evdev_table[evdev->minor] = evdev;
-	return 0;
-}
-
-static void evdev_remove_chrdev(struct evdev *evdev)
-{
-	/*
-	 * Lock evdev table to prevent race with evdev_open()
-	 */
-	mutex_lock(&evdev_table_mutex);
-	evdev_table[evdev->minor] = NULL;
-	mutex_unlock(&evdev_table_mutex);
-}
-
 /*
  * Mark device non-existent. This disables writes, ioctls and
  * prevents new users from opening the device. Already posted
@@ -980,7 +938,8 @@ static void evdev_cleanup(struct evdev *evdev)
 
 	evdev_mark_dead(evdev);
 	evdev_hangup(evdev);
-	evdev_remove_chrdev(evdev);
+
+	cdev_del(&evdev->cdev);
 
 	/* evdev is marked dead so no one else accesses evdev->open */
 	if (evdev->open) {
@@ -991,43 +950,47 @@ static void evdev_cleanup(struct evdev *evdev)
 
 /*
  * Create new evdev device. Note that input core serializes calls
- * to connect and disconnect so we don't need to lock evdev_table here.
+ * to connect and disconnect.
  */
 static int evdev_connect(struct input_handler *handler, struct input_dev *dev,
 			 const struct input_device_id *id)
 {
 	struct evdev *evdev;
 	int minor;
+	int dev_no;
 	int error;
 
-	for (minor = 0; minor < EVDEV_MINORS; minor++)
-		if (!evdev_table[minor])
-			break;
-
-	if (minor == EVDEV_MINORS) {
-		pr_err("no more free evdev devices\n");
-		return -ENFILE;
+	minor = input_get_new_minor(EVDEV_MINOR_BASE, EVDEV_MINORS, true);
+	if (minor < 0) {
+		error = minor;
+		pr_err("failed to reserve new minor: %d\n", error);
+		return error;
 	}
 
 	evdev = kzalloc(sizeof(struct evdev), GFP_KERNEL);
-	if (!evdev)
-		return -ENOMEM;
+	if (!evdev) {
+		error = -ENOMEM;
+		goto err_free_minor;
+	}
 
 	INIT_LIST_HEAD(&evdev->client_list);
 	spin_lock_init(&evdev->client_lock);
 	mutex_init(&evdev->mutex);
 	init_waitqueue_head(&evdev->wait);
-
-	dev_set_name(&evdev->dev, "event%d", minor);
 	evdev->exist = true;
-	evdev->minor = minor;
+
+	dev_no = minor;
+	/* Normalize device number if it falls into legacy range */
+	if (dev_no < EVDEV_MINOR_BASE + EVDEV_MINORS)
+		dev_no -= EVDEV_MINOR_BASE;
+	dev_set_name(&evdev->dev, "event%d", dev_no);
 
 	evdev->handle.dev = input_get_device(dev);
 	evdev->handle.name = dev_name(&evdev->dev);
 	evdev->handle.handler = handler;
 	evdev->handle.private = evdev;
 
-	evdev->dev.devt = MKDEV(INPUT_MAJOR, EVDEV_MINOR_BASE + minor);
+	evdev->dev.devt = MKDEV(INPUT_MAJOR, minor);
 	evdev->dev.class = &input_class;
 	evdev->dev.parent = &dev->dev;
 	evdev->dev.release = evdev_free;
@@ -1037,7 +1000,8 @@ static int evdev_connect(struct input_handler *handler, struct input_dev *dev,
 	if (error)
 		goto err_free_evdev;
 
-	error = evdev_install_chrdev(evdev);
+	cdev_init(&evdev->cdev, &evdev_fops);
+	error = cdev_add(&evdev->cdev, evdev->dev.devt, 1);
 	if (error)
 		goto err_unregister_handle;
 
@@ -1053,6 +1017,8 @@ static int evdev_connect(struct input_handler *handler, struct input_dev *dev,
 	input_unregister_handle(&evdev->handle);
  err_free_evdev:
 	put_device(&evdev->dev);
+ err_free_minor:
+	input_free_minor(minor);
 	return error;
 }
 
@@ -1062,6 +1028,7 @@ static void evdev_disconnect(struct input_handle *handle)
 
 	device_del(&evdev->dev);
 	evdev_cleanup(evdev);
+	input_free_minor(MINOR(evdev->dev.devt));
 	input_unregister_handle(handle);
 	put_device(&evdev->dev);
 }
@@ -1078,7 +1045,7 @@ static struct input_handler evdev_handler = {
 	.events		= evdev_events,
 	.connect	= evdev_connect,
 	.disconnect	= evdev_disconnect,
-	.fops		= &evdev_fops,
+	.legacy_minors	= true,
 	.minor		= EVDEV_MINOR_BASE,
 	.name		= "evdev",
 	.id_table	= evdev_ids,
diff --git a/drivers/input/input.c b/drivers/input/input.c
index ace3f7c4226..53a0ddee787 100644
--- a/drivers/input/input.c
+++ b/drivers/input/input.c
@@ -14,6 +14,7 @@
 
 #include <linux/init.h>
 #include <linux/types.h>
+#include <linux/idr.h>
 #include <linux/input/mt.h>
 #include <linux/module.h>
 #include <linux/slab.h>
@@ -32,7 +33,9 @@ MODULE_AUTHOR("Vojtech Pavlik <vojtech@suse.cz>");
 MODULE_DESCRIPTION("Input core");
 MODULE_LICENSE("GPL");
 
-#define INPUT_DEVICES	256
+#define INPUT_MAX_CHAR_DEVICES		1024
+#define INPUT_FIRST_DYNAMIC_DEV		256
+static DEFINE_IDA(input_ida);
 
 static LIST_HEAD(input_dev_list);
 static LIST_HEAD(input_handler_list);
@@ -45,8 +48,6 @@ static LIST_HEAD(input_handler_list);
  */
 static DEFINE_MUTEX(input_mutex);
 
-static struct input_handler *input_table[8];
-
 static const struct input_value input_value_sync = { EV_SYN, SYN_REPORT, 1 };
 
 static inline int is_event_supported(unsigned int code,
@@ -1218,7 +1219,7 @@ static int input_handlers_seq_show(struct seq_file *seq, void *v)
 	seq_printf(seq, "N: Number=%u Name=%s", state->pos, handler->name);
 	if (handler->filter)
 		seq_puts(seq, " (filter)");
-	if (handler->fops)
+	if (handler->legacy_minors)
 		seq_printf(seq, " Minor=%d", handler->minor);
 	seq_putc(seq, '\n');
 
@@ -2016,22 +2017,14 @@ EXPORT_SYMBOL(input_unregister_device);
 int input_register_handler(struct input_handler *handler)
 {
 	struct input_dev *dev;
-	int retval;
+	int error;
 
-	retval = mutex_lock_interruptible(&input_mutex);
-	if (retval)
-		return retval;
+	error = mutex_lock_interruptible(&input_mutex);
+	if (error)
+		return error;
 
 	INIT_LIST_HEAD(&handler->h_list);
 
-	if (handler->fops != NULL) {
-		if (input_table[handler->minor >> 5]) {
-			retval = -EBUSY;
-			goto out;
-		}
-		input_table[handler->minor >> 5] = handler;
-	}
-
 	list_add_tail(&handler->node, &input_handler_list);
 
 	list_for_each_entry(dev, &input_dev_list, node)
@@ -2039,9 +2032,8 @@ int input_register_handler(struct input_handler *handler)
 
 	input_wakeup_procfs_readers();
 
- out:
 	mutex_unlock(&input_mutex);
-	return retval;
+	return 0;
 }
 EXPORT_SYMBOL(input_register_handler);
 
@@ -2064,9 +2056,6 @@ void input_unregister_handler(struct input_handler *handler)
 
 	list_del_init(&handler->node);
 
-	if (handler->fops != NULL)
-		input_table[handler->minor >> 5] = NULL;
-
 	input_wakeup_procfs_readers();
 
 	mutex_unlock(&input_mutex);
@@ -2183,51 +2172,52 @@ void input_unregister_handle(struct input_handle *handle)
 }
 EXPORT_SYMBOL(input_unregister_handle);
 
-static int input_open_file(struct inode *inode, struct file *file)
+/**
+ * input_get_new_minor - allocates a new input minor number
+ * @legacy_base: beginning or the legacy range to be searched
+ * @legacy_num: size of legacy range
+ * @allow_dynamic: whether we can also take ID from the dynamic range
+ *
+ * This function allocates a new device minor for from input major namespace.
+ * Caller can request legacy minor by specifying @legacy_base and @legacy_num
+ * parameters and whether ID can be allocated from dynamic range if there are
+ * no free IDs in legacy range.
+ */
+int input_get_new_minor(int legacy_base, unsigned int legacy_num,
+			bool allow_dynamic)
 {
-	struct input_handler *handler;
-	const struct file_operations *old_fops, *new_fops = NULL;
-	int err;
-
-	err = mutex_lock_interruptible(&input_mutex);
-	if (err)
-		return err;
-
-	/* No load-on-demand here? */
-	handler = input_table[iminor(inode) >> 5];
-	if (handler)
-		new_fops = fops_get(handler->fops);
-
-	mutex_unlock(&input_mutex);
-
 	/*
-	 * That's _really_ odd. Usually NULL ->open means "nothing special",
-	 * not "no device". Oh, well...
+	 * This function should be called from input handler's ->connect()
+	 * methods, which are serialized with input_mutex, so no additional
+	 * locking is needed here.
 	 */
-	if (!new_fops || !new_fops->open) {
-		fops_put(new_fops);
-		err = -ENODEV;
-		goto out;
+	if (legacy_base >= 0) {
+		int minor = ida_simple_get(&input_ida,
+					   legacy_base,
+					   legacy_base + legacy_num,
+					   GFP_KERNEL);
+		if (minor >= 0 || !allow_dynamic)
+			return minor;
 	}
 
-	old_fops = file->f_op;
-	file->f_op = new_fops;
-
-	err = new_fops->open(inode, file);
-	if (err) {
-		fops_put(file->f_op);
-		file->f_op = fops_get(old_fops);
-	}
-	fops_put(old_fops);
-out:
-	return err;
+	return ida_simple_get(&input_ida,
+			      INPUT_FIRST_DYNAMIC_DEV, INPUT_MAX_CHAR_DEVICES,
+			      GFP_KERNEL);
 }
+EXPORT_SYMBOL(input_get_new_minor);
 
-static const struct file_operations input_fops = {
-	.owner = THIS_MODULE,
-	.open = input_open_file,
-	.llseek = noop_llseek,
-};
+/**
+ * input_free_minor - release previously allocated minor
+ * @minor: minor to be released
+ *
+ * This function releases previously allocated input minor so that it can be
+ * reused later.
+ */
+void input_free_minor(unsigned int minor)
+{
+	ida_simple_remove(&input_ida, minor);
+}
+EXPORT_SYMBOL(input_free_minor);
 
 static int __init input_init(void)
 {
@@ -2243,7 +2233,8 @@ static int __init input_init(void)
 	if (err)
 		goto fail1;
 
-	err = register_chrdev(INPUT_MAJOR, "input", &input_fops);
+	err = register_chrdev_region(MKDEV(INPUT_MAJOR, 0),
+				     INPUT_MAX_CHAR_DEVICES, "input");
 	if (err) {
 		pr_err("unable to register char major %d", INPUT_MAJOR);
 		goto fail2;
@@ -2259,7 +2250,8 @@ static int __init input_init(void)
 static void __exit input_exit(void)
 {
 	input_proc_exit();
-	unregister_chrdev(INPUT_MAJOR, "input");
+	unregister_chrdev_region(MKDEV(INPUT_MAJOR, 0),
+				 INPUT_MAX_CHAR_DEVICES);
 	class_unregister(&input_class);
 }
 
diff --git a/drivers/input/joydev.c b/drivers/input/joydev.c
index 78f323ea1e4..b62b5891f39 100644
--- a/drivers/input/joydev.c
+++ b/drivers/input/joydev.c
@@ -27,6 +27,7 @@
 #include <linux/poll.h>
 #include <linux/init.h>
 #include <linux/device.h>
+#include <linux/cdev.h>
 
 MODULE_AUTHOR("Vojtech Pavlik <vojtech@ucw.cz>");
 MODULE_DESCRIPTION("Joystick device interfaces");
@@ -39,13 +40,13 @@ MODULE_LICENSE("GPL");
 
 struct joydev {
 	int open;
-	int minor;
 	struct input_handle handle;
 	wait_queue_head_t wait;
 	struct list_head client_list;
 	spinlock_t client_lock; /* protects client_list */
 	struct mutex mutex;
 	struct device dev;
+	struct cdev cdev;
 	bool exist;
 
 	struct js_corr corr[ABS_CNT];
@@ -70,9 +71,6 @@ struct joydev_client {
 	struct list_head node;
 };
 
-static struct joydev *joydev_table[JOYDEV_MINORS];
-static DEFINE_MUTEX(joydev_table_mutex);
-
 static int joydev_correct(int value, struct js_corr *corr)
 {
 	switch (corr->type) {
@@ -252,30 +250,14 @@ static int joydev_release(struct inode *inode, struct file *file)
 
 static int joydev_open(struct inode *inode, struct file *file)
 {
+	struct joydev *joydev =
+			container_of(inode->i_cdev, struct joydev, cdev);
 	struct joydev_client *client;
-	struct joydev *joydev;
-	int i = iminor(inode) - JOYDEV_MINOR_BASE;
 	int error;
 
-	if (i >= JOYDEV_MINORS)
-		return -ENODEV;
-
-	error = mutex_lock_interruptible(&joydev_table_mutex);
-	if (error)
-		return error;
-	joydev = joydev_table[i];
-	if (joydev)
-		get_device(&joydev->dev);
-	mutex_unlock(&joydev_table_mutex);
-
-	if (!joydev)
-		return -ENODEV;
-
 	client = kzalloc(sizeof(struct joydev_client), GFP_KERNEL);
-	if (!client) {
-		error = -ENOMEM;
-		goto err_put_joydev;
-	}
+	if (!client)
+		return -ENOMEM;
 
 	spin_lock_init(&client->buffer_lock);
 	client->joydev = joydev;
@@ -288,13 +270,12 @@ static int joydev_open(struct inode *inode, struct file *file)
 	file->private_data = client;
 	nonseekable_open(inode, file);
 
+	get_device(&joydev->dev);
 	return 0;
 
  err_free_client:
 	joydev_detach_client(joydev, client);
 	kfree(client);
- err_put_joydev:
-	put_device(&joydev->dev);
 	return error;
 }
 
@@ -742,19 +723,6 @@ static const struct file_operations joydev_fops = {
 	.llseek		= no_llseek,
 };
 
-static int joydev_install_chrdev(struct joydev *joydev)
-{
-	joydev_table[joydev->minor] = joydev;
-	return 0;
-}
-
-static void joydev_remove_chrdev(struct joydev *joydev)
-{
-	mutex_lock(&joydev_table_mutex);
-	joydev_table[joydev->minor] = NULL;
-	mutex_unlock(&joydev_table_mutex);
-}
-
 /*
  * Mark device non-existent. This disables writes, ioctls and
  * prevents new users from opening the device. Already posted
@@ -773,7 +741,8 @@ static void joydev_cleanup(struct joydev *joydev)
 
 	joydev_mark_dead(joydev);
 	joydev_hangup(joydev);
-	joydev_remove_chrdev(joydev);
+
+	cdev_del(&joydev->cdev);
 
 	/* joydev is marked dead so no one else accesses joydev->open */
 	if (joydev->open)
@@ -798,30 +767,33 @@ static int joydev_connect(struct input_handler *handler, struct input_dev *dev,
 			  const struct input_device_id *id)
 {
 	struct joydev *joydev;
-	int i, j, t, minor;
+	int i, j, t, minor, dev_no;
 	int error;
 
-	for (minor = 0; minor < JOYDEV_MINORS; minor++)
-		if (!joydev_table[minor])
-			break;
-
-	if (minor == JOYDEV_MINORS) {
-		pr_err("no more free joydev devices\n");
-		return -ENFILE;
+	minor = input_get_new_minor(JOYDEV_MINOR_BASE, JOYDEV_MINORS, true);
+	if (minor < 0) {
+		error = minor;
+		pr_err("failed to reserve new minor: %d\n", error);
+		return error;
 	}
 
 	joydev = kzalloc(sizeof(struct joydev), GFP_KERNEL);
-	if (!joydev)
-		return -ENOMEM;
+	if (!joydev) {
+		error = -ENOMEM;
+		goto err_free_minor;
+	}
 
 	INIT_LIST_HEAD(&joydev->client_list);
 	spin_lock_init(&joydev->client_lock);
 	mutex_init(&joydev->mutex);
 	init_waitqueue_head(&joydev->wait);
-
-	dev_set_name(&joydev->dev, "js%d", minor);
 	joydev->exist = true;
-	joydev->minor = minor;
+
+	dev_no = minor;
+	/* Normalize device number if it falls into legacy range */
+	if (dev_no < JOYDEV_MINOR_BASE + JOYDEV_MINORS)
+		dev_no -= JOYDEV_MINOR_BASE;
+	dev_set_name(&joydev->dev, "js%d", dev_no);
 
 	joydev->handle.dev = input_get_device(dev);
 	joydev->handle.name = dev_name(&joydev->dev);
@@ -875,7 +847,7 @@ static int joydev_connect(struct input_handler *handler, struct input_dev *dev,
 		}
 	}
 
-	joydev->dev.devt = MKDEV(INPUT_MAJOR, JOYDEV_MINOR_BASE + minor);
+	joydev->dev.devt = MKDEV(INPUT_MAJOR, minor);
 	joydev->dev.class = &input_class;
 	joydev->dev.parent = &dev->dev;
 	joydev->dev.release = joydev_free;
@@ -885,7 +857,8 @@ static int joydev_connect(struct input_handler *handler, struct input_dev *dev,
 	if (error)
 		goto err_free_joydev;
 
-	error = joydev_install_chrdev(joydev);
+	cdev_init(&joydev->cdev, &joydev_fops);
+	error = cdev_add(&joydev->cdev, joydev->dev.devt, 1);
 	if (error)
 		goto err_unregister_handle;
 
@@ -901,6 +874,8 @@ static int joydev_connect(struct input_handler *handler, struct input_dev *dev,
 	input_unregister_handle(&joydev->handle);
  err_free_joydev:
 	put_device(&joydev->dev);
+ err_free_minor:
+	input_free_minor(minor);
 	return error;
 }
 
@@ -910,6 +885,7 @@ static void joydev_disconnect(struct input_handle *handle)
 
 	device_del(&joydev->dev);
 	joydev_cleanup(joydev);
+	input_free_minor(MINOR(joydev->dev.devt));
 	input_unregister_handle(handle);
 	put_device(&joydev->dev);
 }
@@ -961,7 +937,7 @@ static struct input_handler joydev_handler = {
 	.match		= joydev_match,
 	.connect	= joydev_connect,
 	.disconnect	= joydev_disconnect,
-	.fops		= &joydev_fops,
+	.legacy_minors	= true,
 	.minor		= JOYDEV_MINOR_BASE,
 	.name		= "joydev",
 	.id_table	= joydev_ids,
diff --git a/drivers/input/keyboard/samsung-keypad.c b/drivers/input/keyboard/samsung-keypad.c
index 277e26dc910..9d7a111486f 100644
--- a/drivers/input/keyboard/samsung-keypad.c
+++ b/drivers/input/keyboard/samsung-keypad.c
@@ -431,6 +431,12 @@ static int __devinit samsung_keypad_probe(struct platform_device *pdev)
 		goto err_unmap_base;
 	}
 
+	error = clk_prepare(keypad->clk);
+	if (error) {
+		dev_err(&pdev->dev, "keypad clock prepare failed\n");
+		goto err_put_clk;
+	}
+
 	keypad->input_dev = input_dev;
 	keypad->pdev = pdev;
 	keypad->row_shift = row_shift;
@@ -461,7 +467,7 @@ static int __devinit samsung_keypad_probe(struct platform_device *pdev)
 					   keypad->keycodes, input_dev);
 	if (error) {
 		dev_err(&pdev->dev, "failed to build keymap\n");
-		goto err_put_clk;
+		goto err_unprepare_clk;
 	}
 
 	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
@@ -503,6 +509,8 @@ err_free_irq:
 	pm_runtime_disable(&pdev->dev);
 	device_init_wakeup(&pdev->dev, 0);
 	platform_set_drvdata(pdev, NULL);
+err_unprepare_clk:
+	clk_unprepare(keypad->clk);
 err_put_clk:
 	clk_put(keypad->clk);
 	samsung_keypad_dt_gpio_free(keypad);
@@ -531,6 +539,7 @@ static int __devexit samsung_keypad_remove(struct platform_device *pdev)
 	 */
 	free_irq(keypad->irq, keypad);
 
+	clk_unprepare(keypad->clk);
 	clk_put(keypad->clk);
 	samsung_keypad_dt_gpio_free(keypad);
 
diff --git a/drivers/input/misc/atlas_btns.c b/drivers/input/misc/atlas_btns.c
index 601f7372f9c..26f13131639 100644
--- a/drivers/input/misc/atlas_btns.c
+++ b/drivers/input/misc/atlas_btns.c
@@ -151,22 +151,7 @@ static struct acpi_driver atlas_acpi_driver = {
 		.remove	= atlas_acpi_button_remove,
 	},
 };
-
-static int __init atlas_acpi_init(void)
-{
-	if (acpi_disabled)
-		return -ENODEV;
-
-	return acpi_bus_register_driver(&atlas_acpi_driver);
-}
-
-static void __exit atlas_acpi_exit(void)
-{
-	acpi_bus_unregister_driver(&atlas_acpi_driver);
-}
-
-module_init(atlas_acpi_init);
-module_exit(atlas_acpi_exit);
+module_acpi_driver(atlas_acpi_driver);
 
 MODULE_AUTHOR("Jaya Kumar");
 MODULE_LICENSE("GPL");
diff --git a/drivers/input/mousedev.c b/drivers/input/mousedev.c
index 964e43d81e2..a1b4c37956b 100644
--- a/drivers/input/mousedev.c
+++ b/drivers/input/mousedev.c
@@ -24,10 +24,8 @@
 #include <linux/random.h>
 #include <linux/major.h>
 #include <linux/device.h>
+#include <linux/cdev.h>
 #include <linux/kernel.h>
-#ifdef CONFIG_INPUT_MOUSEDEV_PSAUX
-#include <linux/miscdevice.h>
-#endif
 
 MODULE_AUTHOR("Vojtech Pavlik <vojtech@ucw.cz>");
 MODULE_DESCRIPTION("Mouse (ExplorerPS/2) device interfaces");
@@ -61,17 +59,18 @@ struct mousedev_hw_data {
 
 struct mousedev {
 	int open;
-	int minor;
 	struct input_handle handle;
 	wait_queue_head_t wait;
 	struct list_head client_list;
 	spinlock_t client_lock; /* protects client_list */
 	struct mutex mutex;
 	struct device dev;
+	struct cdev cdev;
 	bool exist;
+	bool is_mixdev;
 
 	struct list_head mixdev_node;
-	int mixdev_open;
+	bool opened_by_mixdev;
 
 	struct mousedev_hw_data packet;
 	unsigned int pkt_count;
@@ -114,10 +113,6 @@ struct mousedev_client {
 static unsigned char mousedev_imps_seq[] = { 0xf3, 200, 0xf3, 100, 0xf3, 80 };
 static unsigned char mousedev_imex_seq[] = { 0xf3, 200, 0xf3, 200, 0xf3, 80 };
 
-static struct input_handler mousedev_handler;
-
-static struct mousedev *mousedev_table[MOUSEDEV_MINORS];
-static DEFINE_MUTEX(mousedev_table_mutex);
 static struct mousedev *mousedev_mix;
 static LIST_HEAD(mousedev_mix_list);
 
@@ -433,7 +428,7 @@ static int mousedev_open_device(struct mousedev *mousedev)
 	if (retval)
 		return retval;
 
-	if (mousedev->minor == MOUSEDEV_MIX)
+	if (mousedev->is_mixdev)
 		mixdev_open_devices();
 	else if (!mousedev->exist)
 		retval = -ENODEV;
@@ -451,7 +446,7 @@ static void mousedev_close_device(struct mousedev *mousedev)
 {
 	mutex_lock(&mousedev->mutex);
 
-	if (mousedev->minor == MOUSEDEV_MIX)
+	if (mousedev->is_mixdev)
 		mixdev_close_devices();
 	else if (mousedev->exist && !--mousedev->open)
 		input_close_device(&mousedev->handle);
@@ -472,11 +467,11 @@ static void mixdev_open_devices(void)
 		return;
 
 	list_for_each_entry(mousedev, &mousedev_mix_list, mixdev_node) {
-		if (!mousedev->mixdev_open) {
+		if (!mousedev->opened_by_mixdev) {
 			if (mousedev_open_device(mousedev))
 				continue;
 
-			mousedev->mixdev_open = 1;
+			mousedev->opened_by_mixdev = true;
 		}
 	}
 }
@@ -494,8 +489,8 @@ static void mixdev_close_devices(void)
 		return;
 
 	list_for_each_entry(mousedev, &mousedev_mix_list, mixdev_node) {
-		if (mousedev->mixdev_open) {
-			mousedev->mixdev_open = 0;
+		if (mousedev->opened_by_mixdev) {
+			mousedev->opened_by_mixdev = false;
 			mousedev_close_device(mousedev);
 		}
 	}
@@ -538,35 +533,17 @@ static int mousedev_open(struct inode *inode, struct file *file)
 	struct mousedev_client *client;
 	struct mousedev *mousedev;
 	int error;
-	int i;
 
 #ifdef CONFIG_INPUT_MOUSEDEV_PSAUX
 	if (imajor(inode) == MISC_MAJOR)
-		i = MOUSEDEV_MIX;
+		mousedev = mousedev_mix;
 	else
 #endif
-		i = iminor(inode) - MOUSEDEV_MINOR_BASE;
-
-	if (i >= MOUSEDEV_MINORS)
-		return -ENODEV;
-
-	error = mutex_lock_interruptible(&mousedev_table_mutex);
-	if (error)
-		return error;
-
-	mousedev = mousedev_table[i];
-	if (mousedev)
-		get_device(&mousedev->dev);
-	mutex_unlock(&mousedev_table_mutex);
-
-	if (!mousedev)
-		return -ENODEV;
+		mousedev = container_of(inode->i_cdev, struct mousedev, cdev);
 
 	client = kzalloc(sizeof(struct mousedev_client), GFP_KERNEL);
-	if (!client) {
-		error = -ENOMEM;
-		goto err_put_mousedev;
-	}
+	if (!client)
+		return -ENOMEM;
 
 	spin_lock_init(&client->packet_lock);
 	client->pos_x = xres / 2;
@@ -579,13 +556,14 @@ static int mousedev_open(struct inode *inode, struct file *file)
 		goto err_free_client;
 
 	file->private_data = client;
+	nonseekable_open(inode, file);
+
+	get_device(&mousedev->dev);
 	return 0;
 
  err_free_client:
 	mousedev_detach_client(mousedev, client);
 	kfree(client);
- err_put_mousedev:
-	put_device(&mousedev->dev);
 	return error;
 }
 
@@ -785,29 +763,16 @@ static unsigned int mousedev_poll(struct file *file, poll_table *wait)
 }
 
 static const struct file_operations mousedev_fops = {
-	.owner =	THIS_MODULE,
-	.read =		mousedev_read,
-	.write =	mousedev_write,
-	.poll =		mousedev_poll,
-	.open =		mousedev_open,
-	.release =	mousedev_release,
-	.fasync =	mousedev_fasync,
-	.llseek = noop_llseek,
+	.owner		= THIS_MODULE,
+	.read		= mousedev_read,
+	.write		= mousedev_write,
+	.poll		= mousedev_poll,
+	.open		= mousedev_open,
+	.release	= mousedev_release,
+	.fasync		= mousedev_fasync,
+	.llseek		= noop_llseek,
 };
 
-static int mousedev_install_chrdev(struct mousedev *mousedev)
-{
-	mousedev_table[mousedev->minor] = mousedev;
-	return 0;
-}
-
-static void mousedev_remove_chrdev(struct mousedev *mousedev)
-{
-	mutex_lock(&mousedev_table_mutex);
-	mousedev_table[mousedev->minor] = NULL;
-	mutex_unlock(&mousedev_table_mutex);
-}
-
 /*
  * Mark device non-existent. This disables writes, ioctls and
  * prevents new users from opening the device. Already posted
@@ -842,24 +807,50 @@ static void mousedev_cleanup(struct mousedev *mousedev)
 
 	mousedev_mark_dead(mousedev);
 	mousedev_hangup(mousedev);
-	mousedev_remove_chrdev(mousedev);
+
+	cdev_del(&mousedev->cdev);
 
 	/* mousedev is marked dead so no one else accesses mousedev->open */
 	if (mousedev->open)
 		input_close_device(handle);
 }
 
+static int mousedev_reserve_minor(bool mixdev)
+{
+	int minor;
+
+	if (mixdev) {
+		minor = input_get_new_minor(MOUSEDEV_MIX, 1, false);
+		if (minor < 0)
+			pr_err("failed to reserve mixdev minor: %d\n", minor);
+	} else {
+		minor = input_get_new_minor(MOUSEDEV_MINOR_BASE,
+					    MOUSEDEV_MINORS, true);
+		if (minor < 0)
+			pr_err("failed to reserve new minor: %d\n", minor);
+	}
+
+	return minor;
+}
+
 static struct mousedev *mousedev_create(struct input_dev *dev,
 					struct input_handler *handler,
-					int minor)
+					bool mixdev)
 {
 	struct mousedev *mousedev;
+	int minor;
 	int error;
 
+	minor = mousedev_reserve_minor(mixdev);
+	if (minor < 0) {
+		error = minor;
+		goto err_out;
+	}
+
 	mousedev = kzalloc(sizeof(struct mousedev), GFP_KERNEL);
 	if (!mousedev) {
 		error = -ENOMEM;
-		goto err_out;
+		goto err_free_minor;
 	}
 
 	INIT_LIST_HEAD(&mousedev->client_list);
@@ -867,16 +858,21 @@ static struct mousedev *mousedev_create(struct input_dev *dev,
 	spin_lock_init(&mousedev->client_lock);
 	mutex_init(&mousedev->mutex);
 	lockdep_set_subclass(&mousedev->mutex,
-			     minor == MOUSEDEV_MIX ? SINGLE_DEPTH_NESTING : 0);
+			     mixdev ? SINGLE_DEPTH_NESTING : 0);
 	init_waitqueue_head(&mousedev->wait);
 
-	if (minor == MOUSEDEV_MIX)
+	if (mixdev) {
 		dev_set_name(&mousedev->dev, "mice");
-	else
-		dev_set_name(&mousedev->dev, "mouse%d", minor);
+	} else {
+		int dev_no = minor;
+		/* Normalize device number if it falls into legacy range */
+		if (dev_no < MOUSEDEV_MINOR_BASE + MOUSEDEV_MINORS)
+			dev_no -= MOUSEDEV_MINOR_BASE;
+		dev_set_name(&mousedev->dev, "mouse%d", dev_no);
+	}
 
-	mousedev->minor = minor;
 	mousedev->exist = true;
+	mousedev->is_mixdev = mixdev;
 	mousedev->handle.dev = input_get_device(dev);
 	mousedev->handle.name = dev_name(&mousedev->dev);
 	mousedev->handle.handler = handler;
@@ -885,17 +881,18 @@ static struct mousedev *mousedev_create(struct input_dev *dev,
 	mousedev->dev.class = &input_class;
 	if (dev)
 		mousedev->dev.parent = &dev->dev;
-	mousedev->dev.devt = MKDEV(INPUT_MAJOR, MOUSEDEV_MINOR_BASE + minor);
+	mousedev->dev.devt = MKDEV(INPUT_MAJOR, minor);
 	mousedev->dev.release = mousedev_free;
 	device_initialize(&mousedev->dev);
 
-	if (minor != MOUSEDEV_MIX) {
+	if (!mixdev) {
 		error = input_register_handle(&mousedev->handle);
 		if (error)
 			goto err_free_mousedev;
 	}
 
-	error = mousedev_install_chrdev(mousedev);
+	cdev_init(&mousedev->cdev, &mousedev_fops);
+	error = cdev_add(&mousedev->cdev, mousedev->dev.devt, 1);
 	if (error)
 		goto err_unregister_handle;
 
@@ -908,10 +905,12 @@ static struct mousedev *mousedev_create(struct input_dev *dev,
  err_cleanup_mousedev:
 	mousedev_cleanup(mousedev);
  err_unregister_handle:
-	if (minor != MOUSEDEV_MIX)
+	if (!mixdev)
 		input_unregister_handle(&mousedev->handle);
  err_free_mousedev:
 	put_device(&mousedev->dev);
+ err_free_minor:
+	input_free_minor(minor);
  err_out:
 	return ERR_PTR(error);
 }
@@ -920,7 +919,8 @@ static void mousedev_destroy(struct mousedev *mousedev)
 {
 	device_del(&mousedev->dev);
 	mousedev_cleanup(mousedev);
-	if (mousedev->minor != MOUSEDEV_MIX)
+	input_free_minor(MINOR(mousedev->dev.devt));
+	if (!mousedev->is_mixdev)
 		input_unregister_handle(&mousedev->handle);
 	put_device(&mousedev->dev);
 }
@@ -938,7 +938,7 @@ static int mixdev_add_device(struct mousedev *mousedev)
 		if (retval)
 			goto out;
 
-		mousedev->mixdev_open = 1;
+		mousedev->opened_by_mixdev = true;
 	}
 
 	get_device(&mousedev->dev);
@@ -953,8 +953,8 @@ static void mixdev_remove_device(struct mousedev *mousedev)
 {
 	mutex_lock(&mousedev_mix->mutex);
 
-	if (mousedev->mixdev_open) {
-		mousedev->mixdev_open = 0;
+	if (mousedev->opened_by_mixdev) {
+		mousedev->opened_by_mixdev = false;
 		mousedev_close_device(mousedev);
 	}
 
@@ -969,19 +969,9 @@ static int mousedev_connect(struct input_handler *handler,
 			    const struct input_device_id *id)
 {
 	struct mousedev *mousedev;
-	int minor;
 	int error;
 
-	for (minor = 0; minor < MOUSEDEV_MINORS; minor++)
-		if (!mousedev_table[minor])
-			break;
-
-	if (minor == MOUSEDEV_MINORS) {
-		pr_err("no more free mousedev devices\n");
-		return -ENFILE;
-	}
-
-	mousedev = mousedev_create(dev, handler, minor);
+	mousedev = mousedev_create(dev, handler, false);
 	if (IS_ERR(mousedev))
 		return PTR_ERR(mousedev);
 
@@ -1054,27 +1044,53 @@ static const struct input_device_id mousedev_ids[] = {
 MODULE_DEVICE_TABLE(input, mousedev_ids);
 
 static struct input_handler mousedev_handler = {
-	.event =	mousedev_event,
-	.connect =	mousedev_connect,
-	.disconnect =	mousedev_disconnect,
-	.fops =		&mousedev_fops,
-	.minor =	MOUSEDEV_MINOR_BASE,
-	.name =		"mousedev",
-	.id_table =	mousedev_ids,
+	.event		= mousedev_event,
+	.connect	= mousedev_connect,
+	.disconnect	= mousedev_disconnect,
+	.legacy_minors	= true,
+	.minor		= MOUSEDEV_MINOR_BASE,
+	.name		= "mousedev",
+	.id_table	= mousedev_ids,
 };
 
 #ifdef CONFIG_INPUT_MOUSEDEV_PSAUX
+#include <linux/miscdevice.h>
+
 static struct miscdevice psaux_mouse = {
-	PSMOUSE_MINOR, "psaux", &mousedev_fops
+	.minor	= PSMOUSE_MINOR,
+	.name	= "psaux",
+	.fops	= &mousedev_fops,
 };
-static int psaux_registered;
+
+static bool psaux_registered;
+
+static void __init mousedev_psaux_register(void)
+{
+	int error;
+
+	error = misc_register(&psaux_mouse);
+	if (error)
+		pr_warn("could not register psaux device, error: %d\n",
+			   error);
+	else
+		psaux_registered = true;
+}
+
+static void __exit mousedev_psaux_unregister(void)
+{
+	if (psaux_registered)
+		misc_deregister(&psaux_mouse);
+}
+#else
+static inline void mousedev_psaux_register(void) { }
+static inline void mousedev_psaux_unregister(void) { }
 #endif
 
 static int __init mousedev_init(void)
 {
 	int error;
 
-	mousedev_mix = mousedev_create(NULL, &mousedev_handler, MOUSEDEV_MIX);
+	mousedev_mix = mousedev_create(NULL, &mousedev_handler, true);
 	if (IS_ERR(mousedev_mix))
 		return PTR_ERR(mousedev_mix);
 
@@ -1084,14 +1100,7 @@ static int __init mousedev_init(void)
 		return error;
 	}
 
-#ifdef CONFIG_INPUT_MOUSEDEV_PSAUX
-	error = misc_register(&psaux_mouse);
-	if (error)
-		pr_warn("could not register psaux device, error: %d\n",
-			   error);
-	else
-		psaux_registered = 1;
-#endif
+	mousedev_psaux_register();
 
 	pr_info("PS/2 mouse device common for all mice\n");
 
@@ -1100,10 +1109,7 @@ static int __init mousedev_init(void)
 
 static void __exit mousedev_exit(void)
 {
-#ifdef CONFIG_INPUT_MOUSEDEV_PSAUX
-	if (psaux_registered)
-		misc_deregister(&psaux_mouse);
-#endif
+	mousedev_psaux_unregister();
 	input_unregister_handler(&mousedev_handler);
 	mousedev_destroy(mousedev_mix);
 }
diff --git a/drivers/input/tablet/wacom_sys.c b/drivers/input/tablet/wacom_sys.c
index 0d3219f2974..9edf9806cff 100644
--- a/drivers/input/tablet/wacom_sys.c
+++ b/drivers/input/tablet/wacom_sys.c
@@ -172,6 +172,76 @@ static void wacom_close(struct input_dev *dev)
 }
 
 /*
+ * Calculate the resolution of the X or Y axis, given appropriate HID data.
+ * This function is little more than hidinput_calc_abs_res stripped down.
+ */
+static int wacom_calc_hid_res(int logical_extents, int physical_extents,
+                              unsigned char unit, unsigned char exponent)
+{
+	int prev, unit_exponent;
+
+	/* Check if the extents are sane */
+	if (logical_extents <= 0 || physical_extents <= 0)
+		return 0;
+
+	/* Get signed value of nybble-sized twos-compliment exponent */
+	unit_exponent = exponent;
+	if (unit_exponent > 7)
+		unit_exponent -= 16;
+
+	/* Convert physical_extents to millimeters */
+	if (unit == 0x11) {		/* If centimeters */
+		unit_exponent += 1;
+	} else if (unit == 0x13) {	/* If inches */
+		prev = physical_extents;
+		physical_extents *= 254;
+		if (physical_extents < prev)
+			return 0;
+		unit_exponent -= 1;
+	} else {
+		return 0;
+	}
+
+	/* Apply negative unit exponent */
+	for (; unit_exponent < 0; unit_exponent++) {
+		prev = logical_extents;
+		logical_extents *= 10;
+		if (logical_extents < prev)
+			return 0;
+	}
+	/* Apply positive unit exponent */
+	for (; unit_exponent > 0; unit_exponent--) {
+		prev = physical_extents;
+		physical_extents *= 10;
+		if (physical_extents < prev)
+			return 0;
+	}
+
+	/* Calculate resolution */
+	return logical_extents / physical_extents;
+}
+
+/*
+ * The physical dimension specified by the HID descriptor is likely not in
+ * the "100th of a mm" units expected by wacom_calculate_touch_res. This
+ * function adjusts the value of [xy]_phy based on the unit and exponent
+ * provided by the HID descriptor. If an error occurs durring conversion
+ * (e.g. from the unit being left unspecified) [xy]_phy is not modified.
+ */
+static void wacom_fix_phy_from_hid(struct wacom_features *features)
+{
+	int xres = wacom_calc_hid_res(features->x_max, features->x_phy,
+					features->unit, features->unitExpo);
+	int yres = wacom_calc_hid_res(features->y_max, features->y_phy,
+					features->unit, features->unitExpo);
+
+	if (xres > 0 && yres > 0) {
+		features->x_phy = (100 * features->x_max) / xres;
+		features->y_phy = (100 * features->y_max) / yres;
+	}
+}
+
+/*
  * Static values for max X/Y and resolution of Pen interface is stored in
  * features. This mean physical size of active area can be computed.
  * This is useful to do when Pen and Touch have same active area of tablet.
@@ -432,56 +502,52 @@ static int wacom_parse_hid(struct usb_interface *intf,
 	return result;
 }
 
-static int wacom_query_tablet_data(struct usb_interface *intf, struct wacom_features *features)
+static int wacom_set_device_mode(struct usb_interface *intf, int report_id, int length, int mode)
 {
 	unsigned char *rep_data;
-	int limit = 0, report_id = 2;
-	int error = -ENOMEM;
+	int error = -ENOMEM, limit = 0;
 
-	rep_data = kmalloc(4, GFP_KERNEL);
+	rep_data = kzalloc(length, GFP_KERNEL);
 	if (!rep_data)
 		return error;
 
-	/* ask to report Wacom data */
+	rep_data[0] = report_id;
+	rep_data[1] = mode;
+
+	do {
+		error = wacom_set_report(intf, WAC_HID_FEATURE_REPORT,
+		                         report_id, rep_data, length, 1);
+		if (error >= 0)
+			error = wacom_get_report(intf, WAC_HID_FEATURE_REPORT,
+			                         report_id, rep_data, length, 1);
+	} while ((error < 0 || rep_data[1] != mode) && limit++ < WAC_MSG_RETRIES);
+
+	kfree(rep_data);
+
+	return error < 0 ? error : 0;
+}
+
+/*
+ * Switch the tablet into its most-capable mode. Wacom tablets are
+ * typically configured to power-up in a mode which sends mouse-like
+ * reports to the OS. To get absolute position, pressure data, etc.
+ * from the tablet, it is necessary to switch the tablet out of this
+ * mode and into one which sends the full range of tablet data.
+ */
+static int wacom_query_tablet_data(struct usb_interface *intf, struct wacom_features *features)
+{
 	if (features->device_type == BTN_TOOL_FINGER) {
-		/* if it is an MT Tablet PC touch */
 		if (features->type > TABLETPC) {
-			do {
-				rep_data[0] = 3;
-				rep_data[1] = 4;
-				rep_data[2] = 0;
-				rep_data[3] = 0;
-				report_id = 3;
-				error = wacom_set_report(intf,
-							 WAC_HID_FEATURE_REPORT,
-							 report_id,
-							 rep_data, 4, 1);
-				if (error >= 0)
-					error = wacom_get_report(intf,
-							WAC_HID_FEATURE_REPORT,
-							report_id,
-							rep_data, 4, 1);
-			} while ((error < 0 || rep_data[1] != 4) &&
-				 limit++ < WAC_MSG_RETRIES);
+			/* MT Tablet PC touch */
+			return wacom_set_device_mode(intf, 3, 4, 4);
+		}
+	} else if (features->device_type == BTN_TOOL_PEN) {
+		if (features->type <= BAMBOO_PT && features->type != WIRELESS) {
+			return wacom_set_device_mode(intf, 2, 2, 2);
 		}
-	} else if (features->type <= BAMBOO_PT &&
-		   features->type != WIRELESS &&
-		   features->device_type == BTN_TOOL_PEN) {
-		do {
-			rep_data[0] = 2;
-			rep_data[1] = 2;
-			error = wacom_set_report(intf, WAC_HID_FEATURE_REPORT,
-						 report_id, rep_data, 2, 1);
-			if (error >= 0)
-				error = wacom_get_report(intf,
-						WAC_HID_FEATURE_REPORT,
-						report_id, rep_data, 2, 1);
-		} while ((error < 0 || rep_data[1] != 2) && limit++ < WAC_MSG_RETRIES);
 	}
 
-	kfree(rep_data);
-
-	return error < 0 ? error : 0;
+	return 0;
 }
 
 static int wacom_retrieve_hid_descriptor(struct usb_interface *intf,
@@ -531,6 +597,7 @@ static int wacom_retrieve_hid_descriptor(struct usb_interface *intf,
 	error = wacom_parse_hid(intf, hid_desc, features);
 	if (error)
 		goto out;
+	wacom_fix_phy_from_hid(features);
 
  out:
 	return error;
diff --git a/drivers/input/tablet/wacom_wac.c b/drivers/input/tablet/wacom_wac.c
index 08b462b6c0d..c3468c8dbd8 100644
--- a/drivers/input/tablet/wacom_wac.c
+++ b/drivers/input/tablet/wacom_wac.c
@@ -25,6 +25,11 @@
 #define WACOM_INTUOS_RES	100
 #define WACOM_INTUOS3_RES	200
 
+/* Scale factor relating reported contact size to logical contact area.
+ * 2^14/pi is a good approximation on Intuos5 and 3rd-gen Bamboo
+ */
+#define WACOM_CONTACT_AREA_SCALE 2607
+
 static int wacom_penpartner_irq(struct wacom_wac *wacom)
 {
 	unsigned char *data = wacom->data;
@@ -326,7 +331,7 @@ static int wacom_intuos_inout(struct wacom_wac *wacom)
 
 	/* Enter report */
 	if ((data[1] & 0xfc) == 0xc0) {
-		if (features->type >= INTUOS5S && features->type <= INTUOS5L)
+		if (features->quirks == WACOM_QUIRK_MULTI_INPUT)
 			wacom->shared->stylus_in_proximity = true;
 
 		/* serial number of the tool */
@@ -414,7 +419,7 @@ static int wacom_intuos_inout(struct wacom_wac *wacom)
 
 	/* Exit report */
 	if ((data[1] & 0xfe) == 0x80) {
-		if (features->type >= INTUOS5S && features->type <= INTUOS5L)
+		if (features->quirks == WACOM_QUIRK_MULTI_INPUT)
 			wacom->shared->stylus_in_proximity = false;
 
 		/*
@@ -1043,11 +1048,19 @@ static void wacom_bpt3_touch_msg(struct wacom_wac *wacom, unsigned char *data)
 	if (touch) {
 		int x = (data[2] << 4) | (data[4] >> 4);
 		int y = (data[3] << 4) | (data[4] & 0x0f);
-		int w = data[6];
+		int a = data[5];
+
+		// "a" is a scaled-down area which we assume is roughly
+		// circular and which can be described as: a=(pi*r^2)/C.
+		int x_res  = input_abs_get_res(input, ABS_X);
+		int y_res  = input_abs_get_res(input, ABS_Y);
+		int width  = 2 * int_sqrt(a * WACOM_CONTACT_AREA_SCALE);
+		int height = width * y_res / x_res;
 
 		input_report_abs(input, ABS_MT_POSITION_X, x);
 		input_report_abs(input, ABS_MT_POSITION_Y, y);
-		input_report_abs(input, ABS_MT_TOUCH_MAJOR, w);
+		input_report_abs(input, ABS_MT_TOUCH_MAJOR, width);
+		input_report_abs(input, ABS_MT_TOUCH_MINOR, height);
 	}
 }
 
@@ -1533,7 +1546,9 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 			input_mt_init_slots(input_dev, features->touch_max, 0);
 
 			input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR,
-			                     0, 255, 0, 0);
+			                     0, features->x_max, 0, 0);
+			input_set_abs_params(input_dev, ABS_MT_TOUCH_MINOR,
+			                     0, features->y_max, 0, 0);
 
 			input_set_abs_params(input_dev, ABS_MT_POSITION_X,
 					     0, features->x_max,
@@ -1641,7 +1656,10 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 
 				input_set_abs_params(input_dev,
 						     ABS_MT_TOUCH_MAJOR,
-						     0, 255, 0, 0);
+						     0, features->x_max, 0, 0);
+				input_set_abs_params(input_dev,
+						     ABS_MT_TOUCH_MINOR,
+						     0, features->y_max, 0, 0);
 			}
 
 			input_set_abs_params(input_dev, ABS_MT_POSITION_X,
diff --git a/drivers/input/touchscreen/atmel_mxt_ts.c b/drivers/input/touchscreen/atmel_mxt_ts.c
index e92615d0b1b..1df2396af00 100644
--- a/drivers/input/touchscreen/atmel_mxt_ts.c
+++ b/drivers/input/touchscreen/atmel_mxt_ts.c
@@ -320,10 +320,8 @@ static bool mxt_object_writable(unsigned int type)
 static void mxt_dump_message(struct device *dev,
 			     struct mxt_message *message)
 {
-	dev_dbg(dev, "reportid: %u\tmessage: %02x %02x %02x %02x %02x %02x %02x\n",
-		message->reportid, message->message[0], message->message[1],
-		message->message[2], message->message[3], message->message[4],
-		message->message[5], message->message[6]);
+	dev_dbg(dev, "reportid: %u\tmessage: %*ph\n",
+		message->reportid, 7, message->message);
 }
 
 static int mxt_check_bootloader(struct i2c_client *client,
diff --git a/drivers/iommu/Kconfig b/drivers/iommu/Kconfig
index 9f69b561f5d..e39f9dbf297 100644
--- a/drivers/iommu/Kconfig
+++ b/drivers/iommu/Kconfig
@@ -42,7 +42,7 @@ config AMD_IOMMU
 	select PCI_PRI
 	select PCI_PASID
 	select IOMMU_API
-	depends on X86_64 && PCI && ACPI
+	depends on X86_64 && PCI && ACPI && X86_IO_APIC
 	---help---
 	  With this option you can enable support for AMD IOMMU hardware in
 	  your system. An IOMMU is a hardware component which provides
diff --git a/drivers/iommu/amd_iommu.c b/drivers/iommu/amd_iommu.c
index e89daf1b21b..55074cba20e 100644
--- a/drivers/iommu/amd_iommu.c
+++ b/drivers/iommu/amd_iommu.c
@@ -31,6 +31,12 @@
 #include <linux/amd-iommu.h>
 #include <linux/notifier.h>
 #include <linux/export.h>
+#include <linux/irq.h>
+#include <linux/msi.h>
+#include <asm/irq_remapping.h>
+#include <asm/io_apic.h>
+#include <asm/apic.h>
+#include <asm/hw_irq.h>
 #include <asm/msidef.h>
 #include <asm/proto.h>
 #include <asm/iommu.h>
@@ -39,6 +45,7 @@
 
 #include "amd_iommu_proto.h"
 #include "amd_iommu_types.h"
+#include "irq_remapping.h"
 
 #define CMD_SET_TYPE(cmd, t) ((cmd)->data[1] |= ((t) << 28))
 
@@ -72,6 +79,9 @@ static DEFINE_SPINLOCK(iommu_pd_list_lock);
 static LIST_HEAD(dev_data_list);
 static DEFINE_SPINLOCK(dev_data_list_lock);
 
+LIST_HEAD(ioapic_map);
+LIST_HEAD(hpet_map);
+
 /*
  * Domain for untranslated devices - only allocated
  * if iommu=pt passed on kernel cmd line.
@@ -92,6 +102,8 @@ struct iommu_cmd {
 	u32 data[4];
 };
 
+struct kmem_cache *amd_iommu_irq_cache;
+
 static void update_domain(struct protection_domain *domain);
 static int __init alloc_passthrough_domain(void);
 
@@ -686,7 +698,7 @@ static void iommu_poll_ppr_log(struct amd_iommu *iommu)
 
 		/*
 		 * Release iommu->lock because ppr-handling might need to
-		 * re-aquire it
+		 * re-acquire it
 		 */
 		spin_unlock_irqrestore(&iommu->lock, flags);
 
@@ -804,7 +816,7 @@ static void build_inv_iommu_pages(struct iommu_cmd *cmd, u64 address,
 	CMD_SET_TYPE(cmd, CMD_INV_IOMMU_PAGES);
 	if (s) /* size bit - we flush more than one 4kb page */
 		cmd->data[2] |= CMD_INV_IOMMU_PAGES_SIZE_MASK;
-	if (pde) /* PDE bit - we wan't flush everything not only the PTEs */
+	if (pde) /* PDE bit - we want to flush everything, not only the PTEs */
 		cmd->data[2] |= CMD_INV_IOMMU_PAGES_PDE_MASK;
 }
 
@@ -899,6 +911,13 @@ static void build_inv_all(struct iommu_cmd *cmd)
 	CMD_SET_TYPE(cmd, CMD_INV_ALL);
 }
 
+static void build_inv_irt(struct iommu_cmd *cmd, u16 devid)
+{
+	memset(cmd, 0, sizeof(*cmd));
+	cmd->data[0] = devid;
+	CMD_SET_TYPE(cmd, CMD_INV_IRT);
+}
+
 /*
  * Writes the command to the IOMMUs command buffer and informs the
  * hardware about the new command.
@@ -1020,12 +1039,32 @@ static void iommu_flush_all(struct amd_iommu *iommu)
 	iommu_completion_wait(iommu);
 }
 
+static void iommu_flush_irt(struct amd_iommu *iommu, u16 devid)
+{
+	struct iommu_cmd cmd;
+
+	build_inv_irt(&cmd, devid);
+
+	iommu_queue_command(iommu, &cmd);
+}
+
+static void iommu_flush_irt_all(struct amd_iommu *iommu)
+{
+	u32 devid;
+
+	for (devid = 0; devid <= MAX_DEV_TABLE_ENTRIES; devid++)
+		iommu_flush_irt(iommu, devid);
+
+	iommu_completion_wait(iommu);
+}
+
 void iommu_flush_all_caches(struct amd_iommu *iommu)
 {
 	if (iommu_feature(iommu, FEATURE_IA)) {
 		iommu_flush_all(iommu);
 	} else {
 		iommu_flush_dte_all(iommu);
+		iommu_flush_irt_all(iommu);
 		iommu_flush_tlb_all(iommu);
 	}
 }
@@ -2155,7 +2194,7 @@ static bool pci_pri_tlp_required(struct pci_dev *pdev)
 }
 
 /*
- * If a device is not yet associated with a domain, this function does
+ * If a device is not yet associated with a domain, this function
  * assigns it visible for the hardware
  */
 static int attach_device(struct device *dev,
@@ -2405,7 +2444,7 @@ static struct protection_domain *get_domain(struct device *dev)
 	if (domain != NULL)
 		return domain;
 
-	/* Device not bount yet - bind it */
+	/* Device not bound yet - bind it */
 	dma_dom = find_protection_domain(devid);
 	if (!dma_dom)
 		dma_dom = amd_iommu_rlookup_table[devid]->default_dom;
@@ -2944,7 +2983,7 @@ static void __init prealloc_protection_domains(void)
 			alloc_passthrough_domain();
 			dev_data->passthrough = true;
 			attach_device(&dev->dev, pt_domain);
-			pr_info("AMD-Vi: Using passthough domain for device %s\n",
+			pr_info("AMD-Vi: Using passthrough domain for device %s\n",
 				dev_name(&dev->dev));
 		}
 
@@ -3316,6 +3355,8 @@ static int amd_iommu_domain_has_cap(struct iommu_domain *domain,
 	switch (cap) {
 	case IOMMU_CAP_CACHE_COHERENCY:
 		return 1;
+	case IOMMU_CAP_INTR_REMAP:
+		return irq_remapping_enabled;
 	}
 
 	return 0;
@@ -3743,3 +3784,466 @@ int amd_iommu_device_info(struct pci_dev *pdev,
 	return 0;
 }
 EXPORT_SYMBOL(amd_iommu_device_info);
+
+#ifdef CONFIG_IRQ_REMAP
+
+/*****************************************************************************
+ *
+ * Interrupt Remapping Implementation
+ *
+ *****************************************************************************/
+
+union irte {
+	u32 val;
+	struct {
+		u32 valid	: 1,
+		    no_fault	: 1,
+		    int_type	: 3,
+		    rq_eoi	: 1,
+		    dm		: 1,
+		    rsvd_1	: 1,
+		    destination	: 8,
+		    vector	: 8,
+		    rsvd_2	: 8;
+	} fields;
+};
+
+#define DTE_IRQ_PHYS_ADDR_MASK	(((1ULL << 45)-1) << 6)
+#define DTE_IRQ_REMAP_INTCTL    (2ULL << 60)
+#define DTE_IRQ_TABLE_LEN       (8ULL << 1)
+#define DTE_IRQ_REMAP_ENABLE    1ULL
+
+static void set_dte_irq_entry(u16 devid, struct irq_remap_table *table)
+{
+	u64 dte;
+
+	dte	= amd_iommu_dev_table[devid].data[2];
+	dte	&= ~DTE_IRQ_PHYS_ADDR_MASK;
+	dte	|= virt_to_phys(table->table);
+	dte	|= DTE_IRQ_REMAP_INTCTL;
+	dte	|= DTE_IRQ_TABLE_LEN;
+	dte	|= DTE_IRQ_REMAP_ENABLE;
+
+	amd_iommu_dev_table[devid].data[2] = dte;
+}
+
+#define IRTE_ALLOCATED (~1U)
+
+static struct irq_remap_table *get_irq_table(u16 devid, bool ioapic)
+{
+	struct irq_remap_table *table = NULL;
+	struct amd_iommu *iommu;
+	unsigned long flags;
+	u16 alias;
+
+	write_lock_irqsave(&amd_iommu_devtable_lock, flags);
+
+	iommu = amd_iommu_rlookup_table[devid];
+	if (!iommu)
+		goto out_unlock;
+
+	table = irq_lookup_table[devid];
+	if (table)
+		goto out;
+
+	alias = amd_iommu_alias_table[devid];
+	table = irq_lookup_table[alias];
+	if (table) {
+		irq_lookup_table[devid] = table;
+		set_dte_irq_entry(devid, table);
+		iommu_flush_dte(iommu, devid);
+		goto out;
+	}
+
+	/* Nothing there yet, allocate new irq remapping table */
+	table = kzalloc(sizeof(*table), GFP_ATOMIC);
+	if (!table)
+		goto out;
+
+	if (ioapic)
+		/* Keep the first 32 indexes free for IOAPIC interrupts */
+		table->min_index = 32;
+
+	table->table = kmem_cache_alloc(amd_iommu_irq_cache, GFP_ATOMIC);
+	if (!table->table) {
+		kfree(table);
+		table = NULL;
+		goto out;
+	}
+
+	memset(table->table, 0, MAX_IRQS_PER_TABLE * sizeof(u32));
+
+	if (ioapic) {
+		int i;
+
+		for (i = 0; i < 32; ++i)
+			table->table[i] = IRTE_ALLOCATED;
+	}
+
+	irq_lookup_table[devid] = table;
+	set_dte_irq_entry(devid, table);
+	iommu_flush_dte(iommu, devid);
+	if (devid != alias) {
+		irq_lookup_table[alias] = table;
+		set_dte_irq_entry(devid, table);
+		iommu_flush_dte(iommu, alias);
+	}
+
+out:
+	iommu_completion_wait(iommu);
+
+out_unlock:
+	write_unlock_irqrestore(&amd_iommu_devtable_lock, flags);
+
+	return table;
+}
+
+static int alloc_irq_index(struct irq_cfg *cfg, u16 devid, int count)
+{
+	struct irq_remap_table *table;
+	unsigned long flags;
+	int index, c;
+
+	table = get_irq_table(devid, false);
+	if (!table)
+		return -ENODEV;
+
+	spin_lock_irqsave(&table->lock, flags);
+
+	/* Scan table for free entries */
+	for (c = 0, index = table->min_index;
+	     index < MAX_IRQS_PER_TABLE;
+	     ++index) {
+		if (table->table[index] == 0)
+			c += 1;
+		else
+			c = 0;
+
+		if (c == count)	{
+			struct irq_2_iommu *irte_info;
+
+			for (; c != 0; --c)
+				table->table[index - c + 1] = IRTE_ALLOCATED;
+
+			index -= count - 1;
+
+			irte_info             = &cfg->irq_2_iommu;
+			irte_info->sub_handle = devid;
+			irte_info->irte_index = index;
+			irte_info->iommu      = (void *)cfg;
+
+			goto out;
+		}
+	}
+
+	index = -ENOSPC;
+
+out:
+	spin_unlock_irqrestore(&table->lock, flags);
+
+	return index;
+}
+
+static int get_irte(u16 devid, int index, union irte *irte)
+{
+	struct irq_remap_table *table;
+	unsigned long flags;
+
+	table = get_irq_table(devid, false);
+	if (!table)
+		return -ENOMEM;
+
+	spin_lock_irqsave(&table->lock, flags);
+	irte->val = table->table[index];
+	spin_unlock_irqrestore(&table->lock, flags);
+
+	return 0;
+}
+
+static int modify_irte(u16 devid, int index, union irte irte)
+{
+	struct irq_remap_table *table;
+	struct amd_iommu *iommu;
+	unsigned long flags;
+
+	iommu = amd_iommu_rlookup_table[devid];
+	if (iommu == NULL)
+		return -EINVAL;
+
+	table = get_irq_table(devid, false);
+	if (!table)
+		return -ENOMEM;
+
+	spin_lock_irqsave(&table->lock, flags);
+	table->table[index] = irte.val;
+	spin_unlock_irqrestore(&table->lock, flags);
+
+	iommu_flush_irt(iommu, devid);
+	iommu_completion_wait(iommu);
+
+	return 0;
+}
+
+static void free_irte(u16 devid, int index)
+{
+	struct irq_remap_table *table;
+	struct amd_iommu *iommu;
+	unsigned long flags;
+
+	iommu = amd_iommu_rlookup_table[devid];
+	if (iommu == NULL)
+		return;
+
+	table = get_irq_table(devid, false);
+	if (!table)
+		return;
+
+	spin_lock_irqsave(&table->lock, flags);
+	table->table[index] = 0;
+	spin_unlock_irqrestore(&table->lock, flags);
+
+	iommu_flush_irt(iommu, devid);
+	iommu_completion_wait(iommu);
+}
+
+static int setup_ioapic_entry(int irq, struct IO_APIC_route_entry *entry,
+			      unsigned int destination, int vector,
+			      struct io_apic_irq_attr *attr)
+{
+	struct irq_remap_table *table;
+	struct irq_2_iommu *irte_info;
+	struct irq_cfg *cfg;
+	union irte irte;
+	int ioapic_id;
+	int index;
+	int devid;
+	int ret;
+
+	cfg = irq_get_chip_data(irq);
+	if (!cfg)
+		return -EINVAL;
+
+	irte_info = &cfg->irq_2_iommu;
+	ioapic_id = mpc_ioapic_id(attr->ioapic);
+	devid     = get_ioapic_devid(ioapic_id);
+
+	if (devid < 0)
+		return devid;
+
+	table = get_irq_table(devid, true);
+	if (table == NULL)
+		return -ENOMEM;
+
+	index = attr->ioapic_pin;
+
+	/* Setup IRQ remapping info */
+	irte_info->sub_handle = devid;
+	irte_info->irte_index = index;
+	irte_info->iommu      = (void *)cfg;
+
+	/* Setup IRTE for IOMMU */
+	irte.val		= 0;
+	irte.fields.vector      = vector;
+	irte.fields.int_type    = apic->irq_delivery_mode;
+	irte.fields.destination = destination;
+	irte.fields.dm          = apic->irq_dest_mode;
+	irte.fields.valid       = 1;
+
+	ret = modify_irte(devid, index, irte);
+	if (ret)
+		return ret;
+
+	/* Setup IOAPIC entry */
+	memset(entry, 0, sizeof(*entry));
+
+	entry->vector        = index;
+	entry->mask          = 0;
+	entry->trigger       = attr->trigger;
+	entry->polarity      = attr->polarity;
+
+	/*
+	 * Mask level triggered irqs.
+	 */
+	if (attr->trigger)
+		entry->mask = 1;
+
+	return 0;
+}
+
+static int set_affinity(struct irq_data *data, const struct cpumask *mask,
+			bool force)
+{
+	struct irq_2_iommu *irte_info;
+	unsigned int dest, irq;
+	struct irq_cfg *cfg;
+	union irte irte;
+	int err;
+
+	if (!config_enabled(CONFIG_SMP))
+		return -1;
+
+	cfg       = data->chip_data;
+	irq       = data->irq;
+	irte_info = &cfg->irq_2_iommu;
+
+	if (!cpumask_intersects(mask, cpu_online_mask))
+		return -EINVAL;
+
+	if (get_irte(irte_info->sub_handle, irte_info->irte_index, &irte))
+		return -EBUSY;
+
+	if (assign_irq_vector(irq, cfg, mask))
+		return -EBUSY;
+
+	err = apic->cpu_mask_to_apicid_and(cfg->domain, mask, &dest);
+	if (err) {
+		if (assign_irq_vector(irq, cfg, data->affinity))
+			pr_err("AMD-Vi: Failed to recover vector for irq %d\n", irq);
+		return err;
+	}
+
+	irte.fields.vector      = cfg->vector;
+	irte.fields.destination = dest;
+
+	modify_irte(irte_info->sub_handle, irte_info->irte_index, irte);
+
+	if (cfg->move_in_progress)
+		send_cleanup_vector(cfg);
+
+	cpumask_copy(data->affinity, mask);
+
+	return 0;
+}
+
+static int free_irq(int irq)
+{
+	struct irq_2_iommu *irte_info;
+	struct irq_cfg *cfg;
+
+	cfg = irq_get_chip_data(irq);
+	if (!cfg)
+		return -EINVAL;
+
+	irte_info = &cfg->irq_2_iommu;
+
+	free_irte(irte_info->sub_handle, irte_info->irte_index);
+
+	return 0;
+}
+
+static void compose_msi_msg(struct pci_dev *pdev,
+			    unsigned int irq, unsigned int dest,
+			    struct msi_msg *msg, u8 hpet_id)
+{
+	struct irq_2_iommu *irte_info;
+	struct irq_cfg *cfg;
+	union irte irte;
+
+	cfg = irq_get_chip_data(irq);
+	if (!cfg)
+		return;
+
+	irte_info = &cfg->irq_2_iommu;
+
+	irte.val		= 0;
+	irte.fields.vector	= cfg->vector;
+	irte.fields.int_type    = apic->irq_delivery_mode;
+	irte.fields.destination	= dest;
+	irte.fields.dm		= apic->irq_dest_mode;
+	irte.fields.valid	= 1;
+
+	modify_irte(irte_info->sub_handle, irte_info->irte_index, irte);
+
+	msg->address_hi = MSI_ADDR_BASE_HI;
+	msg->address_lo = MSI_ADDR_BASE_LO;
+	msg->data       = irte_info->irte_index;
+}
+
+static int msi_alloc_irq(struct pci_dev *pdev, int irq, int nvec)
+{
+	struct irq_cfg *cfg;
+	int index;
+	u16 devid;
+
+	if (!pdev)
+		return -EINVAL;
+
+	cfg = irq_get_chip_data(irq);
+	if (!cfg)
+		return -EINVAL;
+
+	devid = get_device_id(&pdev->dev);
+	index = alloc_irq_index(cfg, devid, nvec);
+
+	return index < 0 ? MAX_IRQS_PER_TABLE : index;
+}
+
+static int msi_setup_irq(struct pci_dev *pdev, unsigned int irq,
+			 int index, int offset)
+{
+	struct irq_2_iommu *irte_info;
+	struct irq_cfg *cfg;
+	u16 devid;
+
+	if (!pdev)
+		return -EINVAL;
+
+	cfg = irq_get_chip_data(irq);
+	if (!cfg)
+		return -EINVAL;
+
+	if (index >= MAX_IRQS_PER_TABLE)
+		return 0;
+
+	devid		= get_device_id(&pdev->dev);
+	irte_info	= &cfg->irq_2_iommu;
+
+	irte_info->sub_handle = devid;
+	irte_info->irte_index = index + offset;
+	irte_info->iommu      = (void *)cfg;
+
+	return 0;
+}
+
+static int setup_hpet_msi(unsigned int irq, unsigned int id)
+{
+	struct irq_2_iommu *irte_info;
+	struct irq_cfg *cfg;
+	int index, devid;
+
+	cfg = irq_get_chip_data(irq);
+	if (!cfg)
+		return -EINVAL;
+
+	irte_info = &cfg->irq_2_iommu;
+	devid     = get_hpet_devid(id);
+	if (devid < 0)
+		return devid;
+
+	index = alloc_irq_index(cfg, devid, 1);
+	if (index < 0)
+		return index;
+
+	irte_info->sub_handle = devid;
+	irte_info->irte_index = index;
+	irte_info->iommu      = (void *)cfg;
+
+	return 0;
+}
+
+struct irq_remap_ops amd_iommu_irq_ops = {
+	.supported		= amd_iommu_supported,
+	.prepare		= amd_iommu_prepare,
+	.enable			= amd_iommu_enable,
+	.disable		= amd_iommu_disable,
+	.reenable		= amd_iommu_reenable,
+	.enable_faulting	= amd_iommu_enable_faulting,
+	.setup_ioapic_entry	= setup_ioapic_entry,
+	.set_affinity		= set_affinity,
+	.free_irq		= free_irq,
+	.compose_msi_msg	= compose_msi_msg,
+	.msi_alloc_irq		= msi_alloc_irq,
+	.msi_setup_irq		= msi_setup_irq,
+	.setup_hpet_msi		= setup_hpet_msi,
+};
+#endif
diff --git a/drivers/iommu/amd_iommu_init.c b/drivers/iommu/amd_iommu_init.c
index 18a89b760aa..18b0d99bd4d 100644
--- a/drivers/iommu/amd_iommu_init.c
+++ b/drivers/iommu/amd_iommu_init.c
@@ -26,16 +26,18 @@
 #include <linux/msi.h>
 #include <linux/amd-iommu.h>
 #include <linux/export.h>
-#include <linux/acpi.h>
 #include <acpi/acpi.h>
 #include <asm/pci-direct.h>
 #include <asm/iommu.h>
 #include <asm/gart.h>
 #include <asm/x86_init.h>
 #include <asm/iommu_table.h>
+#include <asm/io_apic.h>
+#include <asm/irq_remapping.h>
 
 #include "amd_iommu_proto.h"
 #include "amd_iommu_types.h"
+#include "irq_remapping.h"
 
 /*
  * definitions for the ACPI scanning code
@@ -55,6 +57,10 @@
 #define IVHD_DEV_ALIAS_RANGE            0x43
 #define IVHD_DEV_EXT_SELECT             0x46
 #define IVHD_DEV_EXT_SELECT_RANGE       0x47
+#define IVHD_DEV_SPECIAL		0x48
+
+#define IVHD_SPECIAL_IOAPIC		1
+#define IVHD_SPECIAL_HPET		2
 
 #define IVHD_FLAG_HT_TUN_EN_MASK        0x01
 #define IVHD_FLAG_PASSPW_EN_MASK        0x02
@@ -123,6 +129,7 @@ struct ivmd_header {
 } __attribute__((packed));
 
 bool amd_iommu_dump;
+bool amd_iommu_irq_remap __read_mostly;
 
 static bool amd_iommu_detected;
 static bool __initdata amd_iommu_disabled;
@@ -178,7 +185,13 @@ u16 *amd_iommu_alias_table;
 struct amd_iommu **amd_iommu_rlookup_table;
 
 /*
- * AMD IOMMU allows up to 2^16 differend protection domains. This is a bitmap
+ * This table is used to find the irq remapping table for a given device id
+ * quickly.
+ */
+struct irq_remap_table **irq_lookup_table;
+
+/*
+ * AMD IOMMU allows up to 2^16 different protection domains. This is a bitmap
  * to know which ones are already in use.
  */
 unsigned long *amd_iommu_pd_alloc_bitmap;
@@ -478,7 +491,7 @@ static int __init find_last_devid_acpi(struct acpi_table_header *table)
 
 /****************************************************************************
  *
- * The following functions belong the the code path which parses the ACPI table
+ * The following functions belong to the code path which parses the ACPI table
  * the second time. In this ACPI parsing iteration we allocate IOMMU specific
  * data structures, initialize the device/alias/rlookup table and also
  * basically initialize the hardware.
@@ -690,8 +703,33 @@ static void __init set_dev_entry_from_acpi(struct amd_iommu *iommu,
 	set_iommu_for_device(iommu, devid);
 }
 
+static int add_special_device(u8 type, u8 id, u16 devid)
+{
+	struct devid_map *entry;
+	struct list_head *list;
+
+	if (type != IVHD_SPECIAL_IOAPIC && type != IVHD_SPECIAL_HPET)
+		return -EINVAL;
+
+	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
+	if (!entry)
+		return -ENOMEM;
+
+	entry->id    = id;
+	entry->devid = devid;
+
+	if (type == IVHD_SPECIAL_IOAPIC)
+		list = &ioapic_map;
+	else
+		list = &hpet_map;
+
+	list_add_tail(&entry->list, list);
+
+	return 0;
+}
+
 /*
- * Reads the device exclusion range from ACPI and initialize IOMMU with
+ * Reads the device exclusion range from ACPI and initializes the IOMMU with
  * it
  */
 static void __init set_device_exclusion_range(u16 devid, struct ivmd_header *m)
@@ -717,7 +755,7 @@ static void __init set_device_exclusion_range(u16 devid, struct ivmd_header *m)
  * Takes a pointer to an AMD IOMMU entry in the ACPI table and
  * initializes the hardware and our data structures with it.
  */
-static void __init init_iommu_from_acpi(struct amd_iommu *iommu,
+static int __init init_iommu_from_acpi(struct amd_iommu *iommu,
 					struct ivhd_header *h)
 {
 	u8 *p = (u8 *)h;
@@ -867,12 +905,43 @@ static void __init init_iommu_from_acpi(struct amd_iommu *iommu,
 							flags, ext_flags);
 			}
 			break;
+		case IVHD_DEV_SPECIAL: {
+			u8 handle, type;
+			const char *var;
+			u16 devid;
+			int ret;
+
+			handle = e->ext & 0xff;
+			devid  = (e->ext >>  8) & 0xffff;
+			type   = (e->ext >> 24) & 0xff;
+
+			if (type == IVHD_SPECIAL_IOAPIC)
+				var = "IOAPIC";
+			else if (type == IVHD_SPECIAL_HPET)
+				var = "HPET";
+			else
+				var = "UNKNOWN";
+
+			DUMP_printk("  DEV_SPECIAL(%s[%d])\t\tdevid: %02x:%02x.%x\n",
+				    var, (int)handle,
+				    PCI_BUS(devid),
+				    PCI_SLOT(devid),
+				    PCI_FUNC(devid));
+
+			set_dev_entry_from_acpi(iommu, devid, e->flags, 0);
+			ret = add_special_device(type, handle, devid);
+			if (ret)
+				return ret;
+			break;
+		}
 		default:
 			break;
 		}
 
 		p += ivhd_entry_length(p);
 	}
+
+	return 0;
 }
 
 /* Initializes the device->iommu mapping for the driver */
@@ -912,6 +981,8 @@ static void __init free_iommu_all(void)
  */
 static int __init init_iommu_one(struct amd_iommu *iommu, struct ivhd_header *h)
 {
+	int ret;
+
 	spin_lock_init(&iommu->lock);
 
 	/* Add IOMMU to internal data structures */
@@ -947,7 +1018,16 @@ static int __init init_iommu_one(struct amd_iommu *iommu, struct ivhd_header *h)
 
 	iommu->int_enabled = false;
 
-	init_iommu_from_acpi(iommu, h);
+	ret = init_iommu_from_acpi(iommu, h);
+	if (ret)
+		return ret;
+
+	/*
+	 * Make sure IOMMU is not considered to translate itself. The IVRS
+	 * table tells us so, but this is a lie!
+	 */
+	amd_iommu_rlookup_table[iommu->devid] = NULL;
+
 	init_iommu_devices(iommu);
 
 	return 0;
@@ -1115,9 +1195,11 @@ static void print_iommu_info(void)
 				if (iommu_feature(iommu, (1ULL << i)))
 					pr_cont(" %s", feat_str[i]);
 			}
-		}
 		pr_cont("\n");
+		}
 	}
+	if (irq_remapping_enabled)
+		pr_info("AMD-Vi: Interrupt remapping enabled\n");
 }
 
 static int __init amd_iommu_init_pci(void)
@@ -1141,7 +1223,7 @@ static int __init amd_iommu_init_pci(void)
 /****************************************************************************
  *
  * The following functions initialize the MSI interrupts for all IOMMUs
- * in the system. Its a bit challenging because there could be multiple
+ * in the system. It's a bit challenging because there could be multiple
  * IOMMUs per PCI BDF but we can call pci_enable_msi(x) only once per
  * pci_dev.
  *
@@ -1199,7 +1281,7 @@ enable_faults:
  *
  * The next functions belong to the third pass of parsing the ACPI
  * table. In this last pass the memory mapping requirements are
- * gathered (like exclusion and unity mapping reanges).
+ * gathered (like exclusion and unity mapping ranges).
  *
  ****************************************************************************/
 
@@ -1308,7 +1390,7 @@ static int __init init_memory_definitions(struct acpi_table_header *table)
  * Init the device table to not allow DMA access for devices and
  * suppress all page faults
  */
-static void init_device_table(void)
+static void init_device_table_dma(void)
 {
 	u32 devid;
 
@@ -1318,6 +1400,27 @@ static void init_device_table(void)
 	}
 }
 
+static void __init uninit_device_table_dma(void)
+{
+	u32 devid;
+
+	for (devid = 0; devid <= amd_iommu_last_bdf; ++devid) {
+		amd_iommu_dev_table[devid].data[0] = 0ULL;
+		amd_iommu_dev_table[devid].data[1] = 0ULL;
+	}
+}
+
+static void init_device_table(void)
+{
+	u32 devid;
+
+	if (!amd_iommu_irq_remap)
+		return;
+
+	for (devid = 0; devid <= amd_iommu_last_bdf; ++devid)
+		set_dev_entry_bit(devid, DEV_ENTRY_IRQ_TBL_EN);
+}
+
 static void iommu_init_flags(struct amd_iommu *iommu)
 {
 	iommu->acpi_flags & IVHD_FLAG_HT_TUN_EN_MASK ?
@@ -1466,10 +1569,14 @@ static struct syscore_ops amd_iommu_syscore_ops = {
 
 static void __init free_on_init_error(void)
 {
-	amd_iommu_uninit_devices();
+	free_pages((unsigned long)irq_lookup_table,
+		   get_order(rlookup_table_size));
 
-	free_pages((unsigned long)amd_iommu_pd_alloc_bitmap,
-		   get_order(MAX_DOMAIN_ID/8));
+	if (amd_iommu_irq_cache) {
+		kmem_cache_destroy(amd_iommu_irq_cache);
+		amd_iommu_irq_cache = NULL;
+
+	}
 
 	free_pages((unsigned long)amd_iommu_rlookup_table,
 		   get_order(rlookup_table_size));
@@ -1482,8 +1589,6 @@ static void __init free_on_init_error(void)
 
 	free_iommu_all();
 
-	free_unity_maps();
-
 #ifdef CONFIG_GART_IOMMU
 	/*
 	 * We failed to initialize the AMD IOMMU - try fallback to GART
@@ -1494,6 +1599,33 @@ static void __init free_on_init_error(void)
 #endif
 }
 
+static bool __init check_ioapic_information(void)
+{
+	int idx;
+
+	for (idx = 0; idx < nr_ioapics; idx++) {
+		int id = mpc_ioapic_id(idx);
+
+		if (get_ioapic_devid(id) < 0) {
+			pr_err(FW_BUG "AMD-Vi: IO-APIC[%d] not in IVRS table\n", id);
+			pr_err("AMD-Vi: Disabling interrupt remapping due to BIOS Bug\n");
+			return false;
+		}
+	}
+
+	return true;
+}
+
+static void __init free_dma_resources(void)
+{
+	amd_iommu_uninit_devices();
+
+	free_pages((unsigned long)amd_iommu_pd_alloc_bitmap,
+		   get_order(MAX_DOMAIN_ID/8));
+
+	free_unity_maps();
+}
+
 /*
  * This is the hardware init function for AMD IOMMU in the system.
  * This function is called either from amd_iommu_init or from the interrupt
@@ -1580,9 +1712,6 @@ static int __init early_amd_iommu_init(void)
 	if (amd_iommu_pd_alloc_bitmap == NULL)
 		goto out;
 
-	/* init the device table */
-	init_device_table();
-
 	/*
 	 * let all alias entries point to itself
 	 */
@@ -1605,10 +1734,35 @@ static int __init early_amd_iommu_init(void)
 	if (ret)
 		goto out;
 
+	if (amd_iommu_irq_remap)
+		amd_iommu_irq_remap = check_ioapic_information();
+
+	if (amd_iommu_irq_remap) {
+		/*
+		 * Interrupt remapping enabled, create kmem_cache for the
+		 * remapping tables.
+		 */
+		amd_iommu_irq_cache = kmem_cache_create("irq_remap_cache",
+				MAX_IRQS_PER_TABLE * sizeof(u32),
+				IRQ_TABLE_ALIGNMENT,
+				0, NULL);
+		if (!amd_iommu_irq_cache)
+			goto out;
+
+		irq_lookup_table = (void *)__get_free_pages(
+				GFP_KERNEL | __GFP_ZERO,
+				get_order(rlookup_table_size));
+		if (!irq_lookup_table)
+			goto out;
+	}
+
 	ret = init_memory_definitions(ivrs_base);
 	if (ret)
 		goto out;
 
+	/* init the device table */
+	init_device_table();
+
 out:
 	/* Don't leak any ACPI memory */
 	early_acpi_os_unmap_memory((char __iomem *)ivrs_base, ivrs_size);
@@ -1652,13 +1806,22 @@ static bool detect_ivrs(void)
 	/* Make sure ACS will be enabled during PCI probe */
 	pci_request_acs();
 
+	if (!disable_irq_remap)
+		amd_iommu_irq_remap = true;
+
 	return true;
 }
 
 static int amd_iommu_init_dma(void)
 {
+	struct amd_iommu *iommu;
 	int ret;
 
+	init_device_table_dma();
+
+	for_each_iommu(iommu)
+		iommu_flush_all_caches(iommu);
+
 	if (iommu_pass_through)
 		ret = amd_iommu_init_passthrough();
 	else
@@ -1749,7 +1912,48 @@ static int __init iommu_go_to_state(enum iommu_init_state state)
 	return ret;
 }
 
+#ifdef CONFIG_IRQ_REMAP
+int __init amd_iommu_prepare(void)
+{
+	return iommu_go_to_state(IOMMU_ACPI_FINISHED);
+}
+
+int __init amd_iommu_supported(void)
+{
+	return amd_iommu_irq_remap ? 1 : 0;
+}
+
+int __init amd_iommu_enable(void)
+{
+	int ret;
+
+	ret = iommu_go_to_state(IOMMU_ENABLED);
+	if (ret)
+		return ret;
+
+	irq_remapping_enabled = 1;
+
+	return 0;
+}
+
+void amd_iommu_disable(void)
+{
+	amd_iommu_suspend();
+}
+
+int amd_iommu_reenable(int mode)
+{
+	amd_iommu_resume();
+
+	return 0;
+}
 
+int __init amd_iommu_enable_faulting(void)
+{
+	/* We enable MSI later when PCI is initialized */
+	return 0;
+}
+#endif
 
 /*
  * This is the core init function for AMD IOMMU hardware in the system.
@@ -1762,8 +1966,17 @@ static int __init amd_iommu_init(void)
 
 	ret = iommu_go_to_state(IOMMU_INITIALIZED);
 	if (ret) {
-		disable_iommus();
-		free_on_init_error();
+		free_dma_resources();
+		if (!irq_remapping_enabled) {
+			disable_iommus();
+			free_on_init_error();
+		} else {
+			struct amd_iommu *iommu;
+
+			uninit_device_table_dma();
+			for_each_iommu(iommu)
+				iommu_flush_all_caches(iommu);
+		}
 	}
 
 	return ret;
diff --git a/drivers/iommu/amd_iommu_proto.h b/drivers/iommu/amd_iommu_proto.h
index 1a7f41c6cc6..c294961bdd3 100644
--- a/drivers/iommu/amd_iommu_proto.h
+++ b/drivers/iommu/amd_iommu_proto.h
@@ -32,6 +32,14 @@ extern void amd_iommu_uninit_devices(void);
 extern void amd_iommu_init_notifier(void);
 extern void amd_iommu_init_api(void);
 
+/* Needed for interrupt remapping */
+extern int amd_iommu_supported(void);
+extern int amd_iommu_prepare(void);
+extern int amd_iommu_enable(void);
+extern void amd_iommu_disable(void);
+extern int amd_iommu_reenable(int);
+extern int amd_iommu_enable_faulting(void);
+
 /* IOMMUv2 specific functions */
 struct iommu_domain;
 
diff --git a/drivers/iommu/amd_iommu_types.h b/drivers/iommu/amd_iommu_types.h
index d0dab865a8b..c9aa3d079ff 100644
--- a/drivers/iommu/amd_iommu_types.h
+++ b/drivers/iommu/amd_iommu_types.h
@@ -152,6 +152,7 @@
 #define CMD_INV_DEV_ENTRY       0x02
 #define CMD_INV_IOMMU_PAGES	0x03
 #define CMD_INV_IOTLB_PAGES	0x04
+#define CMD_INV_IRT		0x05
 #define CMD_COMPLETE_PPR	0x07
 #define CMD_INV_ALL		0x08
 
@@ -175,6 +176,7 @@
 #define DEV_ENTRY_EX            0x67
 #define DEV_ENTRY_SYSMGT1       0x68
 #define DEV_ENTRY_SYSMGT2       0x69
+#define DEV_ENTRY_IRQ_TBL_EN	0x80
 #define DEV_ENTRY_INIT_PASS     0xb8
 #define DEV_ENTRY_EINT_PASS     0xb9
 #define DEV_ENTRY_NMI_PASS      0xba
@@ -183,6 +185,8 @@
 #define DEV_ENTRY_MODE_MASK	0x07
 #define DEV_ENTRY_MODE_SHIFT	0x09
 
+#define MAX_DEV_TABLE_ENTRIES	0xffff
+
 /* constants to configure the command buffer */
 #define CMD_BUFFER_SIZE    8192
 #define CMD_BUFFER_UNINITIALIZED 1
@@ -255,7 +259,7 @@
 #define PAGE_SIZE_ALIGN(address, pagesize) \
 		((address) & ~((pagesize) - 1))
 /*
- * Creates an IOMMU PTE for an address an a given pagesize
+ * Creates an IOMMU PTE for an address and a given pagesize
  * The PTE has no permission bits set
  * Pagesize is expected to be a power-of-two larger than 4096
  */
@@ -334,6 +338,23 @@ extern bool amd_iommu_np_cache;
 /* Only true if all IOMMUs support device IOTLBs */
 extern bool amd_iommu_iotlb_sup;
 
+#define MAX_IRQS_PER_TABLE	256
+#define IRQ_TABLE_ALIGNMENT	128
+
+struct irq_remap_table {
+	spinlock_t lock;
+	unsigned min_index;
+	u32 *table;
+};
+
+extern struct irq_remap_table **irq_lookup_table;
+
+/* Interrupt remapping feature used? */
+extern bool amd_iommu_irq_remap;
+
+/* kmem_cache to get tables with 128 byte alignement */
+extern struct kmem_cache *amd_iommu_irq_cache;
+
 /*
  * Make iterating over all IOMMUs easier
  */
@@ -404,7 +425,7 @@ struct iommu_dev_data {
 	struct list_head dev_data_list;	  /* For global dev_data_list */
 	struct iommu_dev_data *alias_data;/* The alias dev_data */
 	struct protection_domain *domain; /* Domain the device is bound to */
-	atomic_t bind;			  /* Domain attach reverent count */
+	atomic_t bind;			  /* Domain attach reference count */
 	u16 devid;			  /* PCI Device ID */
 	bool iommu_v2;			  /* Device can make use of IOMMUv2 */
 	bool passthrough;		  /* Default for device is pt_domain */
@@ -565,6 +586,16 @@ struct amd_iommu {
 	u32 stored_l2[0x83];
 };
 
+struct devid_map {
+	struct list_head list;
+	u8 id;
+	u16 devid;
+};
+
+/* Map HPET and IOAPIC ids to the devid used by the IOMMU */
+extern struct list_head ioapic_map;
+extern struct list_head hpet_map;
+
 /*
  * List with all IOMMUs in the system. This list is not locked because it is
  * only written and read at driver initialization or suspend time
@@ -678,6 +709,30 @@ static inline u16 calc_devid(u8 bus, u8 devfn)
 	return (((u16)bus) << 8) | devfn;
 }
 
+static inline int get_ioapic_devid(int id)
+{
+	struct devid_map *entry;
+
+	list_for_each_entry(entry, &ioapic_map, list) {
+		if (entry->id == id)
+			return entry->devid;
+	}
+
+	return -EINVAL;
+}
+
+static inline int get_hpet_devid(int id)
+{
+	struct devid_map *entry;
+
+	list_for_each_entry(entry, &hpet_map, list) {
+		if (entry->id == id)
+			return entry->devid;
+	}
+
+	return -EINVAL;
+}
+
 #ifdef CONFIG_AMD_IOMMU_STATS
 
 struct __iommu_counter {
diff --git a/drivers/iommu/exynos-iommu.c b/drivers/iommu/exynos-iommu.c
index 80bad32aa46..7fe44f83cc3 100644
--- a/drivers/iommu/exynos-iommu.c
+++ b/drivers/iommu/exynos-iommu.c
@@ -840,8 +840,7 @@ static void exynos_iommu_detach_device(struct iommu_domain *domain,
 	if (__exynos_sysmmu_disable(data)) {
 		dev_dbg(dev, "%s: Detached IOMMU with pgtable %#lx\n",
 					__func__, __pa(priv->pgtable));
-		list_del(&data->node);
-		INIT_LIST_HEAD(&data->node);
+		list_del_init(&data->node);
 
 	} else {
 		dev_dbg(dev, "%s: Detaching IOMMU with pgtable %#lx delayed",
diff --git a/drivers/iommu/intel-iommu.c b/drivers/iommu/intel-iommu.c
index db820d7dd0b..d4a4cd445ca 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -589,7 +589,9 @@ static void domain_update_iommu_coherency(struct dmar_domain *domain)
 {
 	int i;
 
-	domain->iommu_coherency = 1;
+	i = find_first_bit(domain->iommu_bmp, g_num_of_iommus);
+
+	domain->iommu_coherency = i < g_num_of_iommus ? 1 : 0;
 
 	for_each_set_bit(i, domain->iommu_bmp, g_num_of_iommus) {
 		if (!ecap_coherent(g_iommus[i]->ecap)) {
diff --git a/drivers/iommu/irq_remapping.c b/drivers/iommu/irq_remapping.c
index 151690db692..faf85d6e33f 100644
--- a/drivers/iommu/irq_remapping.c
+++ b/drivers/iommu/irq_remapping.c
@@ -51,6 +51,11 @@ early_param("intremap", setup_irqremap);
 void __init setup_irq_remapping_ops(void)
 {
 	remap_ops = &intel_irq_remap_ops;
+
+#ifdef CONFIG_AMD_IOMMU
+	if (amd_iommu_irq_ops.prepare() == 0)
+		remap_ops = &amd_iommu_irq_ops;
+#endif
 }
 
 int irq_remapping_supported(void)
diff --git a/drivers/iommu/irq_remapping.h b/drivers/iommu/irq_remapping.h
index b12974cc1df..95363acb583 100644
--- a/drivers/iommu/irq_remapping.h
+++ b/drivers/iommu/irq_remapping.h
@@ -82,6 +82,12 @@ struct irq_remap_ops {
 };
 
 extern struct irq_remap_ops intel_irq_remap_ops;
+extern struct irq_remap_ops amd_iommu_irq_ops;
+
+#else  /* CONFIG_IRQ_REMAP */
+
+#define irq_remapping_enabled 0
+#define disable_irq_remap     1
 
 #endif /* CONFIG_IRQ_REMAP */
 
diff --git a/drivers/iommu/tegra-smmu.c b/drivers/iommu/tegra-smmu.c
index 2a4bb36bc68..0b4d62e0c64 100644
--- a/drivers/iommu/tegra-smmu.c
+++ b/drivers/iommu/tegra-smmu.c
@@ -32,14 +32,55 @@
 #include <linux/io.h>
 #include <linux/of.h>
 #include <linux/of_iommu.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
 
 #include <asm/page.h>
 #include <asm/cacheflush.h>
 
 #include <mach/iomap.h>
-#include <mach/smmu.h>
 #include <mach/tegra-ahb.h>
 
+enum smmu_hwgrp {
+	HWGRP_AFI,
+	HWGRP_AVPC,
+	HWGRP_DC,
+	HWGRP_DCB,
+	HWGRP_EPP,
+	HWGRP_G2,
+	HWGRP_HC,
+	HWGRP_HDA,
+	HWGRP_ISP,
+	HWGRP_MPE,
+	HWGRP_NV,
+	HWGRP_NV2,
+	HWGRP_PPCS,
+	HWGRP_SATA,
+	HWGRP_VDE,
+	HWGRP_VI,
+
+	HWGRP_COUNT,
+
+	HWGRP_END = ~0,
+};
+
+#define HWG_AFI		(1 << HWGRP_AFI)
+#define HWG_AVPC	(1 << HWGRP_AVPC)
+#define HWG_DC		(1 << HWGRP_DC)
+#define HWG_DCB		(1 << HWGRP_DCB)
+#define HWG_EPP		(1 << HWGRP_EPP)
+#define HWG_G2		(1 << HWGRP_G2)
+#define HWG_HC		(1 << HWGRP_HC)
+#define HWG_HDA		(1 << HWGRP_HDA)
+#define HWG_ISP		(1 << HWGRP_ISP)
+#define HWG_MPE		(1 << HWGRP_MPE)
+#define HWG_NV		(1 << HWGRP_NV)
+#define HWG_NV2		(1 << HWGRP_NV2)
+#define HWG_PPCS	(1 << HWGRP_PPCS)
+#define HWG_SATA	(1 << HWGRP_SATA)
+#define HWG_VDE		(1 << HWGRP_VDE)
+#define HWG_VI		(1 << HWGRP_VI)
+
 /* bitmap of the page sizes currently supported */
 #define SMMU_IOMMU_PGSIZES	(SZ_4K)
 
@@ -47,16 +88,29 @@
 #define SMMU_CONFIG_DISABLE			0
 #define SMMU_CONFIG_ENABLE			1
 
-#define SMMU_TLB_CONFIG				0x14
-#define SMMU_TLB_CONFIG_STATS__MASK		(1 << 31)
-#define SMMU_TLB_CONFIG_STATS__ENABLE		(1 << 31)
+/* REVISIT: To support multiple MCs */
+enum {
+	_MC = 0,
+};
+
+enum {
+	_TLB = 0,
+	_PTC,
+};
+
+#define SMMU_CACHE_CONFIG_BASE			0x14
+#define __SMMU_CACHE_CONFIG(mc, cache)		(SMMU_CACHE_CONFIG_BASE + 4 * cache)
+#define SMMU_CACHE_CONFIG(cache)		__SMMU_CACHE_CONFIG(_MC, cache)
+
+#define SMMU_CACHE_CONFIG_STATS_SHIFT		31
+#define SMMU_CACHE_CONFIG_STATS_ENABLE		(1 << SMMU_CACHE_CONFIG_STATS_SHIFT)
+#define SMMU_CACHE_CONFIG_STATS_TEST_SHIFT	30
+#define SMMU_CACHE_CONFIG_STATS_TEST		(1 << SMMU_CACHE_CONFIG_STATS_TEST_SHIFT)
+
 #define SMMU_TLB_CONFIG_HIT_UNDER_MISS__ENABLE	(1 << 29)
 #define SMMU_TLB_CONFIG_ACTIVE_LINES__VALUE	0x10
 #define SMMU_TLB_CONFIG_RESET_VAL		0x20000010
 
-#define SMMU_PTC_CONFIG				0x18
-#define SMMU_PTC_CONFIG_STATS__MASK		(1 << 31)
-#define SMMU_PTC_CONFIG_STATS__ENABLE		(1 << 31)
 #define SMMU_PTC_CONFIG_CACHE__ENABLE		(1 << 29)
 #define SMMU_PTC_CONFIG_INDEX_MAP__PATTERN	0x3f
 #define SMMU_PTC_CONFIG_RESET_VAL		0x2000003f
@@ -86,10 +140,10 @@
 
 #define SMMU_ASID_SECURITY			0x38
 
-#define SMMU_STATS_TLB_HIT_COUNT		0x1f0
-#define SMMU_STATS_TLB_MISS_COUNT		0x1f4
-#define SMMU_STATS_PTC_HIT_COUNT		0x1f8
-#define SMMU_STATS_PTC_MISS_COUNT		0x1fc
+#define SMMU_STATS_CACHE_COUNT_BASE		0x1f0
+
+#define SMMU_STATS_CACHE_COUNT(mc, cache, hitmiss)		\
+	(SMMU_STATS_CACHE_COUNT_BASE + 8 * cache + 4 * hitmiss)
 
 #define SMMU_TRANSLATION_ENABLE_0		0x228
 #define SMMU_TRANSLATION_ENABLE_1		0x22c
@@ -231,6 +285,12 @@ struct smmu_as {
 	spinlock_t		client_lock; /* for client list */
 };
 
+struct smmu_debugfs_info {
+	struct smmu_device *smmu;
+	int mc;
+	int cache;
+};
+
 /*
  * Per SMMU device - IOMMU device
  */
@@ -251,6 +311,9 @@ struct smmu_device {
 	unsigned long translation_enable_2;
 	unsigned long asid_security;
 
+	struct dentry *debugfs_root;
+	struct smmu_debugfs_info *debugfs_info;
+
 	struct device_node *ahb;
 
 	int		num_as;
@@ -412,8 +475,8 @@ static int smmu_setup_regs(struct smmu_device *smmu)
 	smmu_write(smmu, smmu->translation_enable_1, SMMU_TRANSLATION_ENABLE_1);
 	smmu_write(smmu, smmu->translation_enable_2, SMMU_TRANSLATION_ENABLE_2);
 	smmu_write(smmu, smmu->asid_security, SMMU_ASID_SECURITY);
-	smmu_write(smmu, SMMU_TLB_CONFIG_RESET_VAL, SMMU_TLB_CONFIG);
-	smmu_write(smmu, SMMU_PTC_CONFIG_RESET_VAL, SMMU_PTC_CONFIG);
+	smmu_write(smmu, SMMU_TLB_CONFIG_RESET_VAL, SMMU_CACHE_CONFIG(_TLB));
+	smmu_write(smmu, SMMU_PTC_CONFIG_RESET_VAL, SMMU_CACHE_CONFIG(_PTC));
 
 	smmu_flush_regs(smmu, 1);
 
@@ -895,6 +958,175 @@ static struct iommu_ops smmu_iommu_ops = {
 	.pgsize_bitmap	= SMMU_IOMMU_PGSIZES,
 };
 
+/* Should be in the order of enum */
+static const char * const smmu_debugfs_mc[] = { "mc", };
+static const char * const smmu_debugfs_cache[] = {  "tlb", "ptc", };
+
+static ssize_t smmu_debugfs_stats_write(struct file *file,
+					const char __user *buffer,
+					size_t count, loff_t *pos)
+{
+	struct smmu_debugfs_info *info;
+	struct smmu_device *smmu;
+	struct dentry *dent;
+	int i;
+	enum {
+		_OFF = 0,
+		_ON,
+		_RESET,
+	};
+	const char * const command[] = {
+		[_OFF]		= "off",
+		[_ON]		= "on",
+		[_RESET]	= "reset",
+	};
+	char str[] = "reset";
+	u32 val;
+	size_t offs;
+
+	count = min_t(size_t, count, sizeof(str));
+	if (copy_from_user(str, buffer, count))
+		return -EINVAL;
+
+	for (i = 0; i < ARRAY_SIZE(command); i++)
+		if (strncmp(str, command[i],
+			    strlen(command[i])) == 0)
+			break;
+
+	if (i == ARRAY_SIZE(command))
+		return -EINVAL;
+
+	dent = file->f_dentry;
+	info = dent->d_inode->i_private;
+	smmu = info->smmu;
+
+	offs = SMMU_CACHE_CONFIG(info->cache);
+	val = smmu_read(smmu, offs);
+	switch (i) {
+	case _OFF:
+		val &= ~SMMU_CACHE_CONFIG_STATS_ENABLE;
+		val &= ~SMMU_CACHE_CONFIG_STATS_TEST;
+		smmu_write(smmu, val, offs);
+		break;
+	case _ON:
+		val |= SMMU_CACHE_CONFIG_STATS_ENABLE;
+		val &= ~SMMU_CACHE_CONFIG_STATS_TEST;
+		smmu_write(smmu, val, offs);
+		break;
+	case _RESET:
+		val |= SMMU_CACHE_CONFIG_STATS_TEST;
+		smmu_write(smmu, val, offs);
+		val &= ~SMMU_CACHE_CONFIG_STATS_TEST;
+		smmu_write(smmu, val, offs);
+		break;
+	default:
+		BUG();
+		break;
+	}
+
+	dev_dbg(smmu->dev, "%s() %08x, %08x @%08x\n", __func__,
+		val, smmu_read(smmu, offs), offs);
+
+	return count;
+}
+
+static int smmu_debugfs_stats_show(struct seq_file *s, void *v)
+{
+	struct smmu_debugfs_info *info;
+	struct smmu_device *smmu;
+	struct dentry *dent;
+	int i;
+	const char * const stats[] = { "hit", "miss", };
+
+	dent = d_find_alias(s->private);
+	info = dent->d_inode->i_private;
+	smmu = info->smmu;
+
+	for (i = 0; i < ARRAY_SIZE(stats); i++) {
+		u32 val;
+		size_t offs;
+
+		offs = SMMU_STATS_CACHE_COUNT(info->mc, info->cache, i);
+		val = smmu_read(smmu, offs);
+		seq_printf(s, "%s:%08x ", stats[i], val);
+
+		dev_dbg(smmu->dev, "%s() %s %08x @%08x\n", __func__,
+			stats[i], val, offs);
+	}
+	seq_printf(s, "\n");
+
+	return 0;
+}
+
+static int smmu_debugfs_stats_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, smmu_debugfs_stats_show, inode);
+}
+
+static const struct file_operations smmu_debugfs_stats_fops = {
+	.open		= smmu_debugfs_stats_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+	.write		= smmu_debugfs_stats_write,
+};
+
+static void smmu_debugfs_delete(struct smmu_device *smmu)
+{
+	debugfs_remove_recursive(smmu->debugfs_root);
+	kfree(smmu->debugfs_info);
+}
+
+static void smmu_debugfs_create(struct smmu_device *smmu)
+{
+	int i;
+	size_t bytes;
+	struct dentry *root;
+
+	bytes = ARRAY_SIZE(smmu_debugfs_mc) * ARRAY_SIZE(smmu_debugfs_cache) *
+		sizeof(*smmu->debugfs_info);
+	smmu->debugfs_info = kmalloc(bytes, GFP_KERNEL);
+	if (!smmu->debugfs_info)
+		return;
+
+	root = debugfs_create_dir(dev_name(smmu->dev), NULL);
+	if (!root)
+		goto err_out;
+	smmu->debugfs_root = root;
+
+	for (i = 0; i < ARRAY_SIZE(smmu_debugfs_mc); i++) {
+		int j;
+		struct dentry *mc;
+
+		mc = debugfs_create_dir(smmu_debugfs_mc[i], root);
+		if (!mc)
+			goto err_out;
+
+		for (j = 0; j < ARRAY_SIZE(smmu_debugfs_cache); j++) {
+			struct dentry *cache;
+			struct smmu_debugfs_info *info;
+
+			info = smmu->debugfs_info;
+			info += i * ARRAY_SIZE(smmu_debugfs_mc) + j;
+			info->smmu = smmu;
+			info->mc = i;
+			info->cache = j;
+
+			cache = debugfs_create_file(smmu_debugfs_cache[j],
+						    S_IWUGO | S_IRUGO, mc,
+						    (void *)info,
+						    &smmu_debugfs_stats_fops);
+			if (!cache)
+				goto err_out;
+		}
+	}
+
+	return;
+
+err_out:
+	smmu_debugfs_delete(smmu);
+}
+
 static int tegra_smmu_suspend(struct device *dev)
 {
 	struct smmu_device *smmu = dev_get_drvdata(dev);
@@ -999,6 +1231,7 @@ static int tegra_smmu_probe(struct platform_device *pdev)
 	if (!smmu->avp_vector_page)
 		return -ENOMEM;
 
+	smmu_debugfs_create(smmu);
 	smmu_handle = smmu;
 	return 0;
 }
@@ -1008,6 +1241,8 @@ static int tegra_smmu_remove(struct platform_device *pdev)
 	struct smmu_device *smmu = platform_get_drvdata(pdev);
 	int i;
 
+	smmu_debugfs_delete(smmu);
+
 	smmu_write(smmu, SMMU_CONFIG_DISABLE, SMMU_CONFIG);
 	for (i = 0; i < smmu->num_as; i++)
 		free_pdir(&smmu->as[i]);
diff --git a/drivers/isdn/hisax/Kconfig b/drivers/isdn/hisax/Kconfig
index 452fde9edf8..70ecd0c1950 100644
--- a/drivers/isdn/hisax/Kconfig
+++ b/drivers/isdn/hisax/Kconfig
@@ -109,7 +109,7 @@ config HISAX_16_3
 
 config HISAX_TELESPCI
 	bool "Teles PCI"
-	depends on PCI && (BROKEN || !(SPARC || PPC || PARISC || M68K || (MIPS && !CPU_LITTLE_ENDIAN) || FRV))
+	depends on PCI && (BROKEN || !(SPARC || PPC || PARISC || M68K || (MIPS && !CPU_LITTLE_ENDIAN) || FRV || (XTENSA && !CPU_LITTLE_ENDIAN)))
 	help
 	  This enables HiSax support for the Teles PCI.
 	  See <file:Documentation/isdn/README.HiSax> on how to configure it.
@@ -237,7 +237,7 @@ config HISAX_MIC
 
 config HISAX_NETJET
 	bool "NETjet card"
-	depends on PCI && (BROKEN || !(SPARC || PPC || PARISC || M68K || (MIPS && !CPU_LITTLE_ENDIAN) || FRV))
+	depends on PCI && (BROKEN || !(SPARC || PPC || PARISC || M68K || (MIPS && !CPU_LITTLE_ENDIAN) || FRV || (XTENSA && !CPU_LITTLE_ENDIAN)))
 	help
 	  This enables HiSax support for the NetJet from Traverse
 	  Technologies.
@@ -248,7 +248,7 @@ config HISAX_NETJET
 
 config HISAX_NETJET_U
 	bool "NETspider U card"
-	depends on PCI && (BROKEN || !(SPARC || PPC || PARISC || M68K || (MIPS && !CPU_LITTLE_ENDIAN) || FRV))
+	depends on PCI && (BROKEN || !(SPARC || PPC || PARISC || M68K || (MIPS && !CPU_LITTLE_ENDIAN) || FRV || (XTENSA && !CPU_LITTLE_ENDIAN)))
 	help
 	  This enables HiSax support for the Netspider U interface ISDN card
 	  from Traverse Technologies.
@@ -316,7 +316,7 @@ config HISAX_GAZEL
 
 config HISAX_HFC_PCI
 	bool "HFC PCI-Bus cards"
-	depends on PCI && (BROKEN || !(SPARC || PPC || PARISC || M68K || (MIPS && !CPU_LITTLE_ENDIAN) || FRV))
+	depends on PCI && (BROKEN || !(SPARC || PPC || PARISC || M68K || (MIPS && !CPU_LITTLE_ENDIAN) || FRV || (XTENSA && !CPU_LITTLE_ENDIAN)))
 	help
 	  This enables HiSax support for the HFC-S PCI 2BDS0 based cards.
 
@@ -341,7 +341,7 @@ config HISAX_HFC_SX
 
 config HISAX_ENTERNOW_PCI
 	bool "Formula-n enter:now PCI card"
-	depends on HISAX_NETJET && PCI && (BROKEN || !(SPARC || PPC || PARISC || M68K || (MIPS && !CPU_LITTLE_ENDIAN) || FRV))
+	depends on HISAX_NETJET && PCI && (BROKEN || !(SPARC || PPC || PARISC || M68K || (MIPS && !CPU_LITTLE_ENDIAN) || FRV || (XTENSA && !CPU_LITTLE_ENDIAN)))
 	help
 	  This enables HiSax support for the Formula-n enter:now PCI
 	  ISDN card.
diff --git a/drivers/isdn/i4l/isdn_ppp.c b/drivers/isdn/i4l/isdn_ppp.c
index a1e76015082..61d78fa03b1 100644
--- a/drivers/isdn/i4l/isdn_ppp.c
+++ b/drivers/isdn/i4l/isdn_ppp.c
@@ -595,7 +595,7 @@ isdn_ppp_ioctl(int min, struct file *file, unsigned int cmd, unsigned long arg)
 			j = ipc->num / (sizeof(long) * 8);
 			i = ipc->num % (sizeof(long) * 8);
 			if (j < 8)
-				protos[j] |= (0x1 << i);
+				protos[j] |= (1UL << i);
 			ipc = ipc->next;
 		}
 		if ((r = set_arg(argp, protos, 8 * sizeof(long))))
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index 16578d3b52b..f508defc0d9 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -63,6 +63,17 @@ config LEDS_LM3533
 	  hardware-accelerated blinking with maximum on and off periods of 9.8
 	  and 77 seconds respectively.
 
+config LEDS_LM3642
+	tristate "LED support for LM3642 Chip"
+	depends on LEDS_CLASS && I2C
+	select REGMAP_I2C
+	help
+	  This option enables support for LEDs connected to LM3642.
+	  The LM3642 is a 4MHz fixed-frequency synchronous boost
+	  converter plus 1.5A constant current driver for a high-current
+	  white LED.
+
+
 config LEDS_LOCOMO
 	tristate "LED Support for Locomo device"
 	depends on LEDS_CLASS
@@ -192,11 +203,12 @@ config LEDS_LP5521
 	  programming the engines.
 
 config LEDS_LP5523
-	tristate "LED Support for N.S. LP5523 LED driver chip"
+	tristate "LED Support for TI/National LP5523/55231 LED driver chip"
 	depends on LEDS_CLASS && I2C
 	help
-	  If you say yes here you get support for the National Semiconductor
-	  LP5523 LED driver. It is 9 channel chip with programmable engines.
+	  If you say yes here you get support for TI/National Semiconductor
+	  LP5523/55231 LED driver.
+	  It is 9 channel chip with programmable engines.
 	  Driver provides direct control via LED class and interface for
 	  programming the engines.
 
@@ -422,13 +434,13 @@ config LEDS_MAX8997
 	  This option enables support for on-chip LED drivers on
 	  MAXIM MAX8997 PMIC.
 
-config LEDS_LM3556
-	tristate "LED support for LM3556 Chip"
+config LEDS_LM355x
+	tristate "LED support for LM355x Chips, LM3554 and LM3556"
 	depends on LEDS_CLASS && I2C
 	select REGMAP_I2C
 	help
-	  This option enables support for LEDs connected to LM3556.
-	  LM3556 includes Torch, Flash and Indicator functions.
+	  This option enables support for LEDs connected to LM355x.
+	  LM355x includes Torch, Flash and Indicator functions.
 
 config LEDS_OT200
 	tristate "LED support for the Bachmann OT200"
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index a9b627c4f8b..3fb9641b619 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -11,6 +11,7 @@ obj-$(CONFIG_LEDS_BD2802)		+= leds-bd2802.o
 obj-$(CONFIG_LEDS_LOCOMO)		+= leds-locomo.o
 obj-$(CONFIG_LEDS_LM3530)		+= leds-lm3530.o
 obj-$(CONFIG_LEDS_LM3533)		+= leds-lm3533.o
+obj-$(CONFIG_LEDS_LM3642)		+= leds-lm3642.o
 obj-$(CONFIG_LEDS_MIKROTIK_RB532)	+= leds-rb532.o
 obj-$(CONFIG_LEDS_S3C24XX)		+= leds-s3c24xx.o
 obj-$(CONFIG_LEDS_NET48XX)		+= leds-net48xx.o
@@ -48,7 +49,7 @@ obj-$(CONFIG_LEDS_NETXBIG)		+= leds-netxbig.o
 obj-$(CONFIG_LEDS_ASIC3)		+= leds-asic3.o
 obj-$(CONFIG_LEDS_RENESAS_TPU)		+= leds-renesas-tpu.o
 obj-$(CONFIG_LEDS_MAX8997)		+= leds-max8997.o
-obj-$(CONFIG_LEDS_LM3556)		+= leds-lm3556.o
+obj-$(CONFIG_LEDS_LM355x)		+= leds-lm355x.o
 obj-$(CONFIG_LEDS_BLINKM)		+= leds-blinkm.o
 
 # LED SPI Drivers
diff --git a/drivers/leds/led-class.c b/drivers/leds/led-class.c
index c599095bc00..48cce18e9d6 100644
--- a/drivers/leds/led-class.c
+++ b/drivers/leds/led-class.c
@@ -124,6 +124,16 @@ static void led_timer_function(unsigned long data)
 	mod_timer(&led_cdev->blink_timer, jiffies + msecs_to_jiffies(delay));
 }
 
+static void set_brightness_delayed(struct work_struct *ws)
+{
+	struct led_classdev *led_cdev =
+		container_of(ws, struct led_classdev, set_brightness_work);
+
+	led_stop_software_blink(led_cdev);
+
+	__led_set_brightness(led_cdev, led_cdev->delayed_set_value);
+}
+
 /**
  * led_classdev_suspend - suspend an led_classdev.
  * @led_cdev: the led_classdev to suspend.
@@ -191,6 +201,8 @@ int led_classdev_register(struct device *parent, struct led_classdev *led_cdev)
 
 	led_update_brightness(led_cdev);
 
+	INIT_WORK(&led_cdev->set_brightness_work, set_brightness_delayed);
+
 	init_timer(&led_cdev->blink_timer);
 	led_cdev->blink_timer.function = led_timer_function;
 	led_cdev->blink_timer.data = (unsigned long)led_cdev;
@@ -221,7 +233,10 @@ void led_classdev_unregister(struct led_classdev *led_cdev)
 	up_write(&led_cdev->trigger_lock);
 #endif
 
+	cancel_work_sync(&led_cdev->set_brightness_work);
+
 	/* Stop blinking */
+	led_stop_software_blink(led_cdev);
 	led_set_brightness(led_cdev, LED_OFF);
 
 	device_unregister(led_cdev->dev);
diff --git a/drivers/leds/led-core.c b/drivers/leds/led-core.c
index 2ab05af3de3..ce8921a753a 100644
--- a/drivers/leds/led-core.c
+++ b/drivers/leds/led-core.c
@@ -103,13 +103,23 @@ void led_blink_set_oneshot(struct led_classdev *led_cdev,
 }
 EXPORT_SYMBOL(led_blink_set_oneshot);
 
-void led_set_brightness(struct led_classdev *led_cdev,
-			enum led_brightness brightness)
+void led_stop_software_blink(struct led_classdev *led_cdev)
 {
-	/* stop and clear soft-blink timer */
 	del_timer_sync(&led_cdev->blink_timer);
 	led_cdev->blink_delay_on = 0;
 	led_cdev->blink_delay_off = 0;
+}
+EXPORT_SYMBOL_GPL(led_stop_software_blink);
+
+void led_set_brightness(struct led_classdev *led_cdev,
+			enum led_brightness brightness)
+{
+	/* delay brightness setting if need to stop soft-blink timer */
+	if (led_cdev->blink_delay_on || led_cdev->blink_delay_off) {
+		led_cdev->delayed_set_value = brightness;
+		schedule_work(&led_cdev->set_brightness_work);
+		return;
+	}
 
 	__led_set_brightness(led_cdev, brightness);
 }
diff --git a/drivers/leds/led-triggers.c b/drivers/leds/led-triggers.c
index 363975b3c92..262eb419371 100644
--- a/drivers/leds/led-triggers.c
+++ b/drivers/leds/led-triggers.c
@@ -102,6 +102,12 @@ EXPORT_SYMBOL_GPL(led_trigger_show);
 void led_trigger_set(struct led_classdev *led_cdev, struct led_trigger *trig)
 {
 	unsigned long flags;
+	char *event = NULL;
+	char *envp[2];
+	const char *name;
+
+	name = trig ? trig->name : "none";
+	event = kasprintf(GFP_KERNEL, "TRIGGER=%s", name);
 
 	/* Remove any existing trigger */
 	if (led_cdev->trigger) {
@@ -109,6 +115,8 @@ void led_trigger_set(struct led_classdev *led_cdev, struct led_trigger *trig)
 		list_del(&led_cdev->trig_list);
 		write_unlock_irqrestore(&led_cdev->trigger->leddev_list_lock,
 			flags);
+		cancel_work_sync(&led_cdev->set_brightness_work);
+		led_stop_software_blink(led_cdev);
 		if (led_cdev->trigger->deactivate)
 			led_cdev->trigger->deactivate(led_cdev);
 		led_cdev->trigger = NULL;
@@ -122,6 +130,13 @@ void led_trigger_set(struct led_classdev *led_cdev, struct led_trigger *trig)
 		if (trig->activate)
 			trig->activate(led_cdev);
 	}
+
+	if (event) {
+		envp[0] = event;
+		envp[1] = NULL;
+		kobject_uevent_env(&led_cdev->dev->kobj, KOBJ_CHANGE, envp);
+		kfree(event);
+	}
 }
 EXPORT_SYMBOL_GPL(led_trigger_set);
 
@@ -224,7 +239,7 @@ void led_trigger_event(struct led_trigger *trig,
 		struct led_classdev *led_cdev;
 
 		led_cdev = list_entry(entry, struct led_classdev, trig_list);
-		__led_set_brightness(led_cdev, brightness);
+		led_set_brightness(led_cdev, brightness);
 	}
 	read_unlock(&trig->leddev_list_lock);
 }
diff --git a/drivers/leds/leds-clevo-mail.c b/drivers/leds/leds-clevo-mail.c
index 1ed1677c916..e024b0b1c3b 100644
--- a/drivers/leds/leds-clevo-mail.c
+++ b/drivers/leds/leds-clevo-mail.c
@@ -31,7 +31,7 @@ static int __init clevo_mail_led_dmi_callback(const struct dmi_system_id *id)
 }
 
 /*
- * struct mail_led_whitelist - List of known good models
+ * struct clevo_mail_led_dmi_table - List of known good models
  *
  * Contains the known good models this driver is compatible with.
  * When adding a new model try to be as strict as possible. This
@@ -39,7 +39,7 @@ static int __init clevo_mail_led_dmi_callback(const struct dmi_system_id *id)
  * detected as working, but in reality it is not) as low as
  * possible.
  */
-static struct dmi_system_id __initdata mail_led_whitelist[] = {
+static struct dmi_system_id __initdata clevo_mail_led_dmi_table[] = {
 	{
 		.callback = clevo_mail_led_dmi_callback,
 		.ident = "Clevo D410J",
@@ -59,11 +59,10 @@ static struct dmi_system_id __initdata mail_led_whitelist[] = {
 	},
 	{
 		.callback = clevo_mail_led_dmi_callback,
-		.ident = "Positivo Mobile",
+		.ident = "Clevo M5x0V",
 		.matches = {
 			DMI_MATCH(DMI_BOARD_VENDOR, "CLEVO Co. "),
 			DMI_MATCH(DMI_BOARD_NAME, "M5X0V "),
-			DMI_MATCH(DMI_PRODUCT_NAME, "Positivo Mobile"),
 			DMI_MATCH(DMI_PRODUCT_VERSION, "VT6198")
 		}
 	},
@@ -89,6 +88,7 @@ static struct dmi_system_id __initdata mail_led_whitelist[] = {
 	},
 	{ }
 };
+MODULE_DEVICE_TABLE(dmi, clevo_mail_led_dmi_table);
 
 static void clevo_mail_led_set(struct led_classdev *led_cdev,
 				enum led_brightness value)
@@ -180,7 +180,7 @@ static int __init clevo_mail_led_init(void)
 
 	/* Check with the help of DMI if we are running on supported hardware */
 	if (!nodetect) {
-		count = dmi_check_system(mail_led_whitelist);
+		count = dmi_check_system(clevo_mail_led_dmi_table);
 	} else {
 		count = 1;
 		printk(KERN_ERR KBUILD_MODNAME ": Skipping DMI detection. "
diff --git a/drivers/leds/leds-gpio.c b/drivers/leds/leds-gpio.c
index c032b218034..087d1e66f4f 100644
--- a/drivers/leds/leds-gpio.c
+++ b/drivers/leds/leds-gpio.c
@@ -20,6 +20,7 @@
 #include <linux/slab.h>
 #include <linux/workqueue.h>
 #include <linux/module.h>
+#include <linux/pinctrl/consumer.h>
 
 struct gpio_led_data {
 	struct led_classdev cdev;
@@ -170,11 +171,10 @@ static struct gpio_leds_priv * __devinit gpio_leds_create_of(struct platform_dev
 {
 	struct device_node *np = pdev->dev.of_node, *child;
 	struct gpio_leds_priv *priv;
-	int count = 0, ret;
+	int count, ret;
 
 	/* count LEDs in this device, so we know how much to allocate */
-	for_each_child_of_node(np, child)
-		count++;
+	count = of_get_child_count(np);
 	if (!count)
 		return NULL;
 
@@ -228,7 +228,6 @@ static struct gpio_leds_priv * __devinit gpio_leds_create_of(struct platform_dev
 {
 	return NULL;
 }
-#define of_gpio_leds_match NULL
 #endif /* CONFIG_OF_GPIO */
 
 
@@ -236,8 +235,14 @@ static int __devinit gpio_led_probe(struct platform_device *pdev)
 {
 	struct gpio_led_platform_data *pdata = pdev->dev.platform_data;
 	struct gpio_leds_priv *priv;
+	struct pinctrl *pinctrl;
 	int i, ret = 0;
 
+	pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
+	if (IS_ERR(pinctrl))
+		dev_warn(&pdev->dev,
+			"pins are not configured from the driver\n");
+
 	if (pdata && pdata->num_leds) {
 		priv = devm_kzalloc(&pdev->dev,
 				sizeof_gpio_leds_priv(pdata->num_leds),
@@ -270,13 +275,13 @@ static int __devinit gpio_led_probe(struct platform_device *pdev)
 
 static int __devexit gpio_led_remove(struct platform_device *pdev)
 {
-	struct gpio_leds_priv *priv = dev_get_drvdata(&pdev->dev);
+	struct gpio_leds_priv *priv = platform_get_drvdata(pdev);
 	int i;
 
 	for (i = 0; i < priv->num_leds; i++)
 		delete_gpio_led(&priv->leds[i]);
 
-	dev_set_drvdata(&pdev->dev, NULL);
+	platform_set_drvdata(pdev, NULL);
 
 	return 0;
 }
@@ -287,7 +292,7 @@ static struct platform_driver gpio_led_driver = {
 	.driver		= {
 		.name	= "leds-gpio",
 		.owner	= THIS_MODULE,
-		.of_match_table = of_gpio_leds_match,
+		.of_match_table = of_match_ptr(of_gpio_leds_match),
 	},
 };
 
diff --git a/drivers/leds/leds-lm3530.c b/drivers/leds/leds-lm3530.c
index 23637bdb275..b26306f6724 100644
--- a/drivers/leds/leds-lm3530.c
+++ b/drivers/leds/leds-lm3530.c
@@ -150,7 +150,7 @@ static int lm3530_get_mode_from_str(const char *str)
 		if (sysfs_streq(str, mode_map[i].mode))
 			return mode_map[i].mode_val;
 
-	return -1;
+	return -EINVAL;
 }
 
 static void lm3530_als_configure(struct lm3530_platform_data *pdata,
@@ -358,7 +358,7 @@ static ssize_t lm3530_mode_set(struct device *dev, struct device_attribute
 	mode = lm3530_get_mode_from_str(buf);
 	if (mode < 0) {
 		dev_err(dev, "Invalid mode\n");
-		return -EINVAL;
+		return mode;
 	}
 
 	drvdata->mode = mode;
@@ -416,7 +416,7 @@ static int __devinit lm3530_probe(struct i2c_client *client,
 
 	i2c_set_clientdata(client, drvdata);
 
-	drvdata->regulator = regulator_get(&client->dev, "vin");
+	drvdata->regulator = devm_regulator_get(&client->dev, "vin");
 	if (IS_ERR(drvdata->regulator)) {
 		dev_err(&client->dev, "regulator get failed\n");
 		err = PTR_ERR(drvdata->regulator);
@@ -429,15 +429,13 @@ static int __devinit lm3530_probe(struct i2c_client *client,
 		if (err < 0) {
 			dev_err(&client->dev,
 				"Register Init failed: %d\n", err);
-			err = -ENODEV;
-			goto err_reg_init;
+			return err;
 		}
 	}
 	err = led_classdev_register(&client->dev, &drvdata->led_dev);
 	if (err < 0) {
 		dev_err(&client->dev, "Register led class failed: %d\n", err);
-		err = -ENODEV;
-		goto err_class_register;
+		return err;
 	}
 
 	err = device_create_file(drvdata->led_dev.dev, &dev_attr_mode);
@@ -451,9 +449,6 @@ static int __devinit lm3530_probe(struct i2c_client *client,
 
 err_create_file:
 	led_classdev_unregister(&drvdata->led_dev);
-err_class_register:
-err_reg_init:
-	regulator_put(drvdata->regulator);
 	return err;
 }
 
@@ -465,7 +460,6 @@ static int __devexit lm3530_remove(struct i2c_client *client)
 
 	if (drvdata->enable)
 		regulator_disable(drvdata->regulator);
-	regulator_put(drvdata->regulator);
 	led_classdev_unregister(&drvdata->led_dev);
 	return 0;
 }
diff --git a/drivers/leds/leds-lm3556.c b/drivers/leds/leds-lm3556.c
deleted file mode 100644
index 3062abd9a53..00000000000
--- a/drivers/leds/leds-lm3556.c
+++ /dev/null
@@ -1,512 +0,0 @@
-/*
- * Simple driver for Texas Instruments LM3556 LED Flash driver chip (Rev0x03)
- * Copyright (C) 2012 Texas Instruments
- *
- * 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.
- *
- * Please refer Documentation/leds/leds-lm3556.txt file.
- */
-#include <linux/module.h>
-#include <linux/delay.h>
-#include <linux/i2c.h>
-#include <linux/leds.h>
-#include <linux/slab.h>
-#include <linux/platform_device.h>
-#include <linux/fs.h>
-#include <linux/regmap.h>
-#include <linux/platform_data/leds-lm3556.h>
-
-#define REG_FILT_TIME			(0x0)
-#define REG_IVFM_MODE			(0x1)
-#define REG_NTC				(0x2)
-#define REG_INDIC_TIME			(0x3)
-#define REG_INDIC_BLINK			(0x4)
-#define REG_INDIC_PERIOD		(0x5)
-#define REG_TORCH_TIME			(0x6)
-#define REG_CONF			(0x7)
-#define REG_FLASH			(0x8)
-#define REG_I_CTRL			(0x9)
-#define REG_ENABLE			(0xA)
-#define REG_FLAG			(0xB)
-#define REG_MAX				(0xB)
-
-#define IVFM_FILTER_TIME_SHIFT		(3)
-#define UVLO_EN_SHIFT			(7)
-#define HYSTERSIS_SHIFT			(5)
-#define IVM_D_TH_SHIFT			(2)
-#define IVFM_ADJ_MODE_SHIFT		(0)
-#define NTC_EVENT_LVL_SHIFT		(5)
-#define NTC_TRIP_TH_SHIFT		(2)
-#define NTC_BIAS_I_LVL_SHIFT		(0)
-#define INDIC_RAMP_UP_TIME_SHIFT	(3)
-#define INDIC_RAMP_DN_TIME_SHIFT	(0)
-#define INDIC_N_BLANK_SHIFT		(4)
-#define INDIC_PULSE_TIME_SHIFT		(0)
-#define INDIC_N_PERIOD_SHIFT		(0)
-#define TORCH_RAMP_UP_TIME_SHIFT	(3)
-#define TORCH_RAMP_DN_TIME_SHIFT	(0)
-#define STROBE_USUAGE_SHIFT		(7)
-#define STROBE_PIN_POLARITY_SHIFT	(6)
-#define TORCH_PIN_POLARITY_SHIFT	(5)
-#define TX_PIN_POLARITY_SHIFT		(4)
-#define TX_EVENT_LVL_SHIFT		(3)
-#define IVFM_EN_SHIFT			(2)
-#define NTC_MODE_SHIFT			(1)
-#define INDIC_MODE_SHIFT		(0)
-#define INDUCTOR_I_LIMIT_SHIFT		(6)
-#define FLASH_RAMP_TIME_SHIFT		(3)
-#define FLASH_TOUT_TIME_SHIFT		(0)
-#define TORCH_I_SHIFT			(4)
-#define FLASH_I_SHIFT			(0)
-#define NTC_EN_SHIFT			(7)
-#define TX_PIN_EN_SHIFT			(6)
-#define STROBE_PIN_EN_SHIFT		(5)
-#define TORCH_PIN_EN_SHIFT		(4)
-#define PRECHG_MODE_EN_SHIFT		(3)
-#define PASS_MODE_ONLY_EN_SHIFT		(2)
-#define MODE_BITS_SHIFT			(0)
-
-#define IVFM_FILTER_TIME_MASK		(0x3)
-#define UVLO_EN_MASK			(0x1)
-#define HYSTERSIS_MASK			(0x3)
-#define IVM_D_TH_MASK			(0x7)
-#define IVFM_ADJ_MODE_MASK		(0x3)
-#define NTC_EVENT_LVL_MASK		(0x1)
-#define NTC_TRIP_TH_MASK		(0x7)
-#define NTC_BIAS_I_LVL_MASK		(0x3)
-#define INDIC_RAMP_UP_TIME_MASK		(0x7)
-#define INDIC_RAMP_DN_TIME_MASK		(0x7)
-#define INDIC_N_BLANK_MASK		(0x7)
-#define INDIC_PULSE_TIME_MASK		(0x7)
-#define INDIC_N_PERIOD_MASK		(0x7)
-#define TORCH_RAMP_UP_TIME_MASK		(0x7)
-#define TORCH_RAMP_DN_TIME_MASK		(0x7)
-#define STROBE_USUAGE_MASK		(0x1)
-#define STROBE_PIN_POLARITY_MASK	(0x1)
-#define TORCH_PIN_POLARITY_MASK		(0x1)
-#define TX_PIN_POLARITY_MASK		(0x1)
-#define TX_EVENT_LVL_MASK		(0x1)
-#define IVFM_EN_MASK			(0x1)
-#define NTC_MODE_MASK			(0x1)
-#define INDIC_MODE_MASK			(0x1)
-#define INDUCTOR_I_LIMIT_MASK		(0x3)
-#define FLASH_RAMP_TIME_MASK		(0x7)
-#define FLASH_TOUT_TIME_MASK		(0x7)
-#define TORCH_I_MASK			(0x7)
-#define FLASH_I_MASK			(0xF)
-#define NTC_EN_MASK			(0x1)
-#define TX_PIN_EN_MASK			(0x1)
-#define STROBE_PIN_EN_MASK		(0x1)
-#define TORCH_PIN_EN_MASK		(0x1)
-#define PRECHG_MODE_EN_MASK		(0x1)
-#define PASS_MODE_ONLY_EN_MASK		(0x1)
-#define MODE_BITS_MASK			(0x13)
-#define EX_PIN_CONTROL_MASK		(0xF1)
-#define EX_PIN_ENABLE_MASK		(0x70)
-
-enum lm3556_indic_pulse_time {
-	PULSE_TIME_0_MS = 0,
-	PULSE_TIME_32_MS,
-	PULSE_TIME_64_MS,
-	PULSE_TIME_92_MS,
-	PULSE_TIME_128_MS,
-	PULSE_TIME_160_MS,
-	PULSE_TIME_196_MS,
-	PULSE_TIME_224_MS,
-	PULSE_TIME_256_MS,
-	PULSE_TIME_288_MS,
-	PULSE_TIME_320_MS,
-	PULSE_TIME_352_MS,
-	PULSE_TIME_384_MS,
-	PULSE_TIME_416_MS,
-	PULSE_TIME_448_MS,
-	PULSE_TIME_480_MS,
-};
-
-enum lm3556_indic_n_blank {
-	INDIC_N_BLANK_0 = 0,
-	INDIC_N_BLANK_1,
-	INDIC_N_BLANK_2,
-	INDIC_N_BLANK_3,
-	INDIC_N_BLANK_4,
-	INDIC_N_BLANK_5,
-	INDIC_N_BLANK_6,
-	INDIC_N_BLANK_7,
-	INDIC_N_BLANK_8,
-	INDIC_N_BLANK_9,
-	INDIC_N_BLANK_10,
-	INDIC_N_BLANK_11,
-	INDIC_N_BLANK_12,
-	INDIC_N_BLANK_13,
-	INDIC_N_BLANK_14,
-	INDIC_N_BLANK_15,
-};
-
-enum lm3556_indic_period {
-	INDIC_PERIOD_0 = 0,
-	INDIC_PERIOD_1,
-	INDIC_PERIOD_2,
-	INDIC_PERIOD_3,
-	INDIC_PERIOD_4,
-	INDIC_PERIOD_5,
-	INDIC_PERIOD_6,
-	INDIC_PERIOD_7,
-};
-
-enum lm3556_mode {
-	MODES_STASNDBY = 0,
-	MODES_INDIC,
-	MODES_TORCH,
-	MODES_FLASH
-};
-
-#define INDIC_PATTERN_SIZE 4
-
-struct indicator {
-	u8 blinking;
-	u8 period_cnt;
-};
-
-struct lm3556_chip_data {
-	struct device *dev;
-
-	struct led_classdev cdev_flash;
-	struct led_classdev cdev_torch;
-	struct led_classdev cdev_indicator;
-
-	struct lm3556_platform_data *pdata;
-	struct regmap *regmap;
-	struct mutex lock;
-
-	unsigned int last_flag;
-};
-
-/* indicator pattern */
-static struct indicator indicator_pattern[INDIC_PATTERN_SIZE] = {
-	[0] = {(INDIC_N_BLANK_1 << INDIC_N_BLANK_SHIFT)
-	       | PULSE_TIME_32_MS, INDIC_PERIOD_1},
-	[1] = {(INDIC_N_BLANK_15 << INDIC_N_BLANK_SHIFT)
-	       | PULSE_TIME_32_MS, INDIC_PERIOD_2},
-	[2] = {(INDIC_N_BLANK_10 << INDIC_N_BLANK_SHIFT)
-	       | PULSE_TIME_32_MS, INDIC_PERIOD_4},
-	[3] = {(INDIC_N_BLANK_5 << INDIC_N_BLANK_SHIFT)
-	       | PULSE_TIME_32_MS, INDIC_PERIOD_7},
-};
-
-/* chip initialize */
-static int __devinit lm3556_chip_init(struct lm3556_chip_data *chip)
-{
-	unsigned int reg_val;
-	int ret;
-	struct lm3556_platform_data *pdata = chip->pdata;
-
-	/* set config register */
-	ret = regmap_read(chip->regmap, REG_CONF, &reg_val);
-	if (ret < 0) {
-		dev_err(chip->dev, "Failed to read REG_CONF Register\n");
-		goto out;
-	}
-
-	reg_val &= (~EX_PIN_CONTROL_MASK);
-	reg_val |= ((pdata->torch_pin_polarity & 0x01)
-		    << TORCH_PIN_POLARITY_SHIFT);
-	reg_val |= ((pdata->strobe_usuage & 0x01) << STROBE_USUAGE_SHIFT);
-	reg_val |= ((pdata->strobe_pin_polarity & 0x01)
-		    << STROBE_PIN_POLARITY_SHIFT);
-	reg_val |= ((pdata->tx_pin_polarity & 0x01) << TX_PIN_POLARITY_SHIFT);
-	reg_val |= ((pdata->indicator_mode & 0x01) << INDIC_MODE_SHIFT);
-
-	ret = regmap_write(chip->regmap, REG_CONF, reg_val);
-	if (ret < 0) {
-		dev_err(chip->dev, "Failed to write REG_CONF Regisgter\n");
-		goto out;
-	}
-
-	/* set enable register */
-	ret = regmap_read(chip->regmap, REG_ENABLE, &reg_val);
-	if (ret < 0) {
-		dev_err(chip->dev, "Failed to read REG_ENABLE Register\n");
-		goto out;
-	}
-
-	reg_val &= (~EX_PIN_ENABLE_MASK);
-	reg_val |= ((pdata->torch_pin_en & 0x01) << TORCH_PIN_EN_SHIFT);
-	reg_val |= ((pdata->strobe_pin_en & 0x01) << STROBE_PIN_EN_SHIFT);
-	reg_val |= ((pdata->tx_pin_en & 0x01) << TX_PIN_EN_SHIFT);
-
-	ret = regmap_write(chip->regmap, REG_ENABLE, reg_val);
-	if (ret < 0) {
-		dev_err(chip->dev, "Failed to write REG_ENABLE Regisgter\n");
-		goto out;
-	}
-
-out:
-	return ret;
-}
-
-/* chip control */
-static int lm3556_control(struct lm3556_chip_data *chip,
-			  u8 brightness, enum lm3556_mode opmode)
-{
-	int ret;
-	struct lm3556_platform_data *pdata = chip->pdata;
-
-	ret = regmap_read(chip->regmap, REG_FLAG, &chip->last_flag);
-	if (ret < 0) {
-		dev_err(chip->dev, "Failed to read REG_FLAG Register\n");
-		goto out;
-	}
-
-	if (chip->last_flag)
-		dev_info(chip->dev, "Last FLAG is 0x%x\n", chip->last_flag);
-
-	/* brightness 0 means off-state */
-	if (!brightness)
-		opmode = MODES_STASNDBY;
-
-	switch (opmode) {
-	case MODES_TORCH:
-		ret = regmap_update_bits(chip->regmap, REG_I_CTRL,
-					 TORCH_I_MASK << TORCH_I_SHIFT,
-					 (brightness - 1) << TORCH_I_SHIFT);
-
-		if (pdata->torch_pin_en)
-			opmode |= (TORCH_PIN_EN_MASK << TORCH_PIN_EN_SHIFT);
-		break;
-
-	case MODES_FLASH:
-		ret = regmap_update_bits(chip->regmap, REG_I_CTRL,
-					 FLASH_I_MASK << FLASH_I_SHIFT,
-					 (brightness - 1) << FLASH_I_SHIFT);
-		break;
-
-	case MODES_INDIC:
-		ret = regmap_update_bits(chip->regmap, REG_I_CTRL,
-					 TORCH_I_MASK << TORCH_I_SHIFT,
-					 (brightness - 1) << TORCH_I_SHIFT);
-		break;
-
-	case MODES_STASNDBY:
-		if (pdata->torch_pin_en)
-			opmode |= (TORCH_PIN_EN_MASK << TORCH_PIN_EN_SHIFT);
-		break;
-
-	default:
-		return ret;
-	}
-	if (ret < 0) {
-		dev_err(chip->dev, "Failed to write REG_I_CTRL Register\n");
-		goto out;
-	}
-	ret = regmap_update_bits(chip->regmap, REG_ENABLE,
-				 MODE_BITS_MASK << MODE_BITS_SHIFT,
-				 opmode << MODE_BITS_SHIFT);
-
-out:
-	return ret;
-}
-
-/* torch */
-static void lm3556_torch_brightness_set(struct led_classdev *cdev,
-					enum led_brightness brightness)
-{
-	struct lm3556_chip_data *chip =
-	    container_of(cdev, struct lm3556_chip_data, cdev_torch);
-
-	mutex_lock(&chip->lock);
-	lm3556_control(chip, brightness, MODES_TORCH);
-	mutex_unlock(&chip->lock);
-}
-
-/* flash */
-static void lm3556_strobe_brightness_set(struct led_classdev *cdev,
-					 enum led_brightness brightness)
-{
-	struct lm3556_chip_data *chip =
-	    container_of(cdev, struct lm3556_chip_data, cdev_flash);
-
-	mutex_lock(&chip->lock);
-	lm3556_control(chip, brightness, MODES_FLASH);
-	mutex_unlock(&chip->lock);
-}
-
-/* indicator */
-static void lm3556_indicator_brightness_set(struct led_classdev *cdev,
-					    enum led_brightness brightness)
-{
-	struct lm3556_chip_data *chip =
-	    container_of(cdev, struct lm3556_chip_data, cdev_indicator);
-
-	mutex_lock(&chip->lock);
-	lm3556_control(chip, brightness, MODES_INDIC);
-	mutex_unlock(&chip->lock);
-}
-
-/* indicator pattern */
-static ssize_t lm3556_indicator_pattern_store(struct device *dev,
-					      struct device_attribute *devAttr,
-					      const char *buf, size_t size)
-{
-	ssize_t ret;
-	struct led_classdev *led_cdev = dev_get_drvdata(dev);
-	struct lm3556_chip_data *chip =
-	    container_of(led_cdev, struct lm3556_chip_data, cdev_indicator);
-	unsigned int state;
-
-	ret = kstrtouint(buf, 10, &state);
-	if (ret)
-		goto out;
-	if (state > INDIC_PATTERN_SIZE - 1)
-		state = INDIC_PATTERN_SIZE - 1;
-
-	ret = regmap_write(chip->regmap, REG_INDIC_BLINK,
-			   indicator_pattern[state].blinking);
-	if (ret < 0) {
-		dev_err(chip->dev, "Failed to write REG_ENABLE Regisgter\n");
-		goto out;
-	}
-
-	ret = regmap_write(chip->regmap, REG_INDIC_PERIOD,
-			   indicator_pattern[state].period_cnt);
-	if (ret < 0) {
-		dev_err(chip->dev, "Failed to write REG_ENABLE Regisgter\n");
-		goto out;
-	}
-
-	return size;
-out:
-	dev_err(chip->dev, "Indicator pattern doesn't saved\n");
-	return size;
-}
-
-static DEVICE_ATTR(pattern, 0666, NULL, lm3556_indicator_pattern_store);
-
-static const struct regmap_config lm3556_regmap = {
-	.reg_bits = 8,
-	.val_bits = 8,
-	.max_register = REG_MAX,
-};
-
-/* module initialize */
-static int __devinit lm3556_probe(struct i2c_client *client,
-				  const struct i2c_device_id *id)
-{
-	struct lm3556_platform_data *pdata = client->dev.platform_data;
-	struct lm3556_chip_data *chip;
-
-	int err;
-
-	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
-		dev_err(&client->dev, "i2c functionality check fail.\n");
-		return -EOPNOTSUPP;
-	}
-
-	if (pdata == NULL) {
-		dev_err(&client->dev, "Needs Platform Data.\n");
-		return -ENODATA;
-	}
-
-	chip =
-	    devm_kzalloc(&client->dev, sizeof(struct lm3556_chip_data),
-			 GFP_KERNEL);
-	if (!chip)
-		return -ENOMEM;
-
-	chip->dev = &client->dev;
-	chip->pdata = pdata;
-
-	chip->regmap = devm_regmap_init_i2c(client, &lm3556_regmap);
-	if (IS_ERR(chip->regmap)) {
-		err = PTR_ERR(chip->regmap);
-		dev_err(&client->dev, "Failed to allocate register map: %d\n",
-			err);
-		return err;
-	}
-
-	mutex_init(&chip->lock);
-	i2c_set_clientdata(client, chip);
-
-	err = lm3556_chip_init(chip);
-	if (err < 0)
-		goto err_out;
-
-	/* flash */
-	chip->cdev_flash.name = "flash";
-	chip->cdev_flash.max_brightness = 16;
-	chip->cdev_flash.brightness_set = lm3556_strobe_brightness_set;
-	err = led_classdev_register((struct device *)
-				    &client->dev, &chip->cdev_flash);
-	if (err < 0)
-		goto err_out;
-	/* torch */
-	chip->cdev_torch.name = "torch";
-	chip->cdev_torch.max_brightness = 8;
-	chip->cdev_torch.brightness_set = lm3556_torch_brightness_set;
-	err = led_classdev_register((struct device *)
-				    &client->dev, &chip->cdev_torch);
-	if (err < 0)
-		goto err_create_torch_file;
-	/* indicator */
-	chip->cdev_indicator.name = "indicator";
-	chip->cdev_indicator.max_brightness = 8;
-	chip->cdev_indicator.brightness_set = lm3556_indicator_brightness_set;
-	err = led_classdev_register((struct device *)
-				    &client->dev, &chip->cdev_indicator);
-	if (err < 0)
-		goto err_create_indicator_file;
-
-	err = device_create_file(chip->cdev_indicator.dev, &dev_attr_pattern);
-	if (err < 0)
-		goto err_create_pattern_file;
-
-	dev_info(&client->dev, "LM3556 is initialized\n");
-	return 0;
-
-err_create_pattern_file:
-	led_classdev_unregister(&chip->cdev_indicator);
-err_create_indicator_file:
-	led_classdev_unregister(&chip->cdev_torch);
-err_create_torch_file:
-	led_classdev_unregister(&chip->cdev_flash);
-err_out:
-	return err;
-}
-
-static int __devexit lm3556_remove(struct i2c_client *client)
-{
-	struct lm3556_chip_data *chip = i2c_get_clientdata(client);
-
-	device_remove_file(chip->cdev_indicator.dev, &dev_attr_pattern);
-	led_classdev_unregister(&chip->cdev_indicator);
-	led_classdev_unregister(&chip->cdev_torch);
-	led_classdev_unregister(&chip->cdev_flash);
-	regmap_write(chip->regmap, REG_ENABLE, 0);
-	return 0;
-}
-
-static const struct i2c_device_id lm3556_id[] = {
-	{LM3556_NAME, 0},
-	{}
-};
-
-MODULE_DEVICE_TABLE(i2c, lm3556_id);
-
-static struct i2c_driver lm3556_i2c_driver = {
-	.driver = {
-		   .name = LM3556_NAME,
-		   .owner = THIS_MODULE,
-		   .pm = NULL,
-		   },
-	.probe = lm3556_probe,
-	.remove = __devexit_p(lm3556_remove),
-	.id_table = lm3556_id,
-};
-
-module_i2c_driver(lm3556_i2c_driver);
-
-MODULE_DESCRIPTION("Texas Instruments Flash Lighting driver for LM3556");
-MODULE_AUTHOR("Daniel Jeong <daniel.jeong@ti.com>");
-MODULE_AUTHOR("G.Shark Jeong <gshark.jeong@gmail.com>");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/leds/leds-lm355x.c b/drivers/leds/leds-lm355x.c
new file mode 100644
index 00000000000..065ec015d67
--- /dev/null
+++ b/drivers/leds/leds-lm355x.c
@@ -0,0 +1,572 @@
+/*
+* Simple driver for Texas Instruments LM355x LED Flash driver chip
+* Copyright (C) 2012 Texas Instruments
+*
+* 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/i2c.h>
+#include <linux/gpio.h>
+#include <linux/leds.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/fs.h>
+#include <linux/regmap.h>
+#include <linux/workqueue.h>
+#include <linux/platform_data/leds-lm355x.h>
+
+enum lm355x_type {
+	CHIP_LM3554 = 0,
+	CHIP_LM3556,
+};
+
+enum lm355x_regs {
+	REG_FLAG = 0,
+	REG_TORCH_CFG,
+	REG_TORCH_CTRL,
+	REG_STROBE_CFG,
+	REG_FLASH_CTRL,
+	REG_INDI_CFG,
+	REG_INDI_CTRL,
+	REG_OPMODE,
+	REG_MAX,
+};
+
+/* operation mode */
+enum lm355x_mode {
+	MODE_SHDN = 0,
+	MODE_INDIC,
+	MODE_TORCH,
+	MODE_FLASH
+};
+
+/* register map info. */
+struct lm355x_reg_data {
+	u8 regno;
+	u8 mask;
+	u8 shift;
+};
+
+struct lm355x_chip_data {
+	struct device *dev;
+	enum lm355x_type type;
+
+	struct led_classdev cdev_flash;
+	struct led_classdev cdev_torch;
+	struct led_classdev cdev_indicator;
+
+	struct work_struct work_flash;
+	struct work_struct work_torch;
+	struct work_struct work_indicator;
+
+	u8 br_flash;
+	u8 br_torch;
+	u8 br_indicator;
+
+	struct lm355x_platform_data *pdata;
+	struct regmap *regmap;
+	struct mutex lock;
+
+	unsigned int last_flag;
+	struct lm355x_reg_data *regs;
+};
+
+/* specific indicator function for lm3556 */
+enum lm3556_indic_pulse_time {
+	PULSE_TIME_0_MS = 0,
+	PULSE_TIME_32_MS,
+	PULSE_TIME_64_MS,
+	PULSE_TIME_92_MS,
+	PULSE_TIME_128_MS,
+	PULSE_TIME_160_MS,
+	PULSE_TIME_196_MS,
+	PULSE_TIME_224_MS,
+	PULSE_TIME_256_MS,
+	PULSE_TIME_288_MS,
+	PULSE_TIME_320_MS,
+	PULSE_TIME_352_MS,
+	PULSE_TIME_384_MS,
+	PULSE_TIME_416_MS,
+	PULSE_TIME_448_MS,
+	PULSE_TIME_480_MS,
+};
+
+enum lm3556_indic_n_blank {
+	INDIC_N_BLANK_0 = 0,
+	INDIC_N_BLANK_1,
+	INDIC_N_BLANK_2,
+	INDIC_N_BLANK_3,
+	INDIC_N_BLANK_4,
+	INDIC_N_BLANK_5,
+	INDIC_N_BLANK_6,
+	INDIC_N_BLANK_7,
+	INDIC_N_BLANK_8,
+	INDIC_N_BLANK_9,
+	INDIC_N_BLANK_10,
+	INDIC_N_BLANK_11,
+	INDIC_N_BLANK_12,
+	INDIC_N_BLANK_13,
+	INDIC_N_BLANK_14,
+	INDIC_N_BLANK_15,
+};
+
+enum lm3556_indic_period {
+	INDIC_PERIOD_0 = 0,
+	INDIC_PERIOD_1,
+	INDIC_PERIOD_2,
+	INDIC_PERIOD_3,
+	INDIC_PERIOD_4,
+	INDIC_PERIOD_5,
+	INDIC_PERIOD_6,
+	INDIC_PERIOD_7,
+};
+
+#define INDIC_PATTERN_SIZE 4
+
+struct indicator {
+	u8 blinking;
+	u8 period_cnt;
+};
+
+/* indicator pattern data only for lm3556 */
+static struct indicator indicator_pattern[INDIC_PATTERN_SIZE] = {
+	[0] = {(INDIC_N_BLANK_1 << 4) | PULSE_TIME_32_MS, INDIC_PERIOD_1},
+	[1] = {(INDIC_N_BLANK_15 << 4) | PULSE_TIME_32_MS, INDIC_PERIOD_2},
+	[2] = {(INDIC_N_BLANK_10 << 4) | PULSE_TIME_32_MS, INDIC_PERIOD_4},
+	[3] = {(INDIC_N_BLANK_5 << 4) | PULSE_TIME_32_MS, INDIC_PERIOD_7},
+};
+
+static struct lm355x_reg_data lm3554_regs[REG_MAX] = {
+	[REG_FLAG] = {0xD0, 0xBF, 0},
+	[REG_TORCH_CFG] = {0xE0, 0x80, 7},
+	[REG_TORCH_CTRL] = {0xA0, 0x38, 3},
+	[REG_STROBE_CFG] = {0xE0, 0x04, 2},
+	[REG_FLASH_CTRL] = {0xB0, 0x78, 3},
+	[REG_INDI_CFG] = {0xE0, 0x08, 3},
+	[REG_INDI_CTRL] = {0xA0, 0xC0, 6},
+	[REG_OPMODE] = {0xA0, 0x03, 0},
+};
+
+static struct lm355x_reg_data lm3556_regs[REG_MAX] = {
+	[REG_FLAG] = {0x0B, 0xFF, 0},
+	[REG_TORCH_CFG] = {0x0A, 0x10, 4},
+	[REG_TORCH_CTRL] = {0x09, 0x70, 4},
+	[REG_STROBE_CFG] = {0x0A, 0x20, 5},
+	[REG_FLASH_CTRL] = {0x09, 0x0F, 0},
+	[REG_INDI_CFG] = {0xFF, 0xFF, 0},
+	[REG_INDI_CTRL] = {0x09, 0x70, 4},
+	[REG_OPMODE] = {0x0A, 0x03, 0},
+};
+
+static char lm355x_name[][I2C_NAME_SIZE] = {
+	[CHIP_LM3554] = LM3554_NAME,
+	[CHIP_LM3556] = LM3556_NAME,
+};
+
+/* chip initialize */
+static int __devinit lm355x_chip_init(struct lm355x_chip_data *chip)
+{
+	int ret;
+	unsigned int reg_val;
+	struct lm355x_platform_data *pdata = chip->pdata;
+
+	/* input and output pins configuration */
+	switch (chip->type) {
+	case CHIP_LM3554:
+		reg_val = pdata->pin_tx2 | pdata->ntc_pin;
+		ret = regmap_update_bits(chip->regmap, 0xE0, 0x28, reg_val);
+		if (ret < 0)
+			goto out;
+		reg_val = pdata->pass_mode;
+		ret = regmap_update_bits(chip->regmap, 0xA0, 0x04, reg_val);
+		if (ret < 0)
+			goto out;
+		break;
+
+	case CHIP_LM3556:
+		reg_val = pdata->pin_tx2 | pdata->ntc_pin | pdata->pass_mode;
+		ret = regmap_update_bits(chip->regmap, 0x0A, 0xC4, reg_val);
+		if (ret < 0)
+			goto out;
+		break;
+	default:
+		return -ENODATA;
+	}
+
+	return ret;
+out:
+	dev_err(chip->dev, "%s:i2c access fail to register\n", __func__);
+	return ret;
+}
+
+/* chip control */
+static void lm355x_control(struct lm355x_chip_data *chip,
+			   u8 brightness, enum lm355x_mode opmode)
+{
+	int ret;
+	unsigned int reg_val;
+	struct lm355x_platform_data *pdata = chip->pdata;
+	struct lm355x_reg_data *preg = chip->regs;
+
+	ret = regmap_read(chip->regmap, preg[REG_FLAG].regno, &chip->last_flag);
+	if (ret < 0)
+		goto out;
+	if (chip->last_flag & preg[REG_FLAG].mask)
+		dev_info(chip->dev, "%s Last FLAG is 0x%x\n",
+			 lm355x_name[chip->type],
+			 chip->last_flag & preg[REG_FLAG].mask);
+	/* brightness 0 means shutdown */
+	if (!brightness)
+		opmode = MODE_SHDN;
+
+	switch (opmode) {
+	case MODE_TORCH:
+		ret =
+		    regmap_update_bits(chip->regmap, preg[REG_TORCH_CTRL].regno,
+				       preg[REG_TORCH_CTRL].mask,
+				       (brightness - 1)
+				       << preg[REG_TORCH_CTRL].shift);
+		if (ret < 0)
+			goto out;
+
+		if (pdata->pin_tx1 != LM355x_PIN_TORCH_DISABLE) {
+			ret =
+			    regmap_update_bits(chip->regmap,
+					       preg[REG_TORCH_CFG].regno,
+					       preg[REG_TORCH_CFG].mask,
+					       0x01 <<
+					       preg[REG_TORCH_CFG].shift);
+			if (ret < 0)
+				goto out;
+			opmode = MODE_SHDN;
+			dev_info(chip->dev,
+				 "torch brt is set - ext. torch pin mode\n");
+		}
+		break;
+
+	case MODE_FLASH:
+
+		ret =
+		    regmap_update_bits(chip->regmap, preg[REG_FLASH_CTRL].regno,
+				       preg[REG_FLASH_CTRL].mask,
+				       (brightness - 1)
+				       << preg[REG_FLASH_CTRL].shift);
+		if (ret < 0)
+			goto out;
+
+		if (pdata->pin_strobe != LM355x_PIN_STROBE_DISABLE) {
+			if (chip->type == CHIP_LM3554)
+				reg_val = 0x00;
+			else
+				reg_val = 0x01;
+			ret =
+			    regmap_update_bits(chip->regmap,
+					       preg[REG_STROBE_CFG].regno,
+					       preg[REG_STROBE_CFG].mask,
+					       reg_val <<
+					       preg[REG_STROBE_CFG].shift);
+			if (ret < 0)
+				goto out;
+			opmode = MODE_SHDN;
+			dev_info(chip->dev,
+				 "flash brt is set - ext. strobe pin mode\n");
+		}
+		break;
+
+	case MODE_INDIC:
+		ret =
+		    regmap_update_bits(chip->regmap, preg[REG_INDI_CTRL].regno,
+				       preg[REG_INDI_CTRL].mask,
+				       (brightness - 1)
+				       << preg[REG_INDI_CTRL].shift);
+		if (ret < 0)
+			goto out;
+
+		if (pdata->pin_tx2 != LM355x_PIN_TX_DISABLE) {
+			ret =
+			    regmap_update_bits(chip->regmap,
+					       preg[REG_INDI_CFG].regno,
+					       preg[REG_INDI_CFG].mask,
+					       0x01 <<
+					       preg[REG_INDI_CFG].shift);
+			if (ret < 0)
+				goto out;
+			opmode = MODE_SHDN;
+		}
+		break;
+	case MODE_SHDN:
+		break;
+	default:
+		return;
+	}
+	/* operation mode control */
+	ret = regmap_update_bits(chip->regmap, preg[REG_OPMODE].regno,
+				 preg[REG_OPMODE].mask,
+				 opmode << preg[REG_OPMODE].shift);
+	if (ret < 0)
+		goto out;
+	return;
+out:
+	dev_err(chip->dev, "%s:i2c access fail to register\n", __func__);
+	return;
+}
+
+/* torch */
+static void lm355x_deferred_torch_brightness_set(struct work_struct *work)
+{
+	struct lm355x_chip_data *chip =
+	    container_of(work, struct lm355x_chip_data, work_torch);
+
+	mutex_lock(&chip->lock);
+	lm355x_control(chip, chip->br_torch, MODE_TORCH);
+	mutex_unlock(&chip->lock);
+}
+
+static void lm355x_torch_brightness_set(struct led_classdev *cdev,
+					enum led_brightness brightness)
+{
+	struct lm355x_chip_data *chip =
+	    container_of(cdev, struct lm355x_chip_data, cdev_torch);
+
+	chip->br_torch = brightness;
+	schedule_work(&chip->work_torch);
+}
+
+/* flash */
+static void lm355x_deferred_strobe_brightness_set(struct work_struct *work)
+{
+	struct lm355x_chip_data *chip =
+	    container_of(work, struct lm355x_chip_data, work_flash);
+
+	mutex_lock(&chip->lock);
+	lm355x_control(chip, chip->br_flash, MODE_FLASH);
+	mutex_unlock(&chip->lock);
+}
+
+static void lm355x_strobe_brightness_set(struct led_classdev *cdev,
+					 enum led_brightness brightness)
+{
+	struct lm355x_chip_data *chip =
+	    container_of(cdev, struct lm355x_chip_data, cdev_flash);
+
+	chip->br_flash = brightness;
+	schedule_work(&chip->work_flash);
+}
+
+/* indicator */
+static void lm355x_deferred_indicator_brightness_set(struct work_struct *work)
+{
+	struct lm355x_chip_data *chip =
+	    container_of(work, struct lm355x_chip_data, work_indicator);
+
+	mutex_lock(&chip->lock);
+	lm355x_control(chip, chip->br_indicator, MODE_INDIC);
+	mutex_unlock(&chip->lock);
+}
+
+static void lm355x_indicator_brightness_set(struct led_classdev *cdev,
+					    enum led_brightness brightness)
+{
+	struct lm355x_chip_data *chip =
+	    container_of(cdev, struct lm355x_chip_data, cdev_indicator);
+
+	chip->br_indicator = brightness;
+	schedule_work(&chip->work_indicator);
+}
+
+/* indicator pattern only for lm3556*/
+static ssize_t lm3556_indicator_pattern_store(struct device *dev,
+					      struct device_attribute *devAttr,
+					      const char *buf, size_t size)
+{
+	ssize_t ret;
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct lm355x_chip_data *chip =
+	    container_of(led_cdev, struct lm355x_chip_data, cdev_indicator);
+	unsigned int state;
+
+	ret = kstrtouint(buf, 10, &state);
+	if (ret)
+		goto out;
+	if (state > INDIC_PATTERN_SIZE - 1)
+		state = INDIC_PATTERN_SIZE - 1;
+
+	ret = regmap_write(chip->regmap, 0x04,
+			   indicator_pattern[state].blinking);
+	if (ret < 0)
+		goto out;
+
+	ret = regmap_write(chip->regmap, 0x05,
+			   indicator_pattern[state].period_cnt);
+	if (ret < 0)
+		goto out;
+
+	return size;
+out:
+	dev_err(chip->dev, "%s:i2c access fail to register\n", __func__);
+	return size;
+}
+
+static DEVICE_ATTR(pattern, 0666, NULL, lm3556_indicator_pattern_store);
+
+static const struct regmap_config lm355x_regmap = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = 0xFF,
+};
+
+/* module initialize */
+static int __devinit lm355x_probe(struct i2c_client *client,
+				  const struct i2c_device_id *id)
+{
+	struct lm355x_platform_data *pdata = client->dev.platform_data;
+	struct lm355x_chip_data *chip;
+
+	int err;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev, "i2c functionality check fail.\n");
+		return -EOPNOTSUPP;
+	}
+
+	if (pdata == NULL) {
+		dev_err(&client->dev, "needs Platform Data.\n");
+		return -ENODATA;
+	}
+
+	chip = devm_kzalloc(&client->dev,
+			    sizeof(struct lm355x_chip_data), GFP_KERNEL);
+	if (!chip)
+		return -ENOMEM;
+
+	chip->dev = &client->dev;
+	chip->type = id->driver_data;
+	switch (id->driver_data) {
+	case CHIP_LM3554:
+		chip->regs = lm3554_regs;
+		break;
+	case CHIP_LM3556:
+		chip->regs = lm3556_regs;
+		break;
+	default:
+		return -ENOSYS;
+	}
+	chip->pdata = pdata;
+
+	chip->regmap = devm_regmap_init_i2c(client, &lm355x_regmap);
+	if (IS_ERR(chip->regmap)) {
+		err = PTR_ERR(chip->regmap);
+		dev_err(&client->dev,
+			"Failed to allocate register map: %d\n", err);
+		return err;
+	}
+
+	mutex_init(&chip->lock);
+	i2c_set_clientdata(client, chip);
+
+	err = lm355x_chip_init(chip);
+	if (err < 0)
+		goto err_out;
+
+	/* flash */
+	INIT_WORK(&chip->work_flash, lm355x_deferred_strobe_brightness_set);
+	chip->cdev_flash.name = "flash";
+	chip->cdev_flash.max_brightness = 16;
+	chip->cdev_flash.brightness_set = lm355x_strobe_brightness_set;
+	err = led_classdev_register((struct device *)
+				    &client->dev, &chip->cdev_flash);
+	if (err < 0)
+		goto err_out;
+	/* torch */
+	INIT_WORK(&chip->work_torch, lm355x_deferred_torch_brightness_set);
+	chip->cdev_torch.name = "torch";
+	chip->cdev_torch.max_brightness = 8;
+	chip->cdev_torch.brightness_set = lm355x_torch_brightness_set;
+	err = led_classdev_register((struct device *)
+				    &client->dev, &chip->cdev_torch);
+	if (err < 0)
+		goto err_create_torch_file;
+	/* indicator */
+	INIT_WORK(&chip->work_indicator,
+		  lm355x_deferred_indicator_brightness_set);
+	chip->cdev_indicator.name = "indicator";
+	if (id->driver_data == CHIP_LM3554)
+		chip->cdev_indicator.max_brightness = 4;
+	else
+		chip->cdev_indicator.max_brightness = 8;
+	chip->cdev_indicator.brightness_set = lm355x_indicator_brightness_set;
+	err = led_classdev_register((struct device *)
+				    &client->dev, &chip->cdev_indicator);
+	if (err < 0)
+		goto err_create_indicator_file;
+	/* indicator pattern control only for LM3554 */
+	if (id->driver_data == CHIP_LM3556) {
+		err =
+		    device_create_file(chip->cdev_indicator.dev,
+				       &dev_attr_pattern);
+		if (err < 0)
+			goto err_create_pattern_file;
+	}
+
+	dev_info(&client->dev, "%s is initialized\n",
+		 lm355x_name[id->driver_data]);
+	return 0;
+
+err_create_pattern_file:
+	led_classdev_unregister(&chip->cdev_indicator);
+err_create_indicator_file:
+	led_classdev_unregister(&chip->cdev_torch);
+err_create_torch_file:
+	led_classdev_unregister(&chip->cdev_flash);
+err_out:
+	return err;
+}
+
+static int __devexit lm355x_remove(struct i2c_client *client)
+{
+	struct lm355x_chip_data *chip = i2c_get_clientdata(client);
+	struct lm355x_reg_data *preg = chip->regs;
+
+	regmap_write(chip->regmap, preg[REG_OPMODE].regno, 0);
+	if (chip->type == CHIP_LM3556)
+		device_remove_file(chip->cdev_indicator.dev, &dev_attr_pattern);
+	led_classdev_unregister(&chip->cdev_indicator);
+	flush_work(&chip->work_indicator);
+	led_classdev_unregister(&chip->cdev_torch);
+	flush_work(&chip->work_torch);
+	led_classdev_unregister(&chip->cdev_flash);
+	flush_work(&chip->work_flash);
+	dev_info(&client->dev, "%s is removed\n", lm355x_name[chip->type]);
+
+	return 0;
+}
+
+static const struct i2c_device_id lm355x_id[] = {
+	{LM3554_NAME, CHIP_LM3554},
+	{LM3556_NAME, CHIP_LM3556},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, lm355x_id);
+
+static struct i2c_driver lm355x_i2c_driver = {
+	.driver = {
+		   .name = LM355x_NAME,
+		   .owner = THIS_MODULE,
+		   .pm = NULL,
+		   },
+	.probe = lm355x_probe,
+	.remove = __devexit_p(lm355x_remove),
+	.id_table = lm355x_id,
+};
+
+module_i2c_driver(lm355x_i2c_driver);
+
+MODULE_DESCRIPTION("Texas Instruments Flash Lighting driver for LM355x");
+MODULE_AUTHOR("Daniel Jeong <daniel.jeong@ti.com>");
+MODULE_AUTHOR("G.Shark Jeong <gshark.jeong@gmail.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/leds/leds-lm3642.c b/drivers/leds/leds-lm3642.c
new file mode 100644
index 00000000000..3285006e988
--- /dev/null
+++ b/drivers/leds/leds-lm3642.c
@@ -0,0 +1,462 @@
+/*
+* Simple driver for Texas Instruments LM3642 LED Flash driver chip
+* Copyright (C) 2012 Texas Instruments
+*
+* 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/i2c.h>
+#include <linux/leds.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/fs.h>
+#include <linux/regmap.h>
+#include <linux/workqueue.h>
+#include <linux/platform_data/leds-lm3642.h>
+
+#define	REG_FILT_TIME			(0x0)
+#define	REG_IVFM_MODE			(0x1)
+#define	REG_TORCH_TIME			(0x6)
+#define	REG_FLASH			(0x8)
+#define	REG_I_CTRL			(0x9)
+#define	REG_ENABLE			(0xA)
+#define	REG_FLAG			(0xB)
+#define	REG_MAX				(0xB)
+
+#define	UVLO_EN_SHIFT			(7)
+#define	IVM_D_TH_SHIFT			(2)
+#define	TORCH_RAMP_UP_TIME_SHIFT	(3)
+#define	TORCH_RAMP_DN_TIME_SHIFT	(0)
+#define	INDUCTOR_I_LIMIT_SHIFT		(6)
+#define	FLASH_RAMP_TIME_SHIFT		(3)
+#define	FLASH_TOUT_TIME_SHIFT		(0)
+#define	TORCH_I_SHIFT			(4)
+#define	FLASH_I_SHIFT			(0)
+#define	IVFM_SHIFT			(7)
+#define	TX_PIN_EN_SHIFT			(6)
+#define	STROBE_PIN_EN_SHIFT		(5)
+#define	TORCH_PIN_EN_SHIFT		(4)
+#define	MODE_BITS_SHIFT			(0)
+
+#define	UVLO_EN_MASK			(0x1)
+#define	IVM_D_TH_MASK			(0x7)
+#define	TORCH_RAMP_UP_TIME_MASK		(0x7)
+#define	TORCH_RAMP_DN_TIME_MASK		(0x7)
+#define	INDUCTOR_I_LIMIT_MASK		(0x1)
+#define	FLASH_RAMP_TIME_MASK		(0x7)
+#define	FLASH_TOUT_TIME_MASK		(0x7)
+#define	TORCH_I_MASK			(0x7)
+#define	FLASH_I_MASK			(0xF)
+#define	IVFM_MASK			(0x1)
+#define	TX_PIN_EN_MASK			(0x1)
+#define	STROBE_PIN_EN_MASK		(0x1)
+#define	TORCH_PIN_EN_MASK		(0x1)
+#define	MODE_BITS_MASK			(0x73)
+#define EX_PIN_CONTROL_MASK		(0x71)
+#define EX_PIN_ENABLE_MASK		(0x70)
+
+enum lm3642_mode {
+	MODES_STASNDBY = 0,
+	MODES_INDIC,
+	MODES_TORCH,
+	MODES_FLASH
+};
+
+struct lm3642_chip_data {
+	struct device *dev;
+
+	struct led_classdev cdev_flash;
+	struct led_classdev cdev_torch;
+	struct led_classdev cdev_indicator;
+
+	struct work_struct work_flash;
+	struct work_struct work_torch;
+	struct work_struct work_indicator;
+
+	u8 br_flash;
+	u8 br_torch;
+	u8 br_indicator;
+
+	enum lm3642_torch_pin_enable torch_pin;
+	enum lm3642_strobe_pin_enable strobe_pin;
+	enum lm3642_tx_pin_enable tx_pin;
+
+	struct lm3642_platform_data *pdata;
+	struct regmap *regmap;
+	struct mutex lock;
+
+	unsigned int last_flag;
+};
+
+/* chip initialize */
+static int __devinit lm3642_chip_init(struct lm3642_chip_data *chip)
+{
+	int ret;
+	struct lm3642_platform_data *pdata = chip->pdata;
+
+	/* set enable register */
+	ret = regmap_update_bits(chip->regmap, REG_ENABLE, EX_PIN_ENABLE_MASK,
+				 pdata->tx_pin);
+	if (ret < 0)
+		dev_err(chip->dev, "Failed to update REG_ENABLE Register\n");
+	return ret;
+}
+
+/* chip control */
+static int lm3642_control(struct lm3642_chip_data *chip,
+			  u8 brightness, enum lm3642_mode opmode)
+{
+	int ret;
+
+	ret = regmap_read(chip->regmap, REG_FLAG, &chip->last_flag);
+	if (ret < 0) {
+		dev_err(chip->dev, "Failed to read REG_FLAG Register\n");
+		goto out;
+	}
+
+	if (chip->last_flag)
+		dev_info(chip->dev, "Last FLAG is 0x%x\n", chip->last_flag);
+
+	/* brightness 0 means off-state */
+	if (!brightness)
+		opmode = MODES_STASNDBY;
+
+	switch (opmode) {
+	case MODES_TORCH:
+		ret = regmap_update_bits(chip->regmap, REG_I_CTRL,
+					 TORCH_I_MASK << TORCH_I_SHIFT,
+					 (brightness - 1) << TORCH_I_SHIFT);
+
+		if (chip->torch_pin)
+			opmode |= (TORCH_PIN_EN_MASK << TORCH_PIN_EN_SHIFT);
+		break;
+
+	case MODES_FLASH:
+		ret = regmap_update_bits(chip->regmap, REG_I_CTRL,
+					 FLASH_I_MASK << FLASH_I_SHIFT,
+					 (brightness - 1) << FLASH_I_SHIFT);
+
+		if (chip->strobe_pin)
+			opmode |= (STROBE_PIN_EN_MASK << STROBE_PIN_EN_SHIFT);
+		break;
+
+	case MODES_INDIC:
+		ret = regmap_update_bits(chip->regmap, REG_I_CTRL,
+					 TORCH_I_MASK << TORCH_I_SHIFT,
+					 (brightness - 1) << TORCH_I_SHIFT);
+		break;
+
+	case MODES_STASNDBY:
+
+		break;
+
+	default:
+		return ret;
+	}
+	if (ret < 0) {
+		dev_err(chip->dev, "Failed to write REG_I_CTRL Register\n");
+		goto out;
+	}
+
+	if (chip->tx_pin)
+		opmode |= (TX_PIN_EN_MASK << TX_PIN_EN_SHIFT);
+
+	ret = regmap_update_bits(chip->regmap, REG_ENABLE,
+				 MODE_BITS_MASK << MODE_BITS_SHIFT,
+				 opmode << MODE_BITS_SHIFT);
+out:
+	return ret;
+}
+
+/* torch */
+
+/* torch pin config for lm3642*/
+static ssize_t lm3642_torch_pin_store(struct device *dev,
+				      struct device_attribute *devAttr,
+				      const char *buf, size_t size)
+{
+	ssize_t ret;
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct lm3642_chip_data *chip =
+	    container_of(led_cdev, struct lm3642_chip_data, cdev_indicator);
+	unsigned int state;
+
+	ret = kstrtouint(buf, 10, &state);
+	if (ret)
+		goto out_strtoint;
+	if (state != 0)
+		state = 0x01 << TORCH_PIN_EN_SHIFT;
+
+	chip->torch_pin = state;
+	ret = regmap_update_bits(chip->regmap, REG_ENABLE,
+				 TORCH_PIN_EN_MASK << TORCH_PIN_EN_SHIFT,
+				 state);
+	if (ret < 0)
+		goto out;
+
+	return size;
+out:
+	dev_err(chip->dev, "%s:i2c access fail to register\n", __func__);
+	return size;
+out_strtoint:
+	dev_err(chip->dev, "%s: fail to change str to int\n", __func__);
+	return size;
+}
+
+static DEVICE_ATTR(torch_pin, 0666, NULL, lm3642_torch_pin_store);
+
+static void lm3642_deferred_torch_brightness_set(struct work_struct *work)
+{
+	struct lm3642_chip_data *chip =
+	    container_of(work, struct lm3642_chip_data, work_torch);
+
+	mutex_lock(&chip->lock);
+	lm3642_control(chip, chip->br_torch, MODES_TORCH);
+	mutex_unlock(&chip->lock);
+}
+
+static void lm3642_torch_brightness_set(struct led_classdev *cdev,
+					enum led_brightness brightness)
+{
+	struct lm3642_chip_data *chip =
+	    container_of(cdev, struct lm3642_chip_data, cdev_torch);
+
+	chip->br_torch = brightness;
+	schedule_work(&chip->work_torch);
+}
+
+/* flash */
+
+/* strobe pin config for lm3642*/
+static ssize_t lm3642_strobe_pin_store(struct device *dev,
+				       struct device_attribute *devAttr,
+				       const char *buf, size_t size)
+{
+	ssize_t ret;
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct lm3642_chip_data *chip =
+	    container_of(led_cdev, struct lm3642_chip_data, cdev_indicator);
+	unsigned int state;
+
+	ret = kstrtouint(buf, 10, &state);
+	if (ret)
+		goto out_strtoint;
+	if (state != 0)
+		state = 0x01 << STROBE_PIN_EN_SHIFT;
+
+	chip->strobe_pin = state;
+	ret = regmap_update_bits(chip->regmap, REG_ENABLE,
+				 STROBE_PIN_EN_MASK << STROBE_PIN_EN_SHIFT,
+				 state);
+	if (ret < 0)
+		goto out;
+
+	return size;
+out:
+	dev_err(chip->dev, "%s:i2c access fail to register\n", __func__);
+	return size;
+out_strtoint:
+	dev_err(chip->dev, "%s: fail to change str to int\n", __func__);
+	return size;
+}
+
+static DEVICE_ATTR(strobe_pin, 0666, NULL, lm3642_strobe_pin_store);
+
+static void lm3642_deferred_strobe_brightness_set(struct work_struct *work)
+{
+	struct lm3642_chip_data *chip =
+	    container_of(work, struct lm3642_chip_data, work_flash);
+
+	mutex_lock(&chip->lock);
+	lm3642_control(chip, chip->br_flash, MODES_FLASH);
+	mutex_unlock(&chip->lock);
+}
+
+static void lm3642_strobe_brightness_set(struct led_classdev *cdev,
+					 enum led_brightness brightness)
+{
+	struct lm3642_chip_data *chip =
+	    container_of(cdev, struct lm3642_chip_data, cdev_flash);
+
+	chip->br_flash = brightness;
+	schedule_work(&chip->work_flash);
+}
+
+/* indicator */
+static void lm3642_deferred_indicator_brightness_set(struct work_struct *work)
+{
+	struct lm3642_chip_data *chip =
+	    container_of(work, struct lm3642_chip_data, work_indicator);
+
+	mutex_lock(&chip->lock);
+	lm3642_control(chip, chip->br_indicator, MODES_INDIC);
+	mutex_unlock(&chip->lock);
+}
+
+static void lm3642_indicator_brightness_set(struct led_classdev *cdev,
+					    enum led_brightness brightness)
+{
+	struct lm3642_chip_data *chip =
+	    container_of(cdev, struct lm3642_chip_data, cdev_indicator);
+
+	chip->br_indicator = brightness;
+	schedule_work(&chip->work_indicator);
+}
+
+static const struct regmap_config lm3642_regmap = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = REG_MAX,
+};
+
+static int __devinit lm3642_probe(struct i2c_client *client,
+				  const struct i2c_device_id *id)
+{
+	struct lm3642_platform_data *pdata = client->dev.platform_data;
+	struct lm3642_chip_data *chip;
+
+	int err;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev, "i2c functionality check fail.\n");
+		return -EOPNOTSUPP;
+	}
+
+	if (pdata == NULL) {
+		dev_err(&client->dev, "needs Platform Data.\n");
+		return -ENODATA;
+	}
+
+	chip = devm_kzalloc(&client->dev,
+			    sizeof(struct lm3642_chip_data), GFP_KERNEL);
+	if (!chip)
+		return -ENOMEM;
+
+	chip->dev = &client->dev;
+	chip->pdata = pdata;
+
+	chip->tx_pin = pdata->tx_pin;
+	chip->torch_pin = pdata->torch_pin;
+	chip->strobe_pin = pdata->strobe_pin;
+
+	chip->regmap = devm_regmap_init_i2c(client, &lm3642_regmap);
+	if (IS_ERR(chip->regmap)) {
+		err = PTR_ERR(chip->regmap);
+		dev_err(&client->dev, "Failed to allocate register map: %d\n",
+			err);
+		return err;
+	}
+
+	mutex_init(&chip->lock);
+	i2c_set_clientdata(client, chip);
+
+	err = lm3642_chip_init(chip);
+	if (err < 0)
+		goto err_out;
+
+	/* flash */
+	INIT_WORK(&chip->work_flash, lm3642_deferred_strobe_brightness_set);
+	chip->cdev_flash.name = "flash";
+	chip->cdev_flash.max_brightness = 16;
+	chip->cdev_flash.brightness_set = lm3642_strobe_brightness_set;
+	err = led_classdev_register((struct device *)
+				    &client->dev, &chip->cdev_flash);
+	if (err < 0) {
+		dev_err(chip->dev, "failed to register flash\n");
+		goto err_out;
+	}
+	err = device_create_file(chip->cdev_flash.dev, &dev_attr_strobe_pin);
+	if (err < 0) {
+		dev_err(chip->dev, "failed to create strobe-pin file\n");
+		goto err_create_flash_pin_file;
+	}
+
+	/* torch */
+	INIT_WORK(&chip->work_torch, lm3642_deferred_torch_brightness_set);
+	chip->cdev_torch.name = "torch";
+	chip->cdev_torch.max_brightness = 8;
+	chip->cdev_torch.brightness_set = lm3642_torch_brightness_set;
+	err = led_classdev_register((struct device *)
+				    &client->dev, &chip->cdev_torch);
+	if (err < 0) {
+		dev_err(chip->dev, "failed to register torch\n");
+		goto err_create_torch_file;
+	}
+	err = device_create_file(chip->cdev_torch.dev, &dev_attr_torch_pin);
+	if (err < 0) {
+		dev_err(chip->dev, "failed to create torch-pin file\n");
+		goto err_create_torch_pin_file;
+	}
+
+	/* indicator */
+	INIT_WORK(&chip->work_indicator,
+		  lm3642_deferred_indicator_brightness_set);
+	chip->cdev_indicator.name = "indicator";
+	chip->cdev_indicator.max_brightness = 8;
+	chip->cdev_indicator.brightness_set = lm3642_indicator_brightness_set;
+	err = led_classdev_register((struct device *)
+				    &client->dev, &chip->cdev_indicator);
+	if (err < 0) {
+		dev_err(chip->dev, "failed to register indicator\n");
+		goto err_create_indicator_file;
+	}
+
+	dev_info(&client->dev, "LM3642 is initialized\n");
+	return 0;
+
+err_create_indicator_file:
+	device_remove_file(chip->cdev_torch.dev, &dev_attr_torch_pin);
+err_create_torch_pin_file:
+	led_classdev_unregister(&chip->cdev_torch);
+err_create_torch_file:
+	device_remove_file(chip->cdev_flash.dev, &dev_attr_strobe_pin);
+err_create_flash_pin_file:
+	led_classdev_unregister(&chip->cdev_flash);
+err_out:
+	return err;
+}
+
+static int __devexit lm3642_remove(struct i2c_client *client)
+{
+	struct lm3642_chip_data *chip = i2c_get_clientdata(client);
+
+	led_classdev_unregister(&chip->cdev_indicator);
+	flush_work(&chip->work_indicator);
+	device_remove_file(chip->cdev_torch.dev, &dev_attr_torch_pin);
+	led_classdev_unregister(&chip->cdev_torch);
+	flush_work(&chip->work_torch);
+	device_remove_file(chip->cdev_flash.dev, &dev_attr_strobe_pin);
+	led_classdev_unregister(&chip->cdev_flash);
+	flush_work(&chip->work_flash);
+	regmap_write(chip->regmap, REG_ENABLE, 0);
+	return 0;
+}
+
+static const struct i2c_device_id lm3642_id[] = {
+	{LM3642_NAME, 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, lm3642_id);
+
+static struct i2c_driver lm3642_i2c_driver = {
+	.driver = {
+		   .name = LM3642_NAME,
+		   .owner = THIS_MODULE,
+		   .pm = NULL,
+		   },
+	.probe = lm3642_probe,
+	.remove = __devexit_p(lm3642_remove),
+	.id_table = lm3642_id,
+};
+
+module_i2c_driver(lm3642_i2c_driver);
+
+MODULE_DESCRIPTION("Texas Instruments Flash Lighting driver for LM3642");
+MODULE_AUTHOR("Daniel Jeong <daniel.jeong@ti.com>");
+MODULE_AUTHOR("G.Shark Jeong <gshark.jeong@gmail.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/leds/leds-lp5523.c b/drivers/leds/leds-lp5523.c
index fbc12acada9..97994ffdc01 100644
--- a/drivers/leds/leds-lp5523.c
+++ b/drivers/leds/leds-lp5523.c
@@ -104,6 +104,11 @@
 #define LED_ACTIVE(mux, led)		(!!(mux & (0x0001 << led)))
 #define SHIFT_MASK(id)			(((id) - 1) * 2)
 
+enum lp5523_chip_id {
+	LP5523,
+	LP55231,
+};
+
 struct lp5523_engine {
 	int		id;
 	u8		mode;
@@ -150,7 +155,7 @@ static inline struct lp5523_chip *led_to_lp5523(struct lp5523_led *led)
 			    leds[led->id]);
 }
 
-static int lp5523_set_mode(struct lp5523_engine *engine, u8 mode);
+static void lp5523_set_mode(struct lp5523_engine *engine, u8 mode);
 static int lp5523_set_engine_mode(struct lp5523_engine *engine, u8 mode);
 static int lp5523_load_program(struct lp5523_engine *engine, const u8 *pattern);
 
@@ -177,7 +182,7 @@ static int lp5523_detect(struct i2c_client *client)
 	int ret;
 	u8 buf;
 
-	ret = lp5523_write(client, LP5523_REG_ENABLE, 0x40);
+	ret = lp5523_write(client, LP5523_REG_ENABLE, LP5523_ENABLE);
 	if (ret)
 		return ret;
 	ret = lp5523_read(client, LP5523_REG_ENABLE, &buf);
@@ -338,7 +343,8 @@ static int lp5523_mux_parse(const char *buf, u16 *mux, size_t len)
 {
 	int i;
 	u16 tmp_mux = 0;
-	len = len < LP5523_LEDS ? len : LP5523_LEDS;
+
+	len = min_t(int, len, LP5523_LEDS);
 	for (i = 0; i < len; i++) {
 		switch (buf[i]) {
 		case '1':
@@ -546,6 +552,9 @@ static int lp5523_do_store_load(struct lp5523_engine *engine,
 	unsigned cmd;
 	u8 pattern[LP5523_PROGRAM_LENGTH] = {0};
 
+	if (engine->mode != LP5523_CMD_LOAD)
+		return -EINVAL;
+
 	while ((offset < len - 1) && (i < LP5523_PROGRAM_LENGTH)) {
 		/* separate sscanfs because length is working only for %s */
 		ret = sscanf(buf + offset, "%2s%n ", c, &nrchars);
@@ -563,12 +572,7 @@ static int lp5523_do_store_load(struct lp5523_engine *engine,
 		goto fail;
 
 	mutex_lock(&chip->lock);
-
-	if (engine->mode == LP5523_CMD_LOAD)
-		ret = lp5523_load_program(engine, pattern);
-	else
-		ret = -EINVAL;
-
+	ret = lp5523_load_program(engine, pattern);
 	mutex_unlock(&chip->lock);
 
 	if (ret) {
@@ -755,6 +759,7 @@ static struct attribute *lp5523_attributes[] = {
 	&dev_attr_engine2_leds.attr,
 	&dev_attr_engine3_load.attr,
 	&dev_attr_engine3_leds.attr,
+	NULL,
 };
 
 static const struct attribute_group lp5523_group = {
@@ -789,26 +794,28 @@ static void lp5523_unregister_sysfs(struct i2c_client *client)
 /*--------------------------------------------------------------*/
 /*			Set chip operating mode			*/
 /*--------------------------------------------------------------*/
-static int lp5523_set_mode(struct lp5523_engine *engine, u8 mode)
+static void lp5523_set_mode(struct lp5523_engine *engine, u8 mode)
 {
-	int ret = 0;
-
 	/* if in that mode already do nothing, except for run */
 	if (mode == engine->mode && mode != LP5523_CMD_RUN)
-		return 0;
+		return;
 
-	if (mode == LP5523_CMD_RUN) {
-		ret = lp5523_run_program(engine);
-	} else if (mode == LP5523_CMD_LOAD) {
+	switch (mode) {
+	case LP5523_CMD_RUN:
+		lp5523_run_program(engine);
+		break;
+	case LP5523_CMD_LOAD:
 		lp5523_set_engine_mode(engine, LP5523_CMD_DISABLED);
 		lp5523_set_engine_mode(engine, LP5523_CMD_LOAD);
-	} else if (mode == LP5523_CMD_DISABLED) {
+		break;
+	case LP5523_CMD_DISABLED:
 		lp5523_set_engine_mode(engine, LP5523_CMD_DISABLED);
+		break;
+	default:
+		return;
 	}
 
 	engine->mode = mode;
-
-	return ret;
 }
 
 /*--------------------------------------------------------------*/
@@ -827,7 +834,8 @@ static int __init lp5523_init_engine(struct lp5523_engine *engine, int id)
 }
 
 static int __devinit lp5523_init_led(struct lp5523_led *led, struct device *dev,
-			   int chan, struct lp5523_platform_data *pdata)
+			   int chan, struct lp5523_platform_data *pdata,
+			   const char *chip_name)
 {
 	char name[32];
 	int res;
@@ -846,10 +854,14 @@ static int __devinit lp5523_init_led(struct lp5523_led *led, struct device *dev,
 			return -EINVAL;
 		}
 
-		snprintf(name, sizeof(name), "%s:channel%d",
-			pdata->label ?: "lp5523", chan);
+		if (pdata->led_config[chan].name) {
+			led->cdev.name = pdata->led_config[chan].name;
+		} else {
+			snprintf(name, sizeof(name), "%s:channel%d",
+				pdata->label ? : chip_name, chan);
+			led->cdev.name = name;
+		}
 
-		led->cdev.name = name;
 		led->cdev.brightness_set = lp5523_set_brightness;
 		res = led_classdev_register(dev, &led->cdev);
 		if (res < 0) {
@@ -917,7 +929,7 @@ static int __devinit lp5523_probe(struct i2c_client *client,
 	if (ret)
 		goto fail1;
 
-	dev_info(&client->dev, "LP5523 Programmable led chip found\n");
+	dev_info(&client->dev, "%s Programmable led chip found\n", id->name);
 
 	/* Initialize engines */
 	for (i = 0; i < ARRAY_SIZE(chip->engines); i++) {
@@ -945,7 +957,8 @@ static int __devinit lp5523_probe(struct i2c_client *client,
 		INIT_WORK(&chip->leds[led].brightness_work,
 			lp5523_led_brightness_work);
 
-		ret = lp5523_init_led(&chip->leds[led], &client->dev, i, pdata);
+		ret = lp5523_init_led(&chip->leds[led], &client->dev, i, pdata,
+				id->name);
 		if (ret) {
 			dev_err(&client->dev, "error initializing leds\n");
 			goto fail2;
@@ -970,7 +983,7 @@ static int __devinit lp5523_probe(struct i2c_client *client,
 fail2:
 	for (i = 0; i < chip->num_leds; i++) {
 		led_classdev_unregister(&chip->leds[i].cdev);
-		cancel_work_sync(&chip->leds[i].brightness_work);
+		flush_work(&chip->leds[i].brightness_work);
 	}
 fail1:
 	if (pdata->enable)
@@ -985,11 +998,14 @@ static int lp5523_remove(struct i2c_client *client)
 	struct lp5523_chip *chip = i2c_get_clientdata(client);
 	int i;
 
+	/* Disable engine mode */
+	lp5523_write(client, LP5523_REG_OP_MODE, LP5523_CMD_DISABLED);
+
 	lp5523_unregister_sysfs(client);
 
 	for (i = 0; i < chip->num_leds; i++) {
 		led_classdev_unregister(&chip->leds[i].cdev);
-		cancel_work_sync(&chip->leds[i].brightness_work);
+		flush_work(&chip->leds[i].brightness_work);
 	}
 
 	if (chip->pdata->enable)
@@ -1000,7 +1016,8 @@ static int lp5523_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id lp5523_id[] = {
-	{ "lp5523", 0 },
+	{ "lp5523",  LP5523 },
+	{ "lp55231", LP55231 },
 	{ }
 };
 
@@ -1008,7 +1025,7 @@ MODULE_DEVICE_TABLE(i2c, lp5523_id);
 
 static struct i2c_driver lp5523_driver = {
 	.driver = {
-		.name	= "lp5523",
+		.name	= "lp5523x",
 	},
 	.probe		= lp5523_probe,
 	.remove		= lp5523_remove,
diff --git a/drivers/leds/leds-pca9633.c b/drivers/leds/leds-pca9633.c
index edcd706c563..2f2f9c43535 100644
--- a/drivers/leds/leds-pca9633.c
+++ b/drivers/leds/leds-pca9633.c
@@ -22,6 +22,7 @@
 #include <linux/i2c.h>
 #include <linux/workqueue.h>
 #include <linux/slab.h>
+#include <linux/platform_data/leds-pca9633.h>
 
 /* LED select registers determine the source that drives LED outputs */
 #define PCA9633_LED_OFF		0x0	/* LED driver off */
@@ -96,13 +97,13 @@ static int __devinit pca9633_probe(struct i2c_client *client,
 					const struct i2c_device_id *id)
 {
 	struct pca9633_led *pca9633;
-	struct led_platform_data *pdata;
+	struct pca9633_platform_data *pdata;
 	int i, err;
 
 	pdata = client->dev.platform_data;
 
 	if (pdata) {
-		if (pdata->num_leds <= 0 || pdata->num_leds > 4) {
+		if (pdata->leds.num_leds <= 0 || pdata->leds.num_leds > 4) {
 			dev_err(&client->dev, "board info must claim at most 4 LEDs");
 			return -EINVAL;
 		}
@@ -119,14 +120,14 @@ static int __devinit pca9633_probe(struct i2c_client *client,
 		pca9633[i].led_num = i;
 
 		/* Platform data can specify LED names and default triggers */
-		if (pdata && i < pdata->num_leds) {
-			if (pdata->leds[i].name)
+		if (pdata && i < pdata->leds.num_leds) {
+			if (pdata->leds.leds[i].name)
 				snprintf(pca9633[i].name,
 					 sizeof(pca9633[i].name), "pca9633:%s",
-					 pdata->leds[i].name);
-			if (pdata->leds[i].default_trigger)
+					 pdata->leds.leds[i].name);
+			if (pdata->leds.leds[i].default_trigger)
 				pca9633[i].led_cdev.default_trigger =
-					pdata->leds[i].default_trigger;
+					pdata->leds.leds[i].default_trigger;
 		} else {
 			snprintf(pca9633[i].name, sizeof(pca9633[i].name),
 				 "pca9633:%d", i);
@@ -145,6 +146,10 @@ static int __devinit pca9633_probe(struct i2c_client *client,
 	/* Disable LED all-call address and set normal mode */
 	i2c_smbus_write_byte_data(client, PCA9633_MODE1, 0x00);
 
+	/* Configure output: open-drain or totem pole (push-pull) */
+	if (pdata && pdata->outdrv == PCA9633_OPEN_DRAIN)
+		i2c_smbus_write_byte_data(client, PCA9633_MODE2, 0x01);
+
 	/* Turn off LEDs */
 	i2c_smbus_write_byte_data(client, PCA9633_LEDOUT, 0x00);
 
diff --git a/drivers/leds/leds-wm8350.c b/drivers/leds/leds-wm8350.c
index 4c62113f7a7..88f23f84559 100644
--- a/drivers/leds/leds-wm8350.c
+++ b/drivers/leds/leds-wm8350.c
@@ -201,7 +201,7 @@ static int wm8350_led_probe(struct platform_device *pdev)
 	struct regulator *isink, *dcdc;
 	struct wm8350_led *led;
 	struct wm8350_led_platform_data *pdata = pdev->dev.platform_data;
-	int ret, i;
+	int i;
 
 	if (pdata == NULL) {
 		dev_err(&pdev->dev, "no platform data\n");
@@ -214,24 +214,21 @@ static int wm8350_led_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	isink = regulator_get(&pdev->dev, "led_isink");
+	isink = devm_regulator_get(&pdev->dev, "led_isink");
 	if (IS_ERR(isink)) {
 		printk(KERN_ERR "%s: can't get ISINK\n", __func__);
 		return PTR_ERR(isink);
 	}
 
-	dcdc = regulator_get(&pdev->dev, "led_vcc");
+	dcdc = devm_regulator_get(&pdev->dev, "led_vcc");
 	if (IS_ERR(dcdc)) {
 		printk(KERN_ERR "%s: can't get DCDC\n", __func__);
-		ret = PTR_ERR(dcdc);
-		goto err_isink;
+		return PTR_ERR(dcdc);
 	}
 
 	led = devm_kzalloc(&pdev->dev, sizeof(*led), GFP_KERNEL);
-	if (led == NULL) {
-		ret = -ENOMEM;
-		goto err_dcdc;
-	}
+	if (led == NULL)
+		return -ENOMEM;
 
 	led->cdev.brightness_set = wm8350_led_set;
 	led->cdev.default_trigger = pdata->default_trigger;
@@ -257,17 +254,7 @@ static int wm8350_led_probe(struct platform_device *pdev)
 	led->value = LED_OFF;
 	platform_set_drvdata(pdev, led);
 
-	ret = led_classdev_register(&pdev->dev, &led->cdev);
-	if (ret < 0)
-		goto err_dcdc;
-
-	return 0;
-
- err_dcdc:
-	regulator_put(dcdc);
- err_isink:
-	regulator_put(isink);
-	return ret;
+	return led_classdev_register(&pdev->dev, &led->cdev);
 }
 
 static int wm8350_led_remove(struct platform_device *pdev)
@@ -277,8 +264,6 @@ static int wm8350_led_remove(struct platform_device *pdev)
 	led_classdev_unregister(&led->cdev);
 	flush_work(&led->work);
 	wm8350_led_disable(led);
-	regulator_put(led->dcdc);
-	regulator_put(led->isink);
 	return 0;
 }
 
diff --git a/drivers/leds/leds.h b/drivers/leds/leds.h
index d02acd49612..4c50365344a 100644
--- a/drivers/leds/leds.h
+++ b/drivers/leds/leds.h
@@ -32,6 +32,8 @@ static inline int led_get_brightness(struct led_classdev *led_cdev)
 	return led_cdev->brightness;
 }
 
+void led_stop_software_blink(struct led_classdev *led_cdev);
+
 extern struct rw_semaphore leds_list_lock;
 extern struct list_head leds_list;
 
diff --git a/drivers/md/Kconfig b/drivers/md/Kconfig
index d949b781f6f..91a02eeeb31 100644
--- a/drivers/md/Kconfig
+++ b/drivers/md/Kconfig
@@ -216,6 +216,13 @@ config DM_BUFIO
 	 as a cache, holding recently-read blocks in memory and performing
 	 delayed writes.
 
+config DM_BIO_PRISON
+       tristate
+       depends on BLK_DEV_DM && EXPERIMENTAL
+       ---help---
+	 Some bio locking schemes used by other device-mapper targets
+	 including thin provisioning.
+
 source "drivers/md/persistent-data/Kconfig"
 
 config DM_CRYPT
@@ -247,6 +254,7 @@ config DM_THIN_PROVISIONING
        tristate "Thin provisioning target (EXPERIMENTAL)"
        depends on BLK_DEV_DM && EXPERIMENTAL
        select DM_PERSISTENT_DATA
+       select DM_BIO_PRISON
        ---help---
          Provides thin provisioning and snapshots that share a data store.
 
diff --git a/drivers/md/Makefile b/drivers/md/Makefile
index 8b2e0dffe82..94dce8b4932 100644
--- a/drivers/md/Makefile
+++ b/drivers/md/Makefile
@@ -29,6 +29,7 @@ obj-$(CONFIG_MD_FAULTY)		+= faulty.o
 obj-$(CONFIG_BLK_DEV_MD)	+= md-mod.o
 obj-$(CONFIG_BLK_DEV_DM)	+= dm-mod.o
 obj-$(CONFIG_DM_BUFIO)		+= dm-bufio.o
+obj-$(CONFIG_DM_BIO_PRISON)	+= dm-bio-prison.o
 obj-$(CONFIG_DM_CRYPT)		+= dm-crypt.o
 obj-$(CONFIG_DM_DELAY)		+= dm-delay.o
 obj-$(CONFIG_DM_FLAKEY)		+= dm-flakey.o
diff --git a/drivers/md/bitmap.c b/drivers/md/bitmap.c
index 94e7f6ba2e1..7155945f8eb 100644
--- a/drivers/md/bitmap.c
+++ b/drivers/md/bitmap.c
@@ -163,20 +163,17 @@ static struct md_rdev *next_active_rdev(struct md_rdev *rdev, struct mddev *mdde
 	 * As devices are only added or removed when raid_disk is < 0 and
 	 * nr_pending is 0 and In_sync is clear, the entries we return will
 	 * still be in the same position on the list when we re-enter
-	 * list_for_each_continue_rcu.
+	 * list_for_each_entry_continue_rcu.
 	 */
-	struct list_head *pos;
 	rcu_read_lock();
 	if (rdev == NULL)
 		/* start at the beginning */
-		pos = &mddev->disks;
+		rdev = list_entry_rcu(&mddev->disks, struct md_rdev, same_set);
 	else {
 		/* release the previous rdev and start from there. */
 		rdev_dec_pending(rdev, mddev);
-		pos = &rdev->same_set;
 	}
-	list_for_each_continue_rcu(pos, &mddev->disks) {
-		rdev = list_entry(pos, struct md_rdev, same_set);
+	list_for_each_entry_continue_rcu(rdev, &mddev->disks, same_set) {
 		if (rdev->raid_disk >= 0 &&
 		    !test_bit(Faulty, &rdev->flags)) {
 			/* this is a usable devices */
@@ -473,14 +470,10 @@ static int bitmap_new_disk_sb(struct bitmap *bitmap)
 {
 	bitmap_super_t *sb;
 	unsigned long chunksize, daemon_sleep, write_behind;
-	int err = -EINVAL;
 
 	bitmap->storage.sb_page = alloc_page(GFP_KERNEL);
-	if (IS_ERR(bitmap->storage.sb_page)) {
-		err = PTR_ERR(bitmap->storage.sb_page);
-		bitmap->storage.sb_page = NULL;
-		return err;
-	}
+	if (bitmap->storage.sb_page == NULL)
+		return -ENOMEM;
 	bitmap->storage.sb_page->index = 0;
 
 	sb = kmap_atomic(bitmap->storage.sb_page);
diff --git a/drivers/md/dm-bio-prison.c b/drivers/md/dm-bio-prison.c
new file mode 100644
index 00000000000..e4e84156745
--- /dev/null
+++ b/drivers/md/dm-bio-prison.c
@@ -0,0 +1,415 @@
+/*
+ * Copyright (C) 2012 Red Hat, Inc.
+ *
+ * This file is released under the GPL.
+ */
+
+#include "dm.h"
+#include "dm-bio-prison.h"
+
+#include <linux/spinlock.h>
+#include <linux/mempool.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+
+/*----------------------------------------------------------------*/
+
+struct dm_bio_prison_cell {
+	struct hlist_node list;
+	struct dm_bio_prison *prison;
+	struct dm_cell_key key;
+	struct bio *holder;
+	struct bio_list bios;
+};
+
+struct dm_bio_prison {
+	spinlock_t lock;
+	mempool_t *cell_pool;
+
+	unsigned nr_buckets;
+	unsigned hash_mask;
+	struct hlist_head *cells;
+};
+
+/*----------------------------------------------------------------*/
+
+static uint32_t calc_nr_buckets(unsigned nr_cells)
+{
+	uint32_t n = 128;
+
+	nr_cells /= 4;
+	nr_cells = min(nr_cells, 8192u);
+
+	while (n < nr_cells)
+		n <<= 1;
+
+	return n;
+}
+
+static struct kmem_cache *_cell_cache;
+
+/*
+ * @nr_cells should be the number of cells you want in use _concurrently_.
+ * Don't confuse it with the number of distinct keys.
+ */
+struct dm_bio_prison *dm_bio_prison_create(unsigned nr_cells)
+{
+	unsigned i;
+	uint32_t nr_buckets = calc_nr_buckets(nr_cells);
+	size_t len = sizeof(struct dm_bio_prison) +
+		(sizeof(struct hlist_head) * nr_buckets);
+	struct dm_bio_prison *prison = kmalloc(len, GFP_KERNEL);
+
+	if (!prison)
+		return NULL;
+
+	spin_lock_init(&prison->lock);
+	prison->cell_pool = mempool_create_slab_pool(nr_cells, _cell_cache);
+	if (!prison->cell_pool) {
+		kfree(prison);
+		return NULL;
+	}
+
+	prison->nr_buckets = nr_buckets;
+	prison->hash_mask = nr_buckets - 1;
+	prison->cells = (struct hlist_head *) (prison + 1);
+	for (i = 0; i < nr_buckets; i++)
+		INIT_HLIST_HEAD(prison->cells + i);
+
+	return prison;
+}
+EXPORT_SYMBOL_GPL(dm_bio_prison_create);
+
+void dm_bio_prison_destroy(struct dm_bio_prison *prison)
+{
+	mempool_destroy(prison->cell_pool);
+	kfree(prison);
+}
+EXPORT_SYMBOL_GPL(dm_bio_prison_destroy);
+
+static uint32_t hash_key(struct dm_bio_prison *prison, struct dm_cell_key *key)
+{
+	const unsigned long BIG_PRIME = 4294967291UL;
+	uint64_t hash = key->block * BIG_PRIME;
+
+	return (uint32_t) (hash & prison->hash_mask);
+}
+
+static int keys_equal(struct dm_cell_key *lhs, struct dm_cell_key *rhs)
+{
+	       return (lhs->virtual == rhs->virtual) &&
+		       (lhs->dev == rhs->dev) &&
+		       (lhs->block == rhs->block);
+}
+
+static struct dm_bio_prison_cell *__search_bucket(struct hlist_head *bucket,
+						  struct dm_cell_key *key)
+{
+	struct dm_bio_prison_cell *cell;
+	struct hlist_node *tmp;
+
+	hlist_for_each_entry(cell, tmp, bucket, list)
+		if (keys_equal(&cell->key, key))
+			return cell;
+
+	return NULL;
+}
+
+/*
+ * This may block if a new cell needs allocating.  You must ensure that
+ * cells will be unlocked even if the calling thread is blocked.
+ *
+ * Returns 1 if the cell was already held, 0 if @inmate is the new holder.
+ */
+int dm_bio_detain(struct dm_bio_prison *prison, struct dm_cell_key *key,
+		  struct bio *inmate, struct dm_bio_prison_cell **ref)
+{
+	int r = 1;
+	unsigned long flags;
+	uint32_t hash = hash_key(prison, key);
+	struct dm_bio_prison_cell *cell, *cell2;
+
+	BUG_ON(hash > prison->nr_buckets);
+
+	spin_lock_irqsave(&prison->lock, flags);
+
+	cell = __search_bucket(prison->cells + hash, key);
+	if (cell) {
+		bio_list_add(&cell->bios, inmate);
+		goto out;
+	}
+
+	/*
+	 * Allocate a new cell
+	 */
+	spin_unlock_irqrestore(&prison->lock, flags);
+	cell2 = mempool_alloc(prison->cell_pool, GFP_NOIO);
+	spin_lock_irqsave(&prison->lock, flags);
+
+	/*
+	 * We've been unlocked, so we have to double check that
+	 * nobody else has inserted this cell in the meantime.
+	 */
+	cell = __search_bucket(prison->cells + hash, key);
+	if (cell) {
+		mempool_free(cell2, prison->cell_pool);
+		bio_list_add(&cell->bios, inmate);
+		goto out;
+	}
+
+	/*
+	 * Use new cell.
+	 */
+	cell = cell2;
+
+	cell->prison = prison;
+	memcpy(&cell->key, key, sizeof(cell->key));
+	cell->holder = inmate;
+	bio_list_init(&cell->bios);
+	hlist_add_head(&cell->list, prison->cells + hash);
+
+	r = 0;
+
+out:
+	spin_unlock_irqrestore(&prison->lock, flags);
+
+	*ref = cell;
+
+	return r;
+}
+EXPORT_SYMBOL_GPL(dm_bio_detain);
+
+/*
+ * @inmates must have been initialised prior to this call
+ */
+static void __cell_release(struct dm_bio_prison_cell *cell, struct bio_list *inmates)
+{
+	struct dm_bio_prison *prison = cell->prison;
+
+	hlist_del(&cell->list);
+
+	if (inmates) {
+		bio_list_add(inmates, cell->holder);
+		bio_list_merge(inmates, &cell->bios);
+	}
+
+	mempool_free(cell, prison->cell_pool);
+}
+
+void dm_cell_release(struct dm_bio_prison_cell *cell, struct bio_list *bios)
+{
+	unsigned long flags;
+	struct dm_bio_prison *prison = cell->prison;
+
+	spin_lock_irqsave(&prison->lock, flags);
+	__cell_release(cell, bios);
+	spin_unlock_irqrestore(&prison->lock, flags);
+}
+EXPORT_SYMBOL_GPL(dm_cell_release);
+
+/*
+ * There are a couple of places where we put a bio into a cell briefly
+ * before taking it out again.  In these situations we know that no other
+ * bio may be in the cell.  This function releases the cell, and also does
+ * a sanity check.
+ */
+static void __cell_release_singleton(struct dm_bio_prison_cell *cell, struct bio *bio)
+{
+	BUG_ON(cell->holder != bio);
+	BUG_ON(!bio_list_empty(&cell->bios));
+
+	__cell_release(cell, NULL);
+}
+
+void dm_cell_release_singleton(struct dm_bio_prison_cell *cell, struct bio *bio)
+{
+	unsigned long flags;
+	struct dm_bio_prison *prison = cell->prison;
+
+	spin_lock_irqsave(&prison->lock, flags);
+	__cell_release_singleton(cell, bio);
+	spin_unlock_irqrestore(&prison->lock, flags);
+}
+EXPORT_SYMBOL_GPL(dm_cell_release_singleton);
+
+/*
+ * Sometimes we don't want the holder, just the additional bios.
+ */
+static void __cell_release_no_holder(struct dm_bio_prison_cell *cell, struct bio_list *inmates)
+{
+	struct dm_bio_prison *prison = cell->prison;
+
+	hlist_del(&cell->list);
+	bio_list_merge(inmates, &cell->bios);
+
+	mempool_free(cell, prison->cell_pool);
+}
+
+void dm_cell_release_no_holder(struct dm_bio_prison_cell *cell, struct bio_list *inmates)
+{
+	unsigned long flags;
+	struct dm_bio_prison *prison = cell->prison;
+
+	spin_lock_irqsave(&prison->lock, flags);
+	__cell_release_no_holder(cell, inmates);
+	spin_unlock_irqrestore(&prison->lock, flags);
+}
+EXPORT_SYMBOL_GPL(dm_cell_release_no_holder);
+
+void dm_cell_error(struct dm_bio_prison_cell *cell)
+{
+	struct dm_bio_prison *prison = cell->prison;
+	struct bio_list bios;
+	struct bio *bio;
+	unsigned long flags;
+
+	bio_list_init(&bios);
+
+	spin_lock_irqsave(&prison->lock, flags);
+	__cell_release(cell, &bios);
+	spin_unlock_irqrestore(&prison->lock, flags);
+
+	while ((bio = bio_list_pop(&bios)))
+		bio_io_error(bio);
+}
+EXPORT_SYMBOL_GPL(dm_cell_error);
+
+/*----------------------------------------------------------------*/
+
+#define DEFERRED_SET_SIZE 64
+
+struct dm_deferred_entry {
+	struct dm_deferred_set *ds;
+	unsigned count;
+	struct list_head work_items;
+};
+
+struct dm_deferred_set {
+	spinlock_t lock;
+	unsigned current_entry;
+	unsigned sweeper;
+	struct dm_deferred_entry entries[DEFERRED_SET_SIZE];
+};
+
+struct dm_deferred_set *dm_deferred_set_create(void)
+{
+	int i;
+	struct dm_deferred_set *ds;
+
+	ds = kmalloc(sizeof(*ds), GFP_KERNEL);
+	if (!ds)
+		return NULL;
+
+	spin_lock_init(&ds->lock);
+	ds->current_entry = 0;
+	ds->sweeper = 0;
+	for (i = 0; i < DEFERRED_SET_SIZE; i++) {
+		ds->entries[i].ds = ds;
+		ds->entries[i].count = 0;
+		INIT_LIST_HEAD(&ds->entries[i].work_items);
+	}
+
+	return ds;
+}
+EXPORT_SYMBOL_GPL(dm_deferred_set_create);
+
+void dm_deferred_set_destroy(struct dm_deferred_set *ds)
+{
+	kfree(ds);
+}
+EXPORT_SYMBOL_GPL(dm_deferred_set_destroy);
+
+struct dm_deferred_entry *dm_deferred_entry_inc(struct dm_deferred_set *ds)
+{
+	unsigned long flags;
+	struct dm_deferred_entry *entry;
+
+	spin_lock_irqsave(&ds->lock, flags);
+	entry = ds->entries + ds->current_entry;
+	entry->count++;
+	spin_unlock_irqrestore(&ds->lock, flags);
+
+	return entry;
+}
+EXPORT_SYMBOL_GPL(dm_deferred_entry_inc);
+
+static unsigned ds_next(unsigned index)
+{
+	return (index + 1) % DEFERRED_SET_SIZE;
+}
+
+static void __sweep(struct dm_deferred_set *ds, struct list_head *head)
+{
+	while ((ds->sweeper != ds->current_entry) &&
+	       !ds->entries[ds->sweeper].count) {
+		list_splice_init(&ds->entries[ds->sweeper].work_items, head);
+		ds->sweeper = ds_next(ds->sweeper);
+	}
+
+	if ((ds->sweeper == ds->current_entry) && !ds->entries[ds->sweeper].count)
+		list_splice_init(&ds->entries[ds->sweeper].work_items, head);
+}
+
+void dm_deferred_entry_dec(struct dm_deferred_entry *entry, struct list_head *head)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&entry->ds->lock, flags);
+	BUG_ON(!entry->count);
+	--entry->count;
+	__sweep(entry->ds, head);
+	spin_unlock_irqrestore(&entry->ds->lock, flags);
+}
+EXPORT_SYMBOL_GPL(dm_deferred_entry_dec);
+
+/*
+ * Returns 1 if deferred or 0 if no pending items to delay job.
+ */
+int dm_deferred_set_add_work(struct dm_deferred_set *ds, struct list_head *work)
+{
+	int r = 1;
+	unsigned long flags;
+	unsigned next_entry;
+
+	spin_lock_irqsave(&ds->lock, flags);
+	if ((ds->sweeper == ds->current_entry) &&
+	    !ds->entries[ds->current_entry].count)
+		r = 0;
+	else {
+		list_add(work, &ds->entries[ds->current_entry].work_items);
+		next_entry = ds_next(ds->current_entry);
+		if (!ds->entries[next_entry].count)
+			ds->current_entry = next_entry;
+	}
+	spin_unlock_irqrestore(&ds->lock, flags);
+
+	return r;
+}
+EXPORT_SYMBOL_GPL(dm_deferred_set_add_work);
+
+/*----------------------------------------------------------------*/
+
+static int __init dm_bio_prison_init(void)
+{
+	_cell_cache = KMEM_CACHE(dm_bio_prison_cell, 0);
+	if (!_cell_cache)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void __exit dm_bio_prison_exit(void)
+{
+	kmem_cache_destroy(_cell_cache);
+	_cell_cache = NULL;
+}
+
+/*
+ * module hooks
+ */
+module_init(dm_bio_prison_init);
+module_exit(dm_bio_prison_exit);
+
+MODULE_DESCRIPTION(DM_NAME " bio prison");
+MODULE_AUTHOR("Joe Thornber <dm-devel@redhat.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/md/dm-bio-prison.h b/drivers/md/dm-bio-prison.h
new file mode 100644
index 00000000000..4e0ac376700
--- /dev/null
+++ b/drivers/md/dm-bio-prison.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2011-2012 Red Hat, Inc.
+ *
+ * This file is released under the GPL.
+ */
+
+#ifndef DM_BIO_PRISON_H
+#define DM_BIO_PRISON_H
+
+#include "persistent-data/dm-block-manager.h" /* FIXME: for dm_block_t */
+#include "dm-thin-metadata.h" /* FIXME: for dm_thin_id */
+
+#include <linux/list.h>
+#include <linux/bio.h>
+
+/*----------------------------------------------------------------*/
+
+/*
+ * Sometimes we can't deal with a bio straight away.  We put them in prison
+ * where they can't cause any mischief.  Bios are put in a cell identified
+ * by a key, multiple bios can be in the same cell.  When the cell is
+ * subsequently unlocked the bios become available.
+ */
+struct dm_bio_prison;
+struct dm_bio_prison_cell;
+
+/* FIXME: this needs to be more abstract */
+struct dm_cell_key {
+	int virtual;
+	dm_thin_id dev;
+	dm_block_t block;
+};
+
+struct dm_bio_prison *dm_bio_prison_create(unsigned nr_cells);
+void dm_bio_prison_destroy(struct dm_bio_prison *prison);
+
+/*
+ * This may block if a new cell needs allocating.  You must ensure that
+ * cells will be unlocked even if the calling thread is blocked.
+ *
+ * Returns 1 if the cell was already held, 0 if @inmate is the new holder.
+ */
+int dm_bio_detain(struct dm_bio_prison *prison, struct dm_cell_key *key,
+		  struct bio *inmate, struct dm_bio_prison_cell **ref);
+
+void dm_cell_release(struct dm_bio_prison_cell *cell, struct bio_list *bios);
+void dm_cell_release_singleton(struct dm_bio_prison_cell *cell, struct bio *bio); // FIXME: bio arg not needed
+void dm_cell_release_no_holder(struct dm_bio_prison_cell *cell, struct bio_list *inmates);
+void dm_cell_error(struct dm_bio_prison_cell *cell);
+
+/*----------------------------------------------------------------*/
+
+/*
+ * We use the deferred set to keep track of pending reads to shared blocks.
+ * We do this to ensure the new mapping caused by a write isn't performed
+ * until these prior reads have completed.  Otherwise the insertion of the
+ * new mapping could free the old block that the read bios are mapped to.
+ */
+
+struct dm_deferred_set;
+struct dm_deferred_entry;
+
+struct dm_deferred_set *dm_deferred_set_create(void);
+void dm_deferred_set_destroy(struct dm_deferred_set *ds);
+
+struct dm_deferred_entry *dm_deferred_entry_inc(struct dm_deferred_set *ds);
+void dm_deferred_entry_dec(struct dm_deferred_entry *entry, struct list_head *head);
+int dm_deferred_set_add_work(struct dm_deferred_set *ds, struct list_head *work);
+
+/*----------------------------------------------------------------*/
+
+#endif
diff --git a/drivers/md/dm-bufio.c b/drivers/md/dm-bufio.c
index cc06a1e5242..651ca79881d 100644
--- a/drivers/md/dm-bufio.c
+++ b/drivers/md/dm-bufio.c
@@ -280,9 +280,7 @@ static void __cache_size_refresh(void)
 	BUG_ON(!mutex_is_locked(&dm_bufio_clients_lock));
 	BUG_ON(dm_bufio_client_count < 0);
 
-	dm_bufio_cache_size_latch = dm_bufio_cache_size;
-
-	barrier();
+	dm_bufio_cache_size_latch = ACCESS_ONCE(dm_bufio_cache_size);
 
 	/*
 	 * Use default if set to 0 and report the actual cache size used.
@@ -441,8 +439,7 @@ static void __relink_lru(struct dm_buffer *b, int dirty)
 	c->n_buffers[b->list_mode]--;
 	c->n_buffers[dirty]++;
 	b->list_mode = dirty;
-	list_del(&b->lru_list);
-	list_add(&b->lru_list, &c->lru[dirty]);
+	list_move(&b->lru_list, &c->lru[dirty]);
 }
 
 /*----------------------------------------------------------------
@@ -813,7 +810,7 @@ static void __get_memory_limit(struct dm_bufio_client *c,
 {
 	unsigned long buffers;
 
-	if (dm_bufio_cache_size != dm_bufio_cache_size_latch) {
+	if (ACCESS_ONCE(dm_bufio_cache_size) != dm_bufio_cache_size_latch) {
 		mutex_lock(&dm_bufio_clients_lock);
 		__cache_size_refresh();
 		mutex_unlock(&dm_bufio_clients_lock);
@@ -1591,11 +1588,9 @@ EXPORT_SYMBOL_GPL(dm_bufio_client_destroy);
 
 static void cleanup_old_buffers(void)
 {
-	unsigned long max_age = dm_bufio_max_age;
+	unsigned long max_age = ACCESS_ONCE(dm_bufio_max_age);
 	struct dm_bufio_client *c;
 
-	barrier();
-
 	if (max_age > ULONG_MAX / HZ)
 		max_age = ULONG_MAX / HZ;
 
diff --git a/drivers/md/dm-crypt.c b/drivers/md/dm-crypt.c
index 664743d6a6c..bbf459bca61 100644
--- a/drivers/md/dm-crypt.c
+++ b/drivers/md/dm-crypt.c
@@ -798,14 +798,6 @@ static int crypt_convert(struct crypt_config *cc,
 	return 0;
 }
 
-static void dm_crypt_bio_destructor(struct bio *bio)
-{
-	struct dm_crypt_io *io = bio->bi_private;
-	struct crypt_config *cc = io->cc;
-
-	bio_free(bio, cc->bs);
-}
-
 /*
  * Generate a new unfragmented bio with the given size
  * This should never violate the device limitations
@@ -974,7 +966,6 @@ static void clone_init(struct dm_crypt_io *io, struct bio *clone)
 	clone->bi_end_io  = crypt_endio;
 	clone->bi_bdev    = cc->dev->bdev;
 	clone->bi_rw      = io->base_bio->bi_rw;
-	clone->bi_destructor = dm_crypt_bio_destructor;
 }
 
 static int kcryptd_io_read(struct dm_crypt_io *io, gfp_t gfp)
@@ -988,19 +979,14 @@ static int kcryptd_io_read(struct dm_crypt_io *io, gfp_t gfp)
 	 * copy the required bvecs because we need the original
 	 * one in order to decrypt the whole bio data *afterwards*.
 	 */
-	clone = bio_alloc_bioset(gfp, bio_segments(base_bio), cc->bs);
+	clone = bio_clone_bioset(base_bio, gfp, cc->bs);
 	if (!clone)
 		return 1;
 
 	crypt_inc_pending(io);
 
 	clone_init(io, clone);
-	clone->bi_idx = 0;
-	clone->bi_vcnt = bio_segments(base_bio);
-	clone->bi_size = base_bio->bi_size;
 	clone->bi_sector = cc->start + io->sector;
-	memcpy(clone->bi_io_vec, bio_iovec(base_bio),
-	       sizeof(struct bio_vec) * clone->bi_vcnt);
 
 	generic_make_request(clone);
 	return 0;
diff --git a/drivers/md/dm-io.c b/drivers/md/dm-io.c
index ea5dd289fe2..1c46f97d666 100644
--- a/drivers/md/dm-io.c
+++ b/drivers/md/dm-io.c
@@ -249,16 +249,6 @@ static void vm_dp_init(struct dpages *dp, void *data)
 	dp->context_ptr = data;
 }
 
-static void dm_bio_destructor(struct bio *bio)
-{
-	unsigned region;
-	struct io *io;
-
-	retrieve_io_and_region_from_bio(bio, &io, &region);
-
-	bio_free(bio, io->client->bios);
-}
-
 /*
  * Functions for getting the pages from kernel memory.
  */
@@ -317,7 +307,6 @@ static void do_region(int rw, unsigned region, struct dm_io_region *where,
 		bio->bi_sector = where->sector + (where->count - remaining);
 		bio->bi_bdev = where->bdev;
 		bio->bi_end_io = endio;
-		bio->bi_destructor = dm_bio_destructor;
 		store_io_and_region_in_bio(bio, io, region);
 
 		if (rw & REQ_DISCARD) {
diff --git a/drivers/md/dm-mpath.c b/drivers/md/dm-mpath.c
index d778563a4ff..573bd04591b 100644
--- a/drivers/md/dm-mpath.c
+++ b/drivers/md/dm-mpath.c
@@ -1309,13 +1309,14 @@ static int multipath_end_io(struct dm_target *ti, struct request *clone,
 {
 	struct multipath *m = ti->private;
 	struct dm_mpath_io *mpio = map_context->ptr;
-	struct pgpath *pgpath = mpio->pgpath;
+	struct pgpath *pgpath;
 	struct path_selector *ps;
 	int r;
 
 	BUG_ON(!mpio);
 
 	r  = do_end_io(m, clone, error, mpio);
+	pgpath = mpio->pgpath;
 	if (pgpath) {
 		ps = &pgpath->pg->ps;
 		if (ps->type->end_io)
diff --git a/drivers/md/dm-raid.c b/drivers/md/dm-raid.c
index 982e3e390c4..45d94a7e7f6 100644
--- a/drivers/md/dm-raid.c
+++ b/drivers/md/dm-raid.c
@@ -338,6 +338,84 @@ static int validate_region_size(struct raid_set *rs, unsigned long region_size)
 }
 
 /*
+ * validate_rebuild_devices
+ * @rs
+ *
+ * Determine if the devices specified for rebuild can result in a valid
+ * usable array that is capable of rebuilding the given devices.
+ *
+ * Returns: 0 on success, -EINVAL on failure.
+ */
+static int validate_rebuild_devices(struct raid_set *rs)
+{
+	unsigned i, rebuild_cnt = 0;
+	unsigned rebuilds_per_group, copies, d;
+
+	if (!(rs->print_flags & DMPF_REBUILD))
+		return 0;
+
+	for (i = 0; i < rs->md.raid_disks; i++)
+		if (!test_bit(In_sync, &rs->dev[i].rdev.flags))
+			rebuild_cnt++;
+
+	switch (rs->raid_type->level) {
+	case 1:
+		if (rebuild_cnt >= rs->md.raid_disks)
+			goto too_many;
+		break;
+	case 4:
+	case 5:
+	case 6:
+		if (rebuild_cnt > rs->raid_type->parity_devs)
+			goto too_many;
+		break;
+	case 10:
+		copies = raid10_md_layout_to_copies(rs->md.layout);
+		if (rebuild_cnt < copies)
+			break;
+
+		/*
+		 * It is possible to have a higher rebuild count for RAID10,
+		 * as long as the failed devices occur in different mirror
+		 * groups (i.e. different stripes).
+		 *
+		 * Right now, we only allow for "near" copies.  When other
+		 * formats are added, we will have to check those too.
+		 *
+		 * When checking "near" format, make sure no adjacent devices
+		 * have failed beyond what can be handled.  In addition to the
+		 * simple case where the number of devices is a multiple of the
+		 * number of copies, we must also handle cases where the number
+		 * of devices is not a multiple of the number of copies.
+		 * E.g.    dev1 dev2 dev3 dev4 dev5
+		 *          A    A    B    B    C
+		 *          C    D    D    E    E
+		 */
+		rebuilds_per_group = 0;
+		for (i = 0; i < rs->md.raid_disks * copies; i++) {
+			d = i % rs->md.raid_disks;
+			if (!test_bit(In_sync, &rs->dev[d].rdev.flags) &&
+			    (++rebuilds_per_group >= copies))
+				goto too_many;
+			if (!((i + 1) % copies))
+				rebuilds_per_group = 0;
+		}
+		break;
+	default:
+		DMERR("The rebuild parameter is not supported for %s",
+		      rs->raid_type->name);
+		rs->ti->error = "Rebuild not supported for this RAID type";
+		return -EINVAL;
+	}
+
+	return 0;
+
+too_many:
+	rs->ti->error = "Too many rebuild devices specified";
+	return -EINVAL;
+}
+
+/*
  * Possible arguments are...
  *	<chunk_size> [optional_args]
  *
@@ -365,7 +443,7 @@ static int parse_raid_params(struct raid_set *rs, char **argv,
 {
 	char *raid10_format = "near";
 	unsigned raid10_copies = 2;
-	unsigned i, rebuild_cnt = 0;
+	unsigned i;
 	unsigned long value, region_size = 0;
 	sector_t sectors_per_dev = rs->ti->len;
 	sector_t max_io_len;
@@ -461,31 +539,7 @@ static int parse_raid_params(struct raid_set *rs, char **argv,
 
 		/* Parameters that take a numeric value are checked here */
 		if (!strcasecmp(key, "rebuild")) {
-			rebuild_cnt++;
-
-			switch (rs->raid_type->level) {
-			case 1:
-				if (rebuild_cnt >= rs->md.raid_disks) {
-					rs->ti->error = "Too many rebuild devices specified";
-					return -EINVAL;
-				}
-				break;
-			case 4:
-			case 5:
-			case 6:
-				if (rebuild_cnt > rs->raid_type->parity_devs) {
-					rs->ti->error = "Too many rebuild devices specified for given RAID type";
-					return -EINVAL;
-				}
-				break;
-			case 10:
-			default:
-				DMERR("The rebuild parameter is not supported for %s", rs->raid_type->name);
-				rs->ti->error = "Rebuild not supported for this RAID type";
-				return -EINVAL;
-			}
-
-			if (value > rs->md.raid_disks) {
+			if (value >= rs->md.raid_disks) {
 				rs->ti->error = "Invalid rebuild index given";
 				return -EINVAL;
 			}
@@ -608,6 +662,9 @@ static int parse_raid_params(struct raid_set *rs, char **argv,
 	}
 	rs->md.dev_sectors = sectors_per_dev;
 
+	if (validate_rebuild_devices(rs))
+		return -EINVAL;
+
 	/* Assume there are no metadata devices until the drives are parsed */
 	rs->md.persistent = 0;
 	rs->md.external = 1;
@@ -960,6 +1017,19 @@ static int analyse_superblocks(struct dm_target *ti, struct raid_set *rs)
 
 	freshest = NULL;
 	rdev_for_each_safe(rdev, tmp, mddev) {
+		/*
+		 * Skipping super_load due to DMPF_SYNC will cause
+		 * the array to undergo initialization again as
+		 * though it were new.  This is the intended effect
+		 * of the "sync" directive.
+		 *
+		 * When reshaping capability is added, we must ensure
+		 * that the "sync" directive is disallowed during the
+		 * reshape.
+		 */
+		if (rs->print_flags & DMPF_SYNC)
+			continue;
+
 		if (!rdev->meta_bdev)
 			continue;
 
@@ -1360,7 +1430,7 @@ static void raid_resume(struct dm_target *ti)
 
 static struct target_type raid_target = {
 	.name = "raid",
-	.version = {1, 3, 0},
+	.version = {1, 3, 1},
 	.module = THIS_MODULE,
 	.ctr = raid_ctr,
 	.dtr = raid_dtr,
diff --git a/drivers/md/dm-thin.c b/drivers/md/dm-thin.c
index c29410af1e2..058acf3a5ba 100644
--- a/drivers/md/dm-thin.c
+++ b/drivers/md/dm-thin.c
@@ -5,6 +5,7 @@
  */
 
 #include "dm-thin-metadata.h"
+#include "dm-bio-prison.h"
 #include "dm.h"
 
 #include <linux/device-mapper.h>
@@ -21,7 +22,6 @@
  * Tunable constants
  */
 #define ENDIO_HOOK_POOL_SIZE 1024
-#define DEFERRED_SET_SIZE 64
 #define MAPPING_POOL_SIZE 1024
 #define PRISON_CELLS 1024
 #define COMMIT_PERIOD HZ
@@ -58,7 +58,7 @@
  * i) plug io further to this physical block. (see bio_prison code).
  *
  * ii) quiesce any read io to that shared data block.  Obviously
- * including all devices that share this block.  (see deferred_set code)
+ * including all devices that share this block.  (see dm_deferred_set code)
  *
  * iii) copy the data block to a newly allocate block.  This step can be
  * missed out if the io covers the block. (schedule_copy).
@@ -99,381 +99,10 @@
 /*----------------------------------------------------------------*/
 
 /*
- * Sometimes we can't deal with a bio straight away.  We put them in prison
- * where they can't cause any mischief.  Bios are put in a cell identified
- * by a key, multiple bios can be in the same cell.  When the cell is
- * subsequently unlocked the bios become available.
- */
-struct bio_prison;
-
-struct cell_key {
-	int virtual;
-	dm_thin_id dev;
-	dm_block_t block;
-};
-
-struct dm_bio_prison_cell {
-	struct hlist_node list;
-	struct bio_prison *prison;
-	struct cell_key key;
-	struct bio *holder;
-	struct bio_list bios;
-};
-
-struct bio_prison {
-	spinlock_t lock;
-	mempool_t *cell_pool;
-
-	unsigned nr_buckets;
-	unsigned hash_mask;
-	struct hlist_head *cells;
-};
-
-static uint32_t calc_nr_buckets(unsigned nr_cells)
-{
-	uint32_t n = 128;
-
-	nr_cells /= 4;
-	nr_cells = min(nr_cells, 8192u);
-
-	while (n < nr_cells)
-		n <<= 1;
-
-	return n;
-}
-
-static struct kmem_cache *_cell_cache;
-
-/*
- * @nr_cells should be the number of cells you want in use _concurrently_.
- * Don't confuse it with the number of distinct keys.
- */
-static struct bio_prison *prison_create(unsigned nr_cells)
-{
-	unsigned i;
-	uint32_t nr_buckets = calc_nr_buckets(nr_cells);
-	size_t len = sizeof(struct bio_prison) +
-		(sizeof(struct hlist_head) * nr_buckets);
-	struct bio_prison *prison = kmalloc(len, GFP_KERNEL);
-
-	if (!prison)
-		return NULL;
-
-	spin_lock_init(&prison->lock);
-	prison->cell_pool = mempool_create_slab_pool(nr_cells, _cell_cache);
-	if (!prison->cell_pool) {
-		kfree(prison);
-		return NULL;
-	}
-
-	prison->nr_buckets = nr_buckets;
-	prison->hash_mask = nr_buckets - 1;
-	prison->cells = (struct hlist_head *) (prison + 1);
-	for (i = 0; i < nr_buckets; i++)
-		INIT_HLIST_HEAD(prison->cells + i);
-
-	return prison;
-}
-
-static void prison_destroy(struct bio_prison *prison)
-{
-	mempool_destroy(prison->cell_pool);
-	kfree(prison);
-}
-
-static uint32_t hash_key(struct bio_prison *prison, struct cell_key *key)
-{
-	const unsigned long BIG_PRIME = 4294967291UL;
-	uint64_t hash = key->block * BIG_PRIME;
-
-	return (uint32_t) (hash & prison->hash_mask);
-}
-
-static int keys_equal(struct cell_key *lhs, struct cell_key *rhs)
-{
-	       return (lhs->virtual == rhs->virtual) &&
-		       (lhs->dev == rhs->dev) &&
-		       (lhs->block == rhs->block);
-}
-
-static struct dm_bio_prison_cell *__search_bucket(struct hlist_head *bucket,
-						  struct cell_key *key)
-{
-	struct dm_bio_prison_cell *cell;
-	struct hlist_node *tmp;
-
-	hlist_for_each_entry(cell, tmp, bucket, list)
-		if (keys_equal(&cell->key, key))
-			return cell;
-
-	return NULL;
-}
-
-/*
- * This may block if a new cell needs allocating.  You must ensure that
- * cells will be unlocked even if the calling thread is blocked.
- *
- * Returns 1 if the cell was already held, 0 if @inmate is the new holder.
- */
-static int bio_detain(struct bio_prison *prison, struct cell_key *key,
-		      struct bio *inmate, struct dm_bio_prison_cell **ref)
-{
-	int r = 1;
-	unsigned long flags;
-	uint32_t hash = hash_key(prison, key);
-	struct dm_bio_prison_cell *cell, *cell2;
-
-	BUG_ON(hash > prison->nr_buckets);
-
-	spin_lock_irqsave(&prison->lock, flags);
-
-	cell = __search_bucket(prison->cells + hash, key);
-	if (cell) {
-		bio_list_add(&cell->bios, inmate);
-		goto out;
-	}
-
-	/*
-	 * Allocate a new cell
-	 */
-	spin_unlock_irqrestore(&prison->lock, flags);
-	cell2 = mempool_alloc(prison->cell_pool, GFP_NOIO);
-	spin_lock_irqsave(&prison->lock, flags);
-
-	/*
-	 * We've been unlocked, so we have to double check that
-	 * nobody else has inserted this cell in the meantime.
-	 */
-	cell = __search_bucket(prison->cells + hash, key);
-	if (cell) {
-		mempool_free(cell2, prison->cell_pool);
-		bio_list_add(&cell->bios, inmate);
-		goto out;
-	}
-
-	/*
-	 * Use new cell.
-	 */
-	cell = cell2;
-
-	cell->prison = prison;
-	memcpy(&cell->key, key, sizeof(cell->key));
-	cell->holder = inmate;
-	bio_list_init(&cell->bios);
-	hlist_add_head(&cell->list, prison->cells + hash);
-
-	r = 0;
-
-out:
-	spin_unlock_irqrestore(&prison->lock, flags);
-
-	*ref = cell;
-
-	return r;
-}
-
-/*
- * @inmates must have been initialised prior to this call
- */
-static void __cell_release(struct dm_bio_prison_cell *cell, struct bio_list *inmates)
-{
-	struct bio_prison *prison = cell->prison;
-
-	hlist_del(&cell->list);
-
-	if (inmates) {
-		bio_list_add(inmates, cell->holder);
-		bio_list_merge(inmates, &cell->bios);
-	}
-
-	mempool_free(cell, prison->cell_pool);
-}
-
-static void cell_release(struct dm_bio_prison_cell *cell, struct bio_list *bios)
-{
-	unsigned long flags;
-	struct bio_prison *prison = cell->prison;
-
-	spin_lock_irqsave(&prison->lock, flags);
-	__cell_release(cell, bios);
-	spin_unlock_irqrestore(&prison->lock, flags);
-}
-
-/*
- * There are a couple of places where we put a bio into a cell briefly
- * before taking it out again.  In these situations we know that no other
- * bio may be in the cell.  This function releases the cell, and also does
- * a sanity check.
- */
-static void __cell_release_singleton(struct dm_bio_prison_cell *cell, struct bio *bio)
-{
-	BUG_ON(cell->holder != bio);
-	BUG_ON(!bio_list_empty(&cell->bios));
-
-	__cell_release(cell, NULL);
-}
-
-static void cell_release_singleton(struct dm_bio_prison_cell *cell, struct bio *bio)
-{
-	unsigned long flags;
-	struct bio_prison *prison = cell->prison;
-
-	spin_lock_irqsave(&prison->lock, flags);
-	__cell_release_singleton(cell, bio);
-	spin_unlock_irqrestore(&prison->lock, flags);
-}
-
-/*
- * Sometimes we don't want the holder, just the additional bios.
- */
-static void __cell_release_no_holder(struct dm_bio_prison_cell *cell,
-				     struct bio_list *inmates)
-{
-	struct bio_prison *prison = cell->prison;
-
-	hlist_del(&cell->list);
-	bio_list_merge(inmates, &cell->bios);
-
-	mempool_free(cell, prison->cell_pool);
-}
-
-static void cell_release_no_holder(struct dm_bio_prison_cell *cell,
-				   struct bio_list *inmates)
-{
-	unsigned long flags;
-	struct bio_prison *prison = cell->prison;
-
-	spin_lock_irqsave(&prison->lock, flags);
-	__cell_release_no_holder(cell, inmates);
-	spin_unlock_irqrestore(&prison->lock, flags);
-}
-
-static void cell_error(struct dm_bio_prison_cell *cell)
-{
-	struct bio_prison *prison = cell->prison;
-	struct bio_list bios;
-	struct bio *bio;
-	unsigned long flags;
-
-	bio_list_init(&bios);
-
-	spin_lock_irqsave(&prison->lock, flags);
-	__cell_release(cell, &bios);
-	spin_unlock_irqrestore(&prison->lock, flags);
-
-	while ((bio = bio_list_pop(&bios)))
-		bio_io_error(bio);
-}
-
-/*----------------------------------------------------------------*/
-
-/*
- * We use the deferred set to keep track of pending reads to shared blocks.
- * We do this to ensure the new mapping caused by a write isn't performed
- * until these prior reads have completed.  Otherwise the insertion of the
- * new mapping could free the old block that the read bios are mapped to.
- */
-
-struct deferred_set;
-struct deferred_entry {
-	struct deferred_set *ds;
-	unsigned count;
-	struct list_head work_items;
-};
-
-struct deferred_set {
-	spinlock_t lock;
-	unsigned current_entry;
-	unsigned sweeper;
-	struct deferred_entry entries[DEFERRED_SET_SIZE];
-};
-
-static void ds_init(struct deferred_set *ds)
-{
-	int i;
-
-	spin_lock_init(&ds->lock);
-	ds->current_entry = 0;
-	ds->sweeper = 0;
-	for (i = 0; i < DEFERRED_SET_SIZE; i++) {
-		ds->entries[i].ds = ds;
-		ds->entries[i].count = 0;
-		INIT_LIST_HEAD(&ds->entries[i].work_items);
-	}
-}
-
-static struct deferred_entry *ds_inc(struct deferred_set *ds)
-{
-	unsigned long flags;
-	struct deferred_entry *entry;
-
-	spin_lock_irqsave(&ds->lock, flags);
-	entry = ds->entries + ds->current_entry;
-	entry->count++;
-	spin_unlock_irqrestore(&ds->lock, flags);
-
-	return entry;
-}
-
-static unsigned ds_next(unsigned index)
-{
-	return (index + 1) % DEFERRED_SET_SIZE;
-}
-
-static void __sweep(struct deferred_set *ds, struct list_head *head)
-{
-	while ((ds->sweeper != ds->current_entry) &&
-	       !ds->entries[ds->sweeper].count) {
-		list_splice_init(&ds->entries[ds->sweeper].work_items, head);
-		ds->sweeper = ds_next(ds->sweeper);
-	}
-
-	if ((ds->sweeper == ds->current_entry) && !ds->entries[ds->sweeper].count)
-		list_splice_init(&ds->entries[ds->sweeper].work_items, head);
-}
-
-static void ds_dec(struct deferred_entry *entry, struct list_head *head)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&entry->ds->lock, flags);
-	BUG_ON(!entry->count);
-	--entry->count;
-	__sweep(entry->ds, head);
-	spin_unlock_irqrestore(&entry->ds->lock, flags);
-}
-
-/*
- * Returns 1 if deferred or 0 if no pending items to delay job.
- */
-static int ds_add_work(struct deferred_set *ds, struct list_head *work)
-{
-	int r = 1;
-	unsigned long flags;
-	unsigned next_entry;
-
-	spin_lock_irqsave(&ds->lock, flags);
-	if ((ds->sweeper == ds->current_entry) &&
-	    !ds->entries[ds->current_entry].count)
-		r = 0;
-	else {
-		list_add(work, &ds->entries[ds->current_entry].work_items);
-		next_entry = ds_next(ds->current_entry);
-		if (!ds->entries[next_entry].count)
-			ds->current_entry = next_entry;
-	}
-	spin_unlock_irqrestore(&ds->lock, flags);
-
-	return r;
-}
-
-/*----------------------------------------------------------------*/
-
-/*
  * Key building.
  */
 static void build_data_key(struct dm_thin_device *td,
-			   dm_block_t b, struct cell_key *key)
+			   dm_block_t b, struct dm_cell_key *key)
 {
 	key->virtual = 0;
 	key->dev = dm_thin_dev_id(td);
@@ -481,7 +110,7 @@ static void build_data_key(struct dm_thin_device *td,
 }
 
 static void build_virtual_key(struct dm_thin_device *td, dm_block_t b,
-			      struct cell_key *key)
+			      struct dm_cell_key *key)
 {
 	key->virtual = 1;
 	key->dev = dm_thin_dev_id(td);
@@ -534,7 +163,7 @@ struct pool {
 	unsigned low_water_triggered:1;	/* A dm event has been sent */
 	unsigned no_free_space:1;	/* A -ENOSPC warning has been issued */
 
-	struct bio_prison *prison;
+	struct dm_bio_prison *prison;
 	struct dm_kcopyd_client *copier;
 
 	struct workqueue_struct *wq;
@@ -552,8 +181,8 @@ struct pool {
 
 	struct bio_list retry_on_resume_list;
 
-	struct deferred_set shared_read_ds;
-	struct deferred_set all_io_ds;
+	struct dm_deferred_set *shared_read_ds;
+	struct dm_deferred_set *all_io_ds;
 
 	struct dm_thin_new_mapping *next_mapping;
 	mempool_t *mapping_pool;
@@ -660,8 +289,8 @@ static struct pool *__pool_table_lookup_metadata_dev(struct block_device *md_dev
 
 struct dm_thin_endio_hook {
 	struct thin_c *tc;
-	struct deferred_entry *shared_read_entry;
-	struct deferred_entry *all_io_entry;
+	struct dm_deferred_entry *shared_read_entry;
+	struct dm_deferred_entry *all_io_entry;
 	struct dm_thin_new_mapping *overwrite_mapping;
 };
 
@@ -877,7 +506,7 @@ static void cell_defer(struct thin_c *tc, struct dm_bio_prison_cell *cell,
 	unsigned long flags;
 
 	spin_lock_irqsave(&pool->lock, flags);
-	cell_release(cell, &pool->deferred_bios);
+	dm_cell_release(cell, &pool->deferred_bios);
 	spin_unlock_irqrestore(&tc->pool->lock, flags);
 
 	wake_worker(pool);
@@ -896,7 +525,7 @@ static void cell_defer_except(struct thin_c *tc, struct dm_bio_prison_cell *cell
 	bio_list_init(&bios);
 
 	spin_lock_irqsave(&pool->lock, flags);
-	cell_release_no_holder(cell, &pool->deferred_bios);
+	dm_cell_release_no_holder(cell, &pool->deferred_bios);
 	spin_unlock_irqrestore(&pool->lock, flags);
 
 	wake_worker(pool);
@@ -906,7 +535,7 @@ static void process_prepared_mapping_fail(struct dm_thin_new_mapping *m)
 {
 	if (m->bio)
 		m->bio->bi_end_io = m->saved_bi_end_io;
-	cell_error(m->cell);
+	dm_cell_error(m->cell);
 	list_del(&m->list);
 	mempool_free(m, m->tc->pool->mapping_pool);
 }
@@ -921,7 +550,7 @@ static void process_prepared_mapping(struct dm_thin_new_mapping *m)
 		bio->bi_end_io = m->saved_bi_end_io;
 
 	if (m->err) {
-		cell_error(m->cell);
+		dm_cell_error(m->cell);
 		goto out;
 	}
 
@@ -933,7 +562,7 @@ static void process_prepared_mapping(struct dm_thin_new_mapping *m)
 	r = dm_thin_insert_block(tc->td, m->virt_block, m->data_block);
 	if (r) {
 		DMERR("dm_thin_insert_block() failed");
-		cell_error(m->cell);
+		dm_cell_error(m->cell);
 		goto out;
 	}
 
@@ -1067,7 +696,7 @@ static void schedule_copy(struct thin_c *tc, dm_block_t virt_block,
 	m->err = 0;
 	m->bio = NULL;
 
-	if (!ds_add_work(&pool->shared_read_ds, &m->list))
+	if (!dm_deferred_set_add_work(pool->shared_read_ds, &m->list))
 		m->quiesced = 1;
 
 	/*
@@ -1099,7 +728,7 @@ static void schedule_copy(struct thin_c *tc, dm_block_t virt_block,
 		if (r < 0) {
 			mempool_free(m, pool->mapping_pool);
 			DMERR("dm_kcopyd_copy() failed");
-			cell_error(cell);
+			dm_cell_error(cell);
 		}
 	}
 }
@@ -1164,7 +793,7 @@ static void schedule_zero(struct thin_c *tc, dm_block_t virt_block,
 		if (r < 0) {
 			mempool_free(m, pool->mapping_pool);
 			DMERR("dm_kcopyd_zero() failed");
-			cell_error(cell);
+			dm_cell_error(cell);
 		}
 	}
 }
@@ -1276,7 +905,7 @@ static void no_space(struct dm_bio_prison_cell *cell)
 	struct bio_list bios;
 
 	bio_list_init(&bios);
-	cell_release(cell, &bios);
+	dm_cell_release(cell, &bios);
 
 	while ((bio = bio_list_pop(&bios)))
 		retry_on_resume(bio);
@@ -1288,13 +917,13 @@ static void process_discard(struct thin_c *tc, struct bio *bio)
 	unsigned long flags;
 	struct pool *pool = tc->pool;
 	struct dm_bio_prison_cell *cell, *cell2;
-	struct cell_key key, key2;
+	struct dm_cell_key key, key2;
 	dm_block_t block = get_bio_block(tc, bio);
 	struct dm_thin_lookup_result lookup_result;
 	struct dm_thin_new_mapping *m;
 
 	build_virtual_key(tc->td, block, &key);
-	if (bio_detain(tc->pool->prison, &key, bio, &cell))
+	if (dm_bio_detain(tc->pool->prison, &key, bio, &cell))
 		return;
 
 	r = dm_thin_find_block(tc->td, block, 1, &lookup_result);
@@ -1306,8 +935,8 @@ static void process_discard(struct thin_c *tc, struct bio *bio)
 		 * on this block.
 		 */
 		build_data_key(tc->td, lookup_result.block, &key2);
-		if (bio_detain(tc->pool->prison, &key2, bio, &cell2)) {
-			cell_release_singleton(cell, bio);
+		if (dm_bio_detain(tc->pool->prison, &key2, bio, &cell2)) {
+			dm_cell_release_singleton(cell, bio);
 			break;
 		}
 
@@ -1326,7 +955,7 @@ static void process_discard(struct thin_c *tc, struct bio *bio)
 			m->err = 0;
 			m->bio = bio;
 
-			if (!ds_add_work(&pool->all_io_ds, &m->list)) {
+			if (!dm_deferred_set_add_work(pool->all_io_ds, &m->list)) {
 				spin_lock_irqsave(&pool->lock, flags);
 				list_add(&m->list, &pool->prepared_discards);
 				spin_unlock_irqrestore(&pool->lock, flags);
@@ -1338,8 +967,8 @@ static void process_discard(struct thin_c *tc, struct bio *bio)
 			 * a block boundary.  So we submit the discard of a
 			 * partial block appropriately.
 			 */
-			cell_release_singleton(cell, bio);
-			cell_release_singleton(cell2, bio);
+			dm_cell_release_singleton(cell, bio);
+			dm_cell_release_singleton(cell2, bio);
 			if ((!lookup_result.shared) && pool->pf.discard_passdown)
 				remap_and_issue(tc, bio, lookup_result.block);
 			else
@@ -1351,20 +980,20 @@ static void process_discard(struct thin_c *tc, struct bio *bio)
 		/*
 		 * It isn't provisioned, just forget it.
 		 */
-		cell_release_singleton(cell, bio);
+		dm_cell_release_singleton(cell, bio);
 		bio_endio(bio, 0);
 		break;
 
 	default:
 		DMERR("discard: find block unexpectedly returned %d", r);
-		cell_release_singleton(cell, bio);
+		dm_cell_release_singleton(cell, bio);
 		bio_io_error(bio);
 		break;
 	}
 }
 
 static void break_sharing(struct thin_c *tc, struct bio *bio, dm_block_t block,
-			  struct cell_key *key,
+			  struct dm_cell_key *key,
 			  struct dm_thin_lookup_result *lookup_result,
 			  struct dm_bio_prison_cell *cell)
 {
@@ -1384,7 +1013,7 @@ static void break_sharing(struct thin_c *tc, struct bio *bio, dm_block_t block,
 
 	default:
 		DMERR("%s: alloc_data_block() failed, error = %d", __func__, r);
-		cell_error(cell);
+		dm_cell_error(cell);
 		break;
 	}
 }
@@ -1395,14 +1024,14 @@ static void process_shared_bio(struct thin_c *tc, struct bio *bio,
 {
 	struct dm_bio_prison_cell *cell;
 	struct pool *pool = tc->pool;
-	struct cell_key key;
+	struct dm_cell_key key;
 
 	/*
 	 * If cell is already occupied, then sharing is already in the process
 	 * of being broken so we have nothing further to do here.
 	 */
 	build_data_key(tc->td, lookup_result->block, &key);
-	if (bio_detain(pool->prison, &key, bio, &cell))
+	if (dm_bio_detain(pool->prison, &key, bio, &cell))
 		return;
 
 	if (bio_data_dir(bio) == WRITE && bio->bi_size)
@@ -1410,9 +1039,9 @@ static void process_shared_bio(struct thin_c *tc, struct bio *bio,
 	else {
 		struct dm_thin_endio_hook *h = dm_get_mapinfo(bio)->ptr;
 
-		h->shared_read_entry = ds_inc(&pool->shared_read_ds);
+		h->shared_read_entry = dm_deferred_entry_inc(pool->shared_read_ds);
 
-		cell_release_singleton(cell, bio);
+		dm_cell_release_singleton(cell, bio);
 		remap_and_issue(tc, bio, lookup_result->block);
 	}
 }
@@ -1427,7 +1056,7 @@ static void provision_block(struct thin_c *tc, struct bio *bio, dm_block_t block
 	 * Remap empty bios (flushes) immediately, without provisioning.
 	 */
 	if (!bio->bi_size) {
-		cell_release_singleton(cell, bio);
+		dm_cell_release_singleton(cell, bio);
 		remap_and_issue(tc, bio, 0);
 		return;
 	}
@@ -1437,7 +1066,7 @@ static void provision_block(struct thin_c *tc, struct bio *bio, dm_block_t block
 	 */
 	if (bio_data_dir(bio) == READ) {
 		zero_fill_bio(bio);
-		cell_release_singleton(cell, bio);
+		dm_cell_release_singleton(cell, bio);
 		bio_endio(bio, 0);
 		return;
 	}
@@ -1458,7 +1087,7 @@ static void provision_block(struct thin_c *tc, struct bio *bio, dm_block_t block
 	default:
 		DMERR("%s: alloc_data_block() failed, error = %d", __func__, r);
 		set_pool_mode(tc->pool, PM_READ_ONLY);
-		cell_error(cell);
+		dm_cell_error(cell);
 		break;
 	}
 }
@@ -1468,7 +1097,7 @@ static void process_bio(struct thin_c *tc, struct bio *bio)
 	int r;
 	dm_block_t block = get_bio_block(tc, bio);
 	struct dm_bio_prison_cell *cell;
-	struct cell_key key;
+	struct dm_cell_key key;
 	struct dm_thin_lookup_result lookup_result;
 
 	/*
@@ -1476,7 +1105,7 @@ static void process_bio(struct thin_c *tc, struct bio *bio)
 	 * being provisioned so we have nothing further to do here.
 	 */
 	build_virtual_key(tc->td, block, &key);
-	if (bio_detain(tc->pool->prison, &key, bio, &cell))
+	if (dm_bio_detain(tc->pool->prison, &key, bio, &cell))
 		return;
 
 	r = dm_thin_find_block(tc->td, block, 1, &lookup_result);
@@ -1491,7 +1120,7 @@ static void process_bio(struct thin_c *tc, struct bio *bio)
 		 * TODO: this will probably have to change when discard goes
 		 * back in.
 		 */
-		cell_release_singleton(cell, bio);
+		dm_cell_release_singleton(cell, bio);
 
 		if (lookup_result.shared)
 			process_shared_bio(tc, bio, block, &lookup_result);
@@ -1501,7 +1130,7 @@ static void process_bio(struct thin_c *tc, struct bio *bio)
 
 	case -ENODATA:
 		if (bio_data_dir(bio) == READ && tc->origin_dev) {
-			cell_release_singleton(cell, bio);
+			dm_cell_release_singleton(cell, bio);
 			remap_to_origin_and_issue(tc, bio);
 		} else
 			provision_block(tc, bio, block, cell);
@@ -1509,7 +1138,7 @@ static void process_bio(struct thin_c *tc, struct bio *bio)
 
 	default:
 		DMERR("dm_thin_find_block() failed, error = %d", r);
-		cell_release_singleton(cell, bio);
+		dm_cell_release_singleton(cell, bio);
 		bio_io_error(bio);
 		break;
 	}
@@ -1718,7 +1347,7 @@ static struct dm_thin_endio_hook *thin_hook_bio(struct thin_c *tc, struct bio *b
 
 	h->tc = tc;
 	h->shared_read_entry = NULL;
-	h->all_io_entry = bio->bi_rw & REQ_DISCARD ? NULL : ds_inc(&pool->all_io_ds);
+	h->all_io_entry = bio->bi_rw & REQ_DISCARD ? NULL : dm_deferred_entry_inc(pool->all_io_ds);
 	h->overwrite_mapping = NULL;
 
 	return h;
@@ -1928,7 +1557,7 @@ static void __pool_destroy(struct pool *pool)
 	if (dm_pool_metadata_close(pool->pmd) < 0)
 		DMWARN("%s: dm_pool_metadata_close() failed.", __func__);
 
-	prison_destroy(pool->prison);
+	dm_bio_prison_destroy(pool->prison);
 	dm_kcopyd_client_destroy(pool->copier);
 
 	if (pool->wq)
@@ -1938,6 +1567,8 @@ static void __pool_destroy(struct pool *pool)
 		mempool_free(pool->next_mapping, pool->mapping_pool);
 	mempool_destroy(pool->mapping_pool);
 	mempool_destroy(pool->endio_hook_pool);
+	dm_deferred_set_destroy(pool->shared_read_ds);
+	dm_deferred_set_destroy(pool->all_io_ds);
 	kfree(pool);
 }
 
@@ -1976,7 +1607,7 @@ static struct pool *pool_create(struct mapped_device *pool_md,
 		pool->sectors_per_block_shift = __ffs(block_size);
 	pool->low_water_blocks = 0;
 	pool_features_init(&pool->pf);
-	pool->prison = prison_create(PRISON_CELLS);
+	pool->prison = dm_bio_prison_create(PRISON_CELLS);
 	if (!pool->prison) {
 		*error = "Error creating pool's bio prison";
 		err_p = ERR_PTR(-ENOMEM);
@@ -2012,8 +1643,20 @@ static struct pool *pool_create(struct mapped_device *pool_md,
 	pool->low_water_triggered = 0;
 	pool->no_free_space = 0;
 	bio_list_init(&pool->retry_on_resume_list);
-	ds_init(&pool->shared_read_ds);
-	ds_init(&pool->all_io_ds);
+
+	pool->shared_read_ds = dm_deferred_set_create();
+	if (!pool->shared_read_ds) {
+		*error = "Error creating pool's shared read deferred set";
+		err_p = ERR_PTR(-ENOMEM);
+		goto bad_shared_read_ds;
+	}
+
+	pool->all_io_ds = dm_deferred_set_create();
+	if (!pool->all_io_ds) {
+		*error = "Error creating pool's all io deferred set";
+		err_p = ERR_PTR(-ENOMEM);
+		goto bad_all_io_ds;
+	}
 
 	pool->next_mapping = NULL;
 	pool->mapping_pool = mempool_create_slab_pool(MAPPING_POOL_SIZE,
@@ -2042,11 +1685,15 @@ static struct pool *pool_create(struct mapped_device *pool_md,
 bad_endio_hook_pool:
 	mempool_destroy(pool->mapping_pool);
 bad_mapping_pool:
+	dm_deferred_set_destroy(pool->all_io_ds);
+bad_all_io_ds:
+	dm_deferred_set_destroy(pool->shared_read_ds);
+bad_shared_read_ds:
 	destroy_workqueue(pool->wq);
 bad_wq:
 	dm_kcopyd_client_destroy(pool->copier);
 bad_kcopyd_client:
-	prison_destroy(pool->prison);
+	dm_bio_prison_destroy(pool->prison);
 bad_prison:
 	kfree(pool);
 bad_pool:
@@ -2272,15 +1919,6 @@ static int pool_ctr(struct dm_target *ti, unsigned argc, char **argv)
 		goto out_flags_changed;
 	}
 
-	/*
-	 * The block layer requires discard_granularity to be a power of 2.
-	 */
-	if (pf.discard_enabled && !is_power_of_2(block_size)) {
-		ti->error = "Discard support must be disabled when the block size is not a power of 2";
-		r = -EINVAL;
-		goto out_flags_changed;
-	}
-
 	pt->pool = pool;
 	pt->ti = ti;
 	pt->metadata_dev = metadata_dev;
@@ -2762,6 +2400,11 @@ static int pool_merge(struct dm_target *ti, struct bvec_merge_data *bvm,
 	return min(max_size, q->merge_bvec_fn(q, bvm, biovec));
 }
 
+static bool block_size_is_power_of_two(struct pool *pool)
+{
+	return pool->sectors_per_block_shift >= 0;
+}
+
 static void set_discard_limits(struct pool_c *pt, struct queue_limits *limits)
 {
 	struct pool *pool = pt->pool;
@@ -2775,8 +2418,15 @@ static void set_discard_limits(struct pool_c *pt, struct queue_limits *limits)
 	if (pt->adjusted_pf.discard_passdown) {
 		data_limits = &bdev_get_queue(pt->data_dev->bdev)->limits;
 		limits->discard_granularity = data_limits->discard_granularity;
-	} else
+	} else if (block_size_is_power_of_two(pool))
 		limits->discard_granularity = pool->sectors_per_block << SECTOR_SHIFT;
+	else
+		/*
+		 * Use largest power of 2 that is a factor of sectors_per_block
+		 * but at least DATA_DEV_BLOCK_SIZE_MIN_SECTORS.
+		 */
+		limits->discard_granularity = max(1 << (ffs(pool->sectors_per_block) - 1),
+						  DATA_DEV_BLOCK_SIZE_MIN_SECTORS) << SECTOR_SHIFT;
 }
 
 static void pool_io_hints(struct dm_target *ti, struct queue_limits *limits)
@@ -2804,7 +2454,7 @@ static struct target_type pool_target = {
 	.name = "thin-pool",
 	.features = DM_TARGET_SINGLETON | DM_TARGET_ALWAYS_WRITEABLE |
 		    DM_TARGET_IMMUTABLE,
-	.version = {1, 4, 0},
+	.version = {1, 5, 0},
 	.module = THIS_MODULE,
 	.ctr = pool_ctr,
 	.dtr = pool_dtr,
@@ -2979,7 +2629,7 @@ static int thin_endio(struct dm_target *ti,
 
 	if (h->shared_read_entry) {
 		INIT_LIST_HEAD(&work);
-		ds_dec(h->shared_read_entry, &work);
+		dm_deferred_entry_dec(h->shared_read_entry, &work);
 
 		spin_lock_irqsave(&pool->lock, flags);
 		list_for_each_entry_safe(m, tmp, &work, list) {
@@ -2992,7 +2642,7 @@ static int thin_endio(struct dm_target *ti,
 
 	if (h->all_io_entry) {
 		INIT_LIST_HEAD(&work);
-		ds_dec(h->all_io_entry, &work);
+		dm_deferred_entry_dec(h->all_io_entry, &work);
 		spin_lock_irqsave(&pool->lock, flags);
 		list_for_each_entry_safe(m, tmp, &work, list)
 			list_add(&m->list, &pool->prepared_discards);
@@ -3095,7 +2745,7 @@ static void thin_io_hints(struct dm_target *ti, struct queue_limits *limits)
 
 static struct target_type thin_target = {
 	.name = "thin",
-	.version = {1, 4, 0},
+	.version = {1, 5, 0},
 	.module	= THIS_MODULE,
 	.ctr = thin_ctr,
 	.dtr = thin_dtr,
@@ -3125,10 +2775,6 @@ static int __init dm_thin_init(void)
 
 	r = -ENOMEM;
 
-	_cell_cache = KMEM_CACHE(dm_bio_prison_cell, 0);
-	if (!_cell_cache)
-		goto bad_cell_cache;
-
 	_new_mapping_cache = KMEM_CACHE(dm_thin_new_mapping, 0);
 	if (!_new_mapping_cache)
 		goto bad_new_mapping_cache;
@@ -3142,8 +2788,6 @@ static int __init dm_thin_init(void)
 bad_endio_hook_cache:
 	kmem_cache_destroy(_new_mapping_cache);
 bad_new_mapping_cache:
-	kmem_cache_destroy(_cell_cache);
-bad_cell_cache:
 	dm_unregister_target(&pool_target);
 bad_pool_target:
 	dm_unregister_target(&thin_target);
@@ -3156,7 +2800,6 @@ static void dm_thin_exit(void)
 	dm_unregister_target(&thin_target);
 	dm_unregister_target(&pool_target);
 
-	kmem_cache_destroy(_cell_cache);
 	kmem_cache_destroy(_new_mapping_cache);
 	kmem_cache_destroy(_endio_hook_cache);
 }
diff --git a/drivers/md/dm-verity.c b/drivers/md/dm-verity.c
index 892ae2766aa..9e7328bb403 100644
--- a/drivers/md/dm-verity.c
+++ b/drivers/md/dm-verity.c
@@ -438,7 +438,7 @@ static void verity_prefetch_io(struct dm_verity *v, struct dm_verity_io *io)
 		verity_hash_at_level(v, io->block, i, &hash_block_start, NULL);
 		verity_hash_at_level(v, io->block + io->n_blocks - 1, i, &hash_block_end, NULL);
 		if (!i) {
-			unsigned cluster = *(volatile unsigned *)&dm_verity_prefetch_cluster;
+			unsigned cluster = ACCESS_ONCE(dm_verity_prefetch_cluster);
 
 			cluster >>= v->data_dev_block_bits;
 			if (unlikely(!cluster))
diff --git a/drivers/md/dm.c b/drivers/md/dm.c
index 67ffa391edc..02db9183ca0 100644
--- a/drivers/md/dm.c
+++ b/drivers/md/dm.c
@@ -71,6 +71,7 @@ struct dm_target_io {
 	struct dm_io *io;
 	struct dm_target *ti;
 	union map_info info;
+	struct bio clone;
 };
 
 /*
@@ -86,12 +87,17 @@ struct dm_rq_target_io {
 };
 
 /*
- * For request-based dm.
- * One of these is allocated per bio.
+ * For request-based dm - the bio clones we allocate are embedded in these
+ * structs.
+ *
+ * We allocate these with bio_alloc_bioset, using the front_pad parameter when
+ * the bioset is created - this means the bio has to come at the end of the
+ * struct.
  */
 struct dm_rq_clone_bio_info {
 	struct bio *orig;
 	struct dm_rq_target_io *tio;
+	struct bio clone;
 };
 
 union map_info *dm_get_mapinfo(struct bio *bio)
@@ -209,8 +215,12 @@ struct dm_md_mempools {
 
 #define MIN_IOS 256
 static struct kmem_cache *_io_cache;
-static struct kmem_cache *_tio_cache;
 static struct kmem_cache *_rq_tio_cache;
+
+/*
+ * Unused now, and needs to be deleted. But since io_pool is overloaded and it's
+ * still used for _io_cache, I'm leaving this for a later cleanup
+ */
 static struct kmem_cache *_rq_bio_info_cache;
 
 static int __init local_init(void)
@@ -222,14 +232,9 @@ static int __init local_init(void)
 	if (!_io_cache)
 		return r;
 
-	/* allocate a slab for the target ios */
-	_tio_cache = KMEM_CACHE(dm_target_io, 0);
-	if (!_tio_cache)
-		goto out_free_io_cache;
-
 	_rq_tio_cache = KMEM_CACHE(dm_rq_target_io, 0);
 	if (!_rq_tio_cache)
-		goto out_free_tio_cache;
+		goto out_free_io_cache;
 
 	_rq_bio_info_cache = KMEM_CACHE(dm_rq_clone_bio_info, 0);
 	if (!_rq_bio_info_cache)
@@ -255,8 +260,6 @@ out_free_rq_bio_info_cache:
 	kmem_cache_destroy(_rq_bio_info_cache);
 out_free_rq_tio_cache:
 	kmem_cache_destroy(_rq_tio_cache);
-out_free_tio_cache:
-	kmem_cache_destroy(_tio_cache);
 out_free_io_cache:
 	kmem_cache_destroy(_io_cache);
 
@@ -267,7 +270,6 @@ static void local_exit(void)
 {
 	kmem_cache_destroy(_rq_bio_info_cache);
 	kmem_cache_destroy(_rq_tio_cache);
-	kmem_cache_destroy(_tio_cache);
 	kmem_cache_destroy(_io_cache);
 	unregister_blkdev(_major, _name);
 	dm_uevent_exit();
@@ -453,7 +455,7 @@ static void free_io(struct mapped_device *md, struct dm_io *io)
 
 static void free_tio(struct mapped_device *md, struct dm_target_io *tio)
 {
-	mempool_free(tio, md->tio_pool);
+	bio_put(&tio->clone);
 }
 
 static struct dm_rq_target_io *alloc_rq_tio(struct mapped_device *md,
@@ -467,16 +469,6 @@ static void free_rq_tio(struct dm_rq_target_io *tio)
 	mempool_free(tio, tio->md->tio_pool);
 }
 
-static struct dm_rq_clone_bio_info *alloc_bio_info(struct mapped_device *md)
-{
-	return mempool_alloc(md->io_pool, GFP_ATOMIC);
-}
-
-static void free_bio_info(struct dm_rq_clone_bio_info *info)
-{
-	mempool_free(info, info->tio->md->io_pool);
-}
-
 static int md_in_flight(struct mapped_device *md)
 {
 	return atomic_read(&md->pending[READ]) +
@@ -681,13 +673,7 @@ static void clone_endio(struct bio *bio, int error)
 		}
 	}
 
-	/*
-	 * Store md for cleanup instead of tio which is about to get freed.
-	 */
-	bio->bi_private = md->bs;
-
 	free_tio(md, tio);
-	bio_put(bio);
 	dec_pending(io, error);
 }
 
@@ -1007,12 +993,12 @@ int dm_set_target_max_io_len(struct dm_target *ti, sector_t len)
 }
 EXPORT_SYMBOL_GPL(dm_set_target_max_io_len);
 
-static void __map_bio(struct dm_target *ti, struct bio *clone,
-		      struct dm_target_io *tio)
+static void __map_bio(struct dm_target *ti, struct dm_target_io *tio)
 {
 	int r;
 	sector_t sector;
 	struct mapped_device *md;
+	struct bio *clone = &tio->clone;
 
 	clone->bi_end_io = clone_endio;
 	clone->bi_private = tio;
@@ -1036,12 +1022,6 @@ static void __map_bio(struct dm_target *ti, struct bio *clone,
 		/* error the io and bail out, or requeue it if needed */
 		md = tio->io->md;
 		dec_pending(tio->io, r);
-		/*
-		 * Store bio_set for cleanup.
-		 */
-		clone->bi_end_io = NULL;
-		clone->bi_private = md->bs;
-		bio_put(clone);
 		free_tio(md, tio);
 	} else if (r) {
 		DMWARN("unimplemented target map return value: %d", r);
@@ -1059,25 +1039,16 @@ struct clone_info {
 	unsigned short idx;
 };
 
-static void dm_bio_destructor(struct bio *bio)
-{
-	struct bio_set *bs = bio->bi_private;
-
-	bio_free(bio, bs);
-}
-
 /*
  * Creates a little bio that just does part of a bvec.
  */
-static struct bio *split_bvec(struct bio *bio, sector_t sector,
-			      unsigned short idx, unsigned int offset,
-			      unsigned int len, struct bio_set *bs)
+static void split_bvec(struct dm_target_io *tio, struct bio *bio,
+		       sector_t sector, unsigned short idx, unsigned int offset,
+		       unsigned int len, struct bio_set *bs)
 {
-	struct bio *clone;
+	struct bio *clone = &tio->clone;
 	struct bio_vec *bv = bio->bi_io_vec + idx;
 
-	clone = bio_alloc_bioset(GFP_NOIO, 1, bs);
-	clone->bi_destructor = dm_bio_destructor;
 	*clone->bi_io_vec = *bv;
 
 	clone->bi_sector = sector;
@@ -1090,26 +1061,23 @@ static struct bio *split_bvec(struct bio *bio, sector_t sector,
 	clone->bi_flags |= 1 << BIO_CLONED;
 
 	if (bio_integrity(bio)) {
-		bio_integrity_clone(clone, bio, GFP_NOIO, bs);
+		bio_integrity_clone(clone, bio, GFP_NOIO);
 		bio_integrity_trim(clone,
 				   bio_sector_offset(bio, idx, offset), len);
 	}
-
-	return clone;
 }
 
 /*
  * Creates a bio that consists of range of complete bvecs.
  */
-static struct bio *clone_bio(struct bio *bio, sector_t sector,
-			     unsigned short idx, unsigned short bv_count,
-			     unsigned int len, struct bio_set *bs)
+static void clone_bio(struct dm_target_io *tio, struct bio *bio,
+		      sector_t sector, unsigned short idx,
+		      unsigned short bv_count, unsigned int len,
+		      struct bio_set *bs)
 {
-	struct bio *clone;
+	struct bio *clone = &tio->clone;
 
-	clone = bio_alloc_bioset(GFP_NOIO, bio->bi_max_vecs, bs);
 	__bio_clone(clone, bio);
-	clone->bi_destructor = dm_bio_destructor;
 	clone->bi_sector = sector;
 	clone->bi_idx = idx;
 	clone->bi_vcnt = idx + bv_count;
@@ -1117,20 +1085,22 @@ static struct bio *clone_bio(struct bio *bio, sector_t sector,
 	clone->bi_flags &= ~(1 << BIO_SEG_VALID);
 
 	if (bio_integrity(bio)) {
-		bio_integrity_clone(clone, bio, GFP_NOIO, bs);
+		bio_integrity_clone(clone, bio, GFP_NOIO);
 
 		if (idx != bio->bi_idx || clone->bi_size < bio->bi_size)
 			bio_integrity_trim(clone,
 					   bio_sector_offset(bio, idx, 0), len);
 	}
-
-	return clone;
 }
 
 static struct dm_target_io *alloc_tio(struct clone_info *ci,
-				      struct dm_target *ti)
+				      struct dm_target *ti, int nr_iovecs)
 {
-	struct dm_target_io *tio = mempool_alloc(ci->md->tio_pool, GFP_NOIO);
+	struct dm_target_io *tio;
+	struct bio *clone;
+
+	clone = bio_alloc_bioset(GFP_NOIO, nr_iovecs, ci->md->bs);
+	tio = container_of(clone, struct dm_target_io, clone);
 
 	tio->io = ci->io;
 	tio->ti = ti;
@@ -1142,8 +1112,8 @@ static struct dm_target_io *alloc_tio(struct clone_info *ci,
 static void __issue_target_request(struct clone_info *ci, struct dm_target *ti,
 				   unsigned request_nr, sector_t len)
 {
-	struct dm_target_io *tio = alloc_tio(ci, ti);
-	struct bio *clone;
+	struct dm_target_io *tio = alloc_tio(ci, ti, ci->bio->bi_max_vecs);
+	struct bio *clone = &tio->clone;
 
 	tio->info.target_request_nr = request_nr;
 
@@ -1152,15 +1122,14 @@ static void __issue_target_request(struct clone_info *ci, struct dm_target *ti,
 	 * ci->bio->bi_max_vecs is BIO_INLINE_VECS anyway, for both flush
 	 * and discard, so no need for concern about wasted bvec allocations.
 	 */
-	clone = bio_alloc_bioset(GFP_NOIO, ci->bio->bi_max_vecs, ci->md->bs);
-	__bio_clone(clone, ci->bio);
-	clone->bi_destructor = dm_bio_destructor;
+
+	 __bio_clone(clone, ci->bio);
 	if (len) {
 		clone->bi_sector = ci->sector;
 		clone->bi_size = to_bytes(len);
 	}
 
-	__map_bio(ti, clone, tio);
+	__map_bio(ti, tio);
 }
 
 static void __issue_target_requests(struct clone_info *ci, struct dm_target *ti,
@@ -1189,14 +1158,13 @@ static int __clone_and_map_empty_flush(struct clone_info *ci)
  */
 static void __clone_and_map_simple(struct clone_info *ci, struct dm_target *ti)
 {
-	struct bio *clone, *bio = ci->bio;
+	struct bio *bio = ci->bio;
 	struct dm_target_io *tio;
 
-	tio = alloc_tio(ci, ti);
-	clone = clone_bio(bio, ci->sector, ci->idx,
-			  bio->bi_vcnt - ci->idx, ci->sector_count,
-			  ci->md->bs);
-	__map_bio(ti, clone, tio);
+	tio = alloc_tio(ci, ti, bio->bi_max_vecs);
+	clone_bio(tio, bio, ci->sector, ci->idx, bio->bi_vcnt - ci->idx,
+		  ci->sector_count, ci->md->bs);
+	__map_bio(ti, tio);
 	ci->sector_count = 0;
 }
 
@@ -1234,7 +1202,7 @@ static int __clone_and_map_discard(struct clone_info *ci)
 
 static int __clone_and_map(struct clone_info *ci)
 {
-	struct bio *clone, *bio = ci->bio;
+	struct bio *bio = ci->bio;
 	struct dm_target *ti;
 	sector_t len = 0, max;
 	struct dm_target_io *tio;
@@ -1274,10 +1242,10 @@ static int __clone_and_map(struct clone_info *ci)
 			len += bv_len;
 		}
 
-		tio = alloc_tio(ci, ti);
-		clone = clone_bio(bio, ci->sector, ci->idx, i - ci->idx, len,
-				  ci->md->bs);
-		__map_bio(ti, clone, tio);
+		tio = alloc_tio(ci, ti, bio->bi_max_vecs);
+		clone_bio(tio, bio, ci->sector, ci->idx, i - ci->idx, len,
+			  ci->md->bs);
+		__map_bio(ti, tio);
 
 		ci->sector += len;
 		ci->sector_count -= len;
@@ -1302,12 +1270,11 @@ static int __clone_and_map(struct clone_info *ci)
 
 			len = min(remaining, max);
 
-			tio = alloc_tio(ci, ti);
-			clone = split_bvec(bio, ci->sector, ci->idx,
-					   bv->bv_offset + offset, len,
-					   ci->md->bs);
+			tio = alloc_tio(ci, ti, 1);
+			split_bvec(tio, bio, ci->sector, ci->idx,
+				   bv->bv_offset + offset, len, ci->md->bs);
 
-			__map_bio(ti, clone, tio);
+			__map_bio(ti, tio);
 
 			ci->sector += len;
 			ci->sector_count -= len;
@@ -1484,30 +1451,17 @@ void dm_dispatch_request(struct request *rq)
 }
 EXPORT_SYMBOL_GPL(dm_dispatch_request);
 
-static void dm_rq_bio_destructor(struct bio *bio)
-{
-	struct dm_rq_clone_bio_info *info = bio->bi_private;
-	struct mapped_device *md = info->tio->md;
-
-	free_bio_info(info);
-	bio_free(bio, md->bs);
-}
-
 static int dm_rq_bio_constructor(struct bio *bio, struct bio *bio_orig,
 				 void *data)
 {
 	struct dm_rq_target_io *tio = data;
-	struct mapped_device *md = tio->md;
-	struct dm_rq_clone_bio_info *info = alloc_bio_info(md);
-
-	if (!info)
-		return -ENOMEM;
+	struct dm_rq_clone_bio_info *info =
+		container_of(bio, struct dm_rq_clone_bio_info, clone);
 
 	info->orig = bio_orig;
 	info->tio = tio;
 	bio->bi_end_io = end_clone_bio;
 	bio->bi_private = info;
-	bio->bi_destructor = dm_rq_bio_destructor;
 
 	return 0;
 }
@@ -1988,7 +1942,7 @@ static void __bind_mempools(struct mapped_device *md, struct dm_table *t)
 {
 	struct dm_md_mempools *p;
 
-	if (md->io_pool && md->tio_pool && md->bs)
+	if (md->io_pool && (md->tio_pool || dm_table_get_type(t) == DM_TYPE_BIO_BASED) && md->bs)
 		/* the md already has necessary mempools */
 		goto out;
 
@@ -2765,13 +2719,18 @@ struct dm_md_mempools *dm_alloc_md_mempools(unsigned type, unsigned integrity)
 	if (!pools->io_pool)
 		goto free_pools_and_out;
 
-	pools->tio_pool = (type == DM_TYPE_BIO_BASED) ?
-			  mempool_create_slab_pool(MIN_IOS, _tio_cache) :
-			  mempool_create_slab_pool(MIN_IOS, _rq_tio_cache);
-	if (!pools->tio_pool)
-		goto free_io_pool_and_out;
+	pools->tio_pool = NULL;
+	if (type == DM_TYPE_REQUEST_BASED) {
+		pools->tio_pool = mempool_create_slab_pool(MIN_IOS, _rq_tio_cache);
+		if (!pools->tio_pool)
+			goto free_io_pool_and_out;
+	}
 
-	pools->bs = bioset_create(pool_size, 0);
+	pools->bs = (type == DM_TYPE_BIO_BASED) ?
+		bioset_create(pool_size,
+			      offsetof(struct dm_target_io, clone)) :
+		bioset_create(pool_size,
+			      offsetof(struct dm_rq_clone_bio_info, clone));
 	if (!pools->bs)
 		goto free_tio_pool_and_out;
 
@@ -2784,7 +2743,8 @@ free_bioset_and_out:
 	bioset_free(pools->bs);
 
 free_tio_pool_and_out:
-	mempool_destroy(pools->tio_pool);
+	if (pools->tio_pool)
+		mempool_destroy(pools->tio_pool);
 
 free_io_pool_and_out:
 	mempool_destroy(pools->io_pool);
diff --git a/drivers/md/linear.c b/drivers/md/linear.c
index fa211d80fc0..21014836bdb 100644
--- a/drivers/md/linear.c
+++ b/drivers/md/linear.c
@@ -138,6 +138,7 @@ static struct linear_conf *linear_conf(struct mddev *mddev, int raid_disks)
 	struct linear_conf *conf;
 	struct md_rdev *rdev;
 	int i, cnt;
+	bool discard_supported = false;
 
 	conf = kzalloc (sizeof (*conf) + raid_disks*sizeof(struct dev_info),
 			GFP_KERNEL);
@@ -171,6 +172,8 @@ static struct linear_conf *linear_conf(struct mddev *mddev, int raid_disks)
 		conf->array_sectors += rdev->sectors;
 		cnt++;
 
+		if (blk_queue_discard(bdev_get_queue(rdev->bdev)))
+			discard_supported = true;
 	}
 	if (cnt != raid_disks) {
 		printk(KERN_ERR "md/linear:%s: not enough drives present. Aborting!\n",
@@ -178,6 +181,11 @@ static struct linear_conf *linear_conf(struct mddev *mddev, int raid_disks)
 		goto out;
 	}
 
+	if (!discard_supported)
+		queue_flag_clear_unlocked(QUEUE_FLAG_DISCARD, mddev->queue);
+	else
+		queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, mddev->queue);
+
 	/*
 	 * Here we calculate the device offsets.
 	 */
@@ -244,7 +252,9 @@ static int linear_add(struct mddev *mddev, struct md_rdev *rdev)
 	if (!newconf)
 		return -ENOMEM;
 
-	oldconf = rcu_dereference(mddev->private);
+	oldconf = rcu_dereference_protected(mddev->private,
+					    lockdep_is_held(
+						    &mddev->reconfig_mutex));
 	mddev->raid_disks++;
 	rcu_assign_pointer(mddev->private, newconf);
 	md_set_array_sectors(mddev, linear_size(mddev, 0, 0));
@@ -256,7 +266,10 @@ static int linear_add(struct mddev *mddev, struct md_rdev *rdev)
 
 static int linear_stop (struct mddev *mddev)
 {
-	struct linear_conf *conf = mddev->private;
+	struct linear_conf *conf =
+		rcu_dereference_protected(mddev->private,
+					  lockdep_is_held(
+						  &mddev->reconfig_mutex));
 
 	/*
 	 * We do not require rcu protection here since
@@ -326,6 +339,14 @@ static void linear_make_request(struct mddev *mddev, struct bio *bio)
 	bio->bi_sector = bio->bi_sector - start_sector
 		+ tmp_dev->rdev->data_offset;
 	rcu_read_unlock();
+
+	if (unlikely((bio->bi_rw & REQ_DISCARD) &&
+		     !blk_queue_discard(bdev_get_queue(bio->bi_bdev)))) {
+		/* Just ignore it */
+		bio_endio(bio, 0);
+		return;
+	}
+
 	generic_make_request(bio);
 }
 
diff --git a/drivers/md/md.c b/drivers/md/md.c
index 308e87b417e..9ab768acfb6 100644
--- a/drivers/md/md.c
+++ b/drivers/md/md.c
@@ -155,32 +155,17 @@ static int start_readonly;
  * like bio_clone, but with a local bio set
  */
 
-static void mddev_bio_destructor(struct bio *bio)
-{
-	struct mddev *mddev, **mddevp;
-
-	mddevp = (void*)bio;
-	mddev = mddevp[-1];
-
-	bio_free(bio, mddev->bio_set);
-}
-
 struct bio *bio_alloc_mddev(gfp_t gfp_mask, int nr_iovecs,
 			    struct mddev *mddev)
 {
 	struct bio *b;
-	struct mddev **mddevp;
 
 	if (!mddev || !mddev->bio_set)
 		return bio_alloc(gfp_mask, nr_iovecs);
 
-	b = bio_alloc_bioset(gfp_mask, nr_iovecs,
-			     mddev->bio_set);
+	b = bio_alloc_bioset(gfp_mask, nr_iovecs, mddev->bio_set);
 	if (!b)
 		return NULL;
-	mddevp = (void*)b;
-	mddevp[-1] = mddev;
-	b->bi_destructor = mddev_bio_destructor;
 	return b;
 }
 EXPORT_SYMBOL_GPL(bio_alloc_mddev);
@@ -188,32 +173,10 @@ EXPORT_SYMBOL_GPL(bio_alloc_mddev);
 struct bio *bio_clone_mddev(struct bio *bio, gfp_t gfp_mask,
 			    struct mddev *mddev)
 {
-	struct bio *b;
-	struct mddev **mddevp;
-
 	if (!mddev || !mddev->bio_set)
 		return bio_clone(bio, gfp_mask);
 
-	b = bio_alloc_bioset(gfp_mask, bio->bi_max_vecs,
-			     mddev->bio_set);
-	if (!b)
-		return NULL;
-	mddevp = (void*)b;
-	mddevp[-1] = mddev;
-	b->bi_destructor = mddev_bio_destructor;
-	__bio_clone(b, bio);
-	if (bio_integrity(bio)) {
-		int ret;
-
-		ret = bio_integrity_clone(b, bio, gfp_mask, mddev->bio_set);
-
-		if (ret < 0) {
-			bio_put(b);
-			return NULL;
-		}
-	}
-
-	return b;
+	return bio_clone_bioset(bio, gfp_mask, mddev->bio_set);
 }
 EXPORT_SYMBOL_GPL(bio_clone_mddev);
 
@@ -711,7 +674,18 @@ static struct md_rdev * find_rdev_nr(struct mddev *mddev, int nr)
 	return NULL;
 }
 
-static struct md_rdev * find_rdev(struct mddev * mddev, dev_t dev)
+static struct md_rdev *find_rdev_nr_rcu(struct mddev *mddev, int nr)
+{
+	struct md_rdev *rdev;
+
+	rdev_for_each_rcu(rdev, mddev)
+		if (rdev->desc_nr == nr)
+			return rdev;
+
+	return NULL;
+}
+
+static struct md_rdev *find_rdev(struct mddev *mddev, dev_t dev)
 {
 	struct md_rdev *rdev;
 
@@ -722,6 +696,17 @@ static struct md_rdev * find_rdev(struct mddev * mddev, dev_t dev)
 	return NULL;
 }
 
+static struct md_rdev *find_rdev_rcu(struct mddev *mddev, dev_t dev)
+{
+	struct md_rdev *rdev;
+
+	rdev_for_each_rcu(rdev, mddev)
+		if (rdev->bdev->bd_dev == dev)
+			return rdev;
+
+	return NULL;
+}
+
 static struct md_personality *find_pers(int level, char *clevel)
 {
 	struct md_personality *pers;
@@ -2059,8 +2044,14 @@ EXPORT_SYMBOL(md_integrity_register);
 /* Disable data integrity if non-capable/non-matching disk is being added */
 void md_integrity_add_rdev(struct md_rdev *rdev, struct mddev *mddev)
 {
-	struct blk_integrity *bi_rdev = bdev_get_integrity(rdev->bdev);
-	struct blk_integrity *bi_mddev = blk_get_integrity(mddev->gendisk);
+	struct blk_integrity *bi_rdev;
+	struct blk_integrity *bi_mddev;
+
+	if (!mddev->gendisk)
+		return;
+
+	bi_rdev = bdev_get_integrity(rdev->bdev);
+	bi_mddev = blk_get_integrity(mddev->gendisk);
 
 	if (!bi_mddev) /* nothing to do */
 		return;
@@ -3791,6 +3782,8 @@ resync_start_store(struct mddev *mddev, const char *buf, size_t len)
 		return -EINVAL;
 
 	mddev->recovery_cp = n;
+	if (mddev->pers)
+		set_bit(MD_CHANGE_CLEAN, &mddev->flags);
 	return len;
 }
 static struct md_sysfs_entry md_resync_start =
@@ -4268,6 +4261,13 @@ action_store(struct mddev *mddev, const char *page, size_t len)
 		set_bit(MD_RECOVERY_REQUESTED, &mddev->recovery);
 		set_bit(MD_RECOVERY_SYNC, &mddev->recovery);
 	}
+	if (mddev->ro == 2) {
+		/* A write to sync_action is enough to justify
+		 * canceling read-auto mode
+		 */
+		mddev->ro = 0;
+		md_wakeup_thread(mddev->sync_thread);
+	}
 	set_bit(MD_RECOVERY_NEEDED, &mddev->recovery);
 	md_wakeup_thread(mddev->thread);
 	sysfs_notify_dirent_safe(mddev->sysfs_action);
@@ -4278,7 +4278,8 @@ static ssize_t
 mismatch_cnt_show(struct mddev *mddev, char *page)
 {
 	return sprintf(page, "%llu\n",
-		       (unsigned long long) mddev->resync_mismatches);
+		       (unsigned long long)
+		       atomic64_read(&mddev->resync_mismatches));
 }
 
 static struct md_sysfs_entry md_scan_mode =
@@ -4399,6 +4400,10 @@ sync_completed_show(struct mddev *mddev, char *page)
 	if (!test_bit(MD_RECOVERY_RUNNING, &mddev->recovery))
 		return sprintf(page, "none\n");
 
+	if (mddev->curr_resync == 1 ||
+	    mddev->curr_resync == 2)
+		return sprintf(page, "delayed\n");
+
 	if (test_bit(MD_RECOVERY_SYNC, &mddev->recovery) ||
 	    test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery))
 		max_sectors = mddev->resync_max_sectors;
@@ -5006,8 +5011,7 @@ int md_run(struct mddev *mddev)
 	}
 
 	if (mddev->bio_set == NULL)
-		mddev->bio_set = bioset_create(BIO_POOL_SIZE,
-					       sizeof(struct mddev *));
+		mddev->bio_set = bioset_create(BIO_POOL_SIZE, 0);
 
 	spin_lock(&pers_lock);
 	pers = find_pers(mddev->level, mddev->clevel);
@@ -5245,7 +5249,7 @@ static void md_clean(struct mddev *mddev)
 	mddev->new_layout = 0;
 	mddev->new_chunk_sectors = 0;
 	mddev->curr_resync = 0;
-	mddev->resync_mismatches = 0;
+	atomic64_set(&mddev->resync_mismatches, 0);
 	mddev->suspend_lo = mddev->suspend_hi = 0;
 	mddev->sync_speed_min = mddev->sync_speed_max = 0;
 	mddev->recovery = 0;
@@ -5547,8 +5551,9 @@ static int get_array_info(struct mddev * mddev, void __user * arg)
 	int nr,working,insync,failed,spare;
 	struct md_rdev *rdev;
 
-	nr=working=insync=failed=spare=0;
-	rdev_for_each(rdev, mddev) {
+	nr = working = insync = failed = spare = 0;
+	rcu_read_lock();
+	rdev_for_each_rcu(rdev, mddev) {
 		nr++;
 		if (test_bit(Faulty, &rdev->flags))
 			failed++;
@@ -5560,6 +5565,7 @@ static int get_array_info(struct mddev * mddev, void __user * arg)
 				spare++;
 		}
 	}
+	rcu_read_unlock();
 
 	info.major_version = mddev->major_version;
 	info.minor_version = mddev->minor_version;
@@ -5643,7 +5649,8 @@ static int get_disk_info(struct mddev * mddev, void __user * arg)
 	if (copy_from_user(&info, arg, sizeof(info)))
 		return -EFAULT;
 
-	rdev = find_rdev_nr(mddev, info.number);
+	rcu_read_lock();
+	rdev = find_rdev_nr_rcu(mddev, info.number);
 	if (rdev) {
 		info.major = MAJOR(rdev->bdev->bd_dev);
 		info.minor = MINOR(rdev->bdev->bd_dev);
@@ -5662,6 +5669,7 @@ static int get_disk_info(struct mddev * mddev, void __user * arg)
 		info.raid_disk = -1;
 		info.state = (1<<MD_DISK_REMOVED);
 	}
+	rcu_read_unlock();
 
 	if (copy_to_user(arg, &info, sizeof(info)))
 		return -EFAULT;
@@ -6270,18 +6278,22 @@ static int update_array_info(struct mddev *mddev, mdu_array_info_t *info)
 static int set_disk_faulty(struct mddev *mddev, dev_t dev)
 {
 	struct md_rdev *rdev;
+	int err = 0;
 
 	if (mddev->pers == NULL)
 		return -ENODEV;
 
-	rdev = find_rdev(mddev, dev);
+	rcu_read_lock();
+	rdev = find_rdev_rcu(mddev, dev);
 	if (!rdev)
-		return -ENODEV;
-
-	md_error(mddev, rdev);
-	if (!test_bit(Faulty, &rdev->flags))
-		return -EBUSY;
-	return 0;
+		err =  -ENODEV;
+	else {
+		md_error(mddev, rdev);
+		if (!test_bit(Faulty, &rdev->flags))
+			err = -EBUSY;
+	}
+	rcu_read_unlock();
+	return err;
 }
 
 /*
@@ -6353,6 +6365,27 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 		goto abort;
 	}
 
+	/* Some actions do not requires the mutex */
+	switch (cmd) {
+	case GET_ARRAY_INFO:
+		if (!mddev->raid_disks && !mddev->external)
+			err = -ENODEV;
+		else
+			err = get_array_info(mddev, argp);
+		goto abort;
+
+	case GET_DISK_INFO:
+		if (!mddev->raid_disks && !mddev->external)
+			err = -ENODEV;
+		else
+			err = get_disk_info(mddev, argp);
+		goto abort;
+
+	case SET_DISK_FAULTY:
+		err = set_disk_faulty(mddev, new_decode_dev(arg));
+		goto abort;
+	}
+
 	err = mddev_lock(mddev);
 	if (err) {
 		printk(KERN_INFO 
@@ -6425,18 +6458,10 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 	 */
 	switch (cmd)
 	{
-		case GET_ARRAY_INFO:
-			err = get_array_info(mddev, argp);
-			goto done_unlock;
-
 		case GET_BITMAP_FILE:
 			err = get_bitmap_file(mddev, argp);
 			goto done_unlock;
 
-		case GET_DISK_INFO:
-			err = get_disk_info(mddev, argp);
-			goto done_unlock;
-
 		case RESTART_ARRAY_RW:
 			err = restart_array(mddev);
 			goto done_unlock;
@@ -6518,10 +6543,6 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 			err = hot_add_disk(mddev, new_decode_dev(arg));
 			goto done_unlock;
 
-		case SET_DISK_FAULTY:
-			err = set_disk_faulty(mddev, new_decode_dev(arg));
-			goto done_unlock;
-
 		case RUN_ARRAY:
 			err = do_md_run(mddev);
 			goto done_unlock;
@@ -6679,7 +6700,7 @@ static int md_thread(void * arg)
 
 		clear_bit(THREAD_WAKEUP, &thread->flags);
 		if (!kthread_should_stop())
-			thread->run(thread->mddev);
+			thread->run(thread);
 	}
 
 	return 0;
@@ -6694,8 +6715,8 @@ void md_wakeup_thread(struct md_thread *thread)
 	}
 }
 
-struct md_thread *md_register_thread(void (*run) (struct mddev *), struct mddev *mddev,
-				 const char *name)
+struct md_thread *md_register_thread(void (*run) (struct md_thread *),
+		struct mddev *mddev, const char *name)
 {
 	struct md_thread *thread;
 
@@ -6790,7 +6811,11 @@ static void status_resync(struct seq_file *seq, struct mddev * mddev)
 	int scale;
 	unsigned int per_milli;
 
-	resync = mddev->curr_resync - atomic_read(&mddev->recovery_active);
+	if (mddev->curr_resync <= 3)
+		resync = 0;
+	else
+		resync = mddev->curr_resync
+			- atomic_read(&mddev->recovery_active);
 
 	if (test_bit(MD_RECOVERY_SYNC, &mddev->recovery) ||
 	    test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery))
@@ -7016,7 +7041,7 @@ static int md_seq_show(struct seq_file *seq, void *v)
 				if (mddev->curr_resync > 2) {
 					status_resync(seq, mddev);
 					seq_printf(seq, "\n      ");
-				} else if (mddev->curr_resync == 1 || mddev->curr_resync == 2)
+				} else if (mddev->curr_resync >= 1)
 					seq_printf(seq, "\tresync=DELAYED\n      ");
 				else if (mddev->recovery_cp < MaxSector)
 					seq_printf(seq, "\tresync=PENDING\n      ");
@@ -7244,8 +7269,9 @@ EXPORT_SYMBOL_GPL(md_allow_write);
 
 #define SYNC_MARKS	10
 #define	SYNC_MARK_STEP	(3*HZ)
-void md_do_sync(struct mddev *mddev)
+void md_do_sync(struct md_thread *thread)
 {
+	struct mddev *mddev = thread->mddev;
 	struct mddev *mddev2;
 	unsigned int currspeed = 0,
 		 window;
@@ -7349,7 +7375,7 @@ void md_do_sync(struct mddev *mddev)
 		 * which defaults to physical size, but can be virtual size
 		 */
 		max_sectors = mddev->resync_max_sectors;
-		mddev->resync_mismatches = 0;
+		atomic64_set(&mddev->resync_mismatches, 0);
 		/* we don't use the checkpoint if there's a bitmap */
 		if (test_bit(MD_RECOVERY_REQUESTED, &mddev->recovery))
 			j = mddev->resync_min;
@@ -7405,8 +7431,11 @@ void md_do_sync(struct mddev *mddev)
 		       "md: resuming %s of %s from checkpoint.\n",
 		       desc, mdname(mddev));
 		mddev->curr_resync = j;
-	}
+	} else
+		mddev->curr_resync = 3; /* no longer delayed */
 	mddev->curr_resync_completed = j;
+	sysfs_notify(&mddev->kobj, NULL, "sync_completed");
+	md_new_event(mddev);
 
 	blk_start_plug(&plug);
 	while (j < max_sectors) {
@@ -7459,7 +7488,8 @@ void md_do_sync(struct mddev *mddev)
 			break;
 
 		j += sectors;
-		if (j>1) mddev->curr_resync = j;
+		if (j > 2)
+			mddev->curr_resync = j;
 		mddev->curr_mark_cnt = io_sectors;
 		if (last_check == 0)
 			/* this is the earliest that rebuild will be
@@ -7581,8 +7611,6 @@ static int remove_and_add_spares(struct mddev *mddev)
 	int spares = 0;
 	int removed = 0;
 
-	mddev->curr_resync_completed = 0;
-
 	rdev_for_each(rdev, mddev)
 		if (rdev->raid_disk >= 0 &&
 		    !test_bit(Blocked, &rdev->flags) &&
@@ -7777,6 +7805,7 @@ void md_check_recovery(struct mddev *mddev)
 		/* Set RUNNING before clearing NEEDED to avoid
 		 * any transients in the value of "sync_action".
 		 */
+		mddev->curr_resync_completed = 0;
 		set_bit(MD_RECOVERY_RUNNING, &mddev->recovery);
 		/* Clear some bits that don't mean anything, but
 		 * might be left set
@@ -7790,7 +7819,7 @@ void md_check_recovery(struct mddev *mddev)
 		/* no recovery is running.
 		 * remove any failed drives, then
 		 * add spares if possible.
-		 * Spare are also removed and re-added, to allow
+		 * Spares are also removed and re-added, to allow
 		 * the personality to fail the re-add.
 		 */
 
diff --git a/drivers/md/md.h b/drivers/md/md.h
index f385b038589..af443ab868d 100644
--- a/drivers/md/md.h
+++ b/drivers/md/md.h
@@ -282,7 +282,7 @@ struct mddev {
 
 	sector_t			resync_max_sectors; /* may be set by personality */
 
-	sector_t			resync_mismatches; /* count of sectors where
+	atomic64_t			resync_mismatches; /* count of sectors where
 							    * parity/replica mismatch found
 							    */
 
@@ -540,12 +540,13 @@ static inline void sysfs_unlink_rdev(struct mddev *mddev, struct md_rdev *rdev)
 	list_for_each_entry_rcu(rdev, &((mddev)->disks), same_set)
 
 struct md_thread {
-	void			(*run) (struct mddev *mddev);
+	void			(*run) (struct md_thread *thread);
 	struct mddev		*mddev;
 	wait_queue_head_t	wqueue;
 	unsigned long           flags;
 	struct task_struct	*tsk;
 	unsigned long		timeout;
+	void			*private;
 };
 
 #define THREAD_WAKEUP  0
@@ -584,7 +585,7 @@ static inline void safe_put_page(struct page *p)
 extern int register_md_personality(struct md_personality *p);
 extern int unregister_md_personality(struct md_personality *p);
 extern struct md_thread *md_register_thread(
-	void (*run)(struct mddev *mddev),
+	void (*run)(struct md_thread *thread),
 	struct mddev *mddev,
 	const char *name);
 extern void md_unregister_thread(struct md_thread **threadp);
@@ -603,7 +604,7 @@ extern void md_super_write(struct mddev *mddev, struct md_rdev *rdev,
 extern void md_super_wait(struct mddev *mddev);
 extern int sync_page_io(struct md_rdev *rdev, sector_t sector, int size, 
 			struct page *page, int rw, bool metadata_op);
-extern void md_do_sync(struct mddev *mddev);
+extern void md_do_sync(struct md_thread *thread);
 extern void md_new_event(struct mddev *mddev);
 extern int md_allow_write(struct mddev *mddev);
 extern void md_wait_for_blocked_rdev(struct md_rdev *rdev, struct mddev *mddev);
diff --git a/drivers/md/multipath.c b/drivers/md/multipath.c
index 61a1833ebaf..1642eae75a3 100644
--- a/drivers/md/multipath.c
+++ b/drivers/md/multipath.c
@@ -335,8 +335,9 @@ abort:
  *	3.	Performs writes following reads for array syncronising.
  */
 
-static void multipathd (struct mddev *mddev)
+static void multipathd(struct md_thread *thread)
 {
+	struct mddev *mddev = thread->mddev;
 	struct multipath_bh *mp_bh;
 	struct bio *bio;
 	unsigned long flags;
diff --git a/drivers/md/persistent-data/dm-space-map-common.c b/drivers/md/persistent-data/dm-space-map-common.c
index d77602d63c8..f3a9af8cdec 100644
--- a/drivers/md/persistent-data/dm-space-map-common.c
+++ b/drivers/md/persistent-data/dm-space-map-common.c
@@ -434,14 +434,14 @@ int sm_ll_insert(struct ll_disk *ll, dm_block_t b,
 	if (ref_count && !old) {
 		*ev = SM_ALLOC;
 		ll->nr_allocated++;
-		ie_disk.nr_free = cpu_to_le32(le32_to_cpu(ie_disk.nr_free) - 1);
+		le32_add_cpu(&ie_disk.nr_free, -1);
 		if (le32_to_cpu(ie_disk.none_free_before) == bit)
 			ie_disk.none_free_before = cpu_to_le32(bit + 1);
 
 	} else if (old && !ref_count) {
 		*ev = SM_FREE;
 		ll->nr_allocated--;
-		ie_disk.nr_free = cpu_to_le32(le32_to_cpu(ie_disk.nr_free) + 1);
+		le32_add_cpu(&ie_disk.nr_free, 1);
 		ie_disk.none_free_before = cpu_to_le32(min(le32_to_cpu(ie_disk.none_free_before), bit));
 	}
 
diff --git a/drivers/md/raid0.c b/drivers/md/raid0.c
index de63a1fc373..24b359717a7 100644
--- a/drivers/md/raid0.c
+++ b/drivers/md/raid0.c
@@ -88,6 +88,7 @@ static int create_strip_zones(struct mddev *mddev, struct r0conf **private_conf)
 	char b[BDEVNAME_SIZE];
 	char b2[BDEVNAME_SIZE];
 	struct r0conf *conf = kzalloc(sizeof(*conf), GFP_KERNEL);
+	bool discard_supported = false;
 
 	if (!conf)
 		return -ENOMEM;
@@ -195,6 +196,9 @@ static int create_strip_zones(struct mddev *mddev, struct r0conf **private_conf)
 		if (!smallest || (rdev1->sectors < smallest->sectors))
 			smallest = rdev1;
 		cnt++;
+
+		if (blk_queue_discard(bdev_get_queue(rdev1->bdev)))
+			discard_supported = true;
 	}
 	if (cnt != mddev->raid_disks) {
 		printk(KERN_ERR "md/raid0:%s: too few disks (%d of %d) - "
@@ -272,6 +276,11 @@ static int create_strip_zones(struct mddev *mddev, struct r0conf **private_conf)
 	blk_queue_io_opt(mddev->queue,
 			 (mddev->chunk_sectors << 9) * mddev->raid_disks);
 
+	if (!discard_supported)
+		queue_flag_clear_unlocked(QUEUE_FLAG_DISCARD, mddev->queue);
+	else
+		queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, mddev->queue);
+
 	pr_debug("md/raid0:%s: done.\n", mdname(mddev));
 	*private_conf = conf;
 
@@ -422,6 +431,8 @@ static int raid0_run(struct mddev *mddev)
 	if (md_check_no_bitmap(mddev))
 		return -EINVAL;
 	blk_queue_max_hw_sectors(mddev->queue, mddev->chunk_sectors);
+	blk_queue_max_write_same_sectors(mddev->queue, mddev->chunk_sectors);
+	blk_queue_max_discard_sectors(mddev->queue, mddev->chunk_sectors);
 
 	/* if private is not null, we are here after takeover */
 	if (mddev->private == NULL) {
@@ -509,7 +520,7 @@ static void raid0_make_request(struct mddev *mddev, struct bio *bio)
 		sector_t sector = bio->bi_sector;
 		struct bio_pair *bp;
 		/* Sanity check -- queue functions should prevent this happening */
-		if (bio->bi_vcnt != 1 ||
+		if ((bio->bi_vcnt != 1 && bio->bi_vcnt != 0) ||
 		    bio->bi_idx != 0)
 			goto bad_map;
 		/* This is a one page bio that upper layers
@@ -535,6 +546,13 @@ static void raid0_make_request(struct mddev *mddev, struct bio *bio)
 	bio->bi_sector = sector_offset + zone->dev_start +
 		tmp_dev->data_offset;
 
+	if (unlikely((bio->bi_rw & REQ_DISCARD) &&
+		     !blk_queue_discard(bdev_get_queue(bio->bi_bdev)))) {
+		/* Just ignore it */
+		bio_endio(bio, 0);
+		return;
+	}
+
 	generic_make_request(bio);
 	return;
 
diff --git a/drivers/md/raid1.c b/drivers/md/raid1.c
index 611b5f79761..8034fbd6190 100644
--- a/drivers/md/raid1.c
+++ b/drivers/md/raid1.c
@@ -333,9 +333,10 @@ static void raid1_end_read_request(struct bio *bio, int error)
 		spin_unlock_irqrestore(&conf->device_lock, flags);
 	}
 
-	if (uptodate)
+	if (uptodate) {
 		raid_end_bio_io(r1_bio);
-	else {
+		rdev_dec_pending(conf->mirrors[mirror].rdev, conf->mddev);
+	} else {
 		/*
 		 * oops, read error:
 		 */
@@ -349,9 +350,8 @@ static void raid1_end_read_request(struct bio *bio, int error)
 			(unsigned long long)r1_bio->sector);
 		set_bit(R1BIO_ReadError, &r1_bio->state);
 		reschedule_retry(r1_bio);
+		/* don't drop the reference on read_disk yet */
 	}
-
-	rdev_dec_pending(conf->mirrors[mirror].rdev, conf->mddev);
 }
 
 static void close_write(struct r1bio *r1_bio)
@@ -781,7 +781,12 @@ static void flush_pending_writes(struct r1conf *conf)
 		while (bio) { /* submit pending writes */
 			struct bio *next = bio->bi_next;
 			bio->bi_next = NULL;
-			generic_make_request(bio);
+			if (unlikely((bio->bi_rw & REQ_DISCARD) &&
+			    !blk_queue_discard(bdev_get_queue(bio->bi_bdev))))
+				/* Just ignore it */
+				bio_endio(bio, 0);
+			else
+				generic_make_request(bio);
 			bio = next;
 		}
 	} else
@@ -994,6 +999,8 @@ static void make_request(struct mddev *mddev, struct bio * bio)
 	const int rw = bio_data_dir(bio);
 	const unsigned long do_sync = (bio->bi_rw & REQ_SYNC);
 	const unsigned long do_flush_fua = (bio->bi_rw & (REQ_FLUSH | REQ_FUA));
+	const unsigned long do_discard = (bio->bi_rw
+					  & (REQ_DISCARD | REQ_SECURE));
 	struct md_rdev *blocked_rdev;
 	struct blk_plug_cb *cb;
 	struct raid1_plug_cb *plug = NULL;
@@ -1295,7 +1302,7 @@ read_again:
 				   conf->mirrors[i].rdev->data_offset);
 		mbio->bi_bdev = conf->mirrors[i].rdev->bdev;
 		mbio->bi_end_io	= raid1_end_write_request;
-		mbio->bi_rw = WRITE | do_flush_fua | do_sync;
+		mbio->bi_rw = WRITE | do_flush_fua | do_sync | do_discard;
 		mbio->bi_private = r1_bio;
 
 		atomic_inc(&r1_bio->remaining);
@@ -1549,6 +1556,8 @@ static int raid1_add_disk(struct mddev *mddev, struct md_rdev *rdev)
 		clear_bit(Unmerged, &rdev->flags);
 	}
 	md_integrity_add_rdev(rdev, mddev);
+	if (blk_queue_discard(bdev_get_queue(rdev->bdev)))
+		queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, mddev->queue);
 	print_conf(conf);
 	return err;
 }
@@ -1867,7 +1876,7 @@ static int process_checks(struct r1bio *r1_bio)
 		} else
 			j = 0;
 		if (j >= 0)
-			mddev->resync_mismatches += r1_bio->sectors;
+			atomic64_add(r1_bio->sectors, &mddev->resync_mismatches);
 		if (j < 0 || (test_bit(MD_RECOVERY_CHECK, &mddev->recovery)
 			      && test_bit(BIO_UPTODATE, &sbio->bi_flags))) {
 			/* No need to write to this device. */
@@ -2220,6 +2229,7 @@ static void handle_read_error(struct r1conf *conf, struct r1bio *r1_bio)
 		unfreeze_array(conf);
 	} else
 		md_error(mddev, conf->mirrors[r1_bio->read_disk].rdev);
+	rdev_dec_pending(conf->mirrors[r1_bio->read_disk].rdev, conf->mddev);
 
 	bio = r1_bio->bios[r1_bio->read_disk];
 	bdevname(bio->bi_bdev, b);
@@ -2285,8 +2295,9 @@ read_more:
 	}
 }
 
-static void raid1d(struct mddev *mddev)
+static void raid1d(struct md_thread *thread)
 {
+	struct mddev *mddev = thread->mddev;
 	struct r1bio *r1_bio;
 	unsigned long flags;
 	struct r1conf *conf = mddev->private;
@@ -2783,6 +2794,7 @@ static int run(struct mddev *mddev)
 	int i;
 	struct md_rdev *rdev;
 	int ret;
+	bool discard_supported = false;
 
 	if (mddev->level != 1) {
 		printk(KERN_ERR "md/raid1:%s: raid level not set to mirroring (%d)\n",
@@ -2812,6 +2824,8 @@ static int run(struct mddev *mddev)
 			continue;
 		disk_stack_limits(mddev->gendisk, rdev->bdev,
 				  rdev->data_offset << 9);
+		if (blk_queue_discard(bdev_get_queue(rdev->bdev)))
+			discard_supported = true;
 	}
 
 	mddev->degraded = 0;
@@ -2846,6 +2860,13 @@ static int run(struct mddev *mddev)
 		mddev->queue->backing_dev_info.congested_fn = raid1_congested;
 		mddev->queue->backing_dev_info.congested_data = mddev;
 		blk_queue_merge_bvec(mddev->queue, raid1_mergeable_bvec);
+
+		if (discard_supported)
+			queue_flag_set_unlocked(QUEUE_FLAG_DISCARD,
+						mddev->queue);
+		else
+			queue_flag_clear_unlocked(QUEUE_FLAG_DISCARD,
+						  mddev->queue);
 	}
 
 	ret =  md_integrity_register(mddev);
diff --git a/drivers/md/raid10.c b/drivers/md/raid10.c
index 0138a727c1f..906ccbd0f7d 100644
--- a/drivers/md/raid10.c
+++ b/drivers/md/raid10.c
@@ -911,7 +911,12 @@ static void flush_pending_writes(struct r10conf *conf)
 		while (bio) { /* submit pending writes */
 			struct bio *next = bio->bi_next;
 			bio->bi_next = NULL;
-			generic_make_request(bio);
+			if (unlikely((bio->bi_rw & REQ_DISCARD) &&
+			    !blk_queue_discard(bdev_get_queue(bio->bi_bdev))))
+				/* Just ignore it */
+				bio_endio(bio, 0);
+			else
+				generic_make_request(bio);
 			bio = next;
 		}
 	} else
@@ -1050,6 +1055,44 @@ static sector_t choose_data_offset(struct r10bio *r10_bio,
 		return rdev->new_data_offset;
 }
 
+struct raid10_plug_cb {
+	struct blk_plug_cb	cb;
+	struct bio_list		pending;
+	int			pending_cnt;
+};
+
+static void raid10_unplug(struct blk_plug_cb *cb, bool from_schedule)
+{
+	struct raid10_plug_cb *plug = container_of(cb, struct raid10_plug_cb,
+						   cb);
+	struct mddev *mddev = plug->cb.data;
+	struct r10conf *conf = mddev->private;
+	struct bio *bio;
+
+	if (from_schedule) {
+		spin_lock_irq(&conf->device_lock);
+		bio_list_merge(&conf->pending_bio_list, &plug->pending);
+		conf->pending_count += plug->pending_cnt;
+		spin_unlock_irq(&conf->device_lock);
+		md_wakeup_thread(mddev->thread);
+		kfree(plug);
+		return;
+	}
+
+	/* we aren't scheduling, so we can do the write-out directly. */
+	bio = bio_list_get(&plug->pending);
+	bitmap_unplug(mddev->bitmap);
+	wake_up(&conf->wait_barrier);
+
+	while (bio) { /* submit pending writes */
+		struct bio *next = bio->bi_next;
+		bio->bi_next = NULL;
+		generic_make_request(bio);
+		bio = next;
+	}
+	kfree(plug);
+}
+
 static void make_request(struct mddev *mddev, struct bio * bio)
 {
 	struct r10conf *conf = mddev->private;
@@ -1061,8 +1104,12 @@ static void make_request(struct mddev *mddev, struct bio * bio)
 	const int rw = bio_data_dir(bio);
 	const unsigned long do_sync = (bio->bi_rw & REQ_SYNC);
 	const unsigned long do_fua = (bio->bi_rw & REQ_FUA);
+	const unsigned long do_discard = (bio->bi_rw
+					  & (REQ_DISCARD | REQ_SECURE));
 	unsigned long flags;
 	struct md_rdev *blocked_rdev;
+	struct blk_plug_cb *cb;
+	struct raid10_plug_cb *plug = NULL;
 	int sectors_handled;
 	int max_sectors;
 	int sectors;
@@ -1081,7 +1128,7 @@ static void make_request(struct mddev *mddev, struct bio * bio)
 			 || conf->prev.near_copies < conf->prev.raid_disks))) {
 		struct bio_pair *bp;
 		/* Sanity check -- queue functions should prevent this happening */
-		if (bio->bi_vcnt != 1 ||
+		if ((bio->bi_vcnt != 1 && bio->bi_vcnt != 0) ||
 		    bio->bi_idx != 0)
 			goto bad_map;
 		/* This is a one page bio that upper layers
@@ -1410,15 +1457,26 @@ retry_write:
 						      conf->mirrors[d].rdev));
 		mbio->bi_bdev = conf->mirrors[d].rdev->bdev;
 		mbio->bi_end_io	= raid10_end_write_request;
-		mbio->bi_rw = WRITE | do_sync | do_fua;
+		mbio->bi_rw = WRITE | do_sync | do_fua | do_discard;
 		mbio->bi_private = r10_bio;
 
 		atomic_inc(&r10_bio->remaining);
+
+		cb = blk_check_plugged(raid10_unplug, mddev, sizeof(*plug));
+		if (cb)
+			plug = container_of(cb, struct raid10_plug_cb, cb);
+		else
+			plug = NULL;
 		spin_lock_irqsave(&conf->device_lock, flags);
-		bio_list_add(&conf->pending_bio_list, mbio);
-		conf->pending_count++;
+		if (plug) {
+			bio_list_add(&plug->pending, mbio);
+			plug->pending_cnt++;
+		} else {
+			bio_list_add(&conf->pending_bio_list, mbio);
+			conf->pending_count++;
+		}
 		spin_unlock_irqrestore(&conf->device_lock, flags);
-		if (!mddev_check_plugged(mddev))
+		if (!plug)
 			md_wakeup_thread(mddev->thread);
 
 		if (!r10_bio->devs[i].repl_bio)
@@ -1439,7 +1497,7 @@ retry_write:
 					   conf->mirrors[d].replacement));
 		mbio->bi_bdev = conf->mirrors[d].replacement->bdev;
 		mbio->bi_end_io	= raid10_end_write_request;
-		mbio->bi_rw = WRITE | do_sync | do_fua;
+		mbio->bi_rw = WRITE | do_sync | do_fua | do_discard;
 		mbio->bi_private = r10_bio;
 
 		atomic_inc(&r10_bio->remaining);
@@ -1638,7 +1696,7 @@ static int raid10_spare_active(struct mddev *mddev)
 			   && !test_bit(Faulty, &tmp->rdev->flags)
 			   && !test_and_set_bit(In_sync, &tmp->rdev->flags)) {
 			count++;
-			sysfs_notify_dirent(tmp->rdev->sysfs_state);
+			sysfs_notify_dirent_safe(tmp->rdev->sysfs_state);
 		}
 	}
 	spin_lock_irqsave(&conf->device_lock, flags);
@@ -1725,6 +1783,9 @@ static int raid10_add_disk(struct mddev *mddev, struct md_rdev *rdev)
 		clear_bit(Unmerged, &rdev->flags);
 	}
 	md_integrity_add_rdev(rdev, mddev);
+	if (blk_queue_discard(bdev_get_queue(rdev->bdev)))
+		queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, mddev->queue);
+
 	print_conf(conf);
 	return err;
 }
@@ -1952,7 +2013,7 @@ static void sync_request_write(struct mddev *mddev, struct r10bio *r10_bio)
 					break;
 			if (j == vcnt)
 				continue;
-			mddev->resync_mismatches += r10_bio->sectors;
+			atomic64_add(r10_bio->sectors, &mddev->resync_mismatches);
 			if (test_bit(MD_RECOVERY_CHECK, &mddev->recovery))
 				/* Don't fix anything. */
 				continue;
@@ -2673,8 +2734,9 @@ static void handle_write_completed(struct r10conf *conf, struct r10bio *r10_bio)
 	}
 }
 
-static void raid10d(struct mddev *mddev)
+static void raid10d(struct md_thread *thread)
 {
+	struct mddev *mddev = thread->mddev;
 	struct r10bio *r10_bio;
 	unsigned long flags;
 	struct r10conf *conf = mddev->private;
@@ -3158,7 +3220,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 				else {
 					bad_sectors -= (sector - first_bad);
 					if (max_sync > bad_sectors)
-						max_sync = max_sync;
+						max_sync = bad_sectors;
 					continue;
 				}
 			}
@@ -3482,6 +3544,7 @@ static int run(struct mddev *mddev)
 	sector_t size;
 	sector_t min_offset_diff = 0;
 	int first = 1;
+	bool discard_supported = false;
 
 	if (mddev->private == NULL) {
 		conf = setup_conf(mddev);
@@ -3498,6 +3561,8 @@ static int run(struct mddev *mddev)
 
 	chunk_size = mddev->chunk_sectors << 9;
 	if (mddev->queue) {
+		blk_queue_max_discard_sectors(mddev->queue,
+					      mddev->chunk_sectors);
 		blk_queue_io_min(mddev->queue, chunk_size);
 		if (conf->geo.raid_disks % conf->geo.near_copies)
 			blk_queue_io_opt(mddev->queue, chunk_size * conf->geo.raid_disks);
@@ -3543,8 +3608,16 @@ static int run(struct mddev *mddev)
 					  rdev->data_offset << 9);
 
 		disk->head_position = 0;
+
+		if (blk_queue_discard(bdev_get_queue(rdev->bdev)))
+			discard_supported = true;
 	}
 
+	if (discard_supported)
+		queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, mddev->queue);
+	else
+		queue_flag_clear_unlocked(QUEUE_FLAG_DISCARD, mddev->queue);
+
 	/* need to check that every block has at least one working mirror */
 	if (!enough(conf, -1)) {
 		printk(KERN_ERR "md/raid10:%s: not enough operational mirrors.\n",
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index 0689173fd9f..c5439dce029 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -551,6 +551,8 @@ static void ops_run_io(struct stripe_head *sh, struct stripe_head_state *s)
 				rw = WRITE_FUA;
 			else
 				rw = WRITE;
+			if (test_bit(R5_Discard, &sh->dev[i].flags))
+				rw |= REQ_DISCARD;
 		} else if (test_and_clear_bit(R5_Wantread, &sh->dev[i].flags))
 			rw = READ;
 		else if (test_and_clear_bit(R5_WantReplace,
@@ -1174,8 +1176,11 @@ ops_run_biodrain(struct stripe_head *sh, struct dma_async_tx_descriptor *tx)
 					set_bit(R5_WantFUA, &dev->flags);
 				if (wbi->bi_rw & REQ_SYNC)
 					set_bit(R5_SyncIO, &dev->flags);
-				tx = async_copy_data(1, wbi, dev->page,
-					dev->sector, tx);
+				if (wbi->bi_rw & REQ_DISCARD)
+					set_bit(R5_Discard, &dev->flags);
+				else
+					tx = async_copy_data(1, wbi, dev->page,
+						dev->sector, tx);
 				wbi = r5_next_bio(wbi, dev->sector);
 			}
 		}
@@ -1191,7 +1196,7 @@ static void ops_complete_reconstruct(void *stripe_head_ref)
 	int pd_idx = sh->pd_idx;
 	int qd_idx = sh->qd_idx;
 	int i;
-	bool fua = false, sync = false;
+	bool fua = false, sync = false, discard = false;
 
 	pr_debug("%s: stripe %llu\n", __func__,
 		(unsigned long long)sh->sector);
@@ -1199,13 +1204,15 @@ static void ops_complete_reconstruct(void *stripe_head_ref)
 	for (i = disks; i--; ) {
 		fua |= test_bit(R5_WantFUA, &sh->dev[i].flags);
 		sync |= test_bit(R5_SyncIO, &sh->dev[i].flags);
+		discard |= test_bit(R5_Discard, &sh->dev[i].flags);
 	}
 
 	for (i = disks; i--; ) {
 		struct r5dev *dev = &sh->dev[i];
 
 		if (dev->written || i == pd_idx || i == qd_idx) {
-			set_bit(R5_UPTODATE, &dev->flags);
+			if (!discard)
+				set_bit(R5_UPTODATE, &dev->flags);
 			if (fua)
 				set_bit(R5_WantFUA, &dev->flags);
 			if (sync)
@@ -1241,6 +1248,18 @@ ops_run_reconstruct5(struct stripe_head *sh, struct raid5_percpu *percpu,
 	pr_debug("%s: stripe %llu\n", __func__,
 		(unsigned long long)sh->sector);
 
+	for (i = 0; i < sh->disks; i++) {
+		if (pd_idx == i)
+			continue;
+		if (!test_bit(R5_Discard, &sh->dev[i].flags))
+			break;
+	}
+	if (i >= sh->disks) {
+		atomic_inc(&sh->count);
+		set_bit(R5_Discard, &sh->dev[pd_idx].flags);
+		ops_complete_reconstruct(sh);
+		return;
+	}
 	/* check if prexor is active which means only process blocks
 	 * that are part of a read-modify-write (written)
 	 */
@@ -1285,10 +1304,24 @@ ops_run_reconstruct6(struct stripe_head *sh, struct raid5_percpu *percpu,
 {
 	struct async_submit_ctl submit;
 	struct page **blocks = percpu->scribble;
-	int count;
+	int count, i;
 
 	pr_debug("%s: stripe %llu\n", __func__, (unsigned long long)sh->sector);
 
+	for (i = 0; i < sh->disks; i++) {
+		if (sh->pd_idx == i || sh->qd_idx == i)
+			continue;
+		if (!test_bit(R5_Discard, &sh->dev[i].flags))
+			break;
+	}
+	if (i >= sh->disks) {
+		atomic_inc(&sh->count);
+		set_bit(R5_Discard, &sh->dev[sh->pd_idx].flags);
+		set_bit(R5_Discard, &sh->dev[sh->qd_idx].flags);
+		ops_complete_reconstruct(sh);
+		return;
+	}
+
 	count = set_syndrome_sources(blocks, sh);
 
 	atomic_inc(&sh->count);
@@ -2408,11 +2441,11 @@ static int add_stripe_bio(struct stripe_head *sh, struct bio *bi, int dd_idx, in
 		if (sector >= sh->dev[dd_idx].sector + STRIPE_SECTORS)
 			set_bit(R5_OVERWRITE, &sh->dev[dd_idx].flags);
 	}
-	spin_unlock_irq(&sh->stripe_lock);
 
 	pr_debug("added bi b#%llu to stripe s#%llu, disk %d.\n",
 		(unsigned long long)(*bip)->bi_sector,
 		(unsigned long long)sh->sector, dd_idx);
+	spin_unlock_irq(&sh->stripe_lock);
 
 	if (conf->mddev->bitmap && firstwrite) {
 		bitmap_startwrite(conf->mddev->bitmap, sh->sector,
@@ -2479,10 +2512,8 @@ handle_failed_stripe(struct r5conf *conf, struct stripe_head *sh,
 		bi = sh->dev[i].towrite;
 		sh->dev[i].towrite = NULL;
 		spin_unlock_irq(&sh->stripe_lock);
-		if (bi) {
-			s->to_write--;
+		if (bi)
 			bitmap_end = 1;
-		}
 
 		if (test_and_clear_bit(R5_Overlap, &sh->dev[i].flags))
 			wake_up(&conf->wait_for_overlap);
@@ -2524,11 +2555,12 @@ handle_failed_stripe(struct r5conf *conf, struct stripe_head *sh,
 		if (!test_bit(R5_Wantfill, &sh->dev[i].flags) &&
 		    (!test_bit(R5_Insync, &sh->dev[i].flags) ||
 		      test_bit(R5_ReadError, &sh->dev[i].flags))) {
+			spin_lock_irq(&sh->stripe_lock);
 			bi = sh->dev[i].toread;
 			sh->dev[i].toread = NULL;
+			spin_unlock_irq(&sh->stripe_lock);
 			if (test_and_clear_bit(R5_Overlap, &sh->dev[i].flags))
 				wake_up(&conf->wait_for_overlap);
-			if (bi) s->to_read--;
 			while (bi && bi->bi_sector <
 			       sh->dev[i].sector + STRIPE_SECTORS) {
 				struct bio *nextbi =
@@ -2741,7 +2773,8 @@ static void handle_stripe_clean_event(struct r5conf *conf,
 		if (sh->dev[i].written) {
 			dev = &sh->dev[i];
 			if (!test_bit(R5_LOCKED, &dev->flags) &&
-				test_bit(R5_UPTODATE, &dev->flags)) {
+			    (test_bit(R5_UPTODATE, &dev->flags) ||
+			     test_and_clear_bit(R5_Discard, &dev->flags))) {
 				/* We can return any write requests */
 				struct bio *wbi, *wbi2;
 				pr_debug("Return write for disc %d\n", i);
@@ -2775,12 +2808,25 @@ static void handle_stripe_dirtying(struct r5conf *conf,
 				   int disks)
 {
 	int rmw = 0, rcw = 0, i;
-	if (conf->max_degraded == 2) {
-		/* RAID6 requires 'rcw' in current implementation
-		 * Calculate the real rcw later - for now fake it
+	sector_t recovery_cp = conf->mddev->recovery_cp;
+
+	/* RAID6 requires 'rcw' in current implementation.
+	 * Otherwise, check whether resync is now happening or should start.
+	 * If yes, then the array is dirty (after unclean shutdown or
+	 * initial creation), so parity in some stripes might be inconsistent.
+	 * In this case, we need to always do reconstruct-write, to ensure
+	 * that in case of drive failure or read-error correction, we
+	 * generate correct data from the parity.
+	 */
+	if (conf->max_degraded == 2 ||
+	    (recovery_cp < MaxSector && sh->sector >= recovery_cp)) {
+		/* Calculate the real rcw later - for now make it
 		 * look like rcw is cheaper
 		 */
 		rcw = 1; rmw = 2;
+		pr_debug("force RCW max_degraded=%u, recovery_cp=%llu sh->sector=%llu\n",
+			 conf->max_degraded, (unsigned long long)recovery_cp,
+			 (unsigned long long)sh->sector);
 	} else for (i = disks; i--; ) {
 		/* would I have to read this buffer for read_modify_write */
 		struct r5dev *dev = &sh->dev[i];
@@ -2932,7 +2978,7 @@ static void handle_parity_checks5(struct r5conf *conf, struct stripe_head *sh,
 			 */
 			set_bit(STRIPE_INSYNC, &sh->state);
 		else {
-			conf->mddev->resync_mismatches += STRIPE_SECTORS;
+			atomic64_add(STRIPE_SECTORS, &conf->mddev->resync_mismatches);
 			if (test_bit(MD_RECOVERY_CHECK, &conf->mddev->recovery))
 				/* don't try to repair!! */
 				set_bit(STRIPE_INSYNC, &sh->state);
@@ -3084,7 +3130,7 @@ static void handle_parity_checks6(struct r5conf *conf, struct stripe_head *sh,
 				 */
 			}
 		} else {
-			conf->mddev->resync_mismatches += STRIPE_SECTORS;
+			atomic64_add(STRIPE_SECTORS, &conf->mddev->resync_mismatches);
 			if (test_bit(MD_RECOVERY_CHECK, &conf->mddev->recovery))
 				/* don't try to repair!! */
 				set_bit(STRIPE_INSYNC, &sh->state);
@@ -3459,10 +3505,12 @@ static void handle_stripe(struct stripe_head *sh)
 	if (s.written &&
 	    (s.p_failed || ((test_bit(R5_Insync, &pdev->flags)
 			     && !test_bit(R5_LOCKED, &pdev->flags)
-			     && test_bit(R5_UPTODATE, &pdev->flags)))) &&
+			     && (test_bit(R5_UPTODATE, &pdev->flags) ||
+				 test_bit(R5_Discard, &pdev->flags))))) &&
 	    (s.q_failed || ((test_bit(R5_Insync, &qdev->flags)
 			     && !test_bit(R5_LOCKED, &qdev->flags)
-			     && test_bit(R5_UPTODATE, &qdev->flags)))))
+			     && (test_bit(R5_UPTODATE, &qdev->flags) ||
+				 test_bit(R5_Discard, &qdev->flags))))))
 		handle_stripe_clean_event(conf, sh, disks, &s.return_bi);
 
 	/* Now we might consider reading some blocks, either to check/generate
@@ -3489,9 +3537,11 @@ static void handle_stripe(struct stripe_head *sh)
 		/* All the 'written' buffers and the parity block are ready to
 		 * be written back to disk
 		 */
-		BUG_ON(!test_bit(R5_UPTODATE, &sh->dev[sh->pd_idx].flags));
+		BUG_ON(!test_bit(R5_UPTODATE, &sh->dev[sh->pd_idx].flags) &&
+		       !test_bit(R5_Discard, &sh->dev[sh->pd_idx].flags));
 		BUG_ON(sh->qd_idx >= 0 &&
-		       !test_bit(R5_UPTODATE, &sh->dev[sh->qd_idx].flags));
+		       !test_bit(R5_UPTODATE, &sh->dev[sh->qd_idx].flags) &&
+		       !test_bit(R5_Discard, &sh->dev[sh->qd_idx].flags));
 		for (i = disks; i--; ) {
 			struct r5dev *dev = &sh->dev[i];
 			if (test_bit(R5_LOCKED, &dev->flags) &&
@@ -4072,6 +4122,88 @@ static void release_stripe_plug(struct mddev *mddev,
 		release_stripe(sh);
 }
 
+static void make_discard_request(struct mddev *mddev, struct bio *bi)
+{
+	struct r5conf *conf = mddev->private;
+	sector_t logical_sector, last_sector;
+	struct stripe_head *sh;
+	int remaining;
+	int stripe_sectors;
+
+	if (mddev->reshape_position != MaxSector)
+		/* Skip discard while reshape is happening */
+		return;
+
+	logical_sector = bi->bi_sector & ~((sector_t)STRIPE_SECTORS-1);
+	last_sector = bi->bi_sector + (bi->bi_size>>9);
+
+	bi->bi_next = NULL;
+	bi->bi_phys_segments = 1; /* over-loaded to count active stripes */
+
+	stripe_sectors = conf->chunk_sectors *
+		(conf->raid_disks - conf->max_degraded);
+	logical_sector = DIV_ROUND_UP_SECTOR_T(logical_sector,
+					       stripe_sectors);
+	sector_div(last_sector, stripe_sectors);
+
+	logical_sector *= conf->chunk_sectors;
+	last_sector *= conf->chunk_sectors;
+
+	for (; logical_sector < last_sector;
+	     logical_sector += STRIPE_SECTORS) {
+		DEFINE_WAIT(w);
+		int d;
+	again:
+		sh = get_active_stripe(conf, logical_sector, 0, 0, 0);
+		prepare_to_wait(&conf->wait_for_overlap, &w,
+				TASK_UNINTERRUPTIBLE);
+		spin_lock_irq(&sh->stripe_lock);
+		for (d = 0; d < conf->raid_disks; d++) {
+			if (d == sh->pd_idx || d == sh->qd_idx)
+				continue;
+			if (sh->dev[d].towrite || sh->dev[d].toread) {
+				set_bit(R5_Overlap, &sh->dev[d].flags);
+				spin_unlock_irq(&sh->stripe_lock);
+				release_stripe(sh);
+				schedule();
+				goto again;
+			}
+		}
+		finish_wait(&conf->wait_for_overlap, &w);
+		for (d = 0; d < conf->raid_disks; d++) {
+			if (d == sh->pd_idx || d == sh->qd_idx)
+				continue;
+			sh->dev[d].towrite = bi;
+			set_bit(R5_OVERWRITE, &sh->dev[d].flags);
+			raid5_inc_bi_active_stripes(bi);
+		}
+		spin_unlock_irq(&sh->stripe_lock);
+		if (conf->mddev->bitmap) {
+			for (d = 0;
+			     d < conf->raid_disks - conf->max_degraded;
+			     d++)
+				bitmap_startwrite(mddev->bitmap,
+						  sh->sector,
+						  STRIPE_SECTORS,
+						  0);
+			sh->bm_seq = conf->seq_flush + 1;
+			set_bit(STRIPE_BIT_DELAY, &sh->state);
+		}
+
+		set_bit(STRIPE_HANDLE, &sh->state);
+		clear_bit(STRIPE_DELAYED, &sh->state);
+		if (!test_and_set_bit(STRIPE_PREREAD_ACTIVE, &sh->state))
+			atomic_inc(&conf->preread_active_stripes);
+		release_stripe_plug(mddev, sh);
+	}
+
+	remaining = raid5_dec_bi_active_stripes(bi);
+	if (remaining == 0) {
+		md_write_end(mddev);
+		bio_endio(bi, 0);
+	}
+}
+
 static void make_request(struct mddev *mddev, struct bio * bi)
 {
 	struct r5conf *conf = mddev->private;
@@ -4094,6 +4226,11 @@ static void make_request(struct mddev *mddev, struct bio * bi)
 	     chunk_aligned_read(mddev,bi))
 		return;
 
+	if (unlikely(bi->bi_rw & REQ_DISCARD)) {
+		make_discard_request(mddev, bi);
+		return;
+	}
+
 	logical_sector = bi->bi_sector & ~((sector_t)STRIPE_SECTORS-1);
 	last_sector = bi->bi_sector + (bi->bi_size>>9);
 	bi->bi_next = NULL;
@@ -4630,8 +4767,9 @@ static int handle_active_stripes(struct r5conf *conf)
  * During the scan, completed stripes are saved for us by the interrupt
  * handler, so that they will not have to wait for our next wakeup.
  */
-static void raid5d(struct mddev *mddev)
+static void raid5d(struct md_thread *thread)
 {
+	struct mddev *mddev = thread->mddev;
 	struct r5conf *conf = mddev->private;
 	int handled;
 	struct blk_plug plug;
@@ -5366,6 +5504,7 @@ static int run(struct mddev *mddev)
 
 	if (mddev->queue) {
 		int chunk_size;
+		bool discard_supported = true;
 		/* read-ahead size must cover two whole stripes, which
 		 * is 2 * (datadisks) * chunksize where 'n' is the
 		 * number of raid devices
@@ -5385,13 +5524,48 @@ static int run(struct mddev *mddev)
 		blk_queue_io_min(mddev->queue, chunk_size);
 		blk_queue_io_opt(mddev->queue, chunk_size *
 				 (conf->raid_disks - conf->max_degraded));
+		/*
+		 * We can only discard a whole stripe. It doesn't make sense to
+		 * discard data disk but write parity disk
+		 */
+		stripe = stripe * PAGE_SIZE;
+		mddev->queue->limits.discard_alignment = stripe;
+		mddev->queue->limits.discard_granularity = stripe;
+		/*
+		 * unaligned part of discard request will be ignored, so can't
+		 * guarantee discard_zerors_data
+		 */
+		mddev->queue->limits.discard_zeroes_data = 0;
 
 		rdev_for_each(rdev, mddev) {
 			disk_stack_limits(mddev->gendisk, rdev->bdev,
 					  rdev->data_offset << 9);
 			disk_stack_limits(mddev->gendisk, rdev->bdev,
 					  rdev->new_data_offset << 9);
+			/*
+			 * discard_zeroes_data is required, otherwise data
+			 * could be lost. Consider a scenario: discard a stripe
+			 * (the stripe could be inconsistent if
+			 * discard_zeroes_data is 0); write one disk of the
+			 * stripe (the stripe could be inconsistent again
+			 * depending on which disks are used to calculate
+			 * parity); the disk is broken; The stripe data of this
+			 * disk is lost.
+			 */
+			if (!blk_queue_discard(bdev_get_queue(rdev->bdev)) ||
+			    !bdev_get_queue(rdev->bdev)->
+						limits.discard_zeroes_data)
+				discard_supported = false;
 		}
+
+		if (discard_supported &&
+		   mddev->queue->limits.max_discard_sectors >= stripe &&
+		   mddev->queue->limits.discard_granularity >= stripe)
+			queue_flag_set_unlocked(QUEUE_FLAG_DISCARD,
+						mddev->queue);
+		else
+			queue_flag_clear_unlocked(QUEUE_FLAG_DISCARD,
+						mddev->queue);
 	}
 
 	return 0;
@@ -5702,7 +5876,8 @@ static int check_reshape(struct mddev *mddev)
 	if (!check_stripe_cache(mddev))
 		return -ENOSPC;
 
-	return resize_stripes(conf, conf->raid_disks + mddev->delta_disks);
+	return resize_stripes(conf, (conf->previous_raid_disks
+				     + mddev->delta_disks));
 }
 
 static int raid5_start_reshape(struct mddev *mddev)
diff --git a/drivers/md/raid5.h b/drivers/md/raid5.h
index a9fc24901ed..18b2c4a8a1f 100644
--- a/drivers/md/raid5.h
+++ b/drivers/md/raid5.h
@@ -298,6 +298,7 @@ enum r5dev_flags {
 	R5_WantReplace, /* We need to update the replacement, we have read
 			 * data in, and now is a good time to write it out.
 			 */
+	R5_Discard,	/* Discard the stripe */
 };
 
 /*
diff --git a/drivers/media/dvb-core/dvb_frontend.c b/drivers/media/dvb-core/dvb_frontend.c
index 8f58f241c10..7e92793260f 100644
--- a/drivers/media/dvb-core/dvb_frontend.c
+++ b/drivers/media/dvb-core/dvb_frontend.c
@@ -966,6 +966,8 @@ static int dvb_frontend_clear_cache(struct dvb_frontend *fe)
 		break;
 	}
 
+	c->lna = LNA_AUTO;
+
 	return 0;
 }
 
@@ -1054,6 +1056,8 @@ static struct dtv_cmds_h dtv_cmds[DTV_MAX_COMMAND + 1] = {
 	_DTV_CMD(DTV_ATSCMH_SCCC_CODE_MODE_B, 0, 0),
 	_DTV_CMD(DTV_ATSCMH_SCCC_CODE_MODE_C, 0, 0),
 	_DTV_CMD(DTV_ATSCMH_SCCC_CODE_MODE_D, 0, 0),
+
+	_DTV_CMD(DTV_LNA, 0, 0),
 };
 
 static void dtv_property_dump(struct dvb_frontend *fe, struct dtv_property *tvp)
@@ -1440,6 +1444,10 @@ static int dtv_property_process_get(struct dvb_frontend *fe,
 		tvp->u.data = fe->dtv_property_cache.atscmh_sccc_code_mode_d;
 		break;
 
+	case DTV_LNA:
+		tvp->u.data = c->lna;
+		break;
+
 	default:
 		return -EINVAL;
 	}
@@ -1731,10 +1739,6 @@ static int dtv_property_process_set(struct dvb_frontend *fe,
 	case DTV_INTERLEAVING:
 		c->interleaving = tvp->u.data;
 		break;
-	case DTV_LNA:
-		if (fe->ops.set_lna)
-			r = fe->ops.set_lna(fe, tvp->u.data);
-		break;
 
 	/* ISDB-T Support here */
 	case DTV_ISDBT_PARTIAL_RECEPTION:
@@ -1806,6 +1810,12 @@ static int dtv_property_process_set(struct dvb_frontend *fe,
 		fe->dtv_property_cache.atscmh_rs_frame_ensemble = tvp->u.data;
 		break;
 
+	case DTV_LNA:
+		c->lna = tvp->u.data;
+		if (fe->ops.set_lna)
+			r = fe->ops.set_lna(fe);
+		break;
+
 	default:
 		return -EINVAL;
 	}
@@ -2309,7 +2319,7 @@ static int dvb_frontend_ioctl_legacy(struct file *file,
 		fepriv->tune_mode_flags = (unsigned long) parg;
 		err = 0;
 		break;
-	};
+	}
 
 	return err;
 }
diff --git a/drivers/media/dvb-core/dvb_frontend.h b/drivers/media/dvb-core/dvb_frontend.h
index 44a445cee74..97112cd88a1 100644
--- a/drivers/media/dvb-core/dvb_frontend.h
+++ b/drivers/media/dvb-core/dvb_frontend.h
@@ -303,7 +303,7 @@ struct dvb_frontend_ops {
 	int (*dishnetwork_send_legacy_command)(struct dvb_frontend* fe, unsigned long cmd);
 	int (*i2c_gate_ctrl)(struct dvb_frontend* fe, int enable);
 	int (*ts_bus_ctrl)(struct dvb_frontend* fe, int acquire);
-	int (*set_lna)(struct dvb_frontend *, int);
+	int (*set_lna)(struct dvb_frontend *);
 
 	/* These callbacks are for devices that implement their own
 	 * tuning algorithms, rather than a simple swzigzag
@@ -391,6 +391,8 @@ struct dtv_frontend_properties {
 	u8			atscmh_sccc_code_mode_b;
 	u8			atscmh_sccc_code_mode_c;
 	u8			atscmh_sccc_code_mode_d;
+
+	u32			lna;
 };
 
 struct dvb_frontend {
diff --git a/drivers/media/dvb-frontends/a8293.c b/drivers/media/dvb-frontends/a8293.c
index cff44a389b4..74fbb5d58be 100644
--- a/drivers/media/dvb-frontends/a8293.c
+++ b/drivers/media/dvb-frontends/a8293.c
@@ -90,7 +90,7 @@ static int a8293_set_voltage(struct dvb_frontend *fe,
 	default:
 		ret = -EINVAL;
 		goto err;
-	};
+	}
 
 	ret = a8293_wr(priv, &priv->reg[0], 1);
 	if (ret)
diff --git a/drivers/media/dvb-frontends/af9013.c b/drivers/media/dvb-frontends/af9013.c
index e9f04a36577..a204f282882 100644
--- a/drivers/media/dvb-frontends/af9013.c
+++ b/drivers/media/dvb-frontends/af9013.c
@@ -241,7 +241,7 @@ static int af9013_set_gpio(struct af9013_state *state, u8 gpio, u8 gpioval)
 				KBUILD_MODNAME, gpio);
 		ret = -EINVAL;
 		goto err;
-	};
+	}
 
 	switch (gpio) {
 	case 0:
@@ -253,7 +253,7 @@ static int af9013_set_gpio(struct af9013_state *state, u8 gpio, u8 gpioval)
 	default:
 		pos = 4;
 		break;
-	};
+	}
 
 	ret = af9013_wr_reg_bits(state, addr, pos, 4, gpioval);
 	if (ret)
@@ -726,7 +726,7 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
 	default:
 		dev_dbg(&state->i2c->dev, "%s: invalid hierarchy\n", __func__);
 		auto_mode = 1;
-	};
+	}
 
 	switch (c->modulation) {
 	case QAM_AUTO:
diff --git a/drivers/media/dvb-frontends/af9033.c b/drivers/media/dvb-frontends/af9033.c
index 8162d939c4b..464ad878490 100644
--- a/drivers/media/dvb-frontends/af9033.c
+++ b/drivers/media/dvb-frontends/af9033.c
@@ -408,7 +408,7 @@ static int af9033_set_frontend(struct dvb_frontend *fe)
 {
 	struct af9033_state *state = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	int ret, i, spec_inv;
+	int ret, i, spec_inv, sampling_freq;
 	u8 tmp, buf[3], bandwidth_reg_val;
 	u32 if_frequency, freq_cw, adc_freq;
 
@@ -465,18 +465,20 @@ static int af9033_set_frontend(struct dvb_frontend *fe)
 		else
 			if_frequency = 0;
 
-		while (if_frequency > (adc_freq / 2))
-			if_frequency -= adc_freq;
+		sampling_freq = if_frequency;
 
-		if (if_frequency >= 0)
+		while (sampling_freq > (adc_freq / 2))
+			sampling_freq -= adc_freq;
+
+		if (sampling_freq >= 0)
 			spec_inv *= -1;
 		else
-			if_frequency *= -1;
+			sampling_freq *= -1;
 
-		freq_cw = af9033_div(state, if_frequency, adc_freq, 23ul);
+		freq_cw = af9033_div(state, sampling_freq, adc_freq, 23ul);
 
 		if (spec_inv == -1)
-			freq_cw *= -1;
+			freq_cw = 0x800000 - freq_cw;
 
 		/* get adc multiplies */
 		ret = af9033_rd_reg(state, 0x800045, &tmp);
diff --git a/drivers/media/dvb-frontends/bcm3510.c b/drivers/media/dvb-frontends/bcm3510.c
index 033cd7ad3ca..1b77909c0c7 100644
--- a/drivers/media/dvb-frontends/bcm3510.c
+++ b/drivers/media/dvb-frontends/bcm3510.c
@@ -527,7 +527,7 @@ static int bcm3510_set_frontend(struct dvb_frontend *fe)
 			cmd.ACQUIRE1.IF_FREQ = 0x0;
 		default:
 			return -EINVAL;
-	};
+	}
 	cmd.ACQUIRE0.OFFSET = 0;
 	cmd.ACQUIRE0.NTSCSWEEP = 1;
 	cmd.ACQUIRE0.FA = 1;
diff --git a/drivers/media/dvb-frontends/cx24110.c b/drivers/media/dvb-frontends/cx24110.c
index 3180f5b2a6a..0cd6927e654 100644
--- a/drivers/media/dvb-frontends/cx24110.c
+++ b/drivers/media/dvb-frontends/cx24110.c
@@ -218,7 +218,7 @@ static int cx24110_set_fec (struct cx24110_state* state, fe_code_rate_t fec)
 	   } else
 		   return -EOPNOTSUPP;
 /* fixme (low): which is the correct return code? */
-	};
+	}
 	return 0;
 }
 
@@ -275,7 +275,7 @@ static int cx24110_set_symbolrate (struct cx24110_state* state, u32 srate)
 		cx24110_writereg(state,0x07,tmp|0x3);
 		cx24110_writereg(state,0x06,0x78);
 		fclk=90999000UL;
-	};
+	}
 	dprintk("cx24110 debug: fclk %d Hz\n",fclk);
 	/* we need to divide two integers with approx. 27 bits in 32 bit
 	   arithmetic giving a 25 bit result */
@@ -362,7 +362,7 @@ static int cx24110_initfe(struct dvb_frontend* fe)
 
 	for(i = 0; i < ARRAY_SIZE(cx24110_regdata); i++) {
 		cx24110_writereg(state, cx24110_regdata[i].reg, cx24110_regdata[i].data);
-	};
+	}
 
 	return 0;
 }
diff --git a/drivers/media/dvb-frontends/cxd2820r_core.c b/drivers/media/dvb-frontends/cxd2820r_core.c
index 42648643693..9b658c1cf39 100644
--- a/drivers/media/dvb-frontends/cxd2820r_core.c
+++ b/drivers/media/dvb-frontends/cxd2820r_core.c
@@ -688,7 +688,7 @@ struct dvb_frontend *cxd2820r_attach(const struct cxd2820r_config *cfg,
 {
 	struct cxd2820r_priv *priv;
 	int ret;
-	u8 tmp, gpio[GPIO_COUNT];
+	u8 tmp;
 
 	priv = kzalloc(sizeof(struct cxd2820r_priv), GFP_KERNEL);
 	if (!priv) {
@@ -735,6 +735,7 @@ struct dvb_frontend *cxd2820r_attach(const struct cxd2820r_config *cfg,
 		 * Use static GPIO configuration if GPIOLIB is undefined.
 		 * This is fallback condition.
 		 */
+		u8 gpio[GPIO_COUNT];
 		gpio[0] = (*gpio_chip_base >> 0) & 0x07;
 		gpio[1] = (*gpio_chip_base >> 3) & 0x07;
 		gpio[2] = 0;
diff --git a/drivers/media/dvb-frontends/drxd_hard.c b/drivers/media/dvb-frontends/drxd_hard.c
index f380eb43e9d..6d9853750d2 100644
--- a/drivers/media/dvb-frontends/drxd_hard.c
+++ b/drivers/media/dvb-frontends/drxd_hard.c
@@ -991,7 +991,7 @@ static int HI_Command(struct drxd_state *state, u16 cmd, u16 * pResult)
 		if (nrRetries > DRXD_MAX_RETRIES) {
 			status = -1;
 			break;
-		};
+		}
 		status = Read16(state, HI_RA_RAM_SRV_CMD__A, &waitCmd, 0);
 	} while (waitCmd != 0);
 
diff --git a/drivers/media/dvb-frontends/ds3000.c b/drivers/media/dvb-frontends/ds3000.c
index 4c8ac2657c4..5b639087ce4 100644
--- a/drivers/media/dvb-frontends/ds3000.c
+++ b/drivers/media/dvb-frontends/ds3000.c
@@ -30,6 +30,7 @@
 #include "ds3000.h"
 
 static int debug;
+static int force_fw_upload;
 
 #define dprintk(args...) \
 	do { \
@@ -392,11 +393,13 @@ static int ds3000_firmware_ondemand(struct dvb_frontend *fe)
 
 	dprintk("%s()\n", __func__);
 
-	if (ds3000_readreg(state, 0xb2) <= 0)
+	ret = ds3000_readreg(state, 0xb2);
+	if (ret < 0)
 		return ret;
 
-	if (state->skip_fw_load)
-		return 0;
+	if (state->skip_fw_load || !force_fw_upload)
+		return 0;	/* Firmware already uploaded, skipping */
+
 	/* Load firmware */
 	/* request the firmware, this will block until someone uploads it */
 	printk(KERN_INFO "%s: Waiting for firmware upload (%s)...\n", __func__,
@@ -1306,6 +1309,9 @@ static struct dvb_frontend_ops ds3000_ops = {
 module_param(debug, int, 0644);
 MODULE_PARM_DESC(debug, "Activates frontend debugging (default:0)");
 
+module_param(force_fw_upload, int, 0644);
+MODULE_PARM_DESC(force_fw_upload, "Force firmware upload (default:0)");
+
 MODULE_DESCRIPTION("DVB Frontend module for Montage Technology "
 			"DS3000/TS2020 hardware");
 MODULE_AUTHOR("Konstantin Dimitrov");
diff --git a/drivers/media/dvb-frontends/dvb_dummy_fe.c b/drivers/media/dvb-frontends/dvb_dummy_fe.c
index dcfc902c867..d5acc304786 100644
--- a/drivers/media/dvb-frontends/dvb_dummy_fe.c
+++ b/drivers/media/dvb-frontends/dvb_dummy_fe.c
@@ -121,16 +121,13 @@ struct dvb_frontend* dvb_dummy_fe_ofdm_attach(void)
 
 	/* allocate memory for the internal state */
 	state = kzalloc(sizeof(struct dvb_dummy_fe_state), GFP_KERNEL);
-	if (state == NULL) goto error;
+	if (!state)
+		return NULL;
 
 	/* create dvb_frontend */
 	memcpy(&state->frontend.ops, &dvb_dummy_fe_ofdm_ops, sizeof(struct dvb_frontend_ops));
 	state->frontend.demodulator_priv = state;
 	return &state->frontend;
-
-error:
-	kfree(state);
-	return NULL;
 }
 
 static struct dvb_frontend_ops dvb_dummy_fe_qpsk_ops;
@@ -141,16 +138,13 @@ struct dvb_frontend *dvb_dummy_fe_qpsk_attach(void)
 
 	/* allocate memory for the internal state */
 	state = kzalloc(sizeof(struct dvb_dummy_fe_state), GFP_KERNEL);
-	if (state == NULL) goto error;
+	if (!state)
+		return NULL;
 
 	/* create dvb_frontend */
 	memcpy(&state->frontend.ops, &dvb_dummy_fe_qpsk_ops, sizeof(struct dvb_frontend_ops));
 	state->frontend.demodulator_priv = state;
 	return &state->frontend;
-
-error:
-	kfree(state);
-	return NULL;
 }
 
 static struct dvb_frontend_ops dvb_dummy_fe_qam_ops;
@@ -161,16 +155,13 @@ struct dvb_frontend *dvb_dummy_fe_qam_attach(void)
 
 	/* allocate memory for the internal state */
 	state = kzalloc(sizeof(struct dvb_dummy_fe_state), GFP_KERNEL);
-	if (state == NULL) goto error;
+	if (!state)
+		return NULL;
 
 	/* create dvb_frontend */
 	memcpy(&state->frontend.ops, &dvb_dummy_fe_qam_ops, sizeof(struct dvb_frontend_ops));
 	state->frontend.demodulator_priv = state;
 	return &state->frontend;
-
-error:
-	kfree(state);
-	return NULL;
 }
 
 static struct dvb_frontend_ops dvb_dummy_fe_ofdm_ops = {
diff --git a/drivers/media/dvb-frontends/isl6405.c b/drivers/media/dvb-frontends/isl6405.c
index 33d33f4d886..0c642a5bf82 100644
--- a/drivers/media/dvb-frontends/isl6405.c
+++ b/drivers/media/dvb-frontends/isl6405.c
@@ -77,7 +77,7 @@ static int isl6405_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage
 			break;
 		default:
 			return -EINVAL;
-		};
+		}
 	}
 	isl6405->config |= isl6405->override_or;
 	isl6405->config &= isl6405->override_and;
diff --git a/drivers/media/dvb-frontends/isl6421.c b/drivers/media/dvb-frontends/isl6421.c
index 684c8ec166c..0cb3f0f74c9 100644
--- a/drivers/media/dvb-frontends/isl6421.c
+++ b/drivers/media/dvb-frontends/isl6421.c
@@ -63,7 +63,7 @@ static int isl6421_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage
 		break;
 	default:
 		return -EINVAL;
-	};
+	}
 
 	isl6421->config |= isl6421->override_or;
 	isl6421->config &= isl6421->override_and;
diff --git a/drivers/media/dvb-frontends/itd1000.c b/drivers/media/dvb-frontends/itd1000.c
index 316457584fe..c1c3400b217 100644
--- a/drivers/media/dvb-frontends/itd1000.c
+++ b/drivers/media/dvb-frontends/itd1000.c
@@ -231,7 +231,7 @@ static void itd1000_set_lo(struct itd1000_state *state, u32 freq_khz)
 	state->frequency = ((plln * 1000) + (pllf * 1000)/1048576) * 2*FREF;
 	itd_dbg("frequency: %dkHz (wanted) %dkHz (set), PLLF = %d, PLLN = %d\n", freq_khz, state->frequency, pllf, plln);
 
-	itd1000_write_reg(state, PLLNH, 0x80); /* PLLNH */;
+	itd1000_write_reg(state, PLLNH, 0x80); /* PLLNH */
 	itd1000_write_reg(state, PLLNL, plln & 0xff);
 	itd1000_write_reg(state, PLLFH, (itd1000_read_reg(state, PLLFH) & 0xf0) | ((pllf >> 16) & 0x0f));
 	itd1000_write_reg(state, PLLFM, (pllf >> 8) & 0xff);
diff --git a/drivers/media/dvb-frontends/lg2160.c b/drivers/media/dvb-frontends/lg2160.c
index cc11260e99d..5fd14f840ab 100644
--- a/drivers/media/dvb-frontends/lg2160.c
+++ b/drivers/media/dvb-frontends/lg2160.c
@@ -1421,8 +1421,8 @@ struct dvb_frontend *lg2160_attach(const struct lg2160_config *config,
 	       config ? config->i2c_addr : 0);
 
 	state = kzalloc(sizeof(struct lg216x_state), GFP_KERNEL);
-	if (state == NULL)
-		goto fail;
+	if (!state)
+		return NULL;
 
 	state->cfg = config;
 	state->i2c_adap = i2c_adap;
@@ -1449,10 +1449,6 @@ struct dvb_frontend *lg2160_attach(const struct lg2160_config *config,
 	state->frontend.dtv_property_cache.atscmh_parade_id = 1;
 
 	return &state->frontend;
-fail:
-	lg_warn("unable to detect LG216x hardware\n");
-	kfree(state);
-	return NULL;
 }
 EXPORT_SYMBOL(lg2160_attach);
 
diff --git a/drivers/media/dvb-frontends/lnbp21.c b/drivers/media/dvb-frontends/lnbp21.c
index 13437259eea..f3ba7b5faa2 100644
--- a/drivers/media/dvb-frontends/lnbp21.c
+++ b/drivers/media/dvb-frontends/lnbp21.c
@@ -65,7 +65,7 @@ static int lnbp21_set_voltage(struct dvb_frontend *fe,
 		break;
 	default:
 		return -EINVAL;
-	};
+	}
 
 	lnbp21->config |= lnbp21->override_or;
 	lnbp21->config &= lnbp21->override_and;
@@ -108,7 +108,7 @@ static int lnbp21_set_tone(struct dvb_frontend *fe,
 		break;
 	default:
 		return -EINVAL;
-	};
+	}
 
 	lnbp21->config |= lnbp21->override_or;
 	lnbp21->config &= lnbp21->override_and;
diff --git a/drivers/media/dvb-frontends/lnbp22.c b/drivers/media/dvb-frontends/lnbp22.c
index 84ad0390a4a..c463da7f6dc 100644
--- a/drivers/media/dvb-frontends/lnbp22.c
+++ b/drivers/media/dvb-frontends/lnbp22.c
@@ -73,7 +73,7 @@ static int lnbp22_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
 		break;
 	default:
 		return -EINVAL;
-	};
+	}
 
 	dprintk(1, "%s: 0x%02x)\n", __func__, lnbp22->config[3]);
 	return (i2c_transfer(lnbp22->i2c, &msg, 1) == 1) ? 0 : -EIO;
diff --git a/drivers/media/dvb-frontends/s5h1432.c b/drivers/media/dvb-frontends/s5h1432.c
index 8352ce1c955..6ec16a24374 100644
--- a/drivers/media/dvb-frontends/s5h1432.c
+++ b/drivers/media/dvb-frontends/s5h1432.c
@@ -351,8 +351,8 @@ struct dvb_frontend *s5h1432_attach(const struct s5h1432_config *config,
 	printk(KERN_INFO " Enter s5h1432_attach(). attach success!\n");
 	/* allocate memory for the internal state */
 	state = kmalloc(sizeof(struct s5h1432_state), GFP_KERNEL);
-	if (state == NULL)
-		goto error;
+	if (!state)
+		return NULL;
 
 	/* setup the state */
 	state->config = config;
@@ -367,10 +367,6 @@ struct dvb_frontend *s5h1432_attach(const struct s5h1432_config *config,
 	state->frontend.demodulator_priv = state;
 
 	return &state->frontend;
-
-error:
-	kfree(state);
-	return NULL;
 }
 EXPORT_SYMBOL(s5h1432_attach);
 
diff --git a/drivers/media/dvb-frontends/s921.c b/drivers/media/dvb-frontends/s921.c
index cd2288c0714..a271ac3eaec 100644
--- a/drivers/media/dvb-frontends/s921.c
+++ b/drivers/media/dvb-frontends/s921.c
@@ -487,9 +487,9 @@ struct dvb_frontend *s921_attach(const struct s921_config *config,
 		kzalloc(sizeof(struct s921_state), GFP_KERNEL);
 
 	dprintk("\n");
-	if (state == NULL) {
+	if (!state) {
 		rc("Unable to kzalloc\n");
-		goto rcor;
+		return NULL;
 	}
 
 	/* setup the state */
@@ -502,11 +502,6 @@ struct dvb_frontend *s921_attach(const struct s921_config *config,
 	state->frontend.demodulator_priv = state;
 
 	return &state->frontend;
-
-rcor:
-	kfree(state);
-
-	return NULL;
 }
 EXPORT_SYMBOL(s921_attach);
 
diff --git a/drivers/media/dvb-frontends/si21xx.c b/drivers/media/dvb-frontends/si21xx.c
index a68a64800df..73b47cc6a13 100644
--- a/drivers/media/dvb-frontends/si21xx.c
+++ b/drivers/media/dvb-frontends/si21xx.c
@@ -343,7 +343,7 @@ static int si21xx_wait_diseqc_idle(struct si21xx_state *state, int timeout)
 			return -ETIMEDOUT;
 		}
 		msleep(10);
-	};
+	}
 
 	return 0;
 }
@@ -472,7 +472,7 @@ static int si21xx_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t volt)
 		break;
 	default:
 		return -EINVAL;
-	};
+	}
 }
 
 static int si21xx_init(struct dvb_frontend *fe)
diff --git a/drivers/media/dvb-frontends/sp8870.c b/drivers/media/dvb-frontends/sp8870.c
index e37274c8f14..2aa8ef76eba 100644
--- a/drivers/media/dvb-frontends/sp8870.c
+++ b/drivers/media/dvb-frontends/sp8870.c
@@ -188,7 +188,7 @@ static int configure_reg0xc05 (struct dtv_frontend_properties *p, u16 *reg0xc05)
 		break;
 	default:
 		return -EINVAL;
-	};
+	}
 
 	switch (p->hierarchy) {
 	case HIERARCHY_NONE:
@@ -207,7 +207,7 @@ static int configure_reg0xc05 (struct dtv_frontend_properties *p, u16 *reg0xc05)
 		break;
 	default:
 		return -EINVAL;
-	};
+	}
 
 	switch (p->code_rate_HP) {
 	case FEC_1_2:
@@ -229,7 +229,7 @@ static int configure_reg0xc05 (struct dtv_frontend_properties *p, u16 *reg0xc05)
 		break;
 	default:
 		return -EINVAL;
-	};
+	}
 
 	if (known_parameters)
 		*reg0xc05 |= (2 << 1);	/* use specified parameters */
diff --git a/drivers/media/dvb-frontends/sp887x.c b/drivers/media/dvb-frontends/sp887x.c
index f4096ccb226..1bb81b5ae6e 100644
--- a/drivers/media/dvb-frontends/sp887x.c
+++ b/drivers/media/dvb-frontends/sp887x.c
@@ -229,7 +229,7 @@ static int configure_reg0xc05(struct dtv_frontend_properties *p, u16 *reg0xc05)
 		break;
 	default:
 		return -EINVAL;
-	};
+	}
 
 	switch (p->hierarchy) {
 	case HIERARCHY_NONE:
@@ -248,7 +248,7 @@ static int configure_reg0xc05(struct dtv_frontend_properties *p, u16 *reg0xc05)
 		break;
 	default:
 		return -EINVAL;
-	};
+	}
 
 	switch (p->code_rate_HP) {
 	case FEC_1_2:
@@ -270,7 +270,7 @@ static int configure_reg0xc05(struct dtv_frontend_properties *p, u16 *reg0xc05)
 		break;
 	default:
 		return -EINVAL;
-	};
+	}
 
 	if (known_parameters)
 		*reg0xc05 |= (2 << 1);	/* use specified parameters */
diff --git a/drivers/media/dvb-frontends/stb6100.c b/drivers/media/dvb-frontends/stb6100.c
index 2e93e65d2cd..45f9523f968 100644
--- a/drivers/media/dvb-frontends/stb6100.c
+++ b/drivers/media/dvb-frontends/stb6100.c
@@ -575,8 +575,8 @@ struct dvb_frontend *stb6100_attach(struct dvb_frontend *fe,
 	struct stb6100_state *state = NULL;
 
 	state = kzalloc(sizeof (struct stb6100_state), GFP_KERNEL);
-	if (state == NULL)
-		goto error;
+	if (!state)
+		return NULL;
 
 	state->config		= config;
 	state->i2c		= i2c;
@@ -587,10 +587,6 @@ struct dvb_frontend *stb6100_attach(struct dvb_frontend *fe,
 
 	printk("%s: Attaching STB6100 \n", __func__);
 	return fe;
-
-error:
-	kfree(state);
-	return NULL;
 }
 
 static int stb6100_release(struct dvb_frontend *fe)
diff --git a/drivers/media/dvb-frontends/stv0299.c b/drivers/media/dvb-frontends/stv0299.c
index 057b5f8effc..92a6075cd82 100644
--- a/drivers/media/dvb-frontends/stv0299.c
+++ b/drivers/media/dvb-frontends/stv0299.c
@@ -199,7 +199,7 @@ static int stv0299_wait_diseqc_fifo (struct stv0299_state* state, int timeout)
 			return -ETIMEDOUT;
 		}
 		msleep(10);
-	};
+	}
 
 	return 0;
 }
@@ -216,7 +216,7 @@ static int stv0299_wait_diseqc_idle (struct stv0299_state* state, int timeout)
 			return -ETIMEDOUT;
 		}
 		msleep(10);
-	};
+	}
 
 	return 0;
 }
@@ -387,7 +387,7 @@ static int stv0299_set_voltage (struct dvb_frontend* fe, fe_sec_voltage_t voltag
 		break;
 	default:
 		return -EINVAL;
-	};
+	}
 
 	if (state->config->op0_off)
 		reg0x0c &= ~0x10;
diff --git a/drivers/media/dvb-frontends/stv0900_core.c b/drivers/media/dvb-frontends/stv0900_core.c
index 7f1badaf0d0..262dfa503c2 100644
--- a/drivers/media/dvb-frontends/stv0900_core.c
+++ b/drivers/media/dvb-frontends/stv0900_core.c
@@ -1552,8 +1552,8 @@ static int stv0900_status(struct stv0900_internal *intp,
 		bitrate = (stv0900_get_mclk_freq(intp, intp->quartz)/1000000)
 			* (tsbitrate1_val << 8 | tsbitrate0_val);
 		bitrate /= 16384;
-		dprintk("TS bitrate = %d Mbit/sec \n", bitrate);
-	};
+		dprintk("TS bitrate = %d Mbit/sec\n", bitrate);
+	}
 
 	return locked;
 }
diff --git a/drivers/media/dvb-frontends/tda665x.c b/drivers/media/dvb-frontends/tda665x.c
index 2c1c759a4f4..63cc12378d9 100644
--- a/drivers/media/dvb-frontends/tda665x.c
+++ b/drivers/media/dvb-frontends/tda665x.c
@@ -228,8 +228,8 @@ struct dvb_frontend *tda665x_attach(struct dvb_frontend *fe,
 	struct dvb_tuner_info *info;
 
 	state = kzalloc(sizeof(struct tda665x_state), GFP_KERNEL);
-	if (state == NULL)
-		goto exit;
+	if (!state)
+		return NULL;
 
 	state->config		= config;
 	state->i2c		= i2c;
@@ -246,10 +246,6 @@ struct dvb_frontend *tda665x_attach(struct dvb_frontend *fe,
 	printk(KERN_DEBUG "%s: Attaching TDA665x (%s) tuner\n", __func__, info->name);
 
 	return fe;
-
-exit:
-	kfree(state);
-	return NULL;
 }
 EXPORT_SYMBOL(tda665x_attach);
 
diff --git a/drivers/media/dvb-frontends/tda8083.c b/drivers/media/dvb-frontends/tda8083.c
index 15912c96926..9d08350fe4b 100644
--- a/drivers/media/dvb-frontends/tda8083.c
+++ b/drivers/media/dvb-frontends/tda8083.c
@@ -175,7 +175,7 @@ static void tda8083_wait_diseqc_fifo (struct tda8083_state* state, int timeout)
 	       !(tda8083_readreg(state, 0x02) & 0x80))
 	{
 		msleep(50);
-	};
+	}
 }
 
 static int tda8083_set_tone (struct tda8083_state* state, fe_sec_tone_mode_t tone)
@@ -215,7 +215,7 @@ static int tda8083_send_diseqc_burst (struct tda8083_state* state, fe_sec_mini_c
 		break;
 	default:
 		return -EINVAL;
-	};
+	}
 
 	tda8083_wait_diseqc_fifo (state, 100);
 
diff --git a/drivers/media/i2c/cx25840/cx25840-core.c b/drivers/media/i2c/cx25840/cx25840-core.c
index d8eac3e30a7..2cee69e3418 100644
--- a/drivers/media/i2c/cx25840/cx25840-core.c
+++ b/drivers/media/i2c/cx25840/cx25840-core.c
@@ -599,7 +599,7 @@ static void cx23885_initialize(struct i2c_client *client)
 		cx25840_write4(client, 0x114, 0x01bf0c9e);
 		cx25840_write4(client, 0x110, 0x000a030c);
 		break;
-	};
+	}
 
 	/* ADC2 input select */
 	cx25840_write(client, 0x102, 0x10);
diff --git a/drivers/media/i2c/m5mols/m5mols.h b/drivers/media/i2c/m5mols/m5mols.h
index 86c815be348..90a6c520f11 100644
--- a/drivers/media/i2c/m5mols/m5mols.h
+++ b/drivers/media/i2c/m5mols/m5mols.h
@@ -16,9 +16,17 @@
 #ifndef M5MOLS_H
 #define M5MOLS_H
 
+#include <linux/sizes.h>
 #include <media/v4l2-subdev.h>
 #include "m5mols_reg.h"
 
+
+/* An amount of data transmitted in addition to the value
+ * determined by CAPP_JPEG_SIZE_MAX register.
+ */
+#define M5MOLS_JPEG_TAGS_SIZE		0x20000
+#define M5MOLS_MAIN_JPEG_SIZE_MAX	(5 * SZ_1M)
+
 extern int m5mols_debug;
 
 enum m5mols_restype {
@@ -67,12 +75,14 @@ struct m5mols_exif {
 /**
  * struct m5mols_capture - Structure for the capture capability
  * @exif: EXIF information
+ * @buf_size: internal JPEG frame buffer size, in bytes
  * @main: size in bytes of the main image
  * @thumb: size in bytes of the thumb image, if it was accompanied
  * @total: total size in bytes of the produced image
  */
 struct m5mols_capture {
 	struct m5mols_exif exif;
+	unsigned int buf_size;
 	u32 main;
 	u32 thumb;
 	u32 total;
diff --git a/drivers/media/i2c/m5mols/m5mols_capture.c b/drivers/media/i2c/m5mols/m5mols_capture.c
index cb243bd278c..ab34ccedf31 100644
--- a/drivers/media/i2c/m5mols/m5mols_capture.c
+++ b/drivers/media/i2c/m5mols/m5mols_capture.c
@@ -105,6 +105,7 @@ static int m5mols_capture_info(struct m5mols_info *info)
 
 int m5mols_start_capture(struct m5mols_info *info)
 {
+	unsigned int framesize = info->cap.buf_size - M5MOLS_JPEG_TAGS_SIZE;
 	struct v4l2_subdev *sd = &info->sd;
 	int ret;
 
@@ -121,6 +122,8 @@ int m5mols_start_capture(struct m5mols_info *info)
 	if (!ret)
 		ret = m5mols_write(sd, CAPP_MAIN_IMAGE_SIZE, info->resolution);
 	if (!ret)
+		ret = m5mols_write(sd, CAPP_JPEG_SIZE_MAX, framesize);
+	if (!ret)
 		ret = m5mols_set_mode(info, REG_CAPTURE);
 	if (!ret)
 		/* Wait until a frame is captured to ISP internal memory */
diff --git a/drivers/media/i2c/m5mols/m5mols_core.c b/drivers/media/i2c/m5mols/m5mols_core.c
index 2f490ef26c3..8131d651de9 100644
--- a/drivers/media/i2c/m5mols/m5mols_core.c
+++ b/drivers/media/i2c/m5mols/m5mols_core.c
@@ -599,6 +599,51 @@ static int m5mols_set_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 	return ret;
 }
 
+static int m5mols_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
+				 struct v4l2_mbus_frame_desc *fd)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+
+	if (pad != 0 || fd == NULL)
+		return -EINVAL;
+
+	mutex_lock(&info->lock);
+	/*
+	 * .get_frame_desc is only used for compressed formats,
+	 * thus we always return the capture frame parameters here.
+	 */
+	fd->entry[0].length = info->cap.buf_size;
+	fd->entry[0].pixelcode = info->ffmt[M5MOLS_RESTYPE_CAPTURE].code;
+	mutex_unlock(&info->lock);
+
+	fd->entry[0].flags = V4L2_MBUS_FRAME_DESC_FL_LEN_MAX;
+	fd->num_entries = 1;
+
+	return 0;
+}
+
+static int m5mols_set_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
+				 struct v4l2_mbus_frame_desc *fd)
+{
+	struct m5mols_info *info = to_m5mols(sd);
+	struct v4l2_mbus_framefmt *mf = &info->ffmt[M5MOLS_RESTYPE_CAPTURE];
+
+	if (pad != 0 || fd == NULL)
+		return -EINVAL;
+
+	fd->entry[0].flags = V4L2_MBUS_FRAME_DESC_FL_LEN_MAX;
+	fd->num_entries = 1;
+	fd->entry[0].length = clamp_t(u32, fd->entry[0].length,
+				      mf->width * mf->height,
+				      M5MOLS_MAIN_JPEG_SIZE_MAX);
+	mutex_lock(&info->lock);
+	info->cap.buf_size = fd->entry[0].length;
+	mutex_unlock(&info->lock);
+
+	return 0;
+}
+
+
 static int m5mols_enum_mbus_code(struct v4l2_subdev *sd,
 				 struct v4l2_subdev_fh *fh,
 				 struct v4l2_subdev_mbus_code_enum *code)
@@ -615,6 +660,8 @@ static struct v4l2_subdev_pad_ops m5mols_pad_ops = {
 	.enum_mbus_code	= m5mols_enum_mbus_code,
 	.get_fmt	= m5mols_get_fmt,
 	.set_fmt	= m5mols_set_fmt,
+	.get_frame_desc	= m5mols_get_frame_desc,
+	.set_frame_desc	= m5mols_set_frame_desc,
 };
 
 /**
diff --git a/drivers/media/i2c/m5mols/m5mols_reg.h b/drivers/media/i2c/m5mols/m5mols_reg.h
index 14d4be72aef..58d8027508d 100644
--- a/drivers/media/i2c/m5mols/m5mols_reg.h
+++ b/drivers/media/i2c/m5mols/m5mols_reg.h
@@ -310,6 +310,7 @@
 #define REG_JPEG		0x10
 
 #define CAPP_MAIN_IMAGE_SIZE	I2C_REG(CAT_CAPT_PARM, 0x01, 1)
+#define CAPP_JPEG_SIZE_MAX	I2C_REG(CAT_CAPT_PARM, 0x0f, 4)
 #define CAPP_JPEG_RATIO		I2C_REG(CAT_CAPT_PARM, 0x17, 1)
 
 #define CAPP_MCC_MODE		I2C_REG(CAT_CAPT_PARM, 0x1d, 1)
diff --git a/drivers/media/i2c/mt9p031.c b/drivers/media/i2c/mt9p031.c
index 2c0f4077c49..e32833262d3 100644
--- a/drivers/media/i2c/mt9p031.c
+++ b/drivers/media/i2c/mt9p031.c
@@ -574,7 +574,6 @@ static int mt9p031_set_crop(struct v4l2_subdev *subdev,
  * V4L2 subdev control operations
  */
 
-#define V4L2_CID_TEST_PATTERN		(V4L2_CID_USER_BASE | 0x1001)
 #define V4L2_CID_BLC_AUTO		(V4L2_CID_USER_BASE | 0x1002)
 #define V4L2_CID_BLC_TARGET_LEVEL	(V4L2_CID_USER_BASE | 0x1003)
 #define V4L2_CID_BLC_ANALOG_OFFSET	(V4L2_CID_USER_BASE | 0x1004)
@@ -740,18 +739,6 @@ static const char * const mt9p031_test_pattern_menu[] = {
 static const struct v4l2_ctrl_config mt9p031_ctrls[] = {
 	{
 		.ops		= &mt9p031_ctrl_ops,
-		.id		= V4L2_CID_TEST_PATTERN,
-		.type		= V4L2_CTRL_TYPE_MENU,
-		.name		= "Test Pattern",
-		.min		= 0,
-		.max		= ARRAY_SIZE(mt9p031_test_pattern_menu) - 1,
-		.step		= 0,
-		.def		= 0,
-		.flags		= 0,
-		.menu_skip_mask	= 0,
-		.qmenu		= mt9p031_test_pattern_menu,
-	}, {
-		.ops		= &mt9p031_ctrl_ops,
 		.id		= V4L2_CID_BLC_AUTO,
 		.type		= V4L2_CTRL_TYPE_BOOLEAN,
 		.name		= "BLC, Auto",
@@ -950,7 +937,7 @@ static int mt9p031_probe(struct i2c_client *client,
 	mt9p031->model = did->driver_data;
 	mt9p031->reset = -1;
 
-	v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 5);
+	v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 6);
 
 	v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
 			  V4L2_CID_EXPOSURE, MT9P031_SHUTTER_WIDTH_MIN,
@@ -966,6 +953,10 @@ static int mt9p031_probe(struct i2c_client *client,
 	v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
 			  V4L2_CID_PIXEL_RATE, pdata->target_freq,
 			  pdata->target_freq, 1, pdata->target_freq);
+	v4l2_ctrl_new_std_menu_items(&mt9p031->ctrls, &mt9p031_ctrl_ops,
+			  V4L2_CID_TEST_PATTERN,
+			  ARRAY_SIZE(mt9p031_test_pattern_menu) - 1, 0,
+			  0, mt9p031_test_pattern_menu);
 
 	for (i = 0; i < ARRAY_SIZE(mt9p031_ctrls); ++i)
 		v4l2_ctrl_new_custom(&mt9p031->ctrls, &mt9p031_ctrls[i], NULL);
diff --git a/drivers/media/i2c/mt9t001.c b/drivers/media/i2c/mt9t001.c
index 6d343adf891..2e189d8b71b 100644
--- a/drivers/media/i2c/mt9t001.c
+++ b/drivers/media/i2c/mt9t001.c
@@ -371,7 +371,7 @@ static int mt9t001_set_crop(struct v4l2_subdev *subdev,
  * V4L2 subdev control operations
  */
 
-#define V4L2_CID_TEST_PATTERN		(V4L2_CID_USER_BASE | 0x1001)
+#define V4L2_CID_TEST_PATTERN_COLOR	(V4L2_CID_USER_BASE | 0x1001)
 #define V4L2_CID_BLACK_LEVEL_AUTO	(V4L2_CID_USER_BASE | 0x1002)
 #define V4L2_CID_BLACK_LEVEL_OFFSET	(V4L2_CID_USER_BASE | 0x1003)
 #define V4L2_CID_BLACK_LEVEL_CALIBRATE	(V4L2_CID_USER_BASE | 0x1004)
@@ -487,12 +487,11 @@ static int mt9t001_s_ctrl(struct v4l2_ctrl *ctrl)
 				     ctrl->val >> 16);
 
 	case V4L2_CID_TEST_PATTERN:
-		ret = mt9t001_set_output_control(mt9t001,
+		return mt9t001_set_output_control(mt9t001,
 			ctrl->val ? 0 : MT9T001_OUTPUT_CONTROL_TEST_DATA,
 			ctrl->val ? MT9T001_OUTPUT_CONTROL_TEST_DATA : 0);
-		if (ret < 0)
-			return ret;
 
+	case V4L2_CID_TEST_PATTERN_COLOR:
 		return mt9t001_write(client, MT9T001_TEST_DATA, ctrl->val << 2);
 
 	case V4L2_CID_BLACK_LEVEL_AUTO:
@@ -533,12 +532,17 @@ static struct v4l2_ctrl_ops mt9t001_ctrl_ops = {
 	.s_ctrl = mt9t001_s_ctrl,
 };
 
+static const char * const mt9t001_test_pattern_menu[] = {
+	"Disabled",
+	"Enabled",
+};
+
 static const struct v4l2_ctrl_config mt9t001_ctrls[] = {
 	{
 		.ops		= &mt9t001_ctrl_ops,
-		.id		= V4L2_CID_TEST_PATTERN,
+		.id		= V4L2_CID_TEST_PATTERN_COLOR,
 		.type		= V4L2_CTRL_TYPE_INTEGER,
-		.name		= "Test pattern",
+		.name		= "Test Pattern Color",
 		.min		= 0,
 		.max		= 1023,
 		.step		= 1,
@@ -741,7 +745,7 @@ static int mt9t001_probe(struct i2c_client *client,
 		return -ENOMEM;
 
 	v4l2_ctrl_handler_init(&mt9t001->ctrls, ARRAY_SIZE(mt9t001_ctrls) +
-						ARRAY_SIZE(mt9t001_gains) + 3);
+						ARRAY_SIZE(mt9t001_gains) + 4);
 
 	v4l2_ctrl_new_std(&mt9t001->ctrls, &mt9t001_ctrl_ops,
 			  V4L2_CID_EXPOSURE, MT9T001_SHUTTER_WIDTH_MIN,
@@ -752,6 +756,10 @@ static int mt9t001_probe(struct i2c_client *client,
 	v4l2_ctrl_new_std(&mt9t001->ctrls, &mt9t001_ctrl_ops,
 			  V4L2_CID_PIXEL_RATE, pdata->ext_clk, pdata->ext_clk,
 			  1, pdata->ext_clk);
+	v4l2_ctrl_new_std_menu_items(&mt9t001->ctrls, &mt9t001_ctrl_ops,
+			V4L2_CID_TEST_PATTERN,
+			ARRAY_SIZE(mt9t001_test_pattern_menu) - 1, 0,
+			0, mt9t001_test_pattern_menu);
 
 	for (i = 0; i < ARRAY_SIZE(mt9t001_ctrls); ++i)
 		v4l2_ctrl_new_custom(&mt9t001->ctrls, &mt9t001_ctrls[i], NULL);
diff --git a/drivers/media/i2c/mt9v032.c b/drivers/media/i2c/mt9v032.c
index e2177405dad..3f356cb2825 100644
--- a/drivers/media/i2c/mt9v032.c
+++ b/drivers/media/i2c/mt9v032.c
@@ -141,6 +141,10 @@ struct mt9v032 {
 	u16 chip_control;
 	u16 aec_agc;
 	u16 hblank;
+	struct {
+		struct v4l2_ctrl *test_pattern;
+		struct v4l2_ctrl *test_pattern_color;
+	};
 };
 
 static struct mt9v032 *to_mt9v032(struct v4l2_subdev *sd)
@@ -500,7 +504,7 @@ static int mt9v032_set_crop(struct v4l2_subdev *subdev,
  * V4L2 subdev control operations
  */
 
-#define V4L2_CID_TEST_PATTERN		(V4L2_CID_USER_BASE | 0x1001)
+#define V4L2_CID_TEST_PATTERN_COLOR	(V4L2_CID_USER_BASE | 0x1001)
 
 static int mt9v032_s_ctrl(struct v4l2_ctrl *ctrl)
 {
@@ -545,7 +549,7 @@ static int mt9v032_s_ctrl(struct v4l2_ctrl *ctrl)
 		break;
 
 	case V4L2_CID_TEST_PATTERN:
-		switch (ctrl->val) {
+		switch (mt9v032->test_pattern->val) {
 		case 0:
 			data = 0;
 			break;
@@ -562,13 +566,13 @@ static int mt9v032_s_ctrl(struct v4l2_ctrl *ctrl)
 			     | MT9V032_TEST_PATTERN_ENABLE;
 			break;
 		default:
-			data = (ctrl->val << MT9V032_TEST_PATTERN_DATA_SHIFT)
+			data = (mt9v032->test_pattern_color->val <<
+				MT9V032_TEST_PATTERN_DATA_SHIFT)
 			     | MT9V032_TEST_PATTERN_USE_DATA
 			     | MT9V032_TEST_PATTERN_ENABLE
 			     | MT9V032_TEST_PATTERN_FLIP;
 			break;
 		}
-
 		return mt9v032_write(client, MT9V032_TEST_PATTERN, data);
 	}
 
@@ -579,18 +583,24 @@ static struct v4l2_ctrl_ops mt9v032_ctrl_ops = {
 	.s_ctrl = mt9v032_s_ctrl,
 };
 
-static const struct v4l2_ctrl_config mt9v032_ctrls[] = {
-	{
-		.ops		= &mt9v032_ctrl_ops,
-		.id		= V4L2_CID_TEST_PATTERN,
-		.type		= V4L2_CTRL_TYPE_INTEGER,
-		.name		= "Test pattern",
-		.min		= 0,
-		.max		= 1023,
-		.step		= 1,
-		.def		= 0,
-		.flags		= 0,
-	}
+static const char * const mt9v032_test_pattern_menu[] = {
+	"Disabled",
+	"Gray Vertical Shade",
+	"Gray Horizontal Shade",
+	"Gray Diagonal Shade",
+	"Plain",
+};
+
+static const struct v4l2_ctrl_config mt9v032_test_pattern_color = {
+	.ops		= &mt9v032_ctrl_ops,
+	.id		= V4L2_CID_TEST_PATTERN_COLOR,
+	.type		= V4L2_CTRL_TYPE_INTEGER,
+	.name		= "Test Pattern Color",
+	.min		= 0,
+	.max		= 1023,
+	.step		= 1,
+	.def		= 0,
+	.flags		= 0,
 };
 
 /* -----------------------------------------------------------------------------
@@ -741,7 +751,7 @@ static int mt9v032_probe(struct i2c_client *client,
 	mutex_init(&mt9v032->power_lock);
 	mt9v032->pdata = pdata;
 
-	v4l2_ctrl_handler_init(&mt9v032->ctrls, ARRAY_SIZE(mt9v032_ctrls) + 8);
+	v4l2_ctrl_handler_init(&mt9v032->ctrls, 10);
 
 	v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
 			  V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
@@ -763,6 +773,14 @@ static int mt9v032_probe(struct i2c_client *client,
 			  V4L2_CID_VBLANK, MT9V032_VERTICAL_BLANKING_MIN,
 			  MT9V032_VERTICAL_BLANKING_MAX, 1,
 			  MT9V032_VERTICAL_BLANKING_DEF);
+	mt9v032->test_pattern = v4l2_ctrl_new_std_menu_items(&mt9v032->ctrls,
+				&mt9v032_ctrl_ops, V4L2_CID_TEST_PATTERN,
+				ARRAY_SIZE(mt9v032_test_pattern_menu) - 1, 0, 0,
+				mt9v032_test_pattern_menu);
+	mt9v032->test_pattern_color = v4l2_ctrl_new_custom(&mt9v032->ctrls,
+				      &mt9v032_test_pattern_color, NULL);
+
+	v4l2_ctrl_cluster(2, &mt9v032->test_pattern);
 
 	mt9v032->pixel_rate =
 		v4l2_ctrl_new_std(&mt9v032->ctrls, &mt9v032_ctrl_ops,
@@ -784,8 +802,6 @@ static int mt9v032_probe(struct i2c_client *client,
 		v4l2_ctrl_cluster(2, &mt9v032->link_freq);
 	}
 
-	for (i = 0; i < ARRAY_SIZE(mt9v032_ctrls); ++i)
-		v4l2_ctrl_new_custom(&mt9v032->ctrls, &mt9v032_ctrls[i], NULL);
 
 	mt9v032->subdev.ctrl_handler = &mt9v032->ctrls;
 
diff --git a/drivers/media/i2c/soc_camera/ov2640.c b/drivers/media/i2c/soc_camera/ov2640.c
index 78ac5744cb5..d2d298b6354 100644
--- a/drivers/media/i2c/soc_camera/ov2640.c
+++ b/drivers/media/i2c/soc_camera/ov2640.c
@@ -684,6 +684,11 @@ static int ov2640_s_ctrl(struct v4l2_ctrl *ctrl)
 		&container_of(ctrl->handler, struct ov2640_priv, hdl)->subdev;
 	struct i2c_client  *client = v4l2_get_subdevdata(sd);
 	u8 val;
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, BANK_SEL, BANK_SEL_SENS);
+	if (ret < 0)
+		return ret;
 
 	switch (ctrl->id) {
 	case V4L2_CID_VFLIP:
diff --git a/drivers/media/i2c/ths7303.c b/drivers/media/i2c/ths7303.c
index e5c0eedebc5..c31cc04fffd 100644
--- a/drivers/media/i2c/ths7303.c
+++ b/drivers/media/i2c/ths7303.c
@@ -28,6 +28,18 @@
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-chip-ident.h>
 
+#define THS7303_CHANNEL_1	1
+#define THS7303_CHANNEL_2	2
+#define THS7303_CHANNEL_3	3
+
+enum ths7303_filter_mode {
+	THS7303_FILTER_MODE_480I_576I,
+	THS7303_FILTER_MODE_480P_576P,
+	THS7303_FILTER_MODE_720P_1080I,
+	THS7303_FILTER_MODE_1080P,
+	THS7303_FILTER_MODE_DISABLE
+};
+
 MODULE_DESCRIPTION("TI THS7303 video amplifier driver");
 MODULE_AUTHOR("Chaithrika U S");
 MODULE_LICENSE("GPL");
@@ -37,35 +49,96 @@ module_param(debug, int, 0644);
 MODULE_PARM_DESC(debug, "Debug level 0-1");
 
 /* following function is used to set ths7303 */
-static int ths7303_setvalue(struct v4l2_subdev *sd, v4l2_std_id std)
+int ths7303_setval(struct v4l2_subdev *sd, enum ths7303_filter_mode mode)
 {
+	u8 input_bias_chroma = 3;
+	u8 input_bias_luma = 3;
+	int disable = 0;
 	int err = 0;
-	u8 val;
-	struct i2c_client *client;
+	u8 val = 0;
+	u8 temp;
 
-	client = v4l2_get_subdevdata(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
-	if (std & (V4L2_STD_ALL & ~V4L2_STD_SECAM)) {
-		val = 0x02;
-		v4l2_dbg(1, debug, sd, "setting value for SDTV format\n");
-	} else {
-		val = 0x00;
-		v4l2_dbg(1, debug, sd, "disabling all channels\n");
+	if (!client)
+		return -EINVAL;
+
+	switch (mode) {
+	case THS7303_FILTER_MODE_1080P:
+		val = (3 << 6);
+		val |= (3 << 3);
+		break;
+	case THS7303_FILTER_MODE_720P_1080I:
+		val = (2 << 6);
+		val |= (2 << 3);
+		break;
+	case THS7303_FILTER_MODE_480P_576P:
+		val = (1 << 6);
+		val |= (1 << 3);
+		break;
+	case THS7303_FILTER_MODE_480I_576I:
+		break;
+	case THS7303_FILTER_MODE_DISABLE:
+		pr_info("mode disabled\n");
+		/* disable all channels */
+		disable = 1;
+	default:
+		/* disable all channels */
+		disable = 1;
 	}
+	/* Setup channel 2 - Luma - Green */
+	temp = val;
+	if (!disable)
+		val |= input_bias_luma;
+	err = i2c_smbus_write_byte_data(client, THS7303_CHANNEL_2, val);
+	if (err)
+		goto out;
 
-	err |= i2c_smbus_write_byte_data(client, 0x01, val);
-	err |= i2c_smbus_write_byte_data(client, 0x02, val);
-	err |= i2c_smbus_write_byte_data(client, 0x03, val);
+	/* setup two chroma channels */
+	if (!disable)
+		temp |= input_bias_chroma;
 
+	err = i2c_smbus_write_byte_data(client, THS7303_CHANNEL_1, temp);
 	if (err)
-		v4l2_err(sd, "write failed\n");
+		goto out;
 
+	err = i2c_smbus_write_byte_data(client, THS7303_CHANNEL_3, temp);
+	if (err)
+		goto out;
+	return err;
+out:
+	pr_info("write byte data failed\n");
 	return err;
 }
 
 static int ths7303_s_std_output(struct v4l2_subdev *sd, v4l2_std_id norm)
 {
-	return ths7303_setvalue(sd, norm);
+	if (norm & (V4L2_STD_ALL & ~V4L2_STD_SECAM))
+		return ths7303_setval(sd, THS7303_FILTER_MODE_480I_576I);
+	else
+		return ths7303_setval(sd, THS7303_FILTER_MODE_DISABLE);
+}
+
+/* for setting filter for HD output */
+static int ths7303_s_dv_timings(struct v4l2_subdev *sd,
+			       struct v4l2_dv_timings *dv_timings)
+{
+	u32 height = dv_timings->bt.height;
+	int interlaced = dv_timings->bt.interlaced;
+	int res = 0;
+
+	if (height == 1080 && !interlaced)
+		res = ths7303_setval(sd, THS7303_FILTER_MODE_1080P);
+	else if ((height == 720 && !interlaced) ||
+			(height == 1080 && interlaced))
+		res = ths7303_setval(sd, THS7303_FILTER_MODE_720P_1080I);
+	else if ((height == 480 || height == 576) && !interlaced)
+		res = ths7303_setval(sd, THS7303_FILTER_MODE_480P_576P);
+	else
+		/* disable all channels */
+		res = ths7303_setval(sd, THS7303_FILTER_MODE_DISABLE);
+
+	return res;
 }
 
 static int ths7303_g_chip_ident(struct v4l2_subdev *sd,
@@ -78,6 +151,7 @@ static int ths7303_g_chip_ident(struct v4l2_subdev *sd,
 
 static const struct v4l2_subdev_video_ops ths7303_video_ops = {
 	.s_std_output	= ths7303_s_std_output,
+	.s_dv_timings    = ths7303_s_dv_timings,
 };
 
 static const struct v4l2_subdev_core_ops ths7303_core_ops = {
@@ -107,7 +181,7 @@ static int ths7303_probe(struct i2c_client *client,
 
 	v4l2_i2c_subdev_init(sd, client, &ths7303_ops);
 
-	return ths7303_setvalue(sd, std_id);
+	return ths7303_s_std_output(sd, std_id);
 }
 
 static int ths7303_remove(struct i2c_client *client)
diff --git a/drivers/media/i2c/tvp514x.c b/drivers/media/i2c/tvp514x.c
index 1f3943bb87d..d5e10215a28 100644
--- a/drivers/media/i2c/tvp514x.c
+++ b/drivers/media/i2c/tvp514x.c
@@ -519,6 +519,12 @@ static int tvp514x_querystd(struct v4l2_subdev *sd, v4l2_std_id *std_id)
 
 	*std_id = V4L2_STD_UNKNOWN;
 
+	/* To query the standard the TVP514x must power on the ADCs. */
+	if (!decoder->streaming) {
+		tvp514x_s_stream(sd, 1);
+		msleep(LOCK_RETRY_DELAY);
+	}
+
 	/* query the current standard */
 	current_std = tvp514x_query_current_std(sd);
 	if (current_std == STD_INVALID)
@@ -625,25 +631,12 @@ static int tvp514x_s_routing(struct v4l2_subdev *sd,
 	int err;
 	enum tvp514x_input input_sel;
 	enum tvp514x_output output_sel;
-	u8 sync_lock_status, lock_mask;
-	int try_count = LOCK_RETRY_COUNT;
 
 	if ((input >= INPUT_INVALID) ||
 			(output >= OUTPUT_INVALID))
 		/* Index out of bound */
 		return -EINVAL;
 
-	/*
-	 * For the sequence streamon -> streamoff and again s_input
-	 * it fails to lock the signal, since streamoff puts TVP514x
-	 * into power off state which leads to failure in sub-sequent s_input.
-	 *
-	 * So power up the TVP514x device here, since it is important to lock
-	 * the signal at this stage.
-	 */
-	if (!decoder->streaming)
-		tvp514x_s_stream(sd, 1);
-
 	input_sel = input;
 	output_sel = output;
 
@@ -660,64 +653,6 @@ static int tvp514x_s_routing(struct v4l2_subdev *sd,
 
 	decoder->tvp514x_regs[REG_INPUT_SEL].val = input_sel;
 	decoder->tvp514x_regs[REG_OUTPUT_FORMATTER1].val = output_sel;
-
-	/* Clear status */
-	msleep(LOCK_RETRY_DELAY);
-	err =
-	    tvp514x_write_reg(sd, REG_CLEAR_LOST_LOCK, 0x01);
-	if (err)
-		return err;
-
-	switch (input_sel) {
-	case INPUT_CVBS_VI1A:
-	case INPUT_CVBS_VI1B:
-	case INPUT_CVBS_VI1C:
-	case INPUT_CVBS_VI2A:
-	case INPUT_CVBS_VI2B:
-	case INPUT_CVBS_VI2C:
-	case INPUT_CVBS_VI3A:
-	case INPUT_CVBS_VI3B:
-	case INPUT_CVBS_VI3C:
-	case INPUT_CVBS_VI4A:
-		lock_mask = STATUS_CLR_SUBCAR_LOCK_BIT |
-			STATUS_HORZ_SYNC_LOCK_BIT |
-			STATUS_VIRT_SYNC_LOCK_BIT;
-		break;
-
-	case INPUT_SVIDEO_VI2A_VI1A:
-	case INPUT_SVIDEO_VI2B_VI1B:
-	case INPUT_SVIDEO_VI2C_VI1C:
-	case INPUT_SVIDEO_VI2A_VI3A:
-	case INPUT_SVIDEO_VI2B_VI3B:
-	case INPUT_SVIDEO_VI2C_VI3C:
-	case INPUT_SVIDEO_VI4A_VI1A:
-	case INPUT_SVIDEO_VI4A_VI1B:
-	case INPUT_SVIDEO_VI4A_VI1C:
-	case INPUT_SVIDEO_VI4A_VI3A:
-	case INPUT_SVIDEO_VI4A_VI3B:
-	case INPUT_SVIDEO_VI4A_VI3C:
-		lock_mask = STATUS_HORZ_SYNC_LOCK_BIT |
-			STATUS_VIRT_SYNC_LOCK_BIT;
-		break;
-	/* Need to add other interfaces*/
-	default:
-		return -EINVAL;
-	}
-
-	while (try_count-- > 0) {
-		/* Allow decoder to sync up with new input */
-		msleep(LOCK_RETRY_DELAY);
-
-		sync_lock_status = tvp514x_read_reg(sd,
-				REG_STATUS1);
-		if (lock_mask == (sync_lock_status & lock_mask))
-			/* Input detected */
-			break;
-	}
-
-	if (try_count < 0)
-		return -EINVAL;
-
 	decoder->input = input;
 	decoder->output = output;
 
diff --git a/drivers/media/pci/bt8xx/bttv-driver.c b/drivers/media/pci/bt8xx/bttv-driver.c
index b68918c97f6..56c6c77793d 100644
--- a/drivers/media/pci/bt8xx/bttv-driver.c
+++ b/drivers/media/pci/bt8xx/bttv-driver.c
@@ -668,6 +668,12 @@ static const struct v4l2_queryctrl bttv_ctls[] = {
 		.default_value = 32768,
 		.type          = V4L2_CTRL_TYPE_INTEGER,
 	},{
+		.id            = V4L2_CID_COLOR_KILLER,
+		.name          = "Color killer",
+		.minimum       = 0,
+		.maximum       = 1,
+		.type          = V4L2_CTRL_TYPE_BOOLEAN,
+	}, {
 		.id            = V4L2_CID_HUE,
 		.name          = "Hue",
 		.minimum       = 0,
@@ -1474,6 +1480,9 @@ static int bttv_g_ctrl(struct file *file, void *priv,
 	case V4L2_CID_SATURATION:
 		c->value = btv->saturation;
 		break;
+	case V4L2_CID_COLOR_KILLER:
+		c->value = btv->opt_color_killer;
+		break;
 
 	case V4L2_CID_AUDIO_MUTE:
 	case V4L2_CID_AUDIO_VOLUME:
@@ -1526,7 +1535,6 @@ static int bttv_s_ctrl(struct file *file, void *f,
 					struct v4l2_control *c)
 {
 	int err;
-	int val;
 	struct bttv_fh *fh = f;
 	struct bttv *btv = fh->btv;
 
@@ -1547,6 +1555,16 @@ static int bttv_s_ctrl(struct file *file, void *f,
 	case V4L2_CID_SATURATION:
 		bt848_sat(btv, c->value);
 		break;
+	case V4L2_CID_COLOR_KILLER:
+		btv->opt_color_killer = c->value;
+		if (btv->opt_color_killer) {
+			btor(BT848_SCLOOP_CKILL, BT848_E_SCLOOP);
+			btor(BT848_SCLOOP_CKILL, BT848_O_SCLOOP);
+		} else {
+			btand(~BT848_SCLOOP_CKILL, BT848_E_SCLOOP);
+			btand(~BT848_SCLOOP_CKILL, BT848_O_SCLOOP);
+		}
+		break;
 	case V4L2_CID_AUDIO_MUTE:
 		audio_mute(btv, c->value);
 		/* fall through */
@@ -1564,9 +1582,13 @@ static int bttv_s_ctrl(struct file *file, void *f,
 
 	case V4L2_CID_PRIVATE_CHROMA_AGC:
 		btv->opt_chroma_agc = c->value;
-		val = btv->opt_chroma_agc ? BT848_SCLOOP_CAGC : 0;
-		btwrite(val, BT848_E_SCLOOP);
-		btwrite(val, BT848_O_SCLOOP);
+		if (btv->opt_chroma_agc) {
+			btor(BT848_SCLOOP_CAGC, BT848_E_SCLOOP);
+			btor(BT848_SCLOOP_CAGC, BT848_O_SCLOOP);
+		} else {
+			btand(~BT848_SCLOOP_CAGC, BT848_E_SCLOOP);
+			btand(~BT848_SCLOOP_CAGC, BT848_O_SCLOOP);
+		}
 		break;
 	case V4L2_CID_PRIVATE_COMBFILTER:
 		btv->opt_combfilter = c->value;
diff --git a/drivers/media/pci/bt8xx/bttvp.h b/drivers/media/pci/bt8xx/bttvp.h
index 70fd4f23f60..9ec0adba236 100644
--- a/drivers/media/pci/bt8xx/bttvp.h
+++ b/drivers/media/pci/bt8xx/bttvp.h
@@ -429,6 +429,7 @@ struct bttv {
 	int opt_lumafilter;
 	int opt_automute;
 	int opt_chroma_agc;
+	int opt_color_killer;
 	int opt_adc_crush;
 	int opt_vcr_hack;
 	int opt_whitecrush_upper;
diff --git a/drivers/media/pci/bt8xx/dst_ca.c b/drivers/media/pci/bt8xx/dst_ca.c
index ee3884fbc9c..7d96fab7d24 100644
--- a/drivers/media/pci/bt8xx/dst_ca.c
+++ b/drivers/media/pci/bt8xx/dst_ca.c
@@ -646,7 +646,7 @@ static long dst_ca_ioctl(struct file *file, unsigned int cmd, unsigned long ioct
 		dprintk(verbose, DST_CA_INFO, 1, " -->CA_SET_PID Success !");
 	default:
 		result = -EOPNOTSUPP;
-	};
+	}
  free_mem_and_exit:
 	kfree (p_ca_message);
 	kfree (p_ca_slot_info);
diff --git a/drivers/media/pci/cx23885/altera-ci.c b/drivers/media/pci/cx23885/altera-ci.c
index aee7f0dacff..495781ee471 100644
--- a/drivers/media/pci/cx23885/altera-ci.c
+++ b/drivers/media/pci/cx23885/altera-ci.c
@@ -416,7 +416,7 @@ static void netup_read_ci_status(struct work_struct *work)
 				DVB_CA_EN50221_POLL_CAM_READY : 0);
 		ci_dbg_print("%s: setting CI[1] status = 0x%x\n",
 				__func__, inter->state[1]->status);
-	};
+	}
 
 	if (inter->state[0] != NULL) {
 		inter->state[0]->status =
@@ -425,7 +425,7 @@ static void netup_read_ci_status(struct work_struct *work)
 				DVB_CA_EN50221_POLL_CAM_READY : 0);
 		ci_dbg_print("%s: setting CI[0] status = 0x%x\n",
 				__func__, inter->state[0]->status);
-	};
+	}
 }
 
 /* CI irq handler */
diff --git a/drivers/media/pci/cx23885/cimax2.c b/drivers/media/pci/cx23885/cimax2.c
index c9f15d6dec4..6617774a326 100644
--- a/drivers/media/pci/cx23885/cimax2.c
+++ b/drivers/media/pci/cx23885/cimax2.c
@@ -193,7 +193,7 @@ int netup_ci_op_cam(struct dvb_ca_en50221 *en50221, int slot,
 				0, &store, 1);
 		if (ret != 0)
 			return ret;
-	};
+	}
 	state->current_ci_flag = flag;
 
 	mutex_lock(&dev->gpio_lock);
diff --git a/drivers/media/pci/cx23885/cx23885-cards.c b/drivers/media/pci/cx23885/cx23885-cards.c
index 39a4a4b9ed7..5acdf954ff6 100644
--- a/drivers/media/pci/cx23885/cx23885-cards.c
+++ b/drivers/media/pci/cx23885/cx23885-cards.c
@@ -542,11 +542,13 @@ struct cx23885_board cx23885_boards[] = {
 			{
 				.type   = CX23885_VMUX_COMPOSITE1,
 				.vmux   = CX25840_COMPOSITE8,
+				.amux   = CX25840_AUDIO7,
 			},
 			{
 				.type   = CX23885_VMUX_SVIDEO,
 				.vmux   = CX25840_SVIDEO_LUMA3 |
 						CX25840_SVIDEO_CHROMA4,
+				.amux   = CX25840_AUDIO7,
 			},
 			{
 				.type   = CX23885_VMUX_COMPONENT,
@@ -554,6 +556,7 @@ struct cx23885_board cx23885_boards[] = {
 					CX25840_VIN1_CH1 |
 					CX25840_VIN6_CH2 |
 					CX25840_VIN7_CH3,
+				.amux   = CX25840_AUDIO7,
 			},
 		},
 	},
diff --git a/drivers/media/pci/cx23885/cx23885-video.c b/drivers/media/pci/cx23885/cx23885-video.c
index 8c4a9a5f9a5..1a21926ca41 100644
--- a/drivers/media/pci/cx23885/cx23885-video.c
+++ b/drivers/media/pci/cx23885/cx23885-video.c
@@ -508,7 +508,8 @@ static int cx23885_video_mux(struct cx23885_dev *dev, unsigned int input)
 		(dev->board == CX23885_BOARD_HAUPPAUGE_HVR1250) ||
 		(dev->board == CX23885_BOARD_HAUPPAUGE_HVR1255) ||
 		(dev->board == CX23885_BOARD_HAUPPAUGE_HVR1255_22111) ||
-		(dev->board == CX23885_BOARD_HAUPPAUGE_HVR1850)) {
+		(dev->board == CX23885_BOARD_HAUPPAUGE_HVR1850) ||
+		(dev->board == CX23885_BOARD_MYGICA_X8507)) {
 		/* Configure audio routing */
 		v4l2_subdev_call(dev->sd_cx25840, audio, s_routing,
 			INPUT(input)->amux, 0, 0);
diff --git a/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.c b/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.c
index c8c94fbf5d8..d33fc1a2303 100644
--- a/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.c
+++ b/drivers/media/pci/cx25821/cx25821-video-upstream-ch2.c
@@ -761,7 +761,7 @@ int cx25821_vidupstream_init_ch2(struct cx25821_dev *dev, int channel_select,
 	}
 
 	/* Default if filename is empty string */
-	if (strcmp(dev->input_filename_ch2, "") == 0) {
+	if (strcmp(dev->_filename_ch2, "") == 0) {
 		if (dev->_isNTSC_ch2) {
 			dev->_filename_ch2 = (dev->_pixel_format_ch2 ==
 				PIXEL_FRMT_411) ? "/root/vid411.yuv" :
diff --git a/drivers/media/pci/cx25821/cx25821-video-upstream.c b/drivers/media/pci/cx25821/cx25821-video-upstream.c
index 52c13e0b649..6759fff8eb6 100644
--- a/drivers/media/pci/cx25821/cx25821-video-upstream.c
+++ b/drivers/media/pci/cx25821/cx25821-video-upstream.c
@@ -808,7 +808,7 @@ int cx25821_vidupstream_init_ch1(struct cx25821_dev *dev, int channel_select,
 	}
 
 	/* Default if filename is empty string */
-	if (strcmp(dev->input_filename, "") == 0) {
+	if (strcmp(dev->_filename, "") == 0) {
 		if (dev->_isNTSC) {
 			dev->_filename =
 				(dev->_pixel_format == PIXEL_FRMT_411) ?
diff --git a/drivers/media/pci/cx88/cx88-blackbird.c b/drivers/media/pci/cx88/cx88-blackbird.c
index def363fb71c..62184eb919e 100644
--- a/drivers/media/pci/cx88/cx88-blackbird.c
+++ b/drivers/media/pci/cx88/cx88-blackbird.c
@@ -721,7 +721,7 @@ static int vidioc_g_fmt_vid_cap (struct file *file, void *priv,
 
 	f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
 	f->fmt.pix.bytesperline = 0;
-	f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */;
+	f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */
 	f->fmt.pix.colorspace   = V4L2_COLORSPACE_SMPTE170M;
 	f->fmt.pix.width        = dev->width;
 	f->fmt.pix.height       = dev->height;
@@ -739,7 +739,7 @@ static int vidioc_try_fmt_vid_cap (struct file *file, void *priv,
 
 	f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
 	f->fmt.pix.bytesperline = 0;
-	f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */;
+	f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */
 	f->fmt.pix.colorspace   = V4L2_COLORSPACE_SMPTE170M;
 	dprintk(1, "VIDIOC_TRY_FMT: w: %d, h: %d, f: %d\n",
 		dev->width, dev->height, fh->mpegq.field );
@@ -755,7 +755,7 @@ static int vidioc_s_fmt_vid_cap (struct file *file, void *priv,
 
 	f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
 	f->fmt.pix.bytesperline = 0;
-	f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */;
+	f->fmt.pix.sizeimage    = 188 * 4 * mpegbufs; /* 188 * 4 * 1024; */
 	f->fmt.pix.colorspace   = V4L2_COLORSPACE_SMPTE170M;
 	dev->width              = f->fmt.pix.width;
 	dev->height             = f->fmt.pix.height;
diff --git a/drivers/media/pci/cx88/cx88-dvb.c b/drivers/media/pci/cx88/cx88-dvb.c
index d803bba0952..666f83b2f3c 100644
--- a/drivers/media/pci/cx88/cx88-dvb.c
+++ b/drivers/media/pci/cx88/cx88-dvb.c
@@ -896,7 +896,7 @@ static int samsung_smt_7020_set_voltage(struct dvb_frontend *fe,
 		break;
 	default:
 		return -EINVAL;
-	};
+	}
 
 	return (i2c_transfer(&dev->core->i2c_adap, &msg, 1) == 1) ? 0 : -EIO;
 }
diff --git a/drivers/media/pci/cx88/cx88-mpeg.c b/drivers/media/pci/cx88/cx88-mpeg.c
index c04fb618e10..d154bc19735 100644
--- a/drivers/media/pci/cx88/cx88-mpeg.c
+++ b/drivers/media/pci/cx88/cx88-mpeg.c
@@ -450,7 +450,7 @@ static irqreturn_t cx8802_irq(int irq, void *dev_id)
 			cx88_core_irq(core,status);
 		if (status & PCI_INT_TSINT)
 			cx8802_mpeg_irq(dev);
-	};
+	}
 	if (MAX_IRQ_LOOP == loop) {
 		dprintk( 0, "clearing mask\n" );
 		printk(KERN_WARNING "%s/0: irq loop -- clearing mask\n",
diff --git a/drivers/media/pci/cx88/cx88-tvaudio.c b/drivers/media/pci/cx88/cx88-tvaudio.c
index 770ec05b5e9..424fd97495d 100644
--- a/drivers/media/pci/cx88/cx88-tvaudio.c
+++ b/drivers/media/pci/cx88/cx88-tvaudio.c
@@ -373,7 +373,7 @@ static void set_audio_standard_NICAM(struct cx88_core *core, u32 mode)
 		set_audio_registers(core, nicam_bgdki_common);
 		set_audio_registers(core, nicam_default);
 		break;
-	};
+	}
 
 	mode |= EN_DMTRX_LR | EN_DMTRX_BYPASS;
 	set_audio_finish(core, mode);
@@ -639,7 +639,7 @@ static void set_audio_standard_A2(struct cx88_core *core, u32 mode)
 		dprintk("%s Warning: wrong value\n", __func__);
 		return;
 		break;
-	};
+	}
 
 	mode |= EN_FMRADIO_EN_RDS | EN_DMTRX_SUMDIFF;
 	set_audio_finish(core, mode);
diff --git a/drivers/media/pci/cx88/cx88-video.c b/drivers/media/pci/cx88/cx88-video.c
index a146d50d779..05171457bf2 100644
--- a/drivers/media/pci/cx88/cx88-video.c
+++ b/drivers/media/pci/cx88/cx88-video.c
@@ -1535,7 +1535,7 @@ static irqreturn_t cx8800_irq(int irq, void *dev_id)
 			cx88_core_irq(core,status);
 		if (status & PCI_INT_VIDINT)
 			cx8800_vid_irq(dev);
-	};
+	}
 	if (10 == loop) {
 		printk(KERN_WARNING "%s/0: irq loop -- clearing mask\n",
 		       core->name);
diff --git a/drivers/media/pci/meye/meye.c b/drivers/media/pci/meye/meye.c
index 7bc775219f9..e5a76da8608 100644
--- a/drivers/media/pci/meye/meye.c
+++ b/drivers/media/pci/meye/meye.c
@@ -1647,7 +1647,7 @@ static int meye_mmap(struct file *file, struct vm_area_struct *vma)
 
 	vma->vm_ops = &meye_vm_ops;
 	vma->vm_flags &= ~VM_IO;	/* not I/O memory */
-	vma->vm_flags |= VM_RESERVED;	/* avoid to swap out this VMA */
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
 	vma->vm_private_data = (void *) (offset / gbufsize);
 	meye_vm_open(vma);
 
diff --git a/drivers/media/pci/saa7134/saa7134-video.c b/drivers/media/pci/saa7134/saa7134-video.c
index 22f8758d047..4a77124ee70 100644
--- a/drivers/media/pci/saa7134/saa7134-video.c
+++ b/drivers/media/pci/saa7134/saa7134-video.c
@@ -1204,7 +1204,7 @@ int saa7134_s_ctrl_internal(struct saa7134_dev *dev,  struct saa7134_fh *fh, str
 		break;
 	default:
 		/* nothing */;
-	};
+	}
 	switch (c->id) {
 	case V4L2_CID_BRIGHTNESS:
 		dev->ctl_bright = c->value;
diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index f588d6296c7..181c7686e41 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -165,12 +165,12 @@ config VIDEO_SAMSUNG_S5P_JPEG
 	  This is a v4l2 driver for Samsung S5P and EXYNOS4 JPEG codec
 
 config VIDEO_SAMSUNG_S5P_MFC
-	tristate "Samsung S5P MFC 5.1 Video Codec"
+	tristate "Samsung S5P MFC Video Codec"
 	depends on VIDEO_DEV && VIDEO_V4L2 && PLAT_S5P
 	select VIDEOBUF2_DMA_CONTIG
 	default n
 	help
-	    MFC 5.1 driver for V4L2.
+	    MFC 5.1 and 6.x driver for V4L2
 
 config VIDEO_MX2_EMMAPRP
 	tristate "MX2 eMMa-PrP support"
diff --git a/drivers/media/platform/davinci/vpbe.c b/drivers/media/platform/davinci/vpbe.c
index c4a82a1a8a9..69d7a58c92c 100644
--- a/drivers/media/platform/davinci/vpbe.c
+++ b/drivers/media/platform/davinci/vpbe.c
@@ -174,26 +174,6 @@ static int vpbe_get_current_mode_info(struct vpbe_device *vpbe_dev,
 	return 0;
 }
 
-static int vpbe_get_dv_preset_info(struct vpbe_device *vpbe_dev,
-				   unsigned int dv_preset)
-{
-	struct vpbe_config *cfg = vpbe_dev->cfg;
-	struct vpbe_enc_mode_info var;
-	int curr_output = vpbe_dev->current_out_index;
-	int i;
-
-	for (i = 0; i < vpbe_dev->cfg->outputs[curr_output].num_modes; i++) {
-		var = cfg->outputs[curr_output].modes[i];
-		if ((var.timings_type & VPBE_ENC_DV_PRESET) &&
-		  (var.timings.dv_preset == dv_preset)) {
-			vpbe_dev->current_timings = var;
-			return 0;
-		}
-	}
-
-	return -EINVAL;
-}
-
 /* Get std by std id */
 static int vpbe_get_std_info(struct vpbe_device *vpbe_dev,
 			     v4l2_std_id std_id)
@@ -206,7 +186,7 @@ static int vpbe_get_std_info(struct vpbe_device *vpbe_dev,
 	for (i = 0; i < vpbe_dev->cfg->outputs[curr_output].num_modes; i++) {
 		var = cfg->outputs[curr_output].modes[i];
 		if ((var.timings_type & VPBE_ENC_STD) &&
-		  (var.timings.std_id & std_id)) {
+		  (var.std_id & std_id)) {
 			vpbe_dev->current_timings = var;
 			return 0;
 		}
@@ -344,38 +324,42 @@ static unsigned int vpbe_get_output(struct vpbe_device *vpbe_dev)
 }
 
 /**
- * vpbe_s_dv_preset - Set the given preset timings in the encoder
+ * vpbe_s_dv_timings - Set the given preset timings in the encoder
  *
- * Sets the preset if supported by the current encoder. Return the status.
+ * Sets the timings if supported by the current encoder. Return the status.
  * 0 - success & -EINVAL on error
  */
-static int vpbe_s_dv_preset(struct vpbe_device *vpbe_dev,
-		     struct v4l2_dv_preset *dv_preset)
+static int vpbe_s_dv_timings(struct vpbe_device *vpbe_dev,
+		    struct v4l2_dv_timings *dv_timings)
 {
 	struct vpbe_config *cfg = vpbe_dev->cfg;
 	int out_index = vpbe_dev->current_out_index;
+	struct vpbe_output *output = &cfg->outputs[out_index];
 	int sd_index = vpbe_dev->current_sd_index;
-	int ret;
+	int ret, i;
 
 
 	if (!(cfg->outputs[out_index].output.capabilities &
-	    V4L2_OUT_CAP_PRESETS))
+	    V4L2_OUT_CAP_DV_TIMINGS))
 		return -EINVAL;
 
-	ret = vpbe_get_dv_preset_info(vpbe_dev, dv_preset->preset);
-
-	if (ret)
-		return ret;
-
+	for (i = 0; i < output->num_modes; i++) {
+		if (output->modes[i].timings_type == VPBE_ENC_CUSTOM_TIMINGS &&
+		    !memcmp(&output->modes[i].dv_timings,
+				dv_timings, sizeof(*dv_timings)))
+			break;
+	}
+	if (i >= output->num_modes)
+		return -EINVAL;
+	vpbe_dev->current_timings = output->modes[i];
 	mutex_lock(&vpbe_dev->lock);
 
-
 	ret = v4l2_subdev_call(vpbe_dev->encoders[sd_index], video,
-					s_dv_preset, dv_preset);
+					s_dv_timings, dv_timings);
 	if (!ret && (vpbe_dev->amp != NULL)) {
 		/* Call amplifier subdevice */
 		ret = v4l2_subdev_call(vpbe_dev->amp, video,
-				s_dv_preset, dv_preset);
+				s_dv_timings, dv_timings);
 	}
 	/* set the lcd controller output for the given mode */
 	if (!ret) {
@@ -392,17 +376,17 @@ static int vpbe_s_dv_preset(struct vpbe_device *vpbe_dev,
 }
 
 /**
- * vpbe_g_dv_preset - Get the preset in the current encoder
+ * vpbe_g_dv_timings - Get the timings in the current encoder
  *
- * Get the preset in the current encoder. Return the status. 0 - success
+ * Get the timings in the current encoder. Return the status. 0 - success
  * -EINVAL on error
  */
-static int vpbe_g_dv_preset(struct vpbe_device *vpbe_dev,
-		     struct v4l2_dv_preset *dv_preset)
+static int vpbe_g_dv_timings(struct vpbe_device *vpbe_dev,
+		     struct v4l2_dv_timings *dv_timings)
 {
 	if (vpbe_dev->current_timings.timings_type &
-	  VPBE_ENC_DV_PRESET) {
-		dv_preset->preset = vpbe_dev->current_timings.timings.dv_preset;
+	  VPBE_ENC_CUSTOM_TIMINGS) {
+		*dv_timings = vpbe_dev->current_timings.dv_timings;
 		return 0;
 	}
 
@@ -410,13 +394,13 @@ static int vpbe_g_dv_preset(struct vpbe_device *vpbe_dev,
 }
 
 /**
- * vpbe_enum_dv_presets - Enumerate the dv presets in the current encoder
+ * vpbe_enum_dv_timings - Enumerate the dv timings in the current encoder
  *
- * Get the preset in the current encoder. Return the status. 0 - success
+ * Get the timings in the current encoder. Return the status. 0 - success
  * -EINVAL on error
  */
-static int vpbe_enum_dv_presets(struct vpbe_device *vpbe_dev,
-			 struct v4l2_dv_enum_preset *preset_info)
+static int vpbe_enum_dv_timings(struct vpbe_device *vpbe_dev,
+			 struct v4l2_enum_dv_timings *timings)
 {
 	struct vpbe_config *cfg = vpbe_dev->cfg;
 	int out_index = vpbe_dev->current_out_index;
@@ -424,12 +408,12 @@ static int vpbe_enum_dv_presets(struct vpbe_device *vpbe_dev,
 	int j = 0;
 	int i;
 
-	if (!(output->output.capabilities & V4L2_OUT_CAP_PRESETS))
+	if (!(output->output.capabilities & V4L2_OUT_CAP_DV_TIMINGS))
 		return -EINVAL;
 
 	for (i = 0; i < output->num_modes; i++) {
-		if (output->modes[i].timings_type == VPBE_ENC_DV_PRESET) {
-			if (j == preset_info->index)
+		if (output->modes[i].timings_type == VPBE_ENC_CUSTOM_TIMINGS) {
+			if (j == timings->index)
 				break;
 			j++;
 		}
@@ -437,9 +421,8 @@ static int vpbe_enum_dv_presets(struct vpbe_device *vpbe_dev,
 
 	if (i == output->num_modes)
 		return -EINVAL;
-
-	return v4l_fill_dv_preset_info(output->modes[i].timings.dv_preset,
-					preset_info);
+	timings->timings = output->modes[i].dv_timings;
+	return 0;
 }
 
 /**
@@ -489,10 +472,10 @@ static int vpbe_s_std(struct vpbe_device *vpbe_dev, v4l2_std_id *std_id)
  */
 static int vpbe_g_std(struct vpbe_device *vpbe_dev, v4l2_std_id *std_id)
 {
-	struct vpbe_enc_mode_info cur_timings = vpbe_dev->current_timings;
+	struct vpbe_enc_mode_info *cur_timings = &vpbe_dev->current_timings;
 
-	if (cur_timings.timings_type & VPBE_ENC_STD) {
-		*std_id = cur_timings.timings.std_id;
+	if (cur_timings->timings_type & VPBE_ENC_STD) {
+		*std_id = cur_timings->std_id;
 		return 0;
 	}
 
@@ -511,7 +494,7 @@ static int vpbe_set_mode(struct vpbe_device *vpbe_dev,
 {
 	struct vpbe_enc_mode_info *preset_mode = NULL;
 	struct vpbe_config *cfg = vpbe_dev->cfg;
-	struct v4l2_dv_preset dv_preset;
+	struct v4l2_dv_timings dv_timings;
 	struct osd_state *osd_device;
 	int out_index = vpbe_dev->current_out_index;
 	int ret = 0;
@@ -530,11 +513,12 @@ static int vpbe_set_mode(struct vpbe_device *vpbe_dev,
 			 */
 			if (preset_mode->timings_type & VPBE_ENC_STD)
 				return vpbe_s_std(vpbe_dev,
-						 &preset_mode->timings.std_id);
-			if (preset_mode->timings_type & VPBE_ENC_DV_PRESET) {
-				dv_preset.preset =
-					preset_mode->timings.dv_preset;
-				return vpbe_s_dv_preset(vpbe_dev, &dv_preset);
+						 &preset_mode->std_id);
+			if (preset_mode->timings_type &
+						VPBE_ENC_CUSTOM_TIMINGS) {
+				dv_timings =
+					preset_mode->dv_timings;
+				return vpbe_s_dv_timings(vpbe_dev, &dv_timings);
 			}
 		}
 	}
@@ -626,11 +610,11 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
 		vpbe_dev->dac_clk = clk_get(vpbe_dev->pdev, "vpss_dac");
 		if (IS_ERR(vpbe_dev->dac_clk)) {
 			ret =  PTR_ERR(vpbe_dev->dac_clk);
-			goto vpbe_unlock;
+			goto fail_mutex_unlock;
 		}
 		if (clk_enable(vpbe_dev->dac_clk)) {
 			ret =  -ENODEV;
-			goto vpbe_unlock;
+			goto fail_mutex_unlock;
 		}
 	}
 
@@ -642,7 +626,7 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
 	if (ret) {
 		v4l2_err(dev->driver,
 			"Unable to register v4l2 device.\n");
-		goto vpbe_fail_clock;
+		goto fail_clk_put;
 	}
 	v4l2_info(&vpbe_dev->v4l2_dev, "vpbe v4l2 device registered\n");
 
@@ -658,7 +642,7 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
 		v4l2_err(&vpbe_dev->v4l2_dev,
 			"vpbe unable to init venc sub device\n");
 		ret = -ENODEV;
-		goto vpbe_fail_v4l2_device;
+		goto fail_dev_unregister;
 	}
 	/* initialize osd device */
 	osd_device = vpbe_dev->osd_device;
@@ -669,7 +653,7 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
 			v4l2_err(&vpbe_dev->v4l2_dev,
 				 "unable to initialize the OSD device");
 			err = -ENOMEM;
-			goto vpbe_fail_v4l2_device;
+			goto fail_dev_unregister;
 		}
 	}
 
@@ -685,7 +669,7 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
 		v4l2_err(&vpbe_dev->v4l2_dev,
 			"unable to allocate memory for encoders sub devices");
 		ret = -ENOMEM;
-		goto vpbe_fail_v4l2_device;
+		goto fail_dev_unregister;
 	}
 
 	i2c_adap = i2c_get_adapter(vpbe_dev->cfg->i2c_adapter_id);
@@ -711,7 +695,7 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
 					 " failed to register",
 					 enc_info->module_name);
 				ret = -ENODEV;
-				goto vpbe_fail_sd_register;
+				goto fail_kfree_encoders;
 			}
 		} else
 			v4l2_warn(&vpbe_dev->v4l2_dev, "non-i2c encoders"
@@ -730,7 +714,7 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
 					 "amplifier %s failed to register",
 					 amp_info->module_name);
 				ret = -ENODEV;
-				goto vpbe_fail_amp_register;
+				goto fail_kfree_encoders;
 			}
 			v4l2_info(&vpbe_dev->v4l2_dev,
 					  "v4l2 sub device %s registered\n",
@@ -770,16 +754,14 @@ static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
 	/* TBD handling of bootargs for default output and mode */
 	return 0;
 
-vpbe_fail_amp_register:
-	kfree(vpbe_dev->amp);
-vpbe_fail_sd_register:
+fail_kfree_encoders:
 	kfree(vpbe_dev->encoders);
-vpbe_fail_v4l2_device:
+fail_dev_unregister:
 	v4l2_device_unregister(&vpbe_dev->v4l2_dev);
-vpbe_fail_clock:
+fail_clk_put:
 	if (strcmp(vpbe_dev->cfg->module_name, "dm644x-vpbe-display") != 0)
 		clk_put(vpbe_dev->dac_clk);
-vpbe_unlock:
+fail_mutex_unlock:
 	mutex_unlock(&vpbe_dev->lock);
 	return ret;
 }
@@ -810,9 +792,9 @@ static struct vpbe_device_ops vpbe_dev_ops = {
 	.enum_outputs = vpbe_enum_outputs,
 	.set_output = vpbe_set_output,
 	.get_output = vpbe_get_output,
-	.s_dv_preset = vpbe_s_dv_preset,
-	.g_dv_preset = vpbe_g_dv_preset,
-	.enum_dv_presets = vpbe_enum_dv_presets,
+	.s_dv_timings = vpbe_s_dv_timings,
+	.g_dv_timings = vpbe_g_dv_timings,
+	.enum_dv_timings = vpbe_enum_dv_timings,
 	.s_std = vpbe_s_std,
 	.g_std = vpbe_g_std,
 	.initialize = vpbe_initialize,
diff --git a/drivers/media/platform/davinci/vpbe_display.c b/drivers/media/platform/davinci/vpbe_display.c
index 239f37bfa31..161c77650e2 100644
--- a/drivers/media/platform/davinci/vpbe_display.c
+++ b/drivers/media/platform/davinci/vpbe_display.c
@@ -393,7 +393,7 @@ vpbe_disp_calculate_scale_factor(struct vpbe_display *disp_dev,
 	int h_scale;
 	int v_scale;
 
-	v4l2_std_id standard_id = vpbe_dev->current_timings.timings.std_id;
+	v4l2_std_id standard_id = vpbe_dev->current_timings.std_id;
 
 	/*
 	 * Application initially set the image format. Current display
@@ -637,7 +637,7 @@ static int vpbe_display_s_crop(struct file *file, void *priv,
 	struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
 	struct osd_layer_config *cfg = &layer->layer_info.config;
 	struct osd_state *osd_device = disp_dev->osd_device;
-	struct v4l2_rect *rect = &crop->c;
+	struct v4l2_rect rect = crop->c;
 	int ret;
 
 	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
@@ -648,21 +648,21 @@ static int vpbe_display_s_crop(struct file *file, void *priv,
 		return -EINVAL;
 	}
 
-	if (rect->top < 0)
-		rect->top = 0;
-	if (rect->left < 0)
-		rect->left = 0;
+	if (rect.top < 0)
+		rect.top = 0;
+	if (rect.left < 0)
+		rect.left = 0;
 
-	vpbe_disp_check_window_params(disp_dev, rect);
+	vpbe_disp_check_window_params(disp_dev, &rect);
 
 	osd_device->ops.get_layer_config(osd_device,
 			layer->layer_info.id, cfg);
 
 	vpbe_disp_calculate_scale_factor(disp_dev, layer,
-					rect->width,
-					rect->height);
-	vpbe_disp_adj_position(disp_dev, layer, rect->top,
-					rect->left);
+					rect.width,
+					rect.height);
+	vpbe_disp_adj_position(disp_dev, layer, rect.top,
+					rect.left);
 	ret = osd_device->ops.set_layer_config(osd_device,
 				layer->layer_info.id, cfg);
 	if (ret < 0) {
@@ -943,7 +943,7 @@ static int vpbe_display_g_std(struct file *file, void *priv,
 
 	/* Get the standard from the current encoder */
 	if (vpbe_dev->current_timings.timings_type & VPBE_ENC_STD) {
-		*std_id = vpbe_dev->current_timings.timings.std_id;
+		*std_id = vpbe_dev->current_timings.std_id;
 		return 0;
 	}
 
@@ -1029,29 +1029,29 @@ static int vpbe_display_g_output(struct file *file, void *priv,
 }
 
 /**
- * vpbe_display_enum_dv_presets - Enumerate the dv presets
+ * vpbe_display_enum_dv_timings - Enumerate the dv timings
  *
- * enum the preset in the current encoder. Return the status. 0 - success
+ * enum the timings in the current encoder. Return the status. 0 - success
  * -EINVAL on error
  */
 static int
-vpbe_display_enum_dv_presets(struct file *file, void *priv,
-			struct v4l2_dv_enum_preset *preset)
+vpbe_display_enum_dv_timings(struct file *file, void *priv,
+			struct v4l2_enum_dv_timings *timings)
 {
 	struct vpbe_fh *fh = priv;
 	struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
 	int ret;
 
-	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_ENUM_DV_PRESETS\n");
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_ENUM_DV_TIMINGS\n");
 
 	/* Enumerate outputs */
-	if (NULL == vpbe_dev->ops.enum_dv_presets)
+	if (NULL == vpbe_dev->ops.enum_dv_timings)
 		return -EINVAL;
 
-	ret = vpbe_dev->ops.enum_dv_presets(vpbe_dev, preset);
+	ret = vpbe_dev->ops.enum_dv_timings(vpbe_dev, timings);
 	if (ret) {
 		v4l2_err(&vpbe_dev->v4l2_dev,
-			"Failed to enumerate dv presets info\n");
+			"Failed to enumerate dv timings info\n");
 		return -EINVAL;
 	}
 
@@ -1059,21 +1059,21 @@ vpbe_display_enum_dv_presets(struct file *file, void *priv,
 }
 
 /**
- * vpbe_display_s_dv_preset - Set the dv presets
+ * vpbe_display_s_dv_timings - Set the dv timings
  *
- * Set the preset in the current encoder. Return the status. 0 - success
+ * Set the timings in the current encoder. Return the status. 0 - success
  * -EINVAL on error
  */
 static int
-vpbe_display_s_dv_preset(struct file *file, void *priv,
-				struct v4l2_dv_preset *preset)
+vpbe_display_s_dv_timings(struct file *file, void *priv,
+				struct v4l2_dv_timings *timings)
 {
 	struct vpbe_fh *fh = priv;
 	struct vpbe_layer *layer = fh->layer;
 	struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
 	int ret;
 
-	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_S_DV_PRESETS\n");
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_S_DV_TIMINGS\n");
 
 
 	/* If streaming is started, return error */
@@ -1083,13 +1083,13 @@ vpbe_display_s_dv_preset(struct file *file, void *priv,
 	}
 
 	/* Set the given standard in the encoder */
-	if (!vpbe_dev->ops.s_dv_preset)
+	if (!vpbe_dev->ops.s_dv_timings)
 		return -EINVAL;
 
-	ret = vpbe_dev->ops.s_dv_preset(vpbe_dev, preset);
+	ret = vpbe_dev->ops.s_dv_timings(vpbe_dev, timings);
 	if (ret) {
 		v4l2_err(&vpbe_dev->v4l2_dev,
-			"Failed to set the dv presets info\n");
+			"Failed to set the dv timings info\n");
 		return -EINVAL;
 	}
 	/* set the current norm to zero to be consistent. If STD is used
@@ -1101,26 +1101,25 @@ vpbe_display_s_dv_preset(struct file *file, void *priv,
 }
 
 /**
- * vpbe_display_g_dv_preset - Set the dv presets
+ * vpbe_display_g_dv_timings - Set the dv timings
  *
- * Get the preset in the current encoder. Return the status. 0 - success
+ * Get the timings in the current encoder. Return the status. 0 - success
  * -EINVAL on error
  */
 static int
-vpbe_display_g_dv_preset(struct file *file, void *priv,
-				struct v4l2_dv_preset *dv_preset)
+vpbe_display_g_dv_timings(struct file *file, void *priv,
+				struct v4l2_dv_timings *dv_timings)
 {
 	struct vpbe_fh *fh = priv;
 	struct vpbe_device *vpbe_dev = fh->disp_dev->vpbe_dev;
 
-	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_G_DV_PRESETS\n");
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_G_DV_TIMINGS\n");
 
 	/* Get the given standard in the encoder */
 
 	if (vpbe_dev->current_timings.timings_type &
-				VPBE_ENC_DV_PRESET) {
-		dv_preset->preset =
-			vpbe_dev->current_timings.timings.dv_preset;
+				VPBE_ENC_CUSTOM_TIMINGS) {
+		*dv_timings = vpbe_dev->current_timings.dv_timings;
 	} else {
 		return -EINVAL;
 	}
@@ -1572,9 +1571,9 @@ static const struct v4l2_ioctl_ops vpbe_ioctl_ops = {
 	.vidioc_enum_output	 = vpbe_display_enum_output,
 	.vidioc_s_output	 = vpbe_display_s_output,
 	.vidioc_g_output	 = vpbe_display_g_output,
-	.vidioc_s_dv_preset	 = vpbe_display_s_dv_preset,
-	.vidioc_g_dv_preset	 = vpbe_display_g_dv_preset,
-	.vidioc_enum_dv_presets	 = vpbe_display_enum_dv_presets,
+	.vidioc_s_dv_timings	 = vpbe_display_s_dv_timings,
+	.vidioc_g_dv_timings	 = vpbe_display_g_dv_timings,
+	.vidioc_enum_dv_timings	 = vpbe_display_enum_dv_timings,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.vidioc_g_register	 = vpbe_display_g_register,
 	.vidioc_s_register	 = vpbe_display_s_register,
@@ -1639,8 +1638,7 @@ static __devinit int init_vpbe_layer(int i, struct vpbe_display *disp_dev,
 			VPBE_ENC_STD) {
 		vbd->tvnorms = (V4L2_STD_525_60 | V4L2_STD_625_50);
 		vbd->current_norm =
-			disp_dev->vpbe_dev->
-			current_timings.timings.std_id;
+			disp_dev->vpbe_dev->current_timings.std_id;
 	} else
 		vbd->current_norm = 0;
 
diff --git a/drivers/media/platform/davinci/vpbe_venc.c b/drivers/media/platform/davinci/vpbe_venc.c
index 0302669622d..aed7369b962 100644
--- a/drivers/media/platform/davinci/vpbe_venc.c
+++ b/drivers/media/platform/davinci/vpbe_venc.c
@@ -298,7 +298,7 @@ static int venc_set_480p59_94(struct v4l2_subdev *sd)
 		return -EINVAL;
 
 	/* Setup clock at VPSS & VENC for SD */
-	if (pdata->setup_clock(VPBE_ENC_DV_PRESET, V4L2_DV_480P59_94) < 0)
+	if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 27000000) < 0)
 		return -EINVAL;
 
 	venc_enabledigitaloutput(sd, 0);
@@ -345,7 +345,7 @@ static int venc_set_576p50(struct v4l2_subdev *sd)
 	  (pdata->venc_type != VPBE_VERSION_2))
 		return -EINVAL;
 	/* Setup clock at VPSS & VENC for SD */
-	if (pdata->setup_clock(VPBE_ENC_DV_PRESET, V4L2_DV_576P50) < 0)
+	if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 27000000) < 0)
 		return -EINVAL;
 
 	venc_enabledigitaloutput(sd, 0);
@@ -385,7 +385,7 @@ static int venc_set_720p60_internal(struct v4l2_subdev *sd)
 	struct venc_state *venc = to_state(sd);
 	struct venc_platform_data *pdata = venc->pdata;
 
-	if (pdata->setup_clock(VPBE_ENC_DV_PRESET, V4L2_DV_720P60) < 0)
+	if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 74250000) < 0)
 		return -EINVAL;
 
 	venc_enabledigitaloutput(sd, 0);
@@ -413,7 +413,7 @@ static int venc_set_1080i30_internal(struct v4l2_subdev *sd)
 	struct venc_state *venc = to_state(sd);
 	struct venc_platform_data *pdata = venc->pdata;
 
-	if (pdata->setup_clock(VPBE_ENC_DV_PRESET, V4L2_DV_1080P30) < 0)
+	if (pdata->setup_clock(VPBE_ENC_CUSTOM_TIMINGS, 74250000) < 0)
 		return -EINVAL;
 
 	venc_enabledigitaloutput(sd, 0);
@@ -446,26 +446,27 @@ static int venc_s_std_output(struct v4l2_subdev *sd, v4l2_std_id norm)
 	return -EINVAL;
 }
 
-static int venc_s_dv_preset(struct v4l2_subdev *sd,
-			    struct v4l2_dv_preset *dv_preset)
+static int venc_s_dv_timings(struct v4l2_subdev *sd,
+			    struct v4l2_dv_timings *dv_timings)
 {
 	struct venc_state *venc = to_state(sd);
+	u32 height = dv_timings->bt.height;
 	int ret;
 
-	v4l2_dbg(debug, 1, sd, "venc_s_dv_preset\n");
+	v4l2_dbg(debug, 1, sd, "venc_s_dv_timings\n");
 
-	if (dv_preset->preset == V4L2_DV_576P50)
+	if (height == 576)
 		return venc_set_576p50(sd);
-	else if (dv_preset->preset == V4L2_DV_480P59_94)
+	else if (height == 480)
 		return venc_set_480p59_94(sd);
-	else if ((dv_preset->preset == V4L2_DV_720P60) &&
+	else if ((height == 720) &&
 			(venc->pdata->venc_type == VPBE_VERSION_2)) {
 		/* TBD setup internal 720p mode here */
 		ret = venc_set_720p60_internal(sd);
 		/* for DM365 VPBE, there is DAC inside */
 		vdaccfg_write(sd, VDAC_CONFIG_HD_V2);
 		return ret;
-	} else if ((dv_preset->preset == V4L2_DV_1080I30) &&
+	} else if ((height == 1080) &&
 		(venc->pdata->venc_type == VPBE_VERSION_2)) {
 		/* TBD setup internal 1080i mode here */
 		ret = venc_set_1080i30_internal(sd);
@@ -518,7 +519,7 @@ static const struct v4l2_subdev_core_ops venc_core_ops = {
 static const struct v4l2_subdev_video_ops venc_video_ops = {
 	.s_routing = venc_s_routing,
 	.s_std_output = venc_s_std_output,
-	.s_dv_preset = venc_s_dv_preset,
+	.s_dv_timings = venc_s_dv_timings,
 };
 
 static const struct v4l2_subdev_ops venc_ops = {
diff --git a/drivers/media/platform/davinci/vpfe_capture.c b/drivers/media/platform/davinci/vpfe_capture.c
index 48052cbffc2..8be492cd8ed 100644
--- a/drivers/media/platform/davinci/vpfe_capture.c
+++ b/drivers/media/platform/davinci/vpfe_capture.c
@@ -1669,6 +1669,7 @@ static int vpfe_s_crop(struct file *file, void *priv,
 			     const struct v4l2_crop *crop)
 {
 	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct v4l2_rect rect = crop->c;
 	int ret = 0;
 
 	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_s_crop\n");
@@ -1684,7 +1685,7 @@ static int vpfe_s_crop(struct file *file, void *priv,
 	if (ret)
 		return ret;
 
-	if (crop->c.top < 0 || crop->c.left < 0) {
+	if (rect.top < 0 || rect.left < 0) {
 		v4l2_err(&vpfe_dev->v4l2_dev,
 			"doesn't support negative values for top & left\n");
 		ret = -EINVAL;
@@ -1692,26 +1693,26 @@ static int vpfe_s_crop(struct file *file, void *priv,
 	}
 
 	/* adjust the width to 16 pixel boundary */
-	crop->c.width = ((crop->c.width + 15) & ~0xf);
+	rect.width = ((rect.width + 15) & ~0xf);
 
 	/* make sure parameters are valid */
-	if ((crop->c.left + crop->c.width >
+	if ((rect.left + rect.width >
 		vpfe_dev->std_info.active_pixels) ||
-	    (crop->c.top + crop->c.height >
+	    (rect.top + rect.height >
 		vpfe_dev->std_info.active_lines)) {
 		v4l2_err(&vpfe_dev->v4l2_dev, "Error in S_CROP params\n");
 		ret = -EINVAL;
 		goto unlock_out;
 	}
-	ccdc_dev->hw_ops.set_image_window(&crop->c);
-	vpfe_dev->fmt.fmt.pix.width = crop->c.width;
-	vpfe_dev->fmt.fmt.pix.height = crop->c.height;
+	ccdc_dev->hw_ops.set_image_window(&rect);
+	vpfe_dev->fmt.fmt.pix.width = rect.width;
+	vpfe_dev->fmt.fmt.pix.height = rect.height;
 	vpfe_dev->fmt.fmt.pix.bytesperline =
 		ccdc_dev->hw_ops.get_line_length();
 	vpfe_dev->fmt.fmt.pix.sizeimage =
 		vpfe_dev->fmt.fmt.pix.bytesperline *
 		vpfe_dev->fmt.fmt.pix.height;
-	vpfe_dev->crop = crop->c;
+	vpfe_dev->crop = rect;
 unlock_out:
 	mutex_unlock(&vpfe_dev->lock);
 	return ret;
diff --git a/drivers/media/platform/davinci/vpif_capture.c b/drivers/media/platform/davinci/vpif_capture.c
index 0bafecac492..fcabc023885 100644
--- a/drivers/media/platform/davinci/vpif_capture.c
+++ b/drivers/media/platform/davinci/vpif_capture.c
@@ -311,12 +311,13 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
 	}
 
 	/* configure 1 or 2 channel mode */
-	ret = vpif_config_data->setup_input_channel_mode
-					(vpif->std_info.ycmux_mode);
-
-	if (ret < 0) {
-		vpif_dbg(1, debug, "can't set vpif channel mode\n");
-		return ret;
+	if (vpif_config_data->setup_input_channel_mode) {
+		ret = vpif_config_data->
+			setup_input_channel_mode(vpif->std_info.ycmux_mode);
+		if (ret < 0) {
+			vpif_dbg(1, debug, "can't set vpif channel mode\n");
+			return ret;
+		}
 	}
 
 	/* Call vpif_set_params function to set the parameters and addresses */
@@ -863,13 +864,11 @@ static unsigned int vpif_poll(struct file *filep, poll_table * wait)
  */
 static int vpif_open(struct file *filep)
 {
-	struct vpif_capture_config *config = vpif_dev->platform_data;
 	struct video_device *vdev = video_devdata(filep);
 	struct common_obj *common;
 	struct video_obj *vid_ch;
 	struct channel_obj *ch;
 	struct vpif_fh *fh;
-	int i;
 
 	vpif_dbg(2, debug, "vpif_open\n");
 
@@ -878,26 +877,6 @@ static int vpif_open(struct file *filep)
 	vid_ch = &ch->video;
 	common = &ch->common[VPIF_VIDEO_INDEX];
 
-	if (NULL == ch->curr_subdev_info) {
-		/**
-		 * search through the sub device to see a registered
-		 * sub device and make it as current sub device
-		 */
-		for (i = 0; i < config->subdev_count; i++) {
-			if (vpif_obj.sd[i]) {
-				/* the sub device is registered */
-				ch->curr_subdev_info = &config->subdev_info[i];
-				/* make first input as the current input */
-				vid_ch->input_idx = 0;
-				break;
-			}
-		}
-		if (i == config->subdev_count) {
-			vpif_err("No sub device registered\n");
-			return -ENOENT;
-		}
-	}
-
 	/* Allocate memory for the file handle object */
 	fh = kzalloc(sizeof(struct vpif_fh), GFP_KERNEL);
 	if (NULL == fh) {
@@ -997,6 +976,7 @@ static int vpif_reqbufs(struct file *file, void *priv,
 	struct common_obj *common;
 	u8 index = 0;
 	struct vb2_queue *q;
+	int ret;
 
 	vpif_dbg(2, debug, "vpif_reqbufs\n");
 
@@ -1036,8 +1016,12 @@ static int vpif_reqbufs(struct file *file, void *priv,
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct vpif_cap_buffer);
 
-	vb2_queue_init(q);
-
+	ret = vb2_queue_init(q);
+	if (ret) {
+		vpif_err("vpif_capture: vb2_queue_init() failed\n");
+		vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
+		return ret;
+	}
 	/* Set io allowed member of file handle to TRUE */
 	fh->io_allowed[index] = 1;
 	/* Increment io usrs member of channel object to 1 */
@@ -1175,10 +1159,9 @@ static int vpif_streamon(struct file *file, void *priv,
 		return ret;
 
 	/* Enable streamon on the sub device */
-	ret = v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index], video,
-				s_stream, 1);
+	ret = v4l2_subdev_call(ch->sd, video, s_stream, 1);
 
-	if (ret && (ret != -ENOIOCTLCMD)) {
+	if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV) {
 		vpif_dbg(1, debug, "stream on failed in subdev\n");
 		return ret;
 	}
@@ -1238,73 +1221,105 @@ static int vpif_streamoff(struct file *file, void *priv,
 
 	common->started = 0;
 
-	ret = v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index], video,
-				s_stream, 0);
+	ret = v4l2_subdev_call(ch->sd, video, s_stream, 0);
 
-	if (ret && (ret != -ENOIOCTLCMD))
+	if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV)
 		vpif_dbg(1, debug, "stream off failed in subdev\n");
 
 	return vb2_streamoff(&common->buffer_queue, buftype);
 }
 
 /**
- * vpif_map_sub_device_to_input() - Maps sub device to input
- * @ch - ptr to channel
- * @config - ptr to capture configuration
+ * vpif_input_to_subdev() - Maps input to sub device
+ * @vpif_cfg - global config ptr
+ * @chan_cfg - channel config ptr
  * @input_index - Given input index from application
- * @sub_device_index - index into sd table
  *
  * lookup the sub device information for a given input index.
  * we report all the inputs to application. inputs table also
  * has sub device name for the each input
  */
-static struct vpif_subdev_info *vpif_map_sub_device_to_input(
-				struct channel_obj *ch,
-				struct vpif_capture_config *vpif_cfg,
-				int input_index,
-				int *sub_device_index)
+static int vpif_input_to_subdev(
+		struct vpif_capture_config *vpif_cfg,
+		struct vpif_capture_chan_config *chan_cfg,
+		int input_index)
 {
-	struct vpif_capture_chan_config *chan_cfg;
-	struct vpif_subdev_info *subdev_info = NULL;
-	const char *subdev_name = NULL;
+	struct vpif_subdev_info *subdev_info;
+	const char *subdev_name;
 	int i;
 
-	vpif_dbg(2, debug, "vpif_map_sub_device_to_input\n");
+	vpif_dbg(2, debug, "vpif_input_to_subdev\n");
 
-	chan_cfg = &vpif_cfg->chan_config[ch->channel_id];
-
-	/**
-	 * search through the inputs to find the sub device supporting
-	 * the input
-	 */
-	for (i = 0; i < chan_cfg->input_count; i++) {
-		/* For each sub device, loop through input */
-		if (i == input_index) {
-			subdev_name = chan_cfg->inputs[i].subdev_name;
-			break;
-		}
-	}
-
-	/* if reached maximum. return null */
-	if (i == chan_cfg->input_count || (NULL == subdev_name))
-		return subdev_info;
+	subdev_name = chan_cfg->inputs[input_index].subdev_name;
+	if (subdev_name == NULL)
+		return -1;
 
 	/* loop through the sub device list to get the sub device info */
 	for (i = 0; i < vpif_cfg->subdev_count; i++) {
 		subdev_info = &vpif_cfg->subdev_info[i];
 		if (!strcmp(subdev_info->name, subdev_name))
-			break;
+			return i;
+	}
+	return -1;
+}
+
+/**
+ * vpif_set_input() - Select an input
+ * @vpif_cfg - global config ptr
+ * @ch - channel
+ * @_index - Given input index from application
+ *
+ * Select the given input.
+ */
+static int vpif_set_input(
+		struct vpif_capture_config *vpif_cfg,
+		struct channel_obj *ch,
+		int index)
+{
+	struct vpif_capture_chan_config *chan_cfg =
+			&vpif_cfg->chan_config[ch->channel_id];
+	struct vpif_subdev_info *subdev_info = NULL;
+	struct v4l2_subdev *sd = NULL;
+	u32 input = 0, output = 0;
+	int sd_index;
+	int ret;
+
+	sd_index = vpif_input_to_subdev(vpif_cfg, chan_cfg, index);
+	if (sd_index >= 0) {
+		sd = vpif_obj.sd[sd_index];
+		subdev_info = &vpif_cfg->subdev_info[sd_index];
 	}
 
-	if (i == vpif_cfg->subdev_count)
-		return subdev_info;
+	/* first setup input path from sub device to vpif */
+	if (sd && vpif_cfg->setup_input_path) {
+		ret = vpif_cfg->setup_input_path(ch->channel_id,
+				       subdev_info->name);
+		if (ret < 0) {
+			vpif_dbg(1, debug, "couldn't setup input path for the" \
+			" sub device %s, for input index %d\n",
+			subdev_info->name, index);
+			return ret;
+		}
+	}
 
-	/* check if the sub device is registered */
-	if (NULL == vpif_obj.sd[i])
-		return NULL;
+	if (sd) {
+		input = chan_cfg->inputs[index].input_route;
+		output = chan_cfg->inputs[index].output_route;
+		ret = v4l2_subdev_call(sd, video, s_routing,
+				input, output, 0);
+		if (ret < 0 && ret != -ENOIOCTLCMD) {
+			vpif_dbg(1, debug, "Failed to set input\n");
+			return ret;
+		}
+	}
+	ch->input_idx = index;
+	ch->sd = sd;
+	/* copy interface parameters to vpif */
+	ch->vpifparams.iface = chan_cfg->vpif_if;
 
-	*sub_device_index = i;
-	return subdev_info;
+	/* update tvnorms from the sub device input info */
+	ch->video_dev->tvnorms = chan_cfg->inputs[index].input.std;
+	return 0;
 }
 
 /**
@@ -1324,12 +1339,16 @@ static int vpif_querystd(struct file *file, void *priv, v4l2_std_id *std_id)
 	vpif_dbg(2, debug, "vpif_querystd\n");
 
 	/* Call querystd function of decoder device */
-	ret = v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index], video,
-				querystd, std_id);
-	if (ret < 0)
-		vpif_dbg(1, debug, "Failed to set standard for sub devices\n");
+	ret = v4l2_subdev_call(ch->sd, video, querystd, std_id);
 
-	return ret;
+	if (ret == -ENOIOCTLCMD || ret == -ENODEV)
+		return -ENODATA;
+	if (ret) {
+		vpif_dbg(1, debug, "Failed to query standard for sub devices\n");
+		return ret;
+	}
+
+	return 0;
 }
 
 /**
@@ -1397,11 +1416,12 @@ static int vpif_s_std(struct file *file, void *priv, v4l2_std_id *std_id)
 	vpif_config_format(ch);
 
 	/* set standard in the sub device */
-	ret = v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index], core,
-				s_std, *std_id);
-	if (ret < 0)
+	ret = v4l2_subdev_call(ch->sd, core, s_std, *std_id);
+	if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV) {
 		vpif_dbg(1, debug, "Failed to set standard for sub devices\n");
-	return ret;
+		return ret;
+	}
+	return 0;
 }
 
 /**
@@ -1441,10 +1461,8 @@ static int vpif_g_input(struct file *file, void *priv, unsigned int *index)
 {
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
-	struct video_obj *vid_ch = &ch->video;
-
-	*index = vid_ch->input_idx;
 
+	*index = ch->input_idx;
 	return 0;
 }
 
@@ -1461,13 +1479,13 @@ static int vpif_s_input(struct file *file, void *priv, unsigned int index)
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-	struct video_obj *vid_ch = &ch->video;
-	struct vpif_subdev_info *subdev_info;
-	int ret = 0, sd_index = 0;
-	u32 input = 0, output = 0;
+	int ret;
 
 	chan_cfg = &config->chan_config[ch->channel_id];
 
+	if (index >= chan_cfg->input_count)
+		return -EINVAL;
+
 	if (common->started) {
 		vpif_err("Streaming in progress\n");
 		return -EBUSY;
@@ -1486,45 +1504,7 @@ static int vpif_s_input(struct file *file, void *priv, unsigned int index)
 		return ret;
 
 	fh->initialized = 1;
-	subdev_info = vpif_map_sub_device_to_input(ch, config, index,
-						   &sd_index);
-	if (NULL == subdev_info) {
-		vpif_dbg(1, debug,
-			"couldn't lookup sub device for the input index\n");
-		return -EINVAL;
-	}
-
-	/* first setup input path from sub device to vpif */
-	if (config->setup_input_path) {
-		ret = config->setup_input_path(ch->channel_id,
-					       subdev_info->name);
-		if (ret < 0) {
-			vpif_dbg(1, debug, "couldn't setup input path for the"
-				" sub device %s, for input index %d\n",
-				subdev_info->name, index);
-			return ret;
-		}
-	}
-
-	if (subdev_info->can_route) {
-		input = subdev_info->input;
-		output = subdev_info->output;
-		ret = v4l2_subdev_call(vpif_obj.sd[sd_index], video, s_routing,
-					input, output, 0);
-		if (ret < 0) {
-			vpif_dbg(1, debug, "Failed to set input\n");
-			return ret;
-		}
-	}
-	vid_ch->input_idx = index;
-	ch->curr_subdev_info = subdev_info;
-	ch->curr_sd_index = sd_index;
-	/* copy interface parameters to vpif */
-	ch->vpifparams.iface = subdev_info->vpif_if;
-
-	/* update tvnorms from the sub device input info */
-	ch->video_dev->tvnorms = chan_cfg->inputs[index].input.std;
-	return ret;
+	return vpif_set_input(config, ch, index);
 }
 
 /**
@@ -1655,9 +1635,11 @@ static int vpif_querycap(struct file *file, void  *priv,
 {
 	struct vpif_capture_config *config = vpif_dev->platform_data;
 
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
-	strlcpy(cap->driver, "vpif capture", sizeof(cap->driver));
-	strlcpy(cap->bus_info, "VPIF Platform", sizeof(cap->bus_info));
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	snprintf(cap->driver, sizeof(cap->driver), "%s", dev_name(vpif_dev));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+		 dev_name(vpif_dev));
 	strlcpy(cap->card, config->card_name, sizeof(cap->card));
 
 	return 0;
@@ -1730,9 +1712,12 @@ vpif_enum_dv_timings(struct file *file, void *priv,
 {
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
+	int ret;
 
-	return v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index],
-				video, enum_dv_timings, timings);
+	ret = v4l2_subdev_call(ch->sd, video, enum_dv_timings, timings);
+	if (ret == -ENOIOCTLCMD && ret == -ENODEV)
+		return -EINVAL;
+	return ret;
 }
 
 /**
@@ -1747,9 +1732,12 @@ vpif_query_dv_timings(struct file *file, void *priv,
 {
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
+	int ret;
 
-	return v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index],
-				video, query_dv_timings, timings);
+	ret = v4l2_subdev_call(ch->sd, video, query_dv_timings, timings);
+	if (ret == -ENOIOCTLCMD && ret == -ENODEV)
+		return -ENODATA;
+	return ret;
 }
 
 /**
@@ -1775,13 +1763,9 @@ static int vpif_s_dv_timings(struct file *file, void *priv,
 	}
 
 	/* Configure subdevice timings, if any */
-	ret = v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index],
-			video, s_dv_timings, timings);
-	if (ret == -ENOIOCTLCMD) {
-		vpif_dbg(2, debug, "Custom DV timings not supported by "
-				"subdevice\n");
-		return -EINVAL;
-	}
+	ret = v4l2_subdev_call(ch->sd, video, s_dv_timings, timings);
+	if (ret == -ENOIOCTLCMD || ret == -ENODEV)
+		ret = 0;
 	if (ret < 0) {
 		vpif_dbg(2, debug, "Error setting custom DV timings\n");
 		return ret;
@@ -1906,8 +1890,7 @@ static int vpif_dbg_g_register(struct file *file, void *priv,
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
 
-	return v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index], core,
-			g_register, reg);
+	return v4l2_subdev_call(ch->sd, core, g_register, reg);
 }
 
 /*
@@ -1924,8 +1907,7 @@ static int vpif_dbg_s_register(struct file *file, void *priv,
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
 
-	return v4l2_subdev_call(vpif_obj.sd[ch->curr_sd_index], core,
-			s_register, reg);
+	return v4l2_subdev_call(ch->sd, core, s_register, reg);
 }
 #endif
 
@@ -2063,7 +2045,8 @@ static __init int vpif_probe(struct platform_device *pdev)
 {
 	struct vpif_subdev_info *subdevdata;
 	struct vpif_capture_config *config;
-	int i, j, k, m, q, err;
+	int i, j, k, err;
+	int res_idx = 0;
 	struct i2c_adapter *i2c_adap;
 	struct channel_obj *ch;
 	struct common_obj *common;
@@ -2086,18 +2069,19 @@ static __init int vpif_probe(struct platform_device *pdev)
 		return err;
 	}
 
-	k = 0;
-	while ((res = platform_get_resource(pdev, IORESOURCE_IRQ, k))) {
+	while ((res = platform_get_resource(pdev, IORESOURCE_IRQ, res_idx))) {
 		for (i = res->start; i <= res->end; i++) {
 			if (request_irq(i, vpif_channel_isr, IRQF_SHARED,
-					"VPIF_Capture",
-				(void *)(&vpif_obj.dev[k]->channel_id))) {
+					"VPIF_Capture", (void *)
+					(&vpif_obj.dev[res_idx]->channel_id))) {
 				err = -EBUSY;
-				i--;
+				for (j = 0; j < i; j++)
+					free_irq(j, (void *)
+					(&vpif_obj.dev[res_idx]->channel_id));
 				goto vpif_int_err;
 			}
 		}
-		k++;
+		res_idx++;
 	}
 
 	for (i = 0; i < VPIF_CAPTURE_MAX_DEVICES; i++) {
@@ -2111,7 +2095,7 @@ static __init int vpif_probe(struct platform_device *pdev)
 				video_device_release(ch->video_dev);
 			}
 			err = -ENOMEM;
-			goto vpif_dev_alloc_err;
+			goto vpif_int_err;
 		}
 
 		/* Initialize field of video device */
@@ -2142,24 +2126,6 @@ static __init int vpif_probe(struct platform_device *pdev)
 		}
 	}
 
-	for (j = 0; j < VPIF_CAPTURE_MAX_DEVICES; j++) {
-		ch = vpif_obj.dev[j];
-		ch->channel_id = j;
-		common = &(ch->common[VPIF_VIDEO_INDEX]);
-		spin_lock_init(&common->irqlock);
-		mutex_init(&common->lock);
-		ch->video_dev->lock = &common->lock;
-		/* Initialize prio member of channel object */
-		v4l2_prio_init(&ch->prio);
-		err = video_register_device(ch->video_dev,
-					    VFL_TYPE_GRABBER, (j ? 1 : 0));
-		if (err)
-			goto probe_out;
-
-		video_set_drvdata(ch->video_dev, ch);
-
-	}
-
 	i2c_adap = i2c_get_adapter(1);
 	config = pdev->dev.platform_data;
 
@@ -2169,7 +2135,7 @@ static __init int vpif_probe(struct platform_device *pdev)
 	if (vpif_obj.sd == NULL) {
 		vpif_err("unable to allocate memory for subdevice pointers\n");
 		err = -ENOMEM;
-		goto probe_out;
+		goto vpif_sd_error;
 	}
 
 	for (i = 0; i < subdev_count; i++) {
@@ -2186,19 +2152,32 @@ static __init int vpif_probe(struct platform_device *pdev)
 		}
 		v4l2_info(&vpif_obj.v4l2_dev, "registered sub device %s\n",
 			  subdevdata->name);
-
-		if (vpif_obj.sd[i])
-			vpif_obj.sd[i]->grp_id = 1 << i;
 	}
 
+	for (j = 0; j < VPIF_CAPTURE_MAX_DEVICES; j++) {
+		ch = vpif_obj.dev[j];
+		ch->channel_id = j;
+		common = &(ch->common[VPIF_VIDEO_INDEX]);
+		spin_lock_init(&common->irqlock);
+		mutex_init(&common->lock);
+		ch->video_dev->lock = &common->lock;
+		/* Initialize prio member of channel object */
+		v4l2_prio_init(&ch->prio);
+		video_set_drvdata(ch->video_dev, ch);
+
+		/* select input 0 */
+		err = vpif_set_input(config, ch, 0);
+		if (err)
+			goto probe_out;
+
+		err = video_register_device(ch->video_dev,
+					    VFL_TYPE_GRABBER, (j ? 1 : 0));
+		if (err)
+			goto probe_out;
+	}
 	v4l2_info(&vpif_obj.v4l2_dev, "VPIF capture driver initialized\n");
 	return 0;
 
-probe_subdev_out:
-	/* free sub devices memory */
-	kfree(vpif_obj.sd);
-
-	j = VPIF_CAPTURE_MAX_DEVICES;
 probe_out:
 	for (k = 0; k < j; k++) {
 		/* Get the pointer to the channel object */
@@ -2206,22 +2185,23 @@ probe_out:
 		/* Unregister video device */
 		video_unregister_device(ch->video_dev);
 	}
+probe_subdev_out:
+	/* free sub devices memory */
+	kfree(vpif_obj.sd);
 
-vpif_dev_alloc_err:
-	k = VPIF_CAPTURE_MAX_DEVICES-1;
-	res = platform_get_resource(pdev, IORESOURCE_IRQ, k);
-	i = res->end;
-
-vpif_int_err:
-	for (q = k; q >= 0; q--) {
-		for (m = i; m >= (int)res->start; m--)
-			free_irq(m, (void *)(&vpif_obj.dev[q]->channel_id));
-
-		res = platform_get_resource(pdev, IORESOURCE_IRQ, q-1);
-		if (res)
-			i = res->end;
+vpif_sd_error:
+	for (i = 0; i < VPIF_CAPTURE_MAX_DEVICES; i++) {
+		ch = vpif_obj.dev[i];
+		/* Note: does nothing if ch->video_dev == NULL */
+		video_device_release(ch->video_dev);
 	}
+vpif_int_err:
 	v4l2_device_unregister(&vpif_obj.v4l2_dev);
+	for (i = 0; i < res_idx; i++) {
+		res = platform_get_resource(pdev, IORESOURCE_IRQ, i);
+		for (j = res->start; j <= res->end; j++)
+			free_irq(j, (void *)(&vpif_obj.dev[i]->channel_id));
+	}
 	return err;
 }
 
diff --git a/drivers/media/platform/davinci/vpif_capture.h b/drivers/media/platform/davinci/vpif_capture.h
index d24efc17e4c..3d3c1e5cd5d 100644
--- a/drivers/media/platform/davinci/vpif_capture.h
+++ b/drivers/media/platform/davinci/vpif_capture.h
@@ -54,8 +54,6 @@ struct video_obj {
 	/* Currently selected or default standard */
 	v4l2_std_id stdid;
 	struct v4l2_dv_timings dv_timings;
-	/* This is to track the last input that is passed to application */
-	u32 input_idx;
 };
 
 struct vpif_cap_buffer {
@@ -119,10 +117,10 @@ struct channel_obj {
 	u8 initialized;
 	/* Identifies channel */
 	enum vpif_channel_id channel_id;
-	/* index into sd table */
-	int curr_sd_index;
-	/* ptr to current sub device information */
-	struct vpif_subdev_info *curr_subdev_info;
+	/* Current input */
+	u32 input_idx;
+	/* subdev corresponding to the current input, may be NULL */
+	struct v4l2_subdev *sd;
 	/* vpif configuration params */
 	struct vpif_params vpifparams;
 	/* common object array */
@@ -159,10 +157,6 @@ struct vpif_config_params {
 	u32 video_limit[VPIF_CAPTURE_NUM_CHANNELS];
 	u8 max_device_type;
 };
-/* Struct which keeps track of the line numbers for the sliced vbi service */
-struct vpif_service_line {
-	u16 service_id;
-	u16 service_line[2];
-};
+
 #endif				/* End of __KERNEL__ */
 #endif				/* VPIF_CAPTURE_H */
diff --git a/drivers/media/platform/davinci/vpif_display.c b/drivers/media/platform/davinci/vpif_display.c
index a5b88689aba..b716fbd4241 100644
--- a/drivers/media/platform/davinci/vpif_display.c
+++ b/drivers/media/platform/davinci/vpif_display.c
@@ -280,12 +280,13 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
 	}
 
 	/* clock settings */
-	ret =
-	    vpif_config_data->set_clock(ch->vpifparams.std_info.ycmux_mode,
-					ch->vpifparams.std_info.hd_sd);
-	if (ret < 0) {
-		vpif_err("can't set clock\n");
-		return ret;
+	if (vpif_config_data->set_clock) {
+		ret = vpif_config_data->set_clock(ch->vpifparams.std_info.
+		ycmux_mode, ch->vpifparams.std_info.hd_sd);
+		if (ret < 0) {
+			vpif_err("can't set clock\n");
+			return ret;
+		}
 	}
 
 	/* set the parameters and addresses */
@@ -307,7 +308,7 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
 		channel2_intr_assert();
 		channel2_intr_enable(1);
 		enable_channel2(1);
-		if (vpif_config_data->ch2_clip_en)
+		if (vpif_config_data->chan_config[VPIF_CHANNEL2_VIDEO].clip_en)
 			channel2_clipping_enable(1);
 	}
 
@@ -316,7 +317,7 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
 		channel3_intr_assert();
 		channel3_intr_enable(1);
 		enable_channel3(1);
-		if (vpif_config_data->ch3_clip_en)
+		if (vpif_config_data->chan_config[VPIF_CHANNEL3_VIDEO].clip_en)
 			channel3_clipping_enable(1);
 	}
 
@@ -826,9 +827,11 @@ static int vpif_querycap(struct file *file, void  *priv,
 {
 	struct vpif_display_config *config = vpif_dev->platform_data;
 
-	cap->capabilities = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
-	strlcpy(cap->driver, "vpif display", sizeof(cap->driver));
-	strlcpy(cap->bus_info, "Platform", sizeof(cap->bus_info));
+	cap->device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	snprintf(cap->driver, sizeof(cap->driver), "%s", dev_name(vpif_dev));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+		 dev_name(vpif_dev));
 	strlcpy(cap->card, config->card_name, sizeof(cap->card));
 
 	return 0;
@@ -935,6 +938,7 @@ static int vpif_reqbufs(struct file *file, void *priv,
 	enum v4l2_field field;
 	struct vb2_queue *q;
 	u8 index = 0;
+	int ret;
 
 	/* This file handle has not initialized the channel,
 	   It is not allowed to do settings */
@@ -980,8 +984,12 @@ static int vpif_reqbufs(struct file *file, void *priv,
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct vpif_disp_buffer);
 
-	vb2_queue_init(q);
-
+	ret = vb2_queue_init(q);
+	if (ret) {
+		vpif_err("vpif_display: vb2_queue_init() failed\n");
+		vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
+		return ret;
+	}
 	/* Set io allowed member of file handle to TRUE */
 	fh->io_allowed[index] = 1;
 	/* Increment io usrs member of channel object to 1 */
@@ -1173,14 +1181,16 @@ static int vpif_streamoff(struct file *file, void *priv,
 	if (buftype == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
 		/* disable channel */
 		if (VPIF_CHANNEL2_VIDEO == ch->channel_id) {
-			if (vpif_config_data->ch2_clip_en)
+			if (vpif_config_data->
+				chan_config[VPIF_CHANNEL2_VIDEO].clip_en)
 				channel2_clipping_enable(0);
 			enable_channel2(0);
 			channel2_intr_enable(0);
 		}
 		if ((VPIF_CHANNEL3_VIDEO == ch->channel_id) ||
 					(2 == common->started)) {
-			if (vpif_config_data->ch3_clip_en)
+			if (vpif_config_data->
+				chan_config[VPIF_CHANNEL3_VIDEO].clip_en)
 				channel3_clipping_enable(0);
 			enable_channel3(0);
 			channel3_intr_enable(0);
@@ -1213,49 +1223,126 @@ static int vpif_enum_output(struct file *file, void *fh,
 {
 
 	struct vpif_display_config *config = vpif_dev->platform_data;
+	struct vpif_display_chan_config *chan_cfg;
+	struct vpif_fh *vpif_handler = fh;
+	struct channel_obj *ch = vpif_handler->channel;
 
-	if (output->index >= config->output_count) {
+	chan_cfg = &config->chan_config[ch->channel_id];
+	if (output->index >= chan_cfg->output_count) {
 		vpif_dbg(1, debug, "Invalid output index\n");
 		return -EINVAL;
 	}
 
-	strcpy(output->name, config->output[output->index]);
-	output->type = V4L2_OUTPUT_TYPE_ANALOG;
-	output->std = VPIF_V4L2_STD;
+	*output = chan_cfg->outputs[output->index].output;
+	return 0;
+}
+
+/**
+ * vpif_output_to_subdev() - Maps output to sub device
+ * @vpif_cfg - global config ptr
+ * @chan_cfg - channel config ptr
+ * @index - Given output index from application
+ *
+ * lookup the sub device information for a given output index.
+ * we report all the output to application. output table also
+ * has sub device name for the each output
+ */
+static int
+vpif_output_to_subdev(struct vpif_display_config *vpif_cfg,
+		      struct vpif_display_chan_config *chan_cfg, int index)
+{
+	struct vpif_subdev_info *subdev_info;
+	const char *subdev_name;
+	int i;
+
+	vpif_dbg(2, debug, "vpif_output_to_subdev\n");
+
+	if (chan_cfg->outputs == NULL)
+		return -1;
+
+	subdev_name = chan_cfg->outputs[index].subdev_name;
+	if (subdev_name == NULL)
+		return -1;
+
+	/* loop through the sub device list to get the sub device info */
+	for (i = 0; i < vpif_cfg->subdev_count; i++) {
+		subdev_info = &vpif_cfg->subdevinfo[i];
+		if (!strcmp(subdev_info->name, subdev_name))
+			return i;
+	}
+	return -1;
+}
+
+/**
+ * vpif_set_output() - Select an output
+ * @vpif_cfg - global config ptr
+ * @ch - channel
+ * @index - Given output index from application
+ *
+ * Select the given output.
+ */
+static int vpif_set_output(struct vpif_display_config *vpif_cfg,
+		      struct channel_obj *ch, int index)
+{
+	struct vpif_display_chan_config *chan_cfg =
+		&vpif_cfg->chan_config[ch->channel_id];
+	struct vpif_subdev_info *subdev_info = NULL;
+	struct v4l2_subdev *sd = NULL;
+	u32 input = 0, output = 0;
+	int sd_index;
+	int ret;
+
+	sd_index = vpif_output_to_subdev(vpif_cfg, chan_cfg, index);
+	if (sd_index >= 0) {
+		sd = vpif_obj.sd[sd_index];
+		subdev_info = &vpif_cfg->subdevinfo[sd_index];
+	}
+
+	if (sd) {
+		input = chan_cfg->outputs[index].input_route;
+		output = chan_cfg->outputs[index].output_route;
+		ret = v4l2_subdev_call(sd, video, s_routing, input, output, 0);
+		if (ret < 0 && ret != -ENOIOCTLCMD) {
+			vpif_err("Failed to set output\n");
+			return ret;
+		}
 
+	}
+	ch->output_idx = index;
+	ch->sd = sd;
+	if (chan_cfg->outputs != NULL)
+		/* update tvnorms from the sub device output info */
+		ch->video_dev->tvnorms = chan_cfg->outputs[index].output.std;
 	return 0;
 }
 
 static int vpif_s_output(struct file *file, void *priv, unsigned int i)
 {
+	struct vpif_display_config *config = vpif_dev->platform_data;
+	struct vpif_display_chan_config *chan_cfg;
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
-	struct video_obj *vid_ch = &ch->video;
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
-	int ret = 0;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+
+	if (i >= chan_cfg->output_count)
+		return -EINVAL;
 
 	if (common->started) {
 		vpif_err("Streaming in progress\n");
 		return -EBUSY;
 	}
 
-	ret = v4l2_device_call_until_err(&vpif_obj.v4l2_dev, 1, video,
-							s_routing, 0, i, 0);
-
-	if (ret < 0)
-		vpif_err("Failed to set output standard\n");
-
-	vid_ch->output_id = i;
-	return ret;
+	return vpif_set_output(config, ch, i);
 }
 
 static int vpif_g_output(struct file *file, void *priv, unsigned int *i)
 {
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
-	struct video_obj *vid_ch = &ch->video;
 
-	*i = vid_ch->output_id;
+	*i = ch->output_idx;
 
 	return 0;
 }
@@ -1290,10 +1377,12 @@ vpif_enum_dv_timings(struct file *file, void *priv,
 {
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
-	struct video_obj *vid_ch = &ch->video;
+	int ret;
 
-	return v4l2_subdev_call(vpif_obj.sd[vid_ch->output_id],
-			video, enum_dv_timings, timings);
+	ret = v4l2_subdev_call(ch->sd, video, enum_dv_timings, timings);
+	if (ret == -ENOIOCTLCMD && ret == -ENODEV)
+		return -EINVAL;
+	return ret;
 }
 
 /**
@@ -1319,13 +1408,9 @@ static int vpif_s_dv_timings(struct file *file, void *priv,
 	}
 
 	/* Configure subdevice timings, if any */
-	ret = v4l2_subdev_call(vpif_obj.sd[vid_ch->output_id],
-			video, s_dv_timings, timings);
-	if (ret == -ENOIOCTLCMD) {
-		vpif_dbg(2, debug, "Custom DV timings not supported by "
-				"subdevice\n");
-		return -EINVAL;
-	}
+	ret = v4l2_subdev_call(ch->sd, video, s_dv_timings, timings);
+	if (ret == -ENOIOCTLCMD || ret == -ENODEV)
+		ret = 0;
 	if (ret < 0) {
 		vpif_dbg(2, debug, "Error setting custom DV timings\n");
 		return ret;
@@ -1450,10 +1535,8 @@ static int vpif_dbg_g_register(struct file *file, void *priv,
 		struct v4l2_dbg_register *reg){
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
-	struct video_obj *vid_ch = &ch->video;
 
-	return v4l2_subdev_call(vpif_obj.sd[vid_ch->output_id], core,
-			g_register, reg);
+	return v4l2_subdev_call(ch->sd, core, g_register, reg);
 }
 
 /*
@@ -1469,10 +1552,8 @@ static int vpif_dbg_s_register(struct file *file, void *priv,
 		struct v4l2_dbg_register *reg){
 	struct vpif_fh *fh = priv;
 	struct channel_obj *ch = fh->channel;
-	struct video_obj *vid_ch = &ch->video;
 
-	return v4l2_subdev_call(vpif_obj.sd[vid_ch->output_id], core,
-			s_register, reg);
+	return v4l2_subdev_call(ch->sd, core, s_register, reg);
 }
 #endif
 
@@ -1536,9 +1617,6 @@ static struct video_device vpif_video_template = {
 	.name		= "vpif",
 	.fops		= &vpif_fops,
 	.ioctl_ops	= &vpif_ioctl_ops,
-	.tvnorms	= VPIF_V4L2_STD,
-	.current_norm	= V4L2_STD_625_50,
-
 };
 
 /*Configure the channels, buffer sizei, request irq */
@@ -1611,7 +1689,8 @@ static __init int vpif_probe(struct platform_device *pdev)
 {
 	struct vpif_subdev_info *subdevdata;
 	struct vpif_display_config *config;
-	int i, j = 0, k, q, m, err = 0;
+	int i, j = 0, k, err = 0;
+	int res_idx = 0;
 	struct i2c_adapter *i2c_adap;
 	struct common_obj *common;
 	struct channel_obj *ch;
@@ -1634,21 +1713,22 @@ static __init int vpif_probe(struct platform_device *pdev)
 		return err;
 	}
 
-	k = 0;
-	while ((res = platform_get_resource(pdev, IORESOURCE_IRQ, k))) {
+	while ((res = platform_get_resource(pdev, IORESOURCE_IRQ, res_idx))) {
 		for (i = res->start; i <= res->end; i++) {
 			if (request_irq(i, vpif_channel_isr, IRQF_SHARED,
-					"VPIF_Display",
-				(void *)(&vpif_obj.dev[k]->channel_id))) {
+					"VPIF_Display", (void *)
+					(&vpif_obj.dev[res_idx]->channel_id))) {
 				err = -EBUSY;
+				for (j = 0; j < i; j++)
+					free_irq(j, (void *)
+					(&vpif_obj.dev[res_idx]->channel_id));
 				goto vpif_int_err;
 			}
 		}
-		k++;
+		res_idx++;
 	}
 
 	for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) {
-
 		/* Get the pointer to the channel object */
 		ch = vpif_obj.dev[i];
 
@@ -1694,6 +1774,32 @@ static __init int vpif_probe(struct platform_device *pdev)
 		}
 	}
 
+	i2c_adap = i2c_get_adapter(1);
+	config = pdev->dev.platform_data;
+	subdev_count = config->subdev_count;
+	subdevdata = config->subdevinfo;
+	vpif_obj.sd = kzalloc(sizeof(struct v4l2_subdev *) * subdev_count,
+								GFP_KERNEL);
+	if (vpif_obj.sd == NULL) {
+		vpif_err("unable to allocate memory for subdevice pointers\n");
+		err = -ENOMEM;
+		goto vpif_sd_error;
+	}
+
+	for (i = 0; i < subdev_count; i++) {
+		vpif_obj.sd[i] = v4l2_i2c_new_subdev_board(&vpif_obj.v4l2_dev,
+						i2c_adap,
+						&subdevdata[i].board_info,
+						NULL);
+		if (!vpif_obj.sd[i]) {
+			vpif_err("Error registering v4l2 subdevice\n");
+			goto probe_subdev_out;
+		}
+
+		if (vpif_obj.sd[i])
+			vpif_obj.sd[i]->grp_id = 1 << i;
+	}
+
 	for (j = 0; j < VPIF_DISPLAY_MAX_DEVICES; j++) {
 		ch = vpif_obj.dev[j];
 		/* Initialize field of the channel objects */
@@ -1715,6 +1821,8 @@ static __init int vpif_probe(struct platform_device *pdev)
 
 		}
 		ch->initialized = 0;
+		if (subdev_count)
+			ch->sd = vpif_obj.sd[0];
 		ch->channel_id = j;
 		if (j < 2)
 			ch->common[VPIF_VIDEO_INDEX].numbuffers =
@@ -1729,6 +1837,12 @@ static __init int vpif_probe(struct platform_device *pdev)
 		ch->common[VPIF_VIDEO_INDEX].fmt.type =
 						V4L2_BUF_TYPE_VIDEO_OUTPUT;
 		ch->video_dev->lock = &common->lock;
+		video_set_drvdata(ch->video_dev, ch);
+
+		/* select output 0 */
+		err = vpif_set_output(config, ch, 0);
+		if (err)
+			goto probe_out;
 
 		/* register video device */
 		vpif_dbg(1, debug, "channel=%x,channel->video_dev=%x\n",
@@ -1738,42 +1852,12 @@ static __init int vpif_probe(struct platform_device *pdev)
 					  VFL_TYPE_GRABBER, (j ? 3 : 2));
 		if (err < 0)
 			goto probe_out;
-
-		video_set_drvdata(ch->video_dev, ch);
-	}
-
-	i2c_adap = i2c_get_adapter(1);
-	config = pdev->dev.platform_data;
-	subdev_count = config->subdev_count;
-	subdevdata = config->subdevinfo;
-	vpif_obj.sd = kzalloc(sizeof(struct v4l2_subdev *) * subdev_count,
-								GFP_KERNEL);
-	if (vpif_obj.sd == NULL) {
-		vpif_err("unable to allocate memory for subdevice pointers\n");
-		err = -ENOMEM;
-		goto probe_out;
-	}
-
-	for (i = 0; i < subdev_count; i++) {
-		vpif_obj.sd[i] = v4l2_i2c_new_subdev_board(&vpif_obj.v4l2_dev,
-						i2c_adap,
-						&subdevdata[i].board_info,
-						NULL);
-		if (!vpif_obj.sd[i]) {
-			vpif_err("Error registering v4l2 subdevice\n");
-			goto probe_subdev_out;
-		}
-
-		if (vpif_obj.sd[i])
-			vpif_obj.sd[i]->grp_id = 1 << i;
 	}
 
 	v4l2_info(&vpif_obj.v4l2_dev,
 			" VPIF display driver initialized\n");
 	return 0;
 
-probe_subdev_out:
-	kfree(vpif_obj.sd);
 probe_out:
 	for (k = 0; k < j; k++) {
 		ch = vpif_obj.dev[k];
@@ -1781,14 +1865,21 @@ probe_out:
 		video_device_release(ch->video_dev);
 		ch->video_dev = NULL;
 	}
+probe_subdev_out:
+	kfree(vpif_obj.sd);
+vpif_sd_error:
+	for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) {
+		ch = vpif_obj.dev[i];
+		/* Note: does nothing if ch->video_dev == NULL */
+		video_device_release(ch->video_dev);
+	}
 vpif_int_err:
 	v4l2_device_unregister(&vpif_obj.v4l2_dev);
 	vpif_err("VPIF IRQ request failed\n");
-	for (q = k; k >= 0; k--) {
-		for (m = i; m >= res->start; m--)
-			free_irq(m, (void *)(&vpif_obj.dev[k]->channel_id));
-		res = platform_get_resource(pdev, IORESOURCE_IRQ, k-1);
-		m = res->end;
+	for (i = 0; i < res_idx; i++) {
+		res = platform_get_resource(pdev, IORESOURCE_IRQ, i);
+		for (j = res->start; j <= res->end; j++)
+			free_irq(j, (void *)(&vpif_obj.dev[i]->channel_id));
 	}
 
 	return err;
diff --git a/drivers/media/platform/davinci/vpif_display.h b/drivers/media/platform/davinci/vpif_display.h
index f628ebcf367..a5a18f74395 100644
--- a/drivers/media/platform/davinci/vpif_display.h
+++ b/drivers/media/platform/davinci/vpif_display.h
@@ -62,13 +62,6 @@ struct video_obj {
 	v4l2_std_id stdid;		/* Currently selected or default
 					 * standard */
 	struct v4l2_dv_timings dv_timings;
-	u32 output_id;			/* Current output id */
-};
-
-struct vbi_obj {
-	int num_services;
-	struct vpif_vbi_params vbiparams;	/* vpif parameters for the raw
-						 * vbi data */
 };
 
 struct vpif_disp_buffer {
@@ -131,12 +124,13 @@ struct channel_obj {
 					 * which is being displayed */
 	u8 initialized;			/* flag to indicate whether
 					 * encoder is initialized */
+	u32 output_idx;			/* Current output index */
+	struct v4l2_subdev *sd;		/* Current output subdev(may be NULL) */
 
 	enum vpif_channel_id channel_id;/* Identifies channel */
 	struct vpif_params vpifparams;
 	struct common_obj common[VPIF_NUMOBJECTS];
 	struct video_obj video;
-	struct vbi_obj vbi;
 };
 
 /* File handle structure */
@@ -168,12 +162,4 @@ struct vpif_config_params {
 	u8 min_numbuffers;
 };
 
-/* Struct which keeps track of the line numbers for the sliced vbi service */
-struct vpif_service_line {
-	u16 service_id;
-	u16 service_line[2];
-	u16 enc_service_id;
-	u8 bytestowrite;
-};
-
 #endif				/* DAVINCIHD_DISPLAY_H */
diff --git a/drivers/media/platform/exynos-gsc/gsc-regs.c b/drivers/media/platform/exynos-gsc/gsc-regs.c
index 0d8625f03a3..0146b354dc2 100644
--- a/drivers/media/platform/exynos-gsc/gsc-regs.c
+++ b/drivers/media/platform/exynos-gsc/gsc-regs.c
@@ -212,7 +212,7 @@ void gsc_hw_set_in_image_format(struct gsc_ctx *ctx)
 		else
 			cfg |= GSC_IN_YUV422_3P;
 		break;
-	};
+	}
 
 	writel(cfg, dev->regs + GSC_IN_CON);
 }
@@ -332,7 +332,7 @@ void gsc_hw_set_out_image_format(struct gsc_ctx *ctx)
 	case 3:
 		cfg |= GSC_OUT_YUV420_3P;
 		break;
-	};
+	}
 
 end_set:
 	writel(cfg, dev->regs + GSC_OUT_CON);
diff --git a/drivers/media/platform/fsl-viu.c b/drivers/media/platform/fsl-viu.c
index 897250b8864..31ac4dc6924 100644
--- a/drivers/media/platform/fsl-viu.c
+++ b/drivers/media/platform/fsl-viu.c
@@ -864,7 +864,7 @@ int vidioc_s_fbuf(struct file *file, void *priv, const struct v4l2_framebuffer *
 {
 	struct viu_fh  *fh = priv;
 	struct viu_dev *dev = fh->dev;
-	struct v4l2_framebuffer *fb = arg;
+	const struct v4l2_framebuffer *fb = arg;
 	struct viu_fmt *fmt;
 
 	if (!capable(CAP_SYS_ADMIN) && !capable(CAP_SYS_RAWIO))
diff --git a/drivers/media/platform/mem2mem_testdev.c b/drivers/media/platform/mem2mem_testdev.c
index d0363753711..2e2121e9813 100644
--- a/drivers/media/platform/mem2mem_testdev.c
+++ b/drivers/media/platform/mem2mem_testdev.c
@@ -397,8 +397,7 @@ static void device_isr(unsigned long priv)
 	curr_ctx = v4l2_m2m_get_curr_priv(m2mtest_dev->m2m_dev);
 
 	if (NULL == curr_ctx) {
-		printk(KERN_ERR
-			"Instance released before the end of transaction\n");
+		pr_err("Instance released before the end of transaction\n");
 		return;
 	}
 
@@ -894,7 +893,7 @@ static int m2mtest_open(struct file *file)
 
 	if (mutex_lock_interruptible(&dev->dev_mutex))
 		return -ERESTARTSYS;
-	ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
 	if (!ctx) {
 		rc = -ENOMEM;
 		goto open_unlock;
@@ -1020,7 +1019,7 @@ static int m2mtest_probe(struct platform_device *pdev)
 	struct video_device *vfd;
 	int ret;
 
-	dev = kzalloc(sizeof *dev, GFP_KERNEL);
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
 	if (!dev)
 		return -ENOMEM;
 
@@ -1028,7 +1027,7 @@ static int m2mtest_probe(struct platform_device *pdev)
 
 	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
 	if (ret)
-		goto free_dev;
+		return ret;
 
 	atomic_set(&dev->num_inst, 0);
 	mutex_init(&dev->dev_mutex);
@@ -1067,15 +1066,13 @@ static int m2mtest_probe(struct platform_device *pdev)
 
 	return 0;
 
-	v4l2_m2m_release(dev->m2m_dev);
 err_m2m:
+	v4l2_m2m_release(dev->m2m_dev);
 	video_unregister_device(dev->vfd);
 rel_vdev:
 	video_device_release(vfd);
 unreg_dev:
 	v4l2_device_unregister(&dev->v4l2_dev);
-free_dev:
-	kfree(dev);
 
 	return ret;
 }
@@ -1090,7 +1087,6 @@ static int m2mtest_remove(struct platform_device *pdev)
 	del_timer_sync(&dev->timer);
 	video_unregister_device(dev->vfd);
 	v4l2_device_unregister(&dev->v4l2_dev);
-	kfree(dev);
 
 	return 0;
 }
diff --git a/drivers/media/platform/omap/omap_vout.c b/drivers/media/platform/omap/omap_vout.c
index 66ac21d466a..a3b1a34c896 100644
--- a/drivers/media/platform/omap/omap_vout.c
+++ b/drivers/media/platform/omap/omap_vout.c
@@ -455,11 +455,15 @@ static int omapvid_init(struct omap_vout_device *vout, u32 addr)
 
 	win = &vout->win;
 	for (i = 0; i < ovid->num_overlays; i++) {
+		struct omap_dss_device *dssdev;
+
 		ovl = ovid->overlays[i];
-		if (!ovl->manager || !ovl->manager->device)
+		dssdev = ovl->get_device(ovl);
+
+		if (!dssdev)
 			return -EINVAL;
 
-		timing = &ovl->manager->device->panel.timings;
+		timing = &dssdev->panel.timings;
 
 		outw = win->w.width;
 		outh = win->w.height;
@@ -516,8 +520,11 @@ static int omapvid_apply_changes(struct omap_vout_device *vout)
 	struct omapvideo_info *ovid = &vout->vid_info;
 
 	for (i = 0; i < ovid->num_overlays; i++) {
+		struct omap_dss_device *dssdev;
+
 		ovl = ovid->overlays[i];
-		if (!ovl->manager || !ovl->manager->device)
+		dssdev = ovl->get_device(ovl);
+		if (!dssdev)
 			return -EINVAL;
 		ovl->manager->apply(ovl->manager);
 	}
@@ -580,12 +587,14 @@ static void omap_vout_isr(void *arg, unsigned int irqstatus)
 
 	ovid = &vout->vid_info;
 	ovl = ovid->overlays[0];
-	/* get the display device attached to the overlay */
-	if (!ovl->manager || !ovl->manager->device)
-		return;
 
 	mgr_id = ovl->manager->id;
-	cur_display = ovl->manager->device;
+
+	/* get the display device attached to the overlay */
+	cur_display = ovl->get_device(ovl);
+
+	if (!cur_display)
+		return;
 
 	spin_lock(&vout->vbq_lock);
 	do_gettimeofday(&timevalue);
@@ -911,7 +920,7 @@ static int omap_vout_mmap(struct file *file, struct vm_area_struct *vma)
 
 	q->bufs[i]->baddr = vma->vm_start;
 
-	vma->vm_flags |= VM_RESERVED;
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
 	vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
 	vma->vm_ops = &omap_vout_vm_ops;
 	vma->vm_private_data = (void *) vout;
@@ -949,7 +958,9 @@ static int omap_vout_release(struct file *file)
 	/* Disable all the overlay managers connected with this interface */
 	for (i = 0; i < ovid->num_overlays; i++) {
 		struct omap_overlay *ovl = ovid->overlays[i];
-		if (ovl->manager && ovl->manager->device)
+		struct omap_dss_device *dssdev = ovl->get_device(ovl);
+
+		if (dssdev)
 			ovl->disable(ovl);
 	}
 	/* Turn off the pipeline */
@@ -1082,14 +1093,17 @@ static int vidioc_try_fmt_vid_out(struct file *file, void *fh,
 	struct omapvideo_info *ovid;
 	struct omap_video_timings *timing;
 	struct omap_vout_device *vout = fh;
+	struct omap_dss_device *dssdev;
 
 	ovid = &vout->vid_info;
 	ovl = ovid->overlays[0];
+	/* get the display device attached to the overlay */
+	dssdev = ovl->get_device(ovl);
 
-	if (!ovl->manager || !ovl->manager->device)
+	if (!dssdev)
 		return -EINVAL;
-	/* get the display device attached to the overlay */
-	timing = &ovl->manager->device->panel.timings;
+
+	timing = &dssdev->panel.timings;
 
 	vout->fbuf.fmt.height = timing->y_res;
 	vout->fbuf.fmt.width = timing->x_res;
@@ -1106,6 +1120,7 @@ static int vidioc_s_fmt_vid_out(struct file *file, void *fh,
 	struct omapvideo_info *ovid;
 	struct omap_video_timings *timing;
 	struct omap_vout_device *vout = fh;
+	struct omap_dss_device *dssdev;
 
 	if (vout->streaming)
 		return -EBUSY;
@@ -1114,13 +1129,14 @@ static int vidioc_s_fmt_vid_out(struct file *file, void *fh,
 
 	ovid = &vout->vid_info;
 	ovl = ovid->overlays[0];
+	dssdev = ovl->get_device(ovl);
 
 	/* get the display device attached to the overlay */
-	if (!ovl->manager || !ovl->manager->device) {
+	if (!dssdev) {
 		ret = -EINVAL;
 		goto s_fmt_vid_out_exit;
 	}
-	timing = &ovl->manager->device->panel.timings;
+	timing = &dssdev->panel.timings;
 
 	/* We dont support RGB24-packed mode if vrfb rotation
 	 * is enabled*/
@@ -1299,6 +1315,7 @@ static int vidioc_s_crop(struct file *file, void *fh, const struct v4l2_crop *cr
 	struct omapvideo_info *ovid;
 	struct omap_overlay *ovl;
 	struct omap_video_timings *timing;
+	struct omap_dss_device *dssdev;
 
 	if (vout->streaming)
 		return -EBUSY;
@@ -1306,13 +1323,15 @@ static int vidioc_s_crop(struct file *file, void *fh, const struct v4l2_crop *cr
 	mutex_lock(&vout->lock);
 	ovid = &vout->vid_info;
 	ovl = ovid->overlays[0];
+	/* get the display device attached to the overlay */
+	dssdev = ovl->get_device(ovl);
 
-	if (!ovl->manager || !ovl->manager->device) {
+	if (!dssdev) {
 		ret = -EINVAL;
 		goto s_crop_err;
 	}
-	/* get the display device attached to the overlay */
-	timing = &ovl->manager->device->panel.timings;
+
+	timing = &dssdev->panel.timings;
 
 	if (is_rotation_90_or_270(vout)) {
 		vout->fbuf.fmt.height = timing->x_res;
@@ -1668,7 +1687,7 @@ static int vidioc_streamon(struct file *file, void *fh, enum v4l2_buf_type i)
 	for (j = 0; j < ovid->num_overlays; j++) {
 		struct omap_overlay *ovl = ovid->overlays[j];
 
-		if (ovl->manager && ovl->manager->device) {
+		if (ovl->get_device(ovl)) {
 			struct omap_overlay_info info;
 			ovl->get_overlay_info(ovl, &info);
 			info.paddr = addr;
@@ -1691,8 +1710,9 @@ static int vidioc_streamon(struct file *file, void *fh, enum v4l2_buf_type i)
 
 	for (j = 0; j < ovid->num_overlays; j++) {
 		struct omap_overlay *ovl = ovid->overlays[j];
+		struct omap_dss_device *dssdev = ovl->get_device(ovl);
 
-		if (ovl->manager && ovl->manager->device) {
+		if (dssdev) {
 			ret = ovl->enable(ovl);
 			if (ret)
 				goto streamon_err1;
@@ -1727,8 +1747,9 @@ static int vidioc_streamoff(struct file *file, void *fh, enum v4l2_buf_type i)
 
 	for (j = 0; j < ovid->num_overlays; j++) {
 		struct omap_overlay *ovl = ovid->overlays[j];
+		struct omap_dss_device *dssdev = ovl->get_device(ovl);
 
-		if (ovl->manager && ovl->manager->device)
+		if (dssdev)
 			ovl->disable(ovl);
 	}
 
@@ -1891,8 +1912,8 @@ static int __init omap_vout_setup_video_data(struct omap_vout_device *vout)
 	struct video_device *vfd;
 	struct v4l2_pix_format *pix;
 	struct v4l2_control *control;
-	struct omap_dss_device *display =
-		vout->vid_info.overlays[0]->manager->device;
+	struct omap_overlay *ovl = vout->vid_info.overlays[0];
+	struct omap_dss_device *display = ovl->get_device(ovl);
 
 	/* set the default pix */
 	pix = &vout->pix;
@@ -2207,8 +2228,10 @@ static int __init omap_vout_probe(struct platform_device *pdev)
 	 */
 	for (i = 1; i < vid_dev->num_overlays; i++) {
 		ovl = omap_dss_get_overlay(i);
-		if (ovl->manager && ovl->manager->device) {
-			def_display = ovl->manager->device;
+		dssdev = ovl->get_device(ovl);
+
+		if (dssdev) {
+			def_display = dssdev;
 		} else {
 			dev_warn(&pdev->dev, "cannot find display\n");
 			def_display = NULL;
@@ -2255,8 +2278,10 @@ probe_err1:
 	for (i = 1; i < vid_dev->num_overlays; i++) {
 		def_display = NULL;
 		ovl = omap_dss_get_overlay(i);
-		if (ovl->manager && ovl->manager->device)
-			def_display = ovl->manager->device;
+		dssdev = ovl->get_device(ovl);
+
+		if (dssdev)
+			def_display = dssdev;
 
 		if (def_display && def_display->driver)
 			def_display->driver->disable(def_display);
diff --git a/drivers/media/platform/omap3isp/ispreg.h b/drivers/media/platform/omap3isp/ispreg.h
index 084ea77d65a..e2c57f334c5 100644
--- a/drivers/media/platform/omap3isp/ispreg.h
+++ b/drivers/media/platform/omap3isp/ispreg.h
@@ -27,13 +27,13 @@
 #ifndef OMAP3_ISP_REG_H
 #define OMAP3_ISP_REG_H
 
-#include <plat/omap34xx.h>
-
-
 #define CM_CAM_MCLK_HZ			172800000	/* Hz */
 
 /* ISP Submodules offset */
 
+#define L4_34XX_BASE			0x48000000
+#define OMAP3430_ISP_BASE		(L4_34XX_BASE + 0xBC000)
+
 #define OMAP3ISP_REG_BASE		OMAP3430_ISP_BASE
 #define OMAP3ISP_REG(offset)		(OMAP3ISP_REG_BASE + (offset))
 
diff --git a/drivers/media/platform/s5p-fimc/fimc-capture.c b/drivers/media/platform/s5p-fimc/fimc-capture.c
index dded9881522..367efd164d0 100644
--- a/drivers/media/platform/s5p-fimc/fimc-capture.c
+++ b/drivers/media/platform/s5p-fimc/fimc-capture.c
@@ -177,7 +177,9 @@ static int fimc_capture_config_update(struct fimc_ctx *ctx)
 
 void fimc_capture_irq_handler(struct fimc_dev *fimc, int deq_buf)
 {
+	struct v4l2_subdev *csis = fimc->pipeline.subdevs[IDX_CSIS];
 	struct fimc_vid_cap *cap = &fimc->vid_cap;
+	struct fimc_frame *f = &cap->ctx->d_frame;
 	struct fimc_vid_buffer *v_buf;
 	struct timeval *tv;
 	struct timespec ts;
@@ -216,6 +218,25 @@ void fimc_capture_irq_handler(struct fimc_dev *fimc, int deq_buf)
 		if (++cap->buf_index >= FIMC_MAX_OUT_BUFS)
 			cap->buf_index = 0;
 	}
+	/*
+	 * Set up a buffer at MIPI-CSIS if current image format
+	 * requires the frame embedded data capture.
+	 */
+	if (f->fmt->mdataplanes && !list_empty(&cap->active_buf_q)) {
+		unsigned int plane = ffs(f->fmt->mdataplanes) - 1;
+		unsigned int size = f->payload[plane];
+		s32 index = fimc_hw_get_frame_index(fimc);
+		void *vaddr;
+
+		list_for_each_entry(v_buf, &cap->active_buf_q, list) {
+			if (v_buf->index != index)
+				continue;
+			vaddr = vb2_plane_vaddr(&v_buf->vb, plane);
+			v4l2_subdev_call(csis, video, s_rx_buffer,
+					 vaddr, &size);
+			break;
+		}
+	}
 
 	if (cap->active_buf_cnt == 0) {
 		if (deq_buf)
@@ -351,6 +372,8 @@ static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *pfmt,
 		unsigned int size = (wh * fmt->depth[i]) / 8;
 		if (pixm)
 			sizes[i] = max(size, pixm->plane_fmt[i].sizeimage);
+		else if (fimc_fmt_is_user_defined(fmt->color))
+			sizes[i] = frame->payload[i];
 		else
 			sizes[i] = max_t(u32, size, frame->payload[i]);
 
@@ -611,10 +634,10 @@ static struct fimc_fmt *fimc_capture_try_format(struct fimc_ctx *ctx,
 	u32 mask = FMT_FLAGS_CAM;
 	struct fimc_fmt *ffmt;
 
-	/* Color conversion from/to JPEG is not supported */
+	/* Conversion from/to JPEG or User Defined format is not supported */
 	if (code && ctx->s_frame.fmt && pad == FIMC_SD_PAD_SOURCE &&
-	    fimc_fmt_is_jpeg(ctx->s_frame.fmt->color))
-		*code = V4L2_MBUS_FMT_JPEG_1X8;
+	    fimc_fmt_is_user_defined(ctx->s_frame.fmt->color))
+		*code = ctx->s_frame.fmt->mbus_code;
 
 	if (fourcc && *fourcc != V4L2_PIX_FMT_JPEG && pad != FIMC_SD_PAD_SINK)
 		mask |= FMT_FLAGS_M2M;
@@ -628,18 +651,19 @@ static struct fimc_fmt *fimc_capture_try_format(struct fimc_ctx *ctx,
 		*fourcc = ffmt->fourcc;
 
 	if (pad == FIMC_SD_PAD_SINK) {
-		max_w = fimc_fmt_is_jpeg(ffmt->color) ?
+		max_w = fimc_fmt_is_user_defined(ffmt->color) ?
 			pl->scaler_dis_w : pl->scaler_en_w;
 		/* Apply the camera input interface pixel constraints */
 		v4l_bound_align_image(width, max_t(u32, *width, 32), max_w, 4,
 				      height, max_t(u32, *height, 32),
 				      FIMC_CAMIF_MAX_HEIGHT,
-				      fimc_fmt_is_jpeg(ffmt->color) ? 3 : 1,
+				      fimc_fmt_is_user_defined(ffmt->color) ?
+				      3 : 1,
 				      0);
 		return ffmt;
 	}
 	/* Can't scale or crop in transparent (JPEG) transfer mode */
-	if (fimc_fmt_is_jpeg(ffmt->color)) {
+	if (fimc_fmt_is_user_defined(ffmt->color)) {
 		*width  = ctx->s_frame.f_width;
 		*height = ctx->s_frame.f_height;
 		return ffmt;
@@ -684,7 +708,7 @@ static void fimc_capture_try_selection(struct fimc_ctx *ctx,
 	u32 max_sc_h, max_sc_v;
 
 	/* In JPEG transparent transfer mode cropping is not supported */
-	if (fimc_fmt_is_jpeg(ctx->d_frame.fmt->color)) {
+	if (fimc_fmt_is_user_defined(ctx->d_frame.fmt->color)) {
 		r->width  = sink->f_width;
 		r->height = sink->f_height;
 		r->left   = r->top = 0;
@@ -847,6 +871,48 @@ static int fimc_pipeline_try_format(struct fimc_ctx *ctx,
 	return 0;
 }
 
+/**
+ * fimc_get_sensor_frame_desc - query the sensor for media bus frame parameters
+ * @sensor: pointer to the sensor subdev
+ * @plane_fmt: provides plane sizes corresponding to the frame layout entries
+ * @try: true to set the frame parameters, false to query only
+ *
+ * This function is used by this driver only for compressed/blob data formats.
+ */
+static int fimc_get_sensor_frame_desc(struct v4l2_subdev *sensor,
+				      struct v4l2_plane_pix_format *plane_fmt,
+				      unsigned int num_planes, bool try)
+{
+	struct v4l2_mbus_frame_desc fd;
+	int i, ret;
+
+	for (i = 0; i < num_planes; i++)
+		fd.entry[i].length = plane_fmt[i].sizeimage;
+
+	if (try)
+		ret = v4l2_subdev_call(sensor, pad, set_frame_desc, 0, &fd);
+	else
+		ret = v4l2_subdev_call(sensor, pad, get_frame_desc, 0, &fd);
+
+	if (ret < 0)
+		return ret;
+
+	if (num_planes != fd.num_entries)
+		return -EINVAL;
+
+	for (i = 0; i < num_planes; i++)
+		plane_fmt[i].sizeimage = fd.entry[i].length;
+
+	if (fd.entry[0].length > FIMC_MAX_JPEG_BUF_SIZE) {
+		v4l2_err(sensor->v4l2_dev,  "Unsupported buffer size: %u\n",
+			 fd.entry[0].length);
+
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 static int fimc_cap_g_fmt_mplane(struct file *file, void *fh,
 				 struct v4l2_format *f)
 {
@@ -865,7 +931,7 @@ static int fimc_cap_try_fmt_mplane(struct file *file, void *fh,
 	struct v4l2_mbus_framefmt mf;
 	struct fimc_fmt *ffmt = NULL;
 
-	if (pix->pixelformat == V4L2_PIX_FMT_JPEG) {
+	if (fimc_jpeg_fourcc(pix->pixelformat)) {
 		fimc_capture_try_format(ctx, &pix->width, &pix->height,
 					NULL, &pix->pixelformat,
 					FIMC_SD_PAD_SINK);
@@ -879,25 +945,32 @@ static int fimc_cap_try_fmt_mplane(struct file *file, void *fh,
 		return -EINVAL;
 
 	if (!fimc->vid_cap.user_subdev_api) {
-		mf.width  = pix->width;
+		mf.width = pix->width;
 		mf.height = pix->height;
-		mf.code   = ffmt->mbus_code;
+		mf.code = ffmt->mbus_code;
 		fimc_md_graph_lock(fimc);
 		fimc_pipeline_try_format(ctx, &mf, &ffmt, false);
 		fimc_md_graph_unlock(fimc);
-
-		pix->width	 = mf.width;
-		pix->height	 = mf.height;
+		pix->width = mf.width;
+		pix->height = mf.height;
 		if (ffmt)
 			pix->pixelformat = ffmt->fourcc;
 	}
 
 	fimc_adjust_mplane_format(ffmt, pix->width, pix->height, pix);
+
+	if (ffmt->flags & FMT_FLAGS_COMPRESSED)
+		fimc_get_sensor_frame_desc(fimc->pipeline.subdevs[IDX_SENSOR],
+					pix->plane_fmt, ffmt->memplanes, true);
+
 	return 0;
 }
 
-static void fimc_capture_mark_jpeg_xfer(struct fimc_ctx *ctx, bool jpeg)
+static void fimc_capture_mark_jpeg_xfer(struct fimc_ctx *ctx,
+					enum fimc_color_fmt color)
 {
+	bool jpeg = fimc_fmt_is_user_defined(color);
+
 	ctx->scaler.enabled = !jpeg;
 	fimc_ctrls_activate(ctx, !jpeg);
 
@@ -920,7 +993,7 @@ static int fimc_capture_set_format(struct fimc_dev *fimc, struct v4l2_format *f)
 		return -EBUSY;
 
 	/* Pre-configure format at camera interface input, for JPEG only */
-	if (pix->pixelformat == V4L2_PIX_FMT_JPEG) {
+	if (fimc_jpeg_fourcc(pix->pixelformat)) {
 		fimc_capture_try_format(ctx, &pix->width, &pix->height,
 					NULL, &pix->pixelformat,
 					FIMC_SD_PAD_SINK);
@@ -953,7 +1026,16 @@ static int fimc_capture_set_format(struct fimc_dev *fimc, struct v4l2_format *f)
 	}
 
 	fimc_adjust_mplane_format(ff->fmt, pix->width, pix->height, pix);
-	for (i = 0; i < ff->fmt->colplanes; i++)
+
+	if (ff->fmt->flags & FMT_FLAGS_COMPRESSED) {
+		ret = fimc_get_sensor_frame_desc(fimc->pipeline.subdevs[IDX_SENSOR],
+					pix->plane_fmt, ff->fmt->memplanes,
+					true);
+		if (ret < 0)
+			return ret;
+	}
+
+	for (i = 0; i < ff->fmt->memplanes; i++)
 		ff->payload[i] = pix->plane_fmt[i].sizeimage;
 
 	set_frame_bounds(ff, pix->width, pix->height);
@@ -961,7 +1043,7 @@ static int fimc_capture_set_format(struct fimc_dev *fimc, struct v4l2_format *f)
 	if (!(ctx->state & FIMC_COMPOSE))
 		set_frame_crop(ff, 0, 0, pix->width, pix->height);
 
-	fimc_capture_mark_jpeg_xfer(ctx, fimc_fmt_is_jpeg(ff->fmt->color));
+	fimc_capture_mark_jpeg_xfer(ctx, ff->fmt->color);
 
 	/* Reset cropping and set format at the camera interface input */
 	if (!fimc->vid_cap.user_subdev_api) {
@@ -1063,6 +1145,23 @@ static int fimc_pipeline_validate(struct fimc_dev *fimc)
 		    src_fmt.format.height != sink_fmt.format.height ||
 		    src_fmt.format.code != sink_fmt.format.code)
 			return -EPIPE;
+
+		if (sd == fimc->pipeline.subdevs[IDX_SENSOR] &&
+		    fimc_user_defined_mbus_fmt(src_fmt.format.code)) {
+			struct v4l2_plane_pix_format plane_fmt[FIMC_MAX_PLANES];
+			struct fimc_frame *frame = &vid_cap->ctx->d_frame;
+			unsigned int i;
+
+			ret = fimc_get_sensor_frame_desc(sd, plane_fmt,
+							 frame->fmt->memplanes,
+							 false);
+			if (ret < 0)
+				return -EPIPE;
+
+			for (i = 0; i < frame->fmt->memplanes; i++)
+				if (frame->payload[i] < plane_fmt[i].sizeimage)
+					return -EPIPE;
+		}
 	}
 	return 0;
 }
@@ -1424,7 +1523,7 @@ static int fimc_subdev_set_fmt(struct v4l2_subdev *sd,
 	/* Update RGB Alpha control state and value range */
 	fimc_alpha_ctrl_update(ctx);
 
-	fimc_capture_mark_jpeg_xfer(ctx, fimc_fmt_is_jpeg(ffmt->color));
+	fimc_capture_mark_jpeg_xfer(ctx, ffmt->color);
 
 	ff = fmt->pad == FIMC_SD_PAD_SINK ?
 		&ctx->s_frame : &ctx->d_frame;
diff --git a/drivers/media/platform/s5p-fimc/fimc-core.c b/drivers/media/platform/s5p-fimc/fimc-core.c
index 1a445404e73..8d0d2b94a13 100644
--- a/drivers/media/platform/s5p-fimc/fimc-core.c
+++ b/drivers/media/platform/s5p-fimc/fimc-core.c
@@ -184,7 +184,17 @@ static struct fimc_fmt fimc_formats[] = {
 		.memplanes	= 1,
 		.colplanes	= 1,
 		.mbus_code	= V4L2_MBUS_FMT_JPEG_1X8,
-		.flags		= FMT_FLAGS_CAM,
+		.flags		= FMT_FLAGS_CAM | FMT_FLAGS_COMPRESSED,
+	}, {
+		.name		= "S5C73MX interleaved UYVY/JPEG",
+		.fourcc		= V4L2_PIX_FMT_S5C_UYVY_JPG,
+		.color		= FIMC_FMT_YUYV_JPEG,
+		.depth		= { 8 },
+		.memplanes	= 2,
+		.colplanes	= 1,
+		.mdataplanes	= 0x2, /* plane 1 holds frame meta data */
+		.mbus_code	= V4L2_MBUS_FMT_S5C_UYVY_JPEG_1X8,
+		.flags		= FMT_FLAGS_CAM | FMT_FLAGS_COMPRESSED,
 	},
 };
 
@@ -371,7 +381,7 @@ int fimc_prepare_addr(struct fimc_ctx *ctx, struct vb2_buffer *vb,
 		default:
 			return -EINVAL;
 		}
-	} else {
+	} else if (!frame->fmt->mdataplanes) {
 		if (frame->fmt->memplanes >= 2)
 			paddr->cb = vb2_dma_contig_plane_dma_addr(vb, 1);
 
@@ -698,6 +708,11 @@ int fimc_fill_format(struct fimc_frame *frame, struct v4l2_format *f)
 		if (frame->fmt->colplanes == 1) /* packed formats */
 			bpl = (bpl * frame->fmt->depth[0]) / 8;
 		pixm->plane_fmt[i].bytesperline = bpl;
+
+		if (frame->fmt->flags & FMT_FLAGS_COMPRESSED) {
+			pixm->plane_fmt[i].sizeimage = frame->payload[i];
+			continue;
+		}
 		pixm->plane_fmt[i].sizeimage = (frame->o_width *
 			frame->o_height * frame->fmt->depth[i]) / 8;
 	}
diff --git a/drivers/media/platform/s5p-fimc/fimc-core.h b/drivers/media/platform/s5p-fimc/fimc-core.h
index cd716ba6015..c0040d79249 100644
--- a/drivers/media/platform/s5p-fimc/fimc-core.h
+++ b/drivers/media/platform/s5p-fimc/fimc-core.h
@@ -40,6 +40,8 @@
 #define SCALER_MAX_VRATIO	64
 #define DMA_MIN_SIZE		8
 #define FIMC_CAMIF_MAX_HEIGHT	0x2000
+#define FIMC_MAX_JPEG_BUF_SIZE	(10 * SZ_1M)
+#define FIMC_MAX_PLANES		3
 
 /* indices to the clocks array */
 enum {
@@ -83,7 +85,7 @@ enum fimc_datapath {
 };
 
 enum fimc_color_fmt {
-	FIMC_FMT_RGB444 = 0x10,
+	FIMC_FMT_RGB444	= 0x10,
 	FIMC_FMT_RGB555,
 	FIMC_FMT_RGB565,
 	FIMC_FMT_RGB666,
@@ -95,14 +97,15 @@ enum fimc_color_fmt {
 	FIMC_FMT_CBYCRY422,
 	FIMC_FMT_CRYCBY422,
 	FIMC_FMT_YCBCR444_LOCAL,
-	FIMC_FMT_JPEG = 0x40,
-	FIMC_FMT_RAW8 = 0x80,
+	FIMC_FMT_RAW8 = 0x40,
 	FIMC_FMT_RAW10,
 	FIMC_FMT_RAW12,
+	FIMC_FMT_JPEG = 0x80,
+	FIMC_FMT_YUYV_JPEG = 0x100,
 };
 
+#define fimc_fmt_is_user_defined(x) (!!((x) & 0x180))
 #define fimc_fmt_is_rgb(x) (!!((x) & 0x10))
-#define fimc_fmt_is_jpeg(x) (!!((x) & 0x40))
 
 #define IS_M2M(__strt) ((__strt) == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE || \
 			__strt == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
@@ -139,6 +142,7 @@ enum fimc_color_fmt {
  * @memplanes: number of physically non-contiguous data planes
  * @colplanes: number of physically contiguous data planes
  * @depth: per plane driver's private 'number of bits per pixel'
+ * @mdataplanes: bitmask indicating meta data plane(s), (1 << plane_no)
  * @flags: flags indicating which operation mode format applies to
  */
 struct fimc_fmt {
@@ -149,12 +153,14 @@ struct fimc_fmt {
 	u16	memplanes;
 	u16	colplanes;
 	u8	depth[VIDEO_MAX_PLANES];
+	u16	mdataplanes;
 	u16	flags;
 #define FMT_FLAGS_CAM		(1 << 0)
 #define FMT_FLAGS_M2M_IN	(1 << 1)
 #define FMT_FLAGS_M2M_OUT	(1 << 2)
 #define FMT_FLAGS_M2M		(1 << 1 | 1 << 2)
 #define FMT_HAS_ALPHA		(1 << 3)
+#define FMT_FLAGS_COMPRESSED	(1 << 4)
 };
 
 /**
@@ -272,7 +278,7 @@ struct fimc_frame {
 	u32	offs_v;
 	u32	width;
 	u32	height;
-	unsigned long		payload[VIDEO_MAX_PLANES];
+	unsigned int		payload[VIDEO_MAX_PLANES];
 	struct fimc_addr	paddr;
 	struct fimc_dma_offset	dma_offset;
 	struct fimc_fmt		*fmt;
@@ -577,6 +583,18 @@ static inline int tiled_fmt(struct fimc_fmt *fmt)
 	return fmt->fourcc == V4L2_PIX_FMT_NV12MT;
 }
 
+static inline bool fimc_jpeg_fourcc(u32 pixelformat)
+{
+	return (pixelformat == V4L2_PIX_FMT_JPEG ||
+		pixelformat == V4L2_PIX_FMT_S5C_UYVY_JPG);
+}
+
+static inline bool fimc_user_defined_mbus_fmt(u32 code)
+{
+	return (code == V4L2_MBUS_FMT_JPEG_1X8 ||
+		code == V4L2_MBUS_FMT_S5C_UYVY_JPEG_1X8);
+}
+
 /* Return the alpha component bit mask */
 static inline int fimc_get_alpha_mask(struct fimc_fmt *fmt)
 {
diff --git a/drivers/media/platform/s5p-fimc/fimc-m2m.c b/drivers/media/platform/s5p-fimc/fimc-m2m.c
index 6b71d953fd1..4500e44f685 100644
--- a/drivers/media/platform/s5p-fimc/fimc-m2m.c
+++ b/drivers/media/platform/s5p-fimc/fimc-m2m.c
@@ -551,30 +551,31 @@ static int fimc_m2m_try_crop(struct fimc_ctx *ctx, struct v4l2_crop *cr)
 	return 0;
 }
 
-static int fimc_m2m_s_crop(struct file *file, void *fh, const struct v4l2_crop *cr)
+static int fimc_m2m_s_crop(struct file *file, void *fh, const struct v4l2_crop *crop)
 {
 	struct fimc_ctx *ctx = fh_to_ctx(fh);
 	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct v4l2_crop cr = *crop;
 	struct fimc_frame *f;
 	int ret;
 
-	ret = fimc_m2m_try_crop(ctx, cr);
+	ret = fimc_m2m_try_crop(ctx, &cr);
 	if (ret)
 		return ret;
 
-	f = (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) ?
+	f = (cr.type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) ?
 		&ctx->s_frame : &ctx->d_frame;
 
 	/* Check to see if scaling ratio is within supported range */
 	if (fimc_ctx_state_is_set(FIMC_DST_FMT | FIMC_SRC_FMT, ctx)) {
-		if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
-			ret = fimc_check_scaler_ratio(ctx, cr->c.width,
-					cr->c.height, ctx->d_frame.width,
+		if (cr.type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+			ret = fimc_check_scaler_ratio(ctx, cr.c.width,
+					cr.c.height, ctx->d_frame.width,
 					ctx->d_frame.height, ctx->rotation);
 		} else {
 			ret = fimc_check_scaler_ratio(ctx, ctx->s_frame.width,
-					ctx->s_frame.height, cr->c.width,
-					cr->c.height, ctx->rotation);
+					ctx->s_frame.height, cr.c.width,
+					cr.c.height, ctx->rotation);
 		}
 		if (ret) {
 			v4l2_err(&fimc->m2m.vfd, "Out of scaler range\n");
@@ -582,10 +583,10 @@ static int fimc_m2m_s_crop(struct file *file, void *fh, const struct v4l2_crop *
 		}
 	}
 
-	f->offs_h = cr->c.left;
-	f->offs_v = cr->c.top;
-	f->width  = cr->c.width;
-	f->height = cr->c.height;
+	f->offs_h = cr.c.left;
+	f->offs_v = cr.c.top;
+	f->width  = cr.c.width;
+	f->height = cr.c.height;
 
 	fimc_ctx_state_set(FIMC_PARAMS, ctx);
 
diff --git a/drivers/media/platform/s5p-fimc/fimc-reg.c b/drivers/media/platform/s5p-fimc/fimc-reg.c
index 783408fd7d5..2c9d0c06c9e 100644
--- a/drivers/media/platform/s5p-fimc/fimc-reg.c
+++ b/drivers/media/platform/s5p-fimc/fimc-reg.c
@@ -625,7 +625,7 @@ int fimc_hw_set_camera_source(struct fimc_dev *fimc,
 				cfg |= FIMC_REG_CISRCFMT_ITU601_16BIT;
 		} /* else defaults to ITU-R BT.656 8-bit */
 	} else if (cam->bus_type == FIMC_MIPI_CSI2) {
-		if (fimc_fmt_is_jpeg(f->fmt->color))
+		if (fimc_fmt_is_user_defined(f->fmt->color))
 			cfg |= FIMC_REG_CISRCFMT_ITU601_8BIT;
 	}
 
@@ -680,6 +680,7 @@ int fimc_hw_set_camera_type(struct fimc_dev *fimc,
 			tmp = FIMC_REG_CSIIMGFMT_YCBCR422_8BIT;
 			break;
 		case V4L2_MBUS_FMT_JPEG_1X8:
+		case V4L2_MBUS_FMT_S5C_UYVY_JPEG_1X8:
 			tmp = FIMC_REG_CSIIMGFMT_USER(1);
 			cfg |= FIMC_REG_CIGCTRL_CAM_JPEG;
 			break;
@@ -744,13 +745,13 @@ void fimc_hw_dis_capture(struct fimc_dev *dev)
 }
 
 /* Return an index to the buffer actually being written. */
-u32 fimc_hw_get_frame_index(struct fimc_dev *dev)
+s32 fimc_hw_get_frame_index(struct fimc_dev *dev)
 {
-	u32 reg;
+	s32 reg;
 
 	if (dev->variant->has_cistatus2) {
-		reg = readl(dev->regs + FIMC_REG_CISTATUS2) & 0x3F;
-		return reg > 0 ? --reg : reg;
+		reg = readl(dev->regs + FIMC_REG_CISTATUS2) & 0x3f;
+		return reg - 1;
 	}
 
 	reg = readl(dev->regs + FIMC_REG_CISTATUS);
@@ -759,6 +760,18 @@ u32 fimc_hw_get_frame_index(struct fimc_dev *dev)
 		FIMC_REG_CISTATUS_FRAMECNT_SHIFT;
 }
 
+/* Return an index to the buffer being written previously. */
+s32 fimc_hw_get_prev_frame_index(struct fimc_dev *dev)
+{
+	s32 reg;
+
+	if (!dev->variant->has_cistatus2)
+		return -1;
+
+	reg = readl(dev->regs + FIMC_REG_CISTATUS2);
+	return ((reg >> 7) & 0x3f) - 1;
+}
+
 /* Locking: the caller holds fimc->slock */
 void fimc_activate_capture(struct fimc_ctx *ctx)
 {
diff --git a/drivers/media/platform/s5p-fimc/fimc-reg.h b/drivers/media/platform/s5p-fimc/fimc-reg.h
index 579ac8ac03d..b6abfc7b72a 100644
--- a/drivers/media/platform/s5p-fimc/fimc-reg.h
+++ b/drivers/media/platform/s5p-fimc/fimc-reg.h
@@ -307,7 +307,8 @@ void fimc_hw_clear_irq(struct fimc_dev *dev);
 void fimc_hw_enable_scaler(struct fimc_dev *dev, bool on);
 void fimc_hw_activate_input_dma(struct fimc_dev *dev, bool on);
 void fimc_hw_dis_capture(struct fimc_dev *dev);
-u32 fimc_hw_get_frame_index(struct fimc_dev *dev);
+s32 fimc_hw_get_frame_index(struct fimc_dev *dev);
+s32 fimc_hw_get_prev_frame_index(struct fimc_dev *dev);
 void fimc_activate_capture(struct fimc_ctx *ctx);
 void fimc_deactivate_capture(struct fimc_dev *fimc);
 
diff --git a/drivers/media/platform/s5p-fimc/mipi-csis.c b/drivers/media/platform/s5p-fimc/mipi-csis.c
index e92236ac5cf..4c961b1b68e 100644
--- a/drivers/media/platform/s5p-fimc/mipi-csis.c
+++ b/drivers/media/platform/s5p-fimc/mipi-csis.c
@@ -2,7 +2,7 @@
  * Samsung S5P/EXYNOS4 SoC series MIPI-CSI receiver driver
  *
  * Copyright (C) 2011 - 2012 Samsung Electronics Co., Ltd.
- * Sylwester Nawrocki, <s.nawrocki@samsung.com>
+ * Sylwester Nawrocki <s.nawrocki@samsung.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
@@ -98,6 +98,11 @@ MODULE_PARM_DESC(debug, "Debug level (0-2)");
 #define CSIS_MAX_PIX_WIDTH		0xffff
 #define CSIS_MAX_PIX_HEIGHT		0xffff
 
+/* Non-image packet data buffers */
+#define S5PCSIS_PKTDATA_ODD		0x2000
+#define S5PCSIS_PKTDATA_EVEN		0x3000
+#define S5PCSIS_PKTDATA_SIZE		SZ_4K
+
 enum {
 	CSIS_CLK_MUX,
 	CSIS_CLK_GATE,
@@ -110,8 +115,8 @@ static char *csi_clock_name[] = {
 #define NUM_CSIS_CLOCKS	ARRAY_SIZE(csi_clock_name)
 
 static const char * const csis_supply_name[] = {
-	"vdd11", /* 1.1V or 1.2V (s5pc100) MIPI CSI suppply */
-	"vdd18", /* VDD 1.8V and MIPI CSI PLL supply */
+	"vddcore",  /* CSIS Core (1.0V, 1.1V or 1.2V) suppply */
+	"vddio",    /* CSIS I/O and PLL (1.8V) supply */
 };
 #define CSIS_NUM_SUPPLIES ARRAY_SIZE(csis_supply_name)
 
@@ -144,12 +149,18 @@ static const struct s5pcsis_event s5pcsis_events[] = {
 };
 #define S5PCSIS_NUM_EVENTS ARRAY_SIZE(s5pcsis_events)
 
+struct csis_pktbuf {
+	u32 *data;
+	unsigned int len;
+};
+
 /**
  * struct csis_state - the driver's internal state data structure
  * @lock: mutex serializing the subdev and power management operations,
  *        protecting @format and @flags members
  * @pads: CSIS pads array
  * @sd: v4l2_subdev associated with CSIS device instance
+ * @index: the hardware instance index
  * @pdev: CSIS platform device
  * @regs: mmaped I/O registers memory
  * @supplies: CSIS regulator supplies
@@ -159,12 +170,14 @@ static const struct s5pcsis_event s5pcsis_events[] = {
  * @csis_fmt: current CSIS pixel format
  * @format: common media bus format for the source and sink pad
  * @slock: spinlock protecting structure members below
+ * @pkt_buf: the frame embedded (non-image) data buffer
  * @events: MIPI-CSIS event (error) counters
  */
 struct csis_state {
 	struct mutex lock;
 	struct media_pad pads[CSIS_PADS_NUM];
 	struct v4l2_subdev sd;
+	u8 index;
 	struct platform_device *pdev;
 	void __iomem *regs;
 	struct regulator_bulk_data supplies[CSIS_NUM_SUPPLIES];
@@ -175,6 +188,7 @@ struct csis_state {
 	struct v4l2_mbus_framefmt format;
 
 	struct spinlock slock;
+	struct csis_pktbuf pkt_buf;
 	struct s5pcsis_event events[S5PCSIS_NUM_EVENTS];
 };
 
@@ -202,7 +216,11 @@ static const struct csis_pix_format s5pcsis_formats[] = {
 		.code = V4L2_MBUS_FMT_JPEG_1X8,
 		.fmt_reg = S5PCSIS_CFG_FMT_USER(1),
 		.data_alignment = 32,
-	},
+	}, {
+		.code = V4L2_MBUS_FMT_S5C_UYVY_JPEG_1X8,
+		.fmt_reg = S5PCSIS_CFG_FMT_USER(1),
+		.data_alignment = 32,
+	}
 };
 
 #define s5pcsis_write(__csis, __r, __v) writel(__v, __csis->regs + __r)
@@ -266,7 +284,7 @@ static void __s5pcsis_set_format(struct csis_state *state)
 	struct v4l2_mbus_framefmt *mf = &state->format;
 	u32 val;
 
-	v4l2_dbg(1, debug, &state->sd, "fmt: %d, %d x %d\n",
+	v4l2_dbg(1, debug, &state->sd, "fmt: %#x, %d x %d\n",
 		 mf->code, mf->width, mf->height);
 
 	/* Color format */
@@ -304,8 +322,10 @@ static void s5pcsis_set_params(struct csis_state *state)
 		val |= S5PCSIS_CTRL_ALIGN_32BIT;
 	else /* 24-bits */
 		val &= ~S5PCSIS_CTRL_ALIGN_32BIT;
-	/* Not using external clock. */
+
 	val &= ~S5PCSIS_CTRL_WCLK_EXTCLK;
+	if (pdata->wclk_source)
+		val |= S5PCSIS_CTRL_WCLK_EXTCLK;
 	s5pcsis_write(state, S5PCSIS_CTRL, val);
 
 	/* Update the shadow register. */
@@ -529,6 +549,22 @@ static int s5pcsis_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh,
 	return 0;
 }
 
+static int s5pcsis_s_rx_buffer(struct v4l2_subdev *sd, void *buf,
+			       unsigned int *size)
+{
+	struct csis_state *state = sd_to_csis_state(sd);
+	unsigned long flags;
+
+	*size = min_t(unsigned int, *size, S5PCSIS_PKTDATA_SIZE);
+
+	spin_lock_irqsave(&state->slock, flags);
+	state->pkt_buf.data = buf;
+	state->pkt_buf.len = *size;
+	spin_unlock_irqrestore(&state->slock, flags);
+
+	return 0;
+}
+
 static int s5pcsis_log_status(struct v4l2_subdev *sd)
 {
 	struct csis_state *state = sd_to_csis_state(sd);
@@ -566,6 +602,7 @@ static struct v4l2_subdev_pad_ops s5pcsis_pad_ops = {
 };
 
 static struct v4l2_subdev_video_ops s5pcsis_video_ops = {
+	.s_rx_buffer = s5pcsis_s_rx_buffer,
 	.s_stream = s5pcsis_s_stream,
 };
 
@@ -578,13 +615,26 @@ static struct v4l2_subdev_ops s5pcsis_subdev_ops = {
 static irqreturn_t s5pcsis_irq_handler(int irq, void *dev_id)
 {
 	struct csis_state *state = dev_id;
+	struct csis_pktbuf *pktbuf = &state->pkt_buf;
 	unsigned long flags;
 	u32 status;
 
 	status = s5pcsis_read(state, S5PCSIS_INTSRC);
-
 	spin_lock_irqsave(&state->slock, flags);
 
+	if ((status & S5PCSIS_INTSRC_NON_IMAGE_DATA) && pktbuf->data) {
+		u32 offset;
+
+		if (status & S5PCSIS_INTSRC_EVEN)
+			offset = S5PCSIS_PKTDATA_EVEN;
+		else
+			offset = S5PCSIS_PKTDATA_ODD;
+
+		memcpy(pktbuf->data, state->regs + offset, pktbuf->len);
+		pktbuf->data = NULL;
+		rmb();
+	}
+
 	/* Update the event/error counters */
 	if ((status & S5PCSIS_INTSRC_ERRORS) || debug) {
 		int i;
@@ -620,14 +670,15 @@ static int __devinit s5pcsis_probe(struct platform_device *pdev)
 	spin_lock_init(&state->slock);
 
 	state->pdev = pdev;
+	state->index = max(0, pdev->id);
 
 	pdata = pdev->dev.platform_data;
-	if (pdata == NULL || pdata->phy_enable == NULL) {
+	if (pdata == NULL) {
 		dev_err(&pdev->dev, "Platform data not fully specified\n");
 		return -EINVAL;
 	}
 
-	if ((pdev->id == 1 && pdata->lanes > CSIS1_MAX_LANES) ||
+	if ((state->index == 1 && pdata->lanes > CSIS1_MAX_LANES) ||
 	    pdata->lanes > CSIS0_MAX_LANES) {
 		dev_err(&pdev->dev, "Unsupported number of data lanes: %d\n",
 			pdata->lanes);
@@ -710,7 +761,6 @@ e_clkput:
 
 static int s5pcsis_pm_suspend(struct device *dev, bool runtime)
 {
-	struct s5p_platform_mipi_csis *pdata = dev->platform_data;
 	struct platform_device *pdev = to_platform_device(dev);
 	struct v4l2_subdev *sd = platform_get_drvdata(pdev);
 	struct csis_state *state = sd_to_csis_state(sd);
@@ -722,7 +772,7 @@ static int s5pcsis_pm_suspend(struct device *dev, bool runtime)
 	mutex_lock(&state->lock);
 	if (state->flags & ST_POWERED) {
 		s5pcsis_stop_stream(state);
-		ret = pdata->phy_enable(state->pdev, false);
+		ret = s5p_csis_phy_enable(state->index, false);
 		if (ret)
 			goto unlock;
 		ret = regulator_bulk_disable(CSIS_NUM_SUPPLIES,
@@ -741,7 +791,6 @@ static int s5pcsis_pm_suspend(struct device *dev, bool runtime)
 
 static int s5pcsis_pm_resume(struct device *dev, bool runtime)
 {
-	struct s5p_platform_mipi_csis *pdata = dev->platform_data;
 	struct platform_device *pdev = to_platform_device(dev);
 	struct v4l2_subdev *sd = platform_get_drvdata(pdev);
 	struct csis_state *state = sd_to_csis_state(sd);
@@ -759,7 +808,7 @@ static int s5pcsis_pm_resume(struct device *dev, bool runtime)
 					    state->supplies);
 		if (ret)
 			goto unlock;
-		ret = pdata->phy_enable(state->pdev, true);
+		ret = s5p_csis_phy_enable(state->index, true);
 		if (!ret) {
 			state->flags |= ST_POWERED;
 		} else {
diff --git a/drivers/media/platform/s5p-g2d/g2d.c b/drivers/media/platform/s5p-g2d/g2d.c
index 1e3b9dd014c..1bfbc325836 100644
--- a/drivers/media/platform/s5p-g2d/g2d.c
+++ b/drivers/media/platform/s5p-g2d/g2d.c
@@ -507,7 +507,7 @@ static int vidioc_g_crop(struct file *file, void *prv, struct v4l2_crop *cr)
 	return 0;
 }
 
-static int vidioc_try_crop(struct file *file, void *prv, struct v4l2_crop *cr)
+static int vidioc_try_crop(struct file *file, void *prv, const struct v4l2_crop *cr)
 {
 	struct g2d_ctx *ctx = prv;
 	struct g2d_dev *dev = ctx->dev;
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-core.c b/drivers/media/platform/s5p-jpeg/jpeg-core.c
index 394775ae577..17983c4c9a9 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-core.c
+++ b/drivers/media/platform/s5p-jpeg/jpeg-core.c
@@ -1353,7 +1353,7 @@ static int s5p_jpeg_probe(struct platform_device *pdev)
 		return ret;
 	}
 	dev_dbg(&pdev->dev, "clock source %p\n", jpeg->clk);
-	clk_enable(jpeg->clk);
+	clk_prepare_enable(jpeg->clk);
 
 	/* v4l2 device */
 	ret = v4l2_device_register(&pdev->dev, &jpeg->v4l2_dev);
@@ -1460,7 +1460,7 @@ device_register_rollback:
 	v4l2_device_unregister(&jpeg->v4l2_dev);
 
 clk_get_rollback:
-	clk_disable(jpeg->clk);
+	clk_disable_unprepare(jpeg->clk);
 	clk_put(jpeg->clk);
 
 	return ret;
@@ -1480,7 +1480,7 @@ static int s5p_jpeg_remove(struct platform_device *pdev)
 	v4l2_m2m_release(jpeg->m2m_dev);
 	v4l2_device_unregister(&jpeg->v4l2_dev);
 
-	clk_disable(jpeg->clk);
+	clk_disable_unprepare(jpeg->clk);
 	clk_put(jpeg->clk);
 
 	return 0;
diff --git a/drivers/media/platform/s5p-mfc/Makefile b/drivers/media/platform/s5p-mfc/Makefile
index d0663409af0..379008c6d09 100644
--- a/drivers/media/platform/s5p-mfc/Makefile
+++ b/drivers/media/platform/s5p-mfc/Makefile
@@ -1,5 +1,6 @@
 obj-$(CONFIG_VIDEO_SAMSUNG_S5P_MFC) := s5p-mfc.o
-s5p-mfc-y += s5p_mfc.o s5p_mfc_intr.o s5p_mfc_opr.o
+s5p-mfc-y += s5p_mfc.o s5p_mfc_intr.o
 s5p-mfc-y += s5p_mfc_dec.o s5p_mfc_enc.o
-s5p-mfc-y += s5p_mfc_ctrl.o s5p_mfc_cmd.o
-s5p-mfc-y += s5p_mfc_pm.o s5p_mfc_shm.o
+s5p-mfc-y += s5p_mfc_ctrl.o s5p_mfc_pm.o
+s5p-mfc-y += s5p_mfc_opr.o s5p_mfc_opr_v5.o s5p_mfc_opr_v6.o
+s5p-mfc-y += s5p_mfc_cmd.o s5p_mfc_cmd_v5.o s5p_mfc_cmd_v6.o
diff --git a/drivers/media/platform/s5p-mfc/regs-mfc-v6.h b/drivers/media/platform/s5p-mfc/regs-mfc-v6.h
new file mode 100644
index 00000000000..363a97cc768
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/regs-mfc-v6.h
@@ -0,0 +1,408 @@
+/*
+ * Register definition file for Samsung MFC V6.x Interface (FIMV) driver
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.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 _REGS_FIMV_V6_H
+#define _REGS_FIMV_V6_H
+
+#include <linux/kernel.h>
+#include <linux/sizes.h>
+
+#define S5P_FIMV_REG_SIZE_V6	(S5P_FIMV_END_ADDR - S5P_FIMV_START_ADDR)
+#define S5P_FIMV_REG_COUNT_V6	((S5P_FIMV_END_ADDR - S5P_FIMV_START_ADDR) / 4)
+
+/* Number of bits that the buffer address should be shifted for particular
+ * MFC buffers.  */
+#define S5P_FIMV_MEM_OFFSET_V6		0
+
+#define S5P_FIMV_START_ADDR_V6		0x0000
+#define S5P_FIMV_END_ADDR_V6		0xfd80
+
+#define S5P_FIMV_REG_CLEAR_BEGIN_V6	0xf000
+#define S5P_FIMV_REG_CLEAR_COUNT_V6	1024
+
+/* Codec Common Registers */
+#define S5P_FIMV_RISC_ON_V6			0x0000
+#define S5P_FIMV_RISC2HOST_INT_V6		0x003C
+#define S5P_FIMV_HOST2RISC_INT_V6		0x0044
+#define S5P_FIMV_RISC_BASE_ADDRESS_V6		0x0054
+
+#define S5P_FIMV_MFC_RESET_V6			0x1070
+
+#define S5P_FIMV_HOST2RISC_CMD_V6		0x1100
+#define S5P_FIMV_H2R_CMD_EMPTY_V6		0
+#define S5P_FIMV_H2R_CMD_SYS_INIT_V6		1
+#define S5P_FIMV_H2R_CMD_OPEN_INSTANCE_V6	2
+#define S5P_FIMV_CH_SEQ_HEADER_V6		3
+#define S5P_FIMV_CH_INIT_BUFS_V6		4
+#define S5P_FIMV_CH_FRAME_START_V6		5
+#define S5P_FIMV_H2R_CMD_CLOSE_INSTANCE_V6	6
+#define S5P_FIMV_H2R_CMD_SLEEP_V6		7
+#define S5P_FIMV_H2R_CMD_WAKEUP_V6		8
+#define S5P_FIMV_CH_LAST_FRAME_V6		9
+#define S5P_FIMV_H2R_CMD_FLUSH_V6		10
+/* RMVME: REALLOC used? */
+#define S5P_FIMV_CH_FRAME_START_REALLOC_V6	5
+
+#define S5P_FIMV_RISC2HOST_CMD_V6		0x1104
+#define S5P_FIMV_R2H_CMD_EMPTY_V6		0
+#define S5P_FIMV_R2H_CMD_SYS_INIT_RET_V6	1
+#define S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET_V6	2
+#define S5P_FIMV_R2H_CMD_SEQ_DONE_RET_V6	3
+#define S5P_FIMV_R2H_CMD_INIT_BUFFERS_RET_V6	4
+
+#define S5P_FIMV_R2H_CMD_CLOSE_INSTANCE_RET_V6	6
+#define S5P_FIMV_R2H_CMD_SLEEP_RET_V6		7
+#define S5P_FIMV_R2H_CMD_WAKEUP_RET_V6		8
+#define S5P_FIMV_R2H_CMD_COMPLETE_SEQ_RET_V6	9
+#define S5P_FIMV_R2H_CMD_DPB_FLUSH_RET_V6	10
+#define S5P_FIMV_R2H_CMD_NAL_ABORT_RET_V6	11
+#define S5P_FIMV_R2H_CMD_FW_STATUS_RET_V6	12
+#define S5P_FIMV_R2H_CMD_FRAME_DONE_RET_V6	13
+#define S5P_FIMV_R2H_CMD_FIELD_DONE_RET_V6	14
+#define S5P_FIMV_R2H_CMD_SLICE_DONE_RET_V6	15
+#define S5P_FIMV_R2H_CMD_ENC_BUFFER_FUL_RET_V6	16
+#define S5P_FIMV_R2H_CMD_ERR_RET_V6		32
+
+#define S5P_FIMV_FW_VERSION_V6			0xf000
+
+#define S5P_FIMV_INSTANCE_ID_V6			0xf008
+#define S5P_FIMV_CODEC_TYPE_V6			0xf00c
+#define S5P_FIMV_CONTEXT_MEM_ADDR_V6		0xf014
+#define S5P_FIMV_CONTEXT_MEM_SIZE_V6		0xf018
+#define S5P_FIMV_PIXEL_FORMAT_V6		0xf020
+
+#define S5P_FIMV_METADATA_ENABLE_V6		0xf024
+#define S5P_FIMV_DBG_BUFFER_ADDR_V6		0xf030
+#define S5P_FIMV_DBG_BUFFER_SIZE_V6		0xf034
+#define S5P_FIMV_RET_INSTANCE_ID_V6		0xf070
+
+#define S5P_FIMV_ERROR_CODE_V6			0xf074
+#define S5P_FIMV_ERR_WARNINGS_START_V6		160
+#define S5P_FIMV_ERR_DEC_MASK_V6		0xffff
+#define S5P_FIMV_ERR_DEC_SHIFT_V6		0
+#define S5P_FIMV_ERR_DSPL_MASK_V6		0xffff0000
+#define S5P_FIMV_ERR_DSPL_SHIFT_V6		16
+
+#define S5P_FIMV_DBG_BUFFER_OUTPUT_SIZE_V6	0xf078
+#define S5P_FIMV_METADATA_STATUS_V6		0xf07C
+#define S5P_FIMV_METADATA_ADDR_MB_INFO_V6	0xf080
+#define S5P_FIMV_METADATA_SIZE_MB_INFO_V6	0xf084
+
+/* Decoder Registers */
+#define S5P_FIMV_D_CRC_CTRL_V6			0xf0b0
+#define S5P_FIMV_D_DEC_OPTIONS_V6		0xf0b4
+#define S5P_FIMV_D_OPT_FMO_ASO_CTRL_MASK_V6	4
+#define S5P_FIMV_D_OPT_DDELAY_EN_SHIFT_V6	3
+#define S5P_FIMV_D_OPT_LF_CTRL_SHIFT_V6		1
+#define S5P_FIMV_D_OPT_LF_CTRL_MASK_V6		0x3
+#define S5P_FIMV_D_OPT_TILE_MODE_SHIFT_V6	0
+
+#define S5P_FIMV_D_DISPLAY_DELAY_V6		0xf0b8
+
+#define S5P_FIMV_D_SET_FRAME_WIDTH_V6		0xf0bc
+#define S5P_FIMV_D_SET_FRAME_HEIGHT_V6		0xf0c0
+
+#define S5P_FIMV_D_SEI_ENABLE_V6		0xf0c4
+
+/* Buffer setting registers */
+#define S5P_FIMV_D_MIN_NUM_DPB_V6		0xf0f0
+#define S5P_FIMV_D_MIN_LUMA_DPB_SIZE_V6		0xf0f4
+#define S5P_FIMV_D_MIN_CHROMA_DPB_SIZE_V6	0xf0f8
+#define S5P_FIMV_D_MVC_NUM_VIEWS_V6		0xf0fc
+#define S5P_FIMV_D_MIN_NUM_MV_V6		0xf100
+#define S5P_FIMV_D_NUM_DPB_V6			0xf130
+#define S5P_FIMV_D_LUMA_DPB_SIZE_V6		0xf134
+#define S5P_FIMV_D_CHROMA_DPB_SIZE_V6		0xf138
+#define S5P_FIMV_D_MV_BUFFER_SIZE_V6		0xf13c
+
+#define S5P_FIMV_D_LUMA_DPB_V6			0xf140
+#define S5P_FIMV_D_CHROMA_DPB_V6		0xf240
+#define S5P_FIMV_D_MV_BUFFER_V6			0xf340
+
+#define S5P_FIMV_D_SCRATCH_BUFFER_ADDR_V6	0xf440
+#define S5P_FIMV_D_SCRATCH_BUFFER_SIZE_V6	0xf444
+#define S5P_FIMV_D_METADATA_BUFFER_ADDR_V6	0xf448
+#define S5P_FIMV_D_METADATA_BUFFER_SIZE_V6	0xf44c
+#define S5P_FIMV_D_NUM_MV_V6			0xf478
+#define S5P_FIMV_D_CPB_BUFFER_ADDR_V6		0xf4b0
+#define S5P_FIMV_D_CPB_BUFFER_SIZE_V6		0xf4b4
+
+#define S5P_FIMV_D_AVAILABLE_DPB_FLAG_UPPER_V6	0xf4b8
+#define S5P_FIMV_D_AVAILABLE_DPB_FLAG_LOWER_V6	0xf4bc
+#define S5P_FIMV_D_CPB_BUFFER_OFFSET_V6		0xf4c0
+#define S5P_FIMV_D_SLICE_IF_ENABLE_V6		0xf4c4
+#define S5P_FIMV_D_PICTURE_TAG_V6		0xf4c8
+#define S5P_FIMV_D_STREAM_DATA_SIZE_V6		0xf4d0
+
+/* Display information register */
+#define S5P_FIMV_D_DISPLAY_FRAME_WIDTH_V6	0xf500
+#define S5P_FIMV_D_DISPLAY_FRAME_HEIGHT_V6	0xf504
+
+/* Display status */
+#define S5P_FIMV_D_DISPLAY_STATUS_V6		0xf508
+
+#define S5P_FIMV_D_DISPLAY_LUMA_ADDR_V6		0xf50c
+#define S5P_FIMV_D_DISPLAY_CHROMA_ADDR_V6	0xf510
+
+#define S5P_FIMV_D_DISPLAY_FRAME_TYPE_V6	0xf514
+
+#define S5P_FIMV_D_DISPLAY_CROP_INFO1_V6	0xf518
+#define S5P_FIMV_D_DISPLAY_CROP_INFO2_V6	0xf51c
+#define S5P_FIMV_D_DISPLAY_PICTURE_PROFILE_V6	0xf520
+#define S5P_FIMV_D_DISPLAY_LUMA_CRC_TOP_V6	0xf524
+#define S5P_FIMV_D_DISPLAY_CHROMA_CRC_TOP_V6	0xf528
+#define S5P_FIMV_D_DISPLAY_LUMA_CRC_BOT_V6	0xf52c
+#define S5P_FIMV_D_DISPLAY_CHROMA_CRC_BOT_V6	0xf530
+#define S5P_FIMV_D_DISPLAY_ASPECT_RATIO_V6	0xf534
+#define S5P_FIMV_D_DISPLAY_EXTENDED_AR_V6	0xf538
+
+/* Decoded picture information register */
+#define S5P_FIMV_D_DECODED_FRAME_WIDTH_V6	0xf53c
+#define S5P_FIMV_D_DECODED_FRAME_HEIGHT_V6	0xf540
+#define S5P_FIMV_D_DECODED_STATUS_V6		0xf544
+#define S5P_FIMV_DEC_CRC_GEN_MASK_V6		0x1
+#define S5P_FIMV_DEC_CRC_GEN_SHIFT_V6		6
+
+#define S5P_FIMV_D_DECODED_LUMA_ADDR_V6		0xf548
+#define S5P_FIMV_D_DECODED_CHROMA_ADDR_V6	0xf54c
+
+#define S5P_FIMV_D_DECODED_FRAME_TYPE_V6	0xf550
+#define S5P_FIMV_DECODE_FRAME_MASK_V6		7
+
+#define S5P_FIMV_D_DECODED_CROP_INFO1_V6	0xf554
+#define S5P_FIMV_D_DECODED_CROP_INFO2_V6	0xf558
+#define S5P_FIMV_D_DECODED_PICTURE_PROFILE_V6	0xf55c
+#define S5P_FIMV_D_DECODED_NAL_SIZE_V6		0xf560
+#define S5P_FIMV_D_DECODED_LUMA_CRC_TOP_V6	0xf564
+#define S5P_FIMV_D_DECODED_CHROMA_CRC_TOP_V6	0xf568
+#define S5P_FIMV_D_DECODED_LUMA_CRC_BOT_V6	0xf56c
+#define S5P_FIMV_D_DECODED_CHROMA_CRC_BOT_V6	0xf570
+
+/* Returned value register for specific setting */
+#define S5P_FIMV_D_RET_PICTURE_TAG_TOP_V6		0xf574
+#define S5P_FIMV_D_RET_PICTURE_TAG_BOT_V6		0xf578
+#define S5P_FIMV_D_RET_PICTURE_TIME_TOP_V6		0xf57c
+#define S5P_FIMV_D_RET_PICTURE_TIME_BOT_V6		0xf580
+#define S5P_FIMV_D_CHROMA_FORMAT_V6			0xf588
+#define S5P_FIMV_D_MPEG4_INFO_V6			0xf58c
+#define S5P_FIMV_D_H264_INFO_V6				0xf590
+
+#define S5P_FIMV_D_METADATA_ADDR_CONCEALED_MB_V6	0xf594
+#define S5P_FIMV_D_METADATA_SIZE_CONCEALED_MB_V6	0xf598
+#define S5P_FIMV_D_METADATA_ADDR_VC1_PARAM_V6		0xf59c
+#define S5P_FIMV_D_METADATA_SIZE_VC1_PARAM_V6		0xf5a0
+#define S5P_FIMV_D_METADATA_ADDR_SEI_NAL_V6		0xf5a4
+#define S5P_FIMV_D_METADATA_SIZE_SEI_NAL_V6		0xf5a8
+#define S5P_FIMV_D_METADATA_ADDR_VUI_V6			0xf5ac
+#define S5P_FIMV_D_METADATA_SIZE_VUI_V6			0xf5b0
+
+#define S5P_FIMV_D_MVC_VIEW_ID_V6		0xf5b4
+
+/* SEI related information */
+#define S5P_FIMV_D_FRAME_PACK_SEI_AVAIL_V6	0xf5f0
+#define S5P_FIMV_D_FRAME_PACK_ARRGMENT_ID_V6	0xf5f4
+#define S5P_FIMV_D_FRAME_PACK_SEI_INFO_V6	0xf5f8
+#define S5P_FIMV_D_FRAME_PACK_GRID_POS_V6	0xf5fc
+
+/* Encoder Registers */
+#define S5P_FIMV_E_FRAME_WIDTH_V6		0xf770
+#define S5P_FIMV_E_FRAME_HEIGHT_V6		0xf774
+#define S5P_FIMV_E_CROPPED_FRAME_WIDTH_V6	0xf778
+#define S5P_FIMV_E_CROPPED_FRAME_HEIGHT_V6	0xf77c
+#define S5P_FIMV_E_FRAME_CROP_OFFSET_V6		0xf780
+#define S5P_FIMV_E_ENC_OPTIONS_V6		0xf784
+#define S5P_FIMV_E_PICTURE_PROFILE_V6		0xf788
+#define S5P_FIMV_E_FIXED_PICTURE_QP_V6		0xf790
+
+#define S5P_FIMV_E_RC_CONFIG_V6			0xf794
+#define S5P_FIMV_E_RC_QP_BOUND_V6		0xf798
+#define S5P_FIMV_E_RC_RPARAM_V6			0xf79c
+#define S5P_FIMV_E_MB_RC_CONFIG_V6		0xf7a0
+#define S5P_FIMV_E_PADDING_CTRL_V6		0xf7a4
+#define S5P_FIMV_E_MV_HOR_RANGE_V6		0xf7ac
+#define S5P_FIMV_E_MV_VER_RANGE_V6		0xf7b0
+
+#define S5P_FIMV_E_VBV_BUFFER_SIZE_V6		0xf84c
+#define S5P_FIMV_E_VBV_INIT_DELAY_V6		0xf850
+#define S5P_FIMV_E_NUM_DPB_V6			0xf890
+#define S5P_FIMV_E_LUMA_DPB_V6			0xf8c0
+#define S5P_FIMV_E_CHROMA_DPB_V6		0xf904
+#define S5P_FIMV_E_ME_BUFFER_V6			0xf948
+
+#define S5P_FIMV_E_SCRATCH_BUFFER_ADDR_V6	0xf98c
+#define S5P_FIMV_E_SCRATCH_BUFFER_SIZE_V6	0xf990
+#define S5P_FIMV_E_TMV_BUFFER0_V6		0xf994
+#define S5P_FIMV_E_TMV_BUFFER1_V6		0xf998
+#define S5P_FIMV_E_SOURCE_LUMA_ADDR_V6		0xf9f0
+#define S5P_FIMV_E_SOURCE_CHROMA_ADDR_V6	0xf9f4
+#define S5P_FIMV_E_STREAM_BUFFER_ADDR_V6	0xf9f8
+#define S5P_FIMV_E_STREAM_BUFFER_SIZE_V6	0xf9fc
+#define S5P_FIMV_E_ROI_BUFFER_ADDR_V6		0xfA00
+
+#define S5P_FIMV_E_PARAM_CHANGE_V6		0xfa04
+#define S5P_FIMV_E_IR_SIZE_V6			0xfa08
+#define S5P_FIMV_E_GOP_CONFIG_V6		0xfa0c
+#define S5P_FIMV_E_MSLICE_MODE_V6		0xfa10
+#define S5P_FIMV_E_MSLICE_SIZE_MB_V6		0xfa14
+#define S5P_FIMV_E_MSLICE_SIZE_BITS_V6		0xfa18
+#define S5P_FIMV_E_FRAME_INSERTION_V6		0xfa1c
+
+#define S5P_FIMV_E_RC_FRAME_RATE_V6		0xfa20
+#define S5P_FIMV_E_RC_BIT_RATE_V6		0xfa24
+#define S5P_FIMV_E_RC_QP_OFFSET_V6		0xfa28
+#define S5P_FIMV_E_RC_ROI_CTRL_V6		0xfa2c
+#define S5P_FIMV_E_PICTURE_TAG_V6		0xfa30
+#define S5P_FIMV_E_BIT_COUNT_ENABLE_V6		0xfa34
+#define S5P_FIMV_E_MAX_BIT_COUNT_V6		0xfa38
+#define S5P_FIMV_E_MIN_BIT_COUNT_V6		0xfa3c
+
+#define S5P_FIMV_E_METADATA_BUFFER_ADDR_V6		0xfa40
+#define S5P_FIMV_E_METADATA_BUFFER_SIZE_V6		0xfa44
+#define S5P_FIMV_E_STREAM_SIZE_V6			0xfa80
+#define S5P_FIMV_E_SLICE_TYPE_V6			0xfa84
+#define S5P_FIMV_E_PICTURE_COUNT_V6			0xfa88
+#define S5P_FIMV_E_RET_PICTURE_TAG_V6			0xfa8c
+#define S5P_FIMV_E_STREAM_BUFFER_WRITE_POINTER_V6	0xfa90
+
+#define S5P_FIMV_E_ENCODED_SOURCE_LUMA_ADDR_V6		0xfa94
+#define S5P_FIMV_E_ENCODED_SOURCE_CHROMA_ADDR_V6	0xfa98
+#define S5P_FIMV_E_RECON_LUMA_DPB_ADDR_V6		0xfa9c
+#define S5P_FIMV_E_RECON_CHROMA_DPB_ADDR_V6		0xfaa0
+#define S5P_FIMV_E_METADATA_ADDR_ENC_SLICE_V6		0xfaa4
+#define S5P_FIMV_E_METADATA_SIZE_ENC_SLICE_V6		0xfaa8
+
+#define S5P_FIMV_E_MPEG4_OPTIONS_V6		0xfb10
+#define S5P_FIMV_E_MPEG4_HEC_PERIOD_V6		0xfb14
+#define S5P_FIMV_E_ASPECT_RATIO_V6		0xfb50
+#define S5P_FIMV_E_EXTENDED_SAR_V6		0xfb54
+
+#define S5P_FIMV_E_H264_OPTIONS_V6		0xfb58
+#define S5P_FIMV_E_H264_LF_ALPHA_OFFSET_V6	0xfb5c
+#define S5P_FIMV_E_H264_LF_BETA_OFFSET_V6	0xfb60
+#define S5P_FIMV_E_H264_I_PERIOD_V6		0xfb64
+
+#define S5P_FIMV_E_H264_FMO_SLICE_GRP_MAP_TYPE_V6		0xfb68
+#define S5P_FIMV_E_H264_FMO_NUM_SLICE_GRP_MINUS1_V6		0xfb6c
+#define S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_DIR_V6		0xfb70
+#define S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_RATE_MINUS1_V6	0xfb74
+#define S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_0_V6		0xfb78
+#define S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_1_V6		0xfb7c
+#define S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_2_V6		0xfb80
+#define S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_3_V6		0xfb84
+
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_0_V6	0xfb88
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_1_V6	0xfb8c
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_2_V6	0xfb90
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_3_V6	0xfb94
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_4_V6	0xfb98
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_5_V6	0xfb9c
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_6_V6	0xfba0
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_7_V6	0xfba4
+
+#define S5P_FIMV_E_H264_CHROMA_QP_OFFSET_V6	0xfba8
+#define S5P_FIMV_E_H264_NUM_T_LAYER_V6		0xfbac
+
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER0_V6	0xfbb0
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER1_V6	0xfbb4
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER2_V6	0xfbb8
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER3_V6	0xfbbc
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER4_V6	0xfbc0
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER5_V6	0xfbc4
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER6_V6	0xfbc8
+
+#define S5P_FIMV_E_H264_FRAME_PACKING_SEI_INFO_V6		0xfc4c
+#define S5P_FIMV_ENC_FP_ARRANGEMENT_TYPE_SIDE_BY_SIDE_V6	0
+#define S5P_FIMV_ENC_FP_ARRANGEMENT_TYPE_TOP_BOTTOM_V6		1
+#define S5P_FIMV_ENC_FP_ARRANGEMENT_TYPE_TEMPORAL_V6		2
+
+#define S5P_FIMV_E_MVC_FRAME_QP_VIEW1_V6		0xfd40
+#define S5P_FIMV_E_MVC_RC_FRAME_RATE_VIEW1_V6		0xfd44
+#define S5P_FIMV_E_MVC_RC_BIT_RATE_VIEW1_V6		0xfd48
+#define S5P_FIMV_E_MVC_RC_QBOUND_VIEW1_V6		0xfd4c
+#define S5P_FIMV_E_MVC_RC_RPARA_VIEW1_V6		0xfd50
+#define S5P_FIMV_E_MVC_INTER_VIEW_PREDICTION_ON_V6	0xfd80
+
+/* Codec numbers  */
+#define S5P_FIMV_CODEC_NONE_V6		-1
+
+
+#define S5P_FIMV_CODEC_H264_DEC_V6	0
+#define S5P_FIMV_CODEC_H264_MVC_DEC_V6	1
+
+#define S5P_FIMV_CODEC_MPEG4_DEC_V6	3
+#define S5P_FIMV_CODEC_FIMV1_DEC_V6	4
+#define S5P_FIMV_CODEC_FIMV2_DEC_V6	5
+#define S5P_FIMV_CODEC_FIMV3_DEC_V6	6
+#define S5P_FIMV_CODEC_FIMV4_DEC_V6	7
+#define S5P_FIMV_CODEC_H263_DEC_V6	8
+#define S5P_FIMV_CODEC_VC1RCV_DEC_V6	9
+#define S5P_FIMV_CODEC_VC1_DEC_V6	10
+/* FIXME: Add 11~12 */
+#define S5P_FIMV_CODEC_MPEG2_DEC_V6	13
+#define S5P_FIMV_CODEC_VP8_DEC_V6	14
+/* FIXME: Add 15~16 */
+#define S5P_FIMV_CODEC_H264_ENC_V6	20
+#define S5P_FIMV_CODEC_H264_MVC_ENC_V6	21
+
+#define S5P_FIMV_CODEC_MPEG4_ENC_V6	23
+#define S5P_FIMV_CODEC_H263_ENC_V6	24
+
+#define S5P_FIMV_NV12M_HALIGN_V6		16
+#define S5P_FIMV_NV12MT_HALIGN_V6		16
+#define S5P_FIMV_NV12MT_VALIGN_V6		16
+
+#define S5P_FIMV_TMV_BUFFER_ALIGN_V6		16
+#define S5P_FIMV_LUMA_DPB_BUFFER_ALIGN_V6	256
+#define S5P_FIMV_CHROMA_DPB_BUFFER_ALIGN_V6	256
+#define S5P_FIMV_ME_BUFFER_ALIGN_V6		256
+#define S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6	256
+
+#define S5P_FIMV_LUMA_MB_TO_PIXEL_V6		256
+#define S5P_FIMV_CHROMA_MB_TO_PIXEL_V6		128
+#define S5P_FIMV_NUM_TMV_BUFFERS_V6		2
+
+#define S5P_FIMV_MAX_FRAME_SIZE_V6		(2 * SZ_1M)
+#define S5P_FIMV_NUM_PIXELS_IN_MB_ROW_V6	16
+#define S5P_FIMV_NUM_PIXELS_IN_MB_COL_V6	16
+
+/* Buffer size requirements defined by hardware */
+#define S5P_FIMV_TMV_BUFFER_SIZE_V6(w, h)	(((w) + 1) * ((h) + 1) * 8)
+#define S5P_FIMV_ME_BUFFER_SIZE_V6(imw, imh, mbw, mbh) \
+	((DIV_ROUND_UP(imw, 64) *  DIV_ROUND_UP(imh, 64) * 256) + \
+	 (DIV_ROUND_UP((mbw) * (mbh), 32) * 16))
+#define S5P_FIMV_SCRATCH_BUF_SIZE_H264_DEC_V6(w, h)	(((w) * 192) + 64)
+#define S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_DEC_V6(w, h) \
+			((w) * ((h) * 64 + 144) + (2048/16 * (h) * 64) + \
+			 (2048/16 * 256 + 8320))
+#define S5P_FIMV_SCRATCH_BUF_SIZE_VC1_DEC_V6(w, h) \
+						(2096 * ((w) + (h) + 1))
+#define S5P_FIMV_SCRATCH_BUF_SIZE_H263_DEC_V6(w, h)	((w) * 400)
+#define S5P_FIMV_SCRATCH_BUF_SIZE_VP8_DEC_V6(w, h) \
+			((w) * 32 + (h) * 128 + (((w) + 1) / 2) * 64 + 2112)
+#define S5P_FIMV_SCRATCH_BUF_SIZE_H264_ENC_V6(w, h) \
+			(((w) * 64) + (((w) + 1) * 16) + (4096 * 16))
+#define S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_ENC_V6(w, h) \
+			(((w) * 16) + (((w) + 1) * 16))
+
+/* MFC Context buffer sizes */
+#define MFC_CTX_BUF_SIZE_V6		(28 * SZ_1K)	/*  28KB */
+#define MFC_H264_DEC_CTX_BUF_SIZE_V6	(2 * SZ_1M)	/*  2MB */
+#define MFC_OTHER_DEC_CTX_BUF_SIZE_V6	(20 * SZ_1K)	/*  20KB */
+#define MFC_H264_ENC_CTX_BUF_SIZE_V6	(100 * SZ_1K)	/* 100KB */
+#define MFC_OTHER_ENC_CTX_BUF_SIZE_V6	(12 * SZ_1K)	/*  12KB */
+
+/* MFCv6 variant defines */
+#define MAX_FW_SIZE_V6			(SZ_1M)		/* 1MB */
+#define MAX_CPB_SIZE_V6			(3 * SZ_1M)	/* 3MB */
+#define MFC_VERSION_V6			0x61
+#define MFC_NUM_PORTS_V6		1
+
+#endif /* _REGS_FIMV_V6_H */
diff --git a/drivers/media/platform/s5p-mfc/regs-mfc.h b/drivers/media/platform/s5p-mfc/regs-mfc.h
index a19bece41ba..9319e93599a 100644
--- a/drivers/media/platform/s5p-mfc/regs-mfc.h
+++ b/drivers/media/platform/s5p-mfc/regs-mfc.h
@@ -12,6 +12,9 @@
 #ifndef _REGS_FIMV_H
 #define _REGS_FIMV_H
 
+#include <linux/kernel.h>
+#include <linux/sizes.h>
+
 #define S5P_FIMV_REG_SIZE	(S5P_FIMV_END_ADDR - S5P_FIMV_START_ADDR)
 #define S5P_FIMV_REG_COUNT	((S5P_FIMV_END_ADDR - S5P_FIMV_START_ADDR) / 4)
 
@@ -144,6 +147,7 @@
 #define S5P_FIMV_ENC_PROFILE_H264_MAIN			0
 #define S5P_FIMV_ENC_PROFILE_H264_HIGH			1
 #define S5P_FIMV_ENC_PROFILE_H264_BASELINE		2
+#define S5P_FIMV_ENC_PROFILE_H264_CONSTRAINED_BASELINE	3
 #define S5P_FIMV_ENC_PROFILE_MPEG4_SIMPLE		0
 #define S5P_FIMV_ENC_PROFILE_MPEG4_ADVANCED_SIMPLE	1
 #define S5P_FIMV_ENC_PIC_STRUCT		0x083c /* picture field/frame flag */
@@ -213,6 +217,7 @@
 #define S5P_FIMV_DEC_STATUS_RESOLUTION_MASK		(3<<4)
 #define S5P_FIMV_DEC_STATUS_RESOLUTION_INC		(1<<4)
 #define S5P_FIMV_DEC_STATUS_RESOLUTION_DEC		(2<<4)
+#define S5P_FIMV_DEC_STATUS_RESOLUTION_SHIFT		4
 
 /* Decode frame address */
 #define S5P_FIMV_DECODE_Y_ADR			0x2024
@@ -377,6 +382,16 @@
 #define S5P_FIMV_R2H_CMD_EDFU_INIT_RET		16
 #define S5P_FIMV_R2H_CMD_ERR_RET		32
 
+/* Dummy definition for MFCv6 compatibilty */
+#define S5P_FIMV_CODEC_H264_MVC_DEC		-1
+#define S5P_FIMV_R2H_CMD_FIELD_DONE_RET		-1
+#define S5P_FIMV_MFC_RESET			-1
+#define S5P_FIMV_RISC_ON			-1
+#define S5P_FIMV_RISC_BASE_ADDRESS		-1
+#define S5P_FIMV_CODEC_VP8_DEC			-1
+#define S5P_FIMV_REG_CLEAR_BEGIN		0
+#define S5P_FIMV_REG_CLEAR_COUNT		0
+
 /* Error handling defines */
 #define S5P_FIMV_ERR_WARNINGS_START		145
 #define S5P_FIMV_ERR_DEC_MASK			0xFFFF
@@ -414,5 +429,31 @@
 #define S5P_FIMV_SHARED_EXTENDED_SAR		0x0078
 #define S5P_FIMV_SHARED_H264_I_PERIOD		0x009C
 #define S5P_FIMV_SHARED_RC_CONTROL_CONFIG	0x00A0
+#define S5P_FIMV_SHARED_DISP_FRAME_TYPE_SHIFT	2
+
+/* Offset used by the hardware to store addresses */
+#define MFC_OFFSET_SHIFT	11
+
+#define FIRMWARE_ALIGN		(128 * SZ_1K)	/* 128KB */
+#define MFC_H264_CTX_BUF_SIZE	(600 * SZ_1K)	/* 600KB per H264 instance */
+#define MFC_CTX_BUF_SIZE	(10 * SZ_1K)	/* 10KB per instance */
+#define DESC_BUF_SIZE		(128 * SZ_1K)	/* 128KB for DESC buffer */
+#define SHARED_BUF_SIZE		(8 * SZ_1K)	/* 8KB for shared buffer */
+
+#define DEF_CPB_SIZE		(256 * SZ_1K)	/* 256KB */
+#define MAX_CPB_SIZE		(4 * SZ_1M)	/* 4MB */
+#define MAX_FW_SIZE		(384 * SZ_1K)
+
+#define MFC_VERSION		0x51
+#define MFC_NUM_PORTS		2
+
+#define S5P_FIMV_SHARED_FRAME_PACK_SEI_AVAIL    0x16C
+#define S5P_FIMV_SHARED_FRAME_PACK_ARRGMENT_ID  0x170
+#define S5P_FIMV_SHARED_FRAME_PACK_SEI_INFO     0x174
+#define S5P_FIMV_SHARED_FRAME_PACK_GRID_POS     0x178
+
+/* Values for resolution change in display status */
+#define S5P_FIMV_RES_INCREASE	1
+#define S5P_FIMV_RES_DECREASE	2
 
 #endif /* _REGS_FIMV_H */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc.c b/drivers/media/platform/s5p-mfc/s5p_mfc.c
index 5587ef15ca4..130f4ac8649 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc.c
@@ -22,15 +22,15 @@
 #include <media/v4l2-event.h>
 #include <linux/workqueue.h>
 #include <media/videobuf2-core.h>
-#include "regs-mfc.h"
+#include "s5p_mfc_common.h"
 #include "s5p_mfc_ctrl.h"
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_dec.h"
 #include "s5p_mfc_enc.h"
 #include "s5p_mfc_intr.h"
 #include "s5p_mfc_opr.h"
+#include "s5p_mfc_cmd.h"
 #include "s5p_mfc_pm.h"
-#include "s5p_mfc_shm.h"
 
 #define S5P_MFC_NAME		"s5p-mfc"
 #define S5P_MFC_DEC_NAME	"s5p-mfc-dec"
@@ -149,10 +149,12 @@ static void s5p_mfc_watchdog_worker(struct work_struct *work)
 		if (!ctx)
 			continue;
 		ctx->state = MFCINST_ERROR;
-		s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
-		s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
+		s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->dst_queue,
+				&ctx->vq_dst);
+		s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->src_queue,
+				&ctx->vq_src);
 		clear_work_bit(ctx);
-		wake_up_ctx(ctx, S5P_FIMV_R2H_CMD_ERR_RET, 0);
+		wake_up_ctx(ctx, S5P_MFC_R2H_CMD_ERR_RET, 0);
 	}
 	clear_bit(0, &dev->hw_lock);
 	spin_unlock_irqrestore(&dev->irqlock, flags);
@@ -199,6 +201,7 @@ static void s5p_mfc_clear_int_flags(struct s5p_mfc_dev *dev)
 static void s5p_mfc_handle_frame_all_extracted(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_buf *dst_buf;
+	struct s5p_mfc_dev *dev = ctx->dev;
 
 	ctx->state = MFCINST_FINISHED;
 	ctx->sequence++;
@@ -213,8 +216,8 @@ static void s5p_mfc_handle_frame_all_extracted(struct s5p_mfc_ctx *ctx)
 		ctx->dst_queue_cnt--;
 		dst_buf->b->v4l2_buf.sequence = (ctx->sequence++);
 
-		if (s5p_mfc_read_shm(ctx, PIC_TIME_TOP) ==
-			s5p_mfc_read_shm(ctx, PIC_TIME_BOT))
+		if (s5p_mfc_hw_call(dev->mfc_ops, get_pic_type_top, ctx) ==
+			s5p_mfc_hw_call(dev->mfc_ops, get_pic_type_bot, ctx))
 			dst_buf->b->v4l2_buf.field = V4L2_FIELD_NONE;
 		else
 			dst_buf->b->v4l2_buf.field = V4L2_FIELD_INTERLACED;
@@ -228,8 +231,11 @@ static void s5p_mfc_handle_frame_copy_time(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	struct s5p_mfc_buf  *dst_buf, *src_buf;
-	size_t dec_y_addr = s5p_mfc_get_dec_y_adr();
-	unsigned int frame_type = s5p_mfc_get_frame_type();
+	size_t dec_y_addr;
+	unsigned int frame_type;
+
+	dec_y_addr = s5p_mfc_hw_call(dev->mfc_ops, get_dec_y_adr, dev);
+	frame_type = s5p_mfc_hw_call(dev->mfc_ops, get_dec_frame_type, dev);
 
 	/* Copy timestamp / timecode from decoded src to dst and set
 	   appropraite flags */
@@ -265,10 +271,13 @@ static void s5p_mfc_handle_frame_new(struct s5p_mfc_ctx *ctx, unsigned int err)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	struct s5p_mfc_buf  *dst_buf;
-	size_t dspl_y_addr = s5p_mfc_get_dspl_y_adr();
-	unsigned int frame_type = s5p_mfc_get_frame_type();
+	size_t dspl_y_addr;
+	unsigned int frame_type;
 	unsigned int index;
 
+	dspl_y_addr = s5p_mfc_hw_call(dev->mfc_ops, get_dspl_y_adr, dev);
+	frame_type = s5p_mfc_hw_call(dev->mfc_ops, get_dec_frame_type, dev);
+
 	/* If frame is same as previous then skip and do not dequeue */
 	if (frame_type == S5P_FIMV_DECODE_FRAME_SKIPPED) {
 		if (!ctx->after_packed_pb)
@@ -285,8 +294,10 @@ static void s5p_mfc_handle_frame_new(struct s5p_mfc_ctx *ctx, unsigned int err)
 			list_del(&dst_buf->list);
 			ctx->dst_queue_cnt--;
 			dst_buf->b->v4l2_buf.sequence = ctx->sequence;
-			if (s5p_mfc_read_shm(ctx, PIC_TIME_TOP) ==
-				s5p_mfc_read_shm(ctx, PIC_TIME_BOT))
+			if (s5p_mfc_hw_call(dev->mfc_ops,
+					get_pic_type_top, ctx) ==
+				s5p_mfc_hw_call(dev->mfc_ops,
+					get_pic_type_bot, ctx))
 				dst_buf->b->v4l2_buf.field = V4L2_FIELD_NONE;
 			else
 				dst_buf->b->v4l2_buf.field =
@@ -317,21 +328,23 @@ static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
 
 	unsigned int index;
 
-	dst_frame_status = s5p_mfc_get_dspl_status()
+	dst_frame_status = s5p_mfc_hw_call(dev->mfc_ops, get_dspl_status, dev)
 				& S5P_FIMV_DEC_STATUS_DECODING_STATUS_MASK;
-	res_change = s5p_mfc_get_dspl_status()
-				& S5P_FIMV_DEC_STATUS_RESOLUTION_MASK;
+	res_change = (s5p_mfc_hw_call(dev->mfc_ops, get_dspl_status, dev)
+				& S5P_FIMV_DEC_STATUS_RESOLUTION_MASK)
+				>> S5P_FIMV_DEC_STATUS_RESOLUTION_SHIFT;
 	mfc_debug(2, "Frame Status: %x\n", dst_frame_status);
 	if (ctx->state == MFCINST_RES_CHANGE_INIT)
 		ctx->state = MFCINST_RES_CHANGE_FLUSH;
-	if (res_change) {
+	if (res_change == S5P_FIMV_RES_INCREASE ||
+		res_change == S5P_FIMV_RES_DECREASE) {
 		ctx->state = MFCINST_RES_CHANGE_INIT;
-		s5p_mfc_clear_int_flags(dev);
+		s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 		wake_up_ctx(ctx, reason, err);
 		if (test_and_clear_bit(0, &dev->hw_lock) == 0)
 			BUG();
 		s5p_mfc_clock_off();
-		s5p_mfc_try_run(dev);
+		s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 		return;
 	}
 	if (ctx->dpb_flush_flag)
@@ -365,9 +378,12 @@ static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
 		&& !list_empty(&ctx->src_queue)) {
 		src_buf = list_entry(ctx->src_queue.next, struct s5p_mfc_buf,
 								list);
-		ctx->consumed_stream += s5p_mfc_get_consumed_stream();
-		if (ctx->codec_mode != S5P_FIMV_CODEC_H264_DEC &&
-			s5p_mfc_get_frame_type() == S5P_FIMV_DECODE_FRAME_P_FRAME
+		ctx->consumed_stream += s5p_mfc_hw_call(dev->mfc_ops,
+						get_consumed_stream, dev);
+		if (ctx->codec_mode != S5P_MFC_CODEC_H264_DEC &&
+			s5p_mfc_hw_call(dev->mfc_ops,
+				get_dec_frame_type, dev) ==
+					S5P_FIMV_DECODE_FRAME_P_FRAME
 					&& ctx->consumed_stream + STUFF_BYTE <
 					src_buf->b->v4l2_planes[0].bytesused) {
 			/* Run MFC again on the same buffer */
@@ -379,7 +395,7 @@ static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
 			ctx->consumed_stream = 0;
 			list_del(&src_buf->list);
 			ctx->src_queue_cnt--;
-			if (s5p_mfc_err_dec(err) > 0)
+			if (s5p_mfc_hw_call(dev->mfc_ops, err_dec, err) > 0)
 				vb2_buffer_done(src_buf->b, VB2_BUF_STATE_ERROR);
 			else
 				vb2_buffer_done(src_buf->b, VB2_BUF_STATE_DONE);
@@ -390,12 +406,12 @@ leave_handle_frame:
 	if ((ctx->src_queue_cnt == 0 && ctx->state != MFCINST_FINISHING)
 				    || ctx->dst_queue_cnt < ctx->dpb_count)
 		clear_work_bit(ctx);
-	s5p_mfc_clear_int_flags(dev);
+	s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 	wake_up_ctx(ctx, reason, err);
 	if (test_and_clear_bit(0, &dev->hw_lock) == 0)
 		BUG();
 	s5p_mfc_clock_off();
-	s5p_mfc_try_run(dev);
+	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 }
 
 /* Error handling for interrupt */
@@ -412,7 +428,7 @@ static void s5p_mfc_handle_error(struct s5p_mfc_ctx *ctx,
 
 	dev = ctx->dev;
 	mfc_err("Interrupt Error: %08x\n", err);
-	s5p_mfc_clear_int_flags(dev);
+	s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 	wake_up_dev(dev, reason, err);
 
 	/* Error recovery is dependent on the state of context */
@@ -441,9 +457,11 @@ static void s5p_mfc_handle_error(struct s5p_mfc_ctx *ctx,
 		ctx->state = MFCINST_ERROR;
 		/* Mark all dst buffers as having an error */
 		spin_lock_irqsave(&dev->irqlock, flags);
-		s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
+		s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->dst_queue,
+				&ctx->vq_dst);
 		/* Mark all src buffers as having an error */
-		s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
+		s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->src_queue,
+				&ctx->vq_src);
 		spin_unlock_irqrestore(&dev->irqlock, flags);
 		if (test_and_clear_bit(0, &dev->hw_lock) == 0)
 			BUG();
@@ -461,7 +479,6 @@ static void s5p_mfc_handle_seq_done(struct s5p_mfc_ctx *ctx,
 				 unsigned int reason, unsigned int err)
 {
 	struct s5p_mfc_dev *dev;
-	unsigned int guard_width, guard_height;
 
 	if (ctx == NULL)
 		return;
@@ -470,55 +487,44 @@ static void s5p_mfc_handle_seq_done(struct s5p_mfc_ctx *ctx,
 		if (ctx->c_ops->post_seq_start(ctx))
 			mfc_err("post_seq_start() failed\n");
 	} else {
-		ctx->img_width = s5p_mfc_get_img_width();
-		ctx->img_height = s5p_mfc_get_img_height();
-
-		ctx->buf_width = ALIGN(ctx->img_width,
-						S5P_FIMV_NV12MT_HALIGN);
-		ctx->buf_height = ALIGN(ctx->img_height,
-						S5P_FIMV_NV12MT_VALIGN);
-		mfc_debug(2, "SEQ Done: Movie dimensions %dx%d, "
-			"buffer dimensions: %dx%d\n", ctx->img_width,
-				ctx->img_height, ctx->buf_width,
-						ctx->buf_height);
-		if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC) {
-			ctx->luma_size = ALIGN(ctx->buf_width *
-				ctx->buf_height, S5P_FIMV_DEC_BUF_ALIGN);
-			ctx->chroma_size = ALIGN(ctx->buf_width *
-					 ALIGN((ctx->img_height >> 1),
-					       S5P_FIMV_NV12MT_VALIGN),
-					       S5P_FIMV_DEC_BUF_ALIGN);
-			ctx->mv_size = ALIGN(ctx->buf_width *
-					ALIGN((ctx->buf_height >> 2),
-					S5P_FIMV_NV12MT_VALIGN),
-					S5P_FIMV_DEC_BUF_ALIGN);
-		} else {
-			guard_width = ALIGN(ctx->img_width + 24,
-					S5P_FIMV_NV12MT_HALIGN);
-			guard_height = ALIGN(ctx->img_height + 16,
-						S5P_FIMV_NV12MT_VALIGN);
-			ctx->luma_size = ALIGN(guard_width *
-				guard_height, S5P_FIMV_DEC_BUF_ALIGN);
-			guard_width = ALIGN(ctx->img_width + 16,
-						S5P_FIMV_NV12MT_HALIGN);
-			guard_height = ALIGN((ctx->img_height >> 1) + 4,
-						S5P_FIMV_NV12MT_VALIGN);
-			ctx->chroma_size = ALIGN(guard_width *
-				guard_height, S5P_FIMV_DEC_BUF_ALIGN);
-			ctx->mv_size = 0;
-		}
-		ctx->dpb_count = s5p_mfc_get_dpb_count();
+		ctx->img_width = s5p_mfc_hw_call(dev->mfc_ops, get_img_width,
+				dev);
+		ctx->img_height = s5p_mfc_hw_call(dev->mfc_ops, get_img_height,
+				dev);
+
+		s5p_mfc_hw_call(dev->mfc_ops, dec_calc_dpb_size, ctx);
+
+		ctx->dpb_count = s5p_mfc_hw_call(dev->mfc_ops, get_dpb_count,
+				dev);
+		ctx->mv_count = s5p_mfc_hw_call(dev->mfc_ops, get_mv_count,
+				dev);
 		if (ctx->img_width == 0 || ctx->img_height == 0)
 			ctx->state = MFCINST_ERROR;
 		else
 			ctx->state = MFCINST_HEAD_PARSED;
+
+		if ((ctx->codec_mode == S5P_MFC_CODEC_H264_DEC ||
+			ctx->codec_mode == S5P_MFC_CODEC_H264_MVC_DEC) &&
+				!list_empty(&ctx->src_queue)) {
+			struct s5p_mfc_buf *src_buf;
+			src_buf = list_entry(ctx->src_queue.next,
+					struct s5p_mfc_buf, list);
+			if (s5p_mfc_hw_call(dev->mfc_ops, get_consumed_stream,
+						dev) <
+					src_buf->b->v4l2_planes[0].bytesused)
+				ctx->head_processed = 0;
+			else
+				ctx->head_processed = 1;
+		} else {
+			ctx->head_processed = 1;
+		}
 	}
-	s5p_mfc_clear_int_flags(dev);
+	s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 	clear_work_bit(ctx);
 	if (test_and_clear_bit(0, &dev->hw_lock) == 0)
 		BUG();
 	s5p_mfc_clock_off();
-	s5p_mfc_try_run(dev);
+	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 	wake_up_ctx(ctx, reason, err);
 }
 
@@ -533,14 +539,14 @@ static void s5p_mfc_handle_init_buffers(struct s5p_mfc_ctx *ctx,
 	if (ctx == NULL)
 		return;
 	dev = ctx->dev;
-	s5p_mfc_clear_int_flags(dev);
+	s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 	ctx->int_type = reason;
 	ctx->int_err = err;
 	ctx->int_cond = 1;
 	clear_work_bit(ctx);
 	if (err == 0) {
 		ctx->state = MFCINST_RUNNING;
-		if (!ctx->dpb_flush_flag) {
+		if (!ctx->dpb_flush_flag && ctx->head_processed) {
 			spin_lock_irqsave(&dev->irqlock, flags);
 			if (!list_empty(&ctx->src_queue)) {
 				src_buf = list_entry(ctx->src_queue.next,
@@ -560,7 +566,7 @@ static void s5p_mfc_handle_init_buffers(struct s5p_mfc_ctx *ctx,
 		s5p_mfc_clock_off();
 
 		wake_up(&ctx->queue);
-		s5p_mfc_try_run(dev);
+		s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 	} else {
 		if (test_and_clear_bit(0, &dev->hw_lock) == 0)
 			BUG();
@@ -602,7 +608,7 @@ static void s5p_mfc_handle_stream_complete(struct s5p_mfc_ctx *ctx,
 
 	s5p_mfc_clock_off();
 	wake_up(&ctx->queue);
-	s5p_mfc_try_run(dev);
+	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 }
 
 /* Interrupt processing */
@@ -618,81 +624,83 @@ static irqreturn_t s5p_mfc_irq(int irq, void *priv)
 	atomic_set(&dev->watchdog_cnt, 0);
 	ctx = dev->ctx[dev->curr_ctx];
 	/* Get the reason of interrupt and the error code */
-	reason = s5p_mfc_get_int_reason();
-	err = s5p_mfc_get_int_err();
+	reason = s5p_mfc_hw_call(dev->mfc_ops, get_int_reason, dev);
+	err = s5p_mfc_hw_call(dev->mfc_ops, get_int_err, dev);
 	mfc_debug(1, "Int reason: %d (err: %08x)\n", reason, err);
 	switch (reason) {
-	case S5P_FIMV_R2H_CMD_ERR_RET:
+	case S5P_MFC_R2H_CMD_ERR_RET:
 		/* An error has occured */
 		if (ctx->state == MFCINST_RUNNING &&
-			s5p_mfc_err_dec(err) >= S5P_FIMV_ERR_WARNINGS_START)
+			s5p_mfc_hw_call(dev->mfc_ops, err_dec, err) >=
+				dev->warn_start)
 			s5p_mfc_handle_frame(ctx, reason, err);
 		else
 			s5p_mfc_handle_error(ctx, reason, err);
 		clear_bit(0, &dev->enter_suspend);
 		break;
 
-	case S5P_FIMV_R2H_CMD_SLICE_DONE_RET:
-	case S5P_FIMV_R2H_CMD_FRAME_DONE_RET:
+	case S5P_MFC_R2H_CMD_SLICE_DONE_RET:
+	case S5P_MFC_R2H_CMD_FIELD_DONE_RET:
+	case S5P_MFC_R2H_CMD_FRAME_DONE_RET:
 		if (ctx->c_ops->post_frame_start) {
 			if (ctx->c_ops->post_frame_start(ctx))
 				mfc_err("post_frame_start() failed\n");
-			s5p_mfc_clear_int_flags(dev);
+			s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 			wake_up_ctx(ctx, reason, err);
 			if (test_and_clear_bit(0, &dev->hw_lock) == 0)
 				BUG();
 			s5p_mfc_clock_off();
-			s5p_mfc_try_run(dev);
+			s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 		} else {
 			s5p_mfc_handle_frame(ctx, reason, err);
 		}
 		break;
 
-	case S5P_FIMV_R2H_CMD_SEQ_DONE_RET:
+	case S5P_MFC_R2H_CMD_SEQ_DONE_RET:
 		s5p_mfc_handle_seq_done(ctx, reason, err);
 		break;
 
-	case S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET:
-		ctx->inst_no = s5p_mfc_get_inst_no();
+	case S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET:
+		ctx->inst_no = s5p_mfc_hw_call(dev->mfc_ops, get_inst_no, dev);
 		ctx->state = MFCINST_GOT_INST;
 		clear_work_bit(ctx);
 		wake_up(&ctx->queue);
 		goto irq_cleanup_hw;
 
-	case S5P_FIMV_R2H_CMD_CLOSE_INSTANCE_RET:
+	case S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET:
 		clear_work_bit(ctx);
 		ctx->state = MFCINST_FREE;
 		wake_up(&ctx->queue);
 		goto irq_cleanup_hw;
 
-	case S5P_FIMV_R2H_CMD_SYS_INIT_RET:
-	case S5P_FIMV_R2H_CMD_FW_STATUS_RET:
-	case S5P_FIMV_R2H_CMD_SLEEP_RET:
-	case S5P_FIMV_R2H_CMD_WAKEUP_RET:
+	case S5P_MFC_R2H_CMD_SYS_INIT_RET:
+	case S5P_MFC_R2H_CMD_FW_STATUS_RET:
+	case S5P_MFC_R2H_CMD_SLEEP_RET:
+	case S5P_MFC_R2H_CMD_WAKEUP_RET:
 		if (ctx)
 			clear_work_bit(ctx);
-		s5p_mfc_clear_int_flags(dev);
+		s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 		wake_up_dev(dev, reason, err);
 		clear_bit(0, &dev->hw_lock);
 		clear_bit(0, &dev->enter_suspend);
 		break;
 
-	case S5P_FIMV_R2H_CMD_INIT_BUFFERS_RET:
+	case S5P_MFC_R2H_CMD_INIT_BUFFERS_RET:
 		s5p_mfc_handle_init_buffers(ctx, reason, err);
 		break;
 
-	case S5P_FIMV_R2H_CMD_ENC_COMPLETE_RET:
+	case S5P_MFC_R2H_CMD_COMPLETE_SEQ_RET:
 		s5p_mfc_handle_stream_complete(ctx, reason, err);
 		break;
 
 	default:
 		mfc_debug(2, "Unknown int reason\n");
-		s5p_mfc_clear_int_flags(dev);
+		s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 	}
 	mfc_debug_leave();
 	return IRQ_HANDLED;
 irq_cleanup_hw:
-	s5p_mfc_clear_int_flags(dev);
+	s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 	ctx->int_type = reason;
 	ctx->int_err = err;
 	ctx->int_cond = 1;
@@ -701,7 +709,7 @@ irq_cleanup_hw:
 
 	s5p_mfc_clock_off();
 
-	s5p_mfc_try_run(dev);
+	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 	mfc_debug(2, "Exit via irq_cleanup_hw\n");
 	return IRQ_HANDLED;
 }
@@ -749,6 +757,7 @@ static int s5p_mfc_open(struct file *file)
 	if (s5p_mfc_get_node_type(file) == MFCNODE_DECODER) {
 		ctx->type = MFCINST_DECODER;
 		ctx->c_ops = get_dec_codec_ops();
+		s5p_mfc_dec_init(ctx);
 		/* Setup ctrl handler */
 		ret = s5p_mfc_dec_ctrls_setup(ctx);
 		if (ret) {
@@ -761,6 +770,7 @@ static int s5p_mfc_open(struct file *file)
 		/* only for encoder */
 		INIT_LIST_HEAD(&ctx->ref_queue);
 		ctx->ref_queue_cnt = 0;
+		s5p_mfc_enc_init(ctx);
 		/* Setup ctrl handler */
 		ret = s5p_mfc_enc_ctrls_setup(ctx);
 		if (ret) {
@@ -886,19 +896,20 @@ static int s5p_mfc_release(struct file *file)
 		ctx->state = MFCINST_RETURN_INST;
 		set_work_bit_irqsave(ctx);
 		s5p_mfc_clean_ctx_int_flags(ctx);
-		s5p_mfc_try_run(dev);
+		s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 		/* Wait until instance is returned or timeout occured */
 		if (s5p_mfc_wait_for_done_ctx
-		    (ctx, S5P_FIMV_R2H_CMD_CLOSE_INSTANCE_RET, 0)) {
+		    (ctx, S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET, 0)) {
 			s5p_mfc_clock_off();
 			mfc_err("Err returning instance\n");
 		}
 		mfc_debug(2, "After free instance\n");
 		/* Free resources */
-		s5p_mfc_release_codec_buffers(ctx);
-		s5p_mfc_release_instance_buffer(ctx);
+		s5p_mfc_hw_call(dev->mfc_ops, release_codec_buffers, ctx);
+		s5p_mfc_hw_call(dev->mfc_ops, release_instance_buffer, ctx);
 		if (ctx->type == MFCINST_DECODER)
-			s5p_mfc_release_dec_desc_buffer(ctx);
+			s5p_mfc_hw_call(dev->mfc_ops, release_dec_desc_buffer,
+					ctx);
 
 		ctx->inst_no = MFC_NO_INSTANCE_SET;
 	}
@@ -910,6 +921,7 @@ static int s5p_mfc_release(struct file *file)
 		mfc_debug(2, "Last instance - release firmware\n");
 		/* reset <-> F/W release */
 		s5p_mfc_reset(dev);
+		s5p_mfc_deinit_hw(dev);
 		s5p_mfc_release_firmware(dev);
 		del_timer_sync(&dev->watchdog_timer);
 		if (s5p_mfc_power_off() < 0)
@@ -1041,6 +1053,9 @@ static int s5p_mfc_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
+	dev->variant = (struct s5p_mfc_variant *)
+		platform_get_device_id(pdev)->driver_data;
+
 	ret = s5p_mfc_init_pm(dev);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "failed to get mfc clock source\n");
@@ -1076,6 +1091,7 @@ static int s5p_mfc_probe(struct platform_device *pdev)
 		ret = -ENODEV;
 		goto err_res;
 	}
+
 	dev->mem_dev_r = device_find_child(&dev->plat_dev->dev, "s5p-mfc-r",
 					   match_child);
 	if (!dev->mem_dev_r) {
@@ -1139,6 +1155,7 @@ static int s5p_mfc_probe(struct platform_device *pdev)
 	vfd->release	= video_device_release,
 	vfd->lock	= &dev->mfc_mutex;
 	vfd->v4l2_dev	= &dev->v4l2_dev;
+	vfd->vfl_dir	= VFL_DIR_M2M;
 	snprintf(vfd->name, sizeof(vfd->name), "%s", S5P_MFC_ENC_NAME);
 	dev->vfd_enc	= vfd;
 	ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
@@ -1160,6 +1177,10 @@ static int s5p_mfc_probe(struct platform_device *pdev)
 	dev->watchdog_timer.data = (unsigned long)dev;
 	dev->watchdog_timer.function = s5p_mfc_watchdog;
 
+	/* Initialize HW ops and commands based on MFC version */
+	s5p_mfc_init_hw_ops(dev);
+	s5p_mfc_init_hw_cmds(dev);
+
 	pr_debug("%s--\n", __func__);
 	return 0;
 
@@ -1280,9 +1301,78 @@ static const struct dev_pm_ops s5p_mfc_pm_ops = {
 			   NULL)
 };
 
+struct s5p_mfc_buf_size_v5 mfc_buf_size_v5 = {
+	.h264_ctx	= MFC_H264_CTX_BUF_SIZE,
+	.non_h264_ctx	= MFC_CTX_BUF_SIZE,
+	.dsc		= DESC_BUF_SIZE,
+	.shm		= SHARED_BUF_SIZE,
+};
+
+struct s5p_mfc_buf_size buf_size_v5 = {
+	.fw	= MAX_FW_SIZE,
+	.cpb	= MAX_CPB_SIZE,
+	.priv	= &mfc_buf_size_v5,
+};
+
+struct s5p_mfc_buf_align mfc_buf_align_v5 = {
+	.base = MFC_BASE_ALIGN_ORDER,
+};
+
+static struct s5p_mfc_variant mfc_drvdata_v5 = {
+	.version	= MFC_VERSION,
+	.port_num	= MFC_NUM_PORTS,
+	.buf_size	= &buf_size_v5,
+	.buf_align	= &mfc_buf_align_v5,
+	.mclk_name	= "sclk_mfc",
+	.fw_name	= "s5p-mfc.fw",
+};
+
+struct s5p_mfc_buf_size_v6 mfc_buf_size_v6 = {
+	.dev_ctx	= MFC_CTX_BUF_SIZE_V6,
+	.h264_dec_ctx	= MFC_H264_DEC_CTX_BUF_SIZE_V6,
+	.other_dec_ctx	= MFC_OTHER_DEC_CTX_BUF_SIZE_V6,
+	.h264_enc_ctx	= MFC_H264_ENC_CTX_BUF_SIZE_V6,
+	.other_enc_ctx	= MFC_OTHER_ENC_CTX_BUF_SIZE_V6,
+};
+
+struct s5p_mfc_buf_size buf_size_v6 = {
+	.fw	= MAX_FW_SIZE_V6,
+	.cpb	= MAX_CPB_SIZE_V6,
+	.priv	= &mfc_buf_size_v6,
+};
+
+struct s5p_mfc_buf_align mfc_buf_align_v6 = {
+	.base = 0,
+};
+
+static struct s5p_mfc_variant mfc_drvdata_v6 = {
+	.version	= MFC_VERSION_V6,
+	.port_num	= MFC_NUM_PORTS_V6,
+	.buf_size	= &buf_size_v6,
+	.buf_align	= &mfc_buf_align_v6,
+	.mclk_name      = "aclk_333",
+	.fw_name        = "s5p-mfc-v6.fw",
+};
+
+static struct platform_device_id mfc_driver_ids[] = {
+	{
+		.name = "s5p-mfc",
+		.driver_data = (unsigned long)&mfc_drvdata_v5,
+	}, {
+		.name = "s5p-mfc-v5",
+		.driver_data = (unsigned long)&mfc_drvdata_v5,
+	}, {
+		.name = "s5p-mfc-v6",
+		.driver_data = (unsigned long)&mfc_drvdata_v6,
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(platform, mfc_driver_ids);
+
 static struct platform_driver s5p_mfc_driver = {
-	.probe	= s5p_mfc_probe,
-	.remove	= __devexit_p(s5p_mfc_remove),
+	.probe		= s5p_mfc_probe,
+	.remove		= __devexit_p(s5p_mfc_remove),
+	.id_table	= mfc_driver_ids,
 	.driver	= {
 		.name	= S5P_MFC_NAME,
 		.owner	= THIS_MODULE,
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.c b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.c
index 91a415573bd..f0a41c95df8 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.c
@@ -1,7 +1,7 @@
 /*
  * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.c
  *
- * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ * Copyright (C) 2012 Samsung Electronics Co., Ltd.
  *		http://www.samsung.com/
  *
  * This program is free software; you can redistribute it and/or modify
@@ -10,111 +10,20 @@
  * (at your option) any later version.
  */
 
-#include "regs-mfc.h"
 #include "s5p_mfc_cmd.h"
 #include "s5p_mfc_common.h"
 #include "s5p_mfc_debug.h"
+#include "s5p_mfc_cmd_v5.h"
+#include "s5p_mfc_cmd_v6.h"
 
-/* This function is used to send a command to the MFC */
-static int s5p_mfc_cmd_host2risc(struct s5p_mfc_dev *dev, int cmd,
-						struct s5p_mfc_cmd_args *args)
-{
-	int cur_cmd;
-	unsigned long timeout;
-
-	timeout = jiffies + msecs_to_jiffies(MFC_BW_TIMEOUT);
-	/* wait until host to risc command register becomes 'H2R_CMD_EMPTY' */
-	do {
-		if (time_after(jiffies, timeout)) {
-			mfc_err("Timeout while waiting for hardware\n");
-			return -EIO;
-		}
-		cur_cmd = mfc_read(dev, S5P_FIMV_HOST2RISC_CMD);
-	} while (cur_cmd != S5P_FIMV_H2R_CMD_EMPTY);
-	mfc_write(dev, args->arg[0], S5P_FIMV_HOST2RISC_ARG1);
-	mfc_write(dev, args->arg[1], S5P_FIMV_HOST2RISC_ARG2);
-	mfc_write(dev, args->arg[2], S5P_FIMV_HOST2RISC_ARG3);
-	mfc_write(dev, args->arg[3], S5P_FIMV_HOST2RISC_ARG4);
-	/* Issue the command */
-	mfc_write(dev, cmd, S5P_FIMV_HOST2RISC_CMD);
-	return 0;
-}
-
-/* Initialize the MFC */
-int s5p_mfc_sys_init_cmd(struct s5p_mfc_dev *dev)
-{
-	struct s5p_mfc_cmd_args h2r_args;
-
-	memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
-	h2r_args.arg[0] = dev->fw_size;
-	return s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_SYS_INIT, &h2r_args);
-}
-
-/* Suspend the MFC hardware */
-int s5p_mfc_sleep_cmd(struct s5p_mfc_dev *dev)
-{
-	struct s5p_mfc_cmd_args h2r_args;
-
-	memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
-	return s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_SLEEP, &h2r_args);
-}
+static struct s5p_mfc_hw_cmds *s5p_mfc_cmds;
 
-/* Wake up the MFC hardware */
-int s5p_mfc_wakeup_cmd(struct s5p_mfc_dev *dev)
+void s5p_mfc_init_hw_cmds(struct s5p_mfc_dev *dev)
 {
-	struct s5p_mfc_cmd_args h2r_args;
+	if (IS_MFCV6(dev))
+		s5p_mfc_cmds = s5p_mfc_init_hw_cmds_v6();
+	else
+		s5p_mfc_cmds = s5p_mfc_init_hw_cmds_v5();
 
-	memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
-	return s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_WAKEUP, &h2r_args);
+	dev->mfc_cmds = s5p_mfc_cmds;
 }
-
-
-int s5p_mfc_open_inst_cmd(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-	struct s5p_mfc_cmd_args h2r_args;
-	int ret;
-
-	/* Preparing decoding - getting instance number */
-	mfc_debug(2, "Getting instance number (codec: %d)\n", ctx->codec_mode);
-	dev->curr_ctx = ctx->num;
-	memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
-	h2r_args.arg[0] = ctx->codec_mode;
-	h2r_args.arg[1] = 0; /* no crc & no pixelcache */
-	h2r_args.arg[2] = ctx->ctx_ofs;
-	h2r_args.arg[3] = ctx->ctx_size;
-	ret = s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_OPEN_INSTANCE,
-								&h2r_args);
-	if (ret) {
-		mfc_err("Failed to create a new instance\n");
-		ctx->state = MFCINST_ERROR;
-	}
-	return ret;
-}
-
-int s5p_mfc_close_inst_cmd(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-	struct s5p_mfc_cmd_args h2r_args;
-	int ret;
-
-	if (ctx->state == MFCINST_FREE) {
-		mfc_err("Instance already returned\n");
-		ctx->state = MFCINST_ERROR;
-		return -EINVAL;
-	}
-	/* Closing decoding instance  */
-	mfc_debug(2, "Returning instance number %d\n", ctx->inst_no);
-	dev->curr_ctx = ctx->num;
-	memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
-	h2r_args.arg[0] = ctx->inst_no;
-	ret = s5p_mfc_cmd_host2risc(dev, S5P_FIMV_H2R_CMD_CLOSE_INSTANCE,
-								&h2r_args);
-	if (ret) {
-		mfc_err("Failed to return an instance\n");
-		ctx->state = MFCINST_ERROR;
-		return -EINVAL;
-	}
-	return 0;
-}
-
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.h b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.h
index 8b090d3723e..282e6c78070 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.h
@@ -1,7 +1,7 @@
 /*
  * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.h
  *
- * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ * Copyright (C) 2012 Samsung Electronics Co., Ltd.
  *		http://www.samsung.com/
  *
  * This program is free software; you can redistribute it and/or modify
@@ -21,10 +21,15 @@ struct s5p_mfc_cmd_args {
 	unsigned int	arg[MAX_H2R_ARG];
 };
 
-int s5p_mfc_sys_init_cmd(struct s5p_mfc_dev *dev);
-int s5p_mfc_sleep_cmd(struct s5p_mfc_dev *dev);
-int s5p_mfc_wakeup_cmd(struct s5p_mfc_dev *dev);
-int s5p_mfc_open_inst_cmd(struct s5p_mfc_ctx *ctx);
-int s5p_mfc_close_inst_cmd(struct s5p_mfc_ctx *ctx);
+struct s5p_mfc_hw_cmds {
+	int (*cmd_host2risc)(struct s5p_mfc_dev *dev, int cmd,
+				struct s5p_mfc_cmd_args *args);
+	int (*sys_init_cmd)(struct s5p_mfc_dev *dev);
+	int (*sleep_cmd)(struct s5p_mfc_dev *dev);
+	int (*wakeup_cmd)(struct s5p_mfc_dev *dev);
+	int (*open_inst_cmd)(struct s5p_mfc_ctx *ctx);
+	int (*close_inst_cmd)(struct s5p_mfc_ctx *ctx);
+};
 
+void s5p_mfc_init_hw_cmds(struct s5p_mfc_dev *dev);
 #endif /* S5P_MFC_CMD_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
new file mode 100644
index 00000000000..138778083c6
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
@@ -0,0 +1,166 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.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.
+ */
+
+#include "regs-mfc.h"
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_debug.h"
+
+/* This function is used to send a command to the MFC */
+int s5p_mfc_cmd_host2risc_v5(struct s5p_mfc_dev *dev, int cmd,
+				struct s5p_mfc_cmd_args *args)
+{
+	int cur_cmd;
+	unsigned long timeout;
+
+	timeout = jiffies + msecs_to_jiffies(MFC_BW_TIMEOUT);
+	/* wait until host to risc command register becomes 'H2R_CMD_EMPTY' */
+	do {
+		if (time_after(jiffies, timeout)) {
+			mfc_err("Timeout while waiting for hardware\n");
+			return -EIO;
+		}
+		cur_cmd = mfc_read(dev, S5P_FIMV_HOST2RISC_CMD);
+	} while (cur_cmd != S5P_FIMV_H2R_CMD_EMPTY);
+	mfc_write(dev, args->arg[0], S5P_FIMV_HOST2RISC_ARG1);
+	mfc_write(dev, args->arg[1], S5P_FIMV_HOST2RISC_ARG2);
+	mfc_write(dev, args->arg[2], S5P_FIMV_HOST2RISC_ARG3);
+	mfc_write(dev, args->arg[3], S5P_FIMV_HOST2RISC_ARG4);
+	/* Issue the command */
+	mfc_write(dev, cmd, S5P_FIMV_HOST2RISC_CMD);
+	return 0;
+}
+
+/* Initialize the MFC */
+int s5p_mfc_sys_init_cmd_v5(struct s5p_mfc_dev *dev)
+{
+	struct s5p_mfc_cmd_args h2r_args;
+
+	memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+	h2r_args.arg[0] = dev->fw_size;
+	return s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_SYS_INIT,
+			&h2r_args);
+}
+
+/* Suspend the MFC hardware */
+int s5p_mfc_sleep_cmd_v5(struct s5p_mfc_dev *dev)
+{
+	struct s5p_mfc_cmd_args h2r_args;
+
+	memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+	return s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_SLEEP, &h2r_args);
+}
+
+/* Wake up the MFC hardware */
+int s5p_mfc_wakeup_cmd_v5(struct s5p_mfc_dev *dev)
+{
+	struct s5p_mfc_cmd_args h2r_args;
+
+	memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+	return s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_WAKEUP,
+			&h2r_args);
+}
+
+
+int s5p_mfc_open_inst_cmd_v5(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_cmd_args h2r_args;
+	int ret;
+
+	/* Preparing decoding - getting instance number */
+	mfc_debug(2, "Getting instance number (codec: %d)\n", ctx->codec_mode);
+	dev->curr_ctx = ctx->num;
+	memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+	switch (ctx->codec_mode) {
+	case S5P_MFC_CODEC_H264_DEC:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_H264_DEC;
+		break;
+	case S5P_MFC_CODEC_VC1_DEC:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_VC1_DEC;
+		break;
+	case S5P_MFC_CODEC_MPEG4_DEC:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_MPEG4_DEC;
+		break;
+	case S5P_MFC_CODEC_MPEG2_DEC:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_MPEG2_DEC;
+		break;
+	case S5P_MFC_CODEC_H263_DEC:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_H263_DEC;
+		break;
+	case S5P_MFC_CODEC_VC1RCV_DEC:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_VC1RCV_DEC;
+		break;
+	case S5P_MFC_CODEC_H264_ENC:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_H264_ENC;
+		break;
+	case S5P_MFC_CODEC_MPEG4_ENC:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_MPEG4_ENC;
+		break;
+	case S5P_MFC_CODEC_H263_ENC:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_H263_ENC;
+		break;
+	default:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_NONE;
+	};
+	h2r_args.arg[1] = 0; /* no crc & no pixelcache */
+	h2r_args.arg[2] = ctx->ctx.ofs;
+	h2r_args.arg[3] = ctx->ctx.size;
+	ret = s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_OPEN_INSTANCE,
+								&h2r_args);
+	if (ret) {
+		mfc_err("Failed to create a new instance\n");
+		ctx->state = MFCINST_ERROR;
+	}
+	return ret;
+}
+
+int s5p_mfc_close_inst_cmd_v5(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_cmd_args h2r_args;
+	int ret;
+
+	if (ctx->state == MFCINST_FREE) {
+		mfc_err("Instance already returned\n");
+		ctx->state = MFCINST_ERROR;
+		return -EINVAL;
+	}
+	/* Closing decoding instance  */
+	mfc_debug(2, "Returning instance number %d\n", ctx->inst_no);
+	dev->curr_ctx = ctx->num;
+	memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+	h2r_args.arg[0] = ctx->inst_no;
+	ret = s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_CLOSE_INSTANCE,
+								&h2r_args);
+	if (ret) {
+		mfc_err("Failed to return an instance\n");
+		ctx->state = MFCINST_ERROR;
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/* Initialize cmd function pointers for MFC v5 */
+static struct s5p_mfc_hw_cmds s5p_mfc_cmds_v5 = {
+	.cmd_host2risc = s5p_mfc_cmd_host2risc_v5,
+	.sys_init_cmd = s5p_mfc_sys_init_cmd_v5,
+	.sleep_cmd = s5p_mfc_sleep_cmd_v5,
+	.wakeup_cmd = s5p_mfc_wakeup_cmd_v5,
+	.open_inst_cmd = s5p_mfc_open_inst_cmd_v5,
+	.close_inst_cmd = s5p_mfc_close_inst_cmd_v5,
+};
+
+struct s5p_mfc_hw_cmds *s5p_mfc_init_hw_cmds_v5(void)
+{
+	return &s5p_mfc_cmds_v5;
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.h b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.h
new file mode 100644
index 00000000000..6928a5514c1
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.h
@@ -0,0 +1,20 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.h
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.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.
+ */
+
+#ifndef S5P_MFC_CMD_V5_H_
+#define S5P_MFC_CMD_V5_H_
+
+#include "s5p_mfc_common.h"
+
+struct s5p_mfc_hw_cmds *s5p_mfc_init_hw_cmds_v5(void);
+
+#endif /* S5P_MFC_CMD_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
new file mode 100644
index 00000000000..754bfbcb1c4
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
@@ -0,0 +1,156 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.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.
+ */
+
+#include "s5p_mfc_common.h"
+
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_opr.h"
+
+int s5p_mfc_cmd_host2risc_v6(struct s5p_mfc_dev *dev, int cmd,
+				struct s5p_mfc_cmd_args *args)
+{
+	mfc_debug(2, "Issue the command: %d\n", cmd);
+
+	/* Reset RISC2HOST command */
+	mfc_write(dev, 0x0, S5P_FIMV_RISC2HOST_CMD_V6);
+
+	/* Issue the command */
+	mfc_write(dev, cmd, S5P_FIMV_HOST2RISC_CMD_V6);
+	mfc_write(dev, 0x1, S5P_FIMV_HOST2RISC_INT_V6);
+
+	return 0;
+}
+
+int s5p_mfc_sys_init_cmd_v6(struct s5p_mfc_dev *dev)
+{
+	struct s5p_mfc_cmd_args h2r_args;
+	struct s5p_mfc_buf_size_v6 *buf_size = dev->variant->buf_size->priv;
+
+	s5p_mfc_hw_call(dev->mfc_ops, alloc_dev_context_buffer, dev);
+	mfc_write(dev, dev->ctx_buf.dma, S5P_FIMV_CONTEXT_MEM_ADDR_V6);
+	mfc_write(dev, buf_size->dev_ctx, S5P_FIMV_CONTEXT_MEM_SIZE_V6);
+	return s5p_mfc_cmd_host2risc_v6(dev, S5P_FIMV_H2R_CMD_SYS_INIT_V6,
+					&h2r_args);
+}
+
+int s5p_mfc_sleep_cmd_v6(struct s5p_mfc_dev *dev)
+{
+	struct s5p_mfc_cmd_args h2r_args;
+
+	memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+	return s5p_mfc_cmd_host2risc_v6(dev, S5P_FIMV_H2R_CMD_SLEEP_V6,
+			&h2r_args);
+}
+
+int s5p_mfc_wakeup_cmd_v6(struct s5p_mfc_dev *dev)
+{
+	struct s5p_mfc_cmd_args h2r_args;
+
+	memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+	return s5p_mfc_cmd_host2risc_v6(dev, S5P_FIMV_H2R_CMD_WAKEUP_V6,
+					&h2r_args);
+}
+
+/* Open a new instance and get its number */
+int s5p_mfc_open_inst_cmd_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_cmd_args h2r_args;
+	int codec_type;
+
+	mfc_debug(2, "Requested codec mode: %d\n", ctx->codec_mode);
+	dev->curr_ctx = ctx->num;
+	switch (ctx->codec_mode) {
+	case S5P_MFC_CODEC_H264_DEC:
+		codec_type = S5P_FIMV_CODEC_H264_DEC_V6;
+		break;
+	case S5P_MFC_CODEC_H264_MVC_DEC:
+		codec_type = S5P_FIMV_CODEC_H264_MVC_DEC_V6;
+		break;
+	case S5P_MFC_CODEC_VC1_DEC:
+		codec_type = S5P_FIMV_CODEC_VC1_DEC_V6;
+		break;
+	case S5P_MFC_CODEC_MPEG4_DEC:
+		codec_type = S5P_FIMV_CODEC_MPEG4_DEC_V6;
+		break;
+	case S5P_MFC_CODEC_MPEG2_DEC:
+		codec_type = S5P_FIMV_CODEC_MPEG2_DEC_V6;
+		break;
+	case S5P_MFC_CODEC_H263_DEC:
+		codec_type = S5P_FIMV_CODEC_H263_DEC_V6;
+		break;
+	case S5P_MFC_CODEC_VC1RCV_DEC:
+		codec_type = S5P_FIMV_CODEC_VC1RCV_DEC_V6;
+		break;
+	case S5P_MFC_CODEC_VP8_DEC:
+		codec_type = S5P_FIMV_CODEC_VP8_DEC_V6;
+		break;
+	case S5P_MFC_CODEC_H264_ENC:
+		codec_type = S5P_FIMV_CODEC_H264_ENC_V6;
+		break;
+	case S5P_MFC_CODEC_H264_MVC_ENC:
+		codec_type = S5P_FIMV_CODEC_H264_MVC_ENC_V6;
+		break;
+	case S5P_MFC_CODEC_MPEG4_ENC:
+		codec_type = S5P_FIMV_CODEC_MPEG4_ENC_V6;
+		break;
+	case S5P_MFC_CODEC_H263_ENC:
+		codec_type = S5P_FIMV_CODEC_H263_ENC_V6;
+		break;
+	default:
+		codec_type = S5P_FIMV_CODEC_NONE_V6;
+	};
+	mfc_write(dev, codec_type, S5P_FIMV_CODEC_TYPE_V6);
+	mfc_write(dev, ctx->ctx.dma, S5P_FIMV_CONTEXT_MEM_ADDR_V6);
+	mfc_write(dev, ctx->ctx.size, S5P_FIMV_CONTEXT_MEM_SIZE_V6);
+	mfc_write(dev, 0, S5P_FIMV_D_CRC_CTRL_V6); /* no crc */
+
+	return s5p_mfc_cmd_host2risc_v6(dev, S5P_FIMV_H2R_CMD_OPEN_INSTANCE_V6,
+					&h2r_args);
+}
+
+/* Close instance */
+int s5p_mfc_close_inst_cmd_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_cmd_args h2r_args;
+	int ret = 0;
+
+	dev->curr_ctx = ctx->num;
+	if (ctx->state != MFCINST_FREE) {
+		mfc_write(dev, ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+		ret = s5p_mfc_cmd_host2risc_v6(dev,
+					S5P_FIMV_H2R_CMD_CLOSE_INSTANCE_V6,
+					&h2r_args);
+	} else {
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+/* Initialize cmd function pointers for MFC v6 */
+static struct s5p_mfc_hw_cmds s5p_mfc_cmds_v6 = {
+	.cmd_host2risc = s5p_mfc_cmd_host2risc_v6,
+	.sys_init_cmd = s5p_mfc_sys_init_cmd_v6,
+	.sleep_cmd = s5p_mfc_sleep_cmd_v6,
+	.wakeup_cmd = s5p_mfc_wakeup_cmd_v6,
+	.open_inst_cmd = s5p_mfc_open_inst_cmd_v6,
+	.close_inst_cmd = s5p_mfc_close_inst_cmd_v6,
+};
+
+struct s5p_mfc_hw_cmds *s5p_mfc_init_hw_cmds_v6(void)
+{
+	return &s5p_mfc_cmds_v6;
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.h b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.h
new file mode 100644
index 00000000000..b7a8e57837b
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.h
@@ -0,0 +1,20 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.h
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.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.
+ */
+
+#ifndef S5P_MFC_CMD_V6_H_
+#define S5P_MFC_CMD_V6_H_
+
+#include "s5p_mfc_common.h"
+
+struct s5p_mfc_hw_cmds *s5p_mfc_init_hw_cmds_v6(void);
+
+#endif /* S5P_MFC_CMD_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
index 519b0d66d8d..f02e0497ca9 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
@@ -16,13 +16,14 @@
 #ifndef S5P_MFC_COMMON_H_
 #define S5P_MFC_COMMON_H_
 
-#include "regs-mfc.h"
 #include <linux/platform_device.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
 #include <media/videobuf2-core.h>
+#include "regs-mfc.h"
+#include "regs-mfc-v6.h"
 
 /* Definitions related to MFC memory */
 
@@ -30,17 +31,6 @@
 *  while mmaping */
 #define DST_QUEUE_OFF_BASE      (TASK_SIZE / 2)
 
-/* Offset used by the hardware to store addresses */
-#define MFC_OFFSET_SHIFT	11
-
-#define FIRMWARE_ALIGN		0x20000		/* 128KB */
-#define MFC_H264_CTX_BUF_SIZE	0x96000		/* 600KB per H264 instance */
-#define MFC_CTX_BUF_SIZE	0x2800		/* 10KB per instance */
-#define DESC_BUF_SIZE		0x20000		/* 128KB for DESC buffer */
-#define SHARED_BUF_SIZE		0x2000		/* 8KB for shared buffer */
-
-#define DEF_CPB_SIZE		0x40000		/* 512KB */
-
 #define MFC_BANK1_ALLOC_CTX	0
 #define MFC_BANK2_ALLOC_CTX	1
 
@@ -74,7 +64,40 @@ static inline dma_addr_t s5p_mfc_mem_cookie(void *a, void *b)
 #define MFC_ENC_CAP_PLANE_COUNT	1
 #define MFC_ENC_OUT_PLANE_COUNT	2
 #define STUFF_BYTE		4
-#define MFC_MAX_CTRLS		64
+#define MFC_MAX_CTRLS		70
+
+#define S5P_MFC_CODEC_NONE		-1
+#define S5P_MFC_CODEC_H264_DEC		0
+#define S5P_MFC_CODEC_H264_MVC_DEC	1
+#define S5P_MFC_CODEC_VC1_DEC		2
+#define S5P_MFC_CODEC_MPEG4_DEC		3
+#define S5P_MFC_CODEC_MPEG2_DEC		4
+#define S5P_MFC_CODEC_H263_DEC		5
+#define S5P_MFC_CODEC_VC1RCV_DEC	6
+#define S5P_MFC_CODEC_VP8_DEC		7
+
+#define S5P_MFC_CODEC_H264_ENC		20
+#define S5P_MFC_CODEC_H264_MVC_ENC	21
+#define S5P_MFC_CODEC_MPEG4_ENC		22
+#define S5P_MFC_CODEC_H263_ENC		23
+
+#define S5P_MFC_R2H_CMD_EMPTY			0
+#define S5P_MFC_R2H_CMD_SYS_INIT_RET		1
+#define S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET	2
+#define S5P_MFC_R2H_CMD_SEQ_DONE_RET		3
+#define S5P_MFC_R2H_CMD_INIT_BUFFERS_RET	4
+#define S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET	6
+#define S5P_MFC_R2H_CMD_SLEEP_RET		7
+#define S5P_MFC_R2H_CMD_WAKEUP_RET		8
+#define S5P_MFC_R2H_CMD_COMPLETE_SEQ_RET	9
+#define S5P_MFC_R2H_CMD_DPB_FLUSH_RET		10
+#define S5P_MFC_R2H_CMD_NAL_ABORT_RET		11
+#define S5P_MFC_R2H_CMD_FW_STATUS_RET		12
+#define S5P_MFC_R2H_CMD_FRAME_DONE_RET		13
+#define S5P_MFC_R2H_CMD_FIELD_DONE_RET		14
+#define S5P_MFC_R2H_CMD_SLICE_DONE_RET		15
+#define S5P_MFC_R2H_CMD_ENC_BUFFER_FUL_RET	16
+#define S5P_MFC_R2H_CMD_ERR_RET			32
 
 #define mfc_read(dev, offset)		readl(dev->regs_base + (offset))
 #define mfc_write(dev, data, offset)	writel((data), dev->regs_base + \
@@ -177,6 +200,58 @@ struct s5p_mfc_pm {
 	struct device	*device;
 };
 
+struct s5p_mfc_buf_size_v5 {
+	unsigned int h264_ctx;
+	unsigned int non_h264_ctx;
+	unsigned int dsc;
+	unsigned int shm;
+};
+
+struct s5p_mfc_buf_size_v6 {
+	unsigned int dev_ctx;
+	unsigned int h264_dec_ctx;
+	unsigned int other_dec_ctx;
+	unsigned int h264_enc_ctx;
+	unsigned int other_enc_ctx;
+};
+
+struct s5p_mfc_buf_size {
+	unsigned int fw;
+	unsigned int cpb;
+	void *priv;
+};
+
+struct s5p_mfc_buf_align {
+	unsigned int base;
+};
+
+struct s5p_mfc_variant {
+	unsigned int version;
+	unsigned int port_num;
+	struct s5p_mfc_buf_size *buf_size;
+	struct s5p_mfc_buf_align *buf_align;
+	char	*mclk_name;
+	char	*fw_name;
+};
+
+/**
+ * struct s5p_mfc_priv_buf - represents internal used buffer
+ * @alloc:		allocation-specific context for each buffer
+ *			(videobuf2 allocator)
+ * @ofs:		offset of each buffer, will be used for MFC
+ * @virt:		kernel virtual address, only valid when the
+ *			buffer accessed by driver
+ * @dma:		DMA address, only valid when kernel DMA API used
+ * @size:		size of the buffer
+ */
+struct s5p_mfc_priv_buf {
+	void		*alloc;
+	unsigned long	ofs;
+	void		*virt;
+	dma_addr_t	dma;
+	size_t		size;
+};
+
 /**
  * struct s5p_mfc_dev - The struct containing driver internal parameters.
  *
@@ -191,6 +266,7 @@ struct s5p_mfc_pm {
  * @dec_ctrl_handler:	control framework handler for decoding
  * @enc_ctrl_handler:	control framework handler for encoding
  * @pm:			power management control
+ * @variant:		MFC hardware variant information
  * @num_inst:		couter of active MFC instances
  * @irqlock:		lock for operations on videobuf2 queues
  * @condlock:		lock for changing/checking if a context is ready to be
@@ -212,6 +288,10 @@ struct s5p_mfc_pm {
  * @watchdog_work:	worker for the watchdog
  * @alloc_ctx:		videobuf2 allocator contexts for two memory banks
  * @enter_suspend:	flag set when entering suspend
+ * @ctx_buf:		common context memory (MFCv6)
+ * @warn_start:		hardware error code from which warnings start
+ * @mfc_ops:		ops structure holding HW operation function pointers
+ * @mfc_cmds:		cmd structure holding HW commands function pointers
  *
  */
 struct s5p_mfc_dev {
@@ -226,6 +306,7 @@ struct s5p_mfc_dev {
 	struct v4l2_ctrl_handler dec_ctrl_handler;
 	struct v4l2_ctrl_handler enc_ctrl_handler;
 	struct s5p_mfc_pm	pm;
+	struct s5p_mfc_variant	*variant;
 	int num_inst;
 	spinlock_t irqlock;	/* lock when operating on videobuf2 queues */
 	spinlock_t condlock;	/* lock when changing/checking if a context is
@@ -248,6 +329,11 @@ struct s5p_mfc_dev {
 	struct work_struct watchdog_work;
 	void *alloc_ctx[2];
 	unsigned long enter_suspend;
+
+	struct s5p_mfc_priv_buf ctx_buf;
+	int warn_start;
+	struct s5p_mfc_hw_ops *mfc_ops;
+	struct s5p_mfc_hw_cmds *mfc_cmds;
 };
 
 /**
@@ -262,7 +348,6 @@ struct s5p_mfc_h264_enc_params {
 	u8 max_ref_pic;
 	u8 num_ref_pic_4p;
 	int _8x8_transform;
-	int rc_mb;
 	int rc_mb_dark;
 	int rc_mb_smooth;
 	int rc_mb_static;
@@ -281,6 +366,23 @@ struct s5p_mfc_h264_enc_params {
 	enum v4l2_mpeg_video_h264_level level_v4l2;
 	int level;
 	u16 cpb_size;
+	int interlace;
+	u8 hier_qp;
+	u8 hier_qp_type;
+	u8 hier_qp_layer;
+	u8 hier_qp_layer_qp[7];
+	u8 sei_frame_packing;
+	u8 sei_fp_curr_frame_0;
+	u8 sei_fp_arrangement_type;
+
+	u8 fmo;
+	u8 fmo_map_type;
+	u8 fmo_slice_grp;
+	u8 fmo_chg_dir;
+	u32 fmo_chg_rate;
+	u32 fmo_run_len[4];
+	u8 aso;
+	u32 aso_slice_order[8];
 };
 
 /**
@@ -319,9 +421,11 @@ struct s5p_mfc_enc_params {
 	u8 pad_cb;
 	u8 pad_cr;
 	int rc_frame;
+	int rc_mb;
 	u32 rc_bitrate;
 	u16 rc_reaction_coeff;
 	u16 vbv_size;
+	u32 vbv_delay;
 
 	enum v4l2_mpeg_video_header_mode seq_hdr_mode;
 	enum v4l2_mpeg_mfc51_video_frame_skip_mode frame_skip_mode;
@@ -330,7 +434,6 @@ struct s5p_mfc_enc_params {
 	u8 num_b_frame;
 	u32 rc_framerate_num;
 	u32 rc_framerate_denom;
-	int interlace;
 
 	union {
 		struct s5p_mfc_h264_enc_params h264;
@@ -388,6 +491,8 @@ struct s5p_mfc_codec_ops {
  *			decoding buffer
  * @dpb_flush_flag:	flag used to indicate that a DPB buffers are being
  *			flushed
+ * @head_processed:	flag mentioning whether the header data is processed
+ *			completely or not
  * @bank1_buf:		handle to memory allocated for temporary buffers from
  *			memory bank 1
  * @bank1_phys:		address of the temporary buffers from memory bank 1
@@ -412,19 +517,20 @@ struct s5p_mfc_codec_ops {
  * @display_delay_enable:	display delay for H264 enable flag
  * @after_packed_pb:	flag used to track buffer when stream is in
  *			Packed PB format
+ * @sei_fp_parse:	enable/disable parsing of frame packing SEI information
  * @dpb_count:		count of the DPB buffers required by MFC hw
  * @total_dpb_count:	count of DPB buffers with additional buffers
  *			requested by the application
- * @ctx_buf:		handle to the memory associated with this context
- * @ctx_phys:		address of the memory associated with this context
- * @ctx_size:		size of the memory associated with this context
- * @desc_buf:		description buffer for decoding handle
- * @desc_phys:		description buffer for decoding address
- * @shm_alloc:		handle for the shared memory buffer
- * @shm:		virtual address for the shared memory buffer
- * @shm_ofs:		address offset for shared memory
+ * @ctx:		context buffer information
+ * @dsc:		descriptor buffer information
+ * @shm:		shared memory buffer information
+ * @mv_count:		number of MV buffers allocated for decoding
  * @enc_params:		encoding parameters for MFC
  * @enc_dst_buf_size:	size of the buffers for encoder output
+ * @luma_dpb_size:	dpb buffer size for luma
+ * @chroma_dpb_size:	dpb buffer size for chroma
+ * @me_buffer_size:	size of the motion estimation buffer
+ * @tmv_buffer_size:	size of temporal predictor motion vector buffer
  * @frame_type:		used to force the type of the next encoded frame
  * @ref_queue:		list of the reference buffers for encoding
  * @ref_queue_cnt:	number of the buffers in the reference list
@@ -473,6 +579,7 @@ struct s5p_mfc_ctx {
 	unsigned long consumed_stream;
 
 	unsigned int dpb_flush_flag;
+	unsigned int head_processed;
 
 	/* Buffers */
 	void *bank1_buf;
@@ -502,37 +609,41 @@ struct s5p_mfc_ctx {
 	int display_delay;
 	int display_delay_enable;
 	int after_packed_pb;
+	int sei_fp_parse;
 
 	int dpb_count;
 	int total_dpb_count;
-
+	int mv_count;
 	/* Buffers */
-	void *ctx_buf;
-	size_t ctx_phys;
-	size_t ctx_ofs;
-	size_t ctx_size;
-
-	void *desc_buf;
-	size_t desc_phys;
-
-
-	void *shm_alloc;
-	void *shm;
-	size_t shm_ofs;
+	struct s5p_mfc_priv_buf ctx;
+	struct s5p_mfc_priv_buf dsc;
+	struct s5p_mfc_priv_buf shm;
 
 	struct s5p_mfc_enc_params enc_params;
 
 	size_t enc_dst_buf_size;
+	size_t luma_dpb_size;
+	size_t chroma_dpb_size;
+	size_t me_buffer_size;
+	size_t tmv_buffer_size;
 
 	enum v4l2_mpeg_mfc51_video_force_frame_type force_frame_type;
 
 	struct list_head ref_queue;
 	unsigned int ref_queue_cnt;
 
+	enum v4l2_mpeg_video_multi_slice_mode slice_mode;
+	union {
+		unsigned int mb;
+		unsigned int bits;
+	} slice_size;
+
 	struct s5p_mfc_codec_ops *c_ops;
 
 	struct v4l2_ctrl *ctrls[MFC_MAX_CTRLS];
 	struct v4l2_ctrl_handler ctrl_handler;
+	unsigned int frame_tag;
+	size_t scratch_buf_size;
 };
 
 /*
@@ -565,6 +676,9 @@ struct mfc_control {
 	__u8			is_volatile;
 };
 
+/* Macro for making hardware specific calls */
+#define s5p_mfc_hw_call(f, op, args...) \
+	((f && f->op) ? f->op(args) : -ENODEV)
 
 #define fh_to_ctx(__fh) container_of(__fh, struct s5p_mfc_ctx, fh)
 #define ctrl_to_ctx(__ctrl) \
@@ -575,4 +689,9 @@ void set_work_bit(struct s5p_mfc_ctx *ctx);
 void clear_work_bit_irqsave(struct s5p_mfc_ctx *ctx);
 void set_work_bit_irqsave(struct s5p_mfc_ctx *ctx);
 
+#define HAS_PORTNUM(dev)	(dev ? (dev->variant ? \
+				(dev->variant->port_num ? 1 : 0) : 0) : 0)
+#define IS_TWOPORT(dev)		(dev->variant->port_num == 2 ? 1 : 0)
+#define IS_MFCV6(dev)		(dev->variant->version >= 0x60 ? 1 : 0)
+
 #endif /* S5P_MFC_COMMON_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
index 0deba6bc687..585b7b0ed8e 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
@@ -15,11 +15,11 @@
 #include <linux/firmware.h>
 #include <linux/jiffies.h>
 #include <linux/sched.h>
-#include "regs-mfc.h"
 #include "s5p_mfc_cmd.h"
 #include "s5p_mfc_common.h"
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_intr.h"
+#include "s5p_mfc_opr.h"
 #include "s5p_mfc_pm.h"
 
 static void *s5p_mfc_bitproc_buf;
@@ -37,13 +37,19 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev)
 	/* Firmare has to be present as a separate file or compiled
 	 * into kernel. */
 	mfc_debug_enter();
+
 	err = request_firmware((const struct firmware **)&fw_blob,
-				     "s5p-mfc.fw", dev->v4l2_dev.dev);
+				     dev->variant->fw_name, dev->v4l2_dev.dev);
 	if (err != 0) {
 		mfc_err("Firmware is not present in the /lib/firmware directory nor compiled in kernel\n");
 		return -EINVAL;
 	}
-	dev->fw_size = ALIGN(fw_blob->size, FIRMWARE_ALIGN);
+	dev->fw_size = dev->variant->buf_size->fw;
+	if (fw_blob->size > dev->fw_size) {
+		mfc_err("MFC firmware is too big to be loaded\n");
+		release_firmware(fw_blob);
+		return -ENOMEM;
+	}
 	if (s5p_mfc_bitproc_buf) {
 		mfc_err("Attempting to allocate firmware when it seems that it is already loaded\n");
 		release_firmware(fw_blob);
@@ -77,32 +83,37 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev)
 		return -EIO;
 	}
 	dev->bank1 = s5p_mfc_bitproc_phys;
-	b_base = vb2_dma_contig_memops.alloc(
-		dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], 1 << MFC_BASE_ALIGN_ORDER);
-	if (IS_ERR(b_base)) {
-		vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
-		s5p_mfc_bitproc_phys = 0;
-		s5p_mfc_bitproc_buf = NULL;
-		mfc_err("Allocating bank2 base failed\n");
-	release_firmware(fw_blob);
-		return -ENOMEM;
-	}
-	bank2_base_phys = s5p_mfc_mem_cookie(
-		dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], b_base);
-	vb2_dma_contig_memops.put(b_base);
-	if (bank2_base_phys & ((1 << MFC_BASE_ALIGN_ORDER) - 1)) {
-		mfc_err("The base memory for bank 2 is not aligned to 128KB\n");
-		vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
-		s5p_mfc_bitproc_phys = 0;
-		s5p_mfc_bitproc_buf = NULL;
-		release_firmware(fw_blob);
-		return -EIO;
+	if (HAS_PORTNUM(dev) && IS_TWOPORT(dev)) {
+		b_base = vb2_dma_contig_memops.alloc(
+			dev->alloc_ctx[MFC_BANK2_ALLOC_CTX],
+			1 << MFC_BASE_ALIGN_ORDER);
+		if (IS_ERR(b_base)) {
+			vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
+			s5p_mfc_bitproc_phys = 0;
+			s5p_mfc_bitproc_buf = NULL;
+			mfc_err("Allocating bank2 base failed\n");
+			release_firmware(fw_blob);
+			return -ENOMEM;
+		}
+		bank2_base_phys = s5p_mfc_mem_cookie(
+			dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], b_base);
+		vb2_dma_contig_memops.put(b_base);
+		if (bank2_base_phys & ((1 << MFC_BASE_ALIGN_ORDER) - 1)) {
+			mfc_err("The base memory for bank 2 is not aligned to 128KB\n");
+			vb2_dma_contig_memops.put(s5p_mfc_bitproc_buf);
+			s5p_mfc_bitproc_phys = 0;
+			s5p_mfc_bitproc_buf = NULL;
+			release_firmware(fw_blob);
+			return -EIO;
+		}
+		/* Valid buffers passed to MFC encoder with LAST_FRAME command
+		 * should not have address of bank2 - MFC will treat it as a null frame.
+		 * To avoid such situation we set bank2 address below the pool address.
+		 */
+		dev->bank2 = bank2_base_phys - (1 << MFC_BASE_ALIGN_ORDER);
+	} else {
+		dev->bank2 = dev->bank1;
 	}
-	/* Valid buffers passed to MFC encoder with LAST_FRAME command
-	 * should not have address of bank2 - MFC will treat it as a null frame.
-	 * To avoid such situation we set bank2 address below the pool address.
-	 */
-	dev->bank2 = bank2_base_phys - (1 << MFC_BASE_ALIGN_ORDER);
 	memcpy(s5p_mfc_bitproc_virt, fw_blob->data, fw_blob->size);
 	wmb();
 	release_firmware(fw_blob);
@@ -119,8 +130,9 @@ int s5p_mfc_reload_firmware(struct s5p_mfc_dev *dev)
 	/* Firmare has to be present as a separate file or compiled
 	 * into kernel. */
 	mfc_debug_enter();
+
 	err = request_firmware((const struct firmware **)&fw_blob,
-				     "s5p-mfc.fw", dev->v4l2_dev.dev);
+				     dev->variant->fw_name, dev->v4l2_dev.dev);
 	if (err != 0) {
 		mfc_err("Firmware is not present in the /lib/firmware directory nor compiled in kernel\n");
 		return -EINVAL;
@@ -161,46 +173,81 @@ int s5p_mfc_reset(struct s5p_mfc_dev *dev)
 {
 	unsigned int mc_status;
 	unsigned long timeout;
+	int i;
 
 	mfc_debug_enter();
-	/* Stop procedure */
-	/*  reset RISC */
-	mfc_write(dev, 0x3f6, S5P_FIMV_SW_RESET);
-	/*  All reset except for MC */
-	mfc_write(dev, 0x3e2, S5P_FIMV_SW_RESET);
-	mdelay(10);
-
-	timeout = jiffies + msecs_to_jiffies(MFC_BW_TIMEOUT);
-	/* Check MC status */
-	do {
-		if (time_after(jiffies, timeout)) {
-			mfc_err("Timeout while resetting MFC\n");
-			return -EIO;
-		}
 
-		mc_status = mfc_read(dev, S5P_FIMV_MC_STATUS);
+	if (IS_MFCV6(dev)) {
+		/* Reset IP */
+		/*  except RISC, reset */
+		mfc_write(dev, 0xFEE, S5P_FIMV_MFC_RESET_V6);
+		/*  reset release */
+		mfc_write(dev, 0x0, S5P_FIMV_MFC_RESET_V6);
+
+		/* Zero Initialization of MFC registers */
+		mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD_V6);
+		mfc_write(dev, 0, S5P_FIMV_HOST2RISC_CMD_V6);
+		mfc_write(dev, 0, S5P_FIMV_FW_VERSION_V6);
+
+		for (i = 0; i < S5P_FIMV_REG_CLEAR_COUNT_V6; i++)
+			mfc_write(dev, 0, S5P_FIMV_REG_CLEAR_BEGIN_V6 + (i*4));
 
-	} while (mc_status & 0x3);
+		/* Reset */
+		mfc_write(dev, 0, S5P_FIMV_RISC_ON_V6);
+		mfc_write(dev, 0x1FFF, S5P_FIMV_MFC_RESET_V6);
+		mfc_write(dev, 0, S5P_FIMV_MFC_RESET_V6);
+	} else {
+		/* Stop procedure */
+		/*  reset RISC */
+		mfc_write(dev, 0x3f6, S5P_FIMV_SW_RESET);
+		/*  All reset except for MC */
+		mfc_write(dev, 0x3e2, S5P_FIMV_SW_RESET);
+		mdelay(10);
+
+		timeout = jiffies + msecs_to_jiffies(MFC_BW_TIMEOUT);
+		/* Check MC status */
+		do {
+			if (time_after(jiffies, timeout)) {
+				mfc_err("Timeout while resetting MFC\n");
+				return -EIO;
+			}
+
+			mc_status = mfc_read(dev, S5P_FIMV_MC_STATUS);
+
+		} while (mc_status & 0x3);
+
+		mfc_write(dev, 0x0, S5P_FIMV_SW_RESET);
+		mfc_write(dev, 0x3fe, S5P_FIMV_SW_RESET);
+	}
 
-	mfc_write(dev, 0x0, S5P_FIMV_SW_RESET);
-	mfc_write(dev, 0x3fe, S5P_FIMV_SW_RESET);
 	mfc_debug_leave();
 	return 0;
 }
 
 static inline void s5p_mfc_init_memctrl(struct s5p_mfc_dev *dev)
 {
-	mfc_write(dev, dev->bank1, S5P_FIMV_MC_DRAMBASE_ADR_A);
-	mfc_write(dev, dev->bank2, S5P_FIMV_MC_DRAMBASE_ADR_B);
-	mfc_debug(2, "Bank1: %08x, Bank2: %08x\n", dev->bank1, dev->bank2);
+	if (IS_MFCV6(dev)) {
+		mfc_write(dev, dev->bank1, S5P_FIMV_RISC_BASE_ADDRESS_V6);
+		mfc_debug(2, "Base Address : %08x\n", dev->bank1);
+	} else {
+		mfc_write(dev, dev->bank1, S5P_FIMV_MC_DRAMBASE_ADR_A);
+		mfc_write(dev, dev->bank2, S5P_FIMV_MC_DRAMBASE_ADR_B);
+		mfc_debug(2, "Bank1: %08x, Bank2: %08x\n",
+				dev->bank1, dev->bank2);
+	}
 }
 
 static inline void s5p_mfc_clear_cmds(struct s5p_mfc_dev *dev)
 {
-	mfc_write(dev, 0xffffffff, S5P_FIMV_SI_CH0_INST_ID);
-	mfc_write(dev, 0xffffffff, S5P_FIMV_SI_CH1_INST_ID);
-	mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD);
-	mfc_write(dev, 0, S5P_FIMV_HOST2RISC_CMD);
+	if (IS_MFCV6(dev)) {
+		/* Zero initialization should be done before RESET.
+		 * Nothing to do here. */
+	} else {
+		mfc_write(dev, 0xffffffff, S5P_FIMV_SI_CH0_INST_ID);
+		mfc_write(dev, 0xffffffff, S5P_FIMV_SI_CH1_INST_ID);
+		mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD);
+		mfc_write(dev, 0, S5P_FIMV_HOST2RISC_CMD);
+	}
 }
 
 /* Initialize hardware */
@@ -228,9 +275,12 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
 	s5p_mfc_clear_cmds(dev);
 	/* 3. Release reset signal to the RISC */
 	s5p_mfc_clean_dev_int_flags(dev);
-	mfc_write(dev, 0x3ff, S5P_FIMV_SW_RESET);
+	if (IS_MFCV6(dev))
+		mfc_write(dev, 0x1, S5P_FIMV_RISC_ON_V6);
+	else
+		mfc_write(dev, 0x3ff, S5P_FIMV_SW_RESET);
 	mfc_debug(2, "Will now wait for completion of firmware transfer\n");
-	if (s5p_mfc_wait_for_done_dev(dev, S5P_FIMV_R2H_CMD_FW_STATUS_RET)) {
+	if (s5p_mfc_wait_for_done_dev(dev, S5P_MFC_R2H_CMD_FW_STATUS_RET)) {
 		mfc_err("Failed to load firmware\n");
 		s5p_mfc_reset(dev);
 		s5p_mfc_clock_off();
@@ -238,7 +288,7 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
 	}
 	s5p_mfc_clean_dev_int_flags(dev);
 	/* 4. Initialize firmware */
-	ret = s5p_mfc_sys_init_cmd(dev);
+	ret = s5p_mfc_hw_call(dev->mfc_cmds, sys_init_cmd, dev);
 	if (ret) {
 		mfc_err("Failed to send command to MFC - timeout\n");
 		s5p_mfc_reset(dev);
@@ -246,7 +296,7 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
 		return ret;
 	}
 	mfc_debug(2, "Ok, now will write a command to init the system\n");
-	if (s5p_mfc_wait_for_done_dev(dev, S5P_FIMV_R2H_CMD_SYS_INIT_RET)) {
+	if (s5p_mfc_wait_for_done_dev(dev, S5P_MFC_R2H_CMD_SYS_INIT_RET)) {
 		mfc_err("Failed to load firmware\n");
 		s5p_mfc_reset(dev);
 		s5p_mfc_clock_off();
@@ -254,7 +304,7 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
 	}
 	dev->int_cond = 0;
 	if (dev->int_err != 0 || dev->int_type !=
-					S5P_FIMV_R2H_CMD_SYS_INIT_RET) {
+					S5P_MFC_R2H_CMD_SYS_INIT_RET) {
 		/* Failure. */
 		mfc_err("Failed to init firmware - error: %d int: %d\n",
 						dev->int_err, dev->int_type);
@@ -262,7 +312,11 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
 		s5p_mfc_clock_off();
 		return -EIO;
 	}
-	ver = mfc_read(dev, S5P_FIMV_FW_VERSION);
+	if (IS_MFCV6(dev))
+		ver = mfc_read(dev, S5P_FIMV_FW_VERSION_V6);
+	else
+		ver = mfc_read(dev, S5P_FIMV_FW_VERSION);
+
 	mfc_debug(2, "MFC F/W version : %02xyy, %02xmm, %02xdd\n",
 		(ver >> 16) & 0xFF, (ver >> 8) & 0xFF, ver & 0xFF);
 	s5p_mfc_clock_off();
@@ -271,6 +325,17 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
 }
 
 
+/* Deinitialize hardware */
+void s5p_mfc_deinit_hw(struct s5p_mfc_dev *dev)
+{
+	s5p_mfc_clock_on();
+
+	s5p_mfc_reset(dev);
+	s5p_mfc_hw_call(dev->mfc_ops, release_dev_context_buffer, dev);
+
+	s5p_mfc_clock_off();
+}
+
 int s5p_mfc_sleep(struct s5p_mfc_dev *dev)
 {
 	int ret;
@@ -278,19 +343,19 @@ int s5p_mfc_sleep(struct s5p_mfc_dev *dev)
 	mfc_debug_enter();
 	s5p_mfc_clock_on();
 	s5p_mfc_clean_dev_int_flags(dev);
-	ret = s5p_mfc_sleep_cmd(dev);
+	ret = s5p_mfc_hw_call(dev->mfc_cmds, sleep_cmd, dev);
 	if (ret) {
 		mfc_err("Failed to send command to MFC - timeout\n");
 		return ret;
 	}
-	if (s5p_mfc_wait_for_done_dev(dev, S5P_FIMV_R2H_CMD_SLEEP_RET)) {
+	if (s5p_mfc_wait_for_done_dev(dev, S5P_MFC_R2H_CMD_SLEEP_RET)) {
 		mfc_err("Failed to sleep\n");
 		return -EIO;
 	}
 	s5p_mfc_clock_off();
 	dev->int_cond = 0;
 	if (dev->int_err != 0 || dev->int_type !=
-						S5P_FIMV_R2H_CMD_SLEEP_RET) {
+						S5P_MFC_R2H_CMD_SLEEP_RET) {
 		/* Failure. */
 		mfc_err("Failed to sleep - error: %d int: %d\n", dev->int_err,
 								dev->int_type);
@@ -320,22 +385,25 @@ int s5p_mfc_wakeup(struct s5p_mfc_dev *dev)
 	s5p_mfc_clear_cmds(dev);
 	s5p_mfc_clean_dev_int_flags(dev);
 	/* 3. Initialize firmware */
-	ret = s5p_mfc_wakeup_cmd(dev);
+	ret = s5p_mfc_hw_call(dev->mfc_cmds, wakeup_cmd, dev);
 	if (ret) {
 		mfc_err("Failed to send command to MFC - timeout\n");
 		return ret;
 	}
 	/* 4. Release reset signal to the RISC */
-	mfc_write(dev, 0x3ff, S5P_FIMV_SW_RESET);
+	if (IS_MFCV6(dev))
+		mfc_write(dev, 0x1, S5P_FIMV_RISC_ON_V6);
+	else
+		mfc_write(dev, 0x3ff, S5P_FIMV_SW_RESET);
 	mfc_debug(2, "Ok, now will write a command to wakeup the system\n");
-	if (s5p_mfc_wait_for_done_dev(dev, S5P_FIMV_R2H_CMD_WAKEUP_RET)) {
+	if (s5p_mfc_wait_for_done_dev(dev, S5P_MFC_R2H_CMD_WAKEUP_RET)) {
 		mfc_err("Failed to load firmware\n");
 		return -EIO;
 	}
 	s5p_mfc_clock_off();
 	dev->int_cond = 0;
 	if (dev->int_err != 0 || dev->int_type !=
-						S5P_FIMV_R2H_CMD_WAKEUP_RET) {
+						S5P_MFC_R2H_CMD_WAKEUP_RET) {
 		/* Failure. */
 		mfc_err("Failed to wakeup - error: %d int: %d\n", dev->int_err,
 								dev->int_type);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h
index e1e0c544b6a..90aa9b9886d 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h
@@ -20,6 +20,7 @@ int s5p_mfc_alloc_and_load_firmware(struct s5p_mfc_dev *dev);
 int s5p_mfc_reload_firmware(struct s5p_mfc_dev *dev);
 
 int s5p_mfc_init_hw(struct s5p_mfc_dev *dev);
+void s5p_mfc_deinit_hw(struct s5p_mfc_dev *dev);
 
 int s5p_mfc_sleep(struct s5p_mfc_dev *dev);
 int s5p_mfc_wakeup(struct s5p_mfc_dev *dev);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
index 6ee21bb7139..eb6a70b0f82 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
@@ -23,85 +23,114 @@
 #include <linux/workqueue.h>
 #include <media/v4l2-ctrls.h>
 #include <media/videobuf2-core.h>
-#include "regs-mfc.h"
 #include "s5p_mfc_common.h"
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_dec.h"
 #include "s5p_mfc_intr.h"
 #include "s5p_mfc_opr.h"
 #include "s5p_mfc_pm.h"
-#include "s5p_mfc_shm.h"
+
+#define DEF_SRC_FMT_DEC	V4L2_PIX_FMT_H264
+#define DEF_DST_FMT_DEC	V4L2_PIX_FMT_NV12MT_16X16
 
 static struct s5p_mfc_fmt formats[] = {
 	{
+		.name		= "4:2:0 2 Planes 16x16 Tiles",
+		.fourcc		= V4L2_PIX_FMT_NV12MT_16X16,
+		.codec_mode	= S5P_MFC_CODEC_NONE,
+		.type		= MFC_FMT_RAW,
+		.num_planes	= 2,
+	},
+	{
 		.name		= "4:2:0 2 Planes 64x32 Tiles",
 		.fourcc		= V4L2_PIX_FMT_NV12MT,
-		.codec_mode	= S5P_FIMV_CODEC_NONE,
+		.codec_mode	= S5P_MFC_CODEC_NONE,
 		.type		= MFC_FMT_RAW,
 		.num_planes	= 2,
-	 },
+	},
 	{
-		.name = "4:2:0 2 Planes",
-		.fourcc = V4L2_PIX_FMT_NV12M,
-		.codec_mode = S5P_FIMV_CODEC_NONE,
-		.type = MFC_FMT_RAW,
-		.num_planes = 2,
+		.name		= "4:2:0 2 Planes Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV12M,
+		.codec_mode	= S5P_MFC_CODEC_NONE,
+		.type		= MFC_FMT_RAW,
+		.num_planes	= 2,
+	},
+	{
+		.name		= "4:2:0 2 Planes Y/CrCb",
+		.fourcc		= V4L2_PIX_FMT_NV21M,
+		.codec_mode	= S5P_MFC_CODEC_NONE,
+		.type		= MFC_FMT_RAW,
+		.num_planes	= 2,
 	},
 	{
-		.name = "H264 Encoded Stream",
-		.fourcc = V4L2_PIX_FMT_H264,
-		.codec_mode = S5P_FIMV_CODEC_H264_DEC,
-		.type = MFC_FMT_DEC,
-		.num_planes = 1,
+		.name		= "H264 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_H264,
+		.codec_mode	= S5P_MFC_CODEC_H264_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
 	},
 	{
-		.name = "H263 Encoded Stream",
-		.fourcc = V4L2_PIX_FMT_H263,
-		.codec_mode = S5P_FIMV_CODEC_H263_DEC,
-		.type = MFC_FMT_DEC,
-		.num_planes = 1,
+		.name		= "H264/MVC Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_H264_MVC,
+		.codec_mode	= S5P_MFC_CODEC_H264_MVC_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
 	},
 	{
-		.name = "MPEG1 Encoded Stream",
-		.fourcc = V4L2_PIX_FMT_MPEG1,
-		.codec_mode = S5P_FIMV_CODEC_MPEG2_DEC,
-		.type = MFC_FMT_DEC,
-		.num_planes = 1,
+		.name		= "H263 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_H263,
+		.codec_mode	= S5P_MFC_CODEC_H263_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
 	},
 	{
-		.name = "MPEG2 Encoded Stream",
-		.fourcc = V4L2_PIX_FMT_MPEG2,
-		.codec_mode = S5P_FIMV_CODEC_MPEG2_DEC,
-		.type = MFC_FMT_DEC,
-		.num_planes = 1,
+		.name		= "MPEG1 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_MPEG1,
+		.codec_mode	= S5P_MFC_CODEC_MPEG2_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
 	},
 	{
-		.name = "MPEG4 Encoded Stream",
-		.fourcc = V4L2_PIX_FMT_MPEG4,
-		.codec_mode = S5P_FIMV_CODEC_MPEG4_DEC,
-		.type = MFC_FMT_DEC,
-		.num_planes = 1,
+		.name		= "MPEG2 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_MPEG2,
+		.codec_mode	= S5P_MFC_CODEC_MPEG2_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
 	},
 	{
-		.name = "XviD Encoded Stream",
-		.fourcc = V4L2_PIX_FMT_XVID,
-		.codec_mode = S5P_FIMV_CODEC_MPEG4_DEC,
-		.type = MFC_FMT_DEC,
-		.num_planes = 1,
+		.name		= "MPEG4 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_MPEG4,
+		.codec_mode	= S5P_MFC_CODEC_MPEG4_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
 	},
 	{
-		.name = "VC1 Encoded Stream",
-		.fourcc = V4L2_PIX_FMT_VC1_ANNEX_G,
-		.codec_mode = S5P_FIMV_CODEC_VC1_DEC,
-		.type = MFC_FMT_DEC,
-		.num_planes = 1,
+		.name		= "XviD Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_XVID,
+		.codec_mode	= S5P_MFC_CODEC_MPEG4_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
 	},
 	{
-		.name = "VC1 RCV Encoded Stream",
-		.fourcc = V4L2_PIX_FMT_VC1_ANNEX_L,
-		.codec_mode = S5P_FIMV_CODEC_VC1RCV_DEC,
-		.type = MFC_FMT_DEC,
-		.num_planes = 1,
+		.name		= "VC1 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_VC1_ANNEX_G,
+		.codec_mode	= S5P_MFC_CODEC_VC1_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
+	},
+	{
+		.name		= "VC1 RCV Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_VC1_ANNEX_L,
+		.codec_mode	= S5P_MFC_CODEC_VC1RCV_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
+	},
+	{
+		.name		= "VP8 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_VP8,
+		.codec_mode	= S5P_MFC_CODEC_VP8_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
 	},
 };
 
@@ -297,7 +326,7 @@ static int vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
 		/* If the MFC is parsing the header,
 		 * so wait until it is finished */
 		s5p_mfc_clean_ctx_int_flags(ctx);
-		s5p_mfc_wait_for_done_ctx(ctx, S5P_FIMV_R2H_CMD_SEQ_DONE_RET,
+		s5p_mfc_wait_for_done_ctx(ctx, S5P_MFC_R2H_CMD_SEQ_DONE_RET,
 									0);
 	}
 	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
@@ -342,21 +371,36 @@ static int vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
 /* Try format */
 static int vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
 {
+	struct s5p_mfc_dev *dev = video_drvdata(file);
 	struct s5p_mfc_fmt *fmt;
 
-	if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
-		mfc_err("This node supports decoding only\n");
-		return -EINVAL;
-	}
-	fmt = find_format(f, MFC_FMT_DEC);
-	if (!fmt) {
-		mfc_err("Unsupported format\n");
-		return -EINVAL;
-	}
-	if (fmt->type != MFC_FMT_DEC) {
-		mfc_err("\n");
-		return -EINVAL;
+	mfc_debug(2, "Type is %d\n", f->type);
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		fmt = find_format(f, MFC_FMT_DEC);
+		if (!fmt) {
+			mfc_err("Unsupported format for source.\n");
+			return -EINVAL;
+		}
+		if (!IS_MFCV6(dev) && (fmt->fourcc == V4L2_PIX_FMT_VP8)) {
+			mfc_err("Not supported format.\n");
+			return -EINVAL;
+		}
+	} else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		fmt = find_format(f, MFC_FMT_RAW);
+		if (!fmt) {
+			mfc_err("Unsupported format for destination.\n");
+			return -EINVAL;
+		}
+		if (IS_MFCV6(dev) && (fmt->fourcc == V4L2_PIX_FMT_NV12MT)) {
+			mfc_err("Not supported format.\n");
+			return -EINVAL;
+		} else if (!IS_MFCV6(dev) &&
+				(fmt->fourcc != V4L2_PIX_FMT_NV12MT)) {
+			mfc_err("Not supported format.\n");
+			return -EINVAL;
+		}
 	}
+
 	return 0;
 }
 
@@ -379,8 +423,29 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
 		ret = -EBUSY;
 		goto out;
 	}
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		fmt = find_format(f, MFC_FMT_RAW);
+		if (!fmt) {
+			mfc_err("Unsupported format for source.\n");
+			return -EINVAL;
+		}
+		if (!IS_MFCV6(dev) && (fmt->fourcc != V4L2_PIX_FMT_NV12MT)) {
+			mfc_err("Not supported format.\n");
+			return -EINVAL;
+		} else if (IS_MFCV6(dev) &&
+				(fmt->fourcc == V4L2_PIX_FMT_NV12MT)) {
+			mfc_err("Not supported format.\n");
+			return -EINVAL;
+		}
+		ctx->dst_fmt = fmt;
+		mfc_debug_leave();
+		return ret;
+	} else if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		mfc_err("Wrong type error for S_FMT : %d", f->type);
+		return -EINVAL;
+	}
 	fmt = find_format(f, MFC_FMT_DEC);
-	if (!fmt || fmt->codec_mode == S5P_FIMV_CODEC_NONE) {
+	if (!fmt || fmt->codec_mode == S5P_MFC_CODEC_NONE) {
 		mfc_err("Unknown codec\n");
 		ret = -EINVAL;
 		goto out;
@@ -391,6 +456,10 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
 		ret = -EINVAL;
 		goto out;
 	}
+	if (!IS_MFCV6(dev) && (fmt->fourcc == V4L2_PIX_FMT_VP8)) {
+		mfc_err("Not supported format.\n");
+		return -EINVAL;
+	}
 	ctx->src_fmt = fmt;
 	ctx->codec_mode = fmt->codec_mode;
 	mfc_debug(2, "The codec number is: %d\n", ctx->codec_mode);
@@ -476,7 +545,7 @@ static int vidioc_reqbufs(struct file *file, void *priv,
 			return -ENOMEM;
 		}
 		ctx->total_dpb_count = reqbufs->count;
-		ret = s5p_mfc_alloc_codec_buffers(ctx);
+		ret = s5p_mfc_hw_call(dev->mfc_ops, alloc_codec_buffers, ctx);
 		if (ret) {
 			mfc_err("Failed to allocate decoding buffers\n");
 			reqbufs->count = 0;
@@ -492,15 +561,16 @@ static int vidioc_reqbufs(struct file *file, void *priv,
 			reqbufs->count = 0;
 			s5p_mfc_clock_on();
 			ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
-			s5p_mfc_release_codec_buffers(ctx);
+			s5p_mfc_hw_call(dev->mfc_ops, release_codec_buffers,
+					ctx);
 			s5p_mfc_clock_off();
 			return -ENOMEM;
 		}
 		if (s5p_mfc_ctx_ready(ctx))
 			set_work_bit_irqsave(ctx);
-		s5p_mfc_try_run(dev);
+		s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 		s5p_mfc_wait_for_done_ctx(ctx,
-					 S5P_FIMV_R2H_CMD_INIT_BUFFERS_RET, 0);
+					S5P_MFC_R2H_CMD_INIT_BUFFERS_RET, 0);
 	}
 	return ret;
 }
@@ -582,18 +652,22 @@ static int vidioc_streamon(struct file *file, void *priv,
 			ctx->src_bufs_cnt = 0;
 			ctx->capture_state = QUEUE_FREE;
 			ctx->output_state = QUEUE_FREE;
-			s5p_mfc_alloc_instance_buffer(ctx);
-			s5p_mfc_alloc_dec_temp_buffers(ctx);
+			s5p_mfc_hw_call(dev->mfc_ops, alloc_instance_buffer,
+					ctx);
+			s5p_mfc_hw_call(dev->mfc_ops, alloc_dec_temp_buffers,
+					ctx);
 			set_work_bit_irqsave(ctx);
 			s5p_mfc_clean_ctx_int_flags(ctx);
-			s5p_mfc_try_run(dev);
+			s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 
 			if (s5p_mfc_wait_for_done_ctx(ctx,
-				S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET, 0)) {
+				S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET, 0)) {
 				/* Error or timeout */
 				mfc_err("Error getting instance from hardware\n");
-				s5p_mfc_release_instance_buffer(ctx);
-				s5p_mfc_release_dec_desc_buffer(ctx);
+				s5p_mfc_hw_call(dev->mfc_ops,
+						release_instance_buffer, ctx);
+				s5p_mfc_hw_call(dev->mfc_ops,
+						release_dec_desc_buffer, ctx);
 				return -EIO;
 			}
 			mfc_debug(2, "Got instance number: %d\n", ctx->inst_no);
@@ -662,7 +736,7 @@ static int s5p_mfc_dec_g_v_ctrl(struct v4l2_ctrl *ctrl)
 		/* Should wait for the header to be parsed */
 		s5p_mfc_clean_ctx_int_flags(ctx);
 		s5p_mfc_wait_for_done_ctx(ctx,
-				S5P_FIMV_R2H_CMD_SEQ_DONE_RET, 0);
+				S5P_MFC_R2H_CMD_SEQ_DONE_RET, 0);
 		if (ctx->state >= MFCINST_HEAD_PARSED &&
 		    ctx->state < MFCINST_ABORT) {
 			ctrl->val = ctx->dpb_count;
@@ -686,6 +760,7 @@ static int vidioc_g_crop(struct file *file, void *priv,
 		struct v4l2_crop *cr)
 {
 	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+	struct s5p_mfc_dev *dev = ctx->dev;
 	u32 left, right, top, bottom;
 
 	if (ctx->state != MFCINST_HEAD_PARSED &&
@@ -695,10 +770,10 @@ static int vidioc_g_crop(struct file *file, void *priv,
 			return -EINVAL;
 		}
 	if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_H264) {
-		left = s5p_mfc_read_shm(ctx, CROP_INFO_H);
+		left = s5p_mfc_hw_call(dev->mfc_ops, get_crop_info_h, ctx);
 		right = left >> S5P_FIMV_SHARED_CROP_RIGHT_SHIFT;
 		left = left & S5P_FIMV_SHARED_CROP_LEFT_MASK;
-		top = s5p_mfc_read_shm(ctx, CROP_INFO_V);
+		top = s5p_mfc_hw_call(dev->mfc_ops, get_crop_info_v, ctx);
 		bottom = top >> S5P_FIMV_SHARED_CROP_BOTTOM_SHIFT;
 		top = top & S5P_FIMV_SHARED_CROP_TOP_MASK;
 		cr->c.left = left;
@@ -749,6 +824,7 @@ static int s5p_mfc_queue_setup(struct vb2_queue *vq,
 			void *allocators[])
 {
 	struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+	struct s5p_mfc_dev *dev = ctx->dev;
 
 	/* Video output for decoding (source)
 	 * this can be set after getting an instance */
@@ -784,7 +860,13 @@ static int s5p_mfc_queue_setup(struct vb2_queue *vq,
 	    vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
 		psize[0] = ctx->luma_size;
 		psize[1] = ctx->chroma_size;
-		allocators[0] = ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
+
+		if (IS_MFCV6(dev))
+			allocators[0] =
+				ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
+		else
+			allocators[0] =
+				ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
 		allocators[1] = ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
 	} else if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE &&
 		   ctx->state == MFCINST_INIT) {
@@ -876,7 +958,7 @@ static int s5p_mfc_start_streaming(struct vb2_queue *q, unsigned int count)
 	/* If context is ready then dev = work->data;schedule it to run */
 	if (s5p_mfc_ctx_ready(ctx))
 		set_work_bit_irqsave(ctx);
-	s5p_mfc_try_run(dev);
+	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 	return 0;
 }
 
@@ -892,19 +974,21 @@ static int s5p_mfc_stop_streaming(struct vb2_queue *q)
 		dev->curr_ctx == ctx->num && dev->hw_lock) {
 		ctx->state = MFCINST_ABORT;
 		s5p_mfc_wait_for_done_ctx(ctx,
-					S5P_FIMV_R2H_CMD_FRAME_DONE_RET, 0);
+					S5P_MFC_R2H_CMD_FRAME_DONE_RET, 0);
 		aborted = 1;
 	}
 	spin_lock_irqsave(&dev->irqlock, flags);
 	if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
-		s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
+		s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->dst_queue,
+				&ctx->vq_dst);
 		INIT_LIST_HEAD(&ctx->dst_queue);
 		ctx->dst_queue_cnt = 0;
 		ctx->dpb_flush_flag = 1;
 		ctx->dec_dst_flag = 0;
 	}
 	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
-		s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
+		s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->src_queue,
+				&ctx->vq_src);
 		INIT_LIST_HEAD(&ctx->src_queue);
 		ctx->src_queue_cnt = 0;
 	}
@@ -944,7 +1028,7 @@ static void s5p_mfc_buf_queue(struct vb2_buffer *vb)
 	}
 	if (s5p_mfc_ctx_ready(ctx))
 		set_work_bit_irqsave(ctx);
-	s5p_mfc_try_run(dev);
+	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 }
 
 static struct vb2_ops s5p_mfc_dec_qops = {
@@ -1028,3 +1112,13 @@ void s5p_mfc_dec_ctrls_delete(struct s5p_mfc_ctx *ctx)
 		ctx->ctrls[i] = NULL;
 }
 
+void s5p_mfc_dec_init(struct s5p_mfc_ctx *ctx)
+{
+	struct v4l2_format f;
+	f.fmt.pix_mp.pixelformat = DEF_SRC_FMT_DEC;
+	ctx->src_fmt = find_format(&f, MFC_FMT_DEC);
+	f.fmt.pix_mp.pixelformat = DEF_DST_FMT_DEC;
+	ctx->dst_fmt = find_format(&f, MFC_FMT_RAW);
+	mfc_debug(2, "Default src_fmt is %x, dest_fmt is %x\n",
+			(unsigned int)ctx->src_fmt, (unsigned int)ctx->dst_fmt);
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h
index fdf1d99a9d1..d06a7cab5eb 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h
@@ -19,5 +19,6 @@ const struct v4l2_ioctl_ops *get_dec_v4l2_ioctl_ops(void);
 struct s5p_mfc_fmt *get_dec_def_fmt(bool src);
 int s5p_mfc_dec_ctrls_setup(struct s5p_mfc_ctx *ctx);
 void s5p_mfc_dec_ctrls_delete(struct s5p_mfc_ctx *ctx);
+void s5p_mfc_dec_init(struct s5p_mfc_ctx *ctx);
 
 #endif /* S5P_MFC_DEC_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
index 179e4db60b1..2af6d522f4a 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
@@ -25,48 +25,64 @@
 #include <linux/workqueue.h>
 #include <media/v4l2-ctrls.h>
 #include <media/videobuf2-core.h>
-#include "regs-mfc.h"
 #include "s5p_mfc_common.h"
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_enc.h"
 #include "s5p_mfc_intr.h"
 #include "s5p_mfc_opr.h"
 
+#define DEF_SRC_FMT_ENC	V4L2_PIX_FMT_NV12MT
+#define DEF_DST_FMT_ENC	V4L2_PIX_FMT_H264
+
 static struct s5p_mfc_fmt formats[] = {
 	{
-		.name = "4:2:0 2 Planes 64x32 Tiles",
-		.fourcc = V4L2_PIX_FMT_NV12MT,
-		.codec_mode = S5P_FIMV_CODEC_NONE,
-		.type = MFC_FMT_RAW,
-		.num_planes = 2,
+		.name		= "4:2:0 2 Planes 16x16 Tiles",
+		.fourcc		= V4L2_PIX_FMT_NV12MT_16X16,
+		.codec_mode	= S5P_MFC_CODEC_NONE,
+		.type		= MFC_FMT_RAW,
+		.num_planes	= 2,
+	},
+	{
+		.name		= "4:2:0 2 Planes 64x32 Tiles",
+		.fourcc		= V4L2_PIX_FMT_NV12MT,
+		.codec_mode	= S5P_MFC_CODEC_NONE,
+		.type		= MFC_FMT_RAW,
+		.num_planes	= 2,
+	},
+	{
+		.name		= "4:2:0 2 Planes Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV12M,
+		.codec_mode	= S5P_MFC_CODEC_NONE,
+		.type		= MFC_FMT_RAW,
+		.num_planes	= 2,
 	},
 	{
-		.name = "4:2:0 2 Planes",
-		.fourcc = V4L2_PIX_FMT_NV12M,
-		.codec_mode = S5P_FIMV_CODEC_NONE,
-		.type = MFC_FMT_RAW,
-		.num_planes = 2,
+		.name		= "4:2:0 2 Planes Y/CrCb",
+		.fourcc		= V4L2_PIX_FMT_NV21M,
+		.codec_mode	= S5P_MFC_CODEC_NONE,
+		.type		= MFC_FMT_RAW,
+		.num_planes	= 2,
 	},
 	{
-		.name = "H264 Encoded Stream",
-		.fourcc = V4L2_PIX_FMT_H264,
-		.codec_mode = S5P_FIMV_CODEC_H264_ENC,
-		.type = MFC_FMT_ENC,
-		.num_planes = 1,
+		.name		= "H264 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_H264,
+		.codec_mode	= S5P_MFC_CODEC_H264_ENC,
+		.type		= MFC_FMT_ENC,
+		.num_planes	= 1,
 	},
 	{
-		.name = "MPEG4 Encoded Stream",
-		.fourcc = V4L2_PIX_FMT_MPEG4,
-		.codec_mode = S5P_FIMV_CODEC_MPEG4_ENC,
-		.type = MFC_FMT_ENC,
-		.num_planes = 1,
+		.name		= "MPEG4 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_MPEG4,
+		.codec_mode	= S5P_MFC_CODEC_MPEG4_ENC,
+		.type		= MFC_FMT_ENC,
+		.num_planes	= 1,
 	},
 	{
-		.name = "H263 Encoded Stream",
-		.fourcc = V4L2_PIX_FMT_H263,
-		.codec_mode = S5P_FIMV_CODEC_H263_ENC,
-		.type = MFC_FMT_ENC,
-		.num_planes = 1,
+		.name		= "H263 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_H263,
+		.codec_mode	= S5P_MFC_CODEC_H263_ENC,
+		.type		= MFC_FMT_ENC,
+		.num_planes	= 1,
 	},
 };
 
@@ -574,7 +590,8 @@ static int s5p_mfc_ctx_ready(struct s5p_mfc_ctx *ctx)
 	if (ctx->state == MFCINST_GOT_INST && ctx->dst_queue_cnt >= 1)
 		return 1;
 	/* context is ready to encode a frame */
-	if (ctx->state == MFCINST_RUNNING &&
+	if ((ctx->state == MFCINST_RUNNING ||
+		ctx->state == MFCINST_HEAD_PARSED) &&
 		ctx->src_queue_cnt >= 1 && ctx->dst_queue_cnt >= 1)
 		return 1;
 	/* context is ready to encode remaining frames */
@@ -619,7 +636,8 @@ static int enc_pre_seq_start(struct s5p_mfc_ctx *ctx)
 	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
 	dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
 	dst_size = vb2_plane_size(dst_mb->b, 0);
-	s5p_mfc_set_enc_stream_buffer(ctx, dst_addr, dst_size);
+	s5p_mfc_hw_call(dev->mfc_ops, set_enc_stream_buffer, ctx, dst_addr,
+			dst_size);
 	spin_unlock_irqrestore(&dev->irqlock, flags);
 	return 0;
 }
@@ -638,14 +656,23 @@ static int enc_post_seq_start(struct s5p_mfc_ctx *ctx)
 		list_del(&dst_mb->list);
 		ctx->dst_queue_cnt--;
 		vb2_set_plane_payload(dst_mb->b, 0,
-						s5p_mfc_get_enc_strm_size());
+			s5p_mfc_hw_call(dev->mfc_ops, get_enc_strm_size, dev));
 		vb2_buffer_done(dst_mb->b, VB2_BUF_STATE_DONE);
 		spin_unlock_irqrestore(&dev->irqlock, flags);
 	}
-	ctx->state = MFCINST_RUNNING;
-	if (s5p_mfc_ctx_ready(ctx))
-		set_work_bit_irqsave(ctx);
-	s5p_mfc_try_run(dev);
+	if (IS_MFCV6(dev)) {
+		ctx->state = MFCINST_HEAD_PARSED; /* for INIT_BUFFER cmd */
+	} else {
+		ctx->state = MFCINST_RUNNING;
+		if (s5p_mfc_ctx_ready(ctx))
+			set_work_bit_irqsave(ctx);
+		s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+	}
+
+	if (IS_MFCV6(dev))
+		ctx->dpb_count = s5p_mfc_hw_call(dev->mfc_ops,
+				get_enc_dpb_count, dev);
+
 	return 0;
 }
 
@@ -662,14 +689,16 @@ static int enc_pre_frame_start(struct s5p_mfc_ctx *ctx)
 	src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
 	src_y_addr = vb2_dma_contig_plane_dma_addr(src_mb->b, 0);
 	src_c_addr = vb2_dma_contig_plane_dma_addr(src_mb->b, 1);
-	s5p_mfc_set_enc_frame_buffer(ctx, src_y_addr, src_c_addr);
+	s5p_mfc_hw_call(dev->mfc_ops, set_enc_frame_buffer, ctx, src_y_addr,
+			src_c_addr);
 	spin_unlock_irqrestore(&dev->irqlock, flags);
 
 	spin_lock_irqsave(&dev->irqlock, flags);
 	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
 	dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
 	dst_size = vb2_plane_size(dst_mb->b, 0);
-	s5p_mfc_set_enc_stream_buffer(ctx, dst_addr, dst_size);
+	s5p_mfc_hw_call(dev->mfc_ops, set_enc_stream_buffer, ctx, dst_addr,
+			dst_size);
 	spin_unlock_irqrestore(&dev->irqlock, flags);
 
 	return 0;
@@ -685,15 +714,16 @@ static int enc_post_frame_start(struct s5p_mfc_ctx *ctx)
 	unsigned int strm_size;
 	unsigned long flags;
 
-	slice_type = s5p_mfc_get_enc_slice_type();
-	strm_size = s5p_mfc_get_enc_strm_size();
+	slice_type = s5p_mfc_hw_call(dev->mfc_ops, get_enc_slice_type, dev);
+	strm_size = s5p_mfc_hw_call(dev->mfc_ops, get_enc_strm_size, dev);
 	mfc_debug(2, "Encoded slice type: %d", slice_type);
 	mfc_debug(2, "Encoded stream size: %d", strm_size);
 	mfc_debug(2, "Display order: %d",
 		  mfc_read(dev, S5P_FIMV_ENC_SI_PIC_CNT));
 	spin_lock_irqsave(&dev->irqlock, flags);
 	if (slice_type >= 0) {
-		s5p_mfc_get_enc_frame_buffer(ctx, &enc_y_addr, &enc_c_addr);
+		s5p_mfc_hw_call(dev->mfc_ops, get_enc_frame_buffer, ctx,
+				&enc_y_addr, &enc_c_addr);
 		list_for_each_entry(mb_entry, &ctx->src_queue, list) {
 			mb_y_addr = vb2_dma_contig_plane_dma_addr(mb_entry->b, 0);
 			mb_c_addr = vb2_dma_contig_plane_dma_addr(mb_entry->b, 1);
@@ -939,15 +969,16 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
 		pix_fmt_mp->plane_fmt[0].bytesperline = 0;
 		ctx->dst_bufs_cnt = 0;
 		ctx->capture_state = QUEUE_FREE;
-		s5p_mfc_alloc_instance_buffer(ctx);
+		s5p_mfc_hw_call(dev->mfc_ops, alloc_instance_buffer, ctx);
 		set_work_bit_irqsave(ctx);
 		s5p_mfc_clean_ctx_int_flags(ctx);
-		s5p_mfc_try_run(dev);
+		s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 		if (s5p_mfc_wait_for_done_ctx(ctx, \
-				S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET, 1)) {
+				S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET, 1)) {
 				/* Error or timeout */
 			mfc_err("Error getting instance from hardware\n");
-			s5p_mfc_release_instance_buffer(ctx);
+			s5p_mfc_hw_call(dev->mfc_ops, release_instance_buffer,
+					ctx);
 			ret = -EIO;
 			goto out;
 		}
@@ -958,6 +989,17 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
 			mfc_err("failed to set output format\n");
 			return -EINVAL;
 		}
+
+		if (!IS_MFCV6(dev) &&
+				(fmt->fourcc == V4L2_PIX_FMT_NV12MT_16X16)) {
+			mfc_err("Not supported format.\n");
+			return -EINVAL;
+		} else if (IS_MFCV6(dev) &&
+				(fmt->fourcc == V4L2_PIX_FMT_NV12MT)) {
+			mfc_err("Not supported format.\n");
+			return -EINVAL;
+		}
+
 		if (fmt->num_planes != pix_fmt_mp->num_planes) {
 			mfc_err("failed to set output format\n");
 			ret = -EINVAL;
@@ -970,45 +1012,13 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
 		mfc_debug(2, "fmt - w: %d, h: %d, ctx - w: %d, h: %d\n",
 			pix_fmt_mp->width, pix_fmt_mp->height,
 			ctx->img_width, ctx->img_height);
-		if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M) {
-			ctx->buf_width = ALIGN(ctx->img_width,
-							S5P_FIMV_NV12M_HALIGN);
-			ctx->luma_size = ALIGN(ctx->img_width,
-				S5P_FIMV_NV12M_HALIGN) * ALIGN(ctx->img_height,
-				S5P_FIMV_NV12M_LVALIGN);
-			ctx->chroma_size = ALIGN(ctx->img_width,
-				S5P_FIMV_NV12M_HALIGN) * ALIGN((ctx->img_height
-				>> 1), S5P_FIMV_NV12M_CVALIGN);
-
-			ctx->luma_size = ALIGN(ctx->luma_size,
-							S5P_FIMV_NV12M_SALIGN);
-			ctx->chroma_size = ALIGN(ctx->chroma_size,
-							S5P_FIMV_NV12M_SALIGN);
-
-			pix_fmt_mp->plane_fmt[0].sizeimage = ctx->luma_size;
-			pix_fmt_mp->plane_fmt[0].bytesperline = ctx->buf_width;
-			pix_fmt_mp->plane_fmt[1].sizeimage = ctx->chroma_size;
-			pix_fmt_mp->plane_fmt[1].bytesperline = ctx->buf_width;
-
-		} else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT) {
-			ctx->buf_width = ALIGN(ctx->img_width,
-							S5P_FIMV_NV12MT_HALIGN);
-			ctx->luma_size = ALIGN(ctx->img_width,
-				S5P_FIMV_NV12MT_HALIGN)	* ALIGN(ctx->img_height,
-				S5P_FIMV_NV12MT_VALIGN);
-			ctx->chroma_size = ALIGN(ctx->img_width,
-				S5P_FIMV_NV12MT_HALIGN) * ALIGN((ctx->img_height
-				>> 1), S5P_FIMV_NV12MT_VALIGN);
-			ctx->luma_size = ALIGN(ctx->luma_size,
-							S5P_FIMV_NV12MT_SALIGN);
-			ctx->chroma_size = ALIGN(ctx->chroma_size,
-							S5P_FIMV_NV12MT_SALIGN);
-
-			pix_fmt_mp->plane_fmt[0].sizeimage = ctx->luma_size;
-			pix_fmt_mp->plane_fmt[0].bytesperline = ctx->buf_width;
-			pix_fmt_mp->plane_fmt[1].sizeimage = ctx->chroma_size;
-			pix_fmt_mp->plane_fmt[1].bytesperline = ctx->buf_width;
-		}
+
+		s5p_mfc_hw_call(dev->mfc_ops, enc_calc_src_size, ctx);
+		pix_fmt_mp->plane_fmt[0].sizeimage = ctx->luma_size;
+		pix_fmt_mp->plane_fmt[0].bytesperline = ctx->buf_width;
+		pix_fmt_mp->plane_fmt[1].sizeimage = ctx->chroma_size;
+		pix_fmt_mp->plane_fmt[1].bytesperline = ctx->buf_width;
+
 		ctx->src_bufs_cnt = 0;
 		ctx->output_state = QUEUE_FREE;
 	} else {
@@ -1023,6 +1033,7 @@ out:
 static int vidioc_reqbufs(struct file *file, void *priv,
 					  struct v4l2_requestbuffers *reqbufs)
 {
+	struct s5p_mfc_dev *dev = video_drvdata(file);
 	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
 	int ret = 0;
 
@@ -1042,12 +1053,16 @@ static int vidioc_reqbufs(struct file *file, void *priv,
 			return ret;
 		}
 		ctx->capture_state = QUEUE_BUFS_REQUESTED;
-		ret = s5p_mfc_alloc_codec_buffers(ctx);
-		if (ret) {
-			mfc_err("Failed to allocate encoding buffers\n");
-			reqbufs->count = 0;
-			ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
-			return -ENOMEM;
+
+		if (!IS_MFCV6(dev)) {
+			ret = s5p_mfc_hw_call(ctx->dev->mfc_ops,
+					alloc_codec_buffers, ctx);
+			if (ret) {
+				mfc_err("Failed to allocate encoding buffers\n");
+				reqbufs->count = 0;
+				ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
+				return -ENOMEM;
+			}
 		}
 	} else if (reqbufs->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 		if (ctx->output_state != QUEUE_FREE) {
@@ -1310,6 +1325,13 @@ static int s5p_mfc_enc_s_ctrl(struct v4l2_ctrl *ctrl)
 			p->codec.h264.profile =
 				S5P_FIMV_ENC_PROFILE_H264_BASELINE;
 			break;
+		case V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE:
+			if (IS_MFCV6(dev))
+				p->codec.h264.profile =
+				S5P_FIMV_ENC_PROFILE_H264_CONSTRAINED_BASELINE;
+			else
+				ret = -EINVAL;
+			break;
 		default:
 			ret = -EINVAL;
 		}
@@ -1349,7 +1371,7 @@ static int s5p_mfc_enc_s_ctrl(struct v4l2_ctrl *ctrl)
 		p->codec.h264._8x8_transform = ctrl->val;
 		break;
 	case V4L2_CID_MPEG_VIDEO_MB_RC_ENABLE:
-		p->codec.h264.rc_mb = ctrl->val;
+		p->rc_mb = ctrl->val;
 		break;
 	case V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP:
 		p->codec.h264.rc_frame_qp = ctrl->val;
@@ -1500,7 +1522,7 @@ int vidioc_encoder_cmd(struct file *file, void *priv,
 			mfc_debug(2, "EOS: empty src queue, entering finishing state");
 			ctx->state = MFCINST_FINISHING;
 			spin_unlock_irqrestore(&dev->irqlock, flags);
-			s5p_mfc_try_run(dev);
+			s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 		} else {
 			mfc_debug(2, "EOS: marking last buffer of stream");
 			buf = list_entry(ctx->src_queue.prev,
@@ -1583,6 +1605,7 @@ static int s5p_mfc_queue_setup(struct vb2_queue *vq,
 			unsigned int psize[], void *allocators[])
 {
 	struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+	struct s5p_mfc_dev *dev = ctx->dev;
 
 	if (ctx->state != MFCINST_GOT_INST) {
 		mfc_err("inavlid state: %d\n", ctx->state);
@@ -1611,8 +1634,17 @@ static int s5p_mfc_queue_setup(struct vb2_queue *vq,
 			*buf_count = MFC_MAX_BUFFERS;
 		psize[0] = ctx->luma_size;
 		psize[1] = ctx->chroma_size;
-		allocators[0] = ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
-		allocators[1] = ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
+		if (IS_MFCV6(dev)) {
+			allocators[0] =
+				ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
+			allocators[1] =
+				ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
+		} else {
+			allocators[0] =
+				ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
+			allocators[1] =
+				ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
+		}
 	} else {
 		mfc_err("inavlid queue type: %d\n", vq->type);
 		return -EINVAL;
@@ -1715,7 +1747,7 @@ static int s5p_mfc_start_streaming(struct vb2_queue *q, unsigned int count)
 	/* If context is ready then dev = work->data;schedule it to run */
 	if (s5p_mfc_ctx_ready(ctx))
 		set_work_bit_irqsave(ctx);
-	s5p_mfc_try_run(dev);
+	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 	return 0;
 }
 
@@ -1729,19 +1761,21 @@ static int s5p_mfc_stop_streaming(struct vb2_queue *q)
 		ctx->state == MFCINST_RUNNING) &&
 		dev->curr_ctx == ctx->num && dev->hw_lock) {
 		ctx->state = MFCINST_ABORT;
-		s5p_mfc_wait_for_done_ctx(ctx, S5P_FIMV_R2H_CMD_FRAME_DONE_RET,
+		s5p_mfc_wait_for_done_ctx(ctx, S5P_MFC_R2H_CMD_FRAME_DONE_RET,
 					  0);
 	}
 	ctx->state = MFCINST_FINISHED;
 	spin_lock_irqsave(&dev->irqlock, flags);
 	if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
-		s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
+		s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->dst_queue,
+				&ctx->vq_dst);
 		INIT_LIST_HEAD(&ctx->dst_queue);
 		ctx->dst_queue_cnt = 0;
 	}
 	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 		cleanup_ref_queue(ctx);
-		s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
+		s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->src_queue,
+				&ctx->vq_src);
 		INIT_LIST_HEAD(&ctx->src_queue);
 		ctx->src_queue_cnt = 0;
 	}
@@ -1782,7 +1816,7 @@ static void s5p_mfc_buf_queue(struct vb2_buffer *vb)
 	}
 	if (s5p_mfc_ctx_ready(ctx))
 		set_work_bit_irqsave(ctx);
-	s5p_mfc_try_run(dev);
+	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 }
 
 static struct vb2_ops s5p_mfc_enc_qops = {
@@ -1880,3 +1914,13 @@ void s5p_mfc_enc_ctrls_delete(struct s5p_mfc_ctx *ctx)
 	for (i = 0; i < NUM_CTRLS; i++)
 		ctx->ctrls[i] = NULL;
 }
+
+void s5p_mfc_enc_init(struct s5p_mfc_ctx *ctx)
+{
+	struct v4l2_format f;
+	f.fmt.pix_mp.pixelformat = DEF_SRC_FMT_ENC;
+	ctx->src_fmt = find_format(&f, MFC_FMT_RAW);
+	f.fmt.pix_mp.pixelformat = DEF_DST_FMT_ENC;
+	ctx->dst_fmt = find_format(&f, MFC_FMT_ENC);
+}
+
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h
index ca9fd66bd31..5118d46b3a9 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h
@@ -19,5 +19,6 @@ const struct v4l2_ioctl_ops *get_enc_v4l2_ioctl_ops(void);
 struct s5p_mfc_fmt *get_enc_def_fmt(bool src);
 int s5p_mfc_enc_ctrls_setup(struct s5p_mfc_ctx *ctx);
 void s5p_mfc_enc_ctrls_delete(struct s5p_mfc_ctx *ctx);
+void s5p_mfc_enc_init(struct s5p_mfc_ctx *ctx);
 
 #endif /* S5P_MFC_ENC_H_  */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_intr.c b/drivers/media/platform/s5p-mfc/s5p_mfc_intr.c
index 37860e29902..5b8f0e085e6 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_intr.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_intr.c
@@ -17,7 +17,6 @@
 #include <linux/io.h>
 #include <linux/sched.h>
 #include <linux/wait.h>
-#include "regs-mfc.h"
 #include "s5p_mfc_common.h"
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_intr.h"
@@ -28,7 +27,7 @@ int s5p_mfc_wait_for_done_dev(struct s5p_mfc_dev *dev, int command)
 
 	ret = wait_event_interruptible_timeout(dev->queue,
 		(dev->int_cond && (dev->int_type == command
-		|| dev->int_type == S5P_FIMV_R2H_CMD_ERR_RET)),
+		|| dev->int_type == S5P_MFC_R2H_CMD_ERR_RET)),
 		msecs_to_jiffies(MFC_INT_TIMEOUT));
 	if (ret == 0) {
 		mfc_err("Interrupt (dev->int_type:%d, command:%d) timed out\n",
@@ -40,7 +39,7 @@ int s5p_mfc_wait_for_done_dev(struct s5p_mfc_dev *dev, int command)
 	}
 	mfc_debug(1, "Finished waiting (dev->int_type:%d, command: %d)\n",
 							dev->int_type, command);
-	if (dev->int_type == S5P_FIMV_R2H_CMD_ERR_RET)
+	if (dev->int_type == S5P_MFC_R2H_CMD_ERR_RET)
 		return 1;
 	return 0;
 }
@@ -60,12 +59,12 @@ int s5p_mfc_wait_for_done_ctx(struct s5p_mfc_ctx *ctx,
 	if (interrupt) {
 		ret = wait_event_interruptible_timeout(ctx->queue,
 				(ctx->int_cond && (ctx->int_type == command
-			|| ctx->int_type == S5P_FIMV_R2H_CMD_ERR_RET)),
+			|| ctx->int_type == S5P_MFC_R2H_CMD_ERR_RET)),
 					msecs_to_jiffies(MFC_INT_TIMEOUT));
 	} else {
 		ret = wait_event_timeout(ctx->queue,
 				(ctx->int_cond && (ctx->int_type == command
-			|| ctx->int_type == S5P_FIMV_R2H_CMD_ERR_RET)),
+			|| ctx->int_type == S5P_MFC_R2H_CMD_ERR_RET)),
 					msecs_to_jiffies(MFC_INT_TIMEOUT));
 	}
 	if (ret == 0) {
@@ -78,7 +77,7 @@ int s5p_mfc_wait_for_done_ctx(struct s5p_mfc_ctx *ctx,
 	}
 	mfc_debug(1, "Finished waiting (ctx->int_type:%d, command: %d)\n",
 							ctx->int_type, command);
-	if (ctx->int_type == S5P_FIMV_R2H_CMD_ERR_RET)
+	if (ctx->int_type == S5P_MFC_R2H_CMD_ERR_RET)
 		return 1;
 	return 0;
 }
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
index 767a51271dc..6932e90d406 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
@@ -1,10 +1,10 @@
 /*
- * drivers/media/platform/samsung/mfc5/s5p_mfc_opr.c
+ * drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
  *
  * Samsung MFC (Multi Function Codec - FIMV) driver
  * This file contains hw related functions.
  *
- * Kamil Debski, Copyright (c) 2011 Samsung Electronics
+ * Kamil Debski, Copyright (c) 2012 Samsung Electronics Co., Ltd.
  * http://www.samsung.com/
  *
  * This program is free software; you can redistribute it and/or modify
@@ -12,1414 +12,20 @@
  * published by the Free Software Foundation.
  */
 
-#include "regs-mfc.h"
-#include "s5p_mfc_cmd.h"
-#include "s5p_mfc_common.h"
-#include "s5p_mfc_ctrl.h"
-#include "s5p_mfc_debug.h"
-#include "s5p_mfc_intr.h"
 #include "s5p_mfc_opr.h"
-#include "s5p_mfc_pm.h"
-#include "s5p_mfc_shm.h"
-#include <asm/cacheflush.h>
-#include <linux/delay.h>
-#include <linux/dma-mapping.h>
-#include <linux/err.h>
-#include <linux/firmware.h>
-#include <linux/io.h>
-#include <linux/jiffies.h>
-#include <linux/mm.h>
-#include <linux/sched.h>
+#include "s5p_mfc_opr_v5.h"
+#include "s5p_mfc_opr_v6.h"
 
-#define OFFSETA(x)		(((x) - dev->bank1) >> MFC_OFFSET_SHIFT)
-#define OFFSETB(x)		(((x) - dev->bank2) >> MFC_OFFSET_SHIFT)
+static struct s5p_mfc_hw_ops *s5p_mfc_ops;
 
-/* Allocate temporary buffers for decoding */
-int s5p_mfc_alloc_dec_temp_buffers(struct s5p_mfc_ctx *ctx)
+void s5p_mfc_init_hw_ops(struct s5p_mfc_dev *dev)
 {
-	void *desc_virt;
-	struct s5p_mfc_dev *dev = ctx->dev;
-
-	ctx->desc_buf = vb2_dma_contig_memops.alloc(
-			dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], DESC_BUF_SIZE);
-	if (IS_ERR_VALUE((int)ctx->desc_buf)) {
-		ctx->desc_buf = NULL;
-		mfc_err("Allocating DESC buffer failed\n");
-		return -ENOMEM;
-	}
-	ctx->desc_phys = s5p_mfc_mem_cookie(
-			dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->desc_buf);
-	BUG_ON(ctx->desc_phys & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
-	desc_virt = vb2_dma_contig_memops.vaddr(ctx->desc_buf);
-	if (desc_virt == NULL) {
-		vb2_dma_contig_memops.put(ctx->desc_buf);
-		ctx->desc_phys = 0;
-		ctx->desc_buf = NULL;
-		mfc_err("Remapping DESC buffer failed\n");
-		return -ENOMEM;
-	}
-	memset(desc_virt, 0, DESC_BUF_SIZE);
-	wmb();
-	return 0;
-}
-
-/* Release temporary buffers for decoding */
-void s5p_mfc_release_dec_desc_buffer(struct s5p_mfc_ctx *ctx)
-{
-	if (ctx->desc_phys) {
-		vb2_dma_contig_memops.put(ctx->desc_buf);
-		ctx->desc_phys = 0;
-		ctx->desc_buf = NULL;
-	}
-}
-
-/* Allocate codec buffers */
-int s5p_mfc_alloc_codec_buffers(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-	unsigned int enc_ref_y_size = 0;
-	unsigned int enc_ref_c_size = 0;
-	unsigned int guard_width, guard_height;
-
-	if (ctx->type == MFCINST_DECODER) {
-		mfc_debug(2, "Luma size:%d Chroma size:%d MV size:%d\n",
-			  ctx->luma_size, ctx->chroma_size, ctx->mv_size);
-		mfc_debug(2, "Totals bufs: %d\n", ctx->total_dpb_count);
-	} else if (ctx->type == MFCINST_ENCODER) {
-		enc_ref_y_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
-			* ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
-		enc_ref_y_size = ALIGN(enc_ref_y_size, S5P_FIMV_NV12MT_SALIGN);
-
-		if (ctx->codec_mode == S5P_FIMV_CODEC_H264_ENC) {
-			enc_ref_c_size = ALIGN(ctx->img_width,
-						S5P_FIMV_NV12MT_HALIGN)
-						* ALIGN(ctx->img_height >> 1,
-						S5P_FIMV_NV12MT_VALIGN);
-			enc_ref_c_size = ALIGN(enc_ref_c_size,
-							S5P_FIMV_NV12MT_SALIGN);
-		} else {
-			guard_width = ALIGN(ctx->img_width + 16,
-							S5P_FIMV_NV12MT_HALIGN);
-			guard_height = ALIGN((ctx->img_height >> 1) + 4,
-							S5P_FIMV_NV12MT_VALIGN);
-			enc_ref_c_size = ALIGN(guard_width * guard_height,
-					       S5P_FIMV_NV12MT_SALIGN);
-		}
-		mfc_debug(2, "recon luma size: %d chroma size: %d\n",
-			  enc_ref_y_size, enc_ref_c_size);
+	if (IS_MFCV6(dev)) {
+		s5p_mfc_ops = s5p_mfc_init_hw_ops_v6();
+		dev->warn_start = S5P_FIMV_ERR_WARNINGS_START_V6;
 	} else {
-		return -EINVAL;
-	}
-	/* Codecs have different memory requirements */
-	switch (ctx->codec_mode) {
-	case S5P_FIMV_CODEC_H264_DEC:
-		ctx->bank1_size =
-		    ALIGN(S5P_FIMV_DEC_NB_IP_SIZE +
-					S5P_FIMV_DEC_VERT_NB_MV_SIZE,
-					S5P_FIMV_DEC_BUF_ALIGN);
-		ctx->bank2_size = ctx->total_dpb_count * ctx->mv_size;
-		break;
-	case S5P_FIMV_CODEC_MPEG4_DEC:
-		ctx->bank1_size =
-		    ALIGN(S5P_FIMV_DEC_NB_DCAC_SIZE +
-				     S5P_FIMV_DEC_UPNB_MV_SIZE +
-				     S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
-				     S5P_FIMV_DEC_STX_PARSER_SIZE +
-				     S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE,
-				     S5P_FIMV_DEC_BUF_ALIGN);
-		ctx->bank2_size = 0;
-		break;
-	case S5P_FIMV_CODEC_VC1RCV_DEC:
-	case S5P_FIMV_CODEC_VC1_DEC:
-		ctx->bank1_size =
-		    ALIGN(S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE +
-			     S5P_FIMV_DEC_UPNB_MV_SIZE +
-			     S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
-			     S5P_FIMV_DEC_NB_DCAC_SIZE +
-			     3 * S5P_FIMV_DEC_VC1_BITPLANE_SIZE,
-			     S5P_FIMV_DEC_BUF_ALIGN);
-		ctx->bank2_size = 0;
-		break;
-	case S5P_FIMV_CODEC_MPEG2_DEC:
-		ctx->bank1_size = 0;
-		ctx->bank2_size = 0;
-		break;
-	case S5P_FIMV_CODEC_H263_DEC:
-		ctx->bank1_size =
-		    ALIGN(S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE +
-			     S5P_FIMV_DEC_UPNB_MV_SIZE +
-			     S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
-			     S5P_FIMV_DEC_NB_DCAC_SIZE,
-			     S5P_FIMV_DEC_BUF_ALIGN);
-		ctx->bank2_size = 0;
-		break;
-	case S5P_FIMV_CODEC_H264_ENC:
-		ctx->bank1_size = (enc_ref_y_size * 2) +
-				   S5P_FIMV_ENC_UPMV_SIZE +
-				   S5P_FIMV_ENC_COLFLG_SIZE +
-				   S5P_FIMV_ENC_INTRAMD_SIZE +
-				   S5P_FIMV_ENC_NBORINFO_SIZE;
-		ctx->bank2_size = (enc_ref_y_size * 2) +
-				   (enc_ref_c_size * 4) +
-				   S5P_FIMV_ENC_INTRAPRED_SIZE;
-		break;
-	case S5P_FIMV_CODEC_MPEG4_ENC:
-		ctx->bank1_size = (enc_ref_y_size * 2) +
-				   S5P_FIMV_ENC_UPMV_SIZE +
-				   S5P_FIMV_ENC_COLFLG_SIZE +
-				   S5P_FIMV_ENC_ACDCCOEF_SIZE;
-		ctx->bank2_size = (enc_ref_y_size * 2) +
-				   (enc_ref_c_size * 4);
-		break;
-	case S5P_FIMV_CODEC_H263_ENC:
-		ctx->bank1_size = (enc_ref_y_size * 2) +
-				   S5P_FIMV_ENC_UPMV_SIZE +
-				   S5P_FIMV_ENC_ACDCCOEF_SIZE;
-		ctx->bank2_size = (enc_ref_y_size * 2) +
-				   (enc_ref_c_size * 4);
-		break;
-	default:
-		break;
-	}
-	/* Allocate only if memory from bank 1 is necessary */
-	if (ctx->bank1_size > 0) {
-		ctx->bank1_buf = vb2_dma_contig_memops.alloc(
-		dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_size);
-		if (IS_ERR(ctx->bank1_buf)) {
-			ctx->bank1_buf = NULL;
-			printk(KERN_ERR
-			       "Buf alloc for decoding failed (port A)\n");
-			return -ENOMEM;
-		}
-		ctx->bank1_phys = s5p_mfc_mem_cookie(
-		dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_buf);
-		BUG_ON(ctx->bank1_phys & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
-	}
-	/* Allocate only if memory from bank 2 is necessary */
-	if (ctx->bank2_size > 0) {
-		ctx->bank2_buf = vb2_dma_contig_memops.alloc(
-		dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], ctx->bank2_size);
-		if (IS_ERR(ctx->bank2_buf)) {
-			ctx->bank2_buf = NULL;
-			mfc_err("Buf alloc for decoding failed (port B)\n");
-			return -ENOMEM;
-		}
-		ctx->bank2_phys = s5p_mfc_mem_cookie(
-		dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], ctx->bank2_buf);
-		BUG_ON(ctx->bank2_phys & ((1 << MFC_BANK2_ALIGN_ORDER) - 1));
-	}
-	return 0;
-}
-
-/* Release buffers allocated for codec */
-void s5p_mfc_release_codec_buffers(struct s5p_mfc_ctx *ctx)
-{
-	if (ctx->bank1_buf) {
-		vb2_dma_contig_memops.put(ctx->bank1_buf);
-		ctx->bank1_buf = NULL;
-		ctx->bank1_phys = 0;
-		ctx->bank1_size = 0;
-	}
-	if (ctx->bank2_buf) {
-		vb2_dma_contig_memops.put(ctx->bank2_buf);
-		ctx->bank2_buf = NULL;
-		ctx->bank2_phys = 0;
-		ctx->bank2_size = 0;
+		s5p_mfc_ops = s5p_mfc_init_hw_ops_v5();
+		dev->warn_start = S5P_FIMV_ERR_WARNINGS_START;
 	}
+	dev->mfc_ops = s5p_mfc_ops;
 }
-
-/* Allocate memory for instance data buffer */
-int s5p_mfc_alloc_instance_buffer(struct s5p_mfc_ctx *ctx)
-{
-	void *context_virt;
-	struct s5p_mfc_dev *dev = ctx->dev;
-
-	if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC ||
-		ctx->codec_mode == S5P_FIMV_CODEC_H264_ENC)
-		ctx->ctx_size = MFC_H264_CTX_BUF_SIZE;
-	else
-		ctx->ctx_size = MFC_CTX_BUF_SIZE;
-	ctx->ctx_buf = vb2_dma_contig_memops.alloc(
-		dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx_size);
-	if (IS_ERR(ctx->ctx_buf)) {
-		mfc_err("Allocating context buffer failed\n");
-		ctx->ctx_phys = 0;
-		ctx->ctx_buf = NULL;
-		return -ENOMEM;
-	}
-	ctx->ctx_phys = s5p_mfc_mem_cookie(
-		dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx_buf);
-	BUG_ON(ctx->ctx_phys & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
-	ctx->ctx_ofs = OFFSETA(ctx->ctx_phys);
-	context_virt = vb2_dma_contig_memops.vaddr(ctx->ctx_buf);
-	if (context_virt == NULL) {
-		mfc_err("Remapping instance buffer failed\n");
-		vb2_dma_contig_memops.put(ctx->ctx_buf);
-		ctx->ctx_phys = 0;
-		ctx->ctx_buf = NULL;
-		return -ENOMEM;
-	}
-	/* Zero content of the allocated memory */
-	memset(context_virt, 0, ctx->ctx_size);
-	wmb();
-	if (s5p_mfc_init_shm(ctx) < 0) {
-		vb2_dma_contig_memops.put(ctx->ctx_buf);
-		ctx->ctx_phys = 0;
-		ctx->ctx_buf = NULL;
-		return -ENOMEM;
-	}
-	return 0;
-}
-
-/* Release instance buffer */
-void s5p_mfc_release_instance_buffer(struct s5p_mfc_ctx *ctx)
-{
-	if (ctx->ctx_buf) {
-		vb2_dma_contig_memops.put(ctx->ctx_buf);
-		ctx->ctx_phys = 0;
-		ctx->ctx_buf = NULL;
-	}
-	if (ctx->shm_alloc) {
-		vb2_dma_contig_memops.put(ctx->shm_alloc);
-		ctx->shm_alloc = NULL;
-		ctx->shm = NULL;
-	}
-}
-
-/* Set registers for decoding temporary buffers */
-void s5p_mfc_set_dec_desc_buffer(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-
-	mfc_write(dev, OFFSETA(ctx->desc_phys), S5P_FIMV_SI_CH0_DESC_ADR);
-	mfc_write(dev, DESC_BUF_SIZE, S5P_FIMV_SI_CH0_DESC_SIZE);
-}
-
-/* Set registers for shared buffer */
-static void s5p_mfc_set_shared_buffer(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-	mfc_write(dev, ctx->shm_ofs, S5P_FIMV_SI_CH0_HOST_WR_ADR);
-}
-
-/* Set registers for decoding stream buffer */
-int s5p_mfc_set_dec_stream_buffer(struct s5p_mfc_ctx *ctx, int buf_addr,
-		  unsigned int start_num_byte, unsigned int buf_size)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-
-	mfc_write(dev, OFFSETA(buf_addr), S5P_FIMV_SI_CH0_SB_ST_ADR);
-	mfc_write(dev, ctx->dec_src_buf_size, S5P_FIMV_SI_CH0_CPB_SIZE);
-	mfc_write(dev, buf_size, S5P_FIMV_SI_CH0_SB_FRM_SIZE);
-	s5p_mfc_write_shm(ctx, start_num_byte, START_BYTE_NUM);
-	return 0;
-}
-
-/* Set decoding frame buffer */
-int s5p_mfc_set_dec_frame_buffer(struct s5p_mfc_ctx *ctx)
-{
-	unsigned int frame_size, i;
-	unsigned int frame_size_ch, frame_size_mv;
-	struct s5p_mfc_dev *dev = ctx->dev;
-	unsigned int dpb;
-	size_t buf_addr1, buf_addr2;
-	int buf_size1, buf_size2;
-
-	buf_addr1 = ctx->bank1_phys;
-	buf_size1 = ctx->bank1_size;
-	buf_addr2 = ctx->bank2_phys;
-	buf_size2 = ctx->bank2_size;
-	dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) &
-						~S5P_FIMV_DPB_COUNT_MASK;
-	mfc_write(dev, ctx->total_dpb_count | dpb,
-						S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
-	s5p_mfc_set_shared_buffer(ctx);
-	switch (ctx->codec_mode) {
-	case S5P_FIMV_CODEC_H264_DEC:
-		mfc_write(dev, OFFSETA(buf_addr1),
-						S5P_FIMV_H264_VERT_NB_MV_ADR);
-		buf_addr1 += S5P_FIMV_DEC_VERT_NB_MV_SIZE;
-		buf_size1 -= S5P_FIMV_DEC_VERT_NB_MV_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H264_NB_IP_ADR);
-		buf_addr1 += S5P_FIMV_DEC_NB_IP_SIZE;
-		buf_size1 -= S5P_FIMV_DEC_NB_IP_SIZE;
-		break;
-	case S5P_FIMV_CODEC_MPEG4_DEC:
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_NB_DCAC_ADR);
-		buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
-		buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_UP_NB_MV_ADR);
-		buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
-		buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_SA_MV_ADR);
-		buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
-		buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_SP_ADR);
-		buf_addr1 += S5P_FIMV_DEC_STX_PARSER_SIZE;
-		buf_size1 -= S5P_FIMV_DEC_STX_PARSER_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_OT_LINE_ADR);
-		buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
-		buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
-		break;
-	case S5P_FIMV_CODEC_H263_DEC:
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_OT_LINE_ADR);
-		buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
-		buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_UP_NB_MV_ADR);
-		buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
-		buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_SA_MV_ADR);
-		buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
-		buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_NB_DCAC_ADR);
-		buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
-		buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
-		break;
-	case S5P_FIMV_CODEC_VC1_DEC:
-	case S5P_FIMV_CODEC_VC1RCV_DEC:
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_NB_DCAC_ADR);
-		buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
-		buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_OT_LINE_ADR);
-		buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
-		buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_UP_NB_MV_ADR);
-		buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
-		buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_SA_MV_ADR);
-		buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
-		buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE3_ADR);
-		buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
-		buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE2_ADR);
-		buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
-		buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE1_ADR);
-		buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
-		buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
-		break;
-	case S5P_FIMV_CODEC_MPEG2_DEC:
-		break;
-	default:
-		mfc_err("Unknown codec for decoding (%x)\n",
-			ctx->codec_mode);
-		return -EINVAL;
-		break;
-	}
-	frame_size = ctx->luma_size;
-	frame_size_ch = ctx->chroma_size;
-	frame_size_mv = ctx->mv_size;
-	mfc_debug(2, "Frm size: %d ch: %d mv: %d\n", frame_size, frame_size_ch,
-								frame_size_mv);
-	for (i = 0; i < ctx->total_dpb_count; i++) {
-		/* Bank2 */
-		mfc_debug(2, "Luma %d: %x\n", i,
-					ctx->dst_bufs[i].cookie.raw.luma);
-		mfc_write(dev, OFFSETB(ctx->dst_bufs[i].cookie.raw.luma),
-						S5P_FIMV_DEC_LUMA_ADR + i * 4);
-		mfc_debug(2, "\tChroma %d: %x\n", i,
-					ctx->dst_bufs[i].cookie.raw.chroma);
-		mfc_write(dev, OFFSETA(ctx->dst_bufs[i].cookie.raw.chroma),
-					       S5P_FIMV_DEC_CHROMA_ADR + i * 4);
-		if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC) {
-			mfc_debug(2, "\tBuf2: %x, size: %d\n",
-							buf_addr2, buf_size2);
-			mfc_write(dev, OFFSETB(buf_addr2),
-						S5P_FIMV_H264_MV_ADR + i * 4);
-			buf_addr2 += frame_size_mv;
-			buf_size2 -= frame_size_mv;
-		}
-	}
-	mfc_debug(2, "Buf1: %u, buf_size1: %d\n", buf_addr1, buf_size1);
-	mfc_debug(2, "Buf 1/2 size after: %d/%d (frames %d)\n",
-			buf_size1,  buf_size2, ctx->total_dpb_count);
-	if (buf_size1 < 0 || buf_size2 < 0) {
-		mfc_debug(2, "Not enough memory has been allocated\n");
-		return -ENOMEM;
-	}
-	s5p_mfc_write_shm(ctx, frame_size, ALLOC_LUMA_DPB_SIZE);
-	s5p_mfc_write_shm(ctx, frame_size_ch, ALLOC_CHROMA_DPB_SIZE);
-	if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC)
-		s5p_mfc_write_shm(ctx, frame_size_mv, ALLOC_MV_SIZE);
-	mfc_write(dev, ((S5P_FIMV_CH_INIT_BUFS & S5P_FIMV_CH_MASK)
-					<< S5P_FIMV_CH_SHIFT) | (ctx->inst_no),
-						S5P_FIMV_SI_CH0_INST_ID);
-	return 0;
-}
-
-/* Set registers for encoding stream buffer */
-int s5p_mfc_set_enc_stream_buffer(struct s5p_mfc_ctx *ctx,
-		unsigned long addr, unsigned int size)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-
-	mfc_write(dev, OFFSETA(addr), S5P_FIMV_ENC_SI_CH0_SB_ADR);
-	mfc_write(dev, size, S5P_FIMV_ENC_SI_CH0_SB_SIZE);
-	return 0;
-}
-
-void s5p_mfc_set_enc_frame_buffer(struct s5p_mfc_ctx *ctx,
-		unsigned long y_addr, unsigned long c_addr)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-
-	mfc_write(dev, OFFSETB(y_addr), S5P_FIMV_ENC_SI_CH0_CUR_Y_ADR);
-	mfc_write(dev, OFFSETB(c_addr), S5P_FIMV_ENC_SI_CH0_CUR_C_ADR);
-}
-
-void s5p_mfc_get_enc_frame_buffer(struct s5p_mfc_ctx *ctx,
-		unsigned long *y_addr, unsigned long *c_addr)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-
-	*y_addr = dev->bank2 + (mfc_read(dev, S5P_FIMV_ENCODED_Y_ADDR)
-							<< MFC_OFFSET_SHIFT);
-	*c_addr = dev->bank2 + (mfc_read(dev, S5P_FIMV_ENCODED_C_ADDR)
-							<< MFC_OFFSET_SHIFT);
-}
-
-/* Set encoding ref & codec buffer */
-int s5p_mfc_set_enc_ref_buffer(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-	size_t buf_addr1, buf_addr2;
-	size_t buf_size1, buf_size2;
-	unsigned int enc_ref_y_size, enc_ref_c_size;
-	unsigned int guard_width, guard_height;
-	int i;
-
-	buf_addr1 = ctx->bank1_phys;
-	buf_size1 = ctx->bank1_size;
-	buf_addr2 = ctx->bank2_phys;
-	buf_size2 = ctx->bank2_size;
-	enc_ref_y_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
-		* ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
-	enc_ref_y_size = ALIGN(enc_ref_y_size, S5P_FIMV_NV12MT_SALIGN);
-	if (ctx->codec_mode == S5P_FIMV_CODEC_H264_ENC) {
-		enc_ref_c_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
-			* ALIGN((ctx->img_height >> 1), S5P_FIMV_NV12MT_VALIGN);
-		enc_ref_c_size = ALIGN(enc_ref_c_size, S5P_FIMV_NV12MT_SALIGN);
-	} else {
-		guard_width = ALIGN(ctx->img_width + 16,
-						S5P_FIMV_NV12MT_HALIGN);
-		guard_height = ALIGN((ctx->img_height >> 1) + 4,
-						S5P_FIMV_NV12MT_VALIGN);
-		enc_ref_c_size = ALIGN(guard_width * guard_height,
-				       S5P_FIMV_NV12MT_SALIGN);
-	}
-	mfc_debug(2, "buf_size1: %d, buf_size2: %d\n", buf_size1, buf_size2);
-	switch (ctx->codec_mode) {
-	case S5P_FIMV_CODEC_H264_ENC:
-		for (i = 0; i < 2; i++) {
-			mfc_write(dev, OFFSETA(buf_addr1),
-				S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
-			buf_addr1 += enc_ref_y_size;
-			buf_size1 -= enc_ref_y_size;
-
-			mfc_write(dev, OFFSETB(buf_addr2),
-				S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
-			buf_addr2 += enc_ref_y_size;
-			buf_size2 -= enc_ref_y_size;
-		}
-		for (i = 0; i < 4; i++) {
-			mfc_write(dev, OFFSETB(buf_addr2),
-				S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
-			buf_addr2 += enc_ref_c_size;
-			buf_size2 -= enc_ref_c_size;
-		}
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H264_UP_MV_ADR);
-		buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
-		buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1),
-					S5P_FIMV_H264_COZERO_FLAG_ADR);
-		buf_addr1 += S5P_FIMV_ENC_COLFLG_SIZE;
-		buf_size1 -= S5P_FIMV_ENC_COLFLG_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1),
-					S5P_FIMV_H264_UP_INTRA_MD_ADR);
-		buf_addr1 += S5P_FIMV_ENC_INTRAMD_SIZE;
-		buf_size1 -= S5P_FIMV_ENC_INTRAMD_SIZE;
-		mfc_write(dev, OFFSETB(buf_addr2),
-					S5P_FIMV_H264_UP_INTRA_PRED_ADR);
-		buf_addr2 += S5P_FIMV_ENC_INTRAPRED_SIZE;
-		buf_size2 -= S5P_FIMV_ENC_INTRAPRED_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1),
-					S5P_FIMV_H264_NBOR_INFO_ADR);
-		buf_addr1 += S5P_FIMV_ENC_NBORINFO_SIZE;
-		buf_size1 -= S5P_FIMV_ENC_NBORINFO_SIZE;
-		mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
-			buf_size1, buf_size2);
-		break;
-	case S5P_FIMV_CODEC_MPEG4_ENC:
-		for (i = 0; i < 2; i++) {
-			mfc_write(dev, OFFSETA(buf_addr1),
-				S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
-			buf_addr1 += enc_ref_y_size;
-			buf_size1 -= enc_ref_y_size;
-			mfc_write(dev, OFFSETB(buf_addr2),
-				S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
-			buf_addr2 += enc_ref_y_size;
-			buf_size2 -= enc_ref_y_size;
-		}
-		for (i = 0; i < 4; i++) {
-			mfc_write(dev, OFFSETB(buf_addr2),
-				S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
-			buf_addr2 += enc_ref_c_size;
-			buf_size2 -= enc_ref_c_size;
-		}
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_UP_MV_ADR);
-		buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
-		buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1),
-						S5P_FIMV_MPEG4_COZERO_FLAG_ADR);
-		buf_addr1 += S5P_FIMV_ENC_COLFLG_SIZE;
-		buf_size1 -= S5P_FIMV_ENC_COLFLG_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1),
-						S5P_FIMV_MPEG4_ACDC_COEF_ADR);
-		buf_addr1 += S5P_FIMV_ENC_ACDCCOEF_SIZE;
-		buf_size1 -= S5P_FIMV_ENC_ACDCCOEF_SIZE;
-		mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
-			buf_size1, buf_size2);
-		break;
-	case S5P_FIMV_CODEC_H263_ENC:
-		for (i = 0; i < 2; i++) {
-			mfc_write(dev, OFFSETA(buf_addr1),
-				S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
-			buf_addr1 += enc_ref_y_size;
-			buf_size1 -= enc_ref_y_size;
-			mfc_write(dev, OFFSETB(buf_addr2),
-				S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
-			buf_addr2 += enc_ref_y_size;
-			buf_size2 -= enc_ref_y_size;
-		}
-		for (i = 0; i < 4; i++) {
-			mfc_write(dev, OFFSETB(buf_addr2),
-				S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
-			buf_addr2 += enc_ref_c_size;
-			buf_size2 -= enc_ref_c_size;
-		}
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_UP_MV_ADR);
-		buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
-		buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
-		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_ACDC_COEF_ADR);
-		buf_addr1 += S5P_FIMV_ENC_ACDCCOEF_SIZE;
-		buf_size1 -= S5P_FIMV_ENC_ACDCCOEF_SIZE;
-		mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
-			buf_size1, buf_size2);
-		break;
-	default:
-		mfc_err("Unknown codec set for encoding: %d\n",
-			ctx->codec_mode);
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static int s5p_mfc_set_enc_params(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-	struct s5p_mfc_enc_params *p = &ctx->enc_params;
-	unsigned int reg;
-	unsigned int shm;
-
-	/* width */
-	mfc_write(dev, ctx->img_width, S5P_FIMV_ENC_HSIZE_PX);
-	/* height */
-	mfc_write(dev, ctx->img_height, S5P_FIMV_ENC_VSIZE_PX);
-	/* pictype : enable, IDR period */
-	reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
-	reg |= (1 << 18);
-	reg &= ~(0xFFFF);
-	reg |= p->gop_size;
-	mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
-	mfc_write(dev, 0, S5P_FIMV_ENC_B_RECON_WRITE_ON);
-	/* multi-slice control */
-	/* multi-slice MB number or bit size */
-	mfc_write(dev, p->slice_mode, S5P_FIMV_ENC_MSLICE_CTRL);
-	if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB) {
-		mfc_write(dev, p->slice_mb, S5P_FIMV_ENC_MSLICE_MB);
-	} else if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES) {
-		mfc_write(dev, p->slice_bit, S5P_FIMV_ENC_MSLICE_BIT);
-	} else {
-		mfc_write(dev, 0, S5P_FIMV_ENC_MSLICE_MB);
-		mfc_write(dev, 0, S5P_FIMV_ENC_MSLICE_BIT);
-	}
-	/* cyclic intra refresh */
-	mfc_write(dev, p->intra_refresh_mb, S5P_FIMV_ENC_CIR_CTRL);
-	/* memory structure cur. frame */
-	if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M)
-		mfc_write(dev, 0, S5P_FIMV_ENC_MAP_FOR_CUR);
-	else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT)
-		mfc_write(dev, 3, S5P_FIMV_ENC_MAP_FOR_CUR);
-	/* padding control & value */
-	reg = mfc_read(dev, S5P_FIMV_ENC_PADDING_CTRL);
-	if (p->pad) {
-		/** enable */
-		reg |= (1 << 31);
-		/** cr value */
-		reg &= ~(0xFF << 16);
-		reg |= (p->pad_cr << 16);
-		/** cb value */
-		reg &= ~(0xFF << 8);
-		reg |= (p->pad_cb << 8);
-		/** y value */
-		reg &= ~(0xFF);
-		reg |= (p->pad_luma);
-	} else {
-		/** disable & all value clear */
-		reg = 0;
-	}
-	mfc_write(dev, reg, S5P_FIMV_ENC_PADDING_CTRL);
-	/* rate control config. */
-	reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
-	/** frame-level rate control */
-	reg &= ~(0x1 << 9);
-	reg |= (p->rc_frame << 9);
-	mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
-	/* bit rate */
-	if (p->rc_frame)
-		mfc_write(dev, p->rc_bitrate,
-			S5P_FIMV_ENC_RC_BIT_RATE);
-	else
-		mfc_write(dev, 0, S5P_FIMV_ENC_RC_BIT_RATE);
-	/* reaction coefficient */
-	if (p->rc_frame)
-		mfc_write(dev, p->rc_reaction_coeff, S5P_FIMV_ENC_RC_RPARA);
-	shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
-	/* seq header ctrl */
-	shm &= ~(0x1 << 3);
-	shm |= (p->seq_hdr_mode << 3);
-	/* frame skip mode */
-	shm &= ~(0x3 << 1);
-	shm |= (p->frame_skip_mode << 1);
-	s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
-	/* fixed target bit */
-	s5p_mfc_write_shm(ctx, p->fixed_target_bit, RC_CONTROL_CONFIG);
-	return 0;
-}
-
-static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-	struct s5p_mfc_enc_params *p = &ctx->enc_params;
-	struct s5p_mfc_h264_enc_params *p_264 = &p->codec.h264;
-	unsigned int reg;
-	unsigned int shm;
-
-	s5p_mfc_set_enc_params(ctx);
-	/* pictype : number of B */
-	reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
-	/* num_b_frame - 0 ~ 2 */
-	reg &= ~(0x3 << 16);
-	reg |= (p->num_b_frame << 16);
-	mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
-	/* profile & level */
-	reg = mfc_read(dev, S5P_FIMV_ENC_PROFILE);
-	/* level */
-	reg &= ~(0xFF << 8);
-	reg |= (p_264->level << 8);
-	/* profile - 0 ~ 2 */
-	reg &= ~(0x3F);
-	reg |= p_264->profile;
-	mfc_write(dev, reg, S5P_FIMV_ENC_PROFILE);
-	/* interlace  */
-	mfc_write(dev, p->interlace, S5P_FIMV_ENC_PIC_STRUCT);
-	/* height */
-	if (p->interlace)
-		mfc_write(dev, ctx->img_height >> 1, S5P_FIMV_ENC_VSIZE_PX);
-	/* loopfilter ctrl */
-	mfc_write(dev, p_264->loop_filter_mode, S5P_FIMV_ENC_LF_CTRL);
-	/* loopfilter alpha offset */
-	if (p_264->loop_filter_alpha < 0) {
-		reg = 0x10;
-		reg |= (0xFF - p_264->loop_filter_alpha) + 1;
-	} else {
-		reg = 0x00;
-		reg |= (p_264->loop_filter_alpha & 0xF);
-	}
-	mfc_write(dev, reg, S5P_FIMV_ENC_ALPHA_OFF);
-	/* loopfilter beta offset */
-	if (p_264->loop_filter_beta < 0) {
-		reg = 0x10;
-		reg |= (0xFF - p_264->loop_filter_beta) + 1;
-	} else {
-		reg = 0x00;
-		reg |= (p_264->loop_filter_beta & 0xF);
-	}
-	mfc_write(dev, reg, S5P_FIMV_ENC_BETA_OFF);
-	/* entropy coding mode */
-	if (p_264->entropy_mode == V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC)
-		mfc_write(dev, 1, S5P_FIMV_ENC_H264_ENTROPY_MODE);
-	else
-		mfc_write(dev, 0, S5P_FIMV_ENC_H264_ENTROPY_MODE);
-	/* number of ref. picture */
-	reg = mfc_read(dev, S5P_FIMV_ENC_H264_NUM_OF_REF);
-	/* num of ref. pictures of P */
-	reg &= ~(0x3 << 5);
-	reg |= (p_264->num_ref_pic_4p << 5);
-	/* max number of ref. pictures */
-	reg &= ~(0x1F);
-	reg |= p_264->max_ref_pic;
-	mfc_write(dev, reg, S5P_FIMV_ENC_H264_NUM_OF_REF);
-	/* 8x8 transform enable */
-	mfc_write(dev, p_264->_8x8_transform, S5P_FIMV_ENC_H264_TRANS_FLAG);
-	/* rate control config. */
-	reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
-	/* macroblock level rate control */
-	reg &= ~(0x1 << 8);
-	reg |= (p_264->rc_mb << 8);
-	/* frame QP */
-	reg &= ~(0x3F);
-	reg |= p_264->rc_frame_qp;
-	mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
-	/* frame rate */
-	if (p->rc_frame && p->rc_framerate_denom)
-		mfc_write(dev, p->rc_framerate_num * 1000
-			/ p->rc_framerate_denom, S5P_FIMV_ENC_RC_FRAME_RATE);
-	else
-		mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
-	/* max & min value of QP */
-	reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
-	/* max QP */
-	reg &= ~(0x3F << 8);
-	reg |= (p_264->rc_max_qp << 8);
-	/* min QP */
-	reg &= ~(0x3F);
-	reg |= p_264->rc_min_qp;
-	mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
-	/* macroblock adaptive scaling features */
-	if (p_264->rc_mb) {
-		reg = mfc_read(dev, S5P_FIMV_ENC_RC_MB_CTRL);
-		/* dark region */
-		reg &= ~(0x1 << 3);
-		reg |= (p_264->rc_mb_dark << 3);
-		/* smooth region */
-		reg &= ~(0x1 << 2);
-		reg |= (p_264->rc_mb_smooth << 2);
-		/* static region */
-		reg &= ~(0x1 << 1);
-		reg |= (p_264->rc_mb_static << 1);
-		/* high activity region */
-		reg &= ~(0x1);
-		reg |= p_264->rc_mb_activity;
-		mfc_write(dev, reg, S5P_FIMV_ENC_RC_MB_CTRL);
-	}
-	if (!p->rc_frame &&
-	    !p_264->rc_mb) {
-		shm = s5p_mfc_read_shm(ctx, P_B_FRAME_QP);
-		shm &= ~(0xFFF);
-		shm |= ((p_264->rc_b_frame_qp & 0x3F) << 6);
-		shm |= (p_264->rc_p_frame_qp & 0x3F);
-		s5p_mfc_write_shm(ctx, shm, P_B_FRAME_QP);
-	}
-	/* extended encoder ctrl */
-	shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
-	/* AR VUI control */
-	shm &= ~(0x1 << 15);
-	shm |= (p_264->vui_sar << 1);
-	s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
-	if (p_264->vui_sar) {
-		/* aspect ration IDC */
-		shm = s5p_mfc_read_shm(ctx, SAMPLE_ASPECT_RATIO_IDC);
-		shm &= ~(0xFF);
-		shm |= p_264->vui_sar_idc;
-		s5p_mfc_write_shm(ctx, shm, SAMPLE_ASPECT_RATIO_IDC);
-		if (p_264->vui_sar_idc == 0xFF) {
-			/* sample  AR info */
-			shm = s5p_mfc_read_shm(ctx, EXTENDED_SAR);
-			shm &= ~(0xFFFFFFFF);
-			shm |= p_264->vui_ext_sar_width << 16;
-			shm |= p_264->vui_ext_sar_height;
-			s5p_mfc_write_shm(ctx, shm, EXTENDED_SAR);
-		}
-	}
-	/* intra picture period for H.264 */
-	shm = s5p_mfc_read_shm(ctx, H264_I_PERIOD);
-	/* control */
-	shm &= ~(0x1 << 16);
-	shm |= (p_264->open_gop << 16);
-	/* value */
-	if (p_264->open_gop) {
-		shm &= ~(0xFFFF);
-		shm |= p_264->open_gop_size;
-	}
-	s5p_mfc_write_shm(ctx, shm, H264_I_PERIOD);
-	/* extended encoder ctrl */
-	shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
-	/* vbv buffer size */
-	if (p->frame_skip_mode ==
-			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
-		shm &= ~(0xFFFF << 16);
-		shm |= (p_264->cpb_size << 16);
-	}
-	s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
-	return 0;
-}
-
-static int s5p_mfc_set_enc_params_mpeg4(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-	struct s5p_mfc_enc_params *p = &ctx->enc_params;
-	struct s5p_mfc_mpeg4_enc_params *p_mpeg4 = &p->codec.mpeg4;
-	unsigned int reg;
-	unsigned int shm;
-	unsigned int framerate;
-
-	s5p_mfc_set_enc_params(ctx);
-	/* pictype : number of B */
-	reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
-	/* num_b_frame - 0 ~ 2 */
-	reg &= ~(0x3 << 16);
-	reg |= (p->num_b_frame << 16);
-	mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
-	/* profile & level */
-	reg = mfc_read(dev, S5P_FIMV_ENC_PROFILE);
-	/* level */
-	reg &= ~(0xFF << 8);
-	reg |= (p_mpeg4->level << 8);
-	/* profile - 0 ~ 2 */
-	reg &= ~(0x3F);
-	reg |= p_mpeg4->profile;
-	mfc_write(dev, reg, S5P_FIMV_ENC_PROFILE);
-	/* quarter_pixel */
-	mfc_write(dev, p_mpeg4->quarter_pixel, S5P_FIMV_ENC_MPEG4_QUART_PXL);
-	/* qp */
-	if (!p->rc_frame) {
-		shm = s5p_mfc_read_shm(ctx, P_B_FRAME_QP);
-		shm &= ~(0xFFF);
-		shm |= ((p_mpeg4->rc_b_frame_qp & 0x3F) << 6);
-		shm |= (p_mpeg4->rc_p_frame_qp & 0x3F);
-		s5p_mfc_write_shm(ctx, shm, P_B_FRAME_QP);
-	}
-	/* frame rate */
-	if (p->rc_frame) {
-		if (p->rc_framerate_denom > 0) {
-			framerate = p->rc_framerate_num * 1000 /
-						p->rc_framerate_denom;
-			mfc_write(dev, framerate,
-				S5P_FIMV_ENC_RC_FRAME_RATE);
-			shm = s5p_mfc_read_shm(ctx, RC_VOP_TIMING);
-			shm &= ~(0xFFFFFFFF);
-			shm |= (1 << 31);
-			shm |= ((p->rc_framerate_num & 0x7FFF) << 16);
-			shm |= (p->rc_framerate_denom & 0xFFFF);
-			s5p_mfc_write_shm(ctx, shm, RC_VOP_TIMING);
-		}
-	} else {
-		mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
-	}
-	/* rate control config. */
-	reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
-	/* frame QP */
-	reg &= ~(0x3F);
-	reg |= p_mpeg4->rc_frame_qp;
-	mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
-	/* max & min value of QP */
-	reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
-	/* max QP */
-	reg &= ~(0x3F << 8);
-	reg |= (p_mpeg4->rc_max_qp << 8);
-	/* min QP */
-	reg &= ~(0x3F);
-	reg |= p_mpeg4->rc_min_qp;
-	mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
-	/* extended encoder ctrl */
-	shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
-	/* vbv buffer size */
-	if (p->frame_skip_mode ==
-			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
-		shm &= ~(0xFFFF << 16);
-		shm |= (p->vbv_size << 16);
-	}
-	s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
-	return 0;
-}
-
-static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-	struct s5p_mfc_enc_params *p = &ctx->enc_params;
-	struct s5p_mfc_mpeg4_enc_params *p_h263 = &p->codec.mpeg4;
-	unsigned int reg;
-	unsigned int shm;
-
-	s5p_mfc_set_enc_params(ctx);
-	/* qp */
-	if (!p->rc_frame) {
-		shm = s5p_mfc_read_shm(ctx, P_B_FRAME_QP);
-		shm &= ~(0xFFF);
-		shm |= (p_h263->rc_p_frame_qp & 0x3F);
-		s5p_mfc_write_shm(ctx, shm, P_B_FRAME_QP);
-	}
-	/* frame rate */
-	if (p->rc_frame && p->rc_framerate_denom)
-		mfc_write(dev, p->rc_framerate_num * 1000
-			/ p->rc_framerate_denom, S5P_FIMV_ENC_RC_FRAME_RATE);
-	else
-		mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
-	/* rate control config. */
-	reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
-	/* frame QP */
-	reg &= ~(0x3F);
-	reg |= p_h263->rc_frame_qp;
-	mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
-	/* max & min value of QP */
-	reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
-	/* max QP */
-	reg &= ~(0x3F << 8);
-	reg |= (p_h263->rc_max_qp << 8);
-	/* min QP */
-	reg &= ~(0x3F);
-	reg |= p_h263->rc_min_qp;
-	mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
-	/* extended encoder ctrl */
-	shm = s5p_mfc_read_shm(ctx, EXT_ENC_CONTROL);
-	/* vbv buffer size */
-	if (p->frame_skip_mode ==
-			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
-		shm &= ~(0xFFFF << 16);
-		shm |= (p->vbv_size << 16);
-	}
-	s5p_mfc_write_shm(ctx, shm, EXT_ENC_CONTROL);
-	return 0;
-}
-
-/* Initialize decoding */
-int s5p_mfc_init_decode(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-
-	s5p_mfc_set_shared_buffer(ctx);
-	/* Setup loop filter, for decoding this is only valid for MPEG4 */
-	if (ctx->codec_mode == S5P_FIMV_CODEC_MPEG4_DEC)
-		mfc_write(dev, ctx->loop_filter_mpeg4, S5P_FIMV_ENC_LF_CTRL);
-	else
-		mfc_write(dev, 0, S5P_FIMV_ENC_LF_CTRL);
-	mfc_write(dev, ((ctx->slice_interface & S5P_FIMV_SLICE_INT_MASK) <<
-		S5P_FIMV_SLICE_INT_SHIFT) | (ctx->display_delay_enable <<
-		S5P_FIMV_DDELAY_ENA_SHIFT) | ((ctx->display_delay &
-		S5P_FIMV_DDELAY_VAL_MASK) << S5P_FIMV_DDELAY_VAL_SHIFT),
-		S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
-	mfc_write(dev,
-	((S5P_FIMV_CH_SEQ_HEADER & S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT)
-				| (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
-	return 0;
-}
-
-static void s5p_mfc_set_flush(struct s5p_mfc_ctx *ctx, int flush)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-	unsigned int dpb;
-
-	if (flush)
-		dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) | (
-			S5P_FIMV_DPB_FLUSH_MASK << S5P_FIMV_DPB_FLUSH_SHIFT);
-	else
-		dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) &
-			~(S5P_FIMV_DPB_FLUSH_MASK << S5P_FIMV_DPB_FLUSH_SHIFT);
-	mfc_write(dev, dpb, S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
-}
-
-/* Decode a single frame */
-int s5p_mfc_decode_one_frame(struct s5p_mfc_ctx *ctx,
-					enum s5p_mfc_decode_arg last_frame)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-
-	mfc_write(dev, ctx->dec_dst_flag, S5P_FIMV_SI_CH0_RELEASE_BUF);
-	s5p_mfc_set_shared_buffer(ctx);
-	s5p_mfc_set_flush(ctx, ctx->dpb_flush_flag);
-	/* Issue different commands to instance basing on whether it
-	 * is the last frame or not. */
-	switch (last_frame) {
-	case MFC_DEC_FRAME:
-		mfc_write(dev, ((S5P_FIMV_CH_FRAME_START & S5P_FIMV_CH_MASK) <<
-		S5P_FIMV_CH_SHIFT) | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
-		break;
-	case MFC_DEC_LAST_FRAME:
-		mfc_write(dev, ((S5P_FIMV_CH_LAST_FRAME & S5P_FIMV_CH_MASK) <<
-		S5P_FIMV_CH_SHIFT) | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
-		break;
-	case MFC_DEC_RES_CHANGE:
-		mfc_write(dev, ((S5P_FIMV_CH_FRAME_START_REALLOC &
-		S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT) | (ctx->inst_no),
-		S5P_FIMV_SI_CH0_INST_ID);
-		break;
-	}
-	mfc_debug(2, "Decoding a usual frame\n");
-	return 0;
-}
-
-int s5p_mfc_init_encode(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-
-	if (ctx->codec_mode == S5P_FIMV_CODEC_H264_ENC)
-		s5p_mfc_set_enc_params_h264(ctx);
-	else if (ctx->codec_mode == S5P_FIMV_CODEC_MPEG4_ENC)
-		s5p_mfc_set_enc_params_mpeg4(ctx);
-	else if (ctx->codec_mode == S5P_FIMV_CODEC_H263_ENC)
-		s5p_mfc_set_enc_params_h263(ctx);
-	else {
-		mfc_err("Unknown codec for encoding (%x)\n",
-			ctx->codec_mode);
-		return -EINVAL;
-	}
-	s5p_mfc_set_shared_buffer(ctx);
-	mfc_write(dev, ((S5P_FIMV_CH_SEQ_HEADER << 16) & 0x70000) |
-		(ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
-	return 0;
-}
-
-/* Encode a single frame */
-int s5p_mfc_encode_one_frame(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-	int cmd;
-	/* memory structure cur. frame */
-	if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M)
-		mfc_write(dev, 0, S5P_FIMV_ENC_MAP_FOR_CUR);
-	else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT)
-		mfc_write(dev, 3, S5P_FIMV_ENC_MAP_FOR_CUR);
-	s5p_mfc_set_shared_buffer(ctx);
-
-	if (ctx->state == MFCINST_FINISHING)
-		cmd = S5P_FIMV_CH_LAST_FRAME;
-	else
-		cmd = S5P_FIMV_CH_FRAME_START;
-	mfc_write(dev, ((cmd & S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT)
-				| (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
-
-	return 0;
-}
-
-static int s5p_mfc_get_new_ctx(struct s5p_mfc_dev *dev)
-{
-	unsigned long flags;
-	int new_ctx;
-	int cnt;
-
-	spin_lock_irqsave(&dev->condlock, flags);
-	new_ctx = (dev->curr_ctx + 1) % MFC_NUM_CONTEXTS;
-	cnt = 0;
-	while (!test_bit(new_ctx, &dev->ctx_work_bits)) {
-		new_ctx = (new_ctx + 1) % MFC_NUM_CONTEXTS;
-		if (++cnt > MFC_NUM_CONTEXTS) {
-			/* No contexts to run */
-			spin_unlock_irqrestore(&dev->condlock, flags);
-			return -EAGAIN;
-		}
-	}
-	spin_unlock_irqrestore(&dev->condlock, flags);
-	return new_ctx;
-}
-
-static void s5p_mfc_run_res_change(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-
-	s5p_mfc_set_dec_stream_buffer(ctx, 0, 0, 0);
-	dev->curr_ctx = ctx->num;
-	s5p_mfc_clean_ctx_int_flags(ctx);
-	s5p_mfc_decode_one_frame(ctx, MFC_DEC_RES_CHANGE);
-}
-
-static int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx, int last_frame)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-	struct s5p_mfc_buf *temp_vb;
-	unsigned long flags;
-	unsigned int index;
-
-	spin_lock_irqsave(&dev->irqlock, flags);
-	/* Frames are being decoded */
-	if (list_empty(&ctx->src_queue)) {
-		mfc_debug(2, "No src buffers\n");
-		spin_unlock_irqrestore(&dev->irqlock, flags);
-		return -EAGAIN;
-	}
-	/* Get the next source buffer */
-	temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
-	temp_vb->flags |= MFC_BUF_FLAG_USED;
-	s5p_mfc_set_dec_stream_buffer(ctx,
-		vb2_dma_contig_plane_dma_addr(temp_vb->b, 0), ctx->consumed_stream,
-					temp_vb->b->v4l2_planes[0].bytesused);
-	spin_unlock_irqrestore(&dev->irqlock, flags);
-	index = temp_vb->b->v4l2_buf.index;
-	dev->curr_ctx = ctx->num;
-	s5p_mfc_clean_ctx_int_flags(ctx);
-	if (temp_vb->b->v4l2_planes[0].bytesused == 0) {
-		last_frame = MFC_DEC_LAST_FRAME;
-		mfc_debug(2, "Setting ctx->state to FINISHING\n");
-		ctx->state = MFCINST_FINISHING;
-	}
-	s5p_mfc_decode_one_frame(ctx, last_frame);
-	return 0;
-}
-
-static int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-	unsigned long flags;
-	struct s5p_mfc_buf *dst_mb;
-	struct s5p_mfc_buf *src_mb;
-	unsigned long src_y_addr, src_c_addr, dst_addr;
-	unsigned int dst_size;
-
-	spin_lock_irqsave(&dev->irqlock, flags);
-	if (list_empty(&ctx->src_queue) && ctx->state != MFCINST_FINISHING) {
-		mfc_debug(2, "no src buffers\n");
-		spin_unlock_irqrestore(&dev->irqlock, flags);
-		return -EAGAIN;
-	}
-	if (list_empty(&ctx->dst_queue)) {
-		mfc_debug(2, "no dst buffers\n");
-		spin_unlock_irqrestore(&dev->irqlock, flags);
-		return -EAGAIN;
-	}
-	if (list_empty(&ctx->src_queue)) {
-		/* send null frame */
-		s5p_mfc_set_enc_frame_buffer(ctx, dev->bank2, dev->bank2);
-		src_mb = NULL;
-	} else {
-		src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf,
-									list);
-		src_mb->flags |= MFC_BUF_FLAG_USED;
-		if (src_mb->b->v4l2_planes[0].bytesused == 0) {
-			/* send null frame */
-			s5p_mfc_set_enc_frame_buffer(ctx, dev->bank2,
-								dev->bank2);
-			ctx->state = MFCINST_FINISHING;
-		} else {
-			src_y_addr = vb2_dma_contig_plane_dma_addr(src_mb->b,
-									0);
-			src_c_addr = vb2_dma_contig_plane_dma_addr(src_mb->b,
-									1);
-			s5p_mfc_set_enc_frame_buffer(ctx, src_y_addr,
-								src_c_addr);
-			if (src_mb->flags & MFC_BUF_FLAG_EOS)
-				ctx->state = MFCINST_FINISHING;
-		}
-	}
-	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
-	dst_mb->flags |= MFC_BUF_FLAG_USED;
-	dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
-	dst_size = vb2_plane_size(dst_mb->b, 0);
-	s5p_mfc_set_enc_stream_buffer(ctx, dst_addr, dst_size);
-	spin_unlock_irqrestore(&dev->irqlock, flags);
-	dev->curr_ctx = ctx->num;
-	s5p_mfc_clean_ctx_int_flags(ctx);
-	mfc_debug(2, "encoding buffer with index=%d state=%d",
-			src_mb ? src_mb->b->v4l2_buf.index : -1, ctx->state);
-	s5p_mfc_encode_one_frame(ctx);
-	return 0;
-}
-
-static void s5p_mfc_run_init_dec(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-	unsigned long flags;
-	struct s5p_mfc_buf *temp_vb;
-
-	/* Initializing decoding - parsing header */
-	spin_lock_irqsave(&dev->irqlock, flags);
-	mfc_debug(2, "Preparing to init decoding\n");
-	temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
-	s5p_mfc_set_dec_desc_buffer(ctx);
-	mfc_debug(2, "Header size: %d\n", temp_vb->b->v4l2_planes[0].bytesused);
-	s5p_mfc_set_dec_stream_buffer(ctx,
-				vb2_dma_contig_plane_dma_addr(temp_vb->b, 0),
-				0, temp_vb->b->v4l2_planes[0].bytesused);
-	spin_unlock_irqrestore(&dev->irqlock, flags);
-	dev->curr_ctx = ctx->num;
-	s5p_mfc_clean_ctx_int_flags(ctx);
-	s5p_mfc_init_decode(ctx);
-}
-
-static void s5p_mfc_run_init_enc(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-	unsigned long flags;
-	struct s5p_mfc_buf *dst_mb;
-	unsigned long dst_addr;
-	unsigned int dst_size;
-
-	s5p_mfc_set_enc_ref_buffer(ctx);
-	spin_lock_irqsave(&dev->irqlock, flags);
-	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
-	dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
-	dst_size = vb2_plane_size(dst_mb->b, 0);
-	s5p_mfc_set_enc_stream_buffer(ctx, dst_addr, dst_size);
-	spin_unlock_irqrestore(&dev->irqlock, flags);
-	dev->curr_ctx = ctx->num;
-	s5p_mfc_clean_ctx_int_flags(ctx);
-	s5p_mfc_init_encode(ctx);
-}
-
-static int s5p_mfc_run_init_dec_buffers(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-	unsigned long flags;
-	struct s5p_mfc_buf *temp_vb;
-	int ret;
-
-	/*
-	 * Header was parsed now starting processing
-	 * First set the output frame buffers
-	 */
-	if (ctx->capture_state != QUEUE_BUFS_MMAPED) {
-		mfc_err("It seems that not all destionation buffers were "
-			"mmaped\nMFC requires that all destination are mmaped "
-			"before starting processing\n");
-		return -EAGAIN;
-	}
-	spin_lock_irqsave(&dev->irqlock, flags);
-	if (list_empty(&ctx->src_queue)) {
-		mfc_err("Header has been deallocated in the middle of"
-			" initialization\n");
-		spin_unlock_irqrestore(&dev->irqlock, flags);
-		return -EIO;
-	}
-	temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
-	mfc_debug(2, "Header size: %d\n", temp_vb->b->v4l2_planes[0].bytesused);
-	s5p_mfc_set_dec_stream_buffer(ctx,
-				vb2_dma_contig_plane_dma_addr(temp_vb->b, 0),
-				0, temp_vb->b->v4l2_planes[0].bytesused);
-	spin_unlock_irqrestore(&dev->irqlock, flags);
-	dev->curr_ctx = ctx->num;
-	s5p_mfc_clean_ctx_int_flags(ctx);
-	ret = s5p_mfc_set_dec_frame_buffer(ctx);
-	if (ret) {
-		mfc_err("Failed to alloc frame mem\n");
-		ctx->state = MFCINST_ERROR;
-	}
-	return ret;
-}
-
-/* Try running an operation on hardware */
-void s5p_mfc_try_run(struct s5p_mfc_dev *dev)
-{
-	struct s5p_mfc_ctx *ctx;
-	int new_ctx;
-	unsigned int ret = 0;
-
-	if (test_bit(0, &dev->enter_suspend)) {
-		mfc_debug(1, "Entering suspend so do not schedule any jobs\n");
-		return;
-	}
-	/* Check whether hardware is not running */
-	if (test_and_set_bit(0, &dev->hw_lock) != 0) {
-		/* This is perfectly ok, the scheduled ctx should wait */
-		mfc_debug(1, "Couldn't lock HW\n");
-		return;
-	}
-	/* Choose the context to run */
-	new_ctx = s5p_mfc_get_new_ctx(dev);
-	if (new_ctx < 0) {
-		/* No contexts to run */
-		if (test_and_clear_bit(0, &dev->hw_lock) == 0) {
-			mfc_err("Failed to unlock hardware\n");
-			return;
-		}
-		mfc_debug(1, "No ctx is scheduled to be run\n");
-		return;
-	}
-	ctx = dev->ctx[new_ctx];
-	/* Got context to run in ctx */
-	/*
-	 * Last frame has already been sent to MFC.
-	 * Now obtaining frames from MFC buffer
-	 */
-	s5p_mfc_clock_on();
-	if (ctx->type == MFCINST_DECODER) {
-		s5p_mfc_set_dec_desc_buffer(ctx);
-		switch (ctx->state) {
-		case MFCINST_FINISHING:
-			s5p_mfc_run_dec_frame(ctx, MFC_DEC_LAST_FRAME);
-			break;
-		case MFCINST_RUNNING:
-			ret = s5p_mfc_run_dec_frame(ctx, MFC_DEC_FRAME);
-			break;
-		case MFCINST_INIT:
-			s5p_mfc_clean_ctx_int_flags(ctx);
-			ret = s5p_mfc_open_inst_cmd(ctx);
-			break;
-		case MFCINST_RETURN_INST:
-			s5p_mfc_clean_ctx_int_flags(ctx);
-			ret = s5p_mfc_close_inst_cmd(ctx);
-			break;
-		case MFCINST_GOT_INST:
-			s5p_mfc_run_init_dec(ctx);
-			break;
-		case MFCINST_HEAD_PARSED:
-			ret = s5p_mfc_run_init_dec_buffers(ctx);
-			mfc_debug(1, "head parsed\n");
-			break;
-		case MFCINST_RES_CHANGE_INIT:
-			s5p_mfc_run_res_change(ctx);
-			break;
-		case MFCINST_RES_CHANGE_FLUSH:
-			s5p_mfc_run_dec_frame(ctx, MFC_DEC_FRAME);
-			break;
-		case MFCINST_RES_CHANGE_END:
-			mfc_debug(2, "Finished remaining frames after resolution change\n");
-			ctx->capture_state = QUEUE_FREE;
-			mfc_debug(2, "Will re-init the codec\n");
-			s5p_mfc_run_init_dec(ctx);
-			break;
-		default:
-			ret = -EAGAIN;
-		}
-	} else if (ctx->type == MFCINST_ENCODER) {
-		switch (ctx->state) {
-		case MFCINST_FINISHING:
-		case MFCINST_RUNNING:
-			ret = s5p_mfc_run_enc_frame(ctx);
-			break;
-		case MFCINST_INIT:
-			s5p_mfc_clean_ctx_int_flags(ctx);
-			ret = s5p_mfc_open_inst_cmd(ctx);
-			break;
-		case MFCINST_RETURN_INST:
-			s5p_mfc_clean_ctx_int_flags(ctx);
-			ret = s5p_mfc_close_inst_cmd(ctx);
-			break;
-		case MFCINST_GOT_INST:
-			s5p_mfc_run_init_enc(ctx);
-			break;
-		default:
-			ret = -EAGAIN;
-		}
-	} else {
-		mfc_err("Invalid context type: %d\n", ctx->type);
-		ret = -EAGAIN;
-	}
-
-	if (ret) {
-		/* Free hardware lock */
-		if (test_and_clear_bit(0, &dev->hw_lock) == 0)
-			mfc_err("Failed to unlock hardware\n");
-
-		/* This is in deed imporant, as no operation has been
-		 * scheduled, reduce the clock count as no one will
-		 * ever do this, because no interrupt related to this try_run
-		 * will ever come from hardware. */
-		s5p_mfc_clock_off();
-	}
-}
-
-
-void s5p_mfc_cleanup_queue(struct list_head *lh, struct vb2_queue *vq)
-{
-	struct s5p_mfc_buf *b;
-	int i;
-
-	while (!list_empty(lh)) {
-		b = list_entry(lh->next, struct s5p_mfc_buf, list);
-		for (i = 0; i < b->b->num_planes; i++)
-			vb2_set_plane_payload(b->b, i, 0);
-		vb2_buffer_done(b->b, VB2_BUF_STATE_ERROR);
-		list_del(&b->list);
-	}
-}
-
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
index 2ad3def052f..420abecafec 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
@@ -1,10 +1,10 @@
 /*
- * drivers/media/platform/samsung/mfc5/s5p_mfc_opr.h
+ * drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
  *
  * Header file for Samsung MFC (Multi Function Codec - FIMV) driver
  * Contains declarations of hw related functions.
  *
- * Kamil Debski, Copyright (C) 2011 Samsung Electronics
+ * Kamil Debski, Copyright (C) 2012 Samsung Electronics Co., Ltd.
  * http://www.samsung.com/
  *
  * This program is free software; you can redistribute it and/or modify
@@ -17,77 +17,68 @@
 
 #include "s5p_mfc_common.h"
 
-int s5p_mfc_init_decode(struct s5p_mfc_ctx *ctx);
-int s5p_mfc_init_encode(struct s5p_mfc_ctx *mfc_ctx);
+struct s5p_mfc_hw_ops {
+	int (*alloc_dec_temp_buffers)(struct s5p_mfc_ctx *ctx);
+	void (*release_dec_desc_buffer)(struct s5p_mfc_ctx *ctx);
+	int (*alloc_codec_buffers)(struct s5p_mfc_ctx *ctx);
+	void (*release_codec_buffers)(struct s5p_mfc_ctx *ctx);
+	int (*alloc_instance_buffer)(struct s5p_mfc_ctx *ctx);
+	void (*release_instance_buffer)(struct s5p_mfc_ctx *ctx);
+	int (*alloc_dev_context_buffer)(struct s5p_mfc_dev *dev);
+	void (*release_dev_context_buffer)(struct s5p_mfc_dev *dev);
+	void (*dec_calc_dpb_size)(struct s5p_mfc_ctx *ctx);
+	void (*enc_calc_src_size)(struct s5p_mfc_ctx *ctx);
+	int (*set_dec_stream_buffer)(struct s5p_mfc_ctx *ctx,
+			int buf_addr, unsigned int start_num_byte,
+			unsigned int buf_size);
+	int (*set_dec_frame_buffer)(struct s5p_mfc_ctx *ctx);
+	int (*set_enc_stream_buffer)(struct s5p_mfc_ctx *ctx,
+			unsigned long addr, unsigned int size);
+	void (*set_enc_frame_buffer)(struct s5p_mfc_ctx *ctx,
+			unsigned long y_addr, unsigned long c_addr);
+	void (*get_enc_frame_buffer)(struct s5p_mfc_ctx *ctx,
+			unsigned long *y_addr, unsigned long *c_addr);
+	int (*set_enc_ref_buffer)(struct s5p_mfc_ctx *ctx);
+	int (*init_decode)(struct s5p_mfc_ctx *ctx);
+	int (*init_encode)(struct s5p_mfc_ctx *ctx);
+	int (*encode_one_frame)(struct s5p_mfc_ctx *ctx);
+	void (*try_run)(struct s5p_mfc_dev *dev);
+	void (*cleanup_queue)(struct list_head *lh,
+			struct vb2_queue *vq);
+	void (*clear_int_flags)(struct s5p_mfc_dev *dev);
+	void (*write_info)(struct s5p_mfc_ctx *ctx, unsigned int data,
+			unsigned int ofs);
+	unsigned int (*read_info)(struct s5p_mfc_ctx *ctx,
+			unsigned int ofs);
+	int (*get_dspl_y_adr)(struct s5p_mfc_dev *dev);
+	int (*get_dec_y_adr)(struct s5p_mfc_dev *dev);
+	int (*get_dspl_status)(struct s5p_mfc_dev *dev);
+	int (*get_dec_status)(struct s5p_mfc_dev *dev);
+	int (*get_dec_frame_type)(struct s5p_mfc_dev *dev);
+	int (*get_disp_frame_type)(struct s5p_mfc_ctx *ctx);
+	int (*get_consumed_stream)(struct s5p_mfc_dev *dev);
+	int (*get_int_reason)(struct s5p_mfc_dev *dev);
+	int (*get_int_err)(struct s5p_mfc_dev *dev);
+	int (*err_dec)(unsigned int err);
+	int (*err_dspl)(unsigned int err);
+	int (*get_img_width)(struct s5p_mfc_dev *dev);
+	int (*get_img_height)(struct s5p_mfc_dev *dev);
+	int (*get_dpb_count)(struct s5p_mfc_dev *dev);
+	int (*get_mv_count)(struct s5p_mfc_dev *dev);
+	int (*get_inst_no)(struct s5p_mfc_dev *dev);
+	int (*get_enc_strm_size)(struct s5p_mfc_dev *dev);
+	int (*get_enc_slice_type)(struct s5p_mfc_dev *dev);
+	int (*get_enc_dpb_count)(struct s5p_mfc_dev *dev);
+	int (*get_enc_pic_count)(struct s5p_mfc_dev *dev);
+	int (*get_sei_avail_status)(struct s5p_mfc_ctx *ctx);
+	int (*get_mvc_num_views)(struct s5p_mfc_dev *dev);
+	int (*get_mvc_view_id)(struct s5p_mfc_dev *dev);
+	unsigned int (*get_pic_type_top)(struct s5p_mfc_ctx *ctx);
+	unsigned int (*get_pic_type_bot)(struct s5p_mfc_ctx *ctx);
+	unsigned int (*get_crop_info_h)(struct s5p_mfc_ctx *ctx);
+	unsigned int (*get_crop_info_v)(struct s5p_mfc_ctx *ctx);
+};
 
-/* Decoding functions */
-int s5p_mfc_set_dec_frame_buffer(struct s5p_mfc_ctx *ctx);
-int s5p_mfc_set_dec_stream_buffer(struct s5p_mfc_ctx *ctx, int buf_addr,
-						  unsigned int start_num_byte,
-						  unsigned int buf_size);
-
-/* Encoding functions */
-void s5p_mfc_set_enc_frame_buffer(struct s5p_mfc_ctx *ctx,
-		unsigned long y_addr, unsigned long c_addr);
-int s5p_mfc_set_enc_stream_buffer(struct s5p_mfc_ctx *ctx,
-		unsigned long addr, unsigned int size);
-void s5p_mfc_get_enc_frame_buffer(struct s5p_mfc_ctx *ctx,
-		unsigned long *y_addr, unsigned long *c_addr);
-int s5p_mfc_set_enc_ref_buffer(struct s5p_mfc_ctx *mfc_ctx);
-
-int s5p_mfc_decode_one_frame(struct s5p_mfc_ctx *ctx,
-					enum s5p_mfc_decode_arg last_frame);
-int s5p_mfc_encode_one_frame(struct s5p_mfc_ctx *mfc_ctx);
-
-/* Memory allocation */
-int s5p_mfc_alloc_dec_temp_buffers(struct s5p_mfc_ctx *ctx);
-void s5p_mfc_set_dec_desc_buffer(struct s5p_mfc_ctx *ctx);
-void s5p_mfc_release_dec_desc_buffer(struct s5p_mfc_ctx *ctx);
-
-int s5p_mfc_alloc_codec_buffers(struct s5p_mfc_ctx *ctx);
-void s5p_mfc_release_codec_buffers(struct s5p_mfc_ctx *ctx);
-
-int s5p_mfc_alloc_instance_buffer(struct s5p_mfc_ctx *ctx);
-void s5p_mfc_release_instance_buffer(struct s5p_mfc_ctx *ctx);
-
-void s5p_mfc_try_run(struct s5p_mfc_dev *dev);
-void s5p_mfc_cleanup_queue(struct list_head *lh, struct vb2_queue *vq);
-
-#define s5p_mfc_get_dspl_y_adr()	(readl(dev->regs_base + \
-					S5P_FIMV_SI_DISPLAY_Y_ADR) << \
-					MFC_OFFSET_SHIFT)
-#define s5p_mfc_get_dec_y_adr()		(readl(dev->regs_base + \
-					S5P_FIMV_SI_DECODE_Y_ADR) << \
-					MFC_OFFSET_SHIFT)
-#define s5p_mfc_get_dspl_status()	readl(dev->regs_base + \
-						S5P_FIMV_SI_DISPLAY_STATUS)
-#define s5p_mfc_get_dec_status()	readl(dev->regs_base + \
-						S5P_FIMV_SI_DECODE_STATUS)
-#define s5p_mfc_get_frame_type()	(readl(dev->regs_base + \
-						S5P_FIMV_DECODE_FRAME_TYPE) \
-					& S5P_FIMV_DECODE_FRAME_MASK)
-#define s5p_mfc_get_consumed_stream()	readl(dev->regs_base + \
-						S5P_FIMV_SI_CONSUMED_BYTES)
-#define s5p_mfc_get_int_reason()	(readl(dev->regs_base + \
-					S5P_FIMV_RISC2HOST_CMD) & \
-					S5P_FIMV_RISC2HOST_CMD_MASK)
-#define s5p_mfc_get_int_err()		readl(dev->regs_base + \
-						S5P_FIMV_RISC2HOST_ARG2)
-#define s5p_mfc_err_dec(x)		(((x) & S5P_FIMV_ERR_DEC_MASK) >> \
-							S5P_FIMV_ERR_DEC_SHIFT)
-#define s5p_mfc_err_dspl(x)		(((x) & S5P_FIMV_ERR_DSPL_MASK) >> \
-							S5P_FIMV_ERR_DSPL_SHIFT)
-#define s5p_mfc_get_img_width()		readl(dev->regs_base + \
-						S5P_FIMV_SI_HRESOL)
-#define s5p_mfc_get_img_height()	readl(dev->regs_base + \
-						S5P_FIMV_SI_VRESOL)
-#define s5p_mfc_get_dpb_count()		readl(dev->regs_base + \
-						S5P_FIMV_SI_BUF_NUMBER)
-#define s5p_mfc_get_inst_no()		readl(dev->regs_base + \
-						S5P_FIMV_RISC2HOST_ARG1)
-#define s5p_mfc_get_enc_strm_size()	readl(dev->regs_base + \
-						S5P_FIMV_ENC_SI_STRM_SIZE)
-#define s5p_mfc_get_enc_slice_type()	readl(dev->regs_base + \
-						S5P_FIMV_ENC_SI_SLICE_TYPE)
+void s5p_mfc_init_hw_ops(struct s5p_mfc_dev *dev);
 
 #endif /* S5P_MFC_OPR_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
new file mode 100644
index 00000000000..bf7d010a410
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
@@ -0,0 +1,1794 @@
+/*
+ * drivers/media/platform/samsung/mfc5/s5p_mfc_opr_v5.c
+ *
+ * Samsung MFC (Multi Function Codec - FIMV) driver
+ * This file contains hw related functions.
+ *
+ * Kamil Debski, Copyright (c) 2011 Samsung Electronics
+ * http://www.samsung.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 "s5p_mfc_common.h"
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_ctrl.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_pm.h"
+#include "s5p_mfc_opr.h"
+#include "s5p_mfc_opr_v5.h"
+#include <asm/cacheflush.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/firmware.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+
+#define OFFSETA(x)		(((x) - dev->bank1) >> MFC_OFFSET_SHIFT)
+#define OFFSETB(x)		(((x) - dev->bank2) >> MFC_OFFSET_SHIFT)
+
+/* Allocate temporary buffers for decoding */
+int s5p_mfc_alloc_dec_temp_buffers_v5(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf_size_v5 *buf_size = dev->variant->buf_size->priv;
+
+	ctx->dsc.alloc = vb2_dma_contig_memops.alloc(
+			dev->alloc_ctx[MFC_BANK1_ALLOC_CTX],
+			buf_size->dsc);
+	if (IS_ERR_VALUE((int)ctx->dsc.alloc)) {
+		ctx->dsc.alloc = NULL;
+		mfc_err("Allocating DESC buffer failed\n");
+		return -ENOMEM;
+	}
+	ctx->dsc.dma = s5p_mfc_mem_cookie(
+			dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->dsc.alloc);
+	BUG_ON(ctx->dsc.dma & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+	ctx->dsc.virt = vb2_dma_contig_memops.vaddr(ctx->dsc.alloc);
+	if (ctx->dsc.virt == NULL) {
+		vb2_dma_contig_memops.put(ctx->dsc.alloc);
+		ctx->dsc.dma = 0;
+		ctx->dsc.alloc = NULL;
+		mfc_err("Remapping DESC buffer failed\n");
+		return -ENOMEM;
+	}
+	memset(ctx->dsc.virt, 0, buf_size->dsc);
+	wmb();
+	return 0;
+}
+
+/* Release temporary buffers for decoding */
+void s5p_mfc_release_dec_desc_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+	if (ctx->dsc.dma) {
+		vb2_dma_contig_memops.put(ctx->dsc.alloc);
+		ctx->dsc.alloc = NULL;
+		ctx->dsc.dma = 0;
+	}
+}
+
+/* Allocate codec buffers */
+int s5p_mfc_alloc_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned int enc_ref_y_size = 0;
+	unsigned int enc_ref_c_size = 0;
+	unsigned int guard_width, guard_height;
+
+	if (ctx->type == MFCINST_DECODER) {
+		mfc_debug(2, "Luma size:%d Chroma size:%d MV size:%d\n",
+			  ctx->luma_size, ctx->chroma_size, ctx->mv_size);
+		mfc_debug(2, "Totals bufs: %d\n", ctx->total_dpb_count);
+	} else if (ctx->type == MFCINST_ENCODER) {
+		enc_ref_y_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+			* ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
+		enc_ref_y_size = ALIGN(enc_ref_y_size, S5P_FIMV_NV12MT_SALIGN);
+
+		if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC) {
+			enc_ref_c_size = ALIGN(ctx->img_width,
+						S5P_FIMV_NV12MT_HALIGN)
+						* ALIGN(ctx->img_height >> 1,
+						S5P_FIMV_NV12MT_VALIGN);
+			enc_ref_c_size = ALIGN(enc_ref_c_size,
+							S5P_FIMV_NV12MT_SALIGN);
+		} else {
+			guard_width = ALIGN(ctx->img_width + 16,
+							S5P_FIMV_NV12MT_HALIGN);
+			guard_height = ALIGN((ctx->img_height >> 1) + 4,
+							S5P_FIMV_NV12MT_VALIGN);
+			enc_ref_c_size = ALIGN(guard_width * guard_height,
+					       S5P_FIMV_NV12MT_SALIGN);
+		}
+		mfc_debug(2, "recon luma size: %d chroma size: %d\n",
+			  enc_ref_y_size, enc_ref_c_size);
+	} else {
+		return -EINVAL;
+	}
+	/* Codecs have different memory requirements */
+	switch (ctx->codec_mode) {
+	case S5P_MFC_CODEC_H264_DEC:
+		ctx->bank1_size =
+		    ALIGN(S5P_FIMV_DEC_NB_IP_SIZE +
+					S5P_FIMV_DEC_VERT_NB_MV_SIZE,
+					S5P_FIMV_DEC_BUF_ALIGN);
+		ctx->bank2_size = ctx->total_dpb_count * ctx->mv_size;
+		break;
+	case S5P_MFC_CODEC_MPEG4_DEC:
+		ctx->bank1_size =
+		    ALIGN(S5P_FIMV_DEC_NB_DCAC_SIZE +
+				     S5P_FIMV_DEC_UPNB_MV_SIZE +
+				     S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
+				     S5P_FIMV_DEC_STX_PARSER_SIZE +
+				     S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE,
+				     S5P_FIMV_DEC_BUF_ALIGN);
+		ctx->bank2_size = 0;
+		break;
+	case S5P_MFC_CODEC_VC1RCV_DEC:
+	case S5P_MFC_CODEC_VC1_DEC:
+		ctx->bank1_size =
+		    ALIGN(S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE +
+			     S5P_FIMV_DEC_UPNB_MV_SIZE +
+			     S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
+			     S5P_FIMV_DEC_NB_DCAC_SIZE +
+			     3 * S5P_FIMV_DEC_VC1_BITPLANE_SIZE,
+			     S5P_FIMV_DEC_BUF_ALIGN);
+		ctx->bank2_size = 0;
+		break;
+	case S5P_MFC_CODEC_MPEG2_DEC:
+		ctx->bank1_size = 0;
+		ctx->bank2_size = 0;
+		break;
+	case S5P_MFC_CODEC_H263_DEC:
+		ctx->bank1_size =
+		    ALIGN(S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE +
+			     S5P_FIMV_DEC_UPNB_MV_SIZE +
+			     S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
+			     S5P_FIMV_DEC_NB_DCAC_SIZE,
+			     S5P_FIMV_DEC_BUF_ALIGN);
+		ctx->bank2_size = 0;
+		break;
+	case S5P_MFC_CODEC_H264_ENC:
+		ctx->bank1_size = (enc_ref_y_size * 2) +
+				   S5P_FIMV_ENC_UPMV_SIZE +
+				   S5P_FIMV_ENC_COLFLG_SIZE +
+				   S5P_FIMV_ENC_INTRAMD_SIZE +
+				   S5P_FIMV_ENC_NBORINFO_SIZE;
+		ctx->bank2_size = (enc_ref_y_size * 2) +
+				   (enc_ref_c_size * 4) +
+				   S5P_FIMV_ENC_INTRAPRED_SIZE;
+		break;
+	case S5P_MFC_CODEC_MPEG4_ENC:
+		ctx->bank1_size = (enc_ref_y_size * 2) +
+				   S5P_FIMV_ENC_UPMV_SIZE +
+				   S5P_FIMV_ENC_COLFLG_SIZE +
+				   S5P_FIMV_ENC_ACDCCOEF_SIZE;
+		ctx->bank2_size = (enc_ref_y_size * 2) +
+				   (enc_ref_c_size * 4);
+		break;
+	case S5P_MFC_CODEC_H263_ENC:
+		ctx->bank1_size = (enc_ref_y_size * 2) +
+				   S5P_FIMV_ENC_UPMV_SIZE +
+				   S5P_FIMV_ENC_ACDCCOEF_SIZE;
+		ctx->bank2_size = (enc_ref_y_size * 2) +
+				   (enc_ref_c_size * 4);
+		break;
+	default:
+		break;
+	}
+	/* Allocate only if memory from bank 1 is necessary */
+	if (ctx->bank1_size > 0) {
+		ctx->bank1_buf = vb2_dma_contig_memops.alloc(
+		dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_size);
+		if (IS_ERR(ctx->bank1_buf)) {
+			ctx->bank1_buf = NULL;
+			printk(KERN_ERR
+			       "Buf alloc for decoding failed (port A)\n");
+			return -ENOMEM;
+		}
+		ctx->bank1_phys = s5p_mfc_mem_cookie(
+		dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_buf);
+		BUG_ON(ctx->bank1_phys & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+	}
+	/* Allocate only if memory from bank 2 is necessary */
+	if (ctx->bank2_size > 0) {
+		ctx->bank2_buf = vb2_dma_contig_memops.alloc(
+		dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], ctx->bank2_size);
+		if (IS_ERR(ctx->bank2_buf)) {
+			ctx->bank2_buf = NULL;
+			mfc_err("Buf alloc for decoding failed (port B)\n");
+			return -ENOMEM;
+		}
+		ctx->bank2_phys = s5p_mfc_mem_cookie(
+		dev->alloc_ctx[MFC_BANK2_ALLOC_CTX], ctx->bank2_buf);
+		BUG_ON(ctx->bank2_phys & ((1 << MFC_BANK2_ALIGN_ORDER) - 1));
+	}
+	return 0;
+}
+
+/* Release buffers allocated for codec */
+void s5p_mfc_release_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
+{
+	if (ctx->bank1_buf) {
+		vb2_dma_contig_memops.put(ctx->bank1_buf);
+		ctx->bank1_buf = NULL;
+		ctx->bank1_phys = 0;
+		ctx->bank1_size = 0;
+	}
+	if (ctx->bank2_buf) {
+		vb2_dma_contig_memops.put(ctx->bank2_buf);
+		ctx->bank2_buf = NULL;
+		ctx->bank2_phys = 0;
+		ctx->bank2_size = 0;
+	}
+}
+
+/* Allocate memory for instance data buffer */
+int s5p_mfc_alloc_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf_size_v5 *buf_size = dev->variant->buf_size->priv;
+
+	if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC ||
+		ctx->codec_mode == S5P_MFC_CODEC_H264_ENC)
+		ctx->ctx.size = buf_size->h264_ctx;
+	else
+		ctx->ctx.size = buf_size->non_h264_ctx;
+	ctx->ctx.alloc = vb2_dma_contig_memops.alloc(
+		dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx.size);
+	if (IS_ERR(ctx->ctx.alloc)) {
+		mfc_err("Allocating context buffer failed\n");
+		ctx->ctx.alloc = NULL;
+		return -ENOMEM;
+	}
+	ctx->ctx.dma = s5p_mfc_mem_cookie(
+		dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx.alloc);
+	BUG_ON(ctx->ctx.dma & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+	ctx->ctx.ofs = OFFSETA(ctx->ctx.dma);
+	ctx->ctx.virt = vb2_dma_contig_memops.vaddr(ctx->ctx.alloc);
+	if (!ctx->ctx.virt) {
+		mfc_err("Remapping instance buffer failed\n");
+		vb2_dma_contig_memops.put(ctx->ctx.alloc);
+		ctx->ctx.alloc = NULL;
+		ctx->ctx.ofs = 0;
+		ctx->ctx.dma = 0;
+		return -ENOMEM;
+	}
+	/* Zero content of the allocated memory */
+	memset(ctx->ctx.virt, 0, ctx->ctx.size);
+	wmb();
+
+	/* Initialize shared memory */
+	ctx->shm.alloc = vb2_dma_contig_memops.alloc(
+			dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], buf_size->shm);
+	if (IS_ERR(ctx->shm.alloc)) {
+		mfc_err("failed to allocate shared memory\n");
+		return PTR_ERR(ctx->shm.alloc);
+	}
+	/* shared memory offset only keeps the offset from base (port a) */
+	ctx->shm.ofs = s5p_mfc_mem_cookie(
+			dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->shm.alloc)
+								- dev->bank1;
+	BUG_ON(ctx->shm.ofs & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+
+	ctx->shm.virt = vb2_dma_contig_memops.vaddr(ctx->shm.alloc);
+	if (!ctx->shm.virt) {
+		vb2_dma_contig_memops.put(ctx->shm.alloc);
+		ctx->shm.alloc = NULL;
+		ctx->shm.ofs = 0;
+		mfc_err("failed to virt addr of shared memory\n");
+		return -ENOMEM;
+	}
+	memset((void *)ctx->shm.virt, 0, buf_size->shm);
+	wmb();
+	return 0;
+}
+
+/* Release instance buffer */
+void s5p_mfc_release_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+	if (ctx->ctx.alloc) {
+		vb2_dma_contig_memops.put(ctx->ctx.alloc);
+		ctx->ctx.alloc = NULL;
+		ctx->ctx.ofs = 0;
+		ctx->ctx.virt = NULL;
+		ctx->ctx.dma = 0;
+	}
+	if (ctx->shm.alloc) {
+		vb2_dma_contig_memops.put(ctx->shm.alloc);
+		ctx->shm.alloc = NULL;
+		ctx->shm.ofs = 0;
+		ctx->shm.virt = NULL;
+	}
+}
+
+int s5p_mfc_alloc_dev_context_buffer_v5(struct s5p_mfc_dev *dev)
+{
+	/* NOP */
+
+	return 0;
+}
+
+void s5p_mfc_release_dev_context_buffer_v5(struct s5p_mfc_dev *dev)
+{
+	/* NOP */
+}
+
+static void s5p_mfc_write_info_v5(struct s5p_mfc_ctx *ctx, unsigned int data,
+			unsigned int ofs)
+{
+	writel(data, (ctx->shm.virt + ofs));
+	wmb();
+}
+
+static unsigned int s5p_mfc_read_info_v5(struct s5p_mfc_ctx *ctx,
+				unsigned int ofs)
+{
+	rmb();
+	return readl(ctx->shm.virt + ofs);
+}
+
+void s5p_mfc_dec_calc_dpb_size_v5(struct s5p_mfc_ctx *ctx)
+{
+	unsigned int guard_width, guard_height;
+
+	ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN);
+	ctx->buf_height = ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
+	mfc_debug(2,
+		"SEQ Done: Movie dimensions %dx%d, buffer dimensions: %dx%d\n",
+		ctx->img_width,	ctx->img_height, ctx->buf_width,
+		ctx->buf_height);
+
+	if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC) {
+		ctx->luma_size = ALIGN(ctx->buf_width * ctx->buf_height,
+				S5P_FIMV_DEC_BUF_ALIGN);
+		ctx->chroma_size = ALIGN(ctx->buf_width *
+				ALIGN((ctx->img_height >> 1),
+					S5P_FIMV_NV12MT_VALIGN),
+				S5P_FIMV_DEC_BUF_ALIGN);
+		ctx->mv_size = ALIGN(ctx->buf_width *
+				ALIGN((ctx->buf_height >> 2),
+					S5P_FIMV_NV12MT_VALIGN),
+				S5P_FIMV_DEC_BUF_ALIGN);
+	} else {
+		guard_width =
+			ALIGN(ctx->img_width + 24, S5P_FIMV_NV12MT_HALIGN);
+		guard_height =
+			ALIGN(ctx->img_height + 16, S5P_FIMV_NV12MT_VALIGN);
+		ctx->luma_size = ALIGN(guard_width * guard_height,
+				S5P_FIMV_DEC_BUF_ALIGN);
+
+		guard_width =
+			ALIGN(ctx->img_width + 16, S5P_FIMV_NV12MT_HALIGN);
+		guard_height =
+			ALIGN((ctx->img_height >> 1) + 4,
+					S5P_FIMV_NV12MT_VALIGN);
+		ctx->chroma_size = ALIGN(guard_width * guard_height,
+				S5P_FIMV_DEC_BUF_ALIGN);
+
+		ctx->mv_size = 0;
+	}
+}
+
+void s5p_mfc_enc_calc_src_size_v5(struct s5p_mfc_ctx *ctx)
+{
+	if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M) {
+		ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12M_HALIGN);
+
+		ctx->luma_size = ALIGN(ctx->img_width, S5P_FIMV_NV12M_HALIGN)
+			* ALIGN(ctx->img_height, S5P_FIMV_NV12M_LVALIGN);
+		ctx->chroma_size = ALIGN(ctx->img_width, S5P_FIMV_NV12M_HALIGN)
+			* ALIGN((ctx->img_height >> 1), S5P_FIMV_NV12M_CVALIGN);
+
+		ctx->luma_size = ALIGN(ctx->luma_size, S5P_FIMV_NV12M_SALIGN);
+		ctx->chroma_size =
+			ALIGN(ctx->chroma_size, S5P_FIMV_NV12M_SALIGN);
+	} else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT) {
+		ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN);
+
+		ctx->luma_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+			* ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
+		ctx->chroma_size =
+			ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+			* ALIGN((ctx->img_height >> 1), S5P_FIMV_NV12MT_VALIGN);
+
+		ctx->luma_size = ALIGN(ctx->luma_size, S5P_FIMV_NV12MT_SALIGN);
+		ctx->chroma_size =
+			ALIGN(ctx->chroma_size, S5P_FIMV_NV12MT_SALIGN);
+	}
+}
+
+/* Set registers for decoding temporary buffers */
+static void s5p_mfc_set_dec_desc_buffer(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf_size_v5 *buf_size = dev->variant->buf_size->priv;
+
+	mfc_write(dev, OFFSETA(ctx->dsc.dma), S5P_FIMV_SI_CH0_DESC_ADR);
+	mfc_write(dev, buf_size->dsc, S5P_FIMV_SI_CH0_DESC_SIZE);
+}
+
+/* Set registers for shared buffer */
+static void s5p_mfc_set_shared_buffer(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	mfc_write(dev, ctx->shm.ofs, S5P_FIMV_SI_CH0_HOST_WR_ADR);
+}
+
+/* Set registers for decoding stream buffer */
+int s5p_mfc_set_dec_stream_buffer_v5(struct s5p_mfc_ctx *ctx, int buf_addr,
+		  unsigned int start_num_byte, unsigned int buf_size)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	mfc_write(dev, OFFSETA(buf_addr), S5P_FIMV_SI_CH0_SB_ST_ADR);
+	mfc_write(dev, ctx->dec_src_buf_size, S5P_FIMV_SI_CH0_CPB_SIZE);
+	mfc_write(dev, buf_size, S5P_FIMV_SI_CH0_SB_FRM_SIZE);
+	s5p_mfc_write_info_v5(ctx, start_num_byte, START_BYTE_NUM);
+	return 0;
+}
+
+/* Set decoding frame buffer */
+int s5p_mfc_set_dec_frame_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+	unsigned int frame_size, i;
+	unsigned int frame_size_ch, frame_size_mv;
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned int dpb;
+	size_t buf_addr1, buf_addr2;
+	int buf_size1, buf_size2;
+
+	buf_addr1 = ctx->bank1_phys;
+	buf_size1 = ctx->bank1_size;
+	buf_addr2 = ctx->bank2_phys;
+	buf_size2 = ctx->bank2_size;
+	dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) &
+						~S5P_FIMV_DPB_COUNT_MASK;
+	mfc_write(dev, ctx->total_dpb_count | dpb,
+						S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
+	s5p_mfc_set_shared_buffer(ctx);
+	switch (ctx->codec_mode) {
+	case S5P_MFC_CODEC_H264_DEC:
+		mfc_write(dev, OFFSETA(buf_addr1),
+						S5P_FIMV_H264_VERT_NB_MV_ADR);
+		buf_addr1 += S5P_FIMV_DEC_VERT_NB_MV_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_VERT_NB_MV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H264_NB_IP_ADR);
+		buf_addr1 += S5P_FIMV_DEC_NB_IP_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_NB_IP_SIZE;
+		break;
+	case S5P_MFC_CODEC_MPEG4_DEC:
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_NB_DCAC_ADR);
+		buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_UP_NB_MV_ADR);
+		buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_SA_MV_ADR);
+		buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_SP_ADR);
+		buf_addr1 += S5P_FIMV_DEC_STX_PARSER_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_STX_PARSER_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_OT_LINE_ADR);
+		buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+		break;
+	case S5P_MFC_CODEC_H263_DEC:
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_OT_LINE_ADR);
+		buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_UP_NB_MV_ADR);
+		buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_SA_MV_ADR);
+		buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_NB_DCAC_ADR);
+		buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
+		break;
+	case S5P_MFC_CODEC_VC1_DEC:
+	case S5P_MFC_CODEC_VC1RCV_DEC:
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_NB_DCAC_ADR);
+		buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_OT_LINE_ADR);
+		buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_UP_NB_MV_ADR);
+		buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_SA_MV_ADR);
+		buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE3_ADR);
+		buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE2_ADR);
+		buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE1_ADR);
+		buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+		break;
+	case S5P_MFC_CODEC_MPEG2_DEC:
+		break;
+	default:
+		mfc_err("Unknown codec for decoding (%x)\n",
+			ctx->codec_mode);
+		return -EINVAL;
+		break;
+	}
+	frame_size = ctx->luma_size;
+	frame_size_ch = ctx->chroma_size;
+	frame_size_mv = ctx->mv_size;
+	mfc_debug(2, "Frm size: %d ch: %d mv: %d\n", frame_size, frame_size_ch,
+								frame_size_mv);
+	for (i = 0; i < ctx->total_dpb_count; i++) {
+		/* Bank2 */
+		mfc_debug(2, "Luma %d: %x\n", i,
+					ctx->dst_bufs[i].cookie.raw.luma);
+		mfc_write(dev, OFFSETB(ctx->dst_bufs[i].cookie.raw.luma),
+						S5P_FIMV_DEC_LUMA_ADR + i * 4);
+		mfc_debug(2, "\tChroma %d: %x\n", i,
+					ctx->dst_bufs[i].cookie.raw.chroma);
+		mfc_write(dev, OFFSETA(ctx->dst_bufs[i].cookie.raw.chroma),
+					       S5P_FIMV_DEC_CHROMA_ADR + i * 4);
+		if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC) {
+			mfc_debug(2, "\tBuf2: %x, size: %d\n",
+							buf_addr2, buf_size2);
+			mfc_write(dev, OFFSETB(buf_addr2),
+						S5P_FIMV_H264_MV_ADR + i * 4);
+			buf_addr2 += frame_size_mv;
+			buf_size2 -= frame_size_mv;
+		}
+	}
+	mfc_debug(2, "Buf1: %u, buf_size1: %d\n", buf_addr1, buf_size1);
+	mfc_debug(2, "Buf 1/2 size after: %d/%d (frames %d)\n",
+			buf_size1,  buf_size2, ctx->total_dpb_count);
+	if (buf_size1 < 0 || buf_size2 < 0) {
+		mfc_debug(2, "Not enough memory has been allocated\n");
+		return -ENOMEM;
+	}
+	s5p_mfc_write_info_v5(ctx, frame_size, ALLOC_LUMA_DPB_SIZE);
+	s5p_mfc_write_info_v5(ctx, frame_size_ch, ALLOC_CHROMA_DPB_SIZE);
+	if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC)
+		s5p_mfc_write_info_v5(ctx, frame_size_mv, ALLOC_MV_SIZE);
+	mfc_write(dev, ((S5P_FIMV_CH_INIT_BUFS & S5P_FIMV_CH_MASK)
+					<< S5P_FIMV_CH_SHIFT) | (ctx->inst_no),
+						S5P_FIMV_SI_CH0_INST_ID);
+	return 0;
+}
+
+/* Set registers for encoding stream buffer */
+int s5p_mfc_set_enc_stream_buffer_v5(struct s5p_mfc_ctx *ctx,
+		unsigned long addr, unsigned int size)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	mfc_write(dev, OFFSETA(addr), S5P_FIMV_ENC_SI_CH0_SB_ADR);
+	mfc_write(dev, size, S5P_FIMV_ENC_SI_CH0_SB_SIZE);
+	return 0;
+}
+
+void s5p_mfc_set_enc_frame_buffer_v5(struct s5p_mfc_ctx *ctx,
+		unsigned long y_addr, unsigned long c_addr)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	mfc_write(dev, OFFSETB(y_addr), S5P_FIMV_ENC_SI_CH0_CUR_Y_ADR);
+	mfc_write(dev, OFFSETB(c_addr), S5P_FIMV_ENC_SI_CH0_CUR_C_ADR);
+}
+
+void s5p_mfc_get_enc_frame_buffer_v5(struct s5p_mfc_ctx *ctx,
+		unsigned long *y_addr, unsigned long *c_addr)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	*y_addr = dev->bank2 + (mfc_read(dev, S5P_FIMV_ENCODED_Y_ADDR)
+							<< MFC_OFFSET_SHIFT);
+	*c_addr = dev->bank2 + (mfc_read(dev, S5P_FIMV_ENCODED_C_ADDR)
+							<< MFC_OFFSET_SHIFT);
+}
+
+/* Set encoding ref & codec buffer */
+int s5p_mfc_set_enc_ref_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	size_t buf_addr1, buf_addr2;
+	size_t buf_size1, buf_size2;
+	unsigned int enc_ref_y_size, enc_ref_c_size;
+	unsigned int guard_width, guard_height;
+	int i;
+
+	buf_addr1 = ctx->bank1_phys;
+	buf_size1 = ctx->bank1_size;
+	buf_addr2 = ctx->bank2_phys;
+	buf_size2 = ctx->bank2_size;
+	enc_ref_y_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+		* ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
+	enc_ref_y_size = ALIGN(enc_ref_y_size, S5P_FIMV_NV12MT_SALIGN);
+	if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC) {
+		enc_ref_c_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+			* ALIGN((ctx->img_height >> 1), S5P_FIMV_NV12MT_VALIGN);
+		enc_ref_c_size = ALIGN(enc_ref_c_size, S5P_FIMV_NV12MT_SALIGN);
+	} else {
+		guard_width = ALIGN(ctx->img_width + 16,
+						S5P_FIMV_NV12MT_HALIGN);
+		guard_height = ALIGN((ctx->img_height >> 1) + 4,
+						S5P_FIMV_NV12MT_VALIGN);
+		enc_ref_c_size = ALIGN(guard_width * guard_height,
+				       S5P_FIMV_NV12MT_SALIGN);
+	}
+	mfc_debug(2, "buf_size1: %d, buf_size2: %d\n", buf_size1, buf_size2);
+	switch (ctx->codec_mode) {
+	case S5P_MFC_CODEC_H264_ENC:
+		for (i = 0; i < 2; i++) {
+			mfc_write(dev, OFFSETA(buf_addr1),
+				S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
+			buf_addr1 += enc_ref_y_size;
+			buf_size1 -= enc_ref_y_size;
+
+			mfc_write(dev, OFFSETB(buf_addr2),
+				S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
+			buf_addr2 += enc_ref_y_size;
+			buf_size2 -= enc_ref_y_size;
+		}
+		for (i = 0; i < 4; i++) {
+			mfc_write(dev, OFFSETB(buf_addr2),
+				S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
+			buf_addr2 += enc_ref_c_size;
+			buf_size2 -= enc_ref_c_size;
+		}
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H264_UP_MV_ADR);
+		buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
+		buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1),
+					S5P_FIMV_H264_COZERO_FLAG_ADR);
+		buf_addr1 += S5P_FIMV_ENC_COLFLG_SIZE;
+		buf_size1 -= S5P_FIMV_ENC_COLFLG_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1),
+					S5P_FIMV_H264_UP_INTRA_MD_ADR);
+		buf_addr1 += S5P_FIMV_ENC_INTRAMD_SIZE;
+		buf_size1 -= S5P_FIMV_ENC_INTRAMD_SIZE;
+		mfc_write(dev, OFFSETB(buf_addr2),
+					S5P_FIMV_H264_UP_INTRA_PRED_ADR);
+		buf_addr2 += S5P_FIMV_ENC_INTRAPRED_SIZE;
+		buf_size2 -= S5P_FIMV_ENC_INTRAPRED_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1),
+					S5P_FIMV_H264_NBOR_INFO_ADR);
+		buf_addr1 += S5P_FIMV_ENC_NBORINFO_SIZE;
+		buf_size1 -= S5P_FIMV_ENC_NBORINFO_SIZE;
+		mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
+			buf_size1, buf_size2);
+		break;
+	case S5P_MFC_CODEC_MPEG4_ENC:
+		for (i = 0; i < 2; i++) {
+			mfc_write(dev, OFFSETA(buf_addr1),
+				S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
+			buf_addr1 += enc_ref_y_size;
+			buf_size1 -= enc_ref_y_size;
+			mfc_write(dev, OFFSETB(buf_addr2),
+				S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
+			buf_addr2 += enc_ref_y_size;
+			buf_size2 -= enc_ref_y_size;
+		}
+		for (i = 0; i < 4; i++) {
+			mfc_write(dev, OFFSETB(buf_addr2),
+				S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
+			buf_addr2 += enc_ref_c_size;
+			buf_size2 -= enc_ref_c_size;
+		}
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_UP_MV_ADR);
+		buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
+		buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1),
+						S5P_FIMV_MPEG4_COZERO_FLAG_ADR);
+		buf_addr1 += S5P_FIMV_ENC_COLFLG_SIZE;
+		buf_size1 -= S5P_FIMV_ENC_COLFLG_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1),
+						S5P_FIMV_MPEG4_ACDC_COEF_ADR);
+		buf_addr1 += S5P_FIMV_ENC_ACDCCOEF_SIZE;
+		buf_size1 -= S5P_FIMV_ENC_ACDCCOEF_SIZE;
+		mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
+			buf_size1, buf_size2);
+		break;
+	case S5P_MFC_CODEC_H263_ENC:
+		for (i = 0; i < 2; i++) {
+			mfc_write(dev, OFFSETA(buf_addr1),
+				S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
+			buf_addr1 += enc_ref_y_size;
+			buf_size1 -= enc_ref_y_size;
+			mfc_write(dev, OFFSETB(buf_addr2),
+				S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
+			buf_addr2 += enc_ref_y_size;
+			buf_size2 -= enc_ref_y_size;
+		}
+		for (i = 0; i < 4; i++) {
+			mfc_write(dev, OFFSETB(buf_addr2),
+				S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
+			buf_addr2 += enc_ref_c_size;
+			buf_size2 -= enc_ref_c_size;
+		}
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_UP_MV_ADR);
+		buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
+		buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_ACDC_COEF_ADR);
+		buf_addr1 += S5P_FIMV_ENC_ACDCCOEF_SIZE;
+		buf_size1 -= S5P_FIMV_ENC_ACDCCOEF_SIZE;
+		mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
+			buf_size1, buf_size2);
+		break;
+	default:
+		mfc_err("Unknown codec set for encoding: %d\n",
+			ctx->codec_mode);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int s5p_mfc_set_enc_params(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	unsigned int reg;
+	unsigned int shm;
+
+	/* width */
+	mfc_write(dev, ctx->img_width, S5P_FIMV_ENC_HSIZE_PX);
+	/* height */
+	mfc_write(dev, ctx->img_height, S5P_FIMV_ENC_VSIZE_PX);
+	/* pictype : enable, IDR period */
+	reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+	reg |= (1 << 18);
+	reg &= ~(0xFFFF);
+	reg |= p->gop_size;
+	mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+	mfc_write(dev, 0, S5P_FIMV_ENC_B_RECON_WRITE_ON);
+	/* multi-slice control */
+	/* multi-slice MB number or bit size */
+	mfc_write(dev, p->slice_mode, S5P_FIMV_ENC_MSLICE_CTRL);
+	if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB) {
+		mfc_write(dev, p->slice_mb, S5P_FIMV_ENC_MSLICE_MB);
+	} else if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES) {
+		mfc_write(dev, p->slice_bit, S5P_FIMV_ENC_MSLICE_BIT);
+	} else {
+		mfc_write(dev, 0, S5P_FIMV_ENC_MSLICE_MB);
+		mfc_write(dev, 0, S5P_FIMV_ENC_MSLICE_BIT);
+	}
+	/* cyclic intra refresh */
+	mfc_write(dev, p->intra_refresh_mb, S5P_FIMV_ENC_CIR_CTRL);
+	/* memory structure cur. frame */
+	if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M)
+		mfc_write(dev, 0, S5P_FIMV_ENC_MAP_FOR_CUR);
+	else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT)
+		mfc_write(dev, 3, S5P_FIMV_ENC_MAP_FOR_CUR);
+	/* padding control & value */
+	reg = mfc_read(dev, S5P_FIMV_ENC_PADDING_CTRL);
+	if (p->pad) {
+		/** enable */
+		reg |= (1 << 31);
+		/** cr value */
+		reg &= ~(0xFF << 16);
+		reg |= (p->pad_cr << 16);
+		/** cb value */
+		reg &= ~(0xFF << 8);
+		reg |= (p->pad_cb << 8);
+		/** y value */
+		reg &= ~(0xFF);
+		reg |= (p->pad_luma);
+	} else {
+		/** disable & all value clear */
+		reg = 0;
+	}
+	mfc_write(dev, reg, S5P_FIMV_ENC_PADDING_CTRL);
+	/* rate control config. */
+	reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+	/** frame-level rate control */
+	reg &= ~(0x1 << 9);
+	reg |= (p->rc_frame << 9);
+	mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+	/* bit rate */
+	if (p->rc_frame)
+		mfc_write(dev, p->rc_bitrate,
+			S5P_FIMV_ENC_RC_BIT_RATE);
+	else
+		mfc_write(dev, 0, S5P_FIMV_ENC_RC_BIT_RATE);
+	/* reaction coefficient */
+	if (p->rc_frame)
+		mfc_write(dev, p->rc_reaction_coeff, S5P_FIMV_ENC_RC_RPARA);
+	shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+	/* seq header ctrl */
+	shm &= ~(0x1 << 3);
+	shm |= (p->seq_hdr_mode << 3);
+	/* frame skip mode */
+	shm &= ~(0x3 << 1);
+	shm |= (p->frame_skip_mode << 1);
+	s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+	/* fixed target bit */
+	s5p_mfc_write_info_v5(ctx, p->fixed_target_bit, RC_CONTROL_CONFIG);
+	return 0;
+}
+
+static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	struct s5p_mfc_h264_enc_params *p_264 = &p->codec.h264;
+	unsigned int reg;
+	unsigned int shm;
+
+	s5p_mfc_set_enc_params(ctx);
+	/* pictype : number of B */
+	reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+	/* num_b_frame - 0 ~ 2 */
+	reg &= ~(0x3 << 16);
+	reg |= (p->num_b_frame << 16);
+	mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+	/* profile & level */
+	reg = mfc_read(dev, S5P_FIMV_ENC_PROFILE);
+	/* level */
+	reg &= ~(0xFF << 8);
+	reg |= (p_264->level << 8);
+	/* profile - 0 ~ 2 */
+	reg &= ~(0x3F);
+	reg |= p_264->profile;
+	mfc_write(dev, reg, S5P_FIMV_ENC_PROFILE);
+	/* interlace  */
+	mfc_write(dev, p_264->interlace, S5P_FIMV_ENC_PIC_STRUCT);
+	/* height */
+	if (p_264->interlace)
+		mfc_write(dev, ctx->img_height >> 1, S5P_FIMV_ENC_VSIZE_PX);
+	/* loopfilter ctrl */
+	mfc_write(dev, p_264->loop_filter_mode, S5P_FIMV_ENC_LF_CTRL);
+	/* loopfilter alpha offset */
+	if (p_264->loop_filter_alpha < 0) {
+		reg = 0x10;
+		reg |= (0xFF - p_264->loop_filter_alpha) + 1;
+	} else {
+		reg = 0x00;
+		reg |= (p_264->loop_filter_alpha & 0xF);
+	}
+	mfc_write(dev, reg, S5P_FIMV_ENC_ALPHA_OFF);
+	/* loopfilter beta offset */
+	if (p_264->loop_filter_beta < 0) {
+		reg = 0x10;
+		reg |= (0xFF - p_264->loop_filter_beta) + 1;
+	} else {
+		reg = 0x00;
+		reg |= (p_264->loop_filter_beta & 0xF);
+	}
+	mfc_write(dev, reg, S5P_FIMV_ENC_BETA_OFF);
+	/* entropy coding mode */
+	if (p_264->entropy_mode == V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC)
+		mfc_write(dev, 1, S5P_FIMV_ENC_H264_ENTROPY_MODE);
+	else
+		mfc_write(dev, 0, S5P_FIMV_ENC_H264_ENTROPY_MODE);
+	/* number of ref. picture */
+	reg = mfc_read(dev, S5P_FIMV_ENC_H264_NUM_OF_REF);
+	/* num of ref. pictures of P */
+	reg &= ~(0x3 << 5);
+	reg |= (p_264->num_ref_pic_4p << 5);
+	/* max number of ref. pictures */
+	reg &= ~(0x1F);
+	reg |= p_264->max_ref_pic;
+	mfc_write(dev, reg, S5P_FIMV_ENC_H264_NUM_OF_REF);
+	/* 8x8 transform enable */
+	mfc_write(dev, p_264->_8x8_transform, S5P_FIMV_ENC_H264_TRANS_FLAG);
+	/* rate control config. */
+	reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+	/* macroblock level rate control */
+	reg &= ~(0x1 << 8);
+	reg |= (p->rc_mb << 8);
+	/* frame QP */
+	reg &= ~(0x3F);
+	reg |= p_264->rc_frame_qp;
+	mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+	/* frame rate */
+	if (p->rc_frame && p->rc_framerate_denom)
+		mfc_write(dev, p->rc_framerate_num * 1000
+			/ p->rc_framerate_denom, S5P_FIMV_ENC_RC_FRAME_RATE);
+	else
+		mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
+	/* max & min value of QP */
+	reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
+	/* max QP */
+	reg &= ~(0x3F << 8);
+	reg |= (p_264->rc_max_qp << 8);
+	/* min QP */
+	reg &= ~(0x3F);
+	reg |= p_264->rc_min_qp;
+	mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
+	/* macroblock adaptive scaling features */
+	if (p->rc_mb) {
+		reg = mfc_read(dev, S5P_FIMV_ENC_RC_MB_CTRL);
+		/* dark region */
+		reg &= ~(0x1 << 3);
+		reg |= (p_264->rc_mb_dark << 3);
+		/* smooth region */
+		reg &= ~(0x1 << 2);
+		reg |= (p_264->rc_mb_smooth << 2);
+		/* static region */
+		reg &= ~(0x1 << 1);
+		reg |= (p_264->rc_mb_static << 1);
+		/* high activity region */
+		reg &= ~(0x1);
+		reg |= p_264->rc_mb_activity;
+		mfc_write(dev, reg, S5P_FIMV_ENC_RC_MB_CTRL);
+	}
+	if (!p->rc_frame && !p->rc_mb) {
+		shm = s5p_mfc_read_info_v5(ctx, P_B_FRAME_QP);
+		shm &= ~(0xFFF);
+		shm |= ((p_264->rc_b_frame_qp & 0x3F) << 6);
+		shm |= (p_264->rc_p_frame_qp & 0x3F);
+		s5p_mfc_write_info_v5(ctx, shm, P_B_FRAME_QP);
+	}
+	/* extended encoder ctrl */
+	shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+	/* AR VUI control */
+	shm &= ~(0x1 << 15);
+	shm |= (p_264->vui_sar << 1);
+	s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+	if (p_264->vui_sar) {
+		/* aspect ration IDC */
+		shm = s5p_mfc_read_info_v5(ctx, SAMPLE_ASPECT_RATIO_IDC);
+		shm &= ~(0xFF);
+		shm |= p_264->vui_sar_idc;
+		s5p_mfc_write_info_v5(ctx, shm, SAMPLE_ASPECT_RATIO_IDC);
+		if (p_264->vui_sar_idc == 0xFF) {
+			/* sample  AR info */
+			shm = s5p_mfc_read_info_v5(ctx, EXTENDED_SAR);
+			shm &= ~(0xFFFFFFFF);
+			shm |= p_264->vui_ext_sar_width << 16;
+			shm |= p_264->vui_ext_sar_height;
+			s5p_mfc_write_info_v5(ctx, shm, EXTENDED_SAR);
+		}
+	}
+	/* intra picture period for H.264 */
+	shm = s5p_mfc_read_info_v5(ctx, H264_I_PERIOD);
+	/* control */
+	shm &= ~(0x1 << 16);
+	shm |= (p_264->open_gop << 16);
+	/* value */
+	if (p_264->open_gop) {
+		shm &= ~(0xFFFF);
+		shm |= p_264->open_gop_size;
+	}
+	s5p_mfc_write_info_v5(ctx, shm, H264_I_PERIOD);
+	/* extended encoder ctrl */
+	shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+	/* vbv buffer size */
+	if (p->frame_skip_mode ==
+			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+		shm &= ~(0xFFFF << 16);
+		shm |= (p_264->cpb_size << 16);
+	}
+	s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+	return 0;
+}
+
+static int s5p_mfc_set_enc_params_mpeg4(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	struct s5p_mfc_mpeg4_enc_params *p_mpeg4 = &p->codec.mpeg4;
+	unsigned int reg;
+	unsigned int shm;
+	unsigned int framerate;
+
+	s5p_mfc_set_enc_params(ctx);
+	/* pictype : number of B */
+	reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+	/* num_b_frame - 0 ~ 2 */
+	reg &= ~(0x3 << 16);
+	reg |= (p->num_b_frame << 16);
+	mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+	/* profile & level */
+	reg = mfc_read(dev, S5P_FIMV_ENC_PROFILE);
+	/* level */
+	reg &= ~(0xFF << 8);
+	reg |= (p_mpeg4->level << 8);
+	/* profile - 0 ~ 2 */
+	reg &= ~(0x3F);
+	reg |= p_mpeg4->profile;
+	mfc_write(dev, reg, S5P_FIMV_ENC_PROFILE);
+	/* quarter_pixel */
+	mfc_write(dev, p_mpeg4->quarter_pixel, S5P_FIMV_ENC_MPEG4_QUART_PXL);
+	/* qp */
+	if (!p->rc_frame) {
+		shm = s5p_mfc_read_info_v5(ctx, P_B_FRAME_QP);
+		shm &= ~(0xFFF);
+		shm |= ((p_mpeg4->rc_b_frame_qp & 0x3F) << 6);
+		shm |= (p_mpeg4->rc_p_frame_qp & 0x3F);
+		s5p_mfc_write_info_v5(ctx, shm, P_B_FRAME_QP);
+	}
+	/* frame rate */
+	if (p->rc_frame) {
+		if (p->rc_framerate_denom > 0) {
+			framerate = p->rc_framerate_num * 1000 /
+						p->rc_framerate_denom;
+			mfc_write(dev, framerate,
+				S5P_FIMV_ENC_RC_FRAME_RATE);
+			shm = s5p_mfc_read_info_v5(ctx, RC_VOP_TIMING);
+			shm &= ~(0xFFFFFFFF);
+			shm |= (1 << 31);
+			shm |= ((p->rc_framerate_num & 0x7FFF) << 16);
+			shm |= (p->rc_framerate_denom & 0xFFFF);
+			s5p_mfc_write_info_v5(ctx, shm, RC_VOP_TIMING);
+		}
+	} else {
+		mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
+	}
+	/* rate control config. */
+	reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+	/* frame QP */
+	reg &= ~(0x3F);
+	reg |= p_mpeg4->rc_frame_qp;
+	mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+	/* max & min value of QP */
+	reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
+	/* max QP */
+	reg &= ~(0x3F << 8);
+	reg |= (p_mpeg4->rc_max_qp << 8);
+	/* min QP */
+	reg &= ~(0x3F);
+	reg |= p_mpeg4->rc_min_qp;
+	mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
+	/* extended encoder ctrl */
+	shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+	/* vbv buffer size */
+	if (p->frame_skip_mode ==
+			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+		shm &= ~(0xFFFF << 16);
+		shm |= (p->vbv_size << 16);
+	}
+	s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+	return 0;
+}
+
+static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	struct s5p_mfc_mpeg4_enc_params *p_h263 = &p->codec.mpeg4;
+	unsigned int reg;
+	unsigned int shm;
+
+	s5p_mfc_set_enc_params(ctx);
+	/* qp */
+	if (!p->rc_frame) {
+		shm = s5p_mfc_read_info_v5(ctx, P_B_FRAME_QP);
+		shm &= ~(0xFFF);
+		shm |= (p_h263->rc_p_frame_qp & 0x3F);
+		s5p_mfc_write_info_v5(ctx, shm, P_B_FRAME_QP);
+	}
+	/* frame rate */
+	if (p->rc_frame && p->rc_framerate_denom)
+		mfc_write(dev, p->rc_framerate_num * 1000
+			/ p->rc_framerate_denom, S5P_FIMV_ENC_RC_FRAME_RATE);
+	else
+		mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
+	/* rate control config. */
+	reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+	/* frame QP */
+	reg &= ~(0x3F);
+	reg |= p_h263->rc_frame_qp;
+	mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+	/* max & min value of QP */
+	reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
+	/* max QP */
+	reg &= ~(0x3F << 8);
+	reg |= (p_h263->rc_max_qp << 8);
+	/* min QP */
+	reg &= ~(0x3F);
+	reg |= p_h263->rc_min_qp;
+	mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
+	/* extended encoder ctrl */
+	shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+	/* vbv buffer size */
+	if (p->frame_skip_mode ==
+			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+		shm &= ~(0xFFFF << 16);
+		shm |= (p->vbv_size << 16);
+	}
+	s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+	return 0;
+}
+
+/* Initialize decoding */
+int s5p_mfc_init_decode_v5(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	s5p_mfc_set_shared_buffer(ctx);
+	/* Setup loop filter, for decoding this is only valid for MPEG4 */
+	if (ctx->codec_mode == S5P_MFC_CODEC_MPEG4_DEC)
+		mfc_write(dev, ctx->loop_filter_mpeg4, S5P_FIMV_ENC_LF_CTRL);
+	else
+		mfc_write(dev, 0, S5P_FIMV_ENC_LF_CTRL);
+	mfc_write(dev, ((ctx->slice_interface & S5P_FIMV_SLICE_INT_MASK) <<
+		S5P_FIMV_SLICE_INT_SHIFT) | (ctx->display_delay_enable <<
+		S5P_FIMV_DDELAY_ENA_SHIFT) | ((ctx->display_delay &
+		S5P_FIMV_DDELAY_VAL_MASK) << S5P_FIMV_DDELAY_VAL_SHIFT),
+		S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
+	mfc_write(dev,
+	((S5P_FIMV_CH_SEQ_HEADER & S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT)
+				| (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+	return 0;
+}
+
+static void s5p_mfc_set_flush(struct s5p_mfc_ctx *ctx, int flush)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned int dpb;
+
+	if (flush)
+		dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) | (
+			S5P_FIMV_DPB_FLUSH_MASK << S5P_FIMV_DPB_FLUSH_SHIFT);
+	else
+		dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) &
+			~(S5P_FIMV_DPB_FLUSH_MASK << S5P_FIMV_DPB_FLUSH_SHIFT);
+	mfc_write(dev, dpb, S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
+}
+
+/* Decode a single frame */
+int s5p_mfc_decode_one_frame_v5(struct s5p_mfc_ctx *ctx,
+					enum s5p_mfc_decode_arg last_frame)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	mfc_write(dev, ctx->dec_dst_flag, S5P_FIMV_SI_CH0_RELEASE_BUF);
+	s5p_mfc_set_shared_buffer(ctx);
+	s5p_mfc_set_flush(ctx, ctx->dpb_flush_flag);
+	/* Issue different commands to instance basing on whether it
+	 * is the last frame or not. */
+	switch (last_frame) {
+	case MFC_DEC_FRAME:
+		mfc_write(dev, ((S5P_FIMV_CH_FRAME_START & S5P_FIMV_CH_MASK) <<
+		S5P_FIMV_CH_SHIFT) | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+		break;
+	case MFC_DEC_LAST_FRAME:
+		mfc_write(dev, ((S5P_FIMV_CH_LAST_FRAME & S5P_FIMV_CH_MASK) <<
+		S5P_FIMV_CH_SHIFT) | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+		break;
+	case MFC_DEC_RES_CHANGE:
+		mfc_write(dev, ((S5P_FIMV_CH_FRAME_START_REALLOC &
+		S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT) | (ctx->inst_no),
+		S5P_FIMV_SI_CH0_INST_ID);
+		break;
+	}
+	mfc_debug(2, "Decoding a usual frame\n");
+	return 0;
+}
+
+int s5p_mfc_init_encode_v5(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC)
+		s5p_mfc_set_enc_params_h264(ctx);
+	else if (ctx->codec_mode == S5P_MFC_CODEC_MPEG4_ENC)
+		s5p_mfc_set_enc_params_mpeg4(ctx);
+	else if (ctx->codec_mode == S5P_MFC_CODEC_H263_ENC)
+		s5p_mfc_set_enc_params_h263(ctx);
+	else {
+		mfc_err("Unknown codec for encoding (%x)\n",
+			ctx->codec_mode);
+		return -EINVAL;
+	}
+	s5p_mfc_set_shared_buffer(ctx);
+	mfc_write(dev, ((S5P_FIMV_CH_SEQ_HEADER << 16) & 0x70000) |
+		(ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+	return 0;
+}
+
+/* Encode a single frame */
+int s5p_mfc_encode_one_frame_v5(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	int cmd;
+	/* memory structure cur. frame */
+	if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M)
+		mfc_write(dev, 0, S5P_FIMV_ENC_MAP_FOR_CUR);
+	else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT)
+		mfc_write(dev, 3, S5P_FIMV_ENC_MAP_FOR_CUR);
+	s5p_mfc_set_shared_buffer(ctx);
+
+	if (ctx->state == MFCINST_FINISHING)
+		cmd = S5P_FIMV_CH_LAST_FRAME;
+	else
+		cmd = S5P_FIMV_CH_FRAME_START;
+	mfc_write(dev, ((cmd & S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT)
+				| (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+
+	return 0;
+}
+
+static int s5p_mfc_get_new_ctx(struct s5p_mfc_dev *dev)
+{
+	unsigned long flags;
+	int new_ctx;
+	int cnt;
+
+	spin_lock_irqsave(&dev->condlock, flags);
+	new_ctx = (dev->curr_ctx + 1) % MFC_NUM_CONTEXTS;
+	cnt = 0;
+	while (!test_bit(new_ctx, &dev->ctx_work_bits)) {
+		new_ctx = (new_ctx + 1) % MFC_NUM_CONTEXTS;
+		if (++cnt > MFC_NUM_CONTEXTS) {
+			/* No contexts to run */
+			spin_unlock_irqrestore(&dev->condlock, flags);
+			return -EAGAIN;
+		}
+	}
+	spin_unlock_irqrestore(&dev->condlock, flags);
+	return new_ctx;
+}
+
+static void s5p_mfc_run_res_change(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	s5p_mfc_set_dec_stream_buffer_v5(ctx, 0, 0, 0);
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_clean_ctx_int_flags(ctx);
+	s5p_mfc_decode_one_frame_v5(ctx, MFC_DEC_RES_CHANGE);
+}
+
+static int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx, int last_frame)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf *temp_vb;
+	unsigned long flags;
+	unsigned int index;
+
+	spin_lock_irqsave(&dev->irqlock, flags);
+	/* Frames are being decoded */
+	if (list_empty(&ctx->src_queue)) {
+		mfc_debug(2, "No src buffers\n");
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+		return -EAGAIN;
+	}
+	/* Get the next source buffer */
+	temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+	temp_vb->flags |= MFC_BUF_FLAG_USED;
+	s5p_mfc_set_dec_stream_buffer_v5(ctx,
+		vb2_dma_contig_plane_dma_addr(temp_vb->b, 0),
+		ctx->consumed_stream, temp_vb->b->v4l2_planes[0].bytesused);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+	index = temp_vb->b->v4l2_buf.index;
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_clean_ctx_int_flags(ctx);
+	if (temp_vb->b->v4l2_planes[0].bytesused == 0) {
+		last_frame = MFC_DEC_LAST_FRAME;
+		mfc_debug(2, "Setting ctx->state to FINISHING\n");
+		ctx->state = MFCINST_FINISHING;
+	}
+	s5p_mfc_decode_one_frame_v5(ctx, last_frame);
+	return 0;
+}
+
+static int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long flags;
+	struct s5p_mfc_buf *dst_mb;
+	struct s5p_mfc_buf *src_mb;
+	unsigned long src_y_addr, src_c_addr, dst_addr;
+	unsigned int dst_size;
+
+	spin_lock_irqsave(&dev->irqlock, flags);
+	if (list_empty(&ctx->src_queue) && ctx->state != MFCINST_FINISHING) {
+		mfc_debug(2, "no src buffers\n");
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+		return -EAGAIN;
+	}
+	if (list_empty(&ctx->dst_queue)) {
+		mfc_debug(2, "no dst buffers\n");
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+		return -EAGAIN;
+	}
+	if (list_empty(&ctx->src_queue)) {
+		/* send null frame */
+		s5p_mfc_set_enc_frame_buffer_v5(ctx, dev->bank2, dev->bank2);
+		src_mb = NULL;
+	} else {
+		src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf,
+									list);
+		src_mb->flags |= MFC_BUF_FLAG_USED;
+		if (src_mb->b->v4l2_planes[0].bytesused == 0) {
+			/* send null frame */
+			s5p_mfc_set_enc_frame_buffer_v5(ctx, dev->bank2,
+								dev->bank2);
+			ctx->state = MFCINST_FINISHING;
+		} else {
+			src_y_addr = vb2_dma_contig_plane_dma_addr(src_mb->b,
+									0);
+			src_c_addr = vb2_dma_contig_plane_dma_addr(src_mb->b,
+									1);
+			s5p_mfc_set_enc_frame_buffer_v5(ctx, src_y_addr,
+								src_c_addr);
+			if (src_mb->flags & MFC_BUF_FLAG_EOS)
+				ctx->state = MFCINST_FINISHING;
+		}
+	}
+	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+	dst_mb->flags |= MFC_BUF_FLAG_USED;
+	dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
+	dst_size = vb2_plane_size(dst_mb->b, 0);
+	s5p_mfc_set_enc_stream_buffer_v5(ctx, dst_addr, dst_size);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_clean_ctx_int_flags(ctx);
+	mfc_debug(2, "encoding buffer with index=%d state=%d",
+			src_mb ? src_mb->b->v4l2_buf.index : -1, ctx->state);
+	s5p_mfc_encode_one_frame_v5(ctx);
+	return 0;
+}
+
+static void s5p_mfc_run_init_dec(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long flags;
+	struct s5p_mfc_buf *temp_vb;
+
+	/* Initializing decoding - parsing header */
+	spin_lock_irqsave(&dev->irqlock, flags);
+	mfc_debug(2, "Preparing to init decoding\n");
+	temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+	s5p_mfc_set_dec_desc_buffer(ctx);
+	mfc_debug(2, "Header size: %d\n", temp_vb->b->v4l2_planes[0].bytesused);
+	s5p_mfc_set_dec_stream_buffer_v5(ctx,
+				vb2_dma_contig_plane_dma_addr(temp_vb->b, 0),
+				0, temp_vb->b->v4l2_planes[0].bytesused);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_clean_ctx_int_flags(ctx);
+	s5p_mfc_init_decode_v5(ctx);
+}
+
+static void s5p_mfc_run_init_enc(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long flags;
+	struct s5p_mfc_buf *dst_mb;
+	unsigned long dst_addr;
+	unsigned int dst_size;
+
+	s5p_mfc_set_enc_ref_buffer_v5(ctx);
+	spin_lock_irqsave(&dev->irqlock, flags);
+	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+	dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
+	dst_size = vb2_plane_size(dst_mb->b, 0);
+	s5p_mfc_set_enc_stream_buffer_v5(ctx, dst_addr, dst_size);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_clean_ctx_int_flags(ctx);
+	s5p_mfc_init_encode_v5(ctx);
+}
+
+static int s5p_mfc_run_init_dec_buffers(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long flags;
+	struct s5p_mfc_buf *temp_vb;
+	int ret;
+
+	/*
+	 * Header was parsed now starting processing
+	 * First set the output frame buffers
+	 */
+	if (ctx->capture_state != QUEUE_BUFS_MMAPED) {
+		mfc_err("It seems that not all destionation buffers were "
+			"mmaped\nMFC requires that all destination are mmaped "
+			"before starting processing\n");
+		return -EAGAIN;
+	}
+	spin_lock_irqsave(&dev->irqlock, flags);
+	if (list_empty(&ctx->src_queue)) {
+		mfc_err("Header has been deallocated in the middle of"
+			" initialization\n");
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+		return -EIO;
+	}
+	temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+	mfc_debug(2, "Header size: %d\n", temp_vb->b->v4l2_planes[0].bytesused);
+	s5p_mfc_set_dec_stream_buffer_v5(ctx,
+				vb2_dma_contig_plane_dma_addr(temp_vb->b, 0),
+				0, temp_vb->b->v4l2_planes[0].bytesused);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_clean_ctx_int_flags(ctx);
+	ret = s5p_mfc_set_dec_frame_buffer_v5(ctx);
+	if (ret) {
+		mfc_err("Failed to alloc frame mem\n");
+		ctx->state = MFCINST_ERROR;
+	}
+	return ret;
+}
+
+/* Try running an operation on hardware */
+void s5p_mfc_try_run_v5(struct s5p_mfc_dev *dev)
+{
+	struct s5p_mfc_ctx *ctx;
+	int new_ctx;
+	unsigned int ret = 0;
+
+	if (test_bit(0, &dev->enter_suspend)) {
+		mfc_debug(1, "Entering suspend so do not schedule any jobs\n");
+		return;
+	}
+	/* Check whether hardware is not running */
+	if (test_and_set_bit(0, &dev->hw_lock) != 0) {
+		/* This is perfectly ok, the scheduled ctx should wait */
+		mfc_debug(1, "Couldn't lock HW\n");
+		return;
+	}
+	/* Choose the context to run */
+	new_ctx = s5p_mfc_get_new_ctx(dev);
+	if (new_ctx < 0) {
+		/* No contexts to run */
+		if (test_and_clear_bit(0, &dev->hw_lock) == 0) {
+			mfc_err("Failed to unlock hardware\n");
+			return;
+		}
+		mfc_debug(1, "No ctx is scheduled to be run\n");
+		return;
+	}
+	ctx = dev->ctx[new_ctx];
+	/* Got context to run in ctx */
+	/*
+	 * Last frame has already been sent to MFC.
+	 * Now obtaining frames from MFC buffer
+	 */
+	s5p_mfc_clock_on();
+	if (ctx->type == MFCINST_DECODER) {
+		s5p_mfc_set_dec_desc_buffer(ctx);
+		switch (ctx->state) {
+		case MFCINST_FINISHING:
+			s5p_mfc_run_dec_frame(ctx, MFC_DEC_LAST_FRAME);
+			break;
+		case MFCINST_RUNNING:
+			ret = s5p_mfc_run_dec_frame(ctx, MFC_DEC_FRAME);
+			break;
+		case MFCINST_INIT:
+			s5p_mfc_clean_ctx_int_flags(ctx);
+			ret = s5p_mfc_hw_call(dev->mfc_cmds, open_inst_cmd,
+					ctx);
+			break;
+		case MFCINST_RETURN_INST:
+			s5p_mfc_clean_ctx_int_flags(ctx);
+			ret = s5p_mfc_hw_call(dev->mfc_cmds, close_inst_cmd,
+					ctx);
+			break;
+		case MFCINST_GOT_INST:
+			s5p_mfc_run_init_dec(ctx);
+			break;
+		case MFCINST_HEAD_PARSED:
+			ret = s5p_mfc_run_init_dec_buffers(ctx);
+			mfc_debug(1, "head parsed\n");
+			break;
+		case MFCINST_RES_CHANGE_INIT:
+			s5p_mfc_run_res_change(ctx);
+			break;
+		case MFCINST_RES_CHANGE_FLUSH:
+			s5p_mfc_run_dec_frame(ctx, MFC_DEC_FRAME);
+			break;
+		case MFCINST_RES_CHANGE_END:
+			mfc_debug(2, "Finished remaining frames after resolution change\n");
+			ctx->capture_state = QUEUE_FREE;
+			mfc_debug(2, "Will re-init the codec\n");
+			s5p_mfc_run_init_dec(ctx);
+			break;
+		default:
+			ret = -EAGAIN;
+		}
+	} else if (ctx->type == MFCINST_ENCODER) {
+		switch (ctx->state) {
+		case MFCINST_FINISHING:
+		case MFCINST_RUNNING:
+			ret = s5p_mfc_run_enc_frame(ctx);
+			break;
+		case MFCINST_INIT:
+			s5p_mfc_clean_ctx_int_flags(ctx);
+			ret = s5p_mfc_hw_call(dev->mfc_cmds, open_inst_cmd,
+					ctx);
+			break;
+		case MFCINST_RETURN_INST:
+			s5p_mfc_clean_ctx_int_flags(ctx);
+			ret = s5p_mfc_hw_call(dev->mfc_cmds, close_inst_cmd,
+					ctx);
+			break;
+		case MFCINST_GOT_INST:
+			s5p_mfc_run_init_enc(ctx);
+			break;
+		default:
+			ret = -EAGAIN;
+		}
+	} else {
+		mfc_err("Invalid context type: %d\n", ctx->type);
+		ret = -EAGAIN;
+	}
+
+	if (ret) {
+		/* Free hardware lock */
+		if (test_and_clear_bit(0, &dev->hw_lock) == 0)
+			mfc_err("Failed to unlock hardware\n");
+
+		/* This is in deed imporant, as no operation has been
+		 * scheduled, reduce the clock count as no one will
+		 * ever do this, because no interrupt related to this try_run
+		 * will ever come from hardware. */
+		s5p_mfc_clock_off();
+	}
+}
+
+
+void s5p_mfc_cleanup_queue_v5(struct list_head *lh, struct vb2_queue *vq)
+{
+	struct s5p_mfc_buf *b;
+	int i;
+
+	while (!list_empty(lh)) {
+		b = list_entry(lh->next, struct s5p_mfc_buf, list);
+		for (i = 0; i < b->b->num_planes; i++)
+			vb2_set_plane_payload(b->b, i, 0);
+		vb2_buffer_done(b->b, VB2_BUF_STATE_ERROR);
+		list_del(&b->list);
+	}
+}
+
+void s5p_mfc_clear_int_flags_v5(struct s5p_mfc_dev *dev)
+{
+	mfc_write(dev, 0, S5P_FIMV_RISC_HOST_INT);
+	mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD);
+	mfc_write(dev, 0xffff, S5P_FIMV_SI_RTN_CHID);
+}
+
+int s5p_mfc_get_dspl_y_adr_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_SI_DISPLAY_Y_ADR) << MFC_OFFSET_SHIFT;
+}
+
+int s5p_mfc_get_dec_y_adr_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_SI_DECODE_Y_ADR) << MFC_OFFSET_SHIFT;
+}
+
+int s5p_mfc_get_dspl_status_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_SI_DISPLAY_STATUS);
+}
+
+int s5p_mfc_get_dec_status_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_SI_DECODE_STATUS);
+}
+
+int s5p_mfc_get_dec_frame_type_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_DECODE_FRAME_TYPE) &
+		S5P_FIMV_DECODE_FRAME_MASK;
+}
+
+int s5p_mfc_get_disp_frame_type_v5(struct s5p_mfc_ctx *ctx)
+{
+	return (s5p_mfc_read_info_v5(ctx, DISP_PIC_FRAME_TYPE) >>
+			S5P_FIMV_SHARED_DISP_FRAME_TYPE_SHIFT) &
+			S5P_FIMV_DECODE_FRAME_MASK;
+}
+
+int s5p_mfc_get_consumed_stream_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_SI_CONSUMED_BYTES);
+}
+
+int s5p_mfc_get_int_reason_v5(struct s5p_mfc_dev *dev)
+{
+	int reason;
+	reason = mfc_read(dev, S5P_FIMV_RISC2HOST_CMD) &
+		S5P_FIMV_RISC2HOST_CMD_MASK;
+	switch (reason) {
+	case S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET:
+		reason = S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_CLOSE_INSTANCE_RET:
+		reason = S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_SEQ_DONE_RET:
+		reason = S5P_MFC_R2H_CMD_SEQ_DONE_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_FRAME_DONE_RET:
+		reason = S5P_MFC_R2H_CMD_FRAME_DONE_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_SLICE_DONE_RET:
+		reason = S5P_MFC_R2H_CMD_SLICE_DONE_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_SYS_INIT_RET:
+		reason = S5P_MFC_R2H_CMD_SYS_INIT_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_FW_STATUS_RET:
+		reason = S5P_MFC_R2H_CMD_FW_STATUS_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_SLEEP_RET:
+		reason = S5P_MFC_R2H_CMD_SLEEP_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_WAKEUP_RET:
+		reason = S5P_MFC_R2H_CMD_WAKEUP_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_INIT_BUFFERS_RET:
+		reason = S5P_MFC_R2H_CMD_INIT_BUFFERS_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_ENC_COMPLETE_RET:
+		reason = S5P_MFC_R2H_CMD_COMPLETE_SEQ_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_ERR_RET:
+		reason = S5P_MFC_R2H_CMD_ERR_RET;
+		break;
+	default:
+		reason = S5P_MFC_R2H_CMD_EMPTY;
+	};
+	return reason;
+}
+
+int s5p_mfc_get_int_err_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_RISC2HOST_ARG2);
+}
+
+int s5p_mfc_err_dec_v5(unsigned int err)
+{
+	return (err & S5P_FIMV_ERR_DEC_MASK) >> S5P_FIMV_ERR_DEC_SHIFT;
+}
+
+int s5p_mfc_err_dspl_v5(unsigned int err)
+{
+	return (err & S5P_FIMV_ERR_DSPL_MASK) >> S5P_FIMV_ERR_DSPL_SHIFT;
+}
+
+int s5p_mfc_get_img_width_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_SI_HRESOL);
+}
+
+int s5p_mfc_get_img_height_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_SI_VRESOL);
+}
+
+int s5p_mfc_get_dpb_count_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_SI_BUF_NUMBER);
+}
+
+int s5p_mfc_get_mv_count_v5(struct s5p_mfc_dev *dev)
+{
+	/* NOP */
+	return -1;
+}
+
+int s5p_mfc_get_inst_no_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_RISC2HOST_ARG1);
+}
+
+int s5p_mfc_get_enc_strm_size_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_ENC_SI_STRM_SIZE);
+}
+
+int s5p_mfc_get_enc_slice_type_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_ENC_SI_SLICE_TYPE);
+}
+
+int s5p_mfc_get_enc_dpb_count_v5(struct s5p_mfc_dev *dev)
+{
+	return -1;
+}
+
+int s5p_mfc_get_enc_pic_count_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_ENC_SI_PIC_CNT);
+}
+
+int s5p_mfc_get_sei_avail_status_v5(struct s5p_mfc_ctx *ctx)
+{
+	return s5p_mfc_read_info_v5(ctx, FRAME_PACK_SEI_AVAIL);
+}
+
+int s5p_mfc_get_mvc_num_views_v5(struct s5p_mfc_dev *dev)
+{
+	return -1;
+}
+
+int s5p_mfc_get_mvc_view_id_v5(struct s5p_mfc_dev *dev)
+{
+	return -1;
+}
+
+unsigned int s5p_mfc_get_pic_type_top_v5(struct s5p_mfc_ctx *ctx)
+{
+	return s5p_mfc_read_info_v5(ctx, PIC_TIME_TOP);
+}
+
+unsigned int s5p_mfc_get_pic_type_bot_v5(struct s5p_mfc_ctx *ctx)
+{
+	return s5p_mfc_read_info_v5(ctx, PIC_TIME_BOT);
+}
+
+unsigned int s5p_mfc_get_crop_info_h_v5(struct s5p_mfc_ctx *ctx)
+{
+	return s5p_mfc_read_info_v5(ctx, CROP_INFO_H);
+}
+
+unsigned int s5p_mfc_get_crop_info_v_v5(struct s5p_mfc_ctx *ctx)
+{
+	return s5p_mfc_read_info_v5(ctx, CROP_INFO_V);
+}
+
+/* Initialize opr function pointers for MFC v5 */
+static struct s5p_mfc_hw_ops s5p_mfc_ops_v5 = {
+	.alloc_dec_temp_buffers = s5p_mfc_alloc_dec_temp_buffers_v5,
+	.release_dec_desc_buffer = s5p_mfc_release_dec_desc_buffer_v5,
+	.alloc_codec_buffers = s5p_mfc_alloc_codec_buffers_v5,
+	.release_codec_buffers = s5p_mfc_release_codec_buffers_v5,
+	.alloc_instance_buffer = s5p_mfc_alloc_instance_buffer_v5,
+	.release_instance_buffer = s5p_mfc_release_instance_buffer_v5,
+	.alloc_dev_context_buffer = s5p_mfc_alloc_dev_context_buffer_v5,
+	.release_dev_context_buffer = s5p_mfc_release_dev_context_buffer_v5,
+	.dec_calc_dpb_size = s5p_mfc_dec_calc_dpb_size_v5,
+	.enc_calc_src_size = s5p_mfc_enc_calc_src_size_v5,
+	.set_dec_stream_buffer = s5p_mfc_set_dec_stream_buffer_v5,
+	.set_dec_frame_buffer = s5p_mfc_set_dec_frame_buffer_v5,
+	.set_enc_stream_buffer = s5p_mfc_set_enc_stream_buffer_v5,
+	.set_enc_frame_buffer = s5p_mfc_set_enc_frame_buffer_v5,
+	.get_enc_frame_buffer = s5p_mfc_get_enc_frame_buffer_v5,
+	.set_enc_ref_buffer = s5p_mfc_set_enc_ref_buffer_v5,
+	.init_decode = s5p_mfc_init_decode_v5,
+	.init_encode = s5p_mfc_init_encode_v5,
+	.encode_one_frame = s5p_mfc_encode_one_frame_v5,
+	.try_run = s5p_mfc_try_run_v5,
+	.cleanup_queue = s5p_mfc_cleanup_queue_v5,
+	.clear_int_flags = s5p_mfc_clear_int_flags_v5,
+	.write_info = s5p_mfc_write_info_v5,
+	.read_info = s5p_mfc_read_info_v5,
+	.get_dspl_y_adr = s5p_mfc_get_dspl_y_adr_v5,
+	.get_dec_y_adr = s5p_mfc_get_dec_y_adr_v5,
+	.get_dspl_status = s5p_mfc_get_dspl_status_v5,
+	.get_dec_status = s5p_mfc_get_dec_status_v5,
+	.get_dec_frame_type = s5p_mfc_get_dec_frame_type_v5,
+	.get_disp_frame_type = s5p_mfc_get_disp_frame_type_v5,
+	.get_consumed_stream = s5p_mfc_get_consumed_stream_v5,
+	.get_int_reason = s5p_mfc_get_int_reason_v5,
+	.get_int_err = s5p_mfc_get_int_err_v5,
+	.err_dec = s5p_mfc_err_dec_v5,
+	.err_dspl = s5p_mfc_err_dspl_v5,
+	.get_img_width = s5p_mfc_get_img_width_v5,
+	.get_img_height = s5p_mfc_get_img_height_v5,
+	.get_dpb_count = s5p_mfc_get_dpb_count_v5,
+	.get_mv_count = s5p_mfc_get_mv_count_v5,
+	.get_inst_no = s5p_mfc_get_inst_no_v5,
+	.get_enc_strm_size = s5p_mfc_get_enc_strm_size_v5,
+	.get_enc_slice_type = s5p_mfc_get_enc_slice_type_v5,
+	.get_enc_dpb_count = s5p_mfc_get_enc_dpb_count_v5,
+	.get_enc_pic_count = s5p_mfc_get_enc_pic_count_v5,
+	.get_sei_avail_status = s5p_mfc_get_sei_avail_status_v5,
+	.get_mvc_num_views = s5p_mfc_get_mvc_num_views_v5,
+	.get_mvc_view_id = s5p_mfc_get_mvc_view_id_v5,
+	.get_pic_type_top = s5p_mfc_get_pic_type_top_v5,
+	.get_pic_type_bot = s5p_mfc_get_pic_type_bot_v5,
+	.get_crop_info_h = s5p_mfc_get_crop_info_h_v5,
+	.get_crop_info_v = s5p_mfc_get_crop_info_v_v5,
+};
+
+struct s5p_mfc_hw_ops *s5p_mfc_init_hw_ops_v5(void)
+{
+	return &s5p_mfc_ops_v5;
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_shm.h b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.h
index 416ebd7ba35..ffee39a127d 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_shm.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.h
@@ -1,17 +1,22 @@
 /*
- * linux/drivers/media/platform/s5p-mfc/s5p_mfc_shm.h
+ * drivers/media/platform/samsung/mfc5/s5p_mfc_opr_v5.h
  *
- * Copyright (c) 2011 Samsung Electronics Co., Ltd.
- *		http://www.samsung.com/
+ * Header file for Samsung MFC (Multi Function Codec - FIMV) driver
+ * Contains declarations of hw related functions.
+ *
+ * Kamil Debski, Copyright (C) 2011 Samsung Electronics
+ * http://www.samsung.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.
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
  */
 
-#ifndef S5P_MFC_SHM_H_
-#define S5P_MFC_SHM_H_
+#ifndef S5P_MFC_OPR_V5_H_
+#define S5P_MFC_OPR_V5_H_
+
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_opr.h"
 
 enum MFC_SHM_OFS {
 	EXTENEDED_DECODE_STATUS	= 0x00,	/* D */
@@ -71,20 +76,10 @@ enum MFC_SHM_OFS {
 	DBG_HISTORY_INPUT1	= 0xD4,	/* C */
 	DBG_HISTORY_OUTPUT	= 0xD8,	/* C */
 	HIERARCHICAL_P_QP	= 0xE0, /* E, H.264 */
+	FRAME_PACK_SEI_ENABLE	= 0x168, /* C */
+	FRAME_PACK_SEI_AVAIL	= 0x16c, /* D */
+	FRAME_PACK_SEI_INFO	= 0x17c, /* E */
 };
 
-int s5p_mfc_init_shm(struct s5p_mfc_ctx *ctx);
-
-#define s5p_mfc_write_shm(ctx, x, ofs)		\
-	do {					\
-		writel(x, (ctx->shm + ofs));	\
-		wmb();				\
-	} while (0)
-
-static inline u32 s5p_mfc_read_shm(struct s5p_mfc_ctx *ctx, unsigned int ofs)
-{
-	rmb();
-	return readl(ctx->shm + ofs);
-}
-
-#endif /* S5P_MFC_SHM_H_ */
+struct s5p_mfc_hw_ops *s5p_mfc_init_hw_ops_v5(void);
+#endif /* S5P_MFC_OPR_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
new file mode 100644
index 00000000000..50b5bee3c44
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
@@ -0,0 +1,1956 @@
+/*
+ * drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
+ *
+ * Samsung MFC (Multi Function Codec - FIMV) driver
+ * This file contains hw related functions.
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.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.
+ */
+
+#undef DEBUG
+
+#include <linux/delay.h>
+#include <linux/mm.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
+#include <linux/firmware.h>
+#include <linux/err.h>
+#include <linux/sched.h>
+#include <linux/dma-mapping.h>
+
+#include <asm/cacheflush.h>
+
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_pm.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_opr.h"
+#include "s5p_mfc_opr_v6.h"
+
+/* #define S5P_MFC_DEBUG_REGWRITE  */
+#ifdef S5P_MFC_DEBUG_REGWRITE
+#undef writel
+#define writel(v, r)							\
+	do {								\
+		pr_err("MFCWRITE(%p): %08x\n", r, (unsigned int)v);	\
+	__raw_writel(v, r);						\
+	} while (0)
+#endif /* S5P_MFC_DEBUG_REGWRITE */
+
+#define READL(offset)		readl(dev->regs_base + (offset))
+#define WRITEL(data, offset)	writel((data), dev->regs_base + (offset))
+#define OFFSETA(x)		(((x) - dev->port_a) >> S5P_FIMV_MEM_OFFSET)
+#define OFFSETB(x)		(((x) - dev->port_b) >> S5P_FIMV_MEM_OFFSET)
+
+/* Allocate temporary buffers for decoding */
+int s5p_mfc_alloc_dec_temp_buffers_v6(struct s5p_mfc_ctx *ctx)
+{
+	/* NOP */
+
+	return 0;
+}
+
+/* Release temproary buffers for decoding */
+void s5p_mfc_release_dec_desc_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+	/* NOP */
+}
+
+int s5p_mfc_get_dec_status_v6(struct s5p_mfc_dev *dev)
+{
+	/* NOP */
+	return -1;
+}
+
+/* Allocate codec buffers */
+int s5p_mfc_alloc_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned int mb_width, mb_height;
+
+	mb_width = MB_WIDTH(ctx->img_width);
+	mb_height = MB_HEIGHT(ctx->img_height);
+
+	if (ctx->type == MFCINST_DECODER) {
+		mfc_debug(2, "Luma size:%d Chroma size:%d MV size:%d\n",
+			  ctx->luma_size, ctx->chroma_size, ctx->mv_size);
+		mfc_debug(2, "Totals bufs: %d\n", ctx->total_dpb_count);
+	} else if (ctx->type == MFCINST_ENCODER) {
+		ctx->tmv_buffer_size = S5P_FIMV_NUM_TMV_BUFFERS_V6 *
+			ALIGN(S5P_FIMV_TMV_BUFFER_SIZE_V6(mb_width, mb_height),
+			S5P_FIMV_TMV_BUFFER_ALIGN_V6);
+		ctx->luma_dpb_size = ALIGN((mb_width * mb_height) *
+				S5P_FIMV_LUMA_MB_TO_PIXEL_V6,
+				S5P_FIMV_LUMA_DPB_BUFFER_ALIGN_V6);
+		ctx->chroma_dpb_size = ALIGN((mb_width * mb_height) *
+				S5P_FIMV_CHROMA_MB_TO_PIXEL_V6,
+				S5P_FIMV_CHROMA_DPB_BUFFER_ALIGN_V6);
+		ctx->me_buffer_size = ALIGN(S5P_FIMV_ME_BUFFER_SIZE_V6(
+					ctx->img_width, ctx->img_height,
+					mb_width, mb_height),
+					S5P_FIMV_ME_BUFFER_ALIGN_V6);
+
+		mfc_debug(2, "recon luma size: %d chroma size: %d\n",
+			  ctx->luma_dpb_size, ctx->chroma_dpb_size);
+	} else {
+		return -EINVAL;
+	}
+
+	/* Codecs have different memory requirements */
+	switch (ctx->codec_mode) {
+	case S5P_MFC_CODEC_H264_DEC:
+	case S5P_MFC_CODEC_H264_MVC_DEC:
+		ctx->scratch_buf_size =
+			S5P_FIMV_SCRATCH_BUF_SIZE_H264_DEC_V6(
+					mb_width,
+					mb_height);
+		ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+				S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+		ctx->bank1_size =
+			ctx->scratch_buf_size +
+			(ctx->mv_count * ctx->mv_size);
+		break;
+	case S5P_MFC_CODEC_MPEG4_DEC:
+		ctx->scratch_buf_size =
+			S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_DEC_V6(
+					mb_width,
+					mb_height);
+		ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+				S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+		ctx->bank1_size = ctx->scratch_buf_size;
+		break;
+	case S5P_MFC_CODEC_VC1RCV_DEC:
+	case S5P_MFC_CODEC_VC1_DEC:
+		ctx->scratch_buf_size =
+			S5P_FIMV_SCRATCH_BUF_SIZE_VC1_DEC_V6(
+					mb_width,
+					mb_height);
+		ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+				S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+		ctx->bank1_size = ctx->scratch_buf_size;
+		break;
+	case S5P_MFC_CODEC_MPEG2_DEC:
+		ctx->bank1_size = 0;
+		ctx->bank2_size = 0;
+		break;
+	case S5P_MFC_CODEC_H263_DEC:
+		ctx->scratch_buf_size =
+			S5P_FIMV_SCRATCH_BUF_SIZE_H263_DEC_V6(
+					mb_width,
+					mb_height);
+		ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+				S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+		ctx->bank1_size = ctx->scratch_buf_size;
+		break;
+	case S5P_MFC_CODEC_VP8_DEC:
+		ctx->scratch_buf_size =
+			S5P_FIMV_SCRATCH_BUF_SIZE_VP8_DEC_V6(
+					mb_width,
+					mb_height);
+		ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+				S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+		ctx->bank1_size = ctx->scratch_buf_size;
+		break;
+	case S5P_MFC_CODEC_H264_ENC:
+		ctx->scratch_buf_size =
+			S5P_FIMV_SCRATCH_BUF_SIZE_H264_ENC_V6(
+					mb_width,
+					mb_height);
+		ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+				S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+		ctx->bank1_size =
+			ctx->scratch_buf_size + ctx->tmv_buffer_size +
+			(ctx->dpb_count * (ctx->luma_dpb_size +
+			ctx->chroma_dpb_size + ctx->me_buffer_size));
+		ctx->bank2_size = 0;
+		break;
+	case S5P_MFC_CODEC_MPEG4_ENC:
+	case S5P_MFC_CODEC_H263_ENC:
+		ctx->scratch_buf_size =
+			S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_ENC_V6(
+					mb_width,
+					mb_height);
+		ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+				S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+		ctx->bank1_size =
+			ctx->scratch_buf_size + ctx->tmv_buffer_size +
+			(ctx->dpb_count * (ctx->luma_dpb_size +
+			ctx->chroma_dpb_size + ctx->me_buffer_size));
+		ctx->bank2_size = 0;
+		break;
+	default:
+		break;
+	}
+
+	/* Allocate only if memory from bank 1 is necessary */
+	if (ctx->bank1_size > 0) {
+		ctx->bank1_buf = vb2_dma_contig_memops.alloc(
+		dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_size);
+		if (IS_ERR(ctx->bank1_buf)) {
+			ctx->bank1_buf = 0;
+			pr_err("Buf alloc for decoding failed (port A)\n");
+			return -ENOMEM;
+		}
+		ctx->bank1_phys = s5p_mfc_mem_cookie(
+			dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->bank1_buf);
+		BUG_ON(ctx->bank1_phys & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+	}
+
+	return 0;
+}
+
+/* Release buffers allocated for codec */
+void s5p_mfc_release_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
+{
+	if (ctx->bank1_buf) {
+		vb2_dma_contig_memops.put(ctx->bank1_buf);
+		ctx->bank1_buf = 0;
+		ctx->bank1_phys = 0;
+		ctx->bank1_size = 0;
+	}
+}
+
+/* Allocate memory for instance data buffer */
+int s5p_mfc_alloc_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf_size_v6 *buf_size = dev->variant->buf_size->priv;
+
+	mfc_debug_enter();
+
+	switch (ctx->codec_mode) {
+	case S5P_MFC_CODEC_H264_DEC:
+	case S5P_MFC_CODEC_H264_MVC_DEC:
+		ctx->ctx.size = buf_size->h264_dec_ctx;
+		break;
+	case S5P_MFC_CODEC_MPEG4_DEC:
+	case S5P_MFC_CODEC_H263_DEC:
+	case S5P_MFC_CODEC_VC1RCV_DEC:
+	case S5P_MFC_CODEC_VC1_DEC:
+	case S5P_MFC_CODEC_MPEG2_DEC:
+	case S5P_MFC_CODEC_VP8_DEC:
+		ctx->ctx.size = buf_size->other_dec_ctx;
+		break;
+	case S5P_MFC_CODEC_H264_ENC:
+		ctx->ctx.size = buf_size->h264_enc_ctx;
+		break;
+	case S5P_MFC_CODEC_MPEG4_ENC:
+	case S5P_MFC_CODEC_H263_ENC:
+		ctx->ctx.size = buf_size->other_enc_ctx;
+		break;
+	default:
+		ctx->ctx.size = 0;
+		mfc_err("Codec type(%d) should be checked!\n", ctx->codec_mode);
+		break;
+	}
+
+	ctx->ctx.alloc = vb2_dma_contig_memops.alloc(
+		dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx.size);
+	if (IS_ERR(ctx->ctx.alloc)) {
+		mfc_err("Allocating context buffer failed.\n");
+		return PTR_ERR(ctx->ctx.alloc);
+	}
+
+	ctx->ctx.dma = s5p_mfc_mem_cookie(
+		dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], ctx->ctx.alloc);
+
+	ctx->ctx.virt = vb2_dma_contig_memops.vaddr(ctx->ctx.alloc);
+	if (!ctx->ctx.virt) {
+		vb2_dma_contig_memops.put(ctx->ctx.alloc);
+		ctx->ctx.alloc = NULL;
+		ctx->ctx.dma = 0;
+		ctx->ctx.virt = NULL;
+
+		mfc_err("Remapping context buffer failed.\n");
+		return -ENOMEM;
+	}
+
+	memset(ctx->ctx.virt, 0, ctx->ctx.size);
+	wmb();
+
+	mfc_debug_leave();
+
+	return 0;
+}
+
+/* Release instance buffer */
+void s5p_mfc_release_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+	mfc_debug_enter();
+
+	if (ctx->ctx.alloc) {
+		vb2_dma_contig_memops.put(ctx->ctx.alloc);
+		ctx->ctx.alloc = NULL;
+		ctx->ctx.dma = 0;
+		ctx->ctx.virt = NULL;
+	}
+
+	mfc_debug_leave();
+}
+
+/* Allocate context buffers for SYS_INIT */
+int s5p_mfc_alloc_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
+{
+	struct s5p_mfc_buf_size_v6 *buf_size = dev->variant->buf_size->priv;
+
+	mfc_debug_enter();
+
+	dev->ctx_buf.alloc = vb2_dma_contig_memops.alloc(
+			dev->alloc_ctx[MFC_BANK1_ALLOC_CTX], buf_size->dev_ctx);
+	if (IS_ERR(dev->ctx_buf.alloc)) {
+		mfc_err("Allocating DESC buffer failed.\n");
+		return PTR_ERR(dev->ctx_buf.alloc);
+	}
+
+	dev->ctx_buf.dma = s5p_mfc_mem_cookie(
+			dev->alloc_ctx[MFC_BANK1_ALLOC_CTX],
+			dev->ctx_buf.alloc);
+
+	dev->ctx_buf.virt = vb2_dma_contig_memops.vaddr(dev->ctx_buf.alloc);
+	if (!dev->ctx_buf.virt) {
+		vb2_dma_contig_memops.put(dev->ctx_buf.alloc);
+		dev->ctx_buf.alloc = NULL;
+		dev->ctx_buf.dma = 0;
+
+		mfc_err("Remapping DESC buffer failed.\n");
+		return -ENOMEM;
+	}
+
+	memset(dev->ctx_buf.virt, 0, buf_size->dev_ctx);
+	wmb();
+
+	mfc_debug_leave();
+
+	return 0;
+}
+
+/* Release context buffers for SYS_INIT */
+void s5p_mfc_release_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
+{
+	if (dev->ctx_buf.alloc) {
+		vb2_dma_contig_memops.put(dev->ctx_buf.alloc);
+		dev->ctx_buf.alloc = NULL;
+		dev->ctx_buf.dma = 0;
+		dev->ctx_buf.virt = NULL;
+	}
+}
+
+static int calc_plane(int width, int height)
+{
+	int mbX, mbY;
+
+	mbX = DIV_ROUND_UP(width, S5P_FIMV_NUM_PIXELS_IN_MB_ROW_V6);
+	mbY = DIV_ROUND_UP(height, S5P_FIMV_NUM_PIXELS_IN_MB_COL_V6);
+
+	if (width * height < S5P_FIMV_MAX_FRAME_SIZE_V6)
+		mbY = (mbY + 1) / 2 * 2;
+
+	return (mbX * S5P_FIMV_NUM_PIXELS_IN_MB_COL_V6) *
+		(mbY * S5P_FIMV_NUM_PIXELS_IN_MB_ROW_V6);
+}
+
+void s5p_mfc_dec_calc_dpb_size_v6(struct s5p_mfc_ctx *ctx)
+{
+	ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN_V6);
+	ctx->buf_height = ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN_V6);
+	mfc_debug(2, "SEQ Done: Movie dimensions %dx%d,\n"
+			"buffer dimensions: %dx%d\n", ctx->img_width,
+			ctx->img_height, ctx->buf_width, ctx->buf_height);
+
+	ctx->luma_size = calc_plane(ctx->img_width, ctx->img_height);
+	ctx->chroma_size = calc_plane(ctx->img_width, (ctx->img_height >> 1));
+	if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC ||
+			ctx->codec_mode == S5P_MFC_CODEC_H264_MVC_DEC) {
+		ctx->mv_size = S5P_MFC_DEC_MV_SIZE_V6(ctx->img_width,
+				ctx->img_height);
+		ctx->mv_size = ALIGN(ctx->mv_size, 16);
+	} else {
+		ctx->mv_size = 0;
+	}
+}
+
+void s5p_mfc_enc_calc_src_size_v6(struct s5p_mfc_ctx *ctx)
+{
+	unsigned int mb_width, mb_height;
+
+	mb_width = MB_WIDTH(ctx->img_width);
+	mb_height = MB_HEIGHT(ctx->img_height);
+
+	ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12M_HALIGN_V6);
+	ctx->luma_size = ALIGN((mb_width * mb_height) * 256, 256);
+	ctx->chroma_size = ALIGN((mb_width * mb_height) * 128, 256);
+}
+
+/* Set registers for decoding stream buffer */
+int s5p_mfc_set_dec_stream_buffer_v6(struct s5p_mfc_ctx *ctx, int buf_addr,
+		  unsigned int start_num_byte, unsigned int strm_size)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf_size *buf_size = dev->variant->buf_size;
+
+	mfc_debug_enter();
+	mfc_debug(2, "inst_no: %d, buf_addr: 0x%08x,\n"
+		"buf_size: 0x%08x (%d)\n",
+		ctx->inst_no, buf_addr, strm_size, strm_size);
+	WRITEL(strm_size, S5P_FIMV_D_STREAM_DATA_SIZE_V6);
+	WRITEL(buf_addr, S5P_FIMV_D_CPB_BUFFER_ADDR_V6);
+	WRITEL(buf_size->cpb, S5P_FIMV_D_CPB_BUFFER_SIZE_V6);
+	WRITEL(start_num_byte, S5P_FIMV_D_CPB_BUFFER_OFFSET_V6);
+
+	mfc_debug_leave();
+	return 0;
+}
+
+/* Set decoding frame buffer */
+int s5p_mfc_set_dec_frame_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+	unsigned int frame_size, i;
+	unsigned int frame_size_ch, frame_size_mv;
+	struct s5p_mfc_dev *dev = ctx->dev;
+	size_t buf_addr1;
+	int buf_size1;
+	int align_gap;
+
+	buf_addr1 = ctx->bank1_phys;
+	buf_size1 = ctx->bank1_size;
+
+	mfc_debug(2, "Buf1: %p (%d)\n", (void *)buf_addr1, buf_size1);
+	mfc_debug(2, "Total DPB COUNT: %d\n", ctx->total_dpb_count);
+	mfc_debug(2, "Setting display delay to %d\n", ctx->display_delay);
+
+	WRITEL(ctx->total_dpb_count, S5P_FIMV_D_NUM_DPB_V6);
+	WRITEL(ctx->luma_size, S5P_FIMV_D_LUMA_DPB_SIZE_V6);
+	WRITEL(ctx->chroma_size, S5P_FIMV_D_CHROMA_DPB_SIZE_V6);
+
+	WRITEL(buf_addr1, S5P_FIMV_D_SCRATCH_BUFFER_ADDR_V6);
+	WRITEL(ctx->scratch_buf_size, S5P_FIMV_D_SCRATCH_BUFFER_SIZE_V6);
+	buf_addr1 += ctx->scratch_buf_size;
+	buf_size1 -= ctx->scratch_buf_size;
+
+	if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC ||
+			ctx->codec_mode == S5P_FIMV_CODEC_H264_MVC_DEC){
+		WRITEL(ctx->mv_size, S5P_FIMV_D_MV_BUFFER_SIZE_V6);
+		WRITEL(ctx->mv_count, S5P_FIMV_D_NUM_MV_V6);
+	}
+
+	frame_size = ctx->luma_size;
+	frame_size_ch = ctx->chroma_size;
+	frame_size_mv = ctx->mv_size;
+	mfc_debug(2, "Frame size: %d ch: %d mv: %d\n",
+			frame_size, frame_size_ch, frame_size_mv);
+
+	for (i = 0; i < ctx->total_dpb_count; i++) {
+		/* Bank2 */
+		mfc_debug(2, "Luma %d: %x\n", i,
+					ctx->dst_bufs[i].cookie.raw.luma);
+		WRITEL(ctx->dst_bufs[i].cookie.raw.luma,
+				S5P_FIMV_D_LUMA_DPB_V6 + i * 4);
+		mfc_debug(2, "\tChroma %d: %x\n", i,
+					ctx->dst_bufs[i].cookie.raw.chroma);
+		WRITEL(ctx->dst_bufs[i].cookie.raw.chroma,
+				S5P_FIMV_D_CHROMA_DPB_V6 + i * 4);
+	}
+	if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC ||
+			ctx->codec_mode == S5P_MFC_CODEC_H264_MVC_DEC) {
+		for (i = 0; i < ctx->mv_count; i++) {
+			/* To test alignment */
+			align_gap = buf_addr1;
+			buf_addr1 = ALIGN(buf_addr1, 16);
+			align_gap = buf_addr1 - align_gap;
+			buf_size1 -= align_gap;
+
+			mfc_debug(2, "\tBuf1: %x, size: %d\n",
+					buf_addr1, buf_size1);
+			WRITEL(buf_addr1, S5P_FIMV_D_MV_BUFFER_V6 + i * 4);
+			buf_addr1 += frame_size_mv;
+			buf_size1 -= frame_size_mv;
+		}
+	}
+
+	mfc_debug(2, "Buf1: %u, buf_size1: %d (frames %d)\n",
+			buf_addr1, buf_size1, ctx->total_dpb_count);
+	if (buf_size1 < 0) {
+		mfc_debug(2, "Not enough memory has been allocated.\n");
+		return -ENOMEM;
+	}
+
+	WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+			S5P_FIMV_CH_INIT_BUFS_V6, NULL);
+
+	mfc_debug(2, "After setting buffers.\n");
+	return 0;
+}
+
+/* Set registers for encoding stream buffer */
+int s5p_mfc_set_enc_stream_buffer_v6(struct s5p_mfc_ctx *ctx,
+		unsigned long addr, unsigned int size)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	WRITEL(addr, S5P_FIMV_E_STREAM_BUFFER_ADDR_V6); /* 16B align */
+	WRITEL(size, S5P_FIMV_E_STREAM_BUFFER_SIZE_V6);
+
+	mfc_debug(2, "stream buf addr: 0x%08lx, size: 0x%d",
+		addr, size);
+
+	return 0;
+}
+
+void s5p_mfc_set_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
+		unsigned long y_addr, unsigned long c_addr)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	WRITEL(y_addr, S5P_FIMV_E_SOURCE_LUMA_ADDR_V6); /* 256B align */
+	WRITEL(c_addr, S5P_FIMV_E_SOURCE_CHROMA_ADDR_V6);
+
+	mfc_debug(2, "enc src y buf addr: 0x%08lx", y_addr);
+	mfc_debug(2, "enc src c buf addr: 0x%08lx", c_addr);
+}
+
+void s5p_mfc_get_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
+		unsigned long *y_addr, unsigned long *c_addr)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long enc_recon_y_addr, enc_recon_c_addr;
+
+	*y_addr = READL(S5P_FIMV_E_ENCODED_SOURCE_LUMA_ADDR_V6);
+	*c_addr = READL(S5P_FIMV_E_ENCODED_SOURCE_CHROMA_ADDR_V6);
+
+	enc_recon_y_addr = READL(S5P_FIMV_E_RECON_LUMA_DPB_ADDR_V6);
+	enc_recon_c_addr = READL(S5P_FIMV_E_RECON_CHROMA_DPB_ADDR_V6);
+
+	mfc_debug(2, "recon y addr: 0x%08lx", enc_recon_y_addr);
+	mfc_debug(2, "recon c addr: 0x%08lx", enc_recon_c_addr);
+}
+
+/* Set encoding ref & codec buffer */
+int s5p_mfc_set_enc_ref_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	size_t buf_addr1, buf_size1;
+	int i;
+
+	mfc_debug_enter();
+
+	buf_addr1 = ctx->bank1_phys;
+	buf_size1 = ctx->bank1_size;
+
+	mfc_debug(2, "Buf1: %p (%d)\n", (void *)buf_addr1, buf_size1);
+
+	for (i = 0; i < ctx->dpb_count; i++) {
+		WRITEL(buf_addr1, S5P_FIMV_E_LUMA_DPB_V6 + (4 * i));
+		buf_addr1 += ctx->luma_dpb_size;
+		WRITEL(buf_addr1, S5P_FIMV_E_CHROMA_DPB_V6 + (4 * i));
+		buf_addr1 += ctx->chroma_dpb_size;
+		WRITEL(buf_addr1, S5P_FIMV_E_ME_BUFFER_V6 + (4 * i));
+		buf_addr1 += ctx->me_buffer_size;
+		buf_size1 -= (ctx->luma_dpb_size + ctx->chroma_dpb_size +
+			ctx->me_buffer_size);
+	}
+
+	WRITEL(buf_addr1, S5P_FIMV_E_SCRATCH_BUFFER_ADDR_V6);
+	WRITEL(ctx->scratch_buf_size, S5P_FIMV_E_SCRATCH_BUFFER_SIZE_V6);
+	buf_addr1 += ctx->scratch_buf_size;
+	buf_size1 -= ctx->scratch_buf_size;
+
+	WRITEL(buf_addr1, S5P_FIMV_E_TMV_BUFFER0_V6);
+	buf_addr1 += ctx->tmv_buffer_size >> 1;
+	WRITEL(buf_addr1, S5P_FIMV_E_TMV_BUFFER1_V6);
+	buf_addr1 += ctx->tmv_buffer_size >> 1;
+	buf_size1 -= ctx->tmv_buffer_size;
+
+	mfc_debug(2, "Buf1: %u, buf_size1: %d (ref frames %d)\n",
+			buf_addr1, buf_size1, ctx->dpb_count);
+	if (buf_size1 < 0) {
+		mfc_debug(2, "Not enough memory has been allocated.\n");
+		return -ENOMEM;
+	}
+
+	WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+			S5P_FIMV_CH_INIT_BUFS_V6, NULL);
+
+	mfc_debug_leave();
+
+	return 0;
+}
+
+static int s5p_mfc_set_slice_mode(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	/* multi-slice control */
+	/* multi-slice MB number or bit size */
+	WRITEL(ctx->slice_mode, S5P_FIMV_E_MSLICE_MODE_V6);
+	if (ctx->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB) {
+		WRITEL(ctx->slice_size.mb, S5P_FIMV_E_MSLICE_SIZE_MB_V6);
+	} else if (ctx->slice_mode ==
+			V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES) {
+		WRITEL(ctx->slice_size.bits, S5P_FIMV_E_MSLICE_SIZE_BITS_V6);
+	} else {
+		WRITEL(0x0, S5P_FIMV_E_MSLICE_SIZE_MB_V6);
+		WRITEL(0x0, S5P_FIMV_E_MSLICE_SIZE_BITS_V6);
+	}
+
+	return 0;
+}
+
+static int s5p_mfc_set_enc_params(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	unsigned int reg = 0;
+
+	mfc_debug_enter();
+
+	/* width */
+	WRITEL(ctx->img_width, S5P_FIMV_E_FRAME_WIDTH_V6); /* 16 align */
+	/* height */
+	WRITEL(ctx->img_height, S5P_FIMV_E_FRAME_HEIGHT_V6); /* 16 align */
+
+	/* cropped width */
+	WRITEL(ctx->img_width, S5P_FIMV_E_CROPPED_FRAME_WIDTH_V6);
+	/* cropped height */
+	WRITEL(ctx->img_height, S5P_FIMV_E_CROPPED_FRAME_HEIGHT_V6);
+	/* cropped offset */
+	WRITEL(0x0, S5P_FIMV_E_FRAME_CROP_OFFSET_V6);
+
+	/* pictype : IDR period */
+	reg = 0;
+	reg |= p->gop_size & 0xFFFF;
+	WRITEL(reg, S5P_FIMV_E_GOP_CONFIG_V6);
+
+	/* multi-slice control */
+	/* multi-slice MB number or bit size */
+	ctx->slice_mode = p->slice_mode;
+	reg = 0;
+	if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB) {
+		reg |= (0x1 << 3);
+		WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+		ctx->slice_size.mb = p->slice_mb;
+	} else if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES) {
+		reg |= (0x1 << 3);
+		WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+		ctx->slice_size.bits = p->slice_bit;
+	} else {
+		reg &= ~(0x1 << 3);
+		WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+	}
+
+	s5p_mfc_set_slice_mode(ctx);
+
+	/* cyclic intra refresh */
+	WRITEL(p->intra_refresh_mb, S5P_FIMV_E_IR_SIZE_V6);
+	reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+	if (p->intra_refresh_mb == 0)
+		reg &= ~(0x1 << 4);
+	else
+		reg |= (0x1 << 4);
+	WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+
+	/* 'NON_REFERENCE_STORE_ENABLE' for debugging */
+	reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+	reg &= ~(0x1 << 9);
+	WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+
+	/* memory structure cur. frame */
+	if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M) {
+		/* 0: Linear, 1: 2D tiled*/
+		reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+		reg &= ~(0x1 << 7);
+		WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+		/* 0: NV12(CbCr), 1: NV21(CrCb) */
+		WRITEL(0x0, S5P_FIMV_PIXEL_FORMAT_V6);
+	} else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV21M) {
+		/* 0: Linear, 1: 2D tiled*/
+		reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+		reg &= ~(0x1 << 7);
+		WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+		/* 0: NV12(CbCr), 1: NV21(CrCb) */
+		WRITEL(0x1, S5P_FIMV_PIXEL_FORMAT_V6);
+	} else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT_16X16) {
+		/* 0: Linear, 1: 2D tiled*/
+		reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+		reg |= (0x1 << 7);
+		WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+		/* 0: NV12(CbCr), 1: NV21(CrCb) */
+		WRITEL(0x0, S5P_FIMV_PIXEL_FORMAT_V6);
+	}
+
+	/* memory structure recon. frame */
+	/* 0: Linear, 1: 2D tiled */
+	reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+	reg |= (0x1 << 8);
+	WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+
+	/* padding control & value */
+	WRITEL(0x0, S5P_FIMV_E_PADDING_CTRL_V6);
+	if (p->pad) {
+		reg = 0;
+		/** enable */
+		reg |= (1 << 31);
+		/** cr value */
+		reg |= ((p->pad_cr & 0xFF) << 16);
+		/** cb value */
+		reg |= ((p->pad_cb & 0xFF) << 8);
+		/** y value */
+		reg |= p->pad_luma & 0xFF;
+		WRITEL(reg, S5P_FIMV_E_PADDING_CTRL_V6);
+	}
+
+	/* rate control config. */
+	reg = 0;
+	/* frame-level rate control */
+	reg |= ((p->rc_frame & 0x1) << 9);
+	WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+
+	/* bit rate */
+	if (p->rc_frame)
+		WRITEL(p->rc_bitrate,
+			S5P_FIMV_E_RC_BIT_RATE_V6);
+	else
+		WRITEL(1, S5P_FIMV_E_RC_BIT_RATE_V6);
+
+	/* reaction coefficient */
+	if (p->rc_frame) {
+		if (p->rc_reaction_coeff < TIGHT_CBR_MAX) /* tight CBR */
+			WRITEL(1, S5P_FIMV_E_RC_RPARAM_V6);
+		else					  /* loose CBR */
+			WRITEL(2, S5P_FIMV_E_RC_RPARAM_V6);
+	}
+
+	/* seq header ctrl */
+	reg = READL(S5P_FIMV_E_ENC_OPTIONS_V6);
+	reg &= ~(0x1 << 2);
+	reg |= ((p->seq_hdr_mode & 0x1) << 2);
+
+	/* frame skip mode */
+	reg &= ~(0x3);
+	reg |= (p->frame_skip_mode & 0x3);
+	WRITEL(reg, S5P_FIMV_E_ENC_OPTIONS_V6);
+
+	/* 'DROP_CONTROL_ENABLE', disable */
+	reg = READL(S5P_FIMV_E_RC_CONFIG_V6);
+	reg &= ~(0x1 << 10);
+	WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+
+	/* setting for MV range [16, 256] */
+	reg = 0;
+	reg &= ~(0x3FFF);
+	reg = 256;
+	WRITEL(reg, S5P_FIMV_E_MV_HOR_RANGE_V6);
+
+	reg = 0;
+	reg &= ~(0x3FFF);
+	reg = 256;
+	WRITEL(reg, S5P_FIMV_E_MV_VER_RANGE_V6);
+
+	WRITEL(0x0, S5P_FIMV_E_FRAME_INSERTION_V6);
+	WRITEL(0x0, S5P_FIMV_E_ROI_BUFFER_ADDR_V6);
+	WRITEL(0x0, S5P_FIMV_E_PARAM_CHANGE_V6);
+	WRITEL(0x0, S5P_FIMV_E_RC_ROI_CTRL_V6);
+	WRITEL(0x0, S5P_FIMV_E_PICTURE_TAG_V6);
+
+	WRITEL(0x0, S5P_FIMV_E_BIT_COUNT_ENABLE_V6);
+	WRITEL(0x0, S5P_FIMV_E_MAX_BIT_COUNT_V6);
+	WRITEL(0x0, S5P_FIMV_E_MIN_BIT_COUNT_V6);
+
+	WRITEL(0x0, S5P_FIMV_E_METADATA_BUFFER_ADDR_V6);
+	WRITEL(0x0, S5P_FIMV_E_METADATA_BUFFER_SIZE_V6);
+
+	mfc_debug_leave();
+
+	return 0;
+}
+
+static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	struct s5p_mfc_h264_enc_params *p_h264 = &p->codec.h264;
+	unsigned int reg = 0;
+	int i;
+
+	mfc_debug_enter();
+
+	s5p_mfc_set_enc_params(ctx);
+
+	/* pictype : number of B */
+	reg = READL(S5P_FIMV_E_GOP_CONFIG_V6);
+	reg &= ~(0x3 << 16);
+	reg |= ((p->num_b_frame & 0x3) << 16);
+	WRITEL(reg, S5P_FIMV_E_GOP_CONFIG_V6);
+
+	/* profile & level */
+	reg = 0;
+	/** level */
+	reg |= ((p_h264->level & 0xFF) << 8);
+	/** profile - 0 ~ 3 */
+	reg |= p_h264->profile & 0x3F;
+	WRITEL(reg, S5P_FIMV_E_PICTURE_PROFILE_V6);
+
+	/* rate control config. */
+	reg = READL(S5P_FIMV_E_RC_CONFIG_V6);
+	/** macroblock level rate control */
+	reg &= ~(0x1 << 8);
+	reg |= ((p->rc_mb & 0x1) << 8);
+	WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+	/** frame QP */
+	reg &= ~(0x3F);
+	reg |= p_h264->rc_frame_qp & 0x3F;
+	WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+
+	/* max & min value of QP */
+	reg = 0;
+	/** max QP */
+	reg |= ((p_h264->rc_max_qp & 0x3F) << 8);
+	/** min QP */
+	reg |= p_h264->rc_min_qp & 0x3F;
+	WRITEL(reg, S5P_FIMV_E_RC_QP_BOUND_V6);
+
+	/* other QPs */
+	WRITEL(0x0, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+	if (!p->rc_frame && !p->rc_mb) {
+		reg = 0;
+		reg |= ((p_h264->rc_b_frame_qp & 0x3F) << 16);
+		reg |= ((p_h264->rc_p_frame_qp & 0x3F) << 8);
+		reg |= p_h264->rc_frame_qp & 0x3F;
+		WRITEL(reg, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+	}
+
+	/* frame rate */
+	if (p->rc_frame && p->rc_framerate_num && p->rc_framerate_denom) {
+		reg = 0;
+		reg |= ((p->rc_framerate_num & 0xFFFF) << 16);
+		reg |= p->rc_framerate_denom & 0xFFFF;
+		WRITEL(reg, S5P_FIMV_E_RC_FRAME_RATE_V6);
+	}
+
+	/* vbv buffer size */
+	if (p->frame_skip_mode ==
+			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+		WRITEL(p_h264->cpb_size & 0xFFFF,
+				S5P_FIMV_E_VBV_BUFFER_SIZE_V6);
+
+		if (p->rc_frame)
+			WRITEL(p->vbv_delay, S5P_FIMV_E_VBV_INIT_DELAY_V6);
+	}
+
+	/* interlace */
+	reg = 0;
+	reg |= ((p_h264->interlace & 0x1) << 3);
+	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+	/* height */
+	if (p_h264->interlace) {
+		WRITEL(ctx->img_height >> 1,
+				S5P_FIMV_E_FRAME_HEIGHT_V6); /* 32 align */
+		/* cropped height */
+		WRITEL(ctx->img_height >> 1,
+				S5P_FIMV_E_CROPPED_FRAME_HEIGHT_V6);
+	}
+
+	/* loop filter ctrl */
+	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	reg &= ~(0x3 << 1);
+	reg |= ((p_h264->loop_filter_mode & 0x3) << 1);
+	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+	/* loopfilter alpha offset */
+	if (p_h264->loop_filter_alpha < 0) {
+		reg = 0x10;
+		reg |= (0xFF - p_h264->loop_filter_alpha) + 1;
+	} else {
+		reg = 0x00;
+		reg |= (p_h264->loop_filter_alpha & 0xF);
+	}
+	WRITEL(reg, S5P_FIMV_E_H264_LF_ALPHA_OFFSET_V6);
+
+	/* loopfilter beta offset */
+	if (p_h264->loop_filter_beta < 0) {
+		reg = 0x10;
+		reg |= (0xFF - p_h264->loop_filter_beta) + 1;
+	} else {
+		reg = 0x00;
+		reg |= (p_h264->loop_filter_beta & 0xF);
+	}
+	WRITEL(reg, S5P_FIMV_E_H264_LF_BETA_OFFSET_V6);
+
+	/* entropy coding mode */
+	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	reg &= ~(0x1);
+	reg |= p_h264->entropy_mode & 0x1;
+	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+	/* number of ref. picture */
+	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	reg &= ~(0x1 << 7);
+	reg |= (((p_h264->num_ref_pic_4p - 1) & 0x1) << 7);
+	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+	/* 8x8 transform enable */
+	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	reg &= ~(0x3 << 12);
+	reg |= ((p_h264->_8x8_transform & 0x3) << 12);
+	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+	/* macroblock adaptive scaling features */
+	WRITEL(0x0, S5P_FIMV_E_MB_RC_CONFIG_V6);
+	if (p->rc_mb) {
+		reg = 0;
+		/** dark region */
+		reg |= ((p_h264->rc_mb_dark & 0x1) << 3);
+		/** smooth region */
+		reg |= ((p_h264->rc_mb_smooth & 0x1) << 2);
+		/** static region */
+		reg |= ((p_h264->rc_mb_static & 0x1) << 1);
+		/** high activity region */
+		reg |= p_h264->rc_mb_activity & 0x1;
+		WRITEL(reg, S5P_FIMV_E_MB_RC_CONFIG_V6);
+	}
+
+	/* aspect ratio VUI */
+	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	reg &= ~(0x1 << 5);
+	reg |= ((p_h264->vui_sar & 0x1) << 5);
+	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+	WRITEL(0x0, S5P_FIMV_E_ASPECT_RATIO_V6);
+	WRITEL(0x0, S5P_FIMV_E_EXTENDED_SAR_V6);
+	if (p_h264->vui_sar) {
+		/* aspect ration IDC */
+		reg = 0;
+		reg |= p_h264->vui_sar_idc & 0xFF;
+		WRITEL(reg, S5P_FIMV_E_ASPECT_RATIO_V6);
+		if (p_h264->vui_sar_idc == 0xFF) {
+			/* extended SAR */
+			reg = 0;
+			reg |= (p_h264->vui_ext_sar_width & 0xFFFF) << 16;
+			reg |= p_h264->vui_ext_sar_height & 0xFFFF;
+			WRITEL(reg, S5P_FIMV_E_EXTENDED_SAR_V6);
+		}
+	}
+
+	/* intra picture period for H.264 open GOP */
+	/* control */
+	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	reg &= ~(0x1 << 4);
+	reg |= ((p_h264->open_gop & 0x1) << 4);
+	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+	/* value */
+	WRITEL(0x0, S5P_FIMV_E_H264_I_PERIOD_V6);
+	if (p_h264->open_gop) {
+		reg = 0;
+		reg |= p_h264->open_gop_size & 0xFFFF;
+		WRITEL(reg, S5P_FIMV_E_H264_I_PERIOD_V6);
+	}
+
+	/* 'WEIGHTED_BI_PREDICTION' for B is disable */
+	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	reg &= ~(0x3 << 9);
+	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+	/* 'CONSTRAINED_INTRA_PRED_ENABLE' is disable */
+	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	reg &= ~(0x1 << 14);
+	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+	/* ASO */
+	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	reg &= ~(0x1 << 6);
+	reg |= ((p_h264->aso & 0x1) << 6);
+	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+
+	/* hier qp enable */
+	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	reg &= ~(0x1 << 8);
+	reg |= ((p_h264->open_gop & 0x1) << 8);
+	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+	reg = 0;
+	if (p_h264->hier_qp && p_h264->hier_qp_layer) {
+		reg |= (p_h264->hier_qp_type & 0x1) << 0x3;
+		reg |= p_h264->hier_qp_layer & 0x7;
+		WRITEL(reg, S5P_FIMV_E_H264_NUM_T_LAYER_V6);
+		/* QP value for each layer */
+		for (i = 0; i < (p_h264->hier_qp_layer & 0x7); i++)
+			WRITEL(p_h264->hier_qp_layer_qp[i],
+				S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER0_V6 +
+				i * 4);
+	}
+	/* number of coding layer should be zero when hierarchical is disable */
+	WRITEL(reg, S5P_FIMV_E_H264_NUM_T_LAYER_V6);
+
+	/* frame packing SEI generation */
+	reg = READL(S5P_FIMV_E_H264_OPTIONS_V6);
+	reg &= ~(0x1 << 25);
+	reg |= ((p_h264->sei_frame_packing & 0x1) << 25);
+	WRITEL(reg, S5P_FIMV_E_H264_OPTIONS_V6);
+	if (p_h264->sei_frame_packing) {
+		reg = 0;
+		/** current frame0 flag */
+		reg |= ((p_h264->sei_fp_curr_frame_0 & 0x1) << 2);
+		/** arrangement type */
+		reg |= p_h264->sei_fp_arrangement_type & 0x3;
+		WRITEL(reg, S5P_FIMV_E_H264_FRAME_PACKING_SEI_INFO_V6);
+	}
+
+	if (p_h264->fmo) {
+		switch (p_h264->fmo_map_type) {
+		case V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_INTERLEAVED_SLICES:
+			if (p_h264->fmo_slice_grp > 4)
+				p_h264->fmo_slice_grp = 4;
+			for (i = 0; i < (p_h264->fmo_slice_grp & 0xF); i++)
+				WRITEL(p_h264->fmo_run_len[i] - 1,
+				S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_0_V6 +
+				i * 4);
+			break;
+		case V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_SCATTERED_SLICES:
+			if (p_h264->fmo_slice_grp > 4)
+				p_h264->fmo_slice_grp = 4;
+			break;
+		case V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_RASTER_SCAN:
+		case V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_WIPE_SCAN:
+			if (p_h264->fmo_slice_grp > 2)
+				p_h264->fmo_slice_grp = 2;
+			WRITEL(p_h264->fmo_chg_dir & 0x1,
+				S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_DIR_V6);
+			/* the valid range is 0 ~ number of macroblocks -1 */
+			WRITEL(p_h264->fmo_chg_rate,
+				S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_RATE_MINUS1_V6);
+			break;
+		default:
+			mfc_err("Unsupported map type for FMO: %d\n",
+					p_h264->fmo_map_type);
+			p_h264->fmo_map_type = 0;
+			p_h264->fmo_slice_grp = 1;
+			break;
+		}
+
+		WRITEL(p_h264->fmo_map_type,
+				S5P_FIMV_E_H264_FMO_SLICE_GRP_MAP_TYPE_V6);
+		WRITEL(p_h264->fmo_slice_grp - 1,
+				S5P_FIMV_E_H264_FMO_NUM_SLICE_GRP_MINUS1_V6);
+	} else {
+		WRITEL(0, S5P_FIMV_E_H264_FMO_NUM_SLICE_GRP_MINUS1_V6);
+	}
+
+	mfc_debug_leave();
+
+	return 0;
+}
+
+static int s5p_mfc_set_enc_params_mpeg4(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	struct s5p_mfc_mpeg4_enc_params *p_mpeg4 = &p->codec.mpeg4;
+	unsigned int reg = 0;
+
+	mfc_debug_enter();
+
+	s5p_mfc_set_enc_params(ctx);
+
+	/* pictype : number of B */
+	reg = READL(S5P_FIMV_E_GOP_CONFIG_V6);
+	reg &= ~(0x3 << 16);
+	reg |= ((p->num_b_frame & 0x3) << 16);
+	WRITEL(reg, S5P_FIMV_E_GOP_CONFIG_V6);
+
+	/* profile & level */
+	reg = 0;
+	/** level */
+	reg |= ((p_mpeg4->level & 0xFF) << 8);
+	/** profile - 0 ~ 1 */
+	reg |= p_mpeg4->profile & 0x3F;
+	WRITEL(reg, S5P_FIMV_E_PICTURE_PROFILE_V6);
+
+	/* rate control config. */
+	reg = READL(S5P_FIMV_E_RC_CONFIG_V6);
+	/** macroblock level rate control */
+	reg &= ~(0x1 << 8);
+	reg |= ((p->rc_mb & 0x1) << 8);
+	WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+	/** frame QP */
+	reg &= ~(0x3F);
+	reg |= p_mpeg4->rc_frame_qp & 0x3F;
+	WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+
+	/* max & min value of QP */
+	reg = 0;
+	/** max QP */
+	reg |= ((p_mpeg4->rc_max_qp & 0x3F) << 8);
+	/** min QP */
+	reg |= p_mpeg4->rc_min_qp & 0x3F;
+	WRITEL(reg, S5P_FIMV_E_RC_QP_BOUND_V6);
+
+	/* other QPs */
+	WRITEL(0x0, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+	if (!p->rc_frame && !p->rc_mb) {
+		reg = 0;
+		reg |= ((p_mpeg4->rc_b_frame_qp & 0x3F) << 16);
+		reg |= ((p_mpeg4->rc_p_frame_qp & 0x3F) << 8);
+		reg |= p_mpeg4->rc_frame_qp & 0x3F;
+		WRITEL(reg, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+	}
+
+	/* frame rate */
+	if (p->rc_frame && p->rc_framerate_num && p->rc_framerate_denom) {
+		reg = 0;
+		reg |= ((p->rc_framerate_num & 0xFFFF) << 16);
+		reg |= p->rc_framerate_denom & 0xFFFF;
+		WRITEL(reg, S5P_FIMV_E_RC_FRAME_RATE_V6);
+	}
+
+	/* vbv buffer size */
+	if (p->frame_skip_mode ==
+			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+		WRITEL(p->vbv_size & 0xFFFF, S5P_FIMV_E_VBV_BUFFER_SIZE_V6);
+
+		if (p->rc_frame)
+			WRITEL(p->vbv_delay, S5P_FIMV_E_VBV_INIT_DELAY_V6);
+	}
+
+	/* Disable HEC */
+	WRITEL(0x0, S5P_FIMV_E_MPEG4_OPTIONS_V6);
+	WRITEL(0x0, S5P_FIMV_E_MPEG4_HEC_PERIOD_V6);
+
+	mfc_debug_leave();
+
+	return 0;
+}
+
+static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	struct s5p_mfc_mpeg4_enc_params *p_h263 = &p->codec.mpeg4;
+	unsigned int reg = 0;
+
+	mfc_debug_enter();
+
+	s5p_mfc_set_enc_params(ctx);
+
+	/* profile & level */
+	reg = 0;
+	/** profile */
+	reg |= (0x1 << 4);
+	WRITEL(reg, S5P_FIMV_E_PICTURE_PROFILE_V6);
+
+	/* rate control config. */
+	reg = READL(S5P_FIMV_E_RC_CONFIG_V6);
+	/** macroblock level rate control */
+	reg &= ~(0x1 << 8);
+	reg |= ((p->rc_mb & 0x1) << 8);
+	WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+	/** frame QP */
+	reg &= ~(0x3F);
+	reg |= p_h263->rc_frame_qp & 0x3F;
+	WRITEL(reg, S5P_FIMV_E_RC_CONFIG_V6);
+
+	/* max & min value of QP */
+	reg = 0;
+	/** max QP */
+	reg |= ((p_h263->rc_max_qp & 0x3F) << 8);
+	/** min QP */
+	reg |= p_h263->rc_min_qp & 0x3F;
+	WRITEL(reg, S5P_FIMV_E_RC_QP_BOUND_V6);
+
+	/* other QPs */
+	WRITEL(0x0, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+	if (!p->rc_frame && !p->rc_mb) {
+		reg = 0;
+		reg |= ((p_h263->rc_b_frame_qp & 0x3F) << 16);
+		reg |= ((p_h263->rc_p_frame_qp & 0x3F) << 8);
+		reg |= p_h263->rc_frame_qp & 0x3F;
+		WRITEL(reg, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+	}
+
+	/* frame rate */
+	if (p->rc_frame && p->rc_framerate_num && p->rc_framerate_denom) {
+		reg = 0;
+		reg |= ((p->rc_framerate_num & 0xFFFF) << 16);
+		reg |= p->rc_framerate_denom & 0xFFFF;
+		WRITEL(reg, S5P_FIMV_E_RC_FRAME_RATE_V6);
+	}
+
+	/* vbv buffer size */
+	if (p->frame_skip_mode ==
+			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+		WRITEL(p->vbv_size & 0xFFFF, S5P_FIMV_E_VBV_BUFFER_SIZE_V6);
+
+		if (p->rc_frame)
+			WRITEL(p->vbv_delay, S5P_FIMV_E_VBV_INIT_DELAY_V6);
+	}
+
+	mfc_debug_leave();
+
+	return 0;
+}
+
+/* Initialize decoding */
+int s5p_mfc_init_decode_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned int reg = 0;
+	int fmo_aso_ctrl = 0;
+
+	mfc_debug_enter();
+	mfc_debug(2, "InstNo: %d/%d\n", ctx->inst_no,
+			S5P_FIMV_CH_SEQ_HEADER_V6);
+	mfc_debug(2, "BUFs: %08x %08x %08x\n",
+		  READL(S5P_FIMV_D_CPB_BUFFER_ADDR_V6),
+		  READL(S5P_FIMV_D_CPB_BUFFER_ADDR_V6),
+		  READL(S5P_FIMV_D_CPB_BUFFER_ADDR_V6));
+
+	/* FMO_ASO_CTRL - 0: Enable, 1: Disable */
+	reg |= (fmo_aso_ctrl << S5P_FIMV_D_OPT_FMO_ASO_CTRL_MASK_V6);
+
+	/* When user sets desplay_delay to 0,
+	 * It works as "display_delay enable" and delay set to 0.
+	 * If user wants display_delay disable, It should be
+	 * set to negative value. */
+	if (ctx->display_delay >= 0) {
+		reg |= (0x1 << S5P_FIMV_D_OPT_DDELAY_EN_SHIFT_V6);
+		WRITEL(ctx->display_delay, S5P_FIMV_D_DISPLAY_DELAY_V6);
+	}
+	/* Setup loop filter, for decoding this is only valid for MPEG4 */
+	if (ctx->codec_mode == S5P_MFC_CODEC_MPEG4_DEC) {
+		mfc_debug(2, "Set loop filter to: %d\n",
+				ctx->loop_filter_mpeg4);
+		reg |= (ctx->loop_filter_mpeg4 <<
+				S5P_FIMV_D_OPT_LF_CTRL_SHIFT_V6);
+	}
+	if (ctx->dst_fmt->fourcc == V4L2_PIX_FMT_NV12MT_16X16)
+		reg |= (0x1 << S5P_FIMV_D_OPT_TILE_MODE_SHIFT_V6);
+
+	WRITEL(reg, S5P_FIMV_D_DEC_OPTIONS_V6);
+
+	/* 0: NV12(CbCr), 1: NV21(CrCb) */
+	if (ctx->dst_fmt->fourcc == V4L2_PIX_FMT_NV21M)
+		WRITEL(0x1, S5P_FIMV_PIXEL_FORMAT_V6);
+	else
+		WRITEL(0x0, S5P_FIMV_PIXEL_FORMAT_V6);
+
+	/* sei parse */
+	WRITEL(ctx->sei_fp_parse & 0x1, S5P_FIMV_D_SEI_ENABLE_V6);
+
+	WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+			S5P_FIMV_CH_SEQ_HEADER_V6, NULL);
+
+	mfc_debug_leave();
+	return 0;
+}
+
+static inline void s5p_mfc_set_flush(struct s5p_mfc_ctx *ctx, int flush)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned int dpb;
+	if (flush)
+		dpb = READL(S5P_FIMV_SI_CH0_DPB_CONF_CTRL) | (1 << 14);
+	else
+		dpb = READL(S5P_FIMV_SI_CH0_DPB_CONF_CTRL) & ~(1 << 14);
+	WRITEL(dpb, S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
+}
+
+/* Decode a single frame */
+int s5p_mfc_decode_one_frame_v6(struct s5p_mfc_ctx *ctx,
+			enum s5p_mfc_decode_arg last_frame)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	WRITEL(ctx->dec_dst_flag, S5P_FIMV_D_AVAILABLE_DPB_FLAG_LOWER_V6);
+	WRITEL(ctx->slice_interface & 0x1, S5P_FIMV_D_SLICE_IF_ENABLE_V6);
+
+	WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+	/* Issue different commands to instance basing on whether it
+	 * is the last frame or not. */
+	switch (last_frame) {
+	case 0:
+		s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+				S5P_FIMV_CH_FRAME_START_V6, NULL);
+		break;
+	case 1:
+		s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+				S5P_FIMV_CH_LAST_FRAME_V6, NULL);
+		break;
+	default:
+		mfc_err("Unsupported last frame arg.\n");
+		return -EINVAL;
+	}
+
+	mfc_debug(2, "Decoding a usual frame.\n");
+	return 0;
+}
+
+int s5p_mfc_init_encode_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC)
+		s5p_mfc_set_enc_params_h264(ctx);
+	else if (ctx->codec_mode == S5P_MFC_CODEC_MPEG4_ENC)
+		s5p_mfc_set_enc_params_mpeg4(ctx);
+	else if (ctx->codec_mode == S5P_MFC_CODEC_H263_ENC)
+		s5p_mfc_set_enc_params_h263(ctx);
+	else {
+		mfc_err("Unknown codec for encoding (%x).\n",
+			ctx->codec_mode);
+		return -EINVAL;
+	}
+
+	WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+			S5P_FIMV_CH_SEQ_HEADER_V6, NULL);
+
+	return 0;
+}
+
+int s5p_mfc_h264_set_aso_slice_order_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	struct s5p_mfc_h264_enc_params *p_h264 = &p->codec.h264;
+	int i;
+
+	if (p_h264->aso) {
+		for (i = 0; i < 8; i++)
+			WRITEL(p_h264->aso_slice_order[i],
+				S5P_FIMV_E_H264_ASO_SLICE_ORDER_0_V6 + i * 4);
+	}
+	return 0;
+}
+
+/* Encode a single frame */
+int s5p_mfc_encode_one_frame_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	mfc_debug(2, "++\n");
+
+	/* memory structure cur. frame */
+
+	if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC)
+		s5p_mfc_h264_set_aso_slice_order_v6(ctx);
+
+	s5p_mfc_set_slice_mode(ctx);
+
+	WRITEL(ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+			S5P_FIMV_CH_FRAME_START_V6, NULL);
+
+	mfc_debug(2, "--\n");
+
+	return 0;
+}
+
+static inline int s5p_mfc_get_new_ctx(struct s5p_mfc_dev *dev)
+{
+	unsigned long flags;
+	int new_ctx;
+	int cnt;
+
+	spin_lock_irqsave(&dev->condlock, flags);
+	mfc_debug(2, "Previos context: %d (bits %08lx)\n", dev->curr_ctx,
+							dev->ctx_work_bits);
+	new_ctx = (dev->curr_ctx + 1) % MFC_NUM_CONTEXTS;
+	cnt = 0;
+	while (!test_bit(new_ctx, &dev->ctx_work_bits)) {
+		new_ctx = (new_ctx + 1) % MFC_NUM_CONTEXTS;
+		cnt++;
+		if (cnt > MFC_NUM_CONTEXTS) {
+			/* No contexts to run */
+			spin_unlock_irqrestore(&dev->condlock, flags);
+			return -EAGAIN;
+		}
+	}
+	spin_unlock_irqrestore(&dev->condlock, flags);
+	return new_ctx;
+}
+
+static inline void s5p_mfc_run_dec_last_frames(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf *temp_vb;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->irqlock, flags);
+
+	/* Frames are being decoded */
+	if (list_empty(&ctx->src_queue)) {
+		mfc_debug(2, "No src buffers.\n");
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+		return;
+	}
+	/* Get the next source buffer */
+	temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+	temp_vb->flags |= MFC_BUF_FLAG_USED;
+	s5p_mfc_set_dec_stream_buffer_v6(ctx,
+			vb2_dma_contig_plane_dma_addr(temp_vb->b, 0), 0, 0);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_clean_ctx_int_flags(ctx);
+	s5p_mfc_decode_one_frame_v6(ctx, 1);
+}
+
+static inline int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf *temp_vb;
+	unsigned long flags;
+	int last_frame = 0;
+	unsigned int index;
+
+	spin_lock_irqsave(&dev->irqlock, flags);
+
+	/* Frames are being decoded */
+	if (list_empty(&ctx->src_queue)) {
+		mfc_debug(2, "No src buffers.\n");
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+		return -EAGAIN;
+	}
+	/* Get the next source buffer */
+	temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+	temp_vb->flags |= MFC_BUF_FLAG_USED;
+	s5p_mfc_set_dec_stream_buffer_v6(ctx,
+		vb2_dma_contig_plane_dma_addr(temp_vb->b, 0),
+			ctx->consumed_stream,
+			temp_vb->b->v4l2_planes[0].bytesused);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+
+	index = temp_vb->b->v4l2_buf.index;
+
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_clean_ctx_int_flags(ctx);
+	if (temp_vb->b->v4l2_planes[0].bytesused == 0) {
+		last_frame = 1;
+		mfc_debug(2, "Setting ctx->state to FINISHING\n");
+		ctx->state = MFCINST_FINISHING;
+	}
+	s5p_mfc_decode_one_frame_v6(ctx, last_frame);
+
+	return 0;
+}
+
+static inline int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long flags;
+	struct s5p_mfc_buf *dst_mb;
+	struct s5p_mfc_buf *src_mb;
+	unsigned long src_y_addr, src_c_addr, dst_addr;
+	/*
+	unsigned int src_y_size, src_c_size;
+	*/
+	unsigned int dst_size;
+	unsigned int index;
+
+	spin_lock_irqsave(&dev->irqlock, flags);
+
+	if (list_empty(&ctx->src_queue)) {
+		mfc_debug(2, "no src buffers.\n");
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+		return -EAGAIN;
+	}
+
+	if (list_empty(&ctx->dst_queue)) {
+		mfc_debug(2, "no dst buffers.\n");
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+		return -EAGAIN;
+	}
+
+	src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+	src_mb->flags |= MFC_BUF_FLAG_USED;
+	src_y_addr = vb2_dma_contig_plane_dma_addr(src_mb->b, 0);
+	src_c_addr = vb2_dma_contig_plane_dma_addr(src_mb->b, 1);
+
+	mfc_debug(2, "enc src y addr: 0x%08lx", src_y_addr);
+	mfc_debug(2, "enc src c addr: 0x%08lx", src_c_addr);
+
+	s5p_mfc_set_enc_frame_buffer_v6(ctx, src_y_addr, src_c_addr);
+
+	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+	dst_mb->flags |= MFC_BUF_FLAG_USED;
+	dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
+	dst_size = vb2_plane_size(dst_mb->b, 0);
+
+	s5p_mfc_set_enc_stream_buffer_v6(ctx, dst_addr, dst_size);
+
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+
+	index = src_mb->b->v4l2_buf.index;
+
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_clean_ctx_int_flags(ctx);
+	s5p_mfc_encode_one_frame_v6(ctx);
+
+	return 0;
+}
+
+static inline void s5p_mfc_run_init_dec(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long flags;
+	struct s5p_mfc_buf *temp_vb;
+
+	/* Initializing decoding - parsing header */
+	spin_lock_irqsave(&dev->irqlock, flags);
+	mfc_debug(2, "Preparing to init decoding.\n");
+	temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+	mfc_debug(2, "Header size: %d\n", temp_vb->b->v4l2_planes[0].bytesused);
+	s5p_mfc_set_dec_stream_buffer_v6(ctx,
+		vb2_dma_contig_plane_dma_addr(temp_vb->b, 0), 0,
+			temp_vb->b->v4l2_planes[0].bytesused);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_clean_ctx_int_flags(ctx);
+	s5p_mfc_init_decode_v6(ctx);
+}
+
+static inline void s5p_mfc_run_init_enc(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long flags;
+	struct s5p_mfc_buf *dst_mb;
+	unsigned long dst_addr;
+	unsigned int dst_size;
+
+	spin_lock_irqsave(&dev->irqlock, flags);
+
+	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+	dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
+	dst_size = vb2_plane_size(dst_mb->b, 0);
+	s5p_mfc_set_enc_stream_buffer_v6(ctx, dst_addr, dst_size);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_clean_ctx_int_flags(ctx);
+	s5p_mfc_init_encode_v6(ctx);
+}
+
+static inline int s5p_mfc_run_init_dec_buffers(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	int ret;
+	/* Header was parsed now start processing
+	 * First set the output frame buffers
+	 * s5p_mfc_alloc_dec_buffers(ctx); */
+
+	if (ctx->capture_state != QUEUE_BUFS_MMAPED) {
+		mfc_err("It seems that not all destionation buffers were\n"
+			"mmaped.MFC requires that all destination are mmaped\n"
+			"before starting processing.\n");
+		return -EAGAIN;
+	}
+
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_clean_ctx_int_flags(ctx);
+	ret = s5p_mfc_set_dec_frame_buffer_v6(ctx);
+	if (ret) {
+		mfc_err("Failed to alloc frame mem.\n");
+		ctx->state = MFCINST_ERROR;
+	}
+	return ret;
+}
+
+static inline int s5p_mfc_run_init_enc_buffers(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	int ret;
+
+	ret = s5p_mfc_alloc_codec_buffers_v6(ctx);
+	if (ret) {
+		mfc_err("Failed to allocate encoding buffers.\n");
+		return -ENOMEM;
+	}
+
+	/* Header was generated now starting processing
+	 * First set the reference frame buffers
+	 */
+	if (ctx->capture_state != QUEUE_BUFS_REQUESTED) {
+		mfc_err("It seems that destionation buffers were not\n"
+			"requested.MFC requires that header should be generated\n"
+			"before allocating codec buffer.\n");
+		return -EAGAIN;
+	}
+
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_clean_ctx_int_flags(ctx);
+	ret = s5p_mfc_set_enc_ref_buffer_v6(ctx);
+	if (ret) {
+		mfc_err("Failed to alloc frame mem.\n");
+		ctx->state = MFCINST_ERROR;
+	}
+	return ret;
+}
+
+/* Try running an operation on hardware */
+void s5p_mfc_try_run_v6(struct s5p_mfc_dev *dev)
+{
+	struct s5p_mfc_ctx *ctx;
+	int new_ctx;
+	unsigned int ret = 0;
+
+	mfc_debug(1, "Try run dev: %p\n", dev);
+
+	/* Check whether hardware is not running */
+	if (test_and_set_bit(0, &dev->hw_lock) != 0) {
+		/* This is perfectly ok, the scheduled ctx should wait */
+		mfc_debug(1, "Couldn't lock HW.\n");
+		return;
+	}
+
+	/* Choose the context to run */
+	new_ctx = s5p_mfc_get_new_ctx(dev);
+	if (new_ctx < 0) {
+		/* No contexts to run */
+		if (test_and_clear_bit(0, &dev->hw_lock) == 0) {
+			mfc_err("Failed to unlock hardware.\n");
+			return;
+		}
+
+		mfc_debug(1, "No ctx is scheduled to be run.\n");
+		return;
+	}
+
+	mfc_debug(1, "New context: %d\n", new_ctx);
+	ctx = dev->ctx[new_ctx];
+	mfc_debug(1, "Seting new context to %p\n", ctx);
+	/* Got context to run in ctx */
+	mfc_debug(1, "ctx->dst_queue_cnt=%d ctx->dpb_count=%d ctx->src_queue_cnt=%d\n",
+		ctx->dst_queue_cnt, ctx->dpb_count, ctx->src_queue_cnt);
+	mfc_debug(1, "ctx->state=%d\n", ctx->state);
+	/* Last frame has already been sent to MFC
+	 * Now obtaining frames from MFC buffer */
+
+	s5p_mfc_clock_on();
+	if (ctx->type == MFCINST_DECODER) {
+		switch (ctx->state) {
+		case MFCINST_FINISHING:
+			s5p_mfc_run_dec_last_frames(ctx);
+			break;
+		case MFCINST_RUNNING:
+			ret = s5p_mfc_run_dec_frame(ctx);
+			break;
+		case MFCINST_INIT:
+			s5p_mfc_clean_ctx_int_flags(ctx);
+			ret = s5p_mfc_hw_call(dev->mfc_cmds, open_inst_cmd,
+					ctx);
+			break;
+		case MFCINST_RETURN_INST:
+			s5p_mfc_clean_ctx_int_flags(ctx);
+			ret = s5p_mfc_hw_call(dev->mfc_cmds, close_inst_cmd,
+					ctx);
+			break;
+		case MFCINST_GOT_INST:
+			s5p_mfc_run_init_dec(ctx);
+			break;
+		case MFCINST_HEAD_PARSED:
+			ret = s5p_mfc_run_init_dec_buffers(ctx);
+			break;
+		case MFCINST_RES_CHANGE_INIT:
+			s5p_mfc_run_dec_last_frames(ctx);
+			break;
+		case MFCINST_RES_CHANGE_FLUSH:
+			s5p_mfc_run_dec_last_frames(ctx);
+			break;
+		case MFCINST_RES_CHANGE_END:
+			mfc_debug(2, "Finished remaining frames after resolution change.\n");
+			ctx->capture_state = QUEUE_FREE;
+			mfc_debug(2, "Will re-init the codec`.\n");
+			s5p_mfc_run_init_dec(ctx);
+			break;
+		default:
+			ret = -EAGAIN;
+		}
+	} else if (ctx->type == MFCINST_ENCODER) {
+		switch (ctx->state) {
+		case MFCINST_FINISHING:
+		case MFCINST_RUNNING:
+			ret = s5p_mfc_run_enc_frame(ctx);
+			break;
+		case MFCINST_INIT:
+			ret = s5p_mfc_hw_call(dev->mfc_cmds, open_inst_cmd,
+					ctx);
+			break;
+		case MFCINST_RETURN_INST:
+			ret = s5p_mfc_hw_call(dev->mfc_cmds, close_inst_cmd,
+					ctx);
+			break;
+		case MFCINST_GOT_INST:
+			s5p_mfc_run_init_enc(ctx);
+			break;
+		case MFCINST_HEAD_PARSED: /* Only for MFC6.x */
+			ret = s5p_mfc_run_init_enc_buffers(ctx);
+			break;
+		default:
+			ret = -EAGAIN;
+		}
+	} else {
+		mfc_err("invalid context type: %d\n", ctx->type);
+		ret = -EAGAIN;
+	}
+
+	if (ret) {
+		/* Free hardware lock */
+		if (test_and_clear_bit(0, &dev->hw_lock) == 0)
+			mfc_err("Failed to unlock hardware.\n");
+
+		/* This is in deed imporant, as no operation has been
+		 * scheduled, reduce the clock count as no one will
+		 * ever do this, because no interrupt related to this try_run
+		 * will ever come from hardware. */
+		s5p_mfc_clock_off();
+	}
+}
+
+
+void s5p_mfc_cleanup_queue_v6(struct list_head *lh, struct vb2_queue *vq)
+{
+	struct s5p_mfc_buf *b;
+	int i;
+
+	while (!list_empty(lh)) {
+		b = list_entry(lh->next, struct s5p_mfc_buf, list);
+		for (i = 0; i < b->b->num_planes; i++)
+			vb2_set_plane_payload(b->b, i, 0);
+		vb2_buffer_done(b->b, VB2_BUF_STATE_ERROR);
+		list_del(&b->list);
+	}
+}
+
+void s5p_mfc_clear_int_flags_v6(struct s5p_mfc_dev *dev)
+{
+	mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD_V6);
+	mfc_write(dev, 0, S5P_FIMV_RISC2HOST_INT_V6);
+}
+
+void s5p_mfc_write_info_v6(struct s5p_mfc_ctx *ctx, unsigned int data,
+		unsigned int ofs)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	s5p_mfc_clock_on();
+	WRITEL(data, ofs);
+	s5p_mfc_clock_off();
+}
+
+unsigned int s5p_mfc_read_info_v6(struct s5p_mfc_ctx *ctx, unsigned int ofs)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	int ret;
+
+	s5p_mfc_clock_on();
+	ret = READL(ofs);
+	s5p_mfc_clock_off();
+
+	return ret;
+}
+
+int s5p_mfc_get_dspl_y_adr_v6(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_D_DISPLAY_LUMA_ADDR_V6);
+}
+
+int s5p_mfc_get_dec_y_adr_v6(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_D_DISPLAY_LUMA_ADDR_V6);
+}
+
+int s5p_mfc_get_dspl_status_v6(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_D_DISPLAY_STATUS_V6);
+}
+
+int s5p_mfc_get_decoded_status_v6(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_D_DECODED_STATUS_V6);
+}
+
+int s5p_mfc_get_dec_frame_type_v6(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_D_DECODED_FRAME_TYPE_V6) &
+		S5P_FIMV_DECODE_FRAME_MASK_V6;
+}
+
+int s5p_mfc_get_disp_frame_type_v6(struct s5p_mfc_ctx *ctx)
+{
+	return mfc_read(ctx->dev, S5P_FIMV_D_DISPLAY_FRAME_TYPE_V6) &
+		S5P_FIMV_DECODE_FRAME_MASK_V6;
+}
+
+int s5p_mfc_get_consumed_stream_v6(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_D_DECODED_NAL_SIZE_V6);
+}
+
+int s5p_mfc_get_int_reason_v6(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_RISC2HOST_CMD_V6) &
+		S5P_FIMV_RISC2HOST_CMD_MASK;
+}
+
+int s5p_mfc_get_int_err_v6(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_ERROR_CODE_V6);
+}
+
+int s5p_mfc_err_dec_v6(unsigned int err)
+{
+	return (err & S5P_FIMV_ERR_DEC_MASK_V6) >> S5P_FIMV_ERR_DEC_SHIFT_V6;
+}
+
+int s5p_mfc_err_dspl_v6(unsigned int err)
+{
+	return (err & S5P_FIMV_ERR_DSPL_MASK_V6) >> S5P_FIMV_ERR_DSPL_SHIFT_V6;
+}
+
+int s5p_mfc_get_img_width_v6(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_D_DISPLAY_FRAME_WIDTH_V6);
+}
+
+int s5p_mfc_get_img_height_v6(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_D_DISPLAY_FRAME_HEIGHT_V6);
+}
+
+int s5p_mfc_get_dpb_count_v6(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_D_MIN_NUM_DPB_V6);
+}
+
+int s5p_mfc_get_mv_count_v6(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_D_MIN_NUM_MV_V6);
+}
+
+int s5p_mfc_get_inst_no_v6(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_RET_INSTANCE_ID_V6);
+}
+
+int s5p_mfc_get_enc_dpb_count_v6(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_E_NUM_DPB_V6);
+}
+
+int s5p_mfc_get_enc_strm_size_v6(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_E_STREAM_SIZE_V6);
+}
+
+int s5p_mfc_get_enc_slice_type_v6(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_E_SLICE_TYPE_V6);
+}
+
+int s5p_mfc_get_enc_pic_count_v6(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_E_PICTURE_COUNT_V6);
+}
+
+int s5p_mfc_get_sei_avail_status_v6(struct s5p_mfc_ctx *ctx)
+{
+	return mfc_read(ctx->dev, S5P_FIMV_D_FRAME_PACK_SEI_AVAIL_V6);
+}
+
+int s5p_mfc_get_mvc_num_views_v6(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_D_MVC_NUM_VIEWS_V6);
+}
+
+int s5p_mfc_get_mvc_view_id_v6(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_D_MVC_VIEW_ID_V6);
+}
+
+unsigned int s5p_mfc_get_pic_type_top_v6(struct s5p_mfc_ctx *ctx)
+{
+	return s5p_mfc_read_info_v6(ctx, PIC_TIME_TOP_V6);
+}
+
+unsigned int s5p_mfc_get_pic_type_bot_v6(struct s5p_mfc_ctx *ctx)
+{
+	return s5p_mfc_read_info_v6(ctx, PIC_TIME_BOT_V6);
+}
+
+unsigned int s5p_mfc_get_crop_info_h_v6(struct s5p_mfc_ctx *ctx)
+{
+	return s5p_mfc_read_info_v6(ctx, CROP_INFO_H_V6);
+}
+
+unsigned int s5p_mfc_get_crop_info_v_v6(struct s5p_mfc_ctx *ctx)
+{
+	return s5p_mfc_read_info_v6(ctx, CROP_INFO_V_V6);
+}
+
+/* Initialize opr function pointers for MFC v6 */
+static struct s5p_mfc_hw_ops s5p_mfc_ops_v6 = {
+	.alloc_dec_temp_buffers = s5p_mfc_alloc_dec_temp_buffers_v6,
+	.release_dec_desc_buffer = s5p_mfc_release_dec_desc_buffer_v6,
+	.alloc_codec_buffers = s5p_mfc_alloc_codec_buffers_v6,
+	.release_codec_buffers = s5p_mfc_release_codec_buffers_v6,
+	.alloc_instance_buffer = s5p_mfc_alloc_instance_buffer_v6,
+	.release_instance_buffer = s5p_mfc_release_instance_buffer_v6,
+	.alloc_dev_context_buffer =
+		s5p_mfc_alloc_dev_context_buffer_v6,
+	.release_dev_context_buffer =
+		s5p_mfc_release_dev_context_buffer_v6,
+	.dec_calc_dpb_size = s5p_mfc_dec_calc_dpb_size_v6,
+	.enc_calc_src_size = s5p_mfc_enc_calc_src_size_v6,
+	.set_dec_stream_buffer = s5p_mfc_set_dec_stream_buffer_v6,
+	.set_dec_frame_buffer = s5p_mfc_set_dec_frame_buffer_v6,
+	.set_enc_stream_buffer = s5p_mfc_set_enc_stream_buffer_v6,
+	.set_enc_frame_buffer = s5p_mfc_set_enc_frame_buffer_v6,
+	.get_enc_frame_buffer = s5p_mfc_get_enc_frame_buffer_v6,
+	.set_enc_ref_buffer = s5p_mfc_set_enc_ref_buffer_v6,
+	.init_decode = s5p_mfc_init_decode_v6,
+	.init_encode = s5p_mfc_init_encode_v6,
+	.encode_one_frame = s5p_mfc_encode_one_frame_v6,
+	.try_run = s5p_mfc_try_run_v6,
+	.cleanup_queue = s5p_mfc_cleanup_queue_v6,
+	.clear_int_flags = s5p_mfc_clear_int_flags_v6,
+	.write_info = s5p_mfc_write_info_v6,
+	.read_info = s5p_mfc_read_info_v6,
+	.get_dspl_y_adr = s5p_mfc_get_dspl_y_adr_v6,
+	.get_dec_y_adr = s5p_mfc_get_dec_y_adr_v6,
+	.get_dspl_status = s5p_mfc_get_dspl_status_v6,
+	.get_dec_status = s5p_mfc_get_dec_status_v6,
+	.get_dec_frame_type = s5p_mfc_get_dec_frame_type_v6,
+	.get_disp_frame_type = s5p_mfc_get_disp_frame_type_v6,
+	.get_consumed_stream = s5p_mfc_get_consumed_stream_v6,
+	.get_int_reason = s5p_mfc_get_int_reason_v6,
+	.get_int_err = s5p_mfc_get_int_err_v6,
+	.err_dec = s5p_mfc_err_dec_v6,
+	.err_dspl = s5p_mfc_err_dspl_v6,
+	.get_img_width = s5p_mfc_get_img_width_v6,
+	.get_img_height = s5p_mfc_get_img_height_v6,
+	.get_dpb_count = s5p_mfc_get_dpb_count_v6,
+	.get_mv_count = s5p_mfc_get_mv_count_v6,
+	.get_inst_no = s5p_mfc_get_inst_no_v6,
+	.get_enc_strm_size = s5p_mfc_get_enc_strm_size_v6,
+	.get_enc_slice_type = s5p_mfc_get_enc_slice_type_v6,
+	.get_enc_dpb_count = s5p_mfc_get_enc_dpb_count_v6,
+	.get_enc_pic_count = s5p_mfc_get_enc_pic_count_v6,
+	.get_sei_avail_status = s5p_mfc_get_sei_avail_status_v6,
+	.get_mvc_num_views = s5p_mfc_get_mvc_num_views_v6,
+	.get_mvc_view_id = s5p_mfc_get_mvc_view_id_v6,
+	.get_pic_type_top = s5p_mfc_get_pic_type_top_v6,
+	.get_pic_type_bot = s5p_mfc_get_pic_type_bot_v6,
+	.get_crop_info_h = s5p_mfc_get_crop_info_h_v6,
+	.get_crop_info_v = s5p_mfc_get_crop_info_v_v6,
+};
+
+struct s5p_mfc_hw_ops *s5p_mfc_init_hw_ops_v6(void)
+{
+	return &s5p_mfc_ops_v6;
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h
new file mode 100644
index 00000000000..ab164efa127
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h
@@ -0,0 +1,50 @@
+/*
+ * drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h
+ *
+ * Header file for Samsung MFC (Multi Function Codec - FIMV) driver
+ * Contains declarations of hw related functions.
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.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 S5P_MFC_OPR_V6_H_
+#define S5P_MFC_OPR_V6_H_
+
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_opr.h"
+
+#define MFC_CTRL_MODE_CUSTOM	MFC_CTRL_MODE_SFR
+
+#define MB_WIDTH(x_size)		DIV_ROUND_UP(x_size, 16)
+#define MB_HEIGHT(y_size)		DIV_ROUND_UP(y_size, 16)
+#define S5P_MFC_DEC_MV_SIZE_V6(x, y)	(MB_WIDTH(x) * \
+					(((MB_HEIGHT(y)+1)/2)*2) * 64 + 128)
+
+/* Definition */
+#define ENC_MULTI_SLICE_MB_MAX		((1 << 30) - 1)
+#define ENC_MULTI_SLICE_BIT_MIN		2800
+#define ENC_INTRA_REFRESH_MB_MAX	((1 << 18) - 1)
+#define ENC_VBV_BUF_SIZE_MAX		((1 << 30) - 1)
+#define ENC_H264_LOOP_FILTER_AB_MIN	-12
+#define ENC_H264_LOOP_FILTER_AB_MAX	12
+#define ENC_H264_RC_FRAME_RATE_MAX	((1 << 16) - 1)
+#define ENC_H263_RC_FRAME_RATE_MAX	((1 << 16) - 1)
+#define ENC_H264_PROFILE_MAX		3
+#define ENC_H264_LEVEL_MAX		42
+#define ENC_MPEG4_VOP_TIME_RES_MAX	((1 << 16) - 1)
+#define FRAME_DELTA_H264_H263		1
+#define TIGHT_CBR_MAX			10
+
+/* Definitions for shared memory compatibility */
+#define PIC_TIME_TOP_V6		S5P_FIMV_D_RET_PICTURE_TAG_TOP_V6
+#define PIC_TIME_BOT_V6		S5P_FIMV_D_RET_PICTURE_TAG_BOT_V6
+#define CROP_INFO_H_V6		S5P_FIMV_D_DISPLAY_CROP_INFO1_V6
+#define CROP_INFO_V_V6		S5P_FIMV_D_DISPLAY_CROP_INFO2_V6
+
+struct s5p_mfc_hw_ops *s5p_mfc_init_hw_ops_v6(void);
+#endif /* S5P_MFC_OPR_V6_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c b/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c
index 0503d14ac94..367db755228 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c
@@ -20,7 +20,6 @@
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_pm.h"
 
-#define MFC_CLKNAME		"sclk_mfc"
 #define MFC_GATE_CLK_NAME	"mfc"
 
 #define CLK_DEBUG
@@ -51,7 +50,7 @@ int s5p_mfc_init_pm(struct s5p_mfc_dev *dev)
 		goto err_p_ip_clk;
 	}
 
-	pm->clock = clk_get(&dev->plat_dev->dev, MFC_CLKNAME);
+	pm->clock = clk_get(&dev->plat_dev->dev, dev->variant->mclk_name);
 	if (IS_ERR(pm->clock)) {
 		mfc_err("Failed to get MFC clock\n");
 		ret = PTR_ERR(pm->clock);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_shm.c b/drivers/media/platform/s5p-mfc/s5p_mfc_shm.c
deleted file mode 100644
index b5933d233a4..00000000000
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_shm.c
+++ /dev/null
@@ -1,47 +0,0 @@
-/*
- * linux/drivers/media/platform/s5p-mfc/s5p_mfc_shm.c
- *
- * Copyright (c) 2010 Samsung Electronics Co., Ltd.
- *		http://www.samsung.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.
- */
-
-#ifdef CONFIG_ARCH_EXYNOS4
-#include <linux/dma-mapping.h>
-#endif
-#include <linux/io.h>
-#include "s5p_mfc_common.h"
-#include "s5p_mfc_debug.h"
-
-int s5p_mfc_init_shm(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-	void *shm_alloc_ctx = dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
-
-	ctx->shm_alloc = vb2_dma_contig_memops.alloc(shm_alloc_ctx,
-							SHARED_BUF_SIZE);
-	if (IS_ERR(ctx->shm_alloc)) {
-		mfc_err("failed to allocate shared memory\n");
-		return PTR_ERR(ctx->shm_alloc);
-	}
-	/* shm_ofs only keeps the offset from base (port a) */
-	ctx->shm_ofs = s5p_mfc_mem_cookie(shm_alloc_ctx, ctx->shm_alloc)
-								- dev->bank1;
-	BUG_ON(ctx->shm_ofs & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
-	ctx->shm = vb2_dma_contig_memops.vaddr(ctx->shm_alloc);
-	if (!ctx->shm) {
-		vb2_dma_contig_memops.put(ctx->shm_alloc);
-		ctx->shm_ofs = 0;
-		ctx->shm_alloc = NULL;
-		mfc_err("failed to virt addr of shared memory\n");
-		return -ENOMEM;
-	}
-	memset((void *)ctx->shm, 0, SHARED_BUF_SIZE);
-	wmb();
-	return 0;
-}
-
diff --git a/drivers/media/platform/soc_camera/mx2_camera.c b/drivers/media/platform/soc_camera/mx2_camera.c
index 403d7f17bfa..9fd9d1c5b21 100644
--- a/drivers/media/platform/soc_camera/mx2_camera.c
+++ b/drivers/media/platform/soc_camera/mx2_camera.c
@@ -1376,6 +1376,7 @@ static int mx2_camera_try_fmt(struct soc_camera_device *icd,
 	__u32 pixfmt = pix->pixelformat;
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct mx2_camera_dev *pcdev = ici->priv;
+	struct mx2_fmt_cfg *emma_prp;
 	unsigned int width_limit;
 	int ret;
 
@@ -1438,12 +1439,11 @@ static int mx2_camera_try_fmt(struct soc_camera_device *icd,
 		__func__, pcdev->s_width, pcdev->s_height);
 
 	/* If the sensor does not support image size try PrP resizing */
-	pcdev->emma_prp = mx27_emma_prp_get_format(xlate->code,
+	emma_prp = mx27_emma_prp_get_format(xlate->code,
 						   xlate->host_fmt->fourcc);
 
-	memset(pcdev->resizing, 0, sizeof(pcdev->resizing));
 	if ((mf.width != pix->width || mf.height != pix->height) &&
-		pcdev->emma_prp->cfg.in_fmt == PRP_CNTL_DATA_IN_YUV422) {
+		emma_prp->cfg.in_fmt == PRP_CNTL_DATA_IN_YUV422) {
 		if (mx2_emmaprp_resize(pcdev, &mf, pix, false) < 0)
 			dev_dbg(icd->parent, "%s: can't resize\n", __func__);
 	}
@@ -1655,6 +1655,7 @@ static int __devinit mx27_camera_emma_init(struct platform_device *pdev)
 	irq_emma = platform_get_irq(pdev, 1);
 	if (!res_emma || !irq_emma) {
 		dev_err(pcdev->dev, "no EMMA resources\n");
+		err = -ENODEV;
 		goto out;
 	}
 
diff --git a/drivers/media/platform/soc_camera/soc_camera.c b/drivers/media/platform/soc_camera/soc_camera.c
index 3be92944f8e..d3f0b84e2d7 100644
--- a/drivers/media/platform/soc_camera/soc_camera.c
+++ b/drivers/media/platform/soc_camera/soc_camera.c
@@ -950,11 +950,11 @@ static int soc_camera_s_selection(struct file *file, void *fh,
 
 	/* In all these cases cropping emulation will not help */
 	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-	    (s->target != V4L2_SEL_TGT_COMPOSE_ACTIVE &&
-	     s->target != V4L2_SEL_TGT_CROP_ACTIVE))
+	    (s->target != V4L2_SEL_TGT_COMPOSE &&
+	     s->target != V4L2_SEL_TGT_CROP))
 		return -EINVAL;
 
-	if (s->target == V4L2_SEL_TGT_COMPOSE_ACTIVE) {
+	if (s->target == V4L2_SEL_TGT_COMPOSE) {
 		/* No output size change during a running capture! */
 		if (is_streaming(ici, icd) &&
 		    (icd->user_width != s->r.width ||
@@ -974,7 +974,7 @@ static int soc_camera_s_selection(struct file *file, void *fh,
 
 	ret = ici->ops->set_selection(icd, s);
 	if (!ret &&
-	    s->target == V4L2_SEL_TGT_COMPOSE_ACTIVE) {
+	    s->target == V4L2_SEL_TGT_COMPOSE) {
 		icd->user_width = s->r.width;
 		icd->user_height = s->r.height;
 		if (!icd->streamer)
@@ -1184,7 +1184,8 @@ static int soc_camera_probe(struct soc_camera_device *icd)
 	sd->grp_id = soc_camera_grp_id(icd);
 	v4l2_set_subdev_hostdata(sd, icd);
 
-	if (v4l2_ctrl_add_handler(&icd->ctrl_handler, sd->ctrl_handler, NULL))
+	ret = v4l2_ctrl_add_handler(&icd->ctrl_handler, sd->ctrl_handler, NULL);
+	if (ret < 0)
 		goto ectrl;
 
 	/* At this point client .probe() should have run already */
@@ -1529,12 +1530,11 @@ static int __devinit soc_camera_pdrv_probe(struct platform_device *pdev)
 {
 	struct soc_camera_link *icl = pdev->dev.platform_data;
 	struct soc_camera_device *icd;
-	int ret;
 
 	if (!icl)
 		return -EINVAL;
 
-	icd = kzalloc(sizeof(*icd), GFP_KERNEL);
+	icd = devm_kzalloc(&pdev->dev, sizeof(*icd), GFP_KERNEL);
 	if (!icd)
 		return -ENOMEM;
 
@@ -1543,19 +1543,10 @@ static int __devinit soc_camera_pdrv_probe(struct platform_device *pdev)
 	icd->pdev = &pdev->dev;
 	platform_set_drvdata(pdev, icd);
 
-	ret = soc_camera_device_register(icd);
-	if (ret < 0)
-		goto escdevreg;
-
 	icd->user_width		= DEFAULT_WIDTH;
 	icd->user_height	= DEFAULT_HEIGHT;
 
-	return 0;
-
-escdevreg:
-	kfree(icd);
-
-	return ret;
+	return soc_camera_device_register(icd);
 }
 
 /*
@@ -1572,8 +1563,6 @@ static int __devexit soc_camera_pdrv_remove(struct platform_device *pdev)
 
 	list_del(&icd->list);
 
-	kfree(icd);
-
 	return 0;
 }
 
@@ -1586,18 +1575,7 @@ static struct platform_driver __refdata soc_camera_pdrv = {
 	},
 };
 
-static int __init soc_camera_init(void)
-{
-	return platform_driver_register(&soc_camera_pdrv);
-}
-
-static void __exit soc_camera_exit(void)
-{
-	platform_driver_unregister(&soc_camera_pdrv);
-}
-
-module_init(soc_camera_init);
-module_exit(soc_camera_exit);
+module_platform_driver(soc_camera_pdrv);
 
 MODULE_DESCRIPTION("Image capture bus driver");
 MODULE_AUTHOR("Guennadi Liakhovetski <kernel@pengutronix.de>");
diff --git a/drivers/media/platform/vino.c b/drivers/media/platform/vino.c
index 790d96cffee..70b0bf4b290 100644
--- a/drivers/media/platform/vino.c
+++ b/drivers/media/platform/vino.c
@@ -3950,7 +3950,7 @@ found:
 
 	fb->map_count = 1;
 
-	vma->vm_flags |= VM_DONTEXPAND | VM_RESERVED;
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
 	vma->vm_flags &= ~VM_IO;
 	vma->vm_private_data = fb;
 	vma->vm_file = file;
diff --git a/drivers/media/radio/si470x/radio-si470x-i2c.c b/drivers/media/radio/si470x/radio-si470x-i2c.c
index e5024cfd27a..4ef55ec8045 100644
--- a/drivers/media/radio/si470x/radio-si470x-i2c.c
+++ b/drivers/media/radio/si470x/radio-si470x-i2c.c
@@ -308,7 +308,7 @@ static irqreturn_t si470x_i2c_interrupt(int irq, void *dev_id)
 					READCHAN_BLERD) >> 10;
 			rds = radio->registers[RDSD];
 			break;
-		};
+		}
 
 		/* Fill the V4L2 RDS buffer */
 		put_unaligned_le16(rds, &tmpbuf);
diff --git a/drivers/media/radio/si470x/radio-si470x-usb.c b/drivers/media/radio/si470x/radio-si470x-usb.c
index be076f7181e..62f3edec39b 100644
--- a/drivers/media/radio/si470x/radio-si470x-usb.c
+++ b/drivers/media/radio/si470x/radio-si470x-usb.c
@@ -446,7 +446,7 @@ static void si470x_int_in_callback(struct urb *urb)
 						READCHAN_BLERD) >> 10;
 				rds = radio->registers[RDSD];
 				break;
-			};
+			}
 
 			/* Fill the V4L2 RDS buffer */
 			put_unaligned_le16(rds, &tmpbuf);
diff --git a/drivers/media/radio/si4713-i2c.c b/drivers/media/radio/si4713-i2c.c
index a9e6d17015e..e3079c142c5 100644
--- a/drivers/media/radio/si4713-i2c.c
+++ b/drivers/media/radio/si4713-i2c.c
@@ -1009,7 +1009,7 @@ static int si4713_choose_econtrol_action(struct si4713_device *sdev, u32 id,
 
 	default:
 		rval = -EINVAL;
-	};
+	}
 
 	return rval;
 }
@@ -1081,7 +1081,7 @@ static int si4713_write_econtrol_string(struct si4713_device *sdev,
 	default:
 		rval = -EINVAL;
 		break;
-	};
+	}
 
 exit:
 	return rval;
@@ -1130,7 +1130,7 @@ static int si4713_write_econtrol_tune(struct si4713_device *sdev,
 	default:
 		rval = -EINVAL;
 		goto unlock;
-	};
+	}
 
 	if (sdev->power_state)
 		rval = si4713_tx_tune_power(sdev, power, antcap);
@@ -1420,7 +1420,7 @@ static int si4713_read_econtrol_string(struct si4713_device *sdev,
 	default:
 		rval = -EINVAL;
 		break;
-	};
+	}
 
 exit:
 	return rval;
@@ -1473,7 +1473,7 @@ static int si4713_read_econtrol_tune(struct si4713_device *sdev,
 		break;
 	default:
 		rval = -EINVAL;
-	};
+	}
 
 unlock:
 	mutex_unlock(&sdev->mutex);
@@ -1698,7 +1698,7 @@ static int si4713_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
 	default:
 		rval = -EINVAL;
 		break;
-	};
+	}
 
 	return rval;
 }
diff --git a/drivers/media/rc/ene_ir.c b/drivers/media/rc/ene_ir.c
index 647dd951b0e..d05ac15b5de 100644
--- a/drivers/media/rc/ene_ir.c
+++ b/drivers/media/rc/ene_ir.c
@@ -881,10 +881,13 @@ static int ene_set_tx_mask(struct rc_dev *rdev, u32 tx_mask)
 static int ene_set_tx_carrier(struct rc_dev *rdev, u32 carrier)
 {
 	struct ene_device *dev = rdev->priv;
-	u32 period = 2000000 / carrier;
+	u32 period;
 
 	dbg("TX: attempt to set tx carrier to %d kHz", carrier);
+	if (carrier == 0)
+		return -EINVAL;
 
+	period = 2000000 / carrier;
 	if (period && (period > ENE_CIRMOD_PRD_MAX ||
 			period < ENE_CIRMOD_PRD_MIN)) {
 
diff --git a/drivers/media/rc/iguanair.c b/drivers/media/rc/iguanair.c
index 1e4c68a5cec..51d7057aca0 100644
--- a/drivers/media/rc/iguanair.c
+++ b/drivers/media/rc/iguanair.c
@@ -28,6 +28,7 @@
 #include <media/rc-core.h>
 
 #define DRIVER_NAME "iguanair"
+#define BUF_SIZE 152
 
 struct iguanair {
 	struct rc_dev *rc;
@@ -35,26 +36,23 @@ struct iguanair {
 	struct device *dev;
 	struct usb_device *udev;
 
-	int pipe_out;
 	uint16_t version;
 	uint8_t bufsize;
+	uint8_t cycle_overhead;
 
 	struct mutex lock;
 
 	/* receiver support */
 	bool receiver_on;
-	dma_addr_t dma_in;
+	dma_addr_t dma_in, dma_out;
 	uint8_t *buf_in;
-	struct urb *urb_in;
+	struct urb *urb_in, *urb_out;
 	struct completion completion;
 
 	/* transmit support */
 	bool tx_overflow;
 	uint32_t carrier;
-	uint8_t cycle_overhead;
-	uint8_t channels;
-	uint8_t busy4;
-	uint8_t busy7;
+	struct send_packet *packet;
 
 	char name[64];
 	char phys[64];
@@ -73,7 +71,8 @@ struct iguanair {
 #define DIR_IN			0xdc
 #define DIR_OUT			0xcd
 
-#define MAX_PACKET_SIZE		8u
+#define MAX_IN_PACKET		8u
+#define MAX_OUT_PACKET		(sizeof(struct send_packet) + BUF_SIZE)
 #define TIMEOUT			1000
 #define RX_RESOLUTION		21333
 
@@ -191,20 +190,25 @@ static void iguanair_rx(struct urb *urb)
 		dev_warn(ir->dev, "failed to resubmit urb: %d\n", rc);
 }
 
-static int iguanair_send(struct iguanair *ir, void *data, unsigned size)
+static void iguanair_irq_out(struct urb *urb)
 {
-	int rc, transferred;
+	struct iguanair *ir = urb->context;
+
+	if (urb->status)
+		dev_dbg(ir->dev, "Error: out urb status = %d\n", urb->status);
+}
+
+static int iguanair_send(struct iguanair *ir, unsigned size)
+{
+	int rc;
 
 	INIT_COMPLETION(ir->completion);
 
-	rc = usb_interrupt_msg(ir->udev, ir->pipe_out, data, size,
-							&transferred, TIMEOUT);
+	ir->urb_out->transfer_buffer_length = size;
+	rc = usb_submit_urb(ir->urb_out, GFP_KERNEL);
 	if (rc)
 		return rc;
 
-	if (transferred != size)
-		return -EIO;
-
 	if (wait_for_completion_timeout(&ir->completion, TIMEOUT) == 0)
 		return -ETIMEDOUT;
 
@@ -213,14 +217,13 @@ static int iguanair_send(struct iguanair *ir, void *data, unsigned size)
 
 static int iguanair_get_features(struct iguanair *ir)
 {
-	struct packet packet;
 	int rc;
 
-	packet.start = 0;
-	packet.direction = DIR_OUT;
-	packet.cmd = CMD_GET_VERSION;
+	ir->packet->header.start = 0;
+	ir->packet->header.direction = DIR_OUT;
+	ir->packet->header.cmd = CMD_GET_VERSION;
 
-	rc = iguanair_send(ir, &packet, sizeof(packet));
+	rc = iguanair_send(ir, sizeof(ir->packet->header));
 	if (rc) {
 		dev_info(ir->dev, "failed to get version\n");
 		goto out;
@@ -235,17 +238,23 @@ static int iguanair_get_features(struct iguanair *ir)
 	ir->bufsize = 150;
 	ir->cycle_overhead = 65;
 
-	packet.cmd = CMD_GET_BUFSIZE;
+	ir->packet->header.cmd = CMD_GET_BUFSIZE;
 
-	rc = iguanair_send(ir, &packet, sizeof(packet));
+	rc = iguanair_send(ir, sizeof(ir->packet->header));
 	if (rc) {
 		dev_info(ir->dev, "failed to get buffer size\n");
 		goto out;
 	}
 
-	packet.cmd = CMD_GET_FEATURES;
+	if (ir->bufsize > BUF_SIZE) {
+		dev_info(ir->dev, "buffer size %u larger than expected\n",
+								ir->bufsize);
+		ir->bufsize = BUF_SIZE;
+	}
+
+	ir->packet->header.cmd = CMD_GET_FEATURES;
 
-	rc = iguanair_send(ir, &packet, sizeof(packet));
+	rc = iguanair_send(ir, sizeof(ir->packet->header));
 	if (rc) {
 		dev_info(ir->dev, "failed to get features\n");
 		goto out;
@@ -257,13 +266,18 @@ out:
 
 static int iguanair_receiver(struct iguanair *ir, bool enable)
 {
-	struct packet packet = { 0, DIR_OUT, enable ?
-				CMD_RECEIVER_ON : CMD_RECEIVER_OFF };
+	int rc;
+
+	ir->packet->header.start = 0;
+	ir->packet->header.direction = DIR_OUT;
+	ir->packet->header.cmd = enable ? CMD_RECEIVER_ON : CMD_RECEIVER_OFF;
 
 	if (enable)
 		ir_raw_event_reset(ir->rc);
 
-	return iguanair_send(ir, &packet, sizeof(packet));
+	rc = iguanair_send(ir, sizeof(ir->packet->header));
+
+	return rc;
 }
 
 /*
@@ -308,8 +322,8 @@ static int iguanair_set_tx_carrier(struct rc_dev *dev, uint32_t carrier)
 		fours = (cycles - sevens * 7) / 4;
 
 		/* magic happens here */
-		ir->busy7 = (4 - sevens) * 2;
-		ir->busy4 = 110 - fours;
+		ir->packet->busy7 = (4 - sevens) * 2;
+		ir->packet->busy4 = 110 - fours;
 	}
 
 	mutex_unlock(&ir->lock);
@@ -325,7 +339,7 @@ static int iguanair_set_tx_mask(struct rc_dev *dev, uint32_t mask)
 		return 4;
 
 	mutex_lock(&ir->lock);
-	ir->channels = mask;
+	ir->packet->channels = mask << 4;
 	mutex_unlock(&ir->lock);
 
 	return 0;
@@ -337,16 +351,9 @@ static int iguanair_tx(struct rc_dev *dev, unsigned *txbuf, unsigned count)
 	uint8_t space;
 	unsigned i, size, periods, bytes;
 	int rc;
-	struct send_packet *packet;
 
 	mutex_lock(&ir->lock);
 
-	packet = kmalloc(sizeof(*packet) + ir->bufsize, GFP_KERNEL);
-	if (!packet) {
-		rc = -ENOMEM;
-		goto out;
-	}
-
 	/* convert from us to carrier periods */
 	for (i = space = size = 0; i < count; i++) {
 		periods = DIV_ROUND_CLOSEST(txbuf[i] * ir->carrier, 1000000);
@@ -356,11 +363,11 @@ static int iguanair_tx(struct rc_dev *dev, unsigned *txbuf, unsigned count)
 			break;
 		}
 		while (periods > 127) {
-			packet->payload[size++] = 127 | space;
+			ir->packet->payload[size++] = 127 | space;
 			periods -= 127;
 		}
 
-		packet->payload[size++] = periods | space;
+		ir->packet->payload[size++] = periods | space;
 		space ^= 0x80;
 	}
 
@@ -369,36 +376,19 @@ static int iguanair_tx(struct rc_dev *dev, unsigned *txbuf, unsigned count)
 		goto out;
 	}
 
-	packet->header.start = 0;
-	packet->header.direction = DIR_OUT;
-	packet->header.cmd = CMD_SEND;
-	packet->length = size;
-	packet->channels = ir->channels << 4;
-	packet->busy7 = ir->busy7;
-	packet->busy4 = ir->busy4;
-
-	if (ir->receiver_on) {
-		rc = iguanair_receiver(ir, false);
-		if (rc) {
-			dev_warn(ir->dev, "disable receiver before transmit failed\n");
-			goto out;
-		}
-	}
+	ir->packet->header.start = 0;
+	ir->packet->header.direction = DIR_OUT;
+	ir->packet->header.cmd = CMD_SEND;
+	ir->packet->length = size;
 
 	ir->tx_overflow = false;
 
-	rc = iguanair_send(ir, packet, size + 8);
+	rc = iguanair_send(ir, sizeof(*ir->packet) + size);
 
 	if (rc == 0 && ir->tx_overflow)
 		rc = -EOVERFLOW;
 
-	if (ir->receiver_on) {
-		if (iguanair_receiver(ir, true))
-			dev_warn(ir->dev, "re-enable receiver after transmit failed\n");
-	}
-
 out:
-	kfree(packet);
 	mutex_unlock(&ir->lock);
 
 	return rc ? rc : count;
@@ -411,8 +401,6 @@ static int iguanair_open(struct rc_dev *rdev)
 
 	mutex_lock(&ir->lock);
 
-	BUG_ON(ir->receiver_on);
-
 	rc = iguanair_receiver(ir, true);
 	if (rc == 0)
 		ir->receiver_on = true;
@@ -443,7 +431,7 @@ static int __devinit iguanair_probe(struct usb_interface *intf,
 	struct usb_device *udev = interface_to_usbdev(intf);
 	struct iguanair *ir;
 	struct rc_dev *rc;
-	int ret, pipein;
+	int ret, pipein, pipeout;
 	struct usb_host_interface *idesc;
 
 	ir = kzalloc(sizeof(*ir), GFP_KERNEL);
@@ -453,11 +441,14 @@ static int __devinit iguanair_probe(struct usb_interface *intf,
 		goto out;
 	}
 
-	ir->buf_in = usb_alloc_coherent(udev, MAX_PACKET_SIZE, GFP_KERNEL,
+	ir->buf_in = usb_alloc_coherent(udev, MAX_IN_PACKET, GFP_KERNEL,
 								&ir->dma_in);
+	ir->packet = usb_alloc_coherent(udev, MAX_OUT_PACKET, GFP_KERNEL,
+								&ir->dma_out);
 	ir->urb_in = usb_alloc_urb(0, GFP_KERNEL);
+	ir->urb_out = usb_alloc_urb(0, GFP_KERNEL);
 
-	if (!ir->buf_in || !ir->urb_in) {
+	if (!ir->buf_in || !ir->packet || !ir->urb_in || !ir->urb_out) {
 		ret = -ENOMEM;
 		goto out;
 	}
@@ -472,13 +463,18 @@ static int __devinit iguanair_probe(struct usb_interface *intf,
 	ir->rc = rc;
 	ir->dev = &intf->dev;
 	ir->udev = udev;
-	ir->pipe_out = usb_sndintpipe(udev,
-				idesc->endpoint[1].desc.bEndpointAddress);
 	mutex_init(&ir->lock);
+
 	init_completion(&ir->completion);
+	pipeout = usb_sndintpipe(udev,
+				idesc->endpoint[1].desc.bEndpointAddress);
+	usb_fill_int_urb(ir->urb_out, udev, pipeout, ir->packet, MAX_OUT_PACKET,
+						iguanair_irq_out, ir, 1);
+	ir->urb_out->transfer_dma = ir->dma_out;
+	ir->urb_out->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
 
 	pipein = usb_rcvintpipe(udev, idesc->endpoint[0].desc.bEndpointAddress);
-	usb_fill_int_urb(ir->urb_in, udev, pipein, ir->buf_in, MAX_PACKET_SIZE,
+	usb_fill_int_urb(ir->urb_in, udev, pipein, ir->buf_in, MAX_IN_PACKET,
 							 iguanair_rx, ir, 1);
 	ir->urb_in->transfer_dma = ir->dma_in;
 	ir->urb_in->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
@@ -528,11 +524,14 @@ static int __devinit iguanair_probe(struct usb_interface *intf,
 	return 0;
 out2:
 	usb_kill_urb(ir->urb_in);
+	usb_kill_urb(ir->urb_out);
 out:
 	if (ir) {
 		usb_free_urb(ir->urb_in);
-		usb_free_coherent(udev, MAX_PACKET_SIZE, ir->buf_in,
-								ir->dma_in);
+		usb_free_urb(ir->urb_out);
+		usb_free_coherent(udev, MAX_IN_PACKET, ir->buf_in, ir->dma_in);
+		usb_free_coherent(udev, MAX_OUT_PACKET, ir->packet,
+								ir->dma_out);
 	}
 	rc_free_device(rc);
 	kfree(ir);
@@ -546,8 +545,11 @@ static void __devexit iguanair_disconnect(struct usb_interface *intf)
 	rc_unregister_device(ir->rc);
 	usb_set_intfdata(intf, NULL);
 	usb_kill_urb(ir->urb_in);
+	usb_kill_urb(ir->urb_out);
 	usb_free_urb(ir->urb_in);
-	usb_free_coherent(ir->udev, MAX_PACKET_SIZE, ir->buf_in, ir->dma_in);
+	usb_free_urb(ir->urb_out);
+	usb_free_coherent(ir->udev, MAX_IN_PACKET, ir->buf_in, ir->dma_in);
+	usb_free_coherent(ir->udev, MAX_OUT_PACKET, ir->packet, ir->dma_out);
 	kfree(ir);
 }
 
@@ -565,6 +567,7 @@ static int iguanair_suspend(struct usb_interface *intf, pm_message_t message)
 	}
 
 	usb_kill_urb(ir->urb_in);
+	usb_kill_urb(ir->urb_out);
 
 	mutex_unlock(&ir->lock);
 
diff --git a/drivers/media/rc/ir-lirc-codec.c b/drivers/media/rc/ir-lirc-codec.c
index 569124b03de..870c93052fd 100644
--- a/drivers/media/rc/ir-lirc-codec.c
+++ b/drivers/media/rc/ir-lirc-codec.c
@@ -203,13 +203,13 @@ static long ir_lirc_ioctl(struct file *filep, unsigned int cmd,
 	/* TX settings */
 	case LIRC_SET_TRANSMITTER_MASK:
 		if (!dev->s_tx_mask)
-			return -EINVAL;
+			return -ENOSYS;
 
 		return dev->s_tx_mask(dev, val);
 
 	case LIRC_SET_SEND_CARRIER:
 		if (!dev->s_tx_carrier)
-			return -EINVAL;
+			return -ENOSYS;
 
 		return dev->s_tx_carrier(dev, val);
 
diff --git a/drivers/media/rc/keymaps/rc-msi-digivox-ii.c b/drivers/media/rc/keymaps/rc-msi-digivox-ii.c
index c64e9e30045..2fa71d0d72d 100644
--- a/drivers/media/rc/keymaps/rc-msi-digivox-ii.c
+++ b/drivers/media/rc/keymaps/rc-msi-digivox-ii.c
@@ -22,24 +22,24 @@
 #include <linux/module.h>
 
 static struct rc_map_table msi_digivox_ii[] = {
-	{ 0x0002, KEY_2 },
-	{ 0x0003, KEY_UP },              /* up */
-	{ 0x0004, KEY_3 },
-	{ 0x0005, KEY_CHANNELDOWN },
-	{ 0x0008, KEY_5 },
-	{ 0x0009, KEY_0 },
-	{ 0x000b, KEY_8 },
-	{ 0x000d, KEY_DOWN },            /* down */
-	{ 0x0010, KEY_9 },
-	{ 0x0011, KEY_7 },
-	{ 0x0014, KEY_VOLUMEUP },
-	{ 0x0015, KEY_CHANNELUP },
-	{ 0x0016, KEY_OK },
-	{ 0x0017, KEY_POWER2 },
-	{ 0x001a, KEY_1 },
-	{ 0x001c, KEY_4 },
-	{ 0x001d, KEY_6 },
-	{ 0x001f, KEY_VOLUMEDOWN },
+	{ 0x0302, KEY_2 },
+	{ 0x0303, KEY_UP },              /* up */
+	{ 0x0304, KEY_3 },
+	{ 0x0305, KEY_CHANNELDOWN },
+	{ 0x0308, KEY_5 },
+	{ 0x0309, KEY_0 },
+	{ 0x030b, KEY_8 },
+	{ 0x030d, KEY_DOWN },            /* down */
+	{ 0x0310, KEY_9 },
+	{ 0x0311, KEY_7 },
+	{ 0x0314, KEY_VOLUMEUP },
+	{ 0x0315, KEY_CHANNELUP },
+	{ 0x0316, KEY_OK },
+	{ 0x0317, KEY_POWER2 },
+	{ 0x031a, KEY_1 },
+	{ 0x031c, KEY_4 },
+	{ 0x031d, KEY_6 },
+	{ 0x031f, KEY_VOLUMEDOWN },
 };
 
 static struct rc_map_list msi_digivox_ii_map = {
diff --git a/drivers/media/rc/nuvoton-cir.c b/drivers/media/rc/nuvoton-cir.c
index 699eef39128..2ea913a44ae 100644
--- a/drivers/media/rc/nuvoton-cir.c
+++ b/drivers/media/rc/nuvoton-cir.c
@@ -517,6 +517,9 @@ static int nvt_set_tx_carrier(struct rc_dev *dev, u32 carrier)
 	struct nvt_dev *nvt = dev->priv;
 	u16 val;
 
+	if (carrier == 0)
+		return -EINVAL;
+
 	nvt_cir_reg_write(nvt, 1, CIR_CP);
 	val = 3000000 / (carrier) - 1;
 	nvt_cir_reg_write(nvt, val & 0xff, CIR_CC);
diff --git a/drivers/media/rc/redrat3.c b/drivers/media/rc/redrat3.c
index 49731b1a9c5..9f5a17bb5ef 100644
--- a/drivers/media/rc/redrat3.c
+++ b/drivers/media/rc/redrat3.c
@@ -890,6 +890,9 @@ static int redrat3_set_tx_carrier(struct rc_dev *rcdev, u32 carrier)
 	struct device *dev = rr3->dev;
 
 	rr3_dbg(dev, "Setting modulation frequency to %u", carrier);
+	if (carrier == 0)
+		return -EINVAL;
+
 	rr3->carrier = carrier;
 
 	return carrier;
diff --git a/drivers/media/rc/winbond-cir.c b/drivers/media/rc/winbond-cir.c
index 30ae1f24abc..7c9b5f33113 100644
--- a/drivers/media/rc/winbond-cir.c
+++ b/drivers/media/rc/winbond-cir.c
@@ -184,7 +184,7 @@ enum wbcir_txstate {
 };
 
 /* Misc */
-#define WBCIR_NAME	"Winbond CIR"
+#define WBCIR_NAME	"winbond-cir"
 #define WBCIR_ID_FAMILY          0xF1 /* Family ID for the WPCD376I	*/
 #define	WBCIR_ID_CHIP            0x04 /* Chip ID for the WPCD376I	*/
 #define INVALID_SCANCODE   0x7FFFFFFF /* Invalid with all protos	*/
diff --git a/drivers/media/tuners/mt2063.c b/drivers/media/tuners/mt2063.c
index 0ed9091ff48..2e1a02e360f 100644
--- a/drivers/media/tuners/mt2063.c
+++ b/drivers/media/tuners/mt2063.c
@@ -245,7 +245,7 @@ struct mt2063_state {
 /*
  * mt2063_write - Write data into the I2C bus
  */
-static u32 mt2063_write(struct mt2063_state *state, u8 reg, u8 *data, u32 len)
+static int mt2063_write(struct mt2063_state *state, u8 reg, u8 *data, u32 len)
 {
 	struct dvb_frontend *fe = state->frontend;
 	int ret;
@@ -277,9 +277,9 @@ static u32 mt2063_write(struct mt2063_state *state, u8 reg, u8 *data, u32 len)
 /*
  * mt2063_write - Write register data into the I2C bus, caching the value
  */
-static u32 mt2063_setreg(struct mt2063_state *state, u8 reg, u8 val)
+static int mt2063_setreg(struct mt2063_state *state, u8 reg, u8 val)
 {
-	u32 status;
+	int status;
 
 	dprintk(2, "\n");
 
@@ -298,10 +298,10 @@ static u32 mt2063_setreg(struct mt2063_state *state, u8 reg, u8 val)
 /*
  * mt2063_read - Read data from the I2C bus
  */
-static u32 mt2063_read(struct mt2063_state *state,
+static int mt2063_read(struct mt2063_state *state,
 			   u8 subAddress, u8 *pData, u32 cnt)
 {
-	u32 status = 0;	/* Status to be returned        */
+	int status = 0;	/* Status to be returned        */
 	struct dvb_frontend *fe = state->frontend;
 	u32 i = 0;
 
@@ -816,7 +816,7 @@ static u32 IsSpurInBand(struct MT2063_AvoidSpursData_t *pAS_Info,
  */
 static u32 MT2063_AvoidSpurs(struct MT2063_AvoidSpursData_t *pAS_Info)
 {
-	u32 status = 0;
+	int status = 0;
 	u32 fm, fp;		/*  restricted range on LO's        */
 	pAS_Info->bSpurAvoided = 0;
 	pAS_Info->nSpursFound = 0;
@@ -935,14 +935,14 @@ static u32 MT2063_AvoidSpurs(struct MT2063_AvoidSpursData_t *pAS_Info)
  *
  * This function returns 0, if no lock, 1 if locked and a value < 1 if error
  */
-static unsigned int mt2063_lockStatus(struct mt2063_state *state)
+static int mt2063_lockStatus(struct mt2063_state *state)
 {
 	const u32 nMaxWait = 100;	/*  wait a maximum of 100 msec   */
 	const u32 nPollRate = 2;	/*  poll status bits every 2 ms */
 	const u32 nMaxLoops = nMaxWait / nPollRate;
 	const u8 LO1LK = 0x80;
 	u8 LO2LK = 0x08;
-	u32 status;
+	int status;
 	u32 nDelays = 0;
 
 	dprintk(2, "\n");
@@ -1069,7 +1069,7 @@ static u32 mt2063_get_dnc_output_enable(struct mt2063_state *state,
 static u32 mt2063_set_dnc_output_enable(struct mt2063_state *state,
 					enum MT2063_DNC_Output_Enable nValue)
 {
-	u32 status = 0;	/* Status to be returned        */
+	int status = 0;	/* Status to be returned        */
 	u8 val = 0;
 
 	dprintk(2, "\n");
@@ -1203,7 +1203,7 @@ static u32 mt2063_set_dnc_output_enable(struct mt2063_state *state,
 static u32 MT2063_SetReceiverMode(struct mt2063_state *state,
 				  enum mt2063_delivery_sys Mode)
 {
-	u32 status = 0;	/* Status to be returned        */
+	int status = 0;	/* Status to be returned        */
 	u8 val;
 	u32 longval;
 
@@ -1345,7 +1345,7 @@ static u32 MT2063_SetReceiverMode(struct mt2063_state *state,
 static u32 MT2063_ClearPowerMaskBits(struct mt2063_state *state,
 				     enum MT2063_Mask_Bits Bits)
 {
-	u32 status = 0;
+	int status = 0;
 
 	dprintk(2, "\n");
 	Bits = (enum MT2063_Mask_Bits)(Bits & MT2063_ALL_SD);	/* Only valid bits for this tuner */
@@ -1374,7 +1374,7 @@ static u32 MT2063_ClearPowerMaskBits(struct mt2063_state *state,
  */
 static u32 MT2063_SoftwareShutdown(struct mt2063_state *state, u8 Shutdown)
 {
-	u32 status;
+	int status;
 
 	dprintk(2, "\n");
 	if (Shutdown == 1)
@@ -1540,7 +1540,7 @@ static u32 FindClearTuneFilter(struct mt2063_state *state, u32 f_in)
 static u32 MT2063_Tune(struct mt2063_state *state, u32 f_in)
 {				/* RF input center frequency   */
 
-	u32 status = 0;
+	int status = 0;
 	u32 LO1;		/*  1st LO register value           */
 	u32 Num1;		/*  Numerator for LO1 reg. value    */
 	u32 f_IF1;		/*  1st IF requested                */
@@ -1803,7 +1803,7 @@ static const u8 MT2063B3_defaults[] = {
 
 static int mt2063_init(struct dvb_frontend *fe)
 {
-	u32 status;
+	int status;
 	struct mt2063_state *state = fe->tuner_priv;
 	u8 all_resets = 0xF0;	/* reset/load bits */
 	const u8 *def = NULL;
@@ -2249,8 +2249,8 @@ struct dvb_frontend *mt2063_attach(struct dvb_frontend *fe,
 	dprintk(2, "\n");
 
 	state = kzalloc(sizeof(struct mt2063_state), GFP_KERNEL);
-	if (state == NULL)
-		goto error;
+	if (!state)
+		return NULL;
 
 	state->config = config;
 	state->i2c = i2c;
@@ -2261,18 +2261,15 @@ struct dvb_frontend *mt2063_attach(struct dvb_frontend *fe,
 
 	printk(KERN_INFO "%s: Attaching MT2063\n", __func__);
 	return fe;
-
-error:
-	kfree(state);
-	return NULL;
 }
 EXPORT_SYMBOL_GPL(mt2063_attach);
 
+#if 0
 /*
  * Ancillary routines visible outside mt2063
  * FIXME: Remove them in favor of using standard tuner callbacks
  */
-unsigned int tuner_MT2063_SoftwareShutdown(struct dvb_frontend *fe)
+static int tuner_MT2063_SoftwareShutdown(struct dvb_frontend *fe)
 {
 	struct mt2063_state *state = fe->tuner_priv;
 	int err = 0;
@@ -2285,9 +2282,8 @@ unsigned int tuner_MT2063_SoftwareShutdown(struct dvb_frontend *fe)
 
 	return err;
 }
-EXPORT_SYMBOL_GPL(tuner_MT2063_SoftwareShutdown);
 
-unsigned int tuner_MT2063_ClearPowerMaskBits(struct dvb_frontend *fe)
+static int tuner_MT2063_ClearPowerMaskBits(struct dvb_frontend *fe)
 {
 	struct mt2063_state *state = fe->tuner_priv;
 	int err = 0;
@@ -2300,7 +2296,7 @@ unsigned int tuner_MT2063_ClearPowerMaskBits(struct dvb_frontend *fe)
 
 	return err;
 }
-EXPORT_SYMBOL_GPL(tuner_MT2063_ClearPowerMaskBits);
+#endif
 
 MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
 MODULE_DESCRIPTION("MT2063 Silicon tuner");
diff --git a/drivers/media/tuners/mt2063.h b/drivers/media/tuners/mt2063.h
index 3f5cfd93713..ab24170c157 100644
--- a/drivers/media/tuners/mt2063.h
+++ b/drivers/media/tuners/mt2063.h
@@ -23,10 +23,6 @@ static inline struct dvb_frontend *mt2063_attach(struct dvb_frontend *fe,
 	return NULL;
 }
 
-/* FIXME: Should use the standard DVB attachment interfaces */
-unsigned int tuner_MT2063_SoftwareShutdown(struct dvb_frontend *fe);
-unsigned int tuner_MT2063_ClearPowerMaskBits(struct dvb_frontend *fe);
-
 #endif /* CONFIG_DVB_MT2063 */
 
 #endif /* __MT2063_H__ */
diff --git a/drivers/media/tuners/tda18271-common.c b/drivers/media/tuners/tda18271-common.c
index 221171eeb0c..18c77afe2e4 100644
--- a/drivers/media/tuners/tda18271-common.c
+++ b/drivers/media/tuners/tda18271-common.c
@@ -187,7 +187,8 @@ int tda18271_read_extended(struct dvb_frontend *fe)
 	return (ret == 2 ? 0 : ret);
 }
 
-int tda18271_write_regs(struct dvb_frontend *fe, int idx, int len)
+static int __tda18271_write_regs(struct dvb_frontend *fe, int idx, int len,
+			bool lock_i2c)
 {
 	struct tda18271_priv *priv = fe->tuner_priv;
 	unsigned char *regs = priv->tda18271_regs;
@@ -198,7 +199,6 @@ int tda18271_write_regs(struct dvb_frontend *fe, int idx, int len)
 
 	BUG_ON((len == 0) || (idx + len > sizeof(buf)));
 
-
 	switch (priv->small_i2c) {
 	case TDA18271_03_BYTE_CHUNK_INIT:
 		max = 3;
@@ -214,7 +214,19 @@ int tda18271_write_regs(struct dvb_frontend *fe, int idx, int len)
 		max = 39;
 	}
 
-	tda18271_i2c_gate_ctrl(fe, 1);
+
+	/*
+	 * If lock_i2c is true, it will take the I2C bus for tda18271 private
+	 * usage during the entire write ops, as otherwise, bad things could
+	 * happen.
+	 * During device init, several write operations will happen. So,
+	 * tda18271_init_regs controls the I2C lock directly,
+	 * disabling lock_i2c here.
+	 */
+	if (lock_i2c) {
+		tda18271_i2c_gate_ctrl(fe, 1);
+		i2c_lock_adapter(priv->i2c_props.adap);
+	}
 	while (len) {
 		if (max > len)
 			max = len;
@@ -226,14 +238,17 @@ int tda18271_write_regs(struct dvb_frontend *fe, int idx, int len)
 		msg.len = max + 1;
 
 		/* write registers */
-		ret = i2c_transfer(priv->i2c_props.adap, &msg, 1);
+		ret = __i2c_transfer(priv->i2c_props.adap, &msg, 1);
 		if (ret != 1)
 			break;
 
 		idx += max;
 		len -= max;
 	}
-	tda18271_i2c_gate_ctrl(fe, 0);
+	if (lock_i2c) {
+		i2c_unlock_adapter(priv->i2c_props.adap);
+		tda18271_i2c_gate_ctrl(fe, 0);
+	}
 
 	if (ret != 1)
 		tda_err("ERROR: idx = 0x%x, len = %d, "
@@ -242,10 +257,16 @@ int tda18271_write_regs(struct dvb_frontend *fe, int idx, int len)
 	return (ret == 1 ? 0 : ret);
 }
 
+int tda18271_write_regs(struct dvb_frontend *fe, int idx, int len)
+{
+	return __tda18271_write_regs(fe, idx, len, true);
+}
+
 /*---------------------------------------------------------------------*/
 
-int tda18271_charge_pump_source(struct dvb_frontend *fe,
-				enum tda18271_pll pll, int force)
+static int __tda18271_charge_pump_source(struct dvb_frontend *fe,
+					 enum tda18271_pll pll, int force,
+					 bool lock_i2c)
 {
 	struct tda18271_priv *priv = fe->tuner_priv;
 	unsigned char *regs = priv->tda18271_regs;
@@ -255,9 +276,16 @@ int tda18271_charge_pump_source(struct dvb_frontend *fe,
 	regs[r_cp] &= ~0x20;
 	regs[r_cp] |= ((force & 1) << 5);
 
-	return tda18271_write_regs(fe, r_cp, 1);
+	return __tda18271_write_regs(fe, r_cp, 1, lock_i2c);
+}
+
+int tda18271_charge_pump_source(struct dvb_frontend *fe,
+				enum tda18271_pll pll, int force)
+{
+	return __tda18271_charge_pump_source(fe, pll, force, true);
 }
 
+
 int tda18271_init_regs(struct dvb_frontend *fe)
 {
 	struct tda18271_priv *priv = fe->tuner_priv;
@@ -267,6 +295,13 @@ int tda18271_init_regs(struct dvb_frontend *fe)
 		i2c_adapter_id(priv->i2c_props.adap),
 		priv->i2c_props.addr);
 
+	/*
+	 * Don't let any other I2C transfer to happen at adapter during init,
+	 * as those could cause bad things
+	 */
+	tda18271_i2c_gate_ctrl(fe, 1);
+	i2c_lock_adapter(priv->i2c_props.adap);
+
 	/* initialize registers */
 	switch (priv->id) {
 	case TDA18271HDC1:
@@ -352,28 +387,28 @@ int tda18271_init_regs(struct dvb_frontend *fe)
 	regs[R_EB22] = 0x48;
 	regs[R_EB23] = 0xb0;
 
-	tda18271_write_regs(fe, 0x00, TDA18271_NUM_REGS);
+	__tda18271_write_regs(fe, 0x00, TDA18271_NUM_REGS, false);
 
 	/* setup agc1 gain */
 	regs[R_EB17] = 0x00;
-	tda18271_write_regs(fe, R_EB17, 1);
+	__tda18271_write_regs(fe, R_EB17, 1, false);
 	regs[R_EB17] = 0x03;
-	tda18271_write_regs(fe, R_EB17, 1);
+	__tda18271_write_regs(fe, R_EB17, 1, false);
 	regs[R_EB17] = 0x43;
-	tda18271_write_regs(fe, R_EB17, 1);
+	__tda18271_write_regs(fe, R_EB17, 1, false);
 	regs[R_EB17] = 0x4c;
-	tda18271_write_regs(fe, R_EB17, 1);
+	__tda18271_write_regs(fe, R_EB17, 1, false);
 
 	/* setup agc2 gain */
 	if ((priv->id) == TDA18271HDC1) {
 		regs[R_EB20] = 0xa0;
-		tda18271_write_regs(fe, R_EB20, 1);
+		__tda18271_write_regs(fe, R_EB20, 1, false);
 		regs[R_EB20] = 0xa7;
-		tda18271_write_regs(fe, R_EB20, 1);
+		__tda18271_write_regs(fe, R_EB20, 1, false);
 		regs[R_EB20] = 0xe7;
-		tda18271_write_regs(fe, R_EB20, 1);
+		__tda18271_write_regs(fe, R_EB20, 1, false);
 		regs[R_EB20] = 0xec;
-		tda18271_write_regs(fe, R_EB20, 1);
+		__tda18271_write_regs(fe, R_EB20, 1, false);
 	}
 
 	/* image rejection calibration */
@@ -391,21 +426,21 @@ int tda18271_init_regs(struct dvb_frontend *fe)
 	regs[R_MD2] = 0x08;
 	regs[R_MD3] = 0x00;
 
-	tda18271_write_regs(fe, R_EP3, 11);
+	__tda18271_write_regs(fe, R_EP3, 11, false);
 
 	if ((priv->id) == TDA18271HDC2) {
 		/* main pll cp source on */
-		tda18271_charge_pump_source(fe, TDA18271_MAIN_PLL, 1);
+		__tda18271_charge_pump_source(fe, TDA18271_MAIN_PLL, 1, false);
 		msleep(1);
 
 		/* main pll cp source off */
-		tda18271_charge_pump_source(fe, TDA18271_MAIN_PLL, 0);
+		__tda18271_charge_pump_source(fe, TDA18271_MAIN_PLL, 0, false);
 	}
 
 	msleep(5); /* pll locking */
 
 	/* launch detector */
-	tda18271_write_regs(fe, R_EP1, 1);
+	__tda18271_write_regs(fe, R_EP1, 1, false);
 	msleep(5); /* wanted low measurement */
 
 	regs[R_EP5] = 0x85;
@@ -413,11 +448,11 @@ int tda18271_init_regs(struct dvb_frontend *fe)
 	regs[R_CD1] = 0x66;
 	regs[R_CD2] = 0x70;
 
-	tda18271_write_regs(fe, R_EP3, 7);
+	__tda18271_write_regs(fe, R_EP3, 7, false);
 	msleep(5); /* pll locking */
 
 	/* launch optimization algorithm */
-	tda18271_write_regs(fe, R_EP2, 1);
+	__tda18271_write_regs(fe, R_EP2, 1, false);
 	msleep(30); /* image low optimization completion */
 
 	/* mid-band */
@@ -428,11 +463,11 @@ int tda18271_init_regs(struct dvb_frontend *fe)
 	regs[R_MD1] = 0x73;
 	regs[R_MD2] = 0x1a;
 
-	tda18271_write_regs(fe, R_EP3, 11);
+	__tda18271_write_regs(fe, R_EP3, 11, false);
 	msleep(5); /* pll locking */
 
 	/* launch detector */
-	tda18271_write_regs(fe, R_EP1, 1);
+	__tda18271_write_regs(fe, R_EP1, 1, false);
 	msleep(5); /* wanted mid measurement */
 
 	regs[R_EP5] = 0x86;
@@ -440,11 +475,11 @@ int tda18271_init_regs(struct dvb_frontend *fe)
 	regs[R_CD1] = 0x66;
 	regs[R_CD2] = 0xa0;
 
-	tda18271_write_regs(fe, R_EP3, 7);
+	__tda18271_write_regs(fe, R_EP3, 7, false);
 	msleep(5); /* pll locking */
 
 	/* launch optimization algorithm */
-	tda18271_write_regs(fe, R_EP2, 1);
+	__tda18271_write_regs(fe, R_EP2, 1, false);
 	msleep(30); /* image mid optimization completion */
 
 	/* high-band */
@@ -456,30 +491,33 @@ int tda18271_init_regs(struct dvb_frontend *fe)
 	regs[R_MD1] = 0x71;
 	regs[R_MD2] = 0xcd;
 
-	tda18271_write_regs(fe, R_EP3, 11);
+	__tda18271_write_regs(fe, R_EP3, 11, false);
 	msleep(5); /* pll locking */
 
 	/* launch detector */
-	tda18271_write_regs(fe, R_EP1, 1);
+	__tda18271_write_regs(fe, R_EP1, 1, false);
 	msleep(5); /* wanted high measurement */
 
 	regs[R_EP5] = 0x87;
 	regs[R_CD1] = 0x65;
 	regs[R_CD2] = 0x50;
 
-	tda18271_write_regs(fe, R_EP3, 7);
+	__tda18271_write_regs(fe, R_EP3, 7, false);
 	msleep(5); /* pll locking */
 
 	/* launch optimization algorithm */
-	tda18271_write_regs(fe, R_EP2, 1);
+	__tda18271_write_regs(fe, R_EP2, 1, false);
 	msleep(30); /* image high optimization completion */
 
 	/* return to normal mode */
 	regs[R_EP4] = 0x64;
-	tda18271_write_regs(fe, R_EP4, 1);
+	__tda18271_write_regs(fe, R_EP4, 1, false);
 
 	/* synchronize */
-	tda18271_write_regs(fe, R_EP1, 1);
+	__tda18271_write_regs(fe, R_EP1, 1, false);
+
+	i2c_unlock_adapter(priv->i2c_props.adap);
+	tda18271_i2c_gate_ctrl(fe, 0);
 
 	return 0;
 }
diff --git a/drivers/media/usb/dvb-usb-v2/af9015.c b/drivers/media/usb/dvb-usb-v2/af9015.c
index 824f1911ee2..3d7526e28d4 100644
--- a/drivers/media/usb/dvb-usb-v2/af9015.c
+++ b/drivers/media/usb/dvb-usb-v2/af9015.c
@@ -500,7 +500,7 @@ static int af9015_read_config(struct dvb_usb_device *d)
 		case 3:
 			state->af9013_config[i].clock = 25000000;
 			break;
-		};
+		}
 		dev_dbg(&d->udev->dev, "%s: [%d] xtal=%d set clock=%d\n",
 				__func__, i, val,
 				state->af9013_config[i].clock);
@@ -568,7 +568,7 @@ static int af9015_read_config(struct dvb_usb_device *d)
 					"supported, please report!\n",
 					KBUILD_MODNAME, val);
 			return -ENODEV;
-		};
+		}
 
 		state->af9013_config[i].tuner = val;
 		dev_dbg(&d->udev->dev, "%s: [%d] tuner id=%d\n",
diff --git a/drivers/media/usb/dvb-usb-v2/af9035.c b/drivers/media/usb/dvb-usb-v2/af9035.c
index aabd3fc03ea..ea27eaff4e3 100644
--- a/drivers/media/usb/dvb-usb-v2/af9035.c
+++ b/drivers/media/usb/dvb-usb-v2/af9035.c
@@ -520,7 +520,7 @@ static int af9035_read_config(struct dvb_usb_device *d)
 			dev_warn(&d->udev->dev, "%s: tuner id=%02x not " \
 					"supported, please report!",
 					KBUILD_MODNAME, tmp);
-		};
+		}
 
 		/* tuner IF frequency */
 		ret = af9035_rd_reg(d, EEPROM_1_IFFREQ_L + eeprom_shift, &tmp);
diff --git a/drivers/media/usb/dvb-usb/a800.c b/drivers/media/usb/dvb-usb/a800.c
index 8d7fef84afd..83684ed023c 100644
--- a/drivers/media/usb/dvb-usb/a800.c
+++ b/drivers/media/usb/dvb-usb/a800.c
@@ -93,7 +93,7 @@ static int a800_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
 	/* call the universal NEC remote processor, to find out the key's state and event */
 	dvb_usb_nec_rc_key_to_event(d,key,event,state);
 	if (key[0] != 0)
-		deb_rc("key: %x %x %x %x %x\n",key[0],key[1],key[2],key[3],key[4]);
+		deb_rc("key: %*ph\n", 5, key);
 	ret = 0;
 out:
 	kfree(key);
diff --git a/drivers/media/usb/dvb-usb/cinergyT2-core.c b/drivers/media/usb/dvb-usb/cinergyT2-core.c
index 0a98548ecd1..9fd1527494e 100644
--- a/drivers/media/usb/dvb-usb/cinergyT2-core.c
+++ b/drivers/media/usb/dvb-usb/cinergyT2-core.c
@@ -172,8 +172,7 @@ static int cinergyt2_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
 		if (*event != d->last_event)
 			st->rc_counter = 0;
 
-		deb_rc("key: %x %x %x %x %x\n",
-		       key[0], key[1], key[2], key[3], key[4]);
+		deb_rc("key: %*ph\n", 5, key);
 	}
 	return 0;
 }
diff --git a/drivers/media/usb/dvb-usb/dibusb-common.c b/drivers/media/usb/dvb-usb/dibusb-common.c
index a76bbb29ca3..af0d4321845 100644
--- a/drivers/media/usb/dvb-usb/dibusb-common.c
+++ b/drivers/media/usb/dvb-usb/dibusb-common.c
@@ -473,7 +473,7 @@ int dibusb_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
 	dvb_usb_generic_rw(d,&cmd,1,key,5,0);
 	dvb_usb_nec_rc_key_to_event(d,key,event,state);
 	if (key[0] != 0)
-		deb_info("key: %x %x %x %x %x\n",key[0],key[1],key[2],key[3],key[4]);
+		deb_info("key: %*ph\n", 5, key);
 	return 0;
 }
 EXPORT_SYMBOL(dibusb_rc_query);
diff --git a/drivers/media/usb/dvb-usb/digitv.c b/drivers/media/usb/dvb-usb/digitv.c
index ff34419a4c8..772bde3c502 100644
--- a/drivers/media/usb/dvb-usb/digitv.c
+++ b/drivers/media/usb/dvb-usb/digitv.c
@@ -253,7 +253,7 @@ static int digitv_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
 	}
 
 	if (key[0] != 0)
-		deb_rc("key: %x %x %x %x %x\n",key[0],key[1],key[2],key[3],key[4]);
+		deb_rc("key: %*ph\n", 5, key);
 	return 0;
 }
 
diff --git a/drivers/media/usb/dvb-usb/dtt200u.c b/drivers/media/usb/dvb-usb/dtt200u.c
index 66f205c112b..c357fb3b0a8 100644
--- a/drivers/media/usb/dvb-usb/dtt200u.c
+++ b/drivers/media/usb/dvb-usb/dtt200u.c
@@ -84,7 +84,7 @@ static int dtt200u_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
 	dvb_usb_generic_rw(d,&cmd,1,key,5,0);
 	dvb_usb_nec_rc_key_to_event(d,key,event,state);
 	if (key[0] != 0)
-		deb_info("key: %x %x %x %x %x\n",key[0],key[1],key[2],key[3],key[4]);
+		deb_info("key: %*ph\n", 5, key);
 	return 0;
 }
 
diff --git a/drivers/media/usb/dvb-usb/m920x.c b/drivers/media/usb/dvb-usb/m920x.c
index 288af29a8bb..661bb75be95 100644
--- a/drivers/media/usb/dvb-usb/m920x.c
+++ b/drivers/media/usb/dvb-usb/m920x.c
@@ -358,7 +358,7 @@ static int m920x_firmware_download(struct usb_device *udev, const struct firmwar
 
 	if ((ret = m920x_read(udev, M9206_FILTER, 0x0, 0x8000, read, 4)) != 0)
 		goto done;
-	deb("%x %x %x %x\n", read[0], read[1], read[2], read[3]);
+	deb("%*ph\n", 4, read);
 
 	if ((ret = m920x_read(udev, M9206_FW, 0x0, 0x0, read, 1)) != 0)
 		goto done;
diff --git a/drivers/media/usb/dvb-usb/technisat-usb2.c b/drivers/media/usb/dvb-usb/technisat-usb2.c
index acefaa89cc5..7a8c8c18590 100644
--- a/drivers/media/usb/dvb-usb/technisat-usb2.c
+++ b/drivers/media/usb/dvb-usb/technisat-usb2.c
@@ -677,6 +677,7 @@ static struct usb_device_id technisat_usb2_id_table[] = {
 	{ USB_DEVICE(USB_VID_TECHNISAT, USB_PID_TECHNISAT_USB2_DVB_S2) },
 	{ 0 }		/* Terminating entry */
 };
+MODULE_DEVICE_TABLE(usb, technisat_usb2_id_table);
 
 /* device description */
 static struct dvb_usb_device_properties technisat_usb2_devices = {
diff --git a/drivers/media/usb/em28xx/em28xx-cards.c b/drivers/media/usb/em28xx/em28xx-cards.c
index f7297ae76b4..16a84f9f46d 100644
--- a/drivers/media/usb/em28xx/em28xx-cards.c
+++ b/drivers/media/usb/em28xx/em28xx-cards.c
@@ -2203,7 +2203,7 @@ EXPORT_SYMBOL_GPL(em28xx_tuner_callback);
 
 static inline void em28xx_set_model(struct em28xx *dev)
 {
-	memcpy(&dev->board, &em28xx_boards[dev->model], sizeof(dev->board));
+	dev->board = em28xx_boards[dev->model];
 
 	/* Those are the default values for the majority of boards
 	   Use those values if not specified otherwise at boards entry
diff --git a/drivers/media/usb/em28xx/em28xx-dvb.c b/drivers/media/usb/em28xx/em28xx-dvb.c
index 913e5227897..13ae821949e 100644
--- a/drivers/media/usb/em28xx/em28xx-dvb.c
+++ b/drivers/media/usb/em28xx/em28xx-dvb.c
@@ -574,18 +574,19 @@ static void pctv_520e_init(struct em28xx *dev)
 		i2c_master_send(&dev->i2c_client, regs[i].r, regs[i].len);
 };
 
-static int em28xx_pctv_290e_set_lna(struct dvb_frontend *fe, int val)
+static int em28xx_pctv_290e_set_lna(struct dvb_frontend *fe)
 {
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	struct em28xx *dev = fe->dvb->priv;
 #ifdef CONFIG_GPIOLIB
 	struct em28xx_dvb *dvb = dev->dvb;
 	int ret;
 	unsigned long flags;
 
-	if (val)
-		flags = GPIOF_OUT_INIT_LOW;
+	if (c->lna == 1)
+		flags = GPIOF_OUT_INIT_HIGH; /* enable LNA */
 	else
-		flags = GPIOF_OUT_INIT_HIGH;
+		flags = GPIOF_OUT_INIT_LOW; /* disable LNA */
 
 	ret = gpio_request_one(dvb->lna_gpio, flags, NULL);
 	if (ret)
@@ -595,8 +596,8 @@ static int em28xx_pctv_290e_set_lna(struct dvb_frontend *fe, int val)
 
 	return ret;
 #else
-	dev_warn(&dev->udev->dev, "%s: LNA control is disabled\n",
-			KBUILD_MODNAME);
+	dev_warn(&dev->udev->dev, "%s: LNA control is disabled (lna=%u)\n",
+			KBUILD_MODNAME, c->lna);
 	return 0;
 #endif
 }
diff --git a/drivers/media/usb/sn9c102/sn9c102_core.c b/drivers/media/usb/sn9c102/sn9c102_core.c
index 19ea780b16f..5bfc8e2f018 100644
--- a/drivers/media/usb/sn9c102/sn9c102_core.c
+++ b/drivers/media/usb/sn9c102/sn9c102_core.c
@@ -2126,8 +2126,7 @@ static int sn9c102_mmap(struct file* filp, struct vm_area_struct *vma)
 		return -EINVAL;
 	}
 
-	vma->vm_flags |= VM_IO;
-	vma->vm_flags |= VM_RESERVED;
+	vma->vm_flags |= VM_IO | VM_DONTEXPAND | VM_DONTDUMP;
 
 	pos = cam->frame[i].bufmem;
 	while (size > 0) { /* size is page-aligned */
diff --git a/drivers/media/usb/stk1160/stk1160-core.c b/drivers/media/usb/stk1160/stk1160-core.c
index b6274084606..34a26e0cfe7 100644
--- a/drivers/media/usb/stk1160/stk1160-core.c
+++ b/drivers/media/usb/stk1160/stk1160-core.c
@@ -100,12 +100,21 @@ int stk1160_write_reg(struct stk1160 *dev, u16 reg, u16 value)
 
 void stk1160_select_input(struct stk1160 *dev)
 {
+	int route;
 	static const u8 gctrl[] = {
-		0x98, 0x90, 0x88, 0x80
+		0x98, 0x90, 0x88, 0x80, 0x98
 	};
 
-	if (dev->ctl_input < ARRAY_SIZE(gctrl))
+	if (dev->ctl_input == STK1160_SVIDEO_INPUT)
+		route = SAA7115_SVIDEO3;
+	else
+		route = SAA7115_COMPOSITE0;
+
+	if (dev->ctl_input < ARRAY_SIZE(gctrl)) {
+		v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_routing,
+				route, 0, 0);
 		stk1160_write_reg(dev, STK1160_GCTRL, gctrl[dev->ctl_input]);
+	}
 }
 
 /* TODO: We should break this into pieces */
@@ -351,8 +360,6 @@ static int stk1160_probe(struct usb_interface *interface,
 
 	/* i2c reset saa711x */
 	v4l2_device_call_all(&dev->v4l2_dev, 0, core, reset, 0);
-	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_routing,
-				0, 0, 0);
 	v4l2_device_call_all(&dev->v4l2_dev, 0, video, s_stream, 0);
 
 	/* reset stk1160 to default values */
diff --git a/drivers/media/usb/stk1160/stk1160-v4l.c b/drivers/media/usb/stk1160/stk1160-v4l.c
index fe6e857969c..6694f9e2ca5 100644
--- a/drivers/media/usb/stk1160/stk1160-v4l.c
+++ b/drivers/media/usb/stk1160/stk1160-v4l.c
@@ -419,7 +419,12 @@ static int vidioc_enum_input(struct file *file, void *priv,
 	if (i->index > STK1160_MAX_INPUT)
 		return -EINVAL;
 
-	sprintf(i->name, "Composite%d", i->index);
+	/* S-Video special handling */
+	if (i->index == STK1160_SVIDEO_INPUT)
+		sprintf(i->name, "S-Video");
+	else
+		sprintf(i->name, "Composite%d", i->index);
+
 	i->type = V4L2_INPUT_TYPE_CAMERA;
 	i->std = dev->vdev.tvnorms;
 	return 0;
diff --git a/drivers/media/usb/stk1160/stk1160.h b/drivers/media/usb/stk1160/stk1160.h
index 3feba0033f9..68c8707d36a 100644
--- a/drivers/media/usb/stk1160/stk1160.h
+++ b/drivers/media/usb/stk1160/stk1160.h
@@ -46,7 +46,8 @@
 
 #define STK1160_MIN_PKT_SIZE 3072
 
-#define STK1160_MAX_INPUT 3
+#define STK1160_MAX_INPUT 4
+#define STK1160_SVIDEO_INPUT 4
 
 #define STK1160_I2C_TIMEOUT 100
 
diff --git a/drivers/media/usb/usbvision/usbvision-video.c b/drivers/media/usb/usbvision/usbvision-video.c
index f67018ed379..5c36a57e659 100644
--- a/drivers/media/usb/usbvision/usbvision-video.c
+++ b/drivers/media/usb/usbvision/usbvision-video.c
@@ -1108,8 +1108,7 @@ static int usbvision_mmap(struct file *file, struct vm_area_struct *vma)
 	}
 
 	/* VM_IO is eventually going to replace PageReserved altogether */
-	vma->vm_flags |= VM_IO;
-	vma->vm_flags |= VM_RESERVED;	/* avoid to swap out this VMA */
+	vma->vm_flags |= VM_IO | VM_DONTEXPAND | VM_DONTDUMP;
 
 	pos = usbvision->frame[i].data;
 	while (size > 0) {
diff --git a/drivers/media/usb/uvc/uvc_queue.c b/drivers/media/usb/uvc/uvc_queue.c
index 5577381b5bf..18a91fae6bc 100644
--- a/drivers/media/usb/uvc/uvc_queue.c
+++ b/drivers/media/usb/uvc/uvc_queue.c
@@ -122,21 +122,27 @@ static struct vb2_ops uvc_queue_qops = {
 	.buf_finish = uvc_buffer_finish,
 };
 
-void uvc_queue_init(struct uvc_video_queue *queue, enum v4l2_buf_type type,
+int uvc_queue_init(struct uvc_video_queue *queue, enum v4l2_buf_type type,
 		    int drop_corrupted)
 {
+	int ret;
+
 	queue->queue.type = type;
 	queue->queue.io_modes = VB2_MMAP | VB2_USERPTR;
 	queue->queue.drv_priv = queue;
 	queue->queue.buf_struct_size = sizeof(struct uvc_buffer);
 	queue->queue.ops = &uvc_queue_qops;
 	queue->queue.mem_ops = &vb2_vmalloc_memops;
-	vb2_queue_init(&queue->queue);
+	ret = vb2_queue_init(&queue->queue);
+	if (ret)
+		return ret;
 
 	mutex_init(&queue->mutex);
 	spin_lock_init(&queue->irqlock);
 	INIT_LIST_HEAD(&queue->irqqueue);
 	queue->flags = drop_corrupted ? UVC_QUEUE_DROP_CORRUPTED : 0;
+
+	return 0;
 }
 
 /* -----------------------------------------------------------------------------
diff --git a/drivers/media/usb/uvc/uvc_video.c b/drivers/media/usb/uvc/uvc_video.c
index 1c15b4227bd..57c3076a462 100644
--- a/drivers/media/usb/uvc/uvc_video.c
+++ b/drivers/media/usb/uvc/uvc_video.c
@@ -1755,7 +1755,9 @@ int uvc_video_init(struct uvc_streaming *stream)
 	atomic_set(&stream->active, 0);
 
 	/* Initialize the video buffers queue. */
-	uvc_queue_init(&stream->queue, stream->type, !uvc_no_drop_param);
+	ret = uvc_queue_init(&stream->queue, stream->type, !uvc_no_drop_param);
+	if (ret)
+		return ret;
 
 	/* Alternate setting 0 should be the default, yet the XBox Live Vision
 	 * Cam (and possibly other devices) crash or otherwise misbehave if
diff --git a/drivers/media/usb/uvc/uvcvideo.h b/drivers/media/usb/uvc/uvcvideo.h
index 3764040475b..af216ec45e3 100644
--- a/drivers/media/usb/uvc/uvcvideo.h
+++ b/drivers/media/usb/uvc/uvcvideo.h
@@ -600,7 +600,7 @@ extern struct uvc_driver uvc_driver;
 extern struct uvc_entity *uvc_entity_by_id(struct uvc_device *dev, int id);
 
 /* Video buffers queue management. */
-extern void uvc_queue_init(struct uvc_video_queue *queue,
+extern int uvc_queue_init(struct uvc_video_queue *queue,
 		enum v4l2_buf_type type, int drop_corrupted);
 extern int uvc_alloc_buffers(struct uvc_video_queue *queue,
 		struct v4l2_requestbuffers *rb);
diff --git a/drivers/media/v4l2-core/v4l2-ctrls.c b/drivers/media/v4l2-core/v4l2-ctrls.c
index 631cdc0e0bd..f6ee201d934 100644
--- a/drivers/media/v4l2-core/v4l2-ctrls.c
+++ b/drivers/media/v4l2-core/v4l2-ctrls.c
@@ -384,6 +384,25 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
 		"Extended SAR",
 		NULL,
 	};
+	static const char * const h264_fp_arrangement_type[] = {
+		"Checkerboard",
+		"Column",
+		"Row",
+		"Side by Side",
+		"Top Bottom",
+		"Temporal",
+		NULL,
+	};
+	static const char * const h264_fmo_map_type[] = {
+		"Interleaved Slices",
+		"Scattered Slices",
+		"Foreground with Leftover",
+		"Box Out",
+		"Raster Scan",
+		"Wipe Scan",
+		"Explicit",
+		NULL,
+	};
 	static const char * const mpeg_mpeg4_level[] = {
 		"0",
 		"0b",
@@ -508,6 +527,10 @@ const char * const *v4l2_ctrl_get_menu(u32 id)
 		return h264_profile;
 	case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_IDC:
 		return vui_sar_idc;
+	case V4L2_CID_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE:
+		return h264_fp_arrangement_type;
+	case V4L2_CID_MPEG_VIDEO_H264_FMO_MAP_TYPE:
+		return h264_fmo_map_type;
 	case V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL:
 		return mpeg_mpeg4_level;
 	case V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE:
@@ -643,6 +666,22 @@ const char *v4l2_ctrl_get_name(u32 id)
 	case V4L2_CID_MPEG_VIDEO_H264_VUI_EXT_SAR_WIDTH:	return "Horizontal Size of SAR";
 	case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_ENABLE:		return "Aspect Ratio VUI Enable";
 	case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_IDC:		return "VUI Aspect Ratio IDC";
+	case V4L2_CID_MPEG_VIDEO_H264_SEI_FRAME_PACKING:	return "H264 Enable Frame Packing SEI";
+	case V4L2_CID_MPEG_VIDEO_H264_SEI_FP_CURRENT_FRAME_0:	return "H264 Set Curr. Frame as Frame0";
+	case V4L2_CID_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE:	return "H264 FP Arrangement Type";
+	case V4L2_CID_MPEG_VIDEO_H264_FMO:			return "H264 Flexible MB Ordering";
+	case V4L2_CID_MPEG_VIDEO_H264_FMO_MAP_TYPE:		return "H264 Map Type for FMO";
+	case V4L2_CID_MPEG_VIDEO_H264_FMO_SLICE_GROUP:		return "H264 FMO Number of Slice Groups";
+	case V4L2_CID_MPEG_VIDEO_H264_FMO_CHANGE_DIRECTION:	return "H264 FMO Direction of Change";
+	case V4L2_CID_MPEG_VIDEO_H264_FMO_CHANGE_RATE:		return "H264 FMO Size of 1st Slice Grp";
+	case V4L2_CID_MPEG_VIDEO_H264_FMO_RUN_LENGTH:		return "H264 FMO No. of Consecutive MBs";
+	case V4L2_CID_MPEG_VIDEO_H264_ASO:			return "H264 Arbitrary Slice Ordering";
+	case V4L2_CID_MPEG_VIDEO_H264_ASO_SLICE_ORDER:		return "H264 ASO Slice Order";
+	case V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING:	return "Enable H264 Hierarchical Coding";
+	case V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING_TYPE:	return "H264 Hierarchical Coding Type";
+	case V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING_LAYER:return "H264 Number of HC Layers";
+	case V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING_LAYER_QP:
+								return "H264 Set QP Value for HC Layers";
 	case V4L2_CID_MPEG_VIDEO_MPEG4_I_FRAME_QP:		return "MPEG4 I-Frame QP Value";
 	case V4L2_CID_MPEG_VIDEO_MPEG4_P_FRAME_QP:		return "MPEG4 P-Frame QP Value";
 	case V4L2_CID_MPEG_VIDEO_MPEG4_B_FRAME_QP:		return "MPEG4 B-Frame QP Value";
@@ -657,6 +696,7 @@ const char *v4l2_ctrl_get_name(u32 id)
 	case V4L2_CID_MPEG_VIDEO_VBV_SIZE:			return "VBV Buffer Size";
 	case V4L2_CID_MPEG_VIDEO_DEC_PTS:			return "Video Decoder PTS";
 	case V4L2_CID_MPEG_VIDEO_DEC_FRAME:			return "Video Decoder Frame Count";
+	case V4L2_CID_MPEG_VIDEO_VBV_DELAY:			return "Initial Delay for VBV Control";
 
 	/* CAMERA controls */
 	/* Keep the order of the 'case's the same as in videodev2.h! */
@@ -749,6 +789,7 @@ const char *v4l2_ctrl_get_name(u32 id)
 	case V4L2_CID_IMAGE_PROC_CLASS:		return "Image Processing Controls";
 	case V4L2_CID_LINK_FREQ:		return "Link Frequency";
 	case V4L2_CID_PIXEL_RATE:		return "Pixel Rate";
+	case V4L2_CID_TEST_PATTERN:		return "Test Pattern";
 
 	/* DV controls */
 	case V4L2_CID_DV_CLASS:			return "Digital Video Controls";
@@ -853,6 +894,8 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 	case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE:
 	case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
 	case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_IDC:
+	case V4L2_CID_MPEG_VIDEO_H264_SEI_FP_ARRANGEMENT_TYPE:
+	case V4L2_CID_MPEG_VIDEO_H264_FMO_MAP_TYPE:
 	case V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL:
 	case V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE:
 	case V4L2_CID_JPEG_CHROMA_SUBSAMPLING:
@@ -862,6 +905,7 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 	case V4L2_CID_DV_TX_MODE:
 	case V4L2_CID_DV_TX_RGB_RANGE:
 	case V4L2_CID_DV_RX_RGB_RANGE:
+	case V4L2_CID_TEST_PATTERN:
 		*type = V4L2_CTRL_TYPE_MENU;
 		break;
 	case V4L2_CID_LINK_FREQ:
@@ -1648,6 +1692,36 @@ struct v4l2_ctrl *v4l2_ctrl_new_std_menu(struct v4l2_ctrl_handler *hdl,
 }
 EXPORT_SYMBOL(v4l2_ctrl_new_std_menu);
 
+/* Helper function for standard menu controls with driver defined menu */
+struct v4l2_ctrl *v4l2_ctrl_new_std_menu_items(struct v4l2_ctrl_handler *hdl,
+			const struct v4l2_ctrl_ops *ops, u32 id, s32 max,
+			s32 mask, s32 def, const char * const *qmenu)
+{
+	enum v4l2_ctrl_type type;
+	const char *name;
+	u32 flags;
+	s32 step;
+	s32 min;
+
+	/* v4l2_ctrl_new_std_menu_items() should only be called for
+	 * standard controls without a standard menu.
+	 */
+	if (v4l2_ctrl_get_menu(id)) {
+		handler_set_err(hdl, -EINVAL);
+		return NULL;
+	}
+
+	v4l2_ctrl_fill(id, &name, &type, &min, &max, &step, &def, &flags);
+	if (type != V4L2_CTRL_TYPE_MENU || qmenu == NULL) {
+		handler_set_err(hdl, -EINVAL);
+		return NULL;
+	}
+	return v4l2_ctrl_new(hdl, ops, id, name, type, 0, max, mask, def,
+			     flags, qmenu, NULL, NULL);
+
+}
+EXPORT_SYMBOL(v4l2_ctrl_new_std_menu_items);
+
 /* Helper function for standard integer menu controls */
 struct v4l2_ctrl *v4l2_ctrl_new_int_menu(struct v4l2_ctrl_handler *hdl,
 			const struct v4l2_ctrl_ops *ops,
diff --git a/drivers/media/v4l2-core/v4l2-ioctl.c b/drivers/media/v4l2-core/v4l2-ioctl.c
index 9d3e46c446a..8f388ff31eb 100644
--- a/drivers/media/v4l2-core/v4l2-ioctl.c
+++ b/drivers/media/v4l2-core/v4l2-ioctl.c
@@ -157,8 +157,7 @@ static const char *v4l2_memory_names[] = {
 	[V4L2_MEMORY_OVERLAY] = "overlay",
 };
 
-#define prt_names(a, arr) ((((a) >= 0) && ((a) < ARRAY_SIZE(arr))) ? \
-			   arr[a] : "unknown")
+#define prt_names(a, arr) (((unsigned)(a)) < ARRAY_SIZE(arr) ? arr[a] : "unknown")
 
 /* ------------------------------------------------------------------ */
 /* debug help functions                                               */
@@ -2188,6 +2187,7 @@ static int check_array_args(unsigned int cmd, void *parg, size_t *array_size,
 	int ret = 0;
 
 	switch (cmd) {
+	case VIDIOC_PREPARE_BUF:
 	case VIDIOC_QUERYBUF:
 	case VIDIOC_QBUF:
 	case VIDIOC_DQBUF: {
@@ -2211,6 +2211,10 @@ static int check_array_args(unsigned int cmd, void *parg, size_t *array_size,
 		struct v4l2_subdev_edid *edid = parg;
 
 		if (edid->blocks) {
+			if (edid->blocks > 256) {
+				ret = -EINVAL;
+				break;
+			}
 			*user_ptr = (void __user *)edid->edid;
 			*kernel_ptr = (void *)&edid->edid;
 			*array_size = edid->blocks * 128;
diff --git a/drivers/media/v4l2-core/videobuf-dma-sg.c b/drivers/media/v4l2-core/videobuf-dma-sg.c
index f300deafd26..828e7c10bd7 100644
--- a/drivers/media/v4l2-core/videobuf-dma-sg.c
+++ b/drivers/media/v4l2-core/videobuf-dma-sg.c
@@ -582,7 +582,7 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 	map->count    = 1;
 	map->q        = q;
 	vma->vm_ops   = &videobuf_vm_ops;
-	vma->vm_flags |= VM_DONTEXPAND | VM_RESERVED;
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
 	vma->vm_flags &= ~VM_IO; /* using shared anonymous pages */
 	vma->vm_private_data = map;
 	dprintk(1, "mmap %p: q=%p %08lx-%08lx pgoff %08lx bufs %d-%d\n",
diff --git a/drivers/media/v4l2-core/videobuf-vmalloc.c b/drivers/media/v4l2-core/videobuf-vmalloc.c
index df142580e44..2ff7fcc77b1 100644
--- a/drivers/media/v4l2-core/videobuf-vmalloc.c
+++ b/drivers/media/v4l2-core/videobuf-vmalloc.c
@@ -270,7 +270,7 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 	}
 
 	vma->vm_ops          = &videobuf_vm_ops;
-	vma->vm_flags       |= VM_DONTEXPAND | VM_RESERVED;
+	vma->vm_flags       |= VM_DONTEXPAND | VM_DONTDUMP;
 	vma->vm_private_data = map;
 
 	dprintk(1, "mmap %p: q=%p %08lx-%08lx (%lx) pgoff %08lx buf %d\n",
diff --git a/drivers/media/v4l2-core/videobuf2-core.c b/drivers/media/v4l2-core/videobuf2-core.c
index e6a26b433e8..432df119af2 100644
--- a/drivers/media/v4l2-core/videobuf2-core.c
+++ b/drivers/media/v4l2-core/videobuf2-core.c
@@ -276,6 +276,9 @@ static void __vb2_queue_free(struct vb2_queue *q, unsigned int buffers)
  */
 static int __verify_planes_array(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 {
+	if (!V4L2_TYPE_IS_MULTIPLANAR(b->type))
+		return 0;
+
 	/* Is memory for copying plane information present? */
 	if (NULL == b->m.planes) {
 		dprintk(1, "Multi-planar buffer passed but "
@@ -331,10 +334,9 @@ static bool __buffers_in_use(struct vb2_queue *q)
  * __fill_v4l2_buffer() - fill in a struct v4l2_buffer with information to be
  * returned to userspace
  */
-static int __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
+static void __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
 {
 	struct vb2_queue *q = vb->vb2_queue;
-	int ret;
 
 	/* Copy back data such as timestamp, flags, etc. */
 	memcpy(b, &vb->v4l2_buf, offsetof(struct v4l2_buffer, m));
@@ -342,14 +344,11 @@ static int __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
 	b->reserved = vb->v4l2_buf.reserved;
 
 	if (V4L2_TYPE_IS_MULTIPLANAR(q->type)) {
-		ret = __verify_planes_array(vb, b);
-		if (ret)
-			return ret;
-
 		/*
 		 * Fill in plane-related data if userspace provided an array
-		 * for it. The memory and size is verified above.
+		 * for it. The caller has already verified memory and size.
 		 */
+		b->length = vb->num_planes;
 		memcpy(b->m.planes, vb->v4l2_planes,
 			b->length * sizeof(struct v4l2_plane));
 	} else {
@@ -391,8 +390,6 @@ static int __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
 
 	if (__buffer_in_use(q, vb))
 		b->flags |= V4L2_BUF_FLAG_MAPPED;
-
-	return 0;
 }
 
 /**
@@ -411,6 +408,7 @@ static int __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
 int vb2_querybuf(struct vb2_queue *q, struct v4l2_buffer *b)
 {
 	struct vb2_buffer *vb;
+	int ret;
 
 	if (b->type != q->type) {
 		dprintk(1, "querybuf: wrong buffer type\n");
@@ -422,8 +420,10 @@ int vb2_querybuf(struct vb2_queue *q, struct v4l2_buffer *b)
 		return -EINVAL;
 	}
 	vb = q->bufs[b->index];
-
-	return __fill_v4l2_buffer(vb, b);
+	ret = __verify_planes_array(vb, b);
+	if (!ret)
+		__fill_v4l2_buffer(vb, b);
+	return ret;
 }
 EXPORT_SYMBOL(vb2_querybuf);
 
@@ -813,24 +813,16 @@ void vb2_buffer_done(struct vb2_buffer *vb, enum vb2_buffer_state state)
 EXPORT_SYMBOL_GPL(vb2_buffer_done);
 
 /**
- * __fill_vb2_buffer() - fill a vb2_buffer with information provided in
- * a v4l2_buffer by the userspace
+ * __fill_vb2_buffer() - fill a vb2_buffer with information provided in a
+ * v4l2_buffer by the userspace. The caller has already verified that struct
+ * v4l2_buffer has a valid number of planes.
  */
-static int __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b,
+static void __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b,
 				struct v4l2_plane *v4l2_planes)
 {
 	unsigned int plane;
-	int ret;
 
 	if (V4L2_TYPE_IS_MULTIPLANAR(b->type)) {
-		/*
-		 * Verify that the userspace gave us a valid array for
-		 * plane information.
-		 */
-		ret = __verify_planes_array(vb, b);
-		if (ret)
-			return ret;
-
 		/* Fill in driver-provided information for OUTPUT types */
 		if (V4L2_TYPE_IS_OUTPUT(b->type)) {
 			/*
@@ -872,8 +864,6 @@ static int __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b,
 	vb->v4l2_buf.field = b->field;
 	vb->v4l2_buf.timestamp = b->timestamp;
 	vb->v4l2_buf.flags = b->flags & ~V4L2_BUFFER_STATE_FLAGS;
-
-	return 0;
 }
 
 /**
@@ -888,10 +878,8 @@ static int __qbuf_userptr(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 	int ret;
 	int write = !V4L2_TYPE_IS_OUTPUT(q->type);
 
-	/* Verify and copy relevant information provided by the userspace */
-	ret = __fill_vb2_buffer(vb, b, planes);
-	if (ret)
-		return ret;
+	/* Copy relevant information provided by the userspace */
+	__fill_vb2_buffer(vb, b, planes);
 
 	for (plane = 0; plane < vb->num_planes; ++plane) {
 		/* Skip the plane if already verified */
@@ -966,7 +954,8 @@ err:
  */
 static int __qbuf_mmap(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 {
-	return __fill_vb2_buffer(vb, b, vb->v4l2_planes);
+	__fill_vb2_buffer(vb, b, vb->v4l2_planes);
+	return 0;
 }
 
 /**
@@ -1059,7 +1048,9 @@ int vb2_prepare_buf(struct vb2_queue *q, struct v4l2_buffer *b)
 		dprintk(1, "%s(): invalid buffer state %d\n", __func__, vb->state);
 		return -EINVAL;
 	}
-
+	ret = __verify_planes_array(vb, b);
+	if (ret < 0)
+		return ret;
 	ret = __buf_prepare(vb, b);
 	if (ret < 0)
 		return ret;
@@ -1147,6 +1138,9 @@ int vb2_qbuf(struct vb2_queue *q, struct v4l2_buffer *b)
 		ret = -EINVAL;
 		goto unlock;
 	}
+	ret = __verify_planes_array(vb, b);
+	if (ret)
+		goto unlock;
 
 	switch (vb->state) {
 	case VB2_BUF_STATE_DEQUEUED:
@@ -1243,8 +1237,10 @@ static int __vb2_wait_for_done_vb(struct vb2_queue *q, int nonblocking)
 		 * the locks or return an error if one occurred.
 		 */
 		call_qop(q, wait_finish, q);
-		if (ret)
+		if (ret) {
+			dprintk(1, "Sleep was interrupted\n");
 			return ret;
+		}
 	}
 	return 0;
 }
@@ -1255,7 +1251,7 @@ static int __vb2_wait_for_done_vb(struct vb2_queue *q, int nonblocking)
  * Will sleep if required for nonblocking == false.
  */
 static int __vb2_get_done_vb(struct vb2_queue *q, struct vb2_buffer **vb,
-				int nonblocking)
+				struct v4l2_buffer *b, int nonblocking)
 {
 	unsigned long flags;
 	int ret;
@@ -1273,10 +1269,16 @@ static int __vb2_get_done_vb(struct vb2_queue *q, struct vb2_buffer **vb,
 	 */
 	spin_lock_irqsave(&q->done_lock, flags);
 	*vb = list_first_entry(&q->done_list, struct vb2_buffer, done_entry);
-	list_del(&(*vb)->done_entry);
+	/*
+	 * Only remove the buffer from done_list if v4l2_buffer can handle all
+	 * the planes.
+	 */
+	ret = __verify_planes_array(*vb, b);
+	if (!ret)
+		list_del(&(*vb)->done_entry);
 	spin_unlock_irqrestore(&q->done_lock, flags);
 
-	return 0;
+	return ret;
 }
 
 /**
@@ -1335,12 +1337,9 @@ int vb2_dqbuf(struct vb2_queue *q, struct v4l2_buffer *b, bool nonblocking)
 		dprintk(1, "dqbuf: invalid buffer type\n");
 		return -EINVAL;
 	}
-
-	ret = __vb2_get_done_vb(q, &vb, nonblocking);
-	if (ret < 0) {
-		dprintk(1, "dqbuf: error getting next done buffer\n");
+	ret = __vb2_get_done_vb(q, &vb, b, nonblocking);
+	if (ret < 0)
 		return ret;
-	}
 
 	ret = call_qop(q, buf_finish, vb);
 	if (ret) {
diff --git a/drivers/media/v4l2-core/videobuf2-memops.c b/drivers/media/v4l2-core/videobuf2-memops.c
index 504cd4cbe29..051ea3571b2 100644
--- a/drivers/media/v4l2-core/videobuf2-memops.c
+++ b/drivers/media/v4l2-core/videobuf2-memops.c
@@ -163,7 +163,7 @@ int vb2_mmap_pfn_range(struct vm_area_struct *vma, unsigned long paddr,
 		return ret;
 	}
 
-	vma->vm_flags		|= VM_DONTEXPAND | VM_RESERVED;
+	vma->vm_flags		|= VM_DONTEXPAND | VM_DONTDUMP;
 	vma->vm_private_data	= priv;
 	vma->vm_ops		= vm_ops;
 
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index 99c73352c43..b151b7c1bd5 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -60,16 +60,6 @@ config ATMEL_PWM
 	  purposes including software controlled power-efficient backlights
 	  on LCD displays, motor control, and waveform generation.
 
-config AB8500_PWM
-	bool "AB8500 PWM support"
-	depends on AB8500_CORE && ARCH_U8500
-	select HAVE_PWM
-	depends on !PWM
-	help
-	  This driver exports functions to enable/disble/config/free Pulse
-	  Width Modulation in the Analog Baseband Chip AB8500.
-	  It is used by led and backlight driver to control the intensity.
-
 config ATMEL_TCLIB
 	bool "Atmel AT32/AT91 Timer/Counter Library"
 	depends on (AVR32 || ARCH_AT91)
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index b88df7a350b..2129377c0de 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -44,7 +44,6 @@ obj-$(CONFIG_VMWARE_BALLOON)	+= vmw_balloon.o
 obj-$(CONFIG_ARM_CHARLCD)	+= arm-charlcd.o
 obj-$(CONFIG_PCH_PHUB)		+= pch_phub.o
 obj-y				+= ti-st/
-obj-$(CONFIG_AB8500_PWM)	+= ab8500-pwm.o
 obj-y				+= lis3lv02d/
 obj-y				+= carma/
 obj-$(CONFIG_USB_SWITCH_FSA9480) += fsa9480.o
diff --git a/drivers/misc/carma/carma-fpga.c b/drivers/misc/carma/carma-fpga.c
index 0c43297ed9a..8835eabb3b8 100644
--- a/drivers/misc/carma/carma-fpga.c
+++ b/drivers/misc/carma/carma-fpga.c
@@ -1243,8 +1243,6 @@ static int data_mmap(struct file *filp, struct vm_area_struct *vma)
 		return -EINVAL;
 	}
 
-	/* IO memory (stop cacheing) */
-	vma->vm_flags |= VM_IO | VM_RESERVED;
 	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
 
 	return io_remap_pfn_range(vma, vma->vm_start, addr, vsize,
diff --git a/drivers/misc/sgi-gru/grufile.c b/drivers/misc/sgi-gru/grufile.c
index ecafa4ba238..492c8cac69a 100644
--- a/drivers/misc/sgi-gru/grufile.c
+++ b/drivers/misc/sgi-gru/grufile.c
@@ -108,9 +108,8 @@ static int gru_file_mmap(struct file *file, struct vm_area_struct *vma)
 				vma->vm_end & (GRU_GSEG_PAGESIZE - 1))
 		return -EINVAL;
 
-	vma->vm_flags |=
-	    (VM_IO | VM_DONTCOPY | VM_LOCKED | VM_DONTEXPAND | VM_PFNMAP |
-			VM_RESERVED);
+	vma->vm_flags |= VM_IO | VM_PFNMAP | VM_LOCKED |
+			 VM_DONTCOPY | VM_DONTEXPAND | VM_DONTDUMP;
 	vma->vm_page_prot = PAGE_SHARED;
 	vma->vm_ops = &gru_vm_ops;
 
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index 8ac5246e2ab..06c42cfb7c3 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -26,6 +26,7 @@
 #include <linux/suspend.h>
 #include <linux/fault-inject.h>
 #include <linux/random.h>
+#include <linux/slab.h>
 
 #include <linux/mmc/card.h>
 #include <linux/mmc/host.h>
@@ -41,6 +42,12 @@
 #include "sd_ops.h"
 #include "sdio_ops.h"
 
+/*
+ * Background operations can take a long time, depending on the housekeeping
+ * operations the card has to perform.
+ */
+#define MMC_BKOPS_MAX_TIMEOUT	(4 * 60 * 1000) /* max time to wait in ms */
+
 static struct workqueue_struct *workqueue;
 static const unsigned freqs[] = { 400000, 300000, 200000, 100000 };
 
@@ -245,6 +252,70 @@ mmc_start_request(struct mmc_host *host, struct mmc_request *mrq)
 	host->ops->request(host, mrq);
 }
 
+/**
+ *	mmc_start_bkops - start BKOPS for supported cards
+ *	@card: MMC card to start BKOPS
+ *	@form_exception: A flag to indicate if this function was
+ *			 called due to an exception raised by the card
+ *
+ *	Start background operations whenever requested.
+ *	When the urgent BKOPS bit is set in a R1 command response
+ *	then background operations should be started immediately.
+*/
+void mmc_start_bkops(struct mmc_card *card, bool from_exception)
+{
+	int err;
+	int timeout;
+	bool use_busy_signal;
+
+	BUG_ON(!card);
+
+	if (!card->ext_csd.bkops_en || mmc_card_doing_bkops(card))
+		return;
+
+	err = mmc_read_bkops_status(card);
+	if (err) {
+		pr_err("%s: Failed to read bkops status: %d\n",
+		       mmc_hostname(card->host), err);
+		return;
+	}
+
+	if (!card->ext_csd.raw_bkops_status)
+		return;
+
+	if (card->ext_csd.raw_bkops_status < EXT_CSD_BKOPS_LEVEL_2 &&
+	    from_exception)
+		return;
+
+	mmc_claim_host(card->host);
+	if (card->ext_csd.raw_bkops_status >= EXT_CSD_BKOPS_LEVEL_2) {
+		timeout = MMC_BKOPS_MAX_TIMEOUT;
+		use_busy_signal = true;
+	} else {
+		timeout = 0;
+		use_busy_signal = false;
+	}
+
+	err = __mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
+			EXT_CSD_BKOPS_START, 1, timeout, use_busy_signal);
+	if (err) {
+		pr_warn("%s: Error %d starting bkops\n",
+			mmc_hostname(card->host), err);
+		goto out;
+	}
+
+	/*
+	 * For urgent bkops status (LEVEL_2 and more)
+	 * bkops executed synchronously, otherwise
+	 * the operation is in progress
+	 */
+	if (!use_busy_signal)
+		mmc_card_set_doing_bkops(card);
+out:
+	mmc_release_host(card->host);
+}
+EXPORT_SYMBOL(mmc_start_bkops);
+
 static void mmc_wait_done(struct mmc_request *mrq)
 {
 	complete(&mrq->completion);
@@ -354,6 +425,14 @@ struct mmc_async_req *mmc_start_req(struct mmc_host *host,
 	if (host->areq) {
 		mmc_wait_for_req_done(host, host->areq->mrq);
 		err = host->areq->err_check(host->card, host->areq);
+		/*
+		 * Check BKOPS urgency for each R1 response
+		 */
+		if (host->card && mmc_card_mmc(host->card) &&
+		    ((mmc_resp_type(host->areq->mrq->cmd) == MMC_RSP_R1) ||
+		     (mmc_resp_type(host->areq->mrq->cmd) == MMC_RSP_R1B)) &&
+		    (host->areq->mrq->cmd->resp[0] & R1_EXCEPTION_EVENT))
+			mmc_start_bkops(host->card, true);
 	}
 
 	if (!err && areq)
@@ -398,7 +477,7 @@ EXPORT_SYMBOL(mmc_wait_for_req);
  *	@card: the MMC card associated with the HPI transfer
  *
  *	Issued High Priority Interrupt, and check for card status
- *	util out-of prg-state.
+ *	until out-of prg-state.
  */
 int mmc_interrupt_hpi(struct mmc_card *card)
 {
@@ -424,8 +503,9 @@ int mmc_interrupt_hpi(struct mmc_card *card)
 	case R1_STATE_IDLE:
 	case R1_STATE_READY:
 	case R1_STATE_STBY:
+	case R1_STATE_TRAN:
 		/*
-		 * In idle states, HPI is not needed and the caller
+		 * In idle and transfer states, HPI is not needed and the caller
 		 * can issue the next intended command immediately
 		 */
 		goto out;
@@ -489,6 +569,64 @@ int mmc_wait_for_cmd(struct mmc_host *host, struct mmc_command *cmd, int retries
 EXPORT_SYMBOL(mmc_wait_for_cmd);
 
 /**
+ *	mmc_stop_bkops - stop ongoing BKOPS
+ *	@card: MMC card to check BKOPS
+ *
+ *	Send HPI command to stop ongoing background operations to
+ *	allow rapid servicing of foreground operations, e.g. read/
+ *	writes. Wait until the card comes out of the programming state
+ *	to avoid errors in servicing read/write requests.
+ */
+int mmc_stop_bkops(struct mmc_card *card)
+{
+	int err = 0;
+
+	BUG_ON(!card);
+	err = mmc_interrupt_hpi(card);
+
+	/*
+	 * If err is EINVAL, we can't issue an HPI.
+	 * It should complete the BKOPS.
+	 */
+	if (!err || (err == -EINVAL)) {
+		mmc_card_clr_doing_bkops(card);
+		err = 0;
+	}
+
+	return err;
+}
+EXPORT_SYMBOL(mmc_stop_bkops);
+
+int mmc_read_bkops_status(struct mmc_card *card)
+{
+	int err;
+	u8 *ext_csd;
+
+	/*
+	 * In future work, we should consider storing the entire ext_csd.
+	 */
+	ext_csd = kmalloc(512, GFP_KERNEL);
+	if (!ext_csd) {
+		pr_err("%s: could not allocate buffer to receive the ext_csd.\n",
+		       mmc_hostname(card->host));
+		return -ENOMEM;
+	}
+
+	mmc_claim_host(card->host);
+	err = mmc_send_ext_csd(card, ext_csd);
+	mmc_release_host(card->host);
+	if (err)
+		goto out;
+
+	card->ext_csd.raw_bkops_status = ext_csd[EXT_CSD_BKOPS_STATUS];
+	card->ext_csd.raw_exception_status = ext_csd[EXT_CSD_EXP_EVENTS_STATUS];
+out:
+	kfree(ext_csd);
+	return err;
+}
+EXPORT_SYMBOL(mmc_read_bkops_status);
+
+/**
  *	mmc_set_data_timeout - set the timeout for a data command
  *	@data: data phase for command
  *	@card: the MMC card associated with the data transfer
@@ -975,7 +1113,8 @@ int mmc_regulator_set_ocr(struct mmc_host *mmc,
 		int		tmp;
 		int		voltage;
 
-		/* REVISIT mmc_vddrange_to_ocrmask() may have set some
+		/*
+		 * REVISIT mmc_vddrange_to_ocrmask() may have set some
 		 * bits this regulator doesn't quite support ... don't
 		 * be too picky, most cards and regulators are OK with
 		 * a 0.1V range goof (it's a small error percentage).
@@ -989,12 +1128,13 @@ int mmc_regulator_set_ocr(struct mmc_host *mmc,
 			max_uV = min_uV + 100 * 1000;
 		}
 
-		/* avoid needless changes to this voltage; the regulator
-		 * might not allow this operation
+		/*
+		 * If we're using a fixed/static regulator, don't call
+		 * regulator_set_voltage; it would fail.
 		 */
 		voltage = regulator_get_voltage(supply);
 
-		if (mmc->caps2 & MMC_CAP2_BROKEN_VOLTAGE)
+		if (regulator_count_voltages(supply) == 1)
 			min_uV = max_uV = voltage;
 
 		if (voltage < 0)
@@ -1133,48 +1273,6 @@ void mmc_set_driver_type(struct mmc_host *host, unsigned int drv_type)
 	mmc_host_clk_release(host);
 }
 
-static void mmc_poweroff_notify(struct mmc_host *host)
-{
-	struct mmc_card *card;
-	unsigned int timeout;
-	unsigned int notify_type = EXT_CSD_NO_POWER_NOTIFICATION;
-	int err = 0;
-
-	card = host->card;
-	mmc_claim_host(host);
-
-	/*
-	 * Send power notify command only if card
-	 * is mmc and notify state is powered ON
-	 */
-	if (card && mmc_card_mmc(card) &&
-	    (card->poweroff_notify_state == MMC_POWERED_ON)) {
-
-		if (host->power_notify_type == MMC_HOST_PW_NOTIFY_SHORT) {
-			notify_type = EXT_CSD_POWER_OFF_SHORT;
-			timeout = card->ext_csd.generic_cmd6_time;
-			card->poweroff_notify_state = MMC_POWEROFF_SHORT;
-		} else {
-			notify_type = EXT_CSD_POWER_OFF_LONG;
-			timeout = card->ext_csd.power_off_longtime;
-			card->poweroff_notify_state = MMC_POWEROFF_LONG;
-		}
-
-		err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
-				 EXT_CSD_POWER_OFF_NOTIFICATION,
-				 notify_type, timeout);
-
-		if (err && err != -EBADMSG)
-			pr_err("Device failed to respond within %d poweroff "
-			       "time. Forcefully powering down the device\n",
-			       timeout);
-
-		/* Set the card state to no notification after the poweroff */
-		card->poweroff_notify_state = MMC_NO_POWER_NOTIFICATION;
-	}
-	mmc_release_host(host);
-}
-
 /*
  * Apply power to the MMC stack.  This is a two-stage process.
  * First, we enable power to the card without the clock running.
@@ -1237,8 +1335,6 @@ static void mmc_power_up(struct mmc_host *host)
 
 void mmc_power_off(struct mmc_host *host)
 {
-	int err = 0;
-
 	if (host->ios.power_mode == MMC_POWER_OFF)
 		return;
 
@@ -1247,22 +1343,6 @@ void mmc_power_off(struct mmc_host *host)
 	host->ios.clock = 0;
 	host->ios.vdd = 0;
 
-	/*
-	 * For eMMC 4.5 device send AWAKE command before
-	 * POWER_OFF_NOTIFY command, because in sleep state
-	 * eMMC 4.5 devices respond to only RESET and AWAKE cmd
-	 */
-	if (host->card && mmc_card_is_sleep(host->card) &&
-	    host->bus_ops->resume) {
-		err = host->bus_ops->resume(host);
-
-		if (!err)
-			mmc_poweroff_notify(host);
-		else
-			pr_warning("%s: error %d during resume "
-				   "(continue with poweroff sequence)\n",
-				   mmc_hostname(host), err);
-	}
 
 	/*
 	 * Reset ocr mask to be the highest possible voltage supported for
@@ -2052,6 +2132,11 @@ void mmc_rescan(struct work_struct *work)
 	if (host->rescan_disable)
 		return;
 
+	/* If there is a non-removable card registered, only scan once */
+	if ((host->caps & MMC_CAP_NONREMOVABLE) && host->rescan_entered)
+		return;
+	host->rescan_entered = 1;
+
 	mmc_bus_get(host);
 
 	/*
@@ -2327,9 +2412,14 @@ int mmc_suspend_host(struct mmc_host *host)
 
 	mmc_bus_get(host);
 	if (host->bus_ops && !host->bus_dead) {
-
-		if (host->bus_ops->suspend)
+		if (host->bus_ops->suspend) {
+			if (mmc_card_doing_bkops(host->card)) {
+				err = mmc_stop_bkops(host->card);
+				if (err)
+					goto out;
+			}
 			err = host->bus_ops->suspend(host);
+		}
 
 		if (err == -ENOSYS || !host->bus_ops->resume) {
 			/*
@@ -2411,15 +2501,24 @@ int mmc_pm_notify(struct notifier_block *notify_block,
 	struct mmc_host *host = container_of(
 		notify_block, struct mmc_host, pm_notify);
 	unsigned long flags;
-
+	int err = 0;
 
 	switch (mode) {
 	case PM_HIBERNATION_PREPARE:
 	case PM_SUSPEND_PREPARE:
+		if (host->card && mmc_card_mmc(host->card) &&
+		    mmc_card_doing_bkops(host->card)) {
+			err = mmc_stop_bkops(host->card);
+			if (err) {
+				pr_err("%s: didn't stop bkops\n",
+					mmc_hostname(host));
+				return err;
+			}
+			mmc_card_clr_doing_bkops(host->card);
+		}
 
 		spin_lock_irqsave(&host->lock, flags);
 		host->rescan_disable = 1;
-		host->power_notify_type = MMC_HOST_PW_NOTIFY_SHORT;
 		spin_unlock_irqrestore(&host->lock, flags);
 		cancel_delayed_work_sync(&host->detect);
 
@@ -2443,7 +2542,6 @@ int mmc_pm_notify(struct notifier_block *notify_block,
 
 		spin_lock_irqsave(&host->lock, flags);
 		host->rescan_disable = 0;
-		host->power_notify_type = MMC_HOST_PW_NOTIFY_LONG;
 		spin_unlock_irqrestore(&host->lock, flags);
 		mmc_detect_change(host, 0);
 
diff --git a/drivers/mmc/core/debugfs.c b/drivers/mmc/core/debugfs.c
index 9ab5b17d488..d96c643dde1 100644
--- a/drivers/mmc/core/debugfs.c
+++ b/drivers/mmc/core/debugfs.c
@@ -281,7 +281,7 @@ static int mmc_ext_csd_open(struct inode *inode, struct file *filp)
 	if (err)
 		goto out_free;
 
-	for (i = 511; i >= 0; i--)
+	for (i = 0; i < 512; i++)
 		n += sprintf(buf + n, "%02x", ext_csd[i]);
 	n += sprintf(buf + n, "\n");
 	BUG_ON(n != EXT_CSD_STR_LEN);
diff --git a/drivers/mmc/core/mmc.c b/drivers/mmc/core/mmc.c
index 396b25891bb..7cc46382fd6 100644
--- a/drivers/mmc/core/mmc.c
+++ b/drivers/mmc/core/mmc.c
@@ -463,6 +463,17 @@ static int mmc_read_ext_csd(struct mmc_card *card, u8 *ext_csd)
 	}
 
 	if (card->ext_csd.rev >= 5) {
+		/* check whether the eMMC card supports BKOPS */
+		if (ext_csd[EXT_CSD_BKOPS_SUPPORT] & 0x1) {
+			card->ext_csd.bkops = 1;
+			card->ext_csd.bkops_en = ext_csd[EXT_CSD_BKOPS_EN];
+			card->ext_csd.raw_bkops_status =
+				ext_csd[EXT_CSD_BKOPS_STATUS];
+			if (!card->ext_csd.bkops_en)
+				pr_info("%s: BKOPS_EN bit is not set\n",
+					mmc_hostname(card->host));
+		}
+
 		/* check whether the eMMC card supports HPI */
 		if (ext_csd[EXT_CSD_HPI_FEATURES] & 0x1) {
 			card->ext_csd.hpi = 1;
@@ -996,7 +1007,7 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 		 * so check for success and update the flag
 		 */
 		if (!err)
-			card->poweroff_notify_state = MMC_POWERED_ON;
+			card->ext_csd.power_off_notification = EXT_CSD_POWER_ON;
 	}
 
 	/*
@@ -1262,6 +1273,35 @@ err:
 	return err;
 }
 
+static int mmc_can_poweroff_notify(const struct mmc_card *card)
+{
+	return card &&
+		mmc_card_mmc(card) &&
+		(card->ext_csd.power_off_notification == EXT_CSD_POWER_ON);
+}
+
+static int mmc_poweroff_notify(struct mmc_card *card, unsigned int notify_type)
+{
+	unsigned int timeout = card->ext_csd.generic_cmd6_time;
+	int err;
+
+	/* Use EXT_CSD_POWER_OFF_SHORT as default notification type. */
+	if (notify_type == EXT_CSD_POWER_OFF_LONG)
+		timeout = card->ext_csd.power_off_longtime;
+
+	err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
+			 EXT_CSD_POWER_OFF_NOTIFICATION,
+			 notify_type, timeout);
+	if (err)
+		pr_err("%s: Power Off Notification timed out, %u\n",
+		       mmc_hostname(card->host), timeout);
+
+	/* Disable the power off notification after the switch operation. */
+	card->ext_csd.power_off_notification = EXT_CSD_NO_POWER_NOTIFICATION;
+
+	return err;
+}
+
 /*
  * Host is being removed. Free up the current card.
  */
@@ -1322,11 +1362,11 @@ static int mmc_suspend(struct mmc_host *host)
 	BUG_ON(!host->card);
 
 	mmc_claim_host(host);
-	if (mmc_card_can_sleep(host)) {
+	if (mmc_can_poweroff_notify(host->card))
+		err = mmc_poweroff_notify(host->card, EXT_CSD_POWER_OFF_SHORT);
+	else if (mmc_card_can_sleep(host))
 		err = mmc_card_sleep(host);
-		if (!err)
-			mmc_card_set_sleep(host->card);
-	} else if (!mmc_host_is_spi(host))
+	else if (!mmc_host_is_spi(host))
 		err = mmc_deselect_cards(host);
 	host->card->state &= ~(MMC_STATE_HIGHSPEED | MMC_STATE_HIGHSPEED_200);
 	mmc_release_host(host);
@@ -1348,11 +1388,7 @@ static int mmc_resume(struct mmc_host *host)
 	BUG_ON(!host->card);
 
 	mmc_claim_host(host);
-	if (mmc_card_is_sleep(host->card)) {
-		err = mmc_card_awake(host);
-		mmc_card_clr_sleep(host->card);
-	} else
-		err = mmc_init_card(host, host->ocr, host->card);
+	err = mmc_init_card(host, host->ocr, host->card);
 	mmc_release_host(host);
 
 	return err;
@@ -1363,7 +1399,6 @@ static int mmc_power_restore(struct mmc_host *host)
 	int ret;
 
 	host->card->state &= ~(MMC_STATE_HIGHSPEED | MMC_STATE_HIGHSPEED_200);
-	mmc_card_clr_sleep(host->card);
 	mmc_claim_host(host);
 	ret = mmc_init_card(host, host->ocr, host->card);
 	mmc_release_host(host);
diff --git a/drivers/mmc/core/mmc_ops.c b/drivers/mmc/core/mmc_ops.c
index 0ed2cc5f35b..a0e172042e6 100644
--- a/drivers/mmc/core/mmc_ops.c
+++ b/drivers/mmc/core/mmc_ops.c
@@ -230,6 +230,10 @@ mmc_send_cxd_native(struct mmc_host *host, u32 arg, u32 *cxd, int opcode)
 	return 0;
 }
 
+/*
+ * NOTE: void *buf, caller for the buf is required to use DMA-capable
+ * buffer or on-stack buffer (with some overhead in callee).
+ */
 static int
 mmc_send_cxd_data(struct mmc_card *card, struct mmc_host *host,
 		u32 opcode, void *buf, unsigned len)
@@ -239,13 +243,19 @@ mmc_send_cxd_data(struct mmc_card *card, struct mmc_host *host,
 	struct mmc_data data = {0};
 	struct scatterlist sg;
 	void *data_buf;
+	int is_on_stack;
 
-	/* dma onto stack is unsafe/nonportable, but callers to this
-	 * routine normally provide temporary on-stack buffers ...
-	 */
-	data_buf = kmalloc(len, GFP_KERNEL);
-	if (data_buf == NULL)
-		return -ENOMEM;
+	is_on_stack = object_is_on_stack(buf);
+	if (is_on_stack) {
+		/*
+		 * dma onto stack is unsafe/nonportable, but callers to this
+		 * routine normally provide temporary on-stack buffers ...
+		 */
+		data_buf = kmalloc(len, GFP_KERNEL);
+		if (!data_buf)
+			return -ENOMEM;
+	} else
+		data_buf = buf;
 
 	mrq.cmd = &cmd;
 	mrq.data = &data;
@@ -280,8 +290,10 @@ mmc_send_cxd_data(struct mmc_card *card, struct mmc_host *host,
 
 	mmc_wait_for_req(host, &mrq);
 
-	memcpy(buf, data_buf, len);
-	kfree(data_buf);
+	if (is_on_stack) {
+		memcpy(buf, data_buf, len);
+		kfree(data_buf);
+	}
 
 	if (cmd.error)
 		return cmd.error;
@@ -294,24 +306,32 @@ mmc_send_cxd_data(struct mmc_card *card, struct mmc_host *host,
 int mmc_send_csd(struct mmc_card *card, u32 *csd)
 {
 	int ret, i;
+	u32 *csd_tmp;
 
 	if (!mmc_host_is_spi(card->host))
 		return mmc_send_cxd_native(card->host, card->rca << 16,
 				csd, MMC_SEND_CSD);
 
-	ret = mmc_send_cxd_data(card, card->host, MMC_SEND_CSD, csd, 16);
+	csd_tmp = kmalloc(16, GFP_KERNEL);
+	if (!csd_tmp)
+		return -ENOMEM;
+
+	ret = mmc_send_cxd_data(card, card->host, MMC_SEND_CSD, csd_tmp, 16);
 	if (ret)
-		return ret;
+		goto err;
 
 	for (i = 0;i < 4;i++)
-		csd[i] = be32_to_cpu(csd[i]);
+		csd[i] = be32_to_cpu(csd_tmp[i]);
 
-	return 0;
+err:
+	kfree(csd_tmp);
+	return ret;
 }
 
 int mmc_send_cid(struct mmc_host *host, u32 *cid)
 {
 	int ret, i;
+	u32 *cid_tmp;
 
 	if (!mmc_host_is_spi(host)) {
 		if (!host->card)
@@ -320,14 +340,20 @@ int mmc_send_cid(struct mmc_host *host, u32 *cid)
 				cid, MMC_SEND_CID);
 	}
 
-	ret = mmc_send_cxd_data(NULL, host, MMC_SEND_CID, cid, 16);
+	cid_tmp = kmalloc(16, GFP_KERNEL);
+	if (!cid_tmp)
+		return -ENOMEM;
+
+	ret = mmc_send_cxd_data(NULL, host, MMC_SEND_CID, cid_tmp, 16);
 	if (ret)
-		return ret;
+		goto err;
 
 	for (i = 0;i < 4;i++)
-		cid[i] = be32_to_cpu(cid[i]);
+		cid[i] = be32_to_cpu(cid_tmp[i]);
 
-	return 0;
+err:
+	kfree(cid_tmp);
+	return ret;
 }
 
 int mmc_send_ext_csd(struct mmc_card *card, u8 *ext_csd)
@@ -367,18 +393,19 @@ int mmc_spi_set_crc(struct mmc_host *host, int use_crc)
 }
 
 /**
- *	mmc_switch - modify EXT_CSD register
+ *	__mmc_switch - modify EXT_CSD register
  *	@card: the MMC card associated with the data transfer
  *	@set: cmd set values
  *	@index: EXT_CSD register index
  *	@value: value to program into EXT_CSD register
  *	@timeout_ms: timeout (ms) for operation performed by register write,
  *                   timeout of zero implies maximum possible timeout
+ *	@use_busy_signal: use the busy signal as response type
  *
  *	Modifies the EXT_CSD register for selected card.
  */
-int mmc_switch(struct mmc_card *card, u8 set, u8 index, u8 value,
-	       unsigned int timeout_ms)
+int __mmc_switch(struct mmc_card *card, u8 set, u8 index, u8 value,
+	       unsigned int timeout_ms, bool use_busy_signal)
 {
 	int err;
 	struct mmc_command cmd = {0};
@@ -392,13 +419,23 @@ int mmc_switch(struct mmc_card *card, u8 set, u8 index, u8 value,
 		  (index << 16) |
 		  (value << 8) |
 		  set;
-	cmd.flags = MMC_RSP_SPI_R1B | MMC_RSP_R1B | MMC_CMD_AC;
+	cmd.flags = MMC_CMD_AC;
+	if (use_busy_signal)
+		cmd.flags |= MMC_RSP_SPI_R1B | MMC_RSP_R1B;
+	else
+		cmd.flags |= MMC_RSP_SPI_R1 | MMC_RSP_R1;
+
+
 	cmd.cmd_timeout_ms = timeout_ms;
 
 	err = mmc_wait_for_cmd(card->host, &cmd, MMC_CMD_RETRIES);
 	if (err)
 		return err;
 
+	/* No need to check card status in case of unblocking command */
+	if (!use_busy_signal)
+		return 0;
+
 	/* Must check status to be sure of no errors */
 	do {
 		err = mmc_send_status(card, &status);
@@ -423,6 +460,13 @@ int mmc_switch(struct mmc_card *card, u8 set, u8 index, u8 value,
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(__mmc_switch);
+
+int mmc_switch(struct mmc_card *card, u8 set, u8 index, u8 value,
+		unsigned int timeout_ms)
+{
+	return __mmc_switch(card, set, index, value, timeout_ms, true);
+}
 EXPORT_SYMBOL_GPL(mmc_switch);
 
 int mmc_send_status(struct mmc_card *card, u32 *status)
diff --git a/drivers/mmc/core/sdio_bus.c b/drivers/mmc/core/sdio_bus.c
index 236842ec955..6bf68799fe9 100644
--- a/drivers/mmc/core/sdio_bus.c
+++ b/drivers/mmc/core/sdio_bus.c
@@ -193,14 +193,7 @@ static int sdio_bus_remove(struct device *dev)
 }
 
 #ifdef CONFIG_PM
-
-static int pm_no_operation(struct device *dev)
-{
-	return 0;
-}
-
 static const struct dev_pm_ops sdio_bus_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(pm_no_operation, pm_no_operation)
 	SET_RUNTIME_PM_OPS(
 		pm_generic_runtime_suspend,
 		pm_generic_runtime_resume,
diff --git a/drivers/mmc/core/slot-gpio.c b/drivers/mmc/core/slot-gpio.c
index 058242916ce..08c6b3dfe08 100644
--- a/drivers/mmc/core/slot-gpio.c
+++ b/drivers/mmc/core/slot-gpio.c
@@ -100,7 +100,13 @@ int mmc_gpio_request_ro(struct mmc_host *host, unsigned int gpio)
 
 	ctx = host->slot.handler_priv;
 
-	return gpio_request_one(gpio, GPIOF_DIR_IN, ctx->ro_label);
+	ret = gpio_request_one(gpio, GPIOF_DIR_IN, ctx->ro_label);
+	if (ret < 0)
+		return ret;
+
+	ctx->ro_gpio = gpio;
+
+	return 0;
 }
 EXPORT_SYMBOL(mmc_gpio_request_ro);
 
diff --git a/drivers/mmc/host/Kconfig b/drivers/mmc/host/Kconfig
index aa131b32e3b..9bf10e7bbfa 100644
--- a/drivers/mmc/host/Kconfig
+++ b/drivers/mmc/host/Kconfig
@@ -540,6 +540,15 @@ config MMC_DW_PLTFM
 
 	  If unsure, say Y.
 
+config MMC_DW_EXYNOS
+	tristate "Exynos specific extentions for Synopsys DW Memory Card Interface"
+	depends on MMC_DW
+	select MMC_DW_PLTFM
+	help
+	  This selects support for Samsung Exynos SoC specific extensions to the
+	  Synopsys DesignWare Memory Card Interface driver. Select this option
+	  for platforms based on Exynos4 and Exynos5 SoC's.
+
 config MMC_DW_PCI
 	tristate "Synopsys Designware MCI support on PCI bus"
 	depends on MMC_DW && PCI
diff --git a/drivers/mmc/host/Makefile b/drivers/mmc/host/Makefile
index 8922b06be92..17ad0a7ba40 100644
--- a/drivers/mmc/host/Makefile
+++ b/drivers/mmc/host/Makefile
@@ -39,6 +39,7 @@ obj-$(CONFIG_MMC_VIA_SDMMC)	+= via-sdmmc.o
 obj-$(CONFIG_SDH_BFIN)		+= bfin_sdh.o
 obj-$(CONFIG_MMC_DW)		+= dw_mmc.o
 obj-$(CONFIG_MMC_DW_PLTFM)	+= dw_mmc-pltfm.o
+obj-$(CONFIG_MMC_DW_EXYNOS)	+= dw_mmc-exynos.o
 obj-$(CONFIG_MMC_DW_PCI)	+= dw_mmc-pci.o
 obj-$(CONFIG_MMC_SH_MMCIF)	+= sh_mmcif.o
 obj-$(CONFIG_MMC_JZ4740)	+= jz4740_mmc.o
diff --git a/drivers/mmc/host/atmel-mci-regs.h b/drivers/mmc/host/atmel-mci-regs.h
index ab56f7db531..c97001e1522 100644
--- a/drivers/mmc/host/atmel-mci-regs.h
+++ b/drivers/mmc/host/atmel-mci-regs.h
@@ -140,6 +140,13 @@
 #define atmci_writel(port,reg,value)			\
 	__raw_writel((value), (port)->regs + reg)
 
+/* On AVR chips the Peripheral DMA Controller is not connected to MCI. */
+#ifdef CONFIG_AVR32
+#	define ATMCI_PDC_CONNECTED	0
+#else
+#	define ATMCI_PDC_CONNECTED	1
+#endif
+
 /*
  * Fix sconfig's burst size according to atmel MCI. We need to convert them as:
  * 1 -> 0, 4 -> 1, 8 -> 2, 16 -> 3.
diff --git a/drivers/mmc/host/atmel-mci.c b/drivers/mmc/host/atmel-mci.c
index 852d5fbda63..ddf096e3803 100644
--- a/drivers/mmc/host/atmel-mci.c
+++ b/drivers/mmc/host/atmel-mci.c
@@ -19,6 +19,9 @@
 #include <linux/interrupt.h>
 #include <linux/ioport.h>
 #include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
 #include <linux/platform_device.h>
 #include <linux/scatterlist.h>
 #include <linux/seq_file.h>
@@ -71,7 +74,7 @@ enum atmci_pdc_buf {
 };
 
 struct atmel_mci_caps {
-	bool    has_dma;
+	bool    has_dma_conf_reg;
 	bool    has_pdc;
 	bool    has_cfg_reg;
 	bool    has_cstor_reg;
@@ -418,7 +421,7 @@ static int atmci_regs_show(struct seq_file *s, void *v)
 	atmci_show_status_reg(s, "SR", buf[ATMCI_SR / 4]);
 	atmci_show_status_reg(s, "IMR", buf[ATMCI_IMR / 4]);
 
-	if (host->caps.has_dma) {
+	if (host->caps.has_dma_conf_reg) {
 		u32 val;
 
 		val = buf[ATMCI_DMA / 4];
@@ -500,6 +503,70 @@ err:
 	dev_err(&mmc->class_dev, "failed to initialize debugfs for slot\n");
 }
 
+#if defined(CONFIG_OF)
+static const struct of_device_id atmci_dt_ids[] = {
+	{ .compatible = "atmel,hsmci" },
+	{ /* sentinel */ }
+};
+
+MODULE_DEVICE_TABLE(of, atmci_dt_ids);
+
+static struct mci_platform_data __devinit*
+atmci_of_init(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct device_node *cnp;
+	struct mci_platform_data *pdata;
+	u32 slot_id;
+
+	if (!np) {
+		dev_err(&pdev->dev, "device node not found\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata) {
+		dev_err(&pdev->dev, "could not allocate memory for pdata\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	for_each_child_of_node(np, cnp) {
+		if (of_property_read_u32(cnp, "reg", &slot_id)) {
+			dev_warn(&pdev->dev, "reg property is missing for %s\n",
+				 cnp->full_name);
+			continue;
+		}
+
+		if (slot_id >= ATMCI_MAX_NR_SLOTS) {
+			dev_warn(&pdev->dev, "can't have more than %d slots\n",
+			         ATMCI_MAX_NR_SLOTS);
+			break;
+		}
+
+		if (of_property_read_u32(cnp, "bus-width",
+		                         &pdata->slot[slot_id].bus_width))
+			pdata->slot[slot_id].bus_width = 1;
+
+		pdata->slot[slot_id].detect_pin =
+			of_get_named_gpio(cnp, "cd-gpios", 0);
+
+		pdata->slot[slot_id].detect_is_active_high =
+			of_property_read_bool(cnp, "cd-inverted");
+
+		pdata->slot[slot_id].wp_pin =
+			of_get_named_gpio(cnp, "wp-gpios", 0);
+	}
+
+	return pdata;
+}
+#else /* CONFIG_OF */
+static inline struct mci_platform_data*
+atmci_of_init(struct platform_device *dev)
+{
+	return ERR_PTR(-EINVAL);
+}
+#endif
+
 static inline unsigned int atmci_get_version(struct atmel_mci *host)
 {
 	return atmci_readl(host, ATMCI_VERSION) & 0x00000fff;
@@ -774,7 +841,7 @@ static void atmci_dma_complete(void *arg)
 
 	dev_vdbg(&host->pdev->dev, "DMA complete\n");
 
-	if (host->caps.has_dma)
+	if (host->caps.has_dma_conf_reg)
 		/* Disable DMA hardware handshaking on MCI */
 		atmci_writel(host, ATMCI_DMA, atmci_readl(host, ATMCI_DMA) & ~ATMCI_DMAEN);
 
@@ -961,7 +1028,9 @@ atmci_prepare_data_dma(struct atmel_mci *host, struct mmc_data *data)
 		maxburst = atmci_convert_chksize(host->dma_conf.dst_maxburst);
 	}
 
-	atmci_writel(host, ATMCI_DMA, ATMCI_DMA_CHKSIZE(maxburst) | ATMCI_DMAEN);
+	if (host->caps.has_dma_conf_reg)
+		atmci_writel(host, ATMCI_DMA, ATMCI_DMA_CHKSIZE(maxburst) |
+			ATMCI_DMAEN);
 
 	sglen = dma_map_sg(chan->device->dev, data->sg,
 			data->sg_len, direction);
@@ -2046,6 +2115,13 @@ static int __init atmci_init_slot(struct atmel_mci *host,
 	slot->sdc_reg = sdc_reg;
 	slot->sdio_irq = sdio_irq;
 
+	dev_dbg(&mmc->class_dev,
+	        "slot[%u]: bus_width=%u, detect_pin=%d, "
+		"detect_is_active_high=%s, wp_pin=%d\n",
+		id, slot_data->bus_width, slot_data->detect_pin,
+		slot_data->detect_is_active_high ? "true" : "false",
+		slot_data->wp_pin);
+
 	mmc->ops = &atmci_ops;
 	mmc->f_min = DIV_ROUND_UP(host->bus_hz, 512);
 	mmc->f_max = host->bus_hz / 2;
@@ -2169,7 +2245,10 @@ static bool atmci_configure_dma(struct atmel_mci *host)
 
 	pdata = host->pdev->dev.platform_data;
 
-	if (pdata && find_slave_dev(pdata->dma_slave)) {
+	if (!pdata)
+		return false;
+
+	if (pdata->dma_slave && find_slave_dev(pdata->dma_slave)) {
 		dma_cap_mask_t mask;
 
 		/* Try to grab a DMA channel */
@@ -2210,8 +2289,8 @@ static void __init atmci_get_cap(struct atmel_mci *host)
 	dev_info(&host->pdev->dev,
 			"version: 0x%x\n", version);
 
-	host->caps.has_dma = 0;
-	host->caps.has_pdc = 1;
+	host->caps.has_dma_conf_reg = 0;
+	host->caps.has_pdc = ATMCI_PDC_CONNECTED;
 	host->caps.has_cfg_reg = 0;
 	host->caps.has_cstor_reg = 0;
 	host->caps.has_highspeed = 0;
@@ -2228,12 +2307,7 @@ static void __init atmci_get_cap(struct atmel_mci *host)
 		host->caps.has_odd_clk_div = 1;
 	case 0x400:
 	case 0x300:
-#ifdef CONFIG_AT_HDMAC
-		host->caps.has_dma = 1;
-#else
-		dev_info(&host->pdev->dev,
-			"has dma capability but dma engine is not selected, then use pio\n");
-#endif
+		host->caps.has_dma_conf_reg = 1;
 		host->caps.has_pdc = 0;
 		host->caps.has_cfg_reg = 1;
 		host->caps.has_cstor_reg = 1;
@@ -2268,8 +2342,14 @@ static int __init atmci_probe(struct platform_device *pdev)
 	if (!regs)
 		return -ENXIO;
 	pdata = pdev->dev.platform_data;
-	if (!pdata)
-		return -ENXIO;
+	if (!pdata) {
+		pdata = atmci_of_init(pdev);
+		if (IS_ERR(pdata)) {
+			dev_err(&pdev->dev, "platform data not available\n");
+			return PTR_ERR(pdata);
+		}
+	}
+
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0)
 		return irq;
@@ -2308,7 +2388,7 @@ static int __init atmci_probe(struct platform_device *pdev)
 
 	/* Get MCI capabilities and set operations according to it */
 	atmci_get_cap(host);
-	if (host->caps.has_dma && atmci_configure_dma(host)) {
+	if (atmci_configure_dma(host)) {
 		host->prepare_data = &atmci_prepare_data_dma;
 		host->submit_data = &atmci_submit_data_dma;
 		host->stop_transfer = &atmci_stop_transfer_dma;
@@ -2487,6 +2567,7 @@ static struct platform_driver atmci_driver = {
 	.driver		= {
 		.name		= "atmel_mci",
 		.pm		= ATMCI_PM_OPS,
+		.of_match_table	= of_match_ptr(atmci_dt_ids),
 	},
 };
 
diff --git a/drivers/mmc/host/bfin_sdh.c b/drivers/mmc/host/bfin_sdh.c
index a17dd7363ce..b9b463eca1e 100644
--- a/drivers/mmc/host/bfin_sdh.c
+++ b/drivers/mmc/host/bfin_sdh.c
@@ -24,9 +24,7 @@
 #include <asm/portmux.h>
 #include <asm/bfin_sdh.h>
 
-#if defined(CONFIG_BF51x)
-#define bfin_read_SDH_PWR_CTL		bfin_read_RSI_PWR_CTL
-#define bfin_write_SDH_PWR_CTL		bfin_write_RSI_PWR_CTL
+#if defined(CONFIG_BF51x) || defined(__ADSPBF60x__)
 #define bfin_read_SDH_CLK_CTL		bfin_read_RSI_CLK_CTL
 #define bfin_write_SDH_CLK_CTL		bfin_write_RSI_CLK_CTL
 #define bfin_write_SDH_ARGUMENT		bfin_write_RSI_ARGUMENT
@@ -45,8 +43,16 @@
 #define bfin_write_SDH_E_STATUS		bfin_write_RSI_E_STATUS
 #define bfin_read_SDH_STATUS		bfin_read_RSI_STATUS
 #define bfin_write_SDH_MASK0		bfin_write_RSI_MASK0
+#define bfin_write_SDH_E_MASK		bfin_write_RSI_E_MASK
 #define bfin_read_SDH_CFG		bfin_read_RSI_CFG
 #define bfin_write_SDH_CFG		bfin_write_RSI_CFG
+# if defined(__ADSPBF60x__)
+#  define bfin_read_SDH_BLK_SIZE	bfin_read_RSI_BLKSZ
+#  define bfin_write_SDH_BLK_SIZE	bfin_write_RSI_BLKSZ
+# else
+#  define bfin_read_SDH_PWR_CTL		bfin_read_RSI_PWR_CTL
+#  define bfin_write_SDH_PWR_CTL	bfin_write_RSI_PWR_CTL
+# endif
 #endif
 
 struct sdh_host {
@@ -62,6 +68,7 @@ struct sdh_host {
 	dma_addr_t		sg_dma;
 	int			dma_len;
 
+	unsigned long		sclk;
 	unsigned int		imask;
 	unsigned int		power_mode;
 	unsigned int		clk_div;
@@ -127,11 +134,15 @@ static int sdh_setup_data(struct sdh_host *host, struct mmc_data *data)
 	/* Only supports power-of-2 block size */
 	if (data->blksz & (data->blksz - 1))
 		return -EINVAL;
+#ifndef RSI_BLKSZ
 	data_ctl |= ((ffs(data->blksz) - 1) << 4);
+#else
+        bfin_write_SDH_BLK_SIZE(data->blksz);
+#endif
 
 	bfin_write_SDH_DATA_CTL(data_ctl);
 	/* the time of a host clock period in ns */
-	cycle_ns = 1000000000 / (get_sclk() / (2 * (host->clk_div + 1)));
+	cycle_ns = 1000000000 / (host->sclk / (2 * (host->clk_div + 1)));
 	timeout = data->timeout_ns / cycle_ns;
 	timeout += data->timeout_clks;
 	bfin_write_SDH_DATA_TIMER(timeout);
@@ -145,8 +156,13 @@ static int sdh_setup_data(struct sdh_host *host, struct mmc_data *data)
 
 	sdh_enable_stat_irq(host, (DAT_CRC_FAIL | DAT_TIME_OUT | DAT_END));
 	host->dma_len = dma_map_sg(mmc_dev(host->mmc), data->sg, data->sg_len, host->dma_dir);
-#if defined(CONFIG_BF54x)
-	dma_cfg |= DMAFLOW_ARRAY | NDSIZE_5 | RESTART | WDSIZE_32 | DMAEN;
+#if defined(CONFIG_BF54x) || defined(CONFIG_BF60x)
+	dma_cfg |= DMAFLOW_ARRAY | RESTART | WDSIZE_32 | DMAEN;
+# ifdef RSI_BLKSZ
+	dma_cfg |= PSIZE_32 | NDSIZE_3;
+# else
+	dma_cfg |= NDSIZE_5;
+# endif
 	{
 		struct scatterlist *sg;
 		int i;
@@ -156,7 +172,7 @@ static int sdh_setup_data(struct sdh_host *host, struct mmc_data *data)
 			host->sg_cpu[i].x_count = sg_dma_len(sg) / 4;
 			host->sg_cpu[i].x_modify = 4;
 			dev_dbg(mmc_dev(host->mmc), "%d: start_addr:0x%lx, "
-				"cfg:0x%x, x_count:0x%x, x_modify:0x%x\n",
+				"cfg:0x%lx, x_count:0x%lx, x_modify:0x%lx\n",
 				i, host->sg_cpu[i].start_addr,
 				host->sg_cpu[i].cfg, host->sg_cpu[i].x_count,
 				host->sg_cpu[i].x_modify);
@@ -172,6 +188,7 @@ static int sdh_setup_data(struct sdh_host *host, struct mmc_data *data)
 	set_dma_curr_desc_addr(host->dma_ch, (unsigned long *)host->sg_dma);
 	set_dma_x_count(host->dma_ch, 0);
 	set_dma_x_modify(host->dma_ch, 0);
+	SSYNC();
 	set_dma_config(host->dma_ch, dma_cfg);
 #elif defined(CONFIG_BF51x)
 	/* RSI DMA doesn't work in array mode */
@@ -179,6 +196,7 @@ static int sdh_setup_data(struct sdh_host *host, struct mmc_data *data)
 	set_dma_start_addr(host->dma_ch, sg_dma_address(&data->sg[0]));
 	set_dma_x_count(host->dma_ch, length / 4);
 	set_dma_x_modify(host->dma_ch, 4);
+	SSYNC();
 	set_dma_config(host->dma_ch, dma_cfg);
 #endif
 	bfin_write_SDH_DATA_CTL(bfin_read_SDH_DATA_CTL() | DTX_DMA_E | DTX_E);
@@ -296,7 +314,6 @@ static int sdh_data_done(struct sdh_host *host, unsigned int stat)
 	else
 		data->bytes_xfered = 0;
 
-	sdh_disable_stat_irq(host, DAT_END | DAT_TIME_OUT | DAT_CRC_FAIL | RX_OVERRUN | TX_UNDERRUN);
 	bfin_write_SDH_STATUS_CLR(DAT_END_STAT | DAT_TIMEOUT_STAT | \
 			DAT_CRC_FAIL_STAT | DAT_BLK_END_STAT | RX_OVERRUN | TX_UNDERRUN);
 	bfin_write_SDH_DATA_CTL(0);
@@ -321,74 +338,115 @@ static void sdh_request(struct mmc_host *mmc, struct mmc_request *mrq)
 	dev_dbg(mmc_dev(host->mmc), "%s enter, mrp:%p, cmd:%p\n", __func__, mrq, mrq->cmd);
 	WARN_ON(host->mrq != NULL);
 
+	spin_lock(&host->lock);
 	host->mrq = mrq;
 	host->data = mrq->data;
 
 	if (mrq->data && mrq->data->flags & MMC_DATA_READ) {
 		ret = sdh_setup_data(host, mrq->data);
 		if (ret)
-			return;
+			goto data_err;
 	}
 
 	sdh_start_cmd(host, mrq->cmd);
+data_err:
+	spin_unlock(&host->lock);
 }
 
 static void sdh_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 {
 	struct sdh_host *host;
-	unsigned long flags;
 	u16 clk_ctl = 0;
+#ifndef RSI_BLKSZ
 	u16 pwr_ctl = 0;
+#endif
 	u16 cfg;
 	host = mmc_priv(mmc);
 
-	spin_lock_irqsave(&host->lock, flags);
-	if (ios->clock) {
-		unsigned long  sys_clk, ios_clk;
-		unsigned char clk_div;
-		ios_clk = 2 * ios->clock;
-		sys_clk = get_sclk();
-		clk_div = sys_clk / ios_clk;
-		if (sys_clk % ios_clk == 0)
-			clk_div -= 1;
-		clk_div = min_t(unsigned char, clk_div, 0xFF);
-		clk_ctl |= clk_div;
-		clk_ctl |= CLK_E;
-		host->clk_div = clk_div;
-	} else
-		sdh_stop_clock(host);
-
-	if (ios->bus_mode == MMC_BUSMODE_OPENDRAIN)
-#ifdef CONFIG_SDH_BFIN_MISSING_CMD_PULLUP_WORKAROUND
-		pwr_ctl |= ROD_CTL;
-#else
-		pwr_ctl |= SD_CMD_OD | ROD_CTL;
-#endif
+	spin_lock(&host->lock);
 
-	if (ios->bus_width == MMC_BUS_WIDTH_4) {
-		cfg = bfin_read_SDH_CFG();
+	cfg = bfin_read_SDH_CFG();
+	cfg |= MWE;
+	switch (ios->bus_width) {
+	case MMC_BUS_WIDTH_4:
+#ifndef RSI_BLKSZ
 		cfg &= ~PD_SDDAT3;
+#endif
 		cfg |= PUP_SDDAT3;
 		/* Enable 4 bit SDIO */
-		cfg |= (SD4E | MWE);
-		bfin_write_SDH_CFG(cfg);
-		clk_ctl |= WIDE_BUS;
-	} else {
-		cfg = bfin_read_SDH_CFG();
-		cfg |= MWE;
-		bfin_write_SDH_CFG(cfg);
+		cfg |= SD4E;
+		clk_ctl |= WIDE_BUS_4;
+		break;
+	case MMC_BUS_WIDTH_8:
+#ifndef RSI_BLKSZ
+		cfg &= ~PD_SDDAT3;
+#endif
+		cfg |= PUP_SDDAT3;
+		/* Disable 4 bit SDIO */
+		cfg &= ~SD4E;
+		clk_ctl |= BYTE_BUS_8;
+		break;
+	default:
+		cfg &= ~PUP_SDDAT3;
+		/* Disable 4 bit SDIO */
+		cfg &= ~SD4E;
 	}
 
-	bfin_write_SDH_CLK_CTL(clk_ctl);
-
 	host->power_mode = ios->power_mode;
-	if (ios->power_mode == MMC_POWER_ON)
+#ifndef RSI_BLKSZ
+	if (ios->bus_mode == MMC_BUSMODE_OPENDRAIN) {
+		pwr_ctl |= ROD_CTL;
+# ifndef CONFIG_SDH_BFIN_MISSING_CMD_PULLUP_WORKAROUND
+		pwr_ctl |= SD_CMD_OD;
+# endif
+	}
+
+	if (ios->power_mode != MMC_POWER_OFF)
 		pwr_ctl |= PWR_ON;
+	else
+		pwr_ctl &= ~PWR_ON;
 
 	bfin_write_SDH_PWR_CTL(pwr_ctl);
+#else
+# ifndef CONFIG_SDH_BFIN_MISSING_CMD_PULLUP_WORKAROUND
+	if (ios->bus_mode == MMC_BUSMODE_OPENDRAIN)
+		cfg |= SD_CMD_OD;
+	else
+		cfg &= ~SD_CMD_OD;
+# endif
+
+
+	if (ios->power_mode != MMC_POWER_OFF)
+		cfg |= PWR_ON;
+	else
+		cfg &= ~PWR_ON;
+
+	bfin_write_SDH_CFG(cfg);
+#endif
 	SSYNC();
 
-	spin_unlock_irqrestore(&host->lock, flags);
+	if (ios->power_mode == MMC_POWER_ON && ios->clock) {
+		unsigned char clk_div;
+		clk_div = (get_sclk() / ios->clock - 1) / 2;
+		clk_div = min_t(unsigned char, clk_div, 0xFF);
+		clk_ctl |= clk_div;
+		clk_ctl |= CLK_E;
+		host->clk_div = clk_div;
+		bfin_write_SDH_CLK_CTL(clk_ctl);
+
+	} else
+		sdh_stop_clock(host);
+
+	/* set up sdh interrupt mask*/
+	if (ios->power_mode == MMC_POWER_ON)
+		bfin_write_SDH_MASK0(DAT_END | DAT_TIME_OUT | DAT_CRC_FAIL |
+			RX_OVERRUN | TX_UNDERRUN | CMD_SENT | CMD_RESP_END |
+			CMD_TIME_OUT | CMD_CRC_FAIL);
+	else
+		bfin_write_SDH_MASK0(0);
+	SSYNC();
+
+	spin_unlock(&host->lock);
 
 	dev_dbg(mmc_dev(host->mmc), "SDH: clk_div = 0x%x actual clock:%ld expected clock:%d\n",
 		host->clk_div,
@@ -405,7 +463,7 @@ static irqreturn_t sdh_dma_irq(int irq, void *devid)
 {
 	struct sdh_host *host = devid;
 
-	dev_dbg(mmc_dev(host->mmc), "%s enter, irq_stat: 0x%04x\n", __func__,
+	dev_dbg(mmc_dev(host->mmc), "%s enter, irq_stat: 0x%04lx\n", __func__,
 		get_dma_curr_irqstat(host->dma_ch));
 	clear_dma_irqstat(host->dma_ch);
 	SSYNC();
@@ -420,6 +478,9 @@ static irqreturn_t sdh_stat_irq(int irq, void *devid)
 	int handled = 0;
 
 	dev_dbg(mmc_dev(host->mmc), "%s enter\n", __func__);
+
+	spin_lock(&host->lock);
+
 	status = bfin_read_SDH_E_STATUS();
 	if (status & SD_CARD_DET) {
 		mmc_detect_change(host->mmc, 0);
@@ -437,11 +498,30 @@ static irqreturn_t sdh_stat_irq(int irq, void *devid)
 	if (status & (DAT_END | DAT_TIME_OUT | DAT_CRC_FAIL | RX_OVERRUN | TX_UNDERRUN))
 		handled |= sdh_data_done(host, status);
 
+	spin_unlock(&host->lock);
+
 	dev_dbg(mmc_dev(host->mmc), "%s exit\n\n", __func__);
 
 	return IRQ_RETVAL(handled);
 }
 
+static void sdh_reset(void)
+{
+#if defined(CONFIG_BF54x)
+	/* Secure Digital Host shares DMA with Nand controller */
+	bfin_write_DMAC1_PERIMUX(bfin_read_DMAC1_PERIMUX() | 0x1);
+#endif
+
+	bfin_write_SDH_CFG(bfin_read_SDH_CFG() | CLKS_EN);
+	SSYNC();
+
+	/* Disable card inserting detection pin. set MMC_CAP_NEEDS_POLL, and
+	 * mmc stack will do the detection.
+	 */
+	bfin_write_SDH_CFG((bfin_read_SDH_CFG() & 0x1F) | (PUP_SDDAT | PUP_SDDAT3));
+	SSYNC();
+}
+
 static int __devinit sdh_probe(struct platform_device *pdev)
 {
 	struct mmc_host *mmc;
@@ -462,8 +542,16 @@ static int __devinit sdh_probe(struct platform_device *pdev)
 	}
 
 	mmc->ops = &sdh_ops;
-	mmc->max_segs = 32;
+#if defined(CONFIG_BF51x)
+	mmc->max_segs = 1;
+#else
+	mmc->max_segs = PAGE_SIZE / sizeof(struct dma_desc_array);
+#endif
+#ifdef RSI_BLKSZ
+	mmc->max_seg_size = -1;
+#else
 	mmc->max_seg_size = 1 << 16;
+#endif
 	mmc->max_blk_size = 1 << 11;
 	mmc->max_blk_count = 1 << 11;
 	mmc->max_req_size = PAGE_SIZE;
@@ -473,6 +561,7 @@ static int __devinit sdh_probe(struct platform_device *pdev)
 	mmc->caps = MMC_CAP_4_BIT_DATA | MMC_CAP_NEEDS_POLL;
 	host = mmc_priv(mmc);
 	host->mmc = mmc;
+	host->sclk = get_sclk();
 
 	spin_lock_init(&host->lock);
 	host->irq = drv_data->irq_int0;
@@ -497,7 +586,6 @@ static int __devinit sdh_probe(struct platform_device *pdev)
 	}
 
 	platform_set_drvdata(pdev, mmc);
-	mmc_add_host(mmc);
 
 	ret = request_irq(host->irq, sdh_stat_irq, 0, "SDH Status IRQ", host);
 	if (ret) {
@@ -510,20 +598,10 @@ static int __devinit sdh_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "unable to request peripheral pins\n");
 		goto out4;
 	}
-#if defined(CONFIG_BF54x)
-	/* Secure Digital Host shares DMA with Nand controller */
-	bfin_write_DMAC1_PERIMUX(bfin_read_DMAC1_PERIMUX() | 0x1);
-#endif
-
-	bfin_write_SDH_CFG(bfin_read_SDH_CFG() | CLKS_EN);
-	SSYNC();
 
-	/* Disable card inserting detection pin. set MMC_CAP_NEES_POLL, and
-	 * mmc stack will do the detection.
-	 */
-	bfin_write_SDH_CFG((bfin_read_SDH_CFG() & 0x1F) | (PUP_SDDAT | PUP_SDDAT3));
-	SSYNC();
+	sdh_reset();
 
+	mmc_add_host(mmc);
 	return 0;
 
 out4:
@@ -571,7 +649,6 @@ static int sdh_suspend(struct platform_device *dev, pm_message_t state)
 	if (mmc)
 		ret = mmc_suspend_host(mmc);
 
-	bfin_write_SDH_PWR_CTL(bfin_read_SDH_PWR_CTL() & ~PWR_ON);
 	peripheral_free_list(drv_data->pin_req);
 
 	return ret;
@@ -589,16 +666,7 @@ static int sdh_resume(struct platform_device *dev)
 		return ret;
 	}
 
-	bfin_write_SDH_PWR_CTL(bfin_read_SDH_PWR_CTL() | PWR_ON);
-#if defined(CONFIG_BF54x)
-	/* Secure Digital Host shares DMA with Nand controller */
-	bfin_write_DMAC1_PERIMUX(bfin_read_DMAC1_PERIMUX() | 0x1);
-#endif
-	bfin_write_SDH_CFG(bfin_read_SDH_CFG() | CLKS_EN);
-	SSYNC();
-
-	bfin_write_SDH_CFG((bfin_read_SDH_CFG() & 0x1F) | (PUP_SDDAT | PUP_SDDAT3));
-	SSYNC();
+	sdh_reset();
 
 	if (mmc)
 		ret = mmc_resume_host(mmc);
diff --git a/drivers/mmc/host/davinci_mmc.c b/drivers/mmc/host/davinci_mmc.c
index 3dfd3473269..20636772c09 100644
--- a/drivers/mmc/host/davinci_mmc.c
+++ b/drivers/mmc/host/davinci_mmc.c
@@ -30,11 +30,12 @@
 #include <linux/io.h>
 #include <linux/irq.h>
 #include <linux/delay.h>
+#include <linux/dmaengine.h>
 #include <linux/dma-mapping.h>
+#include <linux/edma.h>
 #include <linux/mmc/mmc.h>
 
 #include <linux/platform_data/mmc-davinci.h>
-#include <mach/edma.h>
 
 /*
  * Register Definitions
@@ -200,21 +201,13 @@ struct mmc_davinci_host {
 	u32 bytes_left;
 
 	u32 rxdma, txdma;
+	struct dma_chan *dma_tx;
+	struct dma_chan *dma_rx;
 	bool use_dma;
 	bool do_dma;
 	bool sdio_int;
 	bool active_request;
 
-	/* Scatterlist DMA uses one or more parameter RAM entries:
-	 * the main one (associated with rxdma or txdma) plus zero or
-	 * more links.  The entries for a given transfer differ only
-	 * by memory buffer (address, length) and link field.
-	 */
-	struct edmacc_param	tx_template;
-	struct edmacc_param	rx_template;
-	unsigned		n_link;
-	u32			links[MAX_NR_SG - 1];
-
 	/* For PIO we walk scatterlists one segment at a time. */
 	unsigned int		sg_len;
 	struct scatterlist *sg;
@@ -410,153 +403,74 @@ static void mmc_davinci_start_command(struct mmc_davinci_host *host,
 
 static void davinci_abort_dma(struct mmc_davinci_host *host)
 {
-	int sync_dev;
+	struct dma_chan *sync_dev;
 
 	if (host->data_dir == DAVINCI_MMC_DATADIR_READ)
-		sync_dev = host->rxdma;
+		sync_dev = host->dma_rx;
 	else
-		sync_dev = host->txdma;
-
-	edma_stop(sync_dev);
-	edma_clean_channel(sync_dev);
-}
-
-static void
-mmc_davinci_xfer_done(struct mmc_davinci_host *host, struct mmc_data *data);
-
-static void mmc_davinci_dma_cb(unsigned channel, u16 ch_status, void *data)
-{
-	if (DMA_COMPLETE != ch_status) {
-		struct mmc_davinci_host *host = data;
-
-		/* Currently means:  DMA Event Missed, or "null" transfer
-		 * request was seen.  In the future, TC errors (like bad
-		 * addresses) might be presented too.
-		 */
-		dev_warn(mmc_dev(host->mmc), "DMA %s error\n",
-			(host->data->flags & MMC_DATA_WRITE)
-				? "write" : "read");
-		host->data->error = -EIO;
-		mmc_davinci_xfer_done(host, host->data);
-	}
-}
-
-/* Set up tx or rx template, to be modified and updated later */
-static void __init mmc_davinci_dma_setup(struct mmc_davinci_host *host,
-		bool tx, struct edmacc_param *template)
-{
-	unsigned	sync_dev;
-	const u16	acnt = 4;
-	const u16	bcnt = rw_threshold >> 2;
-	const u16	ccnt = 0;
-	u32		src_port = 0;
-	u32		dst_port = 0;
-	s16		src_bidx, dst_bidx;
-	s16		src_cidx, dst_cidx;
-
-	/*
-	 * A-B Sync transfer:  each DMA request is for one "frame" of
-	 * rw_threshold bytes, broken into "acnt"-size chunks repeated
-	 * "bcnt" times.  Each segment needs "ccnt" such frames; since
-	 * we tell the block layer our mmc->max_seg_size limit, we can
-	 * trust (later) that it's within bounds.
-	 *
-	 * The FIFOs are read/written in 4-byte chunks (acnt == 4) and
-	 * EDMA will optimize memory operations to use larger bursts.
-	 */
-	if (tx) {
-		sync_dev = host->txdma;
-
-		/* src_prt, ccnt, and link to be set up later */
-		src_bidx = acnt;
-		src_cidx = acnt * bcnt;
-
-		dst_port = host->mem_res->start + DAVINCI_MMCDXR;
-		dst_bidx = 0;
-		dst_cidx = 0;
-	} else {
-		sync_dev = host->rxdma;
-
-		src_port = host->mem_res->start + DAVINCI_MMCDRR;
-		src_bidx = 0;
-		src_cidx = 0;
-
-		/* dst_prt, ccnt, and link to be set up later */
-		dst_bidx = acnt;
-		dst_cidx = acnt * bcnt;
-	}
-
-	/*
-	 * We can't use FIFO mode for the FIFOs because MMC FIFO addresses
-	 * are not 256-bit (32-byte) aligned.  So we use INCR, and the W8BIT
-	 * parameter is ignored.
-	 */
-	edma_set_src(sync_dev, src_port, INCR, W8BIT);
-	edma_set_dest(sync_dev, dst_port, INCR, W8BIT);
+		sync_dev = host->dma_tx;
 
-	edma_set_src_index(sync_dev, src_bidx, src_cidx);
-	edma_set_dest_index(sync_dev, dst_bidx, dst_cidx);
-
-	edma_set_transfer_params(sync_dev, acnt, bcnt, ccnt, 8, ABSYNC);
-
-	edma_read_slot(sync_dev, template);
-
-	/* don't bother with irqs or chaining */
-	template->opt |= EDMA_CHAN_SLOT(sync_dev) << 12;
+	dmaengine_terminate_all(sync_dev);
 }
 
-static void mmc_davinci_send_dma_request(struct mmc_davinci_host *host,
+static int mmc_davinci_send_dma_request(struct mmc_davinci_host *host,
 		struct mmc_data *data)
 {
-	struct edmacc_param	*template;
-	int			channel, slot;
-	unsigned		link;
-	struct scatterlist	*sg;
-	unsigned		sg_len;
-	unsigned		bytes_left = host->bytes_left;
-	const unsigned		shift = ffs(rw_threshold) - 1;
+	struct dma_chan *chan;
+	struct dma_async_tx_descriptor *desc;
+	int ret = 0;
 
 	if (host->data_dir == DAVINCI_MMC_DATADIR_WRITE) {
-		template = &host->tx_template;
-		channel = host->txdma;
+		struct dma_slave_config dma_tx_conf = {
+			.direction = DMA_MEM_TO_DEV,
+			.dst_addr = host->mem_res->start + DAVINCI_MMCDXR,
+			.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES,
+			.dst_maxburst =
+				rw_threshold / DMA_SLAVE_BUSWIDTH_4_BYTES,
+		};
+		chan = host->dma_tx;
+		dmaengine_slave_config(host->dma_tx, &dma_tx_conf);
+
+		desc = dmaengine_prep_slave_sg(host->dma_tx,
+				data->sg,
+				host->sg_len,
+				DMA_MEM_TO_DEV,
+				DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+		if (!desc) {
+			dev_dbg(mmc_dev(host->mmc),
+				"failed to allocate DMA TX descriptor");
+			ret = -1;
+			goto out;
+		}
 	} else {
-		template = &host->rx_template;
-		channel = host->rxdma;
-	}
-
-	/* We know sg_len and ccnt will never be out of range because
-	 * we told the mmc layer which in turn tells the block layer
-	 * to ensure that it only hands us one scatterlist segment
-	 * per EDMA PARAM entry.  Update the PARAM
-	 * entries needed for each segment of this scatterlist.
-	 */
-	for (slot = channel, link = 0, sg = data->sg, sg_len = host->sg_len;
-			sg_len-- != 0 && bytes_left;
-			sg = sg_next(sg), slot = host->links[link++]) {
-		u32		buf = sg_dma_address(sg);
-		unsigned	count = sg_dma_len(sg);
-
-		template->link_bcntrld = sg_len
-				? (EDMA_CHAN_SLOT(host->links[link]) << 5)
-				: 0xffff;
-
-		if (count > bytes_left)
-			count = bytes_left;
-		bytes_left -= count;
-
-		if (host->data_dir == DAVINCI_MMC_DATADIR_WRITE)
-			template->src = buf;
-		else
-			template->dst = buf;
-		template->ccnt = count >> shift;
-
-		edma_write_slot(slot, template);
+		struct dma_slave_config dma_rx_conf = {
+			.direction = DMA_DEV_TO_MEM,
+			.src_addr = host->mem_res->start + DAVINCI_MMCDRR,
+			.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES,
+			.src_maxburst =
+				rw_threshold / DMA_SLAVE_BUSWIDTH_4_BYTES,
+		};
+		chan = host->dma_rx;
+		dmaengine_slave_config(host->dma_rx, &dma_rx_conf);
+
+		desc = dmaengine_prep_slave_sg(host->dma_rx,
+				data->sg,
+				host->sg_len,
+				DMA_DEV_TO_MEM,
+				DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+		if (!desc) {
+			dev_dbg(mmc_dev(host->mmc),
+				"failed to allocate DMA RX descriptor");
+			ret = -1;
+			goto out;
+		}
 	}
 
-	if (host->version == MMC_CTLR_VERSION_2)
-		edma_clear_event(channel);
+	dmaengine_submit(desc);
+	dma_async_issue_pending(chan);
 
-	edma_start(channel);
+out:
+	return ret;
 }
 
 static int mmc_davinci_start_dma_transfer(struct mmc_davinci_host *host,
@@ -564,6 +478,7 @@ static int mmc_davinci_start_dma_transfer(struct mmc_davinci_host *host,
 {
 	int i;
 	int mask = rw_threshold - 1;
+	int ret = 0;
 
 	host->sg_len = dma_map_sg(mmc_dev(host->mmc), data->sg, data->sg_len,
 				((data->flags & MMC_DATA_WRITE)
@@ -583,70 +498,48 @@ static int mmc_davinci_start_dma_transfer(struct mmc_davinci_host *host,
 	}
 
 	host->do_dma = 1;
-	mmc_davinci_send_dma_request(host, data);
+	ret = mmc_davinci_send_dma_request(host, data);
 
-	return 0;
+	return ret;
 }
 
 static void __init_or_module
 davinci_release_dma_channels(struct mmc_davinci_host *host)
 {
-	unsigned	i;
-
 	if (!host->use_dma)
 		return;
 
-	for (i = 0; i < host->n_link; i++)
-		edma_free_slot(host->links[i]);
-
-	edma_free_channel(host->txdma);
-	edma_free_channel(host->rxdma);
+	dma_release_channel(host->dma_tx);
+	dma_release_channel(host->dma_rx);
 }
 
 static int __init davinci_acquire_dma_channels(struct mmc_davinci_host *host)
 {
-	u32 link_size;
-	int r, i;
-
-	/* Acquire master DMA write channel */
-	r = edma_alloc_channel(host->txdma, mmc_davinci_dma_cb, host,
-			EVENTQ_DEFAULT);
-	if (r < 0) {
-		dev_warn(mmc_dev(host->mmc), "alloc %s channel err %d\n",
-				"tx", r);
-		return r;
-	}
-	mmc_davinci_dma_setup(host, true, &host->tx_template);
-
-	/* Acquire master DMA read channel */
-	r = edma_alloc_channel(host->rxdma, mmc_davinci_dma_cb, host,
-			EVENTQ_DEFAULT);
-	if (r < 0) {
-		dev_warn(mmc_dev(host->mmc), "alloc %s channel err %d\n",
-				"rx", r);
-		goto free_master_write;
+	int r;
+	dma_cap_mask_t mask;
+
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+
+	host->dma_tx =
+		dma_request_channel(mask, edma_filter_fn, &host->txdma);
+	if (!host->dma_tx) {
+		dev_err(mmc_dev(host->mmc), "Can't get dma_tx channel\n");
+		return -ENODEV;
 	}
-	mmc_davinci_dma_setup(host, false, &host->rx_template);
 
-	/* Allocate parameter RAM slots, which will later be bound to a
-	 * channel as needed to handle a scatterlist.
-	 */
-	link_size = min_t(unsigned, host->nr_sg, ARRAY_SIZE(host->links));
-	for (i = 0; i < link_size; i++) {
-		r = edma_alloc_slot(EDMA_CTLR(host->txdma), EDMA_SLOT_ANY);
-		if (r < 0) {
-			dev_dbg(mmc_dev(host->mmc), "dma PaRAM alloc --> %d\n",
-				r);
-			break;
-		}
-		host->links[i] = r;
+	host->dma_rx =
+		dma_request_channel(mask, edma_filter_fn, &host->rxdma);
+	if (!host->dma_rx) {
+		dev_err(mmc_dev(host->mmc), "Can't get dma_rx channel\n");
+		r = -ENODEV;
+		goto free_master_write;
 	}
-	host->n_link = i;
 
 	return 0;
 
 free_master_write:
-	edma_free_channel(host->txdma);
+	dma_release_channel(host->dma_tx);
 
 	return r;
 }
@@ -1359,7 +1252,7 @@ static int __init davinci_mmcsd_probe(struct platform_device *pdev)
 	 * Each hw_seg uses one EDMA parameter RAM slot, always one
 	 * channel and then usually some linked slots.
 	 */
-	mmc->max_segs		= 1 + host->n_link;
+	mmc->max_segs		= MAX_NR_SG;
 
 	/* EDMA limit per hw segment (one or two MBytes) */
 	mmc->max_seg_size	= MAX_CCNT * rw_threshold;
diff --git a/drivers/mmc/host/dw_mmc-exynos.c b/drivers/mmc/host/dw_mmc-exynos.c
new file mode 100644
index 00000000000..660bbc52886
--- /dev/null
+++ b/drivers/mmc/host/dw_mmc-exynos.c
@@ -0,0 +1,253 @@
+/*
+ * Exynos Specific Extensions for Synopsys DW Multimedia Card Interface driver
+ *
+ * Copyright (C) 2012, Samsung Electronics Co., 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.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/dw_mmc.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+
+#include "dw_mmc.h"
+#include "dw_mmc-pltfm.h"
+
+#define NUM_PINS(x)			(x + 2)
+
+#define SDMMC_CLKSEL			0x09C
+#define SDMMC_CLKSEL_CCLK_SAMPLE(x)	(((x) & 7) << 0)
+#define SDMMC_CLKSEL_CCLK_DRIVE(x)	(((x) & 7) << 16)
+#define SDMMC_CLKSEL_CCLK_DIVIDER(x)	(((x) & 7) << 24)
+#define SDMMC_CLKSEL_GET_DRV_WD3(x)	(((x) >> 16) & 0x7)
+#define SDMMC_CLKSEL_TIMING(x, y, z)	(SDMMC_CLKSEL_CCLK_SAMPLE(x) |	\
+					SDMMC_CLKSEL_CCLK_DRIVE(y) |	\
+					SDMMC_CLKSEL_CCLK_DIVIDER(z))
+
+#define SDMMC_CMD_USE_HOLD_REG		BIT(29)
+
+#define EXYNOS4210_FIXED_CIU_CLK_DIV	2
+#define EXYNOS4412_FIXED_CIU_CLK_DIV	4
+
+/* Variations in Exynos specific dw-mshc controller */
+enum dw_mci_exynos_type {
+	DW_MCI_TYPE_EXYNOS4210,
+	DW_MCI_TYPE_EXYNOS4412,
+	DW_MCI_TYPE_EXYNOS5250,
+};
+
+/* Exynos implementation specific driver private data */
+struct dw_mci_exynos_priv_data {
+	enum dw_mci_exynos_type		ctrl_type;
+	u8				ciu_div;
+	u32				sdr_timing;
+	u32				ddr_timing;
+};
+
+static struct dw_mci_exynos_compatible {
+	char				*compatible;
+	enum dw_mci_exynos_type		ctrl_type;
+} exynos_compat[] = {
+	{
+		.compatible	= "samsung,exynos4210-dw-mshc",
+		.ctrl_type	= DW_MCI_TYPE_EXYNOS4210,
+	}, {
+		.compatible	= "samsung,exynos4412-dw-mshc",
+		.ctrl_type	= DW_MCI_TYPE_EXYNOS4412,
+	}, {
+		.compatible	= "samsung,exynos5250-dw-mshc",
+		.ctrl_type	= DW_MCI_TYPE_EXYNOS5250,
+	},
+};
+
+static int dw_mci_exynos_priv_init(struct dw_mci *host)
+{
+	struct dw_mci_exynos_priv_data *priv;
+	int idx;
+
+	priv = devm_kzalloc(host->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv) {
+		dev_err(host->dev, "mem alloc failed for private data\n");
+		return -ENOMEM;
+	}
+
+	for (idx = 0; idx < ARRAY_SIZE(exynos_compat); idx++) {
+		if (of_device_is_compatible(host->dev->of_node,
+					exynos_compat[idx].compatible))
+			priv->ctrl_type = exynos_compat[idx].ctrl_type;
+	}
+
+	host->priv = priv;
+	return 0;
+}
+
+static int dw_mci_exynos_setup_clock(struct dw_mci *host)
+{
+	struct dw_mci_exynos_priv_data *priv = host->priv;
+
+	if (priv->ctrl_type == DW_MCI_TYPE_EXYNOS5250)
+		host->bus_hz /= (priv->ciu_div + 1);
+	else if (priv->ctrl_type == DW_MCI_TYPE_EXYNOS4412)
+		host->bus_hz /= EXYNOS4412_FIXED_CIU_CLK_DIV;
+	else if (priv->ctrl_type == DW_MCI_TYPE_EXYNOS4210)
+		host->bus_hz /= EXYNOS4210_FIXED_CIU_CLK_DIV;
+
+	return 0;
+}
+
+static void dw_mci_exynos_prepare_command(struct dw_mci *host, u32 *cmdr)
+{
+	/*
+	 * Exynos4412 and Exynos5250 extends the use of CMD register with the
+	 * use of bit 29 (which is reserved on standard MSHC controllers) for
+	 * optionally bypassing the HOLD register for command and data. The
+	 * HOLD register should be bypassed in case there is no phase shift
+	 * applied on CMD/DATA that is sent to the card.
+	 */
+	if (SDMMC_CLKSEL_GET_DRV_WD3(mci_readl(host, CLKSEL)))
+		*cmdr |= SDMMC_CMD_USE_HOLD_REG;
+}
+
+static void dw_mci_exynos_set_ios(struct dw_mci *host, struct mmc_ios *ios)
+{
+	struct dw_mci_exynos_priv_data *priv = host->priv;
+
+	if (ios->timing == MMC_TIMING_UHS_DDR50)
+		mci_writel(host, CLKSEL, priv->ddr_timing);
+	else
+		mci_writel(host, CLKSEL, priv->sdr_timing);
+}
+
+static int dw_mci_exynos_parse_dt(struct dw_mci *host)
+{
+	struct dw_mci_exynos_priv_data *priv = host->priv;
+	struct device_node *np = host->dev->of_node;
+	u32 timing[2];
+	u32 div = 0;
+	int ret;
+
+	of_property_read_u32(np, "samsung,dw-mshc-ciu-div", &div);
+	priv->ciu_div = div;
+
+	ret = of_property_read_u32_array(np,
+			"samsung,dw-mshc-sdr-timing", timing, 2);
+	if (ret)
+		return ret;
+
+	priv->sdr_timing = SDMMC_CLKSEL_TIMING(timing[0], timing[1], div);
+
+	ret = of_property_read_u32_array(np,
+			"samsung,dw-mshc-ddr-timing", timing, 2);
+	if (ret)
+		return ret;
+
+	priv->ddr_timing = SDMMC_CLKSEL_TIMING(timing[0], timing[1], div);
+	return 0;
+}
+
+static int dw_mci_exynos_setup_bus(struct dw_mci *host,
+				struct device_node *slot_np, u8 bus_width)
+{
+	int idx, gpio, ret;
+
+	if (!slot_np)
+		return -EINVAL;
+
+	/* cmd + clock + bus-width pins */
+	for (idx = 0; idx < NUM_PINS(bus_width); idx++) {
+		gpio = of_get_gpio(slot_np, idx);
+		if (!gpio_is_valid(gpio)) {
+			dev_err(host->dev, "invalid gpio: %d\n", gpio);
+			return -EINVAL;
+		}
+
+		ret = devm_gpio_request(host->dev, gpio, "dw-mci-bus");
+		if (ret) {
+			dev_err(host->dev, "gpio [%d] request failed\n", gpio);
+			return -EBUSY;
+		}
+	}
+
+	gpio = of_get_named_gpio(slot_np, "wp-gpios", 0);
+	if (gpio_is_valid(gpio)) {
+		if (devm_gpio_request(host->dev, gpio, "dw-mci-wp"))
+			dev_info(host->dev, "gpio [%d] request failed\n",
+						gpio);
+	} else {
+		dev_info(host->dev, "wp gpio not available");
+		host->pdata->quirks |= DW_MCI_QUIRK_NO_WRITE_PROTECT;
+	}
+
+	if (host->pdata->quirks & DW_MCI_QUIRK_BROKEN_CARD_DETECTION)
+		return 0;
+
+	gpio = of_get_named_gpio(slot_np, "samsung,cd-pinmux-gpio", 0);
+	if (gpio_is_valid(gpio)) {
+		if (devm_gpio_request(host->dev, gpio, "dw-mci-cd"))
+			dev_err(host->dev, "gpio [%d] request failed\n", gpio);
+	} else {
+		dev_info(host->dev, "cd gpio not available");
+	}
+
+	return 0;
+}
+
+/* Exynos5250 controller specific capabilities */
+static unsigned long exynos5250_dwmmc_caps[4] = {
+	MMC_CAP_UHS_DDR50 | MMC_CAP_1_8V_DDR |
+		MMC_CAP_8_BIT_DATA | MMC_CAP_CMD23,
+	MMC_CAP_CMD23,
+	MMC_CAP_CMD23,
+	MMC_CAP_CMD23,
+};
+
+static struct dw_mci_drv_data exynos5250_drv_data = {
+	.caps			= exynos5250_dwmmc_caps,
+	.init			= dw_mci_exynos_priv_init,
+	.setup_clock		= dw_mci_exynos_setup_clock,
+	.prepare_command	= dw_mci_exynos_prepare_command,
+	.set_ios		= dw_mci_exynos_set_ios,
+	.parse_dt		= dw_mci_exynos_parse_dt,
+	.setup_bus		= dw_mci_exynos_setup_bus,
+};
+
+static const struct of_device_id dw_mci_exynos_match[] = {
+	{ .compatible = "samsung,exynos5250-dw-mshc",
+			.data = (void *)&exynos5250_drv_data, },
+	{},
+};
+MODULE_DEVICE_TABLE(of, dw_mci_pltfm_match);
+
+int dw_mci_exynos_probe(struct platform_device *pdev)
+{
+	struct dw_mci_drv_data *drv_data;
+	const struct of_device_id *match;
+
+	match = of_match_node(dw_mci_exynos_match, pdev->dev.of_node);
+	drv_data = match->data;
+	return dw_mci_pltfm_register(pdev, drv_data);
+}
+
+static struct platform_driver dw_mci_exynos_pltfm_driver = {
+	.probe		= dw_mci_exynos_probe,
+	.remove		= __exit_p(dw_mci_pltfm_remove),
+	.driver		= {
+		.name		= "dwmmc_exynos",
+		.of_match_table	= of_match_ptr(dw_mci_exynos_match),
+		.pm		= &dw_mci_pltfm_pmops,
+	},
+};
+
+module_platform_driver(dw_mci_exynos_pltfm_driver);
+
+MODULE_DESCRIPTION("Samsung Specific DW-MSHC Driver Extension");
+MODULE_AUTHOR("Thomas Abraham <thomas.ab@samsung.com");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:dwmmc-exynos");
diff --git a/drivers/mmc/host/dw_mmc-pci.c b/drivers/mmc/host/dw_mmc-pci.c
index dc0d25a013e..edb37e9135a 100644
--- a/drivers/mmc/host/dw_mmc-pci.c
+++ b/drivers/mmc/host/dw_mmc-pci.c
@@ -59,7 +59,7 @@ static int __devinit dw_mci_pci_probe(struct pci_dev *pdev,
 
 	host->irq = pdev->irq;
 	host->irq_flags = IRQF_SHARED;
-	host->dev = pdev->dev;
+	host->dev = &pdev->dev;
 	host->pdata = &pci_board_data;
 
 	host->regs = pci_iomap(pdev, PCI_BAR_NO, COMPLETE_BAR);
@@ -140,18 +140,7 @@ static struct pci_driver dw_mci_pci_driver = {
 	},
 };
 
-static int __init dw_mci_init(void)
-{
-	return pci_register_driver(&dw_mci_pci_driver);
-}
-
-static void __exit dw_mci_exit(void)
-{
-	pci_unregister_driver(&dw_mci_pci_driver);
-}
-
-module_init(dw_mci_init);
-module_exit(dw_mci_exit);
+module_pci_driver(dw_mci_pci_driver);
 
 MODULE_DESCRIPTION("DW Multimedia Card PCI Interface driver");
 MODULE_AUTHOR("Shashidhar Hiremath <shashidharh@vayavyalabs.com>");
diff --git a/drivers/mmc/host/dw_mmc-pltfm.c b/drivers/mmc/host/dw_mmc-pltfm.c
index 92ec3eb3aae..c960ca7ffbe 100644
--- a/drivers/mmc/host/dw_mmc-pltfm.c
+++ b/drivers/mmc/host/dw_mmc-pltfm.c
@@ -19,59 +19,63 @@
 #include <linux/mmc/host.h>
 #include <linux/mmc/mmc.h>
 #include <linux/mmc/dw_mmc.h>
+#include <linux/of.h>
+
 #include "dw_mmc.h"
 
-static int dw_mci_pltfm_probe(struct platform_device *pdev)
+int dw_mci_pltfm_register(struct platform_device *pdev,
+				struct dw_mci_drv_data *drv_data)
 {
 	struct dw_mci *host;
 	struct resource	*regs;
 	int ret;
 
-	host = kzalloc(sizeof(struct dw_mci), GFP_KERNEL);
+	host = devm_kzalloc(&pdev->dev, sizeof(struct dw_mci), GFP_KERNEL);
 	if (!host)
 		return -ENOMEM;
 
 	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!regs) {
-		ret = -ENXIO;
-		goto err_free;
-	}
+	if (!regs)
+		return -ENXIO;
 
 	host->irq = platform_get_irq(pdev, 0);
-	if (host->irq < 0) {
-		ret = host->irq;
-		goto err_free;
-	}
+	if (host->irq < 0)
+		return host->irq;
 
-	host->dev = pdev->dev;
+	host->drv_data = drv_data;
+	host->dev = &pdev->dev;
 	host->irq_flags = 0;
 	host->pdata = pdev->dev.platform_data;
-	ret = -ENOMEM;
-	host->regs = ioremap(regs->start, resource_size(regs));
+	host->regs = devm_request_and_ioremap(&pdev->dev, regs);
 	if (!host->regs)
-		goto err_free;
+		return -ENOMEM;
+
+	if (host->drv_data->init) {
+		ret = host->drv_data->init(host);
+		if (ret)
+			return ret;
+	}
+
 	platform_set_drvdata(pdev, host);
 	ret = dw_mci_probe(host);
-	if (ret)
-		goto err_out;
-	return ret;
-err_out:
-	iounmap(host->regs);
-err_free:
-	kfree(host);
 	return ret;
 }
+EXPORT_SYMBOL_GPL(dw_mci_pltfm_register);
 
-static int __exit dw_mci_pltfm_remove(struct platform_device *pdev)
+static int __devinit dw_mci_pltfm_probe(struct platform_device *pdev)
+{
+	return dw_mci_pltfm_register(pdev, NULL);
+}
+
+static int __devexit dw_mci_pltfm_remove(struct platform_device *pdev)
 {
 	struct dw_mci *host = platform_get_drvdata(pdev);
 
 	platform_set_drvdata(pdev, NULL);
 	dw_mci_remove(host);
-	iounmap(host->regs);
-	kfree(host);
 	return 0;
 }
+EXPORT_SYMBOL_GPL(dw_mci_pltfm_remove);
 
 #ifdef CONFIG_PM_SLEEP
 /*
@@ -105,12 +109,20 @@ static int dw_mci_pltfm_resume(struct device *dev)
 #define dw_mci_pltfm_resume	NULL
 #endif /* CONFIG_PM_SLEEP */
 
-static SIMPLE_DEV_PM_OPS(dw_mci_pltfm_pmops, dw_mci_pltfm_suspend, dw_mci_pltfm_resume);
+SIMPLE_DEV_PM_OPS(dw_mci_pltfm_pmops, dw_mci_pltfm_suspend, dw_mci_pltfm_resume);
+EXPORT_SYMBOL_GPL(dw_mci_pltfm_pmops);
+
+static const struct of_device_id dw_mci_pltfm_match[] = {
+	{ .compatible = "snps,dw-mshc", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, dw_mci_pltfm_match);
 
 static struct platform_driver dw_mci_pltfm_driver = {
 	.remove		= __exit_p(dw_mci_pltfm_remove),
 	.driver		= {
 		.name		= "dw_mmc",
+		.of_match_table	= of_match_ptr(dw_mci_pltfm_match),
 		.pm		= &dw_mci_pltfm_pmops,
 	},
 };
diff --git a/drivers/mmc/host/dw_mmc-pltfm.h b/drivers/mmc/host/dw_mmc-pltfm.h
new file mode 100644
index 00000000000..301f24541fc
--- /dev/null
+++ b/drivers/mmc/host/dw_mmc-pltfm.h
@@ -0,0 +1,20 @@
+/*
+ * Synopsys DesignWare Multimedia Card Interface Platform driver
+ *
+ * Copyright (C) 2012, Samsung Electronics Co., 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.
+ */
+
+#ifndef _DW_MMC_PLTFM_H_
+#define _DW_MMC_PLTFM_H_
+
+extern int dw_mci_pltfm_register(struct platform_device *pdev,
+				struct dw_mci_drv_data *drv_data);
+extern int __devexit dw_mci_pltfm_remove(struct platform_device *pdev);
+extern const struct dev_pm_ops dw_mci_pltfm_pmops;
+
+#endif /* _DW_MMC_PLTFM_H_ */
diff --git a/drivers/mmc/host/dw_mmc.c b/drivers/mmc/host/dw_mmc.c
index af40d227bec..c2828f35c3b 100644
--- a/drivers/mmc/host/dw_mmc.c
+++ b/drivers/mmc/host/dw_mmc.c
@@ -33,6 +33,7 @@
 #include <linux/bitops.h>
 #include <linux/regulator/consumer.h>
 #include <linux/workqueue.h>
+#include <linux/of.h>
 
 #include "dw_mmc.h"
 
@@ -230,6 +231,7 @@ static void dw_mci_set_timeout(struct dw_mci *host)
 static u32 dw_mci_prepare_command(struct mmc_host *mmc, struct mmc_command *cmd)
 {
 	struct mmc_data	*data;
+	struct dw_mci_slot *slot = mmc_priv(mmc);
 	u32 cmdr;
 	cmd->error = -EINPROGRESS;
 
@@ -259,6 +261,9 @@ static u32 dw_mci_prepare_command(struct mmc_host *mmc, struct mmc_command *cmd)
 			cmdr |= SDMMC_CMD_DAT_WR;
 	}
 
+	if (slot->host->drv_data->prepare_command)
+		slot->host->drv_data->prepare_command(slot->host, &cmdr);
+
 	return cmdr;
 }
 
@@ -266,7 +271,7 @@ static void dw_mci_start_command(struct dw_mci *host,
 				 struct mmc_command *cmd, u32 cmd_flags)
 {
 	host->cmd = cmd;
-	dev_vdbg(&host->dev,
+	dev_vdbg(host->dev,
 		 "start command: ARGR=0x%08x CMDR=0x%08x\n",
 		 cmd->arg, cmd_flags);
 
@@ -308,7 +313,7 @@ static void dw_mci_dma_cleanup(struct dw_mci *host)
 
 	if (data)
 		if (!data->host_cookie)
-			dma_unmap_sg(&host->dev,
+			dma_unmap_sg(host->dev,
 				     data->sg,
 				     data->sg_len,
 				     dw_mci_get_dma_dir(data));
@@ -334,7 +339,7 @@ static void dw_mci_idmac_complete_dma(struct dw_mci *host)
 {
 	struct mmc_data *data = host->data;
 
-	dev_vdbg(&host->dev, "DMA complete\n");
+	dev_vdbg(host->dev, "DMA complete\n");
 
 	host->dma_ops->cleanup(host);
 
@@ -405,23 +410,11 @@ static void dw_mci_idmac_start_dma(struct dw_mci *host, unsigned int sg_len)
 static int dw_mci_idmac_init(struct dw_mci *host)
 {
 	struct idmac_desc *p;
-	int i, dma_support;
+	int i;
 
 	/* Number of descriptors in the ring buffer */
 	host->ring_size = PAGE_SIZE / sizeof(struct idmac_desc);
 
-	/* Check if Hardware Configuration Register has support for DMA */
-	dma_support = (mci_readl(host, HCON) >> 16) & 0x3;
-
-	if (!dma_support || dma_support > 2) {
-		dev_err(&host->dev,
-			"Host Controller does not support IDMA Tx.\n");
-		host->dma_ops = NULL;
-		return -ENODEV;
-	}
-
-	dev_info(&host->dev, "Using internal DMA controller.\n");
-
 	/* Forward link the descriptor list */
 	for (i = 0, p = host->sg_cpu; i < host->ring_size - 1; i++, p++)
 		p->des3 = host->sg_dma + (sizeof(struct idmac_desc) * (i + 1));
@@ -476,7 +469,7 @@ static int dw_mci_pre_dma_transfer(struct dw_mci *host,
 			return -EINVAL;
 	}
 
-	sg_len = dma_map_sg(&host->dev,
+	sg_len = dma_map_sg(host->dev,
 			    data->sg,
 			    data->sg_len,
 			    dw_mci_get_dma_dir(data));
@@ -519,7 +512,7 @@ static void dw_mci_post_req(struct mmc_host *mmc,
 		return;
 
 	if (data->host_cookie)
-		dma_unmap_sg(&slot->host->dev,
+		dma_unmap_sg(slot->host->dev,
 			     data->sg,
 			     data->sg_len,
 			     dw_mci_get_dma_dir(data));
@@ -545,7 +538,7 @@ static int dw_mci_submit_data_dma(struct dw_mci *host, struct mmc_data *data)
 
 	host->using_dma = 1;
 
-	dev_vdbg(&host->dev,
+	dev_vdbg(host->dev,
 		 "sd sg_cpu: %#lx sg_dma: %#lx sg_len: %d\n",
 		 (unsigned long)host->sg_cpu, (unsigned long)host->sg_dma,
 		 sg_len);
@@ -814,6 +807,9 @@ static void dw_mci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		slot->clock = ios->clock;
 	}
 
+	if (slot->host->drv_data->set_ios)
+		slot->host->drv_data->set_ios(slot->host, ios);
+
 	switch (ios->power_mode) {
 	case MMC_POWER_UP:
 		set_bit(DW_MMC_CARD_NEED_INIT, &slot->flags);
@@ -830,7 +826,9 @@ static int dw_mci_get_ro(struct mmc_host *mmc)
 	struct dw_mci_board *brd = slot->host->pdata;
 
 	/* Use platform get_ro function, else try on board write protect */
-	if (brd->get_ro)
+	if (brd->quirks & DW_MCI_QUIRK_NO_WRITE_PROTECT)
+		read_only = 0;
+	else if (brd->get_ro)
 		read_only = brd->get_ro(slot->id);
 	else
 		read_only =
@@ -939,12 +937,12 @@ static void dw_mci_request_end(struct dw_mci *host, struct mmc_request *mrq)
 		slot = list_entry(host->queue.next,
 				  struct dw_mci_slot, queue_node);
 		list_del(&slot->queue_node);
-		dev_vdbg(&host->dev, "list not empty: %s is next\n",
+		dev_vdbg(host->dev, "list not empty: %s is next\n",
 			 mmc_hostname(slot->mmc));
 		host->state = STATE_SENDING_CMD;
 		dw_mci_start_request(host, slot);
 	} else {
-		dev_vdbg(&host->dev, "list empty\n");
+		dev_vdbg(host->dev, "list empty\n");
 		host->state = STATE_IDLE;
 	}
 
@@ -1083,7 +1081,7 @@ static void dw_mci_tasklet_func(unsigned long priv)
 					data->bytes_xfered = 0;
 					data->error = -ETIMEDOUT;
 				} else {
-					dev_err(&host->dev,
+					dev_err(host->dev,
 						"data FIFO error "
 						"(status=%08x)\n",
 						status);
@@ -1767,12 +1765,60 @@ static void dw_mci_work_routine_card(struct work_struct *work)
 	}
 }
 
-static int __init dw_mci_init_slot(struct dw_mci *host, unsigned int id)
+#ifdef CONFIG_OF
+/* given a slot id, find out the device node representing that slot */
+static struct device_node *dw_mci_of_find_slot_node(struct device *dev, u8 slot)
+{
+	struct device_node *np;
+	const __be32 *addr;
+	int len;
+
+	if (!dev || !dev->of_node)
+		return NULL;
+
+	for_each_child_of_node(dev->of_node, np) {
+		addr = of_get_property(np, "reg", &len);
+		if (!addr || (len < sizeof(int)))
+			continue;
+		if (be32_to_cpup(addr) == slot)
+			return np;
+	}
+	return NULL;
+}
+
+/* find out bus-width for a given slot */
+static u32 dw_mci_of_get_bus_wd(struct device *dev, u8 slot)
+{
+	struct device_node *np = dw_mci_of_find_slot_node(dev, slot);
+	u32 bus_wd = 1;
+
+	if (!np)
+		return 1;
+
+	if (of_property_read_u32(np, "bus-width", &bus_wd))
+		dev_err(dev, "bus-width property not found, assuming width"
+			       " as 1\n");
+	return bus_wd;
+}
+#else /* CONFIG_OF */
+static u32 dw_mci_of_get_bus_wd(struct device *dev, u8 slot)
+{
+	return 1;
+}
+static struct device_node *dw_mci_of_find_slot_node(struct device *dev, u8 slot)
+{
+	return NULL;
+}
+#endif /* CONFIG_OF */
+
+static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 {
 	struct mmc_host *mmc;
 	struct dw_mci_slot *slot;
+	int ctrl_id, ret;
+	u8 bus_width;
 
-	mmc = mmc_alloc_host(sizeof(struct dw_mci_slot), &host->dev);
+	mmc = mmc_alloc_host(sizeof(struct dw_mci_slot), host->dev);
 	if (!mmc)
 		return -ENOMEM;
 
@@ -1780,6 +1826,7 @@ static int __init dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 	slot->id = id;
 	slot->mmc = mmc;
 	slot->host = host;
+	host->slot[id] = slot;
 
 	mmc->ops = &dw_mci_ops;
 	mmc->f_min = DIV_ROUND_UP(host->bus_hz, 510);
@@ -1800,21 +1847,44 @@ static int __init dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 	if (host->pdata->caps)
 		mmc->caps = host->pdata->caps;
 
+	if (host->dev->of_node) {
+		ctrl_id = of_alias_get_id(host->dev->of_node, "mshc");
+		if (ctrl_id < 0)
+			ctrl_id = 0;
+	} else {
+		ctrl_id = to_platform_device(host->dev)->id;
+	}
+	if (host->drv_data && host->drv_data->caps)
+		mmc->caps |= host->drv_data->caps[ctrl_id];
+
 	if (host->pdata->caps2)
 		mmc->caps2 = host->pdata->caps2;
 
 	if (host->pdata->get_bus_wd)
-		if (host->pdata->get_bus_wd(slot->id) >= 4)
-			mmc->caps |= MMC_CAP_4_BIT_DATA;
+		bus_width = host->pdata->get_bus_wd(slot->id);
+	else if (host->dev->of_node)
+		bus_width = dw_mci_of_get_bus_wd(host->dev, slot->id);
+	else
+		bus_width = 1;
+
+	if (host->drv_data->setup_bus) {
+		struct device_node *slot_np;
+		slot_np = dw_mci_of_find_slot_node(host->dev, slot->id);
+		ret = host->drv_data->setup_bus(host, slot_np, bus_width);
+		if (ret)
+			goto err_setup_bus;
+	}
+
+	switch (bus_width) {
+	case 8:
+		mmc->caps |= MMC_CAP_8_BIT_DATA;
+	case 4:
+		mmc->caps |= MMC_CAP_4_BIT_DATA;
+	}
 
 	if (host->pdata->quirks & DW_MCI_QUIRK_HIGHSPEED)
 		mmc->caps |= MMC_CAP_SD_HIGHSPEED | MMC_CAP_MMC_HIGHSPEED;
 
-	if (mmc->caps2 & MMC_CAP2_POWEROFF_NOTIFY)
-		mmc->power_notify_type = MMC_HOST_PW_NOTIFY_SHORT;
-	else
-		mmc->power_notify_type = MMC_HOST_PW_NOTIFY_NONE;
-
 	if (host->pdata->blk_settings) {
 		mmc->max_segs = host->pdata->blk_settings->max_segs;
 		mmc->max_blk_size = host->pdata->blk_settings->max_blk_size;
@@ -1850,7 +1920,6 @@ static int __init dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 	else
 		clear_bit(DW_MMC_CARD_PRESENT, &slot->flags);
 
-	host->slot[id] = slot;
 	mmc_add_host(mmc);
 
 #if defined(CONFIG_DEBUG_FS)
@@ -1867,6 +1936,10 @@ static int __init dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 	queue_work(host->card_workqueue, &host->card_work);
 
 	return 0;
+
+err_setup_bus:
+	mmc_free_host(mmc);
+	return -EINVAL;
 }
 
 static void dw_mci_cleanup_slot(struct dw_mci_slot *slot, unsigned int id)
@@ -1884,10 +1957,10 @@ static void dw_mci_cleanup_slot(struct dw_mci_slot *slot, unsigned int id)
 static void dw_mci_init_dma(struct dw_mci *host)
 {
 	/* Alloc memory for sg translation */
-	host->sg_cpu = dma_alloc_coherent(&host->dev, PAGE_SIZE,
+	host->sg_cpu = dma_alloc_coherent(host->dev, PAGE_SIZE,
 					  &host->sg_dma, GFP_KERNEL);
 	if (!host->sg_cpu) {
-		dev_err(&host->dev, "%s: could not alloc DMA memory\n",
+		dev_err(host->dev, "%s: could not alloc DMA memory\n",
 			__func__);
 		goto no_dma;
 	}
@@ -1895,6 +1968,7 @@ static void dw_mci_init_dma(struct dw_mci *host)
 	/* Determine which DMA interface to use */
 #ifdef CONFIG_MMC_DW_IDMAC
 	host->dma_ops = &dw_mci_idmac_ops;
+	dev_info(&host->dev, "Using internal DMA controller.\n");
 #endif
 
 	if (!host->dma_ops)
@@ -1903,12 +1977,12 @@ static void dw_mci_init_dma(struct dw_mci *host)
 	if (host->dma_ops->init && host->dma_ops->start &&
 	    host->dma_ops->stop && host->dma_ops->cleanup) {
 		if (host->dma_ops->init(host)) {
-			dev_err(&host->dev, "%s: Unable to initialize "
+			dev_err(host->dev, "%s: Unable to initialize "
 				"DMA Controller.\n", __func__);
 			goto no_dma;
 		}
 	} else {
-		dev_err(&host->dev, "DMA initialization not found.\n");
+		dev_err(host->dev, "DMA initialization not found.\n");
 		goto no_dma;
 	}
 
@@ -1916,7 +1990,7 @@ static void dw_mci_init_dma(struct dw_mci *host)
 	return;
 
 no_dma:
-	dev_info(&host->dev, "Using PIO mode.\n");
+	dev_info(host->dev, "Using PIO mode.\n");
 	host->use_dma = 0;
 	return;
 }
@@ -1942,30 +2016,133 @@ static bool mci_wait_reset(struct device *dev, struct dw_mci *host)
 	return false;
 }
 
+#ifdef CONFIG_OF
+static struct dw_mci_of_quirks {
+	char *quirk;
+	int id;
+} of_quirks[] = {
+	{
+		.quirk	= "supports-highspeed",
+		.id	= DW_MCI_QUIRK_HIGHSPEED,
+	}, {
+		.quirk	= "broken-cd",
+		.id	= DW_MCI_QUIRK_BROKEN_CARD_DETECTION,
+	},
+};
+
+static struct dw_mci_board *dw_mci_parse_dt(struct dw_mci *host)
+{
+	struct dw_mci_board *pdata;
+	struct device *dev = host->dev;
+	struct device_node *np = dev->of_node;
+	int idx, ret;
+
+	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata) {
+		dev_err(dev, "could not allocate memory for pdata\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	/* find out number of slots supported */
+	if (of_property_read_u32(dev->of_node, "num-slots",
+				&pdata->num_slots)) {
+		dev_info(dev, "num-slots property not found, "
+				"assuming 1 slot is available\n");
+		pdata->num_slots = 1;
+	}
+
+	/* get quirks */
+	for (idx = 0; idx < ARRAY_SIZE(of_quirks); idx++)
+		if (of_get_property(np, of_quirks[idx].quirk, NULL))
+			pdata->quirks |= of_quirks[idx].id;
+
+	if (of_property_read_u32(np, "fifo-depth", &pdata->fifo_depth))
+		dev_info(dev, "fifo-depth property not found, using "
+				"value of FIFOTH register as default\n");
+
+	of_property_read_u32(np, "card-detect-delay", &pdata->detect_delay_ms);
+
+	if (host->drv_data->parse_dt) {
+		ret = host->drv_data->parse_dt(host);
+		if (ret)
+			return ERR_PTR(ret);
+	}
+
+	return pdata;
+}
+
+#else /* CONFIG_OF */
+static struct dw_mci_board *dw_mci_parse_dt(struct dw_mci *host)
+{
+	return ERR_PTR(-EINVAL);
+}
+#endif /* CONFIG_OF */
+
 int dw_mci_probe(struct dw_mci *host)
 {
 	int width, i, ret = 0;
 	u32 fifo_size;
+	int init_slots = 0;
 
-	if (!host->pdata || !host->pdata->init) {
-		dev_err(&host->dev,
-			"Platform data must supply init function\n");
-		return -ENODEV;
+	if (!host->pdata) {
+		host->pdata = dw_mci_parse_dt(host);
+		if (IS_ERR(host->pdata)) {
+			dev_err(host->dev, "platform data not available\n");
+			return -EINVAL;
+		}
 	}
 
 	if (!host->pdata->select_slot && host->pdata->num_slots > 1) {
-		dev_err(&host->dev,
+		dev_err(host->dev,
 			"Platform data must supply select_slot function\n");
 		return -ENODEV;
 	}
 
-	if (!host->pdata->bus_hz) {
-		dev_err(&host->dev,
+	host->biu_clk = clk_get(host->dev, "biu");
+	if (IS_ERR(host->biu_clk)) {
+		dev_dbg(host->dev, "biu clock not available\n");
+	} else {
+		ret = clk_prepare_enable(host->biu_clk);
+		if (ret) {
+			dev_err(host->dev, "failed to enable biu clock\n");
+			clk_put(host->biu_clk);
+			return ret;
+		}
+	}
+
+	host->ciu_clk = clk_get(host->dev, "ciu");
+	if (IS_ERR(host->ciu_clk)) {
+		dev_dbg(host->dev, "ciu clock not available\n");
+	} else {
+		ret = clk_prepare_enable(host->ciu_clk);
+		if (ret) {
+			dev_err(host->dev, "failed to enable ciu clock\n");
+			clk_put(host->ciu_clk);
+			goto err_clk_biu;
+		}
+	}
+
+	if (IS_ERR(host->ciu_clk))
+		host->bus_hz = host->pdata->bus_hz;
+	else
+		host->bus_hz = clk_get_rate(host->ciu_clk);
+
+	if (host->drv_data->setup_clock) {
+		ret = host->drv_data->setup_clock(host);
+		if (ret) {
+			dev_err(host->dev,
+				"implementation specific clock setup failed\n");
+			goto err_clk_ciu;
+		}
+	}
+
+	if (!host->bus_hz) {
+		dev_err(host->dev,
 			"Platform data must supply bus speed\n");
-		return -ENODEV;
+		ret = -ENODEV;
+		goto err_clk_ciu;
 	}
 
-	host->bus_hz = host->pdata->bus_hz;
 	host->quirks = host->pdata->quirks;
 
 	spin_lock_init(&host->lock);
@@ -1998,7 +2175,7 @@ int dw_mci_probe(struct dw_mci *host)
 	}
 
 	/* Reset all blocks */
-	if (!mci_wait_reset(&host->dev, host))
+	if (!mci_wait_reset(host->dev, host))
 		return -ENODEV;
 
 	host->dma_ops = host->pdata->dma_ops;
@@ -2054,10 +2231,18 @@ int dw_mci_probe(struct dw_mci *host)
 	/* We need at least one slot to succeed */
 	for (i = 0; i < host->num_slots; i++) {
 		ret = dw_mci_init_slot(host, i);
-		if (ret) {
-			ret = -ENODEV;
-			goto err_init_slot;
-		}
+		if (ret)
+			dev_dbg(host->dev, "slot %d init failed\n", i);
+		else
+			init_slots++;
+	}
+
+	if (init_slots) {
+		dev_info(host->dev, "%d slots initialized\n", init_slots);
+	} else {
+		dev_dbg(host->dev, "attempted to initialize %d slots, "
+					"but failed on all\n", host->num_slots);
+		goto err_init_slot;
 	}
 
 	/*
@@ -2065,7 +2250,7 @@ int dw_mci_probe(struct dw_mci *host)
 	 * Need to check the version-id and set data-offset for DATA register.
 	 */
 	host->verid = SDMMC_GET_VERID(mci_readl(host, VERID));
-	dev_info(&host->dev, "Version ID is %04x\n", host->verid);
+	dev_info(host->dev, "Version ID is %04x\n", host->verid);
 
 	if (host->verid < DW_MMC_240A)
 		host->data_offset = DATA_OFFSET;
@@ -2082,22 +2267,16 @@ int dw_mci_probe(struct dw_mci *host)
 		   DW_MCI_ERROR_FLAGS | SDMMC_INT_CD);
 	mci_writel(host, CTRL, SDMMC_CTRL_INT_ENABLE); /* Enable mci interrupt */
 
-	dev_info(&host->dev, "DW MMC controller at irq %d, "
+	dev_info(host->dev, "DW MMC controller at irq %d, "
 		 "%d bit host data width, "
 		 "%u deep fifo\n",
 		 host->irq, width, fifo_size);
 	if (host->quirks & DW_MCI_QUIRK_IDMAC_DTO)
-		dev_info(&host->dev, "Internal DMAC interrupt fix enabled.\n");
+		dev_info(host->dev, "Internal DMAC interrupt fix enabled.\n");
 
 	return 0;
 
 err_init_slot:
-	/* De-init any initialized slots */
-	while (i > 0) {
-		if (host->slot[i])
-			dw_mci_cleanup_slot(host->slot[i], i);
-		i--;
-	}
 	free_irq(host->irq, host);
 
 err_workqueue:
@@ -2106,13 +2285,24 @@ err_workqueue:
 err_dmaunmap:
 	if (host->use_dma && host->dma_ops->exit)
 		host->dma_ops->exit(host);
-	dma_free_coherent(&host->dev, PAGE_SIZE,
+	dma_free_coherent(host->dev, PAGE_SIZE,
 			  host->sg_cpu, host->sg_dma);
 
 	if (host->vmmc) {
 		regulator_disable(host->vmmc);
 		regulator_put(host->vmmc);
 	}
+
+err_clk_ciu:
+	if (!IS_ERR(host->ciu_clk)) {
+		clk_disable_unprepare(host->ciu_clk);
+		clk_put(host->ciu_clk);
+	}
+err_clk_biu:
+	if (!IS_ERR(host->biu_clk)) {
+		clk_disable_unprepare(host->biu_clk);
+		clk_put(host->biu_clk);
+	}
 	return ret;
 }
 EXPORT_SYMBOL(dw_mci_probe);
@@ -2125,7 +2315,7 @@ void dw_mci_remove(struct dw_mci *host)
 	mci_writel(host, INTMASK, 0); /* disable all mmc interrupt first */
 
 	for (i = 0; i < host->num_slots; i++) {
-		dev_dbg(&host->dev, "remove slot %d\n", i);
+		dev_dbg(host->dev, "remove slot %d\n", i);
 		if (host->slot[i])
 			dw_mci_cleanup_slot(host->slot[i], i);
 	}
@@ -2136,7 +2326,7 @@ void dw_mci_remove(struct dw_mci *host)
 
 	free_irq(host->irq, host);
 	destroy_workqueue(host->card_workqueue);
-	dma_free_coherent(&host->dev, PAGE_SIZE, host->sg_cpu, host->sg_dma);
+	dma_free_coherent(host->dev, PAGE_SIZE, host->sg_cpu, host->sg_dma);
 
 	if (host->use_dma && host->dma_ops->exit)
 		host->dma_ops->exit(host);
@@ -2146,6 +2336,12 @@ void dw_mci_remove(struct dw_mci *host)
 		regulator_put(host->vmmc);
 	}
 
+	if (!IS_ERR(host->ciu_clk))
+		clk_disable_unprepare(host->ciu_clk);
+	if (!IS_ERR(host->biu_clk))
+		clk_disable_unprepare(host->biu_clk);
+	clk_put(host->ciu_clk);
+	clk_put(host->biu_clk);
 }
 EXPORT_SYMBOL(dw_mci_remove);
 
@@ -2188,7 +2384,7 @@ int dw_mci_resume(struct dw_mci *host)
 	if (host->vmmc)
 		regulator_enable(host->vmmc);
 
-	if (!mci_wait_reset(&host->dev, host)) {
+	if (!mci_wait_reset(host->dev, host)) {
 		ret = -ENODEV;
 		return ret;
 	}
diff --git a/drivers/mmc/host/dw_mmc.h b/drivers/mmc/host/dw_mmc.h
index 15c27e17c23..53b8fd987e4 100644
--- a/drivers/mmc/host/dw_mmc.h
+++ b/drivers/mmc/host/dw_mmc.h
@@ -182,4 +182,28 @@ extern int dw_mci_suspend(struct dw_mci *host);
 extern int dw_mci_resume(struct dw_mci *host);
 #endif
 
+/**
+ * dw_mci driver data - dw-mshc implementation specific driver data.
+ * @caps: mmc subsystem specified capabilities of the controller(s).
+ * @init: early implementation specific initialization.
+ * @setup_clock: implementation specific clock configuration.
+ * @prepare_command: handle CMD register extensions.
+ * @set_ios: handle bus specific extensions.
+ * @parse_dt: parse implementation specific device tree properties.
+ * @setup_bus: initialize io-interface
+ *
+ * Provide controller implementation specific extensions. The usage of this
+ * data structure is fully optional and usage of each member in this structure
+ * is optional as well.
+ */
+struct dw_mci_drv_data {
+	unsigned long	*caps;
+	int		(*init)(struct dw_mci *host);
+	int		(*setup_clock)(struct dw_mci *host);
+	void		(*prepare_command)(struct dw_mci *host, u32 *cmdr);
+	void		(*set_ios)(struct dw_mci *host, struct mmc_ios *ios);
+	int		(*parse_dt)(struct dw_mci *host);
+	int		(*setup_bus)(struct dw_mci *host,
+				struct device_node *slot_np, u8 bus_width);
+};
 #endif /* _DW_MMC_H_ */
diff --git a/drivers/mmc/host/mmc_spi.c b/drivers/mmc/host/mmc_spi.c
index 273306c68d5..a600eabbd6c 100644
--- a/drivers/mmc/host/mmc_spi.c
+++ b/drivers/mmc/host/mmc_spi.c
@@ -1532,20 +1532,7 @@ static struct spi_driver mmc_spi_driver = {
 	.remove =	__devexit_p(mmc_spi_remove),
 };
 
-
-static int __init mmc_spi_init(void)
-{
-	return spi_register_driver(&mmc_spi_driver);
-}
-module_init(mmc_spi_init);
-
-
-static void __exit mmc_spi_exit(void)
-{
-	spi_unregister_driver(&mmc_spi_driver);
-}
-module_exit(mmc_spi_exit);
-
+module_spi_driver(mmc_spi_driver);
 
 MODULE_AUTHOR("Mike Lavender, David Brownell, "
 		"Hans-Peter Nilsson, Jan Nikitenko");
diff --git a/drivers/mmc/host/mmci.c b/drivers/mmc/host/mmci.c
index 50ff19a6236..edc3e9baf0e 100644
--- a/drivers/mmc/host/mmci.c
+++ b/drivers/mmc/host/mmci.c
@@ -1309,14 +1309,10 @@ static int __devinit mmci_probe(struct amba_device *dev,
 		goto host_free;
 	}
 
-	ret = clk_prepare(host->clk);
+	ret = clk_prepare_enable(host->clk);
 	if (ret)
 		goto clk_free;
 
-	ret = clk_enable(host->clk);
-	if (ret)
-		goto clk_unprep;
-
 	host->plat = plat;
 	host->variant = variant;
 	host->mclk = clk_get_rate(host->clk);
@@ -1515,9 +1511,7 @@ static int __devinit mmci_probe(struct amba_device *dev,
  err_gpio_cd:
 	iounmap(host->base);
  clk_disable:
-	clk_disable(host->clk);
- clk_unprep:
-	clk_unprepare(host->clk);
+	clk_disable_unprepare(host->clk);
  clk_free:
 	clk_put(host->clk);
  host_free:
@@ -1564,8 +1558,7 @@ static int __devexit mmci_remove(struct amba_device *dev)
 			gpio_free(host->gpio_cd);
 
 		iounmap(host->base);
-		clk_disable(host->clk);
-		clk_unprepare(host->clk);
+		clk_disable_unprepare(host->clk);
 		clk_put(host->clk);
 
 		if (host->vcc)
diff --git a/drivers/mmc/host/mxcmmc.c b/drivers/mmc/host/mxcmmc.c
index 7b1161de01d..565c2e4fac7 100644
--- a/drivers/mmc/host/mxcmmc.c
+++ b/drivers/mmc/host/mxcmmc.c
@@ -44,6 +44,7 @@
 #include <mach/hardware.h>
 
 #define DRIVER_NAME "mxc-mmc"
+#define MXCMCI_TIMEOUT_MS 10000
 
 #define MMC_REG_STR_STP_CLK		0x00
 #define MMC_REG_STATUS			0x04
@@ -150,6 +151,8 @@ struct mxcmci_host {
 	int			dmareq;
 	struct dma_slave_config dma_slave_config;
 	struct imx_dma_data	dma_data;
+
+	struct timer_list	watchdog;
 };
 
 static void mxcmci_set_clk_rate(struct mxcmci_host *host, unsigned int clk_ios);
@@ -271,9 +274,32 @@ static int mxcmci_setup_data(struct mxcmci_host *host, struct mmc_data *data)
 	dmaengine_submit(host->desc);
 	dma_async_issue_pending(host->dma);
 
+	mod_timer(&host->watchdog, jiffies + msecs_to_jiffies(MXCMCI_TIMEOUT_MS));
+
 	return 0;
 }
 
+static void mxcmci_cmd_done(struct mxcmci_host *host, unsigned int stat);
+static void mxcmci_data_done(struct mxcmci_host *host, unsigned int stat);
+
+static void mxcmci_dma_callback(void *data)
+{
+	struct mxcmci_host *host = data;
+	u32 stat;
+
+	del_timer(&host->watchdog);
+
+	stat = readl(host->base + MMC_REG_STATUS);
+	writel(stat & ~STATUS_DATA_TRANS_DONE, host->base + MMC_REG_STATUS);
+
+	dev_dbg(mmc_dev(host->mmc), "%s: 0x%08x\n", __func__, stat);
+
+	if (stat & STATUS_READ_OP_DONE)
+		writel(STATUS_READ_OP_DONE, host->base + MMC_REG_STATUS);
+
+	mxcmci_data_done(host, stat);
+}
+
 static int mxcmci_start_cmd(struct mxcmci_host *host, struct mmc_command *cmd,
 		unsigned int cmdat)
 {
@@ -305,8 +331,14 @@ static int mxcmci_start_cmd(struct mxcmci_host *host, struct mmc_command *cmd,
 
 	int_cntr = INT_END_CMD_RES_EN;
 
-	if (mxcmci_use_dma(host))
-		int_cntr |= INT_READ_OP_EN | INT_WRITE_OP_DONE_EN;
+	if (mxcmci_use_dma(host)) {
+		if (host->dma_dir == DMA_FROM_DEVICE) {
+			host->desc->callback = mxcmci_dma_callback;
+			host->desc->callback_param = host;
+		} else {
+			int_cntr |= INT_WRITE_OP_DONE_EN;
+		}
+	}
 
 	spin_lock_irqsave(&host->lock, flags);
 	if (host->use_sdio)
@@ -345,11 +377,9 @@ static int mxcmci_finish_data(struct mxcmci_host *host, unsigned int stat)
 	struct mmc_data *data = host->data;
 	int data_error;
 
-	if (mxcmci_use_dma(host)) {
-		dmaengine_terminate_all(host->dma);
+	if (mxcmci_use_dma(host))
 		dma_unmap_sg(host->dma->device->dev, data->sg, data->sg_len,
 				host->dma_dir);
-	}
 
 	if (stat & STATUS_ERR_MASK) {
 		dev_dbg(mmc_dev(host->mmc), "request failed. status: 0x%08x\n",
@@ -624,8 +654,10 @@ static irqreturn_t mxcmci_irq(int irq, void *devid)
 		mxcmci_cmd_done(host, stat);
 
 	if (mxcmci_use_dma(host) &&
-		  (stat & (STATUS_DATA_TRANS_DONE | STATUS_WRITE_OP_DONE)))
+		  (stat & (STATUS_DATA_TRANS_DONE | STATUS_WRITE_OP_DONE))) {
+		del_timer(&host->watchdog);
 		mxcmci_data_done(host, stat);
+	}
 
 	if (host->default_irq_mask &&
 		  (stat & (STATUS_CARD_INSERTION | STATUS_CARD_REMOVAL)))
@@ -836,6 +868,34 @@ static bool filter(struct dma_chan *chan, void *param)
 	return true;
 }
 
+static void mxcmci_watchdog(unsigned long data)
+{
+	struct mmc_host *mmc = (struct mmc_host *)data;
+	struct mxcmci_host *host = mmc_priv(mmc);
+	struct mmc_request *req = host->req;
+	unsigned int stat = readl(host->base + MMC_REG_STATUS);
+
+	if (host->dma_dir == DMA_FROM_DEVICE) {
+		dmaengine_terminate_all(host->dma);
+		dev_err(mmc_dev(host->mmc),
+			"%s: read time out (status = 0x%08x)\n",
+			__func__, stat);
+	} else {
+		dev_err(mmc_dev(host->mmc),
+			"%s: write time out (status = 0x%08x)\n",
+			__func__, stat);
+		mxcmci_softreset(host);
+	}
+
+	/* Mark transfer as erroneus and inform the upper layers */
+
+	host->data->error = -ETIMEDOUT;
+	host->req = NULL;
+	host->cmd = NULL;
+	host->data = NULL;
+	mmc_request_done(host->mmc, req);
+}
+
 static const struct mmc_host_ops mxcmci_ops = {
 	.request		= mxcmci_request,
 	.set_ios		= mxcmci_set_ios,
@@ -968,6 +1028,10 @@ static int mxcmci_probe(struct platform_device *pdev)
 
 	mmc_add_host(mmc);
 
+	init_timer(&host->watchdog);
+	host->watchdog.function = &mxcmci_watchdog;
+	host->watchdog.data = (unsigned long)mmc;
+
 	return 0;
 
 out_free_irq:
diff --git a/drivers/mmc/host/mxs-mmc.c b/drivers/mmc/host/mxs-mmc.c
index bb4c2bf04d0..80d1e6d4b0a 100644
--- a/drivers/mmc/host/mxs-mmc.c
+++ b/drivers/mmc/host/mxs-mmc.c
@@ -525,7 +525,7 @@ static void mxs_mmc_enable_sdio_irq(struct mmc_host *mmc, int enable)
 		writel(BM_SSP_CTRL0_SDIO_IRQ_CHECK,
 		       ssp->base + HW_SSP_CTRL0 + STMP_OFFSET_REG_SET);
 		writel(BM_SSP_CTRL1_SDIO_IRQ_EN,
-		       host->base + HW_SSP_CTRL1(host) + STMP_OFFSET_REG_SET);
+		       ssp->base + HW_SSP_CTRL1(ssp) + STMP_OFFSET_REG_SET);
 	} else {
 		writel(BM_SSP_CTRL0_SDIO_IRQ_CHECK,
 		       ssp->base + HW_SSP_CTRL0 + STMP_OFFSET_REG_CLR);
diff --git a/drivers/mmc/host/omap.c b/drivers/mmc/host/omap.c
index c6259a82954..48ad361613e 100644
--- a/drivers/mmc/host/omap.c
+++ b/drivers/mmc/host/omap.c
@@ -27,16 +27,10 @@
 #include <linux/mmc/card.h>
 #include <linux/clk.h>
 #include <linux/scatterlist.h>
-#include <linux/i2c/tps65010.h>
 #include <linux/slab.h>
 
-#include <asm/io.h>
-#include <asm/irq.h>
-
 #include <plat/mmc.h>
-#include <asm/gpio.h>
 #include <plat/dma.h>
-#include <plat/fpga.h>
 
 #define	OMAP_MMC_REG_CMD	0x00
 #define	OMAP_MMC_REG_ARGL	0x01
@@ -105,7 +99,6 @@ struct mmc_omap_slot {
 	u16			saved_con;
 	u16			bus_mode;
 	unsigned int		fclk_freq;
-	unsigned		powered:1;
 
 	struct tasklet_struct	cover_tasklet;
 	struct timer_list       cover_timer;
@@ -137,7 +130,6 @@ struct mmc_omap_host {
 	unsigned int		phys_base;
 	int			irq;
 	unsigned char		bus_mode;
-	unsigned char		hw_bus_mode;
 	unsigned int		reg_shift;
 
 	struct work_struct	cmd_abort_work;
@@ -695,22 +687,29 @@ mmc_omap_xfer_data(struct mmc_omap_host *host, int write)
 	host->buffer += nwords;
 }
 
-static inline void mmc_omap_report_irq(u16 status)
+#ifdef CONFIG_MMC_DEBUG
+static void mmc_omap_report_irq(struct mmc_omap_host *host, u16 status)
 {
 	static const char *mmc_omap_status_bits[] = {
 		"EOC", "CD", "CB", "BRS", "EOFB", "DTO", "DCRC", "CTO",
 		"CCRC", "CRW", "AF", "AE", "OCRB", "CIRQ", "CERR"
 	};
-	int i, c = 0;
+	int i;
+	char res[64], *buf = res;
+
+	buf += sprintf(buf, "MMC IRQ 0x%x:", status);
 
 	for (i = 0; i < ARRAY_SIZE(mmc_omap_status_bits); i++)
-		if (status & (1 << i)) {
-			if (c)
-				printk(" ");
-			printk("%s", mmc_omap_status_bits[i]);
-			c++;
-		}
+		if (status & (1 << i))
+			buf += sprintf(buf, " %s", mmc_omap_status_bits[i]);
+	dev_vdbg(mmc_dev(host->mmc), "%s\n", res);
 }
+#else
+static void mmc_omap_report_irq(struct mmc_omap_host *host, u16 status)
+{
+}
+#endif
+
 
 static irqreturn_t mmc_omap_irq(int irq, void *dev_id)
 {
@@ -744,12 +743,10 @@ static irqreturn_t mmc_omap_irq(int irq, void *dev_id)
 			cmd = host->cmd->opcode;
 		else
 			cmd = -1;
-#ifdef CONFIG_MMC_DEBUG
 		dev_dbg(mmc_dev(host->mmc), "MMC IRQ %04x (CMD %d): ",
 			status, cmd);
-		mmc_omap_report_irq(status);
-		printk("\n");
-#endif
+		mmc_omap_report_irq(host, status);
+
 		if (host->total_bytes_left) {
 			if ((status & OMAP_MMC_STAT_A_FULL) ||
 			    (status & OMAP_MMC_STAT_END_OF_DATA))
diff --git a/drivers/mmc/host/omap_hsmmc.c b/drivers/mmc/host/omap_hsmmc.c
index 38adc330c00..54bfd0cc106 100644
--- a/drivers/mmc/host/omap_hsmmc.c
+++ b/drivers/mmc/host/omap_hsmmc.c
@@ -35,7 +35,6 @@
 #include <linux/mmc/core.h>
 #include <linux/mmc/mmc.h>
 #include <linux/io.h>
-#include <linux/semaphore.h>
 #include <linux/gpio.h>
 #include <linux/regulator/consumer.h>
 #include <linux/pm_runtime.h>
@@ -44,7 +43,6 @@
 #include <plat/cpu.h>
 
 /* OMAP HSMMC Host Controller Registers */
-#define OMAP_HSMMC_SYSCONFIG	0x0010
 #define OMAP_HSMMC_SYSSTATUS	0x0014
 #define OMAP_HSMMC_CON		0x002C
 #define OMAP_HSMMC_BLK		0x0104
@@ -161,8 +159,6 @@ struct omap_hsmmc_host {
 	unsigned int		dma_sg_idx;
 	unsigned char		bus_mode;
 	unsigned char		power_mode;
-	u32			*buffer;
-	u32			bytesleft;
 	int			suspended;
 	int			irq;
 	int			use_dma, dma_ch;
@@ -171,7 +167,6 @@ struct omap_hsmmc_host {
 	int			slot_id;
 	int			response_busy;
 	int			context_loss;
-	int			vdd;
 	int			protect_card;
 	int			reqs_blocked;
 	int			use_reg;
@@ -300,12 +295,12 @@ static int omap_hsmmc_reg_get(struct omap_hsmmc_host *host)
 	struct regulator *reg;
 	int ocr_value = 0;
 
-	mmc_slot(host).set_power = omap_hsmmc_set_power;
-
 	reg = regulator_get(host->dev, "vmmc");
 	if (IS_ERR(reg)) {
 		dev_dbg(host->dev, "vmmc regulator missing\n");
+		return PTR_ERR(reg);
 	} else {
+		mmc_slot(host).set_power = omap_hsmmc_set_power;
 		host->vcc = reg;
 		ocr_value = mmc_regulator_get_ocrmask(reg);
 		if (!mmc_slot(host).ocr_mask) {
@@ -495,7 +490,7 @@ static void omap_hsmmc_set_clock(struct omap_hsmmc_host *host)
 	unsigned long regval;
 	unsigned long timeout;
 
-	dev_dbg(mmc_dev(host->mmc), "Set clock to %uHz\n", ios->clock);
+	dev_vdbg(mmc_dev(host->mmc), "Set clock to %uHz\n", ios->clock);
 
 	omap_hsmmc_stop_clock(host);
 
@@ -579,21 +574,8 @@ static int omap_hsmmc_context_restore(struct omap_hsmmc_host *host)
 	if (host->context_loss == context_loss)
 		return 1;
 
-	/* Wait for hardware reset */
-	timeout = jiffies + msecs_to_jiffies(MMC_TIMEOUT_MS);
-	while ((OMAP_HSMMC_READ(host->base, SYSSTATUS) & RESETDONE) != RESETDONE
-		&& time_before(jiffies, timeout))
-		;
-
-	/* Do software reset */
-	OMAP_HSMMC_WRITE(host->base, SYSCONFIG, SOFTRESET);
-	timeout = jiffies + msecs_to_jiffies(MMC_TIMEOUT_MS);
-	while ((OMAP_HSMMC_READ(host->base, SYSSTATUS) & RESETDONE) != RESETDONE
-		&& time_before(jiffies, timeout))
-		;
-
-	OMAP_HSMMC_WRITE(host->base, SYSCONFIG,
-			OMAP_HSMMC_READ(host->base, SYSCONFIG) | AUTOIDLE);
+	if (!OMAP_HSMMC_READ(host->base, SYSSTATUS) & RESETDONE)
+		return 1;
 
 	if (host->pdata->controller_flags & OMAP_HSMMC_SUPPORTS_DUAL_VOLT) {
 		if (host->power_mode != MMC_POWER_OFF &&
@@ -745,7 +727,7 @@ omap_hsmmc_start_command(struct omap_hsmmc_host *host, struct mmc_command *cmd,
 {
 	int cmdreg = 0, resptype = 0, cmdtype = 0;
 
-	dev_dbg(mmc_dev(host->mmc), "%s: CMD%d, argument 0x%08x\n",
+	dev_vdbg(mmc_dev(host->mmc), "%s: CMD%d, argument 0x%08x\n",
 		mmc_hostname(host->mmc), cmd->opcode, cmd->arg);
 	host->cmd = cmd;
 
@@ -934,7 +916,7 @@ static void omap_hsmmc_dbg_report_irq(struct omap_hsmmc_host *host, u32 status)
 			buf += len;
 		}
 
-	dev_dbg(mmc_dev(host->mmc), "%s\n", res);
+	dev_vdbg(mmc_dev(host->mmc), "%s\n", res);
 }
 #else
 static inline void omap_hsmmc_dbg_report_irq(struct omap_hsmmc_host *host,
@@ -981,72 +963,40 @@ static inline void omap_hsmmc_reset_controller_fsm(struct omap_hsmmc_host *host,
 			__func__);
 }
 
+static void hsmmc_command_incomplete(struct omap_hsmmc_host *host, int err)
+{
+	omap_hsmmc_reset_controller_fsm(host, SRC);
+	host->cmd->error = err;
+
+	if (host->data) {
+		omap_hsmmc_reset_controller_fsm(host, SRD);
+		omap_hsmmc_dma_cleanup(host, err);
+	}
+
+}
+
 static void omap_hsmmc_do_irq(struct omap_hsmmc_host *host, int status)
 {
 	struct mmc_data *data;
 	int end_cmd = 0, end_trans = 0;
 
-	if (!host->req_in_progress) {
-		do {
-			OMAP_HSMMC_WRITE(host->base, STAT, status);
-			/* Flush posted write */
-			status = OMAP_HSMMC_READ(host->base, STAT);
-		} while (status & INT_EN_MASK);
-		return;
-	}
-
 	data = host->data;
-	dev_dbg(mmc_dev(host->mmc), "IRQ Status is %x\n", status);
+	dev_vdbg(mmc_dev(host->mmc), "IRQ Status is %x\n", status);
 
 	if (status & ERR) {
 		omap_hsmmc_dbg_report_irq(host, status);
-		if ((status & CMD_TIMEOUT) ||
-			(status & CMD_CRC)) {
-			if (host->cmd) {
-				if (status & CMD_TIMEOUT) {
-					omap_hsmmc_reset_controller_fsm(host,
-									SRC);
-					host->cmd->error = -ETIMEDOUT;
-				} else {
-					host->cmd->error = -EILSEQ;
-				}
-				end_cmd = 1;
-			}
-			if (host->data || host->response_busy) {
-				if (host->data)
-					omap_hsmmc_dma_cleanup(host,
-								-ETIMEDOUT);
-				host->response_busy = 0;
-				omap_hsmmc_reset_controller_fsm(host, SRD);
-			}
-		}
-		if ((status & DATA_TIMEOUT) ||
-			(status & DATA_CRC)) {
-			if (host->data || host->response_busy) {
-				int err = (status & DATA_TIMEOUT) ?
-						-ETIMEDOUT : -EILSEQ;
-
-				if (host->data)
-					omap_hsmmc_dma_cleanup(host, err);
-				else
-					host->mrq->cmd->error = err;
-				host->response_busy = 0;
-				omap_hsmmc_reset_controller_fsm(host, SRD);
-				end_trans = 1;
-			}
-		}
-		if (status & CARD_ERR) {
-			dev_dbg(mmc_dev(host->mmc),
-				"Ignoring card err CMD%d\n", host->cmd->opcode);
-			if (host->cmd)
-				end_cmd = 1;
-			if (host->data)
-				end_trans = 1;
+		if (status & (CMD_TIMEOUT | DATA_TIMEOUT))
+			hsmmc_command_incomplete(host, -ETIMEDOUT);
+		else if (status & (CMD_CRC | DATA_CRC))
+			hsmmc_command_incomplete(host, -EILSEQ);
+
+		end_cmd = 1;
+		if (host->data || host->response_busy) {
+			end_trans = 1;
+			host->response_busy = 0;
 		}
 	}
 
-	OMAP_HSMMC_WRITE(host->base, STAT, status);
-
 	if (end_cmd || ((status & CC) && host->cmd))
 		omap_hsmmc_cmd_done(host, host->cmd);
 	if ((end_trans || (status & TC)) && host->mrq)
@@ -1062,11 +1012,13 @@ static irqreturn_t omap_hsmmc_irq(int irq, void *dev_id)
 	int status;
 
 	status = OMAP_HSMMC_READ(host->base, STAT);
-	do {
+	while (status & INT_EN_MASK && host->req_in_progress) {
 		omap_hsmmc_do_irq(host, status);
+
 		/* Flush posted write */
+		OMAP_HSMMC_WRITE(host->base, STAT, status);
 		status = OMAP_HSMMC_READ(host->base, STAT);
-	} while (status & INT_EN_MASK);
+	}
 
 	return IRQ_HANDLED;
 }
@@ -1501,12 +1453,10 @@ static void omap_hsmmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		case MMC_POWER_OFF:
 			mmc_slot(host).set_power(host->dev, host->slot_id,
 						 0, 0);
-			host->vdd = 0;
 			break;
 		case MMC_POWER_UP:
 			mmc_slot(host).set_power(host->dev, host->slot_id,
 						 1, ios->vdd);
-			host->vdd = ios->vdd;
 			break;
 		case MMC_POWER_ON:
 			do_send_init_stream = 1;
@@ -1598,10 +1548,6 @@ static void omap_hsmmc_conf_bus_power(struct omap_hsmmc_host *host)
 	value = OMAP_HSMMC_READ(host->base, CAPA);
 	OMAP_HSMMC_WRITE(host->base, CAPA, value | capa);
 
-	/* Set the controller to AUTO IDLE mode */
-	value = OMAP_HSMMC_READ(host->base, SYSCONFIG);
-	OMAP_HSMMC_WRITE(host->base, SYSCONFIG, value | AUTOIDLE);
-
 	/* Set SD bus power bit */
 	set_sd_bus_power(host);
 }
@@ -1659,8 +1605,6 @@ static int omap_hsmmc_regs_show(struct seq_file *s, void *data)
 
 	pm_runtime_get_sync(host->dev);
 
-	seq_printf(s, "SYSCONFIG:\t0x%08x\n",
-			OMAP_HSMMC_READ(host->base, SYSCONFIG));
 	seq_printf(s, "CON:\t\t0x%08x\n",
 			OMAP_HSMMC_READ(host->base, CON));
 	seq_printf(s, "HCTL:\t\t0x%08x\n",
@@ -2105,8 +2049,7 @@ static int omap_hsmmc_suspend(struct device *dev)
 	if (ret) {
 		host->suspended = 0;
 		if (host->pdata->resume) {
-			ret = host->pdata->resume(dev, host->slot_id);
-			if (ret)
+			if (host->pdata->resume(dev, host->slot_id))
 				dev_dbg(dev, "Unmask interrupt failed\n");
 		}
 		goto err;
diff --git a/drivers/mmc/host/pxamci.c b/drivers/mmc/host/pxamci.c
index ca3915dac03..3f9d6d577a9 100644
--- a/drivers/mmc/host/pxamci.c
+++ b/drivers/mmc/host/pxamci.c
@@ -30,6 +30,9 @@
 #include <linux/regulator/consumer.h>
 #include <linux/gpio.h>
 #include <linux/gfp.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+#include <linux/of_device.h>
 
 #include <asm/sizes.h>
 
@@ -573,6 +576,50 @@ static irqreturn_t pxamci_detect_irq(int irq, void *devid)
 	return IRQ_HANDLED;
 }
 
+#ifdef CONFIG_OF
+static const struct of_device_id pxa_mmc_dt_ids[] = {
+        { .compatible = "marvell,pxa-mmc" },
+        { }
+};
+
+MODULE_DEVICE_TABLE(of, pxa_mmc_dt_ids);
+
+static int __devinit pxamci_of_init(struct platform_device *pdev)
+{
+        struct device_node *np = pdev->dev.of_node;
+        struct pxamci_platform_data *pdata;
+        u32 tmp;
+
+        if (!np)
+                return 0;
+
+        pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+        if (!pdata)
+                return -ENOMEM;
+
+	pdata->gpio_card_detect =
+		of_get_named_gpio(np, "cd-gpios", 0);
+	pdata->gpio_card_ro =
+		of_get_named_gpio(np, "wp-gpios", 0);
+
+	/* pxa-mmc specific */
+	pdata->gpio_power =
+		of_get_named_gpio(np, "pxa-mmc,gpio-power", 0);
+
+	if (of_property_read_u32(np, "pxa-mmc,detect-delay-ms", &tmp) == 0)
+		pdata->detect_delay_ms = tmp;
+
+        pdev->dev.platform_data = pdata;
+
+        return 0;
+}
+#else
+static int __devinit pxamci_of_init(struct platform_device *pdev)
+{
+        return 0;
+}
+#endif
+
 static int pxamci_probe(struct platform_device *pdev)
 {
 	struct mmc_host *mmc;
@@ -580,6 +627,10 @@ static int pxamci_probe(struct platform_device *pdev)
 	struct resource *r, *dmarx, *dmatx;
 	int ret, irq, gpio_cd = -1, gpio_ro = -1, gpio_power = -1;
 
+	ret = pxamci_of_init(pdev);
+	if (ret)
+		return ret;
+
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	irq = platform_get_irq(pdev, 0);
 	if (!r || irq < 0)
@@ -866,6 +917,7 @@ static struct platform_driver pxamci_driver = {
 	.driver		= {
 		.name	= DRIVER_NAME,
 		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(pxa_mmc_dt_ids),
 #ifdef CONFIG_PM
 		.pm	= &pxamci_pm_ops,
 #endif
diff --git a/drivers/mmc/host/sdhci-dove.c b/drivers/mmc/host/sdhci-dove.c
index a6e53a1ebb0..90140eb03e3 100644
--- a/drivers/mmc/host/sdhci-dove.c
+++ b/drivers/mmc/host/sdhci-dove.c
@@ -24,6 +24,7 @@
 #include <linux/err.h>
 #include <linux/module.h>
 #include <linux/mmc/host.h>
+#include <linux/of.h>
 
 #include "sdhci-pltfm.h"
 
@@ -126,11 +127,18 @@ static int __devexit sdhci_dove_remove(struct platform_device *pdev)
 	return sdhci_pltfm_unregister(pdev);
 }
 
+static const struct of_device_id sdhci_dove_of_match_table[] __devinitdata = {
+	{ .compatible = "marvell,dove-sdhci", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, sdhci_dove_of_match_table);
+
 static struct platform_driver sdhci_dove_driver = {
 	.driver		= {
 		.name	= "sdhci-dove",
 		.owner	= THIS_MODULE,
 		.pm	= SDHCI_PLTFM_PMOPS,
+		.of_match_table = of_match_ptr(sdhci_dove_of_match_table),
 	},
 	.probe		= sdhci_dove_probe,
 	.remove		= __devexit_p(sdhci_dove_remove),
diff --git a/drivers/mmc/host/sdhci-of-esdhc.c b/drivers/mmc/host/sdhci-of-esdhc.c
index f8eb1fb0c92..ae5fcbfa1ee 100644
--- a/drivers/mmc/host/sdhci-of-esdhc.c
+++ b/drivers/mmc/host/sdhci-of-esdhc.c
@@ -21,6 +21,32 @@
 #include "sdhci-pltfm.h"
 #include "sdhci-esdhc.h"
 
+#define VENDOR_V_22	0x12
+static u32 esdhc_readl(struct sdhci_host *host, int reg)
+{
+	u32 ret;
+
+	ret = in_be32(host->ioaddr + reg);
+	/*
+	 * The bit of ADMA flag in eSDHC is not compatible with standard
+	 * SDHC register, so set fake flag SDHCI_CAN_DO_ADMA2 when ADMA is
+	 * supported by eSDHC.
+	 * And for many FSL eSDHC controller, the reset value of field
+	 * SDHCI_CAN_DO_ADMA1 is one, but some of them can't support ADMA,
+	 * only these vendor version is greater than 2.2/0x12 support ADMA.
+	 * For FSL eSDHC, must aligned 4-byte, so use 0xFC to read the
+	 * the verdor version number, oxFE is SDHCI_HOST_VERSION.
+	 */
+	if ((reg == SDHCI_CAPABILITIES) && (ret & SDHCI_CAN_DO_ADMA1)) {
+		u32 tmp = in_be32(host->ioaddr + SDHCI_SLOT_INT_STATUS);
+		tmp = (tmp & SDHCI_VENDOR_VER_MASK) >> SDHCI_VENDOR_VER_SHIFT;
+		if (tmp > VENDOR_V_22)
+			ret |= SDHCI_CAN_DO_ADMA2;
+	}
+
+	return ret;
+}
+
 static u16 esdhc_readw(struct sdhci_host *host, int reg)
 {
 	u16 ret;
@@ -144,7 +170,7 @@ static void esdhc_of_resume(struct sdhci_host *host)
 #endif
 
 static struct sdhci_ops sdhci_esdhc_ops = {
-	.read_l = sdhci_be32bs_readl,
+	.read_l = esdhc_readl,
 	.read_w = esdhc_readw,
 	.read_b = esdhc_readb,
 	.write_l = sdhci_be32bs_writel,
@@ -161,9 +187,13 @@ static struct sdhci_ops sdhci_esdhc_ops = {
 };
 
 static struct sdhci_pltfm_data sdhci_esdhc_pdata = {
-	/* card detection could be handled via GPIO */
+	/*
+	 * card detection could be handled via GPIO
+	 * eSDHC cannot support End Attribute in NOP ADMA descriptor
+	 */
 	.quirks = ESDHC_DEFAULT_QUIRKS | SDHCI_QUIRK_BROKEN_CARD_DETECTION
-		| SDHCI_QUIRK_NO_CARD_NO_RESET,
+		| SDHCI_QUIRK_NO_CARD_NO_RESET
+		| SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC,
 	.ops = &sdhci_esdhc_ops,
 };
 
diff --git a/drivers/mmc/host/sdhci-pci.c b/drivers/mmc/host/sdhci-pci.c
index 9722d43d614..4bb74b042a0 100644
--- a/drivers/mmc/host/sdhci-pci.c
+++ b/drivers/mmc/host/sdhci-pci.c
@@ -1476,24 +1476,7 @@ static struct pci_driver sdhci_driver = {
 	},
 };
 
-/*****************************************************************************\
- *                                                                           *
- * Driver init/exit                                                          *
- *                                                                           *
-\*****************************************************************************/
-
-static int __init sdhci_drv_init(void)
-{
-	return pci_register_driver(&sdhci_driver);
-}
-
-static void __exit sdhci_drv_exit(void)
-{
-	pci_unregister_driver(&sdhci_driver);
-}
-
-module_init(sdhci_drv_init);
-module_exit(sdhci_drv_exit);
+module_pci_driver(sdhci_driver);
 
 MODULE_AUTHOR("Pierre Ossman <pierre@ossman.eu>");
 MODULE_DESCRIPTION("Secure Digital Host Controller Interface PCI driver");
diff --git a/drivers/mmc/host/sdhci-pltfm.c b/drivers/mmc/host/sdhci-pltfm.c
index d9a4ef4f1ed..65551a9709c 100644
--- a/drivers/mmc/host/sdhci-pltfm.c
+++ b/drivers/mmc/host/sdhci-pltfm.c
@@ -75,6 +75,9 @@ void sdhci_get_of_property(struct platform_device *pdev)
 		if (sdhci_of_wp_inverted(np))
 			host->quirks |= SDHCI_QUIRK_INVERTED_WRITE_PROTECT;
 
+		if (of_get_property(np, "broken-cd", NULL))
+			host->quirks |= SDHCI_QUIRK_BROKEN_CARD_DETECTION;
+
 		if (of_device_is_compatible(np, "fsl,p2020-rev1-esdhc"))
 			host->quirks |= SDHCI_QUIRK_BROKEN_DMA;
 
diff --git a/drivers/mmc/host/sdhci-pxav2.c b/drivers/mmc/host/sdhci-pxav2.c
index b6ee8857e22..8e63a9c04e3 100644
--- a/drivers/mmc/host/sdhci-pxav2.c
+++ b/drivers/mmc/host/sdhci-pxav2.c
@@ -197,7 +197,7 @@ static int __devinit sdhci_pxav2_probe(struct platform_device *pdev)
 		goto err_clk_get;
 	}
 	pltfm_host->clk = clk;
-	clk_enable(clk);
+	clk_prepare_enable(clk);
 
 	host->quirks = SDHCI_QUIRK_BROKEN_ADMA
 		| SDHCI_QUIRK_BROKEN_TIMEOUT_VAL
@@ -239,7 +239,7 @@ static int __devinit sdhci_pxav2_probe(struct platform_device *pdev)
 	return 0;
 
 err_add_host:
-	clk_disable(clk);
+	clk_disable_unprepare(clk);
 	clk_put(clk);
 err_clk_get:
 	sdhci_pltfm_free(pdev);
@@ -255,7 +255,7 @@ static int __devexit sdhci_pxav2_remove(struct platform_device *pdev)
 
 	sdhci_remove_host(host, 1);
 
-	clk_disable(pltfm_host->clk);
+	clk_disable_unprepare(pltfm_host->clk);
 	clk_put(pltfm_host->clk);
 	sdhci_pltfm_free(pdev);
 	kfree(pxa);
diff --git a/drivers/mmc/host/sdhci-pxav3.c b/drivers/mmc/host/sdhci-pxav3.c
index 07fe3834fe0..e918a2bb3af 100644
--- a/drivers/mmc/host/sdhci-pxav3.c
+++ b/drivers/mmc/host/sdhci-pxav3.c
@@ -24,12 +24,14 @@
 #include <linux/gpio.h>
 #include <linux/mmc/card.h>
 #include <linux/mmc/host.h>
+#include <linux/mmc/slot-gpio.h>
 #include <linux/platform_data/pxa_sdhci.h>
 #include <linux/slab.h>
 #include <linux/delay.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
+#include <linux/of_gpio.h>
 
 #include "sdhci.h"
 #include "sdhci-pltfm.h"
@@ -182,6 +184,7 @@ static struct sdhci_pxa_platdata *pxav3_get_mmc_pdata(struct device *dev)
 	struct device_node *np = dev->of_node;
 	u32 bus_width;
 	u32 clk_delay_cycles;
+	enum of_gpio_flags gpio_flags;
 
 	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
 	if (!pdata)
@@ -198,6 +201,10 @@ static struct sdhci_pxa_platdata *pxav3_get_mmc_pdata(struct device *dev)
 	if (clk_delay_cycles > 0)
 		pdata->clk_delay_cycles = clk_delay_cycles;
 
+	pdata->ext_cd_gpio = of_get_named_gpio_flags(np, "cd-gpios", 0, &gpio_flags);
+	if (gpio_flags != OF_GPIO_ACTIVE_LOW)
+		pdata->host_caps2 |= MMC_CAP2_CD_ACTIVE_HIGH;
+
 	return pdata;
 }
 #else
@@ -231,14 +238,14 @@ static int __devinit sdhci_pxav3_probe(struct platform_device *pdev)
 	pltfm_host = sdhci_priv(host);
 	pltfm_host->priv = pxa;
 
-	clk = clk_get(dev, "PXA-SDHCLK");
+	clk = clk_get(dev, NULL);
 	if (IS_ERR(clk)) {
 		dev_err(dev, "failed to get io clock\n");
 		ret = PTR_ERR(clk);
 		goto err_clk_get;
 	}
 	pltfm_host->clk = clk;
-	clk_enable(clk);
+	clk_prepare_enable(clk);
 
 	host->quirks = SDHCI_QUIRK_BROKEN_TIMEOUT_VAL
 		| SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC
@@ -266,12 +273,25 @@ static int __devinit sdhci_pxav3_probe(struct platform_device *pdev)
 			host->quirks |= pdata->quirks;
 		if (pdata->host_caps)
 			host->mmc->caps |= pdata->host_caps;
+		if (pdata->host_caps2)
+			host->mmc->caps2 |= pdata->host_caps2;
 		if (pdata->pm_caps)
 			host->mmc->pm_caps |= pdata->pm_caps;
+
+		if (gpio_is_valid(pdata->ext_cd_gpio)) {
+			ret = mmc_gpio_request_cd(host->mmc, pdata->ext_cd_gpio);
+			if (ret) {
+				dev_err(mmc_dev(host->mmc),
+					"failed to allocate card detect gpio\n");
+				goto err_cd_req;
+			}
+		}
 	}
 
 	host->ops = &pxav3_sdhci_ops;
 
+	sdhci_get_of_property(pdev);
+
 	ret = sdhci_add_host(host);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to add host\n");
@@ -283,8 +303,10 @@ static int __devinit sdhci_pxav3_probe(struct platform_device *pdev)
 	return 0;
 
 err_add_host:
-	clk_disable(clk);
+	clk_disable_unprepare(clk);
 	clk_put(clk);
+	mmc_gpio_free_cd(host->mmc);
+err_cd_req:
 err_clk_get:
 	sdhci_pltfm_free(pdev);
 	kfree(pxa);
@@ -296,11 +318,16 @@ static int __devexit sdhci_pxav3_remove(struct platform_device *pdev)
 	struct sdhci_host *host = platform_get_drvdata(pdev);
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
 	struct sdhci_pxa *pxa = pltfm_host->priv;
+	struct sdhci_pxa_platdata *pdata = pdev->dev.platform_data;
 
 	sdhci_remove_host(host, 1);
 
-	clk_disable(pltfm_host->clk);
+	clk_disable_unprepare(pltfm_host->clk);
 	clk_put(pltfm_host->clk);
+
+	if (gpio_is_valid(pdata->ext_cd_gpio))
+		mmc_gpio_free_cd(host->mmc);
+
 	sdhci_pltfm_free(pdev);
 	kfree(pxa);
 
diff --git a/drivers/mmc/host/sdhci-s3c.c b/drivers/mmc/host/sdhci-s3c.c
index a50c205ea20..2903949594c 100644
--- a/drivers/mmc/host/sdhci-s3c.c
+++ b/drivers/mmc/host/sdhci-s3c.c
@@ -34,6 +34,9 @@
 
 #define MAX_BUS_CLK	(4)
 
+/* Number of gpio's used is max data bus width + command and clock lines */
+#define NUM_GPIOS(x)	(x + 2)
+
 /**
  * struct sdhci_s3c - S3C SDHCI instance
  * @host: The SDHCI host created
@@ -41,6 +44,7 @@
  * @ioarea: The resource created when we claimed the IO area.
  * @pdata: The platform data for this controller.
  * @cur_clk: The index of the current bus clock.
+ * @gpios: List of gpio numbers parsed from device tree.
  * @clk_io: The clock for the internal bus interface.
  * @clk_bus: The clocks that are available for the SD/MMC bus clock.
  */
@@ -52,6 +56,7 @@ struct sdhci_s3c {
 	unsigned int		cur_clk;
 	int			ext_cd_irq;
 	int			ext_cd_gpio;
+	int			*gpios;
 
 	struct clk		*clk_io;
 	struct clk		*clk_bus[MAX_BUS_CLK];
@@ -166,7 +171,7 @@ static unsigned int sdhci_s3c_consider_clock(struct sdhci_s3c *ourhost,
 	dev_dbg(&ourhost->pdev->dev, "clk %d: rate %ld, want %d, got %ld\n",
 		src, rate, wanted, rate / div);
 
-	return (wanted - (rate / div));
+	return wanted - (rate / div);
 }
 
 /**
@@ -203,10 +208,12 @@ static void sdhci_s3c_set_clock(struct sdhci_host *host, unsigned int clock)
 		 best_src, clock, best);
 
 	/* select the new clock source */
-
 	if (ourhost->cur_clk != best_src) {
 		struct clk *clk = ourhost->clk_bus[best_src];
 
+		clk_enable(clk);
+		clk_disable(ourhost->clk_bus[ourhost->cur_clk]);
+
 		/* turn clock off to card before changing clock source */
 		writew(0, host->ioaddr + SDHCI_CLOCK_CONTROL);
 
@@ -288,6 +295,7 @@ static unsigned int sdhci_cmu_get_min_clock(struct sdhci_host *host)
 static void sdhci_cmu_set_clock(struct sdhci_host *host, unsigned int clock)
 {
 	struct sdhci_s3c *ourhost = to_s3c(host);
+	struct device *dev = &ourhost->pdev->dev;
 	unsigned long timeout;
 	u16 clk = 0;
 
@@ -309,8 +317,8 @@ static void sdhci_cmu_set_clock(struct sdhci_host *host, unsigned int clock)
 	while (!((clk = sdhci_readw(host, SDHCI_CLOCK_CONTROL))
 		& SDHCI_CLOCK_INT_STABLE)) {
 		if (timeout == 0) {
-			printk(KERN_ERR "%s: Internal clock never "
-				"stabilised.\n", mmc_hostname(host->mmc));
+			dev_err(dev, "%s: Internal clock never stabilised.\n",
+				mmc_hostname(host->mmc));
 			return;
 		}
 		timeout--;
@@ -404,7 +412,9 @@ static void sdhci_s3c_setup_card_detect_gpio(struct sdhci_s3c *sc)
 		if (sc->ext_cd_irq &&
 		    request_threaded_irq(sc->ext_cd_irq, NULL,
 					 sdhci_s3c_gpio_card_detect_thread,
-					 IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+					 IRQF_TRIGGER_RISING |
+					 IRQF_TRIGGER_FALLING |
+					 IRQF_ONESHOT,
 					 dev_name(dev), sc) == 0) {
 			int status = gpio_get_value(sc->ext_cd_gpio);
 			if (pdata->ext_cd_gpio_invert)
@@ -419,9 +429,121 @@ static void sdhci_s3c_setup_card_detect_gpio(struct sdhci_s3c *sc)
 	}
 }
 
+#ifdef CONFIG_OF
+static int __devinit sdhci_s3c_parse_dt(struct device *dev,
+		struct sdhci_host *host, struct s3c_sdhci_platdata *pdata)
+{
+	struct device_node *node = dev->of_node;
+	struct sdhci_s3c *ourhost = to_s3c(host);
+	u32 max_width;
+	int gpio, cnt, ret;
+
+	/* if the bus-width property is not specified, assume width as 1 */
+	if (of_property_read_u32(node, "bus-width", &max_width))
+		max_width = 1;
+	pdata->max_width = max_width;
+
+	ourhost->gpios = devm_kzalloc(dev, NUM_GPIOS(pdata->max_width) *
+				sizeof(int), GFP_KERNEL);
+	if (!ourhost->gpios)
+		return -ENOMEM;
+
+	/* get the card detection method */
+	if (of_get_property(node, "broken-cd", 0)) {
+		pdata->cd_type = S3C_SDHCI_CD_NONE;
+		goto setup_bus;
+	}
+
+	if (of_get_property(node, "non-removable", 0)) {
+		pdata->cd_type = S3C_SDHCI_CD_PERMANENT;
+		goto setup_bus;
+	}
+
+	gpio = of_get_named_gpio(node, "cd-gpios", 0);
+	if (gpio_is_valid(gpio)) {
+		pdata->cd_type = S3C_SDHCI_CD_GPIO;
+		goto found_cd;
+	} else if (gpio != -ENOENT) {
+		dev_err(dev, "invalid card detect gpio specified\n");
+		return -EINVAL;
+	}
+
+	gpio = of_get_named_gpio(node, "samsung,cd-pinmux-gpio", 0);
+	if (gpio_is_valid(gpio)) {
+		pdata->cd_type = S3C_SDHCI_CD_INTERNAL;
+		goto found_cd;
+	} else if (gpio != -ENOENT) {
+		dev_err(dev, "invalid card detect gpio specified\n");
+		return -EINVAL;
+	}
+
+	dev_info(dev, "assuming no card detect line available\n");
+	pdata->cd_type = S3C_SDHCI_CD_NONE;
+
+ found_cd:
+	if (pdata->cd_type == S3C_SDHCI_CD_GPIO) {
+		pdata->ext_cd_gpio = gpio;
+		ourhost->ext_cd_gpio = -1;
+		if (of_get_property(node, "cd-inverted", NULL))
+			pdata->ext_cd_gpio_invert = 1;
+	} else if (pdata->cd_type == S3C_SDHCI_CD_INTERNAL) {
+		ret = gpio_request(gpio, "sdhci-cd");
+		if (ret) {
+			dev_err(dev, "card detect gpio request failed\n");
+			return -EINVAL;
+		}
+		ourhost->ext_cd_gpio = gpio;
+	}
+
+ setup_bus:
+	/* get the gpios for command, clock and data lines */
+	for (cnt = 0; cnt < NUM_GPIOS(pdata->max_width); cnt++) {
+		gpio = of_get_gpio(node, cnt);
+		if (!gpio_is_valid(gpio)) {
+			dev_err(dev, "invalid gpio[%d]\n", cnt);
+			goto err_free_dt_cd_gpio;
+		}
+		ourhost->gpios[cnt] = gpio;
+	}
+
+	for (cnt = 0; cnt < NUM_GPIOS(pdata->max_width); cnt++) {
+		ret = gpio_request(ourhost->gpios[cnt], "sdhci-gpio");
+		if (ret) {
+			dev_err(dev, "gpio[%d] request failed\n", cnt);
+			goto err_free_dt_gpios;
+		}
+	}
+
+	return 0;
+
+ err_free_dt_gpios:
+	while (--cnt >= 0)
+		gpio_free(ourhost->gpios[cnt]);
+ err_free_dt_cd_gpio:
+	if (pdata->cd_type == S3C_SDHCI_CD_INTERNAL)
+		gpio_free(ourhost->ext_cd_gpio);
+	return -EINVAL;
+}
+#else
+static int __devinit sdhci_s3c_parse_dt(struct device *dev,
+		struct sdhci_host *host, struct s3c_sdhci_platdata *pdata)
+{
+	return -EINVAL;
+}
+#endif
+
+static const struct of_device_id sdhci_s3c_dt_match[];
+
 static inline struct sdhci_s3c_drv_data *sdhci_s3c_get_driver_data(
 			struct platform_device *pdev)
 {
+#ifdef CONFIG_OF
+	if (pdev->dev.of_node) {
+		const struct of_device_id *match;
+		match = of_match_node(sdhci_s3c_dt_match, pdev->dev.of_node);
+		return (struct sdhci_s3c_drv_data *)match->data;
+	}
+#endif
 	return (struct sdhci_s3c_drv_data *)
 			platform_get_device_id(pdev)->driver_data;
 }
@@ -436,7 +558,7 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
 	struct resource *res;
 	int ret, irq, ptr, clks;
 
-	if (!pdev->dev.platform_data) {
+	if (!pdev->dev.platform_data && !pdev->dev.of_node) {
 		dev_err(dev, "no device data specified\n");
 		return -ENOENT;
 	}
@@ -452,21 +574,28 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
 		dev_err(dev, "sdhci_alloc_host() failed\n");
 		return PTR_ERR(host);
 	}
+	sc = sdhci_priv(host);
 
 	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
 	if (!pdata) {
 		ret = -ENOMEM;
-		goto err_io_clk;
+		goto err_pdata;
+	}
+
+	if (pdev->dev.of_node) {
+		ret = sdhci_s3c_parse_dt(&pdev->dev, host, pdata);
+		if (ret)
+			goto err_pdata;
+	} else {
+		memcpy(pdata, pdev->dev.platform_data, sizeof(*pdata));
+		sc->ext_cd_gpio = -1; /* invalid gpio number */
 	}
-	memcpy(pdata, pdev->dev.platform_data, sizeof(*pdata));
 
 	drv_data = sdhci_s3c_get_driver_data(pdev);
-	sc = sdhci_priv(host);
 
 	sc->host = host;
 	sc->pdev = pdev;
 	sc->pdata = pdata;
-	sc->ext_cd_gpio = -1; /* invalid gpio number */
 
 	platform_set_drvdata(pdev, host);
 
@@ -486,9 +615,8 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
 
 		snprintf(name, 14, "mmc_busclk.%d", ptr);
 		clk = clk_get(dev, name);
-		if (IS_ERR(clk)) {
+		if (IS_ERR(clk))
 			continue;
-		}
 
 		clks++;
 		sc->clk_bus[ptr] = clk;
@@ -499,8 +627,6 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
 		 */
 		sc->cur_clk = ptr;
 
-		clk_enable(clk);
-
 		dev_info(dev, "clock source %d: %s (%ld Hz)\n",
 			 ptr, name, clk_get_rate(clk));
 	}
@@ -511,6 +637,10 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
 		goto err_no_busclks;
 	}
 
+#ifndef CONFIG_PM_RUNTIME
+	clk_enable(sc->clk_bus[sc->cur_clk]);
+#endif
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	host->ioaddr = devm_request_and_ioremap(&pdev->dev, res);
 	if (!host->ioaddr) {
@@ -616,12 +746,17 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
 	    gpio_is_valid(pdata->ext_cd_gpio))
 		sdhci_s3c_setup_card_detect_gpio(sc);
 
+#ifdef CONFIG_PM_RUNTIME
+	clk_disable(sc->clk_io);
+#endif
 	return 0;
 
  err_req_regs:
+#ifndef CONFIG_PM_RUNTIME
+	clk_disable(sc->clk_bus[sc->cur_clk]);
+#endif
 	for (ptr = 0; ptr < MAX_BUS_CLK; ptr++) {
 		if (sc->clk_bus[ptr]) {
-			clk_disable(sc->clk_bus[ptr]);
 			clk_put(sc->clk_bus[ptr]);
 		}
 	}
@@ -631,6 +766,12 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
 	clk_put(sc->clk_io);
 
  err_io_clk:
+	for (ptr = 0; ptr < NUM_GPIOS(sc->pdata->max_width); ptr++)
+		gpio_free(sc->gpios[ptr]);
+	if (pdata->cd_type == S3C_SDHCI_CD_INTERNAL)
+		gpio_free(sc->ext_cd_gpio);
+
+ err_pdata:
 	sdhci_free_host(host);
 
 	return ret;
@@ -638,9 +779,9 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
 
 static int __devexit sdhci_s3c_remove(struct platform_device *pdev)
 {
-	struct s3c_sdhci_platdata *pdata = pdev->dev.platform_data;
 	struct sdhci_host *host =  platform_get_drvdata(pdev);
 	struct sdhci_s3c *sc = sdhci_priv(host);
+	struct s3c_sdhci_platdata *pdata = sc->pdata;
 	int ptr;
 
 	if (pdata->cd_type == S3C_SDHCI_CD_EXTERNAL && pdata->ext_cd_cleanup)
@@ -652,19 +793,30 @@ static int __devexit sdhci_s3c_remove(struct platform_device *pdev)
 	if (gpio_is_valid(sc->ext_cd_gpio))
 		gpio_free(sc->ext_cd_gpio);
 
+#ifdef CONFIG_PM_RUNTIME
+	clk_enable(sc->clk_io);
+#endif
 	sdhci_remove_host(host, 1);
 
+	pm_runtime_dont_use_autosuspend(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
 
-	for (ptr = 0; ptr < 3; ptr++) {
+#ifndef CONFIG_PM_RUNTIME
+	clk_disable(sc->clk_bus[sc->cur_clk]);
+#endif
+	for (ptr = 0; ptr < MAX_BUS_CLK; ptr++) {
 		if (sc->clk_bus[ptr]) {
-			clk_disable(sc->clk_bus[ptr]);
 			clk_put(sc->clk_bus[ptr]);
 		}
 	}
 	clk_disable(sc->clk_io);
 	clk_put(sc->clk_io);
 
+	if (pdev->dev.of_node) {
+		for (ptr = 0; ptr < NUM_GPIOS(sc->pdata->max_width); ptr++)
+			gpio_free(sc->gpios[ptr]);
+	}
+
 	sdhci_free_host(host);
 	platform_set_drvdata(pdev, NULL);
 
@@ -691,15 +843,28 @@ static int sdhci_s3c_resume(struct device *dev)
 static int sdhci_s3c_runtime_suspend(struct device *dev)
 {
 	struct sdhci_host *host = dev_get_drvdata(dev);
+	struct sdhci_s3c *ourhost = to_s3c(host);
+	struct clk *busclk = ourhost->clk_io;
+	int ret;
+
+	ret = sdhci_runtime_suspend_host(host);
 
-	return sdhci_runtime_suspend_host(host);
+	clk_disable(ourhost->clk_bus[ourhost->cur_clk]);
+	clk_disable(busclk);
+	return ret;
 }
 
 static int sdhci_s3c_runtime_resume(struct device *dev)
 {
 	struct sdhci_host *host = dev_get_drvdata(dev);
+	struct sdhci_s3c *ourhost = to_s3c(host);
+	struct clk *busclk = ourhost->clk_io;
+	int ret;
 
-	return sdhci_runtime_resume_host(host);
+	clk_enable(busclk);
+	clk_enable(ourhost->clk_bus[ourhost->cur_clk]);
+	ret = sdhci_runtime_resume_host(host);
+	return ret;
 }
 #endif
 
@@ -737,6 +902,16 @@ static struct platform_device_id sdhci_s3c_driver_ids[] = {
 };
 MODULE_DEVICE_TABLE(platform, sdhci_s3c_driver_ids);
 
+#ifdef CONFIG_OF
+static const struct of_device_id sdhci_s3c_dt_match[] = {
+	{ .compatible = "samsung,s3c6410-sdhci", },
+	{ .compatible = "samsung,exynos4210-sdhci",
+		.data = (void *)EXYNOS4_SDHCI_DRV_DATA },
+	{},
+};
+MODULE_DEVICE_TABLE(of, sdhci_s3c_dt_match);
+#endif
+
 static struct platform_driver sdhci_s3c_driver = {
 	.probe		= sdhci_s3c_probe,
 	.remove		= __devexit_p(sdhci_s3c_remove),
@@ -744,6 +919,7 @@ static struct platform_driver sdhci_s3c_driver = {
 	.driver		= {
 		.owner	= THIS_MODULE,
 		.name	= "s3c-sdhci",
+		.of_match_table = of_match_ptr(sdhci_s3c_dt_match),
 		.pm	= SDHCI_S3C_PMOPS,
 	},
 };
diff --git a/drivers/mmc/host/sdhci-spear.c b/drivers/mmc/host/sdhci-spear.c
index 423da8194cd..6be89c032de 100644
--- a/drivers/mmc/host/sdhci-spear.c
+++ b/drivers/mmc/host/sdhci-spear.c
@@ -20,6 +20,8 @@
 #include <linux/module.h>
 #include <linux/interrupt.h>
 #include <linux/irq.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
 #include <linux/platform_device.h>
 #include <linux/pm.h>
 #include <linux/slab.h>
@@ -68,8 +70,42 @@ static irqreturn_t sdhci_gpio_irq(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+#ifdef CONFIG_OF
+static struct sdhci_plat_data * __devinit
+sdhci_probe_config_dt(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct sdhci_plat_data *pdata = NULL;
+	int cd_gpio;
+
+	cd_gpio = of_get_named_gpio(np, "cd-gpios", 0);
+	if (!gpio_is_valid(cd_gpio))
+		cd_gpio = -1;
+
+	/* If pdata is required */
+	if (cd_gpio != -1) {
+		pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+		if (!pdata) {
+			dev_err(&pdev->dev, "DT: kzalloc failed\n");
+			return ERR_PTR(-ENOMEM);
+		}
+	}
+
+	pdata->card_int_gpio = cd_gpio;
+
+	return pdata;
+}
+#else
+static struct sdhci_plat_data * __devinit
+sdhci_probe_config_dt(struct platform_device *pdev)
+{
+	return ERR_PTR(-ENOSYS);
+}
+#endif
+
 static int __devinit sdhci_probe(struct platform_device *pdev)
 {
+	struct device_node *np = pdev->dev.of_node;
 	struct sdhci_host *host;
 	struct resource *iomem;
 	struct spear_sdhci *sdhci;
@@ -104,14 +140,22 @@ static int __devinit sdhci_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	ret = clk_enable(sdhci->clk);
+	ret = clk_prepare_enable(sdhci->clk);
 	if (ret) {
 		dev_dbg(&pdev->dev, "Error enabling clock\n");
 		goto put_clk;
 	}
 
-	/* overwrite platform_data */
-	sdhci->data = dev_get_platdata(&pdev->dev);
+	if (np) {
+		sdhci->data = sdhci_probe_config_dt(pdev);
+		if (IS_ERR(sdhci->data)) {
+			dev_err(&pdev->dev, "DT: Failed to get pdata\n");
+			return -ENODEV;
+		}
+	} else {
+		sdhci->data = dev_get_platdata(&pdev->dev);
+	}
+
 	pdev->dev.platform_data = sdhci;
 
 	if (pdev->dev.parent)
@@ -216,7 +260,7 @@ set_drvdata:
 free_host:
 	sdhci_free_host(host);
 disable_clk:
-	clk_disable(sdhci->clk);
+	clk_disable_unprepare(sdhci->clk);
 put_clk:
 	clk_put(sdhci->clk);
 err:
@@ -238,7 +282,7 @@ static int __devexit sdhci_remove(struct platform_device *pdev)
 
 	sdhci_remove_host(host, dead);
 	sdhci_free_host(host);
-	clk_disable(sdhci->clk);
+	clk_disable_unprepare(sdhci->clk);
 	clk_put(sdhci->clk);
 
 	return 0;
@@ -253,7 +297,7 @@ static int sdhci_suspend(struct device *dev)
 
 	ret = sdhci_suspend_host(host);
 	if (!ret)
-		clk_disable(sdhci->clk);
+		clk_disable_unprepare(sdhci->clk);
 
 	return ret;
 }
@@ -264,7 +308,7 @@ static int sdhci_resume(struct device *dev)
 	struct spear_sdhci *sdhci = dev_get_platdata(dev);
 	int ret;
 
-	ret = clk_enable(sdhci->clk);
+	ret = clk_prepare_enable(sdhci->clk);
 	if (ret) {
 		dev_dbg(dev, "Resume: Error enabling clock\n");
 		return ret;
@@ -276,11 +320,20 @@ static int sdhci_resume(struct device *dev)
 
 static SIMPLE_DEV_PM_OPS(sdhci_pm_ops, sdhci_suspend, sdhci_resume);
 
+#ifdef CONFIG_OF
+static const struct of_device_id sdhci_spear_id_table[] = {
+	{ .compatible = "st,spear300-sdhci" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, sdhci_spear_id_table);
+#endif
+
 static struct platform_driver sdhci_driver = {
 	.driver = {
 		.name	= "sdhci",
 		.owner	= THIS_MODULE,
 		.pm	= &sdhci_pm_ops,
+		.of_match_table = of_match_ptr(sdhci_spear_id_table),
 	},
 	.probe		= sdhci_probe,
 	.remove		= __devexit_p(sdhci_remove),
diff --git a/drivers/mmc/host/sdhci-tegra.c b/drivers/mmc/host/sdhci-tegra.c
index d43e7462941..f9eb9162370 100644
--- a/drivers/mmc/host/sdhci-tegra.c
+++ b/drivers/mmc/host/sdhci-tegra.c
@@ -27,7 +27,6 @@
 
 #include <asm/gpio.h>
 
-#include <mach/gpio-tegra.h>
 #include <linux/platform_data/mmc-sdhci-tegra.h>
 
 #include "sdhci-pltfm.h"
@@ -257,10 +256,9 @@ static int __devinit sdhci_tegra_probe(struct platform_device *pdev)
 	int rc;
 
 	match = of_match_device(sdhci_tegra_dt_match, &pdev->dev);
-	if (match)
-		soc_data = match->data;
-	else
-		soc_data = &soc_data_tegra20;
+	if (!match)
+		return -EINVAL;
+	soc_data = match->data;
 
 	host = sdhci_pltfm_init(pdev, soc_data->pdata);
 	if (IS_ERR(host))
diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index 9a11dc39921..7922adb4238 100644
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -28,6 +28,7 @@
 #include <linux/mmc/mmc.h>
 #include <linux/mmc/host.h>
 #include <linux/mmc/card.h>
+#include <linux/mmc/slot-gpio.h>
 
 #include "sdhci.h"
 
@@ -1293,6 +1294,13 @@ static void sdhci_request(struct mmc_host *mmc, struct mmc_request *mrq)
 		present = sdhci_readl(host, SDHCI_PRESENT_STATE) &
 				SDHCI_CARD_PRESENT;
 
+	/* If we're using a cd-gpio, testing the presence bit might fail. */
+	if (!present) {
+		int ret = mmc_gpio_get_cd(host->mmc);
+		if (ret > 0)
+			present = true;
+	}
+
 	if (!present || host->flags & SDHCI_DEVICE_DEAD) {
 		host->mrq->cmd->error = -ENOMEDIUM;
 		tasklet_schedule(&host->finish_tasklet);
@@ -1597,57 +1605,65 @@ static void sdhci_enable_sdio_irq(struct mmc_host *mmc, int enable)
 	spin_unlock_irqrestore(&host->lock, flags);
 }
 
-static int sdhci_do_start_signal_voltage_switch(struct sdhci_host *host,
-						struct mmc_ios *ios)
+static int sdhci_do_3_3v_signal_voltage_switch(struct sdhci_host *host,
+						u16 ctrl)
 {
-	u8 pwr;
-	u16 clk, ctrl;
-	u32 present_state;
+	int ret;
 
-	/*
-	 * Signal Voltage Switching is only applicable for Host Controllers
-	 * v3.00 and above.
-	 */
-	if (host->version < SDHCI_SPEC_300)
-		return 0;
+	/* Set 1.8V Signal Enable in the Host Control2 register to 0 */
+	ctrl &= ~SDHCI_CTRL_VDD_180;
+	sdhci_writew(host, ctrl, SDHCI_HOST_CONTROL2);
 
-	/*
-	 * We first check whether the request is to set signalling voltage
-	 * to 3.3V. If so, we change the voltage to 3.3V and return quickly.
-	 */
+	if (host->vqmmc) {
+		ret = regulator_set_voltage(host->vqmmc, 3300000, 3300000);
+		if (ret) {
+			pr_warning("%s: Switching to 3.3V signalling voltage "
+				   " failed\n", mmc_hostname(host->mmc));
+			return -EIO;
+		}
+	}
+	/* Wait for 5ms */
+	usleep_range(5000, 5500);
+
+	/* 3.3V regulator output should be stable within 5 ms */
 	ctrl = sdhci_readw(host, SDHCI_HOST_CONTROL2);
-	if (ios->signal_voltage == MMC_SIGNAL_VOLTAGE_330) {
-		/* Set 1.8V Signal Enable in the Host Control2 register to 0 */
-		ctrl &= ~SDHCI_CTRL_VDD_180;
-		sdhci_writew(host, ctrl, SDHCI_HOST_CONTROL2);
+	if (!(ctrl & SDHCI_CTRL_VDD_180))
+		return 0;
 
-		/* Wait for 5ms */
-		usleep_range(5000, 5500);
+	pr_warning("%s: 3.3V regulator output did not became stable\n",
+		   mmc_hostname(host->mmc));
 
-		/* 3.3V regulator output should be stable within 5 ms */
-		ctrl = sdhci_readw(host, SDHCI_HOST_CONTROL2);
-		if (!(ctrl & SDHCI_CTRL_VDD_180))
-			return 0;
-		else {
-			pr_info(DRIVER_NAME ": Switching to 3.3V "
-				"signalling voltage failed\n");
-			return -EIO;
-		}
-	} else if (!(ctrl & SDHCI_CTRL_VDD_180) &&
-		  (ios->signal_voltage == MMC_SIGNAL_VOLTAGE_180)) {
-		/* Stop SDCLK */
-		clk = sdhci_readw(host, SDHCI_CLOCK_CONTROL);
-		clk &= ~SDHCI_CLOCK_CARD_EN;
-		sdhci_writew(host, clk, SDHCI_CLOCK_CONTROL);
+	return -EIO;
+}
 
-		/* Check whether DAT[3:0] is 0000 */
-		present_state = sdhci_readl(host, SDHCI_PRESENT_STATE);
-		if (!((present_state & SDHCI_DATA_LVL_MASK) >>
-		       SDHCI_DATA_LVL_SHIFT)) {
-			/*
-			 * Enable 1.8V Signal Enable in the Host Control2
-			 * register
-			 */
+static int sdhci_do_1_8v_signal_voltage_switch(struct sdhci_host *host,
+						u16 ctrl)
+{
+	u8 pwr;
+	u16 clk;
+	u32 present_state;
+	int ret;
+
+	/* Stop SDCLK */
+	clk = sdhci_readw(host, SDHCI_CLOCK_CONTROL);
+	clk &= ~SDHCI_CLOCK_CARD_EN;
+	sdhci_writew(host, clk, SDHCI_CLOCK_CONTROL);
+
+	/* Check whether DAT[3:0] is 0000 */
+	present_state = sdhci_readl(host, SDHCI_PRESENT_STATE);
+	if (!((present_state & SDHCI_DATA_LVL_MASK) >>
+	       SDHCI_DATA_LVL_SHIFT)) {
+		/*
+		 * Enable 1.8V Signal Enable in the Host Control2
+		 * register
+		 */
+		if (host->vqmmc)
+			ret = regulator_set_voltage(host->vqmmc,
+				1800000, 1800000);
+		else
+			ret = 0;
+
+		if (!ret) {
 			ctrl |= SDHCI_CTRL_VDD_180;
 			sdhci_writew(host, ctrl, SDHCI_HOST_CONTROL2);
 
@@ -1656,7 +1672,7 @@ static int sdhci_do_start_signal_voltage_switch(struct sdhci_host *host,
 
 			ctrl = sdhci_readw(host, SDHCI_HOST_CONTROL2);
 			if (ctrl & SDHCI_CTRL_VDD_180) {
-				/* Provide SDCLK again and wait for 1ms*/
+				/* Provide SDCLK again and wait for 1ms */
 				clk = sdhci_readw(host, SDHCI_CLOCK_CONTROL);
 				clk |= SDHCI_CLOCK_CARD_EN;
 				sdhci_writew(host, clk, SDHCI_CLOCK_CONTROL);
@@ -1673,29 +1689,55 @@ static int sdhci_do_start_signal_voltage_switch(struct sdhci_host *host,
 					return 0;
 			}
 		}
+	}
 
-		/*
-		 * If we are here, that means the switch to 1.8V signaling
-		 * failed. We power cycle the card, and retry initialization
-		 * sequence by setting S18R to 0.
-		 */
-		pwr = sdhci_readb(host, SDHCI_POWER_CONTROL);
-		pwr &= ~SDHCI_POWER_ON;
-		sdhci_writeb(host, pwr, SDHCI_POWER_CONTROL);
-		if (host->vmmc)
-			regulator_disable(host->vmmc);
+	/*
+	 * If we are here, that means the switch to 1.8V signaling
+	 * failed. We power cycle the card, and retry initialization
+	 * sequence by setting S18R to 0.
+	 */
+	pwr = sdhci_readb(host, SDHCI_POWER_CONTROL);
+	pwr &= ~SDHCI_POWER_ON;
+	sdhci_writeb(host, pwr, SDHCI_POWER_CONTROL);
+	if (host->vmmc)
+		regulator_disable(host->vmmc);
 
-		/* Wait for 1ms as per the spec */
-		usleep_range(1000, 1500);
-		pwr |= SDHCI_POWER_ON;
-		sdhci_writeb(host, pwr, SDHCI_POWER_CONTROL);
-		if (host->vmmc)
-			regulator_enable(host->vmmc);
+	/* Wait for 1ms as per the spec */
+	usleep_range(1000, 1500);
+	pwr |= SDHCI_POWER_ON;
+	sdhci_writeb(host, pwr, SDHCI_POWER_CONTROL);
+	if (host->vmmc)
+		regulator_enable(host->vmmc);
 
-		pr_info(DRIVER_NAME ": Switching to 1.8V signalling "
-			"voltage failed, retrying with S18R set to 0\n");
-		return -EAGAIN;
-	} else
+	pr_warning("%s: Switching to 1.8V signalling voltage failed, "
+		   "retrying with S18R set to 0\n", mmc_hostname(host->mmc));
+
+	return -EAGAIN;
+}
+
+static int sdhci_do_start_signal_voltage_switch(struct sdhci_host *host,
+						struct mmc_ios *ios)
+{
+	u16 ctrl;
+
+	/*
+	 * Signal Voltage Switching is only applicable for Host Controllers
+	 * v3.00 and above.
+	 */
+	if (host->version < SDHCI_SPEC_300)
+		return 0;
+
+	/*
+	 * We first check whether the request is to set signalling voltage
+	 * to 3.3V. If so, we change the voltage to 3.3V and return quickly.
+	 */
+	ctrl = sdhci_readw(host, SDHCI_HOST_CONTROL2);
+	if (ios->signal_voltage == MMC_SIGNAL_VOLTAGE_330)
+		return sdhci_do_3_3v_signal_voltage_switch(host, ctrl);
+	else if (!(ctrl & SDHCI_CTRL_VDD_180) &&
+			(ios->signal_voltage == MMC_SIGNAL_VOLTAGE_180))
+		return sdhci_do_1_8v_signal_voltage_switch(host, ctrl);
+	else
 		/* No signal voltage switch required */
 		return 0;
 }
@@ -2802,6 +2844,18 @@ int sdhci_add_host(struct sdhci_host *host)
 	    !(host->mmc->caps & MMC_CAP_NONREMOVABLE))
 		mmc->caps |= MMC_CAP_NEEDS_POLL;
 
+	/* If vqmmc regulator and no 1.8V signalling, then there's no UHS */
+	host->vqmmc = regulator_get(mmc_dev(mmc), "vqmmc");
+	if (IS_ERR(host->vqmmc)) {
+		pr_info("%s: no vqmmc regulator found\n", mmc_hostname(mmc));
+		host->vqmmc = NULL;
+	}
+	else if (regulator_is_supported_voltage(host->vqmmc, 1800000, 1800000))
+		regulator_enable(host->vqmmc);
+	else
+		caps[1] &= ~(SDHCI_SUPPORT_SDR104 | SDHCI_SUPPORT_SDR50 |
+		       SDHCI_SUPPORT_DDR50);
+
 	/* Any UHS-I mode in caps implies SDR12 and SDR25 support. */
 	if (caps[1] & (SDHCI_SUPPORT_SDR104 | SDHCI_SUPPORT_SDR50 |
 		       SDHCI_SUPPORT_DDR50))
@@ -2832,15 +2886,6 @@ int sdhci_add_host(struct sdhci_host *host)
 	if (caps[1] & SDHCI_DRIVER_TYPE_D)
 		mmc->caps |= MMC_CAP_DRIVER_TYPE_D;
 
-	/*
-	 * If Power Off Notify capability is enabled by the host,
-	 * set notify to short power off notify timeout value.
-	 */
-	if (mmc->caps2 & MMC_CAP2_POWEROFF_NOTIFY)
-		mmc->power_notify_type = MMC_HOST_PW_NOTIFY_SHORT;
-	else
-		mmc->power_notify_type = MMC_HOST_PW_NOTIFY_NONE;
-
 	/* Initial value for re-tuning timer count */
 	host->tuning_count = (caps[1] & SDHCI_RETUNING_TIMER_COUNT_MASK) >>
 			      SDHCI_RETUNING_TIMER_COUNT_SHIFT;
@@ -2862,7 +2907,8 @@ int sdhci_add_host(struct sdhci_host *host)
 	if (IS_ERR(host->vmmc)) {
 		pr_info("%s: no vmmc regulator found\n", mmc_hostname(mmc));
 		host->vmmc = NULL;
-	}
+	} else
+		regulator_enable(host->vmmc);
 
 #ifdef CONFIG_REGULATOR
 	if (host->vmmc) {
@@ -3119,8 +3165,15 @@ void sdhci_remove_host(struct sdhci_host *host, int dead)
 	tasklet_kill(&host->card_tasklet);
 	tasklet_kill(&host->finish_tasklet);
 
-	if (host->vmmc)
+	if (host->vmmc) {
+		regulator_disable(host->vmmc);
 		regulator_put(host->vmmc);
+	}
+
+	if (host->vqmmc) {
+		regulator_disable(host->vqmmc);
+		regulator_put(host->vqmmc);
+	}
 
 	kfree(host->adma_desc);
 	kfree(host->align_buffer);
diff --git a/drivers/mmc/host/sh_mmcif.c b/drivers/mmc/host/sh_mmcif.c
index 5d8142773fa..11d2bc3b51d 100644
--- a/drivers/mmc/host/sh_mmcif.c
+++ b/drivers/mmc/host/sh_mmcif.c
@@ -1213,7 +1213,9 @@ static irqreturn_t sh_mmcif_intr(int irq, void *dev_id)
 		sh_mmcif_writel(host->addr, MMCIF_CE_INT, ~INT_BUFRE);
 		sh_mmcif_bitclr(host, MMCIF_CE_INT_MASK, MASK_MBUFRE);
 	} else if (state & INT_DTRANE) {
-		sh_mmcif_writel(host->addr, MMCIF_CE_INT, ~INT_DTRANE);
+		sh_mmcif_writel(host->addr, MMCIF_CE_INT,
+			~(INT_CMD12DRE | INT_CMD12RBE |
+			  INT_CMD12CRE | INT_DTRANE));
 		sh_mmcif_bitclr(host, MMCIF_CE_INT_MASK, MASK_MDTRANE);
 	} else if (state & INT_CMD12RBE) {
 		sh_mmcif_writel(host->addr, MMCIF_CE_INT,
@@ -1229,6 +1231,10 @@ static irqreturn_t sh_mmcif_intr(int irq, void *dev_id)
 		host->sd_error = true;
 		dev_dbg(&host->pd->dev, "int err state = %08x\n", state);
 	}
+	if (host->state == STATE_IDLE) {
+		dev_info(&host->pd->dev, "Spurious IRQ status 0x%x", state);
+		return IRQ_HANDLED;
+	}
 	if (state & ~(INT_CMD12RBE | INT_CMD12CRE)) {
 		if (!host->dma_active)
 			return IRQ_WAKE_THREAD;
diff --git a/drivers/mmc/host/via-sdmmc.c b/drivers/mmc/host/via-sdmmc.c
index 4b83c43f950..f18becef156 100644
--- a/drivers/mmc/host/via-sdmmc.c
+++ b/drivers/mmc/host/via-sdmmc.c
@@ -1337,21 +1337,7 @@ static struct pci_driver via_sd_driver = {
 	.resume = via_sd_resume,
 };
 
-static int __init via_sd_drv_init(void)
-{
-	pr_info(DRV_NAME ": VIA SD/MMC Card Reader driver "
-		"(C) 2008 VIA Technologies, Inc.\n");
-
-	return pci_register_driver(&via_sd_driver);
-}
-
-static void __exit via_sd_drv_exit(void)
-{
-	pci_unregister_driver(&via_sd_driver);
-}
-
-module_init(via_sd_drv_init);
-module_exit(via_sd_drv_exit);
+module_pci_driver(via_sd_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("VIA Technologies Inc.");
diff --git a/drivers/mmc/host/vub300.c b/drivers/mmc/host/vub300.c
index 58eab9ac1d0..d5655a63eda 100644
--- a/drivers/mmc/host/vub300.c
+++ b/drivers/mmc/host/vub300.c
@@ -2358,9 +2358,9 @@ error5:
 	 * which is contained at the end of struct mmc
 	 */
 error4:
-	usb_free_urb(command_out_urb);
-error1:
 	usb_free_urb(command_res_urb);
+error1:
+	usb_free_urb(command_out_urb);
 error0:
 	return retval;
 }
diff --git a/drivers/mtd/Kconfig b/drivers/mtd/Kconfig
index 27143e042af..73fcbbeb78d 100644
--- a/drivers/mtd/Kconfig
+++ b/drivers/mtd/Kconfig
@@ -148,6 +148,13 @@ config MTD_BCM63XX_PARTS
 	  This provides partions parsing for BCM63xx devices with CFE
 	  bootloaders.
 
+config MTD_BCM47XX_PARTS
+	tristate "BCM47XX partitioning support"
+	depends on BCM47XX
+	help
+	  This provides partitions parser for devices based on BCM47xx
+	  boards.
+
 comment "User Modules And Translation Layers"
 
 config MTD_CHAR
diff --git a/drivers/mtd/Makefile b/drivers/mtd/Makefile
index f90135429dc..18a38e55b2f 100644
--- a/drivers/mtd/Makefile
+++ b/drivers/mtd/Makefile
@@ -12,6 +12,7 @@ obj-$(CONFIG_MTD_CMDLINE_PARTS) += cmdlinepart.o
 obj-$(CONFIG_MTD_AFS_PARTS)	+= afs.o
 obj-$(CONFIG_MTD_AR7_PARTS)	+= ar7part.o
 obj-$(CONFIG_MTD_BCM63XX_PARTS)	+= bcm63xxpart.o
+obj-$(CONFIG_MTD_BCM47XX_PARTS)	+= bcm47xxpart.o
 
 # 'Users' - code which presents functionality to userspace.
 obj-$(CONFIG_MTD_CHAR)		+= mtdchar.o
diff --git a/drivers/mtd/bcm47xxpart.c b/drivers/mtd/bcm47xxpart.c
new file mode 100644
index 00000000000..e06d782489a
--- /dev/null
+++ b/drivers/mtd/bcm47xxpart.c
@@ -0,0 +1,202 @@
+/*
+ * BCM47XX MTD partitioning
+ *
+ * Copyright © 2012 Rafał Miłecki <zajec5@gmail.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/module.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/partitions.h>
+#include <asm/mach-bcm47xx/nvram.h>
+
+/* 10 parts were found on sflash on Netgear WNDR4500 */
+#define BCM47XXPART_MAX_PARTS		12
+
+/*
+ * Amount of bytes we read when analyzing each block of flash memory.
+ * Set it big enough to allow detecting partition and reading important data.
+ */
+#define BCM47XXPART_BYTES_TO_READ	0x404
+
+/* Magics */
+#define BOARD_DATA_MAGIC		0x5246504D	/* MPFR */
+#define POT_MAGIC1			0x54544f50	/* POTT */
+#define POT_MAGIC2			0x504f		/* OP */
+#define ML_MAGIC1			0x39685a42
+#define ML_MAGIC2			0x26594131
+#define TRX_MAGIC			0x30524448
+
+struct trx_header {
+	uint32_t magic;
+	uint32_t length;
+	uint32_t crc32;
+	uint16_t flags;
+	uint16_t version;
+	uint32_t offset[3];
+} __packed;
+
+static void bcm47xxpart_add_part(struct mtd_partition *part, char *name,
+				 u64 offset, uint32_t mask_flags)
+{
+	part->name = name;
+	part->offset = offset;
+	part->mask_flags = mask_flags;
+}
+
+static int bcm47xxpart_parse(struct mtd_info *master,
+			     struct mtd_partition **pparts,
+			     struct mtd_part_parser_data *data)
+{
+	struct mtd_partition *parts;
+	uint8_t i, curr_part = 0;
+	uint32_t *buf;
+	size_t bytes_read;
+	uint32_t offset;
+	uint32_t blocksize = 0x10000;
+	struct trx_header *trx;
+
+	/* Alloc */
+	parts = kzalloc(sizeof(struct mtd_partition) * BCM47XXPART_MAX_PARTS,
+			GFP_KERNEL);
+	buf = kzalloc(BCM47XXPART_BYTES_TO_READ, GFP_KERNEL);
+
+	/* Parse block by block looking for magics */
+	for (offset = 0; offset <= master->size - blocksize;
+	     offset += blocksize) {
+		/* Nothing more in higher memory */
+		if (offset >= 0x2000000)
+			break;
+
+		if (curr_part > BCM47XXPART_MAX_PARTS) {
+			pr_warn("Reached maximum number of partitions, scanning stopped!\n");
+			break;
+		}
+
+		/* Read beginning of the block */
+		if (mtd_read(master, offset, BCM47XXPART_BYTES_TO_READ,
+			     &bytes_read, (uint8_t *)buf) < 0) {
+			pr_err("mtd_read error while parsing (offset: 0x%X)!\n",
+			       offset);
+			continue;
+		}
+
+		/* CFE has small NVRAM at 0x400 */
+		if (buf[0x400 / 4] == NVRAM_HEADER) {
+			bcm47xxpart_add_part(&parts[curr_part++], "boot",
+					     offset, MTD_WRITEABLE);
+			continue;
+		}
+
+		/* Standard NVRAM */
+		if (buf[0x000 / 4] == NVRAM_HEADER) {
+			bcm47xxpart_add_part(&parts[curr_part++], "nvram",
+					     offset, 0);
+			continue;
+		}
+
+		/*
+		 * board_data starts with board_id which differs across boards,
+		 * but we can use 'MPFR' (hopefully) magic at 0x100
+		 */
+		if (buf[0x100 / 4] == BOARD_DATA_MAGIC) {
+			bcm47xxpart_add_part(&parts[curr_part++], "board_data",
+					     offset, MTD_WRITEABLE);
+			continue;
+		}
+
+		/* POT(TOP) */
+		if (buf[0x000 / 4] == POT_MAGIC1 &&
+		    (buf[0x004 / 4] & 0xFFFF) == POT_MAGIC2) {
+			bcm47xxpart_add_part(&parts[curr_part++], "POT", offset,
+					     MTD_WRITEABLE);
+			continue;
+		}
+
+		/* ML */
+		if (buf[0x010 / 4] == ML_MAGIC1 &&
+		    buf[0x014 / 4] == ML_MAGIC2) {
+			bcm47xxpart_add_part(&parts[curr_part++], "ML", offset,
+					     MTD_WRITEABLE);
+			continue;
+		}
+
+		/* TRX */
+		if (buf[0x000 / 4] == TRX_MAGIC) {
+			trx = (struct trx_header *)buf;
+
+			i = 0;
+			/* We have LZMA loader if offset[2] points to sth */
+			if (trx->offset[2]) {
+				bcm47xxpart_add_part(&parts[curr_part++],
+						     "loader",
+						     offset + trx->offset[i],
+						     0);
+				i++;
+			}
+
+			bcm47xxpart_add_part(&parts[curr_part++], "linux",
+					     offset + trx->offset[i], 0);
+			i++;
+
+			/*
+			 * Pure rootfs size is known and can be calculated as:
+			 * trx->length - trx->offset[i]. We don't fill it as
+			 * we want to have jffs2 (overlay) in the same mtd.
+			 */
+			bcm47xxpart_add_part(&parts[curr_part++], "rootfs",
+					     offset + trx->offset[i], 0);
+			i++;
+
+			/*
+			 * We have whole TRX scanned, skip to the next part. Use
+			 * roundown (not roundup), as the loop will increase
+			 * offset in next step.
+			 */
+			offset = rounddown(offset + trx->length, blocksize);
+			continue;
+		}
+	}
+	kfree(buf);
+
+	/*
+	 * Assume that partitions end at the beginning of the one they are
+	 * followed by.
+	 */
+	for (i = 0; i < curr_part - 1; i++)
+		parts[i].size = parts[i + 1].offset - parts[i].offset;
+	if (curr_part > 0)
+		parts[curr_part - 1].size =
+				master->size - parts[curr_part - 1].offset;
+
+	*pparts = parts;
+	return curr_part;
+};
+
+static struct mtd_part_parser bcm47xxpart_mtd_parser = {
+	.owner = THIS_MODULE,
+	.parse_fn = bcm47xxpart_parse,
+	.name = "bcm47xxpart",
+};
+
+static int __init bcm47xxpart_init(void)
+{
+	return register_mtd_parser(&bcm47xxpart_mtd_parser);
+}
+
+static void __exit bcm47xxpart_exit(void)
+{
+	deregister_mtd_parser(&bcm47xxpart_mtd_parser);
+}
+
+module_init(bcm47xxpart_init);
+module_exit(bcm47xxpart_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("MTD partitioning for BCM47XX flash memories");
diff --git a/drivers/mtd/chips/Kconfig b/drivers/mtd/chips/Kconfig
index b1e3c26edd6..e469b01d40d 100644
--- a/drivers/mtd/chips/Kconfig
+++ b/drivers/mtd/chips/Kconfig
@@ -43,9 +43,6 @@ choice
 	prompt "Flash cmd/query data swapping"
 	depends on MTD_CFI_ADV_OPTIONS
 	default MTD_CFI_NOSWAP
-
-config MTD_CFI_NOSWAP
-	bool "NO"
 	---help---
 	  This option defines the way in which the CPU attempts to arrange
 	  data bits when writing the 'magic' commands to the chips. Saying
@@ -55,12 +52,8 @@ config MTD_CFI_NOSWAP
 	  Specific arrangements are possible with the BIG_ENDIAN_BYTE and
 	  LITTLE_ENDIAN_BYTE, if the bytes are reversed.
 
-	  If you have a LART, on which the data (and address) lines were
-	  connected in a fashion which ensured that the nets were as short
-	  as possible, resulting in a bit-shuffling which seems utterly
-	  random to the untrained eye, you need the LART_ENDIAN_BYTE option.
-
-	  Yes, there really exists something sicker than PDP-endian :)
+config MTD_CFI_NOSWAP
+	bool "NO"
 
 config MTD_CFI_BE_BYTE_SWAP
 	bool "BIG_ENDIAN_BYTE"
diff --git a/drivers/mtd/chips/cfi_cmdset_0001.c b/drivers/mtd/chips/cfi_cmdset_0001.c
index dbbd2edfb81..77514430f1f 100644
--- a/drivers/mtd/chips/cfi_cmdset_0001.c
+++ b/drivers/mtd/chips/cfi_cmdset_0001.c
@@ -2043,7 +2043,7 @@ static int __xipram do_xxlock_oneblock(struct map_info *map, struct flchip *chip
 {
 	struct cfi_private *cfi = map->fldrv_priv;
 	struct cfi_pri_intelext *extp = cfi->cmdset_priv;
-	int udelay;
+	int mdelay;
 	int ret;
 
 	adr += chip->start;
@@ -2072,9 +2072,17 @@ static int __xipram do_xxlock_oneblock(struct map_info *map, struct flchip *chip
 	 * If Instant Individual Block Locking supported then no need
 	 * to delay.
 	 */
-	udelay = (!extp || !(extp->FeatureSupport & (1 << 5))) ? 1000000/HZ : 0;
+	/*
+	 * Unlocking may take up to 1.4 seconds on some Intel flashes. So
+	 * lets use a max of 1.5 seconds (1500ms) as timeout.
+	 *
+	 * See "Clear Block Lock-Bits Time" on page 40 in
+	 * "3 Volt Intel StrataFlash Memory" 28F128J3,28F640J3,28F320J3 manual
+	 * from February 2003
+	 */
+	mdelay = (!extp || !(extp->FeatureSupport & (1 << 5))) ? 1500 : 0;
 
-	ret = WAIT_TIMEOUT(map, chip, adr, udelay, udelay * 100);
+	ret = WAIT_TIMEOUT(map, chip, adr, mdelay, mdelay * 1000);
 	if (ret) {
 		map_write(map, CMD(0x70), adr);
 		chip->state = FL_STATUS;
diff --git a/drivers/mtd/chips/cfi_cmdset_0002.c b/drivers/mtd/chips/cfi_cmdset_0002.c
index 22d0493a026..5ff5c4a1694 100644
--- a/drivers/mtd/chips/cfi_cmdset_0002.c
+++ b/drivers/mtd/chips/cfi_cmdset_0002.c
@@ -431,6 +431,68 @@ static void cfi_fixup_major_minor(struct cfi_private *cfi,
 	}
 }
 
+static int is_m29ew(struct cfi_private *cfi)
+{
+	if (cfi->mfr == CFI_MFR_INTEL &&
+	    ((cfi->device_type == CFI_DEVICETYPE_X8 && (cfi->id & 0xff) == 0x7e) ||
+	     (cfi->device_type == CFI_DEVICETYPE_X16 && cfi->id == 0x227e)))
+		return 1;
+	return 0;
+}
+
+/*
+ * From TN-13-07: Patching the Linux Kernel and U-Boot for M29 Flash, page 20:
+ * Some revisions of the M29EW suffer from erase suspend hang ups. In
+ * particular, it can occur when the sequence
+ * Erase Confirm -> Suspend -> Program -> Resume
+ * causes a lockup due to internal timing issues. The consequence is that the
+ * erase cannot be resumed without inserting a dummy command after programming
+ * and prior to resuming. [...] The work-around is to issue a dummy write cycle
+ * that writes an F0 command code before the RESUME command.
+ */
+static void cfi_fixup_m29ew_erase_suspend(struct map_info *map,
+					  unsigned long adr)
+{
+	struct cfi_private *cfi = map->fldrv_priv;
+	/* before resume, insert a dummy 0xF0 cycle for Micron M29EW devices */
+	if (is_m29ew(cfi))
+		map_write(map, CMD(0xF0), adr);
+}
+
+/*
+ * From TN-13-07: Patching the Linux Kernel and U-Boot for M29 Flash, page 22:
+ *
+ * Some revisions of the M29EW (for example, A1 and A2 step revisions)
+ * are affected by a problem that could cause a hang up when an ERASE SUSPEND
+ * command is issued after an ERASE RESUME operation without waiting for a
+ * minimum delay.  The result is that once the ERASE seems to be completed
+ * (no bits are toggling), the contents of the Flash memory block on which
+ * the erase was ongoing could be inconsistent with the expected values
+ * (typically, the array value is stuck to the 0xC0, 0xC4, 0x80, or 0x84
+ * values), causing a consequent failure of the ERASE operation.
+ * The occurrence of this issue could be high, especially when file system
+ * operations on the Flash are intensive.  As a result, it is recommended
+ * that a patch be applied.  Intensive file system operations can cause many
+ * calls to the garbage routine to free Flash space (also by erasing physical
+ * Flash blocks) and as a result, many consecutive SUSPEND and RESUME
+ * commands can occur.  The problem disappears when a delay is inserted after
+ * the RESUME command by using the udelay() function available in Linux.
+ * The DELAY value must be tuned based on the customer's platform.
+ * The maximum value that fixes the problem in all cases is 500us.
+ * But, in our experience, a delay of 30 µs to 50 µs is sufficient
+ * in most cases.
+ * We have chosen 500µs because this latency is acceptable.
+ */
+static void cfi_fixup_m29ew_delay_after_resume(struct cfi_private *cfi)
+{
+	/*
+	 * Resolving the Delay After Resume Issue see Micron TN-13-07
+	 * Worst case delay must be 500µs but 30-50µs should be ok as well
+	 */
+	if (is_m29ew(cfi))
+		cfi_udelay(500);
+}
+
 struct mtd_info *cfi_cmdset_0002(struct map_info *map, int primary)
 {
 	struct cfi_private *cfi = map->fldrv_priv;
@@ -776,7 +838,10 @@ static void put_chip(struct map_info *map, struct flchip *chip, unsigned long ad
 
 	switch(chip->oldstate) {
 	case FL_ERASING:
+		cfi_fixup_m29ew_erase_suspend(map,
+			chip->in_progress_block_addr);
 		map_write(map, cfi->sector_erase_cmd, chip->in_progress_block_addr);
+		cfi_fixup_m29ew_delay_after_resume(cfi);
 		chip->oldstate = FL_READY;
 		chip->state = FL_ERASING;
 		break;
@@ -916,6 +981,8 @@ static void __xipram xip_udelay(struct map_info *map, struct flchip *chip,
 			/* Disallow XIP again */
 			local_irq_disable();
 
+			/* Correct Erase Suspend Hangups for M29EW */
+			cfi_fixup_m29ew_erase_suspend(map, adr);
 			/* Resume the write or erase operation */
 			map_write(map, cfi->sector_erase_cmd, adr);
 			chip->state = oldstate;
diff --git a/drivers/mtd/cmdlinepart.c b/drivers/mtd/cmdlinepart.c
index 4558e0f4d07..aed1b8a63c9 100644
--- a/drivers/mtd/cmdlinepart.c
+++ b/drivers/mtd/cmdlinepart.c
@@ -39,11 +39,10 @@
 
 #include <linux/kernel.h>
 #include <linux/slab.h>
-
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/partitions.h>
-#include <linux/bootmem.h>
 #include <linux/module.h>
+#include <linux/err.h>
 
 /* error message prefix */
 #define ERRP "mtd: "
@@ -72,7 +71,7 @@ static struct cmdline_mtd_partition *partitions;
 
 /* the command line passed to mtdpart_setup() */
 static char *cmdline;
-static int cmdline_parsed = 0;
+static int cmdline_parsed;
 
 /*
  * Parse one partition definition for an MTD. Since there can be many
@@ -83,15 +82,14 @@ static int cmdline_parsed = 0;
  * syntax has been verified ok.
  */
 static struct mtd_partition * newpart(char *s,
-                                      char **retptr,
-                                      int *num_parts,
-                                      int this_part,
-                                      unsigned char **extra_mem_ptr,
-                                      int extra_mem_size)
+				      char **retptr,
+				      int *num_parts,
+				      int this_part,
+				      unsigned char **extra_mem_ptr,
+				      int extra_mem_size)
 {
 	struct mtd_partition *parts;
-	unsigned long size;
-	unsigned long offset = OFFSET_CONTINUOUS;
+	unsigned long size, offset = OFFSET_CONTINUOUS;
 	char *name;
 	int name_len;
 	unsigned char *extra_mem;
@@ -99,124 +97,106 @@ static struct mtd_partition * newpart(char *s,
 	unsigned int mask_flags;
 
 	/* fetch the partition size */
-	if (*s == '-')
-	{	/* assign all remaining space to this partition */
+	if (*s == '-') {
+		/* assign all remaining space to this partition */
 		size = SIZE_REMAINING;
 		s++;
-	}
-	else
-	{
+	} else {
 		size = memparse(s, &s);
-		if (size < PAGE_SIZE)
-		{
+		if (size < PAGE_SIZE) {
 			printk(KERN_ERR ERRP "partition size too small (%lx)\n", size);
-			return NULL;
+			return ERR_PTR(-EINVAL);
 		}
 	}
 
 	/* fetch partition name and flags */
 	mask_flags = 0; /* this is going to be a regular partition */
 	delim = 0;
-        /* check for offset */
-        if (*s == '@')
-	{
-                s++;
-                offset = memparse(s, &s);
-        }
-        /* now look for name */
+
+	/* check for offset */
+	if (*s == '@') {
+		s++;
+		offset = memparse(s, &s);
+	}
+
+	/* now look for name */
 	if (*s == '(')
-	{
 		delim = ')';
-	}
 
-	if (delim)
-	{
+	if (delim) {
 		char *p;
 
-	    	name = ++s;
+		name = ++s;
 		p = strchr(name, delim);
-		if (!p)
-		{
+		if (!p) {
 			printk(KERN_ERR ERRP "no closing %c found in partition name\n", delim);
-			return NULL;
+			return ERR_PTR(-EINVAL);
 		}
 		name_len = p - name;
 		s = p + 1;
-	}
-	else
-	{
-	    	name = NULL;
+	} else {
+		name = NULL;
 		name_len = 13; /* Partition_000 */
 	}
 
 	/* record name length for memory allocation later */
 	extra_mem_size += name_len + 1;
 
-        /* test for options */
-        if (strncmp(s, "ro", 2) == 0)
-	{
+	/* test for options */
+	if (strncmp(s, "ro", 2) == 0) {
 		mask_flags |= MTD_WRITEABLE;
 		s += 2;
-        }
+	}
 
-        /* if lk is found do NOT unlock the MTD partition*/
-        if (strncmp(s, "lk", 2) == 0)
-	{
+	/* if lk is found do NOT unlock the MTD partition*/
+	if (strncmp(s, "lk", 2) == 0) {
 		mask_flags |= MTD_POWERUP_LOCK;
 		s += 2;
-        }
+	}
 
 	/* test if more partitions are following */
-	if (*s == ',')
-	{
-		if (size == SIZE_REMAINING)
-		{
+	if (*s == ',') {
+		if (size == SIZE_REMAINING) {
 			printk(KERN_ERR ERRP "no partitions allowed after a fill-up partition\n");
-			return NULL;
+			return ERR_PTR(-EINVAL);
 		}
 		/* more partitions follow, parse them */
 		parts = newpart(s + 1, &s, num_parts, this_part + 1,
 				&extra_mem, extra_mem_size);
-		if (!parts)
-			return NULL;
-	}
-	else
-	{	/* this is the last partition: allocate space for all */
+		if (IS_ERR(parts))
+			return parts;
+	} else {
+		/* this is the last partition: allocate space for all */
 		int alloc_size;
 
 		*num_parts = this_part + 1;
 		alloc_size = *num_parts * sizeof(struct mtd_partition) +
 			     extra_mem_size;
+
 		parts = kzalloc(alloc_size, GFP_KERNEL);
 		if (!parts)
-			return NULL;
+			return ERR_PTR(-ENOMEM);
 		extra_mem = (unsigned char *)(parts + *num_parts);
 	}
+
 	/* enter this partition (offset will be calculated later if it is zero at this point) */
 	parts[this_part].size = size;
 	parts[this_part].offset = offset;
 	parts[this_part].mask_flags = mask_flags;
 	if (name)
-	{
 		strlcpy(extra_mem, name, name_len + 1);
-	}
 	else
-	{
 		sprintf(extra_mem, "Partition_%03d", this_part);
-	}
 	parts[this_part].name = extra_mem;
 	extra_mem += name_len + 1;
 
 	dbg(("partition %d: name <%s>, offset %llx, size %llx, mask flags %x\n",
-	     this_part,
-	     parts[this_part].name,
-	     parts[this_part].offset,
-	     parts[this_part].size,
-	     parts[this_part].mask_flags));
+	     this_part, parts[this_part].name, parts[this_part].offset,
+	     parts[this_part].size, parts[this_part].mask_flags));
 
 	/* return (updated) pointer to extra_mem memory */
 	if (extra_mem_ptr)
-	  *extra_mem_ptr = extra_mem;
+		*extra_mem_ptr = extra_mem;
 
 	/* return (updated) pointer command line string */
 	*retptr = s;
@@ -236,16 +216,16 @@ static int mtdpart_setup_real(char *s)
 	{
 		struct cmdline_mtd_partition *this_mtd;
 		struct mtd_partition *parts;
-	    	int mtd_id_len;
-		int num_parts;
+		int mtd_id_len, num_parts;
 		char *p, *mtd_id;
 
-	    	mtd_id = s;
+		mtd_id = s;
+
 		/* fetch <mtd-id> */
-		if (!(p = strchr(s, ':')))
-		{
+		p = strchr(s, ':');
+		if (!p) {
 			printk(KERN_ERR ERRP "no mtd-id\n");
-			return 0;
+			return -EINVAL;
 		}
 		mtd_id_len = p - mtd_id;
 
@@ -262,8 +242,7 @@ static int mtdpart_setup_real(char *s)
 				(unsigned char**)&this_mtd, /* out: extra mem */
 				mtd_id_len + 1 + sizeof(*this_mtd) +
 				sizeof(void*)-1 /*alignment*/);
-		if(!parts)
-		{
+		if (IS_ERR(parts)) {
 			/*
 			 * An error occurred. We're either:
 			 * a) out of memory, or
@@ -271,12 +250,12 @@ static int mtdpart_setup_real(char *s)
 			 * Either way, this mtd is hosed and we're
 			 * unlikely to succeed in parsing any more
 			 */
-			 return 0;
+			 return PTR_ERR(parts);
 		 }
 
 		/* align this_mtd */
 		this_mtd = (struct cmdline_mtd_partition *)
-			ALIGN((unsigned long)this_mtd, sizeof(void*));
+				ALIGN((unsigned long)this_mtd, sizeof(void *));
 		/* enter results */
 		this_mtd->parts = parts;
 		this_mtd->num_parts = num_parts;
@@ -296,14 +275,14 @@ static int mtdpart_setup_real(char *s)
 			break;
 
 		/* does another spec follow? */
-		if (*s != ';')
-		{
+		if (*s != ';') {
 			printk(KERN_ERR ERRP "bad character after partition (%c)\n", *s);
-			return 0;
+			return -EINVAL;
 		}
 		s++;
 	}
-	return 1;
+
+	return 0;
 }
 
 /*
@@ -318,44 +297,58 @@ static int parse_cmdline_partitions(struct mtd_info *master,
 				    struct mtd_part_parser_data *data)
 {
 	unsigned long offset;
-	int i;
+	int i, err;
 	struct cmdline_mtd_partition *part;
 	const char *mtd_id = master->name;
 
 	/* parse command line */
-	if (!cmdline_parsed)
-		mtdpart_setup_real(cmdline);
+	if (!cmdline_parsed) {
+		err = mtdpart_setup_real(cmdline);
+		if (err)
+			return err;
+	}
 
-	for(part = partitions; part; part = part->next)
-	{
-		if ((!mtd_id) || (!strcmp(part->mtd_id, mtd_id)))
-		{
-			for(i = 0, offset = 0; i < part->num_parts; i++)
-			{
+	for (part = partitions; part; part = part->next) {
+		if ((!mtd_id) || (!strcmp(part->mtd_id, mtd_id))) {
+			for (i = 0, offset = 0; i < part->num_parts; i++) {
 				if (part->parts[i].offset == OFFSET_CONTINUOUS)
-				  part->parts[i].offset = offset;
+					part->parts[i].offset = offset;
 				else
-				  offset = part->parts[i].offset;
+					offset = part->parts[i].offset;
+
 				if (part->parts[i].size == SIZE_REMAINING)
-				  part->parts[i].size = master->size - offset;
-				if (offset + part->parts[i].size > master->size)
-				{
+					part->parts[i].size = master->size - offset;
+
+				if (part->parts[i].size == 0) {
+					printk(KERN_WARNING ERRP
+					       "%s: skipping zero sized partition\n",
+					       part->mtd_id);
+					part->num_parts--;
+					memmove(&part->parts[i],
+						&part->parts[i + 1],
+						sizeof(*part->parts) * (part->num_parts - i));
+					continue;
+				}
+
+				if (offset + part->parts[i].size > master->size) {
 					printk(KERN_WARNING ERRP
 					       "%s: partitioning exceeds flash size, truncating\n",
 					       part->mtd_id);
 					part->parts[i].size = master->size - offset;
-					part->num_parts = i;
 				}
 				offset += part->parts[i].size;
 			}
+
 			*pparts = kmemdup(part->parts,
 					sizeof(*part->parts) * part->num_parts,
 					GFP_KERNEL);
 			if (!*pparts)
 				return -ENOMEM;
+
 			return part->num_parts;
 		}
 	}
+
 	return 0;
 }
 
diff --git a/drivers/mtd/devices/Kconfig b/drivers/mtd/devices/Kconfig
index 4cdb2af7bf4..27f80cd8aef 100644
--- a/drivers/mtd/devices/Kconfig
+++ b/drivers/mtd/devices/Kconfig
@@ -97,7 +97,7 @@ config MTD_M25P80
 	  doesn't support the JEDEC ID instruction.
 
 config M25PXX_USE_FAST_READ
-	bool "Use FAST_READ OPCode allowing SPI CLK <= 50MHz"
+	bool "Use FAST_READ OPCode allowing SPI CLK >= 50MHz"
 	depends on MTD_M25P80
 	default y
 	help
@@ -120,6 +120,14 @@ config MTD_SST25L
 	  Set up your spi devices with the right board-specific platform data,
 	  if you want to specify device partitioning.
 
+config MTD_BCM47XXSFLASH
+	tristate "R/O support for serial flash on BCMA bus"
+	depends on BCMA_SFLASH
+	help
+	  BCMA bus can have various flash memories attached, they are
+	  registered by bcma as platform devices. This enables driver for
+	  serial flash memories (only read-only mode is implemented).
+
 config MTD_SLRAM
 	tristate "Uncached system RAM"
 	help
diff --git a/drivers/mtd/devices/Makefile b/drivers/mtd/devices/Makefile
index a4dd1d822b6..395733a30ef 100644
--- a/drivers/mtd/devices/Makefile
+++ b/drivers/mtd/devices/Makefile
@@ -19,5 +19,6 @@ obj-$(CONFIG_MTD_DATAFLASH)	+= mtd_dataflash.o
 obj-$(CONFIG_MTD_M25P80)	+= m25p80.o
 obj-$(CONFIG_MTD_SPEAR_SMI)	+= spear_smi.o
 obj-$(CONFIG_MTD_SST25L)	+= sst25l.o
+obj-$(CONFIG_MTD_BCM47XXSFLASH)	+= bcm47xxsflash.o
 
 CFLAGS_docg3.o			+= -I$(src)
\ No newline at end of file
diff --git a/drivers/mtd/devices/bcm47xxsflash.c b/drivers/mtd/devices/bcm47xxsflash.c
new file mode 100644
index 00000000000..2dc5a6f3fd5
--- /dev/null
+++ b/drivers/mtd/devices/bcm47xxsflash.c
@@ -0,0 +1,105 @@
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/mtd/mtd.h>
+#include <linux/platform_device.h>
+#include <linux/bcma/bcma.h>
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Serial flash driver for BCMA bus");
+
+static const char *probes[] = { "bcm47xxpart", NULL };
+
+static int bcm47xxsflash_read(struct mtd_info *mtd, loff_t from, size_t len,
+			      size_t *retlen, u_char *buf)
+{
+	struct bcma_sflash *sflash = mtd->priv;
+
+	/* Check address range */
+	if ((from + len) > mtd->size)
+		return -EINVAL;
+
+	memcpy_fromio(buf, (void __iomem *)KSEG0ADDR(sflash->window + from),
+		      len);
+
+	return len;
+}
+
+static void bcm47xxsflash_fill_mtd(struct bcma_sflash *sflash,
+				   struct mtd_info *mtd)
+{
+	mtd->priv = sflash;
+	mtd->name = "bcm47xxsflash";
+	mtd->owner = THIS_MODULE;
+	mtd->type = MTD_ROM;
+	mtd->size = sflash->size;
+	mtd->_read = bcm47xxsflash_read;
+
+	/* TODO: implement writing support and verify/change following code */
+	mtd->flags = MTD_CAP_ROM;
+	mtd->writebufsize = mtd->writesize = 1;
+}
+
+static int bcm47xxsflash_probe(struct platform_device *pdev)
+{
+	struct bcma_sflash *sflash = dev_get_platdata(&pdev->dev);
+	int err;
+
+	sflash->mtd = kzalloc(sizeof(struct mtd_info), GFP_KERNEL);
+	if (!sflash->mtd) {
+		err = -ENOMEM;
+		goto out;
+	}
+	bcm47xxsflash_fill_mtd(sflash, sflash->mtd);
+
+	err = mtd_device_parse_register(sflash->mtd, probes, NULL, NULL, 0);
+	if (err) {
+		pr_err("Failed to register MTD device: %d\n", err);
+		goto err_dev_reg;
+	}
+
+	return 0;
+
+err_dev_reg:
+	kfree(sflash->mtd);
+out:
+	return err;
+}
+
+static int __devexit bcm47xxsflash_remove(struct platform_device *pdev)
+{
+	struct bcma_sflash *sflash = dev_get_platdata(&pdev->dev);
+
+	mtd_device_unregister(sflash->mtd);
+	kfree(sflash->mtd);
+
+	return 0;
+}
+
+static struct platform_driver bcma_sflash_driver = {
+	.remove = __devexit_p(bcm47xxsflash_remove),
+	.driver = {
+		.name = "bcma_sflash",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init bcm47xxsflash_init(void)
+{
+	int err;
+
+	err = platform_driver_probe(&bcma_sflash_driver, bcm47xxsflash_probe);
+	if (err)
+		pr_err("Failed to register BCMA serial flash driver: %d\n",
+		       err);
+
+	return err;
+}
+
+static void __exit bcm47xxsflash_exit(void)
+{
+	platform_driver_unregister(&bcma_sflash_driver);
+}
+
+module_init(bcm47xxsflash_init);
+module_exit(bcm47xxsflash_exit);
diff --git a/drivers/mtd/devices/doc2001plus.c b/drivers/mtd/devices/doc2001plus.c
index 04eb2e4aa50..4f2220ad892 100644
--- a/drivers/mtd/devices/doc2001plus.c
+++ b/drivers/mtd/devices/doc2001plus.c
@@ -659,23 +659,15 @@ static int doc_read(struct mtd_info *mtd, loff_t from, size_t len,
 #ifdef ECC_DEBUG
 			printk("%s(%d): Millennium Plus ECC error (from=0x%x:\n",
 				__FILE__, __LINE__, (int)from);
-			printk("        syndrome= %02x:%02x:%02x:%02x:%02x:"
-				"%02x\n",
-				syndrome[0], syndrome[1], syndrome[2],
-				syndrome[3], syndrome[4], syndrome[5]);
-			printk("          eccbuf= %02x:%02x:%02x:%02x:%02x:"
-				"%02x\n",
-				eccbuf[0], eccbuf[1], eccbuf[2],
-				eccbuf[3], eccbuf[4], eccbuf[5]);
+			printk("        syndrome= %*phC\n", 6, syndrome);
+			printk("        eccbuf= %*phC\n", 6, eccbuf);
 #endif
 				ret = -EIO;
 		}
 	}
 
 #ifdef PSYCHO_DEBUG
-	printk("ECC DATA at %lx: %2.2X %2.2X %2.2X %2.2X %2.2X %2.2X\n",
-	       (long)from, eccbuf[0], eccbuf[1], eccbuf[2], eccbuf[3],
-	       eccbuf[4], eccbuf[5]);
+	printk("ECC DATA at %lx: %*ph\n", (long)from, 6, eccbuf);
 #endif
 	/* disable the ECC engine */
 	WriteDOC(DOC_ECC_DIS, docptr , Mplus_ECCConf);
diff --git a/drivers/mtd/devices/docg3.c b/drivers/mtd/devices/docg3.c
index f70854d728f..d34d83b8f9c 100644
--- a/drivers/mtd/devices/docg3.c
+++ b/drivers/mtd/devices/docg3.c
@@ -919,19 +919,13 @@ static int doc_read_oob(struct mtd_info *mtd, loff_t from,
 		eccconf1 = doc_register_readb(docg3, DOC_ECCCONF1);
 
 		if (nboob >= DOC_LAYOUT_OOB_SIZE) {
-			doc_dbg("OOB - INFO: %02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
-				oobbuf[0], oobbuf[1], oobbuf[2], oobbuf[3],
-				oobbuf[4], oobbuf[5], oobbuf[6]);
+			doc_dbg("OOB - INFO: %*phC\n", 7, oobbuf);
 			doc_dbg("OOB - HAMMING: %02x\n", oobbuf[7]);
-			doc_dbg("OOB - BCH_ECC: %02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
-				oobbuf[8], oobbuf[9], oobbuf[10], oobbuf[11],
-				oobbuf[12], oobbuf[13], oobbuf[14]);
+			doc_dbg("OOB - BCH_ECC: %*phC\n", 7, oobbuf + 8);
 			doc_dbg("OOB - UNUSED: %02x\n", oobbuf[15]);
 		}
 		doc_dbg("ECC checks: ECCConf1=%x\n", eccconf1);
-		doc_dbg("ECC HW_ECC: %02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
-			hwecc[0], hwecc[1], hwecc[2], hwecc[3], hwecc[4],
-			hwecc[5], hwecc[6]);
+		doc_dbg("ECC HW_ECC: %*phC\n", 7, hwecc);
 
 		ret = -EIO;
 		if (is_prot_seq_error(docg3))
diff --git a/drivers/mtd/devices/m25p80.c b/drivers/mtd/devices/m25p80.c
index 5d0d68c3fe2..03838bab1f5 100644
--- a/drivers/mtd/devices/m25p80.c
+++ b/drivers/mtd/devices/m25p80.c
@@ -633,11 +633,14 @@ static const struct spi_device_id m25p_ids[] = {
 	{ "at26df161a", INFO(0x1f4601, 0, 64 * 1024, 32, SECT_4K) },
 	{ "at26df321",  INFO(0x1f4700, 0, 64 * 1024, 64, SECT_4K) },
 
+	{ "at45db081d", INFO(0x1f2500, 0, 64 * 1024, 16, SECT_4K) },
+
 	/* EON -- en25xxx */
 	{ "en25f32", INFO(0x1c3116, 0, 64 * 1024,  64, SECT_4K) },
 	{ "en25p32", INFO(0x1c2016, 0, 64 * 1024,  64, 0) },
 	{ "en25q32b", INFO(0x1c3016, 0, 64 * 1024,  64, 0) },
 	{ "en25p64", INFO(0x1c2017, 0, 64 * 1024, 128, 0) },
+	{ "en25q64", INFO(0x1c3017, 0, 64 * 1024, 128, SECT_4K) },
 
 	/* Everspin */
 	{ "mr25h256", CAT25_INFO(  32 * 1024, 1, 256, 2) },
@@ -646,6 +649,7 @@ static const struct spi_device_id m25p_ids[] = {
 	{ "160s33b",  INFO(0x898911, 0, 64 * 1024,  32, 0) },
 	{ "320s33b",  INFO(0x898912, 0, 64 * 1024,  64, 0) },
 	{ "640s33b",  INFO(0x898913, 0, 64 * 1024, 128, 0) },
+	{ "n25q064",  INFO(0x20ba17, 0, 64 * 1024, 128, 0) },
 
 	/* Macronix */
 	{ "mx25l2005a",  INFO(0xc22012, 0, 64 * 1024,   4, SECT_4K) },
@@ -659,15 +663,15 @@ static const struct spi_device_id m25p_ids[] = {
 	{ "mx25l25635e", INFO(0xc22019, 0, 64 * 1024, 512, 0) },
 	{ "mx25l25655e", INFO(0xc22619, 0, 64 * 1024, 512, 0) },
 
+	/* Micron */
+	{ "n25q128",  INFO(0x20ba18, 0, 64 * 1024, 256, 0) },
+	{ "n25q256a", INFO(0x20ba19, 0, 64 * 1024, 512, SECT_4K) },
+
 	/* Spansion -- single (large) sector size only, at least
 	 * for the chips listed here (without boot sectors).
 	 */
-	{ "s25sl004a",  INFO(0x010212,      0,  64 * 1024,   8, 0) },
-	{ "s25sl008a",  INFO(0x010213,      0,  64 * 1024,  16, 0) },
-	{ "s25sl016a",  INFO(0x010214,      0,  64 * 1024,  32, 0) },
-	{ "s25sl032a",  INFO(0x010215,      0,  64 * 1024,  64, 0) },
-	{ "s25sl032p",  INFO(0x010215, 0x4d00,  64 * 1024,  64, SECT_4K) },
-	{ "s25sl064a",  INFO(0x010216,      0,  64 * 1024, 128, 0) },
+	{ "s25sl032p",  INFO(0x010215, 0x4d00,  64 * 1024,  64, 0) },
+	{ "s25sl064p",  INFO(0x010216, 0x4d00,  64 * 1024, 128, 0) },
 	{ "s25fl256s0", INFO(0x010219, 0x4d00, 256 * 1024, 128, 0) },
 	{ "s25fl256s1", INFO(0x010219, 0x4d01,  64 * 1024, 512, 0) },
 	{ "s25fl512s",  INFO(0x010220, 0x4d00, 256 * 1024, 256, 0) },
@@ -676,6 +680,11 @@ static const struct spi_device_id m25p_ids[] = {
 	{ "s25sl12801", INFO(0x012018, 0x0301,  64 * 1024, 256, 0) },
 	{ "s25fl129p0", INFO(0x012018, 0x4d00, 256 * 1024,  64, 0) },
 	{ "s25fl129p1", INFO(0x012018, 0x4d01,  64 * 1024, 256, 0) },
+	{ "s25sl004a",  INFO(0x010212,      0,  64 * 1024,   8, 0) },
+	{ "s25sl008a",  INFO(0x010213,      0,  64 * 1024,  16, 0) },
+	{ "s25sl016a",  INFO(0x010214,      0,  64 * 1024,  32, 0) },
+	{ "s25sl032a",  INFO(0x010215,      0,  64 * 1024,  64, 0) },
+	{ "s25sl064a",  INFO(0x010216,      0,  64 * 1024, 128, 0) },
 	{ "s25fl016k",  INFO(0xef4015,      0,  64 * 1024,  32, SECT_4K) },
 	{ "s25fl064k",  INFO(0xef4017,      0,  64 * 1024, 128, SECT_4K) },
 
@@ -699,6 +708,7 @@ static const struct spi_device_id m25p_ids[] = {
 	{ "m25p32",  INFO(0x202016,  0,  64 * 1024,  64, 0) },
 	{ "m25p64",  INFO(0x202017,  0,  64 * 1024, 128, 0) },
 	{ "m25p128", INFO(0x202018,  0, 256 * 1024,  64, 0) },
+	{ "n25q032", INFO(0x20ba16,  0,  64 * 1024,  64, 0) },
 
 	{ "m25p05-nonjedec",  INFO(0, 0,  32 * 1024,   2, 0) },
 	{ "m25p10-nonjedec",  INFO(0, 0,  32 * 1024,   4, 0) },
@@ -714,6 +724,7 @@ static const struct spi_device_id m25p_ids[] = {
 	{ "m45pe80", INFO(0x204014,  0, 64 * 1024,   16, 0) },
 	{ "m45pe16", INFO(0x204015,  0, 64 * 1024,   32, 0) },
 
+	{ "m25pe20", INFO(0x208012,  0, 64 * 1024,  4,       0) },
 	{ "m25pe80", INFO(0x208014,  0, 64 * 1024, 16,       0) },
 	{ "m25pe16", INFO(0x208015,  0, 64 * 1024, 32, SECT_4K) },
 
@@ -730,6 +741,7 @@ static const struct spi_device_id m25p_ids[] = {
 	{ "w25x16", INFO(0xef3015, 0, 64 * 1024,  32, SECT_4K) },
 	{ "w25x32", INFO(0xef3016, 0, 64 * 1024,  64, SECT_4K) },
 	{ "w25q32", INFO(0xef4016, 0, 64 * 1024,  64, SECT_4K) },
+	{ "w25q32dw", INFO(0xef6016, 0, 64 * 1024,  64, SECT_4K) },
 	{ "w25x64", INFO(0xef3017, 0, 64 * 1024, 128, SECT_4K) },
 	{ "w25q64", INFO(0xef4017, 0, 64 * 1024, 128, SECT_4K) },
 	{ "w25q80", INFO(0xef5014, 0, 64 * 1024,  16, SECT_4K) },
diff --git a/drivers/mtd/devices/spear_smi.c b/drivers/mtd/devices/spear_smi.c
index 67960362681..dcc3c951153 100644
--- a/drivers/mtd/devices/spear_smi.c
+++ b/drivers/mtd/devices/spear_smi.c
@@ -26,6 +26,7 @@
 #include <linux/module.h>
 #include <linux/param.h>
 #include <linux/platform_device.h>
+#include <linux/pm.h>
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/partitions.h>
 #include <linux/mtd/spear_smi.h>
@@ -240,8 +241,8 @@ static int spear_smi_read_sr(struct spear_smi *dev, u32 bank)
 	/* copy dev->status (lower 16 bits) in order to release lock */
 	if (ret > 0)
 		ret = dev->status & 0xffff;
-	else
-		ret = -EIO;
+	else if (ret == 0)
+		ret = -ETIMEDOUT;
 
 	/* restore the ctrl regs state */
 	writel(ctrlreg1, dev->io_base + SMI_CR1);
@@ -269,16 +270,19 @@ static int spear_smi_wait_till_ready(struct spear_smi *dev, u32 bank,
 	finish = jiffies + timeout;
 	do {
 		status = spear_smi_read_sr(dev, bank);
-		if (status < 0)
-			continue; /* try till timeout */
-		else if (!(status & SR_WIP))
+		if (status < 0) {
+			if (status == -ETIMEDOUT)
+				continue; /* try till finish */
+			return status;
+		} else if (!(status & SR_WIP)) {
 			return 0;
+		}
 
 		cond_resched();
 	} while (!time_after_eq(jiffies, finish));
 
 	dev_err(&dev->pdev->dev, "smi controller is busy, timeout\n");
-	return status;
+	return -EBUSY;
 }
 
 /**
@@ -335,6 +339,9 @@ static void spear_smi_hw_init(struct spear_smi *dev)
 	val = HOLD1 | BANK_EN | DSEL_TIME | (prescale << 8);
 
 	mutex_lock(&dev->lock);
+	/* clear all interrupt conditions */
+	writel(0, dev->io_base + SMI_SR);
+
 	writel(val, dev->io_base + SMI_CR1);
 	mutex_unlock(&dev->lock);
 }
@@ -391,11 +398,11 @@ static int spear_smi_write_enable(struct spear_smi *dev, u32 bank)
 	writel(ctrlreg1, dev->io_base + SMI_CR1);
 	writel(0, dev->io_base + SMI_CR2);
 
-	if (ret <= 0) {
+	if (ret == 0) {
 		ret = -EIO;
 		dev_err(&dev->pdev->dev,
 			"smi controller failed on write enable\n");
-	} else {
+	} else if (ret > 0) {
 		/* check whether write mode status is set for required bank */
 		if (dev->status & (1 << (bank + WM_SHIFT)))
 			ret = 0;
@@ -462,10 +469,10 @@ static int spear_smi_erase_sector(struct spear_smi *dev,
 	ret = wait_event_interruptible_timeout(dev->cmd_complete,
 			dev->status & TFF, SMI_CMD_TIMEOUT);
 
-	if (ret <= 0) {
+	if (ret == 0) {
 		ret = -EIO;
 		dev_err(&dev->pdev->dev, "sector erase failed\n");
-	} else
+	} else if (ret > 0)
 		ret = 0; /* success */
 
 	/* restore ctrl regs */
@@ -820,7 +827,7 @@ static int spear_smi_setup_banks(struct platform_device *pdev,
 	if (!flash_info)
 		return -ENODEV;
 
-	flash = kzalloc(sizeof(*flash), GFP_ATOMIC);
+	flash = devm_kzalloc(&pdev->dev, sizeof(*flash), GFP_ATOMIC);
 	if (!flash)
 		return -ENOMEM;
 	flash->bank = bank;
@@ -831,15 +838,13 @@ static int spear_smi_setup_banks(struct platform_device *pdev,
 	flash_index = spear_smi_probe_flash(dev, bank);
 	if (flash_index < 0) {
 		dev_info(&dev->pdev->dev, "smi-nor%d not found\n", bank);
-		ret = flash_index;
-		goto err_probe;
+		return flash_index;
 	}
 	/* map the memory for nor flash chip */
-	flash->base_addr = ioremap(flash_info->mem_base, flash_info->size);
-	if (!flash->base_addr) {
-		ret = -EIO;
-		goto err_probe;
-	}
+	flash->base_addr = devm_ioremap(&pdev->dev, flash_info->mem_base,
+					flash_info->size);
+	if (!flash->base_addr)
+		return -EIO;
 
 	dev->flash[bank] = flash;
 	flash->mtd.priv = dev;
@@ -881,17 +886,10 @@ static int spear_smi_setup_banks(struct platform_device *pdev,
 					count);
 	if (ret) {
 		dev_err(&dev->pdev->dev, "Err MTD partition=%d\n", ret);
-		goto err_map;
+		return ret;
 	}
 
 	return 0;
-
-err_map:
-	iounmap(flash->base_addr);
-
-err_probe:
-	kfree(flash);
-	return ret;
 }
 
 /**
@@ -928,20 +926,13 @@ static int __devinit spear_smi_probe(struct platform_device *pdev)
 		}
 	} else {
 		pdata = dev_get_platdata(&pdev->dev);
-		if (pdata < 0) {
+		if (!pdata) {
 			ret = -ENODEV;
 			dev_err(&pdev->dev, "no platform data\n");
 			goto err;
 		}
 	}
 
-	smi_base = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!smi_base) {
-		ret = -ENODEV;
-		dev_err(&pdev->dev, "invalid smi base address\n");
-		goto err;
-	}
-
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0) {
 		ret = -ENODEV;
@@ -949,32 +940,26 @@ static int __devinit spear_smi_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	dev = kzalloc(sizeof(*dev), GFP_ATOMIC);
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_ATOMIC);
 	if (!dev) {
 		ret = -ENOMEM;
 		dev_err(&pdev->dev, "mem alloc fail\n");
 		goto err;
 	}
 
-	smi_base = request_mem_region(smi_base->start, resource_size(smi_base),
-			pdev->name);
-	if (!smi_base) {
-		ret = -EBUSY;
-		dev_err(&pdev->dev, "request mem region fail\n");
-		goto err_mem;
-	}
+	smi_base = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
-	dev->io_base = ioremap(smi_base->start, resource_size(smi_base));
+	dev->io_base = devm_request_and_ioremap(&pdev->dev, smi_base);
 	if (!dev->io_base) {
 		ret = -EIO;
-		dev_err(&pdev->dev, "ioremap fail\n");
-		goto err_ioremap;
+		dev_err(&pdev->dev, "devm_request_and_ioremap fail\n");
+		goto err;
 	}
 
 	dev->pdev = pdev;
 	dev->clk_rate = pdata->clk_rate;
 
-	if (dev->clk_rate < 0 || dev->clk_rate > SMI_MAX_CLOCK_FREQ)
+	if (dev->clk_rate > SMI_MAX_CLOCK_FREQ)
 		dev->clk_rate = SMI_MAX_CLOCK_FREQ;
 
 	dev->num_flashes = pdata->num_flashes;
@@ -984,17 +969,18 @@ static int __devinit spear_smi_probe(struct platform_device *pdev)
 		dev->num_flashes = MAX_NUM_FLASH_CHIP;
 	}
 
-	dev->clk = clk_get(&pdev->dev, NULL);
+	dev->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(dev->clk)) {
 		ret = PTR_ERR(dev->clk);
-		goto err_clk;
+		goto err;
 	}
 
 	ret = clk_prepare_enable(dev->clk);
 	if (ret)
-		goto err_clk_prepare_enable;
+		goto err;
 
-	ret = request_irq(irq, spear_smi_int_handler, 0, pdev->name, dev);
+	ret = devm_request_irq(&pdev->dev, irq, spear_smi_int_handler, 0,
+			       pdev->name, dev);
 	if (ret) {
 		dev_err(&dev->pdev->dev, "SMI IRQ allocation failed\n");
 		goto err_irq;
@@ -1017,18 +1003,9 @@ static int __devinit spear_smi_probe(struct platform_device *pdev)
 	return 0;
 
 err_bank_setup:
-	free_irq(irq, dev);
 	platform_set_drvdata(pdev, NULL);
 err_irq:
 	clk_disable_unprepare(dev->clk);
-err_clk_prepare_enable:
-	clk_put(dev->clk);
-err_clk:
-	iounmap(dev->io_base);
-err_ioremap:
-	release_mem_region(smi_base->start, resource_size(smi_base));
-err_mem:
-	kfree(dev);
 err:
 	return ret;
 }
@@ -1042,11 +1019,8 @@ err:
 static int __devexit spear_smi_remove(struct platform_device *pdev)
 {
 	struct spear_smi *dev;
-	struct spear_smi_plat_data *pdata;
 	struct spear_snor_flash *flash;
-	struct resource *smi_base;
-	int ret;
-	int i, irq;
+	int ret, i;
 
 	dev = platform_get_drvdata(pdev);
 	if (!dev) {
@@ -1054,8 +1028,6 @@ static int __devexit spear_smi_remove(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	pdata = dev_get_platdata(&pdev->dev);
-
 	/* clean up for all nor flash */
 	for (i = 0; i < dev->num_flashes; i++) {
 		flash = dev->flash[i];
@@ -1066,49 +1038,41 @@ static int __devexit spear_smi_remove(struct platform_device *pdev)
 		ret = mtd_device_unregister(&flash->mtd);
 		if (ret)
 			dev_err(&pdev->dev, "error removing mtd\n");
-
-		iounmap(flash->base_addr);
-		kfree(flash);
 	}
 
-	irq = platform_get_irq(pdev, 0);
-	free_irq(irq, dev);
-
 	clk_disable_unprepare(dev->clk);
-	clk_put(dev->clk);
-	iounmap(dev->io_base);
-	kfree(dev);
-
-	smi_base = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	release_mem_region(smi_base->start, resource_size(smi_base));
 	platform_set_drvdata(pdev, NULL);
 
 	return 0;
 }
 
-int spear_smi_suspend(struct platform_device *pdev, pm_message_t state)
+#ifdef CONFIG_PM
+static int spear_smi_suspend(struct device *dev)
 {
-	struct spear_smi *dev = platform_get_drvdata(pdev);
+	struct spear_smi *sdev = dev_get_drvdata(dev);
 
-	if (dev && dev->clk)
-		clk_disable_unprepare(dev->clk);
+	if (sdev && sdev->clk)
+		clk_disable_unprepare(sdev->clk);
 
 	return 0;
 }
 
-int spear_smi_resume(struct platform_device *pdev)
+static int spear_smi_resume(struct device *dev)
 {
-	struct spear_smi *dev = platform_get_drvdata(pdev);
+	struct spear_smi *sdev = dev_get_drvdata(dev);
 	int ret = -EPERM;
 
-	if (dev && dev->clk)
-		ret = clk_prepare_enable(dev->clk);
+	if (sdev && sdev->clk)
+		ret = clk_prepare_enable(sdev->clk);
 
 	if (!ret)
-		spear_smi_hw_init(dev);
+		spear_smi_hw_init(sdev);
 	return ret;
 }
 
+static SIMPLE_DEV_PM_OPS(spear_smi_pm_ops, spear_smi_suspend, spear_smi_resume);
+#endif
+
 #ifdef CONFIG_OF
 static const struct of_device_id spear_smi_id_table[] = {
 	{ .compatible = "st,spear600-smi" },
@@ -1123,11 +1087,12 @@ static struct platform_driver spear_smi_driver = {
 		.bus = &platform_bus_type,
 		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(spear_smi_id_table),
+#ifdef CONFIG_PM
+		.pm = &spear_smi_pm_ops,
+#endif
 	},
 	.probe = spear_smi_probe,
 	.remove = __devexit_p(spear_smi_remove),
-	.suspend = spear_smi_suspend,
-	.resume = spear_smi_resume,
 };
 
 static int spear_smi_init(void)
diff --git a/drivers/mtd/maps/Kconfig b/drivers/mtd/maps/Kconfig
index 5ba2458e799..2e47c2ed0a2 100644
--- a/drivers/mtd/maps/Kconfig
+++ b/drivers/mtd/maps/Kconfig
@@ -373,7 +373,7 @@ config MTD_FORTUNET
 	  have such a board, say 'Y'.
 
 config MTD_AUTCPU12
-	tristate "NV-RAM mapping AUTCPU12 board"
+	bool "NV-RAM mapping AUTCPU12 board"
 	depends on ARCH_AUTCPU12
 	help
 	  This enables access to the NV-RAM on autronix autcpu12 board.
@@ -443,22 +443,10 @@ config MTD_GPIO_ADDR
 
 config MTD_UCLINUX
 	bool "Generic uClinux RAM/ROM filesystem support"
-	depends on MTD_RAM=y && !MMU
+	depends on MTD_RAM=y && (!MMU || COLDFIRE)
 	help
 	  Map driver to support image based filesystems for uClinux.
 
-config MTD_WRSBC8260
-	tristate "Map driver for WindRiver PowerQUICC II MPC82xx board"
-	depends on (SBC82xx || SBC8560)
-	select MTD_MAP_BANK_WIDTH_4
-	select MTD_MAP_BANK_WIDTH_1
-	select MTD_CFI_I1
-	select MTD_CFI_I4
-	help
-	  Map driver for WindRiver PowerQUICC II MPC82xx board. Drives
-	  all three flash regions on CS0, CS1 and CS6 if they are configured
-	  correctly by the boot loader.
-
 config MTD_DMV182
         tristate "Map driver for Dy-4 SVME/DMV-182 board."
         depends on DMV182
diff --git a/drivers/mtd/maps/Makefile b/drivers/mtd/maps/Makefile
index 68a9a91d344..deb43e9a1e7 100644
--- a/drivers/mtd/maps/Makefile
+++ b/drivers/mtd/maps/Makefile
@@ -47,7 +47,6 @@ obj-$(CONFIG_MTD_SCB2_FLASH)	+= scb2_flash.o
 obj-$(CONFIG_MTD_H720X)		+= h720x-flash.o
 obj-$(CONFIG_MTD_IXP4XX)	+= ixp4xx.o
 obj-$(CONFIG_MTD_IXP2000)	+= ixp2000.o
-obj-$(CONFIG_MTD_WRSBC8260)	+= wr_sbc82xx_flash.o
 obj-$(CONFIG_MTD_DMV182)	+= dmv182.o
 obj-$(CONFIG_MTD_PLATRAM)	+= plat-ram.o
 obj-$(CONFIG_MTD_INTEL_VR_NOR)	+= intel_vr_nor.o
diff --git a/drivers/mtd/maps/autcpu12-nvram.c b/drivers/mtd/maps/autcpu12-nvram.c
index e5bfd0e093b..76fb594bb1d 100644
--- a/drivers/mtd/maps/autcpu12-nvram.c
+++ b/drivers/mtd/maps/autcpu12-nvram.c
@@ -15,43 +15,54 @@
  * 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 <linux/sizes.h>
 
-#include <linux/module.h>
 #include <linux/types.h>
 #include <linux/kernel.h>
-#include <linux/ioport.h>
 #include <linux/init.h>
-#include <asm/io.h>
-#include <asm/sizes.h>
-#include <mach/hardware.h>
-#include <mach/autcpu12.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/map.h>
-#include <linux/mtd/partitions.h>
-
-
-static struct mtd_info *sram_mtd;
 
-struct map_info autcpu12_sram_map = {
-	.name = "SRAM",
-	.size = 32768,
-	.bankwidth = 4,
-	.phys = 0x12000000,
+struct autcpu12_nvram_priv {
+	struct mtd_info *mtd;
+	struct map_info map;
 };
 
-static int __init init_autcpu12_sram (void)
+static int __devinit autcpu12_nvram_probe(struct platform_device *pdev)
 {
-	int err, save0, save1;
+	map_word tmp, save0, save1;
+	struct resource *res;
+	struct autcpu12_nvram_priv *priv;
 
-	autcpu12_sram_map.virt = ioremap(0x12000000, SZ_128K);
-	if (!autcpu12_sram_map.virt) {
-		printk("Failed to ioremap autcpu12 NV-RAM space\n");
-		err = -EIO;
-		goto out;
+	priv = devm_kzalloc(&pdev->dev,
+			    sizeof(struct autcpu12_nvram_priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, priv);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "failed to get memory resource\n");
+		return -ENOENT;
+	}
+
+	priv->map.bankwidth	= 4;
+	priv->map.phys		= res->start;
+	priv->map.size		= resource_size(res);
+	priv->map.virt		= devm_request_and_ioremap(&pdev->dev, res);
+	strcpy((char *)priv->map.name, res->name);
+	if (!priv->map.virt) {
+		dev_err(&pdev->dev, "failed to remap mem resource\n");
+		return -EBUSY;
 	}
-	simple_map_init(&autcpu_sram_map);
+
+	simple_map_init(&priv->map);
 
 	/*
 	 * Check for 32K/128K
@@ -61,65 +72,59 @@ static int __init init_autcpu12_sram (void)
 	 * Read	and check result on ofs 0x0
 	 * Restore contents
 	 */
-	save0 = map_read32(&autcpu12_sram_map,0);
-	save1 = map_read32(&autcpu12_sram_map,0x10000);
-	map_write32(&autcpu12_sram_map,~save0,0x10000);
-	/* if we find this pattern on 0x0, we have 32K size
-	 * restore contents and exit
-	 */
-	if ( map_read32(&autcpu12_sram_map,0) != save0) {
-		map_write32(&autcpu12_sram_map,save0,0x0);
-		goto map;
+	save0 = map_read(&priv->map, 0);
+	save1 = map_read(&priv->map, 0x10000);
+	tmp.x[0] = ~save0.x[0];
+	map_write(&priv->map, tmp, 0x10000);
+	tmp = map_read(&priv->map, 0);
+	/* if we find this pattern on 0x0, we have 32K size */
+	if (!map_word_equal(&priv->map, tmp, save0)) {
+		map_write(&priv->map, save0, 0x0);
+		priv->map.size = SZ_32K;
+	} else
+		map_write(&priv->map, save1, 0x10000);
+
+	priv->mtd = do_map_probe("map_ram", &priv->map);
+	if (!priv->mtd) {
+		dev_err(&pdev->dev, "probing failed\n");
+		return -ENXIO;
 	}
-	/* We have a 128K found, restore 0x10000 and set size
-	 * to 128K
-	 */
-	map_write32(&autcpu12_sram_map,save1,0x10000);
-	autcpu12_sram_map.size = SZ_128K;
-
-map:
-	sram_mtd = do_map_probe("map_ram", &autcpu12_sram_map);
-	if (!sram_mtd) {
-		printk("NV-RAM probe failed\n");
-		err = -ENXIO;
-		goto out_ioremap;
-	}
-
-	sram_mtd->owner = THIS_MODULE;
-	sram_mtd->erasesize = 16;
 
-	if (mtd_device_register(sram_mtd, NULL, 0)) {
-		printk("NV-RAM device addition failed\n");
-		err = -ENOMEM;
-		goto out_probe;
+	priv->mtd->owner	= THIS_MODULE;
+	priv->mtd->erasesize	= 16;
+	priv->mtd->dev.parent	= &pdev->dev;
+	if (!mtd_device_register(priv->mtd, NULL, 0)) {
+		dev_info(&pdev->dev,
+			 "NV-RAM device size %ldKiB registered on AUTCPU12\n",
+			 priv->map.size / SZ_1K);
+		return 0;
 	}
 
-	printk("NV-RAM device size %ldKiB registered on AUTCPU12\n",autcpu12_sram_map.size/SZ_1K);
-
-	return 0;
-
-out_probe:
-	map_destroy(sram_mtd);
-	sram_mtd = 0;
-
-out_ioremap:
-	iounmap((void *)autcpu12_sram_map.virt);
-out:
-	return err;
+	map_destroy(priv->mtd);
+	dev_err(&pdev->dev, "NV-RAM device addition failed\n");
+	return -ENOMEM;
 }
 
-static void __exit cleanup_autcpu12_maps(void)
+static int __devexit autcpu12_nvram_remove(struct platform_device *pdev)
 {
-	if (sram_mtd) {
-		mtd_device_unregister(sram_mtd);
-		map_destroy(sram_mtd);
-		iounmap((void *)autcpu12_sram_map.virt);
-	}
+	struct autcpu12_nvram_priv *priv = platform_get_drvdata(pdev);
+
+	mtd_device_unregister(priv->mtd);
+	map_destroy(priv->mtd);
+
+	return 0;
 }
 
-module_init(init_autcpu12_sram);
-module_exit(cleanup_autcpu12_maps);
+static struct platform_driver autcpu12_nvram_driver = {
+	.driver		= {
+		.name	= "autcpu12_nvram",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= autcpu12_nvram_probe,
+	.remove		= __devexit_p(autcpu12_nvram_remove),
+};
+module_platform_driver(autcpu12_nvram_driver);
 
 MODULE_AUTHOR("Thomas Gleixner");
-MODULE_DESCRIPTION("autcpu12 NV-RAM map driver");
+MODULE_DESCRIPTION("autcpu12 NVRAM map driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/mtd/maps/pci.c b/drivers/mtd/maps/pci.c
index f14ce0af763..1c30c1a307f 100644
--- a/drivers/mtd/maps/pci.c
+++ b/drivers/mtd/maps/pci.c
@@ -43,26 +43,14 @@ static map_word mtd_pci_read8(struct map_info *_map, unsigned long ofs)
 	struct map_pci_info *map = (struct map_pci_info *)_map;
 	map_word val;
 	val.x[0]= readb(map->base + map->translate(map, ofs));
-//	printk("read8 : %08lx => %02x\n", ofs, val.x[0]);
 	return val;
 }
 
-#if 0
-static map_word mtd_pci_read16(struct map_info *_map, unsigned long ofs)
-{
-	struct map_pci_info *map = (struct map_pci_info *)_map;
-	map_word val;
-	val.x[0] = readw(map->base + map->translate(map, ofs));
-//	printk("read16: %08lx => %04x\n", ofs, val.x[0]);
-	return val;
-}
-#endif
 static map_word mtd_pci_read32(struct map_info *_map, unsigned long ofs)
 {
 	struct map_pci_info *map = (struct map_pci_info *)_map;
 	map_word val;
 	val.x[0] = readl(map->base + map->translate(map, ofs));
-//	printk("read32: %08lx => %08x\n", ofs, val.x[0]);
 	return val;
 }
 
@@ -75,22 +63,12 @@ static void mtd_pci_copyfrom(struct map_info *_map, void *to, unsigned long from
 static void mtd_pci_write8(struct map_info *_map, map_word val, unsigned long ofs)
 {
 	struct map_pci_info *map = (struct map_pci_info *)_map;
-//	printk("write8 : %08lx <= %02x\n", ofs, val.x[0]);
 	writeb(val.x[0], map->base + map->translate(map, ofs));
 }
 
-#if 0
-static void mtd_pci_write16(struct map_info *_map, map_word val, unsigned long ofs)
-{
-	struct map_pci_info *map = (struct map_pci_info *)_map;
-//	printk("write16: %08lx <= %04x\n", ofs, val.x[0]);
-	writew(val.x[0], map->base + map->translate(map, ofs));
-}
-#endif
 static void mtd_pci_write32(struct map_info *_map, map_word val, unsigned long ofs)
 {
 	struct map_pci_info *map = (struct map_pci_info *)_map;
-//	printk("write32: %08lx <= %08x\n", ofs, val.x[0]);
 	writel(val.x[0], map->base + map->translate(map, ofs));
 }
 
@@ -358,4 +336,3 @@ MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Russell King <rmk@arm.linux.org.uk>");
 MODULE_DESCRIPTION("Generic PCI map driver");
 MODULE_DEVICE_TABLE(pci, mtd_pci_ids);
-
diff --git a/drivers/mtd/maps/physmap_of.c b/drivers/mtd/maps/physmap_of.c
index 2e6fb6831d5..6f19acadb06 100644
--- a/drivers/mtd/maps/physmap_of.c
+++ b/drivers/mtd/maps/physmap_of.c
@@ -169,6 +169,7 @@ static int __devinit of_flash_probe(struct platform_device *dev)
 	struct mtd_info **mtd_list = NULL;
 	resource_size_t res_size;
 	struct mtd_part_parser_data ppdata;
+	bool map_indirect;
 
 	match = of_match_device(of_flash_match, &dev->dev);
 	if (!match)
@@ -192,6 +193,8 @@ static int __devinit of_flash_probe(struct platform_device *dev)
 	}
 	count /= reg_tuple_size;
 
+	map_indirect = of_property_read_bool(dp, "no-unaligned-direct-access");
+
 	err = -ENOMEM;
 	info = kzalloc(sizeof(struct of_flash) +
 		       sizeof(struct of_flash_list) * count, GFP_KERNEL);
@@ -247,6 +250,17 @@ static int __devinit of_flash_probe(struct platform_device *dev)
 
 		simple_map_init(&info->list[i].map);
 
+		/*
+		 * On some platforms (e.g. MPC5200) a direct 1:1 mapping
+		 * may cause problems with JFFS2 usage, as the local bus (LPB)
+		 * doesn't support unaligned accesses as implemented in the
+		 * JFFS2 code via memcpy(). By setting NO_XIP, the
+		 * flash will not be exposed directly to the MTD users
+		 * (e.g. JFFS2) any more.
+		 */
+		if (map_indirect)
+			info->list[i].map.phys = NO_XIP;
+
 		if (probe_type) {
 			info->list[i].mtd = do_map_probe(probe_type,
 							 &info->list[i].map);
diff --git a/drivers/mtd/maps/rbtx4939-flash.c b/drivers/mtd/maps/rbtx4939-flash.c
index 6f52e1f288b..49c3fe715ee 100644
--- a/drivers/mtd/maps/rbtx4939-flash.c
+++ b/drivers/mtd/maps/rbtx4939-flash.c
@@ -100,8 +100,6 @@ static int rbtx4939_flash_probe(struct platform_device *dev)
 		goto err_out;
 	}
 	info->mtd->owner = THIS_MODULE;
-	if (err)
-		goto err_out;
 	err = mtd_device_parse_register(info->mtd, NULL, NULL, pdata->parts,
 					pdata->nr_parts);
 
diff --git a/drivers/mtd/maps/uclinux.c b/drivers/mtd/maps/uclinux.c
index c3bb304eca0..299bf88a6f4 100644
--- a/drivers/mtd/maps/uclinux.c
+++ b/drivers/mtd/maps/uclinux.c
@@ -67,10 +67,16 @@ static int __init uclinux_mtd_init(void)
 	printk("uclinux[mtd]: RAM probe address=0x%x size=0x%x\n",
 	       	(int) mapp->phys, (int) mapp->size);
 
-	mapp->virt = ioremap_nocache(mapp->phys, mapp->size);
+	/*
+	 * The filesystem is guaranteed to be in direct mapped memory. It is
+	 * directly following the kernels own bss region. Following the same
+	 * mechanism used by architectures setting up traditional initrds we
+	 * use phys_to_virt to get the virtual address of its start.
+	 */
+	mapp->virt = phys_to_virt(mapp->phys);
 
 	if (mapp->virt == 0) {
-		printk("uclinux[mtd]: ioremap_nocache() failed\n");
+		printk("uclinux[mtd]: no virtual mapping?\n");
 		return(-EIO);
 	}
 
@@ -79,7 +85,6 @@ static int __init uclinux_mtd_init(void)
 	mtd = do_map_probe("map_ram", mapp);
 	if (!mtd) {
 		printk("uclinux[mtd]: failed to find a mapping?\n");
-		iounmap(mapp->virt);
 		return(-ENXIO);
 	}
 
@@ -102,10 +107,8 @@ static void __exit uclinux_mtd_cleanup(void)
 		map_destroy(uclinux_ram_mtdinfo);
 		uclinux_ram_mtdinfo = NULL;
 	}
-	if (uclinux_ram_map.virt) {
-		iounmap((void *) uclinux_ram_map.virt);
+	if (uclinux_ram_map.virt)
 		uclinux_ram_map.virt = 0;
-	}
 }
 
 /****************************************************************************/
diff --git a/drivers/mtd/maps/wr_sbc82xx_flash.c b/drivers/mtd/maps/wr_sbc82xx_flash.c
deleted file mode 100644
index e7534c82f93..00000000000
--- a/drivers/mtd/maps/wr_sbc82xx_flash.c
+++ /dev/null
@@ -1,174 +0,0 @@
-/*
- * Map for flash chips on Wind River PowerQUICC II SBC82xx board.
- *
- * Copyright (C) 2004 Red Hat, Inc.
- *
- * Author: David Woodhouse <dwmw2@infradead.org>
- *
- */
-
-#include <linux/module.h>
-#include <linux/types.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/slab.h>
-#include <asm/io.h>
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/map.h>
-#include <linux/mtd/partitions.h>
-
-#include <asm/immap_cpm2.h>
-
-static struct mtd_info *sbcmtd[3];
-
-struct map_info sbc82xx_flash_map[3] = {
-	{.name = "Boot flash"},
-	{.name = "Alternate boot flash"},
-	{.name = "User flash"}
-};
-
-static struct mtd_partition smallflash_parts[] = {
-	{
-		.name =		"space",
-		.size =		0x100000,
-		.offset =	0,
-	}, {
-		.name =		"bootloader",
-		.size =		MTDPART_SIZ_FULL,
-		.offset =	MTDPART_OFS_APPEND,
-	}
-};
-
-static struct mtd_partition bigflash_parts[] = {
-	{
-		.name =		"bootloader",
-		.size =		0x00100000,
-		.offset =	0,
-	}, {
-		.name =		"file system",
-		.size =		0x01f00000,
-		.offset =	MTDPART_OFS_APPEND,
-	}, {
-		.name =		"boot config",
-		.size =		0x00100000,
-		.offset =	MTDPART_OFS_APPEND,
-	}, {
-		.name =		"space",
-		.size =		0x01f00000,
-		.offset =	MTDPART_OFS_APPEND,
-	}
-};
-
-static const char *part_probes[] __initconst = {"cmdlinepart", "RedBoot", NULL};
-
-#define init_sbc82xx_one_flash(map, br, or)			\
-do {								\
-	(map).phys = (br & 1) ? (br & 0xffff8000) : 0;		\
-	(map).size = (br & 1) ? (~(or & 0xffff8000) + 1) : 0;	\
-	switch (br & 0x00001800) {				\
-	case 0x00000000:					\
-	case 0x00000800:	(map).bankwidth = 1;	break;	\
-	case 0x00001000:	(map).bankwidth = 2;	break;	\
-	case 0x00001800:	(map).bankwidth = 4;	break;	\
-	}							\
-} while (0);
-
-static int __init init_sbc82xx_flash(void)
-{
-	volatile memctl_cpm2_t *mc = &cpm2_immr->im_memctl;
-	int bigflash;
-	int i;
-
-#ifdef CONFIG_SBC8560
-	mc = ioremap(0xff700000 + 0x5000, sizeof(memctl_cpm2_t));
-#else
-	mc = &cpm2_immr->im_memctl;
-#endif
-
-	bigflash = 1;
-	if ((mc->memc_br0 & 0x00001800) == 0x00001800)
-		bigflash = 0;
-
-	init_sbc82xx_one_flash(sbc82xx_flash_map[0], mc->memc_br0, mc->memc_or0);
-	init_sbc82xx_one_flash(sbc82xx_flash_map[1], mc->memc_br6, mc->memc_or6);
-	init_sbc82xx_one_flash(sbc82xx_flash_map[2], mc->memc_br1, mc->memc_or1);
-
-#ifdef CONFIG_SBC8560
-	iounmap((void *) mc);
-#endif
-
-	for (i=0; i<3; i++) {
-		int8_t flashcs[3] = { 0, 6, 1 };
-		int nr_parts;
-		struct mtd_partition *defparts;
-
-		printk(KERN_NOTICE "PowerQUICC II %s (%ld MiB on CS%d",
-		       sbc82xx_flash_map[i].name,
-		       (sbc82xx_flash_map[i].size >> 20),
-		       flashcs[i]);
-		if (!sbc82xx_flash_map[i].phys) {
-			/* We know it can't be at zero. */
-			printk("): disabled by bootloader.\n");
-			continue;
-		}
-		printk(" at %08lx)\n",  sbc82xx_flash_map[i].phys);
-
-		sbc82xx_flash_map[i].virt = ioremap(sbc82xx_flash_map[i].phys,
-						    sbc82xx_flash_map[i].size);
-
-		if (!sbc82xx_flash_map[i].virt) {
-			printk("Failed to ioremap\n");
-			continue;
-		}
-
-		simple_map_init(&sbc82xx_flash_map[i]);
-
-		sbcmtd[i] = do_map_probe("cfi_probe", &sbc82xx_flash_map[i]);
-
-		if (!sbcmtd[i])
-			continue;
-
-		sbcmtd[i]->owner = THIS_MODULE;
-
-		/* No partitioning detected. Use default */
-		if (i == 2) {
-			defparts = NULL;
-			nr_parts = 0;
-		} else if (i == bigflash) {
-			defparts = bigflash_parts;
-			nr_parts = ARRAY_SIZE(bigflash_parts);
-		} else {
-			defparts = smallflash_parts;
-			nr_parts = ARRAY_SIZE(smallflash_parts);
-		}
-
-		mtd_device_parse_register(sbcmtd[i], part_probes, NULL,
-					  defparts, nr_parts);
-	}
-	return 0;
-}
-
-static void __exit cleanup_sbc82xx_flash(void)
-{
-	int i;
-
-	for (i=0; i<3; i++) {
-		if (!sbcmtd[i])
-			continue;
-
-		mtd_device_unregister(sbcmtd[i]);
-
-		map_destroy(sbcmtd[i]);
-
-		iounmap((void *)sbc82xx_flash_map[i].virt);
-		sbc82xx_flash_map[i].virt = 0;
-	}
-}
-
-module_init(init_sbc82xx_flash);
-module_exit(cleanup_sbc82xx_flash);
-
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("David Woodhouse <dwmw2@infradead.org>");
-MODULE_DESCRIPTION("Flash map driver for WindRiver PowerQUICC II");
diff --git a/drivers/mtd/mtdchar.c b/drivers/mtd/mtdchar.c
index a6e74514e66..82c06165d3d 100644
--- a/drivers/mtd/mtdchar.c
+++ b/drivers/mtd/mtdchar.c
@@ -1162,7 +1162,11 @@ static int mtdchar_mmap(struct file *file, struct vm_area_struct *vma)
 	resource_size_t start, off;
 	unsigned long len, vma_len;
 
-	if (mtd->type == MTD_RAM || mtd->type == MTD_ROM) {
+        /* This is broken because it assumes the MTD device is map-based
+	   and that mtd->priv is a valid struct map_info.  It should be
+	   replaced with something that uses the mtd_get_unmapped_area()
+	   operation properly. */
+	if (0 /*mtd->type == MTD_RAM || mtd->type == MTD_ROM*/) {
 		off = get_vm_offset(vma);
 		start = map->phys;
 		len = PAGE_ALIGN((start & ~PAGE_MASK) + map->size);
@@ -1182,7 +1186,7 @@ static int mtdchar_mmap(struct file *file, struct vm_area_struct *vma)
 			return -EINVAL;
 		if (set_vm_offset(vma, off) < 0)
 			return -EINVAL;
-		vma->vm_flags |= VM_IO | VM_RESERVED;
+		vma->vm_flags |= VM_IO | VM_DONTEXPAND | VM_DONTDUMP;
 
 #ifdef pgprot_noncached
 		if (file->f_flags & O_DSYNC || off >= __pa(high_memory))
diff --git a/drivers/mtd/mtdcore.c b/drivers/mtd/mtdcore.c
index 575730744fd..374c46dff7d 100644
--- a/drivers/mtd/mtdcore.c
+++ b/drivers/mtd/mtdcore.c
@@ -858,6 +858,27 @@ int mtd_panic_write(struct mtd_info *mtd, loff_t to, size_t len, size_t *retlen,
 }
 EXPORT_SYMBOL_GPL(mtd_panic_write);
 
+int mtd_read_oob(struct mtd_info *mtd, loff_t from, struct mtd_oob_ops *ops)
+{
+	int ret_code;
+	ops->retlen = ops->oobretlen = 0;
+	if (!mtd->_read_oob)
+		return -EOPNOTSUPP;
+	/*
+	 * In cases where ops->datbuf != NULL, mtd->_read_oob() has semantics
+	 * similar to mtd->_read(), returning a non-negative integer
+	 * representing max bitflips. In other cases, mtd->_read_oob() may
+	 * return -EUCLEAN. In all cases, perform similar logic to mtd_read().
+	 */
+	ret_code = mtd->_read_oob(mtd, from, ops);
+	if (unlikely(ret_code < 0))
+		return ret_code;
+	if (mtd->ecc_strength == 0)
+		return 0;	/* device lacks ecc */
+	return ret_code >= mtd->bitflip_threshold ? -EUCLEAN : 0;
+}
+EXPORT_SYMBOL_GPL(mtd_read_oob);
+
 /*
  * Method to access the protection register area, present in some flash
  * devices. The user data is one time programmable but the factory data is read
@@ -1056,8 +1077,7 @@ EXPORT_SYMBOL_GPL(mtd_writev);
  * until the request succeeds or until the allocation size falls below
  * the system page size. This attempts to make sure it does not adversely
  * impact system performance, so when allocating more than one page, we
- * ask the memory allocator to avoid re-trying, swapping, writing back
- * or performing I/O.
+ * ask the memory allocator to avoid re-trying.
  *
  * Note, this function also makes sure that the allocated buffer is aligned to
  * the MTD device's min. I/O unit, i.e. the "mtd->writesize" value.
@@ -1071,8 +1091,7 @@ EXPORT_SYMBOL_GPL(mtd_writev);
  */
 void *mtd_kmalloc_up_to(const struct mtd_info *mtd, size_t *size)
 {
-	gfp_t flags = __GFP_NOWARN | __GFP_WAIT |
-		       __GFP_NORETRY | __GFP_NO_KSWAPD;
+	gfp_t flags = __GFP_NOWARN | __GFP_WAIT | __GFP_NORETRY;
 	size_t min_alloc = max_t(size_t, mtd->writesize, PAGE_SIZE);
 	void *kbuf;
 
diff --git a/drivers/mtd/mtdoops.c b/drivers/mtd/mtdoops.c
index 438737a1f59..f5b3f91fa1c 100644
--- a/drivers/mtd/mtdoops.c
+++ b/drivers/mtd/mtdoops.c
@@ -169,14 +169,7 @@ static void mtdoops_workfunc_erase(struct work_struct *work)
 			cxt->nextpage = 0;
 	}
 
-	while (1) {
-		ret = mtd_block_isbad(mtd, cxt->nextpage * record_size);
-		if (!ret)
-			break;
-		if (ret < 0) {
-			printk(KERN_ERR "mtdoops: block_isbad failed, aborting\n");
-			return;
-		}
+	while ((ret = mtd_block_isbad(mtd, cxt->nextpage * record_size)) > 0) {
 badblock:
 		printk(KERN_WARNING "mtdoops: bad block at %08lx\n",
 		       cxt->nextpage * record_size);
@@ -190,6 +183,11 @@ badblock:
 		}
 	}
 
+	if (ret < 0) {
+		printk(KERN_ERR "mtdoops: mtd_block_isbad failed, aborting\n");
+		return;
+	}
+
 	for (j = 0, ret = -1; (j < 3) && (ret < 0); j++)
 		ret = mtdoops_erase_block(cxt, cxt->nextpage * record_size);
 
diff --git a/drivers/mtd/mtdpart.c b/drivers/mtd/mtdpart.c
index 3a49e6de5e6..70fa70a8318 100644
--- a/drivers/mtd/mtdpart.c
+++ b/drivers/mtd/mtdpart.c
@@ -711,6 +711,8 @@ static const char *default_mtd_part_types[] = {
  * partition parsers, specified in @types. However, if @types is %NULL, then
  * the default list of parsers is used. The default list contains only the
  * "cmdlinepart" and "ofpart" parsers ATM.
+ * Note: If there are more then one parser in @types, the kernel only takes the
+ * partitions parsed out by the first parser.
  *
  * This function may return:
  * o a negative error code in case of failure
@@ -735,11 +737,12 @@ int parse_mtd_partitions(struct mtd_info *master, const char **types,
 		if (!parser)
 			continue;
 		ret = (*parser->parse_fn)(master, pparts, data);
+		put_partition_parser(parser);
 		if (ret > 0) {
 			printk(KERN_NOTICE "%d %s partitions found on MTD device %s\n",
 			       ret, parser->name, master->name);
+			break;
 		}
-		put_partition_parser(parser);
 	}
 	return ret;
 }
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index 598cd0a3ade..4883139460b 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -22,15 +22,6 @@ menuconfig MTD_NAND
 
 if MTD_NAND
 
-config MTD_NAND_VERIFY_WRITE
-	bool "Verify NAND page writes"
-	help
-	  This adds an extra check when data is written to the flash. The
-	  NAND flash device internally checks only bits transitioning
-	  from 1 to 0. There is a rare possibility that even though the
-	  device thinks the write was successful, a bit could have been
-	  flipped accidentally due to device wear or something else.
-
 config MTD_NAND_BCH
 	tristate
 	select BCH
@@ -267,22 +258,6 @@ config MTD_NAND_S3C2410_CLKSTOP
 	  when the is NAND chip selected or released, but will save
 	  approximately 5mA of power when there is nothing happening.
 
-config MTD_NAND_BCM_UMI
-	tristate "NAND Flash support for BCM Reference Boards"
-	depends on ARCH_BCMRING
-	help
-	  This enables the NAND flash controller on the BCM UMI block.
-
-	  No board specific support is done by this driver, each board
-	  must advertise a platform_device for the driver to attach.
-
-config MTD_NAND_BCM_UMI_HWCS
-	bool "BCM UMI NAND Hardware CS"
-	depends on MTD_NAND_BCM_UMI
-	help
-	  Enable the use of the BCM UMI block's internal CS using NAND.
-	  This should only be used if you know the external NAND CS can toggle.
-
 config MTD_NAND_DISKONCHIP
 	tristate "DiskOnChip 2000, Millennium and Millennium Plus (NAND reimplementation) (EXPERIMENTAL)"
 	depends on EXPERIMENTAL
@@ -356,7 +331,7 @@ config MTD_NAND_DISKONCHIP_BBTWRITE
 
 config MTD_NAND_DOCG4
 	tristate "Support for DiskOnChip G4 (EXPERIMENTAL)"
-	depends on EXPERIMENTAL
+	depends on EXPERIMENTAL && HAS_IOMEM
 	select BCH
 	select BITREVERSE
 	help
@@ -414,6 +389,28 @@ config MTD_NAND_PXA3xx
 	  This enables the driver for the NAND flash device found on
 	  PXA3xx processors
 
+config MTD_NAND_SLC_LPC32XX
+	tristate "NXP LPC32xx SLC Controller"
+	depends on ARCH_LPC32XX
+	help
+	  Enables support for NXP's LPC32XX SLC (i.e. for Single Level Cell
+	  chips) NAND controller. This is the default for the PHYTEC 3250
+	  reference board which contains a NAND256R3A2CZA6 chip.
+
+	  Please check the actual NAND chip connected and its support
+	  by the SLC NAND controller.
+
+config MTD_NAND_MLC_LPC32XX
+	tristate "NXP LPC32xx MLC Controller"
+	depends on ARCH_LPC32XX
+	help
+	  Uses the LPC32XX MLC (i.e. for Multi Level Cell chips) NAND
+	  controller. This is the default for the WORK92105 controller
+	  board.
+
+	  Please check the actual NAND chip connected and its support
+	  by the MLC NAND controller.
+
 config MTD_NAND_CM_X270
 	tristate "Support for NAND Flash on CM-X270 modules"
 	depends on MACH_ARMCORE
@@ -439,10 +436,10 @@ config MTD_NAND_NANDSIM
 	  MTD nand layer.
 
 config MTD_NAND_GPMI_NAND
-        bool "GPMI NAND Flash Controller driver"
+        tristate "GPMI NAND Flash Controller driver"
         depends on MTD_NAND && MXS_DMA
         help
-	 Enables NAND Flash support for IMX23 or IMX28.
+	 Enables NAND Flash support for IMX23, IMX28 or IMX6.
 	 The GPMI controller is very powerful, with the help of BCH
 	 module, it can do the hardware ECC. The GPMI supports several
 	 NAND flashs at the same time. The GPMI may conflicts with other
@@ -510,7 +507,7 @@ config MTD_NAND_MPC5121_NFC
 
 config MTD_NAND_MXC
 	tristate "MXC NAND support"
-	depends on IMX_HAVE_PLATFORM_MXC_NAND
+	depends on ARCH_MXC
 	help
 	  This enables the driver for the NAND flash controller on the
 	  MXC processors.
@@ -567,4 +564,12 @@ config MTD_NAND_FSMC
 	  Enables support for NAND Flash chips on the ST Microelectronics
 	  Flexible Static Memory Controller (FSMC)
 
+config MTD_NAND_XWAY
+	tristate "Support for NAND on Lantiq XWAY SoC"
+	depends on LANTIQ && SOC_TYPE_XWAY
+	select MTD_NAND_PLATFORM
+	help
+	  Enables support for NAND Flash chips on Lantiq XWAY SoCs. NAND is attached
+	  to the External Bus Unit (EBU).
+
 endif # MTD_NAND
diff --git a/drivers/mtd/nand/Makefile b/drivers/mtd/nand/Makefile
index d4b4d8739bd..2cbd0916b73 100644
--- a/drivers/mtd/nand/Makefile
+++ b/drivers/mtd/nand/Makefile
@@ -40,16 +40,18 @@ obj-$(CONFIG_MTD_NAND_ORION)		+= orion_nand.o
 obj-$(CONFIG_MTD_NAND_FSL_ELBC)		+= fsl_elbc_nand.o
 obj-$(CONFIG_MTD_NAND_FSL_IFC)		+= fsl_ifc_nand.o
 obj-$(CONFIG_MTD_NAND_FSL_UPM)		+= fsl_upm.o
+obj-$(CONFIG_MTD_NAND_SLC_LPC32XX)      += lpc32xx_slc.o
+obj-$(CONFIG_MTD_NAND_MLC_LPC32XX)      += lpc32xx_mlc.o
 obj-$(CONFIG_MTD_NAND_SH_FLCTL)		+= sh_flctl.o
 obj-$(CONFIG_MTD_NAND_MXC)		+= mxc_nand.o
 obj-$(CONFIG_MTD_NAND_SOCRATES)		+= socrates_nand.o
 obj-$(CONFIG_MTD_NAND_TXX9NDFMC)	+= txx9ndfmc.o
 obj-$(CONFIG_MTD_NAND_NUC900)		+= nuc900_nand.o
 obj-$(CONFIG_MTD_NAND_NOMADIK)		+= nomadik_nand.o
-obj-$(CONFIG_MTD_NAND_BCM_UMI)		+= bcm_umi_nand.o nand_bcm_umi.o
 obj-$(CONFIG_MTD_NAND_MPC5121_NFC)	+= mpc5121_nfc.o
 obj-$(CONFIG_MTD_NAND_RICOH)		+= r852.o
 obj-$(CONFIG_MTD_NAND_JZ4740)		+= jz4740_nand.o
 obj-$(CONFIG_MTD_NAND_GPMI_NAND)	+= gpmi-nand/
+obj-$(CONFIG_MTD_NAND_XWAY)		+= xway_nand.o
 
 nand-objs := nand_base.o nand_bbt.o
diff --git a/drivers/mtd/nand/ams-delta.c b/drivers/mtd/nand/ams-delta.c
index a7040af0853..9e7723aa7ac 100644
--- a/drivers/mtd/nand/ams-delta.c
+++ b/drivers/mtd/nand/ams-delta.c
@@ -107,18 +107,6 @@ static void ams_delta_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 		buf[i] = ams_delta_read_byte(mtd);
 }
 
-static int ams_delta_verify_buf(struct mtd_info *mtd, const u_char *buf,
-				int len)
-{
-	int i;
-
-	for (i=0; i<len; i++)
-		if (buf[i] != ams_delta_read_byte(mtd))
-			return -EFAULT;
-
-	return 0;
-}
-
 /*
  * Command control function
  *
@@ -237,7 +225,6 @@ static int __devinit ams_delta_init(struct platform_device *pdev)
 	this->read_byte = ams_delta_read_byte;
 	this->write_buf = ams_delta_write_buf;
 	this->read_buf = ams_delta_read_buf;
-	this->verify_buf = ams_delta_verify_buf;
 	this->cmd_ctrl = ams_delta_hwcontrol;
 	if (gpio_request(AMS_DELTA_GPIO_PIN_NAND_RB, "nand_rdy") == 0) {
 		this->dev_ready = ams_delta_nand_ready;
diff --git a/drivers/mtd/nand/atmel_nand.c b/drivers/mtd/nand/atmel_nand.c
index 97ac6712bb1..91445578330 100644
--- a/drivers/mtd/nand/atmel_nand.c
+++ b/drivers/mtd/nand/atmel_nand.c
@@ -1,20 +1,22 @@
 /*
- *  Copyright (C) 2003 Rick Bronson
+ *  Copyright © 2003 Rick Bronson
  *
  *  Derived from drivers/mtd/nand/autcpu12.c
- *	 Copyright (c) 2001 Thomas Gleixner (gleixner@autronix.de)
+ *	 Copyright © 2001 Thomas Gleixner (gleixner@autronix.de)
  *
  *  Derived from drivers/mtd/spia.c
- *	 Copyright (C) 2000 Steven J. Hill (sjhill@cotw.com)
+ *	 Copyright © 2000 Steven J. Hill (sjhill@cotw.com)
  *
  *
  *  Add Hardware ECC support for AT91SAM9260 / AT91SAM9263
- *     Richard Genoud (richard.genoud@gmail.com), Adeneo Copyright (C) 2007
+ *     Richard Genoud (richard.genoud@gmail.com), Adeneo Copyright © 2007
  *
  *     Derived from Das U-Boot source code
  *     		(u-boot-1.1.5/board/atmel/at91sam9263ek/nand.c)
- *     (C) Copyright 2006 ATMEL Rousset, Lacressonniere Nicolas
+ *     © Copyright 2006 ATMEL Rousset, Lacressonniere Nicolas
  *
+ *  Add Programmable Multibit ECC support for various AT91 SoC
+ *     © Copyright 2012 ATMEL, Hong Xu
  *
  * 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
@@ -93,8 +95,36 @@ struct atmel_nand_host {
 
 	struct completion	comp;
 	struct dma_chan		*dma_chan;
+
+	bool			has_pmecc;
+	u8			pmecc_corr_cap;
+	u16			pmecc_sector_size;
+	u32			pmecc_lookup_table_offset;
+
+	int			pmecc_bytes_per_sector;
+	int			pmecc_sector_number;
+	int			pmecc_degree;	/* Degree of remainders */
+	int			pmecc_cw_len;	/* Length of codeword */
+
+	void __iomem		*pmerrloc_base;
+	void __iomem		*pmecc_rom_base;
+
+	/* lookup table for alpha_to and index_of */
+	void __iomem		*pmecc_alpha_to;
+	void __iomem		*pmecc_index_of;
+
+	/* data for pmecc computation */
+	int16_t			*pmecc_partial_syn;
+	int16_t			*pmecc_si;
+	int16_t			*pmecc_smu;	/* Sigma table */
+	int16_t			*pmecc_lmu;	/* polynomal order */
+	int			*pmecc_mu;
+	int			*pmecc_dmu;
+	int			*pmecc_delta;
 };
 
+static struct nand_ecclayout atmel_pmecc_oobinfo;
+
 static int cpu_has_dma(void)
 {
 	return cpu_is_at91sam9rl() || cpu_is_at91sam9g45();
@@ -288,6 +318,703 @@ static void atmel_write_buf(struct mtd_info *mtd, const u8 *buf, int len)
 }
 
 /*
+ * Return number of ecc bytes per sector according to sector size and
+ * correction capability
+ *
+ * Following table shows what at91 PMECC supported:
+ * Correction Capability	Sector_512_bytes	Sector_1024_bytes
+ * =====================	================	=================
+ *                2-bits                 4-bytes                  4-bytes
+ *                4-bits                 7-bytes                  7-bytes
+ *                8-bits                13-bytes                 14-bytes
+ *               12-bits                20-bytes                 21-bytes
+ *               24-bits                39-bytes                 42-bytes
+ */
+static int __devinit pmecc_get_ecc_bytes(int cap, int sector_size)
+{
+	int m = 12 + sector_size / 512;
+	return (m * cap + 7) / 8;
+}
+
+static void __devinit pmecc_config_ecc_layout(struct nand_ecclayout *layout,
+	int oobsize, int ecc_len)
+{
+	int i;
+
+	layout->eccbytes = ecc_len;
+
+	/* ECC will occupy the last ecc_len bytes continuously */
+	for (i = 0; i < ecc_len; i++)
+		layout->eccpos[i] = oobsize - ecc_len + i;
+
+	layout->oobfree[0].offset = 2;
+	layout->oobfree[0].length =
+		oobsize - ecc_len - layout->oobfree[0].offset;
+}
+
+static void __devinit __iomem *pmecc_get_alpha_to(struct atmel_nand_host *host)
+{
+	int table_size;
+
+	table_size = host->pmecc_sector_size == 512 ?
+		PMECC_LOOKUP_TABLE_SIZE_512 : PMECC_LOOKUP_TABLE_SIZE_1024;
+
+	return host->pmecc_rom_base + host->pmecc_lookup_table_offset +
+			table_size * sizeof(int16_t);
+}
+
+static void pmecc_data_free(struct atmel_nand_host *host)
+{
+	kfree(host->pmecc_partial_syn);
+	kfree(host->pmecc_si);
+	kfree(host->pmecc_lmu);
+	kfree(host->pmecc_smu);
+	kfree(host->pmecc_mu);
+	kfree(host->pmecc_dmu);
+	kfree(host->pmecc_delta);
+}
+
+static int __devinit pmecc_data_alloc(struct atmel_nand_host *host)
+{
+	const int cap = host->pmecc_corr_cap;
+
+	host->pmecc_partial_syn = kzalloc((2 * cap + 1) * sizeof(int16_t),
+					GFP_KERNEL);
+	host->pmecc_si = kzalloc((2 * cap + 1) * sizeof(int16_t), GFP_KERNEL);
+	host->pmecc_lmu = kzalloc((cap + 1) * sizeof(int16_t), GFP_KERNEL);
+	host->pmecc_smu = kzalloc((cap + 2) * (2 * cap + 1) * sizeof(int16_t),
+					GFP_KERNEL);
+	host->pmecc_mu = kzalloc((cap + 1) * sizeof(int), GFP_KERNEL);
+	host->pmecc_dmu = kzalloc((cap + 1) * sizeof(int), GFP_KERNEL);
+	host->pmecc_delta = kzalloc((cap + 1) * sizeof(int), GFP_KERNEL);
+
+	if (host->pmecc_partial_syn &&
+			host->pmecc_si &&
+			host->pmecc_lmu &&
+			host->pmecc_smu &&
+			host->pmecc_mu &&
+			host->pmecc_dmu &&
+			host->pmecc_delta)
+		return 0;
+
+	/* error happened */
+	pmecc_data_free(host);
+	return -ENOMEM;
+}
+
+static void pmecc_gen_syndrome(struct mtd_info *mtd, int sector)
+{
+	struct nand_chip *nand_chip = mtd->priv;
+	struct atmel_nand_host *host = nand_chip->priv;
+	int i;
+	uint32_t value;
+
+	/* Fill odd syndromes */
+	for (i = 0; i < host->pmecc_corr_cap; i++) {
+		value = pmecc_readl_rem_relaxed(host->ecc, sector, i / 2);
+		if (i & 1)
+			value >>= 16;
+		value &= 0xffff;
+		host->pmecc_partial_syn[(2 * i) + 1] = (int16_t)value;
+	}
+}
+
+static void pmecc_substitute(struct mtd_info *mtd)
+{
+	struct nand_chip *nand_chip = mtd->priv;
+	struct atmel_nand_host *host = nand_chip->priv;
+	int16_t __iomem *alpha_to = host->pmecc_alpha_to;
+	int16_t __iomem *index_of = host->pmecc_index_of;
+	int16_t *partial_syn = host->pmecc_partial_syn;
+	const int cap = host->pmecc_corr_cap;
+	int16_t *si;
+	int i, j;
+
+	/* si[] is a table that holds the current syndrome value,
+	 * an element of that table belongs to the field
+	 */
+	si = host->pmecc_si;
+
+	memset(&si[1], 0, sizeof(int16_t) * (2 * cap - 1));
+
+	/* Computation 2t syndromes based on S(x) */
+	/* Odd syndromes */
+	for (i = 1; i < 2 * cap; i += 2) {
+		for (j = 0; j < host->pmecc_degree; j++) {
+			if (partial_syn[i] & ((unsigned short)0x1 << j))
+				si[i] = readw_relaxed(alpha_to + i * j) ^ si[i];
+		}
+	}
+	/* Even syndrome = (Odd syndrome) ** 2 */
+	for (i = 2, j = 1; j <= cap; i = ++j << 1) {
+		if (si[j] == 0) {
+			si[i] = 0;
+		} else {
+			int16_t tmp;
+
+			tmp = readw_relaxed(index_of + si[j]);
+			tmp = (tmp * 2) % host->pmecc_cw_len;
+			si[i] = readw_relaxed(alpha_to + tmp);
+		}
+	}
+
+	return;
+}
+
+static void pmecc_get_sigma(struct mtd_info *mtd)
+{
+	struct nand_chip *nand_chip = mtd->priv;
+	struct atmel_nand_host *host = nand_chip->priv;
+
+	int16_t *lmu = host->pmecc_lmu;
+	int16_t *si = host->pmecc_si;
+	int *mu = host->pmecc_mu;
+	int *dmu = host->pmecc_dmu;	/* Discrepancy */
+	int *delta = host->pmecc_delta; /* Delta order */
+	int cw_len = host->pmecc_cw_len;
+	const int16_t cap = host->pmecc_corr_cap;
+	const int num = 2 * cap + 1;
+	int16_t __iomem	*index_of = host->pmecc_index_of;
+	int16_t __iomem	*alpha_to = host->pmecc_alpha_to;
+	int i, j, k;
+	uint32_t dmu_0_count, tmp;
+	int16_t *smu = host->pmecc_smu;
+
+	/* index of largest delta */
+	int ro;
+	int largest;
+	int diff;
+
+	dmu_0_count = 0;
+
+	/* First Row */
+
+	/* Mu */
+	mu[0] = -1;
+
+	memset(smu, 0, sizeof(int16_t) * num);
+	smu[0] = 1;
+
+	/* discrepancy set to 1 */
+	dmu[0] = 1;
+	/* polynom order set to 0 */
+	lmu[0] = 0;
+	delta[0] = (mu[0] * 2 - lmu[0]) >> 1;
+
+	/* Second Row */
+
+	/* Mu */
+	mu[1] = 0;
+	/* Sigma(x) set to 1 */
+	memset(&smu[num], 0, sizeof(int16_t) * num);
+	smu[num] = 1;
+
+	/* discrepancy set to S1 */
+	dmu[1] = si[1];
+
+	/* polynom order set to 0 */
+	lmu[1] = 0;
+
+	delta[1] = (mu[1] * 2 - lmu[1]) >> 1;
+
+	/* Init the Sigma(x) last row */
+	memset(&smu[(cap + 1) * num], 0, sizeof(int16_t) * num);
+
+	for (i = 1; i <= cap; i++) {
+		mu[i + 1] = i << 1;
+		/* Begin Computing Sigma (Mu+1) and L(mu) */
+		/* check if discrepancy is set to 0 */
+		if (dmu[i] == 0) {
+			dmu_0_count++;
+
+			tmp = ((cap - (lmu[i] >> 1) - 1) / 2);
+			if ((cap - (lmu[i] >> 1) - 1) & 0x1)
+				tmp += 2;
+			else
+				tmp += 1;
+
+			if (dmu_0_count == tmp) {
+				for (j = 0; j <= (lmu[i] >> 1) + 1; j++)
+					smu[(cap + 1) * num + j] =
+							smu[i * num + j];
+
+				lmu[cap + 1] = lmu[i];
+				return;
+			}
+
+			/* copy polynom */
+			for (j = 0; j <= lmu[i] >> 1; j++)
+				smu[(i + 1) * num + j] = smu[i * num + j];
+
+			/* copy previous polynom order to the next */
+			lmu[i + 1] = lmu[i];
+		} else {
+			ro = 0;
+			largest = -1;
+			/* find largest delta with dmu != 0 */
+			for (j = 0; j < i; j++) {
+				if ((dmu[j]) && (delta[j] > largest)) {
+					largest = delta[j];
+					ro = j;
+				}
+			}
+
+			/* compute difference */
+			diff = (mu[i] - mu[ro]);
+
+			/* Compute degree of the new smu polynomial */
+			if ((lmu[i] >> 1) > ((lmu[ro] >> 1) + diff))
+				lmu[i + 1] = lmu[i];
+			else
+				lmu[i + 1] = ((lmu[ro] >> 1) + diff) * 2;
+
+			/* Init smu[i+1] with 0 */
+			for (k = 0; k < num; k++)
+				smu[(i + 1) * num + k] = 0;
+
+			/* Compute smu[i+1] */
+			for (k = 0; k <= lmu[ro] >> 1; k++) {
+				int16_t a, b, c;
+
+				if (!(smu[ro * num + k] && dmu[i]))
+					continue;
+				a = readw_relaxed(index_of + dmu[i]);
+				b = readw_relaxed(index_of + dmu[ro]);
+				c = readw_relaxed(index_of + smu[ro * num + k]);
+				tmp = a + (cw_len - b) + c;
+				a = readw_relaxed(alpha_to + tmp % cw_len);
+				smu[(i + 1) * num + (k + diff)] = a;
+			}
+
+			for (k = 0; k <= lmu[i] >> 1; k++)
+				smu[(i + 1) * num + k] ^= smu[i * num + k];
+		}
+
+		/* End Computing Sigma (Mu+1) and L(mu) */
+		/* In either case compute delta */
+		delta[i + 1] = (mu[i + 1] * 2 - lmu[i + 1]) >> 1;
+
+		/* Do not compute discrepancy for the last iteration */
+		if (i >= cap)
+			continue;
+
+		for (k = 0; k <= (lmu[i + 1] >> 1); k++) {
+			tmp = 2 * (i - 1);
+			if (k == 0) {
+				dmu[i + 1] = si[tmp + 3];
+			} else if (smu[(i + 1) * num + k] && si[tmp + 3 - k]) {
+				int16_t a, b, c;
+				a = readw_relaxed(index_of +
+						smu[(i + 1) * num + k]);
+				b = si[2 * (i - 1) + 3 - k];
+				c = readw_relaxed(index_of + b);
+				tmp = a + c;
+				tmp %= cw_len;
+				dmu[i + 1] = readw_relaxed(alpha_to + tmp) ^
+					dmu[i + 1];
+			}
+		}
+	}
+
+	return;
+}
+
+static int pmecc_err_location(struct mtd_info *mtd)
+{
+	struct nand_chip *nand_chip = mtd->priv;
+	struct atmel_nand_host *host = nand_chip->priv;
+	unsigned long end_time;
+	const int cap = host->pmecc_corr_cap;
+	const int num = 2 * cap + 1;
+	int sector_size = host->pmecc_sector_size;
+	int err_nbr = 0;	/* number of error */
+	int roots_nbr;		/* number of roots */
+	int i;
+	uint32_t val;
+	int16_t *smu = host->pmecc_smu;
+
+	pmerrloc_writel(host->pmerrloc_base, ELDIS, PMERRLOC_DISABLE);
+
+	for (i = 0; i <= host->pmecc_lmu[cap + 1] >> 1; i++) {
+		pmerrloc_writel_sigma_relaxed(host->pmerrloc_base, i,
+				      smu[(cap + 1) * num + i]);
+		err_nbr++;
+	}
+
+	val = (err_nbr - 1) << 16;
+	if (sector_size == 1024)
+		val |= 1;
+
+	pmerrloc_writel(host->pmerrloc_base, ELCFG, val);
+	pmerrloc_writel(host->pmerrloc_base, ELEN,
+			sector_size * 8 + host->pmecc_degree * cap);
+
+	end_time = jiffies + msecs_to_jiffies(PMECC_MAX_TIMEOUT_MS);
+	while (!(pmerrloc_readl_relaxed(host->pmerrloc_base, ELISR)
+		 & PMERRLOC_CALC_DONE)) {
+		if (unlikely(time_after(jiffies, end_time))) {
+			dev_err(host->dev, "PMECC: Timeout to calculate error location.\n");
+			return -1;
+		}
+		cpu_relax();
+	}
+
+	roots_nbr = (pmerrloc_readl_relaxed(host->pmerrloc_base, ELISR)
+		& PMERRLOC_ERR_NUM_MASK) >> 8;
+	/* Number of roots == degree of smu hence <= cap */
+	if (roots_nbr == host->pmecc_lmu[cap + 1] >> 1)
+		return err_nbr - 1;
+
+	/* Number of roots does not match the degree of smu
+	 * unable to correct error */
+	return -1;
+}
+
+static void pmecc_correct_data(struct mtd_info *mtd, uint8_t *buf, uint8_t *ecc,
+		int sector_num, int extra_bytes, int err_nbr)
+{
+	struct nand_chip *nand_chip = mtd->priv;
+	struct atmel_nand_host *host = nand_chip->priv;
+	int i = 0;
+	int byte_pos, bit_pos, sector_size, pos;
+	uint32_t tmp;
+	uint8_t err_byte;
+
+	sector_size = host->pmecc_sector_size;
+
+	while (err_nbr) {
+		tmp = pmerrloc_readl_el_relaxed(host->pmerrloc_base, i) - 1;
+		byte_pos = tmp / 8;
+		bit_pos  = tmp % 8;
+
+		if (byte_pos >= (sector_size + extra_bytes))
+			BUG();	/* should never happen */
+
+		if (byte_pos < sector_size) {
+			err_byte = *(buf + byte_pos);
+			*(buf + byte_pos) ^= (1 << bit_pos);
+
+			pos = sector_num * host->pmecc_sector_size + byte_pos;
+			dev_info(host->dev, "Bit flip in data area, byte_pos: %d, bit_pos: %d, 0x%02x -> 0x%02x\n",
+				pos, bit_pos, err_byte, *(buf + byte_pos));
+		} else {
+			/* Bit flip in OOB area */
+			tmp = sector_num * host->pmecc_bytes_per_sector
+					+ (byte_pos - sector_size);
+			err_byte = ecc[tmp];
+			ecc[tmp] ^= (1 << bit_pos);
+
+			pos = tmp + nand_chip->ecc.layout->eccpos[0];
+			dev_info(host->dev, "Bit flip in OOB, oob_byte_pos: %d, bit_pos: %d, 0x%02x -> 0x%02x\n",
+				pos, bit_pos, err_byte, ecc[tmp]);
+		}
+
+		i++;
+		err_nbr--;
+	}
+
+	return;
+}
+
+static int pmecc_correction(struct mtd_info *mtd, u32 pmecc_stat, uint8_t *buf,
+	u8 *ecc)
+{
+	struct nand_chip *nand_chip = mtd->priv;
+	struct atmel_nand_host *host = nand_chip->priv;
+	int i, err_nbr, eccbytes;
+	uint8_t *buf_pos;
+
+	eccbytes = nand_chip->ecc.bytes;
+	for (i = 0; i < eccbytes; i++)
+		if (ecc[i] != 0xff)
+			goto normal_check;
+	/* Erased page, return OK */
+	return 0;
+
+normal_check:
+	for (i = 0; i < host->pmecc_sector_number; i++) {
+		err_nbr = 0;
+		if (pmecc_stat & 0x1) {
+			buf_pos = buf + i * host->pmecc_sector_size;
+
+			pmecc_gen_syndrome(mtd, i);
+			pmecc_substitute(mtd);
+			pmecc_get_sigma(mtd);
+
+			err_nbr = pmecc_err_location(mtd);
+			if (err_nbr == -1) {
+				dev_err(host->dev, "PMECC: Too many errors\n");
+				mtd->ecc_stats.failed++;
+				return -EIO;
+			} else {
+				pmecc_correct_data(mtd, buf_pos, ecc, i,
+					host->pmecc_bytes_per_sector, err_nbr);
+				mtd->ecc_stats.corrected += err_nbr;
+			}
+		}
+		pmecc_stat >>= 1;
+	}
+
+	return 0;
+}
+
+static int atmel_nand_pmecc_read_page(struct mtd_info *mtd,
+	struct nand_chip *chip, uint8_t *buf, int oob_required, int page)
+{
+	struct atmel_nand_host *host = chip->priv;
+	int eccsize = chip->ecc.size;
+	uint8_t *oob = chip->oob_poi;
+	uint32_t *eccpos = chip->ecc.layout->eccpos;
+	uint32_t stat;
+	unsigned long end_time;
+
+	pmecc_writel(host->ecc, CTRL, PMECC_CTRL_RST);
+	pmecc_writel(host->ecc, CTRL, PMECC_CTRL_DISABLE);
+	pmecc_writel(host->ecc, CFG, (pmecc_readl_relaxed(host->ecc, CFG)
+		& ~PMECC_CFG_WRITE_OP) | PMECC_CFG_AUTO_ENABLE);
+
+	pmecc_writel(host->ecc, CTRL, PMECC_CTRL_ENABLE);
+	pmecc_writel(host->ecc, CTRL, PMECC_CTRL_DATA);
+
+	chip->read_buf(mtd, buf, eccsize);
+	chip->read_buf(mtd, oob, mtd->oobsize);
+
+	end_time = jiffies + msecs_to_jiffies(PMECC_MAX_TIMEOUT_MS);
+	while ((pmecc_readl_relaxed(host->ecc, SR) & PMECC_SR_BUSY)) {
+		if (unlikely(time_after(jiffies, end_time))) {
+			dev_err(host->dev, "PMECC: Timeout to get error status.\n");
+			return -EIO;
+		}
+		cpu_relax();
+	}
+
+	stat = pmecc_readl_relaxed(host->ecc, ISR);
+	if (stat != 0)
+		if (pmecc_correction(mtd, stat, buf, &oob[eccpos[0]]) != 0)
+			return -EIO;
+
+	return 0;
+}
+
+static int atmel_nand_pmecc_write_page(struct mtd_info *mtd,
+		struct nand_chip *chip, const uint8_t *buf, int oob_required)
+{
+	struct atmel_nand_host *host = chip->priv;
+	uint32_t *eccpos = chip->ecc.layout->eccpos;
+	int i, j;
+	unsigned long end_time;
+
+	pmecc_writel(host->ecc, CTRL, PMECC_CTRL_RST);
+	pmecc_writel(host->ecc, CTRL, PMECC_CTRL_DISABLE);
+
+	pmecc_writel(host->ecc, CFG, (pmecc_readl_relaxed(host->ecc, CFG) |
+		PMECC_CFG_WRITE_OP) & ~PMECC_CFG_AUTO_ENABLE);
+
+	pmecc_writel(host->ecc, CTRL, PMECC_CTRL_ENABLE);
+	pmecc_writel(host->ecc, CTRL, PMECC_CTRL_DATA);
+
+	chip->write_buf(mtd, (u8 *)buf, mtd->writesize);
+
+	end_time = jiffies + msecs_to_jiffies(PMECC_MAX_TIMEOUT_MS);
+	while ((pmecc_readl_relaxed(host->ecc, SR) & PMECC_SR_BUSY)) {
+		if (unlikely(time_after(jiffies, end_time))) {
+			dev_err(host->dev, "PMECC: Timeout to get ECC value.\n");
+			return -EIO;
+		}
+		cpu_relax();
+	}
+
+	for (i = 0; i < host->pmecc_sector_number; i++) {
+		for (j = 0; j < host->pmecc_bytes_per_sector; j++) {
+			int pos;
+
+			pos = i * host->pmecc_bytes_per_sector + j;
+			chip->oob_poi[eccpos[pos]] =
+				pmecc_readb_ecc_relaxed(host->ecc, i, j);
+		}
+	}
+	chip->write_buf(mtd, chip->oob_poi, mtd->oobsize);
+
+	return 0;
+}
+
+static void atmel_pmecc_core_init(struct mtd_info *mtd)
+{
+	struct nand_chip *nand_chip = mtd->priv;
+	struct atmel_nand_host *host = nand_chip->priv;
+	uint32_t val = 0;
+	struct nand_ecclayout *ecc_layout;
+
+	pmecc_writel(host->ecc, CTRL, PMECC_CTRL_RST);
+	pmecc_writel(host->ecc, CTRL, PMECC_CTRL_DISABLE);
+
+	switch (host->pmecc_corr_cap) {
+	case 2:
+		val = PMECC_CFG_BCH_ERR2;
+		break;
+	case 4:
+		val = PMECC_CFG_BCH_ERR4;
+		break;
+	case 8:
+		val = PMECC_CFG_BCH_ERR8;
+		break;
+	case 12:
+		val = PMECC_CFG_BCH_ERR12;
+		break;
+	case 24:
+		val = PMECC_CFG_BCH_ERR24;
+		break;
+	}
+
+	if (host->pmecc_sector_size == 512)
+		val |= PMECC_CFG_SECTOR512;
+	else if (host->pmecc_sector_size == 1024)
+		val |= PMECC_CFG_SECTOR1024;
+
+	switch (host->pmecc_sector_number) {
+	case 1:
+		val |= PMECC_CFG_PAGE_1SECTOR;
+		break;
+	case 2:
+		val |= PMECC_CFG_PAGE_2SECTORS;
+		break;
+	case 4:
+		val |= PMECC_CFG_PAGE_4SECTORS;
+		break;
+	case 8:
+		val |= PMECC_CFG_PAGE_8SECTORS;
+		break;
+	}
+
+	val |= (PMECC_CFG_READ_OP | PMECC_CFG_SPARE_DISABLE
+		| PMECC_CFG_AUTO_DISABLE);
+	pmecc_writel(host->ecc, CFG, val);
+
+	ecc_layout = nand_chip->ecc.layout;
+	pmecc_writel(host->ecc, SAREA, mtd->oobsize - 1);
+	pmecc_writel(host->ecc, SADDR, ecc_layout->eccpos[0]);
+	pmecc_writel(host->ecc, EADDR,
+			ecc_layout->eccpos[ecc_layout->eccbytes - 1]);
+	/* See datasheet about PMECC Clock Control Register */
+	pmecc_writel(host->ecc, CLK, 2);
+	pmecc_writel(host->ecc, IDR, 0xff);
+	pmecc_writel(host->ecc, CTRL, PMECC_CTRL_ENABLE);
+}
+
+static int __init atmel_pmecc_nand_init_params(struct platform_device *pdev,
+					 struct atmel_nand_host *host)
+{
+	struct mtd_info *mtd = &host->mtd;
+	struct nand_chip *nand_chip = &host->nand_chip;
+	struct resource *regs, *regs_pmerr, *regs_rom;
+	int cap, sector_size, err_no;
+
+	cap = host->pmecc_corr_cap;
+	sector_size = host->pmecc_sector_size;
+	dev_info(host->dev, "Initialize PMECC params, cap: %d, sector: %d\n",
+		 cap, sector_size);
+
+	regs = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (!regs) {
+		dev_warn(host->dev,
+			"Can't get I/O resource regs for PMECC controller, rolling back on software ECC\n");
+		nand_chip->ecc.mode = NAND_ECC_SOFT;
+		return 0;
+	}
+
+	host->ecc = ioremap(regs->start, resource_size(regs));
+	if (host->ecc == NULL) {
+		dev_err(host->dev, "ioremap failed\n");
+		err_no = -EIO;
+		goto err_pmecc_ioremap;
+	}
+
+	regs_pmerr = platform_get_resource(pdev, IORESOURCE_MEM, 2);
+	regs_rom = platform_get_resource(pdev, IORESOURCE_MEM, 3);
+	if (regs_pmerr && regs_rom) {
+		host->pmerrloc_base = ioremap(regs_pmerr->start,
+			resource_size(regs_pmerr));
+		host->pmecc_rom_base = ioremap(regs_rom->start,
+			resource_size(regs_rom));
+	}
+
+	if (!host->pmerrloc_base || !host->pmecc_rom_base) {
+		dev_err(host->dev,
+			"Can not get I/O resource for PMECC ERRLOC controller or ROM!\n");
+		err_no = -EIO;
+		goto err_pmloc_ioremap;
+	}
+
+	/* ECC is calculated for the whole page (1 step) */
+	nand_chip->ecc.size = mtd->writesize;
+
+	/* set ECC page size and oob layout */
+	switch (mtd->writesize) {
+	case 2048:
+		host->pmecc_degree = PMECC_GF_DIMENSION_13;
+		host->pmecc_cw_len = (1 << host->pmecc_degree) - 1;
+		host->pmecc_sector_number = mtd->writesize / sector_size;
+		host->pmecc_bytes_per_sector = pmecc_get_ecc_bytes(
+			cap, sector_size);
+		host->pmecc_alpha_to = pmecc_get_alpha_to(host);
+		host->pmecc_index_of = host->pmecc_rom_base +
+			host->pmecc_lookup_table_offset;
+
+		nand_chip->ecc.steps = 1;
+		nand_chip->ecc.strength = cap;
+		nand_chip->ecc.bytes = host->pmecc_bytes_per_sector *
+				       host->pmecc_sector_number;
+		if (nand_chip->ecc.bytes > mtd->oobsize - 2) {
+			dev_err(host->dev, "No room for ECC bytes\n");
+			err_no = -EINVAL;
+			goto err_no_ecc_room;
+		}
+		pmecc_config_ecc_layout(&atmel_pmecc_oobinfo,
+					mtd->oobsize,
+					nand_chip->ecc.bytes);
+		nand_chip->ecc.layout = &atmel_pmecc_oobinfo;
+		break;
+	case 512:
+	case 1024:
+	case 4096:
+		/* TODO */
+		dev_warn(host->dev,
+			"Unsupported page size for PMECC, use Software ECC\n");
+	default:
+		/* page size not handled by HW ECC */
+		/* switching back to soft ECC */
+		nand_chip->ecc.mode = NAND_ECC_SOFT;
+		return 0;
+	}
+
+	/* Allocate data for PMECC computation */
+	err_no = pmecc_data_alloc(host);
+	if (err_no) {
+		dev_err(host->dev,
+				"Cannot allocate memory for PMECC computation!\n");
+		goto err_pmecc_data_alloc;
+	}
+
+	nand_chip->ecc.read_page = atmel_nand_pmecc_read_page;
+	nand_chip->ecc.write_page = atmel_nand_pmecc_write_page;
+
+	atmel_pmecc_core_init(mtd);
+
+	return 0;
+
+err_pmecc_data_alloc:
+err_no_ecc_room:
+err_pmloc_ioremap:
+	iounmap(host->ecc);
+	if (host->pmerrloc_base)
+		iounmap(host->pmerrloc_base);
+	if (host->pmecc_rom_base)
+		iounmap(host->pmecc_rom_base);
+err_pmecc_ioremap:
+	return err_no;
+}
+
+/*
  * Calculate HW ECC
  *
  * function called after a write
@@ -481,7 +1208,8 @@ static void atmel_nand_hwctl(struct mtd_info *mtd, int mode)
 static int __devinit atmel_of_init_port(struct atmel_nand_host *host,
 					 struct device_node *np)
 {
-	u32 val;
+	u32 val, table_offset;
+	u32 offset[2];
 	int ecc_mode;
 	struct atmel_nand_data *board = &host->board;
 	enum of_gpio_flags flags;
@@ -517,6 +1245,50 @@ static int __devinit atmel_of_init_port(struct atmel_nand_host *host,
 	board->enable_pin = of_get_gpio(np, 1);
 	board->det_pin = of_get_gpio(np, 2);
 
+	host->has_pmecc = of_property_read_bool(np, "atmel,has-pmecc");
+
+	if (!(board->ecc_mode == NAND_ECC_HW) || !host->has_pmecc)
+		return 0;	/* Not using PMECC */
+
+	/* use PMECC, get correction capability, sector size and lookup
+	 * table offset.
+	 */
+	if (of_property_read_u32(np, "atmel,pmecc-cap", &val) != 0) {
+		dev_err(host->dev, "Cannot decide PMECC Capability\n");
+		return -EINVAL;
+	} else if ((val != 2) && (val != 4) && (val != 8) && (val != 12) &&
+	    (val != 24)) {
+		dev_err(host->dev,
+			"Unsupported PMECC correction capability: %d; should be 2, 4, 8, 12 or 24\n",
+			val);
+		return -EINVAL;
+	}
+	host->pmecc_corr_cap = (u8)val;
+
+	if (of_property_read_u32(np, "atmel,pmecc-sector-size", &val) != 0) {
+		dev_err(host->dev, "Cannot decide PMECC Sector Size\n");
+		return -EINVAL;
+	} else if ((val != 512) && (val != 1024)) {
+		dev_err(host->dev,
+			"Unsupported PMECC sector size: %d; should be 512 or 1024 bytes\n",
+			val);
+		return -EINVAL;
+	}
+	host->pmecc_sector_size = (u16)val;
+
+	if (of_property_read_u32_array(np, "atmel,pmecc-lookup-table-offset",
+			offset, 2) != 0) {
+		dev_err(host->dev, "Cannot get PMECC lookup table offset\n");
+		return -EINVAL;
+	}
+	table_offset = host->pmecc_sector_size == 512 ? offset[0] : offset[1];
+
+	if (!table_offset) {
+		dev_err(host->dev, "Invalid PMECC lookup table offset\n");
+		return -EINVAL;
+	}
+	host->pmecc_lookup_table_offset = table_offset;
+
 	return 0;
 }
 #else
@@ -527,6 +1299,66 @@ static int __devinit atmel_of_init_port(struct atmel_nand_host *host,
 }
 #endif
 
+static int __init atmel_hw_nand_init_params(struct platform_device *pdev,
+					 struct atmel_nand_host *host)
+{
+	struct mtd_info *mtd = &host->mtd;
+	struct nand_chip *nand_chip = &host->nand_chip;
+	struct resource		*regs;
+
+	regs = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (!regs) {
+		dev_err(host->dev,
+			"Can't get I/O resource regs, use software ECC\n");
+		nand_chip->ecc.mode = NAND_ECC_SOFT;
+		return 0;
+	}
+
+	host->ecc = ioremap(regs->start, resource_size(regs));
+	if (host->ecc == NULL) {
+		dev_err(host->dev, "ioremap failed\n");
+		return -EIO;
+	}
+
+	/* ECC is calculated for the whole page (1 step) */
+	nand_chip->ecc.size = mtd->writesize;
+
+	/* set ECC page size and oob layout */
+	switch (mtd->writesize) {
+	case 512:
+		nand_chip->ecc.layout = &atmel_oobinfo_small;
+		ecc_writel(host->ecc, MR, ATMEL_ECC_PAGESIZE_528);
+		break;
+	case 1024:
+		nand_chip->ecc.layout = &atmel_oobinfo_large;
+		ecc_writel(host->ecc, MR, ATMEL_ECC_PAGESIZE_1056);
+		break;
+	case 2048:
+		nand_chip->ecc.layout = &atmel_oobinfo_large;
+		ecc_writel(host->ecc, MR, ATMEL_ECC_PAGESIZE_2112);
+		break;
+	case 4096:
+		nand_chip->ecc.layout = &atmel_oobinfo_large;
+		ecc_writel(host->ecc, MR, ATMEL_ECC_PAGESIZE_4224);
+		break;
+	default:
+		/* page size not handled by HW ECC */
+		/* switching back to soft ECC */
+		nand_chip->ecc.mode = NAND_ECC_SOFT;
+		return 0;
+	}
+
+	/* set up for HW ECC */
+	nand_chip->ecc.calculate = atmel_nand_calculate;
+	nand_chip->ecc.correct = atmel_nand_correct;
+	nand_chip->ecc.hwctl = atmel_nand_hwctl;
+	nand_chip->ecc.read_page = atmel_nand_read_page;
+	nand_chip->ecc.bytes = 4;
+	nand_chip->ecc.strength = 1;
+
+	return 0;
+}
+
 /*
  * Probe for the NAND device.
  */
@@ -535,7 +1367,6 @@ static int __init atmel_nand_probe(struct platform_device *pdev)
 	struct atmel_nand_host *host;
 	struct mtd_info *mtd;
 	struct nand_chip *nand_chip;
-	struct resource *regs;
 	struct resource *mem;
 	struct mtd_part_parser_data ppdata = {};
 	int res;
@@ -568,7 +1399,7 @@ static int __init atmel_nand_probe(struct platform_device *pdev)
 	if (pdev->dev.of_node) {
 		res = atmel_of_init_port(host, pdev->dev.of_node);
 		if (res)
-			goto err_nand_ioremap;
+			goto err_ecc_ioremap;
 	} else {
 		memcpy(&host->board, pdev->dev.platform_data,
 		       sizeof(struct atmel_nand_data));
@@ -583,33 +1414,45 @@ static int __init atmel_nand_probe(struct platform_device *pdev)
 	nand_chip->IO_ADDR_W = host->io_base;
 	nand_chip->cmd_ctrl = atmel_nand_cmd_ctrl;
 
-	if (gpio_is_valid(host->board.rdy_pin))
-		nand_chip->dev_ready = atmel_nand_device_ready;
+	if (gpio_is_valid(host->board.rdy_pin)) {
+		res = gpio_request(host->board.rdy_pin, "nand_rdy");
+		if (res < 0) {
+			dev_err(&pdev->dev,
+				"can't request rdy gpio %d\n",
+				host->board.rdy_pin);
+			goto err_ecc_ioremap;
+		}
 
-	nand_chip->ecc.mode = host->board.ecc_mode;
+		res = gpio_direction_input(host->board.rdy_pin);
+		if (res < 0) {
+			dev_err(&pdev->dev,
+				"can't request input direction rdy gpio %d\n",
+				host->board.rdy_pin);
+			goto err_ecc_ioremap;
+		}
 
-	regs = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	if (!regs && nand_chip->ecc.mode == NAND_ECC_HW) {
-		printk(KERN_ERR "atmel_nand: can't get I/O resource "
-				"regs\nFalling back on software ECC\n");
-		nand_chip->ecc.mode = NAND_ECC_SOFT;
+		nand_chip->dev_ready = atmel_nand_device_ready;
 	}
 
-	if (nand_chip->ecc.mode == NAND_ECC_HW) {
-		host->ecc = ioremap(regs->start, resource_size(regs));
-		if (host->ecc == NULL) {
-			printk(KERN_ERR "atmel_nand: ioremap failed\n");
-			res = -EIO;
+	if (gpio_is_valid(host->board.enable_pin)) {
+		res = gpio_request(host->board.enable_pin, "nand_enable");
+		if (res < 0) {
+			dev_err(&pdev->dev,
+				"can't request enable gpio %d\n",
+				host->board.enable_pin);
+			goto err_ecc_ioremap;
+		}
+
+		res = gpio_direction_output(host->board.enable_pin, 1);
+		if (res < 0) {
+			dev_err(&pdev->dev,
+				"can't request output direction enable gpio %d\n",
+				host->board.enable_pin);
 			goto err_ecc_ioremap;
 		}
-		nand_chip->ecc.calculate = atmel_nand_calculate;
-		nand_chip->ecc.correct = atmel_nand_correct;
-		nand_chip->ecc.hwctl = atmel_nand_hwctl;
-		nand_chip->ecc.read_page = atmel_nand_read_page;
-		nand_chip->ecc.bytes = 4;
-		nand_chip->ecc.strength = 1;
 	}
 
+	nand_chip->ecc.mode = host->board.ecc_mode;
 	nand_chip->chip_delay = 20;		/* 20us command delay time */
 
 	if (host->board.bus_width_16)	/* 16-bit bus width */
@@ -622,6 +1465,22 @@ static int __init atmel_nand_probe(struct platform_device *pdev)
 	atmel_nand_enable(host);
 
 	if (gpio_is_valid(host->board.det_pin)) {
+		res = gpio_request(host->board.det_pin, "nand_det");
+		if (res < 0) {
+			dev_err(&pdev->dev,
+				"can't request det gpio %d\n",
+				host->board.det_pin);
+			goto err_no_card;
+		}
+
+		res = gpio_direction_input(host->board.det_pin);
+		if (res < 0) {
+			dev_err(&pdev->dev,
+				"can't request input direction det gpio %d\n",
+				host->board.det_pin);
+			goto err_no_card;
+		}
+
 		if (gpio_get_value(host->board.det_pin)) {
 			printk(KERN_INFO "No SmartMedia card inserted.\n");
 			res = -ENXIO;
@@ -661,40 +1520,13 @@ static int __init atmel_nand_probe(struct platform_device *pdev)
 	}
 
 	if (nand_chip->ecc.mode == NAND_ECC_HW) {
-		/* ECC is calculated for the whole page (1 step) */
-		nand_chip->ecc.size = mtd->writesize;
-
-		/* set ECC page size and oob layout */
-		switch (mtd->writesize) {
-		case 512:
-			nand_chip->ecc.layout = &atmel_oobinfo_small;
-			ecc_writel(host->ecc, MR, ATMEL_ECC_PAGESIZE_528);
-			break;
-		case 1024:
-			nand_chip->ecc.layout = &atmel_oobinfo_large;
-			ecc_writel(host->ecc, MR, ATMEL_ECC_PAGESIZE_1056);
-			break;
-		case 2048:
-			nand_chip->ecc.layout = &atmel_oobinfo_large;
-			ecc_writel(host->ecc, MR, ATMEL_ECC_PAGESIZE_2112);
-			break;
-		case 4096:
-			nand_chip->ecc.layout = &atmel_oobinfo_large;
-			ecc_writel(host->ecc, MR, ATMEL_ECC_PAGESIZE_4224);
-			break;
-		default:
-			/* page size not handled by HW ECC */
-			/* switching back to soft ECC */
-			nand_chip->ecc.mode = NAND_ECC_SOFT;
-			nand_chip->ecc.calculate = NULL;
-			nand_chip->ecc.correct = NULL;
-			nand_chip->ecc.hwctl = NULL;
-			nand_chip->ecc.read_page = NULL;
-			nand_chip->ecc.postpad = 0;
-			nand_chip->ecc.prepad = 0;
-			nand_chip->ecc.bytes = 0;
-			break;
-		}
+		if (host->has_pmecc)
+			res = atmel_pmecc_nand_init_params(pdev, host);
+		else
+			res = atmel_hw_nand_init_params(pdev, host);
+
+		if (res != 0)
+			goto err_hw_ecc;
 	}
 
 	/* second phase scan */
@@ -711,14 +1543,23 @@ static int __init atmel_nand_probe(struct platform_device *pdev)
 		return res;
 
 err_scan_tail:
+	if (host->has_pmecc && host->nand_chip.ecc.mode == NAND_ECC_HW) {
+		pmecc_writel(host->ecc, CTRL, PMECC_CTRL_DISABLE);
+		pmecc_data_free(host);
+	}
+	if (host->ecc)
+		iounmap(host->ecc);
+	if (host->pmerrloc_base)
+		iounmap(host->pmerrloc_base);
+	if (host->pmecc_rom_base)
+		iounmap(host->pmecc_rom_base);
+err_hw_ecc:
 err_scan_ident:
 err_no_card:
 	atmel_nand_disable(host);
 	platform_set_drvdata(pdev, NULL);
 	if (host->dma_chan)
 		dma_release_channel(host->dma_chan);
-	if (host->ecc)
-		iounmap(host->ecc);
 err_ecc_ioremap:
 	iounmap(host->io_base);
 err_nand_ioremap:
@@ -738,8 +1579,28 @@ static int __exit atmel_nand_remove(struct platform_device *pdev)
 
 	atmel_nand_disable(host);
 
+	if (host->has_pmecc && host->nand_chip.ecc.mode == NAND_ECC_HW) {
+		pmecc_writel(host->ecc, CTRL, PMECC_CTRL_DISABLE);
+		pmerrloc_writel(host->pmerrloc_base, ELDIS,
+				PMERRLOC_DISABLE);
+		pmecc_data_free(host);
+	}
+
+	if (gpio_is_valid(host->board.det_pin))
+		gpio_free(host->board.det_pin);
+
+	if (gpio_is_valid(host->board.enable_pin))
+		gpio_free(host->board.enable_pin);
+
+	if (gpio_is_valid(host->board.rdy_pin))
+		gpio_free(host->board.rdy_pin);
+
 	if (host->ecc)
 		iounmap(host->ecc);
+	if (host->pmecc_rom_base)
+		iounmap(host->pmecc_rom_base);
+	if (host->pmerrloc_base)
+		iounmap(host->pmerrloc_base);
 
 	if (host->dma_chan)
 		dma_release_channel(host->dma_chan);
diff --git a/drivers/mtd/nand/atmel_nand_ecc.h b/drivers/mtd/nand/atmel_nand_ecc.h
index 578c776e135..8a1e9a68675 100644
--- a/drivers/mtd/nand/atmel_nand_ecc.h
+++ b/drivers/mtd/nand/atmel_nand_ecc.h
@@ -3,7 +3,7 @@
  * Based on AT91SAM9260 datasheet revision B.
  *
  * Copyright (C) 2007 Andrew Victor
- * Copyright (C) 2007 Atmel Corporation.
+ * Copyright (C) 2007 - 2012 Atmel Corporation.
  *
  * 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
@@ -36,4 +36,116 @@
 #define ATMEL_ECC_NPR		0x10			/* NParity register */
 #define		ATMEL_ECC_NPARITY	(0xffff << 0)		/* NParity */
 
+/* PMECC Register Definitions */
+#define ATMEL_PMECC_CFG			0x000	/* Configuration Register */
+#define		PMECC_CFG_BCH_ERR2		(0 << 0)
+#define		PMECC_CFG_BCH_ERR4		(1 << 0)
+#define		PMECC_CFG_BCH_ERR8		(2 << 0)
+#define		PMECC_CFG_BCH_ERR12		(3 << 0)
+#define		PMECC_CFG_BCH_ERR24		(4 << 0)
+
+#define		PMECC_CFG_SECTOR512		(0 << 4)
+#define		PMECC_CFG_SECTOR1024		(1 << 4)
+
+#define		PMECC_CFG_PAGE_1SECTOR		(0 << 8)
+#define		PMECC_CFG_PAGE_2SECTORS		(1 << 8)
+#define		PMECC_CFG_PAGE_4SECTORS		(2 << 8)
+#define		PMECC_CFG_PAGE_8SECTORS		(3 << 8)
+
+#define		PMECC_CFG_READ_OP		(0 << 12)
+#define		PMECC_CFG_WRITE_OP		(1 << 12)
+
+#define		PMECC_CFG_SPARE_ENABLE		(1 << 16)
+#define		PMECC_CFG_SPARE_DISABLE		(0 << 16)
+
+#define		PMECC_CFG_AUTO_ENABLE		(1 << 20)
+#define		PMECC_CFG_AUTO_DISABLE		(0 << 20)
+
+#define ATMEL_PMECC_SAREA		0x004	/* Spare area size */
+#define ATMEL_PMECC_SADDR		0x008	/* PMECC starting address */
+#define ATMEL_PMECC_EADDR		0x00c	/* PMECC ending address */
+#define ATMEL_PMECC_CLK			0x010	/* PMECC clock control */
+#define		PMECC_CLK_133MHZ		(2 << 0)
+
+#define ATMEL_PMECC_CTRL		0x014	/* PMECC control register */
+#define		PMECC_CTRL_RST			(1 << 0)
+#define		PMECC_CTRL_DATA			(1 << 1)
+#define		PMECC_CTRL_USER			(1 << 2)
+#define		PMECC_CTRL_ENABLE		(1 << 4)
+#define		PMECC_CTRL_DISABLE		(1 << 5)
+
+#define ATMEL_PMECC_SR			0x018	/* PMECC status register */
+#define		PMECC_SR_BUSY			(1 << 0)
+#define		PMECC_SR_ENABLE			(1 << 4)
+
+#define ATMEL_PMECC_IER			0x01c	/* PMECC interrupt enable */
+#define		PMECC_IER_ENABLE		(1 << 0)
+#define ATMEL_PMECC_IDR			0x020	/* PMECC interrupt disable */
+#define		PMECC_IER_DISABLE		(1 << 0)
+#define ATMEL_PMECC_IMR			0x024	/* PMECC interrupt mask */
+#define		PMECC_IER_MASK			(1 << 0)
+#define ATMEL_PMECC_ISR			0x028	/* PMECC interrupt status */
+#define ATMEL_PMECC_ECCx		0x040	/* PMECC ECC x */
+#define ATMEL_PMECC_REMx		0x240	/* PMECC REM x */
+
+/* PMERRLOC Register Definitions */
+#define ATMEL_PMERRLOC_ELCFG		0x000	/* Error location config */
+#define		PMERRLOC_ELCFG_SECTOR_512	(0 << 0)
+#define		PMERRLOC_ELCFG_SECTOR_1024	(1 << 0)
+#define		PMERRLOC_ELCFG_NUM_ERRORS(n)	((n) << 16)
+
+#define ATMEL_PMERRLOC_ELPRIM		0x004	/* Error location primitive */
+#define ATMEL_PMERRLOC_ELEN		0x008	/* Error location enable */
+#define ATMEL_PMERRLOC_ELDIS		0x00c	/* Error location disable */
+#define		PMERRLOC_DISABLE		(1 << 0)
+
+#define ATMEL_PMERRLOC_ELSR		0x010	/* Error location status */
+#define		PMERRLOC_ELSR_BUSY		(1 << 0)
+#define ATMEL_PMERRLOC_ELIER		0x014	/* Error location int enable */
+#define ATMEL_PMERRLOC_ELIDR		0x018	/* Error location int disable */
+#define ATMEL_PMERRLOC_ELIMR		0x01c	/* Error location int mask */
+#define ATMEL_PMERRLOC_ELISR		0x020	/* Error location int status */
+#define		PMERRLOC_ERR_NUM_MASK		(0x1f << 8)
+#define		PMERRLOC_CALC_DONE		(1 << 0)
+#define ATMEL_PMERRLOC_SIGMAx		0x028	/* Error location SIGMA x */
+#define ATMEL_PMERRLOC_ELx		0x08c	/* Error location x */
+
+/* Register access macros for PMECC */
+#define pmecc_readl_relaxed(addr, reg) \
+	readl_relaxed((addr) + ATMEL_PMECC_##reg)
+
+#define pmecc_writel(addr, reg, value) \
+	writel((value), (addr) + ATMEL_PMECC_##reg)
+
+#define pmecc_readb_ecc_relaxed(addr, sector, n) \
+	readb_relaxed((addr) + ATMEL_PMECC_ECCx + ((sector) * 0x40) + (n))
+
+#define pmecc_readl_rem_relaxed(addr, sector, n) \
+	readl_relaxed((addr) + ATMEL_PMECC_REMx + ((sector) * 0x40) + ((n) * 4))
+
+#define pmerrloc_readl_relaxed(addr, reg) \
+	readl_relaxed((addr) + ATMEL_PMERRLOC_##reg)
+
+#define pmerrloc_writel(addr, reg, value) \
+	writel((value), (addr) + ATMEL_PMERRLOC_##reg)
+
+#define pmerrloc_writel_sigma_relaxed(addr, n, value) \
+	writel_relaxed((value), (addr) + ATMEL_PMERRLOC_SIGMAx + ((n) * 4))
+
+#define pmerrloc_readl_sigma_relaxed(addr, n) \
+	readl_relaxed((addr) + ATMEL_PMERRLOC_SIGMAx + ((n) * 4))
+
+#define pmerrloc_readl_el_relaxed(addr, n) \
+	readl_relaxed((addr) + ATMEL_PMERRLOC_ELx + ((n) * 4))
+
+/* Galois field dimension */
+#define PMECC_GF_DIMENSION_13			13
+#define PMECC_GF_DIMENSION_14			14
+
+#define PMECC_LOOKUP_TABLE_SIZE_512		0x2000
+#define PMECC_LOOKUP_TABLE_SIZE_1024		0x4000
+
+/* Time out value for reading PMECC status register */
+#define PMECC_MAX_TIMEOUT_MS			100
+
 #endif
diff --git a/drivers/mtd/nand/au1550nd.c b/drivers/mtd/nand/au1550nd.c
index 9f609d2dcf6..5c47b200045 100644
--- a/drivers/mtd/nand/au1550nd.c
+++ b/drivers/mtd/nand/au1550nd.c
@@ -141,28 +141,6 @@ static void au_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 }
 
 /**
- * au_verify_buf -  Verify chip data against buffer
- * @mtd:	MTD device structure
- * @buf:	buffer containing the data to compare
- * @len:	number of bytes to compare
- *
- * verify function for 8bit buswidth
- */
-static int au_verify_buf(struct mtd_info *mtd, const u_char *buf, int len)
-{
-	int i;
-	struct nand_chip *this = mtd->priv;
-
-	for (i = 0; i < len; i++) {
-		if (buf[i] != readb(this->IO_ADDR_R))
-			return -EFAULT;
-		au_sync();
-	}
-
-	return 0;
-}
-
-/**
  * au_write_buf16 -  write buffer to chip
  * @mtd:	MTD device structure
  * @buf:	data buffer
@@ -205,29 +183,6 @@ static void au_read_buf16(struct mtd_info *mtd, u_char *buf, int len)
 	}
 }
 
-/**
- * au_verify_buf16 -  Verify chip data against buffer
- * @mtd:	MTD device structure
- * @buf:	buffer containing the data to compare
- * @len:	number of bytes to compare
- *
- * verify function for 16bit buswidth
- */
-static int au_verify_buf16(struct mtd_info *mtd, const u_char *buf, int len)
-{
-	int i;
-	struct nand_chip *this = mtd->priv;
-	u16 *p = (u16 *) buf;
-	len >>= 1;
-
-	for (i = 0; i < len; i++) {
-		if (p[i] != readw(this->IO_ADDR_R))
-			return -EFAULT;
-		au_sync();
-	}
-	return 0;
-}
-
 /* Select the chip by setting nCE to low */
 #define NAND_CTL_SETNCE		1
 /* Deselect the chip by setting nCE to high */
@@ -516,7 +471,6 @@ static int __devinit au1550nd_probe(struct platform_device *pdev)
 	this->read_word = au_read_word;
 	this->write_buf = (pd->devwidth) ? au_write_buf16 : au_write_buf;
 	this->read_buf = (pd->devwidth) ? au_read_buf16 : au_read_buf;
-	this->verify_buf = (pd->devwidth) ? au_verify_buf16 : au_verify_buf;
 
 	ret = nand_scan(&ctx->info, 1);
 	if (ret) {
diff --git a/drivers/mtd/nand/bcm_umi_bch.c b/drivers/mtd/nand/bcm_umi_bch.c
deleted file mode 100644
index 5914bb32e00..00000000000
--- a/drivers/mtd/nand/bcm_umi_bch.c
+++ /dev/null
@@ -1,217 +0,0 @@
-/*****************************************************************************
-* Copyright 2004 - 2009 Broadcom Corporation.  All rights reserved.
-*
-* 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, available at
-* http://www.broadcom.com/licenses/GPLv2.php (the "GPL").
-*
-* 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.
-*****************************************************************************/
-
-/* ---- Include Files ---------------------------------------------------- */
-#include "nand_bcm_umi.h"
-
-/* ---- External Variable Declarations ----------------------------------- */
-/* ---- External Function Prototypes ------------------------------------- */
-/* ---- Public Variables ------------------------------------------------- */
-/* ---- Private Constants and Types -------------------------------------- */
-
-/* ---- Private Function Prototypes -------------------------------------- */
-static int bcm_umi_bch_read_page_hwecc(struct mtd_info *mtd,
-	struct nand_chip *chip, uint8_t *buf, int oob_required, int page);
-static void bcm_umi_bch_write_page_hwecc(struct mtd_info *mtd,
-	struct nand_chip *chip, const uint8_t *buf, int oob_required);
-
-/* ---- Private Variables ------------------------------------------------ */
-
-/*
-** nand_hw_eccoob
-** New oob placement block for use with hardware ecc generation.
-*/
-static struct nand_ecclayout nand_hw_eccoob_512 = {
-	/* Reserve 5 for BI indicator */
-	.oobfree = {
-#if (NAND_ECC_NUM_BYTES > 3)
-		    {.offset = 0, .length = 2}
-#else
-		    {.offset = 0, .length = 5},
-		    {.offset = 6, .length = 7}
-#endif
-		    }
-};
-
-/*
-** We treat the OOB for a 2K page as if it were 4 512 byte oobs,
-** except the BI is at byte 0.
-*/
-static struct nand_ecclayout nand_hw_eccoob_2048 = {
-	/* Reserve 0 as BI indicator */
-	.oobfree = {
-#if (NAND_ECC_NUM_BYTES > 10)
-		    {.offset = 1, .length = 2},
-#elif (NAND_ECC_NUM_BYTES > 7)
-		    {.offset = 1, .length = 5},
-		    {.offset = 16, .length = 6},
-		    {.offset = 32, .length = 6},
-		    {.offset = 48, .length = 6}
-#else
-		    {.offset = 1, .length = 8},
-		    {.offset = 16, .length = 9},
-		    {.offset = 32, .length = 9},
-		    {.offset = 48, .length = 9}
-#endif
-		    }
-};
-
-/* We treat the OOB for a 4K page as if it were 8 512 byte oobs,
- * except the BI is at byte 0. */
-static struct nand_ecclayout nand_hw_eccoob_4096 = {
-	/* Reserve 0 as BI indicator */
-	.oobfree = {
-#if (NAND_ECC_NUM_BYTES > 10)
-		    {.offset = 1, .length = 2},
-		    {.offset = 16, .length = 3},
-		    {.offset = 32, .length = 3},
-		    {.offset = 48, .length = 3},
-		    {.offset = 64, .length = 3},
-		    {.offset = 80, .length = 3},
-		    {.offset = 96, .length = 3},
-		    {.offset = 112, .length = 3}
-#else
-		    {.offset = 1, .length = 5},
-		    {.offset = 16, .length = 6},
-		    {.offset = 32, .length = 6},
-		    {.offset = 48, .length = 6},
-		    {.offset = 64, .length = 6},
-		    {.offset = 80, .length = 6},
-		    {.offset = 96, .length = 6},
-		    {.offset = 112, .length = 6}
-#endif
-		    }
-};
-
-/* ---- Private Functions ------------------------------------------------ */
-/* ==== Public Functions ================================================= */
-
-/****************************************************************************
-*
-*  bcm_umi_bch_read_page_hwecc - hardware ecc based page read function
-*  @mtd:	mtd info structure
-*  @chip:	nand chip info structure
-*  @buf:	buffer to store read data
-*  @oob_required:	caller expects OOB data read to chip->oob_poi
-*
-***************************************************************************/
-static int bcm_umi_bch_read_page_hwecc(struct mtd_info *mtd,
-				       struct nand_chip *chip, uint8_t * buf,
-				       int oob_required, int page)
-{
-	int sectorIdx = 0;
-	int eccsize = chip->ecc.size;
-	int eccsteps = chip->ecc.steps;
-	uint8_t *datap = buf;
-	uint8_t eccCalc[NAND_ECC_NUM_BYTES];
-	int sectorOobSize = mtd->oobsize / eccsteps;
-	int stat;
-	unsigned int max_bitflips = 0;
-
-	for (sectorIdx = 0; sectorIdx < eccsteps;
-			sectorIdx++, datap += eccsize) {
-		if (sectorIdx > 0) {
-			/* Seek to page location within sector */
-			chip->cmdfunc(mtd, NAND_CMD_RNDOUT, sectorIdx * eccsize,
-				      -1);
-		}
-
-		/* Enable hardware ECC before reading the buf */
-		nand_bcm_umi_bch_enable_read_hwecc();
-
-		/* Read in data */
-		bcm_umi_nand_read_buf(mtd, datap, eccsize);
-
-		/* Pause hardware ECC after reading the buf */
-		nand_bcm_umi_bch_pause_read_ecc_calc();
-
-		/* Read the OOB ECC */
-		chip->cmdfunc(mtd, NAND_CMD_RNDOUT,
-			      mtd->writesize + sectorIdx * sectorOobSize, -1);
-		nand_bcm_umi_bch_read_oobEcc(mtd->writesize, eccCalc,
-					     NAND_ECC_NUM_BYTES,
-					     chip->oob_poi +
-					     sectorIdx * sectorOobSize);
-
-		/* Correct any ECC detected errors */
-		stat =
-		    nand_bcm_umi_bch_correct_page(datap, eccCalc,
-						  NAND_ECC_NUM_BYTES);
-
-		/* Update Stats */
-		if (stat < 0) {
-#if defined(NAND_BCM_UMI_DEBUG)
-			printk(KERN_WARNING "%s uncorr_err sectorIdx=%d\n",
-			       __func__, sectorIdx);
-			printk(KERN_WARNING
-			       "%s data %02x %02x %02x %02x "
-					 "%02x %02x %02x %02x\n",
-			       __func__, datap[0], datap[1], datap[2], datap[3],
-			       datap[4], datap[5], datap[6], datap[7]);
-			printk(KERN_WARNING
-			       "%s ecc  %02x %02x %02x %02x "
-					 "%02x %02x %02x %02x %02x %02x "
-					 "%02x %02x %02x\n",
-			       __func__, eccCalc[0], eccCalc[1], eccCalc[2],
-			       eccCalc[3], eccCalc[4], eccCalc[5], eccCalc[6],
-			       eccCalc[7], eccCalc[8], eccCalc[9], eccCalc[10],
-			       eccCalc[11], eccCalc[12]);
-			BUG();
-#endif
-			mtd->ecc_stats.failed++;
-		} else {
-#if defined(NAND_BCM_UMI_DEBUG)
-			if (stat > 0) {
-				printk(KERN_INFO
-				       "%s %d correctable_errors detected\n",
-				       __func__, stat);
-			}
-#endif
-			mtd->ecc_stats.corrected += stat;
-			max_bitflips = max_t(unsigned int, max_bitflips, stat);
-		}
-	}
-	return max_bitflips;
-}
-
-/****************************************************************************
-*
-*  bcm_umi_bch_write_page_hwecc - hardware ecc based page write function
-*  @mtd:	mtd info structure
-*  @chip:	nand chip info structure
-*  @buf:	data buffer
-*  @oob_required:	must write chip->oob_poi to OOB
-*
-***************************************************************************/
-static void bcm_umi_bch_write_page_hwecc(struct mtd_info *mtd,
-	struct nand_chip *chip, const uint8_t *buf, int oob_required)
-{
-	int sectorIdx = 0;
-	int eccsize = chip->ecc.size;
-	int eccsteps = chip->ecc.steps;
-	const uint8_t *datap = buf;
-	uint8_t *oobp = chip->oob_poi;
-	int sectorOobSize = mtd->oobsize / eccsteps;
-
-	for (sectorIdx = 0; sectorIdx < eccsteps;
-	     sectorIdx++, datap += eccsize, oobp += sectorOobSize) {
-		/* Enable hardware ECC before writing the buf */
-		nand_bcm_umi_bch_enable_write_hwecc();
-		bcm_umi_nand_write_buf(mtd, datap, eccsize);
-		nand_bcm_umi_bch_write_oobEcc(mtd->writesize, oobp,
-					      NAND_ECC_NUM_BYTES);
-	}
-
-	bcm_umi_nand_write_buf(mtd, chip->oob_poi, mtd->oobsize);
-}
diff --git a/drivers/mtd/nand/bcm_umi_nand.c b/drivers/mtd/nand/bcm_umi_nand.c
deleted file mode 100644
index d0d1bd4d0e7..00000000000
--- a/drivers/mtd/nand/bcm_umi_nand.c
+++ /dev/null
@@ -1,555 +0,0 @@
-/*****************************************************************************
-* Copyright 2004 - 2009 Broadcom Corporation.  All rights reserved.
-*
-* 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, available at
-* http://www.broadcom.com/licenses/GPLv2.php (the "GPL").
-*
-* 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.
-*****************************************************************************/
-
-/* ---- Include Files ---------------------------------------------------- */
-#include <linux/module.h>
-#include <linux/types.h>
-#include <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/string.h>
-#include <linux/ioport.h>
-#include <linux/device.h>
-#include <linux/delay.h>
-#include <linux/err.h>
-#include <linux/io.h>
-#include <linux/platform_device.h>
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/nand.h>
-#include <linux/mtd/nand_ecc.h>
-#include <linux/mtd/partitions.h>
-
-#include <asm/mach-types.h>
-
-#include <mach/reg_nand.h>
-#include <mach/reg_umi.h>
-
-#include "nand_bcm_umi.h"
-
-#include <mach/memory_settings.h>
-
-#define USE_DMA 1
-#include <mach/dma.h>
-#include <linux/dma-mapping.h>
-#include <linux/completion.h>
-
-/* ---- External Variable Declarations ----------------------------------- */
-/* ---- External Function Prototypes ------------------------------------- */
-/* ---- Public Variables ------------------------------------------------- */
-/* ---- Private Constants and Types -------------------------------------- */
-static const __devinitconst char gBanner[] = KERN_INFO \
-	"BCM UMI MTD NAND Driver: 1.00\n";
-
-#if NAND_ECC_BCH
-static uint8_t scan_ff_pattern[] = { 0xff };
-
-static struct nand_bbt_descr largepage_bbt = {
-	.options = 0,
-	.offs = 0,
-	.len = 1,
-	.pattern = scan_ff_pattern
-};
-#endif
-
-/*
-** Preallocate a buffer to avoid having to do this every dma operation.
-** This is the size of the preallocated coherent DMA buffer.
-*/
-#if USE_DMA
-#define DMA_MIN_BUFLEN	512
-#define DMA_MAX_BUFLEN	PAGE_SIZE
-#define USE_DIRECT_IO(len)	(((len) < DMA_MIN_BUFLEN) || \
-	((len) > DMA_MAX_BUFLEN))
-
-/*
- * The current NAND data space goes from 0x80001900 to 0x80001FFF,
- * which is only 0x700 = 1792 bytes long. This is too small for 2K, 4K page
- * size NAND flash. Need to break the DMA down to multiple 1Ks.
- *
- * Need to make sure REG_NAND_DATA_PADDR + DMA_MAX_LEN < 0x80002000
- */
-#define DMA_MAX_LEN             1024
-
-#else /* !USE_DMA */
-#define DMA_MIN_BUFLEN          0
-#define DMA_MAX_BUFLEN          0
-#define USE_DIRECT_IO(len)      1
-#endif
-/* ---- Private Function Prototypes -------------------------------------- */
-static void bcm_umi_nand_read_buf(struct mtd_info *mtd, u_char * buf, int len);
-static void bcm_umi_nand_write_buf(struct mtd_info *mtd, const u_char * buf,
-				   int len);
-
-/* ---- Private Variables ------------------------------------------------ */
-static struct mtd_info *board_mtd;
-static void __iomem *bcm_umi_io_base;
-static void *virtPtr;
-static dma_addr_t physPtr;
-static struct completion nand_comp;
-
-/* ---- Private Functions ------------------------------------------------ */
-#if NAND_ECC_BCH
-#include "bcm_umi_bch.c"
-#else
-#include "bcm_umi_hamming.c"
-#endif
-
-#if USE_DMA
-
-/* Handler called when the DMA finishes. */
-static void nand_dma_handler(DMA_Device_t dev, int reason, void *userData)
-{
-	complete(&nand_comp);
-}
-
-static int nand_dma_init(void)
-{
-	int rc;
-
-	rc = dma_set_device_handler(DMA_DEVICE_NAND_MEM_TO_MEM,
-		nand_dma_handler, NULL);
-	if (rc != 0) {
-		printk(KERN_ERR "dma_set_device_handler failed: %d\n", rc);
-		return rc;
-	}
-
-	virtPtr =
-	    dma_alloc_coherent(NULL, DMA_MAX_BUFLEN, &physPtr, GFP_KERNEL);
-	if (virtPtr == NULL) {
-		printk(KERN_ERR "NAND - Failed to allocate memory for DMA buffer\n");
-		return -ENOMEM;
-	}
-
-	return 0;
-}
-
-static void nand_dma_term(void)
-{
-	if (virtPtr != NULL)
-		dma_free_coherent(NULL, DMA_MAX_BUFLEN, virtPtr, physPtr);
-}
-
-static void nand_dma_read(void *buf, int len)
-{
-	int offset = 0;
-	int tmp_len = 0;
-	int len_left = len;
-	DMA_Handle_t hndl;
-
-	if (virtPtr == NULL)
-		panic("nand_dma_read: virtPtr == NULL\n");
-
-	if ((void *)physPtr == NULL)
-		panic("nand_dma_read: physPtr == NULL\n");
-
-	hndl = dma_request_channel(DMA_DEVICE_NAND_MEM_TO_MEM);
-	if (hndl < 0) {
-		printk(KERN_ERR
-		       "nand_dma_read: unable to allocate dma channel: %d\n",
-		       (int)hndl);
-		panic("\n");
-	}
-
-	while (len_left > 0) {
-		if (len_left > DMA_MAX_LEN) {
-			tmp_len = DMA_MAX_LEN;
-			len_left -= DMA_MAX_LEN;
-		} else {
-			tmp_len = len_left;
-			len_left = 0;
-		}
-
-		init_completion(&nand_comp);
-		dma_transfer_mem_to_mem(hndl, REG_NAND_DATA_PADDR,
-					physPtr + offset, tmp_len);
-		wait_for_completion(&nand_comp);
-
-		offset += tmp_len;
-	}
-
-	dma_free_channel(hndl);
-
-	if (buf != NULL)
-		memcpy(buf, virtPtr, len);
-}
-
-static void nand_dma_write(const void *buf, int len)
-{
-	int offset = 0;
-	int tmp_len = 0;
-	int len_left = len;
-	DMA_Handle_t hndl;
-
-	if (buf == NULL)
-		panic("nand_dma_write: buf == NULL\n");
-
-	if (virtPtr == NULL)
-		panic("nand_dma_write: virtPtr == NULL\n");
-
-	if ((void *)physPtr == NULL)
-		panic("nand_dma_write: physPtr == NULL\n");
-
-	memcpy(virtPtr, buf, len);
-
-
-	hndl = dma_request_channel(DMA_DEVICE_NAND_MEM_TO_MEM);
-	if (hndl < 0) {
-		printk(KERN_ERR
-		       "nand_dma_write: unable to allocate dma channel: %d\n",
-		       (int)hndl);
-		panic("\n");
-	}
-
-	while (len_left > 0) {
-		if (len_left > DMA_MAX_LEN) {
-			tmp_len = DMA_MAX_LEN;
-			len_left -= DMA_MAX_LEN;
-		} else {
-			tmp_len = len_left;
-			len_left = 0;
-		}
-
-		init_completion(&nand_comp);
-		dma_transfer_mem_to_mem(hndl, physPtr + offset,
-					REG_NAND_DATA_PADDR, tmp_len);
-		wait_for_completion(&nand_comp);
-
-		offset += tmp_len;
-	}
-
-	dma_free_channel(hndl);
-}
-
-#endif
-
-static int nand_dev_ready(struct mtd_info *mtd)
-{
-	return nand_bcm_umi_dev_ready();
-}
-
-/****************************************************************************
-*
-*  bcm_umi_nand_inithw
-*
-*   This routine does the necessary hardware (board-specific)
-*   initializations.  This includes setting up the timings, etc.
-*
-***************************************************************************/
-int bcm_umi_nand_inithw(void)
-{
-	/* Configure nand timing parameters */
-	writel(readl(&REG_UMI_NAND_TCR) & ~0x7ffff, &REG_UMI_NAND_TCR);
-	writel(readl(&REG_UMI_NAND_TCR) | HW_CFG_NAND_TCR, &REG_UMI_NAND_TCR);
-
-#if !defined(CONFIG_MTD_NAND_BCM_UMI_HWCS)
-	/* enable software control of CS */
-	writel(readl(&REG_UMI_NAND_TCR) | REG_UMI_NAND_TCR_CS_SWCTRL, &REG_UMI_NAND_TCR);
-#endif
-
-	/* keep NAND chip select asserted */
-	writel(readl(&REG_UMI_NAND_RCSR) | REG_UMI_NAND_RCSR_CS_ASSERTED, &REG_UMI_NAND_RCSR);
-
-	writel(readl(&REG_UMI_NAND_TCR) & ~REG_UMI_NAND_TCR_WORD16, &REG_UMI_NAND_TCR);
-	/* enable writes to flash */
-	writel(readl(&REG_UMI_MMD_ICR) | REG_UMI_MMD_ICR_FLASH_WP, &REG_UMI_MMD_ICR);
-
-	writel(NAND_CMD_RESET, bcm_umi_io_base + REG_NAND_CMD_OFFSET);
-	nand_bcm_umi_wait_till_ready();
-
-#if NAND_ECC_BCH
-	nand_bcm_umi_bch_config_ecc(NAND_ECC_NUM_BYTES);
-#endif
-
-	return 0;
-}
-
-/* Used to turn latch the proper register for access. */
-static void bcm_umi_nand_hwcontrol(struct mtd_info *mtd, int cmd,
-				   unsigned int ctrl)
-{
-	/* send command to hardware */
-	struct nand_chip *chip = mtd->priv;
-	if (ctrl & NAND_CTRL_CHANGE) {
-		if (ctrl & NAND_CLE) {
-			chip->IO_ADDR_W = bcm_umi_io_base + REG_NAND_CMD_OFFSET;
-			goto CMD;
-		}
-		if (ctrl & NAND_ALE) {
-			chip->IO_ADDR_W =
-			    bcm_umi_io_base + REG_NAND_ADDR_OFFSET;
-			goto CMD;
-		}
-		chip->IO_ADDR_W = bcm_umi_io_base + REG_NAND_DATA8_OFFSET;
-	}
-
-CMD:
-	/* Send command to chip directly */
-	if (cmd != NAND_CMD_NONE)
-		writeb(cmd, chip->IO_ADDR_W);
-}
-
-static void bcm_umi_nand_write_buf(struct mtd_info *mtd, const u_char * buf,
-				   int len)
-{
-	if (USE_DIRECT_IO(len)) {
-		/* Do it the old way if the buffer is small or too large.
-		 * Probably quicker than starting and checking dma. */
-		int i;
-		struct nand_chip *this = mtd->priv;
-
-		for (i = 0; i < len; i++)
-			writeb(buf[i], this->IO_ADDR_W);
-	}
-#if USE_DMA
-	else
-		nand_dma_write(buf, len);
-#endif
-}
-
-static void bcm_umi_nand_read_buf(struct mtd_info *mtd, u_char * buf, int len)
-{
-	if (USE_DIRECT_IO(len)) {
-		int i;
-		struct nand_chip *this = mtd->priv;
-
-		for (i = 0; i < len; i++)
-			buf[i] = readb(this->IO_ADDR_R);
-	}
-#if USE_DMA
-	else
-		nand_dma_read(buf, len);
-#endif
-}
-
-static uint8_t readbackbuf[NAND_MAX_PAGESIZE];
-static int bcm_umi_nand_verify_buf(struct mtd_info *mtd, const u_char * buf,
-				   int len)
-{
-	/*
-	 * Try to readback page with ECC correction. This is necessary
-	 * for MLC parts which may have permanently stuck bits.
-	 */
-	struct nand_chip *chip = mtd->priv;
-	int ret = chip->ecc.read_page(mtd, chip, readbackbuf, 0, 0);
-	if (ret < 0)
-		return -EFAULT;
-	else {
-		if (memcmp(readbackbuf, buf, len) == 0)
-			return 0;
-
-		return -EFAULT;
-	}
-	return 0;
-}
-
-static int __devinit bcm_umi_nand_probe(struct platform_device *pdev)
-{
-	struct nand_chip *this;
-	struct resource *r;
-	int err = 0;
-
-	printk(gBanner);
-
-	/* Allocate memory for MTD device structure and private data */
-	board_mtd =
-	    kmalloc(sizeof(struct mtd_info) + sizeof(struct nand_chip),
-		    GFP_KERNEL);
-	if (!board_mtd) {
-		printk(KERN_WARNING
-		       "Unable to allocate NAND MTD device structure.\n");
-		return -ENOMEM;
-	}
-
-	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-
-	if (!r) {
-		err = -ENXIO;
-		goto out_free;
-	}
-
-	/* map physical address */
-	bcm_umi_io_base = ioremap(r->start, resource_size(r));
-
-	if (!bcm_umi_io_base) {
-		printk(KERN_ERR "ioremap to access BCM UMI NAND chip failed\n");
-		err = -EIO;
-		goto out_free;
-	}
-
-	/* Get pointer to private data */
-	this = (struct nand_chip *)(&board_mtd[1]);
-
-	/* Initialize structures */
-	memset((char *)board_mtd, 0, sizeof(struct mtd_info));
-	memset((char *)this, 0, sizeof(struct nand_chip));
-
-	/* Link the private data with the MTD structure */
-	board_mtd->priv = this;
-
-	/* Initialize the NAND hardware.  */
-	if (bcm_umi_nand_inithw() < 0) {
-		printk(KERN_ERR "BCM UMI NAND chip could not be initialized\n");
-		err = -EIO;
-		goto out_unmap;
-	}
-
-	/* Set address of NAND IO lines */
-	this->IO_ADDR_W = bcm_umi_io_base + REG_NAND_DATA8_OFFSET;
-	this->IO_ADDR_R = bcm_umi_io_base + REG_NAND_DATA8_OFFSET;
-
-	/* Set command delay time, see datasheet for correct value */
-	this->chip_delay = 0;
-	/* Assign the device ready function, if available */
-	this->dev_ready = nand_dev_ready;
-	this->options = 0;
-
-	this->write_buf = bcm_umi_nand_write_buf;
-	this->read_buf = bcm_umi_nand_read_buf;
-	this->verify_buf = bcm_umi_nand_verify_buf;
-
-	this->cmd_ctrl = bcm_umi_nand_hwcontrol;
-	this->ecc.mode = NAND_ECC_HW;
-	this->ecc.size = 512;
-	this->ecc.bytes = NAND_ECC_NUM_BYTES;
-#if NAND_ECC_BCH
-	this->ecc.read_page = bcm_umi_bch_read_page_hwecc;
-	this->ecc.write_page = bcm_umi_bch_write_page_hwecc;
-#else
-	this->ecc.correct = nand_correct_data512;
-	this->ecc.calculate = bcm_umi_hamming_get_hw_ecc;
-	this->ecc.hwctl = bcm_umi_hamming_enable_hwecc;
-#endif
-
-#if USE_DMA
-	err = nand_dma_init();
-	if (err != 0)
-		goto out_unmap;
-#endif
-
-	/* Figure out the size of the device that we have.
-	 * We need to do this to figure out which ECC
-	 * layout we'll be using.
-	 */
-
-	err = nand_scan_ident(board_mtd, 1, NULL);
-	if (err) {
-		printk(KERN_ERR "nand_scan failed: %d\n", err);
-		goto out_unmap;
-	}
-
-	/* Now that we know the nand size, we can setup the ECC layout */
-
-	switch (board_mtd->writesize) {	/* writesize is the pagesize */
-	case 4096:
-		this->ecc.layout = &nand_hw_eccoob_4096;
-		break;
-	case 2048:
-		this->ecc.layout = &nand_hw_eccoob_2048;
-		break;
-	case 512:
-		this->ecc.layout = &nand_hw_eccoob_512;
-		break;
-	default:
-		{
-			printk(KERN_ERR "NAND - Unrecognized pagesize: %d\n",
-					 board_mtd->writesize);
-			err = -EINVAL;
-			goto out_unmap;
-		}
-	}
-
-#if NAND_ECC_BCH
-	if (board_mtd->writesize > 512) {
-		if (this->bbt_options & NAND_BBT_USE_FLASH)
-			largepage_bbt.options = NAND_BBT_SCAN2NDPAGE;
-		this->badblock_pattern = &largepage_bbt;
-	}
-
-	this->ecc.strength = 8;
-
-#endif
-
-	/* Now finish off the scan, now that ecc.layout has been initialized. */
-
-	err = nand_scan_tail(board_mtd);
-	if (err) {
-		printk(KERN_ERR "nand_scan failed: %d\n", err);
-		goto out_unmap;
-	}
-
-	/* Register the partitions */
-	board_mtd->name = "bcm_umi-nand";
-	mtd_device_parse_register(board_mtd, NULL, NULL, NULL, 0);
-
-	/* Return happy */
-	return 0;
-out_unmap:
-	iounmap(bcm_umi_io_base);
-out_free:
-	kfree(board_mtd);
-	return err;
-}
-
-static int bcm_umi_nand_remove(struct platform_device *pdev)
-{
-#if USE_DMA
-	nand_dma_term();
-#endif
-
-	/* Release resources, unregister device */
-	nand_release(board_mtd);
-
-	/* unmap physical address */
-	iounmap(bcm_umi_io_base);
-
-	/* Free the MTD device structure */
-	kfree(board_mtd);
-
-	return 0;
-}
-
-#ifdef CONFIG_PM
-static int bcm_umi_nand_suspend(struct platform_device *pdev,
-				pm_message_t state)
-{
-	printk(KERN_ERR "MTD NAND suspend is being called\n");
-	return 0;
-}
-
-static int bcm_umi_nand_resume(struct platform_device *pdev)
-{
-	printk(KERN_ERR "MTD NAND resume is being called\n");
-	return 0;
-}
-#else
-#define bcm_umi_nand_suspend   NULL
-#define bcm_umi_nand_resume    NULL
-#endif
-
-static struct platform_driver nand_driver = {
-	.driver = {
-		   .name = "bcm-nand",
-		   .owner = THIS_MODULE,
-		   },
-	.probe = bcm_umi_nand_probe,
-	.remove = bcm_umi_nand_remove,
-	.suspend = bcm_umi_nand_suspend,
-	.resume = bcm_umi_nand_resume,
-};
-
-module_platform_driver(nand_driver);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Broadcom");
-MODULE_DESCRIPTION("BCM UMI MTD NAND driver");
diff --git a/drivers/mtd/nand/bf5xx_nand.c b/drivers/mtd/nand/bf5xx_nand.c
index 3f1c18599cb..ab0caa74eb4 100644
--- a/drivers/mtd/nand/bf5xx_nand.c
+++ b/drivers/mtd/nand/bf5xx_nand.c
@@ -566,11 +566,13 @@ static int bf5xx_nand_read_page_raw(struct mtd_info *mtd, struct nand_chip *chip
 	return 0;
 }
 
-static void bf5xx_nand_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
-		const uint8_t *buf, int oob_required)
+static int bf5xx_nand_write_page_raw(struct mtd_info *mtd,
+		struct nand_chip *chip,	const uint8_t *buf, int oob_required)
 {
 	bf5xx_nand_write_buf(mtd, buf, mtd->writesize);
 	bf5xx_nand_write_buf(mtd, chip->oob_poi, mtd->oobsize);
+
+	return 0;
 }
 
 /*
diff --git a/drivers/mtd/nand/cafe_nand.c b/drivers/mtd/nand/cafe_nand.c
index f3f6cfedd69..2bb7170502c 100644
--- a/drivers/mtd/nand/cafe_nand.c
+++ b/drivers/mtd/nand/cafe_nand.c
@@ -377,7 +377,7 @@ static int cafe_nand_read_oob(struct mtd_info *mtd, struct nand_chip *chip,
  * @buf:	buffer to store read data
  * @oob_required:	caller expects OOB data read to chip->oob_poi
  *
- * The hw generator calculates the error syndrome automatically. Therefor
+ * The hw generator calculates the error syndrome automatically. Therefore
  * we need a special oob layout and handling.
  */
 static int cafe_nand_read_page(struct mtd_info *mtd, struct nand_chip *chip,
@@ -520,7 +520,7 @@ static struct nand_bbt_descr cafe_bbt_mirror_descr_512 = {
 };
 
 
-static void cafe_nand_write_page_lowlevel(struct mtd_info *mtd,
+static int cafe_nand_write_page_lowlevel(struct mtd_info *mtd,
 					  struct nand_chip *chip,
 					  const uint8_t *buf, int oob_required)
 {
@@ -531,6 +531,8 @@ static void cafe_nand_write_page_lowlevel(struct mtd_info *mtd,
 
 	/* Set up ECC autogeneration */
 	cafe->ctl2 |= (1<<30);
+
+	return 0;
 }
 
 static int cafe_nand_write_page(struct mtd_info *mtd, struct nand_chip *chip,
@@ -542,9 +544,12 @@ static int cafe_nand_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 	chip->cmdfunc(mtd, NAND_CMD_SEQIN, 0x00, page);
 
 	if (unlikely(raw))
-		chip->ecc.write_page_raw(mtd, chip, buf, oob_required);
+		status = chip->ecc.write_page_raw(mtd, chip, buf, oob_required);
 	else
-		chip->ecc.write_page(mtd, chip, buf, oob_required);
+		status = chip->ecc.write_page(mtd, chip, buf, oob_required);
+
+	if (status < 0)
+		return status;
 
 	/*
 	 * Cached progamming disabled for now, Not sure if its worth the
@@ -571,13 +576,6 @@ static int cafe_nand_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 		status = chip->waitfunc(mtd, chip);
 	}
 
-#ifdef CONFIG_MTD_NAND_VERIFY_WRITE
-	/* Send command to read back the data */
-	chip->cmdfunc(mtd, NAND_CMD_READ0, 0, page);
-
-	if (chip->verify_buf(mtd, buf, mtd->writesize))
-		return -EIO;
-#endif
 	return 0;
 }
 
diff --git a/drivers/mtd/nand/cmx270_nand.c b/drivers/mtd/nand/cmx270_nand.c
index 1024bfc05c8..39b2ef84881 100644
--- a/drivers/mtd/nand/cmx270_nand.c
+++ b/drivers/mtd/nand/cmx270_nand.c
@@ -76,18 +76,6 @@ static void cmx270_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 		*buf++ = readl(this->IO_ADDR_R) >> 16;
 }
 
-static int cmx270_verify_buf(struct mtd_info *mtd, const u_char *buf, int len)
-{
-	int i;
-	struct nand_chip *this = mtd->priv;
-
-	for (i=0; i<len; i++)
-		if (buf[i] != (u_char)(readl(this->IO_ADDR_R) >> 16))
-			return -EFAULT;
-
-	return 0;
-}
-
 static inline void nand_cs_on(void)
 {
 	gpio_set_value(GPIO_NAND_CS, 0);
@@ -209,7 +197,6 @@ static int __init cmx270_init(void)
 	this->read_byte = cmx270_read_byte;
 	this->read_buf = cmx270_read_buf;
 	this->write_buf = cmx270_write_buf;
-	this->verify_buf = cmx270_verify_buf;
 
 	/* Scan to find existence of the device */
 	if (nand_scan (cmx270_nand_mtd, 1)) {
diff --git a/drivers/mtd/nand/davinci_nand.c b/drivers/mtd/nand/davinci_nand.c
index f1deb1ee2c9..945047ad095 100644
--- a/drivers/mtd/nand/davinci_nand.c
+++ b/drivers/mtd/nand/davinci_nand.c
@@ -33,6 +33,7 @@
 #include <linux/mtd/nand.h>
 #include <linux/mtd/partitions.h>
 #include <linux/slab.h>
+#include <linux/of_device.h>
 
 #include <linux/platform_data/mtd-davinci.h>
 #include <linux/platform_data/mtd-davinci-aemif.h>
@@ -518,9 +519,75 @@ static struct nand_ecclayout hwecc4_2048 __initconst = {
 	},
 };
 
+#if defined(CONFIG_OF)
+static const struct of_device_id davinci_nand_of_match[] = {
+	{.compatible = "ti,davinci-nand", },
+	{},
+}
+MODULE_DEVICE_TABLE(of, davinci_nand_of_match);
+
+static struct davinci_nand_pdata
+	*nand_davinci_get_pdata(struct platform_device *pdev)
+{
+	if (!pdev->dev.platform_data && pdev->dev.of_node) {
+		struct davinci_nand_pdata *pdata;
+		const char *mode;
+		u32 prop;
+		int len;
+
+		pdata =  devm_kzalloc(&pdev->dev,
+				sizeof(struct davinci_nand_pdata),
+				GFP_KERNEL);
+		pdev->dev.platform_data = pdata;
+		if (!pdata)
+			return NULL;
+		if (!of_property_read_u32(pdev->dev.of_node,
+			"ti,davinci-chipselect", &prop))
+			pdev->id = prop;
+		if (!of_property_read_u32(pdev->dev.of_node,
+			"ti,davinci-mask-ale", &prop))
+			pdata->mask_ale = prop;
+		if (!of_property_read_u32(pdev->dev.of_node,
+			"ti,davinci-mask-cle", &prop))
+			pdata->mask_cle = prop;
+		if (!of_property_read_u32(pdev->dev.of_node,
+			"ti,davinci-mask-chipsel", &prop))
+			pdata->mask_chipsel = prop;
+		if (!of_property_read_string(pdev->dev.of_node,
+			"ti,davinci-ecc-mode", &mode)) {
+			if (!strncmp("none", mode, 4))
+				pdata->ecc_mode = NAND_ECC_NONE;
+			if (!strncmp("soft", mode, 4))
+				pdata->ecc_mode = NAND_ECC_SOFT;
+			if (!strncmp("hw", mode, 2))
+				pdata->ecc_mode = NAND_ECC_HW;
+		}
+		if (!of_property_read_u32(pdev->dev.of_node,
+			"ti,davinci-ecc-bits", &prop))
+			pdata->ecc_bits = prop;
+		if (!of_property_read_u32(pdev->dev.of_node,
+			"ti,davinci-nand-buswidth", &prop))
+			if (prop == 16)
+				pdata->options |= NAND_BUSWIDTH_16;
+		if (of_find_property(pdev->dev.of_node,
+			"ti,davinci-nand-use-bbt", &len))
+			pdata->bbt_options = NAND_BBT_USE_FLASH;
+	}
+
+	return pdev->dev.platform_data;
+}
+#else
+#define davinci_nand_of_match NULL
+static struct davinci_nand_pdata
+	*nand_davinci_get_pdata(struct platform_device *pdev)
+{
+	return pdev->dev.platform_data;
+}
+#endif
+
 static int __init nand_davinci_probe(struct platform_device *pdev)
 {
-	struct davinci_nand_pdata	*pdata = pdev->dev.platform_data;
+	struct davinci_nand_pdata	*pdata;
 	struct davinci_nand_info	*info;
 	struct resource			*res1;
 	struct resource			*res2;
@@ -530,6 +597,7 @@ static int __init nand_davinci_probe(struct platform_device *pdev)
 	uint32_t			val;
 	nand_ecc_modes_t		ecc_mode;
 
+	pdata = nand_davinci_get_pdata(pdev);
 	/* insist on board-specific configuration */
 	if (!pdata)
 		return -ENODEV;
@@ -656,7 +724,7 @@ static int __init nand_davinci_probe(struct platform_device *pdev)
 		goto err_clk;
 	}
 
-	ret = clk_enable(info->clk);
+	ret = clk_prepare_enable(info->clk);
 	if (ret < 0) {
 		dev_dbg(&pdev->dev, "unable to enable AEMIF clock, err %d\n",
 			ret);
@@ -767,7 +835,7 @@ syndrome_done:
 
 err_scan:
 err_timing:
-	clk_disable(info->clk);
+	clk_disable_unprepare(info->clk);
 
 err_clk_enable:
 	clk_put(info->clk);
@@ -804,7 +872,7 @@ static int __exit nand_davinci_remove(struct platform_device *pdev)
 
 	nand_release(&info->mtd);
 
-	clk_disable(info->clk);
+	clk_disable_unprepare(info->clk);
 	clk_put(info->clk);
 
 	kfree(info);
@@ -816,6 +884,8 @@ static struct platform_driver nand_davinci_driver = {
 	.remove		= __exit_p(nand_davinci_remove),
 	.driver		= {
 		.name	= "davinci_nand",
+		.owner	= THIS_MODULE,
+		.of_match_table = davinci_nand_of_match,
 	},
 };
 MODULE_ALIAS("platform:davinci_nand");
diff --git a/drivers/mtd/nand/denali.c b/drivers/mtd/nand/denali.c
index 0650aafa0dd..e706a237170 100644
--- a/drivers/mtd/nand/denali.c
+++ b/drivers/mtd/nand/denali.c
@@ -1028,7 +1028,7 @@ static void denali_setup_dma(struct denali_nand_info *denali, int op)
 
 /* writes a page. user specifies type, and this function handles the
  * configuration details. */
-static void write_page(struct mtd_info *mtd, struct nand_chip *chip,
+static int write_page(struct mtd_info *mtd, struct nand_chip *chip,
 			const uint8_t *buf, bool raw_xfer)
 {
 	struct denali_nand_info *denali = mtd_to_denali(mtd);
@@ -1078,6 +1078,8 @@ static void write_page(struct mtd_info *mtd, struct nand_chip *chip,
 
 	denali_enable_dma(denali, false);
 	dma_sync_single_for_cpu(denali->dev, addr, size, DMA_TO_DEVICE);
+
+	return 0;
 }
 
 /* NAND core entry points */
@@ -1086,24 +1088,24 @@ static void write_page(struct mtd_info *mtd, struct nand_chip *chip,
  * writing a page with ECC or without is similar, all the work is done
  * by write_page above.
  * */
-static void denali_write_page(struct mtd_info *mtd, struct nand_chip *chip,
+static int denali_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 				const uint8_t *buf, int oob_required)
 {
 	/* for regular page writes, we let HW handle all the ECC
 	 * data written to the device. */
-	write_page(mtd, chip, buf, false);
+	return write_page(mtd, chip, buf, false);
 }
 
 /* This is the callback that the NAND core calls to write a page without ECC.
  * raw access is similar to ECC page writes, so all the work is done in the
  * write_page() function above.
  */
-static void denali_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
+static int denali_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
 					const uint8_t *buf, int oob_required)
 {
 	/* for raw page writes, we want to disable ECC and simply write
 	   whatever data is in the buffer. */
-	write_page(mtd, chip, buf, true);
+	return write_page(mtd, chip, buf, true);
 }
 
 static int denali_write_oob(struct mtd_info *mtd, struct nand_chip *chip,
diff --git a/drivers/mtd/nand/diskonchip.c b/drivers/mtd/nand/diskonchip.c
index e2ca067631c..256eb30f618 100644
--- a/drivers/mtd/nand/diskonchip.c
+++ b/drivers/mtd/nand/diskonchip.c
@@ -376,19 +376,6 @@ static void doc2000_readbuf_dword(struct mtd_info *mtd, u_char *buf, int len)
 	}
 }
 
-static int doc2000_verifybuf(struct mtd_info *mtd, const u_char *buf, int len)
-{
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
-	void __iomem *docptr = doc->virtadr;
-	int i;
-
-	for (i = 0; i < len; i++)
-		if (buf[i] != ReadDOC(docptr, 2k_CDSN_IO))
-			return -EFAULT;
-	return 0;
-}
-
 static uint16_t __init doc200x_ident_chip(struct mtd_info *mtd, int nr)
 {
 	struct nand_chip *this = mtd->priv;
@@ -526,26 +513,6 @@ static void doc2001_readbuf(struct mtd_info *mtd, u_char *buf, int len)
 	buf[i] = ReadDOC(docptr, LastDataRead);
 }
 
-static int doc2001_verifybuf(struct mtd_info *mtd, const u_char *buf, int len)
-{
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
-	void __iomem *docptr = doc->virtadr;
-	int i;
-
-	/* Start read pipeline */
-	ReadDOC(docptr, ReadPipeInit);
-
-	for (i = 0; i < len - 1; i++)
-		if (buf[i] != ReadDOC(docptr, Mil_CDSN_IO)) {
-			ReadDOC(docptr, LastDataRead);
-			return i;
-		}
-	if (buf[i] != ReadDOC(docptr, LastDataRead))
-		return i;
-	return 0;
-}
-
 static u_char doc2001plus_read_byte(struct mtd_info *mtd)
 {
 	struct nand_chip *this = mtd->priv;
@@ -610,33 +577,6 @@ static void doc2001plus_readbuf(struct mtd_info *mtd, u_char *buf, int len)
 		printk("\n");
 }
 
-static int doc2001plus_verifybuf(struct mtd_info *mtd, const u_char *buf, int len)
-{
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
-	void __iomem *docptr = doc->virtadr;
-	int i;
-
-	if (debug)
-		printk("verifybuf of %d bytes: ", len);
-
-	/* Start read pipeline */
-	ReadDOC(docptr, Mplus_ReadPipeInit);
-	ReadDOC(docptr, Mplus_ReadPipeInit);
-
-	for (i = 0; i < len - 2; i++)
-		if (buf[i] != ReadDOC(docptr, Mil_CDSN_IO)) {
-			ReadDOC(docptr, Mplus_LastDataRead);
-			ReadDOC(docptr, Mplus_LastDataRead);
-			return i;
-		}
-	if (buf[len - 2] != ReadDOC(docptr, Mplus_LastDataRead))
-		return len - 2;
-	if (buf[len - 1] != ReadDOC(docptr, Mplus_LastDataRead))
-		return len - 1;
-	return 0;
-}
-
 static void doc2001plus_select_chip(struct mtd_info *mtd, int chip)
 {
 	struct nand_chip *this = mtd->priv;
@@ -1432,7 +1372,6 @@ static inline int __init doc2000_init(struct mtd_info *mtd)
 	this->read_byte = doc2000_read_byte;
 	this->write_buf = doc2000_writebuf;
 	this->read_buf = doc2000_readbuf;
-	this->verify_buf = doc2000_verifybuf;
 	this->scan_bbt = nftl_scan_bbt;
 
 	doc->CDSNControl = CDSN_CTRL_FLASH_IO | CDSN_CTRL_ECC_IO;
@@ -1449,7 +1388,6 @@ static inline int __init doc2001_init(struct mtd_info *mtd)
 	this->read_byte = doc2001_read_byte;
 	this->write_buf = doc2001_writebuf;
 	this->read_buf = doc2001_readbuf;
-	this->verify_buf = doc2001_verifybuf;
 
 	ReadDOC(doc->virtadr, ChipID);
 	ReadDOC(doc->virtadr, ChipID);
@@ -1480,7 +1418,6 @@ static inline int __init doc2001plus_init(struct mtd_info *mtd)
 	this->read_byte = doc2001plus_read_byte;
 	this->write_buf = doc2001plus_writebuf;
 	this->read_buf = doc2001plus_readbuf;
-	this->verify_buf = doc2001plus_verifybuf;
 	this->scan_bbt = inftl_scan_bbt;
 	this->cmd_ctrl = NULL;
 	this->select_chip = doc2001plus_select_chip;
diff --git a/drivers/mtd/nand/docg4.c b/drivers/mtd/nand/docg4.c
index a225e49a562..799da5d1c85 100644
--- a/drivers/mtd/nand/docg4.c
+++ b/drivers/mtd/nand/docg4.c
@@ -378,9 +378,9 @@ static int correct_data(struct mtd_info *mtd, uint8_t *buf, int page)
 		 * bit flips(s) are not reported in stats.
 		 */
 
-		if (doc->oob_buf[15]) {
+		if (nand->oob_poi[15]) {
 			int bit, numsetbits = 0;
-			unsigned long written_flag = doc->oob_buf[15];
+			unsigned long written_flag = nand->oob_poi[15];
 			for_each_set_bit(bit, &written_flag, 8)
 				numsetbits++;
 			if (numsetbits > 4) { /* assume blank */
@@ -428,7 +428,7 @@ static int correct_data(struct mtd_info *mtd, uint8_t *buf, int page)
 		/* if error within oob area preceeding ecc bytes... */
 		if (errpos[i] > DOCG4_PAGE_SIZE * 8)
 			change_bit(errpos[i] - DOCG4_PAGE_SIZE * 8,
-				   (unsigned long *)doc->oob_buf);
+				   (unsigned long *)nand->oob_poi);
 
 		else    /* error in page data */
 			change_bit(errpos[i], (unsigned long *)buf);
@@ -748,18 +748,12 @@ static int read_page(struct mtd_info *mtd, struct nand_chip *nand,
 
 	docg4_read_buf(mtd, buf, DOCG4_PAGE_SIZE); /* read the page data */
 
-	/*
-	 * Diskonchips read oob immediately after a page read.  Mtd
-	 * infrastructure issues a separate command for reading oob after the
-	 * page is read.  So we save the oob bytes in a local buffer and just
-	 * copy it if the next command reads oob from the same page.
-	 */
-
+	/* this device always reads oob after page data */
 	/* first 14 oob bytes read from I/O reg */
-	docg4_read_buf(mtd, doc->oob_buf, 14);
+	docg4_read_buf(mtd, nand->oob_poi, 14);
 
 	/* last 2 read from another reg */
-	buf16 = (uint16_t *)(doc->oob_buf + 14);
+	buf16 = (uint16_t *)(nand->oob_poi + 14);
 	*buf16 = readw(docptr + DOCG4_MYSTERY_REG);
 
 	write_nop(docptr);
@@ -782,6 +776,8 @@ static int read_page(struct mtd_info *mtd, struct nand_chip *nand,
 	}
 
 	writew(0, docptr + DOC_DATAEND);
+	if (bits_corrected == -EBADMSG)	  /* uncorrectable errors */
+		return 0;
 	return bits_corrected;
 }
 
@@ -807,21 +803,6 @@ static int docg4_read_oob(struct mtd_info *mtd, struct nand_chip *nand,
 
 	dev_dbg(doc->dev, "%s: page %x\n", __func__, page);
 
-	/*
-	 * Oob bytes are read as part of a normal page read.  If the previous
-	 * nand command was a read of the page whose oob is now being read, just
-	 * copy the oob bytes that we saved in a local buffer and avoid a
-	 * separate oob read.
-	 */
-	if (doc->last_command.command == NAND_CMD_READ0 &&
-	    doc->last_command.page == page) {
-		memcpy(nand->oob_poi, doc->oob_buf, 16);
-		return 0;
-	}
-
-	/*
-	 * Separate read of oob data only.
-	 */
 	docg4_command(mtd, NAND_CMD_READ0, nand->ecc.size, page);
 
 	writew(DOC_ECCCONF0_READ_MODE | DOCG4_OOB_SIZE, docptr + DOC_ECCCONF0);
@@ -898,7 +879,7 @@ static void docg4_erase_block(struct mtd_info *mtd, int page)
 	write_nop(docptr);
 }
 
-static void write_page(struct mtd_info *mtd, struct nand_chip *nand,
+static int write_page(struct mtd_info *mtd, struct nand_chip *nand,
 		       const uint8_t *buf, bool use_ecc)
 {
 	struct docg4_priv *doc = nand->priv;
@@ -950,15 +931,17 @@ static void write_page(struct mtd_info *mtd, struct nand_chip *nand,
 	write_nop(docptr);
 	writew(0, docptr + DOC_DATAEND);
 	write_nop(docptr);
+
+	return 0;
 }
 
-static void docg4_write_page_raw(struct mtd_info *mtd, struct nand_chip *nand,
+static int docg4_write_page_raw(struct mtd_info *mtd, struct nand_chip *nand,
 				 const uint8_t *buf, int oob_required)
 {
 	return write_page(mtd, nand, buf, false);
 }
 
-static void docg4_write_page(struct mtd_info *mtd, struct nand_chip *nand,
+static int docg4_write_page(struct mtd_info *mtd, struct nand_chip *nand,
 			     const uint8_t *buf, int oob_required)
 {
 	return write_page(mtd, nand, buf, true);
diff --git a/drivers/mtd/nand/fsl_elbc_nand.c b/drivers/mtd/nand/fsl_elbc_nand.c
index 78429380611..cc1480a5e4c 100644
--- a/drivers/mtd/nand/fsl_elbc_nand.c
+++ b/drivers/mtd/nand/fsl_elbc_nand.c
@@ -614,41 +614,6 @@ static void fsl_elbc_read_buf(struct mtd_info *mtd, u8 *buf, int len)
 		        len, avail);
 }
 
-/*
- * Verify buffer against the FCM Controller Data Buffer
- */
-static int fsl_elbc_verify_buf(struct mtd_info *mtd, const u_char *buf, int len)
-{
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_elbc_mtd *priv = chip->priv;
-	struct fsl_elbc_fcm_ctrl *elbc_fcm_ctrl = priv->ctrl->nand;
-	int i;
-
-	if (len < 0) {
-		dev_err(priv->dev, "write_buf of %d bytes", len);
-		return -EINVAL;
-	}
-
-	if ((unsigned int)len >
-			elbc_fcm_ctrl->read_bytes - elbc_fcm_ctrl->index) {
-		dev_err(priv->dev,
-			"verify_buf beyond end of buffer "
-			"(%d requested, %u available)\n",
-			len, elbc_fcm_ctrl->read_bytes - elbc_fcm_ctrl->index);
-
-		elbc_fcm_ctrl->index = elbc_fcm_ctrl->read_bytes;
-		return -EINVAL;
-	}
-
-	for (i = 0; i < len; i++)
-		if (in_8(&elbc_fcm_ctrl->addr[elbc_fcm_ctrl->index + i])
-				!= buf[i])
-			break;
-
-	elbc_fcm_ctrl->index += len;
-	return i == len && elbc_fcm_ctrl->status == LTESR_CC ? 0 : -EIO;
-}
-
 /* This function is called after Program and Erase Operations to
  * check for success or failure.
  */
@@ -766,11 +731,13 @@ static int fsl_elbc_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 /* ECC will be calculated automatically, and errors will be detected in
  * waitfunc.
  */
-static void fsl_elbc_write_page(struct mtd_info *mtd, struct nand_chip *chip,
+static int fsl_elbc_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 				const uint8_t *buf, int oob_required)
 {
 	fsl_elbc_write_buf(mtd, buf, mtd->writesize);
 	fsl_elbc_write_buf(mtd, chip->oob_poi, mtd->oobsize);
+
+	return 0;
 }
 
 static int fsl_elbc_chip_init(struct fsl_elbc_mtd *priv)
@@ -796,7 +763,6 @@ static int fsl_elbc_chip_init(struct fsl_elbc_mtd *priv)
 	chip->read_byte = fsl_elbc_read_byte;
 	chip->write_buf = fsl_elbc_write_buf;
 	chip->read_buf = fsl_elbc_read_buf;
-	chip->verify_buf = fsl_elbc_verify_buf;
 	chip->select_chip = fsl_elbc_select_chip;
 	chip->cmdfunc = fsl_elbc_cmdfunc;
 	chip->waitfunc = fsl_elbc_wait;
@@ -805,7 +771,6 @@ static int fsl_elbc_chip_init(struct fsl_elbc_mtd *priv)
 	chip->bbt_md = &bbt_mirror_descr;
 
 	/* set up nand options */
-	chip->options = NAND_NO_READRDY;
 	chip->bbt_options = NAND_BBT_USE_FLASH;
 
 	chip->controller = &elbc_fcm_ctrl->controller;
@@ -916,7 +881,8 @@ static int __devinit fsl_elbc_nand_probe(struct platform_device *pdev)
 	elbc_fcm_ctrl->chips[bank] = priv;
 	priv->bank = bank;
 	priv->ctrl = fsl_lbc_ctrl_dev;
-	priv->dev = dev;
+	priv->dev = &pdev->dev;
+	dev_set_drvdata(priv->dev, priv);
 
 	priv->vbase = ioremap(res.start, resource_size(&res));
 	if (!priv->vbase) {
@@ -963,11 +929,10 @@ err:
 
 static int fsl_elbc_nand_remove(struct platform_device *pdev)
 {
-	int i;
 	struct fsl_elbc_fcm_ctrl *elbc_fcm_ctrl = fsl_lbc_ctrl_dev->nand;
-	for (i = 0; i < MAX_BANKS; i++)
-		if (elbc_fcm_ctrl->chips[i])
-			fsl_elbc_chip_remove(elbc_fcm_ctrl->chips[i]);
+	struct fsl_elbc_mtd *priv = dev_get_drvdata(&pdev->dev);
+
+	fsl_elbc_chip_remove(priv);
 
 	mutex_lock(&fsl_elbc_nand_mutex);
 	elbc_fcm_ctrl->counter--;
diff --git a/drivers/mtd/nand/fsl_ifc_nand.c b/drivers/mtd/nand/fsl_ifc_nand.c
index 01e2f2e87d8..3551a99076b 100644
--- a/drivers/mtd/nand/fsl_ifc_nand.c
+++ b/drivers/mtd/nand/fsl_ifc_nand.c
@@ -194,7 +194,7 @@ static int is_blank(struct mtd_info *mtd, unsigned int bufnum)
 	struct nand_chip *chip = mtd->priv;
 	struct fsl_ifc_mtd *priv = chip->priv;
 	u8 __iomem *addr = priv->vbase + bufnum * (mtd->writesize * 2);
-	u32 __iomem *mainarea = (u32 *)addr;
+	u32 __iomem *mainarea = (u32 __iomem *)addr;
 	u8 __iomem *oob = addr + mtd->writesize;
 	int i;
 
@@ -592,8 +592,8 @@ static uint8_t fsl_ifc_read_byte16(struct mtd_info *mtd)
 	 * next byte.
 	 */
 	if (ifc_nand_ctrl->index < ifc_nand_ctrl->read_bytes) {
-		data = in_be16((uint16_t *)&ifc_nand_ctrl->
-					addr[ifc_nand_ctrl->index]);
+		data = in_be16((uint16_t __iomem *)&ifc_nand_ctrl->
+			       addr[ifc_nand_ctrl->index]);
 		ifc_nand_ctrl->index += 2;
 		return (uint8_t) data;
 	}
@@ -628,46 +628,6 @@ static void fsl_ifc_read_buf(struct mtd_info *mtd, u8 *buf, int len)
 }
 
 /*
- * Verify buffer against the IFC Controller Data Buffer
- */
-static int fsl_ifc_verify_buf(struct mtd_info *mtd,
-			       const u_char *buf, int len)
-{
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_ifc_mtd *priv = chip->priv;
-	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
-	struct fsl_ifc_nand_ctrl *nctrl = ifc_nand_ctrl;
-	int i;
-
-	if (len < 0) {
-		dev_err(priv->dev, "%s: write_buf of %d bytes", __func__, len);
-		return -EINVAL;
-	}
-
-	if ((unsigned int)len > nctrl->read_bytes - nctrl->index) {
-		dev_err(priv->dev,
-			"%s: beyond end of buffer (%d requested, %u available)\n",
-			__func__, len, nctrl->read_bytes - nctrl->index);
-
-		nctrl->index = nctrl->read_bytes;
-		return -EINVAL;
-	}
-
-	for (i = 0; i < len; i++)
-		if (in_8(&nctrl->addr[nctrl->index + i]) != buf[i])
-			break;
-
-	nctrl->index += len;
-
-	if (i != len)
-		return -EIO;
-	if (ctrl->nand_stat != IFC_NAND_EVTER_STAT_OPC)
-		return -EIO;
-
-	return 0;
-}
-
-/*
  * This function is called after Program and Erase Operations to
  * check for success or failure.
  */
@@ -722,11 +682,13 @@ static int fsl_ifc_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 /* ECC will be calculated automatically, and errors will be detected in
  * waitfunc.
  */
-static void fsl_ifc_write_page(struct mtd_info *mtd, struct nand_chip *chip,
+static int fsl_ifc_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 			       const uint8_t *buf, int oob_required)
 {
 	fsl_ifc_write_buf(mtd, buf, mtd->writesize);
 	fsl_ifc_write_buf(mtd, chip->oob_poi, mtd->oobsize);
+
+	return 0;
 }
 
 static int fsl_ifc_chip_init_tail(struct mtd_info *mtd)
@@ -844,7 +806,6 @@ static int fsl_ifc_chip_init(struct fsl_ifc_mtd *priv)
 
 	chip->write_buf = fsl_ifc_write_buf;
 	chip->read_buf = fsl_ifc_read_buf;
-	chip->verify_buf = fsl_ifc_verify_buf;
 	chip->select_chip = fsl_ifc_select_chip;
 	chip->cmdfunc = fsl_ifc_cmdfunc;
 	chip->waitfunc = fsl_ifc_wait;
@@ -855,7 +816,6 @@ static int fsl_ifc_chip_init(struct fsl_ifc_mtd *priv)
 	out_be32(&ifc->ifc_nand.ncfgr, 0x0);
 
 	/* set up nand options */
-	chip->options = NAND_NO_READRDY;
 	chip->bbt_options = NAND_BBT_USE_FLASH;
 
 
diff --git a/drivers/mtd/nand/gpio.c b/drivers/mtd/nand/gpio.c
index 27000a5f5f4..bc73bc5f271 100644
--- a/drivers/mtd/nand/gpio.c
+++ b/drivers/mtd/nand/gpio.c
@@ -100,23 +100,6 @@ static void gpio_nand_readbuf(struct mtd_info *mtd, u_char *buf, int len)
 	readsb(this->IO_ADDR_R, buf, len);
 }
 
-static int gpio_nand_verifybuf(struct mtd_info *mtd, const u_char *buf, int len)
-{
-	struct nand_chip *this = mtd->priv;
-	unsigned char read, *p = (unsigned char *) buf;
-	int i, err = 0;
-
-	for (i = 0; i < len; i++) {
-		read = readb(this->IO_ADDR_R);
-		if (read != p[i]) {
-			pr_debug("%s: err at %d (read %04x vs %04x)\n",
-			       __func__, i, read, p[i]);
-			err = -EFAULT;
-		}
-	}
-	return err;
-}
-
 static void gpio_nand_writebuf16(struct mtd_info *mtd, const u_char *buf,
 				 int len)
 {
@@ -148,26 +131,6 @@ static void gpio_nand_readbuf16(struct mtd_info *mtd, u_char *buf, int len)
 	}
 }
 
-static int gpio_nand_verifybuf16(struct mtd_info *mtd, const u_char *buf,
-				 int len)
-{
-	struct nand_chip *this = mtd->priv;
-	unsigned short read, *p = (unsigned short *) buf;
-	int i, err = 0;
-	len >>= 1;
-
-	for (i = 0; i < len; i++) {
-		read = readw(this->IO_ADDR_R);
-		if (read != p[i]) {
-			pr_debug("%s: err at %d (read %04x vs %04x)\n",
-			       __func__, i, read, p[i]);
-			err = -EFAULT;
-		}
-	}
-	return err;
-}
-
-
 static int gpio_nand_devready(struct mtd_info *mtd)
 {
 	struct gpiomtd *gpiomtd = gpio_nand_getpriv(mtd);
@@ -391,11 +354,9 @@ static int __devinit gpio_nand_probe(struct platform_device *dev)
 	if (this->options & NAND_BUSWIDTH_16) {
 		this->read_buf   = gpio_nand_readbuf16;
 		this->write_buf  = gpio_nand_writebuf16;
-		this->verify_buf = gpio_nand_verifybuf16;
 	} else {
 		this->read_buf   = gpio_nand_readbuf;
 		this->write_buf  = gpio_nand_writebuf;
-		this->verify_buf = gpio_nand_verifybuf;
 	}
 
 	/* set the mtd private data for the nand driver */
@@ -456,20 +417,7 @@ static struct platform_driver gpio_nand_driver = {
 	},
 };
 
-static int __init gpio_nand_init(void)
-{
-	printk(KERN_INFO "GPIO NAND driver, © 2004 Simtec Electronics\n");
-
-	return platform_driver_register(&gpio_nand_driver);
-}
-
-static void __exit gpio_nand_exit(void)
-{
-	platform_driver_unregister(&gpio_nand_driver);
-}
-
-module_init(gpio_nand_init);
-module_exit(gpio_nand_exit);
+module_platform_driver(gpio_nand_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Ben Dooks <ben@simtec.co.uk>");
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-lib.c b/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
index a1f43329ad4..3502accd4bc 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
@@ -26,7 +26,7 @@
 #include "gpmi-regs.h"
 #include "bch-regs.h"
 
-struct timing_threshod timing_default_threshold = {
+static struct timing_threshod timing_default_threshold = {
 	.max_data_setup_cycles       = (BM_GPMI_TIMING0_DATA_SETUP >>
 						BP_GPMI_TIMING0_DATA_SETUP),
 	.internal_data_setup_in_ns   = 0,
@@ -124,12 +124,42 @@ error:
 	return -ETIMEDOUT;
 }
 
+static int __gpmi_enable_clk(struct gpmi_nand_data *this, bool v)
+{
+	struct clk *clk;
+	int ret;
+	int i;
+
+	for (i = 0; i < GPMI_CLK_MAX; i++) {
+		clk = this->resources.clock[i];
+		if (!clk)
+			break;
+
+		if (v) {
+			ret = clk_prepare_enable(clk);
+			if (ret)
+				goto err_clk;
+		} else {
+			clk_disable_unprepare(clk);
+		}
+	}
+	return 0;
+
+err_clk:
+	for (; i > 0; i--)
+		clk_disable_unprepare(this->resources.clock[i - 1]);
+	return ret;
+}
+
+#define gpmi_enable_clk(x) __gpmi_enable_clk(x, true)
+#define gpmi_disable_clk(x) __gpmi_enable_clk(x, false)
+
 int gpmi_init(struct gpmi_nand_data *this)
 {
 	struct resources *r = &this->resources;
 	int ret;
 
-	ret = clk_prepare_enable(r->clock);
+	ret = gpmi_enable_clk(this);
 	if (ret)
 		goto err_out;
 	ret = gpmi_reset_block(r->gpmi_regs, false);
@@ -149,7 +179,7 @@ int gpmi_init(struct gpmi_nand_data *this)
 	/* Select BCH ECC. */
 	writel(BM_GPMI_CTRL1_BCH_MODE, r->gpmi_regs + HW_GPMI_CTRL1_SET);
 
-	clk_disable_unprepare(r->clock);
+	gpmi_disable_clk(this);
 	return 0;
 err_out:
 	return ret;
@@ -205,7 +235,7 @@ int bch_set_geometry(struct gpmi_nand_data *this)
 	ecc_strength  = bch_geo->ecc_strength >> 1;
 	page_size     = bch_geo->page_size;
 
-	ret = clk_prepare_enable(r->clock);
+	ret = gpmi_enable_clk(this);
 	if (ret)
 		goto err_out;
 
@@ -240,7 +270,7 @@ int bch_set_geometry(struct gpmi_nand_data *this)
 	writel(BM_BCH_CTRL_COMPLETE_IRQ_EN,
 				r->bch_regs + HW_BCH_CTRL_SET);
 
-	clk_disable_unprepare(r->clock);
+	gpmi_disable_clk(this);
 	return 0;
 err_out:
 	return ret;
@@ -263,6 +293,7 @@ static int gpmi_nfc_compute_hardware_timing(struct gpmi_nand_data *this,
 					struct gpmi_nfc_hardware_timing *hw)
 {
 	struct timing_threshod *nfc = &timing_default_threshold;
+	struct resources *r = &this->resources;
 	struct nand_chip *nand = &this->nand;
 	struct nand_timing target = this->timing;
 	bool improved_timing_is_available;
@@ -302,8 +333,9 @@ static int gpmi_nfc_compute_hardware_timing(struct gpmi_nand_data *this,
 		(target.tRHOH_in_ns >= 0) ;
 
 	/* Inspect the clock. */
+	nfc->clock_frequency_in_hz = clk_get_rate(r->clock[0]);
 	clock_frequency_in_hz = nfc->clock_frequency_in_hz;
-	clock_period_in_ns    = 1000000000 / clock_frequency_in_hz;
+	clock_period_in_ns    = NSEC_PER_SEC / clock_frequency_in_hz;
 
 	/*
 	 * The NFC quantizes setup and hold parameters in terms of clock cycles.
@@ -698,17 +730,230 @@ return_results:
 	hw->address_setup_in_cycles = address_setup_in_cycles;
 	hw->use_half_periods        = dll_use_half_periods;
 	hw->sample_delay_factor     = sample_delay_factor;
+	hw->device_busy_timeout     = GPMI_DEFAULT_BUSY_TIMEOUT;
+	hw->wrn_dly_sel             = BV_GPMI_CTRL1_WRN_DLY_SEL_4_TO_8NS;
 
 	/* Return success. */
 	return 0;
 }
 
+/*
+ * <1> Firstly, we should know what's the GPMI-clock means.
+ *     The GPMI-clock is the internal clock in the gpmi nand controller.
+ *     If you set 100MHz to gpmi nand controller, the GPMI-clock's period
+ *     is 10ns. Mark the GPMI-clock's period as GPMI-clock-period.
+ *
+ * <2> Secondly, we should know what's the frequency on the nand chip pins.
+ *     The frequency on the nand chip pins is derived from the GPMI-clock.
+ *     We can get it from the following equation:
+ *
+ *         F = G / (DS + DH)
+ *
+ *         F  : the frequency on the nand chip pins.
+ *         G  : the GPMI clock, such as 100MHz.
+ *         DS : GPMI_HW_GPMI_TIMING0:DATA_SETUP
+ *         DH : GPMI_HW_GPMI_TIMING0:DATA_HOLD
+ *
+ * <3> Thirdly, when the frequency on the nand chip pins is above 33MHz,
+ *     the nand EDO(extended Data Out) timing could be applied.
+ *     The GPMI implements a feedback read strobe to sample the read data.
+ *     The feedback read strobe can be delayed to support the nand EDO timing
+ *     where the read strobe may deasserts before the read data is valid, and
+ *     read data is valid for some time after read strobe.
+ *
+ *     The following figure illustrates some aspects of a NAND Flash read:
+ *
+ *                   |<---tREA---->|
+ *                   |             |
+ *                   |         |   |
+ *                   |<--tRP-->|   |
+ *                   |         |   |
+ *                  __          ___|__________________________________
+ *     RDN            \________/   |
+ *                                 |
+ *                                 /---------\
+ *     Read Data    --------------<           >---------
+ *                                 \---------/
+ *                                |     |
+ *                                |<-D->|
+ *     FeedbackRDN  ________             ____________
+ *                          \___________/
+ *
+ *          D stands for delay, set in the HW_GPMI_CTRL1:RDN_DELAY.
+ *
+ *
+ * <4> Now, we begin to describe how to compute the right RDN_DELAY.
+ *
+ *  4.1) From the aspect of the nand chip pins:
+ *        Delay = (tREA + C - tRP)               {1}
+ *
+ *        tREA : the maximum read access time. From the ONFI nand standards,
+ *               we know that tREA is 16ns in mode 5, tREA is 20ns is mode 4.
+ *               Please check it in : www.onfi.org
+ *        C    : a constant for adjust the delay. default is 4.
+ *        tRP  : the read pulse width.
+ *               Specified by the HW_GPMI_TIMING0:DATA_SETUP:
+ *                    tRP = (GPMI-clock-period) * DATA_SETUP
+ *
+ *  4.2) From the aspect of the GPMI nand controller:
+ *         Delay = RDN_DELAY * 0.125 * RP        {2}
+ *
+ *         RP   : the DLL reference period.
+ *            if (GPMI-clock-period > DLL_THRETHOLD)
+ *                   RP = GPMI-clock-period / 2;
+ *            else
+ *                   RP = GPMI-clock-period;
+ *
+ *            Set the HW_GPMI_CTRL1:HALF_PERIOD if GPMI-clock-period
+ *            is greater DLL_THRETHOLD. In other SOCs, the DLL_THRETHOLD
+ *            is 16ns, but in mx6q, we use 12ns.
+ *
+ *  4.3) since {1} equals {2}, we get:
+ *
+ *                    (tREA + 4 - tRP) * 8
+ *         RDN_DELAY = ---------------------     {3}
+ *                           RP
+ *
+ *  4.4) We only support the fastest asynchronous mode of ONFI nand.
+ *       For some ONFI nand, the mode 4 is the fastest mode;
+ *       while for some ONFI nand, the mode 5 is the fastest mode.
+ *       So we only support the mode 4 and mode 5. It is no need to
+ *       support other modes.
+ */
+static void gpmi_compute_edo_timing(struct gpmi_nand_data *this,
+			struct gpmi_nfc_hardware_timing *hw)
+{
+	struct resources *r = &this->resources;
+	unsigned long rate = clk_get_rate(r->clock[0]);
+	int mode = this->timing_mode;
+	int dll_threshold = 16; /* in ns */
+	unsigned long delay;
+	unsigned long clk_period;
+	int t_rea;
+	int c = 4;
+	int t_rp;
+	int rp;
+
+	/*
+	 * [1] for GPMI_HW_GPMI_TIMING0:
+	 *     The async mode requires 40MHz for mode 4, 50MHz for mode 5.
+	 *     The GPMI can support 100MHz at most. So if we want to
+	 *     get the 40MHz or 50MHz, we have to set DS=1, DH=1.
+	 *     Set the ADDRESS_SETUP to 0 in mode 4.
+	 */
+	hw->data_setup_in_cycles = 1;
+	hw->data_hold_in_cycles = 1;
+	hw->address_setup_in_cycles = ((mode == 5) ? 1 : 0);
+
+	/* [2] for GPMI_HW_GPMI_TIMING1 */
+	hw->device_busy_timeout = 0x9000;
+
+	/* [3] for GPMI_HW_GPMI_CTRL1 */
+	hw->wrn_dly_sel = BV_GPMI_CTRL1_WRN_DLY_SEL_NO_DELAY;
+
+	if (GPMI_IS_MX6Q(this))
+		dll_threshold = 12;
+
+	/*
+	 * Enlarge 10 times for the numerator and denominator in {3}.
+	 * This make us to get more accurate result.
+	 */
+	clk_period = NSEC_PER_SEC / (rate / 10);
+	dll_threshold *= 10;
+	t_rea = ((mode == 5) ? 16 : 20) * 10;
+	c *= 10;
+
+	t_rp = clk_period * 1; /* DATA_SETUP is 1 */
+
+	if (clk_period > dll_threshold) {
+		hw->use_half_periods = 1;
+		rp = clk_period / 2;
+	} else {
+		hw->use_half_periods = 0;
+		rp = clk_period;
+	}
+
+	/*
+	 * Multiply the numerator with 10, we could do a round off:
+	 *      7.8 round up to 8; 7.4 round down to 7.
+	 */
+	delay  = (((t_rea + c - t_rp) * 8) * 10) / rp;
+	delay = (delay + 5) / 10;
+
+	hw->sample_delay_factor = delay;
+}
+
+static int enable_edo_mode(struct gpmi_nand_data *this, int mode)
+{
+	struct resources  *r = &this->resources;
+	struct nand_chip *nand = &this->nand;
+	struct mtd_info	 *mtd = &this->mtd;
+	uint8_t feature[ONFI_SUBFEATURE_PARAM_LEN] = {};
+	unsigned long rate;
+	int ret;
+
+	nand->select_chip(mtd, 0);
+
+	/* [1] send SET FEATURE commond to NAND */
+	feature[0] = mode;
+	ret = nand->onfi_set_features(mtd, nand,
+				ONFI_FEATURE_ADDR_TIMING_MODE, feature);
+	if (ret)
+		goto err_out;
+
+	/* [2] send GET FEATURE command to double-check the timing mode */
+	memset(feature, 0, ONFI_SUBFEATURE_PARAM_LEN);
+	ret = nand->onfi_get_features(mtd, nand,
+				ONFI_FEATURE_ADDR_TIMING_MODE, feature);
+	if (ret || feature[0] != mode)
+		goto err_out;
+
+	nand->select_chip(mtd, -1);
+
+	/* [3] set the main IO clock, 100MHz for mode 5, 80MHz for mode 4. */
+	rate = (mode == 5) ? 100000000 : 80000000;
+	clk_set_rate(r->clock[0], rate);
+
+	/* Let the gpmi_begin() re-compute the timing again. */
+	this->flags &= ~GPMI_TIMING_INIT_OK;
+
+	this->flags |= GPMI_ASYNC_EDO_ENABLED;
+	this->timing_mode = mode;
+	dev_info(this->dev, "enable the asynchronous EDO mode %d\n", mode);
+	return 0;
+
+err_out:
+	nand->select_chip(mtd, -1);
+	dev_err(this->dev, "mode:%d ,failed in set feature.\n", mode);
+	return -EINVAL;
+}
+
+int gpmi_extra_init(struct gpmi_nand_data *this)
+{
+	struct nand_chip *chip = &this->nand;
+
+	/* Enable the asynchronous EDO feature. */
+	if (GPMI_IS_MX6Q(this) && chip->onfi_version) {
+		int mode = onfi_get_async_timing_mode(chip);
+
+		/* We only support the timing mode 4 and mode 5. */
+		if (mode & ONFI_TIMING_MODE_5)
+			mode = 5;
+		else if (mode & ONFI_TIMING_MODE_4)
+			mode = 4;
+		else
+			return 0;
+
+		return enable_edo_mode(this, mode);
+	}
+	return 0;
+}
+
 /* Begin the I/O */
 void gpmi_begin(struct gpmi_nand_data *this)
 {
 	struct resources *r = &this->resources;
-	struct timing_threshod *nfc = &timing_default_threshold;
-	unsigned char  *gpmi_regs = r->gpmi_regs;
+	void __iomem *gpmi_regs = r->gpmi_regs;
 	unsigned int   clock_period_in_ns;
 	uint32_t       reg;
 	unsigned int   dll_wait_time_in_us;
@@ -716,60 +961,66 @@ void gpmi_begin(struct gpmi_nand_data *this)
 	int ret;
 
 	/* Enable the clock. */
-	ret = clk_prepare_enable(r->clock);
+	ret = gpmi_enable_clk(this);
 	if (ret) {
 		pr_err("We failed in enable the clk\n");
 		goto err_out;
 	}
 
-	/* set ready/busy timeout */
-	writel(0x500 << BP_GPMI_TIMING1_BUSY_TIMEOUT,
-		gpmi_regs + HW_GPMI_TIMING1);
-
-	/* Get the timing information we need. */
-	nfc->clock_frequency_in_hz = clk_get_rate(r->clock);
-	clock_period_in_ns = 1000000000 / nfc->clock_frequency_in_hz;
+	/* Only initialize the timing once */
+	if (this->flags & GPMI_TIMING_INIT_OK)
+		return;
+	this->flags |= GPMI_TIMING_INIT_OK;
 
-	gpmi_nfc_compute_hardware_timing(this, &hw);
+	if (this->flags & GPMI_ASYNC_EDO_ENABLED)
+		gpmi_compute_edo_timing(this, &hw);
+	else
+		gpmi_nfc_compute_hardware_timing(this, &hw);
 
-	/* Set up all the simple timing parameters. */
+	/* [1] Set HW_GPMI_TIMING0 */
 	reg = BF_GPMI_TIMING0_ADDRESS_SETUP(hw.address_setup_in_cycles) |
 		BF_GPMI_TIMING0_DATA_HOLD(hw.data_hold_in_cycles)         |
 		BF_GPMI_TIMING0_DATA_SETUP(hw.data_setup_in_cycles)       ;
 
 	writel(reg, gpmi_regs + HW_GPMI_TIMING0);
 
-	/*
-	 * DLL_ENABLE must be set to 0 when setting RDN_DELAY or HALF_PERIOD.
-	 */
+	/* [2] Set HW_GPMI_TIMING1 */
+	writel(BF_GPMI_TIMING1_BUSY_TIMEOUT(hw.device_busy_timeout),
+		gpmi_regs + HW_GPMI_TIMING1);
+
+	/* [3] The following code is to set the HW_GPMI_CTRL1. */
+
+	/* Set the WRN_DLY_SEL */
+	writel(BM_GPMI_CTRL1_WRN_DLY_SEL, gpmi_regs + HW_GPMI_CTRL1_CLR);
+	writel(BF_GPMI_CTRL1_WRN_DLY_SEL(hw.wrn_dly_sel),
+					gpmi_regs + HW_GPMI_CTRL1_SET);
+
+	/* DLL_ENABLE must be set to 0 when setting RDN_DELAY or HALF_PERIOD. */
 	writel(BM_GPMI_CTRL1_DLL_ENABLE, gpmi_regs + HW_GPMI_CTRL1_CLR);
 
 	/* Clear out the DLL control fields. */
-	writel(BM_GPMI_CTRL1_RDN_DELAY,   gpmi_regs + HW_GPMI_CTRL1_CLR);
-	writel(BM_GPMI_CTRL1_HALF_PERIOD, gpmi_regs + HW_GPMI_CTRL1_CLR);
+	reg = BM_GPMI_CTRL1_RDN_DELAY | BM_GPMI_CTRL1_HALF_PERIOD;
+	writel(reg, gpmi_regs + HW_GPMI_CTRL1_CLR);
 
 	/* If no sample delay is called for, return immediately. */
 	if (!hw.sample_delay_factor)
 		return;
 
-	/* Configure the HALF_PERIOD flag. */
-	if (hw.use_half_periods)
-		writel(BM_GPMI_CTRL1_HALF_PERIOD,
-						gpmi_regs + HW_GPMI_CTRL1_SET);
+	/* Set RDN_DELAY or HALF_PERIOD. */
+	reg = ((hw.use_half_periods) ? BM_GPMI_CTRL1_HALF_PERIOD : 0)
+		| BF_GPMI_CTRL1_RDN_DELAY(hw.sample_delay_factor);
 
-	/* Set the delay factor. */
-	writel(BF_GPMI_CTRL1_RDN_DELAY(hw.sample_delay_factor),
-						gpmi_regs + HW_GPMI_CTRL1_SET);
+	writel(reg, gpmi_regs + HW_GPMI_CTRL1_SET);
 
-	/* Enable the DLL. */
+	/* At last, we enable the DLL. */
 	writel(BM_GPMI_CTRL1_DLL_ENABLE, gpmi_regs + HW_GPMI_CTRL1_SET);
 
 	/*
 	 * After we enable the GPMI DLL, we have to wait 64 clock cycles before
-	 * we can use the GPMI.
-	 *
-	 * Calculate the amount of time we need to wait, in microseconds.
+	 * we can use the GPMI. Calculate the amount of time we need to wait,
+	 * in microseconds.
 	 */
+	clock_period_in_ns = NSEC_PER_SEC / clk_get_rate(r->clock[0]);
 	dll_wait_time_in_us = (clock_period_in_ns * 64) / 1000;
 
 	if (!dll_wait_time_in_us)
@@ -784,8 +1035,7 @@ err_out:
 
 void gpmi_end(struct gpmi_nand_data *this)
 {
-	struct resources *r = &this->resources;
-	clk_disable_unprepare(r->clock);
+	gpmi_disable_clk(this);
 }
 
 /* Clears a BCH interrupt. */
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-nand.c b/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
index a6cad5caba7..d79696b2f19 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
@@ -18,6 +18,9 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/clk.h>
 #include <linux/slab.h>
 #include <linux/interrupt.h>
@@ -27,6 +30,7 @@
 #include <linux/pinctrl/consumer.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
+#include <linux/of_mtd.h>
 #include "gpmi-nand.h"
 
 /* add our owner bbt descriptor */
@@ -113,7 +117,7 @@ int common_nfc_set_geometry(struct gpmi_nand_data *this)
 	/* We use the same ECC strength for all chunks. */
 	geo->ecc_strength = get_ecc_strength(this);
 	if (!geo->ecc_strength) {
-		pr_err("We get a wrong ECC strength.\n");
+		pr_err("wrong ECC strength.\n");
 		return -EINVAL;
 	}
 
@@ -316,7 +320,7 @@ acquire_register_block(struct gpmi_nand_data *this, const char *res_name)
 	struct platform_device *pdev = this->pdev;
 	struct resources *res = &this->resources;
 	struct resource *r;
-	void *p;
+	void __iomem *p;
 
 	r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res_name);
 	if (!r) {
@@ -423,8 +427,8 @@ static int __devinit acquire_dma_channels(struct gpmi_nand_data *this)
 	struct platform_device *pdev = this->pdev;
 	struct resource *r_dma;
 	struct device_node *dn;
-	int dma_channel;
-	unsigned int ret;
+	u32 dma_channel;
+	int ret;
 	struct dma_chan *dma_chan;
 	dma_cap_mask_t mask;
 
@@ -464,9 +468,73 @@ acquire_err:
 	return -EINVAL;
 }
 
+static void gpmi_put_clks(struct gpmi_nand_data *this)
+{
+	struct resources *r = &this->resources;
+	struct clk *clk;
+	int i;
+
+	for (i = 0; i < GPMI_CLK_MAX; i++) {
+		clk = r->clock[i];
+		if (clk) {
+			clk_put(clk);
+			r->clock[i] = NULL;
+		}
+	}
+}
+
+static char *extra_clks_for_mx6q[GPMI_CLK_MAX] = {
+	"gpmi_apb", "gpmi_bch", "gpmi_bch_apb", "per1_bch",
+};
+
+static int __devinit gpmi_get_clks(struct gpmi_nand_data *this)
+{
+	struct resources *r = &this->resources;
+	char **extra_clks = NULL;
+	struct clk *clk;
+	int i;
+
+	/* The main clock is stored in the first. */
+	r->clock[0] = clk_get(this->dev, "gpmi_io");
+	if (IS_ERR(r->clock[0]))
+		goto err_clock;
+
+	/* Get extra clocks */
+	if (GPMI_IS_MX6Q(this))
+		extra_clks = extra_clks_for_mx6q;
+	if (!extra_clks)
+		return 0;
+
+	for (i = 1; i < GPMI_CLK_MAX; i++) {
+		if (extra_clks[i - 1] == NULL)
+			break;
+
+		clk = clk_get(this->dev, extra_clks[i - 1]);
+		if (IS_ERR(clk))
+			goto err_clock;
+
+		r->clock[i] = clk;
+	}
+
+	if (GPMI_IS_MX6Q(this))
+		/*
+		 * Set the default value for the gpmi clock in mx6q:
+		 *
+		 * If you want to use the ONFI nand which is in the
+		 * Synchronous Mode, you should change the clock as you need.
+		 */
+		clk_set_rate(r->clock[0], 22000000);
+
+	return 0;
+
+err_clock:
+	dev_dbg(this->dev, "failed in finding the clocks.\n");
+	gpmi_put_clks(this);
+	return -ENOMEM;
+}
+
 static int __devinit acquire_resources(struct gpmi_nand_data *this)
 {
-	struct resources *res = &this->resources;
 	struct pinctrl *pinctrl;
 	int ret;
 
@@ -492,12 +560,9 @@ static int __devinit acquire_resources(struct gpmi_nand_data *this)
 		goto exit_pin;
 	}
 
-	res->clock = clk_get(&this->pdev->dev, NULL);
-	if (IS_ERR(res->clock)) {
-		pr_err("can not get the clock\n");
-		ret = -ENOENT;
+	ret = gpmi_get_clks(this);
+	if (ret)
 		goto exit_clock;
-	}
 	return 0;
 
 exit_clock:
@@ -512,9 +577,7 @@ exit_regs:
 
 static void release_resources(struct gpmi_nand_data *this)
 {
-	struct resources *r = &this->resources;
-
-	clk_put(r->clock);
+	gpmi_put_clks(this);
 	release_register_block(this);
 	release_bch_irq(this);
 	release_dma_channels(this);
@@ -667,12 +730,12 @@ static int gpmi_alloc_dma_buffer(struct gpmi_nand_data *this)
 	struct device *dev = this->dev;
 
 	/* [1] Allocate a command buffer. PAGE_SIZE is enough. */
-	this->cmd_buffer = kzalloc(PAGE_SIZE, GFP_DMA);
+	this->cmd_buffer = kzalloc(PAGE_SIZE, GFP_DMA | GFP_KERNEL);
 	if (this->cmd_buffer == NULL)
 		goto error_alloc;
 
 	/* [2] Allocate a read/write data buffer. PAGE_SIZE is enough. */
-	this->data_buffer_dma = kzalloc(PAGE_SIZE, GFP_DMA);
+	this->data_buffer_dma = kzalloc(PAGE_SIZE, GFP_DMA | GFP_KERNEL);
 	if (this->data_buffer_dma == NULL)
 		goto error_alloc;
 
@@ -930,7 +993,7 @@ exit_nfc:
 	return ret;
 }
 
-static void gpmi_ecc_write_page(struct mtd_info *mtd, struct nand_chip *chip,
+static int gpmi_ecc_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 				const uint8_t *buf, int oob_required)
 {
 	struct gpmi_nand_data *this = chip->priv;
@@ -972,7 +1035,7 @@ static void gpmi_ecc_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 				&payload_virt, &payload_phys);
 		if (ret) {
 			pr_err("Inadequate payload DMA buffer\n");
-			return;
+			return 0;
 		}
 
 		ret = send_page_prepare(this,
@@ -1002,6 +1065,8 @@ exit_auxiliary:
 				nfc_geo->payload_size,
 				payload_virt, payload_phys);
 	}
+
+	return 0;
 }
 
 /*
@@ -1064,6 +1129,9 @@ exit_auxiliary:
  * ECC-based or raw view of the page is implicit in which function it calls
  * (there is a similar pair of ECC-based/raw functions for writing).
  *
+ * FIXME: The following paragraph is incorrect, now that there exist
+ * ecc.read_oob_raw and ecc.write_oob_raw functions.
+ *
  * Since MTD assumes the OOB is not covered by ECC, there is no pair of
  * ECC-based/raw functions for reading or or writing the OOB. The fact that the
  * caller wants an ECC-based or raw view of the page is not propagated down to
@@ -1190,7 +1258,6 @@ static int mx23_check_transcription_stamp(struct gpmi_nand_data *this)
 	unsigned int search_area_size_in_strides;
 	unsigned int stride;
 	unsigned int page;
-	loff_t byte;
 	uint8_t *buffer = chip->buffers->databuf;
 	int saved_chip_number;
 	int found_an_ncb_fingerprint = false;
@@ -1207,9 +1274,8 @@ static int mx23_check_transcription_stamp(struct gpmi_nand_data *this)
 	dev_dbg(dev, "Scanning for an NCB fingerprint...\n");
 
 	for (stride = 0; stride < search_area_size_in_strides; stride++) {
-		/* Compute the page and byte addresses. */
+		/* Compute the page addresses. */
 		page = stride * rom_geo->stride_size_in_pages;
-		byte = page   * mtd->writesize;
 
 		dev_dbg(dev, "Looking for a fingerprint in page 0x%x\n", page);
 
@@ -1251,7 +1317,6 @@ static int mx23_write_transcription_stamp(struct gpmi_nand_data *this)
 	unsigned int block;
 	unsigned int stride;
 	unsigned int page;
-	loff_t       byte;
 	uint8_t      *buffer = chip->buffers->databuf;
 	int saved_chip_number;
 	int status;
@@ -1300,9 +1365,8 @@ static int mx23_write_transcription_stamp(struct gpmi_nand_data *this)
 	/* Loop through the first search area, writing NCB fingerprints. */
 	dev_dbg(dev, "Writing NCB fingerprints...\n");
 	for (stride = 0; stride < search_area_size_in_strides; stride++) {
-		/* Compute the page and byte addresses. */
+		/* Compute the page addresses. */
 		page = stride * rom_geo->stride_size_in_pages;
-		byte = page   * mtd->writesize;
 
 		/* Write the first page of the current stride. */
 		dev_dbg(dev, "Writing an NCB fingerprint in page 0x%x\n", page);
@@ -1436,6 +1500,7 @@ static int gpmi_pre_bbt_scan(struct gpmi_nand_data  *this)
 	/* Adjust the ECC strength according to the chip. */
 	this->nand.ecc.strength = this->bch_geometry.ecc_strength;
 	this->mtd.ecc_strength = this->bch_geometry.ecc_strength;
+	this->mtd.bitflip_threshold = this->bch_geometry.ecc_strength;
 
 	/* NAND boot init, depends on the gpmi_set_geometry(). */
 	return nand_boot_init(this);
@@ -1452,11 +1517,19 @@ static int gpmi_scan_bbt(struct mtd_info *mtd)
 	if (ret)
 		return ret;
 
+	/*
+	 * Can we enable the extra features? such as EDO or Sync mode.
+	 *
+	 * We do not check the return value now. That's means if we fail in
+	 * enable the extra features, we still can run in the normal way.
+	 */
+	gpmi_extra_init(this);
+
 	/* use the default BBT implementation */
 	return nand_default_bbt(mtd);
 }
 
-void gpmi_nfc_exit(struct gpmi_nand_data *this)
+static void gpmi_nfc_exit(struct gpmi_nand_data *this)
 {
 	nand_release(&this->mtd);
 	gpmi_free_dma_buffer(this);
@@ -1497,6 +1570,8 @@ static int __devinit gpmi_nfc_init(struct gpmi_nand_data *this)
 	chip->ecc.size		= 1;
 	chip->ecc.strength	= 8;
 	chip->ecc.layout	= &gpmi_hw_ecclayout;
+	if (of_get_nand_on_flash_bbt(this->dev->of_node))
+		chip->bbt_options |= NAND_BBT_USE_FLASH | NAND_BBT_NO_OOB;
 
 	/* Allocate a temporary DMA buffer for reading ID in the nand_scan() */
 	this->bch_geometry.payload_size = 1024;
@@ -1579,6 +1654,8 @@ static int __devinit gpmi_nand_probe(struct platform_device *pdev)
 	if (ret)
 		goto exit_nfc_init;
 
+	dev_info(this->dev, "driver registered.\n");
+
 	return 0;
 
 exit_nfc_init:
@@ -1586,10 +1663,12 @@ exit_nfc_init:
 exit_acquire_resources:
 	platform_set_drvdata(pdev, NULL);
 	kfree(this);
+	dev_err(this->dev, "driver registration failed: %d\n", ret);
+
 	return ret;
 }
 
-static int __exit gpmi_nand_remove(struct platform_device *pdev)
+static int __devexit gpmi_nand_remove(struct platform_device *pdev)
 {
 	struct gpmi_nand_data *this = platform_get_drvdata(pdev);
 
@@ -1606,29 +1685,10 @@ static struct platform_driver gpmi_nand_driver = {
 		.of_match_table = gpmi_nand_id_table,
 	},
 	.probe   = gpmi_nand_probe,
-	.remove  = __exit_p(gpmi_nand_remove),
+	.remove  = __devexit_p(gpmi_nand_remove),
 	.id_table = gpmi_ids,
 };
-
-static int __init gpmi_nand_init(void)
-{
-	int err;
-
-	err = platform_driver_register(&gpmi_nand_driver);
-	if (err == 0)
-		printk(KERN_INFO "GPMI NAND driver registered. (IMX)\n");
-	else
-		pr_err("i.MX GPMI NAND driver registration failed\n");
-	return err;
-}
-
-static void __exit gpmi_nand_exit(void)
-{
-	platform_driver_unregister(&gpmi_nand_driver);
-}
-
-module_init(gpmi_nand_init);
-module_exit(gpmi_nand_exit);
+module_platform_driver(gpmi_nand_driver);
 
 MODULE_AUTHOR("Freescale Semiconductor, Inc.");
 MODULE_DESCRIPTION("i.MX GPMI NAND Flash Controller Driver");
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-nand.h b/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
index ce5daa16092..7ac25c1e58f 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
@@ -22,14 +22,15 @@
 #include <linux/dma-mapping.h>
 #include <linux/fsl/mxs-dma.h>
 
+#define GPMI_CLK_MAX 5 /* MX6Q needs five clocks */
 struct resources {
-	void          *gpmi_regs;
-	void          *bch_regs;
+	void __iomem  *gpmi_regs;
+	void __iomem  *bch_regs;
 	unsigned int  bch_low_interrupt;
 	unsigned int  bch_high_interrupt;
 	unsigned int  dma_low_channel;
 	unsigned int  dma_high_channel;
-	struct clk    *clock;
+	struct clk    *clock[GPMI_CLK_MAX];
 };
 
 /**
@@ -121,6 +122,11 @@ struct nand_timing {
 };
 
 struct gpmi_nand_data {
+	/* flags */
+#define GPMI_ASYNC_EDO_ENABLED	(1 << 0)
+#define GPMI_TIMING_INIT_OK	(1 << 1)
+	int			flags;
+
 	/* System Interface */
 	struct device		*dev;
 	struct platform_device	*pdev;
@@ -131,6 +137,7 @@ struct gpmi_nand_data {
 
 	/* Flash Hardware */
 	struct nand_timing	timing;
+	int			timing_mode;
 
 	/* BCH */
 	struct bch_geometry	bch_geometry;
@@ -188,16 +195,28 @@ struct gpmi_nand_data {
  * @data_setup_in_cycles:      The data setup time, in cycles.
  * @data_hold_in_cycles:       The data hold time, in cycles.
  * @address_setup_in_cycles:   The address setup time, in cycles.
+ * @device_busy_timeout:       The timeout waiting for NAND Ready/Busy,
+ *                             this value is the number of cycles multiplied
+ *                             by 4096.
  * @use_half_periods:          Indicates the clock is running slowly, so the
  *                             NFC DLL should use half-periods.
  * @sample_delay_factor:       The sample delay factor.
+ * @wrn_dly_sel:               The delay on the GPMI write strobe.
  */
 struct gpmi_nfc_hardware_timing {
+	/* for HW_GPMI_TIMING0 */
 	uint8_t  data_setup_in_cycles;
 	uint8_t  data_hold_in_cycles;
 	uint8_t  address_setup_in_cycles;
+
+	/* for HW_GPMI_TIMING1 */
+	uint16_t device_busy_timeout;
+#define GPMI_DEFAULT_BUSY_TIMEOUT	0x500 /* default busy timeout value.*/
+
+	/* for HW_GPMI_CTRL1 */
 	bool     use_half_periods;
 	uint8_t  sample_delay_factor;
+	uint8_t  wrn_dly_sel;
 };
 
 /**
@@ -246,6 +265,7 @@ extern int start_dma_with_bch_irq(struct gpmi_nand_data *,
 
 /* GPMI-NAND helper function library */
 extern int gpmi_init(struct gpmi_nand_data *);
+extern int gpmi_extra_init(struct gpmi_nand_data *);
 extern void gpmi_clear_bch(struct gpmi_nand_data *);
 extern void gpmi_dump_info(struct gpmi_nand_data *);
 extern int bch_set_geometry(struct gpmi_nand_data *);
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-regs.h b/drivers/mtd/nand/gpmi-nand/gpmi-regs.h
index 83431240e2f..53397cc290f 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-regs.h
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-regs.h
@@ -108,6 +108,15 @@
 #define HW_GPMI_CTRL1_CLR				0x00000068
 #define HW_GPMI_CTRL1_TOG				0x0000006c
 
+#define BP_GPMI_CTRL1_WRN_DLY_SEL			22
+#define BM_GPMI_CTRL1_WRN_DLY_SEL	(0x3 << BP_GPMI_CTRL1_WRN_DLY_SEL)
+#define BF_GPMI_CTRL1_WRN_DLY_SEL(v)  \
+	(((v) << BP_GPMI_CTRL1_WRN_DLY_SEL) & BM_GPMI_CTRL1_WRN_DLY_SEL)
+#define BV_GPMI_CTRL1_WRN_DLY_SEL_4_TO_8NS		0x0
+#define BV_GPMI_CTRL1_WRN_DLY_SEL_6_TO_10NS		0x1
+#define BV_GPMI_CTRL1_WRN_DLY_SEL_7_TO_12NS		0x2
+#define BV_GPMI_CTRL1_WRN_DLY_SEL_NO_DELAY		0x3
+
 #define BM_GPMI_CTRL1_BCH_MODE				(1 << 18)
 
 #define BP_GPMI_CTRL1_DLL_ENABLE			17
@@ -154,6 +163,9 @@
 
 #define HW_GPMI_TIMING1					0x00000080
 #define BP_GPMI_TIMING1_BUSY_TIMEOUT			16
+#define BM_GPMI_TIMING1_BUSY_TIMEOUT	(0xffff << BP_GPMI_TIMING1_BUSY_TIMEOUT)
+#define BF_GPMI_TIMING1_BUSY_TIMEOUT(v)		\
+	(((v) << BP_GPMI_TIMING1_BUSY_TIMEOUT) & BM_GPMI_TIMING1_BUSY_TIMEOUT)
 
 #define HW_GPMI_TIMING2					0x00000090
 #define HW_GPMI_DATA					0x000000a0
diff --git a/drivers/mtd/nand/lpc32xx_mlc.c b/drivers/mtd/nand/lpc32xx_mlc.c
new file mode 100644
index 00000000000..c29b7ac1f6a
--- /dev/null
+++ b/drivers/mtd/nand/lpc32xx_mlc.c
@@ -0,0 +1,924 @@
+/*
+ * Driver for NAND MLC Controller in LPC32xx
+ *
+ * Author: Roland Stigge <stigge@antcom.de>
+ *
+ * Copyright © 2011 WORK Microwave GmbH
+ * Copyright © 2011, 2012 Roland Stigge
+ *
+ * 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.
+ *
+ *
+ * NAND Flash Controller Operation:
+ * - Read: Auto Decode
+ * - Write: Auto Encode
+ * - Tested Page Sizes: 2048, 4096
+ */
+
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/mtd/partitions.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/delay.h>
+#include <linux/completion.h>
+#include <linux/interrupt.h>
+#include <linux/of.h>
+#include <linux/of_mtd.h>
+#include <linux/of_gpio.h>
+#include <linux/mtd/lpc32xx_mlc.h>
+#include <linux/io.h>
+#include <linux/mm.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
+#include <linux/mtd/nand_ecc.h>
+
+#define DRV_NAME "lpc32xx_mlc"
+
+/**********************************************************************
+* MLC NAND controller register offsets
+**********************************************************************/
+
+#define MLC_BUFF(x)			(x + 0x00000)
+#define MLC_DATA(x)			(x + 0x08000)
+#define MLC_CMD(x)			(x + 0x10000)
+#define MLC_ADDR(x)			(x + 0x10004)
+#define MLC_ECC_ENC_REG(x)		(x + 0x10008)
+#define MLC_ECC_DEC_REG(x)		(x + 0x1000C)
+#define MLC_ECC_AUTO_ENC_REG(x)		(x + 0x10010)
+#define MLC_ECC_AUTO_DEC_REG(x)		(x + 0x10014)
+#define MLC_RPR(x)			(x + 0x10018)
+#define MLC_WPR(x)			(x + 0x1001C)
+#define MLC_RUBP(x)			(x + 0x10020)
+#define MLC_ROBP(x)			(x + 0x10024)
+#define MLC_SW_WP_ADD_LOW(x)		(x + 0x10028)
+#define MLC_SW_WP_ADD_HIG(x)		(x + 0x1002C)
+#define MLC_ICR(x)			(x + 0x10030)
+#define MLC_TIME_REG(x)			(x + 0x10034)
+#define MLC_IRQ_MR(x)			(x + 0x10038)
+#define MLC_IRQ_SR(x)			(x + 0x1003C)
+#define MLC_LOCK_PR(x)			(x + 0x10044)
+#define MLC_ISR(x)			(x + 0x10048)
+#define MLC_CEH(x)			(x + 0x1004C)
+
+/**********************************************************************
+* MLC_CMD bit definitions
+**********************************************************************/
+#define MLCCMD_RESET			0xFF
+
+/**********************************************************************
+* MLC_ICR bit definitions
+**********************************************************************/
+#define MLCICR_WPROT			(1 << 3)
+#define MLCICR_LARGEBLOCK		(1 << 2)
+#define MLCICR_LONGADDR			(1 << 1)
+#define MLCICR_16BIT			(1 << 0)  /* unsupported by LPC32x0! */
+
+/**********************************************************************
+* MLC_TIME_REG bit definitions
+**********************************************************************/
+#define MLCTIMEREG_TCEA_DELAY(n)	(((n) & 0x03) << 24)
+#define MLCTIMEREG_BUSY_DELAY(n)	(((n) & 0x1F) << 19)
+#define MLCTIMEREG_NAND_TA(n)		(((n) & 0x07) << 16)
+#define MLCTIMEREG_RD_HIGH(n)		(((n) & 0x0F) << 12)
+#define MLCTIMEREG_RD_LOW(n)		(((n) & 0x0F) << 8)
+#define MLCTIMEREG_WR_HIGH(n)		(((n) & 0x0F) << 4)
+#define MLCTIMEREG_WR_LOW(n)		(((n) & 0x0F) << 0)
+
+/**********************************************************************
+* MLC_IRQ_MR and MLC_IRQ_SR bit definitions
+**********************************************************************/
+#define MLCIRQ_NAND_READY		(1 << 5)
+#define MLCIRQ_CONTROLLER_READY		(1 << 4)
+#define MLCIRQ_DECODE_FAILURE		(1 << 3)
+#define MLCIRQ_DECODE_ERROR		(1 << 2)
+#define MLCIRQ_ECC_READY		(1 << 1)
+#define MLCIRQ_WRPROT_FAULT		(1 << 0)
+
+/**********************************************************************
+* MLC_LOCK_PR bit definitions
+**********************************************************************/
+#define MLCLOCKPR_MAGIC			0xA25E
+
+/**********************************************************************
+* MLC_ISR bit definitions
+**********************************************************************/
+#define MLCISR_DECODER_FAILURE		(1 << 6)
+#define MLCISR_ERRORS			((1 << 4) | (1 << 5))
+#define MLCISR_ERRORS_DETECTED		(1 << 3)
+#define MLCISR_ECC_READY		(1 << 2)
+#define MLCISR_CONTROLLER_READY		(1 << 1)
+#define MLCISR_NAND_READY		(1 << 0)
+
+/**********************************************************************
+* MLC_CEH bit definitions
+**********************************************************************/
+#define MLCCEH_NORMAL			(1 << 0)
+
+struct lpc32xx_nand_cfg_mlc {
+	uint32_t tcea_delay;
+	uint32_t busy_delay;
+	uint32_t nand_ta;
+	uint32_t rd_high;
+	uint32_t rd_low;
+	uint32_t wr_high;
+	uint32_t wr_low;
+	int wp_gpio;
+	struct mtd_partition *parts;
+	unsigned num_parts;
+};
+
+static struct nand_ecclayout lpc32xx_nand_oob = {
+	.eccbytes = 40,
+	.eccpos = { 6,  7,  8,  9, 10, 11, 12, 13, 14, 15,
+		   22, 23, 24, 25, 26, 27, 28, 29, 30, 31,
+		   38, 39, 40, 41, 42, 43, 44, 45, 46, 47,
+		   54, 55, 56, 57, 58, 59, 60, 61, 62, 63 },
+	.oobfree = {
+		{ .offset = 0,
+		  .length = 6, },
+		{ .offset = 16,
+		  .length = 6, },
+		{ .offset = 32,
+		  .length = 6, },
+		{ .offset = 48,
+		  .length = 6, },
+		},
+};
+
+static struct nand_bbt_descr lpc32xx_nand_bbt = {
+	.options = NAND_BBT_ABSPAGE | NAND_BBT_2BIT | NAND_BBT_NO_OOB |
+		   NAND_BBT_WRITE,
+	.pages = { 524224, 0, 0, 0, 0, 0, 0, 0 },
+};
+
+static struct nand_bbt_descr lpc32xx_nand_bbt_mirror = {
+	.options = NAND_BBT_ABSPAGE | NAND_BBT_2BIT | NAND_BBT_NO_OOB |
+		   NAND_BBT_WRITE,
+	.pages = { 524160, 0, 0, 0, 0, 0, 0, 0 },
+};
+
+struct lpc32xx_nand_host {
+	struct nand_chip	nand_chip;
+	struct lpc32xx_mlc_platform_data *pdata;
+	struct clk		*clk;
+	struct mtd_info		mtd;
+	void __iomem		*io_base;
+	int			irq;
+	struct lpc32xx_nand_cfg_mlc	*ncfg;
+	struct completion       comp_nand;
+	struct completion       comp_controller;
+	uint32_t llptr;
+	/*
+	 * Physical addresses of ECC buffer, DMA data buffers, OOB data buffer
+	 */
+	dma_addr_t		oob_buf_phy;
+	/*
+	 * Virtual addresses of ECC buffer, DMA data buffers, OOB data buffer
+	 */
+	uint8_t			*oob_buf;
+	/* Physical address of DMA base address */
+	dma_addr_t		io_base_phy;
+
+	struct completion	comp_dma;
+	struct dma_chan		*dma_chan;
+	struct dma_slave_config	dma_slave_config;
+	struct scatterlist	sgl;
+	uint8_t			*dma_buf;
+	uint8_t			*dummy_buf;
+	int			mlcsubpages; /* number of 512bytes-subpages */
+};
+
+/*
+ * Activate/Deactivate DMA Operation:
+ *
+ * Using the PL080 DMA Controller for transferring the 512 byte subpages
+ * instead of doing readl() / writel() in a loop slows it down significantly.
+ * Measurements via getnstimeofday() upon 512 byte subpage reads reveal:
+ *
+ * - readl() of 128 x 32 bits in a loop: ~20us
+ * - DMA read of 512 bytes (32 bit, 4...128 words bursts): ~60us
+ * - DMA read of 512 bytes (32 bit, no bursts): ~100us
+ *
+ * This applies to the transfer itself. In the DMA case: only the
+ * wait_for_completion() (DMA setup _not_ included).
+ *
+ * Note that the 512 bytes subpage transfer is done directly from/to a
+ * FIFO/buffer inside the NAND controller. Most of the time (~400-800us for a
+ * 2048 bytes page) is spent waiting for the NAND IRQ, anyway. (The NAND
+ * controller transferring data between its internal buffer to/from the NAND
+ * chip.)
+ *
+ * Therefore, using the PL080 DMA is disabled by default, for now.
+ *
+ */
+static int use_dma;
+
+static void lpc32xx_nand_setup(struct lpc32xx_nand_host *host)
+{
+	uint32_t clkrate, tmp;
+
+	/* Reset MLC controller */
+	writel(MLCCMD_RESET, MLC_CMD(host->io_base));
+	udelay(1000);
+
+	/* Get base clock for MLC block */
+	clkrate = clk_get_rate(host->clk);
+	if (clkrate == 0)
+		clkrate = 104000000;
+
+	/* Unlock MLC_ICR
+	 * (among others, will be locked again automatically) */
+	writew(MLCLOCKPR_MAGIC, MLC_LOCK_PR(host->io_base));
+
+	/* Configure MLC Controller: Large Block, 5 Byte Address */
+	tmp = MLCICR_LARGEBLOCK | MLCICR_LONGADDR;
+	writel(tmp, MLC_ICR(host->io_base));
+
+	/* Unlock MLC_TIME_REG
+	 * (among others, will be locked again automatically) */
+	writew(MLCLOCKPR_MAGIC, MLC_LOCK_PR(host->io_base));
+
+	/* Compute clock setup values, see LPC and NAND manual */
+	tmp = 0;
+	tmp |= MLCTIMEREG_TCEA_DELAY(clkrate / host->ncfg->tcea_delay + 1);
+	tmp |= MLCTIMEREG_BUSY_DELAY(clkrate / host->ncfg->busy_delay + 1);
+	tmp |= MLCTIMEREG_NAND_TA(clkrate / host->ncfg->nand_ta + 1);
+	tmp |= MLCTIMEREG_RD_HIGH(clkrate / host->ncfg->rd_high + 1);
+	tmp |= MLCTIMEREG_RD_LOW(clkrate / host->ncfg->rd_low);
+	tmp |= MLCTIMEREG_WR_HIGH(clkrate / host->ncfg->wr_high + 1);
+	tmp |= MLCTIMEREG_WR_LOW(clkrate / host->ncfg->wr_low);
+	writel(tmp, MLC_TIME_REG(host->io_base));
+
+	/* Enable IRQ for CONTROLLER_READY and NAND_READY */
+	writeb(MLCIRQ_CONTROLLER_READY | MLCIRQ_NAND_READY,
+			MLC_IRQ_MR(host->io_base));
+
+	/* Normal nCE operation: nCE controlled by controller */
+	writel(MLCCEH_NORMAL, MLC_CEH(host->io_base));
+}
+
+/*
+ * Hardware specific access to control lines
+ */
+static void lpc32xx_nand_cmd_ctrl(struct mtd_info *mtd, int cmd,
+				  unsigned int ctrl)
+{
+	struct nand_chip *nand_chip = mtd->priv;
+	struct lpc32xx_nand_host *host = nand_chip->priv;
+
+	if (cmd != NAND_CMD_NONE) {
+		if (ctrl & NAND_CLE)
+			writel(cmd, MLC_CMD(host->io_base));
+		else
+			writel(cmd, MLC_ADDR(host->io_base));
+	}
+}
+
+/*
+ * Read Device Ready (NAND device _and_ controller ready)
+ */
+static int lpc32xx_nand_device_ready(struct mtd_info *mtd)
+{
+	struct nand_chip *nand_chip = mtd->priv;
+	struct lpc32xx_nand_host *host = nand_chip->priv;
+
+	if ((readb(MLC_ISR(host->io_base)) &
+	     (MLCISR_CONTROLLER_READY | MLCISR_NAND_READY)) ==
+	    (MLCISR_CONTROLLER_READY | MLCISR_NAND_READY))
+		return  1;
+
+	return 0;
+}
+
+static irqreturn_t lpc3xxx_nand_irq(int irq, struct lpc32xx_nand_host *host)
+{
+	uint8_t sr;
+
+	/* Clear interrupt flag by reading status */
+	sr = readb(MLC_IRQ_SR(host->io_base));
+	if (sr & MLCIRQ_NAND_READY)
+		complete(&host->comp_nand);
+	if (sr & MLCIRQ_CONTROLLER_READY)
+		complete(&host->comp_controller);
+
+	return IRQ_HANDLED;
+}
+
+static int lpc32xx_waitfunc_nand(struct mtd_info *mtd, struct nand_chip *chip)
+{
+	struct lpc32xx_nand_host *host = chip->priv;
+
+	if (readb(MLC_ISR(host->io_base)) & MLCISR_NAND_READY)
+		goto exit;
+
+	wait_for_completion(&host->comp_nand);
+
+	while (!(readb(MLC_ISR(host->io_base)) & MLCISR_NAND_READY)) {
+		/* Seems to be delayed sometimes by controller */
+		dev_dbg(&mtd->dev, "Warning: NAND not ready.\n");
+		cpu_relax();
+	}
+
+exit:
+	return NAND_STATUS_READY;
+}
+
+static int lpc32xx_waitfunc_controller(struct mtd_info *mtd,
+				       struct nand_chip *chip)
+{
+	struct lpc32xx_nand_host *host = chip->priv;
+
+	if (readb(MLC_ISR(host->io_base)) & MLCISR_CONTROLLER_READY)
+		goto exit;
+
+	wait_for_completion(&host->comp_controller);
+
+	while (!(readb(MLC_ISR(host->io_base)) &
+		 MLCISR_CONTROLLER_READY)) {
+		dev_dbg(&mtd->dev, "Warning: Controller not ready.\n");
+		cpu_relax();
+	}
+
+exit:
+	return NAND_STATUS_READY;
+}
+
+static int lpc32xx_waitfunc(struct mtd_info *mtd, struct nand_chip *chip)
+{
+	lpc32xx_waitfunc_nand(mtd, chip);
+	lpc32xx_waitfunc_controller(mtd, chip);
+
+	return NAND_STATUS_READY;
+}
+
+/*
+ * Enable NAND write protect
+ */
+static void lpc32xx_wp_enable(struct lpc32xx_nand_host *host)
+{
+	if (gpio_is_valid(host->ncfg->wp_gpio))
+		gpio_set_value(host->ncfg->wp_gpio, 0);
+}
+
+/*
+ * Disable NAND write protect
+ */
+static void lpc32xx_wp_disable(struct lpc32xx_nand_host *host)
+{
+	if (gpio_is_valid(host->ncfg->wp_gpio))
+		gpio_set_value(host->ncfg->wp_gpio, 1);
+}
+
+static void lpc32xx_dma_complete_func(void *completion)
+{
+	complete(completion);
+}
+
+static int lpc32xx_xmit_dma(struct mtd_info *mtd, void *mem, int len,
+			    enum dma_transfer_direction dir)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct lpc32xx_nand_host *host = chip->priv;
+	struct dma_async_tx_descriptor *desc;
+	int flags = DMA_CTRL_ACK | DMA_PREP_INTERRUPT;
+	int res;
+
+	sg_init_one(&host->sgl, mem, len);
+
+	res = dma_map_sg(host->dma_chan->device->dev, &host->sgl, 1,
+			 DMA_BIDIRECTIONAL);
+	if (res != 1) {
+		dev_err(mtd->dev.parent, "Failed to map sg list\n");
+		return -ENXIO;
+	}
+	desc = dmaengine_prep_slave_sg(host->dma_chan, &host->sgl, 1, dir,
+				       flags);
+	if (!desc) {
+		dev_err(mtd->dev.parent, "Failed to prepare slave sg\n");
+		goto out1;
+	}
+
+	init_completion(&host->comp_dma);
+	desc->callback = lpc32xx_dma_complete_func;
+	desc->callback_param = &host->comp_dma;
+
+	dmaengine_submit(desc);
+	dma_async_issue_pending(host->dma_chan);
+
+	wait_for_completion_timeout(&host->comp_dma, msecs_to_jiffies(1000));
+
+	dma_unmap_sg(host->dma_chan->device->dev, &host->sgl, 1,
+		     DMA_BIDIRECTIONAL);
+	return 0;
+out1:
+	dma_unmap_sg(host->dma_chan->device->dev, &host->sgl, 1,
+		     DMA_BIDIRECTIONAL);
+	return -ENXIO;
+}
+
+static int lpc32xx_read_page(struct mtd_info *mtd, struct nand_chip *chip,
+			     uint8_t *buf, int oob_required, int page)
+{
+	struct lpc32xx_nand_host *host = chip->priv;
+	int i, j;
+	uint8_t *oobbuf = chip->oob_poi;
+	uint32_t mlc_isr;
+	int res;
+	uint8_t *dma_buf;
+	bool dma_mapped;
+
+	if ((void *)buf <= high_memory) {
+		dma_buf = buf;
+		dma_mapped = true;
+	} else {
+		dma_buf = host->dma_buf;
+		dma_mapped = false;
+	}
+
+	/* Writing Command and Address */
+	chip->cmdfunc(mtd, NAND_CMD_READ0, 0, page);
+
+	/* For all sub-pages */
+	for (i = 0; i < host->mlcsubpages; i++) {
+		/* Start Auto Decode Command */
+		writeb(0x00, MLC_ECC_AUTO_DEC_REG(host->io_base));
+
+		/* Wait for Controller Ready */
+		lpc32xx_waitfunc_controller(mtd, chip);
+
+		/* Check ECC Error status */
+		mlc_isr = readl(MLC_ISR(host->io_base));
+		if (mlc_isr & MLCISR_DECODER_FAILURE) {
+			mtd->ecc_stats.failed++;
+			dev_warn(&mtd->dev, "%s: DECODER_FAILURE\n", __func__);
+		} else if (mlc_isr & MLCISR_ERRORS_DETECTED) {
+			mtd->ecc_stats.corrected += ((mlc_isr >> 4) & 0x3) + 1;
+		}
+
+		/* Read 512 + 16 Bytes */
+		if (use_dma) {
+			res = lpc32xx_xmit_dma(mtd, dma_buf + i * 512, 512,
+					       DMA_DEV_TO_MEM);
+			if (res)
+				return res;
+		} else {
+			for (j = 0; j < (512 >> 2); j++) {
+				*((uint32_t *)(buf)) =
+					readl(MLC_BUFF(host->io_base));
+				buf += 4;
+			}
+		}
+		for (j = 0; j < (16 >> 2); j++) {
+			*((uint32_t *)(oobbuf)) =
+				readl(MLC_BUFF(host->io_base));
+			oobbuf += 4;
+		}
+	}
+
+	if (use_dma && !dma_mapped)
+		memcpy(buf, dma_buf, mtd->writesize);
+
+	return 0;
+}
+
+static int lpc32xx_write_page_lowlevel(struct mtd_info *mtd,
+				       struct nand_chip *chip,
+				       const uint8_t *buf, int oob_required)
+{
+	struct lpc32xx_nand_host *host = chip->priv;
+	const uint8_t *oobbuf = chip->oob_poi;
+	uint8_t *dma_buf = (uint8_t *)buf;
+	int res;
+	int i, j;
+
+	if (use_dma && (void *)buf >= high_memory) {
+		dma_buf = host->dma_buf;
+		memcpy(dma_buf, buf, mtd->writesize);
+	}
+
+	for (i = 0; i < host->mlcsubpages; i++) {
+		/* Start Encode */
+		writeb(0x00, MLC_ECC_ENC_REG(host->io_base));
+
+		/* Write 512 + 6 Bytes to Buffer */
+		if (use_dma) {
+			res = lpc32xx_xmit_dma(mtd, dma_buf + i * 512, 512,
+					       DMA_MEM_TO_DEV);
+			if (res)
+				return res;
+		} else {
+			for (j = 0; j < (512 >> 2); j++) {
+				writel(*((uint32_t *)(buf)),
+				       MLC_BUFF(host->io_base));
+				buf += 4;
+			}
+		}
+		writel(*((uint32_t *)(oobbuf)), MLC_BUFF(host->io_base));
+		oobbuf += 4;
+		writew(*((uint16_t *)(oobbuf)), MLC_BUFF(host->io_base));
+		oobbuf += 12;
+
+		/* Auto Encode w/ Bit 8 = 0 (see LPC MLC Controller manual) */
+		writeb(0x00, MLC_ECC_AUTO_ENC_REG(host->io_base));
+
+		/* Wait for Controller Ready */
+		lpc32xx_waitfunc_controller(mtd, chip);
+	}
+	return 0;
+}
+
+static int lpc32xx_write_page(struct mtd_info *mtd, struct nand_chip *chip,
+			      const uint8_t *buf, int oob_required, int page,
+			      int cached, int raw)
+{
+	int res;
+
+	chip->cmdfunc(mtd, NAND_CMD_SEQIN, 0x00, page);
+	res = lpc32xx_write_page_lowlevel(mtd, chip, buf, oob_required);
+	chip->cmdfunc(mtd, NAND_CMD_PAGEPROG, -1, -1);
+	lpc32xx_waitfunc(mtd, chip);
+
+	return res;
+}
+
+static int lpc32xx_read_oob(struct mtd_info *mtd, struct nand_chip *chip,
+			    int page)
+{
+	struct lpc32xx_nand_host *host = chip->priv;
+
+	/* Read whole page - necessary with MLC controller! */
+	lpc32xx_read_page(mtd, chip, host->dummy_buf, 1, page);
+
+	return 0;
+}
+
+static int lpc32xx_write_oob(struct mtd_info *mtd, struct nand_chip *chip,
+			      int page)
+{
+	/* None, write_oob conflicts with the automatic LPC MLC ECC decoder! */
+	return 0;
+}
+
+/* Prepares MLC for transfers with H/W ECC enabled: always enabled anyway */
+static void lpc32xx_ecc_enable(struct mtd_info *mtd, int mode)
+{
+	/* Always enabled! */
+}
+
+static int lpc32xx_dma_setup(struct lpc32xx_nand_host *host)
+{
+	struct mtd_info *mtd = &host->mtd;
+	dma_cap_mask_t mask;
+
+	if (!host->pdata || !host->pdata->dma_filter) {
+		dev_err(mtd->dev.parent, "no DMA platform data\n");
+		return -ENOENT;
+	}
+
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+	host->dma_chan = dma_request_channel(mask, host->pdata->dma_filter,
+					     "nand-mlc");
+	if (!host->dma_chan) {
+		dev_err(mtd->dev.parent, "Failed to request DMA channel\n");
+		return -EBUSY;
+	}
+
+	/*
+	 * Set direction to a sensible value even if the dmaengine driver
+	 * should ignore it. With the default (DMA_MEM_TO_MEM), the amba-pl08x
+	 * driver criticizes it as "alien transfer direction".
+	 */
+	host->dma_slave_config.direction = DMA_DEV_TO_MEM;
+	host->dma_slave_config.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+	host->dma_slave_config.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+	host->dma_slave_config.src_maxburst = 128;
+	host->dma_slave_config.dst_maxburst = 128;
+	/* DMA controller does flow control: */
+	host->dma_slave_config.device_fc = false;
+	host->dma_slave_config.src_addr = MLC_BUFF(host->io_base_phy);
+	host->dma_slave_config.dst_addr = MLC_BUFF(host->io_base_phy);
+	if (dmaengine_slave_config(host->dma_chan, &host->dma_slave_config)) {
+		dev_err(mtd->dev.parent, "Failed to setup DMA slave\n");
+		goto out1;
+	}
+
+	return 0;
+out1:
+	dma_release_channel(host->dma_chan);
+	return -ENXIO;
+}
+
+static struct lpc32xx_nand_cfg_mlc *lpc32xx_parse_dt(struct device *dev)
+{
+	struct lpc32xx_nand_cfg_mlc *ncfg;
+	struct device_node *np = dev->of_node;
+
+	ncfg = devm_kzalloc(dev, sizeof(*ncfg), GFP_KERNEL);
+	if (!ncfg) {
+		dev_err(dev, "could not allocate memory for platform data\n");
+		return NULL;
+	}
+
+	of_property_read_u32(np, "nxp,tcea-delay", &ncfg->tcea_delay);
+	of_property_read_u32(np, "nxp,busy-delay", &ncfg->busy_delay);
+	of_property_read_u32(np, "nxp,nand-ta", &ncfg->nand_ta);
+	of_property_read_u32(np, "nxp,rd-high", &ncfg->rd_high);
+	of_property_read_u32(np, "nxp,rd-low", &ncfg->rd_low);
+	of_property_read_u32(np, "nxp,wr-high", &ncfg->wr_high);
+	of_property_read_u32(np, "nxp,wr-low", &ncfg->wr_low);
+
+	if (!ncfg->tcea_delay || !ncfg->busy_delay || !ncfg->nand_ta ||
+	    !ncfg->rd_high || !ncfg->rd_low || !ncfg->wr_high ||
+	    !ncfg->wr_low) {
+		dev_err(dev, "chip parameters not specified correctly\n");
+		return NULL;
+	}
+
+	ncfg->wp_gpio = of_get_named_gpio(np, "gpios", 0);
+
+	return ncfg;
+}
+
+/*
+ * Probe for NAND controller
+ */
+static int __devinit lpc32xx_nand_probe(struct platform_device *pdev)
+{
+	struct lpc32xx_nand_host *host;
+	struct mtd_info *mtd;
+	struct nand_chip *nand_chip;
+	struct resource *rc;
+	int res;
+	struct mtd_part_parser_data ppdata = {};
+
+	/* Allocate memory for the device structure (and zero it) */
+	host = devm_kzalloc(&pdev->dev, sizeof(*host), GFP_KERNEL);
+	if (!host) {
+		dev_err(&pdev->dev, "failed to allocate device structure.\n");
+		return -ENOMEM;
+	}
+
+	rc = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (rc == NULL) {
+		dev_err(&pdev->dev, "No memory resource found for device!\r\n");
+		return -ENXIO;
+	}
+
+	host->io_base = devm_request_and_ioremap(&pdev->dev, rc);
+	if (host->io_base == NULL) {
+		dev_err(&pdev->dev, "ioremap failed\n");
+		return -EIO;
+	}
+	host->io_base_phy = rc->start;
+
+	mtd = &host->mtd;
+	nand_chip = &host->nand_chip;
+	if (pdev->dev.of_node)
+		host->ncfg = lpc32xx_parse_dt(&pdev->dev);
+	if (!host->ncfg) {
+		dev_err(&pdev->dev,
+			"Missing or bad NAND config from device tree\n");
+		return -ENOENT;
+	}
+	if (host->ncfg->wp_gpio == -EPROBE_DEFER)
+		return -EPROBE_DEFER;
+	if (gpio_is_valid(host->ncfg->wp_gpio) &&
+			gpio_request(host->ncfg->wp_gpio, "NAND WP")) {
+		dev_err(&pdev->dev, "GPIO not available\n");
+		return -EBUSY;
+	}
+	lpc32xx_wp_disable(host);
+
+	host->pdata = pdev->dev.platform_data;
+
+	nand_chip->priv = host;		/* link the private data structures */
+	mtd->priv = nand_chip;
+	mtd->owner = THIS_MODULE;
+	mtd->dev.parent = &pdev->dev;
+
+	/* Get NAND clock */
+	host->clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(host->clk)) {
+		dev_err(&pdev->dev, "Clock initialization failure\n");
+		res = -ENOENT;
+		goto err_exit1;
+	}
+	clk_enable(host->clk);
+
+	nand_chip->cmd_ctrl = lpc32xx_nand_cmd_ctrl;
+	nand_chip->dev_ready = lpc32xx_nand_device_ready;
+	nand_chip->chip_delay = 25; /* us */
+	nand_chip->IO_ADDR_R = MLC_DATA(host->io_base);
+	nand_chip->IO_ADDR_W = MLC_DATA(host->io_base);
+
+	/* Init NAND controller */
+	lpc32xx_nand_setup(host);
+
+	platform_set_drvdata(pdev, host);
+
+	/* Initialize function pointers */
+	nand_chip->ecc.hwctl = lpc32xx_ecc_enable;
+	nand_chip->ecc.read_page_raw = lpc32xx_read_page;
+	nand_chip->ecc.read_page = lpc32xx_read_page;
+	nand_chip->ecc.write_page_raw = lpc32xx_write_page_lowlevel;
+	nand_chip->ecc.write_page = lpc32xx_write_page_lowlevel;
+	nand_chip->ecc.write_oob = lpc32xx_write_oob;
+	nand_chip->ecc.read_oob = lpc32xx_read_oob;
+	nand_chip->ecc.strength = 4;
+	nand_chip->write_page = lpc32xx_write_page;
+	nand_chip->waitfunc = lpc32xx_waitfunc;
+
+	nand_chip->bbt_options = NAND_BBT_USE_FLASH | NAND_BBT_NO_OOB;
+	nand_chip->bbt_td = &lpc32xx_nand_bbt;
+	nand_chip->bbt_md = &lpc32xx_nand_bbt_mirror;
+
+	/* bitflip_threshold's default is defined as ecc_strength anyway.
+	 * Unfortunately, it is set only later at add_mtd_device(). Meanwhile
+	 * being 0, it causes bad block table scanning errors in
+	 * nand_scan_tail(), so preparing it here. */
+	mtd->bitflip_threshold = nand_chip->ecc.strength;
+
+	if (use_dma) {
+		res = lpc32xx_dma_setup(host);
+		if (res) {
+			res = -EIO;
+			goto err_exit2;
+		}
+	}
+
+	/*
+	 * Scan to find existance of the device and
+	 * Get the type of NAND device SMALL block or LARGE block
+	 */
+	if (nand_scan_ident(mtd, 1, NULL)) {
+		res = -ENXIO;
+		goto err_exit3;
+	}
+
+	host->dma_buf = devm_kzalloc(&pdev->dev, mtd->writesize, GFP_KERNEL);
+	if (!host->dma_buf) {
+		dev_err(&pdev->dev, "Error allocating dma_buf memory\n");
+		res = -ENOMEM;
+		goto err_exit3;
+	}
+
+	host->dummy_buf = devm_kzalloc(&pdev->dev, mtd->writesize, GFP_KERNEL);
+	if (!host->dummy_buf) {
+		dev_err(&pdev->dev, "Error allocating dummy_buf memory\n");
+		res = -ENOMEM;
+		goto err_exit3;
+	}
+
+	nand_chip->ecc.mode = NAND_ECC_HW;
+	nand_chip->ecc.size = mtd->writesize;
+	nand_chip->ecc.layout = &lpc32xx_nand_oob;
+	host->mlcsubpages = mtd->writesize / 512;
+
+	/* initially clear interrupt status */
+	readb(MLC_IRQ_SR(host->io_base));
+
+	init_completion(&host->comp_nand);
+	init_completion(&host->comp_controller);
+
+	host->irq = platform_get_irq(pdev, 0);
+	if ((host->irq < 0) || (host->irq >= NR_IRQS)) {
+		dev_err(&pdev->dev, "failed to get platform irq\n");
+		res = -EINVAL;
+		goto err_exit3;
+	}
+
+	if (request_irq(host->irq, (irq_handler_t)&lpc3xxx_nand_irq,
+			IRQF_TRIGGER_HIGH, DRV_NAME, host)) {
+		dev_err(&pdev->dev, "Error requesting NAND IRQ\n");
+		res = -ENXIO;
+		goto err_exit3;
+	}
+
+	/*
+	 * Fills out all the uninitialized function pointers with the defaults
+	 * And scans for a bad block table if appropriate.
+	 */
+	if (nand_scan_tail(mtd)) {
+		res = -ENXIO;
+		goto err_exit4;
+	}
+
+	mtd->name = DRV_NAME;
+
+	ppdata.of_node = pdev->dev.of_node;
+	res = mtd_device_parse_register(mtd, NULL, &ppdata, host->ncfg->parts,
+					host->ncfg->num_parts);
+	if (!res)
+		return res;
+
+	nand_release(mtd);
+
+err_exit4:
+	free_irq(host->irq, host);
+err_exit3:
+	if (use_dma)
+		dma_release_channel(host->dma_chan);
+err_exit2:
+	clk_disable(host->clk);
+	clk_put(host->clk);
+	platform_set_drvdata(pdev, NULL);
+err_exit1:
+	lpc32xx_wp_enable(host);
+	gpio_free(host->ncfg->wp_gpio);
+
+	return res;
+}
+
+/*
+ * Remove NAND device
+ */
+static int __devexit lpc32xx_nand_remove(struct platform_device *pdev)
+{
+	struct lpc32xx_nand_host *host = platform_get_drvdata(pdev);
+	struct mtd_info *mtd = &host->mtd;
+
+	nand_release(mtd);
+	free_irq(host->irq, host);
+	if (use_dma)
+		dma_release_channel(host->dma_chan);
+
+	clk_disable(host->clk);
+	clk_put(host->clk);
+	platform_set_drvdata(pdev, NULL);
+
+	lpc32xx_wp_enable(host);
+	gpio_free(host->ncfg->wp_gpio);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int lpc32xx_nand_resume(struct platform_device *pdev)
+{
+	struct lpc32xx_nand_host *host = platform_get_drvdata(pdev);
+
+	/* Re-enable NAND clock */
+	clk_enable(host->clk);
+
+	/* Fresh init of NAND controller */
+	lpc32xx_nand_setup(host);
+
+	/* Disable write protect */
+	lpc32xx_wp_disable(host);
+
+	return 0;
+}
+
+static int lpc32xx_nand_suspend(struct platform_device *pdev, pm_message_t pm)
+{
+	struct lpc32xx_nand_host *host = platform_get_drvdata(pdev);
+
+	/* Enable write protect for safety */
+	lpc32xx_wp_enable(host);
+
+	/* Disable clock */
+	clk_disable(host->clk);
+	return 0;
+}
+
+#else
+#define lpc32xx_nand_resume NULL
+#define lpc32xx_nand_suspend NULL
+#endif
+
+static const struct of_device_id lpc32xx_nand_match[] = {
+	{ .compatible = "nxp,lpc3220-mlc" },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, lpc32xx_nand_match);
+
+static struct platform_driver lpc32xx_nand_driver = {
+	.probe		= lpc32xx_nand_probe,
+	.remove		= __devexit_p(lpc32xx_nand_remove),
+	.resume		= lpc32xx_nand_resume,
+	.suspend	= lpc32xx_nand_suspend,
+	.driver		= {
+		.name	= DRV_NAME,
+		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(lpc32xx_nand_match),
+	},
+};
+
+module_platform_driver(lpc32xx_nand_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Roland Stigge <stigge@antcom.de>");
+MODULE_DESCRIPTION("NAND driver for the NXP LPC32XX MLC controller");
diff --git a/drivers/mtd/nand/lpc32xx_slc.c b/drivers/mtd/nand/lpc32xx_slc.c
new file mode 100644
index 00000000000..32409c45d47
--- /dev/null
+++ b/drivers/mtd/nand/lpc32xx_slc.c
@@ -0,0 +1,1039 @@
+/*
+ * NXP LPC32XX NAND SLC driver
+ *
+ * Authors:
+ *    Kevin Wells <kevin.wells@nxp.com>
+ *    Roland Stigge <stigge@antcom.de>
+ *
+ * Copyright © 2011 NXP Semiconductors
+ * Copyright © 2012 Roland Stigge
+ *
+ * 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/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/mtd/partitions.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/mm.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
+#include <linux/mtd/nand_ecc.h>
+#include <linux/gpio.h>
+#include <linux/of.h>
+#include <linux/of_mtd.h>
+#include <linux/of_gpio.h>
+#include <linux/mtd/lpc32xx_slc.h>
+
+#define LPC32XX_MODNAME		"lpc32xx-nand"
+
+/**********************************************************************
+* SLC NAND controller register offsets
+**********************************************************************/
+
+#define SLC_DATA(x)		(x + 0x000)
+#define SLC_ADDR(x)		(x + 0x004)
+#define SLC_CMD(x)		(x + 0x008)
+#define SLC_STOP(x)		(x + 0x00C)
+#define SLC_CTRL(x)		(x + 0x010)
+#define SLC_CFG(x)		(x + 0x014)
+#define SLC_STAT(x)		(x + 0x018)
+#define SLC_INT_STAT(x)		(x + 0x01C)
+#define SLC_IEN(x)		(x + 0x020)
+#define SLC_ISR(x)		(x + 0x024)
+#define SLC_ICR(x)		(x + 0x028)
+#define SLC_TAC(x)		(x + 0x02C)
+#define SLC_TC(x)		(x + 0x030)
+#define SLC_ECC(x)		(x + 0x034)
+#define SLC_DMA_DATA(x)		(x + 0x038)
+
+/**********************************************************************
+* slc_ctrl register definitions
+**********************************************************************/
+#define SLCCTRL_SW_RESET	(1 << 2) /* Reset the NAND controller bit */
+#define SLCCTRL_ECC_CLEAR	(1 << 1) /* Reset ECC bit */
+#define SLCCTRL_DMA_START	(1 << 0) /* Start DMA channel bit */
+
+/**********************************************************************
+* slc_cfg register definitions
+**********************************************************************/
+#define SLCCFG_CE_LOW		(1 << 5) /* Force CE low bit */
+#define SLCCFG_DMA_ECC		(1 << 4) /* Enable DMA ECC bit */
+#define SLCCFG_ECC_EN		(1 << 3) /* ECC enable bit */
+#define SLCCFG_DMA_BURST	(1 << 2) /* DMA burst bit */
+#define SLCCFG_DMA_DIR		(1 << 1) /* DMA write(0)/read(1) bit */
+#define SLCCFG_WIDTH		(1 << 0) /* External device width, 0=8bit */
+
+/**********************************************************************
+* slc_stat register definitions
+**********************************************************************/
+#define SLCSTAT_DMA_FIFO	(1 << 2) /* DMA FIFO has data bit */
+#define SLCSTAT_SLC_FIFO	(1 << 1) /* SLC FIFO has data bit */
+#define SLCSTAT_NAND_READY	(1 << 0) /* NAND device is ready bit */
+
+/**********************************************************************
+* slc_int_stat, slc_ien, slc_isr, and slc_icr register definitions
+**********************************************************************/
+#define SLCSTAT_INT_TC		(1 << 1) /* Transfer count bit */
+#define SLCSTAT_INT_RDY_EN	(1 << 0) /* Ready interrupt bit */
+
+/**********************************************************************
+* slc_tac register definitions
+**********************************************************************/
+/* Clock setting for RDY write sample wait time in 2*n clocks */
+#define SLCTAC_WDR(n)		(((n) & 0xF) << 28)
+/* Write pulse width in clock cycles, 1 to 16 clocks */
+#define SLCTAC_WWIDTH(n)	(((n) & 0xF) << 24)
+/* Write hold time of control and data signals, 1 to 16 clocks */
+#define SLCTAC_WHOLD(n)		(((n) & 0xF) << 20)
+/* Write setup time of control and data signals, 1 to 16 clocks */
+#define SLCTAC_WSETUP(n)	(((n) & 0xF) << 16)
+/* Clock setting for RDY read sample wait time in 2*n clocks */
+#define SLCTAC_RDR(n)		(((n) & 0xF) << 12)
+/* Read pulse width in clock cycles, 1 to 16 clocks */
+#define SLCTAC_RWIDTH(n)	(((n) & 0xF) << 8)
+/* Read hold time of control and data signals, 1 to 16 clocks */
+#define SLCTAC_RHOLD(n)		(((n) & 0xF) << 4)
+/* Read setup time of control and data signals, 1 to 16 clocks */
+#define SLCTAC_RSETUP(n)	(((n) & 0xF) << 0)
+
+/**********************************************************************
+* slc_ecc register definitions
+**********************************************************************/
+/* ECC line party fetch macro */
+#define SLCECC_TO_LINEPAR(n)	(((n) >> 6) & 0x7FFF)
+#define SLCECC_TO_COLPAR(n)	((n) & 0x3F)
+
+/*
+ * DMA requires storage space for the DMA local buffer and the hardware ECC
+ * storage area. The DMA local buffer is only used if DMA mapping fails
+ * during runtime.
+ */
+#define LPC32XX_DMA_DATA_SIZE		4096
+#define LPC32XX_ECC_SAVE_SIZE		((4096 / 256) * 4)
+
+/* Number of bytes used for ECC stored in NAND per 256 bytes */
+#define LPC32XX_SLC_DEV_ECC_BYTES	3
+
+/*
+ * If the NAND base clock frequency can't be fetched, this frequency will be
+ * used instead as the base. This rate is used to setup the timing registers
+ * used for NAND accesses.
+ */
+#define LPC32XX_DEF_BUS_RATE		133250000
+
+/* Milliseconds for DMA FIFO timeout (unlikely anyway) */
+#define LPC32XX_DMA_TIMEOUT		100
+
+/*
+ * NAND ECC Layout for small page NAND devices
+ * Note: For large and huge page devices, the default layouts are used
+ */
+static struct nand_ecclayout lpc32xx_nand_oob_16 = {
+	.eccbytes = 6,
+	.eccpos = {10, 11, 12, 13, 14, 15},
+	.oobfree = {
+		{ .offset = 0, .length = 4 },
+		{ .offset = 6, .length = 4 },
+	},
+};
+
+static u8 bbt_pattern[] = {'B', 'b', 't', '0' };
+static u8 mirror_pattern[] = {'1', 't', 'b', 'B' };
+
+/*
+ * Small page FLASH BBT descriptors, marker at offset 0, version at offset 6
+ * Note: Large page devices used the default layout
+ */
+static struct nand_bbt_descr bbt_smallpage_main_descr = {
+	.options = NAND_BBT_LASTBLOCK | NAND_BBT_CREATE | NAND_BBT_WRITE
+		| NAND_BBT_2BIT | NAND_BBT_VERSION | NAND_BBT_PERCHIP,
+	.offs =	0,
+	.len = 4,
+	.veroffs = 6,
+	.maxblocks = 4,
+	.pattern = bbt_pattern
+};
+
+static struct nand_bbt_descr bbt_smallpage_mirror_descr = {
+	.options = NAND_BBT_LASTBLOCK | NAND_BBT_CREATE | NAND_BBT_WRITE
+		| NAND_BBT_2BIT | NAND_BBT_VERSION | NAND_BBT_PERCHIP,
+	.offs =	0,
+	.len = 4,
+	.veroffs = 6,
+	.maxblocks = 4,
+	.pattern = mirror_pattern
+};
+
+/*
+ * NAND platform configuration structure
+ */
+struct lpc32xx_nand_cfg_slc {
+	uint32_t wdr_clks;
+	uint32_t wwidth;
+	uint32_t whold;
+	uint32_t wsetup;
+	uint32_t rdr_clks;
+	uint32_t rwidth;
+	uint32_t rhold;
+	uint32_t rsetup;
+	bool use_bbt;
+	int wp_gpio;
+	struct mtd_partition *parts;
+	unsigned num_parts;
+};
+
+struct lpc32xx_nand_host {
+	struct nand_chip	nand_chip;
+	struct lpc32xx_slc_platform_data *pdata;
+	struct clk		*clk;
+	struct mtd_info		mtd;
+	void __iomem		*io_base;
+	struct lpc32xx_nand_cfg_slc *ncfg;
+
+	struct completion	comp;
+	struct dma_chan		*dma_chan;
+	uint32_t		dma_buf_len;
+	struct dma_slave_config	dma_slave_config;
+	struct scatterlist	sgl;
+
+	/*
+	 * DMA and CPU addresses of ECC work area and data buffer
+	 */
+	uint32_t		*ecc_buf;
+	uint8_t			*data_buf;
+	dma_addr_t		io_base_dma;
+};
+
+static void lpc32xx_nand_setup(struct lpc32xx_nand_host *host)
+{
+	uint32_t clkrate, tmp;
+
+	/* Reset SLC controller */
+	writel(SLCCTRL_SW_RESET, SLC_CTRL(host->io_base));
+	udelay(1000);
+
+	/* Basic setup */
+	writel(0, SLC_CFG(host->io_base));
+	writel(0, SLC_IEN(host->io_base));
+	writel((SLCSTAT_INT_TC | SLCSTAT_INT_RDY_EN),
+		SLC_ICR(host->io_base));
+
+	/* Get base clock for SLC block */
+	clkrate = clk_get_rate(host->clk);
+	if (clkrate == 0)
+		clkrate = LPC32XX_DEF_BUS_RATE;
+
+	/* Compute clock setup values */
+	tmp = SLCTAC_WDR(host->ncfg->wdr_clks) |
+		SLCTAC_WWIDTH(1 + (clkrate / host->ncfg->wwidth)) |
+		SLCTAC_WHOLD(1 + (clkrate / host->ncfg->whold)) |
+		SLCTAC_WSETUP(1 + (clkrate / host->ncfg->wsetup)) |
+		SLCTAC_RDR(host->ncfg->rdr_clks) |
+		SLCTAC_RWIDTH(1 + (clkrate / host->ncfg->rwidth)) |
+		SLCTAC_RHOLD(1 + (clkrate / host->ncfg->rhold)) |
+		SLCTAC_RSETUP(1 + (clkrate / host->ncfg->rsetup));
+	writel(tmp, SLC_TAC(host->io_base));
+}
+
+/*
+ * Hardware specific access to control lines
+ */
+static void lpc32xx_nand_cmd_ctrl(struct mtd_info *mtd, int cmd,
+	unsigned int ctrl)
+{
+	uint32_t tmp;
+	struct nand_chip *chip = mtd->priv;
+	struct lpc32xx_nand_host *host = chip->priv;
+
+	/* Does CE state need to be changed? */
+	tmp = readl(SLC_CFG(host->io_base));
+	if (ctrl & NAND_NCE)
+		tmp |= SLCCFG_CE_LOW;
+	else
+		tmp &= ~SLCCFG_CE_LOW;
+	writel(tmp, SLC_CFG(host->io_base));
+
+	if (cmd != NAND_CMD_NONE) {
+		if (ctrl & NAND_CLE)
+			writel(cmd, SLC_CMD(host->io_base));
+		else
+			writel(cmd, SLC_ADDR(host->io_base));
+	}
+}
+
+/*
+ * Read the Device Ready pin
+ */
+static int lpc32xx_nand_device_ready(struct mtd_info *mtd)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct lpc32xx_nand_host *host = chip->priv;
+	int rdy = 0;
+
+	if ((readl(SLC_STAT(host->io_base)) & SLCSTAT_NAND_READY) != 0)
+		rdy = 1;
+
+	return rdy;
+}
+
+/*
+ * Enable NAND write protect
+ */
+static void lpc32xx_wp_enable(struct lpc32xx_nand_host *host)
+{
+	if (gpio_is_valid(host->ncfg->wp_gpio))
+		gpio_set_value(host->ncfg->wp_gpio, 0);
+}
+
+/*
+ * Disable NAND write protect
+ */
+static void lpc32xx_wp_disable(struct lpc32xx_nand_host *host)
+{
+	if (gpio_is_valid(host->ncfg->wp_gpio))
+		gpio_set_value(host->ncfg->wp_gpio, 1);
+}
+
+/*
+ * Prepares SLC for transfers with H/W ECC enabled
+ */
+static void lpc32xx_nand_ecc_enable(struct mtd_info *mtd, int mode)
+{
+	/* Hardware ECC is enabled automatically in hardware as needed */
+}
+
+/*
+ * Calculates the ECC for the data
+ */
+static int lpc32xx_nand_ecc_calculate(struct mtd_info *mtd,
+				      const unsigned char *buf,
+				      unsigned char *code)
+{
+	/*
+	 * ECC is calculated automatically in hardware during syndrome read
+	 * and write operations, so it doesn't need to be calculated here.
+	 */
+	return 0;
+}
+
+/*
+ * Read a single byte from NAND device
+ */
+static uint8_t lpc32xx_nand_read_byte(struct mtd_info *mtd)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct lpc32xx_nand_host *host = chip->priv;
+
+	return (uint8_t)readl(SLC_DATA(host->io_base));
+}
+
+/*
+ * Simple device read without ECC
+ */
+static void lpc32xx_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct lpc32xx_nand_host *host = chip->priv;
+
+	/* Direct device read with no ECC */
+	while (len-- > 0)
+		*buf++ = (uint8_t)readl(SLC_DATA(host->io_base));
+}
+
+/*
+ * Simple device write without ECC
+ */
+static void lpc32xx_nand_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct lpc32xx_nand_host *host = chip->priv;
+
+	/* Direct device write with no ECC */
+	while (len-- > 0)
+		writel((uint32_t)*buf++, SLC_DATA(host->io_base));
+}
+
+/*
+ * Read the OOB data from the device without ECC using FIFO method
+ */
+static int lpc32xx_nand_read_oob_syndrome(struct mtd_info *mtd,
+					  struct nand_chip *chip, int page)
+{
+	chip->cmdfunc(mtd, NAND_CMD_READOOB, 0, page);
+	chip->read_buf(mtd, chip->oob_poi, mtd->oobsize);
+
+	return 0;
+}
+
+/*
+ * Write the OOB data to the device without ECC using FIFO method
+ */
+static int lpc32xx_nand_write_oob_syndrome(struct mtd_info *mtd,
+	struct nand_chip *chip, int page)
+{
+	int status;
+
+	chip->cmdfunc(mtd, NAND_CMD_SEQIN, mtd->writesize, page);
+	chip->write_buf(mtd, chip->oob_poi, mtd->oobsize);
+
+	/* Send command to program the OOB data */
+	chip->cmdfunc(mtd, NAND_CMD_PAGEPROG, -1, -1);
+
+	status = chip->waitfunc(mtd, chip);
+
+	return status & NAND_STATUS_FAIL ? -EIO : 0;
+}
+
+/*
+ * Fills in the ECC fields in the OOB buffer with the hardware generated ECC
+ */
+static void lpc32xx_slc_ecc_copy(uint8_t *spare, const uint32_t *ecc, int count)
+{
+	int i;
+
+	for (i = 0; i < (count * 3); i += 3) {
+		uint32_t ce = ecc[i / 3];
+		ce = ~(ce << 2) & 0xFFFFFF;
+		spare[i + 2] = (uint8_t)(ce & 0xFF);
+		ce >>= 8;
+		spare[i + 1] = (uint8_t)(ce & 0xFF);
+		ce >>= 8;
+		spare[i] = (uint8_t)(ce & 0xFF);
+	}
+}
+
+static void lpc32xx_dma_complete_func(void *completion)
+{
+	complete(completion);
+}
+
+static int lpc32xx_xmit_dma(struct mtd_info *mtd, dma_addr_t dma,
+			    void *mem, int len, enum dma_transfer_direction dir)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct lpc32xx_nand_host *host = chip->priv;
+	struct dma_async_tx_descriptor *desc;
+	int flags = DMA_CTRL_ACK | DMA_PREP_INTERRUPT;
+	int res;
+
+	host->dma_slave_config.direction = dir;
+	host->dma_slave_config.src_addr = dma;
+	host->dma_slave_config.dst_addr = dma;
+	host->dma_slave_config.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+	host->dma_slave_config.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+	host->dma_slave_config.src_maxburst = 4;
+	host->dma_slave_config.dst_maxburst = 4;
+	/* DMA controller does flow control: */
+	host->dma_slave_config.device_fc = false;
+	if (dmaengine_slave_config(host->dma_chan, &host->dma_slave_config)) {
+		dev_err(mtd->dev.parent, "Failed to setup DMA slave\n");
+		return -ENXIO;
+	}
+
+	sg_init_one(&host->sgl, mem, len);
+
+	res = dma_map_sg(host->dma_chan->device->dev, &host->sgl, 1,
+			 DMA_BIDIRECTIONAL);
+	if (res != 1) {
+		dev_err(mtd->dev.parent, "Failed to map sg list\n");
+		return -ENXIO;
+	}
+	desc = dmaengine_prep_slave_sg(host->dma_chan, &host->sgl, 1, dir,
+				       flags);
+	if (!desc) {
+		dev_err(mtd->dev.parent, "Failed to prepare slave sg\n");
+		goto out1;
+	}
+
+	init_completion(&host->comp);
+	desc->callback = lpc32xx_dma_complete_func;
+	desc->callback_param = &host->comp;
+
+	dmaengine_submit(desc);
+	dma_async_issue_pending(host->dma_chan);
+
+	wait_for_completion_timeout(&host->comp, msecs_to_jiffies(1000));
+
+	dma_unmap_sg(host->dma_chan->device->dev, &host->sgl, 1,
+		     DMA_BIDIRECTIONAL);
+
+	return 0;
+out1:
+	dma_unmap_sg(host->dma_chan->device->dev, &host->sgl, 1,
+		     DMA_BIDIRECTIONAL);
+	return -ENXIO;
+}
+
+/*
+ * DMA read/write transfers with ECC support
+ */
+static int lpc32xx_xfer(struct mtd_info *mtd, uint8_t *buf, int eccsubpages,
+			int read)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct lpc32xx_nand_host *host = chip->priv;
+	int i, status = 0;
+	unsigned long timeout;
+	int res;
+	enum dma_transfer_direction dir =
+		read ? DMA_DEV_TO_MEM : DMA_MEM_TO_DEV;
+	uint8_t *dma_buf;
+	bool dma_mapped;
+
+	if ((void *)buf <= high_memory) {
+		dma_buf = buf;
+		dma_mapped = true;
+	} else {
+		dma_buf = host->data_buf;
+		dma_mapped = false;
+		if (!read)
+			memcpy(host->data_buf, buf, mtd->writesize);
+	}
+
+	if (read) {
+		writel(readl(SLC_CFG(host->io_base)) |
+		       SLCCFG_DMA_DIR | SLCCFG_ECC_EN | SLCCFG_DMA_ECC |
+		       SLCCFG_DMA_BURST, SLC_CFG(host->io_base));
+	} else {
+		writel((readl(SLC_CFG(host->io_base)) |
+			SLCCFG_ECC_EN | SLCCFG_DMA_ECC | SLCCFG_DMA_BURST) &
+		       ~SLCCFG_DMA_DIR,
+			SLC_CFG(host->io_base));
+	}
+
+	/* Clear initial ECC */
+	writel(SLCCTRL_ECC_CLEAR, SLC_CTRL(host->io_base));
+
+	/* Transfer size is data area only */
+	writel(mtd->writesize, SLC_TC(host->io_base));
+
+	/* Start transfer in the NAND controller */
+	writel(readl(SLC_CTRL(host->io_base)) | SLCCTRL_DMA_START,
+	       SLC_CTRL(host->io_base));
+
+	for (i = 0; i < chip->ecc.steps; i++) {
+		/* Data */
+		res = lpc32xx_xmit_dma(mtd, SLC_DMA_DATA(host->io_base_dma),
+				       dma_buf + i * chip->ecc.size,
+				       mtd->writesize / chip->ecc.steps, dir);
+		if (res)
+			return res;
+
+		/* Always _read_ ECC */
+		if (i == chip->ecc.steps - 1)
+			break;
+		if (!read) /* ECC availability delayed on write */
+			udelay(10);
+		res = lpc32xx_xmit_dma(mtd, SLC_ECC(host->io_base_dma),
+				       &host->ecc_buf[i], 4, DMA_DEV_TO_MEM);
+		if (res)
+			return res;
+	}
+
+	/*
+	 * According to NXP, the DMA can be finished here, but the NAND
+	 * controller may still have buffered data. After porting to using the
+	 * dmaengine DMA driver (amba-pl080), the condition (DMA_FIFO empty)
+	 * appears to be always true, according to tests. Keeping the check for
+	 * safety reasons for now.
+	 */
+	if (readl(SLC_STAT(host->io_base)) & SLCSTAT_DMA_FIFO) {
+		dev_warn(mtd->dev.parent, "FIFO not empty!\n");
+		timeout = jiffies + msecs_to_jiffies(LPC32XX_DMA_TIMEOUT);
+		while ((readl(SLC_STAT(host->io_base)) & SLCSTAT_DMA_FIFO) &&
+		       time_before(jiffies, timeout))
+			cpu_relax();
+		if (!time_before(jiffies, timeout)) {
+			dev_err(mtd->dev.parent, "FIFO held data too long\n");
+			status = -EIO;
+		}
+	}
+
+	/* Read last calculated ECC value */
+	if (!read)
+		udelay(10);
+	host->ecc_buf[chip->ecc.steps - 1] =
+		readl(SLC_ECC(host->io_base));
+
+	/* Flush DMA */
+	dmaengine_terminate_all(host->dma_chan);
+
+	if (readl(SLC_STAT(host->io_base)) & SLCSTAT_DMA_FIFO ||
+	    readl(SLC_TC(host->io_base))) {
+		/* Something is left in the FIFO, something is wrong */
+		dev_err(mtd->dev.parent, "DMA FIFO failure\n");
+		status = -EIO;
+	}
+
+	/* Stop DMA & HW ECC */
+	writel(readl(SLC_CTRL(host->io_base)) & ~SLCCTRL_DMA_START,
+	       SLC_CTRL(host->io_base));
+	writel(readl(SLC_CFG(host->io_base)) &
+	       ~(SLCCFG_DMA_DIR | SLCCFG_ECC_EN | SLCCFG_DMA_ECC |
+		 SLCCFG_DMA_BURST), SLC_CFG(host->io_base));
+
+	if (!dma_mapped && read)
+		memcpy(buf, host->data_buf, mtd->writesize);
+
+	return status;
+}
+
+/*
+ * Read the data and OOB data from the device, use ECC correction with the
+ * data, disable ECC for the OOB data
+ */
+static int lpc32xx_nand_read_page_syndrome(struct mtd_info *mtd,
+					   struct nand_chip *chip, uint8_t *buf,
+					   int oob_required, int page)
+{
+	struct lpc32xx_nand_host *host = chip->priv;
+	int stat, i, status;
+	uint8_t *oobecc, tmpecc[LPC32XX_ECC_SAVE_SIZE];
+
+	/* Issue read command */
+	chip->cmdfunc(mtd, NAND_CMD_READ0, 0, page);
+
+	/* Read data and oob, calculate ECC */
+	status = lpc32xx_xfer(mtd, buf, chip->ecc.steps, 1);
+
+	/* Get OOB data */
+	chip->read_buf(mtd, chip->oob_poi, mtd->oobsize);
+
+	/* Convert to stored ECC format */
+	lpc32xx_slc_ecc_copy(tmpecc, (uint32_t *) host->ecc_buf, chip->ecc.steps);
+
+	/* Pointer to ECC data retrieved from NAND spare area */
+	oobecc = chip->oob_poi + chip->ecc.layout->eccpos[0];
+
+	for (i = 0; i < chip->ecc.steps; i++) {
+		stat = chip->ecc.correct(mtd, buf, oobecc,
+					 &tmpecc[i * chip->ecc.bytes]);
+		if (stat < 0)
+			mtd->ecc_stats.failed++;
+		else
+			mtd->ecc_stats.corrected += stat;
+
+		buf += chip->ecc.size;
+		oobecc += chip->ecc.bytes;
+	}
+
+	return status;
+}
+
+/*
+ * Read the data and OOB data from the device, no ECC correction with the
+ * data or OOB data
+ */
+static int lpc32xx_nand_read_page_raw_syndrome(struct mtd_info *mtd,
+					       struct nand_chip *chip,
+					       uint8_t *buf, int oob_required,
+					       int page)
+{
+	/* Issue read command */
+	chip->cmdfunc(mtd, NAND_CMD_READ0, 0, page);
+
+	/* Raw reads can just use the FIFO interface */
+	chip->read_buf(mtd, buf, chip->ecc.size * chip->ecc.steps);
+	chip->read_buf(mtd, chip->oob_poi, mtd->oobsize);
+
+	return 0;
+}
+
+/*
+ * Write the data and OOB data to the device, use ECC with the data,
+ * disable ECC for the OOB data
+ */
+static int lpc32xx_nand_write_page_syndrome(struct mtd_info *mtd,
+					    struct nand_chip *chip,
+					    const uint8_t *buf, int oob_required)
+{
+	struct lpc32xx_nand_host *host = chip->priv;
+	uint8_t *pb = chip->oob_poi + chip->ecc.layout->eccpos[0];
+	int error;
+
+	/* Write data, calculate ECC on outbound data */
+	error = lpc32xx_xfer(mtd, (uint8_t *)buf, chip->ecc.steps, 0);
+	if (error)
+		return error;
+
+	/*
+	 * The calculated ECC needs some manual work done to it before
+	 * committing it to NAND. Process the calculated ECC and place
+	 * the resultant values directly into the OOB buffer. */
+	lpc32xx_slc_ecc_copy(pb, (uint32_t *)host->ecc_buf, chip->ecc.steps);
+
+	/* Write ECC data to device */
+	chip->write_buf(mtd, chip->oob_poi, mtd->oobsize);
+	return 0;
+}
+
+/*
+ * Write the data and OOB data to the device, no ECC correction with the
+ * data or OOB data
+ */
+static int lpc32xx_nand_write_page_raw_syndrome(struct mtd_info *mtd,
+						struct nand_chip *chip,
+						const uint8_t *buf,
+						int oob_required)
+{
+	/* Raw writes can just use the FIFO interface */
+	chip->write_buf(mtd, buf, chip->ecc.size * chip->ecc.steps);
+	chip->write_buf(mtd, chip->oob_poi, mtd->oobsize);
+	return 0;
+}
+
+static int lpc32xx_nand_dma_setup(struct lpc32xx_nand_host *host)
+{
+	struct mtd_info *mtd = &host->mtd;
+	dma_cap_mask_t mask;
+
+	if (!host->pdata || !host->pdata->dma_filter) {
+		dev_err(mtd->dev.parent, "no DMA platform data\n");
+		return -ENOENT;
+	}
+
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+	host->dma_chan = dma_request_channel(mask, host->pdata->dma_filter,
+					     "nand-slc");
+	if (!host->dma_chan) {
+		dev_err(mtd->dev.parent, "Failed to request DMA channel\n");
+		return -EBUSY;
+	}
+
+	return 0;
+}
+
+static struct lpc32xx_nand_cfg_slc *lpc32xx_parse_dt(struct device *dev)
+{
+	struct lpc32xx_nand_cfg_slc *ncfg;
+	struct device_node *np = dev->of_node;
+
+	ncfg = devm_kzalloc(dev, sizeof(*ncfg), GFP_KERNEL);
+	if (!ncfg) {
+		dev_err(dev, "could not allocate memory for NAND config\n");
+		return NULL;
+	}
+
+	of_property_read_u32(np, "nxp,wdr-clks", &ncfg->wdr_clks);
+	of_property_read_u32(np, "nxp,wwidth", &ncfg->wwidth);
+	of_property_read_u32(np, "nxp,whold", &ncfg->whold);
+	of_property_read_u32(np, "nxp,wsetup", &ncfg->wsetup);
+	of_property_read_u32(np, "nxp,rdr-clks", &ncfg->rdr_clks);
+	of_property_read_u32(np, "nxp,rwidth", &ncfg->rwidth);
+	of_property_read_u32(np, "nxp,rhold", &ncfg->rhold);
+	of_property_read_u32(np, "nxp,rsetup", &ncfg->rsetup);
+
+	if (!ncfg->wdr_clks || !ncfg->wwidth || !ncfg->whold ||
+	    !ncfg->wsetup || !ncfg->rdr_clks || !ncfg->rwidth ||
+	    !ncfg->rhold || !ncfg->rsetup) {
+		dev_err(dev, "chip parameters not specified correctly\n");
+		return NULL;
+	}
+
+	ncfg->use_bbt = of_get_nand_on_flash_bbt(np);
+	ncfg->wp_gpio = of_get_named_gpio(np, "gpios", 0);
+
+	return ncfg;
+}
+
+/*
+ * Probe for NAND controller
+ */
+static int __devinit lpc32xx_nand_probe(struct platform_device *pdev)
+{
+	struct lpc32xx_nand_host *host;
+	struct mtd_info *mtd;
+	struct nand_chip *chip;
+	struct resource *rc;
+	struct mtd_part_parser_data ppdata = {};
+	int res;
+
+	rc = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (rc == NULL) {
+		dev_err(&pdev->dev, "No memory resource found for device\n");
+		return -EBUSY;
+	}
+
+	/* Allocate memory for the device structure (and zero it) */
+	host = devm_kzalloc(&pdev->dev, sizeof(*host), GFP_KERNEL);
+	if (!host) {
+		dev_err(&pdev->dev, "failed to allocate device structure\n");
+		return -ENOMEM;
+	}
+	host->io_base_dma = rc->start;
+
+	host->io_base = devm_request_and_ioremap(&pdev->dev, rc);
+	if (host->io_base == NULL) {
+		dev_err(&pdev->dev, "ioremap failed\n");
+		return -ENOMEM;
+	}
+
+	if (pdev->dev.of_node)
+		host->ncfg = lpc32xx_parse_dt(&pdev->dev);
+	if (!host->ncfg) {
+		dev_err(&pdev->dev,
+			"Missing or bad NAND config from device tree\n");
+		return -ENOENT;
+	}
+	if (host->ncfg->wp_gpio == -EPROBE_DEFER)
+		return -EPROBE_DEFER;
+	if (gpio_is_valid(host->ncfg->wp_gpio) &&
+			gpio_request(host->ncfg->wp_gpio, "NAND WP")) {
+		dev_err(&pdev->dev, "GPIO not available\n");
+		return -EBUSY;
+	}
+	lpc32xx_wp_disable(host);
+
+	host->pdata = pdev->dev.platform_data;
+
+	mtd = &host->mtd;
+	chip = &host->nand_chip;
+	chip->priv = host;
+	mtd->priv = chip;
+	mtd->owner = THIS_MODULE;
+	mtd->dev.parent = &pdev->dev;
+
+	/* Get NAND clock */
+	host->clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(host->clk)) {
+		dev_err(&pdev->dev, "Clock failure\n");
+		res = -ENOENT;
+		goto err_exit1;
+	}
+	clk_enable(host->clk);
+
+	/* Set NAND IO addresses and command/ready functions */
+	chip->IO_ADDR_R = SLC_DATA(host->io_base);
+	chip->IO_ADDR_W = SLC_DATA(host->io_base);
+	chip->cmd_ctrl = lpc32xx_nand_cmd_ctrl;
+	chip->dev_ready = lpc32xx_nand_device_ready;
+	chip->chip_delay = 20; /* 20us command delay time */
+
+	/* Init NAND controller */
+	lpc32xx_nand_setup(host);
+
+	platform_set_drvdata(pdev, host);
+
+	/* NAND callbacks for LPC32xx SLC hardware */
+	chip->ecc.mode = NAND_ECC_HW_SYNDROME;
+	chip->read_byte = lpc32xx_nand_read_byte;
+	chip->read_buf = lpc32xx_nand_read_buf;
+	chip->write_buf = lpc32xx_nand_write_buf;
+	chip->ecc.read_page_raw = lpc32xx_nand_read_page_raw_syndrome;
+	chip->ecc.read_page = lpc32xx_nand_read_page_syndrome;
+	chip->ecc.write_page_raw = lpc32xx_nand_write_page_raw_syndrome;
+	chip->ecc.write_page = lpc32xx_nand_write_page_syndrome;
+	chip->ecc.write_oob = lpc32xx_nand_write_oob_syndrome;
+	chip->ecc.read_oob = lpc32xx_nand_read_oob_syndrome;
+	chip->ecc.calculate = lpc32xx_nand_ecc_calculate;
+	chip->ecc.correct = nand_correct_data;
+	chip->ecc.strength = 1;
+	chip->ecc.hwctl = lpc32xx_nand_ecc_enable;
+
+	/* bitflip_threshold's default is defined as ecc_strength anyway.
+	 * Unfortunately, it is set only later at add_mtd_device(). Meanwhile
+	 * being 0, it causes bad block table scanning errors in
+	 * nand_scan_tail(), so preparing it here already. */
+	mtd->bitflip_threshold = chip->ecc.strength;
+
+	/*
+	 * Allocate a large enough buffer for a single huge page plus
+	 * extra space for the spare area and ECC storage area
+	 */
+	host->dma_buf_len = LPC32XX_DMA_DATA_SIZE + LPC32XX_ECC_SAVE_SIZE;
+	host->data_buf = devm_kzalloc(&pdev->dev, host->dma_buf_len,
+				      GFP_KERNEL);
+	if (host->data_buf == NULL) {
+		dev_err(&pdev->dev, "Error allocating memory\n");
+		res = -ENOMEM;
+		goto err_exit2;
+	}
+
+	res = lpc32xx_nand_dma_setup(host);
+	if (res) {
+		res = -EIO;
+		goto err_exit2;
+	}
+
+	/* Find NAND device */
+	if (nand_scan_ident(mtd, 1, NULL)) {
+		res = -ENXIO;
+		goto err_exit3;
+	}
+
+	/* OOB and ECC CPU and DMA work areas */
+	host->ecc_buf = (uint32_t *)(host->data_buf + LPC32XX_DMA_DATA_SIZE);
+
+	/*
+	 * Small page FLASH has a unique OOB layout, but large and huge
+	 * page FLASH use the standard layout. Small page FLASH uses a
+	 * custom BBT marker layout.
+	 */
+	if (mtd->writesize <= 512)
+		chip->ecc.layout = &lpc32xx_nand_oob_16;
+
+	/* These sizes remain the same regardless of page size */
+	chip->ecc.size = 256;
+	chip->ecc.bytes = LPC32XX_SLC_DEV_ECC_BYTES;
+	chip->ecc.prepad = chip->ecc.postpad = 0;
+
+	/* Avoid extra scan if using BBT, setup BBT support */
+	if (host->ncfg->use_bbt) {
+		chip->options |= NAND_SKIP_BBTSCAN;
+		chip->bbt_options |= NAND_BBT_USE_FLASH;
+
+		/*
+		 * Use a custom BBT marker setup for small page FLASH that
+		 * won't interfere with the ECC layout. Large and huge page
+		 * FLASH use the standard layout.
+		 */
+		if (mtd->writesize <= 512) {
+			chip->bbt_td = &bbt_smallpage_main_descr;
+			chip->bbt_md = &bbt_smallpage_mirror_descr;
+		}
+	}
+
+	/*
+	 * Fills out all the uninitialized function pointers with the defaults
+	 */
+	if (nand_scan_tail(mtd)) {
+		res = -ENXIO;
+		goto err_exit3;
+	}
+
+	/* Standard layout in FLASH for bad block tables */
+	if (host->ncfg->use_bbt) {
+		if (nand_default_bbt(mtd) < 0)
+			dev_err(&pdev->dev,
+			       "Error initializing default bad block tables\n");
+	}
+
+	mtd->name = "nxp_lpc3220_slc";
+	ppdata.of_node = pdev->dev.of_node;
+	res = mtd_device_parse_register(mtd, NULL, &ppdata, host->ncfg->parts,
+					host->ncfg->num_parts);
+	if (!res)
+		return res;
+
+	nand_release(mtd);
+
+err_exit3:
+	dma_release_channel(host->dma_chan);
+err_exit2:
+	clk_disable(host->clk);
+	clk_put(host->clk);
+	platform_set_drvdata(pdev, NULL);
+err_exit1:
+	lpc32xx_wp_enable(host);
+	gpio_free(host->ncfg->wp_gpio);
+
+	return res;
+}
+
+/*
+ * Remove NAND device.
+ */
+static int __devexit lpc32xx_nand_remove(struct platform_device *pdev)
+{
+	uint32_t tmp;
+	struct lpc32xx_nand_host *host = platform_get_drvdata(pdev);
+	struct mtd_info *mtd = &host->mtd;
+
+	nand_release(mtd);
+	dma_release_channel(host->dma_chan);
+
+	/* Force CE high */
+	tmp = readl(SLC_CTRL(host->io_base));
+	tmp &= ~SLCCFG_CE_LOW;
+	writel(tmp, SLC_CTRL(host->io_base));
+
+	clk_disable(host->clk);
+	clk_put(host->clk);
+	platform_set_drvdata(pdev, NULL);
+	lpc32xx_wp_enable(host);
+	gpio_free(host->ncfg->wp_gpio);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int lpc32xx_nand_resume(struct platform_device *pdev)
+{
+	struct lpc32xx_nand_host *host = platform_get_drvdata(pdev);
+
+	/* Re-enable NAND clock */
+	clk_enable(host->clk);
+
+	/* Fresh init of NAND controller */
+	lpc32xx_nand_setup(host);
+
+	/* Disable write protect */
+	lpc32xx_wp_disable(host);
+
+	return 0;
+}
+
+static int lpc32xx_nand_suspend(struct platform_device *pdev, pm_message_t pm)
+{
+	uint32_t tmp;
+	struct lpc32xx_nand_host *host = platform_get_drvdata(pdev);
+
+	/* Force CE high */
+	tmp = readl(SLC_CTRL(host->io_base));
+	tmp &= ~SLCCFG_CE_LOW;
+	writel(tmp, SLC_CTRL(host->io_base));
+
+	/* Enable write protect for safety */
+	lpc32xx_wp_enable(host);
+
+	/* Disable clock */
+	clk_disable(host->clk);
+
+	return 0;
+}
+
+#else
+#define lpc32xx_nand_resume NULL
+#define lpc32xx_nand_suspend NULL
+#endif
+
+static const struct of_device_id lpc32xx_nand_match[] = {
+	{ .compatible = "nxp,lpc3220-slc" },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, lpc32xx_nand_match);
+
+static struct platform_driver lpc32xx_nand_driver = {
+	.probe		= lpc32xx_nand_probe,
+	.remove		= __devexit_p(lpc32xx_nand_remove),
+	.resume		= lpc32xx_nand_resume,
+	.suspend	= lpc32xx_nand_suspend,
+	.driver		= {
+		.name	= LPC32XX_MODNAME,
+		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(lpc32xx_nand_match),
+	},
+};
+
+module_platform_driver(lpc32xx_nand_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Kevin Wells <kevin.wells@nxp.com>");
+MODULE_AUTHOR("Roland Stigge <stigge@antcom.de>");
+MODULE_DESCRIPTION("NAND driver for the NXP LPC32XX SLC controller");
diff --git a/drivers/mtd/nand/mpc5121_nfc.c b/drivers/mtd/nand/mpc5121_nfc.c
index c259c24d798..f776c8577b8 100644
--- a/drivers/mtd/nand/mpc5121_nfc.c
+++ b/drivers/mtd/nand/mpc5121_nfc.c
@@ -506,27 +506,6 @@ static void mpc5121_nfc_write_buf(struct mtd_info *mtd,
 	mpc5121_nfc_buf_copy(mtd, (u_char *)buf, len, 1);
 }
 
-/* Compare buffer with NAND flash */
-static int mpc5121_nfc_verify_buf(struct mtd_info *mtd,
-						const u_char *buf, int len)
-{
-	u_char tmp[256];
-	uint bsize;
-
-	while (len) {
-		bsize = min(len, 256);
-		mpc5121_nfc_read_buf(mtd, tmp, bsize);
-
-		if (memcmp(buf, tmp, bsize))
-			return 1;
-
-		buf += bsize;
-		len -= bsize;
-	}
-
-	return 0;
-}
-
 /* Read byte from NFC buffers */
 static u8 mpc5121_nfc_read_byte(struct mtd_info *mtd)
 {
@@ -732,7 +711,6 @@ static int __devinit mpc5121_nfc_probe(struct platform_device *op)
 	chip->read_word = mpc5121_nfc_read_word;
 	chip->read_buf = mpc5121_nfc_read_buf;
 	chip->write_buf = mpc5121_nfc_write_buf;
-	chip->verify_buf = mpc5121_nfc_verify_buf;
 	chip->select_chip = mpc5121_nfc_select_chip;
 	chip->bbt_options = NAND_BBT_USE_FLASH;
 	chip->ecc.mode = NAND_ECC_SOFT;
diff --git a/drivers/mtd/nand/mxc_nand.c b/drivers/mtd/nand/mxc_nand.c
index 5683604967d..72e31d86030 100644
--- a/drivers/mtd/nand/mxc_nand.c
+++ b/drivers/mtd/nand/mxc_nand.c
@@ -43,8 +43,8 @@
 
 #define nfc_is_v21()		(cpu_is_mx25() || cpu_is_mx35())
 #define nfc_is_v1()		(cpu_is_mx31() || cpu_is_mx27() || cpu_is_mx21())
-#define nfc_is_v3_2()		(cpu_is_mx51() || cpu_is_mx53())
-#define nfc_is_v3()		nfc_is_v3_2()
+#define nfc_is_v3_2a()		cpu_is_mx51()
+#define nfc_is_v3_2b()		cpu_is_mx53()
 
 /* Addresses for NFC registers */
 #define NFC_V1_V2_BUF_SIZE		(host->regs + 0x00)
@@ -122,7 +122,7 @@
 #define NFC_V3_CONFIG2_2CMD_PHASES		(1 << 4)
 #define NFC_V3_CONFIG2_NUM_ADDR_PHASE0		(1 << 5)
 #define NFC_V3_CONFIG2_ECC_MODE_8		(1 << 6)
-#define NFC_V3_CONFIG2_PPB(x)			(((x) & 0x3) << 7)
+#define NFC_V3_CONFIG2_PPB(x, shift)		(((x) & 0x3) << shift)
 #define NFC_V3_CONFIG2_NUM_ADDR_PHASE1(x)	(((x) & 0x3) << 12)
 #define NFC_V3_CONFIG2_INT_MSK			(1 << 15)
 #define NFC_V3_CONFIG2_ST_CMD(x)		(((x) & 0xff) << 24)
@@ -174,6 +174,7 @@ struct mxc_nand_devtype_data {
 	int spare_len;
 	int eccbytes;
 	int eccsize;
+	int ppb_shift;
 };
 
 struct mxc_nand_host {
@@ -745,14 +746,6 @@ static void mxc_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 	host->buf_start += n;
 }
 
-/* Used by the upper layer to verify the data in NAND Flash
- * with the data in the buf. */
-static int mxc_nand_verify_buf(struct mtd_info *mtd,
-				const u_char *buf, int len)
-{
-	return -EFAULT;
-}
-
 /* This function is used by upper layer for select and
  * deselect of the NAND chip */
 static void mxc_nand_select_chip_v1_v3(struct mtd_info *mtd, int chip)
@@ -784,7 +777,7 @@ static void mxc_nand_select_chip_v2(struct mtd_info *mtd, int chip)
 	if (chip == -1) {
 		/* Disable the NFC clock */
 		if (host->clk_act) {
-			clk_disable(host->clk);
+			clk_disable_unprepare(host->clk);
 			host->clk_act = 0;
 		}
 		return;
@@ -792,7 +785,7 @@ static void mxc_nand_select_chip_v2(struct mtd_info *mtd, int chip)
 
 	if (!host->clk_act) {
 		/* Enable the NFC clock */
-		clk_enable(host->clk);
+		clk_prepare_enable(host->clk);
 		host->clk_act = 1;
 	}
 
@@ -1021,7 +1014,9 @@ static void preset_v3(struct mtd_info *mtd)
 	}
 
 	if (mtd->writesize) {
-		config2 |= NFC_V3_CONFIG2_PPB(ffs(mtd->erasesize / mtd->writesize) - 6);
+		config2 |= NFC_V3_CONFIG2_PPB(
+				ffs(mtd->erasesize / mtd->writesize) - 6,
+				host->devtype_data->ppb_shift);
 		host->eccsize = get_eccsize(mtd);
 		if (host->eccsize == 8)
 			config2 |= NFC_V3_CONFIG2_ECC_MODE_8;
@@ -1234,7 +1229,7 @@ static const struct mxc_nand_devtype_data imx25_nand_devtype_data = {
 	.eccsize = 0,
 };
 
-/* v3: i.MX51, i.MX53 */
+/* v3.2a: i.MX51 */
 static const struct mxc_nand_devtype_data imx51_nand_devtype_data = {
 	.preset = preset_v3,
 	.send_cmd = send_cmd_v3,
@@ -1258,6 +1253,34 @@ static const struct mxc_nand_devtype_data imx51_nand_devtype_data = {
 	.spare_len = 64,
 	.eccbytes = 0,
 	.eccsize = 0,
+	.ppb_shift = 7,
+};
+
+/* v3.2b: i.MX53 */
+static const struct mxc_nand_devtype_data imx53_nand_devtype_data = {
+	.preset = preset_v3,
+	.send_cmd = send_cmd_v3,
+	.send_addr = send_addr_v3,
+	.send_page = send_page_v3,
+	.send_read_id = send_read_id_v3,
+	.get_dev_status = get_dev_status_v3,
+	.check_int = check_int_v3,
+	.irq_control = irq_control_v3,
+	.get_ecc_status = get_ecc_status_v3,
+	.ecclayout_512 = &nandv2_hw_eccoob_smallpage,
+	.ecclayout_2k = &nandv2_hw_eccoob_largepage,
+	.ecclayout_4k = &nandv2_hw_eccoob_smallpage, /* XXX: needs fix */
+	.select_chip = mxc_nand_select_chip_v1_v3,
+	.correct_data = mxc_nand_correct_data_v2_v3,
+	.irqpending_quirk = 0,
+	.needs_ip = 1,
+	.regs_offset = 0,
+	.spare0_offset = 0x1000,
+	.axi_offset = 0x1e00,
+	.spare_len = 64,
+	.eccbytes = 0,
+	.eccsize = 0,
+	.ppb_shift = 8,
 };
 
 #ifdef CONFIG_OF_MTD
@@ -1274,6 +1297,9 @@ static const struct of_device_id mxcnd_dt_ids[] = {
 	}, {
 		.compatible = "fsl,imx51-nand",
 		.data = &imx51_nand_devtype_data,
+	}, {
+		.compatible = "fsl,imx53-nand",
+		.data = &imx53_nand_devtype_data,
 	},
 	{ /* sentinel */ }
 };
@@ -1327,15 +1353,17 @@ static int __init mxcnd_probe_pdata(struct mxc_nand_host *host)
 			host->devtype_data = &imx27_nand_devtype_data;
 	} else if (nfc_is_v21()) {
 		host->devtype_data = &imx25_nand_devtype_data;
-	} else if (nfc_is_v3_2()) {
+	} else if (nfc_is_v3_2a()) {
 		host->devtype_data = &imx51_nand_devtype_data;
+	} else if (nfc_is_v3_2b()) {
+		host->devtype_data = &imx53_nand_devtype_data;
 	} else
 		BUG();
 
 	return 0;
 }
 
-static int __init mxcnd_probe(struct platform_device *pdev)
+static int __devinit mxcnd_probe(struct platform_device *pdev)
 {
 	struct nand_chip *this;
 	struct mtd_info *mtd;
@@ -1344,8 +1372,8 @@ static int __init mxcnd_probe(struct platform_device *pdev)
 	int err = 0;
 
 	/* Allocate memory for MTD device structure and private data */
-	host = kzalloc(sizeof(struct mxc_nand_host) + NAND_MAX_PAGESIZE +
-			NAND_MAX_OOBSIZE, GFP_KERNEL);
+	host = devm_kzalloc(&pdev->dev, sizeof(struct mxc_nand_host) +
+			NAND_MAX_PAGESIZE + NAND_MAX_OOBSIZE, GFP_KERNEL);
 	if (!host)
 		return -ENOMEM;
 
@@ -1370,36 +1398,38 @@ static int __init mxcnd_probe(struct platform_device *pdev)
 	this->read_word = mxc_nand_read_word;
 	this->write_buf = mxc_nand_write_buf;
 	this->read_buf = mxc_nand_read_buf;
-	this->verify_buf = mxc_nand_verify_buf;
 
-	host->clk = clk_get(&pdev->dev, "nfc");
-	if (IS_ERR(host->clk)) {
-		err = PTR_ERR(host->clk);
-		goto eclk;
-	}
+	host->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(host->clk))
+		return PTR_ERR(host->clk);
 
-	clk_prepare_enable(host->clk);
-	host->clk_act = 1;
+	err = mxcnd_probe_dt(host);
+	if (err > 0)
+		err = mxcnd_probe_pdata(host);
+	if (err < 0)
+		return err;
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		err = -ENODEV;
-		goto eres;
-	}
+	if (host->devtype_data->needs_ip) {
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+		if (!res)
+			return -ENODEV;
+		host->regs_ip = devm_request_and_ioremap(&pdev->dev, res);
+		if (!host->regs_ip)
+			return -ENOMEM;
 
-	host->base = ioremap(res->start, resource_size(res));
-	if (!host->base) {
-		err = -ENOMEM;
-		goto eres;
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	} else {
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	}
 
-	host->main_area0 = host->base;
+	if (!res)
+		return -ENODEV;
 
-	err = mxcnd_probe_dt(host);
-	if (err > 0)
-		err = mxcnd_probe_pdata(host);
-	if (err < 0)
-		goto eirq;
+	host->base = devm_request_and_ioremap(&pdev->dev, res);
+	if (!host->base)
+		return -ENOMEM;
+
+	host->main_area0 = host->base;
 
 	if (host->devtype_data->regs_offset)
 		host->regs = host->base + host->devtype_data->regs_offset;
@@ -1414,19 +1444,6 @@ static int __init mxcnd_probe(struct platform_device *pdev)
 	this->ecc.size = 512;
 	this->ecc.layout = host->devtype_data->ecclayout_512;
 
-	if (host->devtype_data->needs_ip) {
-		res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-		if (!res) {
-			err = -ENODEV;
-			goto eirq;
-		}
-		host->regs_ip = ioremap(res->start, resource_size(res));
-		if (!host->regs_ip) {
-			err = -ENOMEM;
-			goto eirq;
-		}
-	}
-
 	if (host->pdata.hw_ecc) {
 		this->ecc.calculate = mxc_nand_calculate_ecc;
 		this->ecc.hwctl = mxc_nand_enable_hwecc;
@@ -1458,9 +1475,13 @@ static int __init mxcnd_probe(struct platform_device *pdev)
 	 */
 	host->devtype_data->irq_control(host, 0);
 
-	err = request_irq(host->irq, mxc_nfc_irq, IRQF_DISABLED, DRIVER_NAME, host);
+	err = devm_request_irq(&pdev->dev, host->irq, mxc_nfc_irq,
+			IRQF_DISABLED, DRIVER_NAME, host);
 	if (err)
-		goto eirq;
+		return err;
+
+	clk_prepare_enable(host->clk);
+	host->clk_act = 1;
 
 	/*
 	 * Now that we "own" the interrupt make sure the interrupt mask bit is
@@ -1512,15 +1533,7 @@ static int __init mxcnd_probe(struct platform_device *pdev)
 	return 0;
 
 escan:
-	free_irq(host->irq, host);
-eirq:
-	if (host->regs_ip)
-		iounmap(host->regs_ip);
-	iounmap(host->base);
-eres:
-	clk_put(host->clk);
-eclk:
-	kfree(host);
+	clk_disable_unprepare(host->clk);
 
 	return err;
 }
@@ -1529,16 +1542,9 @@ static int __devexit mxcnd_remove(struct platform_device *pdev)
 {
 	struct mxc_nand_host *host = platform_get_drvdata(pdev);
 
-	clk_put(host->clk);
-
 	platform_set_drvdata(pdev, NULL);
 
 	nand_release(&host->mtd);
-	free_irq(host->irq, host);
-	if (host->regs_ip)
-		iounmap(host->regs_ip);
-	iounmap(host->base);
-	kfree(host);
 
 	return 0;
 }
@@ -1549,22 +1555,10 @@ static struct platform_driver mxcnd_driver = {
 		   .owner = THIS_MODULE,
 		   .of_match_table = of_match_ptr(mxcnd_dt_ids),
 	},
+	.probe = mxcnd_probe,
 	.remove = __devexit_p(mxcnd_remove),
 };
-
-static int __init mxc_nd_init(void)
-{
-	return platform_driver_probe(&mxcnd_driver, mxcnd_probe);
-}
-
-static void __exit mxc_nd_cleanup(void)
-{
-	/* Unregister the device structure */
-	platform_driver_unregister(&mxcnd_driver);
-}
-
-module_init(mxc_nd_init);
-module_exit(mxc_nd_cleanup);
+module_platform_driver(mxcnd_driver);
 
 MODULE_AUTHOR("Freescale Semiconductor, Inc.");
 MODULE_DESCRIPTION("MXC NAND MTD driver");
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index a11253a0fca..ec6841d8e95 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -243,25 +243,6 @@ static void nand_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 }
 
 /**
- * nand_verify_buf - [DEFAULT] Verify chip data against buffer
- * @mtd: MTD device structure
- * @buf: buffer containing the data to compare
- * @len: number of bytes to compare
- *
- * Default verify function for 8bit buswidth.
- */
-static int nand_verify_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
-{
-	int i;
-	struct nand_chip *chip = mtd->priv;
-
-	for (i = 0; i < len; i++)
-		if (buf[i] != readb(chip->IO_ADDR_R))
-			return -EFAULT;
-	return 0;
-}
-
-/**
  * nand_write_buf16 - [DEFAULT] write buffer to chip
  * @mtd: MTD device structure
  * @buf: data buffer
@@ -301,28 +282,6 @@ static void nand_read_buf16(struct mtd_info *mtd, uint8_t *buf, int len)
 }
 
 /**
- * nand_verify_buf16 - [DEFAULT] Verify chip data against buffer
- * @mtd: MTD device structure
- * @buf: buffer containing the data to compare
- * @len: number of bytes to compare
- *
- * Default verify function for 16bit buswidth.
- */
-static int nand_verify_buf16(struct mtd_info *mtd, const uint8_t *buf, int len)
-{
-	int i;
-	struct nand_chip *chip = mtd->priv;
-	u16 *p = (u16 *) buf;
-	len >>= 1;
-
-	for (i = 0; i < len; i++)
-		if (p[i] != readw(chip->IO_ADDR_R))
-			return -EFAULT;
-
-	return 0;
-}
-
-/**
  * nand_block_bad - [DEFAULT] Read bad block marker from the chip
  * @mtd: MTD device structure
  * @ofs: offset from device start
@@ -1525,7 +1484,8 @@ static int nand_do_read_ops(struct mtd_info *mtd, loff_t from,
 				ret = chip->ecc.read_page_raw(mtd, chip, bufpoi,
 							      oob_required,
 							      page);
-			else if (!aligned && NAND_SUBPAGE_READ(chip) && !oob)
+			else if (!aligned && NAND_HAS_SUBPAGE_READ(chip) &&
+				 !oob)
 				ret = chip->ecc.read_subpage(mtd, chip,
 							col, bytes, bufpoi);
 			else
@@ -1542,7 +1502,7 @@ static int nand_do_read_ops(struct mtd_info *mtd, loff_t from,
 
 			/* Transfer not aligned data */
 			if (!aligned) {
-				if (!NAND_SUBPAGE_READ(chip) && !oob &&
+				if (!NAND_HAS_SUBPAGE_READ(chip) && !oob &&
 				    !(mtd->ecc_stats.failed - stats.failed) &&
 				    (ops->mode != MTD_OPS_RAW)) {
 					chip->pagebuf = realpage;
@@ -1565,14 +1525,6 @@ static int nand_do_read_ops(struct mtd_info *mtd, loff_t from,
 					oobreadlen -= toread;
 				}
 			}
-
-			if (!(chip->options & NAND_NO_READRDY)) {
-				/* Apply delay or wait for ready/busy pin */
-				if (!chip->dev_ready)
-					udelay(chip->chip_delay);
-				else
-					nand_wait_ready(mtd);
-			}
 		} else {
 			memcpy(buf, chip->buffers->databuf + col, bytes);
 			buf += bytes;
@@ -1633,7 +1585,7 @@ static int nand_read(struct mtd_info *mtd, loff_t from, size_t len,
 	ops.len = len;
 	ops.datbuf = buf;
 	ops.oobbuf = NULL;
-	ops.mode = 0;
+	ops.mode = MTD_OPS_PLACE_OOB;
 	ret = nand_do_read_ops(mtd, from, &ops);
 	*retlen = ops.retlen;
 	nand_release_device(mtd);
@@ -1837,14 +1789,6 @@ static int nand_do_read_oob(struct mtd_info *mtd, loff_t from,
 		len = min(len, readlen);
 		buf = nand_transfer_oob(chip, buf, ops, len);
 
-		if (!(chip->options & NAND_NO_READRDY)) {
-			/* Apply delay or wait for ready/busy pin */
-			if (!chip->dev_ready)
-				udelay(chip->chip_delay);
-			else
-				nand_wait_ready(mtd);
-		}
-
 		readlen -= len;
 		if (!readlen)
 			break;
@@ -1927,12 +1871,14 @@ out:
  *
  * Not for syndrome calculating ECC controllers, which use a special oob layout.
  */
-static void nand_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
+static int nand_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
 				const uint8_t *buf, int oob_required)
 {
 	chip->write_buf(mtd, buf, mtd->writesize);
 	if (oob_required)
 		chip->write_buf(mtd, chip->oob_poi, mtd->oobsize);
+
+	return 0;
 }
 
 /**
@@ -1944,7 +1890,7 @@ static void nand_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
  *
  * We need a special oob layout and handling even when ECC isn't checked.
  */
-static void nand_write_page_raw_syndrome(struct mtd_info *mtd,
+static int nand_write_page_raw_syndrome(struct mtd_info *mtd,
 					struct nand_chip *chip,
 					const uint8_t *buf, int oob_required)
 {
@@ -1974,6 +1920,8 @@ static void nand_write_page_raw_syndrome(struct mtd_info *mtd,
 	size = mtd->oobsize - (oob - chip->oob_poi);
 	if (size)
 		chip->write_buf(mtd, oob, size);
+
+	return 0;
 }
 /**
  * nand_write_page_swecc - [REPLACEABLE] software ECC based page write function
@@ -1982,7 +1930,7 @@ static void nand_write_page_raw_syndrome(struct mtd_info *mtd,
  * @buf: data buffer
  * @oob_required: must write chip->oob_poi to OOB
  */
-static void nand_write_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
+static int nand_write_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
 				  const uint8_t *buf, int oob_required)
 {
 	int i, eccsize = chip->ecc.size;
@@ -1999,7 +1947,7 @@ static void nand_write_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
 	for (i = 0; i < chip->ecc.total; i++)
 		chip->oob_poi[eccpos[i]] = ecc_calc[i];
 
-	chip->ecc.write_page_raw(mtd, chip, buf, 1);
+	return chip->ecc.write_page_raw(mtd, chip, buf, 1);
 }
 
 /**
@@ -2009,7 +1957,7 @@ static void nand_write_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
  * @buf: data buffer
  * @oob_required: must write chip->oob_poi to OOB
  */
-static void nand_write_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
+static int nand_write_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
 				  const uint8_t *buf, int oob_required)
 {
 	int i, eccsize = chip->ecc.size;
@@ -2029,6 +1977,8 @@ static void nand_write_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
 		chip->oob_poi[eccpos[i]] = ecc_calc[i];
 
 	chip->write_buf(mtd, chip->oob_poi, mtd->oobsize);
+
+	return 0;
 }
 
 /**
@@ -2041,7 +1991,7 @@ static void nand_write_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
  * The hw generator calculates the error syndrome automatically. Therefore we
  * need a special oob layout and handling.
  */
-static void nand_write_page_syndrome(struct mtd_info *mtd,
+static int nand_write_page_syndrome(struct mtd_info *mtd,
 				    struct nand_chip *chip,
 				    const uint8_t *buf, int oob_required)
 {
@@ -2075,6 +2025,8 @@ static void nand_write_page_syndrome(struct mtd_info *mtd,
 	i = mtd->oobsize - (oob - chip->oob_poi);
 	if (i)
 		chip->write_buf(mtd, oob, i);
+
+	return 0;
 }
 
 /**
@@ -2096,9 +2048,12 @@ static int nand_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 	chip->cmdfunc(mtd, NAND_CMD_SEQIN, 0x00, page);
 
 	if (unlikely(raw))
-		chip->ecc.write_page_raw(mtd, chip, buf, oob_required);
+		status = chip->ecc.write_page_raw(mtd, chip, buf, oob_required);
 	else
-		chip->ecc.write_page(mtd, chip, buf, oob_required);
+		status = chip->ecc.write_page(mtd, chip, buf, oob_required);
+
+	if (status < 0)
+		return status;
 
 	/*
 	 * Cached progamming disabled for now. Not sure if it's worth the
@@ -2125,16 +2080,6 @@ static int nand_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 		status = chip->waitfunc(mtd, chip);
 	}
 
-#ifdef CONFIG_MTD_NAND_VERIFY_WRITE
-	/* Send command to read back the data */
-	chip->cmdfunc(mtd, NAND_CMD_READ0, 0, page);
-
-	if (chip->verify_buf(mtd, buf, mtd->writesize))
-		return -EIO;
-
-	/* Make sure the next page prog is preceded by a status read */
-	chip->cmdfunc(mtd, NAND_CMD_STATUS, -1, -1);
-#endif
 	return 0;
 }
 
@@ -2336,7 +2281,7 @@ static int panic_nand_write(struct mtd_info *mtd, loff_t to, size_t len,
 	ops.len = len;
 	ops.datbuf = (uint8_t *)buf;
 	ops.oobbuf = NULL;
-	ops.mode = 0;
+	ops.mode = MTD_OPS_PLACE_OOB;
 
 	ret = nand_do_write_ops(mtd, to, &ops);
 
@@ -2365,7 +2310,7 @@ static int nand_write(struct mtd_info *mtd, loff_t to, size_t len,
 	ops.len = len;
 	ops.datbuf = (uint8_t *)buf;
 	ops.oobbuf = NULL;
-	ops.mode = 0;
+	ops.mode = MTD_OPS_PLACE_OOB;
 	ret = nand_do_write_ops(mtd, to, &ops);
 	*retlen = ops.retlen;
 	nand_release_device(mtd);
@@ -2755,6 +2700,50 @@ static int nand_block_markbad(struct mtd_info *mtd, loff_t ofs)
 }
 
 /**
+ * nand_onfi_set_features- [REPLACEABLE] set features for ONFI nand
+ * @mtd: MTD device structure
+ * @chip: nand chip info structure
+ * @addr: feature address.
+ * @subfeature_param: the subfeature parameters, a four bytes array.
+ */
+static int nand_onfi_set_features(struct mtd_info *mtd, struct nand_chip *chip,
+			int addr, uint8_t *subfeature_param)
+{
+	int status;
+
+	if (!chip->onfi_version)
+		return -EINVAL;
+
+	chip->cmdfunc(mtd, NAND_CMD_SET_FEATURES, addr, -1);
+	chip->write_buf(mtd, subfeature_param, ONFI_SUBFEATURE_PARAM_LEN);
+	status = chip->waitfunc(mtd, chip);
+	if (status & NAND_STATUS_FAIL)
+		return -EIO;
+	return 0;
+}
+
+/**
+ * nand_onfi_get_features- [REPLACEABLE] get features for ONFI nand
+ * @mtd: MTD device structure
+ * @chip: nand chip info structure
+ * @addr: feature address.
+ * @subfeature_param: the subfeature parameters, a four bytes array.
+ */
+static int nand_onfi_get_features(struct mtd_info *mtd, struct nand_chip *chip,
+			int addr, uint8_t *subfeature_param)
+{
+	if (!chip->onfi_version)
+		return -EINVAL;
+
+	/* clear the sub feature parameters */
+	memset(subfeature_param, 0, ONFI_SUBFEATURE_PARAM_LEN);
+
+	chip->cmdfunc(mtd, NAND_CMD_GET_FEATURES, addr, -1);
+	chip->read_buf(mtd, subfeature_param, ONFI_SUBFEATURE_PARAM_LEN);
+	return 0;
+}
+
+/**
  * nand_suspend - [MTD Interface] Suspend the NAND flash
  * @mtd: MTD device structure
  */
@@ -2809,8 +2798,6 @@ static void nand_set_defaults(struct nand_chip *chip, int busw)
 		chip->write_buf = busw ? nand_write_buf16 : nand_write_buf;
 	if (!chip->read_buf)
 		chip->read_buf = busw ? nand_read_buf16 : nand_read_buf;
-	if (!chip->verify_buf)
-		chip->verify_buf = busw ? nand_verify_buf16 : nand_verify_buf;
 	if (!chip->scan_bbt)
 		chip->scan_bbt = nand_default_bbt;
 
@@ -2914,14 +2901,250 @@ static int nand_flash_detect_onfi(struct mtd_info *mtd, struct nand_chip *chip,
 	if (le16_to_cpu(p->features) & 1)
 		*busw = NAND_BUSWIDTH_16;
 
-	chip->options &= ~NAND_CHIPOPTIONS_MSK;
-	chip->options |= NAND_NO_READRDY & NAND_CHIPOPTIONS_MSK;
-
 	pr_info("ONFI flash detected\n");
 	return 1;
 }
 
 /*
+ * nand_id_has_period - Check if an ID string has a given wraparound period
+ * @id_data: the ID string
+ * @arrlen: the length of the @id_data array
+ * @period: the period of repitition
+ *
+ * Check if an ID string is repeated within a given sequence of bytes at
+ * specific repetition interval period (e.g., {0x20,0x01,0x7F,0x20} has a
+ * period of 2). This is a helper function for nand_id_len(). Returns non-zero
+ * if the repetition has a period of @period; otherwise, returns zero.
+ */
+static int nand_id_has_period(u8 *id_data, int arrlen, int period)
+{
+	int i, j;
+	for (i = 0; i < period; i++)
+		for (j = i + period; j < arrlen; j += period)
+			if (id_data[i] != id_data[j])
+				return 0;
+	return 1;
+}
+
+/*
+ * nand_id_len - Get the length of an ID string returned by CMD_READID
+ * @id_data: the ID string
+ * @arrlen: the length of the @id_data array
+
+ * Returns the length of the ID string, according to known wraparound/trailing
+ * zero patterns. If no pattern exists, returns the length of the array.
+ */
+static int nand_id_len(u8 *id_data, int arrlen)
+{
+	int last_nonzero, period;
+
+	/* Find last non-zero byte */
+	for (last_nonzero = arrlen - 1; last_nonzero >= 0; last_nonzero--)
+		if (id_data[last_nonzero])
+			break;
+
+	/* All zeros */
+	if (last_nonzero < 0)
+		return 0;
+
+	/* Calculate wraparound period */
+	for (period = 1; period < arrlen; period++)
+		if (nand_id_has_period(id_data, arrlen, period))
+			break;
+
+	/* There's a repeated pattern */
+	if (period < arrlen)
+		return period;
+
+	/* There are trailing zeros */
+	if (last_nonzero < arrlen - 1)
+		return last_nonzero + 1;
+
+	/* No pattern detected */
+	return arrlen;
+}
+
+/*
+ * Many new NAND share similar device ID codes, which represent the size of the
+ * chip. The rest of the parameters must be decoded according to generic or
+ * manufacturer-specific "extended ID" decoding patterns.
+ */
+static void nand_decode_ext_id(struct mtd_info *mtd, struct nand_chip *chip,
+				u8 id_data[8], int *busw)
+{
+	int extid, id_len;
+	/* The 3rd id byte holds MLC / multichip data */
+	chip->cellinfo = id_data[2];
+	/* The 4th id byte is the important one */
+	extid = id_data[3];
+
+	id_len = nand_id_len(id_data, 8);
+
+	/*
+	 * Field definitions are in the following datasheets:
+	 * Old style (4,5 byte ID): Samsung K9GAG08U0M (p.32)
+	 * New style   (6 byte ID): Samsung K9GAG08U0F (p.44)
+	 * Hynix MLC   (6 byte ID): Hynix H27UBG8T2B (p.22)
+	 *
+	 * Check for ID length, cell type, and Hynix/Samsung ID to decide what
+	 * to do.
+	 */
+	if (id_len == 6 && id_data[0] == NAND_MFR_SAMSUNG) {
+		/* Calc pagesize */
+		mtd->writesize = 2048 << (extid & 0x03);
+		extid >>= 2;
+		/* Calc oobsize */
+		switch (((extid >> 2) & 0x04) | (extid & 0x03)) {
+		case 1:
+			mtd->oobsize = 128;
+			break;
+		case 2:
+			mtd->oobsize = 218;
+			break;
+		case 3:
+			mtd->oobsize = 400;
+			break;
+		case 4:
+			mtd->oobsize = 436;
+			break;
+		case 5:
+			mtd->oobsize = 512;
+			break;
+		case 6:
+		default: /* Other cases are "reserved" (unknown) */
+			mtd->oobsize = 640;
+			break;
+		}
+		extid >>= 2;
+		/* Calc blocksize */
+		mtd->erasesize = (128 * 1024) <<
+			(((extid >> 1) & 0x04) | (extid & 0x03));
+		*busw = 0;
+	} else if (id_len == 6 && id_data[0] == NAND_MFR_HYNIX &&
+			(chip->cellinfo & NAND_CI_CELLTYPE_MSK)) {
+		unsigned int tmp;
+
+		/* Calc pagesize */
+		mtd->writesize = 2048 << (extid & 0x03);
+		extid >>= 2;
+		/* Calc oobsize */
+		switch (((extid >> 2) & 0x04) | (extid & 0x03)) {
+		case 0:
+			mtd->oobsize = 128;
+			break;
+		case 1:
+			mtd->oobsize = 224;
+			break;
+		case 2:
+			mtd->oobsize = 448;
+			break;
+		case 3:
+			mtd->oobsize = 64;
+			break;
+		case 4:
+			mtd->oobsize = 32;
+			break;
+		case 5:
+			mtd->oobsize = 16;
+			break;
+		default:
+			mtd->oobsize = 640;
+			break;
+		}
+		extid >>= 2;
+		/* Calc blocksize */
+		tmp = ((extid >> 1) & 0x04) | (extid & 0x03);
+		if (tmp < 0x03)
+			mtd->erasesize = (128 * 1024) << tmp;
+		else if (tmp == 0x03)
+			mtd->erasesize = 768 * 1024;
+		else
+			mtd->erasesize = (64 * 1024) << tmp;
+		*busw = 0;
+	} else {
+		/* Calc pagesize */
+		mtd->writesize = 1024 << (extid & 0x03);
+		extid >>= 2;
+		/* Calc oobsize */
+		mtd->oobsize = (8 << (extid & 0x01)) *
+			(mtd->writesize >> 9);
+		extid >>= 2;
+		/* Calc blocksize. Blocksize is multiples of 64KiB */
+		mtd->erasesize = (64 * 1024) << (extid & 0x03);
+		extid >>= 2;
+		/* Get buswidth information */
+		*busw = (extid & 0x01) ? NAND_BUSWIDTH_16 : 0;
+	}
+}
+
+/*
+ * Old devices have chip data hardcoded in the device ID table. nand_decode_id
+ * decodes a matching ID table entry and assigns the MTD size parameters for
+ * the chip.
+ */
+static void nand_decode_id(struct mtd_info *mtd, struct nand_chip *chip,
+				struct nand_flash_dev *type, u8 id_data[8],
+				int *busw)
+{
+	int maf_id = id_data[0];
+
+	mtd->erasesize = type->erasesize;
+	mtd->writesize = type->pagesize;
+	mtd->oobsize = mtd->writesize / 32;
+	*busw = type->options & NAND_BUSWIDTH_16;
+
+	/*
+	 * Check for Spansion/AMD ID + repeating 5th, 6th byte since
+	 * some Spansion chips have erasesize that conflicts with size
+	 * listed in nand_ids table.
+	 * Data sheet (5 byte ID): Spansion S30ML-P ORNAND (p.39)
+	 */
+	if (maf_id == NAND_MFR_AMD && id_data[4] != 0x00 && id_data[5] == 0x00
+			&& id_data[6] == 0x00 && id_data[7] == 0x00
+			&& mtd->writesize == 512) {
+		mtd->erasesize = 128 * 1024;
+		mtd->erasesize <<= ((id_data[3] & 0x03) << 1);
+	}
+}
+
+/*
+ * Set the bad block marker/indicator (BBM/BBI) patterns according to some
+ * heuristic patterns using various detected parameters (e.g., manufacturer,
+ * page size, cell-type information).
+ */
+static void nand_decode_bbm_options(struct mtd_info *mtd,
+				    struct nand_chip *chip, u8 id_data[8])
+{
+	int maf_id = id_data[0];
+
+	/* Set the bad block position */
+	if (mtd->writesize > 512 || (chip->options & NAND_BUSWIDTH_16))
+		chip->badblockpos = NAND_LARGE_BADBLOCK_POS;
+	else
+		chip->badblockpos = NAND_SMALL_BADBLOCK_POS;
+
+	/*
+	 * Bad block marker is stored in the last page of each block on Samsung
+	 * and Hynix MLC devices; stored in first two pages of each block on
+	 * Micron devices with 2KiB pages and on SLC Samsung, Hynix, Toshiba,
+	 * AMD/Spansion, and Macronix.  All others scan only the first page.
+	 */
+	if ((chip->cellinfo & NAND_CI_CELLTYPE_MSK) &&
+			(maf_id == NAND_MFR_SAMSUNG ||
+			 maf_id == NAND_MFR_HYNIX))
+		chip->bbt_options |= NAND_BBT_SCANLASTPAGE;
+	else if ((!(chip->cellinfo & NAND_CI_CELLTYPE_MSK) &&
+				(maf_id == NAND_MFR_SAMSUNG ||
+				 maf_id == NAND_MFR_HYNIX ||
+				 maf_id == NAND_MFR_TOSHIBA ||
+				 maf_id == NAND_MFR_AMD ||
+				 maf_id == NAND_MFR_MACRONIX)) ||
+			(mtd->writesize == 2048 &&
+			 maf_id == NAND_MFR_MICRON))
+		chip->bbt_options |= NAND_BBT_SCAN2NDPAGE;
+}
+
+/*
  * Get the flash and manufacturer id and lookup if the type is supported.
  */
 static struct nand_flash_dev *nand_get_flash_type(struct mtd_info *mtd,
@@ -2932,7 +3155,6 @@ static struct nand_flash_dev *nand_get_flash_type(struct mtd_info *mtd,
 {
 	int i, maf_idx;
 	u8 id_data[8];
-	int ret;
 
 	/* Select the device */
 	chip->select_chip(mtd, 0);
@@ -2959,7 +3181,8 @@ static struct nand_flash_dev *nand_get_flash_type(struct mtd_info *mtd,
 
 	chip->cmdfunc(mtd, NAND_CMD_READID, 0x00, -1);
 
-	for (i = 0; i < 2; i++)
+	/* Read entire ID string */
+	for (i = 0; i < 8; i++)
 		id_data[i] = chip->read_byte(mtd);
 
 	if (id_data[0] != *maf_id || id_data[1] != *dev_id) {
@@ -2979,18 +3202,10 @@ static struct nand_flash_dev *nand_get_flash_type(struct mtd_info *mtd,
 	chip->onfi_version = 0;
 	if (!type->name || !type->pagesize) {
 		/* Check is chip is ONFI compliant */
-		ret = nand_flash_detect_onfi(mtd, chip, &busw);
-		if (ret)
+		if (nand_flash_detect_onfi(mtd, chip, &busw))
 			goto ident_done;
 	}
 
-	chip->cmdfunc(mtd, NAND_CMD_READID, 0x00, -1);
-
-	/* Read entire ID string */
-
-	for (i = 0; i < 8; i++)
-		id_data[i] = chip->read_byte(mtd);
-
 	if (!type->name)
 		return ERR_PTR(-ENODEV);
 
@@ -3003,86 +3218,13 @@ static struct nand_flash_dev *nand_get_flash_type(struct mtd_info *mtd,
 		/* Set the pagesize, oobsize, erasesize by the driver */
 		busw = chip->init_size(mtd, chip, id_data);
 	} else if (!type->pagesize) {
-		int extid;
-		/* The 3rd id byte holds MLC / multichip data */
-		chip->cellinfo = id_data[2];
-		/* The 4th id byte is the important one */
-		extid = id_data[3];
-
-		/*
-		 * Field definitions are in the following datasheets:
-		 * Old style (4,5 byte ID): Samsung K9GAG08U0M (p.32)
-		 * New style   (6 byte ID): Samsung K9GBG08U0M (p.40)
-		 *
-		 * Check for wraparound + Samsung ID + nonzero 6th byte
-		 * to decide what to do.
-		 */
-		if (id_data[0] == id_data[6] && id_data[1] == id_data[7] &&
-				id_data[0] == NAND_MFR_SAMSUNG &&
-				(chip->cellinfo & NAND_CI_CELLTYPE_MSK) &&
-				id_data[5] != 0x00) {
-			/* Calc pagesize */
-			mtd->writesize = 2048 << (extid & 0x03);
-			extid >>= 2;
-			/* Calc oobsize */
-			switch (extid & 0x03) {
-			case 1:
-				mtd->oobsize = 128;
-				break;
-			case 2:
-				mtd->oobsize = 218;
-				break;
-			case 3:
-				mtd->oobsize = 400;
-				break;
-			default:
-				mtd->oobsize = 436;
-				break;
-			}
-			extid >>= 2;
-			/* Calc blocksize */
-			mtd->erasesize = (128 * 1024) <<
-				(((extid >> 1) & 0x04) | (extid & 0x03));
-			busw = 0;
-		} else {
-			/* Calc pagesize */
-			mtd->writesize = 1024 << (extid & 0x03);
-			extid >>= 2;
-			/* Calc oobsize */
-			mtd->oobsize = (8 << (extid & 0x01)) *
-				(mtd->writesize >> 9);
-			extid >>= 2;
-			/* Calc blocksize. Blocksize is multiples of 64KiB */
-			mtd->erasesize = (64 * 1024) << (extid & 0x03);
-			extid >>= 2;
-			/* Get buswidth information */
-			busw = (extid & 0x01) ? NAND_BUSWIDTH_16 : 0;
-		}
+		/* Decode parameters from extended ID */
+		nand_decode_ext_id(mtd, chip, id_data, &busw);
 	} else {
-		/*
-		 * Old devices have chip data hardcoded in the device id table.
-		 */
-		mtd->erasesize = type->erasesize;
-		mtd->writesize = type->pagesize;
-		mtd->oobsize = mtd->writesize / 32;
-		busw = type->options & NAND_BUSWIDTH_16;
-
-		/*
-		 * Check for Spansion/AMD ID + repeating 5th, 6th byte since
-		 * some Spansion chips have erasesize that conflicts with size
-		 * listed in nand_ids table.
-		 * Data sheet (5 byte ID): Spansion S30ML-P ORNAND (p.39)
-		 */
-		if (*maf_id == NAND_MFR_AMD && id_data[4] != 0x00 &&
-				id_data[5] == 0x00 && id_data[6] == 0x00 &&
-				id_data[7] == 0x00 && mtd->writesize == 512) {
-			mtd->erasesize = 128 * 1024;
-			mtd->erasesize <<= ((id_data[3] & 0x03) << 1);
-		}
+		nand_decode_id(mtd, chip, type, id_data, &busw);
 	}
-	/* Get chip options, preserve non chip based options */
-	chip->options &= ~NAND_CHIPOPTIONS_MSK;
-	chip->options |= type->options & NAND_CHIPOPTIONS_MSK;
+	/* Get chip options */
+	chip->options |= type->options;
 
 	/*
 	 * Check if chip is not a Samsung device. Do not clear the
@@ -3112,6 +3254,8 @@ ident_done:
 		return ERR_PTR(-EINVAL);
 	}
 
+	nand_decode_bbm_options(mtd, chip, id_data);
+
 	/* Calculate the address shift from the page size */
 	chip->page_shift = ffs(mtd->writesize) - 1;
 	/* Convert chipsize to number of pages per chip -1 */
@@ -3128,33 +3272,6 @@ ident_done:
 
 	chip->badblockbits = 8;
 
-	/* Set the bad block position */
-	if (mtd->writesize > 512 || (busw & NAND_BUSWIDTH_16))
-		chip->badblockpos = NAND_LARGE_BADBLOCK_POS;
-	else
-		chip->badblockpos = NAND_SMALL_BADBLOCK_POS;
-
-	/*
-	 * Bad block marker is stored in the last page of each block
-	 * on Samsung and Hynix MLC devices; stored in first two pages
-	 * of each block on Micron devices with 2KiB pages and on
-	 * SLC Samsung, Hynix, Toshiba, AMD/Spansion, and Macronix.
-	 * All others scan only the first page.
-	 */
-	if ((chip->cellinfo & NAND_CI_CELLTYPE_MSK) &&
-			(*maf_id == NAND_MFR_SAMSUNG ||
-			 *maf_id == NAND_MFR_HYNIX))
-		chip->bbt_options |= NAND_BBT_SCANLASTPAGE;
-	else if ((!(chip->cellinfo & NAND_CI_CELLTYPE_MSK) &&
-				(*maf_id == NAND_MFR_SAMSUNG ||
-				 *maf_id == NAND_MFR_HYNIX ||
-				 *maf_id == NAND_MFR_TOSHIBA ||
-				 *maf_id == NAND_MFR_AMD ||
-				 *maf_id == NAND_MFR_MACRONIX)) ||
-			(mtd->writesize == 2048 &&
-			 *maf_id == NAND_MFR_MICRON))
-		chip->bbt_options |= NAND_BBT_SCAN2NDPAGE;
-
 	/* Check for AND chips with 4 page planes */
 	if (chip->options & NAND_4PAGE_ARRAY)
 		chip->erase_cmd = multi_erase_cmd;
@@ -3284,6 +3401,12 @@ int nand_scan_tail(struct mtd_info *mtd)
 	if (!chip->write_page)
 		chip->write_page = nand_write_page;
 
+	/* set for ONFI nand */
+	if (!chip->onfi_set_features)
+		chip->onfi_set_features = nand_onfi_set_features;
+	if (!chip->onfi_get_features)
+		chip->onfi_get_features = nand_onfi_get_features;
+
 	/*
 	 * Check ECC mode, default to software if 3byte/512byte hardware ECC is
 	 * selected and we have 256 byte pagesize fallback to software ECC
@@ -3477,6 +3600,10 @@ int nand_scan_tail(struct mtd_info *mtd)
 	/* Invalidate the pagebuffer reference */
 	chip->pagebuf = -1;
 
+	/* Large page NAND with SOFT_ECC should support subpage reads */
+	if ((chip->ecc.mode == NAND_ECC_SOFT) && (chip->page_shift > 9))
+		chip->options |= NAND_SUBPAGE_READ;
+
 	/* Fill in remaining MTD driver data */
 	mtd->type = MTD_NANDFLASH;
 	mtd->flags = (chip->options & NAND_ROM) ? MTD_CAP_ROM :
diff --git a/drivers/mtd/nand/nand_bbt.c b/drivers/mtd/nand/nand_bbt.c
index 30d1319ff06..916d6e9c0ab 100644
--- a/drivers/mtd/nand/nand_bbt.c
+++ b/drivers/mtd/nand/nand_bbt.c
@@ -4,7 +4,7 @@
  *  Overview:
  *   Bad block table support for the NAND driver
  *
- *  Copyright (C) 2004 Thomas Gleixner (tglx@linutronix.de)
+ *  Copyright © 2004 Thomas Gleixner (tglx@linutronix.de)
  *
  * 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
@@ -22,7 +22,7 @@
  * BBT on flash. If a BBT is found then the contents are read and the memory
  * based BBT is created. If a mirrored BBT is selected then the mirror is
  * searched too and the versions are compared. If the mirror has a greater
- * version number than the mirror BBT is used to build the memory based BBT.
+ * version number, then the mirror BBT is used to build the memory based BBT.
  * If the tables are not versioned, then we "or" the bad block information.
  * If one of the BBTs is out of date or does not exist it is (re)created.
  * If no BBT exists at all then the device is scanned for factory marked
@@ -62,21 +62,20 @@
 #include <linux/slab.h>
 #include <linux/types.h>
 #include <linux/mtd/mtd.h>
+#include <linux/mtd/bbm.h>
 #include <linux/mtd/nand.h>
 #include <linux/mtd/nand_ecc.h>
 #include <linux/bitops.h>
 #include <linux/delay.h>
 #include <linux/vmalloc.h>
 #include <linux/export.h>
+#include <linux/string.h>
 
 static int check_pattern_no_oob(uint8_t *buf, struct nand_bbt_descr *td)
 {
-	int ret;
-
-	ret = memcmp(buf, td->pattern, td->len);
-	if (!ret)
-		return ret;
-	return -1;
+	if (memcmp(buf, td->pattern, td->len))
+		return -1;
+	return 0;
 }
 
 /**
@@ -92,19 +91,16 @@ static int check_pattern_no_oob(uint8_t *buf, struct nand_bbt_descr *td)
  */
 static int check_pattern(uint8_t *buf, int len, int paglen, struct nand_bbt_descr *td)
 {
-	int i, end = 0;
+	int end = 0;
 	uint8_t *p = buf;
 
 	if (td->options & NAND_BBT_NO_OOB)
 		return check_pattern_no_oob(buf, td);
 
 	end = paglen + td->offs;
-	if (td->options & NAND_BBT_SCANEMPTY) {
-		for (i = 0; i < end; i++) {
-			if (p[i] != 0xff)
-				return -1;
-		}
-	}
+	if (td->options & NAND_BBT_SCANEMPTY)
+		if (memchr_inv(p, 0xff, end))
+			return -1;
 	p += end;
 
 	/* Compare the pattern */
@@ -114,10 +110,8 @@ static int check_pattern(uint8_t *buf, int len, int paglen, struct nand_bbt_desc
 	if (td->options & NAND_BBT_SCANEMPTY) {
 		p += td->len;
 		end += td->len;
-		for (i = end; i < len; i++) {
-			if (*p++ != 0xff)
-				return -1;
-		}
+		if (memchr_inv(p, 0xff, len - end))
+			return -1;
 	}
 	return 0;
 }
@@ -133,14 +127,9 @@ static int check_pattern(uint8_t *buf, int len, int paglen, struct nand_bbt_desc
  */
 static int check_short_pattern(uint8_t *buf, struct nand_bbt_descr *td)
 {
-	int i;
-	uint8_t *p = buf;
-
 	/* Compare the pattern */
-	for (i = 0; i < td->len; i++) {
-		if (p[td->offs + i] != td->pattern[i])
-			return -1;
-	}
+	if (memcmp(buf + td->offs, td->pattern, td->len))
+		return -1;
 	return 0;
 }
 
@@ -288,7 +277,7 @@ static int read_abs_bbt(struct mtd_info *mtd, uint8_t *buf, struct nand_bbt_desc
 }
 
 /* BBT marker is in the first page, no OOB */
-static int scan_read_raw_data(struct mtd_info *mtd, uint8_t *buf, loff_t offs,
+static int scan_read_data(struct mtd_info *mtd, uint8_t *buf, loff_t offs,
 			 struct nand_bbt_descr *td)
 {
 	size_t retlen;
@@ -301,14 +290,24 @@ static int scan_read_raw_data(struct mtd_info *mtd, uint8_t *buf, loff_t offs,
 	return mtd_read(mtd, offs, len, &retlen, buf);
 }
 
-/* Scan read raw data from flash */
-static int scan_read_raw_oob(struct mtd_info *mtd, uint8_t *buf, loff_t offs,
+/**
+ * scan_read_oob - [GENERIC] Scan data+OOB region to buffer
+ * @mtd: MTD device structure
+ * @buf: temporary buffer
+ * @offs: offset at which to scan
+ * @len: length of data region to read
+ *
+ * Scan read data from data+OOB. May traverse multiple pages, interleaving
+ * page,OOB,page,OOB,... in buf. Completes transfer and returns the "strongest"
+ * ECC condition (error or bitflip). May quit on the first (non-ECC) error.
+ */
+static int scan_read_oob(struct mtd_info *mtd, uint8_t *buf, loff_t offs,
 			 size_t len)
 {
 	struct mtd_oob_ops ops;
-	int res;
+	int res, ret = 0;
 
-	ops.mode = MTD_OPS_RAW;
+	ops.mode = MTD_OPS_PLACE_OOB;
 	ops.ooboffs = 0;
 	ops.ooblen = mtd->oobsize;
 
@@ -318,24 +317,27 @@ static int scan_read_raw_oob(struct mtd_info *mtd, uint8_t *buf, loff_t offs,
 		ops.oobbuf = buf + ops.len;
 
 		res = mtd_read_oob(mtd, offs, &ops);
-
-		if (res)
-			return res;
+		if (res) {
+			if (!mtd_is_bitflip_or_eccerr(res))
+				return res;
+			else if (mtd_is_eccerr(res) || !ret)
+				ret = res;
+		}
 
 		buf += mtd->oobsize + mtd->writesize;
 		len -= mtd->writesize;
 		offs += mtd->writesize;
 	}
-	return 0;
+	return ret;
 }
 
-static int scan_read_raw(struct mtd_info *mtd, uint8_t *buf, loff_t offs,
+static int scan_read(struct mtd_info *mtd, uint8_t *buf, loff_t offs,
 			 size_t len, struct nand_bbt_descr *td)
 {
 	if (td->options & NAND_BBT_NO_OOB)
-		return scan_read_raw_data(mtd, buf, offs, td);
+		return scan_read_data(mtd, buf, offs, td);
 	else
-		return scan_read_raw_oob(mtd, buf, offs, len);
+		return scan_read_oob(mtd, buf, offs, len);
 }
 
 /* Scan write data with oob to flash */
@@ -373,14 +375,14 @@ static u32 bbt_get_ver_offs(struct mtd_info *mtd, struct nand_bbt_descr *td)
  * Read the bad block table(s) for all chips starting at a given page. We
  * assume that the bbt bits are in consecutive order.
  */
-static int read_abs_bbts(struct mtd_info *mtd, uint8_t *buf,
-			 struct nand_bbt_descr *td, struct nand_bbt_descr *md)
+static void read_abs_bbts(struct mtd_info *mtd, uint8_t *buf,
+			  struct nand_bbt_descr *td, struct nand_bbt_descr *md)
 {
 	struct nand_chip *this = mtd->priv;
 
 	/* Read the primary version, if available */
 	if (td->options & NAND_BBT_VERSION) {
-		scan_read_raw(mtd, buf, (loff_t)td->pages[0] << this->page_shift,
+		scan_read(mtd, buf, (loff_t)td->pages[0] << this->page_shift,
 			      mtd->writesize, td);
 		td->version[0] = buf[bbt_get_ver_offs(mtd, td)];
 		pr_info("Bad block table at page %d, version 0x%02X\n",
@@ -389,28 +391,27 @@ static int read_abs_bbts(struct mtd_info *mtd, uint8_t *buf,
 
 	/* Read the mirror version, if available */
 	if (md && (md->options & NAND_BBT_VERSION)) {
-		scan_read_raw(mtd, buf, (loff_t)md->pages[0] << this->page_shift,
-			      mtd->writesize, td);
+		scan_read(mtd, buf, (loff_t)md->pages[0] << this->page_shift,
+			      mtd->writesize, md);
 		md->version[0] = buf[bbt_get_ver_offs(mtd, md)];
 		pr_info("Bad block table at page %d, version 0x%02X\n",
 			 md->pages[0], md->version[0]);
 	}
-	return 1;
 }
 
 /* Scan a given block full */
 static int scan_block_full(struct mtd_info *mtd, struct nand_bbt_descr *bd,
 			   loff_t offs, uint8_t *buf, size_t readlen,
-			   int scanlen, int len)
+			   int scanlen, int numpages)
 {
 	int ret, j;
 
-	ret = scan_read_raw_oob(mtd, buf, offs, readlen);
+	ret = scan_read_oob(mtd, buf, offs, readlen);
 	/* Ignore ECC errors when checking for BBM */
 	if (ret && !mtd_is_bitflip_or_eccerr(ret))
 		return ret;
 
-	for (j = 0; j < len; j++, buf += scanlen) {
+	for (j = 0; j < numpages; j++, buf += scanlen) {
 		if (check_pattern(buf, scanlen, mtd->writesize, bd))
 			return 1;
 	}
@@ -419,7 +420,7 @@ static int scan_block_full(struct mtd_info *mtd, struct nand_bbt_descr *bd,
 
 /* Scan a given block partially */
 static int scan_block_fast(struct mtd_info *mtd, struct nand_bbt_descr *bd,
-			   loff_t offs, uint8_t *buf, int len)
+			   loff_t offs, uint8_t *buf, int numpages)
 {
 	struct mtd_oob_ops ops;
 	int j, ret;
@@ -430,7 +431,7 @@ static int scan_block_fast(struct mtd_info *mtd, struct nand_bbt_descr *bd,
 	ops.datbuf = NULL;
 	ops.mode = MTD_OPS_PLACE_OOB;
 
-	for (j = 0; j < len; j++) {
+	for (j = 0; j < numpages; j++) {
 		/*
 		 * Read the full oob until read_oob is fixed to handle single
 		 * byte reads for 16 bit buswidth.
@@ -463,7 +464,7 @@ static int create_bbt(struct mtd_info *mtd, uint8_t *buf,
 	struct nand_bbt_descr *bd, int chip)
 {
 	struct nand_chip *this = mtd->priv;
-	int i, numblocks, len, scanlen;
+	int i, numblocks, numpages, scanlen;
 	int startblock;
 	loff_t from;
 	size_t readlen;
@@ -471,11 +472,11 @@ static int create_bbt(struct mtd_info *mtd, uint8_t *buf,
 	pr_info("Scanning device for bad blocks\n");
 
 	if (bd->options & NAND_BBT_SCANALLPAGES)
-		len = 1 << (this->bbt_erase_shift - this->page_shift);
+		numpages = 1 << (this->bbt_erase_shift - this->page_shift);
 	else if (bd->options & NAND_BBT_SCAN2NDPAGE)
-		len = 2;
+		numpages = 2;
 	else
-		len = 1;
+		numpages = 1;
 
 	if (!(bd->options & NAND_BBT_SCANEMPTY)) {
 		/* We need only read few bytes from the OOB area */
@@ -484,7 +485,7 @@ static int create_bbt(struct mtd_info *mtd, uint8_t *buf,
 	} else {
 		/* Full page content should be read */
 		scanlen = mtd->writesize + mtd->oobsize;
-		readlen = len * mtd->writesize;
+		readlen = numpages * mtd->writesize;
 	}
 
 	if (chip == -1) {
@@ -508,7 +509,7 @@ static int create_bbt(struct mtd_info *mtd, uint8_t *buf,
 	}
 
 	if (this->bbt_options & NAND_BBT_SCANLASTPAGE)
-		from += mtd->erasesize - (mtd->writesize * len);
+		from += mtd->erasesize - (mtd->writesize * numpages);
 
 	for (i = startblock; i < numblocks;) {
 		int ret;
@@ -517,9 +518,9 @@ static int create_bbt(struct mtd_info *mtd, uint8_t *buf,
 
 		if (bd->options & NAND_BBT_SCANALLPAGES)
 			ret = scan_block_full(mtd, bd, from, buf, readlen,
-					      scanlen, len);
+					      scanlen, numpages);
 		else
-			ret = scan_block_fast(mtd, bd, from, buf, len);
+			ret = scan_block_fast(mtd, bd, from, buf, numpages);
 
 		if (ret < 0)
 			return ret;
@@ -594,7 +595,7 @@ static int search_bbt(struct mtd_info *mtd, uint8_t *buf, struct nand_bbt_descr
 			loff_t offs = (loff_t)actblock << this->bbt_erase_shift;
 
 			/* Read first page */
-			scan_read_raw(mtd, buf, offs, mtd->writesize, td);
+			scan_read(mtd, buf, offs, mtd->writesize, td);
 			if (!check_pattern(buf, scanlen, mtd->writesize, td)) {
 				td->pages[i] = actblock << blocktopage;
 				if (td->options & NAND_BBT_VERSION) {
@@ -626,7 +627,9 @@ static int search_bbt(struct mtd_info *mtd, uint8_t *buf, struct nand_bbt_descr
  *
  * Search and read the bad block table(s).
  */
-static int search_read_bbts(struct mtd_info *mtd, uint8_t * buf, struct nand_bbt_descr *td, struct nand_bbt_descr *md)
+static void search_read_bbts(struct mtd_info *mtd, uint8_t *buf,
+			     struct nand_bbt_descr *td,
+			     struct nand_bbt_descr *md)
 {
 	/* Search the primary table */
 	search_bbt(mtd, buf, td);
@@ -634,9 +637,6 @@ static int search_read_bbts(struct mtd_info *mtd, uint8_t * buf, struct nand_bbt
 	/* Search the mirror table */
 	if (md)
 		search_bbt(mtd, buf, md);
-
-	/* Force result check */
-	return 1;
 }
 
 /**
@@ -1162,14 +1162,13 @@ int nand_scan_bbt(struct mtd_info *mtd, struct nand_bbt_descr *bd)
 
 	/* Is the bbt at a given page? */
 	if (td->options & NAND_BBT_ABSPAGE) {
-		res = read_abs_bbts(mtd, buf, td, md);
+		read_abs_bbts(mtd, buf, td, md);
 	} else {
 		/* Search the bad block table using a pattern in oob */
-		res = search_read_bbts(mtd, buf, td, md);
+		search_read_bbts(mtd, buf, td, md);
 	}
 
-	if (res)
-		res = check_create(mtd, buf, bd);
+	res = check_create(mtd, buf, bd);
 
 	/* Prevent the bbt regions from erasing / writing */
 	mark_bbt_region(mtd, td);
@@ -1260,7 +1259,7 @@ static struct nand_bbt_descr bbt_main_descr = {
 	.offs =	8,
 	.len = 4,
 	.veroffs = 12,
-	.maxblocks = 4,
+	.maxblocks = NAND_BBT_SCAN_MAXBLOCKS,
 	.pattern = bbt_pattern
 };
 
@@ -1270,27 +1269,27 @@ static struct nand_bbt_descr bbt_mirror_descr = {
 	.offs =	8,
 	.len = 4,
 	.veroffs = 12,
-	.maxblocks = 4,
+	.maxblocks = NAND_BBT_SCAN_MAXBLOCKS,
 	.pattern = mirror_pattern
 };
 
-static struct nand_bbt_descr bbt_main_no_bbt_descr = {
+static struct nand_bbt_descr bbt_main_no_oob_descr = {
 	.options = NAND_BBT_LASTBLOCK | NAND_BBT_CREATE | NAND_BBT_WRITE
 		| NAND_BBT_2BIT | NAND_BBT_VERSION | NAND_BBT_PERCHIP
 		| NAND_BBT_NO_OOB,
 	.len = 4,
 	.veroffs = 4,
-	.maxblocks = 4,
+	.maxblocks = NAND_BBT_SCAN_MAXBLOCKS,
 	.pattern = bbt_pattern
 };
 
-static struct nand_bbt_descr bbt_mirror_no_bbt_descr = {
+static struct nand_bbt_descr bbt_mirror_no_oob_descr = {
 	.options = NAND_BBT_LASTBLOCK | NAND_BBT_CREATE | NAND_BBT_WRITE
 		| NAND_BBT_2BIT | NAND_BBT_VERSION | NAND_BBT_PERCHIP
 		| NAND_BBT_NO_OOB,
 	.len = 4,
 	.veroffs = 4,
-	.maxblocks = 4,
+	.maxblocks = NAND_BBT_SCAN_MAXBLOCKS,
 	.pattern = mirror_pattern
 };
 
@@ -1355,8 +1354,8 @@ int nand_default_bbt(struct mtd_info *mtd)
 		/* Use the default pattern descriptors */
 		if (!this->bbt_td) {
 			if (this->bbt_options & NAND_BBT_NO_OOB) {
-				this->bbt_td = &bbt_main_no_bbt_descr;
-				this->bbt_md = &bbt_mirror_no_bbt_descr;
+				this->bbt_td = &bbt_main_no_oob_descr;
+				this->bbt_md = &bbt_mirror_no_oob_descr;
 			} else {
 				this->bbt_td = &bbt_main_descr;
 				this->bbt_md = &bbt_mirror_descr;
@@ -1406,3 +1405,4 @@ int nand_isbad_bbt(struct mtd_info *mtd, loff_t offs, int allowbbt)
 
 EXPORT_SYMBOL(nand_scan_bbt);
 EXPORT_SYMBOL(nand_default_bbt);
+EXPORT_SYMBOL_GPL(nand_update_bbt);
diff --git a/drivers/mtd/nand/nand_bcm_umi.c b/drivers/mtd/nand/nand_bcm_umi.c
deleted file mode 100644
index 46a6bc9c4b7..00000000000
--- a/drivers/mtd/nand/nand_bcm_umi.c
+++ /dev/null
@@ -1,149 +0,0 @@
-/*****************************************************************************
-* Copyright 2004 - 2009 Broadcom Corporation.  All rights reserved.
-*
-* 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, available at
-* http://www.broadcom.com/licenses/GPLv2.php (the "GPL").
-*
-* 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.
-*****************************************************************************/
-
-/* ---- Include Files ---------------------------------------------------- */
-#include <mach/reg_umi.h>
-#include "nand_bcm_umi.h"
-#ifdef BOOT0_BUILD
-#include <uart.h>
-#endif
-
-/* ---- External Variable Declarations ----------------------------------- */
-/* ---- External Function Prototypes ------------------------------------- */
-/* ---- Public Variables ------------------------------------------------- */
-/* ---- Private Constants and Types -------------------------------------- */
-/* ---- Private Function Prototypes -------------------------------------- */
-/* ---- Private Variables ------------------------------------------------ */
-/* ---- Private Functions ------------------------------------------------ */
-
-#if NAND_ECC_BCH
-/****************************************************************************
-*  nand_bch_ecc_flip_bit - Routine to flip an errored bit
-*
-*  PURPOSE:
-*     This is a helper routine that flips the bit (0 -> 1 or 1 -> 0) of the
-*     errored bit specified
-*
-*  PARAMETERS:
-*     datap - Container that holds the 512 byte data
-*     errorLocation - Location of the bit that needs to be flipped
-*
-*  RETURNS:
-*     None
-****************************************************************************/
-static void nand_bcm_umi_bch_ecc_flip_bit(uint8_t *datap, int errorLocation)
-{
-	int locWithinAByte = (errorLocation & REG_UMI_BCH_ERR_LOC_BYTE) >> 0;
-	int locWithinAWord = (errorLocation & REG_UMI_BCH_ERR_LOC_WORD) >> 3;
-	int locWithinAPage = (errorLocation & REG_UMI_BCH_ERR_LOC_PAGE) >> 5;
-
-	uint8_t errorByte = 0;
-	uint8_t byteMask = 1 << locWithinAByte;
-
-	/* BCH uses big endian, need to change the location
-	 * bits to little endian */
-	locWithinAWord = 3 - locWithinAWord;
-
-	errorByte = datap[locWithinAPage * sizeof(uint32_t) + locWithinAWord];
-
-#ifdef BOOT0_BUILD
-	puthexs("\nECC Correct Offset: ",
-		locWithinAPage * sizeof(uint32_t) + locWithinAWord);
-	puthexs(" errorByte:", errorByte);
-	puthex8(" Bit: ", locWithinAByte);
-#endif
-
-	if (errorByte & byteMask) {
-		/* bit needs to be cleared */
-		errorByte &= ~byteMask;
-	} else {
-		/* bit needs to be set */
-		errorByte |= byteMask;
-	}
-
-	/* write back the value with the fixed bit */
-	datap[locWithinAPage * sizeof(uint32_t) + locWithinAWord] = errorByte;
-}
-
-/****************************************************************************
-*  nand_correct_page_bch - Routine to correct bit errors when reading NAND
-*
-*  PURPOSE:
-*     This routine reads the BCH registers to determine if there are any bit
-*     errors during the read of the last 512 bytes of data + ECC bytes.  If
-*     errors exists, the routine fixes it.
-*
-*  PARAMETERS:
-*     datap - Container that holds the 512 byte data
-*
-*  RETURNS:
-*     0 or greater = Number of errors corrected
-*                    (No errors are found or errors have been fixed)
-*    -1 = Error(s) cannot be fixed
-****************************************************************************/
-int nand_bcm_umi_bch_correct_page(uint8_t *datap, uint8_t *readEccData,
-				  int numEccBytes)
-{
-	int numErrors;
-	int errorLocation;
-	int idx;
-	uint32_t regValue;
-
-	/* wait for read ECC to be valid */
-	regValue = nand_bcm_umi_bch_poll_read_ecc_calc();
-
-	/*
-	 * read the control status register to determine if there
-	 * are error'ed bits
-	 * see if errors are correctible
-	 */
-	if ((regValue & REG_UMI_BCH_CTRL_STATUS_UNCORR_ERR) > 0) {
-		int i;
-
-		for (i = 0; i < numEccBytes; i++) {
-			if (readEccData[i] != 0xff) {
-				/* errors cannot be fixed, return -1 */
-				return -1;
-			}
-		}
-		/* If ECC is unprogrammed then we can't correct,
-		 * assume everything OK */
-		return 0;
-	}
-
-	if ((regValue & REG_UMI_BCH_CTRL_STATUS_CORR_ERR) == 0) {
-		/* no errors */
-		return 0;
-	}
-
-	/*
-	 * Fix errored bits by doing the following:
-	 * 1. Read the number of errors in the control and status register
-	 * 2. Read the error location registers that corresponds to the number
-	 *    of errors reported
-	 * 3. Invert the bit in the data
-	 */
-	numErrors = (regValue & REG_UMI_BCH_CTRL_STATUS_NB_CORR_ERROR) >> 20;
-
-	for (idx = 0; idx < numErrors; idx++) {
-		errorLocation =
-		    REG_UMI_BCH_ERR_LOC_ADDR(idx) & REG_UMI_BCH_ERR_LOC_MASK;
-
-		/* Flip bit */
-		nand_bcm_umi_bch_ecc_flip_bit(datap, errorLocation);
-	}
-	/* Errors corrected */
-	return numErrors;
-}
-#endif
diff --git a/drivers/mtd/nand/nand_bcm_umi.h b/drivers/mtd/nand/nand_bcm_umi.h
deleted file mode 100644
index d90186684db..00000000000
--- a/drivers/mtd/nand/nand_bcm_umi.h
+++ /dev/null
@@ -1,336 +0,0 @@
-/*****************************************************************************
-* Copyright 2003 - 2009 Broadcom Corporation.  All rights reserved.
-*
-* 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, available at
-* http://www.broadcom.com/licenses/GPLv2.php (the "GPL").
-*
-* 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.
-*****************************************************************************/
-#ifndef NAND_BCM_UMI_H
-#define NAND_BCM_UMI_H
-
-/* ---- Include Files ---------------------------------------------------- */
-#include <mach/reg_umi.h>
-#include <mach/reg_nand.h>
-#include <mach/cfg_global.h>
-
-/* ---- Constants and Types ---------------------------------------------- */
-#if (CFG_GLOBAL_CHIP_FAMILY == CFG_GLOBAL_CHIP_FAMILY_BCMRING)
-#define NAND_ECC_BCH (CFG_GLOBAL_CHIP_REV > 0xA0)
-#else
-#define NAND_ECC_BCH 0
-#endif
-
-#define CFG_GLOBAL_NAND_ECC_BCH_NUM_BYTES	13
-
-#if NAND_ECC_BCH
-#ifdef BOOT0_BUILD
-#define NAND_ECC_NUM_BYTES 13
-#else
-#define NAND_ECC_NUM_BYTES CFG_GLOBAL_NAND_ECC_BCH_NUM_BYTES
-#endif
-#else
-#define NAND_ECC_NUM_BYTES 3
-#endif
-
-#define NAND_DATA_ACCESS_SIZE 512
-
-/* ---- Variable Externs ------------------------------------------ */
-/* ---- Function Prototypes --------------------------------------- */
-int nand_bcm_umi_bch_correct_page(uint8_t *datap, uint8_t *readEccData,
-				  int numEccBytes);
-
-/* Check in device is ready */
-static inline int nand_bcm_umi_dev_ready(void)
-{
-	return readl(&REG_UMI_NAND_RCSR) & REG_UMI_NAND_RCSR_RDY;
-}
-
-/* Wait until device is ready */
-static inline void nand_bcm_umi_wait_till_ready(void)
-{
-	while (nand_bcm_umi_dev_ready() == 0)
-		;
-}
-
-/* Enable Hamming ECC */
-static inline void nand_bcm_umi_hamming_enable_hwecc(void)
-{
-	/* disable and reset ECC, 512 byte page */
-	writel(readl(&REG_UMI_NAND_ECC_CSR) & ~(REG_UMI_NAND_ECC_CSR_ECC_ENABLE |
-		REG_UMI_NAND_ECC_CSR_256BYTE), &REG_UMI_NAND_ECC_CSR);
-	/* enable ECC */
-	writel(readl(&REG_UMI_NAND_ECC_CSR) | REG_UMI_NAND_ECC_CSR_ECC_ENABLE,
-		&REG_UMI_NAND_ECC_CSR);
-}
-
-#if NAND_ECC_BCH
-/* BCH ECC specifics */
-#define ECC_BITS_PER_CORRECTABLE_BIT 13
-
-/* Enable BCH Read ECC */
-static inline void nand_bcm_umi_bch_enable_read_hwecc(void)
-{
-	/* disable and reset ECC */
-	writel(REG_UMI_BCH_CTRL_STATUS_RD_ECC_VALID, &REG_UMI_BCH_CTRL_STATUS);
-	/* Turn on ECC */
-	writel(REG_UMI_BCH_CTRL_STATUS_ECC_RD_EN, &REG_UMI_BCH_CTRL_STATUS);
-}
-
-/* Enable BCH Write ECC */
-static inline void nand_bcm_umi_bch_enable_write_hwecc(void)
-{
-	/* disable and reset ECC */
-	writel(REG_UMI_BCH_CTRL_STATUS_WR_ECC_VALID, &REG_UMI_BCH_CTRL_STATUS);
-	/* Turn on ECC */
-	writel(REG_UMI_BCH_CTRL_STATUS_ECC_WR_EN, &REG_UMI_BCH_CTRL_STATUS);
-}
-
-/* Config number of BCH ECC bytes */
-static inline void nand_bcm_umi_bch_config_ecc(uint8_t numEccBytes)
-{
-	uint32_t nValue;
-	uint32_t tValue;
-	uint32_t kValue;
-	uint32_t numBits = numEccBytes * 8;
-
-	/* disable and reset ECC */
-	writel(REG_UMI_BCH_CTRL_STATUS_WR_ECC_VALID |
-	       REG_UMI_BCH_CTRL_STATUS_RD_ECC_VALID,
-	       &REG_UMI_BCH_CTRL_STATUS);
-
-	/* Every correctible bit requires 13 ECC bits */
-	tValue = (uint32_t) (numBits / ECC_BITS_PER_CORRECTABLE_BIT);
-
-	/* Total data in number of bits for generating and computing BCH ECC */
-	nValue = (NAND_DATA_ACCESS_SIZE + numEccBytes) * 8;
-
-	/* K parameter is used internally.  K = N - (T * 13) */
-	kValue = nValue - (tValue * ECC_BITS_PER_CORRECTABLE_BIT);
-
-	/* Write the settings */
-	writel(nValue, &REG_UMI_BCH_N);
-	writel(tValue, &REG_UMI_BCH_T);
-	writel(kValue, &REG_UMI_BCH_K);
-}
-
-/* Pause during ECC read calculation to skip bytes in OOB */
-static inline void nand_bcm_umi_bch_pause_read_ecc_calc(void)
-{
-	writel(REG_UMI_BCH_CTRL_STATUS_ECC_RD_EN | REG_UMI_BCH_CTRL_STATUS_PAUSE_ECC_DEC, &REG_UMI_BCH_CTRL_STATUS);
-}
-
-/* Resume during ECC read calculation after skipping bytes in OOB */
-static inline void nand_bcm_umi_bch_resume_read_ecc_calc(void)
-{
-	writel(REG_UMI_BCH_CTRL_STATUS_ECC_RD_EN, &REG_UMI_BCH_CTRL_STATUS);
-}
-
-/* Poll read ECC calc to check when hardware completes */
-static inline uint32_t nand_bcm_umi_bch_poll_read_ecc_calc(void)
-{
-	uint32_t regVal;
-
-	do {
-		/* wait for ECC to be valid */
-		regVal = readl(&REG_UMI_BCH_CTRL_STATUS);
-	} while ((regVal & REG_UMI_BCH_CTRL_STATUS_RD_ECC_VALID) == 0);
-
-	return regVal;
-}
-
-/* Poll write ECC calc to check when hardware completes */
-static inline void nand_bcm_umi_bch_poll_write_ecc_calc(void)
-{
-	/* wait for ECC to be valid */
-	while ((readl(&REG_UMI_BCH_CTRL_STATUS) & REG_UMI_BCH_CTRL_STATUS_WR_ECC_VALID)
-	       == 0)
-		;
-}
-
-/* Read the OOB and ECC, for kernel write OOB to a buffer */
-#if defined(__KERNEL__) && !defined(STANDALONE)
-static inline void nand_bcm_umi_bch_read_oobEcc(uint32_t pageSize,
-	uint8_t *eccCalc, int numEccBytes, uint8_t *oobp)
-#else
-static inline void nand_bcm_umi_bch_read_oobEcc(uint32_t pageSize,
-	uint8_t *eccCalc, int numEccBytes)
-#endif
-{
-	int eccPos = 0;
-	int numToRead = 16;	/* There are 16 bytes per sector in the OOB */
-
-	/* ECC is already paused when this function is called */
-	if (pageSize != NAND_DATA_ACCESS_SIZE) {
-		/* skip BI */
-#if defined(__KERNEL__) && !defined(STANDALONE)
-		*oobp++ = readb(&REG_NAND_DATA8);
-#else
-		readb(&REG_NAND_DATA8);
-#endif
-		numToRead--;
-	}
-
-	while (numToRead > numEccBytes) {
-		/* skip free oob region */
-#if defined(__KERNEL__) && !defined(STANDALONE)
-		*oobp++ = readb(&REG_NAND_DATA8);
-#else
-		readb(&REG_NAND_DATA8);
-#endif
-		numToRead--;
-	}
-
-	if (pageSize == NAND_DATA_ACCESS_SIZE) {
-		/* read ECC bytes before BI */
-		nand_bcm_umi_bch_resume_read_ecc_calc();
-
-		while (numToRead > 11) {
-#if defined(__KERNEL__) && !defined(STANDALONE)
-			*oobp = readb(&REG_NAND_DATA8);
-			eccCalc[eccPos++] = *oobp;
-			oobp++;
-#else
-			eccCalc[eccPos++] = readb(&REG_NAND_DATA8);
-#endif
-			numToRead--;
-		}
-
-		nand_bcm_umi_bch_pause_read_ecc_calc();
-
-		if (numToRead == 11) {
-			/* read BI */
-#if defined(__KERNEL__) && !defined(STANDALONE)
-			*oobp++ = readb(&REG_NAND_DATA8);
-#else
-			readb(&REG_NAND_DATA8);
-#endif
-			numToRead--;
-		}
-
-	}
-	/* read ECC bytes */
-	nand_bcm_umi_bch_resume_read_ecc_calc();
-	while (numToRead) {
-#if defined(__KERNEL__) && !defined(STANDALONE)
-		*oobp = readb(&REG_NAND_DATA8);
-		eccCalc[eccPos++] = *oobp;
-		oobp++;
-#else
-		eccCalc[eccPos++] = readb(&REG_NAND_DATA8);
-#endif
-		numToRead--;
-	}
-}
-
-/* Helper function to write ECC */
-static inline void NAND_BCM_UMI_ECC_WRITE(int numEccBytes, int eccBytePos,
-					  uint8_t *oobp, uint8_t eccVal)
-{
-	if (eccBytePos <= numEccBytes)
-		*oobp = eccVal;
-}
-
-/* Write OOB with ECC */
-static inline void nand_bcm_umi_bch_write_oobEcc(uint32_t pageSize,
-						 uint8_t *oobp, int numEccBytes)
-{
-	uint32_t eccVal = 0xffffffff;
-
-	/* wait for write ECC to be valid */
-	nand_bcm_umi_bch_poll_write_ecc_calc();
-
-	/*
-	 ** Get the hardware ecc from the 32-bit result registers.
-	 ** Read after 512 byte accesses. Format B3B2B1B0
-	 ** where B3 = ecc3, etc.
-	 */
-
-	if (pageSize == NAND_DATA_ACCESS_SIZE) {
-		/* Now fill in the ECC bytes */
-		if (numEccBytes >= 13)
-			eccVal = readl(&REG_UMI_BCH_WR_ECC_3);
-
-		/* Usually we skip CM in oob[0,1] */
-		NAND_BCM_UMI_ECC_WRITE(numEccBytes, 15, &oobp[0],
-			(eccVal >> 16) & 0xff);
-		NAND_BCM_UMI_ECC_WRITE(numEccBytes, 14, &oobp[1],
-			(eccVal >> 8) & 0xff);
-
-		/* Write ECC in oob[2,3,4] */
-		NAND_BCM_UMI_ECC_WRITE(numEccBytes, 13, &oobp[2],
-			eccVal & 0xff);	/* ECC 12 */
-
-		if (numEccBytes >= 9)
-			eccVal = readl(&REG_UMI_BCH_WR_ECC_2);
-
-		NAND_BCM_UMI_ECC_WRITE(numEccBytes, 12, &oobp[3],
-			(eccVal >> 24) & 0xff);	/* ECC11 */
-		NAND_BCM_UMI_ECC_WRITE(numEccBytes, 11, &oobp[4],
-			(eccVal >> 16) & 0xff);	/* ECC10 */
-
-		/* Always Skip BI in oob[5] */
-	} else {
-		/* Always Skip BI in oob[0] */
-
-		/* Now fill in the ECC bytes */
-		if (numEccBytes >= 13)
-			eccVal = readl(&REG_UMI_BCH_WR_ECC_3);
-
-		/* Usually skip CM in oob[1,2] */
-		NAND_BCM_UMI_ECC_WRITE(numEccBytes, 15, &oobp[1],
-			(eccVal >> 16) & 0xff);
-		NAND_BCM_UMI_ECC_WRITE(numEccBytes, 14, &oobp[2],
-			(eccVal >> 8) & 0xff);
-
-		/* Write ECC in oob[3-15] */
-		NAND_BCM_UMI_ECC_WRITE(numEccBytes, 13, &oobp[3],
-			eccVal & 0xff);	/* ECC12 */
-
-		if (numEccBytes >= 9)
-			eccVal = readl(&REG_UMI_BCH_WR_ECC_2);
-
-		NAND_BCM_UMI_ECC_WRITE(numEccBytes, 12, &oobp[4],
-			(eccVal >> 24) & 0xff);	/* ECC11 */
-		NAND_BCM_UMI_ECC_WRITE(numEccBytes, 11, &oobp[5],
-			(eccVal >> 16) & 0xff);	/* ECC10 */
-	}
-
-	/* Fill in the remainder of ECC locations */
-	NAND_BCM_UMI_ECC_WRITE(numEccBytes, 10, &oobp[6],
-		(eccVal >> 8) & 0xff);	/* ECC9 */
-	NAND_BCM_UMI_ECC_WRITE(numEccBytes, 9, &oobp[7],
-		eccVal & 0xff);	/* ECC8 */
-
-	if (numEccBytes >= 5)
-		eccVal = readl(&REG_UMI_BCH_WR_ECC_1);
-
-	NAND_BCM_UMI_ECC_WRITE(numEccBytes, 8, &oobp[8],
-		(eccVal >> 24) & 0xff);	/* ECC7 */
-	NAND_BCM_UMI_ECC_WRITE(numEccBytes, 7, &oobp[9],
-		(eccVal >> 16) & 0xff);	/* ECC6 */
-	NAND_BCM_UMI_ECC_WRITE(numEccBytes, 6, &oobp[10],
-		(eccVal >> 8) & 0xff);	/* ECC5 */
-	NAND_BCM_UMI_ECC_WRITE(numEccBytes, 5, &oobp[11],
-		eccVal & 0xff);	/* ECC4 */
-
-	if (numEccBytes >= 1)
-		eccVal = readl(&REG_UMI_BCH_WR_ECC_0);
-
-	NAND_BCM_UMI_ECC_WRITE(numEccBytes, 4, &oobp[12],
-		(eccVal >> 24) & 0xff);	/* ECC3 */
-	NAND_BCM_UMI_ECC_WRITE(numEccBytes, 3, &oobp[13],
-		(eccVal >> 16) & 0xff);	/* ECC2 */
-	NAND_BCM_UMI_ECC_WRITE(numEccBytes, 2, &oobp[14],
-		(eccVal >> 8) & 0xff);	/* ECC1 */
-	NAND_BCM_UMI_ECC_WRITE(numEccBytes, 1, &oobp[15],
-		eccVal & 0xff);	/* ECC0 */
-}
-#endif
-
-#endif /* NAND_BCM_UMI_H */
diff --git a/drivers/mtd/nand/nand_ids.c b/drivers/mtd/nand/nand_ids.c
index 621b70b7a15..e3aa2748a6e 100644
--- a/drivers/mtd/nand/nand_ids.c
+++ b/drivers/mtd/nand/nand_ids.c
@@ -70,7 +70,7 @@ struct nand_flash_dev nand_flash_ids[] = {
 	 * These are the new chips with large page size. The pagesize and the
 	 * erasesize is determined from the extended id bytes
 	 */
-#define LP_OPTIONS (NAND_SAMSUNG_LP_OPTIONS | NAND_NO_READRDY)
+#define LP_OPTIONS NAND_SAMSUNG_LP_OPTIONS
 #define LP_OPTIONS16 (LP_OPTIONS | NAND_BUSWIDTH_16)
 
 	/* 512 Megabit */
@@ -157,7 +157,7 @@ struct nand_flash_dev nand_flash_ids[] = {
 	 * writes possible, but not implemented now
 	 */
 	{"AND 128MiB 3,3V 8-bit",	0x01, 2048, 128, 0x4000,
-	 NAND_IS_AND | NAND_NO_READRDY | NAND_4PAGE_ARRAY | BBT_AUTO_REFRESH},
+	 NAND_IS_AND | NAND_4PAGE_ARRAY | BBT_AUTO_REFRESH},
 
 	{NULL,}
 };
@@ -174,8 +174,9 @@ struct nand_manufacturers nand_manuf_ids[] = {
 	{NAND_MFR_STMICRO, "ST Micro"},
 	{NAND_MFR_HYNIX, "Hynix"},
 	{NAND_MFR_MICRON, "Micron"},
-	{NAND_MFR_AMD, "AMD"},
+	{NAND_MFR_AMD, "AMD/Spansion"},
 	{NAND_MFR_MACRONIX, "Macronix"},
+	{NAND_MFR_EON, "Eon"},
 	{0x0, "Unknown"}
 };
 
diff --git a/drivers/mtd/nand/nandsim.c b/drivers/mtd/nand/nandsim.c
index cf0cd314681..a932c485eb0 100644
--- a/drivers/mtd/nand/nandsim.c
+++ b/drivers/mtd/nand/nandsim.c
@@ -447,8 +447,6 @@ static unsigned int rptwear_cnt = 0;
 /* MTD structure for NAND controller */
 static struct mtd_info *nsmtd;
 
-static u_char ns_verify_buf[NS_LARGEST_PAGE_SIZE];
-
 /*
  * Allocate array of page pointers, create slab allocation for an array
  * and initialize the array by NULL pointers.
@@ -2189,19 +2187,6 @@ static void ns_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 	return;
 }
 
-static int ns_nand_verify_buf(struct mtd_info *mtd, const u_char *buf, int len)
-{
-	ns_nand_read_buf(mtd, (u_char *)&ns_verify_buf[0], len);
-
-	if (!memcmp(buf, &ns_verify_buf[0], len)) {
-		NS_DBG("verify_buf: the buffer is OK\n");
-		return 0;
-	} else {
-		NS_DBG("verify_buf: the buffer is wrong\n");
-		return -EFAULT;
-	}
-}
-
 /*
  * Module initialization function
  */
@@ -2236,7 +2221,6 @@ static int __init ns_init_module(void)
 	chip->dev_ready  = ns_device_ready;
 	chip->write_buf  = ns_nand_write_buf;
 	chip->read_buf   = ns_nand_read_buf;
-	chip->verify_buf = ns_nand_verify_buf;
 	chip->read_word  = ns_nand_read_word;
 	chip->ecc.mode   = NAND_ECC_SOFT;
 	/* The NAND_SKIP_BBTSCAN option is necessary for 'overridesize' */
@@ -2333,6 +2317,7 @@ static int __init ns_init_module(void)
 		uint64_t new_size = (uint64_t)nsmtd->erasesize << overridesize;
 		if (new_size >> overridesize != nsmtd->erasesize) {
 			NS_ERR("overridesize is too big\n");
+			retval = -EINVAL;
 			goto err_exit;
 		}
 		/* N.B. This relies on nand_scan not doing anything with the size before we change it */
diff --git a/drivers/mtd/nand/ndfc.c b/drivers/mtd/nand/ndfc.c
index 2b6f632cf27..5fd3f010e3a 100644
--- a/drivers/mtd/nand/ndfc.c
+++ b/drivers/mtd/nand/ndfc.c
@@ -140,18 +140,6 @@ static void ndfc_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 		out_be32(ndfc->ndfcbase + NDFC_DATA, *p++);
 }
 
-static int ndfc_verify_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
-{
-	struct nand_chip *chip = mtd->priv;
-	struct ndfc_controller *ndfc = chip->priv;
-	uint32_t *p = (uint32_t *) buf;
-
-	for(;len > 0; len -= 4)
-		if (*p++ != in_be32(ndfc->ndfcbase + NDFC_DATA))
-			return -EFAULT;
-	return 0;
-}
-
 /*
  * Initialize chip structure
  */
@@ -172,7 +160,6 @@ static int ndfc_chip_init(struct ndfc_controller *ndfc,
 	chip->controller = &ndfc->ndfc_control;
 	chip->read_buf = ndfc_read_buf;
 	chip->write_buf = ndfc_write_buf;
-	chip->verify_buf = ndfc_verify_buf;
 	chip->ecc.correct = nand_correct_data;
 	chip->ecc.hwctl = ndfc_enable_hwecc;
 	chip->ecc.calculate = ndfc_calculate_ecc;
diff --git a/drivers/mtd/nand/nuc900_nand.c b/drivers/mtd/nand/nuc900_nand.c
index 8febe46e110..94dc46bc118 100644
--- a/drivers/mtd/nand/nuc900_nand.c
+++ b/drivers/mtd/nand/nuc900_nand.c
@@ -112,22 +112,6 @@ static void nuc900_nand_write_buf(struct mtd_info *mtd,
 		write_data_reg(nand, buf[i]);
 }
 
-static int nuc900_verify_buf(struct mtd_info *mtd,
-			     const unsigned char *buf, int len)
-{
-	int i;
-	struct nuc900_nand *nand;
-
-	nand = container_of(mtd, struct nuc900_nand, mtd);
-
-	for (i = 0; i < len; i++) {
-		if (buf[i] != (unsigned char)read_data_reg(nand))
-			return -EFAULT;
-	}
-
-	return 0;
-}
-
 static int nuc900_check_rb(struct nuc900_nand *nand)
 {
 	unsigned int val;
@@ -292,7 +276,6 @@ static int __devinit nuc900_nand_probe(struct platform_device *pdev)
 	chip->read_byte		= nuc900_nand_read_byte;
 	chip->write_buf		= nuc900_nand_write_buf;
 	chip->read_buf		= nuc900_nand_read_buf;
-	chip->verify_buf	= nuc900_verify_buf;
 	chip->chip_delay	= 50;
 	chip->options		= 0;
 	chip->ecc.mode		= NAND_ECC_SOFT;
diff --git a/drivers/mtd/nand/omap2.c b/drivers/mtd/nand/omap2.c
index fc8111278d1..5b313862064 100644
--- a/drivers/mtd/nand/omap2.c
+++ b/drivers/mtd/nand/omap2.c
@@ -425,7 +425,7 @@ static void omap_nand_dma_callback(void *data)
 }
 
 /*
- * omap_nand_dma_transfer: configer and start dma transfer
+ * omap_nand_dma_transfer: configure and start dma transfer
  * @mtd: MTD device structure
  * @addr: virtual address in RAM of source/destination
  * @len: number of data bytes to be transferred
@@ -546,7 +546,7 @@ static void omap_write_buf_dma_pref(struct mtd_info *mtd,
 }
 
 /*
- * omap_nand_irq - GMPC irq handler
+ * omap_nand_irq - GPMC irq handler
  * @this_irq: gpmc irq number
  * @dev: omap_nand_info structure pointer is passed here
  */
@@ -698,27 +698,6 @@ out_copy:
 }
 
 /**
- * omap_verify_buf - Verify chip data against buffer
- * @mtd: MTD device structure
- * @buf: buffer containing the data to compare
- * @len: number of bytes to compare
- */
-static int omap_verify_buf(struct mtd_info *mtd, const u_char * buf, int len)
-{
-	struct omap_nand_info *info = container_of(mtd, struct omap_nand_info,
-							mtd);
-	u16 *p = (u16 *) buf;
-
-	len >>= 1;
-	while (len--) {
-		if (*p++ != cpu_to_le16(readw(info->nand.IO_ADDR_R)))
-			return -EFAULT;
-	}
-
-	return 0;
-}
-
-/**
  * gen_true_ecc - This function will generate true ECC value
  * @ecc_buf: buffer to store ecc code
  *
@@ -1326,8 +1305,8 @@ static int __devinit omap_nand_probe(struct platform_device *pdev)
 
 	/*
 	 * If RDY/BSY line is connected to OMAP then use the omap ready
-	 * funcrtion and the generic nand_wait function which reads the status
-	 * register after monitoring the RDY/BSY line.Otherwise use a standard
+	 * function and the generic nand_wait function which reads the status
+	 * register after monitoring the RDY/BSY line. Otherwise use a standard
 	 * chip delay which is slightly more than tR (AC Timing) of the NAND
 	 * device and read status register until you get a failure or success
 	 */
@@ -1428,9 +1407,7 @@ static int __devinit omap_nand_probe(struct platform_device *pdev)
 		goto out_release_mem_region;
 	}
 
-	info->nand.verify_buf = omap_verify_buf;
-
-	/* selsect the ecc type */
+	/* select the ecc type */
 	if (pdata->ecc_opt == OMAP_ECC_HAMMING_CODE_DEFAULT)
 		info->nand.ecc.mode = NAND_ECC_SOFT;
 	else if ((pdata->ecc_opt == OMAP_ECC_HAMMING_CODE_HW) ||
@@ -1536,7 +1513,8 @@ static int omap_nand_remove(struct platform_device *pdev)
 	/* Release NAND device, its internal structures and partitions */
 	nand_release(&info->mtd);
 	iounmap(info->nand.IO_ADDR_R);
-	kfree(&info->mtd);
+	release_mem_region(info->phys_base, NAND_IO_SIZE);
+	kfree(info);
 	return 0;
 }
 
diff --git a/drivers/mtd/nand/orion_nand.c b/drivers/mtd/nand/orion_nand.c
index 131b58a133f..aefaf8cd31e 100644
--- a/drivers/mtd/nand/orion_nand.c
+++ b/drivers/mtd/nand/orion_nand.c
@@ -21,7 +21,6 @@
 #include <linux/err.h>
 #include <asm/io.h>
 #include <asm/sizes.h>
-#include <mach/hardware.h>
 #include <linux/platform_data/mtd-orion_nand.h>
 
 static void orion_nand_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl)
diff --git a/drivers/mtd/nand/plat_nand.c b/drivers/mtd/nand/plat_nand.c
index 1bcb5204042..a47ee68a0cf 100644
--- a/drivers/mtd/nand/plat_nand.c
+++ b/drivers/mtd/nand/plat_nand.c
@@ -37,6 +37,11 @@ static int __devinit plat_nand_probe(struct platform_device *pdev)
 	const char **part_types;
 	int err = 0;
 
+	if (!pdata) {
+		dev_err(&pdev->dev, "platform_nand_data is missing\n");
+		return -EINVAL;
+	}
+
 	if (pdata->chip.nr_chips < 1) {
 		dev_err(&pdev->dev, "invalid number of chips specified\n");
 		return -EINVAL;
diff --git a/drivers/mtd/nand/pxa3xx_nand.c b/drivers/mtd/nand/pxa3xx_nand.c
index c45227173ef..37ee75c7bac 100644
--- a/drivers/mtd/nand/pxa3xx_nand.c
+++ b/drivers/mtd/nand/pxa3xx_nand.c
@@ -683,11 +683,13 @@ static void pxa3xx_nand_cmdfunc(struct mtd_info *mtd, unsigned command,
 	info->state = STATE_IDLE;
 }
 
-static void pxa3xx_nand_write_page_hwecc(struct mtd_info *mtd,
+static int pxa3xx_nand_write_page_hwecc(struct mtd_info *mtd,
 		struct nand_chip *chip, const uint8_t *buf, int oob_required)
 {
 	chip->write_buf(mtd, buf, mtd->writesize);
 	chip->write_buf(mtd, chip->oob_poi, mtd->oobsize);
+
+	return 0;
 }
 
 static int pxa3xx_nand_read_page_hwecc(struct mtd_info *mtd,
@@ -771,12 +773,6 @@ static void pxa3xx_nand_write_buf(struct mtd_info *mtd,
 	info->buf_start += real_len;
 }
 
-static int pxa3xx_nand_verify_buf(struct mtd_info *mtd,
-		const uint8_t *buf, int len)
-{
-	return 0;
-}
-
 static void pxa3xx_nand_select_chip(struct mtd_info *mtd, int chip)
 {
 	return;
@@ -1007,7 +1003,6 @@ KEEP_CONFIG:
 	chip->ecc.size = host->page_size;
 	chip->ecc.strength = 1;
 
-	chip->options |= NAND_NO_READRDY;
 	if (host->reg_ndcr & NDCR_DWIDTH_M)
 		chip->options |= NAND_BUSWIDTH_16;
 
@@ -1070,7 +1065,6 @@ static int alloc_nand_resource(struct platform_device *pdev)
 		chip->read_byte		= pxa3xx_nand_read_byte;
 		chip->read_buf		= pxa3xx_nand_read_buf;
 		chip->write_buf		= pxa3xx_nand_write_buf;
-		chip->verify_buf	= pxa3xx_nand_verify_buf;
 	}
 
 	spin_lock_init(&chip->controller->lock);
diff --git a/drivers/mtd/nand/r852.c b/drivers/mtd/nand/r852.c
index 8cb627751c9..4495f8551fa 100644
--- a/drivers/mtd/nand/r852.c
+++ b/drivers/mtd/nand/r852.c
@@ -309,27 +309,6 @@ static uint8_t r852_read_byte(struct mtd_info *mtd)
 	return r852_read_reg(dev, R852_DATALINE);
 }
 
-
-/*
- * Readback the buffer to verify it
- */
-int r852_verify_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
-{
-	struct r852_device *dev = r852_get_dev(mtd);
-
-	/* We can't be sure about anything here... */
-	if (dev->card_unstable)
-		return -1;
-
-	/* This will never happen, unless you wired up a nand chip
-		with > 512 bytes page size to the reader */
-	if (len > SM_SECTOR_SIZE)
-		return 0;
-
-	r852_read_buf(mtd, dev->tmp_buffer, len);
-	return memcmp(buf, dev->tmp_buffer, len);
-}
-
 /*
  * Control several chip lines & send commands
  */
@@ -882,7 +861,6 @@ int  r852_probe(struct pci_dev *pci_dev, const struct pci_device_id *id)
 	chip->read_byte = r852_read_byte;
 	chip->read_buf = r852_read_buf;
 	chip->write_buf = r852_write_buf;
-	chip->verify_buf = r852_verify_buf;
 
 	/* ecc */
 	chip->ecc.mode = NAND_ECC_HW_SYNDROME;
diff --git a/drivers/mtd/nand/s3c2410.c b/drivers/mtd/nand/s3c2410.c
index d8040619ad8..295e4bedad9 100644
--- a/drivers/mtd/nand/s3c2410.c
+++ b/drivers/mtd/nand/s3c2410.c
@@ -21,6 +21,8 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */
 
+#define pr_fmt(fmt) "nand-s3c2410: " fmt
+
 #ifdef CONFIG_MTD_NAND_S3C2410_DEBUG
 #define DEBUG
 #endif
@@ -30,6 +32,7 @@
 #include <linux/init.h>
 #include <linux/kernel.h>
 #include <linux/string.h>
+#include <linux/io.h>
 #include <linux/ioport.h>
 #include <linux/platform_device.h>
 #include <linux/delay.h>
@@ -43,24 +46,9 @@
 #include <linux/mtd/nand_ecc.h>
 #include <linux/mtd/partitions.h>
 
-#include <asm/io.h>
-
 #include <plat/regs-nand.h>
 #include <linux/platform_data/mtd-nand-s3c2410.h>
 
-#ifdef CONFIG_MTD_NAND_S3C2410_HWECC
-static int hardware_ecc = 1;
-#else
-static int hardware_ecc = 0;
-#endif
-
-#ifdef CONFIG_MTD_NAND_S3C2410_CLKSTOP
-static const int clock_stop = 1;
-#else
-static const int clock_stop = 0;
-#endif
-
-
 /* new oob placement block for use with hardware ecc generation
  */
 
@@ -109,9 +97,8 @@ enum s3c_nand_clk_state {
  * @mtds: An array of MTD instances on this controoler.
  * @platform: The platform data for this board.
  * @device: The platform device we bound to.
- * @area: The IO area resource that came from request_mem_region().
  * @clk: The clock resource for this controller.
- * @regs: The area mapped for the hardware registers described by @area.
+ * @regs: The area mapped for the hardware registers.
  * @sel_reg: Pointer to the register controlling the NAND selection.
  * @sel_bit: The bit in @sel_reg to select the NAND chip.
  * @mtd_count: The number of MTDs created from this controller.
@@ -128,7 +115,6 @@ struct s3c2410_nand_info {
 
 	/* device info */
 	struct device			*device;
-	struct resource			*area;
 	struct clk			*clk;
 	void __iomem			*regs;
 	void __iomem			*sel_reg;
@@ -169,7 +155,11 @@ static struct s3c2410_platform_nand *to_nand_plat(struct platform_device *dev)
 
 static inline int allow_clk_suspend(struct s3c2410_nand_info *info)
 {
-	return clock_stop;
+#ifdef CONFIG_MTD_NAND_S3C2410_CLKSTOP
+	return 1;
+#else
+	return 0;
+#endif
 }
 
 /**
@@ -215,7 +205,8 @@ static int s3c_nand_calc_rate(int wanted, unsigned long clk, int max)
 	pr_debug("result %d from %ld, %d\n", result, clk, wanted);
 
 	if (result > max) {
-		printk("%d ns is too big for current clock rate %ld\n", wanted, clk);
+		pr_err("%d ns is too big for current clock rate %ld\n",
+			wanted, clk);
 		return -1;
 	}
 
@@ -225,7 +216,7 @@ static int s3c_nand_calc_rate(int wanted, unsigned long clk, int max)
 	return result;
 }
 
-#define to_ns(ticks,clk) (((ticks) * NS_IN_KHZ) / (unsigned int)(clk))
+#define to_ns(ticks, clk) (((ticks) * NS_IN_KHZ) / (unsigned int)(clk))
 
 /* controller setup */
 
@@ -268,7 +259,8 @@ static int s3c2410_nand_setrate(struct s3c2410_nand_info *info)
 	}
 
 	dev_info(info->device, "Tacls=%d, %dns Twrph0=%d %dns, Twrph1=%d %dns\n",
-	       tacls, to_ns(tacls, clkrate), twrph0, to_ns(twrph0, clkrate), twrph1, to_ns(twrph1, clkrate));
+		tacls, to_ns(tacls, clkrate), twrph0, to_ns(twrph0, clkrate),
+						twrph1, to_ns(twrph1, clkrate));
 
 	switch (info->cpu_type) {
 	case TYPE_S3C2410:
@@ -325,13 +317,13 @@ static int s3c2410_nand_inithw(struct s3c2410_nand_info *info)
 	if (ret < 0)
 		return ret;
 
- 	switch (info->cpu_type) {
- 	case TYPE_S3C2410:
+	switch (info->cpu_type) {
+	case TYPE_S3C2410:
 	default:
 		break;
 
- 	case TYPE_S3C2440:
- 	case TYPE_S3C2412:
+	case TYPE_S3C2440:
+	case TYPE_S3C2412:
 		/* enable the controller and de-assert nFCE */
 
 		writel(S3C2440_NFCONT_ENABLE, info->regs + S3C2440_NFCONT);
@@ -450,6 +442,7 @@ static int s3c2412_nand_devready(struct mtd_info *mtd)
 
 /* ECC handling functions */
 
+#ifdef CONFIG_MTD_NAND_S3C2410_HWECC
 static int s3c2410_nand_correct_data(struct mtd_info *mtd, u_char *dat,
 				     u_char *read_ecc, u_char *calc_ecc)
 {
@@ -463,10 +456,8 @@ static int s3c2410_nand_correct_data(struct mtd_info *mtd, u_char *dat,
 	diff1 = read_ecc[1] ^ calc_ecc[1];
 	diff2 = read_ecc[2] ^ calc_ecc[2];
 
-	pr_debug("%s: rd %02x%02x%02x calc %02x%02x%02x diff %02x%02x%02x\n",
-		 __func__,
-		 read_ecc[0], read_ecc[1], read_ecc[2],
-		 calc_ecc[0], calc_ecc[1], calc_ecc[2],
+	pr_debug("%s: rd %*phN calc %*phN diff %02x%02x%02x\n",
+		 __func__, 3, read_ecc, 3, calc_ecc,
 		 diff0, diff1, diff2);
 
 	if (diff0 == 0 && diff1 == 0 && diff2 == 0)
@@ -546,7 +537,8 @@ static void s3c2412_nand_enable_hwecc(struct mtd_info *mtd, int mode)
 	unsigned long ctrl;
 
 	ctrl = readl(info->regs + S3C2440_NFCONT);
-	writel(ctrl | S3C2412_NFCONT_INIT_MAIN_ECC, info->regs + S3C2440_NFCONT);
+	writel(ctrl | S3C2412_NFCONT_INIT_MAIN_ECC,
+	       info->regs + S3C2440_NFCONT);
 }
 
 static void s3c2440_nand_enable_hwecc(struct mtd_info *mtd, int mode)
@@ -558,7 +550,8 @@ static void s3c2440_nand_enable_hwecc(struct mtd_info *mtd, int mode)
 	writel(ctrl | S3C2440_NFCONT_INITECC, info->regs + S3C2440_NFCONT);
 }
 
-static int s3c2410_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat, u_char *ecc_code)
+static int s3c2410_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat,
+				      u_char *ecc_code)
 {
 	struct s3c2410_nand_info *info = s3c2410_nand_mtd_toinfo(mtd);
 
@@ -566,13 +559,13 @@ static int s3c2410_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat, u
 	ecc_code[1] = readb(info->regs + S3C2410_NFECC + 1);
 	ecc_code[2] = readb(info->regs + S3C2410_NFECC + 2);
 
-	pr_debug("%s: returning ecc %02x%02x%02x\n", __func__,
-		 ecc_code[0], ecc_code[1], ecc_code[2]);
+	pr_debug("%s: returning ecc %*phN\n", __func__, 3, ecc_code);
 
 	return 0;
 }
 
-static int s3c2412_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat, u_char *ecc_code)
+static int s3c2412_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat,
+				      u_char *ecc_code)
 {
 	struct s3c2410_nand_info *info = s3c2410_nand_mtd_toinfo(mtd);
 	unsigned long ecc = readl(info->regs + S3C2412_NFMECC0);
@@ -581,12 +574,13 @@ static int s3c2412_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat, u
 	ecc_code[1] = ecc >> 8;
 	ecc_code[2] = ecc >> 16;
 
-	pr_debug("calculate_ecc: returning ecc %02x,%02x,%02x\n", ecc_code[0], ecc_code[1], ecc_code[2]);
+	pr_debug("%s: returning ecc %*phN\n", __func__, 3, ecc_code);
 
 	return 0;
 }
 
-static int s3c2440_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat, u_char *ecc_code)
+static int s3c2440_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat,
+				      u_char *ecc_code)
 {
 	struct s3c2410_nand_info *info = s3c2410_nand_mtd_toinfo(mtd);
 	unsigned long ecc = readl(info->regs + S3C2440_NFMECC0);
@@ -599,6 +593,7 @@ static int s3c2440_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat, u
 
 	return 0;
 }
+#endif
 
 /* over-ride the standard functions for a little more speed. We can
  * use read/write block to move the data buffers to/from the controller
@@ -625,13 +620,15 @@ static void s3c2440_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 	}
 }
 
-static void s3c2410_nand_write_buf(struct mtd_info *mtd, const u_char *buf, int len)
+static void s3c2410_nand_write_buf(struct mtd_info *mtd, const u_char *buf,
+				   int len)
 {
 	struct nand_chip *this = mtd->priv;
 	writesb(this->IO_ADDR_W, buf, len);
 }
 
-static void s3c2440_nand_write_buf(struct mtd_info *mtd, const u_char *buf, int len)
+static void s3c2440_nand_write_buf(struct mtd_info *mtd, const u_char *buf,
+				   int len)
 {
 	struct s3c2410_nand_info *info = s3c2410_nand_mtd_toinfo(mtd);
 
@@ -675,7 +672,8 @@ static inline int s3c2410_nand_cpufreq_register(struct s3c2410_nand_info *info)
 					 CPUFREQ_TRANSITION_NOTIFIER);
 }
 
-static inline void s3c2410_nand_cpufreq_deregister(struct s3c2410_nand_info *info)
+static inline void
+s3c2410_nand_cpufreq_deregister(struct s3c2410_nand_info *info)
 {
 	cpufreq_unregister_notifier(&info->freq_transition,
 				    CPUFREQ_TRANSITION_NOTIFIER);
@@ -687,7 +685,8 @@ static inline int s3c2410_nand_cpufreq_register(struct s3c2410_nand_info *info)
 	return 0;
 }
 
-static inline void s3c2410_nand_cpufreq_deregister(struct s3c2410_nand_info *info)
+static inline void
+s3c2410_nand_cpufreq_deregister(struct s3c2410_nand_info *info)
 {
 }
 #endif
@@ -717,29 +716,12 @@ static int s3c24xx_nand_remove(struct platform_device *pdev)
 			pr_debug("releasing mtd %d (%p)\n", mtdno, ptr);
 			nand_release(&ptr->mtd);
 		}
-
-		kfree(info->mtds);
 	}
 
 	/* free the common resources */
 
-	if (!IS_ERR(info->clk)) {
+	if (!IS_ERR(info->clk))
 		s3c2410_nand_clk_set_state(info, CLOCK_DISABLE);
-		clk_put(info->clk);
-	}
-
-	if (info->regs != NULL) {
-		iounmap(info->regs);
-		info->regs = NULL;
-	}
-
-	if (info->area != NULL) {
-		release_resource(info->area);
-		kfree(info->area);
-		info->area = NULL;
-	}
-
-	kfree(info);
 
 	return 0;
 }
@@ -810,7 +792,7 @@ static void s3c2410_nand_init_chip(struct s3c2410_nand_info *info,
 			dev_info(info->device, "System booted from NAND\n");
 
 		break;
-  	}
+	}
 
 	chip->IO_ADDR_R = chip->IO_ADDR_W;
 
@@ -819,32 +801,31 @@ static void s3c2410_nand_init_chip(struct s3c2410_nand_info *info,
 	nmtd->mtd.owner    = THIS_MODULE;
 	nmtd->set	   = set;
 
-	if (hardware_ecc) {
+#ifdef CONFIG_MTD_NAND_S3C2410_HWECC
+	chip->ecc.calculate = s3c2410_nand_calculate_ecc;
+	chip->ecc.correct   = s3c2410_nand_correct_data;
+	chip->ecc.mode	    = NAND_ECC_HW;
+	chip->ecc.strength  = 1;
+
+	switch (info->cpu_type) {
+	case TYPE_S3C2410:
+		chip->ecc.hwctl	    = s3c2410_nand_enable_hwecc;
 		chip->ecc.calculate = s3c2410_nand_calculate_ecc;
-		chip->ecc.correct   = s3c2410_nand_correct_data;
-		chip->ecc.mode	    = NAND_ECC_HW;
-		chip->ecc.strength  = 1;
-
-		switch (info->cpu_type) {
-		case TYPE_S3C2410:
-			chip->ecc.hwctl	    = s3c2410_nand_enable_hwecc;
-			chip->ecc.calculate = s3c2410_nand_calculate_ecc;
-			break;
-
-		case TYPE_S3C2412:
-  			chip->ecc.hwctl     = s3c2412_nand_enable_hwecc;
-  			chip->ecc.calculate = s3c2412_nand_calculate_ecc;
-			break;
-
-		case TYPE_S3C2440:
-  			chip->ecc.hwctl     = s3c2440_nand_enable_hwecc;
-  			chip->ecc.calculate = s3c2440_nand_calculate_ecc;
-			break;
+		break;
 
-		}
-	} else {
-		chip->ecc.mode	    = NAND_ECC_SOFT;
+	case TYPE_S3C2412:
+		chip->ecc.hwctl     = s3c2412_nand_enable_hwecc;
+		chip->ecc.calculate = s3c2412_nand_calculate_ecc;
+		break;
+
+	case TYPE_S3C2440:
+		chip->ecc.hwctl     = s3c2440_nand_enable_hwecc;
+		chip->ecc.calculate = s3c2440_nand_calculate_ecc;
+		break;
 	}
+#else
+	chip->ecc.mode	    = NAND_ECC_SOFT;
+#endif
 
 	if (set->ecc_layout != NULL)
 		chip->ecc.layout = set->ecc_layout;
@@ -921,7 +902,7 @@ static void s3c2410_nand_update_chip(struct s3c2410_nand_info *info,
 static int s3c24xx_nand_probe(struct platform_device *pdev)
 {
 	struct s3c2410_platform_nand *plat = to_nand_plat(pdev);
-	enum s3c_cpu_type cpu_type; 
+	enum s3c_cpu_type cpu_type;
 	struct s3c2410_nand_info *info;
 	struct s3c2410_nand_mtd *nmtd;
 	struct s3c2410_nand_set *sets;
@@ -935,7 +916,7 @@ static int s3c24xx_nand_probe(struct platform_device *pdev)
 
 	pr_debug("s3c2410_nand_probe(%p)\n", pdev);
 
-	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	info = devm_kzalloc(&pdev->dev, sizeof(*info), GFP_KERNEL);
 	if (info == NULL) {
 		dev_err(&pdev->dev, "no memory for flash info\n");
 		err = -ENOMEM;
@@ -949,7 +930,7 @@ static int s3c24xx_nand_probe(struct platform_device *pdev)
 
 	/* get the clock source and enable it */
 
-	info->clk = clk_get(&pdev->dev, "nand");
+	info->clk = devm_clk_get(&pdev->dev, "nand");
 	if (IS_ERR(info->clk)) {
 		dev_err(&pdev->dev, "failed to get clock\n");
 		err = -ENOENT;
@@ -961,22 +942,14 @@ static int s3c24xx_nand_probe(struct platform_device *pdev)
 	/* allocate and map the resource */
 
 	/* currently we assume we have the one resource */
-	res  = pdev->resource;
+	res = pdev->resource;
 	size = resource_size(res);
 
-	info->area = request_mem_region(res->start, size, pdev->name);
-
-	if (info->area == NULL) {
-		dev_err(&pdev->dev, "cannot reserve register region\n");
-		err = -ENOENT;
-		goto exit_error;
-	}
-
-	info->device     = &pdev->dev;
-	info->platform   = plat;
-	info->regs       = ioremap(res->start, size);
-	info->cpu_type   = cpu_type;
+	info->device	= &pdev->dev;
+	info->platform	= plat;
+	info->cpu_type	= cpu_type;
 
+	info->regs	= devm_request_and_ioremap(&pdev->dev, res);
 	if (info->regs == NULL) {
 		dev_err(&pdev->dev, "cannot reserve register region\n");
 		err = -EIO;
@@ -999,7 +972,7 @@ static int s3c24xx_nand_probe(struct platform_device *pdev)
 	/* allocate our information */
 
 	size = nr_sets * sizeof(*info->mtds);
-	info->mtds = kzalloc(size, GFP_KERNEL);
+	info->mtds = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
 	if (info->mtds == NULL) {
 		dev_err(&pdev->dev, "failed to allocate mtd storage\n");
 		err = -ENOMEM;
@@ -1011,7 +984,8 @@ static int s3c24xx_nand_probe(struct platform_device *pdev)
 	nmtd = info->mtds;
 
 	for (setno = 0; setno < nr_sets; setno++, nmtd++) {
-		pr_debug("initialising set %d (%p, info %p)\n", setno, nmtd, info);
+		pr_debug("initialising set %d (%p, info %p)\n",
+			 setno, nmtd, info);
 
 		s3c2410_nand_init_chip(info, nmtd, sets);
 
@@ -1134,20 +1108,7 @@ static struct platform_driver s3c24xx_nand_driver = {
 	},
 };
 
-static int __init s3c2410_nand_init(void)
-{
-	printk("S3C24XX NAND Driver, (c) 2004 Simtec Electronics\n");
-
-	return platform_driver_register(&s3c24xx_nand_driver);
-}
-
-static void __exit s3c2410_nand_exit(void)
-{
-	platform_driver_unregister(&s3c24xx_nand_driver);
-}
-
-module_init(s3c2410_nand_init);
-module_exit(s3c2410_nand_exit);
+module_platform_driver(s3c24xx_nand_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Ben Dooks <ben@simtec.co.uk>");
diff --git a/drivers/mtd/nand/sh_flctl.c b/drivers/mtd/nand/sh_flctl.c
index aa9b8a5e0b8..4fbfe96e37a 100644
--- a/drivers/mtd/nand/sh_flctl.c
+++ b/drivers/mtd/nand/sh_flctl.c
@@ -24,10 +24,12 @@
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/delay.h>
+#include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/slab.h>
+#include <linux/string.h>
 
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/nand.h>
@@ -43,11 +45,17 @@ static struct nand_ecclayout flctl_4secc_oob_16 = {
 };
 
 static struct nand_ecclayout flctl_4secc_oob_64 = {
-	.eccbytes = 10,
-	.eccpos = {48, 49, 50, 51, 52, 53, 54, 55, 56, 57},
+	.eccbytes = 4 * 10,
+	.eccpos = {
+		 6,  7,  8,  9, 10, 11, 12, 13, 14, 15,
+		22, 23, 24, 25, 26, 27, 28, 29, 30, 31,
+		38, 39, 40, 41, 42, 43, 44, 45, 46, 47,
+		54, 55, 56, 57, 58, 59, 60, 61, 62, 63 },
 	.oobfree = {
-		{.offset = 60,
-		. length = 4} },
+		{.offset =  2, .length = 4},
+		{.offset = 16, .length = 6},
+		{.offset = 32, .length = 6},
+		{.offset = 48, .length = 6} },
 };
 
 static uint8_t scan_ff_pattern[] = { 0xff, 0xff };
@@ -61,15 +69,15 @@ static struct nand_bbt_descr flctl_4secc_smallpage = {
 
 static struct nand_bbt_descr flctl_4secc_largepage = {
 	.options = NAND_BBT_SCAN2NDPAGE,
-	.offs = 58,
+	.offs = 0,
 	.len = 2,
 	.pattern = scan_ff_pattern,
 };
 
 static void empty_fifo(struct sh_flctl *flctl)
 {
-	writel(0x000c0000, FLINTDMACR(flctl));	/* FIFO Clear */
-	writel(0x00000000, FLINTDMACR(flctl));	/* Clear Error flags */
+	writel(flctl->flintdmacr_base | AC1CLR | AC0CLR, FLINTDMACR(flctl));
+	writel(flctl->flintdmacr_base, FLINTDMACR(flctl));
 }
 
 static void start_translation(struct sh_flctl *flctl)
@@ -158,27 +166,56 @@ static void wait_wfifo_ready(struct sh_flctl *flctl)
 	timeout_error(flctl, __func__);
 }
 
-static int wait_recfifo_ready(struct sh_flctl *flctl, int sector_number)
+static enum flctl_ecc_res_t wait_recfifo_ready
+		(struct sh_flctl *flctl, int sector_number)
 {
 	uint32_t timeout = LOOP_TIMEOUT_MAX;
-	int checked[4];
 	void __iomem *ecc_reg[4];
 	int i;
+	int state = FL_SUCCESS;
 	uint32_t data, size;
 
-	memset(checked, 0, sizeof(checked));
-
+	/*
+	 * First this loops checks in FLDTCNTR if we are ready to read out the
+	 * oob data. This is the case if either all went fine without errors or
+	 * if the bottom part of the loop corrected the errors or marked them as
+	 * uncorrectable and the controller is given time to push the data into
+	 * the FIFO.
+	 */
 	while (timeout--) {
+		/* check if all is ok and we can read out the OOB */
 		size = readl(FLDTCNTR(flctl)) >> 24;
-		if (size & 0xFF)
-			return 0;	/* success */
+		if ((size & 0xFF) == 4)
+			return state;
+
+		/* check if a correction code has been calculated */
+		if (!(readl(FL4ECCCR(flctl)) & _4ECCEND)) {
+			/*
+			 * either we wait for the fifo to be filled or a
+			 * correction pattern is being generated
+			 */
+			udelay(1);
+			continue;
+		}
 
-		if (readl(FL4ECCCR(flctl)) & _4ECCFA)
-			return 1;	/* can't correct */
+		/* check for an uncorrectable error */
+		if (readl(FL4ECCCR(flctl)) & _4ECCFA) {
+			/* check if we face a non-empty page */
+			for (i = 0; i < 512; i++) {
+				if (flctl->done_buff[i] != 0xff) {
+					state = FL_ERROR; /* can't correct */
+					break;
+				}
+			}
 
-		udelay(1);
-		if (!(readl(FL4ECCCR(flctl)) & _4ECCEND))
+			if (state == FL_SUCCESS)
+				dev_dbg(&flctl->pdev->dev,
+				"reading empty sector %d, ecc error ignored\n",
+				sector_number);
+
+			writel(0, FL4ECCCR(flctl));
 			continue;
+		}
 
 		/* start error correction */
 		ecc_reg[0] = FL4ECCRESULT0(flctl);
@@ -187,28 +224,26 @@ static int wait_recfifo_ready(struct sh_flctl *flctl, int sector_number)
 		ecc_reg[3] = FL4ECCRESULT3(flctl);
 
 		for (i = 0; i < 3; i++) {
+			uint8_t org;
+			int index;
+
 			data = readl(ecc_reg[i]);
-			if (data != INIT_FL4ECCRESULT_VAL && !checked[i]) {
-				uint8_t org;
-				int index;
-
-				if (flctl->page_size)
-					index = (512 * sector_number) +
-						(data >> 16);
-				else
-					index = data >> 16;
-
-				org = flctl->done_buff[index];
-				flctl->done_buff[index] = org ^ (data & 0xFF);
-				checked[i] = 1;
-			}
-		}
 
+			if (flctl->page_size)
+				index = (512 * sector_number) +
+					(data >> 16);
+			else
+				index = data >> 16;
+
+			org = flctl->done_buff[index];
+			flctl->done_buff[index] = org ^ (data & 0xFF);
+		}
+		state = FL_REPAIRABLE;
 		writel(0, FL4ECCCR(flctl));
 	}
 
 	timeout_error(flctl, __func__);
-	return 1;	/* timeout */
+	return FL_TIMEOUT;	/* timeout */
 }
 
 static void wait_wecfifo_ready(struct sh_flctl *flctl)
@@ -241,31 +276,33 @@ static void read_fiforeg(struct sh_flctl *flctl, int rlen, int offset)
 {
 	int i, len_4align;
 	unsigned long *buf = (unsigned long *)&flctl->done_buff[offset];
-	void *fifo_addr = (void *)FLDTFIFO(flctl);
 
 	len_4align = (rlen + 3) / 4;
 
 	for (i = 0; i < len_4align; i++) {
 		wait_rfifo_ready(flctl);
-		buf[i] = readl(fifo_addr);
+		buf[i] = readl(FLDTFIFO(flctl));
 		buf[i] = be32_to_cpu(buf[i]);
 	}
 }
 
-static int read_ecfiforeg(struct sh_flctl *flctl, uint8_t *buff, int sector)
+static enum flctl_ecc_res_t read_ecfiforeg
+		(struct sh_flctl *flctl, uint8_t *buff, int sector)
 {
 	int i;
+	enum flctl_ecc_res_t res;
 	unsigned long *ecc_buf = (unsigned long *)buff;
-	void *fifo_addr = (void *)FLECFIFO(flctl);
 
-	for (i = 0; i < 4; i++) {
-		if (wait_recfifo_ready(flctl , sector))
-			return 1;
-		ecc_buf[i] = readl(fifo_addr);
-		ecc_buf[i] = be32_to_cpu(ecc_buf[i]);
+	res = wait_recfifo_ready(flctl , sector);
+
+	if (res != FL_ERROR) {
+		for (i = 0; i < 4; i++) {
+			ecc_buf[i] = readl(FLECFIFO(flctl));
+			ecc_buf[i] = be32_to_cpu(ecc_buf[i]);
+		}
 	}
 
-	return 0;
+	return res;
 }
 
 static void write_fiforeg(struct sh_flctl *flctl, int rlen, int offset)
@@ -281,6 +318,18 @@ static void write_fiforeg(struct sh_flctl *flctl, int rlen, int offset)
 	}
 }
 
+static void write_ec_fiforeg(struct sh_flctl *flctl, int rlen, int offset)
+{
+	int i, len_4align;
+	unsigned long *data = (unsigned long *)&flctl->done_buff[offset];
+
+	len_4align = (rlen + 3) / 4;
+	for (i = 0; i < len_4align; i++) {
+		wait_wecfifo_ready(flctl);
+		writel(cpu_to_be32(data[i]), FLECFIFO(flctl));
+	}
+}
+
 static void set_cmd_regs(struct mtd_info *mtd, uint32_t cmd, uint32_t flcmcdr_val)
 {
 	struct sh_flctl *flctl = mtd_to_flctl(mtd);
@@ -346,73 +395,65 @@ static void set_cmd_regs(struct mtd_info *mtd, uint32_t cmd, uint32_t flcmcdr_va
 static int flctl_read_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
 				uint8_t *buf, int oob_required, int page)
 {
-	int i, eccsize = chip->ecc.size;
-	int eccbytes = chip->ecc.bytes;
-	int eccsteps = chip->ecc.steps;
-	uint8_t *p = buf;
-	struct sh_flctl *flctl = mtd_to_flctl(mtd);
-
-	for (i = 0; eccsteps; eccsteps--, i += eccbytes, p += eccsize)
-		chip->read_buf(mtd, p, eccsize);
-
-	for (i = 0; eccsteps; eccsteps--, i += eccbytes, p += eccsize) {
-		if (flctl->hwecc_cant_correct[i])
-			mtd->ecc_stats.failed++;
-		else
-			mtd->ecc_stats.corrected += 0; /* FIXME */
-	}
-
+	chip->read_buf(mtd, buf, mtd->writesize);
+	if (oob_required)
+		chip->read_buf(mtd, chip->oob_poi, mtd->oobsize);
 	return 0;
 }
 
-static void flctl_write_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
+static int flctl_write_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
 				   const uint8_t *buf, int oob_required)
 {
-	int i, eccsize = chip->ecc.size;
-	int eccbytes = chip->ecc.bytes;
-	int eccsteps = chip->ecc.steps;
-	const uint8_t *p = buf;
-
-	for (i = 0; eccsteps; eccsteps--, i += eccbytes, p += eccsize)
-		chip->write_buf(mtd, p, eccsize);
+	chip->write_buf(mtd, buf, mtd->writesize);
+	chip->write_buf(mtd, chip->oob_poi, mtd->oobsize);
+	return 0;
 }
 
 static void execmd_read_page_sector(struct mtd_info *mtd, int page_addr)
 {
 	struct sh_flctl *flctl = mtd_to_flctl(mtd);
 	int sector, page_sectors;
+	enum flctl_ecc_res_t ecc_result;
 
-	if (flctl->page_size)
-		page_sectors = 4;
-	else
-		page_sectors = 1;
-
-	writel(readl(FLCMNCR(flctl)) | ACM_SACCES_MODE | _4ECCCORRECT,
-		 FLCMNCR(flctl));
+	page_sectors = flctl->page_size ? 4 : 1;
 
 	set_cmd_regs(mtd, NAND_CMD_READ0,
 		(NAND_CMD_READSTART << 8) | NAND_CMD_READ0);
 
-	for (sector = 0; sector < page_sectors; sector++) {
-		int ret;
+	writel(readl(FLCMNCR(flctl)) | ACM_SACCES_MODE | _4ECCCORRECT,
+		 FLCMNCR(flctl));
+	writel(readl(FLCMDCR(flctl)) | page_sectors, FLCMDCR(flctl));
+	writel(page_addr << 2, FLADR(flctl));
 
-		empty_fifo(flctl);
-		writel(readl(FLCMDCR(flctl)) | 1, FLCMDCR(flctl));
-		writel(page_addr << 2 | sector, FLADR(flctl));
+	empty_fifo(flctl);
+	start_translation(flctl);
 
-		start_translation(flctl);
+	for (sector = 0; sector < page_sectors; sector++) {
 		read_fiforeg(flctl, 512, 512 * sector);
 
-		ret = read_ecfiforeg(flctl,
+		ecc_result = read_ecfiforeg(flctl,
 			&flctl->done_buff[mtd->writesize + 16 * sector],
 			sector);
 
-		if (ret)
-			flctl->hwecc_cant_correct[sector] = 1;
-
-		writel(0x0, FL4ECCCR(flctl));
-		wait_completion(flctl);
+		switch (ecc_result) {
+		case FL_REPAIRABLE:
+			dev_info(&flctl->pdev->dev,
+				"applied ecc on page 0x%x", page_addr);
+			flctl->mtd.ecc_stats.corrected++;
+			break;
+		case FL_ERROR:
+			dev_warn(&flctl->pdev->dev,
+				"page 0x%x contains corrupted data\n",
+				page_addr);
+			flctl->mtd.ecc_stats.failed++;
+			break;
+		default:
+			;
+		}
 	}
+
+	wait_completion(flctl);
+
 	writel(readl(FLCMNCR(flctl)) & ~(ACM_SACCES_MODE | _4ECCCORRECT),
 			FLCMNCR(flctl));
 }
@@ -420,30 +461,20 @@ static void execmd_read_page_sector(struct mtd_info *mtd, int page_addr)
 static void execmd_read_oob(struct mtd_info *mtd, int page_addr)
 {
 	struct sh_flctl *flctl = mtd_to_flctl(mtd);
+	int page_sectors = flctl->page_size ? 4 : 1;
+	int i;
 
 	set_cmd_regs(mtd, NAND_CMD_READ0,
 		(NAND_CMD_READSTART << 8) | NAND_CMD_READ0);
 
 	empty_fifo(flctl);
-	if (flctl->page_size) {
-		int i;
-		/* In case that the page size is 2k */
-		for (i = 0; i < 16 * 3; i++)
-			flctl->done_buff[i] = 0xFF;
-
-		set_addr(mtd, 3 * 528 + 512, page_addr);
-		writel(16, FLDTCNTR(flctl));
 
-		start_translation(flctl);
-		read_fiforeg(flctl, 16, 16 * 3);
-		wait_completion(flctl);
-	} else {
-		/* In case that the page size is 512b */
-		set_addr(mtd, 512, page_addr);
+	for (i = 0; i < page_sectors; i++) {
+		set_addr(mtd, (512 + 16) * i + 512 , page_addr);
 		writel(16, FLDTCNTR(flctl));
 
 		start_translation(flctl);
-		read_fiforeg(flctl, 16, 0);
+		read_fiforeg(flctl, 16, 16 * i);
 		wait_completion(flctl);
 	}
 }
@@ -451,34 +482,26 @@ static void execmd_read_oob(struct mtd_info *mtd, int page_addr)
 static void execmd_write_page_sector(struct mtd_info *mtd)
 {
 	struct sh_flctl *flctl = mtd_to_flctl(mtd);
-	int i, page_addr = flctl->seqin_page_addr;
+	int page_addr = flctl->seqin_page_addr;
 	int sector, page_sectors;
 
-	if (flctl->page_size)
-		page_sectors = 4;
-	else
-		page_sectors = 1;
-
-	writel(readl(FLCMNCR(flctl)) | ACM_SACCES_MODE, FLCMNCR(flctl));
+	page_sectors = flctl->page_size ? 4 : 1;
 
 	set_cmd_regs(mtd, NAND_CMD_PAGEPROG,
 			(NAND_CMD_PAGEPROG << 8) | NAND_CMD_SEQIN);
 
-	for (sector = 0; sector < page_sectors; sector++) {
-		empty_fifo(flctl);
-		writel(readl(FLCMDCR(flctl)) | 1, FLCMDCR(flctl));
-		writel(page_addr << 2 | sector, FLADR(flctl));
+	empty_fifo(flctl);
+	writel(readl(FLCMNCR(flctl)) | ACM_SACCES_MODE, FLCMNCR(flctl));
+	writel(readl(FLCMDCR(flctl)) | page_sectors, FLCMDCR(flctl));
+	writel(page_addr << 2, FLADR(flctl));
+	start_translation(flctl);
 
-		start_translation(flctl);
+	for (sector = 0; sector < page_sectors; sector++) {
 		write_fiforeg(flctl, 512, 512 * sector);
-
-		for (i = 0; i < 4; i++) {
-			wait_wecfifo_ready(flctl); /* wait for write ready */
-			writel(0xFFFFFFFF, FLECFIFO(flctl));
-		}
-		wait_completion(flctl);
+		write_ec_fiforeg(flctl, 16, mtd->writesize + 16 * sector);
 	}
 
+	wait_completion(flctl);
 	writel(readl(FLCMNCR(flctl)) & ~ACM_SACCES_MODE, FLCMNCR(flctl));
 }
 
@@ -488,18 +511,12 @@ static void execmd_write_oob(struct mtd_info *mtd)
 	int page_addr = flctl->seqin_page_addr;
 	int sector, page_sectors;
 
-	if (flctl->page_size) {
-		sector = 3;
-		page_sectors = 4;
-	} else {
-		sector = 0;
-		page_sectors = 1;
-	}
+	page_sectors = flctl->page_size ? 4 : 1;
 
 	set_cmd_regs(mtd, NAND_CMD_PAGEPROG,
 			(NAND_CMD_PAGEPROG << 8) | NAND_CMD_SEQIN);
 
-	for (; sector < page_sectors; sector++) {
+	for (sector = 0; sector < page_sectors; sector++) {
 		empty_fifo(flctl);
 		set_addr(mtd, sector * 528 + 512, page_addr);
 		writel(16, FLDTCNTR(flctl));	/* set read size */
@@ -731,10 +748,9 @@ static void flctl_select_chip(struct mtd_info *mtd, int chipnr)
 static void flctl_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 {
 	struct sh_flctl *flctl = mtd_to_flctl(mtd);
-	int i, index = flctl->index;
+	int index = flctl->index;
 
-	for (i = 0; i < len; i++)
-		flctl->done_buff[index + i] = buf[i];
+	memcpy(&flctl->done_buff[index], buf, len);
 	flctl->index += len;
 }
 
@@ -763,20 +779,11 @@ static uint16_t flctl_read_word(struct mtd_info *mtd)
 
 static void flctl_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 {
-	int i;
-
-	for (i = 0; i < len; i++)
-		buf[i] = flctl_read_byte(mtd);
-}
-
-static int flctl_verify_buf(struct mtd_info *mtd, const u_char *buf, int len)
-{
-	int i;
+	struct sh_flctl *flctl = mtd_to_flctl(mtd);
+	int index = flctl->index;
 
-	for (i = 0; i < len; i++)
-		if (buf[i] != flctl_read_byte(mtd))
-			return -EFAULT;
-	return 0;
+	memcpy(buf, &flctl->done_buff[index], len);
+	flctl->index += len;
 }
 
 static int flctl_chip_init_tail(struct mtd_info *mtd)
@@ -831,7 +838,7 @@ static int flctl_chip_init_tail(struct mtd_info *mtd)
 		chip->ecc.mode = NAND_ECC_HW;
 
 		/* 4 symbols ECC enabled */
-		flctl->flcmncr_base |= _4ECCEN | ECCPOS2 | ECCPOS_02;
+		flctl->flcmncr_base |= _4ECCEN;
 	} else {
 		chip->ecc.mode = NAND_ECC_SOFT;
 	}
@@ -839,6 +846,16 @@ static int flctl_chip_init_tail(struct mtd_info *mtd)
 	return 0;
 }
 
+static irqreturn_t flctl_handle_flste(int irq, void *dev_id)
+{
+	struct sh_flctl *flctl = dev_id;
+
+	dev_err(&flctl->pdev->dev, "flste irq: %x\n", readl(FLINTDMACR(flctl)));
+	writel(flctl->flintdmacr_base, FLINTDMACR(flctl));
+
+	return IRQ_HANDLED;
+}
+
 static int __devinit flctl_probe(struct platform_device *pdev)
 {
 	struct resource *res;
@@ -847,6 +864,7 @@ static int __devinit flctl_probe(struct platform_device *pdev)
 	struct nand_chip *nand;
 	struct sh_flctl_platform_data *pdata;
 	int ret = -ENXIO;
+	int irq;
 
 	pdata = pdev->dev.platform_data;
 	if (pdata == NULL) {
@@ -872,14 +890,27 @@ static int __devinit flctl_probe(struct platform_device *pdev)
 		goto err_iomap;
 	}
 
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "failed to get flste irq data\n");
+		goto err_flste;
+	}
+
+	ret = request_irq(irq, flctl_handle_flste, IRQF_SHARED, "flste", flctl);
+	if (ret) {
+		dev_err(&pdev->dev, "request interrupt failed.\n");
+		goto err_flste;
+	}
+
 	platform_set_drvdata(pdev, flctl);
 	flctl_mtd = &flctl->mtd;
 	nand = &flctl->chip;
 	flctl_mtd->priv = nand;
 	flctl->pdev = pdev;
-	flctl->flcmncr_base = pdata->flcmncr_val;
 	flctl->hwecc = pdata->has_hwecc;
 	flctl->holden = pdata->use_holden;
+	flctl->flcmncr_base = pdata->flcmncr_val;
+	flctl->flintdmacr_base = flctl->hwecc ? (STERINTE | ECERB) : STERINTE;
 
 	/* Set address of hardware control function */
 	/* 20 us command delay time */
@@ -888,7 +919,6 @@ static int __devinit flctl_probe(struct platform_device *pdev)
 	nand->read_byte = flctl_read_byte;
 	nand->write_buf = flctl_write_buf;
 	nand->read_buf = flctl_read_buf;
-	nand->verify_buf = flctl_verify_buf;
 	nand->select_chip = flctl_select_chip;
 	nand->cmdfunc = flctl_cmdfunc;
 
@@ -918,6 +948,9 @@ static int __devinit flctl_probe(struct platform_device *pdev)
 
 err_chip:
 	pm_runtime_disable(&pdev->dev);
+	free_irq(irq, flctl);
+err_flste:
+	iounmap(flctl->reg);
 err_iomap:
 	kfree(flctl);
 	return ret;
@@ -929,6 +962,8 @@ static int __devexit flctl_remove(struct platform_device *pdev)
 
 	nand_release(&flctl->mtd);
 	pm_runtime_disable(&pdev->dev);
+	free_irq(platform_get_irq(pdev, 0), flctl);
+	iounmap(flctl->reg);
 	kfree(flctl);
 
 	return 0;
diff --git a/drivers/mtd/nand/socrates_nand.c b/drivers/mtd/nand/socrates_nand.c
index e02b08bcf0c..f3f28fafbf7 100644
--- a/drivers/mtd/nand/socrates_nand.c
+++ b/drivers/mtd/nand/socrates_nand.c
@@ -98,24 +98,6 @@ static uint16_t socrates_nand_read_word(struct mtd_info *mtd)
 	return word;
 }
 
-/**
- * socrates_nand_verify_buf -  Verify chip data against buffer
- * @mtd:	MTD device structure
- * @buf:	buffer containing the data to compare
- * @len:	number of bytes to compare
- */
-static int socrates_nand_verify_buf(struct mtd_info *mtd, const u8 *buf,
-		int len)
-{
-	int i;
-
-	for (i = 0; i < len; i++) {
-		if (buf[i] != socrates_nand_read_byte(mtd))
-			return -EFAULT;
-	}
-	return 0;
-}
-
 /*
  * Hardware specific access to control-lines
  */
@@ -201,7 +183,6 @@ static int __devinit socrates_nand_probe(struct platform_device *ofdev)
 	nand_chip->read_word = socrates_nand_read_word;
 	nand_chip->write_buf = socrates_nand_write_buf;
 	nand_chip->read_buf = socrates_nand_read_buf;
-	nand_chip->verify_buf = socrates_nand_verify_buf;
 	nand_chip->dev_ready = socrates_nand_device_ready;
 
 	nand_chip->ecc.mode = NAND_ECC_SOFT;	/* enable ECC */
diff --git a/drivers/mtd/nand/tmio_nand.c b/drivers/mtd/nand/tmio_nand.c
index 5aa518081c5..508e9e04b09 100644
--- a/drivers/mtd/nand/tmio_nand.c
+++ b/drivers/mtd/nand/tmio_nand.c
@@ -256,18 +256,6 @@ static void tmio_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 	tmio_ioread16_rep(tmio->fcr + FCR_DATA, buf, len >> 1);
 }
 
-static int
-tmio_nand_verify_buf(struct mtd_info *mtd, const u_char *buf, int len)
-{
-	struct tmio_nand *tmio = mtd_to_tmio(mtd);
-	u16				*p = (u16 *) buf;
-
-	for (len >>= 1; len; len--)
-		if (*(p++) != tmio_ioread16(tmio->fcr + FCR_DATA))
-			return -EFAULT;
-	return 0;
-}
-
 static void tmio_nand_enable_hwecc(struct mtd_info *mtd, int mode)
 {
 	struct tmio_nand *tmio = mtd_to_tmio(mtd);
@@ -424,7 +412,6 @@ static int tmio_probe(struct platform_device *dev)
 	nand_chip->read_byte = tmio_nand_read_byte;
 	nand_chip->write_buf = tmio_nand_write_buf;
 	nand_chip->read_buf = tmio_nand_read_buf;
-	nand_chip->verify_buf = tmio_nand_verify_buf;
 
 	/* set eccmode using hardware ECC */
 	nand_chip->ecc.mode = NAND_ECC_HW;
diff --git a/drivers/mtd/nand/txx9ndfmc.c b/drivers/mtd/nand/txx9ndfmc.c
index 26398dcf21c..e3d7266e256 100644
--- a/drivers/mtd/nand/txx9ndfmc.c
+++ b/drivers/mtd/nand/txx9ndfmc.c
@@ -131,18 +131,6 @@ static void txx9ndfmc_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 		*buf++ = __raw_readl(ndfdtr);
 }
 
-static int txx9ndfmc_verify_buf(struct mtd_info *mtd, const uint8_t *buf,
-				int len)
-{
-	struct platform_device *dev = mtd_to_platdev(mtd);
-	void __iomem *ndfdtr = ndregaddr(dev, TXX9_NDFDTR);
-
-	while (len--)
-		if (*buf++ != (uint8_t)__raw_readl(ndfdtr))
-			return -EFAULT;
-	return 0;
-}
-
 static void txx9ndfmc_cmd_ctrl(struct mtd_info *mtd, int cmd,
 			       unsigned int ctrl)
 {
@@ -346,7 +334,6 @@ static int __init txx9ndfmc_probe(struct platform_device *dev)
 		chip->read_byte = txx9ndfmc_read_byte;
 		chip->read_buf = txx9ndfmc_read_buf;
 		chip->write_buf = txx9ndfmc_write_buf;
-		chip->verify_buf = txx9ndfmc_verify_buf;
 		chip->cmd_ctrl = txx9ndfmc_cmd_ctrl;
 		chip->dev_ready = txx9ndfmc_dev_ready;
 		chip->ecc.calculate = txx9ndfmc_calculate_ecc;
diff --git a/drivers/mtd/nand/xway_nand.c b/drivers/mtd/nand/xway_nand.c
new file mode 100644
index 00000000000..3f81dc8f214
--- /dev/null
+++ b/drivers/mtd/nand/xway_nand.c
@@ -0,0 +1,201 @@
+/*
+ *  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.
+ *
+ *  Copyright © 2012 John Crispin <blogic@openwrt.org>
+ */
+
+#include <linux/mtd/nand.h>
+#include <linux/of_gpio.h>
+#include <linux/of_platform.h>
+
+#include <lantiq_soc.h>
+
+/* nand registers */
+#define EBU_ADDSEL1		0x24
+#define EBU_NAND_CON		0xB0
+#define EBU_NAND_WAIT		0xB4
+#define EBU_NAND_ECC0		0xB8
+#define EBU_NAND_ECC_AC		0xBC
+
+/* nand commands */
+#define NAND_CMD_ALE		(1 << 2)
+#define NAND_CMD_CLE		(1 << 3)
+#define NAND_CMD_CS		(1 << 4)
+#define NAND_WRITE_CMD_RESET	0xff
+#define NAND_WRITE_CMD		(NAND_CMD_CS | NAND_CMD_CLE)
+#define NAND_WRITE_ADDR		(NAND_CMD_CS | NAND_CMD_ALE)
+#define NAND_WRITE_DATA		(NAND_CMD_CS)
+#define NAND_READ_DATA		(NAND_CMD_CS)
+#define NAND_WAIT_WR_C		(1 << 3)
+#define NAND_WAIT_RD		(0x1)
+
+/* we need to tel the ebu which addr we mapped the nand to */
+#define ADDSEL1_MASK(x)		(x << 4)
+#define ADDSEL1_REGEN		1
+
+/* we need to tell the EBU that we have nand attached and set it up properly */
+#define BUSCON1_SETUP		(1 << 22)
+#define BUSCON1_BCGEN_RES	(0x3 << 12)
+#define BUSCON1_WAITWRC2	(2 << 8)
+#define BUSCON1_WAITRDC2	(2 << 6)
+#define BUSCON1_HOLDC1		(1 << 4)
+#define BUSCON1_RECOVC1		(1 << 2)
+#define BUSCON1_CMULT4		1
+
+#define NAND_CON_CE		(1 << 20)
+#define NAND_CON_OUT_CS1	(1 << 10)
+#define NAND_CON_IN_CS1		(1 << 8)
+#define NAND_CON_PRE_P		(1 << 7)
+#define NAND_CON_WP_P		(1 << 6)
+#define NAND_CON_SE_P		(1 << 5)
+#define NAND_CON_CS_P		(1 << 4)
+#define NAND_CON_CSMUX		(1 << 1)
+#define NAND_CON_NANDM		1
+
+static void xway_reset_chip(struct nand_chip *chip)
+{
+	unsigned long nandaddr = (unsigned long) chip->IO_ADDR_W;
+	unsigned long flags;
+
+	nandaddr &= ~NAND_WRITE_ADDR;
+	nandaddr |= NAND_WRITE_CMD;
+
+	/* finish with a reset */
+	spin_lock_irqsave(&ebu_lock, flags);
+	writeb(NAND_WRITE_CMD_RESET, (void __iomem *) nandaddr);
+	while ((ltq_ebu_r32(EBU_NAND_WAIT) & NAND_WAIT_WR_C) == 0)
+		;
+	spin_unlock_irqrestore(&ebu_lock, flags);
+}
+
+static void xway_select_chip(struct mtd_info *mtd, int chip)
+{
+
+	switch (chip) {
+	case -1:
+		ltq_ebu_w32_mask(NAND_CON_CE, 0, EBU_NAND_CON);
+		ltq_ebu_w32_mask(NAND_CON_NANDM, 0, EBU_NAND_CON);
+		break;
+	case 0:
+		ltq_ebu_w32_mask(0, NAND_CON_NANDM, EBU_NAND_CON);
+		ltq_ebu_w32_mask(0, NAND_CON_CE, EBU_NAND_CON);
+		break;
+	default:
+		BUG();
+	}
+}
+
+static void xway_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl)
+{
+	struct nand_chip *this = mtd->priv;
+	unsigned long nandaddr = (unsigned long) this->IO_ADDR_W;
+	unsigned long flags;
+
+	if (ctrl & NAND_CTRL_CHANGE) {
+		nandaddr &= ~(NAND_WRITE_CMD | NAND_WRITE_ADDR);
+		if (ctrl & NAND_CLE)
+			nandaddr |= NAND_WRITE_CMD;
+		else
+			nandaddr |= NAND_WRITE_ADDR;
+		this->IO_ADDR_W = (void __iomem *) nandaddr;
+	}
+
+	if (cmd != NAND_CMD_NONE) {
+		spin_lock_irqsave(&ebu_lock, flags);
+		writeb(cmd, this->IO_ADDR_W);
+		while ((ltq_ebu_r32(EBU_NAND_WAIT) & NAND_WAIT_WR_C) == 0)
+			;
+		spin_unlock_irqrestore(&ebu_lock, flags);
+	}
+}
+
+static int xway_dev_ready(struct mtd_info *mtd)
+{
+	return ltq_ebu_r32(EBU_NAND_WAIT) & NAND_WAIT_RD;
+}
+
+static unsigned char xway_read_byte(struct mtd_info *mtd)
+{
+	struct nand_chip *this = mtd->priv;
+	unsigned long nandaddr = (unsigned long) this->IO_ADDR_R;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&ebu_lock, flags);
+	ret = ltq_r8((void __iomem *)(nandaddr + NAND_READ_DATA));
+	spin_unlock_irqrestore(&ebu_lock, flags);
+
+	return ret;
+}
+
+static int xway_nand_probe(struct platform_device *pdev)
+{
+	struct nand_chip *this = platform_get_drvdata(pdev);
+	unsigned long nandaddr = (unsigned long) this->IO_ADDR_W;
+	const __be32 *cs = of_get_property(pdev->dev.of_node,
+					"lantiq,cs", NULL);
+	u32 cs_flag = 0;
+
+	/* load our CS from the DT. Either we find a valid 1 or default to 0 */
+	if (cs && (*cs == 1))
+		cs_flag = NAND_CON_IN_CS1 | NAND_CON_OUT_CS1;
+
+	/* setup the EBU to run in NAND mode on our base addr */
+	ltq_ebu_w32(CPHYSADDR(nandaddr)
+		| ADDSEL1_MASK(3) | ADDSEL1_REGEN, EBU_ADDSEL1);
+
+	ltq_ebu_w32(BUSCON1_SETUP | BUSCON1_BCGEN_RES | BUSCON1_WAITWRC2
+		| BUSCON1_WAITRDC2 | BUSCON1_HOLDC1 | BUSCON1_RECOVC1
+		| BUSCON1_CMULT4, LTQ_EBU_BUSCON1);
+
+	ltq_ebu_w32(NAND_CON_NANDM | NAND_CON_CSMUX | NAND_CON_CS_P
+		| NAND_CON_SE_P | NAND_CON_WP_P | NAND_CON_PRE_P
+		| cs_flag, EBU_NAND_CON);
+
+	/* finish with a reset */
+	xway_reset_chip(this);
+
+	return 0;
+}
+
+/* allow users to override the partition in DT using the cmdline */
+static const char *part_probes[] = { "cmdlinepart", "ofpart", NULL };
+
+static struct platform_nand_data xway_nand_data = {
+	.chip = {
+		.nr_chips		= 1,
+		.chip_delay		= 30,
+		.part_probe_types	= part_probes,
+	},
+	.ctrl = {
+		.probe		= xway_nand_probe,
+		.cmd_ctrl	= xway_cmd_ctrl,
+		.dev_ready	= xway_dev_ready,
+		.select_chip	= xway_select_chip,
+		.read_byte	= xway_read_byte,
+	}
+};
+
+/*
+ * Try to find the node inside the DT. If it is available attach out
+ * platform_nand_data
+ */
+static int __init xway_register_nand(void)
+{
+	struct device_node *node;
+	struct platform_device *pdev;
+
+	node = of_find_compatible_node(NULL, NULL, "lantiq,nand-xway");
+	if (!node)
+		return -ENOENT;
+	pdev = of_find_device_by_node(node);
+	if (!pdev)
+		return -EINVAL;
+	pdev->dev.platform_data = &xway_nand_data;
+	of_node_put(node);
+	return 0;
+}
+
+subsys_initcall(xway_register_nand);
diff --git a/drivers/mtd/sm_ftl.c b/drivers/mtd/sm_ftl.c
index 9e2dfd517aa..8dd6ba52404 100644
--- a/drivers/mtd/sm_ftl.c
+++ b/drivers/mtd/sm_ftl.c
@@ -346,7 +346,6 @@ static int sm_write_sector(struct sm_ftl *ftl,
 	ret = mtd_write_oob(mtd, sm_mkoffset(ftl, zone, block, boffset), &ops);
 
 	/* Now we assume that hardware will catch write bitflip errors */
-	/* If you are paranoid, use CONFIG_MTD_NAND_VERIFY_WRITE */
 
 	if (ret) {
 		dbg("write to block %d at zone %d, failed with error %d",
diff --git a/drivers/mtd/tests/Makefile b/drivers/mtd/tests/Makefile
index b44dcab940d..bd0065c0d35 100644
--- a/drivers/mtd/tests/Makefile
+++ b/drivers/mtd/tests/Makefile
@@ -6,3 +6,4 @@ obj-$(CONFIG_MTD_TESTS) += mtd_stresstest.o
 obj-$(CONFIG_MTD_TESTS) += mtd_subpagetest.o
 obj-$(CONFIG_MTD_TESTS) += mtd_torturetest.o
 obj-$(CONFIG_MTD_TESTS) += mtd_nandecctest.o
+obj-$(CONFIG_MTD_TESTS) += mtd_nandbiterrs.o
diff --git a/drivers/mtd/tests/mtd_nandbiterrs.c b/drivers/mtd/tests/mtd_nandbiterrs.c
new file mode 100644
index 00000000000..cc8d62cb280
--- /dev/null
+++ b/drivers/mtd/tests/mtd_nandbiterrs.c
@@ -0,0 +1,460 @@
+/*
+ * Copyright © 2012 NetCommWireless
+ * Iwo Mergler <Iwo.Mergler@netcommwireless.com.au>
+ *
+ * Test for multi-bit error recovery on a NAND page This mostly tests the
+ * ECC controller / driver.
+ *
+ * There are two test modes:
+ *
+ *	0 - artificially inserting bit errors until the ECC fails
+ *	    This is the default method and fairly quick. It should
+ *	    be independent of the quality of the FLASH.
+ *
+ *	1 - re-writing the same pattern repeatedly until the ECC fails.
+ *	    This method relies on the physics of NAND FLASH to eventually
+ *	    generate '0' bits if '1' has been written sufficient times.
+ *	    Depending on the NAND, the first bit errors will appear after
+ *	    1000 or more writes and then will usually snowball, reaching the
+ *	    limits of the ECC quickly.
+ *
+ *	    The test stops after 10000 cycles, should your FLASH be
+ *	    exceptionally good and not generate bit errors before that. Try
+ *	    a different page in that case.
+ *
+ * Please note that neither of these tests will significantly 'use up' any
+ * FLASH endurance. Only a maximum of two erase operations will be performed.
+ *
+ *
+ * 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; see the file COPYING. If not, write to the Free Software
+ * Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/mtd/mtd.h>
+#include <linux/err.h>
+#include <linux/mtd/nand.h>
+#include <linux/slab.h>
+
+#define msg(FMT, VA...) pr_info("mtd_nandbiterrs: "FMT, ##VA)
+
+static int dev;
+module_param(dev, int, S_IRUGO);
+MODULE_PARM_DESC(dev, "MTD device number to use");
+
+static unsigned page_offset;
+module_param(page_offset, uint, S_IRUGO);
+MODULE_PARM_DESC(page_offset, "Page number relative to dev start");
+
+static unsigned seed;
+module_param(seed, uint, S_IRUGO);
+MODULE_PARM_DESC(seed, "Random seed");
+
+static int mode;
+module_param(mode, int, S_IRUGO);
+MODULE_PARM_DESC(mode, "0=incremental errors, 1=overwrite test");
+
+static unsigned max_overwrite = 10000;
+
+static loff_t   offset;     /* Offset of the page we're using. */
+static unsigned eraseblock; /* Eraseblock number for our page. */
+
+/* We assume that the ECC can correct up to a certain number
+ * of biterrors per subpage. */
+static unsigned subsize;  /* Size of subpages */
+static unsigned subcount; /* Number of subpages per page */
+
+static struct mtd_info *mtd;   /* MTD device */
+
+static uint8_t *wbuffer; /* One page write / compare buffer */
+static uint8_t *rbuffer; /* One page read buffer */
+
+/* 'random' bytes from known offsets */
+static uint8_t hash(unsigned offset)
+{
+	unsigned v = offset;
+	unsigned char c;
+	v ^= 0x7f7edfd3;
+	v = v ^ (v >> 3);
+	v = v ^ (v >> 5);
+	v = v ^ (v >> 13);
+	c = v & 0xFF;
+	/* Reverse bits of result. */
+	c = (c & 0x0F) << 4 | (c & 0xF0) >> 4;
+	c = (c & 0x33) << 2 | (c & 0xCC) >> 2;
+	c = (c & 0x55) << 1 | (c & 0xAA) >> 1;
+	return c;
+}
+
+static int erase_block(void)
+{
+	int err;
+	struct erase_info ei;
+	loff_t addr = eraseblock * mtd->erasesize;
+
+	msg("erase_block\n");
+
+	memset(&ei, 0, sizeof(struct erase_info));
+	ei.mtd  = mtd;
+	ei.addr = addr;
+	ei.len  = mtd->erasesize;
+
+	err = mtd_erase(mtd, &ei);
+	if (err || ei.state == MTD_ERASE_FAILED) {
+		msg("error %d while erasing\n", err);
+		if (!err)
+			err = -EIO;
+		return err;
+	}
+
+	return 0;
+}
+
+/* Writes wbuffer to page */
+static int write_page(int log)
+{
+	int err = 0;
+	size_t written;
+
+	if (log)
+		msg("write_page\n");
+
+	err = mtd_write(mtd, offset, mtd->writesize, &written, wbuffer);
+	if (err || written != mtd->writesize) {
+		msg("error: write failed at %#llx\n", (long long)offset);
+		if (!err)
+			err = -EIO;
+	}
+
+	return err;
+}
+
+/* Re-writes the data area while leaving the OOB alone. */
+static int rewrite_page(int log)
+{
+	int err = 0;
+	struct mtd_oob_ops ops;
+
+	if (log)
+		msg("rewrite page\n");
+
+	ops.mode      = MTD_OPS_RAW; /* No ECC */
+	ops.len       = mtd->writesize;
+	ops.retlen    = 0;
+	ops.ooblen    = 0;
+	ops.oobretlen = 0;
+	ops.ooboffs   = 0;
+	ops.datbuf    = wbuffer;
+	ops.oobbuf    = NULL;
+
+	err = mtd_write_oob(mtd, offset, &ops);
+	if (err || ops.retlen != mtd->writesize) {
+		msg("error: write_oob failed (%d)\n", err);
+		if (!err)
+			err = -EIO;
+	}
+
+	return err;
+}
+
+/* Reads page into rbuffer. Returns number of corrected bit errors (>=0)
+ * or error (<0) */
+static int read_page(int log)
+{
+	int err = 0;
+	size_t read;
+	struct mtd_ecc_stats oldstats;
+
+	if (log)
+		msg("read_page\n");
+
+	/* Saving last mtd stats */
+	memcpy(&oldstats, &mtd->ecc_stats, sizeof(oldstats));
+
+	err = mtd_read(mtd, offset, mtd->writesize, &read, rbuffer);
+	if (err == -EUCLEAN)
+		err = mtd->ecc_stats.corrected - oldstats.corrected;
+
+	if (err < 0 || read != mtd->writesize) {
+		msg("error: read failed at %#llx\n", (long long)offset);
+		if (err >= 0)
+			err = -EIO;
+	}
+
+	return err;
+}
+
+/* Verifies rbuffer against random sequence */
+static int verify_page(int log)
+{
+	unsigned i, errs = 0;
+
+	if (log)
+		msg("verify_page\n");
+
+	for (i = 0; i < mtd->writesize; i++) {
+		if (rbuffer[i] != hash(i+seed)) {
+			msg("Error: page offset %u, expected %02x, got %02x\n",
+				i, hash(i+seed), rbuffer[i]);
+			errs++;
+		}
+	}
+
+	if (errs)
+		return -EIO;
+	else
+		return 0;
+}
+
+#define CBIT(v, n) ((v) & (1 << (n)))
+#define BCLR(v, n) ((v) = (v) & ~(1 << (n)))
+
+/* Finds the first '1' bit in wbuffer starting at offset 'byte'
+ * and sets it to '0'. */
+static int insert_biterror(unsigned byte)
+{
+	int bit;
+
+	while (byte < mtd->writesize) {
+		for (bit = 7; bit >= 0; bit--) {
+			if (CBIT(wbuffer[byte], bit)) {
+				BCLR(wbuffer[byte], bit);
+				msg("Inserted biterror @ %u/%u\n", byte, bit);
+				return 0;
+			}
+		}
+		byte++;
+	}
+	msg("biterror: Failed to find a '1' bit\n");
+	return -EIO;
+}
+
+/* Writes 'random' data to page and then introduces deliberate bit
+ * errors into the page, while verifying each step. */
+static int incremental_errors_test(void)
+{
+	int err = 0;
+	unsigned i;
+	unsigned errs_per_subpage = 0;
+
+	msg("incremental biterrors test\n");
+
+	for (i = 0; i < mtd->writesize; i++)
+		wbuffer[i] = hash(i+seed);
+
+	err = write_page(1);
+	if (err)
+		goto exit;
+
+	while (1) {
+
+		err = rewrite_page(1);
+		if (err)
+			goto exit;
+
+		err = read_page(1);
+		if (err > 0)
+			msg("Read reported %d corrected bit errors\n", err);
+		if (err < 0) {
+			msg("After %d biterrors per subpage, read reported error %d\n",
+				errs_per_subpage, err);
+			err = 0;
+			goto exit;
+		}
+
+		err = verify_page(1);
+		if (err) {
+			msg("ECC failure, read data is incorrect despite read success\n");
+			goto exit;
+		}
+
+		msg("Successfully corrected %d bit errors per subpage\n",
+			errs_per_subpage);
+
+		for (i = 0; i < subcount; i++) {
+			err = insert_biterror(i * subsize);
+			if (err < 0)
+				goto exit;
+		}
+		errs_per_subpage++;
+	}
+
+exit:
+	return err;
+}
+
+
+/* Writes 'random' data to page and then re-writes that same data repeatedly.
+   This eventually develops bit errors (bits written as '1' will slowly become
+   '0'), which are corrected as far as the ECC is capable of. */
+static int overwrite_test(void)
+{
+	int err = 0;
+	unsigned i;
+	unsigned max_corrected = 0;
+	unsigned opno = 0;
+	/* We don't expect more than this many correctable bit errors per
+	 * page. */
+	#define MAXBITS 512
+	static unsigned bitstats[MAXBITS]; /* bit error histogram. */
+
+	memset(bitstats, 0, sizeof(bitstats));
+
+	msg("overwrite biterrors test\n");
+
+	for (i = 0; i < mtd->writesize; i++)
+		wbuffer[i] = hash(i+seed);
+
+	err = write_page(1);
+	if (err)
+		goto exit;
+
+	while (opno < max_overwrite) {
+
+		err = rewrite_page(0);
+		if (err)
+			break;
+
+		err = read_page(0);
+		if (err >= 0) {
+			if (err >= MAXBITS) {
+				msg("Implausible number of bit errors corrected\n");
+				err = -EIO;
+				break;
+			}
+			bitstats[err]++;
+			if (err > max_corrected) {
+				max_corrected = err;
+				msg("Read reported %d corrected bit errors\n",
+					err);
+			}
+		} else { /* err < 0 */
+			msg("Read reported error %d\n", err);
+			err = 0;
+			break;
+		}
+
+		err = verify_page(0);
+		if (err) {
+			bitstats[max_corrected] = opno;
+			msg("ECC failure, read data is incorrect despite read success\n");
+			break;
+		}
+
+		opno++;
+	}
+
+	/* At this point bitstats[0] contains the number of ops with no bit
+	 * errors, bitstats[1] the number of ops with 1 bit error, etc. */
+	msg("Bit error histogram (%d operations total):\n", opno);
+	for (i = 0; i < max_corrected; i++)
+		msg("Page reads with %3d corrected bit errors: %d\n",
+			i, bitstats[i]);
+
+exit:
+	return err;
+}
+
+static int __init mtd_nandbiterrs_init(void)
+{
+	int err = 0;
+
+	msg("\n");
+	msg("==================================================\n");
+	msg("MTD device: %d\n", dev);
+
+	mtd = get_mtd_device(NULL, dev);
+	if (IS_ERR(mtd)) {
+		err = PTR_ERR(mtd);
+		msg("error: cannot get MTD device\n");
+		goto exit_mtddev;
+	}
+
+	if (mtd->type != MTD_NANDFLASH) {
+		msg("this test requires NAND flash\n");
+		err = -ENODEV;
+		goto exit_nand;
+	}
+
+	msg("MTD device size %llu, eraseblock=%u, page=%u, oob=%u\n",
+		(unsigned long long)mtd->size, mtd->erasesize,
+		mtd->writesize, mtd->oobsize);
+
+	subsize  = mtd->writesize >> mtd->subpage_sft;
+	subcount = mtd->writesize / subsize;
+
+	msg("Device uses %d subpages of %d bytes\n", subcount, subsize);
+
+	offset     = page_offset * mtd->writesize;
+	eraseblock = mtd_div_by_eb(offset, mtd);
+
+	msg("Using page=%u, offset=%llu, eraseblock=%u\n",
+		page_offset, offset, eraseblock);
+
+	wbuffer = kmalloc(mtd->writesize, GFP_KERNEL);
+	if (!wbuffer) {
+		err = -ENOMEM;
+		goto exit_wbuffer;
+	}
+
+	rbuffer = kmalloc(mtd->writesize, GFP_KERNEL);
+	if (!rbuffer) {
+		err = -ENOMEM;
+		goto exit_rbuffer;
+	}
+
+	err = erase_block();
+	if (err)
+		goto exit_error;
+
+	if (mode == 0)
+		err = incremental_errors_test();
+	else
+		err = overwrite_test();
+
+	if (err)
+		goto exit_error;
+
+	/* We leave the block un-erased in case of test failure. */
+	err = erase_block();
+	if (err)
+		goto exit_error;
+
+	err = -EIO;
+	msg("finished successfully.\n");
+	msg("==================================================\n");
+
+exit_error:
+	kfree(rbuffer);
+exit_rbuffer:
+	kfree(wbuffer);
+exit_wbuffer:
+	/* Nothing */
+exit_nand:
+	put_mtd_device(mtd);
+exit_mtddev:
+	return err;
+}
+
+static void __exit mtd_nandbiterrs_exit(void)
+{
+	return;
+}
+
+module_init(mtd_nandbiterrs_init);
+module_exit(mtd_nandbiterrs_exit);
+
+MODULE_DESCRIPTION("NAND bit error recovery test");
+MODULE_AUTHOR("Iwo Mergler");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mtd/tests/mtd_nandecctest.c b/drivers/mtd/tests/mtd_nandecctest.c
index 70d6d7d0d65..b437fa42507 100644
--- a/drivers/mtd/tests/mtd_nandecctest.c
+++ b/drivers/mtd/tests/mtd_nandecctest.c
@@ -4,60 +4,287 @@
 #include <linux/random.h>
 #include <linux/string.h>
 #include <linux/bitops.h>
-#include <linux/jiffies.h>
+#include <linux/slab.h>
 #include <linux/mtd/nand_ecc.h>
 
+/*
+ * Test the implementation for software ECC
+ *
+ * No actual MTD device is needed, So we don't need to warry about losing
+ * important data by human error.
+ *
+ * This covers possible patterns of corruption which can be reliably corrected
+ * or detected.
+ */
+
 #if defined(CONFIG_MTD_NAND) || defined(CONFIG_MTD_NAND_MODULE)
 
-static void inject_single_bit_error(void *data, size_t size)
+struct nand_ecc_test {
+	const char *name;
+	void (*prepare)(void *, void *, void *, void *, const size_t);
+	int (*verify)(void *, void *, void *, const size_t);
+};
+
+/*
+ * The reason for this __change_bit_le() instead of __change_bit() is to inject
+ * bit error properly within the region which is not a multiple of
+ * sizeof(unsigned long) on big-endian systems
+ */
+#ifdef __LITTLE_ENDIAN
+#define __change_bit_le(nr, addr) __change_bit(nr, addr)
+#elif defined(__BIG_ENDIAN)
+#define __change_bit_le(nr, addr) \
+		__change_bit((nr) ^ ((BITS_PER_LONG - 1) & ~0x7), addr)
+#else
+#error "Unknown byte order"
+#endif
+
+static void single_bit_error_data(void *error_data, void *correct_data,
+				size_t size)
 {
-	unsigned long offset = random32() % (size * BITS_PER_BYTE);
+	unsigned int offset = random32() % (size * BITS_PER_BYTE);
 
-	__change_bit(offset, data);
+	memcpy(error_data, correct_data, size);
+	__change_bit_le(offset, error_data);
 }
 
-static unsigned char data[512];
-static unsigned char error_data[512];
+static void double_bit_error_data(void *error_data, void *correct_data,
+				size_t size)
+{
+	unsigned int offset[2];
+
+	offset[0] = random32() % (size * BITS_PER_BYTE);
+	do {
+		offset[1] = random32() % (size * BITS_PER_BYTE);
+	} while (offset[0] == offset[1]);
 
-static int nand_ecc_test(const size_t size)
+	memcpy(error_data, correct_data, size);
+
+	__change_bit_le(offset[0], error_data);
+	__change_bit_le(offset[1], error_data);
+}
+
+static unsigned int random_ecc_bit(size_t size)
 {
-	unsigned char code[3];
-	unsigned char error_code[3];
-	char testname[30];
+	unsigned int offset = random32() % (3 * BITS_PER_BYTE);
+
+	if (size == 256) {
+		/*
+		 * Don't inject a bit error into the insignificant bits (16th
+		 * and 17th bit) in ECC code for 256 byte data block
+		 */
+		while (offset == 16 || offset == 17)
+			offset = random32() % (3 * BITS_PER_BYTE);
+	}
 
-	BUG_ON(sizeof(data) < size);
+	return offset;
+}
 
-	sprintf(testname, "nand-ecc-%zu", size);
+static void single_bit_error_ecc(void *error_ecc, void *correct_ecc,
+				size_t size)
+{
+	unsigned int offset = random_ecc_bit(size);
 
-	get_random_bytes(data, size);
+	memcpy(error_ecc, correct_ecc, 3);
+	__change_bit_le(offset, error_ecc);
+}
 
-	memcpy(error_data, data, size);
-	inject_single_bit_error(error_data, size);
+static void double_bit_error_ecc(void *error_ecc, void *correct_ecc,
+				size_t size)
+{
+	unsigned int offset[2];
 
-	__nand_calculate_ecc(data, size, code);
-	__nand_calculate_ecc(error_data, size, error_code);
-	__nand_correct_data(error_data, code, error_code, size);
+	offset[0] = random_ecc_bit(size);
+	do {
+		offset[1] = random_ecc_bit(size);
+	} while (offset[0] == offset[1]);
 
-	if (!memcmp(data, error_data, size)) {
-		printk(KERN_INFO "mtd_nandecctest: ok - %s\n", testname);
+	memcpy(error_ecc, correct_ecc, 3);
+	__change_bit_le(offset[0], error_ecc);
+	__change_bit_le(offset[1], error_ecc);
+}
+
+static void no_bit_error(void *error_data, void *error_ecc,
+		void *correct_data, void *correct_ecc, const size_t size)
+{
+	memcpy(error_data, correct_data, size);
+	memcpy(error_ecc, correct_ecc, 3);
+}
+
+static int no_bit_error_verify(void *error_data, void *error_ecc,
+				void *correct_data, const size_t size)
+{
+	unsigned char calc_ecc[3];
+	int ret;
+
+	__nand_calculate_ecc(error_data, size, calc_ecc);
+	ret = __nand_correct_data(error_data, error_ecc, calc_ecc, size);
+	if (ret == 0 && !memcmp(correct_data, error_data, size))
 		return 0;
-	}
 
-	printk(KERN_ERR "mtd_nandecctest: not ok - %s\n", testname);
+	return -EINVAL;
+}
+
+static void single_bit_error_in_data(void *error_data, void *error_ecc,
+		void *correct_data, void *correct_ecc, const size_t size)
+{
+	single_bit_error_data(error_data, correct_data, size);
+	memcpy(error_ecc, correct_ecc, 3);
+}
+
+static void single_bit_error_in_ecc(void *error_data, void *error_ecc,
+		void *correct_data, void *correct_ecc, const size_t size)
+{
+	memcpy(error_data, correct_data, size);
+	single_bit_error_ecc(error_ecc, correct_ecc, size);
+}
+
+static int single_bit_error_correct(void *error_data, void *error_ecc,
+				void *correct_data, const size_t size)
+{
+	unsigned char calc_ecc[3];
+	int ret;
 
-	printk(KERN_DEBUG "hexdump of data:\n");
-	print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET, 16, 4,
-			data, size, false);
-	printk(KERN_DEBUG "hexdump of error data:\n");
-	print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET, 16, 4,
+	__nand_calculate_ecc(error_data, size, calc_ecc);
+	ret = __nand_correct_data(error_data, error_ecc, calc_ecc, size);
+	if (ret == 1 && !memcmp(correct_data, error_data, size))
+		return 0;
+
+	return -EINVAL;
+}
+
+static void double_bit_error_in_data(void *error_data, void *error_ecc,
+		void *correct_data, void *correct_ecc, const size_t size)
+{
+	double_bit_error_data(error_data, correct_data, size);
+	memcpy(error_ecc, correct_ecc, 3);
+}
+
+static void single_bit_error_in_data_and_ecc(void *error_data, void *error_ecc,
+		void *correct_data, void *correct_ecc, const size_t size)
+{
+	single_bit_error_data(error_data, correct_data, size);
+	single_bit_error_ecc(error_ecc, correct_ecc, size);
+}
+
+static void double_bit_error_in_ecc(void *error_data, void *error_ecc,
+		void *correct_data, void *correct_ecc, const size_t size)
+{
+	memcpy(error_data, correct_data, size);
+	double_bit_error_ecc(error_ecc, correct_ecc, size);
+}
+
+static int double_bit_error_detect(void *error_data, void *error_ecc,
+				void *correct_data, const size_t size)
+{
+	unsigned char calc_ecc[3];
+	int ret;
+
+	__nand_calculate_ecc(error_data, size, calc_ecc);
+	ret = __nand_correct_data(error_data, error_ecc, calc_ecc, size);
+
+	return (ret == -1) ? 0 : -EINVAL;
+}
+
+static const struct nand_ecc_test nand_ecc_test[] = {
+	{
+		.name = "no-bit-error",
+		.prepare = no_bit_error,
+		.verify = no_bit_error_verify,
+	},
+	{
+		.name = "single-bit-error-in-data-correct",
+		.prepare = single_bit_error_in_data,
+		.verify = single_bit_error_correct,
+	},
+	{
+		.name = "single-bit-error-in-ecc-correct",
+		.prepare = single_bit_error_in_ecc,
+		.verify = single_bit_error_correct,
+	},
+	{
+		.name = "double-bit-error-in-data-detect",
+		.prepare = double_bit_error_in_data,
+		.verify = double_bit_error_detect,
+	},
+	{
+		.name = "single-bit-error-in-data-and-ecc-detect",
+		.prepare = single_bit_error_in_data_and_ecc,
+		.verify = double_bit_error_detect,
+	},
+	{
+		.name = "double-bit-error-in-ecc-detect",
+		.prepare = double_bit_error_in_ecc,
+		.verify = double_bit_error_detect,
+	},
+};
+
+static void dump_data_ecc(void *error_data, void *error_ecc, void *correct_data,
+			void *correct_ecc, const size_t size)
+{
+	pr_info("hexdump of error data:\n");
+	print_hex_dump(KERN_INFO, "", DUMP_PREFIX_OFFSET, 16, 4,
 			error_data, size, false);
+	print_hex_dump(KERN_INFO, "hexdump of error ecc: ",
+			DUMP_PREFIX_NONE, 16, 1, error_ecc, 3, false);
+
+	pr_info("hexdump of correct data:\n");
+	print_hex_dump(KERN_INFO, "", DUMP_PREFIX_OFFSET, 16, 4,
+			correct_data, size, false);
+	print_hex_dump(KERN_INFO, "hexdump of correct ecc: ",
+			DUMP_PREFIX_NONE, 16, 1, correct_ecc, 3, false);
+}
+
+static int nand_ecc_test_run(const size_t size)
+{
+	int i;
+	int err = 0;
+	void *error_data;
+	void *error_ecc;
+	void *correct_data;
+	void *correct_ecc;
 
-	return -1;
+	error_data = kmalloc(size, GFP_KERNEL);
+	error_ecc = kmalloc(3, GFP_KERNEL);
+	correct_data = kmalloc(size, GFP_KERNEL);
+	correct_ecc = kmalloc(3, GFP_KERNEL);
+
+	if (!error_data || !error_ecc || !correct_data || !correct_ecc) {
+		err = -ENOMEM;
+		goto error;
+	}
+
+	get_random_bytes(correct_data, size);
+	__nand_calculate_ecc(correct_data, size, correct_ecc);
+
+	for (i = 0; i < ARRAY_SIZE(nand_ecc_test); i++) {
+		nand_ecc_test[i].prepare(error_data, error_ecc,
+				correct_data, correct_ecc, size);
+		err = nand_ecc_test[i].verify(error_data, error_ecc,
+						correct_data, size);
+
+		if (err) {
+			pr_err("mtd_nandecctest: not ok - %s-%zd\n",
+				nand_ecc_test[i].name, size);
+			dump_data_ecc(error_data, error_ecc,
+				correct_data, correct_ecc, size);
+			break;
+		}
+		pr_info("mtd_nandecctest: ok - %s-%zd\n",
+			nand_ecc_test[i].name, size);
+	}
+error:
+	kfree(error_data);
+	kfree(error_ecc);
+	kfree(correct_data);
+	kfree(correct_ecc);
+
+	return err;
 }
 
 #else
 
-static int nand_ecc_test(const size_t size)
+static int nand_ecc_test_run(const size_t size)
 {
 	return 0;
 }
@@ -66,12 +293,13 @@ static int nand_ecc_test(const size_t size)
 
 static int __init ecc_test_init(void)
 {
-	srandom32(jiffies);
+	int err;
 
-	nand_ecc_test(256);
-	nand_ecc_test(512);
+	err = nand_ecc_test_run(256);
+	if (err)
+		return err;
 
-	return 0;
+	return nand_ecc_test_run(512);
 }
 
 static void __exit ecc_test_exit(void)
diff --git a/drivers/mtd/tests/mtd_speedtest.c b/drivers/mtd/tests/mtd_speedtest.c
index 2aec4f3b72b..42b0f7456fc 100644
--- a/drivers/mtd/tests/mtd_speedtest.c
+++ b/drivers/mtd/tests/mtd_speedtest.c
@@ -26,6 +26,7 @@
 #include <linux/mtd/mtd.h>
 #include <linux/slab.h>
 #include <linux/sched.h>
+#include <linux/random.h>
 
 #define PRINT_PREF KERN_INFO "mtd_speedtest: "
 
@@ -47,25 +48,13 @@ static int ebcnt;
 static int pgcnt;
 static int goodebcnt;
 static struct timeval start, finish;
-static unsigned long next = 1;
-
-static inline unsigned int simple_rand(void)
-{
-	next = next * 1103515245 + 12345;
-	return (unsigned int)((next / 65536) % 32768);
-}
-
-static inline void simple_srand(unsigned long seed)
-{
-	next = seed;
-}
 
 static void set_random_data(unsigned char *buf, size_t len)
 {
 	size_t i;
 
 	for (i = 0; i < len; ++i)
-		buf[i] = simple_rand();
+		buf[i] = random32();
 }
 
 static int erase_eraseblock(int ebnum)
@@ -407,7 +396,6 @@ static int __init mtd_speedtest_init(void)
 		goto out;
 	}
 
-	simple_srand(1);
 	set_random_data(iobuf, mtd->erasesize);
 
 	err = scan_for_bad_eraseblocks();
diff --git a/drivers/mtd/tests/mtd_stresstest.c b/drivers/mtd/tests/mtd_stresstest.c
index 7b33f22d0b5..cb268cebf01 100644
--- a/drivers/mtd/tests/mtd_stresstest.c
+++ b/drivers/mtd/tests/mtd_stresstest.c
@@ -27,6 +27,7 @@
 #include <linux/slab.h>
 #include <linux/sched.h>
 #include <linux/vmalloc.h>
+#include <linux/random.h>
 
 #define PRINT_PREF KERN_INFO "mtd_stresstest: "
 
@@ -48,28 +49,13 @@ static int pgsize;
 static int bufsize;
 static int ebcnt;
 static int pgcnt;
-static unsigned long next = 1;
-
-static inline unsigned int simple_rand(void)
-{
-	next = next * 1103515245 + 12345;
-	return (unsigned int)((next / 65536) % 32768);
-}
-
-static inline void simple_srand(unsigned long seed)
-{
-	next = seed;
-}
 
 static int rand_eb(void)
 {
-	int eb;
+	unsigned int eb;
 
 again:
-	if (ebcnt < 32768)
-		eb = simple_rand();
-	else
-		eb = (simple_rand() << 15) | simple_rand();
+	eb = random32();
 	/* Read or write up 2 eraseblocks at a time - hence 'ebcnt - 1' */
 	eb %= (ebcnt - 1);
 	if (bbt[eb])
@@ -79,24 +65,18 @@ again:
 
 static int rand_offs(void)
 {
-	int offs;
+	unsigned int offs;
 
-	if (bufsize < 32768)
-		offs = simple_rand();
-	else
-		offs = (simple_rand() << 15) | simple_rand();
+	offs = random32();
 	offs %= bufsize;
 	return offs;
 }
 
 static int rand_len(int offs)
 {
-	int len;
+	unsigned int len;
 
-	if (bufsize < 32768)
-		len = simple_rand();
-	else
-		len = (simple_rand() << 15) | simple_rand();
+	len = random32();
 	len %= (bufsize - offs);
 	return len;
 }
@@ -211,7 +191,7 @@ static int do_write(void)
 
 static int do_operation(void)
 {
-	if (simple_rand() & 1)
+	if (random32() & 1)
 		return do_read();
 	else
 		return do_write();
@@ -302,9 +282,8 @@ static int __init mtd_stresstest_init(void)
 	}
 	for (i = 0; i < ebcnt; i++)
 		offsets[i] = mtd->erasesize;
-	simple_srand(current->pid);
 	for (i = 0; i < bufsize; i++)
-		writebuf[i] = simple_rand();
+		writebuf[i] = random32();
 
 	err = scan_for_bad_eraseblocks();
 	if (err)
diff --git a/drivers/mtd/ubi/Kconfig b/drivers/mtd/ubi/Kconfig
index 271a842f8c3..36663af56d8 100644
--- a/drivers/mtd/ubi/Kconfig
+++ b/drivers/mtd/ubi/Kconfig
@@ -56,6 +56,27 @@ config MTD_UBI_BEB_LIMIT
 
 	  Leave the default value if unsure.
 
+config MTD_UBI_FASTMAP
+	bool "UBI Fastmap (Experimental feature)"
+	default n
+	help
+	   Important: this feature is experimental so far and the on-flash
+	   format for fastmap may change in the next kernel versions
+
+	   Fastmap is a mechanism which allows attaching an UBI device
+	   in nearly constant time. Instead of scanning the whole MTD device it
+	   only has to locate a checkpoint (called fastmap) on the device.
+	   The on-flash fastmap contains all information needed to attach
+	   the device. Using fastmap makes only sense on large devices where
+	   attaching by scanning takes long. UBI will not automatically install
+	   a fastmap on old images, but you can set the UBI module parameter
+	   fm_autoconvert to 1 if you want so. Please note that fastmap-enabled
+	   images are still usable with UBI implementations without
+	   fastmap support. On typical flash devices the whole fastmap fits
+	   into one PEB. UBI will reserve PEBs to hold two fastmaps.
+
+	   If in doubt, say "N".
+
 config MTD_UBI_GLUEBI
 	tristate "MTD devices emulation driver (gluebi)"
 	help
diff --git a/drivers/mtd/ubi/Makefile b/drivers/mtd/ubi/Makefile
index a0803ac7471..b46b0c97858 100644
--- a/drivers/mtd/ubi/Makefile
+++ b/drivers/mtd/ubi/Makefile
@@ -2,5 +2,6 @@ obj-$(CONFIG_MTD_UBI) += ubi.o
 
 ubi-y += vtbl.o vmt.o upd.o build.o cdev.o kapi.o eba.o io.o wl.o attach.o
 ubi-y += misc.o debug.o
+ubi-$(CONFIG_MTD_UBI_FASTMAP) += fastmap.o
 
 obj-$(CONFIG_MTD_UBI_GLUEBI) += gluebi.o
diff --git a/drivers/mtd/ubi/attach.c b/drivers/mtd/ubi/attach.c
index f7adf53e4f4..fec406b4553 100644
--- a/drivers/mtd/ubi/attach.c
+++ b/drivers/mtd/ubi/attach.c
@@ -300,7 +300,7 @@ static struct ubi_ainf_volume *add_volume(struct ubi_attach_info *ai,
 }
 
 /**
- * compare_lebs - find out which logical eraseblock is newer.
+ * ubi_compare_lebs - find out which logical eraseblock is newer.
  * @ubi: UBI device description object
  * @aeb: first logical eraseblock to compare
  * @pnum: physical eraseblock number of the second logical eraseblock to
@@ -319,7 +319,7 @@ static struct ubi_ainf_volume *add_volume(struct ubi_attach_info *ai,
  *     o bit 2 is cleared: the older LEB is not corrupted;
  *     o bit 2 is set: the older LEB is corrupted.
  */
-static int compare_lebs(struct ubi_device *ubi, const struct ubi_ainf_peb *aeb,
+int ubi_compare_lebs(struct ubi_device *ubi, const struct ubi_ainf_peb *aeb,
 			int pnum, const struct ubi_vid_hdr *vid_hdr)
 {
 	void *buf;
@@ -337,7 +337,7 @@ static int compare_lebs(struct ubi_device *ubi, const struct ubi_ainf_peb *aeb,
 		 * support these images anymore. Well, those images still work,
 		 * but only if no unclean reboots happened.
 		 */
-		ubi_err("unsupported on-flash UBI format\n");
+		ubi_err("unsupported on-flash UBI format");
 		return -EINVAL;
 	}
 
@@ -507,7 +507,7 @@ int ubi_add_to_av(struct ubi_device *ubi, struct ubi_attach_info *ai, int pnum,
 		 * sequence numbers. We still can attach these images, unless
 		 * there is a need to distinguish between old and new
 		 * eraseblocks, in which case we'll refuse the image in
-		 * 'compare_lebs()'. In other words, we attach old clean
+		 * 'ubi_compare_lebs()'. In other words, we attach old clean
 		 * images, but refuse attaching old images with duplicated
 		 * logical eraseblocks because there was an unclean reboot.
 		 */
@@ -523,7 +523,7 @@ int ubi_add_to_av(struct ubi_device *ubi, struct ubi_attach_info *ai, int pnum,
 		 * Now we have to drop the older one and preserve the newer
 		 * one.
 		 */
-		cmp_res = compare_lebs(ubi, aeb, pnum, vid_hdr);
+		cmp_res = ubi_compare_lebs(ubi, aeb, pnum, vid_hdr);
 		if (cmp_res < 0)
 			return cmp_res;
 
@@ -748,7 +748,7 @@ struct ubi_ainf_peb *ubi_early_get_peb(struct ubi_device *ubi,
 /**
  * check_corruption - check the data area of PEB.
  * @ubi: UBI device description object
- * @vid_hrd: the (corrupted) VID header of this PEB
+ * @vid_hdr: the (corrupted) VID header of this PEB
  * @pnum: the physical eraseblock number to check
  *
  * This is a helper function which is used to distinguish between VID header
@@ -810,6 +810,8 @@ out_unlock:
  * @ubi: UBI device description object
  * @ai: attaching information
  * @pnum: the physical eraseblock number
+ * @vid: The volume ID of the found volume will be stored in this pointer
+ * @sqnum: The sqnum of the found volume will be stored in this pointer
  *
  * This function reads UBI headers of PEB @pnum, checks them, and adds
  * information about this PEB to the corresponding list or RB-tree in the
@@ -817,10 +819,10 @@ out_unlock:
  * successfully handled and a negative error code in case of failure.
  */
 static int scan_peb(struct ubi_device *ubi, struct ubi_attach_info *ai,
-		    int pnum)
+		    int pnum, int *vid, unsigned long long *sqnum)
 {
 	long long uninitialized_var(ec);
-	int err, bitflips = 0, vol_id, ec_err = 0;
+	int err, bitflips = 0, vol_id = -1, ec_err = 0;
 
 	dbg_bld("scan PEB %d", pnum);
 
@@ -991,14 +993,21 @@ static int scan_peb(struct ubi_device *ubi, struct ubi_attach_info *ai,
 	}
 
 	vol_id = be32_to_cpu(vidh->vol_id);
+	if (vid)
+		*vid = vol_id;
+	if (sqnum)
+		*sqnum = be64_to_cpu(vidh->sqnum);
 	if (vol_id > UBI_MAX_VOLUMES && vol_id != UBI_LAYOUT_VOLUME_ID) {
 		int lnum = be32_to_cpu(vidh->lnum);
 
 		/* Unsupported internal volume */
 		switch (vidh->compat) {
 		case UBI_COMPAT_DELETE:
-			ubi_msg("\"delete\" compatible internal volume %d:%d found, will remove it",
-				vol_id, lnum);
+			if (vol_id != UBI_FM_SB_VOLUME_ID
+			    && vol_id != UBI_FM_DATA_VOLUME_ID) {
+				ubi_msg("\"delete\" compatible internal volume %d:%d found, will remove it",
+					vol_id, lnum);
+			}
 			err = add_to_list(ai, pnum, vol_id, lnum,
 					  ec, 1, &ai->erase);
 			if (err)
@@ -1121,51 +1130,126 @@ static int late_analysis(struct ubi_device *ubi, struct ubi_attach_info *ai)
 }
 
 /**
+ * destroy_av - free volume attaching information.
+ * @av: volume attaching information
+ * @ai: attaching information
+ *
+ * This function destroys the volume attaching information.
+ */
+static void destroy_av(struct ubi_attach_info *ai, struct ubi_ainf_volume *av)
+{
+	struct ubi_ainf_peb *aeb;
+	struct rb_node *this = av->root.rb_node;
+
+	while (this) {
+		if (this->rb_left)
+			this = this->rb_left;
+		else if (this->rb_right)
+			this = this->rb_right;
+		else {
+			aeb = rb_entry(this, struct ubi_ainf_peb, u.rb);
+			this = rb_parent(this);
+			if (this) {
+				if (this->rb_left == &aeb->u.rb)
+					this->rb_left = NULL;
+				else
+					this->rb_right = NULL;
+			}
+
+			kmem_cache_free(ai->aeb_slab_cache, aeb);
+		}
+	}
+	kfree(av);
+}
+
+/**
+ * destroy_ai - destroy attaching information.
+ * @ai: attaching information
+ */
+static void destroy_ai(struct ubi_attach_info *ai)
+{
+	struct ubi_ainf_peb *aeb, *aeb_tmp;
+	struct ubi_ainf_volume *av;
+	struct rb_node *rb;
+
+	list_for_each_entry_safe(aeb, aeb_tmp, &ai->alien, u.list) {
+		list_del(&aeb->u.list);
+		kmem_cache_free(ai->aeb_slab_cache, aeb);
+	}
+	list_for_each_entry_safe(aeb, aeb_tmp, &ai->erase, u.list) {
+		list_del(&aeb->u.list);
+		kmem_cache_free(ai->aeb_slab_cache, aeb);
+	}
+	list_for_each_entry_safe(aeb, aeb_tmp, &ai->corr, u.list) {
+		list_del(&aeb->u.list);
+		kmem_cache_free(ai->aeb_slab_cache, aeb);
+	}
+	list_for_each_entry_safe(aeb, aeb_tmp, &ai->free, u.list) {
+		list_del(&aeb->u.list);
+		kmem_cache_free(ai->aeb_slab_cache, aeb);
+	}
+
+	/* Destroy the volume RB-tree */
+	rb = ai->volumes.rb_node;
+	while (rb) {
+		if (rb->rb_left)
+			rb = rb->rb_left;
+		else if (rb->rb_right)
+			rb = rb->rb_right;
+		else {
+			av = rb_entry(rb, struct ubi_ainf_volume, rb);
+
+			rb = rb_parent(rb);
+			if (rb) {
+				if (rb->rb_left == &av->rb)
+					rb->rb_left = NULL;
+				else
+					rb->rb_right = NULL;
+			}
+
+			destroy_av(ai, av);
+		}
+	}
+
+	if (ai->aeb_slab_cache)
+		kmem_cache_destroy(ai->aeb_slab_cache);
+
+	kfree(ai);
+}
+
+/**
  * scan_all - scan entire MTD device.
  * @ubi: UBI device description object
+ * @ai: attach info object
+ * @start: start scanning at this PEB
  *
  * This function does full scanning of an MTD device and returns complete
  * information about it in form of a "struct ubi_attach_info" object. In case
  * of failure, an error code is returned.
  */
-static struct ubi_attach_info *scan_all(struct ubi_device *ubi)
+static int scan_all(struct ubi_device *ubi, struct ubi_attach_info *ai,
+		    int start)
 {
 	int err, pnum;
 	struct rb_node *rb1, *rb2;
 	struct ubi_ainf_volume *av;
 	struct ubi_ainf_peb *aeb;
-	struct ubi_attach_info *ai;
-
-	ai = kzalloc(sizeof(struct ubi_attach_info), GFP_KERNEL);
-	if (!ai)
-		return ERR_PTR(-ENOMEM);
-
-	INIT_LIST_HEAD(&ai->corr);
-	INIT_LIST_HEAD(&ai->free);
-	INIT_LIST_HEAD(&ai->erase);
-	INIT_LIST_HEAD(&ai->alien);
-	ai->volumes = RB_ROOT;
 
 	err = -ENOMEM;
-	ai->aeb_slab_cache = kmem_cache_create("ubi_aeb_slab_cache",
-					       sizeof(struct ubi_ainf_peb),
-					       0, 0, NULL);
-	if (!ai->aeb_slab_cache)
-		goto out_ai;
 
 	ech = kzalloc(ubi->ec_hdr_alsize, GFP_KERNEL);
 	if (!ech)
-		goto out_ai;
+		return err;
 
 	vidh = ubi_zalloc_vid_hdr(ubi, GFP_KERNEL);
 	if (!vidh)
 		goto out_ech;
 
-	for (pnum = 0; pnum < ubi->peb_count; pnum++) {
+	for (pnum = start; pnum < ubi->peb_count; pnum++) {
 		cond_resched();
 
 		dbg_gen("process PEB %d", pnum);
-		err = scan_peb(ubi, ai, pnum);
+		err = scan_peb(ubi, ai, pnum, NULL, NULL);
 		if (err < 0)
 			goto out_vidh;
 	}
@@ -1210,32 +1294,144 @@ static struct ubi_attach_info *scan_all(struct ubi_device *ubi)
 	ubi_free_vid_hdr(ubi, vidh);
 	kfree(ech);
 
-	return ai;
+	return 0;
 
 out_vidh:
 	ubi_free_vid_hdr(ubi, vidh);
 out_ech:
 	kfree(ech);
-out_ai:
-	ubi_destroy_ai(ai);
-	return ERR_PTR(err);
+	return err;
+}
+
+#ifdef CONFIG_MTD_UBI_FASTMAP
+
+/**
+ * scan_fastmap - try to find a fastmap and attach from it.
+ * @ubi: UBI device description object
+ * @ai: attach info object
+ *
+ * Returns 0 on success, negative return values indicate an internal
+ * error.
+ * UBI_NO_FASTMAP denotes that no fastmap was found.
+ * UBI_BAD_FASTMAP denotes that the found fastmap was invalid.
+ */
+static int scan_fast(struct ubi_device *ubi, struct ubi_attach_info *ai)
+{
+	int err, pnum, fm_anchor = -1;
+	unsigned long long max_sqnum = 0;
+
+	err = -ENOMEM;
+
+	ech = kzalloc(ubi->ec_hdr_alsize, GFP_KERNEL);
+	if (!ech)
+		goto out;
+
+	vidh = ubi_zalloc_vid_hdr(ubi, GFP_KERNEL);
+	if (!vidh)
+		goto out_ech;
+
+	for (pnum = 0; pnum < UBI_FM_MAX_START; pnum++) {
+		int vol_id = -1;
+		unsigned long long sqnum = -1;
+		cond_resched();
+
+		dbg_gen("process PEB %d", pnum);
+		err = scan_peb(ubi, ai, pnum, &vol_id, &sqnum);
+		if (err < 0)
+			goto out_vidh;
+
+		if (vol_id == UBI_FM_SB_VOLUME_ID && sqnum > max_sqnum) {
+			max_sqnum = sqnum;
+			fm_anchor = pnum;
+		}
+	}
+
+	ubi_free_vid_hdr(ubi, vidh);
+	kfree(ech);
+
+	if (fm_anchor < 0)
+		return UBI_NO_FASTMAP;
+
+	return ubi_scan_fastmap(ubi, ai, fm_anchor);
+
+out_vidh:
+	ubi_free_vid_hdr(ubi, vidh);
+out_ech:
+	kfree(ech);
+out:
+	return err;
+}
+
+#endif
+
+static struct ubi_attach_info *alloc_ai(const char *slab_name)
+{
+	struct ubi_attach_info *ai;
+
+	ai = kzalloc(sizeof(struct ubi_attach_info), GFP_KERNEL);
+	if (!ai)
+		return ai;
+
+	INIT_LIST_HEAD(&ai->corr);
+	INIT_LIST_HEAD(&ai->free);
+	INIT_LIST_HEAD(&ai->erase);
+	INIT_LIST_HEAD(&ai->alien);
+	ai->volumes = RB_ROOT;
+	ai->aeb_slab_cache = kmem_cache_create(slab_name,
+					       sizeof(struct ubi_ainf_peb),
+					       0, 0, NULL);
+	if (!ai->aeb_slab_cache) {
+		kfree(ai);
+		ai = NULL;
+	}
+
+	return ai;
 }
 
 /**
  * ubi_attach - attach an MTD device.
  * @ubi: UBI device descriptor
+ * @force_scan: if set to non-zero attach by scanning
  *
  * This function returns zero in case of success and a negative error code in
  * case of failure.
  */
-int ubi_attach(struct ubi_device *ubi)
+int ubi_attach(struct ubi_device *ubi, int force_scan)
 {
 	int err;
 	struct ubi_attach_info *ai;
 
-	ai = scan_all(ubi);
-	if (IS_ERR(ai))
-		return PTR_ERR(ai);
+	ai = alloc_ai("ubi_aeb_slab_cache");
+	if (!ai)
+		return -ENOMEM;
+
+#ifdef CONFIG_MTD_UBI_FASTMAP
+	/* On small flash devices we disable fastmap in any case. */
+	if ((int)mtd_div_by_eb(ubi->mtd->size, ubi->mtd) <= UBI_FM_MAX_START) {
+		ubi->fm_disabled = 1;
+		force_scan = 1;
+	}
+
+	if (force_scan)
+		err = scan_all(ubi, ai, 0);
+	else {
+		err = scan_fast(ubi, ai);
+		if (err > 0) {
+			if (err != UBI_NO_FASTMAP) {
+				destroy_ai(ai);
+				ai = alloc_ai("ubi_aeb_slab_cache2");
+				if (!ai)
+					return -ENOMEM;
+			}
+
+			err = scan_all(ubi, ai, UBI_FM_MAX_START);
+		}
+	}
+#else
+	err = scan_all(ubi, ai, 0);
+#endif
+	if (err)
+		goto out_ai;
 
 	ubi->bad_peb_count = ai->bad_peb_count;
 	ubi->good_peb_count = ubi->peb_count - ubi->bad_peb_count;
@@ -1256,7 +1452,29 @@ int ubi_attach(struct ubi_device *ubi)
 	if (err)
 		goto out_wl;
 
-	ubi_destroy_ai(ai);
+#ifdef CONFIG_MTD_UBI_FASTMAP
+	if (ubi->fm && ubi->dbg->chk_gen) {
+		struct ubi_attach_info *scan_ai;
+
+		scan_ai = alloc_ai("ubi_ckh_aeb_slab_cache");
+		if (!scan_ai)
+			goto out_wl;
+
+		err = scan_all(ubi, scan_ai, 0);
+		if (err) {
+			destroy_ai(scan_ai);
+			goto out_wl;
+		}
+
+		err = self_check_eba(ubi, ai, scan_ai);
+		destroy_ai(scan_ai);
+
+		if (err)
+			goto out_wl;
+	}
+#endif
+
+	destroy_ai(ai);
 	return 0;
 
 out_wl:
@@ -1265,99 +1483,11 @@ out_vtbl:
 	ubi_free_internal_volumes(ubi);
 	vfree(ubi->vtbl);
 out_ai:
-	ubi_destroy_ai(ai);
+	destroy_ai(ai);
 	return err;
 }
 
 /**
- * destroy_av - free volume attaching information.
- * @av: volume attaching information
- * @ai: attaching information
- *
- * This function destroys the volume attaching information.
- */
-static void destroy_av(struct ubi_attach_info *ai, struct ubi_ainf_volume *av)
-{
-	struct ubi_ainf_peb *aeb;
-	struct rb_node *this = av->root.rb_node;
-
-	while (this) {
-		if (this->rb_left)
-			this = this->rb_left;
-		else if (this->rb_right)
-			this = this->rb_right;
-		else {
-			aeb = rb_entry(this, struct ubi_ainf_peb, u.rb);
-			this = rb_parent(this);
-			if (this) {
-				if (this->rb_left == &aeb->u.rb)
-					this->rb_left = NULL;
-				else
-					this->rb_right = NULL;
-			}
-
-			kmem_cache_free(ai->aeb_slab_cache, aeb);
-		}
-	}
-	kfree(av);
-}
-
-/**
- * ubi_destroy_ai - destroy attaching information.
- * @ai: attaching information
- */
-void ubi_destroy_ai(struct ubi_attach_info *ai)
-{
-	struct ubi_ainf_peb *aeb, *aeb_tmp;
-	struct ubi_ainf_volume *av;
-	struct rb_node *rb;
-
-	list_for_each_entry_safe(aeb, aeb_tmp, &ai->alien, u.list) {
-		list_del(&aeb->u.list);
-		kmem_cache_free(ai->aeb_slab_cache, aeb);
-	}
-	list_for_each_entry_safe(aeb, aeb_tmp, &ai->erase, u.list) {
-		list_del(&aeb->u.list);
-		kmem_cache_free(ai->aeb_slab_cache, aeb);
-	}
-	list_for_each_entry_safe(aeb, aeb_tmp, &ai->corr, u.list) {
-		list_del(&aeb->u.list);
-		kmem_cache_free(ai->aeb_slab_cache, aeb);
-	}
-	list_for_each_entry_safe(aeb, aeb_tmp, &ai->free, u.list) {
-		list_del(&aeb->u.list);
-		kmem_cache_free(ai->aeb_slab_cache, aeb);
-	}
-
-	/* Destroy the volume RB-tree */
-	rb = ai->volumes.rb_node;
-	while (rb) {
-		if (rb->rb_left)
-			rb = rb->rb_left;
-		else if (rb->rb_right)
-			rb = rb->rb_right;
-		else {
-			av = rb_entry(rb, struct ubi_ainf_volume, rb);
-
-			rb = rb_parent(rb);
-			if (rb) {
-				if (rb->rb_left == &av->rb)
-					rb->rb_left = NULL;
-				else
-					rb->rb_right = NULL;
-			}
-
-			destroy_av(ai, av);
-		}
-	}
-
-	if (ai->aeb_slab_cache)
-		kmem_cache_destroy(ai->aeb_slab_cache);
-
-	kfree(ai);
-}
-
-/**
  * self_check_ai - check the attaching information.
  * @ubi: UBI device description object
  * @ai: attaching information
diff --git a/drivers/mtd/ubi/build.c b/drivers/mtd/ubi/build.c
index 34977039850..344b4cb49d4 100644
--- a/drivers/mtd/ubi/build.c
+++ b/drivers/mtd/ubi/build.c
@@ -76,7 +76,10 @@ static int __initdata mtd_devs;
 
 /* MTD devices specification parameters */
 static struct mtd_dev_param __initdata mtd_dev_param[UBI_MAX_DEVICES];
-
+#ifdef CONFIG_MTD_UBI_FASTMAP
+/* UBI module parameter to enable fastmap automatically on non-fastmap images */
+static bool fm_autoconvert;
+#endif
 /* Root UBI "class" object (corresponds to '/<sysfs>/class/ubi/') */
 struct class *ubi_class;
 
@@ -153,6 +156,19 @@ int ubi_volume_notify(struct ubi_device *ubi, struct ubi_volume *vol, int ntype)
 
 	ubi_do_get_device_info(ubi, &nt.di);
 	ubi_do_get_volume_info(ubi, vol, &nt.vi);
+
+#ifdef CONFIG_MTD_UBI_FASTMAP
+	switch (ntype) {
+	case UBI_VOLUME_ADDED:
+	case UBI_VOLUME_REMOVED:
+	case UBI_VOLUME_RESIZED:
+	case UBI_VOLUME_RENAMED:
+		if (ubi_update_fastmap(ubi)) {
+			ubi_err("Unable to update fastmap!");
+			ubi_ro_mode(ubi);
+		}
+	}
+#endif
 	return blocking_notifier_call_chain(&ubi_notifiers, ntype, &nt);
 }
 
@@ -918,10 +934,40 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num,
 	ubi->vid_hdr_offset = vid_hdr_offset;
 	ubi->autoresize_vol_id = -1;
 
+#ifdef CONFIG_MTD_UBI_FASTMAP
+	ubi->fm_pool.used = ubi->fm_pool.size = 0;
+	ubi->fm_wl_pool.used = ubi->fm_wl_pool.size = 0;
+
+	/*
+	 * fm_pool.max_size is 5% of the total number of PEBs but it's also
+	 * between UBI_FM_MAX_POOL_SIZE and UBI_FM_MIN_POOL_SIZE.
+	 */
+	ubi->fm_pool.max_size = min(((int)mtd_div_by_eb(ubi->mtd->size,
+		ubi->mtd) / 100) * 5, UBI_FM_MAX_POOL_SIZE);
+	if (ubi->fm_pool.max_size < UBI_FM_MIN_POOL_SIZE)
+		ubi->fm_pool.max_size = UBI_FM_MIN_POOL_SIZE;
+
+	ubi->fm_wl_pool.max_size = UBI_FM_WL_POOL_SIZE;
+	ubi->fm_disabled = !fm_autoconvert;
+
+	if (!ubi->fm_disabled && (int)mtd_div_by_eb(ubi->mtd->size, ubi->mtd)
+	    <= UBI_FM_MAX_START) {
+		ubi_err("More than %i PEBs are needed for fastmap, sorry.",
+			UBI_FM_MAX_START);
+		ubi->fm_disabled = 1;
+	}
+
+	ubi_msg("default fastmap pool size: %d", ubi->fm_pool.max_size);
+	ubi_msg("default fastmap WL pool size: %d", ubi->fm_wl_pool.max_size);
+#else
+	ubi->fm_disabled = 1;
+#endif
 	mutex_init(&ubi->buf_mutex);
 	mutex_init(&ubi->ckvol_mutex);
 	mutex_init(&ubi->device_mutex);
 	spin_lock_init(&ubi->volumes_lock);
+	mutex_init(&ubi->fm_mutex);
+	init_rwsem(&ubi->fm_sem);
 
 	ubi_msg("attaching mtd%d to ubi%d", mtd->index, ubi_num);
 
@@ -934,11 +980,17 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num,
 	if (!ubi->peb_buf)
 		goto out_free;
 
+#ifdef CONFIG_MTD_UBI_FASTMAP
+	ubi->fm_size = ubi_calc_fm_size(ubi);
+	ubi->fm_buf = vzalloc(ubi->fm_size);
+	if (!ubi->fm_buf)
+		goto out_free;
+#endif
 	err = ubi_debugging_init_dev(ubi);
 	if (err)
 		goto out_free;
 
-	err = ubi_attach(ubi);
+	err = ubi_attach(ubi, 0);
 	if (err) {
 		ubi_err("failed to attach mtd%d, error %d", mtd->index, err);
 		goto out_debugging;
@@ -1012,6 +1064,7 @@ out_debugging:
 	ubi_debugging_exit_dev(ubi);
 out_free:
 	vfree(ubi->peb_buf);
+	vfree(ubi->fm_buf);
 	if (ref)
 		put_device(&ubi->dev);
 	else
@@ -1061,7 +1114,11 @@ int ubi_detach_mtd_dev(int ubi_num, int anyway)
 	ubi_assert(ubi_num == ubi->ubi_num);
 	ubi_notify_all(ubi, UBI_VOLUME_REMOVED, NULL);
 	ubi_msg("detaching mtd%d from ubi%d", ubi->mtd->index, ubi_num);
-
+#ifdef CONFIG_MTD_UBI_FASTMAP
+	/* If we don't write a new fastmap at detach time we lose all
+	 * EC updates that have been made since the last written fastmap. */
+	ubi_update_fastmap(ubi);
+#endif
 	/*
 	 * Before freeing anything, we have to stop the background thread to
 	 * prevent it from doing anything on this device while we are freeing.
@@ -1077,12 +1134,14 @@ int ubi_detach_mtd_dev(int ubi_num, int anyway)
 
 	ubi_debugfs_exit_dev(ubi);
 	uif_close(ubi);
+
 	ubi_wl_close(ubi);
 	ubi_free_internal_volumes(ubi);
 	vfree(ubi->vtbl);
 	put_mtd_device(ubi->mtd);
 	ubi_debugging_exit_dev(ubi);
 	vfree(ubi->peb_buf);
+	vfree(ubi->fm_buf);
 	ubi_msg("mtd%d is detached from ubi%d", ubi->mtd->index, ubi->ubi_num);
 	put_device(&ubi->dev);
 	return 0;
@@ -1404,7 +1463,10 @@ MODULE_PARM_DESC(mtd, "MTD devices to attach. Parameter format: mtd=<name|num|pa
 		      "Example 2: mtd=content,1984 mtd=4 - attach MTD device with name \"content\" using VID header offset 1984, and MTD device number 4 with default VID header offset.\n"
 		      "Example 3: mtd=/dev/mtd1,0,25 - attach MTD device /dev/mtd1 using default VID header offset and reserve 25*nand_size_in_blocks/1024 erase blocks for bad block handling.\n"
 		      "\t(e.g. if the NAND *chipset* has 4096 PEB, 100 will be reserved for this UBI device).");
-
+#ifdef CONFIG_MTD_UBI_FASTMAP
+module_param(fm_autoconvert, bool, 0644);
+MODULE_PARM_DESC(fm_autoconvert, "Set this parameter to enable fastmap automatically on images without a fastmap.");
+#endif
 MODULE_VERSION(__stringify(UBI_VERSION));
 MODULE_DESCRIPTION("UBI - Unsorted Block Images");
 MODULE_AUTHOR("Artem Bityutskiy");
diff --git a/drivers/mtd/ubi/eba.c b/drivers/mtd/ubi/eba.c
index a26d7d25317..0e11671dadc 100644
--- a/drivers/mtd/ubi/eba.c
+++ b/drivers/mtd/ubi/eba.c
@@ -57,7 +57,7 @@
  * global sequence counter value. It also increases the global sequence
  * counter.
  */
-static unsigned long long next_sqnum(struct ubi_device *ubi)
+unsigned long long ubi_next_sqnum(struct ubi_device *ubi)
 {
 	unsigned long long sqnum;
 
@@ -340,7 +340,9 @@ int ubi_eba_unmap_leb(struct ubi_device *ubi, struct ubi_volume *vol,
 
 	dbg_eba("erase LEB %d:%d, PEB %d", vol_id, lnum, pnum);
 
+	down_read(&ubi->fm_sem);
 	vol->eba_tbl[lnum] = UBI_LEB_UNMAPPED;
+	up_read(&ubi->fm_sem);
 	err = ubi_wl_put_peb(ubi, vol_id, lnum, pnum, 0);
 
 out_unlock:
@@ -521,7 +523,7 @@ retry:
 		goto out_put;
 	}
 
-	vid_hdr->sqnum = cpu_to_be64(next_sqnum(ubi));
+	vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
 	err = ubi_io_write_vid_hdr(ubi, new_pnum, vid_hdr);
 	if (err)
 		goto write_error;
@@ -548,7 +550,9 @@ retry:
 	mutex_unlock(&ubi->buf_mutex);
 	ubi_free_vid_hdr(ubi, vid_hdr);
 
+	down_read(&ubi->fm_sem);
 	vol->eba_tbl[lnum] = new_pnum;
+	up_read(&ubi->fm_sem);
 	ubi_wl_put_peb(ubi, vol_id, lnum, pnum, 1);
 
 	ubi_msg("data was successfully recovered");
@@ -632,7 +636,7 @@ int ubi_eba_write_leb(struct ubi_device *ubi, struct ubi_volume *vol, int lnum,
 	}
 
 	vid_hdr->vol_type = UBI_VID_DYNAMIC;
-	vid_hdr->sqnum = cpu_to_be64(next_sqnum(ubi));
+	vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
 	vid_hdr->vol_id = cpu_to_be32(vol_id);
 	vid_hdr->lnum = cpu_to_be32(lnum);
 	vid_hdr->compat = ubi_get_compat(ubi, vol_id);
@@ -665,7 +669,9 @@ retry:
 		}
 	}
 
+	down_read(&ubi->fm_sem);
 	vol->eba_tbl[lnum] = pnum;
+	up_read(&ubi->fm_sem);
 
 	leb_write_unlock(ubi, vol_id, lnum);
 	ubi_free_vid_hdr(ubi, vid_hdr);
@@ -692,7 +698,7 @@ write_error:
 		return err;
 	}
 
-	vid_hdr->sqnum = cpu_to_be64(next_sqnum(ubi));
+	vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
 	ubi_msg("try another PEB");
 	goto retry;
 }
@@ -745,7 +751,7 @@ int ubi_eba_write_leb_st(struct ubi_device *ubi, struct ubi_volume *vol,
 		return err;
 	}
 
-	vid_hdr->sqnum = cpu_to_be64(next_sqnum(ubi));
+	vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
 	vid_hdr->vol_id = cpu_to_be32(vol_id);
 	vid_hdr->lnum = cpu_to_be32(lnum);
 	vid_hdr->compat = ubi_get_compat(ubi, vol_id);
@@ -783,7 +789,9 @@ retry:
 	}
 
 	ubi_assert(vol->eba_tbl[lnum] < 0);
+	down_read(&ubi->fm_sem);
 	vol->eba_tbl[lnum] = pnum;
+	up_read(&ubi->fm_sem);
 
 	leb_write_unlock(ubi, vol_id, lnum);
 	ubi_free_vid_hdr(ubi, vid_hdr);
@@ -810,7 +818,7 @@ write_error:
 		return err;
 	}
 
-	vid_hdr->sqnum = cpu_to_be64(next_sqnum(ubi));
+	vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
 	ubi_msg("try another PEB");
 	goto retry;
 }
@@ -862,7 +870,7 @@ int ubi_eba_atomic_leb_change(struct ubi_device *ubi, struct ubi_volume *vol,
 	if (err)
 		goto out_mutex;
 
-	vid_hdr->sqnum = cpu_to_be64(next_sqnum(ubi));
+	vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
 	vid_hdr->vol_id = cpu_to_be32(vol_id);
 	vid_hdr->lnum = cpu_to_be32(lnum);
 	vid_hdr->compat = ubi_get_compat(ubi, vol_id);
@@ -904,7 +912,9 @@ retry:
 			goto out_leb_unlock;
 	}
 
+	down_read(&ubi->fm_sem);
 	vol->eba_tbl[lnum] = pnum;
+	up_read(&ubi->fm_sem);
 
 out_leb_unlock:
 	leb_write_unlock(ubi, vol_id, lnum);
@@ -930,7 +940,7 @@ write_error:
 		goto out_leb_unlock;
 	}
 
-	vid_hdr->sqnum = cpu_to_be64(next_sqnum(ubi));
+	vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
 	ubi_msg("try another PEB");
 	goto retry;
 }
@@ -1089,7 +1099,7 @@ int ubi_eba_copy_leb(struct ubi_device *ubi, int from, int to,
 		vid_hdr->data_size = cpu_to_be32(data_size);
 		vid_hdr->data_crc = cpu_to_be32(crc);
 	}
-	vid_hdr->sqnum = cpu_to_be64(next_sqnum(ubi));
+	vid_hdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
 
 	err = ubi_io_write_vid_hdr(ubi, to, vid_hdr);
 	if (err) {
@@ -1151,7 +1161,9 @@ int ubi_eba_copy_leb(struct ubi_device *ubi, int from, int to,
 	}
 
 	ubi_assert(vol->eba_tbl[lnum] == from);
+	down_read(&ubi->fm_sem);
 	vol->eba_tbl[lnum] = to;
+	up_read(&ubi->fm_sem);
 
 out_unlock_buf:
 	mutex_unlock(&ubi->buf_mutex);
@@ -1202,6 +1214,102 @@ static void print_rsvd_warning(struct ubi_device *ubi,
 }
 
 /**
+ * self_check_eba - run a self check on the EBA table constructed by fastmap.
+ * @ubi: UBI device description object
+ * @ai_fastmap: UBI attach info object created by fastmap
+ * @ai_scan: UBI attach info object created by scanning
+ *
+ * Returns < 0 in case of an internal error, 0 otherwise.
+ * If a bad EBA table entry was found it will be printed out and
+ * ubi_assert() triggers.
+ */
+int self_check_eba(struct ubi_device *ubi, struct ubi_attach_info *ai_fastmap,
+		   struct ubi_attach_info *ai_scan)
+{
+	int i, j, num_volumes, ret = 0;
+	int **scan_eba, **fm_eba;
+	struct ubi_ainf_volume *av;
+	struct ubi_volume *vol;
+	struct ubi_ainf_peb *aeb;
+	struct rb_node *rb;
+
+	num_volumes = ubi->vtbl_slots + UBI_INT_VOL_COUNT;
+
+	scan_eba = kmalloc(sizeof(*scan_eba) * num_volumes, GFP_KERNEL);
+	if (!scan_eba)
+		return -ENOMEM;
+
+	fm_eba = kmalloc(sizeof(*fm_eba) * num_volumes, GFP_KERNEL);
+	if (!fm_eba) {
+		kfree(scan_eba);
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < num_volumes; i++) {
+		vol = ubi->volumes[i];
+		if (!vol)
+			continue;
+
+		scan_eba[i] = kmalloc(vol->reserved_pebs * sizeof(**scan_eba),
+				      GFP_KERNEL);
+		if (!scan_eba[i]) {
+			ret = -ENOMEM;
+			goto out_free;
+		}
+
+		fm_eba[i] = kmalloc(vol->reserved_pebs * sizeof(**fm_eba),
+				    GFP_KERNEL);
+		if (!fm_eba[i]) {
+			ret = -ENOMEM;
+			goto out_free;
+		}
+
+		for (j = 0; j < vol->reserved_pebs; j++)
+			scan_eba[i][j] = fm_eba[i][j] = UBI_LEB_UNMAPPED;
+
+		av = ubi_find_av(ai_scan, idx2vol_id(ubi, i));
+		if (!av)
+			continue;
+
+		ubi_rb_for_each_entry(rb, aeb, &av->root, u.rb)
+			scan_eba[i][aeb->lnum] = aeb->pnum;
+
+		av = ubi_find_av(ai_fastmap, idx2vol_id(ubi, i));
+		if (!av)
+			continue;
+
+		ubi_rb_for_each_entry(rb, aeb, &av->root, u.rb)
+			fm_eba[i][aeb->lnum] = aeb->pnum;
+
+		for (j = 0; j < vol->reserved_pebs; j++) {
+			if (scan_eba[i][j] != fm_eba[i][j]) {
+				if (scan_eba[i][j] == UBI_LEB_UNMAPPED ||
+					fm_eba[i][j] == UBI_LEB_UNMAPPED)
+					continue;
+
+				ubi_err("LEB:%i:%i is PEB:%i instead of %i!",
+					vol->vol_id, i, fm_eba[i][j],
+					scan_eba[i][j]);
+				ubi_assert(0);
+			}
+		}
+	}
+
+out_free:
+	for (i = 0; i < num_volumes; i++) {
+		if (!ubi->volumes[i])
+			continue;
+
+		kfree(scan_eba[i]);
+		kfree(fm_eba[i]);
+	}
+
+	kfree(scan_eba);
+	kfree(fm_eba);
+	return ret;
+}
+
+/**
  * ubi_eba_init - initialize the EBA sub-system using attaching information.
  * @ubi: UBI device description object
  * @ai: attaching information
diff --git a/drivers/mtd/ubi/fastmap.c b/drivers/mtd/ubi/fastmap.c
new file mode 100644
index 00000000000..1a5f53c090d
--- /dev/null
+++ b/drivers/mtd/ubi/fastmap.c
@@ -0,0 +1,1537 @@
+/*
+ * Copyright (c) 2012 Linutronix GmbH
+ * Author: Richard Weinberger <richard@nod.at>
+ *
+ * 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; version 2.
+ *
+ * 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/crc32.h>
+#include "ubi.h"
+
+/**
+ * ubi_calc_fm_size - calculates the fastmap size in bytes for an UBI device.
+ * @ubi: UBI device description object
+ */
+size_t ubi_calc_fm_size(struct ubi_device *ubi)
+{
+	size_t size;
+
+	size = sizeof(struct ubi_fm_hdr) + \
+		sizeof(struct ubi_fm_scan_pool) + \
+		sizeof(struct ubi_fm_scan_pool) + \
+		(ubi->peb_count * sizeof(struct ubi_fm_ec)) + \
+		(sizeof(struct ubi_fm_eba) + \
+		(ubi->peb_count * sizeof(__be32))) + \
+		sizeof(struct ubi_fm_volhdr) * UBI_MAX_VOLUMES;
+	return roundup(size, ubi->leb_size);
+}
+
+
+/**
+ * new_fm_vhdr - allocate a new volume header for fastmap usage.
+ * @ubi: UBI device description object
+ * @vol_id: the VID of the new header
+ *
+ * Returns a new struct ubi_vid_hdr on success.
+ * NULL indicates out of memory.
+ */
+static struct ubi_vid_hdr *new_fm_vhdr(struct ubi_device *ubi, int vol_id)
+{
+	struct ubi_vid_hdr *new;
+
+	new = ubi_zalloc_vid_hdr(ubi, GFP_KERNEL);
+	if (!new)
+		goto out;
+
+	new->vol_type = UBI_VID_DYNAMIC;
+	new->vol_id = cpu_to_be32(vol_id);
+
+	/* UBI implementations without fastmap support have to delete the
+	 * fastmap.
+	 */
+	new->compat = UBI_COMPAT_DELETE;
+
+out:
+	return new;
+}
+
+/**
+ * add_aeb - create and add a attach erase block to a given list.
+ * @ai: UBI attach info object
+ * @list: the target list
+ * @pnum: PEB number of the new attach erase block
+ * @ec: erease counter of the new LEB
+ * @scrub: scrub this PEB after attaching
+ *
+ * Returns 0 on success, < 0 indicates an internal error.
+ */
+static int add_aeb(struct ubi_attach_info *ai, struct list_head *list,
+		   int pnum, int ec, int scrub)
+{
+	struct ubi_ainf_peb *aeb;
+
+	aeb = kmem_cache_alloc(ai->aeb_slab_cache, GFP_KERNEL);
+	if (!aeb)
+		return -ENOMEM;
+
+	aeb->pnum = pnum;
+	aeb->ec = ec;
+	aeb->lnum = -1;
+	aeb->scrub = scrub;
+	aeb->copy_flag = aeb->sqnum = 0;
+
+	ai->ec_sum += aeb->ec;
+	ai->ec_count++;
+
+	if (ai->max_ec < aeb->ec)
+		ai->max_ec = aeb->ec;
+
+	if (ai->min_ec > aeb->ec)
+		ai->min_ec = aeb->ec;
+
+	list_add_tail(&aeb->u.list, list);
+
+	return 0;
+}
+
+/**
+ * add_vol - create and add a new volume to ubi_attach_info.
+ * @ai: ubi_attach_info object
+ * @vol_id: VID of the new volume
+ * @used_ebs: number of used EBS
+ * @data_pad: data padding value of the new volume
+ * @vol_type: volume type
+ * @last_eb_bytes: number of bytes in the last LEB
+ *
+ * Returns the new struct ubi_ainf_volume on success.
+ * NULL indicates an error.
+ */
+static struct ubi_ainf_volume *add_vol(struct ubi_attach_info *ai, int vol_id,
+				       int used_ebs, int data_pad, u8 vol_type,
+				       int last_eb_bytes)
+{
+	struct ubi_ainf_volume *av;
+	struct rb_node **p = &ai->volumes.rb_node, *parent = NULL;
+
+	while (*p) {
+		parent = *p;
+		av = rb_entry(parent, struct ubi_ainf_volume, rb);
+
+		if (vol_id > av->vol_id)
+			p = &(*p)->rb_left;
+		else if (vol_id > av->vol_id)
+			p = &(*p)->rb_right;
+	}
+
+	av = kmalloc(sizeof(struct ubi_ainf_volume), GFP_KERNEL);
+	if (!av)
+		goto out;
+
+	av->highest_lnum = av->leb_count = 0;
+	av->vol_id = vol_id;
+	av->used_ebs = used_ebs;
+	av->data_pad = data_pad;
+	av->last_data_size = last_eb_bytes;
+	av->compat = 0;
+	av->vol_type = vol_type;
+	av->root = RB_ROOT;
+
+	dbg_bld("found volume (ID %i)", vol_id);
+
+	rb_link_node(&av->rb, parent, p);
+	rb_insert_color(&av->rb, &ai->volumes);
+
+out:
+	return av;
+}
+
+/**
+ * assign_aeb_to_av - assigns a SEB to a given ainf_volume and removes it
+ * from it's original list.
+ * @ai: ubi_attach_info object
+ * @aeb: the to be assigned SEB
+ * @av: target scan volume
+ */
+static void assign_aeb_to_av(struct ubi_attach_info *ai,
+			     struct ubi_ainf_peb *aeb,
+			     struct ubi_ainf_volume *av)
+{
+	struct ubi_ainf_peb *tmp_aeb;
+	struct rb_node **p = &ai->volumes.rb_node, *parent = NULL;
+
+	p = &av->root.rb_node;
+	while (*p) {
+		parent = *p;
+
+		tmp_aeb = rb_entry(parent, struct ubi_ainf_peb, u.rb);
+		if (aeb->lnum != tmp_aeb->lnum) {
+			if (aeb->lnum < tmp_aeb->lnum)
+				p = &(*p)->rb_left;
+			else
+				p = &(*p)->rb_right;
+
+			continue;
+		} else
+			break;
+	}
+
+	list_del(&aeb->u.list);
+	av->leb_count++;
+
+	rb_link_node(&aeb->u.rb, parent, p);
+	rb_insert_color(&aeb->u.rb, &av->root);
+}
+
+/**
+ * update_vol - inserts or updates a LEB which was found a pool.
+ * @ubi: the UBI device object
+ * @ai: attach info object
+ * @av: the volume this LEB belongs to
+ * @new_vh: the volume header derived from new_aeb
+ * @new_aeb: the AEB to be examined
+ *
+ * Returns 0 on success, < 0 indicates an internal error.
+ */
+static int update_vol(struct ubi_device *ubi, struct ubi_attach_info *ai,
+		      struct ubi_ainf_volume *av, struct ubi_vid_hdr *new_vh,
+		      struct ubi_ainf_peb *new_aeb)
+{
+	struct rb_node **p = &av->root.rb_node, *parent = NULL;
+	struct ubi_ainf_peb *aeb, *victim;
+	int cmp_res;
+
+	while (*p) {
+		parent = *p;
+		aeb = rb_entry(parent, struct ubi_ainf_peb, u.rb);
+
+		if (be32_to_cpu(new_vh->lnum) != aeb->lnum) {
+			if (be32_to_cpu(new_vh->lnum) < aeb->lnum)
+				p = &(*p)->rb_left;
+			else
+				p = &(*p)->rb_right;
+
+			continue;
+		}
+
+		/* This case can happen if the fastmap gets written
+		 * because of a volume change (creation, deletion, ..).
+		 * Then a PEB can be within the persistent EBA and the pool.
+		 */
+		if (aeb->pnum == new_aeb->pnum) {
+			ubi_assert(aeb->lnum == new_aeb->lnum);
+			kmem_cache_free(ai->aeb_slab_cache, new_aeb);
+
+			return 0;
+		}
+
+		cmp_res = ubi_compare_lebs(ubi, aeb, new_aeb->pnum, new_vh);
+		if (cmp_res < 0)
+			return cmp_res;
+
+		/* new_aeb is newer */
+		if (cmp_res & 1) {
+			victim = kmem_cache_alloc(ai->aeb_slab_cache,
+				GFP_KERNEL);
+			if (!victim)
+				return -ENOMEM;
+
+			victim->ec = aeb->ec;
+			victim->pnum = aeb->pnum;
+			list_add_tail(&victim->u.list, &ai->erase);
+
+			if (av->highest_lnum == be32_to_cpu(new_vh->lnum))
+				av->last_data_size = \
+					be32_to_cpu(new_vh->data_size);
+
+			dbg_bld("vol %i: AEB %i's PEB %i is the newer",
+				av->vol_id, aeb->lnum, new_aeb->pnum);
+
+			aeb->ec = new_aeb->ec;
+			aeb->pnum = new_aeb->pnum;
+			aeb->copy_flag = new_vh->copy_flag;
+			aeb->scrub = new_aeb->scrub;
+			kmem_cache_free(ai->aeb_slab_cache, new_aeb);
+
+		/* new_aeb is older */
+		} else {
+			dbg_bld("vol %i: AEB %i's PEB %i is old, dropping it",
+				av->vol_id, aeb->lnum, new_aeb->pnum);
+			list_add_tail(&new_aeb->u.list, &ai->erase);
+		}
+
+		return 0;
+	}
+	/* This LEB is new, let's add it to the volume */
+
+	if (av->highest_lnum <= be32_to_cpu(new_vh->lnum)) {
+		av->highest_lnum = be32_to_cpu(new_vh->lnum);
+		av->last_data_size = be32_to_cpu(new_vh->data_size);
+	}
+
+	if (av->vol_type == UBI_STATIC_VOLUME)
+		av->used_ebs = be32_to_cpu(new_vh->used_ebs);
+
+	av->leb_count++;
+
+	rb_link_node(&new_aeb->u.rb, parent, p);
+	rb_insert_color(&new_aeb->u.rb, &av->root);
+
+	return 0;
+}
+
+/**
+ * process_pool_aeb - we found a non-empty PEB in a pool.
+ * @ubi: UBI device object
+ * @ai: attach info object
+ * @new_vh: the volume header derived from new_aeb
+ * @new_aeb: the AEB to be examined
+ *
+ * Returns 0 on success, < 0 indicates an internal error.
+ */
+static int process_pool_aeb(struct ubi_device *ubi, struct ubi_attach_info *ai,
+			    struct ubi_vid_hdr *new_vh,
+			    struct ubi_ainf_peb *new_aeb)
+{
+	struct ubi_ainf_volume *av, *tmp_av = NULL;
+	struct rb_node **p = &ai->volumes.rb_node, *parent = NULL;
+	int found = 0;
+
+	if (be32_to_cpu(new_vh->vol_id) == UBI_FM_SB_VOLUME_ID ||
+		be32_to_cpu(new_vh->vol_id) == UBI_FM_DATA_VOLUME_ID) {
+		kmem_cache_free(ai->aeb_slab_cache, new_aeb);
+
+		return 0;
+	}
+
+	/* Find the volume this SEB belongs to */
+	while (*p) {
+		parent = *p;
+		tmp_av = rb_entry(parent, struct ubi_ainf_volume, rb);
+
+		if (be32_to_cpu(new_vh->vol_id) > tmp_av->vol_id)
+			p = &(*p)->rb_left;
+		else if (be32_to_cpu(new_vh->vol_id) < tmp_av->vol_id)
+			p = &(*p)->rb_right;
+		else {
+			found = 1;
+			break;
+		}
+	}
+
+	if (found)
+		av = tmp_av;
+	else {
+		ubi_err("orphaned volume in fastmap pool!");
+		return UBI_BAD_FASTMAP;
+	}
+
+	ubi_assert(be32_to_cpu(new_vh->vol_id) == av->vol_id);
+
+	return update_vol(ubi, ai, av, new_vh, new_aeb);
+}
+
+/**
+ * unmap_peb - unmap a PEB.
+ * If fastmap detects a free PEB in the pool it has to check whether
+ * this PEB has been unmapped after writing the fastmap.
+ *
+ * @ai: UBI attach info object
+ * @pnum: The PEB to be unmapped
+ */
+static void unmap_peb(struct ubi_attach_info *ai, int pnum)
+{
+	struct ubi_ainf_volume *av;
+	struct rb_node *node, *node2;
+	struct ubi_ainf_peb *aeb;
+
+	for (node = rb_first(&ai->volumes); node; node = rb_next(node)) {
+		av = rb_entry(node, struct ubi_ainf_volume, rb);
+
+		for (node2 = rb_first(&av->root); node2;
+		     node2 = rb_next(node2)) {
+			aeb = rb_entry(node2, struct ubi_ainf_peb, u.rb);
+			if (aeb->pnum == pnum) {
+				rb_erase(&aeb->u.rb, &av->root);
+				kmem_cache_free(ai->aeb_slab_cache, aeb);
+				return;
+			}
+		}
+	}
+}
+
+/**
+ * scan_pool - scans a pool for changed (no longer empty PEBs).
+ * @ubi: UBI device object
+ * @ai: attach info object
+ * @pebs: an array of all PEB numbers in the to be scanned pool
+ * @pool_size: size of the pool (number of entries in @pebs)
+ * @max_sqnum: pointer to the maximal sequence number
+ * @eba_orphans: list of PEBs which need to be scanned
+ * @free: list of PEBs which are most likely free (and go into @ai->free)
+ *
+ * Returns 0 on success, if the pool is unusable UBI_BAD_FASTMAP is returned.
+ * < 0 indicates an internal error.
+ */
+static int scan_pool(struct ubi_device *ubi, struct ubi_attach_info *ai,
+		     int *pebs, int pool_size, unsigned long long *max_sqnum,
+		     struct list_head *eba_orphans, struct list_head *free)
+{
+	struct ubi_vid_hdr *vh;
+	struct ubi_ec_hdr *ech;
+	struct ubi_ainf_peb *new_aeb, *tmp_aeb;
+	int i, pnum, err, found_orphan, ret = 0;
+
+	ech = kzalloc(ubi->ec_hdr_alsize, GFP_KERNEL);
+	if (!ech)
+		return -ENOMEM;
+
+	vh = ubi_zalloc_vid_hdr(ubi, GFP_KERNEL);
+	if (!vh) {
+		kfree(ech);
+		return -ENOMEM;
+	}
+
+	dbg_bld("scanning fastmap pool: size = %i", pool_size);
+
+	/*
+	 * Now scan all PEBs in the pool to find changes which have been made
+	 * after the creation of the fastmap
+	 */
+	for (i = 0; i < pool_size; i++) {
+		int scrub = 0;
+
+		pnum = be32_to_cpu(pebs[i]);
+
+		if (ubi_io_is_bad(ubi, pnum)) {
+			ubi_err("bad PEB in fastmap pool!");
+			ret = UBI_BAD_FASTMAP;
+			goto out;
+		}
+
+		err = ubi_io_read_ec_hdr(ubi, pnum, ech, 0);
+		if (err && err != UBI_IO_BITFLIPS) {
+			ubi_err("unable to read EC header! PEB:%i err:%i",
+				pnum, err);
+			ret = err > 0 ? UBI_BAD_FASTMAP : err;
+			goto out;
+		} else if (ret == UBI_IO_BITFLIPS)
+			scrub = 1;
+
+		if (be32_to_cpu(ech->image_seq) != ubi->image_seq) {
+			ubi_err("bad image seq: 0x%x, expected: 0x%x",
+				be32_to_cpu(ech->image_seq), ubi->image_seq);
+			err = UBI_BAD_FASTMAP;
+			goto out;
+		}
+
+		err = ubi_io_read_vid_hdr(ubi, pnum, vh, 0);
+		if (err == UBI_IO_FF || err == UBI_IO_FF_BITFLIPS) {
+			unsigned long long ec = be64_to_cpu(ech->ec);
+			unmap_peb(ai, pnum);
+			dbg_bld("Adding PEB to free: %i", pnum);
+			if (err == UBI_IO_FF_BITFLIPS)
+				add_aeb(ai, free, pnum, ec, 1);
+			else
+				add_aeb(ai, free, pnum, ec, 0);
+			continue;
+		} else if (err == 0 || err == UBI_IO_BITFLIPS) {
+			dbg_bld("Found non empty PEB:%i in pool", pnum);
+
+			if (err == UBI_IO_BITFLIPS)
+				scrub = 1;
+
+			found_orphan = 0;
+			list_for_each_entry(tmp_aeb, eba_orphans, u.list) {
+				if (tmp_aeb->pnum == pnum) {
+					found_orphan = 1;
+					break;
+				}
+			}
+			if (found_orphan) {
+				kmem_cache_free(ai->aeb_slab_cache, tmp_aeb);
+				list_del(&tmp_aeb->u.list);
+			}
+
+			new_aeb = kmem_cache_alloc(ai->aeb_slab_cache,
+						   GFP_KERNEL);
+			if (!new_aeb) {
+				ret = -ENOMEM;
+				goto out;
+			}
+
+			new_aeb->ec = be64_to_cpu(ech->ec);
+			new_aeb->pnum = pnum;
+			new_aeb->lnum = be32_to_cpu(vh->lnum);
+			new_aeb->sqnum = be64_to_cpu(vh->sqnum);
+			new_aeb->copy_flag = vh->copy_flag;
+			new_aeb->scrub = scrub;
+
+			if (*max_sqnum < new_aeb->sqnum)
+				*max_sqnum = new_aeb->sqnum;
+
+			err = process_pool_aeb(ubi, ai, vh, new_aeb);
+			if (err) {
+				ret = err > 0 ? UBI_BAD_FASTMAP : err;
+				goto out;
+			}
+		} else {
+			/* We are paranoid and fall back to scanning mode */
+			ubi_err("fastmap pool PEBs contains damaged PEBs!");
+			ret = err > 0 ? UBI_BAD_FASTMAP : err;
+			goto out;
+		}
+
+	}
+
+out:
+	ubi_free_vid_hdr(ubi, vh);
+	kfree(ech);
+	return ret;
+}
+
+/**
+ * count_fastmap_pebs - Counts the PEBs found by fastmap.
+ * @ai: The UBI attach info object
+ */
+static int count_fastmap_pebs(struct ubi_attach_info *ai)
+{
+	struct ubi_ainf_peb *aeb;
+	struct ubi_ainf_volume *av;
+	struct rb_node *rb1, *rb2;
+	int n = 0;
+
+	list_for_each_entry(aeb, &ai->erase, u.list)
+		n++;
+
+	list_for_each_entry(aeb, &ai->free, u.list)
+		n++;
+
+	 ubi_rb_for_each_entry(rb1, av, &ai->volumes, rb)
+		ubi_rb_for_each_entry(rb2, aeb, &av->root, u.rb)
+			n++;
+
+	return n;
+}
+
+/**
+ * ubi_attach_fastmap - creates ubi_attach_info from a fastmap.
+ * @ubi: UBI device object
+ * @ai: UBI attach info object
+ * @fm: the fastmap to be attached
+ *
+ * Returns 0 on success, UBI_BAD_FASTMAP if the found fastmap was unusable.
+ * < 0 indicates an internal error.
+ */
+static int ubi_attach_fastmap(struct ubi_device *ubi,
+			      struct ubi_attach_info *ai,
+			      struct ubi_fastmap_layout *fm)
+{
+	struct list_head used, eba_orphans, free;
+	struct ubi_ainf_volume *av;
+	struct ubi_ainf_peb *aeb, *tmp_aeb, *_tmp_aeb;
+	struct ubi_ec_hdr *ech;
+	struct ubi_fm_sb *fmsb;
+	struct ubi_fm_hdr *fmhdr;
+	struct ubi_fm_scan_pool *fmpl1, *fmpl2;
+	struct ubi_fm_ec *fmec;
+	struct ubi_fm_volhdr *fmvhdr;
+	struct ubi_fm_eba *fm_eba;
+	int ret, i, j, pool_size, wl_pool_size;
+	size_t fm_pos = 0, fm_size = ubi->fm_size;
+	unsigned long long max_sqnum = 0;
+	void *fm_raw = ubi->fm_buf;
+
+	INIT_LIST_HEAD(&used);
+	INIT_LIST_HEAD(&free);
+	INIT_LIST_HEAD(&eba_orphans);
+	INIT_LIST_HEAD(&ai->corr);
+	INIT_LIST_HEAD(&ai->free);
+	INIT_LIST_HEAD(&ai->erase);
+	INIT_LIST_HEAD(&ai->alien);
+	ai->volumes = RB_ROOT;
+	ai->min_ec = UBI_MAX_ERASECOUNTER;
+
+	ai->aeb_slab_cache = kmem_cache_create("ubi_ainf_peb_slab",
+					       sizeof(struct ubi_ainf_peb),
+					       0, 0, NULL);
+	if (!ai->aeb_slab_cache) {
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	fmsb = (struct ubi_fm_sb *)(fm_raw);
+	ai->max_sqnum = fmsb->sqnum;
+	fm_pos += sizeof(struct ubi_fm_sb);
+	if (fm_pos >= fm_size)
+		goto fail_bad;
+
+	fmhdr = (struct ubi_fm_hdr *)(fm_raw + fm_pos);
+	fm_pos += sizeof(*fmhdr);
+	if (fm_pos >= fm_size)
+		goto fail_bad;
+
+	if (be32_to_cpu(fmhdr->magic) != UBI_FM_HDR_MAGIC) {
+		ubi_err("bad fastmap header magic: 0x%x, expected: 0x%x",
+			be32_to_cpu(fmhdr->magic), UBI_FM_HDR_MAGIC);
+		goto fail_bad;
+	}
+
+	fmpl1 = (struct ubi_fm_scan_pool *)(fm_raw + fm_pos);
+	fm_pos += sizeof(*fmpl1);
+	if (fm_pos >= fm_size)
+		goto fail_bad;
+	if (be32_to_cpu(fmpl1->magic) != UBI_FM_POOL_MAGIC) {
+		ubi_err("bad fastmap pool magic: 0x%x, expected: 0x%x",
+			be32_to_cpu(fmpl1->magic), UBI_FM_POOL_MAGIC);
+		goto fail_bad;
+	}
+
+	fmpl2 = (struct ubi_fm_scan_pool *)(fm_raw + fm_pos);
+	fm_pos += sizeof(*fmpl2);
+	if (fm_pos >= fm_size)
+		goto fail_bad;
+	if (be32_to_cpu(fmpl2->magic) != UBI_FM_POOL_MAGIC) {
+		ubi_err("bad fastmap pool magic: 0x%x, expected: 0x%x",
+			be32_to_cpu(fmpl2->magic), UBI_FM_POOL_MAGIC);
+		goto fail_bad;
+	}
+
+	pool_size = be16_to_cpu(fmpl1->size);
+	wl_pool_size = be16_to_cpu(fmpl2->size);
+	fm->max_pool_size = be16_to_cpu(fmpl1->max_size);
+	fm->max_wl_pool_size = be16_to_cpu(fmpl2->max_size);
+
+	if (pool_size > UBI_FM_MAX_POOL_SIZE || pool_size < 0) {
+		ubi_err("bad pool size: %i", pool_size);
+		goto fail_bad;
+	}
+
+	if (wl_pool_size > UBI_FM_MAX_POOL_SIZE || wl_pool_size < 0) {
+		ubi_err("bad WL pool size: %i", wl_pool_size);
+		goto fail_bad;
+	}
+
+
+	if (fm->max_pool_size > UBI_FM_MAX_POOL_SIZE ||
+	    fm->max_pool_size < 0) {
+		ubi_err("bad maximal pool size: %i", fm->max_pool_size);
+		goto fail_bad;
+	}
+
+	if (fm->max_wl_pool_size > UBI_FM_MAX_POOL_SIZE ||
+	    fm->max_wl_pool_size < 0) {
+		ubi_err("bad maximal WL pool size: %i", fm->max_wl_pool_size);
+		goto fail_bad;
+	}
+
+	/* read EC values from free list */
+	for (i = 0; i < be32_to_cpu(fmhdr->free_peb_count); i++) {
+		fmec = (struct ubi_fm_ec *)(fm_raw + fm_pos);
+		fm_pos += sizeof(*fmec);
+		if (fm_pos >= fm_size)
+			goto fail_bad;
+
+		add_aeb(ai, &ai->free, be32_to_cpu(fmec->pnum),
+			be32_to_cpu(fmec->ec), 0);
+	}
+
+	/* read EC values from used list */
+	for (i = 0; i < be32_to_cpu(fmhdr->used_peb_count); i++) {
+		fmec = (struct ubi_fm_ec *)(fm_raw + fm_pos);
+		fm_pos += sizeof(*fmec);
+		if (fm_pos >= fm_size)
+			goto fail_bad;
+
+		add_aeb(ai, &used, be32_to_cpu(fmec->pnum),
+			be32_to_cpu(fmec->ec), 0);
+	}
+
+	/* read EC values from scrub list */
+	for (i = 0; i < be32_to_cpu(fmhdr->scrub_peb_count); i++) {
+		fmec = (struct ubi_fm_ec *)(fm_raw + fm_pos);
+		fm_pos += sizeof(*fmec);
+		if (fm_pos >= fm_size)
+			goto fail_bad;
+
+		add_aeb(ai, &used, be32_to_cpu(fmec->pnum),
+			be32_to_cpu(fmec->ec), 1);
+	}
+
+	/* read EC values from erase list */
+	for (i = 0; i < be32_to_cpu(fmhdr->erase_peb_count); i++) {
+		fmec = (struct ubi_fm_ec *)(fm_raw + fm_pos);
+		fm_pos += sizeof(*fmec);
+		if (fm_pos >= fm_size)
+			goto fail_bad;
+
+		add_aeb(ai, &ai->erase, be32_to_cpu(fmec->pnum),
+			be32_to_cpu(fmec->ec), 1);
+	}
+
+	ai->mean_ec = div_u64(ai->ec_sum, ai->ec_count);
+	ai->bad_peb_count = be32_to_cpu(fmhdr->bad_peb_count);
+
+	/* Iterate over all volumes and read their EBA table */
+	for (i = 0; i < be32_to_cpu(fmhdr->vol_count); i++) {
+		fmvhdr = (struct ubi_fm_volhdr *)(fm_raw + fm_pos);
+		fm_pos += sizeof(*fmvhdr);
+		if (fm_pos >= fm_size)
+			goto fail_bad;
+
+		if (be32_to_cpu(fmvhdr->magic) != UBI_FM_VHDR_MAGIC) {
+			ubi_err("bad fastmap vol header magic: 0x%x, " \
+				"expected: 0x%x",
+				be32_to_cpu(fmvhdr->magic), UBI_FM_VHDR_MAGIC);
+			goto fail_bad;
+		}
+
+		av = add_vol(ai, be32_to_cpu(fmvhdr->vol_id),
+			     be32_to_cpu(fmvhdr->used_ebs),
+			     be32_to_cpu(fmvhdr->data_pad),
+			     fmvhdr->vol_type,
+			     be32_to_cpu(fmvhdr->last_eb_bytes));
+
+		if (!av)
+			goto fail_bad;
+
+		ai->vols_found++;
+		if (ai->highest_vol_id < be32_to_cpu(fmvhdr->vol_id))
+			ai->highest_vol_id = be32_to_cpu(fmvhdr->vol_id);
+
+		fm_eba = (struct ubi_fm_eba *)(fm_raw + fm_pos);
+		fm_pos += sizeof(*fm_eba);
+		fm_pos += (sizeof(__be32) * be32_to_cpu(fm_eba->reserved_pebs));
+		if (fm_pos >= fm_size)
+			goto fail_bad;
+
+		if (be32_to_cpu(fm_eba->magic) != UBI_FM_EBA_MAGIC) {
+			ubi_err("bad fastmap EBA header magic: 0x%x, " \
+				"expected: 0x%x",
+				be32_to_cpu(fm_eba->magic), UBI_FM_EBA_MAGIC);
+			goto fail_bad;
+		}
+
+		for (j = 0; j < be32_to_cpu(fm_eba->reserved_pebs); j++) {
+			int pnum = be32_to_cpu(fm_eba->pnum[j]);
+
+			if ((int)be32_to_cpu(fm_eba->pnum[j]) < 0)
+				continue;
+
+			aeb = NULL;
+			list_for_each_entry(tmp_aeb, &used, u.list) {
+				if (tmp_aeb->pnum == pnum)
+					aeb = tmp_aeb;
+			}
+
+			/* This can happen if a PEB is already in an EBA known
+			 * by this fastmap but the PEB itself is not in the used
+			 * list.
+			 * In this case the PEB can be within the fastmap pool
+			 * or while writing the fastmap it was in the protection
+			 * queue.
+			 */
+			if (!aeb) {
+				aeb = kmem_cache_alloc(ai->aeb_slab_cache,
+						       GFP_KERNEL);
+				if (!aeb) {
+					ret = -ENOMEM;
+
+					goto fail;
+				}
+
+				aeb->lnum = j;
+				aeb->pnum = be32_to_cpu(fm_eba->pnum[j]);
+				aeb->ec = -1;
+				aeb->scrub = aeb->copy_flag = aeb->sqnum = 0;
+				list_add_tail(&aeb->u.list, &eba_orphans);
+				continue;
+			}
+
+			aeb->lnum = j;
+
+			if (av->highest_lnum <= aeb->lnum)
+				av->highest_lnum = aeb->lnum;
+
+			assign_aeb_to_av(ai, aeb, av);
+
+			dbg_bld("inserting PEB:%i (LEB %i) to vol %i",
+				aeb->pnum, aeb->lnum, av->vol_id);
+		}
+
+		ech = kzalloc(ubi->ec_hdr_alsize, GFP_KERNEL);
+		if (!ech) {
+			ret = -ENOMEM;
+			goto fail;
+		}
+
+		list_for_each_entry_safe(tmp_aeb, _tmp_aeb, &eba_orphans,
+					 u.list) {
+			int err;
+
+			if (ubi_io_is_bad(ubi, tmp_aeb->pnum)) {
+				ubi_err("bad PEB in fastmap EBA orphan list");
+				ret = UBI_BAD_FASTMAP;
+				kfree(ech);
+				goto fail;
+			}
+
+			err = ubi_io_read_ec_hdr(ubi, tmp_aeb->pnum, ech, 0);
+			if (err && err != UBI_IO_BITFLIPS) {
+				ubi_err("unable to read EC header! PEB:%i " \
+					"err:%i", tmp_aeb->pnum, err);
+				ret = err > 0 ? UBI_BAD_FASTMAP : err;
+				kfree(ech);
+
+				goto fail;
+			} else if (err == UBI_IO_BITFLIPS)
+				tmp_aeb->scrub = 1;
+
+			tmp_aeb->ec = be64_to_cpu(ech->ec);
+			assign_aeb_to_av(ai, tmp_aeb, av);
+		}
+
+		kfree(ech);
+	}
+
+	ret = scan_pool(ubi, ai, fmpl1->pebs, pool_size, &max_sqnum,
+			&eba_orphans, &free);
+	if (ret)
+		goto fail;
+
+	ret = scan_pool(ubi, ai, fmpl2->pebs, wl_pool_size, &max_sqnum,
+			&eba_orphans, &free);
+	if (ret)
+		goto fail;
+
+	if (max_sqnum > ai->max_sqnum)
+		ai->max_sqnum = max_sqnum;
+
+	list_for_each_entry_safe(tmp_aeb, _tmp_aeb, &free, u.list) {
+		list_del(&tmp_aeb->u.list);
+		list_add_tail(&tmp_aeb->u.list, &ai->free);
+	}
+
+	/*
+	 * If fastmap is leaking PEBs (must not happen), raise a
+	 * fat warning and fall back to scanning mode.
+	 * We do this here because in ubi_wl_init() it's too late
+	 * and we cannot fall back to scanning.
+	 */
+	if (WARN_ON(count_fastmap_pebs(ai) != ubi->peb_count -
+		    ai->bad_peb_count - fm->used_blocks))
+		goto fail_bad;
+
+	return 0;
+
+fail_bad:
+	ret = UBI_BAD_FASTMAP;
+fail:
+	return ret;
+}
+
+/**
+ * ubi_scan_fastmap - scan the fastmap.
+ * @ubi: UBI device object
+ * @ai: UBI attach info to be filled
+ * @fm_anchor: The fastmap starts at this PEB
+ *
+ * Returns 0 on success, UBI_NO_FASTMAP if no fastmap was found,
+ * UBI_BAD_FASTMAP if one was found but is not usable.
+ * < 0 indicates an internal error.
+ */
+int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
+		     int fm_anchor)
+{
+	struct ubi_fm_sb *fmsb, *fmsb2;
+	struct ubi_vid_hdr *vh;
+	struct ubi_ec_hdr *ech;
+	struct ubi_fastmap_layout *fm;
+	int i, used_blocks, pnum, ret = 0;
+	size_t fm_size;
+	__be32 crc, tmp_crc;
+	unsigned long long sqnum = 0;
+
+	mutex_lock(&ubi->fm_mutex);
+	memset(ubi->fm_buf, 0, ubi->fm_size);
+
+	fmsb = kmalloc(sizeof(*fmsb), GFP_KERNEL);
+	if (!fmsb) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	fm = kzalloc(sizeof(*fm), GFP_KERNEL);
+	if (!fm) {
+		ret = -ENOMEM;
+		kfree(fmsb);
+		goto out;
+	}
+
+	ret = ubi_io_read(ubi, fmsb, fm_anchor, ubi->leb_start, sizeof(*fmsb));
+	if (ret && ret != UBI_IO_BITFLIPS)
+		goto free_fm_sb;
+	else if (ret == UBI_IO_BITFLIPS)
+		fm->to_be_tortured[0] = 1;
+
+	if (be32_to_cpu(fmsb->magic) != UBI_FM_SB_MAGIC) {
+		ubi_err("bad super block magic: 0x%x, expected: 0x%x",
+			be32_to_cpu(fmsb->magic), UBI_FM_SB_MAGIC);
+		ret = UBI_BAD_FASTMAP;
+		goto free_fm_sb;
+	}
+
+	if (fmsb->version != UBI_FM_FMT_VERSION) {
+		ubi_err("bad fastmap version: %i, expected: %i",
+			fmsb->version, UBI_FM_FMT_VERSION);
+		ret = UBI_BAD_FASTMAP;
+		goto free_fm_sb;
+	}
+
+	used_blocks = be32_to_cpu(fmsb->used_blocks);
+	if (used_blocks > UBI_FM_MAX_BLOCKS || used_blocks < 1) {
+		ubi_err("number of fastmap blocks is invalid: %i", used_blocks);
+		ret = UBI_BAD_FASTMAP;
+		goto free_fm_sb;
+	}
+
+	fm_size = ubi->leb_size * used_blocks;
+	if (fm_size != ubi->fm_size) {
+		ubi_err("bad fastmap size: %zi, expected: %zi", fm_size,
+			ubi->fm_size);
+		ret = UBI_BAD_FASTMAP;
+		goto free_fm_sb;
+	}
+
+	ech = kzalloc(ubi->ec_hdr_alsize, GFP_KERNEL);
+	if (!ech) {
+		ret = -ENOMEM;
+		goto free_fm_sb;
+	}
+
+	vh = ubi_zalloc_vid_hdr(ubi, GFP_KERNEL);
+	if (!vh) {
+		ret = -ENOMEM;
+		goto free_hdr;
+	}
+
+	for (i = 0; i < used_blocks; i++) {
+		pnum = be32_to_cpu(fmsb->block_loc[i]);
+
+		if (ubi_io_is_bad(ubi, pnum)) {
+			ret = UBI_BAD_FASTMAP;
+			goto free_hdr;
+		}
+
+		ret = ubi_io_read_ec_hdr(ubi, pnum, ech, 0);
+		if (ret && ret != UBI_IO_BITFLIPS) {
+			ubi_err("unable to read fastmap block# %i EC (PEB: %i)",
+				i, pnum);
+			if (ret > 0)
+				ret = UBI_BAD_FASTMAP;
+			goto free_hdr;
+		} else if (ret == UBI_IO_BITFLIPS)
+			fm->to_be_tortured[i] = 1;
+
+		if (!ubi->image_seq)
+			ubi->image_seq = be32_to_cpu(ech->image_seq);
+
+		if (be32_to_cpu(ech->image_seq) != ubi->image_seq) {
+			ret = UBI_BAD_FASTMAP;
+			goto free_hdr;
+		}
+
+		ret = ubi_io_read_vid_hdr(ubi, pnum, vh, 0);
+		if (ret && ret != UBI_IO_BITFLIPS) {
+			ubi_err("unable to read fastmap block# %i (PEB: %i)",
+				i, pnum);
+			goto free_hdr;
+		}
+
+		if (i == 0) {
+			if (be32_to_cpu(vh->vol_id) != UBI_FM_SB_VOLUME_ID) {
+				ubi_err("bad fastmap anchor vol_id: 0x%x," \
+					" expected: 0x%x",
+					be32_to_cpu(vh->vol_id),
+					UBI_FM_SB_VOLUME_ID);
+				ret = UBI_BAD_FASTMAP;
+				goto free_hdr;
+			}
+		} else {
+			if (be32_to_cpu(vh->vol_id) != UBI_FM_DATA_VOLUME_ID) {
+				ubi_err("bad fastmap data vol_id: 0x%x," \
+					" expected: 0x%x",
+					be32_to_cpu(vh->vol_id),
+					UBI_FM_DATA_VOLUME_ID);
+				ret = UBI_BAD_FASTMAP;
+				goto free_hdr;
+			}
+		}
+
+		if (sqnum < be64_to_cpu(vh->sqnum))
+			sqnum = be64_to_cpu(vh->sqnum);
+
+		ret = ubi_io_read(ubi, ubi->fm_buf + (ubi->leb_size * i), pnum,
+				  ubi->leb_start, ubi->leb_size);
+		if (ret && ret != UBI_IO_BITFLIPS) {
+			ubi_err("unable to read fastmap block# %i (PEB: %i, " \
+				"err: %i)", i, pnum, ret);
+			goto free_hdr;
+		}
+	}
+
+	kfree(fmsb);
+	fmsb = NULL;
+
+	fmsb2 = (struct ubi_fm_sb *)(ubi->fm_buf);
+	tmp_crc = be32_to_cpu(fmsb2->data_crc);
+	fmsb2->data_crc = 0;
+	crc = crc32(UBI_CRC32_INIT, ubi->fm_buf, fm_size);
+	if (crc != tmp_crc) {
+		ubi_err("fastmap data CRC is invalid");
+		ubi_err("CRC should be: 0x%x, calc: 0x%x", tmp_crc, crc);
+		ret = UBI_BAD_FASTMAP;
+		goto free_hdr;
+	}
+
+	fmsb2->sqnum = sqnum;
+
+	fm->used_blocks = used_blocks;
+
+	ret = ubi_attach_fastmap(ubi, ai, fm);
+	if (ret) {
+		if (ret > 0)
+			ret = UBI_BAD_FASTMAP;
+		goto free_hdr;
+	}
+
+	for (i = 0; i < used_blocks; i++) {
+		struct ubi_wl_entry *e;
+
+		e = kmem_cache_alloc(ubi_wl_entry_slab, GFP_KERNEL);
+		if (!e) {
+			while (i--)
+				kfree(fm->e[i]);
+
+			ret = -ENOMEM;
+			goto free_hdr;
+		}
+
+		e->pnum = be32_to_cpu(fmsb2->block_loc[i]);
+		e->ec = be32_to_cpu(fmsb2->block_ec[i]);
+		fm->e[i] = e;
+	}
+
+	ubi->fm = fm;
+	ubi->fm_pool.max_size = ubi->fm->max_pool_size;
+	ubi->fm_wl_pool.max_size = ubi->fm->max_wl_pool_size;
+	ubi_msg("attached by fastmap");
+	ubi_msg("fastmap pool size: %d", ubi->fm_pool.max_size);
+	ubi_msg("fastmap WL pool size: %d", ubi->fm_wl_pool.max_size);
+	ubi->fm_disabled = 0;
+
+	ubi_free_vid_hdr(ubi, vh);
+	kfree(ech);
+out:
+	mutex_unlock(&ubi->fm_mutex);
+	if (ret == UBI_BAD_FASTMAP)
+		ubi_err("Attach by fastmap failed, doing a full scan!");
+	return ret;
+
+free_hdr:
+	ubi_free_vid_hdr(ubi, vh);
+	kfree(ech);
+free_fm_sb:
+	kfree(fmsb);
+	kfree(fm);
+	goto out;
+}
+
+/**
+ * ubi_write_fastmap - writes a fastmap.
+ * @ubi: UBI device object
+ * @new_fm: the to be written fastmap
+ *
+ * Returns 0 on success, < 0 indicates an internal error.
+ */
+static int ubi_write_fastmap(struct ubi_device *ubi,
+			     struct ubi_fastmap_layout *new_fm)
+{
+	size_t fm_pos = 0;
+	void *fm_raw;
+	struct ubi_fm_sb *fmsb;
+	struct ubi_fm_hdr *fmh;
+	struct ubi_fm_scan_pool *fmpl1, *fmpl2;
+	struct ubi_fm_ec *fec;
+	struct ubi_fm_volhdr *fvh;
+	struct ubi_fm_eba *feba;
+	struct rb_node *node;
+	struct ubi_wl_entry *wl_e;
+	struct ubi_volume *vol;
+	struct ubi_vid_hdr *avhdr, *dvhdr;
+	struct ubi_work *ubi_wrk;
+	int ret, i, j, free_peb_count, used_peb_count, vol_count;
+	int scrub_peb_count, erase_peb_count;
+
+	fm_raw = ubi->fm_buf;
+	memset(ubi->fm_buf, 0, ubi->fm_size);
+
+	avhdr = new_fm_vhdr(ubi, UBI_FM_SB_VOLUME_ID);
+	if (!avhdr) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	dvhdr = new_fm_vhdr(ubi, UBI_FM_DATA_VOLUME_ID);
+	if (!dvhdr) {
+		ret = -ENOMEM;
+		goto out_kfree;
+	}
+
+	spin_lock(&ubi->volumes_lock);
+	spin_lock(&ubi->wl_lock);
+
+	fmsb = (struct ubi_fm_sb *)fm_raw;
+	fm_pos += sizeof(*fmsb);
+	ubi_assert(fm_pos <= ubi->fm_size);
+
+	fmh = (struct ubi_fm_hdr *)(fm_raw + fm_pos);
+	fm_pos += sizeof(*fmh);
+	ubi_assert(fm_pos <= ubi->fm_size);
+
+	fmsb->magic = cpu_to_be32(UBI_FM_SB_MAGIC);
+	fmsb->version = UBI_FM_FMT_VERSION;
+	fmsb->used_blocks = cpu_to_be32(new_fm->used_blocks);
+	/* the max sqnum will be filled in while *reading* the fastmap */
+	fmsb->sqnum = 0;
+
+	fmh->magic = cpu_to_be32(UBI_FM_HDR_MAGIC);
+	free_peb_count = 0;
+	used_peb_count = 0;
+	scrub_peb_count = 0;
+	erase_peb_count = 0;
+	vol_count = 0;
+
+	fmpl1 = (struct ubi_fm_scan_pool *)(fm_raw + fm_pos);
+	fm_pos += sizeof(*fmpl1);
+	fmpl1->magic = cpu_to_be32(UBI_FM_POOL_MAGIC);
+	fmpl1->size = cpu_to_be16(ubi->fm_pool.size);
+	fmpl1->max_size = cpu_to_be16(ubi->fm_pool.max_size);
+
+	for (i = 0; i < ubi->fm_pool.size; i++)
+		fmpl1->pebs[i] = cpu_to_be32(ubi->fm_pool.pebs[i]);
+
+	fmpl2 = (struct ubi_fm_scan_pool *)(fm_raw + fm_pos);
+	fm_pos += sizeof(*fmpl2);
+	fmpl2->magic = cpu_to_be32(UBI_FM_POOL_MAGIC);
+	fmpl2->size = cpu_to_be16(ubi->fm_wl_pool.size);
+	fmpl2->max_size = cpu_to_be16(ubi->fm_wl_pool.max_size);
+
+	for (i = 0; i < ubi->fm_wl_pool.size; i++)
+		fmpl2->pebs[i] = cpu_to_be32(ubi->fm_wl_pool.pebs[i]);
+
+	for (node = rb_first(&ubi->free); node; node = rb_next(node)) {
+		wl_e = rb_entry(node, struct ubi_wl_entry, u.rb);
+		fec = (struct ubi_fm_ec *)(fm_raw + fm_pos);
+
+		fec->pnum = cpu_to_be32(wl_e->pnum);
+		fec->ec = cpu_to_be32(wl_e->ec);
+
+		free_peb_count++;
+		fm_pos += sizeof(*fec);
+		ubi_assert(fm_pos <= ubi->fm_size);
+	}
+	fmh->free_peb_count = cpu_to_be32(free_peb_count);
+
+	for (node = rb_first(&ubi->used); node; node = rb_next(node)) {
+		wl_e = rb_entry(node, struct ubi_wl_entry, u.rb);
+		fec = (struct ubi_fm_ec *)(fm_raw + fm_pos);
+
+		fec->pnum = cpu_to_be32(wl_e->pnum);
+		fec->ec = cpu_to_be32(wl_e->ec);
+
+		used_peb_count++;
+		fm_pos += sizeof(*fec);
+		ubi_assert(fm_pos <= ubi->fm_size);
+	}
+	fmh->used_peb_count = cpu_to_be32(used_peb_count);
+
+	for (node = rb_first(&ubi->scrub); node; node = rb_next(node)) {
+		wl_e = rb_entry(node, struct ubi_wl_entry, u.rb);
+		fec = (struct ubi_fm_ec *)(fm_raw + fm_pos);
+
+		fec->pnum = cpu_to_be32(wl_e->pnum);
+		fec->ec = cpu_to_be32(wl_e->ec);
+
+		scrub_peb_count++;
+		fm_pos += sizeof(*fec);
+		ubi_assert(fm_pos <= ubi->fm_size);
+	}
+	fmh->scrub_peb_count = cpu_to_be32(scrub_peb_count);
+
+
+	list_for_each_entry(ubi_wrk, &ubi->works, list) {
+		if (ubi_is_erase_work(ubi_wrk)) {
+			wl_e = ubi_wrk->e;
+			ubi_assert(wl_e);
+
+			fec = (struct ubi_fm_ec *)(fm_raw + fm_pos);
+
+			fec->pnum = cpu_to_be32(wl_e->pnum);
+			fec->ec = cpu_to_be32(wl_e->ec);
+
+			erase_peb_count++;
+			fm_pos += sizeof(*fec);
+			ubi_assert(fm_pos <= ubi->fm_size);
+		}
+	}
+	fmh->erase_peb_count = cpu_to_be32(erase_peb_count);
+
+	for (i = 0; i < UBI_MAX_VOLUMES + UBI_INT_VOL_COUNT; i++) {
+		vol = ubi->volumes[i];
+
+		if (!vol)
+			continue;
+
+		vol_count++;
+
+		fvh = (struct ubi_fm_volhdr *)(fm_raw + fm_pos);
+		fm_pos += sizeof(*fvh);
+		ubi_assert(fm_pos <= ubi->fm_size);
+
+		fvh->magic = cpu_to_be32(UBI_FM_VHDR_MAGIC);
+		fvh->vol_id = cpu_to_be32(vol->vol_id);
+		fvh->vol_type = vol->vol_type;
+		fvh->used_ebs = cpu_to_be32(vol->used_ebs);
+		fvh->data_pad = cpu_to_be32(vol->data_pad);
+		fvh->last_eb_bytes = cpu_to_be32(vol->last_eb_bytes);
+
+		ubi_assert(vol->vol_type == UBI_DYNAMIC_VOLUME ||
+			vol->vol_type == UBI_STATIC_VOLUME);
+
+		feba = (struct ubi_fm_eba *)(fm_raw + fm_pos);
+		fm_pos += sizeof(*feba) + (sizeof(__be32) * vol->reserved_pebs);
+		ubi_assert(fm_pos <= ubi->fm_size);
+
+		for (j = 0; j < vol->reserved_pebs; j++)
+			feba->pnum[j] = cpu_to_be32(vol->eba_tbl[j]);
+
+		feba->reserved_pebs = cpu_to_be32(j);
+		feba->magic = cpu_to_be32(UBI_FM_EBA_MAGIC);
+	}
+	fmh->vol_count = cpu_to_be32(vol_count);
+	fmh->bad_peb_count = cpu_to_be32(ubi->bad_peb_count);
+
+	avhdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
+	avhdr->lnum = 0;
+
+	spin_unlock(&ubi->wl_lock);
+	spin_unlock(&ubi->volumes_lock);
+
+	dbg_bld("writing fastmap SB to PEB %i", new_fm->e[0]->pnum);
+	ret = ubi_io_write_vid_hdr(ubi, new_fm->e[0]->pnum, avhdr);
+	if (ret) {
+		ubi_err("unable to write vid_hdr to fastmap SB!");
+		goto out_kfree;
+	}
+
+	for (i = 0; i < new_fm->used_blocks; i++) {
+		fmsb->block_loc[i] = cpu_to_be32(new_fm->e[i]->pnum);
+		fmsb->block_ec[i] = cpu_to_be32(new_fm->e[i]->ec);
+	}
+
+	fmsb->data_crc = 0;
+	fmsb->data_crc = cpu_to_be32(crc32(UBI_CRC32_INIT, fm_raw,
+					   ubi->fm_size));
+
+	for (i = 1; i < new_fm->used_blocks; i++) {
+		dvhdr->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
+		dvhdr->lnum = cpu_to_be32(i);
+		dbg_bld("writing fastmap data to PEB %i sqnum %llu",
+			new_fm->e[i]->pnum, be64_to_cpu(dvhdr->sqnum));
+		ret = ubi_io_write_vid_hdr(ubi, new_fm->e[i]->pnum, dvhdr);
+		if (ret) {
+			ubi_err("unable to write vid_hdr to PEB %i!",
+				new_fm->e[i]->pnum);
+			goto out_kfree;
+		}
+	}
+
+	for (i = 0; i < new_fm->used_blocks; i++) {
+		ret = ubi_io_write(ubi, fm_raw + (i * ubi->leb_size),
+			new_fm->e[i]->pnum, ubi->leb_start, ubi->leb_size);
+		if (ret) {
+			ubi_err("unable to write fastmap to PEB %i!",
+				new_fm->e[i]->pnum);
+			goto out_kfree;
+		}
+	}
+
+	ubi_assert(new_fm);
+	ubi->fm = new_fm;
+
+	dbg_bld("fastmap written!");
+
+out_kfree:
+	ubi_free_vid_hdr(ubi, avhdr);
+	ubi_free_vid_hdr(ubi, dvhdr);
+out:
+	return ret;
+}
+
+/**
+ * erase_block - Manually erase a PEB.
+ * @ubi: UBI device object
+ * @pnum: PEB to be erased
+ *
+ * Returns the new EC value on success, < 0 indicates an internal error.
+ */
+static int erase_block(struct ubi_device *ubi, int pnum)
+{
+	int ret;
+	struct ubi_ec_hdr *ec_hdr;
+	long long ec;
+
+	ec_hdr = kzalloc(ubi->ec_hdr_alsize, GFP_KERNEL);
+	if (!ec_hdr)
+		return -ENOMEM;
+
+	ret = ubi_io_read_ec_hdr(ubi, pnum, ec_hdr, 0);
+	if (ret < 0)
+		goto out;
+	else if (ret && ret != UBI_IO_BITFLIPS) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	ret = ubi_io_sync_erase(ubi, pnum, 0);
+	if (ret < 0)
+		goto out;
+
+	ec = be64_to_cpu(ec_hdr->ec);
+	ec += ret;
+	if (ec > UBI_MAX_ERASECOUNTER) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	ec_hdr->ec = cpu_to_be64(ec);
+	ret = ubi_io_write_ec_hdr(ubi, pnum, ec_hdr);
+	if (ret < 0)
+		goto out;
+
+	ret = ec;
+out:
+	kfree(ec_hdr);
+	return ret;
+}
+
+/**
+ * invalidate_fastmap - destroys a fastmap.
+ * @ubi: UBI device object
+ * @fm: the fastmap to be destroyed
+ *
+ * Returns 0 on success, < 0 indicates an internal error.
+ */
+static int invalidate_fastmap(struct ubi_device *ubi,
+			      struct ubi_fastmap_layout *fm)
+{
+	int ret, i;
+	struct ubi_vid_hdr *vh;
+
+	ret = erase_block(ubi, fm->e[0]->pnum);
+	if (ret < 0)
+		return ret;
+
+	vh = new_fm_vhdr(ubi, UBI_FM_SB_VOLUME_ID);
+	if (!vh)
+		return -ENOMEM;
+
+	/* deleting the current fastmap SB is not enough, an old SB may exist,
+	 * so create a (corrupted) SB such that fastmap will find it and fall
+	 * back to scanning mode in any case */
+	vh->sqnum = cpu_to_be64(ubi_next_sqnum(ubi));
+	ret = ubi_io_write_vid_hdr(ubi, fm->e[0]->pnum, vh);
+
+	for (i = 0; i < fm->used_blocks; i++)
+		ubi_wl_put_fm_peb(ubi, fm->e[i], i, fm->to_be_tortured[i]);
+
+	return ret;
+}
+
+/**
+ * ubi_update_fastmap - will be called by UBI if a volume changes or
+ * a fastmap pool becomes full.
+ * @ubi: UBI device object
+ *
+ * Returns 0 on success, < 0 indicates an internal error.
+ */
+int ubi_update_fastmap(struct ubi_device *ubi)
+{
+	int ret, i;
+	struct ubi_fastmap_layout *new_fm, *old_fm;
+	struct ubi_wl_entry *tmp_e;
+
+	mutex_lock(&ubi->fm_mutex);
+
+	ubi_refill_pools(ubi);
+
+	if (ubi->ro_mode || ubi->fm_disabled) {
+		mutex_unlock(&ubi->fm_mutex);
+		return 0;
+	}
+
+	ret = ubi_ensure_anchor_pebs(ubi);
+	if (ret) {
+		mutex_unlock(&ubi->fm_mutex);
+		return ret;
+	}
+
+	new_fm = kzalloc(sizeof(*new_fm), GFP_KERNEL);
+	if (!new_fm) {
+		mutex_unlock(&ubi->fm_mutex);
+		return -ENOMEM;
+	}
+
+	new_fm->used_blocks = ubi->fm_size / ubi->leb_size;
+
+	for (i = 0; i < new_fm->used_blocks; i++) {
+		new_fm->e[i] = kmem_cache_alloc(ubi_wl_entry_slab, GFP_KERNEL);
+		if (!new_fm->e[i]) {
+			while (i--)
+				kfree(new_fm->e[i]);
+
+			kfree(new_fm);
+			mutex_unlock(&ubi->fm_mutex);
+			return -ENOMEM;
+		}
+	}
+
+	old_fm = ubi->fm;
+	ubi->fm = NULL;
+
+	if (new_fm->used_blocks > UBI_FM_MAX_BLOCKS) {
+		ubi_err("fastmap too large");
+		ret = -ENOSPC;
+		goto err;
+	}
+
+	for (i = 1; i < new_fm->used_blocks; i++) {
+		spin_lock(&ubi->wl_lock);
+		tmp_e = ubi_wl_get_fm_peb(ubi, 0);
+		spin_unlock(&ubi->wl_lock);
+
+		if (!tmp_e && !old_fm) {
+			int j;
+			ubi_err("could not get any free erase block");
+
+			for (j = 1; j < i; j++)
+				ubi_wl_put_fm_peb(ubi, new_fm->e[j], j, 0);
+
+			ret = -ENOSPC;
+			goto err;
+		} else if (!tmp_e && old_fm) {
+			ret = erase_block(ubi, old_fm->e[i]->pnum);
+			if (ret < 0) {
+				int j;
+
+				for (j = 1; j < i; j++)
+					ubi_wl_put_fm_peb(ubi, new_fm->e[j],
+							  j, 0);
+
+				ubi_err("could not erase old fastmap PEB");
+				goto err;
+			}
+
+			new_fm->e[i]->pnum = old_fm->e[i]->pnum;
+			new_fm->e[i]->ec = old_fm->e[i]->ec;
+		} else {
+			new_fm->e[i]->pnum = tmp_e->pnum;
+			new_fm->e[i]->ec = tmp_e->ec;
+
+			if (old_fm)
+				ubi_wl_put_fm_peb(ubi, old_fm->e[i], i,
+						  old_fm->to_be_tortured[i]);
+		}
+	}
+
+	spin_lock(&ubi->wl_lock);
+	tmp_e = ubi_wl_get_fm_peb(ubi, 1);
+	spin_unlock(&ubi->wl_lock);
+
+	if (old_fm) {
+		/* no fresh anchor PEB was found, reuse the old one */
+		if (!tmp_e) {
+			ret = erase_block(ubi, old_fm->e[0]->pnum);
+			if (ret < 0) {
+				int i;
+				ubi_err("could not erase old anchor PEB");
+
+				for (i = 1; i < new_fm->used_blocks; i++)
+					ubi_wl_put_fm_peb(ubi, new_fm->e[i],
+							  i, 0);
+				goto err;
+			}
+
+			new_fm->e[0]->pnum = old_fm->e[0]->pnum;
+			new_fm->e[0]->ec = ret;
+		} else {
+			/* we've got a new anchor PEB, return the old one */
+			ubi_wl_put_fm_peb(ubi, old_fm->e[0], 0,
+					  old_fm->to_be_tortured[0]);
+
+			new_fm->e[0]->pnum = tmp_e->pnum;
+			new_fm->e[0]->ec = tmp_e->ec;
+		}
+	} else {
+		if (!tmp_e) {
+			int i;
+			ubi_err("could not find any anchor PEB");
+
+			for (i = 1; i < new_fm->used_blocks; i++)
+				ubi_wl_put_fm_peb(ubi, new_fm->e[i], i, 0);
+
+			ret = -ENOSPC;
+			goto err;
+		}
+
+		new_fm->e[0]->pnum = tmp_e->pnum;
+		new_fm->e[0]->ec = tmp_e->ec;
+	}
+
+	down_write(&ubi->work_sem);
+	down_write(&ubi->fm_sem);
+	ret = ubi_write_fastmap(ubi, new_fm);
+	up_write(&ubi->fm_sem);
+	up_write(&ubi->work_sem);
+
+	if (ret)
+		goto err;
+
+out_unlock:
+	mutex_unlock(&ubi->fm_mutex);
+	kfree(old_fm);
+	return ret;
+
+err:
+	kfree(new_fm);
+
+	ubi_warn("Unable to write new fastmap, err=%i", ret);
+
+	ret = 0;
+	if (old_fm) {
+		ret = invalidate_fastmap(ubi, old_fm);
+		if (ret < 0)
+			ubi_err("Unable to invalidiate current fastmap!");
+		else if (ret)
+			ret = 0;
+	}
+	goto out_unlock;
+}
diff --git a/drivers/mtd/ubi/ubi-media.h b/drivers/mtd/ubi/ubi-media.h
index 468ffbc0eab..ac2b24d1783 100644
--- a/drivers/mtd/ubi/ubi-media.h
+++ b/drivers/mtd/ubi/ubi-media.h
@@ -375,4 +375,141 @@ struct ubi_vtbl_record {
 	__be32  crc;
 } __packed;
 
+/* UBI fastmap on-flash data structures */
+
+#define UBI_FM_SB_VOLUME_ID	(UBI_LAYOUT_VOLUME_ID + 1)
+#define UBI_FM_DATA_VOLUME_ID	(UBI_LAYOUT_VOLUME_ID + 2)
+
+/* fastmap on-flash data structure format version */
+#define UBI_FM_FMT_VERSION	1
+
+#define UBI_FM_SB_MAGIC		0x7B11D69F
+#define UBI_FM_HDR_MAGIC	0xD4B82EF7
+#define UBI_FM_VHDR_MAGIC	0xFA370ED1
+#define UBI_FM_POOL_MAGIC	0x67AF4D08
+#define UBI_FM_EBA_MAGIC	0xf0c040a8
+
+/* A fastmap supber block can be located between PEB 0 and
+ * UBI_FM_MAX_START */
+#define UBI_FM_MAX_START	64
+
+/* A fastmap can use up to UBI_FM_MAX_BLOCKS PEBs */
+#define UBI_FM_MAX_BLOCKS	32
+
+/* 5% of the total number of PEBs have to be scanned while attaching
+ * from a fastmap.
+ * But the size of this pool is limited to be between UBI_FM_MIN_POOL_SIZE and
+ * UBI_FM_MAX_POOL_SIZE */
+#define UBI_FM_MIN_POOL_SIZE	8
+#define UBI_FM_MAX_POOL_SIZE	256
+
+#define UBI_FM_WL_POOL_SIZE	25
+
+/**
+ * struct ubi_fm_sb - UBI fastmap super block
+ * @magic: fastmap super block magic number (%UBI_FM_SB_MAGIC)
+ * @version: format version of this fastmap
+ * @data_crc: CRC over the fastmap data
+ * @used_blocks: number of PEBs used by this fastmap
+ * @block_loc: an array containing the location of all PEBs of the fastmap
+ * @block_ec: the erase counter of each used PEB
+ * @sqnum: highest sequence number value at the time while taking the fastmap
+ *
+ */
+struct ubi_fm_sb {
+	__be32 magic;
+	__u8 version;
+	__u8 padding1[3];
+	__be32 data_crc;
+	__be32 used_blocks;
+	__be32 block_loc[UBI_FM_MAX_BLOCKS];
+	__be32 block_ec[UBI_FM_MAX_BLOCKS];
+	__be64 sqnum;
+	__u8 padding2[32];
+} __packed;
+
+/**
+ * struct ubi_fm_hdr - header of the fastmap data set
+ * @magic: fastmap header magic number (%UBI_FM_HDR_MAGIC)
+ * @free_peb_count: number of free PEBs known by this fastmap
+ * @used_peb_count: number of used PEBs known by this fastmap
+ * @scrub_peb_count: number of to be scrubbed PEBs known by this fastmap
+ * @bad_peb_count: number of bad PEBs known by this fastmap
+ * @erase_peb_count: number of bad PEBs which have to be erased
+ * @vol_count: number of UBI volumes known by this fastmap
+ */
+struct ubi_fm_hdr {
+	__be32 magic;
+	__be32 free_peb_count;
+	__be32 used_peb_count;
+	__be32 scrub_peb_count;
+	__be32 bad_peb_count;
+	__be32 erase_peb_count;
+	__be32 vol_count;
+	__u8 padding[4];
+} __packed;
+
+/* struct ubi_fm_hdr is followed by two struct ubi_fm_scan_pool */
+
+/**
+ * struct ubi_fm_scan_pool - Fastmap pool PEBs to be scanned while attaching
+ * @magic: pool magic numer (%UBI_FM_POOL_MAGIC)
+ * @size: current pool size
+ * @max_size: maximal pool size
+ * @pebs: an array containing the location of all PEBs in this pool
+ */
+struct ubi_fm_scan_pool {
+	__be32 magic;
+	__be16 size;
+	__be16 max_size;
+	__be32 pebs[UBI_FM_MAX_POOL_SIZE];
+	__be32 padding[4];
+} __packed;
+
+/* ubi_fm_scan_pool is followed by nfree+nused struct ubi_fm_ec records */
+
+/**
+ * struct ubi_fm_ec - stores the erase counter of a PEB
+ * @pnum: PEB number
+ * @ec: ec of this PEB
+ */
+struct ubi_fm_ec {
+	__be32 pnum;
+	__be32 ec;
+} __packed;
+
+/**
+ * struct ubi_fm_volhdr - Fastmap volume header
+ * it identifies the start of an eba table
+ * @magic: Fastmap volume header magic number (%UBI_FM_VHDR_MAGIC)
+ * @vol_id: volume id of the fastmapped volume
+ * @vol_type: type of the fastmapped volume
+ * @data_pad: data_pad value of the fastmapped volume
+ * @used_ebs: number of used LEBs within this volume
+ * @last_eb_bytes: number of bytes used in the last LEB
+ */
+struct ubi_fm_volhdr {
+	__be32 magic;
+	__be32 vol_id;
+	__u8 vol_type;
+	__u8 padding1[3];
+	__be32 data_pad;
+	__be32 used_ebs;
+	__be32 last_eb_bytes;
+	__u8 padding2[8];
+} __packed;
+
+/* struct ubi_fm_volhdr is followed by one struct ubi_fm_eba records */
+
+/**
+ * struct ubi_fm_eba - denotes an association beween a PEB and LEB
+ * @magic: EBA table magic number
+ * @reserved_pebs: number of table entries
+ * @pnum: PEB number of LEB (LEB is the index)
+ */
+struct ubi_fm_eba {
+	__be32 magic;
+	__be32 reserved_pebs;
+	__be32 pnum[0];
+} __packed;
 #endif /* !__UBI_MEDIA_H__ */
diff --git a/drivers/mtd/ubi/ubi.h b/drivers/mtd/ubi/ubi.h
index 383ee43d242..7d57469723c 100644
--- a/drivers/mtd/ubi/ubi.h
+++ b/drivers/mtd/ubi/ubi.h
@@ -133,6 +133,17 @@ enum {
 	MOVE_RETRY,
 };
 
+/*
+ * Return codes of the fastmap sub-system
+ *
+ * UBI_NO_FASTMAP: No fastmap super block was found
+ * UBI_BAD_FASTMAP: A fastmap was found but it's unusable
+ */
+enum {
+	UBI_NO_FASTMAP = 1,
+	UBI_BAD_FASTMAP,
+};
+
 /**
  * struct ubi_wl_entry - wear-leveling entry.
  * @u.rb: link in the corresponding (free/used) RB-tree
@@ -199,6 +210,41 @@ struct ubi_rename_entry {
 struct ubi_volume_desc;
 
 /**
+ * struct ubi_fastmap_layout - in-memory fastmap data structure.
+ * @e: PEBs used by the current fastmap
+ * @to_be_tortured: if non-zero tortured this PEB
+ * @used_blocks: number of used PEBs
+ * @max_pool_size: maximal size of the user pool
+ * @max_wl_pool_size: maximal size of the pool used by the WL sub-system
+ */
+struct ubi_fastmap_layout {
+	struct ubi_wl_entry *e[UBI_FM_MAX_BLOCKS];
+	int to_be_tortured[UBI_FM_MAX_BLOCKS];
+	int used_blocks;
+	int max_pool_size;
+	int max_wl_pool_size;
+};
+
+/**
+ * struct ubi_fm_pool - in-memory fastmap pool
+ * @pebs: PEBs in this pool
+ * @used: number of used PEBs
+ * @size: total number of PEBs in this pool
+ * @max_size: maximal size of the pool
+ *
+ * A pool gets filled with up to max_size.
+ * If all PEBs within the pool are used a new fastmap will be written
+ * to the flash and the pool gets refilled with empty PEBs.
+ *
+ */
+struct ubi_fm_pool {
+	int pebs[UBI_FM_MAX_POOL_SIZE];
+	int used;
+	int size;
+	int max_size;
+};
+
+/**
  * struct ubi_volume - UBI volume description data structure.
  * @dev: device object to make use of the the Linux device model
  * @cdev: character device object to create character device
@@ -333,9 +379,21 @@ struct ubi_wl_entry;
  * @ltree: the lock tree
  * @alc_mutex: serializes "atomic LEB change" operations
  *
+ * @fm_disabled: non-zero if fastmap is disabled (default)
+ * @fm: in-memory data structure of the currently used fastmap
+ * @fm_pool: in-memory data structure of the fastmap pool
+ * @fm_wl_pool: in-memory data structure of the fastmap pool used by the WL
+ *		sub-system
+ * @fm_mutex: serializes ubi_update_fastmap() and protects @fm_buf
+ * @fm_buf: vmalloc()'d buffer which holds the raw fastmap
+ * @fm_size: fastmap size in bytes
+ * @fm_sem: allows ubi_update_fastmap() to block EBA table changes
+ * @fm_work: fastmap work queue
+ *
  * @used: RB-tree of used physical eraseblocks
  * @erroneous: RB-tree of erroneous used physical eraseblocks
  * @free: RB-tree of free physical eraseblocks
+ * @free_count: Contains the number of elements in @free
  * @scrub: RB-tree of physical eraseblocks which need scrubbing
  * @pq: protection queue (contain physical eraseblocks which are temporarily
  *      protected from the wear-leveling worker)
@@ -426,10 +484,22 @@ struct ubi_device {
 	struct rb_root ltree;
 	struct mutex alc_mutex;
 
+	/* Fastmap stuff */
+	int fm_disabled;
+	struct ubi_fastmap_layout *fm;
+	struct ubi_fm_pool fm_pool;
+	struct ubi_fm_pool fm_wl_pool;
+	struct rw_semaphore fm_sem;
+	struct mutex fm_mutex;
+	void *fm_buf;
+	size_t fm_size;
+	struct work_struct fm_work;
+
 	/* Wear-leveling sub-system's stuff */
 	struct rb_root used;
 	struct rb_root erroneous;
 	struct rb_root free;
+	int free_count;
 	struct rb_root scrub;
 	struct list_head pq[UBI_PROT_QUEUE_LEN];
 	int pq_head;
@@ -596,6 +666,32 @@ struct ubi_attach_info {
 	struct kmem_cache *aeb_slab_cache;
 };
 
+/**
+ * struct ubi_work - UBI work description data structure.
+ * @list: a link in the list of pending works
+ * @func: worker function
+ * @e: physical eraseblock to erase
+ * @vol_id: the volume ID on which this erasure is being performed
+ * @lnum: the logical eraseblock number
+ * @torture: if the physical eraseblock has to be tortured
+ * @anchor: produce a anchor PEB to by used by fastmap
+ *
+ * The @func pointer points to the worker function. If the @cancel argument is
+ * not zero, the worker has to free the resources and exit immediately. The
+ * worker has to return zero in case of success and a negative error code in
+ * case of failure.
+ */
+struct ubi_work {
+	struct list_head list;
+	int (*func)(struct ubi_device *ubi, struct ubi_work *wrk, int cancel);
+	/* The below fields are only relevant to erasure works */
+	struct ubi_wl_entry *e;
+	int vol_id;
+	int lnum;
+	int torture;
+	int anchor;
+};
+
 #include "debug.h"
 
 extern struct kmem_cache *ubi_wl_entry_slab;
@@ -606,7 +702,7 @@ extern struct class *ubi_class;
 extern struct mutex ubi_devices_mutex;
 extern struct blocking_notifier_head ubi_notifiers;
 
-/* scan.c */
+/* attach.c */
 int ubi_add_to_av(struct ubi_device *ubi, struct ubi_attach_info *ai, int pnum,
 		  int ec, const struct ubi_vid_hdr *vid_hdr, int bitflips);
 struct ubi_ainf_volume *ubi_find_av(const struct ubi_attach_info *ai,
@@ -614,7 +710,7 @@ struct ubi_ainf_volume *ubi_find_av(const struct ubi_attach_info *ai,
 void ubi_remove_av(struct ubi_attach_info *ai, struct ubi_ainf_volume *av);
 struct ubi_ainf_peb *ubi_early_get_peb(struct ubi_device *ubi,
 				       struct ubi_attach_info *ai);
-int ubi_attach(struct ubi_device *ubi);
+int ubi_attach(struct ubi_device *ubi, int force_scan);
 void ubi_destroy_ai(struct ubi_attach_info *ai);
 
 /* vtbl.c */
@@ -664,6 +760,9 @@ int ubi_eba_atomic_leb_change(struct ubi_device *ubi, struct ubi_volume *vol,
 int ubi_eba_copy_leb(struct ubi_device *ubi, int from, int to,
 		     struct ubi_vid_hdr *vid_hdr);
 int ubi_eba_init(struct ubi_device *ubi, struct ubi_attach_info *ai);
+unsigned long long ubi_next_sqnum(struct ubi_device *ubi);
+int self_check_eba(struct ubi_device *ubi, struct ubi_attach_info *ai_fastmap,
+		   struct ubi_attach_info *ai_scan);
 
 /* wl.c */
 int ubi_wl_get_peb(struct ubi_device *ubi);
@@ -674,6 +773,12 @@ int ubi_wl_scrub_peb(struct ubi_device *ubi, int pnum);
 int ubi_wl_init(struct ubi_device *ubi, struct ubi_attach_info *ai);
 void ubi_wl_close(struct ubi_device *ubi);
 int ubi_thread(void *u);
+struct ubi_wl_entry *ubi_wl_get_fm_peb(struct ubi_device *ubi, int anchor);
+int ubi_wl_put_fm_peb(struct ubi_device *ubi, struct ubi_wl_entry *used_e,
+		      int lnum, int torture);
+int ubi_is_erase_work(struct ubi_work *wrk);
+void ubi_refill_pools(struct ubi_device *ubi);
+int ubi_ensure_anchor_pebs(struct ubi_device *ubi);
 
 /* io.c */
 int ubi_io_read(const struct ubi_device *ubi, void *buf, int pnum, int offset,
@@ -711,6 +816,15 @@ void ubi_free_internal_volumes(struct ubi_device *ubi);
 void ubi_do_get_device_info(struct ubi_device *ubi, struct ubi_device_info *di);
 void ubi_do_get_volume_info(struct ubi_device *ubi, struct ubi_volume *vol,
 			    struct ubi_volume_info *vi);
+/* scan.c */
+int ubi_compare_lebs(struct ubi_device *ubi, const struct ubi_ainf_peb *aeb,
+		      int pnum, const struct ubi_vid_hdr *vid_hdr);
+
+/* fastmap.c */
+size_t ubi_calc_fm_size(struct ubi_device *ubi);
+int ubi_update_fastmap(struct ubi_device *ubi);
+int ubi_scan_fastmap(struct ubi_device *ubi, struct ubi_attach_info *ai,
+		     int fm_anchor);
 
 /*
  * ubi_rb_for_each_entry - walk an RB-tree.
diff --git a/drivers/mtd/ubi/wl.c b/drivers/mtd/ubi/wl.c
index 032fc57f109..da7b44998b4 100644
--- a/drivers/mtd/ubi/wl.c
+++ b/drivers/mtd/ubi/wl.c
@@ -135,36 +135,48 @@
  */
 #define WL_MAX_FAILURES 32
 
-/**
- * struct ubi_work - UBI work description data structure.
- * @list: a link in the list of pending works
- * @func: worker function
- * @e: physical eraseblock to erase
- * @vol_id: the volume ID on which this erasure is being performed
- * @lnum: the logical eraseblock number
- * @torture: if the physical eraseblock has to be tortured
- *
- * The @func pointer points to the worker function. If the @cancel argument is
- * not zero, the worker has to free the resources and exit immediately. The
- * worker has to return zero in case of success and a negative error code in
- * case of failure.
- */
-struct ubi_work {
-	struct list_head list;
-	int (*func)(struct ubi_device *ubi, struct ubi_work *wrk, int cancel);
-	/* The below fields are only relevant to erasure works */
-	struct ubi_wl_entry *e;
-	int vol_id;
-	int lnum;
-	int torture;
-};
-
 static int self_check_ec(struct ubi_device *ubi, int pnum, int ec);
 static int self_check_in_wl_tree(const struct ubi_device *ubi,
 				 struct ubi_wl_entry *e, struct rb_root *root);
 static int self_check_in_pq(const struct ubi_device *ubi,
 			    struct ubi_wl_entry *e);
 
+#ifdef CONFIG_MTD_UBI_FASTMAP
+/**
+ * update_fastmap_work_fn - calls ubi_update_fastmap from a work queue
+ * @wrk: the work description object
+ */
+static void update_fastmap_work_fn(struct work_struct *wrk)
+{
+	struct ubi_device *ubi = container_of(wrk, struct ubi_device, fm_work);
+	ubi_update_fastmap(ubi);
+}
+
+/**
+ *  ubi_ubi_is_fm_block - returns 1 if a PEB is currently used in a fastmap.
+ *  @ubi: UBI device description object
+ *  @pnum: the to be checked PEB
+ */
+static int ubi_is_fm_block(struct ubi_device *ubi, int pnum)
+{
+	int i;
+
+	if (!ubi->fm)
+		return 0;
+
+	for (i = 0; i < ubi->fm->used_blocks; i++)
+		if (ubi->fm->e[i]->pnum == pnum)
+			return 1;
+
+	return 0;
+}
+#else
+static int ubi_is_fm_block(struct ubi_device *ubi, int pnum)
+{
+	return 0;
+}
+#endif
+
 /**
  * wl_tree_add - add a wear-leveling entry to a WL RB-tree.
  * @e: the wear-leveling entry to add
@@ -261,18 +273,16 @@ static int produce_free_peb(struct ubi_device *ubi)
 {
 	int err;
 
-	spin_lock(&ubi->wl_lock);
 	while (!ubi->free.rb_node) {
 		spin_unlock(&ubi->wl_lock);
 
 		dbg_wl("do one work synchronously");
 		err = do_work(ubi);
-		if (err)
-			return err;
 
 		spin_lock(&ubi->wl_lock);
+		if (err)
+			return err;
 	}
-	spin_unlock(&ubi->wl_lock);
 
 	return 0;
 }
@@ -339,16 +349,18 @@ static void prot_queue_add(struct ubi_device *ubi, struct ubi_wl_entry *e)
 
 /**
  * find_wl_entry - find wear-leveling entry closest to certain erase counter.
+ * @ubi: UBI device description object
  * @root: the RB-tree where to look for
  * @diff: maximum possible difference from the smallest erase counter
  *
  * This function looks for a wear leveling entry with erase counter closest to
  * min + @diff, where min is the smallest erase counter.
  */
-static struct ubi_wl_entry *find_wl_entry(struct rb_root *root, int diff)
+static struct ubi_wl_entry *find_wl_entry(struct ubi_device *ubi,
+					  struct rb_root *root, int diff)
 {
 	struct rb_node *p;
-	struct ubi_wl_entry *e;
+	struct ubi_wl_entry *e, *prev_e = NULL;
 	int max;
 
 	e = rb_entry(rb_first(root), struct ubi_wl_entry, u.rb);
@@ -363,35 +375,143 @@ static struct ubi_wl_entry *find_wl_entry(struct rb_root *root, int diff)
 			p = p->rb_left;
 		else {
 			p = p->rb_right;
+			prev_e = e;
 			e = e1;
 		}
 	}
 
+	/* If no fastmap has been written and this WL entry can be used
+	 * as anchor PEB, hold it back and return the second best WL entry
+	 * such that fastmap can use the anchor PEB later. */
+	if (prev_e && !ubi->fm_disabled &&
+	    !ubi->fm && e->pnum < UBI_FM_MAX_START)
+		return prev_e;
+
 	return e;
 }
 
 /**
- * ubi_wl_get_peb - get a physical eraseblock.
+ * find_mean_wl_entry - find wear-leveling entry with medium erase counter.
+ * @ubi: UBI device description object
+ * @root: the RB-tree where to look for
+ *
+ * This function looks for a wear leveling entry with medium erase counter,
+ * but not greater or equivalent than the lowest erase counter plus
+ * %WL_FREE_MAX_DIFF/2.
+ */
+static struct ubi_wl_entry *find_mean_wl_entry(struct ubi_device *ubi,
+					       struct rb_root *root)
+{
+	struct ubi_wl_entry *e, *first, *last;
+
+	first = rb_entry(rb_first(root), struct ubi_wl_entry, u.rb);
+	last = rb_entry(rb_last(root), struct ubi_wl_entry, u.rb);
+
+	if (last->ec - first->ec < WL_FREE_MAX_DIFF) {
+		e = rb_entry(root->rb_node, struct ubi_wl_entry, u.rb);
+
+#ifdef CONFIG_MTD_UBI_FASTMAP
+		/* If no fastmap has been written and this WL entry can be used
+		 * as anchor PEB, hold it back and return the second best
+		 * WL entry such that fastmap can use the anchor PEB later. */
+		if (e && !ubi->fm_disabled && !ubi->fm &&
+		    e->pnum < UBI_FM_MAX_START)
+			e = rb_entry(rb_next(root->rb_node),
+				     struct ubi_wl_entry, u.rb);
+#endif
+	} else
+		e = find_wl_entry(ubi, root, WL_FREE_MAX_DIFF/2);
+
+	return e;
+}
+
+#ifdef CONFIG_MTD_UBI_FASTMAP
+/**
+ * find_anchor_wl_entry - find wear-leveling entry to used as anchor PEB.
+ * @root: the RB-tree where to look for
+ */
+static struct ubi_wl_entry *find_anchor_wl_entry(struct rb_root *root)
+{
+	struct rb_node *p;
+	struct ubi_wl_entry *e, *victim = NULL;
+	int max_ec = UBI_MAX_ERASECOUNTER;
+
+	ubi_rb_for_each_entry(p, e, root, u.rb) {
+		if (e->pnum < UBI_FM_MAX_START && e->ec < max_ec) {
+			victim = e;
+			max_ec = e->ec;
+		}
+	}
+
+	return victim;
+}
+
+static int anchor_pebs_avalible(struct rb_root *root)
+{
+	struct rb_node *p;
+	struct ubi_wl_entry *e;
+
+	ubi_rb_for_each_entry(p, e, root, u.rb)
+		if (e->pnum < UBI_FM_MAX_START)
+			return 1;
+
+	return 0;
+}
+
+/**
+ * ubi_wl_get_fm_peb - find a physical erase block with a given maximal number.
+ * @ubi: UBI device description object
+ * @anchor: This PEB will be used as anchor PEB by fastmap
+ *
+ * The function returns a physical erase block with a given maximal number
+ * and removes it from the wl subsystem.
+ * Must be called with wl_lock held!
+ */
+struct ubi_wl_entry *ubi_wl_get_fm_peb(struct ubi_device *ubi, int anchor)
+{
+	struct ubi_wl_entry *e = NULL;
+
+	if (!ubi->free.rb_node || (ubi->free_count - ubi->beb_rsvd_pebs < 1))
+		goto out;
+
+	if (anchor)
+		e = find_anchor_wl_entry(&ubi->free);
+	else
+		e = find_mean_wl_entry(ubi, &ubi->free);
+
+	if (!e)
+		goto out;
+
+	self_check_in_wl_tree(ubi, e, &ubi->free);
+
+	/* remove it from the free list,
+	 * the wl subsystem does no longer know this erase block */
+	rb_erase(&e->u.rb, &ubi->free);
+	ubi->free_count--;
+out:
+	return e;
+}
+#endif
+
+/**
+ * __wl_get_peb - get a physical eraseblock.
  * @ubi: UBI device description object
  *
  * This function returns a physical eraseblock in case of success and a
  * negative error code in case of failure. Might sleep.
  */
-int ubi_wl_get_peb(struct ubi_device *ubi)
+static int __wl_get_peb(struct ubi_device *ubi)
 {
 	int err;
-	struct ubi_wl_entry *e, *first, *last;
+	struct ubi_wl_entry *e;
 
 retry:
-	spin_lock(&ubi->wl_lock);
 	if (!ubi->free.rb_node) {
 		if (ubi->works_count == 0) {
-			ubi_assert(list_empty(&ubi->works));
 			ubi_err("no free eraseblocks");
-			spin_unlock(&ubi->wl_lock);
+			ubi_assert(list_empty(&ubi->works));
 			return -ENOSPC;
 		}
-		spin_unlock(&ubi->wl_lock);
 
 		err = produce_free_peb(ubi);
 		if (err < 0)
@@ -399,13 +519,11 @@ retry:
 		goto retry;
 	}
 
-	first = rb_entry(rb_first(&ubi->free), struct ubi_wl_entry, u.rb);
-	last = rb_entry(rb_last(&ubi->free), struct ubi_wl_entry, u.rb);
-
-	if (last->ec - first->ec < WL_FREE_MAX_DIFF)
-		e = rb_entry(ubi->free.rb_node, struct ubi_wl_entry, u.rb);
-	else
-		e = find_wl_entry(&ubi->free, WL_FREE_MAX_DIFF/2);
+	e = find_mean_wl_entry(ubi, &ubi->free);
+	if (!e) {
+		ubi_err("no free eraseblocks");
+		return -ENOSPC;
+	}
 
 	self_check_in_wl_tree(ubi, e, &ubi->free);
 
@@ -414,10 +532,14 @@ retry:
 	 * be protected from being moved for some time.
 	 */
 	rb_erase(&e->u.rb, &ubi->free);
+	ubi->free_count--;
 	dbg_wl("PEB %d EC %d", e->pnum, e->ec);
+#ifndef CONFIG_MTD_UBI_FASTMAP
+	/* We have to enqueue e only if fastmap is disabled,
+	 * is fastmap enabled prot_queue_add() will be called by
+	 * ubi_wl_get_peb() after removing e from the pool. */
 	prot_queue_add(ubi, e);
-	spin_unlock(&ubi->wl_lock);
-
+#endif
 	err = ubi_self_check_all_ff(ubi, e->pnum, ubi->vid_hdr_aloffset,
 				    ubi->peb_size - ubi->vid_hdr_aloffset);
 	if (err) {
@@ -428,6 +550,150 @@ retry:
 	return e->pnum;
 }
 
+#ifdef CONFIG_MTD_UBI_FASTMAP
+/**
+ * return_unused_pool_pebs - returns unused PEB to the free tree.
+ * @ubi: UBI device description object
+ * @pool: fastmap pool description object
+ */
+static void return_unused_pool_pebs(struct ubi_device *ubi,
+				    struct ubi_fm_pool *pool)
+{
+	int i;
+	struct ubi_wl_entry *e;
+
+	for (i = pool->used; i < pool->size; i++) {
+		e = ubi->lookuptbl[pool->pebs[i]];
+		wl_tree_add(e, &ubi->free);
+		ubi->free_count++;
+	}
+}
+
+/**
+ * refill_wl_pool - refills all the fastmap pool used by the
+ * WL sub-system.
+ * @ubi: UBI device description object
+ */
+static void refill_wl_pool(struct ubi_device *ubi)
+{
+	struct ubi_wl_entry *e;
+	struct ubi_fm_pool *pool = &ubi->fm_wl_pool;
+
+	return_unused_pool_pebs(ubi, pool);
+
+	for (pool->size = 0; pool->size < pool->max_size; pool->size++) {
+		if (!ubi->free.rb_node ||
+		   (ubi->free_count - ubi->beb_rsvd_pebs < 5))
+			break;
+
+		e = find_wl_entry(ubi, &ubi->free, WL_FREE_MAX_DIFF);
+		self_check_in_wl_tree(ubi, e, &ubi->free);
+		rb_erase(&e->u.rb, &ubi->free);
+		ubi->free_count--;
+
+		pool->pebs[pool->size] = e->pnum;
+	}
+	pool->used = 0;
+}
+
+/**
+ * refill_wl_user_pool - refills all the fastmap pool used by ubi_wl_get_peb.
+ * @ubi: UBI device description object
+ */
+static void refill_wl_user_pool(struct ubi_device *ubi)
+{
+	struct ubi_fm_pool *pool = &ubi->fm_pool;
+
+	return_unused_pool_pebs(ubi, pool);
+
+	for (pool->size = 0; pool->size < pool->max_size; pool->size++) {
+		if (!ubi->free.rb_node ||
+		   (ubi->free_count - ubi->beb_rsvd_pebs < 1))
+			break;
+
+		pool->pebs[pool->size] = __wl_get_peb(ubi);
+		if (pool->pebs[pool->size] < 0)
+			break;
+	}
+	pool->used = 0;
+}
+
+/**
+ * ubi_refill_pools - refills all fastmap PEB pools.
+ * @ubi: UBI device description object
+ */
+void ubi_refill_pools(struct ubi_device *ubi)
+{
+	spin_lock(&ubi->wl_lock);
+	refill_wl_pool(ubi);
+	refill_wl_user_pool(ubi);
+	spin_unlock(&ubi->wl_lock);
+}
+
+/* ubi_wl_get_peb - works exaclty like __wl_get_peb but keeps track of
+ * the fastmap pool.
+ */
+int ubi_wl_get_peb(struct ubi_device *ubi)
+{
+	int ret;
+	struct ubi_fm_pool *pool = &ubi->fm_pool;
+	struct ubi_fm_pool *wl_pool = &ubi->fm_wl_pool;
+
+	if (!pool->size || !wl_pool->size || pool->used == pool->size ||
+	    wl_pool->used == wl_pool->size)
+		ubi_update_fastmap(ubi);
+
+	/* we got not a single free PEB */
+	if (!pool->size)
+		ret = -ENOSPC;
+	else {
+		spin_lock(&ubi->wl_lock);
+		ret = pool->pebs[pool->used++];
+		prot_queue_add(ubi, ubi->lookuptbl[ret]);
+		spin_unlock(&ubi->wl_lock);
+	}
+
+	return ret;
+}
+
+/* get_peb_for_wl - returns a PEB to be used internally by the WL sub-system.
+ *
+ * @ubi: UBI device description object
+ */
+static struct ubi_wl_entry *get_peb_for_wl(struct ubi_device *ubi)
+{
+	struct ubi_fm_pool *pool = &ubi->fm_wl_pool;
+	int pnum;
+
+	if (pool->used == pool->size || !pool->size) {
+		/* We cannot update the fastmap here because this
+		 * function is called in atomic context.
+		 * Let's fail here and refill/update it as soon as possible. */
+		schedule_work(&ubi->fm_work);
+		return NULL;
+	} else {
+		pnum = pool->pebs[pool->used++];
+		return ubi->lookuptbl[pnum];
+	}
+}
+#else
+static struct ubi_wl_entry *get_peb_for_wl(struct ubi_device *ubi)
+{
+	return find_wl_entry(ubi, &ubi->free, WL_FREE_MAX_DIFF);
+}
+
+int ubi_wl_get_peb(struct ubi_device *ubi)
+{
+	int peb;
+
+	spin_lock(&ubi->wl_lock);
+	peb = __wl_get_peb(ubi);
+	spin_unlock(&ubi->wl_lock);
+
+	return peb;
+}
+#endif
+
 /**
  * prot_queue_del - remove a physical eraseblock from the protection queue.
  * @ubi: UBI device description object
@@ -558,14 +824,14 @@ repeat:
 }
 
 /**
- * schedule_ubi_work - schedule a work.
+ * __schedule_ubi_work - schedule a work.
  * @ubi: UBI device description object
  * @wrk: the work to schedule
  *
  * This function adds a work defined by @wrk to the tail of the pending works
- * list.
+ * list. Can only be used of ubi->work_sem is already held in read mode!
  */
-static void schedule_ubi_work(struct ubi_device *ubi, struct ubi_work *wrk)
+static void __schedule_ubi_work(struct ubi_device *ubi, struct ubi_work *wrk)
 {
 	spin_lock(&ubi->wl_lock);
 	list_add_tail(&wrk->list, &ubi->works);
@@ -576,9 +842,35 @@ static void schedule_ubi_work(struct ubi_device *ubi, struct ubi_work *wrk)
 	spin_unlock(&ubi->wl_lock);
 }
 
+/**
+ * schedule_ubi_work - schedule a work.
+ * @ubi: UBI device description object
+ * @wrk: the work to schedule
+ *
+ * This function adds a work defined by @wrk to the tail of the pending works
+ * list.
+ */
+static void schedule_ubi_work(struct ubi_device *ubi, struct ubi_work *wrk)
+{
+	down_read(&ubi->work_sem);
+	__schedule_ubi_work(ubi, wrk);
+	up_read(&ubi->work_sem);
+}
+
 static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
 			int cancel);
 
+#ifdef CONFIG_MTD_UBI_FASTMAP
+/**
+ * ubi_is_erase_work - checks whether a work is erase work.
+ * @wrk: The work object to be checked
+ */
+int ubi_is_erase_work(struct ubi_work *wrk)
+{
+	return wrk->func == erase_worker;
+}
+#endif
+
 /**
  * schedule_erase - schedule an erase work.
  * @ubi: UBI device description object
@@ -595,6 +887,9 @@ static int schedule_erase(struct ubi_device *ubi, struct ubi_wl_entry *e,
 {
 	struct ubi_work *wl_wrk;
 
+	ubi_assert(e);
+	ubi_assert(!ubi_is_fm_block(ubi, e->pnum));
+
 	dbg_wl("schedule erasure of PEB %d, EC %d, torture %d",
 	       e->pnum, e->ec, torture);
 
@@ -613,6 +908,79 @@ static int schedule_erase(struct ubi_device *ubi, struct ubi_wl_entry *e,
 }
 
 /**
+ * do_sync_erase - run the erase worker synchronously.
+ * @ubi: UBI device description object
+ * @e: the WL entry of the physical eraseblock to erase
+ * @vol_id: the volume ID that last used this PEB
+ * @lnum: the last used logical eraseblock number for the PEB
+ * @torture: if the physical eraseblock has to be tortured
+ *
+ */
+static int do_sync_erase(struct ubi_device *ubi, struct ubi_wl_entry *e,
+			 int vol_id, int lnum, int torture)
+{
+	struct ubi_work *wl_wrk;
+
+	dbg_wl("sync erase of PEB %i", e->pnum);
+
+	wl_wrk = kmalloc(sizeof(struct ubi_work), GFP_NOFS);
+	if (!wl_wrk)
+		return -ENOMEM;
+
+	wl_wrk->e = e;
+	wl_wrk->vol_id = vol_id;
+	wl_wrk->lnum = lnum;
+	wl_wrk->torture = torture;
+
+	return erase_worker(ubi, wl_wrk, 0);
+}
+
+#ifdef CONFIG_MTD_UBI_FASTMAP
+/**
+ * ubi_wl_put_fm_peb - returns a PEB used in a fastmap to the wear-leveling
+ * sub-system.
+ * see: ubi_wl_put_peb()
+ *
+ * @ubi: UBI device description object
+ * @fm_e: physical eraseblock to return
+ * @lnum: the last used logical eraseblock number for the PEB
+ * @torture: if this physical eraseblock has to be tortured
+ */
+int ubi_wl_put_fm_peb(struct ubi_device *ubi, struct ubi_wl_entry *fm_e,
+		      int lnum, int torture)
+{
+	struct ubi_wl_entry *e;
+	int vol_id, pnum = fm_e->pnum;
+
+	dbg_wl("PEB %d", pnum);
+
+	ubi_assert(pnum >= 0);
+	ubi_assert(pnum < ubi->peb_count);
+
+	spin_lock(&ubi->wl_lock);
+	e = ubi->lookuptbl[pnum];
+
+	/* This can happen if we recovered from a fastmap the very
+	 * first time and writing now a new one. In this case the wl system
+	 * has never seen any PEB used by the original fastmap.
+	 */
+	if (!e) {
+		e = fm_e;
+		ubi_assert(e->ec >= 0);
+		ubi->lookuptbl[pnum] = e;
+	} else {
+		e->ec = fm_e->ec;
+		kfree(fm_e);
+	}
+
+	spin_unlock(&ubi->wl_lock);
+
+	vol_id = lnum ? UBI_FM_DATA_VOLUME_ID : UBI_FM_SB_VOLUME_ID;
+	return schedule_erase(ubi, e, vol_id, lnum, torture);
+}
+#endif
+
+/**
  * wear_leveling_worker - wear-leveling worker function.
  * @ubi: UBI device description object
  * @wrk: the work object
@@ -627,6 +995,9 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 {
 	int err, scrubbing = 0, torture = 0, protect = 0, erroneous = 0;
 	int vol_id = -1, uninitialized_var(lnum);
+#ifdef CONFIG_MTD_UBI_FASTMAP
+	int anchor = wrk->anchor;
+#endif
 	struct ubi_wl_entry *e1, *e2;
 	struct ubi_vid_hdr *vid_hdr;
 
@@ -660,14 +1031,35 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 		goto out_cancel;
 	}
 
+#ifdef CONFIG_MTD_UBI_FASTMAP
+	/* Check whether we need to produce an anchor PEB */
+	if (!anchor)
+		anchor = !anchor_pebs_avalible(&ubi->free);
+
+	if (anchor) {
+		e1 = find_anchor_wl_entry(&ubi->used);
+		if (!e1)
+			goto out_cancel;
+		e2 = get_peb_for_wl(ubi);
+		if (!e2)
+			goto out_cancel;
+
+		self_check_in_wl_tree(ubi, e1, &ubi->used);
+		rb_erase(&e1->u.rb, &ubi->used);
+		dbg_wl("anchor-move PEB %d to PEB %d", e1->pnum, e2->pnum);
+	} else if (!ubi->scrub.rb_node) {
+#else
 	if (!ubi->scrub.rb_node) {
+#endif
 		/*
 		 * Now pick the least worn-out used physical eraseblock and a
 		 * highly worn-out free physical eraseblock. If the erase
 		 * counters differ much enough, start wear-leveling.
 		 */
 		e1 = rb_entry(rb_first(&ubi->used), struct ubi_wl_entry, u.rb);
-		e2 = find_wl_entry(&ubi->free, WL_FREE_MAX_DIFF);
+		e2 = get_peb_for_wl(ubi);
+		if (!e2)
+			goto out_cancel;
 
 		if (!(e2->ec - e1->ec >= UBI_WL_THRESHOLD)) {
 			dbg_wl("no WL needed: min used EC %d, max free EC %d",
@@ -682,14 +1074,15 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 		/* Perform scrubbing */
 		scrubbing = 1;
 		e1 = rb_entry(rb_first(&ubi->scrub), struct ubi_wl_entry, u.rb);
-		e2 = find_wl_entry(&ubi->free, WL_FREE_MAX_DIFF);
+		e2 = get_peb_for_wl(ubi);
+		if (!e2)
+			goto out_cancel;
+
 		self_check_in_wl_tree(ubi, e1, &ubi->scrub);
 		rb_erase(&e1->u.rb, &ubi->scrub);
 		dbg_wl("scrub PEB %d to PEB %d", e1->pnum, e2->pnum);
 	}
 
-	self_check_in_wl_tree(ubi, e2, &ubi->free);
-	rb_erase(&e2->u.rb, &ubi->free);
 	ubi->move_from = e1;
 	ubi->move_to = e2;
 	spin_unlock(&ubi->wl_lock);
@@ -806,7 +1199,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 	ubi->move_to_put = ubi->wl_scheduled = 0;
 	spin_unlock(&ubi->wl_lock);
 
-	err = schedule_erase(ubi, e1, vol_id, lnum, 0);
+	err = do_sync_erase(ubi, e1, vol_id, lnum, 0);
 	if (err) {
 		kmem_cache_free(ubi_wl_entry_slab, e1);
 		if (e2)
@@ -821,7 +1214,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 		 */
 		dbg_wl("PEB %d (LEB %d:%d) was put meanwhile, erase",
 		       e2->pnum, vol_id, lnum);
-		err = schedule_erase(ubi, e2, vol_id, lnum, 0);
+		err = do_sync_erase(ubi, e2, vol_id, lnum, 0);
 		if (err) {
 			kmem_cache_free(ubi_wl_entry_slab, e2);
 			goto out_ro;
@@ -860,7 +1253,7 @@ out_not_moved:
 	spin_unlock(&ubi->wl_lock);
 
 	ubi_free_vid_hdr(ubi, vid_hdr);
-	err = schedule_erase(ubi, e2, vol_id, lnum, torture);
+	err = do_sync_erase(ubi, e2, vol_id, lnum, torture);
 	if (err) {
 		kmem_cache_free(ubi_wl_entry_slab, e2);
 		goto out_ro;
@@ -901,12 +1294,13 @@ out_cancel:
 /**
  * ensure_wear_leveling - schedule wear-leveling if it is needed.
  * @ubi: UBI device description object
+ * @nested: set to non-zero if this function is called from UBI worker
  *
  * This function checks if it is time to start wear-leveling and schedules it
  * if yes. This function returns zero in case of success and a negative error
  * code in case of failure.
  */
-static int ensure_wear_leveling(struct ubi_device *ubi)
+static int ensure_wear_leveling(struct ubi_device *ubi, int nested)
 {
 	int err = 0;
 	struct ubi_wl_entry *e1;
@@ -934,7 +1328,7 @@ static int ensure_wear_leveling(struct ubi_device *ubi)
 		 * %UBI_WL_THRESHOLD.
 		 */
 		e1 = rb_entry(rb_first(&ubi->used), struct ubi_wl_entry, u.rb);
-		e2 = find_wl_entry(&ubi->free, WL_FREE_MAX_DIFF);
+		e2 = find_wl_entry(ubi, &ubi->free, WL_FREE_MAX_DIFF);
 
 		if (!(e2->ec - e1->ec >= UBI_WL_THRESHOLD))
 			goto out_unlock;
@@ -951,8 +1345,12 @@ static int ensure_wear_leveling(struct ubi_device *ubi)
 		goto out_cancel;
 	}
 
+	wrk->anchor = 0;
 	wrk->func = &wear_leveling_worker;
-	schedule_ubi_work(ubi, wrk);
+	if (nested)
+		__schedule_ubi_work(ubi, wrk);
+	else
+		schedule_ubi_work(ubi, wrk);
 	return err;
 
 out_cancel:
@@ -963,6 +1361,38 @@ out_unlock:
 	return err;
 }
 
+#ifdef CONFIG_MTD_UBI_FASTMAP
+/**
+ * ubi_ensure_anchor_pebs - schedule wear-leveling to produce an anchor PEB.
+ * @ubi: UBI device description object
+ */
+int ubi_ensure_anchor_pebs(struct ubi_device *ubi)
+{
+	struct ubi_work *wrk;
+
+	spin_lock(&ubi->wl_lock);
+	if (ubi->wl_scheduled) {
+		spin_unlock(&ubi->wl_lock);
+		return 0;
+	}
+	ubi->wl_scheduled = 1;
+	spin_unlock(&ubi->wl_lock);
+
+	wrk = kmalloc(sizeof(struct ubi_work), GFP_NOFS);
+	if (!wrk) {
+		spin_lock(&ubi->wl_lock);
+		ubi->wl_scheduled = 0;
+		spin_unlock(&ubi->wl_lock);
+		return -ENOMEM;
+	}
+
+	wrk->anchor = 1;
+	wrk->func = &wear_leveling_worker;
+	schedule_ubi_work(ubi, wrk);
+	return 0;
+}
+#endif
+
 /**
  * erase_worker - physical eraseblock erase worker function.
  * @ubi: UBI device description object
@@ -993,6 +1423,8 @@ static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
 	dbg_wl("erase PEB %d EC %d LEB %d:%d",
 	       pnum, e->ec, wl_wrk->vol_id, wl_wrk->lnum);
 
+	ubi_assert(!ubi_is_fm_block(ubi, e->pnum));
+
 	err = sync_erase(ubi, e, wl_wrk->torture);
 	if (!err) {
 		/* Fine, we've erased it successfully */
@@ -1000,6 +1432,7 @@ static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
 
 		spin_lock(&ubi->wl_lock);
 		wl_tree_add(e, &ubi->free);
+		ubi->free_count++;
 		spin_unlock(&ubi->wl_lock);
 
 		/*
@@ -1009,7 +1442,7 @@ static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
 		serve_prot_queue(ubi);
 
 		/* And take care about wear-leveling */
-		err = ensure_wear_leveling(ubi);
+		err = ensure_wear_leveling(ubi, 1);
 		return err;
 	}
 
@@ -1247,7 +1680,7 @@ retry:
 	 * Technically scrubbing is the same as wear-leveling, so it is done
 	 * by the WL worker.
 	 */
-	return ensure_wear_leveling(ubi);
+	return ensure_wear_leveling(ubi, 0);
 }
 
 /**
@@ -1428,7 +1861,7 @@ static void cancel_pending(struct ubi_device *ubi)
  */
 int ubi_wl_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 {
-	int err, i;
+	int err, i, reserved_pebs, found_pebs = 0;
 	struct rb_node *rb1, *rb2;
 	struct ubi_ainf_volume *av;
 	struct ubi_ainf_peb *aeb, *tmp;
@@ -1440,6 +1873,9 @@ int ubi_wl_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 	init_rwsem(&ubi->work_sem);
 	ubi->max_ec = ai->max_ec;
 	INIT_LIST_HEAD(&ubi->works);
+#ifdef CONFIG_MTD_UBI_FASTMAP
+	INIT_WORK(&ubi->fm_work, update_fastmap_work_fn);
+#endif
 
 	sprintf(ubi->bgt_name, UBI_BGT_NAME_PATTERN, ubi->ubi_num);
 
@@ -1461,13 +1897,17 @@ int ubi_wl_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 
 		e->pnum = aeb->pnum;
 		e->ec = aeb->ec;
+		ubi_assert(!ubi_is_fm_block(ubi, e->pnum));
 		ubi->lookuptbl[e->pnum] = e;
 		if (schedule_erase(ubi, e, aeb->vol_id, aeb->lnum, 0)) {
 			kmem_cache_free(ubi_wl_entry_slab, e);
 			goto out_free;
 		}
+
+		found_pebs++;
 	}
 
+	ubi->free_count = 0;
 	list_for_each_entry(aeb, &ai->free, u.list) {
 		cond_resched();
 
@@ -1478,8 +1918,14 @@ int ubi_wl_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 		e->pnum = aeb->pnum;
 		e->ec = aeb->ec;
 		ubi_assert(e->ec >= 0);
+		ubi_assert(!ubi_is_fm_block(ubi, e->pnum));
+
 		wl_tree_add(e, &ubi->free);
+		ubi->free_count++;
+
 		ubi->lookuptbl[e->pnum] = e;
+
+		found_pebs++;
 	}
 
 	ubi_rb_for_each_entry(rb1, av, &ai->volumes, rb) {
@@ -1493,6 +1939,7 @@ int ubi_wl_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 			e->pnum = aeb->pnum;
 			e->ec = aeb->ec;
 			ubi->lookuptbl[e->pnum] = e;
+
 			if (!aeb->scrub) {
 				dbg_wl("add PEB %d EC %d to the used tree",
 				       e->pnum, e->ec);
@@ -1502,22 +1949,38 @@ int ubi_wl_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 				       e->pnum, e->ec);
 				wl_tree_add(e, &ubi->scrub);
 			}
+
+			found_pebs++;
 		}
 	}
 
-	if (ubi->avail_pebs < WL_RESERVED_PEBS) {
+	dbg_wl("found %i PEBs", found_pebs);
+
+	if (ubi->fm)
+		ubi_assert(ubi->good_peb_count == \
+			   found_pebs + ubi->fm->used_blocks);
+	else
+		ubi_assert(ubi->good_peb_count == found_pebs);
+
+	reserved_pebs = WL_RESERVED_PEBS;
+#ifdef CONFIG_MTD_UBI_FASTMAP
+	/* Reserve enough LEBs to store two fastmaps. */
+	reserved_pebs += (ubi->fm_size / ubi->leb_size) * 2;
+#endif
+
+	if (ubi->avail_pebs < reserved_pebs) {
 		ubi_err("no enough physical eraseblocks (%d, need %d)",
-			ubi->avail_pebs, WL_RESERVED_PEBS);
+			ubi->avail_pebs, reserved_pebs);
 		if (ubi->corr_peb_count)
 			ubi_err("%d PEBs are corrupted and not used",
 				ubi->corr_peb_count);
 		goto out_free;
 	}
-	ubi->avail_pebs -= WL_RESERVED_PEBS;
-	ubi->rsvd_pebs += WL_RESERVED_PEBS;
+	ubi->avail_pebs -= reserved_pebs;
+	ubi->rsvd_pebs += reserved_pebs;
 
 	/* Schedule wear-leveling if needed */
-	err = ensure_wear_leveling(ubi);
+	err = ensure_wear_leveling(ubi, 0);
 	if (err)
 		goto out_free;
 
@@ -1596,7 +2059,7 @@ static int self_check_ec(struct ubi_device *ubi, int pnum, int ec)
 	}
 
 	read_ec = be64_to_cpu(ec_hdr->ec);
-	if (ec != read_ec) {
+	if (ec != read_ec && read_ec - ec > 1) {
 		ubi_err("self-check failed for PEB %d", pnum);
 		ubi_err("read EC is %lld, should be %d", read_ec, ec);
 		dump_stack();
diff --git a/drivers/net/ethernet/amd/amd8111e.c b/drivers/net/ethernet/amd/amd8111e.c
index 64d0d9c1afa..3491d4312fc 100644
--- a/drivers/net/ethernet/amd/amd8111e.c
+++ b/drivers/net/ethernet/amd/amd8111e.c
@@ -1845,6 +1845,7 @@ static int __devinit amd8111e_probe_one(struct pci_dev *pdev,
 	if((pm_cap = pci_find_capability(pdev, PCI_CAP_ID_PM))==0){
 		printk(KERN_ERR "amd8111e: No Power Management capability, "
 		       "exiting.\n");
+		err = -ENODEV;
 		goto err_free_reg;
 	}
 
@@ -1852,6 +1853,7 @@ static int __devinit amd8111e_probe_one(struct pci_dev *pdev,
 	if (pci_set_dma_mask(pdev, DMA_BIT_MASK(32)) < 0) {
 		printk(KERN_ERR "amd8111e: DMA not supported,"
 			"exiting.\n");
+		err = -ENODEV;
 		goto err_free_reg;
 	}
 
diff --git a/drivers/net/ethernet/amd/au1000_eth.c b/drivers/net/ethernet/amd/au1000_eth.c
index 397596b078d..f195acfa2df 100644
--- a/drivers/net/ethernet/amd/au1000_eth.c
+++ b/drivers/net/ethernet/amd/au1000_eth.c
@@ -1174,8 +1174,10 @@ static int __devinit au1000_probe(struct platform_device *pdev)
 	snprintf(aup->mii_bus->id, MII_BUS_ID_SIZE, "%s-%x",
 		pdev->name, aup->mac_id);
 	aup->mii_bus->irq = kmalloc(sizeof(int)*PHY_MAX_ADDR, GFP_KERNEL);
-	if (aup->mii_bus->irq == NULL)
+	if (aup->mii_bus->irq == NULL) {
+		err = -ENOMEM;
 		goto err_out;
+	}
 
 	for (i = 0; i < PHY_MAX_ADDR; ++i)
 		aup->mii_bus->irq[i] = PHY_POLL;
@@ -1190,7 +1192,8 @@ static int __devinit au1000_probe(struct platform_device *pdev)
 		goto err_mdiobus_reg;
 	}
 
-	if (au1000_mii_probe(dev) != 0)
+	err = au1000_mii_probe(dev);
+	if (err != 0)
 		goto err_out;
 
 	pDBfree = NULL;
@@ -1205,6 +1208,7 @@ static int __devinit au1000_probe(struct platform_device *pdev)
 	}
 	aup->pDBfree = pDBfree;
 
+	err = -ENODEV;
 	for (i = 0; i < NUM_RX_DMA; i++) {
 		pDB = au1000_GetFreeDB(aup);
 		if (!pDB)
@@ -1213,6 +1217,8 @@ static int __devinit au1000_probe(struct platform_device *pdev)
 		aup->rx_dma_ring[i]->buff_stat = (unsigned)pDB->dma_addr;
 		aup->rx_db_inuse[i] = pDB;
 	}
+
+	err = -ENODEV;
 	for (i = 0; i < NUM_TX_DMA; i++) {
 		pDB = au1000_GetFreeDB(aup);
 		if (!pDB)
diff --git a/drivers/net/ethernet/broadcom/bcm63xx_enet.h b/drivers/net/ethernet/broadcom/bcm63xx_enet.h
index 0e3048b788c..133d5857b9e 100644
--- a/drivers/net/ethernet/broadcom/bcm63xx_enet.h
+++ b/drivers/net/ethernet/broadcom/bcm63xx_enet.h
@@ -10,6 +10,7 @@
 #include <bcm63xx_regs.h>
 #include <bcm63xx_irq.h>
 #include <bcm63xx_io.h>
+#include <bcm63xx_iudma.h>
 
 /* default number of descriptor */
 #define BCMENET_DEF_RX_DESC	64
@@ -31,35 +32,6 @@
 #define BCMENET_MAX_MTU		2046
 
 /*
- * rx/tx dma descriptor
- */
-struct bcm_enet_desc {
-	u32 len_stat;
-	u32 address;
-};
-
-#define DMADESC_LENGTH_SHIFT	16
-#define DMADESC_LENGTH_MASK	(0xfff << DMADESC_LENGTH_SHIFT)
-#define DMADESC_OWNER_MASK	(1 << 15)
-#define DMADESC_EOP_MASK	(1 << 14)
-#define DMADESC_SOP_MASK	(1 << 13)
-#define DMADESC_ESOP_MASK	(DMADESC_EOP_MASK | DMADESC_SOP_MASK)
-#define DMADESC_WRAP_MASK	(1 << 12)
-
-#define DMADESC_UNDER_MASK	(1 << 9)
-#define DMADESC_APPEND_CRC	(1 << 8)
-#define DMADESC_OVSIZE_MASK	(1 << 4)
-#define DMADESC_RXER_MASK	(1 << 2)
-#define DMADESC_CRC_MASK	(1 << 1)
-#define DMADESC_OV_MASK		(1 << 0)
-#define DMADESC_ERR_MASK	(DMADESC_UNDER_MASK | \
-				DMADESC_OVSIZE_MASK | \
-				DMADESC_RXER_MASK | \
-				DMADESC_CRC_MASK | \
-				DMADESC_OV_MASK)
-
-
-/*
  * MIB Counters register definitions
 */
 #define ETH_MIB_TX_GD_OCTETS			0
diff --git a/drivers/net/ethernet/calxeda/xgmac.c b/drivers/net/ethernet/calxeda/xgmac.c
index 2b4b4f529ab..16814b34d4b 100644
--- a/drivers/net/ethernet/calxeda/xgmac.c
+++ b/drivers/net/ethernet/calxeda/xgmac.c
@@ -375,7 +375,6 @@ struct xgmac_priv {
 	unsigned int tx_tail;
 
 	void __iomem *base;
-	struct sk_buff_head rx_recycle;
 	unsigned int dma_buf_sz;
 	dma_addr_t dma_rx_phy;
 	dma_addr_t dma_tx_phy;
@@ -672,9 +671,7 @@ static void xgmac_rx_refill(struct xgmac_priv *priv)
 		p = priv->dma_rx + entry;
 
 		if (priv->rx_skbuff[entry] == NULL) {
-			skb = __skb_dequeue(&priv->rx_recycle);
-			if (skb == NULL)
-				skb = netdev_alloc_skb(priv->dev, priv->dma_buf_sz);
+			skb = netdev_alloc_skb(priv->dev, priv->dma_buf_sz);
 			if (unlikely(skb == NULL))
 				break;
 
@@ -887,17 +884,7 @@ static void xgmac_tx_complete(struct xgmac_priv *priv)
 				       desc_get_buf_len(p), DMA_TO_DEVICE);
 		}
 
-		/*
-		 * If there's room in the queue (limit it to size)
-		 * we add this skb back into the pool,
-		 * if it's the right size.
-		 */
-		if ((skb_queue_len(&priv->rx_recycle) <
-			DMA_RX_RING_SZ) &&
-			skb_recycle_check(skb, priv->dma_buf_sz))
-			__skb_queue_head(&priv->rx_recycle, skb);
-		else
-			dev_kfree_skb(skb);
+		dev_kfree_skb(skb);
 	}
 
 	if (dma_ring_space(priv->tx_head, priv->tx_tail, DMA_TX_RING_SZ) >
@@ -1016,7 +1003,6 @@ static int xgmac_open(struct net_device *dev)
 			dev->dev_addr);
 	}
 
-	skb_queue_head_init(&priv->rx_recycle);
 	memset(&priv->xstats, 0, sizeof(struct xgmac_extra_stats));
 
 	/* Initialize the XGMAC and descriptors */
@@ -1053,7 +1039,6 @@ static int xgmac_stop(struct net_device *dev)
 		napi_disable(&priv->napi);
 
 	writel(0, priv->base + XGMAC_DMA_INTR_ENA);
-	skb_queue_purge(&priv->rx_recycle);
 
 	/* Disable the MAC core */
 	xgmac_mac_disable(priv->base);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
index 31752b24434..a4da893ac1e 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
@@ -696,6 +696,7 @@ int t4_seeprom_wp(struct adapter *adapter, bool enable);
 int get_vpd_params(struct adapter *adapter, struct vpd_params *p);
 int t4_load_fw(struct adapter *adapter, const u8 *fw_data, unsigned int size);
 unsigned int t4_flash_cfg_addr(struct adapter *adapter);
+int t4_load_cfg(struct adapter *adapter, const u8 *cfg_data, unsigned int size);
 int t4_check_fw_version(struct adapter *adapter);
 int t4_prep_adapter(struct adapter *adapter);
 int t4_port_init(struct adapter *adap, int mbox, int pf, int vf);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
index 6b9f6bb2f7e..604f4f87f55 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
@@ -443,7 +443,10 @@ int dbfifo_int_thresh = 10; /* 10 == 640 entry threshold */
 module_param(dbfifo_int_thresh, int, 0644);
 MODULE_PARM_DESC(dbfifo_int_thresh, "doorbell fifo interrupt threshold");
 
-int dbfifo_drain_delay = 1000; /* usecs to sleep while draining the dbfifo */
+/*
+ * usecs to sleep while draining the dbfifo
+ */
+static int dbfifo_drain_delay = 1000;
 module_param(dbfifo_drain_delay, int, 0644);
 MODULE_PARM_DESC(dbfifo_drain_delay,
 		 "usecs to sleep while draining the dbfifo");
@@ -636,7 +639,7 @@ static void name_msix_vecs(struct adapter *adap)
 static int request_msix_queue_irqs(struct adapter *adap)
 {
 	struct sge *s = &adap->sge;
-	int err, ethqidx, ofldqidx = 0, rdmaqidx = 0, msi = 2;
+	int err, ethqidx, ofldqidx = 0, rdmaqidx = 0, msi_index = 2;
 
 	err = request_irq(adap->msix_info[1].vec, t4_sge_intr_msix, 0,
 			  adap->msix_info[1].desc, &s->fw_evtq);
@@ -644,56 +647,60 @@ static int request_msix_queue_irqs(struct adapter *adap)
 		return err;
 
 	for_each_ethrxq(s, ethqidx) {
-		err = request_irq(adap->msix_info[msi].vec, t4_sge_intr_msix, 0,
-				  adap->msix_info[msi].desc,
+		err = request_irq(adap->msix_info[msi_index].vec,
+				  t4_sge_intr_msix, 0,
+				  adap->msix_info[msi_index].desc,
 				  &s->ethrxq[ethqidx].rspq);
 		if (err)
 			goto unwind;
-		msi++;
+		msi_index++;
 	}
 	for_each_ofldrxq(s, ofldqidx) {
-		err = request_irq(adap->msix_info[msi].vec, t4_sge_intr_msix, 0,
-				  adap->msix_info[msi].desc,
+		err = request_irq(adap->msix_info[msi_index].vec,
+				  t4_sge_intr_msix, 0,
+				  adap->msix_info[msi_index].desc,
 				  &s->ofldrxq[ofldqidx].rspq);
 		if (err)
 			goto unwind;
-		msi++;
+		msi_index++;
 	}
 	for_each_rdmarxq(s, rdmaqidx) {
-		err = request_irq(adap->msix_info[msi].vec, t4_sge_intr_msix, 0,
-				  adap->msix_info[msi].desc,
+		err = request_irq(adap->msix_info[msi_index].vec,
+				  t4_sge_intr_msix, 0,
+				  adap->msix_info[msi_index].desc,
 				  &s->rdmarxq[rdmaqidx].rspq);
 		if (err)
 			goto unwind;
-		msi++;
+		msi_index++;
 	}
 	return 0;
 
 unwind:
 	while (--rdmaqidx >= 0)
-		free_irq(adap->msix_info[--msi].vec,
+		free_irq(adap->msix_info[--msi_index].vec,
 			 &s->rdmarxq[rdmaqidx].rspq);
 	while (--ofldqidx >= 0)
-		free_irq(adap->msix_info[--msi].vec,
+		free_irq(adap->msix_info[--msi_index].vec,
 			 &s->ofldrxq[ofldqidx].rspq);
 	while (--ethqidx >= 0)
-		free_irq(adap->msix_info[--msi].vec, &s->ethrxq[ethqidx].rspq);
+		free_irq(adap->msix_info[--msi_index].vec,
+			 &s->ethrxq[ethqidx].rspq);
 	free_irq(adap->msix_info[1].vec, &s->fw_evtq);
 	return err;
 }
 
 static void free_msix_queue_irqs(struct adapter *adap)
 {
-	int i, msi = 2;
+	int i, msi_index = 2;
 	struct sge *s = &adap->sge;
 
 	free_irq(adap->msix_info[1].vec, &s->fw_evtq);
 	for_each_ethrxq(s, i)
-		free_irq(adap->msix_info[msi++].vec, &s->ethrxq[i].rspq);
+		free_irq(adap->msix_info[msi_index++].vec, &s->ethrxq[i].rspq);
 	for_each_ofldrxq(s, i)
-		free_irq(adap->msix_info[msi++].vec, &s->ofldrxq[i].rspq);
+		free_irq(adap->msix_info[msi_index++].vec, &s->ofldrxq[i].rspq);
 	for_each_rdmarxq(s, i)
-		free_irq(adap->msix_info[msi++].vec, &s->rdmarxq[i].rspq);
+		free_irq(adap->msix_info[msi_index++].vec, &s->rdmarxq[i].rspq);
 }
 
 /**
@@ -2535,9 +2542,8 @@ static int read_eq_indices(struct adapter *adap, u16 qid, u16 *pidx, u16 *cidx)
 
 	ret = t4_mem_win_read_len(adap, addr, (__be32 *)&indices, 8);
 	if (!ret) {
-		indices = be64_to_cpu(indices);
-		*cidx = (indices >> 25) & 0xffff;
-		*pidx = (indices >> 9) & 0xffff;
+		*cidx = (be64_to_cpu(indices) >> 25) & 0xffff;
+		*pidx = (be64_to_cpu(indices) >> 9) & 0xffff;
 	}
 	return ret;
 }
@@ -3634,10 +3640,10 @@ static int adap_init0_no_config(struct adapter *adapter, int reset)
 	 * field selections will fit in the 36-bit budget.
 	 */
 	if (tp_vlan_pri_map != TP_VLAN_PRI_MAP_DEFAULT) {
-		int i, bits = 0;
+		int j, bits = 0;
 
-		for (i = TP_VLAN_PRI_MAP_FIRST; i <= TP_VLAN_PRI_MAP_LAST; i++)
-			switch (tp_vlan_pri_map & (1 << i)) {
+		for (j = TP_VLAN_PRI_MAP_FIRST; j <= TP_VLAN_PRI_MAP_LAST; j++)
+			switch (tp_vlan_pri_map & (1 << j)) {
 			case 0:
 				/* compressed filter field not enabled */
 				break;
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
index 137a24438d9..32eec15fe4c 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
@@ -380,9 +380,11 @@ static int t4_mem_win_rw(struct adapter *adap, u32 addr, __be32 *data, int dir)
 	/* Collecting data 4 bytes at a time upto MEMWIN0_APERTURE */
 	for (i = 0; i < MEMWIN0_APERTURE; i = i+0x4) {
 		if (dir)
-			*data++ = t4_read_reg(adap, (MEMWIN0_BASE + i));
+			*data++ = (__force __be32) t4_read_reg(adap,
+							(MEMWIN0_BASE + i));
 		else
-			t4_write_reg(adap, (MEMWIN0_BASE + i), *data++);
+			t4_write_reg(adap, (MEMWIN0_BASE + i),
+				     (__force u32) *data++);
 	}
 
 	return 0;
@@ -417,7 +419,7 @@ static int t4_memory_rw(struct adapter *adap, int mtype, u32 addr, u32 len,
 	if ((addr & 0x3) || (len & 0x3))
 		return -EINVAL;
 
-	data = vmalloc(MEMWIN0_APERTURE/sizeof(__be32));
+	data = vmalloc(MEMWIN0_APERTURE);
 	if (!data)
 		return -ENOMEM;
 
@@ -744,7 +746,7 @@ static int t4_read_flash(struct adapter *adapter, unsigned int addr,
 		if (ret)
 			return ret;
 		if (byte_oriented)
-			*data = htonl(*data);
+			*data = (__force __u32) (htonl(*data));
 	}
 	return 0;
 }
@@ -992,7 +994,7 @@ int t4_load_fw(struct adapter *adap, const u8 *fw_data, unsigned int size)
 	int ret, addr;
 	unsigned int i;
 	u8 first_page[SF_PAGE_SIZE];
-	const u32 *p = (const u32 *)fw_data;
+	const __be32 *p = (const __be32 *)fw_data;
 	const struct fw_hdr *hdr = (const struct fw_hdr *)fw_data;
 	unsigned int sf_sec_size = adap->params.sf_size / adap->params.sf_nsec;
 	unsigned int fw_img_start = adap->params.sf_fw_start;
@@ -2315,7 +2317,8 @@ int t4_mem_win_read_len(struct adapter *adap, u32 addr, __be32 *data, int len)
 	t4_read_reg(adap, PCIE_MEM_ACCESS_OFFSET);
 
 	for (i = 0; i < len; i += 4)
-		*data++ = t4_read_reg(adap, (MEMWIN0_BASE + off + i));
+		*data++ = (__force __be32) t4_read_reg(adap,
+						(MEMWIN0_BASE + off + i));
 
 	return 0;
 }
diff --git a/drivers/net/ethernet/dec/tulip/dmfe.c b/drivers/net/ethernet/dec/tulip/dmfe.c
index 4d6fe604fa6..d23755ea9bc 100644
--- a/drivers/net/ethernet/dec/tulip/dmfe.c
+++ b/drivers/net/ethernet/dec/tulip/dmfe.c
@@ -446,13 +446,17 @@ static int __devinit dmfe_init_one (struct pci_dev *pdev,
 	/* Allocate Tx/Rx descriptor memory */
 	db->desc_pool_ptr = pci_alloc_consistent(pdev, sizeof(struct tx_desc) *
 			DESC_ALL_CNT + 0x20, &db->desc_pool_dma_ptr);
-	if (!db->desc_pool_ptr)
+	if (!db->desc_pool_ptr) {
+		err = -ENOMEM;
 		goto err_out_res;
+	}
 
 	db->buf_pool_ptr = pci_alloc_consistent(pdev, TX_BUF_ALLOC *
 			TX_DESC_CNT + 4, &db->buf_pool_dma_ptr);
-	if (!db->buf_pool_ptr)
+	if (!db->buf_pool_ptr) {
+		err = -ENOMEM;
 		goto err_out_free_desc;
+	}
 
 	db->first_tx_desc = (struct tx_desc *) db->desc_pool_ptr;
 	db->first_tx_desc_dma = db->desc_pool_dma_ptr;
@@ -462,8 +466,10 @@ static int __devinit dmfe_init_one (struct pci_dev *pdev,
 	db->chip_id = ent->driver_data;
 	/* IO type range. */
 	db->ioaddr = pci_iomap(pdev, 0, 0);
-	if (!db->ioaddr)
+	if (!db->ioaddr) {
+		err = -ENOMEM;
 		goto err_out_free_buf;
+	}
 
 	db->chip_revision = pdev->revision;
 	db->wol_mode = 0;
diff --git a/drivers/net/ethernet/emulex/benet/be_main.c b/drivers/net/ethernet/emulex/benet/be_main.c
index eb3f2cb3b93..d1b6cc58763 100644
--- a/drivers/net/ethernet/emulex/benet/be_main.c
+++ b/drivers/net/ethernet/emulex/benet/be_main.c
@@ -2129,8 +2129,11 @@ void be_detect_error(struct be_adapter *adapter)
 		ue_hi = (ue_hi & ~ue_hi_mask);
 	}
 
-	if (ue_lo || ue_hi ||
-		sliport_status & SLIPORT_STATUS_ERR_MASK) {
+	/* On certain platforms BE hardware can indicate spurious UEs.
+	 * Allow the h/w to stop working completely in case of a real UE.
+	 * Hence not setting the hw_error for UE detection.
+	 */
+	if (sliport_status & SLIPORT_STATUS_ERR_MASK) {
 		adapter->hw_error = true;
 		dev_err(&adapter->pdev->dev,
 			"Error detected in the card\n");
diff --git a/drivers/net/ethernet/freescale/gianfar.c b/drivers/net/ethernet/freescale/gianfar.c
index a1b52ec3b93..1d03dcdd5e5 100644
--- a/drivers/net/ethernet/freescale/gianfar.c
+++ b/drivers/net/ethernet/freescale/gianfar.c
@@ -1765,7 +1765,6 @@ static void free_skb_resources(struct gfar_private *priv)
 			  sizeof(struct rxbd8) * priv->total_rx_ring_size,
 			  priv->tx_queue[0]->tx_bd_base,
 			  priv->tx_queue[0]->tx_bd_dma_base);
-	skb_queue_purge(&priv->rx_recycle);
 }
 
 void gfar_start(struct net_device *dev)
@@ -1943,8 +1942,6 @@ static int gfar_enet_open(struct net_device *dev)
 
 	enable_napi(priv);
 
-	skb_queue_head_init(&priv->rx_recycle);
-
 	/* Initialize a bunch of registers */
 	init_registers(dev);
 
@@ -2533,16 +2530,7 @@ static int gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
 
 		bytes_sent += skb->len;
 
-		/* If there's room in the queue (limit it to rx_buffer_size)
-		 * we add this skb back into the pool, if it's the right size
-		 */
-		if (skb_queue_len(&priv->rx_recycle) < rx_queue->rx_ring_size &&
-		    skb_recycle_check(skb, priv->rx_buffer_size +
-				      RXBUF_ALIGNMENT)) {
-			gfar_align_skb(skb);
-			skb_queue_head(&priv->rx_recycle, skb);
-		} else
-			dev_kfree_skb_any(skb);
+		dev_kfree_skb_any(skb);
 
 		tx_queue->tx_skbuff[skb_dirtytx] = NULL;
 
@@ -2608,7 +2596,7 @@ static void gfar_new_rxbdp(struct gfar_priv_rx_q *rx_queue, struct rxbd8 *bdp,
 static struct sk_buff *gfar_alloc_skb(struct net_device *dev)
 {
 	struct gfar_private *priv = netdev_priv(dev);
-	struct sk_buff *skb = NULL;
+	struct sk_buff *skb;
 
 	skb = netdev_alloc_skb(dev, priv->rx_buffer_size + RXBUF_ALIGNMENT);
 	if (!skb)
@@ -2621,14 +2609,7 @@ static struct sk_buff *gfar_alloc_skb(struct net_device *dev)
 
 struct sk_buff *gfar_new_skb(struct net_device *dev)
 {
-	struct gfar_private *priv = netdev_priv(dev);
-	struct sk_buff *skb = NULL;
-
-	skb = skb_dequeue(&priv->rx_recycle);
-	if (!skb)
-		skb = gfar_alloc_skb(dev);
-
-	return skb;
+	return gfar_alloc_skb(dev);
 }
 
 static inline void count_errors(unsigned short status, struct net_device *dev)
@@ -2787,7 +2768,7 @@ int gfar_clean_rx_ring(struct gfar_priv_rx_q *rx_queue, int rx_work_limit)
 			if (unlikely(!newskb))
 				newskb = skb;
 			else if (skb)
-				skb_queue_head(&priv->rx_recycle, skb);
+				dev_kfree_skb(skb);
 		} else {
 			/* Increment the number of packets */
 			rx_queue->stats.rx_packets++;
diff --git a/drivers/net/ethernet/freescale/gianfar.h b/drivers/net/ethernet/freescale/gianfar.h
index 4141ef2ddaf..22eabc13ca9 100644
--- a/drivers/net/ethernet/freescale/gianfar.h
+++ b/drivers/net/ethernet/freescale/gianfar.h
@@ -1080,8 +1080,6 @@ struct gfar_private {
 
 	u32 cur_filer_idx;
 
-	struct sk_buff_head rx_recycle;
-
 	/* RX queue filer rule set*/
 	struct ethtool_rx_list rx_list;
 	struct mutex rx_queue_access;
diff --git a/drivers/net/ethernet/freescale/ucc_geth.c b/drivers/net/ethernet/freescale/ucc_geth.c
index 16428843922..0a70bb55d1b 100644
--- a/drivers/net/ethernet/freescale/ucc_geth.c
+++ b/drivers/net/ethernet/freescale/ucc_geth.c
@@ -209,14 +209,12 @@ static struct list_head *dequeue(struct list_head *lh)
 static struct sk_buff *get_new_skb(struct ucc_geth_private *ugeth,
 		u8 __iomem *bd)
 {
-	struct sk_buff *skb = NULL;
+	struct sk_buff *skb;
 
-	skb = __skb_dequeue(&ugeth->rx_recycle);
+	skb = netdev_alloc_skb(ugeth->ndev,
+			       ugeth->ug_info->uf_info.max_rx_buf_length +
+			       UCC_GETH_RX_DATA_BUF_ALIGNMENT);
 	if (!skb)
-		skb = netdev_alloc_skb(ugeth->ndev,
-				      ugeth->ug_info->uf_info.max_rx_buf_length +
-				      UCC_GETH_RX_DATA_BUF_ALIGNMENT);
-	if (skb == NULL)
 		return NULL;
 
 	/* We need the data buffer to be aligned properly.  We will reserve
@@ -2020,8 +2018,6 @@ static void ucc_geth_memclean(struct ucc_geth_private *ugeth)
 		iounmap(ugeth->ug_regs);
 		ugeth->ug_regs = NULL;
 	}
-
-	skb_queue_purge(&ugeth->rx_recycle);
 }
 
 static void ucc_geth_set_multi(struct net_device *dev)
@@ -2230,8 +2226,6 @@ static int ucc_struct_init(struct ucc_geth_private *ugeth)
 		return -ENOMEM;
 	}
 
-	skb_queue_head_init(&ugeth->rx_recycle);
-
 	return 0;
 }
 
@@ -3274,12 +3268,7 @@ static int ucc_geth_rx(struct ucc_geth_private *ugeth, u8 rxQ, int rx_work_limit
 			if (netif_msg_rx_err(ugeth))
 				ugeth_err("%s, %d: ERROR!!! skb - 0x%08x",
 					   __func__, __LINE__, (u32) skb);
-			if (skb) {
-				skb->data = skb->head + NET_SKB_PAD;
-				skb->len = 0;
-				skb_reset_tail_pointer(skb);
-				__skb_queue_head(&ugeth->rx_recycle, skb);
-			}
+			dev_kfree_skb(skb);
 
 			ugeth->rx_skbuff[rxQ][ugeth->skb_currx[rxQ]] = NULL;
 			dev->stats.rx_dropped++;
@@ -3349,13 +3338,7 @@ static int ucc_geth_tx(struct net_device *dev, u8 txQ)
 
 		dev->stats.tx_packets++;
 
-		if (skb_queue_len(&ugeth->rx_recycle) < RX_BD_RING_LEN &&
-			     skb_recycle_check(skb,
-				    ugeth->ug_info->uf_info.max_rx_buf_length +
-				    UCC_GETH_RX_DATA_BUF_ALIGNMENT))
-			__skb_queue_head(&ugeth->rx_recycle, skb);
-		else
-			dev_kfree_skb(skb);
+		dev_kfree_skb(skb);
 
 		ugeth->tx_skbuff[txQ][ugeth->skb_dirtytx[txQ]] = NULL;
 		ugeth->skb_dirtytx[txQ] =
diff --git a/drivers/net/ethernet/freescale/ucc_geth.h b/drivers/net/ethernet/freescale/ucc_geth.h
index f71b3e7b12d..75f337163ce 100644
--- a/drivers/net/ethernet/freescale/ucc_geth.h
+++ b/drivers/net/ethernet/freescale/ucc_geth.h
@@ -1214,8 +1214,6 @@ struct ucc_geth_private {
 	/* index of the first skb which hasn't been transmitted yet. */
 	u16 skb_dirtytx[NUM_TX_QUEUES];
 
-	struct sk_buff_head rx_recycle;
-
 	struct ugeth_mii_info *mii_info;
 	struct phy_device *phydev;
 	phy_interface_t phy_interface;
diff --git a/drivers/net/ethernet/intel/e1000e/e1000.h b/drivers/net/ethernet/intel/e1000e/e1000.h
index cb3356c9af8..04668b47a1d 100644
--- a/drivers/net/ethernet/intel/e1000e/e1000.h
+++ b/drivers/net/ethernet/intel/e1000e/e1000.h
@@ -175,13 +175,13 @@ struct e1000_info;
 /*
  * in the case of WTHRESH, it appears at least the 82571/2 hardware
  * writes back 4 descriptors when WTHRESH=5, and 3 descriptors when
- * WTHRESH=4, and since we want 64 bytes at a time written back, set
- * it to 5
+ * WTHRESH=4, so a setting of 5 gives the most efficient bus
+ * utilization but to avoid possible Tx stalls, set it to 1
  */
 #define E1000_TXDCTL_DMA_BURST_ENABLE                          \
 	(E1000_TXDCTL_GRAN | /* set descriptor granularity */  \
 	 E1000_TXDCTL_COUNT_DESC |                             \
-	 (5 << 16) | /* wthresh must be +1 more than desired */\
+	 (1 << 16) | /* wthresh must be +1 more than desired */\
 	 (1 << 8)  | /* hthresh */                             \
 	 0x1f)       /* pthresh */
 
diff --git a/drivers/net/ethernet/intel/e1000e/hw.h b/drivers/net/ethernet/intel/e1000e/hw.h
index ed5b40985ed..d37bfd96c98 100644
--- a/drivers/net/ethernet/intel/e1000e/hw.h
+++ b/drivers/net/ethernet/intel/e1000e/hw.h
@@ -412,6 +412,8 @@ enum e1e_registers {
 #define E1000_DEV_ID_PCH2_LV_V			0x1503
 #define E1000_DEV_ID_PCH_LPT_I217_LM		0x153A
 #define E1000_DEV_ID_PCH_LPT_I217_V		0x153B
+#define E1000_DEV_ID_PCH_LPTLP_I218_LM		0x155A
+#define E1000_DEV_ID_PCH_LPTLP_I218_V		0x1559
 
 #define E1000_REVISION_4 4
 
diff --git a/drivers/net/ethernet/intel/e1000e/netdev.c b/drivers/net/ethernet/intel/e1000e/netdev.c
index fb659dd8db0..f444eb0b76d 100644
--- a/drivers/net/ethernet/intel/e1000e/netdev.c
+++ b/drivers/net/ethernet/intel/e1000e/netdev.c
@@ -2831,7 +2831,7 @@ static void e1000_configure_tx(struct e1000_adapter *adapter)
 		 * set up some performance related parameters to encourage the
 		 * hardware to use the bus more efficiently in bursts, depends
 		 * on the tx_int_delay to be enabled,
-		 * wthresh = 5 ==> burst write a cacheline (64 bytes) at a time
+		 * wthresh = 1 ==> burst write is disabled to avoid Tx stalls
 		 * hthresh = 1 ==> prefetch when one or more available
 		 * pthresh = 0x1f ==> prefetch if internal cache 31 or less
 		 * BEWARE: this seems to work but should be considered first if
@@ -6558,6 +6558,8 @@ static DEFINE_PCI_DEVICE_TABLE(e1000_pci_tbl) = {
 
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_LPT_I217_LM), board_pch_lpt },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_LPT_I217_V), board_pch_lpt },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_LPTLP_I218_LM), board_pch_lpt },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_LPTLP_I218_V), board_pch_lpt },
 
 	{ 0, 0, 0, 0, 0, 0, 0 }	/* terminate list */
 };
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe.h b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
index 5bd26763554..30efc9f0f47 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
@@ -410,7 +410,7 @@ static inline u16 ixgbe_desc_unused(struct ixgbe_ring *ring)
 #define IXGBE_TX_CTXTDESC(R, i)	    \
 	(&(((struct ixgbe_adv_tx_context_desc *)((R)->desc))[i]))
 
-#define IXGBE_MAX_JUMBO_FRAME_SIZE        16128
+#define IXGBE_MAX_JUMBO_FRAME_SIZE	9728 /* Maximum Supported Size 9.5KB */
 #ifdef IXGBE_FCOE
 /* Use 3K as the baby jumbo frame size for FCoE */
 #define IXGBE_FCOE_JUMBO_FRAME_SIZE       3072
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h b/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
index 383b4e1cd17..4a9c9c28568 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
@@ -175,7 +175,7 @@ struct ixgbevf_q_vector {
 #define IXGBEVF_TX_CTXTDESC(R, i)	    \
 	(&(((struct ixgbe_adv_tx_context_desc *)((R)->desc))[i]))
 
-#define IXGBE_MAX_JUMBO_FRAME_SIZE        16128
+#define IXGBE_MAX_JUMBO_FRAME_SIZE	9728 /* Maximum Supported Size 9.5KB */
 
 #define OTHER_VECTOR 1
 #define NON_Q_VECTORS (OTHER_VECTOR)
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
index 0ee9bd4819f..de1ad506665 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
@@ -1747,6 +1747,7 @@ err_tx_ring_allocation:
  **/
 static int ixgbevf_set_interrupt_capability(struct ixgbevf_adapter *adapter)
 {
+	struct net_device *netdev = adapter->netdev;
 	int err = 0;
 	int vector, v_budget;
 
@@ -1775,6 +1776,12 @@ static int ixgbevf_set_interrupt_capability(struct ixgbevf_adapter *adapter)
 
 	ixgbevf_acquire_msix_vectors(adapter, v_budget);
 
+	err = netif_set_real_num_tx_queues(netdev, adapter->num_tx_queues);
+	if (err)
+		goto out;
+
+	err = netif_set_real_num_rx_queues(netdev, adapter->num_rx_queues);
+
 out:
 	return err;
 }
diff --git a/drivers/net/ethernet/jme.c b/drivers/net/ethernet/jme.c
index c911d883c27..f8064df10cc 100644
--- a/drivers/net/ethernet/jme.c
+++ b/drivers/net/ethernet/jme.c
@@ -27,6 +27,7 @@
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/pci.h>
+#include <linux/pci-aspm.h>
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/ethtool.h>
@@ -2973,6 +2974,9 @@ jme_init_one(struct pci_dev *pdev,
 	/*
 	 * set up PCI device basics
 	 */
+	pci_disable_link_state(pdev, PCIE_LINK_STATE_L0S | PCIE_LINK_STATE_L1 |
+			       PCIE_LINK_STATE_CLKPM);
+
 	rc = pci_enable_device(pdev);
 	if (rc) {
 		pr_err("Cannot enable PCI device\n");
diff --git a/drivers/net/ethernet/marvell/mv643xx_eth.c b/drivers/net/ethernet/marvell/mv643xx_eth.c
index 087b9e0669f..84c13263c51 100644
--- a/drivers/net/ethernet/marvell/mv643xx_eth.c
+++ b/drivers/net/ethernet/marvell/mv643xx_eth.c
@@ -412,7 +412,6 @@ struct mv643xx_eth_private {
 	u8 work_rx_refill;
 
 	int skb_size;
-	struct sk_buff_head rx_recycle;
 
 	/*
 	 * RX state.
@@ -673,9 +672,7 @@ static int rxq_refill(struct rx_queue *rxq, int budget)
 		struct rx_desc *rx_desc;
 		int size;
 
-		skb = __skb_dequeue(&mp->rx_recycle);
-		if (skb == NULL)
-			skb = netdev_alloc_skb(mp->dev, mp->skb_size);
+		skb = netdev_alloc_skb(mp->dev, mp->skb_size);
 
 		if (skb == NULL) {
 			mp->oom = 1;
@@ -989,14 +986,7 @@ static int txq_reclaim(struct tx_queue *txq, int budget, int force)
 				       desc->byte_cnt, DMA_TO_DEVICE);
 		}
 
-		if (skb != NULL) {
-			if (skb_queue_len(&mp->rx_recycle) <
-					mp->rx_ring_size &&
-			    skb_recycle_check(skb, mp->skb_size))
-				__skb_queue_head(&mp->rx_recycle, skb);
-			else
-				dev_kfree_skb(skb);
-		}
+		dev_kfree_skb(skb);
 	}
 
 	__netif_tx_unlock(nq);
@@ -2349,8 +2339,6 @@ static int mv643xx_eth_open(struct net_device *dev)
 
 	napi_enable(&mp->napi);
 
-	skb_queue_head_init(&mp->rx_recycle);
-
 	mp->int_mask = INT_EXT;
 
 	for (i = 0; i < mp->rxq_count; i++) {
@@ -2445,8 +2433,6 @@ static int mv643xx_eth_stop(struct net_device *dev)
 	mib_counters_update(mp);
 	del_timer_sync(&mp->mib_counters_timer);
 
-	skb_queue_purge(&mp->rx_recycle);
-
 	for (i = 0; i < mp->rxq_count; i++)
 		rxq_deinit(mp->rxq + i);
 	for (i = 0; i < mp->txq_count; i++)
diff --git a/drivers/net/ethernet/marvell/skge.c b/drivers/net/ethernet/marvell/skge.c
index 5a30bf82309..9b9c2ac5c4c 100644
--- a/drivers/net/ethernet/marvell/skge.c
+++ b/drivers/net/ethernet/marvell/skge.c
@@ -3189,7 +3189,7 @@ static int skge_poll(struct napi_struct *napi, int to_do)
 	if (work_done < to_do) {
 		unsigned long flags;
 
-		napi_gro_flush(napi);
+		napi_gro_flush(napi, false);
 		spin_lock_irqsave(&hw->hw_lock, flags);
 		__napi_complete(napi);
 		hw->intr_mask |= napimask[skge->port];
@@ -3945,8 +3945,10 @@ static int __devinit skge_probe(struct pci_dev *pdev,
 		skge_board_name(hw), hw->chip_rev);
 
 	dev = skge_devinit(hw, 0, using_dac);
-	if (!dev)
+	if (!dev) {
+		err = -ENOMEM;
 		goto err_out_led_off;
+	}
 
 	/* Some motherboards are broken and has zero in ROM. */
 	if (!is_valid_ether_addr(dev->dev_addr))
@@ -4153,6 +4155,13 @@ static struct dmi_system_id skge_32bit_dma_boards[] = {
 			DMI_MATCH(DMI_BOARD_NAME, "nForce"),
 		},
 	},
+	{
+		.ident = "ASUS P5NSLI",
+		.matches = {
+			DMI_MATCH(DMI_BOARD_VENDOR, "ASUSTeK Computer INC."),
+			DMI_MATCH(DMI_BOARD_NAME, "P5NSLI")
+		},
+	},
 	{}
 };
 
diff --git a/drivers/net/ethernet/marvell/sky2.c b/drivers/net/ethernet/marvell/sky2.c
index 2b0748dba8b..78946feab4a 100644
--- a/drivers/net/ethernet/marvell/sky2.c
+++ b/drivers/net/ethernet/marvell/sky2.c
@@ -4924,6 +4924,7 @@ static int __devinit sky2_probe(struct pci_dev *pdev,
 
 	if (~reg == 0) {
 		dev_err(&pdev->dev, "PCI configuration read error\n");
+		err = -EIO;
 		goto err_out;
 	}
 
@@ -4993,8 +4994,10 @@ static int __devinit sky2_probe(struct pci_dev *pdev,
 	hw->st_size = hw->ports * roundup_pow_of_two(3*RX_MAX_PENDING + TX_MAX_PENDING);
 	hw->st_le = pci_alloc_consistent(pdev, hw->st_size * sizeof(struct sky2_status_le),
 					 &hw->st_dma);
-	if (!hw->st_le)
+	if (!hw->st_le) {
+		err = -ENOMEM;
 		goto err_out_reset;
+	}
 
 	dev_info(&pdev->dev, "Yukon-2 %s chip revision %d\n",
 		 sky2_name(hw->chip_id, buf1, sizeof(buf1)), hw->chip_rev);
diff --git a/drivers/net/ethernet/natsemi/natsemi.c b/drivers/net/ethernet/natsemi/natsemi.c
index 5b61d12f8b9..dbaaa99a0d4 100644
--- a/drivers/net/ethernet/natsemi/natsemi.c
+++ b/drivers/net/ethernet/natsemi/natsemi.c
@@ -947,8 +947,8 @@ static int __devinit natsemi_probe1 (struct pci_dev *pdev,
 	i = register_netdev(dev);
 	if (i)
 		goto err_register_netdev;
-
-	if (NATSEMI_CREATE_FILE(pdev, dspcfg_workaround))
+	i = NATSEMI_CREATE_FILE(pdev, dspcfg_workaround);
+	if (i)
 		goto err_create_file;
 
 	if (netif_msg_drv(np)) {
diff --git a/drivers/net/ethernet/natsemi/xtsonic.c b/drivers/net/ethernet/natsemi/xtsonic.c
index e01c0a07a93..7dfe88398d7 100644
--- a/drivers/net/ethernet/natsemi/xtsonic.c
+++ b/drivers/net/ethernet/natsemi/xtsonic.c
@@ -205,6 +205,7 @@ static int __init sonic_probe1(struct net_device *dev)
 	if (lp->descriptors == NULL) {
 		printk(KERN_ERR "%s: couldn't alloc DMA memory for "
 				" descriptors.\n", dev_name(lp->device));
+		err = -ENOMEM;
 		goto out;
 	}
 
diff --git a/drivers/net/ethernet/octeon/octeon_mgmt.c b/drivers/net/ethernet/octeon/octeon_mgmt.c
index a688a2ddcfd..f97719c4851 100644
--- a/drivers/net/ethernet/octeon/octeon_mgmt.c
+++ b/drivers/net/ethernet/octeon/octeon_mgmt.c
@@ -3,13 +3,14 @@
  * License.  See the file "COPYING" in the main directory of this archive
  * for more details.
  *
- * Copyright (C) 2009 Cavium Networks
+ * Copyright (C) 2009-2012 Cavium, Inc
  */
 
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
 #include <linux/etherdevice.h>
 #include <linux/capability.h>
+#include <linux/net_tstamp.h>
 #include <linux/interrupt.h>
 #include <linux/netdevice.h>
 #include <linux/spinlock.h>
@@ -33,8 +34,7 @@
 
 #define OCTEON_MGMT_NAPI_WEIGHT 16
 
-/*
- * Ring sizes that are powers of two allow for more efficient modulo
+/* Ring sizes that are powers of two allow for more efficient modulo
  * opertions.
  */
 #define OCTEON_MGMT_RX_RING_SIZE 512
@@ -93,6 +93,7 @@ union mgmt_port_ring_entry {
 #define AGL_GMX_RX_ADR_CAM4		0x1a0
 #define AGL_GMX_RX_ADR_CAM5		0x1a8
 
+#define AGL_GMX_TX_CLK			0x208
 #define AGL_GMX_TX_STATS_CTL		0x268
 #define AGL_GMX_TX_CTL			0x270
 #define AGL_GMX_TX_STAT0		0x280
@@ -110,8 +111,10 @@ struct octeon_mgmt {
 	struct net_device *netdev;
 	u64 mix;
 	u64 agl;
+	u64 agl_prt_ctl;
 	int port;
 	int irq;
+	bool has_rx_tstamp;
 	u64 *tx_ring;
 	dma_addr_t tx_ring_handle;
 	unsigned int tx_next;
@@ -131,6 +134,7 @@ struct octeon_mgmt {
 	spinlock_t lock;
 	unsigned int last_duplex;
 	unsigned int last_link;
+	unsigned int last_speed;
 	struct device *dev;
 	struct napi_struct napi;
 	struct tasklet_struct tx_clean_tasklet;
@@ -140,6 +144,8 @@ struct octeon_mgmt {
 	resource_size_t mix_size;
 	resource_size_t agl_phys;
 	resource_size_t agl_size;
+	resource_size_t agl_prt_ctl_phys;
+	resource_size_t agl_prt_ctl_size;
 };
 
 static void octeon_mgmt_set_rx_irq(struct octeon_mgmt *p, int enable)
@@ -166,22 +172,22 @@ static void octeon_mgmt_set_tx_irq(struct octeon_mgmt *p, int enable)
 	spin_unlock_irqrestore(&p->lock, flags);
 }
 
-static inline void octeon_mgmt_enable_rx_irq(struct octeon_mgmt *p)
+static void octeon_mgmt_enable_rx_irq(struct octeon_mgmt *p)
 {
 	octeon_mgmt_set_rx_irq(p, 1);
 }
 
-static inline void octeon_mgmt_disable_rx_irq(struct octeon_mgmt *p)
+static void octeon_mgmt_disable_rx_irq(struct octeon_mgmt *p)
 {
 	octeon_mgmt_set_rx_irq(p, 0);
 }
 
-static inline void octeon_mgmt_enable_tx_irq(struct octeon_mgmt *p)
+static void octeon_mgmt_enable_tx_irq(struct octeon_mgmt *p)
 {
 	octeon_mgmt_set_tx_irq(p, 1);
 }
 
-static inline void octeon_mgmt_disable_tx_irq(struct octeon_mgmt *p)
+static void octeon_mgmt_disable_tx_irq(struct octeon_mgmt *p)
 {
 	octeon_mgmt_set_tx_irq(p, 0);
 }
@@ -233,6 +239,28 @@ static void octeon_mgmt_rx_fill_ring(struct net_device *netdev)
 	}
 }
 
+static ktime_t ptp_to_ktime(u64 ptptime)
+{
+	ktime_t ktimebase;
+	u64 ptpbase;
+	unsigned long flags;
+
+	local_irq_save(flags);
+	/* Fill the icache with the code */
+	ktime_get_real();
+	/* Flush all pending operations */
+	mb();
+	/* Read the time and PTP clock as close together as
+	 * possible. It is important that this sequence take the same
+	 * amount of time to reduce jitter
+	 */
+	ktimebase = ktime_get_real();
+	ptpbase = cvmx_read_csr(CVMX_MIO_PTP_CLOCK_HI);
+	local_irq_restore(flags);
+
+	return ktime_sub_ns(ktimebase, ptpbase - ptptime);
+}
+
 static void octeon_mgmt_clean_tx_buffers(struct octeon_mgmt *p)
 {
 	union cvmx_mixx_orcnt mix_orcnt;
@@ -272,6 +300,20 @@ static void octeon_mgmt_clean_tx_buffers(struct octeon_mgmt *p)
 
 		dma_unmap_single(p->dev, re.s.addr, re.s.len,
 				 DMA_TO_DEVICE);
+
+		/* Read the hardware TX timestamp if one was recorded */
+		if (unlikely(re.s.tstamp)) {
+			struct skb_shared_hwtstamps ts;
+			/* Read the timestamp */
+			u64 ns = cvmx_read_csr(CVMX_MIXX_TSTAMP(p->port));
+			/* Remove the timestamp from the FIFO */
+			cvmx_write_csr(CVMX_MIXX_TSCTL(p->port), 0);
+			/* Tell the kernel about the timestamp */
+			ts.syststamp = ptp_to_ktime(ns);
+			ts.hwtstamp = ns_to_ktime(ns);
+			skb_tstamp_tx(skb, &ts);
+		}
+
 		dev_kfree_skb_any(skb);
 		cleaned++;
 
@@ -372,14 +414,23 @@ static int octeon_mgmt_receive_one(struct octeon_mgmt *p)
 		/* A good packet, send it up. */
 		skb_put(skb, re.s.len);
 good:
+		/* Process the RX timestamp if it was recorded */
+		if (p->has_rx_tstamp) {
+			/* The first 8 bytes are the timestamp */
+			u64 ns = *(u64 *)skb->data;
+			struct skb_shared_hwtstamps *ts;
+			ts = skb_hwtstamps(skb);
+			ts->hwtstamp = ns_to_ktime(ns);
+			ts->syststamp = ptp_to_ktime(ns);
+			__skb_pull(skb, 8);
+		}
 		skb->protocol = eth_type_trans(skb, netdev);
 		netdev->stats.rx_packets++;
 		netdev->stats.rx_bytes += skb->len;
 		netif_receive_skb(skb);
 		rc = 0;
 	} else if (re.s.code == RING_ENTRY_CODE_MORE) {
-		/*
-		 * Packet split across skbs.  This can happen if we
+		/* Packet split across skbs.  This can happen if we
 		 * increase the MTU.  Buffers that are already in the
 		 * rx ring can then end up being too small.  As the rx
 		 * ring is refilled, buffers sized for the new MTU
@@ -409,8 +460,7 @@ good:
 	} else {
 		/* Some other error, discard it. */
 		dev_kfree_skb_any(skb);
-		/*
-		 * Error statistics are accumulated in
+		/* Error statistics are accumulated in
 		 * octeon_mgmt_update_rx_stats.
 		 */
 	}
@@ -488,7 +538,7 @@ static void octeon_mgmt_reset_hw(struct octeon_mgmt *p)
 	mix_ctl.s.reset = 1;
 	cvmx_write_csr(p->mix + MIX_CTL, mix_ctl.u64);
 	cvmx_read_csr(p->mix + MIX_CTL);
-	cvmx_wait(64);
+	octeon_io_clk_delay(64);
 
 	mix_bist.u64 = cvmx_read_csr(p->mix + MIX_BIST);
 	if (mix_bist.u64)
@@ -537,8 +587,7 @@ static void octeon_mgmt_set_rx_filtering(struct net_device *netdev)
 		cam_mode = 0;
 		available_cam_entries = 8;
 	} else {
-		/*
-		 * One CAM entry for the primary address, leaves seven
+		/* One CAM entry for the primary address, leaves seven
 		 * for the secondary addresses.
 		 */
 		available_cam_entries = 7 - netdev->uc.count;
@@ -595,12 +644,10 @@ static void octeon_mgmt_set_rx_filtering(struct net_device *netdev)
 
 static int octeon_mgmt_set_mac_address(struct net_device *netdev, void *addr)
 {
-	struct sockaddr *sa = addr;
+	int r = eth_mac_addr(netdev, addr);
 
-	if (!is_valid_ether_addr(sa->sa_data))
-		return -EADDRNOTAVAIL;
-
-	memcpy(netdev->dev_addr, sa->sa_data, ETH_ALEN);
+	if (r)
+		return r;
 
 	octeon_mgmt_set_rx_filtering(netdev);
 
@@ -612,8 +659,7 @@ static int octeon_mgmt_change_mtu(struct net_device *netdev, int new_mtu)
 	struct octeon_mgmt *p = netdev_priv(netdev);
 	int size_without_fcs = new_mtu + OCTEON_MGMT_RX_HEADROOM;
 
-	/*
-	 * Limit the MTU to make sure the ethernet packets are between
+	/* Limit the MTU to make sure the ethernet packets are between
 	 * 64 bytes and 16383 bytes.
 	 */
 	if (size_without_fcs < 64 || size_without_fcs > 16383) {
@@ -656,53 +702,258 @@ static irqreturn_t octeon_mgmt_interrupt(int cpl, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+static int octeon_mgmt_ioctl_hwtstamp(struct net_device *netdev,
+				      struct ifreq *rq, int cmd)
+{
+	struct octeon_mgmt *p = netdev_priv(netdev);
+	struct hwtstamp_config config;
+	union cvmx_mio_ptp_clock_cfg ptp;
+	union cvmx_agl_gmx_rxx_frm_ctl rxx_frm_ctl;
+	bool have_hw_timestamps = false;
+
+	if (copy_from_user(&config, rq->ifr_data, sizeof(config)))
+		return -EFAULT;
+
+	if (config.flags) /* reserved for future extensions */
+		return -EINVAL;
+
+	/* Check the status of hardware for tiemstamps */
+	if (OCTEON_IS_MODEL(OCTEON_CN6XXX)) {
+		/* Get the current state of the PTP clock */
+		ptp.u64 = cvmx_read_csr(CVMX_MIO_PTP_CLOCK_CFG);
+		if (!ptp.s.ext_clk_en) {
+			/* The clock has not been configured to use an
+			 * external source.  Program it to use the main clock
+			 * reference.
+			 */
+			u64 clock_comp = (NSEC_PER_SEC << 32) /	octeon_get_io_clock_rate();
+			if (!ptp.s.ptp_en)
+				cvmx_write_csr(CVMX_MIO_PTP_CLOCK_COMP, clock_comp);
+			pr_info("PTP Clock: Using sclk reference at %lld Hz\n",
+				(NSEC_PER_SEC << 32) / clock_comp);
+		} else {
+			/* The clock is already programmed to use a GPIO */
+			u64 clock_comp = cvmx_read_csr(CVMX_MIO_PTP_CLOCK_COMP);
+			pr_info("PTP Clock: Using GPIO %d at %lld Hz\n",
+				ptp.s.ext_clk_in,
+				(NSEC_PER_SEC << 32) / clock_comp);
+		}
+
+		/* Enable the clock if it wasn't done already */
+		if (!ptp.s.ptp_en) {
+			ptp.s.ptp_en = 1;
+			cvmx_write_csr(CVMX_MIO_PTP_CLOCK_CFG, ptp.u64);
+		}
+		have_hw_timestamps = true;
+	}
+
+	if (!have_hw_timestamps)
+		return -EINVAL;
+
+	switch (config.tx_type) {
+	case HWTSTAMP_TX_OFF:
+	case HWTSTAMP_TX_ON:
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	switch (config.rx_filter) {
+	case HWTSTAMP_FILTER_NONE:
+		p->has_rx_tstamp = false;
+		rxx_frm_ctl.u64 = cvmx_read_csr(p->agl + AGL_GMX_RX_FRM_CTL);
+		rxx_frm_ctl.s.ptp_mode = 0;
+		cvmx_write_csr(p->agl + AGL_GMX_RX_FRM_CTL, rxx_frm_ctl.u64);
+		break;
+	case HWTSTAMP_FILTER_ALL:
+	case HWTSTAMP_FILTER_SOME:
+	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
+	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
+	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+		p->has_rx_tstamp = have_hw_timestamps;
+		config.rx_filter = HWTSTAMP_FILTER_ALL;
+		if (p->has_rx_tstamp) {
+			rxx_frm_ctl.u64 = cvmx_read_csr(p->agl + AGL_GMX_RX_FRM_CTL);
+			rxx_frm_ctl.s.ptp_mode = 1;
+			cvmx_write_csr(p->agl + AGL_GMX_RX_FRM_CTL, rxx_frm_ctl.u64);
+		}
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	if (copy_to_user(rq->ifr_data, &config, sizeof(config)))
+		return -EFAULT;
+
+	return 0;
+}
+
 static int octeon_mgmt_ioctl(struct net_device *netdev,
 			     struct ifreq *rq, int cmd)
 {
 	struct octeon_mgmt *p = netdev_priv(netdev);
 
-	if (!netif_running(netdev))
+	switch (cmd) {
+	case SIOCSHWTSTAMP:
+		return octeon_mgmt_ioctl_hwtstamp(netdev, rq, cmd);
+	default:
+		if (p->phydev)
+			return phy_mii_ioctl(p->phydev, rq, cmd);
 		return -EINVAL;
+	}
+}
 
-	if (!p->phydev)
-		return -EINVAL;
+static void octeon_mgmt_disable_link(struct octeon_mgmt *p)
+{
+	union cvmx_agl_gmx_prtx_cfg prtx_cfg;
 
-	return phy_mii_ioctl(p->phydev, rq, cmd);
+	/* Disable GMX before we make any changes. */
+	prtx_cfg.u64 = cvmx_read_csr(p->agl + AGL_GMX_PRT_CFG);
+	prtx_cfg.s.en = 0;
+	prtx_cfg.s.tx_en = 0;
+	prtx_cfg.s.rx_en = 0;
+	cvmx_write_csr(p->agl + AGL_GMX_PRT_CFG, prtx_cfg.u64);
+
+	if (OCTEON_IS_MODEL(OCTEON_CN6XXX)) {
+		int i;
+		for (i = 0; i < 10; i++) {
+			prtx_cfg.u64 = cvmx_read_csr(p->agl + AGL_GMX_PRT_CFG);
+			if (prtx_cfg.s.tx_idle == 1 || prtx_cfg.s.rx_idle == 1)
+				break;
+			mdelay(1);
+			i++;
+		}
+	}
+}
+
+static void octeon_mgmt_enable_link(struct octeon_mgmt *p)
+{
+	union cvmx_agl_gmx_prtx_cfg prtx_cfg;
+
+	/* Restore the GMX enable state only if link is set */
+	prtx_cfg.u64 = cvmx_read_csr(p->agl + AGL_GMX_PRT_CFG);
+	prtx_cfg.s.tx_en = 1;
+	prtx_cfg.s.rx_en = 1;
+	prtx_cfg.s.en = 1;
+	cvmx_write_csr(p->agl + AGL_GMX_PRT_CFG, prtx_cfg.u64);
+}
+
+static void octeon_mgmt_update_link(struct octeon_mgmt *p)
+{
+	union cvmx_agl_gmx_prtx_cfg prtx_cfg;
+
+	prtx_cfg.u64 = cvmx_read_csr(p->agl + AGL_GMX_PRT_CFG);
+
+	if (!p->phydev->link)
+		prtx_cfg.s.duplex = 1;
+	else
+		prtx_cfg.s.duplex = p->phydev->duplex;
+
+	switch (p->phydev->speed) {
+	case 10:
+		prtx_cfg.s.speed = 0;
+		prtx_cfg.s.slottime = 0;
+
+		if (OCTEON_IS_MODEL(OCTEON_CN6XXX)) {
+			prtx_cfg.s.burst = 1;
+			prtx_cfg.s.speed_msb = 1;
+		}
+		break;
+	case 100:
+		prtx_cfg.s.speed = 0;
+		prtx_cfg.s.slottime = 0;
+
+		if (OCTEON_IS_MODEL(OCTEON_CN6XXX)) {
+			prtx_cfg.s.burst = 1;
+			prtx_cfg.s.speed_msb = 0;
+		}
+		break;
+	case 1000:
+		/* 1000 MBits is only supported on 6XXX chips */
+		if (OCTEON_IS_MODEL(OCTEON_CN6XXX)) {
+			prtx_cfg.s.speed = 1;
+			prtx_cfg.s.speed_msb = 0;
+			/* Only matters for half-duplex */
+			prtx_cfg.s.slottime = 1;
+			prtx_cfg.s.burst = p->phydev->duplex;
+		}
+		break;
+	case 0:  /* No link */
+	default:
+		break;
+	}
+
+	/* Write the new GMX setting with the port still disabled. */
+	cvmx_write_csr(p->agl + AGL_GMX_PRT_CFG, prtx_cfg.u64);
+
+	/* Read GMX CFG again to make sure the config is completed. */
+	prtx_cfg.u64 = cvmx_read_csr(p->agl + AGL_GMX_PRT_CFG);
+
+	if (OCTEON_IS_MODEL(OCTEON_CN6XXX)) {
+		union cvmx_agl_gmx_txx_clk agl_clk;
+		union cvmx_agl_prtx_ctl prtx_ctl;
+
+		prtx_ctl.u64 = cvmx_read_csr(p->agl_prt_ctl);
+		agl_clk.u64 = cvmx_read_csr(p->agl + AGL_GMX_TX_CLK);
+		/* MII (both speeds) and RGMII 1000 speed. */
+		agl_clk.s.clk_cnt = 1;
+		if (prtx_ctl.s.mode == 0) { /* RGMII mode */
+			if (p->phydev->speed == 10)
+				agl_clk.s.clk_cnt = 50;
+			else if (p->phydev->speed == 100)
+				agl_clk.s.clk_cnt = 5;
+		}
+		cvmx_write_csr(p->agl + AGL_GMX_TX_CLK, agl_clk.u64);
+	}
 }
 
 static void octeon_mgmt_adjust_link(struct net_device *netdev)
 {
 	struct octeon_mgmt *p = netdev_priv(netdev);
-	union cvmx_agl_gmx_prtx_cfg prtx_cfg;
 	unsigned long flags;
 	int link_changed = 0;
 
+	if (!p->phydev)
+		return;
+
 	spin_lock_irqsave(&p->lock, flags);
-	if (p->phydev->link) {
-		if (!p->last_link)
-			link_changed = 1;
-		if (p->last_duplex != p->phydev->duplex) {
-			p->last_duplex = p->phydev->duplex;
-			prtx_cfg.u64 = cvmx_read_csr(p->agl + AGL_GMX_PRT_CFG);
-			prtx_cfg.s.duplex = p->phydev->duplex;
-			cvmx_write_csr(p->agl + AGL_GMX_PRT_CFG, prtx_cfg.u64);
-		}
-	} else {
-		if (p->last_link)
-			link_changed = -1;
+
+
+	if (!p->phydev->link && p->last_link)
+		link_changed = -1;
+
+	if (p->phydev->link
+	    && (p->last_duplex != p->phydev->duplex
+		|| p->last_link != p->phydev->link
+		|| p->last_speed != p->phydev->speed)) {
+		octeon_mgmt_disable_link(p);
+		link_changed = 1;
+		octeon_mgmt_update_link(p);
+		octeon_mgmt_enable_link(p);
 	}
+
 	p->last_link = p->phydev->link;
+	p->last_speed = p->phydev->speed;
+	p->last_duplex = p->phydev->duplex;
+
 	spin_unlock_irqrestore(&p->lock, flags);
 
 	if (link_changed != 0) {
 		if (link_changed > 0) {
-			netif_carrier_on(netdev);
 			pr_info("%s: Link is up - %d/%s\n", netdev->name,
 				p->phydev->speed,
 				DUPLEX_FULL == p->phydev->duplex ?
 				"Full" : "Half");
 		} else {
-			netif_carrier_off(netdev);
 			pr_info("%s: Link is down\n", netdev->name);
 		}
 	}
@@ -723,9 +974,7 @@ static int octeon_mgmt_init_phy(struct net_device *netdev)
 				   PHY_INTERFACE_MODE_MII);
 
 	if (!p->phydev)
-		return -1;
-
-	phy_start_aneg(p->phydev);
+		return -ENODEV;
 
 	return 0;
 }
@@ -733,12 +982,10 @@ static int octeon_mgmt_init_phy(struct net_device *netdev)
 static int octeon_mgmt_open(struct net_device *netdev)
 {
 	struct octeon_mgmt *p = netdev_priv(netdev);
-	int port = p->port;
 	union cvmx_mixx_ctl mix_ctl;
 	union cvmx_agl_gmx_inf_mode agl_gmx_inf_mode;
 	union cvmx_mixx_oring1 oring1;
 	union cvmx_mixx_iring1 iring1;
-	union cvmx_agl_gmx_prtx_cfg prtx_cfg;
 	union cvmx_agl_gmx_rxx_frm_ctl rxx_frm_ctl;
 	union cvmx_mixx_irhwm mix_irhwm;
 	union cvmx_mixx_orhwm mix_orhwm;
@@ -785,9 +1032,30 @@ static int octeon_mgmt_open(struct net_device *netdev)
 		} while (mix_ctl.s.reset);
 	}
 
-	agl_gmx_inf_mode.u64 = 0;
-	agl_gmx_inf_mode.s.en = 1;
-	cvmx_write_csr(CVMX_AGL_GMX_INF_MODE, agl_gmx_inf_mode.u64);
+	if (OCTEON_IS_MODEL(OCTEON_CN5XXX)) {
+		agl_gmx_inf_mode.u64 = 0;
+		agl_gmx_inf_mode.s.en = 1;
+		cvmx_write_csr(CVMX_AGL_GMX_INF_MODE, agl_gmx_inf_mode.u64);
+	}
+	if (OCTEON_IS_MODEL(OCTEON_CN56XX_PASS1_X)
+		|| OCTEON_IS_MODEL(OCTEON_CN52XX_PASS1_X)) {
+		/* Force compensation values, as they are not
+		 * determined properly by HW
+		 */
+		union cvmx_agl_gmx_drv_ctl drv_ctl;
+
+		drv_ctl.u64 = cvmx_read_csr(CVMX_AGL_GMX_DRV_CTL);
+		if (p->port) {
+			drv_ctl.s.byp_en1 = 1;
+			drv_ctl.s.nctl1 = 6;
+			drv_ctl.s.pctl1 = 6;
+		} else {
+			drv_ctl.s.byp_en = 1;
+			drv_ctl.s.nctl = 6;
+			drv_ctl.s.pctl = 6;
+		}
+		cvmx_write_csr(CVMX_AGL_GMX_DRV_CTL, drv_ctl.u64);
+	}
 
 	oring1.u64 = 0;
 	oring1.s.obase = p->tx_ring_handle >> 3;
@@ -799,18 +1067,12 @@ static int octeon_mgmt_open(struct net_device *netdev)
 	iring1.s.isize = OCTEON_MGMT_RX_RING_SIZE;
 	cvmx_write_csr(p->mix + MIX_IRING1, iring1.u64);
 
-	/* Disable packet I/O. */
-	prtx_cfg.u64 = cvmx_read_csr(p->agl + AGL_GMX_PRT_CFG);
-	prtx_cfg.s.en = 0;
-	cvmx_write_csr(p->agl + AGL_GMX_PRT_CFG, prtx_cfg.u64);
-
 	memcpy(sa.sa_data, netdev->dev_addr, ETH_ALEN);
 	octeon_mgmt_set_mac_address(netdev, &sa);
 
 	octeon_mgmt_change_mtu(netdev, netdev->mtu);
 
-	/*
-	 * Enable the port HW. Packets are not allowed until
+	/* Enable the port HW. Packets are not allowed until
 	 * cvmx_mgmt_port_enable() is called.
 	 */
 	mix_ctl.u64 = 0;
@@ -819,27 +1081,70 @@ static int octeon_mgmt_open(struct net_device *netdev)
 	mix_ctl.s.nbtarb = 0;       /* Arbitration mode */
 	/* MII CB-request FIFO programmable high watermark */
 	mix_ctl.s.mrq_hwm = 1;
+#ifdef __LITTLE_ENDIAN
+	mix_ctl.s.lendian = 1;
+#endif
 	cvmx_write_csr(p->mix + MIX_CTL, mix_ctl.u64);
 
-	if (OCTEON_IS_MODEL(OCTEON_CN56XX_PASS1_X)
-	    || OCTEON_IS_MODEL(OCTEON_CN52XX_PASS1_X)) {
-		/*
-		 * Force compensation values, as they are not
-		 * determined properly by HW
-		 */
-		union cvmx_agl_gmx_drv_ctl drv_ctl;
+	/* Read the PHY to find the mode of the interface. */
+	if (octeon_mgmt_init_phy(netdev)) {
+		dev_err(p->dev, "Cannot initialize PHY on MIX%d.\n", p->port);
+		goto err_noirq;
+	}
 
-		drv_ctl.u64 = cvmx_read_csr(CVMX_AGL_GMX_DRV_CTL);
-		if (port) {
-			drv_ctl.s.byp_en1 = 1;
-			drv_ctl.s.nctl1 = 6;
-			drv_ctl.s.pctl1 = 6;
-		} else {
-			drv_ctl.s.byp_en = 1;
-			drv_ctl.s.nctl = 6;
-			drv_ctl.s.pctl = 6;
+	/* Set the mode of the interface, RGMII/MII. */
+	if (OCTEON_IS_MODEL(OCTEON_CN6XXX) && p->phydev) {
+		union cvmx_agl_prtx_ctl agl_prtx_ctl;
+		int rgmii_mode = (p->phydev->supported &
+				  (SUPPORTED_1000baseT_Half | SUPPORTED_1000baseT_Full)) != 0;
+
+		agl_prtx_ctl.u64 = cvmx_read_csr(p->agl_prt_ctl);
+		agl_prtx_ctl.s.mode = rgmii_mode ? 0 : 1;
+		cvmx_write_csr(p->agl_prt_ctl,	agl_prtx_ctl.u64);
+
+		/* MII clocks counts are based on the 125Mhz
+		 * reference, which has an 8nS period. So our delays
+		 * need to be multiplied by this factor.
+		 */
+#define NS_PER_PHY_CLK 8
+
+		/* Take the DLL and clock tree out of reset */
+		agl_prtx_ctl.u64 = cvmx_read_csr(p->agl_prt_ctl);
+		agl_prtx_ctl.s.clkrst = 0;
+		if (rgmii_mode) {
+			agl_prtx_ctl.s.dllrst = 0;
+			agl_prtx_ctl.s.clktx_byp = 0;
 		}
-		cvmx_write_csr(CVMX_AGL_GMX_DRV_CTL, drv_ctl.u64);
+		cvmx_write_csr(p->agl_prt_ctl,	agl_prtx_ctl.u64);
+		cvmx_read_csr(p->agl_prt_ctl); /* Force write out before wait */
+
+		/* Wait for the DLL to lock. External 125 MHz
+		 * reference clock must be stable at this point.
+		 */
+		ndelay(256 * NS_PER_PHY_CLK);
+
+		/* Enable the interface */
+		agl_prtx_ctl.u64 = cvmx_read_csr(p->agl_prt_ctl);
+		agl_prtx_ctl.s.enable = 1;
+		cvmx_write_csr(p->agl_prt_ctl, agl_prtx_ctl.u64);
+
+		/* Read the value back to force the previous write */
+		agl_prtx_ctl.u64 = cvmx_read_csr(p->agl_prt_ctl);
+
+		/* Enable the compensation controller */
+		agl_prtx_ctl.s.comp = 1;
+		agl_prtx_ctl.s.drv_byp = 0;
+		cvmx_write_csr(p->agl_prt_ctl,	agl_prtx_ctl.u64);
+		/* Force write out before wait. */
+		cvmx_read_csr(p->agl_prt_ctl);
+
+		/* For compensation state to lock. */
+		ndelay(1040 * NS_PER_PHY_CLK);
+
+		/* Some Ethernet switches cannot handle standard
+		 * Interframe Gap, increase to 16 bytes.
+		 */
+		cvmx_write_csr(CVMX_AGL_GMX_TX_IFG, 0x88);
 	}
 
 	octeon_mgmt_rx_fill_ring(netdev);
@@ -870,7 +1175,7 @@ static int octeon_mgmt_open(struct net_device *netdev)
 
 	/* Interrupt when we have 1 or more packets to clean.  */
 	mix_orhwm.u64 = 0;
-	mix_orhwm.s.orhwm = 1;
+	mix_orhwm.s.orhwm = 0;
 	cvmx_write_csr(p->mix + MIX_ORHWM, mix_orhwm.u64);
 
 	/* Enable receive and transmit interrupts */
@@ -879,13 +1184,12 @@ static int octeon_mgmt_open(struct net_device *netdev)
 	mix_intena.s.othena = 1;
 	cvmx_write_csr(p->mix + MIX_INTENA, mix_intena.u64);
 
-
 	/* Enable packet I/O. */
 
 	rxx_frm_ctl.u64 = 0;
+	rxx_frm_ctl.s.ptp_mode = p->has_rx_tstamp ? 1 : 0;
 	rxx_frm_ctl.s.pre_align = 1;
-	/*
-	 * When set, disables the length check for non-min sized pkts
+	/* When set, disables the length check for non-min sized pkts
 	 * with padding in the client data.
 	 */
 	rxx_frm_ctl.s.pad_len = 1;
@@ -903,33 +1207,26 @@ static int octeon_mgmt_open(struct net_device *netdev)
 	rxx_frm_ctl.s.ctl_drp = 1;
 	/* Strip off the preamble */
 	rxx_frm_ctl.s.pre_strp = 1;
-	/*
-	 * This port is configured to send PREAMBLE+SFD to begin every
+	/* This port is configured to send PREAMBLE+SFD to begin every
 	 * frame.  GMX checks that the PREAMBLE is sent correctly.
 	 */
 	rxx_frm_ctl.s.pre_chk = 1;
 	cvmx_write_csr(p->agl + AGL_GMX_RX_FRM_CTL, rxx_frm_ctl.u64);
 
-	/* Enable the AGL block */
-	agl_gmx_inf_mode.u64 = 0;
-	agl_gmx_inf_mode.s.en = 1;
-	cvmx_write_csr(CVMX_AGL_GMX_INF_MODE, agl_gmx_inf_mode.u64);
-
-	/* Configure the port duplex and enables */
-	prtx_cfg.u64 = cvmx_read_csr(p->agl + AGL_GMX_PRT_CFG);
-	prtx_cfg.s.tx_en = 1;
-	prtx_cfg.s.rx_en = 1;
-	prtx_cfg.s.en = 1;
-	p->last_duplex = 1;
-	prtx_cfg.s.duplex = p->last_duplex;
-	cvmx_write_csr(p->agl + AGL_GMX_PRT_CFG, prtx_cfg.u64);
+	/* Configure the port duplex, speed and enables */
+	octeon_mgmt_disable_link(p);
+	if (p->phydev)
+		octeon_mgmt_update_link(p);
+	octeon_mgmt_enable_link(p);
 
 	p->last_link = 0;
-	netif_carrier_off(netdev);
-
-	if (octeon_mgmt_init_phy(netdev)) {
-		dev_err(p->dev, "Cannot initialize PHY.\n");
-		goto err_noirq;
+	p->last_speed = 0;
+	/* PHY is not present in simulator. The carrier is enabled
+	 * while initializing the phy for simulator, leave it enabled.
+	 */
+	if (p->phydev) {
+		netif_carrier_off(netdev);
+		phy_start_aneg(p->phydev);
 	}
 
 	netif_wake_queue(netdev);
@@ -959,6 +1256,7 @@ static int octeon_mgmt_stop(struct net_device *netdev)
 
 	if (p->phydev)
 		phy_disconnect(p->phydev);
+	p->phydev = NULL;
 
 	netif_carrier_off(netdev);
 
@@ -991,6 +1289,7 @@ static int octeon_mgmt_xmit(struct sk_buff *skb, struct net_device *netdev)
 	int rv = NETDEV_TX_BUSY;
 
 	re.d64 = 0;
+	re.s.tstamp = ((skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) != 0);
 	re.s.len = skb->len;
 	re.s.addr = dma_map_single(p->dev, skb->data,
 				   skb->len,
@@ -1031,6 +1330,7 @@ static int octeon_mgmt_xmit(struct sk_buff *skb, struct net_device *netdev)
 	/* Ring the bell.  */
 	cvmx_write_csr(p->mix + MIX_ORING2, 1);
 
+	netdev->trans_start = jiffies;
 	rv = NETDEV_TX_OK;
 out:
 	octeon_mgmt_update_tx_stats(netdev);
@@ -1068,7 +1368,7 @@ static int octeon_mgmt_get_settings(struct net_device *netdev,
 	if (p->phydev)
 		return phy_ethtool_gset(p->phydev, cmd);
 
-	return -EINVAL;
+	return -EOPNOTSUPP;
 }
 
 static int octeon_mgmt_set_settings(struct net_device *netdev,
@@ -1082,23 +1382,37 @@ static int octeon_mgmt_set_settings(struct net_device *netdev,
 	if (p->phydev)
 		return phy_ethtool_sset(p->phydev, cmd);
 
-	return -EINVAL;
+	return -EOPNOTSUPP;
+}
+
+static int octeon_mgmt_nway_reset(struct net_device *dev)
+{
+	struct octeon_mgmt *p = netdev_priv(dev);
+
+	if (!capable(CAP_NET_ADMIN))
+		return -EPERM;
+
+	if (p->phydev)
+		return phy_start_aneg(p->phydev);
+
+	return -EOPNOTSUPP;
 }
 
 static const struct ethtool_ops octeon_mgmt_ethtool_ops = {
 	.get_drvinfo = octeon_mgmt_get_drvinfo,
-	.get_link = ethtool_op_get_link,
 	.get_settings = octeon_mgmt_get_settings,
-	.set_settings = octeon_mgmt_set_settings
+	.set_settings = octeon_mgmt_set_settings,
+	.nway_reset = octeon_mgmt_nway_reset,
+	.get_link = ethtool_op_get_link,
 };
 
 static const struct net_device_ops octeon_mgmt_ops = {
 	.ndo_open =			octeon_mgmt_open,
 	.ndo_stop =			octeon_mgmt_stop,
 	.ndo_start_xmit =		octeon_mgmt_xmit,
-	.ndo_set_rx_mode = 		octeon_mgmt_set_rx_filtering,
+	.ndo_set_rx_mode =		octeon_mgmt_set_rx_filtering,
 	.ndo_set_mac_address =		octeon_mgmt_set_mac_address,
-	.ndo_do_ioctl = 		octeon_mgmt_ioctl,
+	.ndo_do_ioctl =			octeon_mgmt_ioctl,
 	.ndo_change_mtu =		octeon_mgmt_change_mtu,
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller =		octeon_mgmt_poll_controller,
@@ -1113,6 +1427,7 @@ static int __devinit octeon_mgmt_probe(struct platform_device *pdev)
 	const u8 *mac;
 	struct resource *res_mix;
 	struct resource *res_agl;
+	struct resource *res_agl_prt_ctl;
 	int len;
 	int result;
 
@@ -1120,6 +1435,8 @@ static int __devinit octeon_mgmt_probe(struct platform_device *pdev)
 	if (netdev == NULL)
 		return -ENOMEM;
 
+	SET_NETDEV_DEV(netdev, &pdev->dev);
+
 	dev_set_drvdata(&pdev->dev, netdev);
 	p = netdev_priv(netdev);
 	netif_napi_add(netdev, &p->napi, octeon_mgmt_napi_poll,
@@ -1127,6 +1444,7 @@ static int __devinit octeon_mgmt_probe(struct platform_device *pdev)
 
 	p->netdev = netdev;
 	p->dev = &pdev->dev;
+	p->has_rx_tstamp = false;
 
 	data = of_get_property(pdev->dev.of_node, "cell-index", &len);
 	if (data && len == sizeof(*data)) {
@@ -1159,10 +1477,19 @@ static int __devinit octeon_mgmt_probe(struct platform_device *pdev)
 		goto err;
 	}
 
+	res_agl_prt_ctl = platform_get_resource(pdev, IORESOURCE_MEM, 3);
+	if (res_agl_prt_ctl == NULL) {
+		dev_err(&pdev->dev, "no 'reg' resource\n");
+		result = -ENXIO;
+		goto err;
+	}
+
 	p->mix_phys = res_mix->start;
 	p->mix_size = resource_size(res_mix);
 	p->agl_phys = res_agl->start;
 	p->agl_size = resource_size(res_agl);
+	p->agl_prt_ctl_phys = res_agl_prt_ctl->start;
+	p->agl_prt_ctl_size = resource_size(res_agl_prt_ctl);
 
 
 	if (!devm_request_mem_region(&pdev->dev, p->mix_phys, p->mix_size,
@@ -1181,10 +1508,18 @@ static int __devinit octeon_mgmt_probe(struct platform_device *pdev)
 		goto err;
 	}
 
+	if (!devm_request_mem_region(&pdev->dev, p->agl_prt_ctl_phys,
+				     p->agl_prt_ctl_size, res_agl_prt_ctl->name)) {
+		result = -ENXIO;
+		dev_err(&pdev->dev, "request_mem_region (%s) failed\n",
+			res_agl_prt_ctl->name);
+		goto err;
+	}
 
 	p->mix = (u64)devm_ioremap(&pdev->dev, p->mix_phys, p->mix_size);
 	p->agl = (u64)devm_ioremap(&pdev->dev, p->agl_phys, p->agl_size);
-
+	p->agl_prt_ctl = (u64)devm_ioremap(&pdev->dev, p->agl_prt_ctl_phys,
+					   p->agl_prt_ctl_size);
 	spin_lock_init(&p->lock);
 
 	skb_queue_head_init(&p->tx_list);
@@ -1199,14 +1534,19 @@ static int __devinit octeon_mgmt_probe(struct platform_device *pdev)
 
 	mac = of_get_mac_address(pdev->dev.of_node);
 
-	if (mac)
-		memcpy(netdev->dev_addr, mac, 6);
+	if (mac && is_valid_ether_addr(mac)) {
+		memcpy(netdev->dev_addr, mac, ETH_ALEN);
+		netdev->addr_assign_type &= ~NET_ADDR_RANDOM;
+	} else {
+		eth_hw_addr_random(netdev);
+	}
 
 	p->phy_np = of_parse_phandle(pdev->dev.of_node, "phy-handle", 0);
 
 	pdev->dev.coherent_dma_mask = DMA_BIT_MASK(64);
 	pdev->dev.dma_mask = &pdev->dev.coherent_dma_mask;
 
+	netif_carrier_off(netdev);
 	result = register_netdev(netdev);
 	if (result)
 		goto err;
diff --git a/drivers/net/ethernet/oki-semi/pch_gbe/Kconfig b/drivers/net/ethernet/oki-semi/pch_gbe/Kconfig
index 97302419a37..5296cc8d3cb 100644
--- a/drivers/net/ethernet/oki-semi/pch_gbe/Kconfig
+++ b/drivers/net/ethernet/oki-semi/pch_gbe/Kconfig
@@ -26,6 +26,9 @@ if PCH_GBE
 config PCH_PTP
 	bool "PCH PTP clock support"
 	default n
+	depends on EXPERIMENTAL
+	select PPS
+	select PTP_1588_CLOCK
 	select PTP_1588_CLOCK_PCH
 	---help---
 	  Say Y here if you want to use Precision Time Protocol (PTP) in the
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
index 473ce134ca6..24ad17ec7fc 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
@@ -1601,7 +1601,8 @@ qlcnic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	adapter->netdev  = netdev;
 	adapter->pdev    = pdev;
 
-	if (qlcnic_alloc_adapter_resources(adapter))
+	err = qlcnic_alloc_adapter_resources(adapter);
+	if (err)
 		goto err_out_free_netdev;
 
 	adapter->dev_rst_time = jiffies;
diff --git a/drivers/net/ethernet/realtek/8139cp.c b/drivers/net/ethernet/realtek/8139cp.c
index 995d0cfc4c0..1c818254b7b 100644
--- a/drivers/net/ethernet/realtek/8139cp.c
+++ b/drivers/net/ethernet/realtek/8139cp.c
@@ -563,7 +563,7 @@ rx_next:
 		if (cpr16(IntrStatus) & cp_rx_intr_mask)
 			goto rx_status_loop;
 
-		napi_gro_flush(napi);
+		napi_gro_flush(napi, false);
 		spin_lock_irqsave(&cp->lock, flags);
 		__napi_complete(napi);
 		cpw16_f(IntrMask, cp_intr_mask);
diff --git a/drivers/net/ethernet/renesas/sh_eth.c b/drivers/net/ethernet/renesas/sh_eth.c
index bad8f2eec9b..c8bfea0524d 100644
--- a/drivers/net/ethernet/renesas/sh_eth.c
+++ b/drivers/net/ethernet/renesas/sh_eth.c
@@ -2438,6 +2438,7 @@ static int sh_eth_drv_probe(struct platform_device *pdev)
 		rtsu = platform_get_resource(pdev, IORESOURCE_MEM, 1);
 		if (!rtsu) {
 			dev_err(&pdev->dev, "Not found TSU resource\n");
+			ret = -ENODEV;
 			goto out_release;
 		}
 		mdp->tsu_addr = ioremap(rtsu->start,
diff --git a/drivers/net/ethernet/sfc/ptp.c b/drivers/net/ethernet/sfc/ptp.c
index 5b3dd028ce8..0767043f44a 100644
--- a/drivers/net/ethernet/sfc/ptp.c
+++ b/drivers/net/ethernet/sfc/ptp.c
@@ -640,8 +640,7 @@ static void efx_ptp_drop_time_expired_events(struct efx_nic *efx)
 			evt = list_entry(cursor, struct efx_ptp_event_rx,
 					 link);
 			if (time_after(jiffies, evt->expiry)) {
-				list_del(&evt->link);
-				list_add(&evt->link, &ptp->evt_free_list);
+				list_move(&evt->link, &ptp->evt_free_list);
 				netif_warn(efx, hw, efx->net_dev,
 					   "PTP rx event dropped\n");
 			}
@@ -684,8 +683,7 @@ static enum ptp_packet_state efx_ptp_match_rx(struct efx_nic *efx,
 
 			match->state = PTP_PACKET_STATE_MATCHED;
 			rc = PTP_PACKET_STATE_MATCHED;
-			list_del(&evt->link);
-			list_add(&evt->link, &ptp->evt_free_list);
+			list_move(&evt->link, &ptp->evt_free_list);
 			break;
 		}
 	}
@@ -820,8 +818,7 @@ static int efx_ptp_stop(struct efx_nic *efx)
 	/* Drop any pending receive events */
 	spin_lock_bh(&efx->ptp_data->evt_lock);
 	list_for_each_safe(cursor, next, &efx->ptp_data->evt_list) {
-		list_del(cursor);
-		list_add(cursor, &efx->ptp_data->evt_free_list);
+		list_move(cursor, &efx->ptp_data->evt_free_list);
 	}
 	spin_unlock_bh(&efx->ptp_data->evt_lock);
 
diff --git a/drivers/net/ethernet/sis/sis900.c b/drivers/net/ethernet/sis/sis900.c
index 203d9c6ec23..fb9f6b38511 100644
--- a/drivers/net/ethernet/sis/sis900.c
+++ b/drivers/net/ethernet/sis/sis900.c
@@ -478,8 +478,10 @@ static int __devinit sis900_probe(struct pci_dev *pci_dev,
 
 	/* IO region. */
 	ioaddr = pci_iomap(pci_dev, 0, 0);
-	if (!ioaddr)
+	if (!ioaddr) {
+		ret = -ENOMEM;
 		goto err_out_cleardev;
+	}
 
 	sis_priv = netdev_priv(net_dev);
 	sis_priv->ioaddr = ioaddr;
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
index e872e1da313..7d51a65ab09 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
@@ -50,7 +50,6 @@ struct stmmac_priv {
 	unsigned int dirty_rx;
 	struct sk_buff **rx_skbuff;
 	dma_addr_t *rx_skbuff_dma;
-	struct sk_buff_head rx_recycle;
 
 	struct net_device *dev;
 	dma_addr_t dma_rx_phy;
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 3be88331d17..c6cdbc4eb05 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -747,18 +747,7 @@ static void stmmac_tx(struct stmmac_priv *priv)
 		priv->hw->ring->clean_desc3(p);
 
 		if (likely(skb != NULL)) {
-			/*
-			 * If there's room in the queue (limit it to size)
-			 * we add this skb back into the pool,
-			 * if it's the right size.
-			 */
-			if ((skb_queue_len(&priv->rx_recycle) <
-				priv->dma_rx_size) &&
-				skb_recycle_check(skb, priv->dma_buf_sz))
-				__skb_queue_head(&priv->rx_recycle, skb);
-			else
-				dev_kfree_skb(skb);
-
+			dev_kfree_skb(skb);
 			priv->tx_skbuff[entry] = NULL;
 		}
 
@@ -1169,7 +1158,6 @@ static int stmmac_open(struct net_device *dev)
 	priv->eee_enabled = stmmac_eee_init(priv);
 
 	napi_enable(&priv->napi);
-	skb_queue_head_init(&priv->rx_recycle);
 	netif_start_queue(dev);
 
 	return 0;
@@ -1222,7 +1210,6 @@ static int stmmac_release(struct net_device *dev)
 		kfree(priv->tm);
 #endif
 	napi_disable(&priv->napi);
-	skb_queue_purge(&priv->rx_recycle);
 
 	/* Free the IRQ lines */
 	free_irq(dev->irq, dev);
@@ -1388,10 +1375,7 @@ static inline void stmmac_rx_refill(struct stmmac_priv *priv)
 		if (likely(priv->rx_skbuff[entry] == NULL)) {
 			struct sk_buff *skb;
 
-			skb = __skb_dequeue(&priv->rx_recycle);
-			if (skb == NULL)
-				skb = netdev_alloc_skb_ip_align(priv->dev,
-								bfsize);
+			skb = netdev_alloc_skb_ip_align(priv->dev, bfsize);
 
 			if (unlikely(skb == NULL))
 				break;
diff --git a/drivers/net/ethernet/sun/niu.c b/drivers/net/ethernet/sun/niu.c
index 8419bf385e0..275b430aeb7 100644
--- a/drivers/net/ethernet/sun/niu.c
+++ b/drivers/net/ethernet/sun/niu.c
@@ -9788,6 +9788,7 @@ static int __devinit niu_pci_init_one(struct pci_dev *pdev,
 
 	if (!pci_is_pcie(pdev)) {
 		dev_err(&pdev->dev, "Cannot find PCI Express capability, aborting\n");
+		err = -ENODEV;
 		goto err_out_free_res;
 	}
 
diff --git a/drivers/net/ethernet/sun/sungem.c b/drivers/net/ethernet/sun/sungem.c
index 9ae12d0c963..6c8695ec7cb 100644
--- a/drivers/net/ethernet/sun/sungem.c
+++ b/drivers/net/ethernet/sun/sungem.c
@@ -2963,7 +2963,8 @@ static int __devinit gem_init_one(struct pci_dev *pdev,
 		goto err_out_iounmap;
 	}
 
-	if (gem_get_device_address(gp))
+	err = gem_get_device_address(gp);
+	if (err)
 		goto err_out_free_consistent;
 
 	dev->netdev_ops = &gem_netdev_ops;
diff --git a/drivers/net/irda/irtty-sir.c b/drivers/net/irda/irtty-sir.c
index 30087ca23a0..6e4d4b62c9a 100644
--- a/drivers/net/irda/irtty-sir.c
+++ b/drivers/net/irda/irtty-sir.c
@@ -459,8 +459,10 @@ static int irtty_open(struct tty_struct *tty)
 
 	/* allocate private device info block */
 	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
-	if (!priv)
+	if (!priv) {
+		ret = -ENOMEM;
 		goto out_put;
+	}
 
 	priv->magic = IRTTY_MAGIC;
 	priv->tty = tty;
diff --git a/drivers/net/irda/mcs7780.c b/drivers/net/irda/mcs7780.c
index 1a00b5990cb..f07c340990d 100644
--- a/drivers/net/irda/mcs7780.c
+++ b/drivers/net/irda/mcs7780.c
@@ -920,8 +920,10 @@ static int mcs_probe(struct usb_interface *intf,
 
 	ndev->netdev_ops = &mcs_netdev_ops;
 
-	if (!intf->cur_altsetting)
+	if (!intf->cur_altsetting) {
+		ret = -ENOMEM;
 		goto error2;
+	}
 
 	ret = mcs_find_endpoints(mcs, intf->cur_altsetting->endpoint,
 				 intf->cur_altsetting->desc.bNumEndpoints);
diff --git a/drivers/net/irda/pxaficp_ir.c b/drivers/net/irda/pxaficp_ir.c
index 002a442bf73..858de05bdb7 100644
--- a/drivers/net/irda/pxaficp_ir.c
+++ b/drivers/net/irda/pxaficp_ir.c
@@ -846,8 +846,10 @@ static int pxa_irda_probe(struct platform_device *pdev)
 		goto err_mem_2;
 
 	dev = alloc_irdadev(sizeof(struct pxa_irda));
-	if (!dev)
+	if (!dev) {
+		err = -ENOMEM;
 		goto err_mem_3;
+	}
 
 	SET_NETDEV_DEV(dev, &pdev->dev);
 	si = netdev_priv(dev);
diff --git a/drivers/net/irda/sa1100_ir.c b/drivers/net/irda/sa1100_ir.c
index e25067552b2..42fde9ed23e 100644
--- a/drivers/net/irda/sa1100_ir.c
+++ b/drivers/net/irda/sa1100_ir.c
@@ -940,8 +940,10 @@ static int sa1100_irda_probe(struct platform_device *pdev)
 		goto err_mem_3;
 
 	dev = alloc_irdadev(sizeof(struct sa1100_irda));
-	if (!dev)
+	if (!dev) {
+		err = -ENOMEM;
 		goto err_mem_4;
+	}
 
 	SET_NETDEV_DEV(dev, &pdev->dev);
 
diff --git a/drivers/net/irda/sh_irda.c b/drivers/net/irda/sh_irda.c
index eb315b8d07a..4b746d9bd8e 100644
--- a/drivers/net/irda/sh_irda.c
+++ b/drivers/net/irda/sh_irda.c
@@ -808,8 +808,8 @@ static int __devinit sh_irda_probe(struct platform_device *pdev)
 		goto err_mem_4;
 
 	platform_set_drvdata(pdev, ndev);
-
-	if (request_irq(irq, sh_irda_irq, IRQF_DISABLED, "sh_irda", self)) {
+	err = request_irq(irq, sh_irda_irq, IRQF_DISABLED, "sh_irda", self);
+	if (err) {
 		dev_warn(&pdev->dev, "Unable to attach sh_irda interrupt\n");
 		goto err_mem_4;
 	}
diff --git a/drivers/net/irda/sh_sir.c b/drivers/net/irda/sh_sir.c
index 79510942556..624ac1939e8 100644
--- a/drivers/net/irda/sh_sir.c
+++ b/drivers/net/irda/sh_sir.c
@@ -741,6 +741,7 @@ static int __devinit sh_sir_probe(struct platform_device *pdev)
 	self->clk = clk_get(&pdev->dev, clk_name);
 	if (IS_ERR(self->clk)) {
 		dev_err(&pdev->dev, "cannot get clock \"%s\"\n", clk_name);
+		err = -ENODEV;
 		goto err_mem_3;
 	}
 
@@ -760,8 +761,8 @@ static int __devinit sh_sir_probe(struct platform_device *pdev)
 		goto err_mem_4;
 
 	platform_set_drvdata(pdev, ndev);
-
-	if (request_irq(irq, sh_sir_irq, IRQF_DISABLED, "sh_sir", self)) {
+	err = request_irq(irq, sh_sir_irq, IRQF_DISABLED, "sh_sir", self);
+	if (err) {
 		dev_warn(&pdev->dev, "Unable to attach sh_sir interrupt\n");
 		goto err_mem_4;
 	}
diff --git a/drivers/net/phy/mdio_bus.c b/drivers/net/phy/mdio_bus.c
index 170eb411ab5..c1ef3000ea6 100644
--- a/drivers/net/phy/mdio_bus.c
+++ b/drivers/net/phy/mdio_bus.c
@@ -26,6 +26,7 @@
 #include <linux/delay.h>
 #include <linux/device.h>
 #include <linux/of_device.h>
+#include <linux/of_mdio.h>
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/skbuff.h>
diff --git a/drivers/net/usb/cdc_eem.c b/drivers/net/usb/cdc_eem.c
index 434d5af8e6f..c81e278629f 100644
--- a/drivers/net/usb/cdc_eem.c
+++ b/drivers/net/usb/cdc_eem.c
@@ -244,8 +244,12 @@ static int eem_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
 			 *  - suspend: peripheral ready to suspend
 			 *  - response: suggest N millisec polling
 			 *  - response complete: suggest N sec polling
+			 *
+			 * Suspend is reported and maybe heeded.
 			 */
 			case 2:		/* Suspend hint */
+				usbnet_device_suggests_idle(dev);
+				continue;
 			case 3:		/* Response hint */
 			case 4:		/* Response complete hint */
 				continue;
diff --git a/drivers/net/usb/kaweth.c b/drivers/net/usb/kaweth.c
index c75e11e1b38..afb117c16d2 100644
--- a/drivers/net/usb/kaweth.c
+++ b/drivers/net/usb/kaweth.c
@@ -424,7 +424,7 @@ static int kaweth_download_firmware(struct kaweth_device *kaweth,
 
 	netdev_dbg(kaweth->net,
 		   "Downloading firmware at %p to kaweth device at %p\n",
-		   fw->data, kaweth);
+		   kaweth->firmware_buf, kaweth);
 	netdev_dbg(kaweth->net, "Firmware length: %d\n", data_len);
 
 	return kaweth_control(kaweth,
diff --git a/drivers/net/usb/mcs7830.c b/drivers/net/usb/mcs7830.c
index 03c2d8d653d..cc7e72010ac 100644
--- a/drivers/net/usb/mcs7830.c
+++ b/drivers/net/usb/mcs7830.c
@@ -117,6 +117,7 @@ enum {
 struct mcs7830_data {
 	u8 multi_filter[8];
 	u8 config;
+	u8 link_counter;
 };
 
 static const char driver_name[] = "MOSCHIP usb-ethernet driver";
@@ -632,20 +633,31 @@ static int mcs7830_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
 static void mcs7830_status(struct usbnet *dev, struct urb *urb)
 {
 	u8 *buf = urb->transfer_buffer;
-	bool link;
+	bool link, link_changed;
+	struct mcs7830_data *data = mcs7830_get_data(dev);
 
 	if (urb->actual_length < 16)
 		return;
 
 	link = !(buf[1] & 0x20);
-	if (netif_carrier_ok(dev->net) != link) {
-		if (link) {
-			netif_carrier_on(dev->net);
-			usbnet_defer_kevent(dev, EVENT_LINK_RESET);
-		} else
-			netif_carrier_off(dev->net);
-		netdev_dbg(dev->net, "Link Status is: %d\n", link);
-	}
+	link_changed = netif_carrier_ok(dev->net) != link;
+	if (link_changed) {
+		data->link_counter++;
+		/*
+		   track link state 20 times to guard against erroneous
+		   link state changes reported sometimes by the chip
+		 */
+		if (data->link_counter > 20) {
+			data->link_counter = 0;
+			if (link) {
+				netif_carrier_on(dev->net);
+				usbnet_defer_kevent(dev, EVENT_LINK_RESET);
+			} else
+				netif_carrier_off(dev->net);
+			netdev_dbg(dev->net, "Link Status is: %d\n", link);
+		}
+	} else
+		data->link_counter = 0;
 }
 
 static const struct driver_info moschip_info = {
diff --git a/drivers/net/usb/usbnet.c b/drivers/net/usb/usbnet.c
index fc9f578a1e2..f9819d10b1f 100644
--- a/drivers/net/usb/usbnet.c
+++ b/drivers/net/usb/usbnet.c
@@ -1588,10 +1588,27 @@ int usbnet_resume (struct usb_interface *intf)
 			tasklet_schedule (&dev->bh);
 		}
 	}
+
+	if (test_and_clear_bit(EVENT_DEVICE_REPORT_IDLE, &dev->flags))
+		usb_autopm_get_interface_no_resume(intf);
+
 	return 0;
 }
 EXPORT_SYMBOL_GPL(usbnet_resume);
 
+/*
+ * Either a subdriver implements manage_power, then it is assumed to always
+ * be ready to be suspended or it reports the readiness to be suspended
+ * explicitly
+ */
+void usbnet_device_suggests_idle(struct usbnet *dev)
+{
+	if (!test_and_set_bit(EVENT_DEVICE_REPORT_IDLE, &dev->flags)) {
+		dev->intf->needs_remote_wakeup = 1;
+		usb_autopm_put_interface_async(dev->intf);
+	}
+}
+EXPORT_SYMBOL(usbnet_device_suggests_idle);
 
 /*-------------------------------------------------------------------------*/
 
diff --git a/drivers/net/vxlan.c b/drivers/net/vxlan.c
index 51de9edb55f..607976c0016 100644
--- a/drivers/net/vxlan.c
+++ b/drivers/net/vxlan.c
@@ -28,7 +28,6 @@
 #include <linux/igmp.h>
 #include <linux/etherdevice.h>
 #include <linux/if_ether.h>
-#include <linux/version.h>
 #include <linux/hash.h>
 #include <net/ip.h>
 #include <net/icmp.h>
@@ -107,6 +106,8 @@ struct vxlan_dev {
 	__be32	          gaddr;	/* multicast group */
 	__be32		  saddr;	/* source address */
 	unsigned int      link;		/* link to multicast over */
+	__u16		  port_min;	/* source port range */
+	__u16		  port_max;
 	__u8		  tos;		/* TOS override */
 	__u8		  ttl;
 	bool		  learn;
@@ -229,9 +230,9 @@ static u32 eth_hash(const unsigned char *addr)
 
 	/* only want 6 bytes */
 #ifdef __BIG_ENDIAN
-	value <<= 16;
-#else
 	value >>= 16;
+#else
+	value <<= 16;
 #endif
 	return hash_64(value, FDB_HASH_BITS);
 }
@@ -536,7 +537,6 @@ static int vxlan_udp_encap_recv(struct sock *sk, struct sk_buff *skb)
 	}
 
 	__skb_pull(skb, sizeof(struct vxlanhdr));
-	skb_postpull_rcsum(skb, eth_hdr(skb), sizeof(struct vxlanhdr));
 
 	/* Is this VNI defined? */
 	vni = ntohl(vxh->vx_vni) >> 8;
@@ -555,7 +555,6 @@ static int vxlan_udp_encap_recv(struct sock *sk, struct sk_buff *skb)
 	/* Re-examine inner Ethernet packet */
 	oip = ip_hdr(skb);
 	skb->protocol = eth_type_trans(skb, vxlan->dev);
-	skb_postpull_rcsum(skb, eth_hdr(skb), ETH_HLEN);
 
 	/* Ignore packet loops (and multicast echo) */
 	if (compare_ether_addr(eth_hdr(skb)->h_source,
@@ -567,6 +566,7 @@ static int vxlan_udp_encap_recv(struct sock *sk, struct sk_buff *skb)
 
 	__skb_tunnel_rx(skb, vxlan->dev);
 	skb_reset_network_header(skb);
+	skb->ip_summed = CHECKSUM_NONE;
 
 	err = IP_ECN_decapsulate(oip, skb);
 	if (unlikely(err)) {
@@ -622,46 +622,89 @@ static inline u8 vxlan_ecn_encap(u8 tos,
 	return INET_ECN_encapsulate(tos, inner);
 }
 
+static __be32 vxlan_find_dst(struct vxlan_dev *vxlan, struct sk_buff *skb)
+{
+	const struct ethhdr *eth = (struct ethhdr *) skb->data;
+	const struct vxlan_fdb *f;
+
+	if (is_multicast_ether_addr(eth->h_dest))
+		return vxlan->gaddr;
+
+	f = vxlan_find_mac(vxlan, eth->h_dest);
+	if (f)
+		return f->remote_ip;
+	else
+		return vxlan->gaddr;
+
+}
+
+static void vxlan_sock_free(struct sk_buff *skb)
+{
+	sock_put(skb->sk);
+}
+
+/* On transmit, associate with the tunnel socket */
+static void vxlan_set_owner(struct net_device *dev, struct sk_buff *skb)
+{
+	struct vxlan_net *vn = net_generic(dev_net(dev), vxlan_net_id);
+	struct sock *sk = vn->sock->sk;
+
+	skb_orphan(skb);
+	sock_hold(sk);
+	skb->sk = sk;
+	skb->destructor = vxlan_sock_free;
+}
+
+/* Compute source port for outgoing packet
+ *   first choice to use L4 flow hash since it will spread
+ *     better and maybe available from hardware
+ *   secondary choice is to use jhash on the Ethernet header
+ */
+static u16 vxlan_src_port(const struct vxlan_dev *vxlan, struct sk_buff *skb)
+{
+	unsigned int range = (vxlan->port_max - vxlan->port_min) + 1;
+	u32 hash;
+
+	hash = skb_get_rxhash(skb);
+	if (!hash)
+		hash = jhash(skb->data, 2 * ETH_ALEN,
+			     (__force u32) skb->protocol);
+
+	return (((u64) hash * range) >> 32) + vxlan->port_min;
+}
+
 /* Transmit local packets over Vxlan
  *
  * Outer IP header inherits ECN and DF from inner header.
  * Outer UDP destination is the VXLAN assigned port.
- *           source port is based on hash of flow if available
- *                       otherwise use a random value
+ *           source port is based on hash of flow
  */
 static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct vxlan_dev *vxlan = netdev_priv(dev);
 	struct rtable *rt;
-	const struct ethhdr *eth;
 	const struct iphdr *old_iph;
 	struct iphdr *iph;
 	struct vxlanhdr *vxh;
 	struct udphdr *uh;
 	struct flowi4 fl4;
-	struct vxlan_fdb *f;
 	unsigned int pkt_len = skb->len;
-	u32 hash;
 	__be32 dst;
+	__u16 src_port;
 	__be16 df = 0;
 	__u8 tos, ttl;
 	int err;
 
+	dst = vxlan_find_dst(vxlan, skb);
+	if (!dst)
+		goto drop;
+
 	/* Need space for new headers (invalidates iph ptr) */
 	if (skb_cow_head(skb, VXLAN_HEADROOM))
 		goto drop;
 
-	eth = (void *)skb->data;
 	old_iph = ip_hdr(skb);
 
-	if (!is_multicast_ether_addr(eth->h_dest) &&
-	    (f = vxlan_find_mac(vxlan, eth->h_dest)))
-		dst = f->remote_ip;
-	else if (vxlan->gaddr) {
-		dst = vxlan->gaddr;
-	} else
-		goto drop;
-
 	ttl = vxlan->ttl;
 	if (!ttl && IN_MULTICAST(ntohl(dst)))
 		ttl = 1;
@@ -670,11 +713,15 @@ static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (tos == 1)
 		tos = vxlan_get_dsfield(old_iph, skb);
 
-	hash = skb_get_rxhash(skb);
+	src_port = vxlan_src_port(vxlan, skb);
+
+	memset(&fl4, 0, sizeof(fl4));
+	fl4.flowi4_oif = vxlan->link;
+	fl4.flowi4_tos = RT_TOS(tos);
+	fl4.daddr = dst;
+	fl4.saddr = vxlan->saddr;
 
-	rt = ip_route_output_gre(dev_net(dev), &fl4, dst,
-				 vxlan->saddr, vxlan->vni,
-				 RT_TOS(tos), vxlan->link);
+	rt = ip_route_output_key(dev_net(dev), &fl4);
 	if (IS_ERR(rt)) {
 		netdev_dbg(dev, "no route to %pI4\n", &dst);
 		dev->stats.tx_carrier_errors++;
@@ -703,7 +750,7 @@ static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
 	uh = udp_hdr(skb);
 
 	uh->dest = htons(vxlan_port);
-	uh->source = hash ? :random32();
+	uh->source = htons(src_port);
 
 	uh->len = htons(skb->len);
 	uh->check = 0;
@@ -716,10 +763,12 @@ static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
 	iph->frag_off	= df;
 	iph->protocol	= IPPROTO_UDP;
 	iph->tos	= vxlan_ecn_encap(tos, old_iph, skb);
-	iph->daddr	= fl4.daddr;
+	iph->daddr	= dst;
 	iph->saddr	= fl4.saddr;
 	iph->ttl	= ttl ? : ip4_dst_hoplimit(&rt->dst);
 
+	vxlan_set_owner(dev, skb);
+
 	/* See __IPTUNNEL_XMIT */
 	skb->ip_summed = CHECKSUM_NONE;
 	ip_select_ident(iph, &rt->dst, NULL);
@@ -929,9 +978,11 @@ static void vxlan_setup(struct net_device *dev)
 {
 	struct vxlan_dev *vxlan = netdev_priv(dev);
 	unsigned h;
+	int low, high;
 
 	eth_hw_addr_random(dev);
 	ether_setup(dev);
+	dev->hard_header_len = ETH_HLEN + VXLAN_HEADROOM;
 
 	dev->netdev_ops = &vxlan_netdev_ops;
 	dev->destructor = vxlan_free;
@@ -948,6 +999,10 @@ static void vxlan_setup(struct net_device *dev)
 	vxlan->age_timer.function = vxlan_cleanup;
 	vxlan->age_timer.data = (unsigned long) vxlan;
 
+	inet_get_local_port_range(&low, &high);
+	vxlan->port_min = low;
+	vxlan->port_max = high;
+
 	vxlan->dev = dev;
 
 	for (h = 0; h < FDB_HASH_SIZE; ++h)
@@ -964,6 +1019,7 @@ static const struct nla_policy vxlan_policy[IFLA_VXLAN_MAX + 1] = {
 	[IFLA_VXLAN_LEARNING]	= { .type = NLA_U8 },
 	[IFLA_VXLAN_AGEING]	= { .type = NLA_U32 },
 	[IFLA_VXLAN_LIMIT]	= { .type = NLA_U32 },
+	[IFLA_VXLAN_PORT_RANGE] = { .len  = sizeof(struct ifla_vxlan_port_range) },
 };
 
 static int vxlan_validate(struct nlattr *tb[], struct nlattr *data[])
@@ -996,6 +1052,18 @@ static int vxlan_validate(struct nlattr *tb[], struct nlattr *data[])
 			return -EADDRNOTAVAIL;
 		}
 	}
+
+	if (data[IFLA_VXLAN_PORT_RANGE]) {
+		const struct ifla_vxlan_port_range *p
+			= nla_data(data[IFLA_VXLAN_PORT_RANGE]);
+
+		if (ntohs(p->high) < ntohs(p->low)) {
+			pr_debug("port range %u .. %u not valid\n",
+				 ntohs(p->low), ntohs(p->high));
+			return -EINVAL;
+		}
+	}
+
 	return 0;
 }
 
@@ -1022,14 +1090,18 @@ static int vxlan_newlink(struct net *net, struct net_device *dev,
 	if (data[IFLA_VXLAN_LOCAL])
 		vxlan->saddr = nla_get_be32(data[IFLA_VXLAN_LOCAL]);
 
-	if (data[IFLA_VXLAN_LINK]) {
-		vxlan->link = nla_get_u32(data[IFLA_VXLAN_LINK]);
+	if (data[IFLA_VXLAN_LINK] &&
+	    (vxlan->link = nla_get_u32(data[IFLA_VXLAN_LINK]))) {
+		struct net_device *lowerdev
+			 = __dev_get_by_index(net, vxlan->link);
 
-		if (!tb[IFLA_MTU]) {
-			struct net_device *lowerdev;
-			lowerdev = __dev_get_by_index(net, vxlan->link);
-			dev->mtu = lowerdev->mtu - VXLAN_HEADROOM;
+		if (!lowerdev) {
+			pr_info("ifindex %d does not exist\n", vxlan->link);
+			return -ENODEV;
 		}
+
+		if (!tb[IFLA_MTU])
+			dev->mtu = lowerdev->mtu - VXLAN_HEADROOM;
 	}
 
 	if (data[IFLA_VXLAN_TOS])
@@ -1046,6 +1118,13 @@ static int vxlan_newlink(struct net *net, struct net_device *dev,
 	if (data[IFLA_VXLAN_LIMIT])
 		vxlan->addrmax = nla_get_u32(data[IFLA_VXLAN_LIMIT]);
 
+	if (data[IFLA_VXLAN_PORT_RANGE]) {
+		const struct ifla_vxlan_port_range *p
+			= nla_data(data[IFLA_VXLAN_PORT_RANGE]);
+		vxlan->port_min = ntohs(p->low);
+		vxlan->port_max = ntohs(p->high);
+	}
+
 	err = register_netdevice(dev);
 	if (!err)
 		hlist_add_head_rcu(&vxlan->hlist, vni_head(net, vxlan->vni));
@@ -1074,23 +1153,28 @@ static size_t vxlan_get_size(const struct net_device *dev)
 		nla_total_size(sizeof(__u8)) +	/* IFLA_VXLAN_LEARNING */
 		nla_total_size(sizeof(__u32)) +	/* IFLA_VXLAN_AGEING */
 		nla_total_size(sizeof(__u32)) +	/* IFLA_VXLAN_LIMIT */
+		nla_total_size(sizeof(struct ifla_vxlan_port_range)) +
 		0;
 }
 
 static int vxlan_fill_info(struct sk_buff *skb, const struct net_device *dev)
 {
 	const struct vxlan_dev *vxlan = netdev_priv(dev);
+	struct ifla_vxlan_port_range ports = {
+		.low =  htons(vxlan->port_min),
+		.high = htons(vxlan->port_max),
+	};
 
 	if (nla_put_u32(skb, IFLA_VXLAN_ID, vxlan->vni))
 		goto nla_put_failure;
 
-	if (vxlan->gaddr && nla_put_u32(skb, IFLA_VXLAN_GROUP, vxlan->gaddr))
+	if (vxlan->gaddr && nla_put_be32(skb, IFLA_VXLAN_GROUP, vxlan->gaddr))
 		goto nla_put_failure;
 
 	if (vxlan->link && nla_put_u32(skb, IFLA_VXLAN_LINK, vxlan->link))
 		goto nla_put_failure;
 
-	if (vxlan->saddr && nla_put_u32(skb, IFLA_VXLAN_LOCAL, vxlan->saddr))
+	if (vxlan->saddr && nla_put_be32(skb, IFLA_VXLAN_LOCAL, vxlan->saddr))
 		goto nla_put_failure;
 
 	if (nla_put_u8(skb, IFLA_VXLAN_TTL, vxlan->ttl) ||
@@ -1100,6 +1184,9 @@ static int vxlan_fill_info(struct sk_buff *skb, const struct net_device *dev)
 	    nla_put_u32(skb, IFLA_VXLAN_LIMIT, vxlan->addrmax))
 		goto nla_put_failure;
 
+	if (nla_put(skb, IFLA_VXLAN_PORT_RANGE, sizeof(ports), &ports))
+		goto nla_put_failure;
+
 	return 0;
 
 nla_put_failure:
diff --git a/drivers/net/wan/farsync.c b/drivers/net/wan/farsync.c
index 1a623183cbe..b6271325f80 100644
--- a/drivers/net/wan/farsync.c
+++ b/drivers/net/wan/farsync.c
@@ -597,7 +597,7 @@ fst_q_work_item(u64 * queue, int card_index)
 	 * bottom half for the card.  Note the limitation of 64 cards.
 	 * That ought to be enough
 	 */
-	mask = 1 << card_index;
+	mask = (u64)1 << card_index;
 	*queue |= mask;
 	spin_unlock_irqrestore(&fst_work_q_lock, flags);
 }
diff --git a/drivers/net/wireless/ath/ath5k/base.c b/drivers/net/wireless/ath/ath5k/base.c
index 9fd6d9a9942..9f31cfa56cc 100644
--- a/drivers/net/wireless/ath/ath5k/base.c
+++ b/drivers/net/wireless/ath/ath5k/base.c
@@ -1804,7 +1804,7 @@ ath5k_beacon_update(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 {
 	int ret;
 	struct ath5k_hw *ah = hw->priv;
-	struct ath5k_vif *avf = (void *)vif->drv_priv;
+	struct ath5k_vif *avf;
 	struct sk_buff *skb;
 
 	if (WARN_ON(!vif)) {
@@ -1819,6 +1819,7 @@ ath5k_beacon_update(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 		goto out;
 	}
 
+	avf = (void *)vif->drv_priv;
 	ath5k_txbuf_free_skb(ah, avf->bbuf);
 	avf->bbuf->skb = skb;
 	ret = ath5k_beacon_setup(ah, avf->bbuf);
diff --git a/drivers/net/wireless/ath/ath9k/beacon.c b/drivers/net/wireless/ath/ath9k/beacon.c
index 76f07d8c272..1b48414dca9 100644
--- a/drivers/net/wireless/ath/ath9k/beacon.c
+++ b/drivers/net/wireless/ath/ath9k/beacon.c
@@ -120,7 +120,7 @@ static void ath9k_tx_cabq(struct ieee80211_hw *hw, struct sk_buff *skb)
 
 	if (ath_tx_start(hw, skb, &txctl) != 0) {
 		ath_dbg(common, XMIT, "CABQ TX failed\n");
-		dev_kfree_skb_any(skb);
+		ieee80211_free_txskb(hw, skb);
 	}
 }
 
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index f9a6ec5cf47..8e1559aba49 100644
--- a/drivers/net/wireless/ath/ath9k/hw.c
+++ b/drivers/net/wireless/ath/ath9k/hw.c
@@ -1450,9 +1450,14 @@ static bool ath9k_hw_set_reset_reg(struct ath_hw *ah, u32 type)
 	REG_WRITE(ah, AR_RTC_FORCE_WAKE,
 		  AR_RTC_FORCE_WAKE_EN | AR_RTC_FORCE_WAKE_ON_INT);
 
+	if (!ah->reset_power_on)
+		type = ATH9K_RESET_POWER_ON;
+
 	switch (type) {
 	case ATH9K_RESET_POWER_ON:
 		ret = ath9k_hw_set_reset_power_on(ah);
+		if (!ret)
+			ah->reset_power_on = true;
 		break;
 	case ATH9K_RESET_WARM:
 	case ATH9K_RESET_COLD:
diff --git a/drivers/net/wireless/ath/ath9k/hw.h b/drivers/net/wireless/ath/ath9k/hw.h
index 566a4ce4f15..dbc1b7a4cbf 100644
--- a/drivers/net/wireless/ath/ath9k/hw.h
+++ b/drivers/net/wireless/ath/ath9k/hw.h
@@ -741,6 +741,7 @@ struct ath_hw {
 	u32 rfkill_polarity;
 	u32 ah_flags;
 
+	bool reset_power_on;
 	bool htc_reset_init;
 
 	enum nl80211_iftype opmode;
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index 31ab82e3ba8..dd45edfa6ba 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -639,8 +639,7 @@ static int ath9k_start(struct ieee80211_hw *hw)
 		ath_err(common,
 			"Unable to reset hardware; reset status %d (freq %u MHz)\n",
 			r, curchan->center_freq);
-		spin_unlock_bh(&sc->sc_pcu_lock);
-		goto mutex_unlock;
+		ah->reset_power_on = false;
 	}
 
 	/* Setup our intr mask. */
@@ -665,11 +664,8 @@ static int ath9k_start(struct ieee80211_hw *hw)
 	clear_bit(SC_OP_INVALID, &sc->sc_flags);
 	sc->sc_ah->is_monitoring = false;
 
-	if (!ath_complete_reset(sc, false)) {
-		r = -EIO;
-		spin_unlock_bh(&sc->sc_pcu_lock);
-		goto mutex_unlock;
-	}
+	if (!ath_complete_reset(sc, false))
+		ah->reset_power_on = false;
 
 	if (ah->led_pin >= 0) {
 		ath9k_hw_cfg_output(ah, ah->led_pin,
@@ -688,12 +684,11 @@ static int ath9k_start(struct ieee80211_hw *hw)
 	if (ah->caps.pcie_lcr_extsync_en && common->bus_ops->extn_synch_en)
 		common->bus_ops->extn_synch_en(common);
 
-mutex_unlock:
 	mutex_unlock(&sc->mutex);
 
 	ath9k_ps_restore(sc);
 
-	return r;
+	return 0;
 }
 
 static void ath9k_tx(struct ieee80211_hw *hw,
@@ -770,7 +765,7 @@ static void ath9k_tx(struct ieee80211_hw *hw,
 
 	return;
 exit:
-	dev_kfree_skb_any(skb);
+	ieee80211_free_txskb(hw, skb);
 }
 
 static void ath9k_stop(struct ieee80211_hw *hw)
diff --git a/drivers/net/wireless/ath/ath9k/pci.c b/drivers/net/wireless/ath/ath9k/pci.c
index 0e630a99b68..f088f4bf9a2 100644
--- a/drivers/net/wireless/ath/ath9k/pci.c
+++ b/drivers/net/wireless/ath/ath9k/pci.c
@@ -324,6 +324,10 @@ static int ath_pci_suspend(struct device *device)
 static int ath_pci_resume(struct device *device)
 {
 	struct pci_dev *pdev = to_pci_dev(device);
+	struct ieee80211_hw *hw = pci_get_drvdata(pdev);
+	struct ath_softc *sc = hw->priv;
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
 	u32 val;
 
 	/*
@@ -335,6 +339,9 @@ static int ath_pci_resume(struct device *device)
 	if ((val & 0x0000ff00) != 0)
 		pci_write_config_dword(pdev, 0x40, val & 0xffff00ff);
 
+	ath_pci_aspm_init(common);
+	ah->reset_power_on = false;
+
 	return 0;
 }
 
diff --git a/drivers/net/wireless/ath/ath9k/xmit.c b/drivers/net/wireless/ath/ath9k/xmit.c
index 36618e3a5e6..378bd70256b 100644
--- a/drivers/net/wireless/ath/ath9k/xmit.c
+++ b/drivers/net/wireless/ath/ath9k/xmit.c
@@ -66,8 +66,7 @@ static void ath_tx_update_baw(struct ath_softc *sc, struct ath_atx_tid *tid,
 static struct ath_buf *ath_tx_setup_buffer(struct ath_softc *sc,
 					   struct ath_txq *txq,
 					   struct ath_atx_tid *tid,
-					   struct sk_buff *skb,
-					   bool dequeue);
+					   struct sk_buff *skb);
 
 enum {
 	MCS_HT20,
@@ -176,7 +175,15 @@ static void ath_tx_flush_tid(struct ath_softc *sc, struct ath_atx_tid *tid)
 		fi = get_frame_info(skb);
 		bf = fi->bf;
 
-		if (bf && fi->retries) {
+		if (!bf) {
+			bf = ath_tx_setup_buffer(sc, txq, tid, skb);
+			if (!bf) {
+				ieee80211_free_txskb(sc->hw, skb);
+				continue;
+			}
+		}
+
+		if (fi->retries) {
 			list_add_tail(&bf->list, &bf_head);
 			ath_tx_update_baw(sc, tid, bf->bf_state.seqno);
 			ath_tx_complete_buf(sc, bf, txq, &bf_head, &ts, 0);
@@ -785,10 +792,13 @@ static enum ATH_AGGR_STATUS ath_tx_form_aggr(struct ath_softc *sc,
 		fi = get_frame_info(skb);
 		bf = fi->bf;
 		if (!fi->bf)
-			bf = ath_tx_setup_buffer(sc, txq, tid, skb, true);
+			bf = ath_tx_setup_buffer(sc, txq, tid, skb);
 
-		if (!bf)
+		if (!bf) {
+			__skb_unlink(skb, &tid->buf_q);
+			ieee80211_free_txskb(sc->hw, skb);
 			continue;
+		}
 
 		bf->bf_state.bf_type = BUF_AMPDU | BUF_AGGR;
 		seqno = bf->bf_state.seqno;
@@ -1731,9 +1741,11 @@ static void ath_tx_send_ampdu(struct ath_softc *sc, struct ath_atx_tid *tid,
 		return;
 	}
 
-	bf = ath_tx_setup_buffer(sc, txctl->txq, tid, skb, false);
-	if (!bf)
+	bf = ath_tx_setup_buffer(sc, txctl->txq, tid, skb);
+	if (!bf) {
+		ieee80211_free_txskb(sc->hw, skb);
 		return;
+	}
 
 	bf->bf_state.bf_type = BUF_AMPDU;
 	INIT_LIST_HEAD(&bf_head);
@@ -1757,11 +1769,6 @@ static void ath_tx_send_normal(struct ath_softc *sc, struct ath_txq *txq,
 	struct ath_buf *bf;
 
 	bf = fi->bf;
-	if (!bf)
-		bf = ath_tx_setup_buffer(sc, txq, tid, skb, false);
-
-	if (!bf)
-		return;
 
 	INIT_LIST_HEAD(&bf_head);
 	list_add_tail(&bf->list, &bf_head);
@@ -1839,8 +1846,7 @@ u8 ath_txchainmask_reduction(struct ath_softc *sc, u8 chainmask, u32 rate)
 static struct ath_buf *ath_tx_setup_buffer(struct ath_softc *sc,
 					   struct ath_txq *txq,
 					   struct ath_atx_tid *tid,
-					   struct sk_buff *skb,
-					   bool dequeue)
+					   struct sk_buff *skb)
 {
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_frame_info *fi = get_frame_info(skb);
@@ -1852,7 +1858,7 @@ static struct ath_buf *ath_tx_setup_buffer(struct ath_softc *sc,
 	bf = ath_tx_get_buffer(sc);
 	if (!bf) {
 		ath_dbg(common, XMIT, "TX buffers are full\n");
-		goto error;
+		return NULL;
 	}
 
 	ATH_TXBUF_RESET(bf);
@@ -1881,18 +1887,12 @@ static struct ath_buf *ath_tx_setup_buffer(struct ath_softc *sc,
 		ath_err(ath9k_hw_common(sc->sc_ah),
 			"dma_mapping_error() on TX\n");
 		ath_tx_return_buffer(sc, bf);
-		goto error;
+		return NULL;
 	}
 
 	fi->bf = bf;
 
 	return bf;
-
-error:
-	if (dequeue)
-		__skb_unlink(skb, &tid->buf_q);
-	dev_kfree_skb_any(skb);
-	return NULL;
 }
 
 /* FIXME: tx power */
@@ -1921,9 +1921,14 @@ static void ath_tx_start_dma(struct ath_softc *sc, struct sk_buff *skb,
 		 */
 		ath_tx_send_ampdu(sc, tid, skb, txctl);
 	} else {
-		bf = ath_tx_setup_buffer(sc, txctl->txq, tid, skb, false);
-		if (!bf)
+		bf = ath_tx_setup_buffer(sc, txctl->txq, tid, skb);
+		if (!bf) {
+			if (txctl->paprd)
+				dev_kfree_skb_any(skb);
+			else
+				ieee80211_free_txskb(sc->hw, skb);
 			return;
+		}
 
 		bf->bf_state.bfs_paprd = txctl->paprd;
 
diff --git a/drivers/net/wireless/ath/carl9170/carl9170.h b/drivers/net/wireless/ath/carl9170/carl9170.h
index 2aa4a59c72c..2df17f1e49e 100644
--- a/drivers/net/wireless/ath/carl9170/carl9170.h
+++ b/drivers/net/wireless/ath/carl9170/carl9170.h
@@ -303,6 +303,7 @@ struct ar9170 {
 	unsigned long queue_stop_timeout[__AR9170_NUM_TXQ];
 	unsigned long max_queue_stop_timeout[__AR9170_NUM_TXQ];
 	bool needs_full_reset;
+	bool force_usb_reset;
 	atomic_t pending_restarts;
 
 	/* interface mode settings */
diff --git a/drivers/net/wireless/ath/carl9170/main.c b/drivers/net/wireless/ath/carl9170/main.c
index 67997b39aba..25a1e2f4f73 100644
--- a/drivers/net/wireless/ath/carl9170/main.c
+++ b/drivers/net/wireless/ath/carl9170/main.c
@@ -465,27 +465,26 @@ static void carl9170_restart_work(struct work_struct *work)
 {
 	struct ar9170 *ar = container_of(work, struct ar9170,
 					 restart_work);
-	int err;
+	int err = -EIO;
 
 	ar->usedkeys = 0;
 	ar->filter_state = 0;
 	carl9170_cancel_worker(ar);
 
 	mutex_lock(&ar->mutex);
-	err = carl9170_usb_restart(ar);
-	if (net_ratelimit()) {
-		if (err) {
-			dev_err(&ar->udev->dev, "Failed to restart device "
-				" (%d).\n", err);
-		 } else {
-			dev_info(&ar->udev->dev, "device restarted "
-				 "successfully.\n");
+	if (!ar->force_usb_reset) {
+		err = carl9170_usb_restart(ar);
+		if (net_ratelimit()) {
+			if (err)
+				dev_err(&ar->udev->dev, "Failed to restart device (%d).\n", err);
+			else
+				dev_info(&ar->udev->dev, "device restarted successfully.\n");
 		}
 	}
-
 	carl9170_zap_queues(ar);
 	mutex_unlock(&ar->mutex);
-	if (!err) {
+
+	if (!err && !ar->force_usb_reset) {
 		ar->restart_counter++;
 		atomic_set(&ar->pending_restarts, 0);
 
@@ -526,10 +525,10 @@ void carl9170_restart(struct ar9170 *ar, const enum carl9170_restart_reasons r)
 	if (!ar->registered)
 		return;
 
-	if (IS_ACCEPTING_CMD(ar) && !ar->needs_full_reset)
-		ieee80211_queue_work(ar->hw, &ar->restart_work);
-	else
-		carl9170_usb_reset(ar);
+	if (!IS_ACCEPTING_CMD(ar) || ar->needs_full_reset)
+		ar->force_usb_reset = true;
+
+	ieee80211_queue_work(ar->hw, &ar->restart_work);
 
 	/*
 	 * At this point, the device instance might have vanished/disabled.
diff --git a/drivers/net/wireless/mwifiex/cfg80211.c b/drivers/net/wireless/mwifiex/cfg80211.c
index 2691620393e..0679458a1ba 100644
--- a/drivers/net/wireless/mwifiex/cfg80211.c
+++ b/drivers/net/wireless/mwifiex/cfg80211.c
@@ -1596,8 +1596,9 @@ done:
 		}
 	}
 
-	if (mwifiex_bss_start(priv, bss, &req_ssid))
-		return -EFAULT;
+	ret = mwifiex_bss_start(priv, bss, &req_ssid);
+	if (ret)
+		return ret;
 
 	if (mode == NL80211_IFTYPE_ADHOC) {
 		/* Inform the BSS information to kernel, otherwise
@@ -1652,9 +1653,19 @@ done:
 			"info: association to bssid %pM failed\n",
 			priv->cfg_bssid);
 		memset(priv->cfg_bssid, 0, ETH_ALEN);
+
+		if (ret > 0)
+			cfg80211_connect_result(priv->netdev, priv->cfg_bssid,
+						NULL, 0, NULL, 0, ret,
+						GFP_KERNEL);
+		else
+			cfg80211_connect_result(priv->netdev, priv->cfg_bssid,
+						NULL, 0, NULL, 0,
+						WLAN_STATUS_UNSPECIFIED_FAILURE,
+						GFP_KERNEL);
 	}
 
-	return ret;
+	return 0;
 }
 
 /*
@@ -1802,7 +1813,7 @@ mwifiex_cfg80211_scan(struct wiphy *wiphy,
 {
 	struct net_device *dev = request->wdev->netdev;
 	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
-	int i, offset;
+	int i, offset, ret;
 	struct ieee80211_channel *chan;
 	struct ieee_types_header *ie;
 
@@ -1855,8 +1866,12 @@ mwifiex_cfg80211_scan(struct wiphy *wiphy,
 
 		priv->user_scan_cfg->chan_list[i].scan_time = 0;
 	}
-	if (mwifiex_scan_networks(priv, priv->user_scan_cfg))
-		return -EFAULT;
+
+	ret = mwifiex_scan_networks(priv, priv->user_scan_cfg);
+	if (ret) {
+		dev_err(priv->adapter->dev, "scan failed: %d\n", ret);
+		return ret;
+	}
 
 	if (request->ie && request->ie_len) {
 		for (i = 0; i < MWIFIEX_MAX_VSIE_NUM; i++) {
diff --git a/drivers/net/wireless/mwifiex/join.c b/drivers/net/wireless/mwifiex/join.c
index 82e63cee1e9..7b0858af8f5 100644
--- a/drivers/net/wireless/mwifiex/join.c
+++ b/drivers/net/wireless/mwifiex/join.c
@@ -1180,16 +1180,18 @@ int mwifiex_ret_802_11_ad_hoc(struct mwifiex_private *priv,
 	struct mwifiex_adapter *adapter = priv->adapter;
 	struct host_cmd_ds_802_11_ad_hoc_result *adhoc_result;
 	struct mwifiex_bssdescriptor *bss_desc;
+	u16 reason_code;
 
 	adhoc_result = &resp->params.adhoc_result;
 
 	bss_desc = priv->attempted_bss_desc;
 
 	/* Join result code 0 --> SUCCESS */
-	if (le16_to_cpu(resp->result)) {
+	reason_code = le16_to_cpu(resp->result);
+	if (reason_code) {
 		dev_err(priv->adapter->dev, "ADHOC_RESP: failed\n");
 		if (priv->media_connected)
-			mwifiex_reset_connect_state(priv);
+			mwifiex_reset_connect_state(priv, reason_code);
 
 		memset(&priv->curr_bss_params.bss_descriptor,
 		       0x00, sizeof(struct mwifiex_bssdescriptor));
diff --git a/drivers/net/wireless/mwifiex/main.h b/drivers/net/wireless/mwifiex/main.h
index bfb3fa69805..c2d0ab146af 100644
--- a/drivers/net/wireless/mwifiex/main.h
+++ b/drivers/net/wireless/mwifiex/main.h
@@ -847,7 +847,7 @@ int mwifiex_cmd_802_11_associate(struct mwifiex_private *priv,
 				 struct mwifiex_bssdescriptor *bss_desc);
 int mwifiex_ret_802_11_associate(struct mwifiex_private *priv,
 				 struct host_cmd_ds_command *resp);
-void mwifiex_reset_connect_state(struct mwifiex_private *priv);
+void mwifiex_reset_connect_state(struct mwifiex_private *priv, u16 reason);
 u8 mwifiex_band_to_radio_type(u8 band);
 int mwifiex_deauthenticate(struct mwifiex_private *priv, u8 *mac);
 int mwifiex_adhoc_start(struct mwifiex_private *priv,
diff --git a/drivers/net/wireless/mwifiex/scan.c b/drivers/net/wireless/mwifiex/scan.c
index e36a75988f8..00b658d3b6e 100644
--- a/drivers/net/wireless/mwifiex/scan.c
+++ b/drivers/net/wireless/mwifiex/scan.c
@@ -1296,7 +1296,7 @@ mwifiex_radio_type_to_band(u8 radio_type)
 int mwifiex_scan_networks(struct mwifiex_private *priv,
 			  const struct mwifiex_user_scan_cfg *user_scan_in)
 {
-	int ret = 0;
+	int ret;
 	struct mwifiex_adapter *adapter = priv->adapter;
 	struct cmd_ctrl_node *cmd_node;
 	union mwifiex_scan_cmd_config_tlv *scan_cfg_out;
@@ -1309,25 +1309,26 @@ int mwifiex_scan_networks(struct mwifiex_private *priv,
 	unsigned long flags;
 
 	if (adapter->scan_processing) {
-		dev_dbg(adapter->dev, "cmd: Scan already in process...\n");
-		return ret;
+		dev_err(adapter->dev, "cmd: Scan already in process...\n");
+		return -EBUSY;
 	}
 
-	spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
-	adapter->scan_processing = true;
-	spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, flags);
-
 	if (priv->scan_block) {
-		dev_dbg(adapter->dev,
+		dev_err(adapter->dev,
 			"cmd: Scan is blocked during association...\n");
-		return ret;
+		return -EBUSY;
 	}
 
+	spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
+	adapter->scan_processing = true;
+	spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, flags);
+
 	scan_cfg_out = kzalloc(sizeof(union mwifiex_scan_cmd_config_tlv),
 								GFP_KERNEL);
 	if (!scan_cfg_out) {
 		dev_err(adapter->dev, "failed to alloc scan_cfg_out\n");
-		return -ENOMEM;
+		ret = -ENOMEM;
+		goto done;
 	}
 
 	buf_size = sizeof(struct mwifiex_chan_scan_param_set) *
@@ -1336,7 +1337,8 @@ int mwifiex_scan_networks(struct mwifiex_private *priv,
 	if (!scan_chan_list) {
 		dev_err(adapter->dev, "failed to alloc scan_chan_list\n");
 		kfree(scan_cfg_out);
-		return -ENOMEM;
+		ret = -ENOMEM;
+		goto done;
 	}
 
 	mwifiex_config_scan(priv, user_scan_in, &scan_cfg_out->config,
@@ -1364,14 +1366,16 @@ int mwifiex_scan_networks(struct mwifiex_private *priv,
 			spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
 					       flags);
 		}
-	} else {
-		spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
-		adapter->scan_processing = true;
-		spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, flags);
 	}
 
 	kfree(scan_cfg_out);
 	kfree(scan_chan_list);
+done:
+	if (ret) {
+		spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
+		adapter->scan_processing = false;
+		spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, flags);
+	}
 	return ret;
 }
 
@@ -1430,8 +1434,8 @@ int mwifiex_check_network_compatibility(struct mwifiex_private *priv,
 			ret = mwifiex_is_network_compatible(priv, bss_desc,
 							    priv->bss_mode);
 			if (ret)
-				dev_err(priv->adapter->dev, "cannot find ssid "
-					"%s\n", bss_desc->ssid.ssid);
+				dev_err(priv->adapter->dev,
+					"Incompatible network settings\n");
 			break;
 		default:
 			ret = 0;
diff --git a/drivers/net/wireless/mwifiex/sta_cmdresp.c b/drivers/net/wireless/mwifiex/sta_cmdresp.c
index e380171c4c5..09e6a267f56 100644
--- a/drivers/net/wireless/mwifiex/sta_cmdresp.c
+++ b/drivers/net/wireless/mwifiex/sta_cmdresp.c
@@ -545,7 +545,7 @@ static int mwifiex_ret_802_11_deauthenticate(struct mwifiex_private *priv,
 	if (!memcmp(resp->params.deauth.mac_addr,
 		    &priv->curr_bss_params.bss_descriptor.mac_address,
 		    sizeof(resp->params.deauth.mac_addr)))
-		mwifiex_reset_connect_state(priv);
+		mwifiex_reset_connect_state(priv, WLAN_REASON_DEAUTH_LEAVING);
 
 	return 0;
 }
@@ -558,7 +558,7 @@ static int mwifiex_ret_802_11_deauthenticate(struct mwifiex_private *priv,
 static int mwifiex_ret_802_11_ad_hoc_stop(struct mwifiex_private *priv,
 					  struct host_cmd_ds_command *resp)
 {
-	mwifiex_reset_connect_state(priv);
+	mwifiex_reset_connect_state(priv, WLAN_REASON_DEAUTH_LEAVING);
 	return 0;
 }
 
diff --git a/drivers/net/wireless/mwifiex/sta_event.c b/drivers/net/wireless/mwifiex/sta_event.c
index aafde30e714..8132119e1a2 100644
--- a/drivers/net/wireless/mwifiex/sta_event.c
+++ b/drivers/net/wireless/mwifiex/sta_event.c
@@ -41,7 +41,7 @@
  *      - Sends a disconnect event to upper layers/applications.
  */
 void
-mwifiex_reset_connect_state(struct mwifiex_private *priv)
+mwifiex_reset_connect_state(struct mwifiex_private *priv, u16 reason_code)
 {
 	struct mwifiex_adapter *adapter = priv->adapter;
 
@@ -117,10 +117,10 @@ mwifiex_reset_connect_state(struct mwifiex_private *priv)
 	priv->media_connected = false;
 	dev_dbg(adapter->dev,
 		"info: successfully disconnected from %pM: reason code %d\n",
-		priv->cfg_bssid, WLAN_REASON_DEAUTH_LEAVING);
+		priv->cfg_bssid, reason_code);
 	if (priv->bss_mode == NL80211_IFTYPE_STATION) {
-		cfg80211_disconnected(priv->netdev, WLAN_REASON_DEAUTH_LEAVING,
-				      NULL, 0, GFP_KERNEL);
+		cfg80211_disconnected(priv->netdev, reason_code, NULL, 0,
+				      GFP_KERNEL);
 	}
 	memset(priv->cfg_bssid, 0, ETH_ALEN);
 
@@ -186,7 +186,7 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 	struct mwifiex_adapter *adapter = priv->adapter;
 	int ret = 0;
 	u32 eventcause = adapter->event_cause;
-	u16 ctrl;
+	u16 ctrl, reason_code;
 
 	switch (eventcause) {
 	case EVENT_DUMMY_HOST_WAKEUP_SIGNAL:
@@ -204,22 +204,31 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 	case EVENT_DEAUTHENTICATED:
 		dev_dbg(adapter->dev, "event: Deauthenticated\n");
 		adapter->dbg.num_event_deauth++;
-		if (priv->media_connected)
-			mwifiex_reset_connect_state(priv);
+		if (priv->media_connected) {
+			reason_code =
+				le16_to_cpu(*(__le16 *)adapter->event_body);
+			mwifiex_reset_connect_state(priv, reason_code);
+		}
 		break;
 
 	case EVENT_DISASSOCIATED:
 		dev_dbg(adapter->dev, "event: Disassociated\n");
 		adapter->dbg.num_event_disassoc++;
-		if (priv->media_connected)
-			mwifiex_reset_connect_state(priv);
+		if (priv->media_connected) {
+			reason_code =
+				le16_to_cpu(*(__le16 *)adapter->event_body);
+			mwifiex_reset_connect_state(priv, reason_code);
+		}
 		break;
 
 	case EVENT_LINK_LOST:
 		dev_dbg(adapter->dev, "event: Link lost\n");
 		adapter->dbg.num_event_link_lost++;
-		if (priv->media_connected)
-			mwifiex_reset_connect_state(priv);
+		if (priv->media_connected) {
+			reason_code =
+				le16_to_cpu(*(__le16 *)adapter->event_body);
+			mwifiex_reset_connect_state(priv, reason_code);
+		}
 		break;
 
 	case EVENT_PS_SLEEP:
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index 540c94f8505..01dc8891070 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -2252,9 +2252,9 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 	 */
 	if (rt2x00_rt(rt2x00dev, RT3352)) {
 		rt2800_bbp_write(rt2x00dev, 27, 0x0);
-		rt2800_bbp_write(rt2x00dev, 62, 0x26 + rt2x00dev->lna_gain);
+		rt2800_bbp_write(rt2x00dev, 66, 0x26 + rt2x00dev->lna_gain);
 		rt2800_bbp_write(rt2x00dev, 27, 0x20);
-		rt2800_bbp_write(rt2x00dev, 62, 0x26 + rt2x00dev->lna_gain);
+		rt2800_bbp_write(rt2x00dev, 66, 0x26 + rt2x00dev->lna_gain);
 	} else {
 		rt2800_bbp_write(rt2x00dev, 62, 0x37 - rt2x00dev->lna_gain);
 		rt2800_bbp_write(rt2x00dev, 63, 0x37 - rt2x00dev->lna_gain);
diff --git a/drivers/net/xen-netback/netback.c b/drivers/net/xen-netback/netback.c
index 4ebfcf3d8a3..f2d6b78d901 100644
--- a/drivers/net/xen-netback/netback.c
+++ b/drivers/net/xen-netback/netback.c
@@ -335,21 +335,35 @@ unsigned int xen_netbk_count_skb_slots(struct xenvif *vif, struct sk_buff *skb)
 
 	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
 		unsigned long size = skb_frag_size(&skb_shinfo(skb)->frags[i]);
+		unsigned long offset = skb_shinfo(skb)->frags[i].page_offset;
 		unsigned long bytes;
+
+		offset &= ~PAGE_MASK;
+
 		while (size > 0) {
+			BUG_ON(offset >= PAGE_SIZE);
 			BUG_ON(copy_off > MAX_BUFFER_OFFSET);
 
-			if (start_new_rx_buffer(copy_off, size, 0)) {
+			bytes = PAGE_SIZE - offset;
+
+			if (bytes > size)
+				bytes = size;
+
+			if (start_new_rx_buffer(copy_off, bytes, 0)) {
 				count++;
 				copy_off = 0;
 			}
 
-			bytes = size;
 			if (copy_off + bytes > MAX_BUFFER_OFFSET)
 				bytes = MAX_BUFFER_OFFSET - copy_off;
 
 			copy_off += bytes;
+
+			offset += bytes;
 			size -= bytes;
+
+			if (offset == PAGE_SIZE)
+				offset = 0;
 		}
 	}
 	return count;
@@ -403,14 +417,24 @@ static void netbk_gop_frag_copy(struct xenvif *vif, struct sk_buff *skb,
 	unsigned long bytes;
 
 	/* Data must not cross a page boundary. */
-	BUG_ON(size + offset > PAGE_SIZE);
+	BUG_ON(size + offset > PAGE_SIZE<<compound_order(page));
 
 	meta = npo->meta + npo->meta_prod - 1;
 
+	/* Skip unused frames from start of page */
+	page += offset >> PAGE_SHIFT;
+	offset &= ~PAGE_MASK;
+
 	while (size > 0) {
+		BUG_ON(offset >= PAGE_SIZE);
 		BUG_ON(npo->copy_off > MAX_BUFFER_OFFSET);
 
-		if (start_new_rx_buffer(npo->copy_off, size, *head)) {
+		bytes = PAGE_SIZE - offset;
+
+		if (bytes > size)
+			bytes = size;
+
+		if (start_new_rx_buffer(npo->copy_off, bytes, *head)) {
 			/*
 			 * Netfront requires there to be some data in the head
 			 * buffer.
@@ -420,7 +444,6 @@ static void netbk_gop_frag_copy(struct xenvif *vif, struct sk_buff *skb,
 			meta = get_next_rx_buffer(vif, npo);
 		}
 
-		bytes = size;
 		if (npo->copy_off + bytes > MAX_BUFFER_OFFSET)
 			bytes = MAX_BUFFER_OFFSET - npo->copy_off;
 
@@ -453,6 +476,13 @@ static void netbk_gop_frag_copy(struct xenvif *vif, struct sk_buff *skb,
 		offset += bytes;
 		size -= bytes;
 
+		/* Next frame */
+		if (offset == PAGE_SIZE && size) {
+			BUG_ON(!PageCompound(page));
+			page++;
+			offset = 0;
+		}
+
 		/* Leave a gap for the GSO descriptor. */
 		if (*head && skb_shinfo(skb)->gso_size && !vif->gso_prefix)
 			vif->rx.req_cons++;
diff --git a/drivers/oprofile/buffer_sync.c b/drivers/oprofile/buffer_sync.c
index f34b5b29fb9..d93b2b6b1f7 100644
--- a/drivers/oprofile/buffer_sync.c
+++ b/drivers/oprofile/buffer_sync.c
@@ -216,7 +216,7 @@ static inline unsigned long fast_get_dcookie(struct path *path)
 }
 
 
-/* Look up the dcookie for the task's first VM_EXECUTABLE mapping,
+/* Look up the dcookie for the task's mm->exe_file,
  * which corresponds loosely to "application name". This is
  * not strictly necessary but allows oprofile to associate
  * shared-library samples with particular applications
@@ -224,21 +224,10 @@ static inline unsigned long fast_get_dcookie(struct path *path)
 static unsigned long get_exec_dcookie(struct mm_struct *mm)
 {
 	unsigned long cookie = NO_COOKIE;
-	struct vm_area_struct *vma;
-
-	if (!mm)
-		goto out;
 
-	for (vma = mm->mmap; vma; vma = vma->vm_next) {
-		if (!vma->vm_file)
-			continue;
-		if (!(vma->vm_flags & VM_EXECUTABLE))
-			continue;
-		cookie = fast_get_dcookie(&vma->vm_file->f_path);
-		break;
-	}
+	if (mm && mm->exe_file)
+		cookie = fast_get_dcookie(&mm->exe_file->f_path);
 
-out:
 	return cookie;
 }
 
diff --git a/drivers/parport/Kconfig b/drivers/parport/Kconfig
index d92185a5523..4b6e4e7aca8 100644
--- a/drivers/parport/Kconfig
+++ b/drivers/parport/Kconfig
@@ -36,7 +36,7 @@ if PARPORT
 config PARPORT_PC
 	tristate "PC-style hardware"
 	depends on (!SPARC64 || PCI) && !SPARC32 && !M32R && !FRV && \
-		(!M68K || ISA) && !MN10300 && !AVR32 && !BLACKFIN
+		(!M68K || ISA) && !MN10300 && !AVR32 && !BLACKFIN && !XTENSA
 	---help---
 	  You should say Y here if you have a PC-style parallel port. All
 	  IBM PC compatible computers and some Alphas have PC-style
diff --git a/drivers/pinctrl/Kconfig b/drivers/pinctrl/Kconfig
index 33e3df9e39c..7bf914df6e9 100644
--- a/drivers/pinctrl/Kconfig
+++ b/drivers/pinctrl/Kconfig
@@ -68,10 +68,21 @@ config PINCTRL_IMX6Q
 	help
 	  Say Y here to enable the imx6q pinctrl driver
 
+config PINCTRL_LANTIQ
+	bool
+	depends on LANTIQ
+	select PINMUX
+	select PINCONF
+
 config PINCTRL_PXA3xx
 	bool
 	select PINMUX
 
+config PINCTRL_FALCON
+	bool
+	depends on SOC_FALCON
+	depends on PINCTRL_LANTIQ
+
 config PINCTRL_MMP2
 	bool "MMP2 pin controller driver"
 	depends on ARCH_MMP
@@ -199,6 +210,11 @@ config PINCTRL_ARMADA_XP
 
 source "drivers/pinctrl/spear/Kconfig"
 
+config PINCTRL_XWAY
+	bool
+	depends on SOC_TYPE_XWAY
+	depends on PINCTRL_LANTIQ
+
 endmenu
 
 endif
diff --git a/drivers/pinctrl/Makefile b/drivers/pinctrl/Makefile
index f162e019630..f395ba5cec2 100644
--- a/drivers/pinctrl/Makefile
+++ b/drivers/pinctrl/Makefile
@@ -16,6 +16,7 @@ obj-$(CONFIG_PINCTRL_IMX51)	+= pinctrl-imx51.o
 obj-$(CONFIG_PINCTRL_IMX53)	+= pinctrl-imx53.o
 obj-$(CONFIG_PINCTRL_IMX6Q)	+= pinctrl-imx6q.o
 obj-$(CONFIG_PINCTRL_PXA3xx)	+= pinctrl-pxa3xx.o
+obj-$(CONFIG_PINCTRL_FALCON)	+= pinctrl-falcon.o
 obj-$(CONFIG_PINCTRL_MMP2)	+= pinctrl-mmp2.o
 obj-$(CONFIG_PINCTRL_MXS)	+= pinctrl-mxs.o
 obj-$(CONFIG_PINCTRL_IMX23)	+= pinctrl-imx23.o
@@ -40,5 +41,7 @@ obj-$(CONFIG_PINCTRL_DOVE)	+= pinctrl-dove.o
 obj-$(CONFIG_PINCTRL_KIRKWOOD)	+= pinctrl-kirkwood.o
 obj-$(CONFIG_PINCTRL_ARMADA_370) += pinctrl-armada-370.o
 obj-$(CONFIG_PINCTRL_ARMADA_XP)  += pinctrl-armada-xp.o
+obj-$(CONFIG_PINCTRL_XWAY)	+= pinctrl-xway.o
+obj-$(CONFIG_PINCTRL_LANTIQ)	+= pinctrl-lantiq.o
 
 obj-$(CONFIG_PLAT_SPEAR)	+= spear/
diff --git a/drivers/pinctrl/pinctrl-falcon.c b/drivers/pinctrl/pinctrl-falcon.c
new file mode 100644
index 00000000000..ee730590347
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-falcon.c
@@ -0,0 +1,468 @@
+/*
+ *  linux/drivers/pinctrl/pinmux-falcon.c
+ *  based on linux/drivers/pinctrl/pinmux-pxa910.c
+ *
+ *  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.
+ *
+ *  Copyright (C) 2012 Thomas Langer <thomas.langer@lantiq.com>
+ *  Copyright (C) 2012 John Crispin <blogic@openwrt.org>
+ */
+
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/export.h>
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/of_address.h>
+#include <linux/of_gpio.h>
+#include <linux/platform_device.h>
+
+#include "pinctrl-lantiq.h"
+
+#include <lantiq_soc.h>
+
+/* Multiplexer Control Register */
+#define LTQ_PADC_MUX(x)         (x * 0x4)
+/* Pull Up Enable Register */
+#define LTQ_PADC_PUEN		0x80
+/* Pull Down Enable Register */
+#define LTQ_PADC_PDEN		0x84
+/* Slew Rate Control Register */
+#define LTQ_PADC_SRC		0x88
+/* Drive Current Control Register */
+#define LTQ_PADC_DCC		0x8C
+/* Pad Control Availability Register */
+#define LTQ_PADC_AVAIL          0xF0
+
+#define pad_r32(p, reg)		ltq_r32(p + reg)
+#define pad_w32(p, val, reg)	ltq_w32(val, p + reg)
+#define pad_w32_mask(c, clear, set, reg) \
+		pad_w32(c, (pad_r32(c, reg) & ~(clear)) | (set), reg)
+
+#define pad_getbit(m, r, p)	(!!(ltq_r32(m + r) & (1 << p)))
+
+#define PORTS			5
+#define PINS			32
+#define PORT(x)                 (x / PINS)
+#define PORT_PIN(x)             (x % PINS)
+
+#define MFP_FALCON(a, f0, f1, f2, f3)		\
+{						\
+	.name = #a,				\
+	.pin = a,				\
+	.func = {				\
+		FALCON_MUX_##f0,		\
+		FALCON_MUX_##f1,		\
+		FALCON_MUX_##f2,		\
+		FALCON_MUX_##f3,		\
+	},					\
+}
+
+#define GRP_MUX(a, m, p)	\
+{				\
+	.name = a,		\
+	.mux = FALCON_MUX_##m,	\
+	.pins = p,		\
+	.npins = ARRAY_SIZE(p),	\
+}
+
+enum falcon_mux {
+	FALCON_MUX_GPIO = 0,
+	FALCON_MUX_RST,
+	FALCON_MUX_NTR,
+	FALCON_MUX_MDIO,
+	FALCON_MUX_LED,
+	FALCON_MUX_SPI,
+	FALCON_MUX_ASC,
+	FALCON_MUX_I2C,
+	FALCON_MUX_HOSTIF,
+	FALCON_MUX_SLIC,
+	FALCON_MUX_JTAG,
+	FALCON_MUX_PCM,
+	FALCON_MUX_MII,
+	FALCON_MUX_PHY,
+	FALCON_MUX_NONE = 0xffff,
+};
+
+static struct pinctrl_pin_desc falcon_pads[PORTS * PINS];
+static int pad_count[PORTS];
+
+static void lantiq_load_pin_desc(struct pinctrl_pin_desc *d, int bank, int len)
+{
+	int base = bank * PINS;
+	int i;
+
+	for (i = 0; i < len; i++) {
+		/* strlen("ioXYZ") + 1 = 6 */
+		char *name = kzalloc(6, GFP_KERNEL);
+		snprintf(name, 6, "io%d", base + i);
+		d[i].number = base + i;
+		d[i].name = name;
+	}
+	pad_count[bank] = len;
+}
+
+static struct ltq_mfp_pin falcon_mfp[] = {
+	/*	pin		f0	f1	f2	f3 */
+	MFP_FALCON(GPIO0,	RST,	GPIO,   NONE,   NONE),
+	MFP_FALCON(GPIO1,	GPIO,	GPIO,   NONE,   NONE),
+	MFP_FALCON(GPIO2,	GPIO,	GPIO,   NONE,   NONE),
+	MFP_FALCON(GPIO3,	GPIO,	GPIO,   NONE,   NONE),
+	MFP_FALCON(GPIO4,	NTR,	GPIO,   NONE,   NONE),
+	MFP_FALCON(GPIO5,	NTR,	GPIO,   NONE,   NONE),
+	MFP_FALCON(GPIO6,	RST,	GPIO,   NONE,   NONE),
+	MFP_FALCON(GPIO7,	MDIO,	GPIO,   NONE,   NONE),
+	MFP_FALCON(GPIO8,	MDIO,	GPIO,   NONE,   NONE),
+	MFP_FALCON(GPIO9,	LED,	GPIO,   NONE,   NONE),
+	MFP_FALCON(GPIO10,	LED,	GPIO,   NONE,   NONE),
+	MFP_FALCON(GPIO11,	LED,	GPIO,   NONE,   NONE),
+	MFP_FALCON(GPIO12,	LED,	GPIO,   NONE,   NONE),
+	MFP_FALCON(GPIO13,	LED,	GPIO,   NONE,   NONE),
+	MFP_FALCON(GPIO14,	LED,	GPIO,   NONE,   NONE),
+	MFP_FALCON(GPIO32,	ASC,	GPIO,   NONE,   NONE),
+	MFP_FALCON(GPIO33,	ASC,	GPIO,   NONE,   NONE),
+	MFP_FALCON(GPIO34,	SPI,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO35,	SPI,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO36,	SPI,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO37,	SPI,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO38,	SPI,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO39,	I2C,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO40,	I2C,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO41,	HOSTIF,	GPIO,	HOSTIF,	JTAG),
+	MFP_FALCON(GPIO42,	HOSTIF,	GPIO,	HOSTIF,	NONE),
+	MFP_FALCON(GPIO43,	SLIC,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO44,	SLIC,	GPIO,	PCM,	ASC),
+	MFP_FALCON(GPIO45,	SLIC,	GPIO,	PCM,	ASC),
+	MFP_FALCON(GPIO64,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO65,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO66,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO67,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO68,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO69,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO70,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO71,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO72,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO73,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO74,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO75,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO76,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO77,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO78,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO79,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO80,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO81,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO82,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO83,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO84,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO85,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO86,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO87,	MII,	GPIO,	NONE,	NONE),
+	MFP_FALCON(GPIO88,	PHY,	GPIO,	NONE,	NONE),
+};
+
+static const unsigned pins_por[] = {GPIO0};
+static const unsigned pins_ntr[] = {GPIO4};
+static const unsigned pins_ntr8k[] = {GPIO5};
+static const unsigned pins_hrst[] = {GPIO6};
+static const unsigned pins_mdio[] = {GPIO7, GPIO8};
+static const unsigned pins_bled[] = {GPIO7, GPIO10, GPIO11,
+					GPIO12, GPIO13, GPIO14};
+static const unsigned pins_asc0[] = {GPIO32, GPIO33};
+static const unsigned pins_spi[] = {GPIO34, GPIO35, GPIO36};
+static const unsigned pins_spi_cs0[] = {GPIO37};
+static const unsigned pins_spi_cs1[] = {GPIO38};
+static const unsigned pins_i2c[] = {GPIO39, GPIO40};
+static const unsigned pins_jtag[] = {GPIO41};
+static const unsigned pins_slic[] = {GPIO43, GPIO44, GPIO45};
+static const unsigned pins_pcm[] = {GPIO44, GPIO45};
+static const unsigned pins_asc1[] = {GPIO44, GPIO45};
+
+static struct ltq_pin_group falcon_grps[] = {
+	GRP_MUX("por", RST, pins_por),
+	GRP_MUX("ntr", NTR, pins_ntr),
+	GRP_MUX("ntr8k", NTR, pins_ntr8k),
+	GRP_MUX("hrst", RST, pins_hrst),
+	GRP_MUX("mdio", MDIO, pins_mdio),
+	GRP_MUX("bootled", LED, pins_bled),
+	GRP_MUX("asc0", ASC, pins_asc0),
+	GRP_MUX("spi", SPI, pins_spi),
+	GRP_MUX("spi cs0", SPI, pins_spi_cs0),
+	GRP_MUX("spi cs1", SPI, pins_spi_cs1),
+	GRP_MUX("i2c", I2C, pins_i2c),
+	GRP_MUX("jtag", JTAG, pins_jtag),
+	GRP_MUX("slic", SLIC, pins_slic),
+	GRP_MUX("pcm", PCM, pins_pcm),
+	GRP_MUX("asc1", ASC, pins_asc1),
+};
+
+static const char * const ltq_rst_grps[] = {"por", "hrst"};
+static const char * const ltq_ntr_grps[] = {"ntr", "ntr8k"};
+static const char * const ltq_mdio_grps[] = {"mdio"};
+static const char * const ltq_bled_grps[] = {"bootled"};
+static const char * const ltq_asc_grps[] = {"asc0", "asc1"};
+static const char * const ltq_spi_grps[] = {"spi", "spi cs0", "spi cs1"};
+static const char * const ltq_i2c_grps[] = {"i2c"};
+static const char * const ltq_jtag_grps[] = {"jtag"};
+static const char * const ltq_slic_grps[] = {"slic"};
+static const char * const ltq_pcm_grps[] = {"pcm"};
+
+static struct ltq_pmx_func falcon_funcs[] = {
+	{"rst",		ARRAY_AND_SIZE(ltq_rst_grps)},
+	{"ntr",		ARRAY_AND_SIZE(ltq_ntr_grps)},
+	{"mdio",	ARRAY_AND_SIZE(ltq_mdio_grps)},
+	{"led",		ARRAY_AND_SIZE(ltq_bled_grps)},
+	{"asc",		ARRAY_AND_SIZE(ltq_asc_grps)},
+	{"spi",		ARRAY_AND_SIZE(ltq_spi_grps)},
+	{"i2c",		ARRAY_AND_SIZE(ltq_i2c_grps)},
+	{"jtag",	ARRAY_AND_SIZE(ltq_jtag_grps)},
+	{"slic",	ARRAY_AND_SIZE(ltq_slic_grps)},
+	{"pcm",		ARRAY_AND_SIZE(ltq_pcm_grps)},
+};
+
+
+
+
+/* ---------  pinconf related code --------- */
+static int falcon_pinconf_group_get(struct pinctrl_dev *pctrldev,
+				unsigned group, unsigned long *config)
+{
+	return -ENOTSUPP;
+}
+
+static int falcon_pinconf_group_set(struct pinctrl_dev *pctrldev,
+				unsigned group, unsigned long config)
+{
+	return -ENOTSUPP;
+}
+
+static int falcon_pinconf_get(struct pinctrl_dev *pctrldev,
+				unsigned pin, unsigned long *config)
+{
+	struct ltq_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
+	enum ltq_pinconf_param param = LTQ_PINCONF_UNPACK_PARAM(*config);
+	void __iomem *mem = info->membase[PORT(pin)];
+
+	switch (param) {
+	case LTQ_PINCONF_PARAM_DRIVE_CURRENT:
+		*config = LTQ_PINCONF_PACK(param,
+			!!pad_getbit(mem, LTQ_PADC_DCC, PORT_PIN(pin)));
+		break;
+
+	case LTQ_PINCONF_PARAM_SLEW_RATE:
+		*config = LTQ_PINCONF_PACK(param,
+			!!pad_getbit(mem, LTQ_PADC_SRC, PORT_PIN(pin)));
+		break;
+
+	case LTQ_PINCONF_PARAM_PULL:
+		if (pad_getbit(mem, LTQ_PADC_PDEN, PORT_PIN(pin)))
+			*config = LTQ_PINCONF_PACK(param, 1);
+		else if (pad_getbit(mem, LTQ_PADC_PUEN, PORT_PIN(pin)))
+			*config = LTQ_PINCONF_PACK(param, 2);
+		else
+			*config = LTQ_PINCONF_PACK(param, 0);
+
+		break;
+
+	default:
+		return -ENOTSUPP;
+	}
+
+	return 0;
+}
+
+static int falcon_pinconf_set(struct pinctrl_dev *pctrldev,
+			unsigned pin, unsigned long config)
+{
+	enum ltq_pinconf_param param = LTQ_PINCONF_UNPACK_PARAM(config);
+	int arg = LTQ_PINCONF_UNPACK_ARG(config);
+	struct ltq_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
+	void __iomem *mem = info->membase[PORT(pin)];
+	u32 reg;
+
+	switch (param) {
+	case LTQ_PINCONF_PARAM_DRIVE_CURRENT:
+		reg = LTQ_PADC_DCC;
+		break;
+
+	case LTQ_PINCONF_PARAM_SLEW_RATE:
+		reg = LTQ_PADC_SRC;
+		break;
+
+	case LTQ_PINCONF_PARAM_PULL:
+		if (arg == 1)
+			reg = LTQ_PADC_PDEN;
+		else
+			reg = LTQ_PADC_PUEN;
+		break;
+
+	default:
+		pr_err("%s: Invalid config param %04x\n",
+		pinctrl_dev_get_name(pctrldev), param);
+		return -ENOTSUPP;
+	}
+
+	pad_w32(mem, BIT(PORT_PIN(pin)), reg);
+	if (!(pad_r32(mem, reg) & BIT(PORT_PIN(pin))))
+		return -ENOTSUPP;
+	return 0;
+}
+
+static void falcon_pinconf_dbg_show(struct pinctrl_dev *pctrldev,
+			struct seq_file *s, unsigned offset)
+{
+}
+
+static void falcon_pinconf_group_dbg_show(struct pinctrl_dev *pctrldev,
+			struct seq_file *s, unsigned selector)
+{
+}
+
+struct pinconf_ops falcon_pinconf_ops = {
+	.pin_config_get			= falcon_pinconf_get,
+	.pin_config_set			= falcon_pinconf_set,
+	.pin_config_group_get		= falcon_pinconf_group_get,
+	.pin_config_group_set		= falcon_pinconf_group_set,
+	.pin_config_dbg_show		= falcon_pinconf_dbg_show,
+	.pin_config_group_dbg_show	= falcon_pinconf_group_dbg_show,
+};
+
+static struct pinctrl_desc falcon_pctrl_desc = {
+	.owner		= THIS_MODULE,
+	.pins		= falcon_pads,
+	.confops	= &falcon_pinconf_ops,
+};
+
+static inline int falcon_mux_apply(struct pinctrl_dev *pctrldev,
+			int mfp, int mux)
+{
+	struct ltq_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
+	int port = PORT(info->mfp[mfp].pin);
+
+	if ((port >= PORTS) || (!info->membase[port]))
+		return -ENODEV;
+
+	pad_w32(info->membase[port], mux,
+		LTQ_PADC_MUX(PORT_PIN(info->mfp[mfp].pin)));
+	return 0;
+}
+
+static const struct ltq_cfg_param falcon_cfg_params[] = {
+	{"lantiq,pull",			LTQ_PINCONF_PARAM_PULL},
+	{"lantiq,drive-current",	LTQ_PINCONF_PARAM_DRIVE_CURRENT},
+	{"lantiq,slew-rate",		LTQ_PINCONF_PARAM_SLEW_RATE},
+};
+
+static struct ltq_pinmux_info falcon_info = {
+	.desc		= &falcon_pctrl_desc,
+	.apply_mux	= falcon_mux_apply,
+};
+
+
+
+
+/* --------- register the pinctrl layer --------- */
+
+int pinctrl_falcon_get_range_size(int id)
+{
+	u32 avail;
+
+	if ((id >= PORTS) || (!falcon_info.membase[id]))
+		return -EINVAL;
+
+	avail = pad_r32(falcon_info.membase[id], LTQ_PADC_AVAIL);
+
+	return fls(avail);
+}
+
+void pinctrl_falcon_add_gpio_range(struct pinctrl_gpio_range *range)
+{
+	pinctrl_add_gpio_range(falcon_info.pctrl, range);
+}
+
+static int pinctrl_falcon_probe(struct platform_device *pdev)
+{
+	struct device_node *np;
+	int pad_count = 0;
+	int ret = 0;
+
+	/* load and remap the pad resources of the different banks */
+	for_each_compatible_node(np, NULL, "lantiq,pad-falcon") {
+		struct platform_device *ppdev = of_find_device_by_node(np);
+		const __be32 *bank = of_get_property(np, "lantiq,bank", NULL);
+		struct resource res;
+		u32 avail;
+		int pins;
+
+		if (!ppdev) {
+			dev_err(&pdev->dev, "failed to find pad pdev\n");
+			continue;
+		}
+		if (!bank || *bank >= PORTS)
+			continue;
+		if (of_address_to_resource(np, 0, &res))
+			continue;
+		falcon_info.clk[*bank] = clk_get(&ppdev->dev, NULL);
+		if (IS_ERR(falcon_info.clk[*bank])) {
+			dev_err(&ppdev->dev, "failed to get clock\n");
+			return PTR_ERR(falcon_info.clk[*bank]);
+		}
+		falcon_info.membase[*bank] =
+				devm_request_and_ioremap(&pdev->dev, &res);
+		if (!falcon_info.membase[*bank]) {
+			dev_err(&pdev->dev,
+				"Failed to remap memory for bank %d\n",
+				*bank);
+			return -ENOMEM;
+		}
+		avail = pad_r32(falcon_info.membase[*bank],
+					LTQ_PADC_AVAIL);
+		pins = fls(avail);
+		lantiq_load_pin_desc(&falcon_pads[pad_count], *bank, pins);
+		pad_count += pins;
+		clk_enable(falcon_info.clk[*bank]);
+		dev_dbg(&pdev->dev, "found %s with %d pads\n",
+				res.name, pins);
+	}
+	dev_dbg(&pdev->dev, "found a total of %d pads\n", pad_count);
+	falcon_pctrl_desc.name	= dev_name(&pdev->dev);
+	falcon_pctrl_desc.npins	= pad_count;
+
+	falcon_info.mfp		= falcon_mfp;
+	falcon_info.num_mfp	= ARRAY_SIZE(falcon_mfp);
+	falcon_info.grps	= falcon_grps;
+	falcon_info.num_grps	= ARRAY_SIZE(falcon_grps);
+	falcon_info.funcs	= falcon_funcs;
+	falcon_info.num_funcs	= ARRAY_SIZE(falcon_funcs);
+
+	ret = ltq_pinctrl_register(pdev, &falcon_info);
+	if (!ret)
+		dev_info(&pdev->dev, "Init done\n");
+	return ret;
+}
+
+static const struct of_device_id falcon_match[] = {
+	{ .compatible = "lantiq,pinctrl-falcon" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, falcon_match);
+
+static struct platform_driver pinctrl_falcon_driver = {
+	.probe = pinctrl_falcon_probe,
+	.driver = {
+		.name = "pinctrl-falcon",
+		.owner = THIS_MODULE,
+		.of_match_table = falcon_match,
+	},
+};
+
+int __init pinctrl_falcon_init(void)
+{
+	return platform_driver_register(&pinctrl_falcon_driver);
+}
+
+core_initcall_sync(pinctrl_falcon_init);
diff --git a/drivers/pinctrl/pinctrl-lantiq.c b/drivers/pinctrl/pinctrl-lantiq.c
new file mode 100644
index 00000000000..07ba7682cf2
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-lantiq.c
@@ -0,0 +1,342 @@
+/*
+ *  linux/drivers/pinctrl/pinctrl-lantiq.c
+ *  based on linux/drivers/pinctrl/pinctrl-pxa3xx.c
+ *
+ *  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
+ *  publishhed by the Free Software Foundation.
+ *
+ *  Copyright (C) 2012 John Crispin <blogic@openwrt.org>
+ */
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+
+#include "pinctrl-lantiq.h"
+
+static int ltq_get_group_count(struct pinctrl_dev *pctrldev)
+{
+	struct ltq_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
+	return info->num_grps;
+}
+
+static const char *ltq_get_group_name(struct pinctrl_dev *pctrldev,
+					 unsigned selector)
+{
+	struct ltq_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
+	if (selector >= info->num_grps)
+		return NULL;
+	return info->grps[selector].name;
+}
+
+static int ltq_get_group_pins(struct pinctrl_dev *pctrldev,
+				 unsigned selector,
+				 const unsigned **pins,
+				 unsigned *num_pins)
+{
+	struct ltq_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
+	if (selector >= info->num_grps)
+		return -EINVAL;
+	*pins = info->grps[selector].pins;
+	*num_pins = info->grps[selector].npins;
+	return 0;
+}
+
+void ltq_pinctrl_dt_free_map(struct pinctrl_dev *pctldev,
+				struct pinctrl_map *map, unsigned num_maps)
+{
+	int i;
+
+	for (i = 0; i < num_maps; i++)
+		if (map[i].type == PIN_MAP_TYPE_CONFIGS_PIN)
+			kfree(map[i].data.configs.configs);
+	kfree(map);
+}
+
+static void ltq_pinctrl_pin_dbg_show(struct pinctrl_dev *pctldev,
+					struct seq_file *s,
+					unsigned offset)
+{
+	seq_printf(s, " %s", dev_name(pctldev->dev));
+}
+
+static int ltq_pinctrl_dt_subnode_to_map(struct pinctrl_dev *pctldev,
+				struct device_node *np,
+				struct pinctrl_map **map)
+{
+	struct ltq_pinmux_info *info = pinctrl_dev_get_drvdata(pctldev);
+	unsigned long configs[3];
+	unsigned num_configs = 0;
+	struct property *prop;
+	const char *group, *pin;
+	const char *function;
+	int ret, i;
+
+	ret = of_property_read_string(np, "lantiq,function", &function);
+	if (!ret) {
+		of_property_for_each_string(np, "lantiq,groups", prop, group) {
+			(*map)->type = PIN_MAP_TYPE_MUX_GROUP;
+			(*map)->name = function;
+			(*map)->data.mux.group = group;
+			(*map)->data.mux.function = function;
+			(*map)++;
+		}
+		if (of_find_property(np, "lantiq,pins", NULL))
+			dev_err(pctldev->dev,
+				"%s mixes pins and groups settings\n",
+				np->name);
+		return 0;
+	}
+
+	for (i = 0; i < info->num_params; i++) {
+		u32 val;
+		int ret = of_property_read_u32(np,
+				info->params[i].property, &val);
+		if (!ret)
+			configs[num_configs++] =
+				LTQ_PINCONF_PACK(info->params[i].param,
+				val);
+	}
+
+	if (!num_configs)
+		return -EINVAL;
+
+	of_property_for_each_string(np, "lantiq,pins", prop, pin) {
+		(*map)->data.configs.configs = kmemdup(configs,
+					num_configs * sizeof(unsigned long),
+					GFP_KERNEL);
+		(*map)->type = PIN_MAP_TYPE_CONFIGS_PIN;
+		(*map)->name = pin;
+		(*map)->data.configs.group_or_pin = pin;
+		(*map)->data.configs.num_configs = num_configs;
+		(*map)++;
+	}
+	return 0;
+}
+
+static int ltq_pinctrl_dt_subnode_size(struct device_node *np)
+{
+	int ret;
+
+	ret = of_property_count_strings(np, "lantiq,groups");
+	if (ret < 0)
+		ret = of_property_count_strings(np, "lantiq,pins");
+	return ret;
+}
+
+int ltq_pinctrl_dt_node_to_map(struct pinctrl_dev *pctldev,
+				struct device_node *np_config,
+				struct pinctrl_map **map,
+				unsigned *num_maps)
+{
+	struct pinctrl_map *tmp;
+	struct device_node *np;
+	int ret;
+
+	*num_maps = 0;
+	for_each_child_of_node(np_config, np)
+		*num_maps += ltq_pinctrl_dt_subnode_size(np);
+	*map = kzalloc(*num_maps * sizeof(struct pinctrl_map), GFP_KERNEL);
+	if (!*map)
+		return -ENOMEM;
+	tmp = *map;
+
+	for_each_child_of_node(np_config, np) {
+		ret = ltq_pinctrl_dt_subnode_to_map(pctldev, np, &tmp);
+		if (ret < 0) {
+			ltq_pinctrl_dt_free_map(pctldev, *map, *num_maps);
+			return ret;
+		}
+	}
+	return 0;
+}
+
+static struct pinctrl_ops ltq_pctrl_ops = {
+	.get_groups_count	= ltq_get_group_count,
+	.get_group_name		= ltq_get_group_name,
+	.get_group_pins		= ltq_get_group_pins,
+	.pin_dbg_show		= ltq_pinctrl_pin_dbg_show,
+	.dt_node_to_map		= ltq_pinctrl_dt_node_to_map,
+	.dt_free_map		= ltq_pinctrl_dt_free_map,
+};
+
+static int ltq_pmx_func_count(struct pinctrl_dev *pctrldev)
+{
+	struct ltq_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
+
+	return info->num_funcs;
+}
+
+static const char *ltq_pmx_func_name(struct pinctrl_dev *pctrldev,
+					 unsigned selector)
+{
+	struct ltq_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
+
+	if (selector >= info->num_funcs)
+		return NULL;
+
+	return info->funcs[selector].name;
+}
+
+static int ltq_pmx_get_groups(struct pinctrl_dev *pctrldev,
+				unsigned func,
+				const char * const **groups,
+				unsigned * const num_groups)
+{
+	struct ltq_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
+
+	*groups = info->funcs[func].groups;
+	*num_groups = info->funcs[func].num_groups;
+
+	return 0;
+}
+
+/* Return function number. If failure, return negative value. */
+static int match_mux(const struct ltq_mfp_pin *mfp, unsigned mux)
+{
+	int i;
+	for (i = 0; i < LTQ_MAX_MUX; i++) {
+		if (mfp->func[i] == mux)
+			break;
+	}
+	if (i >= LTQ_MAX_MUX)
+		return -EINVAL;
+	return i;
+}
+
+/* dont assume .mfp is linearly mapped. find the mfp with the correct .pin */
+static int match_mfp(const struct ltq_pinmux_info *info, int pin)
+{
+	int i;
+	for (i = 0; i < info->num_mfp; i++) {
+		if (info->mfp[i].pin == pin)
+			return i;
+	}
+	return -1;
+}
+
+/* check whether current pin configuration is valid. Negative for failure */
+static int match_group_mux(const struct ltq_pin_group *grp,
+			   const struct ltq_pinmux_info *info,
+			   unsigned mux)
+{
+	int i, pin, ret = 0;
+	for (i = 0; i < grp->npins; i++) {
+		pin = match_mfp(info, grp->pins[i]);
+		if (pin < 0) {
+			dev_err(info->dev, "could not find mfp for pin %d\n",
+				grp->pins[i]);
+			return -EINVAL;
+		}
+		ret = match_mux(&info->mfp[pin], mux);
+		if (ret < 0) {
+			dev_err(info->dev, "Can't find mux %d on pin%d\n",
+				mux, pin);
+			break;
+		}
+	}
+	return ret;
+}
+
+static int ltq_pmx_enable(struct pinctrl_dev *pctrldev,
+				unsigned func,
+				unsigned group)
+{
+	struct ltq_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
+	const struct ltq_pin_group *pin_grp = &info->grps[group];
+	int i, pin, pin_func, ret;
+
+	if (!pin_grp->npins ||
+		(match_group_mux(pin_grp, info, pin_grp->mux) < 0)) {
+		dev_err(info->dev, "Failed to set the pin group: %s\n",
+			info->grps[group].name);
+		return -EINVAL;
+	}
+	for (i = 0; i < pin_grp->npins; i++) {
+		pin = match_mfp(info, pin_grp->pins[i]);
+		if (pin < 0) {
+			dev_err(info->dev, "could not find mfp for pin %d\n",
+				pin_grp->pins[i]);
+			return -EINVAL;
+		}
+		pin_func = match_mux(&info->mfp[pin], pin_grp->mux);
+		ret = info->apply_mux(pctrldev, pin, pin_func);
+		if (ret) {
+			dev_err(info->dev,
+				"failed to apply mux %d for pin %d\n",
+				pin_func, pin);
+			return ret;
+		}
+	}
+	return 0;
+}
+
+static void ltq_pmx_disable(struct pinctrl_dev *pctrldev,
+				unsigned func,
+				unsigned group)
+{
+	/*
+	 * Nothing to do here. However, pinconf_check_ops() requires this
+	 * callback to be defined.
+	 */
+}
+
+static int ltq_pmx_gpio_request_enable(struct pinctrl_dev *pctrldev,
+				struct pinctrl_gpio_range *range,
+				unsigned pin)
+{
+	struct ltq_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
+	int mfp = match_mfp(info, pin + (range->id * 32));
+	int pin_func;
+
+	if (mfp < 0) {
+		dev_err(info->dev, "could not find mfp for pin %d\n", pin);
+		return -EINVAL;
+	}
+
+	pin_func = match_mux(&info->mfp[mfp], 0);
+	if (pin_func < 0) {
+		dev_err(info->dev, "No GPIO function on pin%d\n", mfp);
+		return -EINVAL;
+	}
+
+	return info->apply_mux(pctrldev, mfp, pin_func);
+}
+
+static struct pinmux_ops ltq_pmx_ops = {
+	.get_functions_count	= ltq_pmx_func_count,
+	.get_function_name	= ltq_pmx_func_name,
+	.get_function_groups	= ltq_pmx_get_groups,
+	.enable			= ltq_pmx_enable,
+	.disable		= ltq_pmx_disable,
+	.gpio_request_enable	= ltq_pmx_gpio_request_enable,
+};
+
+/*
+ * allow different socs to register with the generic part of the lanti
+ * pinctrl code
+ */
+int ltq_pinctrl_register(struct platform_device *pdev,
+				struct ltq_pinmux_info *info)
+{
+	struct pinctrl_desc *desc;
+
+	if (!info)
+		return -EINVAL;
+	desc = info->desc;
+	desc->pctlops = &ltq_pctrl_ops;
+	desc->pmxops = &ltq_pmx_ops;
+	info->dev = &pdev->dev;
+
+	info->pctrl = pinctrl_register(desc, &pdev->dev, info);
+	if (!info->pctrl) {
+		dev_err(&pdev->dev, "failed to register LTQ pinmux driver\n");
+		return -EINVAL;
+	}
+	platform_set_drvdata(pdev, info);
+	return 0;
+}
diff --git a/drivers/pinctrl/pinctrl-lantiq.h b/drivers/pinctrl/pinctrl-lantiq.h
new file mode 100644
index 00000000000..4419d32a0ad
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-lantiq.h
@@ -0,0 +1,194 @@
+/*
+ *  linux/drivers/pinctrl/pinctrl-lantiq.h
+ *  based on linux/drivers/pinctrl/pinctrl-pxa3xx.h
+ *
+ *  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
+ *  publishhed by the Free Software Foundation.
+ *
+ *  Copyright (C) 2012 John Crispin <blogic@openwrt.org>
+ */
+
+#ifndef __PINCTRL_LANTIQ_H
+
+#include <linux/clkdev.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinconf.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/pinctrl/machine.h>
+
+#include "core.h"
+
+#define ARRAY_AND_SIZE(x)	(x), ARRAY_SIZE(x)
+
+#define LTQ_MAX_MUX		4
+#define MFPR_FUNC_MASK		0x3
+
+#define LTQ_PINCONF_PACK(param, arg)		((param) << 16 | (arg))
+#define LTQ_PINCONF_UNPACK_PARAM(conf)		((conf) >> 16)
+#define LTQ_PINCONF_UNPACK_ARG(conf)		((conf) & 0xffff)
+
+enum ltq_pinconf_param {
+	LTQ_PINCONF_PARAM_PULL,
+	LTQ_PINCONF_PARAM_OPEN_DRAIN,
+	LTQ_PINCONF_PARAM_DRIVE_CURRENT,
+	LTQ_PINCONF_PARAM_SLEW_RATE,
+};
+
+struct ltq_cfg_param {
+	const char *property;
+	enum ltq_pinconf_param param;
+};
+
+struct ltq_mfp_pin {
+	const char *name;
+	const unsigned int pin;
+	const unsigned short func[LTQ_MAX_MUX];
+};
+
+struct ltq_pin_group {
+	const char *name;
+	const unsigned mux;
+	const unsigned *pins;
+	const unsigned npins;
+};
+
+struct ltq_pmx_func {
+	const char *name;
+	const char * const *groups;
+	const unsigned num_groups;
+};
+
+struct ltq_pinmux_info {
+	struct device *dev;
+	struct pinctrl_dev *pctrl;
+
+	/* we need to manage up to 5 pad controllers */
+	void __iomem *membase[5];
+
+	/* the descriptor for the subsystem */
+	struct pinctrl_desc *desc;
+
+	/* we expose our pads to the subsystem */
+	struct pinctrl_pin_desc *pads;
+
+	/* the number of pads. this varies between socs */
+	unsigned int num_pads;
+
+	/* these are our multifunction pins */
+	const struct ltq_mfp_pin *mfp;
+	unsigned int num_mfp;
+
+	/* a number of multifunction pins can be grouped together */
+	const struct ltq_pin_group *grps;
+	unsigned int num_grps;
+
+	/* a mapping between function string and id */
+	const struct ltq_pmx_func *funcs;
+	unsigned int num_funcs;
+
+	/* the pinconf options that we are able to read from the DT */
+	const struct ltq_cfg_param *params;
+	unsigned int num_params;
+
+	/* the pad controller can have a irq mapping  */
+	const unsigned *exin;
+	unsigned int num_exin;
+
+	/* we need 5 clocks max */
+	struct clk *clk[5];
+
+	/* soc specific callback used to apply muxing */
+	int (*apply_mux)(struct pinctrl_dev *pctrldev, int pin, int mux);
+};
+
+enum ltq_pin {
+	GPIO0 = 0,
+	GPIO1,
+	GPIO2,
+	GPIO3,
+	GPIO4,
+	GPIO5,
+	GPIO6,
+	GPIO7,
+	GPIO8,
+	GPIO9,
+	GPIO10, /* 10 */
+	GPIO11,
+	GPIO12,
+	GPIO13,
+	GPIO14,
+	GPIO15,
+	GPIO16,
+	GPIO17,
+	GPIO18,
+	GPIO19,
+	GPIO20, /* 20 */
+	GPIO21,
+	GPIO22,
+	GPIO23,
+	GPIO24,
+	GPIO25,
+	GPIO26,
+	GPIO27,
+	GPIO28,
+	GPIO29,
+	GPIO30, /* 30 */
+	GPIO31,
+	GPIO32,
+	GPIO33,
+	GPIO34,
+	GPIO35,
+	GPIO36,
+	GPIO37,
+	GPIO38,
+	GPIO39,
+	GPIO40, /* 40 */
+	GPIO41,
+	GPIO42,
+	GPIO43,
+	GPIO44,
+	GPIO45,
+	GPIO46,
+	GPIO47,
+	GPIO48,
+	GPIO49,
+	GPIO50, /* 50 */
+	GPIO51,
+	GPIO52,
+	GPIO53,
+	GPIO54,
+	GPIO55,
+
+	GPIO64,
+	GPIO65,
+	GPIO66,
+	GPIO67,
+	GPIO68,
+	GPIO69,
+	GPIO70,
+	GPIO71,
+	GPIO72,
+	GPIO73,
+	GPIO74,
+	GPIO75,
+	GPIO76,
+	GPIO77,
+	GPIO78,
+	GPIO79,
+	GPIO80,
+	GPIO81,
+	GPIO82,
+	GPIO83,
+	GPIO84,
+	GPIO85,
+	GPIO86,
+	GPIO87,
+	GPIO88,
+};
+
+extern int ltq_pinctrl_register(struct platform_device *pdev,
+				   struct ltq_pinmux_info *info);
+extern int ltq_pinctrl_unregister(struct platform_device *pdev);
+#endif	/* __PINCTRL_PXA3XX_H */
diff --git a/drivers/pinctrl/pinctrl-nomadik-db8500.c b/drivers/pinctrl/pinctrl-nomadik-db8500.c
index ec6209dd7c3..debaa75b055 100644
--- a/drivers/pinctrl/pinctrl-nomadik-db8500.c
+++ b/drivers/pinctrl/pinctrl-nomadik-db8500.c
@@ -725,10 +725,10 @@ static const struct nmk_pingroup nmk_db8500_groups[] = {
 	DB8500_PIN_GROUP(spi0_c_1, NMK_GPIO_ALT_C),
 	DB8500_PIN_GROUP(usbsim_c_2, NMK_GPIO_ALT_C),
 	DB8500_PIN_GROUP(i2c3_c_2, NMK_GPIO_ALT_C),
-	/* Other alt C1 column, these are still configured as alt C */
-	DB8500_PIN_GROUP(kp_oc1_1, NMK_GPIO_ALT_C),
-	DB8500_PIN_GROUP(spi2_oc1_1, NMK_GPIO_ALT_C),
-	DB8500_PIN_GROUP(spi2_oc1_2, NMK_GPIO_ALT_C),
+	/* Other alt C1 column */
+	DB8500_PIN_GROUP(kp_oc1_1, NMK_GPIO_ALT_C1),
+	DB8500_PIN_GROUP(spi2_oc1_1, NMK_GPIO_ALT_C1),
+	DB8500_PIN_GROUP(spi2_oc1_2, NMK_GPIO_ALT_C1),
 };
 
 /* We use this macro to define the groups applicable to a function */
@@ -860,6 +860,284 @@ static const struct nmk_function nmk_db8500_functions[] = {
 	FUNCTION(spi2),
 };
 
+static const struct prcm_gpiocr_altcx_pin_desc db8500_altcx_pins[] = {
+	PRCM_GPIOCR_ALTCX(23,	true, PRCM_IDX_GPIOCR1, 9,	/* STMAPE_CLK_a */
+				true, PRCM_IDX_GPIOCR1, 7,	/* SBAG_CLK_a */
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(24,	true, PRCM_IDX_GPIOCR1, 9,	/* STMAPE or U2_RXD ??? */
+				true, PRCM_IDX_GPIOCR1, 7,	/* SBAG_VAL_a */
+				true, PRCM_IDX_GPIOCR1, 10,	/* STM_MOD_CMD0 */
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(25,	true, PRCM_IDX_GPIOCR1, 9,	/* STMAPE_DAT_a[0] */
+				true, PRCM_IDX_GPIOCR1, 7,	/* SBAG_D_a[0] */
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(26,	true, PRCM_IDX_GPIOCR1, 9,	/* STMAPE_DAT_a[1] */
+				true, PRCM_IDX_GPIOCR1, 7,	/* SBAG_D_a[1] */
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(27,	true, PRCM_IDX_GPIOCR1, 9,	/* STMAPE_DAT_a[2] */
+				true, PRCM_IDX_GPIOCR1, 7,	/* SBAG_D_a[2] */
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(28,	true, PRCM_IDX_GPIOCR1, 9,	/* STMAPE_DAT_a[3] */
+				true, PRCM_IDX_GPIOCR1, 7,	/* SBAG_D_a[3] */
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(29,	false, 0, 0,
+				false, 0, 0,
+				true, PRCM_IDX_GPIOCR1, 10,	/* STM_MOD_CMD0 */
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(30,	false, 0, 0,
+				false, 0, 0,
+				true, PRCM_IDX_GPIOCR1, 10,	/* STM_MOD_CMD0 */
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(31,	false, 0, 0,
+				false, 0, 0,
+				true, PRCM_IDX_GPIOCR1, 10,	/* STM_MOD_CMD0 */
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(32,	false, 0, 0,
+				false, 0, 0,
+				true, PRCM_IDX_GPIOCR1, 10,	/* STM_MOD_CMD0 */
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(68,	true, PRCM_IDX_GPIOCR1, 18,	/* REMAP_SELECT_ON */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(69,	true, PRCM_IDX_GPIOCR1, 18,	/* REMAP_SELECT_ON */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(70,	true, PRCM_IDX_GPIOCR1, 5,	/* PTM_A9_D23 */
+				true, PRCM_IDX_GPIOCR2, 2,	/* DBG_ETM_R4_CMD0 */
+				true, PRCM_IDX_GPIOCR1, 11,	/* STM_MOD_CMD1 */
+				true, PRCM_IDX_GPIOCR1, 8	/* SBAG_CLK */
+	),
+	PRCM_GPIOCR_ALTCX(71,	true, PRCM_IDX_GPIOCR1, 5,	/* PTM_A9_D22 */
+				true, PRCM_IDX_GPIOCR2, 2,	/* DBG_ETM_R4_CMD0 */
+				true, PRCM_IDX_GPIOCR1, 11,	/* STM_MOD_CMD1 */
+				true, PRCM_IDX_GPIOCR1, 8	/* SBAG_D3 */
+	),
+	PRCM_GPIOCR_ALTCX(72,	true, PRCM_IDX_GPIOCR1, 5,	/* PTM_A9_D21 */
+				true, PRCM_IDX_GPIOCR2, 2,	/* DBG_ETM_R4_CMD0 */
+				true, PRCM_IDX_GPIOCR1, 11,	/* STM_MOD_CMD1 */
+				true, PRCM_IDX_GPIOCR1, 8	/* SBAG_D2 */
+	),
+	PRCM_GPIOCR_ALTCX(73,	true, PRCM_IDX_GPIOCR1, 5,	/* PTM_A9_D20 */
+				true, PRCM_IDX_GPIOCR2, 2,	/* DBG_ETM_R4_CMD0 */
+				true, PRCM_IDX_GPIOCR1, 11,	/* STM_MOD_CMD1 */
+				true, PRCM_IDX_GPIOCR1, 8	/* SBAG_D1 */
+	),
+	PRCM_GPIOCR_ALTCX(74,	true, PRCM_IDX_GPIOCR1, 5,	/* PTM_A9_D19 */
+				true, PRCM_IDX_GPIOCR2, 2,	/* DBG_ETM_R4_CMD0 */
+				true, PRCM_IDX_GPIOCR1, 11,	/* STM_MOD_CMD1 */
+				true, PRCM_IDX_GPIOCR1, 8	/* SBAG_D0 */
+	),
+	PRCM_GPIOCR_ALTCX(75,	true, PRCM_IDX_GPIOCR1, 5,	/* PTM_A9_D18 */
+				true, PRCM_IDX_GPIOCR2, 2,	/* DBG_ETM_R4_CMD0 */
+				true, PRCM_IDX_GPIOCR1, 0,	/* DBG_UARTMOD_CMD0 */
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(76,	true, PRCM_IDX_GPIOCR1, 5,	/* PTM_A9_D17 */
+				true, PRCM_IDX_GPIOCR2, 2,	/* DBG_ETM_R4_CMD0 */
+				true, PRCM_IDX_GPIOCR1, 0,	/* DBG_UARTMOD_CMD0 */
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(77,	true, PRCM_IDX_GPIOCR1, 5,	/* PTM_A9_D16 */
+				true, PRCM_IDX_GPIOCR2, 2,	/* DBG_ETM_R4_CMD0 */
+				false, 0, 0,
+				true, PRCM_IDX_GPIOCR1, 8	/* SBAG_VAL */
+	),
+	PRCM_GPIOCR_ALTCX(86,	true, PRCM_IDX_GPIOCR1, 12,	/* KP_O3 */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(87,	true, PRCM_IDX_GPIOCR1, 12,	/* KP_O2 */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(88,	true, PRCM_IDX_GPIOCR1, 12,	/* KP_I3 */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(89,	true, PRCM_IDX_GPIOCR1, 12,	/* KP_I2 */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(90,	true, PRCM_IDX_GPIOCR1, 12,	/* KP_O1 */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(91,	true, PRCM_IDX_GPIOCR1, 12,	/* KP_O0 */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(92,	true, PRCM_IDX_GPIOCR1, 12,	/* KP_I1 */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(93,	true, PRCM_IDX_GPIOCR1, 12,	/* KP_I0 */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(96,	true, PRCM_IDX_GPIOCR2, 3,	/* RF_INT */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(97,	true, PRCM_IDX_GPIOCR2, 1,	/* RF_CTRL */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(151,	false, 0, 0,
+				true, PRCM_IDX_GPIOCR1, 6,	/* PTM_A9_CTL */
+				true, PRCM_IDX_GPIOCR1, 15,	/* DBG_ETM_R4_CMD1*/
+				true, PRCM_IDX_GPIOCR1, 25	/* HW_OBS17 */
+	),
+	PRCM_GPIOCR_ALTCX(152,	true, PRCM_IDX_GPIOCR1, 4,	/* Hx_CLK */
+				true, PRCM_IDX_GPIOCR1, 6,	/* PTM_A9_CLK */
+				true, PRCM_IDX_GPIOCR1, 15,	/* DBG_ETM_R4_CMD1*/
+				true, PRCM_IDX_GPIOCR1, 25	/* HW_OBS16 */
+	),
+	PRCM_GPIOCR_ALTCX(153,	true, PRCM_IDX_GPIOCR1, 1,	/* UARTMOD_CMD1 */
+				true, PRCM_IDX_GPIOCR1, 14,	/* PTM_A9_D15 */
+				true, PRCM_IDX_GPIOCR1, 19,	/* DBG_ETM_R4_CMD2 */
+				true, PRCM_IDX_GPIOCR1, 25	/* HW_OBS15 */
+	),
+	PRCM_GPIOCR_ALTCX(154,	true, PRCM_IDX_GPIOCR1, 1,	/* UARTMOD_CMD1 */
+				true, PRCM_IDX_GPIOCR1, 14,	/* PTM_A9_D14 */
+				true, PRCM_IDX_GPIOCR1, 19,	/* DBG_ETM_R4_CMD2 */
+				true, PRCM_IDX_GPIOCR1, 25	/* HW_OBS14 */
+	),
+	PRCM_GPIOCR_ALTCX(155,	true, PRCM_IDX_GPIOCR1, 13,	/* STM_MOD_CMD2 */
+				true, PRCM_IDX_GPIOCR1, 14,	/* PTM_A9_D13 */
+				true, PRCM_IDX_GPIOCR1, 19,	/* DBG_ETM_R4_CMD2 */
+				true, PRCM_IDX_GPIOCR1, 25	/* HW_OBS13 */
+	),
+	PRCM_GPIOCR_ALTCX(156,	true, PRCM_IDX_GPIOCR1, 13,	/* STM_MOD_CMD2 */
+				true, PRCM_IDX_GPIOCR1, 14,	/* PTM_A9_D12 */
+				true, PRCM_IDX_GPIOCR1, 19,	/* DBG_ETM_R4_CMD2 */
+				true, PRCM_IDX_GPIOCR1, 25	/* HW_OBS12 */
+	),
+	PRCM_GPIOCR_ALTCX(157,	true, PRCM_IDX_GPIOCR1, 13,	/* STM_MOD_CMD2 */
+				true, PRCM_IDX_GPIOCR1, 14,	/* PTM_A9_D11 */
+				true, PRCM_IDX_GPIOCR1, 19,	/* DBG_ETM_R4_CMD2 */
+				true, PRCM_IDX_GPIOCR1, 25	/* HW_OBS11 */
+	),
+	PRCM_GPIOCR_ALTCX(158,	true, PRCM_IDX_GPIOCR1, 13,	/* STM_MOD_CMD2 */
+				true, PRCM_IDX_GPIOCR1, 14,	/* PTM_A9_D10 */
+				true, PRCM_IDX_GPIOCR1, 19,	/* DBG_ETM_R4_CMD2 */
+				true, PRCM_IDX_GPIOCR1, 25	/* HW_OBS10 */
+	),
+	PRCM_GPIOCR_ALTCX(159,	true, PRCM_IDX_GPIOCR1, 13,	/* STM_MOD_CMD2 */
+				true, PRCM_IDX_GPIOCR1, 14,	/* PTM_A9_D9 */
+				true, PRCM_IDX_GPIOCR1, 19,	/* DBG_ETM_R4_CMD2 */
+				true, PRCM_IDX_GPIOCR1, 25	/* HW_OBS9 */
+	),
+	PRCM_GPIOCR_ALTCX(160,	false, 0, 0,
+				true, PRCM_IDX_GPIOCR1, 14,	/* PTM_A9_D8 */
+				true, PRCM_IDX_GPIOCR1, 19,	/* DBG_ETM_R4_CMD2 */
+				true, PRCM_IDX_GPIOCR1, 25	/* HW_OBS8 */
+	),
+	PRCM_GPIOCR_ALTCX(161,	true, PRCM_IDX_GPIOCR1, 4,	/* Hx_GPIO7 */
+				true, PRCM_IDX_GPIOCR1, 6,	/* PTM_A9_D7 */
+				true, PRCM_IDX_GPIOCR1, 15,	/* DBG_ETM_R4_CMD1*/
+				true, PRCM_IDX_GPIOCR1, 24	/* HW_OBS7 */
+	),
+	PRCM_GPIOCR_ALTCX(162,	true, PRCM_IDX_GPIOCR1, 4,	/* Hx_GPIO6 */
+				true, PRCM_IDX_GPIOCR1, 6,	/* PTM_A9_D6 */
+				true, PRCM_IDX_GPIOCR1, 15,	/* DBG_ETM_R4_CMD1*/
+				true, PRCM_IDX_GPIOCR1, 24	/* HW_OBS6 */
+	),
+	PRCM_GPIOCR_ALTCX(163,	true, PRCM_IDX_GPIOCR1, 4,	/* Hx_GPIO5 */
+				true, PRCM_IDX_GPIOCR1, 6,	/* PTM_A9_D5 */
+				true, PRCM_IDX_GPIOCR1, 15,	/* DBG_ETM_R4_CMD1*/
+				true, PRCM_IDX_GPIOCR1, 24	/* HW_OBS5 */
+	),
+	PRCM_GPIOCR_ALTCX(164,	true, PRCM_IDX_GPIOCR1, 4,	/* Hx_GPIO4 */
+				true, PRCM_IDX_GPIOCR1, 6,	/* PTM_A9_D4 */
+				true, PRCM_IDX_GPIOCR1, 15,	/* DBG_ETM_R4_CMD1*/
+				true, PRCM_IDX_GPIOCR1, 24	/* HW_OBS4 */
+	),
+	PRCM_GPIOCR_ALTCX(165,	true, PRCM_IDX_GPIOCR1, 4,	/* Hx_GPIO3 */
+				true, PRCM_IDX_GPIOCR1, 6,	/* PTM_A9_D3 */
+				true, PRCM_IDX_GPIOCR1, 15,	/* DBG_ETM_R4_CMD1*/
+				true, PRCM_IDX_GPIOCR1, 24	/* HW_OBS3 */
+	),
+	PRCM_GPIOCR_ALTCX(166,	true, PRCM_IDX_GPIOCR1, 4,	/* Hx_GPIO2 */
+				true, PRCM_IDX_GPIOCR1, 6,	/* PTM_A9_D2 */
+				true, PRCM_IDX_GPIOCR1, 15,	/* DBG_ETM_R4_CMD1*/
+				true, PRCM_IDX_GPIOCR1, 24	/* HW_OBS2 */
+	),
+	PRCM_GPIOCR_ALTCX(167,	true, PRCM_IDX_GPIOCR1, 4,	/* Hx_GPIO1 */
+				true, PRCM_IDX_GPIOCR1, 6,	/* PTM_A9_D1 */
+				true, PRCM_IDX_GPIOCR1, 15,	/* DBG_ETM_R4_CMD1*/
+				true, PRCM_IDX_GPIOCR1, 24	/* HW_OBS1 */
+	),
+	PRCM_GPIOCR_ALTCX(168,	true, PRCM_IDX_GPIOCR1, 4,	/* Hx_GPIO0 */
+				true, PRCM_IDX_GPIOCR1, 6,	/* PTM_A9_D0 */
+				true, PRCM_IDX_GPIOCR1, 15,	/* DBG_ETM_R4_CMD1*/
+				true, PRCM_IDX_GPIOCR1, 24	/* HW_OBS0 */
+	),
+	PRCM_GPIOCR_ALTCX(170,	true, PRCM_IDX_GPIOCR2, 2,	/* RF_INT */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(171,	true, PRCM_IDX_GPIOCR2, 0,	/* RF_CTRL */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(215,	true, PRCM_IDX_GPIOCR1, 23,	/* SPI2_TXD */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(216,	true, PRCM_IDX_GPIOCR1, 23,	/* SPI2_FRM */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(217,	true, PRCM_IDX_GPIOCR1, 23,	/* SPI2_CLK */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(218,	true, PRCM_IDX_GPIOCR1, 23,	/* SPI2_RXD */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+};
+
+static const u16 db8500_prcm_gpiocr_regs[] = {
+	[PRCM_IDX_GPIOCR1] = 0x138,
+	[PRCM_IDX_GPIOCR2] = 0x574,
+};
+
 static const struct nmk_pinctrl_soc_data nmk_db8500_soc = {
 	.gpio_ranges = nmk_db8500_ranges,
 	.gpio_num_ranges = ARRAY_SIZE(nmk_db8500_ranges),
@@ -869,6 +1147,9 @@ static const struct nmk_pinctrl_soc_data nmk_db8500_soc = {
 	.nfunctions = ARRAY_SIZE(nmk_db8500_functions),
 	.groups = nmk_db8500_groups,
 	.ngroups = ARRAY_SIZE(nmk_db8500_groups),
+	.altcx_pins = db8500_altcx_pins,
+	.npins_altcx = ARRAY_SIZE(db8500_altcx_pins),
+	.prcm_gpiocr_registers = db8500_prcm_gpiocr_regs,
 };
 
 void __devinit
diff --git a/drivers/pinctrl/pinctrl-nomadik-db8540.c b/drivers/pinctrl/pinctrl-nomadik-db8540.c
index 3daf665c84c..52fc30181f7 100644
--- a/drivers/pinctrl/pinctrl-nomadik-db8540.c
+++ b/drivers/pinctrl/pinctrl-nomadik-db8540.c
@@ -778,50 +778,50 @@ static const struct nmk_pingroup nmk_db8540_groups[] = {
 	DB8540_PIN_GROUP(spi0_c_1, NMK_GPIO_ALT_C),
 	DB8540_PIN_GROUP(i2c3_c_1, NMK_GPIO_ALT_C),
 
-	/* Other alt C1 column, these are still configured as alt C */
-	DB8540_PIN_GROUP(spi3_oc1_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(stmape_oc1_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(u2_oc1_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(remap0_oc1_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(remap1_oc1_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(modobsrefclk_oc1_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(modobspwrctrl_oc1_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(modobsclkout_oc1_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(moduart1_oc1_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(modprcmudbg_oc1_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(modobsresout_oc1_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(modaccgpo_oc1_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(kp_oc1_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(modxmip_oc1_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(i2c6_oc1_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(u2txrx_oc1_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(u2ctsrts_oc1_1, NMK_GPIO_ALT_C),
+	/* Other alt C1 column */
+	DB8540_PIN_GROUP(spi3_oc1_1, NMK_GPIO_ALT_C1),
+	DB8540_PIN_GROUP(stmape_oc1_1, NMK_GPIO_ALT_C1),
+	DB8540_PIN_GROUP(u2_oc1_1, NMK_GPIO_ALT_C1),
+	DB8540_PIN_GROUP(remap0_oc1_1, NMK_GPIO_ALT_C1),
+	DB8540_PIN_GROUP(remap1_oc1_1, NMK_GPIO_ALT_C1),
+	DB8540_PIN_GROUP(modobsrefclk_oc1_1, NMK_GPIO_ALT_C1),
+	DB8540_PIN_GROUP(modobspwrctrl_oc1_1, NMK_GPIO_ALT_C1),
+	DB8540_PIN_GROUP(modobsclkout_oc1_1, NMK_GPIO_ALT_C1),
+	DB8540_PIN_GROUP(moduart1_oc1_1, NMK_GPIO_ALT_C1),
+	DB8540_PIN_GROUP(modprcmudbg_oc1_1, NMK_GPIO_ALT_C1),
+	DB8540_PIN_GROUP(modobsresout_oc1_1, NMK_GPIO_ALT_C1),
+	DB8540_PIN_GROUP(modaccgpo_oc1_1, NMK_GPIO_ALT_C1),
+	DB8540_PIN_GROUP(kp_oc1_1, NMK_GPIO_ALT_C1),
+	DB8540_PIN_GROUP(modxmip_oc1_1, NMK_GPIO_ALT_C1),
+	DB8540_PIN_GROUP(i2c6_oc1_1, NMK_GPIO_ALT_C1),
+	DB8540_PIN_GROUP(u2txrx_oc1_1, NMK_GPIO_ALT_C1),
+	DB8540_PIN_GROUP(u2ctsrts_oc1_1, NMK_GPIO_ALT_C1),
 
-	/* Other alt C2 column, these are still configured as alt C */
-	DB8540_PIN_GROUP(sbag_oc2_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(hxclk_oc2_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(modaccuart_oc2_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(stmmod_oc2_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(moduartstmmux_oc2_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(hxgpio_oc2_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(sbag_oc2_2, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(modobsservice_oc2_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(moduart0_oc2_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(stmape_oc2_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(u2_oc2_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(modxmip_oc2_1, NMK_GPIO_ALT_C),
+	/* Other alt C2 column */
+	DB8540_PIN_GROUP(sbag_oc2_1, NMK_GPIO_ALT_C2),
+	DB8540_PIN_GROUP(hxclk_oc2_1, NMK_GPIO_ALT_C2),
+	DB8540_PIN_GROUP(modaccuart_oc2_1, NMK_GPIO_ALT_C2),
+	DB8540_PIN_GROUP(stmmod_oc2_1, NMK_GPIO_ALT_C2),
+	DB8540_PIN_GROUP(moduartstmmux_oc2_1, NMK_GPIO_ALT_C2),
+	DB8540_PIN_GROUP(hxgpio_oc2_1, NMK_GPIO_ALT_C2),
+	DB8540_PIN_GROUP(sbag_oc2_2, NMK_GPIO_ALT_C2),
+	DB8540_PIN_GROUP(modobsservice_oc2_1, NMK_GPIO_ALT_C2),
+	DB8540_PIN_GROUP(moduart0_oc2_1, NMK_GPIO_ALT_C2),
+	DB8540_PIN_GROUP(stmape_oc2_1, NMK_GPIO_ALT_C2),
+	DB8540_PIN_GROUP(u2_oc2_1, NMK_GPIO_ALT_C2),
+	DB8540_PIN_GROUP(modxmip_oc2_1, NMK_GPIO_ALT_C2),
 
-	/* Other alt C3 column, these are still configured as alt C */
-	DB8540_PIN_GROUP(modaccgpo_oc3_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(tpui_oc3_1, NMK_GPIO_ALT_C),
+	/* Other alt C3 column */
+	DB8540_PIN_GROUP(modaccgpo_oc3_1, NMK_GPIO_ALT_C3),
+	DB8540_PIN_GROUP(tpui_oc3_1, NMK_GPIO_ALT_C3),
 
-	/* Other alt C4 column, these are still configured as alt C */
-	DB8540_PIN_GROUP(hwobs_oc4_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(moduart1txrx_oc4_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(moduart1rtscts_oc4_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(modaccuarttxrx_oc4_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(modaccuartrtscts_oc4_1, NMK_GPIO_ALT_C),
-	DB8540_PIN_GROUP(stmmod_oc4_1, NMK_GPIO_ALT_C),
+	/* Other alt C4 column */
+	DB8540_PIN_GROUP(hwobs_oc4_1, NMK_GPIO_ALT_C4),
+	DB8540_PIN_GROUP(moduart1txrx_oc4_1, NMK_GPIO_ALT_C4),
+	DB8540_PIN_GROUP(moduart1rtscts_oc4_1, NMK_GPIO_ALT_C4),
+	DB8540_PIN_GROUP(modaccuarttxrx_oc4_1, NMK_GPIO_ALT_C4),
+	DB8540_PIN_GROUP(modaccuartrtscts_oc4_1, NMK_GPIO_ALT_C4),
+	DB8540_PIN_GROUP(stmmod_oc4_1, NMK_GPIO_ALT_C4),
 
 };
 
@@ -981,6 +981,265 @@ static const struct nmk_function nmk_db8540_functions[] = {
 	FUNCTION(usb)
 };
 
+static const struct prcm_gpiocr_altcx_pin_desc db8540_altcx_pins[] = {
+	PRCM_GPIOCR_ALTCX(8,	true, PRCM_IDX_GPIOCR1, 20,	/* SPI3_CLK */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(9,	true, PRCM_IDX_GPIOCR1, 20,	/* SPI3_RXD */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(10,	true, PRCM_IDX_GPIOCR1, 20,	/* SPI3_FRM */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(11,	true, PRCM_IDX_GPIOCR1, 20,	/* SPI3_TXD */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(23,	true, PRCM_IDX_GPIOCR1, 9,	/* STMAPE_CLK_a */
+				true, PRCM_IDX_GPIOCR2, 10,	/* SBAG_CLK_a */
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(24,	true, PRCM_IDX_GPIOCR3, 30,	/* U2_RXD_g */
+				true, PRCM_IDX_GPIOCR2, 10,	/* SBAG_VAL_a */
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(25,	true, PRCM_IDX_GPIOCR1, 9,	/* STMAPE_DAT_a[0] */
+				true, PRCM_IDX_GPIOCR2, 10,	/* SBAG_D_a[0] */
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(26,	true, PRCM_IDX_GPIOCR1, 9,	/* STMAPE_DAT_a[1] */
+				true, PRCM_IDX_GPIOCR2, 10,	/* SBAG_D_a[1] */
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(27,	true, PRCM_IDX_GPIOCR1, 9,	/* STMAPE_DAT_a[2] */
+				true, PRCM_IDX_GPIOCR2, 10,	/* SBAG_D_a[2] */
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(28,	true, PRCM_IDX_GPIOCR1, 9,	/* STMAPE_DAT_a[3] */
+				true, PRCM_IDX_GPIOCR2, 10,	/* SBAG_D_a[3] */
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(64,	true, PRCM_IDX_GPIOCR1, 15,	/* MODOBS_REFCLK_REQ */
+				false, 0, 0,
+				true, PRCM_IDX_GPIOCR1, 2,	/* TPIU_CTL */
+				true, PRCM_IDX_GPIOCR2, 23	/* HW_OBS_APE_PRCMU[17] */
+	),
+	PRCM_GPIOCR_ALTCX(65,	true, PRCM_IDX_GPIOCR1, 19,	/* MODOBS_PWRCTRL0 */
+				true, PRCM_IDX_GPIOCR1, 24,	/* Hx_CLK */
+				true, PRCM_IDX_GPIOCR1, 2,	/* TPIU_CLK */
+				true, PRCM_IDX_GPIOCR2, 24	/* HW_OBS_APE_PRCMU[16] */
+	),
+	PRCM_GPIOCR_ALTCX(66,	true, PRCM_IDX_GPIOCR1, 15,	/* MODOBS_CLKOUT1 */
+				false, 0, 0,
+				true, PRCM_IDX_GPIOCR1, 2,	/* TPIU_D[15] */
+				true, PRCM_IDX_GPIOCR2, 25	/* HW_OBS_APE_PRCMU[15] */
+	),
+	PRCM_GPIOCR_ALTCX(67,	true, PRCM_IDX_GPIOCR1, 1,	/* MODUART1_TXD_a */
+				true, PRCM_IDX_GPIOCR1, 6,	/* MODACCUART_TXD_a */
+				true, PRCM_IDX_GPIOCR1, 2,	/* TPIU_D[14] */
+				true, PRCM_IDX_GPIOCR2, 26	/* HW_OBS_APE_PRCMU[14] */
+	),
+	PRCM_GPIOCR_ALTCX(70,	true, PRCM_IDX_GPIOCR3, 6,	/* MOD_PRCMU_DEBUG[17] */
+				true, PRCM_IDX_GPIOCR1, 10,	/* STMMOD_CLK_b */
+				true, PRCM_IDX_GPIOCR1, 2,	/* TPIU_D[13] */
+				true, PRCM_IDX_GPIOCR2, 27	/* HW_OBS_APE_PRCMU[13] */
+	),
+	PRCM_GPIOCR_ALTCX(71,	true, PRCM_IDX_GPIOCR3, 6,	/* MOD_PRCMU_DEBUG[16] */
+				true, PRCM_IDX_GPIOCR1, 10,	/* STMMOD_DAT_b[3] */
+				true, PRCM_IDX_GPIOCR1, 2,	/* TPIU_D[12] */
+				true, PRCM_IDX_GPIOCR2, 27	/* HW_OBS_APE_PRCMU[12] */
+	),
+	PRCM_GPIOCR_ALTCX(72,	true, PRCM_IDX_GPIOCR3, 6,	/* MOD_PRCMU_DEBUG[15] */
+				true, PRCM_IDX_GPIOCR1, 10,	/* STMMOD_DAT_b[2] */
+				true, PRCM_IDX_GPIOCR1, 2,	/* TPIU_D[11] */
+				true, PRCM_IDX_GPIOCR2, 27	/* HW_OBS_APE_PRCMU[11] */
+	),
+	PRCM_GPIOCR_ALTCX(73,	true, PRCM_IDX_GPIOCR3, 6,	/* MOD_PRCMU_DEBUG[14] */
+				true, PRCM_IDX_GPIOCR1, 10,	/* STMMOD_DAT_b[1] */
+				true, PRCM_IDX_GPIOCR1, 2,	/* TPIU_D[10] */
+				true, PRCM_IDX_GPIOCR2, 27	/* HW_OBS_APE_PRCMU[10] */
+	),
+	PRCM_GPIOCR_ALTCX(74,	true, PRCM_IDX_GPIOCR3, 6,	/* MOD_PRCMU_DEBUG[13] */
+				true, PRCM_IDX_GPIOCR1, 10,	/* STMMOD_DAT_b[0] */
+				true, PRCM_IDX_GPIOCR1, 2,	/* TPIU_D[9] */
+				true, PRCM_IDX_GPIOCR2, 27	/* HW_OBS_APE_PRCMU[9] */
+	),
+	PRCM_GPIOCR_ALTCX(75,	true, PRCM_IDX_GPIOCR1, 12,	/* MODOBS_RESOUT0_N */
+				true, PRCM_IDX_GPIOCR2, 1,	/* MODUART_STMMUX_RXD_b */
+				true, PRCM_IDX_GPIOCR1, 2,	/* TPIU_D[8] */
+				true, PRCM_IDX_GPIOCR2, 28	/* HW_OBS_APE_PRCMU[8] */
+	),
+	PRCM_GPIOCR_ALTCX(76,	true, PRCM_IDX_GPIOCR3, 7,	/* MOD_PRCMU_DEBUG[12] */
+				true, PRCM_IDX_GPIOCR1, 25,	/* Hx_GPIO[7] */
+				true, PRCM_IDX_GPIOCR1, 2,	/* TPIU_D[7] */
+				true, PRCM_IDX_GPIOCR2, 29	/* HW_OBS_APE_PRCMU[7] */
+	),
+	PRCM_GPIOCR_ALTCX(77,	true, PRCM_IDX_GPIOCR3, 7,	/* MOD_PRCMU_DEBUG[11] */
+				true, PRCM_IDX_GPIOCR1, 25,	/* Hx_GPIO[6] */
+				true, PRCM_IDX_GPIOCR1, 2,	/* TPIU_D[6] */
+				true, PRCM_IDX_GPIOCR2, 29	/* HW_OBS_APE_PRCMU[6] */
+	),
+	PRCM_GPIOCR_ALTCX(78,	true, PRCM_IDX_GPIOCR3, 7,	/* MOD_PRCMU_DEBUG[10] */
+				true, PRCM_IDX_GPIOCR1, 25,	/* Hx_GPIO[5] */
+				true, PRCM_IDX_GPIOCR1, 2,	/* TPIU_D[5] */
+				true, PRCM_IDX_GPIOCR2, 29	/* HW_OBS_APE_PRCMU[5] */
+	),
+	PRCM_GPIOCR_ALTCX(79,	true, PRCM_IDX_GPIOCR3, 7,	/* MOD_PRCMU_DEBUG[9] */
+				true, PRCM_IDX_GPIOCR1, 25,	/* Hx_GPIO[4] */
+				true, PRCM_IDX_GPIOCR1, 2,	/* TPIU_D[4] */
+				true, PRCM_IDX_GPIOCR2, 29	/* HW_OBS_APE_PRCMU[4] */
+	),
+	PRCM_GPIOCR_ALTCX(80,	true, PRCM_IDX_GPIOCR1, 26,	/* MODACC_GPO[0] */
+				true, PRCM_IDX_GPIOCR1, 25,	/* Hx_GPIO[3] */
+				true, PRCM_IDX_GPIOCR1, 2,	/* TPIU_D[3] */
+				true, PRCM_IDX_GPIOCR2, 30	/* HW_OBS_APE_PRCMU[3] */
+	),
+	PRCM_GPIOCR_ALTCX(81,	true, PRCM_IDX_GPIOCR2, 17,	/* MODACC_GPO[1] */
+				true, PRCM_IDX_GPIOCR1, 25,	/* Hx_GPIO[2] */
+				true, PRCM_IDX_GPIOCR1, 2,	/* TPIU_D[2] */
+				true, PRCM_IDX_GPIOCR2, 30	/* HW_OBS_APE_PRCMU[2] */
+	),
+	PRCM_GPIOCR_ALTCX(82,	true, PRCM_IDX_GPIOCR3, 8,	/* MOD_PRCMU_DEBUG[8] */
+				true, PRCM_IDX_GPIOCR1, 25,	/* Hx_GPIO[1] */
+				true, PRCM_IDX_GPIOCR1, 2,	/* TPIU_D[1] */
+				true, PRCM_IDX_GPIOCR2, 31	/* HW_OBS_APE_PRCMU[1] */
+	),
+	PRCM_GPIOCR_ALTCX(83,	true, PRCM_IDX_GPIOCR3, 8,	/* MOD_PRCMU_DEBUG[7] */
+				true, PRCM_IDX_GPIOCR1, 25,	/* Hx_GPIO[0] */
+				true, PRCM_IDX_GPIOCR1, 2,	/* TPIU_D[0] */
+				true, PRCM_IDX_GPIOCR2, 31	/* HW_OBS_APE_PRCMU[0] */
+	),
+	PRCM_GPIOCR_ALTCX(84,	true, PRCM_IDX_GPIOCR3, 9,	/* MOD_PRCMU_DEBUG[6] */
+				true, PRCM_IDX_GPIOCR1, 8,	/* SBAG_CLK_b */
+				true, PRCM_IDX_GPIOCR1, 3,	/* TPIU_D[23] */
+				true, PRCM_IDX_GPIOCR1, 16	/* MODUART1_RXD_b */
+	),
+	PRCM_GPIOCR_ALTCX(85,	true, PRCM_IDX_GPIOCR3, 9,	/* MOD_PRCMU_DEBUG[5] */
+				true, PRCM_IDX_GPIOCR1, 8,	/* SBAG_D_b[3] */
+				true, PRCM_IDX_GPIOCR1, 3,	/* TPIU_D[22] */
+				true, PRCM_IDX_GPIOCR1, 16	/* MODUART1_TXD_b */
+	),
+	PRCM_GPIOCR_ALTCX(86,	true, PRCM_IDX_GPIOCR3, 9,	/* MOD_PRCMU_DEBUG[0] */
+				true, PRCM_IDX_GPIOCR2, 18,	/* STMAPE_DAT_b[0] */
+				true, PRCM_IDX_GPIOCR1, 14,	/* TPIU_D[25] */
+				true, PRCM_IDX_GPIOCR1, 11	/* STMMOD_DAT_c[0] */
+	),
+	PRCM_GPIOCR_ALTCX(87,	true, PRCM_IDX_GPIOCR3, 0,	/* MODACC_GPO_a[5] */
+				true, PRCM_IDX_GPIOCR2, 3,	/* U2_RXD_c */
+				true, PRCM_IDX_GPIOCR1, 4,	/* TPIU_D[24] */
+				true, PRCM_IDX_GPIOCR1, 21	/* MODUART_STMMUX_RXD_c */
+	),
+	PRCM_GPIOCR_ALTCX(151,	true, PRCM_IDX_GPIOCR1, 18,	/* REMAP0 */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(152,	true, PRCM_IDX_GPIOCR1, 18,	/* REMAP1 */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(153,	true, PRCM_IDX_GPIOCR3, 2,	/* KP_O_b[6] */
+				true, PRCM_IDX_GPIOCR1, 8,	/* SBAG_D_b[2] */
+				true, PRCM_IDX_GPIOCR1, 3,	/* TPIU_D[21] */
+				true, PRCM_IDX_GPIOCR1, 0	/* MODUART1_RTS */
+	),
+	PRCM_GPIOCR_ALTCX(154,	true, PRCM_IDX_GPIOCR3, 2,	/* KP_I_b[6] */
+				true, PRCM_IDX_GPIOCR1, 8,	/* SBAG_D_b[1] */
+				true, PRCM_IDX_GPIOCR1, 3,	/* TPIU_D[20] */
+				true, PRCM_IDX_GPIOCR1, 0	/* MODUART1_CTS */
+	),
+	PRCM_GPIOCR_ALTCX(155,	true, PRCM_IDX_GPIOCR3, 3,	/* KP_O_b[5] */
+				true, PRCM_IDX_GPIOCR1, 8,	/* SBAG_D_b[0] */
+				true, PRCM_IDX_GPIOCR1, 3,	/* TPIU_D[19] */
+				true, PRCM_IDX_GPIOCR1, 5	/* MODACCUART_RXD_c */
+	),
+	PRCM_GPIOCR_ALTCX(156,	true, PRCM_IDX_GPIOCR3, 3,	/* KP_O_b[4] */
+				true, PRCM_IDX_GPIOCR1, 8,	/* SBAG_VAL_b */
+				true, PRCM_IDX_GPIOCR1, 3,	/* TPIU_D[18] */
+				true, PRCM_IDX_GPIOCR1, 5	/* MODACCUART_TXD_b */
+	),
+	PRCM_GPIOCR_ALTCX(157,	true, PRCM_IDX_GPIOCR3, 4,	/* KP_I_b[5] */
+				true, PRCM_IDX_GPIOCR1, 23,	/* MODOBS_SERVICE_N */
+				true, PRCM_IDX_GPIOCR1, 3,	/* TPIU_D[17] */
+				true, PRCM_IDX_GPIOCR1, 14	/* MODACCUART_RTS */
+	),
+	PRCM_GPIOCR_ALTCX(158,	true, PRCM_IDX_GPIOCR3, 4,	/* KP_I_b[4] */
+				true, PRCM_IDX_GPIOCR2, 0,	/* U2_TXD_c */
+				true, PRCM_IDX_GPIOCR1, 3,	/* TPIU_D[16] */
+				true, PRCM_IDX_GPIOCR1, 14	/* MODACCUART_CTS */
+	),
+	PRCM_GPIOCR_ALTCX(159,	true, PRCM_IDX_GPIOCR3, 5,	/* KP_O_b[3] */
+				true, PRCM_IDX_GPIOCR3, 10,	/* MODUART0_RXD */
+				true, PRCM_IDX_GPIOCR1, 4,	/* TPIU_D[31] */
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(160,	true, PRCM_IDX_GPIOCR3, 5,	/* KP_I_b[3] */
+				true, PRCM_IDX_GPIOCR3, 10,	/* MODUART0_TXD */
+				true, PRCM_IDX_GPIOCR1, 4,	/* TPIU_D[30] */
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(161,	true, PRCM_IDX_GPIOCR3, 9,	/* MOD_PRCMU_DEBUG[4] */
+				true, PRCM_IDX_GPIOCR2, 18,	/* STMAPE_CLK_b */
+				true, PRCM_IDX_GPIOCR1, 4,	/* TPIU_D[29] */
+				true, PRCM_IDX_GPIOCR1, 11	/* STMMOD_CLK_c */
+	),
+	PRCM_GPIOCR_ALTCX(162,	true, PRCM_IDX_GPIOCR3, 9,	/* MOD_PRCMU_DEBUG[3] */
+				true, PRCM_IDX_GPIOCR2, 18,	/* STMAPE_DAT_b[3] */
+				true, PRCM_IDX_GPIOCR1, 4,	/* TPIU_D[28] */
+				true, PRCM_IDX_GPIOCR1, 11	/* STMMOD_DAT_c[3] */
+	),
+	PRCM_GPIOCR_ALTCX(163,	true, PRCM_IDX_GPIOCR3, 9,	/* MOD_PRCMU_DEBUG[2] */
+				true, PRCM_IDX_GPIOCR2, 18,	/* STMAPE_DAT_b[2] */
+				true, PRCM_IDX_GPIOCR1, 4,	/* TPIU_D[27] */
+				true, PRCM_IDX_GPIOCR1, 11	/* STMMOD_DAT_c[2] */
+	),
+	PRCM_GPIOCR_ALTCX(164,	true, PRCM_IDX_GPIOCR3, 9,	/* MOD_PRCMU_DEBUG[1] */
+				true, PRCM_IDX_GPIOCR2, 18,	/* STMAPE_DAT_b[1] */
+				true, PRCM_IDX_GPIOCR1, 4,	/* TPIU_D[26] */
+				true, PRCM_IDX_GPIOCR1, 11	/* STMMOD_DAT_c[1] */
+	),
+	PRCM_GPIOCR_ALTCX(204,	true, PRCM_IDX_GPIOCR2, 2,	/* U2_RXD_f */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(205,	true, PRCM_IDX_GPIOCR2, 2,	/* U2_TXD_f */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(206,	true, PRCM_IDX_GPIOCR2, 2,	/* U2_CTSn_b */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+	PRCM_GPIOCR_ALTCX(207,	true, PRCM_IDX_GPIOCR2, 2,	/* U2_RTSn_b */
+				false, 0, 0,
+				false, 0, 0,
+				false, 0, 0
+	),
+};
+
+static const u16 db8540_prcm_gpiocr_regs[] = {
+	[PRCM_IDX_GPIOCR1] = 0x138,
+	[PRCM_IDX_GPIOCR2] = 0x574,
+	[PRCM_IDX_GPIOCR3] = 0x2bc,
+};
+
 static const struct nmk_pinctrl_soc_data nmk_db8540_soc = {
 	.gpio_ranges = nmk_db8540_ranges,
 	.gpio_num_ranges = ARRAY_SIZE(nmk_db8540_ranges),
@@ -990,6 +1249,9 @@ static const struct nmk_pinctrl_soc_data nmk_db8540_soc = {
 	.nfunctions = ARRAY_SIZE(nmk_db8540_functions),
 	.groups = nmk_db8540_groups,
 	.ngroups = ARRAY_SIZE(nmk_db8540_groups),
+	.altcx_pins = db8540_altcx_pins,
+	.npins_altcx = ARRAY_SIZE(db8540_altcx_pins),
+	.prcm_gpiocr_registers = db8540_prcm_gpiocr_regs,
 };
 
 void __devinit
diff --git a/drivers/pinctrl/pinctrl-nomadik.c b/drivers/pinctrl/pinctrl-nomadik.c
index 6030a513f3c..fec9c30133d 100644
--- a/drivers/pinctrl/pinctrl-nomadik.c
+++ b/drivers/pinctrl/pinctrl-nomadik.c
@@ -30,6 +30,7 @@
 #include <linux/pinctrl/pinconf.h>
 /* Since we request GPIOs from ourself */
 #include <linux/pinctrl/consumer.h>
+#include <linux/mfd/dbx500-prcmu.h>
 
 #include <asm/mach/irq.h>
 
@@ -237,6 +238,89 @@ nmk_gpio_disable_lazy_irq(struct nmk_gpio_chip *nmk_chip, unsigned offset)
 	dev_dbg(nmk_chip->chip.dev, "%d: clearing interrupt mask\n", gpio);
 }
 
+static void nmk_prcm_altcx_set_mode(struct nmk_pinctrl *npct,
+	unsigned offset, unsigned alt_num)
+{
+	int i;
+	u16 reg;
+	u8 bit;
+	u8 alt_index;
+	const struct prcm_gpiocr_altcx_pin_desc *pin_desc;
+	const u16 *gpiocr_regs;
+
+	if (alt_num > PRCM_IDX_GPIOCR_ALTC_MAX) {
+		dev_err(npct->dev, "PRCM GPIOCR: alternate-C%i is invalid\n",
+			alt_num);
+		return;
+	}
+
+	for (i = 0 ; i < npct->soc->npins_altcx ; i++) {
+		if (npct->soc->altcx_pins[i].pin == offset)
+			break;
+	}
+	if (i == npct->soc->npins_altcx) {
+		dev_dbg(npct->dev, "PRCM GPIOCR: pin %i is not found\n",
+			offset);
+		return;
+	}
+
+	pin_desc = npct->soc->altcx_pins + i;
+	gpiocr_regs = npct->soc->prcm_gpiocr_registers;
+
+	/*
+	 * If alt_num is NULL, just clear current ALTCx selection
+	 * to make sure we come back to a pure ALTC selection
+	 */
+	if (!alt_num) {
+		for (i = 0 ; i < PRCM_IDX_GPIOCR_ALTC_MAX ; i++) {
+			if (pin_desc->altcx[i].used == true) {
+				reg = gpiocr_regs[pin_desc->altcx[i].reg_index];
+				bit = pin_desc->altcx[i].control_bit;
+				if (prcmu_read(reg) & BIT(bit)) {
+					prcmu_write_masked(reg, BIT(bit), 0);
+					dev_dbg(npct->dev,
+						"PRCM GPIOCR: pin %i: alternate-C%i has been disabled\n",
+						offset, i+1);
+				}
+			}
+		}
+		return;
+	}
+
+	alt_index = alt_num - 1;
+	if (pin_desc->altcx[alt_index].used == false) {
+		dev_warn(npct->dev,
+			"PRCM GPIOCR: pin %i: alternate-C%i does not exist\n",
+			offset, alt_num);
+		return;
+	}
+
+	/*
+	 * Check if any other ALTCx functions are activated on this pin
+	 * and disable it first.
+	 */
+	for (i = 0 ; i < PRCM_IDX_GPIOCR_ALTC_MAX ; i++) {
+		if (i == alt_index)
+			continue;
+		if (pin_desc->altcx[i].used == true) {
+			reg = gpiocr_regs[pin_desc->altcx[i].reg_index];
+			bit = pin_desc->altcx[i].control_bit;
+			if (prcmu_read(reg) & BIT(bit)) {
+				prcmu_write_masked(reg, BIT(bit), 0);
+				dev_dbg(npct->dev,
+					"PRCM GPIOCR: pin %i: alternate-C%i has been disabled\n",
+					offset, i+1);
+			}
+		}
+	}
+
+	reg = gpiocr_regs[pin_desc->altcx[alt_index].reg_index];
+	bit = pin_desc->altcx[alt_index].control_bit;
+	dev_dbg(npct->dev, "PRCM GPIOCR: pin %i: alternate-C%i has been selected\n",
+		offset, alt_index+1);
+	prcmu_write_masked(reg, BIT(bit), BIT(bit));
+}
+
 static void __nmk_config_pin(struct nmk_gpio_chip *nmk_chip, unsigned offset,
 			     pin_cfg_t cfg, bool sleep, unsigned int *slpmregs)
 {
@@ -1287,9 +1371,19 @@ static int __devinit nmk_gpio_probe(struct platform_device *dev)
 
 	platform_set_drvdata(dev, nmk_chip);
 
-	nmk_chip->domain = irq_domain_add_legacy(np, NMK_GPIO_PER_CHIP,
-						NOMADIK_GPIO_TO_IRQ(pdata->first_gpio),
-						0, &nmk_gpio_irq_simple_ops, nmk_chip);
+	if (np) {
+		/* The DT case will just grab a set of IRQ numbers */
+		nmk_chip->domain = irq_domain_add_linear(np, NMK_GPIO_PER_CHIP,
+				&nmk_gpio_irq_simple_ops, nmk_chip);
+	} else {
+		/* Non-DT legacy mode, use hardwired IRQ numbers */
+		int irq_start;
+
+		irq_start = NOMADIK_GPIO_TO_IRQ(pdata->first_gpio);
+		nmk_chip->domain = irq_domain_add_simple(NULL,
+				NMK_GPIO_PER_CHIP, irq_start,
+				&nmk_gpio_irq_simple_ops, nmk_chip);
+	}
 	if (!nmk_chip->domain) {
 		dev_err(&dev->dev, "failed to create irqdomain\n");
 		ret = -ENOSYS;
@@ -1441,7 +1535,7 @@ static int nmk_pmx_enable(struct pinctrl_dev *pctldev, unsigned function,
 	 * IOFORCE will switch *all* ports to their sleepmode setting to as
 	 * to avoid glitches. (Not just one port!)
 	 */
-	glitch = (g->altsetting == NMK_GPIO_ALT_C);
+	glitch = ((g->altsetting & NMK_GPIO_ALT_C) == NMK_GPIO_ALT_C);
 
 	if (glitch) {
 		spin_lock_irqsave(&nmk_gpio_slpm_lock, flags);
@@ -1491,8 +1585,21 @@ static int nmk_pmx_enable(struct pinctrl_dev *pctldev, unsigned function,
 		 */
 		nmk_gpio_disable_lazy_irq(nmk_chip, bit);
 
-		__nmk_gpio_set_mode_safe(nmk_chip, bit, g->altsetting, glitch);
+		__nmk_gpio_set_mode_safe(nmk_chip, bit,
+			(g->altsetting & NMK_GPIO_ALT_C), glitch);
 		clk_disable(nmk_chip->clk);
+
+		/*
+		 * Call PRCM GPIOCR config function in case ALTC
+		 * has been selected:
+		 * - If selection is a ALTCx, some bits in PRCM GPIOCR registers
+		 *   must be set.
+		 * - If selection is pure ALTC and previous selection was ALTCx,
+		 *   then some bits in PRCM GPIOCR registers must be cleared.
+		 */
+		if ((g->altsetting & NMK_GPIO_ALT_C) == NMK_GPIO_ALT_C)
+			nmk_prcm_altcx_set_mode(npct, g->pins[i],
+				g->altsetting >> NMK_GPIO_ALT_CX_SHIFT);
 	}
 
 	/* When all pins are successfully reconfigured we get here */
diff --git a/drivers/pinctrl/pinctrl-nomadik.h b/drivers/pinctrl/pinctrl-nomadik.h
index 5c99f1c62df..eef316e979a 100644
--- a/drivers/pinctrl/pinctrl-nomadik.h
+++ b/drivers/pinctrl/pinctrl-nomadik.h
@@ -8,6 +8,78 @@
 #define PINCTRL_NMK_DB8500	1
 #define PINCTRL_NMK_DB8540	2
 
+#define PRCM_GPIOCR_ALTCX(pin_num,\
+	altc1_used, altc1_ri, altc1_cb,\
+	altc2_used, altc2_ri, altc2_cb,\
+	altc3_used, altc3_ri, altc3_cb,\
+	altc4_used, altc4_ri, altc4_cb)\
+{\
+	.pin = pin_num,\
+	.altcx[PRCM_IDX_GPIOCR_ALTC1] = {\
+		.used = altc1_used,\
+		.reg_index = altc1_ri,\
+		.control_bit = altc1_cb\
+	},\
+	.altcx[PRCM_IDX_GPIOCR_ALTC2] = {\
+		.used = altc2_used,\
+		.reg_index = altc2_ri,\
+		.control_bit = altc2_cb\
+	},\
+	.altcx[PRCM_IDX_GPIOCR_ALTC3] = {\
+		.used = altc3_used,\
+		.reg_index = altc3_ri,\
+		.control_bit = altc3_cb\
+	},\
+	.altcx[PRCM_IDX_GPIOCR_ALTC4] = {\
+		.used = altc4_used,\
+		.reg_index = altc4_ri,\
+		.control_bit = altc4_cb\
+	},\
+}
+
+/**
+ * enum prcm_gpiocr_reg_index
+ * Used to reference an PRCM GPIOCR register address.
+ */
+enum prcm_gpiocr_reg_index {
+	PRCM_IDX_GPIOCR1,
+	PRCM_IDX_GPIOCR2,
+	PRCM_IDX_GPIOCR3
+};
+/**
+ * enum prcm_gpiocr_altcx_index
+ * Used to reference an Other alternate-C function.
+ */
+enum prcm_gpiocr_altcx_index {
+	PRCM_IDX_GPIOCR_ALTC1,
+	PRCM_IDX_GPIOCR_ALTC2,
+	PRCM_IDX_GPIOCR_ALTC3,
+	PRCM_IDX_GPIOCR_ALTC4,
+	PRCM_IDX_GPIOCR_ALTC_MAX,
+};
+
+/**
+ * struct prcm_gpio_altcx - Other alternate-C function
+ * @used: other alternate-C function availability
+ * @reg_index: PRCM GPIOCR register index used to control the function
+ * @control_bit: PRCM GPIOCR bit used to control the function
+ */
+struct prcm_gpiocr_altcx {
+	bool used:1;
+	u8 reg_index:2;
+	u8 control_bit:5;
+} __packed;
+
+/**
+ * struct prcm_gpio_altcx_pin_desc - Other alternate-C pin
+ * @pin: The pin number
+ * @altcx: array of other alternate-C[1-4] functions
+ */
+struct prcm_gpiocr_altcx_pin_desc {
+	unsigned short pin;
+	struct prcm_gpiocr_altcx altcx[PRCM_IDX_GPIOCR_ALTC_MAX];
+};
+
 /**
  * struct nmk_function - Nomadik pinctrl mux function
  * @name: The name of the function, exported to pinctrl core.
@@ -50,6 +122,9 @@ struct nmk_pingroup {
  * @nfunction:	The number of entries in @functions.
  * @groups:	An array describing all pin groups the pin SoC supports.
  * @ngroups:	The number of entries in @groups.
+ * @altcx_pins:	The pins that support Other alternate-C function on this SoC
+ * @npins_altcx: The number of Other alternate-C pins
+ * @prcm_gpiocr_registers: The array of PRCM GPIOCR registers on this SoC
  */
 struct nmk_pinctrl_soc_data {
 	struct pinctrl_gpio_range *gpio_ranges;
@@ -60,6 +135,9 @@ struct nmk_pinctrl_soc_data {
 	unsigned nfunctions;
 	const struct nmk_pingroup *groups;
 	unsigned ngroups;
+	const struct prcm_gpiocr_altcx_pin_desc *altcx_pins;
+	unsigned npins_altcx;
+	const u16 *prcm_gpiocr_registers;
 };
 
 #ifdef CONFIG_PINCTRL_STN8815
diff --git a/drivers/pinctrl/pinctrl-xway.c b/drivers/pinctrl/pinctrl-xway.c
new file mode 100644
index 00000000000..f8d917d40c9
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-xway.c
@@ -0,0 +1,781 @@
+/*
+ *  linux/drivers/pinctrl/pinmux-xway.c
+ *  based on linux/drivers/pinctrl/pinmux-pxa910.c
+ *
+ *  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
+ *  publishhed by the Free Software Foundation.
+ *
+ *  Copyright (C) 2012 John Crispin <blogic@openwrt.org>
+ */
+
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/of_address.h>
+#include <linux/of_gpio.h>
+#include <linux/ioport.h>
+#include <linux/io.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+
+#include "pinctrl-lantiq.h"
+
+#include <lantiq_soc.h>
+
+/* we have 3 1/2 banks of 16 bit each */
+#define PINS			16
+#define PORT3			3
+#define PORT(x)			(x / PINS)
+#define PORT_PIN(x)		(x % PINS)
+
+/* we have 2 mux bits that can be set for each pin */
+#define MUX_ALT0	0x1
+#define MUX_ALT1	0x2
+
+/*
+ * each bank has this offset apart from the 1/2 bank that is mixed into the
+ * other 3 ranges
+ */
+#define REG_OFF			0x30
+
+/* these are the offsets to our registers */
+#define GPIO_BASE(p)		(REG_OFF * PORT(p))
+#define GPIO_OUT(p)		GPIO_BASE(p)
+#define GPIO_IN(p)		(GPIO_BASE(p) + 0x04)
+#define GPIO_DIR(p)		(GPIO_BASE(p) + 0x08)
+#define GPIO_ALT0(p)		(GPIO_BASE(p) + 0x0C)
+#define GPIO_ALT1(p)		(GPIO_BASE(p) + 0x10)
+#define GPIO_OD(p)		(GPIO_BASE(p) + 0x14)
+#define GPIO_PUDSEL(p)		(GPIO_BASE(p) + 0x1c)
+#define GPIO_PUDEN(p)		(GPIO_BASE(p) + 0x20)
+
+/* the 1/2 port needs special offsets for some registers */
+#define GPIO3_OD		(GPIO_BASE(0) + 0x24)
+#define GPIO3_PUDSEL		(GPIO_BASE(0) + 0x28)
+#define GPIO3_PUDEN		(GPIO_BASE(0) + 0x2C)
+#define GPIO3_ALT1		(GPIO_BASE(PINS) + 0x24)
+
+/* macros to help us access the registers */
+#define gpio_getbit(m, r, p)	(!!(ltq_r32(m + r) & BIT(p)))
+#define gpio_setbit(m, r, p)	ltq_w32_mask(0, BIT(p), m + r)
+#define gpio_clearbit(m, r, p)	ltq_w32_mask(BIT(p), 0, m + r)
+
+#define MFP_XWAY(a, f0, f1, f2, f3)	\
+	{				\
+		.name = #a,		\
+		.pin = a,		\
+		.func = {		\
+			XWAY_MUX_##f0,	\
+			XWAY_MUX_##f1,	\
+			XWAY_MUX_##f2,	\
+			XWAY_MUX_##f3,	\
+		},			\
+	}
+
+#define GRP_MUX(a, m, p)		\
+	{ .name = a, .mux = XWAY_MUX_##m, .pins = p, .npins = ARRAY_SIZE(p), }
+
+#define FUNC_MUX(f, m)		\
+	{ .func = f, .mux = XWAY_MUX_##m, }
+
+#define XWAY_MAX_PIN		32
+#define XR9_MAX_PIN		56
+
+enum xway_mux {
+	XWAY_MUX_GPIO = 0,
+	XWAY_MUX_SPI,
+	XWAY_MUX_ASC,
+	XWAY_MUX_PCI,
+	XWAY_MUX_CGU,
+	XWAY_MUX_EBU,
+	XWAY_MUX_JTAG,
+	XWAY_MUX_EXIN,
+	XWAY_MUX_TDM,
+	XWAY_MUX_STP,
+	XWAY_MUX_SIN,
+	XWAY_MUX_GPT,
+	XWAY_MUX_NMI,
+	XWAY_MUX_MDIO,
+	XWAY_MUX_MII,
+	XWAY_MUX_EPHY,
+	XWAY_MUX_DFE,
+	XWAY_MUX_SDIO,
+	XWAY_MUX_NONE = 0xffff,
+};
+
+static const struct ltq_mfp_pin xway_mfp[] = {
+	/*       pin    f0	f1	f2	f3   */
+	MFP_XWAY(GPIO0, GPIO,	EXIN,	NONE,	TDM),
+	MFP_XWAY(GPIO1, GPIO,	EXIN,	NONE,	NONE),
+	MFP_XWAY(GPIO2, GPIO,	CGU,	EXIN,	NONE),
+	MFP_XWAY(GPIO3, GPIO,	CGU,	NONE,	PCI),
+	MFP_XWAY(GPIO4, GPIO,	STP,	NONE,	ASC),
+	MFP_XWAY(GPIO5, GPIO,	STP,	NONE,	NONE),
+	MFP_XWAY(GPIO6, GPIO,	STP,	GPT,	ASC),
+	MFP_XWAY(GPIO7, GPIO,	CGU,	PCI,	NONE),
+	MFP_XWAY(GPIO8, GPIO,	CGU,	NMI,	NONE),
+	MFP_XWAY(GPIO9, GPIO,	ASC,	SPI,	EXIN),
+	MFP_XWAY(GPIO10, GPIO,	ASC,	SPI,	NONE),
+	MFP_XWAY(GPIO11, GPIO,	ASC,	PCI,	SPI),
+	MFP_XWAY(GPIO12, GPIO,	ASC,	NONE,	NONE),
+	MFP_XWAY(GPIO13, GPIO,	EBU,	SPI,	NONE),
+	MFP_XWAY(GPIO14, GPIO,	CGU,	PCI,	NONE),
+	MFP_XWAY(GPIO15, GPIO,	SPI,	JTAG,	NONE),
+	MFP_XWAY(GPIO16, GPIO,	SPI,	NONE,	JTAG),
+	MFP_XWAY(GPIO17, GPIO,	SPI,	NONE,	JTAG),
+	MFP_XWAY(GPIO18, GPIO,	SPI,	NONE,	JTAG),
+	MFP_XWAY(GPIO19, GPIO,	PCI,	NONE,	NONE),
+	MFP_XWAY(GPIO20, GPIO,	JTAG,	NONE,	NONE),
+	MFP_XWAY(GPIO21, GPIO,	PCI,	EBU,	GPT),
+	MFP_XWAY(GPIO22, GPIO,	SPI,	NONE,	NONE),
+	MFP_XWAY(GPIO23, GPIO,	EBU,	PCI,	STP),
+	MFP_XWAY(GPIO24, GPIO,	EBU,	TDM,	PCI),
+	MFP_XWAY(GPIO25, GPIO,	TDM,	NONE,	ASC),
+	MFP_XWAY(GPIO26, GPIO,	EBU,	NONE,	TDM),
+	MFP_XWAY(GPIO27, GPIO,	TDM,	NONE,	ASC),
+	MFP_XWAY(GPIO28, GPIO,	GPT,	NONE,	NONE),
+	MFP_XWAY(GPIO29, GPIO,	PCI,	NONE,	NONE),
+	MFP_XWAY(GPIO30, GPIO,	PCI,	NONE,	NONE),
+	MFP_XWAY(GPIO31, GPIO,	EBU,	PCI,	NONE),
+	MFP_XWAY(GPIO32, GPIO,	NONE,	NONE,	EBU),
+	MFP_XWAY(GPIO33, GPIO,	NONE,	NONE,	EBU),
+	MFP_XWAY(GPIO34, GPIO,	NONE,	NONE,	EBU),
+	MFP_XWAY(GPIO35, GPIO,	NONE,	NONE,	EBU),
+	MFP_XWAY(GPIO36, GPIO,	SIN,	NONE,	EBU),
+	MFP_XWAY(GPIO37, GPIO,	PCI,	NONE,	NONE),
+	MFP_XWAY(GPIO38, GPIO,	PCI,	NONE,	NONE),
+	MFP_XWAY(GPIO39, GPIO,	EXIN,	NONE,	NONE),
+	MFP_XWAY(GPIO40, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO41, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO42, GPIO,	MDIO,	NONE,	NONE),
+	MFP_XWAY(GPIO43, GPIO,	MDIO,	NONE,	NONE),
+	MFP_XWAY(GPIO44, GPIO,	NONE,	NONE,	SIN),
+	MFP_XWAY(GPIO45, GPIO,	NONE,	NONE,	SIN),
+	MFP_XWAY(GPIO46, GPIO,	NONE,	NONE,	EXIN),
+	MFP_XWAY(GPIO47, GPIO,	NONE,	NONE,	SIN),
+	MFP_XWAY(GPIO48, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO49, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO50, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO51, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO52, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO53, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO54, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO55, GPIO,	NONE,	NONE,	NONE),
+};
+
+static const struct ltq_mfp_pin ase_mfp[] = {
+	/*       pin    f0	f1	f2	f3   */
+	MFP_XWAY(GPIO0, GPIO,	EXIN,	MII,	TDM),
+	MFP_XWAY(GPIO1, GPIO,	STP,	DFE,	EBU),
+	MFP_XWAY(GPIO2, GPIO,	STP,	DFE,	EPHY),
+	MFP_XWAY(GPIO3, GPIO,	STP,	EPHY,	EBU),
+	MFP_XWAY(GPIO4, GPIO,	GPT,	EPHY,	MII),
+	MFP_XWAY(GPIO5, GPIO,	MII,	ASC,	GPT),
+	MFP_XWAY(GPIO6, GPIO,	MII,	ASC,	EXIN),
+	MFP_XWAY(GPIO7, GPIO,	SPI,	MII,	JTAG),
+	MFP_XWAY(GPIO8, GPIO,	SPI,	MII,	JTAG),
+	MFP_XWAY(GPIO9, GPIO,	SPI,	MII,	JTAG),
+	MFP_XWAY(GPIO10, GPIO,	SPI,	MII,	JTAG),
+	MFP_XWAY(GPIO11, GPIO,	EBU,	CGU,	JTAG),
+	MFP_XWAY(GPIO12, GPIO,	EBU,	MII,	SDIO),
+	MFP_XWAY(GPIO13, GPIO,	EBU,	MII,	CGU),
+	MFP_XWAY(GPIO14, GPIO,	EBU,	SPI,	CGU),
+	MFP_XWAY(GPIO15, GPIO,	EBU,	SPI,	SDIO),
+	MFP_XWAY(GPIO16, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO17, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO18, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO19, GPIO,	EBU,	MII,	SDIO),
+	MFP_XWAY(GPIO20, GPIO,	EBU,	MII,	SDIO),
+	MFP_XWAY(GPIO21, GPIO,	EBU,	MII,	SDIO),
+	MFP_XWAY(GPIO22, GPIO,	EBU,	MII,	CGU),
+	MFP_XWAY(GPIO23, GPIO,	EBU,	MII,	CGU),
+	MFP_XWAY(GPIO24, GPIO,	EBU,	NONE,	MII),
+	MFP_XWAY(GPIO25, GPIO,	EBU,	MII,	GPT),
+	MFP_XWAY(GPIO26, GPIO,	EBU,	MII,	SDIO),
+	MFP_XWAY(GPIO27, GPIO,	EBU,	NONE,	MII),
+	MFP_XWAY(GPIO28, GPIO,	MII,	EBU,	SDIO),
+	MFP_XWAY(GPIO29, GPIO,	EBU,	MII,	EXIN),
+	MFP_XWAY(GPIO30, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO31, GPIO,	NONE,	NONE,	NONE),
+};
+
+static const unsigned pins_jtag[] = {GPIO15, GPIO16, GPIO17, GPIO19, GPIO35};
+static const unsigned pins_asc0[] = {GPIO11, GPIO12};
+static const unsigned pins_asc0_cts_rts[] = {GPIO9, GPIO10};
+static const unsigned pins_stp[] = {GPIO4, GPIO5, GPIO6};
+static const unsigned pins_nmi[] = {GPIO8};
+static const unsigned pins_mdio[] = {GPIO42, GPIO43};
+
+static const unsigned pins_ebu_a24[] = {GPIO13};
+static const unsigned pins_ebu_clk[] = {GPIO21};
+static const unsigned pins_ebu_cs1[] = {GPIO23};
+static const unsigned pins_ebu_a23[] = {GPIO24};
+static const unsigned pins_ebu_wait[] = {GPIO26};
+static const unsigned pins_ebu_a25[] = {GPIO31};
+static const unsigned pins_ebu_rdy[] = {GPIO48};
+static const unsigned pins_ebu_rd[] = {GPIO49};
+
+static const unsigned pins_nand_ale[] = {GPIO13};
+static const unsigned pins_nand_cs1[] = {GPIO23};
+static const unsigned pins_nand_cle[] = {GPIO24};
+static const unsigned pins_nand_rdy[] = {GPIO48};
+static const unsigned pins_nand_rd[] = {GPIO49};
+
+static const unsigned pins_exin0[] = {GPIO0};
+static const unsigned pins_exin1[] = {GPIO1};
+static const unsigned pins_exin2[] = {GPIO2};
+static const unsigned pins_exin3[] = {GPIO39};
+static const unsigned pins_exin4[] = {GPIO46};
+static const unsigned pins_exin5[] = {GPIO9};
+
+static const unsigned pins_spi[] = {GPIO16, GPIO17, GPIO18};
+static const unsigned pins_spi_cs1[] = {GPIO15};
+static const unsigned pins_spi_cs2[] = {GPIO21};
+static const unsigned pins_spi_cs3[] = {GPIO13};
+static const unsigned pins_spi_cs4[] = {GPIO10};
+static const unsigned pins_spi_cs5[] = {GPIO9};
+static const unsigned pins_spi_cs6[] = {GPIO11};
+
+static const unsigned pins_gpt1[] = {GPIO28};
+static const unsigned pins_gpt2[] = {GPIO21};
+static const unsigned pins_gpt3[] = {GPIO6};
+
+static const unsigned pins_clkout0[] = {GPIO8};
+static const unsigned pins_clkout1[] = {GPIO7};
+static const unsigned pins_clkout2[] = {GPIO3};
+static const unsigned pins_clkout3[] = {GPIO2};
+
+static const unsigned pins_pci_gnt1[] = {GPIO30};
+static const unsigned pins_pci_gnt2[] = {GPIO23};
+static const unsigned pins_pci_gnt3[] = {GPIO19};
+static const unsigned pins_pci_gnt4[] = {GPIO38};
+static const unsigned pins_pci_req1[] = {GPIO29};
+static const unsigned pins_pci_req2[] = {GPIO31};
+static const unsigned pins_pci_req3[] = {GPIO3};
+static const unsigned pins_pci_req4[] = {GPIO37};
+
+static const unsigned ase_pins_jtag[] = {GPIO7, GPIO8, GPIO9, GPIO10, GPIO11};
+static const unsigned ase_pins_asc[] = {GPIO5, GPIO6};
+static const unsigned ase_pins_stp[] = {GPIO1, GPIO2, GPIO3};
+static const unsigned ase_pins_ephy[] = {GPIO2, GPIO3, GPIO4};
+static const unsigned ase_pins_dfe[] = {GPIO1, GPIO2};
+
+static const unsigned ase_pins_spi[] = {GPIO8, GPIO9, GPIO10};
+static const unsigned ase_pins_spi_cs1[] = {GPIO7};
+static const unsigned ase_pins_spi_cs2[] = {GPIO15};
+static const unsigned ase_pins_spi_cs3[] = {GPIO14};
+
+static const unsigned ase_pins_exin0[] = {GPIO6};
+static const unsigned ase_pins_exin1[] = {GPIO29};
+static const unsigned ase_pins_exin2[] = {GPIO0};
+
+static const unsigned ase_pins_gpt1[] = {GPIO5};
+static const unsigned ase_pins_gpt2[] = {GPIO4};
+static const unsigned ase_pins_gpt3[] = {GPIO25};
+
+static const struct ltq_pin_group xway_grps[] = {
+	GRP_MUX("exin0", EXIN, pins_exin0),
+	GRP_MUX("exin1", EXIN, pins_exin1),
+	GRP_MUX("exin2", EXIN, pins_exin2),
+	GRP_MUX("jtag", JTAG, pins_jtag),
+	GRP_MUX("ebu a23", EBU, pins_ebu_a23),
+	GRP_MUX("ebu a24", EBU, pins_ebu_a24),
+	GRP_MUX("ebu a25", EBU, pins_ebu_a25),
+	GRP_MUX("ebu clk", EBU, pins_ebu_clk),
+	GRP_MUX("ebu cs1", EBU, pins_ebu_cs1),
+	GRP_MUX("ebu wait", EBU, pins_ebu_wait),
+	GRP_MUX("nand ale", EBU, pins_nand_ale),
+	GRP_MUX("nand cs1", EBU, pins_nand_cs1),
+	GRP_MUX("nand cle", EBU, pins_nand_cle),
+	GRP_MUX("spi", SPI, pins_spi),
+	GRP_MUX("spi_cs1", SPI, pins_spi_cs1),
+	GRP_MUX("spi_cs2", SPI, pins_spi_cs2),
+	GRP_MUX("spi_cs3", SPI, pins_spi_cs3),
+	GRP_MUX("spi_cs4", SPI, pins_spi_cs4),
+	GRP_MUX("spi_cs5", SPI, pins_spi_cs5),
+	GRP_MUX("spi_cs6", SPI, pins_spi_cs6),
+	GRP_MUX("asc0", ASC, pins_asc0),
+	GRP_MUX("asc0 cts rts", ASC, pins_asc0_cts_rts),
+	GRP_MUX("stp", STP, pins_stp),
+	GRP_MUX("nmi", NMI, pins_nmi),
+	GRP_MUX("gpt1", GPT, pins_gpt1),
+	GRP_MUX("gpt2", GPT, pins_gpt2),
+	GRP_MUX("gpt3", GPT, pins_gpt3),
+	GRP_MUX("clkout0", CGU, pins_clkout0),
+	GRP_MUX("clkout1", CGU, pins_clkout1),
+	GRP_MUX("clkout2", CGU, pins_clkout2),
+	GRP_MUX("clkout3", CGU, pins_clkout3),
+	GRP_MUX("gnt1", PCI, pins_pci_gnt1),
+	GRP_MUX("gnt2", PCI, pins_pci_gnt2),
+	GRP_MUX("gnt3", PCI, pins_pci_gnt3),
+	GRP_MUX("req1", PCI, pins_pci_req1),
+	GRP_MUX("req2", PCI, pins_pci_req2),
+	GRP_MUX("req3", PCI, pins_pci_req3),
+/* xrx only */
+	GRP_MUX("nand rdy", EBU, pins_nand_rdy),
+	GRP_MUX("nand rd", EBU, pins_nand_rd),
+	GRP_MUX("exin3", EXIN, pins_exin3),
+	GRP_MUX("exin4", EXIN, pins_exin4),
+	GRP_MUX("exin5", EXIN, pins_exin5),
+	GRP_MUX("gnt4", PCI, pins_pci_gnt4),
+	GRP_MUX("req4", PCI, pins_pci_gnt4),
+	GRP_MUX("mdio", MDIO, pins_mdio),
+};
+
+static const struct ltq_pin_group ase_grps[] = {
+	GRP_MUX("exin0", EXIN, ase_pins_exin0),
+	GRP_MUX("exin1", EXIN, ase_pins_exin1),
+	GRP_MUX("exin2", EXIN, ase_pins_exin2),
+	GRP_MUX("jtag", JTAG, ase_pins_jtag),
+	GRP_MUX("stp", STP, ase_pins_stp),
+	GRP_MUX("asc", ASC, ase_pins_asc),
+	GRP_MUX("gpt1", GPT, ase_pins_gpt1),
+	GRP_MUX("gpt2", GPT, ase_pins_gpt2),
+	GRP_MUX("gpt3", GPT, ase_pins_gpt3),
+	GRP_MUX("ephy", EPHY, ase_pins_ephy),
+	GRP_MUX("dfe", DFE, ase_pins_dfe),
+	GRP_MUX("spi", SPI, ase_pins_spi),
+	GRP_MUX("spi_cs1", SPI, ase_pins_spi_cs1),
+	GRP_MUX("spi_cs2", SPI, ase_pins_spi_cs2),
+	GRP_MUX("spi_cs3", SPI, ase_pins_spi_cs3),
+};
+
+static const char * const xway_pci_grps[] = {"gnt1", "gnt2",
+						"gnt3", "req1",
+						"req2", "req3"};
+static const char * const xway_spi_grps[] = {"spi", "spi_cs1",
+						"spi_cs2", "spi_cs3",
+						"spi_cs4", "spi_cs5",
+						"spi_cs6"};
+static const char * const xway_cgu_grps[] = {"clkout0", "clkout1",
+						"clkout2", "clkout3"};
+static const char * const xway_ebu_grps[] = {"ebu a23", "ebu a24",
+						"ebu a25", "ebu cs1",
+						"ebu wait", "ebu clk",
+						"nand ale", "nand cs1",
+						"nand cle"};
+static const char * const xway_exin_grps[] = {"exin0", "exin1", "exin2"};
+static const char * const xway_gpt_grps[] = {"gpt1", "gpt2", "gpt3"};
+static const char * const xway_asc_grps[] = {"asc0", "asc0 cts rts"};
+static const char * const xway_jtag_grps[] = {"jtag"};
+static const char * const xway_stp_grps[] = {"stp"};
+static const char * const xway_nmi_grps[] = {"nmi"};
+
+/* ar9/vr9/gr9 */
+static const char * const xrx_mdio_grps[] = {"mdio"};
+static const char * const xrx_ebu_grps[] = {"ebu a23", "ebu a24",
+						"ebu a25", "ebu cs1",
+						"ebu wait", "ebu clk",
+						"nand ale", "nand cs1",
+						"nand cle", "nand rdy",
+						"nand rd"};
+static const char * const xrx_exin_grps[] = {"exin0", "exin1", "exin2",
+						"exin3", "exin4", "exin5"};
+static const char * const xrx_pci_grps[] = {"gnt1", "gnt2",
+						"gnt3", "gnt4",
+						"req1", "req2",
+						"req3", "req4"};
+
+/* ase */
+static const char * const ase_exin_grps[] = {"exin0", "exin1", "exin2"};
+static const char * const ase_gpt_grps[] = {"gpt1", "gpt2", "gpt3"};
+static const char * const ase_dfe_grps[] = {"dfe"};
+static const char * const ase_ephy_grps[] = {"ephy"};
+static const char * const ase_asc_grps[] = {"asc"};
+static const char * const ase_jtag_grps[] = {"jtag"};
+static const char * const ase_stp_grps[] = {"stp"};
+static const char * const ase_spi_grps[] = {"spi", "spi_cs1",
+						"spi_cs2", "spi_cs3"};
+
+static const struct ltq_pmx_func danube_funcs[] = {
+	{"spi",		ARRAY_AND_SIZE(xway_spi_grps)},
+	{"asc",		ARRAY_AND_SIZE(xway_asc_grps)},
+	{"cgu",		ARRAY_AND_SIZE(xway_cgu_grps)},
+	{"jtag",	ARRAY_AND_SIZE(xway_jtag_grps)},
+	{"exin",	ARRAY_AND_SIZE(xway_exin_grps)},
+	{"stp",		ARRAY_AND_SIZE(xway_stp_grps)},
+	{"gpt",		ARRAY_AND_SIZE(xway_gpt_grps)},
+	{"nmi",		ARRAY_AND_SIZE(xway_nmi_grps)},
+	{"pci",		ARRAY_AND_SIZE(xway_pci_grps)},
+	{"ebu",		ARRAY_AND_SIZE(xway_ebu_grps)},
+};
+
+static const struct ltq_pmx_func xrx_funcs[] = {
+	{"spi",		ARRAY_AND_SIZE(xway_spi_grps)},
+	{"asc",		ARRAY_AND_SIZE(xway_asc_grps)},
+	{"cgu",		ARRAY_AND_SIZE(xway_cgu_grps)},
+	{"jtag",	ARRAY_AND_SIZE(xway_jtag_grps)},
+	{"exin",	ARRAY_AND_SIZE(xrx_exin_grps)},
+	{"stp",		ARRAY_AND_SIZE(xway_stp_grps)},
+	{"gpt",		ARRAY_AND_SIZE(xway_gpt_grps)},
+	{"nmi",		ARRAY_AND_SIZE(xway_nmi_grps)},
+	{"pci",		ARRAY_AND_SIZE(xrx_pci_grps)},
+	{"ebu",		ARRAY_AND_SIZE(xrx_ebu_grps)},
+	{"mdio",	ARRAY_AND_SIZE(xrx_mdio_grps)},
+};
+
+static const struct ltq_pmx_func ase_funcs[] = {
+	{"spi",		ARRAY_AND_SIZE(ase_spi_grps)},
+	{"asc",		ARRAY_AND_SIZE(ase_asc_grps)},
+	{"jtag",	ARRAY_AND_SIZE(ase_jtag_grps)},
+	{"exin",	ARRAY_AND_SIZE(ase_exin_grps)},
+	{"stp",		ARRAY_AND_SIZE(ase_stp_grps)},
+	{"gpt",		ARRAY_AND_SIZE(ase_gpt_grps)},
+	{"ephy",	ARRAY_AND_SIZE(ase_ephy_grps)},
+	{"dfe",		ARRAY_AND_SIZE(ase_dfe_grps)},
+};
+
+/* ---------  pinconf related code --------- */
+static int xway_pinconf_get(struct pinctrl_dev *pctldev,
+				unsigned pin,
+				unsigned long *config)
+{
+	struct ltq_pinmux_info *info = pinctrl_dev_get_drvdata(pctldev);
+	enum ltq_pinconf_param param = LTQ_PINCONF_UNPACK_PARAM(*config);
+	int port = PORT(pin);
+	u32 reg;
+
+	switch (param) {
+	case LTQ_PINCONF_PARAM_OPEN_DRAIN:
+		if (port == PORT3)
+			reg = GPIO3_OD;
+		else
+			reg = GPIO_OD(port);
+		*config = LTQ_PINCONF_PACK(param,
+			!!gpio_getbit(info->membase[0], reg, PORT_PIN(port)));
+		break;
+
+	case LTQ_PINCONF_PARAM_PULL:
+		if (port == PORT3)
+			reg = GPIO3_PUDEN;
+		else
+			reg = GPIO_PUDEN(port);
+		if (!gpio_getbit(info->membase[0], reg, PORT_PIN(port))) {
+			*config = LTQ_PINCONF_PACK(param, 0);
+			break;
+		}
+
+		if (port == PORT3)
+			reg = GPIO3_PUDSEL;
+		else
+			reg = GPIO_PUDSEL(port);
+		if (!gpio_getbit(info->membase[0], reg, PORT_PIN(port)))
+			*config = LTQ_PINCONF_PACK(param, 2);
+		else
+			*config = LTQ_PINCONF_PACK(param, 1);
+		break;
+
+	default:
+		dev_err(pctldev->dev, "Invalid config param %04x\n", param);
+		return -ENOTSUPP;
+	}
+	return 0;
+}
+
+static int xway_pinconf_set(struct pinctrl_dev *pctldev,
+				unsigned pin,
+				unsigned long config)
+{
+	struct ltq_pinmux_info *info = pinctrl_dev_get_drvdata(pctldev);
+	enum ltq_pinconf_param param = LTQ_PINCONF_UNPACK_PARAM(config);
+	int arg = LTQ_PINCONF_UNPACK_ARG(config);
+	int port = PORT(pin);
+	u32 reg;
+
+	switch (param) {
+	case LTQ_PINCONF_PARAM_OPEN_DRAIN:
+		if (port == PORT3)
+			reg = GPIO3_OD;
+		else
+			reg = GPIO_OD(port);
+		gpio_setbit(info->membase[0], reg, PORT_PIN(port));
+		break;
+
+	case LTQ_PINCONF_PARAM_PULL:
+		if (port == PORT3)
+			reg = GPIO3_PUDEN;
+		else
+			reg = GPIO_PUDEN(port);
+		if (arg == 0) {
+			gpio_clearbit(info->membase[0], reg, PORT_PIN(port));
+			break;
+		}
+		gpio_setbit(info->membase[0], reg, PORT_PIN(port));
+
+		if (port == PORT3)
+			reg = GPIO3_PUDSEL;
+		else
+			reg = GPIO_PUDSEL(port);
+		if (arg == 1)
+			gpio_clearbit(info->membase[0], reg, PORT_PIN(port));
+		else if (arg == 2)
+			gpio_setbit(info->membase[0], reg, PORT_PIN(port));
+		else
+			dev_err(pctldev->dev, "Invalid pull value %d\n", arg);
+		break;
+
+	default:
+		dev_err(pctldev->dev, "Invalid config param %04x\n", param);
+		return -ENOTSUPP;
+	}
+	return 0;
+}
+
+struct pinconf_ops xway_pinconf_ops = {
+	.pin_config_get	= xway_pinconf_get,
+	.pin_config_set	= xway_pinconf_set,
+};
+
+static struct pinctrl_desc xway_pctrl_desc = {
+	.owner		= THIS_MODULE,
+	.confops	= &xway_pinconf_ops,
+};
+
+static inline int xway_mux_apply(struct pinctrl_dev *pctrldev,
+				int pin, int mux)
+{
+	struct ltq_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
+	int port = PORT(pin);
+	u32 alt1_reg = GPIO_ALT1(pin);
+
+	if (port == PORT3)
+		alt1_reg = GPIO3_ALT1;
+
+	if (mux & MUX_ALT0)
+		gpio_setbit(info->membase[0], GPIO_ALT0(pin), PORT_PIN(pin));
+	else
+		gpio_clearbit(info->membase[0], GPIO_ALT0(pin), PORT_PIN(pin));
+
+	if (mux & MUX_ALT1)
+		gpio_setbit(info->membase[0], alt1_reg, PORT_PIN(pin));
+	else
+		gpio_clearbit(info->membase[0], alt1_reg, PORT_PIN(pin));
+
+	return 0;
+}
+
+static const struct ltq_cfg_param xway_cfg_params[] = {
+	{"lantiq,pull",		LTQ_PINCONF_PARAM_PULL},
+	{"lantiq,open-drain",	LTQ_PINCONF_PARAM_OPEN_DRAIN},
+};
+
+static struct ltq_pinmux_info xway_info = {
+	.desc		= &xway_pctrl_desc,
+	.apply_mux	= xway_mux_apply,
+	.params		= xway_cfg_params,
+	.num_params	= ARRAY_SIZE(xway_cfg_params),
+};
+
+/* ---------  gpio_chip related code --------- */
+static void xway_gpio_set(struct gpio_chip *chip, unsigned int pin, int val)
+{
+	struct ltq_pinmux_info *info = dev_get_drvdata(chip->dev);
+
+	if (val)
+		gpio_setbit(info->membase[0], GPIO_OUT(pin), PORT_PIN(pin));
+	else
+		gpio_clearbit(info->membase[0], GPIO_OUT(pin), PORT_PIN(pin));
+}
+
+static int xway_gpio_get(struct gpio_chip *chip, unsigned int pin)
+{
+	struct ltq_pinmux_info *info = dev_get_drvdata(chip->dev);
+
+	return gpio_getbit(info->membase[0], GPIO_IN(pin), PORT_PIN(pin));
+}
+
+static int xway_gpio_dir_in(struct gpio_chip *chip, unsigned int pin)
+{
+	struct ltq_pinmux_info *info = dev_get_drvdata(chip->dev);
+
+	gpio_clearbit(info->membase[0], GPIO_DIR(pin), PORT_PIN(pin));
+
+	return 0;
+}
+
+static int xway_gpio_dir_out(struct gpio_chip *chip, unsigned int pin, int val)
+{
+	struct ltq_pinmux_info *info = dev_get_drvdata(chip->dev);
+
+	gpio_setbit(info->membase[0], GPIO_DIR(pin), PORT_PIN(pin));
+	xway_gpio_set(chip, pin, val);
+
+	return 0;
+}
+
+static int xway_gpio_req(struct gpio_chip *chip, unsigned offset)
+{
+	int gpio = chip->base + offset;
+
+	return pinctrl_request_gpio(gpio);
+}
+
+static void xway_gpio_free(struct gpio_chip *chip, unsigned offset)
+{
+	int gpio = chip->base + offset;
+
+	pinctrl_free_gpio(gpio);
+}
+
+static struct gpio_chip xway_chip = {
+	.label = "gpio-xway",
+	.direction_input = xway_gpio_dir_in,
+	.direction_output = xway_gpio_dir_out,
+	.get = xway_gpio_get,
+	.set = xway_gpio_set,
+	.request = xway_gpio_req,
+	.free = xway_gpio_free,
+	.base = -1,
+};
+
+
+/* --------- register the pinctrl layer --------- */
+static const unsigned xway_exin_pin_map[] = {GPIO0, GPIO1, GPIO2, GPIO39, GPIO46, GPIO9};
+static const unsigned ase_exin_pins_map[] = {GPIO6, GPIO29, GPIO0};
+
+static struct pinctrl_xway_soc {
+	int pin_count;
+	const struct ltq_mfp_pin *mfp;
+	const struct ltq_pin_group *grps;
+	unsigned int num_grps;
+	const struct ltq_pmx_func *funcs;
+	unsigned int num_funcs;
+	const unsigned *exin;
+	unsigned int num_exin;
+} soc_cfg[] = {
+	/* legacy xway */
+	{XWAY_MAX_PIN, xway_mfp,
+		xway_grps, ARRAY_SIZE(xway_grps),
+		danube_funcs, ARRAY_SIZE(danube_funcs),
+		xway_exin_pin_map, 3},
+	/* xway xr9 series */
+	{XR9_MAX_PIN, xway_mfp,
+		xway_grps, ARRAY_SIZE(xway_grps),
+		xrx_funcs, ARRAY_SIZE(xrx_funcs),
+		xway_exin_pin_map, 6},
+	/* xway ase series */
+	{XWAY_MAX_PIN, ase_mfp,
+		ase_grps, ARRAY_SIZE(ase_grps),
+		ase_funcs, ARRAY_SIZE(ase_funcs),
+		ase_exin_pins_map, 3},
+};
+
+static struct pinctrl_gpio_range xway_gpio_range = {
+	.name	= "XWAY GPIO",
+	.gc	= &xway_chip,
+};
+
+static const struct of_device_id xway_match[] = {
+	{ .compatible = "lantiq,pinctrl-xway", .data = &soc_cfg[0]},
+	{ .compatible = "lantiq,pinctrl-xr9", .data = &soc_cfg[1]},
+	{ .compatible = "lantiq,pinctrl-ase", .data = &soc_cfg[2]},
+	{},
+};
+MODULE_DEVICE_TABLE(of, xway_match);
+
+static int __devinit pinmux_xway_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match;
+	const struct pinctrl_xway_soc *xway_soc;
+	struct resource *res;
+	int ret, i;
+
+	/* get and remap our register range */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "Failed to get resource\n");
+		return -ENOENT;
+	}
+	xway_info.membase[0] = devm_request_and_ioremap(&pdev->dev, res);
+	if (!xway_info.membase[0]) {
+		dev_err(&pdev->dev, "Failed to remap resource\n");
+		return -ENOMEM;
+	}
+
+	match = of_match_device(xway_match, &pdev->dev);
+	if (match)
+		xway_soc = (const struct pinctrl_xway_soc *) match->data;
+	else
+		xway_soc = &soc_cfg[0];
+
+	/* find out how many pads we have */
+	xway_chip.ngpio = xway_soc->pin_count;
+
+	/* load our pad descriptors */
+	xway_info.pads = devm_kzalloc(&pdev->dev,
+			sizeof(struct pinctrl_pin_desc) * xway_chip.ngpio,
+			GFP_KERNEL);
+	if (!xway_info.pads) {
+		dev_err(&pdev->dev, "Failed to allocate pads\n");
+		return -ENOMEM;
+	}
+	for (i = 0; i < xway_chip.ngpio; i++) {
+		/* strlen("ioXY") + 1 = 5 */
+		char *name = devm_kzalloc(&pdev->dev, 5, GFP_KERNEL);
+
+		if (!name) {
+			dev_err(&pdev->dev, "Failed to allocate pad name\n");
+			return -ENOMEM;
+		}
+		snprintf(name, 5, "io%d", i);
+		xway_info.pads[i].number = GPIO0 + i;
+		xway_info.pads[i].name = name;
+	}
+	xway_pctrl_desc.pins = xway_info.pads;
+
+	/* load the gpio chip */
+	xway_chip.dev = &pdev->dev;
+	of_gpiochip_add(&xway_chip);
+	ret = gpiochip_add(&xway_chip);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to register gpio chip\n");
+		return ret;
+	}
+
+	/* setup the data needed by pinctrl */
+	xway_pctrl_desc.name	= dev_name(&pdev->dev);
+	xway_pctrl_desc.npins	= xway_chip.ngpio;
+
+	xway_info.num_pads	= xway_chip.ngpio;
+	xway_info.num_mfp	= xway_chip.ngpio;
+	xway_info.mfp		= xway_soc->mfp;
+	xway_info.grps		= xway_soc->grps;
+	xway_info.num_grps	= xway_soc->num_grps;
+	xway_info.funcs		= xway_soc->funcs;
+	xway_info.num_funcs	= xway_soc->num_funcs;
+	xway_info.exin		= xway_soc->exin;
+	xway_info.num_exin	= xway_soc->num_exin;
+
+	/* register with the generic lantiq layer */
+	ret = ltq_pinctrl_register(pdev, &xway_info);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to register pinctrl driver\n");
+		return ret;
+	}
+
+	/* finish with registering the gpio range in pinctrl */
+	xway_gpio_range.npins = xway_chip.ngpio;
+	xway_gpio_range.base = xway_chip.base;
+	pinctrl_add_gpio_range(xway_info.pctrl, &xway_gpio_range);
+	dev_info(&pdev->dev, "Init done\n");
+	return 0;
+}
+
+static struct platform_driver pinmux_xway_driver = {
+	.probe	= pinmux_xway_probe,
+	.driver = {
+		.name	= "pinctrl-xway",
+		.owner	= THIS_MODULE,
+		.of_match_table = xway_match,
+	},
+};
+
+static int __init pinmux_xway_init(void)
+{
+	return platform_driver_register(&pinmux_xway_driver);
+}
+
+core_initcall_sync(pinmux_xway_init);
diff --git a/drivers/platform/x86/acerhdf.c b/drivers/platform/x86/acerhdf.c
index 39abb150bdd..84c56881ba8 100644
--- a/drivers/platform/x86/acerhdf.c
+++ b/drivers/platform/x86/acerhdf.c
@@ -329,7 +329,8 @@ static int acerhdf_bind(struct thermal_zone_device *thermal,
 	if (cdev != cl_dev)
 		return 0;
 
-	if (thermal_zone_bind_cooling_device(thermal, 0, cdev)) {
+	if (thermal_zone_bind_cooling_device(thermal, 0, cdev,
+			THERMAL_NO_LIMIT, THERMAL_NO_LIMIT)) {
 		pr_err("error binding cooling dev\n");
 		return -EINVAL;
 	}
@@ -661,7 +662,7 @@ static int acerhdf_register_thermal(void)
 		return -EINVAL;
 
 	thz_dev = thermal_zone_device_register("acerhdf", 1, 0, NULL,
-					      &acerhdf_dev_ops, 0, 0, 0,
+					      &acerhdf_dev_ops, 0,
 					      (kernelmode) ? interval*1000 : 0);
 	if (IS_ERR(thz_dev))
 		return -EINVAL;
diff --git a/drivers/platform/x86/hp_accel.c b/drivers/platform/x86/hp_accel.c
index 6b9af989632..18d74f29dcb 100644
--- a/drivers/platform/x86/hp_accel.c
+++ b/drivers/platform/x86/hp_accel.c
@@ -382,31 +382,8 @@ static struct acpi_driver lis3lv02d_driver = {
 	},
 	.drv.pm = HP_ACCEL_PM,
 };
-
-static int __init lis3lv02d_init_module(void)
-{
-	int ret;
-
-	if (acpi_disabled)
-		return -ENODEV;
-
-	ret = acpi_bus_register_driver(&lis3lv02d_driver);
-	if (ret < 0)
-		return ret;
-
-	pr_info("driver loaded\n");
-
-	return 0;
-}
-
-static void __exit lis3lv02d_exit_module(void)
-{
-	acpi_bus_unregister_driver(&lis3lv02d_driver);
-}
+module_acpi_driver(lis3lv02d_driver);
 
 MODULE_DESCRIPTION("Glue between LIS3LV02Dx and HP ACPI BIOS and support for disk protection LED.");
 MODULE_AUTHOR("Yan Burman, Eric Piel, Pavel Machek");
 MODULE_LICENSE("GPL");
-
-module_init(lis3lv02d_init_module);
-module_exit(lis3lv02d_exit_module);
diff --git a/drivers/platform/x86/ideapad-laptop.c b/drivers/platform/x86/ideapad-laptop.c
index dae7abe1d71..5ff4f2e314d 100644
--- a/drivers/platform/x86/ideapad-laptop.c
+++ b/drivers/platform/x86/ideapad-laptop.c
@@ -917,20 +917,8 @@ static struct acpi_driver ideapad_acpi_driver = {
 	.drv.pm = &ideapad_pm,
 	.owner = THIS_MODULE,
 };
-
-static int __init ideapad_acpi_module_init(void)
-{
-	return acpi_bus_register_driver(&ideapad_acpi_driver);
-}
-
-static void __exit ideapad_acpi_module_exit(void)
-{
-	acpi_bus_unregister_driver(&ideapad_acpi_driver);
-}
+module_acpi_driver(ideapad_acpi_driver);
 
 MODULE_AUTHOR("David Woodhouse <dwmw2@infradead.org>");
 MODULE_DESCRIPTION("IdeaPad ACPI Extras");
 MODULE_LICENSE("GPL");
-
-module_init(ideapad_acpi_module_init);
-module_exit(ideapad_acpi_module_exit);
diff --git a/drivers/platform/x86/intel_mid_thermal.c b/drivers/platform/x86/intel_mid_thermal.c
index 3a27113deda..c8097616dd6 100644
--- a/drivers/platform/x86/intel_mid_thermal.c
+++ b/drivers/platform/x86/intel_mid_thermal.c
@@ -502,7 +502,7 @@ static int mid_thermal_probe(struct platform_device *pdev)
 			goto err;
 		}
 		pinfo->tzd[i] = thermal_zone_device_register(name[i],
-				0, 0, td_info, &tzd_ops, 0, 0, 0, 0);
+				0, 0, td_info, &tzd_ops, 0, 0);
 		if (IS_ERR(pinfo->tzd[i])) {
 			kfree(td_info);
 			ret = PTR_ERR(pinfo->tzd[i]);
diff --git a/drivers/platform/x86/topstar-laptop.c b/drivers/platform/x86/topstar-laptop.c
index d528daa0e81..d727bfee89a 100644
--- a/drivers/platform/x86/topstar-laptop.c
+++ b/drivers/platform/x86/topstar-laptop.c
@@ -186,27 +186,7 @@ static struct acpi_driver acpi_topstar_driver = {
 		.notify = acpi_topstar_notify,
 	},
 };
-
-static int __init topstar_laptop_init(void)
-{
-	int ret;
-
-	ret = acpi_bus_register_driver(&acpi_topstar_driver);
-	if (ret < 0)
-		return ret;
-
-	pr_info("ACPI extras driver loaded\n");
-
-	return 0;
-}
-
-static void __exit topstar_laptop_exit(void)
-{
-	acpi_bus_unregister_driver(&acpi_topstar_driver);
-}
-
-module_init(topstar_laptop_init);
-module_exit(topstar_laptop_exit);
+module_acpi_driver(acpi_topstar_driver);
 
 MODULE_AUTHOR("Herton Ronaldo Krzesinski");
 MODULE_DESCRIPTION("Topstar Laptop ACPI Extras driver");
diff --git a/drivers/platform/x86/toshiba_bluetooth.c b/drivers/platform/x86/toshiba_bluetooth.c
index 5e5d6317d69..e95be0b7485 100644
--- a/drivers/platform/x86/toshiba_bluetooth.c
+++ b/drivers/platform/x86/toshiba_bluetooth.c
@@ -122,30 +122,10 @@ static int toshiba_bt_rfkill_add(struct acpi_device *device)
 	return result;
 }
 
-static int __init toshiba_bt_rfkill_init(void)
-{
-	int result;
-
-	result = acpi_bus_register_driver(&toshiba_bt_rfkill_driver);
-	if (result < 0) {
-		ACPI_DEBUG_PRINT((ACPI_DB_ERROR,
-				  "Error registering driver\n"));
-		return result;
-	}
-
-	return 0;
-}
-
 static int toshiba_bt_rfkill_remove(struct acpi_device *device, int type)
 {
 	/* clean up */
 	return 0;
 }
 
-static void __exit toshiba_bt_rfkill_exit(void)
-{
-	acpi_bus_unregister_driver(&toshiba_bt_rfkill_driver);
-}
-
-module_init(toshiba_bt_rfkill_init);
-module_exit(toshiba_bt_rfkill_exit);
+module_acpi_driver(toshiba_bt_rfkill_driver);
diff --git a/drivers/platform/x86/xo15-ebook.c b/drivers/platform/x86/xo15-ebook.c
index 38ba39d7ca7..16d340c3b85 100644
--- a/drivers/platform/x86/xo15-ebook.c
+++ b/drivers/platform/x86/xo15-ebook.c
@@ -170,16 +170,4 @@ static struct acpi_driver xo15_ebook_driver = {
 	},
 	.drv.pm = &ebook_switch_pm,
 };
-
-static int __init xo15_ebook_init(void)
-{
-	return acpi_bus_register_driver(&xo15_ebook_driver);
-}
-
-static void __exit xo15_ebook_exit(void)
-{
-	acpi_bus_unregister_driver(&xo15_ebook_driver);
-}
-
-module_init(xo15_ebook_init);
-module_exit(xo15_ebook_exit);
+module_acpi_driver(xo15_ebook_driver);
diff --git a/drivers/pnp/pnpacpi/core.c b/drivers/pnp/pnpacpi/core.c
index 507a8e2b9a4..26b5d4b18dd 100644
--- a/drivers/pnp/pnpacpi/core.c
+++ b/drivers/pnp/pnpacpi/core.c
@@ -321,14 +321,9 @@ static int __init acpi_pnp_match(struct device *dev, void *_pnp)
 {
 	struct acpi_device *acpi = to_acpi_device(dev);
 	struct pnp_dev *pnp = _pnp;
-	struct device *physical_device;
-
-	physical_device = acpi_get_physical_device(acpi->handle);
-	if (physical_device)
-		put_device(physical_device);
 
 	/* true means it matched */
-	return !physical_device
+	return !acpi->physical_node_count
 	    && compare_pnp_id(pnp->id, acpi_device_hid(acpi));
 }
 
diff --git a/drivers/power/avs/smartreflex.c b/drivers/power/avs/smartreflex.c
index d4957b4edb6..24768a27e1d 100644
--- a/drivers/power/avs/smartreflex.c
+++ b/drivers/power/avs/smartreflex.c
@@ -930,7 +930,7 @@ static int __init omap_sr_probe(struct platform_device *pdev)
 	if (!sr_info->base) {
 		dev_err(&pdev->dev, "%s: ioremap fail\n", __func__);
 		ret = -ENOMEM;
-		goto err_release_region;
+		goto err_free_name;
 	}
 
 	if (irq)
@@ -969,7 +969,7 @@ static int __init omap_sr_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "%s: Unable to create debugfs directory\n",
 			__func__);
 		ret = PTR_ERR(sr_info->dbg_dir);
-		goto err_free_name;
+		goto err_debugfs;
 	}
 
 	(void) debugfs_create_file("autocomp", S_IRUGO | S_IWUSR,
@@ -1013,11 +1013,11 @@ static int __init omap_sr_probe(struct platform_device *pdev)
 
 err_debugfs:
 	debugfs_remove_recursive(sr_info->dbg_dir);
-err_free_name:
-	kfree(sr_info->name);
 err_iounmap:
 	list_del(&sr_info->node);
 	iounmap(sr_info->base);
+err_free_name:
+	kfree(sr_info->name);
 err_release_region:
 	release_mem_region(mem->start, resource_size(mem));
 err_free_devinfo:
diff --git a/drivers/power/power_supply_core.c b/drivers/power/power_supply_core.c
index 08cc8a3c15a..2436f135001 100644
--- a/drivers/power/power_supply_core.c
+++ b/drivers/power/power_supply_core.c
@@ -201,7 +201,7 @@ static int psy_register_thermal(struct power_supply *psy)
 	for (i = 0; i < psy->num_properties; i++) {
 		if (psy->properties[i] == POWER_SUPPLY_PROP_TEMP) {
 			psy->tzd = thermal_zone_device_register(psy->name, 0, 0,
-					psy, &psy_tzd_ops, 0, 0, 0, 0);
+					psy, &psy_tzd_ops, 0, 0);
 			if (IS_ERR(psy->tzd))
 				return PTR_ERR(psy->tzd);
 			break;
diff --git a/drivers/pwm/Kconfig b/drivers/pwm/Kconfig
index d7c6b83097c..ed81720e7b2 100644
--- a/drivers/pwm/Kconfig
+++ b/drivers/pwm/Kconfig
@@ -1,6 +1,5 @@
 menuconfig PWM
 	bool "Pulse-Width Modulation (PWM) Support"
-	depends on !MACH_JZ4740 && !PUV3_PWM
 	help
 	  Generic Pulse-Width Modulation (PWM) support.
 
@@ -29,6 +28,15 @@ menuconfig PWM
 
 if PWM
 
+config PWM_AB8500
+	tristate "AB8500 PWM support"
+	depends on AB8500_CORE && ARCH_U8500
+	help
+	  Generic PWM framework driver for Analog Baseband AB8500.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pwm-ab8500.
+
 config PWM_BFIN
 	tristate "Blackfin PWM support"
 	depends on BFIN_GPTIMERS
@@ -47,6 +55,16 @@ config PWM_IMX
 	  To compile this driver as a module, choose M here: the module
 	  will be called pwm-imx.
 
+config PWM_JZ4740
+	tristate "Ingenic JZ4740 PWM support"
+	depends on MACH_JZ4740
+	help
+	  Generic PWM framework driver for Ingenic JZ4740 based
+	  machines.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pwm-jz4740.
+
 config PWM_LPC32XX
 	tristate "LPC32XX PWM support"
 	depends on ARCH_LPC32XX
@@ -67,6 +85,15 @@ config PWM_MXS
 	  To compile this driver as a module, choose M here: the module
 	  will be called pwm-mxs.
 
+config PWM_PUV3
+	tristate "PKUnity NetBook-0916 PWM support"
+	depends on ARCH_PUV3
+	help
+	  Generic PWM framework driver for PKUnity NetBook-0916.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pwm-puv3.
+
 config PWM_PXA
 	tristate "PXA PWM support"
 	depends on ARCH_PXA
diff --git a/drivers/pwm/Makefile b/drivers/pwm/Makefile
index 78f123dca30..acfe4821c58 100644
--- a/drivers/pwm/Makefile
+++ b/drivers/pwm/Makefile
@@ -1,8 +1,11 @@
 obj-$(CONFIG_PWM)		+= core.o
+obj-$(CONFIG_PWM_AB8500)	+= pwm-ab8500.o
 obj-$(CONFIG_PWM_BFIN)		+= pwm-bfin.o
 obj-$(CONFIG_PWM_IMX)		+= pwm-imx.o
+obj-$(CONFIG_PWM_JZ4740)	+= pwm-jz4740.o
 obj-$(CONFIG_PWM_LPC32XX)	+= pwm-lpc32xx.o
 obj-$(CONFIG_PWM_MXS)		+= pwm-mxs.o
+obj-$(CONFIG_PWM_PUV3)		+= pwm-puv3.o
 obj-$(CONFIG_PWM_PXA)		+= pwm-pxa.o
 obj-$(CONFIG_PWM_SAMSUNG)	+= pwm-samsung.o
 obj-$(CONFIG_PWM_TEGRA)		+= pwm-tegra.o
diff --git a/drivers/pwm/core.c b/drivers/pwm/core.c
index c6e05078d3a..f5acdaa5270 100644
--- a/drivers/pwm/core.c
+++ b/drivers/pwm/core.c
@@ -371,7 +371,7 @@ EXPORT_SYMBOL_GPL(pwm_free);
  */
 int pwm_config(struct pwm_device *pwm, int duty_ns, int period_ns)
 {
-	if (!pwm || period_ns == 0 || duty_ns > period_ns)
+	if (!pwm || duty_ns < 0 || period_ns <= 0 || duty_ns > period_ns)
 		return -EINVAL;
 
 	return pwm->chip->ops->config(pwm->chip, pwm, duty_ns, period_ns);
@@ -379,6 +379,28 @@ int pwm_config(struct pwm_device *pwm, int duty_ns, int period_ns)
 EXPORT_SYMBOL_GPL(pwm_config);
 
 /**
+ * pwm_set_polarity() - configure the polarity of a PWM signal
+ * @pwm: PWM device
+ * @polarity: new polarity of the PWM signal
+ *
+ * Note that the polarity cannot be configured while the PWM device is enabled
+ */
+int pwm_set_polarity(struct pwm_device *pwm, enum pwm_polarity polarity)
+{
+	if (!pwm || !pwm->chip->ops)
+		return -EINVAL;
+
+	if (!pwm->chip->ops->set_polarity)
+		return -ENOSYS;
+
+	if (test_bit(PWMF_ENABLED, &pwm->flags))
+		return -EBUSY;
+
+	return pwm->chip->ops->set_polarity(pwm->chip, pwm, polarity);
+}
+EXPORT_SYMBOL_GPL(pwm_set_polarity);
+
+/**
  * pwm_enable() - start a PWM output toggling
  * @pwm: PWM device
  */
@@ -624,6 +646,64 @@ out:
 }
 EXPORT_SYMBOL_GPL(pwm_put);
 
+static void devm_pwm_release(struct device *dev, void *res)
+{
+	pwm_put(*(struct pwm_device **)res);
+}
+
+/**
+ * devm_pwm_get() - resource managed pwm_get()
+ * @dev: device for PWM consumer
+ * @con_id: consumer name
+ *
+ * This function performs like pwm_get() but the acquired PWM device will
+ * automatically be released on driver detach.
+ */
+struct pwm_device *devm_pwm_get(struct device *dev, const char *con_id)
+{
+	struct pwm_device **ptr, *pwm;
+
+	ptr = devres_alloc(devm_pwm_release, sizeof(**ptr), GFP_KERNEL);
+	if (!ptr)
+		return ERR_PTR(-ENOMEM);
+
+	pwm = pwm_get(dev, con_id);
+	if (!IS_ERR(pwm)) {
+		*ptr = pwm;
+		devres_add(dev, ptr);
+	} else {
+		devres_free(ptr);
+	}
+
+	return pwm;
+}
+EXPORT_SYMBOL_GPL(devm_pwm_get);
+
+static int devm_pwm_match(struct device *dev, void *res, void *data)
+{
+	struct pwm_device **p = res;
+
+	if (WARN_ON(!p || !*p))
+		return 0;
+
+	return *p == data;
+}
+
+/**
+ * devm_pwm_put() - resource managed pwm_put()
+ * @dev: device for PWM consumer
+ * @pwm: PWM device
+ *
+ * Release a PWM previously allocated using devm_pwm_get(). Calling this
+ * function is usually not needed because devm-allocated resources are
+ * automatically released on driver detach.
+ */
+void devm_pwm_put(struct device *dev, struct pwm_device *pwm)
+{
+	WARN_ON(devres_release(dev, devm_pwm_release, devm_pwm_match, pwm));
+}
+EXPORT_SYMBOL_GPL(devm_pwm_put);
+
 #ifdef CONFIG_DEBUG_FS
 static void pwm_dbg_show(struct pwm_chip *chip, struct seq_file *s)
 {
diff --git a/drivers/misc/ab8500-pwm.c b/drivers/pwm/pwm-ab8500.c
index d7a9aa14e5d..cfb72ca873d 100644
--- a/drivers/misc/ab8500-pwm.c
+++ b/drivers/pwm/pwm-ab8500.c
@@ -24,16 +24,12 @@
 #define ENABLE_PWM			1
 #define DISABLE_PWM			0
 
-struct pwm_device {
-	struct device *dev;
-	struct list_head node;
-	const char *label;
-	unsigned int pwm_id;
+struct ab8500_pwm_chip {
+	struct pwm_chip chip;
 };
 
-static LIST_HEAD(pwm_list);
-
-int pwm_config(struct pwm_device *pwm, int duty_ns, int period_ns)
+static int ab8500_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
+			     int duty_ns, int period_ns)
 {
 	int ret = 0;
 	unsigned int higher_val, lower_val;
@@ -50,95 +46,94 @@ int pwm_config(struct pwm_device *pwm, int duty_ns, int period_ns)
 	 */
 	higher_val = ((duty_ns & 0x0300) >> 8);
 
-	reg = AB8500_PWM_OUT_CTRL1_REG + ((pwm->pwm_id - 1) * 2);
+	reg = AB8500_PWM_OUT_CTRL1_REG + ((chip->base - 1) * 2);
 
-	ret = abx500_set_register_interruptible(pwm->dev, AB8500_MISC,
+	ret = abx500_set_register_interruptible(chip->dev, AB8500_MISC,
 			reg, (u8)lower_val);
 	if (ret < 0)
 		return ret;
-	ret = abx500_set_register_interruptible(pwm->dev, AB8500_MISC,
+	ret = abx500_set_register_interruptible(chip->dev, AB8500_MISC,
 			(reg + 1), (u8)higher_val);
 
 	return ret;
 }
-EXPORT_SYMBOL(pwm_config);
 
-int pwm_enable(struct pwm_device *pwm)
+static int ab8500_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
 	int ret;
 
-	ret = abx500_mask_and_set_register_interruptible(pwm->dev,
+	ret = abx500_mask_and_set_register_interruptible(chip->dev,
 				AB8500_MISC, AB8500_PWM_OUT_CTRL7_REG,
-				1 << (pwm->pwm_id-1), ENABLE_PWM);
+				1 << (chip->base - 1), ENABLE_PWM);
 	if (ret < 0)
-		dev_err(pwm->dev, "%s: Failed to disable PWM, Error %d\n",
+		dev_err(chip->dev, "%s: Failed to disable PWM, Error %d\n",
 							pwm->label, ret);
 	return ret;
 }
-EXPORT_SYMBOL(pwm_enable);
 
-void pwm_disable(struct pwm_device *pwm)
+static void ab8500_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
 	int ret;
 
-	ret = abx500_mask_and_set_register_interruptible(pwm->dev,
+	ret = abx500_mask_and_set_register_interruptible(chip->dev,
 				AB8500_MISC, AB8500_PWM_OUT_CTRL7_REG,
-				1 << (pwm->pwm_id-1), DISABLE_PWM);
+				1 << (chip->base - 1), DISABLE_PWM);
 	if (ret < 0)
-		dev_err(pwm->dev, "%s: Failed to disable PWM, Error %d\n",
+		dev_err(chip->dev, "%s: Failed to disable PWM, Error %d\n",
 							pwm->label, ret);
 	return;
 }
-EXPORT_SYMBOL(pwm_disable);
-
-struct pwm_device *pwm_request(int pwm_id, const char *label)
-{
-	struct pwm_device *pwm;
-
-	list_for_each_entry(pwm, &pwm_list, node) {
-		if (pwm->pwm_id == pwm_id) {
-			pwm->label = label;
-			pwm->pwm_id = pwm_id;
-			return pwm;
-		}
-	}
-
-	return ERR_PTR(-ENOENT);
-}
-EXPORT_SYMBOL(pwm_request);
 
-void pwm_free(struct pwm_device *pwm)
-{
-	pwm_disable(pwm);
-}
-EXPORT_SYMBOL(pwm_free);
+static const struct pwm_ops ab8500_pwm_ops = {
+	.config = ab8500_pwm_config,
+	.enable = ab8500_pwm_enable,
+	.disable = ab8500_pwm_disable,
+};
 
 static int __devinit ab8500_pwm_probe(struct platform_device *pdev)
 {
-	struct pwm_device *pwm;
+	struct ab8500_pwm_chip *ab8500;
+	int err;
+
 	/*
 	 * Nothing to be done in probe, this is required to get the
 	 * device which is required for ab8500 read and write
 	 */
-	pwm = kzalloc(sizeof(struct pwm_device), GFP_KERNEL);
-	if (pwm == NULL) {
+	ab8500 = kzalloc(sizeof(*ab8500), GFP_KERNEL);
+	if (ab8500 == NULL) {
 		dev_err(&pdev->dev, "failed to allocate memory\n");
 		return -ENOMEM;
 	}
-	pwm->dev = &pdev->dev;
-	pwm->pwm_id = pdev->id;
-	list_add_tail(&pwm->node, &pwm_list);
-	platform_set_drvdata(pdev, pwm);
-	dev_dbg(pwm->dev, "pwm probe successful\n");
+
+	ab8500->chip.dev = &pdev->dev;
+	ab8500->chip.ops = &ab8500_pwm_ops;
+	ab8500->chip.base = pdev->id;
+	ab8500->chip.npwm = 1;
+
+	err = pwmchip_add(&ab8500->chip);
+	if (err < 0) {
+		kfree(ab8500);
+		return err;
+	}
+
+	dev_dbg(&pdev->dev, "pwm probe successful\n");
+	platform_set_drvdata(pdev, ab8500);
+
 	return 0;
 }
 
 static int __devexit ab8500_pwm_remove(struct platform_device *pdev)
 {
-	struct pwm_device *pwm = platform_get_drvdata(pdev);
-	list_del(&pwm->node);
+	struct ab8500_pwm_chip *ab8500 = platform_get_drvdata(pdev);
+	int err;
+
+	err = pwmchip_remove(&ab8500->chip);
+	if (err < 0)
+		return err;
+
 	dev_dbg(&pdev->dev, "pwm driver removed\n");
-	kfree(pwm);
+	kfree(ab8500);
+
 	return 0;
 }
 
@@ -150,19 +145,8 @@ static struct platform_driver ab8500_pwm_driver = {
 	.probe = ab8500_pwm_probe,
 	.remove = __devexit_p(ab8500_pwm_remove),
 };
+module_platform_driver(ab8500_pwm_driver);
 
-static int __init ab8500_pwm_init(void)
-{
-	return platform_driver_register(&ab8500_pwm_driver);
-}
-
-static void __exit ab8500_pwm_exit(void)
-{
-	platform_driver_unregister(&ab8500_pwm_driver);
-}
-
-subsys_initcall(ab8500_pwm_init);
-module_exit(ab8500_pwm_exit);
 MODULE_AUTHOR("Arun MURTHY <arun.murthy@stericsson.com>");
 MODULE_DESCRIPTION("AB8500 Pulse Width Modulation Driver");
 MODULE_ALIAS("platform:ab8500-pwm");
diff --git a/drivers/pwm/pwm-bfin.c b/drivers/pwm/pwm-bfin.c
index d53c4e7941e..5da8e185e83 100644
--- a/drivers/pwm/pwm-bfin.c
+++ b/drivers/pwm/pwm-bfin.c
@@ -69,9 +69,6 @@ static int bfin_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 	unsigned long period, duty;
 	unsigned long long val;
 
-	if (duty_ns < 0 || duty_ns > period_ns)
-		return -EINVAL;
-
 	val = (unsigned long long)get_sclk() * period_ns;
 	do_div(val, NSEC_PER_SEC);
 	period = val;
diff --git a/drivers/pwm/pwm-imx.c b/drivers/pwm/pwm-imx.c
index 2a0b3533397..8a5d3ae2946 100644
--- a/drivers/pwm/pwm-imx.c
+++ b/drivers/pwm/pwm-imx.c
@@ -16,8 +16,7 @@
 #include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/pwm.h>
-#include <mach/hardware.h>
-
+#include <linux/of_device.h>
 
 /* i.MX1 and i.MX21 share the same PWM function block: */
 
@@ -25,6 +24,7 @@
 #define MX1_PWMS    0x04   /* PWM Sample Register */
 #define MX1_PWMP    0x08   /* PWM Period Register */
 
+#define MX1_PWMC_EN		(1 << 4)
 
 /* i.MX27, i.MX31, i.MX35 share the same PWM function block: */
 
@@ -40,110 +40,165 @@
 #define MX3_PWMCR_EN              (1 << 0)
 
 struct imx_chip {
-	struct clk	*clk;
+	struct clk	*clk_per;
+	struct clk	*clk_ipg;
 
-	int		clk_enabled;
+	int		enabled;
 	void __iomem	*mmio_base;
 
 	struct pwm_chip	chip;
+
+	int (*config)(struct pwm_chip *chip,
+		struct pwm_device *pwm, int duty_ns, int period_ns);
+	void (*set_enable)(struct pwm_chip *chip, bool enable);
 };
 
 #define to_imx_chip(chip)	container_of(chip, struct imx_chip, chip)
 
-static int imx_pwm_config(struct pwm_chip *chip,
+static int imx_pwm_config_v1(struct pwm_chip *chip,
 		struct pwm_device *pwm, int duty_ns, int period_ns)
 {
 	struct imx_chip *imx = to_imx_chip(chip);
 
-	if (!(cpu_is_mx1() || cpu_is_mx21())) {
-		unsigned long long c;
-		unsigned long period_cycles, duty_cycles, prescale;
-		u32 cr;
-
-		c = clk_get_rate(imx->clk);
-		c = c * period_ns;
-		do_div(c, 1000000000);
-		period_cycles = c;
-
-		prescale = period_cycles / 0x10000 + 1;
-
-		period_cycles /= prescale;
-		c = (unsigned long long)period_cycles * duty_ns;
-		do_div(c, period_ns);
-		duty_cycles = c;
-
-		/*
-		 * according to imx pwm RM, the real period value should be
-		 * PERIOD value in PWMPR plus 2.
-		 */
-		if (period_cycles > 2)
-			period_cycles -= 2;
-		else
-			period_cycles = 0;
-
-		writel(duty_cycles, imx->mmio_base + MX3_PWMSAR);
-		writel(period_cycles, imx->mmio_base + MX3_PWMPR);
-
-		cr = MX3_PWMCR_PRESCALER(prescale) |
-			MX3_PWMCR_DOZEEN | MX3_PWMCR_WAITEN |
-			MX3_PWMCR_DBGEN | MX3_PWMCR_EN;
-
-		if (cpu_is_mx25())
-			cr |= MX3_PWMCR_CLKSRC_IPG;
-		else
-			cr |= MX3_PWMCR_CLKSRC_IPG_HIGH;
-
-		writel(cr, imx->mmio_base + MX3_PWMCR);
-	} else if (cpu_is_mx1() || cpu_is_mx21()) {
-		/* The PWM subsystem allows for exact frequencies. However,
-		 * I cannot connect a scope on my device to the PWM line and
-		 * thus cannot provide the program the PWM controller
-		 * exactly. Instead, I'm relying on the fact that the
-		 * Bootloader (u-boot or WinCE+haret) has programmed the PWM
-		 * function group already. So I'll just modify the PWM sample
-		 * register to follow the ratio of duty_ns vs. period_ns
-		 * accordingly.
-		 *
-		 * This is good enough for programming the brightness of
-		 * the LCD backlight.
-		 *
-		 * The real implementation would divide PERCLK[0] first by
-		 * both the prescaler (/1 .. /128) and then by CLKSEL
-		 * (/2 .. /16).
-		 */
-		u32 max = readl(imx->mmio_base + MX1_PWMP);
-		u32 p = max * duty_ns / period_ns;
-		writel(max - p, imx->mmio_base + MX1_PWMS);
-	} else {
-		BUG();
-	}
+	/*
+	 * The PWM subsystem allows for exact frequencies. However,
+	 * I cannot connect a scope on my device to the PWM line and
+	 * thus cannot provide the program the PWM controller
+	 * exactly. Instead, I'm relying on the fact that the
+	 * Bootloader (u-boot or WinCE+haret) has programmed the PWM
+	 * function group already. So I'll just modify the PWM sample
+	 * register to follow the ratio of duty_ns vs. period_ns
+	 * accordingly.
+	 *
+	 * This is good enough for programming the brightness of
+	 * the LCD backlight.
+	 *
+	 * The real implementation would divide PERCLK[0] first by
+	 * both the prescaler (/1 .. /128) and then by CLKSEL
+	 * (/2 .. /16).
+	 */
+	u32 max = readl(imx->mmio_base + MX1_PWMP);
+	u32 p = max * duty_ns / period_ns;
+	writel(max - p, imx->mmio_base + MX1_PWMS);
 
 	return 0;
 }
 
+static void imx_pwm_set_enable_v1(struct pwm_chip *chip, bool enable)
+{
+	struct imx_chip *imx = to_imx_chip(chip);
+	u32 val;
+
+	val = readl(imx->mmio_base + MX1_PWMC);
+
+	if (enable)
+		val |= MX1_PWMC_EN;
+	else
+		val &= ~MX1_PWMC_EN;
+
+	writel(val, imx->mmio_base + MX1_PWMC);
+}
+
+static int imx_pwm_config_v2(struct pwm_chip *chip,
+		struct pwm_device *pwm, int duty_ns, int period_ns)
+{
+	struct imx_chip *imx = to_imx_chip(chip);
+	unsigned long long c;
+	unsigned long period_cycles, duty_cycles, prescale;
+	u32 cr;
+
+	c = clk_get_rate(imx->clk_per);
+	c = c * period_ns;
+	do_div(c, 1000000000);
+	period_cycles = c;
+
+	prescale = period_cycles / 0x10000 + 1;
+
+	period_cycles /= prescale;
+	c = (unsigned long long)period_cycles * duty_ns;
+	do_div(c, period_ns);
+	duty_cycles = c;
+
+	/*
+	 * according to imx pwm RM, the real period value should be
+	 * PERIOD value in PWMPR plus 2.
+	 */
+	if (period_cycles > 2)
+		period_cycles -= 2;
+	else
+		period_cycles = 0;
+
+	writel(duty_cycles, imx->mmio_base + MX3_PWMSAR);
+	writel(period_cycles, imx->mmio_base + MX3_PWMPR);
+
+	cr = MX3_PWMCR_PRESCALER(prescale) |
+		MX3_PWMCR_DOZEEN | MX3_PWMCR_WAITEN |
+		MX3_PWMCR_DBGEN | MX3_PWMCR_CLKSRC_IPG_HIGH;
+
+	if (imx->enabled)
+		cr |= MX3_PWMCR_EN;
+
+	writel(cr, imx->mmio_base + MX3_PWMCR);
+
+	return 0;
+}
+
+static void imx_pwm_set_enable_v2(struct pwm_chip *chip, bool enable)
+{
+	struct imx_chip *imx = to_imx_chip(chip);
+	u32 val;
+
+	val = readl(imx->mmio_base + MX3_PWMCR);
+
+	if (enable)
+		val |= MX3_PWMCR_EN;
+	else
+		val &= ~MX3_PWMCR_EN;
+
+	writel(val, imx->mmio_base + MX3_PWMCR);
+}
+
+static int imx_pwm_config(struct pwm_chip *chip,
+		struct pwm_device *pwm, int duty_ns, int period_ns)
+{
+	struct imx_chip *imx = to_imx_chip(chip);
+	int ret;
+
+	ret = clk_prepare_enable(imx->clk_ipg);
+	if (ret)
+		return ret;
+
+	ret = imx->config(chip, pwm, duty_ns, period_ns);
+
+	clk_disable_unprepare(imx->clk_ipg);
+
+	return ret;
+}
+
 static int imx_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
 	struct imx_chip *imx = to_imx_chip(chip);
-	int rc = 0;
+	int ret;
 
-	if (!imx->clk_enabled) {
-		rc = clk_prepare_enable(imx->clk);
-		if (!rc)
-			imx->clk_enabled = 1;
-	}
-	return rc;
+	ret = clk_prepare_enable(imx->clk_per);
+	if (ret)
+		return ret;
+
+	imx->set_enable(chip, true);
+
+	imx->enabled = 1;
+
+	return 0;
 }
 
 static void imx_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
 	struct imx_chip *imx = to_imx_chip(chip);
 
-	writel(0, imx->mmio_base + MX3_PWMCR);
+	imx->set_enable(chip, false);
 
-	if (imx->clk_enabled) {
-		clk_disable_unprepare(imx->clk);
-		imx->clk_enabled = 0;
-	}
+	clk_disable_unprepare(imx->clk_per);
+	imx->enabled = 0;
 }
 
 static struct pwm_ops imx_pwm_ops = {
@@ -153,30 +208,66 @@ static struct pwm_ops imx_pwm_ops = {
 	.owner = THIS_MODULE,
 };
 
+struct imx_pwm_data {
+	int (*config)(struct pwm_chip *chip,
+		struct pwm_device *pwm, int duty_ns, int period_ns);
+	void (*set_enable)(struct pwm_chip *chip, bool enable);
+};
+
+static struct imx_pwm_data imx_pwm_data_v1 = {
+	.config = imx_pwm_config_v1,
+	.set_enable = imx_pwm_set_enable_v1,
+};
+
+static struct imx_pwm_data imx_pwm_data_v2 = {
+	.config = imx_pwm_config_v2,
+	.set_enable = imx_pwm_set_enable_v2,
+};
+
+static const struct of_device_id imx_pwm_dt_ids[] = {
+	{ .compatible = "fsl,imx1-pwm", .data = &imx_pwm_data_v1, },
+	{ .compatible = "fsl,imx27-pwm", .data = &imx_pwm_data_v2, },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, imx_pwm_dt_ids);
+
 static int __devinit imx_pwm_probe(struct platform_device *pdev)
 {
+	const struct of_device_id *of_id =
+			of_match_device(imx_pwm_dt_ids, &pdev->dev);
+	struct imx_pwm_data *data;
 	struct imx_chip *imx;
 	struct resource *r;
 	int ret = 0;
 
+	if (!of_id)
+		return -ENODEV;
+
 	imx = devm_kzalloc(&pdev->dev, sizeof(*imx), GFP_KERNEL);
 	if (imx == NULL) {
 		dev_err(&pdev->dev, "failed to allocate memory\n");
 		return -ENOMEM;
 	}
 
-	imx->clk = devm_clk_get(&pdev->dev, "pwm");
+	imx->clk_per = devm_clk_get(&pdev->dev, "per");
+	if (IS_ERR(imx->clk_per)) {
+		dev_err(&pdev->dev, "getting per clock failed with %ld\n",
+				PTR_ERR(imx->clk_per));
+		return PTR_ERR(imx->clk_per);
+	}
 
-	if (IS_ERR(imx->clk))
-		return PTR_ERR(imx->clk);
+	imx->clk_ipg = devm_clk_get(&pdev->dev, "ipg");
+	if (IS_ERR(imx->clk_ipg)) {
+		dev_err(&pdev->dev, "getting ipg clock failed with %ld\n",
+				PTR_ERR(imx->clk_ipg));
+		return PTR_ERR(imx->clk_ipg);
+	}
 
 	imx->chip.ops = &imx_pwm_ops;
 	imx->chip.dev = &pdev->dev;
 	imx->chip.base = -1;
 	imx->chip.npwm = 1;
 
-	imx->clk_enabled = 0;
-
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (r == NULL) {
 		dev_err(&pdev->dev, "no memory resource defined\n");
@@ -187,6 +278,10 @@ static int __devinit imx_pwm_probe(struct platform_device *pdev)
 	if (imx->mmio_base == NULL)
 		return -EADDRNOTAVAIL;
 
+	data = of_id->data;
+	imx->config = data->config;
+	imx->set_enable = data->set_enable;
+
 	ret = pwmchip_add(&imx->chip);
 	if (ret < 0)
 		return ret;
@@ -208,23 +303,14 @@ static int __devexit imx_pwm_remove(struct platform_device *pdev)
 
 static struct platform_driver imx_pwm_driver = {
 	.driver		= {
-		.name	= "mxc_pwm",
+		.name	= "imx-pwm",
+		.of_match_table = of_match_ptr(imx_pwm_dt_ids),
 	},
 	.probe		= imx_pwm_probe,
 	.remove		= __devexit_p(imx_pwm_remove),
 };
 
-static int __init imx_pwm_init(void)
-{
-	return platform_driver_register(&imx_pwm_driver);
-}
-arch_initcall(imx_pwm_init);
-
-static void __exit imx_pwm_exit(void)
-{
-	platform_driver_unregister(&imx_pwm_driver);
-}
-module_exit(imx_pwm_exit);
+module_platform_driver(imx_pwm_driver);
 
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Sascha Hauer <s.hauer@pengutronix.de>");
diff --git a/drivers/pwm/pwm-jz4740.c b/drivers/pwm/pwm-jz4740.c
new file mode 100644
index 00000000000..10250fcefb9
--- /dev/null
+++ b/drivers/pwm/pwm-jz4740.c
@@ -0,0 +1,221 @@
+/*
+ *  Copyright (C) 2010, Lars-Peter Clausen <lars@metafoo.de>
+ *  JZ4740 platform PWM support
+ *
+ *  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.
+ *
+ *  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.,
+ *  675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pwm.h>
+
+#include <asm/mach-jz4740/gpio.h>
+#include <asm/mach-jz4740/timer.h>
+
+#define NUM_PWM 8
+
+static const unsigned int jz4740_pwm_gpio_list[NUM_PWM] = {
+	JZ_GPIO_PWM0,
+	JZ_GPIO_PWM1,
+	JZ_GPIO_PWM2,
+	JZ_GPIO_PWM3,
+	JZ_GPIO_PWM4,
+	JZ_GPIO_PWM5,
+	JZ_GPIO_PWM6,
+	JZ_GPIO_PWM7,
+};
+
+struct jz4740_pwm_chip {
+	struct pwm_chip chip;
+	struct clk *clk;
+};
+
+static inline struct jz4740_pwm_chip *to_jz4740(struct pwm_chip *chip)
+{
+	return container_of(chip, struct jz4740_pwm_chip, chip);
+}
+
+static int jz4740_pwm_request(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	unsigned int gpio = jz4740_pwm_gpio_list[pwm->hwpwm];
+	int ret;
+
+	/*
+	 * Timers 0 and 1 are used for system tasks, so they are unavailable
+	 * for use as PWMs.
+	 */
+	if (pwm->hwpwm < 2)
+		return -EBUSY;
+
+	ret = gpio_request(gpio, pwm->label);
+	if (ret) {
+		dev_err(chip->dev, "Failed to request GPIO#%u for PWM: %d\n",
+			gpio, ret);
+		return ret;
+	}
+
+	jz_gpio_set_function(gpio, JZ_GPIO_FUNC_PWM);
+
+	jz4740_timer_start(pwm->hwpwm);
+
+	return 0;
+}
+
+static void jz4740_pwm_free(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	unsigned int gpio = jz4740_pwm_gpio_list[pwm->hwpwm];
+
+	jz4740_timer_set_ctrl(pwm->hwpwm, 0);
+
+	jz_gpio_set_function(gpio, JZ_GPIO_FUNC_NONE);
+	gpio_free(gpio);
+
+	jz4740_timer_stop(pwm->hwpwm);
+}
+
+static int jz4740_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	uint32_t ctrl = jz4740_timer_get_ctrl(pwm->pwm);
+
+	ctrl |= JZ_TIMER_CTRL_PWM_ENABLE;
+	jz4740_timer_set_ctrl(pwm->hwpwm, ctrl);
+	jz4740_timer_enable(pwm->hwpwm);
+
+	return 0;
+}
+
+static void jz4740_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	uint32_t ctrl = jz4740_timer_get_ctrl(pwm->hwpwm);
+
+	ctrl &= ~JZ_TIMER_CTRL_PWM_ENABLE;
+	jz4740_timer_disable(pwm->hwpwm);
+	jz4740_timer_set_ctrl(pwm->hwpwm, ctrl);
+}
+
+static int jz4740_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
+			     int duty_ns, int period_ns)
+{
+	struct jz4740_pwm_chip *jz4740 = to_jz4740(pwm->chip);
+	unsigned long long tmp;
+	unsigned long period, duty;
+	unsigned int prescaler = 0;
+	uint16_t ctrl;
+	bool is_enabled;
+
+	tmp = (unsigned long long)clk_get_rate(jz4740->clk) * period_ns;
+	do_div(tmp, 1000000000);
+	period = tmp;
+
+	while (period > 0xffff && prescaler < 6) {
+		period >>= 2;
+		++prescaler;
+	}
+
+	if (prescaler == 6)
+		return -EINVAL;
+
+	tmp = (unsigned long long)period * duty_ns;
+	do_div(tmp, period_ns);
+	duty = period - tmp;
+
+	if (duty >= period)
+		duty = period - 1;
+
+	is_enabled = jz4740_timer_is_enabled(pwm->hwpwm);
+	if (is_enabled)
+		jz4740_pwm_disable(chip, pwm);
+
+	jz4740_timer_set_count(pwm->hwpwm, 0);
+	jz4740_timer_set_duty(pwm->hwpwm, duty);
+	jz4740_timer_set_period(pwm->hwpwm, period);
+
+	ctrl = JZ_TIMER_CTRL_PRESCALER(prescaler) | JZ_TIMER_CTRL_SRC_EXT |
+		JZ_TIMER_CTRL_PWM_ABBRUPT_SHUTDOWN;
+
+	jz4740_timer_set_ctrl(pwm->hwpwm, ctrl);
+
+	if (is_enabled)
+		jz4740_pwm_enable(chip, pwm);
+
+	return 0;
+}
+
+static const struct pwm_ops jz4740_pwm_ops = {
+	.request = jz4740_pwm_request,
+	.free = jz4740_pwm_free,
+	.config = jz4740_pwm_config,
+	.enable = jz4740_pwm_enable,
+	.disable = jz4740_pwm_disable,
+	.owner = THIS_MODULE,
+};
+
+static int __devinit jz4740_pwm_probe(struct platform_device *pdev)
+{
+	struct jz4740_pwm_chip *jz4740;
+	int ret;
+
+	jz4740 = devm_kzalloc(&pdev->dev, sizeof(*jz4740), GFP_KERNEL);
+	if (!jz4740)
+		return -ENOMEM;
+
+	jz4740->clk = clk_get(NULL, "ext");
+	if (IS_ERR(jz4740->clk))
+		return PTR_ERR(jz4740->clk);
+
+	jz4740->chip.dev = &pdev->dev;
+	jz4740->chip.ops = &jz4740_pwm_ops;
+	jz4740->chip.npwm = NUM_PWM;
+	jz4740->chip.base = -1;
+
+	ret = pwmchip_add(&jz4740->chip);
+	if (ret < 0) {
+		clk_put(jz4740->clk);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, jz4740);
+
+	return 0;
+}
+
+static int __devexit jz4740_pwm_remove(struct platform_device *pdev)
+{
+	struct jz4740_pwm_chip *jz4740 = platform_get_drvdata(pdev);
+	int ret;
+
+	ret = pwmchip_remove(&jz4740->chip);
+	if (ret < 0)
+		return ret;
+
+	clk_put(jz4740->clk);
+
+	return 0;
+}
+
+static struct platform_driver jz4740_pwm_driver = {
+	.driver = {
+		.name = "jz4740-pwm",
+		.owner = THIS_MODULE,
+	},
+	.probe = jz4740_pwm_probe,
+	.remove = __devexit_p(jz4740_pwm_remove),
+};
+module_platform_driver(jz4740_pwm_driver);
+
+MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
+MODULE_DESCRIPTION("Ingenic JZ4740 PWM driver");
+MODULE_ALIAS("platform:jz4740-pwm");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pwm/pwm-puv3.c b/drivers/pwm/pwm-puv3.c
new file mode 100644
index 00000000000..2a93f37c46a
--- /dev/null
+++ b/drivers/pwm/pwm-puv3.c
@@ -0,0 +1,161 @@
+/*
+ * linux/arch/unicore32/kernel/pwm.c
+ *
+ * Code specific to PKUnity SoC and UniCore ISA
+ *
+ *	Maintained by GUAN Xue-tao <gxt@mprc.pku.edu.cn>
+ *	Copyright (C) 2001-2010 Guan Xuetao
+ *
+ * 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/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/pwm.h>
+
+#include <asm/div64.h>
+#include <mach/hardware.h>
+
+struct puv3_pwm_chip {
+	struct pwm_chip chip;
+	void __iomem *base;
+	struct clk *clk;
+	bool enabled;
+};
+
+static inline struct puv3_pwm_chip *to_puv3(struct pwm_chip *chip)
+{
+	return container_of(chip, struct puv3_pwm_chip, chip);
+}
+
+/*
+ * period_ns = 10^9 * (PRESCALE + 1) * (PV + 1) / PWM_CLK_RATE
+ * duty_ns   = 10^9 * (PRESCALE + 1) * DC / PWM_CLK_RATE
+ */
+static int puv3_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
+			   int duty_ns, int period_ns)
+{
+	unsigned long period_cycles, prescale, pv, dc;
+	struct puv3_pwm_chip *puv3 = to_puv3(chip);
+	unsigned long long c;
+
+	c = clk_get_rate(puv3->clk);
+	c = c * period_ns;
+	do_div(c, 1000000000);
+	period_cycles = c;
+
+	if (period_cycles < 1)
+		period_cycles = 1;
+
+	prescale = (period_cycles - 1) / 1024;
+	pv = period_cycles / (prescale + 1) - 1;
+
+	if (prescale > 63)
+		return -EINVAL;
+
+	if (duty_ns == period_ns)
+		dc = OST_PWMDCCR_FDCYCLE;
+	else
+		dc = (pv + 1) * duty_ns / period_ns;
+
+	/*
+	 * NOTE: the clock to PWM has to be enabled first
+	 * before writing to the registers
+	 */
+	clk_prepare_enable(puv3->clk);
+
+	writel(prescale, puv3->base + OST_PWM_PWCR);
+	writel(pv - dc, puv3->base + OST_PWM_DCCR);
+	writel(pv, puv3->base + OST_PWM_PCR);
+
+	clk_disable_unprepare(puv3->clk);
+
+	return 0;
+}
+
+static int puv3_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct puv3_pwm_chip *puv3 = to_puv3(chip);
+
+	return clk_prepare_enable(puv3->clk);
+}
+
+static void puv3_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
+{
+	struct puv3_pwm_chip *puv3 = to_puv3(chip);
+
+	clk_disable_unprepare(puv3->clk);
+}
+
+static const struct pwm_ops puv3_pwm_ops = {
+	.config = puv3_pwm_config,
+	.enable = puv3_pwm_enable,
+	.disable = puv3_pwm_disable,
+	.owner = THIS_MODULE,
+};
+
+static int __devinit pwm_probe(struct platform_device *pdev)
+{
+	struct puv3_pwm_chip *puv3;
+	struct resource *r;
+	int ret;
+
+	puv3 = devm_kzalloc(&pdev->dev, sizeof(*puv3), GFP_KERNEL);
+	if (puv3 == NULL) {
+		dev_err(&pdev->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	puv3->clk = devm_clk_get(&pdev->dev, "OST_CLK");
+	if (IS_ERR(puv3->clk))
+		return PTR_ERR(puv3->clk);
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (r == NULL) {
+		dev_err(&pdev->dev, "no memory resource defined\n");
+		return -ENODEV;
+	}
+
+	puv3->base = devm_request_and_ioremap(&pdev->dev, r);
+	if (puv3->base == NULL)
+		return -EADDRNOTAVAIL;
+
+	puv3->chip.dev = &pdev->dev;
+	puv3->chip.ops = &puv3_pwm_ops;
+	puv3->chip.base = -1;
+	puv3->chip.npwm = 1;
+
+	ret = pwmchip_add(&puv3->chip);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "pwmchip_add() failed: %d\n", ret);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, puv3);
+	return 0;
+}
+
+static int __devexit pwm_remove(struct platform_device *pdev)
+{
+	struct puv3_pwm_chip *puv3 = platform_get_drvdata(pdev);
+
+	return pwmchip_remove(&puv3->chip);
+}
+
+static struct platform_driver puv3_pwm_driver = {
+	.driver = {
+		.name = "PKUnity-v3-PWM",
+	},
+	.probe = pwm_probe,
+	.remove = __devexit_p(pwm_remove),
+};
+module_platform_driver(puv3_pwm_driver);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pwm/pwm-pxa.c b/drivers/pwm/pwm-pxa.c
index bd5867a1c70..260c3a88564 100644
--- a/drivers/pwm/pwm-pxa.c
+++ b/drivers/pwm/pwm-pxa.c
@@ -70,9 +70,6 @@ static int pxa_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 	unsigned long offset;
 	int rc;
 
-	if (period_ns == 0 || duty_ns > period_ns)
-		return -EINVAL;
-
 	offset = pwm->hwpwm ? 0x10 : 0;
 
 	c = clk_get_rate(pc->clk);
diff --git a/drivers/pwm/pwm-samsung.c b/drivers/pwm/pwm-samsung.c
index e5187c0ade9..023a3bee76e 100644
--- a/drivers/pwm/pwm-samsung.c
+++ b/drivers/pwm/pwm-samsung.c
@@ -126,9 +126,6 @@ static int s3c_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 	if (period_ns > NS_IN_HZ || duty_ns > NS_IN_HZ)
 		return -ERANGE;
 
-	if (duty_ns > period_ns)
-		return -EINVAL;
-
 	if (period_ns == s3c->period_ns &&
 	    duty_ns == s3c->duty_ns)
 		return 0;
diff --git a/drivers/pwm/pwm-tiecap.c b/drivers/pwm/pwm-tiecap.c
index 4b6688909fe..d6d4cf05565 100644
--- a/drivers/pwm/pwm-tiecap.c
+++ b/drivers/pwm/pwm-tiecap.c
@@ -32,6 +32,7 @@
 #define CAP3			0x10
 #define CAP4			0x14
 #define ECCTL2			0x2A
+#define ECCTL2_APWM_POL_LOW	BIT(10)
 #define ECCTL2_APWM_MODE	BIT(9)
 #define ECCTL2_SYNC_SEL_DISA	(BIT(7) | BIT(6))
 #define ECCTL2_TSCTR_FREERUN	BIT(4)
@@ -59,7 +60,7 @@ static int ecap_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 	unsigned long period_cycles, duty_cycles;
 	unsigned int reg_val;
 
-	if (period_ns < 0 || duty_ns < 0 || period_ns > NSEC_PER_SEC)
+	if (period_ns > NSEC_PER_SEC)
 		return -ERANGE;
 
 	c = pc->clk_rate;
@@ -111,6 +112,26 @@ static int ecap_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 	return 0;
 }
 
+static int ecap_pwm_set_polarity(struct pwm_chip *chip, struct pwm_device *pwm,
+		enum pwm_polarity polarity)
+{
+	struct ecap_pwm_chip *pc = to_ecap_pwm_chip(chip);
+	unsigned short reg_val;
+
+	pm_runtime_get_sync(pc->chip.dev);
+	reg_val = readw(pc->mmio_base + ECCTL2);
+	if (polarity == PWM_POLARITY_INVERSED)
+		/* Duty cycle defines LOW period of PWM */
+		reg_val |= ECCTL2_APWM_POL_LOW;
+	else
+		/* Duty cycle defines HIGH period of PWM */
+		reg_val &= ~ECCTL2_APWM_POL_LOW;
+
+	writew(reg_val, pc->mmio_base + ECCTL2);
+	pm_runtime_put_sync(pc->chip.dev);
+	return 0;
+}
+
 static int ecap_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
 	struct ecap_pwm_chip *pc = to_ecap_pwm_chip(chip);
@@ -157,6 +178,7 @@ static void ecap_pwm_free(struct pwm_chip *chip, struct pwm_device *pwm)
 static const struct pwm_ops ecap_pwm_ops = {
 	.free		= ecap_pwm_free,
 	.config		= ecap_pwm_config,
+	.set_polarity	= ecap_pwm_set_polarity,
 	.enable		= ecap_pwm_enable,
 	.disable	= ecap_pwm_disable,
 	.owner		= THIS_MODULE,
diff --git a/drivers/pwm/pwm-tiehrpwm.c b/drivers/pwm/pwm-tiehrpwm.c
index b1996bcd5b7..d3c1dff0a0d 100644
--- a/drivers/pwm/pwm-tiehrpwm.c
+++ b/drivers/pwm/pwm-tiehrpwm.c
@@ -81,6 +81,15 @@
 #define AQCTL_ZRO_FRCHIGH	BIT(1)
 #define AQCTL_ZRO_FRCTOGGLE	(BIT(1) | BIT(0))
 
+#define AQCTL_CHANA_POLNORMAL	(AQCTL_CAU_FRCLOW | AQCTL_PRD_FRCHIGH | \
+				AQCTL_ZRO_FRCHIGH)
+#define AQCTL_CHANA_POLINVERSED	(AQCTL_CAU_FRCHIGH | AQCTL_PRD_FRCLOW | \
+				AQCTL_ZRO_FRCLOW)
+#define AQCTL_CHANB_POLNORMAL	(AQCTL_CBU_FRCLOW | AQCTL_PRD_FRCHIGH | \
+				AQCTL_ZRO_FRCHIGH)
+#define AQCTL_CHANB_POLINVERSED	(AQCTL_CBU_FRCHIGH | AQCTL_PRD_FRCLOW | \
+				AQCTL_ZRO_FRCLOW)
+
 #define AQSFRC_RLDCSF_MASK	(BIT(7) | BIT(6))
 #define AQSFRC_RLDCSF_ZRO	0
 #define AQSFRC_RLDCSF_PRD	BIT(6)
@@ -105,6 +114,7 @@ struct ehrpwm_pwm_chip {
 	unsigned int	clk_rate;
 	void __iomem	*mmio_base;
 	unsigned long period_cycles[NUM_PWM_CHANNEL];
+	enum pwm_polarity polarity[NUM_PWM_CHANNEL];
 };
 
 static inline struct ehrpwm_pwm_chip *to_ehrpwm_pwm_chip(struct pwm_chip *chip)
@@ -165,39 +175,37 @@ static int set_prescale_div(unsigned long rqst_prescaler,
 	return 1;
 }
 
-static void configure_chans(struct ehrpwm_pwm_chip *pc, int chan,
-		unsigned long duty_cycles)
+static void configure_polarity(struct ehrpwm_pwm_chip *pc, int chan)
 {
-	int cmp_reg, aqctl_reg;
+	int aqctl_reg;
 	unsigned short aqctl_val, aqctl_mask;
 
 	/*
-	 * Channels can be configured from action qualifier module.
-	 * Channel 0 configured with compare A register and for
-	 * up-counter mode.
-	 * Channel 1 configured with compare B register and for
-	 * up-counter mode.
+	 * Configure PWM output to HIGH/LOW level on counter
+	 * reaches compare register value and LOW/HIGH level
+	 * on counter value reaches period register value and
+	 * zero value on counter
 	 */
 	if (chan == 1) {
 		aqctl_reg = AQCTLB;
-		cmp_reg = CMPB;
-		/* Configure PWM Low from compare B value */
-		aqctl_val = AQCTL_CBU_FRCLOW;
 		aqctl_mask = AQCTL_CBU_MASK;
+
+		if (pc->polarity[chan] == PWM_POLARITY_INVERSED)
+			aqctl_val = AQCTL_CHANB_POLINVERSED;
+		else
+			aqctl_val = AQCTL_CHANB_POLNORMAL;
 	} else {
-		cmp_reg = CMPA;
 		aqctl_reg = AQCTLA;
-		/* Configure PWM Low from compare A value*/
-		aqctl_val = AQCTL_CAU_FRCLOW;
 		aqctl_mask = AQCTL_CAU_MASK;
+
+		if (pc->polarity[chan] == PWM_POLARITY_INVERSED)
+			aqctl_val = AQCTL_CHANA_POLINVERSED;
+		else
+			aqctl_val = AQCTL_CHANA_POLNORMAL;
 	}
 
-	/* Configure PWM High from period value and zero value */
-	aqctl_val |= AQCTL_PRD_FRCHIGH | AQCTL_ZRO_FRCHIGH;
 	aqctl_mask |= AQCTL_PRD_MASK | AQCTL_ZRO_MASK;
-	ehrpwm_modify(pc->mmio_base,  aqctl_reg, aqctl_mask, aqctl_val);
-
-	ehrpwm_write(pc->mmio_base,  cmp_reg, duty_cycles);
+	ehrpwm_modify(pc->mmio_base, aqctl_reg, aqctl_mask, aqctl_val);
 }
 
 /*
@@ -211,9 +219,9 @@ static int ehrpwm_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 	unsigned long long c;
 	unsigned long period_cycles, duty_cycles;
 	unsigned short ps_divval, tb_divval;
-	int i;
+	int i, cmp_reg;
 
-	if (period_ns < 0 || duty_ns < 0 || period_ns > NSEC_PER_SEC)
+	if (period_ns > NSEC_PER_SEC)
 		return -ERANGE;
 
 	c = pc->clk_rate;
@@ -278,12 +286,29 @@ static int ehrpwm_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 	ehrpwm_modify(pc->mmio_base, TBCTL, TBCTL_CTRMODE_MASK,
 			TBCTL_CTRMODE_UP);
 
-	/* Configure the channel for duty cycle */
-	configure_chans(pc, pwm->hwpwm, duty_cycles);
+	if (pwm->hwpwm == 1)
+		/* Channel 1 configured with compare B register */
+		cmp_reg = CMPB;
+	else
+		/* Channel 0 configured with compare A register */
+		cmp_reg = CMPA;
+
+	ehrpwm_write(pc->mmio_base, cmp_reg, duty_cycles);
+
 	pm_runtime_put_sync(chip->dev);
 	return 0;
 }
 
+static int ehrpwm_pwm_set_polarity(struct pwm_chip *chip,
+		struct pwm_device *pwm,	enum pwm_polarity polarity)
+{
+	struct ehrpwm_pwm_chip *pc = to_ehrpwm_pwm_chip(chip);
+
+	/* Configuration of polarity in hardware delayed, do at enable */
+	pc->polarity[pwm->hwpwm] = polarity;
+	return 0;
+}
+
 static int ehrpwm_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
 	struct ehrpwm_pwm_chip *pc = to_ehrpwm_pwm_chip(chip);
@@ -307,6 +332,9 @@ static int ehrpwm_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 
 	ehrpwm_modify(pc->mmio_base, AQCSFRC, aqcsfrc_mask, aqcsfrc_val);
 
+	/* Channels polarity can be configured from action qualifier module */
+	configure_polarity(pc, pwm->hwpwm);
+
 	/* Enable time counter for free_run */
 	ehrpwm_modify(pc->mmio_base, TBCTL, TBCTL_RUN_MASK, TBCTL_FREE_RUN);
 	return 0;
@@ -358,6 +386,7 @@ static void ehrpwm_pwm_free(struct pwm_chip *chip, struct pwm_device *pwm)
 static const struct pwm_ops ehrpwm_pwm_ops = {
 	.free		= ehrpwm_pwm_free,
 	.config		= ehrpwm_pwm_config,
+	.set_polarity	= ehrpwm_pwm_set_polarity,
 	.enable		= ehrpwm_pwm_enable,
 	.disable	= ehrpwm_pwm_disable,
 	.owner		= THIS_MODULE,
diff --git a/drivers/rapidio/rio-scan.c b/drivers/rapidio/rio-scan.c
index 48e9041dd1e..07da58bb495 100644
--- a/drivers/rapidio/rio-scan.c
+++ b/drivers/rapidio/rio-scan.c
@@ -55,9 +55,9 @@ static int rio_mport_phys_table[] = {
 };
 
 
-/*
+/**
  * rio_destid_alloc - Allocate next available destID for given network
- * net: RIO network
+ * @net: RIO network
  *
  * Returns next available device destination ID for the specified RIO network.
  * Marks allocated ID as one in use.
@@ -69,14 +69,9 @@ static u16 rio_destid_alloc(struct rio_net *net)
 	struct rio_id_table *idtab = &net->destid_table;
 
 	spin_lock(&idtab->lock);
-	destid = find_next_zero_bit(idtab->table, idtab->max, idtab->next);
-	if (destid >= idtab->max)
-		destid = find_first_zero_bit(idtab->table, idtab->max);
+	destid = find_first_zero_bit(idtab->table, idtab->max);
 
 	if (destid < idtab->max) {
-		idtab->next = destid + 1;
-		if (idtab->next >= idtab->max)
-			idtab->next = 0;
 		set_bit(destid, idtab->table);
 		destid += idtab->start;
 	} else
@@ -86,10 +81,10 @@ static u16 rio_destid_alloc(struct rio_net *net)
 	return (u16)destid;
 }
 
-/*
+/**
  * rio_destid_reserve - Reserve the specivied destID
- * net: RIO network
- * destid: destID to reserve
+ * @net: RIO network
+ * @destid: destID to reserve
  *
  * Tries to reserve the specified destID.
  * Returns 0 if successfull.
@@ -106,10 +101,10 @@ static int rio_destid_reserve(struct rio_net *net, u16 destid)
 	return oldbit;
 }
 
-/*
+/**
  * rio_destid_free - free a previously allocated destID
- * net: RIO network
- * destid: destID to free
+ * @net: RIO network
+ * @destid: destID to free
  *
  * Makes the specified destID available for use.
  */
@@ -123,9 +118,9 @@ static void rio_destid_free(struct rio_net *net, u16 destid)
 	spin_unlock(&idtab->lock);
 }
 
-/*
+/**
  * rio_destid_first - return first destID in use
- * net: RIO network
+ * @net: RIO network
  */
 static u16 rio_destid_first(struct rio_net *net)
 {
@@ -142,10 +137,10 @@ static u16 rio_destid_first(struct rio_net *net)
 	return (u16)destid;
 }
 
-/*
+/**
  * rio_destid_next - return next destID in use
- * net: RIO network
- * from: destination ID from which search shall continue
+ * @net: RIO network
+ * @from: destination ID from which search shall continue
  */
 static u16 rio_destid_next(struct rio_net *net, u16 from)
 {
@@ -1163,8 +1158,8 @@ static struct rio_net __devinit *rio_alloc_net(struct rio_mport *port,
 
 	net = kzalloc(sizeof(struct rio_net), GFP_KERNEL);
 	if (net && do_enum) {
-		net->destid_table.table = kzalloc(
-			BITS_TO_LONGS(RIO_MAX_ROUTE_ENTRIES(port->sys_size)) *
+		net->destid_table.table = kcalloc(
+			BITS_TO_LONGS(RIO_MAX_ROUTE_ENTRIES(port->sys_size)),
 			sizeof(long),
 			GFP_KERNEL);
 
@@ -1174,7 +1169,6 @@ static struct rio_net __devinit *rio_alloc_net(struct rio_mport *port,
 			net = NULL;
 		} else {
 			net->destid_table.start = start;
-			net->destid_table.next = 0;
 			net->destid_table.max =
 					RIO_MAX_ROUTE_ENTRIES(port->sys_size);
 			spin_lock_init(&net->destid_table.lock);
@@ -1391,7 +1385,7 @@ int __devinit rio_disc_mport(struct rio_mport *mport)
 		while (time_before(jiffies, to_end)) {
 			if (rio_enum_complete(mport))
 				goto enum_done;
-			schedule_timeout_uninterruptible(msecs_to_jiffies(10));
+			msleep(10);
 		}
 
 		pr_debug("RIO: discovery timeout on mport %d %s\n",
diff --git a/drivers/rapidio/rio.c b/drivers/rapidio/rio.c
index d4bd69013c5..c17ae22567e 100644
--- a/drivers/rapidio/rio.c
+++ b/drivers/rapidio/rio.c
@@ -1275,49 +1275,68 @@ static void __devinit disc_work_handler(struct work_struct *_work)
 	pr_debug("RIO: discovery work for mport %d %s\n",
 		 work->mport->id, work->mport->name);
 	rio_disc_mport(work->mport);
-
-	kfree(work);
 }
 
 int __devinit rio_init_mports(void)
 {
 	struct rio_mport *port;
 	struct rio_disc_work *work;
-	int no_disc = 0;
+	int n = 0;
+
+	if (!next_portid)
+		return -ENODEV;
 
+	/*
+	 * First, run enumerations and check if we need to perform discovery
+	 * on any of the registered mports.
+	 */
 	list_for_each_entry(port, &rio_mports, node) {
 		if (port->host_deviceid >= 0)
 			rio_enum_mport(port);
-		else if (!no_disc) {
-			if (!rio_wq) {
-				rio_wq = alloc_workqueue("riodisc", 0, 0);
-				if (!rio_wq) {
-					pr_err("RIO: unable allocate rio_wq\n");
-					no_disc = 1;
-					continue;
-				}
-			}
-
-			work = kzalloc(sizeof *work, GFP_KERNEL);
-			if (!work) {
-				pr_err("RIO: no memory for work struct\n");
-				no_disc = 1;
-				continue;
-			}
-
-			work->mport = port;
-			INIT_WORK(&work->work, disc_work_handler);
-			queue_work(rio_wq, &work->work);
-		}
+		else
+			n++;
+	}
+
+	if (!n)
+		goto no_disc;
+
+	/*
+	 * If we have mports that require discovery schedule a discovery work
+	 * for each of them. If the code below fails to allocate needed
+	 * resources, exit without error to keep results of enumeration
+	 * process (if any).
+	 * TODO: Implement restart of dicovery process for all or
+	 * individual discovering mports.
+	 */
+	rio_wq = alloc_workqueue("riodisc", 0, 0);
+	if (!rio_wq) {
+		pr_err("RIO: unable allocate rio_wq\n");
+		goto no_disc;
 	}
 
-	if (rio_wq) {
-		pr_debug("RIO: flush discovery workqueue\n");
-		flush_workqueue(rio_wq);
-		pr_debug("RIO: flush discovery workqueue finished\n");
+	work = kcalloc(n, sizeof *work, GFP_KERNEL);
+	if (!work) {
+		pr_err("RIO: no memory for work struct\n");
 		destroy_workqueue(rio_wq);
+		goto no_disc;
 	}
 
+	n = 0;
+	list_for_each_entry(port, &rio_mports, node) {
+		if (port->host_deviceid < 0) {
+			work[n].mport = port;
+			INIT_WORK(&work[n].work, disc_work_handler);
+			queue_work(rio_wq, &work[n].work);
+			n++;
+		}
+	}
+
+	flush_workqueue(rio_wq);
+	pr_debug("RIO: destroy discovery workqueue\n");
+	destroy_workqueue(rio_wq);
+	kfree(work);
+
+no_disc:
 	rio_init();
 
 	return 0;
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index e069f176a82..19c03ab2bdc 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -59,6 +59,7 @@ comment "RTC interfaces"
 config RTC_INTF_SYSFS
 	boolean "/sys/class/rtc/rtcN (sysfs)"
 	depends on SYSFS
+	default RTC_CLASS
 	help
 	  Say yes here if you want to use your RTCs using sysfs interfaces,
 	  /sys/class/rtc/rtc0 through /sys/.../rtcN.
@@ -68,6 +69,7 @@ config RTC_INTF_SYSFS
 config RTC_INTF_PROC
 	boolean "/proc/driver/rtc (procfs for rtcN)"
 	depends on PROC_FS
+	default RTC_CLASS
 	help
 	  Say yes here if you want to use your system clock RTC through
 	  the proc interface, /proc/driver/rtc.
@@ -79,6 +81,7 @@ config RTC_INTF_PROC
 
 config RTC_INTF_DEV
 	boolean "/dev/rtcN (character devices)"
+	default RTC_CLASS
 	help
 	  Say yes here if you want to use your RTCs using the /dev
 	  interfaces, which "udev" sets up as /dev/rtc0 through
diff --git a/drivers/s390/block/dcssblk.c b/drivers/s390/block/dcssblk.c
index a5a55da2a1a..b6ad0de0793 100644
--- a/drivers/s390/block/dcssblk.c
+++ b/drivers/s390/block/dcssblk.c
@@ -69,23 +69,9 @@ static ssize_t dcssblk_add_store(struct device * dev, struct device_attribute *a
 				  size_t count);
 static ssize_t dcssblk_remove_store(struct device * dev, struct device_attribute *attr, const char * buf,
 				  size_t count);
-static ssize_t dcssblk_save_store(struct device * dev, struct device_attribute *attr, const char * buf,
-				  size_t count);
-static ssize_t dcssblk_save_show(struct device *dev, struct device_attribute *attr, char *buf);
-static ssize_t dcssblk_shared_store(struct device * dev, struct device_attribute *attr, const char * buf,
-				  size_t count);
-static ssize_t dcssblk_shared_show(struct device *dev, struct device_attribute *attr, char *buf);
-static ssize_t dcssblk_seglist_show(struct device *dev,
-				struct device_attribute *attr,
-				char *buf);
 
 static DEVICE_ATTR(add, S_IWUSR, NULL, dcssblk_add_store);
 static DEVICE_ATTR(remove, S_IWUSR, NULL, dcssblk_remove_store);
-static DEVICE_ATTR(save, S_IWUSR | S_IRUSR, dcssblk_save_show,
-		   dcssblk_save_store);
-static DEVICE_ATTR(shared, S_IWUSR | S_IRUSR, dcssblk_shared_show,
-		   dcssblk_shared_store);
-static DEVICE_ATTR(seglist, S_IRUSR, dcssblk_seglist_show, NULL);
 
 static struct device *dcssblk_root_dev;
 
@@ -416,6 +402,8 @@ out:
 	up_write(&dcssblk_devices_sem);
 	return rc;
 }
+static DEVICE_ATTR(shared, S_IWUSR | S_IRUSR, dcssblk_shared_show,
+		   dcssblk_shared_store);
 
 /*
  * device attribute for save operation on current copy
@@ -476,6 +464,8 @@ dcssblk_save_store(struct device *dev, struct device_attribute *attr, const char
 	up_write(&dcssblk_devices_sem);
 	return count;
 }
+static DEVICE_ATTR(save, S_IWUSR | S_IRUSR, dcssblk_save_show,
+		   dcssblk_save_store);
 
 /*
  * device attribute for showing all segments in a device
@@ -502,6 +492,21 @@ dcssblk_seglist_show(struct device *dev, struct device_attribute *attr,
 	up_read(&dcssblk_devices_sem);
 	return i;
 }
+static DEVICE_ATTR(seglist, S_IRUSR, dcssblk_seglist_show, NULL);
+
+static struct attribute *dcssblk_dev_attrs[] = {
+	&dev_attr_shared.attr,
+	&dev_attr_save.attr,
+	&dev_attr_seglist.attr,
+	NULL,
+};
+static struct attribute_group dcssblk_dev_attr_group = {
+	.attrs = dcssblk_dev_attrs,
+};
+static const struct attribute_group *dcssblk_dev_attr_groups[] = {
+	&dcssblk_dev_attr_group,
+	NULL,
+};
 
 /*
  * device attribute for adding devices
@@ -590,6 +595,7 @@ dcssblk_add_store(struct device *dev, struct device_attribute *attr, const char
 
 	dev_set_name(&dev_info->dev, dev_info->segment_name);
 	dev_info->dev.release = dcssblk_release_segment;
+	dev_info->dev.groups = dcssblk_dev_attr_groups;
 	INIT_LIST_HEAD(&dev_info->lh);
 	dev_info->gd = alloc_disk(DCSSBLK_MINORS_PER_DISK);
 	if (dev_info->gd == NULL) {
@@ -637,21 +643,10 @@ dcssblk_add_store(struct device *dev, struct device_attribute *attr, const char
 	 * register the device
 	 */
 	rc = device_register(&dev_info->dev);
-	if (rc) {
-		module_put(THIS_MODULE);
-		goto dev_list_del;
-	}
-	get_device(&dev_info->dev);
-	rc = device_create_file(&dev_info->dev, &dev_attr_shared);
-	if (rc)
-		goto unregister_dev;
-	rc = device_create_file(&dev_info->dev, &dev_attr_save);
-	if (rc)
-		goto unregister_dev;
-	rc = device_create_file(&dev_info->dev, &dev_attr_seglist);
 	if (rc)
-		goto unregister_dev;
+		goto put_dev;
 
+	get_device(&dev_info->dev);
 	add_disk(dev_info->gd);
 
 	switch (dev_info->segment_type) {
@@ -668,12 +663,11 @@ dcssblk_add_store(struct device *dev, struct device_attribute *attr, const char
 	rc = count;
 	goto out;
 
-unregister_dev:
+put_dev:
 	list_del(&dev_info->lh);
 	blk_cleanup_queue(dev_info->dcssblk_queue);
 	dev_info->gd->queue = NULL;
 	put_disk(dev_info->gd);
-	device_unregister(&dev_info->dev);
 	list_for_each_entry(seg_info, &dev_info->seg_list, lh) {
 		segment_unload(seg_info->segment_name);
 	}
diff --git a/drivers/s390/crypto/zcrypt_pcixcc.c b/drivers/s390/crypto/zcrypt_pcixcc.c
index c7275e303a0..899ffa19f5e 100644
--- a/drivers/s390/crypto/zcrypt_pcixcc.c
+++ b/drivers/s390/crypto/zcrypt_pcixcc.c
@@ -39,7 +39,6 @@
 #include "zcrypt_msgtype6.h"
 #include "zcrypt_pcixcc.h"
 #include "zcrypt_cca_key.h"
-#include "zcrypt_msgtype6.h"
 
 #define PCIXCC_MIN_MOD_SIZE	 16	/*  128 bits	*/
 #define PCIXCC_MIN_MOD_SIZE_OLD	 64	/*  512 bits	*/
diff --git a/drivers/scsi/bfa/bfa_core.c b/drivers/scsi/bfa/bfa_core.c
index b7c326f7a6d..342d7d9c099 100644
--- a/drivers/scsi/bfa/bfa_core.c
+++ b/drivers/scsi/bfa/bfa_core.c
@@ -165,6 +165,16 @@ bfa_com_phy_attach(struct bfa_s *bfa, bfa_boolean_t mincfg)
 	bfa_phy_memclaim(phy, phy_dma->kva_curp, phy_dma->dma_curp, mincfg);
 }
 
+static void
+bfa_com_fru_attach(struct bfa_s *bfa, bfa_boolean_t mincfg)
+{
+	struct bfa_fru_s	*fru = BFA_FRU(bfa);
+	struct bfa_mem_dma_s	*fru_dma = BFA_MEM_FRU_DMA(bfa);
+
+	bfa_fru_attach(fru, &bfa->ioc, bfa, bfa->trcmod, mincfg);
+	bfa_fru_memclaim(fru, fru_dma->kva_curp, fru_dma->dma_curp, mincfg);
+}
+
 /*
  * BFA IOC FC related definitions
  */
@@ -274,6 +284,15 @@ bfa_iocfc_sm_initing(struct bfa_iocfc_s *iocfc, enum iocfc_event event)
 	case IOCFC_E_IOC_ENABLED:
 		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_dconf_read);
 		break;
+
+	case IOCFC_E_DISABLE:
+		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_disabling);
+		break;
+
+	case IOCFC_E_STOP:
+		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_stopping);
+		break;
+
 	case IOCFC_E_IOC_FAILED:
 		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_init_failed);
 		break;
@@ -298,6 +317,15 @@ bfa_iocfc_sm_dconf_read(struct bfa_iocfc_s *iocfc, enum iocfc_event event)
 	case IOCFC_E_DCONF_DONE:
 		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_init_cfg_wait);
 		break;
+
+	case IOCFC_E_DISABLE:
+		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_disabling);
+		break;
+
+	case IOCFC_E_STOP:
+		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_stopping);
+		break;
+
 	case IOCFC_E_IOC_FAILED:
 		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_init_failed);
 		break;
@@ -322,6 +350,15 @@ bfa_iocfc_sm_init_cfg_wait(struct bfa_iocfc_s *iocfc, enum iocfc_event event)
 	case IOCFC_E_CFG_DONE:
 		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_init_cfg_done);
 		break;
+
+	case IOCFC_E_DISABLE:
+		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_disabling);
+		break;
+
+	case IOCFC_E_STOP:
+		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_stopping);
+		break;
+
 	case IOCFC_E_IOC_FAILED:
 		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_init_failed);
 		break;
@@ -433,6 +470,12 @@ bfa_iocfc_sm_stopping(struct bfa_iocfc_s *iocfc, enum iocfc_event event)
 		bfa_cb_queue(iocfc->bfa, &iocfc->bfa->iocfc.stop_hcb_qe,
 			     bfa_iocfc_stop_cb, iocfc->bfa);
 		break;
+
+	case IOCFC_E_IOC_ENABLED:
+	case IOCFC_E_DCONF_DONE:
+	case IOCFC_E_CFG_DONE:
+		break;
+
 	default:
 		bfa_sm_fault(iocfc->bfa, event);
 		break;
@@ -454,6 +497,15 @@ bfa_iocfc_sm_enabling(struct bfa_iocfc_s *iocfc, enum iocfc_event event)
 	case IOCFC_E_IOC_ENABLED:
 		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_cfg_wait);
 		break;
+
+	case IOCFC_E_DISABLE:
+		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_disabling);
+		break;
+
+	case IOCFC_E_STOP:
+		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_dconf_write);
+		break;
+
 	case IOCFC_E_IOC_FAILED:
 		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_failed);
 
@@ -493,6 +545,13 @@ bfa_iocfc_sm_cfg_wait(struct bfa_iocfc_s *iocfc, enum iocfc_event event)
 			     bfa_iocfc_enable_cb, iocfc->bfa);
 		iocfc->bfa->iocfc.cb_reqd = BFA_FALSE;
 		break;
+	case IOCFC_E_DISABLE:
+		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_disabling);
+		break;
+
+	case IOCFC_E_STOP:
+		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_dconf_write);
+		break;
 	case IOCFC_E_IOC_FAILED:
 		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_failed);
 		if (iocfc->bfa->iocfc.cb_reqd == BFA_FALSE)
@@ -524,6 +583,10 @@ bfa_iocfc_sm_disabling(struct bfa_iocfc_s *iocfc, enum iocfc_event event)
 	case IOCFC_E_IOC_DISABLED:
 		bfa_fsm_set_state(iocfc, bfa_iocfc_sm_disabled);
 		break;
+	case IOCFC_E_IOC_ENABLED:
+	case IOCFC_E_DCONF_DONE:
+	case IOCFC_E_CFG_DONE:
+		break;
 	default:
 		bfa_sm_fault(iocfc->bfa, event);
 		break;
@@ -785,19 +848,20 @@ void
 bfa_isr_enable(struct bfa_s *bfa)
 {
 	u32 umsk;
-	int pci_func = bfa_ioc_pcifn(&bfa->ioc);
+	int port_id = bfa_ioc_portid(&bfa->ioc);
 
-	bfa_trc(bfa, pci_func);
+	bfa_trc(bfa, bfa_ioc_pcifn(&bfa->ioc));
+	bfa_trc(bfa, port_id);
 
 	bfa_msix_ctrl_install(bfa);
 
 	if (bfa_asic_id_ct2(bfa->ioc.pcidev.device_id)) {
 		umsk = __HFN_INT_ERR_MASK_CT2;
-		umsk |= pci_func == 0 ?
+		umsk |= port_id == 0 ?
 			__HFN_INT_FN0_MASK_CT2 : __HFN_INT_FN1_MASK_CT2;
 	} else {
 		umsk = __HFN_INT_ERR_MASK;
-		umsk |= pci_func == 0 ? __HFN_INT_FN0_MASK : __HFN_INT_FN1_MASK;
+		umsk |= port_id == 0 ? __HFN_INT_FN0_MASK : __HFN_INT_FN1_MASK;
 	}
 
 	writel(umsk, bfa->iocfc.bfa_regs.intr_status);
@@ -930,7 +994,8 @@ bfa_iocfc_send_cfg(void *bfa_arg)
 		cfg_info->single_msix_vec = 1;
 	cfg_info->endian_sig = BFI_IOC_ENDIAN_SIG;
 	cfg_info->num_cqs = cfg->fwcfg.num_cqs;
-	cfg_info->num_ioim_reqs = cpu_to_be16(cfg->fwcfg.num_ioim_reqs);
+	cfg_info->num_ioim_reqs = cpu_to_be16(bfa_fcpim_get_throttle_cfg(bfa,
+					       cfg->fwcfg.num_ioim_reqs));
 	cfg_info->num_fwtio_reqs = cpu_to_be16(cfg->fwcfg.num_fwtio_reqs);
 
 	bfa_dma_be_addr_set(cfg_info->cfgrsp_addr, iocfc->cfgrsp_dma.pa);
@@ -1192,10 +1257,14 @@ bfa_iocfc_qreg(struct bfa_s *bfa, struct bfi_iocfc_qreg_s *qreg)
 static void
 bfa_iocfc_res_recfg(struct bfa_s *bfa, struct bfa_iocfc_fwcfg_s *fwcfg)
 {
+	struct bfa_iocfc_s	*iocfc   = &bfa->iocfc;
+	struct bfi_iocfc_cfg_s	*cfg_info = iocfc->cfginfo;
+
 	bfa_fcxp_res_recfg(bfa, fwcfg->num_fcxp_reqs);
 	bfa_uf_res_recfg(bfa, fwcfg->num_uf_bufs);
 	bfa_rport_res_recfg(bfa, fwcfg->num_rports);
-	bfa_fcp_res_recfg(bfa, fwcfg->num_ioim_reqs);
+	bfa_fcp_res_recfg(bfa, cpu_to_be16(cfg_info->num_ioim_reqs),
+			  fwcfg->num_ioim_reqs);
 	bfa_tskim_res_recfg(bfa, fwcfg->num_tskim_reqs);
 }
 
@@ -1693,6 +1762,7 @@ bfa_cfg_get_meminfo(struct bfa_iocfc_cfg_s *cfg, struct bfa_meminfo_s *meminfo,
 	struct bfa_mem_dma_s *flash_dma = BFA_MEM_FLASH_DMA(bfa);
 	struct bfa_mem_dma_s *diag_dma = BFA_MEM_DIAG_DMA(bfa);
 	struct bfa_mem_dma_s *phy_dma = BFA_MEM_PHY_DMA(bfa);
+	struct bfa_mem_dma_s *fru_dma = BFA_MEM_FRU_DMA(bfa);
 
 	WARN_ON((cfg == NULL) || (meminfo == NULL));
 
@@ -1717,6 +1787,8 @@ bfa_cfg_get_meminfo(struct bfa_iocfc_cfg_s *cfg, struct bfa_meminfo_s *meminfo,
 	bfa_mem_dma_setup(meminfo, diag_dma, bfa_diag_meminfo());
 	bfa_mem_dma_setup(meminfo, phy_dma,
 			  bfa_phy_meminfo(cfg->drvcfg.min_cfg));
+	bfa_mem_dma_setup(meminfo, fru_dma,
+			  bfa_fru_meminfo(cfg->drvcfg.min_cfg));
 }
 
 /*
@@ -1789,6 +1861,7 @@ bfa_attach(struct bfa_s *bfa, void *bfad, struct bfa_iocfc_cfg_s *cfg,
 	bfa_com_flash_attach(bfa, cfg->drvcfg.min_cfg);
 	bfa_com_diag_attach(bfa);
 	bfa_com_phy_attach(bfa, cfg->drvcfg.min_cfg);
+	bfa_com_fru_attach(bfa, cfg->drvcfg.min_cfg);
 }
 
 /*
diff --git a/drivers/scsi/bfa/bfa_defs.h b/drivers/scsi/bfa/bfa_defs.h
index b5a1595cc0a..0efdf312b42 100644
--- a/drivers/scsi/bfa/bfa_defs.h
+++ b/drivers/scsi/bfa/bfa_defs.h
@@ -159,10 +159,13 @@ enum bfa_status {
 	BFA_STATUS_BEACON_ON    = 72,   /* Port Beacon already on */
 	BFA_STATUS_ENOFSAVE	= 78,	/*  No saved firmware trace */
 	BFA_STATUS_IOC_DISABLED = 82,   /* IOC is already disabled */
+	BFA_STATUS_ERROR_TRL_ENABLED  = 87,   /* TRL is enabled */
+	BFA_STATUS_ERROR_QOS_ENABLED  = 88,   /* QoS is enabled */
 	BFA_STATUS_NO_SFP_DEV = 89,	/* No SFP device check or replace SFP */
 	BFA_STATUS_MEMTEST_FAILED = 90, /* Memory test failed contact support */
 	BFA_STATUS_LEDTEST_OP = 109, /* LED test is operating */
 	BFA_STATUS_INVALID_MAC  = 134, /*  Invalid MAC address */
+	BFA_STATUS_CMD_NOTSUPP_CNA = 146, /* Command not supported for CNA */
 	BFA_STATUS_PBC		= 154, /*  Operation not allowed for pre-boot
 					*  configuration */
 	BFA_STATUS_BAD_FWCFG = 156,	/* Bad firmware configuration */
@@ -184,6 +187,17 @@ enum bfa_status {
 	BFA_STATUS_FAA_ACQ_ADDR = 200,	/* Acquiring addr */
 	BFA_STATUS_ERROR_TRUNK_ENABLED = 203,	/* Trunk enabled on adapter */
 	BFA_STATUS_MAX_ENTRY_REACHED = 212,	/* MAX entry reached */
+	BFA_STATUS_TOPOLOGY_LOOP = 230, /* Topology is set to Loop */
+	BFA_STATUS_LOOP_UNSUPP_MEZZ = 231, /* Loop topology is not supported
+					    * on mezz cards */
+	BFA_STATUS_INVALID_BW = 233,	/* Invalid bandwidth value */
+	BFA_STATUS_QOS_BW_INVALID = 234,   /* Invalid QOS bandwidth
+					    * configuration */
+	BFA_STATUS_DPORT_ENABLED = 235, /* D-port mode is already enabled */
+	BFA_STATUS_DPORT_DISABLED = 236, /* D-port mode is already disabled */
+	BFA_STATUS_CMD_NOTSUPP_MEZZ = 239, /* Cmd not supported for MEZZ card */
+	BFA_STATUS_FRU_NOT_PRESENT = 240, /* fru module not present */
+	BFA_STATUS_DPORT_ERR = 245,	/* D-port mode is enabled */
 	BFA_STATUS_MAX_VAL		/* Unknown error code */
 };
 #define bfa_status_t enum bfa_status
@@ -249,6 +263,10 @@ struct bfa_adapter_attr_s {
 
 	u8		is_mezz;
 	u8		trunk_capable;
+	u8		mfg_day;	/* manufacturing day */
+	u8		mfg_month;	/* manufacturing month */
+	u16		mfg_year;	/* manufacturing year */
+	u16		rsvd;
 };
 
 /*
@@ -499,6 +517,17 @@ struct bfa_ioc_aen_data_s {
 };
 
 /*
+ *	D-port states
+ *
+*/
+enum bfa_dport_state {
+	BFA_DPORT_ST_DISABLED	= 0,	/* D-port is Disabled */
+	BFA_DPORT_ST_DISABLING	= 1,	/* D-port is Disabling */
+	BFA_DPORT_ST_ENABLING	= 2,	/* D-port is Enabling */
+	BFA_DPORT_ST_ENABLED	= 3,	/* D-port is Enabled */
+};
+
+/*
  * ---------------------- mfg definitions ------------
  */
 
@@ -722,7 +751,8 @@ struct bfa_ablk_cfg_pf_s {
 	u8	rsvd[1];
 	u16	num_qpairs;
 	u16	num_vectors;
-	u32	bw;
+	u16	bw_min;
+	u16	bw_max;
 };
 
 struct bfa_ablk_cfg_port_s {
@@ -889,11 +919,40 @@ struct sfp_diag_ext_s {
 	u8	ext_status_ctl[2];
 };
 
+/*
+ * Diagnostic: Data Fields -- Address A2h
+ * General Use Fields: User Writable Table - Features's Control Registers
+ * Total 32 bytes
+ */
+struct sfp_usr_eeprom_s {
+	u8	rsvd1[2];       /* 128-129 */
+	u8	ewrap;          /* 130 */
+	u8	rsvd2[2];       /*  */
+	u8	owrap;          /* 133 */
+	u8	rsvd3[2];       /*  */
+	u8	prbs;           /* 136: PRBS 7 generator */
+	u8	rsvd4[2];       /*  */
+	u8	tx_eqz_16;      /* 139: TX Equalizer (16xFC) */
+	u8	tx_eqz_8;       /* 140: TX Equalizer (8xFC) */
+	u8	rsvd5[2];       /*  */
+	u8	rx_emp_16;      /* 143: RX Emphasis (16xFC) */
+	u8	rx_emp_8;       /* 144: RX Emphasis (8xFC) */
+	u8	rsvd6[2];       /*  */
+	u8	tx_eye_adj;     /* 147: TX eye Threshold Adjust */
+	u8	rsvd7[3];       /*  */
+	u8	tx_eye_qctl;    /* 151: TX eye Quality Control */
+	u8	tx_eye_qres;    /* 152: TX eye Quality Result */
+	u8	rsvd8[2];       /*  */
+	u8	poh[3];         /* 155-157: Power On Hours */
+	u8	rsvd9[2];       /*  */
+};
+
 struct sfp_mem_s {
 	struct sfp_srlid_base_s	srlid_base;
 	struct sfp_srlid_ext_s	srlid_ext;
 	struct sfp_diag_base_s	diag_base;
 	struct sfp_diag_ext_s	diag_ext;
+	struct sfp_usr_eeprom_s usr_eeprom;
 };
 
 /*
diff --git a/drivers/scsi/bfa/bfa_defs_svc.h b/drivers/scsi/bfa/bfa_defs_svc.h
index 36756ce0e58..ec03c8cd8da 100644
--- a/drivers/scsi/bfa/bfa_defs_svc.h
+++ b/drivers/scsi/bfa/bfa_defs_svc.h
@@ -258,6 +258,7 @@ struct bfa_fw_port_lksm_stats_s {
 	u32    hwsm_lrr_rx;        /*  No. of times LRR rx-ed by HWSM      */
 	u32    hwsm_lr_rx;         /*  No. of times LR rx-ed by HWSM       */
 	u32    bbsc_lr;		   /* LKSM LR tx for credit recovery       */
+	u32	rsvd;
 };
 
 struct bfa_fw_port_snsm_stats_s {
@@ -270,6 +271,9 @@ struct bfa_fw_port_snsm_stats_s {
 	u32    sync_lost;          /*  Sync loss count                     */
 	u32    sig_lost;           /*  Signal loss count                   */
 	u32    asn8g_attempts;	   /* SNSM HWSM at 8Gbps attempts	   */
+	u32    adapt_success;	   /* SNSM adaptation success	*/
+	u32    adapt_fails;	   /* SNSM adaptation failures */
+	u32    adapt_ign_fails;	   /* SNSM adaptation failures ignored */
 };
 
 struct bfa_fw_port_physm_stats_s {
@@ -324,12 +328,46 @@ struct bfa_fw_fcoe_port_stats_s {
 	struct bfa_fw_fip_stats_s		fip_stats;
 };
 
+/**
+ * @brief LPSM statistics
+ */
+struct bfa_fw_lpsm_stats_s {
+	u32	cls_rx;		/* LPSM cls_rx			*/
+	u32	cls_tx;		/* LPSM cls_tx			*/
+	u32	arbf0_rx;	/* LPSM abrf0 rcvd		*/
+	u32	arbf0_tx;	/* LPSM abrf0 xmit		*/
+	u32	init_rx;	/* LPSM loop init start		*/
+	u32	unexp_hwst;	/* LPSM unknown hw state	*/
+	u32	unexp_frame;	/* LPSM unknown_frame		*/
+	u32	unexp_prim;	/* LPSM unexpected primitive	*/
+	u32	prev_alpa_unavail; /* LPSM prev alpa unavailable */
+	u32	alpa_unavail;	/* LPSM alpa not available	*/
+	u32	lip_rx;		/* LPSM lip rcvd		*/
+	u32	lip_f7f7_rx;	/* LPSM lip f7f7 rcvd		*/
+	u32	lip_f8_rx;	/* LPSM lip f8 rcvd		*/
+	u32	lip_f8f7_rx;	/* LPSM lip f8f7 rcvd		*/
+	u32	lip_other_rx;	/* LPSM lip other rcvd		*/
+	u32	lip_tx;		/* LPSM lip xmit		*/
+	u32	retry_tov;	/* LPSM retry TOV		*/
+	u32	lip_tov;	/* LPSM LIP wait TOV		*/
+	u32	idle_tov;	/* LPSM idle wait TOV		*/
+	u32	arbf0_tov;	/* LPSM arbfo wait TOV		*/
+	u32	stop_loop_tov;	/* LPSM stop loop wait TOV	*/
+	u32	lixa_tov;	/* LPSM lisa wait TOV		*/
+	u32	lixx_tov;	/* LPSM lilp/lirp wait TOV	*/
+	u32	cls_tov;	/* LPSM cls wait TOV		*/
+	u32	sler;		/* LPSM SLER recvd		*/
+	u32	failed;		/* LPSM failed			*/
+	u32	success;	/* LPSM online			*/
+};
+
 /*
  * IOC firmware FC uport stats
  */
 struct bfa_fw_fc_uport_stats_s {
 	struct bfa_fw_port_snsm_stats_s		snsm_stats;
 	struct bfa_fw_port_lksm_stats_s		lksm_stats;
+	struct bfa_fw_lpsm_stats_s		lpsm_stats;
 };
 
 /*
@@ -357,11 +395,6 @@ struct bfa_fw_fcxchg_stats_s {
 	u32	ua_state_inv;
 };
 
-struct bfa_fw_lpsm_stats_s {
-	u32	cls_rx;
-	u32	cls_tx;
-};
-
 /*
  *  Trunk statistics
  */
@@ -454,7 +487,6 @@ struct bfa_fw_stats_s {
 	struct bfa_fw_io_stats_s	io_stats;
 	struct bfa_fw_port_stats_s	port_stats;
 	struct bfa_fw_fcxchg_stats_s	fcxchg_stats;
-	struct bfa_fw_lpsm_stats_s	lpsm_stats;
 	struct bfa_fw_lps_stats_s	lps_stats;
 	struct bfa_fw_trunk_stats_s	trunk_stats;
 	struct bfa_fw_advsm_stats_s	advsm_stats;
@@ -494,13 +526,23 @@ enum bfa_qos_bw_alloc {
 	BFA_QOS_BW_LOW  =  10,	/*  bandwidth allocation for Low */
 };
 #pragma pack(1)
+
+struct bfa_qos_bw_s {
+	u8	qos_bw_set;
+	u8	high;
+	u8	med;
+	u8	low;
+};
+
 /*
  * QoS attribute returned in QoS Query
  */
 struct bfa_qos_attr_s {
-	u8		state;		/*  QoS current state */
-	u8		rsvd[3];
-	u32  total_bb_cr;		/*  Total BB Credits */
+	u8	state;		/*  QoS current state */
+	u8	rsvd1[3];
+	u32	total_bb_cr;	/*  Total BB Credits */
+	struct bfa_qos_bw_s qos_bw;	/* QOS bw cfg */
+	struct bfa_qos_bw_s qos_bw_op;	/* QOS bw operational */
 };
 
 /*
@@ -692,7 +734,8 @@ enum bfa_port_states {
 	BFA_PORT_ST_FWMISMATCH		= 12,
 	BFA_PORT_ST_PREBOOT_DISABLED	= 13,
 	BFA_PORT_ST_TOGGLING_QWAIT	= 14,
-	BFA_PORT_ST_ACQ_ADDR		= 15,
+	BFA_PORT_ST_FAA_MISCONFIG	= 15,
+	BFA_PORT_ST_DPORT		= 16,
 	BFA_PORT_ST_MAX_STATE,
 };
 
@@ -714,9 +757,11 @@ enum bfa_port_type {
  */
 enum bfa_port_topology {
 	BFA_PORT_TOPOLOGY_NONE = 0,	/*  No valid topology */
-	BFA_PORT_TOPOLOGY_P2P  = 1,	/*  P2P only */
-	BFA_PORT_TOPOLOGY_LOOP = 2,	/*  LOOP topology */
-	BFA_PORT_TOPOLOGY_AUTO = 3,	/*  auto topology selection */
+	BFA_PORT_TOPOLOGY_P2P_OLD_VER = 1, /* P2P def for older ver */
+	BFA_PORT_TOPOLOGY_LOOP = 2,	/* LOOP topology */
+	BFA_PORT_TOPOLOGY_AUTO_OLD_VER = 3, /* auto def for older ver */
+	BFA_PORT_TOPOLOGY_AUTO = 4,	/* auto topology selection */
+	BFA_PORT_TOPOLOGY_P2P = 5,	/* P2P only */
 };
 
 /*
@@ -760,6 +805,7 @@ enum bfa_port_linkstate_rsn {
 	BFA_PORT_LINKSTATE_RSN_LOCAL_FAULT	= 9,
 	BFA_PORT_LINKSTATE_RSN_REMOTE_FAULT	= 10,
 	BFA_PORT_LINKSTATE_RSN_TIMEOUT		= 11,
+	BFA_PORT_LINKSTATE_RSN_FAA_MISCONFIG	= 12,
 
 
 
@@ -833,6 +879,19 @@ struct bfa_lunmask_cfg_s {
 	struct bfa_lun_mask_s	lun_list[MAX_LUN_MASK_CFG];
 };
 
+struct bfa_throttle_cfg_s {
+	u16	is_valid;
+	u16	value;
+	u32	rsvd;
+};
+
+struct bfa_defs_fcpim_throttle_s {
+	u16	max_value;
+	u16	cur_value;
+	u16	cfg_value;
+	u16	rsvd;
+};
+
 /*
  *      Physical port configuration
  */
@@ -851,9 +910,10 @@ struct bfa_port_cfg_s {
 	u8	 bb_scn;	/*  BB_SCN value from FLOGI Exchg */
 	u8	 bb_scn_state;	/*  Config state of BB_SCN */
 	u8	 faa_state;	/*  FAA enabled/disabled        */
-	u8	 rsvd[1];
+	u8	 rsvd1;
 	u16	 path_tov;	/*  device path timeout	*/
 	u16	 q_depth;	/*  SCSI Queue depth		*/
+	struct bfa_qos_bw_s qos_bw;	/* QOS bandwidth	*/
 };
 #pragma pack()
 
@@ -901,7 +961,7 @@ struct bfa_port_attr_s {
 
 	/* FCoE specific  */
 	u16			fcoe_vlan;
-	u8			rsvd1[2];
+	u8			rsvd1[6];
 };
 
 /*
@@ -971,6 +1031,13 @@ struct bfa_trunk_vc_attr_s {
 	u16 vc_credits[8];
 };
 
+struct bfa_fcport_loop_info_s {
+	u8	myalpa;		/* alpa claimed */
+	u8	alpabm_val;	/* alpa bitmap valid or not (1 or 0) */
+	u8	resvd[6];
+	struct fc_alpabm_s alpabm;	/* alpa bitmap */
+};
+
 /*
  *	Link state information
  */
@@ -981,13 +1048,18 @@ struct bfa_port_link_s {
 	u8	 speed;		/*  Link speed (1/2/4/8 G) */
 	u32	 linkstate_opt; /*  Linkstate optional data (debug) */
 	u8	 trunked;	/*  Trunked or not (1 or 0) */
-	u8	 resvd[3];
+	u8	 resvd[7];
 	struct bfa_qos_attr_s  qos_attr;   /* QoS Attributes */
 	union {
-		struct bfa_qos_vc_attr_s qos_vc_attr;  /*  VC info from ELP */
-		struct bfa_trunk_vc_attr_s trunk_vc_attr;
-		struct bfa_fcport_fcf_s fcf; /*  FCF information (for FCoE) */
-	} vc_fcf;
+		struct bfa_fcport_loop_info_s loop_info;
+		union {
+			struct bfa_qos_vc_attr_s qos_vc_attr;
+					/*  VC info from ELP */
+			struct bfa_trunk_vc_attr_s trunk_vc_attr;
+			struct bfa_fcport_fcf_s fcf;
+					/*  FCF information (for FCoE) */
+		} vc_fcf;
+	} attr;
 };
 #pragma pack()
 
@@ -1112,6 +1184,9 @@ struct bfa_port_fc_stats_s {
 	u64     tx_frames;      /*  Tx frames                   */
 	u64     tx_words;       /*  Tx words                    */
 	u64     tx_lip;         /*  Tx LIP                      */
+	u64	tx_lip_f7f7;	/*  Tx LIP_F7F7		*/
+	u64	tx_lip_f8f7;	/*  Tx LIP_F8F7		*/
+	u64	tx_arbf0;	/*  Tx ARB F0			*/
 	u64     tx_nos;         /*  Tx NOS                      */
 	u64     tx_ols;         /*  Tx OLS                      */
 	u64     tx_lr;          /*  Tx LR                       */
@@ -1119,6 +1194,9 @@ struct bfa_port_fc_stats_s {
 	u64     rx_frames;      /*  Rx frames                   */
 	u64     rx_words;       /*  Rx words                    */
 	u64     lip_count;      /*  Rx LIP                      */
+	u64	rx_lip_f7f7;	/*  Rx LIP_F7F7		*/
+	u64	rx_lip_f8f7;	/*  Rx LIP_F8F7		*/
+	u64	rx_arbf0;	/*  Rx ARB F0			*/
 	u64     nos_count;      /*  Rx NOS                      */
 	u64     ols_count;      /*  Rx OLS                      */
 	u64     lr_count;       /*  Rx LR                       */
@@ -1140,6 +1218,7 @@ struct bfa_port_fc_stats_s {
 	u64	bbsc_frames_lost; /* Credit Recovery-Frames Lost  */
 	u64	bbsc_credits_lost; /* Credit Recovery-Credits Lost */
 	u64	bbsc_link_resets; /* Credit Recovery-Link Resets   */
+	u64	loop_timeouts;	/*  Loop timeouts		*/
 };
 
 /*
diff --git a/drivers/scsi/bfa/bfa_fc.h b/drivers/scsi/bfa/bfa_fc.h
index e0beb4d7e26..bea821b9803 100644
--- a/drivers/scsi/bfa/bfa_fc.h
+++ b/drivers/scsi/bfa/bfa_fc.h
@@ -24,6 +24,7 @@ typedef u64 wwn_t;
 
 #define WWN_NULL	(0)
 #define FC_SYMNAME_MAX	256	/*  max name server symbolic name size */
+#define FC_ALPA_MAX	128
 
 #pragma pack(1)
 
@@ -1015,6 +1016,10 @@ struct fc_symname_s {
 	u8         symname[FC_SYMNAME_MAX];
 };
 
+struct fc_alpabm_s {
+	u8	alpa_bm[FC_ALPA_MAX / 8];
+};
+
 /*
  * protocol default timeout values
  */
diff --git a/drivers/scsi/bfa/bfa_fcbuild.c b/drivers/scsi/bfa/bfa_fcbuild.c
index 273cee90b3b..dce787f6cca 100644
--- a/drivers/scsi/bfa/bfa_fcbuild.c
+++ b/drivers/scsi/bfa/bfa_fcbuild.c
@@ -228,6 +228,10 @@ fc_plogi_x_build(struct fchs_s *fchs, void *pld, u32 d_id, u32 s_id,
 
 	memcpy(plogi, &plogi_tmpl, sizeof(struct fc_logi_s));
 
+	/* For FC AL bb_cr is 0 and altbbcred is 1 */
+	if (!bb_cr)
+		plogi->csp.altbbcred = 1;
+
 	plogi->els_cmd.els_code = els_code;
 	if (els_code == FC_ELS_PLOGI)
 		fc_els_req_build(fchs, d_id, s_id, ox_id);
diff --git a/drivers/scsi/bfa/bfa_fcpim.c b/drivers/scsi/bfa/bfa_fcpim.c
index 1633963c66c..27b56096235 100644
--- a/drivers/scsi/bfa/bfa_fcpim.c
+++ b/drivers/scsi/bfa/bfa_fcpim.c
@@ -158,6 +158,7 @@ enum bfa_tskim_event {
 	BFA_TSKIM_SM_IOS_DONE	= 7,	/*  IO and sub TM completions	*/
 	BFA_TSKIM_SM_CLEANUP	= 8,	/*  TM cleanup on ITN offline	*/
 	BFA_TSKIM_SM_CLEANUP_DONE = 9,	/*  TM abort completion	*/
+	BFA_TSKIM_SM_UTAG	= 10,	/*  TM completion unknown tag  */
 };
 
 /*
@@ -3036,7 +3037,7 @@ bfa_ioim_abort(struct bfa_ioim_s *ioim)
 static void
 bfa_tskim_sm_uninit(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 {
-	bfa_trc(tskim->bfa, event);
+	bfa_trc(tskim->bfa, tskim->tsk_tag << 16 | event);
 
 	switch (event) {
 	case BFA_TSKIM_SM_START:
@@ -3074,7 +3075,7 @@ bfa_tskim_sm_uninit(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 static void
 bfa_tskim_sm_active(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 {
-	bfa_trc(tskim->bfa, event);
+	bfa_trc(tskim->bfa, tskim->tsk_tag << 16 | event);
 
 	switch (event) {
 	case BFA_TSKIM_SM_DONE:
@@ -3110,7 +3111,7 @@ bfa_tskim_sm_active(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 static void
 bfa_tskim_sm_cleanup(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 {
-	bfa_trc(tskim->bfa, event);
+	bfa_trc(tskim->bfa, tskim->tsk_tag << 16 | event);
 
 	switch (event) {
 	case BFA_TSKIM_SM_DONE:
@@ -3119,6 +3120,7 @@ bfa_tskim_sm_cleanup(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 		 */
 		break;
 
+	case BFA_TSKIM_SM_UTAG:
 	case BFA_TSKIM_SM_CLEANUP_DONE:
 		bfa_sm_set_state(tskim, bfa_tskim_sm_iocleanup);
 		bfa_tskim_cleanup_ios(tskim);
@@ -3138,7 +3140,7 @@ bfa_tskim_sm_cleanup(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 static void
 bfa_tskim_sm_iocleanup(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 {
-	bfa_trc(tskim->bfa, event);
+	bfa_trc(tskim->bfa, tskim->tsk_tag << 16 | event);
 
 	switch (event) {
 	case BFA_TSKIM_SM_IOS_DONE:
@@ -3170,7 +3172,7 @@ bfa_tskim_sm_iocleanup(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 static void
 bfa_tskim_sm_qfull(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 {
-	bfa_trc(tskim->bfa, event);
+	bfa_trc(tskim->bfa, tskim->tsk_tag << 16 | event);
 
 	switch (event) {
 	case BFA_TSKIM_SM_QRESUME:
@@ -3207,7 +3209,7 @@ static void
 bfa_tskim_sm_cleanup_qfull(struct bfa_tskim_s *tskim,
 		enum bfa_tskim_event event)
 {
-	bfa_trc(tskim->bfa, event);
+	bfa_trc(tskim->bfa, tskim->tsk_tag << 16 | event);
 
 	switch (event) {
 	case BFA_TSKIM_SM_DONE:
@@ -3238,7 +3240,7 @@ bfa_tskim_sm_cleanup_qfull(struct bfa_tskim_s *tskim,
 static void
 bfa_tskim_sm_hcb(struct bfa_tskim_s *tskim, enum bfa_tskim_event event)
 {
-	bfa_trc(tskim->bfa, event);
+	bfa_trc(tskim->bfa, tskim->tsk_tag << 16 | event);
 
 	switch (event) {
 	case BFA_TSKIM_SM_HCB:
@@ -3560,6 +3562,8 @@ bfa_tskim_isr(struct bfa_s *bfa, struct bfi_msg_s *m)
 	if (rsp->tsk_status == BFI_TSKIM_STS_ABORTED) {
 		bfa_stats(tskim->itnim, tm_cleanup_comps);
 		bfa_sm_send_event(tskim, BFA_TSKIM_SM_CLEANUP_DONE);
+	} else if (rsp->tsk_status == BFI_TSKIM_STS_UTAG) {
+		bfa_sm_send_event(tskim, BFA_TSKIM_SM_UTAG);
 	} else {
 		bfa_stats(tskim->itnim, tm_fw_rsps);
 		bfa_sm_send_event(tskim, BFA_TSKIM_SM_DONE);
@@ -3699,6 +3703,7 @@ bfa_fcp_attach(struct bfa_s *bfa, void *bfad, struct bfa_iocfc_cfg_s *cfg,
 	struct bfa_mem_dma_s *seg_ptr;
 	u16	idx, nsegs, num_io_req;
 
+	fcp->max_ioim_reqs = cfg->fwcfg.num_ioim_reqs;
 	fcp->num_ioim_reqs = cfg->fwcfg.num_ioim_reqs;
 	fcp->num_fwtio_reqs  = cfg->fwcfg.num_fwtio_reqs;
 	fcp->num_itns   = cfg->fwcfg.num_rports;
@@ -3721,6 +3726,7 @@ bfa_fcp_attach(struct bfa_s *bfa, void *bfad, struct bfa_iocfc_cfg_s *cfg,
 		bfa_iocfc_set_snsbase(bfa, idx, fcp->snsbase[idx].pa);
 	}
 
+	fcp->throttle_update_required = 1;
 	bfa_fcpim_attach(fcp, bfad, cfg, pcidev);
 
 	bfa_iotag_attach(fcp);
@@ -3759,23 +3765,33 @@ bfa_fcp_iocdisable(struct bfa_s *bfa)
 {
 	struct bfa_fcp_mod_s *fcp = BFA_FCP_MOD(bfa);
 
-	/* Enqueue unused ioim resources to free_q */
-	list_splice_tail_init(&fcp->iotag_unused_q, &fcp->iotag_ioim_free_q);
-
 	bfa_fcpim_iocdisable(fcp);
 }
 
 void
-bfa_fcp_res_recfg(struct bfa_s *bfa, u16 num_ioim_fw)
+bfa_fcp_res_recfg(struct bfa_s *bfa, u16 num_ioim_fw, u16 max_ioim_fw)
 {
 	struct bfa_fcp_mod_s	*mod = BFA_FCP_MOD(bfa);
 	struct list_head	*qe;
 	int	i;
 
+	/* Update io throttle value only once during driver load time */
+	if (!mod->throttle_update_required)
+		return;
+
 	for (i = 0; i < (mod->num_ioim_reqs - num_ioim_fw); i++) {
 		bfa_q_deq_tail(&mod->iotag_ioim_free_q, &qe);
 		list_add_tail(qe, &mod->iotag_unused_q);
 	}
+
+	if (mod->num_ioim_reqs != num_ioim_fw) {
+		bfa_trc(bfa, mod->num_ioim_reqs);
+		bfa_trc(bfa, num_ioim_fw);
+	}
+
+	mod->max_ioim_reqs = max_ioim_fw;
+	mod->num_ioim_reqs = num_ioim_fw;
+	mod->throttle_update_required = 0;
 }
 
 void
@@ -3833,3 +3849,88 @@ bfa_iotag_attach(struct bfa_fcp_mod_s *fcp)
 
 	bfa_mem_kva_curp(fcp) = (u8 *) iotag;
 }
+
+
+/**
+ * To send config req, first try to use throttle value from flash
+ * If 0, then use driver parameter
+ * We need to use min(flash_val, drv_val) because
+ * memory allocation was done based on this cfg'd value
+ */
+u16
+bfa_fcpim_get_throttle_cfg(struct bfa_s *bfa, u16 drv_cfg_param)
+{
+	u16 tmp;
+	struct bfa_fcp_mod_s *fcp = BFA_FCP_MOD(bfa);
+
+	/*
+	 * If throttle value from flash is already in effect after driver is
+	 * loaded then until next load, always return current value instead
+	 * of actual flash value
+	 */
+	if (!fcp->throttle_update_required)
+		return (u16)fcp->num_ioim_reqs;
+
+	tmp = bfa_dconf_read_data_valid(bfa) ? bfa_fcpim_read_throttle(bfa) : 0;
+	if (!tmp || (tmp > drv_cfg_param))
+		tmp = drv_cfg_param;
+
+	return tmp;
+}
+
+bfa_status_t
+bfa_fcpim_write_throttle(struct bfa_s *bfa, u16 value)
+{
+	if (!bfa_dconf_get_min_cfg(bfa)) {
+		BFA_DCONF_MOD(bfa)->dconf->throttle_cfg.value = value;
+		BFA_DCONF_MOD(bfa)->dconf->throttle_cfg.is_valid = 1;
+		return BFA_STATUS_OK;
+	}
+
+	return BFA_STATUS_FAILED;
+}
+
+u16
+bfa_fcpim_read_throttle(struct bfa_s *bfa)
+{
+	struct bfa_throttle_cfg_s *throttle_cfg =
+			&(BFA_DCONF_MOD(bfa)->dconf->throttle_cfg);
+
+	return ((!bfa_dconf_get_min_cfg(bfa)) ?
+	       ((throttle_cfg->is_valid == 1) ? (throttle_cfg->value) : 0) : 0);
+}
+
+bfa_status_t
+bfa_fcpim_throttle_set(struct bfa_s *bfa, u16 value)
+{
+	/* in min cfg no commands should run. */
+	if ((bfa_dconf_get_min_cfg(bfa) == BFA_TRUE) ||
+	    (!bfa_dconf_read_data_valid(bfa)))
+		return BFA_STATUS_FAILED;
+
+	bfa_fcpim_write_throttle(bfa, value);
+
+	return bfa_dconf_update(bfa);
+}
+
+bfa_status_t
+bfa_fcpim_throttle_get(struct bfa_s *bfa, void *buf)
+{
+	struct bfa_fcpim_s *fcpim = BFA_FCPIM(bfa);
+	struct bfa_defs_fcpim_throttle_s throttle;
+
+	if ((bfa_dconf_get_min_cfg(bfa) == BFA_TRUE) ||
+	    (!bfa_dconf_read_data_valid(bfa)))
+		return BFA_STATUS_FAILED;
+
+	memset(&throttle, 0, sizeof(struct bfa_defs_fcpim_throttle_s));
+
+	throttle.cur_value = (u16)(fcpim->fcp->num_ioim_reqs);
+	throttle.cfg_value = bfa_fcpim_read_throttle(bfa);
+	if (!throttle.cfg_value)
+		throttle.cfg_value = throttle.cur_value;
+	throttle.max_value = (u16)(fcpim->fcp->max_ioim_reqs);
+	memcpy(buf, &throttle, sizeof(struct bfa_defs_fcpim_throttle_s));
+
+	return BFA_STATUS_OK;
+}
diff --git a/drivers/scsi/bfa/bfa_fcpim.h b/drivers/scsi/bfa/bfa_fcpim.h
index 36f26da80f7..e693af6e593 100644
--- a/drivers/scsi/bfa/bfa_fcpim.h
+++ b/drivers/scsi/bfa/bfa_fcpim.h
@@ -42,7 +42,7 @@ void bfa_itn_create(struct bfa_s *bfa, struct bfa_rport_s *rport,
 		void (*isr)(struct bfa_s *bfa, struct bfi_msg_s *m));
 void bfa_itn_isr(struct bfa_s *bfa, struct bfi_msg_s *m);
 void bfa_iotag_attach(struct bfa_fcp_mod_s *fcp);
-void bfa_fcp_res_recfg(struct bfa_s *bfa, u16 num_ioim_fw);
+void bfa_fcp_res_recfg(struct bfa_s *bfa, u16 num_ioim_fw, u16 max_ioim_fw);
 
 #define BFA_FCP_MOD(_hal)	(&(_hal)->modules.fcp_mod)
 #define BFA_MEM_FCP_KVA(__bfa)	(&(BFA_FCP_MOD(__bfa)->kva_seg))
@@ -51,7 +51,9 @@ void bfa_fcp_res_recfg(struct bfa_s *bfa, u16 num_ioim_fw);
 #define BFA_ITN_FROM_TAG(_fcp, _tag)	\
 	((_fcp)->itn_arr + ((_tag) & ((_fcp)->num_itns - 1)))
 #define BFA_SNSINFO_FROM_TAG(_fcp, _tag) \
-	bfa_mem_get_dmabuf_kva(_fcp, _tag, BFI_IOIM_SNSLEN)
+	bfa_mem_get_dmabuf_kva(_fcp, (_tag & BFA_IOIM_IOTAG_MASK),	\
+	BFI_IOIM_SNSLEN)
+
 
 #define BFA_ITNIM_MIN   32
 #define BFA_ITNIM_MAX   1024
@@ -148,6 +150,7 @@ struct bfa_fcp_mod_s {
 	struct list_head	iotag_unused_q;	/* unused IO resources*/
 	struct bfa_iotag_s	*iotag_arr;
 	struct bfa_itn_s	*itn_arr;
+	int			max_ioim_reqs;
 	int			num_ioim_reqs;
 	int			num_fwtio_reqs;
 	int			num_itns;
@@ -155,6 +158,7 @@ struct bfa_fcp_mod_s {
 	struct bfa_fcpim_s	fcpim;
 	struct bfa_mem_dma_s	dma_seg[BFA_FCP_DMA_SEGS];
 	struct bfa_mem_kva_s	kva_seg;
+	int			throttle_update_required;
 };
 
 /*
@@ -416,5 +420,10 @@ bfa_status_t	bfa_fcpim_lunmask_delete(struct bfa_s *bfa, u16 vf_id,
 bfa_status_t	bfa_fcpim_lunmask_add(struct bfa_s *bfa, u16 vf_id,
 				wwn_t *pwwn, wwn_t rpwwn, struct scsi_lun lun);
 bfa_status_t	bfa_fcpim_lunmask_clear(struct bfa_s *bfa);
+u16		bfa_fcpim_read_throttle(struct bfa_s *bfa);
+bfa_status_t	bfa_fcpim_write_throttle(struct bfa_s *bfa, u16 value);
+bfa_status_t	bfa_fcpim_throttle_set(struct bfa_s *bfa, u16 value);
+bfa_status_t	bfa_fcpim_throttle_get(struct bfa_s *bfa, void *buf);
+u16     bfa_fcpim_get_throttle_cfg(struct bfa_s *bfa, u16 drv_cfg_param);
 
 #endif /* __BFA_FCPIM_H__ */
diff --git a/drivers/scsi/bfa/bfa_fcs.c b/drivers/scsi/bfa/bfa_fcs.c
index fd3e84d32bd..d428808fb37 100644
--- a/drivers/scsi/bfa/bfa_fcs.c
+++ b/drivers/scsi/bfa/bfa_fcs.c
@@ -303,16 +303,30 @@ static void
 bfa_fcs_fabric_sm_created(struct bfa_fcs_fabric_s *fabric,
 			  enum bfa_fcs_fabric_event event)
 {
+	struct bfa_s	*bfa = fabric->fcs->bfa;
+
 	bfa_trc(fabric->fcs, fabric->bport.port_cfg.pwwn);
 	bfa_trc(fabric->fcs, event);
 
 	switch (event) {
 	case BFA_FCS_FABRIC_SM_START:
-		if (bfa_fcport_is_linkup(fabric->fcs->bfa)) {
+		if (!bfa_fcport_is_linkup(fabric->fcs->bfa)) {
+			bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_linkdown);
+			break;
+		}
+		if (bfa_fcport_get_topology(bfa) ==
+				BFA_PORT_TOPOLOGY_LOOP) {
+			fabric->fab_type = BFA_FCS_FABRIC_LOOP;
+			fabric->bport.pid = bfa_fcport_get_myalpa(bfa);
+			fabric->bport.pid = bfa_hton3b(fabric->bport.pid);
+			bfa_sm_set_state(fabric,
+					bfa_fcs_fabric_sm_online);
+			bfa_fcs_fabric_set_opertype(fabric);
+			bfa_fcs_lport_online(&fabric->bport);
+		} else {
 			bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_flogi);
 			bfa_fcs_fabric_login(fabric);
-		} else
-			bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_linkdown);
+		}
 		break;
 
 	case BFA_FCS_FABRIC_SM_LINK_UP:
@@ -337,16 +351,28 @@ static void
 bfa_fcs_fabric_sm_linkdown(struct bfa_fcs_fabric_s *fabric,
 			   enum bfa_fcs_fabric_event event)
 {
+	struct bfa_s	*bfa = fabric->fcs->bfa;
+
 	bfa_trc(fabric->fcs, fabric->bport.port_cfg.pwwn);
 	bfa_trc(fabric->fcs, event);
 
 	switch (event) {
 	case BFA_FCS_FABRIC_SM_LINK_UP:
-		bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_flogi);
-		bfa_fcs_fabric_login(fabric);
+		if (bfa_fcport_get_topology(bfa) != BFA_PORT_TOPOLOGY_LOOP) {
+			bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_flogi);
+			bfa_fcs_fabric_login(fabric);
+			break;
+		}
+		fabric->fab_type = BFA_FCS_FABRIC_LOOP;
+		fabric->bport.pid = bfa_fcport_get_myalpa(bfa);
+		fabric->bport.pid = bfa_hton3b(fabric->bport.pid);
+		bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_online);
+		bfa_fcs_fabric_set_opertype(fabric);
+		bfa_fcs_lport_online(&fabric->bport);
 		break;
 
 	case BFA_FCS_FABRIC_SM_RETRY_OP:
+	case BFA_FCS_FABRIC_SM_LOOPBACK:
 		break;
 
 	case BFA_FCS_FABRIC_SM_DELETE:
@@ -595,14 +621,20 @@ void
 bfa_fcs_fabric_sm_online(struct bfa_fcs_fabric_s *fabric,
 			 enum bfa_fcs_fabric_event event)
 {
+	struct bfa_s	*bfa = fabric->fcs->bfa;
+
 	bfa_trc(fabric->fcs, fabric->bport.port_cfg.pwwn);
 	bfa_trc(fabric->fcs, event);
 
 	switch (event) {
 	case BFA_FCS_FABRIC_SM_LINK_DOWN:
 		bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_linkdown);
-		bfa_sm_send_event(fabric->lps, BFA_LPS_SM_OFFLINE);
-		bfa_fcs_fabric_notify_offline(fabric);
+		if (bfa_fcport_get_topology(bfa) == BFA_PORT_TOPOLOGY_LOOP) {
+			bfa_fcs_lport_offline(&fabric->bport);
+		} else {
+			bfa_sm_send_event(fabric->lps, BFA_LPS_SM_OFFLINE);
+			bfa_fcs_fabric_notify_offline(fabric);
+		}
 		break;
 
 	case BFA_FCS_FABRIC_SM_DELETE:
@@ -719,20 +751,29 @@ static void
 bfa_fcs_fabric_sm_stopping(struct bfa_fcs_fabric_s *fabric,
 			   enum bfa_fcs_fabric_event event)
 {
+	struct bfa_s	*bfa = fabric->fcs->bfa;
+
 	bfa_trc(fabric->fcs, fabric->bport.port_cfg.pwwn);
 	bfa_trc(fabric->fcs, event);
 
 	switch (event) {
 	case BFA_FCS_FABRIC_SM_STOPCOMP:
-		bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_cleanup);
-		bfa_sm_send_event(fabric->lps, BFA_LPS_SM_LOGOUT);
+		if (bfa_fcport_get_topology(bfa) == BFA_PORT_TOPOLOGY_LOOP) {
+			bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_created);
+		} else {
+			bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_cleanup);
+			bfa_sm_send_event(fabric->lps, BFA_LPS_SM_LOGOUT);
+		}
 		break;
 
 	case BFA_FCS_FABRIC_SM_LINK_UP:
 		break;
 
 	case BFA_FCS_FABRIC_SM_LINK_DOWN:
-		bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_cleanup);
+		if (bfa_fcport_get_topology(bfa) == BFA_PORT_TOPOLOGY_LOOP)
+			bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_created);
+		else
+			bfa_sm_set_state(fabric, bfa_fcs_fabric_sm_cleanup);
 		break;
 
 	default:
@@ -975,9 +1016,6 @@ bfa_fcs_fabric_login(struct bfa_fcs_fabric_s *fabric)
 	struct bfa_lport_cfg_s	*pcfg = &fabric->bport.port_cfg;
 	u8			alpa = 0, bb_scn = 0;
 
-	if (bfa_fcport_get_topology(bfa) == BFA_PORT_TOPOLOGY_LOOP)
-		alpa = bfa_fcport_get_myalpa(bfa);
-
 	if (bfa_fcs_fabric_is_bbscn_enabled(fabric) &&
 	    (!fabric->fcs->bbscn_flogi_rjt))
 		bb_scn = BFA_FCS_PORT_DEF_BB_SCN;
diff --git a/drivers/scsi/bfa/bfa_fcs.h b/drivers/scsi/bfa/bfa_fcs.h
index 6c4377cb287..a449706c6bc 100644
--- a/drivers/scsi/bfa/bfa_fcs.h
+++ b/drivers/scsi/bfa/bfa_fcs.h
@@ -118,9 +118,9 @@ struct bfa_fcs_lport_fab_s {
 #define	MAX_ALPA_COUNT	127
 
 struct bfa_fcs_lport_loop_s {
-	u8         num_alpa;	/*  Num of ALPA entries in the map */
-	u8         alpa_pos_map[MAX_ALPA_COUNT];	/*  ALPA Positional
-							 *Map */
+	u8	num_alpa;	/*  Num of ALPA entries in the map */
+	u8	alpabm_valid;	/* alpa bitmap valid or not (1 or 0) */
+	u8	alpa_pos_map[MAX_ALPA_COUNT]; /*  ALPA Positional Map */
 	struct bfa_fcs_lport_s *port;	/*  parent port */
 };
 
@@ -175,6 +175,7 @@ enum bfa_fcs_fabric_type {
 	BFA_FCS_FABRIC_UNKNOWN = 0,
 	BFA_FCS_FABRIC_SWITCHED = 1,
 	BFA_FCS_FABRIC_N2N = 2,
+	BFA_FCS_FABRIC_LOOP = 3,
 };
 
 
@@ -350,9 +351,10 @@ void		bfa_fcs_lport_ns_util_send_rspn_id(void *cbarg,
 				struct bfa_fcxp_s *fcxp_alloced);
 void            bfa_fcs_lport_scn_init(struct bfa_fcs_lport_s *vport);
 void            bfa_fcs_lport_scn_offline(struct bfa_fcs_lport_s *vport);
-void            bfa_fcs_lport_scn_online(struct bfa_fcs_lport_s *vport);
+void            bfa_fcs_lport_fab_scn_online(struct bfa_fcs_lport_s *vport);
 void            bfa_fcs_lport_scn_process_rscn(struct bfa_fcs_lport_s *port,
 					      struct fchs_s *rx_frame, u32 len);
+void		bfa_fcs_lport_lip_scn_online(bfa_fcs_lport_t *port);
 
 struct bfa_fcs_vport_s {
 	struct list_head		qe;		/*  queue elem	*/
@@ -453,6 +455,7 @@ struct bfa_fcs_rport_s {
 	struct bfa_rport_stats_s stats;	/*  rport stats */
 	enum bfa_rport_function	scsi_function;  /*  Initiator/Target */
 	struct bfa_fcs_rpf_s rpf;	/* Rport features module */
+	bfa_boolean_t   scn_online;	/* SCN online flag */
 };
 
 static inline struct bfa_rport_s *
@@ -639,9 +642,9 @@ struct bfa_fcs_fdmi_hba_attr_s {
 	u8         model[16];
 	u8         model_desc[256];
 	u8         hw_version[8];
-	u8         driver_version[8];
+	u8         driver_version[BFA_VERSION_LEN];
 	u8         option_rom_ver[BFA_VERSION_LEN];
-	u8         fw_version[8];
+	u8         fw_version[BFA_VERSION_LEN];
 	u8         os_name[256];
 	__be32        max_ct_pyld;
 };
@@ -733,7 +736,7 @@ enum rport_event {
 	RPSM_EVENT_LOGO_IMP     = 5,    /*  implicit logo for SLER      */
 	RPSM_EVENT_FCXP_SENT    = 6,    /*  Frame from has been sent    */
 	RPSM_EVENT_DELETE       = 7,    /*  RPORT delete request        */
-	RPSM_EVENT_SCN          = 8,    /*  state change notification   */
+	RPSM_EVENT_FAB_SCN	= 8,    /*  state change notification   */
 	RPSM_EVENT_ACCEPTED     = 9,    /*  Good response from remote device */
 	RPSM_EVENT_FAILED       = 10,   /*  Request to rport failed.    */
 	RPSM_EVENT_TIMEOUT      = 11,   /*  Rport SM timeout event      */
@@ -744,7 +747,9 @@ enum rport_event {
 	RPSM_EVENT_ADDRESS_DISC = 16,   /*  Need to Discover rport's PID */
 	RPSM_EVENT_PRLO_RCVD   = 17,    /*  PRLO from remote device     */
 	RPSM_EVENT_PLOGI_RETRY = 18,    /*  Retry PLOGI continuously */
-	RPSM_EVENT_FC4_FCS_ONLINE = 19, /*!< FC-4 FCS online complete */
+	RPSM_EVENT_SCN_OFFLINE = 19,	/* loop scn offline		*/
+	RPSM_EVENT_SCN_ONLINE   = 20,	/* loop scn online		*/
+	RPSM_EVENT_FC4_FCS_ONLINE = 21, /* FC-4 FCS online complete */
 };
 
 /*
@@ -763,7 +768,7 @@ enum bfa_fcs_itnim_event {
 	BFA_FCS_ITNIM_SM_DELETE = 10,   /*  delete event from rport */
 	BFA_FCS_ITNIM_SM_PRLO = 11,     /*  delete event from rport */
 	BFA_FCS_ITNIM_SM_RSP_NOT_SUPP = 12, /* cmd not supported rsp */
-	BFA_FCS_ITNIM_SM_HAL_ONLINE = 13, /*!< bfa rport online event */
+	BFA_FCS_ITNIM_SM_HAL_ONLINE = 13, /* bfa rport online event */
 };
 
 /*
diff --git a/drivers/scsi/bfa/bfa_fcs_lport.c b/drivers/scsi/bfa/bfa_fcs_lport.c
index 3b75f6fb2de..1224d0462a4 100644
--- a/drivers/scsi/bfa/bfa_fcs_lport.c
+++ b/drivers/scsi/bfa/bfa_fcs_lport.c
@@ -23,6 +23,34 @@
 
 BFA_TRC_FILE(FCS, PORT);
 
+/*
+ * ALPA to LIXA bitmap mapping
+ *
+ * ALPA 0x00 (Word 0, Bit 30) is invalid for N_Ports. Also Word 0 Bit 31
+ * is for L_bit (login required) and is filled as ALPA 0x00 here.
+ */
+static const u8 loop_alpa_map[] = {
+	0x00, 0x00, 0x01, 0x02, 0x04, 0x08, 0x0F, 0x10, /* Word 0 Bits 31..24 */
+	0x17, 0x18, 0x1B, 0x1D, 0x1E, 0x1F, 0x23, 0x25, /* Word 0 Bits 23..16 */
+	0x26, 0x27, 0x29, 0x2A, 0x2B, 0x2C, 0x2D, 0x2E, /* Word 0 Bits 15..08 */
+	0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x39, 0x3A, /* Word 0 Bits 07..00 */
+
+	0x3C, 0x43, 0x45, 0x46, 0x47, 0x49, 0x4A, 0x4B, /* Word 1 Bits 31..24 */
+	0x4C, 0x4D, 0x4E, 0x51, 0x52, 0x53, 0x54, 0x55, /* Word 1 Bits 23..16 */
+	0x56, 0x59, 0x5A, 0x5C, 0x63, 0x65, 0x66, 0x67, /* Word 1 Bits 15..08 */
+	0x69, 0x6A, 0x6B, 0x6C, 0x6D, 0x6E, 0x71, 0x72, /* Word 1 Bits 07..00 */
+
+	0x73, 0x74, 0x75, 0x76, 0x79, 0x7A, 0x7C, 0x80, /* Word 2 Bits 31..24 */
+	0x81, 0x82, 0x84, 0x88, 0x8F, 0x90, 0x97, 0x98, /* Word 2 Bits 23..16 */
+	0x9B, 0x9D, 0x9E, 0x9F, 0xA3, 0xA5, 0xA6, 0xA7, /* Word 2 Bits 15..08 */
+	0xA9, 0xAA, 0xAB, 0xAC, 0xAD, 0xAE, 0xB1, 0xB2, /* Word 2 Bits 07..00 */
+
+	0xB3, 0xB4, 0xB5, 0xB6, 0xB9, 0xBA, 0xBC, 0xC3, /* Word 3 Bits 31..24 */
+	0xC5, 0xC6, 0xC7, 0xC9, 0xCA, 0xCB, 0xCC, 0xCD, /* Word 3 Bits 23..16 */
+	0xCE, 0xD1, 0xD2, 0xD3, 0xD4, 0xD5, 0xD6, 0xD9, /* Word 3 Bits 15..08 */
+	0xDA, 0xDC, 0xE0, 0xE1, 0xE2, 0xE4, 0xE8, 0xEF, /* Word 3 Bits 07..00 */
+};
+
 static void     bfa_fcs_lport_send_ls_rjt(struct bfa_fcs_lport_s *port,
 					 struct fchs_s *rx_fchs, u8 reason_code,
 					 u8 reason_code_expl);
@@ -51,6 +79,10 @@ static void	bfa_fcs_lport_n2n_init(struct bfa_fcs_lport_s *port);
 static void	bfa_fcs_lport_n2n_online(struct bfa_fcs_lport_s *port);
 static void	bfa_fcs_lport_n2n_offline(struct bfa_fcs_lport_s *port);
 
+static void	bfa_fcs_lport_loop_init(struct bfa_fcs_lport_s *port);
+static void	bfa_fcs_lport_loop_online(struct bfa_fcs_lport_s *port);
+static void	bfa_fcs_lport_loop_offline(struct bfa_fcs_lport_s *port);
+
 static struct {
 	void		(*init) (struct bfa_fcs_lport_s *port);
 	void		(*online) (struct bfa_fcs_lport_s *port);
@@ -62,7 +94,9 @@ static struct {
 	bfa_fcs_lport_fab_init, bfa_fcs_lport_fab_online,
 			bfa_fcs_lport_fab_offline}, {
 	bfa_fcs_lport_n2n_init, bfa_fcs_lport_n2n_online,
-			bfa_fcs_lport_n2n_offline},
+			bfa_fcs_lport_n2n_offline}, {
+	bfa_fcs_lport_loop_init, bfa_fcs_lport_loop_online,
+			bfa_fcs_lport_loop_offline},
 	};
 
 /*
@@ -1127,7 +1161,7 @@ static void
 bfa_fcs_lport_fab_online(struct bfa_fcs_lport_s *port)
 {
 	bfa_fcs_lport_ns_online(port);
-	bfa_fcs_lport_scn_online(port);
+	bfa_fcs_lport_fab_scn_online(port);
 }
 
 /*
@@ -1221,6 +1255,98 @@ bfa_fcs_lport_n2n_offline(struct bfa_fcs_lport_s *port)
 	n2n_port->reply_oxid = 0;
 }
 
+void
+bfa_fcport_get_loop_attr(struct bfa_fcs_lport_s *port)
+{
+	int i = 0, j = 0, bit = 0, alpa_bit = 0;
+	u8 k = 0;
+	struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(port->fcs->bfa);
+
+	port->port_topo.ploop.alpabm_valid = fcport->alpabm_valid;
+	port->pid = fcport->myalpa;
+	port->pid = bfa_hton3b(port->pid);
+
+	for (i = 0; i < (FC_ALPA_MAX / 8); i++) {
+		for (j = 0, alpa_bit = 0; j < 8; j++, alpa_bit++) {
+			bfa_trc(port->fcs->bfa, fcport->alpabm.alpa_bm[i]);
+			bit = (fcport->alpabm.alpa_bm[i] & (1 << (7 - j)));
+			if (bit) {
+				port->port_topo.ploop.alpa_pos_map[k] =
+					loop_alpa_map[(i * 8) + alpa_bit];
+				k++;
+				bfa_trc(port->fcs->bfa, k);
+				bfa_trc(port->fcs->bfa,
+					 port->port_topo.ploop.alpa_pos_map[k]);
+			}
+		}
+	}
+	port->port_topo.ploop.num_alpa = k;
+}
+
+/*
+ * Called by fcs/port to initialize Loop topology.
+ */
+static void
+bfa_fcs_lport_loop_init(struct bfa_fcs_lport_s *port)
+{
+}
+
+/*
+ * Called by fcs/port to notify transition to online state.
+ */
+static void
+bfa_fcs_lport_loop_online(struct bfa_fcs_lport_s *port)
+{
+	u8 num_alpa = 0, alpabm_valid = 0;
+	struct bfa_fcs_rport_s *rport;
+	u8 *alpa_map = NULL;
+	int i = 0;
+	u32 pid;
+
+	bfa_fcport_get_loop_attr(port);
+
+	num_alpa = port->port_topo.ploop.num_alpa;
+	alpabm_valid = port->port_topo.ploop.alpabm_valid;
+	alpa_map = port->port_topo.ploop.alpa_pos_map;
+
+	bfa_trc(port->fcs->bfa, port->pid);
+	bfa_trc(port->fcs->bfa, num_alpa);
+	if (alpabm_valid == 1) {
+		for (i = 0; i < num_alpa; i++) {
+			bfa_trc(port->fcs->bfa, alpa_map[i]);
+			if (alpa_map[i] != bfa_hton3b(port->pid)) {
+				pid = alpa_map[i];
+				bfa_trc(port->fcs->bfa, pid);
+				rport = bfa_fcs_lport_get_rport_by_pid(port,
+						bfa_hton3b(pid));
+				if (!rport)
+					rport = bfa_fcs_rport_create(port,
+						bfa_hton3b(pid));
+			}
+		}
+	} else {
+		for (i = 0; i < MAX_ALPA_COUNT; i++) {
+			if (alpa_map[i] != port->pid) {
+				pid = loop_alpa_map[i];
+				bfa_trc(port->fcs->bfa, pid);
+				rport = bfa_fcs_lport_get_rport_by_pid(port,
+						bfa_hton3b(pid));
+				if (!rport)
+					rport = bfa_fcs_rport_create(port,
+						bfa_hton3b(pid));
+			}
+		}
+	}
+}
+
+/*
+ * Called by fcs/port to notify transition to offline state.
+ */
+static void
+bfa_fcs_lport_loop_offline(struct bfa_fcs_lport_s *port)
+{
+}
+
 #define BFA_FCS_FDMI_CMD_MAX_RETRIES 2
 
 /*
@@ -1888,13 +2014,10 @@ bfa_fcs_lport_fdmi_build_rhba_pyld(struct bfa_fcs_lport_fdmi_s *fdmi, u8 *pyld)
 					 sizeof(templen));
 	}
 
-	/*
-	 * f/w Version = driver version
-	 */
 	attr = (struct fdmi_attr_s *) curr_ptr;
 	attr->type = cpu_to_be16(FDMI_HBA_ATTRIB_FW_VERSION);
-	templen = (u16) strlen(fcs_hba_attr->driver_version);
-	memcpy(attr->value, fcs_hba_attr->driver_version, templen);
+	templen = (u16) strlen(fcs_hba_attr->fw_version);
+	memcpy(attr->value, fcs_hba_attr->fw_version, templen);
 	templen = fc_roundup(templen, sizeof(u32));
 	curr_ptr += sizeof(attr->type) + sizeof(templen) + templen;
 	len += templen;
@@ -2296,6 +2419,7 @@ bfa_fcs_fdmi_get_hbaattr(struct bfa_fcs_lport_fdmi_s *fdmi,
 {
 	struct bfa_fcs_lport_s *port = fdmi->ms->port;
 	struct bfa_fcs_driver_info_s  *driver_info = &port->fcs->driver_info;
+	struct bfa_fcs_fdmi_port_attr_s fcs_port_attr;
 
 	memset(hba_attr, 0, sizeof(struct bfa_fcs_fdmi_hba_attr_s));
 
@@ -2331,7 +2455,9 @@ bfa_fcs_fdmi_get_hbaattr(struct bfa_fcs_lport_fdmi_s *fdmi,
 				sizeof(driver_info->host_os_patch));
 	}
 
-	hba_attr->max_ct_pyld = cpu_to_be32(FC_MAX_PDUSZ);
+	/* Retrieve the max frame size from the port attr */
+	bfa_fcs_fdmi_get_portattr(fdmi, &fcs_port_attr);
+	hba_attr->max_ct_pyld = fcs_port_attr.max_frm_size;
 }
 
 static void
@@ -2391,7 +2517,7 @@ bfa_fcs_fdmi_get_portattr(struct bfa_fcs_lport_fdmi_s *fdmi,
 	/*
 	 * Max PDU Size.
 	 */
-	port_attr->max_frm_size = cpu_to_be32(FC_MAX_PDUSZ);
+	port_attr->max_frm_size = cpu_to_be32(pport_attr.pport_cfg.maxfrsize);
 
 	/*
 	 * OS device Name
@@ -5199,7 +5325,7 @@ bfa_fcs_lport_scn_offline(struct bfa_fcs_lport_s *port)
 }
 
 void
-bfa_fcs_lport_scn_online(struct bfa_fcs_lport_s *port)
+bfa_fcs_lport_fab_scn_online(struct bfa_fcs_lport_s *port)
 {
 	struct bfa_fcs_lport_scn_s *scn = BFA_FCS_GET_SCN_FROM_PORT(port);
 
@@ -5621,6 +5747,15 @@ bfa_fcs_lport_clear_stats(struct bfa_fcs_lport_s *fcs_port)
 }
 
 /*
+ * Let new loop map create missing rports
+ */
+void
+bfa_fcs_lport_lip_scn_online(struct bfa_fcs_lport_s *port)
+{
+	bfa_fcs_lport_loop_online(port);
+}
+
+/*
  * FCS virtual port state machine
  */
 
diff --git a/drivers/scsi/bfa/bfa_fcs_rport.c b/drivers/scsi/bfa/bfa_fcs_rport.c
index cc43b2a58ce..58ac643ba9f 100644
--- a/drivers/scsi/bfa/bfa_fcs_rport.c
+++ b/drivers/scsi/bfa/bfa_fcs_rport.c
@@ -106,9 +106,13 @@ static void	bfa_fcs_rport_sm_nsquery_sending(struct bfa_fcs_rport_s *rport,
 						 enum rport_event event);
 static void	bfa_fcs_rport_sm_nsquery(struct bfa_fcs_rport_s *rport,
 					 enum rport_event event);
-static void	bfa_fcs_rport_sm_adisc_sending(struct bfa_fcs_rport_s *rport,
-						enum rport_event event);
-static void	bfa_fcs_rport_sm_adisc(struct bfa_fcs_rport_s *rport,
+static void	bfa_fcs_rport_sm_adisc_online_sending(
+			struct bfa_fcs_rport_s *rport, enum rport_event event);
+static void	bfa_fcs_rport_sm_adisc_online(struct bfa_fcs_rport_s *rport,
+					enum rport_event event);
+static void	bfa_fcs_rport_sm_adisc_offline_sending(struct bfa_fcs_rport_s
+					*rport, enum rport_event event);
+static void	bfa_fcs_rport_sm_adisc_offline(struct bfa_fcs_rport_s *rport,
 					enum rport_event event);
 static void	bfa_fcs_rport_sm_fc4_logorcv(struct bfa_fcs_rport_s *rport,
 						enum rport_event event);
@@ -150,8 +154,10 @@ static struct bfa_sm_table_s rport_sm_table[] = {
 	{BFA_SM(bfa_fcs_rport_sm_online), BFA_RPORT_ONLINE},
 	{BFA_SM(bfa_fcs_rport_sm_nsquery_sending), BFA_RPORT_NSQUERY},
 	{BFA_SM(bfa_fcs_rport_sm_nsquery), BFA_RPORT_NSQUERY},
-	{BFA_SM(bfa_fcs_rport_sm_adisc_sending), BFA_RPORT_ADISC},
-	{BFA_SM(bfa_fcs_rport_sm_adisc), BFA_RPORT_ADISC},
+	{BFA_SM(bfa_fcs_rport_sm_adisc_online_sending), BFA_RPORT_ADISC},
+	{BFA_SM(bfa_fcs_rport_sm_adisc_online), BFA_RPORT_ADISC},
+	{BFA_SM(bfa_fcs_rport_sm_adisc_offline_sending), BFA_RPORT_ADISC},
+	{BFA_SM(bfa_fcs_rport_sm_adisc_offline), BFA_RPORT_ADISC},
 	{BFA_SM(bfa_fcs_rport_sm_fc4_logorcv), BFA_RPORT_LOGORCV},
 	{BFA_SM(bfa_fcs_rport_sm_fc4_logosend), BFA_RPORT_LOGO},
 	{BFA_SM(bfa_fcs_rport_sm_fc4_offline), BFA_RPORT_OFFLINE},
@@ -231,10 +237,19 @@ bfa_fcs_rport_sm_plogi_sending(struct bfa_fcs_rport_s *rport,
 		bfa_fcs_rport_send_plogiacc(rport, NULL);
 		break;
 
+	case RPSM_EVENT_SCN_OFFLINE:
+		bfa_sm_set_state(rport, bfa_fcs_rport_sm_offline);
+		bfa_fcxp_walloc_cancel(rport->fcs->bfa, &rport->fcxp_wqe);
+		bfa_timer_start(rport->fcs->bfa, &rport->timer,
+				bfa_fcs_rport_timeout, rport,
+				bfa_fcs_rport_del_timeout);
+		break;
 	case RPSM_EVENT_ADDRESS_CHANGE:
-	case RPSM_EVENT_SCN:
+	case RPSM_EVENT_FAB_SCN:
 		/* query the NS */
 		bfa_fcxp_walloc_cancel(rport->fcs->bfa, &rport->fcxp_wqe);
+		WARN_ON(!(bfa_fcport_get_topology(rport->port->fcs->bfa) !=
+					BFA_PORT_TOPOLOGY_LOOP));
 		bfa_sm_set_state(rport, bfa_fcs_rport_sm_nsdisc_sending);
 		rport->ns_retries = 0;
 		bfa_fcs_rport_send_nsdisc(rport, NULL);
@@ -280,12 +295,20 @@ bfa_fcs_rport_sm_plogiacc_sending(struct bfa_fcs_rport_s *rport,
 
 	case RPSM_EVENT_PLOGI_RCVD:
 	case RPSM_EVENT_PLOGI_COMP:
-	case RPSM_EVENT_SCN:
+	case RPSM_EVENT_FAB_SCN:
 		/*
 		 * Ignore, SCN is possibly online notification.
 		 */
 		break;
 
+	case RPSM_EVENT_SCN_OFFLINE:
+		bfa_sm_set_state(rport, bfa_fcs_rport_sm_offline);
+		bfa_fcxp_walloc_cancel(rport->fcs->bfa, &rport->fcxp_wqe);
+		bfa_timer_start(rport->fcs->bfa, &rport->timer,
+				bfa_fcs_rport_timeout, rport,
+				bfa_fcs_rport_del_timeout);
+		break;
+
 	case RPSM_EVENT_ADDRESS_CHANGE:
 		bfa_fcxp_walloc_cancel(rport->fcs->bfa, &rport->fcxp_wqe);
 		bfa_sm_set_state(rport, bfa_fcs_rport_sm_nsdisc_sending);
@@ -346,9 +369,19 @@ bfa_fcs_rport_sm_plogi_retry(struct bfa_fcs_rport_s *rport,
 		bfa_fcs_rport_send_plogiacc(rport, NULL);
 		break;
 
+	case RPSM_EVENT_SCN_OFFLINE:
+		bfa_sm_set_state(rport, bfa_fcs_rport_sm_offline);
+		bfa_timer_stop(&rport->timer);
+		bfa_timer_start(rport->fcs->bfa, &rport->timer,
+				bfa_fcs_rport_timeout, rport,
+				bfa_fcs_rport_del_timeout);
+		break;
+
 	case RPSM_EVENT_ADDRESS_CHANGE:
-	case RPSM_EVENT_SCN:
+	case RPSM_EVENT_FAB_SCN:
 		bfa_timer_stop(&rport->timer);
+		WARN_ON(!(bfa_fcport_get_topology(rport->port->fcs->bfa) !=
+					BFA_PORT_TOPOLOGY_LOOP));
 		bfa_sm_set_state(rport, bfa_fcs_rport_sm_nsdisc_sending);
 		rport->ns_retries = 0;
 		bfa_fcs_rport_send_nsdisc(rport, NULL);
@@ -422,7 +455,18 @@ bfa_fcs_rport_sm_plogi(struct bfa_fcs_rport_s *rport, enum rport_event event)
 		}
 		break;
 
-	case	RPSM_EVENT_PLOGI_RETRY:
+	case RPSM_EVENT_SCN_ONLINE:
+		break;
+
+	case RPSM_EVENT_SCN_OFFLINE:
+		bfa_sm_set_state(rport, bfa_fcs_rport_sm_offline);
+		bfa_fcxp_discard(rport->fcxp);
+		bfa_timer_start(rport->fcs->bfa, &rport->timer,
+				bfa_fcs_rport_timeout, rport,
+				bfa_fcs_rport_del_timeout);
+		break;
+
+	case RPSM_EVENT_PLOGI_RETRY:
 		rport->plogi_retries = 0;
 		bfa_sm_set_state(rport, bfa_fcs_rport_sm_plogi_retry);
 		bfa_timer_start(rport->fcs->bfa, &rport->timer,
@@ -440,8 +484,10 @@ bfa_fcs_rport_sm_plogi(struct bfa_fcs_rport_s *rport, enum rport_event event)
 		break;
 
 	case RPSM_EVENT_ADDRESS_CHANGE:
-	case RPSM_EVENT_SCN:
+	case RPSM_EVENT_FAB_SCN:
 		bfa_fcxp_discard(rport->fcxp);
+		WARN_ON(!(bfa_fcport_get_topology(rport->port->fcs->bfa) !=
+					BFA_PORT_TOPOLOGY_LOOP));
 		bfa_sm_set_state(rport, bfa_fcs_rport_sm_nsdisc_sending);
 		rport->ns_retries = 0;
 		bfa_fcs_rport_send_nsdisc(rport, NULL);
@@ -512,7 +558,8 @@ bfa_fcs_rport_sm_fc4_fcs_online(struct bfa_fcs_rport_s *rport,
 	case RPSM_EVENT_PLOGI_COMP:
 	case RPSM_EVENT_LOGO_IMP:
 	case RPSM_EVENT_ADDRESS_CHANGE:
-	case RPSM_EVENT_SCN:
+	case RPSM_EVENT_FAB_SCN:
+	case RPSM_EVENT_SCN_OFFLINE:
 		bfa_sm_set_state(rport, bfa_fcs_rport_sm_fc4_offline);
 		bfa_fcs_rport_fcs_offline_action(rport);
 		break;
@@ -561,9 +608,10 @@ bfa_fcs_rport_sm_hal_online(struct bfa_fcs_rport_s *rport,
 		bfa_fcs_rport_fcs_offline_action(rport);
 		break;
 
-	case RPSM_EVENT_SCN:
+	case RPSM_EVENT_FAB_SCN:
 	case RPSM_EVENT_LOGO_IMP:
 	case RPSM_EVENT_ADDRESS_CHANGE:
+	case RPSM_EVENT_SCN_OFFLINE:
 		bfa_sm_set_state(rport, bfa_fcs_rport_sm_fc4_offline);
 		bfa_fcs_rport_fcs_offline_action(rport);
 		break;
@@ -595,14 +643,15 @@ bfa_fcs_rport_sm_online(struct bfa_fcs_rport_s *rport, enum rport_event event)
 	bfa_trc(rport->fcs, event);
 
 	switch (event) {
-	case RPSM_EVENT_SCN:
+	case RPSM_EVENT_FAB_SCN:
 		if (bfa_fcs_fabric_is_switched(rport->port->fabric)) {
 			bfa_sm_set_state(rport,
 					 bfa_fcs_rport_sm_nsquery_sending);
 			rport->ns_retries = 0;
 			bfa_fcs_rport_send_nsdisc(rport, NULL);
 		} else {
-			bfa_sm_set_state(rport, bfa_fcs_rport_sm_adisc_sending);
+			bfa_sm_set_state(rport,
+				bfa_fcs_rport_sm_adisc_online_sending);
 			bfa_fcs_rport_send_adisc(rport, NULL);
 		}
 		break;
@@ -610,6 +659,7 @@ bfa_fcs_rport_sm_online(struct bfa_fcs_rport_s *rport, enum rport_event event)
 	case RPSM_EVENT_PLOGI_RCVD:
 	case RPSM_EVENT_LOGO_IMP:
 	case RPSM_EVENT_ADDRESS_CHANGE:
+	case RPSM_EVENT_SCN_OFFLINE:
 		bfa_sm_set_state(rport, bfa_fcs_rport_sm_fc4_offline);
 		bfa_fcs_rport_hal_offline_action(rport);
 		break;
@@ -625,6 +675,7 @@ bfa_fcs_rport_sm_online(struct bfa_fcs_rport_s *rport, enum rport_event event)
 		bfa_fcs_rport_hal_offline_action(rport);
 		break;
 
+	case RPSM_EVENT_SCN_ONLINE:
 	case RPSM_EVENT_PLOGI_COMP:
 		break;
 
@@ -656,7 +707,7 @@ bfa_fcs_rport_sm_nsquery_sending(struct bfa_fcs_rport_s *rport,
 		bfa_fcs_rport_hal_offline_action(rport);
 		break;
 
-	case RPSM_EVENT_SCN:
+	case RPSM_EVENT_FAB_SCN:
 		/*
 		 * ignore SCN, wait for response to query itself
 		 */
@@ -696,7 +747,7 @@ bfa_fcs_rport_sm_nsquery(struct bfa_fcs_rport_s *rport, enum rport_event event)
 
 	switch (event) {
 	case RPSM_EVENT_ACCEPTED:
-		bfa_sm_set_state(rport, bfa_fcs_rport_sm_adisc_sending);
+		bfa_sm_set_state(rport, bfa_fcs_rport_sm_adisc_online_sending);
 		bfa_fcs_rport_send_adisc(rport, NULL);
 		break;
 
@@ -718,7 +769,7 @@ bfa_fcs_rport_sm_nsquery(struct bfa_fcs_rport_s *rport, enum rport_event event)
 		bfa_fcs_rport_hal_offline_action(rport);
 		break;
 
-	case RPSM_EVENT_SCN:
+	case RPSM_EVENT_FAB_SCN:
 		break;
 
 	case RPSM_EVENT_LOGO_RCVD:
@@ -747,7 +798,7 @@ bfa_fcs_rport_sm_nsquery(struct bfa_fcs_rport_s *rport, enum rport_event event)
  *	authenticating with rport. FC-4s are paused.
  */
 static void
-bfa_fcs_rport_sm_adisc_sending(struct bfa_fcs_rport_s *rport,
+bfa_fcs_rport_sm_adisc_online_sending(struct bfa_fcs_rport_s *rport,
 	 enum rport_event event)
 {
 	bfa_trc(rport->fcs, rport->pwwn);
@@ -756,7 +807,7 @@ bfa_fcs_rport_sm_adisc_sending(struct bfa_fcs_rport_s *rport,
 
 	switch (event) {
 	case RPSM_EVENT_FCXP_SENT:
-		bfa_sm_set_state(rport, bfa_fcs_rport_sm_adisc);
+		bfa_sm_set_state(rport, bfa_fcs_rport_sm_adisc_online);
 		break;
 
 	case RPSM_EVENT_DELETE:
@@ -779,7 +830,7 @@ bfa_fcs_rport_sm_adisc_sending(struct bfa_fcs_rport_s *rport,
 		bfa_fcs_rport_hal_offline_action(rport);
 		break;
 
-	case RPSM_EVENT_SCN:
+	case RPSM_EVENT_FAB_SCN:
 		break;
 
 	case RPSM_EVENT_PLOGI_RCVD:
@@ -798,7 +849,8 @@ bfa_fcs_rport_sm_adisc_sending(struct bfa_fcs_rport_s *rport,
  *		FC-4s are paused.
  */
 static void
-bfa_fcs_rport_sm_adisc(struct bfa_fcs_rport_s *rport, enum rport_event event)
+bfa_fcs_rport_sm_adisc_online(struct bfa_fcs_rport_s *rport,
+				enum rport_event event)
 {
 	bfa_trc(rport->fcs, rport->pwwn);
 	bfa_trc(rport->fcs, rport->pid);
@@ -831,7 +883,7 @@ bfa_fcs_rport_sm_adisc(struct bfa_fcs_rport_s *rport, enum rport_event event)
 		bfa_fcs_rport_hal_offline_action(rport);
 		break;
 
-	case RPSM_EVENT_SCN:
+	case RPSM_EVENT_FAB_SCN:
 		/*
 		 * already processing RSCN
 		 */
@@ -856,7 +908,96 @@ bfa_fcs_rport_sm_adisc(struct bfa_fcs_rport_s *rport, enum rport_event event)
 }
 
 /*
- *		Rport has sent LOGO. Awaiting FC-4 offline completion callback.
+ * ADISC is being sent for authenticating with rport
+ * Already did offline actions.
+ */
+static void
+bfa_fcs_rport_sm_adisc_offline_sending(struct bfa_fcs_rport_s *rport,
+	enum rport_event event)
+{
+	bfa_trc(rport->fcs, rport->pwwn);
+	bfa_trc(rport->fcs, rport->pid);
+	bfa_trc(rport->fcs, event);
+
+	switch (event) {
+	case RPSM_EVENT_FCXP_SENT:
+		bfa_sm_set_state(rport, bfa_fcs_rport_sm_adisc_offline);
+		break;
+
+	case RPSM_EVENT_DELETE:
+	case RPSM_EVENT_SCN_OFFLINE:
+	case RPSM_EVENT_LOGO_IMP:
+	case RPSM_EVENT_LOGO_RCVD:
+	case RPSM_EVENT_PRLO_RCVD:
+		bfa_sm_set_state(rport, bfa_fcs_rport_sm_offline);
+		bfa_fcxp_walloc_cancel(rport->fcs->bfa,
+			&rport->fcxp_wqe);
+		bfa_timer_start(rport->fcs->bfa, &rport->timer,
+			bfa_fcs_rport_timeout, rport,
+			bfa_fcs_rport_del_timeout);
+		break;
+
+	case RPSM_EVENT_PLOGI_RCVD:
+		bfa_sm_set_state(rport, bfa_fcs_rport_sm_plogiacc_sending);
+		bfa_fcxp_walloc_cancel(rport->fcs->bfa, &rport->fcxp_wqe);
+		bfa_fcs_rport_send_plogiacc(rport, NULL);
+		break;
+
+	default:
+		bfa_sm_fault(rport->fcs, event);
+	}
+}
+
+/*
+ * ADISC to rport
+ * Already did offline actions
+ */
+static void
+bfa_fcs_rport_sm_adisc_offline(struct bfa_fcs_rport_s *rport,
+			enum rport_event event)
+{
+	bfa_trc(rport->fcs, rport->pwwn);
+	bfa_trc(rport->fcs, rport->pid);
+	bfa_trc(rport->fcs, event);
+
+	switch (event) {
+	case RPSM_EVENT_ACCEPTED:
+		bfa_sm_set_state(rport, bfa_fcs_rport_sm_hal_online);
+		bfa_fcs_rport_hal_online(rport);
+		break;
+
+	case RPSM_EVENT_PLOGI_RCVD:
+		bfa_sm_set_state(rport, bfa_fcs_rport_sm_plogiacc_sending);
+		bfa_fcxp_discard(rport->fcxp);
+		bfa_fcs_rport_send_plogiacc(rport, NULL);
+		break;
+
+	case RPSM_EVENT_FAILED:
+		bfa_sm_set_state(rport, bfa_fcs_rport_sm_offline);
+		bfa_timer_start(rport->fcs->bfa, &rport->timer,
+			bfa_fcs_rport_timeout, rport,
+			bfa_fcs_rport_del_timeout);
+		break;
+
+	case RPSM_EVENT_DELETE:
+	case RPSM_EVENT_SCN_OFFLINE:
+	case RPSM_EVENT_LOGO_IMP:
+	case RPSM_EVENT_LOGO_RCVD:
+	case RPSM_EVENT_PRLO_RCVD:
+		bfa_sm_set_state(rport, bfa_fcs_rport_sm_offline);
+		bfa_fcxp_discard(rport->fcxp);
+		bfa_timer_start(rport->fcs->bfa, &rport->timer,
+			bfa_fcs_rport_timeout, rport,
+			bfa_fcs_rport_del_timeout);
+		break;
+
+	default:
+		bfa_sm_fault(rport->fcs, event);
+	}
+}
+
+/*
+ * Rport has sent LOGO. Awaiting FC-4 offline completion callback.
  */
 static void
 bfa_fcs_rport_sm_fc4_logorcv(struct bfa_fcs_rport_s *rport,
@@ -881,6 +1022,8 @@ bfa_fcs_rport_sm_fc4_logorcv(struct bfa_fcs_rport_s *rport,
 		bfa_sm_set_state(rport, bfa_fcs_rport_sm_fc4_off_delete);
 		break;
 
+	case RPSM_EVENT_SCN_ONLINE:
+	case RPSM_EVENT_SCN_OFFLINE:
 	case RPSM_EVENT_HCB_ONLINE:
 	case RPSM_EVENT_LOGO_RCVD:
 	case RPSM_EVENT_PRLO_RCVD:
@@ -945,6 +1088,8 @@ bfa_fcs_rport_sm_fc4_offline(struct bfa_fcs_rport_s *rport,
 		bfa_fcs_rport_hal_offline(rport);
 		break;
 
+	case RPSM_EVENT_SCN_ONLINE:
+		break;
 	case RPSM_EVENT_LOGO_RCVD:
 		/*
 		 * Rport is going offline. Just ack the logo
@@ -956,8 +1101,9 @@ bfa_fcs_rport_sm_fc4_offline(struct bfa_fcs_rport_s *rport,
 		bfa_fcs_rport_send_prlo_acc(rport);
 		break;
 
+	case RPSM_EVENT_SCN_OFFLINE:
 	case RPSM_EVENT_HCB_ONLINE:
-	case RPSM_EVENT_SCN:
+	case RPSM_EVENT_FAB_SCN:
 	case RPSM_EVENT_LOGO_IMP:
 	case RPSM_EVENT_ADDRESS_CHANGE:
 		/*
@@ -1015,6 +1161,19 @@ bfa_fcs_rport_sm_hcb_offline(struct bfa_fcs_rport_s *rport,
 				bfa_fcs_rport_sm_nsdisc_sending);
 			rport->ns_retries = 0;
 			bfa_fcs_rport_send_nsdisc(rport, NULL);
+		} else if (bfa_fcport_get_topology(rport->port->fcs->bfa) ==
+					BFA_PORT_TOPOLOGY_LOOP) {
+			if (rport->scn_online) {
+				bfa_sm_set_state(rport,
+					bfa_fcs_rport_sm_adisc_offline_sending);
+				bfa_fcs_rport_send_adisc(rport, NULL);
+			} else {
+				bfa_sm_set_state(rport,
+					bfa_fcs_rport_sm_offline);
+				bfa_timer_start(rport->fcs->bfa, &rport->timer,
+					bfa_fcs_rport_timeout, rport,
+					bfa_fcs_rport_del_timeout);
+			}
 		} else {
 			bfa_sm_set_state(rport, bfa_fcs_rport_sm_plogi_sending);
 			rport->plogi_retries = 0;
@@ -1027,7 +1186,9 @@ bfa_fcs_rport_sm_hcb_offline(struct bfa_fcs_rport_s *rport,
 		bfa_fcs_rport_free(rport);
 		break;
 
-	case RPSM_EVENT_SCN:
+	case RPSM_EVENT_SCN_ONLINE:
+	case RPSM_EVENT_SCN_OFFLINE:
+	case RPSM_EVENT_FAB_SCN:
 	case RPSM_EVENT_LOGO_RCVD:
 	case RPSM_EVENT_PRLO_RCVD:
 	case RPSM_EVENT_PLOGI_RCVD:
@@ -1106,6 +1267,8 @@ bfa_fcs_rport_sm_hcb_logorcv(struct bfa_fcs_rport_s *rport,
 		bfa_sm_set_state(rport, bfa_fcs_rport_sm_hcb_offline);
 		break;
 
+	case RPSM_EVENT_SCN_ONLINE:
+	case RPSM_EVENT_SCN_OFFLINE:
 	case RPSM_EVENT_LOGO_RCVD:
 	case RPSM_EVENT_PRLO_RCVD:
 		/*
@@ -1146,6 +1309,8 @@ bfa_fcs_rport_sm_hcb_logosend(struct bfa_fcs_rport_s *rport,
 		bfa_sm_set_state(rport, bfa_fcs_rport_sm_delete_pending);
 		break;
 
+	case RPSM_EVENT_SCN_ONLINE:
+	case RPSM_EVENT_SCN_OFFLINE:
 	case RPSM_EVENT_ADDRESS_CHANGE:
 		break;
 
@@ -1172,7 +1337,9 @@ bfa_fcs_rport_sm_logo_sending(struct bfa_fcs_rport_s *rport,
 		bfa_fcs_rport_free(rport);
 		break;
 
-	case RPSM_EVENT_SCN:
+	case RPSM_EVENT_SCN_ONLINE:
+	case RPSM_EVENT_SCN_OFFLINE:
+	case RPSM_EVENT_FAB_SCN:
 	case RPSM_EVENT_ADDRESS_CHANGE:
 		break;
 
@@ -1209,10 +1376,12 @@ bfa_fcs_rport_sm_offline(struct bfa_fcs_rport_s *rport, enum rport_event event)
 		bfa_fcs_rport_free(rport);
 		break;
 
-	case RPSM_EVENT_SCN:
+	case RPSM_EVENT_FAB_SCN:
 	case RPSM_EVENT_ADDRESS_CHANGE:
-		bfa_sm_set_state(rport, bfa_fcs_rport_sm_nsdisc_sending);
 		bfa_timer_stop(&rport->timer);
+		WARN_ON(!(bfa_fcport_get_topology(rport->port->fcs->bfa) !=
+					BFA_PORT_TOPOLOGY_LOOP));
+		bfa_sm_set_state(rport, bfa_fcs_rport_sm_nsdisc_sending);
 		rport->ns_retries = 0;
 		bfa_fcs_rport_send_nsdisc(rport, NULL);
 		break;
@@ -1232,6 +1401,7 @@ bfa_fcs_rport_sm_offline(struct bfa_fcs_rport_s *rport, enum rport_event event)
 	case RPSM_EVENT_LOGO_RCVD:
 	case RPSM_EVENT_PRLO_RCVD:
 	case RPSM_EVENT_LOGO_IMP:
+	case RPSM_EVENT_SCN_OFFLINE:
 		break;
 
 	case RPSM_EVENT_PLOGI_COMP:
@@ -1240,6 +1410,12 @@ bfa_fcs_rport_sm_offline(struct bfa_fcs_rport_s *rport, enum rport_event event)
 		bfa_fcs_rport_fcs_online_action(rport);
 		break;
 
+	case RPSM_EVENT_SCN_ONLINE:
+		bfa_timer_stop(&rport->timer);
+		bfa_sm_set_state(rport, bfa_fcs_rport_sm_plogi_sending);
+		bfa_fcs_rport_send_plogi(rport, NULL);
+		break;
+
 	case RPSM_EVENT_PLOGI_SEND:
 		bfa_timer_stop(&rport->timer);
 		bfa_sm_set_state(rport, bfa_fcs_rport_sm_plogi_sending);
@@ -1280,7 +1456,7 @@ bfa_fcs_rport_sm_nsdisc_sending(struct bfa_fcs_rport_s *rport,
 		bfa_fcs_rport_send_plogiacc(rport, NULL);
 		break;
 
-	case RPSM_EVENT_SCN:
+	case RPSM_EVENT_FAB_SCN:
 	case RPSM_EVENT_LOGO_RCVD:
 	case RPSM_EVENT_PRLO_RCVD:
 	case RPSM_EVENT_PLOGI_SEND:
@@ -1326,7 +1502,7 @@ bfa_fcs_rport_sm_nsdisc_retry(struct bfa_fcs_rport_s *rport,
 		bfa_fcs_rport_send_nsdisc(rport, NULL);
 		break;
 
-	case RPSM_EVENT_SCN:
+	case RPSM_EVENT_FAB_SCN:
 	case RPSM_EVENT_ADDRESS_CHANGE:
 		bfa_sm_set_state(rport, bfa_fcs_rport_sm_nsdisc_sending);
 		bfa_timer_stop(&rport->timer);
@@ -1439,7 +1615,7 @@ bfa_fcs_rport_sm_nsdisc_sent(struct bfa_fcs_rport_s *rport,
 	case RPSM_EVENT_PRLO_RCVD:
 		bfa_fcs_rport_send_prlo_acc(rport);
 		break;
-	case RPSM_EVENT_SCN:
+	case RPSM_EVENT_FAB_SCN:
 		/*
 		 * ignore, wait for NS query response
 		 */
@@ -2546,7 +2722,7 @@ void
 bfa_fcs_rport_scn(struct bfa_fcs_rport_s *rport)
 {
 	rport->stats.rscns++;
-	bfa_sm_send_event(rport, RPSM_EVENT_SCN);
+	bfa_sm_send_event(rport, RPSM_EVENT_FAB_SCN);
 }
 
 /*
@@ -2621,6 +2797,48 @@ bfa_cb_rport_qos_scn_flowid(void *cbarg,
 	bfa_fcs_rport_aen_post(rport, BFA_RPORT_AEN_QOS_FLOWID, &aen_data);
 }
 
+void
+bfa_cb_rport_scn_online(struct bfa_s *bfa)
+{
+	struct bfa_fcs_s *fcs = &((struct bfad_s *)bfa->bfad)->bfa_fcs;
+	struct bfa_fcs_lport_s *port = bfa_fcs_get_base_port(fcs);
+	struct bfa_fcs_rport_s *rp;
+	struct list_head *qe;
+
+	list_for_each(qe, &port->rport_q) {
+		rp = (struct bfa_fcs_rport_s *) qe;
+		bfa_sm_send_event(rp, RPSM_EVENT_SCN_ONLINE);
+		rp->scn_online = BFA_TRUE;
+	}
+
+	if (bfa_fcs_lport_is_online(port))
+		bfa_fcs_lport_lip_scn_online(port);
+}
+
+void
+bfa_cb_rport_scn_no_dev(void *rport)
+{
+	struct bfa_fcs_rport_s *rp = rport;
+
+	bfa_sm_send_event(rp, RPSM_EVENT_SCN_OFFLINE);
+	rp->scn_online = BFA_FALSE;
+}
+
+void
+bfa_cb_rport_scn_offline(struct bfa_s *bfa)
+{
+	struct bfa_fcs_s *fcs = &((struct bfad_s *)bfa->bfad)->bfa_fcs;
+	struct bfa_fcs_lport_s *port = bfa_fcs_get_base_port(fcs);
+	struct bfa_fcs_rport_s *rp;
+	struct list_head *qe;
+
+	list_for_each(qe, &port->rport_q) {
+		rp = (struct bfa_fcs_rport_s *) qe;
+		bfa_sm_send_event(rp, RPSM_EVENT_SCN_OFFLINE);
+		rp->scn_online = BFA_FALSE;
+	}
+}
+
 /*
  *	brief
  *	This routine is a static BFA callback when there is a QoS priority
@@ -2808,6 +3026,9 @@ bfa_fcs_rport_get_attr(struct bfa_fcs_rport_s *rport,
 	struct bfa_rport_qos_attr_s qos_attr;
 	struct bfa_fcs_lport_s *port = rport->port;
 	bfa_port_speed_t rport_speed = rport->rpf.rpsc_speed;
+	struct bfa_port_attr_s port_attr;
+
+	bfa_fcport_get_attr(rport->fcs->bfa, &port_attr);
 
 	memset(rport_attr, 0, sizeof(struct bfa_rport_attr_s));
 	memset(&qos_attr, 0, sizeof(struct bfa_rport_qos_attr_s));
@@ -2838,7 +3059,8 @@ bfa_fcs_rport_get_attr(struct bfa_fcs_rport_s *rport,
 			rport_speed =
 				bfa_fcport_get_ratelim_speed(rport->fcs->bfa);
 
-		if (rport_speed < bfa_fcs_lport_get_rport_max_speed(port))
+		if ((bfa_fcs_lport_get_rport_max_speed(port) !=
+		    BFA_PORT_SPEED_UNKNOWN) && (rport_speed < port_attr.speed))
 			rport_attr->trl_enforced = BFA_TRUE;
 	}
 }
diff --git a/drivers/scsi/bfa/bfa_ioc.c b/drivers/scsi/bfa/bfa_ioc.c
index 75ca8752b9f..0116c1032e2 100644
--- a/drivers/scsi/bfa/bfa_ioc.c
+++ b/drivers/scsi/bfa/bfa_ioc.c
@@ -731,8 +731,7 @@ bfa_iocpf_sm_fwcheck_entry(struct bfa_iocpf_s *iocpf)
 	/*
 	 * Unlock the hw semaphore. Should be here only once per boot.
 	 */
-	readl(iocpf->ioc->ioc_regs.ioc_sem_reg);
-	writel(1, iocpf->ioc->ioc_regs.ioc_sem_reg);
+	bfa_ioc_ownership_reset(iocpf->ioc);
 
 	/*
 	 * unlock init semaphore.
@@ -1751,6 +1750,7 @@ bfa_ioc_getattr_reply(struct bfa_ioc_s *ioc)
 	attr->card_type     = be32_to_cpu(attr->card_type);
 	attr->maxfrsize	    = be16_to_cpu(attr->maxfrsize);
 	ioc->fcmode	= (attr->port_mode == BFI_PORT_MODE_FC);
+	attr->mfg_year	= be16_to_cpu(attr->mfg_year);
 
 	bfa_fsm_send_event(ioc, IOC_E_FWRSP_GETATTR);
 }
@@ -2497,6 +2497,9 @@ bfa_ioc_get_adapter_attr(struct bfa_ioc_s *ioc,
 	ad_attr->cna_capable = bfa_ioc_is_cna(ioc);
 	ad_attr->trunk_capable = (ad_attr->nports > 1) &&
 				  !bfa_ioc_is_cna(ioc) && !ad_attr->is_mezz;
+	ad_attr->mfg_day = ioc_attr->mfg_day;
+	ad_attr->mfg_month = ioc_attr->mfg_month;
+	ad_attr->mfg_year = ioc_attr->mfg_year;
 }
 
 enum bfa_ioc_type_e
@@ -2923,7 +2926,7 @@ bfa_ioc_poll_fwinit(struct bfa_ioc_s *ioc)
 		return;
 	}
 
-	if (ioc->iocpf.poll_time >= BFA_IOC_TOV)
+	if (ioc->iocpf.poll_time >= (3 * BFA_IOC_TOV))
 		bfa_iocpf_timeout(ioc);
 	else {
 		ioc->iocpf.poll_time += BFA_IOC_POLL_TOV;
@@ -3016,7 +3019,6 @@ bfa_ablk_config_swap(struct bfa_ablk_cfg_s *cfg)
 	struct bfa_ablk_cfg_inst_s *cfg_inst;
 	int i, j;
 	u16	be16;
-	u32	be32;
 
 	for (i = 0; i < BFA_ABLK_MAX; i++) {
 		cfg_inst = &cfg->inst[i];
@@ -3027,8 +3029,10 @@ bfa_ablk_config_swap(struct bfa_ablk_cfg_s *cfg)
 			cfg_inst->pf_cfg[j].num_qpairs = be16_to_cpu(be16);
 			be16 = cfg_inst->pf_cfg[j].num_vectors;
 			cfg_inst->pf_cfg[j].num_vectors = be16_to_cpu(be16);
-			be32 = cfg_inst->pf_cfg[j].bw;
-			cfg_inst->pf_cfg[j].bw = be16_to_cpu(be32);
+			be16 = cfg_inst->pf_cfg[j].bw_min;
+			cfg_inst->pf_cfg[j].bw_min = be16_to_cpu(be16);
+			be16 = cfg_inst->pf_cfg[j].bw_max;
+			cfg_inst->pf_cfg[j].bw_max = be16_to_cpu(be16);
 		}
 	}
 }
@@ -3170,7 +3174,8 @@ bfa_ablk_query(struct bfa_ablk_s *ablk, struct bfa_ablk_cfg_s *ablk_cfg,
 
 bfa_status_t
 bfa_ablk_pf_create(struct bfa_ablk_s *ablk, u16 *pcifn,
-		u8 port, enum bfi_pcifn_class personality, int bw,
+		u8 port, enum bfi_pcifn_class personality,
+		u16 bw_min, u16 bw_max,
 		bfa_ablk_cbfn_t cbfn, void *cbarg)
 {
 	struct bfi_ablk_h2i_pf_req_s *m;
@@ -3194,7 +3199,8 @@ bfa_ablk_pf_create(struct bfa_ablk_s *ablk, u16 *pcifn,
 	bfi_h2i_set(m->mh, BFI_MC_ABLK, BFI_ABLK_H2I_PF_CREATE,
 		    bfa_ioc_portid(ablk->ioc));
 	m->pers = cpu_to_be16((u16)personality);
-	m->bw = cpu_to_be32(bw);
+	m->bw_min = cpu_to_be16(bw_min);
+	m->bw_max = cpu_to_be16(bw_max);
 	m->port = port;
 	bfa_ioc_mbox_queue(ablk->ioc, &ablk->mb);
 
@@ -3294,8 +3300,8 @@ bfa_ablk_port_config(struct bfa_ablk_s *ablk, int port, enum bfa_mode_s mode,
 }
 
 bfa_status_t
-bfa_ablk_pf_update(struct bfa_ablk_s *ablk, int pcifn, int bw,
-		bfa_ablk_cbfn_t cbfn, void *cbarg)
+bfa_ablk_pf_update(struct bfa_ablk_s *ablk, int pcifn, u16 bw_min,
+		   u16 bw_max, bfa_ablk_cbfn_t cbfn, void *cbarg)
 {
 	struct bfi_ablk_h2i_pf_req_s *m;
 
@@ -3317,7 +3323,8 @@ bfa_ablk_pf_update(struct bfa_ablk_s *ablk, int pcifn, int bw,
 	bfi_h2i_set(m->mh, BFI_MC_ABLK, BFI_ABLK_H2I_PF_UPDATE,
 		bfa_ioc_portid(ablk->ioc));
 	m->pcifn = (u8)pcifn;
-	m->bw = cpu_to_be32(bw);
+	m->bw_min = cpu_to_be16(bw_min);
+	m->bw_max = cpu_to_be16(bw_max);
 	bfa_ioc_mbox_queue(ablk->ioc, &ablk->mb);
 
 	return BFA_STATUS_OK;
@@ -4680,22 +4687,25 @@ diag_tempsensor_comp(struct bfa_diag_s *diag, bfi_diag_ts_rsp_t *rsp)
 	diag->tsensor.temp->temp = be16_to_cpu(rsp->temp);
 	diag->tsensor.temp->ts_junc = rsp->ts_junc;
 	diag->tsensor.temp->ts_brd = rsp->ts_brd;
-	diag->tsensor.temp->status = BFA_STATUS_OK;
 
 	if (rsp->ts_brd) {
+		/* tsensor.temp->status is brd_temp status */
+		diag->tsensor.temp->status = rsp->status;
 		if (rsp->status == BFA_STATUS_OK) {
 			diag->tsensor.temp->brd_temp =
 				be16_to_cpu(rsp->brd_temp);
-		} else {
-			bfa_trc(diag, rsp->status);
+		} else
 			diag->tsensor.temp->brd_temp = 0;
-			diag->tsensor.temp->status = BFA_STATUS_DEVBUSY;
-		}
 	}
+
+	bfa_trc(diag, rsp->status);
 	bfa_trc(diag, rsp->ts_junc);
 	bfa_trc(diag, rsp->temp);
 	bfa_trc(diag, rsp->ts_brd);
 	bfa_trc(diag, rsp->brd_temp);
+
+	/* tsensor status is always good bcos we always have junction temp */
+	diag->tsensor.status = BFA_STATUS_OK;
 	diag->tsensor.cbfn(diag->tsensor.cbarg, diag->tsensor.status);
 	diag->tsensor.lock = 0;
 }
@@ -4924,6 +4934,7 @@ bfa_diag_tsensor_query(struct bfa_diag_s *diag,
 	diag->tsensor.temp = result;
 	diag->tsensor.cbfn = cbfn;
 	diag->tsensor.cbarg = cbarg;
+	diag->tsensor.status = BFA_STATUS_OK;
 
 	/* Send msg to fw */
 	diag_tempsensor_send(diag);
@@ -5615,7 +5626,7 @@ bfa_dconf_sm_uninit(struct bfa_dconf_mod_s *dconf, enum bfa_dconf_event event)
 		}
 		bfa_sm_set_state(dconf, bfa_dconf_sm_flash_read);
 		bfa_timer_start(dconf->bfa, &dconf->timer,
-			bfa_dconf_timer, dconf, BFA_DCONF_UPDATE_TOV);
+			bfa_dconf_timer, dconf, 2 * BFA_DCONF_UPDATE_TOV);
 		bfa_status = bfa_flash_read_part(BFA_FLASH(dconf->bfa),
 					BFA_FLASH_PART_DRV, dconf->instance,
 					dconf->dconf,
@@ -5655,7 +5666,7 @@ bfa_dconf_sm_flash_read(struct bfa_dconf_mod_s *dconf,
 		break;
 	case BFA_DCONF_SM_TIMEOUT:
 		bfa_sm_set_state(dconf, bfa_dconf_sm_ready);
-		bfa_fsm_send_event(&dconf->bfa->iocfc, IOCFC_E_IOC_FAILED);
+		bfa_ioc_suspend(&dconf->bfa->ioc);
 		break;
 	case BFA_DCONF_SM_EXIT:
 		bfa_timer_stop(&dconf->timer);
@@ -5853,7 +5864,6 @@ bfa_dconf_init_cb(void *arg, bfa_status_t status)
 	struct bfa_s *bfa = arg;
 	struct bfa_dconf_mod_s *dconf = BFA_DCONF_MOD(bfa);
 
-	bfa_sm_send_event(dconf, BFA_DCONF_SM_FLASH_COMP);
 	if (status == BFA_STATUS_OK) {
 		bfa_dconf_read_data_valid(bfa) = BFA_TRUE;
 		if (dconf->dconf->hdr.signature != BFI_DCONF_SIGNATURE)
@@ -5861,6 +5871,7 @@ bfa_dconf_init_cb(void *arg, bfa_status_t status)
 		if (dconf->dconf->hdr.version != BFI_DCONF_VERSION)
 			dconf->dconf->hdr.version = BFI_DCONF_VERSION;
 	}
+	bfa_sm_send_event(dconf, BFA_DCONF_SM_FLASH_COMP);
 	bfa_fsm_send_event(&bfa->iocfc, IOCFC_E_DCONF_DONE);
 }
 
@@ -5945,3 +5956,448 @@ bfa_dconf_modexit(struct bfa_s *bfa)
 	struct bfa_dconf_mod_s *dconf = BFA_DCONF_MOD(bfa);
 	bfa_sm_send_event(dconf, BFA_DCONF_SM_EXIT);
 }
+
+/*
+ * FRU specific functions
+ */
+
+#define BFA_FRU_DMA_BUF_SZ	0x02000		/* 8k dma buffer */
+#define BFA_FRU_CHINOOK_MAX_SIZE 0x10000
+#define BFA_FRU_LIGHTNING_MAX_SIZE 0x200
+
+static void
+bfa_fru_notify(void *cbarg, enum bfa_ioc_event_e event)
+{
+	struct bfa_fru_s *fru = cbarg;
+
+	bfa_trc(fru, event);
+
+	switch (event) {
+	case BFA_IOC_E_DISABLED:
+	case BFA_IOC_E_FAILED:
+		if (fru->op_busy) {
+			fru->status = BFA_STATUS_IOC_FAILURE;
+			fru->cbfn(fru->cbarg, fru->status);
+			fru->op_busy = 0;
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+/*
+ * Send fru write request.
+ *
+ * @param[in] cbarg - callback argument
+ */
+static void
+bfa_fru_write_send(void *cbarg, enum bfi_fru_h2i_msgs msg_type)
+{
+	struct bfa_fru_s *fru = cbarg;
+	struct bfi_fru_write_req_s *msg =
+			(struct bfi_fru_write_req_s *) fru->mb.msg;
+	u32 len;
+
+	msg->offset = cpu_to_be32(fru->addr_off + fru->offset);
+	len = (fru->residue < BFA_FRU_DMA_BUF_SZ) ?
+				fru->residue : BFA_FRU_DMA_BUF_SZ;
+	msg->length = cpu_to_be32(len);
+
+	/*
+	 * indicate if it's the last msg of the whole write operation
+	 */
+	msg->last = (len == fru->residue) ? 1 : 0;
+
+	bfi_h2i_set(msg->mh, BFI_MC_FRU, msg_type, bfa_ioc_portid(fru->ioc));
+	bfa_alen_set(&msg->alen, len, fru->dbuf_pa);
+
+	memcpy(fru->dbuf_kva, fru->ubuf + fru->offset, len);
+	bfa_ioc_mbox_queue(fru->ioc, &fru->mb);
+
+	fru->residue -= len;
+	fru->offset += len;
+}
+
+/*
+ * Send fru read request.
+ *
+ * @param[in] cbarg - callback argument
+ */
+static void
+bfa_fru_read_send(void *cbarg, enum bfi_fru_h2i_msgs msg_type)
+{
+	struct bfa_fru_s *fru = cbarg;
+	struct bfi_fru_read_req_s *msg =
+			(struct bfi_fru_read_req_s *) fru->mb.msg;
+	u32 len;
+
+	msg->offset = cpu_to_be32(fru->addr_off + fru->offset);
+	len = (fru->residue < BFA_FRU_DMA_BUF_SZ) ?
+				fru->residue : BFA_FRU_DMA_BUF_SZ;
+	msg->length = cpu_to_be32(len);
+	bfi_h2i_set(msg->mh, BFI_MC_FRU, msg_type, bfa_ioc_portid(fru->ioc));
+	bfa_alen_set(&msg->alen, len, fru->dbuf_pa);
+	bfa_ioc_mbox_queue(fru->ioc, &fru->mb);
+}
+
+/*
+ * Flash memory info API.
+ *
+ * @param[in] mincfg - minimal cfg variable
+ */
+u32
+bfa_fru_meminfo(bfa_boolean_t mincfg)
+{
+	/* min driver doesn't need fru */
+	if (mincfg)
+		return 0;
+
+	return BFA_ROUNDUP(BFA_FRU_DMA_BUF_SZ, BFA_DMA_ALIGN_SZ);
+}
+
+/*
+ * Flash attach API.
+ *
+ * @param[in] fru - fru structure
+ * @param[in] ioc  - ioc structure
+ * @param[in] dev  - device structure
+ * @param[in] trcmod - trace module
+ * @param[in] logmod - log module
+ */
+void
+bfa_fru_attach(struct bfa_fru_s *fru, struct bfa_ioc_s *ioc, void *dev,
+	struct bfa_trc_mod_s *trcmod, bfa_boolean_t mincfg)
+{
+	fru->ioc = ioc;
+	fru->trcmod = trcmod;
+	fru->cbfn = NULL;
+	fru->cbarg = NULL;
+	fru->op_busy = 0;
+
+	bfa_ioc_mbox_regisr(fru->ioc, BFI_MC_FRU, bfa_fru_intr, fru);
+	bfa_q_qe_init(&fru->ioc_notify);
+	bfa_ioc_notify_init(&fru->ioc_notify, bfa_fru_notify, fru);
+	list_add_tail(&fru->ioc_notify.qe, &fru->ioc->notify_q);
+
+	/* min driver doesn't need fru */
+	if (mincfg) {
+		fru->dbuf_kva = NULL;
+		fru->dbuf_pa = 0;
+	}
+}
+
+/*
+ * Claim memory for fru
+ *
+ * @param[in] fru - fru structure
+ * @param[in] dm_kva - pointer to virtual memory address
+ * @param[in] dm_pa - frusical memory address
+ * @param[in] mincfg - minimal cfg variable
+ */
+void
+bfa_fru_memclaim(struct bfa_fru_s *fru, u8 *dm_kva, u64 dm_pa,
+	bfa_boolean_t mincfg)
+{
+	if (mincfg)
+		return;
+
+	fru->dbuf_kva = dm_kva;
+	fru->dbuf_pa = dm_pa;
+	memset(fru->dbuf_kva, 0, BFA_FRU_DMA_BUF_SZ);
+	dm_kva += BFA_ROUNDUP(BFA_FRU_DMA_BUF_SZ, BFA_DMA_ALIGN_SZ);
+	dm_pa += BFA_ROUNDUP(BFA_FRU_DMA_BUF_SZ, BFA_DMA_ALIGN_SZ);
+}
+
+/*
+ * Update fru vpd image.
+ *
+ * @param[in] fru - fru structure
+ * @param[in] buf - update data buffer
+ * @param[in] len - data buffer length
+ * @param[in] offset - offset relative to starting address
+ * @param[in] cbfn - callback function
+ * @param[in] cbarg - callback argument
+ *
+ * Return status.
+ */
+bfa_status_t
+bfa_fruvpd_update(struct bfa_fru_s *fru, void *buf, u32 len, u32 offset,
+		  bfa_cb_fru_t cbfn, void *cbarg)
+{
+	bfa_trc(fru, BFI_FRUVPD_H2I_WRITE_REQ);
+	bfa_trc(fru, len);
+	bfa_trc(fru, offset);
+
+	if (fru->ioc->asic_gen != BFI_ASIC_GEN_CT2)
+		return BFA_STATUS_FRU_NOT_PRESENT;
+
+	if (fru->ioc->attr->card_type != BFA_MFG_TYPE_CHINOOK)
+		return BFA_STATUS_CMD_NOTSUPP;
+
+	if (!bfa_ioc_is_operational(fru->ioc))
+		return BFA_STATUS_IOC_NON_OP;
+
+	if (fru->op_busy) {
+		bfa_trc(fru, fru->op_busy);
+		return BFA_STATUS_DEVBUSY;
+	}
+
+	fru->op_busy = 1;
+
+	fru->cbfn = cbfn;
+	fru->cbarg = cbarg;
+	fru->residue = len;
+	fru->offset = 0;
+	fru->addr_off = offset;
+	fru->ubuf = buf;
+
+	bfa_fru_write_send(fru, BFI_FRUVPD_H2I_WRITE_REQ);
+
+	return BFA_STATUS_OK;
+}
+
+/*
+ * Read fru vpd image.
+ *
+ * @param[in] fru - fru structure
+ * @param[in] buf - read data buffer
+ * @param[in] len - data buffer length
+ * @param[in] offset - offset relative to starting address
+ * @param[in] cbfn - callback function
+ * @param[in] cbarg - callback argument
+ *
+ * Return status.
+ */
+bfa_status_t
+bfa_fruvpd_read(struct bfa_fru_s *fru, void *buf, u32 len, u32 offset,
+		bfa_cb_fru_t cbfn, void *cbarg)
+{
+	bfa_trc(fru, BFI_FRUVPD_H2I_READ_REQ);
+	bfa_trc(fru, len);
+	bfa_trc(fru, offset);
+
+	if (fru->ioc->asic_gen != BFI_ASIC_GEN_CT2)
+		return BFA_STATUS_FRU_NOT_PRESENT;
+
+	if (fru->ioc->attr->card_type != BFA_MFG_TYPE_CHINOOK)
+		return BFA_STATUS_CMD_NOTSUPP;
+
+	if (!bfa_ioc_is_operational(fru->ioc))
+		return BFA_STATUS_IOC_NON_OP;
+
+	if (fru->op_busy) {
+		bfa_trc(fru, fru->op_busy);
+		return BFA_STATUS_DEVBUSY;
+	}
+
+	fru->op_busy = 1;
+
+	fru->cbfn = cbfn;
+	fru->cbarg = cbarg;
+	fru->residue = len;
+	fru->offset = 0;
+	fru->addr_off = offset;
+	fru->ubuf = buf;
+	bfa_fru_read_send(fru, BFI_FRUVPD_H2I_READ_REQ);
+
+	return BFA_STATUS_OK;
+}
+
+/*
+ * Get maximum size fru vpd image.
+ *
+ * @param[in] fru - fru structure
+ * @param[out] size - maximum size of fru vpd data
+ *
+ * Return status.
+ */
+bfa_status_t
+bfa_fruvpd_get_max_size(struct bfa_fru_s *fru, u32 *max_size)
+{
+	if (fru->ioc->asic_gen != BFI_ASIC_GEN_CT2)
+		return BFA_STATUS_FRU_NOT_PRESENT;
+
+	if (!bfa_ioc_is_operational(fru->ioc))
+		return BFA_STATUS_IOC_NON_OP;
+
+	if (fru->ioc->attr->card_type == BFA_MFG_TYPE_CHINOOK)
+		*max_size = BFA_FRU_CHINOOK_MAX_SIZE;
+	else
+		return BFA_STATUS_CMD_NOTSUPP;
+	return BFA_STATUS_OK;
+}
+/*
+ * tfru write.
+ *
+ * @param[in] fru - fru structure
+ * @param[in] buf - update data buffer
+ * @param[in] len - data buffer length
+ * @param[in] offset - offset relative to starting address
+ * @param[in] cbfn - callback function
+ * @param[in] cbarg - callback argument
+ *
+ * Return status.
+ */
+bfa_status_t
+bfa_tfru_write(struct bfa_fru_s *fru, void *buf, u32 len, u32 offset,
+	       bfa_cb_fru_t cbfn, void *cbarg)
+{
+	bfa_trc(fru, BFI_TFRU_H2I_WRITE_REQ);
+	bfa_trc(fru, len);
+	bfa_trc(fru, offset);
+	bfa_trc(fru, *((u8 *) buf));
+
+	if (fru->ioc->asic_gen != BFI_ASIC_GEN_CT2)
+		return BFA_STATUS_FRU_NOT_PRESENT;
+
+	if (!bfa_ioc_is_operational(fru->ioc))
+		return BFA_STATUS_IOC_NON_OP;
+
+	if (fru->op_busy) {
+		bfa_trc(fru, fru->op_busy);
+		return BFA_STATUS_DEVBUSY;
+	}
+
+	fru->op_busy = 1;
+
+	fru->cbfn = cbfn;
+	fru->cbarg = cbarg;
+	fru->residue = len;
+	fru->offset = 0;
+	fru->addr_off = offset;
+	fru->ubuf = buf;
+
+	bfa_fru_write_send(fru, BFI_TFRU_H2I_WRITE_REQ);
+
+	return BFA_STATUS_OK;
+}
+
+/*
+ * tfru read.
+ *
+ * @param[in] fru - fru structure
+ * @param[in] buf - read data buffer
+ * @param[in] len - data buffer length
+ * @param[in] offset - offset relative to starting address
+ * @param[in] cbfn - callback function
+ * @param[in] cbarg - callback argument
+ *
+ * Return status.
+ */
+bfa_status_t
+bfa_tfru_read(struct bfa_fru_s *fru, void *buf, u32 len, u32 offset,
+	      bfa_cb_fru_t cbfn, void *cbarg)
+{
+	bfa_trc(fru, BFI_TFRU_H2I_READ_REQ);
+	bfa_trc(fru, len);
+	bfa_trc(fru, offset);
+
+	if (fru->ioc->asic_gen != BFI_ASIC_GEN_CT2)
+		return BFA_STATUS_FRU_NOT_PRESENT;
+
+	if (!bfa_ioc_is_operational(fru->ioc))
+		return BFA_STATUS_IOC_NON_OP;
+
+	if (fru->op_busy) {
+		bfa_trc(fru, fru->op_busy);
+		return BFA_STATUS_DEVBUSY;
+	}
+
+	fru->op_busy = 1;
+
+	fru->cbfn = cbfn;
+	fru->cbarg = cbarg;
+	fru->residue = len;
+	fru->offset = 0;
+	fru->addr_off = offset;
+	fru->ubuf = buf;
+	bfa_fru_read_send(fru, BFI_TFRU_H2I_READ_REQ);
+
+	return BFA_STATUS_OK;
+}
+
+/*
+ * Process fru response messages upon receiving interrupts.
+ *
+ * @param[in] fruarg - fru structure
+ * @param[in] msg - message structure
+ */
+void
+bfa_fru_intr(void *fruarg, struct bfi_mbmsg_s *msg)
+{
+	struct bfa_fru_s *fru = fruarg;
+	struct bfi_fru_rsp_s *rsp = (struct bfi_fru_rsp_s *)msg;
+	u32 status;
+
+	bfa_trc(fru, msg->mh.msg_id);
+
+	if (!fru->op_busy) {
+		/*
+		 * receiving response after ioc failure
+		 */
+		bfa_trc(fru, 0x9999);
+		return;
+	}
+
+	switch (msg->mh.msg_id) {
+	case BFI_FRUVPD_I2H_WRITE_RSP:
+	case BFI_TFRU_I2H_WRITE_RSP:
+		status = be32_to_cpu(rsp->status);
+		bfa_trc(fru, status);
+
+		if (status != BFA_STATUS_OK || fru->residue == 0) {
+			fru->status = status;
+			fru->op_busy = 0;
+			if (fru->cbfn)
+				fru->cbfn(fru->cbarg, fru->status);
+		} else {
+			bfa_trc(fru, fru->offset);
+			if (msg->mh.msg_id == BFI_FRUVPD_I2H_WRITE_RSP)
+				bfa_fru_write_send(fru,
+					BFI_FRUVPD_H2I_WRITE_REQ);
+			else
+				bfa_fru_write_send(fru,
+					BFI_TFRU_H2I_WRITE_REQ);
+		}
+		break;
+	case BFI_FRUVPD_I2H_READ_RSP:
+	case BFI_TFRU_I2H_READ_RSP:
+		status = be32_to_cpu(rsp->status);
+		bfa_trc(fru, status);
+
+		if (status != BFA_STATUS_OK) {
+			fru->status = status;
+			fru->op_busy = 0;
+			if (fru->cbfn)
+				fru->cbfn(fru->cbarg, fru->status);
+		} else {
+			u32 len = be32_to_cpu(rsp->length);
+
+			bfa_trc(fru, fru->offset);
+			bfa_trc(fru, len);
+
+			memcpy(fru->ubuf + fru->offset, fru->dbuf_kva, len);
+			fru->residue -= len;
+			fru->offset += len;
+
+			if (fru->residue == 0) {
+				fru->status = status;
+				fru->op_busy = 0;
+				if (fru->cbfn)
+					fru->cbfn(fru->cbarg, fru->status);
+			} else {
+				if (msg->mh.msg_id == BFI_FRUVPD_I2H_READ_RSP)
+					bfa_fru_read_send(fru,
+						BFI_FRUVPD_H2I_READ_REQ);
+				else
+					bfa_fru_read_send(fru,
+						BFI_TFRU_H2I_READ_REQ);
+			}
+		}
+		break;
+	default:
+		WARN_ON(1);
+	}
+}
diff --git a/drivers/scsi/bfa/bfa_ioc.h b/drivers/scsi/bfa/bfa_ioc.h
index b2856f96567..23a90e7b710 100644
--- a/drivers/scsi/bfa/bfa_ioc.h
+++ b/drivers/scsi/bfa/bfa_ioc.h
@@ -702,6 +702,55 @@ void bfa_phy_memclaim(struct bfa_phy_s *phy,
 void bfa_phy_intr(void *phyarg, struct bfi_mbmsg_s *msg);
 
 /*
+ * FRU module specific
+ */
+typedef void (*bfa_cb_fru_t) (void *cbarg, bfa_status_t status);
+
+struct bfa_fru_s {
+	struct bfa_ioc_s *ioc;		/* back pointer to ioc */
+	struct bfa_trc_mod_s *trcmod;	/* trace module */
+	u8		op_busy;	/* operation busy flag */
+	u8		rsv[3];
+	u32		residue;	/* residual length */
+	u32		offset;		/* offset */
+	bfa_status_t	status;		/* status */
+	u8		*dbuf_kva;	/* dma buf virtual address */
+	u64		dbuf_pa;	/* dma buf physical address */
+	struct bfa_reqq_wait_s reqq_wait; /* to wait for room in reqq */
+	bfa_cb_fru_t	cbfn;		/* user callback function */
+	void		*cbarg;		/* user callback arg */
+	u8		*ubuf;		/* user supplied buffer */
+	struct bfa_cb_qe_s	hcb_qe;	/* comp: BFA callback qelem */
+	u32		addr_off;	/* fru address offset */
+	struct bfa_mbox_cmd_s mb;	/* mailbox */
+	struct bfa_ioc_notify_s ioc_notify; /* ioc event notify */
+	struct bfa_mem_dma_s	fru_dma;
+};
+
+#define BFA_FRU(__bfa)	(&(__bfa)->modules.fru)
+#define BFA_MEM_FRU_DMA(__bfa)	(&(BFA_FRU(__bfa)->fru_dma))
+
+bfa_status_t bfa_fruvpd_update(struct bfa_fru_s *fru,
+			void *buf, u32 len, u32 offset,
+			bfa_cb_fru_t cbfn, void *cbarg);
+bfa_status_t bfa_fruvpd_read(struct bfa_fru_s *fru,
+			void *buf, u32 len, u32 offset,
+			bfa_cb_fru_t cbfn, void *cbarg);
+bfa_status_t bfa_fruvpd_get_max_size(struct bfa_fru_s *fru, u32 *max_size);
+bfa_status_t bfa_tfru_write(struct bfa_fru_s *fru,
+			void *buf, u32 len, u32 offset,
+			bfa_cb_fru_t cbfn, void *cbarg);
+bfa_status_t bfa_tfru_read(struct bfa_fru_s *fru,
+			void *buf, u32 len, u32 offset,
+			bfa_cb_fru_t cbfn, void *cbarg);
+u32	bfa_fru_meminfo(bfa_boolean_t mincfg);
+void bfa_fru_attach(struct bfa_fru_s *fru, struct bfa_ioc_s *ioc,
+		void *dev, struct bfa_trc_mod_s *trcmod, bfa_boolean_t mincfg);
+void bfa_fru_memclaim(struct bfa_fru_s *fru,
+		u8 *dm_kva, u64 dm_pa, bfa_boolean_t mincfg);
+void bfa_fru_intr(void *fruarg, struct bfi_mbmsg_s *msg);
+
+/*
  * Driver Config( dconf) specific
  */
 #define BFI_DCONF_SIGNATURE	0xabcdabcd
@@ -716,6 +765,7 @@ struct bfa_dconf_hdr_s {
 struct bfa_dconf_s {
 	struct bfa_dconf_hdr_s		hdr;
 	struct bfa_lunmask_cfg_s	lun_mask;
+	struct bfa_throttle_cfg_s	throttle_cfg;
 };
 #pragma pack()
 
@@ -738,6 +788,8 @@ struct bfa_dconf_mod_s {
 #define bfa_dconf_read_data_valid(__bfa)	\
 	(BFA_DCONF_MOD(__bfa)->read_data_valid)
 #define BFA_DCONF_UPDATE_TOV	5000	/* memtest timeout in msec */
+#define bfa_dconf_get_min_cfg(__bfa)	\
+	(BFA_DCONF_MOD(__bfa)->min_cfg)
 
 void	bfa_dconf_modinit(struct bfa_s *bfa);
 void	bfa_dconf_modexit(struct bfa_s *bfa);
@@ -761,7 +813,8 @@ bfa_status_t	bfa_dconf_update(struct bfa_s *bfa);
 #define bfa_ioc_maxfrsize(__ioc)	((__ioc)->attr->maxfrsize)
 #define bfa_ioc_rx_bbcredit(__ioc)	((__ioc)->attr->rx_bbcredit)
 #define bfa_ioc_speed_sup(__ioc)	\
-	BFI_ADAPTER_GETP(SPEED, (__ioc)->attr->adapter_prop)
+	((bfa_ioc_is_cna(__ioc)) ? BFA_PORT_SPEED_10GBPS :	\
+	 BFI_ADAPTER_GETP(SPEED, (__ioc)->attr->adapter_prop))
 #define bfa_ioc_get_nports(__ioc)	\
 	BFI_ADAPTER_GETP(NPORTS, (__ioc)->attr->adapter_prop)
 
@@ -885,12 +938,12 @@ bfa_status_t bfa_ablk_port_config(struct bfa_ablk_s *ablk, int port,
 		enum bfa_mode_s mode, int max_pf, int max_vf,
 		bfa_ablk_cbfn_t cbfn, void *cbarg);
 bfa_status_t bfa_ablk_pf_create(struct bfa_ablk_s *ablk, u16 *pcifn,
-		u8 port, enum bfi_pcifn_class personality, int bw,
-		bfa_ablk_cbfn_t cbfn, void *cbarg);
+		u8 port, enum bfi_pcifn_class personality,
+		u16 bw_min, u16 bw_max, bfa_ablk_cbfn_t cbfn, void *cbarg);
 bfa_status_t bfa_ablk_pf_delete(struct bfa_ablk_s *ablk, int pcifn,
 		bfa_ablk_cbfn_t cbfn, void *cbarg);
-bfa_status_t bfa_ablk_pf_update(struct bfa_ablk_s *ablk, int pcifn, int bw,
-		bfa_ablk_cbfn_t cbfn, void *cbarg);
+bfa_status_t bfa_ablk_pf_update(struct bfa_ablk_s *ablk, int pcifn,
+		u16 bw_min, u16 bw_max, bfa_ablk_cbfn_t cbfn, void *cbarg);
 bfa_status_t bfa_ablk_optrom_en(struct bfa_ablk_s *ablk,
 		bfa_ablk_cbfn_t cbfn, void *cbarg);
 bfa_status_t bfa_ablk_optrom_dis(struct bfa_ablk_s *ablk,
diff --git a/drivers/scsi/bfa/bfa_ioc_ct.c b/drivers/scsi/bfa/bfa_ioc_ct.c
index 2eb0c6a2938..de4e726a126 100644
--- a/drivers/scsi/bfa/bfa_ioc_ct.c
+++ b/drivers/scsi/bfa/bfa_ioc_ct.c
@@ -57,13 +57,6 @@ bfa_ioc_ct_firmware_lock(struct bfa_ioc_s *ioc)
 	u32 usecnt;
 	struct bfi_ioc_image_hdr_s fwhdr;
 
-	/*
-	 * If bios boot (flash based) -- do not increment usage count
-	 */
-	if (bfa_cb_image_get_size(bfa_ioc_asic_gen(ioc)) <
-						BFA_IOC_FWIMG_MINSZ)
-		return BFA_TRUE;
-
 	bfa_ioc_sem_get(ioc->ioc_regs.ioc_usage_sem_reg);
 	usecnt = readl(ioc->ioc_regs.ioc_usage_reg);
 
@@ -115,13 +108,6 @@ bfa_ioc_ct_firmware_unlock(struct bfa_ioc_s *ioc)
 	u32 usecnt;
 
 	/*
-	 * If bios boot (flash based) -- do not decrement usage count
-	 */
-	if (bfa_cb_image_get_size(bfa_ioc_asic_gen(ioc)) <
-						BFA_IOC_FWIMG_MINSZ)
-		return;
-
-	/*
 	 * decrement usage count
 	 */
 	bfa_ioc_sem_get(ioc->ioc_regs.ioc_usage_sem_reg);
@@ -400,13 +386,12 @@ static void
 bfa_ioc_ct_ownership_reset(struct bfa_ioc_s *ioc)
 {
 
-	if (bfa_ioc_is_cna(ioc)) {
-		bfa_ioc_sem_get(ioc->ioc_regs.ioc_usage_sem_reg);
-		writel(0, ioc->ioc_regs.ioc_usage_reg);
-		readl(ioc->ioc_regs.ioc_usage_sem_reg);
-		writel(1, ioc->ioc_regs.ioc_usage_sem_reg);
-	}
+	bfa_ioc_sem_get(ioc->ioc_regs.ioc_usage_sem_reg);
+	writel(0, ioc->ioc_regs.ioc_usage_reg);
+	readl(ioc->ioc_regs.ioc_usage_sem_reg);
+	writel(1, ioc->ioc_regs.ioc_usage_sem_reg);
 
+	writel(0, ioc->ioc_regs.ioc_fail_sync);
 	/*
 	 * Read the hw sem reg to make sure that it is locked
 	 * before we clear it. If it is not locked, writing 1
@@ -759,25 +744,6 @@ bfa_ioc_ct2_mem_init(void __iomem *rb)
 void
 bfa_ioc_ct2_mac_reset(void __iomem *rb)
 {
-	u32	r32;
-
-	bfa_ioc_ct2_sclk_init(rb);
-	bfa_ioc_ct2_lclk_init(rb);
-
-	/*
-	 * release soft reset on s_clk & l_clk
-	 */
-	r32 = readl((rb + CT2_APP_PLL_SCLK_CTL_REG));
-	writel(r32 & ~__APP_PLL_SCLK_LOGIC_SOFT_RESET,
-		(rb + CT2_APP_PLL_SCLK_CTL_REG));
-
-	/*
-	 * release soft reset on s_clk & l_clk
-	 */
-	r32 = readl((rb + CT2_APP_PLL_LCLK_CTL_REG));
-	writel(r32 & ~__APP_PLL_LCLK_LOGIC_SOFT_RESET,
-		(rb + CT2_APP_PLL_LCLK_CTL_REG));
-
 	/* put port0, port1 MAC & AHB in reset */
 	writel((__CSI_MAC_RESET | __CSI_MAC_AHB_RESET),
 		rb + CT2_CSI_MAC_CONTROL_REG(0));
@@ -785,8 +751,21 @@ bfa_ioc_ct2_mac_reset(void __iomem *rb)
 		rb + CT2_CSI_MAC_CONTROL_REG(1));
 }
 
+static void
+bfa_ioc_ct2_enable_flash(void __iomem *rb)
+{
+	u32 r32;
+
+	r32 = readl((rb + PSS_GPIO_OUT_REG));
+	writel(r32 & ~1, (rb + PSS_GPIO_OUT_REG));
+	r32 = readl((rb + PSS_GPIO_OE_REG));
+	writel(r32 | 1, (rb + PSS_GPIO_OE_REG));
+}
+
 #define CT2_NFC_MAX_DELAY	1000
-#define CT2_NFC_VER_VALID	0x143
+#define CT2_NFC_PAUSE_MAX_DELAY 4000
+#define CT2_NFC_VER_VALID	0x147
+#define CT2_NFC_STATE_RUNNING   0x20000001
 #define BFA_IOC_PLL_POLL	1000000
 
 static bfa_boolean_t
@@ -802,6 +781,20 @@ bfa_ioc_ct2_nfc_halted(void __iomem *rb)
 }
 
 static void
+bfa_ioc_ct2_nfc_halt(void __iomem *rb)
+{
+	int	i;
+
+	writel(__HALT_NFC_CONTROLLER, rb + CT2_NFC_CSR_SET_REG);
+	for (i = 0; i < CT2_NFC_MAX_DELAY; i++) {
+		if (bfa_ioc_ct2_nfc_halted(rb))
+			break;
+		udelay(1000);
+	}
+	WARN_ON(!bfa_ioc_ct2_nfc_halted(rb));
+}
+
+static void
 bfa_ioc_ct2_nfc_resume(void __iomem *rb)
 {
 	u32	r32;
@@ -817,105 +810,142 @@ bfa_ioc_ct2_nfc_resume(void __iomem *rb)
 	WARN_ON(1);
 }
 
-bfa_status_t
-bfa_ioc_ct2_pll_init(void __iomem *rb, enum bfi_asic_mode mode)
+static void
+bfa_ioc_ct2_clk_reset(void __iomem *rb)
 {
-	u32 wgn, r32, nfc_ver, i;
+	u32 r32;
 
-	wgn = readl(rb + CT2_WGN_STATUS);
-	nfc_ver = readl(rb + CT2_RSC_GPR15_REG);
+	bfa_ioc_ct2_sclk_init(rb);
+	bfa_ioc_ct2_lclk_init(rb);
 
-	if ((wgn == (__A2T_AHB_LOAD | __WGN_READY)) &&
-	    (nfc_ver >= CT2_NFC_VER_VALID)) {
-		if (bfa_ioc_ct2_nfc_halted(rb))
-			bfa_ioc_ct2_nfc_resume(rb);
+	/*
+	 * release soft reset on s_clk & l_clk
+	 */
+	r32 = readl((rb + CT2_APP_PLL_SCLK_CTL_REG));
+	writel(r32 & ~__APP_PLL_SCLK_LOGIC_SOFT_RESET,
+			(rb + CT2_APP_PLL_SCLK_CTL_REG));
 
-		writel(__RESET_AND_START_SCLK_LCLK_PLLS,
-		       rb + CT2_CSI_FW_CTL_SET_REG);
+	r32 = readl((rb + CT2_APP_PLL_LCLK_CTL_REG));
+	writel(r32 & ~__APP_PLL_LCLK_LOGIC_SOFT_RESET,
+			(rb + CT2_APP_PLL_LCLK_CTL_REG));
 
-		for (i = 0; i < BFA_IOC_PLL_POLL; i++) {
-			r32 = readl(rb + CT2_APP_PLL_LCLK_CTL_REG);
-			if (r32 & __RESET_AND_START_SCLK_LCLK_PLLS)
-				break;
-		}
+}
 
-		WARN_ON(!(r32 & __RESET_AND_START_SCLK_LCLK_PLLS));
+static void
+bfa_ioc_ct2_nfc_clk_reset(void __iomem *rb)
+{
+	u32 r32, i;
 
-		for (i = 0; i < BFA_IOC_PLL_POLL; i++) {
-			r32 = readl(rb + CT2_APP_PLL_LCLK_CTL_REG);
-			if (!(r32 & __RESET_AND_START_SCLK_LCLK_PLLS))
-				break;
-		}
+	r32 = readl((rb + PSS_CTL_REG));
+	r32 |= (__PSS_LPU0_RESET | __PSS_LPU1_RESET);
+	writel(r32, (rb + PSS_CTL_REG));
+
+	writel(__RESET_AND_START_SCLK_LCLK_PLLS, rb + CT2_CSI_FW_CTL_SET_REG);
 
-		WARN_ON(r32 & __RESET_AND_START_SCLK_LCLK_PLLS);
+	for (i = 0; i < BFA_IOC_PLL_POLL; i++) {
+		r32 = readl(rb + CT2_NFC_FLASH_STS_REG);
+
+		if ((r32 & __FLASH_PLL_INIT_AND_RESET_IN_PROGRESS))
+			break;
+	}
+	WARN_ON(!(r32 & __FLASH_PLL_INIT_AND_RESET_IN_PROGRESS));
+
+	for (i = 0; i < BFA_IOC_PLL_POLL; i++) {
+		r32 = readl(rb + CT2_NFC_FLASH_STS_REG);
+
+		if (!(r32 & __FLASH_PLL_INIT_AND_RESET_IN_PROGRESS))
+			break;
+	}
+	WARN_ON((r32 & __FLASH_PLL_INIT_AND_RESET_IN_PROGRESS));
+
+	r32 = readl(rb + CT2_CSI_FW_CTL_REG);
+	WARN_ON((r32 & __RESET_AND_START_SCLK_LCLK_PLLS));
+}
+
+static void
+bfa_ioc_ct2_wait_till_nfc_running(void __iomem *rb)
+{
+	u32 r32;
+	int i;
+
+	if (bfa_ioc_ct2_nfc_halted(rb))
+		bfa_ioc_ct2_nfc_resume(rb);
+	for (i = 0; i < CT2_NFC_PAUSE_MAX_DELAY; i++) {
+		r32 = readl(rb + CT2_NFC_STS_REG);
+		if (r32 == CT2_NFC_STATE_RUNNING)
+			return;
 		udelay(1000);
+	}
 
-		r32 = readl(rb + CT2_CSI_FW_CTL_REG);
-		WARN_ON(r32 & __RESET_AND_START_SCLK_LCLK_PLLS);
-	} else {
-		writel(__HALT_NFC_CONTROLLER, rb + CT2_NFC_CSR_SET_REG);
-		for (i = 0; i < CT2_NFC_MAX_DELAY; i++) {
-			r32 = readl(rb + CT2_NFC_CSR_SET_REG);
-			if (r32 & __NFC_CONTROLLER_HALTED)
-				break;
-			udelay(1000);
-		}
+	r32 = readl(rb + CT2_NFC_STS_REG);
+	WARN_ON(!(r32 == CT2_NFC_STATE_RUNNING));
+}
 
-		bfa_ioc_ct2_mac_reset(rb);
-		bfa_ioc_ct2_sclk_init(rb);
-		bfa_ioc_ct2_lclk_init(rb);
+bfa_status_t
+bfa_ioc_ct2_pll_init(void __iomem *rb, enum bfi_asic_mode mode)
+{
+	u32 wgn, r32, nfc_ver;
 
-		/*
-		 * release soft reset on s_clk & l_clk
-		 */
-		r32 = readl(rb + CT2_APP_PLL_SCLK_CTL_REG);
-		writel(r32 & ~__APP_PLL_SCLK_LOGIC_SOFT_RESET,
-		       (rb + CT2_APP_PLL_SCLK_CTL_REG));
+	wgn = readl(rb + CT2_WGN_STATUS);
 
+	if (wgn == (__WGN_READY | __GLBL_PF_VF_CFG_RDY)) {
 		/*
-		 * release soft reset on s_clk & l_clk
+		 * If flash is corrupted, enable flash explicitly
 		 */
-		r32 = readl(rb + CT2_APP_PLL_LCLK_CTL_REG);
-		writel(r32 & ~__APP_PLL_LCLK_LOGIC_SOFT_RESET,
-		      (rb + CT2_APP_PLL_LCLK_CTL_REG));
-	}
+		bfa_ioc_ct2_clk_reset(rb);
+		bfa_ioc_ct2_enable_flash(rb);
 
-	/*
-	 * Announce flash device presence, if flash was corrupted.
-	 */
-	if (wgn == (__WGN_READY | __GLBL_PF_VF_CFG_RDY)) {
-		r32 = readl(rb + PSS_GPIO_OUT_REG);
-		writel(r32 & ~1, (rb + PSS_GPIO_OUT_REG));
-		r32 = readl(rb + PSS_GPIO_OE_REG);
-		writel(r32 | 1, (rb + PSS_GPIO_OE_REG));
+		bfa_ioc_ct2_mac_reset(rb);
+
+		bfa_ioc_ct2_clk_reset(rb);
+		bfa_ioc_ct2_enable_flash(rb);
+
+	} else {
+		nfc_ver = readl(rb + CT2_RSC_GPR15_REG);
+
+		if ((nfc_ver >= CT2_NFC_VER_VALID) &&
+		    (wgn == (__A2T_AHB_LOAD | __WGN_READY))) {
+
+			bfa_ioc_ct2_wait_till_nfc_running(rb);
+
+			bfa_ioc_ct2_nfc_clk_reset(rb);
+		} else {
+			bfa_ioc_ct2_nfc_halt(rb);
+
+			bfa_ioc_ct2_clk_reset(rb);
+			bfa_ioc_ct2_mac_reset(rb);
+			bfa_ioc_ct2_clk_reset(rb);
+
+		}
 	}
 
 	/*
 	 * Mask the interrupts and clear any
-	 * pending interrupts.
+	 * pending interrupts left by BIOS/EFI
 	 */
+
 	writel(1, (rb + CT2_LPU0_HOSTFN_MBOX0_MSK));
 	writel(1, (rb + CT2_LPU1_HOSTFN_MBOX0_MSK));
 
 	/* For first time initialization, no need to clear interrupts */
 	r32 = readl(rb + HOST_SEM5_REG);
 	if (r32 & 0x1) {
-		r32 = readl(rb + CT2_LPU0_HOSTFN_CMD_STAT);
+		r32 = readl((rb + CT2_LPU0_HOSTFN_CMD_STAT));
 		if (r32 == 1) {
-			writel(1, rb + CT2_LPU0_HOSTFN_CMD_STAT);
+			writel(1, (rb + CT2_LPU0_HOSTFN_CMD_STAT));
 			readl((rb + CT2_LPU0_HOSTFN_CMD_STAT));
 		}
-		r32 = readl(rb + CT2_LPU1_HOSTFN_CMD_STAT);
+		r32 = readl((rb + CT2_LPU1_HOSTFN_CMD_STAT));
 		if (r32 == 1) {
-			writel(1, rb + CT2_LPU1_HOSTFN_CMD_STAT);
-			readl(rb + CT2_LPU1_HOSTFN_CMD_STAT);
+			writel(1, (rb + CT2_LPU1_HOSTFN_CMD_STAT));
+			readl((rb + CT2_LPU1_HOSTFN_CMD_STAT));
 		}
 	}
 
 	bfa_ioc_ct2_mem_init(rb);
 
-	writel(BFI_IOC_UNINIT, rb + CT2_BFA_IOC0_STATE_REG);
-	writel(BFI_IOC_UNINIT, rb + CT2_BFA_IOC1_STATE_REG);
+	writel(BFI_IOC_UNINIT, (rb + CT2_BFA_IOC0_STATE_REG));
+	writel(BFI_IOC_UNINIT, (rb + CT2_BFA_IOC1_STATE_REG));
 
 	return BFA_STATUS_OK;
 }
diff --git a/drivers/scsi/bfa/bfa_modules.h b/drivers/scsi/bfa/bfa_modules.h
index 189fff71e3c..a14c784ff3f 100644
--- a/drivers/scsi/bfa/bfa_modules.h
+++ b/drivers/scsi/bfa/bfa_modules.h
@@ -45,6 +45,7 @@ struct bfa_modules_s {
 	struct bfa_diag_s	diag_mod;	/*  diagnostics module	*/
 	struct bfa_phy_s	phy;		/*  phy module		*/
 	struct bfa_dconf_mod_s	dconf_mod;	/*  DCONF common module	*/
+	struct bfa_fru_s	fru;		/*  fru module		*/
 };
 
 /*
diff --git a/drivers/scsi/bfa/bfa_port.c b/drivers/scsi/bfa/bfa_port.c
index 95e4ad8759a..8ea7697deb9 100644
--- a/drivers/scsi/bfa/bfa_port.c
+++ b/drivers/scsi/bfa/bfa_port.c
@@ -250,6 +250,12 @@ bfa_port_enable(struct bfa_port_s *port, bfa_port_endis_cbfn_t cbfn,
 		return BFA_STATUS_IOC_FAILURE;
 	}
 
+	/* if port is d-port enabled, return error */
+	if (port->dport_enabled) {
+		bfa_trc(port, BFA_STATUS_DPORT_ERR);
+		return BFA_STATUS_DPORT_ERR;
+	}
+
 	if (port->endis_pending) {
 		bfa_trc(port, BFA_STATUS_DEVBUSY);
 		return BFA_STATUS_DEVBUSY;
@@ -300,6 +306,12 @@ bfa_port_disable(struct bfa_port_s *port, bfa_port_endis_cbfn_t cbfn,
 		return BFA_STATUS_IOC_FAILURE;
 	}
 
+	/* if port is d-port enabled, return error */
+	if (port->dport_enabled) {
+		bfa_trc(port, BFA_STATUS_DPORT_ERR);
+		return BFA_STATUS_DPORT_ERR;
+	}
+
 	if (port->endis_pending) {
 		bfa_trc(port, BFA_STATUS_DEVBUSY);
 		return BFA_STATUS_DEVBUSY;
@@ -431,6 +443,10 @@ bfa_port_notify(void *arg, enum bfa_ioc_event_e event)
 			port->endis_cbfn = NULL;
 			port->endis_pending = BFA_FALSE;
 		}
+
+		/* clear D-port mode */
+		if (port->dport_enabled)
+			bfa_port_set_dportenabled(port, BFA_FALSE);
 		break;
 	default:
 		break;
@@ -467,6 +483,7 @@ bfa_port_attach(struct bfa_port_s *port, struct bfa_ioc_s *ioc,
 	port->stats_cbfn = NULL;
 	port->endis_cbfn = NULL;
 	port->pbc_disabled = BFA_FALSE;
+	port->dport_enabled = BFA_FALSE;
 
 	bfa_ioc_mbox_regisr(port->ioc, BFI_MC_PORT, bfa_port_isr, port);
 	bfa_q_qe_init(&port->ioc_notify);
@@ -483,6 +500,21 @@ bfa_port_attach(struct bfa_port_s *port, struct bfa_ioc_s *ioc,
 }
 
 /*
+ * bfa_port_set_dportenabled();
+ *
+ * Port module- set pbc disabled flag
+ *
+ * @param[in] port - Pointer to the Port module data structure
+ *
+ * @return void
+ */
+void
+bfa_port_set_dportenabled(struct bfa_port_s *port, bfa_boolean_t enabled)
+{
+	port->dport_enabled = enabled;
+}
+
+/*
  *	CEE module specific definitions
  */
 
diff --git a/drivers/scsi/bfa/bfa_port.h b/drivers/scsi/bfa/bfa_port.h
index 947f897328d..2fcab6bc628 100644
--- a/drivers/scsi/bfa/bfa_port.h
+++ b/drivers/scsi/bfa/bfa_port.h
@@ -45,6 +45,7 @@ struct bfa_port_s {
 	bfa_status_t			endis_status;
 	struct bfa_ioc_notify_s		ioc_notify;
 	bfa_boolean_t			pbc_disabled;
+	bfa_boolean_t			dport_enabled;
 	struct bfa_mem_dma_s		port_dma;
 };
 
@@ -66,6 +67,8 @@ bfa_status_t bfa_port_disable(struct bfa_port_s *port,
 u32     bfa_port_meminfo(void);
 void	     bfa_port_mem_claim(struct bfa_port_s *port,
 				 u8 *dma_kva, u64 dma_pa);
+void	bfa_port_set_dportenabled(struct bfa_port_s *port,
+				  bfa_boolean_t enabled);
 
 /*
  * CEE declaration
diff --git a/drivers/scsi/bfa/bfa_svc.c b/drivers/scsi/bfa/bfa_svc.c
index b2538d60db3..299c1c889b3 100644
--- a/drivers/scsi/bfa/bfa_svc.c
+++ b/drivers/scsi/bfa/bfa_svc.c
@@ -67,6 +67,9 @@ enum bfa_fcport_sm_event {
 	BFA_FCPORT_SM_LINKDOWN	= 7,	/*  firmware linkup down	*/
 	BFA_FCPORT_SM_QRESUME	= 8,	/*  CQ space available	*/
 	BFA_FCPORT_SM_HWFAIL	= 9,	/*  IOC h/w failure		*/
+	BFA_FCPORT_SM_DPORTENABLE = 10, /*  enable dport      */
+	BFA_FCPORT_SM_DPORTDISABLE = 11,/*  disable dport     */
+	BFA_FCPORT_SM_FAA_MISCONFIG = 12,	/* FAA misconfiguratin */
 };
 
 /*
@@ -197,6 +200,10 @@ static void     bfa_fcport_sm_iocdown(struct bfa_fcport_s *fcport,
 					enum bfa_fcport_sm_event event);
 static void     bfa_fcport_sm_iocfail(struct bfa_fcport_s *fcport,
 					enum bfa_fcport_sm_event event);
+static void	bfa_fcport_sm_dport(struct bfa_fcport_s *fcport,
+					enum bfa_fcport_sm_event event);
+static void	bfa_fcport_sm_faa_misconfig(struct bfa_fcport_s *fcport,
+					enum bfa_fcport_sm_event event);
 
 static void     bfa_fcport_ln_sm_dn(struct bfa_fcport_ln_s *ln,
 					enum bfa_fcport_ln_sm_event event);
@@ -226,6 +233,8 @@ static struct bfa_sm_table_s hal_port_sm_table[] = {
 	{BFA_SM(bfa_fcport_sm_stopped), BFA_PORT_ST_STOPPED},
 	{BFA_SM(bfa_fcport_sm_iocdown), BFA_PORT_ST_IOCDOWN},
 	{BFA_SM(bfa_fcport_sm_iocfail), BFA_PORT_ST_IOCDOWN},
+	{BFA_SM(bfa_fcport_sm_dport), BFA_PORT_ST_DPORT},
+	{BFA_SM(bfa_fcport_sm_faa_misconfig), BFA_PORT_ST_FAA_MISCONFIG},
 };
 
 
@@ -1244,6 +1253,12 @@ bfa_lps_sm_init(struct bfa_lps_s *lps, enum bfa_lps_event event)
 		 * Just ignore
 		 */
 		break;
+	case BFA_LPS_SM_SET_N2N_PID:
+		/*
+		 * When topology is set to loop, bfa_lps_set_n2n_pid() sends
+		 * this event. Ignore this event.
+		 */
+		break;
 
 	default:
 		bfa_sm_fault(lps->bfa, event);
@@ -1261,6 +1276,7 @@ bfa_lps_sm_login(struct bfa_lps_s *lps, enum bfa_lps_event event)
 
 	switch (event) {
 	case BFA_LPS_SM_FWRSP:
+	case BFA_LPS_SM_OFFLINE:
 		if (lps->status == BFA_STATUS_OK) {
 			bfa_sm_set_state(lps, bfa_lps_sm_online);
 			if (lps->fdisc)
@@ -1289,7 +1305,6 @@ bfa_lps_sm_login(struct bfa_lps_s *lps, enum bfa_lps_event event)
 		bfa_lps_login_comp(lps);
 		break;
 
-	case BFA_LPS_SM_OFFLINE:
 	case BFA_LPS_SM_DELETE:
 		bfa_sm_set_state(lps, bfa_lps_sm_init);
 		break;
@@ -2169,6 +2184,12 @@ bfa_fcport_sm_enabling_qwait(struct bfa_fcport_s *fcport,
 		bfa_sm_set_state(fcport, bfa_fcport_sm_iocdown);
 		break;
 
+	case BFA_FCPORT_SM_FAA_MISCONFIG:
+		bfa_fcport_reset_linkinfo(fcport);
+		bfa_fcport_aen_post(fcport, BFA_PORT_AEN_DISCONNECT);
+		bfa_sm_set_state(fcport, bfa_fcport_sm_faa_misconfig);
+		break;
+
 	default:
 		bfa_sm_fault(fcport->bfa, event);
 	}
@@ -2225,6 +2246,12 @@ bfa_fcport_sm_enabling(struct bfa_fcport_s *fcport,
 		bfa_sm_set_state(fcport, bfa_fcport_sm_iocdown);
 		break;
 
+	case BFA_FCPORT_SM_FAA_MISCONFIG:
+		bfa_fcport_reset_linkinfo(fcport);
+		bfa_fcport_aen_post(fcport, BFA_PORT_AEN_DISCONNECT);
+		bfa_sm_set_state(fcport, bfa_fcport_sm_faa_misconfig);
+		break;
+
 	default:
 		bfa_sm_fault(fcport->bfa, event);
 	}
@@ -2250,11 +2277,11 @@ bfa_fcport_sm_linkdown(struct bfa_fcport_s *fcport,
 		if (!bfa_ioc_get_fcmode(&fcport->bfa->ioc)) {
 
 			bfa_trc(fcport->bfa,
-				pevent->link_state.vc_fcf.fcf.fipenabled);
+				pevent->link_state.attr.vc_fcf.fcf.fipenabled);
 			bfa_trc(fcport->bfa,
-				pevent->link_state.vc_fcf.fcf.fipfailed);
+				pevent->link_state.attr.vc_fcf.fcf.fipfailed);
 
-			if (pevent->link_state.vc_fcf.fcf.fipfailed)
+			if (pevent->link_state.attr.vc_fcf.fcf.fipfailed)
 				bfa_plog_str(fcport->bfa->plog, BFA_PL_MID_HAL,
 					BFA_PL_EID_FIP_FCF_DISC, 0,
 					"FIP FCF Discovery Failed");
@@ -2311,6 +2338,12 @@ bfa_fcport_sm_linkdown(struct bfa_fcport_s *fcport,
 		bfa_sm_set_state(fcport, bfa_fcport_sm_iocdown);
 		break;
 
+	case BFA_FCPORT_SM_FAA_MISCONFIG:
+		bfa_fcport_reset_linkinfo(fcport);
+		bfa_fcport_aen_post(fcport, BFA_PORT_AEN_DISCONNECT);
+		bfa_sm_set_state(fcport, bfa_fcport_sm_faa_misconfig);
+		break;
+
 	default:
 		bfa_sm_fault(fcport->bfa, event);
 	}
@@ -2404,6 +2437,12 @@ bfa_fcport_sm_linkup(struct bfa_fcport_s *fcport,
 		}
 		break;
 
+	case BFA_FCPORT_SM_FAA_MISCONFIG:
+		bfa_fcport_reset_linkinfo(fcport);
+		bfa_fcport_aen_post(fcport, BFA_PORT_AEN_DISCONNECT);
+		bfa_sm_set_state(fcport, bfa_fcport_sm_faa_misconfig);
+		break;
+
 	default:
 		bfa_sm_fault(fcport->bfa, event);
 	}
@@ -2449,6 +2488,12 @@ bfa_fcport_sm_disabling_qwait(struct bfa_fcport_s *fcport,
 		bfa_reqq_wcancel(&fcport->reqq_wait);
 		break;
 
+	case BFA_FCPORT_SM_FAA_MISCONFIG:
+		bfa_fcport_reset_linkinfo(fcport);
+		bfa_fcport_aen_post(fcport, BFA_PORT_AEN_DISCONNECT);
+		bfa_sm_set_state(fcport, bfa_fcport_sm_faa_misconfig);
+		break;
+
 	default:
 		bfa_sm_fault(fcport->bfa, event);
 	}
@@ -2600,6 +2645,10 @@ bfa_fcport_sm_disabled(struct bfa_fcport_s *fcport,
 		bfa_sm_set_state(fcport, bfa_fcport_sm_iocfail);
 		break;
 
+	case BFA_FCPORT_SM_DPORTENABLE:
+		bfa_sm_set_state(fcport, bfa_fcport_sm_dport);
+		break;
+
 	default:
 		bfa_sm_fault(fcport->bfa, event);
 	}
@@ -2680,6 +2729,81 @@ bfa_fcport_sm_iocfail(struct bfa_fcport_s *fcport,
 	}
 }
 
+static void
+bfa_fcport_sm_dport(struct bfa_fcport_s *fcport, enum bfa_fcport_sm_event event)
+{
+	bfa_trc(fcport->bfa, event);
+
+	switch (event) {
+	case BFA_FCPORT_SM_DPORTENABLE:
+	case BFA_FCPORT_SM_DISABLE:
+	case BFA_FCPORT_SM_ENABLE:
+	case BFA_FCPORT_SM_START:
+		/*
+		 * Ignore event for a port that is dport
+		 */
+		break;
+
+	case BFA_FCPORT_SM_STOP:
+		bfa_sm_set_state(fcport, bfa_fcport_sm_stopped);
+		break;
+
+	case BFA_FCPORT_SM_HWFAIL:
+		bfa_sm_set_state(fcport, bfa_fcport_sm_iocfail);
+		break;
+
+	case BFA_FCPORT_SM_DPORTDISABLE:
+		bfa_sm_set_state(fcport, bfa_fcport_sm_disabled);
+		break;
+
+	default:
+		bfa_sm_fault(fcport->bfa, event);
+	}
+}
+
+static void
+bfa_fcport_sm_faa_misconfig(struct bfa_fcport_s *fcport,
+			    enum bfa_fcport_sm_event event)
+{
+	bfa_trc(fcport->bfa, event);
+
+	switch (event) {
+	case BFA_FCPORT_SM_DPORTENABLE:
+	case BFA_FCPORT_SM_ENABLE:
+	case BFA_FCPORT_SM_START:
+		/*
+		 * Ignore event for a port as there is FAA misconfig
+		 */
+		break;
+
+	case BFA_FCPORT_SM_DISABLE:
+		if (bfa_fcport_send_disable(fcport))
+			bfa_sm_set_state(fcport, bfa_fcport_sm_disabling);
+		else
+			bfa_sm_set_state(fcport, bfa_fcport_sm_disabling_qwait);
+
+		bfa_fcport_reset_linkinfo(fcport);
+		bfa_fcport_scn(fcport, BFA_PORT_LINKDOWN, BFA_FALSE);
+		bfa_plog_str(fcport->bfa->plog, BFA_PL_MID_HAL,
+			     BFA_PL_EID_PORT_DISABLE, 0, "Port Disable");
+		bfa_fcport_aen_post(fcport, BFA_PORT_AEN_DISABLE);
+		break;
+
+	case BFA_FCPORT_SM_STOP:
+		bfa_sm_set_state(fcport, bfa_fcport_sm_stopped);
+		break;
+
+	case BFA_FCPORT_SM_HWFAIL:
+		bfa_fcport_reset_linkinfo(fcport);
+		bfa_fcport_scn(fcport, BFA_PORT_LINKDOWN, BFA_FALSE);
+		bfa_sm_set_state(fcport, bfa_fcport_sm_iocdown);
+		break;
+
+	default:
+		bfa_sm_fault(fcport->bfa, event);
+	}
+}
+
 /*
  * Link state is down
  */
@@ -2943,6 +3067,7 @@ bfa_fcport_attach(struct bfa_s *bfa, void *bfad, struct bfa_iocfc_cfg_s *cfg,
 	 */
 	do_gettimeofday(&tv);
 	fcport->stats_reset_time = tv.tv_sec;
+	fcport->stats_dma_ready = BFA_FALSE;
 
 	/*
 	 * initialize and set default configuration
@@ -2953,6 +3078,9 @@ bfa_fcport_attach(struct bfa_s *bfa, void *bfad, struct bfa_iocfc_cfg_s *cfg,
 	port_cfg->maxfrsize = 0;
 
 	port_cfg->trl_def_speed = BFA_PORT_SPEED_1GBPS;
+	port_cfg->qos_bw.high = BFA_QOS_BW_HIGH;
+	port_cfg->qos_bw.med = BFA_QOS_BW_MED;
+	port_cfg->qos_bw.low = BFA_QOS_BW_LOW;
 
 	INIT_LIST_HEAD(&fcport->stats_pending_q);
 	INIT_LIST_HEAD(&fcport->statsclr_pending_q);
@@ -2996,6 +3124,21 @@ bfa_fcport_iocdisable(struct bfa_s *bfa)
 	bfa_trunk_iocdisable(bfa);
 }
 
+/*
+ * Update loop info in fcport for SCN online
+ */
+static void
+bfa_fcport_update_loop_info(struct bfa_fcport_s *fcport,
+			struct bfa_fcport_loop_info_s *loop_info)
+{
+	fcport->myalpa = loop_info->myalpa;
+	fcport->alpabm_valid =
+			loop_info->alpabm_val;
+	memcpy(fcport->alpabm.alpa_bm,
+			loop_info->alpabm.alpa_bm,
+			sizeof(struct fc_alpabm_s));
+}
+
 static void
 bfa_fcport_update_linkinfo(struct bfa_fcport_s *fcport)
 {
@@ -3005,12 +3148,15 @@ bfa_fcport_update_linkinfo(struct bfa_fcport_s *fcport)
 	fcport->speed = pevent->link_state.speed;
 	fcport->topology = pevent->link_state.topology;
 
-	if (fcport->topology == BFA_PORT_TOPOLOGY_LOOP)
-		fcport->myalpa = 0;
+	if (fcport->topology == BFA_PORT_TOPOLOGY_LOOP) {
+		bfa_fcport_update_loop_info(fcport,
+				&pevent->link_state.attr.loop_info);
+		return;
+	}
 
 	/* QoS Details */
 	fcport->qos_attr = pevent->link_state.qos_attr;
-	fcport->qos_vc_attr = pevent->link_state.vc_fcf.qos_vc_attr;
+	fcport->qos_vc_attr = pevent->link_state.attr.vc_fcf.qos_vc_attr;
 
 	/*
 	 * update trunk state if applicable
@@ -3019,7 +3165,8 @@ bfa_fcport_update_linkinfo(struct bfa_fcport_s *fcport)
 		trunk->attr.state = BFA_TRUNK_DISABLED;
 
 	/* update FCoE specific */
-	fcport->fcoe_vlan = be16_to_cpu(pevent->link_state.vc_fcf.fcf.vlan);
+	fcport->fcoe_vlan =
+		be16_to_cpu(pevent->link_state.attr.vc_fcf.fcf.vlan);
 
 	bfa_trc(fcport->bfa, fcport->speed);
 	bfa_trc(fcport->bfa, fcport->topology);
@@ -3453,6 +3600,7 @@ bfa_fcport_isr(struct bfa_s *bfa, struct bfi_msg_s *msg)
 	case BFI_FCPORT_I2H_ENABLE_RSP:
 		if (fcport->msgtag == i2hmsg.penable_rsp->msgtag) {
 
+			fcport->stats_dma_ready = BFA_TRUE;
 			if (fcport->use_flash_cfg) {
 				fcport->cfg = i2hmsg.penable_rsp->port_cfg;
 				fcport->cfg.maxfrsize =
@@ -3468,6 +3616,8 @@ bfa_fcport_isr(struct bfa_s *bfa, struct bfi_msg_s *msg)
 				else
 					fcport->trunk.attr.state =
 						BFA_TRUNK_DISABLED;
+				fcport->qos_attr.qos_bw =
+					i2hmsg.penable_rsp->port_cfg.qos_bw;
 				fcport->use_flash_cfg = BFA_FALSE;
 			}
 
@@ -3476,6 +3626,9 @@ bfa_fcport_isr(struct bfa_s *bfa, struct bfi_msg_s *msg)
 			else
 				fcport->qos_attr.state = BFA_QOS_DISABLED;
 
+			fcport->qos_attr.qos_bw_op =
+					i2hmsg.penable_rsp->port_cfg.qos_bw;
+
 			bfa_sm_send_event(fcport, BFA_FCPORT_SM_FWRSP);
 		}
 		break;
@@ -3488,8 +3641,17 @@ bfa_fcport_isr(struct bfa_s *bfa, struct bfi_msg_s *msg)
 	case BFI_FCPORT_I2H_EVENT:
 		if (i2hmsg.event->link_state.linkstate == BFA_PORT_LINKUP)
 			bfa_sm_send_event(fcport, BFA_FCPORT_SM_LINKUP);
-		else
-			bfa_sm_send_event(fcport, BFA_FCPORT_SM_LINKDOWN);
+		else {
+			if (i2hmsg.event->link_state.linkstate_rsn ==
+			    BFA_PORT_LINKSTATE_RSN_FAA_MISCONFIG)
+				bfa_sm_send_event(fcport,
+						  BFA_FCPORT_SM_FAA_MISCONFIG);
+			else
+				bfa_sm_send_event(fcport,
+						  BFA_FCPORT_SM_LINKDOWN);
+		}
+		fcport->qos_attr.qos_bw_op =
+				i2hmsg.event->link_state.qos_attr.qos_bw_op;
 		break;
 
 	case BFI_FCPORT_I2H_TRUNK_SCN:
@@ -3609,6 +3771,9 @@ bfa_fcport_cfg_speed(struct bfa_s *bfa, enum bfa_port_speed speed)
 
 	if (fcport->cfg.trunked == BFA_TRUE)
 		return BFA_STATUS_TRUNK_ENABLED;
+	if ((fcport->cfg.topology == BFA_PORT_TOPOLOGY_LOOP) &&
+			(speed == BFA_PORT_SPEED_16GBPS))
+		return BFA_STATUS_UNSUPP_SPEED;
 	if ((speed != BFA_PORT_SPEED_AUTO) && (speed > fcport->speed_sup)) {
 		bfa_trc(bfa, fcport->speed_sup);
 		return BFA_STATUS_UNSUPP_SPEED;
@@ -3663,7 +3828,26 @@ bfa_fcport_cfg_topology(struct bfa_s *bfa, enum bfa_port_topology topology)
 
 	switch (topology) {
 	case BFA_PORT_TOPOLOGY_P2P:
+		break;
+
 	case BFA_PORT_TOPOLOGY_LOOP:
+		if ((bfa_fcport_is_qos_enabled(bfa) != BFA_FALSE) ||
+			(fcport->qos_attr.state != BFA_QOS_DISABLED))
+			return BFA_STATUS_ERROR_QOS_ENABLED;
+		if (fcport->cfg.ratelimit != BFA_FALSE)
+			return BFA_STATUS_ERROR_TRL_ENABLED;
+		if ((bfa_fcport_is_trunk_enabled(bfa) != BFA_FALSE) ||
+			(fcport->trunk.attr.state != BFA_TRUNK_DISABLED))
+			return BFA_STATUS_ERROR_TRUNK_ENABLED;
+		if ((bfa_fcport_get_speed(bfa) == BFA_PORT_SPEED_16GBPS) ||
+			(fcport->cfg.speed == BFA_PORT_SPEED_16GBPS))
+			return BFA_STATUS_UNSUPP_SPEED;
+		if (bfa_mfg_is_mezz(bfa->ioc.attr->card_type))
+			return BFA_STATUS_LOOP_UNSUPP_MEZZ;
+		if (bfa_fcport_is_dport(bfa) != BFA_FALSE)
+			return BFA_STATUS_DPORT_ERR;
+		break;
+
 	case BFA_PORT_TOPOLOGY_AUTO:
 		break;
 
@@ -3686,6 +3870,17 @@ bfa_fcport_get_topology(struct bfa_s *bfa)
 	return fcport->topology;
 }
 
+/**
+ * Get config topology.
+ */
+enum bfa_port_topology
+bfa_fcport_get_cfg_topology(struct bfa_s *bfa)
+{
+	struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(bfa);
+
+	return fcport->cfg.topology;
+}
+
 bfa_status_t
 bfa_fcport_cfg_hardalpa(struct bfa_s *bfa, u8 alpa)
 {
@@ -3761,9 +3956,11 @@ bfa_fcport_get_maxfrsize(struct bfa_s *bfa)
 u8
 bfa_fcport_get_rx_bbcredit(struct bfa_s *bfa)
 {
-	struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(bfa);
+	if (bfa_fcport_get_topology(bfa) != BFA_PORT_TOPOLOGY_LOOP)
+		return (BFA_FCPORT_MOD(bfa))->cfg.rx_bbcredit;
 
-	return fcport->cfg.rx_bbcredit;
+	else
+		return 0;
 }
 
 void
@@ -3850,8 +4047,9 @@ bfa_fcport_get_stats(struct bfa_s *bfa, struct bfa_cb_pending_q_s *cb)
 {
 	struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(bfa);
 
-	if (bfa_ioc_is_disabled(&bfa->ioc))
-		return BFA_STATUS_IOC_DISABLED;
+	if (!bfa_iocfc_is_operational(bfa) ||
+	    !fcport->stats_dma_ready)
+		return BFA_STATUS_IOC_NON_OP;
 
 	if (!list_empty(&fcport->statsclr_pending_q))
 		return BFA_STATUS_DEVBUSY;
@@ -3876,6 +4074,10 @@ bfa_fcport_clear_stats(struct bfa_s *bfa, struct bfa_cb_pending_q_s *cb)
 {
 	struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(bfa);
 
+	if (!bfa_iocfc_is_operational(bfa) ||
+	    !fcport->stats_dma_ready)
+		return BFA_STATUS_IOC_NON_OP;
+
 	if (!list_empty(&fcport->stats_pending_q))
 		return BFA_STATUS_DEVBUSY;
 
@@ -3905,6 +4107,40 @@ bfa_fcport_is_disabled(struct bfa_s *bfa)
 }
 
 bfa_boolean_t
+bfa_fcport_is_dport(struct bfa_s *bfa)
+{
+	struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(bfa);
+
+	return (bfa_sm_to_state(hal_port_sm_table, fcport->sm) ==
+		BFA_PORT_ST_DPORT);
+}
+
+bfa_status_t
+bfa_fcport_set_qos_bw(struct bfa_s *bfa, struct bfa_qos_bw_s *qos_bw)
+{
+	struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(bfa);
+	enum bfa_ioc_type_e ioc_type = bfa_get_type(bfa);
+
+	bfa_trc(bfa, ioc_type);
+
+	if ((qos_bw->high == 0) || (qos_bw->med == 0) || (qos_bw->low == 0))
+		return BFA_STATUS_QOS_BW_INVALID;
+
+	if ((qos_bw->high + qos_bw->med + qos_bw->low) != 100)
+		return BFA_STATUS_QOS_BW_INVALID;
+
+	if ((qos_bw->med > qos_bw->high) || (qos_bw->low > qos_bw->med) ||
+	    (qos_bw->low > qos_bw->high))
+		return BFA_STATUS_QOS_BW_INVALID;
+
+	if ((ioc_type == BFA_IOC_TYPE_FC) &&
+	    (fcport->cfg.topology != BFA_PORT_TOPOLOGY_LOOP))
+		fcport->cfg.qos_bw = *qos_bw;
+
+	return BFA_STATUS_OK;
+}
+
+bfa_boolean_t
 bfa_fcport_is_ratelim(struct bfa_s *bfa)
 {
 	struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(bfa);
@@ -3981,6 +4217,26 @@ bfa_fcport_is_trunk_enabled(struct bfa_s *bfa)
 	return fcport->cfg.trunked;
 }
 
+void
+bfa_fcport_dportenable(struct bfa_s *bfa)
+{
+	/*
+	 * Assume caller check for port is in disable state
+	 */
+	bfa_sm_send_event(BFA_FCPORT_MOD(bfa), BFA_FCPORT_SM_DPORTENABLE);
+	bfa_port_set_dportenabled(&bfa->modules.port, BFA_TRUE);
+}
+
+void
+bfa_fcport_dportdisable(struct bfa_s *bfa)
+{
+	/*
+	 * Assume caller check for port is in disable state
+	 */
+	bfa_sm_send_event(BFA_FCPORT_MOD(bfa), BFA_FCPORT_SM_DPORTDISABLE);
+	bfa_port_set_dportenabled(&bfa->modules.port, BFA_FALSE);
+}
+
 /*
  * Rport State machine functions
  */
@@ -4707,6 +4963,21 @@ bfa_rport_isr(struct bfa_s *bfa, struct bfi_msg_s *m)
 		bfa_sm_send_event(rp, BFA_RPORT_SM_QOS_SCN);
 		break;
 
+	case BFI_RPORT_I2H_LIP_SCN_ONLINE:
+		bfa_fcport_update_loop_info(BFA_FCPORT_MOD(bfa),
+				&msg.lip_scn->loop_info);
+		bfa_cb_rport_scn_online(bfa);
+		break;
+
+	case BFI_RPORT_I2H_LIP_SCN_OFFLINE:
+		bfa_cb_rport_scn_offline(bfa);
+		break;
+
+	case BFI_RPORT_I2H_NO_DEV:
+		rp = BFA_RPORT_FROM_TAG(bfa, msg.lip_scn->bfa_handle);
+		bfa_cb_rport_scn_no_dev(rp->rport_drv);
+		break;
+
 	default:
 		bfa_trc(bfa, m->mhdr.msg_id);
 		WARN_ON(1);
@@ -5348,6 +5619,37 @@ bfa_uf_res_recfg(struct bfa_s *bfa, u16 num_uf_fw)
 }
 
 /*
+ *	Dport forward declaration
+ */
+
+/*
+ * BFA DPORT state machine events
+ */
+enum bfa_dport_sm_event {
+	BFA_DPORT_SM_ENABLE	= 1,	/* dport enable event         */
+	BFA_DPORT_SM_DISABLE    = 2,    /* dport disable event        */
+	BFA_DPORT_SM_FWRSP      = 3,    /* fw enable/disable rsp      */
+	BFA_DPORT_SM_QRESUME    = 4,    /* CQ space available         */
+	BFA_DPORT_SM_HWFAIL     = 5,    /* IOC h/w failure            */
+};
+
+static void bfa_dport_sm_disabled(struct bfa_dport_s *dport,
+				  enum bfa_dport_sm_event event);
+static void bfa_dport_sm_enabling_qwait(struct bfa_dport_s *dport,
+				  enum bfa_dport_sm_event event);
+static void bfa_dport_sm_enabling(struct bfa_dport_s *dport,
+				  enum bfa_dport_sm_event event);
+static void bfa_dport_sm_enabled(struct bfa_dport_s *dport,
+				 enum bfa_dport_sm_event event);
+static void bfa_dport_sm_disabling_qwait(struct bfa_dport_s *dport,
+				 enum bfa_dport_sm_event event);
+static void bfa_dport_sm_disabling(struct bfa_dport_s *dport,
+				   enum bfa_dport_sm_event event);
+static void bfa_dport_qresume(void *cbarg);
+static void bfa_dport_req_comp(struct bfa_dport_s *dport,
+			       bfi_diag_dport_rsp_t *msg);
+
+/*
  *	BFA fcdiag module
  */
 #define BFA_DIAG_QTEST_TOV	1000    /* msec */
@@ -5377,15 +5679,24 @@ bfa_fcdiag_attach(struct bfa_s *bfa, void *bfad, struct bfa_iocfc_cfg_s *cfg,
 		struct bfa_pcidev_s *pcidev)
 {
 	struct bfa_fcdiag_s *fcdiag = BFA_FCDIAG_MOD(bfa);
+	struct bfa_dport_s  *dport = &fcdiag->dport;
+
 	fcdiag->bfa             = bfa;
 	fcdiag->trcmod  = bfa->trcmod;
 	/* The common DIAG attach bfa_diag_attach() will do all memory claim */
+	dport->bfa = bfa;
+	bfa_sm_set_state(dport, bfa_dport_sm_disabled);
+	bfa_reqq_winit(&dport->reqq_wait, bfa_dport_qresume, dport);
+	dport->cbfn = NULL;
+	dport->cbarg = NULL;
 }
 
 static void
 bfa_fcdiag_iocdisable(struct bfa_s *bfa)
 {
 	struct bfa_fcdiag_s *fcdiag = BFA_FCDIAG_MOD(bfa);
+	struct bfa_dport_s *dport = &fcdiag->dport;
+
 	bfa_trc(fcdiag, fcdiag->lb.lock);
 	if (fcdiag->lb.lock) {
 		fcdiag->lb.status = BFA_STATUS_IOC_FAILURE;
@@ -5393,6 +5704,8 @@ bfa_fcdiag_iocdisable(struct bfa_s *bfa)
 		fcdiag->lb.lock = 0;
 		bfa_fcdiag_set_busy_status(fcdiag);
 	}
+
+	bfa_sm_send_event(dport, BFA_DPORT_SM_HWFAIL);
 }
 
 static void
@@ -5577,6 +5890,9 @@ bfa_fcdiag_intr(struct bfa_s *bfa, struct bfi_msg_s *msg)
 	case BFI_DIAG_I2H_QTEST:
 		bfa_fcdiag_queuetest_comp(fcdiag, (bfi_diag_qtest_rsp_t *)msg);
 		break;
+	case BFI_DIAG_I2H_DPORT:
+		bfa_dport_req_comp(&fcdiag->dport, (bfi_diag_dport_rsp_t *)msg);
+		break;
 	default:
 		bfa_trc(fcdiag, msg->mhdr.msg_id);
 		WARN_ON(1);
@@ -5646,12 +5962,18 @@ bfa_fcdiag_loopback(struct bfa_s *bfa, enum bfa_port_opmode opmode,
 		}
 	}
 
+	/*
+	 * For CT2, 1G is not supported
+	 */
+	if ((speed == BFA_PORT_SPEED_1GBPS) &&
+	    (bfa_asic_id_ct2(bfa->ioc.pcidev.device_id))) {
+		bfa_trc(fcdiag, speed);
+		return BFA_STATUS_UNSUPP_SPEED;
+	}
+
 	/* For Mezz card, port speed entered needs to be checked */
 	if (bfa_mfg_is_mezz(bfa->ioc.attr->card_type)) {
 		if (bfa_ioc_get_type(&bfa->ioc) == BFA_IOC_TYPE_FC) {
-			if ((speed == BFA_PORT_SPEED_1GBPS) &&
-			    (bfa_asic_id_ct2(bfa->ioc.pcidev.device_id)))
-				return BFA_STATUS_UNSUPP_SPEED;
 			if (!(speed == BFA_PORT_SPEED_1GBPS ||
 			      speed == BFA_PORT_SPEED_2GBPS ||
 			      speed == BFA_PORT_SPEED_4GBPS ||
@@ -5764,3 +6086,379 @@ bfa_fcdiag_lb_is_running(struct bfa_s *bfa)
 	struct bfa_fcdiag_s *fcdiag = BFA_FCDIAG_MOD(bfa);
 	return fcdiag->lb.lock ?  BFA_STATUS_DIAG_BUSY : BFA_STATUS_OK;
 }
+
+/*
+ *	D-port
+ */
+static bfa_boolean_t bfa_dport_send_req(struct bfa_dport_s *dport,
+					enum bfi_dport_req req);
+static void
+bfa_cb_fcdiag_dport(struct bfa_dport_s *dport, bfa_status_t bfa_status)
+{
+	if (dport->cbfn != NULL) {
+		dport->cbfn(dport->cbarg, bfa_status);
+		dport->cbfn = NULL;
+		dport->cbarg = NULL;
+	}
+}
+
+static void
+bfa_dport_sm_disabled(struct bfa_dport_s *dport, enum bfa_dport_sm_event event)
+{
+	bfa_trc(dport->bfa, event);
+
+	switch (event) {
+	case BFA_DPORT_SM_ENABLE:
+		bfa_fcport_dportenable(dport->bfa);
+		if (bfa_dport_send_req(dport, BFI_DPORT_ENABLE))
+			bfa_sm_set_state(dport, bfa_dport_sm_enabling);
+		else
+			bfa_sm_set_state(dport, bfa_dport_sm_enabling_qwait);
+		break;
+
+	case BFA_DPORT_SM_DISABLE:
+		/* Already disabled */
+		break;
+
+	case BFA_DPORT_SM_HWFAIL:
+		/* ignore */
+		break;
+
+	default:
+		bfa_sm_fault(dport->bfa, event);
+	}
+}
+
+static void
+bfa_dport_sm_enabling_qwait(struct bfa_dport_s *dport,
+			    enum bfa_dport_sm_event event)
+{
+	bfa_trc(dport->bfa, event);
+
+	switch (event) {
+	case BFA_DPORT_SM_QRESUME:
+		bfa_sm_set_state(dport, bfa_dport_sm_enabling);
+		bfa_dport_send_req(dport, BFI_DPORT_ENABLE);
+		break;
+
+	case BFA_DPORT_SM_HWFAIL:
+		bfa_reqq_wcancel(&dport->reqq_wait);
+		bfa_sm_set_state(dport, bfa_dport_sm_disabled);
+		bfa_cb_fcdiag_dport(dport, BFA_STATUS_FAILED);
+		break;
+
+	default:
+		bfa_sm_fault(dport->bfa, event);
+	}
+}
+
+static void
+bfa_dport_sm_enabling(struct bfa_dport_s *dport, enum bfa_dport_sm_event event)
+{
+	bfa_trc(dport->bfa, event);
+
+	switch (event) {
+	case BFA_DPORT_SM_FWRSP:
+		bfa_sm_set_state(dport, bfa_dport_sm_enabled);
+		break;
+
+	case BFA_DPORT_SM_HWFAIL:
+		bfa_sm_set_state(dport, bfa_dport_sm_disabled);
+		bfa_cb_fcdiag_dport(dport, BFA_STATUS_FAILED);
+		break;
+
+	default:
+		bfa_sm_fault(dport->bfa, event);
+	}
+}
+
+static void
+bfa_dport_sm_enabled(struct bfa_dport_s *dport, enum bfa_dport_sm_event event)
+{
+	bfa_trc(dport->bfa, event);
+
+	switch (event) {
+	case BFA_DPORT_SM_ENABLE:
+		/* Already enabled */
+		break;
+
+	case BFA_DPORT_SM_DISABLE:
+		bfa_fcport_dportdisable(dport->bfa);
+		if (bfa_dport_send_req(dport, BFI_DPORT_DISABLE))
+			bfa_sm_set_state(dport, bfa_dport_sm_disabling);
+		else
+			bfa_sm_set_state(dport, bfa_dport_sm_disabling_qwait);
+		break;
+
+	case BFA_DPORT_SM_HWFAIL:
+		bfa_sm_set_state(dport, bfa_dport_sm_disabled);
+		break;
+
+	default:
+		bfa_sm_fault(dport->bfa, event);
+	}
+}
+
+static void
+bfa_dport_sm_disabling_qwait(struct bfa_dport_s *dport,
+			     enum bfa_dport_sm_event event)
+{
+	bfa_trc(dport->bfa, event);
+
+	switch (event) {
+	case BFA_DPORT_SM_QRESUME:
+		bfa_sm_set_state(dport, bfa_dport_sm_disabling);
+		bfa_dport_send_req(dport, BFI_DPORT_DISABLE);
+		break;
+
+	case BFA_DPORT_SM_HWFAIL:
+		bfa_sm_set_state(dport, bfa_dport_sm_disabled);
+		bfa_reqq_wcancel(&dport->reqq_wait);
+		bfa_cb_fcdiag_dport(dport, BFA_STATUS_OK);
+		break;
+
+	default:
+		bfa_sm_fault(dport->bfa, event);
+	}
+}
+
+static void
+bfa_dport_sm_disabling(struct bfa_dport_s *dport, enum bfa_dport_sm_event event)
+{
+	bfa_trc(dport->bfa, event);
+
+	switch (event) {
+	case BFA_DPORT_SM_FWRSP:
+		bfa_sm_set_state(dport, bfa_dport_sm_disabled);
+		break;
+
+	case BFA_DPORT_SM_HWFAIL:
+		bfa_sm_set_state(dport, bfa_dport_sm_disabled);
+		bfa_cb_fcdiag_dport(dport, BFA_STATUS_OK);
+		break;
+
+	default:
+		bfa_sm_fault(dport->bfa, event);
+	}
+}
+
+
+static bfa_boolean_t
+bfa_dport_send_req(struct bfa_dport_s *dport, enum bfi_dport_req req)
+{
+	struct bfi_diag_dport_req_s *m;
+
+	/*
+	 * Increment message tag before queue check, so that responses to old
+	 * requests are discarded.
+	 */
+	dport->msgtag++;
+
+	/*
+	 * check for room in queue to send request now
+	 */
+	m = bfa_reqq_next(dport->bfa, BFA_REQQ_DIAG);
+	if (!m) {
+		bfa_reqq_wait(dport->bfa, BFA_REQQ_PORT, &dport->reqq_wait);
+		return BFA_FALSE;
+	}
+
+	bfi_h2i_set(m->mh, BFI_MC_DIAG, BFI_DIAG_H2I_DPORT,
+		    bfa_fn_lpu(dport->bfa));
+	m->req  = req;
+	m->msgtag = dport->msgtag;
+
+	/*
+	 * queue I/O message to firmware
+	 */
+	bfa_reqq_produce(dport->bfa, BFA_REQQ_DIAG, m->mh);
+
+	return BFA_TRUE;
+}
+
+static void
+bfa_dport_qresume(void *cbarg)
+{
+	struct bfa_dport_s *dport = cbarg;
+
+	bfa_sm_send_event(dport, BFA_DPORT_SM_QRESUME);
+}
+
+static void
+bfa_dport_req_comp(struct bfa_dport_s *dport, bfi_diag_dport_rsp_t *msg)
+{
+	bfa_sm_send_event(dport, BFA_DPORT_SM_FWRSP);
+	bfa_cb_fcdiag_dport(dport, msg->status);
+}
+
+/*
+ * Dport enable
+ *
+ * @param[in] *bfa            - bfa data struct
+ */
+bfa_status_t
+bfa_dport_enable(struct bfa_s *bfa, bfa_cb_diag_t cbfn, void *cbarg)
+{
+	struct bfa_fcdiag_s *fcdiag = BFA_FCDIAG_MOD(bfa);
+	struct bfa_dport_s  *dport = &fcdiag->dport;
+
+	/*
+	 * Dport is not support in MEZZ card
+	 */
+	if (bfa_mfg_is_mezz(dport->bfa->ioc.attr->card_type)) {
+		bfa_trc(dport->bfa, BFA_STATUS_PBC);
+		return BFA_STATUS_CMD_NOTSUPP_MEZZ;
+	}
+
+	/*
+	 * Check to see if IOC is down
+	*/
+	if (!bfa_iocfc_is_operational(bfa))
+		return BFA_STATUS_IOC_NON_OP;
+
+	/* if port is PBC disabled, return error */
+	if (bfa_fcport_is_pbcdisabled(bfa)) {
+		bfa_trc(dport->bfa, BFA_STATUS_PBC);
+		return BFA_STATUS_PBC;
+	}
+
+	/*
+	 * Check if port mode is FC port
+	 */
+	if (bfa_ioc_get_type(&bfa->ioc) != BFA_IOC_TYPE_FC) {
+		bfa_trc(dport->bfa, bfa_ioc_get_type(&bfa->ioc));
+		return BFA_STATUS_CMD_NOTSUPP_CNA;
+	}
+
+	/*
+	 * Check if port is in LOOP mode
+	 */
+	if ((bfa_fcport_get_cfg_topology(bfa) == BFA_PORT_TOPOLOGY_LOOP) ||
+	    (bfa_fcport_get_topology(bfa) == BFA_PORT_TOPOLOGY_LOOP)) {
+		bfa_trc(dport->bfa, 0);
+		return BFA_STATUS_TOPOLOGY_LOOP;
+	}
+
+	/*
+	 * Check if port is TRUNK mode
+	 */
+	if (bfa_fcport_is_trunk_enabled(bfa)) {
+		bfa_trc(dport->bfa, 0);
+		return BFA_STATUS_ERROR_TRUNK_ENABLED;
+	}
+
+	/*
+	 * Check to see if port is disable or in dport state
+	 */
+	if ((bfa_fcport_is_disabled(bfa) == BFA_FALSE) &&
+	    (bfa_fcport_is_dport(bfa) == BFA_FALSE)) {
+		bfa_trc(dport->bfa, 0);
+		return BFA_STATUS_PORT_NOT_DISABLED;
+	}
+
+	/*
+	 * Check if dport is busy
+	 */
+	if (bfa_sm_cmp_state(dport, bfa_dport_sm_enabling) ||
+	    bfa_sm_cmp_state(dport, bfa_dport_sm_enabling_qwait) ||
+	    bfa_sm_cmp_state(dport, bfa_dport_sm_disabling) ||
+	    bfa_sm_cmp_state(dport, bfa_dport_sm_disabling_qwait)) {
+		return BFA_STATUS_DEVBUSY;
+	}
+
+	/*
+	 * Check if dport is already enabled
+	 */
+	if (bfa_sm_cmp_state(dport, bfa_dport_sm_enabled)) {
+		bfa_trc(dport->bfa, 0);
+		return BFA_STATUS_DPORT_ENABLED;
+	}
+
+	dport->cbfn = cbfn;
+	dport->cbarg = cbarg;
+
+	bfa_sm_send_event(dport, BFA_DPORT_SM_ENABLE);
+	return BFA_STATUS_OK;
+}
+
+/*
+ *	Dport disable
+ *
+ *	@param[in] *bfa            - bfa data struct
+ */
+bfa_status_t
+bfa_dport_disable(struct bfa_s *bfa, bfa_cb_diag_t cbfn, void *cbarg)
+{
+	struct bfa_fcdiag_s *fcdiag = BFA_FCDIAG_MOD(bfa);
+	struct bfa_dport_s *dport = &fcdiag->dport;
+
+	if (bfa_ioc_is_disabled(&bfa->ioc))
+		return BFA_STATUS_IOC_DISABLED;
+
+	/* if port is PBC disabled, return error */
+	if (bfa_fcport_is_pbcdisabled(bfa)) {
+		bfa_trc(dport->bfa, BFA_STATUS_PBC);
+		return BFA_STATUS_PBC;
+	}
+
+	/*
+	 * Check to see if port is disable or in dport state
+	 */
+	if ((bfa_fcport_is_disabled(bfa) == BFA_FALSE) &&
+	    (bfa_fcport_is_dport(bfa) == BFA_FALSE)) {
+		bfa_trc(dport->bfa, 0);
+		return BFA_STATUS_PORT_NOT_DISABLED;
+	}
+
+	/*
+	 * Check if dport is busy
+	 */
+	if (bfa_sm_cmp_state(dport, bfa_dport_sm_enabling) ||
+	    bfa_sm_cmp_state(dport, bfa_dport_sm_enabling_qwait) ||
+	    bfa_sm_cmp_state(dport, bfa_dport_sm_disabling) ||
+	    bfa_sm_cmp_state(dport, bfa_dport_sm_disabling_qwait))
+		return BFA_STATUS_DEVBUSY;
+
+	/*
+	 * Check if dport is already disabled
+	 */
+	if (bfa_sm_cmp_state(dport, bfa_dport_sm_disabled)) {
+		bfa_trc(dport->bfa, 0);
+		return BFA_STATUS_DPORT_DISABLED;
+	}
+
+	dport->cbfn = cbfn;
+	dport->cbarg = cbarg;
+
+	bfa_sm_send_event(dport, BFA_DPORT_SM_DISABLE);
+	return BFA_STATUS_OK;
+}
+
+/*
+ *	Get D-port state
+ *
+ * @param[in] *bfa            - bfa data struct
+ */
+
+bfa_status_t
+bfa_dport_get_state(struct bfa_s *bfa, enum bfa_dport_state *state)
+{
+	struct bfa_fcdiag_s *fcdiag = BFA_FCDIAG_MOD(bfa);
+	struct bfa_dport_s *dport = &fcdiag->dport;
+
+	if (bfa_sm_cmp_state(dport, bfa_dport_sm_enabled))
+		*state = BFA_DPORT_ST_ENABLED;
+	else if (bfa_sm_cmp_state(dport, bfa_dport_sm_enabling) ||
+		 bfa_sm_cmp_state(dport, bfa_dport_sm_enabling_qwait))
+		*state = BFA_DPORT_ST_ENABLING;
+	else if (bfa_sm_cmp_state(dport, bfa_dport_sm_disabled))
+		*state = BFA_DPORT_ST_DISABLED;
+	else if (bfa_sm_cmp_state(dport, bfa_dport_sm_disabling) ||
+		 bfa_sm_cmp_state(dport, bfa_dport_sm_disabling_qwait))
+		*state = BFA_DPORT_ST_DISABLING;
+	else {
+		bfa_trc(dport->bfa, BFA_STATUS_EINVAL);
+		return BFA_STATUS_EINVAL;
+	}
+	return BFA_STATUS_OK;
+}
diff --git a/drivers/scsi/bfa/bfa_svc.h b/drivers/scsi/bfa/bfa_svc.h
index 1abcf7c5166..8d7fbecfcb2 100644
--- a/drivers/scsi/bfa/bfa_svc.h
+++ b/drivers/scsi/bfa/bfa_svc.h
@@ -474,8 +474,10 @@ struct bfa_fcport_s {
 	/*  supported speeds */
 	enum bfa_port_speed speed;	/*  current speed */
 	enum bfa_port_topology topology;	/*  current topology */
-	u8			myalpa;	/*  my ALPA in LOOP topology */
 	u8			rsvd[3];
+	u8			myalpa;	/*  my ALPA in LOOP topology */
+	u8			alpabm_valid; /* alpa bitmap valid or not */
+	struct fc_alpabm_s	alpabm;	/* alpa bitmap */
 	struct bfa_port_cfg_s	cfg;	/*  current port configuration */
 	bfa_boolean_t		use_flash_cfg; /* get port cfg from flash */
 	struct bfa_qos_attr_s  qos_attr;   /* QoS Attributes */
@@ -512,6 +514,7 @@ struct bfa_fcport_s {
 	struct bfa_fcport_trunk_s trunk;
 	u16		fcoe_vlan;
 	struct bfa_mem_dma_s	fcport_dma;
+	bfa_boolean_t		stats_dma_ready;
 };
 
 #define BFA_FCPORT_MOD(__bfa)	(&(__bfa)->modules.fcport)
@@ -534,6 +537,7 @@ enum bfa_port_speed bfa_fcport_get_speed(struct bfa_s *bfa);
 bfa_status_t bfa_fcport_cfg_topology(struct bfa_s *bfa,
 				     enum bfa_port_topology topo);
 enum bfa_port_topology bfa_fcport_get_topology(struct bfa_s *bfa);
+enum bfa_port_topology bfa_fcport_get_cfg_topology(struct bfa_s *bfa);
 bfa_status_t bfa_fcport_cfg_hardalpa(struct bfa_s *bfa, u8 alpa);
 bfa_boolean_t bfa_fcport_get_hardalpa(struct bfa_s *bfa, u8 *alpa);
 u8 bfa_fcport_get_myalpa(struct bfa_s *bfa);
@@ -547,6 +551,9 @@ void bfa_fcport_event_register(struct bfa_s *bfa,
 			void (*event_cbfn) (void *cbarg,
 			enum bfa_port_linkstate event), void *event_cbarg);
 bfa_boolean_t bfa_fcport_is_disabled(struct bfa_s *bfa);
+bfa_boolean_t bfa_fcport_is_dport(struct bfa_s *bfa);
+bfa_status_t bfa_fcport_set_qos_bw(struct bfa_s *bfa,
+				   struct bfa_qos_bw_s *qos_bw);
 enum bfa_port_speed bfa_fcport_get_ratelim_speed(struct bfa_s *bfa);
 
 void bfa_fcport_set_tx_bbcredit(struct bfa_s *bfa, u16 tx_bbcredit, u8 bb_scn);
@@ -560,6 +567,8 @@ bfa_status_t bfa_fcport_clear_stats(struct bfa_s *bfa,
 			struct bfa_cb_pending_q_s *cb);
 bfa_boolean_t bfa_fcport_is_qos_enabled(struct bfa_s *bfa);
 bfa_boolean_t bfa_fcport_is_trunk_enabled(struct bfa_s *bfa);
+void bfa_fcport_dportenable(struct bfa_s *bfa);
+void bfa_fcport_dportdisable(struct bfa_s *bfa);
 bfa_status_t bfa_fcport_is_pbcdisabled(struct bfa_s *bfa);
 void bfa_fcport_cfg_faa(struct bfa_s *bfa, u8 state);
 
@@ -575,6 +584,9 @@ void bfa_cb_rport_offline(void *rport);
 void bfa_cb_rport_qos_scn_flowid(void *rport,
 				 struct bfa_rport_qos_attr_s old_qos_attr,
 				 struct bfa_rport_qos_attr_s new_qos_attr);
+void bfa_cb_rport_scn_online(struct bfa_s *bfa);
+void bfa_cb_rport_scn_offline(struct bfa_s *bfa);
+void bfa_cb_rport_scn_no_dev(void *rp);
 void bfa_cb_rport_qos_scn_prio(void *rport,
 			       struct bfa_rport_qos_attr_s old_qos_attr,
 			       struct bfa_rport_qos_attr_s new_qos_attr);
@@ -697,11 +709,21 @@ struct bfa_fcdiag_lb_s {
 	u32        status;
 };
 
+struct bfa_dport_s {
+	struct bfa_s	*bfa;		/* Back pointer to BFA	*/
+	bfa_sm_t	sm;		/* finite state machine */
+	u32		msgtag;		/* firmware msg tag for reply */
+	struct bfa_reqq_wait_s reqq_wait;
+	bfa_cb_diag_t	cbfn;
+	void		*cbarg;
+};
+
 struct bfa_fcdiag_s {
 	struct bfa_s    *bfa;           /* Back pointer to BFA */
 	struct bfa_trc_mod_s   *trcmod;
 	struct bfa_fcdiag_lb_s lb;
 	struct bfa_fcdiag_qtest_s qtest;
+	struct bfa_dport_s	dport;
 };
 
 #define BFA_FCDIAG_MOD(__bfa)	(&(__bfa)->modules.fcdiag)
@@ -717,5 +739,11 @@ bfa_status_t	bfa_fcdiag_queuetest(struct bfa_s *bfa, u32 ignore,
 			u32 queue, struct bfa_diag_qtest_result_s *result,
 			bfa_cb_diag_t cbfn, void *cbarg);
 bfa_status_t	bfa_fcdiag_lb_is_running(struct bfa_s *bfa);
+bfa_status_t	bfa_dport_enable(struct bfa_s *bfa, bfa_cb_diag_t cbfn,
+				 void *cbarg);
+bfa_status_t	bfa_dport_disable(struct bfa_s *bfa, bfa_cb_diag_t cbfn,
+				  void *cbarg);
+bfa_status_t	bfa_dport_get_state(struct bfa_s *bfa,
+				    enum bfa_dport_state *state);
 
 #endif /* __BFA_SVC_H__ */
diff --git a/drivers/scsi/bfa/bfad.c b/drivers/scsi/bfa/bfad.c
index c37494916a1..895b0e516e0 100644
--- a/drivers/scsi/bfa/bfad.c
+++ b/drivers/scsi/bfa/bfad.c
@@ -63,9 +63,9 @@ int		max_rport_logins = BFA_FCS_MAX_RPORT_LOGINS;
 u32	bfi_image_cb_size, bfi_image_ct_size, bfi_image_ct2_size;
 u32	*bfi_image_cb, *bfi_image_ct, *bfi_image_ct2;
 
-#define BFAD_FW_FILE_CB		"cbfw.bin"
-#define BFAD_FW_FILE_CT		"ctfw.bin"
-#define BFAD_FW_FILE_CT2	"ct2fw.bin"
+#define BFAD_FW_FILE_CB		"cbfw-3.1.0.0.bin"
+#define BFAD_FW_FILE_CT		"ctfw-3.1.0.0.bin"
+#define BFAD_FW_FILE_CT2	"ct2fw-3.1.0.0.bin"
 
 static u32 *bfad_load_fwimg(struct pci_dev *pdev);
 static void bfad_free_fwimg(void);
diff --git a/drivers/scsi/bfa/bfad_bsg.c b/drivers/scsi/bfa/bfad_bsg.c
index 0afa39076ce..555e7db94a1 100644
--- a/drivers/scsi/bfa/bfad_bsg.c
+++ b/drivers/scsi/bfa/bfad_bsg.c
@@ -33,7 +33,7 @@ bfad_iocmd_ioc_enable(struct bfad_s *bfad, void *cmd)
 	/* If IOC is not in disabled state - return */
 	if (!bfa_ioc_is_disabled(&bfad->bfa.ioc)) {
 		spin_unlock_irqrestore(&bfad->bfad_lock, flags);
-		iocmd->status = BFA_STATUS_IOC_FAILURE;
+		iocmd->status = BFA_STATUS_OK;
 		return rc;
 	}
 
@@ -54,6 +54,12 @@ bfad_iocmd_ioc_disable(struct bfad_s *bfad, void *cmd)
 	unsigned long	flags;
 
 	spin_lock_irqsave(&bfad->bfad_lock, flags);
+	if (bfa_ioc_is_disabled(&bfad->bfa.ioc)) {
+		spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+		iocmd->status = BFA_STATUS_OK;
+		return rc;
+	}
+
 	if (bfad->disable_active) {
 		spin_unlock_irqrestore(&bfad->bfad_lock, flags);
 		return -EBUSY;
@@ -101,9 +107,10 @@ bfad_iocmd_ioc_get_info(struct bfad_s *bfad, void *cmd)
 
 	/* set adapter hw path */
 	strcpy(iocmd->adapter_hwpath, bfad->pci_name);
-	i = strlen(iocmd->adapter_hwpath) - 1;
-	while (iocmd->adapter_hwpath[i] != '.')
-		i--;
+	for (i = 0; iocmd->adapter_hwpath[i] != ':' && i < BFA_STRING_32; i++)
+		;
+	for (; iocmd->adapter_hwpath[++i] != ':' && i < BFA_STRING_32; )
+		;
 	iocmd->adapter_hwpath[i] = '\0';
 	iocmd->status = BFA_STATUS_OK;
 	return 0;
@@ -880,6 +887,19 @@ out:
 }
 
 int
+bfad_iocmd_qos_set_bw(struct bfad_s *bfad, void *pcmd)
+{
+	struct bfa_bsg_qos_bw_s *iocmd = (struct bfa_bsg_qos_bw_s *)pcmd;
+	unsigned long	flags;
+
+	spin_lock_irqsave(&bfad->bfad_lock, flags);
+	iocmd->status = bfa_fcport_set_qos_bw(&bfad->bfa, &iocmd->qos_bw);
+	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+
+	return 0;
+}
+
+int
 bfad_iocmd_ratelim(struct bfad_s *bfad, unsigned int cmd, void *pcmd)
 {
 	struct bfa_bsg_gen_s *iocmd = (struct bfa_bsg_gen_s *)pcmd;
@@ -888,16 +908,22 @@ bfad_iocmd_ratelim(struct bfad_s *bfad, unsigned int cmd, void *pcmd)
 
 	spin_lock_irqsave(&bfad->bfad_lock, flags);
 
-	if (cmd == IOCMD_RATELIM_ENABLE)
-		fcport->cfg.ratelimit = BFA_TRUE;
-	else if (cmd == IOCMD_RATELIM_DISABLE)
-		fcport->cfg.ratelimit = BFA_FALSE;
+	if ((fcport->cfg.topology == BFA_PORT_TOPOLOGY_LOOP) &&
+		(fcport->topology == BFA_PORT_TOPOLOGY_LOOP))
+		iocmd->status = BFA_STATUS_TOPOLOGY_LOOP;
+	else {
+		if (cmd == IOCMD_RATELIM_ENABLE)
+			fcport->cfg.ratelimit = BFA_TRUE;
+		else if (cmd == IOCMD_RATELIM_DISABLE)
+			fcport->cfg.ratelimit = BFA_FALSE;
 
-	if (fcport->cfg.trl_def_speed == BFA_PORT_SPEED_UNKNOWN)
-		fcport->cfg.trl_def_speed = BFA_PORT_SPEED_1GBPS;
+		if (fcport->cfg.trl_def_speed == BFA_PORT_SPEED_UNKNOWN)
+			fcport->cfg.trl_def_speed = BFA_PORT_SPEED_1GBPS;
+
+		iocmd->status = BFA_STATUS_OK;
+	}
 
 	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
-	iocmd->status = BFA_STATUS_OK;
 
 	return 0;
 }
@@ -919,8 +945,13 @@ bfad_iocmd_ratelim_speed(struct bfad_s *bfad, unsigned int cmd, void *pcmd)
 		return 0;
 	}
 
-	fcport->cfg.trl_def_speed = iocmd->speed;
-	iocmd->status = BFA_STATUS_OK;
+	if ((fcport->cfg.topology == BFA_PORT_TOPOLOGY_LOOP) &&
+		(fcport->topology == BFA_PORT_TOPOLOGY_LOOP))
+		iocmd->status = BFA_STATUS_TOPOLOGY_LOOP;
+	else {
+		fcport->cfg.trl_def_speed = iocmd->speed;
+		iocmd->status = BFA_STATUS_OK;
+	}
 	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
 
 	return 0;
@@ -1167,8 +1198,8 @@ bfad_iocmd_pcifn_create(struct bfad_s *bfad, void *cmd)
 	spin_lock_irqsave(&bfad->bfad_lock, flags);
 	iocmd->status = bfa_ablk_pf_create(&bfad->bfa.modules.ablk,
 				&iocmd->pcifn_id, iocmd->port,
-				iocmd->pcifn_class, iocmd->bandwidth,
-				bfad_hcb_comp, &fcomp);
+				iocmd->pcifn_class, iocmd->bw_min,
+				iocmd->bw_max, bfad_hcb_comp, &fcomp);
 	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
 	if (iocmd->status != BFA_STATUS_OK)
 		goto out;
@@ -1211,8 +1242,8 @@ bfad_iocmd_pcifn_bw(struct bfad_s *bfad, void *cmd)
 	init_completion(&fcomp.comp);
 	spin_lock_irqsave(&bfad->bfad_lock, flags);
 	iocmd->status = bfa_ablk_pf_update(&bfad->bfa.modules.ablk,
-				iocmd->pcifn_id, iocmd->bandwidth,
-				bfad_hcb_comp, &fcomp);
+				iocmd->pcifn_id, iocmd->bw_min,
+				iocmd->bw_max, bfad_hcb_comp, &fcomp);
 	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
 	bfa_trc(bfad, iocmd->status);
 	if (iocmd->status != BFA_STATUS_OK)
@@ -1736,6 +1767,52 @@ bfad_iocmd_diag_lb_stat(struct bfad_s *bfad, void *cmd)
 }
 
 int
+bfad_iocmd_diag_cfg_dport(struct bfad_s *bfad, unsigned int cmd, void *pcmd)
+{
+	struct bfa_bsg_gen_s *iocmd = (struct bfa_bsg_gen_s *)pcmd;
+	unsigned long	flags;
+	struct bfad_hal_comp fcomp;
+
+	init_completion(&fcomp.comp);
+	spin_lock_irqsave(&bfad->bfad_lock, flags);
+	if (cmd == IOCMD_DIAG_DPORT_ENABLE)
+		iocmd->status = bfa_dport_enable(&bfad->bfa,
+					bfad_hcb_comp, &fcomp);
+	else if (cmd == IOCMD_DIAG_DPORT_DISABLE)
+		iocmd->status = bfa_dport_disable(&bfad->bfa,
+					bfad_hcb_comp, &fcomp);
+	else {
+		bfa_trc(bfad, 0);
+		spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+		return -EINVAL;
+	}
+	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+
+	if (iocmd->status != BFA_STATUS_OK)
+		bfa_trc(bfad, iocmd->status);
+	else {
+		wait_for_completion(&fcomp.comp);
+		iocmd->status = fcomp.status;
+	}
+
+	return 0;
+}
+
+int
+bfad_iocmd_diag_dport_get_state(struct bfad_s *bfad, void *pcmd)
+{
+	struct bfa_bsg_diag_dport_get_state_s *iocmd =
+			(struct bfa_bsg_diag_dport_get_state_s *)pcmd;
+	unsigned long	flags;
+
+	spin_lock_irqsave(&bfad->bfad_lock, flags);
+	iocmd->status = bfa_dport_get_state(&bfad->bfa, &iocmd->state);
+	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+
+	return 0;
+}
+
+int
 bfad_iocmd_phy_get_attr(struct bfad_s *bfad, void *cmd)
 {
 	struct bfa_bsg_phy_attr_s *iocmd =
@@ -2052,7 +2129,7 @@ bfad_iocmd_boot_cfg(struct bfad_s *bfad, void *cmd)
 	init_completion(&fcomp.comp);
 	spin_lock_irqsave(&bfad->bfad_lock, flags);
 	iocmd->status = bfa_flash_update_part(BFA_FLASH(&bfad->bfa),
-			BFA_FLASH_PART_BOOT, PCI_FUNC(bfad->pcidev->devfn),
+			BFA_FLASH_PART_BOOT, bfad->bfa.ioc.port_id,
 			&iocmd->cfg, sizeof(struct bfa_boot_cfg_s), 0,
 			bfad_hcb_comp, &fcomp);
 	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
@@ -2074,7 +2151,7 @@ bfad_iocmd_boot_query(struct bfad_s *bfad, void *cmd)
 	init_completion(&fcomp.comp);
 	spin_lock_irqsave(&bfad->bfad_lock, flags);
 	iocmd->status = bfa_flash_read_part(BFA_FLASH(&bfad->bfa),
-			BFA_FLASH_PART_BOOT, PCI_FUNC(bfad->pcidev->devfn),
+			BFA_FLASH_PART_BOOT, bfad->bfa.ioc.port_id,
 			&iocmd->cfg, sizeof(struct bfa_boot_cfg_s), 0,
 			bfad_hcb_comp, &fcomp);
 	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
@@ -2161,22 +2238,31 @@ bfad_iocmd_cfg_trunk(struct bfad_s *bfad, void *cmd, unsigned int v_cmd)
 
 	spin_lock_irqsave(&bfad->bfad_lock, flags);
 
-	if (v_cmd == IOCMD_TRUNK_ENABLE) {
-		trunk->attr.state = BFA_TRUNK_OFFLINE;
-		bfa_fcport_disable(&bfad->bfa);
-		fcport->cfg.trunked = BFA_TRUE;
-	} else if (v_cmd == IOCMD_TRUNK_DISABLE) {
-		trunk->attr.state = BFA_TRUNK_DISABLED;
-		bfa_fcport_disable(&bfad->bfa);
-		fcport->cfg.trunked = BFA_FALSE;
-	}
+	if (bfa_fcport_is_dport(&bfad->bfa))
+		return BFA_STATUS_DPORT_ERR;
 
-	if (!bfa_fcport_is_disabled(&bfad->bfa))
-		bfa_fcport_enable(&bfad->bfa);
+	if ((fcport->cfg.topology == BFA_PORT_TOPOLOGY_LOOP) ||
+		(fcport->topology == BFA_PORT_TOPOLOGY_LOOP))
+		iocmd->status = BFA_STATUS_TOPOLOGY_LOOP;
+	else {
+		if (v_cmd == IOCMD_TRUNK_ENABLE) {
+			trunk->attr.state = BFA_TRUNK_OFFLINE;
+			bfa_fcport_disable(&bfad->bfa);
+			fcport->cfg.trunked = BFA_TRUE;
+		} else if (v_cmd == IOCMD_TRUNK_DISABLE) {
+			trunk->attr.state = BFA_TRUNK_DISABLED;
+			bfa_fcport_disable(&bfad->bfa);
+			fcport->cfg.trunked = BFA_FALSE;
+		}
+
+		if (!bfa_fcport_is_disabled(&bfad->bfa))
+			bfa_fcport_enable(&bfad->bfa);
+
+		iocmd->status = BFA_STATUS_OK;
+	}
 
 	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
 
-	iocmd->status = BFA_STATUS_OK;
 	return 0;
 }
 
@@ -2189,12 +2275,17 @@ bfad_iocmd_trunk_get_attr(struct bfad_s *bfad, void *cmd)
 	unsigned long	flags;
 
 	spin_lock_irqsave(&bfad->bfad_lock, flags);
-	memcpy((void *)&iocmd->attr, (void *)&trunk->attr,
-		sizeof(struct bfa_trunk_attr_s));
-	iocmd->attr.port_id = bfa_lps_get_base_pid(&bfad->bfa);
+	if ((fcport->cfg.topology == BFA_PORT_TOPOLOGY_LOOP) ||
+		(fcport->topology == BFA_PORT_TOPOLOGY_LOOP))
+		iocmd->status = BFA_STATUS_TOPOLOGY_LOOP;
+	else {
+		memcpy((void *)&iocmd->attr, (void *)&trunk->attr,
+			sizeof(struct bfa_trunk_attr_s));
+		iocmd->attr.port_id = bfa_lps_get_base_pid(&bfad->bfa);
+		iocmd->status = BFA_STATUS_OK;
+	}
 	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
 
-	iocmd->status = BFA_STATUS_OK;
 	return 0;
 }
 
@@ -2207,14 +2298,22 @@ bfad_iocmd_qos(struct bfad_s *bfad, void *cmd, unsigned int v_cmd)
 
 	spin_lock_irqsave(&bfad->bfad_lock, flags);
 	if (bfa_ioc_get_type(&bfad->bfa.ioc) == BFA_IOC_TYPE_FC) {
-		if (v_cmd == IOCMD_QOS_ENABLE)
-			fcport->cfg.qos_enabled = BFA_TRUE;
-		else if (v_cmd == IOCMD_QOS_DISABLE)
-			fcport->cfg.qos_enabled = BFA_FALSE;
+		if ((fcport->cfg.topology == BFA_PORT_TOPOLOGY_LOOP) &&
+		(fcport->topology == BFA_PORT_TOPOLOGY_LOOP))
+			iocmd->status = BFA_STATUS_TOPOLOGY_LOOP;
+		else {
+			if (v_cmd == IOCMD_QOS_ENABLE)
+				fcport->cfg.qos_enabled = BFA_TRUE;
+			else if (v_cmd == IOCMD_QOS_DISABLE) {
+				fcport->cfg.qos_enabled = BFA_FALSE;
+				fcport->cfg.qos_bw.high = BFA_QOS_BW_HIGH;
+				fcport->cfg.qos_bw.med = BFA_QOS_BW_MED;
+				fcport->cfg.qos_bw.low = BFA_QOS_BW_LOW;
+			}
+		}
 	}
 	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
 
-	iocmd->status = BFA_STATUS_OK;
 	return 0;
 }
 
@@ -2226,11 +2325,21 @@ bfad_iocmd_qos_get_attr(struct bfad_s *bfad, void *cmd)
 	unsigned long	flags;
 
 	spin_lock_irqsave(&bfad->bfad_lock, flags);
-	iocmd->attr.state = fcport->qos_attr.state;
-	iocmd->attr.total_bb_cr = be32_to_cpu(fcport->qos_attr.total_bb_cr);
+	if ((fcport->cfg.topology == BFA_PORT_TOPOLOGY_LOOP) &&
+		(fcport->topology == BFA_PORT_TOPOLOGY_LOOP))
+		iocmd->status = BFA_STATUS_TOPOLOGY_LOOP;
+	else {
+		iocmd->attr.state = fcport->qos_attr.state;
+		iocmd->attr.total_bb_cr =
+			be32_to_cpu(fcport->qos_attr.total_bb_cr);
+		iocmd->attr.qos_bw.high = fcport->cfg.qos_bw.high;
+		iocmd->attr.qos_bw.med = fcport->cfg.qos_bw.med;
+		iocmd->attr.qos_bw.low = fcport->cfg.qos_bw.low;
+		iocmd->attr.qos_bw_op = fcport->qos_attr.qos_bw_op;
+		iocmd->status = BFA_STATUS_OK;
+	}
 	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
 
-	iocmd->status = BFA_STATUS_OK;
 	return 0;
 }
 
@@ -2274,6 +2383,7 @@ bfad_iocmd_qos_get_stats(struct bfad_s *bfad, void *cmd)
 	struct bfad_hal_comp fcomp;
 	unsigned long	flags;
 	struct bfa_cb_pending_q_s cb_qe;
+	struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(&bfad->bfa);
 
 	init_completion(&fcomp.comp);
 	bfa_pending_q_init(&cb_qe, (bfa_cb_cbfn_t)bfad_hcb_comp,
@@ -2281,7 +2391,11 @@ bfad_iocmd_qos_get_stats(struct bfad_s *bfad, void *cmd)
 
 	spin_lock_irqsave(&bfad->bfad_lock, flags);
 	WARN_ON(!bfa_ioc_get_fcmode(&bfad->bfa.ioc));
-	iocmd->status = bfa_fcport_get_stats(&bfad->bfa, &cb_qe);
+	if ((fcport->cfg.topology == BFA_PORT_TOPOLOGY_LOOP) &&
+		(fcport->topology == BFA_PORT_TOPOLOGY_LOOP))
+		iocmd->status = BFA_STATUS_TOPOLOGY_LOOP;
+	else
+		iocmd->status = bfa_fcport_get_stats(&bfad->bfa, &cb_qe);
 	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
 	if (iocmd->status != BFA_STATUS_OK) {
 		bfa_trc(bfad, iocmd->status);
@@ -2300,6 +2414,7 @@ bfad_iocmd_qos_reset_stats(struct bfad_s *bfad, void *cmd)
 	struct bfad_hal_comp fcomp;
 	unsigned long	flags;
 	struct bfa_cb_pending_q_s cb_qe;
+	struct bfa_fcport_s *fcport = BFA_FCPORT_MOD(&bfad->bfa);
 
 	init_completion(&fcomp.comp);
 	bfa_pending_q_init(&cb_qe, (bfa_cb_cbfn_t)bfad_hcb_comp,
@@ -2307,7 +2422,11 @@ bfad_iocmd_qos_reset_stats(struct bfad_s *bfad, void *cmd)
 
 	spin_lock_irqsave(&bfad->bfad_lock, flags);
 	WARN_ON(!bfa_ioc_get_fcmode(&bfad->bfa.ioc));
-	iocmd->status = bfa_fcport_clear_stats(&bfad->bfa, &cb_qe);
+	if ((fcport->cfg.topology == BFA_PORT_TOPOLOGY_LOOP) &&
+		(fcport->topology == BFA_PORT_TOPOLOGY_LOOP))
+		iocmd->status = BFA_STATUS_TOPOLOGY_LOOP;
+	else
+		iocmd->status = bfa_fcport_clear_stats(&bfad->bfa, &cb_qe);
 	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
 	if (iocmd->status != BFA_STATUS_OK) {
 		bfa_trc(bfad, iocmd->status);
@@ -2435,6 +2554,139 @@ bfad_iocmd_fcpim_cfg_lunmask(struct bfad_s *bfad, void *cmd, unsigned int v_cmd)
 	return 0;
 }
 
+int
+bfad_iocmd_fcpim_throttle_query(struct bfad_s *bfad, void *cmd)
+{
+	struct bfa_bsg_fcpim_throttle_s *iocmd =
+			(struct bfa_bsg_fcpim_throttle_s *)cmd;
+	unsigned long   flags;
+
+	spin_lock_irqsave(&bfad->bfad_lock, flags);
+	iocmd->status = bfa_fcpim_throttle_get(&bfad->bfa,
+				(void *)&iocmd->throttle);
+	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+
+	return 0;
+}
+
+int
+bfad_iocmd_fcpim_throttle_set(struct bfad_s *bfad, void *cmd)
+{
+	struct bfa_bsg_fcpim_throttle_s *iocmd =
+			(struct bfa_bsg_fcpim_throttle_s *)cmd;
+	unsigned long	flags;
+
+	spin_lock_irqsave(&bfad->bfad_lock, flags);
+	iocmd->status = bfa_fcpim_throttle_set(&bfad->bfa,
+				iocmd->throttle.cfg_value);
+	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+
+	return 0;
+}
+
+int
+bfad_iocmd_tfru_read(struct bfad_s *bfad, void *cmd)
+{
+	struct bfa_bsg_tfru_s *iocmd =
+			(struct bfa_bsg_tfru_s *)cmd;
+	struct bfad_hal_comp fcomp;
+	unsigned long flags = 0;
+
+	init_completion(&fcomp.comp);
+	spin_lock_irqsave(&bfad->bfad_lock, flags);
+	iocmd->status = bfa_tfru_read(BFA_FRU(&bfad->bfa),
+				&iocmd->data, iocmd->len, iocmd->offset,
+				bfad_hcb_comp, &fcomp);
+	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+	if (iocmd->status == BFA_STATUS_OK) {
+		wait_for_completion(&fcomp.comp);
+		iocmd->status = fcomp.status;
+	}
+
+	return 0;
+}
+
+int
+bfad_iocmd_tfru_write(struct bfad_s *bfad, void *cmd)
+{
+	struct bfa_bsg_tfru_s *iocmd =
+			(struct bfa_bsg_tfru_s *)cmd;
+	struct bfad_hal_comp fcomp;
+	unsigned long flags = 0;
+
+	init_completion(&fcomp.comp);
+	spin_lock_irqsave(&bfad->bfad_lock, flags);
+	iocmd->status = bfa_tfru_write(BFA_FRU(&bfad->bfa),
+				&iocmd->data, iocmd->len, iocmd->offset,
+				bfad_hcb_comp, &fcomp);
+	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+	if (iocmd->status == BFA_STATUS_OK) {
+		wait_for_completion(&fcomp.comp);
+		iocmd->status = fcomp.status;
+	}
+
+	return 0;
+}
+
+int
+bfad_iocmd_fruvpd_read(struct bfad_s *bfad, void *cmd)
+{
+	struct bfa_bsg_fruvpd_s *iocmd =
+			(struct bfa_bsg_fruvpd_s *)cmd;
+	struct bfad_hal_comp fcomp;
+	unsigned long flags = 0;
+
+	init_completion(&fcomp.comp);
+	spin_lock_irqsave(&bfad->bfad_lock, flags);
+	iocmd->status = bfa_fruvpd_read(BFA_FRU(&bfad->bfa),
+				&iocmd->data, iocmd->len, iocmd->offset,
+				bfad_hcb_comp, &fcomp);
+	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+	if (iocmd->status == BFA_STATUS_OK) {
+		wait_for_completion(&fcomp.comp);
+		iocmd->status = fcomp.status;
+	}
+
+	return 0;
+}
+
+int
+bfad_iocmd_fruvpd_update(struct bfad_s *bfad, void *cmd)
+{
+	struct bfa_bsg_fruvpd_s *iocmd =
+			(struct bfa_bsg_fruvpd_s *)cmd;
+	struct bfad_hal_comp fcomp;
+	unsigned long flags = 0;
+
+	init_completion(&fcomp.comp);
+	spin_lock_irqsave(&bfad->bfad_lock, flags);
+	iocmd->status = bfa_fruvpd_update(BFA_FRU(&bfad->bfa),
+				&iocmd->data, iocmd->len, iocmd->offset,
+				bfad_hcb_comp, &fcomp);
+	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+	if (iocmd->status == BFA_STATUS_OK) {
+		wait_for_completion(&fcomp.comp);
+		iocmd->status = fcomp.status;
+	}
+
+	return 0;
+}
+
+int
+bfad_iocmd_fruvpd_get_max_size(struct bfad_s *bfad, void *cmd)
+{
+	struct bfa_bsg_fruvpd_max_size_s *iocmd =
+			(struct bfa_bsg_fruvpd_max_size_s *)cmd;
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(&bfad->bfad_lock, flags);
+	iocmd->status = bfa_fruvpd_get_max_size(BFA_FRU(&bfad->bfa),
+						&iocmd->max_size);
+	spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+
+	return 0;
+}
+
 static int
 bfad_iocmd_handler(struct bfad_s *bfad, unsigned int cmd, void *iocmd,
 		unsigned int payload_len)
@@ -2660,6 +2912,13 @@ bfad_iocmd_handler(struct bfad_s *bfad, unsigned int cmd, void *iocmd,
 	case IOCMD_DIAG_LB_STAT:
 		rc = bfad_iocmd_diag_lb_stat(bfad, iocmd);
 		break;
+	case IOCMD_DIAG_DPORT_ENABLE:
+	case IOCMD_DIAG_DPORT_DISABLE:
+		rc = bfad_iocmd_diag_cfg_dport(bfad, cmd, iocmd);
+		break;
+	case IOCMD_DIAG_DPORT_GET_STATE:
+		rc = bfad_iocmd_diag_dport_get_state(bfad, iocmd);
+		break;
 	case IOCMD_PHY_GET_ATTR:
 		rc = bfad_iocmd_phy_get_attr(bfad, iocmd);
 		break;
@@ -2741,6 +3000,9 @@ bfad_iocmd_handler(struct bfad_s *bfad, unsigned int cmd, void *iocmd,
 	case IOCMD_QOS_RESET_STATS:
 		rc = bfad_iocmd_qos_reset_stats(bfad, iocmd);
 		break;
+	case IOCMD_QOS_SET_BW:
+		rc = bfad_iocmd_qos_set_bw(bfad, iocmd);
+		break;
 	case IOCMD_VF_GET_STATS:
 		rc = bfad_iocmd_vf_get_stats(bfad, iocmd);
 		break;
@@ -2759,6 +3021,29 @@ bfad_iocmd_handler(struct bfad_s *bfad, unsigned int cmd, void *iocmd,
 	case IOCMD_FCPIM_LUNMASK_DELETE:
 		rc = bfad_iocmd_fcpim_cfg_lunmask(bfad, iocmd, cmd);
 		break;
+	case IOCMD_FCPIM_THROTTLE_QUERY:
+		rc = bfad_iocmd_fcpim_throttle_query(bfad, iocmd);
+		break;
+	case IOCMD_FCPIM_THROTTLE_SET:
+		rc = bfad_iocmd_fcpim_throttle_set(bfad, iocmd);
+		break;
+	/* TFRU */
+	case IOCMD_TFRU_READ:
+		rc = bfad_iocmd_tfru_read(bfad, iocmd);
+		break;
+	case IOCMD_TFRU_WRITE:
+		rc = bfad_iocmd_tfru_write(bfad, iocmd);
+		break;
+	/* FRU */
+	case IOCMD_FRUVPD_READ:
+		rc = bfad_iocmd_fruvpd_read(bfad, iocmd);
+		break;
+	case IOCMD_FRUVPD_UPDATE:
+		rc = bfad_iocmd_fruvpd_update(bfad, iocmd);
+		break;
+	case IOCMD_FRUVPD_GET_MAX_SIZE:
+		rc = bfad_iocmd_fruvpd_get_max_size(bfad, iocmd);
+		break;
 	default:
 		rc = -EINVAL;
 		break;
diff --git a/drivers/scsi/bfa/bfad_bsg.h b/drivers/scsi/bfa/bfad_bsg.h
index 8c569ddb750..15e1fc8e796 100644
--- a/drivers/scsi/bfa/bfad_bsg.h
+++ b/drivers/scsi/bfa/bfad_bsg.h
@@ -141,6 +141,17 @@ enum {
 	IOCMD_FCPIM_LUNMASK_QUERY,
 	IOCMD_FCPIM_LUNMASK_ADD,
 	IOCMD_FCPIM_LUNMASK_DELETE,
+	IOCMD_DIAG_DPORT_ENABLE,
+	IOCMD_DIAG_DPORT_DISABLE,
+	IOCMD_DIAG_DPORT_GET_STATE,
+	IOCMD_QOS_SET_BW,
+	IOCMD_FCPIM_THROTTLE_QUERY,
+	IOCMD_FCPIM_THROTTLE_SET,
+	IOCMD_TFRU_READ,
+	IOCMD_TFRU_WRITE,
+	IOCMD_FRUVPD_READ,
+	IOCMD_FRUVPD_UPDATE,
+	IOCMD_FRUVPD_GET_MAX_SIZE,
 };
 
 struct bfa_bsg_gen_s {
@@ -463,7 +474,8 @@ struct bfa_bsg_pcifn_s {
 	bfa_status_t		status;
 	u16			bfad_num;
 	u16			pcifn_id;
-	u32			bandwidth;
+	u16			bw_min;
+	u16			bw_max;
 	u8			port;
 	enum bfi_pcifn_class	pcifn_class;
 	u8			rsvd[1];
@@ -613,6 +625,13 @@ struct bfa_bsg_diag_lb_stat_s {
 	u16		rsvd;
 };
 
+struct bfa_bsg_diag_dport_get_state_s {
+	bfa_status_t	status;
+	u16		bfad_num;
+	u16		rsvd;
+	enum bfa_dport_state state;
+};
+
 struct bfa_bsg_phy_attr_s {
 	bfa_status_t	status;
 	u16	bfad_num;
@@ -694,6 +713,13 @@ struct bfa_bsg_qos_vc_attr_s {
 	struct bfa_qos_vc_attr_s attr;
 };
 
+struct bfa_bsg_qos_bw_s {
+	bfa_status_t	status;
+	u16		bfad_num;
+	u16		rsvd;
+	struct bfa_qos_bw_s qos_bw;
+};
+
 struct bfa_bsg_vf_stats_s {
 	bfa_status_t	status;
 	u16		bfad_num;
@@ -722,6 +748,41 @@ struct bfa_bsg_fcpim_lunmask_s {
 	struct scsi_lun	lun;
 };
 
+struct bfa_bsg_fcpim_throttle_s {
+	bfa_status_t	status;
+	u16		bfad_num;
+	u16		vf_id;
+	struct bfa_defs_fcpim_throttle_s throttle;
+};
+
+#define BFA_TFRU_DATA_SIZE		64
+#define BFA_MAX_FRUVPD_TRANSFER_SIZE	0x1000
+
+struct bfa_bsg_tfru_s {
+	bfa_status_t	status;
+	u16		bfad_num;
+	u16		rsvd;
+	u32		offset;
+	u32		len;
+	u8		data[BFA_TFRU_DATA_SIZE];
+};
+
+struct bfa_bsg_fruvpd_s {
+	bfa_status_t	status;
+	u16		bfad_num;
+	u16		rsvd;
+	u32		offset;
+	u32		len;
+	u8		data[BFA_MAX_FRUVPD_TRANSFER_SIZE];
+};
+
+struct bfa_bsg_fruvpd_max_size_s {
+	bfa_status_t	status;
+	u16		bfad_num;
+	u16		rsvd;
+	u32		max_size;
+};
+
 struct bfa_bsg_fcpt_s {
 	bfa_status_t    status;
 	u16		vf_id;
diff --git a/drivers/scsi/bfa/bfad_drv.h b/drivers/scsi/bfa/bfad_drv.h
index 1840651ce1d..0c64a04f01f 100644
--- a/drivers/scsi/bfa/bfad_drv.h
+++ b/drivers/scsi/bfa/bfad_drv.h
@@ -57,7 +57,7 @@
 #ifdef BFA_DRIVER_VERSION
 #define BFAD_DRIVER_VERSION    BFA_DRIVER_VERSION
 #else
-#define BFAD_DRIVER_VERSION    "3.1.2.0"
+#define BFAD_DRIVER_VERSION    "3.1.2.1"
 #endif
 
 #define BFAD_PROTO_NAME FCPI_NAME
diff --git a/drivers/scsi/bfa/bfi.h b/drivers/scsi/bfa/bfi.h
index b2ba0b2e91b..57b146bca18 100644
--- a/drivers/scsi/bfa/bfi.h
+++ b/drivers/scsi/bfa/bfi.h
@@ -210,7 +210,8 @@ enum bfi_mclass {
 	BFI_MC_PORT		= 21,	/*  Physical port		    */
 	BFI_MC_SFP		= 22,	/*  SFP module	*/
 	BFI_MC_PHY		= 25,   /*  External PHY message class	*/
-	BFI_MC_MAX		= 32
+	BFI_MC_FRU		= 34,
+	BFI_MC_MAX		= 35
 };
 
 #define BFI_IOC_MAX_CQS		4
@@ -288,6 +289,9 @@ struct bfi_ioc_attr_s {
 	char		optrom_version[BFA_VERSION_LEN];
 	struct		bfa_mfg_vpd_s	vpd;
 	u32	card_type;	/*  card type			*/
+	u8	mfg_day;	/* manufacturing day */
+	u8	mfg_month;	/* manufacturing month */
+	u16	mfg_year;	/* manufacturing year */
 };
 
 /*
@@ -687,7 +691,8 @@ struct bfi_ablk_h2i_pf_req_s {
 	u8			pcifn;
 	u8			port;
 	u16			pers;
-	u32			bw;
+	u16			bw_min; /* percent BW @ max speed */
+	u16			bw_max; /* percent BW @ max speed */
 };
 
 /* BFI_ABLK_H2I_OPTROM_ENABLE, BFI_ABLK_H2I_OPTROM_DISABLE */
@@ -957,6 +962,7 @@ enum bfi_diag_h2i {
 	BFI_DIAG_H2I_TEMPSENSOR = 4,
 	BFI_DIAG_H2I_LEDTEST = 5,
 	BFI_DIAG_H2I_QTEST      = 6,
+	BFI_DIAG_H2I_DPORT	= 7,
 };
 
 enum bfi_diag_i2h {
@@ -966,6 +972,7 @@ enum bfi_diag_i2h {
 	BFI_DIAG_I2H_TEMPSENSOR = BFA_I2HM(BFI_DIAG_H2I_TEMPSENSOR),
 	BFI_DIAG_I2H_LEDTEST = BFA_I2HM(BFI_DIAG_H2I_LEDTEST),
 	BFI_DIAG_I2H_QTEST      = BFA_I2HM(BFI_DIAG_H2I_QTEST),
+	BFI_DIAG_I2H_DPORT	= BFA_I2HM(BFI_DIAG_H2I_DPORT),
 };
 
 #define BFI_DIAG_MAX_SGES	2
@@ -1052,6 +1059,23 @@ struct bfi_diag_qtest_req_s {
 #define bfi_diag_qtest_rsp_t struct bfi_diag_qtest_req_s
 
 /*
+ *	D-port test
+ */
+enum bfi_dport_req {
+	BFI_DPORT_DISABLE	= 0,	/* disable dport request	*/
+	BFI_DPORT_ENABLE	= 1,	/* enable dport request		*/
+};
+
+struct bfi_diag_dport_req_s {
+	struct bfi_mhdr_s	mh;	/* 4 bytes                      */
+	u8			req;    /* request 1: enable 0: disable */
+	u8			status; /* reply status			*/
+	u8			rsvd[2];
+	u32			msgtag; /* msgtag for reply		*/
+};
+#define bfi_diag_dport_rsp_t struct bfi_diag_dport_req_s
+
+/*
  *	PHY module specific
  */
 enum bfi_phy_h2i_msgs_e {
@@ -1147,6 +1171,50 @@ struct bfi_phy_write_rsp_s {
 	u32			length;
 };
 
+enum bfi_fru_h2i_msgs {
+	BFI_FRUVPD_H2I_WRITE_REQ = 1,
+	BFI_FRUVPD_H2I_READ_REQ = 2,
+	BFI_TFRU_H2I_WRITE_REQ = 3,
+	BFI_TFRU_H2I_READ_REQ = 4,
+};
+
+enum bfi_fru_i2h_msgs {
+	BFI_FRUVPD_I2H_WRITE_RSP = BFA_I2HM(1),
+	BFI_FRUVPD_I2H_READ_RSP = BFA_I2HM(2),
+	BFI_TFRU_I2H_WRITE_RSP = BFA_I2HM(3),
+	BFI_TFRU_I2H_READ_RSP = BFA_I2HM(4),
+};
+
+/*
+ * FRU write request
+ */
+struct bfi_fru_write_req_s {
+	struct bfi_mhdr_s	mh;	/* Common msg header */
+	u8			last;
+	u8			rsv[3];
+	u32			offset;
+	u32			length;
+	struct bfi_alen_s	alen;
+};
+
+/*
+ * FRU read request
+ */
+struct bfi_fru_read_req_s {
+	struct bfi_mhdr_s	mh;	/* Common msg header */
+	u32			offset;
+	u32			length;
+	struct bfi_alen_s	alen;
+};
+
+/*
+ * FRU response
+ */
+struct bfi_fru_rsp_s {
+	struct bfi_mhdr_s	mh;	/* Common msg header */
+	u32			status;
+	u32			length;
+};
 #pragma pack()
 
 #endif /* __BFI_H__ */
diff --git a/drivers/scsi/bfa/bfi_ms.h b/drivers/scsi/bfa/bfi_ms.h
index d4220e13caf..5ae2c167b2c 100644
--- a/drivers/scsi/bfa/bfi_ms.h
+++ b/drivers/scsi/bfa/bfi_ms.h
@@ -426,6 +426,7 @@ struct bfi_lps_login_req_s {
 	u8		auth_en;
 	u8		lps_role;
 	u8		bb_scn;
+	u32		vvl_flag;
 };
 
 struct bfi_lps_login_rsp_s {
@@ -499,6 +500,9 @@ enum bfi_rport_i2h_msgs {
 	BFI_RPORT_I2H_CREATE_RSP = BFA_I2HM(1),
 	BFI_RPORT_I2H_DELETE_RSP = BFA_I2HM(2),
 	BFI_RPORT_I2H_QOS_SCN    = BFA_I2HM(3),
+	BFI_RPORT_I2H_LIP_SCN_ONLINE =	BFA_I2HM(4),
+	BFI_RPORT_I2H_LIP_SCN_OFFLINE = BFA_I2HM(5),
+	BFI_RPORT_I2H_NO_DEV	= BFA_I2HM(6),
 };
 
 struct bfi_rport_create_req_s {
@@ -551,6 +555,14 @@ struct bfi_rport_qos_scn_s {
 	struct bfa_rport_qos_attr_s new_qos_attr;  /* New QoS Attributes */
 };
 
+struct bfi_rport_lip_scn_s {
+	struct bfi_mhdr_s  mh;		/*!< common msg header	*/
+	u16	bfa_handle;	/*!< host rport handle	*/
+	u8		status;		/*!< scn online status	*/
+	u8		rsvd;
+	struct bfa_fcport_loop_info_s	loop_info;
+};
+
 union bfi_rport_h2i_msg_u {
 	struct bfi_msg_s		*msg;
 	struct bfi_rport_create_req_s	*create_req;
@@ -563,6 +575,7 @@ union bfi_rport_i2h_msg_u {
 	struct bfi_rport_create_rsp_s	*create_rsp;
 	struct bfi_rport_delete_rsp_s	*delete_rsp;
 	struct bfi_rport_qos_scn_s	*qos_scn_evt;
+	struct bfi_rport_lip_scn_s	*lip_scn;
 };
 
 /*
@@ -828,6 +841,7 @@ enum bfi_tskim_status {
 	 */
 	BFI_TSKIM_STS_TIMEOUT  = 10,	/*  TM request timedout	*/
 	BFI_TSKIM_STS_ABORTED  = 11,	/*  Aborted on host request */
+	BFI_TSKIM_STS_UTAG     = 12,	/*  unknown tag for request */
 };
 
 struct bfi_tskim_rsp_s {
diff --git a/drivers/scsi/bfa/bfi_reg.h b/drivers/scsi/bfa/bfi_reg.h
index ed5f159e186..99133bcf53f 100644
--- a/drivers/scsi/bfa/bfi_reg.h
+++ b/drivers/scsi/bfa/bfi_reg.h
@@ -338,6 +338,7 @@ enum {
 #define __A2T_AHB_LOAD			0x00000800
 #define __WGN_READY			0x00000400
 #define __GLBL_PF_VF_CFG_RDY		0x00000200
+#define CT2_NFC_STS_REG			0x00027410
 #define CT2_NFC_CSR_CLR_REG		0x00027420
 #define CT2_NFC_CSR_SET_REG		0x00027424
 #define __HALT_NFC_CONTROLLER		0x00000002
@@ -355,6 +356,8 @@ enum {
 	(CT2_CSI_MAC0_CONTROL_REG +	\
 	(__n) * (CT2_CSI_MAC1_CONTROL_REG - CT2_CSI_MAC0_CONTROL_REG))
 
+#define CT2_NFC_FLASH_STS_REG		0x00014834
+#define __FLASH_PLL_INIT_AND_RESET_IN_PROGRESS	0x00000020
 /*
  * Name semaphore registers based on usage
  */
diff --git a/drivers/scsi/fcoe/fcoe.c b/drivers/scsi/fcoe/fcoe.c
index 078d262ac7c..666b7ac4475 100644
--- a/drivers/scsi/fcoe/fcoe.c
+++ b/drivers/scsi/fcoe/fcoe.c
@@ -1643,7 +1643,7 @@ static int fcoe_xmit(struct fc_lport *lport, struct fc_frame *fp)
 	skb_reset_network_header(skb);
 	skb->mac_len = elen;
 	skb->protocol = htons(ETH_P_FCOE);
-	skb->priority = port->priority;
+	skb->priority = fcoe->priority;
 
 	if (fcoe->netdev->priv_flags & IFF_802_1Q_VLAN &&
 	    fcoe->realdev->features & NETIF_F_HW_VLAN_TX) {
@@ -1917,7 +1917,6 @@ static int fcoe_dcb_app_notification(struct notifier_block *notifier,
 	struct fcoe_ctlr *ctlr;
 	struct fcoe_interface *fcoe;
 	struct net_device *netdev;
-	struct fcoe_port *port;
 	int prio;
 
 	if (entry->app.selector != DCB_APP_IDTYPE_ETHTYPE)
@@ -1946,10 +1945,8 @@ static int fcoe_dcb_app_notification(struct notifier_block *notifier,
 	    entry->app.protocol == ETH_P_FCOE)
 		ctlr->priority = prio;
 
-	if (entry->app.protocol == ETH_P_FCOE) {
-		port = lport_priv(ctlr->lp);
-		port->priority = prio;
-	}
+	if (entry->app.protocol == ETH_P_FCOE)
+		fcoe->priority = prio;
 
 	return NOTIFY_OK;
 }
@@ -2180,7 +2177,6 @@ static void fcoe_dcb_create(struct fcoe_interface *fcoe)
 	u8 fup, up;
 	struct net_device *netdev = fcoe->realdev;
 	struct fcoe_ctlr *ctlr = fcoe_to_ctlr(fcoe);
-	struct fcoe_port *port = lport_priv(ctlr->lp);
 	struct dcb_app app = {
 				.priority = 0,
 				.protocol = ETH_P_FCOE
@@ -2202,8 +2198,8 @@ static void fcoe_dcb_create(struct fcoe_interface *fcoe)
 			fup = dcb_getapp(netdev, &app);
 		}
 
-		port->priority = ffs(up) ? ffs(up) - 1 : 0;
-		ctlr->priority = ffs(fup) ? ffs(fup) - 1 : port->priority;
+		fcoe->priority = ffs(up) ? ffs(up) - 1 : 0;
+		ctlr->priority = ffs(fup) ? ffs(fup) - 1 : fcoe->priority;
 	}
 #endif
 }
diff --git a/drivers/scsi/fcoe/fcoe.h b/drivers/scsi/fcoe/fcoe.h
index a624add4f8e..b42dc32cb5e 100644
--- a/drivers/scsi/fcoe/fcoe.h
+++ b/drivers/scsi/fcoe/fcoe.h
@@ -71,6 +71,7 @@ do {                                                            	\
  * @oem:	      The offload exchange manager for all local port
  *		      instances associated with this port
  * @removed:	      Indicates fcoe interface removed from net device
+ * @priority:	      Priority for the FCoE packet (DCB)
  * This structure is 1:1 with a net device.
  */
 struct fcoe_interface {
@@ -81,6 +82,7 @@ struct fcoe_interface {
 	struct packet_type fip_packet_type;
 	struct fc_exch_mgr *oem;
 	u8	removed;
+	u8	priority;
 };
 
 #define fcoe_to_ctlr(x)						\
diff --git a/drivers/scsi/libfc/fc_fcp.c b/drivers/scsi/libfc/fc_fcp.c
index 14243fa5f8e..fcb9d0b20ee 100644
--- a/drivers/scsi/libfc/fc_fcp.c
+++ b/drivers/scsi/libfc/fc_fcp.c
@@ -851,7 +851,8 @@ static void fc_fcp_resp(struct fc_fcp_pkt *fsp, struct fc_frame *fp)
 			fc_rp_info = (struct fcp_resp_rsp_info *)(rp_ex + 1);
 			if (flags & FCP_RSP_LEN_VAL) {
 				respl = ntohl(rp_ex->fr_rsp_len);
-				if (respl != sizeof(*fc_rp_info))
+				if ((respl != FCP_RESP_RSP_INFO_LEN4) &&
+				    (respl != FCP_RESP_RSP_INFO_LEN8))
 					goto len_err;
 				if (fsp->wait_for_comp) {
 					/* Abuse cdb_status for rsp code */
diff --git a/drivers/scsi/lpfc/lpfc.h b/drivers/scsi/lpfc/lpfc.h
index a184c2443a6..69b59935b53 100644
--- a/drivers/scsi/lpfc/lpfc.h
+++ b/drivers/scsi/lpfc/lpfc.h
@@ -27,6 +27,8 @@
 
 struct lpfc_sli2_slim;
 
+#define ELX_MODEL_NAME_SIZE	80
+
 #define LPFC_PCI_DEV_LP		0x1
 #define LPFC_PCI_DEV_OC		0x2
 
diff --git a/drivers/scsi/lpfc/lpfc_attr.c b/drivers/scsi/lpfc/lpfc_attr.c
index b032562aa0d..ad16e54ac38 100644
--- a/drivers/scsi/lpfc/lpfc_attr.c
+++ b/drivers/scsi/lpfc/lpfc_attr.c
@@ -3935,6 +3935,12 @@ MODULE_PARM_DESC(lpfc_fcp_look_ahead, "Look ahead for completions");
 # 	- Only meaningful if BG is turned on (lpfc_enable_bg=1).
 #	- Allows you to ultimately specify which profiles to use
 #	- Default will result in registering capabilities for all profiles.
+#	- SHOST_DIF_TYPE1_PROTECTION	1
+#		HBA supports T10 DIF Type 1: HBA to Target Type 1 Protection
+#	- SHOST_DIX_TYPE0_PROTECTION	8
+#		HBA supports DIX Type 0: Host to HBA protection only
+#	- SHOST_DIX_TYPE1_PROTECTION	16
+#		HBA supports DIX Type 1: Host to HBA  Type 1 protection
 #
 */
 unsigned int lpfc_prot_mask = SHOST_DIF_TYPE1_PROTECTION |
@@ -3947,7 +3953,7 @@ MODULE_PARM_DESC(lpfc_prot_mask, "host protection mask");
 /*
 # lpfc_prot_guard: i
 #	- Bit mask of protection guard types to register with the SCSI mid-layer
-# 	- Guard types are currently either 1) IP checksum 2) T10-DIF CRC
+#	- Guard types are currently either 1) T10-DIF CRC 2) IP checksum
 #	- Allows you to ultimately specify which profiles to use
 #	- Default will result in registering capabilities for all guard types
 #
diff --git a/drivers/scsi/lpfc/lpfc_crtn.h b/drivers/scsi/lpfc/lpfc_crtn.h
index e470c489de0..4380a44000b 100644
--- a/drivers/scsi/lpfc/lpfc_crtn.h
+++ b/drivers/scsi/lpfc/lpfc_crtn.h
@@ -467,3 +467,4 @@ int lpfc_sli4_read_config(struct lpfc_hba *);
 void lpfc_sli4_node_prep(struct lpfc_hba *);
 int lpfc_sli4_xri_sgl_update(struct lpfc_hba *);
 void lpfc_free_sgl_list(struct lpfc_hba *, struct list_head *);
+uint32_t lpfc_sli_port_speed_get(struct lpfc_hba *);
diff --git a/drivers/scsi/lpfc/lpfc_els.c b/drivers/scsi/lpfc/lpfc_els.c
index cfe533bc979..f19e9b6f9f1 100644
--- a/drivers/scsi/lpfc/lpfc_els.c
+++ b/drivers/scsi/lpfc/lpfc_els.c
@@ -809,6 +809,8 @@ lpfc_cmpl_els_flogi_nport(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 	phba->fc_ratov = FF_DEF_RATOV;
 	rc = memcmp(&vport->fc_portname, &sp->portName,
 		    sizeof(vport->fc_portname));
+	memcpy(&phba->fc_fabparam, sp, sizeof(struct serv_parm));
+
 	if (rc >= 0) {
 		/* This side will initiate the PLOGI */
 		spin_lock_irq(shost->host_lock);
@@ -3160,7 +3162,8 @@ lpfc_els_retry(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 				retry = 1;
 				break;
 			}
-			if (cmd == ELS_CMD_PLOGI) {
+			if ((cmd == ELS_CMD_PLOGI) ||
+			    (cmd == ELS_CMD_PRLI)) {
 				delay = 1000;
 				maxretry = lpfc_max_els_tries + 1;
 				retry = 1;
@@ -3305,7 +3308,7 @@ lpfc_els_retry(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 			ndlp->nlp_prev_state = ndlp->nlp_state;
 			if (cmd == ELS_CMD_PRLI)
 				lpfc_nlp_set_state(vport, ndlp,
-					NLP_STE_REG_LOGIN_ISSUE);
+					NLP_STE_PRLI_ISSUE);
 			else
 				lpfc_nlp_set_state(vport, ndlp,
 					NLP_STE_NPR_NODE);
diff --git a/drivers/scsi/lpfc/lpfc_hbadisc.c b/drivers/scsi/lpfc/lpfc_hbadisc.c
index e9845d2ecf1..d7096ad94d3 100644
--- a/drivers/scsi/lpfc/lpfc_hbadisc.c
+++ b/drivers/scsi/lpfc/lpfc_hbadisc.c
@@ -1506,9 +1506,10 @@ lpfc_match_fcf_conn_list(struct lpfc_hba *phba,
 		}
 	}
 
-	/* If FCF not available return 0 */
+	/* FCF not valid/available or solicitation in progress */
 	if (!bf_get(lpfc_fcf_record_fcf_avail, new_fcf_record) ||
-		!bf_get(lpfc_fcf_record_fcf_valid, new_fcf_record))
+	    !bf_get(lpfc_fcf_record_fcf_valid, new_fcf_record) ||
+	    bf_get(lpfc_fcf_record_fcf_sol, new_fcf_record))
 		return 0;
 
 	if (!(phba->hba_flag & HBA_FIP_SUPPORT)) {
@@ -1842,6 +1843,7 @@ lpfc_sli4_log_fcf_record_info(struct lpfc_hba *phba,
 			"\tFCF_Index     : x%x\n"
 			"\tFCF_Avail     : x%x\n"
 			"\tFCF_Valid     : x%x\n"
+			"\tFCF_SOL       : x%x\n"
 			"\tFIP_Priority  : x%x\n"
 			"\tMAC_Provider  : x%x\n"
 			"\tLowest VLANID : x%x\n"
@@ -1852,6 +1854,7 @@ lpfc_sli4_log_fcf_record_info(struct lpfc_hba *phba,
 			bf_get(lpfc_fcf_record_fcf_index, fcf_record),
 			bf_get(lpfc_fcf_record_fcf_avail, fcf_record),
 			bf_get(lpfc_fcf_record_fcf_valid, fcf_record),
+			bf_get(lpfc_fcf_record_fcf_sol, fcf_record),
 			fcf_record->fip_priority,
 			bf_get(lpfc_fcf_record_mac_addr_prov, fcf_record),
 			vlan_id,
@@ -2185,12 +2188,14 @@ lpfc_mbx_cmpl_fcf_scan_read_fcf_rec(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 					       new_fcf_record));
 		lpfc_printf_log(phba, KERN_WARNING, LOG_FIP,
 				"2781 FCF (x%x) failed connection "
-				"list check: (x%x/x%x)\n",
+				"list check: (x%x/x%x/%x)\n",
 				bf_get(lpfc_fcf_record_fcf_index,
 				       new_fcf_record),
 				bf_get(lpfc_fcf_record_fcf_avail,
 				       new_fcf_record),
 				bf_get(lpfc_fcf_record_fcf_valid,
+				       new_fcf_record),
+				bf_get(lpfc_fcf_record_fcf_sol,
 				       new_fcf_record));
 		if ((phba->fcf.fcf_flag & FCF_IN_USE) &&
 		    lpfc_sli4_fcf_record_match(phba, &phba->fcf.current_rec,
diff --git a/drivers/scsi/lpfc/lpfc_hw4.h b/drivers/scsi/lpfc/lpfc_hw4.h
index 834b699cac7..2cdeb5434fb 100644
--- a/drivers/scsi/lpfc/lpfc_hw4.h
+++ b/drivers/scsi/lpfc/lpfc_hw4.h
@@ -1305,6 +1305,11 @@ struct lpfc_mbx_mq_create_ext {
 #define lpfc_mbx_mq_create_ext_async_evt_link_SHIFT	LPFC_TRAILER_CODE_LINK
 #define lpfc_mbx_mq_create_ext_async_evt_link_MASK	0x00000001
 #define lpfc_mbx_mq_create_ext_async_evt_link_WORD	async_evt_bmap
+#define LPFC_EVT_CODE_LINK_NO_LINK	0x0
+#define LPFC_EVT_CODE_LINK_10_MBIT	0x1
+#define LPFC_EVT_CODE_LINK_100_MBIT	0x2
+#define LPFC_EVT_CODE_LINK_1_GBIT	0x3
+#define LPFC_EVT_CODE_LINK_10_GBIT	0x4
 #define lpfc_mbx_mq_create_ext_async_evt_fip_SHIFT	LPFC_TRAILER_CODE_FCOE
 #define lpfc_mbx_mq_create_ext_async_evt_fip_MASK	0x00000001
 #define lpfc_mbx_mq_create_ext_async_evt_fip_WORD	async_evt_bmap
@@ -1314,6 +1319,13 @@ struct lpfc_mbx_mq_create_ext {
 #define lpfc_mbx_mq_create_ext_async_evt_fc_SHIFT	LPFC_TRAILER_CODE_FC
 #define lpfc_mbx_mq_create_ext_async_evt_fc_MASK	0x00000001
 #define lpfc_mbx_mq_create_ext_async_evt_fc_WORD	async_evt_bmap
+#define LPFC_EVT_CODE_FC_NO_LINK	0x0
+#define LPFC_EVT_CODE_FC_1_GBAUD	0x1
+#define LPFC_EVT_CODE_FC_2_GBAUD	0x2
+#define LPFC_EVT_CODE_FC_4_GBAUD	0x4
+#define LPFC_EVT_CODE_FC_8_GBAUD	0x8
+#define LPFC_EVT_CODE_FC_10_GBAUD	0xA
+#define LPFC_EVT_CODE_FC_16_GBAUD	0x10
 #define lpfc_mbx_mq_create_ext_async_evt_sli_SHIFT	LPFC_TRAILER_CODE_SLI
 #define lpfc_mbx_mq_create_ext_async_evt_sli_MASK	0x00000001
 #define lpfc_mbx_mq_create_ext_async_evt_sli_WORD	async_evt_bmap
@@ -1695,8 +1707,14 @@ struct fcf_record {
 #define lpfc_fcf_record_fc_map_2_MASK		0x000000FF
 #define lpfc_fcf_record_fc_map_2_WORD		word7
 #define lpfc_fcf_record_fcf_valid_SHIFT		24
-#define lpfc_fcf_record_fcf_valid_MASK		0x000000FF
+#define lpfc_fcf_record_fcf_valid_MASK		0x00000001
 #define lpfc_fcf_record_fcf_valid_WORD		word7
+#define lpfc_fcf_record_fcf_fc_SHIFT		25
+#define lpfc_fcf_record_fcf_fc_MASK		0x00000001
+#define lpfc_fcf_record_fcf_fc_WORD		word7
+#define lpfc_fcf_record_fcf_sol_SHIFT		31
+#define lpfc_fcf_record_fcf_sol_MASK		0x00000001
+#define lpfc_fcf_record_fcf_sol_WORD		word7
 	uint32_t word8;
 #define lpfc_fcf_record_fcf_index_SHIFT		0
 #define lpfc_fcf_record_fcf_index_MASK		0x0000FFFF
diff --git a/drivers/scsi/lpfc/lpfc_init.c b/drivers/scsi/lpfc/lpfc_init.c
index 8a55a586dd6..7dc4218d9c4 100644
--- a/drivers/scsi/lpfc/lpfc_init.c
+++ b/drivers/scsi/lpfc/lpfc_init.c
@@ -1892,8 +1892,10 @@ lpfc_get_hba_model_desc(struct lpfc_hba *phba, uint8_t *mdp, uint8_t *descp)
 		max_speed = 4;
 	else if (phba->lmt & LMT_2Gb)
 		max_speed = 2;
-	else
+	else if (phba->lmt & LMT_1Gb)
 		max_speed = 1;
+	else
+		max_speed = 0;
 
 	vp = &phba->vpd;
 
@@ -2078,9 +2080,13 @@ lpfc_get_hba_model_desc(struct lpfc_hba *phba, uint8_t *mdp, uint8_t *descp)
 	if (descp && descp[0] == '\0') {
 		if (oneConnect)
 			snprintf(descp, 255,
-				"Emulex OneConnect %s, %s Initiator, Port %s",
+				"Emulex OneConnect %s, %s Initiator %s",
 				m.name, m.function,
 				phba->Port);
+		else if (max_speed == 0)
+			snprintf(descp, 255,
+				"Emulex %s %s %s ",
+				m.name, m.bus, m.function);
 		else
 			snprintf(descp, 255,
 				"Emulex %s %d%s %s %s",
@@ -3502,6 +3508,119 @@ lpfc_sli4_parse_latt_link_speed(struct lpfc_hba *phba,
 }
 
 /**
+ * lpfc_sli_port_speed_get - Get sli3 link speed code to link speed
+ * @phba: pointer to lpfc hba data structure.
+ *
+ * This routine is to get an SLI3 FC port's link speed in Mbps.
+ *
+ * Return: link speed in terms of Mbps.
+ **/
+uint32_t
+lpfc_sli_port_speed_get(struct lpfc_hba *phba)
+{
+	uint32_t link_speed;
+
+	if (!lpfc_is_link_up(phba))
+		return 0;
+
+	switch (phba->fc_linkspeed) {
+	case LPFC_LINK_SPEED_1GHZ:
+		link_speed = 1000;
+		break;
+	case LPFC_LINK_SPEED_2GHZ:
+		link_speed = 2000;
+		break;
+	case LPFC_LINK_SPEED_4GHZ:
+		link_speed = 4000;
+		break;
+	case LPFC_LINK_SPEED_8GHZ:
+		link_speed = 8000;
+		break;
+	case LPFC_LINK_SPEED_10GHZ:
+		link_speed = 10000;
+		break;
+	case LPFC_LINK_SPEED_16GHZ:
+		link_speed = 16000;
+		break;
+	default:
+		link_speed = 0;
+	}
+	return link_speed;
+}
+
+/**
+ * lpfc_sli4_port_speed_parse - Parse async evt link speed code to link speed
+ * @phba: pointer to lpfc hba data structure.
+ * @evt_code: asynchronous event code.
+ * @speed_code: asynchronous event link speed code.
+ *
+ * This routine is to parse the giving SLI4 async event link speed code into
+ * value of Mbps for the link speed.
+ *
+ * Return: link speed in terms of Mbps.
+ **/
+static uint32_t
+lpfc_sli4_port_speed_parse(struct lpfc_hba *phba, uint32_t evt_code,
+			   uint8_t speed_code)
+{
+	uint32_t port_speed;
+
+	switch (evt_code) {
+	case LPFC_TRAILER_CODE_LINK:
+		switch (speed_code) {
+		case LPFC_EVT_CODE_LINK_NO_LINK:
+			port_speed = 0;
+			break;
+		case LPFC_EVT_CODE_LINK_10_MBIT:
+			port_speed = 10;
+			break;
+		case LPFC_EVT_CODE_LINK_100_MBIT:
+			port_speed = 100;
+			break;
+		case LPFC_EVT_CODE_LINK_1_GBIT:
+			port_speed = 1000;
+			break;
+		case LPFC_EVT_CODE_LINK_10_GBIT:
+			port_speed = 10000;
+			break;
+		default:
+			port_speed = 0;
+		}
+		break;
+	case LPFC_TRAILER_CODE_FC:
+		switch (speed_code) {
+		case LPFC_EVT_CODE_FC_NO_LINK:
+			port_speed = 0;
+			break;
+		case LPFC_EVT_CODE_FC_1_GBAUD:
+			port_speed = 1000;
+			break;
+		case LPFC_EVT_CODE_FC_2_GBAUD:
+			port_speed = 2000;
+			break;
+		case LPFC_EVT_CODE_FC_4_GBAUD:
+			port_speed = 4000;
+			break;
+		case LPFC_EVT_CODE_FC_8_GBAUD:
+			port_speed = 8000;
+			break;
+		case LPFC_EVT_CODE_FC_10_GBAUD:
+			port_speed = 10000;
+			break;
+		case LPFC_EVT_CODE_FC_16_GBAUD:
+			port_speed = 16000;
+			break;
+		default:
+			port_speed = 0;
+		}
+		break;
+	default:
+		port_speed = 0;
+	}
+	return port_speed;
+}
+
+/**
  * lpfc_sli4_async_link_evt - Process the asynchronous FCoE link event
  * @phba: pointer to lpfc hba data structure.
  * @acqe_link: pointer to the async link completion queue entry.
@@ -3558,7 +3677,8 @@ lpfc_sli4_async_link_evt(struct lpfc_hba *phba,
 
 	/* Keep the link status for extra SLI4 state machine reference */
 	phba->sli4_hba.link_state.speed =
-				bf_get(lpfc_acqe_link_speed, acqe_link);
+			lpfc_sli4_port_speed_parse(phba, LPFC_TRAILER_CODE_LINK,
+				bf_get(lpfc_acqe_link_speed, acqe_link));
 	phba->sli4_hba.link_state.duplex =
 				bf_get(lpfc_acqe_link_duplex, acqe_link);
 	phba->sli4_hba.link_state.status =
@@ -3570,7 +3690,8 @@ lpfc_sli4_async_link_evt(struct lpfc_hba *phba,
 	phba->sli4_hba.link_state.fault =
 				bf_get(lpfc_acqe_link_fault, acqe_link);
 	phba->sli4_hba.link_state.logical_speed =
-			bf_get(lpfc_acqe_logical_link_speed, acqe_link);
+			bf_get(lpfc_acqe_logical_link_speed, acqe_link) * 10;
+
 	lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
 			"2900 Async FC/FCoE Link event - Speed:%dGBit "
 			"duplex:x%x LA Type:x%x Port Type:%d Port Number:%d "
@@ -3580,7 +3701,7 @@ lpfc_sli4_async_link_evt(struct lpfc_hba *phba,
 			phba->sli4_hba.link_state.status,
 			phba->sli4_hba.link_state.type,
 			phba->sli4_hba.link_state.number,
-			phba->sli4_hba.link_state.logical_speed * 10,
+			phba->sli4_hba.link_state.logical_speed,
 			phba->sli4_hba.link_state.fault);
 	/*
 	 * For FC Mode: issue the READ_TOPOLOGY mailbox command to fetch
@@ -3652,7 +3773,8 @@ lpfc_sli4_async_fc_evt(struct lpfc_hba *phba, struct lpfc_acqe_fc_la *acqe_fc)
 	}
 	/* Keep the link status for extra SLI4 state machine reference */
 	phba->sli4_hba.link_state.speed =
-				bf_get(lpfc_acqe_fc_la_speed, acqe_fc);
+			lpfc_sli4_port_speed_parse(phba, LPFC_TRAILER_CODE_FC,
+				bf_get(lpfc_acqe_fc_la_speed, acqe_fc));
 	phba->sli4_hba.link_state.duplex = LPFC_ASYNC_LINK_DUPLEX_FULL;
 	phba->sli4_hba.link_state.topology =
 				bf_get(lpfc_acqe_fc_la_topology, acqe_fc);
@@ -3665,7 +3787,7 @@ lpfc_sli4_async_fc_evt(struct lpfc_hba *phba, struct lpfc_acqe_fc_la *acqe_fc)
 	phba->sli4_hba.link_state.fault =
 				bf_get(lpfc_acqe_link_fault, acqe_fc);
 	phba->sli4_hba.link_state.logical_speed =
-				bf_get(lpfc_acqe_fc_la_llink_spd, acqe_fc);
+				bf_get(lpfc_acqe_fc_la_llink_spd, acqe_fc) * 10;
 	lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
 			"2896 Async FC event - Speed:%dGBaud Topology:x%x "
 			"LA Type:x%x Port Type:%d Port Number:%d Logical speed:"
@@ -3675,7 +3797,7 @@ lpfc_sli4_async_fc_evt(struct lpfc_hba *phba, struct lpfc_acqe_fc_la *acqe_fc)
 			phba->sli4_hba.link_state.status,
 			phba->sli4_hba.link_state.type,
 			phba->sli4_hba.link_state.number,
-			phba->sli4_hba.link_state.logical_speed * 10,
+			phba->sli4_hba.link_state.logical_speed,
 			phba->sli4_hba.link_state.fault);
 	pmb = (LPFC_MBOXQ_t *)mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
 	if (!pmb) {
@@ -3783,14 +3905,18 @@ lpfc_sli4_async_sli_evt(struct lpfc_hba *phba, struct lpfc_acqe_sli *acqe_sli)
 	case LPFC_SLI_EVENT_STATUS_VALID:
 		return; /* no message if the sfp is okay */
 	case LPFC_SLI_EVENT_STATUS_NOT_PRESENT:
-		sprintf(message, "Not installed");
+		sprintf(message, "Optics faulted/incorrectly installed/not " \
+				"installed - Reseat optics, if issue not "
+				"resolved, replace.");
 		break;
 	case LPFC_SLI_EVENT_STATUS_WRONG_TYPE:
 		sprintf(message,
-			"Optics of two types installed");
+			"Optics of two types installed - Remove one optic or " \
+			"install matching pair of optics.");
 		break;
 	case LPFC_SLI_EVENT_STATUS_UNSUPPORTED:
-		sprintf(message, "Incompatible optics");
+		sprintf(message, "Incompatible optics - Replace with " \
+				"compatible optics for card to function.");
 		break;
 	default:
 		/* firmware is reporting a status we don't know about */
@@ -4161,11 +4287,11 @@ lpfc_sli4_async_grp5_evt(struct lpfc_hba *phba,
 	phba->fcoe_eventtag = acqe_grp5->event_tag;
 	prev_ll_spd = phba->sli4_hba.link_state.logical_speed;
 	phba->sli4_hba.link_state.logical_speed =
-		(bf_get(lpfc_acqe_grp5_llink_spd, acqe_grp5));
+		(bf_get(lpfc_acqe_grp5_llink_spd, acqe_grp5)) * 10;
 	lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
 			"2789 GRP5 Async Event: Updating logical link speed "
-			"from %dMbps to %dMbps\n", (prev_ll_spd * 10),
-			(phba->sli4_hba.link_state.logical_speed*10));
+			"from %dMbps to %dMbps\n", prev_ll_spd,
+			phba->sli4_hba.link_state.logical_speed);
 }
 
 /**
@@ -4947,7 +5073,7 @@ lpfc_sli4_driver_resource_setup(struct lpfc_hba *phba)
 	}
 
 	phba->sli4_hba.msix_entries = kzalloc((sizeof(struct msix_entry) *
-				      phba->sli4_hba.cfg_eqn), GFP_KERNEL);
+				      phba->cfg_fcp_io_channel), GFP_KERNEL);
 	if (!phba->sli4_hba.msix_entries) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 				"2573 Failed allocate memory for msi-x "
@@ -6559,7 +6685,8 @@ lpfc_sli4_queue_verify(struct lpfc_hba *phba)
 		i++;
 	}
 	if (i < cfg_fcp_io_channel) {
-		lpfc_printf_log(phba, KERN_WARNING, LOG_INIT,
+		lpfc_printf_log(phba,
+				KERN_ERR, LOG_INIT,
 				"3188 Reducing IO channels to match number of "
 				"CPUs: from %d to %d\n", cfg_fcp_io_channel, i);
 		cfg_fcp_io_channel = i;
@@ -6567,8 +6694,8 @@ lpfc_sli4_queue_verify(struct lpfc_hba *phba)
 
 	if (cfg_fcp_io_channel >
 	    phba->sli4_hba.max_cfg_param.max_eq) {
-		cfg_fcp_io_channel = phba->sli4_hba.max_cfg_param.max_eq;
-		if (cfg_fcp_io_channel < LPFC_FCP_IO_CHAN_MIN) {
+		if (phba->sli4_hba.max_cfg_param.max_eq <
+		    LPFC_FCP_IO_CHAN_MIN) {
 			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 					"2574 Not enough EQs (%d) from the "
 					"pci function for supporting FCP "
@@ -6577,13 +6704,12 @@ lpfc_sli4_queue_verify(struct lpfc_hba *phba)
 					phba->cfg_fcp_io_channel);
 			goto out_error;
 		}
-		lpfc_printf_log(phba, KERN_WARNING, LOG_INIT,
-				"2575 Not enough EQs (%d) from the pci "
-				"function for supporting the requested "
-				"FCP EQs (%d), the actual FCP EQs can "
-				"be supported: %d\n",
-				phba->sli4_hba.max_cfg_param.max_eq,
-				phba->cfg_fcp_io_channel, cfg_fcp_io_channel);
+		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+				"2575 Reducing IO channels to match number of "
+				"available EQs: from %d to %d\n",
+				cfg_fcp_io_channel,
+				phba->sli4_hba.max_cfg_param.max_eq);
+		cfg_fcp_io_channel = phba->sli4_hba.max_cfg_param.max_eq;
 	}
 
 	/* Eventually cfg_fcp_eq_count / cfg_fcp_wq_count will be depricated */
@@ -6592,7 +6718,6 @@ lpfc_sli4_queue_verify(struct lpfc_hba *phba)
 	phba->cfg_fcp_eq_count = cfg_fcp_io_channel;
 	phba->cfg_fcp_wq_count = cfg_fcp_io_channel;
 	phba->cfg_fcp_io_channel = cfg_fcp_io_channel;
-	phba->sli4_hba.cfg_eqn = cfg_fcp_io_channel;
 
 	/* Get EQ depth from module parameter, fake the default for now */
 	phba->sli4_hba.eq_esize = LPFC_EQE_SIZE_4B;
@@ -8095,11 +8220,11 @@ lpfc_sli4_enable_msix(struct lpfc_hba *phba)
 	int vectors, rc, index;
 
 	/* Set up MSI-X multi-message vectors */
-	for (index = 0; index < phba->sli4_hba.cfg_eqn; index++)
+	for (index = 0; index < phba->cfg_fcp_io_channel; index++)
 		phba->sli4_hba.msix_entries[index].entry = index;
 
 	/* Configure MSI-X capability structure */
-	vectors = phba->sli4_hba.cfg_eqn;
+	vectors = phba->cfg_fcp_io_channel;
 enable_msix_vectors:
 	rc = pci_enable_msix(phba->pcidev, phba->sli4_hba.msix_entries,
 			     vectors);
@@ -8142,8 +8267,14 @@ enable_msix_vectors:
 			goto cfg_fail_out;
 		}
 	}
-	phba->sli4_hba.msix_vec_nr = vectors;
 
+	if (vectors != phba->cfg_fcp_io_channel) {
+		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+				"3238 Reducing IO channels to match number of "
+				"MSI-X vectors, requested %d got %d\n",
+				phba->cfg_fcp_io_channel, vectors);
+		phba->cfg_fcp_io_channel = vectors;
+	}
 	return rc;
 
 cfg_fail_out:
@@ -8171,7 +8302,7 @@ lpfc_sli4_disable_msix(struct lpfc_hba *phba)
 	int index;
 
 	/* Free up MSI-X multi-message vectors */
-	for (index = 0; index < phba->sli4_hba.msix_vec_nr; index++)
+	for (index = 0; index < phba->cfg_fcp_io_channel; index++)
 		free_irq(phba->sli4_hba.msix_entries[index].vector,
 			 &phba->sli4_hba.fcp_eq_hdl[index]);
 
@@ -9304,23 +9435,28 @@ lpfc_sli4_get_els_iocb_cnt(struct lpfc_hba *phba)
 
 /**
  * lpfc_write_firmware - attempt to write a firmware image to the port
- * @phba: pointer to lpfc hba data structure.
  * @fw: pointer to firmware image returned from request_firmware.
+ * @phba: pointer to lpfc hba data structure.
  *
- * returns the number of bytes written if write is successful.
- * returns a negative error value if there were errors.
- * returns 0 if firmware matches currently active firmware on port.
  **/
-int
-lpfc_write_firmware(struct lpfc_hba *phba, const struct firmware *fw)
+static void
+lpfc_write_firmware(const struct firmware *fw, void *context)
 {
+	struct lpfc_hba *phba = (struct lpfc_hba *)context;
 	char fwrev[FW_REV_STR_SIZE];
-	struct lpfc_grp_hdr *image = (struct lpfc_grp_hdr *)fw->data;
+	struct lpfc_grp_hdr *image;
 	struct list_head dma_buffer_list;
 	int i, rc = 0;
 	struct lpfc_dmabuf *dmabuf, *next;
 	uint32_t offset = 0, temp_offset = 0;
 
+	/* It can be null, sanity check */
+	if (!fw) {
+		rc = -ENXIO;
+		goto out;
+	}
+	image = (struct lpfc_grp_hdr *)fw->data;
+
 	INIT_LIST_HEAD(&dma_buffer_list);
 	if ((be32_to_cpu(image->magic_number) != LPFC_GROUP_OJECT_MAGIC_NUM) ||
 	    (bf_get_be32(lpfc_grp_hdr_file_type, image) !=
@@ -9333,12 +9469,13 @@ lpfc_write_firmware(struct lpfc_hba *phba, const struct firmware *fw)
 				be32_to_cpu(image->magic_number),
 				bf_get_be32(lpfc_grp_hdr_file_type, image),
 				bf_get_be32(lpfc_grp_hdr_id, image));
-		return -EINVAL;
+		rc = -EINVAL;
+		goto release_out;
 	}
 	lpfc_decode_firmware_rev(phba, fwrev, 1);
 	if (strncmp(fwrev, image->revision, strnlen(image->revision, 16))) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
-				"3023 Updating Firmware. Current Version:%s "
+				"3023 Updating Firmware, Current Version:%s "
 				"New Version:%s\n",
 				fwrev, image->revision);
 		for (i = 0; i < LPFC_MBX_WR_CONFIG_MAX_BDE; i++) {
@@ -9346,7 +9483,7 @@ lpfc_write_firmware(struct lpfc_hba *phba, const struct firmware *fw)
 					 GFP_KERNEL);
 			if (!dmabuf) {
 				rc = -ENOMEM;
-				goto out;
+				goto release_out;
 			}
 			dmabuf->virt = dma_alloc_coherent(&phba->pcidev->dev,
 							  SLI4_PAGE_SIZE,
@@ -9355,7 +9492,7 @@ lpfc_write_firmware(struct lpfc_hba *phba, const struct firmware *fw)
 			if (!dmabuf->virt) {
 				kfree(dmabuf);
 				rc = -ENOMEM;
-				goto out;
+				goto release_out;
 			}
 			list_add_tail(&dmabuf->list, &dma_buffer_list);
 		}
@@ -9375,23 +9512,24 @@ lpfc_write_firmware(struct lpfc_hba *phba, const struct firmware *fw)
 			}
 			rc = lpfc_wr_object(phba, &dma_buffer_list,
 				    (fw->size - offset), &offset);
-			if (rc) {
-				lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
-						"3024 Firmware update failed. "
-						"%d\n", rc);
-				goto out;
-			}
+			if (rc)
+				goto release_out;
 		}
 		rc = offset;
 	}
-out:
+
+release_out:
 	list_for_each_entry_safe(dmabuf, next, &dma_buffer_list, list) {
 		list_del(&dmabuf->list);
 		dma_free_coherent(&phba->pcidev->dev, SLI4_PAGE_SIZE,
 				  dmabuf->virt, dmabuf->phys);
 		kfree(dmabuf);
 	}
-	return rc;
+	release_firmware(fw);
+out:
+	lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+			"3024 Firmware update done: %d.", rc);
+	return;
 }
 
 /**
@@ -9418,12 +9556,11 @@ lpfc_pci_probe_one_s4(struct pci_dev *pdev, const struct pci_device_id *pid)
 	struct lpfc_hba   *phba;
 	struct lpfc_vport *vport = NULL;
 	struct Scsi_Host  *shost = NULL;
-	int error;
+	int error, ret;
 	uint32_t cfg_mode, intr_mode;
 	int mcnt;
 	int adjusted_fcp_io_channel;
-	const struct firmware *fw;
-	uint8_t file_name[16];
+	uint8_t file_name[ELX_MODEL_NAME_SIZE];
 
 	/* Allocate memory for HBA structure */
 	phba = lpfc_hba_alloc(pdev);
@@ -9525,9 +9662,6 @@ lpfc_pci_probe_one_s4(struct pci_dev *pdev, const struct pci_device_id *pid)
 		/* Default to single EQ for non-MSI-X */
 		if (phba->intr_type != MSIX)
 			adjusted_fcp_io_channel = 1;
-		else if (phba->sli4_hba.msix_vec_nr <
-					phba->cfg_fcp_io_channel)
-			adjusted_fcp_io_channel = phba->sli4_hba.msix_vec_nr;
 		else
 			adjusted_fcp_io_channel = phba->cfg_fcp_io_channel;
 		phba->cfg_fcp_io_channel = adjusted_fcp_io_channel;
@@ -9572,12 +9706,12 @@ lpfc_pci_probe_one_s4(struct pci_dev *pdev, const struct pci_device_id *pid)
 	/* check for firmware upgrade or downgrade (if_type 2 only) */
 	if (bf_get(lpfc_sli_intf_if_type, &phba->sli4_hba.sli_intf) ==
 	    LPFC_SLI_INTF_IF_TYPE_2) {
-		snprintf(file_name, 16, "%s.grp", phba->ModelName);
-		error = request_firmware(&fw, file_name, &phba->pcidev->dev);
-		if (!error) {
-			lpfc_write_firmware(phba, fw);
-			release_firmware(fw);
-		}
+		snprintf(file_name, ELX_MODEL_NAME_SIZE, "%s.grp",
+			 phba->ModelName);
+		ret = request_firmware_nowait(THIS_MODULE, FW_ACTION_HOTPLUG,
+					file_name, &phba->pcidev->dev,
+					GFP_KERNEL, (void *)phba,
+					lpfc_write_firmware);
 	}
 
 	/* Check if there are static vports to be created. */
diff --git a/drivers/scsi/lpfc/lpfc_scsi.c b/drivers/scsi/lpfc/lpfc_scsi.c
index 64013f3097a..7f45ac9964a 100644
--- a/drivers/scsi/lpfc/lpfc_scsi.c
+++ b/drivers/scsi/lpfc/lpfc_scsi.c
@@ -3829,9 +3829,9 @@ lpfc_scsi_cmd_iocb_cmpl(struct lpfc_hba *phba, struct lpfc_iocbq *pIocbIn,
 	cmd->scsi_done(cmd);
 
 	if (phba->cfg_poll & ENABLE_FCP_RING_POLLING) {
-		spin_lock_irq(&phba->hbalock);
+		spin_lock_irqsave(&phba->hbalock, flags);
 		lpfc_cmd->pCmd = NULL;
-		spin_unlock_irq(&phba->hbalock);
+		spin_unlock_irqrestore(&phba->hbalock, flags);
 
 		/*
 		 * If there is a thread waiting for command completion
@@ -3871,9 +3871,9 @@ lpfc_scsi_cmd_iocb_cmpl(struct lpfc_hba *phba, struct lpfc_iocbq *pIocbIn,
 		}
 	}
 
-	spin_lock_irq(&phba->hbalock);
+	spin_lock_irqsave(&phba->hbalock, flags);
 	lpfc_cmd->pCmd = NULL;
-	spin_unlock_irq(&phba->hbalock);
+	spin_unlock_irqrestore(&phba->hbalock, flags);
 
 	/*
 	 * If there is a thread waiting for command completion
@@ -4163,7 +4163,7 @@ lpfc_info(struct Scsi_Host *host)
 {
 	struct lpfc_vport *vport = (struct lpfc_vport *) host->hostdata;
 	struct lpfc_hba   *phba = vport->phba;
-	int len;
+	int len, link_speed = 0;
 	static char  lpfcinfobuf[384];
 
 	memset(lpfcinfobuf,0,384);
@@ -4184,12 +4184,18 @@ lpfc_info(struct Scsi_Host *host)
 				 phba->Port);
 		}
 		len = strlen(lpfcinfobuf);
-		if (phba->sli4_hba.link_state.logical_speed) {
-			snprintf(lpfcinfobuf + len,
-				 384-len,
-				 " Logical Link Speed: %d Mbps",
-				 phba->sli4_hba.link_state.logical_speed * 10);
+		if (phba->sli_rev <= LPFC_SLI_REV3) {
+			link_speed = lpfc_sli_port_speed_get(phba);
+		} else {
+			if (phba->sli4_hba.link_state.logical_speed)
+				link_speed =
+				      phba->sli4_hba.link_state.logical_speed;
+			else
+				link_speed = phba->sli4_hba.link_state.speed;
 		}
+		if (link_speed != 0)
+			snprintf(lpfcinfobuf + len, 384-len,
+				 " Logical Link Speed: %d Mbps", link_speed);
 	}
 	return lpfcinfobuf;
 }
@@ -4398,16 +4404,17 @@ lpfc_abort_handler(struct scsi_cmnd *cmnd)
 	struct lpfc_scsi_buf *lpfc_cmd;
 	IOCB_t *cmd, *icmd;
 	int ret = SUCCESS, status = 0;
+	unsigned long flags;
 	DECLARE_WAIT_QUEUE_HEAD_ONSTACK(waitq);
 
 	status = fc_block_scsi_eh(cmnd);
 	if (status != 0 && status != SUCCESS)
 		return status;
 
-	spin_lock_irq(&phba->hbalock);
+	spin_lock_irqsave(&phba->hbalock, flags);
 	/* driver queued commands are in process of being flushed */
 	if (phba->hba_flag & HBA_FCP_IOQ_FLUSH) {
-		spin_unlock_irq(&phba->hbalock);
+		spin_unlock_irqrestore(&phba->hbalock, flags);
 		lpfc_printf_vlog(vport, KERN_WARNING, LOG_FCP,
 			"3168 SCSI Layer abort requested I/O has been "
 			"flushed by LLD.\n");
@@ -4416,7 +4423,7 @@ lpfc_abort_handler(struct scsi_cmnd *cmnd)
 
 	lpfc_cmd = (struct lpfc_scsi_buf *)cmnd->host_scribble;
 	if (!lpfc_cmd || !lpfc_cmd->pCmd) {
-		spin_unlock_irq(&phba->hbalock);
+		spin_unlock_irqrestore(&phba->hbalock, flags);
 		lpfc_printf_vlog(vport, KERN_WARNING, LOG_FCP,
 			 "2873 SCSI Layer I/O Abort Request IO CMPL Status "
 			 "x%x ID %d LUN %d\n",
@@ -4427,7 +4434,7 @@ lpfc_abort_handler(struct scsi_cmnd *cmnd)
 	iocb = &lpfc_cmd->cur_iocbq;
 	/* the command is in process of being cancelled */
 	if (!(iocb->iocb_flag & LPFC_IO_ON_TXCMPLQ)) {
-		spin_unlock_irq(&phba->hbalock);
+		spin_unlock_irqrestore(&phba->hbalock, flags);
 		lpfc_printf_vlog(vport, KERN_WARNING, LOG_FCP,
 			"3169 SCSI Layer abort requested I/O has been "
 			"cancelled by LLD.\n");
@@ -4484,7 +4491,7 @@ lpfc_abort_handler(struct scsi_cmnd *cmnd)
 	abtsiocb->iocb_cmpl = lpfc_sli_abort_fcp_cmpl;
 	abtsiocb->vport = vport;
 	/* no longer need the lock after this point */
-	spin_unlock_irq(&phba->hbalock);
+	spin_unlock_irqrestore(&phba->hbalock, flags);
 
 	if (lpfc_sli_issue_iocb(phba, LPFC_FCP_RING, abtsiocb, 0) ==
 	    IOCB_ERROR) {
@@ -4516,7 +4523,7 @@ lpfc_abort_handler(struct scsi_cmnd *cmnd)
 	goto out;
 
 out_unlock:
-	spin_unlock_irq(&phba->hbalock);
+	spin_unlock_irqrestore(&phba->hbalock, flags);
 out:
 	lpfc_printf_vlog(vport, KERN_WARNING, LOG_FCP,
 			 "0749 SCSI Layer I/O Abort Request Status x%x ID %d "
diff --git a/drivers/scsi/lpfc/lpfc_sli.c b/drivers/scsi/lpfc/lpfc_sli.c
index 219bf534ef9..d7f3313ef88 100644
--- a/drivers/scsi/lpfc/lpfc_sli.c
+++ b/drivers/scsi/lpfc/lpfc_sli.c
@@ -3964,9 +3964,9 @@ lpfc_sli4_brdreset(struct lpfc_hba *phba)
 	pci_write_config_word(phba->pcidev, PCI_COMMAND, (cfg_value &
 			      ~(PCI_COMMAND_PARITY | PCI_COMMAND_SERR)));
 
-	/* Perform FCoE PCI function reset */
-	lpfc_sli4_queue_destroy(phba);
+	/* Perform FCoE PCI function reset before freeing queue memory */
 	rc = lpfc_pci_function_reset(phba);
+	lpfc_sli4_queue_destroy(phba);
 
 	/* Restore PCI cmd register */
 	pci_write_config_word(phba->pcidev, PCI_COMMAND, cfg_value);
@@ -7072,6 +7072,40 @@ lpfc_sli4_async_mbox_unblock(struct lpfc_hba *phba)
 }
 
 /**
+ * lpfc_sli4_wait_bmbx_ready - Wait for bootstrap mailbox register ready
+ * @phba: Pointer to HBA context object.
+ * @mboxq: Pointer to mailbox object.
+ *
+ * The function waits for the bootstrap mailbox register ready bit from
+ * port for twice the regular mailbox command timeout value.
+ *
+ *      0 - no timeout on waiting for bootstrap mailbox register ready.
+ *      MBXERR_ERROR - wait for bootstrap mailbox register timed out.
+ **/
+static int
+lpfc_sli4_wait_bmbx_ready(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
+{
+	uint32_t db_ready;
+	unsigned long timeout;
+	struct lpfc_register bmbx_reg;
+
+	timeout = msecs_to_jiffies(lpfc_mbox_tmo_val(phba, mboxq)
+				   * 1000) + jiffies;
+
+	do {
+		bmbx_reg.word0 = readl(phba->sli4_hba.BMBXregaddr);
+		db_ready = bf_get(lpfc_bmbx_rdy, &bmbx_reg);
+		if (!db_ready)
+			msleep(2);
+
+		if (time_after(jiffies, timeout))
+			return MBXERR_ERROR;
+	} while (!db_ready);
+
+	return 0;
+}
+
+/**
  * lpfc_sli4_post_sync_mbox - Post an SLI4 mailbox to the bootstrap mailbox
  * @phba: Pointer to HBA context object.
  * @mboxq: Pointer to mailbox object.
@@ -7092,15 +7126,12 @@ lpfc_sli4_post_sync_mbox(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 {
 	int rc = MBX_SUCCESS;
 	unsigned long iflag;
-	uint32_t db_ready;
 	uint32_t mcqe_status;
 	uint32_t mbx_cmnd;
-	unsigned long timeout;
 	struct lpfc_sli *psli = &phba->sli;
 	struct lpfc_mqe *mb = &mboxq->u.mqe;
 	struct lpfc_bmbx_create *mbox_rgn;
 	struct dma_address *dma_address;
-	struct lpfc_register bmbx_reg;
 
 	/*
 	 * Only one mailbox can be active to the bootstrap mailbox region
@@ -7124,6 +7155,11 @@ lpfc_sli4_post_sync_mbox(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 	phba->sli.mbox_active = mboxq;
 	spin_unlock_irqrestore(&phba->hbalock, iflag);
 
+	/* wait for bootstrap mbox register for readyness */
+	rc = lpfc_sli4_wait_bmbx_ready(phba, mboxq);
+	if (rc)
+		goto exit;
+
 	/*
 	 * Initialize the bootstrap memory region to avoid stale data areas
 	 * in the mailbox post.  Then copy the caller's mailbox contents to
@@ -7138,35 +7174,18 @@ lpfc_sli4_post_sync_mbox(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 	dma_address = &phba->sli4_hba.bmbx.dma_address;
 	writel(dma_address->addr_hi, phba->sli4_hba.BMBXregaddr);
 
-	timeout = msecs_to_jiffies(lpfc_mbox_tmo_val(phba, mboxq)
-				   * 1000) + jiffies;
-	do {
-		bmbx_reg.word0 = readl(phba->sli4_hba.BMBXregaddr);
-		db_ready = bf_get(lpfc_bmbx_rdy, &bmbx_reg);
-		if (!db_ready)
-			msleep(2);
-
-		if (time_after(jiffies, timeout)) {
-			rc = MBXERR_ERROR;
-			goto exit;
-		}
-	} while (!db_ready);
+	/* wait for bootstrap mbox register for hi-address write done */
+	rc = lpfc_sli4_wait_bmbx_ready(phba, mboxq);
+	if (rc)
+		goto exit;
 
 	/* Post the low mailbox dma address to the port. */
 	writel(dma_address->addr_lo, phba->sli4_hba.BMBXregaddr);
-	timeout = msecs_to_jiffies(lpfc_mbox_tmo_val(phba, mboxq)
-				   * 1000) + jiffies;
-	do {
-		bmbx_reg.word0 = readl(phba->sli4_hba.BMBXregaddr);
-		db_ready = bf_get(lpfc_bmbx_rdy, &bmbx_reg);
-		if (!db_ready)
-			msleep(2);
 
-		if (time_after(jiffies, timeout)) {
-			rc = MBXERR_ERROR;
-			goto exit;
-		}
-	} while (!db_ready);
+	/* wait for bootstrap mbox register for low address write done */
+	rc = lpfc_sli4_wait_bmbx_ready(phba, mboxq);
+	if (rc)
+		goto exit;
 
 	/*
 	 * Read the CQ to ensure the mailbox has completed.
@@ -8090,6 +8109,8 @@ lpfc_sli4_iocb2wqe(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq,
 		bf_set(wqe_lenloc, &wqe->fcp_icmd.wqe_com,
 		       LPFC_WQE_LENLOC_NONE);
 		bf_set(wqe_ebde_cnt, &wqe->fcp_icmd.wqe_com, 0);
+		bf_set(wqe_erp, &wqe->fcp_icmd.wqe_com,
+		       iocbq->iocb.ulpFCP2Rcvy);
 		break;
 	case CMD_GEN_REQUEST64_CR:
 		/* For this command calculate the xmit length of the
@@ -12099,6 +12120,7 @@ lpfc_modify_fcp_eq_delay(struct lpfc_hba *phba, uint16_t startq)
 	struct lpfc_queue *eq;
 	int cnt, rc, length, status = 0;
 	uint32_t shdr_status, shdr_add_status;
+	uint32_t result;
 	int fcp_eqidx;
 	union lpfc_sli4_cfg_shdr *shdr;
 	uint16_t dmult;
@@ -12117,8 +12139,11 @@ lpfc_modify_fcp_eq_delay(struct lpfc_hba *phba, uint16_t startq)
 	eq_delay = &mbox->u.mqe.un.eq_delay;
 
 	/* Calculate delay multiper from maximum interrupt per second */
-	dmult = phba->cfg_fcp_imax / phba->cfg_fcp_io_channel;
-	dmult = LPFC_DMULT_CONST/dmult - 1;
+	result = phba->cfg_fcp_imax / phba->cfg_fcp_io_channel;
+	if (result > LPFC_DMULT_CONST)
+		dmult = 0;
+	else
+		dmult = LPFC_DMULT_CONST/result - 1;
 
 	cnt = 0;
 	for (fcp_eqidx = startq; fcp_eqidx < phba->cfg_fcp_io_channel;
@@ -12174,7 +12199,7 @@ lpfc_modify_fcp_eq_delay(struct lpfc_hba *phba, uint16_t startq)
  * fails this function will return -ENXIO.
  **/
 uint32_t
-lpfc_eq_create(struct lpfc_hba *phba, struct lpfc_queue *eq, uint16_t imax)
+lpfc_eq_create(struct lpfc_hba *phba, struct lpfc_queue *eq, uint32_t imax)
 {
 	struct lpfc_mbx_eq_create *eq_create;
 	LPFC_MBOXQ_t *mbox;
@@ -12206,7 +12231,10 @@ lpfc_eq_create(struct lpfc_hba *phba, struct lpfc_queue *eq, uint16_t imax)
 	       LPFC_EQE_SIZE);
 	bf_set(lpfc_eq_context_valid, &eq_create->u.request.context, 1);
 	/* Calculate delay multiper from maximum interrupt per second */
-	dmult = LPFC_DMULT_CONST/imax - 1;
+	if (imax > LPFC_DMULT_CONST)
+		dmult = 0;
+	else
+		dmult = LPFC_DMULT_CONST/imax - 1;
 	bf_set(lpfc_eq_context_delay_multi, &eq_create->u.request.context,
 	       dmult);
 	switch (eq->entry_count) {
diff --git a/drivers/scsi/lpfc/lpfc_sli4.h b/drivers/scsi/lpfc/lpfc_sli4.h
index bd4bc4342ae..f44a06a4c6e 100644
--- a/drivers/scsi/lpfc/lpfc_sli4.h
+++ b/drivers/scsi/lpfc/lpfc_sli4.h
@@ -37,7 +37,7 @@
 /* Multi-queue arrangement for FCP EQ/CQ/WQ tuples */
 #define LPFC_FCP_IO_CHAN_DEF       4
 #define LPFC_FCP_IO_CHAN_MIN       1
-#define LPFC_FCP_IO_CHAN_MAX       8
+#define LPFC_FCP_IO_CHAN_MAX       16
 
 /*
  * Provide the default FCF Record attributes used by the driver
@@ -168,7 +168,7 @@ struct lpfc_queue {
 };
 
 struct lpfc_sli4_link {
-	uint8_t speed;
+	uint16_t speed;
 	uint8_t duplex;
 	uint8_t status;
 	uint8_t type;
@@ -490,8 +490,6 @@ struct lpfc_sli4_hba {
 	struct lpfc_pc_sli4_params pc_sli4_params;
 	struct msix_entry *msix_entries;
 	uint8_t handler_name[LPFC_FCP_IO_CHAN_MAX][LPFC_SLI4_HANDLER_NAME_SZ];
-	uint32_t cfg_eqn;
-	uint32_t msix_vec_nr;
 	struct lpfc_fcp_eq_hdl *fcp_eq_hdl; /* FCP per-WQ handle */
 
 	/* Pointers to the constructed SLI4 queues */
@@ -626,7 +624,7 @@ void lpfc_sli4_hba_reset(struct lpfc_hba *);
 struct lpfc_queue *lpfc_sli4_queue_alloc(struct lpfc_hba *, uint32_t,
 			uint32_t);
 void lpfc_sli4_queue_free(struct lpfc_queue *);
-uint32_t lpfc_eq_create(struct lpfc_hba *, struct lpfc_queue *, uint16_t);
+uint32_t lpfc_eq_create(struct lpfc_hba *, struct lpfc_queue *, uint32_t);
 uint32_t lpfc_modify_fcp_eq_delay(struct lpfc_hba *, uint16_t);
 uint32_t lpfc_cq_create(struct lpfc_hba *, struct lpfc_queue *,
 			struct lpfc_queue *, uint32_t, uint32_t);
diff --git a/drivers/scsi/lpfc/lpfc_version.h b/drivers/scsi/lpfc/lpfc_version.h
index 04265a1c4e5..0c2149189dd 100644
--- a/drivers/scsi/lpfc/lpfc_version.h
+++ b/drivers/scsi/lpfc/lpfc_version.h
@@ -18,7 +18,7 @@
  * included with this package.                                     *
  *******************************************************************/
 
-#define LPFC_DRIVER_VERSION "8.3.34"
+#define LPFC_DRIVER_VERSION "8.3.35"
 #define LPFC_DRIVER_NAME		"lpfc"
 
 /* Used for SLI 2/3 */
diff --git a/drivers/scsi/megaraid/megaraid_sas.h b/drivers/scsi/megaraid/megaraid_sas.h
index fcb005fa4bd..16b7a72a70c 100644
--- a/drivers/scsi/megaraid/megaraid_sas.h
+++ b/drivers/scsi/megaraid/megaraid_sas.h
@@ -1,7 +1,7 @@
 /*
  *  Linux MegaRAID driver for SAS based RAID controllers
  *
- *  Copyright (c) 2009-2011  LSI Corporation.
+ *  Copyright (c) 2003-2012  LSI Corporation.
  *
  *  This program is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU General Public License
@@ -33,9 +33,9 @@
 /*
  * MegaRAID SAS Driver meta data
  */
-#define MEGASAS_VERSION				"00.00.06.18-rc1"
-#define MEGASAS_RELDATE				"Jun. 17, 2012"
-#define MEGASAS_EXT_VERSION			"Tue. Jun. 17 17:00:00 PDT 2012"
+#define MEGASAS_VERSION				"06.504.01.00-rc1"
+#define MEGASAS_RELDATE				"Oct. 1, 2012"
+#define MEGASAS_EXT_VERSION			"Mon. Oct. 1 17:00:00 PDT 2012"
 
 /*
  * Device IDs
diff --git a/drivers/scsi/megaraid/megaraid_sas_base.c b/drivers/scsi/megaraid/megaraid_sas_base.c
index 0393ec478cd..d2c5366aff7 100644
--- a/drivers/scsi/megaraid/megaraid_sas_base.c
+++ b/drivers/scsi/megaraid/megaraid_sas_base.c
@@ -1,7 +1,7 @@
 /*
  *  Linux MegaRAID driver for SAS based RAID controllers
  *
- *  Copyright (c) 2009-2011  LSI Corporation.
+ *  Copyright (c) 2003-2012  LSI Corporation.
  *
  *  This program is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU General Public License
@@ -18,7 +18,7 @@
  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  *
  *  FILE: megaraid_sas_base.c
- *  Version : v00.00.06.18-rc1
+ *  Version : v06.504.01.00-rc1
  *
  *  Authors: LSI Corporation
  *           Sreenivas Bagalkote
@@ -71,6 +71,10 @@ static int msix_disable;
 module_param(msix_disable, int, S_IRUGO);
 MODULE_PARM_DESC(msix_disable, "Disable MSI-X interrupt handling. Default: 0");
 
+static unsigned int msix_vectors;
+module_param(msix_vectors, int, S_IRUGO);
+MODULE_PARM_DESC(msix_vectors, "MSI-X max vector count. Default: Set by FW");
+
 static int throttlequeuedepth = MEGASAS_THROTTLE_QUEUE_DEPTH;
 module_param(throttlequeuedepth, int, S_IRUGO);
 MODULE_PARM_DESC(throttlequeuedepth,
@@ -3520,6 +3524,10 @@ static int megasas_init_fw(struct megasas_instance *instance)
 			instance->msix_vectors = (readl(&instance->reg_set->
 							outbound_scratch_pad_2
 							  ) & 0x1F) + 1;
+			if (msix_vectors)
+				instance->msix_vectors =
+					min(msix_vectors,
+					    instance->msix_vectors);
 		} else
 			instance->msix_vectors = 1;
 		/* Don't bother allocating more MSI-X vectors than cpus */
@@ -5233,7 +5241,6 @@ megasas_aen_polling(struct work_struct *work)
 
 		case MR_EVT_PD_REMOVED:
 			if (megasas_get_pd_list(instance) == 0) {
-			megasas_get_pd_list(instance);
 			for (i = 0; i < MEGASAS_MAX_PD_CHANNELS; i++) {
 				for (j = 0;
 				j < MEGASAS_MAX_DEV_PER_CHANNEL;
diff --git a/drivers/scsi/megaraid/megaraid_sas_fp.c b/drivers/scsi/megaraid/megaraid_sas_fp.c
index e3d251a2e26..a11df82474e 100644
--- a/drivers/scsi/megaraid/megaraid_sas_fp.c
+++ b/drivers/scsi/megaraid/megaraid_sas_fp.c
@@ -1,7 +1,7 @@
 /*
  *  Linux MegaRAID driver for SAS based RAID controllers
  *
- *  Copyright (c) 2009-2011  LSI Corporation.
+ *  Copyright (c) 2009-2012  LSI Corporation.
  *
  *  This program is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU General Public License
diff --git a/drivers/scsi/megaraid/megaraid_sas_fusion.c b/drivers/scsi/megaraid/megaraid_sas_fusion.c
index ddf094e7d0a..74030aff69a 100644
--- a/drivers/scsi/megaraid/megaraid_sas_fusion.c
+++ b/drivers/scsi/megaraid/megaraid_sas_fusion.c
@@ -1,7 +1,7 @@
 /*
  *  Linux MegaRAID driver for SAS based RAID controllers
  *
- *  Copyright (c) 2009-2011  LSI Corporation.
+ *  Copyright (c) 2009-2012  LSI Corporation.
  *
  *  This program is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU General Public License
@@ -1184,8 +1184,6 @@ megasas_set_pd_lba(struct MPI2_RAID_SCSI_IO_REQUEST *io_request, u8 cdb_len,
 		io_request->CDB.EEDP32.PrimaryReferenceTag =
 			cpu_to_be32(ref_tag);
 		io_request->CDB.EEDP32.PrimaryApplicationTagMask = 0xffff;
-
-		io_request->DataLength = num_blocks * 512;
 		io_request->IoFlags = 32; /* Specify 32-byte cdb */
 
 		/* Transfer length */
@@ -1329,7 +1327,7 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
 			  struct megasas_cmd_fusion *cmd)
 {
 	u8 fp_possible;
-	u32 start_lba_lo, start_lba_hi, device_id;
+	u32 start_lba_lo, start_lba_hi, device_id, datalength = 0;
 	struct MPI2_RAID_SCSI_IO_REQUEST *io_request;
 	union MEGASAS_REQUEST_DESCRIPTOR_UNION *req_desc;
 	struct IO_REQUEST_INFO io_info;
@@ -1355,7 +1353,7 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
 	 * 6-byte READ(0x08) or WRITE(0x0A) cdb
 	 */
 	if (scp->cmd_len == 6) {
-		io_request->DataLength = (u32) scp->cmnd[4];
+		datalength = (u32) scp->cmnd[4];
 		start_lba_lo = ((u32) scp->cmnd[1] << 16) |
 			((u32) scp->cmnd[2] << 8) | (u32) scp->cmnd[3];
 
@@ -1366,7 +1364,7 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
 	 * 10-byte READ(0x28) or WRITE(0x2A) cdb
 	 */
 	else if (scp->cmd_len == 10) {
-		io_request->DataLength = (u32) scp->cmnd[8] |
+		datalength = (u32) scp->cmnd[8] |
 			((u32) scp->cmnd[7] << 8);
 		start_lba_lo = ((u32) scp->cmnd[2] << 24) |
 			((u32) scp->cmnd[3] << 16) |
@@ -1377,7 +1375,7 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
 	 * 12-byte READ(0xA8) or WRITE(0xAA) cdb
 	 */
 	else if (scp->cmd_len == 12) {
-		io_request->DataLength = ((u32) scp->cmnd[6] << 24) |
+		datalength = ((u32) scp->cmnd[6] << 24) |
 			((u32) scp->cmnd[7] << 16) |
 			((u32) scp->cmnd[8] << 8) | (u32) scp->cmnd[9];
 		start_lba_lo = ((u32) scp->cmnd[2] << 24) |
@@ -1389,7 +1387,7 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
 	 * 16-byte READ(0x88) or WRITE(0x8A) cdb
 	 */
 	else if (scp->cmd_len == 16) {
-		io_request->DataLength = ((u32) scp->cmnd[10] << 24) |
+		datalength = ((u32) scp->cmnd[10] << 24) |
 			((u32) scp->cmnd[11] << 16) |
 			((u32) scp->cmnd[12] << 8) | (u32) scp->cmnd[13];
 		start_lba_lo = ((u32) scp->cmnd[6] << 24) |
@@ -1403,8 +1401,9 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
 
 	memset(&io_info, 0, sizeof(struct IO_REQUEST_INFO));
 	io_info.ldStartBlock = ((u64)start_lba_hi << 32) | start_lba_lo;
-	io_info.numBlocks = io_request->DataLength;
+	io_info.numBlocks = datalength;
 	io_info.ldTgtId = device_id;
+	io_request->DataLength = scsi_bufflen(scp);
 
 	if (scp->sc_data_direction == PCI_DMA_FROMDEVICE)
 		io_info.isRead = 1;
@@ -1431,7 +1430,6 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
 	if (fp_possible) {
 		megasas_set_pd_lba(io_request, scp->cmd_len, &io_info, scp,
 				   local_map_ptr, start_lba_lo);
-		io_request->DataLength = scsi_bufflen(scp);
 		io_request->Function = MPI2_FUNCTION_SCSI_IO_REQUEST;
 		cmd->request_desc->SCSIIO.RequestFlags =
 			(MPI2_REQ_DESCRIPT_FLAGS_HIGH_PRIORITY
@@ -1510,7 +1508,8 @@ megasas_build_dcdb_fusion(struct megasas_instance *instance,
 	local_map_ptr = fusion->ld_map[(instance->map_id & 1)];
 
 	/* Check if this is a system PD I/O */
-	if (instance->pd_list[pd_index].driveState == MR_PD_STATE_SYSTEM) {
+	if (scmd->device->channel < MEGASAS_MAX_PD_CHANNELS &&
+	    instance->pd_list[pd_index].driveState == MR_PD_STATE_SYSTEM) {
 		io_request->Function = 0;
 		io_request->DevHandle =
 			local_map_ptr->raidMap.devHndlInfo[device_id].curDevHdl;
@@ -1525,6 +1524,8 @@ megasas_build_dcdb_fusion(struct megasas_instance *instance,
 		cmd->request_desc->SCSIIO.RequestFlags =
 			(MPI2_REQ_DESCRIPT_FLAGS_HIGH_PRIORITY <<
 			 MEGASAS_REQ_DESCRIPT_FLAGS_TYPE_SHIFT);
+		cmd->request_desc->SCSIIO.DevHandle =
+			local_map_ptr->raidMap.devHndlInfo[device_id].curDevHdl;
 	} else {
 		io_request->Function  = MEGASAS_MPI2_FUNCTION_LD_IO_REQUEST;
 		io_request->DevHandle = device_id;
@@ -1732,8 +1733,6 @@ complete_cmd_fusion(struct megasas_instance *instance, u32 MSIxIndex)
 	if (reply_descript_type == MPI2_RPY_DESCRIPT_FLAGS_UNUSED)
 		return IRQ_NONE;
 
-	d_val.word = desc->Words;
-
 	num_completed = 0;
 
 	while ((d_val.u.low != UINT_MAX) && (d_val.u.high != UINT_MAX)) {
@@ -1855,10 +1854,8 @@ megasas_complete_cmd_dpc_fusion(unsigned long instance_addr)
 	}
 	spin_unlock_irqrestore(&instance->hba_lock, flags);
 
-	spin_lock_irqsave(&instance->completion_lock, flags);
 	for (MSIxIndex = 0 ; MSIxIndex < count; MSIxIndex++)
 		complete_cmd_fusion(instance, MSIxIndex);
-	spin_unlock_irqrestore(&instance->completion_lock, flags);
 }
 
 /**
diff --git a/drivers/scsi/megaraid/megaraid_sas_fusion.h b/drivers/scsi/megaraid/megaraid_sas_fusion.h
index 088c9f91da9..a7c64f05199 100644
--- a/drivers/scsi/megaraid/megaraid_sas_fusion.h
+++ b/drivers/scsi/megaraid/megaraid_sas_fusion.h
@@ -1,7 +1,7 @@
 /*
  *  Linux MegaRAID driver for SAS based RAID controllers
  *
- *  Copyright (c) 2009-2011  LSI Corporation.
+ *  Copyright (c) 2009-2012  LSI Corporation.
  *
  *  This program is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU General Public License
diff --git a/drivers/scsi/mvumi.c b/drivers/scsi/mvumi.c
index 783edc7c6b9..c585a925b3c 100644
--- a/drivers/scsi/mvumi.c
+++ b/drivers/scsi/mvumi.c
@@ -35,10 +35,12 @@
 #include <linux/io.h>
 #include <scsi/scsi.h>
 #include <scsi/scsi_cmnd.h>
+#include <scsi/scsi_device.h>
 #include <scsi/scsi_host.h>
 #include <scsi/scsi_transport.h>
 #include <scsi/scsi_eh.h>
 #include <linux/uaccess.h>
+#include <linux/kthread.h>
 
 #include "mvumi.h"
 
@@ -48,6 +50,7 @@ MODULE_DESCRIPTION("Marvell UMI Driver");
 
 static DEFINE_PCI_DEVICE_TABLE(mvumi_pci_table) = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL_2, PCI_DEVICE_ID_MARVELL_MV9143) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL_2, PCI_DEVICE_ID_MARVELL_MV9580) },
 	{ 0 }
 };
 
@@ -118,7 +121,7 @@ static int mvumi_map_pci_addr(struct pci_dev *dev, void **addr_array)
 static struct mvumi_res *mvumi_alloc_mem_resource(struct mvumi_hba *mhba,
 				enum resource_type type, unsigned int size)
 {
-	struct mvumi_res *res = kzalloc(sizeof(*res), GFP_KERNEL);
+	struct mvumi_res *res = kzalloc(sizeof(*res), GFP_ATOMIC);
 
 	if (!res) {
 		dev_err(&mhba->pdev->dev,
@@ -128,7 +131,7 @@ static struct mvumi_res *mvumi_alloc_mem_resource(struct mvumi_hba *mhba,
 
 	switch (type) {
 	case RESOURCE_CACHED_MEMORY:
-		res->virt_addr = kzalloc(size, GFP_KERNEL);
+		res->virt_addr = kzalloc(size, GFP_ATOMIC);
 		if (!res->virt_addr) {
 			dev_err(&mhba->pdev->dev,
 				"unable to allocate memory,size = %d.\n", size);
@@ -222,11 +225,11 @@ static int mvumi_make_sgl(struct mvumi_hba *mhba, struct scsi_cmnd *scmd,
 			m_sg->baseaddr_l = cpu_to_le32(lower_32_bits(busaddr));
 			m_sg->baseaddr_h = cpu_to_le32(upper_32_bits(busaddr));
 			m_sg->flags = 0;
-			m_sg->size = cpu_to_le32(sg_dma_len(&sg[i]));
+			sgd_setsz(mhba, m_sg, cpu_to_le32(sg_dma_len(&sg[i])));
 			if ((i + 1) == *sg_count)
-				m_sg->flags |= SGD_EOT;
+				m_sg->flags |= 1U << mhba->eot_flag;
 
-			m_sg++;
+			sgd_inc(mhba, m_sg);
 		}
 	} else {
 		scmd->SCp.dma_handle = scsi_bufflen(scmd) ?
@@ -237,8 +240,8 @@ static int mvumi_make_sgl(struct mvumi_hba *mhba, struct scsi_cmnd *scmd,
 		busaddr = scmd->SCp.dma_handle;
 		m_sg->baseaddr_l = cpu_to_le32(lower_32_bits(busaddr));
 		m_sg->baseaddr_h = cpu_to_le32(upper_32_bits(busaddr));
-		m_sg->flags = SGD_EOT;
-		m_sg->size = cpu_to_le32(scsi_bufflen(scmd));
+		m_sg->flags = 1U << mhba->eot_flag;
+		sgd_setsz(mhba, m_sg, cpu_to_le32(scsi_bufflen(scmd)));
 		*sg_count = 1;
 	}
 
@@ -267,8 +270,8 @@ static int mvumi_internal_cmd_sgl(struct mvumi_hba *mhba, struct mvumi_cmd *cmd,
 
 	m_sg->baseaddr_l = cpu_to_le32(lower_32_bits(phy_addr));
 	m_sg->baseaddr_h = cpu_to_le32(upper_32_bits(phy_addr));
-	m_sg->flags = SGD_EOT;
-	m_sg->size = cpu_to_le32(size);
+	m_sg->flags = 1U << mhba->eot_flag;
+	sgd_setsz(mhba, m_sg, cpu_to_le32(size));
 
 	return 0;
 }
@@ -285,7 +288,8 @@ static struct mvumi_cmd *mvumi_create_internal_cmd(struct mvumi_hba *mhba,
 	}
 	INIT_LIST_HEAD(&cmd->queue_pointer);
 
-	cmd->frame = kzalloc(mhba->ib_max_size, GFP_KERNEL);
+	cmd->frame = pci_alloc_consistent(mhba->pdev,
+				mhba->ib_max_size, &cmd->frame_phys);
 	if (!cmd->frame) {
 		dev_err(&mhba->pdev->dev, "failed to allocate memory for FW"
 			" frame,size = %d.\n", mhba->ib_max_size);
@@ -297,7 +301,8 @@ static struct mvumi_cmd *mvumi_create_internal_cmd(struct mvumi_hba *mhba,
 		if (mvumi_internal_cmd_sgl(mhba, cmd, buf_size)) {
 			dev_err(&mhba->pdev->dev, "failed to allocate memory"
 						" for internal frame\n");
-			kfree(cmd->frame);
+			pci_free_consistent(mhba->pdev, mhba->ib_max_size,
+					cmd->frame, cmd->frame_phys);
 			kfree(cmd);
 			return NULL;
 		}
@@ -317,7 +322,7 @@ static void mvumi_delete_internal_cmd(struct mvumi_hba *mhba,
 	if (cmd && cmd->frame) {
 		if (cmd->frame->sg_counts) {
 			m_sg = (struct mvumi_sgl *) &cmd->frame->payload[0];
-			size = m_sg->size;
+			sgd_getsz(mhba, m_sg, size);
 
 			phy_addr = (dma_addr_t) m_sg->baseaddr_l |
 				(dma_addr_t) ((m_sg->baseaddr_h << 16) << 16);
@@ -325,7 +330,8 @@ static void mvumi_delete_internal_cmd(struct mvumi_hba *mhba,
 			pci_free_consistent(mhba->pdev, size, cmd->data_buf,
 								phy_addr);
 		}
-		kfree(cmd->frame);
+		pci_free_consistent(mhba->pdev, mhba->ib_max_size,
+				cmd->frame, cmd->frame_phys);
 		kfree(cmd);
 	}
 }
@@ -374,7 +380,8 @@ static void mvumi_free_cmds(struct mvumi_hba *mhba)
 		cmd = list_first_entry(&mhba->cmd_pool, struct mvumi_cmd,
 							queue_pointer);
 		list_del(&cmd->queue_pointer);
-		kfree(cmd->frame);
+		if (!(mhba->hba_capability & HS_CAPABILITY_SUPPORT_DYN_SRC))
+			kfree(cmd->frame);
 		kfree(cmd);
 	}
 }
@@ -396,7 +403,12 @@ static int mvumi_alloc_cmds(struct mvumi_hba *mhba)
 
 		INIT_LIST_HEAD(&cmd->queue_pointer);
 		list_add_tail(&cmd->queue_pointer, &mhba->cmd_pool);
-		cmd->frame = kzalloc(mhba->ib_max_size, GFP_KERNEL);
+		if (mhba->hba_capability & HS_CAPABILITY_SUPPORT_DYN_SRC) {
+			cmd->frame = mhba->ib_frame + i * mhba->ib_max_size;
+			cmd->frame_phys = mhba->ib_frame_phys
+						+ i * mhba->ib_max_size;
+		} else
+			cmd->frame = kzalloc(mhba->ib_max_size, GFP_KERNEL);
 		if (!cmd->frame)
 			goto err_exit;
 	}
@@ -409,48 +421,71 @@ err_exit:
 		cmd = list_first_entry(&mhba->cmd_pool, struct mvumi_cmd,
 						queue_pointer);
 		list_del(&cmd->queue_pointer);
-		kfree(cmd->frame);
+		if (!(mhba->hba_capability & HS_CAPABILITY_SUPPORT_DYN_SRC))
+			kfree(cmd->frame);
 		kfree(cmd);
 	}
 	return -ENOMEM;
 }
 
-static int mvumi_get_ib_list_entry(struct mvumi_hba *mhba, void **ib_entry)
+static unsigned int mvumi_check_ib_list_9143(struct mvumi_hba *mhba)
 {
-	unsigned int ib_rp_reg, cur_ib_entry;
+	unsigned int ib_rp_reg;
+	struct mvumi_hw_regs *regs = mhba->regs;
+
+	ib_rp_reg = ioread32(mhba->regs->inb_read_pointer);
 
+	if (unlikely(((ib_rp_reg & regs->cl_slot_num_mask) ==
+			(mhba->ib_cur_slot & regs->cl_slot_num_mask)) &&
+			((ib_rp_reg & regs->cl_pointer_toggle)
+			 != (mhba->ib_cur_slot & regs->cl_pointer_toggle)))) {
+		dev_warn(&mhba->pdev->dev, "no free slot to use.\n");
+		return 0;
+	}
 	if (atomic_read(&mhba->fw_outstanding) >= mhba->max_io) {
 		dev_warn(&mhba->pdev->dev, "firmware io overflow.\n");
-		return -1;
+		return 0;
+	} else {
+		return mhba->max_io - atomic_read(&mhba->fw_outstanding);
 	}
-	ib_rp_reg = ioread32(mhba->mmio + CLA_INB_READ_POINTER);
+}
 
-	if (unlikely(((ib_rp_reg & CL_SLOT_NUM_MASK) ==
-			(mhba->ib_cur_slot & CL_SLOT_NUM_MASK)) &&
-			((ib_rp_reg & CL_POINTER_TOGGLE) !=
-			(mhba->ib_cur_slot & CL_POINTER_TOGGLE)))) {
-		dev_warn(&mhba->pdev->dev, "no free slot to use.\n");
-		return -1;
-	}
+static unsigned int mvumi_check_ib_list_9580(struct mvumi_hba *mhba)
+{
+	unsigned int count;
+	if (atomic_read(&mhba->fw_outstanding) >= (mhba->max_io - 1))
+		return 0;
+	count = ioread32(mhba->ib_shadow);
+	if (count == 0xffff)
+		return 0;
+	return count;
+}
+
+static void mvumi_get_ib_list_entry(struct mvumi_hba *mhba, void **ib_entry)
+{
+	unsigned int cur_ib_entry;
 
-	cur_ib_entry = mhba->ib_cur_slot & CL_SLOT_NUM_MASK;
+	cur_ib_entry = mhba->ib_cur_slot & mhba->regs->cl_slot_num_mask;
 	cur_ib_entry++;
 	if (cur_ib_entry >= mhba->list_num_io) {
 		cur_ib_entry -= mhba->list_num_io;
-		mhba->ib_cur_slot ^= CL_POINTER_TOGGLE;
+		mhba->ib_cur_slot ^= mhba->regs->cl_pointer_toggle;
+	}
+	mhba->ib_cur_slot &= ~mhba->regs->cl_slot_num_mask;
+	mhba->ib_cur_slot |= (cur_ib_entry & mhba->regs->cl_slot_num_mask);
+	if (mhba->hba_capability & HS_CAPABILITY_SUPPORT_DYN_SRC) {
+		*ib_entry = mhba->ib_list + cur_ib_entry *
+				sizeof(struct mvumi_dyn_list_entry);
+	} else {
+		*ib_entry = mhba->ib_list + cur_ib_entry * mhba->ib_max_size;
 	}
-	mhba->ib_cur_slot &= ~CL_SLOT_NUM_MASK;
-	mhba->ib_cur_slot |= (cur_ib_entry & CL_SLOT_NUM_MASK);
-	*ib_entry = mhba->ib_list + cur_ib_entry * mhba->ib_max_size;
 	atomic_inc(&mhba->fw_outstanding);
-
-	return 0;
 }
 
 static void mvumi_send_ib_list_entry(struct mvumi_hba *mhba)
 {
-	iowrite32(0xfff, mhba->ib_shadow);
-	iowrite32(mhba->ib_cur_slot, mhba->mmio + CLA_INB_WRITE_POINTER);
+	iowrite32(0xffff, mhba->ib_shadow);
+	iowrite32(mhba->ib_cur_slot, mhba->regs->inb_write_pointer);
 }
 
 static char mvumi_check_ob_frame(struct mvumi_hba *mhba,
@@ -480,31 +515,59 @@ static char mvumi_check_ob_frame(struct mvumi_hba *mhba,
 	return 0;
 }
 
-static void mvumi_receive_ob_list_entry(struct mvumi_hba *mhba)
+static int mvumi_check_ob_list_9143(struct mvumi_hba *mhba,
+			unsigned int *cur_obf, unsigned int *assign_obf_end)
 {
-	unsigned int ob_write_reg, ob_write_shadow_reg;
-	unsigned int cur_obf, assign_obf_end, i;
-	struct mvumi_ob_data *ob_data;
-	struct mvumi_rsp_frame *p_outb_frame;
+	unsigned int ob_write, ob_write_shadow;
+	struct mvumi_hw_regs *regs = mhba->regs;
 
 	do {
-		ob_write_reg = ioread32(mhba->mmio + CLA_OUTB_COPY_POINTER);
-		ob_write_shadow_reg = ioread32(mhba->ob_shadow);
-	} while ((ob_write_reg & CL_SLOT_NUM_MASK) != ob_write_shadow_reg);
+		ob_write = ioread32(regs->outb_copy_pointer);
+		ob_write_shadow = ioread32(mhba->ob_shadow);
+	} while ((ob_write & regs->cl_slot_num_mask) != ob_write_shadow);
 
-	cur_obf = mhba->ob_cur_slot & CL_SLOT_NUM_MASK;
-	assign_obf_end = ob_write_reg & CL_SLOT_NUM_MASK;
+	*cur_obf = mhba->ob_cur_slot & mhba->regs->cl_slot_num_mask;
+	*assign_obf_end = ob_write & mhba->regs->cl_slot_num_mask;
 
-	if ((ob_write_reg & CL_POINTER_TOGGLE) !=
-				(mhba->ob_cur_slot & CL_POINTER_TOGGLE)) {
-		assign_obf_end += mhba->list_num_io;
+	if ((ob_write & regs->cl_pointer_toggle) !=
+			(mhba->ob_cur_slot & regs->cl_pointer_toggle)) {
+		*assign_obf_end += mhba->list_num_io;
 	}
+	return 0;
+}
+
+static int mvumi_check_ob_list_9580(struct mvumi_hba *mhba,
+			unsigned int *cur_obf, unsigned int *assign_obf_end)
+{
+	unsigned int ob_write;
+	struct mvumi_hw_regs *regs = mhba->regs;
+
+	ob_write = ioread32(regs->outb_read_pointer);
+	ob_write = ioread32(regs->outb_copy_pointer);
+	*cur_obf = mhba->ob_cur_slot & mhba->regs->cl_slot_num_mask;
+	*assign_obf_end = ob_write & mhba->regs->cl_slot_num_mask;
+	if (*assign_obf_end < *cur_obf)
+		*assign_obf_end += mhba->list_num_io;
+	else if (*assign_obf_end == *cur_obf)
+		return -1;
+	return 0;
+}
+
+static void mvumi_receive_ob_list_entry(struct mvumi_hba *mhba)
+{
+	unsigned int cur_obf, assign_obf_end, i;
+	struct mvumi_ob_data *ob_data;
+	struct mvumi_rsp_frame *p_outb_frame;
+	struct mvumi_hw_regs *regs = mhba->regs;
+
+	if (mhba->instancet->check_ob_list(mhba, &cur_obf, &assign_obf_end))
+		return;
 
 	for (i = (assign_obf_end - cur_obf); i != 0; i--) {
 		cur_obf++;
 		if (cur_obf >= mhba->list_num_io) {
 			cur_obf -= mhba->list_num_io;
-			mhba->ob_cur_slot ^= CL_POINTER_TOGGLE;
+			mhba->ob_cur_slot ^= regs->cl_pointer_toggle;
 		}
 
 		p_outb_frame = mhba->ob_list + cur_obf * mhba->ob_max_size;
@@ -528,7 +591,7 @@ static void mvumi_receive_ob_list_entry(struct mvumi_hba *mhba)
 			ob_data = NULL;
 			if (cur_obf == 0) {
 				cur_obf = mhba->list_num_io - 1;
-				mhba->ob_cur_slot ^= CL_POINTER_TOGGLE;
+				mhba->ob_cur_slot ^= regs->cl_pointer_toggle;
 			} else
 				cur_obf -= 1;
 			break;
@@ -539,18 +602,20 @@ static void mvumi_receive_ob_list_entry(struct mvumi_hba *mhba)
 
 		list_add_tail(&ob_data->list, &mhba->free_ob_list);
 	}
-	mhba->ob_cur_slot &= ~CL_SLOT_NUM_MASK;
-	mhba->ob_cur_slot |= (cur_obf & CL_SLOT_NUM_MASK);
-	iowrite32(mhba->ob_cur_slot, mhba->mmio + CLA_OUTB_READ_POINTER);
+	mhba->ob_cur_slot &= ~regs->cl_slot_num_mask;
+	mhba->ob_cur_slot |= (cur_obf & regs->cl_slot_num_mask);
+	iowrite32(mhba->ob_cur_slot, regs->outb_read_pointer);
 }
 
-static void mvumi_reset(void *regs)
+static void mvumi_reset(struct mvumi_hba *mhba)
 {
-	iowrite32(0, regs + CPU_ENPOINTA_MASK_REG);
-	if (ioread32(regs + CPU_ARM_TO_PCIEA_MSG1) != HANDSHAKE_DONESTATE)
+	struct mvumi_hw_regs *regs = mhba->regs;
+
+	iowrite32(0, regs->enpointa_mask_reg);
+	if (ioread32(regs->arm_to_pciea_msg1) != HANDSHAKE_DONESTATE)
 		return;
 
-	iowrite32(DRBL_SOFT_RESET, regs + CPU_PCIEA_TO_ARM_DRBL_REG);
+	iowrite32(DRBL_SOFT_RESET, regs->pciea_to_arm_drbl_reg);
 }
 
 static unsigned char mvumi_start(struct mvumi_hba *mhba);
@@ -558,7 +623,7 @@ static unsigned char mvumi_start(struct mvumi_hba *mhba);
 static int mvumi_wait_for_outstanding(struct mvumi_hba *mhba)
 {
 	mhba->fw_state = FW_STATE_ABORT;
-	mvumi_reset(mhba->mmio);
+	mvumi_reset(mhba);
 
 	if (mvumi_start(mhba))
 		return FAILED;
@@ -566,6 +631,98 @@ static int mvumi_wait_for_outstanding(struct mvumi_hba *mhba)
 		return SUCCESS;
 }
 
+static int mvumi_wait_for_fw(struct mvumi_hba *mhba)
+{
+	struct mvumi_hw_regs *regs = mhba->regs;
+	u32 tmp;
+	unsigned long before;
+	before = jiffies;
+
+	iowrite32(0, regs->enpointa_mask_reg);
+	tmp = ioread32(regs->arm_to_pciea_msg1);
+	while (tmp != HANDSHAKE_READYSTATE) {
+		iowrite32(DRBL_MU_RESET, regs->pciea_to_arm_drbl_reg);
+		if (time_after(jiffies, before + FW_MAX_DELAY * HZ)) {
+			dev_err(&mhba->pdev->dev,
+				"FW reset failed [0x%x].\n", tmp);
+			return FAILED;
+		}
+
+		msleep(500);
+		rmb();
+		tmp = ioread32(regs->arm_to_pciea_msg1);
+	}
+
+	return SUCCESS;
+}
+
+static void mvumi_backup_bar_addr(struct mvumi_hba *mhba)
+{
+	unsigned char i;
+
+	for (i = 0; i < MAX_BASE_ADDRESS; i++) {
+		pci_read_config_dword(mhba->pdev, 0x10 + i * 4,
+						&mhba->pci_base[i]);
+	}
+}
+
+static void mvumi_restore_bar_addr(struct mvumi_hba *mhba)
+{
+	unsigned char i;
+
+	for (i = 0; i < MAX_BASE_ADDRESS; i++) {
+		if (mhba->pci_base[i])
+			pci_write_config_dword(mhba->pdev, 0x10 + i * 4,
+						mhba->pci_base[i]);
+	}
+}
+
+static unsigned int mvumi_pci_set_master(struct pci_dev *pdev)
+{
+	unsigned int ret = 0;
+	pci_set_master(pdev);
+
+	if (IS_DMA64) {
+		if (pci_set_dma_mask(pdev, DMA_BIT_MASK(64)))
+			ret = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+	} else
+		ret = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+
+	return ret;
+}
+
+static int mvumi_reset_host_9580(struct mvumi_hba *mhba)
+{
+	mhba->fw_state = FW_STATE_ABORT;
+
+	iowrite32(0, mhba->regs->reset_enable);
+	iowrite32(0xf, mhba->regs->reset_request);
+
+	iowrite32(0x10, mhba->regs->reset_enable);
+	iowrite32(0x10, mhba->regs->reset_request);
+	msleep(100);
+	pci_disable_device(mhba->pdev);
+
+	if (pci_enable_device(mhba->pdev)) {
+		dev_err(&mhba->pdev->dev, "enable device failed\n");
+		return FAILED;
+	}
+	if (mvumi_pci_set_master(mhba->pdev)) {
+		dev_err(&mhba->pdev->dev, "set master failed\n");
+		return FAILED;
+	}
+	mvumi_restore_bar_addr(mhba);
+	if (mvumi_wait_for_fw(mhba) == FAILED)
+		return FAILED;
+
+	return mvumi_wait_for_outstanding(mhba);
+}
+
+static int mvumi_reset_host_9143(struct mvumi_hba *mhba)
+{
+	return mvumi_wait_for_outstanding(mhba);
+}
+
 static int mvumi_host_reset(struct scsi_cmnd *scmd)
 {
 	struct mvumi_hba *mhba;
@@ -575,7 +732,7 @@ static int mvumi_host_reset(struct scsi_cmnd *scmd)
 	scmd_printk(KERN_NOTICE, scmd, "RESET -%ld cmd=%x retries=%x\n",
 			scmd->serial_number, scmd->cmnd[0], scmd->retries);
 
-	return mvumi_wait_for_outstanding(mhba);
+	return mhba->instancet->reset_host(mhba);
 }
 
 static int mvumi_issue_blocked_cmd(struct mvumi_hba *mhba,
@@ -628,7 +785,9 @@ static void mvumi_release_fw(struct mvumi_hba *mhba)
 	mvumi_free_cmds(mhba);
 	mvumi_release_mem_resource(mhba);
 	mvumi_unmap_pci_addr(mhba->pdev, mhba->base_addr);
-	kfree(mhba->handshake_page);
+	pci_free_consistent(mhba->pdev, HSP_MAX_SIZE,
+		mhba->handshake_page, mhba->handshake_page_phys);
+	kfree(mhba->regs);
 	pci_release_regions(mhba->pdev);
 }
 
@@ -665,6 +824,7 @@ get_cmd:	cmd = mvumi_create_internal_cmd(mhba, 0);
 		frame->cdb_length = MAX_COMMAND_SIZE;
 		memset(frame->cdb, 0, MAX_COMMAND_SIZE);
 		frame->cdb[0] = SCSI_CMD_MARVELL_SPECIFIC;
+		frame->cdb[1] = CDB_CORE_MODULE;
 		frame->cdb[2] = CDB_CORE_SHUTDOWN;
 
 		mvumi_issue_blocked_cmd(mhba, cmd);
@@ -695,7 +855,7 @@ mvumi_calculate_checksum(struct mvumi_hs_header *p_header,
 	return ret;
 }
 
-void mvumi_hs_build_page(struct mvumi_hba *mhba,
+static void mvumi_hs_build_page(struct mvumi_hba *mhba,
 				struct mvumi_hs_header *hs_header)
 {
 	struct mvumi_hs_page2 *hs_page2;
@@ -710,6 +870,8 @@ void mvumi_hs_build_page(struct mvumi_hba *mhba,
 		hs_header->frame_length = sizeof(*hs_page2) - 4;
 		memset(hs_header->frame_content, 0, hs_header->frame_length);
 		hs_page2->host_type = 3; /* 3 mean linux*/
+		if (mhba->hba_capability & HS_CAPABILITY_SUPPORT_DYN_SRC)
+			hs_page2->host_cap = 0x08;/* host dynamic source mode */
 		hs_page2->host_ver.ver_major = VER_MAJOR;
 		hs_page2->host_ver.ver_minor = VER_MINOR;
 		hs_page2->host_ver.ver_oem = VER_OEM;
@@ -745,8 +907,18 @@ void mvumi_hs_build_page(struct mvumi_hba *mhba,
 		hs_page4->ob_baseaddr_h = upper_32_bits(mhba->ob_list_phys);
 		hs_page4->ib_entry_size = mhba->ib_max_size_setting;
 		hs_page4->ob_entry_size = mhba->ob_max_size_setting;
-		hs_page4->ob_depth = mhba->list_num_io;
-		hs_page4->ib_depth = mhba->list_num_io;
+		if (mhba->hba_capability
+			& HS_CAPABILITY_NEW_PAGE_IO_DEPTH_DEF) {
+			hs_page4->ob_depth = find_first_bit((unsigned long *)
+							    &mhba->list_num_io,
+							    BITS_PER_LONG);
+			hs_page4->ib_depth = find_first_bit((unsigned long *)
+							    &mhba->list_num_io,
+							    BITS_PER_LONG);
+		} else {
+			hs_page4->ob_depth = (u8) mhba->list_num_io;
+			hs_page4->ib_depth = (u8) mhba->list_num_io;
+		}
 		hs_header->checksum = mvumi_calculate_checksum(hs_header,
 						hs_header->frame_length);
 		break;
@@ -774,8 +946,11 @@ static int mvumi_init_data(struct mvumi_hba *mhba)
 		return 0;
 
 	tmp_size = mhba->ib_max_size * mhba->max_io;
+	if (mhba->hba_capability & HS_CAPABILITY_SUPPORT_DYN_SRC)
+		tmp_size += sizeof(struct mvumi_dyn_list_entry) * mhba->max_io;
+
 	tmp_size += 128 + mhba->ob_max_size * mhba->max_io;
-	tmp_size += 8 + sizeof(u32) + 16;
+	tmp_size += 8 + sizeof(u32)*2 + 16;
 
 	res_mgnt = mvumi_alloc_mem_resource(mhba,
 					RESOURCE_UNCACHED_MEMORY, tmp_size);
@@ -793,24 +968,41 @@ static int mvumi_init_data(struct mvumi_hba *mhba)
 	v += offset;
 	mhba->ib_list = v;
 	mhba->ib_list_phys = p;
+	if (mhba->hba_capability & HS_CAPABILITY_SUPPORT_DYN_SRC) {
+		v += sizeof(struct mvumi_dyn_list_entry) * mhba->max_io;
+		p += sizeof(struct mvumi_dyn_list_entry) * mhba->max_io;
+		mhba->ib_frame = v;
+		mhba->ib_frame_phys = p;
+	}
 	v += mhba->ib_max_size * mhba->max_io;
 	p += mhba->ib_max_size * mhba->max_io;
+
 	/* ib shadow */
 	offset = round_up(p, 8) - p;
 	p += offset;
 	v += offset;
 	mhba->ib_shadow = v;
 	mhba->ib_shadow_phys = p;
-	p += sizeof(u32);
-	v += sizeof(u32);
+	p += sizeof(u32)*2;
+	v += sizeof(u32)*2;
 	/* ob shadow */
-	offset = round_up(p, 8) - p;
-	p += offset;
-	v += offset;
-	mhba->ob_shadow = v;
-	mhba->ob_shadow_phys = p;
-	p += 8;
-	v += 8;
+	if (mhba->pdev->device == PCI_DEVICE_ID_MARVELL_MV9580) {
+		offset = round_up(p, 8) - p;
+		p += offset;
+		v += offset;
+		mhba->ob_shadow = v;
+		mhba->ob_shadow_phys = p;
+		p += 8;
+		v += 8;
+	} else {
+		offset = round_up(p, 4) - p;
+		p += offset;
+		v += offset;
+		mhba->ob_shadow = v;
+		mhba->ob_shadow_phys = p;
+		p += 4;
+		v += 4;
+	}
 
 	/* ob list */
 	offset = round_up(p, 128) - p;
@@ -902,6 +1094,12 @@ static int mvumi_hs_process_page(struct mvumi_hba *mhba,
 		dev_dbg(&mhba->pdev->dev, "FW version:%d\n",
 						hs_page1->fw_ver.ver_build);
 
+		if (mhba->hba_capability & HS_CAPABILITY_SUPPORT_COMPACT_SG)
+			mhba->eot_flag = 22;
+		else
+			mhba->eot_flag = 27;
+		if (mhba->hba_capability & HS_CAPABILITY_NEW_PAGE_IO_DEPTH_DEF)
+			mhba->list_num_io = 1 << hs_page1->cl_inout_list_depth;
 		break;
 	default:
 		dev_err(&mhba->pdev->dev, "handshake: page code error\n");
@@ -923,12 +1121,12 @@ static int mvumi_handshake(struct mvumi_hba *mhba)
 {
 	unsigned int hs_state, tmp, hs_fun;
 	struct mvumi_hs_header *hs_header;
-	void *regs = mhba->mmio;
+	struct mvumi_hw_regs *regs = mhba->regs;
 
 	if (mhba->fw_state == FW_STATE_STARTING)
 		hs_state = HS_S_START;
 	else {
-		tmp = ioread32(regs + CPU_ARM_TO_PCIEA_MSG0);
+		tmp = ioread32(regs->arm_to_pciea_msg0);
 		hs_state = HS_GET_STATE(tmp);
 		dev_dbg(&mhba->pdev->dev, "handshake state[0x%x].\n", hs_state);
 		if (HS_GET_STATUS(tmp) != HS_STATUS_OK) {
@@ -943,21 +1141,20 @@ static int mvumi_handshake(struct mvumi_hba *mhba)
 		mhba->fw_state = FW_STATE_HANDSHAKING;
 		HS_SET_STATUS(hs_fun, HS_STATUS_OK);
 		HS_SET_STATE(hs_fun, HS_S_RESET);
-		iowrite32(HANDSHAKE_SIGNATURE, regs + CPU_PCIEA_TO_ARM_MSG1);
-		iowrite32(hs_fun, regs + CPU_PCIEA_TO_ARM_MSG0);
-		iowrite32(DRBL_HANDSHAKE, regs + CPU_PCIEA_TO_ARM_DRBL_REG);
+		iowrite32(HANDSHAKE_SIGNATURE, regs->pciea_to_arm_msg1);
+		iowrite32(hs_fun, regs->pciea_to_arm_msg0);
+		iowrite32(DRBL_HANDSHAKE, regs->pciea_to_arm_drbl_reg);
 		break;
 
 	case HS_S_RESET:
 		iowrite32(lower_32_bits(mhba->handshake_page_phys),
-					regs + CPU_PCIEA_TO_ARM_MSG1);
+					regs->pciea_to_arm_msg1);
 		iowrite32(upper_32_bits(mhba->handshake_page_phys),
-					regs + CPU_ARM_TO_PCIEA_MSG1);
+					regs->arm_to_pciea_msg1);
 		HS_SET_STATUS(hs_fun, HS_STATUS_OK);
 		HS_SET_STATE(hs_fun, HS_S_PAGE_ADDR);
-		iowrite32(hs_fun, regs + CPU_PCIEA_TO_ARM_MSG0);
-		iowrite32(DRBL_HANDSHAKE, regs + CPU_PCIEA_TO_ARM_DRBL_REG);
-
+		iowrite32(hs_fun, regs->pciea_to_arm_msg0);
+		iowrite32(DRBL_HANDSHAKE, regs->pciea_to_arm_drbl_reg);
 		break;
 
 	case HS_S_PAGE_ADDR:
@@ -997,30 +1194,37 @@ static int mvumi_handshake(struct mvumi_hba *mhba)
 			HS_SET_STATE(hs_fun, HS_S_END);
 
 		HS_SET_STATUS(hs_fun, HS_STATUS_OK);
-		iowrite32(hs_fun, regs + CPU_PCIEA_TO_ARM_MSG0);
-		iowrite32(DRBL_HANDSHAKE, regs + CPU_PCIEA_TO_ARM_DRBL_REG);
+		iowrite32(hs_fun, regs->pciea_to_arm_msg0);
+		iowrite32(DRBL_HANDSHAKE, regs->pciea_to_arm_drbl_reg);
 		break;
 
 	case HS_S_END:
 		/* Set communication list ISR */
-		tmp = ioread32(regs + CPU_ENPOINTA_MASK_REG);
-		tmp |= INT_MAP_COMAOUT | INT_MAP_COMAERR;
-		iowrite32(tmp, regs + CPU_ENPOINTA_MASK_REG);
+		tmp = ioread32(regs->enpointa_mask_reg);
+		tmp |= regs->int_comaout | regs->int_comaerr;
+		iowrite32(tmp, regs->enpointa_mask_reg);
 		iowrite32(mhba->list_num_io, mhba->ib_shadow);
 		/* Set InBound List Available count shadow */
 		iowrite32(lower_32_bits(mhba->ib_shadow_phys),
-					regs + CLA_INB_AVAL_COUNT_BASEL);
+					regs->inb_aval_count_basel);
 		iowrite32(upper_32_bits(mhba->ib_shadow_phys),
-					regs + CLA_INB_AVAL_COUNT_BASEH);
-
-		/* Set OutBound List Available count shadow */
-		iowrite32((mhba->list_num_io-1) | CL_POINTER_TOGGLE,
-						mhba->ob_shadow);
-		iowrite32(lower_32_bits(mhba->ob_shadow_phys), regs + 0x5B0);
-		iowrite32(upper_32_bits(mhba->ob_shadow_phys), regs + 0x5B4);
+					regs->inb_aval_count_baseh);
+
+		if (mhba->pdev->device == PCI_DEVICE_ID_MARVELL_MV9143) {
+			/* Set OutBound List Available count shadow */
+			iowrite32((mhba->list_num_io-1) |
+							regs->cl_pointer_toggle,
+							mhba->ob_shadow);
+			iowrite32(lower_32_bits(mhba->ob_shadow_phys),
+							regs->outb_copy_basel);
+			iowrite32(upper_32_bits(mhba->ob_shadow_phys),
+							regs->outb_copy_baseh);
+		}
 
-		mhba->ib_cur_slot = (mhba->list_num_io - 1) | CL_POINTER_TOGGLE;
-		mhba->ob_cur_slot = (mhba->list_num_io - 1) | CL_POINTER_TOGGLE;
+		mhba->ib_cur_slot = (mhba->list_num_io - 1) |
+							regs->cl_pointer_toggle;
+		mhba->ob_cur_slot = (mhba->list_num_io - 1) |
+							regs->cl_pointer_toggle;
 		mhba->fw_state = FW_STATE_STARTED;
 
 		break;
@@ -1040,7 +1244,7 @@ static unsigned char mvumi_handshake_event(struct mvumi_hba *mhba)
 	before = jiffies;
 	mvumi_handshake(mhba);
 	do {
-		isr_status = mhba->instancet->read_fw_status_reg(mhba->mmio);
+		isr_status = mhba->instancet->read_fw_status_reg(mhba);
 
 		if (mhba->fw_state == FW_STATE_STARTED)
 			return 0;
@@ -1062,16 +1266,15 @@ static unsigned char mvumi_handshake_event(struct mvumi_hba *mhba)
 
 static unsigned char mvumi_check_handshake(struct mvumi_hba *mhba)
 {
-	void *regs = mhba->mmio;
 	unsigned int tmp;
 	unsigned long before;
 
 	before = jiffies;
-	tmp = ioread32(regs + CPU_ARM_TO_PCIEA_MSG1);
+	tmp = ioread32(mhba->regs->arm_to_pciea_msg1);
 	while ((tmp != HANDSHAKE_READYSTATE) && (tmp != HANDSHAKE_DONESTATE)) {
 		if (tmp != HANDSHAKE_READYSTATE)
 			iowrite32(DRBL_MU_RESET,
-					regs + CPU_PCIEA_TO_ARM_DRBL_REG);
+					mhba->regs->pciea_to_arm_drbl_reg);
 		if (time_after(jiffies, before + FW_MAX_DELAY * HZ)) {
 			dev_err(&mhba->pdev->dev,
 				"invalid signature [0x%x].\n", tmp);
@@ -1079,7 +1282,7 @@ static unsigned char mvumi_check_handshake(struct mvumi_hba *mhba)
 		}
 		usleep_range(1000, 2000);
 		rmb();
-		tmp = ioread32(regs + CPU_ARM_TO_PCIEA_MSG1);
+		tmp = ioread32(mhba->regs->arm_to_pciea_msg1);
 	}
 
 	mhba->fw_state = FW_STATE_STARTING;
@@ -1100,15 +1303,17 @@ static unsigned char mvumi_check_handshake(struct mvumi_hba *mhba)
 
 static unsigned char mvumi_start(struct mvumi_hba *mhba)
 {
-	void *regs = mhba->mmio;
 	unsigned int tmp;
+	struct mvumi_hw_regs *regs = mhba->regs;
+
 	/* clear Door bell */
-	tmp = ioread32(regs + CPU_ARM_TO_PCIEA_DRBL_REG);
-	iowrite32(tmp, regs + CPU_ARM_TO_PCIEA_DRBL_REG);
+	tmp = ioread32(regs->arm_to_pciea_drbl_reg);
+	iowrite32(tmp, regs->arm_to_pciea_drbl_reg);
 
-	iowrite32(0x3FFFFFFF, regs + CPU_ARM_TO_PCIEA_MASK_REG);
-	tmp = ioread32(regs + CPU_ENPOINTA_MASK_REG) | INT_MAP_DL_CPU2PCIEA;
-	iowrite32(tmp, regs + CPU_ENPOINTA_MASK_REG);
+	iowrite32(regs->int_drbl_int_mask, regs->arm_to_pciea_mask_reg);
+	tmp = ioread32(regs->enpointa_mask_reg) | regs->int_dl_cpu2pciea;
+	iowrite32(tmp, regs->enpointa_mask_reg);
+	msleep(100);
 	if (mvumi_check_handshake(mhba))
 		return -1;
 
@@ -1166,6 +1371,7 @@ static void mvumi_complete_cmd(struct mvumi_hba *mhba, struct mvumi_cmd *cmd,
 	cmd->scmd->scsi_done(scmd);
 	mvumi_return_cmd(mhba, cmd);
 }
+
 static void mvumi_complete_internal_cmd(struct mvumi_hba *mhba,
 						struct mvumi_cmd *cmd,
 					struct mvumi_rsp_frame *ob_frame)
@@ -1210,6 +1416,304 @@ static void mvumi_show_event(struct mvumi_hba *mhba,
 	}
 }
 
+static int mvumi_handle_hotplug(struct mvumi_hba *mhba, u16 devid, int status)
+{
+	struct scsi_device *sdev;
+	int ret = -1;
+
+	if (status == DEVICE_OFFLINE) {
+		sdev = scsi_device_lookup(mhba->shost, 0, devid, 0);
+		if (sdev) {
+			dev_dbg(&mhba->pdev->dev, "remove disk %d-%d-%d.\n", 0,
+								sdev->id, 0);
+			scsi_remove_device(sdev);
+			scsi_device_put(sdev);
+			ret = 0;
+		} else
+			dev_err(&mhba->pdev->dev, " no disk[%d] to remove\n",
+									devid);
+	} else if (status == DEVICE_ONLINE) {
+		sdev = scsi_device_lookup(mhba->shost, 0, devid, 0);
+		if (!sdev) {
+			scsi_add_device(mhba->shost, 0, devid, 0);
+			dev_dbg(&mhba->pdev->dev, " add disk %d-%d-%d.\n", 0,
+								devid, 0);
+			ret = 0;
+		} else {
+			dev_err(&mhba->pdev->dev, " don't add disk %d-%d-%d.\n",
+								0, devid, 0);
+			scsi_device_put(sdev);
+		}
+	}
+	return ret;
+}
+
+static u64 mvumi_inquiry(struct mvumi_hba *mhba,
+	unsigned int id, struct mvumi_cmd *cmd)
+{
+	struct mvumi_msg_frame *frame;
+	u64 wwid = 0;
+	int cmd_alloc = 0;
+	int data_buf_len = 64;
+
+	if (!cmd) {
+		cmd = mvumi_create_internal_cmd(mhba, data_buf_len);
+		if (cmd)
+			cmd_alloc = 1;
+		else
+			return 0;
+	} else {
+		memset(cmd->data_buf, 0, data_buf_len);
+	}
+	cmd->scmd = NULL;
+	cmd->cmd_status = REQ_STATUS_PENDING;
+	atomic_set(&cmd->sync_cmd, 0);
+	frame = cmd->frame;
+	frame->device_id = (u16) id;
+	frame->cmd_flag = CMD_FLAG_DATA_IN;
+	frame->req_function = CL_FUN_SCSI_CMD;
+	frame->cdb_length = 6;
+	frame->data_transfer_length = MVUMI_INQUIRY_LENGTH;
+	memset(frame->cdb, 0, frame->cdb_length);
+	frame->cdb[0] = INQUIRY;
+	frame->cdb[4] = frame->data_transfer_length;
+
+	mvumi_issue_blocked_cmd(mhba, cmd);
+
+	if (cmd->cmd_status == SAM_STAT_GOOD) {
+		if (mhba->pdev->device == PCI_DEVICE_ID_MARVELL_MV9143)
+			wwid = id + 1;
+		else
+			memcpy((void *)&wwid,
+			       (cmd->data_buf + MVUMI_INQUIRY_UUID_OFF),
+			       MVUMI_INQUIRY_UUID_LEN);
+		dev_dbg(&mhba->pdev->dev,
+			"inquiry device(0:%d:0) wwid(%llx)\n", id, wwid);
+	} else {
+		wwid = 0;
+	}
+	if (cmd_alloc)
+		mvumi_delete_internal_cmd(mhba, cmd);
+
+	return wwid;
+}
+
+static void mvumi_detach_devices(struct mvumi_hba *mhba)
+{
+	struct mvumi_device *mv_dev = NULL , *dev_next;
+	struct scsi_device *sdev = NULL;
+
+	mutex_lock(&mhba->device_lock);
+
+	/* detach Hard Disk */
+	list_for_each_entry_safe(mv_dev, dev_next,
+		&mhba->shost_dev_list, list) {
+		mvumi_handle_hotplug(mhba, mv_dev->id, DEVICE_OFFLINE);
+		list_del_init(&mv_dev->list);
+		dev_dbg(&mhba->pdev->dev, "release device(0:%d:0) wwid(%llx)\n",
+			mv_dev->id, mv_dev->wwid);
+		kfree(mv_dev);
+	}
+	list_for_each_entry_safe(mv_dev, dev_next, &mhba->mhba_dev_list, list) {
+		list_del_init(&mv_dev->list);
+		dev_dbg(&mhba->pdev->dev, "release device(0:%d:0) wwid(%llx)\n",
+			mv_dev->id, mv_dev->wwid);
+		kfree(mv_dev);
+	}
+
+	/* detach virtual device */
+	if (mhba->pdev->device == PCI_DEVICE_ID_MARVELL_MV9580)
+		sdev = scsi_device_lookup(mhba->shost, 0,
+						mhba->max_target_id - 1, 0);
+
+	if (sdev) {
+		scsi_remove_device(sdev);
+		scsi_device_put(sdev);
+	}
+
+	mutex_unlock(&mhba->device_lock);
+}
+
+static void mvumi_rescan_devices(struct mvumi_hba *mhba, int id)
+{
+	struct scsi_device *sdev;
+
+	sdev = scsi_device_lookup(mhba->shost, 0, id, 0);
+	if (sdev) {
+		scsi_rescan_device(&sdev->sdev_gendev);
+		scsi_device_put(sdev);
+	}
+}
+
+static int mvumi_match_devices(struct mvumi_hba *mhba, int id, u64 wwid)
+{
+	struct mvumi_device *mv_dev = NULL;
+
+	list_for_each_entry(mv_dev, &mhba->shost_dev_list, list) {
+		if (mv_dev->wwid == wwid) {
+			if (mv_dev->id != id) {
+				dev_err(&mhba->pdev->dev,
+					"%s has same wwid[%llx] ,"
+					" but different id[%d %d]\n",
+					__func__, mv_dev->wwid, mv_dev->id, id);
+				return -1;
+			} else {
+				if (mhba->pdev->device ==
+						PCI_DEVICE_ID_MARVELL_MV9143)
+					mvumi_rescan_devices(mhba, id);
+				return 1;
+			}
+		}
+	}
+	return 0;
+}
+
+static void mvumi_remove_devices(struct mvumi_hba *mhba, int id)
+{
+	struct mvumi_device *mv_dev = NULL, *dev_next;
+
+	list_for_each_entry_safe(mv_dev, dev_next,
+				&mhba->shost_dev_list, list) {
+		if (mv_dev->id == id) {
+			dev_dbg(&mhba->pdev->dev,
+				"detach device(0:%d:0) wwid(%llx) from HOST\n",
+				mv_dev->id, mv_dev->wwid);
+			mvumi_handle_hotplug(mhba, mv_dev->id, DEVICE_OFFLINE);
+			list_del_init(&mv_dev->list);
+			kfree(mv_dev);
+		}
+	}
+}
+
+static int mvumi_probe_devices(struct mvumi_hba *mhba)
+{
+	int id, maxid;
+	u64 wwid = 0;
+	struct mvumi_device *mv_dev = NULL;
+	struct mvumi_cmd *cmd = NULL;
+	int found = 0;
+
+	cmd = mvumi_create_internal_cmd(mhba, 64);
+	if (!cmd)
+		return -1;
+
+	if (mhba->pdev->device == PCI_DEVICE_ID_MARVELL_MV9143)
+		maxid = mhba->max_target_id;
+	else
+		maxid = mhba->max_target_id - 1;
+
+	for (id = 0; id < maxid; id++) {
+		wwid = mvumi_inquiry(mhba, id, cmd);
+		if (!wwid) {
+			/* device no response, remove it */
+			mvumi_remove_devices(mhba, id);
+		} else {
+			/* device response, add it */
+			found = mvumi_match_devices(mhba, id, wwid);
+			if (!found) {
+				mvumi_remove_devices(mhba, id);
+				mv_dev = kzalloc(sizeof(struct mvumi_device),
+								GFP_KERNEL);
+				if (!mv_dev) {
+					dev_err(&mhba->pdev->dev,
+						"%s alloc mv_dev failed\n",
+						__func__);
+					continue;
+				}
+				mv_dev->id = id;
+				mv_dev->wwid = wwid;
+				mv_dev->sdev = NULL;
+				INIT_LIST_HEAD(&mv_dev->list);
+				list_add_tail(&mv_dev->list,
+					      &mhba->mhba_dev_list);
+				dev_dbg(&mhba->pdev->dev,
+					"probe a new device(0:%d:0)"
+					" wwid(%llx)\n", id, mv_dev->wwid);
+			} else if (found == -1)
+				return -1;
+			else
+				continue;
+		}
+	}
+
+	if (cmd)
+		mvumi_delete_internal_cmd(mhba, cmd);
+
+	return 0;
+}
+
+static int mvumi_rescan_bus(void *data)
+{
+	int ret = 0;
+	struct mvumi_hba *mhba = (struct mvumi_hba *) data;
+	struct mvumi_device *mv_dev = NULL , *dev_next;
+
+	while (!kthread_should_stop()) {
+
+		set_current_state(TASK_INTERRUPTIBLE);
+		if (!atomic_read(&mhba->pnp_count))
+			schedule();
+		msleep(1000);
+		atomic_set(&mhba->pnp_count, 0);
+		__set_current_state(TASK_RUNNING);
+
+		mutex_lock(&mhba->device_lock);
+		ret = mvumi_probe_devices(mhba);
+		if (!ret) {
+			list_for_each_entry_safe(mv_dev, dev_next,
+						 &mhba->mhba_dev_list, list) {
+				if (mvumi_handle_hotplug(mhba, mv_dev->id,
+							 DEVICE_ONLINE)) {
+					dev_err(&mhba->pdev->dev,
+						"%s add device(0:%d:0) failed"
+						"wwid(%llx) has exist\n",
+						__func__,
+						mv_dev->id, mv_dev->wwid);
+					list_del_init(&mv_dev->list);
+					kfree(mv_dev);
+				} else {
+					list_move_tail(&mv_dev->list,
+						       &mhba->shost_dev_list);
+				}
+			}
+		}
+		mutex_unlock(&mhba->device_lock);
+	}
+	return 0;
+}
+
+static void mvumi_proc_msg(struct mvumi_hba *mhba,
+					struct mvumi_hotplug_event *param)
+{
+	u16 size = param->size;
+	const unsigned long *ar_bitmap;
+	const unsigned long *re_bitmap;
+	int index;
+
+	if (mhba->fw_flag & MVUMI_FW_ATTACH) {
+		index = -1;
+		ar_bitmap = (const unsigned long *) param->bitmap;
+		re_bitmap = (const unsigned long *) &param->bitmap[size >> 3];
+
+		mutex_lock(&mhba->sas_discovery_mutex);
+		do {
+			index = find_next_zero_bit(ar_bitmap, size, index + 1);
+			if (index >= size)
+				break;
+			mvumi_handle_hotplug(mhba, index, DEVICE_ONLINE);
+		} while (1);
+
+		index = -1;
+		do {
+			index = find_next_zero_bit(re_bitmap, size, index + 1);
+			if (index >= size)
+				break;
+			mvumi_handle_hotplug(mhba, index, DEVICE_OFFLINE);
+		} while (1);
+		mutex_unlock(&mhba->sas_discovery_mutex);
+	}
+}
+
 static void mvumi_notification(struct mvumi_hba *mhba, u8 msg, void *buffer)
 {
 	if (msg == APICDB1_EVENT_GETEVENT) {
@@ -1227,6 +1731,8 @@ static void mvumi_notification(struct mvumi_hba *mhba, u8 msg, void *buffer)
 			param = &er->events[i];
 			mvumi_show_event(mhba, param);
 		}
+	} else if (msg == APICDB1_HOST_GETEVENT) {
+		mvumi_proc_msg(mhba, buffer);
 	}
 }
 
@@ -1271,17 +1777,27 @@ static void mvumi_scan_events(struct work_struct *work)
 	kfree(mu_ev);
 }
 
-static void mvumi_launch_events(struct mvumi_hba *mhba, u8 msg)
+static void mvumi_launch_events(struct mvumi_hba *mhba, u32 isr_status)
 {
 	struct mvumi_events_wq *mu_ev;
 
-	mu_ev = kzalloc(sizeof(*mu_ev), GFP_ATOMIC);
-	if (mu_ev) {
-		INIT_WORK(&mu_ev->work_q, mvumi_scan_events);
-		mu_ev->mhba = mhba;
-		mu_ev->event = msg;
-		mu_ev->param = NULL;
-		schedule_work(&mu_ev->work_q);
+	while (isr_status & (DRBL_BUS_CHANGE | DRBL_EVENT_NOTIFY)) {
+		if (isr_status & DRBL_BUS_CHANGE) {
+			atomic_inc(&mhba->pnp_count);
+			wake_up_process(mhba->dm_thread);
+			isr_status &= ~(DRBL_BUS_CHANGE);
+			continue;
+		}
+
+		mu_ev = kzalloc(sizeof(*mu_ev), GFP_ATOMIC);
+		if (mu_ev) {
+			INIT_WORK(&mu_ev->work_q, mvumi_scan_events);
+			mu_ev->mhba = mhba;
+			mu_ev->event = APICDB1_EVENT_GETEVENT;
+			isr_status &= ~(DRBL_EVENT_NOTIFY);
+			mu_ev->param = NULL;
+			schedule_work(&mu_ev->work_q);
+		}
 	}
 }
 
@@ -1322,16 +1838,17 @@ static irqreturn_t mvumi_isr_handler(int irq, void *devp)
 		return IRQ_NONE;
 	}
 
-	if (mhba->global_isr & INT_MAP_DL_CPU2PCIEA) {
+	if (mhba->global_isr & mhba->regs->int_dl_cpu2pciea) {
+		if (mhba->isr_status & (DRBL_BUS_CHANGE | DRBL_EVENT_NOTIFY))
+			mvumi_launch_events(mhba, mhba->isr_status);
 		if (mhba->isr_status & DRBL_HANDSHAKE_ISR) {
 			dev_warn(&mhba->pdev->dev, "enter handshake again!\n");
 			mvumi_handshake(mhba);
 		}
-		if (mhba->isr_status & DRBL_EVENT_NOTIFY)
-			mvumi_launch_events(mhba, APICDB1_EVENT_GETEVENT);
+
 	}
 
-	if (mhba->global_isr & INT_MAP_COMAOUT)
+	if (mhba->global_isr & mhba->regs->int_comaout)
 		mvumi_receive_ob_list_entry(mhba);
 
 	mhba->global_isr = 0;
@@ -1358,8 +1875,7 @@ static enum mvumi_qc_result mvumi_send_command(struct mvumi_hba *mhba,
 		dev_dbg(&mhba->pdev->dev, "no free tag.\n");
 		return MV_QUEUE_COMMAND_RESULT_NO_RESOURCE;
 	}
-	if (mvumi_get_ib_list_entry(mhba, &ib_entry))
-		return MV_QUEUE_COMMAND_RESULT_NO_RESOURCE;
+	mvumi_get_ib_list_entry(mhba, &ib_entry);
 
 	cmd->frame->tag = tag_get_one(mhba, &mhba->tag_pool);
 	cmd->frame->request_id = mhba->io_seq++;
@@ -1367,21 +1883,35 @@ static enum mvumi_qc_result mvumi_send_command(struct mvumi_hba *mhba,
 	mhba->tag_cmd[cmd->frame->tag] = cmd;
 	frame_len = sizeof(*ib_frame) - 4 +
 				ib_frame->sg_counts * sizeof(struct mvumi_sgl);
-	memcpy(ib_entry, ib_frame, frame_len);
+	if (mhba->hba_capability & HS_CAPABILITY_SUPPORT_DYN_SRC) {
+		struct mvumi_dyn_list_entry *dle;
+		dle = ib_entry;
+		dle->src_low_addr =
+			cpu_to_le32(lower_32_bits(cmd->frame_phys));
+		dle->src_high_addr =
+			cpu_to_le32(upper_32_bits(cmd->frame_phys));
+		dle->if_length = (frame_len >> 2) & 0xFFF;
+	} else {
+		memcpy(ib_entry, ib_frame, frame_len);
+	}
 	return MV_QUEUE_COMMAND_RESULT_SENT;
 }
 
 static void mvumi_fire_cmd(struct mvumi_hba *mhba, struct mvumi_cmd *cmd)
 {
 	unsigned short num_of_cl_sent = 0;
+	unsigned int count;
 	enum mvumi_qc_result result;
 
 	if (cmd)
 		list_add_tail(&cmd->queue_pointer, &mhba->waiting_req_list);
+	count = mhba->instancet->check_ib_list(mhba);
+	if (list_empty(&mhba->waiting_req_list) || !count)
+		return;
 
-	while (!list_empty(&mhba->waiting_req_list)) {
+	do {
 		cmd = list_first_entry(&mhba->waiting_req_list,
-					 struct mvumi_cmd, queue_pointer);
+				       struct mvumi_cmd, queue_pointer);
 		list_del_init(&cmd->queue_pointer);
 		result = mvumi_send_command(mhba, cmd);
 		switch (result) {
@@ -1395,65 +1925,77 @@ static void mvumi_fire_cmd(struct mvumi_hba *mhba, struct mvumi_cmd *cmd)
 
 			return;
 		}
-	}
+	} while (!list_empty(&mhba->waiting_req_list) && count--);
+
 	if (num_of_cl_sent > 0)
 		mvumi_send_ib_list_entry(mhba);
 }
 
 /**
  * mvumi_enable_intr -	Enables interrupts
- * @regs:			FW register set
+ * @mhba:		Adapter soft state
  */
-static void mvumi_enable_intr(void *regs)
+static void mvumi_enable_intr(struct mvumi_hba *mhba)
 {
 	unsigned int mask;
+	struct mvumi_hw_regs *regs = mhba->regs;
 
-	iowrite32(0x3FFFFFFF, regs + CPU_ARM_TO_PCIEA_MASK_REG);
-	mask = ioread32(regs + CPU_ENPOINTA_MASK_REG);
-	mask |= INT_MAP_DL_CPU2PCIEA | INT_MAP_COMAOUT | INT_MAP_COMAERR;
-	iowrite32(mask, regs + CPU_ENPOINTA_MASK_REG);
+	iowrite32(regs->int_drbl_int_mask, regs->arm_to_pciea_mask_reg);
+	mask = ioread32(regs->enpointa_mask_reg);
+	mask |= regs->int_dl_cpu2pciea | regs->int_comaout | regs->int_comaerr;
+	iowrite32(mask, regs->enpointa_mask_reg);
 }
 
 /**
  * mvumi_disable_intr -Disables interrupt
- * @regs:			FW register set
+ * @mhba:		Adapter soft state
  */
-static void mvumi_disable_intr(void *regs)
+static void mvumi_disable_intr(struct mvumi_hba *mhba)
 {
 	unsigned int mask;
+	struct mvumi_hw_regs *regs = mhba->regs;
 
-	iowrite32(0, regs + CPU_ARM_TO_PCIEA_MASK_REG);
-	mask = ioread32(regs + CPU_ENPOINTA_MASK_REG);
-	mask &= ~(INT_MAP_DL_CPU2PCIEA | INT_MAP_COMAOUT | INT_MAP_COMAERR);
-	iowrite32(mask, regs + CPU_ENPOINTA_MASK_REG);
+	iowrite32(0, regs->arm_to_pciea_mask_reg);
+	mask = ioread32(regs->enpointa_mask_reg);
+	mask &= ~(regs->int_dl_cpu2pciea | regs->int_comaout |
+							regs->int_comaerr);
+	iowrite32(mask, regs->enpointa_mask_reg);
 }
 
 static int mvumi_clear_intr(void *extend)
 {
 	struct mvumi_hba *mhba = (struct mvumi_hba *) extend;
 	unsigned int status, isr_status = 0, tmp = 0;
-	void *regs = mhba->mmio;
+	struct mvumi_hw_regs *regs = mhba->regs;
 
-	status = ioread32(regs + CPU_MAIN_INT_CAUSE_REG);
-	if (!(status & INT_MAP_MU) || status == 0xFFFFFFFF)
+	status = ioread32(regs->main_int_cause_reg);
+	if (!(status & regs->int_mu) || status == 0xFFFFFFFF)
 		return 1;
-	if (unlikely(status & INT_MAP_COMAERR)) {
-		tmp = ioread32(regs + CLA_ISR_CAUSE);
-		if (tmp & (CLIC_IN_ERR_IRQ | CLIC_OUT_ERR_IRQ))
-			iowrite32(tmp & (CLIC_IN_ERR_IRQ | CLIC_OUT_ERR_IRQ),
-					regs + CLA_ISR_CAUSE);
-		status ^= INT_MAP_COMAERR;
+	if (unlikely(status & regs->int_comaerr)) {
+		tmp = ioread32(regs->outb_isr_cause);
+		if (mhba->pdev->device == PCI_DEVICE_ID_MARVELL_MV9580) {
+			if (tmp & regs->clic_out_err) {
+				iowrite32(tmp & regs->clic_out_err,
+							regs->outb_isr_cause);
+			}
+		} else {
+			if (tmp & (regs->clic_in_err | regs->clic_out_err))
+				iowrite32(tmp & (regs->clic_in_err |
+						regs->clic_out_err),
+						regs->outb_isr_cause);
+		}
+		status ^= mhba->regs->int_comaerr;
 		/* inbound or outbound parity error, command will timeout */
 	}
-	if (status & INT_MAP_COMAOUT) {
-		tmp = ioread32(regs + CLA_ISR_CAUSE);
-		if (tmp & CLIC_OUT_IRQ)
-			iowrite32(tmp & CLIC_OUT_IRQ, regs + CLA_ISR_CAUSE);
+	if (status & regs->int_comaout) {
+		tmp = ioread32(regs->outb_isr_cause);
+		if (tmp & regs->clic_irq)
+			iowrite32(tmp & regs->clic_irq, regs->outb_isr_cause);
 	}
-	if (status & INT_MAP_DL_CPU2PCIEA) {
-		isr_status = ioread32(regs + CPU_ARM_TO_PCIEA_DRBL_REG);
+	if (status & regs->int_dl_cpu2pciea) {
+		isr_status = ioread32(regs->arm_to_pciea_drbl_reg);
 		if (isr_status)
-			iowrite32(isr_status, regs + CPU_ARM_TO_PCIEA_DRBL_REG);
+			iowrite32(isr_status, regs->arm_to_pciea_drbl_reg);
 	}
 
 	mhba->global_isr = status;
@@ -1464,24 +2006,38 @@ static int mvumi_clear_intr(void *extend)
 
 /**
  * mvumi_read_fw_status_reg - returns the current FW status value
- * @regs:			FW register set
+ * @mhba:		Adapter soft state
  */
-static unsigned int mvumi_read_fw_status_reg(void *regs)
+static unsigned int mvumi_read_fw_status_reg(struct mvumi_hba *mhba)
 {
 	unsigned int status;
 
-	status = ioread32(regs + CPU_ARM_TO_PCIEA_DRBL_REG);
+	status = ioread32(mhba->regs->arm_to_pciea_drbl_reg);
 	if (status)
-		iowrite32(status, regs + CPU_ARM_TO_PCIEA_DRBL_REG);
+		iowrite32(status, mhba->regs->arm_to_pciea_drbl_reg);
 	return status;
 }
 
-static struct mvumi_instance_template mvumi_instance_template = {
+static struct mvumi_instance_template mvumi_instance_9143 = {
 	.fire_cmd = mvumi_fire_cmd,
 	.enable_intr = mvumi_enable_intr,
 	.disable_intr = mvumi_disable_intr,
 	.clear_intr = mvumi_clear_intr,
 	.read_fw_status_reg = mvumi_read_fw_status_reg,
+	.check_ib_list = mvumi_check_ib_list_9143,
+	.check_ob_list = mvumi_check_ob_list_9143,
+	.reset_host = mvumi_reset_host_9143,
+};
+
+static struct mvumi_instance_template mvumi_instance_9580 = {
+	.fire_cmd = mvumi_fire_cmd,
+	.enable_intr = mvumi_enable_intr,
+	.disable_intr = mvumi_disable_intr,
+	.clear_intr = mvumi_clear_intr,
+	.read_fw_status_reg = mvumi_read_fw_status_reg,
+	.check_ib_list = mvumi_check_ib_list_9580,
+	.check_ob_list = mvumi_check_ob_list_9580,
+	.reset_host = mvumi_reset_host_9580,
 };
 
 static int mvumi_slave_configure(struct scsi_device *sdev)
@@ -1681,6 +2237,124 @@ static struct scsi_transport_template mvumi_transport_template = {
 	.eh_timed_out = mvumi_timed_out,
 };
 
+static int mvumi_cfg_hw_reg(struct mvumi_hba *mhba)
+{
+	void *base = NULL;
+	struct mvumi_hw_regs *regs;
+
+	switch (mhba->pdev->device) {
+	case PCI_DEVICE_ID_MARVELL_MV9143:
+		mhba->mmio = mhba->base_addr[0];
+		base = mhba->mmio;
+		if (!mhba->regs) {
+			mhba->regs = kzalloc(sizeof(*regs), GFP_KERNEL);
+			if (mhba->regs == NULL)
+				return -ENOMEM;
+		}
+		regs = mhba->regs;
+
+		/* For Arm */
+		regs->ctrl_sts_reg          = base + 0x20104;
+		regs->rstoutn_mask_reg      = base + 0x20108;
+		regs->sys_soft_rst_reg      = base + 0x2010C;
+		regs->main_int_cause_reg    = base + 0x20200;
+		regs->enpointa_mask_reg     = base + 0x2020C;
+		regs->rstoutn_en_reg        = base + 0xF1400;
+		/* For Doorbell */
+		regs->pciea_to_arm_drbl_reg = base + 0x20400;
+		regs->arm_to_pciea_drbl_reg = base + 0x20408;
+		regs->arm_to_pciea_mask_reg = base + 0x2040C;
+		regs->pciea_to_arm_msg0     = base + 0x20430;
+		regs->pciea_to_arm_msg1     = base + 0x20434;
+		regs->arm_to_pciea_msg0     = base + 0x20438;
+		regs->arm_to_pciea_msg1     = base + 0x2043C;
+
+		/* For Message Unit */
+
+		regs->inb_aval_count_basel  = base + 0x508;
+		regs->inb_aval_count_baseh  = base + 0x50C;
+		regs->inb_write_pointer     = base + 0x518;
+		regs->inb_read_pointer      = base + 0x51C;
+		regs->outb_coal_cfg         = base + 0x568;
+		regs->outb_copy_basel       = base + 0x5B0;
+		regs->outb_copy_baseh       = base + 0x5B4;
+		regs->outb_copy_pointer     = base + 0x544;
+		regs->outb_read_pointer     = base + 0x548;
+		regs->outb_isr_cause        = base + 0x560;
+		regs->outb_coal_cfg         = base + 0x568;
+		/* Bit setting for HW */
+		regs->int_comaout           = 1 << 8;
+		regs->int_comaerr           = 1 << 6;
+		regs->int_dl_cpu2pciea      = 1 << 1;
+		regs->cl_pointer_toggle     = 1 << 12;
+		regs->clic_irq              = 1 << 1;
+		regs->clic_in_err           = 1 << 8;
+		regs->clic_out_err          = 1 << 12;
+		regs->cl_slot_num_mask      = 0xFFF;
+		regs->int_drbl_int_mask     = 0x3FFFFFFF;
+		regs->int_mu = regs->int_dl_cpu2pciea | regs->int_comaout |
+							regs->int_comaerr;
+		break;
+	case PCI_DEVICE_ID_MARVELL_MV9580:
+		mhba->mmio = mhba->base_addr[2];
+		base = mhba->mmio;
+		if (!mhba->regs) {
+			mhba->regs = kzalloc(sizeof(*regs), GFP_KERNEL);
+			if (mhba->regs == NULL)
+				return -ENOMEM;
+		}
+		regs = mhba->regs;
+		/* For Arm */
+		regs->ctrl_sts_reg          = base + 0x20104;
+		regs->rstoutn_mask_reg      = base + 0x1010C;
+		regs->sys_soft_rst_reg      = base + 0x10108;
+		regs->main_int_cause_reg    = base + 0x10200;
+		regs->enpointa_mask_reg     = base + 0x1020C;
+		regs->rstoutn_en_reg        = base + 0xF1400;
+
+		/* For Doorbell */
+		regs->pciea_to_arm_drbl_reg = base + 0x10460;
+		regs->arm_to_pciea_drbl_reg = base + 0x10480;
+		regs->arm_to_pciea_mask_reg = base + 0x10484;
+		regs->pciea_to_arm_msg0     = base + 0x10400;
+		regs->pciea_to_arm_msg1     = base + 0x10404;
+		regs->arm_to_pciea_msg0     = base + 0x10420;
+		regs->arm_to_pciea_msg1     = base + 0x10424;
+
+		/* For reset*/
+		regs->reset_request         = base + 0x10108;
+		regs->reset_enable          = base + 0x1010c;
+
+		/* For Message Unit */
+		regs->inb_aval_count_basel  = base + 0x4008;
+		regs->inb_aval_count_baseh  = base + 0x400C;
+		regs->inb_write_pointer     = base + 0x4018;
+		regs->inb_read_pointer      = base + 0x401C;
+		regs->outb_copy_basel       = base + 0x4058;
+		regs->outb_copy_baseh       = base + 0x405C;
+		regs->outb_copy_pointer     = base + 0x406C;
+		regs->outb_read_pointer     = base + 0x4070;
+		regs->outb_coal_cfg         = base + 0x4080;
+		regs->outb_isr_cause        = base + 0x4088;
+		/* Bit setting for HW */
+		regs->int_comaout           = 1 << 4;
+		regs->int_dl_cpu2pciea      = 1 << 12;
+		regs->int_comaerr           = 1 << 29;
+		regs->cl_pointer_toggle     = 1 << 14;
+		regs->cl_slot_num_mask      = 0x3FFF;
+		regs->clic_irq              = 1 << 0;
+		regs->clic_out_err          = 1 << 1;
+		regs->int_drbl_int_mask     = 0x3FFFFFFF;
+		regs->int_mu = regs->int_dl_cpu2pciea | regs->int_comaout;
+		break;
+	default:
+		return -1;
+		break;
+	}
+
+	return 0;
+}
+
 /**
  * mvumi_init_fw -	Initializes the FW
  * @mhba:		Adapter soft state
@@ -1699,15 +2373,18 @@ static int mvumi_init_fw(struct mvumi_hba *mhba)
 	if (ret)
 		goto fail_ioremap;
 
-	mhba->mmio = mhba->base_addr[0];
-
 	switch (mhba->pdev->device) {
 	case PCI_DEVICE_ID_MARVELL_MV9143:
-		mhba->instancet = &mvumi_instance_template;
+		mhba->instancet = &mvumi_instance_9143;
 		mhba->io_seq = 0;
 		mhba->max_sge = MVUMI_MAX_SG_ENTRY;
 		mhba->request_id_enabled = 1;
 		break;
+	case PCI_DEVICE_ID_MARVELL_MV9580:
+		mhba->instancet = &mvumi_instance_9580;
+		mhba->io_seq = 0;
+		mhba->max_sge = MVUMI_MAX_SG_ENTRY;
+		break;
 	default:
 		dev_err(&mhba->pdev->dev, "device 0x%x not supported!\n",
 							mhba->pdev->device);
@@ -1717,15 +2394,21 @@ static int mvumi_init_fw(struct mvumi_hba *mhba)
 	}
 	dev_dbg(&mhba->pdev->dev, "device id : %04X is found.\n",
 							mhba->pdev->device);
-
-	mhba->handshake_page = kzalloc(HSP_MAX_SIZE, GFP_KERNEL);
+	ret = mvumi_cfg_hw_reg(mhba);
+	if (ret) {
+		dev_err(&mhba->pdev->dev,
+			"failed to allocate memory for reg\n");
+		ret = -ENOMEM;
+		goto fail_alloc_mem;
+	}
+	mhba->handshake_page = pci_alloc_consistent(mhba->pdev, HSP_MAX_SIZE,
+						&mhba->handshake_page_phys);
 	if (!mhba->handshake_page) {
 		dev_err(&mhba->pdev->dev,
 			"failed to allocate memory for handshake\n");
 		ret = -ENOMEM;
-		goto fail_alloc_mem;
+		goto fail_alloc_page;
 	}
-	mhba->handshake_page_phys = virt_to_phys(mhba->handshake_page);
 
 	if (mvumi_start(mhba)) {
 		ret = -EINVAL;
@@ -1739,7 +2422,10 @@ static int mvumi_init_fw(struct mvumi_hba *mhba)
 
 fail_ready_state:
 	mvumi_release_mem_resource(mhba);
-	kfree(mhba->handshake_page);
+	pci_free_consistent(mhba->pdev, HSP_MAX_SIZE,
+		mhba->handshake_page, mhba->handshake_page_phys);
+fail_alloc_page:
+	kfree(mhba->regs);
 fail_alloc_mem:
 	mvumi_unmap_pci_addr(mhba->pdev, mhba->base_addr);
 fail_ioremap:
@@ -1755,6 +2441,7 @@ fail_ioremap:
 static int mvumi_io_attach(struct mvumi_hba *mhba)
 {
 	struct Scsi_Host *host = mhba->shost;
+	struct scsi_device *sdev = NULL;
 	int ret;
 	unsigned int max_sg = (mhba->ib_max_size + 4 -
 		sizeof(struct mvumi_msg_frame)) / sizeof(struct mvumi_sgl);
@@ -1764,7 +2451,7 @@ static int mvumi_io_attach(struct mvumi_hba *mhba)
 	host->can_queue = (mhba->max_io - 1) ? (mhba->max_io - 1) : 1;
 	host->sg_tablesize = mhba->max_sge > max_sg ? max_sg : mhba->max_sge;
 	host->max_sectors = mhba->max_transfer_size / 512;
-	host->cmd_per_lun =  (mhba->max_io - 1) ? (mhba->max_io - 1) : 1;
+	host->cmd_per_lun = (mhba->max_io - 1) ? (mhba->max_io - 1) : 1;
 	host->max_id = mhba->max_target_id;
 	host->max_cmd_len = MAX_COMMAND_SIZE;
 	host->transportt = &mvumi_transport_template;
@@ -1775,9 +2462,43 @@ static int mvumi_io_attach(struct mvumi_hba *mhba)
 		return ret;
 	}
 	mhba->fw_flag |= MVUMI_FW_ATTACH;
-	scsi_scan_host(host);
 
+	mutex_lock(&mhba->sas_discovery_mutex);
+	if (mhba->pdev->device == PCI_DEVICE_ID_MARVELL_MV9580)
+		ret = scsi_add_device(host, 0, mhba->max_target_id - 1, 0);
+	else
+		ret = 0;
+	if (ret) {
+		dev_err(&mhba->pdev->dev, "add virtual device failed\n");
+		mutex_unlock(&mhba->sas_discovery_mutex);
+		goto fail_add_device;
+	}
+
+	mhba->dm_thread = kthread_create(mvumi_rescan_bus,
+						mhba, "mvumi_scanthread");
+	if (IS_ERR(mhba->dm_thread)) {
+		dev_err(&mhba->pdev->dev,
+			"failed to create device scan thread\n");
+		mutex_unlock(&mhba->sas_discovery_mutex);
+		goto fail_create_thread;
+	}
+	atomic_set(&mhba->pnp_count, 1);
+	wake_up_process(mhba->dm_thread);
+
+	mutex_unlock(&mhba->sas_discovery_mutex);
 	return 0;
+
+fail_create_thread:
+	if (mhba->pdev->device == PCI_DEVICE_ID_MARVELL_MV9580)
+		sdev = scsi_device_lookup(mhba->shost, 0,
+						mhba->max_target_id - 1, 0);
+	if (sdev) {
+		scsi_remove_device(sdev);
+		scsi_device_put(sdev);
+	}
+fail_add_device:
+	scsi_remove_host(mhba->shost);
+	return ret;
 }
 
 /**
@@ -1828,8 +2549,12 @@ static int __devinit mvumi_probe_one(struct pci_dev *pdev,
 	INIT_LIST_HEAD(&mhba->free_ob_list);
 	INIT_LIST_HEAD(&mhba->res_list);
 	INIT_LIST_HEAD(&mhba->waiting_req_list);
+	mutex_init(&mhba->device_lock);
+	INIT_LIST_HEAD(&mhba->mhba_dev_list);
+	INIT_LIST_HEAD(&mhba->shost_dev_list);
 	atomic_set(&mhba->fw_outstanding, 0);
 	init_waitqueue_head(&mhba->int_cmd_wait_q);
+	mutex_init(&mhba->sas_discovery_mutex);
 
 	mhba->pdev = pdev;
 	mhba->shost = host;
@@ -1845,19 +2570,22 @@ static int __devinit mvumi_probe_one(struct pci_dev *pdev,
 		dev_err(&pdev->dev, "failed to register IRQ\n");
 		goto fail_init_irq;
 	}
-	mhba->instancet->enable_intr(mhba->mmio);
+
+	mhba->instancet->enable_intr(mhba);
 	pci_set_drvdata(pdev, mhba);
 
 	ret = mvumi_io_attach(mhba);
 	if (ret)
 		goto fail_io_attach;
+
+	mvumi_backup_bar_addr(mhba);
 	dev_dbg(&pdev->dev, "probe mvumi driver successfully.\n");
 
 	return 0;
 
 fail_io_attach:
 	pci_set_drvdata(pdev, NULL);
-	mhba->instancet->disable_intr(mhba->mmio);
+	mhba->instancet->disable_intr(mhba);
 	free_irq(mhba->pdev->irq, mhba);
 fail_init_irq:
 	mvumi_release_fw(mhba);
@@ -1877,11 +2605,17 @@ static void mvumi_detach_one(struct pci_dev *pdev)
 	struct mvumi_hba *mhba;
 
 	mhba = pci_get_drvdata(pdev);
+	if (mhba->dm_thread) {
+		kthread_stop(mhba->dm_thread);
+		mhba->dm_thread = NULL;
+	}
+
+	mvumi_detach_devices(mhba);
 	host = mhba->shost;
 	scsi_remove_host(mhba->shost);
 	mvumi_flush_cache(mhba);
 
-	mhba->instancet->disable_intr(mhba->mmio);
+	mhba->instancet->disable_intr(mhba);
 	free_irq(mhba->pdev->irq, mhba);
 	mvumi_release_fw(mhba);
 	scsi_host_put(host);
@@ -1909,7 +2643,7 @@ static int mvumi_suspend(struct pci_dev *pdev, pm_message_t state)
 	mvumi_flush_cache(mhba);
 
 	pci_set_drvdata(pdev, mhba);
-	mhba->instancet->disable_intr(mhba->mmio);
+	mhba->instancet->disable_intr(mhba);
 	free_irq(mhba->pdev->irq, mhba);
 	mvumi_unmap_pci_addr(pdev, mhba->base_addr);
 	pci_release_regions(pdev);
@@ -1956,8 +2690,13 @@ static int mvumi_resume(struct pci_dev *pdev)
 	if (ret)
 		goto release_regions;
 
+	if (mvumi_cfg_hw_reg(mhba)) {
+		ret = -EINVAL;
+		goto unmap_pci_addr;
+	}
+
 	mhba->mmio = mhba->base_addr[0];
-	mvumi_reset(mhba->mmio);
+	mvumi_reset(mhba);
 
 	if (mvumi_start(mhba)) {
 		ret = -EINVAL;
@@ -1970,7 +2709,7 @@ static int mvumi_resume(struct pci_dev *pdev)
 		dev_err(&pdev->dev, "failed to register IRQ\n");
 		goto unmap_pci_addr;
 	}
-	mhba->instancet->enable_intr(mhba->mmio);
+	mhba->instancet->enable_intr(mhba);
 
 	return 0;
 
diff --git a/drivers/scsi/mvumi.h b/drivers/scsi/mvumi.h
index 10b9237566f..e360135fd1b 100644
--- a/drivers/scsi/mvumi.h
+++ b/drivers/scsi/mvumi.h
@@ -34,51 +34,87 @@
 #define MV_DRIVER_NAME			"mvumi"
 #define PCI_VENDOR_ID_MARVELL_2		0x1b4b
 #define PCI_DEVICE_ID_MARVELL_MV9143	0x9143
+#define PCI_DEVICE_ID_MARVELL_MV9580	0x9580
 
 #define MVUMI_INTERNAL_CMD_WAIT_TIME	45
+#define MVUMI_INQUIRY_LENGTH		44
+#define MVUMI_INQUIRY_UUID_OFF		36
+#define MVUMI_INQUIRY_UUID_LEN		8
 
 #define IS_DMA64			(sizeof(dma_addr_t) == 8)
 
 enum mvumi_qc_result {
-	MV_QUEUE_COMMAND_RESULT_SENT	= 0,
+	MV_QUEUE_COMMAND_RESULT_SENT = 0,
 	MV_QUEUE_COMMAND_RESULT_NO_RESOURCE,
 };
 
-enum {
-	/*******************************************/
-
-	/* ARM Mbus Registers Map	*/
-
-	/*******************************************/
-	CPU_MAIN_INT_CAUSE_REG	= 0x20200,
-	CPU_MAIN_IRQ_MASK_REG	= 0x20204,
-	CPU_MAIN_FIQ_MASK_REG	= 0x20208,
-	CPU_ENPOINTA_MASK_REG	= 0x2020C,
-	CPU_ENPOINTB_MASK_REG	= 0x20210,
-
-	INT_MAP_COMAERR		= 1 << 6,
-	INT_MAP_COMAIN		= 1 << 7,
-	INT_MAP_COMAOUT		= 1 << 8,
-	INT_MAP_COMBERR		= 1 << 9,
-	INT_MAP_COMBIN		= 1 << 10,
-	INT_MAP_COMBOUT		= 1 << 11,
-
-	INT_MAP_COMAINT	= (INT_MAP_COMAOUT | INT_MAP_COMAERR),
-	INT_MAP_COMBINT	= (INT_MAP_COMBOUT | INT_MAP_COMBIN | INT_MAP_COMBERR),
-
-	INT_MAP_DL_PCIEA2CPU	= 1 << 0,
-	INT_MAP_DL_CPU2PCIEA	= 1 << 1,
-
-	/***************************************/
+struct mvumi_hw_regs {
+	/* For CPU */
+	void *main_int_cause_reg;
+	void *enpointa_mask_reg;
+	void *enpointb_mask_reg;
+	void *rstoutn_en_reg;
+	void *ctrl_sts_reg;
+	void *rstoutn_mask_reg;
+	void *sys_soft_rst_reg;
+
+	/* For Doorbell */
+	void *pciea_to_arm_drbl_reg;
+	void *arm_to_pciea_drbl_reg;
+	void *arm_to_pciea_mask_reg;
+	void *pciea_to_arm_msg0;
+	void *pciea_to_arm_msg1;
+	void *arm_to_pciea_msg0;
+	void *arm_to_pciea_msg1;
+
+	/* reset register */
+	void *reset_request;
+	void *reset_enable;
+
+	/* For Message Unit */
+	void *inb_list_basel;
+	void *inb_list_baseh;
+	void *inb_aval_count_basel;
+	void *inb_aval_count_baseh;
+	void *inb_write_pointer;
+	void *inb_read_pointer;
+	void *outb_list_basel;
+	void *outb_list_baseh;
+	void *outb_copy_basel;
+	void *outb_copy_baseh;
+	void *outb_copy_pointer;
+	void *outb_read_pointer;
+	void *inb_isr_cause;
+	void *outb_isr_cause;
+	void *outb_coal_cfg;
+	void *outb_coal_timeout;
+
+	/* Bit setting for HW */
+	u32 int_comaout;
+	u32 int_comaerr;
+	u32 int_dl_cpu2pciea;
+	u32 int_mu;
+	u32 int_drbl_int_mask;
+	u32 int_main_int_mask;
+	u32 cl_pointer_toggle;
+	u32 cl_slot_num_mask;
+	u32 clic_irq;
+	u32 clic_in_err;
+	u32 clic_out_err;
+};
 
-	/* ARM Doorbell Registers Map		*/
+struct mvumi_dyn_list_entry {
+	u32 src_low_addr;
+	u32 src_high_addr;
+	u32 if_length;
+	u32 reserve;
+};
 
-	/***************************************/
-	CPU_PCIEA_TO_ARM_DRBL_REG	= 0x20400,
-	CPU_PCIEA_TO_ARM_MASK_REG	= 0x20404,
-	CPU_ARM_TO_PCIEA_DRBL_REG	= 0x20408,
-	CPU_ARM_TO_PCIEA_MASK_REG	= 0x2040C,
+#define SCSI_CMD_MARVELL_SPECIFIC	0xE1
+#define CDB_CORE_MODULE			0x1
+#define CDB_CORE_SHUTDOWN		0xB
 
+enum {
 	DRBL_HANDSHAKE			= 1 << 0,
 	DRBL_SOFT_RESET			= 1 << 1,
 	DRBL_BUS_CHANGE			= 1 << 2,
@@ -86,46 +122,6 @@ enum {
 	DRBL_MU_RESET			= 1 << 4,
 	DRBL_HANDSHAKE_ISR		= DRBL_HANDSHAKE,
 
-	CPU_PCIEA_TO_ARM_MSG0		= 0x20430,
-	CPU_PCIEA_TO_ARM_MSG1		= 0x20434,
-	CPU_ARM_TO_PCIEA_MSG0		= 0x20438,
-	CPU_ARM_TO_PCIEA_MSG1		= 0x2043C,
-
-	/*******************************************/
-
-	/* ARM Communication List Registers Map    */
-
-	/*******************************************/
-	CLA_INB_LIST_BASEL		= 0x500,
-	CLA_INB_LIST_BASEH		= 0x504,
-	CLA_INB_AVAL_COUNT_BASEL	= 0x508,
-	CLA_INB_AVAL_COUNT_BASEH	= 0x50C,
-	CLA_INB_DESTI_LIST_BASEL	= 0x510,
-	CLA_INB_DESTI_LIST_BASEH	= 0x514,
-	CLA_INB_WRITE_POINTER		= 0x518,
-	CLA_INB_READ_POINTER		= 0x51C,
-
-	CLA_OUTB_LIST_BASEL		= 0x530,
-	CLA_OUTB_LIST_BASEH		= 0x534,
-	CLA_OUTB_SOURCE_LIST_BASEL	= 0x538,
-	CLA_OUTB_SOURCE_LIST_BASEH	= 0x53C,
-	CLA_OUTB_COPY_POINTER		= 0x544,
-	CLA_OUTB_READ_POINTER		= 0x548,
-
-	CLA_ISR_CAUSE			= 0x560,
-	CLA_ISR_MASK			= 0x564,
-
-	INT_MAP_MU		= (INT_MAP_DL_CPU2PCIEA | INT_MAP_COMAINT),
-
-	CL_POINTER_TOGGLE		= 1 << 12,
-
-	CLIC_IN_IRQ			= 1 << 0,
-	CLIC_OUT_IRQ			= 1 << 1,
-	CLIC_IN_ERR_IRQ			= 1 << 8,
-	CLIC_OUT_ERR_IRQ		= 1 << 12,
-
-	CL_SLOT_NUM_MASK		= 0xFFF,
-
 	/*
 	* Command flag is the flag for the CDB command itself
 	*/
@@ -137,15 +133,23 @@ enum {
 	CMD_FLAG_DATA_IN		= 1 << 3,
 	/* 1-host write data */
 	CMD_FLAG_DATA_OUT		= 1 << 4,
-
-	SCSI_CMD_MARVELL_SPECIFIC	= 0xE1,
-	CDB_CORE_SHUTDOWN		= 0xB,
+	CMD_FLAG_PRDT_IN_HOST		= 1 << 5,
 };
 
 #define APICDB0_EVENT			0xF4
 #define APICDB1_EVENT_GETEVENT		0
+#define APICDB1_HOST_GETEVENT		1
 #define MAX_EVENTS_RETURNED		6
 
+#define DEVICE_OFFLINE	0
+#define DEVICE_ONLINE	1
+
+struct mvumi_hotplug_event {
+	u16 size;
+	u8 dummy[2];
+	u8 bitmap[0];
+};
+
 struct mvumi_driver_event {
 	u32	time_stamp;
 	u32	sequence_no;
@@ -172,8 +176,14 @@ struct mvumi_events_wq {
 	void *param;
 };
 
+#define HS_CAPABILITY_SUPPORT_COMPACT_SG	(1U << 4)
+#define HS_CAPABILITY_SUPPORT_PRD_HOST		(1U << 5)
+#define HS_CAPABILITY_SUPPORT_DYN_SRC		(1U << 6)
+#define HS_CAPABILITY_NEW_PAGE_IO_DEPTH_DEF	(1U << 14)
+
 #define MVUMI_MAX_SG_ENTRY	32
 #define SGD_EOT			(1L << 27)
+#define SGD_EOT_CP		(1L << 22)
 
 struct mvumi_sgl {
 	u32	baseaddr_l;
@@ -181,6 +191,39 @@ struct mvumi_sgl {
 	u32	flags;
 	u32	size;
 };
+struct mvumi_compact_sgl {
+	u32	baseaddr_l;
+	u32	baseaddr_h;
+	u32	flags;
+};
+
+#define GET_COMPACT_SGD_SIZE(sgd)	\
+	((((struct mvumi_compact_sgl *)(sgd))->flags) & 0x3FFFFFL)
+
+#define SET_COMPACT_SGD_SIZE(sgd, sz) do {			\
+	(((struct mvumi_compact_sgl *)(sgd))->flags) &= ~0x3FFFFFL;	\
+	(((struct mvumi_compact_sgl *)(sgd))->flags) |= (sz);		\
+} while (0)
+#define sgd_getsz(_mhba, sgd, sz) do {				\
+	if (_mhba->hba_capability & HS_CAPABILITY_SUPPORT_COMPACT_SG)	\
+		(sz) = GET_COMPACT_SGD_SIZE(sgd);	\
+	else \
+		(sz) = (sgd)->size;			\
+} while (0)
+
+#define sgd_setsz(_mhba, sgd, sz) do {				\
+	if (_mhba->hba_capability & HS_CAPABILITY_SUPPORT_COMPACT_SG)	\
+		SET_COMPACT_SGD_SIZE(sgd, sz);		\
+	else \
+		(sgd)->size = (sz);			\
+} while (0)
+
+#define sgd_inc(_mhba, sgd) do {	\
+	if (_mhba->hba_capability & HS_CAPABILITY_SUPPORT_COMPACT_SG)	\
+		sgd = (struct mvumi_sgl *)(((unsigned char *) (sgd)) + 12); \
+	else \
+		sgd = (struct mvumi_sgl *)(((unsigned char *) (sgd)) + 16); \
+} while (0)
 
 struct mvumi_res {
 	struct list_head entry;
@@ -197,7 +240,7 @@ enum resource_type {
 };
 
 struct mvumi_sense_data {
-	u8 error_eode:7;
+	u8 error_code:7;
 	u8 valid:1;
 	u8 segment_number;
 	u8 sense_key:4;
@@ -220,6 +263,7 @@ struct mvumi_sense_data {
 struct mvumi_cmd {
 	struct list_head queue_pointer;
 	struct mvumi_msg_frame *frame;
+	dma_addr_t frame_phys;
 	struct scsi_cmnd *scmd;
 	atomic_t sync_cmd;
 	void *data_buf;
@@ -393,7 +437,8 @@ struct mvumi_hs_page2 {
 	u16 frame_length;
 
 	u8 host_type;
-	u8 reserved[3];
+	u8 host_cap;
+	u8 reserved[2];
 	struct version_info host_ver;
 	u32 system_io_bus;
 	u32 slot_number;
@@ -435,8 +480,17 @@ struct mvumi_tag {
 	unsigned short size;
 };
 
+struct mvumi_device {
+	struct list_head list;
+	struct scsi_device *sdev;
+	u64	wwid;
+	u8	dev_type;
+	int	id;
+};
+
 struct mvumi_hba {
 	void *base_addr[MAX_BASE_ADDRESS];
+	u32 pci_base[MAX_BASE_ADDRESS];
 	void *mmio;
 	struct list_head cmd_pool;
 	struct Scsi_Host *shost;
@@ -449,6 +503,9 @@ struct mvumi_hba {
 	void *ib_list;
 	dma_addr_t ib_list_phys;
 
+	void *ib_frame;
+	dma_addr_t ib_frame_phys;
+
 	void *ob_list;
 	dma_addr_t ob_list_phys;
 
@@ -477,12 +534,14 @@ struct mvumi_hba {
 	unsigned char hba_total_pages;
 	unsigned char fw_flag;
 	unsigned char request_id_enabled;
+	unsigned char eot_flag;
 	unsigned short hba_capability;
 	unsigned short io_seq;
 
 	unsigned int ib_cur_slot;
 	unsigned int ob_cur_slot;
 	unsigned int fw_state;
+	struct mutex sas_discovery_mutex;
 
 	struct list_head ob_data_list;
 	struct list_head free_ob_list;
@@ -491,14 +550,24 @@ struct mvumi_hba {
 
 	struct mvumi_tag tag_pool;
 	struct mvumi_cmd **tag_cmd;
+	struct mvumi_hw_regs *regs;
+	struct mutex device_lock;
+	struct list_head mhba_dev_list;
+	struct list_head shost_dev_list;
+	struct task_struct *dm_thread;
+	atomic_t pnp_count;
 };
 
 struct mvumi_instance_template {
-	void (*fire_cmd)(struct mvumi_hba *, struct mvumi_cmd *);
-	void (*enable_intr)(void *) ;
-	void (*disable_intr)(void *);
-	int (*clear_intr)(void *);
-	unsigned int (*read_fw_status_reg)(void *);
+	void (*fire_cmd) (struct mvumi_hba *, struct mvumi_cmd *);
+	void (*enable_intr) (struct mvumi_hba *);
+	void (*disable_intr) (struct mvumi_hba *);
+	int (*clear_intr) (void *);
+	unsigned int (*read_fw_status_reg) (struct mvumi_hba *);
+	unsigned int (*check_ib_list) (struct mvumi_hba *);
+	int (*check_ob_list) (struct mvumi_hba *, unsigned int *,
+			      unsigned int *);
+	int (*reset_host) (struct mvumi_hba *);
 };
 
 extern struct timezone sys_tz;
diff --git a/drivers/scsi/qla2xxx/qla_init.c b/drivers/scsi/qla2xxx/qla_init.c
index 799a58bb985..48fca47384b 100644
--- a/drivers/scsi/qla2xxx/qla_init.c
+++ b/drivers/scsi/qla2xxx/qla_init.c
@@ -2080,6 +2080,7 @@ qla2x00_configure_hba(scsi_qla_host_t *vha)
 	uint8_t       domain;
 	char		connect_type[22];
 	struct qla_hw_data *ha = vha->hw;
+	unsigned long flags;
 
 	/* Get host addresses. */
 	rval = qla2x00_get_adapter_id(vha,
@@ -2154,9 +2155,9 @@ qla2x00_configure_hba(scsi_qla_host_t *vha)
 	vha->d_id.b.area = area;
 	vha->d_id.b.al_pa = al_pa;
 
-	spin_lock(&ha->vport_slock);
+	spin_lock_irqsave(&ha->vport_slock, flags);
 	qlt_update_vp_map(vha, SET_AL_PA);
-	spin_unlock(&ha->vport_slock);
+	spin_unlock_irqrestore(&ha->vport_slock, flags);
 
 	if (!vha->flags.init_done)
 		ql_log(ql_log_info, vha, 0x2010,
diff --git a/drivers/scsi/qla2xxx/qla_target.c b/drivers/scsi/qla2xxx/qla_target.c
index bddc97c5c8e..0e09d8f433d 100644
--- a/drivers/scsi/qla2xxx/qla_target.c
+++ b/drivers/scsi/qla2xxx/qla_target.c
@@ -1403,7 +1403,7 @@ static void qlt_24xx_send_task_mgmt_ctio(struct scsi_qla_host *ha,
 	ctio->u.status1.scsi_status =
 	    __constant_cpu_to_le16(SS_RESPONSE_INFO_LEN_VALID);
 	ctio->u.status1.response_len = __constant_cpu_to_le16(8);
-	((uint32_t *)ctio->u.status1.sense_data)[0] = cpu_to_be32(resp_code);
+	ctio->u.status1.sense_data[0] = resp_code;
 
 	qla2x00_start_iocbs(ha, ha->req);
 }
diff --git a/drivers/scsi/qla2xxx/tcm_qla2xxx.c b/drivers/scsi/qla2xxx/tcm_qla2xxx.c
index 4752f65a927..2358c16c4c8 100644
--- a/drivers/scsi/qla2xxx/tcm_qla2xxx.c
+++ b/drivers/scsi/qla2xxx/tcm_qla2xxx.c
@@ -735,17 +735,6 @@ static int tcm_qla2xxx_queue_tm_rsp(struct se_cmd *se_cmd)
 	return 0;
 }
 
-static u16 tcm_qla2xxx_get_fabric_sense_len(void)
-{
-	return 0;
-}
-
-static u16 tcm_qla2xxx_set_fabric_sense_len(struct se_cmd *se_cmd,
-					u32 sense_length)
-{
-	return 0;
-}
-
 /* Local pointer to allocated TCM configfs fabric module */
 struct target_fabric_configfs *tcm_qla2xxx_fabric_configfs;
 struct target_fabric_configfs *tcm_qla2xxx_npiv_fabric_configfs;
@@ -1691,8 +1680,6 @@ static struct target_core_fabric_ops tcm_qla2xxx_ops = {
 	.queue_data_in			= tcm_qla2xxx_queue_data_in,
 	.queue_status			= tcm_qla2xxx_queue_status,
 	.queue_tm_rsp			= tcm_qla2xxx_queue_tm_rsp,
-	.get_fabric_sense_len		= tcm_qla2xxx_get_fabric_sense_len,
-	.set_fabric_sense_len		= tcm_qla2xxx_set_fabric_sense_len,
 	/*
 	 * Setup function pointers for generic logic in
 	 * target_core_fabric_configfs.c
@@ -1740,8 +1727,6 @@ static struct target_core_fabric_ops tcm_qla2xxx_npiv_ops = {
 	.queue_data_in			= tcm_qla2xxx_queue_data_in,
 	.queue_status			= tcm_qla2xxx_queue_status,
 	.queue_tm_rsp			= tcm_qla2xxx_queue_tm_rsp,
-	.get_fabric_sense_len		= tcm_qla2xxx_get_fabric_sense_len,
-	.set_fabric_sense_len		= tcm_qla2xxx_set_fabric_sense_len,
 	/*
 	 * Setup function pointers for generic logic in
 	 * target_core_fabric_configfs.c
diff --git a/drivers/scsi/scsi_debug.c b/drivers/scsi/scsi_debug.c
index 57fbd5a3d4e..5cda11c07c6 100644
--- a/drivers/scsi/scsi_debug.c
+++ b/drivers/scsi/scsi_debug.c
@@ -2055,7 +2055,7 @@ static void unmap_region(sector_t lba, unsigned int len)
 		block = lba + alignment;
 		rem = do_div(block, granularity);
 
-		if (rem == 0 && lba + granularity <= end && block < map_size) {
+		if (rem == 0 && lba + granularity < end && block < map_size) {
 			clear_bit(block, map_storep);
 			if (scsi_debug_lbprz)
 				memset(fake_storep +
diff --git a/drivers/scsi/scsi_error.c b/drivers/scsi/scsi_error.c
index de2337f255a..c1b05a83d40 100644
--- a/drivers/scsi/scsi_error.c
+++ b/drivers/scsi/scsi_error.c
@@ -789,7 +789,6 @@ static int scsi_send_eh_cmnd(struct scsi_cmnd *scmd, unsigned char *cmnd,
 			     int cmnd_size, int timeout, unsigned sense_bytes)
 {
 	struct scsi_device *sdev = scmd->device;
-	struct scsi_driver *sdrv = scsi_cmd_to_driver(scmd);
 	struct Scsi_Host *shost = sdev->host;
 	DECLARE_COMPLETION_ONSTACK(done);
 	unsigned long timeleft;
@@ -845,8 +844,11 @@ static int scsi_send_eh_cmnd(struct scsi_cmnd *scmd, unsigned char *cmnd,
 
 	scsi_eh_restore_cmnd(scmd, &ses);
 
-	if (sdrv && sdrv->eh_action)
-		rtn = sdrv->eh_action(scmd, cmnd, cmnd_size, rtn);
+	if (scmd->request->cmd_type != REQ_TYPE_BLOCK_PC) {
+		struct scsi_driver *sdrv = scsi_cmd_to_driver(scmd);
+		if (sdrv->eh_action)
+			rtn = sdrv->eh_action(scmd, cmnd, cmnd_size, rtn);
+	}
 
 	return rtn;
 }
diff --git a/drivers/scsi/sg.c b/drivers/scsi/sg.c
index 9c5c5f2b396..be2c9a6561f 100644
--- a/drivers/scsi/sg.c
+++ b/drivers/scsi/sg.c
@@ -1257,7 +1257,7 @@ sg_mmap(struct file *filp, struct vm_area_struct *vma)
 	}
 
 	sfp->mmap_called = 1;
-	vma->vm_flags |= VM_RESERVED;
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
 	vma->vm_private_data = sfp;
 	vma->vm_ops = &sg_mmap_vm_ops;
 	return 0;
diff --git a/drivers/scsi/storvsc_drv.c b/drivers/scsi/storvsc_drv.c
index 528d52beaa1..01440782feb 100644
--- a/drivers/scsi/storvsc_drv.c
+++ b/drivers/scsi/storvsc_drv.c
@@ -1221,7 +1221,12 @@ static int storvsc_host_reset_handler(struct scsi_cmnd *scmnd)
 	/*
 	 * At this point, all outstanding requests in the adapter
 	 * should have been flushed out and return to us
+	 * There is a potential race here where the host may be in
+	 * the process of responding when we return from here.
+	 * Just wait for all in-transit packets to be accounted for
+	 * before we return from here.
 	 */
+	storvsc_wait_to_drain(stor_device);
 
 	return SUCCESS;
 }
diff --git a/drivers/scsi/virtio_scsi.c b/drivers/scsi/virtio_scsi.c
index 3e79a2f0004..595af1ae442 100644
--- a/drivers/scsi/virtio_scsi.c
+++ b/drivers/scsi/virtio_scsi.c
@@ -219,7 +219,7 @@ static int virtscsi_kick_event(struct virtio_scsi *vscsi,
 	struct scatterlist sg;
 	unsigned long flags;
 
-	sg_set_buf(&sg, &event_node->event, sizeof(struct virtio_scsi_event));
+	sg_init_one(&sg, &event_node->event, sizeof(struct virtio_scsi_event));
 
 	spin_lock_irqsave(&vscsi->event_vq.vq_lock, flags);
 
@@ -279,6 +279,31 @@ static void virtscsi_handle_transport_reset(struct virtio_scsi *vscsi,
 	}
 }
 
+static void virtscsi_handle_param_change(struct virtio_scsi *vscsi,
+					 struct virtio_scsi_event *event)
+{
+	struct scsi_device *sdev;
+	struct Scsi_Host *shost = virtio_scsi_host(vscsi->vdev);
+	unsigned int target = event->lun[1];
+	unsigned int lun = (event->lun[2] << 8) | event->lun[3];
+	u8 asc = event->reason & 255;
+	u8 ascq = event->reason >> 8;
+
+	sdev = scsi_device_lookup(shost, 0, target, lun);
+	if (!sdev) {
+		pr_err("SCSI device %d 0 %d %d not found\n",
+			shost->host_no, target, lun);
+		return;
+	}
+
+	/* Handle "Parameters changed", "Mode parameters changed", and
+	   "Capacity data has changed".  */
+	if (asc == 0x2a && (ascq == 0x00 || ascq == 0x01 || ascq == 0x09))
+		scsi_rescan_device(&sdev->sdev_gendev);
+
+	scsi_device_put(sdev);
+}
+
 static void virtscsi_handle_event(struct work_struct *work)
 {
 	struct virtio_scsi_event_node *event_node =
@@ -297,6 +322,9 @@ static void virtscsi_handle_event(struct work_struct *work)
 	case VIRTIO_SCSI_T_TRANSPORT_RESET:
 		virtscsi_handle_transport_reset(vscsi, event);
 		break;
+	case VIRTIO_SCSI_T_PARAM_CHANGE:
+		virtscsi_handle_param_change(vscsi, event);
+		break;
 	default:
 		pr_err("Unsupport virtio scsi event %x\n", event->event);
 	}
@@ -677,7 +705,11 @@ static int __devinit virtscsi_probe(struct virtio_device *vdev)
 	cmd_per_lun = virtscsi_config_get(vdev, cmd_per_lun) ?: 1;
 	shost->cmd_per_lun = min_t(u32, cmd_per_lun, shost->can_queue);
 	shost->max_sectors = virtscsi_config_get(vdev, max_sectors) ?: 0xFFFF;
-	shost->max_lun = virtscsi_config_get(vdev, max_lun) + 1;
+
+	/* LUNs > 256 are reported with format 1, so they go in the range
+	 * 16640-32767.
+	 */
+	shost->max_lun = virtscsi_config_get(vdev, max_lun) + 1 + 0x4000;
 	shost->max_id = num_targets;
 	shost->max_channel = 0;
 	shost->max_cmd_len = VIRTIO_SCSI_CDB_SIZE;
@@ -733,7 +765,8 @@ static struct virtio_device_id id_table[] = {
 };
 
 static unsigned int features[] = {
-	VIRTIO_SCSI_F_HOTPLUG
+	VIRTIO_SCSI_F_HOTPLUG,
+	VIRTIO_SCSI_F_CHANGE,
 };
 
 static struct virtio_driver virtio_scsi_driver = {
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index ecc31a1f73f..1acae359cab 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -134,6 +134,7 @@ config SPI_DAVINCI
 	tristate "Texas Instruments DaVinci/DA8x/OMAP-L/AM1x SoC SPI controller"
 	depends on ARCH_DAVINCI
 	select SPI_BITBANG
+	select TI_EDMA
 	help
 	  SPI master controller for DaVinci/DA8x/OMAP-L/AM1x SPI modules.
 
@@ -237,6 +238,13 @@ config SPI_OC_TINY
 	help
 	  This is the driver for OpenCores tiny SPI master controller.
 
+config SPI_OCTEON
+	tristate "Cavium OCTEON SPI controller"
+	depends on CPU_CAVIUM_OCTEON
+	help
+	  SPI host driver for the hardware found on some Cavium OCTEON
+	  SOCs.
+
 config SPI_OMAP_UWIRE
 	tristate "OMAP1 MicroWire"
 	depends on ARCH_OMAP1
diff --git a/drivers/spi/Makefile b/drivers/spi/Makefile
index 22fd3a7251b..c48df47e4b0 100644
--- a/drivers/spi/Makefile
+++ b/drivers/spi/Makefile
@@ -39,6 +39,7 @@ obj-$(CONFIG_SPI_MPC52xx)		+= spi-mpc52xx.o
 obj-$(CONFIG_SPI_MXS)			+= spi-mxs.o
 obj-$(CONFIG_SPI_NUC900)		+= spi-nuc900.o
 obj-$(CONFIG_SPI_OC_TINY)		+= spi-oc-tiny.o
+obj-$(CONFIG_SPI_OCTEON)		+= spi-octeon.o
 obj-$(CONFIG_SPI_OMAP_UWIRE)		+= spi-omap-uwire.o
 obj-$(CONFIG_SPI_OMAP_100K)		+= spi-omap-100k.o
 obj-$(CONFIG_SPI_OMAP24XX)		+= spi-omap2-mcspi.o
diff --git a/drivers/spi/spi-davinci.c b/drivers/spi/spi-davinci.c
index 3afe2f4f5b8..147dfa87a64 100644
--- a/drivers/spi/spi-davinci.c
+++ b/drivers/spi/spi-davinci.c
@@ -25,13 +25,14 @@
 #include <linux/platform_device.h>
 #include <linux/err.h>
 #include <linux/clk.h>
+#include <linux/dmaengine.h>
 #include <linux/dma-mapping.h>
+#include <linux/edma.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/spi_bitbang.h>
 #include <linux/slab.h>
 
 #include <linux/platform_data/spi-davinci.h>
-#include <mach/edma.h>
 
 #define SPI_NO_RESOURCE		((resource_size_t)-1)
 
@@ -113,14 +114,6 @@
 #define SPIDEF		0x4c
 #define SPIFMT0		0x50
 
-/* We have 2 DMA channels per CS, one for RX and one for TX */
-struct davinci_spi_dma {
-	int			tx_channel;
-	int			rx_channel;
-	int			dummy_param_slot;
-	enum dma_event_q	eventq;
-};
-
 /* SPI Controller driver's private data. */
 struct davinci_spi {
 	struct spi_bitbang	bitbang;
@@ -134,11 +127,14 @@ struct davinci_spi {
 
 	const void		*tx;
 	void			*rx;
-#define SPI_TMP_BUFSZ	(SMP_CACHE_BYTES + 1)
-	u8			rx_tmp_buf[SPI_TMP_BUFSZ];
 	int			rcount;
 	int			wcount;
-	struct davinci_spi_dma	dma;
+
+	struct dma_chan		*dma_rx;
+	struct dma_chan		*dma_tx;
+	int			dma_rx_chnum;
+	int			dma_tx_chnum;
+
 	struct davinci_spi_platform_data *pdata;
 
 	void			(*get_rx)(u32 rx_data, struct davinci_spi *);
@@ -496,21 +492,23 @@ out:
 	return errors;
 }
 
-static void davinci_spi_dma_callback(unsigned lch, u16 status, void *data)
+static void davinci_spi_dma_rx_callback(void *data)
 {
-	struct davinci_spi *dspi = data;
-	struct davinci_spi_dma *dma = &dspi->dma;
+	struct davinci_spi *dspi = (struct davinci_spi *)data;
 
-	edma_stop(lch);
+	dspi->rcount = 0;
 
-	if (status == DMA_COMPLETE) {
-		if (lch == dma->rx_channel)
-			dspi->rcount = 0;
-		if (lch == dma->tx_channel)
-			dspi->wcount = 0;
-	}
+	if (!dspi->wcount && !dspi->rcount)
+		complete(&dspi->done);
+}
 
-	if ((!dspi->wcount && !dspi->rcount) || (status != DMA_COMPLETE))
+static void davinci_spi_dma_tx_callback(void *data)
+{
+	struct davinci_spi *dspi = (struct davinci_spi *)data;
+
+	dspi->wcount = 0;
+
+	if (!dspi->wcount && !dspi->rcount)
 		complete(&dspi->done);
 }
 
@@ -526,20 +524,20 @@ static void davinci_spi_dma_callback(unsigned lch, u16 status, void *data)
 static int davinci_spi_bufs(struct spi_device *spi, struct spi_transfer *t)
 {
 	struct davinci_spi *dspi;
-	int data_type, ret;
+	int data_type, ret = -ENOMEM;
 	u32 tx_data, spidat1;
 	u32 errors = 0;
 	struct davinci_spi_config *spicfg;
 	struct davinci_spi_platform_data *pdata;
 	unsigned uninitialized_var(rx_buf_count);
-	struct device *sdev;
+	void *dummy_buf = NULL;
+	struct scatterlist sg_rx, sg_tx;
 
 	dspi = spi_master_get_devdata(spi->master);
 	pdata = dspi->pdata;
 	spicfg = (struct davinci_spi_config *)spi->controller_data;
 	if (!spicfg)
 		spicfg = &davinci_spi_default_cfg;
-	sdev = dspi->bitbang.master->dev.parent;
 
 	/* convert len to words based on bits_per_word */
 	data_type = dspi->bytes_per_word[spi->chip_select];
@@ -567,112 +565,83 @@ static int davinci_spi_bufs(struct spi_device *spi, struct spi_transfer *t)
 		spidat1 |= tx_data & 0xFFFF;
 		iowrite32(spidat1, dspi->base + SPIDAT1);
 	} else {
-		struct davinci_spi_dma *dma;
-		unsigned long tx_reg, rx_reg;
-		struct edmacc_param param;
-		void *rx_buf;
-		int b, c;
-
-		dma = &dspi->dma;
-
-		tx_reg = (unsigned long)dspi->pbase + SPIDAT1;
-		rx_reg = (unsigned long)dspi->pbase + SPIBUF;
-
-		/*
-		 * Transmit DMA setup
-		 *
-		 * If there is transmit data, map the transmit buffer, set it
-		 * as the source of data and set the source B index to data
-		 * size. If there is no transmit data, set the transmit register
-		 * as the source of data, and set the source B index to zero.
-		 *
-		 * The destination is always the transmit register itself. And
-		 * the destination never increments.
-		 */
-
-		if (t->tx_buf) {
-			t->tx_dma = dma_map_single(&spi->dev, (void *)t->tx_buf,
-						t->len, DMA_TO_DEVICE);
-			if (dma_mapping_error(&spi->dev, t->tx_dma)) {
-				dev_dbg(sdev, "Unable to DMA map %d bytes"
-						"TX buffer\n", t->len);
-				return -ENOMEM;
-			}
-		}
-
-		/*
-		 * If number of words is greater than 65535, then we need
-		 * to configure a 3 dimension transfer.  Use the BCNTRLD
-		 * feature to allow for transfers that aren't even multiples
-		 * of 65535 (or any other possible b size) by first transferring
-		 * the remainder amount then grabbing the next N blocks of
-		 * 65535 words.
-		 */
-
-		c = dspi->wcount / (SZ_64K - 1);	/* N 65535 Blocks */
-		b = dspi->wcount - c * (SZ_64K - 1);	/* Remainder */
-		if (b)
-			c++;
+		struct dma_slave_config dma_rx_conf = {
+			.direction = DMA_DEV_TO_MEM,
+			.src_addr = (unsigned long)dspi->pbase + SPIBUF,
+			.src_addr_width = data_type,
+			.src_maxburst = 1,
+		};
+		struct dma_slave_config dma_tx_conf = {
+			.direction = DMA_MEM_TO_DEV,
+			.dst_addr = (unsigned long)dspi->pbase + SPIDAT1,
+			.dst_addr_width = data_type,
+			.dst_maxburst = 1,
+		};
+		struct dma_async_tx_descriptor *rxdesc;
+		struct dma_async_tx_descriptor *txdesc;
+		void *buf;
+
+		dummy_buf = kzalloc(t->len, GFP_KERNEL);
+		if (!dummy_buf)
+			goto err_alloc_dummy_buf;
+
+		dmaengine_slave_config(dspi->dma_rx, &dma_rx_conf);
+		dmaengine_slave_config(dspi->dma_tx, &dma_tx_conf);
+
+		sg_init_table(&sg_rx, 1);
+		if (!t->rx_buf)
+			buf = dummy_buf;
 		else
-			b = SZ_64K - 1;
-
-		param.opt = TCINTEN | EDMA_TCC(dma->tx_channel);
-		param.src = t->tx_buf ? t->tx_dma : tx_reg;
-		param.a_b_cnt = b << 16 | data_type;
-		param.dst = tx_reg;
-		param.src_dst_bidx = t->tx_buf ? data_type : 0;
-		param.link_bcntrld = 0xffffffff;
-		param.src_dst_cidx = t->tx_buf ? data_type : 0;
-		param.ccnt = c;
-		edma_write_slot(dma->tx_channel, &param);
-		edma_link(dma->tx_channel, dma->dummy_param_slot);
-
-		/*
-		 * Receive DMA setup
-		 *
-		 * If there is receive buffer, use it to receive data. If there
-		 * is none provided, use a temporary receive buffer. Set the
-		 * destination B index to 0 so effectively only one byte is used
-		 * in the temporary buffer (address does not increment).
-		 *
-		 * The source of receive data is the receive data register. The
-		 * source address never increments.
-		 */
-
-		if (t->rx_buf) {
-			rx_buf = t->rx_buf;
-			rx_buf_count = t->len;
-		} else {
-			rx_buf = dspi->rx_tmp_buf;
-			rx_buf_count = sizeof(dspi->rx_tmp_buf);
+			buf = t->rx_buf;
+		t->rx_dma = dma_map_single(&spi->dev, buf,
+				t->len, DMA_FROM_DEVICE);
+		if (!t->rx_dma) {
+			ret = -EFAULT;
+			goto err_rx_map;
 		}
+		sg_dma_address(&sg_rx) = t->rx_dma;
+		sg_dma_len(&sg_rx) = t->len;
 
-		t->rx_dma = dma_map_single(&spi->dev, rx_buf, rx_buf_count,
-							DMA_FROM_DEVICE);
-		if (dma_mapping_error(&spi->dev, t->rx_dma)) {
-			dev_dbg(sdev, "Couldn't DMA map a %d bytes RX buffer\n",
-								rx_buf_count);
-			if (t->tx_buf)
-				dma_unmap_single(&spi->dev, t->tx_dma, t->len,
-								DMA_TO_DEVICE);
-			return -ENOMEM;
+		sg_init_table(&sg_tx, 1);
+		if (!t->tx_buf)
+			buf = dummy_buf;
+		else
+			buf = (void *)t->tx_buf;
+		t->tx_dma = dma_map_single(&spi->dev, buf,
+				t->len, DMA_FROM_DEVICE);
+		if (!t->tx_dma) {
+			ret = -EFAULT;
+			goto err_tx_map;
 		}
-
-		param.opt = TCINTEN | EDMA_TCC(dma->rx_channel);
-		param.src = rx_reg;
-		param.a_b_cnt = b << 16 | data_type;
-		param.dst = t->rx_dma;
-		param.src_dst_bidx = (t->rx_buf ? data_type : 0) << 16;
-		param.link_bcntrld = 0xffffffff;
-		param.src_dst_cidx = (t->rx_buf ? data_type : 0) << 16;
-		param.ccnt = c;
-		edma_write_slot(dma->rx_channel, &param);
+		sg_dma_address(&sg_tx) = t->tx_dma;
+		sg_dma_len(&sg_tx) = t->len;
+
+		rxdesc = dmaengine_prep_slave_sg(dspi->dma_rx,
+				&sg_rx, 1, DMA_DEV_TO_MEM,
+				DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+		if (!rxdesc)
+			goto err_desc;
+
+		txdesc = dmaengine_prep_slave_sg(dspi->dma_tx,
+				&sg_tx, 1, DMA_MEM_TO_DEV,
+				DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+		if (!txdesc)
+			goto err_desc;
+
+		rxdesc->callback = davinci_spi_dma_rx_callback;
+		rxdesc->callback_param = (void *)dspi;
+		txdesc->callback = davinci_spi_dma_tx_callback;
+		txdesc->callback_param = (void *)dspi;
 
 		if (pdata->cshold_bug)
 			iowrite16(spidat1 >> 16, dspi->base + SPIDAT1 + 2);
 
-		edma_start(dma->rx_channel);
-		edma_start(dma->tx_channel);
+		dmaengine_submit(rxdesc);
+		dmaengine_submit(txdesc);
+
+		dma_async_issue_pending(dspi->dma_rx);
+		dma_async_issue_pending(dspi->dma_tx);
+
 		set_io_bits(dspi->base + SPIINT, SPIINT_DMA_REQ_EN);
 	}
 
@@ -690,15 +659,13 @@ static int davinci_spi_bufs(struct spi_device *spi, struct spi_transfer *t)
 
 	clear_io_bits(dspi->base + SPIINT, SPIINT_MASKALL);
 	if (spicfg->io_type == SPI_IO_TYPE_DMA) {
-
-		if (t->tx_buf)
-			dma_unmap_single(&spi->dev, t->tx_dma, t->len,
-								DMA_TO_DEVICE);
-
-		dma_unmap_single(&spi->dev, t->rx_dma, rx_buf_count,
-							DMA_FROM_DEVICE);
-
 		clear_io_bits(dspi->base + SPIINT, SPIINT_DMA_REQ_EN);
+
+		dma_unmap_single(&spi->dev, t->rx_dma,
+				t->len, DMA_FROM_DEVICE);
+		dma_unmap_single(&spi->dev, t->tx_dma,
+				t->len, DMA_TO_DEVICE);
+		kfree(dummy_buf);
 	}
 
 	clear_io_bits(dspi->base + SPIGCR1, SPIGCR1_SPIENA_MASK);
@@ -716,11 +683,20 @@ static int davinci_spi_bufs(struct spi_device *spi, struct spi_transfer *t)
 	}
 
 	if (dspi->rcount != 0 || dspi->wcount != 0) {
-		dev_err(sdev, "SPI data transfer error\n");
+		dev_err(&spi->dev, "SPI data transfer error\n");
 		return -EIO;
 	}
 
 	return t->len;
+
+err_desc:
+	dma_unmap_single(&spi->dev, t->tx_dma, t->len, DMA_TO_DEVICE);
+err_tx_map:
+	dma_unmap_single(&spi->dev, t->rx_dma, t->len, DMA_FROM_DEVICE);
+err_rx_map:
+	kfree(dummy_buf);
+err_alloc_dummy_buf:
+	return ret;
 }
 
 /**
@@ -751,39 +727,33 @@ static irqreturn_t davinci_spi_irq(s32 irq, void *data)
 
 static int davinci_spi_request_dma(struct davinci_spi *dspi)
 {
+	dma_cap_mask_t mask;
+	struct device *sdev = dspi->bitbang.master->dev.parent;
 	int r;
-	struct davinci_spi_dma *dma = &dspi->dma;
 
-	r = edma_alloc_channel(dma->rx_channel, davinci_spi_dma_callback, dspi,
-								dma->eventq);
-	if (r < 0) {
-		pr_err("Unable to request DMA channel for SPI RX\n");
-		r = -EAGAIN;
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+
+	dspi->dma_rx = dma_request_channel(mask, edma_filter_fn,
+					   &dspi->dma_rx_chnum);
+	if (!dspi->dma_rx) {
+		dev_err(sdev, "request RX DMA channel failed\n");
+		r = -ENODEV;
 		goto rx_dma_failed;
 	}
 
-	r = edma_alloc_channel(dma->tx_channel, davinci_spi_dma_callback, dspi,
-								dma->eventq);
-	if (r < 0) {
-		pr_err("Unable to request DMA channel for SPI TX\n");
-		r = -EAGAIN;
+	dspi->dma_tx = dma_request_channel(mask, edma_filter_fn,
+					   &dspi->dma_tx_chnum);
+	if (!dspi->dma_tx) {
+		dev_err(sdev, "request TX DMA channel failed\n");
+		r = -ENODEV;
 		goto tx_dma_failed;
 	}
 
-	r = edma_alloc_slot(EDMA_CTLR(dma->tx_channel), EDMA_SLOT_ANY);
-	if (r < 0) {
-		pr_err("Unable to request SPI TX DMA param slot\n");
-		r = -EAGAIN;
-		goto param_failed;
-	}
-	dma->dummy_param_slot = r;
-	edma_link(dma->dummy_param_slot, dma->dummy_param_slot);
-
 	return 0;
-param_failed:
-	edma_free_channel(dma->tx_channel);
+
 tx_dma_failed:
-	edma_free_channel(dma->rx_channel);
+	dma_release_channel(dspi->dma_rx);
 rx_dma_failed:
 	return r;
 }
@@ -898,9 +868,8 @@ static int __devinit davinci_spi_probe(struct platform_device *pdev)
 	dspi->bitbang.txrx_bufs = davinci_spi_bufs;
 	if (dma_rx_chan != SPI_NO_RESOURCE &&
 	    dma_tx_chan != SPI_NO_RESOURCE) {
-		dspi->dma.rx_channel = dma_rx_chan;
-		dspi->dma.tx_channel = dma_tx_chan;
-		dspi->dma.eventq = pdata->dma_event_q;
+		dspi->dma_rx_chnum = dma_rx_chan;
+		dspi->dma_tx_chnum = dma_tx_chan;
 
 		ret = davinci_spi_request_dma(dspi);
 		if (ret)
@@ -955,9 +924,8 @@ static int __devinit davinci_spi_probe(struct platform_device *pdev)
 	return ret;
 
 free_dma:
-	edma_free_channel(dspi->dma.tx_channel);
-	edma_free_channel(dspi->dma.rx_channel);
-	edma_free_slot(dspi->dma.dummy_param_slot);
+	dma_release_channel(dspi->dma_rx);
+	dma_release_channel(dspi->dma_tx);
 free_clk:
 	clk_disable(dspi->clk);
 	clk_put(dspi->clk);
diff --git a/drivers/spi/spi-octeon.c b/drivers/spi/spi-octeon.c
new file mode 100644
index 00000000000..ea8fb2efb0f
--- /dev/null
+++ b/drivers/spi/spi-octeon.c
@@ -0,0 +1,362 @@
+/*
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ *
+ * Copyright (C) 2011, 2012 Cavium, Inc.
+ */
+
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/spi/spi.h>
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/of.h>
+
+#include <asm/octeon/octeon.h>
+#include <asm/octeon/cvmx-mpi-defs.h>
+
+#define OCTEON_SPI_CFG 0
+#define OCTEON_SPI_STS 0x08
+#define OCTEON_SPI_TX 0x10
+#define OCTEON_SPI_DAT0 0x80
+
+#define OCTEON_SPI_MAX_BYTES 9
+
+#define OCTEON_SPI_MAX_CLOCK_HZ 16000000
+
+struct octeon_spi {
+	struct spi_master *my_master;
+	u64 register_base;
+	u64 last_cfg;
+	u64 cs_enax;
+};
+
+struct octeon_spi_setup {
+	u32 max_speed_hz;
+	u8 chip_select;
+	u8 mode;
+	u8 bits_per_word;
+};
+
+static void octeon_spi_wait_ready(struct octeon_spi *p)
+{
+	union cvmx_mpi_sts mpi_sts;
+	unsigned int loops = 0;
+
+	do {
+		if (loops++)
+			__delay(500);
+		mpi_sts.u64 = cvmx_read_csr(p->register_base + OCTEON_SPI_STS);
+	} while (mpi_sts.s.busy);
+}
+
+static int octeon_spi_do_transfer(struct octeon_spi *p,
+				  struct spi_message *msg,
+				  struct spi_transfer *xfer,
+				  bool last_xfer)
+{
+	union cvmx_mpi_cfg mpi_cfg;
+	union cvmx_mpi_tx mpi_tx;
+	unsigned int clkdiv;
+	unsigned int speed_hz;
+	int mode;
+	bool cpha, cpol;
+	int bits_per_word;
+	const u8 *tx_buf;
+	u8 *rx_buf;
+	int len;
+	int i;
+
+	struct octeon_spi_setup *msg_setup = spi_get_ctldata(msg->spi);
+
+	speed_hz = msg_setup->max_speed_hz;
+	mode = msg_setup->mode;
+	cpha = mode & SPI_CPHA;
+	cpol = mode & SPI_CPOL;
+	bits_per_word = msg_setup->bits_per_word;
+
+	if (xfer->speed_hz)
+		speed_hz = xfer->speed_hz;
+	if (xfer->bits_per_word)
+		bits_per_word = xfer->bits_per_word;
+
+	if (speed_hz > OCTEON_SPI_MAX_CLOCK_HZ)
+		speed_hz = OCTEON_SPI_MAX_CLOCK_HZ;
+
+	clkdiv = octeon_get_io_clock_rate() / (2 * speed_hz);
+
+	mpi_cfg.u64 = 0;
+
+	mpi_cfg.s.clkdiv = clkdiv;
+	mpi_cfg.s.cshi = (mode & SPI_CS_HIGH) ? 1 : 0;
+	mpi_cfg.s.lsbfirst = (mode & SPI_LSB_FIRST) ? 1 : 0;
+	mpi_cfg.s.wireor = (mode & SPI_3WIRE) ? 1 : 0;
+	mpi_cfg.s.idlelo = cpha != cpol;
+	mpi_cfg.s.cslate = cpha ? 1 : 0;
+	mpi_cfg.s.enable = 1;
+
+	if (msg_setup->chip_select < 4)
+		p->cs_enax |= 1ull << (12 + msg_setup->chip_select);
+	mpi_cfg.u64 |= p->cs_enax;
+
+	if (mpi_cfg.u64 != p->last_cfg) {
+		p->last_cfg = mpi_cfg.u64;
+		cvmx_write_csr(p->register_base + OCTEON_SPI_CFG, mpi_cfg.u64);
+	}
+	tx_buf = xfer->tx_buf;
+	rx_buf = xfer->rx_buf;
+	len = xfer->len;
+	while (len > OCTEON_SPI_MAX_BYTES) {
+		for (i = 0; i < OCTEON_SPI_MAX_BYTES; i++) {
+			u8 d;
+			if (tx_buf)
+				d = *tx_buf++;
+			else
+				d = 0;
+			cvmx_write_csr(p->register_base + OCTEON_SPI_DAT0 + (8 * i), d);
+		}
+		mpi_tx.u64 = 0;
+		mpi_tx.s.csid = msg_setup->chip_select;
+		mpi_tx.s.leavecs = 1;
+		mpi_tx.s.txnum = tx_buf ? OCTEON_SPI_MAX_BYTES : 0;
+		mpi_tx.s.totnum = OCTEON_SPI_MAX_BYTES;
+		cvmx_write_csr(p->register_base + OCTEON_SPI_TX, mpi_tx.u64);
+
+		octeon_spi_wait_ready(p);
+		if (rx_buf)
+			for (i = 0; i < OCTEON_SPI_MAX_BYTES; i++) {
+				u64 v = cvmx_read_csr(p->register_base + OCTEON_SPI_DAT0 + (8 * i));
+				*rx_buf++ = (u8)v;
+			}
+		len -= OCTEON_SPI_MAX_BYTES;
+	}
+
+	for (i = 0; i < len; i++) {
+		u8 d;
+		if (tx_buf)
+			d = *tx_buf++;
+		else
+			d = 0;
+		cvmx_write_csr(p->register_base + OCTEON_SPI_DAT0 + (8 * i), d);
+	}
+
+	mpi_tx.u64 = 0;
+	mpi_tx.s.csid = msg_setup->chip_select;
+	if (last_xfer)
+		mpi_tx.s.leavecs = xfer->cs_change;
+	else
+		mpi_tx.s.leavecs = !xfer->cs_change;
+	mpi_tx.s.txnum = tx_buf ? len : 0;
+	mpi_tx.s.totnum = len;
+	cvmx_write_csr(p->register_base + OCTEON_SPI_TX, mpi_tx.u64);
+
+	octeon_spi_wait_ready(p);
+	if (rx_buf)
+		for (i = 0; i < len; i++) {
+			u64 v = cvmx_read_csr(p->register_base + OCTEON_SPI_DAT0 + (8 * i));
+			*rx_buf++ = (u8)v;
+		}
+
+	if (xfer->delay_usecs)
+		udelay(xfer->delay_usecs);
+
+	return xfer->len;
+}
+
+static int octeon_spi_validate_bpw(struct spi_device *spi, u32 speed)
+{
+	switch (speed) {
+	case 8:
+		break;
+	default:
+		dev_err(&spi->dev, "Error: %d bits per word not supported\n",
+			speed);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int octeon_spi_transfer_one_message(struct spi_master *master,
+					   struct spi_message *msg)
+{
+	struct octeon_spi *p = spi_master_get_devdata(master);
+	unsigned int total_len = 0;
+	int status = 0;
+	struct spi_transfer *xfer;
+
+	/*
+	 * We better have set the configuration via a call to .setup
+	 * before we get here.
+	 */
+	if (spi_get_ctldata(msg->spi) == NULL) {
+		status = -EINVAL;
+		goto err;
+	}
+
+	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
+		if (xfer->bits_per_word) {
+			status = octeon_spi_validate_bpw(msg->spi,
+							 xfer->bits_per_word);
+			if (status)
+				goto err;
+		}
+	}
+
+	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
+		bool last_xfer = &xfer->transfer_list == msg->transfers.prev;
+		int r = octeon_spi_do_transfer(p, msg, xfer, last_xfer);
+		if (r < 0) {
+			status = r;
+			goto err;
+		}
+		total_len += r;
+	}
+err:
+	msg->status = status;
+	msg->actual_length = total_len;
+	spi_finalize_current_message(master);
+	return status;
+}
+
+static struct octeon_spi_setup *octeon_spi_new_setup(struct spi_device *spi)
+{
+	struct octeon_spi_setup *setup = kzalloc(sizeof(*setup), GFP_KERNEL);
+	if (!setup)
+		return NULL;
+
+	setup->max_speed_hz = spi->max_speed_hz;
+	setup->chip_select = spi->chip_select;
+	setup->mode = spi->mode;
+	setup->bits_per_word = spi->bits_per_word;
+	return setup;
+}
+
+static int octeon_spi_setup(struct spi_device *spi)
+{
+	int r;
+	struct octeon_spi_setup *new_setup;
+	struct octeon_spi_setup *old_setup = spi_get_ctldata(spi);
+
+	r = octeon_spi_validate_bpw(spi, spi->bits_per_word);
+	if (r)
+		return r;
+
+	new_setup = octeon_spi_new_setup(spi);
+	if (!new_setup)
+		return -ENOMEM;
+
+	spi_set_ctldata(spi, new_setup);
+	kfree(old_setup);
+
+	return 0;
+}
+
+static void octeon_spi_cleanup(struct spi_device *spi)
+{
+	struct octeon_spi_setup *old_setup = spi_get_ctldata(spi);
+	spi_set_ctldata(spi, NULL);
+	kfree(old_setup);
+}
+
+static int octeon_spi_nop_transfer_hardware(struct spi_master *master)
+{
+	return 0;
+}
+
+static int __devinit octeon_spi_probe(struct platform_device *pdev)
+{
+
+	struct resource *res_mem;
+	struct spi_master *master;
+	struct octeon_spi *p;
+	int err = -ENOENT;
+
+	master = spi_alloc_master(&pdev->dev, sizeof(struct octeon_spi));
+	if (!master)
+		return -ENOMEM;
+	p = spi_master_get_devdata(master);
+	platform_set_drvdata(pdev, p);
+	p->my_master = master;
+
+	res_mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	if (res_mem == NULL) {
+		dev_err(&pdev->dev, "found no memory resource\n");
+		err = -ENXIO;
+		goto fail;
+	}
+	if (!devm_request_mem_region(&pdev->dev, res_mem->start,
+				     resource_size(res_mem), res_mem->name)) {
+		dev_err(&pdev->dev, "request_mem_region failed\n");
+		goto fail;
+	}
+	p->register_base = (u64)devm_ioremap(&pdev->dev, res_mem->start,
+					     resource_size(res_mem));
+
+	/* Dynamic bus numbering */
+	master->bus_num = -1;
+	master->num_chipselect = 4;
+	master->mode_bits = SPI_CPHA |
+			    SPI_CPOL |
+			    SPI_CS_HIGH |
+			    SPI_LSB_FIRST |
+			    SPI_3WIRE;
+
+	master->setup = octeon_spi_setup;
+	master->cleanup = octeon_spi_cleanup;
+	master->prepare_transfer_hardware = octeon_spi_nop_transfer_hardware;
+	master->transfer_one_message = octeon_spi_transfer_one_message;
+	master->unprepare_transfer_hardware = octeon_spi_nop_transfer_hardware;
+
+	master->dev.of_node = pdev->dev.of_node;
+	err = spi_register_master(master);
+	if (err) {
+		dev_err(&pdev->dev, "register master failed: %d\n", err);
+		goto fail;
+	}
+
+	dev_info(&pdev->dev, "OCTEON SPI bus driver\n");
+
+	return 0;
+fail:
+	spi_master_put(master);
+	return err;
+}
+
+static int __devexit octeon_spi_remove(struct platform_device *pdev)
+{
+	struct octeon_spi *p = platform_get_drvdata(pdev);
+	u64 register_base = p->register_base;
+
+	spi_unregister_master(p->my_master);
+
+	/* Clear the CSENA* and put everything in a known state. */
+	cvmx_write_csr(register_base + OCTEON_SPI_CFG, 0);
+
+	return 0;
+}
+
+static struct of_device_id octeon_spi_match[] = {
+	{ .compatible = "cavium,octeon-3010-spi", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, octeon_spi_match);
+
+static struct platform_driver octeon_spi_driver = {
+	.driver = {
+		.name		= "spi-octeon",
+		.owner		= THIS_MODULE,
+		.of_match_table = octeon_spi_match,
+	},
+	.probe		= octeon_spi_probe,
+	.remove		= __devexit_p(octeon_spi_remove),
+};
+
+module_platform_driver(octeon_spi_driver);
+
+MODULE_DESCRIPTION("Cavium, Inc. OCTEON SPI bus driver");
+MODULE_AUTHOR("David Daney");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/android/ashmem.c b/drivers/staging/android/ashmem.c
index 94a740d2883..634b9ae713e 100644
--- a/drivers/staging/android/ashmem.c
+++ b/drivers/staging/android/ashmem.c
@@ -332,7 +332,6 @@ static int ashmem_mmap(struct file *file, struct vm_area_struct *vma)
 	if (vma->vm_file)
 		fput(vma->vm_file);
 	vma->vm_file = asma->file;
-	vma->vm_flags |= VM_CAN_NONLINEAR;
 
 out:
 	mutex_unlock(&ashmem_mutex);
diff --git a/drivers/staging/omap-thermal/omap-thermal-common.c b/drivers/staging/omap-thermal/omap-thermal-common.c
index 46ee0a9f49d..5c0c203b887 100644
--- a/drivers/staging/omap-thermal/omap-thermal-common.c
+++ b/drivers/staging/omap-thermal/omap-thermal-common.c
@@ -126,7 +126,9 @@ static int omap_thermal_bind(struct thermal_zone_device *thermal,
 
 	/* TODO: bind with min and max states */
 	/* Simple thing, two trips, one passive another critical */
-	return thermal_zone_bind_cooling_device(thermal, 0, cdev);
+	return thermal_zone_bind_cooling_device(thermal, 0, cdev,
+						THERMAL_NO_LIMIT,
+						THERMAL_NO_LIMIT);
 }
 
 /* Unbind callback functions for thermal zone */
@@ -268,7 +270,6 @@ int omap_thermal_expose_sensor(struct omap_bandgap *bg_ptr, int id,
 	/* Create thermal zone */
 	data->omap_thermal = thermal_zone_device_register(domain,
 				OMAP_TRIP_NUMBER, 0, data, &omap_thermal_ops,
-				1, 2, /*TODO: remove this when FW allows */
 				FAST_TEMP_MONITORING_RATE,
 				FAST_TEMP_MONITORING_RATE);
 	if (IS_ERR_OR_NULL(data->omap_thermal)) {
diff --git a/drivers/staging/omapdrm/omap_drv.c b/drivers/staging/omapdrm/omap_drv.c
index 2ec5264dd00..ebdb0b67673 100644
--- a/drivers/staging/omapdrm/omap_drv.c
+++ b/drivers/staging/omapdrm/omap_drv.c
@@ -106,7 +106,8 @@ static void dump_video_chains(void)
 	for (i = 0; i < omap_dss_get_num_overlays(); i++) {
 		struct omap_overlay *ovl = omap_dss_get_overlay(i);
 		struct omap_overlay_manager *mgr = ovl->manager;
-		struct omap_dss_device *dssdev = mgr ? mgr->device : NULL;
+		struct omap_dss_device *dssdev = mgr ?
+					mgr->get_device(mgr) : NULL;
 		if (dssdev) {
 			DBG("%d: %s -> %s -> %s", i, ovl->name, mgr->name,
 						dssdev->name);
@@ -185,7 +186,7 @@ static int create_connector(struct drm_device *dev,
 	for (j = 0; j < priv->num_encoders; j++) {
 		struct omap_overlay_manager *mgr =
 			omap_encoder_get_manager(priv->encoders[j]);
-		if (mgr->device == dssdev) {
+		if (mgr->get_device(mgr) == dssdev) {
 			drm_mode_connector_attach_encoder(connector,
 					priv->encoders[j]);
 		}
diff --git a/drivers/staging/omapdrm/omap_gem_dmabuf.c b/drivers/staging/omapdrm/omap_gem_dmabuf.c
index 42728e0cc19..c6f3ef6f57b 100644
--- a/drivers/staging/omapdrm/omap_gem_dmabuf.c
+++ b/drivers/staging/omapdrm/omap_gem_dmabuf.c
@@ -160,7 +160,7 @@ static int omap_gem_dmabuf_mmap(struct dma_buf *buffer,
 		goto out_unlock;
 	}
 
-	vma->vm_flags |= VM_RESERVED | VM_IO | VM_PFNMAP | VM_DONTEXPAND;
+	vma->vm_flags |= VM_IO | VM_PFNMAP | VM_DONTEXPAND | VM_DONTDUMP;
 	vma->vm_ops = obj->dev->driver->gem_vm_ops;
 	vma->vm_private_data = obj;
 	vma->vm_page_prot =  pgprot_writecombine(vm_get_page_prot(vma->vm_flags));
diff --git a/drivers/staging/tidspbridge/rmgr/drv_interface.c b/drivers/staging/tidspbridge/rmgr/drv_interface.c
index bddea1d3b2c..701a11ac676 100644
--- a/drivers/staging/tidspbridge/rmgr/drv_interface.c
+++ b/drivers/staging/tidspbridge/rmgr/drv_interface.c
@@ -261,7 +261,7 @@ static int bridge_mmap(struct file *filp, struct vm_area_struct *vma)
 {
 	u32 status;
 
-	vma->vm_flags |= VM_RESERVED | VM_IO;
+	/* VM_IO | VM_DONTEXPAND | VM_DONTDUMP are set by remap_pfn_range() */
 	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
 
 	dev_dbg(bridge, "%s: vm filp %p start %lx end %lx page_prot %ulx "
diff --git a/drivers/target/iscsi/iscsi_target.c b/drivers/target/iscsi/iscsi_target.c
index 97c0f78c3c9..d6ce2182e67 100644
--- a/drivers/target/iscsi/iscsi_target.c
+++ b/drivers/target/iscsi/iscsi_target.c
@@ -427,7 +427,7 @@ int iscsit_reset_np_thread(
 	return 0;
 }
 
-int iscsit_del_np_comm(struct iscsi_np *np)
+static int iscsit_del_np_comm(struct iscsi_np *np)
 {
 	if (np->np_socket)
 		sock_release(np->np_socket);
@@ -785,10 +785,6 @@ static int iscsit_handle_scsi_cmd(
 
 	hdr			= (struct iscsi_scsi_req *) buf;
 	payload_length		= ntoh24(hdr->dlength);
-	hdr->itt		= be32_to_cpu(hdr->itt);
-	hdr->data_length	= be32_to_cpu(hdr->data_length);
-	hdr->cmdsn		= be32_to_cpu(hdr->cmdsn);
-	hdr->exp_statsn		= be32_to_cpu(hdr->exp_statsn);
 
 	/* FIXME; Add checks for AdditionalHeaderSegment */
 
@@ -852,7 +848,7 @@ done:
 				buf, conn);
 	}
 
-	if ((hdr->data_length == payload_length) &&
+	if ((be32_to_cpu(hdr->data_length )== payload_length) &&
 	    (!(hdr->flags & ISCSI_FLAG_CMD_FINAL))) {
 		pr_err("Expected Data Transfer Length and Length of"
 			" Immediate Data are the same, but ISCSI_FLAG_CMD_FINAL"
@@ -861,7 +857,7 @@ done:
 				buf, conn);
 	}
 
-	if (payload_length > hdr->data_length) {
+	if (payload_length > be32_to_cpu(hdr->data_length)) {
 		pr_err("DataSegmentLength: %u is greater than"
 			" EDTL: %u, protocol error.\n", payload_length,
 				hdr->data_length);
@@ -869,10 +865,10 @@ done:
 				buf, conn);
 	}
 
-	if (payload_length > conn->conn_ops->MaxRecvDataSegmentLength) {
+	if (payload_length > conn->conn_ops->MaxXmitDataSegmentLength) {
 		pr_err("DataSegmentLength: %u is greater than"
-			" MaxRecvDataSegmentLength: %u, protocol error.\n",
-			payload_length, conn->conn_ops->MaxRecvDataSegmentLength);
+			" MaxXmitDataSegmentLength: %u, protocol error.\n",
+			payload_length, conn->conn_ops->MaxXmitDataSegmentLength);
 		return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
 				buf, conn);
 	}
@@ -932,8 +928,8 @@ done:
 		spin_unlock_bh(&conn->sess->ttt_lock);
 	} else if (hdr->flags & ISCSI_FLAG_CMD_WRITE)
 		cmd->targ_xfer_tag = 0xFFFFFFFF;
-	cmd->cmd_sn		= hdr->cmdsn;
-	cmd->exp_stat_sn	= hdr->exp_statsn;
+	cmd->cmd_sn		= be32_to_cpu(hdr->cmdsn);
+	cmd->exp_stat_sn	= be32_to_cpu(hdr->exp_statsn);
 	cmd->first_burst_len	= payload_length;
 
 	if (cmd->data_direction == DMA_FROM_DEVICE) {
@@ -952,8 +948,9 @@ done:
 	 * Initialize struct se_cmd descriptor from target_core_mod infrastructure
 	 */
 	transport_init_se_cmd(&cmd->se_cmd, &lio_target_fabric_configfs->tf_ops,
-			conn->sess->se_sess, hdr->data_length, cmd->data_direction,
-			sam_task_attr, &cmd->sense_buffer[0]);
+			conn->sess->se_sess, be32_to_cpu(hdr->data_length),
+			cmd->data_direction, sam_task_attr,
+			cmd->sense_buffer + 2);
 
 	pr_debug("Got SCSI Command, ITT: 0x%08x, CmdSN: 0x%08x,"
 		" ExpXferLen: %u, Length: %u, CID: %hu\n", hdr->itt,
@@ -1028,7 +1025,7 @@ attach_cmd:
 				1, 0, buf, cmd);
 	}
 
-	iscsit_ack_from_expstatsn(conn, hdr->exp_statsn);
+	iscsit_ack_from_expstatsn(conn, be32_to_cpu(hdr->exp_statsn));
 
 	/*
 	 * If no Immediate Data is attached, it's OK to return now.
@@ -1194,11 +1191,6 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
 
 	hdr			= (struct iscsi_data *) buf;
 	payload_length		= ntoh24(hdr->dlength);
-	hdr->itt		= be32_to_cpu(hdr->itt);
-	hdr->ttt		= be32_to_cpu(hdr->ttt);
-	hdr->exp_statsn		= be32_to_cpu(hdr->exp_statsn);
-	hdr->datasn		= be32_to_cpu(hdr->datasn);
-	hdr->offset		= be32_to_cpu(hdr->offset);
 
 	if (!payload_length) {
 		pr_err("DataOUT payload is ZERO, protocol error.\n");
@@ -1216,10 +1208,10 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
 	}
 	spin_unlock_bh(&conn->sess->session_stats_lock);
 
-	if (payload_length > conn->conn_ops->MaxRecvDataSegmentLength) {
+	if (payload_length > conn->conn_ops->MaxXmitDataSegmentLength) {
 		pr_err("DataSegmentLength: %u is greater than"
-			" MaxRecvDataSegmentLength: %u\n", payload_length,
-			conn->conn_ops->MaxRecvDataSegmentLength);
+			" MaxXmitDataSegmentLength: %u\n", payload_length,
+			conn->conn_ops->MaxXmitDataSegmentLength);
 		return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
 					buf, conn);
 	}
@@ -1250,7 +1242,7 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
 	se_cmd = &cmd->se_cmd;
 	iscsit_mod_dataout_timer(cmd);
 
-	if ((hdr->offset + payload_length) > cmd->se_cmd.data_length) {
+	if ((be32_to_cpu(hdr->offset) + payload_length) > cmd->se_cmd.data_length) {
 		pr_err("DataOut Offset: %u, Length %u greater than"
 			" iSCSI Command EDTL %u, protocol error.\n",
 			hdr->offset, payload_length, cmd->se_cmd.data_length);
@@ -1333,7 +1325,8 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
 	rx_size += payload_length;
 	iov = &cmd->iov_data[0];
 
-	iov_ret = iscsit_map_iovec(cmd, iov, hdr->offset, payload_length);
+	iov_ret = iscsit_map_iovec(cmd, iov, be32_to_cpu(hdr->offset),
+				   payload_length);
 	if (iov_ret < 0)
 		return -1;
 
@@ -1364,7 +1357,8 @@ static int iscsit_handle_data_out(struct iscsi_conn *conn, unsigned char *buf)
 		u32 data_crc;
 
 		data_crc = iscsit_do_crypto_hash_sg(&conn->conn_rx_hash, cmd,
-						    hdr->offset, payload_length, padding,
+						    be32_to_cpu(hdr->offset),
+						    payload_length, padding,
 						    cmd->pad_bytes);
 
 		if (checksum != data_crc) {
@@ -1425,30 +1419,26 @@ static int iscsit_handle_nop_out(
 
 	hdr			= (struct iscsi_nopout *) buf;
 	payload_length		= ntoh24(hdr->dlength);
-	hdr->itt		= be32_to_cpu(hdr->itt);
-	hdr->ttt		= be32_to_cpu(hdr->ttt);
-	hdr->cmdsn		= be32_to_cpu(hdr->cmdsn);
-	hdr->exp_statsn		= be32_to_cpu(hdr->exp_statsn);
 
-	if ((hdr->itt == 0xFFFFFFFF) && !(hdr->opcode & ISCSI_OP_IMMEDIATE)) {
+	if (hdr->itt == RESERVED_ITT && !(hdr->opcode & ISCSI_OP_IMMEDIATE)) {
 		pr_err("NOPOUT ITT is reserved, but Immediate Bit is"
 			" not set, protocol error.\n");
 		return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
 					buf, conn);
 	}
 
-	if (payload_length > conn->conn_ops->MaxRecvDataSegmentLength) {
+	if (payload_length > conn->conn_ops->MaxXmitDataSegmentLength) {
 		pr_err("NOPOUT Ping Data DataSegmentLength: %u is"
-			" greater than MaxRecvDataSegmentLength: %u, protocol"
+			" greater than MaxXmitDataSegmentLength: %u, protocol"
 			" error.\n", payload_length,
-			conn->conn_ops->MaxRecvDataSegmentLength);
+			conn->conn_ops->MaxXmitDataSegmentLength);
 		return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
 					buf, conn);
 	}
 
 	pr_debug("Got NOPOUT Ping %s ITT: 0x%08x, TTT: 0x%09x,"
 		" CmdSN: 0x%08x, ExpStatSN: 0x%08x, Length: %u\n",
-		(hdr->itt == 0xFFFFFFFF) ? "Response" : "Request",
+		hdr->itt == RESERVED_ITT ? "Response" : "Request",
 		hdr->itt, hdr->ttt, hdr->cmdsn, hdr->exp_statsn,
 		payload_length);
 	/*
@@ -1458,7 +1448,7 @@ static int iscsit_handle_nop_out(
 	 * Either way, make sure we allocate an struct iscsi_cmd, as both
 	 * can contain ping data.
 	 */
-	if (hdr->ttt == 0xFFFFFFFF) {
+	if (hdr->ttt == cpu_to_be32(0xFFFFFFFF)) {
 		cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
 		if (!cmd)
 			return iscsit_add_reject(
@@ -1471,12 +1461,12 @@ static int iscsit_handle_nop_out(
 						1 : 0);
 		conn->sess->init_task_tag = cmd->init_task_tag = hdr->itt;
 		cmd->targ_xfer_tag	= 0xFFFFFFFF;
-		cmd->cmd_sn		= hdr->cmdsn;
-		cmd->exp_stat_sn	= hdr->exp_statsn;
+		cmd->cmd_sn		= be32_to_cpu(hdr->cmdsn);
+		cmd->exp_stat_sn	= be32_to_cpu(hdr->exp_statsn);
 		cmd->data_direction	= DMA_NONE;
 	}
 
-	if (payload_length && (hdr->ttt == 0xFFFFFFFF)) {
+	if (payload_length && hdr->ttt == cpu_to_be32(0xFFFFFFFF)) {
 		rx_size = payload_length;
 		ping_data = kzalloc(payload_length + 1, GFP_KERNEL);
 		if (!ping_data) {
@@ -1556,7 +1546,7 @@ static int iscsit_handle_nop_out(
 		pr_debug("Ping Data: \"%s\"\n", ping_data);
 	}
 
-	if (hdr->itt != 0xFFFFFFFF) {
+	if (hdr->itt != RESERVED_ITT) {
 		if (!cmd) {
 			pr_err("Checking CmdSN for NOPOUT,"
 				" but cmd is NULL!\n");
@@ -1569,7 +1559,7 @@ static int iscsit_handle_nop_out(
 		list_add_tail(&cmd->i_conn_node, &conn->conn_cmd_list);
 		spin_unlock_bh(&conn->cmd_lock);
 
-		iscsit_ack_from_expstatsn(conn, hdr->exp_statsn);
+		iscsit_ack_from_expstatsn(conn, be32_to_cpu(hdr->exp_statsn));
 
 		if (hdr->opcode & ISCSI_OP_IMMEDIATE) {
 			iscsit_add_cmd_to_response_queue(cmd, conn,
@@ -1590,11 +1580,11 @@ static int iscsit_handle_nop_out(
 		return 0;
 	}
 
-	if (hdr->ttt != 0xFFFFFFFF) {
+	if (hdr->ttt != cpu_to_be32(0xFFFFFFFF)) {
 		/*
 		 * This was a response to a unsolicited NOPIN ping.
 		 */
-		cmd = iscsit_find_cmd_from_ttt(conn, hdr->ttt);
+		cmd = iscsit_find_cmd_from_ttt(conn, be32_to_cpu(hdr->ttt));
 		if (!cmd)
 			return -1;
 
@@ -1639,12 +1629,6 @@ static int iscsit_handle_task_mgt_cmd(
 	u8 function;
 
 	hdr			= (struct iscsi_tm *) buf;
-	hdr->itt		= be32_to_cpu(hdr->itt);
-	hdr->rtt		= be32_to_cpu(hdr->rtt);
-	hdr->cmdsn		= be32_to_cpu(hdr->cmdsn);
-	hdr->exp_statsn		= be32_to_cpu(hdr->exp_statsn);
-	hdr->refcmdsn		= be32_to_cpu(hdr->refcmdsn);
-	hdr->exp_datasn		= be32_to_cpu(hdr->exp_datasn);
 	hdr->flags &= ~ISCSI_FLAG_CMD_FINAL;
 	function = hdr->flags;
 
@@ -1655,9 +1639,9 @@ static int iscsit_handle_task_mgt_cmd(
 
 	if ((function != ISCSI_TM_FUNC_ABORT_TASK) &&
 	    ((function != ISCSI_TM_FUNC_TASK_REASSIGN) &&
-	     (hdr->rtt != ISCSI_RESERVED_TAG))) {
+	     hdr->rtt != RESERVED_ITT)) {
 		pr_err("RefTaskTag should be set to 0xFFFFFFFF.\n");
-		hdr->rtt = ISCSI_RESERVED_TAG;
+		hdr->rtt = RESERVED_ITT;
 	}
 
 	if ((function == ISCSI_TM_FUNC_TASK_REASSIGN) &&
@@ -1669,8 +1653,8 @@ static int iscsit_handle_task_mgt_cmd(
 					buf, conn);
 	}
 	if ((function != ISCSI_TM_FUNC_ABORT_TASK) &&
-	    (hdr->refcmdsn != ISCSI_RESERVED_TAG))
-		hdr->refcmdsn = ISCSI_RESERVED_TAG;
+	    be32_to_cpu(hdr->refcmdsn) != ISCSI_RESERVED_TAG)
+		hdr->refcmdsn = cpu_to_be32(ISCSI_RESERVED_TAG);
 
 	cmd = iscsit_allocate_cmd(conn, GFP_KERNEL);
 	if (!cmd)
@@ -1700,7 +1684,7 @@ static int iscsit_handle_task_mgt_cmd(
 		transport_init_se_cmd(&cmd->se_cmd,
 				      &lio_target_fabric_configfs->tf_ops,
 				      conn->sess->se_sess, 0, DMA_NONE,
-				      MSG_SIMPLE_TAG, &cmd->sense_buffer[0]);
+				      MSG_SIMPLE_TAG, cmd->sense_buffer + 2);
 
 		switch (function) {
 		case ISCSI_TM_FUNC_ABORT_TASK:
@@ -1747,8 +1731,8 @@ static int iscsit_handle_task_mgt_cmd(
 	cmd->immediate_cmd	= ((hdr->opcode & ISCSI_OP_IMMEDIATE) ? 1 : 0);
 	cmd->init_task_tag	= hdr->itt;
 	cmd->targ_xfer_tag	= 0xFFFFFFFF;
-	cmd->cmd_sn		= hdr->cmdsn;
-	cmd->exp_stat_sn	= hdr->exp_statsn;
+	cmd->cmd_sn		= be32_to_cpu(hdr->cmdsn);
+	cmd->exp_stat_sn	= be32_to_cpu(hdr->exp_statsn);
 	se_tmr			= cmd->se_cmd.se_tmr_req;
 	tmr_req			= cmd->tmr_req;
 	/*
@@ -1832,7 +1816,7 @@ attach:
 					ISCSI_REASON_PROTOCOL_ERROR,
 					1, 0, buf, cmd);
 	}
-	iscsit_ack_from_expstatsn(conn, hdr->exp_statsn);
+	iscsit_ack_from_expstatsn(conn, be32_to_cpu(hdr->exp_statsn));
 
 	if (out_of_order_cmdsn || !(hdr->opcode & ISCSI_OP_IMMEDIATE))
 		return 0;
@@ -1869,15 +1853,11 @@ static int iscsit_handle_text_cmd(
 
 	hdr			= (struct iscsi_text *) buf;
 	payload_length		= ntoh24(hdr->dlength);
-	hdr->itt		= be32_to_cpu(hdr->itt);
-	hdr->ttt		= be32_to_cpu(hdr->ttt);
-	hdr->cmdsn		= be32_to_cpu(hdr->cmdsn);
-	hdr->exp_statsn		= be32_to_cpu(hdr->exp_statsn);
 
-	if (payload_length > conn->conn_ops->MaxRecvDataSegmentLength) {
+	if (payload_length > conn->conn_ops->MaxXmitDataSegmentLength) {
 		pr_err("Unable to accept text parameter length: %u"
-			"greater than MaxRecvDataSegmentLength %u.\n",
-		       payload_length, conn->conn_ops->MaxRecvDataSegmentLength);
+			"greater than MaxXmitDataSegmentLength %u.\n",
+		       payload_length, conn->conn_ops->MaxXmitDataSegmentLength);
 		return iscsit_add_reject(ISCSI_REASON_PROTOCOL_ERROR, 1,
 					buf, conn);
 	}
@@ -1989,15 +1969,15 @@ static int iscsit_handle_text_cmd(
 	cmd->immediate_cmd	= ((hdr->opcode & ISCSI_OP_IMMEDIATE) ? 1 : 0);
 	conn->sess->init_task_tag = cmd->init_task_tag	= hdr->itt;
 	cmd->targ_xfer_tag	= 0xFFFFFFFF;
-	cmd->cmd_sn		= hdr->cmdsn;
-	cmd->exp_stat_sn	= hdr->exp_statsn;
+	cmd->cmd_sn		= be32_to_cpu(hdr->cmdsn);
+	cmd->exp_stat_sn	= be32_to_cpu(hdr->exp_statsn);
 	cmd->data_direction	= DMA_NONE;
 
 	spin_lock_bh(&conn->cmd_lock);
 	list_add_tail(&cmd->i_conn_node, &conn->conn_cmd_list);
 	spin_unlock_bh(&conn->cmd_lock);
 
-	iscsit_ack_from_expstatsn(conn, hdr->exp_statsn);
+	iscsit_ack_from_expstatsn(conn, be32_to_cpu(hdr->exp_statsn));
 
 	if (!(hdr->opcode & ISCSI_OP_IMMEDIATE)) {
 		cmdsn_ret = iscsit_sequence_cmd(conn, cmd, hdr->cmdsn);
@@ -2131,10 +2111,6 @@ static int iscsit_handle_logout_cmd(
 
 	hdr			= (struct iscsi_logout *) buf;
 	reason_code		= (hdr->flags & 0x7f);
-	hdr->itt		= be32_to_cpu(hdr->itt);
-	hdr->cid		= be16_to_cpu(hdr->cid);
-	hdr->cmdsn		= be32_to_cpu(hdr->cmdsn);
-	hdr->exp_statsn	= be32_to_cpu(hdr->exp_statsn);
 
 	if (tiqn) {
 		spin_lock(&tiqn->logout_stats.lock);
@@ -2166,9 +2142,9 @@ static int iscsit_handle_logout_cmd(
 	cmd->immediate_cmd      = ((hdr->opcode & ISCSI_OP_IMMEDIATE) ? 1 : 0);
 	conn->sess->init_task_tag = cmd->init_task_tag  = hdr->itt;
 	cmd->targ_xfer_tag      = 0xFFFFFFFF;
-	cmd->cmd_sn             = hdr->cmdsn;
-	cmd->exp_stat_sn        = hdr->exp_statsn;
-	cmd->logout_cid         = hdr->cid;
+	cmd->cmd_sn             = be32_to_cpu(hdr->cmdsn);
+	cmd->exp_stat_sn        = be32_to_cpu(hdr->exp_statsn);
+	cmd->logout_cid         = be16_to_cpu(hdr->cid);
 	cmd->logout_reason      = reason_code;
 	cmd->data_direction     = DMA_NONE;
 
@@ -2178,7 +2154,7 @@ static int iscsit_handle_logout_cmd(
 	 */
 	if ((reason_code == ISCSI_LOGOUT_REASON_CLOSE_SESSION) ||
 	   ((reason_code == ISCSI_LOGOUT_REASON_CLOSE_CONNECTION) &&
-	    (hdr->cid == conn->cid)))
+	    be16_to_cpu(hdr->cid) == conn->cid))
 		logout_remove = 1;
 
 	spin_lock_bh(&conn->cmd_lock);
@@ -2186,7 +2162,7 @@ static int iscsit_handle_logout_cmd(
 	spin_unlock_bh(&conn->cmd_lock);
 
 	if (reason_code != ISCSI_LOGOUT_REASON_RECOVERY)
-		iscsit_ack_from_expstatsn(conn, hdr->exp_statsn);
+		iscsit_ack_from_expstatsn(conn, be32_to_cpu(hdr->exp_statsn));
 
 	/*
 	 * Immediate commands are executed, well, immediately.
@@ -2219,11 +2195,6 @@ static int iscsit_handle_snack(
 
 	hdr			= (struct iscsi_snack *) buf;
 	hdr->flags		&= ~ISCSI_FLAG_CMD_FINAL;
-	hdr->itt		= be32_to_cpu(hdr->itt);
-	hdr->ttt		= be32_to_cpu(hdr->ttt);
-	hdr->exp_statsn		= be32_to_cpu(hdr->exp_statsn);
-	hdr->begrun		= be32_to_cpu(hdr->begrun);
-	hdr->runlength		= be32_to_cpu(hdr->runlength);
 
 	pr_debug("Got ISCSI_INIT_SNACK, ITT: 0x%08x, ExpStatSN:"
 		" 0x%08x, Type: 0x%02x, BegRun: 0x%08x, RunLength: 0x%08x,"
@@ -2243,13 +2214,18 @@ static int iscsit_handle_snack(
 	switch (hdr->flags & ISCSI_FLAG_SNACK_TYPE_MASK) {
 	case 0:
 		return iscsit_handle_recovery_datain_or_r2t(conn, buf,
-			hdr->itt, hdr->ttt, hdr->begrun, hdr->runlength);
+			hdr->itt,
+			be32_to_cpu(hdr->ttt),
+			be32_to_cpu(hdr->begrun),
+			be32_to_cpu(hdr->runlength));
 	case ISCSI_FLAG_SNACK_TYPE_STATUS:
-		return iscsit_handle_status_snack(conn, hdr->itt, hdr->ttt,
-			hdr->begrun, hdr->runlength);
+		return iscsit_handle_status_snack(conn, hdr->itt,
+			be32_to_cpu(hdr->ttt),
+			be32_to_cpu(hdr->begrun), be32_to_cpu(hdr->runlength));
 	case ISCSI_FLAG_SNACK_TYPE_DATA_ACK:
-		return iscsit_handle_data_ack(conn, hdr->ttt, hdr->begrun,
-			hdr->runlength);
+		return iscsit_handle_data_ack(conn, be32_to_cpu(hdr->ttt),
+			be32_to_cpu(hdr->begrun),
+			be32_to_cpu(hdr->runlength));
 	case ISCSI_FLAG_SNACK_TYPE_RDATA:
 		/* FIXME: Support R-Data SNACK */
 		pr_err("R-Data SNACK Not Supported.\n");
@@ -2414,7 +2390,7 @@ static int iscsit_send_conn_drop_async_message(
 	hdr			= (struct iscsi_async *) cmd->pdu;
 	hdr->opcode		= ISCSI_OP_ASYNC_EVENT;
 	hdr->flags		= ISCSI_FLAG_CMD_FINAL;
-	cmd->init_task_tag	= 0xFFFFFFFF;
+	cmd->init_task_tag	= RESERVED_ITT;
 	cmd->targ_xfer_tag	= 0xFFFFFFFF;
 	put_unaligned_be64(0xFFFFFFFFFFFFFFFFULL, &hdr->rsvd4[0]);
 	cmd->stat_sn		= conn->stat_sn++;
@@ -2536,12 +2512,17 @@ static int iscsit_send_data_in(
 	else
 		put_unaligned_le64(0xFFFFFFFFFFFFFFFFULL, &hdr->lun);
 
-	hdr->itt		= cpu_to_be32(cmd->init_task_tag);
-	hdr->ttt		= (hdr->flags & ISCSI_FLAG_DATA_ACK) ?
-				   cpu_to_be32(cmd->targ_xfer_tag) :
-				   0xFFFFFFFF;
-	hdr->statsn		= (set_statsn) ? cpu_to_be32(cmd->stat_sn) :
-						0xFFFFFFFF;
+	hdr->itt		= cmd->init_task_tag;
+
+	if (hdr->flags & ISCSI_FLAG_DATA_ACK)
+		hdr->ttt		= cpu_to_be32(cmd->targ_xfer_tag);
+	else
+		hdr->ttt		= cpu_to_be32(0xFFFFFFFF);
+	if (set_statsn)
+		hdr->statsn		= cpu_to_be32(cmd->stat_sn);
+	else
+		hdr->statsn		= cpu_to_be32(0xFFFFFFFF);
+
 	hdr->exp_cmdsn		= cpu_to_be32(conn->sess->exp_cmd_sn);
 	hdr->max_cmdsn		= cpu_to_be32(conn->sess->max_cmd_sn);
 	hdr->datasn		= cpu_to_be32(datain.data_sn);
@@ -2708,7 +2689,7 @@ static int iscsit_send_logout_response(
 	hdr->opcode		= ISCSI_OP_LOGOUT_RSP;
 	hdr->flags		|= ISCSI_FLAG_CMD_FINAL;
 	hdr->response		= cmd->logout_response;
-	hdr->itt		= cpu_to_be32(cmd->init_task_tag);
+	hdr->itt		= cmd->init_task_tag;
 	cmd->stat_sn		= conn->stat_sn++;
 	hdr->statsn		= cpu_to_be32(cmd->stat_sn);
 
@@ -2759,7 +2740,7 @@ static int iscsit_send_unsolicited_nopin(
 	memset(hdr, 0, ISCSI_HDR_LEN);
 	hdr->opcode		= ISCSI_OP_NOOP_IN;
 	hdr->flags		|= ISCSI_FLAG_CMD_FINAL;
-	hdr->itt		= cpu_to_be32(cmd->init_task_tag);
+	hdr->itt		= cmd->init_task_tag;
 	hdr->ttt		= cpu_to_be32(cmd->targ_xfer_tag);
 	cmd->stat_sn		= conn->stat_sn;
 	hdr->statsn		= cpu_to_be32(cmd->stat_sn);
@@ -2816,7 +2797,7 @@ static int iscsit_send_nopin_response(
 	hdr->flags		|= ISCSI_FLAG_CMD_FINAL;
 	hton24(hdr->dlength, cmd->buf_ptr_size);
 	put_unaligned_le64(0xFFFFFFFFFFFFFFFFULL, &hdr->lun);
-	hdr->itt		= cpu_to_be32(cmd->init_task_tag);
+	hdr->itt		= cmd->init_task_tag;
 	hdr->ttt		= cpu_to_be32(cmd->targ_xfer_tag);
 	cmd->stat_sn		= conn->stat_sn++;
 	hdr->statsn		= cpu_to_be32(cmd->stat_sn);
@@ -2906,7 +2887,7 @@ static int iscsit_send_r2t(
 	hdr->flags		|= ISCSI_FLAG_CMD_FINAL;
 	int_to_scsilun(cmd->se_cmd.orig_fe_lun,
 			(struct scsi_lun *)&hdr->lun);
-	hdr->itt		= cpu_to_be32(cmd->init_task_tag);
+	hdr->itt		= cmd->init_task_tag;
 	spin_lock_bh(&conn->sess->ttt_lock);
 	r2t->targ_xfer_tag	= conn->sess->targ_xfer_tag++;
 	if (r2t->targ_xfer_tag == 0xFFFFFFFF)
@@ -3074,7 +3055,7 @@ static int iscsit_send_status(
 	}
 	hdr->response		= cmd->iscsi_response;
 	hdr->cmd_status		= cmd->se_cmd.scsi_status;
-	hdr->itt		= cpu_to_be32(cmd->init_task_tag);
+	hdr->itt		= cmd->init_task_tag;
 	hdr->statsn		= cpu_to_be32(cmd->stat_sn);
 
 	iscsit_increment_maxcmdsn(cmd, conn->sess);
@@ -3092,15 +3073,18 @@ static int iscsit_send_status(
 	if (cmd->se_cmd.sense_buffer &&
 	   ((cmd->se_cmd.se_cmd_flags & SCF_TRANSPORT_TASK_SENSE) ||
 	    (cmd->se_cmd.se_cmd_flags & SCF_EMULATED_TASK_SENSE))) {
+		put_unaligned_be16(cmd->se_cmd.scsi_sense_length, cmd->sense_buffer);
+		cmd->se_cmd.scsi_sense_length += sizeof (__be16);
+
 		padding		= -(cmd->se_cmd.scsi_sense_length) & 3;
-		hton24(hdr->dlength, cmd->se_cmd.scsi_sense_length);
-		iov[iov_count].iov_base	= cmd->se_cmd.sense_buffer;
+		hton24(hdr->dlength, (u32)cmd->se_cmd.scsi_sense_length);
+		iov[iov_count].iov_base	= cmd->sense_buffer;
 		iov[iov_count++].iov_len =
 				(cmd->se_cmd.scsi_sense_length + padding);
 		tx_size += cmd->se_cmd.scsi_sense_length;
 
 		if (padding) {
-			memset(cmd->se_cmd.sense_buffer +
+			memset(cmd->sense_buffer +
 				cmd->se_cmd.scsi_sense_length, 0, padding);
 			tx_size += padding;
 			pr_debug("Adding %u bytes of padding to"
@@ -3109,7 +3093,7 @@ static int iscsit_send_status(
 
 		if (conn->conn_ops->DataDigest) {
 			iscsit_do_crypto_hash_buf(&conn->conn_tx_hash,
-				cmd->se_cmd.sense_buffer,
+				cmd->sense_buffer,
 				(cmd->se_cmd.scsi_sense_length + padding),
 				0, NULL, (u8 *)&cmd->data_crc);
 
@@ -3184,7 +3168,7 @@ static int iscsit_send_task_mgt_rsp(
 	hdr->opcode		= ISCSI_OP_SCSI_TMFUNC_RSP;
 	hdr->flags		= ISCSI_FLAG_CMD_FINAL;
 	hdr->response		= iscsit_convert_tcm_tmr_rsp(se_tmr);
-	hdr->itt		= cpu_to_be32(cmd->init_task_tag);
+	hdr->itt		= cmd->init_task_tag;
 	cmd->stat_sn		= conn->stat_sn++;
 	hdr->statsn		= cpu_to_be32(cmd->stat_sn);
 
@@ -3236,7 +3220,7 @@ static bool iscsit_check_inaddr_any(struct iscsi_np *np)
 		struct sockaddr_in * sock_in =
 			(struct sockaddr_in *)&np->np_sockaddr;
 
-		if (sock_in->sin_addr.s_addr == INADDR_ANY)
+		if (sock_in->sin_addr.s_addr == htonl(INADDR_ANY))
 			ret = true;
 	}
 
@@ -3271,7 +3255,6 @@ static int iscsit_build_sendtargets_response(struct iscsi_cmd *cmd)
 		len += 1;
 
 		if ((len + payload_len) > buffer_len) {
-			spin_unlock(&tiqn->tiqn_tpg_lock);
 			end_of_buf = 1;
 			goto eob;
 		}
@@ -3358,7 +3341,7 @@ static int iscsit_send_text_rsp(
 	hdr->opcode		= ISCSI_OP_TEXT_RSP;
 	hdr->flags		|= ISCSI_FLAG_CMD_FINAL;
 	hton24(hdr->dlength, text_length);
-	hdr->itt		= cpu_to_be32(cmd->init_task_tag);
+	hdr->itt		= cmd->init_task_tag;
 	hdr->ttt		= cpu_to_be32(cmd->targ_xfer_tag);
 	cmd->stat_sn		= conn->stat_sn++;
 	hdr->statsn		= cpu_to_be32(cmd->stat_sn);
@@ -3424,6 +3407,7 @@ static int iscsit_send_reject(
 	hdr->opcode		= ISCSI_OP_REJECT;
 	hdr->flags		|= ISCSI_FLAG_CMD_FINAL;
 	hton24(hdr->dlength, ISCSI_HDR_LEN);
+	hdr->ffffffff		= cpu_to_be32(0xffffffff);
 	cmd->stat_sn		= conn->stat_sn++;
 	hdr->statsn		= cpu_to_be32(cmd->stat_sn);
 	hdr->exp_cmdsn	= cpu_to_be32(conn->sess->exp_cmd_sn);
diff --git a/drivers/target/iscsi/iscsi_target.h b/drivers/target/iscsi/iscsi_target.h
index 12abb4c9e34..f1e4f3155ba 100644
--- a/drivers/target/iscsi/iscsi_target.h
+++ b/drivers/target/iscsi/iscsi_target.h
@@ -38,4 +38,9 @@ extern struct kmem_cache *lio_cmd_cache;
 extern struct kmem_cache *lio_qr_cache;
 extern struct kmem_cache *lio_r2t_cache;
 
+extern struct idr sess_idr;
+extern struct mutex auth_id_lock;
+extern spinlock_t sess_idr_lock;
+
+
 #endif   /*** ISCSI_TARGET_H ***/
diff --git a/drivers/target/iscsi/iscsi_target_configfs.c b/drivers/target/iscsi/iscsi_target_configfs.c
index a7b25e783b5..ff6fd4fb624 100644
--- a/drivers/target/iscsi/iscsi_target_configfs.c
+++ b/drivers/target/iscsi/iscsi_target_configfs.c
@@ -135,7 +135,7 @@ static struct configfs_attribute *lio_target_portal_attrs[] = {
 
 #define MAX_PORTAL_LEN		256
 
-struct se_tpg_np *lio_target_call_addnptotpg(
+static struct se_tpg_np *lio_target_call_addnptotpg(
 	struct se_portal_group *se_tpg,
 	struct config_group *group,
 	const char *name)
@@ -1034,6 +1034,9 @@ TPG_PARAM_ATTR(ImmediateData, S_IRUGO | S_IWUSR);
 DEF_TPG_PARAM(MaxRecvDataSegmentLength);
 TPG_PARAM_ATTR(MaxRecvDataSegmentLength, S_IRUGO | S_IWUSR);
 
+DEF_TPG_PARAM(MaxXmitDataSegmentLength);
+TPG_PARAM_ATTR(MaxXmitDataSegmentLength, S_IRUGO | S_IWUSR);
+
 DEF_TPG_PARAM(MaxBurstLength);
 TPG_PARAM_ATTR(MaxBurstLength, S_IRUGO | S_IWUSR);
 
@@ -1079,6 +1082,7 @@ static struct configfs_attribute *lio_target_tpg_param_attrs[] = {
 	&iscsi_tpg_param_InitialR2T.attr,
 	&iscsi_tpg_param_ImmediateData.attr,
 	&iscsi_tpg_param_MaxRecvDataSegmentLength.attr,
+	&iscsi_tpg_param_MaxXmitDataSegmentLength.attr,
 	&iscsi_tpg_param_MaxBurstLength.attr,
 	&iscsi_tpg_param_FirstBurstLength.attr,
 	&iscsi_tpg_param_DefaultTime2Wait.attr,
@@ -1166,7 +1170,7 @@ static struct configfs_attribute *lio_target_tpg_attrs[] = {
 
 /* Start items for lio_target_tiqn_cit */
 
-struct se_portal_group *lio_target_tiqn_addtpg(
+static struct se_portal_group *lio_target_tiqn_addtpg(
 	struct se_wwn *wwn,
 	struct config_group *group,
 	const char *name)
@@ -1216,7 +1220,7 @@ out:
 	return NULL;
 }
 
-void lio_target_tiqn_deltpg(struct se_portal_group *se_tpg)
+static void lio_target_tiqn_deltpg(struct se_portal_group *se_tpg)
 {
 	struct iscsi_portal_group *tpg;
 	struct iscsi_tiqn *tiqn;
@@ -1248,7 +1252,7 @@ static struct configfs_attribute *lio_target_wwn_attrs[] = {
 	NULL,
 };
 
-struct se_wwn *lio_target_call_coreaddtiqn(
+static struct se_wwn *lio_target_call_coreaddtiqn(
 	struct target_fabric_configfs *tf,
 	struct config_group *group,
 	const char *name)
@@ -1296,7 +1300,7 @@ struct se_wwn *lio_target_call_coreaddtiqn(
 	return &tiqn->tiqn_wwn;
 }
 
-void lio_target_call_coredeltiqn(
+static void lio_target_call_coredeltiqn(
 	struct se_wwn *wwn)
 {
 	struct iscsi_tiqn *tiqn = container_of(wwn, struct iscsi_tiqn, tiqn_wwn);
@@ -1471,7 +1475,8 @@ static u32 iscsi_get_task_tag(struct se_cmd *se_cmd)
 {
 	struct iscsi_cmd *cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
 
-	return cmd->init_task_tag;
+	/* only used for printks or comparism with ->ref_task_tag */
+	return (__force u32)cmd->init_task_tag;
 }
 
 static int iscsi_get_cmd_state(struct se_cmd *se_cmd)
@@ -1542,29 +1547,6 @@ static int lio_queue_status(struct se_cmd *se_cmd)
 	return 0;
 }
 
-static u16 lio_set_fabric_sense_len(struct se_cmd *se_cmd, u32 sense_length)
-{
-	unsigned char *buffer = se_cmd->sense_buffer;
-	/*
-	 * From RFC-3720 10.4.7.  Data Segment - Sense and Response Data Segment
-	 * 16-bit SenseLength.
-	 */
-	buffer[0] = ((sense_length >> 8) & 0xff);
-	buffer[1] = (sense_length & 0xff);
-	/*
-	 * Return two byte offset into allocated sense_buffer.
-	 */
-	return 2;
-}
-
-static u16 lio_get_fabric_sense_len(void)
-{
-	/*
-	 * Return two byte offset into allocated sense_buffer.
-	 */
-	return 2;
-}
-
 static int lio_queue_tm_rsp(struct se_cmd *se_cmd)
 {
 	struct iscsi_cmd *cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
@@ -1748,8 +1730,6 @@ int iscsi_target_register_configfs(void)
 	fabric->tf_ops.queue_data_in = &lio_queue_data_in;
 	fabric->tf_ops.queue_status = &lio_queue_status;
 	fabric->tf_ops.queue_tm_rsp = &lio_queue_tm_rsp;
-	fabric->tf_ops.set_fabric_sense_len = &lio_set_fabric_sense_len;
-	fabric->tf_ops.get_fabric_sense_len = &lio_get_fabric_sense_len;
 	/*
 	 * Setup function pointers for generic logic in target_core_fabric_configfs.c
 	 */
diff --git a/drivers/target/iscsi/iscsi_target_core.h b/drivers/target/iscsi/iscsi_target_core.h
index 8a908b28d8b..2ba9f9b9435 100644
--- a/drivers/target/iscsi/iscsi_target_core.h
+++ b/drivers/target/iscsi/iscsi_target_core.h
@@ -25,10 +25,10 @@
 #define NA_DATAOUT_TIMEOUT_RETRIES	5
 #define NA_DATAOUT_TIMEOUT_RETRIES_MAX	15
 #define NA_DATAOUT_TIMEOUT_RETRIES_MIN	1
-#define NA_NOPIN_TIMEOUT		5
+#define NA_NOPIN_TIMEOUT		15
 #define NA_NOPIN_TIMEOUT_MAX		60
 #define NA_NOPIN_TIMEOUT_MIN		3
-#define NA_NOPIN_RESPONSE_TIMEOUT	5
+#define NA_NOPIN_RESPONSE_TIMEOUT	30
 #define NA_NOPIN_RESPONSE_TIMEOUT_MAX	60
 #define NA_NOPIN_RESPONSE_TIMEOUT_MIN	3
 #define NA_RANDOM_DATAIN_PDU_OFFSETS	0
@@ -239,6 +239,7 @@ struct iscsi_conn_ops {
 	u8	HeaderDigest;			/* [0,1] == [None,CRC32C] */
 	u8	DataDigest;			/* [0,1] == [None,CRC32C] */
 	u32	MaxRecvDataSegmentLength;	/* [512..2**24-1] */
+	u32	MaxXmitDataSegmentLength;	/* [512..2**24-1] */
 	u8	OFMarker;			/* [0,1] == [No,Yes] */
 	u8	IFMarker;			/* [0,1] == [No,Yes] */
 	u32	OFMarkInt;			/* [1..65535] */
@@ -360,7 +361,7 @@ struct iscsi_cmd {
 	/* Command flags */
 	enum cmd_flags_table	cmd_flags;
 	/* Initiator Task Tag assigned from Initiator */
-	u32			init_task_tag;
+	itt_t			init_task_tag;
 	/* Target Transfer Tag assigned from Target */
 	u32			targ_xfer_tag;
 	/* CmdSN assigned from Initiator */
@@ -478,7 +479,6 @@ struct iscsi_cmd {
 
 struct iscsi_tmr_req {
 	bool			task_reassign:1;
-	u32			ref_cmd_sn;
 	u32			exp_data_sn;
 	struct iscsi_cmd	*ref_cmd;
 	struct iscsi_conn_recovery *conn_recovery;
@@ -505,7 +505,7 @@ struct iscsi_conn {
 	u32			auth_id;
 	u32			conn_flags;
 	/* Used for iscsi_tx_login_rsp() */
-	u32			login_itt;
+	itt_t			login_itt;
 	u32			exp_statsn;
 	/* Per connection status sequence number */
 	u32			stat_sn;
@@ -578,6 +578,7 @@ struct iscsi_conn_recovery {
 	u16			cid;
 	u32			cmd_count;
 	u32			maxrecvdatasegmentlength;
+	u32			maxxmitdatasegmentlength;
 	int			ready_for_reallegiance;
 	struct list_head	conn_recovery_cmd_list;
 	spinlock_t		conn_recovery_cmd_lock;
@@ -597,7 +598,7 @@ struct iscsi_session {
 	/* state session is currently in */
 	u32			session_state;
 	/* session wide counter: initiator assigned task tag */
-	u32			init_task_tag;
+	itt_t			init_task_tag;
 	/* session wide counter: target assigned task tag */
 	u32			targ_xfer_tag;
 	u32			cmdsn_window;
@@ -663,7 +664,7 @@ struct iscsi_login {
 	u8 version_max;
 	char isid[6];
 	u32 cmd_sn;
-	u32 init_task_tag;
+	itt_t init_task_tag;
 	u32 initial_exp_statsn;
 	u32 rsp_length;
 	u16 cid;
diff --git a/drivers/target/iscsi/iscsi_target_erl0.c b/drivers/target/iscsi/iscsi_target_erl0.c
index 1a02016ecda..8aacf611b86 100644
--- a/drivers/target/iscsi/iscsi_target_erl0.c
+++ b/drivers/target/iscsi/iscsi_target_erl0.c
@@ -48,9 +48,9 @@ void iscsit_set_dataout_sequence_values(
 	if (cmd->unsolicited_data) {
 		cmd->seq_start_offset = cmd->write_data_done;
 		cmd->seq_end_offset = (cmd->write_data_done +
-			(cmd->se_cmd.data_length >
-			 conn->sess->sess_ops->FirstBurstLength) ?
-			conn->sess->sess_ops->FirstBurstLength : cmd->se_cmd.data_length);
+			((cmd->se_cmd.data_length >
+			  conn->sess->sess_ops->FirstBurstLength) ?
+			 conn->sess->sess_ops->FirstBurstLength : cmd->se_cmd.data_length));
 		return;
 	}
 
@@ -95,14 +95,15 @@ static int iscsit_dataout_within_command_recovery_check(
 	 */
 	if (conn->sess->sess_ops->DataSequenceInOrder) {
 		if ((cmd->cmd_flags & ICF_WITHIN_COMMAND_RECOVERY) &&
-		    (cmd->write_data_done != hdr->offset))
+		    cmd->write_data_done != be32_to_cpu(hdr->offset))
 			goto dump;
 
 		cmd->cmd_flags &= ~ICF_WITHIN_COMMAND_RECOVERY;
 	} else {
 		struct iscsi_seq *seq;
 
-		seq = iscsit_get_seq_holder(cmd, hdr->offset, payload_length);
+		seq = iscsit_get_seq_holder(cmd, be32_to_cpu(hdr->offset),
+					    payload_length);
 		if (!seq)
 			return DATAOUT_CANNOT_RECOVER;
 		/*
@@ -111,15 +112,15 @@ static int iscsit_dataout_within_command_recovery_check(
 		cmd->seq_ptr = seq;
 
 		if (conn->sess->sess_ops->DataPDUInOrder) {
-			if ((seq->status ==
-			     DATAOUT_SEQUENCE_WITHIN_COMMAND_RECOVERY) &&
-			   ((seq->offset != hdr->offset) ||
-			    (seq->data_sn != hdr->datasn)))
+			if (seq->status ==
+			    DATAOUT_SEQUENCE_WITHIN_COMMAND_RECOVERY &&
+			   (seq->offset != be32_to_cpu(hdr->offset) ||
+			    seq->data_sn != be32_to_cpu(hdr->datasn)))
 				goto dump;
 		} else {
-			if ((seq->status ==
-			     DATAOUT_SEQUENCE_WITHIN_COMMAND_RECOVERY) &&
-			    (seq->data_sn != hdr->datasn))
+			if (seq->status ==
+			     DATAOUT_SEQUENCE_WITHIN_COMMAND_RECOVERY &&
+			    seq->data_sn != be32_to_cpu(hdr->datasn))
 				goto dump;
 		}
 
@@ -148,12 +149,12 @@ static int iscsit_dataout_check_unsolicited_sequence(
 	u32 payload_length = ntoh24(hdr->dlength);
 
 
-	if ((hdr->offset < cmd->seq_start_offset) ||
-	   ((hdr->offset + payload_length) > cmd->seq_end_offset)) {
+	if ((be32_to_cpu(hdr->offset) < cmd->seq_start_offset) ||
+	   ((be32_to_cpu(hdr->offset) + payload_length) > cmd->seq_end_offset)) {
 		pr_err("Command ITT: 0x%08x with Offset: %u,"
 		" Length: %u outside of Unsolicited Sequence %u:%u while"
 		" DataSequenceInOrder=Yes.\n", cmd->init_task_tag,
-		hdr->offset, payload_length, cmd->seq_start_offset,
+		be32_to_cpu(hdr->offset), payload_length, cmd->seq_start_offset,
 			cmd->seq_end_offset);
 		return DATAOUT_CANNOT_RECOVER;
 	}
@@ -236,12 +237,12 @@ static int iscsit_dataout_check_sequence(
 		 * fullfilling an Recovery R2T, it's best to just dump the
 		 * payload here, instead of erroring out.
 		 */
-		if ((hdr->offset < cmd->seq_start_offset) ||
-		   ((hdr->offset + payload_length) > cmd->seq_end_offset)) {
+		if ((be32_to_cpu(hdr->offset) < cmd->seq_start_offset) ||
+		   ((be32_to_cpu(hdr->offset) + payload_length) > cmd->seq_end_offset)) {
 			pr_err("Command ITT: 0x%08x with Offset: %u,"
 			" Length: %u outside of Sequence %u:%u while"
 			" DataSequenceInOrder=Yes.\n", cmd->init_task_tag,
-			hdr->offset, payload_length, cmd->seq_start_offset,
+			be32_to_cpu(hdr->offset), payload_length, cmd->seq_start_offset,
 				cmd->seq_end_offset);
 
 			if (iscsit_dump_data_payload(conn, payload_length, 1) < 0)
@@ -251,7 +252,8 @@ static int iscsit_dataout_check_sequence(
 
 		next_burst_len = (cmd->next_burst_len + payload_length);
 	} else {
-		seq = iscsit_get_seq_holder(cmd, hdr->offset, payload_length);
+		seq = iscsit_get_seq_holder(cmd, be32_to_cpu(hdr->offset),
+					    payload_length);
 		if (!seq)
 			return DATAOUT_CANNOT_RECOVER;
 		/*
@@ -366,16 +368,16 @@ static int iscsit_dataout_check_datasn(
 		data_sn = seq->data_sn;
 	}
 
-	if (hdr->datasn > data_sn) {
+	if (be32_to_cpu(hdr->datasn) > data_sn) {
 		pr_err("Command ITT: 0x%08x, received DataSN: 0x%08x"
 			" higher than expected 0x%08x.\n", cmd->init_task_tag,
-				hdr->datasn, data_sn);
+				be32_to_cpu(hdr->datasn), data_sn);
 		recovery = 1;
 		goto recover;
-	} else if (hdr->datasn < data_sn) {
+	} else if (be32_to_cpu(hdr->datasn) < data_sn) {
 		pr_err("Command ITT: 0x%08x, received DataSN: 0x%08x"
 			" lower than expected 0x%08x, discarding payload.\n",
-			cmd->init_task_tag, hdr->datasn, data_sn);
+			cmd->init_task_tag, be32_to_cpu(hdr->datasn), data_sn);
 		dump = 1;
 		goto dump;
 	}
@@ -415,26 +417,27 @@ static int iscsit_dataout_pre_datapduinorder_yes(
 	 * error has occured and fail the connection.
 	 */
 	if (conn->sess->sess_ops->DataSequenceInOrder) {
-		if (hdr->offset != cmd->write_data_done) {
+		if (be32_to_cpu(hdr->offset) != cmd->write_data_done) {
 			pr_err("Command ITT: 0x%08x, received offset"
 			" %u different than expected %u.\n", cmd->init_task_tag,
-				hdr->offset, cmd->write_data_done);
+				be32_to_cpu(hdr->offset), cmd->write_data_done);
 			recovery = 1;
 			goto recover;
 		}
 	} else {
 		struct iscsi_seq *seq = cmd->seq_ptr;
 
-		if (hdr->offset > seq->offset) {
+		if (be32_to_cpu(hdr->offset) > seq->offset) {
 			pr_err("Command ITT: 0x%08x, received offset"
 			" %u greater than expected %u.\n", cmd->init_task_tag,
-				hdr->offset, seq->offset);
+				be32_to_cpu(hdr->offset), seq->offset);
 			recovery = 1;
 			goto recover;
-		} else if (hdr->offset < seq->offset) {
+		} else if (be32_to_cpu(hdr->offset) < seq->offset) {
 			pr_err("Command ITT: 0x%08x, received offset"
 			" %u less than expected %u, discarding payload.\n",
-				cmd->init_task_tag, hdr->offset, seq->offset);
+				cmd->init_task_tag, be32_to_cpu(hdr->offset),
+				seq->offset);
 			dump = 1;
 			goto dump;
 		}
@@ -453,7 +456,7 @@ dump:
 		return DATAOUT_CANNOT_RECOVER;
 
 	return (recovery) ? iscsit_recover_dataout_sequence(cmd,
-		hdr->offset, payload_length) :
+		be32_to_cpu(hdr->offset), payload_length) :
 	       (dump) ? DATAOUT_WITHIN_COMMAND_RECOVERY : DATAOUT_NORMAL;
 }
 
@@ -465,7 +468,8 @@ static int iscsit_dataout_pre_datapduinorder_no(
 	struct iscsi_data *hdr = (struct iscsi_data *) buf;
 	u32 payload_length = ntoh24(hdr->dlength);
 
-	pdu = iscsit_get_pdu_holder(cmd, hdr->offset, payload_length);
+	pdu = iscsit_get_pdu_holder(cmd, be32_to_cpu(hdr->offset),
+				    payload_length);
 	if (!pdu)
 		return DATAOUT_CANNOT_RECOVER;
 
@@ -479,7 +483,7 @@ static int iscsit_dataout_pre_datapduinorder_no(
 	case ISCSI_PDU_RECEIVED_OK:
 		pr_err("Command ITT: 0x%08x received already gotten"
 			" Offset: %u, Length: %u\n", cmd->init_task_tag,
-				hdr->offset, payload_length);
+				be32_to_cpu(hdr->offset), payload_length);
 		return iscsit_dump_data_payload(cmd->conn, payload_length, 1);
 	default:
 		return DATAOUT_CANNOT_RECOVER;
@@ -553,7 +557,7 @@ static int iscsit_dataout_post_crc_passed(
 	if (cmd->unsolicited_data) {
 		if ((cmd->first_burst_len + payload_length) ==
 		     conn->sess->sess_ops->FirstBurstLength) {
-			if (iscsit_dataout_update_r2t(cmd, hdr->offset,
+			if (iscsit_dataout_update_r2t(cmd, be32_to_cpu(hdr->offset),
 					payload_length) < 0)
 				return DATAOUT_CANNOT_RECOVER;
 			send_r2t = 1;
@@ -561,7 +565,8 @@ static int iscsit_dataout_post_crc_passed(
 
 		if (!conn->sess->sess_ops->DataPDUInOrder) {
 			ret = iscsit_dataout_update_datapduinorder_no(cmd,
-				hdr->datasn, (hdr->flags & ISCSI_FLAG_CMD_FINAL));
+				be32_to_cpu(hdr->datasn),
+				(hdr->flags & ISCSI_FLAG_CMD_FINAL));
 			if (ret == DATAOUT_CANNOT_RECOVER)
 				return ret;
 		}
@@ -586,7 +591,8 @@ static int iscsit_dataout_post_crc_passed(
 		if (conn->sess->sess_ops->DataSequenceInOrder) {
 			if ((cmd->next_burst_len + payload_length) ==
 			     conn->sess->sess_ops->MaxBurstLength) {
-				if (iscsit_dataout_update_r2t(cmd, hdr->offset,
+				if (iscsit_dataout_update_r2t(cmd,
+						be32_to_cpu(hdr->offset),
 						payload_length) < 0)
 					return DATAOUT_CANNOT_RECOVER;
 				send_r2t = 1;
@@ -594,7 +600,7 @@ static int iscsit_dataout_post_crc_passed(
 
 			if (!conn->sess->sess_ops->DataPDUInOrder) {
 				ret = iscsit_dataout_update_datapduinorder_no(
-						cmd, hdr->datasn,
+						cmd, be32_to_cpu(hdr->datasn),
 						(hdr->flags & ISCSI_FLAG_CMD_FINAL));
 				if (ret == DATAOUT_CANNOT_RECOVER)
 					return ret;
@@ -610,7 +616,8 @@ static int iscsit_dataout_post_crc_passed(
 
 			if ((seq->next_burst_len + payload_length) ==
 			     seq->xfer_len) {
-				if (iscsit_dataout_update_r2t(cmd, hdr->offset,
+				if (iscsit_dataout_update_r2t(cmd,
+						be32_to_cpu(hdr->offset),
 						payload_length) < 0)
 					return DATAOUT_CANNOT_RECOVER;
 				send_r2t = 1;
@@ -618,7 +625,7 @@ static int iscsit_dataout_post_crc_passed(
 
 			if (!conn->sess->sess_ops->DataPDUInOrder) {
 				ret = iscsit_dataout_update_datapduinorder_no(
-						cmd, hdr->datasn,
+						cmd, be32_to_cpu(hdr->datasn),
 						(hdr->flags & ISCSI_FLAG_CMD_FINAL));
 				if (ret == DATAOUT_CANNOT_RECOVER)
 					return ret;
@@ -678,14 +685,15 @@ static int iscsit_dataout_post_crc_failed(
 	}
 
 recover:
-	return iscsit_recover_dataout_sequence(cmd, hdr->offset, payload_length);
+	return iscsit_recover_dataout_sequence(cmd, be32_to_cpu(hdr->offset),
+						payload_length);
 }
 
 /*
  *	Called from iscsit_handle_data_out() before DataOUT Payload is received
  *	and CRC computed.
  */
-extern int iscsit_check_pre_dataout(
+int iscsit_check_pre_dataout(
 	struct iscsi_cmd *cmd,
 	unsigned char *buf)
 {
@@ -789,7 +797,7 @@ static void iscsit_handle_time2retain_timeout(unsigned long data)
 	target_put_session(sess->se_sess);
 }
 
-extern void iscsit_start_time2retain_handler(struct iscsi_session *sess)
+void iscsit_start_time2retain_handler(struct iscsi_session *sess)
 {
 	int tpg_active;
 	/*
@@ -822,7 +830,7 @@ extern void iscsit_start_time2retain_handler(struct iscsi_session *sess)
 /*
  *	Called with spin_lock_bh(&struct se_portal_group->session_lock) held
  */
-extern int iscsit_stop_time2retain_timer(struct iscsi_session *sess)
+int iscsit_stop_time2retain_timer(struct iscsi_session *sess)
 {
 	struct iscsi_portal_group *tpg = ISCSI_TPG_S(sess);
 	struct se_portal_group *se_tpg = &tpg->tpg_se_tpg;
@@ -926,7 +934,7 @@ static void iscsit_handle_connection_cleanup(struct iscsi_conn *conn)
 	}
 }
 
-extern void iscsit_take_action_for_connection_exit(struct iscsi_conn *conn)
+void iscsit_take_action_for_connection_exit(struct iscsi_conn *conn)
 {
 	spin_lock_bh(&conn->state_lock);
 	if (atomic_read(&conn->connection_exit)) {
diff --git a/drivers/target/iscsi/iscsi_target_erl1.c b/drivers/target/iscsi/iscsi_target_erl1.c
index 3df8a2cef86..21f29d91a8c 100644
--- a/drivers/target/iscsi/iscsi_target_erl1.c
+++ b/drivers/target/iscsi/iscsi_target_erl1.c
@@ -466,7 +466,7 @@ static int iscsit_handle_recovery_datain(
 int iscsit_handle_recovery_datain_or_r2t(
 	struct iscsi_conn *conn,
 	unsigned char *buf,
-	u32 init_task_tag,
+	itt_t init_task_tag,
 	u32 targ_xfer_tag,
 	u32 begrun,
 	u32 runlength)
@@ -498,7 +498,7 @@ int iscsit_handle_recovery_datain_or_r2t(
 /* #warning FIXME: Status SNACK needs to be dependent on OPCODE!!! */
 int iscsit_handle_status_snack(
 	struct iscsi_conn *conn,
-	u32 init_task_tag,
+	itt_t init_task_tag,
 	u32 targ_xfer_tag,
 	u32 begrun,
 	u32 runlength)
diff --git a/drivers/target/iscsi/iscsi_target_erl1.h b/drivers/target/iscsi/iscsi_target_erl1.h
index 85e67e29de6..2a3ebf118a3 100644
--- a/drivers/target/iscsi/iscsi_target_erl1.h
+++ b/drivers/target/iscsi/iscsi_target_erl1.h
@@ -7,8 +7,8 @@ extern int iscsit_create_recovery_datain_values_datasequenceinorder_yes(
 extern int iscsit_create_recovery_datain_values_datasequenceinorder_no(
 			struct iscsi_cmd *, struct iscsi_datain_req *);
 extern int iscsit_handle_recovery_datain_or_r2t(struct iscsi_conn *, unsigned char *,
-			u32, u32, u32, u32);
-extern int iscsit_handle_status_snack(struct iscsi_conn *, u32, u32,
+			itt_t, u32, u32, u32);
+extern int iscsit_handle_status_snack(struct iscsi_conn *, itt_t, u32,
 			u32, u32);
 extern int iscsit_handle_data_ack(struct iscsi_conn *, u32, u32, u32);
 extern int iscsit_dataout_datapduinorder_no_fbit(struct iscsi_cmd *, struct iscsi_pdu *);
diff --git a/drivers/target/iscsi/iscsi_target_erl2.c b/drivers/target/iscsi/iscsi_target_erl2.c
index 65aac14fd83..17d8c20094f 100644
--- a/drivers/target/iscsi/iscsi_target_erl2.c
+++ b/drivers/target/iscsi/iscsi_target_erl2.c
@@ -36,7 +36,7 @@
  */
 void iscsit_create_conn_recovery_datain_values(
 	struct iscsi_cmd *cmd,
-	u32 exp_data_sn)
+	__be32 exp_data_sn)
 {
 	u32 data_sn = 0;
 	struct iscsi_conn *conn = cmd->conn;
@@ -44,7 +44,7 @@ void iscsit_create_conn_recovery_datain_values(
 	cmd->next_burst_len = 0;
 	cmd->read_data_done = 0;
 
-	while (exp_data_sn > data_sn) {
+	while (be32_to_cpu(exp_data_sn) > data_sn) {
 		if ((cmd->next_burst_len +
 		     conn->conn_ops->MaxRecvDataSegmentLength) <
 		     conn->sess->sess_ops->MaxBurstLength) {
@@ -193,15 +193,13 @@ int iscsit_remove_active_connection_recovery_entry(
 	return 0;
 }
 
-int iscsit_remove_inactive_connection_recovery_entry(
+static void iscsit_remove_inactive_connection_recovery_entry(
 	struct iscsi_conn_recovery *cr,
 	struct iscsi_session *sess)
 {
 	spin_lock(&sess->cr_i_lock);
 	list_del(&cr->cr_list);
 	spin_unlock(&sess->cr_i_lock);
-
-	return 0;
 }
 
 /*
@@ -421,6 +419,7 @@ int iscsit_prepare_cmds_for_realligance(struct iscsi_conn *conn)
 	cr->cid = conn->cid;
 	cr->cmd_count = cmd_count;
 	cr->maxrecvdatasegmentlength = conn->conn_ops->MaxRecvDataSegmentLength;
+	cr->maxxmitdatasegmentlength = conn->conn_ops->MaxXmitDataSegmentLength;
 	cr->sess = conn->sess;
 
 	iscsit_attach_inactive_connection_recovery_entry(conn->sess, cr);
diff --git a/drivers/target/iscsi/iscsi_target_erl2.h b/drivers/target/iscsi/iscsi_target_erl2.h
index 22f8d24780a..63f2501f3fe 100644
--- a/drivers/target/iscsi/iscsi_target_erl2.h
+++ b/drivers/target/iscsi/iscsi_target_erl2.h
@@ -1,7 +1,7 @@
 #ifndef ISCSI_TARGET_ERL2_H
 #define ISCSI_TARGET_ERL2_H
 
-extern void iscsit_create_conn_recovery_datain_values(struct iscsi_cmd *, u32);
+extern void iscsit_create_conn_recovery_datain_values(struct iscsi_cmd *, __be32);
 extern void iscsit_create_conn_recovery_dataout_values(struct iscsi_cmd *);
 extern struct iscsi_conn_recovery *iscsit_get_inactive_connection_recovery_entry(
 			struct iscsi_session *, u16);
diff --git a/drivers/target/iscsi/iscsi_target_login.c b/drivers/target/iscsi/iscsi_target_login.c
index 6aba4395e8d..cdc8a10939c 100644
--- a/drivers/target/iscsi/iscsi_target_login.c
+++ b/drivers/target/iscsi/iscsi_target_login.c
@@ -39,10 +39,6 @@
 #include "iscsi_target.h"
 #include "iscsi_target_parameters.h"
 
-extern struct idr sess_idr;
-extern struct mutex auth_id_lock;
-extern spinlock_t sess_idr_lock;
-
 static int iscsi_login_init_conn(struct iscsi_conn *conn)
 {
 	INIT_LIST_HEAD(&conn->conn_list);
@@ -196,10 +192,10 @@ int iscsi_check_for_session_reinstatement(struct iscsi_conn *conn)
 static void iscsi_login_set_conn_values(
 	struct iscsi_session *sess,
 	struct iscsi_conn *conn,
-	u16 cid)
+	__be16 cid)
 {
 	conn->sess		= sess;
-	conn->cid		= cid;
+	conn->cid		= be16_to_cpu(cid);
 	/*
 	 * Generate a random Status sequence number (statsn) for the new
 	 * iSCSI connection.
@@ -234,7 +230,7 @@ static int iscsi_login_zero_tsih_s1(
 	iscsi_login_set_conn_values(sess, conn, pdu->cid);
 	sess->init_task_tag	= pdu->itt;
 	memcpy(&sess->isid, pdu->isid, 6);
-	sess->exp_cmd_sn	= pdu->cmdsn;
+	sess->exp_cmd_sn	= be32_to_cpu(pdu->cmdsn);
 	INIT_LIST_HEAD(&sess->sess_conn_list);
 	INIT_LIST_HEAD(&sess->sess_ooo_cmdsn_list);
 	INIT_LIST_HEAD(&sess->cr_active_list);
@@ -275,7 +271,7 @@ static int iscsi_login_zero_tsih_s1(
 	 * The FFP CmdSN window values will be allocated from the TPG's
 	 * Initiator Node's ACL once the login has been successfully completed.
 	 */
-	sess->max_cmd_sn	= pdu->cmdsn;
+	sess->max_cmd_sn	= be32_to_cpu(pdu->cmdsn);
 
 	sess->sess_ops = kzalloc(sizeof(struct iscsi_sess_ops), GFP_KERNEL);
 	if (!sess->sess_ops) {
@@ -453,7 +449,7 @@ static int iscsi_login_non_zero_tsih_s2(
 		   (sess_p->time2retain_timer_flags & ISCSI_TF_EXPIRED))
 			continue;
 		if (!memcmp(sess_p->isid, pdu->isid, 6) &&
-		     (sess_p->tsih == pdu->tsih)) {
+		     (sess_p->tsih == be16_to_cpu(pdu->tsih))) {
 			iscsit_inc_session_usage_count(sess_p);
 			iscsit_stop_time2retain_timer(sess_p);
 			sess = sess_p;
@@ -955,11 +951,7 @@ static int __iscsi_target_login_thread(struct iscsi_np *np)
 	}
 
 	pdu			= (struct iscsi_login_req *) buffer;
-	pdu->cid		= be16_to_cpu(pdu->cid);
-	pdu->tsih		= be16_to_cpu(pdu->tsih);
-	pdu->itt		= be32_to_cpu(pdu->itt);
-	pdu->cmdsn		= be32_to_cpu(pdu->cmdsn);
-	pdu->exp_statsn		= be32_to_cpu(pdu->exp_statsn);
+
 	/*
 	 * Used by iscsit_tx_login_rsp() for Login Resonses PDUs
 	 * when Status-Class != 0.
diff --git a/drivers/target/iscsi/iscsi_target_nego.c b/drivers/target/iscsi/iscsi_target_nego.c
index 2dba448cac1..e9053a04f24 100644
--- a/drivers/target/iscsi/iscsi_target_nego.c
+++ b/drivers/target/iscsi/iscsi_target_nego.c
@@ -44,7 +44,7 @@ void convert_null_to_semi(char *buf, int len)
 			buf[i] = ';';
 }
 
-int strlen_semi(char *buf)
+static int strlen_semi(char *buf)
 {
 	int i = 0;
 
@@ -339,14 +339,14 @@ static int iscsi_target_do_tx_login_io(struct iscsi_conn *conn, struct iscsi_log
 	hton24(login_rsp->dlength, login->rsp_length);
 	memcpy(login_rsp->isid, login->isid, 6);
 	login_rsp->tsih			= cpu_to_be16(login->tsih);
-	login_rsp->itt			= cpu_to_be32(login->init_task_tag);
+	login_rsp->itt			= login->init_task_tag;
 	login_rsp->statsn		= cpu_to_be32(conn->stat_sn++);
 	login_rsp->exp_cmdsn		= cpu_to_be32(conn->sess->exp_cmd_sn);
 	login_rsp->max_cmdsn		= cpu_to_be32(conn->sess->max_cmd_sn);
 
 	pr_debug("Sending Login Response, Flags: 0x%02x, ITT: 0x%08x,"
 		" ExpCmdSN; 0x%08x, MaxCmdSN: 0x%08x, StatSN: 0x%08x, Length:"
-		" %u\n", login_rsp->flags, ntohl(login_rsp->itt),
+		" %u\n", login_rsp->flags, (__force u32)login_rsp->itt,
 		ntohl(login_rsp->exp_cmdsn), ntohl(login_rsp->max_cmdsn),
 		ntohl(login_rsp->statsn), login->rsp_length);
 
@@ -360,12 +360,9 @@ static int iscsi_target_do_tx_login_io(struct iscsi_conn *conn, struct iscsi_log
 		return -1;
 
 	login->rsp_length		= 0;
-	login_rsp->tsih			= be16_to_cpu(login_rsp->tsih);
-	login_rsp->itt			= be32_to_cpu(login_rsp->itt);
-	login_rsp->statsn		= be32_to_cpu(login_rsp->statsn);
 	mutex_lock(&sess->cmdsn_mutex);
-	login_rsp->exp_cmdsn		= be32_to_cpu(sess->exp_cmd_sn);
-	login_rsp->max_cmdsn		= be32_to_cpu(sess->max_cmd_sn);
+	login_rsp->exp_cmdsn		= cpu_to_be32(sess->exp_cmd_sn);
+	login_rsp->max_cmdsn		= cpu_to_be32(sess->max_cmd_sn);
 	mutex_unlock(&sess->cmdsn_mutex);
 
 	return 0;
@@ -381,11 +378,6 @@ static int iscsi_target_do_rx_login_io(struct iscsi_conn *conn, struct iscsi_log
 
 	login_req = (struct iscsi_login_req *) login->req;
 	payload_length			= ntoh24(login_req->dlength);
-	login_req->tsih			= be16_to_cpu(login_req->tsih);
-	login_req->itt			= be32_to_cpu(login_req->itt);
-	login_req->cid			= be16_to_cpu(login_req->cid);
-	login_req->cmdsn		= be32_to_cpu(login_req->cmdsn);
-	login_req->exp_statsn		= be32_to_cpu(login_req->exp_statsn);
 
 	pr_debug("Got Login Command, Flags 0x%02x, ITT: 0x%08x,"
 		" CmdSN: 0x%08x, ExpStatSN: 0x%08x, CID: %hu, Length: %u\n",
@@ -550,7 +542,7 @@ static int iscsi_target_handle_csg_zero(
 			SENDER_INITIATOR|SENDER_RECEIVER,
 			login->req_buf,
 			payload_length,
-			conn->param_list);
+			conn);
 	if (ret < 0)
 		return -1;
 
@@ -627,7 +619,7 @@ static int iscsi_target_handle_csg_one(struct iscsi_conn *conn, struct iscsi_log
 			SENDER_INITIATOR|SENDER_RECEIVER,
 			login->req_buf,
 			payload_length,
-			conn->param_list);
+			conn);
 	if (ret < 0)
 		return -1;
 
@@ -762,11 +754,11 @@ static int iscsi_target_locate_portal(
 	login->version_min	= login_req->min_version;
 	login->version_max	= login_req->max_version;
 	memcpy(login->isid, login_req->isid, 6);
-	login->cmd_sn		= login_req->cmdsn;
+	login->cmd_sn		= be32_to_cpu(login_req->cmdsn);
 	login->init_task_tag	= login_req->itt;
-	login->initial_exp_statsn = login_req->exp_statsn;
-	login->cid		= login_req->cid;
-	login->tsih		= login_req->tsih;
+	login->initial_exp_statsn = be32_to_cpu(login_req->exp_statsn);
+	login->cid		= be16_to_cpu(login_req->cid);
+	login->tsih		= be16_to_cpu(login_req->tsih);
 
 	if (iscsi_target_get_initial_payload(conn, login) < 0)
 		return -1;
@@ -1000,7 +992,6 @@ struct iscsi_login *iscsi_target_init_negotiation(
 	 *	Locates Target Portal from NP -> Target IQN
 	 */
 	if (iscsi_target_locate_portal(np, conn, login) < 0) {
-		pr_err("iSCSI Login negotiation failed.\n");
 		goto out;
 	}
 
diff --git a/drivers/target/iscsi/iscsi_target_parameters.c b/drivers/target/iscsi/iscsi_target_parameters.c
index 240f7aa76ed..90b740048f2 100644
--- a/drivers/target/iscsi/iscsi_target_parameters.c
+++ b/drivers/target/iscsi/iscsi_target_parameters.c
@@ -334,6 +334,13 @@ int iscsi_create_default_params(struct iscsi_param_list **param_list_ptr)
 	if (!param)
 		goto out;
 
+	param = iscsi_set_default_param(pl, MAXXMITDATASEGMENTLENGTH,
+			INITIAL_MAXXMITDATASEGMENTLENGTH,
+			PHASE_OPERATIONAL, SCOPE_CONNECTION_ONLY, SENDER_BOTH,
+			TYPERANGE_512_TO_16777215, USE_ALL);
+	if (!param)
+		goto out;
+
 	param = iscsi_set_default_param(pl, MAXRECVDATASEGMENTLENGTH,
 			INITIAL_MAXRECVDATASEGMENTLENGTH,
 			PHASE_OPERATIONAL, SCOPE_CONNECTION_ONLY, SENDER_BOTH,
@@ -467,6 +474,8 @@ int iscsi_set_keys_to_negotiate(
 			SET_PSTATE_NEGOTIATE(param);
 		} else if (!strcmp(param->name, MAXRECVDATASEGMENTLENGTH)) {
 			SET_PSTATE_NEGOTIATE(param);
+		} else if (!strcmp(param->name, MAXXMITDATASEGMENTLENGTH)) {
+			continue;
 		} else if (!strcmp(param->name, MAXBURSTLENGTH)) {
 			SET_PSTATE_NEGOTIATE(param);
 		} else if (!strcmp(param->name, FIRSTBURSTLENGTH)) {
@@ -1056,7 +1065,8 @@ out:
 	return proposer_values;
 }
 
-static int iscsi_check_acceptor_state(struct iscsi_param *param, char *value)
+static int iscsi_check_acceptor_state(struct iscsi_param *param, char *value,
+				struct iscsi_conn *conn)
 {
 	u8 acceptor_boolean_value = 0, proposer_boolean_value = 0;
 	char *negoitated_value = NULL;
@@ -1131,8 +1141,35 @@ static int iscsi_check_acceptor_state(struct iscsi_param *param, char *value)
 				return -1;
 		}
 
-		if (!strcmp(param->name, MAXRECVDATASEGMENTLENGTH))
-			SET_PSTATE_REPLY_OPTIONAL(param);
+		if (!strcmp(param->name, MAXRECVDATASEGMENTLENGTH)) {
+			struct iscsi_param *param_mxdsl;
+			unsigned long long tmp;
+			int rc;
+
+			rc = strict_strtoull(param->value, 0, &tmp);
+			if (rc < 0)
+				return -1;
+
+			conn->conn_ops->MaxRecvDataSegmentLength = tmp;
+			pr_debug("Saving op->MaxRecvDataSegmentLength from"
+				" original initiator received value: %u\n",
+				conn->conn_ops->MaxRecvDataSegmentLength);
+
+			param_mxdsl = iscsi_find_param_from_key(
+						MAXXMITDATASEGMENTLENGTH,
+						conn->param_list);
+			if (!param_mxdsl)
+				return -1;
+
+			rc = iscsi_update_param_value(param,
+						param_mxdsl->value);
+			if (rc < 0)
+				return -1;
+
+			pr_debug("Updated %s to target MXDSL value: %s\n",
+					param->name, param->value);
+		}
+
 	} else if (IS_TYPE_NUMBER_RANGE(param)) {
 		negoitated_value = iscsi_get_value_from_number_range(
 					param, value);
@@ -1526,8 +1563,9 @@ int iscsi_decode_text_input(
 	u8 sender,
 	char *textbuf,
 	u32 length,
-	struct iscsi_param_list *param_list)
+	struct iscsi_conn *conn)
 {
+	struct iscsi_param_list *param_list = conn->param_list;
 	char *tmpbuf, *start = NULL, *end = NULL;
 
 	tmpbuf = kzalloc(length + 1, GFP_KERNEL);
@@ -1585,7 +1623,7 @@ int iscsi_decode_text_input(
 			}
 			SET_PSTATE_RESPONSE_GOT(param);
 		} else {
-			if (iscsi_check_acceptor_state(param, value) < 0) {
+			if (iscsi_check_acceptor_state(param, value, conn) < 0) {
 				kfree(tmpbuf);
 				return -1;
 			}
@@ -1720,6 +1758,18 @@ void iscsi_set_connection_parameters(
 	pr_debug("---------------------------------------------------"
 			"---------------\n");
 	list_for_each_entry(param, &param_list->param_list, p_list) {
+		/*
+		 * Special case to set MAXXMITDATASEGMENTLENGTH from the
+		 * target requested MaxRecvDataSegmentLength, even though
+		 * this key is not sent over the wire.
+		 */
+		if (!strcmp(param->name, MAXXMITDATASEGMENTLENGTH)) {
+			ops->MaxXmitDataSegmentLength =
+				simple_strtoul(param->value, &tmpptr, 0);
+			pr_debug("MaxXmitDataSegmentLength:     %s\n",
+				param->value);
+		}
+
 		if (!IS_PSTATE_ACCEPTOR(param) && !IS_PSTATE_PROPOSER(param))
 			continue;
 		if (!strcmp(param->name, AUTHMETHOD)) {
@@ -1734,10 +1784,13 @@ void iscsi_set_connection_parameters(
 			pr_debug("DataDigest:                   %s\n",
 				param->value);
 		} else if (!strcmp(param->name, MAXRECVDATASEGMENTLENGTH)) {
-			ops->MaxRecvDataSegmentLength =
-				simple_strtoul(param->value, &tmpptr, 0);
-			pr_debug("MaxRecvDataSegmentLength:     %s\n",
-				param->value);
+			/*
+			 * At this point iscsi_check_acceptor_state() will have
+			 * set ops->MaxRecvDataSegmentLength from the original
+			 * initiator provided value.
+			 */
+			pr_debug("MaxRecvDataSegmentLength:     %u\n",
+				ops->MaxRecvDataSegmentLength);
 		} else if (!strcmp(param->name, OFMARKER)) {
 			ops->OFMarker = !strcmp(param->value, YES);
 			pr_debug("OFMarker:                     %s\n",
diff --git a/drivers/target/iscsi/iscsi_target_parameters.h b/drivers/target/iscsi/iscsi_target_parameters.h
index 6a37fd6f128..1e1b7504a76 100644
--- a/drivers/target/iscsi/iscsi_target_parameters.h
+++ b/drivers/target/iscsi/iscsi_target_parameters.h
@@ -36,7 +36,7 @@ extern void iscsi_release_param_list(struct iscsi_param_list *);
 extern struct iscsi_param *iscsi_find_param_from_key(char *, struct iscsi_param_list *);
 extern int iscsi_extract_key_value(char *, char **, char **);
 extern int iscsi_update_param_value(struct iscsi_param *, char *);
-extern int iscsi_decode_text_input(u8, u8, char *, u32, struct iscsi_param_list *);
+extern int iscsi_decode_text_input(u8, u8, char *, u32, struct iscsi_conn *);
 extern int iscsi_encode_text_output(u8, u8, char *, u32 *,
 			struct iscsi_param_list *);
 extern int iscsi_check_negotiated_keys(struct iscsi_param_list *);
@@ -70,6 +70,7 @@ extern void iscsi_set_session_parameters(struct iscsi_sess_ops *,
 #define INITIALR2T			"InitialR2T"
 #define IMMEDIATEDATA			"ImmediateData"
 #define MAXRECVDATASEGMENTLENGTH	"MaxRecvDataSegmentLength"
+#define MAXXMITDATASEGMENTLENGTH	"MaxXmitDataSegmentLength"
 #define MAXBURSTLENGTH			"MaxBurstLength"
 #define FIRSTBURSTLENGTH		"FirstBurstLength"
 #define DEFAULTTIME2WAIT		"DefaultTime2Wait"
@@ -113,6 +114,10 @@ extern void iscsi_set_session_parameters(struct iscsi_sess_ops *,
 #define INITIAL_INITIALR2T			YES
 #define INITIAL_IMMEDIATEDATA			YES
 #define INITIAL_MAXRECVDATASEGMENTLENGTH	"8192"
+/*
+ * Match outgoing MXDSL default to incoming Open-iSCSI default
+ */
+#define INITIAL_MAXXMITDATASEGMENTLENGTH	"262144"
 #define INITIAL_MAXBURSTLENGTH			"262144"
 #define INITIAL_FIRSTBURSTLENGTH		"65536"
 #define INITIAL_DEFAULTTIME2WAIT		"2"
diff --git a/drivers/target/iscsi/iscsi_target_seq_pdu_list.c b/drivers/target/iscsi/iscsi_target_seq_pdu_list.c
index 85a306e067b..edb592a368e 100644
--- a/drivers/target/iscsi/iscsi_target_seq_pdu_list.c
+++ b/drivers/target/iscsi/iscsi_target_seq_pdu_list.c
@@ -219,8 +219,14 @@ static void iscsit_determine_counts_for_list(
 	int check_immediate = 0;
 	u32 burstlength = 0, offset = 0;
 	u32 unsolicited_data_length = 0;
+	u32 mdsl;
 	struct iscsi_conn *conn = cmd->conn;
 
+	if (cmd->se_cmd.data_direction == DMA_TO_DEVICE)
+		mdsl = cmd->conn->conn_ops->MaxXmitDataSegmentLength;
+	else
+		mdsl = cmd->conn->conn_ops->MaxRecvDataSegmentLength;
+
 	if ((bl->type == PDULIST_IMMEDIATE) ||
 	    (bl->type == PDULIST_IMMEDIATE_AND_UNSOLICITED))
 		check_immediate = 1;
@@ -243,14 +249,13 @@ static void iscsit_determine_counts_for_list(
 			continue;
 		}
 		if (unsolicited_data_length > 0) {
-			if ((offset + conn->conn_ops->MaxRecvDataSegmentLength)
-					>= cmd->se_cmd.data_length) {
+			if ((offset + mdsl) >= cmd->se_cmd.data_length) {
 				unsolicited_data_length -=
 					(cmd->se_cmd.data_length - offset);
 				offset += (cmd->se_cmd.data_length - offset);
 				continue;
 			}
-			if ((offset + conn->conn_ops->MaxRecvDataSegmentLength)
+			if ((offset + mdsl)
 					>= conn->sess->sess_ops->FirstBurstLength) {
 				unsolicited_data_length -=
 					(conn->sess->sess_ops->FirstBurstLength -
@@ -262,17 +267,15 @@ static void iscsit_determine_counts_for_list(
 				continue;
 			}
 
-			offset += conn->conn_ops->MaxRecvDataSegmentLength;
-			unsolicited_data_length -=
-				conn->conn_ops->MaxRecvDataSegmentLength;
+			offset += mdsl;
+			unsolicited_data_length -= mdsl;
 			continue;
 		}
-		if ((offset + conn->conn_ops->MaxRecvDataSegmentLength) >=
-		     cmd->se_cmd.data_length) {
+		if ((offset + mdsl) >= cmd->se_cmd.data_length) {
 			offset += (cmd->se_cmd.data_length - offset);
 			continue;
 		}
-		if ((burstlength + conn->conn_ops->MaxRecvDataSegmentLength) >=
+		if ((burstlength + mdsl) >=
 		     conn->sess->sess_ops->MaxBurstLength) {
 			offset += (conn->sess->sess_ops->MaxBurstLength -
 					burstlength);
@@ -281,8 +284,8 @@ static void iscsit_determine_counts_for_list(
 			continue;
 		}
 
-		burstlength += conn->conn_ops->MaxRecvDataSegmentLength;
-		offset += conn->conn_ops->MaxRecvDataSegmentLength;
+		burstlength += mdsl;
+		offset += mdsl;
 	}
 }
 
@@ -296,12 +299,17 @@ static int iscsit_do_build_pdu_and_seq_lists(
 	struct iscsi_build_list *bl)
 {
 	int check_immediate = 0, datapduinorder, datasequenceinorder;
-	u32 burstlength = 0, offset = 0, i = 0;
+	u32 burstlength = 0, offset = 0, i = 0, mdsl;
 	u32 pdu_count = 0, seq_no = 0, unsolicited_data_length = 0;
 	struct iscsi_conn *conn = cmd->conn;
 	struct iscsi_pdu *pdu = cmd->pdu_list;
 	struct iscsi_seq *seq = cmd->seq_list;
 
+	if (cmd->se_cmd.data_direction == DMA_TO_DEVICE)
+		mdsl = cmd->conn->conn_ops->MaxXmitDataSegmentLength;
+	else
+		mdsl = cmd->conn->conn_ops->MaxRecvDataSegmentLength;
+
 	datapduinorder = conn->sess->sess_ops->DataPDUInOrder;
 	datasequenceinorder = conn->sess->sess_ops->DataSequenceInOrder;
 
@@ -348,9 +356,7 @@ static int iscsit_do_build_pdu_and_seq_lists(
 			continue;
 		}
 		if (unsolicited_data_length > 0) {
-			if ((offset +
-			     conn->conn_ops->MaxRecvDataSegmentLength) >=
-			     cmd->se_cmd.data_length) {
+			if ((offset + mdsl) >= cmd->se_cmd.data_length) {
 				if (!datapduinorder) {
 					pdu[i].type = PDUTYPE_UNSOLICITED;
 					pdu[i].length =
@@ -367,8 +373,7 @@ static int iscsit_do_build_pdu_and_seq_lists(
 				offset += (cmd->se_cmd.data_length - offset);
 				continue;
 			}
-			if ((offset +
-			     conn->conn_ops->MaxRecvDataSegmentLength) >=
+			if ((offset + mdsl) >=
 					conn->sess->sess_ops->FirstBurstLength) {
 				if (!datapduinorder) {
 					pdu[i].type = PDUTYPE_UNSOLICITED;
@@ -396,17 +401,14 @@ static int iscsit_do_build_pdu_and_seq_lists(
 
 			if (!datapduinorder) {
 				pdu[i].type = PDUTYPE_UNSOLICITED;
-				pdu[i++].length =
-				     conn->conn_ops->MaxRecvDataSegmentLength;
+				pdu[i++].length = mdsl;
 			}
-			burstlength += conn->conn_ops->MaxRecvDataSegmentLength;
-			offset += conn->conn_ops->MaxRecvDataSegmentLength;
-			unsolicited_data_length -=
-				conn->conn_ops->MaxRecvDataSegmentLength;
+			burstlength += mdsl;
+			offset += mdsl;
+			unsolicited_data_length -= mdsl;
 			continue;
 		}
-		if ((offset + conn->conn_ops->MaxRecvDataSegmentLength) >=
-		     cmd->se_cmd.data_length) {
+		if ((offset + mdsl) >= cmd->se_cmd.data_length) {
 			if (!datapduinorder) {
 				pdu[i].type = PDUTYPE_NORMAL;
 				pdu[i].length = (cmd->se_cmd.data_length - offset);
@@ -420,7 +422,7 @@ static int iscsit_do_build_pdu_and_seq_lists(
 			offset += (cmd->se_cmd.data_length - offset);
 			continue;
 		}
-		if ((burstlength + conn->conn_ops->MaxRecvDataSegmentLength) >=
+		if ((burstlength + mdsl) >=
 		     conn->sess->sess_ops->MaxBurstLength) {
 			if (!datapduinorder) {
 				pdu[i].type = PDUTYPE_NORMAL;
@@ -445,11 +447,10 @@ static int iscsit_do_build_pdu_and_seq_lists(
 
 		if (!datapduinorder) {
 			pdu[i].type = PDUTYPE_NORMAL;
-			pdu[i++].length =
-				conn->conn_ops->MaxRecvDataSegmentLength;
+			pdu[i++].length = mdsl;
 		}
-		burstlength += conn->conn_ops->MaxRecvDataSegmentLength;
-		offset += conn->conn_ops->MaxRecvDataSegmentLength;
+		burstlength += mdsl;
+		offset += mdsl;
 	}
 
 	if (!datasequenceinorder) {
diff --git a/drivers/target/iscsi/iscsi_target_tmr.c b/drivers/target/iscsi/iscsi_target_tmr.c
index f62fe123d90..4a99820d063 100644
--- a/drivers/target/iscsi/iscsi_target_tmr.c
+++ b/drivers/target/iscsi/iscsi_target_tmr.c
@@ -50,21 +50,20 @@ u8 iscsit_tmr_abort_task(
 	if (!ref_cmd) {
 		pr_err("Unable to locate RefTaskTag: 0x%08x on CID:"
 			" %hu.\n", hdr->rtt, conn->cid);
-		return ((hdr->refcmdsn >= conn->sess->exp_cmd_sn) &&
-			(hdr->refcmdsn <= conn->sess->max_cmd_sn)) ?
+		return (be32_to_cpu(hdr->refcmdsn) >= conn->sess->exp_cmd_sn &&
+			be32_to_cpu(hdr->refcmdsn) <= conn->sess->max_cmd_sn) ?
 			ISCSI_TMF_RSP_COMPLETE : ISCSI_TMF_RSP_NO_TASK;
 	}
-	if (ref_cmd->cmd_sn != hdr->refcmdsn) {
+	if (ref_cmd->cmd_sn != be32_to_cpu(hdr->refcmdsn)) {
 		pr_err("RefCmdSN 0x%08x does not equal"
 			" task's CmdSN 0x%08x. Rejecting ABORT_TASK.\n",
 			hdr->refcmdsn, ref_cmd->cmd_sn);
 		return ISCSI_TMF_RSP_REJECTED;
 	}
 
-	se_tmr->ref_task_tag		= hdr->rtt;
+	se_tmr->ref_task_tag		= (__force u32)hdr->rtt;
 	tmr_req->ref_cmd		= ref_cmd;
-	tmr_req->ref_cmd_sn		= hdr->refcmdsn;
-	tmr_req->exp_data_sn		= hdr->exp_datasn;
+	tmr_req->exp_data_sn		= be32_to_cpu(hdr->exp_datasn);
 
 	return ISCSI_TMF_RSP_COMPLETE;
 }
@@ -146,7 +145,7 @@ u8 iscsit_tmr_task_reassign(
 	}
 	/*
 	 * Temporary check to prevent connection recovery for
-	 * connections with a differing MaxRecvDataSegmentLength.
+	 * connections with a differing Max*DataSegmentLength.
 	 */
 	if (cr->maxrecvdatasegmentlength !=
 	    conn->conn_ops->MaxRecvDataSegmentLength) {
@@ -155,6 +154,13 @@ u8 iscsit_tmr_task_reassign(
 			" TMR TASK_REASSIGN.\n");
 		return ISCSI_TMF_RSP_REJECTED;
 	}
+	if (cr->maxxmitdatasegmentlength !=
+	    conn->conn_ops->MaxXmitDataSegmentLength) {
+		pr_err("Unable to perform connection recovery for"
+			" differing MaxXmitDataSegmentLength, rejecting"
+			" TMR TASK_REASSIGN.\n");
+		return ISCSI_TMF_RSP_REJECTED;
+	}
 
 	ref_lun = scsilun_to_int(&hdr->lun);
 	if (ref_lun != ref_cmd->se_cmd.orig_fe_lun) {
@@ -164,10 +170,9 @@ u8 iscsit_tmr_task_reassign(
 		return ISCSI_TMF_RSP_REJECTED;
 	}
 
-	se_tmr->ref_task_tag		= hdr->rtt;
+	se_tmr->ref_task_tag		= (__force u32)hdr->rtt;
 	tmr_req->ref_cmd		= ref_cmd;
-	tmr_req->ref_cmd_sn		= hdr->refcmdsn;
-	tmr_req->exp_data_sn		= hdr->exp_datasn;
+	tmr_req->exp_data_sn		= be32_to_cpu(hdr->exp_datasn);
 	tmr_req->conn_recovery		= cr;
 	tmr_req->task_reassign		= 1;
 	/*
@@ -455,7 +460,7 @@ static int iscsit_task_reassign_complete(
  *	Right now the only one that its really needed for is
  *	connection recovery releated TASK_REASSIGN.
  */
-extern int iscsit_tmr_post_handler(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
+int iscsit_tmr_post_handler(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
 {
 	struct iscsi_tmr_req *tmr_req = cmd->tmr_req;
 	struct se_tmr_req *se_tmr = cmd->se_cmd.se_tmr_req;
@@ -470,7 +475,7 @@ extern int iscsit_tmr_post_handler(struct iscsi_cmd *cmd, struct iscsi_conn *con
 /*
  *	Nothing to do here, but leave it for good measure. :-)
  */
-int iscsit_task_reassign_prepare_read(
+static int iscsit_task_reassign_prepare_read(
 	struct iscsi_tmr_req *tmr_req,
 	struct iscsi_conn *conn)
 {
@@ -545,7 +550,7 @@ static void iscsit_task_reassign_prepare_unsolicited_dataout(
 	}
 }
 
-int iscsit_task_reassign_prepare_write(
+static int iscsit_task_reassign_prepare_write(
 	struct iscsi_tmr_req *tmr_req,
 	struct iscsi_conn *conn)
 {
diff --git a/drivers/target/iscsi/iscsi_target_tpg.c b/drivers/target/iscsi/iscsi_target_tpg.c
index a38a3f8ab0d..de9ea32b610 100644
--- a/drivers/target/iscsi/iscsi_target_tpg.c
+++ b/drivers/target/iscsi/iscsi_target_tpg.c
@@ -677,6 +677,12 @@ int iscsit_ta_generate_node_acls(
 	pr_debug("iSCSI_TPG[%hu] - Generate Initiator Portal Group ACLs: %s\n",
 		tpg->tpgt, (a->generate_node_acls) ? "Enabled" : "Disabled");
 
+	if (flag == 1 && a->cache_dynamic_acls == 0) {
+		pr_debug("Explicitly setting cache_dynamic_acls=1 when "
+			"generate_node_acls=1\n");
+		a->cache_dynamic_acls = 1;
+	}
+
 	return 0;
 }
 
@@ -716,6 +722,12 @@ int iscsit_ta_cache_dynamic_acls(
 		return -EINVAL;
 	}
 
+	if (a->generate_node_acls == 1 && flag == 0) {
+		pr_debug("Skipping cache_dynamic_acls=0 when"
+			" generate_node_acls=1\n");
+		return 0;
+	}
+
 	a->cache_dynamic_acls = flag;
 	pr_debug("iSCSI_TPG[%hu] - Cache Dynamic Initiator Portal Group"
 		" ACLs %s\n", tpg->tpgt, (a->cache_dynamic_acls) ?
diff --git a/drivers/target/iscsi/iscsi_target_tq.c b/drivers/target/iscsi/iscsi_target_tq.c
index 977e1cf90e8..9d881a000e4 100644
--- a/drivers/target/iscsi/iscsi_target_tq.c
+++ b/drivers/target/iscsi/iscsi_target_tq.c
@@ -40,7 +40,7 @@ static void iscsi_add_ts_to_active_list(struct iscsi_thread_set *ts)
 	spin_unlock(&active_ts_lock);
 }
 
-extern void iscsi_add_ts_to_inactive_list(struct iscsi_thread_set *ts)
+static void iscsi_add_ts_to_inactive_list(struct iscsi_thread_set *ts)
 {
 	spin_lock(&inactive_ts_lock);
 	list_add_tail(&ts->ts_list, &inactive_ts_list);
@@ -76,7 +76,7 @@ static struct iscsi_thread_set *iscsi_get_ts_from_inactive_list(void)
 	return ts;
 }
 
-extern int iscsi_allocate_thread_sets(u32 thread_pair_count)
+int iscsi_allocate_thread_sets(u32 thread_pair_count)
 {
 	int allocated_thread_pair_count = 0, i, thread_id;
 	struct iscsi_thread_set *ts = NULL;
@@ -140,7 +140,7 @@ extern int iscsi_allocate_thread_sets(u32 thread_pair_count)
 	return allocated_thread_pair_count;
 }
 
-extern void iscsi_deallocate_thread_sets(void)
+void iscsi_deallocate_thread_sets(void)
 {
 	u32 released_count = 0;
 	struct iscsi_thread_set *ts = NULL;
diff --git a/drivers/target/iscsi/iscsi_target_tq.h b/drivers/target/iscsi/iscsi_target_tq.h
index 26e6a95ec20..547d1183128 100644
--- a/drivers/target/iscsi/iscsi_target_tq.h
+++ b/drivers/target/iscsi/iscsi_target_tq.h
@@ -5,7 +5,6 @@
  * Defines for thread sets.
  */
 extern int iscsi_thread_set_force_reinstatement(struct iscsi_conn *);
-extern void iscsi_add_ts_to_inactive_list(struct iscsi_thread_set *);
 extern int iscsi_allocate_thread_sets(u32);
 extern void iscsi_deallocate_thread_sets(void);
 extern void iscsi_activate_thread_set(struct iscsi_conn *, struct iscsi_thread_set *);
diff --git a/drivers/target/iscsi/iscsi_target_util.c b/drivers/target/iscsi/iscsi_target_util.c
index b42cdeb153d..afd98ccd40a 100644
--- a/drivers/target/iscsi/iscsi_target_util.c
+++ b/drivers/target/iscsi/iscsi_target_util.c
@@ -274,14 +274,14 @@ static inline int iscsit_check_received_cmdsn(struct iscsi_session *sess, u32 cm
 int iscsit_sequence_cmd(
 	struct iscsi_conn *conn,
 	struct iscsi_cmd *cmd,
-	u32 cmdsn)
+	__be32 cmdsn)
 {
 	int ret;
 	int cmdsn_ret;
 
 	mutex_lock(&conn->sess->cmdsn_mutex);
 
-	cmdsn_ret = iscsit_check_received_cmdsn(conn->sess, cmdsn);
+	cmdsn_ret = iscsit_check_received_cmdsn(conn->sess, be32_to_cpu(cmdsn));
 	switch (cmdsn_ret) {
 	case CMDSN_NORMAL_OPERATION:
 		ret = iscsit_execute_cmd(cmd, 0);
@@ -289,7 +289,7 @@ int iscsit_sequence_cmd(
 			iscsit_execute_ooo_cmdsns(conn->sess);
 		break;
 	case CMDSN_HIGHER_THAN_EXP:
-		ret = iscsit_handle_ooo_cmdsn(conn->sess, cmd, cmdsn);
+		ret = iscsit_handle_ooo_cmdsn(conn->sess, cmd, be32_to_cpu(cmdsn));
 		break;
 	case CMDSN_LOWER_THAN_EXP:
 		cmd->i_state = ISTATE_REMOVE;
@@ -351,7 +351,7 @@ int iscsit_check_unsolicited_dataout(struct iscsi_cmd *cmd, unsigned char *buf)
 
 struct iscsi_cmd *iscsit_find_cmd_from_itt(
 	struct iscsi_conn *conn,
-	u32 init_task_tag)
+	itt_t init_task_tag)
 {
 	struct iscsi_cmd *cmd;
 
@@ -371,7 +371,7 @@ struct iscsi_cmd *iscsit_find_cmd_from_itt(
 
 struct iscsi_cmd *iscsit_find_cmd_from_itt_or_dump(
 	struct iscsi_conn *conn,
-	u32 init_task_tag,
+	itt_t init_task_tag,
 	u32 length)
 {
 	struct iscsi_cmd *cmd;
@@ -417,7 +417,7 @@ int iscsit_find_cmd_for_recovery(
 	struct iscsi_session *sess,
 	struct iscsi_cmd **cmd_ptr,
 	struct iscsi_conn_recovery **cr_ptr,
-	u32 init_task_tag)
+	itt_t init_task_tag)
 {
 	struct iscsi_cmd *cmd = NULL;
 	struct iscsi_conn_recovery *cr;
@@ -855,7 +855,7 @@ static int iscsit_add_nopin(struct iscsi_conn *conn, int want_response)
 	cmd->iscsi_opcode = ISCSI_OP_NOOP_IN;
 	state = (want_response) ? ISTATE_SEND_NOPIN_WANT_RESPONSE :
 				ISTATE_SEND_NOPIN_NO_RESPONSE;
-	cmd->init_task_tag = 0xFFFFFFFF;
+	cmd->init_task_tag = RESERVED_ITT;
 	spin_lock_bh(&conn->sess->ttt_lock);
 	cmd->targ_xfer_tag = (want_response) ? conn->sess->targ_xfer_tag++ :
 			0xFFFFFFFF;
@@ -1222,7 +1222,7 @@ int iscsit_tx_login_rsp(struct iscsi_conn *conn, u8 status_class, u8 status_deta
 	hdr->opcode		= ISCSI_OP_LOGIN_RSP;
 	hdr->status_class	= status_class;
 	hdr->status_detail	= status_detail;
-	hdr->itt		= cpu_to_be32(conn->login_itt);
+	hdr->itt		= conn->login_itt;
 
 	iov.iov_base		= &iscsi_hdr;
 	iov.iov_len		= ISCSI_HDR_LEN;
diff --git a/drivers/target/iscsi/iscsi_target_util.h b/drivers/target/iscsi/iscsi_target_util.h
index e1c729b8a1c..44054bd3543 100644
--- a/drivers/target/iscsi/iscsi_target_util.h
+++ b/drivers/target/iscsi/iscsi_target_util.h
@@ -12,14 +12,14 @@ extern struct iscsi_cmd *iscsit_allocate_cmd(struct iscsi_conn *, gfp_t);
 extern struct iscsi_seq *iscsit_get_seq_holder_for_datain(struct iscsi_cmd *, u32);
 extern struct iscsi_seq *iscsit_get_seq_holder_for_r2t(struct iscsi_cmd *);
 extern struct iscsi_r2t *iscsit_get_holder_for_r2tsn(struct iscsi_cmd *, u32);
-int iscsit_sequence_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd, u32 cmdsn);
+int iscsit_sequence_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd, __be32 cmdsn);
 extern int iscsit_check_unsolicited_dataout(struct iscsi_cmd *, unsigned char *);
-extern struct iscsi_cmd *iscsit_find_cmd_from_itt(struct iscsi_conn *, u32);
+extern struct iscsi_cmd *iscsit_find_cmd_from_itt(struct iscsi_conn *, itt_t);
 extern struct iscsi_cmd *iscsit_find_cmd_from_itt_or_dump(struct iscsi_conn *,
-			u32, u32);
+			itt_t, u32);
 extern struct iscsi_cmd *iscsit_find_cmd_from_ttt(struct iscsi_conn *, u32);
 extern int iscsit_find_cmd_for_recovery(struct iscsi_session *, struct iscsi_cmd **,
-			struct iscsi_conn_recovery **, u32);
+			struct iscsi_conn_recovery **, itt_t);
 extern void iscsit_add_cmd_to_immediate_queue(struct iscsi_cmd *, struct iscsi_conn *, u8);
 extern struct iscsi_queue_req *iscsit_get_cmd_from_immediate_queue(struct iscsi_conn *);
 extern void iscsit_add_cmd_to_response_queue(struct iscsi_cmd *, struct iscsi_conn *, u8);
diff --git a/drivers/target/loopback/tcm_loop.c b/drivers/target/loopback/tcm_loop.c
index 5491c632a15..2d444b1ccd3 100644
--- a/drivers/target/loopback/tcm_loop.c
+++ b/drivers/target/loopback/tcm_loop.c
@@ -166,7 +166,7 @@ static void tcm_loop_submission_work(struct work_struct *work)
 	struct tcm_loop_tpg *tl_tpg;
 	struct scatterlist *sgl_bidi = NULL;
 	u32 sgl_bidi_count = 0;
-	int ret;
+	int rc;
 
 	tl_hba = *(struct tcm_loop_hba **)shost_priv(sc->device->host);
 	tl_tpg = &tl_hba->tl_hba_tpgs[sc->device->id];
@@ -187,12 +187,6 @@ static void tcm_loop_submission_work(struct work_struct *work)
 		set_host_byte(sc, DID_ERROR);
 		goto out_done;
 	}
-
-	transport_init_se_cmd(se_cmd, tl_tpg->tl_se_tpg.se_tpg_tfo,
-			tl_nexus->se_sess,
-			scsi_bufflen(sc), sc->sc_data_direction,
-			tcm_loop_sam_attr(sc), &tl_cmd->tl_sense_buf[0]);
-
 	if (scsi_bidi_cmnd(sc)) {
 		struct scsi_data_buffer *sdb = scsi_in(sc);
 
@@ -201,56 +195,16 @@ static void tcm_loop_submission_work(struct work_struct *work)
 		se_cmd->se_cmd_flags |= SCF_BIDI;
 
 	}
-
-	if (transport_lookup_cmd_lun(se_cmd, tl_cmd->sc->device->lun) < 0) {
-		kmem_cache_free(tcm_loop_cmd_cache, tl_cmd);
+	rc = target_submit_cmd_map_sgls(se_cmd, tl_nexus->se_sess, sc->cmnd,
+			&tl_cmd->tl_sense_buf[0], tl_cmd->sc->device->lun,
+			scsi_bufflen(sc), tcm_loop_sam_attr(sc),
+			sc->sc_data_direction, 0,
+			scsi_sglist(sc), scsi_sg_count(sc),
+			sgl_bidi, sgl_bidi_count);
+	if (rc < 0) {
 		set_host_byte(sc, DID_NO_CONNECT);
 		goto out_done;
 	}
-
-	/*
-	 * Because some userspace code via scsi-generic do not memset their
-	 * associated read buffers, go ahead and do that here for type
-	 * non-data CDBs.  Also note that this is currently guaranteed to be a
-	 * single SGL for this case by target core in
-	 * target_setup_cmd_from_cdb() -> transport_generic_cmd_sequencer().
-	 */
-	if (!(se_cmd->se_cmd_flags & SCF_SCSI_DATA_CDB) &&
-	    se_cmd->data_direction == DMA_FROM_DEVICE) {
-		struct scatterlist *sg = scsi_sglist(sc);
-		unsigned char *buf = kmap(sg_page(sg)) + sg->offset;
-
-		if (buf != NULL) {
-			memset(buf, 0, sg->length);
-			kunmap(sg_page(sg));
-		}
-	}
-
-	ret = target_setup_cmd_from_cdb(se_cmd, sc->cmnd);
-	if (ret == -ENOMEM) {
-		transport_send_check_condition_and_sense(se_cmd,
-				TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE, 0);
-		transport_generic_free_cmd(se_cmd, 0);
-		return;
-	} else if (ret < 0) {
-		if (se_cmd->se_cmd_flags & SCF_SCSI_RESERVATION_CONFLICT)
-			tcm_loop_queue_status(se_cmd);
-		else
-			transport_send_check_condition_and_sense(se_cmd,
-					se_cmd->scsi_sense_reason, 0);
-		transport_generic_free_cmd(se_cmd, 0);
-		return;
-	}
-
-	ret = transport_generic_map_mem_to_cmd(se_cmd, scsi_sglist(sc),
-			scsi_sg_count(sc), sgl_bidi, sgl_bidi_count);
-	if (ret) {
-		transport_send_check_condition_and_sense(se_cmd,
-					se_cmd->scsi_sense_reason, 0);
-		transport_generic_free_cmd(se_cmd, 0);
-		return;
-	}
-	transport_handle_cdb_direct(se_cmd);
 	return;
 
 out_done:
@@ -846,16 +800,6 @@ static int tcm_loop_queue_tm_rsp(struct se_cmd *se_cmd)
 	return 0;
 }
 
-static u16 tcm_loop_set_fabric_sense_len(struct se_cmd *se_cmd, u32 sense_length)
-{
-	return 0;
-}
-
-static u16 tcm_loop_get_fabric_sense_len(void)
-{
-	return 0;
-}
-
 static char *tcm_loop_dump_proto_id(struct tcm_loop_hba *tl_hba)
 {
 	switch (tl_hba->tl_proto_id) {
@@ -1373,8 +1317,6 @@ static int tcm_loop_register_configfs(void)
 	fabric->tf_ops.queue_data_in = &tcm_loop_queue_data_in;
 	fabric->tf_ops.queue_status = &tcm_loop_queue_status;
 	fabric->tf_ops.queue_tm_rsp = &tcm_loop_queue_tm_rsp;
-	fabric->tf_ops.set_fabric_sense_len = &tcm_loop_set_fabric_sense_len;
-	fabric->tf_ops.get_fabric_sense_len = &tcm_loop_get_fabric_sense_len;
 
 	/*
 	 * Setup function pointers for generic logic in target_core_fabric_configfs.c
diff --git a/drivers/target/sbp/sbp_target.c b/drivers/target/sbp/sbp_target.c
index 39ddba584b3..0d6d7c1f025 100644
--- a/drivers/target/sbp/sbp_target.c
+++ b/drivers/target/sbp/sbp_target.c
@@ -660,8 +660,7 @@ static void session_reconnect_expired(struct sbp_session *sess)
 	spin_lock_bh(&sess->lock);
 	list_for_each_entry_safe(login, temp, &sess->login_list, link) {
 		login->sess = NULL;
-		list_del(&login->link);
-		list_add_tail(&login->link, &login_list);
+		list_move_tail(&login->link, &login_list);
 	}
 	spin_unlock_bh(&sess->lock);
 
@@ -1847,16 +1846,6 @@ static int sbp_queue_tm_rsp(struct se_cmd *se_cmd)
 	return 0;
 }
 
-static u16 sbp_set_fabric_sense_len(struct se_cmd *se_cmd, u32 sense_length)
-{
-	return 0;
-}
-
-static u16 sbp_get_fabric_sense_len(void)
-{
-	return 0;
-}
-
 static int sbp_check_stop_free(struct se_cmd *se_cmd)
 {
 	struct sbp_target_request *req = container_of(se_cmd,
@@ -2068,7 +2057,7 @@ static int sbp_update_unit_directory(struct sbp_tport *tport)
 	return ret;
 }
 
-static ssize_t sbp_parse_wwn(const char *name, u64 *wwn, int strict)
+static ssize_t sbp_parse_wwn(const char *name, u64 *wwn)
 {
 	const char *cp;
 	char c, nibble;
@@ -2088,7 +2077,7 @@ static ssize_t sbp_parse_wwn(const char *name, u64 *wwn, int strict)
 		err = 3;
 		if (isdigit(c))
 			nibble = c - '0';
-		else if (isxdigit(c) && (islower(c) || !strict))
+		else if (isxdigit(c))
 			nibble = tolower(c) - 'a' + 10;
 		else
 			goto fail;
@@ -2117,7 +2106,7 @@ static struct se_node_acl *sbp_make_nodeacl(
 	u64 guid = 0;
 	u32 nexus_depth = 1;
 
-	if (sbp_parse_wwn(name, &guid, 1) < 0)
+	if (sbp_parse_wwn(name, &guid) < 0)
 		return ERR_PTR(-EINVAL);
 
 	se_nacl_new = sbp_alloc_fabric_acl(se_tpg);
@@ -2253,7 +2242,7 @@ static struct se_wwn *sbp_make_tport(
 	struct sbp_tport *tport;
 	u64 guid = 0;
 
-	if (sbp_parse_wwn(name, &guid, 1) < 0)
+	if (sbp_parse_wwn(name, &guid) < 0)
 		return ERR_PTR(-EINVAL);
 
 	tport = kzalloc(sizeof(*tport), GFP_KERNEL);
@@ -2534,8 +2523,6 @@ static struct target_core_fabric_ops sbp_ops = {
 	.queue_data_in			= sbp_queue_data_in,
 	.queue_status			= sbp_queue_status,
 	.queue_tm_rsp			= sbp_queue_tm_rsp,
-	.get_fabric_sense_len		= sbp_get_fabric_sense_len,
-	.set_fabric_sense_len		= sbp_set_fabric_sense_len,
 	.check_stop_free		= sbp_check_stop_free,
 
 	.fabric_make_wwn		= sbp_make_tport,
@@ -2556,9 +2543,9 @@ static int sbp_register_configfs(void)
 	int ret;
 
 	fabric = target_fabric_configfs_init(THIS_MODULE, "sbp");
-	if (!fabric) {
+	if (IS_ERR(fabric)) {
 		pr_err("target_fabric_configfs_init() failed\n");
-		return -ENOMEM;
+		return PTR_ERR(fabric);
 	}
 
 	fabric->tf_ops = sbp_ops;
diff --git a/drivers/target/target_core_alua.c b/drivers/target/target_core_alua.c
index 41641ba5482..9a5f9a7aecd 100644
--- a/drivers/target/target_core_alua.c
+++ b/drivers/target/target_core_alua.c
@@ -344,7 +344,7 @@ int target_emulate_set_target_port_groups(struct se_cmd *cmd)
 			 */
 			rtpi = get_unaligned_be16(ptr + 2);
 			/*
-			 * Locate the matching relative target port identifer
+			 * Locate the matching relative target port identifier
 			 * for the struct se_device storage object.
 			 */
 			spin_lock(&dev->se_port_lock);
diff --git a/drivers/target/target_core_configfs.c b/drivers/target/target_core_configfs.c
index 801efa89204..015f5be27bf 100644
--- a/drivers/target/target_core_configfs.c
+++ b/drivers/target/target_core_configfs.c
@@ -457,14 +457,6 @@ static int target_fabric_tf_ops_check(
 		pr_err("Missing tfo->queue_tm_rsp()\n");
 		return -EINVAL;
 	}
-	if (!tfo->set_fabric_sense_len) {
-		pr_err("Missing tfo->set_fabric_sense_len()\n");
-		return -EINVAL;
-	}
-	if (!tfo->get_fabric_sense_len) {
-		pr_err("Missing tfo->get_fabric_sense_len()\n");
-		return -EINVAL;
-	}
 	/*
 	 * We at least require tfo->fabric_make_wwn(), tfo->fabric_drop_wwn()
 	 * tfo->fabric_make_tpg() and tfo->fabric_drop_tpg() in
@@ -1208,7 +1200,7 @@ static ssize_t target_core_dev_pr_show_attr_res_pr_holder_tg_port(
 		" Target Node Endpoint: %s\n", tfo->get_fabric_name(),
 		tfo->tpg_get_wwn(se_tpg));
 	len += sprintf(page+len, "SPC-3 Reservation: Relative Port"
-		" Identifer Tag: %hu %s Portal Group Tag: %hu"
+		" Identifier Tag: %hu %s Portal Group Tag: %hu"
 		" %s Logical Unit: %u\n", lun->lun_sep->sep_rtpi,
 		tfo->get_fabric_name(), tfo->tpg_get_tag(se_tpg),
 		tfo->get_fabric_name(), lun->unpacked_lun);
@@ -3132,6 +3124,7 @@ static int __init target_core_init_configfs(void)
 				GFP_KERNEL);
 	if (!target_cg->default_groups) {
 		pr_err("Unable to allocate target_cg->default_groups\n");
+		ret = -ENOMEM;
 		goto out_global;
 	}
 
@@ -3147,6 +3140,7 @@ static int __init target_core_init_configfs(void)
 				GFP_KERNEL);
 	if (!hba_cg->default_groups) {
 		pr_err("Unable to allocate hba_cg->default_groups\n");
+		ret = -ENOMEM;
 		goto out_global;
 	}
 	config_group_init_type_name(&alua_group,
@@ -3162,6 +3156,7 @@ static int __init target_core_init_configfs(void)
 			GFP_KERNEL);
 	if (!alua_cg->default_groups) {
 		pr_err("Unable to allocate alua_cg->default_groups\n");
+		ret = -ENOMEM;
 		goto out_global;
 	}
 
@@ -3173,14 +3168,17 @@ static int __init target_core_init_configfs(void)
 	 * Add core/alua/lu_gps/default_lu_gp
 	 */
 	lu_gp = core_alua_allocate_lu_gp("default_lu_gp", 1);
-	if (IS_ERR(lu_gp))
+	if (IS_ERR(lu_gp)) {
+		ret = -ENOMEM;
 		goto out_global;
+	}
 
 	lu_gp_cg = &alua_lu_gps_group;
 	lu_gp_cg->default_groups = kzalloc(sizeof(struct config_group) * 2,
 			GFP_KERNEL);
 	if (!lu_gp_cg->default_groups) {
 		pr_err("Unable to allocate lu_gp_cg->default_groups\n");
+		ret = -ENOMEM;
 		goto out_global;
 	}
 
diff --git a/drivers/target/target_core_device.c b/drivers/target/target_core_device.c
index 9fc9a6006ca..8d774da1632 100644
--- a/drivers/target/target_core_device.c
+++ b/drivers/target/target_core_device.c
@@ -531,7 +531,7 @@ static struct se_port *core_alloc_port(struct se_device *dev)
 	}
 again:
 	/*
-	 * Allocate the next RELATIVE TARGET PORT IDENTIFER for this struct se_device
+	 * Allocate the next RELATIVE TARGET PORT IDENTIFIER for this struct se_device
 	 * Here is the table from spc4r17 section 7.7.3.8.
 	 *
 	 *    Table 473 -- RELATIVE TARGET PORT IDENTIFIER field
@@ -548,7 +548,7 @@ again:
 
 	list_for_each_entry(port_tmp, &dev->dev_sep_list, sep_list) {
 		/*
-		 * Make sure RELATIVE TARGET PORT IDENTIFER is unique
+		 * Make sure RELATIVE TARGET PORT IDENTIFIER is unique
 		 * for 16-bit wrap..
 		 */
 		if (port->sep_rtpi == port_tmp->sep_rtpi)
@@ -595,7 +595,7 @@ static void core_export_port(
 	}
 
 	dev->dev_port_count++;
-	port->sep_index = port->sep_rtpi; /* RELATIVE TARGET PORT IDENTIFER */
+	port->sep_index = port->sep_rtpi; /* RELATIVE TARGET PORT IDENTIFIER */
 }
 
 /*
@@ -988,8 +988,9 @@ int se_dev_set_emulate_fua_write(struct se_device *dev, int flag)
 		return -EINVAL;
 	}
 
-	if (flag && dev->transport->fua_write_emulated == 0) {
-		pr_err("fua_write_emulated not supported\n");
+	if (flag &&
+	    dev->transport->transport_type == TRANSPORT_PLUGIN_PHBA_PDEV) {
+		pr_err("emulate_fua_write not supported for pSCSI\n");
 		return -EINVAL;
 	}
 	dev->se_sub_dev->se_dev_attrib.emulate_fua_write = flag;
@@ -1019,8 +1020,9 @@ int se_dev_set_emulate_write_cache(struct se_device *dev, int flag)
 		pr_err("Illegal value %d\n", flag);
 		return -EINVAL;
 	}
-	if (flag && dev->transport->write_cache_emulated == 0) {
-		pr_err("write_cache_emulated not supported\n");
+	if (flag &&
+	    dev->transport->transport_type == TRANSPORT_PLUGIN_PHBA_PDEV) {
+		pr_err("emulate_write_cache not supported for pSCSI\n");
 		return -EINVAL;
 	}
 	dev->se_sub_dev->se_dev_attrib.emulate_write_cache = flag;
diff --git a/drivers/target/target_core_fabric_configfs.c b/drivers/target/target_core_fabric_configfs.c
index ea479e54f5f..bca737bb813 100644
--- a/drivers/target/target_core_fabric_configfs.c
+++ b/drivers/target/target_core_fabric_configfs.c
@@ -22,7 +22,6 @@
 
 #include <linux/module.h>
 #include <linux/moduleparam.h>
-#include <generated/utsrelease.h>
 #include <linux/utsname.h>
 #include <linux/init.h>
 #include <linux/fs.h>
diff --git a/drivers/target/target_core_fabric_lib.c b/drivers/target/target_core_fabric_lib.c
index 283a36e464e..e460d6233a0 100644
--- a/drivers/target/target_core_fabric_lib.c
+++ b/drivers/target/target_core_fabric_lib.c
@@ -338,7 +338,7 @@ u32 iscsi_get_pr_transport_id_len(
 	 * 00b: iSCSI Initiator device TransportID format
 	 */
 	if (pr_reg->isid_present_at_reg) {
-		len += 5; /* For ",i,0x" ASCII seperator */
+		len += 5; /* For ",i,0x" ASCII separator */
 		len += 7; /* For iSCSI Initiator Session ID + Null terminator */
 		*format_code = 1;
 	} else
@@ -415,20 +415,20 @@ char *iscsi_parse_pr_out_transport_id(
 			*out_tid_len = (add_len + 4);
 	}
 	/*
-	 * Check for ',i,0x' seperator between iSCSI Name and iSCSI Initiator
+	 * Check for ',i,0x' separator between iSCSI Name and iSCSI Initiator
 	 * Session ID as defined in Table 390 - iSCSI initiator port TransportID
 	 * format.
 	 */
 	if (format_code == 0x40) {
 		p = strstr(&buf[4], ",i,0x");
 		if (!p) {
-			pr_err("Unable to locate \",i,0x\" seperator"
+			pr_err("Unable to locate \",i,0x\" separator"
 				" for Initiator port identifier: %s\n",
 				&buf[4]);
 			return NULL;
 		}
 		*p = '\0'; /* Terminate iSCSI Name */
-		p += 5; /* Skip over ",i,0x" seperator */
+		p += 5; /* Skip over ",i,0x" separator */
 
 		*port_nexus_ptr = p;
 		/*
diff --git a/drivers/target/target_core_file.c b/drivers/target/target_core_file.c
index cbb5aaf3e56..0360383dfb9 100644
--- a/drivers/target/target_core_file.c
+++ b/drivers/target/target_core_file.c
@@ -125,6 +125,19 @@ static struct se_device *fd_create_virtdevice(
 	 * of pure timestamp updates.
 	 */
 	flags = O_RDWR | O_CREAT | O_LARGEFILE | O_DSYNC;
+	/*
+	 * Optionally allow fd_buffered_io=1 to be enabled for people
+	 * who want use the fs buffer cache as an WriteCache mechanism.
+	 *
+	 * This means that in event of a hard failure, there is a risk
+	 * of silent data-loss if the SCSI client has *not* performed a
+	 * forced unit access (FUA) write, or issued SYNCHRONIZE_CACHE
+	 * to write-out the entire device cache.
+	 */
+	if (fd_dev->fbd_flags & FDBD_HAS_BUFFERED_IO_WCE) {
+		pr_debug("FILEIO: Disabling O_DSYNC, using buffered FILEIO\n");
+		flags &= ~O_DSYNC;
+	}
 
 	file = filp_open(fd_dev->fd_dev_name, flags, 0600);
 	if (IS_ERR(file)) {
@@ -188,6 +201,12 @@ static struct se_device *fd_create_virtdevice(
 	if (!dev)
 		goto fail;
 
+	if (fd_dev->fbd_flags & FDBD_HAS_BUFFERED_IO_WCE) {
+		pr_debug("FILEIO: Forcing setting of emulate_write_cache=1"
+			" with FDBD_HAS_BUFFERED_IO_WCE\n");
+		dev->se_sub_dev->se_dev_attrib.emulate_write_cache = 1;
+	}
+
 	fd_dev->fd_dev_id = fd_host->fd_host_dev_id_count++;
 	fd_dev->fd_queue_depth = dev->queue_depth;
 
@@ -407,6 +426,7 @@ enum {
 static match_table_t tokens = {
 	{Opt_fd_dev_name, "fd_dev_name=%s"},
 	{Opt_fd_dev_size, "fd_dev_size=%s"},
+	{Opt_fd_buffered_io, "fd_buffered_io=%d"},
 	{Opt_err, NULL}
 };
 
@@ -418,7 +438,7 @@ static ssize_t fd_set_configfs_dev_params(
 	struct fd_dev *fd_dev = se_dev->se_dev_su_ptr;
 	char *orig, *ptr, *arg_p, *opts;
 	substring_t args[MAX_OPT_ARGS];
-	int ret = 0, token;
+	int ret = 0, arg, token;
 
 	opts = kstrdup(page, GFP_KERNEL);
 	if (!opts)
@@ -459,6 +479,19 @@ static ssize_t fd_set_configfs_dev_params(
 					" bytes\n", fd_dev->fd_dev_size);
 			fd_dev->fbd_flags |= FBDF_HAS_SIZE;
 			break;
+		case Opt_fd_buffered_io:
+			match_int(args, &arg);
+			if (arg != 1) {
+				pr_err("bogus fd_buffered_io=%d value\n", arg);
+				ret = -EINVAL;
+				goto out;
+			}
+
+			pr_debug("FILEIO: Using buffered I/O"
+				" operations for struct fd_dev\n");
+
+			fd_dev->fbd_flags |= FDBD_HAS_BUFFERED_IO_WCE;
+			break;
 		default:
 			break;
 		}
@@ -490,8 +523,10 @@ static ssize_t fd_show_configfs_dev_params(
 	ssize_t bl = 0;
 
 	bl = sprintf(b + bl, "TCM FILEIO ID: %u", fd_dev->fd_dev_id);
-	bl += sprintf(b + bl, "        File: %s  Size: %llu  Mode: O_DSYNC\n",
-		fd_dev->fd_dev_name, fd_dev->fd_dev_size);
+	bl += sprintf(b + bl, "        File: %s  Size: %llu  Mode: %s\n",
+		fd_dev->fd_dev_name, fd_dev->fd_dev_size,
+		(fd_dev->fbd_flags & FDBD_HAS_BUFFERED_IO_WCE) ?
+		"Buffered-WCE" : "O_DSYNC");
 	return bl;
 }
 
@@ -546,8 +581,6 @@ static struct se_subsystem_api fileio_template = {
 	.name			= "fileio",
 	.owner			= THIS_MODULE,
 	.transport_type		= TRANSPORT_PLUGIN_VHBA_PDEV,
-	.write_cache_emulated	= 1,
-	.fua_write_emulated	= 1,
 	.attach_hba		= fd_attach_hba,
 	.detach_hba		= fd_detach_hba,
 	.allocate_virtdevice	= fd_allocate_virtdevice,
diff --git a/drivers/target/target_core_file.h b/drivers/target/target_core_file.h
index 70ce7fd7111..876ae53ef5b 100644
--- a/drivers/target/target_core_file.h
+++ b/drivers/target/target_core_file.h
@@ -14,6 +14,7 @@
 
 #define FBDF_HAS_PATH		0x01
 #define FBDF_HAS_SIZE		0x02
+#define FDBD_HAS_BUFFERED_IO_WCE 0x04
 
 struct fd_dev {
 	u32		fbd_flags;
diff --git a/drivers/target/target_core_iblock.c b/drivers/target/target_core_iblock.c
index 9ba495477fd..57d7674c501 100644
--- a/drivers/target/target_core_iblock.c
+++ b/drivers/target/target_core_iblock.c
@@ -454,14 +454,11 @@ static ssize_t iblock_set_configfs_dev_params(struct se_hba *hba,
 				ret = -EEXIST;
 				goto out;
 			}
-			arg_p = match_strdup(&args[0]);
-			if (!arg_p) {
-				ret = -ENOMEM;
+			if (match_strlcpy(ib_dev->ibd_udev_path, &args[0],
+				SE_UDEV_PATH_LEN) == 0) {
+				ret = -EINVAL;
 				break;
 			}
-			snprintf(ib_dev->ibd_udev_path, SE_UDEV_PATH_LEN,
-					"%s", arg_p);
-			kfree(arg_p);
 			pr_debug("IBLOCK: Referencing UDEV path: %s\n",
 					ib_dev->ibd_udev_path);
 			ib_dev->ibd_flags |= IBDF_HAS_UDEV_PATH;
@@ -556,14 +553,6 @@ static void iblock_complete_cmd(struct se_cmd *cmd)
 	kfree(ibr);
 }
 
-static void iblock_bio_destructor(struct bio *bio)
-{
-	struct se_cmd *cmd = bio->bi_private;
-	struct iblock_dev *ib_dev = cmd->se_dev->dev_ptr;
-
-	bio_free(bio, ib_dev->ibd_bio_set);
-}
-
 static struct bio *
 iblock_get_bio(struct se_cmd *cmd, sector_t lba, u32 sg_num)
 {
@@ -585,7 +574,6 @@ iblock_get_bio(struct se_cmd *cmd, sector_t lba, u32 sg_num)
 
 	bio->bi_bdev = ib_dev->ibd_bd;
 	bio->bi_private = cmd;
-	bio->bi_destructor = iblock_bio_destructor;
 	bio->bi_end_io = &iblock_bio_done;
 	bio->bi_sector = lba;
 	return bio;
@@ -657,6 +645,12 @@ static int iblock_execute_rw(struct se_cmd *cmd)
 		goto fail;
 	cmd->priv = ibr;
 
+	if (!sgl_nents) {
+		atomic_set(&ibr->pending, 1);
+		iblock_complete_cmd(cmd);
+		return 0;
+	}
+
 	bio = iblock_get_bio(cmd, block_lba, sgl_nents);
 	if (!bio)
 		goto fail_free_ibr;
@@ -769,8 +763,6 @@ static struct se_subsystem_api iblock_template = {
 	.name			= "iblock",
 	.owner			= THIS_MODULE,
 	.transport_type		= TRANSPORT_PLUGIN_VHBA_PDEV,
-	.write_cache_emulated	= 1,
-	.fua_write_emulated	= 1,
 	.attach_hba		= iblock_attach_hba,
 	.detach_hba		= iblock_detach_hba,
 	.allocate_virtdevice	= iblock_allocate_virtdevice,
diff --git a/drivers/target/target_core_pr.c b/drivers/target/target_core_pr.c
index 956c84c6b66..8c323a98c4a 100644
--- a/drivers/target/target_core_pr.c
+++ b/drivers/target/target_core_pr.c
@@ -197,10 +197,10 @@ int target_scsi2_reservation_release(struct se_cmd *cmd)
 {
 	struct se_device *dev = cmd->se_dev;
 	struct se_session *sess = cmd->se_sess;
-	struct se_portal_group *tpg = sess->se_tpg;
+	struct se_portal_group *tpg;
 	int ret = 0, rc;
 
-	if (!sess || !tpg)
+	if (!sess || !sess->se_tpg)
 		goto out;
 	rc = target_check_scsi2_reservation_conflict(cmd);
 	if (rc == 1)
@@ -228,6 +228,7 @@ int target_scsi2_reservation_release(struct se_cmd *cmd)
 		dev->dev_res_bin_isid = 0;
 		dev->dev_flags &= ~DF_SPC2_RESERVATIONS_WITH_ISID;
 	}
+	tpg = sess->se_tpg;
 	pr_debug("SCSI-2 Released reservation for %s LUN: %u ->"
 		" MAPPED LUN: %u for %s\n", tpg->se_tpg_tfo->get_fabric_name(),
 		cmd->se_lun->unpacked_lun, cmd->se_deve->mapped_lun,
@@ -245,7 +246,7 @@ int target_scsi2_reservation_reserve(struct se_cmd *cmd)
 {
 	struct se_device *dev = cmd->se_dev;
 	struct se_session *sess = cmd->se_sess;
-	struct se_portal_group *tpg = sess->se_tpg;
+	struct se_portal_group *tpg;
 	int ret = 0, rc;
 
 	if ((cmd->t_task_cdb[1] & 0x01) &&
@@ -260,7 +261,7 @@ int target_scsi2_reservation_reserve(struct se_cmd *cmd)
 	 * This is currently the case for target_core_mod passthrough struct se_cmd
 	 * ops
 	 */
-	if (!sess || !tpg)
+	if (!sess || !sess->se_tpg)
 		goto out;
 	rc = target_check_scsi2_reservation_conflict(cmd);
 	if (rc == 1)
@@ -272,6 +273,7 @@ int target_scsi2_reservation_reserve(struct se_cmd *cmd)
 	}
 
 	ret = 0;
+	tpg = sess->se_tpg;
 	spin_lock(&dev->dev_reservation_lock);
 	if (dev->dev_reserved_node_acl &&
 	   (dev->dev_reserved_node_acl != sess->se_node_acl)) {
@@ -1620,7 +1622,7 @@ static int core_scsi3_decode_spec_i_port(
 				goto out;
 			}
 			/*
-			 * Locate the desination initiator ACL to be registered
+			 * Locate the destination initiator ACL to be registered
 			 * from the decoded fabric module specific TransportID
 			 * at *i_str.
 			 */
@@ -4257,7 +4259,7 @@ static int core_scsi3_pri_read_full_status(struct se_cmd *cmd)
 			buf[off++] = ((port->sep_rtpi >> 8) & 0xff);
 			buf[off++] = (port->sep_rtpi & 0xff);
 		} else
-			off += 2; /* Skip over RELATIVE TARGET PORT IDENTIFER */
+			off += 2; /* Skip over RELATIVE TARGET PORT IDENTIFIER */
 
 		/*
 		 * Now, have the $FABRIC_MOD fill in the protocol identifier
diff --git a/drivers/target/target_core_pscsi.c b/drivers/target/target_core_pscsi.c
index 9d7ce3daa26..617c086a8a0 100644
--- a/drivers/target/target_core_pscsi.c
+++ b/drivers/target/target_core_pscsi.c
@@ -264,7 +264,7 @@ pscsi_get_inquiry_vpd_device_ident(struct scsi_device *sdev,
 					" length zero!\n");
 			break;
 		}
-		pr_debug("T10 VPD Identifer Length: %d\n", ident_len);
+		pr_debug("T10 VPD Identifier Length: %d\n", ident_len);
 
 		vpd = kzalloc(sizeof(struct t10_vpd), GFP_KERNEL);
 		if (!vpd) {
diff --git a/drivers/target/target_core_sbc.c b/drivers/target/target_core_sbc.c
index a9dd9469e3b..868f8aa04f1 100644
--- a/drivers/target/target_core_sbc.c
+++ b/drivers/target/target_core_sbc.c
@@ -40,8 +40,9 @@
 static int sbc_emulate_readcapacity(struct se_cmd *cmd)
 {
 	struct se_device *dev = cmd->se_dev;
-	unsigned char *buf;
 	unsigned long long blocks_long = dev->transport->get_blocks(dev);
+	unsigned char *rbuf;
+	unsigned char buf[8];
 	u32 blocks;
 
 	if (blocks_long >= 0x00000000ffffffff)
@@ -49,8 +50,6 @@ static int sbc_emulate_readcapacity(struct se_cmd *cmd)
 	else
 		blocks = (u32)blocks_long;
 
-	buf = transport_kmap_data_sg(cmd);
-
 	buf[0] = (blocks >> 24) & 0xff;
 	buf[1] = (blocks >> 16) & 0xff;
 	buf[2] = (blocks >> 8) & 0xff;
@@ -60,7 +59,11 @@ static int sbc_emulate_readcapacity(struct se_cmd *cmd)
 	buf[6] = (dev->se_sub_dev->se_dev_attrib.block_size >> 8) & 0xff;
 	buf[7] = dev->se_sub_dev->se_dev_attrib.block_size & 0xff;
 
-	transport_kunmap_data_sg(cmd);
+	rbuf = transport_kmap_data_sg(cmd);
+	if (rbuf) {
+		memcpy(rbuf, buf, min_t(u32, sizeof(buf), cmd->data_length));
+		transport_kunmap_data_sg(cmd);
+	}
 
 	target_complete_cmd(cmd, GOOD);
 	return 0;
@@ -69,11 +72,11 @@ static int sbc_emulate_readcapacity(struct se_cmd *cmd)
 static int sbc_emulate_readcapacity_16(struct se_cmd *cmd)
 {
 	struct se_device *dev = cmd->se_dev;
-	unsigned char *buf;
+	unsigned char *rbuf;
+	unsigned char buf[32];
 	unsigned long long blocks = dev->transport->get_blocks(dev);
 
-	buf = transport_kmap_data_sg(cmd);
-
+	memset(buf, 0, sizeof(buf));
 	buf[0] = (blocks >> 56) & 0xff;
 	buf[1] = (blocks >> 48) & 0xff;
 	buf[2] = (blocks >> 40) & 0xff;
@@ -93,7 +96,11 @@ static int sbc_emulate_readcapacity_16(struct se_cmd *cmd)
 	if (dev->se_sub_dev->se_dev_attrib.emulate_tpu || dev->se_sub_dev->se_dev_attrib.emulate_tpws)
 		buf[14] = 0x80;
 
-	transport_kunmap_data_sg(cmd);
+	rbuf = transport_kmap_data_sg(cmd);
+	if (rbuf) {
+		memcpy(rbuf, buf, min_t(u32, sizeof(buf), cmd->data_length));
+		transport_kunmap_data_sg(cmd);
+	}
 
 	target_complete_cmd(cmd, GOOD);
 	return 0;
diff --git a/drivers/target/target_core_spc.c b/drivers/target/target_core_spc.c
index 388a922c8f6..9229bd9ad61 100644
--- a/drivers/target/target_core_spc.c
+++ b/drivers/target/target_core_spc.c
@@ -600,30 +600,11 @@ static int spc_emulate_inquiry(struct se_cmd *cmd)
 {
 	struct se_device *dev = cmd->se_dev;
 	struct se_portal_group *tpg = cmd->se_lun->lun_sep->sep_tpg;
-	unsigned char *buf, *map_buf;
+	unsigned char *rbuf;
 	unsigned char *cdb = cmd->t_task_cdb;
+	unsigned char buf[SE_INQUIRY_BUF];
 	int p, ret;
 
-	map_buf = transport_kmap_data_sg(cmd);
-	/*
-	 * If SCF_PASSTHROUGH_SG_TO_MEM_NOALLOC is not set, then we
-	 * know we actually allocated a full page.  Otherwise, if the
-	 * data buffer is too small, allocate a temporary buffer so we
-	 * don't have to worry about overruns in all our INQUIRY
-	 * emulation handling.
-	 */
-	if (cmd->data_length < SE_INQUIRY_BUF &&
-	    (cmd->se_cmd_flags & SCF_PASSTHROUGH_SG_TO_MEM_NOALLOC)) {
-		buf = kzalloc(SE_INQUIRY_BUF, GFP_KERNEL);
-		if (!buf) {
-			transport_kunmap_data_sg(cmd);
-			cmd->scsi_sense_reason = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
-			return -ENOMEM;
-		}
-	} else {
-		buf = map_buf;
-	}
-
 	if (dev == tpg->tpg_virt_lun0.lun_se_dev)
 		buf[0] = 0x3f; /* Not connected */
 	else
@@ -655,11 +636,11 @@ static int spc_emulate_inquiry(struct se_cmd *cmd)
 	ret = -EINVAL;
 
 out:
-	if (buf != map_buf) {
-		memcpy(map_buf, buf, cmd->data_length);
-		kfree(buf);
+	rbuf = transport_kmap_data_sg(cmd);
+	if (rbuf) {
+		memcpy(rbuf, buf, min_t(u32, sizeof(buf), cmd->data_length));
+		transport_kunmap_data_sg(cmd);
 	}
-	transport_kunmap_data_sg(cmd);
 
 	if (!ret)
 		target_complete_cmd(cmd, GOOD);
@@ -803,7 +784,7 @@ static int spc_emulate_modesense(struct se_cmd *cmd)
 	unsigned char *rbuf;
 	int type = dev->transport->get_device_type(dev);
 	int ten = (cmd->t_task_cdb[0] == MODE_SENSE_10);
-	int offset = ten ? 8 : 4;
+	u32 offset = ten ? 8 : 4;
 	int length = 0;
 	unsigned char buf[SE_MODE_PAGE_BUF];
 
@@ -836,6 +817,7 @@ static int spc_emulate_modesense(struct se_cmd *cmd)
 		offset -= 2;
 		buf[0] = (offset >> 8) & 0xff;
 		buf[1] = offset & 0xff;
+		offset += 2;
 
 		if ((cmd->se_lun->lun_access & TRANSPORT_LUNFLAGS_READ_ONLY) ||
 		    (cmd->se_deve &&
@@ -845,13 +827,10 @@ static int spc_emulate_modesense(struct se_cmd *cmd)
 		if ((dev->se_sub_dev->se_dev_attrib.emulate_write_cache > 0) &&
 		    (dev->se_sub_dev->se_dev_attrib.emulate_fua_write > 0))
 			spc_modesense_dpofua(&buf[3], type);
-
-		if ((offset + 2) > cmd->data_length)
-			offset = cmd->data_length;
-
 	} else {
 		offset -= 1;
 		buf[0] = offset & 0xff;
+		offset += 1;
 
 		if ((cmd->se_lun->lun_access & TRANSPORT_LUNFLAGS_READ_ONLY) ||
 		    (cmd->se_deve &&
@@ -861,14 +840,13 @@ static int spc_emulate_modesense(struct se_cmd *cmd)
 		if ((dev->se_sub_dev->se_dev_attrib.emulate_write_cache > 0) &&
 		    (dev->se_sub_dev->se_dev_attrib.emulate_fua_write > 0))
 			spc_modesense_dpofua(&buf[2], type);
-
-		if ((offset + 1) > cmd->data_length)
-			offset = cmd->data_length;
 	}
 
 	rbuf = transport_kmap_data_sg(cmd);
-	memcpy(rbuf, buf, offset);
-	transport_kunmap_data_sg(cmd);
+	if (rbuf) {
+		memcpy(rbuf, buf, min(offset, cmd->data_length));
+		transport_kunmap_data_sg(cmd);
+	}
 
 	target_complete_cmd(cmd, GOOD);
 	return 0;
diff --git a/drivers/target/target_core_stat.c b/drivers/target/target_core_stat.c
index 3d44beb0cf1..cb6b0036ae9 100644
--- a/drivers/target/target_core_stat.c
+++ b/drivers/target/target_core_stat.c
@@ -32,7 +32,6 @@
 #include <linux/delay.h>
 #include <linux/timer.h>
 #include <linux/string.h>
-#include <generated/utsrelease.h>
 #include <linux/utsname.h>
 #include <linux/proc_fs.h>
 #include <linux/seq_file.h>
diff --git a/drivers/target/target_core_tpg.c b/drivers/target/target_core_tpg.c
index b8628a5014b..a531fe282b1 100644
--- a/drivers/target/target_core_tpg.c
+++ b/drivers/target/target_core_tpg.c
@@ -303,7 +303,7 @@ struct se_node_acl *core_tpg_check_initiator_node_acl(
 	}
 	/*
 	 * Here we only create demo-mode MappedLUNs from the active
-	 * TPG LUNs if the fabric is not explictly asking for
+	 * TPG LUNs if the fabric is not explicitly asking for
 	 * tpg_check_demo_mode_login_only() == 1.
 	 */
 	if ((tpg->se_tpg_tfo->tpg_check_demo_mode_login_only == NULL) ||
diff --git a/drivers/target/target_core_transport.c b/drivers/target/target_core_transport.c
index 269f5448839..c33baff86aa 100644
--- a/drivers/target/target_core_transport.c
+++ b/drivers/target/target_core_transport.c
@@ -55,8 +55,6 @@
 #include "target_core_pr.h"
 #include "target_core_ua.h"
 
-static int sub_api_initialized;
-
 static struct workqueue_struct *target_completion_wq;
 static struct kmem_cache *se_sess_cache;
 struct kmem_cache *se_ua_cache;
@@ -195,6 +193,7 @@ u32 scsi_get_new_index(scsi_index_t type)
 void transport_subsystem_check_init(void)
 {
 	int ret;
+	static int sub_api_initialized;
 
 	if (sub_api_initialized)
 		return;
@@ -211,12 +210,7 @@ void transport_subsystem_check_init(void)
 	if (ret != 0)
 		pr_err("Unable to load target_core_pscsi\n");
 
-	ret = request_module("target_core_stgt");
-	if (ret != 0)
-		pr_err("Unable to load target_core_stgt\n");
-
 	sub_api_initialized = 1;
-	return;
 }
 
 struct se_session *transport_init_session(void)
@@ -573,9 +567,7 @@ static void target_complete_failure_work(struct work_struct *work)
  */
 static unsigned char *transport_get_sense_buffer(struct se_cmd *cmd)
 {
-	unsigned char *buffer = cmd->sense_buffer;
 	struct se_device *dev = cmd->se_dev;
-	u32 offset = 0;
 
 	WARN_ON(!cmd->se_lun);
 
@@ -585,14 +577,11 @@ static unsigned char *transport_get_sense_buffer(struct se_cmd *cmd)
 	if (cmd->se_cmd_flags & SCF_SENT_CHECK_CONDITION)
 		return NULL;
 
-	offset = cmd->se_tfo->set_fabric_sense_len(cmd, TRANSPORT_SENSE_BUFFER);
-
-	/* Automatically padded */
-	cmd->scsi_sense_length = TRANSPORT_SENSE_BUFFER + offset;
+	cmd->scsi_sense_length = TRANSPORT_SENSE_BUFFER;
 
 	pr_debug("HBA_[%u]_PLUG[%s]: Requesting sense for SAM STATUS: 0x%02x\n",
 		dev->se_hba->hba_id, dev->transport->name, cmd->scsi_status);
-	return &buffer[offset];
+	return cmd->sense_buffer;
 }
 
 void target_complete_cmd(struct se_cmd *cmd, u8 scsi_status)
@@ -969,7 +958,7 @@ int
 transport_set_vpd_ident(struct t10_vpd *vpd, unsigned char *page_83)
 {
 	static const char hex_str[] = "0123456789abcdef";
-	int j = 0, i = 4; /* offset to start of the identifer */
+	int j = 0, i = 4; /* offset to start of the identifier */
 
 	/*
 	 * The VPD Code Set (encoding)
@@ -1466,8 +1455,9 @@ int transport_handle_cdb_direct(
 }
 EXPORT_SYMBOL(transport_handle_cdb_direct);
 
-/**
- * target_submit_cmd - lookup unpacked lun and submit uninitialized se_cmd
+/*
+ * target_submit_cmd_map_sgls - lookup unpacked lun and submit uninitialized
+ * 			 se_cmd + use pre-allocated SGL memory.
  *
  * @se_cmd: command descriptor to submit
  * @se_sess: associated se_sess for endpoint
@@ -1478,6 +1468,10 @@ EXPORT_SYMBOL(transport_handle_cdb_direct);
  * @task_addr: SAM task attribute
  * @data_dir: DMA data direction
  * @flags: flags for command submission from target_sc_flags_tables
+ * @sgl: struct scatterlist memory for unidirectional mapping
+ * @sgl_count: scatterlist count for unidirectional mapping
+ * @sgl_bidi: struct scatterlist memory for bidirectional READ mapping
+ * @sgl_bidi_count: scatterlist count for bidirectional READ mapping
  *
  * Returns non zero to signal active I/O shutdown failure.  All other
  * setup exceptions will be returned as a SCSI CHECK_CONDITION response,
@@ -1485,10 +1479,12 @@ EXPORT_SYMBOL(transport_handle_cdb_direct);
  *
  * This may only be called from process context, and also currently
  * assumes internal allocation of fabric payload buffer by target-core.
- **/
-int target_submit_cmd(struct se_cmd *se_cmd, struct se_session *se_sess,
+ */
+int target_submit_cmd_map_sgls(struct se_cmd *se_cmd, struct se_session *se_sess,
 		unsigned char *cdb, unsigned char *sense, u32 unpacked_lun,
-		u32 data_length, int task_attr, int data_dir, int flags)
+		u32 data_length, int task_attr, int data_dir, int flags,
+		struct scatterlist *sgl, u32 sgl_count,
+		struct scatterlist *sgl_bidi, u32 sgl_bidi_count)
 {
 	struct se_portal_group *se_tpg;
 	int rc;
@@ -1535,7 +1531,42 @@ int target_submit_cmd(struct se_cmd *se_cmd, struct se_session *se_sess,
 		transport_generic_request_failure(se_cmd);
 		return 0;
 	}
+	/*
+	 * When a non zero sgl_count has been passed perform SGL passthrough
+	 * mapping for pre-allocated fabric memory instead of having target
+	 * core perform an internal SGL allocation..
+	 */
+	if (sgl_count != 0) {
+		BUG_ON(!sgl);
+
+		/*
+		 * A work-around for tcm_loop as some userspace code via
+		 * scsi-generic do not memset their associated read buffers,
+		 * so go ahead and do that here for type non-data CDBs.  Also
+		 * note that this is currently guaranteed to be a single SGL
+		 * for this case by target core in target_setup_cmd_from_cdb()
+		 * -> transport_generic_cmd_sequencer().
+		 */
+		if (!(se_cmd->se_cmd_flags & SCF_SCSI_DATA_CDB) &&
+		     se_cmd->data_direction == DMA_FROM_DEVICE) {
+			unsigned char *buf = NULL;
+
+			if (sgl)
+				buf = kmap(sg_page(sgl)) + sgl->offset;
+
+			if (buf) {
+				memset(buf, 0, sgl->length);
+				kunmap(sg_page(sgl));
+			}
+		}
 
+		rc = transport_generic_map_mem_to_cmd(se_cmd, sgl, sgl_count,
+				sgl_bidi, sgl_bidi_count);
+		if (rc != 0) {
+			transport_generic_request_failure(se_cmd);
+			return 0;
+		}
+	}
 	/*
 	 * Check if we need to delay processing because of ALUA
 	 * Active/NonOptimized primary access state..
@@ -1545,6 +1576,38 @@ int target_submit_cmd(struct se_cmd *se_cmd, struct se_session *se_sess,
 	transport_handle_cdb_direct(se_cmd);
 	return 0;
 }
+EXPORT_SYMBOL(target_submit_cmd_map_sgls);
+
+/*
+ * target_submit_cmd - lookup unpacked lun and submit uninitialized se_cmd
+ *
+ * @se_cmd: command descriptor to submit
+ * @se_sess: associated se_sess for endpoint
+ * @cdb: pointer to SCSI CDB
+ * @sense: pointer to SCSI sense buffer
+ * @unpacked_lun: unpacked LUN to reference for struct se_lun
+ * @data_length: fabric expected data transfer length
+ * @task_addr: SAM task attribute
+ * @data_dir: DMA data direction
+ * @flags: flags for command submission from target_sc_flags_tables
+ *
+ * Returns non zero to signal active I/O shutdown failure.  All other
+ * setup exceptions will be returned as a SCSI CHECK_CONDITION response,
+ * but still return zero here.
+ *
+ * This may only be called from process context, and also currently
+ * assumes internal allocation of fabric payload buffer by target-core.
+ *
+ * It also assumes interal target core SGL memory allocation.
+ */
+int target_submit_cmd(struct se_cmd *se_cmd, struct se_session *se_sess,
+		unsigned char *cdb, unsigned char *sense, u32 unpacked_lun,
+		u32 data_length, int task_attr, int data_dir, int flags)
+{
+	return target_submit_cmd_map_sgls(se_cmd, se_sess, cdb, sense,
+			unpacked_lun, data_length, task_attr, data_dir,
+			flags, NULL, 0, NULL, 0);
+}
 EXPORT_SYMBOL(target_submit_cmd);
 
 static void target_complete_tmr_failure(struct work_struct *work)
@@ -2300,23 +2363,6 @@ int transport_generic_new_cmd(struct se_cmd *cmd)
 		if (ret < 0)
 			goto out_fail;
 	}
-	/*
-	 * If this command doesn't have any payload and we don't have to call
-	 * into the fabric for data transfers, go ahead and complete it right
-	 * away.
-	 */
-	if (!cmd->data_length &&
-	    cmd->t_task_cdb[0] != REQUEST_SENSE &&
-	    cmd->se_dev->transport->transport_type != TRANSPORT_PLUGIN_PHBA_PDEV) {
-		spin_lock_irq(&cmd->t_state_lock);
-		cmd->t_state = TRANSPORT_COMPLETE;
-		cmd->transport_state |= CMD_T_ACTIVE;
-		spin_unlock_irq(&cmd->t_state_lock);
-
-		INIT_WORK(&cmd->work, target_complete_ok_work);
-		queue_work(target_completion_wq, &cmd->work);
-		return 0;
-	}
 
 	atomic_inc(&cmd->t_fe_count);
 
@@ -2771,7 +2817,7 @@ bool transport_wait_for_tasks(struct se_cmd *cmd)
 	spin_lock_irqsave(&cmd->t_state_lock, flags);
 	cmd->transport_state &= ~(CMD_T_ACTIVE | CMD_T_STOP);
 
-	pr_debug("wait_for_tasks: Stopped wait_for_compltion("
+	pr_debug("wait_for_tasks: Stopped wait_for_completion("
 		"&cmd->t_transport_stop_comp) for ITT: 0x%08x\n",
 		cmd->se_tfo->get_task_tag(cmd));
 
@@ -2810,7 +2856,6 @@ int transport_send_check_condition_and_sense(
 {
 	unsigned char *buffer = cmd->sense_buffer;
 	unsigned long flags;
-	int offset;
 	u8 asc = 0, ascq = 0;
 
 	spin_lock_irqsave(&cmd->t_state_lock, flags);
@@ -2826,14 +2871,7 @@ int transport_send_check_condition_and_sense(
 
 	if (!from_transport)
 		cmd->se_cmd_flags |= SCF_EMULATED_TASK_SENSE;
-	/*
-	 * Data Segment and SenseLength of the fabric response PDU.
-	 *
-	 * TRANSPORT_SENSE_BUFFER is now set to SCSI_SENSE_BUFFERSIZE
-	 * from include/scsi/scsi_cmnd.h
-	 */
-	offset = cmd->se_tfo->set_fabric_sense_len(cmd,
-				TRANSPORT_SENSE_BUFFER);
+
 	/*
 	 * Actual SENSE DATA, see SPC-3 7.23.2  SPC_SENSE_KEY_OFFSET uses
 	 * SENSE KEY values from include/scsi/scsi.h
@@ -2841,151 +2879,151 @@ int transport_send_check_condition_and_sense(
 	switch (reason) {
 	case TCM_NON_EXISTENT_LUN:
 		/* CURRENT ERROR */
-		buffer[offset] = 0x70;
-		buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+		buffer[0] = 0x70;
+		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
 		/* ILLEGAL REQUEST */
-		buffer[offset+SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+		buffer[SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
 		/* LOGICAL UNIT NOT SUPPORTED */
-		buffer[offset+SPC_ASC_KEY_OFFSET] = 0x25;
+		buffer[SPC_ASC_KEY_OFFSET] = 0x25;
 		break;
 	case TCM_UNSUPPORTED_SCSI_OPCODE:
 	case TCM_SECTOR_COUNT_TOO_MANY:
 		/* CURRENT ERROR */
-		buffer[offset] = 0x70;
-		buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+		buffer[0] = 0x70;
+		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
 		/* ILLEGAL REQUEST */
-		buffer[offset+SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+		buffer[SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
 		/* INVALID COMMAND OPERATION CODE */
-		buffer[offset+SPC_ASC_KEY_OFFSET] = 0x20;
+		buffer[SPC_ASC_KEY_OFFSET] = 0x20;
 		break;
 	case TCM_UNKNOWN_MODE_PAGE:
 		/* CURRENT ERROR */
-		buffer[offset] = 0x70;
-		buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+		buffer[0] = 0x70;
+		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
 		/* ILLEGAL REQUEST */
-		buffer[offset+SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+		buffer[SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
 		/* INVALID FIELD IN CDB */
-		buffer[offset+SPC_ASC_KEY_OFFSET] = 0x24;
+		buffer[SPC_ASC_KEY_OFFSET] = 0x24;
 		break;
 	case TCM_CHECK_CONDITION_ABORT_CMD:
 		/* CURRENT ERROR */
-		buffer[offset] = 0x70;
-		buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+		buffer[0] = 0x70;
+		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
 		/* ABORTED COMMAND */
-		buffer[offset+SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
+		buffer[SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
 		/* BUS DEVICE RESET FUNCTION OCCURRED */
-		buffer[offset+SPC_ASC_KEY_OFFSET] = 0x29;
-		buffer[offset+SPC_ASCQ_KEY_OFFSET] = 0x03;
+		buffer[SPC_ASC_KEY_OFFSET] = 0x29;
+		buffer[SPC_ASCQ_KEY_OFFSET] = 0x03;
 		break;
 	case TCM_INCORRECT_AMOUNT_OF_DATA:
 		/* CURRENT ERROR */
-		buffer[offset] = 0x70;
-		buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+		buffer[0] = 0x70;
+		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
 		/* ABORTED COMMAND */
-		buffer[offset+SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
+		buffer[SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
 		/* WRITE ERROR */
-		buffer[offset+SPC_ASC_KEY_OFFSET] = 0x0c;
+		buffer[SPC_ASC_KEY_OFFSET] = 0x0c;
 		/* NOT ENOUGH UNSOLICITED DATA */
-		buffer[offset+SPC_ASCQ_KEY_OFFSET] = 0x0d;
+		buffer[SPC_ASCQ_KEY_OFFSET] = 0x0d;
 		break;
 	case TCM_INVALID_CDB_FIELD:
 		/* CURRENT ERROR */
-		buffer[offset] = 0x70;
-		buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+		buffer[0] = 0x70;
+		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
 		/* ILLEGAL REQUEST */
-		buffer[offset+SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+		buffer[SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
 		/* INVALID FIELD IN CDB */
-		buffer[offset+SPC_ASC_KEY_OFFSET] = 0x24;
+		buffer[SPC_ASC_KEY_OFFSET] = 0x24;
 		break;
 	case TCM_INVALID_PARAMETER_LIST:
 		/* CURRENT ERROR */
-		buffer[offset] = 0x70;
-		buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+		buffer[0] = 0x70;
+		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
 		/* ILLEGAL REQUEST */
-		buffer[offset+SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+		buffer[SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
 		/* INVALID FIELD IN PARAMETER LIST */
-		buffer[offset+SPC_ASC_KEY_OFFSET] = 0x26;
+		buffer[SPC_ASC_KEY_OFFSET] = 0x26;
 		break;
 	case TCM_UNEXPECTED_UNSOLICITED_DATA:
 		/* CURRENT ERROR */
-		buffer[offset] = 0x70;
-		buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+		buffer[0] = 0x70;
+		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
 		/* ABORTED COMMAND */
-		buffer[offset+SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
+		buffer[SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
 		/* WRITE ERROR */
-		buffer[offset+SPC_ASC_KEY_OFFSET] = 0x0c;
+		buffer[SPC_ASC_KEY_OFFSET] = 0x0c;
 		/* UNEXPECTED_UNSOLICITED_DATA */
-		buffer[offset+SPC_ASCQ_KEY_OFFSET] = 0x0c;
+		buffer[SPC_ASCQ_KEY_OFFSET] = 0x0c;
 		break;
 	case TCM_SERVICE_CRC_ERROR:
 		/* CURRENT ERROR */
-		buffer[offset] = 0x70;
-		buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+		buffer[0] = 0x70;
+		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
 		/* ABORTED COMMAND */
-		buffer[offset+SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
+		buffer[SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
 		/* PROTOCOL SERVICE CRC ERROR */
-		buffer[offset+SPC_ASC_KEY_OFFSET] = 0x47;
+		buffer[SPC_ASC_KEY_OFFSET] = 0x47;
 		/* N/A */
-		buffer[offset+SPC_ASCQ_KEY_OFFSET] = 0x05;
+		buffer[SPC_ASCQ_KEY_OFFSET] = 0x05;
 		break;
 	case TCM_SNACK_REJECTED:
 		/* CURRENT ERROR */
-		buffer[offset] = 0x70;
-		buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+		buffer[0] = 0x70;
+		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
 		/* ABORTED COMMAND */
-		buffer[offset+SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
+		buffer[SPC_SENSE_KEY_OFFSET] = ABORTED_COMMAND;
 		/* READ ERROR */
-		buffer[offset+SPC_ASC_KEY_OFFSET] = 0x11;
+		buffer[SPC_ASC_KEY_OFFSET] = 0x11;
 		/* FAILED RETRANSMISSION REQUEST */
-		buffer[offset+SPC_ASCQ_KEY_OFFSET] = 0x13;
+		buffer[SPC_ASCQ_KEY_OFFSET] = 0x13;
 		break;
 	case TCM_WRITE_PROTECTED:
 		/* CURRENT ERROR */
-		buffer[offset] = 0x70;
-		buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+		buffer[0] = 0x70;
+		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
 		/* DATA PROTECT */
-		buffer[offset+SPC_SENSE_KEY_OFFSET] = DATA_PROTECT;
+		buffer[SPC_SENSE_KEY_OFFSET] = DATA_PROTECT;
 		/* WRITE PROTECTED */
-		buffer[offset+SPC_ASC_KEY_OFFSET] = 0x27;
+		buffer[SPC_ASC_KEY_OFFSET] = 0x27;
 		break;
 	case TCM_ADDRESS_OUT_OF_RANGE:
 		/* CURRENT ERROR */
-		buffer[offset] = 0x70;
-		buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+		buffer[0] = 0x70;
+		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
 		/* ILLEGAL REQUEST */
-		buffer[offset+SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+		buffer[SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
 		/* LOGICAL BLOCK ADDRESS OUT OF RANGE */
-		buffer[offset+SPC_ASC_KEY_OFFSET] = 0x21;
+		buffer[SPC_ASC_KEY_OFFSET] = 0x21;
 		break;
 	case TCM_CHECK_CONDITION_UNIT_ATTENTION:
 		/* CURRENT ERROR */
-		buffer[offset] = 0x70;
-		buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+		buffer[0] = 0x70;
+		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
 		/* UNIT ATTENTION */
-		buffer[offset+SPC_SENSE_KEY_OFFSET] = UNIT_ATTENTION;
+		buffer[SPC_SENSE_KEY_OFFSET] = UNIT_ATTENTION;
 		core_scsi3_ua_for_check_condition(cmd, &asc, &ascq);
-		buffer[offset+SPC_ASC_KEY_OFFSET] = asc;
-		buffer[offset+SPC_ASCQ_KEY_OFFSET] = ascq;
+		buffer[SPC_ASC_KEY_OFFSET] = asc;
+		buffer[SPC_ASCQ_KEY_OFFSET] = ascq;
 		break;
 	case TCM_CHECK_CONDITION_NOT_READY:
 		/* CURRENT ERROR */
-		buffer[offset] = 0x70;
-		buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+		buffer[0] = 0x70;
+		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
 		/* Not Ready */
-		buffer[offset+SPC_SENSE_KEY_OFFSET] = NOT_READY;
+		buffer[SPC_SENSE_KEY_OFFSET] = NOT_READY;
 		transport_get_sense_codes(cmd, &asc, &ascq);
-		buffer[offset+SPC_ASC_KEY_OFFSET] = asc;
-		buffer[offset+SPC_ASCQ_KEY_OFFSET] = ascq;
+		buffer[SPC_ASC_KEY_OFFSET] = asc;
+		buffer[SPC_ASCQ_KEY_OFFSET] = ascq;
 		break;
 	case TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE:
 	default:
 		/* CURRENT ERROR */
-		buffer[offset] = 0x70;
-		buffer[offset+SPC_ADD_SENSE_LEN_OFFSET] = 10;
+		buffer[0] = 0x70;
+		buffer[SPC_ADD_SENSE_LEN_OFFSET] = 10;
 		/* ILLEGAL REQUEST */
-		buffer[offset+SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
+		buffer[SPC_SENSE_KEY_OFFSET] = ILLEGAL_REQUEST;
 		/* LOGICAL UNIT COMMUNICATION FAILURE */
-		buffer[offset+SPC_ASC_KEY_OFFSET] = 0x80;
+		buffer[SPC_ASC_KEY_OFFSET] = 0x80;
 		break;
 	}
 	/*
@@ -2996,7 +3034,7 @@ int transport_send_check_condition_and_sense(
 	 * Automatically padded, this value is encoded in the fabric's
 	 * data_length response PDU containing the SCSI defined sense data.
 	 */
-	cmd->scsi_sense_length  = TRANSPORT_SENSE_BUFFER + offset;
+	cmd->scsi_sense_length  = TRANSPORT_SENSE_BUFFER;
 
 after_reason:
 	return cmd->se_tfo->queue_status(cmd);
diff --git a/drivers/target/tcm_fc/tfc_cmd.c b/drivers/target/tcm_fc/tfc_cmd.c
index 823e6922249..b406f178ff3 100644
--- a/drivers/target/tcm_fc/tfc_cmd.c
+++ b/drivers/target/tcm_fc/tfc_cmd.c
@@ -19,7 +19,6 @@
 
 #include <linux/module.h>
 #include <linux/moduleparam.h>
-#include <generated/utsrelease.h>
 #include <linux/utsname.h>
 #include <linux/init.h>
 #include <linux/slab.h>
diff --git a/drivers/target/tcm_fc/tfc_conf.c b/drivers/target/tcm_fc/tfc_conf.c
index 9501844fae2..b74feb0d513 100644
--- a/drivers/target/tcm_fc/tfc_conf.c
+++ b/drivers/target/tcm_fc/tfc_conf.c
@@ -495,16 +495,6 @@ static void ft_set_default_node_attr(struct se_node_acl *se_nacl)
 {
 }
 
-static u16 ft_get_fabric_sense_len(void)
-{
-	return 0;
-}
-
-static u16 ft_set_fabric_sense_len(struct se_cmd *se_cmd, u32 sense_len)
-{
-	return 0;
-}
-
 static u32 ft_tpg_get_inst_index(struct se_portal_group *se_tpg)
 {
 	struct ft_tpg *tpg = se_tpg->se_tpg_fabric_ptr;
@@ -542,8 +532,6 @@ static struct target_core_fabric_ops ft_fabric_ops = {
 	.queue_data_in =		ft_queue_data_in,
 	.queue_status =			ft_queue_status,
 	.queue_tm_rsp =			ft_queue_tm_resp,
-	.get_fabric_sense_len =		ft_get_fabric_sense_len,
-	.set_fabric_sense_len =		ft_set_fabric_sense_len,
 	/*
 	 * Setup function pointers for generic logic in
 	 * target_core_fabric_configfs.c
diff --git a/drivers/target/tcm_fc/tfc_io.c b/drivers/target/tcm_fc/tfc_io.c
index ad36ede1a1e..b6fd4cf4284 100644
--- a/drivers/target/tcm_fc/tfc_io.c
+++ b/drivers/target/tcm_fc/tfc_io.c
@@ -28,7 +28,6 @@
 
 #include <linux/module.h>
 #include <linux/moduleparam.h>
-#include <generated/utsrelease.h>
 #include <linux/utsname.h>
 #include <linux/init.h>
 #include <linux/slab.h>
@@ -328,11 +327,12 @@ drop:
  */
 void ft_invl_hw_context(struct ft_cmd *cmd)
 {
-	struct fc_seq *seq = cmd->seq;
+	struct fc_seq *seq;
 	struct fc_exch *ep = NULL;
 	struct fc_lport *lport = NULL;
 
 	BUG_ON(!cmd);
+	seq = cmd->seq;
 
 	/* Cleanup the DDP context in HW if DDP was setup */
 	if (cmd->was_ddp_setup && seq) {
diff --git a/drivers/target/tcm_fc/tfc_sess.c b/drivers/target/tcm_fc/tfc_sess.c
index 3c9e5b57caa..9585010964e 100644
--- a/drivers/target/tcm_fc/tfc_sess.c
+++ b/drivers/target/tcm_fc/tfc_sess.c
@@ -19,7 +19,6 @@
 
 #include <linux/module.h>
 #include <linux/moduleparam.h>
-#include <generated/utsrelease.h>
 #include <linux/utsname.h>
 #include <linux/init.h>
 #include <linux/slab.h>
diff --git a/drivers/thermal/Kconfig b/drivers/thermal/Kconfig
index 3ab2bd540b5..edfd67d2501 100644
--- a/drivers/thermal/Kconfig
+++ b/drivers/thermal/Kconfig
@@ -19,6 +19,17 @@ config THERMAL_HWMON
 	depends on HWMON=y || HWMON=THERMAL
 	default y
 
+config CPU_THERMAL
+	bool "generic cpu cooling support"
+	depends on THERMAL && CPU_FREQ
+	help
+	  This implements the generic cpu cooling mechanism through frequency
+	  reduction, cpu hotplug and any other ways of reducing temperature. An
+	  ACPI version of this already exists(drivers/acpi/processor_thermal.c).
+	  This will be useful for platforms using the generic thermal interface
+	  and not the ACPI interface.
+	  If you want this support, you should say Y here.
+
 config SPEAR_THERMAL
 	bool "SPEAr thermal sensor driver"
 	depends on THERMAL
@@ -27,3 +38,18 @@ config SPEAR_THERMAL
 	help
 	  Enable this to plug the SPEAr thermal sensor driver into the Linux
 	  thermal framework
+
+config RCAR_THERMAL
+	tristate "Renesas R-Car thermal driver"
+	depends on THERMAL
+	depends on ARCH_SHMOBILE
+	help
+	  Enable this to plug the R-Car thermal sensor driver into the Linux
+	  thermal framework
+
+config EXYNOS_THERMAL
+	tristate "Temperature sensor on Samsung EXYNOS"
+	depends on (ARCH_EXYNOS4 || ARCH_EXYNOS5) && THERMAL
+	help
+	  If you say yes here you get support for TMU (Thermal Managment
+	  Unit) on SAMSUNG EXYNOS series of SoC.
diff --git a/drivers/thermal/Makefile b/drivers/thermal/Makefile
index a9fff0bf4b1..885550dc64b 100644
--- a/drivers/thermal/Makefile
+++ b/drivers/thermal/Makefile
@@ -3,4 +3,7 @@
 #
 
 obj-$(CONFIG_THERMAL)		+= thermal_sys.o
-obj-$(CONFIG_SPEAR_THERMAL)		+= spear_thermal.o
\ No newline at end of file
+obj-$(CONFIG_CPU_THERMAL)		+= cpu_cooling.o
+obj-$(CONFIG_SPEAR_THERMAL)		+= spear_thermal.o
+obj-$(CONFIG_RCAR_THERMAL)	+= rcar_thermal.o
+obj-$(CONFIG_EXYNOS_THERMAL)		+= exynos_thermal.o
diff --git a/drivers/thermal/cpu_cooling.c b/drivers/thermal/cpu_cooling.c
new file mode 100644
index 00000000000..cc1c930a90e
--- /dev/null
+++ b/drivers/thermal/cpu_cooling.c
@@ -0,0 +1,449 @@
+/*
+ *  linux/drivers/thermal/cpu_cooling.c
+ *
+ *  Copyright (C) 2012	Samsung Electronics Co., Ltd(http://www.samsung.com)
+ *  Copyright (C) 2012  Amit Daniel <amit.kachhap@linaro.org>
+ *
+ * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ *  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; version 2 of the License.
+ *
+ *  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 <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/thermal.h>
+#include <linux/platform_device.h>
+#include <linux/cpufreq.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/cpu.h>
+#include <linux/cpu_cooling.h>
+
+/**
+ * struct cpufreq_cooling_device
+ * @id: unique integer value corresponding to each cpufreq_cooling_device
+ *	registered.
+ * @cool_dev: thermal_cooling_device pointer to keep track of the the
+ *	egistered cooling device.
+ * @cpufreq_state: integer value representing the current state of cpufreq
+ *	cooling	devices.
+ * @cpufreq_val: integer value representing the absolute value of the clipped
+ *	frequency.
+ * @allowed_cpus: all the cpus involved for this cpufreq_cooling_device.
+ * @node: list_head to link all cpufreq_cooling_device together.
+ *
+ * This structure is required for keeping information of each
+ * cpufreq_cooling_device registered as a list whose head is represented by
+ * cooling_cpufreq_list. In order to prevent corruption of this list a
+ * mutex lock cooling_cpufreq_lock is used.
+ */
+struct cpufreq_cooling_device {
+	int id;
+	struct thermal_cooling_device *cool_dev;
+	unsigned int cpufreq_state;
+	unsigned int cpufreq_val;
+	struct cpumask allowed_cpus;
+	struct list_head node;
+};
+static LIST_HEAD(cooling_cpufreq_list);
+static DEFINE_IDR(cpufreq_idr);
+
+static struct mutex cooling_cpufreq_lock;
+
+/* notify_table passes value to the CPUFREQ_ADJUST callback function. */
+#define NOTIFY_INVALID NULL
+struct cpufreq_cooling_device *notify_device;
+
+/**
+ * get_idr - function to get a unique id.
+ * @idr: struct idr * handle used to create a id.
+ * @id: int * value generated by this function.
+ */
+static int get_idr(struct idr *idr, int *id)
+{
+	int err;
+again:
+	if (unlikely(idr_pre_get(idr, GFP_KERNEL) == 0))
+		return -ENOMEM;
+
+	mutex_lock(&cooling_cpufreq_lock);
+	err = idr_get_new(idr, NULL, id);
+	mutex_unlock(&cooling_cpufreq_lock);
+
+	if (unlikely(err == -EAGAIN))
+		goto again;
+	else if (unlikely(err))
+		return err;
+
+	*id = *id & MAX_IDR_MASK;
+	return 0;
+}
+
+/**
+ * release_idr - function to free the unique id.
+ * @idr: struct idr * handle used for creating the id.
+ * @id: int value representing the unique id.
+ */
+static void release_idr(struct idr *idr, int id)
+{
+	mutex_lock(&cooling_cpufreq_lock);
+	idr_remove(idr, id);
+	mutex_unlock(&cooling_cpufreq_lock);
+}
+
+/* Below code defines functions to be used for cpufreq as cooling device */
+
+/**
+ * is_cpufreq_valid - function to check if a cpu has frequency transition policy.
+ * @cpu: cpu for which check is needed.
+ */
+static int is_cpufreq_valid(int cpu)
+{
+	struct cpufreq_policy policy;
+	return !cpufreq_get_policy(&policy, cpu);
+}
+
+/**
+ * get_cpu_frequency - get the absolute value of frequency from level.
+ * @cpu: cpu for which frequency is fetched.
+ * @level: level of frequency of the CPU
+ *	e.g level=1 --> 1st MAX FREQ, LEVEL=2 ---> 2nd MAX FREQ, .... etc
+ */
+static unsigned int get_cpu_frequency(unsigned int cpu, unsigned long level)
+{
+	int ret = 0, i = 0;
+	unsigned long level_index;
+	bool descend = false;
+	struct cpufreq_frequency_table *table =
+					cpufreq_frequency_get_table(cpu);
+	if (!table)
+		return ret;
+
+	while (table[i].frequency != CPUFREQ_TABLE_END) {
+		if (table[i].frequency == CPUFREQ_ENTRY_INVALID)
+			continue;
+
+		/*check if table in ascending or descending order*/
+		if ((table[i + 1].frequency != CPUFREQ_TABLE_END) &&
+			(table[i + 1].frequency < table[i].frequency)
+			&& !descend) {
+			descend = true;
+		}
+
+		/*return if level matched and table in descending order*/
+		if (descend && i == level)
+			return table[i].frequency;
+		i++;
+	}
+	i--;
+
+	if (level > i || descend)
+		return ret;
+	level_index = i - level;
+
+	/*Scan the table in reverse order and match the level*/
+	while (i >= 0) {
+		if (table[i].frequency == CPUFREQ_ENTRY_INVALID)
+			continue;
+		/*return if level matched*/
+		if (i == level_index)
+			return table[i].frequency;
+		i--;
+	}
+	return ret;
+}
+
+/**
+ * cpufreq_apply_cooling - function to apply frequency clipping.
+ * @cpufreq_device: cpufreq_cooling_device pointer containing frequency
+ *	clipping data.
+ * @cooling_state: value of the cooling state.
+ */
+static int cpufreq_apply_cooling(struct cpufreq_cooling_device *cpufreq_device,
+				unsigned long cooling_state)
+{
+	unsigned int cpuid, clip_freq;
+	struct cpumask *maskPtr = &cpufreq_device->allowed_cpus;
+	unsigned int cpu = cpumask_any(maskPtr);
+
+
+	/* Check if the old cooling action is same as new cooling action */
+	if (cpufreq_device->cpufreq_state == cooling_state)
+		return 0;
+
+	clip_freq = get_cpu_frequency(cpu, cooling_state);
+	if (!clip_freq)
+		return -EINVAL;
+
+	cpufreq_device->cpufreq_state = cooling_state;
+	cpufreq_device->cpufreq_val = clip_freq;
+	notify_device = cpufreq_device;
+
+	for_each_cpu(cpuid, maskPtr) {
+		if (is_cpufreq_valid(cpuid))
+			cpufreq_update_policy(cpuid);
+	}
+
+	notify_device = NOTIFY_INVALID;
+
+	return 0;
+}
+
+/**
+ * cpufreq_thermal_notifier - notifier callback for cpufreq policy change.
+ * @nb:	struct notifier_block * with callback info.
+ * @event: value showing cpufreq event for which this function invoked.
+ * @data: callback-specific data
+ */
+static int cpufreq_thermal_notifier(struct notifier_block *nb,
+					unsigned long event, void *data)
+{
+	struct cpufreq_policy *policy = data;
+	unsigned long max_freq = 0;
+
+	if (event != CPUFREQ_ADJUST || notify_device == NOTIFY_INVALID)
+		return 0;
+
+	if (cpumask_test_cpu(policy->cpu, &notify_device->allowed_cpus))
+		max_freq = notify_device->cpufreq_val;
+
+	/* Never exceed user_policy.max*/
+	if (max_freq > policy->user_policy.max)
+		max_freq = policy->user_policy.max;
+
+	if (policy->max != max_freq)
+		cpufreq_verify_within_limits(policy, 0, max_freq);
+
+	return 0;
+}
+
+/*
+ * cpufreq cooling device callback functions are defined below
+ */
+
+/**
+ * cpufreq_get_max_state - callback function to get the max cooling state.
+ * @cdev: thermal cooling device pointer.
+ * @state: fill this variable with the max cooling state.
+ */
+static int cpufreq_get_max_state(struct thermal_cooling_device *cdev,
+				 unsigned long *state)
+{
+	int ret = -EINVAL, i = 0;
+	struct cpufreq_cooling_device *cpufreq_device;
+	struct cpumask *maskPtr;
+	unsigned int cpu;
+	struct cpufreq_frequency_table *table;
+
+	mutex_lock(&cooling_cpufreq_lock);
+	list_for_each_entry(cpufreq_device, &cooling_cpufreq_list, node) {
+		if (cpufreq_device && cpufreq_device->cool_dev == cdev)
+			break;
+	}
+	if (cpufreq_device == NULL)
+		goto return_get_max_state;
+
+	maskPtr = &cpufreq_device->allowed_cpus;
+	cpu = cpumask_any(maskPtr);
+	table = cpufreq_frequency_get_table(cpu);
+	if (!table) {
+		*state = 0;
+		ret = 0;
+		goto return_get_max_state;
+	}
+
+	while (table[i].frequency != CPUFREQ_TABLE_END) {
+		if (table[i].frequency == CPUFREQ_ENTRY_INVALID)
+			continue;
+		i++;
+	}
+	if (i > 0) {
+		*state = --i;
+		ret = 0;
+	}
+
+return_get_max_state:
+	mutex_unlock(&cooling_cpufreq_lock);
+	return ret;
+}
+
+/**
+ * cpufreq_get_cur_state - callback function to get the current cooling state.
+ * @cdev: thermal cooling device pointer.
+ * @state: fill this variable with the current cooling state.
+ */
+static int cpufreq_get_cur_state(struct thermal_cooling_device *cdev,
+				 unsigned long *state)
+{
+	int ret = -EINVAL;
+	struct cpufreq_cooling_device *cpufreq_device;
+
+	mutex_lock(&cooling_cpufreq_lock);
+	list_for_each_entry(cpufreq_device, &cooling_cpufreq_list, node) {
+		if (cpufreq_device && cpufreq_device->cool_dev == cdev) {
+			*state = cpufreq_device->cpufreq_state;
+			ret = 0;
+			break;
+		}
+	}
+	mutex_unlock(&cooling_cpufreq_lock);
+
+	return ret;
+}
+
+/**
+ * cpufreq_set_cur_state - callback function to set the current cooling state.
+ * @cdev: thermal cooling device pointer.
+ * @state: set this variable to the current cooling state.
+ */
+static int cpufreq_set_cur_state(struct thermal_cooling_device *cdev,
+				 unsigned long state)
+{
+	int ret = -EINVAL;
+	struct cpufreq_cooling_device *cpufreq_device;
+
+	mutex_lock(&cooling_cpufreq_lock);
+	list_for_each_entry(cpufreq_device, &cooling_cpufreq_list, node) {
+		if (cpufreq_device && cpufreq_device->cool_dev == cdev) {
+			ret = 0;
+			break;
+		}
+	}
+	if (!ret)
+		ret = cpufreq_apply_cooling(cpufreq_device, state);
+
+	mutex_unlock(&cooling_cpufreq_lock);
+
+	return ret;
+}
+
+/* Bind cpufreq callbacks to thermal cooling device ops */
+static struct thermal_cooling_device_ops const cpufreq_cooling_ops = {
+	.get_max_state = cpufreq_get_max_state,
+	.get_cur_state = cpufreq_get_cur_state,
+	.set_cur_state = cpufreq_set_cur_state,
+};
+
+/* Notifier for cpufreq policy change */
+static struct notifier_block thermal_cpufreq_notifier_block = {
+	.notifier_call = cpufreq_thermal_notifier,
+};
+
+/**
+ * cpufreq_cooling_register - function to create cpufreq cooling device.
+ * @clip_cpus: cpumask of cpus where the frequency constraints will happen.
+ */
+struct thermal_cooling_device *cpufreq_cooling_register(
+	struct cpumask *clip_cpus)
+{
+	struct thermal_cooling_device *cool_dev;
+	struct cpufreq_cooling_device *cpufreq_dev = NULL;
+	unsigned int cpufreq_dev_count = 0, min = 0, max = 0;
+	char dev_name[THERMAL_NAME_LENGTH];
+	int ret = 0, i;
+	struct cpufreq_policy policy;
+
+	list_for_each_entry(cpufreq_dev, &cooling_cpufreq_list, node)
+		cpufreq_dev_count++;
+
+	/*Verify that all the clip cpus have same freq_min, freq_max limit*/
+	for_each_cpu(i, clip_cpus) {
+		/*continue if cpufreq policy not found and not return error*/
+		if (!cpufreq_get_policy(&policy, i))
+			continue;
+		if (min == 0 && max == 0) {
+			min = policy.cpuinfo.min_freq;
+			max = policy.cpuinfo.max_freq;
+		} else {
+			if (min != policy.cpuinfo.min_freq ||
+				max != policy.cpuinfo.max_freq)
+				return ERR_PTR(-EINVAL);
+}
+	}
+	cpufreq_dev = kzalloc(sizeof(struct cpufreq_cooling_device),
+			GFP_KERNEL);
+	if (!cpufreq_dev)
+		return ERR_PTR(-ENOMEM);
+
+	cpumask_copy(&cpufreq_dev->allowed_cpus, clip_cpus);
+
+	if (cpufreq_dev_count == 0)
+		mutex_init(&cooling_cpufreq_lock);
+
+	ret = get_idr(&cpufreq_idr, &cpufreq_dev->id);
+	if (ret) {
+		kfree(cpufreq_dev);
+		return ERR_PTR(-EINVAL);
+	}
+
+	sprintf(dev_name, "thermal-cpufreq-%d", cpufreq_dev->id);
+
+	cool_dev = thermal_cooling_device_register(dev_name, cpufreq_dev,
+						&cpufreq_cooling_ops);
+	if (!cool_dev) {
+		release_idr(&cpufreq_idr, cpufreq_dev->id);
+		kfree(cpufreq_dev);
+		return ERR_PTR(-EINVAL);
+	}
+	cpufreq_dev->cool_dev = cool_dev;
+	cpufreq_dev->cpufreq_state = 0;
+	mutex_lock(&cooling_cpufreq_lock);
+	list_add_tail(&cpufreq_dev->node, &cooling_cpufreq_list);
+
+	/* Register the notifier for first cpufreq cooling device */
+	if (cpufreq_dev_count == 0)
+		cpufreq_register_notifier(&thermal_cpufreq_notifier_block,
+						CPUFREQ_POLICY_NOTIFIER);
+
+	mutex_unlock(&cooling_cpufreq_lock);
+	return cool_dev;
+}
+EXPORT_SYMBOL(cpufreq_cooling_register);
+
+/**
+ * cpufreq_cooling_unregister - function to remove cpufreq cooling device.
+ * @cdev: thermal cooling device pointer.
+ */
+void cpufreq_cooling_unregister(struct thermal_cooling_device *cdev)
+{
+	struct cpufreq_cooling_device *cpufreq_dev = NULL;
+	unsigned int cpufreq_dev_count = 0;
+
+	mutex_lock(&cooling_cpufreq_lock);
+	list_for_each_entry(cpufreq_dev, &cooling_cpufreq_list, node) {
+		if (cpufreq_dev && cpufreq_dev->cool_dev == cdev)
+			break;
+		cpufreq_dev_count++;
+	}
+
+	if (!cpufreq_dev || cpufreq_dev->cool_dev != cdev) {
+		mutex_unlock(&cooling_cpufreq_lock);
+		return;
+	}
+
+	list_del(&cpufreq_dev->node);
+
+	/* Unregister the notifier for the last cpufreq cooling device */
+	if (cpufreq_dev_count == 1) {
+		cpufreq_unregister_notifier(&thermal_cpufreq_notifier_block,
+					CPUFREQ_POLICY_NOTIFIER);
+	}
+	mutex_unlock(&cooling_cpufreq_lock);
+	thermal_cooling_device_unregister(cpufreq_dev->cool_dev);
+	release_idr(&cpufreq_idr, cpufreq_dev->id);
+	if (cpufreq_dev_count == 1)
+		mutex_destroy(&cooling_cpufreq_lock);
+	kfree(cpufreq_dev);
+}
+EXPORT_SYMBOL(cpufreq_cooling_unregister);
diff --git a/drivers/thermal/exynos_thermal.c b/drivers/thermal/exynos_thermal.c
new file mode 100644
index 00000000000..fd03e8581af
--- /dev/null
+++ b/drivers/thermal/exynos_thermal.c
@@ -0,0 +1,997 @@
+/*
+ * exynos_thermal.c - Samsung EXYNOS TMU (Thermal Management Unit)
+ *
+ *  Copyright (C) 2011 Samsung Electronics
+ *  Donggeun Kim <dg77.kim@samsung.com>
+ *  Amit Daniel Kachhap <amit.kachhap@linaro.org>
+ *
+ * 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 <linux/module.h>
+#include <linux/err.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/clk.h>
+#include <linux/workqueue.h>
+#include <linux/sysfs.h>
+#include <linux/kobject.h>
+#include <linux/io.h>
+#include <linux/mutex.h>
+#include <linux/platform_data/exynos_thermal.h>
+#include <linux/thermal.h>
+#include <linux/cpufreq.h>
+#include <linux/cpu_cooling.h>
+#include <linux/of.h>
+
+#include <plat/cpu.h>
+
+/* Exynos generic registers */
+#define EXYNOS_TMU_REG_TRIMINFO		0x0
+#define EXYNOS_TMU_REG_CONTROL		0x20
+#define EXYNOS_TMU_REG_STATUS		0x28
+#define EXYNOS_TMU_REG_CURRENT_TEMP	0x40
+#define EXYNOS_TMU_REG_INTEN		0x70
+#define EXYNOS_TMU_REG_INTSTAT		0x74
+#define EXYNOS_TMU_REG_INTCLEAR		0x78
+
+#define EXYNOS_TMU_TRIM_TEMP_MASK	0xff
+#define EXYNOS_TMU_GAIN_SHIFT		8
+#define EXYNOS_TMU_REF_VOLTAGE_SHIFT	24
+#define EXYNOS_TMU_CORE_ON		3
+#define EXYNOS_TMU_CORE_OFF		2
+#define EXYNOS_TMU_DEF_CODE_TO_TEMP_OFFSET	50
+
+/* Exynos4210 specific registers */
+#define EXYNOS4210_TMU_REG_THRESHOLD_TEMP	0x44
+#define EXYNOS4210_TMU_REG_TRIG_LEVEL0	0x50
+#define EXYNOS4210_TMU_REG_TRIG_LEVEL1	0x54
+#define EXYNOS4210_TMU_REG_TRIG_LEVEL2	0x58
+#define EXYNOS4210_TMU_REG_TRIG_LEVEL3	0x5C
+#define EXYNOS4210_TMU_REG_PAST_TEMP0	0x60
+#define EXYNOS4210_TMU_REG_PAST_TEMP1	0x64
+#define EXYNOS4210_TMU_REG_PAST_TEMP2	0x68
+#define EXYNOS4210_TMU_REG_PAST_TEMP3	0x6C
+
+#define EXYNOS4210_TMU_TRIG_LEVEL0_MASK	0x1
+#define EXYNOS4210_TMU_TRIG_LEVEL1_MASK	0x10
+#define EXYNOS4210_TMU_TRIG_LEVEL2_MASK	0x100
+#define EXYNOS4210_TMU_TRIG_LEVEL3_MASK	0x1000
+#define EXYNOS4210_TMU_INTCLEAR_VAL	0x1111
+
+/* Exynos5250 and Exynos4412 specific registers */
+#define EXYNOS_TMU_TRIMINFO_CON	0x14
+#define EXYNOS_THD_TEMP_RISE		0x50
+#define EXYNOS_THD_TEMP_FALL		0x54
+#define EXYNOS_EMUL_CON		0x80
+
+#define EXYNOS_TRIMINFO_RELOAD		0x1
+#define EXYNOS_TMU_CLEAR_RISE_INT	0x111
+#define EXYNOS_TMU_CLEAR_FALL_INT	(0x111 << 16)
+#define EXYNOS_MUX_ADDR_VALUE		6
+#define EXYNOS_MUX_ADDR_SHIFT		20
+#define EXYNOS_TMU_TRIP_MODE_SHIFT	13
+
+#define EFUSE_MIN_VALUE 40
+#define EFUSE_MAX_VALUE 100
+
+/* In-kernel thermal framework related macros & definations */
+#define SENSOR_NAME_LEN	16
+#define MAX_TRIP_COUNT	8
+#define MAX_COOLING_DEVICE 4
+
+#define ACTIVE_INTERVAL 500
+#define IDLE_INTERVAL 10000
+#define MCELSIUS	1000
+
+/* CPU Zone information */
+#define PANIC_ZONE      4
+#define WARN_ZONE       3
+#define MONITOR_ZONE    2
+#define SAFE_ZONE       1
+
+#define GET_ZONE(trip) (trip + 2)
+#define GET_TRIP(zone) (zone - 2)
+
+#define EXYNOS_ZONE_COUNT	3
+
+struct exynos_tmu_data {
+	struct exynos_tmu_platform_data *pdata;
+	struct resource *mem;
+	void __iomem *base;
+	int irq;
+	enum soc_type soc;
+	struct work_struct irq_work;
+	struct mutex lock;
+	struct clk *clk;
+	u8 temp_error1, temp_error2;
+};
+
+struct	thermal_trip_point_conf {
+	int trip_val[MAX_TRIP_COUNT];
+	int trip_count;
+};
+
+struct	thermal_cooling_conf {
+	struct freq_clip_table freq_data[MAX_TRIP_COUNT];
+	int freq_clip_count;
+};
+
+struct thermal_sensor_conf {
+	char name[SENSOR_NAME_LEN];
+	int (*read_temperature)(void *data);
+	struct thermal_trip_point_conf trip_data;
+	struct thermal_cooling_conf cooling_data;
+	void *private_data;
+};
+
+struct exynos_thermal_zone {
+	enum thermal_device_mode mode;
+	struct thermal_zone_device *therm_dev;
+	struct thermal_cooling_device *cool_dev[MAX_COOLING_DEVICE];
+	unsigned int cool_dev_size;
+	struct platform_device *exynos4_dev;
+	struct thermal_sensor_conf *sensor_conf;
+	bool bind;
+};
+
+static struct exynos_thermal_zone *th_zone;
+static void exynos_unregister_thermal(void);
+static int exynos_register_thermal(struct thermal_sensor_conf *sensor_conf);
+
+/* Get mode callback functions for thermal zone */
+static int exynos_get_mode(struct thermal_zone_device *thermal,
+			enum thermal_device_mode *mode)
+{
+	if (th_zone)
+		*mode = th_zone->mode;
+	return 0;
+}
+
+/* Set mode callback functions for thermal zone */
+static int exynos_set_mode(struct thermal_zone_device *thermal,
+			enum thermal_device_mode mode)
+{
+	if (!th_zone->therm_dev) {
+		pr_notice("thermal zone not registered\n");
+		return 0;
+	}
+
+	mutex_lock(&th_zone->therm_dev->lock);
+
+	if (mode == THERMAL_DEVICE_ENABLED)
+		th_zone->therm_dev->polling_delay = IDLE_INTERVAL;
+	else
+		th_zone->therm_dev->polling_delay = 0;
+
+	mutex_unlock(&th_zone->therm_dev->lock);
+
+	th_zone->mode = mode;
+	thermal_zone_device_update(th_zone->therm_dev);
+	pr_info("thermal polling set for duration=%d msec\n",
+				th_zone->therm_dev->polling_delay);
+	return 0;
+}
+
+
+/* Get trip type callback functions for thermal zone */
+static int exynos_get_trip_type(struct thermal_zone_device *thermal, int trip,
+				 enum thermal_trip_type *type)
+{
+	switch (GET_ZONE(trip)) {
+	case MONITOR_ZONE:
+	case WARN_ZONE:
+		*type = THERMAL_TRIP_ACTIVE;
+		break;
+	case PANIC_ZONE:
+		*type = THERMAL_TRIP_CRITICAL;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/* Get trip temperature callback functions for thermal zone */
+static int exynos_get_trip_temp(struct thermal_zone_device *thermal, int trip,
+				unsigned long *temp)
+{
+	if (trip < GET_TRIP(MONITOR_ZONE) || trip > GET_TRIP(PANIC_ZONE))
+		return -EINVAL;
+
+	*temp = th_zone->sensor_conf->trip_data.trip_val[trip];
+	/* convert the temperature into millicelsius */
+	*temp = *temp * MCELSIUS;
+
+	return 0;
+}
+
+/* Get critical temperature callback functions for thermal zone */
+static int exynos_get_crit_temp(struct thermal_zone_device *thermal,
+				unsigned long *temp)
+{
+	int ret;
+	/* Panic zone */
+	ret = exynos_get_trip_temp(thermal, GET_TRIP(PANIC_ZONE), temp);
+	return ret;
+}
+
+static int exynos_get_frequency_level(unsigned int cpu, unsigned int freq)
+{
+	int i = 0, ret = -EINVAL;
+	struct cpufreq_frequency_table *table = NULL;
+#ifdef CONFIG_CPU_FREQ
+	table = cpufreq_frequency_get_table(cpu);
+#endif
+	if (!table)
+		return ret;
+
+	while (table[i].frequency != CPUFREQ_TABLE_END) {
+		if (table[i].frequency == CPUFREQ_ENTRY_INVALID)
+			continue;
+		if (table[i].frequency == freq)
+			return i;
+		i++;
+	}
+	return ret;
+}
+
+/* Bind callback functions for thermal zone */
+static int exynos_bind(struct thermal_zone_device *thermal,
+			struct thermal_cooling_device *cdev)
+{
+	int ret = 0, i, tab_size, level;
+	struct freq_clip_table *tab_ptr, *clip_data;
+	struct thermal_sensor_conf *data = th_zone->sensor_conf;
+
+	tab_ptr = (struct freq_clip_table *)data->cooling_data.freq_data;
+	tab_size = data->cooling_data.freq_clip_count;
+
+	if (tab_ptr == NULL || tab_size == 0)
+		return -EINVAL;
+
+	/* find the cooling device registered*/
+	for (i = 0; i < th_zone->cool_dev_size; i++)
+		if (cdev == th_zone->cool_dev[i])
+			break;
+
+	/* No matching cooling device */
+	if (i == th_zone->cool_dev_size)
+		return 0;
+
+	/* Bind the thermal zone to the cpufreq cooling device */
+	for (i = 0; i < tab_size; i++) {
+		clip_data = (struct freq_clip_table *)&(tab_ptr[i]);
+		level = exynos_get_frequency_level(0, clip_data->freq_clip_max);
+		if (level < 0)
+			return 0;
+		switch (GET_ZONE(i)) {
+		case MONITOR_ZONE:
+		case WARN_ZONE:
+			if (thermal_zone_bind_cooling_device(thermal, i, cdev,
+								level, level)) {
+				pr_err("error binding cdev inst %d\n", i);
+				ret = -EINVAL;
+			}
+			th_zone->bind = true;
+			break;
+		default:
+			ret = -EINVAL;
+		}
+	}
+
+	return ret;
+}
+
+/* Unbind callback functions for thermal zone */
+static int exynos_unbind(struct thermal_zone_device *thermal,
+			struct thermal_cooling_device *cdev)
+{
+	int ret = 0, i, tab_size;
+	struct thermal_sensor_conf *data = th_zone->sensor_conf;
+
+	if (th_zone->bind == false)
+		return 0;
+
+	tab_size = data->cooling_data.freq_clip_count;
+
+	if (tab_size == 0)
+		return -EINVAL;
+
+	/* find the cooling device registered*/
+	for (i = 0; i < th_zone->cool_dev_size; i++)
+		if (cdev == th_zone->cool_dev[i])
+			break;
+
+	/* No matching cooling device */
+	if (i == th_zone->cool_dev_size)
+		return 0;
+
+	/* Bind the thermal zone to the cpufreq cooling device */
+	for (i = 0; i < tab_size; i++) {
+		switch (GET_ZONE(i)) {
+		case MONITOR_ZONE:
+		case WARN_ZONE:
+			if (thermal_zone_unbind_cooling_device(thermal, i,
+								cdev)) {
+				pr_err("error unbinding cdev inst=%d\n", i);
+				ret = -EINVAL;
+			}
+			th_zone->bind = false;
+			break;
+		default:
+			ret = -EINVAL;
+		}
+	}
+	return ret;
+}
+
+/* Get temperature callback functions for thermal zone */
+static int exynos_get_temp(struct thermal_zone_device *thermal,
+			unsigned long *temp)
+{
+	void *data;
+
+	if (!th_zone->sensor_conf) {
+		pr_info("Temperature sensor not initialised\n");
+		return -EINVAL;
+	}
+	data = th_zone->sensor_conf->private_data;
+	*temp = th_zone->sensor_conf->read_temperature(data);
+	/* convert the temperature into millicelsius */
+	*temp = *temp * MCELSIUS;
+	return 0;
+}
+
+/* Get the temperature trend */
+static int exynos_get_trend(struct thermal_zone_device *thermal,
+			int trip, enum thermal_trend *trend)
+{
+	if (thermal->temperature >= trip)
+		*trend = THERMAL_TREND_RAISING;
+	else
+		*trend = THERMAL_TREND_DROPPING;
+
+	return 0;
+}
+/* Operation callback functions for thermal zone */
+static struct thermal_zone_device_ops const exynos_dev_ops = {
+	.bind = exynos_bind,
+	.unbind = exynos_unbind,
+	.get_temp = exynos_get_temp,
+	.get_trend = exynos_get_trend,
+	.get_mode = exynos_get_mode,
+	.set_mode = exynos_set_mode,
+	.get_trip_type = exynos_get_trip_type,
+	.get_trip_temp = exynos_get_trip_temp,
+	.get_crit_temp = exynos_get_crit_temp,
+};
+
+/*
+ * This function may be called from interrupt based temperature sensor
+ * when threshold is changed.
+ */
+static void exynos_report_trigger(void)
+{
+	unsigned int i;
+	char data[10];
+	char *envp[] = { data, NULL };
+
+	if (!th_zone || !th_zone->therm_dev)
+		return;
+	if (th_zone->bind == false) {
+		for (i = 0; i < th_zone->cool_dev_size; i++) {
+			if (!th_zone->cool_dev[i])
+				continue;
+			exynos_bind(th_zone->therm_dev,
+					th_zone->cool_dev[i]);
+		}
+	}
+
+	thermal_zone_device_update(th_zone->therm_dev);
+
+	mutex_lock(&th_zone->therm_dev->lock);
+	/* Find the level for which trip happened */
+	for (i = 0; i < th_zone->sensor_conf->trip_data.trip_count; i++) {
+		if (th_zone->therm_dev->last_temperature <
+			th_zone->sensor_conf->trip_data.trip_val[i] * MCELSIUS)
+			break;
+	}
+
+	if (th_zone->mode == THERMAL_DEVICE_ENABLED) {
+		if (i > 0)
+			th_zone->therm_dev->polling_delay = ACTIVE_INTERVAL;
+		else
+			th_zone->therm_dev->polling_delay = IDLE_INTERVAL;
+	}
+
+	snprintf(data, sizeof(data), "%u", i);
+	kobject_uevent_env(&th_zone->therm_dev->device.kobj, KOBJ_CHANGE, envp);
+	mutex_unlock(&th_zone->therm_dev->lock);
+}
+
+/* Register with the in-kernel thermal management */
+static int exynos_register_thermal(struct thermal_sensor_conf *sensor_conf)
+{
+	int ret;
+	struct cpumask mask_val;
+
+	if (!sensor_conf || !sensor_conf->read_temperature) {
+		pr_err("Temperature sensor not initialised\n");
+		return -EINVAL;
+	}
+
+	th_zone = kzalloc(sizeof(struct exynos_thermal_zone), GFP_KERNEL);
+	if (!th_zone)
+		return -ENOMEM;
+
+	th_zone->sensor_conf = sensor_conf;
+	cpumask_set_cpu(0, &mask_val);
+	th_zone->cool_dev[0] = cpufreq_cooling_register(&mask_val);
+	if (IS_ERR(th_zone->cool_dev[0])) {
+		pr_err("Failed to register cpufreq cooling device\n");
+		ret = -EINVAL;
+		goto err_unregister;
+	}
+	th_zone->cool_dev_size++;
+
+	th_zone->therm_dev = thermal_zone_device_register(sensor_conf->name,
+			EXYNOS_ZONE_COUNT, 0, NULL, &exynos_dev_ops, 0,
+			IDLE_INTERVAL);
+
+	if (IS_ERR(th_zone->therm_dev)) {
+		pr_err("Failed to register thermal zone device\n");
+		ret = -EINVAL;
+		goto err_unregister;
+	}
+	th_zone->mode = THERMAL_DEVICE_ENABLED;
+
+	pr_info("Exynos: Kernel Thermal management registered\n");
+
+	return 0;
+
+err_unregister:
+	exynos_unregister_thermal();
+	return ret;
+}
+
+/* Un-Register with the in-kernel thermal management */
+static void exynos_unregister_thermal(void)
+{
+	int i;
+
+	if (!th_zone)
+		return;
+
+	if (th_zone->therm_dev)
+		thermal_zone_device_unregister(th_zone->therm_dev);
+
+	for (i = 0; i < th_zone->cool_dev_size; i++) {
+		if (th_zone->cool_dev[i])
+			cpufreq_cooling_unregister(th_zone->cool_dev[i]);
+	}
+
+	kfree(th_zone);
+	pr_info("Exynos: Kernel Thermal management unregistered\n");
+}
+
+/*
+ * TMU treats temperature as a mapped temperature code.
+ * The temperature is converted differently depending on the calibration type.
+ */
+static int temp_to_code(struct exynos_tmu_data *data, u8 temp)
+{
+	struct exynos_tmu_platform_data *pdata = data->pdata;
+	int temp_code;
+
+	if (data->soc == SOC_ARCH_EXYNOS4210)
+		/* temp should range between 25 and 125 */
+		if (temp < 25 || temp > 125) {
+			temp_code = -EINVAL;
+			goto out;
+		}
+
+	switch (pdata->cal_type) {
+	case TYPE_TWO_POINT_TRIMMING:
+		temp_code = (temp - 25) *
+		    (data->temp_error2 - data->temp_error1) /
+		    (85 - 25) + data->temp_error1;
+		break;
+	case TYPE_ONE_POINT_TRIMMING:
+		temp_code = temp + data->temp_error1 - 25;
+		break;
+	default:
+		temp_code = temp + EXYNOS_TMU_DEF_CODE_TO_TEMP_OFFSET;
+		break;
+	}
+out:
+	return temp_code;
+}
+
+/*
+ * Calculate a temperature value from a temperature code.
+ * The unit of the temperature is degree Celsius.
+ */
+static int code_to_temp(struct exynos_tmu_data *data, u8 temp_code)
+{
+	struct exynos_tmu_platform_data *pdata = data->pdata;
+	int temp;
+
+	if (data->soc == SOC_ARCH_EXYNOS4210)
+		/* temp_code should range between 75 and 175 */
+		if (temp_code < 75 || temp_code > 175) {
+			temp = -ENODATA;
+			goto out;
+		}
+
+	switch (pdata->cal_type) {
+	case TYPE_TWO_POINT_TRIMMING:
+		temp = (temp_code - data->temp_error1) * (85 - 25) /
+		    (data->temp_error2 - data->temp_error1) + 25;
+		break;
+	case TYPE_ONE_POINT_TRIMMING:
+		temp = temp_code - data->temp_error1 + 25;
+		break;
+	default:
+		temp = temp_code - EXYNOS_TMU_DEF_CODE_TO_TEMP_OFFSET;
+		break;
+	}
+out:
+	return temp;
+}
+
+static int exynos_tmu_initialize(struct platform_device *pdev)
+{
+	struct exynos_tmu_data *data = platform_get_drvdata(pdev);
+	struct exynos_tmu_platform_data *pdata = data->pdata;
+	unsigned int status, trim_info, rising_threshold;
+	int ret = 0, threshold_code;
+
+	mutex_lock(&data->lock);
+	clk_enable(data->clk);
+
+	status = readb(data->base + EXYNOS_TMU_REG_STATUS);
+	if (!status) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	if (data->soc == SOC_ARCH_EXYNOS) {
+		__raw_writel(EXYNOS_TRIMINFO_RELOAD,
+				data->base + EXYNOS_TMU_TRIMINFO_CON);
+	}
+	/* Save trimming info in order to perform calibration */
+	trim_info = readl(data->base + EXYNOS_TMU_REG_TRIMINFO);
+	data->temp_error1 = trim_info & EXYNOS_TMU_TRIM_TEMP_MASK;
+	data->temp_error2 = ((trim_info >> 8) & EXYNOS_TMU_TRIM_TEMP_MASK);
+
+	if ((EFUSE_MIN_VALUE > data->temp_error1) ||
+			(data->temp_error1 > EFUSE_MAX_VALUE) ||
+			(data->temp_error2 != 0))
+		data->temp_error1 = pdata->efuse_value;
+
+	if (data->soc == SOC_ARCH_EXYNOS4210) {
+		/* Write temperature code for threshold */
+		threshold_code = temp_to_code(data, pdata->threshold);
+		if (threshold_code < 0) {
+			ret = threshold_code;
+			goto out;
+		}
+		writeb(threshold_code,
+			data->base + EXYNOS4210_TMU_REG_THRESHOLD_TEMP);
+
+		writeb(pdata->trigger_levels[0],
+			data->base + EXYNOS4210_TMU_REG_TRIG_LEVEL0);
+		writeb(pdata->trigger_levels[1],
+			data->base + EXYNOS4210_TMU_REG_TRIG_LEVEL1);
+		writeb(pdata->trigger_levels[2],
+			data->base + EXYNOS4210_TMU_REG_TRIG_LEVEL2);
+		writeb(pdata->trigger_levels[3],
+			data->base + EXYNOS4210_TMU_REG_TRIG_LEVEL3);
+
+		writel(EXYNOS4210_TMU_INTCLEAR_VAL,
+			data->base + EXYNOS_TMU_REG_INTCLEAR);
+	} else if (data->soc == SOC_ARCH_EXYNOS) {
+		/* Write temperature code for threshold */
+		threshold_code = temp_to_code(data, pdata->trigger_levels[0]);
+		if (threshold_code < 0) {
+			ret = threshold_code;
+			goto out;
+		}
+		rising_threshold = threshold_code;
+		threshold_code = temp_to_code(data, pdata->trigger_levels[1]);
+		if (threshold_code < 0) {
+			ret = threshold_code;
+			goto out;
+		}
+		rising_threshold |= (threshold_code << 8);
+		threshold_code = temp_to_code(data, pdata->trigger_levels[2]);
+		if (threshold_code < 0) {
+			ret = threshold_code;
+			goto out;
+		}
+		rising_threshold |= (threshold_code << 16);
+
+		writel(rising_threshold,
+				data->base + EXYNOS_THD_TEMP_RISE);
+		writel(0, data->base + EXYNOS_THD_TEMP_FALL);
+
+		writel(EXYNOS_TMU_CLEAR_RISE_INT|EXYNOS_TMU_CLEAR_FALL_INT,
+				data->base + EXYNOS_TMU_REG_INTCLEAR);
+	}
+out:
+	clk_disable(data->clk);
+	mutex_unlock(&data->lock);
+
+	return ret;
+}
+
+static void exynos_tmu_control(struct platform_device *pdev, bool on)
+{
+	struct exynos_tmu_data *data = platform_get_drvdata(pdev);
+	struct exynos_tmu_platform_data *pdata = data->pdata;
+	unsigned int con, interrupt_en;
+
+	mutex_lock(&data->lock);
+	clk_enable(data->clk);
+
+	con = pdata->reference_voltage << EXYNOS_TMU_REF_VOLTAGE_SHIFT |
+		pdata->gain << EXYNOS_TMU_GAIN_SHIFT;
+
+	if (data->soc == SOC_ARCH_EXYNOS) {
+		con |= pdata->noise_cancel_mode << EXYNOS_TMU_TRIP_MODE_SHIFT;
+		con |= (EXYNOS_MUX_ADDR_VALUE << EXYNOS_MUX_ADDR_SHIFT);
+	}
+
+	if (on) {
+		con |= EXYNOS_TMU_CORE_ON;
+		interrupt_en = pdata->trigger_level3_en << 12 |
+			pdata->trigger_level2_en << 8 |
+			pdata->trigger_level1_en << 4 |
+			pdata->trigger_level0_en;
+	} else {
+		con |= EXYNOS_TMU_CORE_OFF;
+		interrupt_en = 0; /* Disable all interrupts */
+	}
+	writel(interrupt_en, data->base + EXYNOS_TMU_REG_INTEN);
+	writel(con, data->base + EXYNOS_TMU_REG_CONTROL);
+
+	clk_disable(data->clk);
+	mutex_unlock(&data->lock);
+}
+
+static int exynos_tmu_read(struct exynos_tmu_data *data)
+{
+	u8 temp_code;
+	int temp;
+
+	mutex_lock(&data->lock);
+	clk_enable(data->clk);
+
+	temp_code = readb(data->base + EXYNOS_TMU_REG_CURRENT_TEMP);
+	temp = code_to_temp(data, temp_code);
+
+	clk_disable(data->clk);
+	mutex_unlock(&data->lock);
+
+	return temp;
+}
+
+static void exynos_tmu_work(struct work_struct *work)
+{
+	struct exynos_tmu_data *data = container_of(work,
+			struct exynos_tmu_data, irq_work);
+
+	mutex_lock(&data->lock);
+	clk_enable(data->clk);
+
+
+	if (data->soc == SOC_ARCH_EXYNOS)
+		writel(EXYNOS_TMU_CLEAR_RISE_INT,
+				data->base + EXYNOS_TMU_REG_INTCLEAR);
+	else
+		writel(EXYNOS4210_TMU_INTCLEAR_VAL,
+				data->base + EXYNOS_TMU_REG_INTCLEAR);
+
+	clk_disable(data->clk);
+	mutex_unlock(&data->lock);
+	exynos_report_trigger();
+	enable_irq(data->irq);
+}
+
+static irqreturn_t exynos_tmu_irq(int irq, void *id)
+{
+	struct exynos_tmu_data *data = id;
+
+	disable_irq_nosync(irq);
+	schedule_work(&data->irq_work);
+
+	return IRQ_HANDLED;
+}
+static struct thermal_sensor_conf exynos_sensor_conf = {
+	.name			= "exynos-therm",
+	.read_temperature	= (int (*)(void *))exynos_tmu_read,
+};
+
+#if defined(CONFIG_CPU_EXYNOS4210)
+static struct exynos_tmu_platform_data const exynos4210_default_tmu_data = {
+	.threshold = 80,
+	.trigger_levels[0] = 5,
+	.trigger_levels[1] = 20,
+	.trigger_levels[2] = 30,
+	.trigger_level0_en = 1,
+	.trigger_level1_en = 1,
+	.trigger_level2_en = 1,
+	.trigger_level3_en = 0,
+	.gain = 15,
+	.reference_voltage = 7,
+	.cal_type = TYPE_ONE_POINT_TRIMMING,
+	.freq_tab[0] = {
+		.freq_clip_max = 800 * 1000,
+		.temp_level = 85,
+	},
+	.freq_tab[1] = {
+		.freq_clip_max = 200 * 1000,
+		.temp_level = 100,
+	},
+	.freq_tab_count = 2,
+	.type = SOC_ARCH_EXYNOS4210,
+};
+#define EXYNOS4210_TMU_DRV_DATA (&exynos4210_default_tmu_data)
+#else
+#define EXYNOS4210_TMU_DRV_DATA (NULL)
+#endif
+
+#if defined(CONFIG_SOC_EXYNOS5250) || defined(CONFIG_SOC_EXYNOS4412)
+static struct exynos_tmu_platform_data const exynos_default_tmu_data = {
+	.trigger_levels[0] = 85,
+	.trigger_levels[1] = 103,
+	.trigger_levels[2] = 110,
+	.trigger_level0_en = 1,
+	.trigger_level1_en = 1,
+	.trigger_level2_en = 1,
+	.trigger_level3_en = 0,
+	.gain = 8,
+	.reference_voltage = 16,
+	.noise_cancel_mode = 4,
+	.cal_type = TYPE_ONE_POINT_TRIMMING,
+	.efuse_value = 55,
+	.freq_tab[0] = {
+		.freq_clip_max = 800 * 1000,
+		.temp_level = 85,
+	},
+	.freq_tab[1] = {
+		.freq_clip_max = 200 * 1000,
+		.temp_level = 103,
+	},
+	.freq_tab_count = 2,
+	.type = SOC_ARCH_EXYNOS,
+};
+#define EXYNOS_TMU_DRV_DATA (&exynos_default_tmu_data)
+#else
+#define EXYNOS_TMU_DRV_DATA (NULL)
+#endif
+
+#ifdef CONFIG_OF
+static const struct of_device_id exynos_tmu_match[] = {
+	{
+		.compatible = "samsung,exynos4210-tmu",
+		.data = (void *)EXYNOS4210_TMU_DRV_DATA,
+	},
+	{
+		.compatible = "samsung,exynos5250-tmu",
+		.data = (void *)EXYNOS_TMU_DRV_DATA,
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, exynos_tmu_match);
+#else
+#define  exynos_tmu_match NULL
+#endif
+
+static struct platform_device_id exynos_tmu_driver_ids[] = {
+	{
+		.name		= "exynos4210-tmu",
+		.driver_data    = (kernel_ulong_t)EXYNOS4210_TMU_DRV_DATA,
+	},
+	{
+		.name		= "exynos5250-tmu",
+		.driver_data    = (kernel_ulong_t)EXYNOS_TMU_DRV_DATA,
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, exynos4_tmu_driver_ids);
+
+static inline struct  exynos_tmu_platform_data *exynos_get_driver_data(
+			struct platform_device *pdev)
+{
+#ifdef CONFIG_OF
+	if (pdev->dev.of_node) {
+		const struct of_device_id *match;
+		match = of_match_node(exynos_tmu_match, pdev->dev.of_node);
+		if (!match)
+			return NULL;
+		return (struct exynos_tmu_platform_data *) match->data;
+	}
+#endif
+	return (struct exynos_tmu_platform_data *)
+			platform_get_device_id(pdev)->driver_data;
+}
+static int __devinit exynos_tmu_probe(struct platform_device *pdev)
+{
+	struct exynos_tmu_data *data;
+	struct exynos_tmu_platform_data *pdata = pdev->dev.platform_data;
+	int ret, i;
+
+	if (!pdata)
+		pdata = exynos_get_driver_data(pdev);
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "No platform init data supplied.\n");
+		return -ENODEV;
+	}
+	data = devm_kzalloc(&pdev->dev, sizeof(struct exynos_tmu_data),
+					GFP_KERNEL);
+	if (!data) {
+		dev_err(&pdev->dev, "Failed to allocate driver structure\n");
+		return -ENOMEM;
+	}
+
+	data->irq = platform_get_irq(pdev, 0);
+	if (data->irq < 0) {
+		dev_err(&pdev->dev, "Failed to get platform irq\n");
+		return data->irq;
+	}
+
+	INIT_WORK(&data->irq_work, exynos_tmu_work);
+
+	data->mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!data->mem) {
+		dev_err(&pdev->dev, "Failed to get platform resource\n");
+		return -ENOENT;
+	}
+
+	data->base = devm_request_and_ioremap(&pdev->dev, data->mem);
+	if (!data->base) {
+		dev_err(&pdev->dev, "Failed to ioremap memory\n");
+		return -ENODEV;
+	}
+
+	ret = devm_request_irq(&pdev->dev, data->irq, exynos_tmu_irq,
+		IRQF_TRIGGER_RISING, "exynos-tmu", data);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to request irq: %d\n", data->irq);
+		return ret;
+	}
+
+	data->clk = clk_get(NULL, "tmu_apbif");
+	if (IS_ERR(data->clk)) {
+		dev_err(&pdev->dev, "Failed to get clock\n");
+		return  PTR_ERR(data->clk);
+	}
+
+	if (pdata->type == SOC_ARCH_EXYNOS ||
+				pdata->type == SOC_ARCH_EXYNOS4210)
+		data->soc = pdata->type;
+	else {
+		ret = -EINVAL;
+		dev_err(&pdev->dev, "Platform not supported\n");
+		goto err_clk;
+	}
+
+	data->pdata = pdata;
+	platform_set_drvdata(pdev, data);
+	mutex_init(&data->lock);
+
+	ret = exynos_tmu_initialize(pdev);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to initialize TMU\n");
+		goto err_clk;
+	}
+
+	exynos_tmu_control(pdev, true);
+
+	/* Register the sensor with thermal management interface */
+	(&exynos_sensor_conf)->private_data = data;
+	exynos_sensor_conf.trip_data.trip_count = pdata->trigger_level0_en +
+			pdata->trigger_level1_en + pdata->trigger_level2_en +
+			pdata->trigger_level3_en;
+
+	for (i = 0; i < exynos_sensor_conf.trip_data.trip_count; i++)
+		exynos_sensor_conf.trip_data.trip_val[i] =
+			pdata->threshold + pdata->trigger_levels[i];
+
+	exynos_sensor_conf.cooling_data.freq_clip_count =
+						pdata->freq_tab_count;
+	for (i = 0; i < pdata->freq_tab_count; i++) {
+		exynos_sensor_conf.cooling_data.freq_data[i].freq_clip_max =
+					pdata->freq_tab[i].freq_clip_max;
+		exynos_sensor_conf.cooling_data.freq_data[i].temp_level =
+					pdata->freq_tab[i].temp_level;
+	}
+
+	ret = exynos_register_thermal(&exynos_sensor_conf);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to register thermal interface\n");
+		goto err_clk;
+	}
+	return 0;
+err_clk:
+	platform_set_drvdata(pdev, NULL);
+	clk_put(data->clk);
+	return ret;
+}
+
+static int __devexit exynos_tmu_remove(struct platform_device *pdev)
+{
+	struct exynos_tmu_data *data = platform_get_drvdata(pdev);
+
+	exynos_tmu_control(pdev, false);
+
+	exynos_unregister_thermal();
+
+	clk_put(data->clk);
+
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int exynos_tmu_suspend(struct device *dev)
+{
+	exynos_tmu_control(to_platform_device(dev), false);
+
+	return 0;
+}
+
+static int exynos_tmu_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+
+	exynos_tmu_initialize(pdev);
+	exynos_tmu_control(pdev, true);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(exynos_tmu_pm,
+			 exynos_tmu_suspend, exynos_tmu_resume);
+#define EXYNOS_TMU_PM	(&exynos_tmu_pm)
+#else
+#define EXYNOS_TMU_PM	NULL
+#endif
+
+static struct platform_driver exynos_tmu_driver = {
+	.driver = {
+		.name   = "exynos-tmu",
+		.owner  = THIS_MODULE,
+		.pm     = EXYNOS_TMU_PM,
+		.of_match_table = exynos_tmu_match,
+	},
+	.probe = exynos_tmu_probe,
+	.remove	= __devexit_p(exynos_tmu_remove),
+	.id_table = exynos_tmu_driver_ids,
+};
+
+module_platform_driver(exynos_tmu_driver);
+
+MODULE_DESCRIPTION("EXYNOS TMU Driver");
+MODULE_AUTHOR("Donggeun Kim <dg77.kim@samsung.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:exynos-tmu");
diff --git a/drivers/thermal/rcar_thermal.c b/drivers/thermal/rcar_thermal.c
new file mode 100644
index 00000000000..d4452716aaa
--- /dev/null
+++ b/drivers/thermal/rcar_thermal.c
@@ -0,0 +1,260 @@
+/*
+ *  R-Car THS/TSC thermal sensor driver
+ *
+ * Copyright (C) 2012 Renesas Solutions Corp.
+ * Kuninori Morimoto <kuninori.morimoto.gx@renesas.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; version 2 of the License.
+ *
+ *  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 <linux/delay.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/thermal.h>
+
+#define THSCR	0x2c
+#define THSSR	0x30
+
+/* THSCR */
+#define CPTAP	0xf
+
+/* THSSR */
+#define CTEMP	0x3f
+
+
+struct rcar_thermal_priv {
+	void __iomem *base;
+	struct device *dev;
+	spinlock_t lock;
+	u32 comp;
+};
+
+/*
+ *		basic functions
+ */
+static u32 rcar_thermal_read(struct rcar_thermal_priv *priv, u32 reg)
+{
+	unsigned long flags;
+	u32 ret;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	ret = ioread32(priv->base + reg);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return ret;
+}
+
+#if 0 /* no user at this point */
+static void rcar_thermal_write(struct rcar_thermal_priv *priv,
+			       u32 reg, u32 data)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	iowrite32(data, priv->base + reg);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+#endif
+
+static void rcar_thermal_bset(struct rcar_thermal_priv *priv, u32 reg,
+			      u32 mask, u32 data)
+{
+	unsigned long flags;
+	u32 val;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	val = ioread32(priv->base + reg);
+	val &= ~mask;
+	val |= (data & mask);
+	iowrite32(val, priv->base + reg);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+/*
+ *		zone device functions
+ */
+static int rcar_thermal_get_temp(struct thermal_zone_device *zone,
+			   unsigned long *temp)
+{
+	struct rcar_thermal_priv *priv = zone->devdata;
+	int val, min, max, tmp;
+
+	tmp = -200; /* default */
+	while (1) {
+		if (priv->comp < 1 || priv->comp > 12) {
+			dev_err(priv->dev,
+				"THSSR invalid data (%d)\n", priv->comp);
+			priv->comp = 4; /* for next thermal */
+			return -EINVAL;
+		}
+
+		/*
+		 * THS comparator offset and the reference temperature
+		 *
+		 * Comparator	| reference	| Temperature field
+		 * offset	| temperature	| measurement
+		 *		| (degrees C)	| (degrees C)
+		 * -------------+---------------+-------------------
+		 *  1		|  -45		|  -45 to  -30
+		 *  2		|  -30		|  -30 to  -15
+		 *  3		|  -15		|  -15 to    0
+		 *  4		|    0		|    0 to  +15
+		 *  5		|  +15		|  +15 to  +30
+		 *  6		|  +30		|  +30 to  +45
+		 *  7		|  +45		|  +45 to  +60
+		 *  8		|  +60		|  +60 to  +75
+		 *  9		|  +75		|  +75 to  +90
+		 * 10		|  +90		|  +90 to +105
+		 * 11		| +105		| +105 to +120
+		 * 12		| +120		| +120 to +135
+		 */
+
+		/* calculate thermal limitation */
+		min = (priv->comp * 15) - 60;
+		max = min + 15;
+
+		/*
+		 * we need to wait 300us after changing comparator offset
+		 * to get stable temperature.
+		 * see "Usage Notes" on datasheet
+		 */
+		rcar_thermal_bset(priv, THSCR, CPTAP, priv->comp);
+		udelay(300);
+
+		/* calculate current temperature */
+		val = rcar_thermal_read(priv, THSSR) & CTEMP;
+		val = (val * 5) - 65;
+
+		dev_dbg(priv->dev, "comp/min/max/val = %d/%d/%d/%d\n",
+			priv->comp, min, max, val);
+
+		/*
+		 * If val is same as min/max, then,
+		 * it should try again on next comparator.
+		 * But the val might be correct temperature.
+		 * Keep it on "tmp" and compare with next val.
+		 */
+		if (tmp == val)
+			break;
+
+		if (val <= min) {
+			tmp = min;
+			priv->comp--; /* try again */
+		} else if (val >= max) {
+			tmp = max;
+			priv->comp++; /* try again */
+		} else {
+			tmp = val;
+			break;
+		}
+	}
+
+	*temp = tmp;
+	return 0;
+}
+
+static struct thermal_zone_device_ops rcar_thermal_zone_ops = {
+	.get_temp = rcar_thermal_get_temp,
+};
+
+/*
+ *		platform functions
+ */
+static int rcar_thermal_probe(struct platform_device *pdev)
+{
+	struct thermal_zone_device *zone;
+	struct rcar_thermal_priv *priv;
+	struct resource *res;
+	int ret;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "Could not get platform resource\n");
+		return -ENODEV;
+	}
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv) {
+		dev_err(&pdev->dev, "Could not allocate priv\n");
+		return -ENOMEM;
+	}
+
+	priv->comp = 4; /* basic setup */
+	priv->dev = &pdev->dev;
+	spin_lock_init(&priv->lock);
+	priv->base = devm_ioremap_nocache(&pdev->dev,
+					  res->start, resource_size(res));
+	if (!priv->base) {
+		dev_err(&pdev->dev, "Unable to ioremap thermal register\n");
+		ret = -ENOMEM;
+		goto error_free_priv;
+	}
+
+	zone = thermal_zone_device_register("rcar_thermal", 0, priv,
+					    &rcar_thermal_zone_ops, 0, 0);
+	if (IS_ERR(zone)) {
+		dev_err(&pdev->dev, "thermal zone device is NULL\n");
+		ret = PTR_ERR(zone);
+		goto error_iounmap;
+	}
+
+	platform_set_drvdata(pdev, zone);
+
+	dev_info(&pdev->dev, "proved\n");
+
+	return 0;
+
+error_iounmap:
+	devm_iounmap(&pdev->dev, priv->base);
+error_free_priv:
+	devm_kfree(&pdev->dev, priv);
+
+	return ret;
+}
+
+static int rcar_thermal_remove(struct platform_device *pdev)
+{
+	struct thermal_zone_device *zone = platform_get_drvdata(pdev);
+	struct rcar_thermal_priv *priv = zone->devdata;
+
+	thermal_zone_device_unregister(zone);
+	platform_set_drvdata(pdev, NULL);
+
+	devm_iounmap(&pdev->dev, priv->base);
+	devm_kfree(&pdev->dev, priv);
+
+	return 0;
+}
+
+static struct platform_driver rcar_thermal_driver = {
+	.driver	= {
+		.name	= "rcar_thermal",
+	},
+	.probe		= rcar_thermal_probe,
+	.remove		= rcar_thermal_remove,
+};
+module_platform_driver(rcar_thermal_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("R-Car THS/TSC thermal sensor driver");
+MODULE_AUTHOR("Kuninori Morimoto <kuninori.morimoto.gx@renesas.com>");
diff --git a/drivers/thermal/spear_thermal.c b/drivers/thermal/spear_thermal.c
index 5f8ee39f200..9bc969261d0 100644
--- a/drivers/thermal/spear_thermal.c
+++ b/drivers/thermal/spear_thermal.c
@@ -147,7 +147,7 @@ static int spear_thermal_probe(struct platform_device *pdev)
 	writel_relaxed(stdev->flags, stdev->thermal_base);
 
 	spear_thermal = thermal_zone_device_register("spear_thermal", 0, 0,
-				stdev, &ops, 0, 0, 0, 0);
+				stdev, &ops, 0, 0);
 	if (IS_ERR(spear_thermal)) {
 		dev_err(&pdev->dev, "thermal zone device is NULL\n");
 		ret = PTR_ERR(spear_thermal);
diff --git a/drivers/thermal/thermal_sys.c b/drivers/thermal/thermal_sys.c
index efd81bb25e0..9ee42ca4d28 100644
--- a/drivers/thermal/thermal_sys.c
+++ b/drivers/thermal/thermal_sys.c
@@ -41,15 +41,25 @@ MODULE_AUTHOR("Zhang Rui");
 MODULE_DESCRIPTION("Generic thermal management sysfs support");
 MODULE_LICENSE("GPL");
 
-struct thermal_cooling_device_instance {
+#define THERMAL_NO_TARGET -1UL
+/*
+ * This structure is used to describe the behavior of
+ * a certain cooling device on a certain trip point
+ * in a certain thermal zone
+ */
+struct thermal_instance {
 	int id;
 	char name[THERMAL_NAME_LENGTH];
 	struct thermal_zone_device *tz;
 	struct thermal_cooling_device *cdev;
 	int trip;
+	unsigned long upper;	/* Highest cooling state for this trip point */
+	unsigned long lower;	/* Lowest cooling state for this trip point */
+	unsigned long target;	/* expected cooling state */
 	char attr_name[THERMAL_NAME_LENGTH];
 	struct device_attribute attr;
-	struct list_head node;
+	struct list_head tz_node; /* node in tz->thermal_instances */
+	struct list_head cdev_node; /* node in cdev->thermal_instances */
 };
 
 static DEFINE_IDR(thermal_tz_idr);
@@ -308,8 +318,9 @@ passive_store(struct device *dev, struct device_attribute *attr,
 			if (!strncmp("Processor", cdev->type,
 				     sizeof("Processor")))
 				thermal_zone_bind_cooling_device(tz,
-								 THERMAL_TRIPS_NONE,
-								 cdev);
+						THERMAL_TRIPS_NONE, cdev,
+						THERMAL_NO_LIMIT,
+						THERMAL_NO_LIMIT);
 		}
 		mutex_unlock(&thermal_list_lock);
 		if (!tz->passive_delay)
@@ -327,9 +338,6 @@ passive_store(struct device *dev, struct device_attribute *attr,
 		tz->passive_delay = 0;
 	}
 
-	tz->tc1 = 1;
-	tz->tc2 = 1;
-
 	tz->forced_passive = state;
 
 	thermal_zone_device_update(tz);
@@ -425,10 +433,10 @@ static ssize_t
 thermal_cooling_device_trip_point_show(struct device *dev,
 				       struct device_attribute *attr, char *buf)
 {
-	struct thermal_cooling_device_instance *instance;
+	struct thermal_instance *instance;
 
 	instance =
-	    container_of(attr, struct thermal_cooling_device_instance, attr);
+	    container_of(attr, struct thermal_instance, attr);
 
 	if (instance->trip == THERMAL_TRIPS_NONE)
 		return sprintf(buf, "-1\n");
@@ -590,7 +598,7 @@ thermal_add_hwmon_sysfs(struct thermal_zone_device *tz)
 	temp->tz = tz;
 	hwmon->count++;
 
-	snprintf(temp->temp_input.name, THERMAL_NAME_LENGTH,
+	snprintf(temp->temp_input.name, sizeof(temp->temp_input.name),
 		 "temp%d_input", hwmon->count);
 	temp->temp_input.attr.attr.name = temp->temp_input.name;
 	temp->temp_input.attr.attr.mode = 0444;
@@ -603,7 +611,8 @@ thermal_add_hwmon_sysfs(struct thermal_zone_device *tz)
 	if (tz->ops->get_crit_temp) {
 		unsigned long temperature;
 		if (!tz->ops->get_crit_temp(tz, &temperature)) {
-			snprintf(temp->temp_crit.name, THERMAL_NAME_LENGTH,
+			snprintf(temp->temp_crit.name,
+				 sizeof(temp->temp_crit.name),
 				"temp%d_crit", hwmon->count);
 			temp->temp_crit.attr.attr.name = temp->temp_crit.name;
 			temp->temp_crit.attr.attr.mode = 0444;
@@ -704,74 +713,6 @@ static void thermal_zone_device_set_polling(struct thermal_zone_device *tz,
 		cancel_delayed_work(&tz->poll_queue);
 }
 
-static void thermal_zone_device_passive(struct thermal_zone_device *tz,
-					int temp, int trip_temp, int trip)
-{
-	int trend = 0;
-	struct thermal_cooling_device_instance *instance;
-	struct thermal_cooling_device *cdev;
-	long state, max_state;
-
-	/*
-	 * Above Trip?
-	 * -----------
-	 * Calculate the thermal trend (using the passive cooling equation)
-	 * and modify the performance limit for all passive cooling devices
-	 * accordingly.  Note that we assume symmetry.
-	 */
-	if (temp >= trip_temp) {
-		tz->passive = true;
-
-		trend = (tz->tc1 * (temp - tz->last_temperature)) +
-			(tz->tc2 * (temp - trip_temp));
-
-		/* Heating up? */
-		if (trend > 0) {
-			list_for_each_entry(instance, &tz->cooling_devices,
-					    node) {
-				if (instance->trip != trip)
-					continue;
-				cdev = instance->cdev;
-				cdev->ops->get_cur_state(cdev, &state);
-				cdev->ops->get_max_state(cdev, &max_state);
-				if (state++ < max_state)
-					cdev->ops->set_cur_state(cdev, state);
-			}
-		} else if (trend < 0) { /* Cooling off? */
-			list_for_each_entry(instance, &tz->cooling_devices,
-					    node) {
-				if (instance->trip != trip)
-					continue;
-				cdev = instance->cdev;
-				cdev->ops->get_cur_state(cdev, &state);
-				cdev->ops->get_max_state(cdev, &max_state);
-				if (state > 0)
-					cdev->ops->set_cur_state(cdev, --state);
-			}
-		}
-		return;
-	}
-
-	/*
-	 * Below Trip?
-	 * -----------
-	 * Implement passive cooling hysteresis to slowly increase performance
-	 * and avoid thrashing around the passive trip point.  Note that we
-	 * assume symmetry.
-	 */
-	list_for_each_entry(instance, &tz->cooling_devices, node) {
-		if (instance->trip != trip)
-			continue;
-		cdev = instance->cdev;
-		cdev->ops->get_cur_state(cdev, &state);
-		cdev->ops->get_max_state(cdev, &max_state);
-		if (state > 0)
-			cdev->ops->set_cur_state(cdev, --state);
-		if (state == 0)
-			tz->passive = false;
-	}
-}
-
 static void thermal_zone_device_check(struct work_struct *work)
 {
 	struct thermal_zone_device *tz = container_of(work, struct
@@ -791,12 +732,14 @@ static void thermal_zone_device_check(struct work_struct *work)
  */
 int thermal_zone_bind_cooling_device(struct thermal_zone_device *tz,
 				     int trip,
-				     struct thermal_cooling_device *cdev)
+				     struct thermal_cooling_device *cdev,
+				     unsigned long upper, unsigned long lower)
 {
-	struct thermal_cooling_device_instance *dev;
-	struct thermal_cooling_device_instance *pos;
+	struct thermal_instance *dev;
+	struct thermal_instance *pos;
 	struct thermal_zone_device *pos1;
 	struct thermal_cooling_device *pos2;
+	unsigned long max_state;
 	int result;
 
 	if (trip >= tz->trips || (trip < 0 && trip != THERMAL_TRIPS_NONE))
@@ -814,13 +757,26 @@ int thermal_zone_bind_cooling_device(struct thermal_zone_device *tz,
 	if (tz != pos1 || cdev != pos2)
 		return -EINVAL;
 
+	cdev->ops->get_max_state(cdev, &max_state);
+
+	/* lower default 0, upper default max_state */
+	lower = lower == THERMAL_NO_LIMIT ? 0 : lower;
+	upper = upper == THERMAL_NO_LIMIT ? max_state : upper;
+
+	if (lower > upper || upper > max_state)
+		return -EINVAL;
+
 	dev =
-	    kzalloc(sizeof(struct thermal_cooling_device_instance), GFP_KERNEL);
+	    kzalloc(sizeof(struct thermal_instance), GFP_KERNEL);
 	if (!dev)
 		return -ENOMEM;
 	dev->tz = tz;
 	dev->cdev = cdev;
 	dev->trip = trip;
+	dev->upper = upper;
+	dev->lower = lower;
+	dev->target = THERMAL_NO_TARGET;
+
 	result = get_idr(&tz->idr, &tz->lock, &dev->id);
 	if (result)
 		goto free_mem;
@@ -841,13 +797,17 @@ int thermal_zone_bind_cooling_device(struct thermal_zone_device *tz,
 		goto remove_symbol_link;
 
 	mutex_lock(&tz->lock);
-	list_for_each_entry(pos, &tz->cooling_devices, node)
+	mutex_lock(&cdev->lock);
+	list_for_each_entry(pos, &tz->thermal_instances, tz_node)
 	    if (pos->tz == tz && pos->trip == trip && pos->cdev == cdev) {
 		result = -EEXIST;
 		break;
 	}
-	if (!result)
-		list_add_tail(&dev->node, &tz->cooling_devices);
+	if (!result) {
+		list_add_tail(&dev->tz_node, &tz->thermal_instances);
+		list_add_tail(&dev->cdev_node, &cdev->thermal_instances);
+	}
+	mutex_unlock(&cdev->lock);
 	mutex_unlock(&tz->lock);
 
 	if (!result)
@@ -877,16 +837,20 @@ int thermal_zone_unbind_cooling_device(struct thermal_zone_device *tz,
 				       int trip,
 				       struct thermal_cooling_device *cdev)
 {
-	struct thermal_cooling_device_instance *pos, *next;
+	struct thermal_instance *pos, *next;
 
 	mutex_lock(&tz->lock);
-	list_for_each_entry_safe(pos, next, &tz->cooling_devices, node) {
+	mutex_lock(&cdev->lock);
+	list_for_each_entry_safe(pos, next, &tz->thermal_instances, tz_node) {
 		if (pos->tz == tz && pos->trip == trip && pos->cdev == cdev) {
-			list_del(&pos->node);
+			list_del(&pos->tz_node);
+			list_del(&pos->cdev_node);
+			mutex_unlock(&cdev->lock);
 			mutex_unlock(&tz->lock);
 			goto unbind;
 		}
 	}
+	mutex_unlock(&cdev->lock);
 	mutex_unlock(&tz->lock);
 
 	return -ENODEV;
@@ -934,7 +898,7 @@ thermal_cooling_device_register(char *type, void *devdata,
 	struct thermal_zone_device *pos;
 	int result;
 
-	if (strlen(type) >= THERMAL_NAME_LENGTH)
+	if (type && strlen(type) >= THERMAL_NAME_LENGTH)
 		return ERR_PTR(-EINVAL);
 
 	if (!ops || !ops->get_max_state || !ops->get_cur_state ||
@@ -951,8 +915,11 @@ thermal_cooling_device_register(char *type, void *devdata,
 		return ERR_PTR(result);
 	}
 
-	strcpy(cdev->type, type);
+	strcpy(cdev->type, type ? : "");
+	mutex_init(&cdev->lock);
+	INIT_LIST_HEAD(&cdev->thermal_instances);
 	cdev->ops = ops;
+	cdev->updated = true;
 	cdev->device.class = &thermal_class;
 	cdev->devdata = devdata;
 	dev_set_name(&cdev->device, "cooling_device%d", cdev->id);
@@ -1044,6 +1011,136 @@ void thermal_cooling_device_unregister(struct
 }
 EXPORT_SYMBOL(thermal_cooling_device_unregister);
 
+static void thermal_cdev_do_update(struct thermal_cooling_device *cdev)
+{
+	struct thermal_instance *instance;
+	unsigned long target = 0;
+
+	/* cooling device is updated*/
+	if (cdev->updated)
+		return;
+
+	mutex_lock(&cdev->lock);
+	/* Make sure cdev enters the deepest cooling state */
+	list_for_each_entry(instance, &cdev->thermal_instances, cdev_node) {
+		if (instance->target == THERMAL_NO_TARGET)
+			continue;
+		if (instance->target > target)
+			target = instance->target;
+	}
+	mutex_unlock(&cdev->lock);
+	cdev->ops->set_cur_state(cdev, target);
+	cdev->updated = true;
+}
+
+static void thermal_zone_do_update(struct thermal_zone_device *tz)
+{
+	struct thermal_instance *instance;
+
+	list_for_each_entry(instance, &tz->thermal_instances, tz_node)
+		thermal_cdev_do_update(instance->cdev);
+}
+
+/*
+ * Cooling algorithm for both active and passive cooling
+ *
+ * 1. if the temperature is higher than a trip point,
+ *    a. if the trend is THERMAL_TREND_RAISING, use higher cooling
+ *       state for this trip point
+ *    b. if the trend is THERMAL_TREND_DROPPING, use lower cooling
+ *       state for this trip point
+ *
+ * 2. if the temperature is lower than a trip point, use lower
+ *    cooling state for this trip point
+ *
+ * Note that this behaves the same as the previous passive cooling
+ * algorithm.
+ */
+
+static void thermal_zone_trip_update(struct thermal_zone_device *tz,
+				     int trip, long temp)
+{
+	struct thermal_instance *instance;
+	struct thermal_cooling_device *cdev = NULL;
+	unsigned long cur_state, max_state;
+	long trip_temp;
+	enum thermal_trip_type trip_type;
+	enum thermal_trend trend;
+
+	if (trip == THERMAL_TRIPS_NONE) {
+		trip_temp = tz->forced_passive;
+		trip_type = THERMAL_TRIPS_NONE;
+	} else {
+		tz->ops->get_trip_temp(tz, trip, &trip_temp);
+		tz->ops->get_trip_type(tz, trip, &trip_type);
+	}
+
+	if (!tz->ops->get_trend || tz->ops->get_trend(tz, trip, &trend)) {
+		/*
+		 * compare the current temperature and previous temperature
+		 * to get the thermal trend, if no special requirement
+		 */
+		if (tz->temperature > tz->last_temperature)
+			trend = THERMAL_TREND_RAISING;
+		else if (tz->temperature < tz->last_temperature)
+			trend = THERMAL_TREND_DROPPING;
+		else
+			trend = THERMAL_TREND_STABLE;
+	}
+
+	if (temp >= trip_temp) {
+		list_for_each_entry(instance, &tz->thermal_instances, tz_node) {
+			if (instance->trip != trip)
+				continue;
+
+			cdev = instance->cdev;
+
+			cdev->ops->get_cur_state(cdev, &cur_state);
+			cdev->ops->get_max_state(cdev, &max_state);
+
+			if (trend == THERMAL_TREND_RAISING) {
+				cur_state = cur_state < instance->upper ?
+					    (cur_state + 1) : instance->upper;
+			} else if (trend == THERMAL_TREND_DROPPING) {
+				cur_state = cur_state > instance->lower ?
+				    (cur_state - 1) : instance->lower;
+			}
+
+			/* activate a passive thermal instance */
+			if ((trip_type == THERMAL_TRIP_PASSIVE ||
+			     trip_type == THERMAL_TRIPS_NONE) &&
+			     instance->target == THERMAL_NO_TARGET)
+				tz->passive++;
+
+			instance->target = cur_state;
+			cdev->updated = false; /* cooling device needs update */
+		}
+	} else {	/* below trip */
+		list_for_each_entry(instance, &tz->thermal_instances, tz_node) {
+			if (instance->trip != trip)
+				continue;
+
+			/* Do not use the inactive thermal instance */
+			if (instance->target == THERMAL_NO_TARGET)
+				continue;
+			cdev = instance->cdev;
+			cdev->ops->get_cur_state(cdev, &cur_state);
+
+			cur_state = cur_state > instance->lower ?
+				    (cur_state - 1) : THERMAL_NO_TARGET;
+
+			/* deactivate a passive thermal instance */
+			if ((trip_type == THERMAL_TRIP_PASSIVE ||
+			     trip_type == THERMAL_TRIPS_NONE) &&
+			     cur_state == THERMAL_NO_TARGET)
+				tz->passive--;
+			instance->target = cur_state;
+			cdev->updated = false; /* cooling device needs update */
+		}
+	}
+
+	return;
+}
 /**
  * thermal_zone_device_update - force an update of a thermal zone's state
  * @ttz:	the thermal zone to update
@@ -1054,8 +1151,6 @@ void thermal_zone_device_update(struct thermal_zone_device *tz)
 	int count, ret = 0;
 	long temp, trip_temp;
 	enum thermal_trip_type trip_type;
-	struct thermal_cooling_device_instance *instance;
-	struct thermal_cooling_device *cdev;
 
 	mutex_lock(&tz->lock);
 
@@ -1065,6 +1160,9 @@ void thermal_zone_device_update(struct thermal_zone_device *tz)
 		goto leave;
 	}
 
+	tz->last_temperature = tz->temperature;
+	tz->temperature = temp;
+
 	for (count = 0; count < tz->trips; count++) {
 		tz->ops->get_trip_type(tz, count, &trip_type);
 		tz->ops->get_trip_temp(tz, count, &trip_temp);
@@ -1088,32 +1186,18 @@ void thermal_zone_device_update(struct thermal_zone_device *tz)
 					tz->ops->notify(tz, count, trip_type);
 			break;
 		case THERMAL_TRIP_ACTIVE:
-			list_for_each_entry(instance, &tz->cooling_devices,
-					    node) {
-				if (instance->trip != count)
-					continue;
-
-				cdev = instance->cdev;
-
-				if (temp >= trip_temp)
-					cdev->ops->set_cur_state(cdev, 1);
-				else
-					cdev->ops->set_cur_state(cdev, 0);
-			}
+			thermal_zone_trip_update(tz, count, temp);
 			break;
 		case THERMAL_TRIP_PASSIVE:
 			if (temp >= trip_temp || tz->passive)
-				thermal_zone_device_passive(tz, temp,
-							    trip_temp, count);
+				thermal_zone_trip_update(tz, count, temp);
 			break;
 		}
 	}
 
 	if (tz->forced_passive)
-		thermal_zone_device_passive(tz, temp, tz->forced_passive,
-					    THERMAL_TRIPS_NONE);
-
-	tz->last_temperature = temp;
+		thermal_zone_trip_update(tz, THERMAL_TRIPS_NONE, temp);
+	thermal_zone_do_update(tz);
 
 leave:
 	if (tz->passive)
@@ -1236,8 +1320,6 @@ static void remove_trip_attrs(struct thermal_zone_device *tz)
  * @mask:	a bit string indicating the writeablility of trip points
  * @devdata:	private device data
  * @ops:	standard thermal zone device callbacks
- * @tc1:	thermal coefficient 1 for passive calculations
- * @tc2:	thermal coefficient 2 for passive calculations
  * @passive_delay: number of milliseconds to wait between polls when
  *		   performing passive cooling
  * @polling_delay: number of milliseconds to wait between polls when checking
@@ -1245,13 +1327,12 @@ static void remove_trip_attrs(struct thermal_zone_device *tz)
  *		   driven systems)
  *
  * thermal_zone_device_unregister() must be called when the device is no
- * longer needed. The passive cooling formula uses tc1 and tc2 as described in
- * section 11.1.5.1 of the ACPI specification 3.0.
+ * longer needed. The passive cooling depends on the .get_trend() return value.
  */
 struct thermal_zone_device *thermal_zone_device_register(const char *type,
 	int trips, int mask, void *devdata,
 	const struct thermal_zone_device_ops *ops,
-	int tc1, int tc2, int passive_delay, int polling_delay)
+	int passive_delay, int polling_delay)
 {
 	struct thermal_zone_device *tz;
 	struct thermal_cooling_device *pos;
@@ -1260,7 +1341,7 @@ struct thermal_zone_device *thermal_zone_device_register(const char *type,
 	int count;
 	int passive = 0;
 
-	if (strlen(type) >= THERMAL_NAME_LENGTH)
+	if (type && strlen(type) >= THERMAL_NAME_LENGTH)
 		return ERR_PTR(-EINVAL);
 
 	if (trips > THERMAL_MAX_TRIPS || trips < 0 || mask >> trips)
@@ -1273,7 +1354,7 @@ struct thermal_zone_device *thermal_zone_device_register(const char *type,
 	if (!tz)
 		return ERR_PTR(-ENOMEM);
 
-	INIT_LIST_HEAD(&tz->cooling_devices);
+	INIT_LIST_HEAD(&tz->thermal_instances);
 	idr_init(&tz->idr);
 	mutex_init(&tz->lock);
 	result = get_idr(&thermal_tz_idr, &thermal_idr_lock, &tz->id);
@@ -1282,13 +1363,11 @@ struct thermal_zone_device *thermal_zone_device_register(const char *type,
 		return ERR_PTR(result);
 	}
 
-	strcpy(tz->type, type);
+	strcpy(tz->type, type ? : "");
 	tz->ops = ops;
 	tz->device.class = &thermal_class;
 	tz->devdata = devdata;
 	tz->trips = trips;
-	tz->tc1 = tc1;
-	tz->tc2 = tc2;
 	tz->passive_delay = passive_delay;
 	tz->polling_delay = polling_delay;
 
diff --git a/drivers/tty/hvc/hvc_xen.c b/drivers/tty/hvc/hvc_xen.c
index 2944ff88fdc..f4abfe238f9 100644
--- a/drivers/tty/hvc/hvc_xen.c
+++ b/drivers/tty/hvc/hvc_xen.c
@@ -478,7 +478,6 @@ static void xencons_backend_changed(struct xenbus_device *dev,
 	case XenbusStateInitialising:
 	case XenbusStateInitialised:
 	case XenbusStateUnknown:
-	case XenbusStateClosed:
 		break;
 
 	case XenbusStateInitWait:
@@ -488,6 +487,10 @@ static void xencons_backend_changed(struct xenbus_device *dev,
 		xenbus_switch_state(dev, XenbusStateConnected);
 		break;
 
+	case XenbusStateClosed:
+		if (dev->state == XenbusStateClosed)
+			break;
+		/* Missed the backend's CLOSING state -- fallthrough */
 	case XenbusStateClosing:
 		xenbus_frontend_closed(dev);
 		break;
diff --git a/drivers/tty/serial/kgdboc.c b/drivers/tty/serial/kgdboc.c
index c0b334327d9..10020547c60 100644
--- a/drivers/tty/serial/kgdboc.c
+++ b/drivers/tty/serial/kgdboc.c
@@ -97,7 +97,8 @@ static void kgdboc_restore_input(void)
 
 static int kgdboc_register_kbd(char **cptr)
 {
-	if (strncmp(*cptr, "kbd", 3) == 0) {
+	if (strncmp(*cptr, "kbd", 3) == 0 ||
+		strncmp(*cptr, "kdb", 3) == 0) {
 		if (kdb_poll_idx < KDB_POLL_FUNC_MAX) {
 			kdb_poll_funcs[kdb_poll_idx] = kdb_get_kbd_char;
 			kdb_poll_idx++;
diff --git a/drivers/tty/vt/vt.c b/drivers/tty/vt/vt.c
index 999ca63afde..f87d7e8964b 100644
--- a/drivers/tty/vt/vt.c
+++ b/drivers/tty/vt/vt.c
@@ -3442,6 +3442,19 @@ int con_debug_enter(struct vc_data *vc)
 			kdb_set(2, setargs);
 		}
 	}
+	if (vc->vc_cols < 999) {
+		int colcount;
+		char cols[4];
+		const char *setargs[3] = {
+			"set",
+			"COLUMNS",
+			cols,
+		};
+		if (kdbgetintenv(setargs[0], &colcount)) {
+			snprintf(cols, 4, "%i", vc->vc_cols);
+			kdb_set(2, setargs);
+		}
+	}
 #endif /* CONFIG_KGDB_KDB */
 	return ret;
 }
diff --git a/drivers/uio/uio.c b/drivers/uio/uio.c
index a783d533a1a..5110f367f1f 100644
--- a/drivers/uio/uio.c
+++ b/drivers/uio/uio.c
@@ -653,8 +653,6 @@ static int uio_mmap_physical(struct vm_area_struct *vma)
 	if (mi < 0)
 		return -EINVAL;
 
-	vma->vm_flags |= VM_IO | VM_RESERVED;
-
 	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
 
 	return remap_pfn_range(vma,
@@ -666,7 +664,7 @@ static int uio_mmap_physical(struct vm_area_struct *vma)
 
 static int uio_mmap_logical(struct vm_area_struct *vma)
 {
-	vma->vm_flags |= VM_RESERVED;
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
 	vma->vm_ops = &uio_vm_ops;
 	uio_vma_open(vma);
 	return 0;
diff --git a/drivers/usb/class/cdc-acm.c b/drivers/usb/class/cdc-acm.c
index 36f2be4def2..981f2132d12 100644
--- a/drivers/usb/class/cdc-acm.c
+++ b/drivers/usb/class/cdc-acm.c
@@ -1551,6 +1551,9 @@ static const struct usb_device_id acm_ids[] = {
 					   Maybe we should define a new
 					   quirk for this. */
 	},
+	{ USB_DEVICE(0x0572, 0x1340), /* Conexant CX93010-2x UCMxx */
+	.driver_info = NO_UNION_NORMAL,
+	},
 	{ USB_DEVICE(0x1bbb, 0x0003), /* Alcatel OT-I650 */
 	.driver_info = NO_UNION_NORMAL, /* reports zero length descriptor */
 	},
diff --git a/drivers/usb/core/usb-acpi.c b/drivers/usb/core/usb-acpi.c
index 0ef7d42d8ab..cef4252bb31 100644
--- a/drivers/usb/core/usb-acpi.c
+++ b/drivers/usb/core/usb-acpi.c
@@ -87,7 +87,7 @@ static int usb_acpi_check_port_connect_type(struct usb_device *hdev,
 	acpi_status status;
 	struct acpi_buffer buffer = { ACPI_ALLOCATE_BUFFER, NULL };
 	union acpi_object *upc;
-	struct acpi_pld pld;
+	struct acpi_pld_info *pld;
 	int ret = 0;
 
 	/*
@@ -111,16 +111,17 @@ static int usb_acpi_check_port_connect_type(struct usb_device *hdev,
 	}
 
 	if (upc->package.elements[0].integer.value)
-		if (pld.user_visible)
+		if (pld->user_visible)
 			usb_set_hub_port_connect_type(hdev, port1,
 				USB_PORT_CONNECT_TYPE_HOT_PLUG);
 		else
 			usb_set_hub_port_connect_type(hdev, port1,
 				USB_PORT_CONNECT_TYPE_HARD_WIRED);
-	else if (!pld.user_visible)
+	else if (!pld->user_visible)
 		usb_set_hub_port_connect_type(hdev, port1, USB_PORT_NOT_USED);
 
 out:
+	ACPI_FREE(pld);
 	kfree(upc);
 	return ret;
 }
diff --git a/drivers/usb/gadget/tcm_usb_gadget.c b/drivers/usb/gadget/tcm_usb_gadget.c
index eaa1005377f..97e68b38cfd 100644
--- a/drivers/usb/gadget/tcm_usb_gadget.c
+++ b/drivers/usb/gadget/tcm_usb_gadget.c
@@ -1472,16 +1472,6 @@ static int usbg_queue_tm_rsp(struct se_cmd *se_cmd)
 	return 0;
 }
 
-static u16 usbg_set_fabric_sense_len(struct se_cmd *se_cmd, u32 sense_length)
-{
-	return 0;
-}
-
-static u16 usbg_get_fabric_sense_len(void)
-{
-	return 0;
-}
-
 static const char *usbg_check_wwn(const char *name)
 {
 	const char *n;
@@ -1822,7 +1812,7 @@ static ssize_t tcm_usbg_tpg_store_nexus(
 		ret = tcm_usbg_drop_nexus(tpg);
 		return (!ret) ? count : ret;
 	}
-	if (strlen(page) > USBG_NAMELEN) {
+	if (strlen(page) >= USBG_NAMELEN) {
 		pr_err("Emulated NAA Sas Address: %s, exceeds"
 				" max: %d\n", page, USBG_NAMELEN);
 		return -EINVAL;
@@ -1907,8 +1897,6 @@ static struct target_core_fabric_ops usbg_ops = {
 	.queue_data_in			= usbg_send_read_response,
 	.queue_status			= usbg_send_status_response,
 	.queue_tm_rsp			= usbg_queue_tm_rsp,
-	.get_fabric_sense_len		= usbg_get_fabric_sense_len,
-	.set_fabric_sense_len		= usbg_set_fabric_sense_len,
 	.check_stop_free		= usbg_check_stop_free,
 
 	.fabric_make_wwn		= usbg_make_tport,
@@ -1968,7 +1956,6 @@ static void usbg_deregister_configfs(void)
 static struct usb_interface_descriptor bot_intf_desc = {
 	.bLength =              sizeof(bot_intf_desc),
 	.bDescriptorType =      USB_DT_INTERFACE,
-	.bAlternateSetting =	0,
 	.bNumEndpoints =        2,
 	.bAlternateSetting =	USB_G_ALT_INT_BBB,
 	.bInterfaceClass =      USB_CLASS_MASS_STORAGE,
diff --git a/drivers/usb/mon/mon_bin.c b/drivers/usb/mon/mon_bin.c
index 91cd85076a4..9a62e89d6dc 100644
--- a/drivers/usb/mon/mon_bin.c
+++ b/drivers/usb/mon/mon_bin.c
@@ -1247,7 +1247,7 @@ static int mon_bin_mmap(struct file *filp, struct vm_area_struct *vma)
 {
 	/* don't do anything here: "fault" will set up page table entries */
 	vma->vm_ops = &mon_bin_vm_ops;
-	vma->vm_flags |= VM_RESERVED;
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
 	vma->vm_private_data = filp->private_data;
 	mon_bin_vma_open(vma);
 	return 0;
diff --git a/drivers/usb/musb/musb_io.h b/drivers/usb/musb/musb_io.h
index f7c1c8e2dc3..565ad161783 100644
--- a/drivers/usb/musb/musb_io.h
+++ b/drivers/usb/musb/musb_io.h
@@ -40,7 +40,8 @@
 #if !defined(CONFIG_ARM) && !defined(CONFIG_SUPERH) \
 	&& !defined(CONFIG_AVR32) && !defined(CONFIG_PPC32) \
 	&& !defined(CONFIG_PPC64) && !defined(CONFIG_BLACKFIN) \
-	&& !defined(CONFIG_MIPS) && !defined(CONFIG_M68K)
+	&& !defined(CONFIG_MIPS) && !defined(CONFIG_M68K) \
+	&& !defined(CONFIG_XTENSA)
 static inline void readsl(const void __iomem *addr, void *buf, int len)
 	{ insl((unsigned long)addr, buf, len); }
 static inline void readsw(const void __iomem *addr, void *buf, int len)
diff --git a/drivers/vfio/pci/vfio_pci.c b/drivers/vfio/pci/vfio_pci.c
index 6968b723223..6c119944bbb 100644
--- a/drivers/vfio/pci/vfio_pci.c
+++ b/drivers/vfio/pci/vfio_pci.c
@@ -408,7 +408,7 @@ static int vfio_pci_mmap(void *device_data, struct vm_area_struct *vma)
 	struct vfio_pci_device *vdev = device_data;
 	struct pci_dev *pdev = vdev->pdev;
 	unsigned int index;
-	u64 phys_len, req_len, pgoff, req_start, phys;
+	u64 phys_len, req_len, pgoff, req_start;
 	int ret;
 
 	index = vma->vm_pgoff >> (VFIO_PCI_OFFSET_SHIFT - PAGE_SHIFT);
@@ -461,12 +461,11 @@ static int vfio_pci_mmap(void *device_data, struct vm_area_struct *vma)
 	}
 
 	vma->vm_private_data = vdev;
-	vma->vm_flags |= (VM_IO | VM_RESERVED);
+	vma->vm_flags |= VM_IO | VM_DONTEXPAND | VM_DONTDUMP;
 	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	vma->vm_pgoff = (pci_resource_start(pdev, index) >> PAGE_SHIFT) + pgoff;
 
-	phys = (pci_resource_start(pdev, index) >> PAGE_SHIFT) + pgoff;
-
-	return remap_pfn_range(vma, vma->vm_start, phys,
+	return remap_pfn_range(vma, vma->vm_start, vma->vm_pgoff,
 			       req_len, vma->vm_page_prot);
 }
 
diff --git a/drivers/vfio/pci/vfio_pci_intrs.c b/drivers/vfio/pci/vfio_pci_intrs.c
index d8dedc7d391..3639371fa69 100644
--- a/drivers/vfio/pci/vfio_pci_intrs.c
+++ b/drivers/vfio/pci/vfio_pci_intrs.c
@@ -366,6 +366,17 @@ static int vfio_intx_enable(struct vfio_pci_device *vdev)
 		return -ENOMEM;
 
 	vdev->num_ctx = 1;
+
+	/*
+	 * If the virtual interrupt is masked, restore it.  Devices
+	 * supporting DisINTx can be masked at the hardware level
+	 * here, non-PCI-2.3 devices will have to wait until the
+	 * interrupt is enabled.
+	 */
+	vdev->ctx[0].masked = vdev->virq_disabled;
+	if (vdev->pci_2_3)
+		pci_intx(vdev->pdev, !vdev->ctx[0].masked);
+
 	vdev->irq_type = VFIO_PCI_INTX_IRQ_INDEX;
 
 	return 0;
@@ -400,25 +411,26 @@ static int vfio_intx_set_signal(struct vfio_pci_device *vdev, int fd)
 		return PTR_ERR(trigger);
 	}
 
+	vdev->ctx[0].trigger = trigger;
+
 	if (!vdev->pci_2_3)
 		irqflags = 0;
 
 	ret = request_irq(pdev->irq, vfio_intx_handler,
 			  irqflags, vdev->ctx[0].name, vdev);
 	if (ret) {
+		vdev->ctx[0].trigger = NULL;
 		kfree(vdev->ctx[0].name);
 		eventfd_ctx_put(trigger);
 		return ret;
 	}
 
-	vdev->ctx[0].trigger = trigger;
-
 	/*
 	 * INTx disable will stick across the new irq setup,
 	 * disable_irq won't.
 	 */
 	spin_lock_irqsave(&vdev->irqlock, flags);
-	if (!vdev->pci_2_3 && (vdev->ctx[0].masked || vdev->virq_disabled))
+	if (!vdev->pci_2_3 && vdev->ctx[0].masked)
 		disable_irq_nosync(pdev->irq);
 	spin_unlock_irqrestore(&vdev->irqlock, flags);
 
diff --git a/drivers/vhost/tcm_vhost.c b/drivers/vhost/tcm_vhost.c
index ed8e2e6c8df..aa31692064d 100644
--- a/drivers/vhost/tcm_vhost.c
+++ b/drivers/vhost/tcm_vhost.c
@@ -330,17 +330,6 @@ static int tcm_vhost_queue_tm_rsp(struct se_cmd *se_cmd)
 	return 0;
 }
 
-static u16 tcm_vhost_set_fabric_sense_len(struct se_cmd *se_cmd,
-	u32 sense_length)
-{
-	return 0;
-}
-
-static u16 tcm_vhost_get_fabric_sense_len(void)
-{
-	return 0;
-}
-
 static void vhost_scsi_free_cmd(struct tcm_vhost_cmd *tv_cmd)
 {
 	struct se_cmd *se_cmd = &tv_cmd->tvc_se_cmd;
@@ -426,10 +415,7 @@ static struct tcm_vhost_cmd *vhost_scsi_allocate_cmd(
 {
 	struct tcm_vhost_cmd *tv_cmd;
 	struct tcm_vhost_nexus *tv_nexus;
-	struct se_portal_group *se_tpg = &tv_tpg->se_tpg;
 	struct se_session *se_sess;
-	struct se_cmd *se_cmd;
-	int sam_task_attr;
 
 	tv_nexus = tv_tpg->tpg_nexus;
 	if (!tv_nexus) {
@@ -445,23 +431,11 @@ static struct tcm_vhost_cmd *vhost_scsi_allocate_cmd(
 	}
 	INIT_LIST_HEAD(&tv_cmd->tvc_completion_list);
 	tv_cmd->tvc_tag = v_req->tag;
+	tv_cmd->tvc_task_attr = v_req->task_attr;
+	tv_cmd->tvc_exp_data_len = exp_data_len;
+	tv_cmd->tvc_data_direction = data_direction;
+	tv_cmd->tvc_nexus = tv_nexus;
 
-	se_cmd = &tv_cmd->tvc_se_cmd;
-	/*
-	 * Locate the SAM Task Attr from virtio_scsi_cmd_req
-	 */
-	sam_task_attr = v_req->task_attr;
-	/*
-	 * Initialize struct se_cmd descriptor from TCM infrastructure
-	 */
-	transport_init_se_cmd(se_cmd, se_tpg->se_tpg_tfo, se_sess, exp_data_len,
-				data_direction, sam_task_attr,
-				&tv_cmd->tvc_sense_buf[0]);
-
-#if 0	/* FIXME: vhost_scsi_allocate_cmd() BIDI operation */
-	if (bidi)
-		se_cmd->se_cmd_flags |= SCF_BIDI;
-#endif
 	return tv_cmd;
 }
 
@@ -560,37 +534,10 @@ static void tcm_vhost_submission_work(struct work_struct *work)
 {
 	struct tcm_vhost_cmd *tv_cmd =
 		container_of(work, struct tcm_vhost_cmd, work);
+	struct tcm_vhost_nexus *tv_nexus;
 	struct se_cmd *se_cmd = &tv_cmd->tvc_se_cmd;
 	struct scatterlist *sg_ptr, *sg_bidi_ptr = NULL;
 	int rc, sg_no_bidi = 0;
-	/*
-	 * Locate the struct se_lun pointer based on v_req->lun, and
-	 * attach it to struct se_cmd
-	 */
-	rc = transport_lookup_cmd_lun(&tv_cmd->tvc_se_cmd, tv_cmd->tvc_lun);
-	if (rc < 0) {
-		pr_err("Failed to look up lun: %d\n", tv_cmd->tvc_lun);
-		transport_send_check_condition_and_sense(&tv_cmd->tvc_se_cmd,
-			tv_cmd->tvc_se_cmd.scsi_sense_reason, 0);
-		transport_generic_free_cmd(se_cmd, 0);
-		return;
-	}
-
-	rc = target_setup_cmd_from_cdb(se_cmd, tv_cmd->tvc_cdb);
-	if (rc == -ENOMEM) {
-		transport_send_check_condition_and_sense(se_cmd,
-				TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE, 0);
-		transport_generic_free_cmd(se_cmd, 0);
-		return;
-	} else if (rc < 0) {
-		if (se_cmd->se_cmd_flags & SCF_SCSI_RESERVATION_CONFLICT)
-			tcm_vhost_queue_status(se_cmd);
-		else
-			transport_send_check_condition_and_sense(se_cmd,
-					se_cmd->scsi_sense_reason, 0);
-		transport_generic_free_cmd(se_cmd, 0);
-		return;
-	}
 
 	if (tv_cmd->tvc_sgl_count) {
 		sg_ptr = tv_cmd->tvc_sgl;
@@ -608,17 +555,19 @@ static void tcm_vhost_submission_work(struct work_struct *work)
 	} else {
 		sg_ptr = NULL;
 	}
-
-	rc = transport_generic_map_mem_to_cmd(se_cmd, sg_ptr,
-				tv_cmd->tvc_sgl_count, sg_bidi_ptr,
-				sg_no_bidi);
+	tv_nexus = tv_cmd->tvc_nexus;
+
+	rc = target_submit_cmd_map_sgls(se_cmd, tv_nexus->tvn_se_sess,
+			tv_cmd->tvc_cdb, &tv_cmd->tvc_sense_buf[0],
+			tv_cmd->tvc_lun, tv_cmd->tvc_exp_data_len,
+			tv_cmd->tvc_task_attr, tv_cmd->tvc_data_direction,
+			0, sg_ptr, tv_cmd->tvc_sgl_count,
+			sg_bidi_ptr, sg_no_bidi);
 	if (rc < 0) {
 		transport_send_check_condition_and_sense(se_cmd,
-				se_cmd->scsi_sense_reason, 0);
+				TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE, 0);
 		transport_generic_free_cmd(se_cmd, 0);
-		return;
 	}
-	transport_handle_cdb_direct(se_cmd);
 }
 
 static void vhost_scsi_handle_vq(struct vhost_scsi *vs)
@@ -1531,8 +1480,6 @@ static struct target_core_fabric_ops tcm_vhost_ops = {
 	.queue_data_in			= tcm_vhost_queue_data_in,
 	.queue_status			= tcm_vhost_queue_status,
 	.queue_tm_rsp			= tcm_vhost_queue_tm_rsp,
-	.get_fabric_sense_len		= tcm_vhost_get_fabric_sense_len,
-	.set_fabric_sense_len		= tcm_vhost_set_fabric_sense_len,
 	/*
 	 * Setup callers for generic logic in target_core_fabric_configfs.c
 	 */
diff --git a/drivers/vhost/tcm_vhost.h b/drivers/vhost/tcm_vhost.h
index d9e93557d66..7e87c63ecbc 100644
--- a/drivers/vhost/tcm_vhost.h
+++ b/drivers/vhost/tcm_vhost.h
@@ -5,6 +5,12 @@
 struct tcm_vhost_cmd {
 	/* Descriptor from vhost_get_vq_desc() for virt_queue segment */
 	int tvc_vq_desc;
+	/* virtio-scsi initiator task attribute */
+	int tvc_task_attr;
+	/* virtio-scsi initiator data direction */
+	enum dma_data_direction tvc_data_direction;
+	/* Expected data transfer length from virtio-scsi header */
+	u32 tvc_exp_data_len;
 	/* The Tag from include/linux/virtio_scsi.h:struct virtio_scsi_cmd_req */
 	u64 tvc_tag;
 	/* The number of scatterlists associated with this cmd */
@@ -17,6 +23,8 @@ struct tcm_vhost_cmd {
 	struct virtio_scsi_cmd_resp __user *tvc_resp;
 	/* Pointer to vhost_scsi for our device */
 	struct vhost_scsi *tvc_vhost;
+	/* Pointer to vhost nexus memory */
+	struct tcm_vhost_nexus *tvc_nexus;
 	/* The TCM I/O descriptor that is accessed via container_of() */
 	struct se_cmd tvc_se_cmd;
 	/* work item used for cmwq dispatch to tcm_vhost_submission_work() */
diff --git a/drivers/video/68328fb.c b/drivers/video/68328fb.c
index a425d65d5ba..fa44fbed397 100644
--- a/drivers/video/68328fb.c
+++ b/drivers/video/68328fb.c
@@ -400,7 +400,7 @@ static int mc68x328fb_mmap(struct fb_info *info, struct vm_area_struct *vma)
 #ifndef MMU
 	/* this is uClinux (no MMU) specific code */
 
-	vma->vm_flags |= VM_RESERVED;
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
 	vma->vm_start = videomemory;
 
 	return 0;
diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index 20c33c42600..d08d7998a4a 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -2139,21 +2139,6 @@ config FB_UDL
 	  mplayer -vo fbdev. Supports all USB 2.0 era DisplayLink devices.
 	  To compile as a module, choose M here: the module name is udlfb.
 
-config FB_PNX4008_DUM
-	tristate "Display Update Module support on Philips PNX4008 board"
-	depends on FB && ARCH_PNX4008
-	---help---
-	  Say Y here to enable support for PNX4008 Display Update Module (DUM)
-
-config FB_PNX4008_DUM_RGB
-	tristate "RGB Framebuffer support on Philips PNX4008 board"
-	depends on FB_PNX4008_DUM
-	select FB_CFB_FILLRECT
-	select FB_CFB_COPYAREA
-	select FB_CFB_IMAGEBLIT
-	---help---
-	  Say Y here to enable support for PNX4008 RGB Framebuffer
-
 config FB_IBM_GXT4500
 	tristate "Framebuffer support for IBM GXT4500P adaptor"
 	depends on FB && PPC
diff --git a/drivers/video/Makefile b/drivers/video/Makefile
index 194035986af..23e948ebfab 100644
--- a/drivers/video/Makefile
+++ b/drivers/video/Makefile
@@ -127,8 +127,6 @@ obj-$(CONFIG_FB_S3C)		  += s3c-fb.o
 obj-$(CONFIG_FB_S3C2410)	  += s3c2410fb.o
 obj-$(CONFIG_FB_FSL_DIU)	  += fsl-diu-fb.o
 obj-$(CONFIG_FB_COBALT)           += cobalt_lcdfb.o
-obj-$(CONFIG_FB_PNX4008_DUM)	  += pnx4008/
-obj-$(CONFIG_FB_PNX4008_DUM_RGB)  += pnx4008/
 obj-$(CONFIG_FB_IBM_GXT4500)	  += gxt4500.o
 obj-$(CONFIG_FB_PS3)		  += ps3fb.o
 obj-$(CONFIG_FB_SM501)            += sm501fb.o
diff --git a/drivers/video/amifb.c b/drivers/video/amifb.c
index 887df9d8142..7fa1bf82372 100644
--- a/drivers/video/amifb.c
+++ b/drivers/video/amifb.c
@@ -949,7 +949,6 @@ static int round_down_bpp = 1;	/* for mode probing */
 
 
 static int amifb_ilbm = 0;	/* interleaved or normal bitplanes */
-static int amifb_inverse = 0;
 
 static u32 amifb_hfmin __initdata;	/* monitor hfreq lower limit (Hz) */
 static u32 amifb_hfmax __initdata;	/* monitor hfreq upper limit (Hz) */
@@ -2355,7 +2354,6 @@ static int __init amifb_setup(char *options)
 		if (!*this_opt)
 			continue;
 		if (!strcmp(this_opt, "inverse")) {
-			amifb_inverse = 1;
 			fb_invert_cmaps();
 		} else if (!strcmp(this_opt, "ilbm"))
 			amifb_ilbm = 1;
diff --git a/drivers/video/arcfb.c b/drivers/video/arcfb.c
index a1d58e9d307..4659d5da6ff 100644
--- a/drivers/video/arcfb.c
+++ b/drivers/video/arcfb.c
@@ -552,6 +552,7 @@ static int __devinit arcfb_probe(struct platform_device *dev)
 				"arcfb", info)) {
 			printk(KERN_INFO
 				"arcfb: Failed req IRQ %d\n", par->irq);
+			retval = -EBUSY;
 			goto err1;
 		}
 	}
diff --git a/drivers/video/atmel_lcdfb.c b/drivers/video/atmel_lcdfb.c
index 15055395cd9..94cac9f9919 100644
--- a/drivers/video/atmel_lcdfb.c
+++ b/drivers/video/atmel_lcdfb.c
@@ -931,8 +931,10 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
 		}
 
 		info->screen_base = ioremap(info->fix.smem_start, info->fix.smem_len);
-		if (!info->screen_base)
+		if (!info->screen_base) {
+			ret = -ENOMEM;
 			goto release_intmem;
+		}
 
 		/*
 		 * Don't clear the framebuffer -- someone may have set
@@ -960,6 +962,7 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
 	sinfo->mmio = ioremap(info->fix.mmio_start, info->fix.mmio_len);
 	if (!sinfo->mmio) {
 		dev_err(dev, "cannot map LCDC registers\n");
+		ret = -ENOMEM;
 		goto release_mem;
 	}
 
diff --git a/drivers/video/aty/atyfb_base.c b/drivers/video/aty/atyfb_base.c
index 3f2e8c13f1c..868932f904e 100644
--- a/drivers/video/aty/atyfb_base.c
+++ b/drivers/video/aty/atyfb_base.c
@@ -1942,8 +1942,7 @@ static int atyfb_mmap(struct fb_info *info, struct vm_area_struct *vma)
 	off = vma->vm_pgoff << PAGE_SHIFT;
 	size = vma->vm_end - vma->vm_start;
 
-	/* To stop the swapper from even considering these pages. */
-	vma->vm_flags |= (VM_IO | VM_RESERVED);
+	/* VM_IO | VM_DONTEXPAND | VM_DONTDUMP are set by remap_pfn_range() */
 
 	if (((vma->vm_pgoff == 0) && (size == info->fix.smem_len)) ||
 	    ((off == info->fix.smem_len) && (size == PAGE_SIZE)))
diff --git a/drivers/video/backlight/pwm_bl.c b/drivers/video/backlight/pwm_bl.c
index 995f0164c9b..069983ca49f 100644
--- a/drivers/video/backlight/pwm_bl.c
+++ b/drivers/video/backlight/pwm_bl.c
@@ -213,7 +213,7 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 	pb->exit = data->exit;
 	pb->dev = &pdev->dev;
 
-	pb->pwm = pwm_get(&pdev->dev, NULL);
+	pb->pwm = devm_pwm_get(&pdev->dev, NULL);
 	if (IS_ERR(pb->pwm)) {
 		dev_err(&pdev->dev, "unable to request PWM, trying legacy API\n");
 
@@ -246,7 +246,7 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 	if (IS_ERR(bl)) {
 		dev_err(&pdev->dev, "failed to register backlight\n");
 		ret = PTR_ERR(bl);
-		goto err_bl;
+		goto err_alloc;
 	}
 
 	bl->props.brightness = data->dft_brightness;
@@ -255,8 +255,6 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, bl);
 	return 0;
 
-err_bl:
-	pwm_put(pb->pwm);
 err_alloc:
 	if (data->exit)
 		data->exit(&pdev->dev);
@@ -271,7 +269,6 @@ static int pwm_backlight_remove(struct platform_device *pdev)
 	backlight_device_unregister(bl);
 	pwm_config(pb->pwm, 0, pb->period);
 	pwm_disable(pb->pwm);
-	pwm_put(pb->pwm);
 	if (pb->exit)
 		pb->exit(&pdev->dev);
 	return 0;
diff --git a/drivers/video/bf537-lq035.c b/drivers/video/bf537-lq035.c
index befbc80d11f..7347aa1e5e4 100644
--- a/drivers/video/bf537-lq035.c
+++ b/drivers/video/bf537-lq035.c
@@ -760,18 +760,20 @@ static int __devinit bfin_lq035_probe(struct platform_device *pdev)
 	bfin_lq035_fb.flags = FBINFO_DEFAULT;
 
 
-	bfin_lq035_fb.pseudo_palette = kzalloc(sizeof(u32) * 16, GFP_KERNEL);
+	bfin_lq035_fb.pseudo_palette = devm_kzalloc(&pdev->dev,
+						    sizeof(u32) * 16,
+						    GFP_KERNEL);
 	if (bfin_lq035_fb.pseudo_palette == NULL) {
 		pr_err("failed to allocate pseudo_palette\n");
 		ret = -ENOMEM;
-		goto out_palette;
+		goto out_table;
 	}
 
 	if (fb_alloc_cmap(&bfin_lq035_fb.cmap, NBR_PALETTE, 0) < 0) {
 		pr_err("failed to allocate colormap (%d entries)\n",
 			NBR_PALETTE);
 		ret = -EFAULT;
-		goto out_cmap;
+		goto out_table;
 	}
 
 	if (register_framebuffer(&bfin_lq035_fb) < 0) {
@@ -804,9 +806,6 @@ out_lcd:
 	unregister_framebuffer(&bfin_lq035_fb);
 out_reg:
 	fb_dealloc_cmap(&bfin_lq035_fb.cmap);
-out_cmap:
-	kfree(bfin_lq035_fb.pseudo_palette);
-out_palette:
 out_table:
 	dma_free_coherent(NULL, TOTAL_VIDEO_MEM_SIZE, fb_buffer, 0);
 	fb_buffer = NULL;
@@ -834,7 +833,6 @@ static int __devexit bfin_lq035_remove(struct platform_device *pdev)
 	free_dma(CH_PPI);
 
 
-	kfree(bfin_lq035_fb.pseudo_palette);
 	fb_dealloc_cmap(&bfin_lq035_fb.cmap);
 
 
diff --git a/drivers/video/bf54x-lq043fb.c b/drivers/video/bf54x-lq043fb.c
index dc2f0047769..ff5663f5c64 100644
--- a/drivers/video/bf54x-lq043fb.c
+++ b/drivers/video/bf54x-lq043fb.c
@@ -525,6 +525,7 @@ static int __devinit bfin_bf54x_probe(struct platform_device *pdev)
 	info = fbinfo->par;
 	info->fb = fbinfo;
 	info->dev = &pdev->dev;
+	spin_lock_init(&info->lock);
 
 	platform_set_drvdata(pdev, fbinfo);
 
@@ -601,7 +602,8 @@ static int __devinit bfin_bf54x_probe(struct platform_device *pdev)
 
 	fbinfo->fbops = &bfin_bf54x_fb_ops;
 
-	fbinfo->pseudo_palette = kzalloc(sizeof(u32) * 16, GFP_KERNEL);
+	fbinfo->pseudo_palette = devm_kzalloc(&pdev->dev, sizeof(u32) * 16,
+					      GFP_KERNEL);
 	if (!fbinfo->pseudo_palette) {
 		printk(KERN_ERR DRIVER_NAME
 		       "Fail to allocate pseudo_palette\n");
@@ -616,7 +618,7 @@ static int __devinit bfin_bf54x_probe(struct platform_device *pdev)
 		       "Fail to allocate colormap (%d entries)\n",
 		       BFIN_LCD_NBR_PALETTE_ENTRIES);
 		ret = -EFAULT;
-		goto out5;
+		goto out4;
 	}
 
 	if (request_ports(info)) {
@@ -671,8 +673,6 @@ out7:
 	free_ports(info);
 out6:
 	fb_dealloc_cmap(&fbinfo->cmap);
-out5:
-	kfree(fbinfo->pseudo_palette);
 out4:
 	dma_free_coherent(NULL, fbinfo->fix.smem_len, info->fb_buffer,
 			  info->dma_handle);
@@ -699,7 +699,6 @@ static int __devexit bfin_bf54x_remove(struct platform_device *pdev)
 		dma_free_coherent(NULL, fbinfo->fix.smem_len, info->fb_buffer,
 				  info->dma_handle);
 
-	kfree(fbinfo->pseudo_palette);
 	fb_dealloc_cmap(&fbinfo->cmap);
 
 #ifndef NO_BL_SUPPORT
diff --git a/drivers/video/bfin-lq035q1-fb.c b/drivers/video/bfin-lq035q1-fb.c
index 353c02fe8a9..6fbc75c2f0a 100644
--- a/drivers/video/bfin-lq035q1-fb.c
+++ b/drivers/video/bfin-lq035q1-fb.c
@@ -577,6 +577,7 @@ static int __devinit bfin_lq035q1_probe(struct platform_device *pdev)
 	info = fbinfo->par;
 	info->fb = fbinfo;
 	info->dev = &pdev->dev;
+	spin_lock_init(&info->lock);
 
 	info->disp_info = pdev->dev.platform_data;
 
@@ -853,17 +854,7 @@ static struct platform_driver bfin_lq035q1_driver = {
 	},
 };
 
-static int __init bfin_lq035q1_driver_init(void)
-{
-	return platform_driver_register(&bfin_lq035q1_driver);
-}
-module_init(bfin_lq035q1_driver_init);
-
-static void __exit bfin_lq035q1_driver_cleanup(void)
-{
-	platform_driver_unregister(&bfin_lq035q1_driver);
-}
-module_exit(bfin_lq035q1_driver_cleanup);
+module_platform_driver(bfin_lq035q1_driver);
 
 MODULE_DESCRIPTION("Blackfin TFT LCD Driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/bfin-t350mcqb-fb.c b/drivers/video/bfin-t350mcqb-fb.c
index 7a0c05f3537..ae0fb24b8b4 100644
--- a/drivers/video/bfin-t350mcqb-fb.c
+++ b/drivers/video/bfin-t350mcqb-fb.c
@@ -447,6 +447,7 @@ static int __devinit bfin_t350mcqb_probe(struct platform_device *pdev)
 	info = fbinfo->par;
 	info->fb = fbinfo;
 	info->dev = &pdev->dev;
+	spin_lock_init(&info->lock);
 
 	platform_set_drvdata(pdev, fbinfo);
 
diff --git a/drivers/video/bw2.c b/drivers/video/bw2.c
index 7ba74cd4be6..6bea9a93679 100644
--- a/drivers/video/bw2.c
+++ b/drivers/video/bw2.c
@@ -319,8 +319,10 @@ static int __devinit bw2_probe(struct platform_device *op)
 
 	info->screen_base = of_ioremap(&op->resource[0], 0,
 				       info->fix.smem_len, "bw2 ram");
-	if (!info->screen_base)
+	if (!info->screen_base) {
+		err = -ENOMEM;
 		goto out_unmap_regs;
+	}
 
 	bw2_blank(FB_BLANK_UNBLANK, info);
 
diff --git a/drivers/video/cg3.c b/drivers/video/cg3.c
index f927a7b1a8d..c5e7612ff87 100644
--- a/drivers/video/cg3.c
+++ b/drivers/video/cg3.c
@@ -398,7 +398,8 @@ static int __devinit cg3_probe(struct platform_device *op)
 			goto out_unmap_screen;
 	}
 
-	if (fb_alloc_cmap(&info->cmap, 256, 0))
+	err = fb_alloc_cmap(&info->cmap, 256, 0);
+	if (err)
 		goto out_unmap_screen;
 
 	fb_set_cmap(&info->cmap, info);
diff --git a/drivers/video/cobalt_lcdfb.c b/drivers/video/cobalt_lcdfb.c
index eae46f6457e..01a4ee7cc6b 100644
--- a/drivers/video/cobalt_lcdfb.c
+++ b/drivers/video/cobalt_lcdfb.c
@@ -348,7 +348,8 @@ static int __devinit cobalt_lcdfb_probe(struct platform_device *dev)
 	}
 
 	info->screen_size = resource_size(res);
-	info->screen_base = ioremap(res->start, info->screen_size);
+	info->screen_base = devm_ioremap(&dev->dev, res->start,
+					 info->screen_size);
 	info->fbops = &cobalt_lcd_fbops;
 	info->fix = cobalt_lcdfb_fix;
 	info->fix.smem_start = res->start;
@@ -359,7 +360,6 @@ static int __devinit cobalt_lcdfb_probe(struct platform_device *dev)
 
 	retval = register_framebuffer(info);
 	if (retval < 0) {
-		iounmap(info->screen_base);
 		framebuffer_release(info);
 		return retval;
 	}
@@ -380,7 +380,6 @@ static int __devexit cobalt_lcdfb_remove(struct platform_device *dev)
 
 	info = platform_get_drvdata(dev);
 	if (info) {
-		iounmap(info->screen_base);
 		unregister_framebuffer(info);
 		framebuffer_release(info);
 	}
diff --git a/drivers/video/console/font_mini_4x6.c b/drivers/video/console/font_mini_4x6.c
index fa6e698e63c..838caa1cfef 100644
--- a/drivers/video/console/font_mini_4x6.c
+++ b/drivers/video/console/font_mini_4x6.c
@@ -1092,7 +1092,7 @@ static const unsigned char fontdata_mini_4x6[FONTDATAMAX] = {
 	/*{*/ 	/*   Char 124: '|' */
 	0x44,	/*=   [ *  ]        */
 	0x44,	/*=   [ *  ]        */
-	0x00,	/*=   [    ]        */
+	0x44,	/*=   [ *  ]        */
 	0x44,	/*=   [ *  ]        */
 	0x44,	/*=   [ *  ]        */
 	0x00,	/*=   [    ]        */
diff --git a/drivers/video/console/font_sun8x16.c b/drivers/video/console/font_sun8x16.c
index 5abf290c6eb..268151325b8 100644
--- a/drivers/video/console/font_sun8x16.c
+++ b/drivers/video/console/font_sun8x16.c
@@ -127,7 +127,7 @@ static const unsigned char fontdata_sun8x16[FONTDATAMAX] = {
 /*y*/ 0x00,0x00,0x00,0x00,0x00,0xc6,0xc6,0xc6,0xc6,0xc6,0xc6,0x7e,0x06,0x0c,0xf8,0x00,
 /*z*/ 0x00,0x00,0x00,0x00,0x00,0xfe,0xcc,0x18,0x30,0x60,0xc6,0xfe,0x00,0x00,0x00,0x00,
 /*{*/ 0x00,0x00,0x0e,0x18,0x18,0x18,0x70,0x18,0x18,0x18,0x18,0x0e,0x00,0x00,0x00,0x00,
-/*|*/ 0x00,0x00,0x18,0x18,0x18,0x18,0x00,0x18,0x18,0x18,0x18,0x18,0x00,0x00,0x00,0x00,
+/*|*/ 0x00,0x00,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x18,0x00,0x00,0x00,0x00,
 /*}*/ 0x00,0x00,0x70,0x18,0x18,0x18,0x0e,0x18,0x18,0x18,0x18,0x70,0x00,0x00,0x00,0x00,
 /*~*/ 0x00,0x00,0x76,0xdc,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
 /* */ 0x00,0x00,0x00,0x00,0x10,0x38,0x6c,0xc6,0xc6,0xc6,0xfe,0x00,0x00,0x00,0x00,0x00,
diff --git a/drivers/video/cyber2000fb.c b/drivers/video/cyber2000fb.c
index c1527f5b47e..e40125cb313 100644
--- a/drivers/video/cyber2000fb.c
+++ b/drivers/video/cyber2000fb.c
@@ -1804,8 +1804,10 @@ cyberpro_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 
 	cfb->irq = dev->irq;
 	cfb->region = pci_ioremap_bar(dev, 0);
-	if (!cfb->region)
+	if (!cfb->region) {
+		err = -ENOMEM;
 		goto failed_ioremap;
+	}
 
 	cfb->regs = cfb->region + MMIO_OFFSET;
 	cfb->fb.device = &dev->dev;
diff --git a/drivers/video/da8xx-fb.c b/drivers/video/da8xx-fb.c
index 113d43a16f5..80665f66ac1 100644
--- a/drivers/video/da8xx-fb.c
+++ b/drivers/video/da8xx-fb.c
@@ -26,7 +26,9 @@
 #include <linux/device.h>
 #include <linux/platform_device.h>
 #include <linux/uaccess.h>
+#include <linux/pm_runtime.h>
 #include <linux/interrupt.h>
+#include <linux/wait.h>
 #include <linux/clk.h>
 #include <linux/cpufreq.h>
 #include <linux/console.h>
@@ -48,6 +50,7 @@
 #define LCD_PL_LOAD_DONE		BIT(6)
 #define LCD_FIFO_UNDERFLOW		BIT(5)
 #define LCD_SYNC_LOST			BIT(2)
+#define LCD_FRAME_DONE			BIT(0)
 
 /* LCD DMA Control Register */
 #define LCD_DMA_BURST_SIZE(x)		((x) << 4)
@@ -86,6 +89,8 @@
 #define LCD_V2_LIDD_CLK_EN		BIT(1)
 #define LCD_V2_CORE_CLK_EN		BIT(0)
 #define LCD_V2_LPP_B10			26
+#define LCD_V2_TFT_24BPP_MODE		BIT(25)
+#define LCD_V2_TFT_24BPP_UNPACK		BIT(26)
 
 /* LCD Raster Timing 2 Register */
 #define LCD_AC_BIAS_TRANSITIONS_PER_INT(x)	((x) << 16)
@@ -135,6 +140,8 @@ static void __iomem *da8xx_fb_reg_base;
 static struct resource *lcdc_regs;
 static unsigned int lcd_revision;
 static irq_handler_t lcdc_irq_handler;
+static wait_queue_head_t frame_done_wq;
+static int frame_done_flag;
 
 static inline unsigned int lcdc_read(unsigned int addr)
 {
@@ -156,7 +163,6 @@ struct da8xx_fb_par {
 	unsigned int		dma_end;
 	struct clk *lcdc_clk;
 	int irq;
-	unsigned short pseudo_palette[16];
 	unsigned int palette_sz;
 	unsigned int pxl_clk;
 	int blank;
@@ -175,6 +181,7 @@ struct da8xx_fb_par {
 	unsigned int		lcd_fck_rate;
 #endif
 	void (*panel_power_ctrl)(int);
+	u32 pseudo_palette[16];
 };
 
 /* Variable Screen Information */
@@ -288,13 +295,26 @@ static inline void lcd_enable_raster(void)
 }
 
 /* Disable the Raster Engine of the LCD Controller */
-static inline void lcd_disable_raster(void)
+static inline void lcd_disable_raster(bool wait_for_frame_done)
 {
 	u32 reg;
+	int ret;
 
 	reg = lcdc_read(LCD_RASTER_CTRL_REG);
 	if (reg & LCD_RASTER_ENABLE)
 		lcdc_write(reg & ~LCD_RASTER_ENABLE, LCD_RASTER_CTRL_REG);
+	else
+		/* return if already disabled */
+		return;
+
+	if ((wait_for_frame_done == true) && (lcd_revision == LCD_VERSION_2)) {
+		frame_done_flag = 0;
+		ret = wait_event_interruptible_timeout(frame_done_wq,
+				frame_done_flag != 0,
+				msecs_to_jiffies(50));
+		if (ret == 0)
+			pr_err("LCD Controller timed out\n");
+	}
 }
 
 static void lcd_blit(int load_mode, struct da8xx_fb_par *par)
@@ -321,7 +341,8 @@ static void lcd_blit(int load_mode, struct da8xx_fb_par *par)
 		} else {
 			reg_int = lcdc_read(LCD_INT_ENABLE_SET_REG) |
 				LCD_V2_END_OF_FRAME0_INT_ENA |
-				LCD_V2_END_OF_FRAME1_INT_ENA;
+				LCD_V2_END_OF_FRAME1_INT_ENA |
+				LCD_FRAME_DONE;
 			lcdc_write(reg_int, LCD_INT_ENABLE_SET_REG);
 		}
 		reg_dma |= LCD_DUAL_FRAME_BUFFER_ENABLE;
@@ -499,6 +520,9 @@ static int lcd_cfg_frame_buffer(struct da8xx_fb_par *par, u32 width, u32 height,
 {
 	u32 reg;
 
+	if (bpp > 16 && lcd_revision == LCD_VERSION_1)
+		return -EINVAL;
+
 	/* Set the Panel Width */
 	/* Pixels per line = (PPL + 1)*16 */
 	if (lcd_revision == LCD_VERSION_1) {
@@ -542,14 +566,19 @@ static int lcd_cfg_frame_buffer(struct da8xx_fb_par *par, u32 width, u32 height,
 	reg = lcdc_read(LCD_RASTER_CTRL_REG) & ~(1 << 8);
 	if (raster_order)
 		reg |= LCD_RASTER_ORDER;
-	lcdc_write(reg, LCD_RASTER_CTRL_REG);
+
+	par->palette_sz = 16 * 2;
 
 	switch (bpp) {
 	case 1:
 	case 2:
 	case 4:
 	case 16:
-		par->palette_sz = 16 * 2;
+		break;
+	case 24:
+		reg |= LCD_V2_TFT_24BPP_MODE;
+	case 32:
+		reg |= LCD_V2_TFT_24BPP_UNPACK;
 		break;
 
 	case 8:
@@ -560,9 +589,12 @@ static int lcd_cfg_frame_buffer(struct da8xx_fb_par *par, u32 width, u32 height,
 		return -EINVAL;
 	}
 
+	lcdc_write(reg, LCD_RASTER_CTRL_REG);
+
 	return 0;
 }
 
+#define CNVT_TOHW(val, width) ((((val) << (width)) + 0x7FFF - (val)) >> 16)
 static int fb_setcolreg(unsigned regno, unsigned red, unsigned green,
 			      unsigned blue, unsigned transp,
 			      struct fb_info *info)
@@ -578,13 +610,38 @@ static int fb_setcolreg(unsigned regno, unsigned red, unsigned green,
 	if (info->fix.visual == FB_VISUAL_DIRECTCOLOR)
 		return 1;
 
-	if (info->var.bits_per_pixel == 4) {
-		if (regno > 15)
-			return 1;
+	if (info->var.bits_per_pixel > 16 && lcd_revision == LCD_VERSION_1)
+		return -EINVAL;
 
-		if (info->var.grayscale) {
-			pal = regno;
-		} else {
+	switch (info->fix.visual) {
+	case FB_VISUAL_TRUECOLOR:
+		red = CNVT_TOHW(red, info->var.red.length);
+		green = CNVT_TOHW(green, info->var.green.length);
+		blue = CNVT_TOHW(blue, info->var.blue.length);
+		break;
+	case FB_VISUAL_PSEUDOCOLOR:
+		switch (info->var.bits_per_pixel) {
+		case 4:
+			if (regno > 15)
+				return -EINVAL;
+
+			if (info->var.grayscale) {
+				pal = regno;
+			} else {
+				red >>= 4;
+				green >>= 8;
+				blue >>= 12;
+
+				pal = red & 0x0f00;
+				pal |= green & 0x00f0;
+				pal |= blue & 0x000f;
+			}
+			if (regno == 0)
+				pal |= 0x2000;
+			palette[regno] = pal;
+			break;
+
+		case 8:
 			red >>= 4;
 			green >>= 8;
 			blue >>= 12;
@@ -592,36 +649,36 @@ static int fb_setcolreg(unsigned regno, unsigned red, unsigned green,
 			pal = (red & 0x0f00);
 			pal |= (green & 0x00f0);
 			pal |= (blue & 0x000f);
-		}
-		if (regno == 0)
-			pal |= 0x2000;
-		palette[regno] = pal;
-
-	} else if (info->var.bits_per_pixel == 8) {
-		red >>= 4;
-		green >>= 8;
-		blue >>= 12;
-
-		pal = (red & 0x0f00);
-		pal |= (green & 0x00f0);
-		pal |= (blue & 0x000f);
 
-		if (palette[regno] != pal) {
-			update_hw = 1;
-			palette[regno] = pal;
+			if (palette[regno] != pal) {
+				update_hw = 1;
+				palette[regno] = pal;
+			}
+			break;
 		}
-	} else if ((info->var.bits_per_pixel == 16) && regno < 16) {
-		red >>= (16 - info->var.red.length);
-		red <<= info->var.red.offset;
+		break;
+	}
 
-		green >>= (16 - info->var.green.length);
-		green <<= info->var.green.offset;
+	/* Truecolor has hardware independent palette */
+	if (info->fix.visual == FB_VISUAL_TRUECOLOR) {
+		u32 v;
 
-		blue >>= (16 - info->var.blue.length);
-		blue <<= info->var.blue.offset;
+		if (regno > 15)
+			return -EINVAL;
 
-		par->pseudo_palette[regno] = red | green | blue;
+		v = (red << info->var.red.offset) |
+			(green << info->var.green.offset) |
+			(blue << info->var.blue.offset);
 
+		switch (info->var.bits_per_pixel) {
+		case 16:
+			((u16 *) (info->pseudo_palette))[regno] = v;
+			break;
+		case 24:
+		case 32:
+			((u32 *) (info->pseudo_palette))[regno] = v;
+			break;
+		}
 		if (palette[0] != 0x4000) {
 			update_hw = 1;
 			palette[0] = 0x4000;
@@ -634,11 +691,12 @@ static int fb_setcolreg(unsigned regno, unsigned red, unsigned green,
 
 	return 0;
 }
+#undef CNVT_TOHW
 
 static void lcd_reset(struct da8xx_fb_par *par)
 {
 	/* Disable the Raster if previously Enabled */
-	lcd_disable_raster();
+	lcd_disable_raster(false);
 
 	/* DMA has to be disabled */
 	lcdc_write(0, LCD_DMA_CTRL_REG);
@@ -734,7 +792,7 @@ static irqreturn_t lcdc_irq_handler_rev02(int irq, void *arg)
 	u32 stat = lcdc_read(LCD_MASKED_STAT_REG);
 
 	if ((stat & LCD_SYNC_LOST) && (stat & LCD_FIFO_UNDERFLOW)) {
-		lcd_disable_raster();
+		lcd_disable_raster(false);
 		lcdc_write(stat, LCD_MASKED_STAT_REG);
 		lcd_enable_raster();
 	} else if (stat & LCD_PL_LOAD_DONE) {
@@ -744,7 +802,7 @@ static irqreturn_t lcdc_irq_handler_rev02(int irq, void *arg)
 		 * interrupt via the following write to the status register. If
 		 * this is done after then one gets multiple PL done interrupts.
 		 */
-		lcd_disable_raster();
+		lcd_disable_raster(false);
 
 		lcdc_write(stat, LCD_MASKED_STAT_REG);
 
@@ -775,6 +833,14 @@ static irqreturn_t lcdc_irq_handler_rev02(int irq, void *arg)
 			par->vsync_flag = 1;
 			wake_up_interruptible(&par->vsync_wait);
 		}
+
+		/* Set only when controller is disabled and at the end of
+		 * active frame
+		 */
+		if (stat & BIT(0)) {
+			frame_done_flag = 1;
+			wake_up_interruptible(&frame_done_wq);
+		}
 	}
 
 	lcdc_write(0, LCD_END_OF_INT_IND_REG);
@@ -789,7 +855,7 @@ static irqreturn_t lcdc_irq_handler_rev01(int irq, void *arg)
 	u32 reg_ras;
 
 	if ((stat & LCD_SYNC_LOST) && (stat & LCD_FIFO_UNDERFLOW)) {
-		lcd_disable_raster();
+		lcd_disable_raster(false);
 		lcdc_write(stat, LCD_STAT_REG);
 		lcd_enable_raster();
 	} else if (stat & LCD_PL_LOAD_DONE) {
@@ -799,7 +865,7 @@ static irqreturn_t lcdc_irq_handler_rev01(int irq, void *arg)
 		 * interrupt via the following write to the status register. If
 		 * this is done after then one gets multiple PL done interrupts.
 		 */
-		lcd_disable_raster();
+		lcd_disable_raster(false);
 
 		lcdc_write(stat, LCD_STAT_REG);
 
@@ -842,6 +908,9 @@ static int fb_check_var(struct fb_var_screeninfo *var,
 {
 	int err = 0;
 
+	if (var->bits_per_pixel > 16 && lcd_revision == LCD_VERSION_1)
+		return -EINVAL;
+
 	switch (var->bits_per_pixel) {
 	case 1:
 	case 8:
@@ -877,6 +946,26 @@ static int fb_check_var(struct fb_var_screeninfo *var,
 		var->transp.length = 0;
 		var->nonstd = 0;
 		break;
+	case 24:
+		var->red.offset = 16;
+		var->red.length = 8;
+		var->green.offset = 8;
+		var->green.length = 8;
+		var->blue.offset = 0;
+		var->blue.length = 8;
+		var->nonstd = 0;
+		break;
+	case 32:
+		var->transp.offset = 24;
+		var->transp.length = 8;
+		var->red.offset = 16;
+		var->red.length = 8;
+		var->green.offset = 8;
+		var->green.length = 8;
+		var->blue.offset = 0;
+		var->blue.length = 8;
+		var->nonstd = 0;
+		break;
 	default:
 		err = -EINVAL;
 	}
@@ -898,9 +987,10 @@ static int lcd_da8xx_cpufreq_transition(struct notifier_block *nb,
 	if (val == CPUFREQ_POSTCHANGE) {
 		if (par->lcd_fck_rate != clk_get_rate(par->lcdc_clk)) {
 			par->lcd_fck_rate = clk_get_rate(par->lcdc_clk);
-			lcd_disable_raster();
+			lcd_disable_raster(true);
 			lcd_calc_clk_divider(par);
-			lcd_enable_raster();
+			if (par->blank == FB_BLANK_UNBLANK)
+				lcd_enable_raster();
 		}
 	}
 
@@ -935,7 +1025,7 @@ static int __devexit fb_remove(struct platform_device *dev)
 		if (par->panel_power_ctrl)
 			par->panel_power_ctrl(0);
 
-		lcd_disable_raster();
+		lcd_disable_raster(true);
 		lcdc_write(0, LCD_RASTER_CTRL_REG);
 
 		/* disable DMA  */
@@ -948,8 +1038,8 @@ static int __devexit fb_remove(struct platform_device *dev)
 		dma_free_coherent(NULL, par->vram_size, par->vram_virt,
 				  par->vram_phys);
 		free_irq(par->irq, par);
-		clk_disable(par->lcdc_clk);
-		clk_put(par->lcdc_clk);
+		pm_runtime_put_sync(&dev->dev);
+		pm_runtime_disable(&dev->dev);
 		framebuffer_release(info);
 		iounmap(da8xx_fb_reg_base);
 		release_mem_region(lcdc_regs->start, resource_size(lcdc_regs));
@@ -1051,7 +1141,7 @@ static int cfb_blank(int blank, struct fb_info *info)
 		if (par->panel_power_ctrl)
 			par->panel_power_ctrl(0);
 
-		lcd_disable_raster();
+		lcd_disable_raster(true);
 		break;
 	default:
 		ret = -EINVAL;
@@ -1183,9 +1273,9 @@ static int __devinit fb_probe(struct platform_device *device)
 		ret = -ENODEV;
 		goto err_ioremap;
 	}
-	ret = clk_enable(fb_clk);
-	if (ret)
-		goto err_clk_put;
+
+	pm_runtime_enable(&device->dev);
+	pm_runtime_get_sync(&device->dev);
 
 	/* Determine LCD IP Version */
 	switch (lcdc_read(LCD_PID_REG)) {
@@ -1213,7 +1303,7 @@ static int __devinit fb_probe(struct platform_device *device)
 	if (i == ARRAY_SIZE(known_lcd_panels)) {
 		dev_err(&device->dev, "GLCD: No valid panel found\n");
 		ret = -ENODEV;
-		goto err_clk_disable;
+		goto err_pm_runtime_disable;
 	} else
 		dev_info(&device->dev, "GLCD: Found %s panel\n",
 					fb_pdata->type);
@@ -1225,7 +1315,7 @@ static int __devinit fb_probe(struct platform_device *device)
 	if (!da8xx_fb_info) {
 		dev_dbg(&device->dev, "Memory allocation failed for fb_info\n");
 		ret = -ENOMEM;
-		goto err_clk_disable;
+		goto err_pm_runtime_disable;
 	}
 
 	par = da8xx_fb_info->par;
@@ -1356,8 +1446,10 @@ static int __devinit fb_probe(struct platform_device *device)
 
 	if (lcd_revision == LCD_VERSION_1)
 		lcdc_irq_handler = lcdc_irq_handler_rev01;
-	else
+	else {
+		init_waitqueue_head(&frame_done_wq);
 		lcdc_irq_handler = lcdc_irq_handler_rev02;
+	}
 
 	ret = request_irq(par->irq, lcdc_irq_handler, 0,
 			DRIVER_NAME, par);
@@ -1385,11 +1477,9 @@ err_release_fb_mem:
 err_release_fb:
 	framebuffer_release(da8xx_fb_info);
 
-err_clk_disable:
-	clk_disable(fb_clk);
-
-err_clk_put:
-	clk_put(fb_clk);
+err_pm_runtime_disable:
+	pm_runtime_put_sync(&device->dev);
+	pm_runtime_disable(&device->dev);
 
 err_ioremap:
 	iounmap(da8xx_fb_reg_base);
@@ -1401,6 +1491,69 @@ err_request_mem:
 }
 
 #ifdef CONFIG_PM
+struct lcdc_context {
+	u32 clk_enable;
+	u32 ctrl;
+	u32 dma_ctrl;
+	u32 raster_timing_0;
+	u32 raster_timing_1;
+	u32 raster_timing_2;
+	u32 int_enable_set;
+	u32 dma_frm_buf_base_addr_0;
+	u32 dma_frm_buf_ceiling_addr_0;
+	u32 dma_frm_buf_base_addr_1;
+	u32 dma_frm_buf_ceiling_addr_1;
+	u32 raster_ctrl;
+} reg_context;
+
+static void lcd_context_save(void)
+{
+	if (lcd_revision == LCD_VERSION_2) {
+		reg_context.clk_enable = lcdc_read(LCD_CLK_ENABLE_REG);
+		reg_context.int_enable_set = lcdc_read(LCD_INT_ENABLE_SET_REG);
+	}
+
+	reg_context.ctrl = lcdc_read(LCD_CTRL_REG);
+	reg_context.dma_ctrl = lcdc_read(LCD_DMA_CTRL_REG);
+	reg_context.raster_timing_0 = lcdc_read(LCD_RASTER_TIMING_0_REG);
+	reg_context.raster_timing_1 = lcdc_read(LCD_RASTER_TIMING_1_REG);
+	reg_context.raster_timing_2 = lcdc_read(LCD_RASTER_TIMING_2_REG);
+	reg_context.dma_frm_buf_base_addr_0 =
+		lcdc_read(LCD_DMA_FRM_BUF_BASE_ADDR_0_REG);
+	reg_context.dma_frm_buf_ceiling_addr_0 =
+		lcdc_read(LCD_DMA_FRM_BUF_CEILING_ADDR_0_REG);
+	reg_context.dma_frm_buf_base_addr_1 =
+		lcdc_read(LCD_DMA_FRM_BUF_BASE_ADDR_1_REG);
+	reg_context.dma_frm_buf_ceiling_addr_1 =
+		lcdc_read(LCD_DMA_FRM_BUF_CEILING_ADDR_1_REG);
+	reg_context.raster_ctrl = lcdc_read(LCD_RASTER_CTRL_REG);
+	return;
+}
+
+static void lcd_context_restore(void)
+{
+	if (lcd_revision == LCD_VERSION_2) {
+		lcdc_write(reg_context.clk_enable, LCD_CLK_ENABLE_REG);
+		lcdc_write(reg_context.int_enable_set, LCD_INT_ENABLE_SET_REG);
+	}
+
+	lcdc_write(reg_context.ctrl, LCD_CTRL_REG);
+	lcdc_write(reg_context.dma_ctrl, LCD_DMA_CTRL_REG);
+	lcdc_write(reg_context.raster_timing_0, LCD_RASTER_TIMING_0_REG);
+	lcdc_write(reg_context.raster_timing_1, LCD_RASTER_TIMING_1_REG);
+	lcdc_write(reg_context.raster_timing_2, LCD_RASTER_TIMING_2_REG);
+	lcdc_write(reg_context.dma_frm_buf_base_addr_0,
+			LCD_DMA_FRM_BUF_BASE_ADDR_0_REG);
+	lcdc_write(reg_context.dma_frm_buf_ceiling_addr_0,
+			LCD_DMA_FRM_BUF_CEILING_ADDR_0_REG);
+	lcdc_write(reg_context.dma_frm_buf_base_addr_1,
+			LCD_DMA_FRM_BUF_BASE_ADDR_1_REG);
+	lcdc_write(reg_context.dma_frm_buf_ceiling_addr_1,
+			LCD_DMA_FRM_BUF_CEILING_ADDR_1_REG);
+	lcdc_write(reg_context.raster_ctrl, LCD_RASTER_CTRL_REG);
+	return;
+}
+
 static int fb_suspend(struct platform_device *dev, pm_message_t state)
 {
 	struct fb_info *info = platform_get_drvdata(dev);
@@ -1411,8 +1564,9 @@ static int fb_suspend(struct platform_device *dev, pm_message_t state)
 		par->panel_power_ctrl(0);
 
 	fb_set_suspend(info, 1);
-	lcd_disable_raster();
-	clk_disable(par->lcdc_clk);
+	lcd_disable_raster(true);
+	lcd_context_save();
+	pm_runtime_put_sync(&dev->dev);
 	console_unlock();
 
 	return 0;
@@ -1423,11 +1577,14 @@ static int fb_resume(struct platform_device *dev)
 	struct da8xx_fb_par *par = info->par;
 
 	console_lock();
-	clk_enable(par->lcdc_clk);
-	lcd_enable_raster();
+	pm_runtime_get_sync(&dev->dev);
+	lcd_context_restore();
+	if (par->blank == FB_BLANK_UNBLANK) {
+		lcd_enable_raster();
 
-	if (par->panel_power_ctrl)
-		par->panel_power_ctrl(1);
+		if (par->panel_power_ctrl)
+			par->panel_power_ctrl(1);
+	}
 
 	fb_set_suspend(info, 0);
 	console_unlock();
diff --git a/drivers/video/ep93xx-fb.c b/drivers/video/ep93xx-fb.c
index f2c092da84b..755ef3e65ca 100644
--- a/drivers/video/ep93xx-fb.c
+++ b/drivers/video/ep93xx-fb.c
@@ -529,7 +529,8 @@ static int __devinit ep93xxfb_probe(struct platform_device *pdev)
 	 * any of the framebuffer registers.
 	 */
 	fbi->res = res;
-	fbi->mmio_base = ioremap(res->start, resource_size(res));
+	fbi->mmio_base = devm_ioremap(&pdev->dev, res->start,
+				      resource_size(res));
 	if (!fbi->mmio_base) {
 		err = -ENXIO;
 		goto failed_resource;
@@ -553,20 +554,20 @@ static int __devinit ep93xxfb_probe(struct platform_device *pdev)
 	if (err == 0) {
 		dev_err(info->dev, "No suitable video mode found\n");
 		err = -EINVAL;
-		goto failed_mode;
+		goto failed_resource;
 	}
 
 	if (mach_info->setup) {
 		err = mach_info->setup(pdev);
 		if (err)
-			goto failed_mode;
+			goto failed_resource;
 	}
 
 	err = ep93xxfb_check_var(&info->var, info);
 	if (err)
 		goto failed_check;
 
-	fbi->clk = clk_get(info->dev, NULL);
+	fbi->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(fbi->clk)) {
 		err = PTR_ERR(fbi->clk);
 		fbi->clk = NULL;
@@ -578,19 +579,15 @@ static int __devinit ep93xxfb_probe(struct platform_device *pdev)
 
 	err = register_framebuffer(info);
 	if (err)
-		goto failed;
+		goto failed_check;
 
 	dev_info(info->dev, "registered. Mode = %dx%d-%d\n",
 		 info->var.xres, info->var.yres, info->var.bits_per_pixel);
 	return 0;
 
-failed:
-	clk_put(fbi->clk);
 failed_check:
 	if (fbi->mach_info->teardown)
 		fbi->mach_info->teardown(pdev);
-failed_mode:
-	iounmap(fbi->mmio_base);
 failed_resource:
 	ep93xxfb_dealloc_videomem(info);
 failed_videomem:
@@ -609,8 +606,6 @@ static int __devexit ep93xxfb_remove(struct platform_device *pdev)
 
 	unregister_framebuffer(info);
 	clk_disable(fbi->clk);
-	clk_put(fbi->clk);
-	iounmap(fbi->mmio_base);
 	ep93xxfb_dealloc_videomem(info);
 	fb_dealloc_cmap(&info->cmap);
 
diff --git a/drivers/video/exynos/exynos_dp_core.c b/drivers/video/exynos/exynos_dp_core.c
index c6c016a506c..d55470e7541 100644
--- a/drivers/video/exynos/exynos_dp_core.c
+++ b/drivers/video/exynos/exynos_dp_core.c
@@ -29,6 +29,9 @@ static int exynos_dp_init_dp(struct exynos_dp_device *dp)
 
 	exynos_dp_swreset(dp);
 
+	exynos_dp_init_analog_param(dp);
+	exynos_dp_init_interrupt(dp);
+
 	/* SW defined function Normal operation */
 	exynos_dp_enable_sw_function(dp);
 
@@ -260,7 +263,7 @@ static void exynos_dp_set_lane_lane_pre_emphasis(struct exynos_dp_device *dp,
 
 static void exynos_dp_link_start(struct exynos_dp_device *dp)
 {
-	u8 buf[5];
+	u8 buf[4];
 	int lane;
 	int lane_count;
 
@@ -295,10 +298,10 @@ static void exynos_dp_link_start(struct exynos_dp_device *dp)
 	exynos_dp_set_training_pattern(dp, TRAINING_PTN1);
 
 	/* Set RX training pattern */
-	buf[0] = DPCD_SCRAMBLING_DISABLED |
-		 DPCD_TRAINING_PATTERN_1;
 	exynos_dp_write_byte_to_dpcd(dp,
-		DPCD_ADDR_TRAINING_PATTERN_SET, buf[0]);
+		DPCD_ADDR_TRAINING_PATTERN_SET,
+		DPCD_SCRAMBLING_DISABLED |
+		DPCD_TRAINING_PATTERN_1);
 
 	for (lane = 0; lane < lane_count; lane++)
 		buf[lane] = DPCD_PRE_EMPHASIS_PATTERN2_LEVEL0 |
@@ -308,7 +311,7 @@ static void exynos_dp_link_start(struct exynos_dp_device *dp)
 		lane_count, buf);
 }
 
-static unsigned char exynos_dp_get_lane_status(u8 link_status[6], int lane)
+static unsigned char exynos_dp_get_lane_status(u8 link_status[2], int lane)
 {
 	int shift = (lane & 1) * 4;
 	u8 link_value = link_status[lane>>1];
@@ -316,7 +319,7 @@ static unsigned char exynos_dp_get_lane_status(u8 link_status[6], int lane)
 	return (link_value >> shift) & 0xf;
 }
 
-static int exynos_dp_clock_recovery_ok(u8 link_status[6], int lane_count)
+static int exynos_dp_clock_recovery_ok(u8 link_status[2], int lane_count)
 {
 	int lane;
 	u8 lane_status;
@@ -329,22 +332,23 @@ static int exynos_dp_clock_recovery_ok(u8 link_status[6], int lane_count)
 	return 0;
 }
 
-static int exynos_dp_channel_eq_ok(u8 link_status[6], int lane_count)
+static int exynos_dp_channel_eq_ok(u8 link_align[3], int lane_count)
 {
 	int lane;
 	u8 lane_align;
 	u8 lane_status;
 
-	lane_align = link_status[2];
+	lane_align = link_align[2];
 	if ((lane_align & DPCD_INTERLANE_ALIGN_DONE) == 0)
 		return -EINVAL;
 
 	for (lane = 0; lane < lane_count; lane++) {
-		lane_status = exynos_dp_get_lane_status(link_status, lane);
+		lane_status = exynos_dp_get_lane_status(link_align, lane);
 		lane_status &= DPCD_CHANNEL_EQ_BITS;
 		if (lane_status != DPCD_CHANNEL_EQ_BITS)
 			return -EINVAL;
 	}
+
 	return 0;
 }
 
@@ -417,69 +421,17 @@ static unsigned int exynos_dp_get_lane_link_training(
 
 static void exynos_dp_reduce_link_rate(struct exynos_dp_device *dp)
 {
-	if (dp->link_train.link_rate == LINK_RATE_2_70GBPS) {
-		/* set to reduced bit rate */
-		dp->link_train.link_rate = LINK_RATE_1_62GBPS;
-		dev_err(dp->dev, "set to bandwidth %.2x\n",
-			dp->link_train.link_rate);
-		dp->link_train.lt_state = START;
-	} else {
-		exynos_dp_training_pattern_dis(dp);
-		/* set enhanced mode if available */
-		exynos_dp_set_enhanced_mode(dp);
-		dp->link_train.lt_state = FAILED;
-	}
-}
+	exynos_dp_training_pattern_dis(dp);
+	exynos_dp_set_enhanced_mode(dp);
 
-static void exynos_dp_get_adjust_train(struct exynos_dp_device *dp,
-				u8 adjust_request[2])
-{
-	int lane;
-	int lane_count;
-	u8 voltage_swing;
-	u8 pre_emphasis;
-	u8 training_lane;
-
-	lane_count = dp->link_train.lane_count;
-	for (lane = 0; lane < lane_count; lane++) {
-		voltage_swing = exynos_dp_get_adjust_request_voltage(
-						adjust_request, lane);
-		pre_emphasis = exynos_dp_get_adjust_request_pre_emphasis(
-						adjust_request, lane);
-		training_lane = DPCD_VOLTAGE_SWING_SET(voltage_swing) |
-				DPCD_PRE_EMPHASIS_SET(pre_emphasis);
-
-		if (voltage_swing == VOLTAGE_LEVEL_3 ||
-		   pre_emphasis == PRE_EMPHASIS_LEVEL_3) {
-			training_lane |= DPCD_MAX_SWING_REACHED;
-			training_lane |= DPCD_MAX_PRE_EMPHASIS_REACHED;
-		}
-		dp->link_train.training_lane[lane] = training_lane;
-	}
-}
-
-static int exynos_dp_check_max_cr_loop(struct exynos_dp_device *dp,
-					u8 voltage_swing)
-{
-	int lane;
-	int lane_count;
-
-	lane_count = dp->link_train.lane_count;
-	for (lane = 0; lane < lane_count; lane++) {
-		if (voltage_swing == VOLTAGE_LEVEL_3 ||
-			dp->link_train.cr_loop[lane] == MAX_CR_LOOP)
-			return -EINVAL;
-	}
-	return 0;
+	dp->link_train.lt_state = FAILED;
 }
 
 static int exynos_dp_process_clock_recovery(struct exynos_dp_device *dp)
 {
-	u8 data;
-	u8 link_status[6];
+	u8 link_status[2];
 	int lane;
 	int lane_count;
-	u8 buf[5];
 
 	u8 adjust_request[2];
 	u8 voltage_swing;
@@ -488,100 +440,154 @@ static int exynos_dp_process_clock_recovery(struct exynos_dp_device *dp)
 
 	usleep_range(100, 101);
 
-	exynos_dp_read_bytes_from_dpcd(dp, DPCD_ADDR_LANE0_1_STATUS,
-				6, link_status);
 	lane_count = dp->link_train.lane_count;
 
+	exynos_dp_read_bytes_from_dpcd(dp, DPCD_ADDR_LANE0_1_STATUS,
+				2, link_status);
+
 	if (exynos_dp_clock_recovery_ok(link_status, lane_count) == 0) {
 		/* set training pattern 2 for EQ */
 		exynos_dp_set_training_pattern(dp, TRAINING_PTN2);
 
-		adjust_request[0] = link_status[4];
-		adjust_request[1] = link_status[5];
+		for (lane = 0; lane < lane_count; lane++) {
+			exynos_dp_read_bytes_from_dpcd(dp,
+					DPCD_ADDR_ADJUST_REQUEST_LANE0_1,
+					2, adjust_request);
+			voltage_swing = exynos_dp_get_adjust_request_voltage(
+							adjust_request, lane);
+			pre_emphasis = exynos_dp_get_adjust_request_pre_emphasis(
+							adjust_request, lane);
+			training_lane = DPCD_VOLTAGE_SWING_SET(voltage_swing) |
+					DPCD_PRE_EMPHASIS_SET(pre_emphasis);
 
-		exynos_dp_get_adjust_train(dp, adjust_request);
+			if (voltage_swing == VOLTAGE_LEVEL_3)
+				training_lane |= DPCD_MAX_SWING_REACHED;
+			if (pre_emphasis == PRE_EMPHASIS_LEVEL_3)
+				training_lane |= DPCD_MAX_PRE_EMPHASIS_REACHED;
 
-		buf[0] = DPCD_SCRAMBLING_DISABLED |
-			 DPCD_TRAINING_PATTERN_2;
-		exynos_dp_write_byte_to_dpcd(dp,
-			DPCD_ADDR_TRAINING_PATTERN_SET,
-			buf[0]);
+			dp->link_train.training_lane[lane] = training_lane;
 
-		for (lane = 0; lane < lane_count; lane++) {
 			exynos_dp_set_lane_link_training(dp,
 				dp->link_train.training_lane[lane],
 				lane);
-			buf[lane] = dp->link_train.training_lane[lane];
-			exynos_dp_write_byte_to_dpcd(dp,
-				DPCD_ADDR_TRAINING_LANE0_SET + lane,
-				buf[lane]);
 		}
-		dp->link_train.lt_state = EQUALIZER_TRAINING;
-	} else {
-		exynos_dp_read_byte_from_dpcd(dp,
-			DPCD_ADDR_ADJUST_REQUEST_LANE0_1,
-			&data);
-		adjust_request[0] = data;
 
-		exynos_dp_read_byte_from_dpcd(dp,
-			DPCD_ADDR_ADJUST_REQUEST_LANE2_3,
-			&data);
-		adjust_request[1] = data;
+		exynos_dp_write_byte_to_dpcd(dp,
+			DPCD_ADDR_TRAINING_PATTERN_SET,
+			DPCD_SCRAMBLING_DISABLED |
+			DPCD_TRAINING_PATTERN_2);
+
+		exynos_dp_write_bytes_to_dpcd(dp,
+			DPCD_ADDR_TRAINING_LANE0_SET,
+			lane_count,
+			dp->link_train.training_lane);
 
+		dev_info(dp->dev, "Link Training Clock Recovery success\n");
+		dp->link_train.lt_state = EQUALIZER_TRAINING;
+	} else {
 		for (lane = 0; lane < lane_count; lane++) {
 			training_lane = exynos_dp_get_lane_link_training(
 							dp, lane);
+			exynos_dp_read_bytes_from_dpcd(dp,
+					DPCD_ADDR_ADJUST_REQUEST_LANE0_1,
+					2, adjust_request);
 			voltage_swing = exynos_dp_get_adjust_request_voltage(
 							adjust_request, lane);
 			pre_emphasis = exynos_dp_get_adjust_request_pre_emphasis(
 							adjust_request, lane);
-			if ((DPCD_VOLTAGE_SWING_GET(training_lane) == voltage_swing) &&
-			    (DPCD_PRE_EMPHASIS_GET(training_lane) == pre_emphasis))
-				dp->link_train.cr_loop[lane]++;
-			dp->link_train.training_lane[lane] = training_lane;
-		}
 
-		if (exynos_dp_check_max_cr_loop(dp, voltage_swing) != 0) {
-			exynos_dp_reduce_link_rate(dp);
-		} else {
-			exynos_dp_get_adjust_train(dp, adjust_request);
+			if (voltage_swing == VOLTAGE_LEVEL_3 ||
+			    pre_emphasis == PRE_EMPHASIS_LEVEL_3) {
+				dev_err(dp->dev, "voltage or pre emphasis reached max level\n");
+				goto reduce_link_rate;
+			}
 
-			for (lane = 0; lane < lane_count; lane++) {
-				exynos_dp_set_lane_link_training(dp,
-					dp->link_train.training_lane[lane],
-					lane);
-				buf[lane] = dp->link_train.training_lane[lane];
-				exynos_dp_write_byte_to_dpcd(dp,
-					DPCD_ADDR_TRAINING_LANE0_SET + lane,
-					buf[lane]);
+			if ((DPCD_VOLTAGE_SWING_GET(training_lane) ==
+					voltage_swing) &&
+			   (DPCD_PRE_EMPHASIS_GET(training_lane) ==
+					pre_emphasis)) {
+				dp->link_train.cr_loop[lane]++;
+				if (dp->link_train.cr_loop[lane] == MAX_CR_LOOP) {
+					dev_err(dp->dev, "CR Max loop\n");
+					goto reduce_link_rate;
+				}
 			}
+
+			training_lane = DPCD_VOLTAGE_SWING_SET(voltage_swing) |
+					DPCD_PRE_EMPHASIS_SET(pre_emphasis);
+
+			if (voltage_swing == VOLTAGE_LEVEL_3)
+				training_lane |= DPCD_MAX_SWING_REACHED;
+			if (pre_emphasis == PRE_EMPHASIS_LEVEL_3)
+				training_lane |= DPCD_MAX_PRE_EMPHASIS_REACHED;
+
+			dp->link_train.training_lane[lane] = training_lane;
+
+			exynos_dp_set_lane_link_training(dp,
+				dp->link_train.training_lane[lane], lane);
 		}
+
+		exynos_dp_write_bytes_to_dpcd(dp,
+			DPCD_ADDR_TRAINING_LANE0_SET,
+			lane_count,
+			dp->link_train.training_lane);
 	}
 
 	return 0;
+
+reduce_link_rate:
+	exynos_dp_reduce_link_rate(dp);
+	return -EIO;
 }
 
 static int exynos_dp_process_equalizer_training(struct exynos_dp_device *dp)
 {
-	u8 link_status[6];
+	u8 link_status[2];
+	u8 link_align[3];
 	int lane;
 	int lane_count;
-	u8 buf[5];
 	u32 reg;
 
 	u8 adjust_request[2];
+	u8 voltage_swing;
+	u8 pre_emphasis;
+	u8 training_lane;
 
 	usleep_range(400, 401);
 
-	exynos_dp_read_bytes_from_dpcd(dp, DPCD_ADDR_LANE0_1_STATUS,
-				6, link_status);
 	lane_count = dp->link_train.lane_count;
 
+	exynos_dp_read_bytes_from_dpcd(dp, DPCD_ADDR_LANE0_1_STATUS,
+				2, link_status);
+
 	if (exynos_dp_clock_recovery_ok(link_status, lane_count) == 0) {
-		adjust_request[0] = link_status[4];
-		adjust_request[1] = link_status[5];
+		link_align[0] = link_status[0];
+		link_align[1] = link_status[1];
 
-		if (exynos_dp_channel_eq_ok(link_status, lane_count) == 0) {
+		exynos_dp_read_byte_from_dpcd(dp,
+			DPCD_ADDR_LANE_ALIGN_STATUS_UPDATED,
+			&link_align[2]);
+
+		for (lane = 0; lane < lane_count; lane++) {
+			exynos_dp_read_bytes_from_dpcd(dp,
+					DPCD_ADDR_ADJUST_REQUEST_LANE0_1,
+					2, adjust_request);
+			voltage_swing = exynos_dp_get_adjust_request_voltage(
+							adjust_request, lane);
+			pre_emphasis = exynos_dp_get_adjust_request_pre_emphasis(
+							adjust_request, lane);
+			training_lane = DPCD_VOLTAGE_SWING_SET(voltage_swing) |
+					DPCD_PRE_EMPHASIS_SET(pre_emphasis);
+
+			if (voltage_swing == VOLTAGE_LEVEL_3)
+				training_lane |= DPCD_MAX_SWING_REACHED;
+			if (pre_emphasis == PRE_EMPHASIS_LEVEL_3)
+				training_lane |= DPCD_MAX_PRE_EMPHASIS_REACHED;
+
+			dp->link_train.training_lane[lane] = training_lane;
+		}
+
+		if (exynos_dp_channel_eq_ok(link_align, lane_count) == 0) {
 			/* traing pattern Set to Normal */
 			exynos_dp_training_pattern_dis(dp);
 
@@ -596,39 +602,42 @@ static int exynos_dp_process_equalizer_training(struct exynos_dp_device *dp)
 			dp->link_train.lane_count = reg;
 			dev_dbg(dp->dev, "final lane count = %.2x\n",
 				dp->link_train.lane_count);
+
 			/* set enhanced mode if available */
 			exynos_dp_set_enhanced_mode(dp);
-
 			dp->link_train.lt_state = FINISHED;
 		} else {
 			/* not all locked */
 			dp->link_train.eq_loop++;
 
 			if (dp->link_train.eq_loop > MAX_EQ_LOOP) {
-				exynos_dp_reduce_link_rate(dp);
-			} else {
-				exynos_dp_get_adjust_train(dp, adjust_request);
-
-				for (lane = 0; lane < lane_count; lane++) {
-					exynos_dp_set_lane_link_training(dp,
-						dp->link_train.training_lane[lane],
-						lane);
-					buf[lane] = dp->link_train.training_lane[lane];
-					exynos_dp_write_byte_to_dpcd(dp,
-						DPCD_ADDR_TRAINING_LANE0_SET + lane,
-						buf[lane]);
-				}
+				dev_err(dp->dev, "EQ Max loop\n");
+				goto reduce_link_rate;
 			}
+
+			for (lane = 0; lane < lane_count; lane++)
+				exynos_dp_set_lane_link_training(dp,
+					dp->link_train.training_lane[lane],
+					lane);
+
+			exynos_dp_write_bytes_to_dpcd(dp,
+				DPCD_ADDR_TRAINING_LANE0_SET,
+				lane_count,
+				dp->link_train.training_lane);
 		}
 	} else {
-		exynos_dp_reduce_link_rate(dp);
+		goto reduce_link_rate;
 	}
 
 	return 0;
+
+reduce_link_rate:
+	exynos_dp_reduce_link_rate(dp);
+	return -EIO;
 }
 
 static void exynos_dp_get_max_rx_bandwidth(struct exynos_dp_device *dp,
-			u8 *bandwidth)
+					u8 *bandwidth)
 {
 	u8 data;
 
@@ -641,7 +650,7 @@ static void exynos_dp_get_max_rx_bandwidth(struct exynos_dp_device *dp,
 }
 
 static void exynos_dp_get_max_rx_lane_count(struct exynos_dp_device *dp,
-			u8 *lane_count)
+					u8 *lane_count)
 {
 	u8 data;
 
@@ -693,13 +702,7 @@ static void exynos_dp_init_training(struct exynos_dp_device *dp,
 static int exynos_dp_sw_link_training(struct exynos_dp_device *dp)
 {
 	int retval = 0;
-	int training_finished;
-
-	/* Turn off unnecessary lane */
-	if (dp->link_train.lane_count == 1)
-		exynos_dp_set_analog_power_down(dp, CH1_BLOCK, 1);
-
-	training_finished = 0;
+	int training_finished = 0;
 
 	dp->link_train.lt_state = START;
 
@@ -710,10 +713,14 @@ static int exynos_dp_sw_link_training(struct exynos_dp_device *dp)
 			exynos_dp_link_start(dp);
 			break;
 		case CLOCK_RECOVERY:
-			exynos_dp_process_clock_recovery(dp);
+			retval = exynos_dp_process_clock_recovery(dp);
+			if (retval)
+				dev_err(dp->dev, "LT CR failed!\n");
 			break;
 		case EQUALIZER_TRAINING:
-			exynos_dp_process_equalizer_training(dp);
+			retval = exynos_dp_process_equalizer_training(dp);
+			if (retval)
+				dev_err(dp->dev, "LT EQ failed!\n");
 			break;
 		case FINISHED:
 			training_finished = 1;
@@ -872,40 +879,33 @@ static int __devinit exynos_dp_probe(struct platform_device *pdev)
 
 	dp->dev = &pdev->dev;
 
-	dp->clock = clk_get(&pdev->dev, "dp");
+	dp->clock = devm_clk_get(&pdev->dev, "dp");
 	if (IS_ERR(dp->clock)) {
 		dev_err(&pdev->dev, "failed to get clock\n");
 		return PTR_ERR(dp->clock);
 	}
 
-	clk_enable(dp->clock);
+	clk_prepare_enable(dp->clock);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "failed to get registers\n");
-		ret = -EINVAL;
-		goto err_clock;
-	}
 
 	dp->reg_base = devm_request_and_ioremap(&pdev->dev, res);
 	if (!dp->reg_base) {
 		dev_err(&pdev->dev, "failed to ioremap\n");
-		ret = -ENOMEM;
-		goto err_clock;
+		return -ENOMEM;
 	}
 
 	dp->irq = platform_get_irq(pdev, 0);
 	if (!dp->irq) {
 		dev_err(&pdev->dev, "failed to get irq\n");
-		ret = -ENODEV;
-		goto err_clock;
+		return -ENODEV;
 	}
 
 	ret = devm_request_irq(&pdev->dev, dp->irq, exynos_dp_irq_handler, 0,
 				"exynos-dp", dp);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to request irq\n");
-		goto err_clock;
+		return ret;
 	}
 
 	dp->video_info = pdata->video_info;
@@ -917,7 +917,7 @@ static int __devinit exynos_dp_probe(struct platform_device *pdev)
 	ret = exynos_dp_detect_hpd(dp);
 	if (ret) {
 		dev_err(&pdev->dev, "unable to detect hpd\n");
-		goto err_clock;
+		return ret;
 	}
 
 	exynos_dp_handle_edid(dp);
@@ -926,7 +926,7 @@ static int __devinit exynos_dp_probe(struct platform_device *pdev)
 				dp->video_info->link_rate);
 	if (ret) {
 		dev_err(&pdev->dev, "unable to do link train\n");
-		goto err_clock;
+		return ret;
 	}
 
 	exynos_dp_enable_scramble(dp, 1);
@@ -940,17 +940,12 @@ static int __devinit exynos_dp_probe(struct platform_device *pdev)
 	ret = exynos_dp_config_video(dp, dp->video_info);
 	if (ret) {
 		dev_err(&pdev->dev, "unable to config video\n");
-		goto err_clock;
+		return ret;
 	}
 
 	platform_set_drvdata(pdev, dp);
 
 	return 0;
-
-err_clock:
-	clk_put(dp->clock);
-
-	return ret;
 }
 
 static int __devexit exynos_dp_remove(struct platform_device *pdev)
@@ -961,8 +956,7 @@ static int __devexit exynos_dp_remove(struct platform_device *pdev)
 	if (pdata && pdata->phy_exit)
 		pdata->phy_exit();
 
-	clk_disable(dp->clock);
-	clk_put(dp->clock);
+	clk_disable_unprepare(dp->clock);
 
 	return 0;
 }
@@ -977,7 +971,7 @@ static int exynos_dp_suspend(struct device *dev)
 	if (pdata && pdata->phy_exit)
 		pdata->phy_exit();
 
-	clk_disable(dp->clock);
+	clk_disable_unprepare(dp->clock);
 
 	return 0;
 }
@@ -991,7 +985,7 @@ static int exynos_dp_resume(struct device *dev)
 	if (pdata && pdata->phy_init)
 		pdata->phy_init();
 
-	clk_enable(dp->clock);
+	clk_prepare_enable(dp->clock);
 
 	exynos_dp_init_dp(dp);
 
diff --git a/drivers/video/exynos/exynos_dp_core.h b/drivers/video/exynos/exynos_dp_core.h
index 8526e548c38..57b8a6531c0 100644
--- a/drivers/video/exynos/exynos_dp_core.h
+++ b/drivers/video/exynos/exynos_dp_core.h
@@ -43,7 +43,7 @@ void exynos_dp_init_interrupt(struct exynos_dp_device *dp);
 void exynos_dp_reset(struct exynos_dp_device *dp);
 void exynos_dp_swreset(struct exynos_dp_device *dp);
 void exynos_dp_config_interrupt(struct exynos_dp_device *dp);
-u32 exynos_dp_get_pll_lock_status(struct exynos_dp_device *dp);
+enum pll_status exynos_dp_get_pll_lock_status(struct exynos_dp_device *dp);
 void exynos_dp_set_pll_power_down(struct exynos_dp_device *dp, bool enable);
 void exynos_dp_set_analog_power_down(struct exynos_dp_device *dp,
 				enum analog_power_block block,
@@ -105,7 +105,7 @@ u32 exynos_dp_get_lane1_link_training(struct exynos_dp_device *dp);
 u32 exynos_dp_get_lane2_link_training(struct exynos_dp_device *dp);
 u32 exynos_dp_get_lane3_link_training(struct exynos_dp_device *dp);
 void exynos_dp_reset_macro(struct exynos_dp_device *dp);
-int exynos_dp_init_video(struct exynos_dp_device *dp);
+void exynos_dp_init_video(struct exynos_dp_device *dp);
 
 void exynos_dp_set_video_color_format(struct exynos_dp_device *dp,
 				u32 color_depth,
@@ -144,7 +144,7 @@ void exynos_dp_disable_scrambling(struct exynos_dp_device *dp);
 #define DPCD_ADDR_TRAINING_PATTERN_SET		0x0102
 #define DPCD_ADDR_TRAINING_LANE0_SET		0x0103
 #define DPCD_ADDR_LANE0_1_STATUS		0x0202
-#define DPCD_ADDR_LANE_ALIGN__STATUS_UPDATED	0x0204
+#define DPCD_ADDR_LANE_ALIGN_STATUS_UPDATED	0x0204
 #define DPCD_ADDR_ADJUST_REQUEST_LANE0_1	0x0206
 #define DPCD_ADDR_ADJUST_REQUEST_LANE2_3	0x0207
 #define DPCD_ADDR_TEST_REQUEST			0x0218
diff --git a/drivers/video/exynos/exynos_dp_reg.c b/drivers/video/exynos/exynos_dp_reg.c
index 2db5b9aa250..3f5ca8a0d5e 100644
--- a/drivers/video/exynos/exynos_dp_reg.c
+++ b/drivers/video/exynos/exynos_dp_reg.c
@@ -77,7 +77,7 @@ void exynos_dp_init_analog_param(struct exynos_dp_device *dp)
 	writel(reg, dp->reg_base + EXYNOS_DP_ANALOG_CTL_3);
 
 	reg = PD_RING_OSC | AUX_TERMINAL_CTRL_50_OHM |
-		TX_CUR1_2X | TX_CUR_8_MA;
+		TX_CUR1_2X | TX_CUR_16_MA;
 	writel(reg, dp->reg_base + EXYNOS_DP_PLL_FILTER_CTL_1);
 
 	reg = CH3_AMP_400_MV | CH2_AMP_400_MV |
@@ -148,9 +148,6 @@ void exynos_dp_reset(struct exynos_dp_device *dp)
 	writel(0x2, dp->reg_base + EXYNOS_DP_M_AUD_GEN_FILTER_TH);
 
 	writel(0x00000101, dp->reg_base + EXYNOS_DP_SOC_GENERAL_CTL);
-
-	exynos_dp_init_analog_param(dp);
-	exynos_dp_init_interrupt(dp);
 }
 
 void exynos_dp_swreset(struct exynos_dp_device *dp)
@@ -179,7 +176,7 @@ void exynos_dp_config_interrupt(struct exynos_dp_device *dp)
 	writel(reg, dp->reg_base + EXYNOS_DP_INT_STA_MASK);
 }
 
-u32 exynos_dp_get_pll_lock_status(struct exynos_dp_device *dp)
+enum pll_status exynos_dp_get_pll_lock_status(struct exynos_dp_device *dp)
 {
 	u32 reg;
 
@@ -401,6 +398,7 @@ int exynos_dp_start_aux_transaction(struct exynos_dp_device *dp)
 {
 	int reg;
 	int retval = 0;
+	int timeout_loop = 0;
 
 	/* Enable AUX CH operation */
 	reg = readl(dp->reg_base + EXYNOS_DP_AUX_CH_CTL_2);
@@ -409,8 +407,15 @@ int exynos_dp_start_aux_transaction(struct exynos_dp_device *dp)
 
 	/* Is AUX CH command reply received? */
 	reg = readl(dp->reg_base + EXYNOS_DP_INT_STA);
-	while (!(reg & RPLY_RECEIV))
+	while (!(reg & RPLY_RECEIV)) {
+		timeout_loop++;
+		if (DP_TIMEOUT_LOOP_COUNT < timeout_loop) {
+			dev_err(dp->dev, "AUX CH command reply failed!\n");
+			return -ETIMEDOUT;
+		}
 		reg = readl(dp->reg_base + EXYNOS_DP_INT_STA);
+		usleep_range(10, 11);
+	}
 
 	/* Clear interrupt source for AUX CH command reply */
 	writel(RPLY_RECEIV, dp->reg_base + EXYNOS_DP_INT_STA);
@@ -471,7 +476,8 @@ int exynos_dp_write_byte_to_dpcd(struct exynos_dp_device *dp,
 		if (retval == 0)
 			break;
 		else
-			dev_err(dp->dev, "Aux Transaction fail!\n");
+			dev_dbg(dp->dev, "%s: Aux Transaction fail!\n",
+				__func__);
 	}
 
 	return retval;
@@ -511,7 +517,8 @@ int exynos_dp_read_byte_from_dpcd(struct exynos_dp_device *dp,
 		if (retval == 0)
 			break;
 		else
-			dev_err(dp->dev, "Aux Transaction fail!\n");
+			dev_dbg(dp->dev, "%s: Aux Transaction fail!\n",
+				__func__);
 	}
 
 	/* Read data buffer */
@@ -575,7 +582,8 @@ int exynos_dp_write_bytes_to_dpcd(struct exynos_dp_device *dp,
 			if (retval == 0)
 				break;
 			else
-				dev_err(dp->dev, "Aux Transaction fail!\n");
+				dev_dbg(dp->dev, "%s: Aux Transaction fail!\n",
+					__func__);
 		}
 
 		start_offset += cur_data_count;
@@ -632,7 +640,8 @@ int exynos_dp_read_bytes_from_dpcd(struct exynos_dp_device *dp,
 			if (retval == 0)
 				break;
 			else
-				dev_err(dp->dev, "Aux Transaction fail!\n");
+				dev_dbg(dp->dev, "%s: Aux Transaction fail!\n",
+					__func__);
 		}
 
 		for (cur_data_idx = 0; cur_data_idx < cur_data_count;
@@ -677,7 +686,7 @@ int exynos_dp_select_i2c_device(struct exynos_dp_device *dp,
 	/* Start AUX transaction */
 	retval = exynos_dp_start_aux_transaction(dp);
 	if (retval != 0)
-		dev_err(dp->dev, "Aux Transaction fail!\n");
+		dev_dbg(dp->dev, "%s: Aux Transaction fail!\n", __func__);
 
 	return retval;
 }
@@ -717,7 +726,8 @@ int exynos_dp_read_byte_from_i2c(struct exynos_dp_device *dp,
 		if (retval == 0)
 			break;
 		else
-			dev_err(dp->dev, "Aux Transaction fail!\n");
+			dev_dbg(dp->dev, "%s: Aux Transaction fail!\n",
+				__func__);
 	}
 
 	/* Read data */
@@ -777,7 +787,9 @@ int exynos_dp_read_bytes_from_i2c(struct exynos_dp_device *dp,
 				if (retval == 0)
 					break;
 				else
-					dev_err(dp->dev, "Aux Transaction fail!\n");
+					dev_dbg(dp->dev,
+						"%s: Aux Transaction fail!\n",
+						__func__);
 			}
 			/* Check if Rx sends defer */
 			reg = readl(dp->reg_base + EXYNOS_DP_AUX_RX_COMM);
@@ -883,7 +895,9 @@ void exynos_dp_set_lane0_pre_emphasis(struct exynos_dp_device *dp, u32 level)
 {
 	u32 reg;
 
-	reg = level << PRE_EMPHASIS_SET_SHIFT;
+	reg = readl(dp->reg_base + EXYNOS_DP_LN0_LINK_TRAINING_CTL);
+	reg &= ~PRE_EMPHASIS_SET_MASK;
+	reg |= level << PRE_EMPHASIS_SET_SHIFT;
 	writel(reg, dp->reg_base + EXYNOS_DP_LN0_LINK_TRAINING_CTL);
 }
 
@@ -891,7 +905,9 @@ void exynos_dp_set_lane1_pre_emphasis(struct exynos_dp_device *dp, u32 level)
 {
 	u32 reg;
 
-	reg = level << PRE_EMPHASIS_SET_SHIFT;
+	reg = readl(dp->reg_base + EXYNOS_DP_LN1_LINK_TRAINING_CTL);
+	reg &= ~PRE_EMPHASIS_SET_MASK;
+	reg |= level << PRE_EMPHASIS_SET_SHIFT;
 	writel(reg, dp->reg_base + EXYNOS_DP_LN1_LINK_TRAINING_CTL);
 }
 
@@ -899,7 +915,9 @@ void exynos_dp_set_lane2_pre_emphasis(struct exynos_dp_device *dp, u32 level)
 {
 	u32 reg;
 
-	reg = level << PRE_EMPHASIS_SET_SHIFT;
+	reg = readl(dp->reg_base + EXYNOS_DP_LN2_LINK_TRAINING_CTL);
+	reg &= ~PRE_EMPHASIS_SET_MASK;
+	reg |= level << PRE_EMPHASIS_SET_SHIFT;
 	writel(reg, dp->reg_base + EXYNOS_DP_LN2_LINK_TRAINING_CTL);
 }
 
@@ -907,7 +925,9 @@ void exynos_dp_set_lane3_pre_emphasis(struct exynos_dp_device *dp, u32 level)
 {
 	u32 reg;
 
-	reg = level << PRE_EMPHASIS_SET_SHIFT;
+	reg = readl(dp->reg_base + EXYNOS_DP_LN3_LINK_TRAINING_CTL);
+	reg &= ~PRE_EMPHASIS_SET_MASK;
+	reg |= level << PRE_EMPHASIS_SET_SHIFT;
 	writel(reg, dp->reg_base + EXYNOS_DP_LN3_LINK_TRAINING_CTL);
 }
 
@@ -994,7 +1014,7 @@ void exynos_dp_reset_macro(struct exynos_dp_device *dp)
 	writel(reg, dp->reg_base + EXYNOS_DP_PHY_TEST);
 }
 
-int exynos_dp_init_video(struct exynos_dp_device *dp)
+void exynos_dp_init_video(struct exynos_dp_device *dp)
 {
 	u32 reg;
 
@@ -1012,8 +1032,6 @@ int exynos_dp_init_video(struct exynos_dp_device *dp)
 
 	reg = VID_HRES_TH(2) | VID_VRES_TH(0);
 	writel(reg, dp->reg_base + EXYNOS_DP_VIDEO_CTL_8);
-
-	return 0;
 }
 
 void exynos_dp_set_video_color_format(struct exynos_dp_device *dp,
diff --git a/drivers/video/exynos/exynos_dp_reg.h b/drivers/video/exynos/exynos_dp_reg.h
index 125b27cd57a..1f2f014cfe8 100644
--- a/drivers/video/exynos/exynos_dp_reg.h
+++ b/drivers/video/exynos/exynos_dp_reg.h
@@ -187,7 +187,7 @@
 #define PD_RING_OSC				(0x1 << 6)
 #define AUX_TERMINAL_CTRL_50_OHM		(0x2 << 4)
 #define TX_CUR1_2X				(0x1 << 2)
-#define TX_CUR_8_MA				(0x2 << 0)
+#define TX_CUR_16_MA				(0x3 << 0)
 
 /* EXYNOS_DP_TX_AMP_TUNING_CTL */
 #define CH3_AMP_400_MV				(0x0 << 24)
@@ -285,6 +285,7 @@
 #define SW_TRAINING_PATTERN_SET_NORMAL		(0x0 << 0)
 
 /* EXYNOS_DP_LN0_LINK_TRAINING_CTL */
+#define PRE_EMPHASIS_SET_MASK			(0x3 << 3)
 #define PRE_EMPHASIS_SET_SHIFT			(3)
 
 /* EXYNOS_DP_DEBUG_CTL */
diff --git a/drivers/video/exynos/exynos_mipi_dsi.c b/drivers/video/exynos/exynos_mipi_dsi.c
index 663c308d0e7..07d70a3a628 100644
--- a/drivers/video/exynos/exynos_mipi_dsi.c
+++ b/drivers/video/exynos/exynos_mipi_dsi.c
@@ -205,7 +205,8 @@ int exynos_mipi_dsi_register_lcd_device(struct mipi_dsim_lcd_device *lcd_dev)
 	return 0;
 }
 
-struct mipi_dsim_ddi *exynos_mipi_dsi_find_lcd_device(struct mipi_dsim_lcd_driver *lcd_drv)
+static struct mipi_dsim_ddi *exynos_mipi_dsi_find_lcd_device(
+					struct mipi_dsim_lcd_driver *lcd_drv)
 {
 	struct mipi_dsim_ddi *dsim_ddi, *next;
 	struct mipi_dsim_lcd_device *lcd_dev;
@@ -265,7 +266,8 @@ int exynos_mipi_dsi_register_lcd_driver(struct mipi_dsim_lcd_driver *lcd_drv)
 
 }
 
-struct mipi_dsim_ddi *exynos_mipi_dsi_bind_lcd_ddi(struct mipi_dsim_device *dsim,
+static struct mipi_dsim_ddi *exynos_mipi_dsi_bind_lcd_ddi(
+						struct mipi_dsim_device *dsim,
 						const char *name)
 {
 	struct mipi_dsim_ddi *dsim_ddi, *next;
@@ -373,6 +375,7 @@ static int exynos_mipi_dsi_probe(struct platform_device *pdev)
 	dsim->clock = clk_get(&pdev->dev, "dsim0");
 	if (IS_ERR(dsim->clock)) {
 		dev_err(&pdev->dev, "failed to get dsim clock source\n");
+		ret = -ENODEV;
 		goto err_clock_get;
 	}
 
@@ -381,6 +384,7 @@ static int exynos_mipi_dsi_probe(struct platform_device *pdev)
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
 		dev_err(&pdev->dev, "failed to get io memory region\n");
+		ret = -ENODEV;
 		goto err_platform_get;
 	}
 
@@ -405,6 +409,7 @@ static int exynos_mipi_dsi_probe(struct platform_device *pdev)
 	dsim_ddi = exynos_mipi_dsi_bind_lcd_ddi(dsim, dsim_pd->lcd_panel_name);
 	if (!dsim_ddi) {
 		dev_err(&pdev->dev, "mipi_dsim_ddi object not found.\n");
+		ret = -EINVAL;
 		goto err_bind;
 	}
 
diff --git a/drivers/video/exynos/exynos_mipi_dsi_common.c b/drivers/video/exynos/exynos_mipi_dsi_common.c
index 47b533a183b..3cd29a4fc10 100644
--- a/drivers/video/exynos/exynos_mipi_dsi_common.c
+++ b/drivers/video/exynos/exynos_mipi_dsi_common.c
@@ -79,11 +79,6 @@ irqreturn_t exynos_mipi_dsi_interrupt_handler(int irq, void *dev_id)
 	struct mipi_dsim_device *dsim = dev_id;
 	unsigned int intsrc, intmsk;
 
-	if (dsim == NULL) {
-		dev_err(dsim->dev, "%s: wrong parameter\n", __func__);
-		return IRQ_NONE;
-	}
-
 	intsrc = exynos_mipi_dsi_read_interrupt(dsim);
 	intmsk = exynos_mipi_dsi_read_interrupt_mask(dsim);
 	intmsk = ~intmsk & intsrc;
@@ -288,9 +283,6 @@ int exynos_mipi_dsi_wr_data(struct mipi_dsim_device *dsim, unsigned int data_id,
 		mutex_unlock(&dsim->lock);
 		return -EINVAL;
 	}
-
-	mutex_unlock(&dsim->lock);
-	return 0;
 }
 
 static unsigned int exynos_mipi_dsi_long_data_rd(struct mipi_dsim_device *dsim,
diff --git a/drivers/video/fb-puv3.c b/drivers/video/fb-puv3.c
index 60a787fa32c..7d106f1f490 100644
--- a/drivers/video/fb-puv3.c
+++ b/drivers/video/fb-puv3.c
@@ -653,9 +653,8 @@ int unifb_mmap(struct fb_info *info,
 				vma->vm_page_prot))
 		return -EAGAIN;
 
-	vma->vm_flags |= VM_RESERVED;	/* avoid to swap out this VMA */
+	/* VM_IO | VM_DONTEXPAND | VM_DONTDUMP are set by remap_pfn_range() */
 	return 0;
-
 }
 
 static struct fb_ops unifb_ops = {
diff --git a/drivers/video/fb_defio.c b/drivers/video/fb_defio.c
index 64cda560c48..88cad6b8b47 100644
--- a/drivers/video/fb_defio.c
+++ b/drivers/video/fb_defio.c
@@ -166,7 +166,7 @@ static const struct address_space_operations fb_deferred_io_aops = {
 static int fb_deferred_io_mmap(struct fb_info *info, struct vm_area_struct *vma)
 {
 	vma->vm_ops = &fb_deferred_io_vm_ops;
-	vma->vm_flags |= ( VM_RESERVED | VM_DONTEXPAND );
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
 	if (!(info->flags & FBINFO_VIRTFB))
 		vma->vm_flags |= VM_IO;
 	vma->vm_private_data = info;
diff --git a/drivers/video/fbmem.c b/drivers/video/fbmem.c
index 0dff12a1dae..3ff0105a496 100644
--- a/drivers/video/fbmem.c
+++ b/drivers/video/fbmem.c
@@ -1410,8 +1410,7 @@ fb_mmap(struct file *file, struct vm_area_struct * vma)
 		return -EINVAL;
 	off += start;
 	vma->vm_pgoff = off >> PAGE_SHIFT;
-	/* This is an IO map - tell maydump to skip this VMA */
-	vma->vm_flags |= VM_IO | VM_RESERVED;
+	/* VM_IO | VM_DONTEXPAND | VM_DONTDUMP are set by io_remap_pfn_range()*/
 	vma->vm_page_prot = vm_get_page_prot(vma->vm_flags);
 	fb_pgprotect(file, vma, off);
 	if (io_remap_pfn_range(vma, vma->vm_start, off >> PAGE_SHIFT,
diff --git a/drivers/video/fsl-diu-fb.c b/drivers/video/fsl-diu-fb.c
index 458c00664ad..ede9e55413f 100644
--- a/drivers/video/fsl-diu-fb.c
+++ b/drivers/video/fsl-diu-fb.c
@@ -1501,8 +1501,8 @@ static int __devinit fsl_diu_probe(struct platform_device *pdev)
 	unsigned int i;
 	int ret;
 
-	data = dma_alloc_coherent(&pdev->dev, sizeof(struct fsl_diu_data),
-				  &dma_addr, GFP_DMA | __GFP_ZERO);
+	data = dmam_alloc_coherent(&pdev->dev, sizeof(struct fsl_diu_data),
+				   &dma_addr, GFP_DMA | __GFP_ZERO);
 	if (!data)
 		return -ENOMEM;
 	data->dma_addr = dma_addr;
@@ -1628,9 +1628,6 @@ error:
 
 	iounmap(data->diu_reg);
 
-	dma_free_coherent(&pdev->dev, sizeof(struct fsl_diu_data), data,
-			  data->dma_addr);
-
 	return ret;
 }
 
@@ -1648,9 +1645,6 @@ static int fsl_diu_remove(struct platform_device *pdev)
 
 	iounmap(data->diu_reg);
 
-	dma_free_coherent(&pdev->dev, sizeof(struct fsl_diu_data), data,
-			  data->dma_addr);
-
 	return 0;
 }
 
diff --git a/drivers/video/gbefb.c b/drivers/video/gbefb.c
index 7e7b7a9ba27..3dad31975db 100644
--- a/drivers/video/gbefb.c
+++ b/drivers/video/gbefb.c
@@ -20,6 +20,7 @@
 #include <linux/kernel.h>
 #include <linux/mm.h>
 #include <linux/module.h>
+#include <linux/io.h>
 
 #ifdef CONFIG_X86
 #include <asm/mtrr.h>
@@ -28,7 +29,6 @@
 #include <asm/addrspace.h>
 #endif
 #include <asm/byteorder.h>
-#include <asm/io.h>
 #include <asm/tlbflush.h>
 
 #include <video/gbe.h>
@@ -1024,7 +1024,7 @@ static int gbefb_mmap(struct fb_info *info,
 	pgprot_val(vma->vm_page_prot) =
 		pgprot_fb(pgprot_val(vma->vm_page_prot));
 
-	vma->vm_flags |= VM_IO | VM_RESERVED;
+	/* VM_IO | VM_DONTEXPAND | VM_DONTDUMP are set by remap_pfn_range() */
 
 	/* look for the starting tile */
 	tile = &gbe_tiles.cpu[offset >> TILE_SHIFT];
@@ -1156,7 +1156,8 @@ static int __devinit gbefb_probe(struct platform_device *p_dev)
 		goto out_release_framebuffer;
 	}
 
-	gbe = (struct sgi_gbe *) ioremap(GBE_BASE, sizeof(struct sgi_gbe));
+	gbe = (struct sgi_gbe *) devm_ioremap(&p_dev->dev, GBE_BASE,
+					      sizeof(struct sgi_gbe));
 	if (!gbe) {
 		printk(KERN_ERR "gbefb: couldn't map mmio region\n");
 		ret = -ENXIO;
@@ -1170,12 +1171,13 @@ static int __devinit gbefb_probe(struct platform_device *p_dev)
 	if (!gbe_tiles.cpu) {
 		printk(KERN_ERR "gbefb: couldn't allocate tiles table\n");
 		ret = -ENOMEM;
-		goto out_unmap;
+		goto out_release_mem_region;
 	}
 
 	if (gbe_mem_phys) {
 		/* memory was allocated at boot time */
-		gbe_mem = ioremap_nocache(gbe_mem_phys, gbe_mem_size);
+		gbe_mem = devm_ioremap_nocache(&p_dev->dev, gbe_mem_phys,
+					       gbe_mem_size);
 		if (!gbe_mem) {
 			printk(KERN_ERR "gbefb: couldn't map framebuffer\n");
 			ret = -ENOMEM;
@@ -1241,13 +1243,9 @@ static int __devinit gbefb_probe(struct platform_device *p_dev)
 out_gbe_unmap:
 	if (gbe_dma_addr)
 		dma_free_coherent(NULL, gbe_mem_size, gbe_mem, gbe_mem_phys);
-	else
-		iounmap(gbe_mem);
 out_tiles_free:
 	dma_free_coherent(NULL, GBE_TLB_SIZE * sizeof(uint16_t),
 			  (void *)gbe_tiles.cpu, gbe_tiles.dma);
-out_unmap:
-	iounmap(gbe);
 out_release_mem_region:
 	release_mem_region(GBE_BASE, sizeof(struct sgi_gbe));
 out_release_framebuffer:
@@ -1264,12 +1262,9 @@ static int __devexit gbefb_remove(struct platform_device* p_dev)
 	gbe_turn_off();
 	if (gbe_dma_addr)
 		dma_free_coherent(NULL, gbe_mem_size, gbe_mem, gbe_mem_phys);
-	else
-		iounmap(gbe_mem);
 	dma_free_coherent(NULL, GBE_TLB_SIZE * sizeof(uint16_t),
 			  (void *)gbe_tiles.cpu, gbe_tiles.dma);
 	release_mem_region(GBE_BASE, sizeof(struct sgi_gbe));
-	iounmap(gbe);
 	gbefb_remove_sysfs(&p_dev->dev);
 	framebuffer_release(info);
 
diff --git a/drivers/video/hpfb.c b/drivers/video/hpfb.c
index ebf8495ff19..7324865f965 100644
--- a/drivers/video/hpfb.c
+++ b/drivers/video/hpfb.c
@@ -210,6 +210,7 @@ static int __devinit hpfb_init_one(unsigned long phys_base,
 				   unsigned long virt_base)
 {
 	unsigned long fboff, fb_width, fb_height, fb_start;
+	int ret;
 
 	fb_regs = virt_base;
 	fboff = (in_8(fb_regs + HPFB_FBOMSB) << 8) | in_8(fb_regs + HPFB_FBOLSB);
@@ -290,19 +291,29 @@ static int __devinit hpfb_init_one(unsigned long phys_base,
 	fb_info.var   = hpfb_defined;
 	fb_info.screen_base = (char *)fb_start;
 
-	fb_alloc_cmap(&fb_info.cmap, 1 << hpfb_defined.bits_per_pixel, 0);
+	ret = fb_alloc_cmap(&fb_info.cmap, 1 << hpfb_defined.bits_per_pixel, 0);
+	if (ret < 0)
+		goto unmap_screen_base;
 
-	if (register_framebuffer(&fb_info) < 0) {
-		fb_dealloc_cmap(&fb_info.cmap);
-		iounmap(fb_info.screen_base);
-		fb_info.screen_base = NULL;
-		return 1;
-	}
+	ret = register_framebuffer(&fb_info);
+	if (ret < 0)
+		goto dealloc_cmap;
 
 	printk(KERN_INFO "fb%d: %s frame buffer device\n",
 	       fb_info.node, fb_info.fix.id);
 
 	return 0;
+
+dealloc_cmap:
+	fb_dealloc_cmap(&fb_info.cmap);
+
+unmap_screen_base:
+	if (fb_info.screen_base) {
+		iounmap(fb_info.screen_base);
+		fb_info.screen_base = NULL;
+	}
+
+	return ret;
 }
 
 /* 
@@ -345,6 +356,9 @@ static void __devexit hpfb_remove_one(struct dio_dev *d)
 	if (d->scode >= DIOII_SCBASE)
 		iounmap((void *)fb_regs);
 	release_mem_region(d->resource.start, resource_size(&d->resource));
+	fb_dealloc_cmap(&fb_info.cmap);
+	if (fb_info.screen_base)
+		iounmap(fb_info.screen_base);
 }
 
 static struct dio_device_id hpfb_dio_tbl[] = {
diff --git a/drivers/video/imxfb.c b/drivers/video/imxfb.c
index 53ffdfc82a7..cf2688de083 100644
--- a/drivers/video/imxfb.c
+++ b/drivers/video/imxfb.c
@@ -803,6 +803,7 @@ static int __init imxfb_probe(struct platform_device *pdev)
 	fbi->regs = ioremap(res->start, resource_size(res));
 	if (fbi->regs == NULL) {
 		dev_err(&pdev->dev, "Cannot map frame buffer registers\n");
+		ret = -ENOMEM;
 		goto failed_ioremap;
 	}
 
diff --git a/drivers/video/jz4740_fb.c b/drivers/video/jz4740_fb.c
index 3c63fc24bb1..4d25711b998 100644
--- a/drivers/video/jz4740_fb.c
+++ b/drivers/video/jz4740_fb.c
@@ -632,23 +632,10 @@ static int __devinit jzfb_probe(struct platform_device *pdev)
 		return -ENXIO;
 	}
 
-	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!mem) {
-		dev_err(&pdev->dev, "Failed to get register memory resource\n");
-		return -ENXIO;
-	}
-
-	mem = request_mem_region(mem->start, resource_size(mem), pdev->name);
-	if (!mem) {
-		dev_err(&pdev->dev, "Failed to request register memory region\n");
-		return -EBUSY;
-	}
-
 	fb = framebuffer_alloc(sizeof(struct jzfb), &pdev->dev);
 	if (!fb) {
 		dev_err(&pdev->dev, "Failed to allocate framebuffer device\n");
-		ret = -ENOMEM;
-		goto err_release_mem_region;
+		return -ENOMEM;
 	}
 
 	fb->fbops = &jzfb_ops;
@@ -657,27 +644,26 @@ static int __devinit jzfb_probe(struct platform_device *pdev)
 	jzfb = fb->par;
 	jzfb->pdev = pdev;
 	jzfb->pdata = pdata;
-	jzfb->mem = mem;
 
-	jzfb->ldclk = clk_get(&pdev->dev, "lcd");
+	jzfb->ldclk = devm_clk_get(&pdev->dev, "lcd");
 	if (IS_ERR(jzfb->ldclk)) {
 		ret = PTR_ERR(jzfb->ldclk);
 		dev_err(&pdev->dev, "Failed to get lcd clock: %d\n", ret);
 		goto err_framebuffer_release;
 	}
 
-	jzfb->lpclk = clk_get(&pdev->dev, "lcd_pclk");
+	jzfb->lpclk = devm_clk_get(&pdev->dev, "lcd_pclk");
 	if (IS_ERR(jzfb->lpclk)) {
 		ret = PTR_ERR(jzfb->lpclk);
 		dev_err(&pdev->dev, "Failed to get lcd pixel clock: %d\n", ret);
-		goto err_put_ldclk;
+		goto err_framebuffer_release;
 	}
 
-	jzfb->base = ioremap(mem->start, resource_size(mem));
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	jzfb->base = devm_request_and_ioremap(&pdev->dev, mem);
 	if (!jzfb->base) {
-		dev_err(&pdev->dev, "Failed to ioremap register memory region\n");
 		ret = -EBUSY;
-		goto err_put_lpclk;
+		goto err_framebuffer_release;
 	}
 
 	platform_set_drvdata(pdev, jzfb);
@@ -693,7 +679,7 @@ static int __devinit jzfb_probe(struct platform_device *pdev)
 	ret = jzfb_alloc_devmem(jzfb);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to allocate video memory\n");
-		goto err_iounmap;
+		goto err_framebuffer_release;
 	}
 
 	fb->fix = jzfb_fix;
@@ -734,16 +720,8 @@ err_free_devmem:
 
 	fb_dealloc_cmap(&fb->cmap);
 	jzfb_free_devmem(jzfb);
-err_iounmap:
-	iounmap(jzfb->base);
-err_put_lpclk:
-	clk_put(jzfb->lpclk);
-err_put_ldclk:
-	clk_put(jzfb->ldclk);
 err_framebuffer_release:
 	framebuffer_release(fb);
-err_release_mem_region:
-	release_mem_region(mem->start, resource_size(mem));
 	return ret;
 }
 
@@ -756,17 +734,11 @@ static int __devexit jzfb_remove(struct platform_device *pdev)
 	jz_gpio_bulk_free(jz_lcd_ctrl_pins, jzfb_num_ctrl_pins(jzfb));
 	jz_gpio_bulk_free(jz_lcd_data_pins, jzfb_num_data_pins(jzfb));
 
-	iounmap(jzfb->base);
-	release_mem_region(jzfb->mem->start, resource_size(jzfb->mem));
-
 	fb_dealloc_cmap(&jzfb->fb->cmap);
 	jzfb_free_devmem(jzfb);
 
 	platform_set_drvdata(pdev, NULL);
 
-	clk_put(jzfb->lpclk);
-	clk_put(jzfb->ldclk);
-
 	framebuffer_release(jzfb->fb);
 
 	return 0;
diff --git a/drivers/video/mb862xx/mb862xxfbdrv.c b/drivers/video/mb862xx/mb862xxfbdrv.c
index 57d940be5f3..d68e332aa21 100644
--- a/drivers/video/mb862xx/mb862xxfbdrv.c
+++ b/drivers/video/mb862xx/mb862xxfbdrv.c
@@ -1052,12 +1052,14 @@ static int __devinit mb862xx_pci_probe(struct pci_dev *pdev,
 		break;
 	default:
 		/* should never occur */
+		ret = -EIO;
 		goto rel_reg;
 	}
 
 	par->fb_base = ioremap(par->fb_base_phys, par->mapped_vram);
 	if (par->fb_base == NULL) {
 		dev_err(dev, "Cannot map framebuffer\n");
+		ret = -EIO;
 		goto rel_reg;
 	}
 
@@ -1073,11 +1075,13 @@ static int __devinit mb862xx_pci_probe(struct pci_dev *pdev,
 	dev_dbg(dev, "mmio phys 0x%llx 0x%lx\n",
 		(unsigned long long)par->mmio_base_phys, (ulong)par->mmio_len);
 
-	if (mb862xx_pci_gdc_init(par))
+	ret = mb862xx_pci_gdc_init(par);
+	if (ret)
 		goto io_unmap;
 
-	if (request_irq(par->irq, mb862xx_intr, IRQF_SHARED,
-			DRV_NAME, (void *)par)) {
+	ret = request_irq(par->irq, mb862xx_intr, IRQF_SHARED,
+			  DRV_NAME, (void *)par);
+	if (ret) {
 		dev_err(dev, "Cannot request irq\n");
 		goto io_unmap;
 	}
diff --git a/drivers/video/mbx/mbxfb.c b/drivers/video/mbx/mbxfb.c
index 85e4f44bfa6..6563e50413c 100644
--- a/drivers/video/mbx/mbxfb.c
+++ b/drivers/video/mbx/mbxfb.c
@@ -26,8 +26,7 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/uaccess.h>
-
-#include <asm/io.h>
+#include <linux/io.h>
 
 #include <video/mbxfb.h>
 
@@ -939,8 +938,9 @@ static int __devinit mbxfb_probe(struct platform_device *dev)
 	}
 	mfbi->reg_phys_addr = mfbi->reg_res->start;
 
-	mfbi->reg_virt_addr = ioremap_nocache(mfbi->reg_phys_addr,
-					      res_size(mfbi->reg_req));
+	mfbi->reg_virt_addr = devm_ioremap_nocache(&dev->dev,
+						   mfbi->reg_phys_addr,
+						   res_size(mfbi->reg_req));
 	if (!mfbi->reg_virt_addr) {
 		dev_err(&dev->dev, "failed to ioremap Marathon registers\n");
 		ret = -EINVAL;
@@ -948,12 +948,12 @@ static int __devinit mbxfb_probe(struct platform_device *dev)
 	}
 	virt_base_2700 = mfbi->reg_virt_addr;
 
-	mfbi->fb_virt_addr = ioremap_nocache(mfbi->fb_phys_addr,
-					     res_size(mfbi->fb_req));
+	mfbi->fb_virt_addr = devm_ioremap_nocache(&dev->dev, mfbi->fb_phys_addr,
+						  res_size(mfbi->fb_req));
 	if (!mfbi->fb_virt_addr) {
 		dev_err(&dev->dev, "failed to ioremap frame buffer\n");
 		ret = -EINVAL;
-		goto err4;
+		goto err3;
 	}
 
 	fbi->screen_base = (char __iomem *)(mfbi->fb_virt_addr + 0x60000);
@@ -971,7 +971,7 @@ static int __devinit mbxfb_probe(struct platform_device *dev)
 	if (ret < 0) {
 		dev_err(&dev->dev, "fb_alloc_cmap failed\n");
 		ret = -EINVAL;
-		goto err5;
+		goto err3;
 	}
 
 	platform_set_drvdata(dev, fbi);
@@ -996,10 +996,6 @@ static int __devinit mbxfb_probe(struct platform_device *dev)
 
 err6:
 	fb_dealloc_cmap(&fbi->cmap);
-err5:
-	iounmap(mfbi->fb_virt_addr);
-err4:
-	iounmap(mfbi->reg_virt_addr);
 err3:
 	release_mem_region(mfbi->reg_res->start, res_size(mfbi->reg_res));
 err2:
@@ -1026,10 +1022,7 @@ static int __devexit mbxfb_remove(struct platform_device *dev)
 			if (mfbi->platform_remove)
 				mfbi->platform_remove(fbi);
 
-			if (mfbi->fb_virt_addr)
-				iounmap(mfbi->fb_virt_addr);
-			if (mfbi->reg_virt_addr)
-				iounmap(mfbi->reg_virt_addr);
+
 			if (mfbi->reg_req)
 				release_mem_region(mfbi->reg_req->start,
 						   res_size(mfbi->reg_req));
diff --git a/drivers/video/msm/mddi.c b/drivers/video/msm/mddi.c
index bf73f048006..35ac9e8bee6 100644
--- a/drivers/video/msm/mddi.c
+++ b/drivers/video/msm/mddi.c
@@ -26,9 +26,6 @@
 #include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/sched.h>
-#include <mach/msm_iomap.h>
-#include <mach/irqs.h>
-#include <mach/board.h>
 #include <linux/platform_data/video-msm_fb.h>
 #include "mddi_hw.h"
 
diff --git a/drivers/video/msm/mddi_client_nt35399.c b/drivers/video/msm/mddi_client_nt35399.c
index d7a5bf84fb2..f96df32e550 100644
--- a/drivers/video/msm/mddi_client_nt35399.c
+++ b/drivers/video/msm/mddi_client_nt35399.c
@@ -189,8 +189,9 @@ static int mddi_nt35399_probe(struct platform_device *pdev)
 
 	int ret;
 
-	struct panel_info *panel = kzalloc(sizeof(struct panel_info),
-					   GFP_KERNEL);
+	struct panel_info *panel = devm_kzalloc(&pdev->dev,
+						sizeof(struct panel_info),
+						GFP_KERNEL);
 
 	printk(KERN_DEBUG "%s: enter.\n", __func__);
 
@@ -233,7 +234,6 @@ static int mddi_nt35399_remove(struct platform_device *pdev)
 	struct panel_info *panel = platform_get_drvdata(pdev);
 
 	setup_vsync(panel, 0);
-	kfree(panel);
 	return 0;
 }
 
diff --git a/drivers/video/msm/mdp.c b/drivers/video/msm/mdp.c
index 2e0f3bab611..f2566c19e71 100644
--- a/drivers/video/msm/mdp.c
+++ b/drivers/video/msm/mdp.c
@@ -25,7 +25,6 @@
 #include <linux/major.h>
 #include <linux/slab.h>
 
-#include <mach/msm_iomap.h>
 #include <linux/platform_data/video-msm_fb.h>
 #include <linux/platform_device.h>
 #include <linux/export.h>
diff --git a/drivers/video/msm/mdp_hw.h b/drivers/video/msm/mdp_hw.h
index a0bacf581b3..35848d74100 100644
--- a/drivers/video/msm/mdp_hw.h
+++ b/drivers/video/msm/mdp_hw.h
@@ -15,7 +15,6 @@
 #ifndef _MDP_HW_H_
 #define _MDP_HW_H_
 
-#include <mach/msm_iomap.h>
 #include <linux/platform_data/video-msm_fb.h>
 
 struct mdp_info {
diff --git a/drivers/video/mx3fb.c b/drivers/video/mx3fb.c
index d7381088a18..ce1d452464e 100644
--- a/drivers/video/mx3fb.c
+++ b/drivers/video/mx3fb.c
@@ -1568,7 +1568,8 @@ static int mx3fb_remove(struct platform_device *dev)
 
 static struct platform_driver mx3fb_driver = {
 	.driver = {
-		   .name = MX3FB_NAME,
+		.name = MX3FB_NAME,
+		.owner = THIS_MODULE,
 	},
 	.probe = mx3fb_probe,
 	.remove = mx3fb_remove,
diff --git a/drivers/video/nuc900fb.c b/drivers/video/nuc900fb.c
index 93387555337..475dfee82c4 100644
--- a/drivers/video/nuc900fb.c
+++ b/drivers/video/nuc900fb.c
@@ -387,7 +387,7 @@ static int nuc900fb_init_registers(struct fb_info *info)
  *    The buffer should be a non-cached, non-buffered, memory region
  *    to allow palette and pixel writes without flushing the cache.
  */
-static int __init nuc900fb_map_video_memory(struct fb_info *info)
+static int __devinit nuc900fb_map_video_memory(struct fb_info *info)
 {
 	struct nuc900fb_info *fbi = info->par;
 	dma_addr_t map_dma;
diff --git a/drivers/video/omap/hwa742.c b/drivers/video/omap/hwa742.c
index 9f1d23c319c..f349ee6f0ce 100644
--- a/drivers/video/omap/hwa742.c
+++ b/drivers/video/omap/hwa742.c
@@ -27,7 +27,6 @@
 #include <linux/clk.h>
 #include <linux/interrupt.h>
 
-#include <plat/dma.h>
 #include "omapfb.h"
 
 #define HWA742_REV_CODE_REG       0x0
diff --git a/drivers/video/omap/lcd_palmte.c b/drivers/video/omap/lcd_palmte.c
index 88c31eb0cd6..ff4fb624b90 100644
--- a/drivers/video/omap/lcd_palmte.c
+++ b/drivers/video/omap/lcd_palmte.c
@@ -23,7 +23,6 @@
 #include <linux/platform_device.h>
 #include <linux/io.h>
 
-#include <plat/fpga.h>
 #include "omapfb.h"
 
 static int palmte_panel_init(struct lcd_panel *panel,
diff --git a/drivers/video/omap/omapfb_main.c b/drivers/video/omap/omapfb_main.c
index f54b463709e..4351c438b76 100644
--- a/drivers/video/omap/omapfb_main.c
+++ b/drivers/video/omap/omapfb_main.c
@@ -131,15 +131,6 @@ static void omapfb_rqueue_unlock(struct omapfb_device *fbdev)
  * LCD controller and LCD DMA
  * ---------------------------------------------------------------------------
  */
-/* Lookup table to map elem size to elem type. */
-static const int dma_elem_type[] = {
-	0,
-	OMAP_DMA_DATA_TYPE_S8,
-	OMAP_DMA_DATA_TYPE_S16,
-	0,
-	OMAP_DMA_DATA_TYPE_S32,
-};
-
 /*
  * Allocate resources needed for LCD controller and LCD DMA operations. Video
  * memory is allocated from system memory according to the virtual display
diff --git a/drivers/video/omap2/displays/panel-acx565akm.c b/drivers/video/omap2/displays/panel-acx565akm.c
index eaeed4340e0..c835aa70f96 100644
--- a/drivers/video/omap2/displays/panel-acx565akm.c
+++ b/drivers/video/omap2/displays/panel-acx565akm.c
@@ -600,6 +600,9 @@ static int acx_panel_power_on(struct omap_dss_device *dssdev)
 
 	mutex_lock(&md->mutex);
 
+	omapdss_sdi_set_timings(dssdev, &dssdev->panel.timings);
+	omapdss_sdi_set_datapairs(dssdev, dssdev->phy.sdi.datapairs);
+
 	r = omapdss_sdi_display_enable(dssdev);
 	if (r) {
 		pr_err("%s sdi enable failed\n", __func__);
@@ -731,18 +734,9 @@ static int acx_panel_resume(struct omap_dss_device *dssdev)
 static void acx_panel_set_timings(struct omap_dss_device *dssdev,
 		struct omap_video_timings *timings)
 {
-	int r;
-
-	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
-		omapdss_sdi_display_disable(dssdev);
+	omapdss_sdi_set_timings(dssdev, timings);
 
 	dssdev->panel.timings = *timings;
-
-	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
-		r = omapdss_sdi_display_enable(dssdev);
-		if (r)
-			dev_err(&dssdev->dev, "%s enable failed\n", __func__);
-	}
 }
 
 static int acx_panel_check_timings(struct omap_dss_device *dssdev,
diff --git a/drivers/video/omap2/displays/panel-generic-dpi.c b/drivers/video/omap2/displays/panel-generic-dpi.c
index bc5af2500eb..88295c52681 100644
--- a/drivers/video/omap2/displays/panel-generic-dpi.c
+++ b/drivers/video/omap2/displays/panel-generic-dpi.c
@@ -545,6 +545,8 @@ struct panel_drv_data {
 	struct omap_dss_device *dssdev;
 
 	struct panel_config *panel_config;
+
+	struct mutex lock;
 };
 
 static inline struct panel_generic_dpi_data
@@ -563,6 +565,9 @@ static int generic_dpi_panel_power_on(struct omap_dss_device *dssdev)
 	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
 		return 0;
 
+	omapdss_dpi_set_timings(dssdev, &dssdev->panel.timings);
+	omapdss_dpi_set_data_lines(dssdev, dssdev->phy.dpi.data_lines);
+
 	r = omapdss_dpi_display_enable(dssdev);
 	if (r)
 		goto err0;
@@ -634,6 +639,8 @@ static int generic_dpi_panel_probe(struct omap_dss_device *dssdev)
 	drv_data->dssdev = dssdev;
 	drv_data->panel_config = panel_config;
 
+	mutex_init(&drv_data->lock);
+
 	dev_set_drvdata(&dssdev->dev, drv_data);
 
 	return 0;
@@ -652,56 +659,108 @@ static void __exit generic_dpi_panel_remove(struct omap_dss_device *dssdev)
 
 static int generic_dpi_panel_enable(struct omap_dss_device *dssdev)
 {
-	int r = 0;
+	struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
+	int r;
+
+	mutex_lock(&drv_data->lock);
 
 	r = generic_dpi_panel_power_on(dssdev);
 	if (r)
-		return r;
+		goto err;
 
 	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+err:
+	mutex_unlock(&drv_data->lock);
 
-	return 0;
+	return r;
 }
 
 static void generic_dpi_panel_disable(struct omap_dss_device *dssdev)
 {
+	struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
+
+	mutex_lock(&drv_data->lock);
+
 	generic_dpi_panel_power_off(dssdev);
 
 	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+
+	mutex_unlock(&drv_data->lock);
 }
 
 static int generic_dpi_panel_suspend(struct omap_dss_device *dssdev)
 {
+	struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
+
+	mutex_lock(&drv_data->lock);
+
 	generic_dpi_panel_power_off(dssdev);
 
 	dssdev->state = OMAP_DSS_DISPLAY_SUSPENDED;
 
+	mutex_unlock(&drv_data->lock);
+
 	return 0;
 }
 
 static int generic_dpi_panel_resume(struct omap_dss_device *dssdev)
 {
-	int r = 0;
+	struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
+	int r;
+
+	mutex_lock(&drv_data->lock);
 
 	r = generic_dpi_panel_power_on(dssdev);
 	if (r)
-		return r;
+		goto err;
 
 	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
 
-	return 0;
+err:
+	mutex_unlock(&drv_data->lock);
+
+	return r;
 }
 
 static void generic_dpi_panel_set_timings(struct omap_dss_device *dssdev,
 		struct omap_video_timings *timings)
 {
-	dpi_set_timings(dssdev, timings);
+	struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
+
+	mutex_lock(&drv_data->lock);
+
+	omapdss_dpi_set_timings(dssdev, timings);
+
+	dssdev->panel.timings = *timings;
+
+	mutex_unlock(&drv_data->lock);
+}
+
+static void generic_dpi_panel_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
+
+	mutex_lock(&drv_data->lock);
+
+	*timings = dssdev->panel.timings;
+
+	mutex_unlock(&drv_data->lock);
 }
 
 static int generic_dpi_panel_check_timings(struct omap_dss_device *dssdev,
 		struct omap_video_timings *timings)
 {
-	return dpi_check_timings(dssdev, timings);
+	struct panel_drv_data *drv_data = dev_get_drvdata(&dssdev->dev);
+	int r;
+
+	mutex_lock(&drv_data->lock);
+
+	r = dpi_check_timings(dssdev, timings);
+
+	mutex_unlock(&drv_data->lock);
+
+	return r;
 }
 
 static struct omap_dss_driver dpi_driver = {
@@ -714,6 +773,7 @@ static struct omap_dss_driver dpi_driver = {
 	.resume		= generic_dpi_panel_resume,
 
 	.set_timings	= generic_dpi_panel_set_timings,
+	.get_timings	= generic_dpi_panel_get_timings,
 	.check_timings	= generic_dpi_panel_check_timings,
 
 	.driver         = {
diff --git a/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c b/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
index 80280779884..90c1cabf244 100644
--- a/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
+++ b/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
@@ -55,6 +55,9 @@ static int lb035q02_panel_power_on(struct omap_dss_device *dssdev)
 	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
 		return 0;
 
+	omapdss_dpi_set_timings(dssdev, &dssdev->panel.timings);
+	omapdss_dpi_set_data_lines(dssdev, dssdev->phy.dpi.data_lines);
+
 	r = omapdss_dpi_display_enable(dssdev);
 	if (r)
 		goto err0;
diff --git a/drivers/video/omap2/displays/panel-n8x0.c b/drivers/video/omap2/displays/panel-n8x0.c
index e6c115373c0..3fc5ad081a2 100644
--- a/drivers/video/omap2/displays/panel-n8x0.c
+++ b/drivers/video/omap2/displays/panel-n8x0.c
@@ -150,11 +150,17 @@ static void blizzard_ctrl_setup_update(struct omap_dss_device *dssdev,
 			BLIZZARD_SRC_WRITE_LCD :
 			BLIZZARD_SRC_WRITE_LCD_DESTRUCTIVE;
 
-	omap_rfbi_configure(dssdev, 16, 8);
+	omapdss_rfbi_set_pixel_size(dssdev, 16);
+	omapdss_rfbi_set_data_lines(dssdev, 8);
+
+	omap_rfbi_configure(dssdev);
 
 	blizzard_write(BLIZZARD_INPUT_WIN_X_START_0, tmp, 18);
 
-	omap_rfbi_configure(dssdev, 16, 16);
+	omapdss_rfbi_set_pixel_size(dssdev, 16);
+	omapdss_rfbi_set_data_lines(dssdev, 16);
+
+	omap_rfbi_configure(dssdev);
 }
 
 static void mipid_transfer(struct spi_device *spi, int cmd, const u8 *wbuf,
@@ -297,6 +303,12 @@ static int n8x0_panel_power_on(struct omap_dss_device *dssdev)
 			goto err_plat_en;
 	}
 
+	omapdss_rfbi_set_size(dssdev, dssdev->panel.timings.x_res,
+		dssdev->panel.timings.y_res);
+	omapdss_rfbi_set_pixel_size(dssdev, dssdev->ctrl.pixel_size);
+	omapdss_rfbi_set_data_lines(dssdev, dssdev->phy.rfbi.data_lines);
+	omapdss_rfbi_set_interface_timings(dssdev, &dssdev->ctrl.rfbi_timings);
+
 	r = omapdss_rfbi_display_enable(dssdev);
 	if (r)
 		goto err_rfbi_en;
@@ -477,6 +489,7 @@ static int n8x0_panel_probe(struct omap_dss_device *dssdev)
 	dssdev->panel.timings.y_res = 480;
 	dssdev->ctrl.pixel_size = 16;
 	dssdev->ctrl.rfbi_timings = n8x0_panel_timings;
+	dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE;
 
 	memset(&props, 0, sizeof(props));
 	props.max_brightness = 127;
@@ -625,17 +638,25 @@ static int n8x0_panel_update(struct omap_dss_device *dssdev,
 		u16 x, u16 y, u16 w, u16 h)
 {
 	struct panel_drv_data *ddata = get_drv_data(dssdev);
+	u16 dw, dh;
 
 	dev_dbg(&dssdev->dev, "update\n");
 
+	dw = dssdev->panel.timings.x_res;
+	dh = dssdev->panel.timings.y_res;
+
+	if (x != 0 || y != 0 || w != dw || h != dh) {
+		dev_err(&dssdev->dev, "invaid update region %d, %d, %d, %d\n",
+			x, y, w, h);
+		return -EINVAL;
+	}
+
 	mutex_lock(&ddata->lock);
 	rfbi_bus_lock();
 
-	omap_rfbi_prepare_update(dssdev, &x, &y, &w, &h);
-
 	blizzard_ctrl_setup_update(dssdev, x, y, w, h);
 
-	omap_rfbi_update(dssdev, x, y, w, h, update_done, NULL);
+	omap_rfbi_update(dssdev, update_done, NULL);
 
 	mutex_unlock(&ddata->lock);
 
diff --git a/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c b/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c
index b122b0f31c4..908fd268f3d 100644
--- a/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c
+++ b/drivers/video/omap2/displays/panel-nec-nl8048hl11-01b.c
@@ -175,6 +175,9 @@ static int nec_8048_panel_power_on(struct omap_dss_device *dssdev)
 	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
 		return 0;
 
+	omapdss_dpi_set_timings(dssdev, &dssdev->panel.timings);
+	omapdss_dpi_set_data_lines(dssdev, dssdev->phy.dpi.data_lines);
+
 	r = omapdss_dpi_display_enable(dssdev);
 	if (r)
 		goto err0;
diff --git a/drivers/video/omap2/displays/panel-picodlp.c b/drivers/video/omap2/displays/panel-picodlp.c
index 2d35bd38886..9df87640ddd 100644
--- a/drivers/video/omap2/displays/panel-picodlp.c
+++ b/drivers/video/omap2/displays/panel-picodlp.c
@@ -377,6 +377,10 @@ static int picodlp_panel_power_on(struct omap_dss_device *dssdev)
 	 * then only i2c commands can be successfully sent to dpp2600
 	 */
 	msleep(1000);
+
+	omapdss_dpi_set_timings(dssdev, &dssdev->panel.timings);
+	omapdss_dpi_set_data_lines(dssdev, dssdev->phy.dpi.data_lines);
+
 	r = omapdss_dpi_display_enable(dssdev);
 	if (r) {
 		dev_err(&dssdev->dev, "failed to enable DPI\n");
diff --git a/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c b/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
index bd86ba9ccf7..1ec3b277ff1 100644
--- a/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
+++ b/drivers/video/omap2/displays/panel-sharp-ls037v7dw01.c
@@ -142,6 +142,9 @@ static int sharp_ls_power_on(struct omap_dss_device *dssdev)
 	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
 		return 0;
 
+	omapdss_dpi_set_timings(dssdev, &dssdev->panel.timings);
+	omapdss_dpi_set_data_lines(dssdev, dssdev->phy.dpi.data_lines);
+
 	r = omapdss_dpi_display_enable(dssdev);
 	if (r)
 		goto err0;
diff --git a/drivers/video/omap2/displays/panel-taal.c b/drivers/video/omap2/displays/panel-taal.c
index 6b5e6e0e202..f2f644680ca 100644
--- a/drivers/video/omap2/displays/panel-taal.c
+++ b/drivers/video/omap2/displays/panel-taal.c
@@ -121,6 +121,18 @@ struct taal_data {
 
 	struct omap_dss_device *dssdev;
 
+	/* panel specific HW info */
+	struct panel_config *panel_config;
+
+	/* panel HW configuration from DT or platform data */
+	int reset_gpio;
+	int ext_te_gpio;
+
+	bool use_dsi_backlight;
+
+	struct omap_dsi_pin_config pin_config;
+
+	/* runtime variables */
 	bool enabled;
 	u8 rotate;
 	bool mirror;
@@ -145,16 +157,8 @@ struct taal_data {
 	bool ulps_enabled;
 	unsigned ulps_timeout;
 	struct delayed_work ulps_work;
-
-	struct panel_config *panel_config;
 };
 
-static inline struct nokia_dsi_panel_data
-*get_panel_data(const struct omap_dss_device *dssdev)
-{
-	return (struct nokia_dsi_panel_data *) dssdev->data;
-}
-
 static void taal_esd_work(struct work_struct *work);
 static void taal_ulps_work(struct work_struct *work);
 
@@ -371,7 +375,6 @@ static void taal_cancel_ulps_work(struct omap_dss_device *dssdev)
 static int taal_enter_ulps(struct omap_dss_device *dssdev)
 {
 	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-	struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
 	int r;
 
 	if (td->ulps_enabled)
@@ -383,7 +386,8 @@ static int taal_enter_ulps(struct omap_dss_device *dssdev)
 	if (r)
 		goto err;
 
-	disable_irq(gpio_to_irq(panel_data->ext_te_gpio));
+	if (gpio_is_valid(td->ext_te_gpio))
+		disable_irq(gpio_to_irq(td->ext_te_gpio));
 
 	omapdss_dsi_display_disable(dssdev, false, true);
 
@@ -405,7 +409,6 @@ err:
 static int taal_exit_ulps(struct omap_dss_device *dssdev)
 {
 	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-	struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
 	int r;
 
 	if (!td->ulps_enabled)
@@ -425,7 +428,8 @@ static int taal_exit_ulps(struct omap_dss_device *dssdev)
 		goto err2;
 	}
 
-	enable_irq(gpio_to_irq(panel_data->ext_te_gpio));
+	if (gpio_is_valid(td->ext_te_gpio))
+		enable_irq(gpio_to_irq(td->ext_te_gpio));
 
 	taal_queue_ulps_work(dssdev);
 
@@ -438,7 +442,8 @@ err2:
 
 	r = taal_panel_reset(dssdev);
 	if (!r) {
-		enable_irq(gpio_to_irq(panel_data->ext_te_gpio));
+		if (gpio_is_valid(td->ext_te_gpio))
+			enable_irq(gpio_to_irq(td->ext_te_gpio));
 		td->ulps_enabled = false;
 	}
 err1:
@@ -835,94 +840,135 @@ static struct attribute_group taal_attr_group = {
 static void taal_hw_reset(struct omap_dss_device *dssdev)
 {
 	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-	struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
 
-	if (panel_data->reset_gpio == -1)
+	if (!gpio_is_valid(td->reset_gpio))
 		return;
 
-	gpio_set_value(panel_data->reset_gpio, 1);
+	gpio_set_value(td->reset_gpio, 1);
 	if (td->panel_config->reset_sequence.high)
 		udelay(td->panel_config->reset_sequence.high);
 	/* reset the panel */
-	gpio_set_value(panel_data->reset_gpio, 0);
+	gpio_set_value(td->reset_gpio, 0);
 	/* assert reset */
 	if (td->panel_config->reset_sequence.low)
 		udelay(td->panel_config->reset_sequence.low);
-	gpio_set_value(panel_data->reset_gpio, 1);
+	gpio_set_value(td->reset_gpio, 1);
 	/* wait after releasing reset */
 	if (td->panel_config->sleep.hw_reset)
 		msleep(td->panel_config->sleep.hw_reset);
 }
 
+static void taal_probe_pdata(struct taal_data *td,
+		const struct nokia_dsi_panel_data *pdata)
+{
+	td->reset_gpio = pdata->reset_gpio;
+
+	if (pdata->use_ext_te)
+		td->ext_te_gpio = pdata->ext_te_gpio;
+	else
+		td->ext_te_gpio = -1;
+
+	td->esd_interval = pdata->esd_interval;
+	td->ulps_timeout = pdata->ulps_timeout;
+
+	td->use_dsi_backlight = pdata->use_dsi_backlight;
+
+	td->pin_config = pdata->pin_config;
+}
+
 static int taal_probe(struct omap_dss_device *dssdev)
 {
 	struct backlight_properties props;
 	struct taal_data *td;
 	struct backlight_device *bldev = NULL;
-	struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
-	struct panel_config *panel_config = NULL;
 	int r, i;
+	const char *panel_name;
 
 	dev_dbg(&dssdev->dev, "probe\n");
 
-	if (!panel_data || !panel_data->name) {
-		r = -EINVAL;
-		goto err;
+	td = devm_kzalloc(&dssdev->dev, sizeof(*td), GFP_KERNEL);
+	if (!td)
+		return -ENOMEM;
+
+	dev_set_drvdata(&dssdev->dev, td);
+	td->dssdev = dssdev;
+
+	if (dssdev->data) {
+		const struct nokia_dsi_panel_data *pdata = dssdev->data;
+
+		taal_probe_pdata(td, pdata);
+
+		panel_name = pdata->name;
+	} else {
+		return -ENODEV;
 	}
 
+	if (panel_name == NULL)
+		return -EINVAL;
+
 	for (i = 0; i < ARRAY_SIZE(panel_configs); i++) {
-		if (strcmp(panel_data->name, panel_configs[i].name) == 0) {
-			panel_config = &panel_configs[i];
+		if (strcmp(panel_name, panel_configs[i].name) == 0) {
+			td->panel_config = &panel_configs[i];
 			break;
 		}
 	}
 
-	if (!panel_config) {
-		r = -EINVAL;
-		goto err;
-	}
+	if (!td->panel_config)
+		return -EINVAL;
 
-	dssdev->panel.timings = panel_config->timings;
+	dssdev->panel.timings = td->panel_config->timings;
 	dssdev->panel.dsi_pix_fmt = OMAP_DSS_DSI_FMT_RGB888;
-
-	td = kzalloc(sizeof(*td), GFP_KERNEL);
-	if (!td) {
-		r = -ENOMEM;
-		goto err;
-	}
-	td->dssdev = dssdev;
-	td->panel_config = panel_config;
-	td->esd_interval = panel_data->esd_interval;
-	td->ulps_enabled = false;
-	td->ulps_timeout = panel_data->ulps_timeout;
+	dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE |
+		OMAP_DSS_DISPLAY_CAP_TEAR_ELIM;
 
 	mutex_init(&td->lock);
 
 	atomic_set(&td->do_update, 0);
 
-	td->workqueue = create_singlethread_workqueue("taal_esd");
-	if (td->workqueue == NULL) {
-		dev_err(&dssdev->dev, "can't create ESD workqueue\n");
-		r = -ENOMEM;
-		goto err_wq;
+	if (gpio_is_valid(td->reset_gpio)) {
+		r = devm_gpio_request_one(&dssdev->dev, td->reset_gpio,
+				GPIOF_OUT_INIT_LOW, "taal rst");
+		if (r) {
+			dev_err(&dssdev->dev, "failed to request reset gpio\n");
+			return r;
+		}
 	}
-	INIT_DEFERRABLE_WORK(&td->esd_work, taal_esd_work);
-	INIT_DELAYED_WORK(&td->ulps_work, taal_ulps_work);
 
-	dev_set_drvdata(&dssdev->dev, td);
+	if (gpio_is_valid(td->ext_te_gpio)) {
+		r = devm_gpio_request_one(&dssdev->dev, td->ext_te_gpio,
+				GPIOF_IN, "taal irq");
+		if (r) {
+			dev_err(&dssdev->dev, "GPIO request failed\n");
+			return r;
+		}
+
+		r = devm_request_irq(&dssdev->dev, gpio_to_irq(td->ext_te_gpio),
+				taal_te_isr,
+				IRQF_TRIGGER_RISING,
+				"taal vsync", dssdev);
 
-	if (gpio_is_valid(panel_data->reset_gpio)) {
-		r = gpio_request_one(panel_data->reset_gpio, GPIOF_OUT_INIT_LOW,
-				"taal rst");
 		if (r) {
-			dev_err(&dssdev->dev, "failed to request reset gpio\n");
-			goto err_rst_gpio;
+			dev_err(&dssdev->dev, "IRQ request failed\n");
+			return r;
 		}
+
+		INIT_DEFERRABLE_WORK(&td->te_timeout_work,
+					taal_te_timeout_work_callback);
+
+		dev_dbg(&dssdev->dev, "Using GPIO TE\n");
 	}
 
+	td->workqueue = create_singlethread_workqueue("taal_esd");
+	if (td->workqueue == NULL) {
+		dev_err(&dssdev->dev, "can't create ESD workqueue\n");
+		return -ENOMEM;
+	}
+	INIT_DEFERRABLE_WORK(&td->esd_work, taal_esd_work);
+	INIT_DELAYED_WORK(&td->ulps_work, taal_ulps_work);
+
 	taal_hw_reset(dssdev);
 
-	if (panel_data->use_dsi_backlight) {
+	if (td->use_dsi_backlight) {
 		memset(&props, 0, sizeof(struct backlight_properties));
 		props.max_brightness = 255;
 
@@ -943,31 +989,6 @@ static int taal_probe(struct omap_dss_device *dssdev)
 		taal_bl_update_status(bldev);
 	}
 
-	if (panel_data->use_ext_te) {
-		int gpio = panel_data->ext_te_gpio;
-
-		r = gpio_request_one(gpio, GPIOF_IN, "taal irq");
-		if (r) {
-			dev_err(&dssdev->dev, "GPIO request failed\n");
-			goto err_gpio;
-		}
-
-		r = request_irq(gpio_to_irq(gpio), taal_te_isr,
-				IRQF_TRIGGER_RISING,
-				"taal vsync", dssdev);
-
-		if (r) {
-			dev_err(&dssdev->dev, "IRQ request failed\n");
-			gpio_free(gpio);
-			goto err_irq;
-		}
-
-		INIT_DEFERRABLE_WORK(&td->te_timeout_work,
-				     taal_te_timeout_work_callback);
-
-		dev_dbg(&dssdev->dev, "Using GPIO TE\n");
-	}
-
 	r = omap_dsi_request_vc(dssdev, &td->channel);
 	if (r) {
 		dev_err(&dssdev->dev, "failed to get virtual channel\n");
@@ -991,29 +1012,16 @@ static int taal_probe(struct omap_dss_device *dssdev)
 err_vc_id:
 	omap_dsi_release_vc(dssdev, td->channel);
 err_req_vc:
-	if (panel_data->use_ext_te)
-		free_irq(gpio_to_irq(panel_data->ext_te_gpio), dssdev);
-err_irq:
-	if (panel_data->use_ext_te)
-		gpio_free(panel_data->ext_te_gpio);
-err_gpio:
 	if (bldev != NULL)
 		backlight_device_unregister(bldev);
 err_bl:
-	if (gpio_is_valid(panel_data->reset_gpio))
-		gpio_free(panel_data->reset_gpio);
-err_rst_gpio:
 	destroy_workqueue(td->workqueue);
-err_wq:
-	kfree(td);
-err:
 	return r;
 }
 
 static void __exit taal_remove(struct omap_dss_device *dssdev)
 {
 	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-	struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
 	struct backlight_device *bldev;
 
 	dev_dbg(&dssdev->dev, "remove\n");
@@ -1021,12 +1029,6 @@ static void __exit taal_remove(struct omap_dss_device *dssdev)
 	sysfs_remove_group(&dssdev->dev.kobj, &taal_attr_group);
 	omap_dsi_release_vc(dssdev, td->channel);
 
-	if (panel_data->use_ext_te) {
-		int gpio = panel_data->ext_te_gpio;
-		free_irq(gpio_to_irq(gpio), dssdev);
-		gpio_free(gpio);
-	}
-
 	bldev = td->bldev;
 	if (bldev != NULL) {
 		bldev->props.power = FB_BLANK_POWERDOWN;
@@ -1040,26 +1042,31 @@ static void __exit taal_remove(struct omap_dss_device *dssdev)
 
 	/* reset, to be sure that the panel is in a valid state */
 	taal_hw_reset(dssdev);
-
-	if (gpio_is_valid(panel_data->reset_gpio))
-		gpio_free(panel_data->reset_gpio);
-
-	kfree(td);
 }
 
 static int taal_power_on(struct omap_dss_device *dssdev)
 {
 	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-	struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
 	u8 id1, id2, id3;
 	int r;
 
-	r = omapdss_dsi_configure_pins(dssdev, &panel_data->pin_config);
+	r = omapdss_dsi_configure_pins(dssdev, &td->pin_config);
 	if (r) {
 		dev_err(&dssdev->dev, "failed to configure DSI pins\n");
 		goto err0;
 	};
 
+	omapdss_dsi_set_size(dssdev, dssdev->panel.timings.x_res,
+		dssdev->panel.timings.y_res);
+	omapdss_dsi_set_pixel_format(dssdev, OMAP_DSS_DSI_FMT_RGB888);
+	omapdss_dsi_set_operation_mode(dssdev, OMAP_DSS_DSI_CMD_MODE);
+
+	r = omapdss_dsi_set_clocks(dssdev, 216000000, 10000000);
+	if (r) {
+		dev_err(&dssdev->dev, "failed to set HS and LP clocks\n");
+		goto err0;
+	}
+
 	r = omapdss_dsi_display_enable(dssdev);
 	if (r) {
 		dev_err(&dssdev->dev, "failed to enable DSI\n");
@@ -1356,7 +1363,6 @@ static int taal_update(struct omap_dss_device *dssdev,
 				    u16 x, u16 y, u16 w, u16 h)
 {
 	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-	struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
 	int r;
 
 	dev_dbg(&dssdev->dev, "update %d, %d, %d x %d\n", x, y, w, h);
@@ -1380,7 +1386,7 @@ static int taal_update(struct omap_dss_device *dssdev,
 	if (r)
 		goto err;
 
-	if (td->te_enabled && panel_data->use_ext_te) {
+	if (td->te_enabled && gpio_is_valid(td->ext_te_gpio)) {
 		schedule_delayed_work(&td->te_timeout_work,
 				msecs_to_jiffies(250));
 		atomic_set(&td->do_update, 1);
@@ -1419,7 +1425,6 @@ static int taal_sync(struct omap_dss_device *dssdev)
 static int _taal_enable_te(struct omap_dss_device *dssdev, bool enable)
 {
 	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
-	struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
 	int r;
 
 	if (enable)
@@ -1427,7 +1432,7 @@ static int _taal_enable_te(struct omap_dss_device *dssdev, bool enable)
 	else
 		r = taal_dcs_write_0(td, MIPI_DCS_SET_TEAR_OFF);
 
-	if (!panel_data->use_ext_te)
+	if (!gpio_is_valid(td->ext_te_gpio))
 		omapdss_dsi_enable_te(dssdev, enable);
 
 	if (td->panel_config->sleep.enable_te)
@@ -1487,6 +1492,7 @@ static int taal_get_te(struct omap_dss_device *dssdev)
 static int taal_rotate(struct omap_dss_device *dssdev, u8 rotate)
 {
 	struct taal_data *td = dev_get_drvdata(&dssdev->dev);
+	u16 dw, dh;
 	int r;
 
 	dev_dbg(&dssdev->dev, "rotate %d\n", rotate);
@@ -1508,6 +1514,16 @@ static int taal_rotate(struct omap_dss_device *dssdev, u8 rotate)
 			goto err;
 	}
 
+	if (rotate == 0 || rotate == 2) {
+		dw = dssdev->panel.timings.x_res;
+		dh = dssdev->panel.timings.y_res;
+	} else {
+		dw = dssdev->panel.timings.y_res;
+		dh = dssdev->panel.timings.x_res;
+	}
+
+	omapdss_dsi_set_size(dssdev, dw, dh);
+
 	td->rotate = rotate;
 
 	dsi_bus_unlock(dssdev);
@@ -1726,7 +1742,6 @@ static void taal_esd_work(struct work_struct *work)
 	struct taal_data *td = container_of(work, struct taal_data,
 			esd_work.work);
 	struct omap_dss_device *dssdev = td->dssdev;
-	struct nokia_dsi_panel_data *panel_data = get_panel_data(dssdev);
 	u8 state1, state2;
 	int r;
 
@@ -1773,7 +1788,7 @@ static void taal_esd_work(struct work_struct *work)
 	}
 	/* Self-diagnostics result is also shown on TE GPIO line. We need
 	 * to re-enable TE after self diagnostics */
-	if (td->te_enabled && panel_data->use_ext_te) {
+	if (td->te_enabled && gpio_is_valid(td->ext_te_gpio)) {
 		r = taal_dcs_write_1(td, MIPI_DCS_SET_TEAR_ON, 0);
 		if (r)
 			goto err;
diff --git a/drivers/video/omap2/displays/panel-tfp410.c b/drivers/video/omap2/displays/panel-tfp410.c
index 40cc0cfa5d1..383811cf864 100644
--- a/drivers/video/omap2/displays/panel-tfp410.c
+++ b/drivers/video/omap2/displays/panel-tfp410.c
@@ -65,6 +65,9 @@ static int tfp410_power_on(struct omap_dss_device *dssdev)
 	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
 		return 0;
 
+	omapdss_dpi_set_timings(dssdev, &dssdev->panel.timings);
+	omapdss_dpi_set_data_lines(dssdev, dssdev->phy.dpi.data_lines);
+
 	r = omapdss_dpi_display_enable(dssdev);
 	if (r)
 		goto err0;
@@ -116,8 +119,8 @@ static int tfp410_probe(struct omap_dss_device *dssdev)
 	}
 
 	if (gpio_is_valid(ddata->pd_gpio)) {
-		r = gpio_request_one(ddata->pd_gpio, GPIOF_OUT_INIT_LOW,
-				"tfp410 pd");
+		r = devm_gpio_request_one(&dssdev->dev, ddata->pd_gpio,
+				GPIOF_OUT_INIT_LOW, "tfp410 pd");
 		if (r) {
 			dev_err(&dssdev->dev, "Failed to request PD GPIO %d\n",
 					ddata->pd_gpio);
@@ -132,8 +135,7 @@ static int tfp410_probe(struct omap_dss_device *dssdev)
 		if (!adapter) {
 			dev_err(&dssdev->dev, "Failed to get I2C adapter, bus %d\n",
 					i2c_bus_num);
-			r = -EINVAL;
-			goto err_i2c;
+			return -EINVAL;
 		}
 
 		ddata->i2c_adapter = adapter;
@@ -142,10 +144,6 @@ static int tfp410_probe(struct omap_dss_device *dssdev)
 	dev_set_drvdata(&dssdev->dev, ddata);
 
 	return 0;
-err_i2c:
-	if (gpio_is_valid(ddata->pd_gpio))
-		gpio_free(ddata->pd_gpio);
-	return r;
 }
 
 static void __exit tfp410_remove(struct omap_dss_device *dssdev)
@@ -157,9 +155,6 @@ static void __exit tfp410_remove(struct omap_dss_device *dssdev)
 	if (ddata->i2c_adapter)
 		i2c_put_adapter(ddata->i2c_adapter);
 
-	if (gpio_is_valid(ddata->pd_gpio))
-		gpio_free(ddata->pd_gpio);
-
 	dev_set_drvdata(&dssdev->dev, NULL);
 
 	mutex_unlock(&ddata->lock);
@@ -231,7 +226,8 @@ static void tfp410_set_timings(struct omap_dss_device *dssdev,
 	struct panel_drv_data *ddata = dev_get_drvdata(&dssdev->dev);
 
 	mutex_lock(&ddata->lock);
-	dpi_set_timings(dssdev, timings);
+	omapdss_dpi_set_timings(dssdev, timings);
+	dssdev->panel.timings = *timings;
 	mutex_unlock(&ddata->lock);
 }
 
diff --git a/drivers/video/omap2/displays/panel-tpo-td043mtea1.c b/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
index fa7baa650ae..b5e6dbc59f0 100644
--- a/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
+++ b/drivers/video/omap2/displays/panel-tpo-td043mtea1.c
@@ -337,6 +337,9 @@ static int tpo_td043_enable_dss(struct omap_dss_device *dssdev)
 	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
 		return 0;
 
+	omapdss_dpi_set_timings(dssdev, &dssdev->panel.timings);
+	omapdss_dpi_set_data_lines(dssdev, dssdev->phy.dpi.data_lines);
+
 	r = omapdss_dpi_display_enable(dssdev);
 	if (r)
 		goto err0;
@@ -480,7 +483,9 @@ static void tpo_td043_remove(struct omap_dss_device *dssdev)
 static void tpo_td043_set_timings(struct omap_dss_device *dssdev,
 		struct omap_video_timings *timings)
 {
-	dpi_set_timings(dssdev, timings);
+	omapdss_dpi_set_timings(dssdev, timings);
+
+	dssdev->panel.timings = *timings;
 }
 
 static int tpo_td043_check_timings(struct omap_dss_device *dssdev,
diff --git a/drivers/video/omap2/dss/Kconfig b/drivers/video/omap2/dss/Kconfig
index b337a8469fd..80f5390aa13 100644
--- a/drivers/video/omap2/dss/Kconfig
+++ b/drivers/video/omap2/dss/Kconfig
@@ -84,7 +84,7 @@ config OMAP2_DSS_SDI
 
 config OMAP2_DSS_DSI
 	bool "DSI support"
-	depends on ARCH_OMAP3 || ARCH_OMAP4
+	depends on ARCH_OMAP3 || ARCH_OMAP4 || ARCH_OMAP5
         default n
 	help
 	  MIPI DSI (Display Serial Interface) support.
diff --git a/drivers/video/omap2/dss/Makefile b/drivers/video/omap2/dss/Makefile
index 5c450b0f94d..4549869bfe1 100644
--- a/drivers/video/omap2/dss/Makefile
+++ b/drivers/video/omap2/dss/Makefile
@@ -1,9 +1,9 @@
 obj-$(CONFIG_OMAP2_DSS) += omapdss.o
 omapdss-y := core.o dss.o dss_features.o dispc.o dispc_coefs.o display.o \
-	manager.o overlay.o apply.o
+	manager.o manager-sysfs.o overlay.o overlay-sysfs.o output.o apply.o
 omapdss-$(CONFIG_OMAP2_DSS_DPI) += dpi.o
 omapdss-$(CONFIG_OMAP2_DSS_RFBI) += rfbi.o
-omapdss-$(CONFIG_OMAP2_DSS_VENC) += venc.o
+omapdss-$(CONFIG_OMAP2_DSS_VENC) += venc.o venc_panel.o
 omapdss-$(CONFIG_OMAP2_DSS_SDI) += sdi.o
 omapdss-$(CONFIG_OMAP2_DSS_DSI) += dsi.o
 omapdss-$(CONFIG_OMAP4_DSS_HDMI) += hdmi.o \
diff --git a/drivers/video/omap2/dss/apply.c b/drivers/video/omap2/dss/apply.c
index 0fefc68372b..19d66f471b4 100644
--- a/drivers/video/omap2/dss/apply.c
+++ b/drivers/video/omap2/dss/apply.c
@@ -111,9 +111,6 @@ static struct {
 	struct ovl_priv_data ovl_priv_data_array[MAX_DSS_OVERLAYS];
 	struct mgr_priv_data mgr_priv_data_array[MAX_DSS_MANAGERS];
 
-	bool fifo_merge_dirty;
-	bool fifo_merge;
-
 	bool irq_enabled;
 } dss_data;
 
@@ -424,17 +421,25 @@ static void wait_pending_extra_info_updates(void)
 int dss_mgr_wait_for_go(struct omap_overlay_manager *mgr)
 {
 	unsigned long timeout = msecs_to_jiffies(500);
-	struct mgr_priv_data *mp;
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
 	u32 irq;
+	unsigned long flags;
 	int r;
 	int i;
-	struct omap_dss_device *dssdev = mgr->device;
 
-	if (!dssdev || dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
+	spin_lock_irqsave(&data_lock, flags);
+
+	if (mgr_manual_update(mgr)) {
+		spin_unlock_irqrestore(&data_lock, flags);
 		return 0;
+	}
 
-	if (mgr_manual_update(mgr))
+	if (!mp->enabled) {
+		spin_unlock_irqrestore(&data_lock, flags);
 		return 0;
+	}
+
+	spin_unlock_irqrestore(&data_lock, flags);
 
 	r = dispc_runtime_get();
 	if (r)
@@ -442,10 +447,8 @@ int dss_mgr_wait_for_go(struct omap_overlay_manager *mgr)
 
 	irq = dispc_mgr_get_vsync_irq(mgr->id);
 
-	mp = get_mgr_priv(mgr);
 	i = 0;
 	while (1) {
-		unsigned long flags;
 		bool shadow_dirty, dirty;
 
 		spin_lock_irqsave(&data_lock, flags);
@@ -489,21 +492,30 @@ int dss_mgr_wait_for_go_ovl(struct omap_overlay *ovl)
 {
 	unsigned long timeout = msecs_to_jiffies(500);
 	struct ovl_priv_data *op;
-	struct omap_dss_device *dssdev;
+	struct mgr_priv_data *mp;
 	u32 irq;
+	unsigned long flags;
 	int r;
 	int i;
 
 	if (!ovl->manager)
 		return 0;
 
-	dssdev = ovl->manager->device;
+	mp = get_mgr_priv(ovl->manager);
+
+	spin_lock_irqsave(&data_lock, flags);
 
-	if (!dssdev || dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
+	if (ovl_manual_update(ovl)) {
+		spin_unlock_irqrestore(&data_lock, flags);
 		return 0;
+	}
 
-	if (ovl_manual_update(ovl))
+	if (!mp->enabled) {
+		spin_unlock_irqrestore(&data_lock, flags);
 		return 0;
+	}
+
+	spin_unlock_irqrestore(&data_lock, flags);
 
 	r = dispc_runtime_get();
 	if (r)
@@ -514,7 +526,6 @@ int dss_mgr_wait_for_go_ovl(struct omap_overlay *ovl)
 	op = get_ovl_priv(ovl);
 	i = 0;
 	while (1) {
-		unsigned long flags;
 		bool shadow_dirty, dirty;
 
 		spin_lock_irqsave(&data_lock, flags);
@@ -573,7 +584,7 @@ static void dss_ovl_write_regs(struct omap_overlay *ovl)
 
 	replication = dss_ovl_use_replication(mp->lcd_config, oi->color_mode);
 
-	r = dispc_ovl_setup(ovl->id, oi, replication, &mp->timings);
+	r = dispc_ovl_setup(ovl->id, oi, replication, &mp->timings, false);
 	if (r) {
 		/*
 		 * We can't do much here, as this function can be called from
@@ -677,40 +688,11 @@ static void dss_mgr_write_regs_extra(struct omap_overlay_manager *mgr)
 		mp->shadow_extra_info_dirty = true;
 }
 
-static void dss_write_regs_common(void)
-{
-	const int num_mgrs = omap_dss_get_num_overlay_managers();
-	int i;
-
-	if (!dss_data.fifo_merge_dirty)
-		return;
-
-	for (i = 0; i < num_mgrs; ++i) {
-		struct omap_overlay_manager *mgr;
-		struct mgr_priv_data *mp;
-
-		mgr = omap_dss_get_overlay_manager(i);
-		mp = get_mgr_priv(mgr);
-
-		if (mp->enabled) {
-			if (dss_data.fifo_merge_dirty) {
-				dispc_enable_fifomerge(dss_data.fifo_merge);
-				dss_data.fifo_merge_dirty = false;
-			}
-
-			if (mp->updating)
-				mp->shadow_info_dirty = true;
-		}
-	}
-}
-
 static void dss_write_regs(void)
 {
 	const int num_mgrs = omap_dss_get_num_overlay_managers();
 	int i;
 
-	dss_write_regs_common();
-
 	for (i = 0; i < num_mgrs; ++i) {
 		struct omap_overlay_manager *mgr;
 		struct mgr_priv_data *mp;
@@ -799,8 +781,6 @@ void dss_mgr_start_update(struct omap_overlay_manager *mgr)
 	dss_mgr_write_regs(mgr);
 	dss_mgr_write_regs_extra(mgr);
 
-	dss_write_regs_common();
-
 	mp->updating = true;
 
 	if (!dss_data.irq_enabled && need_isr())
@@ -984,20 +964,11 @@ static void dss_apply_ovl_fifo_thresholds(struct omap_overlay *ovl,
 	op->extra_info_dirty = true;
 }
 
-static void dss_apply_fifo_merge(bool use_fifo_merge)
-{
-	if (dss_data.fifo_merge == use_fifo_merge)
-		return;
-
-	dss_data.fifo_merge = use_fifo_merge;
-	dss_data.fifo_merge_dirty = true;
-}
-
-static void dss_ovl_setup_fifo(struct omap_overlay *ovl,
-		bool use_fifo_merge)
+static void dss_ovl_setup_fifo(struct omap_overlay *ovl)
 {
 	struct ovl_priv_data *op = get_ovl_priv(ovl);
 	u32 fifo_low, fifo_high;
+	bool use_fifo_merge = false;
 
 	if (!op->enabled && !op->enabling)
 		return;
@@ -1008,8 +979,7 @@ static void dss_ovl_setup_fifo(struct omap_overlay *ovl,
 	dss_apply_ovl_fifo_thresholds(ovl, fifo_low, fifo_high);
 }
 
-static void dss_mgr_setup_fifos(struct omap_overlay_manager *mgr,
-		bool use_fifo_merge)
+static void dss_mgr_setup_fifos(struct omap_overlay_manager *mgr)
 {
 	struct omap_overlay *ovl;
 	struct mgr_priv_data *mp;
@@ -1020,94 +990,19 @@ static void dss_mgr_setup_fifos(struct omap_overlay_manager *mgr,
 		return;
 
 	list_for_each_entry(ovl, &mgr->overlays, list)
-		dss_ovl_setup_fifo(ovl, use_fifo_merge);
-}
-
-static void dss_setup_fifos(bool use_fifo_merge)
-{
-	const int num_mgrs = omap_dss_get_num_overlay_managers();
-	struct omap_overlay_manager *mgr;
-	int i;
-
-	for (i = 0; i < num_mgrs; ++i) {
-		mgr = omap_dss_get_overlay_manager(i);
-		dss_mgr_setup_fifos(mgr, use_fifo_merge);
-	}
+		dss_ovl_setup_fifo(ovl);
 }
 
-static int get_num_used_managers(void)
+static void dss_setup_fifos(void)
 {
 	const int num_mgrs = omap_dss_get_num_overlay_managers();
 	struct omap_overlay_manager *mgr;
-	struct mgr_priv_data *mp;
 	int i;
-	int enabled_mgrs;
-
-	enabled_mgrs = 0;
 
 	for (i = 0; i < num_mgrs; ++i) {
 		mgr = omap_dss_get_overlay_manager(i);
-		mp = get_mgr_priv(mgr);
-
-		if (!mp->enabled)
-			continue;
-
-		enabled_mgrs++;
+		dss_mgr_setup_fifos(mgr);
 	}
-
-	return enabled_mgrs;
-}
-
-static int get_num_used_overlays(void)
-{
-	const int num_ovls = omap_dss_get_num_overlays();
-	struct omap_overlay *ovl;
-	struct ovl_priv_data *op;
-	struct mgr_priv_data *mp;
-	int i;
-	int enabled_ovls;
-
-	enabled_ovls = 0;
-
-	for (i = 0; i < num_ovls; ++i) {
-		ovl = omap_dss_get_overlay(i);
-		op = get_ovl_priv(ovl);
-
-		if (!op->enabled && !op->enabling)
-			continue;
-
-		mp = get_mgr_priv(ovl->manager);
-
-		if (!mp->enabled)
-			continue;
-
-		enabled_ovls++;
-	}
-
-	return enabled_ovls;
-}
-
-static bool get_use_fifo_merge(void)
-{
-	int enabled_mgrs = get_num_used_managers();
-	int enabled_ovls = get_num_used_overlays();
-
-	if (!dss_has_feature(FEAT_FIFO_MERGE))
-		return false;
-
-	/*
-	 * In theory the only requirement for fifomerge is enabled_ovls <= 1.
-	 * However, if we have two managers enabled and set/unset the fifomerge,
-	 * we need to set the GO bits in particular sequence for the managers,
-	 * and wait in between.
-	 *
-	 * This is rather difficult as new apply calls can happen at any time,
-	 * so we simplify the problem by requiring also that enabled_mgrs <= 1.
-	 * In practice this shouldn't matter, because when only one overlay is
-	 * enabled, most likely only one output is enabled.
-	 */
-
-	return enabled_mgrs <= 1 && enabled_ovls <= 1;
 }
 
 int dss_mgr_enable(struct omap_overlay_manager *mgr)
@@ -1115,7 +1010,6 @@ int dss_mgr_enable(struct omap_overlay_manager *mgr)
 	struct mgr_priv_data *mp = get_mgr_priv(mgr);
 	unsigned long flags;
 	int r;
-	bool fifo_merge;
 
 	mutex_lock(&apply_lock);
 
@@ -1133,23 +1027,11 @@ int dss_mgr_enable(struct omap_overlay_manager *mgr)
 		goto err;
 	}
 
-	/* step 1: setup fifos/fifomerge before enabling the manager */
-
-	fifo_merge = get_use_fifo_merge();
-	dss_setup_fifos(fifo_merge);
-	dss_apply_fifo_merge(fifo_merge);
+	dss_setup_fifos();
 
 	dss_write_regs();
 	dss_set_go_bits();
 
-	spin_unlock_irqrestore(&data_lock, flags);
-
-	/* wait until fifo config is in */
-	wait_pending_extra_info_updates();
-
-	/* step 2: enable the manager */
-	spin_lock_irqsave(&data_lock, flags);
-
 	if (!mgr_manual_update(mgr))
 		mp->updating = true;
 
@@ -1174,7 +1056,6 @@ void dss_mgr_disable(struct omap_overlay_manager *mgr)
 {
 	struct mgr_priv_data *mp = get_mgr_priv(mgr);
 	unsigned long flags;
-	bool fifo_merge;
 
 	mutex_lock(&apply_lock);
 
@@ -1189,16 +1070,8 @@ void dss_mgr_disable(struct omap_overlay_manager *mgr)
 	mp->updating = false;
 	mp->enabled = false;
 
-	fifo_merge = get_use_fifo_merge();
-	dss_setup_fifos(fifo_merge);
-	dss_apply_fifo_merge(fifo_merge);
-
-	dss_write_regs();
-	dss_set_go_bits();
-
 	spin_unlock_irqrestore(&data_lock, flags);
 
-	wait_pending_extra_info_updates();
 out:
 	mutex_unlock(&apply_lock);
 }
@@ -1237,29 +1110,29 @@ void dss_mgr_get_info(struct omap_overlay_manager *mgr,
 	spin_unlock_irqrestore(&data_lock, flags);
 }
 
-int dss_mgr_set_device(struct omap_overlay_manager *mgr,
-		struct omap_dss_device *dssdev)
+int dss_mgr_set_output(struct omap_overlay_manager *mgr,
+		struct omap_dss_output *output)
 {
 	int r;
 
 	mutex_lock(&apply_lock);
 
-	if (dssdev->manager) {
-		DSSERR("display '%s' already has a manager '%s'\n",
-			       dssdev->name, dssdev->manager->name);
+	if (mgr->output) {
+		DSSERR("manager %s is already connected to an output\n",
+			mgr->name);
 		r = -EINVAL;
 		goto err;
 	}
 
-	if ((mgr->supported_displays & dssdev->type) == 0) {
-		DSSERR("display '%s' does not support manager '%s'\n",
-			       dssdev->name, mgr->name);
+	if ((mgr->supported_outputs & output->id) == 0) {
+		DSSERR("output does not support manager %s\n",
+			mgr->name);
 		r = -EINVAL;
 		goto err;
 	}
 
-	dssdev->manager = mgr;
-	mgr->device = dssdev;
+	output->manager = mgr;
+	mgr->output = output;
 
 	mutex_unlock(&apply_lock);
 
@@ -1269,40 +1142,46 @@ err:
 	return r;
 }
 
-int dss_mgr_unset_device(struct omap_overlay_manager *mgr)
+int dss_mgr_unset_output(struct omap_overlay_manager *mgr)
 {
 	int r;
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+	unsigned long flags;
 
 	mutex_lock(&apply_lock);
 
-	if (!mgr->device) {
-		DSSERR("failed to unset display, display not set.\n");
+	if (!mgr->output) {
+		DSSERR("failed to unset output, output not set\n");
 		r = -EINVAL;
 		goto err;
 	}
 
-	/*
-	 * Don't allow currently enabled displays to have the overlay manager
-	 * pulled out from underneath them
-	 */
-	if (mgr->device->state != OMAP_DSS_DISPLAY_DISABLED) {
+	spin_lock_irqsave(&data_lock, flags);
+
+	if (mp->enabled) {
+		DSSERR("output can't be unset when manager is enabled\n");
 		r = -EINVAL;
-		goto err;
+		goto err1;
 	}
 
-	mgr->device->manager = NULL;
-	mgr->device = NULL;
+	spin_unlock_irqrestore(&data_lock, flags);
+
+	mgr->output->manager = NULL;
+	mgr->output = NULL;
 
 	mutex_unlock(&apply_lock);
 
 	return 0;
+err1:
+	spin_unlock_irqrestore(&data_lock, flags);
 err:
 	mutex_unlock(&apply_lock);
+
 	return r;
 }
 
 static void dss_apply_mgr_timings(struct omap_overlay_manager *mgr,
-		struct omap_video_timings *timings)
+		const struct omap_video_timings *timings)
 {
 	struct mgr_priv_data *mp = get_mgr_priv(mgr);
 
@@ -1311,24 +1190,22 @@ static void dss_apply_mgr_timings(struct omap_overlay_manager *mgr,
 }
 
 void dss_mgr_set_timings(struct omap_overlay_manager *mgr,
-		struct omap_video_timings *timings)
+		const struct omap_video_timings *timings)
 {
 	unsigned long flags;
-
-	mutex_lock(&apply_lock);
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
 
 	spin_lock_irqsave(&data_lock, flags);
 
-	dss_apply_mgr_timings(mgr, timings);
-
-	dss_write_regs();
-	dss_set_go_bits();
+	if (mp->updating) {
+		DSSERR("cannot set timings for %s: manager needs to be disabled\n",
+			mgr->name);
+		goto out;
+	}
 
+	dss_apply_mgr_timings(mgr, timings);
+out:
 	spin_unlock_irqrestore(&data_lock, flags);
-
-	wait_pending_extra_info_updates();
-
-	mutex_unlock(&apply_lock);
 }
 
 static void dss_apply_mgr_lcd_config(struct omap_overlay_manager *mgr,
@@ -1346,7 +1223,7 @@ void dss_mgr_set_lcd_config(struct omap_overlay_manager *mgr,
 	unsigned long flags;
 	struct mgr_priv_data *mp = get_mgr_priv(mgr);
 
-	mutex_lock(&apply_lock);
+	spin_lock_irqsave(&data_lock, flags);
 
 	if (mp->enabled) {
 		DSSERR("cannot apply lcd config for %s: manager needs to be disabled\n",
@@ -1354,19 +1231,9 @@ void dss_mgr_set_lcd_config(struct omap_overlay_manager *mgr,
 		goto out;
 	}
 
-	spin_lock_irqsave(&data_lock, flags);
-
 	dss_apply_mgr_lcd_config(mgr, config);
-
-	dss_write_regs();
-	dss_set_go_bits();
-
-	spin_unlock_irqrestore(&data_lock, flags);
-
-	wait_pending_extra_info_updates();
-
 out:
-	mutex_unlock(&apply_lock);
+	spin_unlock_irqrestore(&data_lock, flags);
 }
 
 int dss_ovl_set_info(struct omap_overlay *ovl,
@@ -1483,6 +1350,13 @@ int dss_ovl_unset_manager(struct omap_overlay *ovl)
 		goto err;
 	}
 
+	spin_unlock_irqrestore(&data_lock, flags);
+
+	/* wait for pending extra_info updates to ensure the ovl is disabled */
+	wait_pending_extra_info_updates();
+
+	spin_lock_irqsave(&data_lock, flags);
+
 	op->channel = -1;
 
 	ovl->manager = NULL;
@@ -1517,7 +1391,6 @@ int dss_ovl_enable(struct omap_overlay *ovl)
 {
 	struct ovl_priv_data *op = get_ovl_priv(ovl);
 	unsigned long flags;
-	bool fifo_merge;
 	int r;
 
 	mutex_lock(&apply_lock);
@@ -1527,7 +1400,7 @@ int dss_ovl_enable(struct omap_overlay *ovl)
 		goto err1;
 	}
 
-	if (ovl->manager == NULL || ovl->manager->device == NULL) {
+	if (ovl->manager == NULL || ovl->manager->output == NULL) {
 		r = -EINVAL;
 		goto err1;
 	}
@@ -1543,22 +1416,7 @@ int dss_ovl_enable(struct omap_overlay *ovl)
 		goto err2;
 	}
 
-	/* step 1: configure fifos/fifomerge for currently enabled ovls */
-
-	fifo_merge = get_use_fifo_merge();
-	dss_setup_fifos(fifo_merge);
-	dss_apply_fifo_merge(fifo_merge);
-
-	dss_write_regs();
-	dss_set_go_bits();
-
-	spin_unlock_irqrestore(&data_lock, flags);
-
-	/* wait for fifo configs to go in */
-	wait_pending_extra_info_updates();
-
-	/* step 2: enable the overlay */
-	spin_lock_irqsave(&data_lock, flags);
+	dss_setup_fifos();
 
 	op->enabling = false;
 	dss_apply_ovl_enable(ovl, true);
@@ -1568,9 +1426,6 @@ int dss_ovl_enable(struct omap_overlay *ovl)
 
 	spin_unlock_irqrestore(&data_lock, flags);
 
-	/* wait for overlay to be enabled */
-	wait_pending_extra_info_updates();
-
 	mutex_unlock(&apply_lock);
 
 	return 0;
@@ -1586,7 +1441,6 @@ int dss_ovl_disable(struct omap_overlay *ovl)
 {
 	struct ovl_priv_data *op = get_ovl_priv(ovl);
 	unsigned long flags;
-	bool fifo_merge;
 	int r;
 
 	mutex_lock(&apply_lock);
@@ -1596,39 +1450,19 @@ int dss_ovl_disable(struct omap_overlay *ovl)
 		goto err;
 	}
 
-	if (ovl->manager == NULL || ovl->manager->device == NULL) {
+	if (ovl->manager == NULL || ovl->manager->output == NULL) {
 		r = -EINVAL;
 		goto err;
 	}
 
-	/* step 1: disable the overlay */
 	spin_lock_irqsave(&data_lock, flags);
 
 	dss_apply_ovl_enable(ovl, false);
-
 	dss_write_regs();
 	dss_set_go_bits();
 
 	spin_unlock_irqrestore(&data_lock, flags);
 
-	/* wait for the overlay to be disabled */
-	wait_pending_extra_info_updates();
-
-	/* step 2: configure fifos/fifomerge */
-	spin_lock_irqsave(&data_lock, flags);
-
-	fifo_merge = get_use_fifo_merge();
-	dss_setup_fifos(fifo_merge);
-	dss_apply_fifo_merge(fifo_merge);
-
-	dss_write_regs();
-	dss_set_go_bits();
-
-	spin_unlock_irqrestore(&data_lock, flags);
-
-	/* wait for fifo config to go in */
-	wait_pending_extra_info_updates();
-
 	mutex_unlock(&apply_lock);
 
 	return 0;
diff --git a/drivers/video/omap2/dss/core.c b/drivers/video/omap2/dss/core.c
index 58bd9c27369..b2af72dc20b 100644
--- a/drivers/video/omap2/dss/core.c
+++ b/drivers/video/omap2/dss/core.c
@@ -33,6 +33,7 @@
 #include <linux/device.h>
 #include <linux/regulator/consumer.h>
 #include <linux/suspend.h>
+#include <linux/slab.h>
 
 #include <video/omapdss.h>
 
@@ -57,6 +58,11 @@ bool dss_debug;
 module_param_named(debug, dss_debug, bool, 0644);
 #endif
 
+const char *dss_get_default_display_name(void)
+{
+	return core.default_display_name;
+}
+
 /* REGULATORS */
 
 struct regulator *dss_get_vdds_dsi(void)
@@ -347,17 +353,14 @@ static int dss_driver_probe(struct device *dev)
 	int r;
 	struct omap_dss_driver *dssdrv = to_dss_driver(dev->driver);
 	struct omap_dss_device *dssdev = to_dss_device(dev);
-	bool force;
 
 	DSSDBG("driver_probe: dev %s/%s, drv %s\n",
 				dev_name(dev), dssdev->driver_name,
 				dssdrv->driver.name);
 
-	dss_init_device(core.pdev, dssdev);
-
-	force = core.default_display_name &&
-		strcmp(core.default_display_name, dssdev->name) == 0;
-	dss_recheck_connections(dssdev, force);
+	r = dss_init_device(core.pdev, dssdev);
+	if (r)
+		return r;
 
 	r = dssdrv->probe(dssdev);
 
@@ -416,54 +419,44 @@ void omap_dss_unregister_driver(struct omap_dss_driver *dssdriver)
 EXPORT_SYMBOL(omap_dss_unregister_driver);
 
 /* DEVICE */
-static void reset_device(struct device *dev, int check)
-{
-	u8 *dev_p = (u8 *)dev;
-	u8 *dev_end = dev_p + sizeof(*dev);
-	void *saved_pdata;
-
-	saved_pdata = dev->platform_data;
-	if (check) {
-		/*
-		 * Check if there is any other setting than platform_data
-		 * in struct device; warn that these will be reset by our
-		 * init.
-		 */
-		dev->platform_data = NULL;
-		while (dev_p < dev_end) {
-			if (*dev_p) {
-				WARN("%s: struct device fields will be "
-						"discarded\n",
-				     __func__);
-				break;
-			}
-			dev_p++;
-		}
-	}
-	memset(dev, 0, sizeof(*dev));
-	dev->platform_data = saved_pdata;
-}
-
 
 static void omap_dss_dev_release(struct device *dev)
 {
-	reset_device(dev, 0);
+	struct omap_dss_device *dssdev = to_dss_device(dev);
+	kfree(dssdev);
 }
 
-int omap_dss_register_device(struct omap_dss_device *dssdev,
-		struct device *parent, int disp_num)
+static int disp_num_counter;
+
+struct omap_dss_device *dss_alloc_and_init_device(struct device *parent)
 {
-	WARN_ON(!dssdev->driver_name);
+	struct omap_dss_device *dssdev;
+
+	dssdev = kzalloc(sizeof(*dssdev), GFP_KERNEL);
+	if (!dssdev)
+		return NULL;
 
-	reset_device(&dssdev->dev, 1);
 	dssdev->dev.bus = &dss_bus_type;
 	dssdev->dev.parent = parent;
 	dssdev->dev.release = omap_dss_dev_release;
-	dev_set_name(&dssdev->dev, "display%d", disp_num);
-	return device_register(&dssdev->dev);
+	dev_set_name(&dssdev->dev, "display%d", disp_num_counter++);
+
+	device_initialize(&dssdev->dev);
+
+	return dssdev;
+}
+
+int dss_add_device(struct omap_dss_device *dssdev)
+{
+	return device_add(&dssdev->dev);
+}
+
+void dss_put_device(struct omap_dss_device *dssdev)
+{
+	put_device(&dssdev->dev);
 }
 
-void omap_dss_unregister_device(struct omap_dss_device *dssdev)
+void dss_unregister_device(struct omap_dss_device *dssdev)
 {
 	device_unregister(&dssdev->dev);
 }
@@ -471,15 +464,25 @@ void omap_dss_unregister_device(struct omap_dss_device *dssdev)
 static int dss_unregister_dss_dev(struct device *dev, void *data)
 {
 	struct omap_dss_device *dssdev = to_dss_device(dev);
-	omap_dss_unregister_device(dssdev);
+	dss_unregister_device(dssdev);
 	return 0;
 }
 
-void omap_dss_unregister_child_devices(struct device *parent)
+void dss_unregister_child_devices(struct device *parent)
 {
 	device_for_each_child(parent, NULL, dss_unregister_dss_dev);
 }
 
+void dss_copy_device_pdata(struct omap_dss_device *dst,
+		const struct omap_dss_device *src)
+{
+	u8 *d = (u8 *)dst;
+	u8 *s = (u8 *)src;
+	size_t dsize = sizeof(struct device);
+
+	memcpy(d + dsize, s + dsize, sizeof(struct omap_dss_device) - dsize);
+}
+
 /* BUS */
 static int __init omap_dss_bus_register(void)
 {
diff --git a/drivers/video/omap2/dss/dispc.c b/drivers/video/omap2/dss/dispc.c
index ee9e29639dc..b43477a5fae 100644
--- a/drivers/video/omap2/dss/dispc.c
+++ b/drivers/video/omap2/dss/dispc.c
@@ -38,7 +38,6 @@
 #include <linux/pm_runtime.h>
 
 #include <plat/cpu.h>
-#include <plat/clock.h>
 
 #include <video/omapdss.h>
 
@@ -82,6 +81,30 @@ struct dispc_irq_stats {
 	unsigned irqs[32];
 };
 
+struct dispc_features {
+	u8 sw_start;
+	u8 fp_start;
+	u8 bp_start;
+	u16 sw_max;
+	u16 vp_max;
+	u16 hp_max;
+	int (*calc_scaling) (enum omap_plane plane,
+		const struct omap_video_timings *mgr_timings,
+		u16 width, u16 height, u16 out_width, u16 out_height,
+		enum omap_color_mode color_mode, bool *five_taps,
+		int *x_predecim, int *y_predecim, int *decim_x, int *decim_y,
+		u16 pos_x, unsigned long *core_clk, bool mem_to_mem);
+	unsigned long (*calc_core_clk) (enum omap_plane plane,
+		u16 width, u16 height, u16 out_width, u16 out_height,
+		bool mem_to_mem);
+	u8 num_fifos;
+
+	/* swap GFX & WB fifos */
+	bool gfx_fifo_workaround:1;
+};
+
+#define DISPC_MAX_NR_FIFOS 5
+
 static struct {
 	struct platform_device *pdev;
 	void __iomem    *base;
@@ -91,7 +114,9 @@ static struct {
 	int irq;
 	struct clk *dss_clk;
 
-	u32	fifo_size[MAX_DSS_OVERLAYS];
+	u32 fifo_size[DISPC_MAX_NR_FIFOS];
+	/* maps which plane is using a fifo. fifo-id -> plane-id */
+	int fifo_assignment[DISPC_MAX_NR_FIFOS];
 
 	spinlock_t irq_lock;
 	u32 irq_error_mask;
@@ -102,6 +127,8 @@ static struct {
 	bool		ctx_valid;
 	u32		ctx[DISPC_SZ_REGS / sizeof(u32)];
 
+	const struct dispc_features *feat;
+
 #ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
 	spinlock_t irq_stats_lock;
 	struct dispc_irq_stats irq_stats;
@@ -211,7 +238,14 @@ static const struct {
 	},
 };
 
+struct color_conv_coef {
+	int ry, rcr, rcb, gy, gcr, gcb, by, bcr, bcb;
+	int full_range;
+};
+
 static void _omap_dispc_set_irqs(void);
+static unsigned long dispc_plane_pclk_rate(enum omap_plane plane);
+static unsigned long dispc_plane_lclk_rate(enum omap_plane plane);
 
 static inline void dispc_write_reg(const u16 idx, u32 val)
 {
@@ -509,6 +543,11 @@ u32 dispc_mgr_get_framedone_irq(enum omap_channel channel)
 	return mgr_desc[channel].framedone_irq;
 }
 
+u32 dispc_wb_get_framedone_irq(void)
+{
+	return DISPC_IRQ_FRAMEDONEWB;
+}
+
 bool dispc_mgr_go_busy(enum omap_channel channel)
 {
 	return mgr_fld_read(channel, DISPC_MGR_FLD_GO) == 1;
@@ -536,6 +575,30 @@ void dispc_mgr_go(enum omap_channel channel)
 	mgr_fld_write(channel, DISPC_MGR_FLD_GO, 1);
 }
 
+bool dispc_wb_go_busy(void)
+{
+	return REG_GET(DISPC_CONTROL2, 6, 6) == 1;
+}
+
+void dispc_wb_go(void)
+{
+	enum omap_plane plane = OMAP_DSS_WB;
+	bool enable, go;
+
+	enable = REG_GET(DISPC_OVL_ATTRIBUTES(plane), 0, 0) == 1;
+
+	if (!enable)
+		return;
+
+	go = REG_GET(DISPC_CONTROL2, 6, 6) == 1;
+	if (go) {
+		DSSERR("GO bit not down for WB\n");
+		return;
+	}
+
+	REG_FLD_MOD(DISPC_CONTROL2, 1, 6, 6);
+}
+
 static void dispc_ovl_write_firh_reg(enum omap_plane plane, int reg, u32 value)
 {
 	dispc_write_reg(DISPC_OVL_FIR_COEF_H(plane, reg), value);
@@ -618,41 +681,41 @@ static void dispc_ovl_set_scale_coef(enum omap_plane plane, int fir_hinc,
 	}
 }
 
-static void _dispc_setup_color_conv_coef(void)
-{
-	int i;
-	const struct color_conv_coef {
-		int  ry,  rcr,  rcb,   gy,  gcr,  gcb,   by,  bcr,  bcb;
-		int  full_range;
-	}  ctbl_bt601_5 = {
-		298,  409,    0,  298, -208, -100,  298,    0,  517, 0,
-	};
-
-	const struct color_conv_coef *ct;
 
+static void dispc_ovl_write_color_conv_coef(enum omap_plane plane,
+		const struct color_conv_coef *ct)
+{
 #define CVAL(x, y) (FLD_VAL(x, 26, 16) | FLD_VAL(y, 10, 0))
 
-	ct = &ctbl_bt601_5;
+	dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 0), CVAL(ct->rcr, ct->ry));
+	dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 1), CVAL(ct->gy,  ct->rcb));
+	dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 2), CVAL(ct->gcb, ct->gcr));
+	dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 3), CVAL(ct->bcr, ct->by));
+	dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 4), CVAL(0, ct->bcb));
 
-	for (i = 1; i < dss_feat_get_num_ovls(); i++) {
-		dispc_write_reg(DISPC_OVL_CONV_COEF(i, 0),
-			CVAL(ct->rcr, ct->ry));
-		dispc_write_reg(DISPC_OVL_CONV_COEF(i, 1),
-			CVAL(ct->gy,  ct->rcb));
-		dispc_write_reg(DISPC_OVL_CONV_COEF(i, 2),
-			CVAL(ct->gcb, ct->gcr));
-		dispc_write_reg(DISPC_OVL_CONV_COEF(i, 3),
-			CVAL(ct->bcr, ct->by));
-		dispc_write_reg(DISPC_OVL_CONV_COEF(i, 4),
-			CVAL(0, ct->bcb));
-
-		REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(i), ct->full_range,
-			11, 11);
-	}
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), ct->full_range, 11, 11);
 
 #undef CVAL
 }
 
+static void dispc_setup_color_conv_coef(void)
+{
+	int i;
+	int num_ovl = dss_feat_get_num_ovls();
+	int num_wb = dss_feat_get_num_wbs();
+	const struct color_conv_coef ctbl_bt601_5_ovl = {
+		298, 409, 0, 298, -208, -100, 298, 0, 517, 0,
+	};
+	const struct color_conv_coef ctbl_bt601_5_wb = {
+		66, 112, -38, 129, -94, -74, 25, -18, 112, 0,
+	};
+
+	for (i = 1; i < num_ovl; i++)
+		dispc_ovl_write_color_conv_coef(i, &ctbl_bt601_5_ovl);
+
+	for (; i < num_wb; i++)
+		dispc_ovl_write_color_conv_coef(i, &ctbl_bt601_5_wb);
+}
 
 static void dispc_ovl_set_ba0(enum omap_plane plane, u32 paddr)
 {
@@ -674,24 +737,32 @@ static void dispc_ovl_set_ba1_uv(enum omap_plane plane, u32 paddr)
 	dispc_write_reg(DISPC_OVL_BA1_UV(plane), paddr);
 }
 
-static void dispc_ovl_set_pos(enum omap_plane plane, int x, int y)
+static void dispc_ovl_set_pos(enum omap_plane plane,
+		enum omap_overlay_caps caps, int x, int y)
 {
-	u32 val = FLD_VAL(y, 26, 16) | FLD_VAL(x, 10, 0);
+	u32 val;
+
+	if ((caps & OMAP_DSS_OVL_CAP_POS) == 0)
+		return;
+
+	val = FLD_VAL(y, 26, 16) | FLD_VAL(x, 10, 0);
 
 	dispc_write_reg(DISPC_OVL_POSITION(plane), val);
 }
 
-static void dispc_ovl_set_pic_size(enum omap_plane plane, int width, int height)
+static void dispc_ovl_set_input_size(enum omap_plane plane, int width,
+		int height)
 {
 	u32 val = FLD_VAL(height - 1, 26, 16) | FLD_VAL(width - 1, 10, 0);
 
-	if (plane == OMAP_DSS_GFX)
+	if (plane == OMAP_DSS_GFX || plane == OMAP_DSS_WB)
 		dispc_write_reg(DISPC_OVL_SIZE(plane), val);
 	else
 		dispc_write_reg(DISPC_OVL_PICTURE_SIZE(plane), val);
 }
 
-static void dispc_ovl_set_vid_size(enum omap_plane plane, int width, int height)
+static void dispc_ovl_set_output_size(enum omap_plane plane, int width,
+		int height)
 {
 	u32 val;
 
@@ -699,14 +770,16 @@ static void dispc_ovl_set_vid_size(enum omap_plane plane, int width, int height)
 
 	val = FLD_VAL(height - 1, 26, 16) | FLD_VAL(width - 1, 10, 0);
 
-	dispc_write_reg(DISPC_OVL_SIZE(plane), val);
+	if (plane == OMAP_DSS_WB)
+		dispc_write_reg(DISPC_OVL_PICTURE_SIZE(plane), val);
+	else
+		dispc_write_reg(DISPC_OVL_SIZE(plane), val);
 }
 
-static void dispc_ovl_set_zorder(enum omap_plane plane, u8 zorder)
+static void dispc_ovl_set_zorder(enum omap_plane plane,
+		enum omap_overlay_caps caps, u8 zorder)
 {
-	struct omap_overlay *ovl = omap_dss_get_overlay(plane);
-
-	if ((ovl->caps & OMAP_DSS_OVL_CAP_ZORDER) == 0)
+	if ((caps & OMAP_DSS_OVL_CAP_ZORDER) == 0)
 		return;
 
 	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), zorder, 27, 26);
@@ -723,23 +796,22 @@ static void dispc_ovl_enable_zorder_planes(void)
 		REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(i), 1, 25, 25);
 }
 
-static void dispc_ovl_set_pre_mult_alpha(enum omap_plane plane, bool enable)
+static void dispc_ovl_set_pre_mult_alpha(enum omap_plane plane,
+		enum omap_overlay_caps caps, bool enable)
 {
-	struct omap_overlay *ovl = omap_dss_get_overlay(plane);
-
-	if ((ovl->caps & OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA) == 0)
+	if ((caps & OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA) == 0)
 		return;
 
 	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable ? 1 : 0, 28, 28);
 }
 
-static void dispc_ovl_setup_global_alpha(enum omap_plane plane, u8 global_alpha)
+static void dispc_ovl_setup_global_alpha(enum omap_plane plane,
+		enum omap_overlay_caps caps, u8 global_alpha)
 {
 	static const unsigned shifts[] = { 0, 8, 16, 24, };
 	int shift;
-	struct omap_overlay *ovl = omap_dss_get_overlay(plane);
 
-	if ((ovl->caps & OMAP_DSS_OVL_CAP_GLOBAL_ALPHA) == 0)
+	if ((caps & OMAP_DSS_OVL_CAP_GLOBAL_ALPHA) == 0)
 		return;
 
 	shift = shifts[plane];
@@ -947,10 +1019,17 @@ static enum omap_channel dispc_ovl_get_channel_out(enum omap_plane plane)
 	return channel;
 }
 
+void dispc_wb_set_channel_in(enum dss_writeback_channel channel)
+{
+	enum omap_plane plane = OMAP_DSS_WB;
+
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), channel, 18, 16);
+}
+
 static void dispc_ovl_set_burst_size(enum omap_plane plane,
 		enum omap_burst_size burst_size)
 {
-	static const unsigned shifts[] = { 6, 14, 14, 14, };
+	static const unsigned shifts[] = { 6, 14, 14, 14, 14, };
 	int shift;
 
 	shift = shifts[plane];
@@ -1027,11 +1106,15 @@ static void dispc_ovl_set_vid_color_conv(enum omap_plane plane, bool enable)
 	dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), val);
 }
 
-static void dispc_ovl_enable_replication(enum omap_plane plane, bool enable)
+static void dispc_ovl_enable_replication(enum omap_plane plane,
+		enum omap_overlay_caps caps, bool enable)
 {
 	static const unsigned shifts[] = { 5, 10, 10, 10 };
 	int shift;
 
+	if ((caps & OMAP_DSS_OVL_CAP_REPLICATION) == 0)
+		return;
+
 	shift = shifts[plane];
 	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable, shift, shift);
 }
@@ -1045,10 +1128,10 @@ static void dispc_mgr_set_size(enum omap_channel channel, u16 width,
 	dispc_write_reg(DISPC_SIZE_MGR(channel), val);
 }
 
-static void dispc_read_plane_fifo_sizes(void)
+static void dispc_init_fifos(void)
 {
 	u32 size;
-	int plane;
+	int fifo;
 	u8 start, end;
 	u32 unit;
 
@@ -1056,16 +1139,53 @@ static void dispc_read_plane_fifo_sizes(void)
 
 	dss_feat_get_reg_field(FEAT_REG_FIFOSIZE, &start, &end);
 
-	for (plane = 0; plane < dss_feat_get_num_ovls(); ++plane) {
-		size = REG_GET(DISPC_OVL_FIFO_SIZE_STATUS(plane), start, end);
+	for (fifo = 0; fifo < dispc.feat->num_fifos; ++fifo) {
+		size = REG_GET(DISPC_OVL_FIFO_SIZE_STATUS(fifo), start, end);
 		size *= unit;
-		dispc.fifo_size[plane] = size;
+		dispc.fifo_size[fifo] = size;
+
+		/*
+		 * By default fifos are mapped directly to overlays, fifo 0 to
+		 * ovl 0, fifo 1 to ovl 1, etc.
+		 */
+		dispc.fifo_assignment[fifo] = fifo;
+	}
+
+	/*
+	 * The GFX fifo on OMAP4 is smaller than the other fifos. The small fifo
+	 * causes problems with certain use cases, like using the tiler in 2D
+	 * mode. The below hack swaps the fifos of GFX and WB planes, thus
+	 * giving GFX plane a larger fifo. WB but should work fine with a
+	 * smaller fifo.
+	 */
+	if (dispc.feat->gfx_fifo_workaround) {
+		u32 v;
+
+		v = dispc_read_reg(DISPC_GLOBAL_BUFFER);
+
+		v = FLD_MOD(v, 4, 2, 0); /* GFX BUF top to WB */
+		v = FLD_MOD(v, 4, 5, 3); /* GFX BUF bottom to WB */
+		v = FLD_MOD(v, 0, 26, 24); /* WB BUF top to GFX */
+		v = FLD_MOD(v, 0, 29, 27); /* WB BUF bottom to GFX */
+
+		dispc_write_reg(DISPC_GLOBAL_BUFFER, v);
+
+		dispc.fifo_assignment[OMAP_DSS_GFX] = OMAP_DSS_WB;
+		dispc.fifo_assignment[OMAP_DSS_WB] = OMAP_DSS_GFX;
 	}
 }
 
 static u32 dispc_ovl_get_fifo_size(enum omap_plane plane)
 {
-	return dispc.fifo_size[plane];
+	int fifo;
+	u32 size = 0;
+
+	for (fifo = 0; fifo < dispc.feat->num_fifos; ++fifo) {
+		if (dispc.fifo_assignment[fifo] == plane)
+			size += dispc.fifo_size[fifo];
+	}
+
+	return size;
 }
 
 void dispc_ovl_set_fifo_threshold(enum omap_plane plane, u32 low, u32 high)
@@ -1141,6 +1261,14 @@ void dispc_ovl_compute_fifo_thresholds(enum omap_plane plane,
 	if (manual_update && dss_has_feature(FEAT_OMAP3_DSI_FIFO_BUG)) {
 		*fifo_low = ovl_fifo_size - burst_size * 2;
 		*fifo_high = total_fifo_size - burst_size;
+	} else if (plane == OMAP_DSS_WB) {
+		/*
+		 * Most optimal configuration for writeback is to push out data
+		 * to the interconnect the moment writeback pushes enough pixels
+		 * in the FIFO to form a burst
+		 */
+		*fifo_low = 0;
+		*fifo_high = burst_size;
 	} else {
 		*fifo_low = ovl_fifo_size - burst_size;
 		*fifo_high = total_fifo_size - buf_unit;
@@ -1383,6 +1511,7 @@ static void dispc_ovl_set_scaling_uv(enum omap_plane plane,
 {
 	int scale_x = out_width != orig_width;
 	int scale_y = out_height != orig_height;
+	bool chroma_upscale = plane != OMAP_DSS_WB ? true : false;
 
 	if (!dss_has_feature(FEAT_HANDLE_UV_SEPARATE))
 		return;
@@ -1390,7 +1519,8 @@ static void dispc_ovl_set_scaling_uv(enum omap_plane plane,
 			color_mode != OMAP_DSS_COLOR_UYVY &&
 			color_mode != OMAP_DSS_COLOR_NV12)) {
 		/* reset chroma resampling for RGB formats  */
-		REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane), 0, 8, 8);
+		if (plane != OMAP_DSS_WB)
+			REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane), 0, 8, 8);
 		return;
 	}
 
@@ -1399,23 +1529,34 @@ static void dispc_ovl_set_scaling_uv(enum omap_plane plane,
 
 	switch (color_mode) {
 	case OMAP_DSS_COLOR_NV12:
-		/* UV is subsampled by 2 vertically*/
-		orig_height >>= 1;
-		/* UV is subsampled by 2 horz.*/
-		orig_width >>= 1;
+		if (chroma_upscale) {
+			/* UV is subsampled by 2 horizontally and vertically */
+			orig_height >>= 1;
+			orig_width >>= 1;
+		} else {
+			/* UV is downsampled by 2 horizontally and vertically */
+			orig_height <<= 1;
+			orig_width <<= 1;
+		}
+
 		break;
 	case OMAP_DSS_COLOR_YUV2:
 	case OMAP_DSS_COLOR_UYVY:
-		/*For YUV422 with 90/270 rotation,
-		 *we don't upsample chroma
-		 */
+		/* For YUV422 with 90/270 rotation, we don't upsample chroma */
 		if (rotation == OMAP_DSS_ROT_0 ||
-			rotation == OMAP_DSS_ROT_180)
-			/* UV is subsampled by 2 hrz*/
-			orig_width >>= 1;
+				rotation == OMAP_DSS_ROT_180) {
+			if (chroma_upscale)
+				/* UV is subsampled by 2 horizontally */
+				orig_width >>= 1;
+			else
+				/* UV is downsampled by 2 horizontally */
+				orig_width <<= 1;
+		}
+
 		/* must use FIR for YUV422 if rotated */
 		if (rotation != OMAP_DSS_ROT_0)
 			scale_x = scale_y = true;
+
 		break;
 	default:
 		BUG();
@@ -1431,8 +1572,10 @@ static void dispc_ovl_set_scaling_uv(enum omap_plane plane,
 			out_width, out_height, five_taps,
 				rotation, DISPC_COLOR_COMPONENT_UV);
 
-	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane),
-		(scale_x || scale_y) ? 1 : 0, 8, 8);
+	if (plane != OMAP_DSS_WB)
+		REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane),
+			(scale_x || scale_y) ? 1 : 0, 8, 8);
+
 	/* set H scaling */
 	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), scale_x ? 1 : 0, 5, 5);
 	/* set V scaling */
@@ -1848,22 +1991,19 @@ static void calc_tiler_rotation_offset(u16 screen_width, u16 width,
  * This function is used to avoid synclosts in OMAP3, because of some
  * undocumented horizontal position and timing related limitations.
  */
-static int check_horiz_timing_omap3(enum omap_channel channel,
+static int check_horiz_timing_omap3(enum omap_plane plane,
 		const struct omap_video_timings *t, u16 pos_x,
 		u16 width, u16 height, u16 out_width, u16 out_height)
 {
 	int DS = DIV_ROUND_UP(height, out_height);
-	unsigned long nonactive, lclk, pclk;
+	unsigned long nonactive;
 	static const u8 limits[3] = { 8, 10, 20 };
 	u64 val, blank;
+	unsigned long pclk = dispc_plane_pclk_rate(plane);
+	unsigned long lclk = dispc_plane_lclk_rate(plane);
 	int i;
 
 	nonactive = t->x_res + t->hfp + t->hsw + t->hbp - out_width;
-	pclk = dispc_mgr_pclk_rate(channel);
-	if (dss_mgr_is_lcd(channel))
-		lclk = dispc_mgr_lclk_rate(channel);
-	else
-		lclk = dispc_fclk_rate();
 
 	i = 0;
 	if (out_height < height)
@@ -1900,13 +2040,14 @@ static int check_horiz_timing_omap3(enum omap_channel channel,
 	return 0;
 }
 
-static unsigned long calc_core_clk_five_taps(enum omap_channel channel,
+static unsigned long calc_core_clk_five_taps(enum omap_plane plane,
 		const struct omap_video_timings *mgr_timings, u16 width,
 		u16 height, u16 out_width, u16 out_height,
 		enum omap_color_mode color_mode)
 {
 	u32 core_clk = 0;
-	u64 tmp, pclk = dispc_mgr_pclk_rate(channel);
+	u64 tmp;
+	unsigned long pclk = dispc_plane_pclk_rate(plane);
 
 	if (height <= out_height && width <= out_width)
 		return (unsigned long) pclk;
@@ -1940,11 +2081,22 @@ static unsigned long calc_core_clk_five_taps(enum omap_channel channel,
 	return core_clk;
 }
 
-static unsigned long calc_core_clk(enum omap_channel channel, u16 width,
-		u16 height, u16 out_width, u16 out_height)
+static unsigned long calc_core_clk_24xx(enum omap_plane plane, u16 width,
+		u16 height, u16 out_width, u16 out_height, bool mem_to_mem)
+{
+	unsigned long pclk = dispc_plane_pclk_rate(plane);
+
+	if (height > out_height && width > out_width)
+		return pclk * 4;
+	else
+		return pclk * 2;
+}
+
+static unsigned long calc_core_clk_34xx(enum omap_plane plane, u16 width,
+		u16 height, u16 out_width, u16 out_height, bool mem_to_mem)
 {
 	unsigned int hf, vf;
-	unsigned long pclk = dispc_mgr_pclk_rate(channel);
+	unsigned long pclk = dispc_plane_pclk_rate(plane);
 
 	/*
 	 * FIXME how to determine the 'A' factor
@@ -1959,51 +2111,207 @@ static unsigned long calc_core_clk(enum omap_channel channel, u16 width,
 		hf = 2;
 	else
 		hf = 1;
-
 	if (height > out_height)
 		vf = 2;
 	else
 		vf = 1;
 
-	if (cpu_is_omap24xx()) {
-		if (vf > 1 && hf > 1)
-			return pclk * 4;
-		else
-			return pclk * 2;
-	} else if (cpu_is_omap34xx()) {
-		return pclk * vf * hf;
-	} else {
-		if (hf > 1)
-			return DIV_ROUND_UP(pclk, out_width) * width;
-		else
-			return pclk;
+	return pclk * vf * hf;
+}
+
+static unsigned long calc_core_clk_44xx(enum omap_plane plane, u16 width,
+		u16 height, u16 out_width, u16 out_height, bool mem_to_mem)
+{
+	unsigned long pclk;
+
+	/*
+	 * If the overlay/writeback is in mem to mem mode, there are no
+	 * downscaling limitations with respect to pixel clock, return 1 as
+	 * required core clock to represent that we have sufficient enough
+	 * core clock to do maximum downscaling
+	 */
+	if (mem_to_mem)
+		return 1;
+
+	pclk = dispc_plane_pclk_rate(plane);
+
+	if (width > out_width)
+		return DIV_ROUND_UP(pclk, out_width) * width;
+	else
+		return pclk;
+}
+
+static int dispc_ovl_calc_scaling_24xx(enum omap_plane plane,
+		const struct omap_video_timings *mgr_timings,
+		u16 width, u16 height, u16 out_width, u16 out_height,
+		enum omap_color_mode color_mode, bool *five_taps,
+		int *x_predecim, int *y_predecim, int *decim_x, int *decim_y,
+		u16 pos_x, unsigned long *core_clk, bool mem_to_mem)
+{
+	int error;
+	u16 in_width, in_height;
+	int min_factor = min(*decim_x, *decim_y);
+	const int maxsinglelinewidth =
+			dss_feat_get_param_max(FEAT_PARAM_LINEWIDTH);
+
+	*five_taps = false;
+
+	do {
+		in_height = DIV_ROUND_UP(height, *decim_y);
+		in_width = DIV_ROUND_UP(width, *decim_x);
+		*core_clk = dispc.feat->calc_core_clk(plane, in_width,
+				in_height, out_width, out_height, mem_to_mem);
+		error = (in_width > maxsinglelinewidth || !*core_clk ||
+			*core_clk > dispc_core_clk_rate());
+		if (error) {
+			if (*decim_x == *decim_y) {
+				*decim_x = min_factor;
+				++*decim_y;
+			} else {
+				swap(*decim_x, *decim_y);
+				if (*decim_x < *decim_y)
+					++*decim_x;
+			}
+		}
+	} while (*decim_x <= *x_predecim && *decim_y <= *y_predecim && error);
+
+	if (in_width > maxsinglelinewidth) {
+		DSSERR("Cannot scale max input width exceeded");
+		return -EINVAL;
 	}
+	return 0;
 }
 
-static int dispc_ovl_calc_scaling(enum omap_plane plane,
-		enum omap_channel channel,
+static int dispc_ovl_calc_scaling_34xx(enum omap_plane plane,
 		const struct omap_video_timings *mgr_timings,
 		u16 width, u16 height, u16 out_width, u16 out_height,
 		enum omap_color_mode color_mode, bool *five_taps,
-		int *x_predecim, int *y_predecim, u16 pos_x)
+		int *x_predecim, int *y_predecim, int *decim_x, int *decim_y,
+		u16 pos_x, unsigned long *core_clk, bool mem_to_mem)
 {
-	struct omap_overlay *ovl = omap_dss_get_overlay(plane);
-	const int maxdownscale = dss_feat_get_param_max(FEAT_PARAM_DOWNSCALE);
+	int error;
+	u16 in_width, in_height;
+	int min_factor = min(*decim_x, *decim_y);
+	const int maxsinglelinewidth =
+			dss_feat_get_param_max(FEAT_PARAM_LINEWIDTH);
+
+	do {
+		in_height = DIV_ROUND_UP(height, *decim_y);
+		in_width = DIV_ROUND_UP(width, *decim_x);
+		*core_clk = calc_core_clk_five_taps(plane, mgr_timings,
+			in_width, in_height, out_width, out_height, color_mode);
+
+		error = check_horiz_timing_omap3(plane, mgr_timings,
+				pos_x, in_width, in_height, out_width,
+				out_height);
+
+		if (in_width > maxsinglelinewidth)
+			if (in_height > out_height &&
+						in_height < out_height * 2)
+				*five_taps = false;
+		if (!*five_taps)
+			*core_clk = dispc.feat->calc_core_clk(plane, in_width,
+					in_height, out_width, out_height,
+					mem_to_mem);
+
+		error = (error || in_width > maxsinglelinewidth * 2 ||
+			(in_width > maxsinglelinewidth && *five_taps) ||
+			!*core_clk || *core_clk > dispc_core_clk_rate());
+		if (error) {
+			if (*decim_x == *decim_y) {
+				*decim_x = min_factor;
+				++*decim_y;
+			} else {
+				swap(*decim_x, *decim_y);
+				if (*decim_x < *decim_y)
+					++*decim_x;
+			}
+		}
+	} while (*decim_x <= *x_predecim && *decim_y <= *y_predecim && error);
+
+	if (check_horiz_timing_omap3(plane, mgr_timings, pos_x, width, height,
+		out_width, out_height)){
+			DSSERR("horizontal timing too tight\n");
+			return -EINVAL;
+	}
+
+	if (in_width > (maxsinglelinewidth * 2)) {
+		DSSERR("Cannot setup scaling");
+		DSSERR("width exceeds maximum width possible");
+		return -EINVAL;
+	}
+
+	if (in_width > maxsinglelinewidth && *five_taps) {
+		DSSERR("cannot setup scaling with five taps");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int dispc_ovl_calc_scaling_44xx(enum omap_plane plane,
+		const struct omap_video_timings *mgr_timings,
+		u16 width, u16 height, u16 out_width, u16 out_height,
+		enum omap_color_mode color_mode, bool *five_taps,
+		int *x_predecim, int *y_predecim, int *decim_x, int *decim_y,
+		u16 pos_x, unsigned long *core_clk, bool mem_to_mem)
+{
+	u16 in_width, in_width_max;
+	int decim_x_min = *decim_x;
+	u16 in_height = DIV_ROUND_UP(height, *decim_y);
 	const int maxsinglelinewidth =
 				dss_feat_get_param_max(FEAT_PARAM_LINEWIDTH);
+	unsigned long pclk = dispc_plane_pclk_rate(plane);
+	const int maxdownscale = dss_feat_get_param_max(FEAT_PARAM_DOWNSCALE);
+
+	if (mem_to_mem)
+		in_width_max = DIV_ROUND_UP(out_width, maxdownscale);
+	else
+		in_width_max = dispc_core_clk_rate() /
+					DIV_ROUND_UP(pclk, out_width);
+
+	*decim_x = DIV_ROUND_UP(width, in_width_max);
+
+	*decim_x = *decim_x > decim_x_min ? *decim_x : decim_x_min;
+	if (*decim_x > *x_predecim)
+		return -EINVAL;
+
+	do {
+		in_width = DIV_ROUND_UP(width, *decim_x);
+	} while (*decim_x <= *x_predecim &&
+			in_width > maxsinglelinewidth && ++*decim_x);
+
+	if (in_width > maxsinglelinewidth) {
+		DSSERR("Cannot scale width exceeds max line width");
+		return -EINVAL;
+	}
+
+	*core_clk = dispc.feat->calc_core_clk(plane, in_width, in_height,
+				out_width, out_height, mem_to_mem);
+	return 0;
+}
+
+static int dispc_ovl_calc_scaling(enum omap_plane plane,
+		enum omap_overlay_caps caps,
+		const struct omap_video_timings *mgr_timings,
+		u16 width, u16 height, u16 out_width, u16 out_height,
+		enum omap_color_mode color_mode, bool *five_taps,
+		int *x_predecim, int *y_predecim, u16 pos_x,
+		enum omap_dss_rotation_type rotation_type, bool mem_to_mem)
+{
+	const int maxdownscale = dss_feat_get_param_max(FEAT_PARAM_DOWNSCALE);
 	const int max_decim_limit = 16;
 	unsigned long core_clk = 0;
-	int decim_x, decim_y, error, min_factor;
-	u16 in_width, in_height, in_width_max = 0;
+	int decim_x, decim_y, ret;
 
 	if (width == out_width && height == out_height)
 		return 0;
 
-	if ((ovl->caps & OMAP_DSS_OVL_CAP_SCALE) == 0)
+	if ((caps & OMAP_DSS_OVL_CAP_SCALE) == 0)
 		return -EINVAL;
 
 	*x_predecim = max_decim_limit;
-	*y_predecim = max_decim_limit;
+	*y_predecim = (rotation_type == OMAP_DSS_ROT_TILER &&
+			dss_has_feature(FEAT_BURST_2D)) ? 2 : max_decim_limit;
 
 	if (color_mode == OMAP_DSS_COLOR_CLUT1 ||
 	    color_mode == OMAP_DSS_COLOR_CLUT2 ||
@@ -2018,118 +2326,18 @@ static int dispc_ovl_calc_scaling(enum omap_plane plane,
 	decim_x = DIV_ROUND_UP(DIV_ROUND_UP(width, out_width), maxdownscale);
 	decim_y = DIV_ROUND_UP(DIV_ROUND_UP(height, out_height), maxdownscale);
 
-	min_factor = min(decim_x, decim_y);
-
 	if (decim_x > *x_predecim || out_width > width * 8)
 		return -EINVAL;
 
 	if (decim_y > *y_predecim || out_height > height * 8)
 		return -EINVAL;
 
-	if (cpu_is_omap24xx()) {
-		*five_taps = false;
-
-		do {
-			in_height = DIV_ROUND_UP(height, decim_y);
-			in_width = DIV_ROUND_UP(width, decim_x);
-			core_clk = calc_core_clk(channel, in_width, in_height,
-					out_width, out_height);
-			error = (in_width > maxsinglelinewidth || !core_clk ||
-				core_clk > dispc_core_clk_rate());
-			if (error) {
-				if (decim_x == decim_y) {
-					decim_x = min_factor;
-					decim_y++;
-				} else {
-					swap(decim_x, decim_y);
-					if (decim_x < decim_y)
-						decim_x++;
-				}
-			}
-		} while (decim_x <= *x_predecim && decim_y <= *y_predecim &&
-				error);
-
-		if (in_width > maxsinglelinewidth) {
-			DSSERR("Cannot scale max input width exceeded");
-			return -EINVAL;
-		}
-	} else if (cpu_is_omap34xx()) {
-
-		do {
-			in_height = DIV_ROUND_UP(height, decim_y);
-			in_width = DIV_ROUND_UP(width, decim_x);
-			core_clk = calc_core_clk_five_taps(channel, mgr_timings,
-				in_width, in_height, out_width, out_height,
-				color_mode);
-
-			error = check_horiz_timing_omap3(channel, mgr_timings,
-				pos_x, in_width, in_height, out_width,
-				out_height);
-
-			if (in_width > maxsinglelinewidth)
-				if (in_height > out_height &&
-					in_height < out_height * 2)
-					*five_taps = false;
-			if (!*five_taps)
-				core_clk = calc_core_clk(channel, in_width,
-					in_height, out_width, out_height);
-			error = (error || in_width > maxsinglelinewidth * 2 ||
-				(in_width > maxsinglelinewidth && *five_taps) ||
-				!core_clk || core_clk > dispc_core_clk_rate());
-			if (error) {
-				if (decim_x == decim_y) {
-					decim_x = min_factor;
-					decim_y++;
-				} else {
-					swap(decim_x, decim_y);
-					if (decim_x < decim_y)
-						decim_x++;
-				}
-			}
-		} while (decim_x <= *x_predecim && decim_y <= *y_predecim
-			&& error);
-
-		if (check_horiz_timing_omap3(channel, mgr_timings, pos_x, width,
-			height, out_width, out_height)){
-				DSSERR("horizontal timing too tight\n");
-				return -EINVAL;
-		}
-
-		if (in_width > (maxsinglelinewidth * 2)) {
-			DSSERR("Cannot setup scaling");
-			DSSERR("width exceeds maximum width possible");
-			return -EINVAL;
-		}
-
-		if (in_width > maxsinglelinewidth && *five_taps) {
-			DSSERR("cannot setup scaling with five taps");
-			return -EINVAL;
-		}
-	} else {
-		int decim_x_min = decim_x;
-		in_height = DIV_ROUND_UP(height, decim_y);
-		in_width_max = dispc_core_clk_rate() /
-				DIV_ROUND_UP(dispc_mgr_pclk_rate(channel),
-						out_width);
-		decim_x = DIV_ROUND_UP(width, in_width_max);
-
-		decim_x = decim_x > decim_x_min ? decim_x : decim_x_min;
-		if (decim_x > *x_predecim)
-			return -EINVAL;
-
-		do {
-			in_width = DIV_ROUND_UP(width, decim_x);
-		} while (decim_x <= *x_predecim &&
-				in_width > maxsinglelinewidth && decim_x++);
-
-		if (in_width > maxsinglelinewidth) {
-			DSSERR("Cannot scale width exceeds max line width");
-			return -EINVAL;
-		}
-
-		core_clk = calc_core_clk(channel, in_width, in_height,
-				out_width, out_height);
-	}
+	ret = dispc.feat->calc_scaling(plane, mgr_timings, width, height,
+		out_width, out_height, color_mode, five_taps,
+		x_predecim, y_predecim, &decim_x, &decim_y, pos_x, &core_clk,
+		mem_to_mem);
+	if (ret)
+		return ret;
 
 	DSSDBG("required core clk rate = %lu Hz\n", core_clk);
 	DSSDBG("current core clk rate = %lu Hz\n", dispc_core_clk_rate());
@@ -2147,69 +2355,64 @@ static int dispc_ovl_calc_scaling(enum omap_plane plane,
 	return 0;
 }
 
-int dispc_ovl_setup(enum omap_plane plane, struct omap_overlay_info *oi,
-		bool replication, const struct omap_video_timings *mgr_timings)
+static int dispc_ovl_setup_common(enum omap_plane plane,
+		enum omap_overlay_caps caps, u32 paddr, u32 p_uv_addr,
+		u16 screen_width, int pos_x, int pos_y, u16 width, u16 height,
+		u16 out_width, u16 out_height, enum omap_color_mode color_mode,
+		u8 rotation, bool mirror, u8 zorder, u8 pre_mult_alpha,
+		u8 global_alpha, enum omap_dss_rotation_type rotation_type,
+		bool replication, const struct omap_video_timings *mgr_timings,
+		bool mem_to_mem)
 {
-	struct omap_overlay *ovl = omap_dss_get_overlay(plane);
 	bool five_taps = true;
 	bool fieldmode = 0;
 	int r, cconv = 0;
 	unsigned offset0, offset1;
 	s32 row_inc;
 	s32 pix_inc;
-	u16 frame_height = oi->height;
+	u16 frame_height = height;
 	unsigned int field_offset = 0;
-	u16 in_height = oi->height;
-	u16 in_width = oi->width;
-	u16 out_width, out_height;
-	enum omap_channel channel;
+	u16 in_height = height;
+	u16 in_width = width;
 	int x_predecim = 1, y_predecim = 1;
 	bool ilace = mgr_timings->interlace;
 
-	channel = dispc_ovl_get_channel_out(plane);
-
-	DSSDBG("dispc_ovl_setup %d, pa %x, pa_uv %x, sw %d, %d,%d, %dx%d -> "
-		"%dx%d, cmode %x, rot %d, mir %d, ilace %d chan %d repl %d\n",
-		plane, oi->paddr, oi->p_uv_addr,
-		oi->screen_width, oi->pos_x, oi->pos_y, oi->width, oi->height,
-		oi->out_width, oi->out_height, oi->color_mode, oi->rotation,
-		oi->mirror, ilace, channel, replication);
-
-	if (oi->paddr == 0)
+	if (paddr == 0)
 		return -EINVAL;
 
-	out_width = oi->out_width == 0 ? oi->width : oi->out_width;
-	out_height = oi->out_height == 0 ? oi->height : oi->out_height;
+	out_width = out_width == 0 ? width : out_width;
+	out_height = out_height == 0 ? height : out_height;
 
-	if (ilace && oi->height == out_height)
+	if (ilace && height == out_height)
 		fieldmode = 1;
 
 	if (ilace) {
 		if (fieldmode)
 			in_height /= 2;
-		oi->pos_y /= 2;
+		pos_y /= 2;
 		out_height /= 2;
 
 		DSSDBG("adjusting for ilace: height %d, pos_y %d, "
-				"out_height %d\n",
-				in_height, oi->pos_y, out_height);
+			"out_height %d\n", in_height, pos_y,
+			out_height);
 	}
 
-	if (!dss_feat_color_mode_supported(plane, oi->color_mode))
+	if (!dss_feat_color_mode_supported(plane, color_mode))
 		return -EINVAL;
 
-	r = dispc_ovl_calc_scaling(plane, channel, mgr_timings, in_width,
-			in_height, out_width, out_height, oi->color_mode,
-			&five_taps, &x_predecim, &y_predecim, oi->pos_x);
+	r = dispc_ovl_calc_scaling(plane, caps, mgr_timings, in_width,
+			in_height, out_width, out_height, color_mode,
+			&five_taps, &x_predecim, &y_predecim, pos_x,
+			rotation_type, mem_to_mem);
 	if (r)
 		return r;
 
 	in_width = DIV_ROUND_UP(in_width, x_predecim);
 	in_height = DIV_ROUND_UP(in_height, y_predecim);
 
-	if (oi->color_mode == OMAP_DSS_COLOR_YUV2 ||
-			oi->color_mode == OMAP_DSS_COLOR_UYVY ||
-			oi->color_mode == OMAP_DSS_COLOR_NV12)
+	if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+			color_mode == OMAP_DSS_COLOR_UYVY ||
+			color_mode == OMAP_DSS_COLOR_NV12)
 		cconv = 1;
 
 	if (ilace && !fieldmode) {
@@ -2235,70 +2438,144 @@ int dispc_ovl_setup(enum omap_plane plane, struct omap_overlay_info *oi,
 	row_inc = 0;
 	pix_inc = 0;
 
-	if (oi->rotation_type == OMAP_DSS_ROT_TILER)
-		calc_tiler_rotation_offset(oi->screen_width, in_width,
-				oi->color_mode, fieldmode, field_offset,
+	if (rotation_type == OMAP_DSS_ROT_TILER)
+		calc_tiler_rotation_offset(screen_width, in_width,
+				color_mode, fieldmode, field_offset,
 				&offset0, &offset1, &row_inc, &pix_inc,
 				x_predecim, y_predecim);
-	else if (oi->rotation_type == OMAP_DSS_ROT_DMA)
-		calc_dma_rotation_offset(oi->rotation, oi->mirror,
-				oi->screen_width, in_width, frame_height,
-				oi->color_mode, fieldmode, field_offset,
+	else if (rotation_type == OMAP_DSS_ROT_DMA)
+		calc_dma_rotation_offset(rotation, mirror,
+				screen_width, in_width, frame_height,
+				color_mode, fieldmode, field_offset,
 				&offset0, &offset1, &row_inc, &pix_inc,
 				x_predecim, y_predecim);
 	else
-		calc_vrfb_rotation_offset(oi->rotation, oi->mirror,
-				oi->screen_width, in_width, frame_height,
-				oi->color_mode, fieldmode, field_offset,
+		calc_vrfb_rotation_offset(rotation, mirror,
+				screen_width, in_width, frame_height,
+				color_mode, fieldmode, field_offset,
 				&offset0, &offset1, &row_inc, &pix_inc,
 				x_predecim, y_predecim);
 
 	DSSDBG("offset0 %u, offset1 %u, row_inc %d, pix_inc %d\n",
 			offset0, offset1, row_inc, pix_inc);
 
-	dispc_ovl_set_color_mode(plane, oi->color_mode);
+	dispc_ovl_set_color_mode(plane, color_mode);
 
-	dispc_ovl_configure_burst_type(plane, oi->rotation_type);
+	dispc_ovl_configure_burst_type(plane, rotation_type);
 
-	dispc_ovl_set_ba0(plane, oi->paddr + offset0);
-	dispc_ovl_set_ba1(plane, oi->paddr + offset1);
+	dispc_ovl_set_ba0(plane, paddr + offset0);
+	dispc_ovl_set_ba1(plane, paddr + offset1);
 
-	if (OMAP_DSS_COLOR_NV12 == oi->color_mode) {
-		dispc_ovl_set_ba0_uv(plane, oi->p_uv_addr + offset0);
-		dispc_ovl_set_ba1_uv(plane, oi->p_uv_addr + offset1);
+	if (OMAP_DSS_COLOR_NV12 == color_mode) {
+		dispc_ovl_set_ba0_uv(plane, p_uv_addr + offset0);
+		dispc_ovl_set_ba1_uv(plane, p_uv_addr + offset1);
 	}
 
-
 	dispc_ovl_set_row_inc(plane, row_inc);
 	dispc_ovl_set_pix_inc(plane, pix_inc);
 
-	DSSDBG("%d,%d %dx%d -> %dx%d\n", oi->pos_x, oi->pos_y, in_width,
+	DSSDBG("%d,%d %dx%d -> %dx%d\n", pos_x, pos_y, in_width,
 			in_height, out_width, out_height);
 
-	dispc_ovl_set_pos(plane, oi->pos_x, oi->pos_y);
+	dispc_ovl_set_pos(plane, caps, pos_x, pos_y);
 
-	dispc_ovl_set_pic_size(plane, in_width, in_height);
+	dispc_ovl_set_input_size(plane, in_width, in_height);
 
-	if (ovl->caps & OMAP_DSS_OVL_CAP_SCALE) {
+	if (caps & OMAP_DSS_OVL_CAP_SCALE) {
 		dispc_ovl_set_scaling(plane, in_width, in_height, out_width,
 				   out_height, ilace, five_taps, fieldmode,
-				   oi->color_mode, oi->rotation);
-		dispc_ovl_set_vid_size(plane, out_width, out_height);
+				   color_mode, rotation);
+		dispc_ovl_set_output_size(plane, out_width, out_height);
 		dispc_ovl_set_vid_color_conv(plane, cconv);
 	}
 
-	dispc_ovl_set_rotation_attrs(plane, oi->rotation, oi->mirror,
-			oi->color_mode);
+	dispc_ovl_set_rotation_attrs(plane, rotation, mirror, color_mode);
 
-	dispc_ovl_set_zorder(plane, oi->zorder);
-	dispc_ovl_set_pre_mult_alpha(plane, oi->pre_mult_alpha);
-	dispc_ovl_setup_global_alpha(plane, oi->global_alpha);
+	dispc_ovl_set_zorder(plane, caps, zorder);
+	dispc_ovl_set_pre_mult_alpha(plane, caps, pre_mult_alpha);
+	dispc_ovl_setup_global_alpha(plane, caps, global_alpha);
 
-	dispc_ovl_enable_replication(plane, replication);
+	dispc_ovl_enable_replication(plane, caps, replication);
 
 	return 0;
 }
 
+int dispc_ovl_setup(enum omap_plane plane, const struct omap_overlay_info *oi,
+		bool replication, const struct omap_video_timings *mgr_timings,
+		bool mem_to_mem)
+{
+	int r;
+	struct omap_overlay *ovl = omap_dss_get_overlay(plane);
+	enum omap_channel channel;
+
+	channel = dispc_ovl_get_channel_out(plane);
+
+	DSSDBG("dispc_ovl_setup %d, pa %x, pa_uv %x, sw %d, %d,%d, %dx%d -> "
+		"%dx%d, cmode %x, rot %d, mir %d, chan %d repl %d\n",
+		plane, oi->paddr, oi->p_uv_addr, oi->screen_width, oi->pos_x,
+		oi->pos_y, oi->width, oi->height, oi->out_width, oi->out_height,
+		oi->color_mode, oi->rotation, oi->mirror, channel, replication);
+
+	r = dispc_ovl_setup_common(plane, ovl->caps, oi->paddr, oi->p_uv_addr,
+		oi->screen_width, oi->pos_x, oi->pos_y, oi->width, oi->height,
+		oi->out_width, oi->out_height, oi->color_mode, oi->rotation,
+		oi->mirror, oi->zorder, oi->pre_mult_alpha, oi->global_alpha,
+		oi->rotation_type, replication, mgr_timings, mem_to_mem);
+
+	return r;
+}
+
+int dispc_wb_setup(const struct omap_dss_writeback_info *wi,
+		bool mem_to_mem, const struct omap_video_timings *mgr_timings)
+{
+	int r;
+	u32 l;
+	enum omap_plane plane = OMAP_DSS_WB;
+	const int pos_x = 0, pos_y = 0;
+	const u8 zorder = 0, global_alpha = 0;
+	const bool replication = false;
+	bool truncation;
+	int in_width = mgr_timings->x_res;
+	int in_height = mgr_timings->y_res;
+	enum omap_overlay_caps caps =
+		OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA;
+
+	DSSDBG("dispc_wb_setup, pa %x, pa_uv %x, %d,%d -> %dx%d, cmode %x, "
+		"rot %d, mir %d\n", wi->paddr, wi->p_uv_addr, in_width,
+		in_height, wi->width, wi->height, wi->color_mode, wi->rotation,
+		wi->mirror);
+
+	r = dispc_ovl_setup_common(plane, caps, wi->paddr, wi->p_uv_addr,
+		wi->buf_width, pos_x, pos_y, in_width, in_height, wi->width,
+		wi->height, wi->color_mode, wi->rotation, wi->mirror, zorder,
+		wi->pre_mult_alpha, global_alpha, wi->rotation_type,
+		replication, mgr_timings, mem_to_mem);
+
+	switch (wi->color_mode) {
+	case OMAP_DSS_COLOR_RGB16:
+	case OMAP_DSS_COLOR_RGB24P:
+	case OMAP_DSS_COLOR_ARGB16:
+	case OMAP_DSS_COLOR_RGBA16:
+	case OMAP_DSS_COLOR_RGB12U:
+	case OMAP_DSS_COLOR_ARGB16_1555:
+	case OMAP_DSS_COLOR_XRGB16_1555:
+	case OMAP_DSS_COLOR_RGBX16:
+		truncation = true;
+		break;
+	default:
+		truncation = false;
+		break;
+	}
+
+	/* setup extra DISPC_WB_ATTRIBUTES */
+	l = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
+	l = FLD_MOD(l, truncation, 10, 10);	/* TRUNCATIONENABLE */
+	l = FLD_MOD(l, mem_to_mem, 19, 19);	/* WRITEBACKMODE */
+	dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), l);
+
+	return r;
+}
+
 int dispc_ovl_enable(enum omap_plane plane, bool enable)
 {
 	DSSDBG("dispc_enable_plane %d, %d\n", plane, enable);
@@ -2451,6 +2728,47 @@ void dispc_mgr_enable(enum omap_channel channel, bool enable)
 		BUG();
 }
 
+void dispc_wb_enable(bool enable)
+{
+	enum omap_plane plane = OMAP_DSS_WB;
+	struct completion frame_done_completion;
+	bool is_on;
+	int r;
+	u32 irq;
+
+	is_on = REG_GET(DISPC_OVL_ATTRIBUTES(plane), 0, 0);
+	irq = DISPC_IRQ_FRAMEDONEWB;
+
+	if (!enable && is_on) {
+		init_completion(&frame_done_completion);
+
+		r = omap_dispc_register_isr(dispc_disable_isr,
+				&frame_done_completion, irq);
+		if (r)
+			DSSERR("failed to register FRAMEDONEWB isr\n");
+	}
+
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable ? 1 : 0, 0, 0);
+
+	if (!enable && is_on) {
+		if (!wait_for_completion_timeout(&frame_done_completion,
+					msecs_to_jiffies(100)))
+			DSSERR("timeout waiting for FRAMEDONEWB\n");
+
+		r = omap_dispc_unregister_isr(dispc_disable_isr,
+				&frame_done_completion, irq);
+		if (r)
+			DSSERR("failed to unregister FRAMEDONEWB isr\n");
+	}
+}
+
+bool dispc_wb_is_enabled(void)
+{
+	enum omap_plane plane = OMAP_DSS_WB;
+
+	return REG_GET(DISPC_OVL_ATTRIBUTES(plane), 0, 0);
+}
+
 void dispc_lcd_enable_signal_polarity(bool act_high)
 {
 	if (!dss_has_feature(FEAT_LCDENABLEPOL))
@@ -2605,24 +2923,13 @@ static bool _dispc_mgr_size_ok(u16 width, u16 height)
 static bool _dispc_lcd_timings_ok(int hsw, int hfp, int hbp,
 		int vsw, int vfp, int vbp)
 {
-	if (cpu_is_omap24xx() || omap_rev() < OMAP3430_REV_ES3_0) {
-		if (hsw < 1 || hsw > 64 ||
-				hfp < 1 || hfp > 256 ||
-				hbp < 1 || hbp > 256 ||
-				vsw < 1 || vsw > 64 ||
-				vfp < 0 || vfp > 255 ||
-				vbp < 0 || vbp > 255)
-			return false;
-	} else {
-		if (hsw < 1 || hsw > 256 ||
-				hfp < 1 || hfp > 4096 ||
-				hbp < 1 || hbp > 4096 ||
-				vsw < 1 || vsw > 256 ||
-				vfp < 0 || vfp > 4095 ||
-				vbp < 0 || vbp > 4095)
-			return false;
-	}
-
+	if (hsw < 1 || hsw > dispc.feat->sw_max ||
+			hfp < 1 || hfp > dispc.feat->hp_max ||
+			hbp < 1 || hbp > dispc.feat->hp_max ||
+			vsw < 1 || vsw > dispc.feat->sw_max ||
+			vfp < 0 || vfp > dispc.feat->vp_max ||
+			vbp < 0 || vbp > dispc.feat->vp_max)
+		return false;
 	return true;
 }
 
@@ -2654,19 +2961,12 @@ static void _dispc_mgr_set_lcd_timings(enum omap_channel channel, int hsw,
 	u32 timing_h, timing_v, l;
 	bool onoff, rf, ipc;
 
-	if (cpu_is_omap24xx() || omap_rev() < OMAP3430_REV_ES3_0) {
-		timing_h = FLD_VAL(hsw-1, 5, 0) | FLD_VAL(hfp-1, 15, 8) |
-			FLD_VAL(hbp-1, 27, 20);
-
-		timing_v = FLD_VAL(vsw-1, 5, 0) | FLD_VAL(vfp, 15, 8) |
-			FLD_VAL(vbp, 27, 20);
-	} else {
-		timing_h = FLD_VAL(hsw-1, 7, 0) | FLD_VAL(hfp-1, 19, 8) |
-			FLD_VAL(hbp-1, 31, 20);
-
-		timing_v = FLD_VAL(vsw-1, 7, 0) | FLD_VAL(vfp, 19, 8) |
-			FLD_VAL(vbp, 31, 20);
-	}
+	timing_h = FLD_VAL(hsw-1, dispc.feat->sw_start, 0) |
+			FLD_VAL(hfp-1, dispc.feat->fp_start, 8) |
+			FLD_VAL(hbp-1, dispc.feat->bp_start, 20);
+	timing_v = FLD_VAL(vsw-1, dispc.feat->sw_start, 0) |
+			FLD_VAL(vfp, dispc.feat->fp_start, 8) |
+			FLD_VAL(vbp, dispc.feat->bp_start, 20);
 
 	dispc_write_reg(DISPC_TIMING_H(channel), timing_h);
 	dispc_write_reg(DISPC_TIMING_V(channel), timing_v);
@@ -2872,6 +3172,23 @@ unsigned long dispc_core_clk_rate(void)
 	return fclk / lcd;
 }
 
+static unsigned long dispc_plane_pclk_rate(enum omap_plane plane)
+{
+	enum omap_channel channel = dispc_ovl_get_channel_out(plane);
+
+	return dispc_mgr_pclk_rate(channel);
+}
+
+static unsigned long dispc_plane_lclk_rate(enum omap_plane plane)
+{
+	enum omap_channel channel = dispc_ovl_get_channel_out(plane);
+
+	if (dss_mgr_is_lcd(channel))
+		return dispc_mgr_lclk_rate(channel);
+	else
+		return dispc_fclk_rate();
+
+}
 static void dispc_dump_clocks_channel(struct seq_file *s, enum omap_channel channel)
 {
 	int lcd, pcd;
@@ -3492,7 +3809,7 @@ static void dispc_error_worker(struct work_struct *work)
 					ovl->name);
 			dispc_ovl_enable(ovl->id, false);
 			dispc_mgr_go(ovl->manager->id);
-			mdelay(50);
+			msleep(50);
 		}
 	}
 
@@ -3504,7 +3821,7 @@ static void dispc_error_worker(struct work_struct *work)
 		bit = mgr_desc[i].sync_lost_irq;
 
 		if (bit & errors) {
-			struct omap_dss_device *dssdev = mgr->device;
+			struct omap_dss_device *dssdev = mgr->get_device(mgr);
 			bool enable;
 
 			DSSERR("SYNC_LOST on channel %s, restarting the output "
@@ -3524,7 +3841,7 @@ static void dispc_error_worker(struct work_struct *work)
 			}
 
 			dispc_mgr_go(mgr->id);
-			mdelay(50);
+			msleep(50);
 
 			if (enable)
 				dssdev->driver->enable(dssdev);
@@ -3535,9 +3852,13 @@ static void dispc_error_worker(struct work_struct *work)
 		DSSERR("OCP_ERR\n");
 		for (i = 0; i < omap_dss_get_num_overlay_managers(); ++i) {
 			struct omap_overlay_manager *mgr;
+			struct omap_dss_device *dssdev;
+
 			mgr = omap_dss_get_overlay_manager(i);
-			if (mgr->device && mgr->device->driver)
-				mgr->device->driver->disable(mgr->device);
+			dssdev = mgr->get_device(mgr);
+
+			if (dssdev && dssdev->driver)
+				dssdev->driver->disable(dssdev);
 		}
 	}
 
@@ -3661,17 +3982,98 @@ static void _omap_dispc_initial_config(void)
 	if (dss_has_feature(FEAT_FUNCGATED))
 		REG_FLD_MOD(DISPC_CONFIG, 1, 9, 9);
 
-	_dispc_setup_color_conv_coef();
+	dispc_setup_color_conv_coef();
 
 	dispc_set_loadmode(OMAP_DSS_LOAD_FRAME_ONLY);
 
-	dispc_read_plane_fifo_sizes();
+	dispc_init_fifos();
 
 	dispc_configure_burst_sizes();
 
 	dispc_ovl_enable_zorder_planes();
 }
 
+static const struct dispc_features omap24xx_dispc_feats __initconst = {
+	.sw_start		=	5,
+	.fp_start		=	15,
+	.bp_start		=	27,
+	.sw_max			=	64,
+	.vp_max			=	255,
+	.hp_max			=	256,
+	.calc_scaling		=	dispc_ovl_calc_scaling_24xx,
+	.calc_core_clk		=	calc_core_clk_24xx,
+	.num_fifos		=	3,
+};
+
+static const struct dispc_features omap34xx_rev1_0_dispc_feats __initconst = {
+	.sw_start		=	5,
+	.fp_start		=	15,
+	.bp_start		=	27,
+	.sw_max			=	64,
+	.vp_max			=	255,
+	.hp_max			=	256,
+	.calc_scaling		=	dispc_ovl_calc_scaling_34xx,
+	.calc_core_clk		=	calc_core_clk_34xx,
+	.num_fifos		=	3,
+};
+
+static const struct dispc_features omap34xx_rev3_0_dispc_feats __initconst = {
+	.sw_start		=	7,
+	.fp_start		=	19,
+	.bp_start		=	31,
+	.sw_max			=	256,
+	.vp_max			=	4095,
+	.hp_max			=	4096,
+	.calc_scaling		=	dispc_ovl_calc_scaling_34xx,
+	.calc_core_clk		=	calc_core_clk_34xx,
+	.num_fifos		=	3,
+};
+
+static const struct dispc_features omap44xx_dispc_feats __initconst = {
+	.sw_start		=	7,
+	.fp_start		=	19,
+	.bp_start		=	31,
+	.sw_max			=	256,
+	.vp_max			=	4095,
+	.hp_max			=	4096,
+	.calc_scaling		=	dispc_ovl_calc_scaling_44xx,
+	.calc_core_clk		=	calc_core_clk_44xx,
+	.num_fifos		=	5,
+	.gfx_fifo_workaround	=	true,
+};
+
+static int __init dispc_init_features(struct device *dev)
+{
+	const struct dispc_features *src;
+	struct dispc_features *dst;
+
+	dst = devm_kzalloc(dev, sizeof(*dst), GFP_KERNEL);
+	if (!dst) {
+		dev_err(dev, "Failed to allocate DISPC Features\n");
+		return -ENOMEM;
+	}
+
+	if (cpu_is_omap24xx()) {
+		src = &omap24xx_dispc_feats;
+	} else if (cpu_is_omap34xx()) {
+		if (omap_rev() < OMAP3430_REV_ES3_0)
+			src = &omap34xx_rev1_0_dispc_feats;
+		else
+			src = &omap34xx_rev3_0_dispc_feats;
+	} else if (cpu_is_omap44xx()) {
+		src = &omap44xx_dispc_feats;
+	} else if (soc_is_omap54xx()) {
+		src = &omap44xx_dispc_feats;
+	} else {
+		return -ENODEV;
+	}
+
+	memcpy(dst, src, sizeof(*dst));
+	dispc.feat = dst;
+
+	return 0;
+}
+
 /* DISPC HW IP initialisation */
 static int __init omap_dispchw_probe(struct platform_device *pdev)
 {
@@ -3682,6 +4084,10 @@ static int __init omap_dispchw_probe(struct platform_device *pdev)
 
 	dispc.pdev = pdev;
 
+	r = dispc_init_features(&dispc.pdev->dev);
+	if (r)
+		return r;
+
 	spin_lock_init(&dispc.irq_lock);
 
 #ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
diff --git a/drivers/video/omap2/dss/dispc.h b/drivers/video/omap2/dss/dispc.h
index 92d8a9be86f..222363c6e62 100644
--- a/drivers/video/omap2/dss/dispc.h
+++ b/drivers/video/omap2/dss/dispc.h
@@ -36,6 +36,7 @@
 #define DISPC_CONTROL2			0x0238
 #define DISPC_CONFIG2			0x0620
 #define DISPC_DIVISOR			0x0804
+#define DISPC_GLOBAL_BUFFER		0x0800
 #define DISPC_CONTROL3                  0x0848
 #define DISPC_CONFIG3                   0x084C
 
@@ -355,6 +356,8 @@ static inline u16 DISPC_OVL_BASE(enum omap_plane plane)
 		return 0x014C;
 	case OMAP_DSS_VIDEO3:
 		return 0x0300;
+	case OMAP_DSS_WB:
+		return 0x0500;
 	default:
 		BUG();
 		return 0;
@@ -370,6 +373,7 @@ static inline u16 DISPC_BA0_OFFSET(enum omap_plane plane)
 	case OMAP_DSS_VIDEO2:
 		return 0x0000;
 	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
 		return 0x0008;
 	default:
 		BUG();
@@ -385,6 +389,7 @@ static inline u16 DISPC_BA1_OFFSET(enum omap_plane plane)
 	case OMAP_DSS_VIDEO2:
 		return 0x0004;
 	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
 		return 0x000C;
 	default:
 		BUG();
@@ -404,6 +409,8 @@ static inline u16 DISPC_BA0_UV_OFFSET(enum omap_plane plane)
 		return 0x04BC;
 	case OMAP_DSS_VIDEO3:
 		return 0x0310;
+	case OMAP_DSS_WB:
+		return 0x0118;
 	default:
 		BUG();
 		return 0;
@@ -422,6 +429,8 @@ static inline u16 DISPC_BA1_UV_OFFSET(enum omap_plane plane)
 		return 0x04C0;
 	case OMAP_DSS_VIDEO3:
 		return 0x0314;
+	case OMAP_DSS_WB:
+		return 0x011C;
 	default:
 		BUG();
 		return 0;
@@ -451,6 +460,7 @@ static inline u16 DISPC_SIZE_OFFSET(enum omap_plane plane)
 	case OMAP_DSS_VIDEO2:
 		return 0x000C;
 	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
 		return 0x00A8;
 	default:
 		BUG();
@@ -467,6 +477,7 @@ static inline u16 DISPC_ATTR_OFFSET(enum omap_plane plane)
 	case OMAP_DSS_VIDEO2:
 		return 0x0010;
 	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
 		return 0x0070;
 	default:
 		BUG();
@@ -486,6 +497,8 @@ static inline u16 DISPC_ATTR2_OFFSET(enum omap_plane plane)
 		return 0x04DC;
 	case OMAP_DSS_VIDEO3:
 		return 0x032C;
+	case OMAP_DSS_WB:
+		return 0x0310;
 	default:
 		BUG();
 		return 0;
@@ -501,6 +514,7 @@ static inline u16 DISPC_FIFO_THRESH_OFFSET(enum omap_plane plane)
 	case OMAP_DSS_VIDEO2:
 		return 0x0014;
 	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
 		return 0x008C;
 	default:
 		BUG();
@@ -517,6 +531,7 @@ static inline u16 DISPC_FIFO_SIZE_STATUS_OFFSET(enum omap_plane plane)
 	case OMAP_DSS_VIDEO2:
 		return 0x0018;
 	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
 		return 0x0088;
 	default:
 		BUG();
@@ -533,6 +548,7 @@ static inline u16 DISPC_ROW_INC_OFFSET(enum omap_plane plane)
 	case OMAP_DSS_VIDEO2:
 		return 0x001C;
 	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
 		return 0x00A4;
 	default:
 		BUG();
@@ -549,6 +565,7 @@ static inline u16 DISPC_PIX_INC_OFFSET(enum omap_plane plane)
 	case OMAP_DSS_VIDEO2:
 		return 0x0020;
 	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
 		return 0x0098;
 	default:
 		BUG();
@@ -598,6 +615,7 @@ static inline u16 DISPC_FIR_OFFSET(enum omap_plane plane)
 	case OMAP_DSS_VIDEO2:
 		return 0x0024;
 	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
 		return 0x0090;
 	default:
 		BUG();
@@ -617,6 +635,8 @@ static inline u16 DISPC_FIR2_OFFSET(enum omap_plane plane)
 		return 0x055C;
 	case OMAP_DSS_VIDEO3:
 		return 0x0424;
+	case OMAP_DSS_WB:
+		return 0x290;
 	default:
 		BUG();
 		return 0;
@@ -633,6 +653,7 @@ static inline u16 DISPC_PIC_SIZE_OFFSET(enum omap_plane plane)
 	case OMAP_DSS_VIDEO2:
 		return 0x0028;
 	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
 		return 0x0094;
 	default:
 		BUG();
@@ -651,6 +672,7 @@ static inline u16 DISPC_ACCU0_OFFSET(enum omap_plane plane)
 	case OMAP_DSS_VIDEO2:
 		return 0x002C;
 	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
 		return 0x0000;
 	default:
 		BUG();
@@ -670,6 +692,8 @@ static inline u16 DISPC_ACCU2_0_OFFSET(enum omap_plane plane)
 		return 0x0560;
 	case OMAP_DSS_VIDEO3:
 		return 0x0428;
+	case OMAP_DSS_WB:
+		return 0x0294;
 	default:
 		BUG();
 		return 0;
@@ -686,6 +710,7 @@ static inline u16 DISPC_ACCU1_OFFSET(enum omap_plane plane)
 	case OMAP_DSS_VIDEO2:
 		return 0x0030;
 	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
 		return 0x0004;
 	default:
 		BUG();
@@ -705,6 +730,8 @@ static inline u16 DISPC_ACCU2_1_OFFSET(enum omap_plane plane)
 		return 0x0564;
 	case OMAP_DSS_VIDEO3:
 		return 0x042C;
+	case OMAP_DSS_WB:
+		return 0x0298;
 	default:
 		BUG();
 		return 0;
@@ -722,6 +749,7 @@ static inline u16 DISPC_FIR_COEF_H_OFFSET(enum omap_plane plane, u16 i)
 	case OMAP_DSS_VIDEO2:
 		return 0x0034 + i * 0x8;
 	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
 		return 0x0010 + i * 0x8;
 	default:
 		BUG();
@@ -742,6 +770,8 @@ static inline u16 DISPC_FIR_COEF_H2_OFFSET(enum omap_plane plane, u16 i)
 		return 0x0568 + i * 0x8;
 	case OMAP_DSS_VIDEO3:
 		return 0x0430 + i * 0x8;
+	case OMAP_DSS_WB:
+		return 0x02A0 + i * 0x8;
 	default:
 		BUG();
 		return 0;
@@ -759,6 +789,7 @@ static inline u16 DISPC_FIR_COEF_HV_OFFSET(enum omap_plane plane, u16 i)
 	case OMAP_DSS_VIDEO2:
 		return 0x0038 + i * 0x8;
 	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
 		return 0x0014 + i * 0x8;
 	default:
 		BUG();
@@ -779,6 +810,8 @@ static inline u16 DISPC_FIR_COEF_HV2_OFFSET(enum omap_plane plane, u16 i)
 		return 0x056C + i * 0x8;
 	case OMAP_DSS_VIDEO3:
 		return 0x0434 + i * 0x8;
+	case OMAP_DSS_WB:
+		return 0x02A4 + i * 0x8;
 	default:
 		BUG();
 		return 0;
@@ -795,6 +828,7 @@ static inline u16 DISPC_CONV_COEF_OFFSET(enum omap_plane plane, u16 i)
 	case OMAP_DSS_VIDEO1:
 	case OMAP_DSS_VIDEO2:
 	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
 		return 0x0074 + i * 0x4;
 	default:
 		BUG();
@@ -814,6 +848,7 @@ static inline u16 DISPC_FIR_COEF_V_OFFSET(enum omap_plane plane, u16 i)
 	case OMAP_DSS_VIDEO2:
 		return 0x00B4 + i * 0x4;
 	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
 		return 0x0050 + i * 0x4;
 	default:
 		BUG();
@@ -834,6 +869,8 @@ static inline u16 DISPC_FIR_COEF_V2_OFFSET(enum omap_plane plane, u16 i)
 		return 0x05A8 + i * 0x4;
 	case OMAP_DSS_VIDEO3:
 		return 0x0470 + i * 0x4;
+	case OMAP_DSS_WB:
+		return 0x02E0 + i * 0x4;
 	default:
 		BUG();
 		return 0;
diff --git a/drivers/video/omap2/dss/display.c b/drivers/video/omap2/dss/display.c
index 5bd957e8550..ccf8550fafd 100644
--- a/drivers/video/omap2/dss/display.c
+++ b/drivers/video/omap2/dss/display.c
@@ -142,7 +142,11 @@ static ssize_t display_timings_store(struct device *dev,
 	if (r)
 		return r;
 
+	dssdev->driver->disable(dssdev);
 	dssdev->driver->set_timings(dssdev, &t);
+	r = dssdev->driver->enable(dssdev);
+	if (r)
+		return r;
 
 	return size;
 }
@@ -316,26 +320,117 @@ void omapdss_default_get_timings(struct omap_dss_device *dssdev,
 }
 EXPORT_SYMBOL(omapdss_default_get_timings);
 
-void dss_init_device(struct platform_device *pdev,
+/*
+ * Connect dssdev to a manager if the manager is free or if force is specified.
+ * Connect all overlays to that manager if they are free or if force is
+ * specified.
+ */
+static int dss_init_connections(struct omap_dss_device *dssdev, bool force)
+{
+	struct omap_dss_output *out;
+	struct omap_overlay_manager *mgr;
+	int i, r;
+
+	out = omapdss_get_output_from_dssdev(dssdev);
+
+	WARN_ON(dssdev->output);
+	WARN_ON(out->device);
+
+	r = omapdss_output_set_device(out, dssdev);
+	if (r) {
+		DSSERR("failed to connect output to new device\n");
+		return r;
+	}
+
+	mgr = omap_dss_get_overlay_manager(dssdev->channel);
+
+	if (mgr->output && !force)
+		return 0;
+
+	if (mgr->output)
+		mgr->unset_output(mgr);
+
+	r = mgr->set_output(mgr, out);
+	if (r) {
+		DSSERR("failed to connect manager to output of new device\n");
+
+		/* remove the output-device connection we just made */
+		omapdss_output_unset_device(out);
+		return r;
+	}
+
+	for (i = 0; i < omap_dss_get_num_overlays(); ++i) {
+		struct omap_overlay *ovl = omap_dss_get_overlay(i);
+
+		if (!ovl->manager || force) {
+			if (ovl->manager)
+				ovl->unset_manager(ovl);
+
+			r = ovl->set_manager(ovl, mgr);
+			if (r) {
+				DSSERR("failed to set initial overlay\n");
+				return r;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static void dss_uninit_connections(struct omap_dss_device *dssdev)
+{
+	if (dssdev->output) {
+		struct omap_overlay_manager *mgr = dssdev->output->manager;
+
+		if (mgr)
+			mgr->unset_output(mgr);
+
+		omapdss_output_unset_device(dssdev->output);
+	}
+}
+
+int dss_init_device(struct platform_device *pdev,
 		struct omap_dss_device *dssdev)
 {
 	struct device_attribute *attr;
-	int i;
-	int r;
+	int i, r;
+	const char *def_disp_name = dss_get_default_display_name();
+	bool force;
+
+	force = def_disp_name && strcmp(def_disp_name, dssdev->name) == 0;
+	dss_init_connections(dssdev, force);
 
 	/* create device sysfs files */
 	i = 0;
 	while ((attr = display_sysfs_attrs[i++]) != NULL) {
 		r = device_create_file(&dssdev->dev, attr);
-		if (r)
+		if (r) {
+			for (i = i - 2; i >= 0; i--) {
+				attr = display_sysfs_attrs[i];
+				device_remove_file(&dssdev->dev, attr);
+			}
+
+			dss_uninit_connections(dssdev);
+
 			DSSERR("failed to create sysfs file\n");
+			return r;
+		}
 	}
 
 	/* create display? sysfs links */
 	r = sysfs_create_link(&pdev->dev.kobj, &dssdev->dev.kobj,
 			dev_name(&dssdev->dev));
-	if (r)
+	if (r) {
+		while ((attr = display_sysfs_attrs[i++]) != NULL)
+			device_remove_file(&dssdev->dev, attr);
+
+		dss_uninit_connections(dssdev);
+
 		DSSERR("failed to create sysfs display link\n");
+		return r;
+	}
+
+	return 0;
 }
 
 void dss_uninit_device(struct platform_device *pdev,
@@ -349,8 +444,7 @@ void dss_uninit_device(struct platform_device *pdev,
 	while ((attr = display_sysfs_attrs[i++]) != NULL)
 		device_remove_file(&dssdev->dev, attr);
 
-	if (dssdev->manager)
-		dssdev->manager->unset_device(dssdev->manager);
+	dss_uninit_connections(dssdev);
 }
 
 static int dss_suspend_device(struct device *dev, void *data)
diff --git a/drivers/video/omap2/dss/dpi.c b/drivers/video/omap2/dss/dpi.c
index 3266be23fc0..56748cf8760 100644
--- a/drivers/video/omap2/dss/dpi.c
+++ b/drivers/video/omap2/dss/dpi.c
@@ -29,17 +29,24 @@
 #include <linux/errno.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/consumer.h>
+#include <linux/string.h>
 
 #include <video/omapdss.h>
-#include <plat/cpu.h>
 
 #include "dss.h"
+#include "dss_features.h"
 
 static struct {
 	struct regulator *vdds_dsi_reg;
 	struct platform_device *dsidev;
 
+	struct mutex lock;
+
+	struct omap_video_timings timings;
 	struct dss_lcd_mgr_config mgr_config;
+	int data_lines;
+
+	struct omap_dss_output output;
 } dpi;
 
 static struct platform_device *dpi_get_dsidev(enum omap_dss_clk_source clk)
@@ -121,7 +128,8 @@ static int dpi_set_dispc_clk(struct omap_dss_device *dssdev,
 
 static int dpi_set_mode(struct omap_dss_device *dssdev)
 {
-	struct omap_video_timings *t = &dssdev->panel.timings;
+	struct omap_video_timings *t = &dpi.timings;
+	struct omap_overlay_manager *mgr = dssdev->output->manager;
 	int lck_div = 0, pck_div = 0;
 	unsigned long fck = 0;
 	unsigned long pck;
@@ -146,37 +154,44 @@ static int dpi_set_mode(struct omap_dss_device *dssdev)
 		t->pixel_clock = pck;
 	}
 
-	dss_mgr_set_timings(dssdev->manager, t);
+	dss_mgr_set_timings(mgr, t);
 
 	return 0;
 }
 
 static void dpi_config_lcd_manager(struct omap_dss_device *dssdev)
 {
+	struct omap_overlay_manager *mgr = dssdev->output->manager;
+
 	dpi.mgr_config.io_pad_mode = DSS_IO_PAD_MODE_BYPASS;
 
 	dpi.mgr_config.stallmode = false;
 	dpi.mgr_config.fifohandcheck = false;
 
-	dpi.mgr_config.video_port_width = dssdev->phy.dpi.data_lines;
+	dpi.mgr_config.video_port_width = dpi.data_lines;
 
 	dpi.mgr_config.lcden_sig_polarity = 0;
 
-	dss_mgr_set_lcd_config(dssdev->manager, &dpi.mgr_config);
+	dss_mgr_set_lcd_config(mgr, &dpi.mgr_config);
 }
 
 int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
 {
+	struct omap_dss_output *out = dssdev->output;
 	int r;
 
-	if (cpu_is_omap34xx() && !dpi.vdds_dsi_reg) {
+	mutex_lock(&dpi.lock);
+
+	if (dss_has_feature(FEAT_DPI_USES_VDDS_DSI) && !dpi.vdds_dsi_reg) {
 		DSSERR("no VDSS_DSI regulator\n");
-		return -ENODEV;
+		r = -ENODEV;
+		goto err_no_reg;
 	}
 
-	if (dssdev->manager == NULL) {
-		DSSERR("failed to enable display: no manager\n");
-		return -ENODEV;
+	if (out == NULL || out->manager == NULL) {
+		DSSERR("failed to enable display: no output/manager\n");
+		r = -ENODEV;
+		goto err_no_out_mgr;
 	}
 
 	r = omap_dss_start_device(dssdev);
@@ -185,7 +200,7 @@ int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
 		goto err_start_dev;
 	}
 
-	if (cpu_is_omap34xx()) {
+	if (dss_has_feature(FEAT_DPI_USES_VDDS_DSI)) {
 		r = regulator_enable(dpi.vdds_dsi_reg);
 		if (r)
 			goto err_reg_enable;
@@ -195,6 +210,10 @@ int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
 	if (r)
 		goto err_get_dispc;
 
+	r = dss_dpi_select_source(dssdev->channel);
+	if (r)
+		goto err_src_sel;
+
 	if (dpi_use_dsi_pll(dssdev)) {
 		r = dsi_runtime_get(dpi.dsidev);
 		if (r)
@@ -213,10 +232,12 @@ int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
 
 	mdelay(2);
 
-	r = dss_mgr_enable(dssdev->manager);
+	r = dss_mgr_enable(out->manager);
 	if (r)
 		goto err_mgr_enable;
 
+	mutex_unlock(&dpi.lock);
+
 	return 0;
 
 err_mgr_enable:
@@ -227,20 +248,28 @@ err_dsi_pll_init:
 	if (dpi_use_dsi_pll(dssdev))
 		dsi_runtime_put(dpi.dsidev);
 err_get_dsi:
+err_src_sel:
 	dispc_runtime_put();
 err_get_dispc:
-	if (cpu_is_omap34xx())
+	if (dss_has_feature(FEAT_DPI_USES_VDDS_DSI))
 		regulator_disable(dpi.vdds_dsi_reg);
 err_reg_enable:
 	omap_dss_stop_device(dssdev);
 err_start_dev:
+err_no_out_mgr:
+err_no_reg:
+	mutex_unlock(&dpi.lock);
 	return r;
 }
 EXPORT_SYMBOL(omapdss_dpi_display_enable);
 
 void omapdss_dpi_display_disable(struct omap_dss_device *dssdev)
 {
-	dss_mgr_disable(dssdev->manager);
+	struct omap_overlay_manager *mgr = dssdev->output->manager;
+
+	mutex_lock(&dpi.lock);
+
+	dss_mgr_disable(mgr);
 
 	if (dpi_use_dsi_pll(dssdev)) {
 		dss_select_dispc_clk_source(OMAP_DSS_CLK_SRC_FCK);
@@ -250,44 +279,39 @@ void omapdss_dpi_display_disable(struct omap_dss_device *dssdev)
 
 	dispc_runtime_put();
 
-	if (cpu_is_omap34xx())
+	if (dss_has_feature(FEAT_DPI_USES_VDDS_DSI))
 		regulator_disable(dpi.vdds_dsi_reg);
 
 	omap_dss_stop_device(dssdev);
+
+	mutex_unlock(&dpi.lock);
 }
 EXPORT_SYMBOL(omapdss_dpi_display_disable);
 
-void dpi_set_timings(struct omap_dss_device *dssdev,
-			struct omap_video_timings *timings)
+void omapdss_dpi_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
 {
-	int r;
-
 	DSSDBG("dpi_set_timings\n");
-	dssdev->panel.timings = *timings;
-	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
-		r = dispc_runtime_get();
-		if (r)
-			return;
 
-		dpi_set_mode(dssdev);
+	mutex_lock(&dpi.lock);
 
-		dispc_runtime_put();
-	} else {
-		dss_mgr_set_timings(dssdev->manager, timings);
-	}
+	dpi.timings = *timings;
+
+	mutex_unlock(&dpi.lock);
 }
-EXPORT_SYMBOL(dpi_set_timings);
+EXPORT_SYMBOL(omapdss_dpi_set_timings);
 
 int dpi_check_timings(struct omap_dss_device *dssdev,
 			struct omap_video_timings *timings)
 {
 	int r;
+	struct omap_overlay_manager *mgr = dssdev->output->manager;
 	int lck_div, pck_div;
 	unsigned long fck;
 	unsigned long pck;
 	struct dispc_clock_info dispc_cinfo;
 
-	if (dss_mgr_check_timings(dssdev->manager, timings))
+	if (dss_mgr_check_timings(mgr, timings))
 		return -EINVAL;
 
 	if (timings->pixel_clock == 0)
@@ -325,11 +349,22 @@ int dpi_check_timings(struct omap_dss_device *dssdev,
 }
 EXPORT_SYMBOL(dpi_check_timings);
 
+void omapdss_dpi_set_data_lines(struct omap_dss_device *dssdev, int data_lines)
+{
+	mutex_lock(&dpi.lock);
+
+	dpi.data_lines = data_lines;
+
+	mutex_unlock(&dpi.lock);
+}
+EXPORT_SYMBOL(omapdss_dpi_set_data_lines);
+
 static int __init dpi_init_display(struct omap_dss_device *dssdev)
 {
 	DSSDBG("init_display\n");
 
-	if (cpu_is_omap34xx() && dpi.vdds_dsi_reg == NULL) {
+	if (dss_has_feature(FEAT_DPI_USES_VDDS_DSI) &&
+					dpi.vdds_dsi_reg == NULL) {
 		struct regulator *vdds_dsi;
 
 		vdds_dsi = dss_get_vdds_dsi();
@@ -351,10 +386,14 @@ static int __init dpi_init_display(struct omap_dss_device *dssdev)
 	return 0;
 }
 
-static void __init dpi_probe_pdata(struct platform_device *pdev)
+static struct omap_dss_device * __init dpi_find_dssdev(struct platform_device *pdev)
 {
 	struct omap_dss_board_info *pdata = pdev->dev.platform_data;
-	int i, r;
+	const char *def_disp_name = dss_get_default_display_name();
+	struct omap_dss_device *def_dssdev;
+	int i;
+
+	def_dssdev = NULL;
 
 	for (i = 0; i < pdata->num_devices; ++i) {
 		struct omap_dss_device *dssdev = pdata->devices[i];
@@ -362,21 +401,75 @@ static void __init dpi_probe_pdata(struct platform_device *pdev)
 		if (dssdev->type != OMAP_DISPLAY_TYPE_DPI)
 			continue;
 
-		r = dpi_init_display(dssdev);
-		if (r) {
-			DSSERR("device %s init failed: %d\n", dssdev->name, r);
-			continue;
+		if (def_dssdev == NULL)
+			def_dssdev = dssdev;
+
+		if (def_disp_name != NULL &&
+				strcmp(dssdev->name, def_disp_name) == 0) {
+			def_dssdev = dssdev;
+			break;
 		}
+	}
 
-		r = omap_dss_register_device(dssdev, &pdev->dev, i);
-		if (r)
-			DSSERR("device %s register failed: %d\n",
-					dssdev->name, r);
+	return def_dssdev;
+}
+
+static void __init dpi_probe_pdata(struct platform_device *dpidev)
+{
+	struct omap_dss_device *plat_dssdev;
+	struct omap_dss_device *dssdev;
+	int r;
+
+	plat_dssdev = dpi_find_dssdev(dpidev);
+
+	if (!plat_dssdev)
+		return;
+
+	dssdev = dss_alloc_and_init_device(&dpidev->dev);
+	if (!dssdev)
+		return;
+
+	dss_copy_device_pdata(dssdev, plat_dssdev);
+
+	r = dpi_init_display(dssdev);
+	if (r) {
+		DSSERR("device %s init failed: %d\n", dssdev->name, r);
+		dss_put_device(dssdev);
+		return;
+	}
+
+	r = dss_add_device(dssdev);
+	if (r) {
+		DSSERR("device %s register failed: %d\n", dssdev->name, r);
+		dss_put_device(dssdev);
+		return;
 	}
 }
 
+static void __init dpi_init_output(struct platform_device *pdev)
+{
+	struct omap_dss_output *out = &dpi.output;
+
+	out->pdev = pdev;
+	out->id = OMAP_DSS_OUTPUT_DPI;
+	out->type = OMAP_DISPLAY_TYPE_DPI;
+
+	dss_register_output(out);
+}
+
+static void __exit dpi_uninit_output(struct platform_device *pdev)
+{
+	struct omap_dss_output *out = &dpi.output;
+
+	dss_unregister_output(out);
+}
+
 static int __init omap_dpi_probe(struct platform_device *pdev)
 {
+	mutex_init(&dpi.lock);
+
+	dpi_init_output(pdev);
+
 	dpi_probe_pdata(pdev);
 
 	return 0;
@@ -384,7 +477,9 @@ static int __init omap_dpi_probe(struct platform_device *pdev)
 
 static int __exit omap_dpi_remove(struct platform_device *pdev)
 {
-	omap_dss_unregister_child_devices(&pdev->dev);
+	dss_unregister_child_devices(&pdev->dev);
+
+	dpi_uninit_output(pdev);
 
 	return 0;
 }
diff --git a/drivers/video/omap2/dss/dsi.c b/drivers/video/omap2/dss/dsi.c
index 05ee04667af..d64ac384288 100644
--- a/drivers/video/omap2/dss/dsi.c
+++ b/drivers/video/omap2/dss/dsi.c
@@ -41,7 +41,6 @@
 
 #include <video/omapdss.h>
 #include <video/mipi_display.h>
-#include <plat/clock.h>
 
 #include "dss.h"
 #include "dss_features.h"
@@ -333,6 +332,12 @@ struct dsi_data {
 	unsigned scp_clk_refcount;
 
 	struct dss_lcd_mgr_config mgr_config;
+	struct omap_video_timings timings;
+	enum omap_dss_dsi_pixel_format pix_fmt;
+	enum omap_dss_dsi_mode mode;
+	struct omap_dss_dsi_videomode_timings vm_timings;
+
+	struct omap_dss_output output;
 };
 
 struct dsi_packet_sent_handler_data {
@@ -340,8 +345,6 @@ struct dsi_packet_sent_handler_data {
 	struct completion *completion;
 };
 
-static struct platform_device *dsi_pdev_map[MAX_NUM_DSI];
-
 #ifdef DEBUG
 static bool dsi_perf;
 module_param(dsi_perf, bool, 0644);
@@ -354,12 +357,19 @@ static inline struct dsi_data *dsi_get_dsidrv_data(struct platform_device *dside
 
 static inline struct platform_device *dsi_get_dsidev_from_dssdev(struct omap_dss_device *dssdev)
 {
-	return dsi_pdev_map[dssdev->phy.dsi.module];
+	return dssdev->output->pdev;
 }
 
 struct platform_device *dsi_get_dsidev_from_id(int module)
 {
-	return dsi_pdev_map[module];
+	struct omap_dss_output *out;
+	enum omap_dss_output_id	id;
+
+	id = module == 0 ? OMAP_DSS_OUTPUT_DSI1 : OMAP_DSS_OUTPUT_DSI2;
+
+	out = omap_dss_get_output(id);
+
+	return out->pdev;
 }
 
 static inline void dsi_write_reg(struct platform_device *dsidev,
@@ -1450,6 +1460,148 @@ found:
 	return 0;
 }
 
+static int dsi_pll_calc_ddrfreq(struct platform_device *dsidev,
+		unsigned long req_clkin4ddr, struct dsi_clock_info *cinfo)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct dsi_clock_info cur, best;
+
+	DSSDBG("dsi_pll_calc_ddrfreq\n");
+
+	memset(&best, 0, sizeof(best));
+	memset(&cur, 0, sizeof(cur));
+
+	cur.clkin = clk_get_rate(dsi->sys_clk);
+
+	for (cur.regn = 1; cur.regn < dsi->regn_max; ++cur.regn) {
+		cur.fint = cur.clkin / cur.regn;
+
+		if (cur.fint > dsi->fint_max || cur.fint < dsi->fint_min)
+			continue;
+
+		/* DSIPHY(MHz) = (2 * regm / regn) * clkin */
+		for (cur.regm = 1; cur.regm < dsi->regm_max; ++cur.regm) {
+			unsigned long a, b;
+
+			a = 2 * cur.regm * (cur.clkin/1000);
+			b = cur.regn;
+			cur.clkin4ddr = a / b * 1000;
+
+			if (cur.clkin4ddr > 1800 * 1000 * 1000)
+				break;
+
+			if (abs(cur.clkin4ddr - req_clkin4ddr) <
+					abs(best.clkin4ddr - req_clkin4ddr)) {
+				best = cur;
+				DSSDBG("best %ld\n", best.clkin4ddr);
+			}
+
+			if (cur.clkin4ddr == req_clkin4ddr)
+				goto found;
+		}
+	}
+found:
+	if (cinfo)
+		*cinfo = best;
+
+	return 0;
+}
+
+static void dsi_pll_calc_dsi_fck(struct platform_device *dsidev,
+		struct dsi_clock_info *cinfo)
+{
+	unsigned long max_dsi_fck;
+
+	max_dsi_fck = dss_feat_get_param_max(FEAT_PARAM_DSI_FCK);
+
+	cinfo->regm_dsi = DIV_ROUND_UP(cinfo->clkin4ddr, max_dsi_fck);
+	cinfo->dsi_pll_hsdiv_dsi_clk = cinfo->clkin4ddr / cinfo->regm_dsi;
+}
+
+static int dsi_pll_calc_dispc_fck(struct platform_device *dsidev,
+		unsigned long req_pck, struct dsi_clock_info *cinfo,
+		struct dispc_clock_info *dispc_cinfo)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned regm_dispc, best_regm_dispc;
+	unsigned long dispc_clk, best_dispc_clk;
+	int min_fck_per_pck;
+	unsigned long max_dss_fck;
+	struct dispc_clock_info best_dispc;
+	bool match;
+
+	max_dss_fck = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
+
+	min_fck_per_pck = CONFIG_OMAP2_DSS_MIN_FCK_PER_PCK;
+
+	if (min_fck_per_pck &&
+			req_pck * min_fck_per_pck > max_dss_fck) {
+		DSSERR("Requested pixel clock not possible with the current "
+				"OMAP2_DSS_MIN_FCK_PER_PCK setting. Turning "
+				"the constraint off.\n");
+		min_fck_per_pck = 0;
+	}
+
+retry:
+	best_regm_dispc = 0;
+	best_dispc_clk = 0;
+	memset(&best_dispc, 0, sizeof(best_dispc));
+	match = false;
+
+	for (regm_dispc = 1; regm_dispc < dsi->regm_dispc_max; ++regm_dispc) {
+		struct dispc_clock_info cur_dispc;
+
+		dispc_clk = cinfo->clkin4ddr / regm_dispc;
+
+		/* this will narrow down the search a bit,
+		 * but still give pixclocks below what was
+		 * requested */
+		if (dispc_clk  < req_pck)
+			break;
+
+		if (dispc_clk > max_dss_fck)
+			continue;
+
+		if (min_fck_per_pck && dispc_clk < req_pck * min_fck_per_pck)
+			continue;
+
+		match = true;
+
+		dispc_find_clk_divs(req_pck, dispc_clk, &cur_dispc);
+
+		if (abs(cur_dispc.pck - req_pck) <
+				abs(best_dispc.pck - req_pck)) {
+			best_regm_dispc = regm_dispc;
+			best_dispc_clk = dispc_clk;
+			best_dispc = cur_dispc;
+
+			if (cur_dispc.pck == req_pck)
+				goto found;
+		}
+	}
+
+	if (!match) {
+		if (min_fck_per_pck) {
+			DSSERR("Could not find suitable clock settings.\n"
+					"Turning FCK/PCK constraint off and"
+					"trying again.\n");
+			min_fck_per_pck = 0;
+			goto retry;
+		}
+
+		DSSERR("Could not find suitable clock settings.\n");
+
+		return -EINVAL;
+	}
+found:
+	cinfo->regm_dispc = best_regm_dispc;
+	cinfo->dsi_pll_hsdiv_dispc_clk = best_dispc_clk;
+
+	*dispc_cinfo = best_dispc;
+
+	return 0;
+}
+
 int dsi_pll_set_clock_div(struct platform_device *dsidev,
 		struct dsi_clock_info *cinfo)
 {
@@ -1526,21 +1678,27 @@ int dsi_pll_set_clock_div(struct platform_device *dsidev,
 
 	BUG_ON(cinfo->fint < dsi->fint_min || cinfo->fint > dsi->fint_max);
 
+	l = dsi_read_reg(dsidev, DSI_PLL_CONFIGURATION2);
+
 	if (dss_has_feature(FEAT_DSI_PLL_FREQSEL)) {
 		f = cinfo->fint < 1000000 ? 0x3 :
 			cinfo->fint < 1250000 ? 0x4 :
 			cinfo->fint < 1500000 ? 0x5 :
 			cinfo->fint < 1750000 ? 0x6 :
 			0x7;
-	}
 
-	l = dsi_read_reg(dsidev, DSI_PLL_CONFIGURATION2);
-
-	if (dss_has_feature(FEAT_DSI_PLL_FREQSEL))
 		l = FLD_MOD(l, f, 4, 1);	/* DSI_PLL_FREQSEL */
+	} else if (dss_has_feature(FEAT_DSI_PLL_SELFREQDCO)) {
+		f = cinfo->clkin4ddr < 1000000000 ? 0x2 : 0x4;
+
+		l = FLD_MOD(l, f, 4, 1);	/* PLL_SELFREQDCO */
+	}
+
 	l = FLD_MOD(l, 1, 13, 13);		/* DSI_PLL_REFEN */
 	l = FLD_MOD(l, 0, 14, 14);		/* DSIPHY_CLKINEN */
 	l = FLD_MOD(l, 1, 20, 20);		/* DSI_HSDIVBYPASS */
+	if (dss_has_feature(FEAT_DSI_PLL_REFSEL))
+		l = FLD_MOD(l, 3, 22, 21);	/* REF_SYSCLK = sysclk */
 	dsi_write_reg(dsidev, DSI_PLL_CONFIGURATION2, l);
 
 	REG_FLD_MOD(dsidev, DSI_PLL_GO, 1, 0, 0);	/* DSI_PLL_GO */
@@ -2004,15 +2162,16 @@ static unsigned dsi_get_line_buf_size(struct platform_device *dsidev)
 		return 1194 * 3;	/* 1194x24 bits */
 	case 6:
 		return 1365 * 3;	/* 1365x24 bits */
+	case 7:
+		return 1920 * 3;	/* 1920x24 bits */
 	default:
 		BUG();
 		return 0;
 	}
 }
 
-static int dsi_set_lane_config(struct omap_dss_device *dssdev)
+static int dsi_set_lane_config(struct platform_device *dsidev)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	static const u8 offsets[] = { 0, 4, 8, 12, 16 };
 	static const enum dsi_lane_function functions[] = {
@@ -2136,9 +2295,16 @@ static void dsi_cio_timings(struct platform_device *dsidev)
 	dsi_write_reg(dsidev, DSI_DSIPHY_CFG0, r);
 
 	r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG1);
-	r = FLD_MOD(r, tlpx_half, 22, 16);
+	r = FLD_MOD(r, tlpx_half, 20, 16);
 	r = FLD_MOD(r, tclk_trail, 15, 8);
 	r = FLD_MOD(r, tclk_zero, 7, 0);
+
+	if (dss_has_feature(FEAT_DSI_PHY_DCC)) {
+		r = FLD_MOD(r, 0, 21, 21);	/* DCCEN = disable */
+		r = FLD_MOD(r, 1, 22, 22);	/* CLKINP_DIVBY2EN = enable */
+		r = FLD_MOD(r, 1, 23, 23);	/* CLKINP_SEL = enable */
+	}
+
 	dsi_write_reg(dsidev, DSI_DSIPHY_CFG1, r);
 
 	r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG2);
@@ -2147,10 +2313,9 @@ static void dsi_cio_timings(struct platform_device *dsidev)
 }
 
 /* lane masks have lane 0 at lsb. mask_p for positive lines, n for negative */
-static void dsi_cio_enable_lane_override(struct omap_dss_device *dssdev,
+static void dsi_cio_enable_lane_override(struct platform_device *dsidev,
 		unsigned mask_p, unsigned mask_n)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	int i;
 	u32 l;
@@ -2197,9 +2362,8 @@ static void dsi_cio_disable_lane_override(struct platform_device *dsidev)
 	REG_FLD_MOD(dsidev, DSI_DSIPHY_CFG10, 0, 22, 17);
 }
 
-static int dsi_cio_wait_tx_clk_esc_reset(struct omap_dss_device *dssdev)
+static int dsi_cio_wait_tx_clk_esc_reset(struct platform_device *dsidev)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	int t, i;
 	bool in_use[DSI_MAX_NR_LANES];
@@ -2247,9 +2411,8 @@ static int dsi_cio_wait_tx_clk_esc_reset(struct omap_dss_device *dssdev)
 }
 
 /* return bitmask of enabled lanes, lane0 being the lsb */
-static unsigned dsi_get_lane_mask(struct omap_dss_device *dssdev)
+static unsigned dsi_get_lane_mask(struct platform_device *dsidev)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	unsigned mask = 0;
 	int i;
@@ -2262,16 +2425,15 @@ static unsigned dsi_get_lane_mask(struct omap_dss_device *dssdev)
 	return mask;
 }
 
-static int dsi_cio_init(struct omap_dss_device *dssdev)
+static int dsi_cio_init(struct platform_device *dsidev)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	int r;
 	u32 l;
 
 	DSSDBGF();
 
-	r = dss_dsi_enable_pads(dsi->module_id, dsi_get_lane_mask(dssdev));
+	r = dss_dsi_enable_pads(dsi->module_id, dsi_get_lane_mask(dsidev));
 	if (r)
 		return r;
 
@@ -2288,7 +2450,7 @@ static int dsi_cio_init(struct omap_dss_device *dssdev)
 		goto err_scp_clk_dom;
 	}
 
-	r = dsi_set_lane_config(dssdev);
+	r = dsi_set_lane_config(dsidev);
 	if (r)
 		goto err_scp_clk_dom;
 
@@ -2323,7 +2485,7 @@ static int dsi_cio_init(struct omap_dss_device *dssdev)
 			mask_p |= 1 << i;
 		}
 
-		dsi_cio_enable_lane_override(dssdev, mask_p, 0);
+		dsi_cio_enable_lane_override(dsidev, mask_p, 0);
 	}
 
 	r = dsi_cio_power(dsidev, DSI_COMPLEXIO_POWER_ON);
@@ -2340,7 +2502,7 @@ static int dsi_cio_init(struct omap_dss_device *dssdev)
 	dsi_if_enable(dsidev, false);
 	REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 1, 20, 20); /* LP_CLK_ENABLE */
 
-	r = dsi_cio_wait_tx_clk_esc_reset(dssdev);
+	r = dsi_cio_wait_tx_clk_esc_reset(dsidev);
 	if (r)
 		goto err_tx_clk_esc_rst;
 
@@ -2360,10 +2522,10 @@ static int dsi_cio_init(struct omap_dss_device *dssdev)
 
 	dsi_cio_timings(dsidev);
 
-	if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_VIDEO_MODE) {
+	if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
 		/* DDR_CLK_ALWAYS_ON */
 		REG_FLD_MOD(dsidev, DSI_CLK_CTRL,
-			dssdev->panel.dsi_vm_data.ddr_clk_always_on, 13, 13);
+			dsi->vm_timings.ddr_clk_always_on, 13, 13);
 	}
 
 	dsi->ulps_enabled = false;
@@ -2381,13 +2543,12 @@ err_cio_pwr:
 		dsi_cio_disable_lane_override(dsidev);
 err_scp_clk_dom:
 	dsi_disable_scp_clk(dsidev);
-	dss_dsi_disable_pads(dsi->module_id, dsi_get_lane_mask(dssdev));
+	dss_dsi_disable_pads(dsi->module_id, dsi_get_lane_mask(dsidev));
 	return r;
 }
 
-static void dsi_cio_uninit(struct omap_dss_device *dssdev)
+static void dsi_cio_uninit(struct platform_device *dsidev)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 
 	/* DDR_CLK_ALWAYS_ON */
@@ -2395,7 +2556,7 @@ static void dsi_cio_uninit(struct omap_dss_device *dssdev)
 
 	dsi_cio_power(dsidev, DSI_COMPLEXIO_POWER_OFF);
 	dsi_disable_scp_clk(dsidev);
-	dss_dsi_disable_pads(dsi->module_id, dsi_get_lane_mask(dssdev));
+	dss_dsi_disable_pads(dsi->module_id, dsi_get_lane_mask(dsidev));
 }
 
 static void dsi_config_tx_fifo(struct platform_device *dsidev,
@@ -2685,6 +2846,7 @@ void omapdss_dsi_vc_enable_hs(struct omap_dss_device *dssdev, int channel,
 		bool enable)
 {
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 
 	DSSDBG("dsi_vc_enable_hs(%d, %d)\n", channel, enable);
 
@@ -2701,7 +2863,7 @@ void omapdss_dsi_vc_enable_hs(struct omap_dss_device *dssdev, int channel,
 	dsi_force_tx_stop_mode_io(dsidev);
 
 	/* start the DDR clock by sending a NULL packet */
-	if (dssdev->panel.dsi_vm_data.ddr_clk_always_on && enable)
+	if (dsi->vm_timings.ddr_clk_always_on && enable)
 		dsi_vc_send_null(dssdev, channel);
 }
 EXPORT_SYMBOL(omapdss_dsi_vc_enable_hs);
@@ -2987,10 +3149,9 @@ int dsi_vc_send_null(struct omap_dss_device *dssdev, int channel)
 }
 EXPORT_SYMBOL(dsi_vc_send_null);
 
-static int dsi_vc_write_nosync_common(struct omap_dss_device *dssdev,
+static int dsi_vc_write_nosync_common(struct platform_device *dsidev,
 		int channel, u8 *data, int len, enum dss_dsi_content_type type)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	int r;
 
 	if (len == 0) {
@@ -3021,7 +3182,9 @@ static int dsi_vc_write_nosync_common(struct omap_dss_device *dssdev,
 int dsi_vc_dcs_write_nosync(struct omap_dss_device *dssdev, int channel,
 		u8 *data, int len)
 {
-	return dsi_vc_write_nosync_common(dssdev, channel, data, len,
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+
+	return dsi_vc_write_nosync_common(dsidev, channel, data, len,
 			DSS_DSI_CONTENT_DCS);
 }
 EXPORT_SYMBOL(dsi_vc_dcs_write_nosync);
@@ -3029,7 +3192,9 @@ EXPORT_SYMBOL(dsi_vc_dcs_write_nosync);
 int dsi_vc_generic_write_nosync(struct omap_dss_device *dssdev, int channel,
 		u8 *data, int len)
 {
-	return dsi_vc_write_nosync_common(dssdev, channel, data, len,
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+
+	return dsi_vc_write_nosync_common(dsidev, channel, data, len,
 			DSS_DSI_CONTENT_GENERIC);
 }
 EXPORT_SYMBOL(dsi_vc_generic_write_nosync);
@@ -3040,7 +3205,7 @@ static int dsi_vc_write_common(struct omap_dss_device *dssdev, int channel,
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	int r;
 
-	r = dsi_vc_write_nosync_common(dssdev, channel, data, len, type);
+	r = dsi_vc_write_nosync_common(dsidev, channel, data, len, type);
 	if (r)
 		goto err;
 
@@ -3118,10 +3283,9 @@ int dsi_vc_generic_write_2(struct omap_dss_device *dssdev, int channel,
 }
 EXPORT_SYMBOL(dsi_vc_generic_write_2);
 
-static int dsi_vc_dcs_send_read_request(struct omap_dss_device *dssdev,
+static int dsi_vc_dcs_send_read_request(struct platform_device *dsidev,
 		int channel, u8 dcs_cmd)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	int r;
 
@@ -3139,10 +3303,9 @@ static int dsi_vc_dcs_send_read_request(struct omap_dss_device *dssdev,
 	return 0;
 }
 
-static int dsi_vc_generic_send_read_request(struct omap_dss_device *dssdev,
+static int dsi_vc_generic_send_read_request(struct platform_device *dsidev,
 		int channel, u8 *reqdata, int reqlen)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	u16 data;
 	u8 data_type;
@@ -3291,7 +3454,7 @@ int dsi_vc_dcs_read(struct omap_dss_device *dssdev, int channel, u8 dcs_cmd,
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	int r;
 
-	r = dsi_vc_dcs_send_read_request(dssdev, channel, dcs_cmd);
+	r = dsi_vc_dcs_send_read_request(dsidev, channel, dcs_cmd);
 	if (r)
 		goto err;
 
@@ -3322,7 +3485,7 @@ static int dsi_vc_generic_read(struct omap_dss_device *dssdev, int channel,
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	int r;
 
-	r = dsi_vc_generic_send_read_request(dssdev, channel, reqdata, reqlen);
+	r = dsi_vc_generic_send_read_request(dsidev, channel, reqdata, reqlen);
 	if (r)
 		return r;
 
@@ -3604,15 +3767,15 @@ static void dsi_set_hs_tx_timeout(struct platform_device *dsidev,
 			(total_ticks * 1000) / (fck / 1000 / 1000));
 }
 
-static void dsi_config_vp_num_line_buffers(struct omap_dss_device *dssdev)
+static void dsi_config_vp_num_line_buffers(struct platform_device *dsidev)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	int num_line_buffers;
 
-	if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_VIDEO_MODE) {
-		int bpp = dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt);
+	if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+		int bpp = dsi_get_pixel_size(dsi->pix_fmt);
 		unsigned line_buf_size = dsi_get_line_buf_size(dsidev);
-		struct omap_video_timings *timings = &dssdev->panel.timings;
+		struct omap_video_timings *timings = &dsi->timings;
 		/*
 		 * Don't use line buffers if width is greater than the video
 		 * port's line buffer size
@@ -3630,11 +3793,11 @@ static void dsi_config_vp_num_line_buffers(struct omap_dss_device *dssdev)
 	REG_FLD_MOD(dsidev, DSI_CTRL, num_line_buffers, 13, 12);
 }
 
-static void dsi_config_vp_sync_events(struct omap_dss_device *dssdev)
+static void dsi_config_vp_sync_events(struct platform_device *dsidev)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
-	bool vsync_end = dssdev->panel.dsi_vm_data.vp_vsync_end;
-	bool hsync_end = dssdev->panel.dsi_vm_data.vp_hsync_end;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	bool vsync_end = dsi->vm_timings.vp_vsync_end;
+	bool hsync_end = dsi->vm_timings.vp_hsync_end;
 	u32 r;
 
 	r = dsi_read_reg(dsidev, DSI_CTRL);
@@ -3648,13 +3811,13 @@ static void dsi_config_vp_sync_events(struct omap_dss_device *dssdev)
 	dsi_write_reg(dsidev, DSI_CTRL, r);
 }
 
-static void dsi_config_blanking_modes(struct omap_dss_device *dssdev)
+static void dsi_config_blanking_modes(struct platform_device *dsidev)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
-	int blanking_mode = dssdev->panel.dsi_vm_data.blanking_mode;
-	int hfp_blanking_mode = dssdev->panel.dsi_vm_data.hfp_blanking_mode;
-	int hbp_blanking_mode = dssdev->panel.dsi_vm_data.hbp_blanking_mode;
-	int hsa_blanking_mode = dssdev->panel.dsi_vm_data.hsa_blanking_mode;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int blanking_mode = dsi->vm_timings.blanking_mode;
+	int hfp_blanking_mode = dsi->vm_timings.hfp_blanking_mode;
+	int hbp_blanking_mode = dsi->vm_timings.hbp_blanking_mode;
+	int hsa_blanking_mode = dsi->vm_timings.hsa_blanking_mode;
 	u32 r;
 
 	/*
@@ -3741,8 +3904,8 @@ static void dsi_config_cmd_mode_interleaving(struct omap_dss_device *dssdev)
 	int ddr_clk_pre, ddr_clk_post, enter_hs_mode_lat, exit_hs_mode_lat;
 	int tclk_trail, ths_exit, exiths_clk;
 	bool ddr_alwon;
-	struct omap_video_timings *timings = &dssdev->panel.timings;
-	int bpp = dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt);
+	struct omap_video_timings *timings = &dsi->timings;
+	int bpp = dsi_get_pixel_size(dsi->pix_fmt);
 	int ndl = dsi->num_lanes_used - 1;
 	int dsi_fclk_hsdiv = dssdev->clocks.dsi.regm_dsi + 1;
 	int hsa_interleave_hs = 0, hsa_interleave_lp = 0;
@@ -3852,6 +4015,7 @@ static void dsi_config_cmd_mode_interleaving(struct omap_dss_device *dssdev)
 static int dsi_proto_config(struct omap_dss_device *dssdev)
 {
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	u32 r;
 	int buswidth = 0;
 
@@ -3871,7 +4035,7 @@ static int dsi_proto_config(struct omap_dss_device *dssdev)
 	dsi_set_lp_rx_timeout(dsidev, 0x1fff, true, true);
 	dsi_set_hs_tx_timeout(dsidev, 0x1fff, true, true);
 
-	switch (dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt)) {
+	switch (dsi_get_pixel_size(dsi->pix_fmt)) {
 	case 16:
 		buswidth = 0;
 		break;
@@ -3903,11 +4067,11 @@ static int dsi_proto_config(struct omap_dss_device *dssdev)
 
 	dsi_write_reg(dsidev, DSI_CTRL, r);
 
-	dsi_config_vp_num_line_buffers(dssdev);
+	dsi_config_vp_num_line_buffers(dsidev);
 
-	if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_VIDEO_MODE) {
-		dsi_config_vp_sync_events(dssdev);
-		dsi_config_blanking_modes(dssdev);
+	if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+		dsi_config_vp_sync_events(dsidev);
+		dsi_config_blanking_modes(dsidev);
 		dsi_config_cmd_mode_interleaving(dssdev);
 	}
 
@@ -3919,9 +4083,8 @@ static int dsi_proto_config(struct omap_dss_device *dssdev)
 	return 0;
 }
 
-static void dsi_proto_timings(struct omap_dss_device *dssdev)
+static void dsi_proto_timings(struct platform_device *dsidev)
 {
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	unsigned tlpx, tclk_zero, tclk_prepare, tclk_trail;
 	unsigned tclk_pre, tclk_post;
@@ -3941,7 +4104,7 @@ static void dsi_proto_timings(struct omap_dss_device *dssdev)
 	ths_exit = FLD_GET(r, 7, 0);
 
 	r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG1);
-	tlpx = FLD_GET(r, 22, 16) * 2;
+	tlpx = FLD_GET(r, 20, 16) * 2;
 	tclk_trail = FLD_GET(r, 15, 8);
 	tclk_zero = FLD_GET(r, 7, 0);
 
@@ -3984,18 +4147,18 @@ static void dsi_proto_timings(struct omap_dss_device *dssdev)
 	DSSDBG("enter_hs_mode_lat %u, exit_hs_mode_lat %u\n",
 			enter_hs_mode_lat, exit_hs_mode_lat);
 
-	 if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_VIDEO_MODE) {
+	 if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
 		/* TODO: Implement a video mode check_timings function */
-		int hsa = dssdev->panel.dsi_vm_data.hsa;
-		int hfp = dssdev->panel.dsi_vm_data.hfp;
-		int hbp = dssdev->panel.dsi_vm_data.hbp;
-		int vsa = dssdev->panel.dsi_vm_data.vsa;
-		int vfp = dssdev->panel.dsi_vm_data.vfp;
-		int vbp = dssdev->panel.dsi_vm_data.vbp;
-		int window_sync = dssdev->panel.dsi_vm_data.window_sync;
-		bool hsync_end = dssdev->panel.dsi_vm_data.vp_hsync_end;
-		struct omap_video_timings *timings = &dssdev->panel.timings;
-		int bpp = dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt);
+		int hsa = dsi->vm_timings.hsa;
+		int hfp = dsi->vm_timings.hfp;
+		int hbp = dsi->vm_timings.hbp;
+		int vsa = dsi->vm_timings.vsa;
+		int vfp = dsi->vm_timings.vfp;
+		int vbp = dsi->vm_timings.vbp;
+		int window_sync = dsi->vm_timings.window_sync;
+		bool hsync_end = dsi->vm_timings.vp_hsync_end;
+		struct omap_video_timings *timings = &dsi->timings;
+		int bpp = dsi_get_pixel_size(dsi->pix_fmt);
 		int tl, t_he, width_bytes;
 
 		t_he = hsync_end ?
@@ -4100,16 +4263,84 @@ int omapdss_dsi_configure_pins(struct omap_dss_device *dssdev,
 }
 EXPORT_SYMBOL(omapdss_dsi_configure_pins);
 
-int dsi_enable_video_output(struct omap_dss_device *dssdev, int channel)
+int omapdss_dsi_set_clocks(struct omap_dss_device *dssdev,
+		unsigned long ddr_clk, unsigned long lp_clk)
 {
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct dsi_clock_info cinfo;
+	struct dispc_clock_info dispc_cinfo;
+	unsigned lp_clk_div;
+	unsigned long dsi_fclk;
 	int bpp = dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt);
+	unsigned long pck;
+	int r;
+
+	DSSDBGF("ddr_clk %lu, lp_clk %lu", ddr_clk, lp_clk);
+
+	mutex_lock(&dsi->lock);
+
+	/* Calculate PLL output clock */
+	r = dsi_pll_calc_ddrfreq(dsidev, ddr_clk * 4, &cinfo);
+	if (r)
+		goto err;
+
+	/* Calculate PLL's DSI clock */
+	dsi_pll_calc_dsi_fck(dsidev, &cinfo);
+
+	/* Calculate PLL's DISPC clock and pck & lck divs */
+	pck = cinfo.clkin4ddr / 16 * (dsi->num_lanes_used - 1) * 8 / bpp;
+	DSSDBG("finding dispc dividers for pck %lu\n", pck);
+	r = dsi_pll_calc_dispc_fck(dsidev, pck, &cinfo, &dispc_cinfo);
+	if (r)
+		goto err;
+
+	/* Calculate LP clock */
+	dsi_fclk = cinfo.dsi_pll_hsdiv_dsi_clk;
+	lp_clk_div = DIV_ROUND_UP(dsi_fclk, lp_clk * 2);
+
+	dssdev->clocks.dsi.regn = cinfo.regn;
+	dssdev->clocks.dsi.regm = cinfo.regm;
+	dssdev->clocks.dsi.regm_dispc = cinfo.regm_dispc;
+	dssdev->clocks.dsi.regm_dsi = cinfo.regm_dsi;
+
+	dssdev->clocks.dsi.lp_clk_div = lp_clk_div;
+
+	dssdev->clocks.dispc.channel.lck_div = dispc_cinfo.lck_div;
+	dssdev->clocks.dispc.channel.pck_div = dispc_cinfo.pck_div;
+
+	dssdev->clocks.dispc.dispc_fclk_src = OMAP_DSS_CLK_SRC_FCK;
+
+	dssdev->clocks.dispc.channel.lcd_clk_src =
+		dsi->module_id == 0 ?
+		OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC :
+		OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC;
+
+	dssdev->clocks.dsi.dsi_fclk_src =
+		dsi->module_id == 0 ?
+		OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI :
+		OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI;
+
+	mutex_unlock(&dsi->lock);
+	return 0;
+err:
+	mutex_unlock(&dsi->lock);
+	return r;
+}
+EXPORT_SYMBOL(omapdss_dsi_set_clocks);
+
+int dsi_enable_video_output(struct omap_dss_device *dssdev, int channel)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct omap_overlay_manager *mgr = dssdev->output->manager;
+	int bpp = dsi_get_pixel_size(dsi->pix_fmt);
 	u8 data_type;
 	u16 word_count;
 	int r;
 
-	if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_VIDEO_MODE) {
-		switch (dssdev->panel.dsi_pix_fmt) {
+	if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+		switch (dsi->pix_fmt) {
 		case OMAP_DSS_DSI_FMT_RGB888:
 			data_type = MIPI_DSI_PACKED_PIXEL_STREAM_24;
 			break;
@@ -4133,7 +4364,7 @@ int dsi_enable_video_output(struct omap_dss_device *dssdev, int channel)
 		/* MODE, 1 = video mode */
 		REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), 1, 4, 4);
 
-		word_count = DIV_ROUND_UP(dssdev->panel.timings.x_res * bpp, 8);
+		word_count = DIV_ROUND_UP(dsi->timings.x_res * bpp, 8);
 
 		dsi_vc_write_long_header(dsidev, channel, data_type,
 				word_count, 0);
@@ -4142,9 +4373,9 @@ int dsi_enable_video_output(struct omap_dss_device *dssdev, int channel)
 		dsi_if_enable(dsidev, true);
 	}
 
-	r = dss_mgr_enable(dssdev->manager);
+	r = dss_mgr_enable(mgr);
 	if (r) {
-		if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_VIDEO_MODE) {
+		if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
 			dsi_if_enable(dsidev, false);
 			dsi_vc_enable(dsidev, channel, false);
 		}
@@ -4159,8 +4390,10 @@ EXPORT_SYMBOL(dsi_enable_video_output);
 void dsi_disable_video_output(struct omap_dss_device *dssdev, int channel)
 {
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct omap_overlay_manager *mgr = dssdev->output->manager;
 
-	if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_VIDEO_MODE) {
+	if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
 		dsi_if_enable(dsidev, false);
 		dsi_vc_enable(dsidev, channel, false);
 
@@ -4171,15 +4404,15 @@ void dsi_disable_video_output(struct omap_dss_device *dssdev, int channel)
 		dsi_if_enable(dsidev, true);
 	}
 
-	dss_mgr_disable(dssdev->manager);
+	dss_mgr_disable(mgr);
 }
 EXPORT_SYMBOL(dsi_disable_video_output);
 
-static void dsi_update_screen_dispc(struct omap_dss_device *dssdev,
-		u16 w, u16 h)
+static void dsi_update_screen_dispc(struct omap_dss_device *dssdev)
 {
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct omap_overlay_manager *mgr = dssdev->output->manager;
 	unsigned bytespp;
 	unsigned bytespl;
 	unsigned bytespf;
@@ -4190,12 +4423,14 @@ static void dsi_update_screen_dispc(struct omap_dss_device *dssdev,
 	int r;
 	const unsigned channel = dsi->update_channel;
 	const unsigned line_buf_size = dsi_get_line_buf_size(dsidev);
+	u16 w = dsi->timings.x_res;
+	u16 h = dsi->timings.y_res;
 
 	DSSDBG("dsi_update_screen_dispc(%dx%d)\n", w, h);
 
 	dsi_vc_config_source(dsidev, channel, DSI_VC_SOURCE_VP);
 
-	bytespp	= dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt) / 8;
+	bytespp	= dsi_get_pixel_size(dsi->pix_fmt) / 8;
 	bytespl = w * bytespp;
 	bytespf = bytespl * h;
 
@@ -4239,7 +4474,9 @@ static void dsi_update_screen_dispc(struct omap_dss_device *dssdev,
 		msecs_to_jiffies(250));
 	BUG_ON(r == 0);
 
-	dss_mgr_start_update(dssdev->manager);
+	dss_mgr_set_timings(mgr, &dsi->timings);
+
+	dss_mgr_start_update(mgr);
 
 	if (dsi->te_enabled) {
 		/* disable LP_RX_TO, so that we can receive TE.  Time to wait
@@ -4297,8 +4534,7 @@ static void dsi_framedone_timeout_work_callback(struct work_struct *work)
 
 static void dsi_framedone_irq_callback(void *data, u32 mask)
 {
-	struct omap_dss_device *dssdev = (struct omap_dss_device *) data;
-	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct platform_device *dsidev = (struct platform_device *) data;
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 
 	/* Note: We get FRAMEDONE when DISPC has finished sending pixels and
@@ -4325,13 +4561,14 @@ int omap_dsi_update(struct omap_dss_device *dssdev, int channel,
 	dsi->framedone_callback = callback;
 	dsi->framedone_data = data;
 
-	dssdev->driver->get_resolution(dssdev, &dw, &dh);
+	dw = dsi->timings.x_res;
+	dh = dsi->timings.y_res;
 
 #ifdef DEBUG
 	dsi->update_bytes = dw * dh *
-		dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt) / 8;
+		dsi_get_pixel_size(dsi->pix_fmt) / 8;
 #endif
-	dsi_update_screen_dispc(dssdev, dw, dh);
+	dsi_update_screen_dispc(dssdev);
 
 	return 0;
 }
@@ -4367,28 +4604,22 @@ static int dsi_display_init_dispc(struct omap_dss_device *dssdev)
 {
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
-	struct omap_video_timings timings;
+	struct omap_overlay_manager *mgr = dssdev->output->manager;
 	int r;
 	u32 irq = 0;
 
-	if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_CMD_MODE) {
-		u16 dw, dh;
-
-		dssdev->driver->get_resolution(dssdev, &dw, &dh);
-
-		timings.x_res = dw;
-		timings.y_res = dh;
-		timings.hsw = 1;
-		timings.hfp = 1;
-		timings.hbp = 1;
-		timings.vsw = 1;
-		timings.vfp = 0;
-		timings.vbp = 0;
+	if (dsi->mode == OMAP_DSS_DSI_CMD_MODE) {
+		dsi->timings.hsw = 1;
+		dsi->timings.hfp = 1;
+		dsi->timings.hbp = 1;
+		dsi->timings.vsw = 1;
+		dsi->timings.vfp = 0;
+		dsi->timings.vbp = 0;
 
-		irq = dispc_mgr_get_framedone_irq(dssdev->manager->id);
+		irq = dispc_mgr_get_framedone_irq(mgr->id);
 
 		r = omap_dispc_register_isr(dsi_framedone_irq_callback,
-			(void *) dssdev, irq);
+			(void *) dsidev, irq);
 		if (r) {
 			DSSERR("can't get FRAMEDONE irq\n");
 			goto err;
@@ -4397,8 +4628,6 @@ static int dsi_display_init_dispc(struct omap_dss_device *dssdev)
 		dsi->mgr_config.stallmode = true;
 		dsi->mgr_config.fifohandcheck = true;
 	} else {
-		timings = dssdev->panel.timings;
-
 		dsi->mgr_config.stallmode = false;
 		dsi->mgr_config.fifohandcheck = false;
 	}
@@ -4407,14 +4636,14 @@ static int dsi_display_init_dispc(struct omap_dss_device *dssdev)
 	 * override interlace, logic level and edge related parameters in
 	 * omap_video_timings with default values
 	 */
-	timings.interlace = false;
-	timings.hsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
-	timings.vsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
-	timings.data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
-	timings.de_level = OMAPDSS_SIG_ACTIVE_HIGH;
-	timings.sync_pclk_edge = OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES;
+	dsi->timings.interlace = false;
+	dsi->timings.hsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
+	dsi->timings.vsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
+	dsi->timings.data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+	dsi->timings.de_level = OMAPDSS_SIG_ACTIVE_HIGH;
+	dsi->timings.sync_pclk_edge = OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES;
 
-	dss_mgr_set_timings(dssdev->manager, &timings);
+	dss_mgr_set_timings(mgr, &dsi->timings);
 
 	r = dsi_configure_dispc_clocks(dssdev);
 	if (r)
@@ -4422,29 +4651,33 @@ static int dsi_display_init_dispc(struct omap_dss_device *dssdev)
 
 	dsi->mgr_config.io_pad_mode = DSS_IO_PAD_MODE_BYPASS;
 	dsi->mgr_config.video_port_width =
-			dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt);
+			dsi_get_pixel_size(dsi->pix_fmt);
 	dsi->mgr_config.lcden_sig_polarity = 0;
 
-	dss_mgr_set_lcd_config(dssdev->manager, &dsi->mgr_config);
+	dss_mgr_set_lcd_config(mgr, &dsi->mgr_config);
 
 	return 0;
 err1:
-	if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_CMD_MODE)
+	if (dsi->mode == OMAP_DSS_DSI_CMD_MODE)
 		omap_dispc_unregister_isr(dsi_framedone_irq_callback,
-			(void *) dssdev, irq);
+			(void *) dsidev, irq);
 err:
 	return r;
 }
 
 static void dsi_display_uninit_dispc(struct omap_dss_device *dssdev)
 {
-	if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_CMD_MODE) {
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct omap_overlay_manager *mgr = dssdev->output->manager;
+
+	if (dsi->mode == OMAP_DSS_DSI_CMD_MODE) {
 		u32 irq;
 
-		irq = dispc_mgr_get_framedone_irq(dssdev->manager->id);
+		irq = dispc_mgr_get_framedone_irq(mgr->id);
 
 		omap_dispc_unregister_isr(dsi_framedone_irq_callback,
-			(void *) dssdev, irq);
+			(void *) dsidev, irq);
 	}
 }
 
@@ -4477,6 +4710,7 @@ static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
 {
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct omap_overlay_manager *mgr = dssdev->output->manager;
 	int r;
 
 	r = dsi_pll_init(dsidev, true, true);
@@ -4489,18 +4723,18 @@ static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
 
 	dss_select_dispc_clk_source(dssdev->clocks.dispc.dispc_fclk_src);
 	dss_select_dsi_clk_source(dsi->module_id, dssdev->clocks.dsi.dsi_fclk_src);
-	dss_select_lcd_clk_source(dssdev->manager->id,
+	dss_select_lcd_clk_source(mgr->id,
 			dssdev->clocks.dispc.channel.lcd_clk_src);
 
 	DSSDBG("PLL OK\n");
 
-	r = dsi_cio_init(dssdev);
+	r = dsi_cio_init(dsidev);
 	if (r)
 		goto err2;
 
 	_dsi_print_reset_status(dsidev);
 
-	dsi_proto_timings(dssdev);
+	dsi_proto_timings(dsidev);
 	dsi_set_lp_clk_divisor(dssdev);
 
 	if (1)
@@ -4520,11 +4754,11 @@ static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
 
 	return 0;
 err3:
-	dsi_cio_uninit(dssdev);
+	dsi_cio_uninit(dsidev);
 err2:
 	dss_select_dispc_clk_source(OMAP_DSS_CLK_SRC_FCK);
 	dss_select_dsi_clk_source(dsi->module_id, OMAP_DSS_CLK_SRC_FCK);
-	dss_select_lcd_clk_source(dssdev->manager->id, OMAP_DSS_CLK_SRC_FCK);
+	dss_select_lcd_clk_source(mgr->id, OMAP_DSS_CLK_SRC_FCK);
 
 err1:
 	dsi_pll_uninit(dsidev, true);
@@ -4537,6 +4771,7 @@ static void dsi_display_uninit_dsi(struct omap_dss_device *dssdev,
 {
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct omap_overlay_manager *mgr = dssdev->output->manager;
 
 	if (enter_ulps && !dsi->ulps_enabled)
 		dsi_enter_ulps(dsidev);
@@ -4550,8 +4785,8 @@ static void dsi_display_uninit_dsi(struct omap_dss_device *dssdev,
 
 	dss_select_dispc_clk_source(OMAP_DSS_CLK_SRC_FCK);
 	dss_select_dsi_clk_source(dsi->module_id, OMAP_DSS_CLK_SRC_FCK);
-	dss_select_lcd_clk_source(dssdev->manager->id, OMAP_DSS_CLK_SRC_FCK);
-	dsi_cio_uninit(dssdev);
+	dss_select_lcd_clk_source(mgr->id, OMAP_DSS_CLK_SRC_FCK);
+	dsi_cio_uninit(dsidev);
 	dsi_pll_uninit(dsidev, disconnect_lanes);
 }
 
@@ -4559,6 +4794,7 @@ int omapdss_dsi_display_enable(struct omap_dss_device *dssdev)
 {
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct omap_dss_output *out = dssdev->output;
 	int r = 0;
 
 	DSSDBG("dsi_display_enable\n");
@@ -4567,8 +4803,8 @@ int omapdss_dsi_display_enable(struct omap_dss_device *dssdev)
 
 	mutex_lock(&dsi->lock);
 
-	if (dssdev->manager == NULL) {
-		DSSERR("failed to enable display: no manager\n");
+	if (out == NULL || out->manager == NULL) {
+		DSSERR("failed to enable display: no output/manager\n");
 		r = -ENODEV;
 		goto err_start_dev;
 	}
@@ -4653,17 +4889,83 @@ int omapdss_dsi_enable_te(struct omap_dss_device *dssdev, bool enable)
 }
 EXPORT_SYMBOL(omapdss_dsi_enable_te);
 
-static int __init dsi_init_display(struct omap_dss_device *dssdev)
+void omapdss_dsi_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
 {
 	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 
-	DSSDBG("DSI init\n");
+	mutex_lock(&dsi->lock);
 
-	if (dssdev->panel.dsi_mode == OMAP_DSS_DSI_CMD_MODE) {
-		dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE |
-			OMAP_DSS_DISPLAY_CAP_TEAR_ELIM;
-	}
+	dsi->timings = *timings;
+
+	mutex_unlock(&dsi->lock);
+}
+EXPORT_SYMBOL(omapdss_dsi_set_timings);
+
+void omapdss_dsi_set_size(struct omap_dss_device *dssdev, u16 w, u16 h)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	mutex_lock(&dsi->lock);
+
+	dsi->timings.x_res = w;
+	dsi->timings.y_res = h;
+
+	mutex_unlock(&dsi->lock);
+}
+EXPORT_SYMBOL(omapdss_dsi_set_size);
+
+void omapdss_dsi_set_pixel_format(struct omap_dss_device *dssdev,
+		enum omap_dss_dsi_pixel_format fmt)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	mutex_lock(&dsi->lock);
+
+	dsi->pix_fmt = fmt;
+
+	mutex_unlock(&dsi->lock);
+}
+EXPORT_SYMBOL(omapdss_dsi_set_pixel_format);
+
+void omapdss_dsi_set_operation_mode(struct omap_dss_device *dssdev,
+		enum omap_dss_dsi_mode mode)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	mutex_lock(&dsi->lock);
+
+	dsi->mode = mode;
+
+	mutex_unlock(&dsi->lock);
+}
+EXPORT_SYMBOL(omapdss_dsi_set_operation_mode);
+
+void omapdss_dsi_set_videomode_timings(struct omap_dss_device *dssdev,
+		struct omap_dss_dsi_videomode_timings *timings)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	mutex_lock(&dsi->lock);
+
+	dsi->vm_timings = *timings;
+
+	mutex_unlock(&dsi->lock);
+}
+EXPORT_SYMBOL(omapdss_dsi_set_videomode_timings);
+
+static int __init dsi_init_display(struct omap_dss_device *dssdev)
+{
+	struct platform_device *dsidev =
+			dsi_get_dsidev_from_id(dssdev->phy.dsi.module);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	DSSDBG("DSI init\n");
 
 	if (dsi->vdds_dsi_reg == NULL) {
 		struct regulator *vdds_dsi;
@@ -4806,11 +5108,15 @@ static void dsi_put_clocks(struct platform_device *dsidev)
 		clk_put(dsi->sys_clk);
 }
 
-static void __init dsi_probe_pdata(struct platform_device *dsidev)
+static struct omap_dss_device * __init dsi_find_dssdev(struct platform_device *pdev)
 {
-	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
-	struct omap_dss_board_info *pdata = dsidev->dev.platform_data;
-	int i, r;
+	struct omap_dss_board_info *pdata = pdev->dev.platform_data;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(pdev);
+	const char *def_disp_name = dss_get_default_display_name();
+	struct omap_dss_device *def_dssdev;
+	int i;
+
+	def_dssdev = NULL;
 
 	for (i = 0; i < pdata->num_devices; ++i) {
 		struct omap_dss_device *dssdev = pdata->devices[i];
@@ -4821,19 +5127,73 @@ static void __init dsi_probe_pdata(struct platform_device *dsidev)
 		if (dssdev->phy.dsi.module != dsi->module_id)
 			continue;
 
-		r = dsi_init_display(dssdev);
-		if (r) {
-			DSSERR("device %s init failed: %d\n", dssdev->name, r);
-			continue;
+		if (def_dssdev == NULL)
+			def_dssdev = dssdev;
+
+		if (def_disp_name != NULL &&
+				strcmp(dssdev->name, def_disp_name) == 0) {
+			def_dssdev = dssdev;
+			break;
 		}
+	}
 
-		r = omap_dss_register_device(dssdev, &dsidev->dev, i);
-		if (r)
-			DSSERR("device %s register failed: %d\n",
-					dssdev->name, r);
+	return def_dssdev;
+}
+
+static void __init dsi_probe_pdata(struct platform_device *dsidev)
+{
+	struct omap_dss_device *plat_dssdev;
+	struct omap_dss_device *dssdev;
+	int r;
+
+	plat_dssdev = dsi_find_dssdev(dsidev);
+
+	if (!plat_dssdev)
+		return;
+
+	dssdev = dss_alloc_and_init_device(&dsidev->dev);
+	if (!dssdev)
+		return;
+
+	dss_copy_device_pdata(dssdev, plat_dssdev);
+
+	r = dsi_init_display(dssdev);
+	if (r) {
+		DSSERR("device %s init failed: %d\n", dssdev->name, r);
+		dss_put_device(dssdev);
+		return;
+	}
+
+	r = dss_add_device(dssdev);
+	if (r) {
+		DSSERR("device %s register failed: %d\n", dssdev->name, r);
+		dss_put_device(dssdev);
+		return;
 	}
 }
 
+static void __init dsi_init_output(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct omap_dss_output *out = &dsi->output;
+
+	out->pdev = dsidev;
+	out->id = dsi->module_id == 0 ?
+			OMAP_DSS_OUTPUT_DSI1 : OMAP_DSS_OUTPUT_DSI2;
+
+	out->type = OMAP_DISPLAY_TYPE_DSI;
+
+	dss_register_output(out);
+}
+
+static void __exit dsi_uninit_output(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct omap_dss_output *out = &dsi->output;
+
+	dss_unregister_output(out);
+}
+
 /* DSI1 HW IP initialisation */
 static int __init omap_dsihw_probe(struct platform_device *dsidev)
 {
@@ -4848,7 +5208,6 @@ static int __init omap_dsihw_probe(struct platform_device *dsidev)
 
 	dsi->module_id = dsidev->id;
 	dsi->pdev = dsidev;
-	dsi_pdev_map[dsi->module_id] = dsidev;
 	dev_set_drvdata(&dsidev->dev, dsi);
 
 	spin_lock_init(&dsi->irq_lock);
@@ -4928,6 +5287,8 @@ static int __init omap_dsihw_probe(struct platform_device *dsidev)
 	else
 		dsi->num_lanes_supported = 3;
 
+	dsi_init_output(dsidev);
+
 	dsi_probe_pdata(dsidev);
 
 	dsi_runtime_put(dsidev);
@@ -4957,7 +5318,9 @@ static int __exit omap_dsihw_remove(struct platform_device *dsidev)
 
 	WARN_ON(dsi->scp_clk_refcount > 0);
 
-	omap_dss_unregister_child_devices(&dsidev->dev);
+	dss_unregister_child_devices(&dsidev->dev);
+
+	dsi_uninit_output(dsidev);
 
 	pm_runtime_disable(&dsidev->dev);
 
diff --git a/drivers/video/omap2/dss/dss.c b/drivers/video/omap2/dss/dss.c
index 04b4586113e..2ab1c3e9655 100644
--- a/drivers/video/omap2/dss/dss.c
+++ b/drivers/video/omap2/dss/dss.c
@@ -31,11 +31,11 @@
 #include <linux/clk.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
+#include <linux/gfp.h>
 
 #include <video/omapdss.h>
 
 #include <plat/cpu.h>
-#include <plat/clock.h>
 
 #include "dss.h"
 #include "dss_features.h"
@@ -65,6 +65,13 @@ struct dss_reg {
 static int dss_runtime_get(void);
 static void dss_runtime_put(void);
 
+struct dss_features {
+	u8 fck_div_max;
+	u8 dss_fck_multiplier;
+	const char *clk_name;
+	int (*dpi_select_source)(enum omap_channel channel);
+};
+
 static struct {
 	struct platform_device *pdev;
 	void __iomem    *base;
@@ -83,6 +90,8 @@ static struct {
 
 	bool		ctx_valid;
 	u32		ctx[DSS_SZ_REGS / sizeof(u32)];
+
+	const struct dss_features *feat;
 } dss;
 
 static const char * const dss_generic_clk_source_names[] = {
@@ -144,7 +153,7 @@ static void dss_restore_context(void)
 #undef SR
 #undef RR
 
-void dss_sdi_init(u8 datapairs)
+void dss_sdi_init(int datapairs)
 {
 	u32 l;
 
@@ -236,7 +245,6 @@ const char *dss_get_generic_clk_source_name(enum omap_dss_clk_source clk_src)
 	return dss_generic_clk_source_names[clk_src];
 }
 
-
 void dss_dump_clocks(struct seq_file *s)
 {
 	unsigned long dpll4_ck_rate;
@@ -259,18 +267,10 @@ void dss_dump_clocks(struct seq_file *s)
 
 		seq_printf(s, "dpll4_ck %lu\n", dpll4_ck_rate);
 
-		if (cpu_is_omap3630() || cpu_is_omap44xx())
-			seq_printf(s, "%s (%s) = %lu / %lu  = %lu\n",
-					fclk_name, fclk_real_name,
-					dpll4_ck_rate,
-					dpll4_ck_rate / dpll4_m4_ck_rate,
-					fclk_rate);
-		else
-			seq_printf(s, "%s (%s) = %lu / %lu * 2 = %lu\n",
-					fclk_name, fclk_real_name,
-					dpll4_ck_rate,
-					dpll4_ck_rate / dpll4_m4_ck_rate,
-					fclk_rate);
+		seq_printf(s, "%s (%s) = %lu / %lu * %d  = %lu\n",
+				fclk_name, fclk_real_name, dpll4_ck_rate,
+				dpll4_ck_rate / dpll4_m4_ck_rate,
+				dss.feat->dss_fck_multiplier, fclk_rate);
 	} else {
 		seq_printf(s, "%s (%s) = %lu\n",
 				fclk_name, fclk_real_name,
@@ -431,31 +431,6 @@ enum omap_dss_clk_source dss_get_lcd_clk_source(enum omap_channel channel)
 	}
 }
 
-/* calculate clock rates using dividers in cinfo */
-int dss_calc_clock_rates(struct dss_clock_info *cinfo)
-{
-	if (dss.dpll4_m4_ck) {
-		unsigned long prate;
-		u16 fck_div_max = 16;
-
-		if (cpu_is_omap3630() || cpu_is_omap44xx())
-			fck_div_max = 32;
-
-		if (cinfo->fck_div > fck_div_max || cinfo->fck_div == 0)
-			return -EINVAL;
-
-		prate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
-
-		cinfo->fck = prate / cinfo->fck_div;
-	} else {
-		if (cinfo->fck_div != 0)
-			return -EINVAL;
-		cinfo->fck = clk_get_rate(dss.dss_clk);
-	}
-
-	return 0;
-}
-
 int dss_set_clock_div(struct dss_clock_info *cinfo)
 {
 	if (dss.dpll4_m4_ck) {
@@ -478,26 +453,6 @@ int dss_set_clock_div(struct dss_clock_info *cinfo)
 	return 0;
 }
 
-int dss_get_clock_div(struct dss_clock_info *cinfo)
-{
-	cinfo->fck = clk_get_rate(dss.dss_clk);
-
-	if (dss.dpll4_m4_ck) {
-		unsigned long prate;
-
-		prate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
-
-		if (cpu_is_omap3630() || cpu_is_omap44xx())
-			cinfo->fck_div = prate / (cinfo->fck);
-		else
-			cinfo->fck_div = prate / (cinfo->fck / 2);
-	} else {
-		cinfo->fck_div = 0;
-	}
-
-	return 0;
-}
-
 unsigned long dss_get_dpll4_rate(void)
 {
 	if (dss.dpll4_m4_ck)
@@ -515,7 +470,7 @@ int dss_calc_clock_div(unsigned long req_pck, struct dss_clock_info *dss_cinfo,
 
 	unsigned long fck, max_dss_fck;
 
-	u16 fck_div, fck_div_max = 16;
+	u16 fck_div;
 
 	int match = 0;
 	int min_fck_per_pck;
@@ -525,9 +480,8 @@ int dss_calc_clock_div(unsigned long req_pck, struct dss_clock_info *dss_cinfo,
 	max_dss_fck = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
 
 	fck = clk_get_rate(dss.dss_clk);
-	if (req_pck == dss.cache_req_pck &&
-			((cpu_is_omap34xx() && prate == dss.cache_prate) ||
-			 dss.cache_dss_cinfo.fck == fck)) {
+	if (req_pck == dss.cache_req_pck && prate == dss.cache_prate &&
+		dss.cache_dss_cinfo.fck == fck) {
 		DSSDBG("dispc clock info found from cache.\n");
 		*dss_cinfo = dss.cache_dss_cinfo;
 		*dispc_cinfo = dss.cache_dispc_cinfo;
@@ -564,16 +518,10 @@ retry:
 
 		goto found;
 	} else {
-		if (cpu_is_omap3630() || cpu_is_omap44xx())
-			fck_div_max = 32;
-
-		for (fck_div = fck_div_max; fck_div > 0; --fck_div) {
+		for (fck_div = dss.feat->fck_div_max; fck_div > 0; --fck_div) {
 			struct dispc_clock_info cur_dispc;
 
-			if (fck_div_max == 32)
-				fck = prate / fck_div;
-			else
-				fck = prate / fck_div * 2;
+			fck = prate / fck_div * dss.feat->dss_fck_multiplier;
 
 			if (fck > max_dss_fck)
 				continue;
@@ -648,9 +596,18 @@ void dss_set_dac_pwrdn_bgz(bool enable)
 	REG_FLD_MOD(DSS_CONTROL, enable, 5, 5);	/* DAC Power-Down Control */
 }
 
-void dss_select_hdmi_venc_clk_source(enum dss_hdmi_venc_clk_source_select hdmi)
+void dss_select_hdmi_venc_clk_source(enum dss_hdmi_venc_clk_source_select src)
 {
-	REG_FLD_MOD(DSS_CONTROL, hdmi, 15, 15);	/* VENC_HDMI_SWITCH */
+	enum omap_display_type dp;
+	dp = dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_DIGIT);
+
+	/* Complain about invalid selections */
+	WARN_ON((src == DSS_VENC_TV_CLK) && !(dp & OMAP_DISPLAY_TYPE_VENC));
+	WARN_ON((src == DSS_HDMI_M_PCLK) && !(dp & OMAP_DISPLAY_TYPE_HDMI));
+
+	/* Select only if we have options */
+	if ((dp & OMAP_DISPLAY_TYPE_VENC) && (dp & OMAP_DISPLAY_TYPE_HDMI))
+		REG_FLD_MOD(DSS_CONTROL, src, 15, 15);	/* VENC_HDMI_SWITCH */
 }
 
 enum dss_hdmi_venc_clk_source_select dss_get_hdmi_venc_clk_source(void)
@@ -661,9 +618,71 @@ enum dss_hdmi_venc_clk_source_select dss_get_hdmi_venc_clk_source(void)
 	if ((displays & OMAP_DISPLAY_TYPE_HDMI) == 0)
 		return DSS_VENC_TV_CLK;
 
+	if ((displays & OMAP_DISPLAY_TYPE_VENC) == 0)
+		return DSS_HDMI_M_PCLK;
+
 	return REG_GET(DSS_CONTROL, 15, 15);
 }
 
+static int dss_dpi_select_source_omap2_omap3(enum omap_channel channel)
+{
+	if (channel != OMAP_DSS_CHANNEL_LCD)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int dss_dpi_select_source_omap4(enum omap_channel channel)
+{
+	int val;
+
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD2:
+		val = 0;
+		break;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		val = 1;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	REG_FLD_MOD(DSS_CONTROL, val, 17, 17);
+
+	return 0;
+}
+
+static int dss_dpi_select_source_omap5(enum omap_channel channel)
+{
+	int val;
+
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		val = 1;
+		break;
+	case OMAP_DSS_CHANNEL_LCD2:
+		val = 2;
+		break;
+	case OMAP_DSS_CHANNEL_LCD3:
+		val = 3;
+		break;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		val = 0;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	REG_FLD_MOD(DSS_CONTROL, val, 17, 16);
+
+	return 0;
+}
+
+int dss_dpi_select_source(enum omap_channel channel)
+{
+	return dss.feat->dpi_select_source(channel);
+}
+
 static int dss_get_clocks(void)
 {
 	struct clk *clk;
@@ -678,22 +697,11 @@ static int dss_get_clocks(void)
 
 	dss.dss_clk = clk;
 
-	if (cpu_is_omap34xx()) {
-		clk = clk_get(NULL, "dpll4_m4_ck");
-		if (IS_ERR(clk)) {
-			DSSERR("Failed to get dpll4_m4_ck\n");
-			r = PTR_ERR(clk);
-			goto err;
-		}
-	} else if (cpu_is_omap44xx()) {
-		clk = clk_get(NULL, "dpll_per_m5x2_ck");
-		if (IS_ERR(clk)) {
-			DSSERR("Failed to get dpll_per_m5x2_ck\n");
-			r = PTR_ERR(clk);
-			goto err;
-		}
-	} else { /* omap24xx */
-		clk = NULL;
+	clk = clk_get(NULL, dss.feat->clk_name);
+	if (IS_ERR(clk)) {
+		DSSERR("Failed to get %s\n", dss.feat->clk_name);
+		r = PTR_ERR(clk);
+		goto err;
 	}
 
 	dss.dpll4_m4_ck = clk;
@@ -749,6 +757,71 @@ void dss_debug_dump_clocks(struct seq_file *s)
 }
 #endif
 
+static const struct dss_features omap24xx_dss_feats __initconst = {
+	.fck_div_max		=	16,
+	.dss_fck_multiplier	=	2,
+	.clk_name		=	NULL,
+	.dpi_select_source	=	&dss_dpi_select_source_omap2_omap3,
+};
+
+static const struct dss_features omap34xx_dss_feats __initconst = {
+	.fck_div_max		=	16,
+	.dss_fck_multiplier	=	2,
+	.clk_name		=	"dpll4_m4_ck",
+	.dpi_select_source	=	&dss_dpi_select_source_omap2_omap3,
+};
+
+static const struct dss_features omap3630_dss_feats __initconst = {
+	.fck_div_max		=	32,
+	.dss_fck_multiplier	=	1,
+	.clk_name		=	"dpll4_m4_ck",
+	.dpi_select_source	=	&dss_dpi_select_source_omap2_omap3,
+};
+
+static const struct dss_features omap44xx_dss_feats __initconst = {
+	.fck_div_max		=	32,
+	.dss_fck_multiplier	=	1,
+	.clk_name		=	"dpll_per_m5x2_ck",
+	.dpi_select_source	=	&dss_dpi_select_source_omap4,
+};
+
+static const struct dss_features omap54xx_dss_feats __initconst = {
+	.fck_div_max		=	64,
+	.dss_fck_multiplier	=	1,
+	.clk_name		=	"dpll_per_h12x2_ck",
+	.dpi_select_source	=	&dss_dpi_select_source_omap5,
+};
+
+static int __init dss_init_features(struct device *dev)
+{
+	const struct dss_features *src;
+	struct dss_features *dst;
+
+	dst = devm_kzalloc(dev, sizeof(*dst), GFP_KERNEL);
+	if (!dst) {
+		dev_err(dev, "Failed to allocate local DSS Features\n");
+		return -ENOMEM;
+	}
+
+	if (cpu_is_omap24xx())
+		src = &omap24xx_dss_feats;
+	else if (cpu_is_omap34xx())
+		src = &omap34xx_dss_feats;
+	else if (cpu_is_omap3630())
+		src = &omap3630_dss_feats;
+	else if (cpu_is_omap44xx())
+		src = &omap44xx_dss_feats;
+	else if (soc_is_omap54xx())
+		src = &omap54xx_dss_feats;
+	else
+		return -ENODEV;
+
+	memcpy(dst, src, sizeof(*dst));
+	dss.feat = dst;
+
+	return 0;
+}
+
 /* DSS HW IP initialisation */
 static int __init omap_dsshw_probe(struct platform_device *pdev)
 {
@@ -758,6 +831,10 @@ static int __init omap_dsshw_probe(struct platform_device *pdev)
 
 	dss.pdev = pdev;
 
+	r = dss_init_features(&dss.pdev->dev);
+	if (r)
+		return r;
+
 	dss_mem = platform_get_resource(dss.pdev, IORESOURCE_MEM, 0);
 	if (!dss_mem) {
 		DSSERR("can't get IORESOURCE_MEM DSS\n");
diff --git a/drivers/video/omap2/dss/dss.h b/drivers/video/omap2/dss/dss.h
index f67afe76f21..6728892f9da 100644
--- a/drivers/video/omap2/dss/dss.h
+++ b/drivers/video/omap2/dss/dss.h
@@ -113,6 +113,17 @@ enum dss_dsi_content_type {
 	DSS_DSI_CONTENT_GENERIC,
 };
 
+enum dss_writeback_channel {
+	DSS_WB_LCD1_MGR =	0,
+	DSS_WB_LCD2_MGR =	1,
+	DSS_WB_TV_MGR =		2,
+	DSS_WB_OVL0 =		3,
+	DSS_WB_OVL1 =		4,
+	DSS_WB_OVL2 =		5,
+	DSS_WB_OVL3 =		6,
+	DSS_WB_LCD3_MGR =	7,
+};
+
 struct dss_clock_info {
 	/* rates that we get with dividers below */
 	unsigned long fck;
@@ -175,6 +186,7 @@ struct seq_file;
 struct platform_device;
 
 /* core */
+const char *dss_get_default_display_name(void);
 struct bus_type *dss_get_bus(void);
 struct regulator *dss_get_vdds_dsi(void);
 struct regulator *dss_get_vdds_sdi(void);
@@ -184,10 +196,13 @@ void dss_dsi_disable_pads(int dsi_id, unsigned lane_mask);
 int dss_set_min_bus_tput(struct device *dev, unsigned long tput);
 int dss_debugfs_create_file(const char *name, void (*write)(struct seq_file *));
 
-int omap_dss_register_device(struct omap_dss_device *dssdev,
-		struct device *parent, int disp_num);
-void omap_dss_unregister_device(struct omap_dss_device *dssdev);
-void omap_dss_unregister_child_devices(struct device *parent);
+struct omap_dss_device *dss_alloc_and_init_device(struct device *parent);
+int dss_add_device(struct omap_dss_device *dssdev);
+void dss_unregister_device(struct omap_dss_device *dssdev);
+void dss_unregister_child_devices(struct device *parent);
+void dss_put_device(struct omap_dss_device *dssdev);
+void dss_copy_device_pdata(struct omap_dss_device *dst,
+		const struct omap_dss_device *src);
 
 /* apply */
 void dss_apply_init(void);
@@ -205,8 +220,11 @@ void dss_mgr_get_info(struct omap_overlay_manager *mgr,
 int dss_mgr_set_device(struct omap_overlay_manager *mgr,
 		struct omap_dss_device *dssdev);
 int dss_mgr_unset_device(struct omap_overlay_manager *mgr);
+int dss_mgr_set_output(struct omap_overlay_manager *mgr,
+		struct omap_dss_output *output);
+int dss_mgr_unset_output(struct omap_overlay_manager *mgr);
 void dss_mgr_set_timings(struct omap_overlay_manager *mgr,
-		struct omap_video_timings *timings);
+		const struct omap_video_timings *timings);
 void dss_mgr_set_lcd_config(struct omap_overlay_manager *mgr,
 		const struct dss_lcd_mgr_config *config);
 const struct omap_video_timings *dss_mgr_get_timings(struct omap_overlay_manager *mgr);
@@ -222,12 +240,17 @@ int dss_ovl_set_manager(struct omap_overlay *ovl,
 		struct omap_overlay_manager *mgr);
 int dss_ovl_unset_manager(struct omap_overlay *ovl);
 
+/* output */
+void dss_register_output(struct omap_dss_output *out);
+void dss_unregister_output(struct omap_dss_output *out);
+struct omap_dss_output *omapdss_get_output_from_dssdev(struct omap_dss_device *dssdev);
+
 /* display */
 int dss_suspend_all_devices(void);
 int dss_resume_all_devices(void);
 void dss_disable_all_devices(void);
 
-void dss_init_device(struct platform_device *pdev,
+int dss_init_device(struct platform_device *pdev,
 		struct omap_dss_device *dssdev);
 void dss_uninit_device(struct platform_device *pdev,
 		struct omap_dss_device *dssdev);
@@ -254,22 +277,29 @@ static inline bool dss_mgr_is_lcd(enum omap_channel id)
 		return false;
 }
 
+int dss_manager_kobj_init(struct omap_overlay_manager *mgr,
+		struct platform_device *pdev);
+void dss_manager_kobj_uninit(struct omap_overlay_manager *mgr);
+
 /* overlay */
 void dss_init_overlays(struct platform_device *pdev);
 void dss_uninit_overlays(struct platform_device *pdev);
 void dss_overlay_setup_dispc_manager(struct omap_overlay_manager *mgr);
-void dss_recheck_connections(struct omap_dss_device *dssdev, bool force);
 int dss_ovl_simple_check(struct omap_overlay *ovl,
 		const struct omap_overlay_info *info);
 int dss_ovl_check(struct omap_overlay *ovl, struct omap_overlay_info *info,
 		const struct omap_video_timings *mgr_timings);
 bool dss_ovl_use_replication(struct dss_lcd_mgr_config config,
 		enum omap_color_mode mode);
+int dss_overlay_kobj_init(struct omap_overlay *ovl,
+		struct platform_device *pdev);
+void dss_overlay_kobj_uninit(struct omap_overlay *ovl);
 
 /* DSS */
 int dss_init_platform_driver(void) __init;
 void dss_uninit_platform_driver(void);
 
+int dss_dpi_select_source(enum omap_channel channel);
 void dss_select_hdmi_venc_clk_source(enum dss_hdmi_venc_clk_source_select);
 enum dss_hdmi_venc_clk_source_select dss_get_hdmi_venc_clk_source(void);
 const char *dss_get_generic_clk_source_name(enum omap_dss_clk_source clk_src);
@@ -279,7 +309,7 @@ void dss_dump_clocks(struct seq_file *s);
 void dss_debug_dump_clocks(struct seq_file *s);
 #endif
 
-void dss_sdi_init(u8 datapairs);
+void dss_sdi_init(int datapairs);
 int dss_sdi_enable(void);
 void dss_sdi_disable(void);
 
@@ -296,9 +326,7 @@ void dss_set_venc_output(enum omap_dss_venc_type type);
 void dss_set_dac_pwrdn_bgz(bool enable);
 
 unsigned long dss_get_dpll4_rate(void);
-int dss_calc_clock_rates(struct dss_clock_info *cinfo);
 int dss_set_clock_div(struct dss_clock_info *cinfo);
-int dss_get_clock_div(struct dss_clock_info *cinfo);
 int dss_calc_clock_div(unsigned long req_pck, struct dss_clock_info *dss_cinfo,
 		struct dispc_clock_info *dispc_cinfo);
 
@@ -427,8 +455,9 @@ void dispc_ovl_set_fifo_threshold(enum omap_plane plane, u32 low, u32 high);
 void dispc_ovl_compute_fifo_thresholds(enum omap_plane plane,
 		u32 *fifo_low, u32 *fifo_high, bool use_fifomerge,
 		bool manual_update);
-int dispc_ovl_setup(enum omap_plane plane, struct omap_overlay_info *oi,
-		bool replication, const struct omap_video_timings *mgr_timings);
+int dispc_ovl_setup(enum omap_plane plane, const struct omap_overlay_info *oi,
+		bool replication, const struct omap_video_timings *mgr_timings,
+		bool mem_to_mem);
 int dispc_ovl_enable(enum omap_plane plane, bool enable);
 void dispc_ovl_set_channel_out(enum omap_plane plane,
 		enum omap_channel channel);
@@ -457,6 +486,15 @@ int dispc_mgr_get_clock_div(enum omap_channel channel,
 void dispc_mgr_setup(enum omap_channel channel,
 		struct omap_overlay_manager_info *info);
 
+u32 dispc_wb_get_framedone_irq(void);
+bool dispc_wb_go_busy(void);
+void dispc_wb_go(void);
+void dispc_wb_enable(bool enable);
+bool dispc_wb_is_enabled(void);
+void dispc_wb_set_channel_in(enum dss_writeback_channel channel);
+int dispc_wb_setup(const struct omap_dss_writeback_info *wi,
+		bool mem_to_mem, const struct omap_video_timings *timings);
+
 /* VENC */
 #ifdef CONFIG_OMAP2_DSS_VENC
 int venc_init_platform_driver(void) __init;
@@ -469,6 +507,20 @@ static inline unsigned long venc_get_pixel_clock(void)
 	return 0;
 }
 #endif
+int omapdss_venc_display_enable(struct omap_dss_device *dssdev);
+void omapdss_venc_display_disable(struct omap_dss_device *dssdev);
+void omapdss_venc_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings);
+int omapdss_venc_check_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings);
+u32 omapdss_venc_get_wss(struct omap_dss_device *dssdev);
+int omapdss_venc_set_wss(struct omap_dss_device *dssdev, u32 wss);
+void omapdss_venc_set_type(struct omap_dss_device *dssdev,
+		enum omap_dss_venc_type type);
+void omapdss_venc_invert_vid_out_polarity(struct omap_dss_device *dssdev,
+		bool invert_polarity);
+int venc_panel_init(void);
+void venc_panel_exit(void);
 
 /* HDMI */
 #ifdef CONFIG_OMAP4_DSS_HDMI
@@ -484,7 +536,8 @@ static inline unsigned long hdmi_get_pixel_clock(void)
 #endif
 int omapdss_hdmi_display_enable(struct omap_dss_device *dssdev);
 void omapdss_hdmi_display_disable(struct omap_dss_device *dssdev);
-void omapdss_hdmi_display_set_timing(struct omap_dss_device *dssdev);
+void omapdss_hdmi_display_set_timing(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings);
 int omapdss_hdmi_display_check_timing(struct omap_dss_device *dssdev,
 					struct omap_video_timings *timings);
 int omapdss_hdmi_read_edid(u8 *buf, int len);
diff --git a/drivers/video/omap2/dss/dss_features.c b/drivers/video/omap2/dss/dss_features.c
index 938709724f0..acbc1e1efba 100644
--- a/drivers/video/omap2/dss/dss_features.c
+++ b/drivers/video/omap2/dss/dss_features.c
@@ -46,7 +46,9 @@ struct omap_dss_features {
 
 	const int num_mgrs;
 	const int num_ovls;
+	const int num_wbs;
 	const enum omap_display_type *supported_displays;
+	const enum omap_dss_output_id *supported_outputs;
 	const enum omap_color_mode *supported_color_modes;
 	const enum omap_overlay_caps *overlay_caps;
 	const char * const *clksrc_names;
@@ -106,6 +108,21 @@ static const struct dss_reg_field omap4_dss_reg_fields[] = {
 	[FEAT_REG_DSIPLL_REGM_DSI]		= { 30, 26 },
 };
 
+static const struct dss_reg_field omap5_dss_reg_fields[] = {
+	[FEAT_REG_FIRHINC]			= { 12, 0 },
+	[FEAT_REG_FIRVINC]			= { 28, 16 },
+	[FEAT_REG_FIFOLOWTHRESHOLD]		= { 15, 0 },
+	[FEAT_REG_FIFOHIGHTHRESHOLD]		= { 31, 16 },
+	[FEAT_REG_FIFOSIZE]			= { 15, 0 },
+	[FEAT_REG_HORIZONTALACCU]		= { 10, 0 },
+	[FEAT_REG_VERTICALACCU]			= { 26, 16 },
+	[FEAT_REG_DISPC_CLK_SWITCH]		= { 9, 7 },
+	[FEAT_REG_DSIPLL_REGN]			= { 8, 1 },
+	[FEAT_REG_DSIPLL_REGM]			= { 20, 9 },
+	[FEAT_REG_DSIPLL_REGM_DISPC]		= { 25, 21 },
+	[FEAT_REG_DSIPLL_REGM_DSI]		= { 30, 26 },
+};
+
 static const enum omap_display_type omap2_dss_supported_displays[] = {
 	/* OMAP_DSS_CHANNEL_LCD */
 	OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI,
@@ -144,6 +161,76 @@ static const enum omap_display_type omap4_dss_supported_displays[] = {
 	OMAP_DISPLAY_TYPE_DSI,
 };
 
+static const enum omap_display_type omap5_dss_supported_displays[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
+	OMAP_DISPLAY_TYPE_DSI,
+
+	/* OMAP_DSS_CHANNEL_DIGIT */
+	OMAP_DISPLAY_TYPE_HDMI | OMAP_DISPLAY_TYPE_DPI,
+
+	/* OMAP_DSS_CHANNEL_LCD2 */
+	OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
+	OMAP_DISPLAY_TYPE_DSI,
+};
+
+static const enum omap_dss_output_id omap2_dss_supported_outputs[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI,
+
+	/* OMAP_DSS_CHANNEL_DIGIT */
+	OMAP_DSS_OUTPUT_VENC,
+};
+
+static const enum omap_dss_output_id omap3430_dss_supported_outputs[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+	OMAP_DSS_OUTPUT_SDI | OMAP_DSS_OUTPUT_DSI1,
+
+	/* OMAP_DSS_CHANNEL_DIGIT */
+	OMAP_DSS_OUTPUT_VENC,
+};
+
+static const enum omap_dss_output_id omap3630_dss_supported_outputs[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+	OMAP_DSS_OUTPUT_DSI1,
+
+	/* OMAP_DSS_CHANNEL_DIGIT */
+	OMAP_DSS_OUTPUT_VENC,
+};
+
+static const enum omap_dss_output_id omap4_dss_supported_outputs[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+	OMAP_DSS_OUTPUT_DSI1,
+
+	/* OMAP_DSS_CHANNEL_DIGIT */
+	OMAP_DSS_OUTPUT_VENC | OMAP_DSS_OUTPUT_HDMI |
+	OMAP_DSS_OUTPUT_DPI,
+
+	/* OMAP_DSS_CHANNEL_LCD2 */
+	OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+	OMAP_DSS_OUTPUT_DSI2,
+};
+
+static const enum omap_dss_output_id omap5_dss_supported_outputs[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+	OMAP_DSS_OUTPUT_DSI1 | OMAP_DSS_OUTPUT_DSI2,
+
+	/* OMAP_DSS_CHANNEL_DIGIT */
+	OMAP_DSS_OUTPUT_HDMI | OMAP_DSS_OUTPUT_DPI,
+
+	/* OMAP_DSS_CHANNEL_LCD2 */
+	OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+	OMAP_DSS_OUTPUT_DSI1,
+
+	/* OMAP_DSS_CHANNEL_LCD3 */
+	OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+	OMAP_DSS_OUTPUT_DSI2,
+};
+
 static const enum omap_color_mode omap2_dss_supported_color_modes[] = {
 	/* OMAP_DSS_GFX */
 	OMAP_DSS_COLOR_CLUT1 | OMAP_DSS_COLOR_CLUT2 |
@@ -224,58 +311,80 @@ static const enum omap_color_mode omap4_dss_supported_color_modes[] = {
 	OMAP_DSS_COLOR_ARGB16 | OMAP_DSS_COLOR_XRGB16_1555 |
 	OMAP_DSS_COLOR_ARGB32 | OMAP_DSS_COLOR_RGBX16 |
 	OMAP_DSS_COLOR_RGBX32,
+
+	/* OMAP_DSS_WB */
+	OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB12U |
+	OMAP_DSS_COLOR_YUV2 | OMAP_DSS_COLOR_ARGB16_1555 |
+	OMAP_DSS_COLOR_RGBA32 | OMAP_DSS_COLOR_NV12 |
+	OMAP_DSS_COLOR_RGBA16 | OMAP_DSS_COLOR_RGB24U |
+	OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_UYVY |
+	OMAP_DSS_COLOR_ARGB16 | OMAP_DSS_COLOR_XRGB16_1555 |
+	OMAP_DSS_COLOR_ARGB32 | OMAP_DSS_COLOR_RGBX16 |
+	OMAP_DSS_COLOR_RGBX32,
 };
 
 static const enum omap_overlay_caps omap2_dss_overlay_caps[] = {
 	/* OMAP_DSS_GFX */
-	0,
+	OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
 
 	/* OMAP_DSS_VIDEO1 */
-	OMAP_DSS_OVL_CAP_SCALE,
+	OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_POS |
+		OMAP_DSS_OVL_CAP_REPLICATION,
 
 	/* OMAP_DSS_VIDEO2 */
-	OMAP_DSS_OVL_CAP_SCALE,
+	OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_POS |
+		OMAP_DSS_OVL_CAP_REPLICATION,
 };
 
 static const enum omap_overlay_caps omap3430_dss_overlay_caps[] = {
 	/* OMAP_DSS_GFX */
-	OMAP_DSS_OVL_CAP_GLOBAL_ALPHA,
+	OMAP_DSS_OVL_CAP_GLOBAL_ALPHA | OMAP_DSS_OVL_CAP_POS |
+		OMAP_DSS_OVL_CAP_REPLICATION,
 
 	/* OMAP_DSS_VIDEO1 */
-	OMAP_DSS_OVL_CAP_SCALE,
+	OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_POS |
+		OMAP_DSS_OVL_CAP_REPLICATION,
 
 	/* OMAP_DSS_VIDEO2 */
-	OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA,
+	OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
+		OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
 };
 
 static const enum omap_overlay_caps omap3630_dss_overlay_caps[] = {
 	/* OMAP_DSS_GFX */
-	OMAP_DSS_OVL_CAP_GLOBAL_ALPHA | OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA,
+	OMAP_DSS_OVL_CAP_GLOBAL_ALPHA | OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA |
+		OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
 
 	/* OMAP_DSS_VIDEO1 */
-	OMAP_DSS_OVL_CAP_SCALE,
+	OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_POS |
+		OMAP_DSS_OVL_CAP_REPLICATION,
 
 	/* OMAP_DSS_VIDEO2 */
 	OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
-		OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA,
+		OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_POS |
+		OMAP_DSS_OVL_CAP_REPLICATION,
 };
 
 static const enum omap_overlay_caps omap4_dss_overlay_caps[] = {
 	/* OMAP_DSS_GFX */
 	OMAP_DSS_OVL_CAP_GLOBAL_ALPHA | OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA |
-		OMAP_DSS_OVL_CAP_ZORDER,
+		OMAP_DSS_OVL_CAP_ZORDER | OMAP_DSS_OVL_CAP_POS |
+		OMAP_DSS_OVL_CAP_REPLICATION,
 
 	/* OMAP_DSS_VIDEO1 */
 	OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
-		OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_ZORDER,
+		OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_ZORDER |
+		OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
 
 	/* OMAP_DSS_VIDEO2 */
 	OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
-		OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_ZORDER,
+		OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_ZORDER |
+		OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
 
 	/* OMAP_DSS_VIDEO3 */
 	OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
-		OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_ZORDER,
+		OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_ZORDER |
+		OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
 };
 
 static const char * const omap2_dss_clk_source_names[] = {
@@ -298,6 +407,14 @@ static const char * const omap4_dss_clk_source_names[] = {
 	[OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI]	= "PLL2_CLK2",
 };
 
+static const char * const omap5_dss_clk_source_names[] = {
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]	= "DPLL_DSI1_A_CLK1",
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]	= "DPLL_DSI1_A_CLK2",
+	[OMAP_DSS_CLK_SRC_FCK]			= "DSS_CLK",
+	[OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC]	= "DPLL_DSI1_C_CLK1",
+	[OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI]	= "DPLL_DSI1_C_CLK2",
+};
+
 static const struct dss_param_range omap2_dss_param_range[] = {
 	[FEAT_PARAM_DSS_FCK]			= { 0, 173000000 },
 	[FEAT_PARAM_DSS_PCD]			= { 2, 255 },
@@ -326,6 +443,7 @@ static const struct dss_param_range omap3_dss_param_range[] = {
 	[FEAT_PARAM_DSIPLL_REGM_DSI]		= { 0, (1 << 4) - 1 },
 	[FEAT_PARAM_DSIPLL_FINT]		= { 750000, 2100000 },
 	[FEAT_PARAM_DSIPLL_LPDIV]		= { 1, (1 << 13) - 1},
+	[FEAT_PARAM_DSI_FCK]			= { 0, 173000000 },
 	[FEAT_PARAM_DOWNSCALE]			= { 1, 4 },
 	[FEAT_PARAM_LINEWIDTH]			= { 1, 1024 },
 	[FEAT_PARAM_MGR_WIDTH]			= { 1, 2048 },
@@ -341,6 +459,23 @@ static const struct dss_param_range omap4_dss_param_range[] = {
 	[FEAT_PARAM_DSIPLL_REGM_DSI]		= { 0, (1 << 5) - 1 },
 	[FEAT_PARAM_DSIPLL_FINT]		= { 500000, 2500000 },
 	[FEAT_PARAM_DSIPLL_LPDIV]		= { 0, (1 << 13) - 1 },
+	[FEAT_PARAM_DSI_FCK]			= { 0, 170000000 },
+	[FEAT_PARAM_DOWNSCALE]			= { 1, 4 },
+	[FEAT_PARAM_LINEWIDTH]			= { 1, 2048 },
+	[FEAT_PARAM_MGR_WIDTH]			= { 1, 2048 },
+	[FEAT_PARAM_MGR_HEIGHT]			= { 1, 2048 },
+};
+
+static const struct dss_param_range omap5_dss_param_range[] = {
+	[FEAT_PARAM_DSS_FCK]			= { 0, 200000000 },
+	[FEAT_PARAM_DSS_PCD]			= { 1, 255 },
+	[FEAT_PARAM_DSIPLL_REGN]		= { 0, (1 << 8) - 1 },
+	[FEAT_PARAM_DSIPLL_REGM]		= { 0, (1 << 12) - 1 },
+	[FEAT_PARAM_DSIPLL_REGM_DISPC]		= { 0, (1 << 5) - 1 },
+	[FEAT_PARAM_DSIPLL_REGM_DSI]		= { 0, (1 << 5) - 1 },
+	[FEAT_PARAM_DSIPLL_FINT]		= { 500000, 2500000 },
+	[FEAT_PARAM_DSIPLL_LPDIV]		= { 0, (1 << 13) - 1 },
+	[FEAT_PARAM_DSI_FCK]			= { 0, 170000000 },
 	[FEAT_PARAM_DOWNSCALE]			= { 1, 4 },
 	[FEAT_PARAM_LINEWIDTH]			= { 1, 2048 },
 	[FEAT_PARAM_MGR_WIDTH]			= { 1, 2048 },
@@ -373,6 +508,26 @@ static const enum dss_feat_id omap3430_dss_feat_list[] = {
 	FEAT_ALPHA_FIXED_ZORDER,
 	FEAT_FIFO_MERGE,
 	FEAT_OMAP3_DSI_FIFO_BUG,
+	FEAT_DPI_USES_VDDS_DSI,
+};
+
+static const enum dss_feat_id am35xx_dss_feat_list[] = {
+	FEAT_LCDENABLEPOL,
+	FEAT_LCDENABLESIGNAL,
+	FEAT_PCKFREEENABLE,
+	FEAT_FUNCGATED,
+	FEAT_LINEBUFFERSPLIT,
+	FEAT_ROWREPEATENABLE,
+	FEAT_RESIZECONF,
+	FEAT_DSI_PLL_FREQSEL,
+	FEAT_DSI_REVERSE_TXCLKESC,
+	FEAT_VENC_REQUIRES_TV_DAC_CLK,
+	FEAT_CPR,
+	FEAT_PRELOAD,
+	FEAT_FIR_COEF_V,
+	FEAT_ALPHA_FIXED_ZORDER,
+	FEAT_FIFO_MERGE,
+	FEAT_OMAP3_DSI_FIFO_BUG,
 };
 
 static const enum dss_feat_id omap3630_dss_feat_list[] = {
@@ -447,6 +602,28 @@ static const enum dss_feat_id omap4_dss_feat_list[] = {
 	FEAT_BURST_2D,
 };
 
+static const enum dss_feat_id omap5_dss_feat_list[] = {
+	FEAT_MGR_LCD2,
+	FEAT_CORE_CLK_DIV,
+	FEAT_LCD_CLK_SRC,
+	FEAT_DSI_DCS_CMD_CONFIG_VC,
+	FEAT_DSI_VC_OCP_WIDTH,
+	FEAT_DSI_GNQ,
+	FEAT_HDMI_CTS_SWMODE,
+	FEAT_HDMI_AUDIO_USE_MCLK,
+	FEAT_HANDLE_UV_SEPARATE,
+	FEAT_ATTR2,
+	FEAT_CPR,
+	FEAT_PRELOAD,
+	FEAT_FIR_COEF_V,
+	FEAT_ALPHA_FREE_ZORDER,
+	FEAT_FIFO_MERGE,
+	FEAT_BURST_2D,
+	FEAT_DSI_PLL_SELFREQDCO,
+	FEAT_DSI_PLL_REFSEL,
+	FEAT_DSI_PHY_DCC,
+};
+
 /* OMAP2 DSS Features */
 static const struct omap_dss_features omap2_dss_features = {
 	.reg_fields = omap2_dss_reg_fields,
@@ -458,6 +635,7 @@ static const struct omap_dss_features omap2_dss_features = {
 	.num_mgrs = 2,
 	.num_ovls = 3,
 	.supported_displays = omap2_dss_supported_displays,
+	.supported_outputs = omap2_dss_supported_outputs,
 	.supported_color_modes = omap2_dss_supported_color_modes,
 	.overlay_caps = omap2_dss_overlay_caps,
 	.clksrc_names = omap2_dss_clk_source_names,
@@ -478,6 +656,31 @@ static const struct omap_dss_features omap3430_dss_features = {
 	.num_mgrs = 2,
 	.num_ovls = 3,
 	.supported_displays = omap3430_dss_supported_displays,
+	.supported_outputs = omap3430_dss_supported_outputs,
+	.supported_color_modes = omap3_dss_supported_color_modes,
+	.overlay_caps = omap3430_dss_overlay_caps,
+	.clksrc_names = omap3_dss_clk_source_names,
+	.dss_params = omap3_dss_param_range,
+	.supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_VRFB,
+	.buffer_size_unit = 1,
+	.burst_size_unit = 8,
+};
+
+/*
+ * AM35xx DSS Features. This is basically OMAP3 DSS Features without the
+ * vdds_dsi regulator.
+ */
+static const struct omap_dss_features am35xx_dss_features = {
+	.reg_fields = omap3_dss_reg_fields,
+	.num_reg_fields = ARRAY_SIZE(omap3_dss_reg_fields),
+
+	.features = am35xx_dss_feat_list,
+	.num_features = ARRAY_SIZE(am35xx_dss_feat_list),
+
+	.num_mgrs = 2,
+	.num_ovls = 3,
+	.supported_displays = omap3430_dss_supported_displays,
+	.supported_outputs = omap3430_dss_supported_outputs,
 	.supported_color_modes = omap3_dss_supported_color_modes,
 	.overlay_caps = omap3430_dss_overlay_caps,
 	.clksrc_names = omap3_dss_clk_source_names,
@@ -497,6 +700,7 @@ static const struct omap_dss_features omap3630_dss_features = {
 	.num_mgrs = 2,
 	.num_ovls = 3,
 	.supported_displays = omap3630_dss_supported_displays,
+	.supported_outputs = omap3630_dss_supported_outputs,
 	.supported_color_modes = omap3_dss_supported_color_modes,
 	.overlay_caps = omap3630_dss_overlay_caps,
 	.clksrc_names = omap3_dss_clk_source_names,
@@ -517,7 +721,9 @@ static const struct omap_dss_features omap4430_es1_0_dss_features  = {
 
 	.num_mgrs = 3,
 	.num_ovls = 4,
+	.num_wbs = 1,
 	.supported_displays = omap4_dss_supported_displays,
+	.supported_outputs = omap4_dss_supported_outputs,
 	.supported_color_modes = omap4_dss_supported_color_modes,
 	.overlay_caps = omap4_dss_overlay_caps,
 	.clksrc_names = omap4_dss_clk_source_names,
@@ -537,7 +743,9 @@ static const struct omap_dss_features omap4430_es2_0_1_2_dss_features = {
 
 	.num_mgrs = 3,
 	.num_ovls = 4,
+	.num_wbs = 1,
 	.supported_displays = omap4_dss_supported_displays,
+	.supported_outputs = omap4_dss_supported_outputs,
 	.supported_color_modes = omap4_dss_supported_color_modes,
 	.overlay_caps = omap4_dss_overlay_caps,
 	.clksrc_names = omap4_dss_clk_source_names,
@@ -557,7 +765,9 @@ static const struct omap_dss_features omap4_dss_features = {
 
 	.num_mgrs = 3,
 	.num_ovls = 4,
+	.num_wbs = 1,
 	.supported_displays = omap4_dss_supported_displays,
+	.supported_outputs = omap4_dss_supported_outputs,
 	.supported_color_modes = omap4_dss_supported_color_modes,
 	.overlay_caps = omap4_dss_overlay_caps,
 	.clksrc_names = omap4_dss_clk_source_names,
@@ -567,6 +777,27 @@ static const struct omap_dss_features omap4_dss_features = {
 	.burst_size_unit = 16,
 };
 
+/* OMAP5 DSS Features */
+static const struct omap_dss_features omap5_dss_features = {
+	.reg_fields = omap5_dss_reg_fields,
+	.num_reg_fields = ARRAY_SIZE(omap5_dss_reg_fields),
+
+	.features = omap5_dss_feat_list,
+	.num_features = ARRAY_SIZE(omap5_dss_feat_list),
+
+	.num_mgrs = 3,
+	.num_ovls = 4,
+	.supported_displays = omap5_dss_supported_displays,
+	.supported_outputs = omap5_dss_supported_outputs,
+	.supported_color_modes = omap4_dss_supported_color_modes,
+	.overlay_caps = omap4_dss_overlay_caps,
+	.clksrc_names = omap5_dss_clk_source_names,
+	.dss_params = omap5_dss_param_range,
+	.supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_TILER,
+	.buffer_size_unit = 16,
+	.burst_size_unit = 16,
+};
+
 #if defined(CONFIG_OMAP4_DSS_HDMI)
 /* HDMI OMAP4 Functions*/
 static const struct ti_hdmi_ip_ops omap4_hdmi_functions = {
@@ -612,6 +843,11 @@ int dss_feat_get_num_ovls(void)
 	return omap_current_dss_features->num_ovls;
 }
 
+int dss_feat_get_num_wbs(void)
+{
+	return omap_current_dss_features->num_wbs;
+}
+
 unsigned long dss_feat_get_param_min(enum dss_range_param param)
 {
 	return omap_current_dss_features->dss_params[param].min;
@@ -627,6 +863,11 @@ enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel
 	return omap_current_dss_features->supported_displays[channel];
 }
 
+enum omap_dss_output_id dss_feat_get_supported_outputs(enum omap_channel channel)
+{
+	return omap_current_dss_features->supported_outputs[channel];
+}
+
 enum omap_color_mode dss_feat_get_supported_color_modes(enum omap_plane plane)
 {
 	return omap_current_dss_features->supported_color_modes[plane];
@@ -694,8 +935,13 @@ void dss_features_init(void)
 		omap_current_dss_features = &omap2_dss_features;
 	else if (cpu_is_omap3630())
 		omap_current_dss_features = &omap3630_dss_features;
-	else if (cpu_is_omap34xx())
-		omap_current_dss_features = &omap3430_dss_features;
+	else if (cpu_is_omap34xx()) {
+		if (soc_is_am35xx()) {
+			omap_current_dss_features = &am35xx_dss_features;
+		} else {
+			omap_current_dss_features = &omap3430_dss_features;
+		}
+	}
 	else if (omap_rev() == OMAP4430_REV_ES1_0)
 		omap_current_dss_features = &omap4430_es1_0_dss_features;
 	else if (omap_rev() == OMAP4430_REV_ES2_0 ||
@@ -704,6 +950,8 @@ void dss_features_init(void)
 		omap_current_dss_features = &omap4430_es2_0_1_2_dss_features;
 	else if (cpu_is_omap44xx())
 		omap_current_dss_features = &omap4_dss_features;
+	else if (soc_is_omap54xx())
+		omap_current_dss_features = &omap5_dss_features;
 	else
 		DSSWARN("Unsupported OMAP version");
 }
diff --git a/drivers/video/omap2/dss/dss_features.h b/drivers/video/omap2/dss/dss_features.h
index 996ffcbfed5..9218113b5e8 100644
--- a/drivers/video/omap2/dss/dss_features.h
+++ b/drivers/video/omap2/dss/dss_features.h
@@ -50,6 +50,7 @@ enum dss_feat_id {
 	FEAT_DSI_VC_OCP_WIDTH,
 	FEAT_DSI_REVERSE_TXCLKESC,
 	FEAT_DSI_GNQ,
+	FEAT_DPI_USES_VDDS_DSI,
 	FEAT_HDMI_CTS_SWMODE,
 	FEAT_HDMI_AUDIO_USE_MCLK,
 	FEAT_HANDLE_UV_SEPARATE,
@@ -64,6 +65,9 @@ enum dss_feat_id {
 	/* An unknown HW bug causing the normal FIFO thresholds not to work */
 	FEAT_OMAP3_DSI_FIFO_BUG,
 	FEAT_BURST_2D,
+	FEAT_DSI_PLL_SELFREQDCO,
+	FEAT_DSI_PLL_REFSEL,
+	FEAT_DSI_PHY_DCC,
 };
 
 /* DSS register field id */
@@ -91,6 +95,7 @@ enum dss_range_param {
 	FEAT_PARAM_DSIPLL_REGM_DSI,
 	FEAT_PARAM_DSIPLL_FINT,
 	FEAT_PARAM_DSIPLL_LPDIV,
+	FEAT_PARAM_DSI_FCK,
 	FEAT_PARAM_DOWNSCALE,
 	FEAT_PARAM_LINEWIDTH,
 	FEAT_PARAM_MGR_WIDTH,
@@ -100,9 +105,11 @@ enum dss_range_param {
 /* DSS Feature Functions */
 int dss_feat_get_num_mgrs(void);
 int dss_feat_get_num_ovls(void);
+int dss_feat_get_num_wbs(void);
 unsigned long dss_feat_get_param_min(enum dss_range_param param);
 unsigned long dss_feat_get_param_max(enum dss_range_param param);
 enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel);
+enum omap_dss_output_id dss_feat_get_supported_outputs(enum omap_channel channel);
 enum omap_color_mode dss_feat_get_supported_color_modes(enum omap_plane plane);
 enum omap_overlay_caps dss_feat_get_overlay_caps(enum omap_plane plane);
 bool dss_feat_color_mode_supported(enum omap_plane plane,
diff --git a/drivers/video/omap2/dss/hdmi.c b/drivers/video/omap2/dss/hdmi.c
index 060216fdc57..a48a7dd75b3 100644
--- a/drivers/video/omap2/dss/hdmi.c
+++ b/drivers/video/omap2/dss/hdmi.c
@@ -32,6 +32,8 @@
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/clk.h>
+#include <linux/gpio.h>
+#include <linux/regulator/consumer.h>
 #include <video/omapdss.h>
 
 #include "ti_hdmi.h"
@@ -61,6 +63,13 @@ static struct {
 	struct hdmi_ip_data ip_data;
 
 	struct clk *sys_clk;
+	struct regulator *vdda_hdmi_dac_reg;
+
+	int ct_cp_hpd_gpio;
+	int ls_oe_gpio;
+	int hpd_gpio;
+
+	struct omap_dss_output output;
 } hdmi;
 
 /*
@@ -314,12 +323,47 @@ static void hdmi_runtime_put(void)
 
 static int __init hdmi_init_display(struct omap_dss_device *dssdev)
 {
+	int r;
+
+	struct gpio gpios[] = {
+		{ hdmi.ct_cp_hpd_gpio, GPIOF_OUT_INIT_LOW, "hdmi_ct_cp_hpd" },
+		{ hdmi.ls_oe_gpio, GPIOF_OUT_INIT_LOW, "hdmi_ls_oe" },
+		{ hdmi.hpd_gpio, GPIOF_DIR_IN, "hdmi_hpd" },
+	};
+
 	DSSDBG("init_display\n");
 
 	dss_init_hdmi_ip_ops(&hdmi.ip_data);
+
+	if (hdmi.vdda_hdmi_dac_reg == NULL) {
+		struct regulator *reg;
+
+		reg = devm_regulator_get(&hdmi.pdev->dev, "vdda_hdmi_dac");
+
+		if (IS_ERR(reg)) {
+			DSSERR("can't get VDDA_HDMI_DAC regulator\n");
+			return PTR_ERR(reg);
+		}
+
+		hdmi.vdda_hdmi_dac_reg = reg;
+	}
+
+	r = gpio_request_array(gpios, ARRAY_SIZE(gpios));
+	if (r)
+		return r;
+
 	return 0;
 }
 
+static void __exit hdmi_uninit_display(struct omap_dss_device *dssdev)
+{
+	DSSDBG("uninit_display\n");
+
+	gpio_free(hdmi.ct_cp_hpd_gpio);
+	gpio_free(hdmi.ls_oe_gpio);
+	gpio_free(hdmi.hpd_gpio);
+}
+
 static const struct hdmi_config *hdmi_find_timing(
 					const struct hdmi_config *timings_arr,
 					int len)
@@ -459,32 +503,30 @@ static void hdmi_compute_pll(struct omap_dss_device *dssdev, int phy,
 static int hdmi_power_on(struct omap_dss_device *dssdev)
 {
 	int r;
-	const struct hdmi_config *timing;
 	struct omap_video_timings *p;
+	struct omap_overlay_manager *mgr = dssdev->output->manager;
 	unsigned long phy;
 
+	gpio_set_value(hdmi.ct_cp_hpd_gpio, 1);
+	gpio_set_value(hdmi.ls_oe_gpio, 1);
+
+	/* wait 300us after CT_CP_HPD for the 5V power output to reach 90% */
+	udelay(300);
+
+	r = regulator_enable(hdmi.vdda_hdmi_dac_reg);
+	if (r)
+		goto err_vdac_enable;
+
 	r = hdmi_runtime_get();
 	if (r)
-		return r;
+		goto err_runtime_get;
 
-	dss_mgr_disable(dssdev->manager);
+	dss_mgr_disable(mgr);
 
-	p = &dssdev->panel.timings;
+	p = &hdmi.ip_data.cfg.timings;
 
-	DSSDBG("hdmi_power_on x_res= %d y_res = %d\n",
-		dssdev->panel.timings.x_res,
-		dssdev->panel.timings.y_res);
+	DSSDBG("hdmi_power_on x_res= %d y_res = %d\n", p->x_res, p->y_res);
 
-	timing = hdmi_get_timings();
-	if (timing == NULL) {
-		/* HDMI code 4 corresponds to 640 * 480 VGA */
-		hdmi.ip_data.cfg.cm.code = 4;
-		/* DVI mode 1 corresponds to HDMI 0 to DVI */
-		hdmi.ip_data.cfg.cm.mode = HDMI_DVI;
-		hdmi.ip_data.cfg = vesa_timings[0];
-	} else {
-		hdmi.ip_data.cfg = *timing;
-	}
 	phy = p->pixel_clock;
 
 	hdmi_compute_pll(dssdev, phy, &hdmi.ip_data.pll_data);
@@ -495,13 +537,13 @@ static int hdmi_power_on(struct omap_dss_device *dssdev)
 	r = hdmi.ip_data.ops->pll_enable(&hdmi.ip_data);
 	if (r) {
 		DSSDBG("Failed to lock PLL\n");
-		goto err;
+		goto err_pll_enable;
 	}
 
 	r = hdmi.ip_data.ops->phy_enable(&hdmi.ip_data);
 	if (r) {
 		DSSDBG("Failed to start PHY\n");
-		goto err;
+		goto err_phy_enable;
 	}
 
 	hdmi.ip_data.ops->video_configure(&hdmi.ip_data);
@@ -521,13 +563,13 @@ static int hdmi_power_on(struct omap_dss_device *dssdev)
 	dispc_enable_gamma_table(0);
 
 	/* tv size */
-	dss_mgr_set_timings(dssdev->manager, &dssdev->panel.timings);
+	dss_mgr_set_timings(mgr, p);
 
 	r = hdmi.ip_data.ops->video_enable(&hdmi.ip_data);
 	if (r)
 		goto err_vid_enable;
 
-	r = dss_mgr_enable(dssdev->manager);
+	r = dss_mgr_enable(mgr);
 	if (r)
 		goto err_mgr_enable;
 
@@ -537,20 +579,33 @@ err_mgr_enable:
 	hdmi.ip_data.ops->video_disable(&hdmi.ip_data);
 err_vid_enable:
 	hdmi.ip_data.ops->phy_disable(&hdmi.ip_data);
+err_phy_enable:
 	hdmi.ip_data.ops->pll_disable(&hdmi.ip_data);
-err:
+err_pll_enable:
 	hdmi_runtime_put();
+err_runtime_get:
+	regulator_disable(hdmi.vdda_hdmi_dac_reg);
+err_vdac_enable:
+	gpio_set_value(hdmi.ct_cp_hpd_gpio, 0);
+	gpio_set_value(hdmi.ls_oe_gpio, 0);
 	return -EIO;
 }
 
 static void hdmi_power_off(struct omap_dss_device *dssdev)
 {
-	dss_mgr_disable(dssdev->manager);
+	struct omap_overlay_manager *mgr = dssdev->output->manager;
+
+	dss_mgr_disable(mgr);
 
 	hdmi.ip_data.ops->video_disable(&hdmi.ip_data);
 	hdmi.ip_data.ops->phy_disable(&hdmi.ip_data);
 	hdmi.ip_data.ops->pll_disable(&hdmi.ip_data);
 	hdmi_runtime_put();
+
+	regulator_disable(hdmi.vdda_hdmi_dac_reg);
+
+	gpio_set_value(hdmi.ct_cp_hpd_gpio, 0);
+	gpio_set_value(hdmi.ls_oe_gpio, 0);
 }
 
 int omapdss_hdmi_display_check_timing(struct omap_dss_device *dssdev,
@@ -567,25 +622,22 @@ int omapdss_hdmi_display_check_timing(struct omap_dss_device *dssdev,
 
 }
 
-void omapdss_hdmi_display_set_timing(struct omap_dss_device *dssdev)
+void omapdss_hdmi_display_set_timing(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
 {
 	struct hdmi_cm cm;
+	const struct hdmi_config *t;
 
-	cm = hdmi_get_code(&dssdev->panel.timings);
-	hdmi.ip_data.cfg.cm.code = cm.code;
-	hdmi.ip_data.cfg.cm.mode = cm.mode;
+	mutex_lock(&hdmi.lock);
 
-	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
-		int r;
+	cm = hdmi_get_code(timings);
+	hdmi.ip_data.cfg.cm = cm;
 
-		hdmi_power_off(dssdev);
+	t = hdmi_get_timings();
+	if (t != NULL)
+		hdmi.ip_data.cfg = *t;
 
-		r = hdmi_power_on(dssdev);
-		if (r)
-			DSSERR("failed to power on device\n");
-	} else {
-		dss_mgr_set_timings(dssdev->manager, &dssdev->panel.timings);
-	}
+	mutex_unlock(&hdmi.lock);
 }
 
 static void hdmi_dump_regs(struct seq_file *s)
@@ -640,20 +692,20 @@ bool omapdss_hdmi_detect(void)
 
 int omapdss_hdmi_display_enable(struct omap_dss_device *dssdev)
 {
-	struct omap_dss_hdmi_data *priv = dssdev->data;
+	struct omap_dss_output *out = dssdev->output;
 	int r = 0;
 
 	DSSDBG("ENTER hdmi_display_enable\n");
 
 	mutex_lock(&hdmi.lock);
 
-	if (dssdev->manager == NULL) {
-		DSSERR("failed to enable display: no manager\n");
+	if (out == NULL || out->manager == NULL) {
+		DSSERR("failed to enable display: no output/manager\n");
 		r = -ENODEV;
 		goto err0;
 	}
 
-	hdmi.ip_data.hpd_gpio = priv->hpd_gpio;
+	hdmi.ip_data.hpd_gpio = hdmi.hpd_gpio;
 
 	r = omap_dss_start_device(dssdev);
 	if (r) {
@@ -661,26 +713,15 @@ int omapdss_hdmi_display_enable(struct omap_dss_device *dssdev)
 		goto err0;
 	}
 
-	if (dssdev->platform_enable) {
-		r = dssdev->platform_enable(dssdev);
-		if (r) {
-			DSSERR("failed to enable GPIO's\n");
-			goto err1;
-		}
-	}
-
 	r = hdmi_power_on(dssdev);
 	if (r) {
 		DSSERR("failed to power on device\n");
-		goto err2;
+		goto err1;
 	}
 
 	mutex_unlock(&hdmi.lock);
 	return 0;
 
-err2:
-	if (dssdev->platform_disable)
-		dssdev->platform_disable(dssdev);
 err1:
 	omap_dss_stop_device(dssdev);
 err0:
@@ -696,9 +737,6 @@ void omapdss_hdmi_display_disable(struct omap_dss_device *dssdev)
 
 	hdmi_power_off(dssdev);
 
-	if (dssdev->platform_disable)
-		dssdev->platform_disable(dssdev);
-
 	omap_dss_stop_device(dssdev);
 
 	mutex_unlock(&hdmi.lock);
@@ -869,10 +907,14 @@ int hdmi_audio_config(struct omap_dss_audio *audio)
 
 #endif
 
-static void __init hdmi_probe_pdata(struct platform_device *pdev)
+static struct omap_dss_device * __init hdmi_find_dssdev(struct platform_device *pdev)
 {
 	struct omap_dss_board_info *pdata = pdev->dev.platform_data;
-	int r, i;
+	const char *def_disp_name = dss_get_default_display_name();
+	struct omap_dss_device *def_dssdev;
+	int i;
+
+	def_dssdev = NULL;
 
 	for (i = 0; i < pdata->num_devices; ++i) {
 		struct omap_dss_device *dssdev = pdata->devices[i];
@@ -880,17 +922,76 @@ static void __init hdmi_probe_pdata(struct platform_device *pdev)
 		if (dssdev->type != OMAP_DISPLAY_TYPE_HDMI)
 			continue;
 
-		r = hdmi_init_display(dssdev);
-		if (r) {
-			DSSERR("device %s init failed: %d\n", dssdev->name, r);
-			continue;
+		if (def_dssdev == NULL)
+			def_dssdev = dssdev;
+
+		if (def_disp_name != NULL &&
+				strcmp(dssdev->name, def_disp_name) == 0) {
+			def_dssdev = dssdev;
+			break;
 		}
+	}
 
-		r = omap_dss_register_device(dssdev, &pdev->dev, i);
-		if (r)
-			DSSERR("device %s register failed: %d\n",
-					dssdev->name, r);
+	return def_dssdev;
+}
+
+static void __init hdmi_probe_pdata(struct platform_device *pdev)
+{
+	struct omap_dss_device *plat_dssdev;
+	struct omap_dss_device *dssdev;
+	struct omap_dss_hdmi_data *priv;
+	int r;
+
+	plat_dssdev = hdmi_find_dssdev(pdev);
+
+	if (!plat_dssdev)
+		return;
+
+	dssdev = dss_alloc_and_init_device(&pdev->dev);
+	if (!dssdev)
+		return;
+
+	dss_copy_device_pdata(dssdev, plat_dssdev);
+
+	priv = dssdev->data;
+
+	hdmi.ct_cp_hpd_gpio = priv->ct_cp_hpd_gpio;
+	hdmi.ls_oe_gpio = priv->ls_oe_gpio;
+	hdmi.hpd_gpio = priv->hpd_gpio;
+
+	dssdev->channel = OMAP_DSS_CHANNEL_DIGIT;
+
+	r = hdmi_init_display(dssdev);
+	if (r) {
+		DSSERR("device %s init failed: %d\n", dssdev->name, r);
+		dss_put_device(dssdev);
+		return;
 	}
+
+	r = dss_add_device(dssdev);
+	if (r) {
+		DSSERR("device %s register failed: %d\n", dssdev->name, r);
+		dss_put_device(dssdev);
+		return;
+	}
+}
+
+static void __init hdmi_init_output(struct platform_device *pdev)
+{
+	struct omap_dss_output *out = &hdmi.output;
+
+	out->pdev = pdev;
+	out->id = OMAP_DSS_OUTPUT_HDMI;
+	out->type = OMAP_DISPLAY_TYPE_HDMI;
+
+	dss_register_output(out);
+}
+
+static void __exit hdmi_uninit_output(struct platform_device *pdev)
+{
+	struct omap_dss_output *out = &hdmi.output;
+
+	dss_unregister_output(out);
 }
 
 /* HDMI HW IP initialisation */
@@ -929,23 +1030,37 @@ static int __init omapdss_hdmihw_probe(struct platform_device *pdev)
 	hdmi.ip_data.core_av_offset = HDMI_CORE_AV;
 	hdmi.ip_data.pll_offset = HDMI_PLLCTRL;
 	hdmi.ip_data.phy_offset = HDMI_PHY;
+
 	mutex_init(&hdmi.ip_data.lock);
 
 	hdmi_panel_init();
 
 	dss_debugfs_create_file("hdmi", hdmi_dump_regs);
 
+	hdmi_init_output(pdev);
+
 	hdmi_probe_pdata(pdev);
 
 	return 0;
 }
 
+static int __exit hdmi_remove_child(struct device *dev, void *data)
+{
+	struct omap_dss_device *dssdev = to_dss_device(dev);
+	hdmi_uninit_display(dssdev);
+	return 0;
+}
+
 static int __exit omapdss_hdmihw_remove(struct platform_device *pdev)
 {
-	omap_dss_unregister_child_devices(&pdev->dev);
+	device_for_each_child(&pdev->dev, NULL, hdmi_remove_child);
+
+	dss_unregister_child_devices(&pdev->dev);
 
 	hdmi_panel_exit();
 
+	hdmi_uninit_output(pdev);
+
 	pm_runtime_disable(&pdev->dev);
 
 	hdmi_put_clocks();
diff --git a/drivers/video/omap2/dss/hdmi_panel.c b/drivers/video/omap2/dss/hdmi_panel.c
index e10844faadf..69fb115bab3 100644
--- a/drivers/video/omap2/dss/hdmi_panel.c
+++ b/drivers/video/omap2/dss/hdmi_panel.c
@@ -41,17 +41,34 @@ static struct {
 
 static int hdmi_panel_probe(struct omap_dss_device *dssdev)
 {
+	/* Initialize default timings to VGA in DVI mode */
+	const struct omap_video_timings default_timings = {
+		.x_res		= 640,
+		.y_res		= 480,
+		.pixel_clock	= 25175,
+		.hsw		= 96,
+		.hfp		= 16,
+		.hbp		= 48,
+		.vsw		= 2,
+		.vfp		= 11,
+		.vbp		= 31,
+
+		.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+		.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+
+		.interlace	= false,
+	};
+
 	DSSDBG("ENTER hdmi_panel_probe\n");
 
-	dssdev->panel.timings = (struct omap_video_timings)
-			{ 640, 480, 25175, 96, 16, 48, 2, 11, 31,
-				OMAPDSS_SIG_ACTIVE_LOW, OMAPDSS_SIG_ACTIVE_LOW,
-				false,
-			};
+	dssdev->panel.timings = default_timings;
 
 	DSSDBG("hdmi_panel_probe x_res= %d y_res = %d\n",
 		dssdev->panel.timings.x_res,
 		dssdev->panel.timings.y_res);
+
+	omapdss_hdmi_display_set_timing(dssdev, &dssdev->panel.timings);
+
 	return 0;
 }
 
@@ -228,6 +245,8 @@ static int hdmi_panel_enable(struct omap_dss_device *dssdev)
 		goto err;
 	}
 
+	omapdss_hdmi_display_set_timing(dssdev, &dssdev->panel.timings);
+
 	r = omapdss_hdmi_display_enable(dssdev);
 	if (r) {
 		DSSERR("failed to power on\n");
@@ -336,8 +355,8 @@ static void hdmi_set_timings(struct omap_dss_device *dssdev,
 	 */
 	hdmi_panel_audio_disable(dssdev);
 
+	omapdss_hdmi_display_set_timing(dssdev, timings);
 	dssdev->panel.timings = *timings;
-	omapdss_hdmi_display_set_timing(dssdev);
 
 	mutex_unlock(&hdmi.lock);
 }
diff --git a/drivers/video/omap2/dss/manager-sysfs.c b/drivers/video/omap2/dss/manager-sysfs.c
new file mode 100644
index 00000000000..9a2fb59b6f8
--- /dev/null
+++ b/drivers/video/omap2/dss/manager-sysfs.c
@@ -0,0 +1,512 @@
+/*
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * 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/>.
+ */
+
+#define DSS_SUBSYS_NAME "MANAGER"
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/jiffies.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+static ssize_t manager_name_show(struct omap_overlay_manager *mgr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%s\n", mgr->name);
+}
+
+static ssize_t manager_display_show(struct omap_overlay_manager *mgr, char *buf)
+{
+	struct omap_dss_device *dssdev = mgr->get_device(mgr);
+
+	return snprintf(buf, PAGE_SIZE, "%s\n", dssdev ?
+			dssdev->name : "<none>");
+}
+
+static ssize_t manager_display_store(struct omap_overlay_manager *mgr,
+		const char *buf, size_t size)
+{
+	int r = 0;
+	size_t len = size;
+	struct omap_dss_device *dssdev = NULL;
+
+	int match(struct omap_dss_device *dssdev, void *data)
+	{
+		const char *str = data;
+		return sysfs_streq(dssdev->name, str);
+	}
+
+	if (buf[size-1] == '\n')
+		--len;
+
+	if (len > 0)
+		dssdev = omap_dss_find_device((void *)buf, match);
+
+	if (len > 0 && dssdev == NULL)
+		return -EINVAL;
+
+	if (dssdev)
+		DSSDBG("display %s found\n", dssdev->name);
+
+	if (mgr->output) {
+		r = mgr->unset_output(mgr);
+		if (r) {
+			DSSERR("failed to unset current output\n");
+			goto put_device;
+		}
+	}
+
+	if (dssdev) {
+		struct omap_dss_output *out = dssdev->output;
+
+		/*
+		 * a registered device should have an output connected to it
+		 * already
+		 */
+		if (!out) {
+			DSSERR("device has no output connected to it\n");
+			goto put_device;
+		}
+
+		r = mgr->set_output(mgr, out);
+		if (r) {
+			DSSERR("failed to set manager output\n");
+			goto put_device;
+		}
+
+		r = mgr->apply(mgr);
+		if (r) {
+			DSSERR("failed to apply dispc config\n");
+			goto put_device;
+		}
+	}
+
+put_device:
+	if (dssdev)
+		omap_dss_put_device(dssdev);
+
+	return r ? r : size;
+}
+
+static ssize_t manager_default_color_show(struct omap_overlay_manager *mgr,
+					  char *buf)
+{
+	struct omap_overlay_manager_info info;
+
+	mgr->get_manager_info(mgr, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%#x\n", info.default_color);
+}
+
+static ssize_t manager_default_color_store(struct omap_overlay_manager *mgr,
+					   const char *buf, size_t size)
+{
+	struct omap_overlay_manager_info info;
+	u32 color;
+	int r;
+
+	r = kstrtouint(buf, 0, &color);
+	if (r)
+		return r;
+
+	mgr->get_manager_info(mgr, &info);
+
+	info.default_color = color;
+
+	r = mgr->set_manager_info(mgr, &info);
+	if (r)
+		return r;
+
+	r = mgr->apply(mgr);
+	if (r)
+		return r;
+
+	return size;
+}
+
+static const char *trans_key_type_str[] = {
+	"gfx-destination",
+	"video-source",
+};
+
+static ssize_t manager_trans_key_type_show(struct omap_overlay_manager *mgr,
+					   char *buf)
+{
+	enum omap_dss_trans_key_type key_type;
+	struct omap_overlay_manager_info info;
+
+	mgr->get_manager_info(mgr, &info);
+
+	key_type = info.trans_key_type;
+	BUG_ON(key_type >= ARRAY_SIZE(trans_key_type_str));
+
+	return snprintf(buf, PAGE_SIZE, "%s\n", trans_key_type_str[key_type]);
+}
+
+static ssize_t manager_trans_key_type_store(struct omap_overlay_manager *mgr,
+					    const char *buf, size_t size)
+{
+	enum omap_dss_trans_key_type key_type;
+	struct omap_overlay_manager_info info;
+	int r;
+
+	for (key_type = OMAP_DSS_COLOR_KEY_GFX_DST;
+			key_type < ARRAY_SIZE(trans_key_type_str); key_type++) {
+		if (sysfs_streq(buf, trans_key_type_str[key_type]))
+			break;
+	}
+
+	if (key_type == ARRAY_SIZE(trans_key_type_str))
+		return -EINVAL;
+
+	mgr->get_manager_info(mgr, &info);
+
+	info.trans_key_type = key_type;
+
+	r = mgr->set_manager_info(mgr, &info);
+	if (r)
+		return r;
+
+	r = mgr->apply(mgr);
+	if (r)
+		return r;
+
+	return size;
+}
+
+static ssize_t manager_trans_key_value_show(struct omap_overlay_manager *mgr,
+					    char *buf)
+{
+	struct omap_overlay_manager_info info;
+
+	mgr->get_manager_info(mgr, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%#x\n", info.trans_key);
+}
+
+static ssize_t manager_trans_key_value_store(struct omap_overlay_manager *mgr,
+					     const char *buf, size_t size)
+{
+	struct omap_overlay_manager_info info;
+	u32 key_value;
+	int r;
+
+	r = kstrtouint(buf, 0, &key_value);
+	if (r)
+		return r;
+
+	mgr->get_manager_info(mgr, &info);
+
+	info.trans_key = key_value;
+
+	r = mgr->set_manager_info(mgr, &info);
+	if (r)
+		return r;
+
+	r = mgr->apply(mgr);
+	if (r)
+		return r;
+
+	return size;
+}
+
+static ssize_t manager_trans_key_enabled_show(struct omap_overlay_manager *mgr,
+					      char *buf)
+{
+	struct omap_overlay_manager_info info;
+
+	mgr->get_manager_info(mgr, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", info.trans_enabled);
+}
+
+static ssize_t manager_trans_key_enabled_store(struct omap_overlay_manager *mgr,
+					       const char *buf, size_t size)
+{
+	struct omap_overlay_manager_info info;
+	bool enable;
+	int r;
+
+	r = strtobool(buf, &enable);
+	if (r)
+		return r;
+
+	mgr->get_manager_info(mgr, &info);
+
+	info.trans_enabled = enable;
+
+	r = mgr->set_manager_info(mgr, &info);
+	if (r)
+		return r;
+
+	r = mgr->apply(mgr);
+	if (r)
+		return r;
+
+	return size;
+}
+
+static ssize_t manager_alpha_blending_enabled_show(
+		struct omap_overlay_manager *mgr, char *buf)
+{
+	struct omap_overlay_manager_info info;
+
+	mgr->get_manager_info(mgr, &info);
+
+	WARN_ON(!dss_has_feature(FEAT_ALPHA_FIXED_ZORDER));
+
+	return snprintf(buf, PAGE_SIZE, "%d\n",
+		info.partial_alpha_enabled);
+}
+
+static ssize_t manager_alpha_blending_enabled_store(
+		struct omap_overlay_manager *mgr,
+		const char *buf, size_t size)
+{
+	struct omap_overlay_manager_info info;
+	bool enable;
+	int r;
+
+	WARN_ON(!dss_has_feature(FEAT_ALPHA_FIXED_ZORDER));
+
+	r = strtobool(buf, &enable);
+	if (r)
+		return r;
+
+	mgr->get_manager_info(mgr, &info);
+
+	info.partial_alpha_enabled = enable;
+
+	r = mgr->set_manager_info(mgr, &info);
+	if (r)
+		return r;
+
+	r = mgr->apply(mgr);
+	if (r)
+		return r;
+
+	return size;
+}
+
+static ssize_t manager_cpr_enable_show(struct omap_overlay_manager *mgr,
+		char *buf)
+{
+	struct omap_overlay_manager_info info;
+
+	mgr->get_manager_info(mgr, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", info.cpr_enable);
+}
+
+static ssize_t manager_cpr_enable_store(struct omap_overlay_manager *mgr,
+		const char *buf, size_t size)
+{
+	struct omap_overlay_manager_info info;
+	int r;
+	bool enable;
+
+	if (!dss_has_feature(FEAT_CPR))
+		return -ENODEV;
+
+	r = strtobool(buf, &enable);
+	if (r)
+		return r;
+
+	mgr->get_manager_info(mgr, &info);
+
+	if (info.cpr_enable == enable)
+		return size;
+
+	info.cpr_enable = enable;
+
+	r = mgr->set_manager_info(mgr, &info);
+	if (r)
+		return r;
+
+	r = mgr->apply(mgr);
+	if (r)
+		return r;
+
+	return size;
+}
+
+static ssize_t manager_cpr_coef_show(struct omap_overlay_manager *mgr,
+		char *buf)
+{
+	struct omap_overlay_manager_info info;
+
+	mgr->get_manager_info(mgr, &info);
+
+	return snprintf(buf, PAGE_SIZE,
+			"%d %d %d %d %d %d %d %d %d\n",
+			info.cpr_coefs.rr,
+			info.cpr_coefs.rg,
+			info.cpr_coefs.rb,
+			info.cpr_coefs.gr,
+			info.cpr_coefs.gg,
+			info.cpr_coefs.gb,
+			info.cpr_coefs.br,
+			info.cpr_coefs.bg,
+			info.cpr_coefs.bb);
+}
+
+static ssize_t manager_cpr_coef_store(struct omap_overlay_manager *mgr,
+		const char *buf, size_t size)
+{
+	struct omap_overlay_manager_info info;
+	struct omap_dss_cpr_coefs coefs;
+	int r, i;
+	s16 *arr;
+
+	if (!dss_has_feature(FEAT_CPR))
+		return -ENODEV;
+
+	if (sscanf(buf, "%hd %hd %hd %hd %hd %hd %hd %hd %hd",
+				&coefs.rr, &coefs.rg, &coefs.rb,
+				&coefs.gr, &coefs.gg, &coefs.gb,
+				&coefs.br, &coefs.bg, &coefs.bb) != 9)
+		return -EINVAL;
+
+	arr = (s16[]){ coefs.rr, coefs.rg, coefs.rb,
+		coefs.gr, coefs.gg, coefs.gb,
+		coefs.br, coefs.bg, coefs.bb };
+
+	for (i = 0; i < 9; ++i) {
+		if (arr[i] < -512 || arr[i] > 511)
+			return -EINVAL;
+	}
+
+	mgr->get_manager_info(mgr, &info);
+
+	info.cpr_coefs = coefs;
+
+	r = mgr->set_manager_info(mgr, &info);
+	if (r)
+		return r;
+
+	r = mgr->apply(mgr);
+	if (r)
+		return r;
+
+	return size;
+}
+
+struct manager_attribute {
+	struct attribute attr;
+	ssize_t (*show)(struct omap_overlay_manager *, char *);
+	ssize_t	(*store)(struct omap_overlay_manager *, const char *, size_t);
+};
+
+#define MANAGER_ATTR(_name, _mode, _show, _store) \
+	struct manager_attribute manager_attr_##_name = \
+	__ATTR(_name, _mode, _show, _store)
+
+static MANAGER_ATTR(name, S_IRUGO, manager_name_show, NULL);
+static MANAGER_ATTR(display, S_IRUGO|S_IWUSR,
+		manager_display_show, manager_display_store);
+static MANAGER_ATTR(default_color, S_IRUGO|S_IWUSR,
+		manager_default_color_show, manager_default_color_store);
+static MANAGER_ATTR(trans_key_type, S_IRUGO|S_IWUSR,
+		manager_trans_key_type_show, manager_trans_key_type_store);
+static MANAGER_ATTR(trans_key_value, S_IRUGO|S_IWUSR,
+		manager_trans_key_value_show, manager_trans_key_value_store);
+static MANAGER_ATTR(trans_key_enabled, S_IRUGO|S_IWUSR,
+		manager_trans_key_enabled_show,
+		manager_trans_key_enabled_store);
+static MANAGER_ATTR(alpha_blending_enabled, S_IRUGO|S_IWUSR,
+		manager_alpha_blending_enabled_show,
+		manager_alpha_blending_enabled_store);
+static MANAGER_ATTR(cpr_enable, S_IRUGO|S_IWUSR,
+		manager_cpr_enable_show,
+		manager_cpr_enable_store);
+static MANAGER_ATTR(cpr_coef, S_IRUGO|S_IWUSR,
+		manager_cpr_coef_show,
+		manager_cpr_coef_store);
+
+
+static struct attribute *manager_sysfs_attrs[] = {
+	&manager_attr_name.attr,
+	&manager_attr_display.attr,
+	&manager_attr_default_color.attr,
+	&manager_attr_trans_key_type.attr,
+	&manager_attr_trans_key_value.attr,
+	&manager_attr_trans_key_enabled.attr,
+	&manager_attr_alpha_blending_enabled.attr,
+	&manager_attr_cpr_enable.attr,
+	&manager_attr_cpr_coef.attr,
+	NULL
+};
+
+static ssize_t manager_attr_show(struct kobject *kobj, struct attribute *attr,
+		char *buf)
+{
+	struct omap_overlay_manager *manager;
+	struct manager_attribute *manager_attr;
+
+	manager = container_of(kobj, struct omap_overlay_manager, kobj);
+	manager_attr = container_of(attr, struct manager_attribute, attr);
+
+	if (!manager_attr->show)
+		return -ENOENT;
+
+	return manager_attr->show(manager, buf);
+}
+
+static ssize_t manager_attr_store(struct kobject *kobj, struct attribute *attr,
+		const char *buf, size_t size)
+{
+	struct omap_overlay_manager *manager;
+	struct manager_attribute *manager_attr;
+
+	manager = container_of(kobj, struct omap_overlay_manager, kobj);
+	manager_attr = container_of(attr, struct manager_attribute, attr);
+
+	if (!manager_attr->store)
+		return -ENOENT;
+
+	return manager_attr->store(manager, buf, size);
+}
+
+static const struct sysfs_ops manager_sysfs_ops = {
+	.show = manager_attr_show,
+	.store = manager_attr_store,
+};
+
+static struct kobj_type manager_ktype = {
+	.sysfs_ops = &manager_sysfs_ops,
+	.default_attrs = manager_sysfs_attrs,
+};
+
+int dss_manager_kobj_init(struct omap_overlay_manager *mgr,
+		struct platform_device *pdev)
+{
+	return kobject_init_and_add(&mgr->kobj, &manager_ktype,
+			&pdev->dev.kobj, "manager%d", mgr->id);
+}
+
+void dss_manager_kobj_uninit(struct omap_overlay_manager *mgr)
+{
+	kobject_del(&mgr->kobj);
+	kobject_put(&mgr->kobj);
+}
diff --git a/drivers/video/omap2/dss/manager.c b/drivers/video/omap2/dss/manager.c
index 53710fadc82..c54d2f620ce 100644
--- a/drivers/video/omap2/dss/manager.c
+++ b/drivers/video/omap2/dss/manager.c
@@ -36,463 +36,15 @@
 static int num_managers;
 static struct omap_overlay_manager *managers;
 
-static ssize_t manager_name_show(struct omap_overlay_manager *mgr, char *buf)
+static inline struct omap_dss_device *dss_mgr_get_device(struct omap_overlay_manager *mgr)
 {
-	return snprintf(buf, PAGE_SIZE, "%s\n", mgr->name);
+	return mgr->output ? mgr->output->device : NULL;
 }
 
-static ssize_t manager_display_show(struct omap_overlay_manager *mgr, char *buf)
-{
-	return snprintf(buf, PAGE_SIZE, "%s\n",
-			mgr->device ? mgr->device->name : "<none>");
-}
-
-static ssize_t manager_display_store(struct omap_overlay_manager *mgr,
-		const char *buf, size_t size)
-{
-	int r = 0;
-	size_t len = size;
-	struct omap_dss_device *dssdev = NULL;
-
-	int match(struct omap_dss_device *dssdev, void *data)
-	{
-		const char *str = data;
-		return sysfs_streq(dssdev->name, str);
-	}
-
-	if (buf[size-1] == '\n')
-		--len;
-
-	if (len > 0)
-		dssdev = omap_dss_find_device((void *)buf, match);
-
-	if (len > 0 && dssdev == NULL)
-		return -EINVAL;
-
-	if (dssdev)
-		DSSDBG("display %s found\n", dssdev->name);
-
-	if (mgr->device) {
-		r = mgr->unset_device(mgr);
-		if (r) {
-			DSSERR("failed to unset display\n");
-			goto put_device;
-		}
-	}
-
-	if (dssdev) {
-		r = mgr->set_device(mgr, dssdev);
-		if (r) {
-			DSSERR("failed to set manager\n");
-			goto put_device;
-		}
-
-		r = mgr->apply(mgr);
-		if (r) {
-			DSSERR("failed to apply dispc config\n");
-			goto put_device;
-		}
-	}
-
-put_device:
-	if (dssdev)
-		omap_dss_put_device(dssdev);
-
-	return r ? r : size;
-}
-
-static ssize_t manager_default_color_show(struct omap_overlay_manager *mgr,
-					  char *buf)
-{
-	struct omap_overlay_manager_info info;
-
-	mgr->get_manager_info(mgr, &info);
-
-	return snprintf(buf, PAGE_SIZE, "%#x\n", info.default_color);
-}
-
-static ssize_t manager_default_color_store(struct omap_overlay_manager *mgr,
-					   const char *buf, size_t size)
-{
-	struct omap_overlay_manager_info info;
-	u32 color;
-	int r;
-
-	r = kstrtouint(buf, 0, &color);
-	if (r)
-		return r;
-
-	mgr->get_manager_info(mgr, &info);
-
-	info.default_color = color;
-
-	r = mgr->set_manager_info(mgr, &info);
-	if (r)
-		return r;
-
-	r = mgr->apply(mgr);
-	if (r)
-		return r;
-
-	return size;
-}
-
-static const char *trans_key_type_str[] = {
-	"gfx-destination",
-	"video-source",
-};
-
-static ssize_t manager_trans_key_type_show(struct omap_overlay_manager *mgr,
-					   char *buf)
-{
-	enum omap_dss_trans_key_type key_type;
-	struct omap_overlay_manager_info info;
-
-	mgr->get_manager_info(mgr, &info);
-
-	key_type = info.trans_key_type;
-	BUG_ON(key_type >= ARRAY_SIZE(trans_key_type_str));
-
-	return snprintf(buf, PAGE_SIZE, "%s\n", trans_key_type_str[key_type]);
-}
-
-static ssize_t manager_trans_key_type_store(struct omap_overlay_manager *mgr,
-					    const char *buf, size_t size)
-{
-	enum omap_dss_trans_key_type key_type;
-	struct omap_overlay_manager_info info;
-	int r;
-
-	for (key_type = OMAP_DSS_COLOR_KEY_GFX_DST;
-			key_type < ARRAY_SIZE(trans_key_type_str); key_type++) {
-		if (sysfs_streq(buf, trans_key_type_str[key_type]))
-			break;
-	}
-
-	if (key_type == ARRAY_SIZE(trans_key_type_str))
-		return -EINVAL;
-
-	mgr->get_manager_info(mgr, &info);
-
-	info.trans_key_type = key_type;
-
-	r = mgr->set_manager_info(mgr, &info);
-	if (r)
-		return r;
-
-	r = mgr->apply(mgr);
-	if (r)
-		return r;
-
-	return size;
-}
-
-static ssize_t manager_trans_key_value_show(struct omap_overlay_manager *mgr,
-					    char *buf)
-{
-	struct omap_overlay_manager_info info;
-
-	mgr->get_manager_info(mgr, &info);
-
-	return snprintf(buf, PAGE_SIZE, "%#x\n", info.trans_key);
-}
-
-static ssize_t manager_trans_key_value_store(struct omap_overlay_manager *mgr,
-					     const char *buf, size_t size)
-{
-	struct omap_overlay_manager_info info;
-	u32 key_value;
-	int r;
-
-	r = kstrtouint(buf, 0, &key_value);
-	if (r)
-		return r;
-
-	mgr->get_manager_info(mgr, &info);
-
-	info.trans_key = key_value;
-
-	r = mgr->set_manager_info(mgr, &info);
-	if (r)
-		return r;
-
-	r = mgr->apply(mgr);
-	if (r)
-		return r;
-
-	return size;
-}
-
-static ssize_t manager_trans_key_enabled_show(struct omap_overlay_manager *mgr,
-					      char *buf)
-{
-	struct omap_overlay_manager_info info;
-
-	mgr->get_manager_info(mgr, &info);
-
-	return snprintf(buf, PAGE_SIZE, "%d\n", info.trans_enabled);
-}
-
-static ssize_t manager_trans_key_enabled_store(struct omap_overlay_manager *mgr,
-					       const char *buf, size_t size)
-{
-	struct omap_overlay_manager_info info;
-	bool enable;
-	int r;
-
-	r = strtobool(buf, &enable);
-	if (r)
-		return r;
-
-	mgr->get_manager_info(mgr, &info);
-
-	info.trans_enabled = enable;
-
-	r = mgr->set_manager_info(mgr, &info);
-	if (r)
-		return r;
-
-	r = mgr->apply(mgr);
-	if (r)
-		return r;
-
-	return size;
-}
-
-static ssize_t manager_alpha_blending_enabled_show(
-		struct omap_overlay_manager *mgr, char *buf)
-{
-	struct omap_overlay_manager_info info;
-
-	mgr->get_manager_info(mgr, &info);
-
-	WARN_ON(!dss_has_feature(FEAT_ALPHA_FIXED_ZORDER));
-
-	return snprintf(buf, PAGE_SIZE, "%d\n",
-		info.partial_alpha_enabled);
-}
-
-static ssize_t manager_alpha_blending_enabled_store(
-		struct omap_overlay_manager *mgr,
-		const char *buf, size_t size)
-{
-	struct omap_overlay_manager_info info;
-	bool enable;
-	int r;
-
-	WARN_ON(!dss_has_feature(FEAT_ALPHA_FIXED_ZORDER));
-
-	r = strtobool(buf, &enable);
-	if (r)
-		return r;
-
-	mgr->get_manager_info(mgr, &info);
-
-	info.partial_alpha_enabled = enable;
-
-	r = mgr->set_manager_info(mgr, &info);
-	if (r)
-		return r;
-
-	r = mgr->apply(mgr);
-	if (r)
-		return r;
-
-	return size;
-}
-
-static ssize_t manager_cpr_enable_show(struct omap_overlay_manager *mgr,
-		char *buf)
-{
-	struct omap_overlay_manager_info info;
-
-	mgr->get_manager_info(mgr, &info);
-
-	return snprintf(buf, PAGE_SIZE, "%d\n", info.cpr_enable);
-}
-
-static ssize_t manager_cpr_enable_store(struct omap_overlay_manager *mgr,
-		const char *buf, size_t size)
-{
-	struct omap_overlay_manager_info info;
-	int r;
-	bool enable;
-
-	if (!dss_has_feature(FEAT_CPR))
-		return -ENODEV;
-
-	r = strtobool(buf, &enable);
-	if (r)
-		return r;
-
-	mgr->get_manager_info(mgr, &info);
-
-	if (info.cpr_enable == enable)
-		return size;
-
-	info.cpr_enable = enable;
-
-	r = mgr->set_manager_info(mgr, &info);
-	if (r)
-		return r;
-
-	r = mgr->apply(mgr);
-	if (r)
-		return r;
-
-	return size;
-}
-
-static ssize_t manager_cpr_coef_show(struct omap_overlay_manager *mgr,
-		char *buf)
-{
-	struct omap_overlay_manager_info info;
-
-	mgr->get_manager_info(mgr, &info);
-
-	return snprintf(buf, PAGE_SIZE,
-			"%d %d %d %d %d %d %d %d %d\n",
-			info.cpr_coefs.rr,
-			info.cpr_coefs.rg,
-			info.cpr_coefs.rb,
-			info.cpr_coefs.gr,
-			info.cpr_coefs.gg,
-			info.cpr_coefs.gb,
-			info.cpr_coefs.br,
-			info.cpr_coefs.bg,
-			info.cpr_coefs.bb);
-}
-
-static ssize_t manager_cpr_coef_store(struct omap_overlay_manager *mgr,
-		const char *buf, size_t size)
-{
-	struct omap_overlay_manager_info info;
-	struct omap_dss_cpr_coefs coefs;
-	int r, i;
-	s16 *arr;
-
-	if (!dss_has_feature(FEAT_CPR))
-		return -ENODEV;
-
-	if (sscanf(buf, "%hd %hd %hd %hd %hd %hd %hd %hd %hd",
-				&coefs.rr, &coefs.rg, &coefs.rb,
-				&coefs.gr, &coefs.gg, &coefs.gb,
-				&coefs.br, &coefs.bg, &coefs.bb) != 9)
-		return -EINVAL;
-
-	arr = (s16[]){ coefs.rr, coefs.rg, coefs.rb,
-		coefs.gr, coefs.gg, coefs.gb,
-		coefs.br, coefs.bg, coefs.bb };
-
-	for (i = 0; i < 9; ++i) {
-		if (arr[i] < -512 || arr[i] > 511)
-			return -EINVAL;
-	}
-
-	mgr->get_manager_info(mgr, &info);
-
-	info.cpr_coefs = coefs;
-
-	r = mgr->set_manager_info(mgr, &info);
-	if (r)
-		return r;
-
-	r = mgr->apply(mgr);
-	if (r)
-		return r;
-
-	return size;
-}
-
-struct manager_attribute {
-	struct attribute attr;
-	ssize_t (*show)(struct omap_overlay_manager *, char *);
-	ssize_t	(*store)(struct omap_overlay_manager *, const char *, size_t);
-};
-
-#define MANAGER_ATTR(_name, _mode, _show, _store) \
-	struct manager_attribute manager_attr_##_name = \
-	__ATTR(_name, _mode, _show, _store)
-
-static MANAGER_ATTR(name, S_IRUGO, manager_name_show, NULL);
-static MANAGER_ATTR(display, S_IRUGO|S_IWUSR,
-		manager_display_show, manager_display_store);
-static MANAGER_ATTR(default_color, S_IRUGO|S_IWUSR,
-		manager_default_color_show, manager_default_color_store);
-static MANAGER_ATTR(trans_key_type, S_IRUGO|S_IWUSR,
-		manager_trans_key_type_show, manager_trans_key_type_store);
-static MANAGER_ATTR(trans_key_value, S_IRUGO|S_IWUSR,
-		manager_trans_key_value_show, manager_trans_key_value_store);
-static MANAGER_ATTR(trans_key_enabled, S_IRUGO|S_IWUSR,
-		manager_trans_key_enabled_show,
-		manager_trans_key_enabled_store);
-static MANAGER_ATTR(alpha_blending_enabled, S_IRUGO|S_IWUSR,
-		manager_alpha_blending_enabled_show,
-		manager_alpha_blending_enabled_store);
-static MANAGER_ATTR(cpr_enable, S_IRUGO|S_IWUSR,
-		manager_cpr_enable_show,
-		manager_cpr_enable_store);
-static MANAGER_ATTR(cpr_coef, S_IRUGO|S_IWUSR,
-		manager_cpr_coef_show,
-		manager_cpr_coef_store);
-
-
-static struct attribute *manager_sysfs_attrs[] = {
-	&manager_attr_name.attr,
-	&manager_attr_display.attr,
-	&manager_attr_default_color.attr,
-	&manager_attr_trans_key_type.attr,
-	&manager_attr_trans_key_value.attr,
-	&manager_attr_trans_key_enabled.attr,
-	&manager_attr_alpha_blending_enabled.attr,
-	&manager_attr_cpr_enable.attr,
-	&manager_attr_cpr_coef.attr,
-	NULL
-};
-
-static ssize_t manager_attr_show(struct kobject *kobj, struct attribute *attr,
-		char *buf)
-{
-	struct omap_overlay_manager *manager;
-	struct manager_attribute *manager_attr;
-
-	manager = container_of(kobj, struct omap_overlay_manager, kobj);
-	manager_attr = container_of(attr, struct manager_attribute, attr);
-
-	if (!manager_attr->show)
-		return -ENOENT;
-
-	return manager_attr->show(manager, buf);
-}
-
-static ssize_t manager_attr_store(struct kobject *kobj, struct attribute *attr,
-		const char *buf, size_t size)
-{
-	struct omap_overlay_manager *manager;
-	struct manager_attribute *manager_attr;
-
-	manager = container_of(kobj, struct omap_overlay_manager, kobj);
-	manager_attr = container_of(attr, struct manager_attribute, attr);
-
-	if (!manager_attr->store)
-		return -ENOENT;
-
-	return manager_attr->store(manager, buf, size);
-}
-
-static const struct sysfs_ops manager_sysfs_ops = {
-	.show = manager_attr_show,
-	.store = manager_attr_store,
-};
-
-static struct kobj_type manager_ktype = {
-	.sysfs_ops = &manager_sysfs_ops,
-	.default_attrs = manager_sysfs_attrs,
-};
-
 static int dss_mgr_wait_for_vsync(struct omap_overlay_manager *mgr)
 {
 	unsigned long timeout = msecs_to_jiffies(500);
+	struct omap_dss_device *dssdev = mgr->get_device(mgr);
 	u32 irq;
 	int r;
 
@@ -500,9 +52,9 @@ static int dss_mgr_wait_for_vsync(struct omap_overlay_manager *mgr)
 	if (r)
 		return r;
 
-	if (mgr->device->type == OMAP_DISPLAY_TYPE_VENC)
+	if (dssdev->type == OMAP_DISPLAY_TYPE_VENC)
 		irq = DISPC_IRQ_EVSYNC_ODD;
-	else if (mgr->device->type == OMAP_DISPLAY_TYPE_HDMI)
+	else if (dssdev->type == OMAP_DISPLAY_TYPE_HDMI)
 		irq = DISPC_IRQ_EVSYNC_EVEN;
 	else
 		irq = dispc_mgr_get_vsync_irq(mgr->id);
@@ -547,23 +99,24 @@ int dss_init_overlay_managers(struct platform_device *pdev)
 			break;
 		}
 
-		mgr->set_device = &dss_mgr_set_device;
-		mgr->unset_device = &dss_mgr_unset_device;
+		mgr->set_output = &dss_mgr_set_output;
+		mgr->unset_output = &dss_mgr_unset_output;
 		mgr->apply = &omap_dss_mgr_apply;
 		mgr->set_manager_info = &dss_mgr_set_info;
 		mgr->get_manager_info = &dss_mgr_get_info;
 		mgr->wait_for_go = &dss_mgr_wait_for_go;
 		mgr->wait_for_vsync = &dss_mgr_wait_for_vsync;
+		mgr->get_device = &dss_mgr_get_device;
 
 		mgr->caps = 0;
 		mgr->supported_displays =
 			dss_feat_get_supported_displays(mgr->id);
+		mgr->supported_outputs =
+			dss_feat_get_supported_outputs(mgr->id);
 
 		INIT_LIST_HEAD(&mgr->overlays);
 
-		r = kobject_init_and_add(&mgr->kobj, &manager_ktype,
-				&pdev->dev.kobj, "manager%d", i);
-
+		r = dss_manager_kobj_init(mgr, pdev);
 		if (r)
 			DSSERR("failed to create sysfs file\n");
 	}
@@ -577,9 +130,7 @@ void dss_uninit_overlay_managers(struct platform_device *pdev)
 
 	for (i = 0; i < num_managers; ++i) {
 		struct omap_overlay_manager *mgr = &managers[i];
-
-		kobject_del(&mgr->kobj);
-		kobject_put(&mgr->kobj);
+		dss_manager_kobj_uninit(mgr);
 	}
 
 	kfree(managers);
diff --git a/drivers/video/omap2/dss/output.c b/drivers/video/omap2/dss/output.c
new file mode 100644
index 00000000000..813f26682b7
--- /dev/null
+++ b/drivers/video/omap2/dss/output.c
@@ -0,0 +1,148 @@
+/*
+ * Copyright (C) 2012 Texas Instruments Ltd
+ * Author: Archit Taneja <archit@ti.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/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+
+static LIST_HEAD(output_list);
+static DEFINE_MUTEX(output_lock);
+
+int omapdss_output_set_device(struct omap_dss_output *out,
+		struct omap_dss_device *dssdev)
+{
+	int r;
+
+	mutex_lock(&output_lock);
+
+	if (out->device) {
+		DSSERR("output already has device %s connected to it\n",
+			out->device->name);
+		r = -EINVAL;
+		goto err;
+	}
+
+	if (out->type != dssdev->type) {
+		DSSERR("output type and display type don't match\n");
+		r = -EINVAL;
+		goto err;
+	}
+
+	out->device = dssdev;
+	dssdev->output = out;
+
+	mutex_unlock(&output_lock);
+
+	return 0;
+err:
+	mutex_unlock(&output_lock);
+
+	return r;
+}
+EXPORT_SYMBOL(omapdss_output_set_device);
+
+int omapdss_output_unset_device(struct omap_dss_output *out)
+{
+	int r;
+
+	mutex_lock(&output_lock);
+
+	if (!out->device) {
+		DSSERR("output doesn't have a device connected to it\n");
+		r = -EINVAL;
+		goto err;
+	}
+
+	if (out->device->state != OMAP_DSS_DISPLAY_DISABLED) {
+		DSSERR("device %s is not disabled, cannot unset device\n",
+				out->device->name);
+		r = -EINVAL;
+		goto err;
+	}
+
+	out->device->output = NULL;
+	out->device = NULL;
+
+	mutex_unlock(&output_lock);
+
+	return 0;
+err:
+	mutex_unlock(&output_lock);
+
+	return r;
+}
+EXPORT_SYMBOL(omapdss_output_unset_device);
+
+void dss_register_output(struct omap_dss_output *out)
+{
+	list_add_tail(&out->list, &output_list);
+}
+
+void dss_unregister_output(struct omap_dss_output *out)
+{
+	list_del(&out->list);
+}
+
+struct omap_dss_output *omap_dss_get_output(enum omap_dss_output_id id)
+{
+	struct omap_dss_output *out;
+
+	list_for_each_entry(out, &output_list, list) {
+		if (out->id == id)
+			return out;
+	}
+
+	return NULL;
+}
+
+struct omap_dss_output *omapdss_get_output_from_dssdev(struct omap_dss_device *dssdev)
+{
+	struct omap_dss_output *out = NULL;
+	enum omap_dss_output_id id;
+
+	switch (dssdev->type) {
+	case OMAP_DISPLAY_TYPE_DPI:
+		out = omap_dss_get_output(OMAP_DSS_OUTPUT_DPI);
+		break;
+	case OMAP_DISPLAY_TYPE_DBI:
+		out = omap_dss_get_output(OMAP_DSS_OUTPUT_DBI);
+		break;
+	case OMAP_DISPLAY_TYPE_SDI:
+		out = omap_dss_get_output(OMAP_DSS_OUTPUT_SDI);
+		break;
+	case OMAP_DISPLAY_TYPE_VENC:
+		out = omap_dss_get_output(OMAP_DSS_OUTPUT_VENC);
+		break;
+	case OMAP_DISPLAY_TYPE_HDMI:
+		out = omap_dss_get_output(OMAP_DSS_OUTPUT_HDMI);
+		break;
+	case OMAP_DISPLAY_TYPE_DSI:
+		id = dssdev->phy.dsi.module == 0 ? OMAP_DSS_OUTPUT_DSI1 :
+					OMAP_DSS_OUTPUT_DSI2;
+		out = omap_dss_get_output(id);
+		break;
+	default:
+		break;
+	}
+
+	return out;
+}
diff --git a/drivers/video/omap2/dss/overlay-sysfs.c b/drivers/video/omap2/dss/overlay-sysfs.c
new file mode 100644
index 00000000000..4cc5ddebfb3
--- /dev/null
+++ b/drivers/video/omap2/dss/overlay-sysfs.c
@@ -0,0 +1,456 @@
+/*
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * 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/>.
+ */
+
+#define DSS_SUBSYS_NAME "OVERLAY"
+
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/sysfs.h>
+#include <linux/kobject.h>
+#include <linux/platform_device.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+static ssize_t overlay_name_show(struct omap_overlay *ovl, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%s\n", ovl->name);
+}
+
+static ssize_t overlay_manager_show(struct omap_overlay *ovl, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%s\n",
+			ovl->manager ? ovl->manager->name : "<none>");
+}
+
+static ssize_t overlay_manager_store(struct omap_overlay *ovl, const char *buf,
+		size_t size)
+{
+	int i, r;
+	struct omap_overlay_manager *mgr = NULL;
+	struct omap_overlay_manager *old_mgr;
+	int len = size;
+
+	if (buf[size-1] == '\n')
+		--len;
+
+	if (len > 0) {
+		for (i = 0; i < omap_dss_get_num_overlay_managers(); ++i) {
+			mgr = omap_dss_get_overlay_manager(i);
+
+			if (sysfs_streq(buf, mgr->name))
+				break;
+
+			mgr = NULL;
+		}
+	}
+
+	if (len > 0 && mgr == NULL)
+		return -EINVAL;
+
+	if (mgr)
+		DSSDBG("manager %s found\n", mgr->name);
+
+	if (mgr == ovl->manager)
+		return size;
+
+	old_mgr = ovl->manager;
+
+	r = dispc_runtime_get();
+	if (r)
+		return r;
+
+	/* detach old manager */
+	if (old_mgr) {
+		r = ovl->unset_manager(ovl);
+		if (r) {
+			DSSERR("detach failed\n");
+			goto err;
+		}
+
+		r = old_mgr->apply(old_mgr);
+		if (r)
+			goto err;
+	}
+
+	if (mgr) {
+		r = ovl->set_manager(ovl, mgr);
+		if (r) {
+			DSSERR("Failed to attach overlay\n");
+			goto err;
+		}
+
+		r = mgr->apply(mgr);
+		if (r)
+			goto err;
+	}
+
+	dispc_runtime_put();
+
+	return size;
+
+err:
+	dispc_runtime_put();
+	return r;
+}
+
+static ssize_t overlay_input_size_show(struct omap_overlay *ovl, char *buf)
+{
+	struct omap_overlay_info info;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%d,%d\n",
+			info.width, info.height);
+}
+
+static ssize_t overlay_screen_width_show(struct omap_overlay *ovl, char *buf)
+{
+	struct omap_overlay_info info;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", info.screen_width);
+}
+
+static ssize_t overlay_position_show(struct omap_overlay *ovl, char *buf)
+{
+	struct omap_overlay_info info;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%d,%d\n",
+			info.pos_x, info.pos_y);
+}
+
+static ssize_t overlay_position_store(struct omap_overlay *ovl,
+		const char *buf, size_t size)
+{
+	int r;
+	char *last;
+	struct omap_overlay_info info;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	info.pos_x = simple_strtoul(buf, &last, 10);
+	++last;
+	if (last - buf >= size)
+		return -EINVAL;
+
+	info.pos_y = simple_strtoul(last, &last, 10);
+
+	r = ovl->set_overlay_info(ovl, &info);
+	if (r)
+		return r;
+
+	if (ovl->manager) {
+		r = ovl->manager->apply(ovl->manager);
+		if (r)
+			return r;
+	}
+
+	return size;
+}
+
+static ssize_t overlay_output_size_show(struct omap_overlay *ovl, char *buf)
+{
+	struct omap_overlay_info info;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%d,%d\n",
+			info.out_width, info.out_height);
+}
+
+static ssize_t overlay_output_size_store(struct omap_overlay *ovl,
+		const char *buf, size_t size)
+{
+	int r;
+	char *last;
+	struct omap_overlay_info info;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	info.out_width = simple_strtoul(buf, &last, 10);
+	++last;
+	if (last - buf >= size)
+		return -EINVAL;
+
+	info.out_height = simple_strtoul(last, &last, 10);
+
+	r = ovl->set_overlay_info(ovl, &info);
+	if (r)
+		return r;
+
+	if (ovl->manager) {
+		r = ovl->manager->apply(ovl->manager);
+		if (r)
+			return r;
+	}
+
+	return size;
+}
+
+static ssize_t overlay_enabled_show(struct omap_overlay *ovl, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%d\n", ovl->is_enabled(ovl));
+}
+
+static ssize_t overlay_enabled_store(struct omap_overlay *ovl, const char *buf,
+		size_t size)
+{
+	int r;
+	bool enable;
+
+	r = strtobool(buf, &enable);
+	if (r)
+		return r;
+
+	if (enable)
+		r = ovl->enable(ovl);
+	else
+		r = ovl->disable(ovl);
+
+	if (r)
+		return r;
+
+	return size;
+}
+
+static ssize_t overlay_global_alpha_show(struct omap_overlay *ovl, char *buf)
+{
+	struct omap_overlay_info info;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n",
+			info.global_alpha);
+}
+
+static ssize_t overlay_global_alpha_store(struct omap_overlay *ovl,
+		const char *buf, size_t size)
+{
+	int r;
+	u8 alpha;
+	struct omap_overlay_info info;
+
+	if ((ovl->caps & OMAP_DSS_OVL_CAP_GLOBAL_ALPHA) == 0)
+		return -ENODEV;
+
+	r = kstrtou8(buf, 0, &alpha);
+	if (r)
+		return r;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	info.global_alpha = alpha;
+
+	r = ovl->set_overlay_info(ovl, &info);
+	if (r)
+		return r;
+
+	if (ovl->manager) {
+		r = ovl->manager->apply(ovl->manager);
+		if (r)
+			return r;
+	}
+
+	return size;
+}
+
+static ssize_t overlay_pre_mult_alpha_show(struct omap_overlay *ovl,
+		char *buf)
+{
+	struct omap_overlay_info info;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n",
+			info.pre_mult_alpha);
+}
+
+static ssize_t overlay_pre_mult_alpha_store(struct omap_overlay *ovl,
+		const char *buf, size_t size)
+{
+	int r;
+	u8 alpha;
+	struct omap_overlay_info info;
+
+	if ((ovl->caps & OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA) == 0)
+		return -ENODEV;
+
+	r = kstrtou8(buf, 0, &alpha);
+	if (r)
+		return r;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	info.pre_mult_alpha = alpha;
+
+	r = ovl->set_overlay_info(ovl, &info);
+	if (r)
+		return r;
+
+	if (ovl->manager) {
+		r = ovl->manager->apply(ovl->manager);
+		if (r)
+			return r;
+	}
+
+	return size;
+}
+
+static ssize_t overlay_zorder_show(struct omap_overlay *ovl, char *buf)
+{
+	struct omap_overlay_info info;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", info.zorder);
+}
+
+static ssize_t overlay_zorder_store(struct omap_overlay *ovl,
+		const char *buf, size_t size)
+{
+	int r;
+	u8 zorder;
+	struct omap_overlay_info info;
+
+	if ((ovl->caps & OMAP_DSS_OVL_CAP_ZORDER) == 0)
+		return -ENODEV;
+
+	r = kstrtou8(buf, 0, &zorder);
+	if (r)
+		return r;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	info.zorder = zorder;
+
+	r = ovl->set_overlay_info(ovl, &info);
+	if (r)
+		return r;
+
+	if (ovl->manager) {
+		r = ovl->manager->apply(ovl->manager);
+		if (r)
+			return r;
+	}
+
+	return size;
+}
+
+struct overlay_attribute {
+	struct attribute attr;
+	ssize_t (*show)(struct omap_overlay *, char *);
+	ssize_t	(*store)(struct omap_overlay *, const char *, size_t);
+};
+
+#define OVERLAY_ATTR(_name, _mode, _show, _store) \
+	struct overlay_attribute overlay_attr_##_name = \
+	__ATTR(_name, _mode, _show, _store)
+
+static OVERLAY_ATTR(name, S_IRUGO, overlay_name_show, NULL);
+static OVERLAY_ATTR(manager, S_IRUGO|S_IWUSR,
+		overlay_manager_show, overlay_manager_store);
+static OVERLAY_ATTR(input_size, S_IRUGO, overlay_input_size_show, NULL);
+static OVERLAY_ATTR(screen_width, S_IRUGO, overlay_screen_width_show, NULL);
+static OVERLAY_ATTR(position, S_IRUGO|S_IWUSR,
+		overlay_position_show, overlay_position_store);
+static OVERLAY_ATTR(output_size, S_IRUGO|S_IWUSR,
+		overlay_output_size_show, overlay_output_size_store);
+static OVERLAY_ATTR(enabled, S_IRUGO|S_IWUSR,
+		overlay_enabled_show, overlay_enabled_store);
+static OVERLAY_ATTR(global_alpha, S_IRUGO|S_IWUSR,
+		overlay_global_alpha_show, overlay_global_alpha_store);
+static OVERLAY_ATTR(pre_mult_alpha, S_IRUGO|S_IWUSR,
+		overlay_pre_mult_alpha_show,
+		overlay_pre_mult_alpha_store);
+static OVERLAY_ATTR(zorder, S_IRUGO|S_IWUSR,
+		overlay_zorder_show, overlay_zorder_store);
+
+static struct attribute *overlay_sysfs_attrs[] = {
+	&overlay_attr_name.attr,
+	&overlay_attr_manager.attr,
+	&overlay_attr_input_size.attr,
+	&overlay_attr_screen_width.attr,
+	&overlay_attr_position.attr,
+	&overlay_attr_output_size.attr,
+	&overlay_attr_enabled.attr,
+	&overlay_attr_global_alpha.attr,
+	&overlay_attr_pre_mult_alpha.attr,
+	&overlay_attr_zorder.attr,
+	NULL
+};
+
+static ssize_t overlay_attr_show(struct kobject *kobj, struct attribute *attr,
+		char *buf)
+{
+	struct omap_overlay *overlay;
+	struct overlay_attribute *overlay_attr;
+
+	overlay = container_of(kobj, struct omap_overlay, kobj);
+	overlay_attr = container_of(attr, struct overlay_attribute, attr);
+
+	if (!overlay_attr->show)
+		return -ENOENT;
+
+	return overlay_attr->show(overlay, buf);
+}
+
+static ssize_t overlay_attr_store(struct kobject *kobj, struct attribute *attr,
+		const char *buf, size_t size)
+{
+	struct omap_overlay *overlay;
+	struct overlay_attribute *overlay_attr;
+
+	overlay = container_of(kobj, struct omap_overlay, kobj);
+	overlay_attr = container_of(attr, struct overlay_attribute, attr);
+
+	if (!overlay_attr->store)
+		return -ENOENT;
+
+	return overlay_attr->store(overlay, buf, size);
+}
+
+static const struct sysfs_ops overlay_sysfs_ops = {
+	.show = overlay_attr_show,
+	.store = overlay_attr_store,
+};
+
+static struct kobj_type overlay_ktype = {
+	.sysfs_ops = &overlay_sysfs_ops,
+	.default_attrs = overlay_sysfs_attrs,
+};
+
+int dss_overlay_kobj_init(struct omap_overlay *ovl,
+		struct platform_device *pdev)
+{
+	return kobject_init_and_add(&ovl->kobj, &overlay_ktype,
+			&pdev->dev.kobj, "overlay%d", ovl->id);
+}
+
+void dss_overlay_kobj_uninit(struct omap_overlay *ovl)
+{
+	kobject_del(&ovl->kobj);
+	kobject_put(&ovl->kobj);
+}
diff --git a/drivers/video/omap2/dss/overlay.c b/drivers/video/omap2/dss/overlay.c
index 952c6fad9a8..45f4994bc6b 100644
--- a/drivers/video/omap2/dss/overlay.c
+++ b/drivers/video/omap2/dss/overlay.c
@@ -26,13 +26,11 @@
 #include <linux/module.h>
 #include <linux/err.h>
 #include <linux/sysfs.h>
-#include <linux/kobject.h>
 #include <linux/platform_device.h>
 #include <linux/delay.h>
 #include <linux/slab.h>
 
 #include <video/omapdss.h>
-#include <plat/cpu.h>
 
 #include "dss.h"
 #include "dss_features.h"
@@ -40,417 +38,13 @@
 static int num_overlays;
 static struct omap_overlay *overlays;
 
-static ssize_t overlay_name_show(struct omap_overlay *ovl, char *buf)
+static inline struct omap_dss_device *dss_ovl_get_device(struct omap_overlay *ovl)
 {
-	return snprintf(buf, PAGE_SIZE, "%s\n", ovl->name);
+	return ovl->manager ?
+		(ovl->manager->output ? ovl->manager->output->device : NULL) :
+		NULL;
 }
 
-static ssize_t overlay_manager_show(struct omap_overlay *ovl, char *buf)
-{
-	return snprintf(buf, PAGE_SIZE, "%s\n",
-			ovl->manager ? ovl->manager->name : "<none>");
-}
-
-static ssize_t overlay_manager_store(struct omap_overlay *ovl, const char *buf,
-		size_t size)
-{
-	int i, r;
-	struct omap_overlay_manager *mgr = NULL;
-	struct omap_overlay_manager *old_mgr;
-	int len = size;
-
-	if (buf[size-1] == '\n')
-		--len;
-
-	if (len > 0) {
-		for (i = 0; i < omap_dss_get_num_overlay_managers(); ++i) {
-			mgr = omap_dss_get_overlay_manager(i);
-
-			if (sysfs_streq(buf, mgr->name))
-				break;
-
-			mgr = NULL;
-		}
-	}
-
-	if (len > 0 && mgr == NULL)
-		return -EINVAL;
-
-	if (mgr)
-		DSSDBG("manager %s found\n", mgr->name);
-
-	if (mgr == ovl->manager)
-		return size;
-
-	old_mgr = ovl->manager;
-
-	r = dispc_runtime_get();
-	if (r)
-		return r;
-
-	/* detach old manager */
-	if (old_mgr) {
-		r = ovl->unset_manager(ovl);
-		if (r) {
-			DSSERR("detach failed\n");
-			goto err;
-		}
-
-		r = old_mgr->apply(old_mgr);
-		if (r)
-			goto err;
-	}
-
-	if (mgr) {
-		r = ovl->set_manager(ovl, mgr);
-		if (r) {
-			DSSERR("Failed to attach overlay\n");
-			goto err;
-		}
-
-		r = mgr->apply(mgr);
-		if (r)
-			goto err;
-	}
-
-	dispc_runtime_put();
-
-	return size;
-
-err:
-	dispc_runtime_put();
-	return r;
-}
-
-static ssize_t overlay_input_size_show(struct omap_overlay *ovl, char *buf)
-{
-	struct omap_overlay_info info;
-
-	ovl->get_overlay_info(ovl, &info);
-
-	return snprintf(buf, PAGE_SIZE, "%d,%d\n",
-			info.width, info.height);
-}
-
-static ssize_t overlay_screen_width_show(struct omap_overlay *ovl, char *buf)
-{
-	struct omap_overlay_info info;
-
-	ovl->get_overlay_info(ovl, &info);
-
-	return snprintf(buf, PAGE_SIZE, "%d\n", info.screen_width);
-}
-
-static ssize_t overlay_position_show(struct omap_overlay *ovl, char *buf)
-{
-	struct omap_overlay_info info;
-
-	ovl->get_overlay_info(ovl, &info);
-
-	return snprintf(buf, PAGE_SIZE, "%d,%d\n",
-			info.pos_x, info.pos_y);
-}
-
-static ssize_t overlay_position_store(struct omap_overlay *ovl,
-		const char *buf, size_t size)
-{
-	int r;
-	char *last;
-	struct omap_overlay_info info;
-
-	ovl->get_overlay_info(ovl, &info);
-
-	info.pos_x = simple_strtoul(buf, &last, 10);
-	++last;
-	if (last - buf >= size)
-		return -EINVAL;
-
-	info.pos_y = simple_strtoul(last, &last, 10);
-
-	r = ovl->set_overlay_info(ovl, &info);
-	if (r)
-		return r;
-
-	if (ovl->manager) {
-		r = ovl->manager->apply(ovl->manager);
-		if (r)
-			return r;
-	}
-
-	return size;
-}
-
-static ssize_t overlay_output_size_show(struct omap_overlay *ovl, char *buf)
-{
-	struct omap_overlay_info info;
-
-	ovl->get_overlay_info(ovl, &info);
-
-	return snprintf(buf, PAGE_SIZE, "%d,%d\n",
-			info.out_width, info.out_height);
-}
-
-static ssize_t overlay_output_size_store(struct omap_overlay *ovl,
-		const char *buf, size_t size)
-{
-	int r;
-	char *last;
-	struct omap_overlay_info info;
-
-	ovl->get_overlay_info(ovl, &info);
-
-	info.out_width = simple_strtoul(buf, &last, 10);
-	++last;
-	if (last - buf >= size)
-		return -EINVAL;
-
-	info.out_height = simple_strtoul(last, &last, 10);
-
-	r = ovl->set_overlay_info(ovl, &info);
-	if (r)
-		return r;
-
-	if (ovl->manager) {
-		r = ovl->manager->apply(ovl->manager);
-		if (r)
-			return r;
-	}
-
-	return size;
-}
-
-static ssize_t overlay_enabled_show(struct omap_overlay *ovl, char *buf)
-{
-	return snprintf(buf, PAGE_SIZE, "%d\n", ovl->is_enabled(ovl));
-}
-
-static ssize_t overlay_enabled_store(struct omap_overlay *ovl, const char *buf,
-		size_t size)
-{
-	int r;
-	bool enable;
-
-	r = strtobool(buf, &enable);
-	if (r)
-		return r;
-
-	if (enable)
-		r = ovl->enable(ovl);
-	else
-		r = ovl->disable(ovl);
-
-	if (r)
-		return r;
-
-	return size;
-}
-
-static ssize_t overlay_global_alpha_show(struct omap_overlay *ovl, char *buf)
-{
-	struct omap_overlay_info info;
-
-	ovl->get_overlay_info(ovl, &info);
-
-	return snprintf(buf, PAGE_SIZE, "%d\n",
-			info.global_alpha);
-}
-
-static ssize_t overlay_global_alpha_store(struct omap_overlay *ovl,
-		const char *buf, size_t size)
-{
-	int r;
-	u8 alpha;
-	struct omap_overlay_info info;
-
-	if ((ovl->caps & OMAP_DSS_OVL_CAP_GLOBAL_ALPHA) == 0)
-		return -ENODEV;
-
-	r = kstrtou8(buf, 0, &alpha);
-	if (r)
-		return r;
-
-	ovl->get_overlay_info(ovl, &info);
-
-	info.global_alpha = alpha;
-
-	r = ovl->set_overlay_info(ovl, &info);
-	if (r)
-		return r;
-
-	if (ovl->manager) {
-		r = ovl->manager->apply(ovl->manager);
-		if (r)
-			return r;
-	}
-
-	return size;
-}
-
-static ssize_t overlay_pre_mult_alpha_show(struct omap_overlay *ovl,
-		char *buf)
-{
-	struct omap_overlay_info info;
-
-	ovl->get_overlay_info(ovl, &info);
-
-	return snprintf(buf, PAGE_SIZE, "%d\n",
-			info.pre_mult_alpha);
-}
-
-static ssize_t overlay_pre_mult_alpha_store(struct omap_overlay *ovl,
-		const char *buf, size_t size)
-{
-	int r;
-	u8 alpha;
-	struct omap_overlay_info info;
-
-	if ((ovl->caps & OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA) == 0)
-		return -ENODEV;
-
-	r = kstrtou8(buf, 0, &alpha);
-	if (r)
-		return r;
-
-	ovl->get_overlay_info(ovl, &info);
-
-	info.pre_mult_alpha = alpha;
-
-	r = ovl->set_overlay_info(ovl, &info);
-	if (r)
-		return r;
-
-	if (ovl->manager) {
-		r = ovl->manager->apply(ovl->manager);
-		if (r)
-			return r;
-	}
-
-	return size;
-}
-
-static ssize_t overlay_zorder_show(struct omap_overlay *ovl, char *buf)
-{
-	struct omap_overlay_info info;
-
-	ovl->get_overlay_info(ovl, &info);
-
-	return snprintf(buf, PAGE_SIZE, "%d\n", info.zorder);
-}
-
-static ssize_t overlay_zorder_store(struct omap_overlay *ovl,
-		const char *buf, size_t size)
-{
-	int r;
-	u8 zorder;
-	struct omap_overlay_info info;
-
-	if ((ovl->caps & OMAP_DSS_OVL_CAP_ZORDER) == 0)
-		return -ENODEV;
-
-	r = kstrtou8(buf, 0, &zorder);
-	if (r)
-		return r;
-
-	ovl->get_overlay_info(ovl, &info);
-
-	info.zorder = zorder;
-
-	r = ovl->set_overlay_info(ovl, &info);
-	if (r)
-		return r;
-
-	if (ovl->manager) {
-		r = ovl->manager->apply(ovl->manager);
-		if (r)
-			return r;
-	}
-
-	return size;
-}
-
-struct overlay_attribute {
-	struct attribute attr;
-	ssize_t (*show)(struct omap_overlay *, char *);
-	ssize_t	(*store)(struct omap_overlay *, const char *, size_t);
-};
-
-#define OVERLAY_ATTR(_name, _mode, _show, _store) \
-	struct overlay_attribute overlay_attr_##_name = \
-	__ATTR(_name, _mode, _show, _store)
-
-static OVERLAY_ATTR(name, S_IRUGO, overlay_name_show, NULL);
-static OVERLAY_ATTR(manager, S_IRUGO|S_IWUSR,
-		overlay_manager_show, overlay_manager_store);
-static OVERLAY_ATTR(input_size, S_IRUGO, overlay_input_size_show, NULL);
-static OVERLAY_ATTR(screen_width, S_IRUGO, overlay_screen_width_show, NULL);
-static OVERLAY_ATTR(position, S_IRUGO|S_IWUSR,
-		overlay_position_show, overlay_position_store);
-static OVERLAY_ATTR(output_size, S_IRUGO|S_IWUSR,
-		overlay_output_size_show, overlay_output_size_store);
-static OVERLAY_ATTR(enabled, S_IRUGO|S_IWUSR,
-		overlay_enabled_show, overlay_enabled_store);
-static OVERLAY_ATTR(global_alpha, S_IRUGO|S_IWUSR,
-		overlay_global_alpha_show, overlay_global_alpha_store);
-static OVERLAY_ATTR(pre_mult_alpha, S_IRUGO|S_IWUSR,
-		overlay_pre_mult_alpha_show,
-		overlay_pre_mult_alpha_store);
-static OVERLAY_ATTR(zorder, S_IRUGO|S_IWUSR,
-		overlay_zorder_show, overlay_zorder_store);
-
-static struct attribute *overlay_sysfs_attrs[] = {
-	&overlay_attr_name.attr,
-	&overlay_attr_manager.attr,
-	&overlay_attr_input_size.attr,
-	&overlay_attr_screen_width.attr,
-	&overlay_attr_position.attr,
-	&overlay_attr_output_size.attr,
-	&overlay_attr_enabled.attr,
-	&overlay_attr_global_alpha.attr,
-	&overlay_attr_pre_mult_alpha.attr,
-	&overlay_attr_zorder.attr,
-	NULL
-};
-
-static ssize_t overlay_attr_show(struct kobject *kobj, struct attribute *attr,
-		char *buf)
-{
-	struct omap_overlay *overlay;
-	struct overlay_attribute *overlay_attr;
-
-	overlay = container_of(kobj, struct omap_overlay, kobj);
-	overlay_attr = container_of(attr, struct overlay_attribute, attr);
-
-	if (!overlay_attr->show)
-		return -ENOENT;
-
-	return overlay_attr->show(overlay, buf);
-}
-
-static ssize_t overlay_attr_store(struct kobject *kobj, struct attribute *attr,
-		const char *buf, size_t size)
-{
-	struct omap_overlay *overlay;
-	struct overlay_attribute *overlay_attr;
-
-	overlay = container_of(kobj, struct omap_overlay, kobj);
-	overlay_attr = container_of(attr, struct overlay_attribute, attr);
-
-	if (!overlay_attr->store)
-		return -ENOENT;
-
-	return overlay_attr->store(overlay, buf, size);
-}
-
-static const struct sysfs_ops overlay_sysfs_ops = {
-	.show = overlay_attr_show,
-	.store = overlay_attr_store,
-};
-
-static struct kobj_type overlay_ktype = {
-	.sysfs_ops = &overlay_sysfs_ops,
-	.default_attrs = overlay_sysfs_attrs,
-};
-
 int omap_dss_get_num_overlays(void)
 {
 	return num_overlays;
@@ -507,97 +101,25 @@ void dss_init_overlays(struct platform_device *pdev)
 		ovl->set_overlay_info = &dss_ovl_set_info;
 		ovl->get_overlay_info = &dss_ovl_get_info;
 		ovl->wait_for_go = &dss_mgr_wait_for_go_ovl;
+		ovl->get_device = &dss_ovl_get_device;
 
 		ovl->caps = dss_feat_get_overlay_caps(ovl->id);
 		ovl->supported_modes =
 			dss_feat_get_supported_color_modes(ovl->id);
 
-		r = kobject_init_and_add(&ovl->kobj, &overlay_ktype,
-				&pdev->dev.kobj, "overlay%d", i);
-
+		r = dss_overlay_kobj_init(ovl, pdev);
 		if (r)
 			DSSERR("failed to create sysfs file\n");
 	}
 }
 
-/* connect overlays to the new device, if not already connected. if force
- * selected, connect always. */
-void dss_recheck_connections(struct omap_dss_device *dssdev, bool force)
-{
-	int i;
-	struct omap_overlay_manager *lcd_mgr;
-	struct omap_overlay_manager *tv_mgr;
-	struct omap_overlay_manager *lcd2_mgr = NULL;
-	struct omap_overlay_manager *lcd3_mgr = NULL;
-	struct omap_overlay_manager *mgr = NULL;
-
-	lcd_mgr = omap_dss_get_overlay_manager(OMAP_DSS_CHANNEL_LCD);
-	tv_mgr = omap_dss_get_overlay_manager(OMAP_DSS_CHANNEL_DIGIT);
-	if (dss_has_feature(FEAT_MGR_LCD3))
-		lcd3_mgr = omap_dss_get_overlay_manager(OMAP_DSS_CHANNEL_LCD3);
-	if (dss_has_feature(FEAT_MGR_LCD2))
-		lcd2_mgr = omap_dss_get_overlay_manager(OMAP_DSS_CHANNEL_LCD2);
-
-	if (dssdev->channel == OMAP_DSS_CHANNEL_LCD3) {
-		if (!lcd3_mgr->device || force) {
-			if (lcd3_mgr->device)
-				lcd3_mgr->unset_device(lcd3_mgr);
-			lcd3_mgr->set_device(lcd3_mgr, dssdev);
-			mgr = lcd3_mgr;
-		}
-	} else if (dssdev->channel == OMAP_DSS_CHANNEL_LCD2) {
-		if (!lcd2_mgr->device || force) {
-			if (lcd2_mgr->device)
-				lcd2_mgr->unset_device(lcd2_mgr);
-			lcd2_mgr->set_device(lcd2_mgr, dssdev);
-			mgr = lcd2_mgr;
-		}
-	} else if (dssdev->type != OMAP_DISPLAY_TYPE_VENC
-			&& dssdev->type != OMAP_DISPLAY_TYPE_HDMI) {
-		if (!lcd_mgr->device || force) {
-			if (lcd_mgr->device)
-				lcd_mgr->unset_device(lcd_mgr);
-			lcd_mgr->set_device(lcd_mgr, dssdev);
-			mgr = lcd_mgr;
-		}
-	}
-
-	if (dssdev->type == OMAP_DISPLAY_TYPE_VENC
-			|| dssdev->type == OMAP_DISPLAY_TYPE_HDMI) {
-		if (!tv_mgr->device || force) {
-			if (tv_mgr->device)
-				tv_mgr->unset_device(tv_mgr);
-			tv_mgr->set_device(tv_mgr, dssdev);
-			mgr = tv_mgr;
-		}
-	}
-
-	if (mgr) {
-		dispc_runtime_get();
-
-		for (i = 0; i < dss_feat_get_num_ovls(); i++) {
-			struct omap_overlay *ovl;
-			ovl = omap_dss_get_overlay(i);
-			if (!ovl->manager || force) {
-				if (ovl->manager)
-					ovl->unset_manager(ovl);
-				ovl->set_manager(ovl, mgr);
-			}
-		}
-
-		dispc_runtime_put();
-	}
-}
-
 void dss_uninit_overlays(struct platform_device *pdev)
 {
 	int i;
 
 	for (i = 0; i < num_overlays; ++i) {
 		struct omap_overlay *ovl = &overlays[i];
-
-		kobject_del(&ovl->kobj);
-		kobject_put(&ovl->kobj);
+		dss_overlay_kobj_uninit(ovl);
 	}
 
 	kfree(overlays);
diff --git a/drivers/video/omap2/dss/rfbi.c b/drivers/video/omap2/dss/rfbi.c
index 7c087424b63..7282e5af3e1 100644
--- a/drivers/video/omap2/dss/rfbi.c
+++ b/drivers/video/omap2/dss/rfbi.c
@@ -111,6 +111,13 @@ static struct {
 	struct omap_dss_device *dssdev[2];
 
 	struct semaphore bus_lock;
+
+	struct omap_video_timings timings;
+	int pixel_size;
+	int data_lines;
+	struct rfbi_timings intf_timings;
+
+	struct omap_dss_output output;
 } rfbi;
 
 static inline void rfbi_write_reg(const struct rfbi_reg idx, u32 val)
@@ -300,30 +307,23 @@ void omap_rfbi_write_pixels(const void __iomem *buf, int scr_width,
 }
 EXPORT_SYMBOL(omap_rfbi_write_pixels);
 
-static int rfbi_transfer_area(struct omap_dss_device *dssdev, u16 width,
-		u16 height, void (*callback)(void *data), void *data)
+static int rfbi_transfer_area(struct omap_dss_device *dssdev,
+		void (*callback)(void *data), void *data)
 {
 	u32 l;
 	int r;
-	struct omap_video_timings timings = {
-		.hsw		= 1,
-		.hfp		= 1,
-		.hbp		= 1,
-		.vsw		= 1,
-		.vfp		= 0,
-		.vbp		= 0,
-		.x_res		= width,
-		.y_res		= height,
-	};
+	struct omap_overlay_manager *mgr = dssdev->output->manager;
+	u16 width = rfbi.timings.x_res;
+	u16 height = rfbi.timings.y_res;
 
 	/*BUG_ON(callback == 0);*/
 	BUG_ON(rfbi.framedone_callback != NULL);
 
 	DSSDBG("rfbi_transfer_area %dx%d\n", width, height);
 
-	dss_mgr_set_timings(dssdev->manager, &timings);
+	dss_mgr_set_timings(mgr, &rfbi.timings);
 
-	r = dss_mgr_enable(dssdev->manager);
+	r = dss_mgr_enable(mgr);
 	if (r)
 		return r;
 
@@ -770,62 +770,45 @@ static int rfbi_configure(int rfbi_module, int bpp, int lines)
 	return 0;
 }
 
-int omap_rfbi_configure(struct omap_dss_device *dssdev, int pixel_size,
-		int data_lines)
+int omap_rfbi_configure(struct omap_dss_device *dssdev)
 {
-	return rfbi_configure(dssdev->phy.rfbi.channel, pixel_size, data_lines);
+	return rfbi_configure(dssdev->phy.rfbi.channel, rfbi.pixel_size,
+			rfbi.data_lines);
 }
 EXPORT_SYMBOL(omap_rfbi_configure);
 
-int omap_rfbi_prepare_update(struct omap_dss_device *dssdev,
-		u16 *x, u16 *y, u16 *w, u16 *h)
+int omap_rfbi_update(struct omap_dss_device *dssdev, void (*callback)(void *),
+		void *data)
 {
-	u16 dw, dh;
-	struct omap_video_timings timings = {
-		.hsw		= 1,
-		.hfp		= 1,
-		.hbp		= 1,
-		.vsw		= 1,
-		.vfp		= 0,
-		.vbp		= 0,
-		.x_res		= *w,
-		.y_res		= *h,
-	};
-
-	dssdev->driver->get_resolution(dssdev, &dw, &dh);
-
-	if  (*x > dw || *y > dh)
-		return -EINVAL;
-
-	if (*x + *w > dw)
-		return -EINVAL;
-
-	if (*y + *h > dh)
-		return -EINVAL;
-
-	if (*w == 1)
-		return -EINVAL;
-
-	if (*w == 0 || *h == 0)
-		return -EINVAL;
-
-	dss_mgr_set_timings(dssdev->manager, &timings);
+	return rfbi_transfer_area(dssdev, callback, data);
+}
+EXPORT_SYMBOL(omap_rfbi_update);
 
-	return 0;
+void omapdss_rfbi_set_size(struct omap_dss_device *dssdev, u16 w, u16 h)
+{
+	rfbi.timings.x_res = w;
+	rfbi.timings.y_res = h;
 }
-EXPORT_SYMBOL(omap_rfbi_prepare_update);
+EXPORT_SYMBOL(omapdss_rfbi_set_size);
 
-int omap_rfbi_update(struct omap_dss_device *dssdev,
-		u16 x, u16 y, u16 w, u16 h,
-		void (*callback)(void *), void *data)
+void omapdss_rfbi_set_pixel_size(struct omap_dss_device *dssdev, int pixel_size)
 {
-	int r;
+	rfbi.pixel_size = pixel_size;
+}
+EXPORT_SYMBOL(omapdss_rfbi_set_pixel_size);
 
-	r = rfbi_transfer_area(dssdev, w, h, callback, data);
+void omapdss_rfbi_set_data_lines(struct omap_dss_device *dssdev, int data_lines)
+{
+	rfbi.data_lines = data_lines;
+}
+EXPORT_SYMBOL(omapdss_rfbi_set_data_lines);
 
-	return r;
+void omapdss_rfbi_set_interface_timings(struct omap_dss_device *dssdev,
+		struct rfbi_timings *timings)
+{
+	rfbi.intf_timings = *timings;
 }
-EXPORT_SYMBOL(omap_rfbi_update);
+EXPORT_SYMBOL(omapdss_rfbi_set_interface_timings);
 
 static void rfbi_dump_regs(struct seq_file *s)
 {
@@ -869,6 +852,7 @@ static void rfbi_dump_regs(struct seq_file *s)
 
 static void rfbi_config_lcd_manager(struct omap_dss_device *dssdev)
 {
+	struct omap_overlay_manager *mgr = dssdev->output->manager;
 	struct dss_lcd_mgr_config mgr_config;
 
 	mgr_config.io_pad_mode = DSS_IO_PAD_MODE_RFBI;
@@ -877,18 +861,40 @@ static void rfbi_config_lcd_manager(struct omap_dss_device *dssdev)
 	/* Do we need fifohandcheck for RFBI? */
 	mgr_config.fifohandcheck = false;
 
-	mgr_config.video_port_width = dssdev->ctrl.pixel_size;
+	mgr_config.video_port_width = rfbi.pixel_size;
 	mgr_config.lcden_sig_polarity = 0;
 
-	dss_mgr_set_lcd_config(dssdev->manager, &mgr_config);
+	dss_mgr_set_lcd_config(mgr, &mgr_config);
+
+	/*
+	 * Set rfbi.timings with default values, the x_res and y_res fields
+	 * are expected to be already configured by the panel driver via
+	 * omapdss_rfbi_set_size()
+	 */
+	rfbi.timings.hsw = 1;
+	rfbi.timings.hfp = 1;
+	rfbi.timings.hbp = 1;
+	rfbi.timings.vsw = 1;
+	rfbi.timings.vfp = 0;
+	rfbi.timings.vbp = 0;
+
+	rfbi.timings.interlace = false;
+	rfbi.timings.hsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
+	rfbi.timings.vsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
+	rfbi.timings.data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+	rfbi.timings.de_level = OMAPDSS_SIG_ACTIVE_HIGH;
+	rfbi.timings.sync_pclk_edge = OMAPDSS_DRIVE_SIG_OPPOSITE_EDGES;
+
+	dss_mgr_set_timings(mgr, &rfbi.timings);
 }
 
 int omapdss_rfbi_display_enable(struct omap_dss_device *dssdev)
 {
+	struct omap_dss_output *out = dssdev->output;
 	int r;
 
-	if (dssdev->manager == NULL) {
-		DSSERR("failed to enable display: no manager\n");
+	if (out == NULL || out->manager == NULL) {
+		DSSERR("failed to enable display: no output/manager\n");
 		return -ENODEV;
 	}
 
@@ -911,13 +917,10 @@ int omapdss_rfbi_display_enable(struct omap_dss_device *dssdev)
 
 	rfbi_config_lcd_manager(dssdev);
 
-	rfbi_configure(dssdev->phy.rfbi.channel,
-			       dssdev->ctrl.pixel_size,
-			       dssdev->phy.rfbi.data_lines);
-
-	rfbi_set_timings(dssdev->phy.rfbi.channel,
-			 &dssdev->ctrl.rfbi_timings);
+	rfbi_configure(dssdev->phy.rfbi.channel, rfbi.pixel_size,
+			rfbi.data_lines);
 
+	rfbi_set_timings(dssdev->phy.rfbi.channel, &rfbi.intf_timings);
 
 	return 0;
 err1:
@@ -941,14 +944,17 @@ EXPORT_SYMBOL(omapdss_rfbi_display_disable);
 static int __init rfbi_init_display(struct omap_dss_device *dssdev)
 {
 	rfbi.dssdev[dssdev->phy.rfbi.channel] = dssdev;
-	dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE;
 	return 0;
 }
 
-static void __init rfbi_probe_pdata(struct platform_device *pdev)
+static struct omap_dss_device * __init rfbi_find_dssdev(struct platform_device *pdev)
 {
 	struct omap_dss_board_info *pdata = pdev->dev.platform_data;
-	int i, r;
+	const char *def_disp_name = dss_get_default_display_name();
+	struct omap_dss_device *def_dssdev;
+	int i;
+
+	def_dssdev = NULL;
 
 	for (i = 0; i < pdata->num_devices; ++i) {
 		struct omap_dss_device *dssdev = pdata->devices[i];
@@ -956,17 +962,67 @@ static void __init rfbi_probe_pdata(struct platform_device *pdev)
 		if (dssdev->type != OMAP_DISPLAY_TYPE_DBI)
 			continue;
 
-		r = rfbi_init_display(dssdev);
-		if (r) {
-			DSSERR("device %s init failed: %d\n", dssdev->name, r);
-			continue;
+		if (def_dssdev == NULL)
+			def_dssdev = dssdev;
+
+		if (def_disp_name != NULL &&
+				strcmp(dssdev->name, def_disp_name) == 0) {
+			def_dssdev = dssdev;
+			break;
 		}
+	}
+
+	return def_dssdev;
+}
+
+static void __init rfbi_probe_pdata(struct platform_device *rfbidev)
+{
+	struct omap_dss_device *plat_dssdev;
+	struct omap_dss_device *dssdev;
+	int r;
+
+	plat_dssdev = rfbi_find_dssdev(rfbidev);
+
+	if (!plat_dssdev)
+		return;
+
+	dssdev = dss_alloc_and_init_device(&rfbidev->dev);
+	if (!dssdev)
+		return;
+
+	dss_copy_device_pdata(dssdev, plat_dssdev);
 
-		r = omap_dss_register_device(dssdev, &pdev->dev, i);
-		if (r)
-			DSSERR("device %s register failed: %d\n",
-				dssdev->name, r);
+	r = rfbi_init_display(dssdev);
+	if (r) {
+		DSSERR("device %s init failed: %d\n", dssdev->name, r);
+		dss_put_device(dssdev);
+		return;
 	}
+
+	r = dss_add_device(dssdev);
+	if (r) {
+		DSSERR("device %s register failed: %d\n", dssdev->name, r);
+		dss_put_device(dssdev);
+		return;
+	}
+}
+
+static void __init rfbi_init_output(struct platform_device *pdev)
+{
+	struct omap_dss_output *out = &rfbi.output;
+
+	out->pdev = pdev;
+	out->id = OMAP_DSS_OUTPUT_DBI;
+	out->type = OMAP_DISPLAY_TYPE_DBI;
+
+	dss_register_output(out);
+}
+
+static void __exit rfbi_uninit_output(struct platform_device *pdev)
+{
+	struct omap_dss_output *out = &rfbi.output;
+
+	dss_unregister_output(out);
 }
 
 /* RFBI HW IP initialisation */
@@ -1020,6 +1076,8 @@ static int __init omap_rfbihw_probe(struct platform_device *pdev)
 
 	dss_debugfs_create_file("rfbi", rfbi_dump_regs);
 
+	rfbi_init_output(pdev);
+
 	rfbi_probe_pdata(pdev);
 
 	return 0;
@@ -1031,8 +1089,12 @@ err_runtime_get:
 
 static int __exit omap_rfbihw_remove(struct platform_device *pdev)
 {
-	omap_dss_unregister_child_devices(&pdev->dev);
+	dss_unregister_child_devices(&pdev->dev);
+
+	rfbi_uninit_output(pdev);
+
 	pm_runtime_disable(&pdev->dev);
+
 	return 0;
 }
 
diff --git a/drivers/video/omap2/dss/sdi.c b/drivers/video/omap2/dss/sdi.c
index f43bfe17b3b..7760851f6e5 100644
--- a/drivers/video/omap2/dss/sdi.c
+++ b/drivers/video/omap2/dss/sdi.c
@@ -25,6 +25,7 @@
 #include <linux/regulator/consumer.h>
 #include <linux/export.h>
 #include <linux/platform_device.h>
+#include <linux/string.h>
 
 #include <video/omapdss.h>
 #include "dss.h"
@@ -34,10 +35,16 @@ static struct {
 	struct regulator *vdds_sdi_reg;
 
 	struct dss_lcd_mgr_config mgr_config;
+	struct omap_video_timings timings;
+	int datapairs;
+
+	struct omap_dss_output output;
 } sdi;
 
 static void sdi_config_lcd_manager(struct omap_dss_device *dssdev)
 {
+	struct omap_overlay_manager *mgr = dssdev->output->manager;
+
 	sdi.mgr_config.io_pad_mode = DSS_IO_PAD_MODE_BYPASS;
 
 	sdi.mgr_config.stallmode = false;
@@ -46,19 +53,20 @@ static void sdi_config_lcd_manager(struct omap_dss_device *dssdev)
 	sdi.mgr_config.video_port_width = 24;
 	sdi.mgr_config.lcden_sig_polarity = 1;
 
-	dss_mgr_set_lcd_config(dssdev->manager, &sdi.mgr_config);
+	dss_mgr_set_lcd_config(mgr, &sdi.mgr_config);
 }
 
 int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
 {
-	struct omap_video_timings *t = &dssdev->panel.timings;
+	struct omap_dss_output *out = dssdev->output;
+	struct omap_video_timings *t = &sdi.timings;
 	struct dss_clock_info dss_cinfo;
 	struct dispc_clock_info dispc_cinfo;
 	unsigned long pck;
 	int r;
 
-	if (dssdev->manager == NULL) {
-		DSSERR("failed to enable display: no manager\n");
+	if (out == NULL || out->manager == NULL) {
+		DSSERR("failed to enable display: no output/manager\n");
 		return -ENODEV;
 	}
 
@@ -77,8 +85,8 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
 		goto err_get_dispc;
 
 	/* 15.5.9.1.2 */
-	dssdev->panel.timings.data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
-	dssdev->panel.timings.sync_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+	t->data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+	t->sync_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
 
 	r = dss_calc_clock_div(t->pixel_clock * 1000, &dss_cinfo, &dispc_cinfo);
 	if (r)
@@ -97,7 +105,7 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
 	}
 
 
-	dss_mgr_set_timings(dssdev->manager, t);
+	dss_mgr_set_timings(out->manager, t);
 
 	r = dss_set_clock_div(&dss_cinfo);
 	if (r)
@@ -116,16 +124,15 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
 	 * need to care about the shadow register mechanism for pck-free. The
 	 * exact reason for this is unknown.
 	 */
-	dispc_mgr_set_clock_div(dssdev->manager->id,
-			&sdi.mgr_config.clock_info);
+	dispc_mgr_set_clock_div(out->manager->id, &sdi.mgr_config.clock_info);
 
-	dss_sdi_init(dssdev->phy.sdi.datapairs);
+	dss_sdi_init(sdi.datapairs);
 	r = dss_sdi_enable();
 	if (r)
 		goto err_sdi_enable;
 	mdelay(2);
 
-	r = dss_mgr_enable(dssdev->manager);
+	r = dss_mgr_enable(out->manager);
 	if (r)
 		goto err_mgr_enable;
 
@@ -148,7 +155,9 @@ EXPORT_SYMBOL(omapdss_sdi_display_enable);
 
 void omapdss_sdi_display_disable(struct omap_dss_device *dssdev)
 {
-	dss_mgr_disable(dssdev->manager);
+	struct omap_overlay_manager *mgr = dssdev->output->manager;
+
+	dss_mgr_disable(mgr);
 
 	dss_sdi_disable();
 
@@ -160,6 +169,19 @@ void omapdss_sdi_display_disable(struct omap_dss_device *dssdev)
 }
 EXPORT_SYMBOL(omapdss_sdi_display_disable);
 
+void omapdss_sdi_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	sdi.timings = *timings;
+}
+EXPORT_SYMBOL(omapdss_sdi_set_timings);
+
+void omapdss_sdi_set_datapairs(struct omap_dss_device *dssdev, int datapairs)
+{
+	sdi.datapairs = datapairs;
+}
+EXPORT_SYMBOL(omapdss_sdi_set_datapairs);
+
 static int __init sdi_init_display(struct omap_dss_device *dssdev)
 {
 	DSSDBG("SDI init\n");
@@ -180,10 +202,14 @@ static int __init sdi_init_display(struct omap_dss_device *dssdev)
 	return 0;
 }
 
-static void __init sdi_probe_pdata(struct platform_device *pdev)
+static struct omap_dss_device * __init sdi_find_dssdev(struct platform_device *pdev)
 {
 	struct omap_dss_board_info *pdata = pdev->dev.platform_data;
-	int i, r;
+	const char *def_disp_name = dss_get_default_display_name();
+	struct omap_dss_device *def_dssdev;
+	int i;
+
+	def_dssdev = NULL;
 
 	for (i = 0; i < pdata->num_devices; ++i) {
 		struct omap_dss_device *dssdev = pdata->devices[i];
@@ -191,21 +217,73 @@ static void __init sdi_probe_pdata(struct platform_device *pdev)
 		if (dssdev->type != OMAP_DISPLAY_TYPE_SDI)
 			continue;
 
-		r = sdi_init_display(dssdev);
-		if (r) {
-			DSSERR("device %s init failed: %d\n", dssdev->name, r);
-			continue;
+		if (def_dssdev == NULL)
+			def_dssdev = dssdev;
+
+		if (def_disp_name != NULL &&
+				strcmp(dssdev->name, def_disp_name) == 0) {
+			def_dssdev = dssdev;
+			break;
 		}
+	}
+
+	return def_dssdev;
+}
+
+static void __init sdi_probe_pdata(struct platform_device *sdidev)
+{
+	struct omap_dss_device *plat_dssdev;
+	struct omap_dss_device *dssdev;
+	int r;
+
+	plat_dssdev = sdi_find_dssdev(sdidev);
 
-		r = omap_dss_register_device(dssdev, &pdev->dev, i);
-		if (r)
-			DSSERR("device %s register failed: %d\n",
-					dssdev->name, r);
+	if (!plat_dssdev)
+		return;
+
+	dssdev = dss_alloc_and_init_device(&sdidev->dev);
+	if (!dssdev)
+		return;
+
+	dss_copy_device_pdata(dssdev, plat_dssdev);
+
+	r = sdi_init_display(dssdev);
+	if (r) {
+		DSSERR("device %s init failed: %d\n", dssdev->name, r);
+		dss_put_device(dssdev);
+		return;
 	}
+
+	r = dss_add_device(dssdev);
+	if (r) {
+		DSSERR("device %s register failed: %d\n", dssdev->name, r);
+		dss_put_device(dssdev);
+		return;
+	}
+}
+
+static void __init sdi_init_output(struct platform_device *pdev)
+{
+	struct omap_dss_output *out = &sdi.output;
+
+	out->pdev = pdev;
+	out->id = OMAP_DSS_OUTPUT_SDI;
+	out->type = OMAP_DISPLAY_TYPE_SDI;
+
+	dss_register_output(out);
+}
+
+static void __exit sdi_uninit_output(struct platform_device *pdev)
+{
+	struct omap_dss_output *out = &sdi.output;
+
+	dss_unregister_output(out);
 }
 
 static int __init omap_sdi_probe(struct platform_device *pdev)
 {
+	sdi_init_output(pdev);
+
 	sdi_probe_pdata(pdev);
 
 	return 0;
@@ -213,7 +291,9 @@ static int __init omap_sdi_probe(struct platform_device *pdev)
 
 static int __exit omap_sdi_remove(struct platform_device *pdev)
 {
-	omap_dss_unregister_child_devices(&pdev->dev);
+	dss_unregister_child_devices(&pdev->dev);
+
+	sdi_uninit_output(pdev);
 
 	return 0;
 }
diff --git a/drivers/video/omap2/dss/venc.c b/drivers/video/omap2/dss/venc.c
index 3a220877461..56efa3bb465 100644
--- a/drivers/video/omap2/dss/venc.c
+++ b/drivers/video/omap2/dss/venc.c
@@ -36,7 +36,6 @@
 #include <linux/pm_runtime.h>
 
 #include <video/omapdss.h>
-#include <plat/cpu.h>
 
 #include "dss.h"
 #include "dss_features.h"
@@ -300,6 +299,12 @@ static struct {
 	struct regulator *vdda_dac_reg;
 
 	struct clk	*tv_dac_clk;
+
+	struct omap_video_timings timings;
+	enum omap_dss_venc_type type;
+	bool invert_polarity;
+
+	struct omap_dss_output output;
 } venc;
 
 static inline void venc_write_reg(int idx, u32 val)
@@ -424,65 +429,67 @@ static const struct venc_config *venc_timings_to_config(
 
 static int venc_power_on(struct omap_dss_device *dssdev)
 {
+	struct omap_overlay_manager *mgr = dssdev->output->manager;
 	u32 l;
 	int r;
 
+	r = venc_runtime_get();
+	if (r)
+		goto err0;
+
 	venc_reset();
-	venc_write_config(venc_timings_to_config(&dssdev->panel.timings));
+	venc_write_config(venc_timings_to_config(&venc.timings));
 
-	dss_set_venc_output(dssdev->phy.venc.type);
+	dss_set_venc_output(venc.type);
 	dss_set_dac_pwrdn_bgz(1);
 
 	l = 0;
 
-	if (dssdev->phy.venc.type == OMAP_DSS_VENC_TYPE_COMPOSITE)
+	if (venc.type == OMAP_DSS_VENC_TYPE_COMPOSITE)
 		l |= 1 << 1;
 	else /* S-Video */
 		l |= (1 << 0) | (1 << 2);
 
-	if (dssdev->phy.venc.invert_polarity == false)
+	if (venc.invert_polarity == false)
 		l |= 1 << 3;
 
 	venc_write_reg(VENC_OUTPUT_CONTROL, l);
 
-	dss_mgr_set_timings(dssdev->manager, &dssdev->panel.timings);
+	dss_mgr_set_timings(mgr, &venc.timings);
 
 	r = regulator_enable(venc.vdda_dac_reg);
 	if (r)
-		goto err;
-
-	if (dssdev->platform_enable)
-		dssdev->platform_enable(dssdev);
+		goto err1;
 
-	r = dss_mgr_enable(dssdev->manager);
+	r = dss_mgr_enable(mgr);
 	if (r)
-		goto err;
+		goto err2;
 
 	return 0;
 
-err:
+err2:
+	regulator_disable(venc.vdda_dac_reg);
+err1:
 	venc_write_reg(VENC_OUTPUT_CONTROL, 0);
 	dss_set_dac_pwrdn_bgz(0);
 
-	if (dssdev->platform_disable)
-		dssdev->platform_disable(dssdev);
-
-	regulator_disable(venc.vdda_dac_reg);
-
+	venc_runtime_put();
+err0:
 	return r;
 }
 
 static void venc_power_off(struct omap_dss_device *dssdev)
 {
+	struct omap_overlay_manager *mgr = dssdev->output->manager;
+
 	venc_write_reg(VENC_OUTPUT_CONTROL, 0);
 	dss_set_dac_pwrdn_bgz(0);
 
-	dss_mgr_disable(dssdev->manager);
-
-	if (dssdev->platform_disable)
-		dssdev->platform_disable(dssdev);
+	dss_mgr_disable(mgr);
 
 	regulator_disable(venc.vdda_dac_reg);
+
+	venc_runtime_put();
 }
 
 unsigned long venc_get_pixel_clock(void)
@@ -491,171 +498,83 @@ unsigned long venc_get_pixel_clock(void)
 	return 13500000;
 }
 
-static ssize_t display_output_type_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
+int omapdss_venc_display_enable(struct omap_dss_device *dssdev)
 {
-	struct omap_dss_device *dssdev = to_dss_device(dev);
-	const char *ret;
-
-	switch (dssdev->phy.venc.type) {
-	case OMAP_DSS_VENC_TYPE_COMPOSITE:
-		ret = "composite";
-		break;
-	case OMAP_DSS_VENC_TYPE_SVIDEO:
-		ret = "svideo";
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return snprintf(buf, PAGE_SIZE, "%s\n", ret);
-}
+	struct omap_dss_output *out = dssdev->output;
+	int r;
 
-static ssize_t display_output_type_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t size)
-{
-	struct omap_dss_device *dssdev = to_dss_device(dev);
-	enum omap_dss_venc_type new_type;
-
-	if (sysfs_streq("composite", buf))
-		new_type = OMAP_DSS_VENC_TYPE_COMPOSITE;
-	else if (sysfs_streq("svideo", buf))
-		new_type = OMAP_DSS_VENC_TYPE_SVIDEO;
-	else
-		return -EINVAL;
+	DSSDBG("venc_display_enable\n");
 
 	mutex_lock(&venc.venc_lock);
 
-	if (dssdev->phy.venc.type != new_type) {
-		dssdev->phy.venc.type = new_type;
-		if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
-			venc_power_off(dssdev);
-			venc_power_on(dssdev);
-		}
+	if (out == NULL || out->manager == NULL) {
+		DSSERR("Failed to enable display: no output/manager\n");
+		r = -ENODEV;
+		goto err0;
 	}
 
-	mutex_unlock(&venc.venc_lock);
-
-	return size;
-}
-
-static DEVICE_ATTR(output_type, S_IRUGO | S_IWUSR,
-		display_output_type_show, display_output_type_store);
-
-/* driver */
-static int venc_panel_probe(struct omap_dss_device *dssdev)
-{
-	dssdev->panel.timings = omap_dss_pal_timings;
-
-	return device_create_file(&dssdev->dev, &dev_attr_output_type);
-}
-
-static void venc_panel_remove(struct omap_dss_device *dssdev)
-{
-	device_remove_file(&dssdev->dev, &dev_attr_output_type);
-}
-
-static int venc_panel_enable(struct omap_dss_device *dssdev)
-{
-	int r = 0;
-
-	DSSDBG("venc_enable_display\n");
-
-	mutex_lock(&venc.venc_lock);
-
 	r = omap_dss_start_device(dssdev);
 	if (r) {
 		DSSERR("failed to start device\n");
 		goto err0;
 	}
 
-	if (dssdev->state != OMAP_DSS_DISPLAY_DISABLED) {
-		r = -EINVAL;
-		goto err1;
-	}
+	if (dssdev->platform_enable)
+		dssdev->platform_enable(dssdev);
 
-	r = venc_runtime_get();
-	if (r)
-		goto err1;
 
 	r = venc_power_on(dssdev);
 	if (r)
-		goto err2;
+		goto err1;
 
 	venc.wss_data = 0;
 
-	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
-
 	mutex_unlock(&venc.venc_lock);
+
 	return 0;
-err2:
-	venc_runtime_put();
 err1:
+	if (dssdev->platform_disable)
+		dssdev->platform_disable(dssdev);
 	omap_dss_stop_device(dssdev);
 err0:
 	mutex_unlock(&venc.venc_lock);
-
 	return r;
 }
 
-static void venc_panel_disable(struct omap_dss_device *dssdev)
+void omapdss_venc_display_disable(struct omap_dss_device *dssdev)
 {
-	DSSDBG("venc_disable_display\n");
+	DSSDBG("venc_display_disable\n");
 
 	mutex_lock(&venc.venc_lock);
 
-	if (dssdev->state == OMAP_DSS_DISPLAY_DISABLED)
-		goto end;
-
-	if (dssdev->state == OMAP_DSS_DISPLAY_SUSPENDED) {
-		/* suspended is the same as disabled with venc */
-		dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
-		goto end;
-	}
-
 	venc_power_off(dssdev);
 
-	venc_runtime_put();
-
-	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
-
 	omap_dss_stop_device(dssdev);
-end:
-	mutex_unlock(&venc.venc_lock);
-}
 
-static int venc_panel_suspend(struct omap_dss_device *dssdev)
-{
-	venc_panel_disable(dssdev);
-	return 0;
-}
+	if (dssdev->platform_disable)
+		dssdev->platform_disable(dssdev);
 
-static int venc_panel_resume(struct omap_dss_device *dssdev)
-{
-	return venc_panel_enable(dssdev);
+	mutex_unlock(&venc.venc_lock);
 }
 
-static void venc_set_timings(struct omap_dss_device *dssdev,
-			struct omap_video_timings *timings)
+void omapdss_venc_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
 {
 	DSSDBG("venc_set_timings\n");
 
+	mutex_lock(&venc.venc_lock);
+
 	/* Reset WSS data when the TV standard changes. */
-	if (memcmp(&dssdev->panel.timings, timings, sizeof(*timings)))
+	if (memcmp(&venc.timings, timings, sizeof(*timings)))
 		venc.wss_data = 0;
 
-	dssdev->panel.timings = *timings;
-	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
-		/* turn the venc off and on to get new timings to use */
-		venc_panel_disable(dssdev);
-		venc_panel_enable(dssdev);
-	} else {
-		dss_mgr_set_timings(dssdev->manager, timings);
-	}
+	venc.timings = *timings;
+
+	mutex_unlock(&venc.venc_lock);
 }
 
-static int venc_check_timings(struct omap_dss_device *dssdev,
-			struct omap_video_timings *timings)
+int omapdss_venc_check_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
 {
 	DSSDBG("venc_check_timings\n");
 
@@ -668,13 +587,13 @@ static int venc_check_timings(struct omap_dss_device *dssdev,
 	return -EINVAL;
 }
 
-static u32 venc_get_wss(struct omap_dss_device *dssdev)
+u32 omapdss_venc_get_wss(struct omap_dss_device *dssdev)
 {
 	/* Invert due to VENC_L21_WC_CTL:INV=1 */
 	return (venc.wss_data >> 8) ^ 0xfffff;
 }
 
-static int venc_set_wss(struct omap_dss_device *dssdev,	u32 wss)
+int omapdss_venc_set_wss(struct omap_dss_device *dssdev, u32 wss)
 {
 	const struct venc_config *config;
 	int r;
@@ -683,7 +602,7 @@ static int venc_set_wss(struct omap_dss_device *dssdev,	u32 wss)
 
 	mutex_lock(&venc.venc_lock);
 
-	config = venc_timings_to_config(&dssdev->panel.timings);
+	config = venc_timings_to_config(&venc.timings);
 
 	/* Invert due to VENC_L21_WC_CTL:INV=1 */
 	venc.wss_data = (wss ^ 0xfffff) << 8;
@@ -703,30 +622,25 @@ err:
 	return r;
 }
 
-static struct omap_dss_driver venc_driver = {
-	.probe		= venc_panel_probe,
-	.remove		= venc_panel_remove,
+void omapdss_venc_set_type(struct omap_dss_device *dssdev,
+		enum omap_dss_venc_type type)
+{
+	mutex_lock(&venc.venc_lock);
 
-	.enable		= venc_panel_enable,
-	.disable	= venc_panel_disable,
-	.suspend	= venc_panel_suspend,
-	.resume		= venc_panel_resume,
+	venc.type = type;
 
-	.get_resolution	= omapdss_default_get_resolution,
-	.get_recommended_bpp = omapdss_default_get_recommended_bpp,
+	mutex_unlock(&venc.venc_lock);
+}
 
-	.set_timings	= venc_set_timings,
-	.check_timings	= venc_check_timings,
+void omapdss_venc_invert_vid_out_polarity(struct omap_dss_device *dssdev,
+		bool invert_polarity)
+{
+	mutex_lock(&venc.venc_lock);
 
-	.get_wss	= venc_get_wss,
-	.set_wss	= venc_set_wss,
+	venc.invert_polarity = invert_polarity;
 
-	.driver         = {
-		.name   = "venc",
-		.owner  = THIS_MODULE,
-	},
-};
-/* driver end */
+	mutex_unlock(&venc.venc_lock);
+}
 
 static int __init venc_init_display(struct omap_dss_device *dssdev)
 {
@@ -752,11 +666,6 @@ static void venc_dump_regs(struct seq_file *s)
 {
 #define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, venc_read_reg(r))
 
-	if (cpu_is_omap44xx()) {
-		seq_printf(s, "VENC currently disabled on OMAP44xx\n");
-		return;
-	}
-
 	if (venc_runtime_get())
 		return;
 
@@ -832,10 +741,14 @@ static void venc_put_clocks(void)
 		clk_put(venc.tv_dac_clk);
 }
 
-static void __init venc_probe_pdata(struct platform_device *pdev)
+static struct omap_dss_device * __init venc_find_dssdev(struct platform_device *pdev)
 {
 	struct omap_dss_board_info *pdata = pdev->dev.platform_data;
-	int r, i;
+	const char *def_disp_name = dss_get_default_display_name();
+	struct omap_dss_device *def_dssdev;
+	int i;
+
+	def_dssdev = NULL;
 
 	for (i = 0; i < pdata->num_devices; ++i) {
 		struct omap_dss_device *dssdev = pdata->devices[i];
@@ -843,17 +756,69 @@ static void __init venc_probe_pdata(struct platform_device *pdev)
 		if (dssdev->type != OMAP_DISPLAY_TYPE_VENC)
 			continue;
 
-		r = venc_init_display(dssdev);
-		if (r) {
-			DSSERR("device %s init failed: %d\n", dssdev->name, r);
-			continue;
+		if (def_dssdev == NULL)
+			def_dssdev = dssdev;
+
+		if (def_disp_name != NULL &&
+				strcmp(dssdev->name, def_disp_name) == 0) {
+			def_dssdev = dssdev;
+			break;
 		}
+	}
+
+	return def_dssdev;
+}
+
+static void __init venc_probe_pdata(struct platform_device *vencdev)
+{
+	struct omap_dss_device *plat_dssdev;
+	struct omap_dss_device *dssdev;
+	int r;
+
+	plat_dssdev = venc_find_dssdev(vencdev);
 
-		r = omap_dss_register_device(dssdev, &pdev->dev, i);
-		if (r)
-			DSSERR("device %s register failed: %d\n",
-					dssdev->name, r);
+	if (!plat_dssdev)
+		return;
+
+	dssdev = dss_alloc_and_init_device(&vencdev->dev);
+	if (!dssdev)
+		return;
+
+	dss_copy_device_pdata(dssdev, plat_dssdev);
+
+	dssdev->channel = OMAP_DSS_CHANNEL_DIGIT;
+
+	r = venc_init_display(dssdev);
+	if (r) {
+		DSSERR("device %s init failed: %d\n", dssdev->name, r);
+		dss_put_device(dssdev);
+		return;
 	}
+
+	r = dss_add_device(dssdev);
+	if (r) {
+		DSSERR("device %s register failed: %d\n", dssdev->name, r);
+		dss_put_device(dssdev);
+		return;
+	}
+}
+
+static void __init venc_init_output(struct platform_device *pdev)
+{
+	struct omap_dss_output *out = &venc.output;
+
+	out->pdev = pdev;
+	out->id = OMAP_DSS_OUTPUT_VENC;
+	out->type = OMAP_DISPLAY_TYPE_VENC;
+
+	dss_register_output(out);
+}
+
+static void __exit venc_uninit_output(struct platform_device *pdev)
+{
+	struct omap_dss_output *out = &venc.output;
+
+	dss_unregister_output(out);
 }
 
 /* VENC HW IP initialisation */
@@ -897,17 +862,19 @@ static int __init omap_venchw_probe(struct platform_device *pdev)
 
 	venc_runtime_put();
 
-	r = omap_dss_register_driver(&venc_driver);
+	r = venc_panel_init();
 	if (r)
-		goto err_reg_panel_driver;
+		goto err_panel_init;
 
 	dss_debugfs_create_file("venc", venc_dump_regs);
 
+	venc_init_output(pdev);
+
 	venc_probe_pdata(pdev);
 
 	return 0;
 
-err_reg_panel_driver:
+err_panel_init:
 err_runtime_get:
 	pm_runtime_disable(&pdev->dev);
 	venc_put_clocks();
@@ -916,14 +883,16 @@ err_runtime_get:
 
 static int __exit omap_venchw_remove(struct platform_device *pdev)
 {
-	omap_dss_unregister_child_devices(&pdev->dev);
+	dss_unregister_child_devices(&pdev->dev);
 
 	if (venc.vdda_dac_reg != NULL) {
 		regulator_put(venc.vdda_dac_reg);
 		venc.vdda_dac_reg = NULL;
 	}
 
-	omap_dss_unregister_driver(&venc_driver);
+	venc_panel_exit();
+
+	venc_uninit_output(pdev);
 
 	pm_runtime_disable(&pdev->dev);
 	venc_put_clocks();
@@ -971,16 +940,10 @@ static struct platform_driver omap_venchw_driver = {
 
 int __init venc_init_platform_driver(void)
 {
-	if (cpu_is_omap44xx())
-		return 0;
-
 	return platform_driver_probe(&omap_venchw_driver, omap_venchw_probe);
 }
 
 void __exit venc_uninit_platform_driver(void)
 {
-	if (cpu_is_omap44xx())
-		return;
-
 	platform_driver_unregister(&omap_venchw_driver);
 }
diff --git a/drivers/video/omap2/dss/venc_panel.c b/drivers/video/omap2/dss/venc_panel.c
new file mode 100644
index 00000000000..d55b8784ecf
--- /dev/null
+++ b/drivers/video/omap2/dss/venc_panel.c
@@ -0,0 +1,251 @@
+/*
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * VENC panel 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.
+ *
+ * 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/err.h>
+#include <linux/io.h>
+#include <linux/mutex.h>
+#include <linux/module.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+
+static struct {
+	struct mutex lock;
+} venc_panel;
+
+static ssize_t display_output_type_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct omap_dss_device *dssdev = to_dss_device(dev);
+	const char *ret;
+
+	switch (dssdev->phy.venc.type) {
+	case OMAP_DSS_VENC_TYPE_COMPOSITE:
+		ret = "composite";
+		break;
+	case OMAP_DSS_VENC_TYPE_SVIDEO:
+		ret = "svideo";
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return snprintf(buf, PAGE_SIZE, "%s\n", ret);
+}
+
+static ssize_t display_output_type_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct omap_dss_device *dssdev = to_dss_device(dev);
+	enum omap_dss_venc_type new_type;
+
+	if (sysfs_streq("composite", buf))
+		new_type = OMAP_DSS_VENC_TYPE_COMPOSITE;
+	else if (sysfs_streq("svideo", buf))
+		new_type = OMAP_DSS_VENC_TYPE_SVIDEO;
+	else
+		return -EINVAL;
+
+	mutex_lock(&venc_panel.lock);
+
+	if (dssdev->phy.venc.type != new_type) {
+		dssdev->phy.venc.type = new_type;
+		omapdss_venc_set_type(dssdev, new_type);
+		if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
+			omapdss_venc_display_disable(dssdev);
+			omapdss_venc_display_enable(dssdev);
+		}
+	}
+
+	mutex_unlock(&venc_panel.lock);
+
+	return size;
+}
+
+static DEVICE_ATTR(output_type, S_IRUGO | S_IWUSR,
+		display_output_type_show, display_output_type_store);
+
+static int venc_panel_probe(struct omap_dss_device *dssdev)
+{
+	/* set default timings to PAL */
+	const struct omap_video_timings default_timings = {
+		.x_res		= 720,
+		.y_res		= 574,
+		.pixel_clock	= 13500,
+		.hsw		= 64,
+		.hfp		= 12,
+		.hbp		= 68,
+		.vsw		= 5,
+		.vfp		= 5,
+		.vbp		= 41,
+
+		.vsync_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+		.hsync_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+
+		.interlace	= true,
+	};
+
+	mutex_init(&venc_panel.lock);
+
+	dssdev->panel.timings = default_timings;
+
+	return device_create_file(&dssdev->dev, &dev_attr_output_type);
+}
+
+static void venc_panel_remove(struct omap_dss_device *dssdev)
+{
+	device_remove_file(&dssdev->dev, &dev_attr_output_type);
+}
+
+static int venc_panel_enable(struct omap_dss_device *dssdev)
+{
+	int r;
+
+	dev_dbg(&dssdev->dev, "venc_panel_enable\n");
+
+	mutex_lock(&venc_panel.lock);
+
+	if (dssdev->state != OMAP_DSS_DISPLAY_DISABLED) {
+		r = -EINVAL;
+		goto err;
+	}
+
+	omapdss_venc_set_timings(dssdev, &dssdev->panel.timings);
+	omapdss_venc_set_type(dssdev, dssdev->phy.venc.type);
+	omapdss_venc_invert_vid_out_polarity(dssdev,
+		dssdev->phy.venc.invert_polarity);
+
+	r = omapdss_venc_display_enable(dssdev);
+	if (r)
+		goto err;
+
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+	mutex_unlock(&venc_panel.lock);
+
+	return 0;
+err:
+	mutex_unlock(&venc_panel.lock);
+
+	return r;
+}
+
+static void venc_panel_disable(struct omap_dss_device *dssdev)
+{
+	dev_dbg(&dssdev->dev, "venc_panel_disable\n");
+
+	mutex_lock(&venc_panel.lock);
+
+	if (dssdev->state == OMAP_DSS_DISPLAY_DISABLED)
+		goto end;
+
+	if (dssdev->state == OMAP_DSS_DISPLAY_SUSPENDED) {
+		/* suspended is the same as disabled with venc */
+		dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+		goto end;
+	}
+
+	omapdss_venc_display_disable(dssdev);
+
+	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+end:
+	mutex_unlock(&venc_panel.lock);
+}
+
+static int venc_panel_suspend(struct omap_dss_device *dssdev)
+{
+	venc_panel_disable(dssdev);
+	return 0;
+}
+
+static int venc_panel_resume(struct omap_dss_device *dssdev)
+{
+	return venc_panel_enable(dssdev);
+}
+
+static void venc_panel_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	dev_dbg(&dssdev->dev, "venc_panel_set_timings\n");
+
+	mutex_lock(&venc_panel.lock);
+
+	omapdss_venc_set_timings(dssdev, timings);
+	dssdev->panel.timings = *timings;
+
+	mutex_unlock(&venc_panel.lock);
+}
+
+static int venc_panel_check_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	dev_dbg(&dssdev->dev, "venc_panel_check_timings\n");
+
+	return omapdss_venc_check_timings(dssdev, timings);
+}
+
+static u32 venc_panel_get_wss(struct omap_dss_device *dssdev)
+{
+	dev_dbg(&dssdev->dev, "venc_panel_get_wss\n");
+
+	return omapdss_venc_get_wss(dssdev);
+}
+
+static int venc_panel_set_wss(struct omap_dss_device *dssdev, u32 wss)
+{
+	dev_dbg(&dssdev->dev, "venc_panel_set_wss\n");
+
+	return omapdss_venc_set_wss(dssdev, wss);
+}
+
+static struct omap_dss_driver venc_driver = {
+	.probe		= venc_panel_probe,
+	.remove		= venc_panel_remove,
+
+	.enable		= venc_panel_enable,
+	.disable	= venc_panel_disable,
+	.suspend	= venc_panel_suspend,
+	.resume		= venc_panel_resume,
+
+	.get_resolution	= omapdss_default_get_resolution,
+	.get_recommended_bpp = omapdss_default_get_recommended_bpp,
+
+	.set_timings	= venc_panel_set_timings,
+	.check_timings	= venc_panel_check_timings,
+
+	.get_wss	= venc_panel_get_wss,
+	.set_wss	= venc_panel_set_wss,
+
+	.driver         = {
+		.name   = "venc",
+		.owner  = THIS_MODULE,
+	},
+};
+
+int venc_panel_init(void)
+{
+	return omap_dss_register_driver(&venc_driver);
+}
+
+void venc_panel_exit(void)
+{
+	omap_dss_unregister_driver(&venc_driver);
+}
diff --git a/drivers/video/omap2/omapfb/omapfb-ioctl.c b/drivers/video/omap2/omapfb/omapfb-ioctl.c
index c6cf372d22c..606b89f1235 100644
--- a/drivers/video/omap2/omapfb/omapfb-ioctl.c
+++ b/drivers/video/omap2/omapfb/omapfb-ioctl.c
@@ -599,6 +599,7 @@ int omapfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 	struct omapfb_info *ofbi = FB2OFB(fbi);
 	struct omapfb2_device *fbdev = ofbi->fbdev;
 	struct omap_dss_device *display = fb2display(fbi);
+	struct omap_overlay_manager *mgr;
 
 	union {
 		struct omapfb_update_window_old	uwnd_o;
@@ -786,12 +787,14 @@ int omapfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 
 	case OMAPFB_WAITFORVSYNC:
 		DBG("ioctl WAITFORVSYNC\n");
-		if (!display) {
+		if (!display && !display->output && !display->output->manager) {
 			r = -EINVAL;
 			break;
 		}
 
-		r = display->manager->wait_for_vsync(display->manager);
+		mgr = display->output->manager;
+
+		r = mgr->wait_for_vsync(mgr);
 		break;
 
 	case OMAPFB_WAITFORGO:
diff --git a/drivers/video/omap2/omapfb/omapfb-main.c b/drivers/video/omap2/omapfb/omapfb-main.c
index 3c39aa8de92..16db1589bd9 100644
--- a/drivers/video/omap2/omapfb/omapfb-main.c
+++ b/drivers/video/omap2/omapfb/omapfb-main.c
@@ -1128,7 +1128,7 @@ static int omapfb_mmap(struct fb_info *fbi, struct vm_area_struct *vma)
 	DBG("user mmap region start %lx, len %d, off %lx\n", start, len, off);
 
 	vma->vm_pgoff = off >> PAGE_SHIFT;
-	vma->vm_flags |= VM_IO | VM_RESERVED;
+	/* VM_IO | VM_DONTEXPAND | VM_DONTDUMP are set by remap_pfn_range() */
 	vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
 	vma->vm_ops = &mmap_user_ops;
 	vma->vm_private_data = rg;
@@ -1593,6 +1593,20 @@ static int omapfb_allocate_all_fbs(struct omapfb2_device *fbdev)
 	return 0;
 }
 
+static void omapfb_clear_fb(struct fb_info *fbi)
+{
+	const struct fb_fillrect rect = {
+		.dx = 0,
+		.dy = 0,
+		.width = fbi->var.xres_virtual,
+		.height = fbi->var.yres_virtual,
+		.color = 0,
+		.rop = ROP_COPY,
+	};
+
+	cfb_fillrect(fbi, &rect);
+}
+
 int omapfb_realloc_fbmem(struct fb_info *fbi, unsigned long size, int type)
 {
 	struct omapfb_info *ofbi = FB2OFB(fbi);
@@ -1662,6 +1676,8 @@ int omapfb_realloc_fbmem(struct fb_info *fbi, unsigned long size, int type)
 			goto err;
 	}
 
+	omapfb_clear_fb(fbi);
+
 	return 0;
 err:
 	omapfb_free_fbmem(fbi);
@@ -1946,6 +1962,16 @@ static int omapfb_create_framebuffers(struct omapfb2_device *fbdev)
 		}
 	}
 
+	for (i = 0; i < fbdev->num_fbs; i++) {
+		struct fb_info *fbi = fbdev->fbs[i];
+		struct omapfb_info *ofbi = FB2OFB(fbi);
+
+		if (ofbi->region->size == 0)
+			continue;
+
+		omapfb_clear_fb(fbi);
+	}
+
 	DBG("fb_infos initialized\n");
 
 	for (i = 0; i < fbdev->num_fbs; i++) {
@@ -2354,6 +2380,7 @@ static int __init omapfb_probe(struct platform_device *pdev)
 	struct omap_overlay *ovl;
 	struct omap_dss_device *def_display;
 	struct omap_dss_device *dssdev;
+	struct omap_dss_device *ovl_device;
 
 	DBG("omapfb_probe\n");
 
@@ -2427,8 +2454,9 @@ static int __init omapfb_probe(struct platform_device *pdev)
 	/* gfx overlay should be the default one. find a display
 	 * connected to that, and use it as default display */
 	ovl = omap_dss_get_overlay(0);
-	if (ovl->manager && ovl->manager->device) {
-		def_display = ovl->manager->device;
+	ovl_device = ovl->get_device(ovl);
+	if (ovl_device) {
+		def_display = ovl_device;
 	} else {
 		dev_warn(&pdev->dev, "cannot find default display\n");
 		def_display = NULL;
diff --git a/drivers/video/omap2/omapfb/omapfb.h b/drivers/video/omap2/omapfb/omapfb.h
index 30361a09aec..5ced9b334d3 100644
--- a/drivers/video/omap2/omapfb/omapfb.h
+++ b/drivers/video/omap2/omapfb/omapfb.h
@@ -148,8 +148,9 @@ static inline struct omap_dss_device *fb2display(struct fb_info *fbi)
 
 	/* XXX: returns the display connected to first attached overlay */
 	for (i = 0; i < ofbi->num_overlays; i++) {
-		if (ofbi->overlays[i]->manager)
-			return ofbi->overlays[i]->manager->device;
+		struct omap_overlay *ovl = ofbi->overlays[i];
+
+		return ovl->get_device(ovl);
 	}
 
 	return NULL;
diff --git a/drivers/video/omap2/vram.c b/drivers/video/omap2/vram.c
index 87e421e25af..f2b15c4a75b 100644
--- a/drivers/video/omap2/vram.c
+++ b/drivers/video/omap2/vram.c
@@ -34,7 +34,6 @@
 #include <asm/setup.h>
 
 #include <plat/vram.h>
-#include <plat/dma.h>
 
 #ifdef DEBUG
 #define DBG(format, ...) pr_debug("VRAM: " format, ## __VA_ARGS__)
@@ -250,59 +249,6 @@ int omap_vram_reserve(unsigned long paddr, size_t size)
 }
 EXPORT_SYMBOL(omap_vram_reserve);
 
-static void _omap_vram_dma_cb(int lch, u16 ch_status, void *data)
-{
-	struct completion *compl = data;
-	complete(compl);
-}
-
-static int _omap_vram_clear(u32 paddr, unsigned pages)
-{
-	struct completion compl;
-	unsigned elem_count;
-	unsigned frame_count;
-	int r;
-	int lch;
-
-	init_completion(&compl);
-
-	r = omap_request_dma(OMAP_DMA_NO_DEVICE, "VRAM DMA",
-			_omap_vram_dma_cb,
-			&compl, &lch);
-	if (r) {
-		pr_err("VRAM: request_dma failed for memory clear\n");
-		return -EBUSY;
-	}
-
-	elem_count = pages * PAGE_SIZE / 4;
-	frame_count = 1;
-
-	omap_set_dma_transfer_params(lch, OMAP_DMA_DATA_TYPE_S32,
-			elem_count, frame_count,
-			OMAP_DMA_SYNC_ELEMENT,
-			0, 0);
-
-	omap_set_dma_dest_params(lch, 0, OMAP_DMA_AMODE_POST_INC,
-			paddr, 0, 0);
-
-	omap_set_dma_color_mode(lch, OMAP_DMA_CONSTANT_FILL, 0x000000);
-
-	omap_start_dma(lch);
-
-	if (wait_for_completion_timeout(&compl, msecs_to_jiffies(1000)) == 0) {
-		omap_stop_dma(lch);
-		pr_err("VRAM: dma timeout while clearing memory\n");
-		r = -EIO;
-		goto err;
-	}
-
-	r = 0;
-err:
-	omap_free_dma(lch);
-
-	return r;
-}
-
 static int _omap_vram_alloc(unsigned pages, unsigned long *paddr)
 {
 	struct vram_region *rm;
@@ -337,8 +283,6 @@ found:
 
 		*paddr = start;
 
-		_omap_vram_clear(start, pages);
-
 		return 0;
 	}
 
diff --git a/drivers/video/pnx4008/Makefile b/drivers/video/pnx4008/Makefile
deleted file mode 100644
index 636aaccf01f..00000000000
--- a/drivers/video/pnx4008/Makefile
+++ /dev/null
@@ -1,7 +0,0 @@
-#
-# Makefile for the new PNX4008 framebuffer device driver
-#
-
-obj-$(CONFIG_FB_PNX4008_DUM) += sdum.o
-obj-$(CONFIG_FB_PNX4008_DUM_RGB) += pnxrgbfb.o
-
diff --git a/drivers/video/pnx4008/dum.h b/drivers/video/pnx4008/dum.h
deleted file mode 100644
index 1234d4375d9..00000000000
--- a/drivers/video/pnx4008/dum.h
+++ /dev/null
@@ -1,211 +0,0 @@
-/*
- * linux/drivers/video/pnx4008/dum.h
- *
- * Internal header for SDUM
- *
- * 2005 (c) Koninklijke Philips N.V. 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 __PNX008_DUM_H__
-#define __PNX008_DUM_H__
-
-#include <mach/platform.h>
-
-#define PNX4008_DUMCONF_VA_BASE		IO_ADDRESS(PNX4008_DUMCONF_BASE)
-#define PNX4008_DUM_MAIN_VA_BASE	IO_ADDRESS(PNX4008_DUM_MAINCFG_BASE)
-
-/* DUM CFG ADDRESSES */
-#define DUM_CH_BASE_ADR		(PNX4008_DUMCONF_VA_BASE + 0x00)
-#define DUM_CH_MIN_ADR		(PNX4008_DUMCONF_VA_BASE + 0x00)
-#define DUM_CH_MAX_ADR		(PNX4008_DUMCONF_VA_BASE + 0x04)
-#define DUM_CH_CONF_ADR		(PNX4008_DUMCONF_VA_BASE + 0x08)
-#define DUM_CH_STAT_ADR		(PNX4008_DUMCONF_VA_BASE + 0x0C)
-#define DUM_CH_CTRL_ADR		(PNX4008_DUMCONF_VA_BASE + 0x10)
-
-#define CH_MARG		(0x100 / sizeof(u32))
-#define DUM_CH_MIN(i)	(*((volatile u32 *)DUM_CH_MIN_ADR + (i) * CH_MARG))
-#define DUM_CH_MAX(i)	(*((volatile u32 *)DUM_CH_MAX_ADR + (i) * CH_MARG))
-#define DUM_CH_CONF(i)	(*((volatile u32 *)DUM_CH_CONF_ADR + (i) * CH_MARG))
-#define DUM_CH_STAT(i)	(*((volatile u32 *)DUM_CH_STAT_ADR + (i) * CH_MARG))
-#define DUM_CH_CTRL(i)	(*((volatile u32 *)DUM_CH_CTRL_ADR + (i) * CH_MARG))
-
-#define DUM_CONF_ADR          (PNX4008_DUM_MAIN_VA_BASE + 0x00)
-#define DUM_CTRL_ADR          (PNX4008_DUM_MAIN_VA_BASE + 0x04)
-#define DUM_STAT_ADR          (PNX4008_DUM_MAIN_VA_BASE + 0x08)
-#define DUM_DECODE_ADR        (PNX4008_DUM_MAIN_VA_BASE + 0x0C)
-#define DUM_COM_BASE_ADR      (PNX4008_DUM_MAIN_VA_BASE + 0x10)
-#define DUM_SYNC_C_ADR        (PNX4008_DUM_MAIN_VA_BASE + 0x14)
-#define DUM_CLK_DIV_ADR       (PNX4008_DUM_MAIN_VA_BASE + 0x18)
-#define DUM_DIRTY_LOW_ADR     (PNX4008_DUM_MAIN_VA_BASE + 0x20)
-#define DUM_DIRTY_HIGH_ADR    (PNX4008_DUM_MAIN_VA_BASE + 0x24)
-#define DUM_FORMAT_ADR        (PNX4008_DUM_MAIN_VA_BASE + 0x28)
-#define DUM_WTCFG1_ADR        (PNX4008_DUM_MAIN_VA_BASE + 0x30)
-#define DUM_RTCFG1_ADR        (PNX4008_DUM_MAIN_VA_BASE + 0x34)
-#define DUM_WTCFG2_ADR        (PNX4008_DUM_MAIN_VA_BASE + 0x38)
-#define DUM_RTCFG2_ADR        (PNX4008_DUM_MAIN_VA_BASE + 0x3C)
-#define DUM_TCFG_ADR          (PNX4008_DUM_MAIN_VA_BASE + 0x40)
-#define DUM_OUTP_FORMAT1_ADR  (PNX4008_DUM_MAIN_VA_BASE + 0x44)
-#define DUM_OUTP_FORMAT2_ADR  (PNX4008_DUM_MAIN_VA_BASE + 0x48)
-#define DUM_SYNC_MODE_ADR     (PNX4008_DUM_MAIN_VA_BASE + 0x4C)
-#define DUM_SYNC_OUT_C_ADR    (PNX4008_DUM_MAIN_VA_BASE + 0x50)
-
-#define DUM_CONF              (*(volatile u32 *)(DUM_CONF_ADR))
-#define DUM_CTRL              (*(volatile u32 *)(DUM_CTRL_ADR))
-#define DUM_STAT              (*(volatile u32 *)(DUM_STAT_ADR))
-#define DUM_DECODE            (*(volatile u32 *)(DUM_DECODE_ADR))
-#define DUM_COM_BASE          (*(volatile u32 *)(DUM_COM_BASE_ADR))
-#define DUM_SYNC_C            (*(volatile u32 *)(DUM_SYNC_C_ADR))
-#define DUM_CLK_DIV           (*(volatile u32 *)(DUM_CLK_DIV_ADR))
-#define DUM_DIRTY_LOW         (*(volatile u32 *)(DUM_DIRTY_LOW_ADR))
-#define DUM_DIRTY_HIGH        (*(volatile u32 *)(DUM_DIRTY_HIGH_ADR))
-#define DUM_FORMAT            (*(volatile u32 *)(DUM_FORMAT_ADR))
-#define DUM_WTCFG1            (*(volatile u32 *)(DUM_WTCFG1_ADR))
-#define DUM_RTCFG1            (*(volatile u32 *)(DUM_RTCFG1_ADR))
-#define DUM_WTCFG2            (*(volatile u32 *)(DUM_WTCFG2_ADR))
-#define DUM_RTCFG2            (*(volatile u32 *)(DUM_RTCFG2_ADR))
-#define DUM_TCFG              (*(volatile u32 *)(DUM_TCFG_ADR))
-#define DUM_OUTP_FORMAT1      (*(volatile u32 *)(DUM_OUTP_FORMAT1_ADR))
-#define DUM_OUTP_FORMAT2      (*(volatile u32 *)(DUM_OUTP_FORMAT2_ADR))
-#define DUM_SYNC_MODE         (*(volatile u32 *)(DUM_SYNC_MODE_ADR))
-#define DUM_SYNC_OUT_C        (*(volatile u32 *)(DUM_SYNC_OUT_C_ADR))
-
-/* DUM SLAVE ADDRESSES */
-#define DUM_SLAVE_WRITE_ADR      (PNX4008_DUM_MAINCFG_BASE + 0x0000000)
-#define DUM_SLAVE_READ1_I_ADR    (PNX4008_DUM_MAINCFG_BASE + 0x1000000)
-#define DUM_SLAVE_READ1_R_ADR    (PNX4008_DUM_MAINCFG_BASE + 0x1000004)
-#define DUM_SLAVE_READ2_I_ADR    (PNX4008_DUM_MAINCFG_BASE + 0x1000008)
-#define DUM_SLAVE_READ2_R_ADR    (PNX4008_DUM_MAINCFG_BASE + 0x100000C)
-
-#define DUM_SLAVE_WRITE_W  ((volatile u32 *)(DUM_SLAVE_WRITE_ADR))
-#define DUM_SLAVE_WRITE_HW ((volatile u16 *)(DUM_SLAVE_WRITE_ADR))
-#define DUM_SLAVE_READ1_I  ((volatile u8 *)(DUM_SLAVE_READ1_I_ADR))
-#define DUM_SLAVE_READ1_R  ((volatile u16 *)(DUM_SLAVE_READ1_R_ADR))
-#define DUM_SLAVE_READ2_I  ((volatile u8 *)(DUM_SLAVE_READ2_I_ADR))
-#define DUM_SLAVE_READ2_R  ((volatile u16 *)(DUM_SLAVE_READ2_R_ADR))
-
-/* Sony display register addresses */
-#define DISP_0_REG            (0x00)
-#define DISP_1_REG            (0x01)
-#define DISP_CAL_REG          (0x20)
-#define DISP_ID_REG           (0x2A)
-#define DISP_XMIN_L_REG       (0x30)
-#define DISP_XMIN_H_REG       (0x31)
-#define DISP_YMIN_REG         (0x32)
-#define DISP_XMAX_L_REG       (0x34)
-#define DISP_XMAX_H_REG       (0x35)
-#define DISP_YMAX_REG         (0x36)
-#define DISP_SYNC_EN_REG      (0x38)
-#define DISP_SYNC_RISE_L_REG  (0x3C)
-#define DISP_SYNC_RISE_H_REG  (0x3D)
-#define DISP_SYNC_FALL_L_REG  (0x3E)
-#define DISP_SYNC_FALL_H_REG  (0x3F)
-#define DISP_PIXEL_REG        (0x0B)
-#define DISP_DUMMY1_REG       (0x28)
-#define DISP_DUMMY2_REG       (0x29)
-#define DISP_TIMING_REG       (0x98)
-#define DISP_DUMP_REG         (0x99)
-
-/* Sony display constants */
-#define SONY_ID1              (0x22)
-#define SONY_ID2              (0x23)
-
-/* Philips display register addresses */
-#define PH_DISP_ORIENT_REG    (0x003)
-#define PH_DISP_YPOINT_REG    (0x200)
-#define PH_DISP_XPOINT_REG    (0x201)
-#define PH_DISP_PIXEL_REG     (0x202)
-#define PH_DISP_YMIN_REG      (0x406)
-#define PH_DISP_YMAX_REG      (0x407)
-#define PH_DISP_XMIN_REG      (0x408)
-#define PH_DISP_XMAX_REG      (0x409)
-
-/* Misc constants */
-#define NO_VALID_DISPLAY_FOUND      (0)
-#define DISPLAY2_IS_NOT_CONNECTED   (0)
-
-/* register values */
-#define V_BAC_ENABLE		(BIT(0))
-#define V_BAC_DISABLE_IDLE	(BIT(1))
-#define V_BAC_DISABLE_TRIG	(BIT(2))
-#define V_DUM_RESET		(BIT(3))
-#define V_MUX_RESET		(BIT(4))
-#define BAC_ENABLED		(BIT(0))
-#define BAC_DISABLED		0
-
-/* Sony LCD commands */
-#define V_LCD_STANDBY_OFF	((BIT(25)) | (0 << 16) | DISP_0_REG)
-#define V_LCD_USE_9BIT_BUS	((BIT(25)) | (2 << 16) | DISP_1_REG)
-#define V_LCD_SYNC_RISE_L	((BIT(25)) | (0 << 16) | DISP_SYNC_RISE_L_REG)
-#define V_LCD_SYNC_RISE_H	((BIT(25)) | (0 << 16) | DISP_SYNC_RISE_H_REG)
-#define V_LCD_SYNC_FALL_L	((BIT(25)) | (160 << 16) | DISP_SYNC_FALL_L_REG)
-#define V_LCD_SYNC_FALL_H	((BIT(25)) | (0 << 16) | DISP_SYNC_FALL_H_REG)
-#define V_LCD_SYNC_ENABLE	((BIT(25)) | (128 << 16) | DISP_SYNC_EN_REG)
-#define V_LCD_DISPLAY_ON	((BIT(25)) | (64 << 16) | DISP_0_REG)
-
-enum {
-	PAD_NONE,
-	PAD_512,
-	PAD_1024
-};
-
-enum {
-	RGB888,
-	RGB666,
-	RGB565,
-	BGR565,
-	ARGB1555,
-	ABGR1555,
-	ARGB4444,
-	ABGR4444
-};
-
-struct dum_setup {
-	int sync_neg_edge;
-	int round_robin;
-	int mux_int;
-	int synced_dirty_flag_int;
-	int dirty_flag_int;
-	int error_int;
-	int pf_empty_int;
-	int sf_empty_int;
-	int bac_dis_int;
-	u32 dirty_base_adr;
-	u32 command_base_adr;
-	u32 sync_clk_div;
-	int sync_output;
-	u32 sync_restart_val;
-	u32 set_sync_high;
-	u32 set_sync_low;
-};
-
-struct dum_ch_setup {
-	int disp_no;
-	u32 xmin;
-	u32 ymin;
-	u32 xmax;
-	u32 ymax;
-	int xmirror;
-	int ymirror;
-	int rotate;
-	u32 minadr;
-	u32 maxadr;
-	u32 dirtybuffer;
-	int pad;
-	int format;
-	int hwdirty;
-	int slave_trans;
-};
-
-struct disp_window {
-	u32 xmin_l;
-	u32 xmin_h;
-	u32 ymin;
-	u32 xmax_l;
-	u32 xmax_h;
-	u32 ymax;
-};
-
-#endif				/* #ifndef __PNX008_DUM_H__ */
diff --git a/drivers/video/pnx4008/fbcommon.h b/drivers/video/pnx4008/fbcommon.h
deleted file mode 100644
index 4ebc87dafaf..00000000000
--- a/drivers/video/pnx4008/fbcommon.h
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
- * Copyright (C) 2005 Philips Semiconductors
- *
- * 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, 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; see the file COPYING.  If not, write to
- * the Free Software Foundation, Inc., 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA, or http://www.gnu.org/licenses/gpl.html
-*/
-
-#define QCIF_W  (176)
-#define QCIF_H  (144)
-
-#define CIF_W   (352)
-#define CIF_H   (288)
-
-#define LCD_X_RES	208
-#define LCD_Y_RES	320
-#define LCD_X_PAD	256
-#define LCD_BBP		4	/* Bytes Per Pixel */
-
-#define DISP_MAX_X_SIZE     (320)
-#define DISP_MAX_Y_SIZE     (208)
-
-#define RETURNVAL_BASE (0x400)
-
-enum fb_ioctl_returntype {
-	ENORESOURCESLEFT = RETURNVAL_BASE,
-	ERESOURCESNOTFREED,
-	EPROCNOTOWNER,
-	EFBNOTOWNER,
-	ECOPYFAILED,
-	EIOREMAPFAILED,
-};
diff --git a/drivers/video/pnx4008/pnxrgbfb.c b/drivers/video/pnx4008/pnxrgbfb.c
deleted file mode 100644
index 6d30428e9cf..00000000000
--- a/drivers/video/pnx4008/pnxrgbfb.c
+++ /dev/null
@@ -1,198 +0,0 @@
-/*
- * drivers/video/pnx4008/pnxrgbfb.c
- *
- * PNX4008's framebuffer support
- *
- * Author: Grigory Tolstolytkin <gtolstolytkin@ru.mvista.com>
- * Based on Philips Semiconductors's code
- *
- * Copyrght (c) 2005 MontaVista Software, Inc.
- * Copyright (c) 2005 Philips Semiconductors
- * 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.
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/errno.h>
-#include <linux/string.h>
-#include <linux/mm.h>
-#include <linux/vmalloc.h>
-#include <linux/delay.h>
-#include <linux/interrupt.h>
-#include <linux/fb.h>
-#include <linux/init.h>
-#include <linux/platform_device.h>
-
-#include "sdum.h"
-#include "fbcommon.h"
-
-static u32 colreg[16];
-
-static struct fb_var_screeninfo rgbfb_var __initdata = {
-	.xres = LCD_X_RES,
-	.yres = LCD_Y_RES,
-	.xres_virtual = LCD_X_RES,
-	.yres_virtual = LCD_Y_RES,
-	.bits_per_pixel = 32,
-	.red.offset = 16,
-	.red.length = 8,
-	.green.offset = 8,
-	.green.length = 8,
-	.blue.offset = 0,
-	.blue.length = 8,
-	.left_margin = 0,
-	.right_margin = 0,
-	.upper_margin = 0,
-	.lower_margin = 0,
-	.vmode = FB_VMODE_NONINTERLACED,
-};
-static struct fb_fix_screeninfo rgbfb_fix __initdata = {
-	.id = "RGBFB",
-	.line_length = LCD_X_RES * LCD_BBP,
-	.type = FB_TYPE_PACKED_PIXELS,
-	.visual = FB_VISUAL_TRUECOLOR,
-	.xpanstep = 0,
-	.ypanstep = 0,
-	.ywrapstep = 0,
-	.accel = FB_ACCEL_NONE,
-};
-
-static int channel_owned;
-
-static int no_cursor(struct fb_info *info, struct fb_cursor *cursor)
-{
-	return 0;
-}
-
-static int rgbfb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
-			   u_int transp, struct fb_info *info)
-{
-	if (regno > 15)
-		return 1;
-
-	colreg[regno] = ((red & 0xff00) << 8) | (green & 0xff00) |
-	    ((blue & 0xff00) >> 8);
-	return 0;
-}
-
-static int rgbfb_mmap(struct fb_info *info, struct vm_area_struct *vma)
-{
-	return pnx4008_sdum_mmap(info, vma, NULL);
-}
-
-static struct fb_ops rgbfb_ops = {
-	.fb_mmap = rgbfb_mmap,
-	.fb_setcolreg = rgbfb_setcolreg,
-	.fb_fillrect = cfb_fillrect,
-	.fb_copyarea = cfb_copyarea,
-	.fb_imageblit = cfb_imageblit,
-};
-
-static int rgbfb_remove(struct platform_device *pdev)
-{
-	struct fb_info *info = platform_get_drvdata(pdev);
-
-	if (info) {
-		unregister_framebuffer(info);
-		fb_dealloc_cmap(&info->cmap);
-		framebuffer_release(info);
-		platform_set_drvdata(pdev, NULL);
-	}
-
-	pnx4008_free_dum_channel(channel_owned, pdev->id);
-	pnx4008_set_dum_exit_notification(pdev->id);
-
-	return 0;
-}
-
-static int __devinit rgbfb_probe(struct platform_device *pdev)
-{
-	struct fb_info *info;
-	struct dumchannel_uf chan_uf;
-	int ret;
-	char *option;
-
-	info = framebuffer_alloc(sizeof(u32) * 16, &pdev->dev);
-	if (!info) {
-		ret = -ENOMEM;
-		goto err;
-	}
-
-	pnx4008_get_fb_addresses(FB_TYPE_RGB, (void **)&info->screen_base,
-				 (dma_addr_t *) &rgbfb_fix.smem_start,
-				 &rgbfb_fix.smem_len);
-
-	if ((ret = pnx4008_alloc_dum_channel(pdev->id)) < 0)
-		goto err0;
-	else {
-		channel_owned = ret;
-		chan_uf.channelnr = channel_owned;
-		chan_uf.dirty = (u32 *) NULL;
-		chan_uf.source = (u32 *) rgbfb_fix.smem_start;
-		chan_uf.x_offset = 0;
-		chan_uf.y_offset = 0;
-		chan_uf.width = LCD_X_RES;
-		chan_uf.height = LCD_Y_RES;
-
-		if ((ret = pnx4008_put_dum_channel_uf(chan_uf, pdev->id))< 0)
-			goto err1;
-
-		if ((ret =
-		     pnx4008_set_dum_channel_sync(channel_owned, CONF_SYNC_ON,
-						  pdev->id)) < 0)
-			goto err1;
-
-		if ((ret =
-		     pnx4008_set_dum_channel_dirty_detect(channel_owned,
-							 CONF_DIRTYDETECTION_ON,
-							 pdev->id)) < 0)
-			goto err1;
-	}
-
-	if (!fb_get_options("pnxrgbfb", &option) && option &&
-			!strcmp(option, "nocursor"))
-		rgbfb_ops.fb_cursor = no_cursor;
-
-	info->node = -1;
-	info->flags = FBINFO_FLAG_DEFAULT;
-	info->fbops = &rgbfb_ops;
-	info->fix = rgbfb_fix;
-	info->var = rgbfb_var;
-	info->screen_size = rgbfb_fix.smem_len;
-	info->pseudo_palette = info->par;
-	info->par = NULL;
-
-	ret = fb_alloc_cmap(&info->cmap, 256, 0);
-	if (ret < 0)
-		goto err1;
-
-	ret = register_framebuffer(info);
-	if (ret < 0)
-		goto err2;
-	platform_set_drvdata(pdev, info);
-
-	return 0;
-
-err2:
-	fb_dealloc_cmap(&info->cmap);
-err1:
-	pnx4008_free_dum_channel(channel_owned, pdev->id);
-err0:
-	framebuffer_release(info);
-err:
-	return ret;
-}
-
-static struct platform_driver rgbfb_driver = {
-	.driver = {
-		.name = "pnx4008-rgbfb",
-	},
-	.probe = rgbfb_probe,
-	.remove = rgbfb_remove,
-};
-
-module_platform_driver(rgbfb_driver);
-
-MODULE_LICENSE("GPL");
diff --git a/drivers/video/pnx4008/sdum.c b/drivers/video/pnx4008/sdum.c
deleted file mode 100644
index c5c741452ca..00000000000
--- a/drivers/video/pnx4008/sdum.c
+++ /dev/null
@@ -1,861 +0,0 @@
-/*
- * drivers/video/pnx4008/sdum.c
- *
- * Display Update Master support
- *
- * Authors: Grigory Tolstolytkin <gtolstolytkin@ru.mvista.com>
- *          Vitaly Wool <vitalywool@gmail.com>
- * Based on Philips Semiconductors's code
- *
- * Copyrght (c) 2005-2006 MontaVista Software, Inc.
- * Copyright (c) 2005 Philips Semiconductors
- * 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.
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/errno.h>
-#include <linux/string.h>
-#include <linux/mm.h>
-#include <linux/tty.h>
-#include <linux/vmalloc.h>
-#include <linux/delay.h>
-#include <linux/interrupt.h>
-#include <linux/platform_device.h>
-#include <linux/fb.h>
-#include <linux/init.h>
-#include <linux/dma-mapping.h>
-#include <linux/clk.h>
-#include <linux/gfp.h>
-#include <asm/uaccess.h>
-#include <asm/gpio.h>
-
-#include "sdum.h"
-#include "fbcommon.h"
-#include "dum.h"
-
-/* Framebuffers we have */
-
-static struct pnx4008_fb_addr {
-	int fb_type;
-	long addr_offset;
-	long fb_length;
-} fb_addr[] = {
-	[0] = {
-		FB_TYPE_YUV, 0, 0xB0000
-	},
-	[1] = {
-		FB_TYPE_RGB, 0xB0000, 0x50000
-	},
-};
-
-static struct dum_data {
-	u32 lcd_phys_start;
-	u32 lcd_virt_start;
-	u32 slave_phys_base;
-	u32 *slave_virt_base;
-	int fb_owning_channel[MAX_DUM_CHANNELS];
-	struct dumchannel_uf chan_uf_store[MAX_DUM_CHANNELS];
-} dum_data;
-
-/* Different local helper functions */
-
-static u32 nof_pixels_dx(struct dum_ch_setup *ch_setup)
-{
-	return (ch_setup->xmax - ch_setup->xmin + 1);
-}
-
-static u32 nof_pixels_dy(struct dum_ch_setup *ch_setup)
-{
-	return (ch_setup->ymax - ch_setup->ymin + 1);
-}
-
-static u32 nof_pixels_dxy(struct dum_ch_setup *ch_setup)
-{
-	return (nof_pixels_dx(ch_setup) * nof_pixels_dy(ch_setup));
-}
-
-static u32 nof_bytes(struct dum_ch_setup *ch_setup)
-{
-	u32 r = nof_pixels_dxy(ch_setup);
-	switch (ch_setup->format) {
-	case RGB888:
-	case RGB666:
-		r *= 4;
-		break;
-
-	default:
-		r *= 2;
-		break;
-	}
-	return r;
-}
-
-static u32 build_command(int disp_no, u32 reg, u32 val)
-{
-	return ((disp_no << 26) | BIT(25) | (val << 16) | (disp_no << 10) |
-		(reg << 0));
-}
-
-static u32 build_double_index(int disp_no, u32 val)
-{
-	return ((disp_no << 26) | (val << 16) | (disp_no << 10) | (val << 0));
-}
-
-static void build_disp_window(struct dum_ch_setup * ch_setup, struct disp_window * dw)
-{
-	dw->ymin = ch_setup->ymin;
-	dw->ymax = ch_setup->ymax;
-	dw->xmin_l = ch_setup->xmin & 0xFF;
-	dw->xmin_h = (ch_setup->xmin & BIT(8)) >> 8;
-	dw->xmax_l = ch_setup->xmax & 0xFF;
-	dw->xmax_h = (ch_setup->xmax & BIT(8)) >> 8;
-}
-
-static int put_channel(struct dumchannel chan)
-{
-	int i = chan.channelnr;
-
-	if (i < 0 || i > MAX_DUM_CHANNELS)
-		return -EINVAL;
-	else {
-		DUM_CH_MIN(i) = chan.dum_ch_min;
-		DUM_CH_MAX(i) = chan.dum_ch_max;
-		DUM_CH_CONF(i) = chan.dum_ch_conf;
-		DUM_CH_CTRL(i) = chan.dum_ch_ctrl;
-	}
-
-	return 0;
-}
-
-static void clear_channel(int channr)
-{
-	struct dumchannel chan;
-
-	chan.channelnr = channr;
-	chan.dum_ch_min = 0;
-	chan.dum_ch_max = 0;
-	chan.dum_ch_conf = 0;
-	chan.dum_ch_ctrl = 0;
-
-	put_channel(chan);
-}
-
-static int put_cmd_string(struct cmdstring cmds)
-{
-	u16 *cmd_str_virtaddr;
-	u32 *cmd_ptr0_virtaddr;
-	u32 cmd_str_physaddr;
-
-	int i = cmds.channelnr;
-
-	if (i < 0 || i > MAX_DUM_CHANNELS)
-		return -EINVAL;
-	else if ((cmd_ptr0_virtaddr =
-		  (int *)ioremap_nocache(DUM_COM_BASE,
-					 sizeof(int) * MAX_DUM_CHANNELS)) ==
-		 NULL)
-		return -EIOREMAPFAILED;
-	else {
-		cmd_str_physaddr = ioread32(&cmd_ptr0_virtaddr[cmds.channelnr]);
-		if ((cmd_str_virtaddr =
-		     (u16 *) ioremap_nocache(cmd_str_physaddr,
-					     sizeof(cmds))) == NULL) {
-			iounmap(cmd_ptr0_virtaddr);
-			return -EIOREMAPFAILED;
-		} else {
-			int t;
-			for (t = 0; t < 8; t++)
-				iowrite16(*((u16 *)&cmds.prestringlen + t),
-					  cmd_str_virtaddr + t);
-
-			for (t = 0; t < cmds.prestringlen / 2; t++)
-				 iowrite16(*((u16 *)&cmds.precmd + t),
-					   cmd_str_virtaddr + t + 8);
-
-			for (t = 0; t < cmds.poststringlen / 2; t++)
-				iowrite16(*((u16 *)&cmds.postcmd + t),
-					  cmd_str_virtaddr + t + 8 +
-					  	cmds.prestringlen / 2);
-
-			iounmap(cmd_ptr0_virtaddr);
-			iounmap(cmd_str_virtaddr);
-		}
-	}
-
-	return 0;
-}
-
-static u32 dum_ch_setup(int ch_no, struct dum_ch_setup * ch_setup)
-{
-	struct cmdstring cmds_c;
-	struct cmdstring *cmds = &cmds_c;
-	struct disp_window dw;
-	int standard;
-	u32 orientation = 0;
-	struct dumchannel chan = { 0 };
-	int ret;
-
-	if ((ch_setup->xmirror) || (ch_setup->ymirror) || (ch_setup->rotate)) {
-		standard = 0;
-
-		orientation = BIT(1);	/* always set 9-bit-bus */
-		if (ch_setup->xmirror)
-			orientation |= BIT(4);
-		if (ch_setup->ymirror)
-			orientation |= BIT(3);
-		if (ch_setup->rotate)
-			orientation |= BIT(0);
-	} else
-		standard = 1;
-
-	cmds->channelnr = ch_no;
-
-	/* build command string header */
-	if (standard) {
-		cmds->prestringlen = 32;
-		cmds->poststringlen = 0;
-	} else {
-		cmds->prestringlen = 48;
-		cmds->poststringlen = 16;
-	}
-
-	cmds->format =
-	    (u16) ((ch_setup->disp_no << 4) | (BIT(3)) | (ch_setup->format));
-	cmds->reserved = 0x0;
-	cmds->startaddr_low = (ch_setup->minadr & 0xFFFF);
-	cmds->startaddr_high = (ch_setup->minadr >> 16);
-
-	if ((ch_setup->minadr == 0) && (ch_setup->maxadr == 0)
-	    && (ch_setup->xmin == 0)
-	    && (ch_setup->ymin == 0) && (ch_setup->xmax == 0)
-	    && (ch_setup->ymax == 0)) {
-		cmds->pixdatlen_low = 0;
-		cmds->pixdatlen_high = 0;
-	} else {
-		u32 nbytes = nof_bytes(ch_setup);
-		cmds->pixdatlen_low = (nbytes & 0xFFFF);
-		cmds->pixdatlen_high = (nbytes >> 16);
-	}
-
-	if (ch_setup->slave_trans)
-		cmds->pixdatlen_high |= BIT(15);
-
-	/* build pre-string */
-	build_disp_window(ch_setup, &dw);
-
-	if (standard) {
-		cmds->precmd[0] =
-		    build_command(ch_setup->disp_no, DISP_XMIN_L_REG, 0x99);
-		cmds->precmd[1] =
-		    build_command(ch_setup->disp_no, DISP_XMIN_L_REG,
-				  dw.xmin_l);
-		cmds->precmd[2] =
-		    build_command(ch_setup->disp_no, DISP_XMIN_H_REG,
-				  dw.xmin_h);
-		cmds->precmd[3] =
-		    build_command(ch_setup->disp_no, DISP_YMIN_REG, dw.ymin);
-		cmds->precmd[4] =
-		    build_command(ch_setup->disp_no, DISP_XMAX_L_REG,
-				  dw.xmax_l);
-		cmds->precmd[5] =
-		    build_command(ch_setup->disp_no, DISP_XMAX_H_REG,
-				  dw.xmax_h);
-		cmds->precmd[6] =
-		    build_command(ch_setup->disp_no, DISP_YMAX_REG, dw.ymax);
-		cmds->precmd[7] =
-		    build_double_index(ch_setup->disp_no, DISP_PIXEL_REG);
-	} else {
-		if (dw.xmin_l == ch_no)
-			cmds->precmd[0] =
-			    build_command(ch_setup->disp_no, DISP_XMIN_L_REG,
-					  0x99);
-		else
-			cmds->precmd[0] =
-			    build_command(ch_setup->disp_no, DISP_XMIN_L_REG,
-					  ch_no);
-
-		cmds->precmd[1] =
-		    build_command(ch_setup->disp_no, DISP_XMIN_L_REG,
-				  dw.xmin_l);
-		cmds->precmd[2] =
-		    build_command(ch_setup->disp_no, DISP_XMIN_H_REG,
-				  dw.xmin_h);
-		cmds->precmd[3] =
-		    build_command(ch_setup->disp_no, DISP_YMIN_REG, dw.ymin);
-		cmds->precmd[4] =
-		    build_command(ch_setup->disp_no, DISP_XMAX_L_REG,
-				  dw.xmax_l);
-		cmds->precmd[5] =
-		    build_command(ch_setup->disp_no, DISP_XMAX_H_REG,
-				  dw.xmax_h);
-		cmds->precmd[6] =
-		    build_command(ch_setup->disp_no, DISP_YMAX_REG, dw.ymax);
-		cmds->precmd[7] =
-		    build_command(ch_setup->disp_no, DISP_1_REG, orientation);
-		cmds->precmd[8] =
-		    build_double_index(ch_setup->disp_no, DISP_PIXEL_REG);
-		cmds->precmd[9] =
-		    build_double_index(ch_setup->disp_no, DISP_PIXEL_REG);
-		cmds->precmd[0xA] =
-		    build_double_index(ch_setup->disp_no, DISP_PIXEL_REG);
-		cmds->precmd[0xB] =
-		    build_double_index(ch_setup->disp_no, DISP_PIXEL_REG);
-		cmds->postcmd[0] =
-		    build_command(ch_setup->disp_no, DISP_1_REG, BIT(1));
-		cmds->postcmd[1] =
-		    build_command(ch_setup->disp_no, DISP_DUMMY1_REG, 1);
-		cmds->postcmd[2] =
-		    build_command(ch_setup->disp_no, DISP_DUMMY1_REG, 2);
-		cmds->postcmd[3] =
-		    build_command(ch_setup->disp_no, DISP_DUMMY1_REG, 3);
-	}
-
-	if ((ret = put_cmd_string(cmds_c)) != 0) {
-		return ret;
-	}
-
-	chan.channelnr = cmds->channelnr;
-	chan.dum_ch_min = ch_setup->dirtybuffer + ch_setup->minadr;
-	chan.dum_ch_max = ch_setup->dirtybuffer + ch_setup->maxadr;
-	chan.dum_ch_conf = 0x002;
-	chan.dum_ch_ctrl = 0x04;
-
-	put_channel(chan);
-
-	return 0;
-}
-
-static u32 display_open(int ch_no, int auto_update, u32 * dirty_buffer,
-			u32 * frame_buffer, u32 xpos, u32 ypos, u32 w, u32 h)
-{
-
-	struct dum_ch_setup k;
-	int ret;
-
-	/* keep width & height within display area */
-	if ((xpos + w) > DISP_MAX_X_SIZE)
-		w = DISP_MAX_X_SIZE - xpos;
-
-	if ((ypos + h) > DISP_MAX_Y_SIZE)
-		h = DISP_MAX_Y_SIZE - ypos;
-
-	/* assume 1 display only */
-	k.disp_no = 0;
-	k.xmin = xpos;
-	k.ymin = ypos;
-	k.xmax = xpos + (w - 1);
-	k.ymax = ypos + (h - 1);
-
-	/* adjust min and max values if necessary */
-	if (k.xmin > DISP_MAX_X_SIZE - 1)
-		k.xmin = DISP_MAX_X_SIZE - 1;
-	if (k.ymin > DISP_MAX_Y_SIZE - 1)
-		k.ymin = DISP_MAX_Y_SIZE - 1;
-
-	if (k.xmax > DISP_MAX_X_SIZE - 1)
-		k.xmax = DISP_MAX_X_SIZE - 1;
-	if (k.ymax > DISP_MAX_Y_SIZE - 1)
-		k.ymax = DISP_MAX_Y_SIZE - 1;
-
-	k.xmirror = 0;
-	k.ymirror = 0;
-	k.rotate = 0;
-	k.minadr = (u32) frame_buffer;
-	k.maxadr = (u32) frame_buffer + (((w - 1) << 10) | ((h << 2) - 2));
-	k.pad = PAD_1024;
-	k.dirtybuffer = (u32) dirty_buffer;
-	k.format = RGB888;
-	k.hwdirty = 0;
-	k.slave_trans = 0;
-
-	ret = dum_ch_setup(ch_no, &k);
-
-	return ret;
-}
-
-static void lcd_reset(void)
-{
-	u32 *dum_pio_base = (u32 *)IO_ADDRESS(PNX4008_PIO_BASE);
-
-	udelay(1);
-	iowrite32(BIT(19), &dum_pio_base[2]);
-	udelay(1);
-	iowrite32(BIT(19), &dum_pio_base[1]);
-	udelay(1);
-}
-
-static int dum_init(struct platform_device *pdev)
-{
-	struct clk *clk;
-
-	/* enable DUM clock */
-	clk = clk_get(&pdev->dev, "dum_ck");
-	if (IS_ERR(clk)) {
-		printk(KERN_ERR "pnx4008_dum: Unable to access DUM clock\n");
-		return PTR_ERR(clk);
-	}
-
-	clk_set_rate(clk, 1);
-	clk_put(clk);
-
-	DUM_CTRL = V_DUM_RESET;
-
-	/* set priority to "round-robin". All other params to "false" */
-	DUM_CONF = BIT(9);
-
-	/* Display 1 */
-	DUM_WTCFG1 = PNX4008_DUM_WT_CFG;
-	DUM_RTCFG1 = PNX4008_DUM_RT_CFG;
-	DUM_TCFG = PNX4008_DUM_T_CFG;
-
-	return 0;
-}
-
-static void dum_chan_init(void)
-{
-	int i = 0, ch = 0;
-	u32 *cmdptrs;
-	u32 *cmdstrings;
-
-	DUM_COM_BASE =
-		CMDSTRING_BASEADDR + BYTES_PER_CMDSTRING * NR_OF_CMDSTRINGS;
-
-	if ((cmdptrs =
-	     (u32 *) ioremap_nocache(DUM_COM_BASE,
-				     sizeof(u32) * NR_OF_CMDSTRINGS)) == NULL)
-		return;
-
-	for (ch = 0; ch < NR_OF_CMDSTRINGS; ch++)
-		iowrite32(CMDSTRING_BASEADDR + BYTES_PER_CMDSTRING * ch,
-			  cmdptrs + ch);
-
-	for (ch = 0; ch < MAX_DUM_CHANNELS; ch++)
-		clear_channel(ch);
-
-	/* Clear the cmdstrings */
-	cmdstrings =
-	    (u32 *)ioremap_nocache(*cmdptrs,
-				   BYTES_PER_CMDSTRING * NR_OF_CMDSTRINGS);
-
-	if (!cmdstrings)
-		goto out;
-
-	for (i = 0; i < NR_OF_CMDSTRINGS * BYTES_PER_CMDSTRING / sizeof(u32);
-	     i++)
-		iowrite32(0, cmdstrings + i);
-
-	iounmap((u32 *)cmdstrings);
-
-out:
-	iounmap((u32 *)cmdptrs);
-}
-
-static void lcd_init(void)
-{
-	lcd_reset();
-
-	DUM_OUTP_FORMAT1 = 0; /* RGB666 */
-
-	udelay(1);
-	iowrite32(V_LCD_STANDBY_OFF, dum_data.slave_virt_base);
-	udelay(1);
-	iowrite32(V_LCD_USE_9BIT_BUS, dum_data.slave_virt_base);
-	udelay(1);
-	iowrite32(V_LCD_SYNC_RISE_L, dum_data.slave_virt_base);
-	udelay(1);
-	iowrite32(V_LCD_SYNC_RISE_H, dum_data.slave_virt_base);
-	udelay(1);
-	iowrite32(V_LCD_SYNC_FALL_L, dum_data.slave_virt_base);
-	udelay(1);
-	iowrite32(V_LCD_SYNC_FALL_H, dum_data.slave_virt_base);
-	udelay(1);
-	iowrite32(V_LCD_SYNC_ENABLE, dum_data.slave_virt_base);
-	udelay(1);
-	iowrite32(V_LCD_DISPLAY_ON, dum_data.slave_virt_base);
-	udelay(1);
-}
-
-/* Interface exported to framebuffer drivers */
-
-int pnx4008_get_fb_addresses(int fb_type, void **virt_addr,
-			     dma_addr_t *phys_addr, int *fb_length)
-{
-	int i;
-	int ret = -1;
-	for (i = 0; i < ARRAY_SIZE(fb_addr); i++)
-		if (fb_addr[i].fb_type == fb_type) {
-			*virt_addr = (void *)(dum_data.lcd_virt_start +
-					fb_addr[i].addr_offset);
-			*phys_addr =
-			    dum_data.lcd_phys_start + fb_addr[i].addr_offset;
-			*fb_length = fb_addr[i].fb_length;
-			ret = 0;
-			break;
-		}
-
-	return ret;
-}
-
-EXPORT_SYMBOL(pnx4008_get_fb_addresses);
-
-int pnx4008_alloc_dum_channel(int dev_id)
-{
-	int i = 0;
-
-	while ((i < MAX_DUM_CHANNELS) && (dum_data.fb_owning_channel[i] != -1))
-		i++;
-
-	if (i == MAX_DUM_CHANNELS)
-		return -ENORESOURCESLEFT;
-	else {
-		dum_data.fb_owning_channel[i] = dev_id;
-		return i;
-	}
-}
-
-EXPORT_SYMBOL(pnx4008_alloc_dum_channel);
-
-int pnx4008_free_dum_channel(int channr, int dev_id)
-{
-	if (channr < 0 || channr > MAX_DUM_CHANNELS)
-		return -EINVAL;
-	else if (dum_data.fb_owning_channel[channr] != dev_id)
-		return -EFBNOTOWNER;
-	else {
-		clear_channel(channr);
-		dum_data.fb_owning_channel[channr] = -1;
-	}
-
-	return 0;
-}
-
-EXPORT_SYMBOL(pnx4008_free_dum_channel);
-
-int pnx4008_put_dum_channel_uf(struct dumchannel_uf chan_uf, int dev_id)
-{
-	int i = chan_uf.channelnr;
-	int ret;
-
-	if (i < 0 || i > MAX_DUM_CHANNELS)
-		return -EINVAL;
-	else if (dum_data.fb_owning_channel[i] != dev_id)
-		return -EFBNOTOWNER;
-	else if ((ret =
-		  display_open(chan_uf.channelnr, 0, chan_uf.dirty,
-			       chan_uf.source, chan_uf.y_offset,
-			       chan_uf.x_offset, chan_uf.height,
-			       chan_uf.width)) != 0)
-		return ret;
-	else {
-		dum_data.chan_uf_store[i].dirty = chan_uf.dirty;
-		dum_data.chan_uf_store[i].source = chan_uf.source;
-		dum_data.chan_uf_store[i].x_offset = chan_uf.x_offset;
-		dum_data.chan_uf_store[i].y_offset = chan_uf.y_offset;
-		dum_data.chan_uf_store[i].width = chan_uf.width;
-		dum_data.chan_uf_store[i].height = chan_uf.height;
-	}
-
-	return 0;
-}
-
-EXPORT_SYMBOL(pnx4008_put_dum_channel_uf);
-
-int pnx4008_set_dum_channel_sync(int channr, int val, int dev_id)
-{
-	if (channr < 0 || channr > MAX_DUM_CHANNELS)
-		return -EINVAL;
-	else if (dum_data.fb_owning_channel[channr] != dev_id)
-		return -EFBNOTOWNER;
-	else {
-		if (val == CONF_SYNC_ON) {
-			DUM_CH_CONF(channr) |= CONF_SYNCENABLE;
-			DUM_CH_CONF(channr) |= DUM_CHANNEL_CFG_SYNC_MASK |
-				DUM_CHANNEL_CFG_SYNC_MASK_SET;
-		} else if (val == CONF_SYNC_OFF)
-			DUM_CH_CONF(channr) &= ~CONF_SYNCENABLE;
-		else
-			return -EINVAL;
-	}
-
-	return 0;
-}
-
-EXPORT_SYMBOL(pnx4008_set_dum_channel_sync);
-
-int pnx4008_set_dum_channel_dirty_detect(int channr, int val, int dev_id)
-{
-	if (channr < 0 || channr > MAX_DUM_CHANNELS)
-		return -EINVAL;
-	else if (dum_data.fb_owning_channel[channr] != dev_id)
-		return -EFBNOTOWNER;
-	else {
-		if (val == CONF_DIRTYDETECTION_ON)
-			DUM_CH_CONF(channr) |= CONF_DIRTYENABLE;
-		else if (val == CONF_DIRTYDETECTION_OFF)
-			DUM_CH_CONF(channr) &= ~CONF_DIRTYENABLE;
-		else
-			return -EINVAL;
-	}
-
-	return 0;
-}
-
-EXPORT_SYMBOL(pnx4008_set_dum_channel_dirty_detect);
-
-#if 0 /* Functions not used currently, but likely to be used in future */
-
-static int get_channel(struct dumchannel *p_chan)
-{
-	int i = p_chan->channelnr;
-
-	if (i < 0 || i > MAX_DUM_CHANNELS)
-		return -EINVAL;
-	else {
-		p_chan->dum_ch_min = DUM_CH_MIN(i);
-		p_chan->dum_ch_max = DUM_CH_MAX(i);
-		p_chan->dum_ch_conf = DUM_CH_CONF(i);
-		p_chan->dum_ch_stat = DUM_CH_STAT(i);
-		p_chan->dum_ch_ctrl = 0;	/* WriteOnly control register */
-	}
-
-	return 0;
-}
-
-int pnx4008_get_dum_channel_uf(struct dumchannel_uf *p_chan_uf, int dev_id)
-{
-	int i = p_chan_uf->channelnr;
-
-	if (i < 0 || i > MAX_DUM_CHANNELS)
-		return -EINVAL;
-	else if (dum_data.fb_owning_channel[i] != dev_id)
-		return -EFBNOTOWNER;
-	else {
-		p_chan_uf->dirty = dum_data.chan_uf_store[i].dirty;
-		p_chan_uf->source = dum_data.chan_uf_store[i].source;
-		p_chan_uf->x_offset = dum_data.chan_uf_store[i].x_offset;
-		p_chan_uf->y_offset = dum_data.chan_uf_store[i].y_offset;
-		p_chan_uf->width = dum_data.chan_uf_store[i].width;
-		p_chan_uf->height = dum_data.chan_uf_store[i].height;
-	}
-
-	return 0;
-}
-
-EXPORT_SYMBOL(pnx4008_get_dum_channel_uf);
-
-int pnx4008_get_dum_channel_config(int channr, int dev_id)
-{
-	int ret;
-	struct dumchannel chan;
-
-	if (channr < 0 || channr > MAX_DUM_CHANNELS)
-		return -EINVAL;
-	else if (dum_data.fb_owning_channel[channr] != dev_id)
-		return -EFBNOTOWNER;
-	else {
-		chan.channelnr = channr;
-		if ((ret = get_channel(&chan)) != 0)
-			return ret;
-	}
-
-	return (chan.dum_ch_conf & DUM_CHANNEL_CFG_MASK);
-}
-
-EXPORT_SYMBOL(pnx4008_get_dum_channel_config);
-
-int pnx4008_force_update_dum_channel(int channr, int dev_id)
-{
-	if (channr < 0 || channr > MAX_DUM_CHANNELS)
-		return -EINVAL;
-
-	else if (dum_data.fb_owning_channel[channr] != dev_id)
-		return -EFBNOTOWNER;
-	else
-		DUM_CH_CTRL(channr) = CTRL_SETDIRTY;
-
-	return 0;
-}
-
-EXPORT_SYMBOL(pnx4008_force_update_dum_channel);
-
-#endif
-
-int pnx4008_sdum_mmap(struct fb_info *info, struct vm_area_struct *vma,
-		      struct device *dev)
-{
-	unsigned long off = vma->vm_pgoff << PAGE_SHIFT;
-
-	if (off < info->fix.smem_len) {
-		vma->vm_pgoff += 1;
-		return dma_mmap_writecombine(dev, vma,
-				(void *)dum_data.lcd_virt_start,
-				dum_data.lcd_phys_start,
-				FB_DMA_SIZE);
-	}
-	return -EINVAL;
-}
-
-EXPORT_SYMBOL(pnx4008_sdum_mmap);
-
-int pnx4008_set_dum_exit_notification(int dev_id)
-{
-	int i;
-
-	for (i = 0; i < MAX_DUM_CHANNELS; i++)
-		if (dum_data.fb_owning_channel[i] == dev_id)
-			return -ERESOURCESNOTFREED;
-
-	return 0;
-}
-
-EXPORT_SYMBOL(pnx4008_set_dum_exit_notification);
-
-/* Platform device driver for DUM */
-
-static int sdum_suspend(struct platform_device *pdev, pm_message_t state)
-{
-	int retval = 0;
-	struct clk *clk;
-
-	clk = clk_get(0, "dum_ck");
-	if (!IS_ERR(clk)) {
-		clk_set_rate(clk, 0);
-		clk_put(clk);
-	} else
-		retval = PTR_ERR(clk);
-
-	/* disable BAC */
-	DUM_CTRL = V_BAC_DISABLE_IDLE;
-
-	/* LCD standby & turn off display */
-	lcd_reset();
-
-	return retval;
-}
-
-static int sdum_resume(struct platform_device *pdev)
-{
-	int retval = 0;
-	struct clk *clk;
-
-	clk = clk_get(0, "dum_ck");
-	if (!IS_ERR(clk)) {
-		clk_set_rate(clk, 1);
-		clk_put(clk);
-	} else
-		retval = PTR_ERR(clk);
-
-	/* wait for BAC disable */
-	DUM_CTRL = V_BAC_DISABLE_TRIG;
-
-	while (DUM_CTRL & BAC_ENABLED)
-		udelay(10);
-
-	/* re-init LCD */
-	lcd_init();
-
-	/* enable BAC and reset MUX */
-	DUM_CTRL = V_BAC_ENABLE;
-	udelay(1);
-	DUM_CTRL = V_MUX_RESET;
-	return 0;
-}
-
-static int __devinit sdum_probe(struct platform_device *pdev)
-{
-	int ret = 0, i = 0;
-
-	/* map frame buffer */
-	dum_data.lcd_virt_start = (u32) dma_alloc_writecombine(&pdev->dev,
-						       FB_DMA_SIZE,
-						       &dum_data.lcd_phys_start,
-						       GFP_KERNEL);
-
-	if (!dum_data.lcd_virt_start) {
-		ret = -ENOMEM;
-		goto out_3;
-	}
-
-	/* map slave registers */
-	dum_data.slave_phys_base = PNX4008_DUM_SLAVE_BASE;
-	dum_data.slave_virt_base =
-	    (u32 *) ioremap_nocache(dum_data.slave_phys_base, sizeof(u32));
-
-	if (dum_data.slave_virt_base == NULL) {
-		ret = -ENOMEM;
-		goto out_2;
-	}
-
-	/* initialize DUM and LCD display */
-	ret = dum_init(pdev);
-	if (ret)
-		goto out_1;
-
-	dum_chan_init();
-	lcd_init();
-
-	DUM_CTRL = V_BAC_ENABLE;
-	udelay(1);
-	DUM_CTRL = V_MUX_RESET;
-
-	/* set decode address and sync clock divider */
-	DUM_DECODE = dum_data.lcd_phys_start & DUM_DECODE_MASK;
-	DUM_CLK_DIV = PNX4008_DUM_CLK_DIV;
-
-	for (i = 0; i < MAX_DUM_CHANNELS; i++)
-		dum_data.fb_owning_channel[i] = -1;
-
-	/*setup wakeup interrupt */
-	start_int_set_rising_edge(SE_DISP_SYNC_INT);
-	start_int_ack(SE_DISP_SYNC_INT);
-	start_int_umask(SE_DISP_SYNC_INT);
-
-	return 0;
-
-out_1:
-	iounmap((void *)dum_data.slave_virt_base);
-out_2:
-	dma_free_writecombine(&pdev->dev, FB_DMA_SIZE,
-			(void *)dum_data.lcd_virt_start,
-			dum_data.lcd_phys_start);
-out_3:
-	return ret;
-}
-
-static int sdum_remove(struct platform_device *pdev)
-{
-	struct clk *clk;
-
-	start_int_mask(SE_DISP_SYNC_INT);
-
-	clk = clk_get(0, "dum_ck");
-	if (!IS_ERR(clk)) {
-		clk_set_rate(clk, 0);
-		clk_put(clk);
-	}
-
-	iounmap((void *)dum_data.slave_virt_base);
-
-	dma_free_writecombine(&pdev->dev, FB_DMA_SIZE,
-			(void *)dum_data.lcd_virt_start,
-			dum_data.lcd_phys_start);
-
-	return 0;
-}
-
-static struct platform_driver sdum_driver = {
-	.driver = {
-		.name = "pnx4008-sdum",
-	},
-	.probe = sdum_probe,
-	.remove = sdum_remove,
-	.suspend = sdum_suspend,
-	.resume = sdum_resume,
-};
-
-module_platform_driver(sdum_driver);
-
-MODULE_LICENSE("GPL");
diff --git a/drivers/video/pnx4008/sdum.h b/drivers/video/pnx4008/sdum.h
deleted file mode 100644
index 189c3d64138..00000000000
--- a/drivers/video/pnx4008/sdum.h
+++ /dev/null
@@ -1,136 +0,0 @@
-/*
- * Copyright (C) 2005 Philips Semiconductors
- *
- * 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, 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; see the file COPYING.  If not, write to
- * the Free Software Foundation, Inc., 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA, or http://www.gnu.org/licenses/gpl.html
-*/
-
-#define MAX_DUM_CHANNELS	64
-
-#define RGB_MEM_WINDOW(x) (0x10000000 + (x)*0x00100000)
-
-#define QCIF_OFFSET(x) (((x) == 0) ? 0x00000: ((x) == 1) ? 0x30000: -1)
-#define CIF_OFFSET(x)  (((x) == 0) ? 0x00000: ((x) == 1) ? 0x60000: -1)
-
-#define CTRL_SETDIRTY	 	(0x00000001)
-#define CONF_DIRTYENABLE	(0x00000020)
-#define CONF_SYNCENABLE		(0x00000004)
-
-#define DIRTY_ENABLED(conf)	((conf) & 0x0020)
-#define SYNC_ENABLED(conf) 	((conf) & 0x0004)
-
-/* Display 1 & 2 Write Timing Configuration */
-#define PNX4008_DUM_WT_CFG		0x00372000
-
-/* Display 1 & 2 Read Timing Configuration */
-#define PNX4008_DUM_RT_CFG		0x00003A47
-
-/* DUM Transit State Timing Configuration */
-#define PNX4008_DUM_T_CFG		0x1D	/* 29 HCLK cycles */
-
-/* DUM Sync count clock divider */
-#define PNX4008_DUM_CLK_DIV		0x02DD
-
-/* Memory size for framebuffer, allocated through dma_alloc_writecombine().
- * Must be PAGE aligned
- */
-#define FB_DMA_SIZE (PAGE_ALIGN(SZ_1M + PAGE_SIZE))
-
-#define OFFSET_RGBBUFFER (0xB0000)
-#define OFFSET_YUVBUFFER (0x00000)
-
-#define YUVBUFFER (lcd_video_start + OFFSET_YUVBUFFER)
-#define RGBBUFFER (lcd_video_start + OFFSET_RGBBUFFER)
-
-#define CMDSTRING_BASEADDR	(0x00C000)	/* iram */
-#define BYTES_PER_CMDSTRING	(0x80)
-#define NR_OF_CMDSTRINGS	(64)
-
-#define MAX_NR_PRESTRINGS (0x40)
-#define MAX_NR_POSTSTRINGS (0x40)
-
-/* various mask definitions */
-#define DUM_CLK_ENABLE 0x01
-#define DUM_CLK_DISABLE 0
-#define DUM_DECODE_MASK 0x1FFFFFFF
-#define DUM_CHANNEL_CFG_MASK 0x01FF
-#define DUM_CHANNEL_CFG_SYNC_MASK 0xFFFE00FF
-#define DUM_CHANNEL_CFG_SYNC_MASK_SET 0x0CA00
-
-#define SDUM_RETURNVAL_BASE (0x500)
-
-#define CONF_SYNC_OFF		(0x602)
-#define CONF_SYNC_ON		(0x603)
-
-#define CONF_DIRTYDETECTION_OFF	(0x600)
-#define CONF_DIRTYDETECTION_ON	(0x601)
-
-struct dumchannel_uf {
-	int channelnr;
-	u32 *dirty;
-	u32 *source;
-	u32 x_offset;
-	u32 y_offset;
-	u32 width;
-	u32 height;
-};
-
-enum {
-	FB_TYPE_YUV,
-	FB_TYPE_RGB
-};
-
-struct cmdstring {
-	int channelnr;
-	uint16_t prestringlen;
-	uint16_t poststringlen;
-	uint16_t format;
-	uint16_t reserved;
-	uint16_t startaddr_low;
-	uint16_t startaddr_high;
-	uint16_t pixdatlen_low;
-	uint16_t pixdatlen_high;
-	u32 precmd[MAX_NR_PRESTRINGS];
-	u32 postcmd[MAX_NR_POSTSTRINGS];
-
-};
-
-struct dumchannel {
-	int channelnr;
-	int dum_ch_min;
-	int dum_ch_max;
-	int dum_ch_conf;
-	int dum_ch_stat;
-	int dum_ch_ctrl;
-};
-
-int pnx4008_alloc_dum_channel(int dev_id);
-int pnx4008_free_dum_channel(int channr, int dev_id);
-
-int pnx4008_get_dum_channel_uf(struct dumchannel_uf *pChan_uf, int dev_id);
-int pnx4008_put_dum_channel_uf(struct dumchannel_uf chan_uf, int dev_id);
-
-int pnx4008_set_dum_channel_sync(int channr, int val, int dev_id);
-int pnx4008_set_dum_channel_dirty_detect(int channr, int val, int dev_id);
-
-int pnx4008_force_dum_update_channel(int channr, int dev_id);
-
-int pnx4008_get_dum_channel_config(int channr, int dev_id);
-
-int pnx4008_sdum_mmap(struct fb_info *info, struct vm_area_struct *vma, struct device *dev);
-int pnx4008_set_dum_exit_notification(int dev_id);
-
-int pnx4008_get_fb_addresses(int fb_type, void **virt_addr,
-			     dma_addr_t * phys_addr, int *fb_length);
diff --git a/drivers/video/ps3fb.c b/drivers/video/ps3fb.c
index 4e292f29bf5..0b340d6ff8a 100644
--- a/drivers/video/ps3fb.c
+++ b/drivers/video/ps3fb.c
@@ -1034,6 +1034,7 @@ static int __devinit ps3fb_probe(struct ps3_system_bus_device *dev)
 	if (status) {
 		dev_err(&dev->core, "%s: lv1_gpu_memory_allocate failed: %d\n",
 			__func__, status);
+		retval = -ENOMEM;
 		goto err_close_device;
 	}
 	dev_dbg(&dev->core, "ddr:lpar:0x%llx\n", ddr_lpar);
@@ -1046,6 +1047,7 @@ static int __devinit ps3fb_probe(struct ps3_system_bus_device *dev)
 		dev_err(&dev->core,
 			"%s: lv1_gpu_context_allocate failed: %d\n", __func__,
 			status);
+		retval = -ENOMEM;
 		goto err_gpu_memory_free;
 	}
 
@@ -1053,6 +1055,7 @@ static int __devinit ps3fb_probe(struct ps3_system_bus_device *dev)
 	dinfo = (void __force *)ioremap(lpar_driver_info, 128 * 1024);
 	if (!dinfo) {
 		dev_err(&dev->core, "%s: ioremap failed\n", __func__);
+		retval = -ENOMEM;
 		goto err_gpu_context_free;
 	}
 
@@ -1121,8 +1124,10 @@ static int __devinit ps3fb_probe(struct ps3_system_bus_device *dev)
 	}
 
 	info = framebuffer_alloc(sizeof(struct ps3fb_par), &dev->core);
-	if (!info)
+	if (!info) {
+		retval = -ENOMEM;
 		goto err_context_fb_close;
+	}
 
 	par = info->par;
 	par->mode_id = ~ps3fb_mode;	/* != ps3fb_mode, to trigger change */
diff --git a/drivers/video/s3c-fb.c b/drivers/video/s3c-fb.c
index 69bf9d07c23..2ed7b633bbd 100644
--- a/drivers/video/s3c-fb.c
+++ b/drivers/video/s3c-fb.c
@@ -25,8 +25,8 @@
 #include <linux/interrupt.h>
 #include <linux/pm_runtime.h>
 
+#include <video/samsung_fimd.h>
 #include <mach/map.h>
-#include <plat/regs-fb-v4.h>
 #include <plat/fb.h>
 
 /* This driver will export a number of framebuffer interfaces depending
@@ -1398,35 +1398,28 @@ static int __devinit s3c_fb_probe(struct platform_device *pdev)
 
 	spin_lock_init(&sfb->slock);
 
-	sfb->bus_clk = clk_get(dev, "lcd");
+	sfb->bus_clk = devm_clk_get(dev, "lcd");
 	if (IS_ERR(sfb->bus_clk)) {
 		dev_err(dev, "failed to get bus clock\n");
-		ret = PTR_ERR(sfb->bus_clk);
-		goto err_sfb;
+		return PTR_ERR(sfb->bus_clk);
 	}
 
-	clk_enable(sfb->bus_clk);
+	clk_prepare_enable(sfb->bus_clk);
 
 	if (!sfb->variant.has_clksel) {
-		sfb->lcd_clk = clk_get(dev, "sclk_fimd");
+		sfb->lcd_clk = devm_clk_get(dev, "sclk_fimd");
 		if (IS_ERR(sfb->lcd_clk)) {
 			dev_err(dev, "failed to get lcd clock\n");
 			ret = PTR_ERR(sfb->lcd_clk);
 			goto err_bus_clk;
 		}
 
-		clk_enable(sfb->lcd_clk);
+		clk_prepare_enable(sfb->lcd_clk);
 	}
 
 	pm_runtime_enable(sfb->dev);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(dev, "failed to find registers\n");
-		ret = -ENOENT;
-		goto err_lcd_clk;
-	}
-
 	sfb->regs = devm_request_and_ioremap(dev, res);
 	if (!sfb->regs) {
 		dev_err(dev, "failed to map registers\n");
@@ -1510,16 +1503,12 @@ err_pm_runtime:
 err_lcd_clk:
 	pm_runtime_disable(sfb->dev);
 
-	if (!sfb->variant.has_clksel) {
-		clk_disable(sfb->lcd_clk);
-		clk_put(sfb->lcd_clk);
-	}
+	if (!sfb->variant.has_clksel)
+		clk_disable_unprepare(sfb->lcd_clk);
 
 err_bus_clk:
-	clk_disable(sfb->bus_clk);
-	clk_put(sfb->bus_clk);
+	clk_disable_unprepare(sfb->bus_clk);
 
-err_sfb:
 	return ret;
 }
 
@@ -1541,13 +1530,10 @@ static int __devexit s3c_fb_remove(struct platform_device *pdev)
 		if (sfb->windows[win])
 			s3c_fb_release_win(sfb, sfb->windows[win]);
 
-	if (!sfb->variant.has_clksel) {
-		clk_disable(sfb->lcd_clk);
-		clk_put(sfb->lcd_clk);
-	}
+	if (!sfb->variant.has_clksel)
+		clk_disable_unprepare(sfb->lcd_clk);
 
-	clk_disable(sfb->bus_clk);
-	clk_put(sfb->bus_clk);
+	clk_disable_unprepare(sfb->bus_clk);
 
 	pm_runtime_put_sync(sfb->dev);
 	pm_runtime_disable(sfb->dev);
@@ -1575,9 +1561,9 @@ static int s3c_fb_suspend(struct device *dev)
 	}
 
 	if (!sfb->variant.has_clksel)
-		clk_disable(sfb->lcd_clk);
+		clk_disable_unprepare(sfb->lcd_clk);
 
-	clk_disable(sfb->bus_clk);
+	clk_disable_unprepare(sfb->bus_clk);
 
 	pm_runtime_put_sync(sfb->dev);
 
@@ -1595,10 +1581,10 @@ static int s3c_fb_resume(struct device *dev)
 
 	pm_runtime_get_sync(sfb->dev);
 
-	clk_enable(sfb->bus_clk);
+	clk_prepare_enable(sfb->bus_clk);
 
 	if (!sfb->variant.has_clksel)
-		clk_enable(sfb->lcd_clk);
+		clk_prepare_enable(sfb->lcd_clk);
 
 	/* setup gpio and output polarity controls */
 	pd->setup_gpio();
@@ -1654,9 +1640,9 @@ static int s3c_fb_runtime_suspend(struct device *dev)
 	struct s3c_fb *sfb = platform_get_drvdata(pdev);
 
 	if (!sfb->variant.has_clksel)
-		clk_disable(sfb->lcd_clk);
+		clk_disable_unprepare(sfb->lcd_clk);
 
-	clk_disable(sfb->bus_clk);
+	clk_disable_unprepare(sfb->bus_clk);
 
 	return 0;
 }
@@ -1667,10 +1653,10 @@ static int s3c_fb_runtime_resume(struct device *dev)
 	struct s3c_fb *sfb = platform_get_drvdata(pdev);
 	struct s3c_fb_platdata *pd = sfb->pdata;
 
-	clk_enable(sfb->bus_clk);
+	clk_prepare_enable(sfb->bus_clk);
 
 	if (!sfb->variant.has_clksel)
-		clk_enable(sfb->lcd_clk);
+		clk_prepare_enable(sfb->lcd_clk);
 
 	/* setup gpio and output polarity controls */
 	pd->setup_gpio();
diff --git a/drivers/video/s3c2410fb.c b/drivers/video/s3c2410fb.c
index 77f34c614c8..1083bb9469e 100644
--- a/drivers/video/s3c2410fb.c
+++ b/drivers/video/s3c2410fb.c
@@ -11,6 +11,8 @@
  * Driver based on skeletonfb.c, sa1100fb.c and others.
 */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/err.h>
@@ -48,7 +50,11 @@ static int debug	= 1;
 static int debug;
 #endif
 
-#define dprintk(msg...)	if (debug) printk(KERN_DEBUG "s3c2410fb: " msg);
+#define dprintk(msg...) \
+do { \
+	if (debug) \
+		pr_debug(msg); \
+} while (0)
 
 /* useful functions */
 
@@ -598,11 +604,11 @@ static int s3c2410fb_debug_store(struct device *dev,
 	if (strnicmp(buf, "on", 2) == 0 ||
 	    strnicmp(buf, "1", 1) == 0) {
 		debug = 1;
-		printk(KERN_DEBUG "s3c2410fb: Debug On");
+		dev_dbg(dev, "s3c2410fb: Debug On");
 	} else if (strnicmp(buf, "off", 3) == 0 ||
 		   strnicmp(buf, "0", 1) == 0) {
 		debug = 0;
-		printk(KERN_DEBUG "s3c2410fb: Debug Off");
+		dev_dbg(dev, "s3c2410fb: Debug Off");
 	} else {
 		return -EINVAL;
 	}
@@ -921,7 +927,7 @@ static int __devinit s3c24xxfb_probe(struct platform_device *pdev,
 
 	info->clk = clk_get(NULL, "lcd");
 	if (IS_ERR(info->clk)) {
-		printk(KERN_ERR "failed to get lcd clock source\n");
+		dev_err(&pdev->dev, "failed to get lcd clock source\n");
 		ret = PTR_ERR(info->clk);
 		goto release_irq;
 	}
@@ -929,7 +935,7 @@ static int __devinit s3c24xxfb_probe(struct platform_device *pdev,
 	clk_enable(info->clk);
 	dprintk("got and enabled clock\n");
 
-	usleep_range(1000, 1000);
+	usleep_range(1000, 1100);
 
 	info->clk_rate = clk_get_rate(info->clk);
 
@@ -947,7 +953,7 @@ static int __devinit s3c24xxfb_probe(struct platform_device *pdev,
 	/* Initialize video memory */
 	ret = s3c2410fb_map_video_memory(fbinfo);
 	if (ret) {
-		printk(KERN_ERR "Failed to allocate video RAM: %d\n", ret);
+		dev_err(&pdev->dev, "Failed to allocate video RAM: %d\n", ret);
 		ret = -ENOMEM;
 		goto release_clock;
 	}
@@ -970,7 +976,7 @@ static int __devinit s3c24xxfb_probe(struct platform_device *pdev,
 
 	ret = register_framebuffer(fbinfo);
 	if (ret < 0) {
-		printk(KERN_ERR "Failed to register framebuffer device: %d\n",
+		dev_err(&pdev->dev, "Failed to register framebuffer device: %d\n",
 			ret);
 		goto free_cpufreq;
 	}
@@ -978,9 +984,9 @@ static int __devinit s3c24xxfb_probe(struct platform_device *pdev,
 	/* create device files */
 	ret = device_create_file(&pdev->dev, &dev_attr_debug);
 	if (ret)
-		printk(KERN_ERR "failed to add debug attribute\n");
+		dev_err(&pdev->dev, "failed to add debug attribute\n");
 
-	printk(KERN_INFO "fb%d: %s frame buffer device\n",
+	dev_info(&pdev->dev, "fb%d: %s frame buffer device\n",
 		fbinfo->node, fbinfo->fix.id);
 
 	return 0;
@@ -1028,7 +1034,7 @@ static int __devexit s3c2410fb_remove(struct platform_device *pdev)
 	s3c2410fb_cpufreq_deregister(info);
 
 	s3c2410fb_lcd_enable(info, 0);
-	usleep_range(1000, 1000);
+	usleep_range(1000, 1100);
 
 	s3c2410fb_unmap_video_memory(fbinfo);
 
@@ -1065,7 +1071,7 @@ static int s3c2410fb_suspend(struct platform_device *dev, pm_message_t state)
 	 * the LCD DMA engine is not going to get back on the bus
 	 * before the clock goes off again (bjd) */
 
-	usleep_range(1000, 1000);
+	usleep_range(1000, 1100);
 	clk_disable(info->clk);
 
 	return 0;
@@ -1077,7 +1083,7 @@ static int s3c2410fb_resume(struct platform_device *dev)
 	struct s3c2410fb_info *info = fbinfo->par;
 
 	clk_enable(info->clk);
-	usleep_range(1000, 1000);
+	usleep_range(1000, 1100);
 
 	s3c2410fb_init_registers(fbinfo);
 
@@ -1134,8 +1140,8 @@ static void __exit s3c2410fb_cleanup(void)
 module_init(s3c2410fb_init);
 module_exit(s3c2410fb_cleanup);
 
-MODULE_AUTHOR("Arnaud Patard <arnaud.patard@rtp-net.org>, "
-	      "Ben Dooks <ben-linux@fluff.org>");
+MODULE_AUTHOR("Arnaud Patard <arnaud.patard@rtp-net.org>");
+MODULE_AUTHOR("Ben Dooks <ben-linux@fluff.org>");
 MODULE_DESCRIPTION("Framebuffer driver for the s3c2410");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("platform:s3c2410-lcd");
diff --git a/drivers/video/savage/savagefb_driver.c b/drivers/video/savage/savagefb_driver.c
index 0d0f52c18fd..f4f53b082d0 100644
--- a/drivers/video/savage/savagefb_driver.c
+++ b/drivers/video/savage/savagefb_driver.c
@@ -2266,8 +2266,10 @@ static int __devinit savagefb_probe(struct pci_dev* dev,
 	lpitch = info->var.xres_virtual*((info->var.bits_per_pixel + 7) >> 3);
 	info->var.yres_virtual = info->fix.smem_len/lpitch;
 
-	if (info->var.yres_virtual < info->var.yres)
+	if (info->var.yres_virtual < info->var.yres) {
+		err = -ENOMEM;
 		goto failed;
+	}
 
 #if defined(CONFIG_FB_SAVAGE_ACCEL)
 	/*
diff --git a/drivers/video/sbuslib.c b/drivers/video/sbuslib.c
index 3c1de981a18..296afae442f 100644
--- a/drivers/video/sbuslib.c
+++ b/drivers/video/sbuslib.c
@@ -57,9 +57,8 @@ int sbusfb_mmap_helper(struct sbus_mmap_map *map,
 
 	off = vma->vm_pgoff << PAGE_SHIFT;
 
-	/* To stop the swapper from even considering these pages */
-	vma->vm_flags |= (VM_IO | VM_RESERVED);
-	
+	/* VM_IO | VM_DONTEXPAND | VM_DONTDUMP are set by remap_pfn_range() */
+
 	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
 
 	/* Each page, see which map applies */
diff --git a/drivers/video/sis/initextlfb.c b/drivers/video/sis/initextlfb.c
index 9dec64da401..3ab18f5a375 100644
--- a/drivers/video/sis/initextlfb.c
+++ b/drivers/video/sis/initextlfb.c
@@ -65,7 +65,7 @@ sisfb_mode_rate_to_dclock(struct SiS_Private *SiS_Pr, unsigned char modeno,
     }
 #endif
 
-    if(!(SiS_SearchModeID(SiS_Pr, &ModeNo, &ModeIdIndex))) {;
+    if(!(SiS_SearchModeID(SiS_Pr, &ModeNo, &ModeIdIndex))) {
        printk(KERN_ERR "Could not find mode %x\n", ModeNo);
        return 65000;
     }
diff --git a/drivers/video/smscufx.c b/drivers/video/smscufx.c
index 5533a32c6ca..97bd6620c36 100644
--- a/drivers/video/smscufx.c
+++ b/drivers/video/smscufx.c
@@ -803,7 +803,6 @@ static int ufx_ops_mmap(struct fb_info *info, struct vm_area_struct *vma)
 			size = 0;
 	}
 
-	vma->vm_flags |= VM_RESERVED;	/* avoid to swap out this VMA */
 	return 0;
 }
 
diff --git a/drivers/video/sunxvr1000.c b/drivers/video/sunxvr1000.c
index b7f27acaf81..729a50722bd 100644
--- a/drivers/video/sunxvr1000.c
+++ b/drivers/video/sunxvr1000.c
@@ -141,8 +141,10 @@ static int __devinit gfb_probe(struct platform_device *op)
 
 	gp->fb_base = of_ioremap(&op->resource[6], 0,
 				 gp->fb_size, "gfb fb");
-	if (!gp->fb_base)
+	if (!gp->fb_base) {
+		err = -ENOMEM;
 		goto err_release_fb;
+	}
 
 	err = gfb_set_fbinfo(gp);
 	if (err)
diff --git a/drivers/video/sunxvr2500.c b/drivers/video/sunxvr2500.c
index 5848436c19d..7fbcba86d1a 100644
--- a/drivers/video/sunxvr2500.c
+++ b/drivers/video/sunxvr2500.c
@@ -181,8 +181,10 @@ static int __devinit s3d_pci_register(struct pci_dev *pdev,
 	sp->fb_size = info->fix.line_length * sp->height;
 
 	sp->fb_base = ioremap(sp->fb_base_phys, sp->fb_size);
-	if (!sp->fb_base)
+	if (!sp->fb_base) {
+		err = -ENOMEM;
 		goto err_release_pci;
+	}
 
 	err = s3d_set_fbinfo(sp);
 	if (err)
diff --git a/drivers/video/sunxvr500.c b/drivers/video/sunxvr500.c
index eb931b8626f..6c71b1b4447 100644
--- a/drivers/video/sunxvr500.c
+++ b/drivers/video/sunxvr500.c
@@ -298,8 +298,10 @@ static int __devinit e3d_pci_register(struct pci_dev *pdev,
 		goto err_release_fb;
 	}
 	ep->ramdac = ioremap(ep->regs_base_phys + 0x8000, 0x1000);
-	if (!ep->ramdac)
+	if (!ep->ramdac) {
+		err = -ENOMEM;
 		goto err_release_pci1;
+	}
 
 	ep->fb8_0_off = readl(ep->ramdac + RAMDAC_VID_8FB_0);
 	ep->fb8_0_off -= ep->fb_base_reg;
@@ -343,8 +345,10 @@ static int __devinit e3d_pci_register(struct pci_dev *pdev,
 	ep->fb_size = info->fix.line_length * ep->height;
 
 	ep->fb_base = ioremap(ep->fb_base_phys, ep->fb_size);
-	if (!ep->fb_base)
+	if (!ep->fb_base) {
+		err = -ENOMEM;
 		goto err_release_pci0;
+	}
 
 	err = e3d_set_fbinfo(ep);
 	if (err)
diff --git a/drivers/video/udlfb.c b/drivers/video/udlfb.c
index 8af64148294..86d449ea316 100644
--- a/drivers/video/udlfb.c
+++ b/drivers/video/udlfb.c
@@ -345,7 +345,6 @@ static int dlfb_ops_mmap(struct fb_info *info, struct vm_area_struct *vma)
 			size = 0;
 	}
 
-	vma->vm_flags |= VM_RESERVED;	/* avoid to swap out this VMA */
 	return 0;
 }
 
@@ -647,7 +646,7 @@ static ssize_t dlfb_ops_write(struct fb_info *info, const char __user *buf,
 	result = fb_sys_write(info, buf, count, ppos);
 
 	if (result > 0) {
-		int start = max((int)(offset / info->fix.line_length) - 1, 0);
+		int start = max((int)(offset / info->fix.line_length), 0);
 		int lines = min((u32)((result / info->fix.line_length) + 1),
 				(u32)info->var.yres);
 
diff --git a/drivers/video/uvesafb.c b/drivers/video/uvesafb.c
index b0e2a4261af..2f8f82d874a 100644
--- a/drivers/video/uvesafb.c
+++ b/drivers/video/uvesafb.c
@@ -659,6 +659,8 @@ static int __devinit uvesafb_vbe_getedid(struct uvesafb_ktask *task,
 	task->t.flags = TF_BUF_RET | TF_BUF_ESDI;
 	task->t.buf_len = EDID_LENGTH;
 	task->buf = kzalloc(EDID_LENGTH, GFP_KERNEL);
+	if (!task->buf)
+		return -ENOMEM;
 
 	err = uvesafb_exec(task);
 
diff --git a/drivers/video/vermilion/vermilion.c b/drivers/video/vermilion/vermilion.c
index 970e43d13f5..4709edc3cb7 100644
--- a/drivers/video/vermilion/vermilion.c
+++ b/drivers/video/vermilion/vermilion.c
@@ -1018,7 +1018,6 @@ static int vmlfb_mmap(struct fb_info *info, struct vm_area_struct *vma)
 	offset += vinfo->vram_start;
 	pgprot_val(vma->vm_page_prot) |= _PAGE_PCD;
 	pgprot_val(vma->vm_page_prot) &= ~_PAGE_PWT;
-	vma->vm_flags |= VM_RESERVED | VM_IO;
 	if (remap_pfn_range(vma, vma->vm_start, offset >> PAGE_SHIFT,
 						size, vma->vm_page_prot))
 		return -EAGAIN;
@@ -1168,8 +1167,7 @@ void vmlfb_unregister_subsys(struct vml_sys *sys)
 	list_for_each_entry_safe(entry, next, &global_has_mode, head) {
 		printk(KERN_DEBUG MODULE_NAME ": subsys disable pipe\n");
 		vmlfb_disable_pipe(entry);
-		list_del(&entry->head);
-		list_add_tail(&entry->head, &global_no_mode);
+		list_move_tail(&entry->head, &global_no_mode);
 	}
 	mutex_unlock(&vml_mutex);
 }
diff --git a/drivers/video/vfb.c b/drivers/video/vfb.c
index 501a922aa9d..c7f692525b8 100644
--- a/drivers/video/vfb.c
+++ b/drivers/video/vfb.c
@@ -439,7 +439,6 @@ static int vfb_mmap(struct fb_info *info,
 			size = 0;
 	}
 
-	vma->vm_flags |= VM_RESERVED;	/* avoid to swap out this VMA */
 	return 0;
 
 }
diff --git a/drivers/video/via/via_clock.c b/drivers/video/via/via_clock.c
index af8f26b643c..db1e39277e3 100644
--- a/drivers/video/via/via_clock.c
+++ b/drivers/video/via/via_clock.c
@@ -25,6 +25,7 @@
 
 #include <linux/kernel.h>
 #include <linux/via-core.h>
+#include <asm/olpc.h>
 #include "via_clock.h"
 #include "global.h"
 #include "debug.h"
@@ -289,6 +290,10 @@ static void dummy_set_pll(struct via_pll_config config)
 	printk(KERN_INFO "Using undocumented set PLL.\n%s", via_slap);
 }
 
+static void noop_set_clock_state(u8 state)
+{
+}
+
 void via_clock_init(struct via_clock *clock, int gfx_chip)
 {
 	switch (gfx_chip) {
@@ -346,4 +351,18 @@ void via_clock_init(struct via_clock *clock, int gfx_chip)
 		break;
 
 	}
+
+	if (machine_is_olpc()) {
+		/* The OLPC XO-1.5 cannot suspend/resume reliably if the
+		 * IGA1/IGA2 clocks are set as on or off (memory rot
+		 * occasionally happens during suspend under such
+		 * configurations).
+		 *
+		 * The only known stable scenario is to leave this bits as-is,
+		 * which in their default states are documented to enable the
+		 * clock only when it is needed.
+		 */
+		clock->set_primary_clock_state = noop_set_clock_state;
+		clock->set_secondary_clock_state = noop_set_clock_state;
+	}
 }
diff --git a/drivers/xen/gntalloc.c b/drivers/xen/gntalloc.c
index 934985d14c2..4097987b330 100644
--- a/drivers/xen/gntalloc.c
+++ b/drivers/xen/gntalloc.c
@@ -535,7 +535,7 @@ static int gntalloc_mmap(struct file *filp, struct vm_area_struct *vma)
 
 	vma->vm_private_data = vm_priv;
 
-	vma->vm_flags |= VM_RESERVED | VM_DONTEXPAND;
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
 
 	vma->vm_ops = &gntalloc_vmops;
 
diff --git a/drivers/xen/gntdev.c b/drivers/xen/gntdev.c
index 5df9fd847b2..610bfc6be17 100644
--- a/drivers/xen/gntdev.c
+++ b/drivers/xen/gntdev.c
@@ -720,7 +720,7 @@ static int gntdev_mmap(struct file *flip, struct vm_area_struct *vma)
 
 	vma->vm_ops = &gntdev_vmops;
 
-	vma->vm_flags |= VM_RESERVED|VM_DONTEXPAND;
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
 
 	if (use_ptemod)
 		vma->vm_flags |= VM_DONTCOPY;
diff --git a/drivers/xen/privcmd.c b/drivers/xen/privcmd.c
index ef6389580b8..8adb9cc267f 100644
--- a/drivers/xen/privcmd.c
+++ b/drivers/xen/privcmd.c
@@ -455,7 +455,8 @@ static int privcmd_mmap(struct file *file, struct vm_area_struct *vma)
 {
 	/* DONTCOPY is essential for Xen because copy_page_range doesn't know
 	 * how to recreate these mappings */
-	vma->vm_flags |= VM_RESERVED | VM_IO | VM_DONTCOPY | VM_PFNMAP;
+	vma->vm_flags |= VM_IO | VM_PFNMAP | VM_DONTCOPY |
+			 VM_DONTEXPAND | VM_DONTDUMP;
 	vma->vm_ops = &privcmd_vm_ops;
 	vma->vm_private_data = NULL;
 
diff --git a/drivers/xen/xenbus/xenbus_xs.c b/drivers/xen/xenbus/xenbus_xs.c
index 131dec04794..48220e129f8 100644
--- a/drivers/xen/xenbus/xenbus_xs.c
+++ b/drivers/xen/xenbus/xenbus_xs.c
@@ -48,6 +48,7 @@
 #include <xen/xenbus.h>
 #include <xen/xen.h>
 #include "xenbus_comms.h"
+#include <asm/xen/hypervisor.h>
 
 struct xs_stored_msg {
 	struct list_head list;
@@ -618,7 +619,24 @@ static struct xenbus_watch *find_watch(const char *token)
 
 	return NULL;
 }
+/*
+ * Certain older XenBus toolstack cannot handle reading values that are
+ * not populated. Some Xen 3.4 installation are incapable of doing this
+ * so if we are running on anything older than 4 do not attempt to read
+ * control/platform-feature-xs_reset_watches.
+ */
+static bool xen_strict_xenbus_quirk()
+{
+	uint32_t eax, ebx, ecx, edx, base;
+
+	base = xen_cpuid_base();
+	cpuid(base + 1, &eax, &ebx, &ecx, &edx);
 
+	if ((eax >> 16) < 4)
+		return true;
+	return false;
+
+}
 static void xs_reset_watches(void)
 {
 	int err, supported = 0;
@@ -626,6 +644,9 @@ static void xs_reset_watches(void)
 	if (!xen_hvm_domain() || xen_initial_domain())
 		return;
 
+	if (xen_strict_xenbus_quirk())
+		return;
+
 	err = xenbus_scanf(XBT_NIL, "control",
 			"platform-feature-xs_reset_watches", "%d", &supported);
 	if (err != 1 || !supported)