1 /*
2 * Copyright (c) 2016, Linaro Limited
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 *
8 * 1. Redistributions of source code must retain the above copyright notice,
9 * this list of conditions and the following disclaimer.
10 *
11 * 2. Redistributions in binary form must reproduce the above copyright notice,
12 * this list of conditions and the following disclaimer in the documentation
13 * and/or other materials provided with the distribution.
14 *
15 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
19 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 * POSSIBILITY OF SUCH DAMAGE.
26 */
27
28 #include <fcntl.h>
29 #include <linux/types.h>
30 #include <linux/mmc/ioctl.h>
31 #include <netinet/in.h>
32 #include <pthread.h>
33 #include <rpmb.h>
34 #include <stdbool.h>
35 #include <stdlib.h>
36 #include <string.h>
37 #include <sys/ioctl.h>
38 #include <sys/stat.h>
39 #include <sys/types.h>
40 #include <tee_client_api.h>
41 #include <teec_trace.h>
42 #include <tee_supplicant.h>
43 #include <unistd.h>
44
45 #ifdef RPMB_EMU
46 #include <stdarg.h>
47 #include "hmac_sha2.h"
48 #else
49 #include <errno.h>
50 #endif
51
52 /*
53 * Request and response definitions must be in sync with the secure side
54 */
55
56 /* Request */
57 struct rpmb_req {
58 uint16_t cmd;
59 #define RPMB_CMD_DATA_REQ 0x00
60 #define RPMB_CMD_GET_DEV_INFO 0x01
61 uint16_t dev_id;
62 uint16_t block_count;
63 /* Optional data frames (rpmb_data_frame) follow */
64 };
65 #define RPMB_REQ_DATA(req) ((void *)((struct rpmb_req *)(req) + 1))
66
67 /* Response to device info request */
68 struct rpmb_dev_info {
69 uint8_t cid[16];
70 uint8_t rpmb_size_mult; /* EXT CSD-slice 168: RPMB Size */
71 uint8_t rel_wr_sec_c; /* EXT CSD-slice 222: Reliable Write Sector */
72 /* Count */
73 uint8_t ret_code;
74 #define RPMB_CMD_GET_DEV_INFO_RET_OK 0x00
75 #define RPMB_CMD_GET_DEV_INFO_RET_ERROR 0x01
76 };
77
78 /*
79 * This structure is shared with OP-TEE and the MMC ioctl layer.
80 * It is the "data frame for RPMB access" defined by JEDEC, minus the
81 * start and stop bits.
82 */
83 struct rpmb_data_frame {
84 uint8_t stuff_bytes[196];
85 uint8_t key_mac[32];
86 uint8_t data[256];
87 uint8_t nonce[16];
88 uint32_t write_counter;
89 uint16_t address;
90 uint16_t block_count;
91 uint16_t op_result;
92 #define RPMB_RESULT_OK 0x00
93 #define RPMB_RESULT_GENERAL_FAILURE 0x01
94 #define RPMB_RESULT_AUTH_FAILURE 0x02
95 #define RPMB_RESULT_ADDRESS_FAILURE 0x04
96 #define RPMB_RESULT_AUTH_KEY_NOT_PROGRAMMED 0x07
97 uint16_t msg_type;
98 #define RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM 0x0001
99 #define RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ 0x0002
100 #define RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE 0x0003
101 #define RPMB_MSG_TYPE_REQ_AUTH_DATA_READ 0x0004
102 #define RPMB_MSG_TYPE_REQ_RESULT_READ 0x0005
103 #define RPMB_MSG_TYPE_RESP_AUTH_KEY_PROGRAM 0x0100
104 #define RPMB_MSG_TYPE_RESP_WRITE_COUNTER_VAL_READ 0x0200
105 #define RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE 0x0300
106 #define RPMB_MSG_TYPE_RESP_AUTH_DATA_READ 0x0400
107 };
108
109
110 static pthread_mutex_t rpmb_mutex = PTHREAD_MUTEX_INITIALIZER;
111
112 /*
113 * ioctl() interface
114 * Comes from: uapi/linux/major.h, linux/mmc/core.h
115 */
116
117 #define MMC_BLOCK_MAJOR 179
118
119 /* mmc_ioc_cmd.opcode */
120 #define MMC_SEND_EXT_CSD 8
121 #define MMC_READ_MULTIPLE_BLOCK 18
122 #define MMC_WRITE_MULTIPLE_BLOCK 25
123
124 /* mmc_ioc_cmd.flags */
125 #define MMC_RSP_PRESENT (1 << 0)
126 #define MMC_RSP_136 (1 << 1) /* 136 bit response */
127 #define MMC_RSP_CRC (1 << 2) /* Expect valid CRC */
128 #define MMC_RSP_OPCODE (1 << 4) /* Response contains opcode */
129
130 #define MMC_RSP_R1 (MMC_RSP_PRESENT|MMC_RSP_CRC|MMC_RSP_OPCODE)
131
132 #define MMC_CMD_ADTC (1 << 5) /* Addressed data transfer command */
133
134 /* mmc_ioc_cmd.write_flag */
135 #define MMC_CMD23_ARG_REL_WR (1 << 31) /* CMD23 reliable write */
136
137 /* Maximum number of commands used in a multiple ioc command request */
138 #define RPMB_MAX_IOC_MULTI_CMDS 3
139
140 #ifndef RPMB_EMU
141
142 #define IOCTL(fd, request, ...) \
143 ({ \
144 int ret; \
145 ret = ioctl((fd), (request), ##__VA_ARGS__); \
146 if (ret < 0) \
147 EMSG("ioctl ret=%d errno=%d", ret, errno); \
148 ret; \
149 })
150
151
152 /* Open and/or return file descriptor to RPMB partition of device dev_id */
mmc_rpmb_fd(uint16_t dev_id)153 static int mmc_rpmb_fd(uint16_t dev_id)
154 {
155 static int id;
156 static int fd = -1;
157 char path[PATH_MAX] = { 0 };
158
159 DMSG("dev_id = %u", dev_id);
160 if (fd < 0) {
161 #ifdef __ANDROID__
162 snprintf(path, sizeof(path), "/dev/mmcblk%urpmb", dev_id);
163 #else
164 snprintf(path, sizeof(path), "/dev/mmcblk%urpmb", dev_id);
165 #endif
166 fd = open(path, O_RDWR);
167 if (fd < 0) {
168 EMSG("Could not open %s (%s)", path, strerror(errno));
169 return -1;
170 }
171 id = dev_id;
172 }
173 if (id != dev_id) {
174 EMSG("Only one MMC device is supported");
175 return -1;
176 }
177 return fd;
178 }
179
180 /* Open eMMC device dev_id */
mmc_fd(uint16_t dev_id)181 static int mmc_fd(uint16_t dev_id)
182 {
183 int fd = 0;
184 char path[PATH_MAX] = { 0 };
185
186 DMSG("dev_id = %u", dev_id);
187 #ifdef __ANDROID__
188 snprintf(path, sizeof(path), "/dev/block/mmcblk%u", dev_id);
189 #else
190 snprintf(path, sizeof(path), "/dev/mmcblk%u", dev_id);
191 #endif
192 fd = open(path, O_RDONLY);
193 if (fd < 0)
194 EMSG("Could not open %s (%s)", path, strerror(errno));
195
196 return fd;
197 }
198
close_mmc_fd(int fd)199 static void close_mmc_fd(int fd)
200 {
201 close(fd);
202 }
203
204 /* Device Identification (CID) register is 16 bytes. It is read from sysfs. */
read_cid(uint16_t dev_id,uint8_t * cid)205 static uint32_t read_cid(uint16_t dev_id, uint8_t *cid)
206 {
207 TEEC_Result res = TEEC_ERROR_GENERIC;
208 char path[48] = { 0 };
209 char hex[3] = { 0 };
210 int st = 0;
211 int fd = 0;
212 int i = 0;
213
214 snprintf(path, sizeof(path),
215 "/sys/class/mmc_host/mmc%u/mmc%u:0001/cid", dev_id, dev_id);
216 fd = open(path, O_RDONLY);
217 if (fd < 0) {
218 EMSG("Could not open %s (%s)", path, strerror(errno));
219 return TEEC_ERROR_ITEM_NOT_FOUND;
220 }
221
222 for (i = 0; i < 16; i++) {
223 st = read(fd, hex, 2);
224 if (st < 0) {
225 EMSG("Read CID error (%s)", strerror(errno));
226 res = TEEC_ERROR_NO_DATA;
227 goto err;
228 }
229 cid[i] = (uint8_t)strtol(hex, NULL, 16);
230 }
231 res = TEEC_SUCCESS;
232 err:
233 close(fd);
234 return res;
235 }
236
237 #else /* RPMB_EMU */
238
239 #define IOCTL(fd, request, ...) ioctl_emu((fd), (request), ##__VA_ARGS__)
240
241 /* Emulated rel_wr_sec_c value (reliable write size, *256 bytes) */
242 #define EMU_RPMB_REL_WR_SEC_C 1
243 /* Emulated rpmb_size_mult value (RPMB size, *128 kB) */
244 #define EMU_RPMB_SIZE_MULT 2
245
246 #define EMU_RPMB_SIZE_BYTES (EMU_RPMB_SIZE_MULT * 128 * 1024)
247
248 /* Emulated eMMC device state */
249 struct rpmb_emu {
250 uint8_t buf[EMU_RPMB_SIZE_BYTES];
251 size_t size;
252 uint8_t key[32];
253 bool key_set;
254 uint8_t nonce[16];
255 uint32_t write_counter;
256 struct {
257 uint16_t msg_type;
258 uint16_t op_result;
259 uint16_t address;
260 } last_op;
261 };
262 static struct rpmb_emu rpmb_emu = {
263 .size = EMU_RPMB_SIZE_BYTES
264 };
265
mem_for_fd(int fd)266 static struct rpmb_emu *mem_for_fd(int fd)
267 {
268 static int sfd = -1;
269
270 if (sfd == -1)
271 sfd = fd;
272 if (sfd != fd) {
273 EMSG("Emulating more than 1 RPMB partition is not supported");
274 return NULL;
275 }
276
277 return &rpmb_emu;
278 }
279
280 #if (DEBUGLEVEL >= TRACE_FLOW)
dump_blocks(size_t startblk,size_t numblk,uint8_t * ptr,bool to_mmc)281 static void dump_blocks(size_t startblk, size_t numblk, uint8_t *ptr,
282 bool to_mmc)
283 {
284 char msg[100] = { 0 };
285 size_t i = 0;
286
287 for (i = 0; i < numblk; i++) {
288 snprintf(msg, sizeof(msg), "%s MMC block %zu",
289 to_mmc ? "Write" : "Read", startblk + i);
290 dump_buffer(msg, ptr, 256);
291 ptr += 256;
292 }
293 }
294 #else
dump_blocks(size_t startblk,size_t numblk,uint8_t * ptr,bool to_mmc)295 static void dump_blocks(size_t startblk, size_t numblk, uint8_t *ptr,
296 bool to_mmc)
297 {
298 (void)startblk;
299 (void)numblk;
300 (void)ptr;
301 (void)to_mmc;
302 }
303 #endif
304
305 #define CUC(x) ((const unsigned char *)(x))
hmac_update_frm(hmac_sha256_ctx * ctx,struct rpmb_data_frame * frm)306 static void hmac_update_frm(hmac_sha256_ctx *ctx, struct rpmb_data_frame *frm)
307 {
308 hmac_sha256_update(ctx, CUC(frm->data), 256);
309 hmac_sha256_update(ctx, CUC(frm->nonce), 16);
310 hmac_sha256_update(ctx, CUC(&frm->write_counter), 4);
311 hmac_sha256_update(ctx, CUC(&frm->address), 2);
312 hmac_sha256_update(ctx, CUC(&frm->block_count), 2);
313 hmac_sha256_update(ctx, CUC(&frm->op_result), 2);
314 hmac_sha256_update(ctx, CUC(&frm->msg_type), 2);
315 }
316
is_hmac_valid(struct rpmb_emu * mem,struct rpmb_data_frame * frm,size_t nfrm)317 static bool is_hmac_valid(struct rpmb_emu *mem, struct rpmb_data_frame *frm,
318 size_t nfrm)
319 {
320 uint8_t mac[32] = { 0 };
321 size_t i = 0;
322 hmac_sha256_ctx ctx;
323
324 memset(&ctx, 0, sizeof(ctx));
325
326 if (!mem->key_set) {
327 EMSG("Cannot check MAC (key not set)");
328 return false;
329 }
330
331 hmac_sha256_init(&ctx, mem->key, sizeof(mem->key));
332 for (i = 0; i < nfrm; i++, frm++)
333 hmac_update_frm(&ctx, frm);
334 frm--;
335 hmac_sha256_final(&ctx, mac, 32);
336
337 if (memcmp(mac, frm->key_mac, 32)) {
338 EMSG("Invalid MAC");
339 return false;
340 }
341 return true;
342 }
343
gen_msb1st_result(uint8_t byte)344 static uint16_t gen_msb1st_result(uint8_t byte)
345 {
346 return (uint16_t)byte << 8;
347 }
348
compute_hmac(struct rpmb_emu * mem,struct rpmb_data_frame * frm,size_t nfrm)349 static uint16_t compute_hmac(struct rpmb_emu *mem, struct rpmb_data_frame *frm,
350 size_t nfrm)
351 {
352 size_t i = 0;
353 hmac_sha256_ctx ctx;
354
355 memset(&ctx, 0, sizeof(ctx));
356
357 if (!mem->key_set) {
358 EMSG("Cannot compute MAC (key not set)");
359 return gen_msb1st_result(RPMB_RESULT_AUTH_KEY_NOT_PROGRAMMED);
360 }
361
362 hmac_sha256_init(&ctx, mem->key, sizeof(mem->key));
363 for (i = 0; i < nfrm; i++, frm++)
364 hmac_update_frm(&ctx, frm);
365 frm--;
366 hmac_sha256_final(&ctx, frm->key_mac, 32);
367
368 return gen_msb1st_result(RPMB_RESULT_OK);
369 }
370
ioctl_emu_mem_transfer(struct rpmb_emu * mem,struct rpmb_data_frame * frm,size_t nfrm,int to_mmc)371 static uint16_t ioctl_emu_mem_transfer(struct rpmb_emu *mem,
372 struct rpmb_data_frame *frm,
373 size_t nfrm, int to_mmc)
374 {
375 size_t start = mem->last_op.address * 256;
376 size_t size = nfrm * 256;
377 size_t i = 0;
378 uint8_t *memptr = NULL;
379
380 if (start > mem->size || start + size > mem->size) {
381 EMSG("Transfer bounds exceeed emulated memory");
382 return gen_msb1st_result(RPMB_RESULT_ADDRESS_FAILURE);
383 }
384 if (to_mmc && !is_hmac_valid(mem, frm, nfrm))
385 return gen_msb1st_result(RPMB_RESULT_AUTH_FAILURE);
386
387 DMSG("Transferring %zu 256-byte data block%s %s MMC (block offset=%zu)",
388 nfrm, (nfrm > 1) ? "s" : "", to_mmc ? "to" : "from", start / 256);
389 for (i = 0; i < nfrm; i++) {
390 memptr = mem->buf + start + i * 256;
391 if (to_mmc) {
392 memcpy(memptr, frm[i].data, 256);
393 mem->write_counter++;
394 frm[i].write_counter = htonl(mem->write_counter);
395 frm[i].msg_type =
396 htons(RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE);
397 } else {
398 memcpy(frm[i].data, memptr, 256);
399 frm[i].msg_type =
400 htons(RPMB_MSG_TYPE_RESP_AUTH_DATA_READ);
401 frm[i].address = htons(mem->last_op.address);
402 frm[i].block_count = nfrm;
403 memcpy(frm[i].nonce, mem->nonce, 16);
404 }
405 frm[i].op_result = gen_msb1st_result(RPMB_RESULT_OK);
406 }
407 dump_blocks(mem->last_op.address, nfrm, mem->buf + start, to_mmc);
408
409 if (!to_mmc)
410 compute_hmac(mem, frm, nfrm);
411
412 return gen_msb1st_result(RPMB_RESULT_OK);
413 }
414
ioctl_emu_get_write_result(struct rpmb_emu * mem,struct rpmb_data_frame * frm)415 static void ioctl_emu_get_write_result(struct rpmb_emu *mem,
416 struct rpmb_data_frame *frm)
417 {
418 frm->msg_type = htons(RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE);
419 frm->op_result = mem->last_op.op_result;
420 frm->address = htons(mem->last_op.address);
421 frm->write_counter = htonl(mem->write_counter);
422 compute_hmac(mem, frm, 1);
423 }
424
ioctl_emu_setkey(struct rpmb_emu * mem,struct rpmb_data_frame * frm)425 static uint16_t ioctl_emu_setkey(struct rpmb_emu *mem,
426 struct rpmb_data_frame *frm)
427 {
428 if (mem->key_set) {
429 EMSG("Key already set");
430 return gen_msb1st_result(RPMB_RESULT_GENERAL_FAILURE);
431 }
432 dump_buffer("Setting key", frm->key_mac, 32);
433 memcpy(mem->key, frm->key_mac, 32);
434 mem->key_set = true;
435
436 return gen_msb1st_result(RPMB_RESULT_OK);
437 }
438
ioctl_emu_get_keyprog_result(struct rpmb_emu * mem,struct rpmb_data_frame * frm)439 static void ioctl_emu_get_keyprog_result(struct rpmb_emu *mem,
440 struct rpmb_data_frame *frm)
441 {
442 frm->msg_type =
443 htons(RPMB_MSG_TYPE_RESP_AUTH_KEY_PROGRAM);
444 frm->op_result = mem->last_op.op_result;
445 }
446
ioctl_emu_read_ctr(struct rpmb_emu * mem,struct rpmb_data_frame * frm)447 static void ioctl_emu_read_ctr(struct rpmb_emu *mem,
448 struct rpmb_data_frame *frm)
449 {
450 DMSG("Reading counter");
451 frm->msg_type = htons(RPMB_MSG_TYPE_RESP_WRITE_COUNTER_VAL_READ);
452 frm->write_counter = htonl(mem->write_counter);
453 memcpy(frm->nonce, mem->nonce, 16);
454 frm->op_result = compute_hmac(mem, frm, 1);
455 }
456
read_cid(uint16_t dev_id,uint8_t * cid)457 static uint32_t read_cid(uint16_t dev_id, uint8_t *cid)
458 {
459 /* Taken from an actual eMMC chip */
460 static const uint8_t test_cid[] = {
461 /* MID (Manufacturer ID): Micron */
462 0xfe,
463 /* CBX (Device/BGA): BGA */
464 0x01,
465 /* OID (OEM/Application ID) */
466 0x4e,
467 /* PNM (Product name) "MMC04G" */
468 0x4d, 0x4d, 0x43, 0x30, 0x34, 0x47,
469 /* PRV (Product revision): 4.2 */
470 0x42,
471 /* PSN (Product serial number) */
472 0xc8, 0xf6, 0x55, 0x2a,
473 /*
474 * MDT (Manufacturing date):
475 * June, 2014
476 */
477 0x61,
478 /* (CRC7 (0xA) << 1) | 0x1 */
479 0x15
480 };
481
482 (void)dev_id;
483 memcpy(cid, test_cid, sizeof(test_cid));
484
485 return TEEC_SUCCESS;
486 }
487
ioctl_emu_set_ext_csd(uint8_t * ext_csd)488 static void ioctl_emu_set_ext_csd(uint8_t *ext_csd)
489 {
490 ext_csd[168] = EMU_RPMB_SIZE_MULT;
491 ext_csd[222] = EMU_RPMB_REL_WR_SEC_C;
492 }
493
494 /* A crude emulation of the MMC ioc commands we need for RPMB */
ioctl_emu_cmd(int fd,struct mmc_ioc_cmd * cmd)495 static int ioctl_emu_cmd(int fd, struct mmc_ioc_cmd *cmd)
496 {
497 struct rpmb_data_frame *frm = NULL;
498 uint16_t msg_type = 0;
499 struct rpmb_emu *mem = mem_for_fd(fd);
500
501 if (!mem)
502 return -1;
503
504 switch (cmd->opcode) {
505 case MMC_SEND_EXT_CSD:
506 ioctl_emu_set_ext_csd((uint8_t *)(uintptr_t)cmd->data_ptr);
507 break;
508
509 case MMC_WRITE_MULTIPLE_BLOCK:
510 frm = (struct rpmb_data_frame *)(uintptr_t)cmd->data_ptr;
511 msg_type = ntohs(frm->msg_type);
512
513 switch (msg_type) {
514 case RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM:
515 mem->last_op.msg_type = msg_type;
516 mem->last_op.op_result = ioctl_emu_setkey(mem, frm);
517 break;
518
519 case RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE:
520 mem->last_op.msg_type = msg_type;
521 mem->last_op.address = ntohs(frm->address);
522 mem->last_op.op_result =
523 ioctl_emu_mem_transfer(mem, frm,
524 cmd->blocks, 1);
525 break;
526
527 case RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ:
528 case RPMB_MSG_TYPE_REQ_AUTH_DATA_READ:
529 memcpy(mem->nonce, frm->nonce, 16);
530 mem->last_op.msg_type = msg_type;
531 mem->last_op.address = ntohs(frm->address);
532 break;
533 default:
534 break;
535 }
536 break;
537
538 case MMC_READ_MULTIPLE_BLOCK:
539 frm = (struct rpmb_data_frame *)(uintptr_t)cmd->data_ptr;
540 msg_type = ntohs(frm->msg_type);
541
542 switch (mem->last_op.msg_type) {
543 case RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM:
544 ioctl_emu_get_keyprog_result(mem, frm);
545 break;
546
547 case RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE:
548 ioctl_emu_get_write_result(mem, frm);
549 break;
550
551 case RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ:
552 ioctl_emu_read_ctr(mem, frm);
553 break;
554
555 case RPMB_MSG_TYPE_REQ_AUTH_DATA_READ:
556 ioctl_emu_mem_transfer(mem, frm, cmd->blocks, 0);
557 break;
558
559 default:
560 EMSG("Unexpected");
561 break;
562 }
563 break;
564
565 default:
566 EMSG("Unsupported ioctl opcode 0x%08x", cmd->opcode);
567 return -1;
568 }
569
570 return 0;
571 }
572
ioctl_emu(int fd,unsigned long request,...)573 static int ioctl_emu(int fd, unsigned long request, ...)
574 {
575 struct mmc_ioc_multi_cmd *mcmd = NULL;
576 struct mmc_ioc_cmd *cmd = NULL;
577 size_t i = 0;
578 int res = 0;
579 va_list ap;
580
581 if (request == MMC_IOC_CMD) {
582 va_start(ap, request);
583 cmd = va_arg(ap, struct mmc_ioc_cmd *);
584 va_end(ap);
585
586 res = ioctl_emu_cmd(fd, cmd);
587 } else if (request == MMC_IOC_MULTI_CMD) {
588 va_start(ap, request);
589 mcmd = va_arg(ap, struct mmc_ioc_multi_cmd *);
590 va_end(ap);
591
592 for (i = 0; i < mcmd->num_of_cmds; i++) {
593 res = ioctl_emu_cmd(fd, &mcmd->cmds[i]);
594 if (res)
595 return res;
596 }
597 } else {
598 EMSG("Unsupported ioctl: 0x%lx", request);
599 return -1;
600 }
601
602 return res;
603 }
604
mmc_rpmb_fd(uint16_t dev_id)605 static int mmc_rpmb_fd(uint16_t dev_id)
606 {
607 (void)dev_id;
608
609 /* Any value != -1 will do in test mode */
610 return 0;
611 }
612
mmc_fd(uint16_t dev_id)613 static int mmc_fd(uint16_t dev_id)
614 {
615 (void)dev_id;
616
617 return 0;
618 }
619
close_mmc_fd(int fd)620 static void close_mmc_fd(int fd)
621 {
622 (void)fd;
623 }
624
625 #endif /* RPMB_EMU */
626
627 /*
628 * Extended CSD Register is 512 bytes and defines device properties
629 * and selected modes.
630 */
read_ext_csd(int fd,uint8_t * ext_csd)631 static uint32_t read_ext_csd(int fd, uint8_t *ext_csd)
632 {
633 int st = 0;
634 struct mmc_ioc_cmd cmd = {
635 .blksz = 512,
636 .blocks = 1,
637 .flags = MMC_RSP_R1 | MMC_CMD_ADTC,
638 .opcode = MMC_SEND_EXT_CSD,
639 };
640
641 mmc_ioc_cmd_set_data(cmd, ext_csd);
642
643 st = IOCTL(fd, MMC_IOC_CMD, &cmd);
644 if (st < 0)
645 return TEEC_ERROR_GENERIC;
646
647 return TEEC_SUCCESS;
648 }
649
set_mmc_io_cmd(struct mmc_ioc_cmd * cmd,unsigned int blocks,__u32 opcode,int write_flag)650 static inline void set_mmc_io_cmd(struct mmc_ioc_cmd *cmd, unsigned int blocks,
651 __u32 opcode, int write_flag)
652 {
653 cmd->blksz = 512;
654 cmd->blocks = blocks;
655 cmd->flags = MMC_RSP_R1 | MMC_CMD_ADTC;
656 cmd->opcode = opcode;
657 cmd->write_flag = write_flag;
658 }
659
rpmb_data_req(int fd,struct rpmb_data_frame * req_frm,size_t req_nfrm,struct rpmb_data_frame * rsp_frm,size_t rsp_nfrm)660 static uint32_t rpmb_data_req(int fd, struct rpmb_data_frame *req_frm,
661 size_t req_nfrm, struct rpmb_data_frame *rsp_frm,
662 size_t rsp_nfrm)
663 {
664 TEEC_Result res = TEEC_SUCCESS;
665 int st = 0;
666 size_t i = 0;
667 uint16_t msg_type = ntohs(req_frm->msg_type);
668 struct mmc_ioc_multi_cmd *mcmd = NULL;
669 struct mmc_ioc_cmd *cmd = NULL;
670
671 for (i = 1; i < req_nfrm; i++) {
672 if (req_frm[i].msg_type != msg_type) {
673 EMSG("All request frames shall be of the same type");
674 return TEEC_ERROR_BAD_PARAMETERS;
675 }
676 }
677
678 DMSG("Req: %zu frame(s) of type 0x%04x", req_nfrm, msg_type);
679 DMSG("Rsp: %zu frame(s)", rsp_nfrm);
680
681 mcmd = (struct mmc_ioc_multi_cmd *)
682 calloc(1, sizeof(struct mmc_ioc_multi_cmd) +
683 RPMB_MAX_IOC_MULTI_CMDS * sizeof(struct mmc_ioc_cmd));
684 if (!mcmd)
685 return TEEC_ERROR_OUT_OF_MEMORY;
686
687 switch(msg_type) {
688 case RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM:
689 case RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE:
690 if (rsp_nfrm != 1) {
691 EMSG("Expected only one response frame");
692 res = TEEC_ERROR_BAD_PARAMETERS;
693 goto out;
694 }
695
696 mcmd->num_of_cmds = 3;
697
698 /* Send write request frame(s) */
699 cmd = &mcmd->cmds[0];
700 set_mmc_io_cmd(cmd, req_nfrm, MMC_WRITE_MULTIPLE_BLOCK,
701 1 | MMC_CMD23_ARG_REL_WR);
702 /*
703 * Black magic: tested on a HiKey board with a HardKernel eMMC
704 * module. When postsleep values are zero, the kernel logs
705 * random errors: "mmc_blk_ioctl_cmd: Card Status=0x00000E00"
706 * and ioctl() fails.
707 */
708 cmd->postsleep_min_us = 20000;
709 cmd->postsleep_max_us = 50000;
710 mmc_ioc_cmd_set_data((*cmd), (uintptr_t)req_frm);
711
712 /* Send result request frame */
713 cmd = &mcmd->cmds[1];
714 set_mmc_io_cmd(cmd, req_nfrm, MMC_WRITE_MULTIPLE_BLOCK, 1);
715 memset(rsp_frm, 0, 1);
716 rsp_frm->msg_type = htons(RPMB_MSG_TYPE_REQ_RESULT_READ);
717 mmc_ioc_cmd_set_data((*cmd), (uintptr_t)rsp_frm);
718
719 /* Read response frame */
720 cmd = &mcmd->cmds[2];
721 set_mmc_io_cmd(cmd, rsp_nfrm, MMC_READ_MULTIPLE_BLOCK, 0);
722 mmc_ioc_cmd_set_data((*cmd), (uintptr_t)rsp_frm);
723 break;
724
725 case RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ:
726 if (rsp_nfrm != 1) {
727 EMSG("Expected only one response frame");
728 res = TEEC_ERROR_BAD_PARAMETERS;
729 goto out;
730 }
731 #if __GNUC__ > 6
732 __attribute__((fallthrough));
733 #endif
734
735 case RPMB_MSG_TYPE_REQ_AUTH_DATA_READ:
736 if (req_nfrm != 1) {
737 EMSG("Expected only one request frame");
738 res = TEEC_ERROR_BAD_PARAMETERS;
739 goto out;
740 }
741
742 mcmd->num_of_cmds = 2;
743
744 /* Send request frame */
745 cmd = &mcmd->cmds[0];
746 set_mmc_io_cmd(cmd, req_nfrm, MMC_WRITE_MULTIPLE_BLOCK, 1);
747 mmc_ioc_cmd_set_data((*cmd), (uintptr_t)req_frm);
748
749 /* Read response frames */
750 cmd = &mcmd->cmds[1];
751 set_mmc_io_cmd(cmd, rsp_nfrm, MMC_READ_MULTIPLE_BLOCK, 0);
752 mmc_ioc_cmd_set_data((*cmd), (uintptr_t)rsp_frm);
753 break;
754
755 default:
756 EMSG("Unsupported message type: %d", msg_type);
757 res = TEEC_ERROR_GENERIC;
758 goto out;
759 }
760
761 st = IOCTL(fd, MMC_IOC_MULTI_CMD, mcmd);
762 if (st < 0)
763 res = TEEC_ERROR_GENERIC;
764
765 out:
766 free(mcmd);
767
768 return res;
769 }
770
rpmb_get_dev_info(uint16_t dev_id,struct rpmb_dev_info * info)771 static uint32_t rpmb_get_dev_info(uint16_t dev_id, struct rpmb_dev_info *info)
772 {
773 int fd = 0;
774 uint32_t res = 0;
775 uint8_t ext_csd[512] = { 0 };
776
777 res = read_cid(dev_id, info->cid);
778 if (res != TEEC_SUCCESS)
779 return res;
780
781 fd = mmc_fd(dev_id);
782 if (fd < 0)
783 return TEEC_ERROR_BAD_PARAMETERS;
784
785 res = read_ext_csd(fd, ext_csd);
786 if (res != TEEC_SUCCESS)
787 goto err;
788
789 info->rel_wr_sec_c = ext_csd[222];
790 info->rpmb_size_mult = ext_csd[168];
791 info->ret_code = RPMB_CMD_GET_DEV_INFO_RET_OK;
792
793 err:
794 close_mmc_fd(fd);
795 return res;
796 }
797
798
799 /*
800 * req is one struct rpmb_req followed by one or more struct rpmb_data_frame
801 * rsp is either one struct rpmb_dev_info or one or more struct rpmb_data_frame
802 */
rpmb_process_request_unlocked(void * req,size_t req_size,void * rsp,size_t rsp_size)803 static uint32_t rpmb_process_request_unlocked(void *req, size_t req_size,
804 void *rsp, size_t rsp_size)
805 {
806 struct rpmb_req *sreq = req;
807 size_t req_nfrm = 0;
808 size_t rsp_nfrm = 0;
809 uint32_t res = 0;
810 int fd = 0;
811
812 if (req_size < sizeof(*sreq))
813 return TEEC_ERROR_BAD_PARAMETERS;
814
815 switch (sreq->cmd) {
816 case RPMB_CMD_DATA_REQ:
817 req_nfrm = (req_size - sizeof(struct rpmb_req)) / 512;
818 rsp_nfrm = rsp_size / 512;
819 fd = mmc_rpmb_fd(sreq->dev_id);
820 if (fd < 0)
821 return TEEC_ERROR_BAD_PARAMETERS;
822 res = rpmb_data_req(fd, RPMB_REQ_DATA(req), req_nfrm, rsp,
823 rsp_nfrm);
824 break;
825
826 case RPMB_CMD_GET_DEV_INFO:
827 if (req_size != sizeof(struct rpmb_req) ||
828 rsp_size != sizeof(struct rpmb_dev_info)) {
829 EMSG("Invalid req/rsp size");
830 return TEEC_ERROR_BAD_PARAMETERS;
831 }
832 res = rpmb_get_dev_info(sreq->dev_id,
833 (struct rpmb_dev_info *)rsp);
834 break;
835
836 default:
837 EMSG("Unsupported RPMB command: %d", sreq->cmd);
838 res = TEEC_ERROR_BAD_PARAMETERS;
839 break;
840 }
841
842 return res;
843 }
844
845
rpmb_process_request(void * req,size_t req_size,void * rsp,size_t rsp_size)846 uint32_t rpmb_process_request(void *req, size_t req_size, void *rsp,
847 size_t rsp_size)
848 {
849 uint32_t res = 0;
850
851 tee_supp_mutex_lock(&rpmb_mutex);
852 res = rpmb_process_request_unlocked(req, req_size, rsp, rsp_size);
853 tee_supp_mutex_unlock(&rpmb_mutex);
854
855 return res;
856 }
857