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, 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, min(i, data_size));
740 			}
741 		}
742 		bin_func(dst_reg->digest, src_buf, 20);
743 		dst_reg->valid = true;
744 		dst_modified = true;
745 		break;
746 	case HRE_LOADKEY:
747 		if (hre_op_loadkey(src_reg, dst_reg, data, data_size))
748 			return NULL;
749 		break;
750 	default:
751 		return NULL;
752 	}
753 
754 	if (dst_reg && dst_modified && IS_PCR_HREG(dst_spec)) {
755 		hre_tpm_err = tpm_extend(HREG_IDX(dst_spec), dst_reg->digest,
756 			dst_reg->digest);
757 		if (hre_tpm_err) {
758 			hre_err = HRE_E_TPM_FAILURE;
759 			return NULL;
760 		}
761 	}
762 end:
763 	*ip += 4;
764 	*code_size -= 4;
765 	if (opcode & 0x80) {
766 		*ip += data_size;
767 		*code_size -= data_size;
768 	}
769 
770 	return *ip;
771 }
772 
773 /**
774  * @brief runs a program on the hash register engine.
775  * @param code		pointer to the (HRE) code.
776  * @param code_size	size of the code (in bytes).
777  * @return 0 on success, != 0 on failure.
778  */
779 static int hre_run_program(const uint8_t *code, size_t code_size)
780 {
781 	size_t code_left;
782 	const uint8_t *ip = code;
783 
784 	code_left = code_size;
785 	hre_tpm_err = 0;
786 	hre_err = HRE_E_OK;
787 	while (code_left > 0)
788 		if (!hre_execute_op(&ip, &code_left))
789 			return -1;
790 
791 	return hre_err;
792 }
793 
794 static int check_hmac(struct key_program *hmac,
795 	const uint8_t *data, size_t data_size)
796 {
797 	uint8_t key[20], computed_hmac[20];
798 	uint32_t type;
799 
800 	type = get_unaligned_be32(hmac->code);
801 	if (type != 0)
802 		return 1;
803 	memset(key, 0, sizeof(key));
804 	compute_extend(key, pcr_hregs[1].digest, 20);
805 	compute_extend(key, pcr_hregs[2].digest, 20);
806 	compute_extend(key, pcr_hregs[3].digest, 20);
807 	compute_extend(key, pcr_hregs[4].digest, 20);
808 
809 	sha1_hmac(key, sizeof(key), data, data_size, computed_hmac);
810 
811 	return memcmp(computed_hmac, hmac->code + 4, 20);
812 }
813 
814 static int verify_program(struct key_program *prg)
815 {
816 	uint32_t crc;
817 	crc = crc32(0, prg->code, prg->code_size);
818 
819 	if (crc != prg->code_crc) {
820 		printf("HRC crc mismatch: %08x != %08x\n",
821 		       crc, prg->code_crc);
822 		return 1;
823 	}
824 	return 0;
825 }
826 
827 #if defined(CCDM_FIRST_STAGE) || (defined CCDM_AUTO_FIRST_STAGE)
828 static struct key_program *load_sd_key_program(void)
829 {
830 	u32 code_len, code_offset;
831 	struct mmc *mmc;
832 	u8 buf[128];
833 	struct key_program *result = NULL, *hmac = NULL;
834 	struct key_program header;
835 
836 	mmc = find_mmc_device(0);
837 	if (!mmc)
838 		return NULL;
839 	mmc_init(mmc);
840 
841 	if (ccdm_mmc_read(mmc, 0, buf, sizeof(buf)) <= 0)
842 		goto failure;
843 
844 	code_offset = *(u32 *)(buf + ESDHC_BOOT_IMAGE_ADDR_OFS);
845 	code_len = *(u32 *)(buf + ESDHC_BOOT_IMAGE_SIZE_OFS);
846 
847 	code_offset += code_len;
848 	/* TODO: the following needs to be the size of the 2nd stage env */
849 	code_offset += CONFIG_ENV_SIZE;
850 
851 	if (ccdm_mmc_read(mmc, code_offset, buf, 4*3) < 0)
852 		goto failure;
853 
854 	header.magic = get_unaligned_be32(buf);
855 	header.code_crc = get_unaligned_be32(buf + 4);
856 	header.code_size = get_unaligned_be32(buf + 8);
857 
858 	if (header.magic != MAGIC_KEY_PROGRAM)
859 		goto failure;
860 
861 	result = malloc(sizeof(struct key_program) + header.code_size);
862 	if (!result)
863 		goto failure;
864 	*result = header;
865 
866 	printf("load key program chunk from SD card (%u bytes) ",
867 	       header.code_size);
868 	code_offset += 12;
869 	if (ccdm_mmc_read(mmc, code_offset, result->code, header.code_size)
870 		< 0)
871 		goto failure;
872 	code_offset += header.code_size;
873 	puts("\n");
874 
875 	if (verify_program(result))
876 		goto failure;
877 
878 	if (ccdm_mmc_read(mmc, code_offset, buf, 4*3) < 0)
879 		goto failure;
880 
881 	header.magic = get_unaligned_be32(buf);
882 	header.code_crc = get_unaligned_be32(buf + 4);
883 	header.code_size = get_unaligned_be32(buf + 8);
884 
885 	if (header.magic == MAGIC_HMAC) {
886 		puts("check integrity\n");
887 		hmac = malloc(sizeof(struct key_program) + header.code_size);
888 		if (!hmac)
889 			goto failure;
890 		*hmac = header;
891 		code_offset += 12;
892 		if (ccdm_mmc_read(mmc, code_offset, hmac->code,
893 				  hmac->code_size) < 0)
894 			goto failure;
895 		if (verify_program(hmac))
896 			goto failure;
897 		if (check_hmac(hmac, result->code, result->code_size)) {
898 			puts("key program integrity could not be verified\n");
899 			goto failure;
900 		}
901 		puts("key program verified\n");
902 	}
903 
904 	goto end;
905 failure:
906 	if (result)
907 		free(result);
908 	result = NULL;
909 end:
910 	if (hmac)
911 		free(hmac);
912 
913 	return result;
914 }
915 #endif
916 
917 #ifdef CCDM_SECOND_STAGE
918 /**
919  * @brief load a key program from file system.
920  * @param ifname	interface of the file system
921  * @param dev_part_str	device part of the file system
922  * @param fs_type	tyep of the file system
923  * @param path		path of the file to load.
924  * @return the loaded structure or NULL on failure.
925  */
926 static struct key_program *load_key_chunk(const char *ifname,
927 	const char *dev_part_str, int fs_type,
928 	const char *path)
929 {
930 	struct key_program *result = NULL;
931 	struct key_program header;
932 	uint32_t crc;
933 	uint8_t buf[12];
934 	int i;
935 
936 	if (fs_set_blk_dev(ifname, dev_part_str, fs_type))
937 		goto failure;
938 	i = fs_read(path, (ulong)buf, 0, 12);
939 	if (i < 12)
940 		goto failure;
941 	header.magic = get_unaligned_be32(buf);
942 	header.code_crc = get_unaligned_be32(buf + 4);
943 	header.code_size = get_unaligned_be32(buf + 8);
944 
945 	if (header.magic != MAGIC_HMAC && header.magic != MAGIC_KEY_PROGRAM)
946 		goto failure;
947 
948 	result = malloc(sizeof(struct key_program) + header.code_size);
949 	if (!result)
950 		goto failure;
951 	if (fs_set_blk_dev(ifname, dev_part_str, fs_type))
952 		goto failure;
953 	i = fs_read(path, (ulong)result, 0,
954 		sizeof(struct key_program) + header.code_size);
955 	if (i <= 0)
956 		goto failure;
957 	*result = header;
958 
959 	crc = crc32(0, result->code, result->code_size);
960 
961 	if (crc != result->code_crc) {
962 		printf("%s: HRC crc mismatch: %08x != %08x\n",
963 		       path, crc, result->code_crc);
964 		goto failure;
965 	}
966 	goto end;
967 failure:
968 	if (result) {
969 		free(result);
970 		result = NULL;
971 	}
972 end:
973 	return result;
974 }
975 #endif
976 
977 #if defined(CCDM_FIRST_STAGE) || (defined CCDM_AUTO_FIRST_STAGE)
978 static int first_stage_actions(void)
979 {
980 	int result = 0;
981 	struct key_program *sd_prg = NULL;
982 
983 	puts("CCDM S1: start actions\n");
984 #ifndef CCDM_SECOND_STAGE
985 	if (tpm_continue_self_test())
986 		goto failure;
987 #else
988 	tpm_continue_self_test();
989 #endif
990 	mdelay(37);
991 
992 	if (hre_run_program(prg_stage1_prepare, sizeof(prg_stage1_prepare)))
993 		goto failure;
994 
995 	sd_prg = load_sd_key_program();
996 	if (sd_prg) {
997 		if (hre_run_program(sd_prg->code, sd_prg->code_size))
998 			goto failure;
999 		puts("SD code run successfully\n");
1000 	} else {
1001 		puts("no key program found on SD\n");
1002 		goto failure;
1003 	}
1004 	goto end;
1005 failure:
1006 	result = 1;
1007 end:
1008 	if (sd_prg)
1009 		free(sd_prg);
1010 	printf("CCDM S1: actions done (%d)\n", result);
1011 	return result;
1012 }
1013 #endif
1014 
1015 #ifdef CCDM_FIRST_STAGE
1016 static int first_stage_init(void)
1017 {
1018 	int res = 0;
1019 	puts("CCDM S1\n");
1020 	if (tpm_init() || tpm_startup(TPM_ST_CLEAR))
1021 		return 1;
1022 	res = first_stage_actions();
1023 #ifndef CCDM_SECOND_STAGE
1024 	if (!res) {
1025 		if (bl2_entry)
1026 			(*bl2_entry)();
1027 		res = 1;
1028 	}
1029 #endif
1030 	return res;
1031 }
1032 #endif
1033 
1034 #ifdef CCDM_SECOND_STAGE
1035 static int second_stage_init(void)
1036 {
1037 	static const char mac_suffix[] = ".mac";
1038 	bool did_first_stage_run = true;
1039 	int result = 0;
1040 	char *cptr, *mmcdev = NULL;
1041 	struct key_program *hmac_blob = NULL;
1042 	const char *image_path = "/ccdm.itb";
1043 	char *mac_path = NULL;
1044 	ulong image_addr;
1045 	size_t image_size;
1046 	uint32_t err;
1047 
1048 	printf("CCDM S2\n");
1049 	if (tpm_init())
1050 		return 1;
1051 	err = tpm_startup(TPM_ST_CLEAR);
1052 	if (err != TPM_INVALID_POSTINIT)
1053 		did_first_stage_run = false;
1054 
1055 #ifdef CCDM_AUTO_FIRST_STAGE
1056 	if (!did_first_stage_run && first_stage_actions())
1057 		goto failure;
1058 #else
1059 	if (!did_first_stage_run)
1060 		goto failure;
1061 #endif
1062 
1063 	if (hre_run_program(prg_stage2_prepare, sizeof(prg_stage2_prepare)))
1064 		goto failure;
1065 
1066 	/* run "prepboot" from env to get "mmcdev" set */
1067 	cptr = getenv("prepboot");
1068 	if (cptr && !run_command(cptr, 0))
1069 		mmcdev = getenv("mmcdev");
1070 	if (!mmcdev)
1071 		goto failure;
1072 
1073 	cptr = getenv("ramdiskimage");
1074 	if (cptr)
1075 		image_path = cptr;
1076 
1077 	mac_path = malloc(strlen(image_path) + strlen(mac_suffix) + 1);
1078 	if (mac_path == NULL)
1079 		goto failure;
1080 	strcpy(mac_path, image_path);
1081 	strcat(mac_path, mac_suffix);
1082 
1083 	/* read image from mmcdev (ccdm.itb) */
1084 	image_addr = (ulong)get_image_location();
1085 	if (fs_set_blk_dev("mmc", mmcdev, FS_TYPE_EXT))
1086 		goto failure;
1087 	image_size = fs_read(image_path, image_addr, 0, 0);
1088 	if (image_size <= 0)
1089 		goto failure;
1090 	printf("CCDM image found on %s, %d bytes\n", mmcdev, image_size);
1091 
1092 	hmac_blob = load_key_chunk("mmc", mmcdev, FS_TYPE_EXT, mac_path);
1093 	if (!hmac_blob) {
1094 		puts("failed to load mac file\n");
1095 		goto failure;
1096 	}
1097 	if (verify_program(hmac_blob)) {
1098 		puts("corrupted mac file\n");
1099 		goto failure;
1100 	}
1101 	if (check_hmac(hmac_blob, (u8 *)image_addr, image_size)) {
1102 		puts("image integrity could not be verified\n");
1103 		goto failure;
1104 	}
1105 	puts("CCDM image OK\n");
1106 
1107 	hre_run_program(prg_stage2_success, sizeof(prg_stage2_success));
1108 
1109 	goto end;
1110 failure:
1111 	result = 1;
1112 	hre_run_program(prg_stage_fail, sizeof(prg_stage_fail));
1113 end:
1114 	if (hmac_blob)
1115 		free(hmac_blob);
1116 	if (mac_path)
1117 		free(mac_path);
1118 
1119 	return result;
1120 }
1121 #endif
1122 
1123 int show_self_hash(void)
1124 {
1125 	struct h_reg *hash_ptr;
1126 #ifdef CCDM_SECOND_STAGE
1127 	struct h_reg hash;
1128 
1129 	hash_ptr = &hash;
1130 	if (compute_self_hash(hash_ptr))
1131 		return 1;
1132 #else
1133 	hash_ptr = &fix_hregs[FIX_HREG_SELF_HASH];
1134 #endif
1135 	puts("self hash: ");
1136 	if (hash_ptr && hash_ptr->valid)
1137 		print_buffer(0, hash_ptr->digest, 1, 20, 20);
1138 	else
1139 		puts("INVALID\n");
1140 
1141 	return 0;
1142 }
1143 
1144 /**
1145  * @brief let the system hang.
1146  *
1147  * Called on error.
1148  * Will stop the boot process; display a message and signal the error condition
1149  * by blinking the "status" and the "finder" LED of the controller board.
1150  *
1151  * @note the develop version runs the blink cycle 2 times and then returns.
1152  * The release version never returns.
1153  */
1154 static void ccdm_hang(void)
1155 {
1156 	static const u64 f0 = 0x0ba3bb8ba2e880; /* blink code "finder" LED */
1157 	static const u64 s0 = 0x00f0f0f0f0f0f0; /* blink code "status" LED */
1158 	u64 f, s;
1159 	int i;
1160 #ifdef CCDM_DEVELOP
1161 	int j;
1162 #endif
1163 
1164 	I2C_SET_BUS(I2C_SOC_0);
1165 	pca9698_direction_output(0x22, 0, 0); /* Finder */
1166 	pca9698_direction_output(0x22, 4, 0); /* Status */
1167 
1168 	puts("### ERROR ### Please RESET the board ###\n");
1169 	bootstage_error(BOOTSTAGE_ID_NEED_RESET);
1170 #ifdef CCDM_DEVELOP
1171 	puts("*** ERROR ******** THIS WOULD HANG ******** ERROR ***\n");
1172 	puts("** but we continue since this is a DEVELOP version **\n");
1173 	puts("*** ERROR ******** THIS WOULD HANG ******** ERROR ***\n");
1174 	for (j = 2; j-- > 0;) {
1175 		putc('#');
1176 #else
1177 	for (;;) {
1178 #endif
1179 		f = f0;
1180 		s = s0;
1181 		for (i = 54; i-- > 0;) {
1182 			pca9698_set_value(0x22, 0, !(f & 1));
1183 			pca9698_set_value(0x22, 4, (s & 1));
1184 			f >>= 1;
1185 			s >>= 1;
1186 			mdelay(120);
1187 		}
1188 	}
1189 	puts("\ncontinue...\n");
1190 }
1191 
1192 int startup_ccdm_id_module(void)
1193 {
1194 	int result = 0;
1195 	unsigned int orig_i2c_bus;
1196 
1197 	orig_i2c_bus = i2c_get_bus_num();
1198 	i2c_set_bus_num(I2C_SOC_1);
1199 
1200 	/* goto end; */
1201 
1202 #ifdef CCDM_DEVELOP
1203 	show_self_hash();
1204 #endif
1205 #ifdef CCDM_FIRST_STAGE
1206 	result = first_stage_init();
1207 	if (result) {
1208 		puts("1st stage init failed\n");
1209 		goto failure;
1210 	}
1211 #endif
1212 #ifdef CCDM_SECOND_STAGE
1213 	result = second_stage_init();
1214 	if (result) {
1215 		puts("2nd stage init failed\n");
1216 		goto failure;
1217 	}
1218 #endif
1219 
1220 	goto end;
1221 failure:
1222 	result = 1;
1223 end:
1224 	i2c_set_bus_num(orig_i2c_bus);
1225 	if (result)
1226 		ccdm_hang();
1227 
1228 	return result;
1229 }
1230