[PATCH v5 09/22] xen/arm: vsmmuv3: Add support for cmdqueue handling

[Milan Djokic <milan_djokic@xxxxxxxx>]

From: Rahul Singh <rahul.singh@xxxxxxx>

Add support for virtual cmdqueue handling for guests

Signed-off-by: Rahul Singh <rahul.singh@xxxxxxx>
Signed-off-by: Milan Djokic <milan_djokic@xxxxxxxx>
---
 xen/drivers/passthrough/arm/vsmmu-v3.c | 133 +++++++++++++++++++++++++
 1 file changed, 133 insertions(+)

diff --git a/xen/drivers/passthrough/arm/vsmmu-v3.c 
b/xen/drivers/passthrough/arm/vsmmu-v3.c
index 070fe7af79..780f36bf2a 100644
--- a/xen/drivers/passthrough/arm/vsmmu-v3.c
+++ b/xen/drivers/passthrough/arm/vsmmu-v3.c
@@ -8,6 +8,7 @@
  * This needs to be fully handled in the future.
  */
 
+#include <xen/guest_access.h>
 #include <xen/param.h>
 #include <xen/sched.h>
 #include <xen/domain-layout.h>
@@ -34,6 +35,51 @@
 #define SMMU_EVTQS          19
 #define DWORDS_BYTES        8
 
+/* 
+ * SMMUv3 command definitions
+ * Some commands are fully handled by the emulation layer, while others are
+ * currently treated as architectural no-ops because the required behavior
+ * is either implicitly guaranteed by Xen or not yet modeled explicitly.
+ *
+ * Emulation handled commands:
+ *
+ * - CMD_CFGI_STE
+ * - CMD_TLBI_NH_ASID
+ * - CMD_TLBI_NSNH_ALL
+ * - CMD_TLBI_NH_VA
+ *
+ * No-op/Implicitly handled commands:
+ *
+ * - CMD_SYNC
+ * - CMD_PREFETCH_CFG
+ * - CMD_CFGI_CD
+ * - CMD_CFGI_CD_ALL
+ * - CMD_CFGI_ALL
+ *
+ * TODO: Remaining architecture-defined commands are not supported (error
+ * produced), due to lack of support in SMMUv3 driver / emulation layer
+ * TODO: Range / per-device invalidation not supported atm
+ * TODO: Long-running command queue handling is not implemented (command
+ * consumption should be budgeted with asynchronous resumption).
+ */
+#define CMDQ_OP_PREFETCH_CFG    0x1
+#define CMDQ_OP_CFGI_STE        0x3
+#define CMDQ_OP_CFGI_ALL        0x4
+#define CMDQ_OP_CFGI_CD         0x5
+#define CMDQ_OP_CFGI_CD_ALL     0x6
+#define CMDQ_OP_TLBI_NH_ASID    0x11
+#define CMDQ_OP_TLBI_NH_VA      0x12
+#define CMDQ_OP_TLBI_NSNH_ALL   0x30
+#define CMDQ_OP_CMD_SYNC        0x46
+
+/* Queue Handling */
+#define Q_BASE(q)       ((q)->q_base & Q_BASE_ADDR_MASK)
+#define Q_CONS_ENT(q)   (Q_BASE(q) + Q_IDX(q, (q)->cons) * (q)->ent_size)
+#define Q_PROD_ENT(q)   (Q_BASE(q) + Q_IDX(q, (q)->prod) * (q)->ent_size)
+
+/* Helper Macros */
+#define smmu_cmd_get_command(x)     FIELD_GET(CMDQ_0_OP, x)
+
 /* virtual smmu queue */
 struct arm_vsmmu_queue {
     uint64_t    q_base; /* base register */
@@ -100,6 +146,89 @@ static inline bool smmu_get_evtq_enabled(struct virt_smmu 
*smmu)
     spin_unlock(&smmu->cr0_lock);
 
     return enabled;
+};
+
+/* Queue manipulation functions */
+static bool queue_empty(struct arm_vsmmu_queue *q)
+{
+    return Q_IDX(q, q->prod) == Q_IDX(q, q->cons) &&
+           Q_WRP(q, q->prod) == Q_WRP(q, q->cons);
+}
+
+static void queue_inc_cons(struct arm_vsmmu_queue *q)
+{
+    uint32_t cons = (Q_WRP(q, q->cons) | Q_IDX(q, q->cons)) + 1;
+    q->cons = Q_OVF(q->cons) | Q_WRP(q, cons) | Q_IDX(q, cons);
+}
+
+static void dump_smmu_command(uint64_t *command)
+{
+    gprintk(XENLOG_ERR, "cmd 0x%02llx: %016"PRIx64" %016"PRIx64"\n",
+             smmu_cmd_get_command(command[0]), command[0], command[1]);
+}
+static int arm_vsmmu_handle_cmds(struct virt_smmu *smmu)
+{
+    struct arm_vsmmu_queue *q = &smmu->cmdq;
+    struct domain *d = smmu->d;
+    uint64_t command[CMDQ_ENT_DWORDS];
+    paddr_t addr;
+    int ret = 0;
+
+    if ( !smmu_get_cmdq_enabled(smmu) )
+        return 0;
+
+    /*
+     * TODO: A large number of commands could cause this loop to run for
+     * an extended period while holding the command queue lock, potentially
+     * monopolizing CPU time. Implement commands rescheduling and asynchronous
+     * execution as outlined in the design.
+     */
+    while ( !queue_empty(q) )
+    {
+        int ret;
+
+        addr = Q_CONS_ENT(q);
+        ret = access_guest_memory_by_gpa(d, addr, command,
+                                         sizeof(command), false);
+        if ( ret ) {
+            queue_inc_cons(q);
+            return ret;
+        }
+
+        switch ( smmu_cmd_get_command(command[0]) )
+        {
+        case CMDQ_OP_CFGI_STE:
+            break;
+        case CMDQ_OP_PREFETCH_CFG:
+        case CMDQ_OP_CFGI_CD:
+        case CMDQ_OP_CFGI_CD_ALL:
+        case CMDQ_OP_CFGI_ALL:
+        case CMDQ_OP_CMD_SYNC:
+            break;
+        case CMDQ_OP_TLBI_NH_ASID:
+        case CMDQ_OP_TLBI_NSNH_ALL:
+        case CMDQ_OP_TLBI_NH_VA:
+            ret = iommu_iotlb_flush_all(smmu->d, 1);
+            if ( !ret )
+                break;
+            fallthrough;
+        default:
+            gdprintk(XENLOG_ERR, "vSMMUv3: unhandled command\n");
+            dump_smmu_command(command);
+            break;
+        }
+
+        if ( ret )
+        {
+            gdprintk(XENLOG_ERR,
+                     "vSMMUv3: command error %d while handling command\n",
+                     ret);
+            dump_smmu_command(command);
+        }
+        queue_inc_cons(q);
+    }
+
+    return ret;
 }
 
 static int vsmmuv3_mmio_write(struct vcpu *v, mmio_info_t *info,
@@ -222,6 +351,10 @@ static int vsmmuv3_mmio_write(struct vcpu *v, mmio_info_t 
*info,
             reg32 = smmu->cmdq.prod;
             vreg_reg32_update(&reg32, r, info);
             smmu->cmdq.prod = reg32;
+
+            if ( arm_vsmmu_handle_cmds(smmu) )
+                gdprintk(XENLOG_ERR, "error handling vSMMUv3 commands\n");
+
             spin_unlock(&smmu->cmd_queue_lock);
             break;
 
-- 
2.43.0