1 files changed, 16 insertions, 3 deletions

diff --git a/drivers/mtd/spi-nor/cadence-quadspi.c b/drivers/mtd/spi-nor/cadence-quadspi.c
index d846428ef038..04cedd3a2bf6 100644
--- a/drivers/mtd/spi-nor/cadence-quadspi.c
+++ b/drivers/mtd/spi-nor/cadence-quadspi.c
@@ -644,9 +644,23 @@ static int cqspi_indirect_write_execute(struct spi_nor *nor, loff_t to_addr,
                 ndelay(cqspi->wr_delay);
 
         while (remaining > 0) {
+                size_t write_words, mod_bytes;
+
                 write_bytes = remaining > page_size ? page_size : remaining;
-                iowrite32_rep(cqspi->ahb_base, txbuf,
-                              DIV_ROUND_UP(write_bytes, 4));
+                write_words = write_bytes / 4;
+                mod_bytes = write_bytes % 4;
+                /* Write 4 bytes at a time then single bytes. */
+                if (write_words) {
+                        iowrite32_rep(cqspi->ahb_base, txbuf, write_words);
+                        txbuf += (write_words * 4);
+                }
+                if (mod_bytes) {
+                        unsigned int temp = 0xFFFFFFFF;
+
+                        memcpy(&temp, txbuf, mod_bytes);
+                        iowrite32(temp, cqspi->ahb_base);
+                        txbuf += mod_bytes;
+                }
 
                 if (!wait_for_completion_timeout(&cqspi->transfer_complete,
                                         msecs_to_jiffies(CQSPI_TIMEOUT_MS))) {
@@ -655,7 +669,6 @@ static int cqspi_indirect_write_execute(struct spi_nor *nor, loff_t to_addr,
                         goto failwr;
                 }
 
-                txbuf += write_bytes;
                 remaining -= write_bytes;
 
                 if (remaining > 0)