2 files changed, 62 insertions, 0 deletions

diff --git a/src/base/addr_range.hh b/src/base/addr_range.hh
index cda6ccfb2..84a3d4de5 100644
--- a/src/base/addr_range.hh
+++ b/src/base/addr_range.hh
@@ -471,6 +471,44 @@ class AddrRange
     }
 
     /**
+     * This method adds the interleaving bits removed by
+     * removeIntlvBits.
+     */
+    inline Addr addIntlvBits(Addr a) const
+    {
+        // Get the LSB set from each mask
+        int masks_lsb[masks.size()];
+        for (int i = 0; i < masks.size(); i++) {
+            masks_lsb[i] = ctz64(masks[i]);
+        }
+
+        // Add bits one-by-one from the LSB side.
+        std::sort(masks_lsb, masks_lsb + masks.size());
+        for (int i = 0; i < masks.size(); i++) {
+            const int intlv_bit = masks_lsb[i];
+            if (intlv_bit > 0) {
+                // on every iteration we add one bit from the input
+                // address, and therefore the lowest invtl_bit has
+                // also shifted to the left by i positions.
+                a = insertBits(a << 1, intlv_bit + i - 1, 0, a);
+            } else {
+                a <<= 1;
+            }
+        }
+
+        for (int i = 0; i < masks.size(); i++) {
+            const int lsb = ctz64(masks[i]);
+            const Addr intlv_bit = bits(intlvMatch, i);
+            // Calculate the mask ignoring the LSB
+            const Addr masked = a & masks[i] & ~(1 << lsb);
+            // Set the LSB of the mask to whatever satisfies the selector bit
+            a = insertBits(a, lsb, intlv_bit ^ popCount(masked));
+        }
+
+        return a;
+    }
+
+    /**
      * Determine the offset of an address within the range.
      *
      * This function returns the offset of the given address from the
diff --git a/src/base/addr_range.test.cc b/src/base/addr_range.test.cc
index 54eb1984c..93afbb0e7 100644
--- a/src/base/addr_range.test.cc
+++ b/src/base/addr_range.test.cc
@@ -121,6 +121,16 @@ class AddrRangeBase : public testing::Test {
         }
     }
 
+    void testAddRemoveIntlvBits()
+    {
+        for (Addr addr = start; addr <= end; addr++) {
+            AddrRange &r = range[getIndex(addr)];
+            Addr ch_addr = r.removeIntlvBits(addr);
+            Addr pa = r.addIntlvBits(ch_addr);
+            ASSERT_EQ(addr, pa);
+        }
+    }
+
     static const Addr end = 0x1ffff;
     static const Addr start = 0x0;
     static const int intlvSize = 4;
@@ -162,6 +172,11 @@ TEST_F(AddrRangeCont, AddrRangeGetOffset)
     testGetOffset();
 }
 
+TEST_F(AddrRangeCont, AddrRangeAddRemoveIntlvBits)
+{
+    testAddRemoveIntlvBits();
+}
+
 
 class AddrRangeContLegacy : public AddrRangeCont {
   protected:
@@ -185,6 +200,10 @@ TEST_F(AddrRangeContLegacy, AddrRangeGetOffset)
     testGetOffset();
 }
 
+TEST_F(AddrRangeContLegacy, AddrRangeAddRemoveIntlvBits)
+{
+    testAddRemoveIntlvBits();
+}
 
 class AddrRangeArb : public AddrRangeBase {
   protected:
@@ -218,3 +237,8 @@ TEST_F(AddrRangeArb, AddrRangeGetOffset)
 {
     testGetOffset();
 }
+
+TEST_F(AddrRangeArb, AddrRangeAddRemoveIntlvBits)
+{
+    testAddRemoveIntlvBits();
+}