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