board/gdsys/p1022/controlcenterd-id.c

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