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