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