lpc176x: Add support for gpio_x_reset()

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

2 files changed, 44 insertions, 15 deletions

diff --git a/src/lpc176x/gpio.c b/src/lpc176x/gpio.c
index e36c2860..49363d2a 100644
--- a/src/lpc176x/gpio.c
+++ b/src/lpc176x/gpio.c
@@ -4,6 +4,7 @@
 //
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
+#include <string.h> // ffs
 #include "LPC17xx.h" // LPC_PINCON
 #include "autoconf.h" // CONFIG_CLOCK_FREQ
 #include "board/irq.h" // irq_save
@@ -50,6 +51,16 @@ gpio_peripheral(int bank, int pin, int func, int pullup)
     irq_restore(flag);
 }
 
+// Convert a register and bit location back to an integer pin identifier
+static int
+regs_to_pin(LPC_GPIO_TypeDef *regs, uint32_t bit)
+{
+    int i;
+    for (i=0; i<ARRAY_SIZE(digital_regs); i++)
+        if (digital_regs[i] == regs)
+            return GPIO(i, ffs(bit)-1);
+    return 0;
+}
 
 struct gpio_out
 gpio_out_setup(uint8_t pin, uint8_t val)
@@ -57,17 +68,26 @@ gpio_out_setup(uint8_t pin, uint8_t val)
     if (GPIO2PORT(pin) >= ARRAY_SIZE(digital_regs))
         goto fail;
     LPC_GPIO_TypeDef *regs = digital_regs[GPIO2PORT(pin)];
-    uint32_t bit = GPIO2BIT(pin);
+    struct gpio_out g = { .regs=regs, .bit=GPIO2BIT(pin) };
+    gpio_out_reset(g, val);
+    return g;
+fail:
+    shutdown("Not an output pin");
+}
+
+void
+gpio_out_reset(struct gpio_out g, uint8_t val)
+{
+    LPC_GPIO_TypeDef *regs = g.regs;
+    int pin = regs_to_pin(regs, g.bit);
     irqstatus_t flag = irq_save();
     if (val)
-        regs->FIOSET = bit;
+        regs->FIOSET = g.bit;
     else
-        regs->FIOCLR = bit;
-    regs->FIODIR |= bit;
+        regs->FIOCLR = g.bit;
+    regs->FIODIR |= g.bit;
+    gpio_peripheral(GPIO2PORT(pin), pin % 32, 0, 0);
     irq_restore(flag);
-    return (struct gpio_out){ .regs=regs, .bit=bit };
-fail:
-    shutdown("Not an output pin");
 }
 
 void
@@ -101,18 +121,25 @@ gpio_in_setup(uint8_t pin, int8_t pull_up)
 {
     if (GPIO2PORT(pin) >= ARRAY_SIZE(digital_regs))
         goto fail;
-    uint32_t port = GPIO2PORT(pin);
-    LPC_GPIO_TypeDef *regs = digital_regs[port];
-    uint32_t bit = GPIO2BIT(pin);
-    irqstatus_t flag = irq_save();
-    gpio_peripheral(port, pin % 32, 0, pull_up);
-    regs->FIODIR &= ~bit;
-    irq_restore(flag);
-    return (struct gpio_in){ .regs=regs, .bit=bit };
+    LPC_GPIO_TypeDef *regs = digital_regs[GPIO2PORT(pin)];
+    struct gpio_in g = { .regs=regs, .bit=GPIO2BIT(pin) };
+    gpio_in_reset(g, pull_up);
+    return g;
 fail:
     shutdown("Not an input pin");
 }
 
+void
+gpio_in_reset(struct gpio_in g, int8_t pull_up)
+{
+    LPC_GPIO_TypeDef *regs = g.regs;
+    int pin = regs_to_pin(regs, g.bit);
+    irqstatus_t flag = irq_save();
+    gpio_peripheral(GPIO2PORT(pin), pin % 32, 0, pull_up);
+    regs->FIODIR &= ~g.bit;
+    irq_restore(flag);
+}
+
 uint8_t
 gpio_in_read(struct gpio_in g)
 {
diff --git a/src/lpc176x/gpio.h b/src/lpc176x/gpio.h
index a236ebfa..59041fe3 100644
--- a/src/lpc176x/gpio.h
+++ b/src/lpc176x/gpio.h
@@ -8,6 +8,7 @@ struct gpio_out {
     uint32_t bit;
 };
 struct gpio_out gpio_out_setup(uint8_t pin, uint8_t val);
+void gpio_out_reset(struct gpio_out g, uint8_t val);
 void gpio_out_toggle_noirq(struct gpio_out g);
 void gpio_out_toggle(struct gpio_out g);
 void gpio_out_write(struct gpio_out g, uint8_t val);
@@ -17,6 +18,7 @@ struct gpio_in {
     uint32_t bit;
 };
 struct gpio_in gpio_in_setup(uint8_t pin, int8_t pull_up);
+void gpio_in_reset(struct gpio_in g, int8_t pull_up);
 uint8_t gpio_in_read(struct gpio_in g);
 
 struct gpio_adc {