[U-Boot] [PATCH v2 1/4] kirkwood: add kirkwood_mpp_read function

[Valentin Longchamp]

This function can be used to save current mpp state of all mpp pins
given in the mpp_list argument by reading the mpp registers, in the
second mpp_saved argument.

A later call to kirkwood_mpp_conf function with this saved list will
reset the mpp configuration as it was when kirkwood_mpp_read was called.

Signed-off-by: Valentin Longchamp <valentin.longchamp at keymile.com>
cc: Holger Brunck <holger.brunck at keymile.com>
cc: Prafulla Wadaskar <prafulla at marvell.com>
---
 arch/arm/cpu/arm926ejs/kirkwood/mpp.c    |   41 ++++++++++++++++++++++++++++++
 arch/arm/include/asm/arch-kirkwood/mpp.h |    1 +
 2 files changed, 42 insertions(+), 0 deletions(-)

diff --git a/arch/arm/cpu/arm926ejs/kirkwood/mpp.c b/arch/arm/cpu/arm926ejs/kirkwood/mpp.c
index 3da6c98..9fb9aea 100644
--- a/arch/arm/cpu/arm926ejs/kirkwood/mpp.c
+++ b/arch/arm/cpu/arm926ejs/kirkwood/mpp.c
@@ -80,3 +80,44 @@ void kirkwood_mpp_conf(u32 *mpp_list)
 	debug("\n");
 
 }
+
+void kirkwood_mpp_read(u32 *mpp_list, u32 *mpp_saved)
+{
+	u32 mpp_ctrl[MPP_NR_REGS];
+	unsigned int variant_mask;
+	int i;
+
+	variant_mask = kirkwood_variant();
+	if (!variant_mask)
+		return;
+
+	for (i = 0; i < MPP_NR_REGS; i++) {
+		mpp_ctrl[i] = readl(MPP_CTRL(i));
+		debug(" %08x", mpp_ctrl[i]);
+	}
+
+	while (*mpp_list) {
+		unsigned int num = MPP_NUM(*mpp_list);
+		unsigned int sel;
+		int shift;
+
+		if (num > MPP_MAX) {
+			debug("kirkwood_mpp_conf: invalid MPP "
+					"number (%u)\n", num);
+			continue;
+		}
+		if (!(*mpp_list & variant_mask)) {
+			debug("kirkwood_mpp_conf: requested MPP%u config "
+				"unavailable on this hardware\n", num);
+			continue;
+		}
+
+		shift = (num & 7) << 2;
+		sel = (mpp_ctrl[num / 8] >> shift) & 0xf;
+		*mpp_saved = num | (sel << 8) | variant_mask;
+
+		mpp_list++;
+		mpp_saved++;
+	}
+}
+
diff --git a/arch/arm/include/asm/arch-kirkwood/mpp.h b/arch/arm/include/asm/arch-kirkwood/mpp.h
index b3c090e..22a64b8 100644
--- a/arch/arm/include/asm/arch-kirkwood/mpp.h
+++ b/arch/arm/include/asm/arch-kirkwood/mpp.h
@@ -313,5 +313,6 @@
 #define MPP_MAX			49
 
 void kirkwood_mpp_conf(unsigned int *mpp_list);
+void kirkwood_mpp_read(u32 *mpp_list, u32 *mpp_saved);
 
 #endif
-- 
1.7.1