arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog.c

   1 // SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
   2 /*
   3  * Copyright (C) 2020, STMicroelectronics - All Rights Reserved
   4  */
   5
   6 #include <common.h>
   7 #include <console.h>
   8 #include <dfu.h>
   9 #include <malloc.h>
  10 #include <mmc.h>
  11 #include <part.h>
  12 #include <asm/arch/stm32mp1_smc.h>
  13 #include <dm/uclass.h>
  14 #include <jffs2/load_kernel.h>
  15 #include <linux/list.h>
  16 #include <linux/list_sort.h>
  17 #include <linux/mtd/mtd.h>
  18 #include <linux/sizes.h>
  19
  20 #include "stm32prog.h"
  21
  22 /* Primary GPT header size for 128 entries : 17kB = 34 LBA of 512B */
  23 #define GPT_HEADER_SZ   34
  24
  25 #define OPT_SELECT      BIT(0)
  26 #define OPT_EMPTY       BIT(1)
  27 #define OPT_DELETE      BIT(2)
  28
  29 #define IS_SELECT(part) ((part)->option & OPT_SELECT)
  30 #define IS_EMPTY(part)  ((part)->option & OPT_EMPTY)
  31 #define IS_DELETE(part) ((part)->option & OPT_DELETE)
  32
  33 #define ALT_BUF_LEN                     SZ_1K
  34
  35 #define ROOTFS_MMC0_UUID \
  36         EFI_GUID(0xE91C4E10, 0x16E6, 0x4C0E, \
  37                  0xBD, 0x0E, 0x77, 0xBE, 0xCF, 0x4A, 0x35, 0x82)
  38
  39 #define ROOTFS_MMC1_UUID \
  40         EFI_GUID(0x491F6117, 0x415D, 0x4F53, \
  41                  0x88, 0xC9, 0x6E, 0x0D, 0xE5, 0x4D, 0xEA, 0xC6)
  42
  43 #define ROOTFS_MMC2_UUID \
  44         EFI_GUID(0xFD58F1C7, 0xBE0D, 0x4338, \
  45                  0x88, 0xE9, 0xAD, 0x8F, 0x05, 0x0A, 0xEB, 0x18)
  46
  47 /* RAW parttion (binary / bootloader) used Linux - reserved UUID */
  48 #define LINUX_RESERVED_UUID "8DA63339-0007-60C0-C436-083AC8230908"
  49
  50 /*
  51  * unique partition guid (uuid) for partition named "rootfs"
  52  * on each MMC instance = SD Card or eMMC
  53  * allow fixed kernel bootcmd: "rootf=PARTUID=e91c4e10-..."
  54  */
  55 static const efi_guid_t uuid_mmc[3] = {
  56         ROOTFS_MMC0_UUID,
  57         ROOTFS_MMC1_UUID,
  58         ROOTFS_MMC2_UUID
  59 };
  60
  61 DECLARE_GLOBAL_DATA_PTR;
  62
  63 /* order of column in flash layout file */
  64 enum stm32prog_col_t {
  65         COL_OPTION,
  66         COL_ID,
  67         COL_NAME,
  68         COL_TYPE,
  69         COL_IP,
  70         COL_OFFSET,
  71         COL_NB_STM32
  72 };
  73
  74 /* partition handling routines : CONFIG_CMD_MTDPARTS */
  75 int mtdparts_init(void);
  76 int find_dev_and_part(const char *id, struct mtd_device **dev,
  77                       u8 *part_num, struct part_info **part);
  78
  79 char *stm32prog_get_error(struct stm32prog_data *data)
  80 {
  81         static const char error_msg[] = "Unspecified";
  82
  83         if (strlen(data->error) == 0)
  84                 strcpy(data->error, error_msg);
  85
  86         return data->error;
  87 }
  88
  89 u8 stm32prog_header_check(struct raw_header_s *raw_header,
  90                           struct image_header_s *header)
  91 {
  92         unsigned int i;
  93
  94         header->present = 0;
  95         header->image_checksum = 0x0;
  96         header->image_length = 0x0;
  97
  98         if (!raw_header || !header) {
  99                 pr_debug("%s:no header data\n", __func__);
 100                 return -1;
 101         }
 102         if (raw_header->magic_number !=
 103                 (('S' << 0) | ('T' << 8) | ('M' << 16) | (0x32 << 24))) {
 104                 pr_debug("%s:invalid magic number : 0x%x\n",
 105                          __func__, raw_header->magic_number);
 106                 return -2;
 107         }
 108         /* only header v1.0 supported */
 109         if (raw_header->header_version != 0x00010000) {
 110                 pr_debug("%s:invalid header version : 0x%x\n",
 111                          __func__, raw_header->header_version);
 112                 return -3;
 113         }
 114         if (raw_header->reserved1 != 0x0 || raw_header->reserved2) {
 115                 pr_debug("%s:invalid reserved field\n", __func__);
 116                 return -4;
 117         }
 118         for (i = 0; i < (sizeof(raw_header->padding) / 4); i++) {
 119                 if (raw_header->padding[i] != 0) {
 120                         pr_debug("%s:invalid padding field\n", __func__);
 121                         return -5;
 122                 }
 123         }
 124         header->present = 1;
 125         header->image_checksum = le32_to_cpu(raw_header->image_checksum);
 126         header->image_length = le32_to_cpu(raw_header->image_length);
 127
 128         return 0;
 129 }
 130
 131 static u32 stm32prog_header_checksum(u32 addr, struct image_header_s *header)
 132 {
 133         u32 i, checksum;
 134         u8 *payload;
 135
 136         /* compute checksum on payload */
 137         payload = (u8 *)addr;
 138         checksum = 0;
 139         for (i = header->image_length; i > 0; i--)
 140                 checksum += *(payload++);
 141
 142         return checksum;
 143 }
 144
 145 /* FLASHLAYOUT PARSING *****************************************/
 146 static int parse_option(struct stm32prog_data *data,
 147                         int i, char *p, struct stm32prog_part_t *part)
 148 {
 149         int result = 0;
 150         char *c = p;
 151
 152         part->option = 0;
 153         if (!strcmp(p, "-"))
 154                 return 0;
 155
 156         while (*c) {
 157                 switch (*c) {
 158                 case 'P':
 159                         part->option |= OPT_SELECT;
 160                         break;
 161                 case 'E':
 162                         part->option |= OPT_EMPTY;
 163                         break;
 164                 case 'D':
 165                         part->option |= OPT_DELETE;
 166                         break;
 167                 default:
 168                         result = -EINVAL;
 169                         stm32prog_err("Layout line %d: invalid option '%c' in %s)",
 170                                       i, *c, p);
 171                         return -EINVAL;
 172                 }
 173                 c++;
 174         }
 175         if (!(part->option & OPT_SELECT)) {
 176                 stm32prog_err("Layout line %d: missing 'P' in option %s", i, p);
 177                 return -EINVAL;
 178         }
 179
 180         return result;
 181 }
 182
 183 static int parse_id(struct stm32prog_data *data,
 184                     int i, char *p, struct stm32prog_part_t *part)
 185 {
 186         int result = 0;
 187         unsigned long value;
 188
 189         result = strict_strtoul(p, 0, &value);
 190         part->id = value;
 191         if (result || value > PHASE_LAST_USER) {
 192                 stm32prog_err("Layout line %d: invalid phase value = %s", i, p);
 193                 result = -EINVAL;
 194         }
 195
 196         return result;
 197 }
 198
 199 static int parse_name(struct stm32prog_data *data,
 200                       int i, char *p, struct stm32prog_part_t *part)
 201 {
 202         int result = 0;
 203
 204         if (strlen(p) < sizeof(part->name)) {
 205                 strcpy(part->name, p);
 206         } else {
 207                 stm32prog_err("Layout line %d: partition name too long [%d]: %s",
 208                               i, strlen(p), p);
 209                 result = -EINVAL;
 210         }
 211
 212         return result;
 213 }
 214
 215 static int parse_type(struct stm32prog_data *data,
 216                       int i, char *p, struct stm32prog_part_t *part)
 217 {
 218         int result = 0;
 219         int len = 0;
 220
 221         part->bin_nb = 0;
 222         if (!strncmp(p, "Binary", 6)) {
 223                 part->part_type = PART_BINARY;
 224
 225                 /* search for Binary(X) case */
 226                 len = strlen(p);
 227                 part->bin_nb = 1;
 228                 if (len > 6) {
 229                         if (len < 8 ||
 230                             (p[6] != '(') ||
 231                             (p[len - 1] != ')'))
 232                                 result = -EINVAL;
 233                         else
 234                                 part->bin_nb =
 235                                         simple_strtoul(&p[7], NULL, 10);
 236                 }
 237         } else if (!strcmp(p, "System")) {
 238                 part->part_type = PART_SYSTEM;
 239         } else if (!strcmp(p, "FileSystem")) {
 240                 part->part_type = PART_FILESYSTEM;
 241         } else if (!strcmp(p, "RawImage")) {
 242                 part->part_type = RAW_IMAGE;
 243         } else {
 244                 result = -EINVAL;
 245         }
 246         if (result)
 247                 stm32prog_err("Layout line %d: type parsing error : '%s'",
 248                               i, p);
 249
 250         return result;
 251 }
 252
 253 static int parse_ip(struct stm32prog_data *data,
 254                     int i, char *p, struct stm32prog_part_t *part)
 255 {
 256         int result = 0;
 257         unsigned int len = 0;
 258
 259         part->dev_id = 0;
 260         if (!strcmp(p, "none")) {
 261                 part->target = STM32PROG_NONE;
 262         } else if (!strncmp(p, "mmc", 3)) {
 263                 part->target = STM32PROG_MMC;
 264                 len = 3;
 265         } else if (!strncmp(p, "nor", 3)) {
 266                 part->target = STM32PROG_NOR;
 267                 len = 3;
 268         } else if (!strncmp(p, "nand", 4)) {
 269                 part->target = STM32PROG_NAND;
 270                 len = 4;
 271         } else if (!strncmp(p, "spi-nand", 8)) {
 272                 part->target = STM32PROG_SPI_NAND;
 273                 len = 8;
 274         } else {
 275                 result = -EINVAL;
 276         }
 277         if (len) {
 278                 /* only one digit allowed for device id */
 279                 if (strlen(p) != len + 1) {
 280                         result = -EINVAL;
 281                 } else {
 282                         part->dev_id = p[len] - '0';
 283                         if (part->dev_id > 9)
 284                                 result = -EINVAL;
 285                 }
 286         }
 287         if (result)
 288                 stm32prog_err("Layout line %d: ip parsing error: '%s'", i, p);
 289
 290         return result;
 291 }
 292
 293 static int parse_offset(struct stm32prog_data *data,
 294                         int i, char *p, struct stm32prog_part_t *part)
 295 {
 296         int result = 0;
 297         char *tail;
 298
 299         part->part_id = 0;
 300         part->addr = 0;
 301         part->size = 0;
 302         /* eMMC boot parttion */
 303         if (!strncmp(p, "boot", 4)) {
 304                 if (strlen(p) != 5) {
 305                         result = -EINVAL;
 306                 } else {
 307                         if (p[4] == '1')
 308                                 part->part_id = -1;
 309                         else if (p[4] == '2')
 310                                 part->part_id = -2;
 311                         else
 312                                 result = -EINVAL;
 313                 }
 314                 if (result)
 315                         stm32prog_err("Layout line %d: invalid part '%s'",
 316                                       i, p);
 317         } else {
 318                 part->addr = simple_strtoull(p, &tail, 0);
 319                 if (tail == p || *tail != '\0') {
 320                         stm32prog_err("Layout line %d: invalid offset '%s'",
 321                                       i, p);
 322                         result = -EINVAL;
 323                 }
 324         }
 325
 326         return result;
 327 }
 328
 329 static
 330 int (* const parse[COL_NB_STM32])(struct stm32prog_data *data, int i, char *p,
 331                                   struct stm32prog_part_t *part) = {
 332         [COL_OPTION] = parse_option,
 333         [COL_ID] = parse_id,
 334         [COL_NAME] =  parse_name,
 335         [COL_TYPE] = parse_type,
 336         [COL_IP] = parse_ip,
 337         [COL_OFFSET] = parse_offset,
 338 };
 339
 340 static int parse_flash_layout(struct stm32prog_data *data,
 341                               ulong addr,
 342                               ulong size)
 343 {
 344         int column = 0, part_nb = 0, ret;
 345         bool end_of_line, eof;
 346         char *p, *start, *last, *col;
 347         struct stm32prog_part_t *part;
 348         int part_list_size;
 349         int i;
 350
 351         data->part_nb = 0;
 352
 353         /* check if STM32image is detected */
 354         if (!stm32prog_header_check((struct raw_header_s *)addr,
 355                                     &data->header)) {
 356                 u32 checksum;
 357
 358                 addr = addr + BL_HEADER_SIZE;
 359                 size = data->header.image_length;
 360
 361                 checksum = stm32prog_header_checksum(addr, &data->header);
 362                 if (checksum != data->header.image_checksum) {
 363                         stm32prog_err("Layout: invalid checksum : 0x%x expected 0x%x",
 364                                       checksum, data->header.image_checksum);
 365                         return -EIO;
 366                 }
 367         }
 368         if (!size)
 369                 return -EINVAL;
 370
 371         start = (char *)addr;
 372         last = start + size;
 373
 374         *last = 0x0; /* force null terminated string */
 375         pr_debug("flash layout =\n%s\n", start);
 376
 377         /* calculate expected number of partitions */
 378         part_list_size = 1;
 379         p = start;
 380         while (*p && (p < last)) {
 381                 if (*p++ == '\n') {
 382                         part_list_size++;
 383                         if (p < last && *p == '#')
 384                                 part_list_size--;
 385                 }
 386         }
 387         if (part_list_size > PHASE_LAST_USER) {
 388                 stm32prog_err("Layout: too many partition (%d)",
 389                               part_list_size);
 390                 return -1;
 391         }
 392         part = calloc(sizeof(struct stm32prog_part_t), part_list_size);
 393         if (!part) {
 394                 stm32prog_err("Layout: alloc failed");
 395                 return -ENOMEM;
 396         }
 397         data->part_array = part;
 398
 399         /* main parsing loop */
 400         i = 1;
 401         eof = false;
 402         p = start;
 403         col = start; /* 1st column */
 404         end_of_line = false;
 405         while (!eof) {
 406                 switch (*p) {
 407                 /* CR is ignored and replaced by NULL character */
 408                 case '\r':
 409                         *p = '\0';
 410                         p++;
 411                         continue;
 412                 case '\0':
 413                         end_of_line = true;
 414                         eof = true;
 415                         break;
 416                 case '\n':
 417                         end_of_line = true;
 418                         break;
 419                 case '\t':
 420                         break;
 421                 case '#':
 422                         /* comment line is skipped */
 423                         if (column == 0 && p == col) {
 424                                 while ((p < last) && *p)
 425                                         if (*p++ == '\n')
 426                                                 break;
 427                                 col = p;
 428                                 i++;
 429                                 if (p >= last || !*p) {
 430                                         eof = true;
 431                                         end_of_line = true;
 432                                 }
 433                                 continue;
 434                         }
 435                         /* fall through */
 436                 /* by default continue with the next character */
 437                 default:
 438                         p++;
 439                         continue;
 440                 }
 441
 442                 /* replace by \0: allow string parsing for each column */
 443                 *p = '\0';
 444                 p++;
 445                 if (p >= last) {
 446                         eof = true;
 447                         end_of_line = true;
 448                 }
 449
 450                 /* skip empty line and multiple TAB in tsv file */
 451                 if (strlen(col) == 0) {
 452                         col = p;
 453                         /* skip empty line */
 454                         if (column == 0 && end_of_line) {
 455                                 end_of_line = false;
 456                                 i++;
 457                         }
 458                         continue;
 459                 }
 460
 461                 if (column < COL_NB_STM32) {
 462                         ret = parse[column](data, i, col, part);
 463                         if (ret)
 464                                 return ret;
 465                 }
 466
 467                 /* save the beginning of the next column */
 468                 column++;
 469                 col = p;
 470
 471                 if (!end_of_line)
 472                         continue;
 473
 474                 /* end of the line detected */
 475                 end_of_line = false;
 476
 477                 if (column < COL_NB_STM32) {
 478                         stm32prog_err("Layout line %d: no enought column", i);
 479                         return -EINVAL;
 480                 }
 481                 column = 0;
 482                 part_nb++;
 483                 part++;
 484                 i++;
 485                 if (part_nb >= part_list_size) {
 486                         part = NULL;
 487                         if (!eof) {
 488                                 stm32prog_err("Layout: no enought memory for %d part",
 489                                               part_nb);
 490                                 return -EINVAL;
 491                         }
 492                 }
 493         }
 494         data->part_nb = part_nb;
 495         if (data->part_nb == 0) {
 496                 stm32prog_err("Layout: no partition found");
 497                 return -ENODEV;
 498         }
 499
 500         return 0;
 501 }
 502
 503 static int __init part_cmp(void *priv, struct list_head *a, struct list_head *b)
 504 {
 505         struct stm32prog_part_t *parta, *partb;
 506
 507         parta = container_of(a, struct stm32prog_part_t, list);
 508         partb = container_of(b, struct stm32prog_part_t, list);
 509
 510         if (parta->part_id != partb->part_id)
 511                 return parta->part_id - partb->part_id;
 512         else
 513                 return parta->addr > partb->addr ? 1 : -1;
 514 }
 515
 516 static void get_mtd_by_target(char *string, enum stm32prog_target target,
 517                               int dev_id)
 518 {
 519         const char *dev_str;
 520
 521         switch (target) {
 522         case STM32PROG_NOR:
 523                 dev_str = "nor";
 524                 break;
 525         case STM32PROG_NAND:
 526                 dev_str = "nand";
 527                 break;
 528         case STM32PROG_SPI_NAND:
 529                 dev_str = "spi-nand";
 530                 break;
 531         default:
 532                 dev_str = "invalid";
 533                 break;
 534         }
 535         sprintf(string, "%s%d", dev_str, dev_id);
 536 }
 537
 538 static int init_device(struct stm32prog_data *data,
 539                        struct stm32prog_dev_t *dev)
 540 {
 541         struct mmc *mmc = NULL;
 542         struct blk_desc *block_dev = NULL;
 543 #ifdef CONFIG_MTD
 544         struct mtd_info *mtd = NULL;
 545         char mtd_id[16];
 546 #endif
 547         int part_id;
 548         int ret;
 549         u64 first_addr = 0, last_addr = 0;
 550         struct stm32prog_part_t *part, *next_part;
 551         u64 part_addr, part_size;
 552         bool part_found;
 553         const char *part_name;
 554
 555         switch (dev->target) {
 556 #ifdef CONFIG_MMC
 557         case STM32PROG_MMC:
 558                 mmc = find_mmc_device(dev->dev_id);
 559                 if (mmc_init(mmc)) {
 560                         stm32prog_err("mmc device %d not found", dev->dev_id);
 561                         return -ENODEV;
 562                 }
 563                 block_dev = mmc_get_blk_desc(mmc);
 564                 if (!block_dev) {
 565                         stm32prog_err("mmc device %d not probed", dev->dev_id);
 566                         return -ENODEV;
 567                 }
 568                 dev->erase_size = mmc->erase_grp_size * block_dev->blksz;
 569                 dev->mmc = mmc;
 570
 571                 /* reserve a full erase group for each GTP headers */
 572                 if (mmc->erase_grp_size > GPT_HEADER_SZ) {
 573                         first_addr = dev->erase_size;
 574                         last_addr = (u64)(block_dev->lba -
 575                                           mmc->erase_grp_size) *
 576                                     block_dev->blksz;
 577                 } else {
 578                         first_addr = (u64)GPT_HEADER_SZ * block_dev->blksz;
 579                         last_addr = (u64)(block_dev->lba - GPT_HEADER_SZ - 1) *
 580                                     block_dev->blksz;
 581                 }
 582                 pr_debug("MMC %d: lba=%ld blksz=%ld\n", dev->dev_id,
 583                          block_dev->lba, block_dev->blksz);
 584                 pr_debug(" available address = 0x%llx..0x%llx\n",
 585                          first_addr, last_addr);
 586                 pr_debug(" full_update = %d\n", dev->full_update);
 587                 break;
 588 #endif
 589 #ifdef CONFIG_MTD
 590         case STM32PROG_NOR:
 591         case STM32PROG_NAND:
 592         case STM32PROG_SPI_NAND:
 593                 get_mtd_by_target(mtd_id, dev->target, dev->dev_id);
 594                 pr_debug("%s\n", mtd_id);
 595
 596                 mtdparts_init();
 597                 mtd = get_mtd_device_nm(mtd_id);
 598                 if (IS_ERR(mtd)) {
 599                         stm32prog_err("MTD device %s not found", mtd_id);
 600                         return -ENODEV;
 601                 }
 602                 first_addr = 0;
 603                 last_addr = mtd->size;
 604                 dev->erase_size = mtd->erasesize;
 605                 pr_debug("MTD device %s: size=%lld erasesize=%d\n",
 606                          mtd_id, mtd->size, mtd->erasesize);
 607                 pr_debug(" available address = 0x%llx..0x%llx\n",
 608                          first_addr, last_addr);
 609                 dev->mtd = mtd;
 610                 break;
 611 #endif
 612         default:
 613                 stm32prog_err("unknown device type = %d", dev->target);
 614                 return -ENODEV;
 615         }
 616         pr_debug(" erase size = 0x%x\n", dev->erase_size);
 617         pr_debug(" full_update = %d\n", dev->full_update);
 618
 619         /* order partition list in offset order */
 620         list_sort(NULL, &dev->part_list, &part_cmp);
 621         part_id = 1;
 622         pr_debug("id : Opt Phase     Name target.n dev.n addr     size     part_off part_size\n");
 623         list_for_each_entry(part, &dev->part_list, list) {
 624                 if (part->bin_nb > 1) {
 625                         if ((dev->target != STM32PROG_NAND &&
 626                              dev->target != STM32PROG_SPI_NAND) ||
 627                             part->id >= PHASE_FIRST_USER ||
 628                             strncmp(part->name, "fsbl", 4)) {
 629                                 stm32prog_err("%s (0x%x): multiple binary %d not supported",
 630                                               part->name, part->id,
 631                                               part->bin_nb);
 632                                 return -EINVAL;
 633                         }
 634                 }
 635                 if (part->part_type == RAW_IMAGE) {
 636                         part->part_id = 0x0;
 637                         part->addr = 0x0;
 638                         if (block_dev)
 639                                 part->size = block_dev->lba * block_dev->blksz;
 640                         else
 641                                 part->size = last_addr;
 642                         pr_debug("-- : %1d %02x %14s %02d.%d %02d.%02d %08llx %08llx\n",
 643                                  part->option, part->id, part->name,
 644                                  part->part_type, part->bin_nb, part->target,
 645                                  part->dev_id, part->addr, part->size);
 646                         continue;
 647                 }
 648                 if (part->part_id < 0) { /* boot hw partition for eMMC */
 649                         if (mmc) {
 650                                 part->size = mmc->capacity_boot;
 651                         } else {
 652                                 stm32prog_err("%s (0x%x): hw partition not expected : %d",
 653                                               part->name, part->id,
 654                                               part->part_id);
 655                                 return -ENODEV;
 656                         }
 657                 } else {
 658                         part->part_id = part_id++;
 659
 660                         /* last partition : size to the end of the device */
 661                         if (part->list.next != &dev->part_list) {
 662                                 next_part =
 663                                         container_of(part->list.next,
 664                                                      struct stm32prog_part_t,
 665                                                      list);
 666                                 if (part->addr < next_part->addr) {
 667                                         part->size = next_part->addr -
 668                                                      part->addr;
 669                                 } else {
 670                                         stm32prog_err("%s (0x%x): same address : 0x%llx == %s (0x%x): 0x%llx",
 671                                                       part->name, part->id,
 672                                                       part->addr,
 673                                                       next_part->name,
 674                                                       next_part->id,
 675                                                       next_part->addr);
 676                                         return -EINVAL;
 677                                 }
 678                         } else {
 679                                 if (part->addr <= last_addr) {
 680                                         part->size = last_addr - part->addr;
 681                                 } else {
 682                                         stm32prog_err("%s (0x%x): invalid address 0x%llx (max=0x%llx)",
 683                                                       part->name, part->id,
 684                                                       part->addr, last_addr);
 685                                         return -EINVAL;
 686                                 }
 687                         }
 688                         if (part->addr < first_addr) {
 689                                 stm32prog_err("%s (0x%x): invalid address 0x%llx (min=0x%llx)",
 690                                               part->name, part->id,
 691                                               part->addr, first_addr);
 692                                 return -EINVAL;
 693                         }
 694                 }
 695                 if ((part->addr & ((u64)part->dev->erase_size - 1)) != 0) {
 696                         stm32prog_err("%s (0x%x): not aligned address : 0x%llx on erase size 0x%x",
 697                                       part->name, part->id, part->addr,
 698                                       part->dev->erase_size);
 699                         return -EINVAL;
 700                 }
 701                 pr_debug("%02d : %1d %02x %14s %02d.%d %02d.%02d %08llx %08llx",
 702                          part->part_id, part->option, part->id, part->name,
 703                          part->part_type, part->bin_nb, part->target,
 704                          part->dev_id, part->addr, part->size);
 705
 706                 part_addr = 0;
 707                 part_size = 0;
 708                 part_found = false;
 709
 710                 /* check coherency with existing partition */
 711                 if (block_dev) {
 712                         /*
 713                          * block devices with GPT: check user partition size
 714                          * only for partial update, the GPT partions are be
 715                          * created for full update
 716                          */
 717                         if (dev->full_update || part->part_id < 0) {
 718                                 pr_debug("\n");
 719                                 continue;
 720                         }
 721                         disk_partition_t partinfo;
 722
 723                         ret = part_get_info(block_dev, part->part_id,
 724                                             &partinfo);
 725
 726                         if (ret) {
 727                                 stm32prog_err("%s (0x%x):Couldn't find part %d on device mmc %d",
 728                                               part->name, part->id,
 729                                               part_id, part->dev_id);
 730                                 return -ENODEV;
 731                         }
 732                         part_addr = (u64)partinfo.start * partinfo.blksz;
 733                         part_size = (u64)partinfo.size * partinfo.blksz;
 734                         part_name = (char *)partinfo.name;
 735                         part_found = true;
 736                 }
 737
 738 #ifdef CONFIG_MTD
 739                 if (mtd) {
 740                         char mtd_part_id[32];
 741                         struct part_info *mtd_part;
 742                         struct mtd_device *mtd_dev;
 743                         u8 part_num;
 744
 745                         sprintf(mtd_part_id, "%s,%d", mtd_id,
 746                                 part->part_id - 1);
 747                         ret = find_dev_and_part(mtd_part_id, &mtd_dev,
 748                                                 &part_num, &mtd_part);
 749                         if (ret != 0) {
 750                                 stm32prog_err("%s (0x%x): Invalid MTD partition %s",
 751                                               part->name, part->id,
 752                                               mtd_part_id);
 753                                 return -ENODEV;
 754                         }
 755                         part_addr = mtd_part->offset;
 756                         part_size = mtd_part->size;
 757                         part_name = mtd_part->name;
 758                         part_found = true;
 759                 }
 760 #endif
 761                 if (!part_found) {
 762                         stm32prog_err("%s (0x%x): Invalid partition",
 763                                       part->name, part->id);
 764                         pr_debug("\n");
 765                         continue;
 766                 }
 767
 768                 pr_debug(" %08llx %08llx\n", part_addr, part_size);
 769
 770                 if (part->addr != part_addr) {
 771                         stm32prog_err("%s (0x%x): Bad address for partition %d (%s) = 0x%llx <> 0x%llx expected",
 772                                       part->name, part->id, part->part_id,
 773                                       part_name, part->addr, part_addr);
 774                         return -ENODEV;
 775                 }
 776                 if (part->size != part_size) {
 777                         stm32prog_err("%s (0x%x): Bad size for partition %d (%s) at 0x%llx = 0x%llx <> 0x%llx expected",
 778                                       part->name, part->id, part->part_id,
 779                                       part_name, part->addr, part->size,
 780                                       part_size);
 781                         return -ENODEV;
 782                 }
 783         }
 784         return 0;
 785 }
 786
 787 static int treat_partition_list(struct stm32prog_data *data)
 788 {
 789         int i, j;
 790         struct stm32prog_part_t *part;
 791
 792         for (j = 0; j < STM32PROG_MAX_DEV; j++) {
 793                 data->dev[j].target = STM32PROG_NONE;
 794                 INIT_LIST_HEAD(&data->dev[j].part_list);
 795         }
 796
 797         data->tee_detected = false;
 798         data->fsbl_nor_detected = false;
 799         for (i = 0; i < data->part_nb; i++) {
 800                 part = &data->part_array[i];
 801                 part->alt_id = -1;
 802
 803                 /* skip partition with IP="none" */
 804                 if (part->target == STM32PROG_NONE) {
 805                         if (IS_SELECT(part)) {
 806                                 stm32prog_err("Layout: selected none phase = 0x%x",
 807                                               part->id);
 808                                 return -EINVAL;
 809                         }
 810                         continue;
 811                 }
 812
 813                 if (part->id == PHASE_FLASHLAYOUT ||
 814                     part->id > PHASE_LAST_USER) {
 815                         stm32prog_err("Layout: invalid phase = 0x%x",
 816                                       part->id);
 817                         return -EINVAL;
 818                 }
 819                 for (j = i + 1; j < data->part_nb; j++) {
 820                         if (part->id == data->part_array[j].id) {
 821                                 stm32prog_err("Layout: duplicated phase 0x%x at line %d and %d",
 822                                               part->id, i, j);
 823                                 return -EINVAL;
 824                         }
 825                 }
 826                 for (j = 0; j < STM32PROG_MAX_DEV; j++) {
 827                         if (data->dev[j].target == STM32PROG_NONE) {
 828                                 /* new device found */
 829                                 data->dev[j].target = part->target;
 830                                 data->dev[j].dev_id = part->dev_id;
 831                                 data->dev[j].full_update = true;
 832                                 data->dev_nb++;
 833                                 break;
 834                         } else if ((part->target == data->dev[j].target) &&
 835                                    (part->dev_id == data->dev[j].dev_id)) {
 836                                 break;
 837                         }
 838                 }
 839                 if (j == STM32PROG_MAX_DEV) {
 840                         stm32prog_err("Layout: too many device");
 841                         return -EINVAL;
 842                 }
 843                 switch (part->target)  {
 844                 case STM32PROG_NOR:
 845                         if (!data->fsbl_nor_detected &&
 846                             !strncmp(part->name, "fsbl", 4))
 847                                 data->fsbl_nor_detected = true;
 848                         /* fallthrough */
 849                 case STM32PROG_NAND:
 850                 case STM32PROG_SPI_NAND:
 851                         if (!data->tee_detected &&
 852                             !strncmp(part->name, "tee", 3))
 853                                 data->tee_detected = true;
 854                         break;
 855                 default:
 856                         break;
 857                 }
 858                 part->dev = &data->dev[j];
 859                 if (!IS_SELECT(part))
 860                         part->dev->full_update = false;
 861                 list_add_tail(&part->list, &data->dev[j].part_list);
 862         }
 863
 864         return 0;
 865 }
 866
 867 static int create_partitions(struct stm32prog_data *data)
 868 {
 869 #ifdef CONFIG_MMC
 870         int offset = 0;
 871         const int buflen = SZ_8K;
 872         char *buf;
 873         char uuid[UUID_STR_LEN + 1];
 874         unsigned char *uuid_bin;
 875         unsigned int mmc_id;
 876         int i;
 877         bool rootfs_found;
 878         struct stm32prog_part_t *part;
 879
 880         buf = malloc(buflen);
 881         if (!buf)
 882                 return -ENOMEM;
 883
 884         puts("partitions : ");
 885         /* initialize the selected device */
 886         for (i = 0; i < data->dev_nb; i++) {
 887                 /* create gpt partition support only for full update on MMC */
 888                 if (data->dev[i].target != STM32PROG_MMC ||
 889                     !data->dev[i].full_update)
 890                         continue;
 891
 892                 offset = 0;
 893                 rootfs_found = false;
 894                 memset(buf, 0, buflen);
 895
 896                 list_for_each_entry(part, &data->dev[i].part_list, list) {
 897                         /* skip eMMC boot partitions */
 898                         if (part->part_id < 0)
 899                                 continue;
 900                         /* skip Raw Image */
 901                         if (part->part_type == RAW_IMAGE)
 902                                 continue;
 903
 904                         if (offset + 100 > buflen) {
 905                                 pr_debug("\n%s: buffer too small, %s skippped",
 906                                          __func__, part->name);
 907                                 continue;
 908                         }
 909
 910                         if (!offset)
 911                                 offset += sprintf(buf, "gpt write mmc %d \"",
 912                                                   data->dev[i].dev_id);
 913
 914                         offset += snprintf(buf + offset, buflen - offset,
 915                                            "name=%s,start=0x%llx,size=0x%llx",
 916                                            part->name,
 917                                            part->addr,
 918                                            part->size);
 919
 920                         if (part->part_type == PART_BINARY)
 921                                 offset += snprintf(buf + offset,
 922                                                    buflen - offset,
 923                                                    ",type="
 924                                                    LINUX_RESERVED_UUID);
 925                         else
 926                                 offset += snprintf(buf + offset,
 927                                                    buflen - offset,
 928                                                    ",type=linux");
 929
 930                         if (part->part_type == PART_SYSTEM)
 931                                 offset += snprintf(buf + offset,
 932                                                    buflen - offset,
 933                                                    ",bootable");
 934
 935                         if (!rootfs_found && !strcmp(part->name, "rootfs")) {
 936                                 mmc_id = part->dev_id;
 937                                 rootfs_found = true;
 938                                 if (mmc_id < ARRAY_SIZE(uuid_mmc)) {
 939                                         uuid_bin =
 940                                           (unsigned char *)uuid_mmc[mmc_id].b;
 941                                         uuid_bin_to_str(uuid_bin, uuid,
 942                                                         UUID_STR_FORMAT_GUID);
 943                                         offset += snprintf(buf + offset,
 944                                                            buflen - offset,
 945                                                            ",uuid=%s", uuid);
 946                                 }
 947                         }
 948
 949                         offset += snprintf(buf + offset, buflen - offset, ";");
 950                 }
 951
 952                 if (offset) {
 953                         offset += snprintf(buf + offset, buflen - offset, "\"");
 954                         pr_debug("\ncmd: %s\n", buf);
 955                         if (run_command(buf, 0)) {
 956                                 stm32prog_err("GPT partitionning fail: %s",
 957                                               buf);
 958                                 free(buf);
 959
 960                                 return -1;
 961                         }
 962                 }
 963
 964                 if (data->dev[i].mmc)
 965                         part_init(mmc_get_blk_desc(data->dev[i].mmc));
 966
 967 #ifdef DEBUG
 968                 sprintf(buf, "gpt verify mmc %d", data->dev[i].dev_id);
 969                 pr_debug("\ncmd: %s", buf);
 970                 if (run_command(buf, 0))
 971                         printf("fail !\n");
 972                 else
 973                         printf("OK\n");
 974
 975                 sprintf(buf, "part list mmc %d", data->dev[i].dev_id);
 976                 run_command(buf, 0);
 977 #endif
 978         }
 979         puts("done\n");
 980
 981 #ifdef DEBUG
 982         run_command("mtd list", 0);
 983 #endif
 984         free(buf);
 985 #endif
 986
 987         return 0;
 988 }
 989
 990 static int stm32prog_alt_add(struct stm32prog_data *data,
 991                              struct dfu_entity *dfu,
 992                              struct stm32prog_part_t *part)
 993 {
 994         int ret = 0;
 995         int offset = 0;
 996         char devstr[10];
 997         char dfustr[10];
 998         char buf[ALT_BUF_LEN];
 999         u32 size;
1000         char multiplier,  type;
1001
1002         /* max 3 digit for sector size */
1003         if (part->size > SZ_1M) {
1004                 size = (u32)(part->size / SZ_1M);
1005                 multiplier = 'M';
1006         } else if (part->size > SZ_1K) {
1007                 size = (u32)(part->size / SZ_1K);
1008                 multiplier = 'K';
1009         } else {
1010                 size = (u32)part->size;
1011                 multiplier = 'B';
1012         }
1013         if (IS_SELECT(part) && !IS_EMPTY(part))
1014                 type = 'e'; /*Readable and Writeable*/
1015         else
1016                 type = 'a';/*Readable*/
1017
1018         memset(buf, 0, sizeof(buf));
1019         offset = snprintf(buf, ALT_BUF_LEN - offset,
1020                           "@%s/0x%02x/1*%d%c%c ",
1021                           part->name, part->id,
1022                           size, multiplier, type);
1023
1024         if (part->part_type == RAW_IMAGE) {
1025                 u64 dfu_size;
1026
1027                 if (part->dev->target == STM32PROG_MMC)
1028                         dfu_size = part->size / part->dev->mmc->read_bl_len;
1029                 else
1030                         dfu_size = part->size;
1031                 offset += snprintf(buf + offset, ALT_BUF_LEN - offset,
1032                                    "raw 0x0 0x%llx", dfu_size);
1033         } else if (part->part_id < 0) {
1034                 u64 nb_blk = part->size / part->dev->mmc->read_bl_len;
1035
1036                 offset += snprintf(buf + offset, ALT_BUF_LEN - offset,
1037                                    "raw 0x%llx 0x%llx",
1038                                    part->addr, nb_blk);
1039                 offset += snprintf(buf + offset, ALT_BUF_LEN - offset,
1040                                    " mmcpart %d;", -(part->part_id));
1041         } else {
1042                 if (part->part_type == PART_SYSTEM &&
1043                     (part->target == STM32PROG_NAND ||
1044                      part->target == STM32PROG_NOR ||
1045                      part->target == STM32PROG_SPI_NAND))
1046                         offset += snprintf(buf + offset,
1047                                            ALT_BUF_LEN - offset,
1048                                            "partubi");
1049                 else
1050                         offset += snprintf(buf + offset,
1051                                            ALT_BUF_LEN - offset,
1052                                            "part");
1053                 /* dev_id requested by DFU MMC */
1054                 if (part->target == STM32PROG_MMC)
1055                         offset += snprintf(buf + offset, ALT_BUF_LEN - offset,
1056                                            " %d", part->dev_id);
1057                 offset += snprintf(buf + offset, ALT_BUF_LEN - offset,
1058                                    " %d;", part->part_id);
1059         }
1060         switch (part->target) {
1061 #ifdef CONFIG_MMC
1062         case STM32PROG_MMC:
1063                 sprintf(dfustr, "mmc");
1064                 sprintf(devstr, "%d", part->dev_id);
1065                 break;
1066 #endif
1067 #ifdef CONFIG_MTD
1068         case STM32PROG_NAND:
1069         case STM32PROG_NOR:
1070         case STM32PROG_SPI_NAND:
1071                 sprintf(dfustr, "mtd");
1072                 get_mtd_by_target(devstr, part->target, part->dev_id);
1073                 break;
1074 #endif
1075         default:
1076                 stm32prog_err("invalid target: %d", part->target);
1077                 return -ENODEV;
1078         }
1079         pr_debug("dfu_alt_add(%s,%s,%s)\n", dfustr, devstr, buf);
1080         ret = dfu_alt_add(dfu, dfustr, devstr, buf);
1081         pr_debug("dfu_alt_add(%s,%s,%s) result %d\n",
1082                  dfustr, devstr, buf, ret);
1083
1084         return ret;
1085 }
1086
1087 static int stm32prog_alt_add_virt(struct dfu_entity *dfu,
1088                                   char *name, int phase, int size)
1089 {
1090         int ret = 0;
1091         char devstr[4];
1092         char buf[ALT_BUF_LEN];
1093
1094         sprintf(devstr, "%d", phase);
1095         sprintf(buf, "@%s/0x%02x/1*%dBe", name, phase, size);
1096         ret = dfu_alt_add(dfu, "virt", devstr, buf);
1097         pr_debug("dfu_alt_add(virt,%s,%s) result %d\n", devstr, buf, ret);
1098
1099         return ret;
1100 }
1101
1102 static int dfu_init_entities(struct stm32prog_data *data)
1103 {
1104         int ret = 0;
1105         int phase, i, alt_id;
1106         struct stm32prog_part_t *part;
1107         struct dfu_entity *dfu;
1108         int alt_nb;
1109
1110         alt_nb = 2; /* number of virtual = CMD, OTP*/
1111         if (data->part_nb == 0)
1112                 alt_nb++;  /* +1 for FlashLayout */
1113         else
1114                 for (i = 0; i < data->part_nb; i++) {
1115                         if (data->part_array[i].target != STM32PROG_NONE)
1116                                 alt_nb++;
1117                 }
1118
1119         if (dfu_alt_init(alt_nb, &dfu))
1120                 return -ENODEV;
1121
1122         puts("DFU alt info setting: ");
1123         if (data->part_nb) {
1124                 alt_id = 0;
1125                 for (phase = 1;
1126                      (phase <= PHASE_LAST_USER) &&
1127                      (alt_id < alt_nb) && !ret;
1128                      phase++) {
1129                         /* ordering alt setting by phase id */
1130                         part = NULL;
1131                         for (i = 0; i < data->part_nb; i++) {
1132                                 if (phase == data->part_array[i].id) {
1133                                         part = &data->part_array[i];
1134                                         break;
1135                                 }
1136                         }
1137                         if (!part)
1138                                 continue;
1139                         if (part->target == STM32PROG_NONE)
1140                                 continue;
1141                         part->alt_id = alt_id;
1142                         alt_id++;
1143
1144                         ret = stm32prog_alt_add(data, dfu, part);
1145                 }
1146         } else {
1147                 char buf[ALT_BUF_LEN];
1148
1149                 sprintf(buf, "@FlashLayout/0x%02x/1*256Ke ram %x 40000",
1150                         PHASE_FLASHLAYOUT, STM32_DDR_BASE);
1151                 ret = dfu_alt_add(dfu, "ram", NULL, buf);
1152                 pr_debug("dfu_alt_add(ram, NULL,%s) result %d\n", buf, ret);
1153         }
1154
1155         if (!ret)
1156                 ret = stm32prog_alt_add_virt(dfu, "virtual", PHASE_CMD, 512);
1157
1158         if (!ret)
1159                 ret = stm32prog_alt_add_virt(dfu, "OTP", PHASE_OTP, 512);
1160
1161         if (ret)
1162                 stm32prog_err("dfu init failed: %d", ret);
1163         puts("done\n");
1164
1165 #ifdef DEBUG
1166         dfu_show_entities();
1167 #endif
1168         return ret;
1169 }
1170
1171 int stm32prog_otp_write(struct stm32prog_data *data, u32 offset, u8 *buffer,
1172                         long *size)
1173 {
1174         pr_debug("%s: %x %lx\n", __func__, offset, *size);
1175
1176         if (!data->otp_part) {
1177                 data->otp_part = memalign(CONFIG_SYS_CACHELINE_SIZE, OTP_SIZE);
1178                 if (!data->otp_part)
1179                         return -ENOMEM;
1180         }
1181
1182         if (!offset)
1183                 memset(data->otp_part, 0, OTP_SIZE);
1184
1185         if (offset + *size > OTP_SIZE)
1186                 *size = OTP_SIZE - offset;
1187
1188         memcpy((void *)((u32)data->otp_part + offset), buffer, *size);
1189
1190         return 0;
1191 }
1192
1193 int stm32prog_otp_read(struct stm32prog_data *data, u32 offset, u8 *buffer,
1194                        long *size)
1195 {
1196 #ifndef CONFIG_ARM_SMCCC
1197         stm32prog_err("OTP update not supported");
1198
1199         return -1;
1200 #else
1201         int result = 0;
1202
1203         pr_debug("%s: %x %lx\n", __func__, offset, *size);
1204         /* alway read for first packet */
1205         if (!offset) {
1206                 if (!data->otp_part)
1207                         data->otp_part =
1208                                 memalign(CONFIG_SYS_CACHELINE_SIZE, OTP_SIZE);
1209
1210                 if (!data->otp_part) {
1211                         result = -ENOMEM;
1212                         goto end_otp_read;
1213                 }
1214
1215                 /* init struct with 0 */
1216                 memset(data->otp_part, 0, OTP_SIZE);
1217
1218                 /* call the service */
1219                 result = stm32_smc_exec(STM32_SMC_BSEC, STM32_SMC_READ_ALL,
1220                                         (u32)data->otp_part, 0);
1221                 if (result)
1222                         goto end_otp_read;
1223         }
1224
1225         if (!data->otp_part) {
1226                 result = -ENOMEM;
1227                 goto end_otp_read;
1228         }
1229
1230         if (offset + *size > OTP_SIZE)
1231                 *size = OTP_SIZE - offset;
1232         memcpy(buffer, (void *)((u32)data->otp_part + offset), *size);
1233
1234 end_otp_read:
1235         pr_debug("%s: result %i\n", __func__, result);
1236
1237         return result;
1238 #endif
1239 }
1240
1241 int stm32prog_otp_start(struct stm32prog_data *data)
1242 {
1243 #ifndef CONFIG_ARM_SMCCC
1244         stm32prog_err("OTP update not supported");
1245
1246         return -1;
1247 #else
1248         int result = 0;
1249         struct arm_smccc_res res;
1250
1251         if (!data->otp_part) {
1252                 stm32prog_err("start OTP without data");
1253                 return -1;
1254         }
1255
1256         arm_smccc_smc(STM32_SMC_BSEC, STM32_SMC_WRITE_ALL,
1257                       (u32)data->otp_part, 0, 0, 0, 0, 0, &res);
1258
1259         if (!res.a0) {
1260                 switch (res.a1) {
1261                 case 0:
1262                         result = 0;
1263                         break;
1264                 case 1:
1265                         stm32prog_err("Provisioning");
1266                         result = 0;
1267                         break;
1268                 default:
1269                         pr_err("%s: OTP incorrect value (err = %ld)\n",
1270                                __func__, res.a1);
1271                         result = -EINVAL;
1272                         break;
1273                 }
1274         } else {
1275                 pr_err("%s: Failed to exec svc=%x op=%x in secure mode (err = %ld)\n",
1276                        __func__, STM32_SMC_BSEC, STM32_SMC_WRITE_ALL, res.a0);
1277                 result = -EINVAL;
1278         }
1279
1280         free(data->otp_part);
1281         data->otp_part = NULL;
1282         pr_debug("%s: result %i\n", __func__, result);
1283
1284         return result;
1285 #endif
1286 }
1287
1288 /* copy FSBL on NAND to improve reliability on NAND */
1289 static int stm32prog_copy_fsbl(struct stm32prog_part_t *part)
1290 {
1291         int ret, i;
1292         void *fsbl;
1293         struct image_header_s header;
1294         struct raw_header_s raw_header;
1295         struct dfu_entity *dfu;
1296         long size, offset;
1297
1298         if (part->target != STM32PROG_NAND &&
1299             part->target != STM32PROG_SPI_NAND)
1300                 return -1;
1301
1302         dfu = dfu_get_entity(part->alt_id);
1303
1304         /* read header */
1305         dfu_transaction_cleanup(dfu);
1306         size = BL_HEADER_SIZE;
1307         ret = dfu->read_medium(dfu, 0, (void *)&raw_header, &size);
1308         if (ret)
1309                 return ret;
1310         if (stm32prog_header_check(&raw_header, &header))
1311                 return -1;
1312
1313         /* read header + payload */
1314         size = header.image_length + BL_HEADER_SIZE;
1315         size = round_up(size, part->dev->mtd->erasesize);
1316         fsbl = calloc(1, size);
1317         if (!fsbl)
1318                 return -ENOMEM;
1319         ret = dfu->read_medium(dfu, 0, fsbl, &size);
1320         pr_debug("%s read size=%lx ret=%d\n", __func__, size, ret);
1321         if (ret)
1322                 goto error;
1323
1324         dfu_transaction_cleanup(dfu);
1325         offset = 0;
1326         for (i = part->bin_nb - 1; i > 0; i--) {
1327                 offset += size;
1328                 /* write to the next erase block */
1329                 ret = dfu->write_medium(dfu, offset, fsbl, &size);
1330                 pr_debug("%s copy at ofset=%lx size=%lx ret=%d",
1331                          __func__, offset, size, ret);
1332                 if (ret)
1333                         goto error;
1334         }
1335
1336 error:
1337         free(fsbl);
1338         return ret;
1339 }
1340
1341 static void stm32prog_end_phase(struct stm32prog_data *data)
1342 {
1343         if (data->phase == PHASE_FLASHLAYOUT) {
1344                 if (parse_flash_layout(data, STM32_DDR_BASE, 0))
1345                         stm32prog_err("Layout: invalid FlashLayout");
1346                 return;
1347         }
1348
1349         if (!data->cur_part)
1350                 return;
1351
1352         if (CONFIG_IS_ENABLED(MMC) &&
1353             data->cur_part->part_id < 0) {
1354                 char cmdbuf[60];
1355
1356                 sprintf(cmdbuf, "mmc bootbus %d 0 0 0; mmc partconf %d 1 %d 0",
1357                         data->cur_part->dev_id, data->cur_part->dev_id,
1358                         -(data->cur_part->part_id));
1359                 if (run_command(cmdbuf, 0)) {
1360                         stm32prog_err("commands '%s' failed", cmdbuf);
1361                         return;
1362                 }
1363         }
1364
1365         if (CONFIG_IS_ENABLED(MTD) &&
1366             data->cur_part->bin_nb > 1) {
1367                 if (stm32prog_copy_fsbl(data->cur_part)) {
1368                         stm32prog_err("%s (0x%x): copy of fsbl failed",
1369                                       data->cur_part->name, data->cur_part->id);
1370                         return;
1371                 }
1372         }
1373 }
1374
1375 void stm32prog_do_reset(struct stm32prog_data *data)
1376 {
1377         if (data->phase == PHASE_RESET) {
1378                 data->phase = PHASE_DO_RESET;
1379                 puts("Reset requested\n");
1380         }
1381 }
1382
1383 void stm32prog_next_phase(struct stm32prog_data *data)
1384 {
1385         int phase, i;
1386         struct stm32prog_part_t *part;
1387         bool found;
1388
1389         phase = data->phase;
1390         switch (phase) {
1391         case PHASE_RESET:
1392         case PHASE_END:
1393         case PHASE_DO_RESET:
1394                 return;
1395         }
1396
1397         /* found next selected partition */
1398         data->cur_part = NULL;
1399         data->phase = PHASE_END;
1400         found = false;
1401         do {
1402                 phase++;
1403                 if (phase > PHASE_LAST_USER)
1404                         break;
1405                 for (i = 0; i < data->part_nb; i++) {
1406                         part = &data->part_array[i];
1407                         if (part->id == phase) {
1408                                 if (IS_SELECT(part) && !IS_EMPTY(part)) {
1409                                         data->cur_part = part;
1410                                         data->phase = phase;
1411                                         found = true;
1412                                 }
1413                                 break;
1414                         }
1415                 }
1416         } while (!found);
1417
1418         if (data->phase == PHASE_END)
1419                 puts("Phase=END\n");
1420 }
1421
1422 static int part_delete(struct stm32prog_data *data,
1423                        struct stm32prog_part_t *part)
1424 {
1425         int ret = 0;
1426 #ifdef CONFIG_MMC
1427         unsigned long blks, blks_offset, blks_size;
1428         struct blk_desc *block_dev = NULL;
1429  #endif
1430 #ifdef CONFIG_MTD
1431         char cmdbuf[40];
1432         char devstr[10];
1433 #endif
1434
1435         printf("Erasing %s ", part->name);
1436         switch (part->target) {
1437 #ifdef CONFIG_MMC
1438         case STM32PROG_MMC:
1439                 printf("on mmc %d: ", part->dev->dev_id);
1440                 block_dev = mmc_get_blk_desc(part->dev->mmc);
1441                 blks_offset = lldiv(part->addr, part->dev->mmc->read_bl_len);
1442                 blks_size = lldiv(part->size, part->dev->mmc->read_bl_len);
1443                 /* -1 or -2 : delete boot partition of MMC
1444                  * need to switch to associated hwpart 1 or 2
1445                  */
1446                 if (part->part_id < 0)
1447                         if (blk_select_hwpart_devnum(IF_TYPE_MMC,
1448                                                      part->dev->dev_id,
1449                                                      -part->part_id))
1450                                 return -1;
1451
1452                 blks = blk_derase(block_dev, blks_offset, blks_size);
1453
1454                 /* return to user partition */
1455                 if (part->part_id < 0)
1456                         blk_select_hwpart_devnum(IF_TYPE_MMC,
1457                                                  part->dev->dev_id, 0);
1458                 if (blks != blks_size) {
1459                         ret = -1;
1460                         stm32prog_err("%s (0x%x): MMC erase failed",
1461                                       part->name, part->id);
1462                 }
1463                 break;
1464 #endif
1465 #ifdef CONFIG_MTD
1466         case STM32PROG_NOR:
1467         case STM32PROG_NAND:
1468         case STM32PROG_SPI_NAND:
1469                 get_mtd_by_target(devstr, part->target, part->dev->dev_id);
1470                 printf("on %s: ", devstr);
1471                 sprintf(cmdbuf, "mtd erase %s 0x%llx 0x%llx",
1472                         devstr, part->addr, part->size);
1473                 if (run_command(cmdbuf, 0)) {
1474                         ret = -1;
1475                         stm32prog_err("%s (0x%x): MTD erase commands failed (%s)",
1476                                       part->name, part->id, cmdbuf);
1477                 }
1478                 break;
1479 #endif
1480         default:
1481                 ret = -1;
1482                 stm32prog_err("%s (0x%x): erase invalid", part->name, part->id);
1483                 break;
1484         }
1485         if (!ret)
1486                 printf("done\n");
1487
1488         return ret;
1489 }
1490
1491 static void stm32prog_devices_init(struct stm32prog_data *data)
1492 {
1493         int i;
1494         int ret;
1495         struct stm32prog_part_t *part;
1496
1497         ret = treat_partition_list(data);
1498         if (ret)
1499                 goto error;
1500
1501         /* initialize the selected device */
1502         for (i = 0; i < data->dev_nb; i++) {
1503                 ret = init_device(data, &data->dev[i]);
1504                 if (ret)
1505                         goto error;
1506         }
1507
1508         /* delete RAW partition before create partition */
1509         for (i = 0; i < data->part_nb; i++) {
1510                 part = &data->part_array[i];
1511
1512                 if (part->part_type != RAW_IMAGE)
1513                         continue;
1514
1515                 if (!IS_SELECT(part) || !IS_DELETE(part))
1516                         continue;
1517
1518                 ret = part_delete(data, part);
1519                 if (ret)
1520                         goto error;
1521         }
1522
1523         ret = create_partitions(data);
1524         if (ret)
1525                 goto error;
1526
1527         /* delete partition GPT or MTD */
1528         for (i = 0; i < data->part_nb; i++) {
1529                 part = &data->part_array[i];
1530
1531                 if (part->part_type == RAW_IMAGE)
1532                         continue;
1533
1534                 if (!IS_SELECT(part) || !IS_DELETE(part))
1535                         continue;
1536
1537                 ret = part_delete(data, part);
1538                 if (ret)
1539                         goto error;
1540         }
1541
1542         return;
1543
1544 error:
1545         data->part_nb = 0;
1546 }
1547
1548 int stm32prog_dfu_init(struct stm32prog_data *data)
1549 {
1550         /* init device if no error */
1551         if (data->part_nb)
1552                 stm32prog_devices_init(data);
1553
1554         if (data->part_nb)
1555                 stm32prog_next_phase(data);
1556
1557         /* prepare DFU for device read/write */
1558         dfu_free_entities();
1559         return dfu_init_entities(data);
1560 }
1561
1562 int stm32prog_init(struct stm32prog_data *data, ulong addr, ulong size)
1563 {
1564         memset(data, 0x0, sizeof(*data));
1565         data->phase = PHASE_FLASHLAYOUT;
1566
1567         return parse_flash_layout(data, addr, size);
1568 }
1569
1570 void stm32prog_clean(struct stm32prog_data *data)
1571 {
1572         /* clean */
1573         dfu_free_entities();
1574         free(data->part_array);
1575         free(data->otp_part);
1576         free(data->header_data);
1577 }
1578
1579 /* DFU callback: used after serial and direct DFU USB access */
1580 void dfu_flush_callback(struct dfu_entity *dfu)
1581 {
1582         if (!stm32prog_data)
1583                 return;
1584
1585         if (dfu->dev_type == DFU_DEV_VIRT) {
1586                 if (dfu->data.virt.dev_num == PHASE_OTP)
1587                         stm32prog_otp_start(stm32prog_data);
1588                 return;
1589         }
1590
1591         if (dfu->dev_type == DFU_DEV_RAM) {
1592                 if (dfu->alt == 0 &&
1593                     stm32prog_data->phase == PHASE_FLASHLAYOUT) {
1594                         stm32prog_end_phase(stm32prog_data);
1595                         /* waiting DFU DETACH for reenumeration */
1596                 }
1597         }
1598
1599         if (!stm32prog_data->cur_part)
1600                 return;
1601
1602         if (dfu->alt == stm32prog_data->cur_part->alt_id) {
1603                 stm32prog_end_phase(stm32prog_data);
1604                 stm32prog_next_phase(stm32prog_data);
1605         }
1606 }
1607
1608 void dfu_initiated_callback(struct dfu_entity *dfu)
1609 {
1610         if (!stm32prog_data)
1611                 return;
1612
1613         if (!stm32prog_data->cur_part)
1614                 return;
1615
1616         /* force the saved offset for the current partition */
1617         if (dfu->alt == stm32prog_data->cur_part->alt_id) {
1618                 dfu->offset = stm32prog_data->offset;
1619                 pr_debug("dfu offset = 0x%llx\n", dfu->offset);
1620         }
1621 }