Merge branch 'ufs-fixes' of git://git.kernel.org/pub/scm/linux/kernel/git/viro/vfs

[karo-tx-linux.git] / drivers / pinctrl / freescale / pinctrl-mxs.c

diff --git a/drivers/pinctrl/freescale/pinctrl-mxs.c b/drivers/pinctrl/freescale/pinctrl-mxs.c
index 41b5b07d5a2bf51f6b0623597c294862910de78c..6852010a6d708b5010555cbb141bf57ac31de077 100644 (file)
--- a/drivers/pinctrl/freescale/pinctrl-mxs.c
+++ b/drivers/pinctrl/freescale/pinctrl-mxs.c
@@ -194,6 +194,16 @@ static int mxs_pinctrl_get_func_groups(struct pinctrl_dev *pctldev,
        return 0;
 }
 
+static void mxs_pinctrl_rmwl(u32 value, u32 mask, u8 shift, void __iomem *reg)
+{
+       u32 tmp;
+
+       tmp = readl(reg);
+       tmp &= ~(mask << shift);
+       tmp |= value << shift;
+       writel(tmp, reg);
+}
+
 static int mxs_pinctrl_set_mux(struct pinctrl_dev *pctldev, unsigned selector,
                               unsigned group)
 {
@@ -211,8 +221,7 @@ static int mxs_pinctrl_set_mux(struct pinctrl_dev *pctldev, unsigned selector,
                reg += bank * 0x20 + pin / 16 * 0x10;
                shift = pin % 16 * 2;
 
-               writel(0x3 << shift, reg + CLR);
-               writel(g->muxsel[i] << shift, reg + SET);
+               mxs_pinctrl_rmwl(g->muxsel[i], 0x3, shift, reg);
        }
 
        return 0;
@@ -279,8 +288,7 @@ static int mxs_pinconf_group_set(struct pinctrl_dev *pctldev,
                        /* mA */
                        if (config & MA_PRESENT) {
                                shift = pin % 8 * 4;
-                               writel(0x3 << shift, reg + CLR);
-                               writel(ma << shift, reg + SET);
+                               mxs_pinctrl_rmwl(ma, 0x3, shift, reg);
                        }
 
                        /* vol */