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