diff --git a/lib/gpio.c b/lib/gpio.c
index 6d1d21e4..abf47cf3 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;