1 files changed, 12 insertions, 4 deletions

diff --git a/drivers/pinctrl/freescale/pinctrl-mxs.c b/drivers/pinctrl/freescale/pinctrl-mxs.c
index 41b5b07d5a2b..6852010a6d70 100644
--- 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 */