1 /* 2 * Copyright 2024, 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 <optee_include/OpteeClientRPC.h> 11 #include <optee_include/tee_rpc.h> 12 #include <optee_include/tee_rpc_types.h> 13 #include <optee_include/teesmc_v2.h> 14 #include <optee_include/tee_client_api.h> 15 #include "../../drivers/ufs/ufs.h" 16 #include "../../drivers/ufs/ufs-rockchip-rpmb.h" 17 18 /* 19 * Execute an RPMB storage operation. 20 */ 21 #ifdef CONFIG_ROCKCHIP_UFS_RPMB 22 23 static uint16_t bedata_to_u16(uint8_t *d) 24 { 25 return (d[0] << 8) + (d[1]); 26 } 27 28 static uint64_t bedata_to_u64(uint8_t *d) 29 { 30 uint64_t data = 0; 31 uint64_t temp = 0; 32 33 for (int i = 0; i < 8; i++) { 34 temp = d[i]; 35 data += (temp << ((7 - i) * 8)); 36 } 37 38 return data; 39 } 40 41 static void u16_to_bedata(uint16_t src, uint8_t *d) 42 { 43 d[0] = (src >> 8) & 0xff; 44 d[1] = src & 0xff; 45 } 46 47 static int rpmb_data_req(struct rpmb_data_frame *req_frm, 48 size_t req_nfrm, 49 struct rpmb_data_frame *rsp_frm, 50 size_t rsp_nfrm) 51 { 52 struct rpmb_data_frame *req_packets = NULL; 53 struct rpmb_data_frame *rsp_packets = NULL; 54 uint16_t req_type; 55 int ret = -1; 56 57 req_packets = memalign(CONFIG_SYS_CACHELINE_SIZE, sizeof(struct rpmb_data_frame) * req_nfrm); 58 if (!req_packets) 59 goto out; 60 61 rsp_packets = memalign(CONFIG_SYS_CACHELINE_SIZE, sizeof(struct rpmb_data_frame) * rsp_nfrm); 62 if (!rsp_packets) 63 goto out; 64 65 memcpy(req_packets, req_frm, sizeof(struct rpmb_data_frame) * req_nfrm); 66 67 req_type = bedata_to_u16(req_packets->msg_type); 68 69 switch (req_type) { 70 case TEE_RPC_RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM: 71 case TEE_RPC_RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE: { 72 ret = do_rpmb_op(req_packets, req_nfrm, rsp_packets, rsp_nfrm); 73 break; 74 } 75 case TEE_RPC_RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ: { 76 do_rpmb_op(req_packets, req_nfrm, rsp_packets, rsp_nfrm); 77 ret = TEEC_SUCCESS; 78 break; 79 } 80 case TEE_RPC_RPMB_MSG_TYPE_REQ_AUTH_DATA_READ: { 81 uint16_t block_count; 82 block_count = bedata_to_u16(req_packets->block_count); 83 if (block_count == 0) { 84 u16_to_bedata(rsp_nfrm, req_packets->block_count); 85 } 86 ret = do_rpmb_op(req_packets, req_nfrm, rsp_packets, rsp_nfrm); 87 break; 88 } 89 default: 90 ret = TEEC_ERROR_BAD_PARAMETERS; 91 break; 92 } 93 94 for (int i = 0; i < rsp_nfrm; i++) 95 memcpy(rsp_frm + i, rsp_packets + i, sizeof(struct rpmb_data_frame)); 96 97 out: 98 if (req_packets) 99 free(req_packets); 100 101 if (rsp_packets) 102 free(rsp_packets); 103 104 return ret; 105 } 106 107 static int rpmb_get_dev_info(struct tee_rpc_rpmb_dev_info *info) 108 { 109 uint8_t manufacture_name_idx; 110 uint8_t rpmb_rw_blocks; 111 uint64_t rpmb_block_count; 112 uint8_t desc_buff[QUERY_DESC_MAX_SIZE] = { 0 }; 113 uint8_t str_buff[QUERY_DESC_MAX_SIZE] = { 0 }; 114 uint8_t geo_buff[QUERY_DESC_MAX_SIZE] = { 0 }; 115 uint8_t unit_buf[QUERY_DESC_MAX_SIZE] = { 0 }; 116 117 if (ufs_read_device_desc(desc_buff, sizeof(desc_buff))) { 118 printf("get device desc fail!\n"); 119 return -1; 120 } 121 manufacture_name_idx = desc_buff[0x14]; 122 123 if (ufs_read_string_desc(manufacture_name_idx, str_buff, sizeof(str_buff))) { 124 printf("get device desc fail!\n"); 125 return -1; 126 } 127 128 if (str_buff[0] != 0x12) { 129 printf("manufacture name string length error!\n"); 130 return -1; 131 } 132 133 if (sizeof(info->cid) != 0x10) { 134 printf("cid length error!\n"); 135 return -1; 136 } 137 138 memcpy(info->cid, &str_buff[2], sizeof(info->cid)); 139 140 if (ufs_read_geo_desc(geo_buff, sizeof(geo_buff))) { 141 printf("get geometry desc fail!\n"); 142 return -1; 143 } 144 rpmb_rw_blocks = geo_buff[0x17]; 145 146 if (ufs_read_rpmb_unit_desc(unit_buf, sizeof(unit_buf))) { 147 printf("get rpmb ubit desc fail!\n"); 148 return -1; 149 } 150 rpmb_block_count = bedata_to_u64(&unit_buf[0x0B]); 151 152 info->rel_wr_sec_c = rpmb_rw_blocks; 153 info->rpmb_size_mult = rpmb_block_count / 512; 154 info->ret_code = 0; 155 156 return 0; 157 } 158 159 TEEC_Result ufs_rpmb_process(t_teesmc32_arg *smc_arg) 160 { 161 t_teesmc32_param *smc_param; 162 struct tee_rpc_rpmb_cmd *rpmb_req; 163 struct rpmb_data_frame *req_packets = NULL; 164 struct rpmb_data_frame *rsp_packets = NULL; 165 size_t req_num, rsp_num; 166 struct tee_rpc_rpmb_dev_info *dev_info; 167 TEEC_Result result = TEEC_SUCCESS; 168 TEEC_Result status; 169 170 if (smc_arg->num_params != 2) { 171 result = TEEC_ERROR_BAD_PARAMETERS; 172 goto exit; 173 } 174 175 smc_param = TEESMC32_GET_PARAMS(smc_arg); 176 rpmb_req = (struct tee_rpc_rpmb_cmd *)(size_t) 177 smc_param[0].u.memref.buf_ptr; 178 179 switch (rpmb_req->cmd) { 180 case TEE_RPC_RPMB_CMD_DATA_REQ: { 181 req_packets = (struct rpmb_data_frame *)(rpmb_req + 1); 182 rsp_packets = (struct rpmb_data_frame *)(size_t)smc_param[1].u.memref.buf_ptr; 183 req_num = bedata_to_u16(req_packets->block_count); 184 req_num = (req_num == 0 ? 1 : req_num); 185 rsp_num = (rpmb_req->block_count == 0 ? 1 : rpmb_req->block_count); 186 187 status = rpmb_data_req(req_packets, req_num, rsp_packets, rsp_num); 188 if (status != 0) 189 result = TEEC_ERROR_GENERIC; 190 goto exit; 191 } 192 case TEE_RPC_RPMB_CMD_GET_DEV_INFO: { 193 dev_info = (struct tee_rpc_rpmb_dev_info *) 194 (size_t)smc_param[1].u.memref.buf_ptr; 195 196 status = rpmb_get_dev_info(dev_info); 197 if (status != 0) 198 result = TEEC_ERROR_GENERIC; 199 goto exit; 200 } 201 default: 202 result = TEEC_ERROR_BAD_PARAMETERS; 203 goto exit; 204 } 205 206 exit: 207 smc_arg->ret = result; 208 smc_arg->ret_origin = TEEC_ORIGIN_API; 209 210 return result; 211 } 212 213 #else 214 215 TEEC_Result ufs_rpmb_process(t_teesmc32_arg *smc_arg) 216 { 217 smc_arg = smc_arg; 218 return TEEC_ERROR_NOT_IMPLEMENTED; 219 } 220 221 #endif 222