git.sur5r.net Git - openocd/blob - src/flash/nor/stm32l4x.c

   1 /***************************************************************************
   2  *   Copyright (C) 2015 by Uwe Bonnes                                      *
   3  *   bon@elektron.ikp.physik.tu-darmstadt.de                               *
   4  *
   5  *   This program is free software; you can redistribute it and/or modify  *
   6  *   it under the terms of the GNU General Public License as published by  *
   7  *   the Free Software Foundation; either version 2 of the License, or     *
   8  *   (at your option) any later version.                                   *
   9  *                                                                         *
  10  *   This program is distributed in the hope that it will be useful,       *
  11  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  12  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  13  *   GNU General Public License for more details.                          *
  14  *                                                                         *
  15  *   You should have received a copy of the GNU General Public License     *
  16  *   along with this program; if not, write to the                         *
  17  *   Free Software Foundation, Inc.                                        *
  18  ***************************************************************************/
  19
  20 #ifdef HAVE_CONFIG_H
  21 #include "config.h"
  22 #endif
  23
  24 #include "imp.h"
  25 #include <helper/binarybuffer.h>
  26 #include <target/algorithm.h>
  27 #include <target/armv7m.h>
  28
  29 /* STM32L4xxx series for reference.
  30  *
  31  * RM0351
  32  * http://www.st.com/st-web-ui/static/active/en/resource/technical/document/reference_manual/DM00083560.pdf
  33  *
  34  * STM32L476RG Datasheet (for erase timing)
  35  * http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/DM00108832.pdf
  36  *
  37  *
  38  * The device has normally two banks, but on 512 and 256 kiB devices an
  39  * option byte is available to map all sectors to the first bank.
  40  * Both STM32 banks are treated as one OpenOCD bank, as other STM32 devices
  41  * handlers do!
  42  *
  43  */
  44
  45 /* Erase time can be as high as 25ms, 10x this and assume it's toast... */
  46
  47 #define FLASH_ERASE_TIMEOUT 250
  48
  49 #define STM32_FLASH_BASE    0x40022000
  50 #define STM32_FLASH_ACR     0x40022000
  51 #define STM32_FLASH_KEYR    0x40022008
  52 #define STM32_FLASH_OPTKEYR 0x4002200c
  53 #define STM32_FLASH_SR      0x40022010
  54 #define STM32_FLASH_CR      0x40022014
  55 #define STM32_FLASH_OPTR    0x40022020
  56 #define STM32_FLASH_WRP1AR  0x4002202c
  57 #define STM32_FLASH_WRP2AR  0x40022030
  58 #define STM32_FLASH_WRP1BR  0x4002204c
  59 #define STM32_FLASH_WRP2BR  0x40022050
  60
  61 /* FLASH_CR register bits */
  62
  63 #define FLASH_PG       (1 << 0)
  64 #define FLASH_PER      (1 << 1)
  65 #define FLASH_MER1     (1 << 2)
  66 #define FLASH_PAGE_SHIFT     3
  67 #define FLASH_CR_BKER  (1 << 11)
  68 #define FLASH_MER2     (1 << 15)
  69 #define FLASH_STRT     (1 << 16)
  70 #define FLASH_EOPIE    (1 << 24)
  71 #define FLASH_ERRIE    (1 << 25)
  72 #define FLASH_OPTLOCK  (1 << 30)
  73 #define FLASH_LOCK     (1 << 31)
  74
  75 /* FLASH_SR register bits */
  76
  77 #define FLASH_BSY      (1 << 16)
  78 /* Fast programming not used => related errors not used*/
  79 #define FLASH_PGSERR   (1 << 7) /* Programming sequence error */
  80 #define FLASH_SIZERR   (1 << 6) /* Size  error */
  81 #define FLASH_PGAERR   (1 << 5) /* Programming alignment error */
  82 #define FLASH_WRPERR   (1 << 4) /* Write protection error */
  83 #define FLASH_PROGERR  (1 << 3) /* Programming error */
  84 #define FLASH_OPERR    (1 << 1) /* Operation error */
  85 #define FLASH_EOP      (1 << 0) /* End of operation */
  86
  87 #define FLASH_ERROR (FLASH_PGSERR | FLASH_PGSERR | FLASH_PGAERR | FLASH_WRPERR | FLASH_OPERR)
  88
  89 /* STM32_FLASH_OBR bit definitions (reading) */
  90
  91 #define OPT_DUALBANK   21       /* dual flash bank only */
  92
  93 /* register unlock keys */
  94
  95 #define KEY1           0x45670123
  96 #define KEY2           0xCDEF89AB
  97
  98 /* option register unlock key */
  99 #define OPTKEY1        0x08192A3B
 100 #define OPTKEY2        0x4C5D6E7F
 101
 102
 103 /* other registers */
 104 #define DBGMCU_IDCODE   0xE0042000
 105 #define FLASH_SIZE_REG  0x1FFF75E0
 106
 107 struct stm32l4_options {
 108         uint8_t RDP;
 109         uint16_t bank_b_start;
 110         uint8_t user_options;
 111         uint8_t wpr1a_start;
 112         uint8_t wpr1a_end;
 113         uint8_t wpr1b_start;
 114         uint8_t wpr1b_end;
 115         uint8_t wpr2a_start;
 116         uint8_t wpr2a_end;
 117         uint8_t wpr2b_start;
 118         uint8_t wpr2b_end;
 119     /* Fixme: Handle PCROP */
 120 };
 121
 122 struct stm32l4_flash_bank {
 123         struct stm32l4_options option_bytes;
 124         int probed;
 125 };
 126
 127 /* flash bank stm32l4x <base> <size> 0 0 <target#>
 128  */
 129 FLASH_BANK_COMMAND_HANDLER(stm32l4_flash_bank_command)
 130 {
 131         struct stm32l4_flash_bank *stm32l4_info;
 132
 133         if (CMD_ARGC < 6)
 134                 return ERROR_COMMAND_SYNTAX_ERROR;
 135
 136         stm32l4_info = malloc(sizeof(struct stm32l4_flash_bank));
 137         if (!stm32l4_info)
 138                 return ERROR_FAIL; /* Checkme: What better error to use?*/
 139         bank->driver_priv = stm32l4_info;
 140
 141         stm32l4_info->probed = 0;
 142
 143         return ERROR_OK;
 144 }
 145
 146 static inline int stm32l4_get_flash_reg(struct flash_bank *bank, uint32_t reg)
 147 {
 148         return reg;
 149 }
 150
 151 static inline int stm32l4_get_flash_status(struct flash_bank *bank, uint32_t *status)
 152 {
 153         struct target *target = bank->target;
 154         return target_read_u32(
 155                 target, stm32l4_get_flash_reg(bank, STM32_FLASH_SR), status);
 156 }
 157
 158 static int stm32l4_wait_status_busy(struct flash_bank *bank, int timeout)
 159 {
 160         struct target *target = bank->target;
 161         uint32_t status;
 162         int retval = ERROR_OK;
 163
 164         /* wait for busy to clear */
 165         for (;;) {
 166                 retval = stm32l4_get_flash_status(bank, &status);
 167                 if (retval != ERROR_OK)
 168                         return retval;
 169                 LOG_DEBUG("status: 0x%" PRIx32 "", status);
 170                 if ((status & FLASH_BSY) == 0)
 171                         break;
 172                 if (timeout-- <= 0) {
 173                         LOG_ERROR("timed out waiting for flash");
 174                         return ERROR_FAIL;
 175                 }
 176                 alive_sleep(1);
 177         }
 178
 179
 180         if (status & FLASH_WRPERR) {
 181                 LOG_ERROR("stm32x device protected");
 182                 retval = ERROR_FAIL;
 183         }
 184
 185         /* Clear but report errors */
 186         if (status & FLASH_ERROR) {
 187                 /* If this operation fails, we ignore it and report the original
 188                  * retval
 189                  */
 190                 target_write_u32(target, stm32l4_get_flash_reg(bank, STM32_FLASH_SR),
 191                                 status & FLASH_ERROR);
 192         }
 193         return retval;
 194 }
 195
 196 static int stm32l4_unlock_reg(struct target *target)
 197 {
 198         uint32_t ctrl;
 199
 200         /* first check if not already unlocked
 201          * otherwise writing on STM32_FLASH_KEYR will fail
 202          */
 203         int retval = target_read_u32(target, STM32_FLASH_CR, &ctrl);
 204         if (retval != ERROR_OK)
 205                 return retval;
 206
 207         if ((ctrl & FLASH_LOCK) == 0)
 208                 return ERROR_OK;
 209
 210         /* unlock flash registers */
 211         retval = target_write_u32(target, STM32_FLASH_KEYR, KEY1);
 212         if (retval != ERROR_OK)
 213                 return retval;
 214
 215         retval = target_write_u32(target, STM32_FLASH_KEYR, KEY2);
 216         if (retval != ERROR_OK)
 217                 return retval;
 218
 219         retval = target_read_u32(target, STM32_FLASH_CR, &ctrl);
 220         if (retval != ERROR_OK)
 221                 return retval;
 222
 223         if (ctrl & FLASH_LOCK) {
 224                 LOG_ERROR("flash not unlocked STM32_FLASH_CR: %" PRIx32, ctrl);
 225                 return ERROR_TARGET_FAILURE;
 226         }
 227
 228         return ERROR_OK;
 229 }
 230
 231 static int stm32l4_unlock_option_reg(struct target *target)
 232 {
 233         uint32_t ctrl;
 234
 235         int retval = target_read_u32(target, STM32_FLASH_CR, &ctrl);
 236         if (retval != ERROR_OK)
 237                 return retval;
 238
 239         if ((ctrl & FLASH_OPTLOCK) == 0)
 240                 return ERROR_OK;
 241
 242         /* unlock option registers */
 243         retval = target_write_u32(target, STM32_FLASH_OPTKEYR, OPTKEY1);
 244         if (retval != ERROR_OK)
 245                 return retval;
 246
 247         retval = target_write_u32(target, STM32_FLASH_OPTKEYR, OPTKEY2);
 248         if (retval != ERROR_OK)
 249                 return retval;
 250
 251         retval = target_read_u32(target, STM32_FLASH_CR, &ctrl);
 252         if (retval != ERROR_OK)
 253                 return retval;
 254
 255         if (ctrl & FLASH_OPTLOCK) {
 256                 LOG_ERROR("options not unlocked STM32_FLASH_CR: %" PRIx32, ctrl);
 257                 return ERROR_TARGET_FAILURE;
 258         }
 259
 260         return ERROR_OK;
 261 }
 262
 263 static int stm32l4_read_options(struct flash_bank *bank)
 264 {
 265         uint32_t optiondata;
 266         struct stm32l4_flash_bank *stm32l4_info = NULL;
 267         struct target *target = bank->target;
 268
 269         stm32l4_info = bank->driver_priv;
 270
 271         /* read current option bytes */
 272         int retval = target_read_u32(target, STM32_FLASH_OPTR, &optiondata);
 273         if (retval != ERROR_OK)
 274                 return retval;
 275
 276         stm32l4_info->option_bytes.user_options = (optiondata >> 8) & 0x3ffff;
 277         stm32l4_info->option_bytes.RDP = optiondata & 0xff;
 278
 279         retval = target_read_u32(target, STM32_FLASH_WRP1AR, &optiondata);
 280         if (retval != ERROR_OK)
 281                 return retval;
 282         stm32l4_info->option_bytes.wpr1a_start =  optiondata         & 0xff;
 283         stm32l4_info->option_bytes.wpr1a_end   = (optiondata >> 16)  & 0xff;
 284
 285         retval = target_read_u32(target, STM32_FLASH_WRP2AR, &optiondata);
 286         if (retval != ERROR_OK)
 287                 return retval;
 288         stm32l4_info->option_bytes.wpr2a_start =  optiondata         & 0xff;
 289         stm32l4_info->option_bytes.wpr2a_end   = (optiondata >> 16)  & 0xff;
 290
 291         retval = target_read_u32(target, STM32_FLASH_WRP1BR, &optiondata);
 292         if (retval != ERROR_OK)
 293                 return retval;
 294         stm32l4_info->option_bytes.wpr1b_start =  optiondata         & 0xff;
 295         stm32l4_info->option_bytes.wpr1b_end   = (optiondata >> 16)  & 0xff;
 296
 297         retval = target_read_u32(target, STM32_FLASH_WRP2BR, &optiondata);
 298         if (retval != ERROR_OK)
 299                 return retval;
 300         stm32l4_info->option_bytes.wpr2b_start =  optiondata         & 0xff;
 301         stm32l4_info->option_bytes.wpr2b_end   = (optiondata >> 16)  & 0xff;
 302
 303         if (stm32l4_info->option_bytes.RDP != 0xAA)
 304                 LOG_INFO("Device Security Bit Set");
 305
 306         return ERROR_OK;
 307 }
 308
 309 static int stm32l4_write_options(struct flash_bank *bank)
 310 {
 311         struct stm32l4_flash_bank *stm32l4_info = NULL;
 312         struct target *target = bank->target;
 313         uint32_t optiondata;
 314
 315         stm32l4_info = bank->driver_priv;
 316
 317         (void) optiondata;
 318         (void) stm32l4_info;
 319
 320         int retval = stm32l4_unlock_option_reg(target);
 321         if (retval != ERROR_OK)
 322                 return retval;
 323         /* FIXME: Implement Option writing!*/
 324         return ERROR_OK;
 325 }
 326
 327 static int stm32l4_protect_check(struct flash_bank *bank)
 328 {
 329         struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
 330
 331         /* read write protection settings */
 332         int retval = stm32l4_read_options(bank);
 333         if (retval != ERROR_OK) {
 334                 LOG_DEBUG("unable to read option bytes");
 335                 return retval;
 336         }
 337
 338         for (int i = 0; i < bank->num_sectors; i++) {
 339                 if (i < stm32l4_info->option_bytes.bank_b_start) {
 340                         if (((i >= stm32l4_info->option_bytes.wpr1a_start) &&
 341                                  (i <= stm32l4_info->option_bytes.wpr1a_end)) ||
 342                                 ((i >= stm32l4_info->option_bytes.wpr2a_start) &&
 343                                  (i <= stm32l4_info->option_bytes.wpr2a_end)))
 344                                 bank->sectors[i].is_protected = 1;
 345                         else
 346                                 bank->sectors[i].is_protected = 0;
 347                 } else {
 348                         uint8_t snb;
 349                         snb = i - stm32l4_info->option_bytes.bank_b_start + 256;
 350                         if (((snb >= stm32l4_info->option_bytes.wpr1b_start) &&
 351                                  (snb <= stm32l4_info->option_bytes.wpr1b_end)) ||
 352                                 ((snb >= stm32l4_info->option_bytes.wpr2b_start) &&
 353                                  (snb <= stm32l4_info->option_bytes.wpr2b_end)))
 354                                 bank->sectors[i].is_protected = 1;
 355                         else
 356                                 bank->sectors[i].is_protected = 0;
 357                 }
 358         }
 359         return ERROR_OK;
 360 }
 361
 362 static int stm32l4_erase(struct flash_bank *bank, int first, int last)
 363 {
 364         struct target *target = bank->target;
 365         int i;
 366
 367         assert(first < bank->num_sectors);
 368         assert(last < bank->num_sectors);
 369
 370         if (bank->target->state != TARGET_HALTED) {
 371                 LOG_ERROR("Target not halted");
 372                 return ERROR_TARGET_NOT_HALTED;
 373         }
 374
 375         int retval;
 376         retval = stm32l4_unlock_reg(target);
 377         if (retval != ERROR_OK)
 378                 return retval;
 379
 380         /*
 381         Sector Erase
 382         To erase a sector, follow the procedure below:
 383         1. Check that no Flash memory operation is ongoing by
 384        checking the BSY bit in the FLASH_SR register
 385         2. Set the PER bit and select the page and bank
 386            you wish to erase  in the FLASH_CR register
 387         3. Set the STRT bit in the FLASH_CR register
 388         4. Wait for the BSY bit to be cleared
 389          */
 390         struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
 391
 392         for (i = first; i <= last; i++) {
 393                 uint32_t erase_flags;
 394                 erase_flags = FLASH_PER | FLASH_STRT;
 395
 396                 if  (i >= stm32l4_info->option_bytes.bank_b_start) {
 397                         uint8_t snb;
 398                         snb = (i - stm32l4_info->option_bytes.bank_b_start) + 256;
 399                         erase_flags |= snb << FLASH_PAGE_SHIFT | FLASH_CR_BKER;
 400                 } else
 401                         erase_flags |= i << FLASH_PAGE_SHIFT;
 402                 retval = target_write_u32(target,
 403                                 stm32l4_get_flash_reg(bank, STM32_FLASH_CR), erase_flags);
 404                 if (retval != ERROR_OK)
 405                         return retval;
 406
 407                 retval = stm32l4_wait_status_busy(bank, FLASH_ERASE_TIMEOUT);
 408                 if (retval != ERROR_OK)
 409                         return retval;
 410
 411                 bank->sectors[i].is_erased = 1;
 412         }
 413
 414         retval = target_write_u32(
 415                 target, stm32l4_get_flash_reg(bank, STM32_FLASH_CR), FLASH_LOCK);
 416         if (retval != ERROR_OK)
 417                 return retval;
 418
 419         return ERROR_OK;
 420 }
 421
 422 static int stm32l4_protect(struct flash_bank *bank, int set, int first, int last)
 423 {
 424         struct target *target = bank->target;
 425         struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
 426
 427         if (target->state != TARGET_HALTED) {
 428                 LOG_ERROR("Target not halted");
 429                 return ERROR_TARGET_NOT_HALTED;
 430         }
 431
 432         /* read protection settings */
 433         int retval = stm32l4_read_options(bank);
 434         if (retval != ERROR_OK) {
 435                 LOG_DEBUG("unable to read option bytes");
 436                 return retval;
 437         }
 438
 439         (void)stm32l4_info;
 440         /* FIXME: Write First and last in a valid WRPxx_start/end combo*/
 441         retval = stm32l4_write_options(bank);
 442         if (retval != ERROR_OK)
 443                 return retval;
 444
 445         return ERROR_OK;
 446 }
 447
 448 /* Count is in halfwords */
 449 static int stm32l4_write_block(struct flash_bank *bank, const uint8_t *buffer,
 450                 uint32_t offset, uint32_t count)
 451 {
 452         struct target *target = bank->target;
 453         uint32_t buffer_size = 16384;
 454         struct working_area *write_algorithm;
 455         struct working_area *source;
 456         uint32_t address = bank->base + offset;
 457         struct reg_param reg_params[5];
 458         struct armv7m_algorithm armv7m_info;
 459         int retval = ERROR_OK;
 460
 461         /* See contrib/loaders/flash/stm32l4x.S for source and
 462          * hints how to generate the data!
 463          */
 464
 465         static const uint8_t stm32l4_flash_write_code[] = {
 466                 0xd0, 0xf8, 0x00, 0x80, 0xb8, 0xf1, 0x00, 0x0f, 0x22, 0xd0, 0x45, 0x68,
 467                 0xb8, 0xeb, 0x05, 0x06, 0x07, 0x2e, 0xf5, 0xd3, 0xdf, 0xf8, 0x3c, 0x60,
 468                 0x66, 0x61, 0x55, 0xf8, 0x04, 0x6b, 0x42, 0xf8, 0x04, 0x6b, 0x55, 0xf8,
 469                 0x04, 0x6b, 0x42, 0xf8, 0x04, 0x6b, 0xbf, 0xf3, 0x4f, 0x8f, 0x26, 0x69,
 470                 0x16, 0xf4, 0x80, 0x3f, 0xfb, 0xd1, 0x16, 0xf0, 0xfa, 0x0f, 0x07, 0xd1,
 471                 0x8d, 0x42, 0x28, 0xbf, 0x00, 0xf1, 0x08, 0x05, 0x45, 0x60, 0x01, 0x3b,
 472                 0x13, 0xb1, 0xd9, 0xe7, 0x00, 0x21, 0x41, 0x60, 0x30, 0x46, 0x00, 0xbe,
 473                 0x01, 0x00, 0x00, 0x00
 474         };
 475
 476         if (target_alloc_working_area(target, sizeof(stm32l4_flash_write_code),
 477                         &write_algorithm) != ERROR_OK) {
 478                 LOG_WARNING("no working area available, can't do block memory writes");
 479                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 480         }
 481
 482         retval = target_write_buffer(target, write_algorithm->address,
 483                         sizeof(stm32l4_flash_write_code),
 484                         stm32l4_flash_write_code);
 485         if (retval != ERROR_OK)
 486                 return retval;
 487
 488         /* memory buffer */
 489         while (target_alloc_working_area_try(target, buffer_size, &source) !=
 490                    ERROR_OK) {
 491                 buffer_size /= 2;
 492                 if (buffer_size <= 256) {
 493                         /* we already allocated the writing code, but failed to get a
 494                          * buffer, free the algorithm */
 495                         target_free_working_area(target, write_algorithm);
 496
 497                         LOG_WARNING("no large enough working area available, can't do block memory writes");
 498                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 499                 }
 500         }
 501
 502         armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
 503         armv7m_info.core_mode = ARM_MODE_THREAD;
 504
 505         init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT); /* buffer start, status (out) */
 506         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);    /* buffer end */
 507         init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);    /* target address */
 508         init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);    /* count (double word-64bit) */
 509         init_reg_param(&reg_params[4], "r4", 32, PARAM_OUT);    /* flash base */
 510
 511         buf_set_u32(reg_params[0].value, 0, 32, source->address);
 512         buf_set_u32(reg_params[1].value, 0, 32, source->address + source->size);
 513         buf_set_u32(reg_params[2].value, 0, 32, address);
 514         buf_set_u32(reg_params[3].value, 0, 32, count / 4);
 515         buf_set_u32(reg_params[4].value, 0, 32, STM32_FLASH_BASE);
 516
 517         retval = target_run_flash_async_algorithm(target, buffer, count, 2,
 518                         0, NULL,
 519                         5, reg_params,
 520                         source->address, source->size,
 521                         write_algorithm->address, 0,
 522                         &armv7m_info);
 523
 524         if (retval == ERROR_FLASH_OPERATION_FAILED) {
 525                 LOG_ERROR("error executing stm32l4 flash write algorithm");
 526
 527                 uint32_t error = buf_get_u32(reg_params[0].value, 0, 32) & FLASH_ERROR;
 528
 529                 if (error & FLASH_WRPERR)
 530                         LOG_ERROR("flash memory write protected");
 531
 532                 if (error != 0) {
 533                         LOG_ERROR("flash write failed = %08" PRIx32, error);
 534                         /* Clear but report errors */
 535                         target_write_u32(target, STM32_FLASH_SR, error);
 536                         retval = ERROR_FAIL;
 537                 }
 538         }
 539
 540         target_free_working_area(target, source);
 541         target_free_working_area(target, write_algorithm);
 542
 543         destroy_reg_param(&reg_params[0]);
 544         destroy_reg_param(&reg_params[1]);
 545         destroy_reg_param(&reg_params[2]);
 546         destroy_reg_param(&reg_params[3]);
 547         destroy_reg_param(&reg_params[4]);
 548
 549         return retval;
 550 }
 551
 552 static int stm32l4_write(struct flash_bank *bank, const uint8_t *buffer,
 553                 uint32_t offset, uint32_t count)
 554 {
 555         struct target *target = bank->target;
 556         int retval;
 557
 558         if (bank->target->state != TARGET_HALTED) {
 559                 LOG_ERROR("Target not halted");
 560                 return ERROR_TARGET_NOT_HALTED;
 561         }
 562
 563         if (offset & 0x7) {
 564                 LOG_WARNING("offset 0x%" PRIx32 " breaks required 8-byte alignment",
 565                                         offset);
 566                 return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
 567         }
 568
 569         if (count & 0x7) {
 570                 LOG_WARNING("Padding %d bytes to keep 8-byte write size",
 571                                         count & 7);
 572                 count = (count + 7) & ~7;
 573                 /* This pads the write chunk with random bytes by overrunning the
 574                  * write buffer. Padding with the erased pattern 0xff is purely
 575                  * cosmetical, as 8-byte flash words are ECC secured and the first
 576                  * write will program the ECC bits. A second write would need
 577                  * to reprogramm these ECC bits.
 578                  * But this can only be done after erase!
 579                  */
 580         }
 581
 582         retval = stm32l4_unlock_reg(target);
 583         if (retval != ERROR_OK)
 584                 return retval;
 585
 586         /* Only full double words (8-byte) can be programmed*/
 587         retval = stm32l4_write_block(bank, buffer, offset, count / 2);
 588         if (retval != ERROR_OK) {
 589                 LOG_WARNING("block write failed");
 590                 return retval;
 591                 }
 592
 593         LOG_WARNING("block write succeeded");
 594         return target_write_u32(target, STM32_FLASH_CR, FLASH_LOCK);
 595 }
 596
 597 static int stm32l4_probe(struct flash_bank *bank)
 598 {
 599         struct target *target = bank->target;
 600         struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
 601         int i;
 602         uint16_t flash_size_in_kb = 0xffff;
 603         uint32_t device_id;
 604         uint32_t options;
 605         uint32_t base_address = 0x08000000;
 606
 607         stm32l4_info->probed = 0;
 608
 609         /* read stm32 device id register */
 610         int retval = target_read_u32(target, DBGMCU_IDCODE, &device_id);
 611         if (retval != ERROR_OK)
 612                 return retval;
 613         LOG_INFO("device id = 0x%08" PRIx32 "", device_id);
 614
 615         /* set max flash size depending on family */
 616         switch (device_id & 0xfff) {
 617         case 0x415:
 618                 break;
 619         default:
 620                 LOG_WARNING("Cannot identify target as a STM32L4 family.");
 621                 return ERROR_FAIL;
 622         }
 623
 624         /* get flash size from target. */
 625         retval = target_read_u16(target, FLASH_SIZE_REG, &flash_size_in_kb);
 626
 627         /* get options to for DUAL BANK. */
 628         retval = target_read_u32(target, STM32_FLASH_OPTR, &options);
 629
 630         /* only devices with < 1024 kiB may be set to single bank dual banks */
 631         if ((flash_size_in_kb == 1024) || !(options & OPT_DUALBANK))
 632                 stm32l4_info->option_bytes.bank_b_start = 256;
 633         else
 634                 stm32l4_info->option_bytes.bank_b_start = flash_size_in_kb << 9;
 635
 636         LOG_INFO("flash size = %dkbytes", flash_size_in_kb);
 637
 638         /* did we assign flash size? */
 639         assert((flash_size_in_kb != 0xffff) && flash_size_in_kb);
 640
 641         /* calculate numbers of pages */
 642         int num_pages = flash_size_in_kb / 2;
 643
 644         /* check that calculation result makes sense */
 645         assert(num_pages > 0);
 646
 647         if (bank->sectors) {
 648                 free(bank->sectors);
 649                 bank->sectors = NULL;
 650         }
 651
 652         bank->base = base_address;
 653         bank->size = num_pages * (1 << 11);
 654         bank->num_sectors = num_pages;
 655         bank->sectors = malloc(sizeof(struct flash_sector) * num_pages);
 656         if (!bank->sectors)
 657                 return ERROR_FAIL; /* Checkme: What better error to use?*/
 658
 659         for (i = 0; i < num_pages; i++) {
 660                 bank->sectors[i].offset = i << 11;
 661                 bank->sectors[i].size = 1 << 11;
 662                 bank->sectors[i].is_erased = -1;
 663                 bank->sectors[i].is_protected = 1;
 664         }
 665
 666         stm32l4_info->probed = 1;
 667
 668         return ERROR_OK;
 669 }
 670
 671 static int stm32l4_auto_probe(struct flash_bank *bank)
 672 {
 673         struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
 674         if (stm32l4_info->probed)
 675                 return ERROR_OK;
 676         return stm32l4_probe(bank);
 677 }
 678
 679 static int get_stm32l4_info(struct flash_bank *bank, char *buf, int buf_size)
 680 {
 681         struct target *target = bank->target;
 682         uint32_t dbgmcu_idcode;
 683
 684         /* read stm32 device id register */
 685         int retval = target_read_u32(target, DBGMCU_IDCODE, &dbgmcu_idcode);
 686         if (retval != ERROR_OK)
 687                 return retval;
 688
 689         uint16_t device_id = dbgmcu_idcode & 0xffff;
 690         uint8_t rev_id = dbgmcu_idcode >> 28;
 691         uint8_t rev_minor = 0;
 692         int i;
 693
 694         for (i = 16; i < 28; i++) {
 695                 if (dbgmcu_idcode & (1 << i))
 696                         rev_minor++;
 697                 else
 698                         break;
 699         }
 700
 701         const char *device_str;
 702
 703         switch (device_id) {
 704         case 0x6415:
 705                 device_str = "STM32L4xx";
 706                 break;
 707
 708         default:
 709                 snprintf(buf, buf_size, "Cannot identify target as a STM32L4\n");
 710                 return ERROR_FAIL;
 711         }
 712
 713         snprintf(buf, buf_size, "%s - Rev: %1d.%02d",
 714                          device_str, rev_id, rev_minor);
 715
 716         return ERROR_OK;
 717 }
 718
 719 COMMAND_HANDLER(stm32l4_handle_lock_command)
 720 {
 721         struct target *target = NULL;
 722         struct stm32l4_flash_bank *stm32l4_info = NULL;
 723
 724         if (CMD_ARGC < 1)
 725                 return ERROR_COMMAND_SYNTAX_ERROR;
 726
 727         struct flash_bank *bank;
 728         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 729         if (ERROR_OK != retval)
 730                 return retval;
 731
 732         stm32l4_info = bank->driver_priv;
 733         target = bank->target;
 734
 735         if (target->state != TARGET_HALTED) {
 736                 LOG_ERROR("Target not halted");
 737                 return ERROR_TARGET_NOT_HALTED;
 738         }
 739
 740         if (stm32l4_read_options(bank) != ERROR_OK) {
 741                 command_print(CMD_CTX, "%s failed to read options",
 742                                           bank->driver->name);
 743                 return ERROR_OK;
 744         }
 745
 746         /* set readout protection */
 747         stm32l4_info->option_bytes.RDP = 0;
 748
 749         if (stm32l4_write_options(bank) != ERROR_OK) {
 750                 command_print(CMD_CTX, "%s failed to lock device", bank->driver->name);
 751                 return ERROR_OK;
 752         }
 753
 754         command_print(CMD_CTX, "%s locked", bank->driver->name);
 755
 756         return ERROR_OK;
 757 }
 758
 759 COMMAND_HANDLER(stm32l4_handle_unlock_command)
 760 {
 761         struct target *target = NULL;
 762         struct stm32l4_flash_bank *stm32l4_info = NULL;
 763
 764         if (CMD_ARGC < 1)
 765                 return ERROR_COMMAND_SYNTAX_ERROR;
 766
 767         struct flash_bank *bank;
 768         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 769         if (ERROR_OK != retval)
 770                 return retval;
 771
 772         stm32l4_info = bank->driver_priv;
 773         target = bank->target;
 774
 775         if (target->state != TARGET_HALTED) {
 776                 LOG_ERROR("Target not halted");
 777                 return ERROR_TARGET_NOT_HALTED;
 778         }
 779
 780         if (stm32l4_read_options(bank) != ERROR_OK) {
 781                 command_print(CMD_CTX, "%s failed to read options", bank->driver->name);
 782                 return ERROR_OK;
 783         }
 784
 785         /* clear readout protection and complementary option bytes
 786          * this will also force a device unlock if set */
 787         stm32l4_info->option_bytes.RDP = 0xAA;
 788
 789         if (stm32l4_write_options(bank) != ERROR_OK) {
 790                 command_print(CMD_CTX, "%s failed to unlock device",
 791                                           bank->driver->name);
 792                 return ERROR_OK;
 793         }
 794
 795         command_print(CMD_CTX, "%s unlocked.\n"
 796                         "INFO: a reset or power cycle is required "
 797                         "for the new settings to take effect.", bank->driver->name);
 798
 799         return ERROR_OK;
 800 }
 801
 802 static int stm32l4_mass_erase(struct flash_bank *bank, uint32_t action)
 803 {
 804         int retval;
 805         struct target *target = bank->target;
 806
 807         if (target->state != TARGET_HALTED) {
 808                 LOG_ERROR("Target not halted");
 809                 return ERROR_TARGET_NOT_HALTED;
 810         }
 811
 812         retval = stm32l4_unlock_reg(target);
 813         if (retval != ERROR_OK)
 814                 return retval;
 815
 816         /* mass erase flash memory */
 817         retval = target_write_u32(
 818                 target, stm32l4_get_flash_reg(bank, STM32_FLASH_CR), action);
 819         if (retval != ERROR_OK)
 820                 return retval;
 821         retval = target_write_u32(
 822                 target, stm32l4_get_flash_reg(bank, STM32_FLASH_CR),
 823                 action | FLASH_STRT);
 824         if (retval != ERROR_OK)
 825                 return retval;
 826
 827         retval = stm32l4_wait_status_busy(bank,  FLASH_ERASE_TIMEOUT);
 828         if (retval != ERROR_OK)
 829                 return retval;
 830
 831         retval = target_write_u32(
 832                 target, stm32l4_get_flash_reg(bank, STM32_FLASH_CR), FLASH_LOCK);
 833         if (retval != ERROR_OK)
 834                 return retval;
 835
 836         return ERROR_OK;
 837 }
 838
 839 COMMAND_HANDLER(stm32l4_handle_mass_erase_command)
 840 {
 841         int i;
 842         uint32_t action;
 843
 844         if (CMD_ARGC < 1) {
 845                 command_print(CMD_CTX, "stm32x mass_erase <STM32L4 bank>");
 846                 return ERROR_COMMAND_SYNTAX_ERROR;
 847         }
 848
 849         struct flash_bank *bank;
 850         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 851         if (ERROR_OK != retval)
 852                 return retval;
 853
 854         action =  FLASH_MER1 |  FLASH_MER2;
 855         retval = stm32l4_mass_erase(bank, action);
 856         if (retval == ERROR_OK) {
 857                 /* set all sectors as erased */
 858                 for (i = 0; i < bank->num_sectors; i++)
 859                         bank->sectors[i].is_erased = 1;
 860
 861                 command_print(CMD_CTX, "stm32x mass erase complete");
 862         } else {
 863                 command_print(CMD_CTX, "stm32x mass erase failed");
 864         }
 865
 866         return retval;
 867 }
 868
 869 static const struct command_registration stm32l4_exec_command_handlers[] = {
 870         {
 871                 .name = "lock",
 872                 .handler = stm32l4_handle_lock_command,
 873                 .mode = COMMAND_EXEC,
 874                 .usage = "bank_id",
 875                 .help = "Lock entire flash device.",
 876         },
 877         {
 878                 .name = "unlock",
 879                 .handler = stm32l4_handle_unlock_command,
 880                 .mode = COMMAND_EXEC,
 881                 .usage = "bank_id",
 882                 .help = "Unlock entire protected flash device.",
 883         },
 884         {
 885                 .name = "mass_erase",
 886                 .handler = stm32l4_handle_mass_erase_command,
 887                 .mode = COMMAND_EXEC,
 888                 .usage = "bank_id",
 889                 .help = "Erase entire flash device.",
 890         },
 891         COMMAND_REGISTRATION_DONE
 892 };
 893
 894 static const struct command_registration stm32l4_command_handlers[] = {
 895         {
 896                 .name = "stm32l4x",
 897                 .mode = COMMAND_ANY,
 898                 .help = "stm32l4x flash command group",
 899                 .usage = "",
 900                 .chain = stm32l4_exec_command_handlers,
 901         },
 902         COMMAND_REGISTRATION_DONE
 903 };
 904
 905 struct flash_driver stm32l4x_flash = {
 906         .name = "stm32l4x",
 907         .commands = stm32l4_command_handlers,
 908         .flash_bank_command = stm32l4_flash_bank_command,
 909         .erase = stm32l4_erase,
 910         .protect = stm32l4_protect,
 911         .write = stm32l4_write,
 912         .read = default_flash_read,
 913         .probe = stm32l4_probe,
 914         .auto_probe = stm32l4_auto_probe,
 915         .erase_check = default_flash_blank_check,
 916         .protect_check = stm32l4_protect_check,
 917         .info = get_stm32l4_info,
 918 };