static int set_mode(struct spear_pmx *pmx, int mode)
{
struct spear_pmx_mode *pmx_mode = NULL;
int i;
u32 val;
if (!pmx->machdata->pmx_modes || !pmx->machdata->npmx_modes)
return -EINVAL;
for (i = 0; i < pmx->machdata->npmx_modes; i++) {
if (pmx->machdata->pmx_modes[i]->mode == (1 << mode)) {
pmx_mode = pmx->machdata->pmx_modes[i];
break;
}
}
if (!pmx_mode)
return -EINVAL;
val = pmx_readl(pmx, pmx_mode->reg);
val &= ~pmx_mode->mask;
val |= pmx_mode->val;
pmx_writel(pmx, val, pmx_mode->reg);
pmx->machdata->mode = pmx_mode->mode;
dev_info(pmx->dev, "Configured Mode: %s with id: %x\n\n",
pmx_mode->name ? pmx_mode->name : "no_name",
pmx_mode->reg);
return 0;
}
static void gpio_request_endisable(struct spear_pmx *pmx, int pin,
bool enable)
{
unsigned int regoffset, regindex, bitoffset;
unsigned int val;
/* pin++ as gpio configuration starts from 2nd bit of base register */
pin++;
regindex = pin / 32;
bitoffset = pin % 32;
if (regindex <= 3)
regoffset = PAD_FUNCTION_EN_1 + regindex * sizeof(int *);
else
regoffset = PAD_FUNCTION_EN_5 + (regindex - 4) * sizeof(int *);
val = pmx_readl(pmx, regoffset);
if (enable)
val &= ~(0x1 << bitoffset);
else
val |= 0x1 << bitoffset;
pmx_writel(pmx, val, regoffset);
}
static void muxregs_endisable(struct spear_pmx *pmx,
struct spear_muxreg *muxregs, u8 count, bool enable)
{
struct spear_muxreg *muxreg;
u32 val, temp, j;
for (j = 0; j < count; j++) {
muxreg = &muxregs[j];
val = pmx_readl(pmx, muxreg->reg);
val &= ~muxreg->mask;
if (enable)
temp = muxreg->val;
else
temp = ~muxreg->val;
val |= muxreg->mask & temp;
pmx_writel(pmx, val, muxreg->reg);
}
}
