host: x4xx: gpio: Properly unmap FPGA GPIO values

The FPGA GPIO registers don't exactly match the pin numbering on the
front panel and in the docs. This commit changes the algorithm so that
the API presented to the user matches the front panel.

1 files changed, 26 insertions, 19 deletions

diff --git a/host/lib/usrp/x400/x400_gpio_control.cpp b/host/lib/usrp/x400/x400_gpio_control.cpp
index 18599eb88..706364e87 100644
--- a/host/lib/usrp/x400/x400_gpio_control.cpp
+++ b/host/lib/usrp/x400/x400_gpio_control.cpp
@@ -32,6 +32,7 @@ constexpr uint32_t DIO_DIRECTION_REG = 0x4;
 } // namespace gpio_regmap
 
 // There are two ports, each with 12 pins
+constexpr size_t NUM_PORTS = 2;
 constexpr size_t NUM_PINS_PER_PORT = 12;
 
 // Start of Port B pin numbers relative to Port A:
@@ -84,9 +85,10 @@ void gpio_control::set_gpio_attr(
         _rpcc->dio_set_pin_directions("PORTB", value >> 12);
     }
 
-    _gpios[0]->set_gpio_attr(attr, internalize_value(value));
+    const uint32_t internal_value = map_dio(value);
+    _gpios[0]->set_gpio_attr(attr, internal_value);
     if (is_atr_attr(attr)) {
-        _gpios[1]->set_gpio_attr(attr, internalize_value(value));
+        _gpios[1]->set_gpio_attr(attr, internal_value);
     }
 }
 
@@ -98,23 +100,28 @@ bool gpio_control::is_atr_attr(const uhd::usrp::gpio_atr::gpio_attr_t attr)
            || attr == uhd::usrp::gpio_atr::GPIO_ATR_XX;
 }
 
-uint32_t gpio_control::internalize_value(const uint32_t value)
+uint32_t gpio_control::unmap_dio(const uint32_t raw_form)
 {
-    return (value & 0xFFF) | ((value & 0x00FFF000) << 4);
-}
-
-uint32_t gpio_control::publicize_value(const uint32_t value)
-{
-    return (value & 0xFFF) | ((value & 0x0FFF0000) >> 4);
+    uint32_t result = 0;
+    for (size_t i = 0; i < NUM_PINS_PER_PORT; i++) {
+        if ((raw_form & (1 << i)) != 0) {
+            result |= 1 << _mapper.unmap_value(i);
+        }
+    }
+    for (size_t i = PORT_NUMBER_OFFSET; i < PORT_NUMBER_OFFSET + NUM_PINS_PER_PORT; i++) {
+        if ((raw_form & (1 << i)) != 0) {
+            result |= 1 << _mapper.unmap_value(i);
+        }
+    }
+    return result;
 }
 
-uint32_t gpio_control::unmap_dio(const uint32_t bank, const uint32_t raw_form)
+uint32_t gpio_control::map_dio(const uint32_t user_form)
 {
-    const uint32_t* const mapping = bank == 1 ? PORTB_MAPPING : PORTA_MAPPING;
-    uint32_t result               = 0;
-    for (size_t i = 0; i < NUM_PINS_PER_PORT; i++) {
-        if ((raw_form & (1 << i)) != 0) {
-            result |= 1 << mapping[i];
+    uint32_t result = 0;
+    for (size_t i = 0; i < NUM_PORTS * NUM_PINS_PER_PORT; i++) {
+        if ((user_form & (1 << i)) != 0) {
+            result |= 1 << _mapper.map_value(i);
         }
     }
     return result;
@@ -126,11 +133,11 @@ uint32_t gpio_control::get_gpio_attr(const uhd::usrp::gpio_atr::gpio_attr_t attr
         // Retrieve the actual state from the FPGA mirror of the CPLD state
         const uint32_t raw_value = _regs->peek32(
             gpio_regmap::DIO_MIRROR_WINDOW + gpio_regmap::DIO_DIRECTION_REG);
-        return (unmap_dio(1, raw_value >> 16) << NUM_PINS_PER_PORT)
-               | unmap_dio(0, raw_value & 0xFFFF);
+        return unmap_dio(raw_value);
     }
 
-    return publicize_value(_gpios[0]->get_attr_reg(attr));
+    const uint32_t raw_value = _gpios[0]->get_attr_reg(attr);
+    return unmap_dio(raw_value);
 }
 
 uint32_t uhd::rfnoc::x400::x400_gpio_port_mapping::map_value(const uint32_t& value)
@@ -155,4 +162,4 @@ uint32_t uhd::rfnoc::x400::x400_gpio_port_mapping::unmap_value(const uint32_t& v
     const uint32_t* const mapping = bank == 1 ? PORTB_MAPPING : PORTA_MAPPING;
     UHD_ASSERT_THROW(pin_number < NUM_PINS_PER_PORT);
     return mapping[pin_number] + (bank * NUM_PINS_PER_PORT);
-}
\ No newline at end of file
+}