Support multi-bit mask in STM32L's GPIO functions

The definition of GPIO interface allows passing in multi-bit mask, and
this is what's done by gpio_config_module(). Fix STM32L's function so
that it doesn't accidentally set incorrect GPIO register values.

BUG=chrome-os-partner:22605
TEST=On Kirby, do 'led r 0' and check the value of 0x40020800 is
0x01540000.
BRANCH=None

Change-Id: I9a1c8074aab7345485a590ecf138bf99d0742997
Signed-off-by: Vic Yang <victoryang@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/168739
Reviewed-by: Vincent Palatin <vpalatin@chromium.org>
Reviewed-by: Randall Spangler <rspangler@chromium.org>
Tested-by: Randall Spangler <rspangler@chromium.org>

8 files changed, 51 insertions, 10 deletions

diff --git a/chip/host/gpio.c b/chip/host/gpio.c
index 975476961c..64191aaca5 100644
--- a/chip/host/gpio.c
+++ b/chip/host/gpio.c
@@ -34,7 +34,8 @@ test_mockable void gpio_set_flags_by_mask(uint32_t port, uint32_t mask,
 	/* Nothing */
 }
 
-test_mockable void gpio_set_alternate_function(int port, int mask, int func)
+test_mockable void gpio_set_alternate_function(uint32_t port, uint32_t mask,
+					       int func)
 {
 	/* Nothing */
 }
diff --git a/chip/lm4/gpio.c b/chip/lm4/gpio.c
index 25945ddc28..5834d4b7e5 100644
--- a/chip/lm4/gpio.c
+++ b/chip/lm4/gpio.c
@@ -42,7 +42,7 @@ static int find_gpio_port_index(uint32_t port_base)
 	return -1;
 }
 
-void gpio_set_alternate_function(int port, int mask, int func)
+void gpio_set_alternate_function(uint32_t port, uint32_t mask, int func)
 {
 	int port_index = find_gpio_port_index(port);
 	int cgmask;
diff --git a/chip/stm32/gpio-stm32f.c b/chip/stm32/gpio-stm32f.c
index 450169f3f8..f917d00a0d 100644
--- a/chip/stm32/gpio-stm32f.c
+++ b/chip/stm32/gpio-stm32f.c
@@ -111,7 +111,7 @@ void gpio_set_flags_by_mask(uint32_t port, uint32_t pmask, uint32_t flags)
 	/* Interrupt is enabled by gpio_enable_interrupt() */
 }
 
-void gpio_set_alternate_function(int port, int mask, int func)
+void gpio_set_alternate_function(uint32_t port, uint32_t mask, int func)
 {
 	/* TODO(rspangler): implement me! */
 }
@@ -224,11 +224,10 @@ static void gpio_interrupt(void)
 	STM32_EXTI_PR = pending;
 
 	while (pending) {
-		bit = 31 - __builtin_clz(pending);
+		bit = get_next_bit(&pending);
 		g = exti_events[bit];
 		if (g && g->irq_handler)
 			g->irq_handler(g - gpio_list);
-		pending &= ~(1 << bit);
 	}
 }
 DECLARE_IRQ(STM32_IRQ_EXTI0, gpio_interrupt, 1);
diff --git a/chip/stm32/gpio-stm32l.c b/chip/stm32/gpio-stm32l.c
index 09cc0ad4b3..e2d5a97a43 100644
--- a/chip/stm32/gpio-stm32l.c
+++ b/chip/stm32/gpio-stm32l.c
@@ -20,10 +20,20 @@
 /* For each EXTI bit, record which GPIO entry is using it */
 static const struct gpio_info *exti_events[16];
 
+static uint32_t expand_to_2bit_mask(uint32_t mask)
+{
+	uint32_t mask_out = 0;
+	while (mask) {
+		int bit = get_next_bit(&mask);
+		mask_out |= 3 << (bit * 2);
+	}
+	return mask_out;
+}
+
 void gpio_set_flags_by_mask(uint32_t port, uint32_t mask, uint32_t flags)
 {
 	/* Bitmask for registers with 2 bits per GPIO pin */
-	const uint32_t mask2 = (mask * mask) | (mask * mask * 2);
+	const uint32_t mask2 = expand_to_2bit_mask(mask);
 	uint32_t val;
 
 	/* Set up pullup / pulldown */
@@ -124,7 +134,7 @@ static void gpio_init(void)
 }
 DECLARE_HOOK(HOOK_INIT, gpio_init, HOOK_PRIO_DEFAULT);
 
-void gpio_set_alternate_function(int port, int mask, int func)
+void gpio_set_alternate_function(uint32_t port, uint32_t mask, int func)
 {
 	int bit;
 	uint8_t half;
@@ -134,9 +144,8 @@ void gpio_set_alternate_function(int port, int mask, int func)
 	if (func < 0) {
 		/* Return to normal GPIO function, defaulting to input. */
 		while (mask) {
-			bit = 31 - __builtin_clz(mask);
+			bit = get_next_bit(&mask);
 			moder &= ~(0x3 << (bit * 2));
-			mask &= ~(1 << bit);
 		}
 		STM32_GPIO_MODER(port) = moder;
 		return;
diff --git a/common/util.c b/common/util.c
index 0d7bff2710..f120cdad24 100644
--- a/common/util.c
+++ b/common/util.c
@@ -273,6 +273,13 @@ int uint64divmod(uint64_t *n, int d)
 	return r;
 }
 
+int get_next_bit(uint32_t *mask)
+{
+	int bit = 31 - __builtin_clz(*mask);
+	*mask &= ~(1 << bit);
+	return bit;
+}
+
 
 /****************************************************************************/
 /* stateful conditional stuff */
diff --git a/include/gpio.h b/include/gpio.h
index ce2de2b126..ac25ee8d39 100644
--- a/include/gpio.h
+++ b/include/gpio.h
@@ -193,6 +193,6 @@ void gpio_set_flags_by_mask(uint32_t port, uint32_t mask, uint32_t flags);
  * @param func		Alternate function; if <0, configures the specified
  *			GPIOs for normal GPIO operation.
  */
-void gpio_set_alternate_function(int port, int mask, int func);
+void gpio_set_alternate_function(uint32_t port, uint32_t mask, int func);
 
 #endif  /* __CROS_EC_GPIO_H */
diff --git a/include/util.h b/include/util.h
index 0408940b35..8ba25c1780 100644
--- a/include/util.h
+++ b/include/util.h
@@ -101,6 +101,16 @@ int tolower(int c);
  */
 int uint64divmod(uint64_t *v, int by);
 
+/**
+ * Get-and-clear next bit from mask.
+ *
+ * Starts with most significant bit.
+ *
+ * @param mask Bitmask to extract next bit from. Must NOT be zero.
+ * @return bit position (0..31)
+ */
+int get_next_bit(uint32_t *mask);
+
 
 /****************************************************************************/
 /* Conditional stuff.
diff --git a/test/utils.c b/test/utils.c
index e2230bfd44..60d2c83a0f 100644
--- a/test/utils.c
+++ b/test/utils.c
@@ -154,6 +154,20 @@ static int test_uint64divmod_2(void)
 	TEST_CHECK(r == 0 && n == 0ULL);
 }
 
+static int test_get_next_bit(void)
+{
+	uint32_t mask = 0x10001010;
+
+	TEST_ASSERT(get_next_bit(&mask) == 28);
+	TEST_ASSERT(mask == 0x1010);
+	TEST_ASSERT(get_next_bit(&mask) == 12);
+	TEST_ASSERT(mask == 0x10);
+	TEST_ASSERT(get_next_bit(&mask) == 4);
+	TEST_ASSERT(mask == 0x0);
+
+	return EC_SUCCESS;
+}
+
 static int test_shared_mem(void)
 {
 	int i, j;
@@ -299,6 +313,7 @@ void run_test(void)
 	RUN_TEST(test_uint64divmod_0);
 	RUN_TEST(test_uint64divmod_1);
 	RUN_TEST(test_uint64divmod_2);
+	RUN_TEST(test_get_next_bit);
 	RUN_TEST(test_shared_mem);
 	RUN_TEST(test_scratchpad);
 	RUN_TEST(test_cond_t);