+ fileio_close(&fileio);
+
+ return ERROR_OK;
+}
+
+/* lookup flash bank by address */
+flash_bank_t *get_flash_bank_by_addr(target_t *target, u32 addr)
+{
+ flash_bank_t *c;
+
+ /* cycle through bank list */
+ for (c = flash_banks; c; c = c->next)
+ {
+ /* check whether address belongs to this flash bank */
+ if ((addr >= c->base) && (addr < c->base + c->size) && target == c->target)
+ return c;
+ }
+
+ return NULL;
+}
+
+/* erase given flash region, selects proper bank according to target and address */
+int flash_erase(target_t *target, u32 addr, u32 length)
+{
+ flash_bank_t *c;
+ unsigned long sector_size;
+ int first;
+ int last;
+
+ if ((c = get_flash_bank_by_addr(target, addr)) == NULL)
+ return ERROR_FLASH_DST_OUT_OF_BANK; /* no corresponding bank found */
+
+ /* sanity checks */
+ if (c->size == 0 || c->num_sectors == 0 || c->size % c->num_sectors)
+ return ERROR_FLASH_BANK_INVALID;
+
+ if (length == 0)
+ {
+ /* special case, erase whole bank when length is zero */
+ if (addr != c->base)
+ return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+
+ return c->driver->erase(c, 0, c->num_sectors - 1);
+ }
+
+ /* check whether it fits */
+ if (addr + length > c->base + c->size)
+ return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+
+ /* calculate sector size */
+ sector_size = c->size / c->num_sectors;
+
+ /* check alignment */
+ if ((addr - c->base) % sector_size || length % sector_size)
+ return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+
+ first = (addr - c->base) / sector_size;
+ last = first + length / sector_size - 1;
+ return c->driver->erase(c, first, last);
+}
+
+int flash_write(target_t *target, image_t *image, u32 *image_size, char **error_str, u32 *failed)
+{
+ int last_section;
+ int section;
+ int next_section;
+ int retval;
+
+ *image_size = 0;
+
+ /* for each section in the image */
+ last_section = 0;
+ section = 0;
+ while (section < image->num_sections)
+ {
+ u32 offset = 0;
+ u32 address = image->sections[section].base_address;
+ u32 size = image->sections[section].size;
+
+ /* collect consecutive sections */
+ next_section = section + 1;
+ while ((next_section < image->num_sections)
+ && (image->sections[next_section].base_address == (address + size)))
+ {
+ size += image->sections[next_section].size;
+ next_section++;
+ }
+
+ while (size != 0)
+ {
+ flash_bank_t *c;
+ u32 thisrun_size = size;
+ u32 buffer_size;
+ u32 size_read;
+ u8 *buffer;
+
+ /* find the corresponding flash bank */
+ if ((c = get_flash_bank_by_addr(target, address)) == NULL)
+ {
+ /* mark as failed, and skip the current section */
+ failed[section] = 1;
+ break;
+ }
+
+ /* check whether cumulated sections fit the bank, split into multiple runs if not */
+ if ((address + size) > (c->base + c->size))
+ thisrun_size = c->base + c->size - address;
+
+ buffer = malloc(thisrun_size);
+ buffer_size = 0;
+
+ while (buffer_size < thisrun_size)
+ {
+ u32 thissection_size = image->sections[section].size - offset;
+
+ if ((retval = image_read_section(image, section, offset,
+ MIN(thisrun_size, thissection_size),
+ buffer + buffer_size, &size_read)) != ERROR_OK)
+ {
+ *error_str = malloc(FLASH_MAX_ERROR_STR);
+ snprintf(*error_str, FLASH_MAX_ERROR_STR, "error reading from image");
+ return ERROR_IMAGE_TEMPORARILY_UNAVAILABLE;
+ }
+
+ /* see if we're done with the current section */
+ if (thissection_size < thisrun_size)
+ {
+ /* start with the next section */
+ offset = 0;
+ failed[section] = 0;
+ section++;
+ }
+ else
+ {
+ /* continue inside the current section */
+ offset += size_read;
+ }
+
+ buffer_size += size_read;
+ }
+
+ if ((retval = c->driver->write(c, buffer, address - c->base, thisrun_size)) != ERROR_OK)
+ {
+ int i;
+ /* mark sections as failed */
+ for (i = last_section; i <= section; i++)
+ failed[i] = 1;
+
+ *error_str = malloc(FLASH_MAX_ERROR_STR);
+ switch (retval)
+ {
+ case ERROR_TARGET_NOT_HALTED:
+ snprintf(*error_str, FLASH_MAX_ERROR_STR, "can't flash image while target is running");
+ break;
+ case ERROR_INVALID_ARGUMENTS:
+ snprintf(*error_str, FLASH_MAX_ERROR_STR, "flash driver can't fulfill request");
+ break;
+ case ERROR_FLASH_OPERATION_FAILED:
+ snprintf(*error_str, FLASH_MAX_ERROR_STR, "flash program error");
+ break;
+ case ERROR_FLASH_DST_BREAKS_ALIGNMENT:
+ snprintf(*error_str, FLASH_MAX_ERROR_STR, "offset breaks required alignment");
+ break;
+ case ERROR_FLASH_DST_OUT_OF_BANK:
+ snprintf(*error_str, FLASH_MAX_ERROR_STR, "no flash mapped at requested address");
+ break;
+ case ERROR_FLASH_SECTOR_NOT_ERASED:
+ snprintf(*error_str, FLASH_MAX_ERROR_STR, "destination sector(s) not erased");
+ break;
+ default:
+ snprintf(*error_str, FLASH_MAX_ERROR_STR, "unknown error: %i", retval);
+ }
+
+ free(buffer);
+
+ /* abort operation */
+ return retval;
+ }
+
+ free(buffer);
+
+ address += thisrun_size;
+ size -= thisrun_size;
+ *image_size += thisrun_size;
+ last_section = section;
+ }
+ }