diff --git a/sys/dev/firmware/arm/scmi.h b/sys/dev/firmware/arm/scmi.h
--- a/sys/dev/firmware/arm/scmi.h
+++ b/sys/dev/firmware/arm/scmi.h
@@ -34,12 +34,10 @@
 
 #include "scmi_if.h"
 
-#define	SCMI_LOCK(sc)		mtx_lock(&(sc)->mtx)
-#define	SCMI_UNLOCK(sc)		mtx_unlock(&(sc)->mtx)
-#define	SCMI_ASSERT_LOCKED(sc)	mtx_assert(&(sc)->mtx, MA_OWNED)
 
 #define dprintf(fmt, ...)
 
+#define SCMI_MAX_MSG		32
 #define SCMI_MSG_HDR_SIZE	(sizeof(uint32_t))
 
 enum scmi_chan {
@@ -48,22 +46,38 @@
 	SCMI_CHAN_MAX
 };
 
+struct scmi_transport_desc {
+	bool no_completion_irq;
+	unsigned int reply_timo_ms;
+};
+
+struct scmi_transport;
+
 struct scmi_softc {
-	struct simplebus_softc	simplebus_sc;
-	device_t		dev;
-	struct mtx		mtx;
+	struct simplebus_softc		simplebus_sc;
+	device_t			dev;
+	struct mtx			mtx;
+	struct scmi_transport_desc	trs_desc;
+	struct scmi_transport		*trs;
 };
 
 struct scmi_req {
-	int protocol_id;
-	int message_id;
-	uint32_t msg_header;
-	const void *in_buf;
-	uint32_t in_size;
-	void *out_buf;
-	uint32_t out_size;
+	bool		use_polling;
+	bool		done;
+	LIST_ENTRY(scmi_req)	next;
+	int		protocol_id;
+	int		message_id;
+	int		token;
+	uint32_t	msg_header;
+	const void	*in_buf;
+	uint32_t	in_size;
+	void		*out_buf;
+	uint32_t	out_size;
 };
 
+int scmi_request(device_t dev, struct scmi_req *req);
+void scmi_rx_irq_callback(device_t dev, void *chan, uint32_t hdr);
+
 DECLARE_CLASS(scmi_driver);
 
 int scmi_attach(device_t dev);
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
@@ -31,12 +31,17 @@
 
 #include <sys/param.h>
 #include <sys/systm.h>
+#include <sys/_bitset.h>
+#include <sys/bitset.h>
 #include <sys/bus.h>
 #include <sys/cpu.h>
+#include <sys/endian.h>
 #include <sys/kernel.h>
 #include <sys/lock.h>
+#include <sys/malloc.h>
 #include <sys/module.h>
 #include <sys/mutex.h>
+#include <sys/queue.h>
 
 #include <dev/clk/clk.h>
 #include <dev/fdt/simplebus.h>
@@ -46,6 +51,8 @@
 #include "scmi.h"
 #include "scmi_protocols.h"
 
+#define SCMI_MAX_TOKEN		1024
+
 #define	SCMI_HDR_TOKEN_S		18
 #define SCMI_HDR_TOKEN_BF		(0x3fff)
 #define	SCMI_HDR_TOKEN_M		(SCMI_HDR_TOKEN_BF << SCMI_HDR_TOKEN_S)
@@ -69,42 +76,43 @@
 #define SCMI_MSG_TYPE_DRESP	2
 #define SCMI_MSG_TYPE_NOTIF	3
 
-static int
-scmi_request_locked(struct scmi_softc *sc, struct scmi_req *req)
-{
-	int ret;
-
-	SCMI_ASSERT_LOCKED(sc);
-
-	req->msg_header = req->message_id << SCMI_HDR_MESSAGE_ID_S;
-	/* TODO: Allocate a token */
-	req->msg_header |= SCMI_MSG_TYPE_CMD << SCMI_HDR_MESSAGE_TYPE_S;
-	req->msg_header |= req->protocol_id << SCMI_HDR_PROTOCOL_ID_S;
-
-	ret = SCMI_XFER_MSG(sc->dev, req);
-	if (ret == 0)
-		ret = SCMI_COLLECT_REPLY(sc->dev, req);
+#define SCMI_MSG_TYPE_CHECK(_h, _t)					\
+    ((((_h) & SCMI_HDR_MESSAGE_TYPE_M) >> SCMI_HDR_MESSAGE_TYPE_S) == (_t))
 
-	if (ret == 0  || ret != EBUSY)
-		SCMI_TX_COMPLETE(sc->dev, NULL);
+#define SCMI_IS_MSG_TYPE_NOTIF(h)					\
+    SCMI_MSG_TYPE_CHECK((h), SCMI_MSG_TYPE_NOTIF)
+#define SCMI_IS_MSG_TYPE_DRESP(h)					\
+    SCMI_MSG_TYPE_CHECK((h), SCMI_MSG_TYPE_DRESP)
 
-	return (ret);
-}
+#define SCMI_MSG_TOKEN(_hdr)		\
+    (((_hdr) & SCMI_HDR_TOKEN_M) >> SCMI_HDR_TOKEN_S)
 
-int
-scmi_request(device_t dev, struct scmi_req *req)
-{
-	struct scmi_softc *sc;
-	int error;
+BITSET_DEFINE(_scmi_tokens, SCMI_MAX_TOKEN);
+LIST_HEAD(inflight_head, scmi_req);
+#define	REQHASH(_sc, _tk)		\
+    (&((_sc)->trs->inflight_ht[(_tk) & (_sc)->trs->inflight_mask]))
 
-	sc = device_get_softc(dev);
+struct scmi_transport {
+	unsigned long		next_id;
+	struct _scmi_tokens	avail_tokens;
+	struct inflight_head	*inflight_ht;
+	unsigned long		inflight_mask;
+	struct mtx		mtx;
+};
 
-	SCMI_LOCK(sc);
-	error = scmi_request_locked(sc, req);
-	SCMI_UNLOCK(sc);
+static int		scmi_transport_init(struct scmi_softc *);
+static void		scmi_transport_cleanup(struct scmi_softc *);
+static int		scmi_token_pick(struct scmi_softc *);
+static void		scmi_token_release_unlocked(struct scmi_softc *, int);
+static int		scmi_req_track_inflight(struct scmi_softc *,
+			    struct scmi_req *);
+static int		scmi_req_drop_inflight(struct scmi_softc *,
+			    struct scmi_req *);
+static struct scmi_req *scmi_req_lookup_inflight(struct scmi_softc *, uint32_t);
 
-	return (error);
-}
+static int		scmi_wait_for_response(struct scmi_softc *,
+    struct scmi_req *);
+static void		scmi_process_response(struct scmi_softc *, uint32_t);
 
 int
 scmi_attach(device_t dev)
@@ -120,14 +128,15 @@
 	if (node == -1)
 		return (ENXIO);
 
-	mtx_init(&sc->mtx, device_get_nameunit(dev), "SCMI", MTX_DEF);
-
 	simplebus_init(dev, node);
 
-	error = SCMI_TRANSPORT_INIT(dev);
+	error = scmi_transport_init(sc);
 	if (error != 0)
 		return (error);
 
+	device_printf(dev, "Transport reply timeout initialized to %dms\n",
+	    sc->trs_desc.reply_timo_ms);
+
 	/*
 	 * Allow devices to identify.
 	 */
@@ -147,8 +156,11 @@
 static int
 scmi_detach(device_t dev)
 {
+	struct scmi_softc *sc;
+
+	sc = device_get_softc(dev);
+	scmi_transport_cleanup(sc);
 
-	SCMI_TRANSPORT_CLEANUP(dev);
 	return (0);
 }
 
@@ -164,3 +176,221 @@
 
 DRIVER_MODULE(scmi, simplebus, scmi_driver, 0, 0);
 MODULE_VERSION(scmi, 1);
+
+static int
+scmi_transport_init(struct scmi_softc *sc)
+{
+	struct scmi_transport *trs;
+	int ret;
+
+	trs = malloc(sizeof(*trs), M_DEVBUF, M_ZERO | M_WAITOK);
+
+	BIT_FILL(SCMI_MAX_TOKEN, &trs->avail_tokens);
+	mtx_init(&trs->mtx, "tokens", "SCMI", MTX_SPIN);
+
+	trs->inflight_ht = hashinit(SCMI_MAX_MSG, M_DEVBUF,
+	    &trs->inflight_mask);
+
+	sc->trs = trs;
+	ret = SCMI_TRANSPORT_INIT(sc->dev);
+	if (ret != 0) {
+		free(trs, M_DEVBUF);
+		return (ret);
+	}
+
+	return (0);
+}
+static void
+scmi_transport_cleanup(struct scmi_softc *sc)
+{
+
+	SCMI_TRANSPORT_CLEANUP(sc->dev);
+	mtx_destroy(&sc->trs->mtx);
+	hashdestroy(sc->trs->inflight_ht, M_DEVBUF, sc->trs->inflight_mask);
+	free(sc->trs, M_DEVBUF);
+}
+
+static int
+scmi_token_pick(struct scmi_softc *sc)
+{
+	unsigned long next_msg_id, token;
+
+	mtx_lock_spin(&sc->trs->mtx);
+	/*
+	 * next_id is a monotonically increasing unsigned long that can be used
+	 * for tracing purposes; next_msg_id is a 10-bit sequence number derived
+	 * from it.
+	 */
+	next_msg_id = sc->trs->next_id++ & SCMI_HDR_TOKEN_BF;
+	token = BIT_FFS_AT(SCMI_MAX_TOKEN, &sc->trs->avail_tokens, next_msg_id);
+	/* TODO Account for wrap-arounds and holes */
+	if (token != 0)
+		BIT_CLR(SCMI_MAX_TOKEN, token - 1, &sc->trs->avail_tokens);
+	mtx_unlock_spin(&sc->trs->mtx);
+
+	/*
+	 * BIT_FFS_AT returns 1-indexed values, so 0 means failure to find a
+	 * free slot: all possible SCMI messages are in-flight using all of the
+	 * SCMI_MAX_TOKEN sequence numbers.
+	 */
+	if (!token)
+		return (-EBUSY);
+
+	return ((int)(token - 1));
+}
+
+static void
+scmi_token_release_unlocked(struct scmi_softc *sc, int token)
+{
+
+	BIT_SET(SCMI_MAX_TOKEN, token, &sc->trs->avail_tokens);
+}
+
+static int
+scmi_finalize_req(struct scmi_softc *sc, struct scmi_req *req)
+{
+	uint32_t header = 0;
+
+	req->token = scmi_token_pick(sc);
+	if (req->token < 0)
+		return (EBUSY);
+
+	header = req->message_id;
+	header |= SCMI_MSG_TYPE_CMD << SCMI_HDR_MESSAGE_TYPE_S;
+	header |= req->protocol_id << SCMI_HDR_PROTOCOL_ID_S;
+	header |= req->token << SCMI_HDR_TOKEN_S;
+
+	req->msg_header = htole32(header);
+
+	return (0);
+}
+
+static int
+scmi_req_track_inflight(struct scmi_softc *sc, struct scmi_req *req)
+{
+	int error;
+
+	/* build hdr, pick token */
+	error = scmi_finalize_req(sc, req);
+	if (error != 0)
+		return (error);
+
+	/* TODO Review/simplify locking around inflight ?*/
+	mtx_lock_spin(&sc->trs->mtx);
+	LIST_INSERT_HEAD(REQHASH(sc, req->token), req, next);
+	mtx_unlock_spin(&sc->trs->mtx);
+
+	return (0);
+}
+
+static int
+scmi_req_drop_inflight(struct scmi_softc *sc, struct scmi_req *req)
+{
+
+	mtx_lock_spin(&sc->trs->mtx);
+	LIST_REMOVE(req, next);
+	scmi_token_release_unlocked(sc, req->token);
+	mtx_unlock_spin(&sc->trs->mtx);
+
+	return (0);
+}
+
+static struct scmi_req *
+scmi_req_lookup_inflight(struct scmi_softc *sc, uint32_t hdr)
+{
+	struct scmi_req *req = NULL;
+	unsigned int token;
+
+	token = SCMI_MSG_TOKEN(hdr);
+	mtx_lock_spin(&sc->trs->mtx);
+	LIST_FOREACH(req, REQHASH(sc, token), next) {
+		if (req->token == token)
+			break;
+	}
+	mtx_unlock_spin(&sc->trs->mtx);
+
+	return (req);
+}
+
+static void
+scmi_process_response(struct scmi_softc *sc, uint32_t hdr)
+{
+	struct scmi_req *req;
+
+	req = scmi_req_lookup_inflight(sc, hdr);
+	if (req == NULL) {
+		device_printf(sc->dev,
+		    "Unexpected reply with header |%X| - token: 0x%X Drop.\n",
+		    hdr, SCMI_MSG_TOKEN(hdr));
+		return;
+	}
+
+	req->done = true;
+	wakeup(req);
+}
+
+void
+scmi_rx_irq_callback(device_t dev, void *chan, uint32_t hdr)
+{
+	struct scmi_softc *sc;
+
+	sc = device_get_softc(dev);
+
+	if (SCMI_IS_MSG_TYPE_NOTIF(hdr) || SCMI_IS_MSG_TYPE_DRESP(hdr)) {
+		device_printf(dev, "DRESP/NOTIF unsupported. Drop.\n");
+		SCMI_CLEAR_CHANNEL(dev, chan);
+		return;
+	}
+
+	scmi_process_response(sc, hdr);
+}
+
+static int
+scmi_wait_for_response(struct scmi_softc *sc, struct scmi_req *req)
+{
+	int ret;
+
+	if (req->use_polling) {
+		ret = SCMI_POLL_MSG(sc->dev, req, sc->trs_desc.reply_timo_ms);
+	} else {
+		ret = tsleep(req, 0, "scmi_wait4",
+		    (sc->trs_desc.reply_timo_ms * hz) / 1000);
+		/* Check for lost wakeups since there is no associated lock */
+		if (ret != 0 && req->done)
+			ret = 0;
+	}
+
+	if (ret == 0)
+		SCMI_COLLECT_REPLY(sc->dev, req);
+	else
+		device_printf(sc->dev,
+		    "Request for token 0x%X timed-out.\n", req->token);
+
+	SCMI_TX_COMPLETE(sc->dev, NULL);
+
+	return (ret);
+}
+
+int
+scmi_request(device_t dev, struct scmi_req *req)
+{
+	struct scmi_softc *sc;
+	int error;
+
+	sc = device_get_softc(dev);
+
+	req->use_polling = cold || sc->trs_desc.no_completion_irq;
+
+	/* Set inflight and send using transport specific method - refc-2 */
+	error = scmi_req_track_inflight(sc, req);
+	if (error != 0)
+		return (error);
+
+	error = SCMI_XFER_MSG(sc->dev, req);
+	if (error == 0)
+		error = scmi_wait_for_response(sc, req);
+
+	scmi_req_drop_inflight(sc, req);
+
+	return (error);
+}
diff --git a/sys/dev/firmware/arm/scmi_if.m b/sys/dev/firmware/arm/scmi_if.m
--- a/sys/dev/firmware/arm/scmi_if.m
+++ b/sys/dev/firmware/arm/scmi_if.m
@@ -44,6 +44,12 @@
 	struct scmi_req *req;
 };
 
+METHOD int poll_msg {
+	device_t dev;
+	struct scmi_req *req;
+	unsigned int tmo;
+};
+
 METHOD int collect_reply {
 	device_t dev;
 	struct scmi_req *req;
@@ -53,3 +59,8 @@
 	device_t dev;
 	void *chan;
 };
+
+METHOD void clear_channel {
+	device_t dev;
+	void *chan;
+};
diff --git a/sys/dev/firmware/arm/scmi_mailbox.c b/sys/dev/firmware/arm/scmi_mailbox.c
--- a/sys/dev/firmware/arm/scmi_mailbox.c
+++ b/sys/dev/firmware/arm/scmi_mailbox.c
@@ -38,7 +38,6 @@
 #include <sys/kernel.h>
 #include <sys/lock.h>
 #include <sys/module.h>
-#include <sys/mutex.h>
 
 #include <dev/clk/clk.h>
 #include <dev/fdt/simplebus.h>
@@ -51,34 +50,36 @@
 #include "scmi_protocols.h"
 #include "scmi_shmem.h"
 
+#define SCMI_MBOX_POLL_INTERVAL_MS	3
+
 struct scmi_mailbox_softc {
 	struct scmi_softc	base;
 	device_t		a2p_dev;
 	struct arm_doorbell	*db;
-	int			req_done;
 };
 
 static int	scmi_mailbox_transport_init(device_t);
 static void	scmi_mailbox_transport_cleanup(device_t);
 static int	scmi_mailbox_xfer_msg(device_t, struct scmi_req *);
+static int	scmi_mailbox_poll_msg(device_t, struct scmi_req *,
+    unsigned int);
 static int	scmi_mailbox_collect_reply(device_t, struct scmi_req *);
 static void	scmi_mailbox_tx_complete(device_t, void *);
 
 static int	scmi_mailbox_probe(device_t);
 
 static void
-scmi_mailbox_callback(void *arg)
+scmi_mailbox_a2p_callback(void *arg)
 {
 	struct scmi_mailbox_softc *sc;
+	uint32_t msg_header;
+	int ret;
 
 	sc = arg;
 
-	dprintf("%s sc %p\n", __func__, sc);
-
-	SCMI_LOCK(&sc->base);
-	sc->req_done = 1;
-	wakeup(sc);
-	SCMI_UNLOCK(&sc->base);
+	ret = scmi_shmem_read_msg_header(sc->a2p_dev, &msg_header);
+	if (ret == 0)
+		scmi_rx_irq_callback(sc->base.dev, sc->a2p_dev, msg_header);
 }
 
 static int
@@ -110,7 +111,9 @@
 		return (ENXIO);
 	}
 
-	arm_doorbell_set_handler(sc->db, scmi_mailbox_callback, sc);
+	sc->base.trs_desc.reply_timo_ms = 30;
+
+	arm_doorbell_set_handler(sc->db, scmi_mailbox_a2p_callback, sc);
 
 	return (0);
 }
@@ -129,42 +132,37 @@
 scmi_mailbox_xfer_msg(device_t dev, struct scmi_req *req)
 {
 	struct scmi_mailbox_softc *sc;
-	int ret, timeout;
+	int ret;
 
 	sc = device_get_softc(dev);
-	SCMI_ASSERT_LOCKED(&sc->base);
-
-	sc->req_done = 0;
 
-	ret = scmi_shmem_prepare_msg(sc->a2p_dev, req, cold);
+	ret = scmi_shmem_prepare_msg(sc->a2p_dev, req, req->use_polling);
 	if (ret != 0)
 		return (ret);
 
 	/* Interrupt SCP firmware. */
 	arm_doorbell_set(sc->db);
 
-	timeout = 200;
-
 	dprintf("%s: request\n", __func__);
 
+	return (0);
+}
+
+static int
+scmi_mailbox_poll_msg(device_t dev, struct scmi_req *req, unsigned int tmo_ms)
+{
+	struct scmi_mailbox_softc *sc;
+	unsigned int tmo_loops = tmo_ms / SCMI_MBOX_POLL_INTERVAL_MS;
+
+	sc = device_get_softc(dev);
+
 	do {
-		if (cold) {
-			if (scmi_shmem_poll_msg(sc->a2p_dev))
-				break;
-			DELAY(10000);
-		} else {
-			msleep(sc, &sc->base.mtx, 0, "scmi", hz / 10);
-			if (sc->req_done)
-				break;
-		}
-	} while (timeout--);
-
-	if (timeout <= 0)
-		return (ETIMEDOUT);
-
-	dprintf("%s: got reply, timeout %d\n", __func__, timeout);
+		if (scmi_shmem_poll_msg(sc->a2p_dev, req->msg_header))
+			break;
+		DELAY(SCMI_MBOX_POLL_INTERVAL_MS * 1000);
+	} while (tmo_loops--);
 
-	return (0);
+	return (tmo_loops ? 0 : 1);
 }
 
 static int
@@ -197,6 +195,13 @@
 	scmi_shmem_tx_complete(sc->a2p_dev);
 }
 
+static void
+scmi_mailbox_clear_channel(device_t dev, void *chan)
+{
+	/* Only P2A channel can be cleared forcibly by agent */
+	scmi_shmem_clear_channel(chan);
+}
+
 static int
 scmi_mailbox_probe(device_t dev)
 {
@@ -219,8 +224,10 @@
 	DEVMETHOD(scmi_transport_init,		scmi_mailbox_transport_init),
 	DEVMETHOD(scmi_transport_cleanup,	scmi_mailbox_transport_cleanup),
 	DEVMETHOD(scmi_xfer_msg,		scmi_mailbox_xfer_msg),
+	DEVMETHOD(scmi_poll_msg,		scmi_mailbox_poll_msg),
 	DEVMETHOD(scmi_collect_reply,		scmi_mailbox_collect_reply),
 	DEVMETHOD(scmi_tx_complete,		scmi_mailbox_tx_complete),
+	DEVMETHOD(scmi_clear_channel,		scmi_mailbox_clear_channel),
 
 	DEVMETHOD_END
 };
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
@@ -65,9 +65,10 @@
 device_t	scmi_shmem_get(device_t sdev, phandle_t node, int index);
 int		scmi_shmem_prepare_msg(device_t dev, struct scmi_req *req,
     bool polling);
-bool scmi_shmem_poll_msg(device_t);
+bool scmi_shmem_poll_msg(device_t, uint32_t msg_header);
 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);
+void scmi_shmem_clear_channel(device_t dev);
 
 #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
@@ -233,6 +233,19 @@
 	return (0);
 }
 
+void
+scmi_shmem_clear_channel(device_t dev)
+{
+	uint32_t channel_status = 0;
+
+	if (dev == NULL)
+		return;
+
+	channel_status |= SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE;
+	scmi_shmem_write(dev, SMT_OFFSET_CHAN_STATUS, &channel_status,
+	    SMT_SIZE_CHAN_STATUS);
+}
+
 int
 scmi_shmem_read_msg_header(device_t dev, uint32_t *msg_header)
 {
@@ -283,9 +296,15 @@
 	scmi_shmem_release_channel(sc);
 }
 
-bool scmi_shmem_poll_msg(device_t dev)
+bool scmi_shmem_poll_msg(device_t dev, uint32_t msg_header)
 {
-	uint32_t status;
+	uint32_t status, header;
+
+	scmi_shmem_read(dev, SMT_OFFSET_MSG_HEADER, &header,
+	    SMT_SIZE_MSG_HEADER);
+	/* Bail out if it is NOT what we were polling for. */
+	if (le32toh(header) != msg_header)
+		return (false);
 
 	scmi_shmem_read(dev, SMT_OFFSET_CHAN_STATUS, &status,
 	    SMT_SIZE_CHAN_STATUS);
diff --git a/sys/dev/firmware/arm/scmi_smc.c b/sys/dev/firmware/arm/scmi_smc.c
--- a/sys/dev/firmware/arm/scmi_smc.c
+++ b/sys/dev/firmware/arm/scmi_smc.c
@@ -58,11 +58,11 @@
 
 static int	scmi_smc_transport_init(device_t);
 static int	scmi_smc_xfer_msg(device_t, struct scmi_req *);
+static int	scmi_smc_poll_msg(device_t, struct scmi_req *, unsigned int);
 static int	scmi_smc_collect_reply(device_t, struct scmi_req *);
 static void	scmi_smc_tx_complete(device_t, void *);
 
 static int	scmi_smc_probe(device_t);
-static int	scmi_smc_attach(device_t);
 
 static int
 scmi_smc_transport_init(device_t dev)
@@ -89,6 +89,9 @@
 		return (ENXIO);
 	}
 
+	sc->base.trs_desc.no_completion_irq = true;
+	sc->base.trs_desc.reply_timo_ms = 30;
+
 	return (0);
 }
 
@@ -99,7 +102,6 @@
 	int ret;
 
 	sc = device_get_softc(dev);
-	SCMI_ASSERT_LOCKED(&sc->base);
 
 	ret = scmi_shmem_prepare_msg(sc->a2p_dev, req, cold);
 	if (ret != 0)
@@ -110,6 +112,26 @@
 	return (0);
 }
 
+static int
+scmi_smc_poll_msg(device_t dev, struct scmi_req *req, unsigned int tmo)
+{
+	struct scmi_smc_softc *sc;
+	uint32_t msg_header;
+	int ret;
+
+	sc = device_get_softc(dev);
+
+	/*
+	 * Nothing to poll since commands are completed as soon as smc
+	 * returns ... but did we get back what we were poling for ?
+	 */
+	ret = scmi_shmem_read_msg_header(sc->a2p_dev, &msg_header);
+	if (ret != 0 || msg_header != req->msg_header)
+		return (1);
+
+	return (0);
+}
+
 static int
 scmi_smc_collect_reply(device_t dev, struct scmi_req *req)
 {
@@ -140,40 +162,18 @@
 	if (!ofw_bus_status_okay(dev))
 		return (ENXIO);
 
-	device_set_desc(dev, "ARM SCMI SCM interface driver");
+	device_set_desc(dev, "ARM SCMI SMC Transport driver");
 
 	return (BUS_PROBE_DEFAULT);
 }
 
-static int
-scmi_smc_attach(device_t dev)
-{
-	struct scmi_smc_softc *sc;
-	phandle_t node;
-	ssize_t len;
-
-	sc = device_get_softc(dev);
-
-	node = ofw_bus_get_node(dev);
-	len = OF_getencprop(node, "arm,smc-id", &sc->smc_id,
-	    sizeof(sc->smc_id));
-	if (len <= 0) {
-		device_printf(dev, "No SMC ID found\n");
-		return (EINVAL);
-	}
-
-	device_printf(dev, "smc id %x\n", sc->smc_id);
-
-	return (scmi_attach(dev));
-}
-
 static device_method_t scmi_smc_methods[] = {
 	DEVMETHOD(device_probe,		scmi_smc_probe),
-	DEVMETHOD(device_attach,	scmi_smc_attach),
 
 	/* SCMI interface */
 	DEVMETHOD(scmi_transport_init,		scmi_smc_transport_init),
 	DEVMETHOD(scmi_xfer_msg,		scmi_smc_xfer_msg),
+	DEVMETHOD(scmi_poll_msg,		scmi_smc_poll_msg),
 	DEVMETHOD(scmi_collect_reply,		scmi_smc_collect_reply),
 	DEVMETHOD(scmi_tx_complete,		scmi_smc_tx_complete),