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