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
bedata_to_u16(uint8_t * d)23 static uint16_t bedata_to_u16(uint8_t *d)
24 {
25 return (d[0] << 8) + (d[1]);
26 }
27
bedata_to_u64(uint8_t * d)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
u16_to_bedata(uint16_t src,uint8_t * d)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
rpmb_data_req(struct rpmb_data_frame * req_frm,size_t req_nfrm,struct rpmb_data_frame * rsp_frm,size_t rsp_nfrm)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
rpmb_get_dev_info(struct tee_rpc_rpmb_dev_info * info)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
ufs_rpmb_process(t_teesmc32_arg * smc_arg)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
ufs_rpmb_process(t_teesmc32_arg * smc_arg)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