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