From 45dcb54f014d3d1f5cc3919b5f0c97087d7cb3dd Mon Sep 17 00:00:00 2001 From: David Lechner Date: Mon, 19 Feb 2018 15:57:07 -0600 Subject: [PATCH] pinctrl: pinctrl-single: Fix pcs_request_gpio() when bits_per_mux != 0 This fixes pcs_request_gpio() in the pinctrl-single driver when bits_per_mux != 0. It appears this was overlooked when the multiple pins per register feature was added. Fixes: 4e7e8017a80e ("pinctrl: pinctrl-single: enhance to configure multiple pins of different modules") Signed-off-by: David Lechner Acked-by: Tony Lindgren Signed-off-by: Linus Walleij --- drivers/pinctrl/pinctrl-single.c | 22 +++++++++++++++++++--- 1 file 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;