When a VCMDQ is mapped and VINTF page0 is available, read and write
VCMDQ registers directly via the VINTF page0 backing instead of using
cached register values.

Signed-off-by: Shameer Kolothum <skolothumtho@nvidia.com>
---
 hw/arm/tegra241-cmdqv.c | 44 +++++++++++++++++++++++++++++++++++++++--
 1 file changed, 42 insertions(+), 2 deletions(-)

diff --git a/hw/arm/tegra241-cmdqv.c b/hw/arm/tegra241-cmdqv.c
index 153fd70edb..c7e70d8e1d 100644
--- a/hw/arm/tegra241-cmdqv.c
+++ b/hw/arm/tegra241-cmdqv.c
@@ -38,12 +38,32 @@ static bool tegra241_cmdqv_mmap_vintf_page0(Tegra241CMDQV *cmdqv, Error **errp)
  *
  * The caller normalizes the MMIO offset such that @offset0 always refers
  * to a VCMDQ0_* register, while @index selects the VCMDQ instance.
- *
- * All VCMDQ accesses are currently trapped. Use cached registers
  */
 static uint64_t tegra241_cmdqv_read_vcmdq(Tegra241CMDQV *cmdqv, hwaddr offset0,
                                           int index)
 {
+
+    /*
+     * If this VCMDQ is mapped and VINTF page0 is available, read directly
+     * from the VINTF page0 backing. Otherwise, fall back to cached state.
+     */
+    if (cmdqv->vcmdq[index] && cmdqv->vintf_page0_mapped) {
+        uint64_t off = (index * 0x80) + (offset0 - 0x10000);
+        uint32_t *ptr = (uint32_t *)(cmdqv->vintf_page0 + off);
+
+        switch (offset0) {
+        case A_VCMDQ0_CONS_INDX:
+        case A_VCMDQ0_PROD_INDX:
+        case A_VCMDQ0_CONFIG:
+        case A_VCMDQ0_STATUS:
+        case A_VCMDQ0_GERROR:
+        case A_VCMDQ0_GERRORN:
+            return *ptr;
+        default:
+            break;
+        }
+    }
+
     switch (offset0) {
     case A_VCMDQ0_CONS_INDX:
         return cmdqv->vcmdq_cons_indx[index];
@@ -241,6 +261,26 @@ tegra241_cmdqv_write_vcmdq(Tegra241CMDQV *cmdqv, hwaddr offset0, int index,
                            uint64_t value, unsigned size, Error **errp)
 {
 
+    /*
+     * If this VCMDQ is mapped and VINTF page0 is available, write directly
+     * to the VINTF page0 backing. Otherwise, update cached state.
+     */
+    if (cmdqv->vcmdq[index] && cmdqv->vintf_page0_mapped) {
+        uint64_t off = (index * 0x80) + (offset0 - 0x10000);
+        uint32_t *ptr = (uint32_t *)(cmdqv->vintf_page0 + off);
+
+        switch (offset0) {
+        case A_VCMDQ0_CONS_INDX:
+        case A_VCMDQ0_PROD_INDX:
+        case A_VCMDQ0_CONFIG:
+        case A_VCMDQ0_GERRORN:
+            *ptr = (uint32_t)value;
+            return;
+        default:
+            break;
+        }
+    }
+
     switch (offset0) {
     case A_VCMDQ0_CONS_INDX:
         cmdqv->vcmdq_cons_indx[index] = value;
-- 
2.43.0