git.gag.com Git - fw/openocd/blob - src/flash/nor/core.c

   1 /***************************************************************************
   2  *   Copyright (C) 2005 by Dominic Rath <Dominic.Rath@gmx.de>              *
   3  *   Copyright (C) 2007-2010 Øyvind Harboe <oyvind.harboe@zylin.com>       *
   4  *   Copyright (C) 2008 by Spencer Oliver <spen@spen-soft.co.uk>           *
   5  *   Copyright (C) 2009 Zachary T Welch <zw@superlucidity.net>             *
   6  *   Copyright (C) 2010 by Antonio Borneo <borneo.antonio@gmail.com>       *
   7  *   Copyright (C) 2017-2018 Tomas Vanek <vanekt@fbl.cz>                   *
   8  *                                                                         *
   9  *   This program is free software; you can redistribute it and/or modify  *
  10  *   it under the terms of the GNU General Public License as published by  *
  11  *   the Free Software Foundation; either version 2 of the License, or     *
  12  *   (at your option) any later version.                                   *
  13  *                                                                         *
  14  *   This program is distributed in the hope that it will be useful,       *
  15  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  16  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  17  *   GNU General Public License for more details.                          *
  18  *                                                                         *
  19  *   You should have received a copy of the GNU General Public License     *
  20  *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  21  ***************************************************************************/
  22
  23 #ifdef HAVE_CONFIG_H
  24 #include <config.h>
  25 #endif
  26 #include <flash/common.h>
  27 #include <flash/nor/core.h>
  28 #include <flash/nor/imp.h>
  29 #include <target/image.h>
  30
  31 /**
  32  * @file
  33  * Upper level of NOR flash framework.
  34  * The lower level interfaces are to drivers.  These upper level ones
  35  * primarily support access from Tcl scripts or from GDB.
  36  */
  37
  38 static struct flash_bank *flash_banks;
  39
  40 int flash_driver_erase(struct flash_bank *bank, unsigned int first,
  41                 unsigned int last)
  42 {
  43         int retval;
  44
  45         retval = bank->driver->erase(bank, first, last);
  46         if (retval != ERROR_OK)
  47                 LOG_ERROR("failed erasing sectors %u to %u", first, last);
  48
  49         return retval;
  50 }
  51
  52 int flash_driver_protect(struct flash_bank *bank, int set, unsigned int first,
  53                 unsigned int last)
  54 {
  55         int retval;
  56         unsigned int num_blocks;
  57
  58         if (bank->num_prot_blocks)
  59                 num_blocks = bank->num_prot_blocks;
  60         else
  61                 num_blocks = bank->num_sectors;
  62
  63
  64         /* callers may not supply illegal parameters ... */
  65         if (first > last || last >= num_blocks) {
  66                 LOG_ERROR("illegal protection block range");
  67                 return ERROR_FAIL;
  68         }
  69
  70         /* force "set" to 0/1 */
  71         set = !!set;
  72
  73         if (!bank->driver->protect) {
  74                 LOG_ERROR("Flash protection is not supported.");
  75                 return ERROR_FLASH_OPER_UNSUPPORTED;
  76         }
  77
  78         /* DANGER!
  79          *
  80          * We must not use any cached information about protection state!!!!
  81          *
  82          * There are a million things that could change the protect state:
  83          *
  84          * the target could have reset, power cycled, been hot plugged,
  85          * the application could have run, etc.
  86          *
  87          * Drivers only receive valid protection block range.
  88          */
  89         retval = bank->driver->protect(bank, set, first, last);
  90         if (retval != ERROR_OK)
  91                 LOG_ERROR("failed setting protection for blocks %u to %u", first, last);
  92
  93         return retval;
  94 }
  95
  96 int flash_driver_write(struct flash_bank *bank,
  97         const uint8_t *buffer, uint32_t offset, uint32_t count)
  98 {
  99         int retval;
 100
 101         retval = bank->driver->write(bank, buffer, offset, count);
 102         if (retval != ERROR_OK) {
 103                 LOG_ERROR(
 104                         "error writing to flash at address " TARGET_ADDR_FMT
 105                         " at offset 0x%8.8" PRIx32,
 106                         bank->base,
 107                         offset);
 108         }
 109
 110         return retval;
 111 }
 112
 113 int flash_driver_read(struct flash_bank *bank,
 114         uint8_t *buffer, uint32_t offset, uint32_t count)
 115 {
 116         int retval;
 117
 118         LOG_DEBUG("call flash_driver_read()");
 119
 120         retval = bank->driver->read(bank, buffer, offset, count);
 121         if (retval != ERROR_OK) {
 122                 LOG_ERROR(
 123                         "error reading to flash at address " TARGET_ADDR_FMT
 124                         " at offset 0x%8.8" PRIx32,
 125                         bank->base,
 126                         offset);
 127         }
 128
 129         return retval;
 130 }
 131
 132 int default_flash_read(struct flash_bank *bank,
 133         uint8_t *buffer, uint32_t offset, uint32_t count)
 134 {
 135         return target_read_buffer(bank->target, offset + bank->base, count, buffer);
 136 }
 137
 138 int flash_driver_verify(struct flash_bank *bank,
 139         const uint8_t *buffer, uint32_t offset, uint32_t count)
 140 {
 141         int retval;
 142
 143         retval = bank->driver->verify ? bank->driver->verify(bank, buffer, offset, count) :
 144                 default_flash_verify(bank, buffer, offset, count);
 145         if (retval != ERROR_OK) {
 146                 LOG_ERROR("verify failed in bank at " TARGET_ADDR_FMT " starting at 0x%8.8" PRIx32,
 147                         bank->base, offset);
 148         }
 149
 150         return retval;
 151 }
 152
 153 int default_flash_verify(struct flash_bank *bank,
 154         const uint8_t *buffer, uint32_t offset, uint32_t count)
 155 {
 156         uint32_t target_crc, image_crc;
 157         int retval;
 158
 159         retval = image_calculate_checksum(buffer, count, &image_crc);
 160         if (retval != ERROR_OK)
 161                 return retval;
 162
 163         retval = target_checksum_memory(bank->target, offset + bank->base, count, &target_crc);
 164         if (retval != ERROR_OK)
 165                 return retval;
 166
 167         LOG_DEBUG("addr " TARGET_ADDR_FMT ", len 0x%08" PRIx32 ", crc 0x%08" PRIx32 " 0x%08" PRIx32,
 168                 offset + bank->base, count, ~image_crc, ~target_crc);
 169         if (target_crc == image_crc)
 170                 return ERROR_OK;
 171         else
 172                 return ERROR_FAIL;
 173 }
 174
 175 void flash_bank_add(struct flash_bank *bank)
 176 {
 177         /* put flash bank in linked list */
 178         unsigned bank_num = 0;
 179         if (flash_banks) {
 180                 /* find last flash bank */
 181                 struct flash_bank *p = flash_banks;
 182                 while (p->next) {
 183                         bank_num += 1;
 184                         p = p->next;
 185                 }
 186                 p->next = bank;
 187                 bank_num += 1;
 188         } else
 189                 flash_banks = bank;
 190
 191         bank->bank_number = bank_num;
 192 }
 193
 194 struct flash_bank *flash_bank_list(void)
 195 {
 196         return flash_banks;
 197 }
 198
 199 struct flash_bank *get_flash_bank_by_num_noprobe(unsigned int num)
 200 {
 201         struct flash_bank *p;
 202         unsigned int i = 0;
 203
 204         for (p = flash_banks; p; p = p->next) {
 205                 if (i++ == num)
 206                         return p;
 207         }
 208         LOG_ERROR("flash bank %d does not exist", num);
 209         return NULL;
 210 }
 211
 212 unsigned int flash_get_bank_count(void)
 213 {
 214         struct flash_bank *p;
 215         unsigned int i = 0;
 216         for (p = flash_banks; p; p = p->next)
 217                 i++;
 218         return i;
 219 }
 220
 221 void default_flash_free_driver_priv(struct flash_bank *bank)
 222 {
 223         free(bank->driver_priv);
 224         bank->driver_priv = NULL;
 225 }
 226
 227 void flash_free_all_banks(void)
 228 {
 229         struct flash_bank *bank = flash_banks;
 230         while (bank) {
 231                 struct flash_bank *next = bank->next;
 232                 if (bank->driver->free_driver_priv)
 233                         bank->driver->free_driver_priv(bank);
 234                 else
 235                         LOG_WARNING("Flash driver of %s does not support free_driver_priv()", bank->name);
 236
 237                 /* For 'virtual' flash driver bank->sectors and bank->prot_blocks pointers are copied from
 238                  * master flash_bank structure. They point to memory locations allocated by master flash driver
 239                  * so master driver is responsible for releasing them.
 240                  * Avoid UB caused by double-free memory corruption if flash bank is 'virtual'. */
 241
 242                 if (strcmp(bank->driver->name, "virtual") != 0) {
 243                         free(bank->sectors);
 244                         free(bank->prot_blocks);
 245                 }
 246
 247                 free(bank->name);
 248                 free(bank);
 249                 bank = next;
 250         }
 251         flash_banks = NULL;
 252 }
 253
 254 struct flash_bank *get_flash_bank_by_name_noprobe(const char *name)
 255 {
 256         unsigned requested = get_flash_name_index(name);
 257         unsigned found = 0;
 258
 259         struct flash_bank *bank;
 260         for (bank = flash_banks; bank; bank = bank->next) {
 261                 if (strcmp(bank->name, name) == 0)
 262                         return bank;
 263                 if (!flash_driver_name_matches(bank->driver->name, name))
 264                         continue;
 265                 if (++found < requested)
 266                         continue;
 267                 return bank;
 268         }
 269         return NULL;
 270 }
 271
 272 int get_flash_bank_by_name(const char *name, struct flash_bank **bank_result)
 273 {
 274         struct flash_bank *bank;
 275         int retval;
 276
 277         bank = get_flash_bank_by_name_noprobe(name);
 278         if (bank) {
 279                 retval = bank->driver->auto_probe(bank);
 280
 281                 if (retval != ERROR_OK) {
 282                         LOG_ERROR("auto_probe failed");
 283                         return retval;
 284                 }
 285         }
 286
 287         *bank_result = bank;
 288         return ERROR_OK;
 289 }
 290
 291 int get_flash_bank_by_num(unsigned int num, struct flash_bank **bank)
 292 {
 293         struct flash_bank *p = get_flash_bank_by_num_noprobe(num);
 294         int retval;
 295
 296         if (!p)
 297                 return ERROR_FAIL;
 298
 299         retval = p->driver->auto_probe(p);
 300
 301         if (retval != ERROR_OK) {
 302                 LOG_ERROR("auto_probe failed");
 303                 return retval;
 304         }
 305         *bank = p;
 306         return ERROR_OK;
 307 }
 308
 309 /* lookup flash bank by address, bank not found is success, but
 310  * result_bank is set to NULL. */
 311 int get_flash_bank_by_addr(struct target *target,
 312         target_addr_t addr,
 313         bool check,
 314         struct flash_bank **result_bank)
 315 {
 316         struct flash_bank *c;
 317
 318         /* cycle through bank list */
 319         for (c = flash_banks; c; c = c->next) {
 320                 if (c->target != target)
 321                         continue;
 322
 323                 int retval;
 324                 retval = c->driver->auto_probe(c);
 325
 326                 if (retval != ERROR_OK) {
 327                         LOG_ERROR("auto_probe failed");
 328                         return retval;
 329                 }
 330                 /* check whether address belongs to this flash bank */
 331                 if ((addr >= c->base) && (addr <= c->base + (c->size - 1))) {
 332                         *result_bank = c;
 333                         return ERROR_OK;
 334                 }
 335         }
 336         *result_bank = NULL;
 337         if (check) {
 338                 LOG_ERROR("No flash at address " TARGET_ADDR_FMT, addr);
 339                 return ERROR_FAIL;
 340         }
 341         return ERROR_OK;
 342 }
 343
 344 static int default_flash_mem_blank_check(struct flash_bank *bank)
 345 {
 346         struct target *target = bank->target;
 347         const int buffer_size = 1024;
 348         uint32_t n_bytes;
 349         int retval = ERROR_OK;
 350
 351         if (bank->target->state != TARGET_HALTED) {
 352                 LOG_ERROR("Target not halted");
 353                 return ERROR_TARGET_NOT_HALTED;
 354         }
 355
 356         uint8_t *buffer = malloc(buffer_size);
 357
 358         for (unsigned int i = 0; i < bank->num_sectors; i++) {
 359                 uint32_t j;
 360                 bank->sectors[i].is_erased = 1;
 361
 362                 for (j = 0; j < bank->sectors[i].size; j += buffer_size) {
 363                         uint32_t chunk;
 364                         chunk = buffer_size;
 365                         if (chunk > (bank->sectors[i].size - j))
 366                                 chunk = (bank->sectors[i].size - j);
 367
 368                         retval = target_read_memory(target,
 369                                         bank->base + bank->sectors[i].offset + j,
 370                                         4,
 371                                         chunk/4,
 372                                         buffer);
 373                         if (retval != ERROR_OK)
 374                                 goto done;
 375
 376                         for (n_bytes = 0; n_bytes < chunk; n_bytes++) {
 377                                 if (buffer[n_bytes] != bank->erased_value) {
 378                                         bank->sectors[i].is_erased = 0;
 379                                         break;
 380                                 }
 381                         }
 382                 }
 383         }
 384
 385 done:
 386         free(buffer);
 387
 388         return retval;
 389 }
 390
 391 int default_flash_blank_check(struct flash_bank *bank)
 392 {
 393         struct target *target = bank->target;
 394         int retval;
 395
 396         if (bank->target->state != TARGET_HALTED) {
 397                 LOG_ERROR("Target not halted");
 398                 return ERROR_TARGET_NOT_HALTED;
 399         }
 400
 401         struct target_memory_check_block *block_array;
 402         block_array = malloc(bank->num_sectors * sizeof(struct target_memory_check_block));
 403         if (!block_array)
 404                 return default_flash_mem_blank_check(bank);
 405
 406         for (unsigned int i = 0; i < bank->num_sectors; i++) {
 407                 block_array[i].address = bank->base + bank->sectors[i].offset;
 408                 block_array[i].size = bank->sectors[i].size;
 409                 block_array[i].result = UINT32_MAX; /* erase state unknown */
 410         }
 411
 412         bool fast_check = true;
 413         for (unsigned int i = 0; i < bank->num_sectors; ) {
 414                 retval = target_blank_check_memory(target,
 415                                 block_array + i, bank->num_sectors - i,
 416                                 bank->erased_value);
 417                 if (retval < 1) {
 418                         /* Run slow fallback if the first run gives no result
 419                          * otherwise use possibly incomplete results */
 420                         if (i == 0)
 421                                 fast_check = false;
 422                         break;
 423                 }
 424                 i += retval; /* add number of blocks done this round */
 425         }
 426
 427         if (fast_check) {
 428                 for (unsigned int i = 0; i < bank->num_sectors; i++)
 429                         bank->sectors[i].is_erased = block_array[i].result;
 430                 retval = ERROR_OK;
 431         } else {
 432                 LOG_USER("Running slow fallback erase check - add working memory");
 433                 retval = default_flash_mem_blank_check(bank);
 434         }
 435         free(block_array);
 436
 437         return retval;
 438 }
 439
 440 /* Manipulate given flash region, selecting the bank according to target
 441  * and address.  Maps an address range to a set of sectors, and issues
 442  * the callback() on that set ... e.g. to erase or unprotect its members.
 443  *
 444  * Parameter iterate_protect_blocks switches iteration of protect block
 445  * instead of erase sectors. If there is no protect blocks array, sectors
 446  * are used in iteration, so compatibility for old flash drivers is retained.
 447  *
 448  * The "pad_reason" parameter is a kind of boolean:  when it's NULL, the
 449  * range must fit those sectors exactly.  This is clearly safe; it can't
 450  * erase data which the caller said to leave alone, for example.  If it's
 451  * non-NULL, rather than failing, extra data in the first and/or last
 452  * sectors will be added to the range, and that reason string is used when
 453  * warning about those additions.
 454  */
 455 static int flash_iterate_address_range_inner(struct target *target,
 456         char *pad_reason, target_addr_t addr, uint32_t length,
 457         bool iterate_protect_blocks,
 458         int (*callback)(struct flash_bank *bank, unsigned int first,
 459                 unsigned int last))
 460 {
 461         struct flash_bank *c;
 462         struct flash_sector *block_array;
 463         target_addr_t last_addr = addr + length - 1;    /* the last address of range */
 464         int first = -1;
 465         int last = -1;
 466         int i;
 467         int num_blocks;
 468
 469         int retval = get_flash_bank_by_addr(target, addr, true, &c);
 470         if (retval != ERROR_OK)
 471                 return retval;
 472
 473         if (c->size == 0 || c->num_sectors == 0) {
 474                 LOG_ERROR("Bank is invalid");
 475                 return ERROR_FLASH_BANK_INVALID;
 476         }
 477
 478         if (length == 0) {
 479                 /* special case, erase whole bank when length is zero */
 480                 if (addr != c->base) {
 481                         LOG_ERROR("Whole bank access must start at beginning of bank.");
 482                         return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
 483                 }
 484
 485                 return callback(c, 0, c->num_sectors - 1);
 486         }
 487
 488         /* check whether it all fits in this bank */
 489         if (last_addr > c->base + c->size - 1) {
 490                 LOG_ERROR("Flash access does not fit into bank.");
 491                 return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
 492         }
 493
 494         if (!c->prot_blocks || c->num_prot_blocks == 0) {
 495                 /* flash driver does not define protect blocks, use sectors instead */
 496                 iterate_protect_blocks = false;
 497         }
 498
 499         if (iterate_protect_blocks) {
 500                 block_array = c->prot_blocks;
 501                 num_blocks = c->num_prot_blocks;
 502         } else {
 503                 block_array = c->sectors;
 504                 num_blocks = c->num_sectors;
 505         }
 506
 507         for (i = 0; i < num_blocks; i++) {
 508                 struct flash_sector *f = &block_array[i];
 509                 target_addr_t sector_addr = c->base + f->offset;
 510                 target_addr_t sector_last_addr = sector_addr + f->size - 1;
 511
 512                 /* start only on a sector boundary */
 513                 if (first < 0) {
 514                         /* scanned past the first sector? */
 515                         if (addr < sector_addr)
 516                                 break;
 517
 518                         /* is this the first sector? */
 519                         if (addr == sector_addr)
 520                                 first = i;
 521
 522                         /* Does this need head-padding?  If so, pad and warn;
 523                          * or else force an error.
 524                          *
 525                          * Such padding can make trouble, since *WE* can't
 526                          * ever know if that data was in use.  The warning
 527                          * should help users sort out messes later.
 528                          */
 529                         else if (addr <= sector_last_addr && pad_reason) {
 530                                 /* FIXME say how many bytes (e.g. 80 KB) */
 531                                 LOG_WARNING("Adding extra %s range, "
 532                                         TARGET_ADDR_FMT " .. " TARGET_ADDR_FMT,
 533                                         pad_reason,
 534                                         sector_addr,
 535                                         addr - 1);
 536                                 first = i;
 537                         } else
 538                                 continue;
 539                 }
 540
 541                 /* is this (also?) the last sector? */
 542                 if (last_addr == sector_last_addr) {
 543                         last = i;
 544                         break;
 545                 }
 546
 547                 /* Does this need tail-padding?  If so, pad and warn;
 548                  * or else force an error.
 549                  */
 550                 if (last_addr < sector_last_addr && pad_reason) {
 551                         /* FIXME say how many bytes (e.g. 80 KB) */
 552                         LOG_WARNING("Adding extra %s range, "
 553                                 TARGET_ADDR_FMT " .. " TARGET_ADDR_FMT,
 554                                 pad_reason,
 555                                 last_addr + 1,
 556                                 sector_last_addr);
 557                         last = i;
 558                         break;
 559                 }
 560
 561                 /* MUST finish on a sector boundary */
 562                 if (last_addr < sector_addr)
 563                         break;
 564         }
 565
 566         /* invalid start or end address? */
 567         if (first == -1 || last == -1) {
 568                 LOG_ERROR("address range " TARGET_ADDR_FMT " .. " TARGET_ADDR_FMT
 569                         " is not sector-aligned",
 570                         addr,
 571                         last_addr);
 572                 return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
 573         }
 574
 575         /* The NOR driver may trim this range down, based on what
 576          * sectors are already erased/unprotected.  GDB currently
 577          * blocks such optimizations.
 578          */
 579         return callback(c, first, last);
 580 }
 581
 582 /* The inner fn only handles a single bank, we could be spanning
 583  * multiple chips.
 584  */
 585 static int flash_iterate_address_range(struct target *target,
 586         char *pad_reason, target_addr_t addr, uint32_t length,
 587         bool iterate_protect_blocks,
 588         int (*callback)(struct flash_bank *bank, unsigned int first,
 589                 unsigned int last))
 590 {
 591         struct flash_bank *c;
 592         int retval = ERROR_OK;
 593
 594         /* Danger! zero-length iterations means entire bank! */
 595         do {
 596                 retval = get_flash_bank_by_addr(target, addr, true, &c);
 597                 if (retval != ERROR_OK)
 598                         return retval;
 599
 600                 uint32_t cur_length = length;
 601                 /* check whether it all fits in this bank */
 602                 if (addr + length - 1 > c->base + c->size - 1) {
 603                         LOG_DEBUG("iterating over more than one flash bank.");
 604                         cur_length = c->base + c->size - addr;
 605                 }
 606                 retval = flash_iterate_address_range_inner(target,
 607                                 pad_reason, addr, cur_length,
 608                                 iterate_protect_blocks,
 609                                 callback);
 610                 if (retval != ERROR_OK)
 611                         break;
 612
 613                 length -= cur_length;
 614                 addr += cur_length;
 615         } while (length > 0);
 616
 617         return retval;
 618 }
 619
 620 int flash_erase_address_range(struct target *target,
 621         bool pad, target_addr_t addr, uint32_t length)
 622 {
 623         return flash_iterate_address_range(target, pad ? "erase" : NULL,
 624                 addr, length, false, &flash_driver_erase);
 625 }
 626
 627 static int flash_driver_unprotect(struct flash_bank *bank, unsigned int first,
 628                 unsigned int last)
 629 {
 630         return flash_driver_protect(bank, 0, first, last);
 631 }
 632
 633 int flash_unlock_address_range(struct target *target, target_addr_t addr,
 634                 uint32_t length)
 635 {
 636         /* By default, pad to sector boundaries ... the real issue here
 637          * is that our (only) caller *permanently* removes protection,
 638          * and doesn't restore it.
 639          */
 640         return flash_iterate_address_range(target, "unprotect",
 641                 addr, length, true, &flash_driver_unprotect);
 642 }
 643
 644 static int compare_section(const void *a, const void *b)
 645 {
 646         struct imagesection *b1, *b2;
 647         b1 = *((struct imagesection **)a);
 648         b2 = *((struct imagesection **)b);
 649
 650         if (b1->base_address == b2->base_address)
 651                 return 0;
 652         else if (b1->base_address > b2->base_address)
 653                 return 1;
 654         else
 655                 return -1;
 656 }
 657
 658 /**
 659  * Get aligned start address of a flash write region
 660  */
 661 target_addr_t flash_write_align_start(struct flash_bank *bank, target_addr_t addr)
 662 {
 663         if (addr < bank->base || addr >= bank->base + bank->size
 664                         || bank->write_start_alignment <= 1)
 665                 return addr;
 666
 667         if (bank->write_start_alignment == FLASH_WRITE_ALIGN_SECTOR) {
 668                 uint32_t offset = addr - bank->base;
 669                 uint32_t aligned = 0;
 670                 for (unsigned int sect = 0; sect < bank->num_sectors; sect++) {
 671                         if (bank->sectors[sect].offset > offset)
 672                                 break;
 673
 674                         aligned = bank->sectors[sect].offset;
 675                 }
 676                 return bank->base + aligned;
 677         }
 678
 679         return addr & ~(bank->write_start_alignment - 1);
 680 }
 681
 682 /**
 683  * Get aligned end address of a flash write region
 684  */
 685 target_addr_t flash_write_align_end(struct flash_bank *bank, target_addr_t addr)
 686 {
 687         if (addr < bank->base || addr >= bank->base + bank->size
 688                         || bank->write_end_alignment <= 1)
 689                 return addr;
 690
 691         if (bank->write_end_alignment == FLASH_WRITE_ALIGN_SECTOR) {
 692                 uint32_t offset = addr - bank->base;
 693                 uint32_t aligned = 0;
 694                 for (unsigned int sect = 0; sect < bank->num_sectors; sect++) {
 695                         aligned = bank->sectors[sect].offset + bank->sectors[sect].size - 1;
 696                         if (aligned >= offset)
 697                                 break;
 698                 }
 699                 return bank->base + aligned;
 700         }
 701
 702         return addr | (bank->write_end_alignment - 1);
 703 }
 704
 705 /**
 706  * Check if gap between sections is bigger than minimum required to discontinue flash write
 707  */
 708 static bool flash_write_check_gap(struct flash_bank *bank,
 709                                 target_addr_t addr1, target_addr_t addr2)
 710 {
 711         if (bank->minimal_write_gap == FLASH_WRITE_CONTINUOUS
 712                         || addr1 < bank->base || addr1 >= bank->base + bank->size
 713                         || addr2 < bank->base || addr2 >= bank->base + bank->size)
 714                 return false;
 715
 716         if (bank->minimal_write_gap == FLASH_WRITE_GAP_SECTOR) {
 717                 unsigned int sect;
 718                 uint32_t offset1 = addr1 - bank->base;
 719                 /* find the sector following the one containing addr1 */
 720                 for (sect = 0; sect < bank->num_sectors; sect++) {
 721                         if (bank->sectors[sect].offset > offset1)
 722                                 break;
 723                 }
 724                 if (sect >= bank->num_sectors)
 725                         return false;
 726
 727                 uint32_t offset2 = addr2 - bank->base;
 728                 return bank->sectors[sect].offset + bank->sectors[sect].size <= offset2;
 729         }
 730
 731         target_addr_t aligned1 = flash_write_align_end(bank, addr1);
 732         target_addr_t aligned2 = flash_write_align_start(bank, addr2);
 733         return aligned1 + bank->minimal_write_gap < aligned2;
 734 }
 735
 736
 737 int flash_write_unlock_verify(struct target *target, struct image *image,
 738         uint32_t *written, bool erase, bool unlock, bool write, bool verify)
 739 {
 740         int retval = ERROR_OK;
 741
 742         unsigned int section;
 743         uint32_t section_offset;
 744         struct flash_bank *c;
 745         int *padding;
 746
 747         section = 0;
 748         section_offset = 0;
 749
 750         if (written)
 751                 *written = 0;
 752
 753         if (erase) {
 754                 /* assume all sectors need erasing - stops any problems
 755                  * when flash_write is called multiple times */
 756
 757                 flash_set_dirty();
 758         }
 759
 760         /* allocate padding array */
 761         padding = calloc(image->num_sections, sizeof(*padding));
 762
 763         /* This fn requires all sections to be in ascending order of addresses,
 764          * whereas an image can have sections out of order. */
 765         struct imagesection **sections = malloc(sizeof(struct imagesection *) *
 766                         image->num_sections);
 767
 768         for (unsigned int i = 0; i < image->num_sections; i++)
 769                 sections[i] = &image->sections[i];
 770
 771         qsort(sections, image->num_sections, sizeof(struct imagesection *),
 772                 compare_section);
 773
 774         /* loop until we reach end of the image */
 775         while (section < image->num_sections) {
 776                 uint32_t buffer_idx;
 777                 uint8_t *buffer;
 778                 unsigned int section_last;
 779                 target_addr_t run_address = sections[section]->base_address + section_offset;
 780                 uint32_t run_size = sections[section]->size - section_offset;
 781                 int pad_bytes = 0;
 782
 783                 if (sections[section]->size ==  0) {
 784                         LOG_WARNING("empty section %d", section);
 785                         section++;
 786                         section_offset = 0;
 787                         continue;
 788                 }
 789
 790                 /* find the corresponding flash bank */
 791                 retval = get_flash_bank_by_addr(target, run_address, false, &c);
 792                 if (retval != ERROR_OK)
 793                         goto done;
 794                 if (!c) {
 795                         LOG_WARNING("no flash bank found for address " TARGET_ADDR_FMT, run_address);
 796                         section++;      /* and skip it */
 797                         section_offset = 0;
 798                         continue;
 799                 }
 800
 801                 /* collect consecutive sections which fall into the same bank */
 802                 section_last = section;
 803                 padding[section] = 0;
 804                 while ((run_address + run_size - 1 < c->base + c->size - 1) &&
 805                                 (section_last + 1 < image->num_sections)) {
 806                         /* sections are sorted */
 807                         assert(sections[section_last + 1]->base_address >= c->base);
 808                         if (sections[section_last + 1]->base_address >= (c->base + c->size)) {
 809                                 /* Done with this bank */
 810                                 break;
 811                         }
 812
 813                         /* if we have multiple sections within our image,
 814                          * flash programming could fail due to alignment issues
 815                          * attempt to rebuild a consecutive buffer for the flash loader */
 816                         target_addr_t run_next_addr = run_address + run_size;
 817                         target_addr_t next_section_base = sections[section_last + 1]->base_address;
 818                         if (next_section_base < run_next_addr) {
 819                                 LOG_ERROR("Section at " TARGET_ADDR_FMT
 820                                         " overlaps section ending at " TARGET_ADDR_FMT,
 821                                         next_section_base, run_next_addr);
 822                                 LOG_ERROR("Flash write aborted.");
 823                                 retval = ERROR_FAIL;
 824                                 goto done;
 825                         }
 826
 827                         pad_bytes = next_section_base - run_next_addr;
 828                         if (pad_bytes) {
 829                                 if (flash_write_check_gap(c, run_next_addr - 1, next_section_base)) {
 830                                         LOG_INFO("Flash write discontinued at " TARGET_ADDR_FMT
 831                                                 ", next section at " TARGET_ADDR_FMT,
 832                                                 run_next_addr, next_section_base);
 833                                         break;
 834                                 }
 835                         }
 836                         if (pad_bytes > 0)
 837                                 LOG_INFO("Padding image section %d at " TARGET_ADDR_FMT
 838                                         " with %d bytes",
 839                                         section_last, run_next_addr, pad_bytes);
 840
 841                         padding[section_last] = pad_bytes;
 842                         run_size += pad_bytes;
 843                         run_size += sections[++section_last]->size;
 844                 }
 845
 846                 if (run_address + run_size - 1 > c->base + c->size - 1) {
 847                         /* If we have more than one flash chip back to back, then we limit
 848                          * the current write operation to the current chip.
 849                          */
 850                         LOG_DEBUG("Truncate flash run size to the current flash chip.");
 851
 852                         run_size = c->base + c->size - run_address;
 853                         assert(run_size > 0);
 854                 }
 855
 856                 uint32_t padding_at_start = 0;
 857                 if (c->write_start_alignment || c->write_end_alignment) {
 858                         /* align write region according to bank requirements */
 859                         target_addr_t aligned_start = flash_write_align_start(c, run_address);
 860                         padding_at_start = run_address - aligned_start;
 861                         if (padding_at_start > 0) {
 862                                 LOG_WARNING("Section start address " TARGET_ADDR_FMT
 863                                         " breaks the required alignment of flash bank %s",
 864                                         run_address, c->name);
 865                                 LOG_WARNING("Padding %" PRIu32 " bytes from " TARGET_ADDR_FMT,
 866                                         padding_at_start, aligned_start);
 867
 868                                 run_address -= padding_at_start;
 869                                 run_size += padding_at_start;
 870                         }
 871
 872                         target_addr_t run_end = run_address + run_size - 1;
 873                         target_addr_t aligned_end = flash_write_align_end(c, run_end);
 874                         pad_bytes = aligned_end - run_end;
 875                         if (pad_bytes > 0) {
 876                                 LOG_INFO("Padding image section %d at " TARGET_ADDR_FMT
 877                                         " with %d bytes (bank write end alignment)",
 878                                         section_last, run_end + 1, pad_bytes);
 879
 880                                 padding[section_last] += pad_bytes;
 881                                 run_size += pad_bytes;
 882                         }
 883
 884                 } else if (unlock || erase) {
 885                         /* If we're applying any sector automagic, then pad this
 886                          * (maybe-combined) segment to the end of its last sector.
 887                          */
 888                         uint32_t offset_start = run_address - c->base;
 889                         uint32_t offset_end = offset_start + run_size;
 890                         uint32_t end = offset_end, delta;
 891
 892                         for (unsigned int sector = 0; sector < c->num_sectors; sector++) {
 893                                 end = c->sectors[sector].offset
 894                                         + c->sectors[sector].size;
 895                                 if (offset_end <= end)
 896                                         break;
 897                         }
 898
 899                         delta = end - offset_end;
 900                         padding[section_last] += delta;
 901                         run_size += delta;
 902                 }
 903
 904                 /* allocate buffer */
 905                 buffer = malloc(run_size);
 906                 if (!buffer) {
 907                         LOG_ERROR("Out of memory for flash bank buffer");
 908                         retval = ERROR_FAIL;
 909                         goto done;
 910                 }
 911
 912                 if (padding_at_start)
 913                         memset(buffer, c->default_padded_value, padding_at_start);
 914
 915                 buffer_idx = padding_at_start;
 916
 917                 /* read sections to the buffer */
 918                 while (buffer_idx < run_size) {
 919                         size_t size_read;
 920
 921                         size_read = run_size - buffer_idx;
 922                         if (size_read > sections[section]->size - section_offset)
 923                                 size_read = sections[section]->size - section_offset;
 924
 925                         /* KLUDGE!
 926                          *
 927                          * #¤%#"%¤% we have to figure out the section # from the sorted
 928                          * list of pointers to sections to invoke image_read_section()...
 929                          */
 930                         intptr_t diff = (intptr_t)sections[section] - (intptr_t)image->sections;
 931                         int t_section_num = diff / sizeof(struct imagesection);
 932
 933                         LOG_DEBUG("image_read_section: section = %d, t_section_num = %d, "
 934                                         "section_offset = %"PRIu32", buffer_idx = %"PRIu32", size_read = %zu",
 935                                 section, t_section_num, section_offset,
 936                                 buffer_idx, size_read);
 937                         retval = image_read_section(image, t_section_num, section_offset,
 938                                         size_read, buffer + buffer_idx, &size_read);
 939                         if (retval != ERROR_OK || size_read == 0) {
 940                                 free(buffer);
 941                                 goto done;
 942                         }
 943
 944                         buffer_idx += size_read;
 945                         section_offset += size_read;
 946
 947                         /* see if we need to pad the section */
 948                         if (padding[section]) {
 949                                 memset(buffer + buffer_idx, c->default_padded_value, padding[section]);
 950                                 buffer_idx += padding[section];
 951                         }
 952
 953                         if (section_offset >= sections[section]->size) {
 954                                 section++;
 955                                 section_offset = 0;
 956                         }
 957                 }
 958
 959                 retval = ERROR_OK;
 960
 961                 if (unlock)
 962                         retval = flash_unlock_address_range(target, run_address, run_size);
 963                 if (retval == ERROR_OK) {
 964                         if (erase) {
 965                                 /* calculate and erase sectors */
 966                                 retval = flash_erase_address_range(target,
 967                                                 true, run_address, run_size);
 968                         }
 969                 }
 970
 971                 if (retval == ERROR_OK) {
 972                         if (write) {
 973                                 /* write flash sectors */
 974                                 retval = flash_driver_write(c, buffer, run_address - c->base, run_size);
 975                         }
 976                 }
 977
 978                 if (retval == ERROR_OK) {
 979                         if (verify) {
 980                                 /* verify flash sectors */
 981                                 retval = flash_driver_verify(c, buffer, run_address - c->base, run_size);
 982                         }
 983                 }
 984
 985                 free(buffer);
 986
 987                 if (retval != ERROR_OK) {
 988                         /* abort operation */
 989                         goto done;
 990                 }
 991
 992                 if (written)
 993                         *written += run_size;   /* add run size to total written counter */
 994         }
 995
 996 done:
 997         free(sections);
 998         free(padding);
 999
1000         return retval;
1001 }
1002
1003 int flash_write(struct target *target, struct image *image,
1004         uint32_t *written, bool erase)
1005 {
1006         return flash_write_unlock_verify(target, image, written, erase, false, true, false);
1007 }
1008
1009 struct flash_sector *alloc_block_array(uint32_t offset, uint32_t size,
1010                 unsigned int num_blocks)
1011 {
1012         struct flash_sector *array = calloc(num_blocks, sizeof(struct flash_sector));
1013         if (!array)
1014                 return NULL;
1015
1016         for (unsigned int i = 0; i < num_blocks; i++) {
1017                 array[i].offset = offset;
1018                 array[i].size = size;
1019                 array[i].is_erased = -1;
1020                 array[i].is_protected = -1;
1021                 offset += size;
1022         }
1023
1024         return array;
1025 }