Merge tag 'armsoc-dt' of git://git.kernel.org/pub/scm/linux/kernel/git/arm/arm-soc

[mirror_ubuntu-artful-kernel.git] / drivers / pinctrl / qcom / pinctrl-msm.c

diff --git a/drivers/pinctrl/qcom/pinctrl-msm.c b/drivers/pinctrl/qcom/pinctrl-msm.c
index 1a44e1d033905b8a884f170b710ea219365218c3..51c42d7468837f74344452e8b8ed269646393ef6 100644 (file)
--- a/drivers/pinctrl/qcom/pinctrl-msm.c
+++ b/drivers/pinctrl/qcom/pinctrl-msm.c
@@ -29,6 +29,7 @@
 #include <linux/spinlock.h>
 #include <linux/reboot.h>
 #include <linux/pm.h>
+#include <linux/log2.h>
 
 #include "../core.h"
 #include "../pinconf.h"
@@ -138,10 +139,11 @@ static int msm_pinmux_set_mux(struct pinctrl_dev *pctldev,
        struct msm_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctldev);
        const struct msm_pingroup *g;
        unsigned long flags;
-       u32 val;
+       u32 val, mask;
        int i;
 
        g = &pctrl->soc->groups[group];
+       mask = GENMASK(g->mux_bit + order_base_2(g->nfuncs) - 1, g->mux_bit);
 
        for (i = 0; i < g->nfuncs; i++) {
                if (g->funcs[i] == function)
@@ -154,7 +156,7 @@ static int msm_pinmux_set_mux(struct pinctrl_dev *pctldev,
        spin_lock_irqsave(&pctrl->lock, flags);
 
        val = readl(pctrl->regs + g->ctl_reg);
-       val &= ~(0x7 << g->mux_bit);
+       val &= mask;
        val |= i << g->mux_bit;
        writel(val, pctrl->regs + g->ctl_reg);