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