[PATCH 3/7] arm/raspberrypi: correct GPIO pin function selection.

[ppisa4lists at pikron.com]

From: Pavel Pisa <ppisa at pikron.com>

Original implementation does only bitwise-or with previous register
value for all functions except IN. Switch from one to other function
would lead to incorrect value.
---
 c/src/lib/libbsp/arm/raspberrypi/gpio/rpi-gpio.c    | 18 +++++++++---------
 c/src/lib/libbsp/arm/raspberrypi/include/rpi-gpio.h |  3 ++-
 2 files changed, 11 insertions(+), 10 deletions(-)

diff --git a/c/src/lib/libbsp/arm/raspberrypi/gpio/rpi-gpio.c b/c/src/lib/libbsp/arm/raspberrypi/gpio/rpi-gpio.c
index 642666a..40acd84 100644
--- a/c/src/lib/libbsp/arm/raspberrypi/gpio/rpi-gpio.c
+++ b/c/src/lib/libbsp/arm/raspberrypi/gpio/rpi-gpio.c
@@ -52,15 +52,15 @@ static rtems_status_code rpi_select_pin_function(
   uint32_t type
 ) {
   /* Calculate the pin function select register address. */
-  volatile unsigned int *pin_addr = (unsigned int *) BCM2835_GPIO_REGS_BASE +
-                                    (pin / 10);
-
-  if ( type == RPI_DIGITAL_IN ) {
-    *(pin_addr) &= ~SELECT_PIN_FUNCTION(RPI_DIGITAL_IN, pin);
-  }
-  else {
-    *(pin_addr) |= SELECT_PIN_FUNCTION(type, pin);
-  }
+  volatile uint32_t *pin_addr = (uint32_t *) BCM2835_GPIO_REGS_BASE +
+                                             (pin / 10);
+  uint32_t reg_old;
+  uint32_t reg_new;
+
+  reg_new = reg_old = *pin_addr;
+  reg_new &= ~SELECT_PIN_FUNCTION(RPI_ALT_FUNC_MASK, pin);
+  reg_new |= SELECT_PIN_FUNCTION(type, pin);
+  *pin_addr = reg_new;
 
   return RTEMS_SUCCESSFUL;
 }
diff --git a/c/src/lib/libbsp/arm/raspberrypi/include/rpi-gpio.h b/c/src/lib/libbsp/arm/raspberrypi/include/rpi-gpio.h
index 7f4d802..82ba4b1 100644
--- a/c/src/lib/libbsp/arm/raspberrypi/include/rpi-gpio.h
+++ b/c/src/lib/libbsp/arm/raspberrypi/include/rpi-gpio.h
@@ -26,7 +26,7 @@ extern "C" {
 /**
  * @brief  Raspberry Pi GPIO functions.
  */
-#define RPI_DIGITAL_IN  7
+#define RPI_DIGITAL_IN  0
 #define RPI_DIGITAL_OUT 1
 #define RPI_ALT_FUNC_0  4
 #define RPI_ALT_FUNC_1  5
@@ -34,6 +34,7 @@ extern "C" {
 #define RPI_ALT_FUNC_3  7
 #define RPI_ALT_FUNC_4  3
 #define RPI_ALT_FUNC_5  2
+#define RPI_ALT_FUNC_MASK 7
 
 /**
  * @brief Setups a JTAG interface.
-- 
1.9.1