1 /*
2  * Copyright 2015 Freescale Semiconductor, Inc.
3  *
4  * SPDX-License-Identifier:	GPL-2.0+
5  */
6 
7 #include <common.h>
8 #include <fsl_validate.h>
9 #include <fsl_secboot_err.h>
10 #include <fsl_sfp.h>
11 #include <fsl_sec.h>
12 #include <command.h>
13 #include <malloc.h>
14 #include <dm/uclass.h>
15 #include <u-boot/rsa-mod-exp.h>
16 #include <hash.h>
17 #include <fsl_secboot_err.h>
18 #ifndef CONFIG_MPC85xx
19 #include <asm/arch/immap_ls102xa.h>
20 #endif
21 
22 #define SHA256_BITS	256
23 #define SHA256_BYTES	(256/8)
24 #define SHA256_NIBBLES	(256/4)
25 #define NUM_HEX_CHARS	(sizeof(ulong) * 2)
26 
27 /* This array contains DER value for SHA-256 */
28 static const u8 hash_identifier[] = { 0x30, 0x31, 0x30, 0x0d, 0x06, 0x09, 0x60,
29 		0x86, 0x48, 0x01, 0x65,	0x03, 0x04, 0x02, 0x01, 0x05, 0x00,
30 		0x04, 0x20
31 		};
32 
33 static u8 hash_val[SHA256_BYTES];
34 static const u8 barker_code[ESBC_BARKER_LEN] = { 0x68, 0x39, 0x27, 0x81 };
35 
36 void branch_to_self(void) __attribute__ ((noreturn));
37 
38 /*
39  * This function will put core in infinite loop.
40  * This will be called when the ESBC can not proceed further due
41  * to some unknown errors.
42  */
43 void branch_to_self(void)
44 {
45 	printf("Core is in infinite loop due to errors.\n");
46 self:
47 	goto self;
48 }
49 
50 #if defined(CONFIG_FSL_ISBC_KEY_EXT)
51 static u32 check_ie(struct fsl_secboot_img_priv *img)
52 {
53 	if (img->hdr.ie_flag)
54 		return 1;
55 
56 	return 0;
57 }
58 
59 /* This function returns the CSF Header Address of uboot
60  * For MPC85xx based platforms, the LAW mapping for NOR
61  * flash changes in uboot code. Hence the offset needs
62  * to be calculated and added to the new NOR flash base
63  * address
64  */
65 #if defined(CONFIG_MPC85xx)
66 int get_csf_base_addr(u32 *csf_addr, u32 *flash_base_addr)
67 {
68 	struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_MPC85xx_GUTS_ADDR);
69 	u32 csf_hdr_addr = in_be32(&gur->scratchrw[0]);
70 	u32 csf_flash_offset = csf_hdr_addr & ~(CONFIG_SYS_PBI_FLASH_BASE);
71 	u32 flash_addr, addr;
72 	int found = 0;
73 	int i = 0;
74 
75 	for (i = 0; i < CONFIG_SYS_MAX_FLASH_BANKS; i++) {
76 		flash_addr = flash_info[i].start[0];
77 		addr = flash_info[i].start[0] + csf_flash_offset;
78 		if (memcmp((u8 *)addr, barker_code, ESBC_BARKER_LEN) == 0) {
79 			debug("Barker found on addr %x\n", addr);
80 			found = 1;
81 			break;
82 		}
83 	}
84 
85 	if (!found)
86 		return -1;
87 
88 	*csf_addr = addr;
89 	*flash_base_addr = flash_addr;
90 
91 	return 0;
92 }
93 #else
94 /* For platforms like LS1020, correct flash address is present in
95  * the header. So the function reqturns flash base address as 0
96  */
97 int get_csf_base_addr(u32 *csf_addr, u32 *flash_base_addr)
98 {
99 	struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
100 	u32 csf_hdr_addr = in_be32(&gur->scratchrw[0]);
101 
102 	if (memcmp((u8 *)csf_hdr_addr, barker_code, ESBC_BARKER_LEN))
103 		return -1;
104 
105 	*csf_addr = csf_hdr_addr;
106 	*flash_base_addr = 0;
107 	return 0;
108 }
109 #endif
110 
111 static int get_ie_info_addr(u32 *ie_addr)
112 {
113 	struct fsl_secboot_img_hdr *hdr;
114 	struct fsl_secboot_sg_table *sg_tbl;
115 	u32 flash_base_addr, csf_addr;
116 
117 	if (get_csf_base_addr(&csf_addr, &flash_base_addr))
118 		return -1;
119 
120 	hdr = (struct fsl_secboot_img_hdr *)csf_addr;
121 
122 	/* For SoC's with Trust Architecture v1 with corenet bus
123 	 * the sg table field in CSF header has absolute address
124 	 * for sg table in memory. In other Trust Architecture,
125 	 * this field specifies the offset of sg table from the
126 	 * base address of CSF Header
127 	 */
128 #if defined(CONFIG_FSL_TRUST_ARCH_v1) && defined(CONFIG_FSL_CORENET)
129 	sg_tbl = (struct fsl_secboot_sg_table *)
130 		 (((u32)hdr->psgtable & ~(CONFIG_SYS_PBI_FLASH_BASE)) +
131 		  flash_base_addr);
132 #else
133 	sg_tbl = (struct fsl_secboot_sg_table *)(csf_addr +
134 						 (u32)hdr->psgtable);
135 #endif
136 
137 	/* IE Key Table is the first entry in the SG Table */
138 #if defined(CONFIG_MPC85xx)
139 	*ie_addr = (sg_tbl->src_addr & ~(CONFIG_SYS_PBI_FLASH_BASE)) +
140 		   flash_base_addr;
141 #else
142 	*ie_addr = sg_tbl->src_addr;
143 #endif
144 
145 	debug("IE Table address is %x\n", *ie_addr);
146 	return 0;
147 }
148 
149 #endif
150 
151 #ifdef CONFIG_KEY_REVOCATION
152 /* This function checks srk_table_flag in header and set/reset srk_flag.*/
153 static u32 check_srk(struct fsl_secboot_img_priv *img)
154 {
155 	if (img->hdr.len_kr.srk_table_flag & SRK_FLAG)
156 		return 1;
157 
158 	return 0;
159 }
160 
161 /* This function returns ospr's key_revoc values.*/
162 static u32 get_key_revoc(void)
163 {
164 	struct ccsr_sfp_regs *sfp_regs = (void *)(CONFIG_SYS_SFP_ADDR);
165 	return (sfp_in32(&sfp_regs->ospr) & OSPR_KEY_REVOC_MASK) >>
166 		OSPR_KEY_REVOC_SHIFT;
167 }
168 
169 /* This function checks if selected key is revoked or not.*/
170 static u32 is_key_revoked(u32 keynum, u32 rev_flag)
171 {
172 	if (keynum == UNREVOCABLE_KEY)
173 		return 0;
174 
175 	if ((u32)(1 << (ALIGN_REVOC_KEY - keynum)) & rev_flag)
176 		return 1;
177 
178 	return 0;
179 }
180 
181 /* It validates srk_table key lengths.*/
182 static u32 validate_srk_tbl(struct srk_table *tbl, u32 num_entries)
183 {
184 	int i = 0;
185 	for (i = 0; i < num_entries; i++) {
186 		if (!((tbl[i].key_len == 2 * KEY_SIZE_BYTES/4) ||
187 		      (tbl[i].key_len == 2 * KEY_SIZE_BYTES/2) ||
188 		      (tbl[i].key_len == 2 * KEY_SIZE_BYTES)))
189 			return ERROR_ESBC_CLIENT_HEADER_INV_SRK_ENTRY_KEYLEN;
190 	}
191 	return 0;
192 }
193 #endif
194 
195 /* This function return length of public key.*/
196 static inline u32 get_key_len(struct fsl_secboot_img_priv *img)
197 {
198 	return img->key_len;
199 }
200 
201 /*
202  * Handles the ESBC uboot client header verification failure.
203  * This  function  handles all the errors which might occur in the
204  * parsing and checking of ESBC uboot client header. It will also
205  * set the error bits in the SEC_MON.
206  */
207 static void fsl_secboot_header_verification_failure(void)
208 {
209 	struct ccsr_sec_mon_regs *sec_mon_regs = (void *)
210 						(CONFIG_SYS_SEC_MON_ADDR);
211 	struct ccsr_sfp_regs *sfp_regs = (void *)(CONFIG_SYS_SFP_ADDR);
212 	u32 sts = sec_mon_in32(&sec_mon_regs->hp_stat);
213 
214 	/* 29th bit of OSPR is ITS */
215 	u32 its = sfp_in32(&sfp_regs->ospr) >> 2;
216 
217 	/*
218 	 * Read the SEC_MON status register
219 	 * Read SSM_ST field
220 	 */
221 	sts = sec_mon_in32(&sec_mon_regs->hp_stat);
222 	if ((sts & HPSR_SSM_ST_MASK) == HPSR_SSM_ST_TRUST) {
223 		if (its == 1)
224 			change_sec_mon_state(HPSR_SSM_ST_TRUST,
225 					     HPSR_SSM_ST_SOFT_FAIL);
226 		else
227 			change_sec_mon_state(HPSR_SSM_ST_TRUST,
228 					     HPSR_SSM_ST_NON_SECURE);
229 	}
230 
231 	printf("Generating reset request\n");
232 	do_reset(NULL, 0, 0, NULL);
233 }
234 
235 /*
236  * Handles the ESBC uboot client image verification failure.
237  * This  function  handles all the errors which might occur in the
238  * public key hash comparison and signature verification of
239  * ESBC uboot client image. It will also
240  * set the error bits in the SEC_MON.
241  */
242 static void fsl_secboot_image_verification_failure(void)
243 {
244 	struct ccsr_sec_mon_regs *sec_mon_regs = (void *)
245 						(CONFIG_SYS_SEC_MON_ADDR);
246 	struct ccsr_sfp_regs *sfp_regs = (void *)(CONFIG_SYS_SFP_ADDR);
247 	u32 sts = sec_mon_in32(&sec_mon_regs->hp_stat);
248 
249 	u32 its = (sfp_in32(&sfp_regs->ospr) & ITS_MASK) >> ITS_BIT;
250 
251 	/*
252 	 * Read the SEC_MON status register
253 	 * Read SSM_ST field
254 	 */
255 	sts = sec_mon_in32(&sec_mon_regs->hp_stat);
256 	if ((sts & HPSR_SSM_ST_MASK) == HPSR_SSM_ST_TRUST) {
257 		if (its == 1) {
258 			change_sec_mon_state(HPSR_SSM_ST_TRUST,
259 					     HPSR_SSM_ST_SOFT_FAIL);
260 
261 			printf("Generating reset request\n");
262 			do_reset(NULL, 0, 0, NULL);
263 		} else {
264 			change_sec_mon_state(HPSR_SSM_ST_TRUST,
265 					     HPSR_SSM_ST_NON_SECURE);
266 		}
267 	}
268 }
269 
270 static void fsl_secboot_bootscript_parse_failure(void)
271 {
272 	fsl_secboot_header_verification_failure();
273 }
274 
275 /*
276  * Handles the errors in esbc boot.
277  * This  function  handles all the errors which might occur in the
278  * esbc boot phase. It will call the appropriate api to log the
279  * errors and set the error bits in the SEC_MON.
280  */
281 void fsl_secboot_handle_error(int error)
282 {
283 	const struct fsl_secboot_errcode *e;
284 
285 	for (e = fsl_secboot_errcodes; e->errcode != ERROR_ESBC_CLIENT_MAX;
286 		e++) {
287 		if (e->errcode == error)
288 			printf("ERROR :: %x :: %s\n", error, e->name);
289 	}
290 
291 	switch (error) {
292 	case ERROR_ESBC_CLIENT_HEADER_BARKER:
293 	case ERROR_ESBC_CLIENT_HEADER_IMG_SIZE:
294 	case ERROR_ESBC_CLIENT_HEADER_KEY_LEN:
295 	case ERROR_ESBC_CLIENT_HEADER_SIG_LEN:
296 	case ERROR_ESBC_CLIENT_HEADER_KEY_LEN_NOT_TWICE_SIG_LEN:
297 	case ERROR_ESBC_CLIENT_HEADER_KEY_MOD_1:
298 	case ERROR_ESBC_CLIENT_HEADER_KEY_MOD_2:
299 	case ERROR_ESBC_CLIENT_HEADER_SIG_KEY_MOD:
300 	case ERROR_ESBC_CLIENT_HEADER_SG_ESBC_EP:
301 	case ERROR_ESBC_CLIENT_HEADER_SG_ENTIRES_BAD:
302 #ifdef CONFIG_KEY_REVOCATION
303 	case ERROR_ESBC_CLIENT_HEADER_KEY_REVOKED:
304 	case ERROR_ESBC_CLIENT_HEADER_INVALID_SRK_NUM_ENTRY:
305 	case ERROR_ESBC_CLIENT_HEADER_INVALID_KEY_NUM:
306 	case ERROR_ESBC_CLIENT_HEADER_INV_SRK_ENTRY_KEYLEN:
307 #endif
308 #if defined(CONFIG_FSL_ISBC_KEY_EXT)
309 	/*@fallthrough@*/
310 	case ERROR_ESBC_CLIENT_HEADER_IE_KEY_REVOKED:
311 	case ERROR_ESBC_CLIENT_HEADER_INVALID_IE_NUM_ENTRY:
312 	case ERROR_ESBC_CLIENT_HEADER_INVALID_IE_KEY_NUM:
313 	case ERROR_ESBC_CLIENT_HEADER_INV_IE_ENTRY_KEYLEN:
314 	case ERROR_IE_TABLE_NOT_FOUND:
315 #endif
316 		fsl_secboot_header_verification_failure();
317 		break;
318 	case ERROR_ESBC_SEC_RESET:
319 	case ERROR_ESBC_SEC_DEQ:
320 	case ERROR_ESBC_SEC_ENQ:
321 	case ERROR_ESBC_SEC_DEQ_TO:
322 	case ERROR_ESBC_SEC_JOBQ_STATUS:
323 	case ERROR_ESBC_CLIENT_HASH_COMPARE_KEY:
324 	case ERROR_ESBC_CLIENT_HASH_COMPARE_EM:
325 		fsl_secboot_image_verification_failure();
326 		break;
327 	case ERROR_ESBC_MISSING_BOOTM:
328 		fsl_secboot_bootscript_parse_failure();
329 		break;
330 	case ERROR_ESBC_WRONG_CMD:
331 	default:
332 		branch_to_self();
333 		break;
334 	}
335 }
336 
337 static void fsl_secblk_handle_error(int error)
338 {
339 	switch (error) {
340 	case ERROR_ESBC_SEC_ENQ:
341 		fsl_secboot_handle_error(ERROR_ESBC_SEC_ENQ);
342 		break;
343 	case ERROR_ESBC_SEC_DEQ:
344 		fsl_secboot_handle_error(ERROR_ESBC_SEC_DEQ);
345 		break;
346 	case ERROR_ESBC_SEC_DEQ_TO:
347 		fsl_secboot_handle_error(ERROR_ESBC_SEC_DEQ_TO);
348 		break;
349 	default:
350 		printf("Job Queue Output status %x\n", error);
351 		fsl_secboot_handle_error(ERROR_ESBC_SEC_JOBQ_STATUS);
352 		break;
353 	}
354 }
355 
356 /*
357  * Calculate hash of key obtained via offset present in ESBC uboot
358  * client hdr. This function calculates the hash of key which is obtained
359  * through offset present in ESBC uboot client header.
360  */
361 static int calc_img_key_hash(struct fsl_secboot_img_priv *img)
362 {
363 	struct hash_algo *algo;
364 	void *ctx;
365 	int i, srk = 0;
366 	int ret = 0;
367 	const char *algo_name = "sha256";
368 
369 	/* Calculate hash of the esbc key */
370 	ret = hash_progressive_lookup_algo(algo_name, &algo);
371 	if (ret)
372 		return ret;
373 
374 	ret = algo->hash_init(algo, &ctx);
375 	if (ret)
376 		return ret;
377 
378 	/* Update hash for ESBC key */
379 #ifdef CONFIG_KEY_REVOCATION
380 	if (check_srk(img)) {
381 		ret = algo->hash_update(algo, ctx,
382 			(u8 *)(img->ehdrloc + img->hdr.srk_tbl_off),
383 			img->hdr.len_kr.num_srk * sizeof(struct srk_table), 1);
384 		srk = 1;
385 	}
386 #endif
387 	if (!srk)
388 		ret = algo->hash_update(algo, ctx,
389 			img->img_key, img->key_len, 1);
390 	if (ret)
391 		return ret;
392 
393 	/* Copy hash at destination buffer */
394 	ret = algo->hash_finish(algo, ctx, hash_val, algo->digest_size);
395 	if (ret)
396 		return ret;
397 
398 	for (i = 0; i < SHA256_BYTES; i++)
399 		img->img_key_hash[i] = hash_val[i];
400 
401 	return 0;
402 }
403 
404 /*
405  * Calculate hash of ESBC hdr and ESBC. This function calculates the
406  * single hash of ESBC header and ESBC image. If SG flag is on, all
407  * SG entries are also hashed alongwith the complete SG table.
408  */
409 static int calc_esbchdr_esbc_hash(struct fsl_secboot_img_priv *img)
410 {
411 	struct hash_algo *algo;
412 	void *ctx;
413 	int ret = 0;
414 	int key_hash = 0;
415 	const char *algo_name = "sha256";
416 
417 	/* Calculate the hash of the ESBC */
418 	ret = hash_progressive_lookup_algo(algo_name, &algo);
419 	if (ret)
420 		return ret;
421 
422 	ret = algo->hash_init(algo, &ctx);
423 	/* Copy hash at destination buffer */
424 	if (ret)
425 		return ret;
426 
427 	/* Update hash for CSF Header */
428 	ret = algo->hash_update(algo, ctx,
429 		(u8 *)&img->hdr, sizeof(struct fsl_secboot_img_hdr), 0);
430 	if (ret)
431 		return ret;
432 
433 	/* Update the hash with that of srk table if srk flag is 1
434 	 * If IE Table is selected, key is not added in the hash
435 	 * If neither srk table nor IE key table available, add key
436 	 * from header in the hash calculation
437 	 */
438 #ifdef CONFIG_KEY_REVOCATION
439 	if (check_srk(img)) {
440 		ret = algo->hash_update(algo, ctx,
441 			(u8 *)(img->ehdrloc + img->hdr.srk_tbl_off),
442 			img->hdr.len_kr.num_srk * sizeof(struct srk_table), 0);
443 		key_hash = 1;
444 	}
445 #endif
446 #if defined(CONFIG_FSL_ISBC_KEY_EXT)
447 	if (!key_hash && check_ie(img))
448 		key_hash = 1;
449 #endif
450 	if (!key_hash)
451 		ret = algo->hash_update(algo, ctx,
452 			img->img_key, img->hdr.key_len, 0);
453 	if (ret)
454 		return ret;
455 
456 	/* Update hash for actual Image */
457 	ret = algo->hash_update(algo, ctx,
458 			(u8 *)img->hdr.pimg, img->hdr.img_size, 1);
459 	if (ret)
460 		return ret;
461 
462 	/* Copy hash at destination buffer */
463 	ret = algo->hash_finish(algo, ctx, hash_val, algo->digest_size);
464 	if (ret)
465 		return ret;
466 
467 	return 0;
468 }
469 
470 /*
471  * Construct encoded hash EM' wrt PKCSv1.5. This function calculates the
472  * pointers for padding, DER value and hash. And finally, constructs EM'
473  * which includes hash of complete CSF header and ESBC image. If SG flag
474  * is on, hash of SG table and entries is also included.
475  */
476 static void construct_img_encoded_hash_second(struct fsl_secboot_img_priv *img)
477 {
478 	/*
479 	 * RSA PKCSv1.5 encoding format for encoded message is below
480 	 * EM = 0x0 || 0x1 || PS || 0x0 || DER || Hash
481 	 * PS is Padding String
482 	 * DER is DER value for SHA-256
483 	 * Hash is SHA-256 hash
484 	 * *********************************************************
485 	 * representative points to first byte of EM initially and is
486 	 * filled with 0x0
487 	 * representative is incremented by 1 and second byte is filled
488 	 * with 0x1
489 	 * padding points to third byte of EM
490 	 * digest points to full length of EM - 32 bytes
491 	 * hash_id (DER value) points to 19 bytes before pDigest
492 	 * separator is one byte which separates padding and DER
493 	 */
494 
495 	size_t len;
496 	u8 *representative;
497 	u8 *padding, *digest;
498 	u8 *hash_id, *separator;
499 	int i;
500 
501 	len = (get_key_len(img) / 2) - 1;
502 	representative = img->img_encoded_hash_second;
503 	representative[0] = 0;
504 	representative[1] = 1;  /* block type 1 */
505 
506 	padding = &representative[2];
507 	digest = &representative[1] + len - 32;
508 	hash_id = digest - sizeof(hash_identifier);
509 	separator = hash_id - 1;
510 
511 	/* fill padding area pointed by padding with 0xff */
512 	memset(padding, 0xff, separator - padding);
513 
514 	/* fill byte pointed by separator */
515 	*separator = 0;
516 
517 	/* fill SHA-256 DER value  pointed by HashId */
518 	memcpy(hash_id, hash_identifier, sizeof(hash_identifier));
519 
520 	/* fill hash pointed by Digest */
521 	for (i = 0; i < SHA256_BYTES; i++)
522 		digest[i] = hash_val[i];
523 }
524 
525 /*
526  * Reads and validates the ESBC client header.
527  * This function reads key and signature from the ESBC client header.
528  * If Scatter/Gather flag is on, lengths and offsets of images
529  * present as SG entries are also read. This function also checks
530  * whether the header is valid or not.
531  */
532 static int read_validate_esbc_client_header(struct fsl_secboot_img_priv *img)
533 {
534 	char buf[20];
535 	struct fsl_secboot_img_hdr *hdr = &img->hdr;
536 	void *esbc = (u8 *)img->ehdrloc;
537 	u8 *k, *s;
538 #ifdef CONFIG_KEY_REVOCATION
539 	u32 ret;
540 	u32 key_num, key_revoc_flag, size;
541 #endif
542 #if defined(CONFIG_FSL_ISBC_KEY_EXT)
543 	struct ie_key_info *ie_info;
544 	u32 ie_num, ie_revoc_flag, ie_key_len;
545 #endif
546 	int  key_found = 0;
547 
548 	/* check barker code */
549 	if (memcmp(hdr->barker, barker_code, ESBC_BARKER_LEN))
550 		return ERROR_ESBC_CLIENT_HEADER_BARKER;
551 
552 	sprintf(buf, "%x", hdr->pimg);
553 	setenv("img_addr", buf);
554 
555 	if (!hdr->img_size)
556 		return ERROR_ESBC_CLIENT_HEADER_IMG_SIZE;
557 
558 	/* Key checking*/
559 #ifdef CONFIG_KEY_REVOCATION
560 	if (check_srk(img)) {
561 		if ((hdr->len_kr.num_srk == 0) ||
562 		    (hdr->len_kr.num_srk > MAX_KEY_ENTRIES))
563 			return ERROR_ESBC_CLIENT_HEADER_INVALID_SRK_NUM_ENTRY;
564 
565 		key_num = hdr->len_kr.srk_sel;
566 		if (key_num == 0 || key_num > hdr->len_kr.num_srk)
567 			return ERROR_ESBC_CLIENT_HEADER_INVALID_KEY_NUM;
568 
569 		/* Get revoc key from sfp */
570 		key_revoc_flag = get_key_revoc();
571 		ret = is_key_revoked(key_num, key_revoc_flag);
572 		if (ret)
573 			return ERROR_ESBC_CLIENT_HEADER_KEY_REVOKED;
574 
575 		size = hdr->len_kr.num_srk * sizeof(struct srk_table);
576 
577 		memcpy(&img->srk_tbl, esbc + hdr->srk_tbl_off, size);
578 
579 		ret = validate_srk_tbl(img->srk_tbl, hdr->len_kr.num_srk);
580 
581 		if (ret != 0)
582 			return ret;
583 
584 		img->key_len = img->srk_tbl[key_num - 1].key_len;
585 
586 		memcpy(&img->img_key, &(img->srk_tbl[key_num - 1].pkey),
587 		       img->key_len);
588 
589 		key_found = 1;
590 	}
591 #endif
592 
593 #if defined(CONFIG_FSL_ISBC_KEY_EXT)
594 	if (!key_found && check_ie(img)) {
595 		if (get_ie_info_addr(&img->ie_addr))
596 			return ERROR_IE_TABLE_NOT_FOUND;
597 		ie_info = (struct ie_key_info *)img->ie_addr;
598 		if (ie_info->num_keys == 0 || ie_info->num_keys > 32)
599 			return ERROR_ESBC_CLIENT_HEADER_INVALID_IE_NUM_ENTRY;
600 
601 		ie_num = hdr->ie_key_sel;
602 		if (ie_num == 0 || ie_num > ie_info->num_keys)
603 			return ERROR_ESBC_CLIENT_HEADER_INVALID_IE_KEY_NUM;
604 
605 		ie_revoc_flag = ie_info->key_revok;
606 		if ((u32)(1 << (ie_num - 1)) & ie_revoc_flag)
607 			return ERROR_ESBC_CLIENT_HEADER_IE_KEY_REVOKED;
608 
609 		ie_key_len = ie_info->ie_key_tbl[ie_num - 1].key_len;
610 
611 		if (!((ie_key_len == 2 * KEY_SIZE_BYTES / 4) ||
612 		      (ie_key_len == 2 * KEY_SIZE_BYTES / 2) ||
613 		      (ie_key_len == 2 * KEY_SIZE_BYTES)))
614 			return ERROR_ESBC_CLIENT_HEADER_INV_IE_ENTRY_KEYLEN;
615 
616 		memcpy(&img->img_key, &(ie_info->ie_key_tbl[ie_num - 1].pkey),
617 		       ie_key_len);
618 
619 		img->key_len = ie_key_len;
620 		key_found = 1;
621 	}
622 #endif
623 
624 	if (key_found == 0) {
625 		/* check key length */
626 		if (!((hdr->key_len == 2 * KEY_SIZE_BYTES / 4) ||
627 		      (hdr->key_len == 2 * KEY_SIZE_BYTES / 2) ||
628 		      (hdr->key_len == 2 * KEY_SIZE_BYTES)))
629 			return ERROR_ESBC_CLIENT_HEADER_KEY_LEN;
630 
631 		memcpy(&img->img_key, esbc + hdr->pkey, hdr->key_len);
632 
633 		img->key_len = hdr->key_len;
634 
635 		key_found = 1;
636 	}
637 
638 	/* check signaure */
639 	if (get_key_len(img) == 2 * hdr->sign_len) {
640 		/* check signature length */
641 		if (!((hdr->sign_len == KEY_SIZE_BYTES / 4) ||
642 		      (hdr->sign_len == KEY_SIZE_BYTES / 2) ||
643 		      (hdr->sign_len == KEY_SIZE_BYTES)))
644 			return ERROR_ESBC_CLIENT_HEADER_SIG_LEN;
645 	} else {
646 		return ERROR_ESBC_CLIENT_HEADER_KEY_LEN_NOT_TWICE_SIG_LEN;
647 	}
648 
649 	memcpy(&img->img_sign, esbc + hdr->psign, hdr->sign_len);
650 
651 	/* No SG support */
652 	if (hdr->sg_flag)
653 		return ERROR_ESBC_CLIENT_HEADER_SG;
654 
655 	/* modulus most significant bit should be set */
656 	k = (u8 *)&img->img_key;
657 
658 	if ((k[0] & 0x80) == 0)
659 		return ERROR_ESBC_CLIENT_HEADER_KEY_MOD_1;
660 
661 	/* modulus value should be odd */
662 	if ((k[get_key_len(img) / 2 - 1] & 0x1) == 0)
663 		return ERROR_ESBC_CLIENT_HEADER_KEY_MOD_2;
664 
665 	/* Check signature value < modulus value */
666 	s = (u8 *)&img->img_sign;
667 
668 	if (!(memcmp(s, k, hdr->sign_len) < 0))
669 		return ERROR_ESBC_CLIENT_HEADER_SIG_KEY_MOD;
670 
671 	return ESBC_VALID_HDR;
672 }
673 
674 static inline int str2longbe(const char *p, ulong *num)
675 {
676 	char *endptr;
677 	ulong tmp;
678 
679 	if (!p) {
680 		return 0;
681 	} else {
682 		tmp = simple_strtoul(p, &endptr, 16);
683 		if (sizeof(ulong) == 4)
684 			*num = cpu_to_be32(tmp);
685 		else
686 			*num = cpu_to_be64(tmp);
687 	}
688 
689 	return *p != '\0' && *endptr == '\0';
690 }
691 
692 int fsl_secboot_validate(cmd_tbl_t *cmdtp, int flag, int argc,
693 		char * const argv[])
694 {
695 	struct ccsr_sfp_regs *sfp_regs = (void *)(CONFIG_SYS_SFP_ADDR);
696 	ulong hash[SHA256_BYTES/sizeof(ulong)];
697 	char hash_str[NUM_HEX_CHARS + 1];
698 	ulong addr = simple_strtoul(argv[1], NULL, 16);
699 	struct fsl_secboot_img_priv *img;
700 	struct fsl_secboot_img_hdr *hdr;
701 	void *esbc;
702 	int ret, i, hash_cmd = 0;
703 	u32 srk_hash[8];
704 	uint32_t key_len;
705 	struct key_prop prop;
706 #if !defined(USE_HOSTCC)
707 	struct udevice *mod_exp_dev;
708 #endif
709 
710 	if (argc == 3) {
711 		char *cp = argv[2];
712 		int i = 0;
713 
714 		if (*cp == '0' && *(cp + 1) == 'x')
715 			cp += 2;
716 
717 		/* The input string expected is in hex, where
718 		 * each 4 bits would be represented by a hex
719 		 * sha256 hash is 256 bits long, which would mean
720 		 * num of characters = 256 / 4
721 		 */
722 		if (strlen(cp) != SHA256_NIBBLES) {
723 			printf("%s is not a 256 bits hex string as expected\n",
724 			       argv[2]);
725 			return -1;
726 		}
727 
728 		for (i = 0; i < sizeof(hash)/sizeof(ulong); i++) {
729 			strncpy(hash_str, cp + (i * NUM_HEX_CHARS),
730 				NUM_HEX_CHARS);
731 			hash_str[NUM_HEX_CHARS] = '\0';
732 			if (!str2longbe(hash_str, &hash[i])) {
733 				printf("%s is not a 256 bits hex string ",
734 				       argv[2]);
735 				return -1;
736 			}
737 		}
738 
739 		hash_cmd = 1;
740 	}
741 
742 	img = malloc(sizeof(struct fsl_secboot_img_priv));
743 
744 	if (!img)
745 		return -1;
746 
747 	memset(img, 0, sizeof(struct fsl_secboot_img_priv));
748 
749 	hdr = &img->hdr;
750 	img->ehdrloc = addr;
751 	esbc = (u8 *)img->ehdrloc;
752 
753 	memcpy(hdr, esbc, sizeof(struct fsl_secboot_img_hdr));
754 
755 	/* read and validate esbc header */
756 	ret = read_validate_esbc_client_header(img);
757 
758 	if (ret != ESBC_VALID_HDR) {
759 		fsl_secboot_handle_error(ret);
760 		goto exit;
761 	}
762 
763 	/* SRKH present in SFP */
764 	for (i = 0; i < NUM_SRKH_REGS; i++)
765 		srk_hash[i] = srk_in32(&sfp_regs->srk_hash[i]);
766 
767 	/*
768 	 * Calculate hash of key obtained via offset present in
769 	 * ESBC uboot client hdr
770 	 */
771 	ret = calc_img_key_hash(img);
772 	if (ret) {
773 		fsl_secblk_handle_error(ret);
774 		goto exit;
775 	}
776 
777 	/* Compare hash obtained above with SRK hash present in SFP */
778 	if (hash_cmd)
779 		ret = memcmp(&hash, &img->img_key_hash, SHA256_BYTES);
780 	else
781 		ret = memcmp(srk_hash, img->img_key_hash, SHA256_BYTES);
782 
783 #if defined(CONFIG_FSL_ISBC_KEY_EXT)
784 	if (!hash_cmd && check_ie(img))
785 		ret = 0;
786 #endif
787 
788 	if (ret != 0) {
789 		fsl_secboot_handle_error(ERROR_ESBC_CLIENT_HASH_COMPARE_KEY);
790 		goto exit;
791 	}
792 
793 	ret = calc_esbchdr_esbc_hash(img);
794 	if (ret) {
795 		fsl_secblk_handle_error(ret);
796 		goto exit;
797 	}
798 
799 	/* Construct encoded hash EM' wrt PKCSv1.5 */
800 	construct_img_encoded_hash_second(img);
801 
802 	/* Fill prop structure for public key */
803 	memset(&prop, 0, sizeof(struct key_prop));
804 	key_len = get_key_len(img) / 2;
805 	prop.modulus = img->img_key;
806 	prop.public_exponent = img->img_key + key_len;
807 	prop.num_bits = key_len * 8;
808 	prop.exp_len = key_len;
809 
810 	ret = uclass_get_device(UCLASS_MOD_EXP, 0, &mod_exp_dev);
811 	if (ret) {
812 		printf("RSA: Can't find Modular Exp implementation\n");
813 		return -EINVAL;
814 	}
815 
816 	ret = rsa_mod_exp(mod_exp_dev, img->img_sign, img->hdr.sign_len,
817 			  &prop, img->img_encoded_hash);
818 	if (ret) {
819 		fsl_secblk_handle_error(ret);
820 		goto exit;
821 	}
822 
823 	/*
824 	 * compare the encoded messages EM' and EM wrt RSA PKCSv1.5
825 	 * memcmp returns zero on success
826 	 * memcmp returns non-zero on failure
827 	 */
828 	ret = memcmp(&img->img_encoded_hash_second, &img->img_encoded_hash,
829 		img->hdr.sign_len);
830 
831 	if (ret) {
832 		fsl_secboot_handle_error(ERROR_ESBC_CLIENT_HASH_COMPARE_EM);
833 		goto exit;
834 	}
835 
836 	printf("esbc_validate command successful\n");
837 
838 exit:
839 	return 0;
840 }
841