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.

2 files changed, 44 insertions, 39 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
+}
diff --git a/host/lib/usrp/x400/x400_gpio_control.hpp b/host/lib/usrp/x400/x400_gpio_control.hpp
index 01cfc134e..4e5aed587 100644
--- a/host/lib/usrp/x400/x400_gpio_control.hpp
+++ b/host/lib/usrp/x400/x400_gpio_control.hpp
@@ -16,6 +16,16 @@ namespace uhd { namespace rfnoc { namespace x400 {
 // The name of the X400's GPIO bank
 extern const char* GPIO_BANK_NAME;
 
+class x400_gpio_port_mapping : public uhd::mapper::gpio_port_mapper
+{
+public:
+    x400_gpio_port_mapping(){};
+
+    uint32_t map_value(const uint32_t& value) override;
+
+    uint32_t unmap_value(const uint32_t& value) override;
+};
+
 /*! Abstract X400's GPIO control to match the "gpio_attr" control scheme.
  *
  * The front panel has two ports on it, labelled GPIO0 and GPIO1. The registers
@@ -55,20 +65,15 @@ public:
     uint32_t get_gpio_attr(const usrp::gpio_atr::gpio_attr_t attr);
 
 private:
-    /*! Converts from the public-facing [23:0] format to the internal [31:16],
-     * [11:0] format.
-     */
-    static uint32_t internalize_value(const uint32_t value);
-
-    /*! Converts from the internal [31:16], [11:0] format to the public-facing
-     * [23:0] format.
+    /*! Convert from the internal FPGA pin mapping to the "DIO" mapping. This
+     * matches the "DIO_PORT_MAP" field in MPM's x4xx_periphs.py file.
      */
-    static uint32_t publicize_value(const uint32_t value);
+    uint32_t unmap_dio(const uint32_t raw_form);
 
-    /*! Convert from the internal FPGA pin mapping to the "DIO" mapping. This
+    /*! Convert from the "DIO" mapping to the internal FPGA pin mapping. This
      * matches the "DIO_PORT_MAP" field in MPM's x4xx_periphs.py file.
      */
-    static uint32_t unmap_dio(const uint32_t bank, const uint32_t raw_form);
+    uint32_t map_dio(const uint32_t user_form);
 
     /*! Returns whether the given attribute is setting one of the ATR entries.
      */
@@ -79,15 +84,8 @@ private:
 
     // There are two GPIOs, one for each channel. These two are set in unison.
     std::vector<usrp::gpio_atr::gpio_atr_3000::sptr> _gpios;
-};
-
-class x400_gpio_port_mapping : public uhd::mapper::gpio_port_mapper
-{
-public:
-    x400_gpio_port_mapping(){};
-
-    uint32_t map_value(const uint32_t& value) override;
 
-    uint32_t unmap_value(const uint32_t& value) override;
+    x400_gpio_port_mapping _mapper;
 };
-}}} // namespace uhd::rfnoc::x400
\ No newline at end of file
+
+}}} // namespace uhd::rfnoc::x400