We introduce helpers to read/write into the circular queues. smmuv3_read_cmdq and smmuv3_write_evtq will become static later on.
Signed-off-by: Eric Auger <eric.au...@redhat.com> --- hw/arm/smmuv3-internal.h | 48 ++++++++++++++++++++++++++++++- hw/arm/smmuv3.c | 75 +++++++++++++++++++++++++++++++++++++++++++++++- 2 files changed, 121 insertions(+), 2 deletions(-) diff --git a/hw/arm/smmuv3-internal.h b/hw/arm/smmuv3-internal.h index 2b44ee2..d88f141 100644 --- a/hw/arm/smmuv3-internal.h +++ b/hw/arm/smmuv3-internal.h @@ -215,7 +215,53 @@ static inline int smmu_enabled(SMMUV3State *s) #define SMMU_CMDQ_ERR(s) (SMMU_PENDING_GERRORS(s) & SMMU_GERROR_CMDQ) -void smmuv3_irq_trigger(SMMUV3State *s, SMMUIrq irq, uint32_t gerror_val); void smmuv3_write_gerrorn(SMMUV3State *s, uint32_t gerrorn); +/*************************** + * Queue Handling + ***************************/ + +typedef enum { + CMD_Q_EMPTY, + CMD_Q_FULL, + CMD_Q_PARTIALLY_FILLED, +} SMMUQStatus; + +#define Q_ENTRY(q, idx) (q->base + q->ent_size * idx) +#define Q_WRAP(q, pc) ((pc) >> (q)->shift) +#define Q_IDX(q, pc) ((pc) & ((1 << (q)->shift) - 1)) + +static inline SMMUQStatus __smmu_queue_status(SMMUV3State *s, SMMUQueue *q) +{ + uint32_t prod = Q_IDX(q, q->prod); + uint32_t cons = Q_IDX(q, q->cons); + + if ((prod == cons) && (q->wrap.prod != q->wrap.cons)) { + return CMD_Q_FULL; + } else if ((prod == cons) && (q->wrap.prod == q->wrap.cons)) { + return CMD_Q_EMPTY; + } + return CMD_Q_PARTIALLY_FILLED; +} +#define smmu_is_q_full(s, q) (__smmu_queue_status(s, q) == CMD_Q_FULL) +#define smmu_is_q_empty(s, q) (__smmu_queue_status(s, q) == CMD_Q_EMPTY) + +static inline int __smmu_q_enabled(SMMUV3State *s, uint32_t q) +{ + return smmu_read32_reg(s, SMMU_REG_CR0) & q; +} +#define smmu_cmd_q_enabled(s) __smmu_q_enabled(s, SMMU_CR0_CMDQ_ENABLE) +#define smmu_evt_q_enabled(s) __smmu_q_enabled(s, SMMU_CR0_EVTQ_ENABLE) + +static inline void smmu_write_cmdq_err(SMMUV3State *s, uint32_t err_type) +{ + uint32_t regval = smmu_read32_reg(s, SMMU_REG_CMDQ_CONS); + + smmu_write32_reg(s, SMMU_REG_CMDQ_CONS, + regval | err_type << SMMU_CMD_CONS_ERR_SHIFT); +} + +MemTxResult smmuv3_read_cmdq(SMMUV3State *s, Cmd *cmd); +void smmuv3_write_evtq(SMMUV3State *s, Evt *evt); + #endif diff --git a/hw/arm/smmuv3.c b/hw/arm/smmuv3.c index 468134f..2f96463 100644 --- a/hw/arm/smmuv3.c +++ b/hw/arm/smmuv3.c @@ -36,7 +36,7 @@ * @irq: irq type * @gerror: gerror new value, only relevant if @irq is GERROR */ -void smmuv3_irq_trigger(SMMUV3State *s, SMMUIrq irq, uint32_t gerror_val) +static void smmuv3_irq_trigger(SMMUV3State *s, SMMUIrq irq, uint32_t gerror_val) { uint32_t pending_gerrors = SMMU_PENDING_GERRORS(s); bool pulse = false; @@ -84,6 +84,79 @@ void smmuv3_write_gerrorn(SMMUV3State *s, uint32_t gerrorn) trace_smmuv3_write_gerrorn(gerrorn, sanitized, SMMU_PENDING_GERRORS(s)); } +static MemTxResult smmu_q_read(SMMUQueue *q, void *data) +{ + uint64_t addr = Q_ENTRY(q, Q_IDX(q, q->cons)); + MemTxResult ret; + + ret = smmu_read_sysmem(addr, data, q->ent_size, false); + if (ret != MEMTX_OK) { + return ret; + } + + q->cons++; + if (q->cons == q->entries) { + q->cons = 0; + q->wrap.cons++; + } + + return ret; +} + +static void smmu_q_write(SMMUQueue *q, void *data) +{ + uint64_t addr = Q_ENTRY(q, Q_IDX(q, q->prod)); + + smmu_write_sysmem(addr, data, q->ent_size, false); + + q->prod++; + if (q->prod == q->entries) { + q->prod = 0; + q->wrap.prod++; + } +} + +MemTxResult smmuv3_read_cmdq(SMMUV3State *s, Cmd *cmd) +{ + SMMUQueue *q = &s->cmdq; + MemTxResult ret = smmu_q_read(q, cmd); + uint32_t val = 0; + + if (ret != MEMTX_OK) { + return ret; + } + + val |= (q->wrap.cons << q->shift) | q->cons; + smmu_write32_reg(s, SMMU_REG_CMDQ_CONS, val); + + return ret; +} + +void smmuv3_write_evtq(SMMUV3State *s, Evt *evt) +{ + SMMUQueue *q = &s->evtq; + bool was_empty = smmu_is_q_empty(s, q); + bool was_full = smmu_is_q_full(s, q); + uint32_t val; + + if (!smmu_evt_q_enabled(s)) { + return; + } + + if (was_full) { + return; + } + + smmu_q_write(q, evt); + + val = (q->wrap.prod << q->shift) | q->prod; + smmu_write32_reg(s, SMMU_REG_EVTQ_PROD, val); + + if (was_empty) { + smmuv3_irq_trigger(s, SMMU_IRQ_EVTQ, 0); + } +} + static void smmuv3_init_regs(SMMUV3State *s) { uint32_t data = -- 2.5.5