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