board/gdsys/p1022/controlcenterd-id.c

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