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