diff --git a/sys/dev/firmware/arm/scmi.c b/sys/dev/firmware/arm/scmi.c --- a/sys/dev/firmware/arm/scmi.c +++ b/sys/dev/firmware/arm/scmi.c @@ -91,18 +91,25 @@ ret = SCMI_XFER_MSG(sc->dev); if (ret != 0) - return (ret); + goto out; /* Read header. */ ret = scmi_shmem_read_msg_header(sc->a2p_dev, &reply_header); if (ret != 0) - return (ret); + goto out; + + if (reply_header != req->msg_header) { + ret = EPROTO; + goto out; + } + + ret = scmi_shmem_read_msg_payload(sc->a2p_dev, req->out_buf, + req->out_size); - if (reply_header != req->msg_header) - return (EPROTO); +out: + scmi_shmem_tx_complete(sc->a2p_dev); - return (scmi_shmem_read_msg_payload(sc->a2p_dev, req->out_buf, - req->out_size)); + return (ret); } int diff --git a/sys/dev/firmware/arm/scmi_shmem.h b/sys/dev/firmware/arm/scmi_shmem.h --- a/sys/dev/firmware/arm/scmi_shmem.h +++ b/sys/dev/firmware/arm/scmi_shmem.h @@ -68,5 +68,6 @@ bool scmi_shmem_poll_msg(device_t); int scmi_shmem_read_msg_header(device_t dev, uint32_t *msg_header); int scmi_shmem_read_msg_payload(device_t dev, uint8_t *buf, uint32_t buf_len); +void scmi_shmem_tx_complete(device_t); #endif /* !_ARM64_SCMI_SCMI_SHMEM_H_ */ diff --git a/sys/dev/firmware/arm/scmi_shmem.c b/sys/dev/firmware/arm/scmi_shmem.c --- a/sys/dev/firmware/arm/scmi_shmem.c +++ b/sys/dev/firmware/arm/scmi_shmem.c @@ -36,6 +36,8 @@ #include #include +#include + #include #include #include @@ -45,15 +47,21 @@ #include "scmi_shmem.h" #include "scmi.h" +#define INFLIGHT_NONE 0 +#define INFLIGHT_REQ 1 + struct shmem_softc { device_t dev; device_t parent; int reg; + int inflight; }; static void scmi_shmem_read(device_t, bus_size_t, void *, bus_size_t); static void scmi_shmem_write(device_t, bus_size_t, const void *, bus_size_t); +static void scmi_shmem_acquire_channel(struct shmem_softc *); +static void scmi_shmem_release_channel(struct shmem_softc *); static int shmem_probe(device_t); static int shmem_attach(device_t); @@ -94,6 +102,7 @@ dprintf("%s: reg %x\n", __func__, reg); sc->reg = reg; + atomic_store_rel_int(&sc->inflight, INFLIGHT_NONE); OF_device_register_xref(OF_xref_from_node(node), dev); @@ -167,16 +176,39 @@ return (shmem_dev); } +static void +scmi_shmem_acquire_channel(struct shmem_softc *sc) +{ + + while ((atomic_cmpset_acq_int(&sc->inflight, INFLIGHT_NONE, + INFLIGHT_REQ)) == 0) + DELAY(1000); +} + +static void +scmi_shmem_release_channel(struct shmem_softc *sc) +{ + + atomic_store_rel_int(&sc->inflight, INFLIGHT_NONE); +} + int scmi_shmem_prepare_msg(device_t dev, struct scmi_req *req, bool polling) { + struct shmem_softc *sc; struct scmi_smt_header hdr = {}; uint32_t channel_status; + sc = device_get_softc(dev); + + /* Get exclusive write access to channel */ + scmi_shmem_acquire_channel(sc); + /* Read channel status */ scmi_shmem_read(dev, SMT_OFFSET_CHAN_STATUS, &channel_status, SMT_SIZE_CHAN_STATUS); if ((channel_status & SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE) == 0) { + scmi_shmem_release_channel(sc); device_printf(dev, "Shmem channel busy. Abort !.\n"); return (1); } @@ -242,6 +274,15 @@ return (0); } +void +scmi_shmem_tx_complete(device_t dev) +{ + struct shmem_softc *sc; + + sc = device_get_softc(dev); + scmi_shmem_release_channel(sc); +} + bool scmi_shmem_poll_msg(device_t dev) { uint32_t status;