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 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 }
1225