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 <dirent.h>
29 #include <fcntl.h>
30 #include <linux/types.h>
31 #include <linux/mmc/ioctl.h>
32 #include <netinet/in.h>
33 #include <pthread.h>
34 #include <rpmb.h>
35 #include <stdbool.h>
36 #include <stdlib.h>
37 #include <string.h>
38 #include <sys/ioctl.h>
39 #include <sys/stat.h>
40 #include <sys/types.h>
41 #include <tee_client_api.h>
42 #include <teec_trace.h>
43 #include <tee_supplicant.h>
44 #include <unistd.h>
45
46 #ifdef RPMB_EMU
47 #include <stdarg.h>
48 #include "hmac_sha2.h"
49 #else
50 #include <errno.h>
51 #endif
52
53 /*
54 * Request and response definitions must be in sync with the secure side
55 */
56
57 /* Request */
58 struct rpmb_req {
59 uint16_t cmd;
60 #define RPMB_CMD_DATA_REQ 0x00
61 #define RPMB_CMD_GET_DEV_INFO 0x01
62 uint16_t dev_id;
63 uint16_t block_count;
64 /* Optional data frames (rpmb_data_frame) follow */
65 };
66 #define RPMB_REQ_DATA(req) ((void *)((struct rpmb_req *)(req) + 1))
67
68 #define RPMB_CID_SZ 16
69
70 /* Response to device info request */
71 struct rpmb_dev_info {
72 uint8_t cid[RPMB_CID_SZ];
73 uint8_t rpmb_size_mult; /* EXT CSD-slice 168: RPMB Size */
74 uint8_t rel_wr_sec_c; /* EXT CSD-slice 222: Reliable Write Sector */
75 /* Count */
76 uint8_t ret_code;
77 #define RPMB_CMD_GET_DEV_INFO_RET_OK 0x00
78 #define RPMB_CMD_GET_DEV_INFO_RET_ERROR 0x01
79 };
80
81 /*
82 * This structure is shared with OP-TEE and the MMC ioctl layer.
83 * It is the "data frame for RPMB access" defined by JEDEC, minus the
84 * start and stop bits.
85 */
86 struct rpmb_data_frame {
87 uint8_t stuff_bytes[196];
88 uint8_t key_mac[32];
89 uint8_t data[256];
90 uint8_t nonce[16];
91 uint32_t write_counter;
92 uint16_t address;
93 uint16_t block_count;
94 uint16_t op_result;
95 #define RPMB_RESULT_OK 0x00
96 #define RPMB_RESULT_GENERAL_FAILURE 0x01
97 #define RPMB_RESULT_AUTH_FAILURE 0x02
98 #define RPMB_RESULT_ADDRESS_FAILURE 0x04
99 #define RPMB_RESULT_AUTH_KEY_NOT_PROGRAMMED 0x07
100 uint16_t msg_type;
101 #define RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM 0x0001
102 #define RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ 0x0002
103 #define RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE 0x0003
104 #define RPMB_MSG_TYPE_REQ_AUTH_DATA_READ 0x0004
105 #define RPMB_MSG_TYPE_REQ_RESULT_READ 0x0005
106 #define RPMB_MSG_TYPE_RESP_AUTH_KEY_PROGRAM 0x0100
107 #define RPMB_MSG_TYPE_RESP_WRITE_COUNTER_VAL_READ 0x0200
108 #define RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE 0x0300
109 #define RPMB_MSG_TYPE_RESP_AUTH_DATA_READ 0x0400
110 };
111
112
113 static pthread_mutex_t rpmb_mutex = PTHREAD_MUTEX_INITIALIZER;
114
115 /*
116 * ioctl() interface
117 * Comes from: uapi/linux/major.h, linux/mmc/core.h
118 */
119
120 #define MMC_BLOCK_MAJOR 179
121
122 /* mmc_ioc_cmd.opcode */
123 #define MMC_READ_MULTIPLE_BLOCK 18
124 #define MMC_WRITE_MULTIPLE_BLOCK 25
125
126 /* mmc_ioc_cmd.flags */
127 #define MMC_RSP_PRESENT (1 << 0)
128 #define MMC_RSP_136 (1 << 1) /* 136 bit response */
129 #define MMC_RSP_CRC (1 << 2) /* Expect valid CRC */
130 #define MMC_RSP_OPCODE (1 << 4) /* Response contains opcode */
131
132 #define MMC_RSP_R1 (MMC_RSP_PRESENT|MMC_RSP_CRC|MMC_RSP_OPCODE)
133
134 #define MMC_CMD_ADTC (1 << 5) /* Addressed data transfer command */
135
136 /* mmc_ioc_cmd.write_flag */
137 #define MMC_CMD23_ARG_REL_WR (1 << 31) /* CMD23 reliable write */
138
139 /* Maximum number of commands used in a multiple ioc command request */
140 #define RPMB_MAX_IOC_MULTI_CMDS 3
141
142 #ifndef RPMB_EMU
143
144 #define IOCTL(fd, request, ...) \
145 ({ \
146 int ret; \
147 ret = ioctl((fd), (request), ##__VA_ARGS__); \
148 if (ret < 0) \
149 EMSG("ioctl ret=%d errno=%d", ret, errno); \
150 ret; \
151 })
152
153
154 /* Open and/or return file descriptor to RPMB partition of device dev_id */
mmc_rpmb_fd(uint16_t dev_id)155 static int mmc_rpmb_fd(uint16_t dev_id)
156 {
157 static int id;
158 static int fd = -1;
159 char path[PATH_MAX] = { 0 };
160
161 DMSG("dev_id = %u", dev_id);
162 if (fd < 0) {
163 #ifdef __ANDROID__
164 snprintf(path, sizeof(path), "/dev/mmcblk%urpmb", dev_id);
165 #else
166 snprintf(path, sizeof(path), "/dev/mmcblk%urpmb", dev_id);
167 #endif
168 fd = open(path, O_RDWR);
169 if (fd < 0) {
170 EMSG("Could not open %s (%s)", path, strerror(errno));
171 return -1;
172 }
173 id = dev_id;
174 }
175 if (id != dev_id) {
176 EMSG("Only one MMC device is supported");
177 return -1;
178 }
179 return fd;
180 }
181
182 /*
183 * Read @n bytes from @fd, takes care of short reads and EINTR.
184 * Adapted from “Advanced Programming In the UNIX Environment” by W. Richard
185 * Stevens and Stephen A. Rago, 2013, 3rd Edition, Addison-Wesley
186 * (EINTR handling was added)
187 */
readn(int fd,void * ptr,size_t n)188 static ssize_t readn(int fd, void *ptr, size_t n)
189 {
190 size_t nleft = n;
191 ssize_t nread = 0;
192 uint8_t *p = ptr;
193
194 while (nleft > 0) {
195 if ((nread = read(fd, p, nleft)) < 0) {
196 if (errno == EINTR)
197 continue;
198 if (nleft == n)
199 return -1; /* error, nothing read, return -1 */
200 else
201 break; /* error, return amount read so far */
202 } else if (nread == 0) {
203 break; /* EOF */
204 }
205 nleft -= nread;
206 p += nread;
207 }
208 return n - nleft; /* return >= 0 */
209 }
210
211 /* Size of CID printed in hexadecimal */
212 #define CID_STR_SZ (2 * RPMB_CID_SZ)
213
read_cid_str(uint16_t dev_id,char cid[CID_STR_SZ+1])214 static TEEC_Result read_cid_str(uint16_t dev_id, char cid[CID_STR_SZ + 1])
215 {
216 TEEC_Result res = TEEC_ERROR_GENERIC;
217 char path[48] = { 0 };
218 int fd = 0;
219 int st = 0;
220
221 snprintf(path, sizeof(path),
222 "/sys/class/mmc_host/mmc%u/mmc%u:0001/cid", dev_id, dev_id);
223 fd = open(path, O_RDONLY);
224 if (fd < 0)
225 return TEEC_ERROR_ITEM_NOT_FOUND;
226 st = readn(fd, cid, CID_STR_SZ);
227 if (st != CID_STR_SZ) {
228 EMSG("Read CID error");
229 if (errno)
230 EMSG("%s", strerror(errno));
231 res = TEEC_ERROR_NO_DATA;
232 goto out;
233 }
234 res = TEEC_SUCCESS;
235 out:
236 close(fd);
237 return res;
238 }
239
hexchar2int(char c)240 static int hexchar2int(char c)
241 {
242 if (c >= '0' && c <= '9')
243 return c - '0';
244 if (c >= 'a' && c <= 'f')
245 return c - 'a' + 10;
246 if (c >= 'A' && c <= 'F')
247 return c - 'A' + 10;
248 return -1;
249 }
250
hexbyte2int(char * hex)251 static int hexbyte2int(char *hex)
252 {
253 int v1 = hexchar2int(hex[0]);
254 int v2 = hexchar2int(hex[1]);
255
256 if (v1 < 0 || v2 < 0)
257 return -1;
258 return 16 * v1 + v2;
259 }
260
261 /* Device Identification (CID) register is 16 bytes. It is read from sysfs. */
read_cid(uint16_t dev_id,uint8_t * cid)262 static TEEC_Result read_cid(uint16_t dev_id, uint8_t *cid)
263 {
264 TEEC_Result res = TEEC_ERROR_GENERIC;
265 char cid_str[CID_STR_SZ + 1] = { };
266 int i = 0;
267 int v = 0;
268
269 res = read_cid_str(dev_id, cid_str);
270 if (res)
271 return res;
272
273 for (i = 0; i < RPMB_CID_SZ; i++) {
274 v = hexbyte2int(cid_str + 2 * i);
275 if (v < 0) {
276 EMSG("Invalid CID string: %s", cid_str);
277 return TEEC_ERROR_NO_DATA;
278 }
279 cid[i] = v;
280 }
281 return TEEC_SUCCESS;
282 }
283
read_mmc_sysfs_hex(uint16_t dev_id,const char * file,uint8_t * value)284 static TEEC_Result read_mmc_sysfs_hex(uint16_t dev_id, const char *file, uint8_t *value)
285 {
286 TEEC_Result status = TEEC_SUCCESS;
287 char path[255] = { 0 };
288 char buf[255] = { 0 };
289 char *endp = buf;
290 int fd = 0;
291 int ret = 0;
292
293 snprintf(path, sizeof(path), "/sys/class/mmc_host/mmc%u/mmc%u:0001/%s",
294 dev_id, dev_id, file);
295
296 fd = open(path, O_RDONLY);
297 if (fd < 0) {
298 EMSG("Could not open %s (%s)", path, strerror(errno));
299 return TEEC_ERROR_ITEM_NOT_FOUND;
300 }
301
302 ret = readn(fd, buf, sizeof(buf));
303 if (ret < 0) {
304 EMSG("Read error (%s)", strerror(errno));
305 status = TEEC_ERROR_NO_DATA;
306 goto out;
307 }
308
309 errno = 0;
310 *value = strtol(buf, &endp, 16);
311 if (errno || endp == buf)
312 status = TEEC_ERROR_GENERIC;
313
314 out:
315 close(fd);
316 return status;
317 }
318
read_size_mult(uint16_t dev_id,uint8_t * value)319 static TEEC_Result read_size_mult(uint16_t dev_id, uint8_t *value)
320 {
321 return read_mmc_sysfs_hex(dev_id, "raw_rpmb_size_mult", value);
322 }
323
read_rel_wr_sec_c(uint16_t dev_id,uint8_t * value)324 static TEEC_Result read_rel_wr_sec_c(uint16_t dev_id, uint8_t *value)
325 {
326 return read_mmc_sysfs_hex(dev_id, "rel_sectors", value);
327 }
328
329 /*
330 * - If --rpmb-cid is given, find the eMMC RPMB device number with the specified
331 * CID, cache the number, copy it to @ndev_id and return true. If not found
332 * return false.
333 * - If --rpmb-cid is not given, copy @dev_id to @ndev_id and return true.
334 */
remap_rpmb_dev_id(uint16_t dev_id,uint16_t * ndev_id)335 static bool remap_rpmb_dev_id(uint16_t dev_id, uint16_t *ndev_id)
336 {
337 TEEC_Result res = TEEC_ERROR_GENERIC;
338 static bool found = false;
339 static bool err = false;
340 static uint16_t id = 0;
341 char cid[CID_STR_SZ + 1] = { };
342 struct dirent *dent = NULL;
343 DIR *dir = NULL;
344 int num = 0;
345
346 if (err || found)
347 goto out;
348
349 if (!supplicant_params.rpmb_cid) {
350 id = dev_id;
351 found = true;
352 goto out;
353 }
354
355 dir = opendir("/sys/class/mmc_host");
356 if (!dir) {
357 EMSG("Could not open /sys/class/mmc_host (%s)",
358 strerror(errno));
359 err = true;
360 goto out;
361 }
362
363 while ((dent = readdir(dir))) {
364 if (sscanf(dent->d_name, "%*[^0123456789]%d", &num) != 1)
365 continue;
366 if (num > UINT16_MAX) {
367 EMSG("Too many MMC devices");
368 err = true;
369 break;
370 }
371 id = (uint16_t)num;
372 res = read_cid_str(id, cid);
373 if (res)
374 continue;
375 if (strcmp(cid, supplicant_params.rpmb_cid))
376 continue;
377 IMSG("RPMB device %s is at /dev/mmcblk%urpmb\n", cid, id);
378 found = true;
379 break;
380 }
381
382 closedir(dir);
383
384 if (!found)
385 err = true;
386 out:
387 if (found)
388 *ndev_id = id;
389 return found;
390 }
391
392 #else /* RPMB_EMU */
393
394 #define IOCTL(fd, request, ...) ioctl_emu((fd), (request), ##__VA_ARGS__)
395
396 /* Emulated rel_wr_sec_c value (reliable write size, *256 bytes) */
397 #define EMU_RPMB_REL_WR_SEC_C 1
398 /* Emulated rpmb_size_mult value (RPMB size, *128 kB) */
399 #define EMU_RPMB_SIZE_MULT 2
400
401 #define EMU_RPMB_SIZE_BYTES (EMU_RPMB_SIZE_MULT * 128 * 1024)
402
403 /* Emulated eMMC device state */
404 struct rpmb_emu {
405 uint8_t buf[EMU_RPMB_SIZE_BYTES];
406 size_t size;
407 uint8_t key[32];
408 bool key_set;
409 uint8_t nonce[16];
410 uint32_t write_counter;
411 struct {
412 uint16_t msg_type;
413 uint16_t op_result;
414 uint16_t address;
415 } last_op;
416 };
417 static struct rpmb_emu rpmb_emu = {
418 .size = EMU_RPMB_SIZE_BYTES
419 };
420
mem_for_fd(int fd)421 static struct rpmb_emu *mem_for_fd(int fd)
422 {
423 static int sfd = -1;
424
425 if (sfd == -1)
426 sfd = fd;
427 if (sfd != fd) {
428 EMSG("Emulating more than 1 RPMB partition is not supported");
429 return NULL;
430 }
431
432 return &rpmb_emu;
433 }
434
435 #if (DEBUGLEVEL >= TRACE_FLOW)
dump_blocks(size_t startblk,size_t numblk,uint8_t * ptr,bool to_mmc)436 static void dump_blocks(size_t startblk, size_t numblk, uint8_t *ptr,
437 bool to_mmc)
438 {
439 char msg[100] = { 0 };
440 size_t i = 0;
441
442 for (i = 0; i < numblk; i++) {
443 snprintf(msg, sizeof(msg), "%s MMC block %zu",
444 to_mmc ? "Write" : "Read", startblk + i);
445 dump_buffer(msg, ptr, 256);
446 ptr += 256;
447 }
448 }
449 #else
dump_blocks(size_t startblk,size_t numblk,uint8_t * ptr,bool to_mmc)450 static void dump_blocks(size_t startblk, size_t numblk, uint8_t *ptr,
451 bool to_mmc)
452 {
453 (void)startblk;
454 (void)numblk;
455 (void)ptr;
456 (void)to_mmc;
457 }
458 #endif
459
460 #define CUC(x) ((const unsigned char *)(x))
hmac_update_frm(hmac_sha256_ctx * ctx,struct rpmb_data_frame * frm)461 static void hmac_update_frm(hmac_sha256_ctx *ctx, struct rpmb_data_frame *frm)
462 {
463 hmac_sha256_update(ctx, CUC(frm->data), 256);
464 hmac_sha256_update(ctx, CUC(frm->nonce), 16);
465 hmac_sha256_update(ctx, CUC(&frm->write_counter), 4);
466 hmac_sha256_update(ctx, CUC(&frm->address), 2);
467 hmac_sha256_update(ctx, CUC(&frm->block_count), 2);
468 hmac_sha256_update(ctx, CUC(&frm->op_result), 2);
469 hmac_sha256_update(ctx, CUC(&frm->msg_type), 2);
470 }
471
is_hmac_valid(struct rpmb_emu * mem,struct rpmb_data_frame * frm,size_t nfrm)472 static bool is_hmac_valid(struct rpmb_emu *mem, struct rpmb_data_frame *frm,
473 size_t nfrm)
474 {
475 uint8_t mac[32] = { 0 };
476 size_t i = 0;
477 hmac_sha256_ctx ctx;
478
479 memset(&ctx, 0, sizeof(ctx));
480
481 if (!mem->key_set) {
482 EMSG("Cannot check MAC (key not set)");
483 return false;
484 }
485
486 hmac_sha256_init(&ctx, mem->key, sizeof(mem->key));
487 for (i = 0; i < nfrm; i++, frm++)
488 hmac_update_frm(&ctx, frm);
489 frm--;
490 hmac_sha256_final(&ctx, mac, 32);
491
492 if (memcmp(mac, frm->key_mac, 32)) {
493 EMSG("Invalid MAC");
494 return false;
495 }
496 return true;
497 }
498
gen_msb1st_result(uint8_t byte)499 static uint16_t gen_msb1st_result(uint8_t byte)
500 {
501 return (uint16_t)byte << 8;
502 }
503
compute_hmac(struct rpmb_emu * mem,struct rpmb_data_frame * frm,size_t nfrm)504 static uint16_t compute_hmac(struct rpmb_emu *mem, struct rpmb_data_frame *frm,
505 size_t nfrm)
506 {
507 size_t i = 0;
508 hmac_sha256_ctx ctx;
509
510 memset(&ctx, 0, sizeof(ctx));
511
512 if (!mem->key_set) {
513 EMSG("Cannot compute MAC (key not set)");
514 return gen_msb1st_result(RPMB_RESULT_AUTH_KEY_NOT_PROGRAMMED);
515 }
516
517 hmac_sha256_init(&ctx, mem->key, sizeof(mem->key));
518 for (i = 0; i < nfrm; i++, frm++)
519 hmac_update_frm(&ctx, frm);
520 frm--;
521 hmac_sha256_final(&ctx, frm->key_mac, 32);
522
523 return gen_msb1st_result(RPMB_RESULT_OK);
524 }
525
ioctl_emu_mem_transfer(struct rpmb_emu * mem,struct rpmb_data_frame * frm,size_t nfrm,int to_mmc)526 static uint16_t ioctl_emu_mem_transfer(struct rpmb_emu *mem,
527 struct rpmb_data_frame *frm,
528 size_t nfrm, int to_mmc)
529 {
530 size_t start = mem->last_op.address * 256;
531 size_t size = nfrm * 256;
532 size_t i = 0;
533 uint8_t *memptr = NULL;
534
535 if (start > mem->size || start + size > mem->size) {
536 EMSG("Transfer bounds exceeed emulated memory");
537 return gen_msb1st_result(RPMB_RESULT_ADDRESS_FAILURE);
538 }
539 if (to_mmc && !is_hmac_valid(mem, frm, nfrm))
540 return gen_msb1st_result(RPMB_RESULT_AUTH_FAILURE);
541
542 DMSG("Transferring %zu 256-byte data block%s %s MMC (block offset=%zu)",
543 nfrm, (nfrm > 1) ? "s" : "", to_mmc ? "to" : "from", start / 256);
544 for (i = 0; i < nfrm; i++) {
545 memptr = mem->buf + start + i * 256;
546 if (to_mmc) {
547 memcpy(memptr, frm[i].data, 256);
548 mem->write_counter++;
549 frm[i].write_counter = htonl(mem->write_counter);
550 frm[i].msg_type =
551 htons(RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE);
552 } else {
553 memcpy(frm[i].data, memptr, 256);
554 frm[i].msg_type =
555 htons(RPMB_MSG_TYPE_RESP_AUTH_DATA_READ);
556 frm[i].address = htons(mem->last_op.address);
557 frm[i].block_count = nfrm;
558 memcpy(frm[i].nonce, mem->nonce, 16);
559 }
560 frm[i].op_result = gen_msb1st_result(RPMB_RESULT_OK);
561 }
562 dump_blocks(mem->last_op.address, nfrm, mem->buf + start, to_mmc);
563
564 if (!to_mmc)
565 compute_hmac(mem, frm, nfrm);
566
567 return gen_msb1st_result(RPMB_RESULT_OK);
568 }
569
ioctl_emu_get_write_result(struct rpmb_emu * mem,struct rpmb_data_frame * frm)570 static void ioctl_emu_get_write_result(struct rpmb_emu *mem,
571 struct rpmb_data_frame *frm)
572 {
573 frm->msg_type = htons(RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE);
574 frm->op_result = mem->last_op.op_result;
575 frm->address = htons(mem->last_op.address);
576 frm->write_counter = htonl(mem->write_counter);
577 compute_hmac(mem, frm, 1);
578 }
579
ioctl_emu_setkey(struct rpmb_emu * mem,struct rpmb_data_frame * frm)580 static uint16_t ioctl_emu_setkey(struct rpmb_emu *mem,
581 struct rpmb_data_frame *frm)
582 {
583 if (mem->key_set) {
584 EMSG("Key already set");
585 return gen_msb1st_result(RPMB_RESULT_GENERAL_FAILURE);
586 }
587 dump_buffer("Setting key", frm->key_mac, 32);
588 memcpy(mem->key, frm->key_mac, 32);
589 mem->key_set = true;
590
591 return gen_msb1st_result(RPMB_RESULT_OK);
592 }
593
ioctl_emu_get_keyprog_result(struct rpmb_emu * mem,struct rpmb_data_frame * frm)594 static void ioctl_emu_get_keyprog_result(struct rpmb_emu *mem,
595 struct rpmb_data_frame *frm)
596 {
597 frm->msg_type =
598 htons(RPMB_MSG_TYPE_RESP_AUTH_KEY_PROGRAM);
599 frm->op_result = mem->last_op.op_result;
600 }
601
ioctl_emu_read_ctr(struct rpmb_emu * mem,struct rpmb_data_frame * frm)602 static void ioctl_emu_read_ctr(struct rpmb_emu *mem,
603 struct rpmb_data_frame *frm)
604 {
605 DMSG("Reading counter");
606 frm->msg_type = htons(RPMB_MSG_TYPE_RESP_WRITE_COUNTER_VAL_READ);
607 frm->write_counter = htonl(mem->write_counter);
608 memcpy(frm->nonce, mem->nonce, 16);
609 frm->op_result = compute_hmac(mem, frm, 1);
610 }
611
read_cid(uint16_t dev_id,uint8_t * cid)612 static uint32_t read_cid(uint16_t dev_id, uint8_t *cid)
613 {
614 /* Taken from an actual eMMC chip */
615 static const uint8_t test_cid[] = {
616 /* MID (Manufacturer ID): Micron */
617 0xfe,
618 /* CBX (Device/BGA): BGA */
619 0x01,
620 /* OID (OEM/Application ID) */
621 0x4e,
622 /* PNM (Product name) "MMC04G" */
623 0x4d, 0x4d, 0x43, 0x30, 0x34, 0x47,
624 /* PRV (Product revision): 4.2 */
625 0x42,
626 /* PSN (Product serial number) */
627 0xc8, 0xf6, 0x55, 0x2a,
628 /*
629 * MDT (Manufacturing date):
630 * June, 2014
631 */
632 0x61,
633 /* (CRC7 (0xA) << 1) | 0x1 */
634 0x15
635 };
636
637 (void)dev_id;
638 memcpy(cid, test_cid, sizeof(test_cid));
639
640 return TEEC_SUCCESS;
641 }
642
643 /* A crude emulation of the MMC ioc commands we need for RPMB */
ioctl_emu_cmd(int fd,struct mmc_ioc_cmd * cmd)644 static int ioctl_emu_cmd(int fd, struct mmc_ioc_cmd *cmd)
645 {
646 struct rpmb_data_frame *frm = NULL;
647 uint16_t msg_type = 0;
648 struct rpmb_emu *mem = mem_for_fd(fd);
649
650 if (!mem)
651 return -1;
652
653 switch (cmd->opcode) {
654 case MMC_WRITE_MULTIPLE_BLOCK:
655 frm = (struct rpmb_data_frame *)(uintptr_t)cmd->data_ptr;
656 msg_type = ntohs(frm->msg_type);
657
658 switch (msg_type) {
659 case RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM:
660 mem->last_op.msg_type = msg_type;
661 mem->last_op.op_result = ioctl_emu_setkey(mem, frm);
662 break;
663
664 case RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE:
665 mem->last_op.msg_type = msg_type;
666 mem->last_op.address = ntohs(frm->address);
667 mem->last_op.op_result =
668 ioctl_emu_mem_transfer(mem, frm,
669 cmd->blocks, 1);
670 break;
671
672 case RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ:
673 case RPMB_MSG_TYPE_REQ_AUTH_DATA_READ:
674 memcpy(mem->nonce, frm->nonce, 16);
675 mem->last_op.msg_type = msg_type;
676 mem->last_op.address = ntohs(frm->address);
677 break;
678 default:
679 break;
680 }
681 break;
682
683 case MMC_READ_MULTIPLE_BLOCK:
684 frm = (struct rpmb_data_frame *)(uintptr_t)cmd->data_ptr;
685 msg_type = ntohs(frm->msg_type);
686
687 switch (mem->last_op.msg_type) {
688 case RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM:
689 ioctl_emu_get_keyprog_result(mem, frm);
690 break;
691
692 case RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE:
693 ioctl_emu_get_write_result(mem, frm);
694 break;
695
696 case RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ:
697 ioctl_emu_read_ctr(mem, frm);
698 break;
699
700 case RPMB_MSG_TYPE_REQ_AUTH_DATA_READ:
701 ioctl_emu_mem_transfer(mem, frm, cmd->blocks, 0);
702 break;
703
704 default:
705 EMSG("Unexpected");
706 break;
707 }
708 break;
709
710 default:
711 EMSG("Unsupported ioctl opcode 0x%08x", cmd->opcode);
712 return -1;
713 }
714
715 return 0;
716 }
717
ioctl_emu(int fd,unsigned long request,...)718 static int ioctl_emu(int fd, unsigned long request, ...)
719 {
720 struct mmc_ioc_multi_cmd *mcmd = NULL;
721 struct mmc_ioc_cmd *cmd = NULL;
722 size_t i = 0;
723 int res = 0;
724 va_list ap;
725
726 if (request == MMC_IOC_CMD) {
727 va_start(ap, request);
728 cmd = va_arg(ap, struct mmc_ioc_cmd *);
729 va_end(ap);
730
731 res = ioctl_emu_cmd(fd, cmd);
732 } else if (request == MMC_IOC_MULTI_CMD) {
733 va_start(ap, request);
734 mcmd = va_arg(ap, struct mmc_ioc_multi_cmd *);
735 va_end(ap);
736
737 for (i = 0; i < mcmd->num_of_cmds; i++) {
738 res = ioctl_emu_cmd(fd, &mcmd->cmds[i]);
739 if (res)
740 return res;
741 }
742 } else {
743 EMSG("Unsupported ioctl: 0x%lx", request);
744 return -1;
745 }
746
747 return res;
748 }
749
mmc_rpmb_fd(uint16_t dev_id)750 static int mmc_rpmb_fd(uint16_t dev_id)
751 {
752 (void)dev_id;
753
754 /* Any value != -1 will do in test mode */
755 return 0;
756 }
757
read_size_mult(uint16_t dev_id,uint8_t * value)758 static TEEC_Result read_size_mult(uint16_t dev_id, uint8_t *value)
759 {
760 (void)dev_id;
761
762 *value = EMU_RPMB_SIZE_MULT;
763 return TEEC_SUCCESS;
764 }
765
read_rel_wr_sec_c(uint16_t dev_id,uint8_t * value)766 static TEEC_Result read_rel_wr_sec_c(uint16_t dev_id, uint8_t *value)
767 {
768 (void)dev_id;
769
770 *value = EMU_RPMB_REL_WR_SEC_C;
771 return TEEC_SUCCESS;
772 }
773
remap_rpmb_dev_id(uint16_t dev_id,uint16_t * ndev_id)774 static bool remap_rpmb_dev_id(uint16_t dev_id, uint16_t *ndev_id)
775 {
776 *ndev_id = dev_id;
777 return true;
778 }
779
780 #endif /* RPMB_EMU */
781
set_mmc_io_cmd(struct mmc_ioc_cmd * cmd,unsigned int blocks,__u32 opcode,int write_flag)782 static inline void set_mmc_io_cmd(struct mmc_ioc_cmd *cmd, unsigned int blocks,
783 __u32 opcode, int write_flag)
784 {
785 cmd->blksz = 512;
786 cmd->blocks = blocks;
787 cmd->flags = MMC_RSP_R1 | MMC_CMD_ADTC;
788 cmd->opcode = opcode;
789 cmd->write_flag = write_flag;
790 }
791
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)792 static uint32_t rpmb_data_req(int fd, struct rpmb_data_frame *req_frm,
793 size_t req_nfrm, struct rpmb_data_frame *rsp_frm,
794 size_t rsp_nfrm)
795 {
796 TEEC_Result res = TEEC_SUCCESS;
797 int st = 0;
798 size_t i = 0;
799 uint16_t msg_type = ntohs(req_frm->msg_type);
800 struct mmc_ioc_multi_cmd *mcmd = NULL;
801 struct mmc_ioc_cmd *cmd = NULL;
802
803 for (i = 1; i < req_nfrm; i++) {
804 if (req_frm[i].msg_type != msg_type) {
805 EMSG("All request frames shall be of the same type");
806 return TEEC_ERROR_BAD_PARAMETERS;
807 }
808 }
809
810 DMSG("Req: %zu frame(s) of type 0x%04x", req_nfrm, msg_type);
811 DMSG("Rsp: %zu frame(s)", rsp_nfrm);
812
813 mcmd = (struct mmc_ioc_multi_cmd *)
814 calloc(1, sizeof(struct mmc_ioc_multi_cmd) +
815 RPMB_MAX_IOC_MULTI_CMDS * sizeof(struct mmc_ioc_cmd));
816 if (!mcmd)
817 return TEEC_ERROR_OUT_OF_MEMORY;
818
819 switch(msg_type) {
820 case RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM:
821 case RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE:
822 if (rsp_nfrm != 1) {
823 EMSG("Expected only one response frame");
824 res = TEEC_ERROR_BAD_PARAMETERS;
825 goto out;
826 }
827
828 mcmd->num_of_cmds = 3;
829
830 /* Send write request frame(s) */
831 cmd = &mcmd->cmds[0];
832 set_mmc_io_cmd(cmd, req_nfrm, MMC_WRITE_MULTIPLE_BLOCK,
833 1 | MMC_CMD23_ARG_REL_WR);
834 /*
835 * Black magic: tested on a HiKey board with a HardKernel eMMC
836 * module. When postsleep values are zero, the kernel logs
837 * random errors: "mmc_blk_ioctl_cmd: Card Status=0x00000E00"
838 * and ioctl() fails.
839 */
840 cmd->postsleep_min_us = 20000;
841 cmd->postsleep_max_us = 50000;
842 mmc_ioc_cmd_set_data((*cmd), (uintptr_t)req_frm);
843
844 /* Send result request frame */
845 cmd = &mcmd->cmds[1];
846 set_mmc_io_cmd(cmd, req_nfrm, MMC_WRITE_MULTIPLE_BLOCK, 1);
847 memset(rsp_frm, 0, 1);
848 rsp_frm->msg_type = htons(RPMB_MSG_TYPE_REQ_RESULT_READ);
849 mmc_ioc_cmd_set_data((*cmd), (uintptr_t)rsp_frm);
850
851 /* Read response frame */
852 cmd = &mcmd->cmds[2];
853 set_mmc_io_cmd(cmd, rsp_nfrm, MMC_READ_MULTIPLE_BLOCK, 0);
854 mmc_ioc_cmd_set_data((*cmd), (uintptr_t)rsp_frm);
855 break;
856
857 case RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ:
858 if (rsp_nfrm != 1) {
859 EMSG("Expected only one response frame");
860 res = TEEC_ERROR_BAD_PARAMETERS;
861 goto out;
862 }
863 #if __GNUC__ > 6
864 __attribute__((fallthrough));
865 #endif
866
867 case RPMB_MSG_TYPE_REQ_AUTH_DATA_READ:
868 if (req_nfrm != 1) {
869 EMSG("Expected only one request frame");
870 res = TEEC_ERROR_BAD_PARAMETERS;
871 goto out;
872 }
873
874 mcmd->num_of_cmds = 2;
875
876 /* Send request frame */
877 cmd = &mcmd->cmds[0];
878 set_mmc_io_cmd(cmd, req_nfrm, MMC_WRITE_MULTIPLE_BLOCK, 1);
879 mmc_ioc_cmd_set_data((*cmd), (uintptr_t)req_frm);
880
881 /* Read response frames */
882 cmd = &mcmd->cmds[1];
883 set_mmc_io_cmd(cmd, rsp_nfrm, MMC_READ_MULTIPLE_BLOCK, 0);
884 mmc_ioc_cmd_set_data((*cmd), (uintptr_t)rsp_frm);
885 break;
886
887 default:
888 EMSG("Unsupported message type: %d", msg_type);
889 res = TEEC_ERROR_GENERIC;
890 goto out;
891 }
892
893 st = IOCTL(fd, MMC_IOC_MULTI_CMD, mcmd);
894 if (st < 0)
895 res = TEEC_ERROR_GENERIC;
896
897 out:
898 free(mcmd);
899
900 return res;
901 }
902
rpmb_get_dev_info(uint16_t dev_id,struct rpmb_dev_info * info)903 static uint32_t rpmb_get_dev_info(uint16_t dev_id, struct rpmb_dev_info *info)
904 {
905 TEEC_Result res = TEEC_SUCCESS;
906 uint8_t rpmb_size_mult = 0;
907 uint8_t rel_wr_sec_c = 0;
908
909 res = read_cid(dev_id, info->cid);
910 if (res != TEEC_SUCCESS)
911 return res;
912
913 res = read_size_mult(dev_id, &rpmb_size_mult);
914 if (res != TEEC_SUCCESS)
915 return res;
916 info->rpmb_size_mult = rpmb_size_mult;
917
918 res = read_rel_wr_sec_c(dev_id, &rel_wr_sec_c);
919 if (res != TEEC_SUCCESS)
920 return res;
921 info->rel_wr_sec_c = rel_wr_sec_c;
922
923 info->ret_code = RPMB_CMD_GET_DEV_INFO_RET_OK;
924
925 return res;
926 }
927
928
929 /*
930 * req is one struct rpmb_req followed by one or more struct rpmb_data_frame
931 * rsp is either one struct rpmb_dev_info or one or more struct rpmb_data_frame
932 */
rpmb_process_request_unlocked(void * req,size_t req_size,void * rsp,size_t rsp_size)933 static uint32_t rpmb_process_request_unlocked(void *req, size_t req_size,
934 void *rsp, size_t rsp_size)
935 {
936 struct rpmb_req *sreq = req;
937 size_t req_nfrm = 0;
938 size_t rsp_nfrm = 0;
939 uint16_t dev_id = 0;
940 uint32_t res = 0;
941 int fd = 0;
942
943 if (req_size < sizeof(*sreq))
944 return TEEC_ERROR_BAD_PARAMETERS;
945
946 if (!remap_rpmb_dev_id(sreq->dev_id, &dev_id))
947 return TEEC_ERROR_ITEM_NOT_FOUND;
948
949 switch (sreq->cmd) {
950 case RPMB_CMD_DATA_REQ:
951 req_nfrm = (req_size - sizeof(struct rpmb_req)) / 512;
952 rsp_nfrm = rsp_size / 512;
953 fd = mmc_rpmb_fd(dev_id);
954 if (fd < 0)
955 return TEEC_ERROR_BAD_PARAMETERS;
956 res = rpmb_data_req(fd, RPMB_REQ_DATA(req), req_nfrm, rsp,
957 rsp_nfrm);
958 break;
959
960 case RPMB_CMD_GET_DEV_INFO:
961 if (req_size != sizeof(struct rpmb_req) ||
962 rsp_size != sizeof(struct rpmb_dev_info)) {
963 EMSG("Invalid req/rsp size");
964 return TEEC_ERROR_BAD_PARAMETERS;
965 }
966 res = rpmb_get_dev_info(dev_id, (struct rpmb_dev_info *)rsp);
967 break;
968
969 default:
970 EMSG("Unsupported RPMB command: %d", sreq->cmd);
971 res = TEEC_ERROR_BAD_PARAMETERS;
972 break;
973 }
974
975 return res;
976 }
977
978
rpmb_process_request(void * req,size_t req_size,void * rsp,size_t rsp_size)979 uint32_t rpmb_process_request(void *req, size_t req_size, void *rsp,
980 size_t rsp_size)
981 {
982 uint32_t res = 0;
983
984 tee_supp_mutex_lock(&rpmb_mutex);
985 res = rpmb_process_request_unlocked(req, req_size, rsp, rsp_size);
986 tee_supp_mutex_unlock(&rpmb_mutex);
987
988 return res;
989 }
990