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                 if (retval == ERROR_NOT_IMPLEMENTED)
 433                         LOG_USER("Running slow fallback erase check");
 434                 else
 435                         LOG_USER("Running slow fallback erase check - add working memory");
 436
 437                 retval = default_flash_mem_blank_check(bank);
 438         }
 439         free(block_array);
 440
 441         return retval;
 442 }
 443
 444 /* Manipulate given flash region, selecting the bank according to target
 445  * and address.  Maps an address range to a set of sectors, and issues
 446  * the callback() on that set ... e.g. to erase or unprotect its members.
 447  *
 448  * Parameter iterate_protect_blocks switches iteration of protect block
 449  * instead of erase sectors. If there is no protect blocks array, sectors
 450  * are used in iteration, so compatibility for old flash drivers is retained.
 451  *
 452  * The "pad_reason" parameter is a kind of boolean:  when it's NULL, the
 453  * range must fit those sectors exactly.  This is clearly safe; it can't
 454  * erase data which the caller said to leave alone, for example.  If it's
 455  * non-NULL, rather than failing, extra data in the first and/or last
 456  * sectors will be added to the range, and that reason string is used when
 457  * warning about those additions.
 458  */
 459 static int flash_iterate_address_range_inner(struct target *target,
 460         char *pad_reason, target_addr_t addr, uint32_t length,
 461         bool iterate_protect_blocks,
 462         int (*callback)(struct flash_bank *bank, unsigned int first,
 463                 unsigned int last))
 464 {
 465         struct flash_bank *c;
 466         struct flash_sector *block_array;
 467         target_addr_t last_addr = addr + length - 1;    /* the last address of range */
 468         int first = -1;
 469         int last = -1;
 470         int i;
 471         int num_blocks;
 472
 473         int retval = get_flash_bank_by_addr(target, addr, true, &c);
 474         if (retval != ERROR_OK)
 475                 return retval;
 476
 477         if (c->size == 0 || c->num_sectors == 0) {
 478                 LOG_ERROR("Bank is invalid");
 479                 return ERROR_FLASH_BANK_INVALID;
 480         }
 481
 482         if (length == 0) {
 483                 /* special case, erase whole bank when length is zero */
 484                 if (addr != c->base) {
 485                         LOG_ERROR("Whole bank access must start at beginning of bank.");
 486                         return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
 487                 }
 488
 489                 return callback(c, 0, c->num_sectors - 1);
 490         }
 491
 492         /* check whether it all fits in this bank */
 493         if (last_addr > c->base + c->size - 1) {
 494                 LOG_ERROR("Flash access does not fit into bank.");
 495                 return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
 496         }
 497
 498         if (!c->prot_blocks || c->num_prot_blocks == 0) {
 499                 /* flash driver does not define protect blocks, use sectors instead */
 500                 iterate_protect_blocks = false;
 501         }
 502
 503         if (iterate_protect_blocks) {
 504                 block_array = c->prot_blocks;
 505                 num_blocks = c->num_prot_blocks;
 506         } else {
 507                 block_array = c->sectors;
 508                 num_blocks = c->num_sectors;
 509         }
 510
 511         for (i = 0; i < num_blocks; i++) {
 512                 struct flash_sector *f = &block_array[i];
 513                 target_addr_t sector_addr = c->base + f->offset;
 514                 target_addr_t sector_last_addr = sector_addr + f->size - 1;
 515
 516                 /* start only on a sector boundary */
 517                 if (first < 0) {
 518                         /* scanned past the first sector? */
 519                         if (addr < sector_addr)
 520                                 break;
 521
 522                         /* is this the first sector? */
 523                         if (addr == sector_addr)
 524                                 first = i;
 525
 526                         /* Does this need head-padding?  If so, pad and warn;
 527                          * or else force an error.
 528                          *
 529                          * Such padding can make trouble, since *WE* can't
 530                          * ever know if that data was in use.  The warning
 531                          * should help users sort out messes later.
 532                          */
 533                         else if (addr <= sector_last_addr && pad_reason) {
 534                                 /* FIXME say how many bytes (e.g. 80 KB) */
 535                                 LOG_WARNING("Adding extra %s range, "
 536                                         TARGET_ADDR_FMT " .. " TARGET_ADDR_FMT,
 537                                         pad_reason,
 538                                         sector_addr,
 539                                         addr - 1);
 540                                 first = i;
 541                         } else
 542                                 continue;
 543                 }
 544
 545                 /* is this (also?) the last sector? */
 546                 if (last_addr == sector_last_addr) {
 547                         last = i;
 548                         break;
 549                 }
 550
 551                 /* Does this need tail-padding?  If so, pad and warn;
 552                  * or else force an error.
 553                  */
 554                 if (last_addr < sector_last_addr && pad_reason) {
 555                         /* FIXME say how many bytes (e.g. 80 KB) */
 556                         LOG_WARNING("Adding extra %s range, "
 557                                 TARGET_ADDR_FMT " .. " TARGET_ADDR_FMT,
 558                                 pad_reason,
 559                                 last_addr + 1,
 560                                 sector_last_addr);
 561                         last = i;
 562                         break;
 563                 }
 564
 565                 /* MUST finish on a sector boundary */
 566                 if (last_addr < sector_addr)
 567                         break;
 568         }
 569
 570         /* invalid start or end address? */
 571         if (first == -1 || last == -1) {
 572                 LOG_ERROR("address range " TARGET_ADDR_FMT " .. " TARGET_ADDR_FMT
 573                         " is not sector-aligned",
 574                         addr,
 575                         last_addr);
 576                 return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
 577         }
 578
 579         /* The NOR driver may trim this range down, based on what
 580          * sectors are already erased/unprotected.  GDB currently
 581          * blocks such optimizations.
 582          */
 583         return callback(c, first, last);
 584 }
 585
 586 /* The inner fn only handles a single bank, we could be spanning
 587  * multiple chips.
 588  */
 589 static int flash_iterate_address_range(struct target *target,
 590         char *pad_reason, target_addr_t addr, uint32_t length,
 591         bool iterate_protect_blocks,
 592         int (*callback)(struct flash_bank *bank, unsigned int first,
 593                 unsigned int last))
 594 {
 595         struct flash_bank *c;
 596         int retval = ERROR_OK;
 597
 598         /* Danger! zero-length iterations means entire bank! */
 599         do {
 600                 retval = get_flash_bank_by_addr(target, addr, true, &c);
 601                 if (retval != ERROR_OK)
 602                         return retval;
 603
 604                 uint32_t cur_length = length;
 605                 /* check whether it all fits in this bank */
 606                 if (addr + length - 1 > c->base + c->size - 1) {
 607                         LOG_DEBUG("iterating over more than one flash bank.");
 608                         cur_length = c->base + c->size - addr;
 609                 }
 610                 retval = flash_iterate_address_range_inner(target,
 611                                 pad_reason, addr, cur_length,
 612                                 iterate_protect_blocks,
 613                                 callback);
 614                 if (retval != ERROR_OK)
 615                         break;
 616
 617                 length -= cur_length;
 618                 addr += cur_length;
 619         } while (length > 0);
 620
 621         return retval;
 622 }
 623
 624 int flash_erase_address_range(struct target *target,
 625         bool pad, target_addr_t addr, uint32_t length)
 626 {
 627         return flash_iterate_address_range(target, pad ? "erase" : NULL,
 628                 addr, length, false, &flash_driver_erase);
 629 }
 630
 631 static int flash_driver_unprotect(struct flash_bank *bank, unsigned int first,
 632                 unsigned int last)
 633 {
 634         return flash_driver_protect(bank, 0, first, last);
 635 }
 636
 637 int flash_unlock_address_range(struct target *target, target_addr_t addr,
 638                 uint32_t length)
 639 {
 640         /* By default, pad to sector boundaries ... the real issue here
 641          * is that our (only) caller *permanently* removes protection,
 642          * and doesn't restore it.
 643          */
 644         return flash_iterate_address_range(target, "unprotect",
 645                 addr, length, true, &flash_driver_unprotect);
 646 }
 647
 648 static int compare_section(const void *a, const void *b)
 649 {
 650         struct imagesection *b1, *b2;
 651         b1 = *((struct imagesection **)a);
 652         b2 = *((struct imagesection **)b);
 653
 654         if (b1->base_address == b2->base_address)
 655                 return 0;
 656         else if (b1->base_address > b2->base_address)
 657                 return 1;
 658         else
 659                 return -1;
 660 }
 661
 662 /**
 663  * Get aligned start address of a flash write region
 664  */
 665 target_addr_t flash_write_align_start(struct flash_bank *bank, target_addr_t addr)
 666 {
 667         if (addr < bank->base || addr >= bank->base + bank->size
 668                         || bank->write_start_alignment <= 1)
 669                 return addr;
 670
 671         if (bank->write_start_alignment == FLASH_WRITE_ALIGN_SECTOR) {
 672                 uint32_t offset = addr - bank->base;
 673                 uint32_t aligned = 0;
 674                 for (unsigned int sect = 0; sect < bank->num_sectors; sect++) {
 675                         if (bank->sectors[sect].offset > offset)
 676                                 break;
 677
 678                         aligned = bank->sectors[sect].offset;
 679                 }
 680                 return bank->base + aligned;
 681         }
 682
 683         return addr & ~(bank->write_start_alignment - 1);
 684 }
 685
 686 /**
 687  * Get aligned end address of a flash write region
 688  */
 689 target_addr_t flash_write_align_end(struct flash_bank *bank, target_addr_t addr)
 690 {
 691         if (addr < bank->base || addr >= bank->base + bank->size
 692                         || bank->write_end_alignment <= 1)
 693                 return addr;
 694
 695         if (bank->write_end_alignment == FLASH_WRITE_ALIGN_SECTOR) {
 696                 uint32_t offset = addr - bank->base;
 697                 uint32_t aligned = 0;
 698                 for (unsigned int sect = 0; sect < bank->num_sectors; sect++) {
 699                         aligned = bank->sectors[sect].offset + bank->sectors[sect].size - 1;
 700                         if (aligned >= offset)
 701                                 break;
 702                 }
 703                 return bank->base + aligned;
 704         }
 705
 706         return addr | (bank->write_end_alignment - 1);
 707 }
 708
 709 /**
 710  * Check if gap between sections is bigger than minimum required to discontinue flash write
 711  */
 712 static bool flash_write_check_gap(struct flash_bank *bank,
 713                                 target_addr_t addr1, target_addr_t addr2)
 714 {
 715         if (bank->minimal_write_gap == FLASH_WRITE_CONTINUOUS
 716                         || addr1 < bank->base || addr1 >= bank->base + bank->size
 717                         || addr2 < bank->base || addr2 >= bank->base + bank->size)
 718                 return false;
 719
 720         if (bank->minimal_write_gap == FLASH_WRITE_GAP_SECTOR) {
 721                 unsigned int sect;
 722                 uint32_t offset1 = addr1 - bank->base;
 723                 /* find the sector following the one containing addr1 */
 724                 for (sect = 0; sect < bank->num_sectors; sect++) {
 725                         if (bank->sectors[sect].offset > offset1)
 726                                 break;
 727                 }
 728                 if (sect >= bank->num_sectors)
 729                         return false;
 730
 731                 uint32_t offset2 = addr2 - bank->base;
 732                 return bank->sectors[sect].offset + bank->sectors[sect].size <= offset2;
 733         }
 734
 735         target_addr_t aligned1 = flash_write_align_end(bank, addr1);
 736         target_addr_t aligned2 = flash_write_align_start(bank, addr2);
 737         return aligned1 + bank->minimal_write_gap < aligned2;
 738 }
 739
 740
 741 int flash_write_unlock_verify(struct target *target, struct image *image,
 742         uint32_t *written, bool erase, bool unlock, bool write, bool verify)
 743 {
 744         int retval = ERROR_OK;
 745
 746         unsigned int section;
 747         uint32_t section_offset;
 748         struct flash_bank *c;
 749         int *padding;
 750
 751         section = 0;
 752         section_offset = 0;
 753
 754         if (written)
 755                 *written = 0;
 756
 757         if (erase) {
 758                 /* assume all sectors need erasing - stops any problems
 759                  * when flash_write is called multiple times */
 760
 761                 flash_set_dirty();
 762         }
 763
 764         /* allocate padding array */
 765         padding = calloc(image->num_sections, sizeof(*padding));
 766
 767         /* This fn requires all sections to be in ascending order of addresses,
 768          * whereas an image can have sections out of order. */
 769         struct imagesection **sections = malloc(sizeof(struct imagesection *) *
 770                         image->num_sections);
 771
 772         for (unsigned int i = 0; i < image->num_sections; i++)
 773                 sections[i] = &image->sections[i];
 774
 775         qsort(sections, image->num_sections, sizeof(struct imagesection *),
 776                 compare_section);
 777
 778         /* loop until we reach end of the image */
 779         while (section < image->num_sections) {
 780                 uint32_t buffer_idx;
 781                 uint8_t *buffer;
 782                 unsigned int section_last;
 783                 target_addr_t run_address = sections[section]->base_address + section_offset;
 784                 uint32_t run_size = sections[section]->size - section_offset;
 785                 int pad_bytes = 0;
 786
 787                 if (sections[section]->size ==  0) {
 788                         LOG_WARNING("empty section %d", section);
 789                         section++;
 790                         section_offset = 0;
 791                         continue;
 792                 }
 793
 794                 /* find the corresponding flash bank */
 795                 retval = get_flash_bank_by_addr(target, run_address, false, &c);
 796                 if (retval != ERROR_OK)
 797                         goto done;
 798                 if (!c) {
 799                         LOG_WARNING("no flash bank found for address " TARGET_ADDR_FMT, run_address);
 800                         section++;      /* and skip it */
 801                         section_offset = 0;
 802                         continue;
 803                 }
 804
 805                 /* collect consecutive sections which fall into the same bank */
 806                 section_last = section;
 807                 padding[section] = 0;
 808                 while ((run_address + run_size - 1 < c->base + c->size - 1) &&
 809                                 (section_last + 1 < image->num_sections)) {
 810                         /* sections are sorted */
 811                         assert(sections[section_last + 1]->base_address >= c->base);
 812                         if (sections[section_last + 1]->base_address >= (c->base + c->size)) {
 813                                 /* Done with this bank */
 814                                 break;
 815                         }
 816
 817                         /* if we have multiple sections within our image,
 818                          * flash programming could fail due to alignment issues
 819                          * attempt to rebuild a consecutive buffer for the flash loader */
 820                         target_addr_t run_next_addr = run_address + run_size;
 821                         target_addr_t next_section_base = sections[section_last + 1]->base_address;
 822                         if (next_section_base < run_next_addr) {
 823                                 LOG_ERROR("Section at " TARGET_ADDR_FMT
 824                                         " overlaps section ending at " TARGET_ADDR_FMT,
 825                                         next_section_base, run_next_addr);
 826                                 LOG_ERROR("Flash write aborted.");
 827                                 retval = ERROR_FAIL;
 828                                 goto done;
 829                         }
 830
 831                         pad_bytes = next_section_base - run_next_addr;
 832                         if (pad_bytes) {
 833                                 if (flash_write_check_gap(c, run_next_addr - 1, next_section_base)) {
 834                                         LOG_INFO("Flash write discontinued at " TARGET_ADDR_FMT
 835                                                 ", next section at " TARGET_ADDR_FMT,
 836                                                 run_next_addr, next_section_base);
 837                                         break;
 838                                 }
 839                         }
 840                         if (pad_bytes > 0)
 841                                 LOG_INFO("Padding image section %d at " TARGET_ADDR_FMT
 842                                         " with %d bytes",
 843                                         section_last, run_next_addr, pad_bytes);
 844
 845                         padding[section_last] = pad_bytes;
 846                         run_size += pad_bytes;
 847                         run_size += sections[++section_last]->size;
 848                 }
 849
 850                 if (run_address + run_size - 1 > c->base + c->size - 1) {
 851                         /* If we have more than one flash chip back to back, then we limit
 852                          * the current write operation to the current chip.
 853                          */
 854                         LOG_DEBUG("Truncate flash run size to the current flash chip.");
 855
 856                         run_size = c->base + c->size - run_address;
 857                         assert(run_size > 0);
 858                 }
 859
 860                 uint32_t padding_at_start = 0;
 861                 if (c->write_start_alignment || c->write_end_alignment) {
 862                         /* align write region according to bank requirements */
 863                         target_addr_t aligned_start = flash_write_align_start(c, run_address);
 864                         padding_at_start = run_address - aligned_start;
 865                         if (padding_at_start > 0) {
 866                                 LOG_WARNING("Section start address " TARGET_ADDR_FMT
 867                                         " breaks the required alignment of flash bank %s",
 868                                         run_address, c->name);
 869                                 LOG_WARNING("Padding %" PRIu32 " bytes from " TARGET_ADDR_FMT,
 870                                         padding_at_start, aligned_start);
 871
 872                                 run_address -= padding_at_start;
 873                                 run_size += padding_at_start;
 874                         }
 875
 876                         target_addr_t run_end = run_address + run_size - 1;
 877                         target_addr_t aligned_end = flash_write_align_end(c, run_end);
 878                         pad_bytes = aligned_end - run_end;
 879                         if (pad_bytes > 0) {
 880                                 LOG_INFO("Padding image section %d at " TARGET_ADDR_FMT
 881                                         " with %d bytes (bank write end alignment)",
 882                                         section_last, run_end + 1, pad_bytes);
 883
 884                                 padding[section_last] += pad_bytes;
 885                                 run_size += pad_bytes;
 886                         }
 887
 888                 } else if (unlock || erase) {
 889                         /* If we're applying any sector automagic, then pad this
 890                          * (maybe-combined) segment to the end of its last sector.
 891                          */
 892                         uint32_t offset_start = run_address - c->base;
 893                         uint32_t offset_end = offset_start + run_size;
 894                         uint32_t end = offset_end, delta;
 895
 896                         for (unsigned int sector = 0; sector < c->num_sectors; sector++) {
 897                                 end = c->sectors[sector].offset
 898                                         + c->sectors[sector].size;
 899                                 if (offset_end <= end)
 900                                         break;
 901                         }
 902
 903                         delta = end - offset_end;
 904                         padding[section_last] += delta;
 905                         run_size += delta;
 906                 }
 907
 908                 /* allocate buffer */
 909                 buffer = malloc(run_size);
 910                 if (!buffer) {
 911                         LOG_ERROR("Out of memory for flash bank buffer");
 912                         retval = ERROR_FAIL;
 913                         goto done;
 914                 }
 915
 916                 if (padding_at_start)
 917                         memset(buffer, c->default_padded_value, padding_at_start);
 918
 919                 buffer_idx = padding_at_start;
 920
 921                 /* read sections to the buffer */
 922                 while (buffer_idx < run_size) {
 923                         size_t size_read;
 924
 925                         size_read = run_size - buffer_idx;
 926                         if (size_read > sections[section]->size - section_offset)
 927                                 size_read = sections[section]->size - section_offset;
 928
 929                         /* KLUDGE!
 930                          *
 931                          * #¤%#"%¤% we have to figure out the section # from the sorted
 932                          * list of pointers to sections to invoke image_read_section()...
 933                          */
 934                         intptr_t diff = (intptr_t)sections[section] - (intptr_t)image->sections;
 935                         int t_section_num = diff / sizeof(struct imagesection);
 936
 937                         LOG_DEBUG("image_read_section: section = %d, t_section_num = %d, "
 938                                         "section_offset = %"PRIu32", buffer_idx = %"PRIu32", size_read = %zu",
 939                                 section, t_section_num, section_offset,
 940                                 buffer_idx, size_read);
 941                         retval = image_read_section(image, t_section_num, section_offset,
 942                                         size_read, buffer + buffer_idx, &size_read);
 943                         if (retval != ERROR_OK || size_read == 0) {
 944                                 free(buffer);
 945                                 goto done;
 946                         }
 947
 948                         buffer_idx += size_read;
 949                         section_offset += size_read;
 950
 951                         /* see if we need to pad the section */
 952                         if (padding[section]) {
 953                                 memset(buffer + buffer_idx, c->default_padded_value, padding[section]);
 954                                 buffer_idx += padding[section];
 955                         }
 956
 957                         if (section_offset >= sections[section]->size) {
 958                                 section++;
 959                                 section_offset = 0;
 960                         }
 961                 }
 962
 963                 retval = ERROR_OK;
 964
 965                 if (unlock)
 966                         retval = flash_unlock_address_range(target, run_address, run_size);
 967                 if (retval == ERROR_OK) {
 968                         if (erase) {
 969                                 /* calculate and erase sectors */
 970                                 retval = flash_erase_address_range(target,
 971                                                 true, run_address, run_size);
 972                         }
 973                 }
 974
 975                 if (retval == ERROR_OK) {
 976                         if (write) {
 977                                 /* write flash sectors */
 978                                 retval = flash_driver_write(c, buffer, run_address - c->base, run_size);
 979                         }
 980                 }
 981
 982                 if (retval == ERROR_OK) {
 983                         if (verify) {
 984                                 /* verify flash sectors */
 985                                 retval = flash_driver_verify(c, buffer, run_address - c->base, run_size);
 986                         }
 987                 }
 988
 989                 free(buffer);
 990
 991                 if (retval != ERROR_OK) {
 992                         /* abort operation */
 993                         goto done;
 994                 }
 995
 996                 if (written)
 997                         *written += run_size;   /* add run size to total written counter */
 998         }
 999
1000 done:
1001         free(sections);
1002         free(padding);
1003
1004         return retval;
1005 }
1006
1007 int flash_write(struct target *target, struct image *image,
1008         uint32_t *written, bool erase)
1009 {
1010         return flash_write_unlock_verify(target, image, written, erase, false, true, false);
1011 }
1012
1013 struct flash_sector *alloc_block_array(uint32_t offset, uint32_t size,
1014                 unsigned int num_blocks)
1015 {
1016         struct flash_sector *array = calloc(num_blocks, sizeof(struct flash_sector));
1017         if (!array)
1018                 return NULL;
1019
1020         for (unsigned int i = 0; i < num_blocks; i++) {
1021                 array[i].offset = offset;
1022                 array[i].size = size;
1023                 array[i].is_erased = -1;
1024                 array[i].is_protected = -1;
1025                 offset += size;
1026         }
1027
1028         return array;
1029 }