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