git.sur5r.net Git - u-boot/blob - board/gdsys/p1022/controlcenterd-id.c

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