1 files changed, 52 insertions, 7 deletions

diff --git a/os/ex/Micron/n25q128.c b/os/ex/Micron/n25q128.c
index 079a565a7..03ebd6cc2 100644
--- a/os/ex/Micron/n25q128.c
+++ b/os/ex/Micron/n25q128.c
@@ -93,6 +93,13 @@ static void spi_send_cmd_addr(N25Q128Driver *devp,
   spiSend(devp->config->spip, 4, buf);
 }
 
+static void spi_send_cmd(N25Q128Driver *devp, uint8_t cmd) {
+  uint8_t buf[1];
+
+  buf[0] = cmd;
+  spiSend(devp->config->spip, 1, buf);
+}
+
 static const flash_descriptor_t *get_attributes(void *instance) {
   N25Q128Driver *devp = (N25Q128Driver *)instance;
   SPIDriver *spip = devp->config->spip;
@@ -149,6 +156,7 @@ static flash_error_t program(void *instance, flash_address_t addr,
                              const uint8_t *pp, size_t n) {
   N25Q128Driver *devp = (N25Q128Driver *)instance;
   SPIDriver *spip = devp->config->spip;
+  flash_error_t err;
 
   osalDbgAssert(devp->state == FLASH_READY, "invalid state");
 
@@ -158,31 +166,69 @@ static flash_error_t program(void *instance, flash_address_t addr,
 #endif
   devp->state = FLASH_ACTIVE;
 
-  while (n > 0) {
+  while (n > 0U) {
+    uint8_t sts;
+
+    /* Data size that can be written in a single program page operation.*/
     size_t chunk = (size_t)(((addr | PAGE_MASK) + 1U) - addr);
     if (chunk > n) {
       chunk = n;
     }
 
+    /* Enabling write operation.*/
     spiSelect(spip);
+    spi_send_cmd(devp, N25Q128_CMD_WRITE_ENABLE);
+    spiUnselect(spip);
+    (void) spiPolledExchange(spip, 0xFF);   /* One frame delay.*/
 
+    /* Page program command.*/
+    spiSelect(spip);
     spi_send_cmd_addr(devp, N25Q128_CMD_PAGE_PROGRAM, addr);
     spiSend(spip, chunk, pp);
-
     spiUnselect(spip);
+    (void) spiPolledExchange(spip, 0xFF);   /* One frame delay.*/
 
+    /* Operation end waiting.*/
+    do {
+#if N25Q128_NICE_WAITING == TRUE
+      osalThreadSleepMilliseconds(1);
+#endif
+      /* Read status command.*/
+      spiSelect(spip);
+      spi_send_cmd(devp, N25Q128_CMD_READ_STATUS_REGISTER);
+      spiReceive(spip, 1, &sts);
+      spiUnselect(spip);
+    } while ((sts & N25Q128_STS_BUSY) != 0U);
+
+    /* Checking for errors.*/
+    if ((sts & N25Q128_STS_ALL_ERRORS) != 0U) {
+      /* Clearing status register.*/
+      (void) spiPolledExchange(spip, 0xFF);   /* One frame delay.*/
+      spiSelect(spip);
+      spi_send_cmd(devp, N25Q128_CMD_CLEAR_FLAG_STATUS_REGISTER);
+      spiUnselect(spip);
+
+      /* Program operation failed.*/
+      err = FLASH_PROGRAM_FAILURE;
+      goto exit_error;
+    }
+
+    /* Next page.*/
     addr += chunk;
     pp   += chunk;
     n    -= chunk;
   }
 
-  devp->state = FLASH_READY;
+  /* Program operation succeeded.*/
+  err = FLASH_NO_ERROR;
 
+  /* Common exit path for this function.*/
+exit_error:
+  devp->state = FLASH_READY;
 #if N25Q128_SHARED_SPI == TRUE
   spiReleaseBus(spip);
 #endif
-
-  return FLASH_NO_ERROR;
+  return err;
 }
 
 static flash_error_t read(void *instance, flash_address_t addr,
@@ -198,17 +244,16 @@ static flash_error_t read(void *instance, flash_address_t addr,
 #endif
   devp->state = FLASH_ACTIVE;
 
+  /* Read command.*/
   spiSelect(spip);
   spi_send_cmd_addr(devp, N25Q128_CMD_READ, addr);
   spiReceive(spip, n, rp);
   spiUnselect(spip);
 
   devp->state = FLASH_READY;
-
 #if N25Q128_SHARED_SPI == TRUE
   spiReleaseBus(spip);
 #endif
-
   return FLASH_NO_ERROR;
 }