/* $OpenBSD: smmu.c,v 1.29 2026/01/06 11:57:33 patrick Exp $ */
/*
 * Copyright (c) 2008-2009,2014-2016 Dale Rahn <drahn@dalerahn.com>
 * Copyright (c) 2021 Patrick Wildt <patrick@blueri.se>
 *
 * Permission to use, copy, modify, and distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/device.h>
#include <sys/pool.h>
#include <sys/atomic.h>

#include <machine/bus.h>
#include <machine/cpufunc.h>

#include <uvm/uvm_extern.h>
#include <arm64/vmparam.h>
#include <arm64/pmap.h>

#include <dev/pci/pcivar.h>
#include <arm64/dev/smmuvar.h>
#include <arm64/dev/smmureg.h>

struct smmu_map_state {
        struct extent_region    sms_er;
        bus_addr_t              sms_dva;
        bus_size_t              sms_len;
        bus_size_t              sms_loaded;
};

struct smmuvp0 {
        uint64_t l0[VP_IDX0_CNT];
        struct smmuvp1 *vp[VP_IDX0_CNT];
};

struct smmuvp1 {
        uint64_t l1[VP_IDX1_CNT];
        struct smmuvp2 *vp[VP_IDX1_CNT];
};

struct smmuvp2 {
        uint64_t l2[VP_IDX2_CNT];
        struct smmuvp3 *vp[VP_IDX2_CNT];
};

struct smmuvp3 {
        uint64_t l3[VP_IDX3_CNT];
};

CTASSERT(sizeof(struct smmuvp0) == sizeof(struct smmuvp1));
CTASSERT(sizeof(struct smmuvp0) == sizeof(struct smmuvp2));
CTASSERT(sizeof(struct smmuvp0) != sizeof(struct smmuvp3));

uint32_t smmu_gr0_read_4(struct smmu_softc *, bus_size_t);
void smmu_gr0_write_4(struct smmu_softc *, bus_size_t, uint32_t);
uint32_t smmu_gr1_read_4(struct smmu_softc *, bus_size_t);
void smmu_gr1_write_4(struct smmu_softc *, bus_size_t, uint32_t);
uint32_t smmu_cb_read_4(struct smmu_softc *, int, bus_size_t);
void smmu_cb_write_4(struct smmu_softc *, int, bus_size_t, uint32_t);
uint64_t smmu_cb_read_8(struct smmu_softc *, int, bus_size_t);
void smmu_cb_write_8(struct smmu_softc *, int, bus_size_t, uint64_t);

int smmu_v2_domain_create(struct smmu_domain *);
void smmu_v2_tlbi_va(struct smmu_domain *, vaddr_t);
void smmu_v2_tlb_sync_global(struct smmu_softc *);
void smmu_v2_tlb_sync_context(struct smmu_domain *);

struct smmu_domain *smmu_domain_lookup(struct smmu_softc *, uint32_t);
struct smmu_domain *smmu_domain_create(struct smmu_softc *, uint32_t);

void smmu_set_l1(struct smmu_domain *, uint64_t, struct smmuvp1 *);
void smmu_set_l2(struct smmu_domain *, uint64_t, struct smmuvp1 *,
    struct smmuvp2 *);
void smmu_set_l3(struct smmu_domain *, uint64_t, struct smmuvp2 *,
    struct smmuvp3 *);

int smmu_vp_lookup(struct smmu_domain *, vaddr_t, uint64_t **);
int smmu_vp_enter(struct smmu_domain *, vaddr_t, uint64_t **, int);

uint64_t smmu_fill_pte(struct smmu_domain *, vaddr_t, paddr_t,
    vm_prot_t, int, int);
void smmu_pte_update(struct smmu_domain *, uint64_t, uint64_t *);
void smmu_pte_remove(struct smmu_domain *, vaddr_t);

int smmu_enter(struct smmu_domain *, vaddr_t, paddr_t, vm_prot_t, int, int);
void smmu_map(struct smmu_domain *, vaddr_t, paddr_t, vm_prot_t, int, int);
void smmu_unmap(struct smmu_domain *, vaddr_t);
void smmu_remove(struct smmu_domain *, vaddr_t);

int smmu_load_map(struct smmu_domain *, bus_dmamap_t);
void smmu_unload_map(struct smmu_domain *, bus_dmamap_t);

int smmu_dmamap_create(bus_dma_tag_t , bus_size_t, int,
     bus_size_t, bus_size_t, int, bus_dmamap_t *);
void smmu_dmamap_destroy(bus_dma_tag_t , bus_dmamap_t);
int smmu_dmamap_load(bus_dma_tag_t , bus_dmamap_t, void *,
     bus_size_t, struct proc *, int);
int smmu_dmamap_load_mbuf(bus_dma_tag_t , bus_dmamap_t,
     struct mbuf *, int);
int smmu_dmamap_load_uio(bus_dma_tag_t , bus_dmamap_t,
     struct uio *, int);
int smmu_dmamap_load_raw(bus_dma_tag_t , bus_dmamap_t,
     bus_dma_segment_t *, int, bus_size_t, int);
void smmu_dmamap_unload(bus_dma_tag_t , bus_dmamap_t);

uint32_t smmu_v3_read_4(struct smmu_softc *, bus_size_t);
void smmu_v3_write_4(struct smmu_softc *, bus_size_t, uint32_t);
uint64_t smmu_v3_read_8(struct smmu_softc *, bus_size_t);
void smmu_v3_write_8(struct smmu_softc *, bus_size_t, uint64_t);
int smmu_v3_write_ack(struct smmu_softc *, bus_size_t, bus_size_t,
     uint32_t);

int smmu_v3_domain_create(struct smmu_domain *);
void smmu_v3_cfgi_all(struct smmu_softc *);
void smmu_v3_cfgi_cd(struct smmu_domain *);
void smmu_v3_cfgi_ste(struct smmu_domain *);
void smmu_v3_tlbi_all(struct smmu_softc *, uint64_t);
void smmu_v3_tlbi_asid(struct smmu_domain *);
void smmu_v3_tlbi_va(struct smmu_domain *, vaddr_t);
void smmu_v3_tlb_sync_context(struct smmu_domain *);

struct cfdriver smmu_cd = {
        NULL, "smmu", DV_DULL
};

int
smmu_attach(struct smmu_softc *sc)
{
        SIMPLEQ_INIT(&sc->sc_domains);

        pool_init(&sc->sc_vp_pool, sizeof(struct smmuvp0), PAGE_SIZE, IPL_VM, 0,
            "smmu_vp", NULL);
        pool_setlowat(&sc->sc_vp_pool, 20);
        pool_init(&sc->sc_vp3_pool, sizeof(struct smmuvp3), PAGE_SIZE, IPL_VM, 0,
            "smmu_vp3", NULL);
        pool_setlowat(&sc->sc_vp3_pool, 20);

        return 0;
}

int
smmu_v2_attach(struct smmu_softc *sc)
{
        uint32_t reg;
        int i;

        if (smmu_attach(sc) != 0)
                return ENXIO;

        reg = smmu_gr0_read_4(sc, SMMU_IDR0);
        if (reg & SMMU_IDR0_S1TS)
                sc->sc_has_s1 = 1;
        /*
         * Marvell's 8040 does not support 64-bit writes, hence it
         * is not possible to invalidate stage-2, because the ASID
         * is part of the upper 32-bits and they'd be ignored.
         */
        if (sc->sc_is_ap806)
                sc->sc_has_s1 = 0;
        if (reg & SMMU_IDR0_S2TS)
                sc->sc_has_s2 = 1;
        if (!sc->sc_has_s1 && !sc->sc_has_s2)
                return 1;
        if (reg & SMMU_IDR0_EXIDS)
                sc->sc_has_exids = 1;

        sc->sc_num_streams = 1 << SMMU_IDR0_NUMSIDB(reg);
        if (sc->sc_has_exids)
                sc->sc_num_streams = 1 << 16;
        sc->sc_stream_mask = sc->sc_num_streams - 1;
        if (reg & SMMU_IDR0_SMS) {
                sc->sc_num_streams = SMMU_IDR0_NUMSMRG(reg);
                if (sc->sc_num_streams == 0)
                        return 1;
                sc->sc_smr = mallocarray(sc->sc_num_streams,
                    sizeof(*sc->sc_smr), M_DEVBUF, M_WAITOK | M_ZERO);
        }

        reg = smmu_gr0_read_4(sc, SMMU_IDR1);
        sc->sc_pagesize = 4 * 1024;
        if (reg & SMMU_IDR1_PAGESIZE_64K)
                sc->sc_pagesize = 64 * 1024;
        sc->sc_numpage = 1 << (SMMU_IDR1_NUMPAGENDXB(reg) + 1);

        /* 0 to NUMS2CB == stage-2, NUMS2CB to NUMCB == stage-1 */
        sc->sc_num_context_banks = SMMU_IDR1_NUMCB(reg);
        sc->sc_num_s2_context_banks = SMMU_IDR1_NUMS2CB(reg);
        if (sc->sc_num_s2_context_banks > sc->sc_num_context_banks)
                return 1;
        sc->sc_cb = mallocarray(sc->sc_num_context_banks,
            sizeof(*sc->sc_cb), M_DEVBUF, M_WAITOK | M_ZERO);

        reg = smmu_gr0_read_4(sc, SMMU_IDR2);
        if (reg & SMMU_IDR2_VMID16S)
                sc->sc_has_vmid16s = 1;

        switch (SMMU_IDR2_IAS(reg)) {
        case SMMU_IDR2_IAS_32BIT:
                sc->sc_ipa_bits = 32;
                break;
        case SMMU_IDR2_IAS_36BIT:
                sc->sc_ipa_bits = 36;
                break;
        case SMMU_IDR2_IAS_40BIT:
                sc->sc_ipa_bits = 40;
                break;
        case SMMU_IDR2_IAS_42BIT:
                sc->sc_ipa_bits = 42;
                break;
        case SMMU_IDR2_IAS_44BIT:
                sc->sc_ipa_bits = 44;
                break;
        case SMMU_IDR2_IAS_48BIT:
        default:
                sc->sc_ipa_bits = 48;
                break;
        }
        switch (SMMU_IDR2_OAS(reg)) {
        case SMMU_IDR2_OAS_32BIT:
                sc->sc_pa_bits = 32;
                break;
        case SMMU_IDR2_OAS_36BIT:
                sc->sc_pa_bits = 36;
                break;
        case SMMU_IDR2_OAS_40BIT:
                sc->sc_pa_bits = 40;
                break;
        case SMMU_IDR2_OAS_42BIT:
                sc->sc_pa_bits = 42;
                break;
        case SMMU_IDR2_OAS_44BIT:
                sc->sc_pa_bits = 44;
                break;
        case SMMU_IDR2_OAS_48BIT:
        default:
                sc->sc_pa_bits = 48;
                break;
        }
        switch (SMMU_IDR2_UBS(reg)) {
        case SMMU_IDR2_UBS_32BIT:
                sc->sc_va_bits = 32;
                break;
        case SMMU_IDR2_UBS_36BIT:
                sc->sc_va_bits = 36;
                break;
        case SMMU_IDR2_UBS_40BIT:
                sc->sc_va_bits = 40;
                break;
        case SMMU_IDR2_UBS_42BIT:
                sc->sc_va_bits = 42;
                break;
        case SMMU_IDR2_UBS_44BIT:
                sc->sc_va_bits = 44;
                break;
        case SMMU_IDR2_UBS_49BIT:
        default:
                sc->sc_va_bits = 48;
                break;
        }

        printf(": %u CBs (%u S2-only)",
            sc->sc_num_context_banks, sc->sc_num_s2_context_banks);
        if (sc->sc_is_qcom) {
                /*
                 * In theory we should check if bypass quirk is needed by
                 * modifying S2CR and re-checking if the value is different.
                 * This does not work on the last S2CR, but on the first,
                 * which is in use.  Revisit this once we have other QCOM HW.
                 */
                sc->sc_bypass_quirk = 1;
                printf(", bypass quirk");
                /*
                 * Create special context that is turned off.  This allows us
                 * to map a stream to a context bank where translation is not
                 * happening, and hence bypassed.
                 */
                sc->sc_cb[sc->sc_num_context_banks - 1] =
                    malloc(sizeof(struct smmu_cb), M_DEVBUF, M_WAITOK | M_ZERO);
                smmu_cb_write_4(sc, sc->sc_num_context_banks - 1,
                    SMMU_CB_SCTLR, 0);
                smmu_gr1_write_4(sc, SMMU_CBAR(sc->sc_num_context_banks - 1),
                    SMMU_CBAR_TYPE_S1_TRANS_S2_BYPASS);
        }
        printf("\n");

        /* Clear Global Fault Status Register */
        smmu_gr0_write_4(sc, SMMU_SGFSR, smmu_gr0_read_4(sc, SMMU_SGFSR));

        for (i = 0; i < sc->sc_num_streams; i++) {
                /* On QCOM HW we need to keep current streams running. */
                if (sc->sc_is_qcom && sc->sc_smr &&
                    smmu_gr0_read_4(sc, SMMU_SMR(i)) & SMMU_SMR_VALID) {
                        reg = smmu_gr0_read_4(sc, SMMU_SMR(i));
                        sc->sc_smr[i] = malloc(sizeof(struct smmu_smr),
                            M_DEVBUF, M_WAITOK | M_ZERO);
                        sc->sc_smr[i]->ss_id = (reg >> SMMU_SMR_ID_SHIFT) &
                            SMMU_SMR_ID_MASK;
                        sc->sc_smr[i]->ss_mask = (reg >> SMMU_SMR_MASK_SHIFT) &
                            SMMU_SMR_MASK_MASK;
                        if (sc->sc_bypass_quirk) {
                                smmu_gr0_write_4(sc, SMMU_S2CR(i),
                                    SMMU_S2CR_TYPE_TRANS |
                                    sc->sc_num_context_banks - 1);
                        } else {
                                smmu_gr0_write_4(sc, SMMU_S2CR(i),
                                    SMMU_S2CR_TYPE_BYPASS | 0xff);
                        }
                        continue;
                }
#if 1
                /* Setup all streams to fault by default */
                smmu_gr0_write_4(sc, SMMU_S2CR(i), SMMU_S2CR_TYPE_FAULT);
#else
                /* For stream indexing, USFCFG bypass isn't enough! */
                smmu_gr0_write_4(sc, SMMU_S2CR(i), SMMU_S2CR_TYPE_BYPASS);
#endif
                /*  Disable all stream map registers */
                if (sc->sc_smr)
                        smmu_gr0_write_4(sc, SMMU_SMR(i), 0);
        }

        for (i = 0; i < sc->sc_num_context_banks; i++) {
                /* Disable Context Bank */
                smmu_cb_write_4(sc, i, SMMU_CB_SCTLR, 0);
                /* Clear Context Bank Fault Status Register */
                smmu_cb_write_4(sc, i, SMMU_CB_FSR, SMMU_CB_FSR_MASK);
        }

        /* Invalidate TLB */
        smmu_gr0_write_4(sc, SMMU_TLBIALLH, ~0);
        smmu_gr0_write_4(sc, SMMU_TLBIALLNSNH, ~0);

        if (sc->sc_is_mmu500) {
                reg = smmu_gr0_read_4(sc, SMMU_SACR);
                if (SMMU_IDR7_MAJOR(smmu_gr0_read_4(sc, SMMU_IDR7)) >= 2)
                        reg &= ~SMMU_SACR_MMU500_CACHE_LOCK;
                reg |= SMMU_SACR_MMU500_SMTNMB_TLBEN |
                    SMMU_SACR_MMU500_S2CRB_TLBEN;
                smmu_gr0_write_4(sc, SMMU_SACR, reg);
                for (i = 0; i < sc->sc_num_context_banks; i++) {
                        reg = smmu_cb_read_4(sc, i, SMMU_CB_ACTLR);
                        reg &= ~SMMU_CB_ACTLR_CPRE;
                        smmu_cb_write_4(sc, i, SMMU_CB_ACTLR, reg);
                }
        }

        /* Enable SMMU */
        reg = smmu_gr0_read_4(sc, SMMU_SCR0);
        reg &= ~(SMMU_SCR0_CLIENTPD |
            SMMU_SCR0_FB | SMMU_SCR0_BSU_MASK);
#if 1
        /* Disable bypass for unknown streams */
        reg |= SMMU_SCR0_USFCFG;
#else
        /* Enable bypass for unknown streams */
        reg &= ~SMMU_SCR0_USFCFG;
#endif
        reg |= SMMU_SCR0_GFRE | SMMU_SCR0_GFIE |
            SMMU_SCR0_GCFGFRE | SMMU_SCR0_GCFGFIE |
            SMMU_SCR0_VMIDPNE | SMMU_SCR0_PTM;
        if (sc->sc_has_exids)
                reg |= SMMU_SCR0_EXIDENABLE;
        if (sc->sc_has_vmid16s)
                reg |= SMMU_SCR0_VMID16EN;

        smmu_v2_tlb_sync_global(sc);
        smmu_gr0_write_4(sc, SMMU_SCR0, reg);

        sc->sc_domain_create = smmu_v2_domain_create;
        sc->sc_tlbi_va = smmu_v2_tlbi_va;
        sc->sc_tlb_sync_context = smmu_v2_tlb_sync_context;
        return 0;
}

int
smmu_v2_global_irq(void *cookie)
{
        struct smmu_softc *sc = cookie;
        uint32_t reg;

        reg = smmu_gr0_read_4(sc, SMMU_SGFSR);
        if (reg == 0)
                return 0;

        printf("%s: SGFSR 0x%08x SGFSYNR0 0x%08x SGFSYNR1 0x%08x "
            "SGFSYNR2 0x%08x\n", sc->sc_dev.dv_xname, reg,
            smmu_gr0_read_4(sc, SMMU_SGFSYNR0),
            smmu_gr0_read_4(sc, SMMU_SGFSYNR1),
            smmu_gr0_read_4(sc, SMMU_SGFSYNR2));

        smmu_gr0_write_4(sc, SMMU_SGFSR, reg);

        return 1;
}

int
smmu_v2_context_irq(void *cookie)
{
        struct smmu_cb_irq *cbi = cookie;
        struct smmu_softc *sc = cbi->cbi_sc;
        uint32_t reg;

        reg = smmu_cb_read_4(sc, cbi->cbi_idx, SMMU_CB_FSR);
        if ((reg & SMMU_CB_FSR_MASK) == 0)
                return 0;

        printf("%s: FSR 0x%08x FSYNR0 0x%08x FAR 0x%llx "
            "CBFRSYNRA 0x%08x\n", sc->sc_dev.dv_xname, reg,
            smmu_cb_read_4(sc, cbi->cbi_idx, SMMU_CB_FSYNR0),
            smmu_cb_read_8(sc, cbi->cbi_idx, SMMU_CB_FAR),
            smmu_gr1_read_4(sc, SMMU_CBFRSYNRA(cbi->cbi_idx)));

        smmu_cb_write_4(sc, cbi->cbi_idx, SMMU_CB_FSR, reg);

        return 1;
}

void
smmu_v2_tlb_sync_global(struct smmu_softc *sc)
{
        int i;

        smmu_gr0_write_4(sc, SMMU_STLBGSYNC, ~0);
        for (i = 1000; i > 0; i--) {
                if ((smmu_gr0_read_4(sc, SMMU_STLBGSTATUS) &
                    SMMU_STLBGSTATUS_GSACTIVE) == 0)
                        return;
        }

        printf("%s: global TLB sync timeout\n",
            sc->sc_dev.dv_xname);
}

void
smmu_v2_tlb_sync_context(struct smmu_domain *dom)
{
        struct smmu_softc *sc = dom->sd_sc;
        int i;

        smmu_cb_write_4(sc, dom->sd_cb_idx, SMMU_CB_TLBSYNC, ~0);
        for (i = 1000; i > 0; i--) {
                if ((smmu_cb_read_4(sc, dom->sd_cb_idx, SMMU_CB_TLBSTATUS) &
                    SMMU_CB_TLBSTATUS_SACTIVE) == 0)
                        return;
        }

        printf("%s: context TLB sync timeout\n",
            sc->sc_dev.dv_xname);
}

uint32_t
smmu_gr0_read_4(struct smmu_softc *sc, bus_size_t off)
{
        uint32_t base = 0 * sc->sc_pagesize;

        return bus_space_read_4(sc->sc_iot, sc->sc_ioh, base + off);
}

void
smmu_gr0_write_4(struct smmu_softc *sc, bus_size_t off, uint32_t val)
{
        uint32_t base = 0 * sc->sc_pagesize;

        bus_space_write_4(sc->sc_iot, sc->sc_ioh, base + off, val);
}

uint32_t
smmu_gr1_read_4(struct smmu_softc *sc, bus_size_t off)
{
        uint32_t base = 1 * sc->sc_pagesize;

        return bus_space_read_4(sc->sc_iot, sc->sc_ioh, base + off);
}

void
smmu_gr1_write_4(struct smmu_softc *sc, bus_size_t off, uint32_t val)
{
        uint32_t base = 1 * sc->sc_pagesize;

        bus_space_write_4(sc->sc_iot, sc->sc_ioh, base + off, val);
}

uint32_t
smmu_cb_read_4(struct smmu_softc *sc, int idx, bus_size_t off)
{
        uint32_t base;

        base = sc->sc_numpage * sc->sc_pagesize; /* SMMU_CB_BASE */
        base += idx * sc->sc_pagesize; /* SMMU_CBn_BASE */

        return bus_space_read_4(sc->sc_iot, sc->sc_ioh, base + off);
}

void
smmu_cb_write_4(struct smmu_softc *sc, int idx, bus_size_t off, uint32_t val)
{
        uint32_t base;

        base = sc->sc_numpage * sc->sc_pagesize; /* SMMU_CB_BASE */
        base += idx * sc->sc_pagesize; /* SMMU_CBn_BASE */

        bus_space_write_4(sc->sc_iot, sc->sc_ioh, base + off, val);
}

uint64_t
smmu_cb_read_8(struct smmu_softc *sc, int idx, bus_size_t off)
{
        uint64_t reg;
        uint32_t base;

        base = sc->sc_numpage * sc->sc_pagesize; /* SMMU_CB_BASE */
        base += idx * sc->sc_pagesize; /* SMMU_CBn_BASE */

        if (sc->sc_is_ap806) {
                reg = bus_space_read_4(sc->sc_iot, sc->sc_ioh, base + off + 4);
                reg <<= 32;
                reg |= bus_space_read_4(sc->sc_iot, sc->sc_ioh, base + off + 0);
                return reg;
        }

        return bus_space_read_8(sc->sc_iot, sc->sc_ioh, base + off);
}

void
smmu_cb_write_8(struct smmu_softc *sc, int idx, bus_size_t off, uint64_t val)
{
        uint32_t base;

        base = sc->sc_numpage * sc->sc_pagesize; /* SMMU_CB_BASE */
        base += idx * sc->sc_pagesize; /* SMMU_CBn_BASE */

        if (sc->sc_is_ap806) {
                bus_space_write_4(sc->sc_iot, sc->sc_ioh, base + off + 4,
                    val >> 32);
                bus_space_write_4(sc->sc_iot, sc->sc_ioh, base + off + 0,
                    val & 0xffffffff);
                return;
        }

        bus_space_write_8(sc->sc_iot, sc->sc_ioh, base + off, val);
}

bus_dma_tag_t
smmu_device_map(void *cookie, uint32_t sid, bus_dma_tag_t dmat)
{
        struct smmu_softc *sc = cookie;
        struct smmu_domain *dom;

        dom = smmu_domain_lookup(sc, sid);
        if (dom == NULL)
                return dmat;

        if (dom->sd_dmat == NULL) {
                dom->sd_dmat = malloc(sizeof(*dom->sd_dmat),
                    M_DEVBUF, M_WAITOK);
                memcpy(dom->sd_dmat, dmat, sizeof(*dom->sd_dmat));
                dom->sd_dmat->_cookie = dom;
                dom->sd_dmat->_dmamap_create = smmu_dmamap_create;
                dom->sd_dmat->_dmamap_destroy = smmu_dmamap_destroy;
                dom->sd_dmat->_dmamap_load = smmu_dmamap_load;
                dom->sd_dmat->_dmamap_load_mbuf = smmu_dmamap_load_mbuf;
                dom->sd_dmat->_dmamap_load_uio = smmu_dmamap_load_uio;
                dom->sd_dmat->_dmamap_load_raw = smmu_dmamap_load_raw;
                dom->sd_dmat->_dmamap_unload = smmu_dmamap_unload;
        }

        return dom->sd_dmat;
}

struct smmu_domain *
smmu_domain_lookup(struct smmu_softc *sc, uint32_t sid)
{
        struct smmu_domain *dom;

        SIMPLEQ_FOREACH(dom, &sc->sc_domains, sd_list) {
                if (dom->sd_sid == sid)
                        return dom;
        }

        return smmu_domain_create(sc, sid);
}

struct smmu_domain *
smmu_domain_create(struct smmu_softc *sc, uint32_t sid)
{
        struct smmu_domain *dom;

        dom = malloc(sizeof(*dom), M_DEVBUF, M_WAITOK | M_ZERO);
        mtx_init(&dom->sd_iova_mtx, IPL_VM);
        mtx_init(&dom->sd_pmap_mtx, IPL_VM);
        dom->sd_sc = sc;
        dom->sd_sid = sid;

        /* Prefer stage 1 if possible! */
        if (sc->sc_has_s1)
                dom->sd_stage = 1;
        else
                dom->sd_stage = 2;

        if (sc->sc_domain_create(dom) != 0) {
                free(dom, M_DEVBUF, sizeof(*dom));
                return NULL;
        }

        /* Reserve first page (to catch NULL access) */
        extent_alloc_region(dom->sd_iovamap, 0, PAGE_SIZE, EX_WAITOK);

        SIMPLEQ_INSERT_TAIL(&sc->sc_domains, dom, sd_list);
        return dom;
}

int
smmu_v2_domain_create(struct smmu_domain *dom)
{
        struct smmu_softc *sc = dom->sd_sc;
        uint32_t iovabits, reg;
        paddr_t pa;
        vaddr_t l0va;
        int i, start, end;

        if (dom->sd_stage == 1) {
                start = sc->sc_num_s2_context_banks;
                end = sc->sc_num_context_banks;
        } else {
                start = 0;
                end = sc->sc_num_context_banks;
        }

        for (i = start; i < end; i++) {
                if (sc->sc_cb[i] != NULL)
                        continue;
                sc->sc_cb[i] = malloc(sizeof(struct smmu_cb),
                    M_DEVBUF, M_WAITOK | M_ZERO);
                dom->sd_cb_idx = i;
                break;
        }
        if (i >= end) {
                printf("%s: out of context blocks, I/O device will fail\n",
                    sc->sc_dev.dv_xname);
                return ENXIO;
        }

        /* Stream indexing is easy */
        dom->sd_smr_idx = dom->sd_sid;

        /* Stream mapping is a bit more effort */
        if (sc->sc_smr) {
                for (i = 0; i < sc->sc_num_streams; i++) {
                        /* Take over QCOM SMRs */
                        if (sc->sc_is_qcom && sc->sc_smr[i] != NULL &&
                            sc->sc_smr[i]->ss_dom == NULL &&
                            !((sc->sc_smr[i]->ss_id ^ dom->sd_sid) &
                            ~sc->sc_smr[i]->ss_mask)) {
                                sc->sc_smr[i]->ss_dom = dom;
                                dom->sd_smr_idx = i;
                                break;
                        }
                        if (sc->sc_smr[i] != NULL)
                                continue;
                        sc->sc_smr[i] = malloc(sizeof(struct smmu_smr),
                            M_DEVBUF, M_WAITOK | M_ZERO);
                        sc->sc_smr[i]->ss_dom = dom;
                        sc->sc_smr[i]->ss_id = dom->sd_sid;
                        sc->sc_smr[i]->ss_mask = 0;
                        dom->sd_smr_idx = i;
                        break;
                }

                if (i >= sc->sc_num_streams) {
                        free(sc->sc_cb[dom->sd_cb_idx], M_DEVBUF,
                            sizeof(struct smmu_cb));
                        sc->sc_cb[dom->sd_cb_idx] = NULL;
                        printf("%s: out of streams, I/O device will fail\n",
                            sc->sc_dev.dv_xname);
                        return ENXIO;
                }
        }

        reg = SMMU_CBA2R_VA64;
        if (sc->sc_has_vmid16s)
                reg |= (dom->sd_cb_idx + 1) << SMMU_CBA2R_VMID16_SHIFT;
        smmu_gr1_write_4(sc, SMMU_CBA2R(dom->sd_cb_idx), reg);

        if (dom->sd_stage == 1) {
                reg = SMMU_CBAR_TYPE_S1_TRANS_S2_BYPASS |
                    SMMU_CBAR_BPSHCFG_NSH | SMMU_CBAR_MEMATTR_WB;
        } else {
                reg = SMMU_CBAR_TYPE_S2_TRANS;
                if (!sc->sc_has_vmid16s)
                        reg |= (dom->sd_cb_idx + 1) << SMMU_CBAR_VMID_SHIFT;
        }
        smmu_gr1_write_4(sc, SMMU_CBAR(dom->sd_cb_idx), reg);

        if (dom->sd_stage == 1) {
                reg = SMMU_CB_TCR2_AS | SMMU_CB_TCR2_SEP_UPSTREAM;
                switch (sc->sc_ipa_bits) {
                case 32:
                        reg |= SMMU_CB_TCR2_PASIZE_32BIT;
                        break;
                case 36:
                        reg |= SMMU_CB_TCR2_PASIZE_36BIT;
                        break;
                case 40:
                        reg |= SMMU_CB_TCR2_PASIZE_40BIT;
                        break;
                case 42:
                        reg |= SMMU_CB_TCR2_PASIZE_42BIT;
                        break;
                case 44:
                        reg |= SMMU_CB_TCR2_PASIZE_44BIT;
                        break;
                case 48:
                        reg |= SMMU_CB_TCR2_PASIZE_48BIT;
                        break;
                }
                smmu_cb_write_4(sc, dom->sd_cb_idx, SMMU_CB_TCR2, reg);
        }

        if (dom->sd_stage == 1)
                iovabits = sc->sc_va_bits;
        else
                iovabits = sc->sc_ipa_bits;
        /*
         * Marvell's 8040 does not support 64-bit writes, hence we
         * can only address 44-bits of VA space for TLB invalidation.
         */
        if (sc->sc_is_ap806)
                iovabits = min(44, iovabits);
        if (iovabits >= 40)
                dom->sd_4level = 1;

        reg = SMMU_CB_TCR_TG0_4KB | SMMU_CB_TCR_T0SZ(64 - iovabits);
        if (dom->sd_stage == 1) {
                reg |= SMMU_CB_TCR_EPD1;
        } else {
                if (dom->sd_4level)
                        reg |= SMMU_CB_TCR_S2_SL0_4KB_L0;
                else
                        reg |= SMMU_CB_TCR_S2_SL0_4KB_L1;
                switch (sc->sc_pa_bits) {
                case 32:
                        reg |= SMMU_CB_TCR_S2_PASIZE_32BIT;
                        break;
                case 36:
                        reg |= SMMU_CB_TCR_S2_PASIZE_36BIT;
                        break;
                case 40:
                        reg |= SMMU_CB_TCR_S2_PASIZE_40BIT;
                        break;
                case 42:
                        reg |= SMMU_CB_TCR_S2_PASIZE_42BIT;
                        break;
                case 44:
                        reg |= SMMU_CB_TCR_S2_PASIZE_44BIT;
                        break;
                case 48:
                        reg |= SMMU_CB_TCR_S2_PASIZE_48BIT;
                        break;
                }
        }
        if (sc->sc_coherent)
                reg |= SMMU_CB_TCR_IRGN0_WBWA | SMMU_CB_TCR_ORGN0_WBWA |
                    SMMU_CB_TCR_SH0_ISH;
        else
                reg |= SMMU_CB_TCR_IRGN0_NC | SMMU_CB_TCR_ORGN0_NC |
                    SMMU_CB_TCR_SH0_OSH;
        smmu_cb_write_4(sc, dom->sd_cb_idx, SMMU_CB_TCR, reg);

        if (dom->sd_4level) {
                while (dom->sd_vp.l0 == NULL) {
                        dom->sd_vp.l0 = pool_get(&sc->sc_vp_pool,
                            PR_WAITOK | PR_ZERO);
                }
                l0va = (vaddr_t)dom->sd_vp.l0->l0; /* top level is l0 */
        } else {
                while (dom->sd_vp.l1 == NULL) {
                        dom->sd_vp.l1 = pool_get(&sc->sc_vp_pool,
                            PR_WAITOK | PR_ZERO);
                }
                l0va = (vaddr_t)dom->sd_vp.l1->l1; /* top level is l1 */
        }
        pmap_extract(pmap_kernel(), l0va, &pa);

        if (dom->sd_stage == 1) {
                smmu_cb_write_8(sc, dom->sd_cb_idx, SMMU_CB_TTBR0,
                    (uint64_t)dom->sd_cb_idx << SMMU_CB_TTBR_ASID_SHIFT | pa);
                smmu_cb_write_8(sc, dom->sd_cb_idx, SMMU_CB_TTBR1,
                    (uint64_t)dom->sd_cb_idx << SMMU_CB_TTBR_ASID_SHIFT);
        } else
                smmu_cb_write_8(sc, dom->sd_cb_idx, SMMU_CB_TTBR0, pa);

        if (dom->sd_stage == 1) {
                smmu_cb_write_4(sc, dom->sd_cb_idx, SMMU_CB_MAIR0,
                    SMMU_CB_MAIR_MAIR_ATTR(SMMU_CB_MAIR_DEVICE_nGnRnE, 0) |
                    SMMU_CB_MAIR_MAIR_ATTR(SMMU_CB_MAIR_DEVICE_nGnRE, 1) |
                    SMMU_CB_MAIR_MAIR_ATTR(SMMU_CB_MAIR_DEVICE_NC, 2) |
                    SMMU_CB_MAIR_MAIR_ATTR(SMMU_CB_MAIR_DEVICE_WB, 3));
                smmu_cb_write_4(sc, dom->sd_cb_idx, SMMU_CB_MAIR1,
                    SMMU_CB_MAIR_MAIR_ATTR(SMMU_CB_MAIR_DEVICE_WT, 0));
        }

        reg = SMMU_CB_SCTLR_M | SMMU_CB_SCTLR_TRE | SMMU_CB_SCTLR_AFE |
            SMMU_CB_SCTLR_CFRE | SMMU_CB_SCTLR_CFIE;
        if (dom->sd_stage == 1)
                reg |= SMMU_CB_SCTLR_ASIDPNE;
        smmu_cb_write_4(sc, dom->sd_cb_idx, SMMU_CB_SCTLR, reg);

        /* Point stream to context block */
        reg = SMMU_S2CR_TYPE_TRANS | dom->sd_cb_idx;
        if (sc->sc_has_exids && sc->sc_smr)
                reg |= SMMU_S2CR_EXIDVALID;
        smmu_gr0_write_4(sc, SMMU_S2CR(dom->sd_smr_idx), reg);

        /* Map stream idx to S2CR idx */
        if (sc->sc_smr) {
                reg = sc->sc_smr[dom->sd_smr_idx]->ss_id << SMMU_SMR_ID_SHIFT |
                    sc->sc_smr[dom->sd_smr_idx]->ss_mask << SMMU_SMR_MASK_SHIFT;
                if (!sc->sc_has_exids)
                        reg |= SMMU_SMR_VALID;
                smmu_gr0_write_4(sc, SMMU_SMR(dom->sd_smr_idx), reg);
        }

        snprintf(dom->sd_exname, sizeof(dom->sd_exname), "%s:%x",
            sc->sc_dev.dv_xname, dom->sd_sid);
        dom->sd_iovamap = extent_create(dom->sd_exname, 0,
            (1LL << iovabits) - 1, M_DEVBUF, NULL, 0, EX_WAITOK |
            EX_NOCOALESCE);

        return 0;
}

void
smmu_reserve_region(void *cookie, uint32_t sid, bus_addr_t addr,
    bus_size_t size)
{
        struct smmu_softc *sc = cookie;
        struct smmu_domain *dom;

        dom = smmu_domain_lookup(sc, sid);
        if (dom == NULL)
                return;

        /*
         * Some reserved regions, like PCI BARs, might not be inside our VA map.
         * If it lies outside completely: skip it; if partially: clamp it.
         */
        if (addr > dom->sd_iovamap->ex_end)
                return;
        if (addr + size > dom->sd_iovamap->ex_end)
                size = (dom->sd_iovamap->ex_end - addr) + 1;

        extent_alloc_region(dom->sd_iovamap, addr, size,
            EX_WAITOK | EX_CONFLICTOK);
}

/* basically pmap follows */

/* virtual to physical helpers */
static inline int
VP_IDX0(vaddr_t va)
{
        return (va >> VP_IDX0_POS) & VP_IDX0_MASK;
}

static inline int
VP_IDX1(vaddr_t va)
{
        return (va >> VP_IDX1_POS) & VP_IDX1_MASK;
}

static inline int
VP_IDX2(vaddr_t va)
{
        return (va >> VP_IDX2_POS) & VP_IDX2_MASK;
}

static inline int
VP_IDX3(vaddr_t va)
{
        return (va >> VP_IDX3_POS) & VP_IDX3_MASK;
}

static inline uint64_t
VP_Lx(paddr_t pa)
{
        /*
         * This function takes the pa address given and manipulates it
         * into the form that should be inserted into the VM table.
         */
        return pa | Lx_TYPE_PT;
}

void
smmu_set_l1(struct smmu_domain *dom, uint64_t va, struct smmuvp1 *l1_va)
{
        struct smmu_softc *sc = dom->sd_sc;
        uint64_t pg_entry;
        paddr_t l1_pa;
        int idx0;

        if (pmap_extract(pmap_kernel(), (vaddr_t)l1_va, &l1_pa) == 0)
                panic("%s: unable to find vp pa mapping %p", __func__, l1_va);

        if (l1_pa & (Lx_TABLE_ALIGN-1))
                panic("%s: misaligned L2 table", __func__);

        pg_entry = VP_Lx(l1_pa);

        idx0 = VP_IDX0(va);
        dom->sd_vp.l0->vp[idx0] = l1_va;
        dom->sd_vp.l0->l0[idx0] = pg_entry;
        membar_producer(); /* XXX bus dma sync? */
        if (!sc->sc_coherent)
                cpu_dcache_wb_range((vaddr_t)&dom->sd_vp.l0->l0[idx0],
                    sizeof(dom->sd_vp.l0->l0[idx0]));
}

void
smmu_set_l2(struct smmu_domain *dom, uint64_t va, struct smmuvp1 *vp1,
    struct smmuvp2 *l2_va)
{
        struct smmu_softc *sc = dom->sd_sc;
        uint64_t pg_entry;
        paddr_t l2_pa;
        int idx1;

        if (pmap_extract(pmap_kernel(), (vaddr_t)l2_va, &l2_pa) == 0)
                panic("%s: unable to find vp pa mapping %p", __func__, l2_va);

        if (l2_pa & (Lx_TABLE_ALIGN-1))
                panic("%s: misaligned L2 table", __func__);

        pg_entry = VP_Lx(l2_pa);

        idx1 = VP_IDX1(va);
        vp1->vp[idx1] = l2_va;
        vp1->l1[idx1] = pg_entry;
        membar_producer(); /* XXX bus dma sync? */
        if (!sc->sc_coherent)
                cpu_dcache_wb_range((vaddr_t)&vp1->l1[idx1],
                    sizeof(vp1->l1[idx1]));
}

void
smmu_set_l3(struct smmu_domain *dom, uint64_t va, struct smmuvp2 *vp2,
    struct smmuvp3 *l3_va)
{
        struct smmu_softc *sc = dom->sd_sc;
        uint64_t pg_entry;
        paddr_t l3_pa;
        int idx2;

        if (pmap_extract(pmap_kernel(), (vaddr_t)l3_va, &l3_pa) == 0)
                panic("%s: unable to find vp pa mapping %p", __func__, l3_va);

        if (l3_pa & (Lx_TABLE_ALIGN-1))
                panic("%s: misaligned L2 table", __func__);

        pg_entry = VP_Lx(l3_pa);

        idx2 = VP_IDX2(va);
        vp2->vp[idx2] = l3_va;
        vp2->l2[idx2] = pg_entry;
        membar_producer(); /* XXX bus dma sync? */
        if (!sc->sc_coherent)
                cpu_dcache_wb_range((vaddr_t)&vp2->l2[idx2],
                    sizeof(vp2->l2[idx2]));
}

int
smmu_vp_lookup(struct smmu_domain *dom, vaddr_t va, uint64_t **pl3entry)
{
        struct smmuvp1 *vp1;
        struct smmuvp2 *vp2;
        struct smmuvp3 *vp3;

        if (dom->sd_4level) {
                if (dom->sd_vp.l0 == NULL) {
                        return ENXIO;
                }
                vp1 = dom->sd_vp.l0->vp[VP_IDX0(va)];
        } else {
                vp1 = dom->sd_vp.l1;
        }
        if (vp1 == NULL) {
                return ENXIO;
        }

        vp2 = vp1->vp[VP_IDX1(va)];
        if (vp2 == NULL) {
                return ENXIO;
        }

        vp3 = vp2->vp[VP_IDX2(va)];
        if (vp3 == NULL) {
                return ENXIO;
        }

        if (pl3entry != NULL)
                *pl3entry = &(vp3->l3[VP_IDX3(va)]);

        return 0;
}

int
smmu_vp_enter(struct smmu_domain *dom, vaddr_t va, uint64_t **pl3entry,
    int flags)
{
        struct smmu_softc *sc = dom->sd_sc;
        struct smmuvp1 *vp1;
        struct smmuvp2 *vp2;
        struct smmuvp3 *vp3;

        if (dom->sd_4level) {
                vp1 = dom->sd_vp.l0->vp[VP_IDX0(va)];
                if (vp1 == NULL) {
                        mtx_enter(&dom->sd_pmap_mtx);
                        vp1 = dom->sd_vp.l0->vp[VP_IDX0(va)];
                        if (vp1 == NULL) {
                                vp1 = pool_get(&sc->sc_vp_pool,
                                    PR_NOWAIT | PR_ZERO);
                                if (vp1 == NULL) {
                                        mtx_leave(&dom->sd_pmap_mtx);
                                        return ENOMEM;
                                }
                                smmu_set_l1(dom, va, vp1);
                        }
                        mtx_leave(&dom->sd_pmap_mtx);
                }
        } else {
                vp1 = dom->sd_vp.l1;
        }

        vp2 = vp1->vp[VP_IDX1(va)];
        if (vp2 == NULL) {
                mtx_enter(&dom->sd_pmap_mtx);
                vp2 = vp1->vp[VP_IDX1(va)];
                if (vp2 == NULL) {
                        vp2 = pool_get(&sc->sc_vp_pool, PR_NOWAIT | PR_ZERO);
                        if (vp2 == NULL) {
                                mtx_leave(&dom->sd_pmap_mtx);
                                return ENOMEM;
                        }
                        smmu_set_l2(dom, va, vp1, vp2);
                }
                mtx_leave(&dom->sd_pmap_mtx);
        }

        vp3 = vp2->vp[VP_IDX2(va)];
        if (vp3 == NULL) {
                mtx_enter(&dom->sd_pmap_mtx);
                vp3 = vp2->vp[VP_IDX2(va)];
                if (vp3 == NULL) {
                        vp3 = pool_get(&sc->sc_vp3_pool, PR_NOWAIT | PR_ZERO);
                        if (vp3 == NULL) {
                                mtx_leave(&dom->sd_pmap_mtx);
                                return ENOMEM;
                        }
                        smmu_set_l3(dom, va, vp2, vp3);
                }
                mtx_leave(&dom->sd_pmap_mtx);
        }

        if (pl3entry != NULL)
                *pl3entry = &(vp3->l3[VP_IDX3(va)]);

        return 0;
}

uint64_t
smmu_fill_pte(struct smmu_domain *dom, vaddr_t va, paddr_t pa,
    vm_prot_t prot, int flags, int cache)
{
        uint64_t pted;

        pted = pa & PTE_RPGN;

        switch (cache) {
        case PMAP_CACHE_WB:
                break;
        case PMAP_CACHE_WT:
                break;
        case PMAP_CACHE_CI:
                break;
        case PMAP_CACHE_DEV_NGNRNE:
                break;
        case PMAP_CACHE_DEV_NGNRE:
                break;
        default:
                panic("%s: invalid cache mode", __func__);
        }

        pted |= cache;
        pted |= flags & (PROT_READ|PROT_WRITE|PROT_EXEC);
        return pted;
}

void
smmu_pte_update(struct smmu_domain *dom, uint64_t pted, uint64_t *pl3)
{
        struct smmu_softc *sc = dom->sd_sc;
        uint64_t pte, access_bits;
        uint64_t attr = 0;

        /* see mair in locore.S */
        switch (pted & PMAP_CACHE_BITS) {
        case PMAP_CACHE_WB:
                /* inner and outer writeback */
                if (dom->sd_stage == 1)
                        attr |= ATTR_IDX(PTE_ATTR_WB);
                else
                        attr |= ATTR_IDX(PTE_MEMATTR_WB);
                attr |= ATTR_SH(SH_INNER);
                break;
        case PMAP_CACHE_WT:
                /* inner and outer writethrough */
                if (dom->sd_stage == 1)
                        attr |= ATTR_IDX(PTE_ATTR_WT);
                else
                        attr |= ATTR_IDX(PTE_MEMATTR_WT);
                attr |= ATTR_SH(SH_INNER);
                break;
        case PMAP_CACHE_CI:
                if (dom->sd_stage == 1)
                        attr |= ATTR_IDX(PTE_ATTR_CI);
                else
                        attr |= ATTR_IDX(PTE_MEMATTR_CI);
                attr |= ATTR_SH(SH_INNER);
                break;
        case PMAP_CACHE_DEV_NGNRNE:
                if (dom->sd_stage == 1)
                        attr |= ATTR_IDX(PTE_ATTR_DEV_NGNRNE);
                else
                        attr |= ATTR_IDX(PTE_MEMATTR_DEV_NGNRNE);
                attr |= ATTR_SH(SH_INNER);
                break;
        case PMAP_CACHE_DEV_NGNRE:
                if (dom->sd_stage == 1)
                        attr |= ATTR_IDX(PTE_ATTR_DEV_NGNRE);
                else
                        attr |= ATTR_IDX(PTE_MEMATTR_DEV_NGNRE);
                attr |= ATTR_SH(SH_INNER);
                break;
        default:
                panic("%s: invalid cache mode", __func__);
        }

        access_bits = ATTR_PXN | ATTR_AF;
        if (dom->sd_stage == 1) {
                attr |= ATTR_nG;
                access_bits |= ATTR_AP(1);
                if ((pted & PROT_READ) &&
                    !(pted & PROT_WRITE))
                        access_bits |= ATTR_AP(2);
        } else {
                if (pted & PROT_READ)
                        access_bits |= ATTR_AP(1);
                if (pted & PROT_WRITE)
                        access_bits |= ATTR_AP(2);
        }

        pte = (pted & PTE_RPGN) | attr | access_bits | L3_P;
        *pl3 = pte;
        membar_producer(); /* XXX bus dma sync? */
        if (!sc->sc_coherent)
                cpu_dcache_wb_range((vaddr_t)pl3, sizeof(*pl3));
}

void
smmu_pte_remove(struct smmu_domain *dom, vaddr_t va)
{
        /* put entry into table */
        /* need to deal with ref/change here */
        struct smmu_softc *sc = dom->sd_sc;
        struct smmuvp1 *vp1;
        struct smmuvp2 *vp2;
        struct smmuvp3 *vp3;

        if (dom->sd_4level)
                vp1 = dom->sd_vp.l0->vp[VP_IDX0(va)];
        else
                vp1 = dom->sd_vp.l1;
        if (vp1 == NULL) {
                panic("%s: missing the l1 for va %lx domain %p", __func__,
                    va, dom);
        }
        vp2 = vp1->vp[VP_IDX1(va)];
        if (vp2 == NULL) {
                panic("%s: missing the l2 for va %lx domain %p", __func__,
                    va, dom);
        }
        vp3 = vp2->vp[VP_IDX2(va)];
        if (vp3 == NULL) {
                panic("%s: missing the l3 for va %lx domain %p", __func__,
                    va, dom);
        }
        vp3->l3[VP_IDX3(va)] = 0;
        membar_producer(); /* XXX bus dma sync? */
        if (!sc->sc_coherent)
                cpu_dcache_wb_range((vaddr_t)&vp3->l3[VP_IDX3(va)],
                    sizeof(vp3->l3[VP_IDX3(va)]));
}

int
smmu_enter(struct smmu_domain *dom, vaddr_t va, paddr_t pa, vm_prot_t prot,
    int flags, int cache)
{
        uint64_t *pl3;

        if (smmu_vp_lookup(dom, va, &pl3) != 0) {
                if (smmu_vp_enter(dom, va, &pl3, flags))
                        return ENOMEM;
        }

        if (flags & (PROT_READ|PROT_WRITE|PROT_EXEC))
                smmu_map(dom, va, pa, prot, flags, cache);

        return 0;
}

void
smmu_map(struct smmu_domain *dom, vaddr_t va, paddr_t pa, vm_prot_t prot,
    int flags, int cache)
{
        uint64_t *pl3;
        uint64_t pted;
        int ret;

        /* IOVA must already be allocated */
        ret = smmu_vp_lookup(dom, va, &pl3);
        KASSERT(ret == 0);

        /* Update PTED information for physical address */
        pted = smmu_fill_pte(dom, va, pa, prot, flags, cache);

        /* Insert updated information */
        smmu_pte_update(dom, pted, pl3);
}

void
smmu_unmap(struct smmu_domain *dom, vaddr_t va)
{
        struct smmu_softc *sc = dom->sd_sc;
        int ret;

        /* IOVA must already be allocated */
        ret = smmu_vp_lookup(dom, va, NULL);
        KASSERT(ret == 0);

        /* Remove mapping from pagetable */
        smmu_pte_remove(dom, va);

        sc->sc_tlbi_va(dom, va);
}

void
smmu_v2_tlbi_va(struct smmu_domain *dom, vaddr_t va)
{
        struct smmu_softc *sc = dom->sd_sc;

        /* Invalidate IOTLB */
        if (dom->sd_stage == 1)
                smmu_cb_write_8(sc, dom->sd_cb_idx, SMMU_CB_TLBIVAL,
                    (uint64_t)dom->sd_cb_idx << 48 | va >> PAGE_SHIFT);
        else
                smmu_cb_write_8(sc, dom->sd_cb_idx, SMMU_CB_TLBIIPAS2L,
                    va >> PAGE_SHIFT);
}

void
smmu_remove(struct smmu_domain *dom, vaddr_t va)
{
        /* TODO: garbage collect page tables? */
}

int
smmu_load_map(struct smmu_domain *dom, bus_dmamap_t map)
{
        struct smmu_map_state *sms = map->_dm_cookie;
        u_long dva, maplen;
        int seg;

        maplen = 0;
        for (seg = 0; seg < map->dm_nsegs; seg++) {
                paddr_t pa = map->dm_segs[seg]._ds_paddr;
                psize_t off = pa - trunc_page(pa);
                maplen += round_page(map->dm_segs[seg].ds_len + off);
        }
        KASSERT(maplen <= sms->sms_len);

        dva = sms->sms_dva;
        for (seg = 0; seg < map->dm_nsegs; seg++) {
                paddr_t pa = map->dm_segs[seg]._ds_paddr;
                psize_t off = pa - trunc_page(pa);
                u_long len = round_page(map->dm_segs[seg].ds_len + off);

                map->dm_segs[seg].ds_addr = dva + off;

                pa = trunc_page(pa);
                while (len > 0) {
                        smmu_map(dom, dva, pa,
                            PROT_READ | PROT_WRITE,
                            PROT_READ | PROT_WRITE, PMAP_CACHE_WB);

                        dva += PAGE_SIZE;
                        pa += PAGE_SIZE;
                        len -= PAGE_SIZE;
                        sms->sms_loaded += PAGE_SIZE;
                }
        }

        return 0;
}

void
smmu_unload_map(struct smmu_domain *dom, bus_dmamap_t map)
{
        struct smmu_softc *sc = dom->sd_sc;
        struct smmu_map_state *sms = map->_dm_cookie;
        u_long len, dva;

        if (sms->sms_loaded == 0)
                return;

        dva = sms->sms_dva;
        len = sms->sms_loaded;

        while (len > 0) {
                smmu_unmap(dom, dva);

                dva += PAGE_SIZE;
                len -= PAGE_SIZE;
        }

        sms->sms_loaded = 0;

        sc->sc_tlb_sync_context(dom);
}

int
smmu_dmamap_create(bus_dma_tag_t t, bus_size_t size, int nsegments,
    bus_size_t maxsegsz, bus_size_t boundary, int flags, bus_dmamap_t *dmamap)
{
        struct smmu_domain *dom = t->_cookie;
        struct smmu_softc *sc = dom->sd_sc;
        struct smmu_map_state *sms;
        bus_dmamap_t map;
        u_long dva, len;
        int error;

        error = sc->sc_dmat->_dmamap_create(sc->sc_dmat, size,
            nsegments, maxsegsz, boundary, flags, &map);
        if (error)
                return error;

        sms = malloc(sizeof(*sms), M_DEVBUF, (flags & BUS_DMA_NOWAIT) ?
             (M_NOWAIT|M_ZERO) : (M_WAITOK|M_ZERO));
        if (sms == NULL) {
                sc->sc_dmat->_dmamap_destroy(sc->sc_dmat, map);
                return ENOMEM;
        }

        /* Approximation of maximum pages needed. */
        len = round_page(size) + nsegments * PAGE_SIZE;

        /* Allocate IOVA, and a guard page at the end. */
        mtx_enter(&dom->sd_iova_mtx);
        error = extent_alloc_with_descr(dom->sd_iovamap, len + PAGE_SIZE,
            64 * 1024, 0, 0, EX_NOWAIT, &sms->sms_er, &dva);
        mtx_leave(&dom->sd_iova_mtx);
        if (error) {
                sc->sc_dmat->_dmamap_destroy(sc->sc_dmat, map);
                free(sms, M_DEVBUF, sizeof(*sms));
                return error;
        }

        sms->sms_dva = dva;
        sms->sms_len = len;

        while (len > 0) {
                error = smmu_enter(dom, dva, dva, PROT_READ | PROT_WRITE,
                    PROT_NONE, PMAP_CACHE_WB);
                KASSERT(error == 0); /* FIXME: rollback smmu_enter() */
                dva += PAGE_SIZE;
                len -= PAGE_SIZE;
        }

        map->_dm_cookie = sms;
        *dmamap = map;
        return 0;
}

void
smmu_dmamap_destroy(bus_dma_tag_t t, bus_dmamap_t map)
{
        struct smmu_domain *dom = t->_cookie;
        struct smmu_softc *sc = dom->sd_sc;
        struct smmu_map_state *sms = map->_dm_cookie;
        u_long dva, len;
        int error;

        if (sms->sms_loaded)
                smmu_dmamap_unload(t, map);

        dva = sms->sms_dva;
        len = sms->sms_len;

        while (len > 0) {
                smmu_remove(dom, dva);
                dva += PAGE_SIZE;
                len -= PAGE_SIZE;
        }

        mtx_enter(&dom->sd_iova_mtx);
        error = extent_free(dom->sd_iovamap, sms->sms_dva,
            sms->sms_len + PAGE_SIZE, EX_NOWAIT);
        mtx_leave(&dom->sd_iova_mtx);
        KASSERT(error == 0);

        free(sms, M_DEVBUF, sizeof(*sms));
        sc->sc_dmat->_dmamap_destroy(sc->sc_dmat, map);
}

int
smmu_dmamap_load(bus_dma_tag_t t, bus_dmamap_t map, void *buf,
    bus_size_t buflen, struct proc *p, int flags)
{
        struct smmu_domain *dom = t->_cookie;
        struct smmu_softc *sc = dom->sd_sc;
        int error;

        error = sc->sc_dmat->_dmamap_load(sc->sc_dmat, map,
            buf, buflen, p, flags);
        if (error)
                return error;

        error = smmu_load_map(dom, map);
        if (error)
                sc->sc_dmat->_dmamap_unload(sc->sc_dmat, map);

        return error;
}

int
smmu_dmamap_load_mbuf(bus_dma_tag_t t, bus_dmamap_t map, struct mbuf *m0,
    int flags)
{
        struct smmu_domain *dom = t->_cookie;
        struct smmu_softc *sc = dom->sd_sc;
        int error;

        error = sc->sc_dmat->_dmamap_load_mbuf(sc->sc_dmat, map,
            m0, flags);
        if (error)
                return error;

        error = smmu_load_map(dom, map);
        if (error)
                sc->sc_dmat->_dmamap_unload(sc->sc_dmat, map);

        return error;
}

int
smmu_dmamap_load_uio(bus_dma_tag_t t, bus_dmamap_t map, struct uio *uio,
    int flags)
{
        struct smmu_domain *dom = t->_cookie;
        struct smmu_softc *sc = dom->sd_sc;
        int error;

        error = sc->sc_dmat->_dmamap_load_uio(sc->sc_dmat, map,
            uio, flags);
        if (error)
                return error;

        error = smmu_load_map(dom, map);
        if (error)
                sc->sc_dmat->_dmamap_unload(sc->sc_dmat, map);

        return error;
}

int
smmu_dmamap_load_raw(bus_dma_tag_t t, bus_dmamap_t map, bus_dma_segment_t *segs,
    int nsegs, bus_size_t size, int flags)
{
        struct smmu_domain *dom = t->_cookie;
        struct smmu_softc *sc = dom->sd_sc;
        int error;

        error = sc->sc_dmat->_dmamap_load_raw(sc->sc_dmat, map,
            segs, nsegs, size, flags);
        if (error)
                return error;

        error = smmu_load_map(dom, map);
        if (error)
                sc->sc_dmat->_dmamap_unload(sc->sc_dmat, map);

        return error;
}

void
smmu_dmamap_unload(bus_dma_tag_t t, bus_dmamap_t map)
{
        struct smmu_domain *dom = t->_cookie;
        struct smmu_softc *sc = dom->sd_sc;

        smmu_unload_map(dom, map);
        sc->sc_dmat->_dmamap_unload(sc->sc_dmat, map);
}

#define SMMU_DMA_MAP(_sdm)      ((_sdm)->sdm_map)
#define SMMU_DMA_LEN(_sdm)      ((_sdm)->sdm_size)
#define SMMU_DMA_DVA(_sdm)      ((_sdm)->sdm_map->dm_segs[0].ds_addr)
#define SMMU_DMA_KVA(_sdm)      ((void *)(_sdm)->sdm_kva)

struct smmu_dmamem *
smmu_dmamem_alloc(bus_dma_tag_t dmat, bus_size_t size, bus_size_t align)
{
        struct smmu_dmamem *sdm;
        int nsegs;

        sdm = malloc(sizeof(*sdm), M_DEVBUF, M_WAITOK | M_ZERO);
        sdm->sdm_size = size;

        if (bus_dmamap_create(dmat, size, 1, size, 0,
            BUS_DMA_WAITOK | BUS_DMA_ALLOCNOW, &sdm->sdm_map) != 0)
                goto sdmfree;

        if (bus_dmamem_alloc(dmat, size, align, 0, &sdm->sdm_seg, 1,
            &nsegs, BUS_DMA_WAITOK) != 0)
                goto destroy;

        if (bus_dmamem_map(dmat, &sdm->sdm_seg, nsegs, size,
            &sdm->sdm_kva, BUS_DMA_WAITOK) != 0)
                goto free;

        if (bus_dmamap_load(dmat, sdm->sdm_map, sdm->sdm_kva, size,
            NULL, BUS_DMA_WAITOK) != 0)
                goto unmap;

        bzero(sdm->sdm_kva, size);
        bus_dmamap_sync(dmat, sdm->sdm_map, 0, size,
            BUS_DMASYNC_PREWRITE);

        return sdm;

unmap:
        bus_dmamem_unmap(dmat, sdm->sdm_kva, size);
free:
        bus_dmamem_free(dmat, &sdm->sdm_seg, 1);
destroy:
        bus_dmamap_destroy(dmat, sdm->sdm_map);
sdmfree:
        free(sdm, M_DEVBUF, sizeof(*sdm));

        return NULL;
}

void
smmu_dmamem_free(bus_dma_tag_t dmat, struct smmu_dmamem *sdm)
{
        bus_dmamem_unmap(dmat, sdm->sdm_kva, sdm->sdm_size);
        bus_dmamem_free(dmat, &sdm->sdm_seg, 1);
        bus_dmamap_destroy(dmat, sdm->sdm_map);
        free(sdm, M_DEVBUF, sizeof(*sdm));
}

/* SMMU v3 */
int
smmu_v3_attach(struct smmu_softc *sc)
{
        uint32_t reg;
        int i;

        if (smmu_attach(sc) != 0)
                return ENXIO;

        reg = smmu_v3_read_4(sc, SMMU_V3_IDR0);
        if (!(reg & SMMU_V3_TTF_AA64)) {
                printf(": no support for AA64\n");
                return ENXIO;
        }
        if (reg & SMMU_V3_IDR0_S1P)
                sc->sc_has_s1 = 1;
        if (reg & SMMU_V3_IDR0_S2P)
                sc->sc_has_s2 = 1;
        if (reg & SMMU_V3_IDR0_ASID16)
                sc->v3.sc_has_asid16s = 1;
        if (reg & SMMU_V3_IDR0_PRI)
                sc->v3.sc_has_pri = 1;
        if (reg & SMMU_V3_IDR0_VMID16)
                sc->sc_has_vmid16s = 1;
        if (reg & SMMU_V3_IDR0_CD2L)
                sc->v3.sc_2lvl_cdtab = 1;
        if (SMMU_V3_IDR0_ST_LEVEL(reg) == SMMU_V3_IDR0_ST_LEVEL_2)
                sc->v3.sc_2lvl_strtab = 1;

        reg = smmu_v3_read_4(sc, SMMU_V3_IDR1);
        sc->v3.sc_cmdq.sq_size_log2 = SMMU_V3_IDR1_CMDQS(reg);
        sc->v3.sc_eventq.sq_size_log2 = SMMU_V3_IDR1_EVENTQS(reg);
        sc->v3.sc_priq.sq_size_log2 = SMMU_V3_IDR1_PRIQS(reg);
        sc->v3.sc_sidsize = SMMU_V3_IDR1_SIDSIZE(reg);
        if (sc->v3.sc_sidsize <= 8)
                sc->v3.sc_2lvl_strtab = 0;

        reg = smmu_v3_read_4(sc, SMMU_V3_IDR5);
        switch (SMMU_V3_IDR5_OAS(reg)) {
        case SMMU_V3_IDR5_OAS_32BIT:
                sc->sc_pa_bits = 32;
                break;
        case SMMU_V3_IDR5_OAS_36BIT:
                sc->sc_pa_bits = 36;
                break;
        case SMMU_V3_IDR5_OAS_40BIT:
                sc->sc_pa_bits = 40;
                break;
        case SMMU_V3_IDR5_OAS_42BIT:
                sc->sc_pa_bits = 42;
                break;
        case SMMU_V3_IDR5_OAS_44BIT:
                sc->sc_pa_bits = 44;
                break;
        case SMMU_V3_IDR5_OAS_48BIT:
                sc->sc_pa_bits = 48;
                break;
        case SMMU_V3_IDR5_OAS_52BIT:
        default:
                sc->sc_pa_bits = 52;
                break;
        }
        sc->sc_va_bits = 48;
#if notyet
        if (reg & SMMU_V3_IDR5_VAX)
                sc->sc_va_bits = 52;
#endif
        /* Unless there's no AA64, then it's 40. */
        sc->sc_ipa_bits = sc->sc_pa_bits;

        /* If IDR3.STT=0, maximum is 39. */
        reg = smmu_v3_read_4(sc, SMMU_V3_IDR3);
        if ((reg & SMMU_V3_IDR3_STT) == 0) {
                sc->sc_va_bits = min(sc->sc_va_bits, 39);
                sc->sc_ipa_bits = min(sc->sc_ipa_bits, 39);
        }

        mtx_init(&sc->v3.sc_cmdq_mtx, IPL_VM);
        sc->v3.sc_cmdq.sq_sdm = smmu_dmamem_alloc(sc->sc_dmat,
             (1ULL << sc->v3.sc_cmdq.sq_size_log2) * 2 * sizeof(uint64_t),
             (1ULL << sc->v3.sc_cmdq.sq_size_log2) * 2 * sizeof(uint64_t));
        if (sc->v3.sc_cmdq.sq_sdm == NULL) {
                printf(": can't allocate command queue\n");
                goto out;
        }
        sc->v3.sc_eventq.sq_sdm = smmu_dmamem_alloc(sc->sc_dmat,
             (1ULL << sc->v3.sc_eventq.sq_size_log2) * 4 * sizeof(uint64_t),
             (1ULL << sc->v3.sc_eventq.sq_size_log2) * 4 * sizeof(uint64_t));
        if (sc->v3.sc_eventq.sq_sdm == NULL) {
                printf(": can't allocate event queue\n");
                goto free_cmdq;
        }
        if (sc->v3.sc_has_pri) {
                sc->v3.sc_priq.sq_sdm = smmu_dmamem_alloc(sc->sc_dmat,
                     (1ULL << sc->v3.sc_priq.sq_size_log2) * 2 * sizeof(uint64_t),
                     (1ULL << sc->v3.sc_priq.sq_size_log2) * 2 * sizeof(uint64_t));
                if (sc->v3.sc_priq.sq_sdm == NULL) {
                        printf(": can't allocate pri queue\n");
                        goto free_evtq;
                }
        }

        /* Abort transaction if already enabled. */
        if (smmu_v3_read_4(sc, SMMU_V3_CR0) & SMMU_V3_CR0_SMMUEN) {
                reg = smmu_v3_read_4(sc, SMMU_V3_GBPA);
                reg |= SMMU_V3_GBPA_ABORT;
                smmu_v3_write_4(sc, SMMU_V3_GBPA, reg | SMMU_V3_GBPA_UPDATE);
                for (i = 100000; i > 0; i--) {
                        if (!(smmu_v3_read_4(sc, SMMU_V3_GBPA) & SMMU_V3_GBPA_UPDATE))
                                break;
                }
                if (i == 0) {
                        printf(": failed waiting for update\n");
                        goto free_priq;
                }
        }

        /* Disable SMMU */
        smmu_v3_write_ack(sc, SMMU_V3_CR0, SMMU_V3_CR0ACK, 0);

        smmu_v3_write_4(sc, SMMU_V3_CR1,
            SMMU_V3_CR1_TABLE_SH(SMMU_V3_CR1_SHARE_ISH) |
            SMMU_V3_CR1_TABLE_OC(SMMU_V3_CR1_CACHE_WB) |
            SMMU_V3_CR1_TABLE_IC(SMMU_V3_CR1_CACHE_WB) |
            SMMU_V3_CR1_QUEUE_SH(SMMU_V3_CR1_SHARE_ISH) |
            SMMU_V3_CR1_QUEUE_OC(SMMU_V3_CR1_CACHE_WB) |
            SMMU_V3_CR1_QUEUE_IC(SMMU_V3_CR1_CACHE_WB));
        smmu_v3_write_4(sc, SMMU_V3_CR2, SMMU_V3_CR2_PTM |
            SMMU_V3_CR2_RECINVSID | SMMU_V3_CR2_E2H);

        if (sc->v3.sc_2lvl_strtab) {
                sc->v3.sc_strtab_l1 = smmu_dmamem_alloc(sc->sc_dmat,
                    ((1ULL << sc->v3.sc_sidsize) / 256) * sizeof(uint64_t),
                    ((1ULL << sc->v3.sc_sidsize) / 256) * sizeof(uint64_t));
                if (sc->v3.sc_strtab_l1 == NULL) {
                        printf(": can't allocate strtab\n");
                        goto free_priq;
                }
                smmu_v3_write_8(sc, SMMU_V3_STRTAB_BASE,
                    SMMU_V3_STRTAB_BASE_RA |
                    SMMU_DMA_DVA(sc->v3.sc_strtab_l1));
                smmu_v3_write_4(sc, SMMU_V3_STRTAB_BASE_CFG,
                    SMMU_V3_STRTAB_BASE_CFG_FMT_L2 |
                    SMMU_V3_STRTAB_BASE_CFG_SPLIT(8) |
                    SMMU_V3_STRTAB_BASE_CFG_LOG2SIZE(sc->v3.sc_sidsize));
                sc->v3.sc_strtab_l2 = mallocarray(
                    ((1ULL << sc->v3.sc_sidsize) / 256), sizeof(void *),
                    M_DEVBUF, M_WAITOK | M_ZERO);
        } else {
                sc->v3.sc_strtab_l1 = smmu_dmamem_alloc(sc->sc_dmat,
                    (1ULL << sc->v3.sc_sidsize) * 8 * sizeof(uint64_t),
                    (1ULL << sc->v3.sc_sidsize) * 8 * sizeof(uint64_t));
                if (sc->v3.sc_strtab_l1 == NULL) {
                        printf(": can't allocate strtab\n");
                        goto free_priq;
                }
                smmu_v3_write_8(sc, SMMU_V3_STRTAB_BASE,
                    SMMU_V3_STRTAB_BASE_RA |
                    SMMU_DMA_DVA(sc->v3.sc_strtab_l1));
                smmu_v3_write_4(sc, SMMU_V3_STRTAB_BASE_CFG,
                    SMMU_V3_STRTAB_BASE_CFG_FMT_L1 |
                    SMMU_V3_STRTAB_BASE_CFG_LOG2SIZE(sc->v3.sc_sidsize));
        }

        smmu_v3_write_8(sc, SMMU_V3_CMDQ_BASE,
            SMMU_V3_CMDQ_BASE_RA |
            SMMU_DMA_DVA(sc->v3.sc_cmdq.sq_sdm) |
            SMMU_V3_CMDQ_BASE_LOG2SIZE(sc->v3.sc_cmdq.sq_size_log2));
        smmu_v3_write_4(sc, SMMU_V3_CMDQ_PROD, 0);
        smmu_v3_write_4(sc, SMMU_V3_CMDQ_CONS, 0);
        smmu_v3_write_ack(sc, SMMU_V3_CR0, SMMU_V3_CR0ACK,
            SMMU_V3_CR0_CMDQEN);

        smmu_v3_cfgi_all(sc);
        smmu_v3_tlbi_all(sc, SMMU_V3_CMD_TLBI_EL2_ALL);
        smmu_v3_tlbi_all(sc, SMMU_V3_CMD_TLBI_NSNH_ALL);

        smmu_v3_write_8(sc, SMMU_V3_EVENTQ_BASE,
            SMMU_V3_EVENTQ_BASE_WA |
            SMMU_DMA_DVA(sc->v3.sc_eventq.sq_sdm) |
            SMMU_V3_EVENTQ_BASE_LOG2SIZE(sc->v3.sc_eventq.sq_size_log2));
        smmu_v3_write_4(sc, SMMU_V3_EVENTQ_PROD, 0);
        smmu_v3_write_4(sc, SMMU_V3_EVENTQ_CONS, 0);
        smmu_v3_write_ack(sc, SMMU_V3_CR0, SMMU_V3_CR0ACK,
            smmu_v3_read_4(sc, SMMU_V3_CR0) | SMMU_V3_CR0_EVENTQEN);

        if (sc->v3.sc_has_pri) {
                smmu_v3_write_8(sc, SMMU_V3_PRIQ_BASE,
                    SMMU_V3_PRIQ_BASE_WA |
                    SMMU_DMA_DVA(sc->v3.sc_priq.sq_sdm) |
                    SMMU_V3_PRIQ_BASE_LOG2SIZE(sc->v3.sc_priq.sq_size_log2));
                smmu_v3_write_4(sc, SMMU_V3_PRIQ_PROD, 0);
                smmu_v3_write_4(sc, SMMU_V3_PRIQ_CONS, 0);
                smmu_v3_write_ack(sc, SMMU_V3_CR0, SMMU_V3_CR0ACK,
                    smmu_v3_read_4(sc, SMMU_V3_CR0) | SMMU_V3_CR0_PRIQEN);
        }

        /* Disable MSIs, use wired IRQs, re-enable IRQs. */
        smmu_v3_write_ack(sc, SMMU_V3_IRQ_CTRL, SMMU_V3_IRQ_CTRLACK, 0);
        smmu_v3_write_8(sc, SMMU_V3_GERROR_IRQ_CFG0, 0);
        smmu_v3_write_8(sc, SMMU_V3_EVENTQ_IRQ_CFG0, 0);
        if (sc->v3.sc_has_pri)
                smmu_v3_write_8(sc, SMMU_V3_PRIQ_IRQ_CFG0, 0);
        smmu_v3_write_ack(sc, SMMU_V3_IRQ_CTRL, SMMU_V3_IRQ_CTRLACK,
            SMMU_V3_IRQ_CTRL_GERROR | SMMU_V3_IRQ_CTRL_EVENTQ |
            (sc->v3.sc_has_pri ? SMMU_V3_IRQ_CTRL_PRIQ : 0));

        smmu_v3_write_ack(sc, SMMU_V3_CR0, SMMU_V3_CR0ACK,
            smmu_v3_read_4(sc, SMMU_V3_CR0) | SMMU_V3_CR0_SMMUEN);

        printf("\n");

        sc->sc_domain_create = smmu_v3_domain_create;
        sc->sc_tlbi_va = smmu_v3_tlbi_va;
        sc->sc_tlb_sync_context = smmu_v3_tlb_sync_context;
        return 0;

//free_strtab:
//      smmu_dmamem_free(sc->sc_dmat, sc->v3.sc_strtab_l1);
free_priq:
        if (sc->v3.sc_has_pri)
                smmu_dmamem_free(sc->sc_dmat, sc->v3.sc_priq.sq_sdm);
free_evtq:
        smmu_dmamem_free(sc->sc_dmat, sc->v3.sc_eventq.sq_sdm);
free_cmdq:
        smmu_dmamem_free(sc->sc_dmat, sc->v3.sc_cmdq.sq_sdm);
out:
        return ENXIO;
}

int
smmu_v3_event_irq(void *cookie)
{
        struct smmu_softc *sc = cookie;
        struct smmu_v3_queue *sq = &sc->v3.sc_eventq;
        uint64_t *event;
        uint32_t cons, prod;
        bus_size_t off;
        int handled = 0;

        for (;;) {
                prod = smmu_v3_read_4(sc, SMMU_V3_EVENTQ_PROD);
                if (SMMU_V3_Q_OVF(sq->sq_prod) != SMMU_V3_Q_OVF(prod))
                        printf("%s: event queue overflow\n",
                            sc->sc_dev.dv_xname);
                sq->sq_prod = prod;

                /* Stop if empty. */
                if (SMMU_V3_Q_IDX(sq, sq->sq_cons) ==
                    SMMU_V3_Q_IDX(sq, sq->sq_prod) &&
                    SMMU_V3_Q_WRP(sq, sq->sq_cons) ==
                    SMMU_V3_Q_WRP(sq, sq->sq_prod))
                        break;

                /* Print event information. */
                off = SMMU_V3_Q_IDX(sq, sq->sq_cons) * 4 * sizeof(uint64_t);
                bus_dmamap_sync(sc->sc_dmat, SMMU_DMA_MAP(sq->sq_sdm), off,
                    4 * sizeof(uint64_t), BUS_DMASYNC_POSTWRITE);
                event = SMMU_DMA_KVA(sq->sq_sdm) + off;
                printf("%s: event 0x%llx 0x%llx 0x%llx 0x%llx\n",
                    sc->sc_dev.dv_xname, event[0], event[1], event[2], event[3]);

                /* Let HW know we consumed */
                cons = (SMMU_V3_Q_WRP(sq, sq->sq_cons) |
                    SMMU_V3_Q_IDX(sq, sq->sq_cons)) + 1;
                sq->sq_cons = SMMU_V3_Q_OVF(sq->sq_cons) |
                    SMMU_V3_Q_WRP(sq, cons) |
                    SMMU_V3_Q_IDX(sq, cons);
                membar_sync();
                smmu_v3_write_4(sc, SMMU_V3_EVENTQ_CONS, sq->sq_cons);

                handled = 1;
        }

        /* Sync overflow flag */
        if (SMMU_V3_Q_OVF(sq->sq_prod) != SMMU_V3_Q_OVF(sq->sq_cons)) {
                sq->sq_cons = SMMU_V3_Q_OVF(sq->sq_prod) |
                    SMMU_V3_Q_WRP(sq, sq->sq_cons) |
                    SMMU_V3_Q_IDX(sq, sq->sq_cons);
                membar_sync();
                smmu_v3_write_4(sc, SMMU_V3_EVENTQ_CONS, sq->sq_cons);
        }

        return handled;
}

int
smmu_v3_gerr_irq(void *cookie)
{
        struct smmu_softc *sc = cookie;
        uint32_t gerror, gerrorn;

        gerror = smmu_v3_read_4(sc, SMMU_V3_GERROR);
        gerrorn = smmu_v3_read_4(sc, SMMU_V3_GERRORN);
        smmu_v3_write_4(sc, SMMU_V3_GERRORN, gerror);

        gerror = (gerror ^ gerrorn) & SMMU_V3_GERROR_MASK;

        if (gerror & SMMU_V3_GERROR_CMDQ_ERR) {
                uint32_t cons = smmu_v3_read_4(sc, SMMU_V3_CMDQ_CONS);
                printf("%s: cmdq error 0x%x on cmd idx %u\n",
                    sc->sc_dev.dv_xname, SMMU_V3_CMDQ_CONS_ERR(cons),
                    SMMU_V3_Q_IDX(&sc->v3.sc_cmdq, cons));
        } else {
                printf("%s: gerror 0x%x\n", sc->sc_dev.dv_xname, gerror);
        }

        return 1;
}

int
smmu_v3_priq_irq(void *cookie)
{
        struct smmu_softc *sc = cookie;
        struct smmu_v3_queue *sq = &sc->v3.sc_priq;
        uint64_t *pri;
        uint32_t cons, prod;
        int handled = 0;

        for (;;) {
                prod = smmu_v3_read_4(sc, SMMU_V3_PRIQ_PROD);
                if (SMMU_V3_Q_OVF(sq->sq_prod) != SMMU_V3_Q_OVF(prod))
                        printf("%s: event queue overflow\n",
                            sc->sc_dev.dv_xname);
                sq->sq_prod = prod;

                /* Stop if empty. */
                if (SMMU_V3_Q_IDX(sq, sq->sq_cons) ==
                    SMMU_V3_Q_IDX(sq, sq->sq_prod) &&
                    SMMU_V3_Q_WRP(sq, sq->sq_cons) ==
                    SMMU_V3_Q_WRP(sq, sq->sq_prod))
                        break;

                /* Print pri information. */
                bus_dmamap_sync(sc->sc_dmat, SMMU_DMA_MAP(sq->sq_sdm),
                    SMMU_V3_Q_IDX(sq, sq->sq_cons) * 2 * sizeof(uint64_t),
                    2 * sizeof(uint64_t),
                    BUS_DMASYNC_POSTWRITE);
                pri = SMMU_DMA_KVA(sq->sq_sdm) +
                    SMMU_V3_Q_IDX(sq, sq->sq_cons) * 2 * sizeof(uint64_t);
                printf("%s: pri 0x%llx 0x%llx\n", sc->sc_dev.dv_xname,
                    pri[0], pri[1]);

                /* Increase consumed */
                cons = (SMMU_V3_Q_WRP(sq, sq->sq_cons) |
                    SMMU_V3_Q_IDX(sq, sq->sq_cons)) + 1;
                sq->sq_cons = SMMU_V3_Q_OVF(sq->sq_cons) |
                    SMMU_V3_Q_WRP(sq, cons) |
                    SMMU_V3_Q_IDX(sq, cons);
                membar_sync();
                smmu_v3_write_4(sc, SMMU_V3_PRIQ_CONS, sq->sq_cons);

                handled = 1;
        }

        /* Sync overflow flag */
        if (SMMU_V3_Q_OVF(sq->sq_prod) != SMMU_V3_Q_OVF(sq->sq_cons)) {
                sq->sq_cons = SMMU_V3_Q_OVF(sq->sq_prod) |
                    SMMU_V3_Q_WRP(sq, sq->sq_cons) |
                    SMMU_V3_Q_IDX(sq, sq->sq_cons);
                membar_sync();
                smmu_v3_write_4(sc, SMMU_V3_PRIQ_CONS, sq->sq_cons);
        }

        return handled;
}

uint32_t
smmu_v3_read_4(struct smmu_softc *sc, bus_size_t off)
{
        return bus_space_read_4(sc->sc_iot, sc->sc_ioh, off);
}

void
smmu_v3_write_4(struct smmu_softc *sc, bus_size_t off, uint32_t val)
{
        bus_space_write_4(sc->sc_iot, sc->sc_ioh, off, val);
}

uint64_t
smmu_v3_read_8(struct smmu_softc *sc, bus_size_t off)
{
        return bus_space_read_8(sc->sc_iot, sc->sc_ioh, off);
}

void
smmu_v3_write_8(struct smmu_softc *sc, bus_size_t off, uint64_t val)
{
        bus_space_write_8(sc->sc_iot, sc->sc_ioh, off, val);
}

int
smmu_v3_write_ack(struct smmu_softc *sc, bus_size_t off, bus_size_t ack_off,
    uint32_t val)
{
        int i;

        smmu_v3_write_4(sc, off, val);

        for (i = 100000; i > 0; i--) {
                if (smmu_v3_read_4(sc, ack_off) == val)
                        break;
        }
        if (i == 0) {
                printf("%s: failed waiting for ack\n", sc->sc_dev.dv_xname);
                return ETIMEDOUT;
        }

        return 0;
}

int
smmu_v3_domain_create(struct smmu_domain *dom)
{
        struct smmu_softc *sc = dom->sd_sc;
        uint64_t *cd, *ste;
        paddr_t pa;
        vaddr_t l0va;
        uint32_t iovabits;

        if (dom->sd_sid >= (1ULL << sc->v3.sc_sidsize))
                return EINVAL;

        if (dom->sd_stage != 1)
                return EINVAL;

        if (sc->v3.sc_has_asid16s) {
                if (sc->v3.sc_next_asid == (1 << 16) - 1)
                        return EINVAL;
        } else {
                if (sc->v3.sc_next_asid == (1 << 8) - 1)
                        return EINVAL;
        }
        dom->v3.sd_asid = sc->v3.sc_next_asid++;

        dom->v3.sd_cd = smmu_dmamem_alloc(sc->sc_dmat, 64, 64);
        if (dom->v3.sd_cd == NULL) {
                printf(": can't allocate context descriptor\n");
                return ENOMEM;
        }

        if (dom->sd_stage == 1)
                iovabits = sc->sc_va_bits;
        else
                iovabits = sc->sc_ipa_bits;
        if (iovabits >= 40)
                dom->sd_4level = 1;

        if (dom->sd_4level) {
                while (dom->sd_vp.l0 == NULL) {
                        dom->sd_vp.l0 = pool_get(&sc->sc_vp_pool,
                            PR_WAITOK | PR_ZERO);
                }
                l0va = (vaddr_t)dom->sd_vp.l0->l0; /* top level is l0 */
        } else {
                while (dom->sd_vp.l1 == NULL) {
                        dom->sd_vp.l1 = pool_get(&sc->sc_vp_pool,
                            PR_WAITOK | PR_ZERO);
                }
                l0va = (vaddr_t)dom->sd_vp.l1->l1; /* top level is l1 */
        }
        pmap_extract(pmap_kernel(), l0va, &pa);

        cd = SMMU_DMA_KVA(dom->v3.sd_cd);
        cd[0] =
            SMMU_V3_CD_0_TCR_T0SZ(64 - iovabits) |
            SMMU_V3_CD_0_TCR_TG0_4KB |
            SMMU_V3_CD_0_TCR_IRGN0_WBWA |
            SMMU_V3_CD_0_TCR_ORGN0_WBWA |
            SMMU_V3_CD_0_TCR_SH0_ISH |
            SMMU_V3_CD_0_TCR_EPD1 |
            SMMU_V3_CD_0_V |
            SMMU_V3_CD_0_TCR_IPS_48BIT |
            SMMU_V3_CD_0_AA64 |
            SMMU_V3_CD_0_R |
            SMMU_V3_CD_0_A |
            SMMU_V3_CD_0_ASET |
            SMMU_V3_CD_0_ASID(dom->v3.sd_asid);
        cd[1] = pa;
        cd[3] =
            SMMU_V3_CD_3_MAIR_ATTR(SMMU_V3_CD_3_MAIR_DEVICE_nGnRnE, 0) |
            SMMU_V3_CD_3_MAIR_ATTR(SMMU_V3_CD_3_MAIR_DEVICE_nGnRE, 1) |
            SMMU_V3_CD_3_MAIR_ATTR(SMMU_V3_CD_3_MAIR_DEVICE_NC, 2) |
            SMMU_V3_CD_3_MAIR_ATTR(SMMU_V3_CD_3_MAIR_DEVICE_WB, 3) |
            SMMU_V3_CD_3_MAIR_ATTR(SMMU_V3_CD_3_MAIR_DEVICE_WT, 4);
        bus_dmamap_sync(sc->sc_dmat, SMMU_DMA_MAP(dom->v3.sd_cd), 0,
            SMMU_DMA_LEN(dom->v3.sd_cd), BUS_DMASYNC_PREWRITE);
        smmu_v3_cfgi_cd(dom);

        snprintf(dom->sd_exname, sizeof(dom->sd_exname), "%s:%x",
            sc->sc_dev.dv_xname, dom->sd_sid);
        dom->sd_iovamap = extent_create(dom->sd_exname, 0,
            (1LL << iovabits) - 1, M_DEVBUF, NULL, 0, EX_WAITOK |
            EX_NOCOALESCE);

        if (sc->v3.sc_2lvl_strtab) {
                if (sc->v3.sc_strtab_l2[dom->sd_sid / 256] == NULL) {
                        sc->v3.sc_strtab_l2[dom->sd_sid / 256] =
                            smmu_dmamem_alloc(sc->sc_dmat,
                            256 * 8 * sizeof(uint64_t),
                            64 * 1024);
                        if (sc->v3.sc_strtab_l2[dom->sd_sid / 256] == NULL) {
                                printf("%s: can't allocate strtab\n",
                                    sc->sc_dev.dv_xname);
                                return ENXIO;
                        }
                        ste = SMMU_DMA_KVA(sc->v3.sc_strtab_l1) +
                            (dom->sd_sid / 256) * sizeof(uint64_t);
                        *ste = SMMU_DMA_DVA(sc->v3.sc_strtab_l2[dom->sd_sid / 256]) |
                            (8 /* split */ + 1);
                        bus_dmamap_sync(sc->sc_dmat,
                            SMMU_DMA_MAP(sc->v3.sc_strtab_l1),
                            (dom->sd_sid / 256) * sizeof(uint64_t),
                            sizeof(uint64_t), BUS_DMASYNC_PREWRITE);
                }
                ste = SMMU_DMA_KVA(sc->v3.sc_strtab_l2[dom->sd_sid / 256]) +
                    (dom->sd_sid % 256) * 8 * sizeof(uint64_t);
        } else {
                ste = SMMU_DMA_KVA(sc->v3.sc_strtab_l1) +
                    dom->sd_sid * 8 * sizeof(uint64_t);
        }

        ste[1] =  SMMU_V3_STE_1_S1CIR_WBRA | SMMU_V3_STE_1_S1COR_WBRA |
            SMMU_V3_STE_1_S1CSH_ISH | SMMU_V3_STE_1_EATS_TRANS |
//          SMMU_V3_STE_1_STRW_NSEL1 | SMMU_V3_STE_1_S1DSS_SSID0;
            SMMU_V3_STE_1_STRW_EL2 | SMMU_V3_STE_1_S1DSS_SSID0;
        ste[0] = SMMU_V3_STE_0_V | SMMU_V3_STE_0_CFG_S1_TRANS |
            SMMU_V3_STE_0_S1FMT_LINEAR | SMMU_DMA_DVA(dom->v3.sd_cd);
        if (sc->v3.sc_2lvl_strtab) {
                bus_dmamap_sync(sc->sc_dmat,
                    SMMU_DMA_MAP(sc->v3.sc_strtab_l2[dom->sd_sid / 256]),
                    (dom->sd_sid % 256) * 8 * sizeof(uint64_t),
                    8 * sizeof(uint64_t), BUS_DMASYNC_PREWRITE);
        } else {
                bus_dmamap_sync(sc->sc_dmat,
                    SMMU_DMA_MAP(sc->v3.sc_strtab_l1),
                    dom->sd_sid * 8 * sizeof(uint64_t),
                    8 * sizeof(uint64_t), BUS_DMASYNC_PREWRITE);
        }
        smmu_v3_cfgi_ste(dom);

        smmu_v3_tlbi_asid(dom);
        return 0;
}

int
smmu_v3_sync(struct smmu_softc *sc)
{
        struct smmu_v3_queue *sq = &sc->v3.sc_cmdq;
        volatile uint64_t *cmd;
        uint32_t prod;
        bus_size_t off;
        int i;

        MUTEX_ASSERT_LOCKED(&sc->v3.sc_cmdq_mtx);

        /* TODO: Handle this more properly. */
        sq->sq_cons = smmu_v3_read_4(sc, SMMU_V3_CMDQ_CONS);
        if (SMMU_V3_Q_IDX(sq, sq->sq_cons) == SMMU_V3_Q_IDX(sq, sq->sq_prod) &&
            SMMU_V3_Q_WRP(sq, sq->sq_cons) != SMMU_V3_Q_WRP(sq, sq->sq_prod)) {
                printf("%s: CMDQ ran out of space\n", sc->sc_dev.dv_xname);
                return ETIMEDOUT;
        }

        off = SMMU_V3_Q_IDX(sq, sq->sq_prod) * 2 * sizeof(uint64_t);
        cmd = SMMU_DMA_KVA(sq->sq_sdm) + off;
        bus_dmamap_sync(sc->sc_dmat, SMMU_DMA_MAP(sq->sq_sdm), off,
            2 * sizeof(uint64_t), BUS_DMASYNC_POSTREAD);
        cmd[0] = SMMU_V3_CMD_SYNC | SMMU_V3_CMD_SYNC_0_CS_SEV;
        cmd[1] = 0;
        bus_dmamap_sync(sc->sc_dmat, SMMU_DMA_MAP(sq->sq_sdm), off,
            2 * sizeof(uint64_t), BUS_DMASYNC_PREWRITE);

        /* Let HW know we produced */
        prod = (SMMU_V3_Q_WRP(sq, sq->sq_prod) |
            SMMU_V3_Q_IDX(sq, sq->sq_prod)) + 1;
        sq->sq_prod = SMMU_V3_Q_OVF(sq->sq_prod) |
            SMMU_V3_Q_WRP(sq, prod) | SMMU_V3_Q_IDX(sq, prod);
        membar_sync();
        smmu_v3_write_4(sc, SMMU_V3_CMDQ_PROD, sq->sq_prod);

        /*
         * TODO: In a better world, where CPUs could concurrently put in commands,
         * TODO: we should be able to wait until it has *passed* the prod we set.
         */
        for (i = 100000; i > 0; i--) {
                /* Wait until HW processing caught up with us. */
                sq->sq_cons = smmu_v3_read_4(sc, SMMU_V3_CMDQ_CONS);
                if ((SMMU_V3_Q_WRP(sq, sq->sq_cons) ==
                     SMMU_V3_Q_WRP(sq, sq->sq_prod)) &&
                    (SMMU_V3_Q_IDX(sq, sq->sq_cons) ==
                     SMMU_V3_Q_IDX(sq, sq->sq_prod)))
                        break;
        }
        if (i == 0) {
                printf("%s: timeout waiting for SYNC\n", sc->sc_dev.dv_xname);
                sq->sq_cons = smmu_v3_read_4(sc, SMMU_V3_CMDQ_CONS);
                return ETIMEDOUT;
        }

        return 0;
}

void
smmu_v3_cfgi_all(struct smmu_softc *sc)
{
        struct smmu_v3_queue *sq = &sc->v3.sc_cmdq;
        volatile uint64_t *cmd;
        uint32_t prod;
        bus_size_t off;

        mtx_enter(&sc->v3.sc_cmdq_mtx);

        /* TODO: Handle this more properly. */
        sq->sq_cons = smmu_v3_read_4(sc, SMMU_V3_CMDQ_CONS);
        if (SMMU_V3_Q_IDX(sq, sq->sq_cons) == SMMU_V3_Q_IDX(sq, sq->sq_prod) &&
            SMMU_V3_Q_WRP(sq, sq->sq_cons) != SMMU_V3_Q_WRP(sq, sq->sq_prod)) {
                printf("%s: CMDQ ran out of space\n", sc->sc_dev.dv_xname);
                mtx_leave(&sc->v3.sc_cmdq_mtx);
                return;
        }

        off = SMMU_V3_Q_IDX(sq, sq->sq_prod) * 2 * sizeof(uint64_t);
        cmd = SMMU_DMA_KVA(sq->sq_sdm) + off;
        bus_dmamap_sync(sc->sc_dmat, SMMU_DMA_MAP(sq->sq_sdm), off,
            2 * sizeof(uint64_t), BUS_DMASYNC_POSTREAD);
        cmd[0] = SMMU_V3_CMD_CFGI_STE_RANGE;
        cmd[1] = SMMU_V3_CMD_CFGI_1_RANGE(31);
        bus_dmamap_sync(sc->sc_dmat, SMMU_DMA_MAP(sq->sq_sdm), off,
            2 * sizeof(uint64_t), BUS_DMASYNC_PREWRITE);

        /* Let HW know we produced */
        prod = (SMMU_V3_Q_WRP(sq, sq->sq_prod) |
            SMMU_V3_Q_IDX(sq, sq->sq_prod)) + 1;
        sq->sq_prod = SMMU_V3_Q_OVF(sq->sq_prod) |
            SMMU_V3_Q_WRP(sq, prod) | SMMU_V3_Q_IDX(sq, prod);
        membar_sync();
        smmu_v3_write_4(sc, SMMU_V3_CMDQ_PROD, sq->sq_prod);

        smmu_v3_sync(sc);

        mtx_leave(&sc->v3.sc_cmdq_mtx);
}

void
smmu_v3_cfgi_cd(struct smmu_domain *dom)
{
        struct smmu_softc *sc = dom->sd_sc;
        struct smmu_v3_queue *sq = &sc->v3.sc_cmdq;
        volatile uint64_t *cmd;
        uint32_t prod;
        bus_size_t off;

        mtx_enter(&sc->v3.sc_cmdq_mtx);

        /* TODO: Handle this more properly. */
        sq->sq_cons = smmu_v3_read_4(sc, SMMU_V3_CMDQ_CONS);
        if (SMMU_V3_Q_IDX(sq, sq->sq_cons) == SMMU_V3_Q_IDX(sq, sq->sq_prod) &&
            SMMU_V3_Q_WRP(sq, sq->sq_cons) != SMMU_V3_Q_WRP(sq, sq->sq_prod)) {
                printf("%s: CMDQ ran out of space\n", sc->sc_dev.dv_xname);
                mtx_leave(&sc->v3.sc_cmdq_mtx);
                return;
        }

        off = SMMU_V3_Q_IDX(sq, sq->sq_prod) * 2 * sizeof(uint64_t);
        cmd = SMMU_DMA_KVA(sq->sq_sdm) + off;
        bus_dmamap_sync(sc->sc_dmat, SMMU_DMA_MAP(sq->sq_sdm), off,
            2 * sizeof(uint64_t), BUS_DMASYNC_POSTREAD);
        cmd[0] = SMMU_V3_CMD_CFGI_CD |
            SMMU_V3_CMD_CFGI_0_SID(dom->sd_sid);
        cmd[1] = SMMU_V3_CMD_CFGI_1_LEAF;
        bus_dmamap_sync(sc->sc_dmat, SMMU_DMA_MAP(sq->sq_sdm), off,
            2 * sizeof(uint64_t), BUS_DMASYNC_PREWRITE);

        /* Let HW know we produced */
        prod = (SMMU_V3_Q_WRP(sq, sq->sq_prod) |
            SMMU_V3_Q_IDX(sq, sq->sq_prod)) + 1;
        sq->sq_prod = SMMU_V3_Q_OVF(sq->sq_prod) |
            SMMU_V3_Q_WRP(sq, prod) | SMMU_V3_Q_IDX(sq, prod);
        membar_sync();
        smmu_v3_write_4(sc, SMMU_V3_CMDQ_PROD, sq->sq_prod);

        smmu_v3_sync(sc);

        mtx_leave(&sc->v3.sc_cmdq_mtx);
}

void
smmu_v3_cfgi_ste(struct smmu_domain *dom)
{
        struct smmu_softc *sc = dom->sd_sc;
        struct smmu_v3_queue *sq = &sc->v3.sc_cmdq;
        volatile uint64_t *cmd;
        uint32_t prod;
        bus_size_t off;

        mtx_enter(&sc->v3.sc_cmdq_mtx);

        /* TODO: Handle this more properly. */
        sq->sq_cons = smmu_v3_read_4(sc, SMMU_V3_CMDQ_CONS);
        if (SMMU_V3_Q_IDX(sq, sq->sq_cons) == SMMU_V3_Q_IDX(sq, sq->sq_prod) &&
            SMMU_V3_Q_WRP(sq, sq->sq_cons) != SMMU_V3_Q_WRP(sq, sq->sq_prod)) {
                printf("%s: CMDQ ran out of space\n", sc->sc_dev.dv_xname);
                mtx_leave(&sc->v3.sc_cmdq_mtx);
                return;
        }

        off = SMMU_V3_Q_IDX(sq, sq->sq_prod) * 2 * sizeof(uint64_t);
        cmd = SMMU_DMA_KVA(sq->sq_sdm) + off;
        bus_dmamap_sync(sc->sc_dmat, SMMU_DMA_MAP(sq->sq_sdm), off,
            2 * sizeof(uint64_t), BUS_DMASYNC_POSTREAD);
        cmd[0] = SMMU_V3_CMD_CFGI_STE |
            SMMU_V3_CMD_CFGI_0_SID(dom->sd_sid);
        cmd[1] = SMMU_V3_CMD_CFGI_1_LEAF;
        bus_dmamap_sync(sc->sc_dmat, SMMU_DMA_MAP(sq->sq_sdm), off,
            2 * sizeof(uint64_t), BUS_DMASYNC_PREWRITE);

        /* Let HW know we produced */
        prod = (SMMU_V3_Q_WRP(sq, sq->sq_prod) |
            SMMU_V3_Q_IDX(sq, sq->sq_prod)) + 1;
        sq->sq_prod = SMMU_V3_Q_OVF(sq->sq_prod) |
            SMMU_V3_Q_WRP(sq, prod) | SMMU_V3_Q_IDX(sq, prod);
        membar_sync();
        smmu_v3_write_4(sc, SMMU_V3_CMDQ_PROD, sq->sq_prod);

        smmu_v3_sync(sc);

        mtx_leave(&sc->v3.sc_cmdq_mtx);
}

void
smmu_v3_tlbi_all(struct smmu_softc *sc, uint64_t op)
{
        struct smmu_v3_queue *sq = &sc->v3.sc_cmdq;
        volatile uint64_t *cmd;
        uint32_t prod;
        bus_size_t off;

        mtx_enter(&sc->v3.sc_cmdq_mtx);

        /* TODO: Handle this more properly. */
        sq->sq_cons = smmu_v3_read_4(sc, SMMU_V3_CMDQ_CONS);
        if (SMMU_V3_Q_IDX(sq, sq->sq_cons) == SMMU_V3_Q_IDX(sq, sq->sq_prod) &&
            SMMU_V3_Q_WRP(sq, sq->sq_cons) != SMMU_V3_Q_WRP(sq, sq->sq_prod)) {
                printf("%s: CMDQ ran out of space\n", sc->sc_dev.dv_xname);
                mtx_leave(&sc->v3.sc_cmdq_mtx);
                return;
        }

        off = SMMU_V3_Q_IDX(sq, sq->sq_prod) * 2 * sizeof(uint64_t);
        cmd = SMMU_DMA_KVA(sq->sq_sdm) + off;
        bus_dmamap_sync(sc->sc_dmat, SMMU_DMA_MAP(sq->sq_sdm), off,
            2 * sizeof(uint64_t), BUS_DMASYNC_POSTREAD);
        cmd[0] = op;
        cmd[1] = 0;
        bus_dmamap_sync(sc->sc_dmat, SMMU_DMA_MAP(sq->sq_sdm), off,
            2 * sizeof(uint64_t), BUS_DMASYNC_PREWRITE);

        /* Let HW know we produced */
        prod = (SMMU_V3_Q_WRP(sq, sq->sq_prod) |
            SMMU_V3_Q_IDX(sq, sq->sq_prod)) + 1;
        sq->sq_prod = SMMU_V3_Q_OVF(sq->sq_prod) |
            SMMU_V3_Q_WRP(sq, prod) | SMMU_V3_Q_IDX(sq, prod);
        membar_sync();
        smmu_v3_write_4(sc, SMMU_V3_CMDQ_PROD, sq->sq_prod);

        smmu_v3_sync(sc);

        mtx_leave(&sc->v3.sc_cmdq_mtx);
}

void
smmu_v3_tlbi_asid(struct smmu_domain *dom)
{
        struct smmu_softc *sc = dom->sd_sc;
        struct smmu_v3_queue *sq = &sc->v3.sc_cmdq;
        volatile uint64_t *cmd;
        uint32_t prod;
        bus_size_t off;

        mtx_enter(&sc->v3.sc_cmdq_mtx);

        /* TODO: Handle this more properly. */
        sq->sq_cons = smmu_v3_read_4(sc, SMMU_V3_CMDQ_CONS);
        if (SMMU_V3_Q_IDX(sq, sq->sq_cons) == SMMU_V3_Q_IDX(sq, sq->sq_prod) &&
            SMMU_V3_Q_WRP(sq, sq->sq_cons) != SMMU_V3_Q_WRP(sq, sq->sq_prod)) {
                printf("%s: CMDQ ran out of space\n", sc->sc_dev.dv_xname);
                mtx_leave(&sc->v3.sc_cmdq_mtx);
                return;
        }

        off = SMMU_V3_Q_IDX(sq, sq->sq_prod) * 2 * sizeof(uint64_t);
        cmd = SMMU_DMA_KVA(sq->sq_sdm) + off;
        bus_dmamap_sync(sc->sc_dmat, SMMU_DMA_MAP(sq->sq_sdm), off,
            2 * sizeof(uint64_t), BUS_DMASYNC_POSTREAD);
//      cmd[0] = SMMU_V3_CMD_TLBI_NH_ASID |
//          SMMU_V3_CMD_TLBI_0_ASID(dom->v3.sd_asid);
        cmd[0] = SMMU_V3_CMD_TLBI_EL2_ASID |
            SMMU_V3_CMD_TLBI_0_ASID(dom->v3.sd_asid);
        cmd[1] = 0;
        bus_dmamap_sync(sc->sc_dmat, SMMU_DMA_MAP(sq->sq_sdm), off,
            2 * sizeof(uint64_t), BUS_DMASYNC_PREWRITE);

        /* Let HW know we produced */
        prod = (SMMU_V3_Q_WRP(sq, sq->sq_prod) |
            SMMU_V3_Q_IDX(sq, sq->sq_prod)) + 1;
        sq->sq_prod = SMMU_V3_Q_OVF(sq->sq_prod) |
            SMMU_V3_Q_WRP(sq, prod) | SMMU_V3_Q_IDX(sq, prod);
        membar_sync();
        smmu_v3_write_4(sc, SMMU_V3_CMDQ_PROD, sq->sq_prod);

        smmu_v3_sync(sc);

        mtx_leave(&sc->v3.sc_cmdq_mtx);
}

void
smmu_v3_tlbi_va(struct smmu_domain *dom, vaddr_t va)
{
        struct smmu_softc *sc = dom->sd_sc;
        struct smmu_v3_queue *sq = &sc->v3.sc_cmdq;
        volatile uint64_t *cmd;
        uint32_t prod;
        bus_size_t off;

        mtx_enter(&sc->v3.sc_cmdq_mtx);

        /* TODO: Handle this more properly. */
        sq->sq_cons = smmu_v3_read_4(sc, SMMU_V3_CMDQ_CONS);
        if (SMMU_V3_Q_IDX(sq, sq->sq_cons) == SMMU_V3_Q_IDX(sq, sq->sq_prod) &&
            SMMU_V3_Q_WRP(sq, sq->sq_cons) != SMMU_V3_Q_WRP(sq, sq->sq_prod)) {
                printf("%s: CMDQ ran out of space\n", sc->sc_dev.dv_xname);
                mtx_leave(&sc->v3.sc_cmdq_mtx);
                return;
        }

        off = SMMU_V3_Q_IDX(sq, sq->sq_prod) * 2 * sizeof(uint64_t);
        cmd = SMMU_DMA_KVA(sq->sq_sdm) + off;
        bus_dmamap_sync(sc->sc_dmat, SMMU_DMA_MAP(sq->sq_sdm), off,
            2 * sizeof(uint64_t), BUS_DMASYNC_POSTREAD);
//      cmd[0] = SMMU_V3_CMD_TLBI_NH_VA | SMMU_V3_CMD_TLBI_0_VMID(0) |
//          SMMU_V3_CMD_TLBI_0_ASID(dom->v3.sd_asid);
//      cmd[1] = va | SMMU_V3_CMD_TLBI_1_LEAF;
        cmd[0] = SMMU_V3_CMD_TLBI_EL2_VA | SMMU_V3_CMD_TLBI_0_VMID(0) |
            SMMU_V3_CMD_TLBI_0_ASID(dom->v3.sd_asid);
        cmd[1] = va | SMMU_V3_CMD_TLBI_1_LEAF;
        bus_dmamap_sync(sc->sc_dmat, SMMU_DMA_MAP(sq->sq_sdm), off,
            2 * sizeof(uint64_t), BUS_DMASYNC_PREWRITE);

        /* Let HW know we produced */
        prod = (SMMU_V3_Q_WRP(sq, sq->sq_prod) |
            SMMU_V3_Q_IDX(sq, sq->sq_prod)) + 1;
        sq->sq_prod = SMMU_V3_Q_OVF(sq->sq_prod) |
            SMMU_V3_Q_WRP(sq, prod) | SMMU_V3_Q_IDX(sq, prod);
        membar_sync();
        smmu_v3_write_4(sc, SMMU_V3_CMDQ_PROD, sq->sq_prod);

        /* callee is responsible for smmu_v3_tlb_sync_context() */

        mtx_leave(&sc->v3.sc_cmdq_mtx);
}

void
smmu_v3_tlb_sync_context(struct smmu_domain *dom)
{
        struct smmu_softc *sc = dom->sd_sc;

        mtx_enter(&sc->v3.sc_cmdq_mtx);
        smmu_v3_sync(sc);
        mtx_leave(&sc->v3.sc_cmdq_mtx);
}