/* * Copyright (c) 2016, Linaro Limited * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * 1. Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * * 2. Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #ifdef RPMB_EMU #include #include "hmac_sha2.h" #else #include #endif /* * Request and response definitions must be in sync with the secure side */ /* Request */ struct rpmb_req { uint16_t cmd; #define RPMB_CMD_DATA_REQ 0x00 #define RPMB_CMD_GET_DEV_INFO 0x01 uint16_t dev_id; uint16_t block_count; /* Optional data frames (rpmb_data_frame) follow */ }; #define RPMB_REQ_DATA(req) ((void *)((struct rpmb_req *)(req) + 1)) /* Response to device info request */ struct rpmb_dev_info { uint8_t cid[16]; uint8_t rpmb_size_mult; /* EXT CSD-slice 168: RPMB Size */ uint8_t rel_wr_sec_c; /* EXT CSD-slice 222: Reliable Write Sector */ /* Count */ uint8_t ret_code; #define RPMB_CMD_GET_DEV_INFO_RET_OK 0x00 #define RPMB_CMD_GET_DEV_INFO_RET_ERROR 0x01 }; /* * This structure is shared with OP-TEE and the MMC ioctl layer. * It is the "data frame for RPMB access" defined by JEDEC, minus the * start and stop bits. */ struct rpmb_data_frame { uint8_t stuff_bytes[196]; uint8_t key_mac[32]; uint8_t data[256]; uint8_t nonce[16]; uint32_t write_counter; uint16_t address; uint16_t block_count; uint16_t op_result; #define RPMB_RESULT_OK 0x00 #define RPMB_RESULT_GENERAL_FAILURE 0x01 #define RPMB_RESULT_AUTH_FAILURE 0x02 #define RPMB_RESULT_ADDRESS_FAILURE 0x04 #define RPMB_RESULT_AUTH_KEY_NOT_PROGRAMMED 0x07 uint16_t msg_type; #define RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM 0x0001 #define RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ 0x0002 #define RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE 0x0003 #define RPMB_MSG_TYPE_REQ_AUTH_DATA_READ 0x0004 #define RPMB_MSG_TYPE_REQ_RESULT_READ 0x0005 #define RPMB_MSG_TYPE_RESP_AUTH_KEY_PROGRAM 0x0100 #define RPMB_MSG_TYPE_RESP_WRITE_COUNTER_VAL_READ 0x0200 #define RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE 0x0300 #define RPMB_MSG_TYPE_RESP_AUTH_DATA_READ 0x0400 }; static pthread_mutex_t rpmb_mutex = PTHREAD_MUTEX_INITIALIZER; /* * ioctl() interface * Comes from: uapi/linux/major.h, linux/mmc/core.h */ #define MMC_BLOCK_MAJOR 179 /* mmc_ioc_cmd.opcode */ #define MMC_SEND_EXT_CSD 8 #define MMC_READ_MULTIPLE_BLOCK 18 #define MMC_WRITE_MULTIPLE_BLOCK 25 /* mmc_ioc_cmd.flags */ #define MMC_RSP_PRESENT (1 << 0) #define MMC_RSP_136 (1 << 1) /* 136 bit response */ #define MMC_RSP_CRC (1 << 2) /* Expect valid CRC */ #define MMC_RSP_OPCODE (1 << 4) /* Response contains opcode */ #define MMC_RSP_R1 (MMC_RSP_PRESENT|MMC_RSP_CRC|MMC_RSP_OPCODE) #define MMC_CMD_ADTC (1 << 5) /* Addressed data transfer command */ /* mmc_ioc_cmd.write_flag */ #define MMC_CMD23_ARG_REL_WR (1 << 31) /* CMD23 reliable write */ /* Maximum number of commands used in a multiple ioc command request */ #define RPMB_MAX_IOC_MULTI_CMDS 3 #ifndef RPMB_EMU #define IOCTL(fd, request, ...) \ ({ \ int ret; \ ret = ioctl((fd), (request), ##__VA_ARGS__); \ if (ret < 0) \ EMSG("ioctl ret=%d errno=%d", ret, errno); \ ret; \ }) /* Open and/or return file descriptor to RPMB partition of device dev_id */ static int mmc_rpmb_fd(uint16_t dev_id) { static int id; static int fd = -1; char path[PATH_MAX] = { 0 }; DMSG("dev_id = %u", dev_id); if (fd < 0) { #ifdef __ANDROID__ snprintf(path, sizeof(path), "/dev/mmcblk%urpmb", dev_id); #else snprintf(path, sizeof(path), "/dev/mmcblk%urpmb", dev_id); #endif fd = open(path, O_RDWR); if (fd < 0) { EMSG("Could not open %s (%s)", path, strerror(errno)); return -1; } id = dev_id; } if (id != dev_id) { EMSG("Only one MMC device is supported"); return -1; } return fd; } /* Open eMMC device dev_id */ static int mmc_fd(uint16_t dev_id) { int fd = 0; char path[PATH_MAX] = { 0 }; DMSG("dev_id = %u", dev_id); #ifdef __ANDROID__ snprintf(path, sizeof(path), "/dev/block/mmcblk%u", dev_id); #else snprintf(path, sizeof(path), "/dev/mmcblk%u", dev_id); #endif fd = open(path, O_RDONLY); if (fd < 0) EMSG("Could not open %s (%s)", path, strerror(errno)); return fd; } static void close_mmc_fd(int fd) { close(fd); } /* Device Identification (CID) register is 16 bytes. It is read from sysfs. */ static uint32_t read_cid(uint16_t dev_id, uint8_t *cid) { TEEC_Result res = TEEC_ERROR_GENERIC; char path[48] = { 0 }; char hex[3] = { 0 }; int st = 0; int fd = 0; int i = 0; snprintf(path, sizeof(path), "/sys/class/mmc_host/mmc%u/mmc%u:0001/cid", dev_id, dev_id); fd = open(path, O_RDONLY); if (fd < 0) { EMSG("Could not open %s (%s)", path, strerror(errno)); return TEEC_ERROR_ITEM_NOT_FOUND; } for (i = 0; i < 16; i++) { st = read(fd, hex, 2); if (st < 0) { EMSG("Read CID error (%s)", strerror(errno)); res = TEEC_ERROR_NO_DATA; goto err; } cid[i] = (uint8_t)strtol(hex, NULL, 16); } res = TEEC_SUCCESS; err: close(fd); return res; } #else /* RPMB_EMU */ #define IOCTL(fd, request, ...) ioctl_emu((fd), (request), ##__VA_ARGS__) /* Emulated rel_wr_sec_c value (reliable write size, *256 bytes) */ #define EMU_RPMB_REL_WR_SEC_C 1 /* Emulated rpmb_size_mult value (RPMB size, *128 kB) */ #define EMU_RPMB_SIZE_MULT 2 #define EMU_RPMB_SIZE_BYTES (EMU_RPMB_SIZE_MULT * 128 * 1024) /* Emulated eMMC device state */ struct rpmb_emu { uint8_t buf[EMU_RPMB_SIZE_BYTES]; size_t size; uint8_t key[32]; bool key_set; uint8_t nonce[16]; uint32_t write_counter; struct { uint16_t msg_type; uint16_t op_result; uint16_t address; } last_op; }; static struct rpmb_emu rpmb_emu = { .size = EMU_RPMB_SIZE_BYTES }; static struct rpmb_emu *mem_for_fd(int fd) { static int sfd = -1; if (sfd == -1) sfd = fd; if (sfd != fd) { EMSG("Emulating more than 1 RPMB partition is not supported"); return NULL; } return &rpmb_emu; } #if (DEBUGLEVEL >= TRACE_FLOW) static void dump_blocks(size_t startblk, size_t numblk, uint8_t *ptr, bool to_mmc) { char msg[100] = { 0 }; size_t i = 0; for (i = 0; i < numblk; i++) { snprintf(msg, sizeof(msg), "%s MMC block %zu", to_mmc ? "Write" : "Read", startblk + i); dump_buffer(msg, ptr, 256); ptr += 256; } } #else static void dump_blocks(size_t startblk, size_t numblk, uint8_t *ptr, bool to_mmc) { (void)startblk; (void)numblk; (void)ptr; (void)to_mmc; } #endif #define CUC(x) ((const unsigned char *)(x)) static void hmac_update_frm(hmac_sha256_ctx *ctx, struct rpmb_data_frame *frm) { hmac_sha256_update(ctx, CUC(frm->data), 256); hmac_sha256_update(ctx, CUC(frm->nonce), 16); hmac_sha256_update(ctx, CUC(&frm->write_counter), 4); hmac_sha256_update(ctx, CUC(&frm->address), 2); hmac_sha256_update(ctx, CUC(&frm->block_count), 2); hmac_sha256_update(ctx, CUC(&frm->op_result), 2); hmac_sha256_update(ctx, CUC(&frm->msg_type), 2); } static bool is_hmac_valid(struct rpmb_emu *mem, struct rpmb_data_frame *frm, size_t nfrm) { uint8_t mac[32] = { 0 }; size_t i = 0; hmac_sha256_ctx ctx; memset(&ctx, 0, sizeof(ctx)); if (!mem->key_set) { EMSG("Cannot check MAC (key not set)"); return false; } hmac_sha256_init(&ctx, mem->key, sizeof(mem->key)); for (i = 0; i < nfrm; i++, frm++) hmac_update_frm(&ctx, frm); frm--; hmac_sha256_final(&ctx, mac, 32); if (memcmp(mac, frm->key_mac, 32)) { EMSG("Invalid MAC"); return false; } return true; } static uint16_t gen_msb1st_result(uint8_t byte) { return (uint16_t)byte << 8; } static uint16_t compute_hmac(struct rpmb_emu *mem, struct rpmb_data_frame *frm, size_t nfrm) { size_t i = 0; hmac_sha256_ctx ctx; memset(&ctx, 0, sizeof(ctx)); if (!mem->key_set) { EMSG("Cannot compute MAC (key not set)"); return gen_msb1st_result(RPMB_RESULT_AUTH_KEY_NOT_PROGRAMMED); } hmac_sha256_init(&ctx, mem->key, sizeof(mem->key)); for (i = 0; i < nfrm; i++, frm++) hmac_update_frm(&ctx, frm); frm--; hmac_sha256_final(&ctx, frm->key_mac, 32); return gen_msb1st_result(RPMB_RESULT_OK); } static uint16_t ioctl_emu_mem_transfer(struct rpmb_emu *mem, struct rpmb_data_frame *frm, size_t nfrm, int to_mmc) { size_t start = mem->last_op.address * 256; size_t size = nfrm * 256; size_t i = 0; uint8_t *memptr = NULL; if (start > mem->size || start + size > mem->size) { EMSG("Transfer bounds exceeed emulated memory"); return gen_msb1st_result(RPMB_RESULT_ADDRESS_FAILURE); } if (to_mmc && !is_hmac_valid(mem, frm, nfrm)) return gen_msb1st_result(RPMB_RESULT_AUTH_FAILURE); DMSG("Transferring %zu 256-byte data block%s %s MMC (block offset=%zu)", nfrm, (nfrm > 1) ? "s" : "", to_mmc ? "to" : "from", start / 256); for (i = 0; i < nfrm; i++) { memptr = mem->buf + start + i * 256; if (to_mmc) { memcpy(memptr, frm[i].data, 256); mem->write_counter++; frm[i].write_counter = htonl(mem->write_counter); frm[i].msg_type = htons(RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE); } else { memcpy(frm[i].data, memptr, 256); frm[i].msg_type = htons(RPMB_MSG_TYPE_RESP_AUTH_DATA_READ); frm[i].address = htons(mem->last_op.address); frm[i].block_count = nfrm; memcpy(frm[i].nonce, mem->nonce, 16); } frm[i].op_result = gen_msb1st_result(RPMB_RESULT_OK); } dump_blocks(mem->last_op.address, nfrm, mem->buf + start, to_mmc); if (!to_mmc) compute_hmac(mem, frm, nfrm); return gen_msb1st_result(RPMB_RESULT_OK); } static void ioctl_emu_get_write_result(struct rpmb_emu *mem, struct rpmb_data_frame *frm) { frm->msg_type = htons(RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE); frm->op_result = mem->last_op.op_result; frm->address = htons(mem->last_op.address); frm->write_counter = htonl(mem->write_counter); compute_hmac(mem, frm, 1); } static uint16_t ioctl_emu_setkey(struct rpmb_emu *mem, struct rpmb_data_frame *frm) { if (mem->key_set) { EMSG("Key already set"); return gen_msb1st_result(RPMB_RESULT_GENERAL_FAILURE); } dump_buffer("Setting key", frm->key_mac, 32); memcpy(mem->key, frm->key_mac, 32); mem->key_set = true; return gen_msb1st_result(RPMB_RESULT_OK); } static void ioctl_emu_get_keyprog_result(struct rpmb_emu *mem, struct rpmb_data_frame *frm) { frm->msg_type = htons(RPMB_MSG_TYPE_RESP_AUTH_KEY_PROGRAM); frm->op_result = mem->last_op.op_result; } static void ioctl_emu_read_ctr(struct rpmb_emu *mem, struct rpmb_data_frame *frm) { DMSG("Reading counter"); frm->msg_type = htons(RPMB_MSG_TYPE_RESP_WRITE_COUNTER_VAL_READ); frm->write_counter = htonl(mem->write_counter); memcpy(frm->nonce, mem->nonce, 16); frm->op_result = compute_hmac(mem, frm, 1); } static uint32_t read_cid(uint16_t dev_id, uint8_t *cid) { /* Taken from an actual eMMC chip */ static const uint8_t test_cid[] = { /* MID (Manufacturer ID): Micron */ 0xfe, /* CBX (Device/BGA): BGA */ 0x01, /* OID (OEM/Application ID) */ 0x4e, /* PNM (Product name) "MMC04G" */ 0x4d, 0x4d, 0x43, 0x30, 0x34, 0x47, /* PRV (Product revision): 4.2 */ 0x42, /* PSN (Product serial number) */ 0xc8, 0xf6, 0x55, 0x2a, /* * MDT (Manufacturing date): * June, 2014 */ 0x61, /* (CRC7 (0xA) << 1) | 0x1 */ 0x15 }; (void)dev_id; memcpy(cid, test_cid, sizeof(test_cid)); return TEEC_SUCCESS; } static void ioctl_emu_set_ext_csd(uint8_t *ext_csd) { ext_csd[168] = EMU_RPMB_SIZE_MULT; ext_csd[222] = EMU_RPMB_REL_WR_SEC_C; } /* A crude emulation of the MMC ioc commands we need for RPMB */ static int ioctl_emu_cmd(int fd, struct mmc_ioc_cmd *cmd) { struct rpmb_data_frame *frm = NULL; uint16_t msg_type = 0; struct rpmb_emu *mem = mem_for_fd(fd); if (!mem) return -1; switch (cmd->opcode) { case MMC_SEND_EXT_CSD: ioctl_emu_set_ext_csd((uint8_t *)(uintptr_t)cmd->data_ptr); break; case MMC_WRITE_MULTIPLE_BLOCK: frm = (struct rpmb_data_frame *)(uintptr_t)cmd->data_ptr; msg_type = ntohs(frm->msg_type); switch (msg_type) { case RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM: mem->last_op.msg_type = msg_type; mem->last_op.op_result = ioctl_emu_setkey(mem, frm); break; case RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE: mem->last_op.msg_type = msg_type; mem->last_op.address = ntohs(frm->address); mem->last_op.op_result = ioctl_emu_mem_transfer(mem, frm, cmd->blocks, 1); break; case RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ: case RPMB_MSG_TYPE_REQ_AUTH_DATA_READ: memcpy(mem->nonce, frm->nonce, 16); mem->last_op.msg_type = msg_type; mem->last_op.address = ntohs(frm->address); break; default: break; } break; case MMC_READ_MULTIPLE_BLOCK: frm = (struct rpmb_data_frame *)(uintptr_t)cmd->data_ptr; msg_type = ntohs(frm->msg_type); switch (mem->last_op.msg_type) { case RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM: ioctl_emu_get_keyprog_result(mem, frm); break; case RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE: ioctl_emu_get_write_result(mem, frm); break; case RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ: ioctl_emu_read_ctr(mem, frm); break; case RPMB_MSG_TYPE_REQ_AUTH_DATA_READ: ioctl_emu_mem_transfer(mem, frm, cmd->blocks, 0); break; default: EMSG("Unexpected"); break; } break; default: EMSG("Unsupported ioctl opcode 0x%08x", cmd->opcode); return -1; } return 0; } static int ioctl_emu(int fd, unsigned long request, ...) { struct mmc_ioc_multi_cmd *mcmd = NULL; struct mmc_ioc_cmd *cmd = NULL; size_t i = 0; int res = 0; va_list ap; if (request == MMC_IOC_CMD) { va_start(ap, request); cmd = va_arg(ap, struct mmc_ioc_cmd *); va_end(ap); res = ioctl_emu_cmd(fd, cmd); } else if (request == MMC_IOC_MULTI_CMD) { va_start(ap, request); mcmd = va_arg(ap, struct mmc_ioc_multi_cmd *); va_end(ap); for (i = 0; i < mcmd->num_of_cmds; i++) { res = ioctl_emu_cmd(fd, &mcmd->cmds[i]); if (res) return res; } } else { EMSG("Unsupported ioctl: 0x%lx", request); return -1; } return res; } static int mmc_rpmb_fd(uint16_t dev_id) { (void)dev_id; /* Any value != -1 will do in test mode */ return 0; } static int mmc_fd(uint16_t dev_id) { (void)dev_id; return 0; } static void close_mmc_fd(int fd) { (void)fd; } #endif /* RPMB_EMU */ /* * Extended CSD Register is 512 bytes and defines device properties * and selected modes. */ static uint32_t read_ext_csd(int fd, uint8_t *ext_csd) { int st = 0; struct mmc_ioc_cmd cmd = { .blksz = 512, .blocks = 1, .flags = MMC_RSP_R1 | MMC_CMD_ADTC, .opcode = MMC_SEND_EXT_CSD, }; mmc_ioc_cmd_set_data(cmd, ext_csd); st = IOCTL(fd, MMC_IOC_CMD, &cmd); if (st < 0) return TEEC_ERROR_GENERIC; return TEEC_SUCCESS; } static inline void set_mmc_io_cmd(struct mmc_ioc_cmd *cmd, unsigned int blocks, __u32 opcode, int write_flag) { cmd->blksz = 512; cmd->blocks = blocks; cmd->flags = MMC_RSP_R1 | MMC_CMD_ADTC; cmd->opcode = opcode; cmd->write_flag = write_flag; } static uint32_t 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) { TEEC_Result res = TEEC_SUCCESS; int st = 0; size_t i = 0; uint16_t msg_type = ntohs(req_frm->msg_type); struct mmc_ioc_multi_cmd *mcmd = NULL; struct mmc_ioc_cmd *cmd = NULL; for (i = 1; i < req_nfrm; i++) { if (req_frm[i].msg_type != msg_type) { EMSG("All request frames shall be of the same type"); return TEEC_ERROR_BAD_PARAMETERS; } } DMSG("Req: %zu frame(s) of type 0x%04x", req_nfrm, msg_type); DMSG("Rsp: %zu frame(s)", rsp_nfrm); mcmd = (struct mmc_ioc_multi_cmd *) calloc(1, sizeof(struct mmc_ioc_multi_cmd) + RPMB_MAX_IOC_MULTI_CMDS * sizeof(struct mmc_ioc_cmd)); if (!mcmd) return TEEC_ERROR_OUT_OF_MEMORY; switch(msg_type) { case RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM: case RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE: if (rsp_nfrm != 1) { EMSG("Expected only one response frame"); res = TEEC_ERROR_BAD_PARAMETERS; goto out; } mcmd->num_of_cmds = 3; /* Send write request frame(s) */ cmd = &mcmd->cmds[0]; set_mmc_io_cmd(cmd, req_nfrm, MMC_WRITE_MULTIPLE_BLOCK, 1 | MMC_CMD23_ARG_REL_WR); /* * Black magic: tested on a HiKey board with a HardKernel eMMC * module. When postsleep values are zero, the kernel logs * random errors: "mmc_blk_ioctl_cmd: Card Status=0x00000E00" * and ioctl() fails. */ cmd->postsleep_min_us = 20000; cmd->postsleep_max_us = 50000; mmc_ioc_cmd_set_data((*cmd), (uintptr_t)req_frm); /* Send result request frame */ cmd = &mcmd->cmds[1]; set_mmc_io_cmd(cmd, req_nfrm, MMC_WRITE_MULTIPLE_BLOCK, 1); memset(rsp_frm, 0, 1); rsp_frm->msg_type = htons(RPMB_MSG_TYPE_REQ_RESULT_READ); mmc_ioc_cmd_set_data((*cmd), (uintptr_t)rsp_frm); /* Read response frame */ cmd = &mcmd->cmds[2]; set_mmc_io_cmd(cmd, rsp_nfrm, MMC_READ_MULTIPLE_BLOCK, 0); mmc_ioc_cmd_set_data((*cmd), (uintptr_t)rsp_frm); break; case RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ: if (rsp_nfrm != 1) { EMSG("Expected only one response frame"); res = TEEC_ERROR_BAD_PARAMETERS; goto out; } #if __GNUC__ > 6 __attribute__((fallthrough)); #endif case RPMB_MSG_TYPE_REQ_AUTH_DATA_READ: if (req_nfrm != 1) { EMSG("Expected only one request frame"); res = TEEC_ERROR_BAD_PARAMETERS; goto out; } mcmd->num_of_cmds = 2; /* Send request frame */ cmd = &mcmd->cmds[0]; set_mmc_io_cmd(cmd, req_nfrm, MMC_WRITE_MULTIPLE_BLOCK, 1); mmc_ioc_cmd_set_data((*cmd), (uintptr_t)req_frm); /* Read response frames */ cmd = &mcmd->cmds[1]; set_mmc_io_cmd(cmd, rsp_nfrm, MMC_READ_MULTIPLE_BLOCK, 0); mmc_ioc_cmd_set_data((*cmd), (uintptr_t)rsp_frm); break; default: EMSG("Unsupported message type: %d", msg_type); res = TEEC_ERROR_GENERIC; goto out; } st = IOCTL(fd, MMC_IOC_MULTI_CMD, mcmd); if (st < 0) res = TEEC_ERROR_GENERIC; out: free(mcmd); return res; } static uint32_t rpmb_get_dev_info(uint16_t dev_id, struct rpmb_dev_info *info) { int fd = 0; uint32_t res = 0; uint8_t ext_csd[512] = { 0 }; res = read_cid(dev_id, info->cid); if (res != TEEC_SUCCESS) return res; fd = mmc_fd(dev_id); if (fd < 0) return TEEC_ERROR_BAD_PARAMETERS; res = read_ext_csd(fd, ext_csd); if (res != TEEC_SUCCESS) goto err; info->rel_wr_sec_c = ext_csd[222]; info->rpmb_size_mult = ext_csd[168]; info->ret_code = RPMB_CMD_GET_DEV_INFO_RET_OK; err: close_mmc_fd(fd); return res; } /* * req is one struct rpmb_req followed by one or more struct rpmb_data_frame * rsp is either one struct rpmb_dev_info or one or more struct rpmb_data_frame */ static uint32_t rpmb_process_request_unlocked(void *req, size_t req_size, void *rsp, size_t rsp_size) { struct rpmb_req *sreq = req; size_t req_nfrm = 0; size_t rsp_nfrm = 0; uint32_t res = 0; int fd = 0; if (req_size < sizeof(*sreq)) return TEEC_ERROR_BAD_PARAMETERS; switch (sreq->cmd) { case RPMB_CMD_DATA_REQ: req_nfrm = (req_size - sizeof(struct rpmb_req)) / 512; rsp_nfrm = rsp_size / 512; fd = mmc_rpmb_fd(sreq->dev_id); if (fd < 0) return TEEC_ERROR_BAD_PARAMETERS; res = rpmb_data_req(fd, RPMB_REQ_DATA(req), req_nfrm, rsp, rsp_nfrm); break; case RPMB_CMD_GET_DEV_INFO: if (req_size != sizeof(struct rpmb_req) || rsp_size != sizeof(struct rpmb_dev_info)) { EMSG("Invalid req/rsp size"); return TEEC_ERROR_BAD_PARAMETERS; } res = rpmb_get_dev_info(sreq->dev_id, (struct rpmb_dev_info *)rsp); break; default: EMSG("Unsupported RPMB command: %d", sreq->cmd); res = TEEC_ERROR_BAD_PARAMETERS; break; } return res; } uint32_t rpmb_process_request(void *req, size_t req_size, void *rsp, size_t rsp_size) { uint32_t res = 0; tee_supp_mutex_lock(&rpmb_mutex); res = rpmb_process_request_unlocked(req, req_size, rsp, rsp_size); tee_supp_mutex_unlock(&rpmb_mutex); return res; }