flash: at91samd: flash write code cleaning
[openocd.git] / src / flash / nor / at91samd.c
index a0bfcc3..0491580 100644 (file)
@@ -255,7 +255,6 @@ struct samd_info {
        int num_pages;
        int sector_size;
 
-       bool manual_wp;
        bool probed;
        struct target *target;
        struct samd_info *next;
@@ -384,9 +383,6 @@ static int samd_probe(struct flash_bank *bank)
 
        samd_protect_check(bank);
 
-       /* By default we do not need to send page write commands */
-       chip->manual_wp = false;
-
        /* Done */
        chip->probed = true;
 
@@ -433,35 +429,16 @@ static bool samd_check_error(struct target *target)
 
 static int samd_issue_nvmctrl_command(struct target *target, uint16_t cmd)
 {
+       int res;
+
        if (target->state != TARGET_HALTED) {
                LOG_ERROR("Target not halted");
                return ERROR_TARGET_NOT_HALTED;
        }
 
-       /* Read current configuration. */
-       uint16_t tmp = 0;
-       int res = target_read_u16(target, SAMD_NVMCTRL + SAMD_NVMCTRL_CTRLB,
-                       &tmp);
-       if (res != ERROR_OK)
-               return res;
-
-       /* Set cache disable. */
-       res = target_write_u16(target, SAMD_NVMCTRL + SAMD_NVMCTRL_CTRLB,
-                       tmp | (1<<18));
-       if (res != ERROR_OK)
-               return res;
-
        /* Issue the NVM command */
-       int res_cmd = target_write_u16(target,
+       res = target_write_u16(target,
                        SAMD_NVMCTRL + SAMD_NVMCTRL_CTRLA, SAMD_NVM_CMD(cmd));
-
-       /* Try to restore configuration, regardless of NVM command write
-        * status. */
-       res = target_write_u16(target, SAMD_NVMCTRL + SAMD_NVMCTRL_CTRLB, tmp);
-
-       if (res_cmd != ERROR_OK)
-               return res_cmd;
-
        if (res != ERROR_OK)
                return res;
 
@@ -671,132 +648,19 @@ static int samd_erase(struct flash_bank *bank, int first, int last)
                        return ERROR_FLASH_OPERATION_FAILED;
                }
 
-               if (bank->sectors[s].is_erased != 1) {
-                       /* For each row in that sector */
-                       for (int r = s * rows_in_sector; r < (s + 1) * rows_in_sector; r++) {
-                               res = samd_erase_row(bank->target, r * chip->page_size * 4);
-                               if (res != ERROR_OK) {
-                                       LOG_ERROR("SAMD: failed to erase sector %d", s);
-                                       return res;
-                               }
-                       }
-
-                       bank->sectors[s].is_erased = 1;
-               }
-       }
-
-       return ERROR_OK;
-}
-
-static struct flash_sector *samd_find_sector_by_address(struct flash_bank *bank, uint32_t address)
-{
-       struct samd_info *chip = (struct samd_info *)bank->driver_priv;
-
-       for (int i = 0; i < bank->num_sectors; i++) {
-               if (bank->sectors[i].offset <= address &&
-                   address < bank->sectors[i].offset + chip->sector_size)
-                       return &bank->sectors[i];
-       }
-       return NULL;
-}
-
-/* Write an entire row (four pages) from host buffer 'buf' to row-aligned
- * 'address' in the Flash. */
-static int samd_write_row(struct flash_bank *bank, uint32_t address,
-               const uint8_t *buf)
-{
-       int res;
-       struct samd_info *chip = (struct samd_info *)bank->driver_priv;
-
-       struct flash_sector *sector = samd_find_sector_by_address(bank, address);
-
-       if (!sector) {
-               LOG_ERROR("Can't find sector corresponding to address 0x%08" PRIx32, address);
-               return ERROR_FLASH_OPERATION_FAILED;
-       }
-
-       if (sector->is_protected) {
-               LOG_ERROR("Trying to write to a protected sector at 0x%08" PRIx32, address);
-               return ERROR_FLASH_OPERATION_FAILED;
-       }
-
-       /* Erase the row that we'll be writing to */
-       res = samd_erase_row(bank->target, address);
-       if (res != ERROR_OK)
-               return res;
-
-       /* Now write the pages in this row. */
-       for (unsigned int i = 0; i < 4; i++) {
-               bool error;
-
-               /* Write the page contents to the target's page buffer.  A page write
-                * is issued automatically once the last location is written in the
-                * page buffer (ie: a complete page has been written out). */
-               res = target_write_memory(bank->target, address, 4,
-                               chip->page_size / 4, buf);
-               if (res != ERROR_OK) {
-                       LOG_ERROR("%s: %d", __func__, __LINE__);
-                       return res;
-               }
-
-               /* For some devices automatic page write is not default so we need
-                * to issue a write page CMD to the NVM */
-               if (chip->manual_wp == true) {
-                       res = samd_issue_nvmctrl_command(bank->target, SAMD_NVM_CMD_WP);
+               /* For each row in that sector */
+               for (int r = s * rows_in_sector; r < (s + 1) * rows_in_sector; r++) {
+                       res = samd_erase_row(bank->target, r * chip->page_size * 4);
                        if (res != ERROR_OK) {
-                               LOG_ERROR("%s: %d", __func__, __LINE__);
+                               LOG_ERROR("SAMD: failed to erase sector %d", s);
                                return res;
                        }
                }
-
-               /* Access through AHB is stalled while flash is being programmed */
-               usleep(200);
-
-               error = samd_check_error(bank->target);
-               if (error)
-                       return ERROR_FAIL;
-
-               /* Next page */
-               address += chip->page_size;
-               buf += chip->page_size;
        }
 
-       sector->is_erased = 0;
-
-       return res;
+       return ERROR_OK;
 }
 
-/* Write partial contents into row-aligned 'address' on the Flash from host
- * buffer 'buf' by writing 'nb' of 'buf' at 'row_offset' into the Flash row. */
-static int samd_write_row_partial(struct flash_bank *bank, uint32_t address,
-               const uint8_t *buf, uint32_t row_offset, uint32_t nb)
-{
-       int res;
-       struct samd_info *chip = (struct samd_info *)bank->driver_priv;
-       uint32_t row_size = chip->page_size * 4;
-       uint8_t *rb = malloc(row_size);
-       if (!rb)
-               return ERROR_FAIL;
-
-       assert(row_offset + nb < row_size);
-       assert((address % row_size) == 0);
-
-       /* Retrieve the full row contents from Flash */
-       res = target_read_memory(bank->target, address, 4, row_size / 4, rb);
-       if (res != ERROR_OK) {
-               free(rb);
-               return res;
-       }
-
-       /* Insert our partial row over the data from Flash */
-       memcpy(rb + (row_offset % row_size), buf, nb);
-
-       /* Write the row back out */
-       res = samd_write_row(bank, address, rb);
-       free(rb);
-
-       return res;
-}
 
 static int samd_write(struct flash_bank *bank, const uint8_t *buffer,
                uint32_t offset, uint32_t count)
@@ -804,13 +668,15 @@ static int samd_write(struct flash_bank *bank, const uint8_t *buffer,
        int res;
        uint32_t nvm_ctrlb;
        uint32_t address;
-       uint32_t nb = 0;
+       uint32_t pg_offset;
+       uint32_t nb;
+       uint32_t nw;
        struct samd_info *chip = (struct samd_info *)bank->driver_priv;
-       uint32_t row_size = chip->page_size * 4;
+       uint8_t *pb = NULL;
+       bool manual_wp;
 
        if (bank->target->state != TARGET_HALTED) {
                LOG_ERROR("Target not halted");
-
                return ERROR_TARGET_NOT_HALTED;
        }
 
@@ -826,66 +692,93 @@ static int samd_write(struct flash_bank *bank, const uint8_t *buffer,
                return res;
 
        if (nvm_ctrlb & SAMD_NVM_CTRLB_MANW)
-               chip->manual_wp = true;
+               manual_wp = true;
        else
-               chip->manual_wp = false;
+               manual_wp = false;
 
+       res = samd_issue_nvmctrl_command(bank->target, SAMD_NVM_CMD_PBC);
+       if (res != ERROR_OK) {
+               LOG_ERROR("%s: %d", __func__, __LINE__);
+               return res;
+       }
 
-       if (offset % row_size) {
-               /* We're starting at an unaligned offset so we'll write a partial row
-                * comprising that offset and up to the end of that row. */
-               nb = row_size - (offset % row_size);
-               if (nb > count)
+       while (count) {
+               nb = chip->page_size - offset % chip->page_size;
+               if (count < nb)
                        nb = count;
-       } else if (count < row_size) {
-               /* We're writing an aligned but partial row. */
-               nb = count;
-       }
 
-       address = (offset / row_size) * row_size + bank->base;
+               address = bank->base + offset;
+               pg_offset = offset % chip->page_size;
 
-       if (nb > 0) {
-               res = samd_write_row_partial(bank, address, buffer,
-                               offset % row_size, nb);
-               if (res != ERROR_OK)
-                       return res;
+               if (offset % 4 || (offset + nb) % 4) {
+                       /* Either start or end of write is not word aligned */
+                       if (!pb) {
+                               pb = malloc(chip->page_size);
+                               if (!pb)
+                                       return ERROR_FAIL;
+                       }
 
-               /* We're done with the row contents */
-               count -= nb;
-               offset += nb;
-               buffer += row_size;
-       }
+                       /* Set temporary page buffer to 0xff and overwrite the relevant part */
+                       memset(pb, 0xff, chip->page_size);
+                       memcpy(pb + pg_offset, buffer, nb);
+
+                       /* Align start address to a word boundary */
+                       address -= offset % 4;
+                       pg_offset -= offset % 4;
+                       assert(pg_offset % 4 == 0);
+
+                       /* Extend length to whole words */
+                       nw = (nb + offset % 4 + 3) / 4;
+                       assert(pg_offset + 4 * nw <= chip->page_size);
+
+                       /* Now we have original data extended by 0xff bytes
+                        * to the nearest word boundary on both start and end */
+                       res = target_write_memory(bank->target, address, 4, nw, pb + pg_offset);
+               } else {
+                       assert(nb % 4 == 0);
+                       nw = nb / 4;
+                       assert(pg_offset + 4 * nw <= chip->page_size);
 
-       /* There's at least one aligned row to write out. */
-       if (count >= row_size) {
-               int nr = count / row_size + ((count % row_size) ? 1 : 0);
-               unsigned int r = 0;
-
-               for (unsigned int i = address / row_size;
-                               (i < (address / row_size) + nr) && count > 0; i++) {
-                       address = (i * row_size) + bank->base;
-
-                       if (count >= row_size) {
-                               res = samd_write_row(bank, address, buffer + (r * row_size));
-                               /* Advance one row */
-                               offset += row_size;
-                               count -= row_size;
-                       } else {
-                               res = samd_write_row_partial(bank, address,
-                                               buffer + (r * row_size), 0, count);
-                               /* We're done after this. */
-                               offset += count;
-                               count = 0;
+                       /* Word aligned data, use direct write from buffer */
+                       res = target_write_memory(bank->target, address, 4, nw, buffer);
+               }
+               if (res != ERROR_OK) {
+                       LOG_ERROR("%s: %d", __func__, __LINE__);
+                       goto free_pb;
+               }
+
+               /* Devices with errata 13134 have automatic page write enabled by default
+                * For other devices issue a write page CMD to the NVM
+                * If the page has not been written up to the last word
+                * then issue CMD_WP always */
+               if (manual_wp || pg_offset + 4 * nw < chip->page_size) {
+                       res = samd_issue_nvmctrl_command(bank->target, SAMD_NVM_CMD_WP);
+                       if (res != ERROR_OK) {
+                               LOG_ERROR("%s: %d", __func__, __LINE__);
+                               goto free_pb;
                        }
+               }
 
-                       r++;
+               /* Access through AHB is stalled while flash is being programmed */
+               usleep(200);
 
-                       if (res != ERROR_OK)
-                               return res;
+               if (samd_check_error(bank->target)) {
+                       LOG_ERROR("%s: write failed at address 0x%08" PRIx32, __func__, address);
+                       res = ERROR_FAIL;
+                       goto free_pb;
                }
+
+               /* We're done with the page contents */
+               count -= nb;
+               offset += nb;
+               buffer += nb;
        }
 
-       return ERROR_OK;
+free_pb:
+       if (pb)
+               free(pb);
+
+       return res;
 }
 
 FLASH_BANK_COMMAND_HANDLER(samd_flash_bank_command)