1 /* 2 * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved. 3 * Copyright (c) 2019-2023, Intel Corporation. All rights reserved. 4 * Copyright (c) 2024, Altera Corporation. All rights reserved. 5 * 6 * SPDX-License-Identifier: BSD-3-Clause 7 */ 8 9 #include <assert.h> 10 #include <errno.h> 11 12 #include <arch_helpers.h> 13 #include <common/debug.h> 14 #include <common/tbbr/tbbr_img_def.h> 15 #include <drivers/delay_timer.h> 16 #include <lib/mmio.h> 17 #include <lib/utils.h> 18 #include <tools_share/firmware_image_package.h> 19 20 #include "socfpga_mailbox.h" 21 #include "socfpga_vab.h" 22 23 static size_t get_img_size(uint8_t *img_buf, size_t img_buf_sz) 24 { 25 uint8_t *img_buf_end = img_buf + img_buf_sz; 26 uint32_t cert_sz = get_unaligned_le32(img_buf_end - sizeof(uint32_t)); 27 uint8_t *p = img_buf_end - cert_sz - sizeof(uint32_t); 28 29 /* Ensure p is pointing within the img_buf */ 30 if (p < img_buf || p > (img_buf_end - VAB_CERT_HEADER_SIZE)) 31 return 0; 32 33 if (get_unaligned_le32(p) == SDM_CERT_MAGIC_NUM) 34 return (size_t)(p - img_buf); 35 36 return 0; 37 } 38 39 40 41 int socfpga_vendor_authentication(void **p_image, size_t *p_size) 42 { 43 int retry_count = 20; 44 uint8_t hash384[FCS_SHA384_WORD_SIZE]; 45 uint64_t img_addr, mbox_data_addr; 46 uint32_t img_sz, mbox_data_sz; 47 uint8_t *cert_hash_ptr, *mbox_relocate_data_addr; 48 uint32_t resp = 0, resp_len = 1; 49 int ret = 0; 50 51 img_addr = (uintptr_t)*p_image; 52 img_sz = get_img_size((uint8_t *)img_addr, *p_size); 53 54 if (!img_sz) { 55 NOTICE("VAB certificate not found in image!\n"); 56 return -ENOVABCERT; 57 } 58 59 if (!IS_BYTE_ALIGNED(img_sz, sizeof(uint32_t))) { 60 NOTICE("Image size (%d bytes) not aliged to 4 bytes!\n", img_sz); 61 return -EIMGERR; 62 } 63 64 /* Generate HASH384 from the image */ 65 /* TODO: This part need to cross check !!!!!! */ 66 sha384_csum_wd((uint8_t *)img_addr, img_sz, hash384, CHUNKSZ_PER_WD_RESET); 67 cert_hash_ptr = (uint8_t *)(img_addr + img_sz + 68 VAB_CERT_MAGIC_OFFSET + VAB_CERT_FIT_SHA384_OFFSET); 69 70 /* 71 * Compare the SHA384 found in certificate against the SHA384 72 * calculated from image 73 */ 74 if (memcmp(hash384, cert_hash_ptr, FCS_SHA384_WORD_SIZE)) { 75 NOTICE("SHA384 does not match!\n"); 76 return -EKEYREJECTED; 77 } 78 79 80 mbox_data_addr = img_addr + img_sz - sizeof(uint32_t); 81 /* Size in word (32bits) */ 82 mbox_data_sz = (BYTE_ALIGN(*p_size - img_sz, sizeof(uint32_t))) >> 2; 83 84 NOTICE("mbox_data_addr = %lx mbox_data_sz = %d\n", mbox_data_addr, mbox_data_sz); 85 86 /* TODO: This part need to cross check !!!!!! */ 87 // mbox_relocate_data_addr = (uint8_t *)malloc(mbox_data_sz * sizeof(uint32_t)); 88 // if (!mbox_relocate_data_addr) { 89 // NOTICE("Cannot allocate memory for VAB certificate relocation!\n"); 90 // return -ENOMEM; 91 // } 92 93 memcpy(mbox_relocate_data_addr, (uint8_t *)mbox_data_addr, mbox_data_sz * sizeof(uint32_t)); 94 *(uint32_t *)mbox_relocate_data_addr = 0; 95 96 do { 97 /* Invoke SMC call to ATF to send the VAB certificate to SDM */ 98 ret = mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_VAB_SRC_CERT, 99 (uint32_t *)mbox_relocate_data_addr, mbox_data_sz, 0, &resp, &resp_len); 100 101 /* If SDM is not available, just delay 50ms and retry again */ 102 /* 0x1FF = The device is busy */ 103 if (ret == MBOX_RESP_ERR(0x1FF)) { 104 mdelay(50); 105 } else { 106 break; 107 } 108 } while (--retry_count); 109 110 /* Free the relocate certificate memory space */ 111 zeromem((void *)&mbox_relocate_data_addr, sizeof(uint32_t)); 112 113 114 /* Exclude the size of the VAB certificate from image size */ 115 *p_size = img_sz; 116 117 if (ret) { 118 /* 119 * Unsupported mailbox command or device not in the 120 * owned/secure state 121 */ 122 /* 0x85 = Not allowed under current security setting */ 123 if (ret == MBOX_RESP_ERR(0x85)) { 124 /* SDM bypass authentication */ 125 NOTICE("Image Authentication bypassed at address\n"); 126 return 0; 127 } 128 NOTICE("VAB certificate authentication failed in SDM\n"); 129 /* 0x1FF = The device is busy */ 130 if (ret == MBOX_RESP_ERR(0x1FF)) { 131 NOTICE("Operation timed out\n"); 132 return -ETIMEOUT; 133 } else if (ret == MBOX_WRONG_ID) { 134 NOTICE("No such process\n"); 135 return -EPROCESS; 136 } 137 } else { 138 /* If Certificate Process Status has error */ 139 if (resp) { 140 NOTICE("VAB certificate execution format error\n"); 141 return -EIMGERR; 142 } 143 } 144 145 NOTICE("Image Authentication bypassed at address\n"); 146 return ret; 147 148 } 149 150 static uint32_t get_unaligned_le32(const void *p) 151 { 152 /* TODO: Temp for testing */ 153 //return le32_to_cpup((__le32 *)p); 154 return 0; 155 } 156 157 static void sha512_transform(uint64_t *state, const uint8_t *input) 158 { 159 uint64_t a, b, c, d, e, f, g, h, t1, t2; 160 161 int i; 162 uint64_t W[16]; 163 164 /* load the state into our registers */ 165 a = state[0]; b = state[1]; c = state[2]; d = state[3]; 166 e = state[4]; f = state[5]; g = state[6]; h = state[7]; 167 168 /* now iterate */ 169 for (i = 0 ; i < 80; i += 8) { 170 if (!(i & 8)) { 171 int j; 172 173 if (i < 16) { 174 /* load the input */ 175 for (j = 0; j < 16; j++) 176 LOAD_OP(i + j, W, input); 177 } else { 178 for (j = 0; j < 16; j++) { 179 BLEND_OP(i + j, W); 180 } 181 } 182 } 183 184 t1 = h + e1(e) + Ch(e, f, g) + sha512_K[i] + W[(i & 15)]; 185 t2 = e0(a) + Maj(a, b, c); d += t1; h = t1 + t2; 186 t1 = g + e1(d) + Ch(d, e, f) + sha512_K[i+1] + W[(i & 15) + 1]; 187 t2 = e0(h) + Maj(h, a, b); c += t1; g = t1 + t2; 188 t1 = f + e1(c) + Ch(c, d, e) + sha512_K[i+2] + W[(i & 15) + 2]; 189 t2 = e0(g) + Maj(g, h, a); b += t1; f = t1 + t2; 190 t1 = e + e1(b) + Ch(b, c, d) + sha512_K[i+3] + W[(i & 15) + 3]; 191 t2 = e0(f) + Maj(f, g, h); a += t1; e = t1 + t2; 192 t1 = d + e1(a) + Ch(a, b, c) + sha512_K[i+4] + W[(i & 15) + 4]; 193 t2 = e0(e) + Maj(e, f, g); h += t1; d = t1 + t2; 194 t1 = c + e1(h) + Ch(h, a, b) + sha512_K[i+5] + W[(i & 15) + 5]; 195 t2 = e0(d) + Maj(d, e, f); g += t1; c = t1 + t2; 196 t1 = b + e1(g) + Ch(g, h, a) + sha512_K[i+6] + W[(i & 15) + 6]; 197 t2 = e0(c) + Maj(c, d, e); f += t1; b = t1 + t2; 198 t1 = a + e1(f) + Ch(f, g, h) + sha512_K[i+7] + W[(i & 15) + 7]; 199 t2 = e0(b) + Maj(b, c, d); e += t1; a = t1 + t2; 200 } 201 202 state[0] += a; state[1] += b; state[2] += c; state[3] += d; 203 state[4] += e; state[5] += f; state[6] += g; state[7] += h; 204 205 /* erase our data */ 206 a = b = c = d = e = f = g = h = t1 = t2 = 0; 207 } 208 209 static void sha512_block_fn(sha512_context *sst, const uint8_t *src, 210 int blocks) 211 { 212 while (blocks--) { 213 sha512_transform(sst->state, src); 214 src += SHA512_BLOCK_SIZE; 215 } 216 } 217 218 219 static void sha512_base_do_finalize(sha512_context *sctx) 220 { 221 const int bit_offset = SHA512_BLOCK_SIZE - sizeof(uint64_t[2]); 222 uint64_t *bits = (uint64_t *)(sctx->buf + bit_offset); 223 unsigned int partial = sctx->count[0] % SHA512_BLOCK_SIZE; 224 225 sctx->buf[partial++] = 0x80; 226 if (partial > bit_offset) { 227 memset(sctx->buf + partial, 0x0, SHA512_BLOCK_SIZE - partial); 228 partial = 0; 229 230 sha512_block_fn(sctx, sctx->buf, 1); 231 } 232 233 memset(sctx->buf + partial, 0x0, bit_offset - partial); 234 //fixme bits[0] = cpu_to_be64(sctx->count[1] << 3 | sctx->count[0] >> 61); 235 //fixme bits[1] = cpu_to_be64(sctx->count[0] << 3); 236 bits[0] = (sctx->count[1] << 3 | sctx->count[0] >> 61); 237 bits[1] = (sctx->count[0] << 3); 238 sha512_block_fn(sctx, sctx->buf, 1); 239 } 240 241 static void sha512_base_do_update(sha512_context *sctx, 242 const uint8_t *data, 243 unsigned int len) 244 { 245 unsigned int partial = sctx->count[0] % SHA512_BLOCK_SIZE; 246 247 sctx->count[0] += len; 248 if (sctx->count[0] < len) 249 sctx->count[1]++; 250 251 if (((partial + len) >= SHA512_BLOCK_SIZE)) { 252 int blocks; 253 254 if (partial) { 255 int p = SHA512_BLOCK_SIZE - partial; 256 257 memcpy(sctx->buf + partial, data, p); 258 data += p; 259 len -= p; 260 261 sha512_block_fn(sctx, sctx->buf, 1); 262 } 263 264 blocks = len / SHA512_BLOCK_SIZE; 265 len %= SHA512_BLOCK_SIZE; 266 267 if (blocks) { 268 sha512_block_fn(sctx, data, blocks); 269 data += blocks * SHA512_BLOCK_SIZE; 270 } 271 partial = 0; 272 } 273 if (len) 274 memcpy(sctx->buf + partial, data, len); 275 } 276 277 void sha384_starts(sha512_context *ctx) 278 { 279 ctx->state[0] = SHA384_H0; 280 ctx->state[1] = SHA384_H1; 281 ctx->state[2] = SHA384_H2; 282 ctx->state[3] = SHA384_H3; 283 ctx->state[4] = SHA384_H4; 284 ctx->state[5] = SHA384_H5; 285 ctx->state[6] = SHA384_H6; 286 ctx->state[7] = SHA384_H7; 287 ctx->count[0] = ctx->count[1] = 0; 288 } 289 290 void sha384_update(sha512_context *ctx, const uint8_t *input, uint32_t length) 291 { 292 sha512_base_do_update(ctx, input, length); 293 } 294 295 void sha384_finish(sha512_context *ctx, uint8_t digest[SHA384_SUM_LEN]) 296 { 297 int i; 298 299 sha512_base_do_finalize(ctx); 300 for (i = 0; i < SHA384_SUM_LEN / sizeof(uint64_t); i++) 301 PUT_UINT64_BE(ctx->state[i], digest, i * 8); 302 } 303 304 void sha384_csum_wd(const unsigned char *input, unsigned int ilen, 305 unsigned char *output, unsigned int chunk_sz) 306 { 307 sha512_context ctx; 308 // #if defined(CONFIG_HW_WATCHDOG) || defined(CONFIG_WATCHDOG) 309 // const unsigned char *end; 310 // unsigned char *curr; 311 // int chunk; 312 // #endif 313 314 sha384_starts(&ctx); 315 316 // #if defined(CONFIG_HW_WATCHDOG) || defined(CONFIG_WATCHDOG) 317 // curr = (unsigned char *)input; 318 // end = input + ilen; 319 // while (curr < end) { 320 // chunk = end - curr; 321 // if (chunk > chunk_sz) 322 // chunk = chunk_sz; 323 // sha384_update(&ctx, curr, chunk); 324 // curr += chunk; 325 // schedule(); 326 // } 327 // #else 328 sha384_update(&ctx, input, ilen); 329 // #endif 330 331 sha384_finish(&ctx, output); 332 } 333