aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--lib/gpio.c34
1 files changed, 25 insertions, 9 deletions
diff --git a/lib/gpio.c b/lib/gpio.c
index 6d1d21e..abf47cf 100644
--- a/lib/gpio.c
+++ b/lib/gpio.c
@@ -38,19 +38,35 @@
void gpio_set_mode(u32 gpioport, u8 mode, u8 cnf, u16 gpios)
{
- u16 i, movingbit = 1, offset = 0;
- u32 crl = 0, crh = 0;
+ u16 i, offset = 0;
+ u32 crl = 0, crh = 0, tmp32 = 0;
+ /*
+ * We want to set the config only for the pins mentioned in gpios,
+ * but keeping the others, so read out the actual config first.
+ */
+ crl = GPIO_CRL(gpioport);
+ crh = GPIO_CRH(gpioport);
+
+ /* Iterate over all bits, use i as the bitnumber. */
for (i = 0; i < 16; i++) {
- if ((movingbit & gpios) != movingbit) {
- movingbit <<= 1;
+ /* Only set the config if the bit is set in gpios. */
+ if (!((1 << i) & gpios))
continue;
- }
+
+ /* Calculate bit offset. */
offset = (i < 8) ? (i * 4) : ((i - 8) * 4);
- if (i < 8)
- crl |= (mode << offset) | (cnf << (offset + 2));
- else
- crh |= (mode << offset) | (cnf << (offset + 2));
+
+ /* Use tmp32 to either modify crl or crh. */
+ tmp32 = (i < 8) ? crl : crh;
+
+ /* Modify bits are needed. */
+ tmp32 &= ~(0b1111 << offset); /* Clear the bits first. */
+ tmp32 |= (mode << offset) | (cnf << (offset + 2));
+
+ /* Write tmp32 into crl or crh, leave the other unchanged. */
+ crl = (i < 8) ? tmp32 : crl;
+ crh = (i >= 8) ? tmp32 : crh;
}
GPIO_CRL(gpioport) = crl;