dev-arm: Fix SMMUv3 CMDQ wrapping

SMMU circular queues have a wrap bit which is used in order to
distinguish between an empty queue and a full queue.

According to SMMUv3 spec:

Each index has a wrap flag, represented by the next higher bit adjacent
to the index value contained in PROD and CONS. This bit must toggle each
time the index wraps off the high end and back onto the low end of the
buffer. It is the responsibility of the owner of each index, producer or
consumer, to toggle this bit when the owner updates the index after
wrapping. It is intended that software reads the register, increments or
wraps the index (toggling wrap when required) and writes back both wrap
and index fields at the same time.

Change-Id: Idfeb397141f3627c2878caaeaa2625fadf671d2a
Signed-off-by: Giacomo Travaglini <giacomo.travaglini@arm.com>
Reviewed-by: Michiel Van Tol <michiel.vantol@arm.com>
Reviewed-by: Adrian Herrera <adrian.herrera@arm.com>
Reviewed-on: https://gem5-review.googlesource.com/c/public/gem5/+/19311
Tested-by: kokoro <noreply+kokoro@google.com>
Reviewed-by: Ciro Santilli <ciro.santilli@arm.com>
Maintainer: Andreas Sandberg <andreas.sandberg@arm.com>

1 files changed, 15 insertions, 7 deletions

diff --git a/src/dev/arm/smmu_v3_cmdexec.cc b/src/dev/arm/smmu_v3_cmdexec.cc
index 48896bf96..5b9dc6e4a 100644
--- a/src/dev/arm/smmu_v3_cmdexec.cc
+++ b/src/dev/arm/smmu_v3_cmdexec.cc
@@ -56,20 +56,28 @@ SMMUCommandExecProcess::main(Yield &yield)
         busy = true;
 
         while (true) {
-            int sizeMask = mask(smmu.regs.cmdq_base & Q_BASE_SIZE_MASK);
+            // Masking depending on CMDQ_BASE.LOG2SIZE (log(number of
+            // queue entries)). Example: a value of 0b101 (32 entries)
+            // generates a 0b11111 mask.
+            int size_mask = mask(
+                smmu.regs.cmdq_base & Q_BASE_SIZE_MASK);
 
-            if ((smmu.regs.cmdq_cons & sizeMask) ==
-                    (smmu.regs.cmdq_prod & sizeMask))
+            // In this case the wrap bit is considered (+1)
+            int size_mask_wrap = mask(
+                (smmu.regs.cmdq_base & Q_BASE_SIZE_MASK) + 1);
+
+            if ((smmu.regs.cmdq_cons & size_mask_wrap) ==
+                    (smmu.regs.cmdq_prod & size_mask_wrap))
                 break; // command queue empty
 
-            Addr cmdAddr =
+            Addr cmd_addr =
                 (smmu.regs.cmdq_base & Q_BASE_ADDR_MASK) +
-                (smmu.regs.cmdq_cons & sizeMask) * sizeof(SMMUCommand);
+                (smmu.regs.cmdq_cons & size_mask) * sizeof(SMMUCommand);
 
             // This deliberately resets the error field in cmdq_cons!
-            smmu.regs.cmdq_cons = (smmu.regs.cmdq_cons + 1) & sizeMask;
+            smmu.regs.cmdq_cons = (smmu.regs.cmdq_cons + 1) & size_mask_wrap;
 
-            doRead(yield, cmdAddr, &cmd, sizeof(SMMUCommand));
+            doRead(yield, cmd_addr, &cmd, sizeof(SMMUCommand));
             smmu.processCommand(cmd);
         }