1 /* 2 * Copyright 2017, Rockchip Electronics Co., Ltd 3 * hisping lin, <hisping.lin@rock-chips.com> 4 * 5 * SPDX-License-Identifier: GPL-2.0+ 6 */ 7 #include <common.h> 8 #include <stdlib.h> 9 #include <command.h> 10 #include <mmc.h> 11 #include <optee_include/OpteeClientMem.h> 12 #include <optee_include/OpteeClientRPC.h> 13 #include <optee_include/teesmc.h> 14 #include <optee_include/teesmc_optee.h> 15 #include <optee_include/tee_rpc_types.h> 16 #include <optee_include/tee_rpc.h> 17 #include <optee_include/258be795-f9ca-40e6-a8699ce6886c5d5d.h> 18 19 /* 20 * Memory allocation. 21 * Currently treated the same for both arguments & payloads. 22 */ 23 TEEC_Result OpteeRpcAlloc(uint32_t Size, uint32_t *Address) 24 { 25 TEEC_Result TeecResult = TEEC_SUCCESS; 26 uint32_t AllocAddress; 27 28 *Address = 0; 29 30 if (Size != 0) { 31 AllocAddress = (uint32_t) OpteeClientMemAlloc(Size); 32 33 if (AllocAddress == 0) 34 TeecResult = TEEC_ERROR_OUT_OF_MEMORY; 35 else 36 *Address = AllocAddress; 37 } 38 return TeecResult; 39 } 40 41 /* 42 * Memory free. 43 * Currently treated the same for both arguments & payloads. 44 */ 45 TEEC_Result OpteeRpcFree(uint32_t Address) 46 { 47 OpteeClientMemFree((void *)Address); 48 return TEEC_SUCCESS; 49 } 50 51 /* 52 * Load a TA from storage into memory and provide it back to OpTEE. 53 * Param[0] = IN: struct tee_rpc_load_ta_cmd 54 * Param[1] = IN: all-zero OUT: TA Image allocated 55 */ 56 TEEC_Result OpteeRpcCmdLoadTa(t_teesmc32_arg *TeeSmc32Arg) 57 { 58 TEEC_Result TeecResult = TEEC_SUCCESS; 59 t_teesmc32_param *TeeSmc32Param = NULL; 60 struct tee_rpc_load_ta_cmd *TeeLoadTaCmd = NULL; 61 uint32_t TeeLoadTaCmdSize = 0; 62 63 if (TeeSmc32Arg->num_params != 2) { 64 TeecResult = TEEC_ERROR_BAD_PARAMETERS; 65 goto Exit; 66 } 67 68 TeeSmc32Param = TEESMC32_GET_PARAMS(TeeSmc32Arg); 69 TeeLoadTaCmd = (struct tee_rpc_load_ta_cmd *) 70 TeeSmc32Param[0].u.memref.buf_ptr; 71 TeeLoadTaCmdSize = TeeSmc32Param[0].u.memref.size; 72 73 if ((TeeLoadTaCmd == NULL) || 74 (TeeLoadTaCmdSize != sizeof(*TeeLoadTaCmd))) { 75 TeecResult = TEEC_ERROR_BAD_PARAMETERS; 76 goto Exit; 77 } 78 79 TEEC_Result Status = 0; 80 void *ImageData = NULL; 81 uint32_t ImageSize = 0; 82 uint32_t AllocAddress = 0; 83 84 ImageData = (void *)keymaster_data; 85 ImageSize = keymaster_size; 86 87 if (Status != 0) { 88 TeecResult = TEEC_ERROR_ITEM_NOT_FOUND; 89 goto Exit; 90 } 91 92 AllocAddress = (uint32_t) OpteeClientMemAlloc(ImageSize); 93 94 if (AllocAddress == 0) { 95 TeecResult = TEEC_ERROR_OUT_OF_MEMORY; 96 goto Exit; 97 } 98 99 memcpy((void *)AllocAddress, ImageData, ImageSize); 100 101 debug("...TA loaded at 0x%X of size 0x%X bytes\n", 102 AllocAddress, ImageSize); 103 debug("...AllocAddress[0] 0x%X ; AllocAddress[1] 0x%X bytes\n", 104 *(char *)AllocAddress, *(char *)(AllocAddress+1)); 105 106 TeeLoadTaCmd->va = (void *)AllocAddress; 107 108 TeeSmc32Param[1].u.memref.buf_ptr = AllocAddress; 109 TeeSmc32Param[1].u.memref.size = ImageSize; 110 111 Exit: 112 TeeSmc32Arg->ret = TeecResult; 113 TeeSmc32Arg->ret_origin = TEEC_ORIGIN_API; 114 115 debug("OpteeRpcCmdLoadTa Exit : TeecResult=0x%X\n", TeecResult); 116 117 return TeecResult; 118 } 119 120 /* 121 * Free a previously loaded TA and release the memory 122 * Param[0] = IN: TA Image to free 123 * 124 * Um, why is OpTEE holding on to this memory? The OS code suggests that OpTEE 125 * is using the binary in place out of shared memory but I don't understand how 126 * runtime modifications of the binary are being prevented if that's the case? 127 */ 128 TEEC_Result OpteeRpcCmdFreeTa(t_teesmc32_arg *TeeSmc32Arg) 129 { 130 TEEC_Result TeecResult = TEEC_SUCCESS; 131 t_teesmc32_param *TeeSmc32Param = NULL; 132 uint32_t ImageSize = 0; 133 uint32_t AllocAddress = 0; 134 135 if (TeeSmc32Arg->num_params != 1) { 136 TeecResult = TEEC_ERROR_BAD_PARAMETERS; 137 goto Exit; 138 } 139 140 TeeSmc32Param = TEESMC32_GET_PARAMS(TeeSmc32Arg); 141 142 AllocAddress = TeeSmc32Param[0].u.memref.buf_ptr; 143 ImageSize = TeeSmc32Param[0].u.memref.size; 144 145 debug("OpteeRpcCmdFreeTa Enter: AllocAddress=0x%X, ImageSize=0x%X\n", 146 (uint32_t) AllocAddress, (uint32_t) ImageSize); 147 148 if (AllocAddress == 0) { 149 TeecResult = TEEC_ERROR_BAD_PARAMETERS; 150 goto Exit; 151 } 152 153 OpteeClientMemFree((void *)AllocAddress); 154 155 Exit: 156 TeeSmc32Arg->ret = TeecResult; 157 TeeSmc32Arg->ret_origin = TEEC_ORIGIN_API; 158 159 debug("OpteeRpcCmdFreeTa Exit : TeecResult=0x%X\n", TeecResult); 160 161 return TeecResult; 162 } 163 164 /* 165 * Execute an RPMB storage operation. 166 */ 167 uint16_t global_block_count; 168 TEEC_Result OpteeRpcCmdRpmb(t_teesmc32_arg *TeeSmc32Arg) 169 { 170 struct tee_rpc_rpmb_dev_info *DevInfo; 171 TEEC_Result EfiStatus; 172 uint16_t RequestMsgType, i; 173 EFI_RK_RPMB_DATA_PACKET *RequestPackets; 174 EFI_RK_RPMB_DATA_PACKET *ResponsePackets; 175 EFI_RK_RPMB_DATA_PACKET *tempPackets; 176 EFI_RK_RPMB_DATA_PACKET_BACK *RequestPackets_back; 177 EFI_RK_RPMB_DATA_PACKET_BACK *tempPackets_back; 178 struct tee_rpc_rpmb_cmd *RpmbRequest; 179 TEEC_Result TeecResult = TEEC_SUCCESS; 180 t_teesmc32_param *TeeSmc32Param; 181 struct mmc *mmc; 182 183 debug("Entered RPMB RPC\n"); 184 185 if (TeeSmc32Arg->num_params != 2) { 186 TeecResult = TEEC_ERROR_BAD_PARAMETERS; 187 goto Exit; 188 } 189 190 TeeSmc32Param = TEESMC32_GET_PARAMS(TeeSmc32Arg); 191 RpmbRequest = (struct tee_rpc_rpmb_cmd *) 192 TeeSmc32Param[0].u.memref.buf_ptr; 193 switch (RpmbRequest->cmd) { 194 case TEE_RPC_RPMB_CMD_DATA_REQ: { 195 RequestPackets = (EFI_RK_RPMB_DATA_PACKET *)(RpmbRequest + 1); 196 ResponsePackets = (EFI_RK_RPMB_DATA_PACKET *) 197 TeeSmc32Param[1].u.memref.buf_ptr; 198 199 global_block_count = 200 (RpmbRequest->block_count == 0 ? 201 1 : RpmbRequest->block_count); 202 RequestPackets_back = 203 malloc(sizeof(EFI_RK_RPMB_DATA_PACKET_BACK) 204 * global_block_count); 205 memcpy(RequestPackets_back->stuff, 206 RequestPackets->stuff_bytes, 207 RPMB_STUFF_DATA_SIZE); 208 memcpy(RequestPackets_back->mac, 209 RequestPackets->key_mac, 210 RPMB_KEY_MAC_SIZE); 211 memcpy(RequestPackets_back->data, 212 RequestPackets->data, 213 RPMB_DATA_SIZE); 214 memcpy(RequestPackets_back->nonce, 215 RequestPackets->nonce, 216 RPMB_NONCE_SIZE); 217 RequestPackets_back->write_counter = 218 ((RequestPackets->write_counter[3]) << 24) + 219 ((RequestPackets->write_counter[2]) << 16) + 220 ((RequestPackets->write_counter[1]) << 8) + 221 (RequestPackets->write_counter[0]); 222 RequestPackets_back->address = 223 ((RequestPackets->address[1]) << 8) + 224 (RequestPackets->address[0]); 225 RequestPackets_back->block_count = 226 ((RequestPackets->block_count[1]) << 8) + 227 (RequestPackets->block_count[0]); 228 RequestPackets_back->result = 229 ((RequestPackets->op_result[1]) << 8) + 230 (RequestPackets->op_result[0]); 231 RequestPackets_back->request = 232 ((RequestPackets->msg_type[1]) << 8) + 233 (RequestPackets->msg_type[0]); 234 235 RequestMsgType = RPMB_PACKET_DATA_TO_UINT16( 236 RequestPackets->msg_type); 237 238 debug("RPMB Data request %d\n", RequestMsgType); 239 240 switch (RequestMsgType) { 241 case TEE_RPC_RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM: { 242 EfiStatus = init_rpmb(); 243 if (EfiStatus != 0) { 244 TeecResult = TEEC_ERROR_GENERIC; 245 break; 246 } 247 248 EfiStatus = do_programkey((struct s_rpmb *) 249 RequestPackets_back); 250 251 if (EfiStatus != 0) { 252 TeecResult = TEEC_ERROR_GENERIC; 253 break; 254 } 255 256 EfiStatus = finish_rpmb(); 257 if (EfiStatus != 0) { 258 TeecResult = TEEC_ERROR_GENERIC; 259 break; 260 } 261 262 break; 263 } 264 265 case TEE_RPC_RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ: { 266 EfiStatus = init_rpmb(); 267 if (EfiStatus != 0) { 268 TeecResult = TEEC_ERROR_GENERIC; 269 break; 270 } 271 272 EfiStatus = do_readcounter((struct s_rpmb *) 273 RequestPackets_back); 274 if (EfiStatus != 0) { 275 TeecResult = TEEC_ERROR_GENERIC; 276 break; 277 } 278 279 EfiStatus = finish_rpmb(); 280 if (EfiStatus != 0) { 281 TeecResult = TEEC_ERROR_GENERIC; 282 break; 283 } 284 285 break; 286 } 287 288 case TEE_RPC_RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE: { 289 EfiStatus = init_rpmb(); 290 if (EfiStatus != 0) { 291 TeecResult = TEEC_ERROR_GENERIC; 292 break; 293 } 294 295 EfiStatus = do_authenticatedwrite((struct s_rpmb *) 296 RequestPackets_back); 297 if (EfiStatus != 0) { 298 TeecResult = TEEC_ERROR_GENERIC; 299 break; 300 } 301 302 EfiStatus = finish_rpmb(); 303 304 if (EfiStatus != 0) { 305 TeecResult = TEEC_ERROR_GENERIC; 306 break; 307 } 308 309 break; 310 } 311 312 case TEE_RPC_RPMB_MSG_TYPE_REQ_AUTH_DATA_READ: { 313 EfiStatus = init_rpmb(); 314 if (EfiStatus != 0) { 315 TeecResult = TEEC_ERROR_GENERIC; 316 break; 317 } 318 319 EfiStatus = do_authenticatedread((struct s_rpmb *) 320 RequestPackets_back, global_block_count); 321 if (EfiStatus != 0) { 322 TeecResult = TEEC_ERROR_GENERIC; 323 break; 324 } 325 326 EfiStatus = finish_rpmb(); 327 328 if (EfiStatus != 0) { 329 TeecResult = TEEC_ERROR_GENERIC; 330 break; 331 } 332 333 break; 334 } 335 336 default: 337 TeecResult = TEEC_ERROR_BAD_PARAMETERS; 338 break; 339 } 340 debug("RPMB TeecResult %d\n", TeecResult); 341 break; 342 } 343 344 case TEE_RPC_RPMB_CMD_GET_DEV_INFO: { 345 mmc = do_returnmmc(); 346 347 DevInfo = (struct tee_rpc_rpmb_dev_info *) 348 TeeSmc32Param[1].u.memref.buf_ptr; 349 350 DevInfo->cid[0] = (mmc->cid[0]) >> 24 & 0xff; 351 DevInfo->cid[1] = (mmc->cid[0]) >> 16 & 0xff; 352 DevInfo->cid[2] = (mmc->cid[0]) >> 8 & 0xff; 353 DevInfo->cid[3] = (mmc->cid[0]) & 0xff; 354 DevInfo->cid[4] = (mmc->cid[1]) >> 24 & 0xff; 355 DevInfo->cid[5] = (mmc->cid[1]) >> 16 & 0xff; 356 DevInfo->cid[6] = (mmc->cid[1]) >> 8 & 0xff; 357 DevInfo->cid[7] = (mmc->cid[1]) & 0xff; 358 DevInfo->cid[8] = (mmc->cid[2]) >> 24 & 0xff; 359 DevInfo->cid[9] = (mmc->cid[2]) >> 16 & 0xff; 360 DevInfo->cid[10] = (mmc->cid[2]) >> 8 & 0xff; 361 DevInfo->cid[11] = (mmc->cid[2]) & 0xff; 362 DevInfo->cid[12] = (mmc->cid[3]) >> 24 & 0xff; 363 DevInfo->cid[13] = (mmc->cid[3]) >> 16 & 0xff; 364 DevInfo->cid[14] = (mmc->cid[3]) >> 8 & 0xff; 365 DevInfo->cid[15] = (mmc->cid[3]) & 0xff; 366 DevInfo->rel_wr_sec_c = 1; 367 DevInfo->rpmb_size_mult = 368 (uint8_t)(mmc->capacity_rpmb / (128 * 1024)); 369 DevInfo->ret_code = 0; 370 371 goto Exit; 372 } 373 374 default: 375 TeecResult = TEEC_ERROR_BAD_PARAMETERS; 376 377 goto Exit; 378 } 379 380 tempPackets = ResponsePackets; 381 tempPackets_back = RequestPackets_back; 382 383 for (i = 0; i < global_block_count; i++) { 384 memcpy(tempPackets->stuff_bytes, 385 tempPackets_back->stuff, 386 RPMB_STUFF_DATA_SIZE); 387 memcpy(tempPackets->key_mac, 388 tempPackets_back->mac, 389 RPMB_KEY_MAC_SIZE); 390 memcpy(tempPackets->data, 391 tempPackets_back->data, 392 RPMB_DATA_SIZE); 393 memcpy(tempPackets->nonce, 394 tempPackets_back->nonce, 395 RPMB_NONCE_SIZE); 396 tempPackets->write_counter[3] = 397 ((tempPackets_back->write_counter) >> 24) & 0xFF; 398 tempPackets->write_counter[2] = 399 ((tempPackets_back->write_counter) >> 16) & 0xFF; 400 tempPackets->write_counter[1] = 401 ((tempPackets_back->write_counter) >> 8) & 0xFF; 402 tempPackets->write_counter[0] = 403 (tempPackets_back->write_counter) & 0xFF; 404 tempPackets->address[1] = 405 ((tempPackets_back->address) >> 8) & 0xFF; 406 tempPackets->address[0] = 407 (tempPackets_back->address) & 0xFF; 408 tempPackets->block_count[1] = 409 ((tempPackets_back->block_count) >> 8) & 0xFF; 410 tempPackets->block_count[0] = 411 (tempPackets_back->block_count) & 0xFF; 412 tempPackets->op_result[1] = 413 ((tempPackets_back->result) >> 8) & 0xFF; 414 tempPackets->op_result[0] = 415 (tempPackets_back->result) & 0xFF; 416 tempPackets->msg_type[1] = 417 ((tempPackets_back->request) >> 8) & 0xFF; 418 tempPackets->msg_type[0] = 419 (tempPackets_back->request) & 0xFF; 420 tempPackets++; 421 tempPackets_back++; 422 } 423 424 free(RequestPackets_back); 425 426 Exit: 427 TeeSmc32Arg->ret = TeecResult; 428 TeeSmc32Arg->ret_origin = TEEC_ORIGIN_API; 429 430 return TeecResult; 431 } 432 433 /* 434 * Execute a normal world local file system operation. 435 */ 436 TEEC_Result OpteeRpcCmdFs(t_teesmc32_arg *TeeSmc32Arg) 437 { 438 return TEEC_ERROR_NOT_IMPLEMENTED; 439 } 440 441 442 /* 443 * TBD. 444 */ 445 TEEC_Result OpteeRpcCmdGetTime(t_teesmc32_arg *TeeSmc32Arg) 446 { 447 return TEEC_ERROR_NOT_IMPLEMENTED; 448 } 449 450 451 /* 452 * TBD. 453 */ 454 TEEC_Result OpteeRpcCmdWaitMutex(t_teesmc32_arg *TeeSmc32Arg) 455 { 456 return TEEC_ERROR_NOT_IMPLEMENTED; 457 } 458 459 /* 460 * Handle the callback from secure world. 461 */ 462 TEEC_Result OpteeRpcCallback(ARM_SMC_ARGS *ArmSmcArgs) 463 { 464 TEEC_Result TeecResult = TEEC_SUCCESS; 465 466 debug("OpteeRpcCallback Enter: Arg0=0x%X, Arg1=0x%X, Arg2=0x%X\n", 467 ArmSmcArgs->Arg0, ArmSmcArgs->Arg1, ArmSmcArgs->Arg2); 468 469 switch (TEESMC_RETURN_GET_RPC_FUNC(ArmSmcArgs->Arg0)) { 470 case TEESMC_RPC_FUNC_ALLOC_ARG: { 471 TeecResult = OpteeRpcAlloc(ArmSmcArgs->Arg1, &ArmSmcArgs->Arg1); 472 break; 473 } 474 475 case TEESMC_RPC_FUNC_ALLOC_PAYLOAD: { 476 TeecResult = OpteeRpcAlloc(ArmSmcArgs->Arg1, &ArmSmcArgs->Arg1); 477 break; 478 } 479 480 case TEESMC_RPC_FUNC_FREE_ARG: { 481 TeecResult = OpteeRpcFree(ArmSmcArgs->Arg1); 482 break; 483 } 484 485 case TEESMC_RPC_FUNC_FREE_PAYLOAD: { 486 TeecResult = OpteeRpcFree(ArmSmcArgs->Arg1); 487 break; 488 } 489 490 case TEESMC_RPC_FUNC_IRQ: { 491 break; 492 } 493 494 case TEESMC_RPC_FUNC_CMD: { 495 t_teesmc32_arg *TeeSmc32Arg = 496 (t_teesmc32_arg *)ArmSmcArgs->Arg1; 497 498 switch (TeeSmc32Arg->cmd) { 499 case TEE_RPC_LOAD_TA: { 500 TeecResult = OpteeRpcCmdLoadTa(TeeSmc32Arg); 501 break; 502 } 503 504 case TEE_RPC_FREE_TA: { 505 TeecResult = OpteeRpcCmdFreeTa(TeeSmc32Arg); 506 break; 507 } 508 509 case TEE_RPC_RPMB_CMD: { 510 TeecResult = OpteeRpcCmdRpmb(TeeSmc32Arg); 511 break; 512 } 513 514 case TEE_RPC_FS: { 515 TeecResult = OpteeRpcCmdFs(TeeSmc32Arg); 516 break; 517 } 518 519 case TEE_RPC_GET_TIME: { 520 TeecResult = OpteeRpcCmdGetTime(TeeSmc32Arg); 521 break; 522 } 523 524 case TEE_RPC_WAIT_MUTEX: { 525 TeecResult = OpteeRpcCmdWaitMutex(TeeSmc32Arg); 526 break; 527 } 528 529 default: { 530 printf("...unsupported RPC CMD: cmd=0x%X\n", 531 TeeSmc32Arg->cmd); 532 TeecResult = TEEC_ERROR_NOT_IMPLEMENTED; 533 break; 534 } 535 } 536 537 break; 538 } 539 540 case TEESMC_OPTEE_RPC_FUNC_ALLOC_PAYLOAD: { 541 TeecResult = OpteeRpcAlloc(ArmSmcArgs->Arg1, &ArmSmcArgs->Arg1); 542 ArmSmcArgs->Arg2 = ArmSmcArgs->Arg1; 543 break; 544 } 545 546 case TEESMC_OPTEE_RPC_FUNC_FREE_PAYLOAD: { 547 TeecResult = OpteeRpcFree(ArmSmcArgs->Arg1); 548 break; 549 } 550 551 default: { 552 printf("...unsupported RPC : Arg0=0x%X\n", ArmSmcArgs->Arg0); 553 TeecResult = TEEC_ERROR_NOT_IMPLEMENTED; 554 break; 555 } 556 } 557 558 ArmSmcArgs->Arg0 = TEESMC32_CALL_RETURN_FROM_RPC; 559 debug("OpteeRpcCallback Exit : TeecResult=0x%X\n", TeecResult); 560 561 return TeecResult; 562 } 563