diff options
Diffstat (limited to 'drivers/pinctrl')
| -rw-r--r-- | drivers/pinctrl/pinctrl-single.c | 22 |
1 files changed, 19 insertions, 3 deletions
diff --git a/drivers/pinctrl/pinctrl-single.c b/drivers/pinctrl/pinctrl-single.c index cec75379f936..a7c5eb39b1eb 100644 --- a/drivers/pinctrl/pinctrl-single.c +++ b/drivers/pinctrl/pinctrl-single.c @@ -391,9 +391,25 @@ static int pcs_request_gpio(struct pinctrl_dev *pctldev, || pin < frange->offset) continue; mux_bytes = pcs->width / BITS_PER_BYTE; - data = pcs->read(pcs->base + pin * mux_bytes) & ~pcs->fmask; - data |= frange->gpiofunc; - pcs->write(data, pcs->base + pin * mux_bytes); + + if (pcs->bits_per_mux) { + int byte_num, offset, pin_shift; + + byte_num = (pcs->bits_per_pin * pin) / BITS_PER_BYTE; + offset = (byte_num / mux_bytes) * mux_bytes; + pin_shift = pin % (pcs->width / pcs->bits_per_pin) * + pcs->bits_per_pin; + + data = pcs->read(pcs->base + offset); + data &= ~(pcs->fmask << pin_shift); + data |= frange->gpiofunc << pin_shift; + pcs->write(data, pcs->base + offset); + } else { + data = pcs->read(pcs->base + pin * mux_bytes); + data &= ~pcs->fmask; + data |= frange->gpiofunc; + pcs->write(data, pcs->base + pin * mux_bytes); + } break; } return 0; |