2 files changed, 73 insertions, 0 deletions

diff --git a/arch/arm/include/asm/arch-tegra/pinmux.h b/arch/arm/include/asm/arch-tegra/pinmux.h
index e3eb706..3cc52dd 100644
--- a/arch/arm/include/asm/arch-tegra/pinmux.h
+++ b/arch/arm/include/asm/arch-tegra/pinmux.h
@@ -229,4 +229,20 @@ void pinmux_config_drvgrp_table(const struct pmux_drvgrp_config *config,
 
 #endif /* TEGRA_PMX_SOC_HAS_DRVGRPS */
 
+#ifdef TEGRA_PMX_SOC_HAS_MIPI_PAD_CTRL_GRPS
+struct pmux_mipipadctrlgrp_config {
+	u32 grp:16;	/* pin group PMUX_MIPIPADCTRLGRP_x   */
+	u32 func:8;	/* function to assign PMUX_FUNC_... */
+};
+
+void pinmux_config_mipipadctrlgrp_table(
+	const struct pmux_mipipadctrlgrp_config *config, int len);
+
+struct pmux_mipipadctrlgrp_desc {
+	u8 funcs[2];
+};
+
+extern const struct pmux_mipipadctrlgrp_desc *tegra_soc_mipipadctrl_groups;
+#endif /* TEGRA_PMX_SOC_HAS_MIPI_PAD_CTRL_GRPS */
+
 #endif /* _TEGRA_PINMUX_H_ */
diff --git a/arch/arm/mach-tegra/pinmux-common.c b/arch/arm/mach-tegra/pinmux-common.c
index 96dbb5e..b4a1432 100644
--- a/arch/arm/mach-tegra/pinmux-common.c
+++ b/arch/arm/mach-tegra/pinmux-common.c
@@ -108,6 +108,8 @@
 
 #define DRV_REG(group)	_R(TEGRA_PMX_SOC_DRV_GROUP_BASE_REG + ((group) * 4))
 
+#define MIPIPADCTRL_REG(group)	_R(TEGRA_PMX_SOC_MIPIPADCTRL_BASE_REG + ((group) * 4))
+
 /*
  * We could force arch-tegraNN/pinmux.h to define all of these. However,
  * that's a lot of defines, and for now it's manageable to just put a
@@ -696,3 +698,58 @@ void pinmux_config_drvgrp_table(const struct pmux_drvgrp_config *config,
 		pinmux_config_drvgrp(&config[i]);
 }
 #endif /* TEGRA_PMX_SOC_HAS_DRVGRPS */
+
+#ifdef TEGRA_PMX_SOC_HAS_MIPI_PAD_CTRL_GRPS
+
+#define pmux_mipipadctrlgrp_isvalid(pd) (((pd) >= 0) && ((pd) < PMUX_MIPIPADCTRLGRP_COUNT))
+
+static void pinmux_mipipadctrl_set_func(enum pmux_mipipadctrlgrp grp,
+	enum pmux_func func)
+{
+	u32 *reg = MIPIPADCTRL_REG(grp);
+	int i, mux = -1;
+	u32 val;
+
+	if (func == PMUX_FUNC_DEFAULT)
+		return;
+
+	/* Error check grp and func */
+	assert(pmux_mipipadctrlgrp_isvalid(grp));
+	assert(pmux_func_isvalid(func));
+
+	if (func >= PMUX_FUNC_RSVD1) {
+		mux = (func - PMUX_FUNC_RSVD1) & 1;
+	} else {
+		/* Search for the appropriate function */
+		for (i = 0; i < 2; i++) {
+			if (tegra_soc_mipipadctrl_groups[grp].funcs[i]
+			    == func) {
+				mux = i;
+				break;
+			}
+		}
+	}
+	assert(mux != -1);
+
+	val = readl(reg);
+	val &= ~(1 << 1);
+	val |= (mux << 1);
+	writel(val, reg);
+}
+
+static void pinmux_config_mipipadctrlgrp(const struct pmux_mipipadctrlgrp_config *config)
+{
+	enum pmux_mipipadctrlgrp grp = config->grp;
+
+	pinmux_mipipadctrl_set_func(grp, config->func);
+}
+
+void pinmux_config_mipipadctrlgrp_table(
+	const struct pmux_mipipadctrlgrp_config *config, int len)
+{
+	int i;
+
+	for (i = 0; i < len; i++)
+		pinmux_config_mipipadctrlgrp(&config[i]);
+}
+#endif /* TEGRA_PMX_SOC_HAS_MIPI_PAD_CTRL_GRPS */