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/OpteeClientLoadTa.h> 12 #include <optee_include/OpteeClientMem.h> 13 #include <optee_include/OpteeClientRPC.h> 14 #include <optee_include/teesmc.h> 15 #include <optee_include/teesmc_v2.h> 16 #include <optee_include/teesmc_optee.h> 17 #include <optee_include/tee_rpc_types.h> 18 #include <optee_include/tee_rpc.h> 19 #ifdef CONFIG_OPTEE_V1 20 #include <optee_include/OpteeClientRkFs.h> 21 #endif 22 #ifdef CONFIG_OPTEE_V2 23 #include <optee_include/OpteeClientRkNewFs.h> 24 #endif 25 26 /* 27 * Memory allocation. 28 * Currently treated the same for both arguments & payloads. 29 */ 30 TEEC_Result OpteeRpcAlloc(uint32_t Size, uint32_t *Address) 31 { 32 TEEC_Result TeecResult = TEEC_SUCCESS; 33 size_t AllocAddress; 34 35 *Address = 0; 36 37 if (Size != 0) { 38 AllocAddress = (size_t) OpteeClientMemAlloc(Size); 39 40 if (AllocAddress == 0) 41 TeecResult = TEEC_ERROR_OUT_OF_MEMORY; 42 else 43 *Address = AllocAddress; 44 } 45 return TeecResult; 46 } 47 48 /* 49 * Memory free. 50 * Currently treated the same for both arguments & payloads. 51 */ 52 TEEC_Result OpteeRpcFree(uint32_t Address) 53 { 54 OpteeClientMemFree((void *)(size_t)Address); 55 return TEEC_SUCCESS; 56 } 57 58 TEEC_Result OpteeRpcCmdLoadV2Ta(t_teesmc32_arg *TeeSmc32Arg) 59 { 60 TEEC_Result TeecResult = TEEC_SUCCESS; 61 t_teesmc32_param *TeeSmc32Param = NULL; 62 int ta_found = 0; 63 size_t size = 0; 64 65 if (TeeSmc32Arg->num_params != 2) { 66 TeecResult = TEEC_ERROR_BAD_PARAMETERS; 67 goto Exit; 68 } 69 70 TeeSmc32Param = TEESMC32_GET_PARAMS(TeeSmc32Arg); 71 72 size = TeeSmc32Param[1].u.memref.size; 73 ta_found = search_ta((void *)(size_t)&TeeSmc32Param[0].u.value, 74 (void *)(size_t)TeeSmc32Param[1].u.memref.buf_ptr, &size); 75 if (ta_found == TA_BINARY_FOUND) { 76 TeeSmc32Param[1].u.memref.size = size; 77 TeecResult = TEEC_SUCCESS; 78 } else { 79 printf(" TA not found \n"); 80 TeecResult = TEEC_ERROR_ITEM_NOT_FOUND; 81 } 82 83 Exit: 84 TeeSmc32Arg->ret = TeecResult; 85 TeeSmc32Arg->ret_origin = TEEC_ORIGIN_API; 86 87 debug("TEEC: OpteeRpcCmdLoadV2Ta Exit : TeecResult=0x%X\n", TeecResult); 88 89 return TeecResult; 90 } 91 92 /* 93 * Execute an RPMB storage operation. 94 */ 95 96 uint16_t global_block_count; 97 #ifdef CONFIG_SUPPORT_EMMC_RPMB 98 TEEC_Result OpteeRpcCmdRpmb(t_teesmc32_arg *TeeSmc32Arg) 99 { 100 struct tee_rpc_rpmb_dev_info *DevInfo; 101 TEEC_Result EfiStatus; 102 uint16_t RequestMsgType, i; 103 EFI_RK_RPMB_DATA_PACKET *RequestPackets; 104 EFI_RK_RPMB_DATA_PACKET *ResponsePackets; 105 EFI_RK_RPMB_DATA_PACKET *tempPackets; 106 EFI_RK_RPMB_DATA_PACKET_BACK *RequestPackets_back; 107 EFI_RK_RPMB_DATA_PACKET_BACK *tempPackets_back; 108 struct tee_rpc_rpmb_cmd *RpmbRequest; 109 TEEC_Result TeecResult = TEEC_SUCCESS; 110 t_teesmc32_param *TeeSmc32Param; 111 struct mmc *mmc; 112 113 debug("TEEC: Entered RPMB RPC\n"); 114 115 if (TeeSmc32Arg->num_params != 2) { 116 TeecResult = TEEC_ERROR_BAD_PARAMETERS; 117 goto Exit; 118 } 119 120 TeeSmc32Param = TEESMC32_GET_PARAMS(TeeSmc32Arg); 121 RpmbRequest = (struct tee_rpc_rpmb_cmd *)(size_t) 122 TeeSmc32Param[0].u.memref.buf_ptr; 123 switch (RpmbRequest->cmd) { 124 case TEE_RPC_RPMB_CMD_DATA_REQ: { 125 RequestPackets = (EFI_RK_RPMB_DATA_PACKET *)(RpmbRequest + 1); 126 ResponsePackets = (EFI_RK_RPMB_DATA_PACKET *)(size_t) 127 TeeSmc32Param[1].u.memref.buf_ptr; 128 129 global_block_count = 130 (RpmbRequest->block_count == 0 ? 131 1 : RpmbRequest->block_count); 132 RequestPackets_back = 133 memalign(CONFIG_SYS_CACHELINE_SIZE, 134 sizeof(EFI_RK_RPMB_DATA_PACKET_BACK) * global_block_count); 135 memcpy(RequestPackets_back->stuff, 136 RequestPackets->stuff_bytes, 137 RPMB_STUFF_DATA_SIZE); 138 memcpy(RequestPackets_back->mac, 139 RequestPackets->key_mac, 140 RPMB_KEY_MAC_SIZE); 141 memcpy(RequestPackets_back->data, 142 RequestPackets->data, 143 RPMB_DATA_SIZE); 144 memcpy(RequestPackets_back->nonce, 145 RequestPackets->nonce, 146 RPMB_NONCE_SIZE); 147 RequestPackets_back->write_counter = 148 ((RequestPackets->write_counter[3]) << 24) + 149 ((RequestPackets->write_counter[2]) << 16) + 150 ((RequestPackets->write_counter[1]) << 8) + 151 (RequestPackets->write_counter[0]); 152 RequestPackets_back->address = 153 ((RequestPackets->address[1]) << 8) + 154 (RequestPackets->address[0]); 155 RequestPackets_back->block_count = 156 ((RequestPackets->block_count[1]) << 8) + 157 (RequestPackets->block_count[0]); 158 RequestPackets_back->result = 159 ((RequestPackets->op_result[1]) << 8) + 160 (RequestPackets->op_result[0]); 161 RequestPackets_back->request = 162 ((RequestPackets->msg_type[1]) << 8) + 163 (RequestPackets->msg_type[0]); 164 165 RequestMsgType = RPMB_PACKET_DATA_TO_UINT16( 166 RequestPackets->msg_type); 167 168 debug("TEEC: RPMB Data request %d\n", RequestMsgType); 169 170 switch (RequestMsgType) { 171 case TEE_RPC_RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM: { 172 if (init_rpmb() != 0) { 173 TeecResult = TEEC_ERROR_GENERIC; 174 break; 175 } 176 177 EfiStatus = do_programkey((struct s_rpmb *) 178 RequestPackets_back); 179 180 if (finish_rpmb() != 0) { 181 TeecResult = TEEC_ERROR_GENERIC; 182 break; 183 } 184 185 if (EfiStatus != 0) { 186 TeecResult = TEEC_ERROR_GENERIC; 187 break; 188 } 189 190 break; 191 } 192 193 case TEE_RPC_RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ: { 194 if (init_rpmb() != 0) { 195 TeecResult = TEEC_ERROR_GENERIC; 196 break; 197 } 198 199 EfiStatus = do_readcounter((struct s_rpmb *) 200 RequestPackets_back); 201 202 if (finish_rpmb() != 0) { 203 TeecResult = TEEC_ERROR_GENERIC; 204 break; 205 } 206 207 TeecResult = TEEC_SUCCESS; 208 break; 209 } 210 211 case TEE_RPC_RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE: { 212 if (init_rpmb() != 0) { 213 TeecResult = TEEC_ERROR_GENERIC; 214 break; 215 } 216 217 EfiStatus = do_authenticatedwrite((struct s_rpmb *) 218 RequestPackets_back); 219 220 if (finish_rpmb() != 0) { 221 TeecResult = TEEC_ERROR_GENERIC; 222 break; 223 } 224 225 if (EfiStatus != 0) { 226 TeecResult = TEEC_ERROR_GENERIC; 227 break; 228 } 229 230 break; 231 } 232 233 case TEE_RPC_RPMB_MSG_TYPE_REQ_AUTH_DATA_READ: { 234 if (init_rpmb() != 0) { 235 TeecResult = TEEC_ERROR_GENERIC; 236 break; 237 } 238 239 EfiStatus = do_authenticatedread((struct s_rpmb *) 240 RequestPackets_back, global_block_count); 241 242 if (finish_rpmb() != 0) { 243 TeecResult = TEEC_ERROR_GENERIC; 244 break; 245 } 246 247 if (EfiStatus != 0) { 248 TeecResult = TEEC_ERROR_GENERIC; 249 break; 250 } 251 252 break; 253 } 254 255 default: 256 TeecResult = TEEC_ERROR_BAD_PARAMETERS; 257 break; 258 } 259 debug("TEEC: RPMB TeecResult %d\n", TeecResult); 260 break; 261 } 262 263 case TEE_RPC_RPMB_CMD_GET_DEV_INFO: { 264 if (init_rpmb()) { 265 TeecResult = TEEC_ERROR_GENERIC; 266 goto Exit; 267 } 268 269 mmc = do_returnmmc(); 270 if (finish_rpmb()) { 271 TeecResult = TEEC_ERROR_GENERIC; 272 goto Exit; 273 } 274 275 if (mmc == NULL) { 276 TeecResult = TEEC_ERROR_GENERIC; 277 goto Exit; 278 } 279 280 DevInfo = (struct tee_rpc_rpmb_dev_info *)(size_t) 281 TeeSmc32Param[1].u.memref.buf_ptr; 282 283 DevInfo->cid[0] = (mmc->cid[0]) >> 24 & 0xff; 284 DevInfo->cid[1] = (mmc->cid[0]) >> 16 & 0xff; 285 DevInfo->cid[2] = (mmc->cid[0]) >> 8 & 0xff; 286 DevInfo->cid[3] = (mmc->cid[0]) & 0xff; 287 DevInfo->cid[4] = (mmc->cid[1]) >> 24 & 0xff; 288 DevInfo->cid[5] = (mmc->cid[1]) >> 16 & 0xff; 289 DevInfo->cid[6] = (mmc->cid[1]) >> 8 & 0xff; 290 DevInfo->cid[7] = (mmc->cid[1]) & 0xff; 291 DevInfo->cid[8] = (mmc->cid[2]) >> 24 & 0xff; 292 DevInfo->cid[9] = (mmc->cid[2]) >> 16 & 0xff; 293 DevInfo->cid[10] = (mmc->cid[2]) >> 8 & 0xff; 294 DevInfo->cid[11] = (mmc->cid[2]) & 0xff; 295 DevInfo->cid[12] = (mmc->cid[3]) >> 24 & 0xff; 296 DevInfo->cid[13] = (mmc->cid[3]) >> 16 & 0xff; 297 DevInfo->cid[14] = (mmc->cid[3]) >> 8 & 0xff; 298 DevInfo->cid[15] = (mmc->cid[3]) & 0xff; 299 DevInfo->rel_wr_sec_c = 1; 300 DevInfo->rpmb_size_mult = 301 (uint8_t)(mmc->capacity_rpmb / (128 * 1024)); 302 DevInfo->ret_code = 0; 303 304 goto Exit; 305 } 306 307 default: 308 TeecResult = TEEC_ERROR_BAD_PARAMETERS; 309 310 goto Exit; 311 } 312 313 tempPackets = ResponsePackets; 314 tempPackets_back = RequestPackets_back; 315 316 for (i = 0; i < global_block_count; i++) { 317 memcpy(tempPackets->stuff_bytes, 318 tempPackets_back->stuff, 319 RPMB_STUFF_DATA_SIZE); 320 memcpy(tempPackets->key_mac, 321 tempPackets_back->mac, 322 RPMB_KEY_MAC_SIZE); 323 memcpy(tempPackets->data, 324 tempPackets_back->data, 325 RPMB_DATA_SIZE); 326 memcpy(tempPackets->nonce, 327 tempPackets_back->nonce, 328 RPMB_NONCE_SIZE); 329 tempPackets->write_counter[3] = 330 ((tempPackets_back->write_counter) >> 24) & 0xFF; 331 tempPackets->write_counter[2] = 332 ((tempPackets_back->write_counter) >> 16) & 0xFF; 333 tempPackets->write_counter[1] = 334 ((tempPackets_back->write_counter) >> 8) & 0xFF; 335 tempPackets->write_counter[0] = 336 (tempPackets_back->write_counter) & 0xFF; 337 tempPackets->address[1] = 338 ((tempPackets_back->address) >> 8) & 0xFF; 339 tempPackets->address[0] = 340 (tempPackets_back->address) & 0xFF; 341 tempPackets->block_count[1] = 342 ((tempPackets_back->block_count) >> 8) & 0xFF; 343 tempPackets->block_count[0] = 344 (tempPackets_back->block_count) & 0xFF; 345 tempPackets->op_result[1] = 346 ((tempPackets_back->result) >> 8) & 0xFF; 347 tempPackets->op_result[0] = 348 (tempPackets_back->result) & 0xFF; 349 tempPackets->msg_type[1] = 350 ((tempPackets_back->request) >> 8) & 0xFF; 351 tempPackets->msg_type[0] = 352 (tempPackets_back->request) & 0xFF; 353 tempPackets++; 354 tempPackets_back++; 355 } 356 357 free(RequestPackets_back); 358 359 Exit: 360 TeeSmc32Arg->ret = TeecResult; 361 TeeSmc32Arg->ret_origin = TEEC_ORIGIN_API; 362 363 return TeecResult; 364 } 365 #endif 366 367 /* 368 * Execute a normal world local file system operation. 369 */ 370 TEEC_Result OpteeRpcCmdFs(t_teesmc32_arg *TeeSmc32Arg) 371 { 372 TEEC_Result TeecResult = TEEC_SUCCESS; 373 t_teesmc32_param *TeeSmc32Param; 374 375 TeeSmc32Param = TEESMC32_GET_PARAMS(TeeSmc32Arg); 376 #ifdef CONFIG_OPTEE_V1 377 TeecResult = OpteeClientRkFsProcess((void *)(size_t)TeeSmc32Param[0].u.memref.buf_ptr, 378 TeeSmc32Param[0].u.memref.size); 379 TeeSmc32Arg->ret = TEEC_SUCCESS; 380 #endif 381 #ifdef CONFIG_OPTEE_V2 382 TeecResult = OpteeClientRkFsProcess((size_t)TeeSmc32Arg->num_params, 383 (struct tee_ioctl_param *)TeeSmc32Param); 384 TeeSmc32Arg->ret = TeecResult; 385 #endif 386 return TeecResult; 387 } 388 389 /* 390 * TBD. 391 */ 392 TEEC_Result OpteeRpcCmdGetTime(t_teesmc32_arg *TeeSmc32Arg) 393 { 394 return TEEC_ERROR_NOT_IMPLEMENTED; 395 } 396 397 /* 398 * TBD. 399 */ 400 TEEC_Result OpteeRpcCmdWaitMutex(t_teesmc32_arg *TeeSmc32Arg) 401 { 402 return TEEC_ERROR_NOT_IMPLEMENTED; 403 } 404 405 /* 406 * Handle the callback from secure world. 407 */ 408 TEEC_Result OpteeRpcCallback(ARM_SMC_ARGS *ArmSmcArgs) 409 { 410 TEEC_Result TeecResult = TEEC_SUCCESS; 411 412 //printf("OpteeRpcCallback Enter: Arg0=0x%X, Arg1=0x%X, Arg2=0x%X\n", 413 //ArmSmcArgs->Arg0, ArmSmcArgs->Arg1, ArmSmcArgs->Arg2); 414 415 switch (TEESMC_RETURN_GET_RPC_FUNC(ArmSmcArgs->Arg0)) { 416 case TEESMC_RPC_FUNC_ALLOC_ARG: { 417 debug("TEEC: ArmSmcArgs->Arg1 = 0x%x \n", ArmSmcArgs->Arg1); 418 TeecResult = OpteeRpcAlloc(ArmSmcArgs->Arg1, &ArmSmcArgs->Arg2); 419 ArmSmcArgs->Arg5 = ArmSmcArgs->Arg2; 420 ArmSmcArgs->Arg1 = 0; 421 ArmSmcArgs->Arg4 = 0; 422 break; 423 } 424 425 case TEESMC_RPC_FUNC_ALLOC_PAYLOAD: { 426 TeecResult = OpteeRpcAlloc(ArmSmcArgs->Arg1, &ArmSmcArgs->Arg1); 427 break; 428 } 429 430 case TEESMC_RPC_FUNC_FREE_ARG: { 431 TeecResult = OpteeRpcFree(ArmSmcArgs->Arg2); 432 break; 433 } 434 435 case TEESMC_RPC_FUNC_FREE_PAYLOAD: { 436 TeecResult = OpteeRpcFree(ArmSmcArgs->Arg1); 437 break; 438 } 439 440 case TEESMC_RPC_FUNC_IRQ: { 441 break; 442 } 443 444 case TEESMC_RPC_FUNC_CMD: { 445 t_teesmc32_arg *TeeSmc32Arg = 446 (t_teesmc32_arg *)(size_t)((uint64_t)ArmSmcArgs->Arg1 << 32 | ArmSmcArgs->Arg2); 447 debug("TEEC: TeeSmc32Arg->cmd = 0x%x\n", TeeSmc32Arg->cmd); 448 switch (TeeSmc32Arg->cmd) { 449 case OPTEE_MSG_RPC_CMD_SHM_ALLOC_V2: { 450 uint32_t tempaddr; 451 uint32_t allocsize = TeeSmc32Arg->params[0].u.value.b; 452 TeecResult = OpteeRpcAlloc(allocsize, &tempaddr); 453 debug("TEEC: allocsize = 0x%x tempaddr = 0x%x\n", allocsize, tempaddr); 454 TeeSmc32Arg->params[0].attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT_V2; 455 TeeSmc32Arg->params[0].u.memref.buf_ptr = tempaddr; 456 TeeSmc32Arg->params[0].u.memref.size = allocsize; 457 TeeSmc32Arg->params[0].u.memref.shm_ref = tempaddr; 458 TeeSmc32Arg->ret = TEE_SUCCESS; 459 break; 460 } 461 case OPTEE_MSG_RPC_CMD_SHM_FREE_V2: { 462 uint32_t tempaddr = TeeSmc32Arg->params[0].u.value.b; 463 TeecResult = OpteeRpcFree(tempaddr); 464 break; 465 466 } 467 #ifdef CONFIG_SUPPORT_EMMC_RPMB 468 case OPTEE_MSG_RPC_CMD_RPMB_V2: { 469 TeecResult = OpteeRpcCmdRpmb(TeeSmc32Arg); 470 break; 471 } 472 #endif 473 case OPTEE_MSG_RPC_CMD_FS_V2: { 474 TeecResult = OpteeRpcCmdFs(TeeSmc32Arg); 475 break; 476 } 477 case OPTEE_MSG_RPC_CMD_LOAD_TA_V2: { 478 TeecResult = OpteeRpcCmdLoadV2Ta(TeeSmc32Arg); 479 break; 480 } 481 482 default: { 483 printf("TEEC: ...unsupported RPC CMD: cmd=0x%X\n", 484 TeeSmc32Arg->cmd); 485 TeecResult = TEEC_ERROR_NOT_IMPLEMENTED; 486 break; 487 } 488 } 489 490 break; 491 } 492 493 case TEESMC_OPTEE_RPC_FUNC_ALLOC_PAYLOAD: { 494 TeecResult = OpteeRpcAlloc(ArmSmcArgs->Arg1, &ArmSmcArgs->Arg1); 495 ArmSmcArgs->Arg2 = ArmSmcArgs->Arg1; 496 break; 497 } 498 499 case TEESMC_OPTEE_RPC_FUNC_FREE_PAYLOAD: { 500 TeecResult = OpteeRpcFree(ArmSmcArgs->Arg1); 501 break; 502 } 503 504 default: { 505 printf("TEEC: ...unsupported RPC : Arg0=0x%X\n", ArmSmcArgs->Arg0); 506 TeecResult = TEEC_ERROR_NOT_IMPLEMENTED; 507 break; 508 } 509 } 510 511 ArmSmcArgs->Arg0 = TEESMC32_CALL_RETURN_FROM_RPC; 512 debug("TEEC: OpteeRpcCallback Exit : TeecResult=0x%X\n", TeecResult); 513 514 return TeecResult; 515 } 516