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