1 files changed, 41 insertions, 28 deletions

diff --git a/drivers/firmware/scmi/smt.c b/drivers/firmware/scmi/smt.c
index 3253f4211d6..237871559f0 100644
--- a/drivers/firmware/scmi/smt.c
+++ b/drivers/firmware/scmi/smt.c
@@ -25,9 +25,9 @@ static void scmi_smt_enable_intr(struct scmi_smt *smt, bool enable)
 	struct scmi_smt_header *hdr = (void *)smt->buf;
 
 	if (enable)
-		hdr->flags |= SCMI_SHMEM_FLAG_INTR_ENABLED;
+		iowrite32(ioread32(&hdr->flags) | SCMI_SHMEM_FLAG_INTR_ENABLED, &hdr->flags);
 	else
-		hdr->flags &= ~SCMI_SHMEM_FLAG_INTR_ENABLED;
+		iowrite32(ioread32(&hdr->flags) & ~SCMI_SHMEM_FLAG_INTR_ENABLED, &hdr->flags);
 }
 
 /**
@@ -62,11 +62,17 @@ int scmi_dt_get_smt_buffer(struct udevice *dev, struct scmi_smt *smt)
 		scmi_smt_enable_intr(smt, true);
 
 #ifdef CONFIG_ARM
-	if (dcache_status())
-		mmu_set_region_dcache_behaviour(ALIGN_DOWN((uintptr_t)smt->buf, MMU_SECTION_SIZE),
-						ALIGN(smt->size, MMU_SECTION_SIZE),
-						DCACHE_OFF);
+	if (dcache_status()) {
+		u32 align_size;
 
+		if (IS_ENABLED(CONFIG_ARM64))
+			align_size = PAGE_SIZE;
+		else
+			align_size = MMU_SECTION_SIZE;
+
+		mmu_set_region_dcache_behaviour(ALIGN_DOWN((uintptr_t)smt->buf, align_size),
+						ALIGN(smt->size, align_size), DCACHE_OFF);
+	}
 #endif
 
 	return 0;
@@ -85,24 +91,27 @@ int scmi_write_msg_to_smt(struct udevice *dev, struct scmi_smt *smt,
 	    (!msg->out_msg && msg->out_msg_sz))
 		return -EINVAL;
 
-	if (!(hdr->channel_status & SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE)) {
+	if (!(ioread32(&hdr->channel_status) & SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE)) {
 		dev_dbg(dev, "Channel busy\n");
 		return -EBUSY;
 	}
 
 	if (smt->size < (sizeof(*hdr) + msg->in_msg_sz) ||
 	    smt->size < (sizeof(*hdr) + msg->out_msg_sz)) {
-		dev_dbg(dev, "Buffer too small\n");
+		dev_err(dev,
+			"Buffer write too small: mst->size:%zu, in_msg_sz:%zu, out_msg_sz:%zu\n",
+			smt->size, msg->in_msg_sz, msg->out_msg_sz);
 		return -ETOOSMALL;
 	}
 
 	/* Load message in shared memory */
-	hdr->channel_status &= ~SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE;
-	hdr->length = msg->in_msg_sz + sizeof(hdr->msg_header);
-	hdr->msg_header = SMT_HEADER_TOKEN(0) |
-			  SMT_HEADER_MESSAGE_TYPE(0) |
-			  SMT_HEADER_PROTOCOL_ID(msg->protocol_id) |
-			  SMT_HEADER_MESSAGE_ID(msg->message_id);
+	iowrite32(ioread32(&hdr->channel_status) & ~SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE,
+		  &hdr->channel_status);
+	iowrite32(msg->in_msg_sz + sizeof(hdr->msg_header), &hdr->length);
+	iowrite32(SMT_HEADER_TOKEN(0) |
+		  SMT_HEADER_MESSAGE_TYPE(0) |
+		  SMT_HEADER_PROTOCOL_ID(msg->protocol_id) |
+		  SMT_HEADER_MESSAGE_ID(msg->message_id), &hdr->msg_header);
 
 	memcpy_toio(hdr->msg_payload, msg->in_msg, msg->in_msg_sz);
 
@@ -118,23 +127,24 @@ int scmi_read_resp_from_smt(struct udevice *dev, struct scmi_smt *smt,
 {
 	struct scmi_smt_header *hdr = (void *)smt->buf;
 
-	if (!(hdr->channel_status & SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE)) {
+	if (!(ioread32(&hdr->channel_status) & SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE)) {
 		dev_err(dev, "Channel unexpectedly busy\n");
 		return -EBUSY;
 	}
 
-	if (hdr->channel_status & SCMI_SHMEM_CHAN_STAT_CHANNEL_ERROR) {
+	if (ioread32(&hdr->channel_status) & SCMI_SHMEM_CHAN_STAT_CHANNEL_ERROR) {
 		dev_err(dev, "Channel error reported, reset channel\n");
 		return -ECOMM;
 	}
 
-	if (hdr->length > msg->out_msg_sz + sizeof(hdr->msg_header)) {
-		dev_err(dev, "Buffer to small\n");
+	if (ioread32(&hdr->length) > msg->out_msg_sz + sizeof(hdr->msg_header)) {
+		dev_err(dev, "Buffer too small: hdr->length:%u, out_msg_sz:%zu\n",
+			ioread32(&hdr->length), msg->out_msg_sz);
 		return -ETOOSMALL;
 	}
 
 	/* Get the data */
-	msg->out_msg_sz = hdr->length - sizeof(hdr->msg_header);
+	msg->out_msg_sz = ioread32(&hdr->length) - sizeof(hdr->msg_header);
 	memcpy_fromio(msg->out_msg, hdr->msg_payload, msg->out_msg_sz);
 
 	return 0;
@@ -147,7 +157,8 @@ void scmi_clear_smt_channel(struct scmi_smt *smt)
 {
 	struct scmi_smt_header *hdr = (void *)smt->buf;
 
-	hdr->channel_status &= ~SCMI_SHMEM_CHAN_STAT_CHANNEL_ERROR;
+	iowrite32(ioread32(&hdr->channel_status) & ~SCMI_SHMEM_CHAN_STAT_CHANNEL_ERROR,
+		  &hdr->channel_status);
 }
 
 /**
@@ -165,18 +176,19 @@ int scmi_msg_to_smt_msg(struct udevice *dev, struct scmi_smt *smt,
 
 	if (smt->size < (sizeof(*hdr) + msg->in_msg_sz) ||
 	    smt->size < (sizeof(*hdr) + msg->out_msg_sz)) {
-		dev_dbg(dev, "Buffer too small\n");
+		dev_err(dev, "Buffer too small: mst->size:%zu, in_msg_sz:%zu, out_msg_sz:%zu\n",
+			smt->size, msg->in_msg_sz, msg->out_msg_sz);
 		return -ETOOSMALL;
 	}
 
 	*buf_size = msg->in_msg_sz + sizeof(hdr->msg_header);
 
-	hdr->msg_header = SMT_HEADER_TOKEN(0) |
-			  SMT_HEADER_MESSAGE_TYPE(0) |
-			  SMT_HEADER_PROTOCOL_ID(msg->protocol_id) |
-			  SMT_HEADER_MESSAGE_ID(msg->message_id);
+	iowrite32(SMT_HEADER_TOKEN(0) |
+		  SMT_HEADER_MESSAGE_TYPE(0) |
+		  SMT_HEADER_PROTOCOL_ID(msg->protocol_id) |
+		  SMT_HEADER_MESSAGE_ID(msg->message_id), &hdr->msg_header);
 
-	memcpy(hdr->msg_payload, msg->in_msg, msg->in_msg_sz);
+	memcpy_fromio(hdr->msg_payload, msg->in_msg, msg->in_msg_sz);
 
 	return 0;
 }
@@ -191,12 +203,13 @@ int scmi_msg_from_smt_msg(struct udevice *dev, struct scmi_smt *smt,
 	struct scmi_smt_msg_header *hdr = (void *)smt->buf;
 
 	if (buf_size > msg->out_msg_sz + sizeof(hdr->msg_header)) {
-		dev_err(dev, "Buffer to small\n");
+		dev_err(dev, "Buffer too small: buf_size:%zu, out_msg_sz:%zu\n",
+			buf_size, msg->out_msg_sz);
 		return -ETOOSMALL;
 	}
 
 	msg->out_msg_sz = buf_size - sizeof(hdr->msg_header);
-	memcpy(msg->out_msg, hdr->msg_payload, msg->out_msg_sz);
+	memcpy_toio(msg->out_msg, hdr->msg_payload, msg->out_msg_sz);
 
 	return 0;
 }