board/gdsys/p1022/controlcenterd-id.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * (C) Copyright 2013
   4  * Reinhard Pfau, Guntermann & Drunck GmbH, reinhard.pfau@gdsys.cc
   5  */
   6
   7 /* TODO: some more #ifdef's to avoid unneeded code for stage 1 / stage 2 */
   8
   9 #ifdef CCDM_ID_DEBUG
  10 #define DEBUG
  11 #endif
  12
  13 #include <common.h>
  14 #include <bootstage.h>
  15 #include <command.h>
  16 #include <dm.h>
  17 #include <env.h>
  18 #include <hang.h>
  19 #include <log.h>
  20 #include <malloc.h>
  21 #include <fs.h>
  22 #include <i2c.h>
  23 #include <mmc.h>
  24 #include <tpm-v1.h>
  25 #include <linux/delay.h>
  26 #include <u-boot/crc.h>
  27 #include <u-boot/sha1.h>
  28 #include <asm/byteorder.h>
  29 #include <asm/unaligned.h>
  30 #include <pca9698.h>
  31
  32 #undef CCDM_FIRST_STAGE
  33 #undef CCDM_SECOND_STAGE
  34 #undef CCDM_AUTO_FIRST_STAGE
  35
  36 #ifdef CONFIG_DEVELOP
  37 #define CCDM_DEVELOP
  38 #endif
  39
  40 #ifdef CONFIG_TRAILBLAZER
  41 #define CCDM_FIRST_STAGE
  42 #undef CCDM_SECOND_STAGE
  43 #else
  44 #undef CCDM_FIRST_STAGE
  45 #define CCDM_SECOND_STAGE
  46 #endif
  47
  48 #if defined(CCDM_DEVELOP) && defined(CCDM_SECOND_STAGE) && \
  49         !defined(CCCM_FIRST_STAGE)
  50 #define CCDM_AUTO_FIRST_STAGE
  51 #endif
  52
  53 /* CCDM specific contants */
  54 enum {
  55         /* NV indices */
  56         NV_COMMON_DATA_INDEX    = 0x40000001,
  57         /* magics for key blob chains */
  58         MAGIC_KEY_PROGRAM       = 0x68726500,
  59         MAGIC_HMAC              = 0x68616300,
  60         MAGIC_END_OF_CHAIN      = 0x00000000,
  61         /* sizes */
  62         NV_COMMON_DATA_MIN_SIZE = 3 * sizeof(uint64_t) + 2 * sizeof(uint16_t),
  63 };
  64
  65 /* other constants */
  66 enum {
  67         ESDHC_BOOT_IMAGE_SIG_OFS        = 0x40,
  68         ESDHC_BOOT_IMAGE_SIZE_OFS       = 0x48,
  69         ESDHC_BOOT_IMAGE_ADDR_OFS       = 0x50,
  70         ESDHC_BOOT_IMAGE_TARGET_OFS     = 0x58,
  71         ESDHC_BOOT_IMAGE_ENTRY_OFS      = 0x60,
  72 };
  73
  74 enum {
  75         I2C_SOC_0 = 0,
  76         I2C_SOC_1 = 1,
  77 };
  78
  79 struct key_program {
  80         uint32_t magic;
  81         uint32_t code_crc;
  82         uint32_t code_size;
  83         uint8_t code[];
  84 };
  85
  86 struct h_reg {
  87         bool valid;
  88         uint8_t digest[20];
  89 };
  90
  91
  92 enum access_mode {
  93         HREG_NONE       = 0,
  94         HREG_RD         = 1,
  95         HREG_WR         = 2,
  96         HREG_RDWR       = 3,
  97 };
  98
  99 /* register constants */
 100 enum {
 101         FIX_HREG_DEVICE_ID_HASH = 0,
 102         FIX_HREG_SELF_HASH      = 1,
 103         FIX_HREG_STAGE2_HASH    = 2,
 104         FIX_HREG_VENDOR         = 3,
 105         COUNT_FIX_HREGS
 106 };
 107
 108
 109 /* hre opcodes */
 110 enum {
 111         /* opcodes w/o data */
 112         HRE_NOP         = 0x00,
 113         HRE_SYNC        = HRE_NOP,
 114         HRE_CHECK0      = 0x01,
 115         /* opcodes w/o data, w/ sync dst */
 116         /* opcodes w/ data */
 117         HRE_LOAD        = 0x81,
 118         /* opcodes w/data, w/sync dst */
 119         HRE_XOR         = 0xC1,
 120         HRE_AND         = 0xC2,
 121         HRE_OR          = 0xC3,
 122         HRE_EXTEND      = 0xC4,
 123         HRE_LOADKEY     = 0xC5,
 124 };
 125
 126 /* hre errors */
 127 enum {
 128         HRE_E_OK        = 0,
 129         HRE_E_TPM_FAILURE,
 130         HRE_E_INVALID_HREG,
 131 };
 132
 133 static uint64_t device_id;
 134 static uint64_t device_cl;
 135 static uint64_t device_type;
 136
 137 static uint32_t platform_key_handle;
 138
 139 static void(*bl2_entry)(void);
 140
 141 static struct h_reg pcr_hregs[24];
 142 static struct h_reg fix_hregs[COUNT_FIX_HREGS];
 143 static struct h_reg var_hregs[8];
 144 static uint32_t hre_tpm_err;
 145 static int hre_err = HRE_E_OK;
 146
 147 #define IS_PCR_HREG(spec) ((spec) & 0x20)
 148 #define IS_FIX_HREG(spec) (((spec) & 0x38) == 0x08)
 149 #define IS_VAR_HREG(spec) (((spec) & 0x38) == 0x10)
 150 #define HREG_IDX(spec) ((spec) & (IS_PCR_HREG(spec) ? 0x1f : 0x7))
 151
 152 static int get_tpm(struct udevice **devp)
 153 {
 154         int rc;
 155
 156         rc = uclass_first_device_err(UCLASS_TPM, devp);
 157         if (rc) {
 158                 printf("Could not find TPM (ret=%d)\n", rc);
 159                 return CMD_RET_FAILURE;
 160         }
 161
 162         return 0;
 163 }
 164
 165 static const uint8_t vendor[] = "Guntermann & Drunck";
 166
 167 /**
 168  * @brief read a bunch of data from MMC into memory.
 169  *
 170  * @param mmc   pointer to the mmc structure to use.
 171  * @param src   offset where the data starts on MMC/SD device (in bytes).
 172  * @param dst   pointer to the location where the read data should be stored.
 173  * @param size  number of bytes to read from the MMC/SD device.
 174  * @return number of bytes read or -1 on error.
 175  */
 176 static int ccdm_mmc_read(struct mmc *mmc, u64 src, u8 *dst, int size)
 177 {
 178         int result = 0;
 179         u32 blk_len, ofs;
 180         ulong block_no, n, cnt;
 181         u8 *tmp_buf = NULL;
 182
 183         if (size <= 0)
 184                 goto end;
 185
 186         blk_len = mmc->read_bl_len;
 187         tmp_buf = malloc(blk_len);
 188         if (!tmp_buf)
 189                 goto failure;
 190         block_no = src / blk_len;
 191         ofs = src % blk_len;
 192
 193         if (ofs) {
 194                 n = mmc->block_dev.block_read(&mmc->block_dev, block_no++, 1,
 195                         tmp_buf);
 196                 if (!n)
 197                         goto failure;
 198                 result = min(size, (int)(blk_len - ofs));
 199                 memcpy(dst, tmp_buf + ofs, result);
 200                 dst += result;
 201                 size -= result;
 202         }
 203         cnt = size / blk_len;
 204         if (cnt) {
 205                 n = mmc->block_dev.block_read(&mmc->block_dev, block_no, cnt,
 206                         dst);
 207                 if (n != cnt)
 208                         goto failure;
 209                 size -= cnt * blk_len;
 210                 result += cnt * blk_len;
 211                 dst += cnt * blk_len;
 212                 block_no += cnt;
 213         }
 214         if (size) {
 215                 n = mmc->block_dev.block_read(&mmc->block_dev, block_no++, 1,
 216                         tmp_buf);
 217                 if (!n)
 218                         goto failure;
 219                 memcpy(dst, tmp_buf, size);
 220                 result += size;
 221         }
 222         goto end;
 223 failure:
 224         result = -1;
 225 end:
 226         if (tmp_buf)
 227                 free(tmp_buf);
 228         return result;
 229 }
 230
 231 /**
 232  * @brief returns a location where the 2nd stage bootloader can be(/ is) placed.
 233  *
 234  * @return pointer to the location for/of the 2nd stage bootloader
 235  */
 236 static u8 *get_2nd_stage_bl_location(ulong target_addr)
 237 {
 238         ulong addr;
 239 #ifdef CCDM_SECOND_STAGE
 240         addr = env_get_ulong("loadaddr", 16, CONFIG_LOADADDR);
 241 #else
 242         addr = target_addr;
 243 #endif
 244         return (u8 *)(addr);
 245 }
 246
 247
 248 #ifdef CCDM_SECOND_STAGE
 249 /**
 250  * @brief returns a location where the image can be(/ is) placed.
 251  *
 252  * @return pointer to the location for/of the image
 253  */
 254 static u8 *get_image_location(void)
 255 {
 256         ulong addr;
 257         /* TODO use other area? */
 258         addr = env_get_ulong("loadaddr", 16, CONFIG_LOADADDR);
 259         return (u8 *)(addr);
 260 }
 261 #endif
 262
 263 /**
 264  * @brief get the size of a given (TPM) NV area
 265  * @param index NV index of the area to get size for
 266  * @param size  pointer to the size
 267  * @return 0 on success, != 0 on error
 268  */
 269 static int get_tpm_nv_size(struct udevice *tpm, uint32_t index, uint32_t *size)
 270 {
 271         uint32_t err;
 272         uint8_t info[72];
 273         uint8_t *ptr;
 274         uint16_t v16;
 275
 276         err = tpm_get_capability(tpm, TPM_CAP_NV_INDEX, index,
 277                                  info, sizeof(info));
 278         if (err) {
 279                 printf("tpm_get_capability(CAP_NV_INDEX, %08x) failed: %u\n",
 280                        index, err);
 281                 return 1;
 282         }
 283
 284         /* skip tag and nvIndex */
 285         ptr = info + 6;
 286         /* skip 2 pcr info fields */
 287         v16 = get_unaligned_be16(ptr);
 288         ptr += 2 + v16 + 1 + 20;
 289         v16 = get_unaligned_be16(ptr);
 290         ptr += 2 + v16 + 1 + 20;
 291         /* skip permission and flags */
 292         ptr += 6 + 3;
 293
 294         *size = get_unaligned_be32(ptr);
 295         return 0;
 296 }
 297
 298 /**
 299  * @brief search for a key by usage auth and pub key hash.
 300  * @param auth  usage auth of the key to search for
 301  * @param pubkey_digest (SHA1) hash of the pub key structure of the key
 302  * @param[out] handle   the handle of the key iff found
 303  * @return 0 if key was found in TPM; != 0 if not.
 304  */
 305 static int find_key(struct udevice *tpm, const uint8_t auth[20],
 306                     const uint8_t pubkey_digest[20], uint32_t *handle)
 307 {
 308         uint16_t key_count;
 309         uint32_t key_handles[10];
 310         uint8_t buf[288];
 311         uint8_t *ptr;
 312         uint32_t err;
 313         uint8_t digest[20];
 314         size_t buf_len;
 315         unsigned int i;
 316
 317         /* fetch list of already loaded keys in the TPM */
 318         err = tpm_get_capability(tpm, TPM_CAP_HANDLE, TPM_RT_KEY, buf,
 319                                  sizeof(buf));
 320         if (err)
 321                 return -1;
 322         key_count = get_unaligned_be16(buf);
 323         ptr = buf + 2;
 324         for (i = 0; i < key_count; ++i, ptr += 4)
 325                 key_handles[i] = get_unaligned_be32(ptr);
 326
 327         /* now search a(/ the) key which we can access with the given auth */
 328         for (i = 0; i < key_count; ++i) {
 329                 buf_len = sizeof(buf);
 330                 err = tpm_get_pub_key_oiap(tpm, key_handles[i], auth, buf,
 331                                            &buf_len);
 332                 if (err && err != TPM_AUTHFAIL)
 333                         return -1;
 334                 if (err)
 335                         continue;
 336                 sha1_csum(buf, buf_len, digest);
 337                 if (!memcmp(digest, pubkey_digest, 20)) {
 338                         *handle = key_handles[i];
 339                         return 0;
 340                 }
 341         }
 342         return 1;
 343 }
 344
 345 /**
 346  * @brief read CCDM common data from TPM NV
 347  * @return 0 if CCDM common data was found and read, !=0 if something failed.
 348  */
 349 static int read_common_data(struct udevice *tpm)
 350 {
 351         uint32_t size;
 352         uint32_t err;
 353         uint8_t buf[256];
 354         sha1_context ctx;
 355
 356         if (get_tpm_nv_size(tpm, NV_COMMON_DATA_INDEX, &size) ||
 357             size < NV_COMMON_DATA_MIN_SIZE)
 358                 return 1;
 359         err = tpm_nv_read_value(tpm, NV_COMMON_DATA_INDEX,
 360                                 buf, min(sizeof(buf), size));
 361         if (err) {
 362                 printf("tpm_nv_read_value() failed: %u\n", err);
 363                 return 1;
 364         }
 365
 366         device_id = get_unaligned_be64(buf);
 367         device_cl = get_unaligned_be64(buf + 8);
 368         device_type = get_unaligned_be64(buf + 16);
 369
 370         sha1_starts(&ctx);
 371         sha1_update(&ctx, buf, 24);
 372         sha1_finish(&ctx, fix_hregs[FIX_HREG_DEVICE_ID_HASH].digest);
 373         fix_hregs[FIX_HREG_DEVICE_ID_HASH].valid = true;
 374
 375         platform_key_handle = get_unaligned_be32(buf + 24);
 376
 377         return 0;
 378 }
 379
 380 /**
 381  * @brief compute hash of bootloader itself.
 382  * @param[out] dst      hash register where the hash should be stored
 383  * @return 0 on success, != 0 on failure.
 384  *
 385  * @note MUST be called at a time where the boot loader is accessible at the
 386  * configured location (; so take care when code is reallocated).
 387  */
 388 static int compute_self_hash(struct h_reg *dst)
 389 {
 390         sha1_csum((const uint8_t *)CONFIG_SYS_MONITOR_BASE,
 391                   CONFIG_SYS_MONITOR_LEN, dst->digest);
 392         dst->valid = true;
 393         return 0;
 394 }
 395
 396 int ccdm_compute_self_hash(void)
 397 {
 398         if (!fix_hregs[FIX_HREG_SELF_HASH].valid)
 399                 compute_self_hash(&fix_hregs[FIX_HREG_SELF_HASH]);
 400         return 0;
 401 }
 402
 403 /**
 404  * @brief compute the hash of the 2nd stage boot loader (on SD card)
 405  * @param[out] dst      hash register to store the computed hash
 406  * @return 0 on success, != 0 on failure
 407  *
 408  * Determines the size and location of the 2nd stage boot loader on SD card,
 409  * loads the 2nd stage boot loader and computes the (SHA1) hash value.
 410  * Within the 1st stage boot loader, the 2nd stage boot loader is loaded at
 411  * the desired memory location and the variable @a bl2_entry is set.
 412  *
 413  * @note This sets the variable @a bl2_entry to the entry point when the
 414  * 2nd stage boot loader is loaded at its configured memory location.
 415  */
 416 static int compute_second_stage_hash(struct h_reg *dst)
 417 {
 418         int result = 0;
 419         u32 code_len, code_offset, target_addr, exec_entry;
 420         struct mmc *mmc;
 421         u8 *load_addr = NULL;
 422         u8 buf[128];
 423
 424         mmc = find_mmc_device(0);
 425         if (!mmc)
 426                 goto failure;
 427         mmc_init(mmc);
 428
 429         if (ccdm_mmc_read(mmc, 0, buf, sizeof(buf)) < 0)
 430                 goto failure;
 431
 432         code_offset = *(u32 *)(buf + ESDHC_BOOT_IMAGE_ADDR_OFS);
 433         code_len = *(u32 *)(buf + ESDHC_BOOT_IMAGE_SIZE_OFS);
 434         target_addr = *(u32 *)(buf + ESDHC_BOOT_IMAGE_TARGET_OFS);
 435         exec_entry =  *(u32 *)(buf + ESDHC_BOOT_IMAGE_ENTRY_OFS);
 436
 437         load_addr = get_2nd_stage_bl_location(target_addr);
 438         if (load_addr == (u8 *)target_addr)
 439                 bl2_entry = (void(*)(void))exec_entry;
 440
 441         if (ccdm_mmc_read(mmc, code_offset, load_addr, code_len) < 0)
 442                 goto failure;
 443
 444         sha1_csum(load_addr, code_len, dst->digest);
 445         dst->valid = true;
 446
 447         goto end;
 448 failure:
 449         result = 1;
 450         bl2_entry = NULL;
 451 end:
 452         return result;
 453 }
 454
 455 /**
 456  * @brief get pointer to  hash register by specification
 457  * @param spec  specification of a hash register
 458  * @return pointer to hash register or NULL if @a spec does not qualify a
 459  * valid hash register; NULL else.
 460  */
 461 static struct h_reg *get_hreg(uint8_t spec)
 462 {
 463         uint8_t idx;
 464
 465         idx = HREG_IDX(spec);
 466         if (IS_FIX_HREG(spec)) {
 467                 if (idx < ARRAY_SIZE(fix_hregs))
 468                         return fix_hregs + idx;
 469                 hre_err = HRE_E_INVALID_HREG;
 470         } else if (IS_PCR_HREG(spec)) {
 471                 if (idx < ARRAY_SIZE(pcr_hregs))
 472                         return pcr_hregs + idx;
 473                 hre_err = HRE_E_INVALID_HREG;
 474         } else if (IS_VAR_HREG(spec)) {
 475                 if (idx < ARRAY_SIZE(var_hregs))
 476                         return var_hregs + idx;
 477                 hre_err = HRE_E_INVALID_HREG;
 478         }
 479         return NULL;
 480 }
 481
 482 /**
 483  * @brief get pointer of a hash register by specification and usage.
 484  * @param spec  specification of a hash register
 485  * @param mode  access mode (read or write or read/write)
 486  * @return pointer to hash register if found and valid; NULL else.
 487  *
 488  * This func uses @a get_reg() to determine the hash register for a given spec.
 489  * If a register is found it is validated according to the desired access mode.
 490  * The value of automatic registers (PCR register and fixed registers) is
 491  * loaded or computed on read access.
 492  */
 493 static struct h_reg *access_hreg(struct udevice *tpm, uint8_t spec,
 494                                  enum access_mode mode)
 495 {
 496         struct h_reg *result;
 497
 498         result = get_hreg(spec);
 499         if (!result)
 500                 return NULL;
 501
 502         if (mode & HREG_WR) {
 503                 if (IS_FIX_HREG(spec)) {
 504                         hre_err = HRE_E_INVALID_HREG;
 505                         return NULL;
 506                 }
 507         }
 508         if (mode & HREG_RD) {
 509                 if (!result->valid) {
 510                         if (IS_PCR_HREG(spec)) {
 511                                 hre_tpm_err = tpm_pcr_read(tpm, HREG_IDX(spec),
 512                                         result->digest, 20);
 513                                 result->valid = (hre_tpm_err == TPM_SUCCESS);
 514                         } else if (IS_FIX_HREG(spec)) {
 515                                 switch (HREG_IDX(spec)) {
 516                                 case FIX_HREG_DEVICE_ID_HASH:
 517                                         read_common_data(tpm);
 518                                         break;
 519                                 case FIX_HREG_SELF_HASH:
 520                                         ccdm_compute_self_hash();
 521                                         break;
 522                                 case FIX_HREG_STAGE2_HASH:
 523                                         compute_second_stage_hash(result);
 524                                         break;
 525                                 case FIX_HREG_VENDOR:
 526                                         memcpy(result->digest, vendor, 20);
 527                                         result->valid = true;
 528                                         break;
 529                                 }
 530                         } else {
 531                                 result->valid = true;
 532                         }
 533                 }
 534                 if (!result->valid) {
 535                         hre_err = HRE_E_INVALID_HREG;
 536                         return NULL;
 537                 }
 538         }
 539
 540         return result;
 541 }
 542
 543 static void *compute_and(void *_dst, const void *_src, size_t n)
 544 {
 545         uint8_t *dst = _dst;
 546         const uint8_t *src = _src;
 547         size_t i;
 548
 549         for (i = n; i-- > 0; )
 550                 *dst++ &= *src++;
 551
 552         return _dst;
 553 }
 554
 555 static void *compute_or(void *_dst, const void *_src, size_t n)
 556 {
 557         uint8_t *dst = _dst;
 558         const uint8_t *src = _src;
 559         size_t i;
 560
 561         for (i = n; i-- > 0; )
 562                 *dst++ |= *src++;
 563
 564         return _dst;
 565 }
 566
 567 static void *compute_xor(void *_dst, const void *_src, size_t n)
 568 {
 569         uint8_t *dst = _dst;
 570         const uint8_t *src = _src;
 571         size_t i;
 572
 573         for (i = n; i-- > 0; )
 574                 *dst++ ^= *src++;
 575
 576         return _dst;
 577 }
 578
 579 static void *compute_extend(void *_dst, const void *_src, size_t n)
 580 {
 581         uint8_t digest[20];
 582         sha1_context ctx;
 583
 584         sha1_starts(&ctx);
 585         sha1_update(&ctx, _dst, n);
 586         sha1_update(&ctx, _src, n);
 587         sha1_finish(&ctx, digest);
 588         memcpy(_dst, digest, min(n, sizeof(digest)));
 589
 590         return _dst;
 591 }
 592
 593 static int hre_op_loadkey(struct udevice *tpm, struct h_reg *src_reg,
 594                           struct h_reg *dst_reg, const void *key,
 595                           size_t key_size)
 596 {
 597         uint32_t parent_handle;
 598         uint32_t key_handle;
 599
 600         if (!src_reg || !dst_reg || !src_reg->valid || !dst_reg->valid)
 601                 return -1;
 602         if (find_key(tpm, src_reg->digest, dst_reg->digest, &parent_handle))
 603                 return -1;
 604         hre_tpm_err = tpm_load_key2_oiap(tpm, parent_handle, key, key_size,
 605                                          src_reg->digest, &key_handle);
 606         if (hre_tpm_err) {
 607                 hre_err = HRE_E_TPM_FAILURE;
 608                 return -1;
 609         }
 610         /* TODO remember key handle somehow? */
 611
 612         return 0;
 613 }
 614
 615 /**
 616  * @brief executes the next opcode on the hash register engine.
 617  * @param[in,out] ip    pointer to the opcode (instruction pointer)
 618  * @param[in,out] code_size     (remaining) size of the code
 619  * @return new instruction pointer on success, NULL on error.
 620  */
 621 static const uint8_t *hre_execute_op(struct udevice *tpm, const uint8_t **ip,
 622                                      size_t *code_size)
 623 {
 624         bool dst_modified = false;
 625         uint32_t ins;
 626         uint8_t opcode;
 627         uint8_t src_spec;
 628         uint8_t dst_spec;
 629         uint16_t data_size;
 630         struct h_reg *src_reg, *dst_reg;
 631         uint8_t buf[20];
 632         const uint8_t *src_buf, *data;
 633         uint8_t *ptr;
 634         int i;
 635         void * (*bin_func)(void *, const void *, size_t);
 636
 637         if (*code_size < 4)
 638                 return NULL;
 639
 640         ins = get_unaligned_be32(*ip);
 641         opcode = **ip;
 642         data = *ip + 4;
 643         src_spec = (ins >> 18) & 0x3f;
 644         dst_spec = (ins >> 12) & 0x3f;
 645         data_size = (ins & 0x7ff);
 646
 647         debug("HRE: ins=%08x (op=%02x, s=%02x, d=%02x, L=%d)\n", ins,
 648               opcode, src_spec, dst_spec, data_size);
 649
 650         if ((opcode & 0x80) && (data_size + 4) > *code_size)
 651                 return NULL;
 652
 653         src_reg = access_hreg(tpm, src_spec, HREG_RD);
 654         if (hre_err || hre_tpm_err)
 655                 return NULL;
 656         dst_reg = access_hreg(tpm, dst_spec,
 657                               (opcode & 0x40) ? HREG_RDWR : HREG_WR);
 658         if (hre_err || hre_tpm_err)
 659                 return NULL;
 660
 661         switch (opcode) {
 662         case HRE_NOP:
 663                 goto end;
 664         case HRE_CHECK0:
 665                 if (src_reg) {
 666                         for (i = 0; i < 20; ++i) {
 667                                 if (src_reg->digest[i])
 668                                         return NULL;
 669                         }
 670                 }
 671                 break;
 672         case HRE_LOAD:
 673                 bin_func = memcpy;
 674                 goto do_bin_func;
 675         case HRE_XOR:
 676                 bin_func = compute_xor;
 677                 goto do_bin_func;
 678         case HRE_AND:
 679                 bin_func = compute_and;
 680                 goto do_bin_func;
 681         case HRE_OR:
 682                 bin_func = compute_or;
 683                 goto do_bin_func;
 684         case HRE_EXTEND:
 685                 bin_func = compute_extend;
 686 do_bin_func:
 687                 if (!dst_reg)
 688                         return NULL;
 689                 if (src_reg) {
 690                         src_buf = src_reg->digest;
 691                 } else {
 692                         if (!data_size) {
 693                                 memset(buf, 0, 20);
 694                                 src_buf = buf;
 695                         } else if (data_size == 1) {
 696                                 memset(buf, *data, 20);
 697                                 src_buf = buf;
 698                         } else if (data_size >= 20) {
 699                                 src_buf = data;
 700                         } else {
 701                                 src_buf = buf;
 702                                 for (ptr = (uint8_t *)src_buf, i = 20; i > 0;
 703                                         i -= data_size, ptr += data_size)
 704                                         memcpy(ptr, data,
 705                                                min_t(size_t, i, data_size));
 706                         }
 707                 }
 708                 bin_func(dst_reg->digest, src_buf, 20);
 709                 dst_reg->valid = true;
 710                 dst_modified = true;
 711                 break;
 712         case HRE_LOADKEY:
 713                 if (hre_op_loadkey(tpm, src_reg, dst_reg, data, data_size))
 714                         return NULL;
 715                 break;
 716         default:
 717                 return NULL;
 718         }
 719
 720         if (dst_reg && dst_modified && IS_PCR_HREG(dst_spec)) {
 721                 hre_tpm_err = tpm_extend(tpm, HREG_IDX(dst_spec),
 722                                          dst_reg->digest, dst_reg->digest);
 723                 if (hre_tpm_err) {
 724                         hre_err = HRE_E_TPM_FAILURE;
 725                         return NULL;
 726                 }
 727         }
 728 end:
 729         *ip += 4;
 730         *code_size -= 4;
 731         if (opcode & 0x80) {
 732                 *ip += data_size;
 733                 *code_size -= data_size;
 734         }
 735
 736         return *ip;
 737 }
 738
 739 /**
 740  * @brief runs a program on the hash register engine.
 741  * @param code          pointer to the (HRE) code.
 742  * @param code_size     size of the code (in bytes).
 743  * @return 0 on success, != 0 on failure.
 744  */
 745 static int hre_run_program(struct udevice *tpm, const uint8_t *code,
 746                            size_t code_size)
 747 {
 748         size_t code_left;
 749         const uint8_t *ip = code;
 750
 751         code_left = code_size;
 752         hre_tpm_err = 0;
 753         hre_err = HRE_E_OK;
 754         while (code_left > 0)
 755                 if (!hre_execute_op(tpm, &ip, &code_left))
 756                         return -1;
 757
 758         return hre_err;
 759 }
 760
 761 static int check_hmac(struct key_program *hmac,
 762         const uint8_t *data, size_t data_size)
 763 {
 764         uint8_t key[20], computed_hmac[20];
 765         uint32_t type;
 766
 767         type = get_unaligned_be32(hmac->code);
 768         if (type != 0)
 769                 return 1;
 770         memset(key, 0, sizeof(key));
 771         compute_extend(key, pcr_hregs[1].digest, 20);
 772         compute_extend(key, pcr_hregs[2].digest, 20);
 773         compute_extend(key, pcr_hregs[3].digest, 20);
 774         compute_extend(key, pcr_hregs[4].digest, 20);
 775
 776         sha1_hmac(key, sizeof(key), data, data_size, computed_hmac);
 777
 778         return memcmp(computed_hmac, hmac->code + 4, 20);
 779 }
 780
 781 static int verify_program(struct key_program *prg)
 782 {
 783         uint32_t crc;
 784         crc = crc32(0, prg->code, prg->code_size);
 785
 786         if (crc != prg->code_crc) {
 787                 printf("HRC crc mismatch: %08x != %08x\n",
 788                        crc, prg->code_crc);
 789                 return 1;
 790         }
 791         return 0;
 792 }
 793
 794 #if defined(CCDM_FIRST_STAGE) || (defined CCDM_AUTO_FIRST_STAGE)
 795 static struct key_program *load_sd_key_program(void)
 796 {
 797         u32 code_len, code_offset;
 798         struct mmc *mmc;
 799         u8 buf[128];
 800         struct key_program *result = NULL, *hmac = NULL;
 801         struct key_program header;
 802
 803         mmc = find_mmc_device(0);
 804         if (!mmc)
 805                 return NULL;
 806         mmc_init(mmc);
 807
 808         if (ccdm_mmc_read(mmc, 0, buf, sizeof(buf)) <= 0)
 809                 goto failure;
 810
 811         code_offset = *(u32 *)(buf + ESDHC_BOOT_IMAGE_ADDR_OFS);
 812         code_len = *(u32 *)(buf + ESDHC_BOOT_IMAGE_SIZE_OFS);
 813
 814         code_offset += code_len;
 815         /* TODO: the following needs to be the size of the 2nd stage env */
 816         code_offset += CONFIG_ENV_SIZE;
 817
 818         if (ccdm_mmc_read(mmc, code_offset, buf, 4*3) < 0)
 819                 goto failure;
 820
 821         header.magic = get_unaligned_be32(buf);
 822         header.code_crc = get_unaligned_be32(buf + 4);
 823         header.code_size = get_unaligned_be32(buf + 8);
 824
 825         if (header.magic != MAGIC_KEY_PROGRAM)
 826                 goto failure;
 827
 828         result = malloc(sizeof(struct key_program) + header.code_size);
 829         if (!result)
 830                 goto failure;
 831         *result = header;
 832
 833         printf("load key program chunk from SD card (%u bytes) ",
 834                header.code_size);
 835         code_offset += 12;
 836         if (ccdm_mmc_read(mmc, code_offset, result->code, header.code_size)
 837                 < 0)
 838                 goto failure;
 839         code_offset += header.code_size;
 840         puts("\n");
 841
 842         if (verify_program(result))
 843                 goto failure;
 844
 845         if (ccdm_mmc_read(mmc, code_offset, buf, 4*3) < 0)
 846                 goto failure;
 847
 848         header.magic = get_unaligned_be32(buf);
 849         header.code_crc = get_unaligned_be32(buf + 4);
 850         header.code_size = get_unaligned_be32(buf + 8);
 851
 852         if (header.magic == MAGIC_HMAC) {
 853                 puts("check integrity\n");
 854                 hmac = malloc(sizeof(struct key_program) + header.code_size);
 855                 if (!hmac)
 856                         goto failure;
 857                 *hmac = header;
 858                 code_offset += 12;
 859                 if (ccdm_mmc_read(mmc, code_offset, hmac->code,
 860                                   hmac->code_size) < 0)
 861                         goto failure;
 862                 if (verify_program(hmac))
 863                         goto failure;
 864                 if (check_hmac(hmac, result->code, result->code_size)) {
 865                         puts("key program integrity could not be verified\n");
 866                         goto failure;
 867                 }
 868                 puts("key program verified\n");
 869         }
 870
 871         goto end;
 872 failure:
 873         if (result)
 874                 free(result);
 875         result = NULL;
 876 end:
 877         if (hmac)
 878                 free(hmac);
 879
 880         return result;
 881 }
 882 #endif
 883
 884 #ifdef CCDM_SECOND_STAGE
 885 /**
 886  * @brief load a key program from file system.
 887  * @param ifname        interface of the file system
 888  * @param dev_part_str  device part of the file system
 889  * @param fs_type       tyep of the file system
 890  * @param path          path of the file to load.
 891  * @return the loaded structure or NULL on failure.
 892  */
 893 static struct key_program *load_key_chunk(const char *ifname,
 894         const char *dev_part_str, int fs_type,
 895         const char *path)
 896 {
 897         struct key_program *result = NULL;
 898         struct key_program header;
 899         uint32_t crc;
 900         uint8_t buf[12];
 901         loff_t i;
 902
 903         if (fs_set_blk_dev(ifname, dev_part_str, fs_type))
 904                 goto failure;
 905         if (fs_read(path, (ulong)buf, 0, 12, &i) < 0)
 906                 goto failure;
 907         if (i < 12)
 908                 goto failure;
 909         header.magic = get_unaligned_be32(buf);
 910         header.code_crc = get_unaligned_be32(buf + 4);
 911         header.code_size = get_unaligned_be32(buf + 8);
 912
 913         if (header.magic != MAGIC_HMAC && header.magic != MAGIC_KEY_PROGRAM)
 914                 goto failure;
 915
 916         result = malloc(sizeof(struct key_program) + header.code_size);
 917         if (!result)
 918                 goto failure;
 919         if (fs_set_blk_dev(ifname, dev_part_str, fs_type))
 920                 goto failure;
 921         if (fs_read(path, (ulong)result, 0,
 922                     sizeof(struct key_program) + header.code_size, &i) < 0)
 923                 goto failure;
 924         if (i <= 0)
 925                 goto failure;
 926         *result = header;
 927
 928         crc = crc32(0, result->code, result->code_size);
 929
 930         if (crc != result->code_crc) {
 931                 printf("%s: HRC crc mismatch: %08x != %08x\n",
 932                        path, crc, result->code_crc);
 933                 goto failure;
 934         }
 935         goto end;
 936 failure:
 937         if (result) {
 938                 free(result);
 939                 result = NULL;
 940         }
 941 end:
 942         return result;
 943 }
 944 #endif
 945
 946 #if defined(CCDM_FIRST_STAGE) || (defined CCDM_AUTO_FIRST_STAGE)
 947 static const uint8_t prg_stage1_prepare[] = {
 948         0x00, 0x20, 0x00, 0x00, /* opcode: SYNC f0 */
 949         0x00, 0x24, 0x00, 0x00, /* opcode: SYNC f1 */
 950         0x01, 0x80, 0x00, 0x00, /* opcode: CHECK0 PCR0 */
 951         0x81, 0x22, 0x00, 0x00, /* opcode: LOAD PCR0, f0 */
 952         0x01, 0x84, 0x00, 0x00, /* opcode: CHECK0 PCR1 */
 953         0x81, 0x26, 0x10, 0x00, /* opcode: LOAD PCR1, f1 */
 954         0x01, 0x88, 0x00, 0x00, /* opcode: CHECK0 PCR2 */
 955         0x81, 0x2a, 0x20, 0x00, /* opcode: LOAD PCR2, f2 */
 956         0x01, 0x8c, 0x00, 0x00, /* opcode: CHECK0 PCR3 */
 957         0x81, 0x2e, 0x30, 0x00, /* opcode: LOAD PCR3, f3 */
 958 };
 959
 960 static int first_stage_actions(struct udevice *tpm)
 961 {
 962         int result = 0;
 963         struct key_program *sd_prg = NULL;
 964
 965         puts("CCDM S1: start actions\n");
 966 #ifndef CCDM_SECOND_STAGE
 967         if (tpm_continue_self_test(tpm))
 968                 goto failure;
 969 #else
 970         tpm_continue_self_test(tpm);
 971 #endif
 972         mdelay(37);
 973
 974         if (hre_run_program(tpm, prg_stage1_prepare,
 975                             sizeof(prg_stage1_prepare)))
 976                 goto failure;
 977
 978         sd_prg = load_sd_key_program();
 979         if (sd_prg) {
 980                 if (hre_run_program(tpm, sd_prg->code, sd_prg->code_size))
 981                         goto failure;
 982                 puts("SD code run successfully\n");
 983         } else {
 984                 puts("no key program found on SD\n");
 985                 goto failure;
 986         }
 987         goto end;
 988 failure:
 989         result = 1;
 990 end:
 991         if (sd_prg)
 992                 free(sd_prg);
 993         printf("CCDM S1: actions done (%d)\n", result);
 994         return result;
 995 }
 996 #endif
 997
 998 #ifdef CCDM_FIRST_STAGE
 999 static int first_stage_init(void)
1000 {
1001         struct udevice *tpm;
1002         int ret;
1003
1004         puts("CCDM S1\n");
1005         ret = get_tpm(&tpm);
1006         if (ret || tpm_init(tpm) || tpm_startup(tpm, TPM_ST_CLEAR))
1007                 return 1;
1008         ret = first_stage_actions(tpm);
1009 #ifndef CCDM_SECOND_STAGE
1010         if (!ret) {
1011                 if (bl2_entry)
1012                         (*bl2_entry)();
1013                 ret = 1;
1014         }
1015 #endif
1016         return ret;
1017 }
1018 #endif
1019
1020 #ifdef CCDM_SECOND_STAGE
1021 static const uint8_t prg_stage2_prepare[] = {
1022         0x00, 0x80, 0x00, 0x00, /* opcode: SYNC PCR0 */
1023         0x00, 0x84, 0x00, 0x00, /* opcode: SYNC PCR1 */
1024         0x00, 0x88, 0x00, 0x00, /* opcode: SYNC PCR2 */
1025         0x00, 0x8c, 0x00, 0x00, /* opcode: SYNC PCR3 */
1026         0x00, 0x90, 0x00, 0x00, /* opcode: SYNC PCR4 */
1027 };
1028
1029 static const uint8_t prg_stage2_success[] = {
1030         0x81, 0x02, 0x40, 0x14, /* opcode: LOAD PCR4, #<20B data> */
1031         0x48, 0xfd, 0x95, 0x17, 0xe7, 0x54, 0x6b, 0x68, /* data */
1032         0x92, 0x31, 0x18, 0x05, 0xf8, 0x58, 0x58, 0x3c, /* data */
1033         0xe4, 0xd2, 0x81, 0xe0, /* data */
1034 };
1035
1036 static const uint8_t prg_stage_fail[] = {
1037         0x81, 0x01, 0x00, 0x14, /* opcode: LOAD v0, #<20B data> */
1038         0xc0, 0x32, 0xad, 0xc1, 0xff, 0x62, 0x9c, 0x9b, /* data */
1039         0x66, 0xf2, 0x27, 0x49, 0xad, 0x66, 0x7e, 0x6b, /* data */
1040         0xea, 0xdf, 0x14, 0x4b, /* data */
1041         0x81, 0x42, 0x30, 0x00, /* opcode: LOAD PCR3, v0 */
1042         0x81, 0x42, 0x40, 0x00, /* opcode: LOAD PCR4, v0 */
1043 };
1044
1045 static int second_stage_init(void)
1046 {
1047         static const char mac_suffix[] = ".mac";
1048         bool did_first_stage_run = true;
1049         int result = 0;
1050         char *cptr, *mmcdev = NULL;
1051         struct key_program *hmac_blob = NULL;
1052         const char *image_path = "/ccdm.itb";
1053         char *mac_path = NULL;
1054         ulong image_addr;
1055         loff_t image_size;
1056         struct udevice *tpm;
1057         uint32_t err;
1058         int ret;
1059
1060         printf("CCDM S2\n");
1061         ret = get_tpm(&tpm);
1062         if (ret || tpm_init(tpm))
1063                 return 1;
1064         err = tpm_startup(tpm, TPM_ST_CLEAR);
1065         if (err != TPM_INVALID_POSTINIT)
1066                 did_first_stage_run = false;
1067
1068 #ifdef CCDM_AUTO_FIRST_STAGE
1069         if (!did_first_stage_run && first_stage_actions(tpm))
1070                 goto failure;
1071 #else
1072         if (!did_first_stage_run)
1073                 goto failure;
1074 #endif
1075
1076         if (hre_run_program(tpm, prg_stage2_prepare,
1077                             sizeof(prg_stage2_prepare)))
1078                 goto failure;
1079
1080         /* run "prepboot" from env to get "mmcdev" set */
1081         cptr = env_get("prepboot");
1082         if (cptr && !run_command(cptr, 0))
1083                 mmcdev = env_get("mmcdev");
1084         if (!mmcdev)
1085                 goto failure;
1086
1087         cptr = env_get("ramdiskimage");
1088         if (cptr)
1089                 image_path = cptr;
1090
1091         mac_path = malloc(strlen(image_path) + strlen(mac_suffix) + 1);
1092         if (mac_path == NULL)
1093                 goto failure;
1094         strcpy(mac_path, image_path);
1095         strcat(mac_path, mac_suffix);
1096
1097         /* read image from mmcdev (ccdm.itb) */
1098         image_addr = (ulong)get_image_location();
1099         if (fs_set_blk_dev("mmc", mmcdev, FS_TYPE_EXT))
1100                 goto failure;
1101         if (fs_read(image_path, image_addr, 0, 0, &image_size) < 0)
1102                 goto failure;
1103         if (image_size <= 0)
1104                 goto failure;
1105         printf("CCDM image found on %s, %lld bytes\n", mmcdev, image_size);
1106
1107         hmac_blob = load_key_chunk("mmc", mmcdev, FS_TYPE_EXT, mac_path);
1108         if (!hmac_blob) {
1109                 puts("failed to load mac file\n");
1110                 goto failure;
1111         }
1112         if (verify_program(hmac_blob)) {
1113                 puts("corrupted mac file\n");
1114                 goto failure;
1115         }
1116         if (check_hmac(hmac_blob, (u8 *)image_addr, image_size)) {
1117                 puts("image integrity could not be verified\n");
1118                 goto failure;
1119         }
1120         puts("CCDM image OK\n");
1121
1122         hre_run_program(tpm, prg_stage2_success, sizeof(prg_stage2_success));
1123
1124         goto end;
1125 failure:
1126         result = 1;
1127         hre_run_program(tpm, prg_stage_fail, sizeof(prg_stage_fail));
1128 end:
1129         if (hmac_blob)
1130                 free(hmac_blob);
1131         if (mac_path)
1132                 free(mac_path);
1133
1134         return result;
1135 }
1136 #endif
1137
1138 int show_self_hash(void)
1139 {
1140         struct h_reg *hash_ptr;
1141 #ifdef CCDM_SECOND_STAGE
1142         struct h_reg hash;
1143
1144         hash_ptr = &hash;
1145         if (compute_self_hash(hash_ptr))
1146                 return 1;
1147 #else
1148         hash_ptr = &fix_hregs[FIX_HREG_SELF_HASH];
1149 #endif
1150         puts("self hash: ");
1151         if (hash_ptr && hash_ptr->valid)
1152                 print_buffer(0, hash_ptr->digest, 1, 20, 20);
1153         else
1154                 puts("INVALID\n");
1155
1156         return 0;
1157 }
1158
1159 /**
1160  * @brief let the system hang.
1161  *
1162  * Called on error.
1163  * Will stop the boot process; display a message and signal the error condition
1164  * by blinking the "status" and the "finder" LED of the controller board.
1165  *
1166  * @note the develop version runs the blink cycle 2 times and then returns.
1167  * The release version never returns.
1168  */
1169 static void ccdm_hang(void)
1170 {
1171         static const u64 f0 = 0x0ba3bb8ba2e880; /* blink code "finder" LED */
1172         static const u64 s0 = 0x00f0f0f0f0f0f0; /* blink code "status" LED */
1173         u64 f, s;
1174         int i;
1175 #ifdef CCDM_DEVELOP
1176         int j;
1177 #endif
1178
1179         I2C_SET_BUS(I2C_SOC_0);
1180         pca9698_direction_output(0x22, 0, 0); /* Finder */
1181         pca9698_direction_output(0x22, 4, 0); /* Status */
1182
1183         puts("### ERROR ### Please RESET the board ###\n");
1184         bootstage_error(BOOTSTAGE_ID_NEED_RESET);
1185 #ifdef CCDM_DEVELOP
1186         puts("*** ERROR ******** THIS WOULD HANG ******** ERROR ***\n");
1187         puts("** but we continue since this is a DEVELOP version **\n");
1188         puts("*** ERROR ******** THIS WOULD HANG ******** ERROR ***\n");
1189         for (j = 2; j-- > 0;) {
1190                 putc('#');
1191 #else
1192         for (;;) {
1193 #endif
1194                 f = f0;
1195                 s = s0;
1196                 for (i = 54; i-- > 0;) {
1197                         pca9698_set_value(0x22, 0, !(f & 1));
1198                         pca9698_set_value(0x22, 4, (s & 1));
1199                         f >>= 1;
1200                         s >>= 1;
1201                         mdelay(120);
1202                 }
1203         }
1204         puts("\ncontinue...\n");
1205 }
1206
1207 int startup_ccdm_id_module(void)
1208 {
1209         int result = 0;
1210         unsigned int orig_i2c_bus;
1211
1212         orig_i2c_bus = i2c_get_bus_num();
1213         i2c_set_bus_num(I2C_SOC_1);
1214
1215         /* goto end; */
1216
1217 #ifdef CCDM_DEVELOP
1218         show_self_hash();
1219 #endif
1220 #ifdef CCDM_FIRST_STAGE
1221         result = first_stage_init();
1222         if (result) {
1223                 puts("1st stage init failed\n");
1224                 goto failure;
1225         }
1226 #endif
1227 #ifdef CCDM_SECOND_STAGE
1228         result = second_stage_init();
1229         if (result) {
1230                 puts("2nd stage init failed\n");
1231                 goto failure;
1232         }
1233 #endif
1234
1235         goto end;
1236 failure:
1237         result = 1;
1238 end:
1239         i2c_set_bus_num(orig_i2c_bus);
1240         if (result)
1241                 ccdm_hang();
1242
1243         return result;
1244 }