core/tee/tee_rpmb_fs.c

   1 /*
   2  * Copyright (c) 2014, STMicroelectronics International N.V.
   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 <assert.h>
  29 #include <kernel/tee_common.h>
  30 #include <kernel/mutex.h>
  31 #include <kernel/panic.h>
  32 #include <kernel/tee_common_otp.h>
  33 #include <kernel/thread.h>
  34 #include <mm/core_memprot.h>
  35 #include <mm/tee_mm.h>
  36 #include <optee_msg_supplicant.h>
  37 #include <stdlib.h>
  38 #include <string.h>
  39 #include <string_ext.h>
  40 #include <sys/queue.h>
  41 #include <tee/tee_cryp_provider.h>
  42 #include <tee/tee_fs_defs.h>
  43 #include <tee/tee_fs.h>
  44 #include <tee/tee_fs_key_manager.h>
  45 #include <trace.h>
  46 #include <util.h>
  47
  48 #define RPMB_STORAGE_START_ADDRESS      0
  49 #define RPMB_FS_FAT_START_ADDRESS       512
  50 #define RPMB_BLOCK_SIZE_SHIFT           8
  51
  52 #define RPMB_FS_MAGIC                   0x52504D42
  53 #define FS_VERSION                      2
  54 #define N_ENTRIES                       8
  55
  56 #define FILE_IS_ACTIVE                  (1u << 0)
  57 #define FILE_IS_LAST_ENTRY              (1u << 1)
  58
  59 #define TEE_RPMB_FS_FILENAME_LENGTH 224
  60
  61 /**
  62  * FS parameters: Information often used by internal functions.
  63  * fat_start_address will be set by rpmb_fs_setup().
  64  * rpmb_fs_parameters can be read by any other function.
  65  */
  66 struct rpmb_fs_parameters {
  67         uint32_t fat_start_address;
  68         uint32_t max_rpmb_address;
  69 };
  70
  71 /**
  72  * File entry for a single file in a RPMB_FS partition.
  73  */
  74 struct rpmb_fat_entry {
  75         uint32_t start_address;
  76         uint32_t data_size;
  77         uint32_t flags;
  78         uint32_t write_counter;
  79         uint8_t fek[TEE_FS_KM_FEK_SIZE];
  80         char filename[TEE_RPMB_FS_FILENAME_LENGTH];
  81 };
  82
  83 /**
  84  * FAT entry context with reference to a FAT entry and its
  85  * location in RPMB.
  86  */
  87 struct rpmb_file_handle {
  88         struct rpmb_fat_entry fat_entry;
  89         char filename[TEE_RPMB_FS_FILENAME_LENGTH];
  90         /* Address for current entry in RPMB */
  91         uint32_t rpmb_fat_address;
  92         /* Current position */
  93         uint32_t pos;
  94 };
  95
  96 /**
  97  * RPMB_FS partition data
  98  */
  99 struct rpmb_fs_partition {
 100         uint32_t rpmb_fs_magic;
 101         uint32_t fs_version;
 102         uint32_t write_counter;
 103         uint32_t fat_start_address;
 104         /* Do not use reserved[] for other purpose than partition data. */
 105         uint8_t reserved[112];
 106 };
 107
 108 /**
 109  * A node in a list of directory entries. entry->name is a
 110  * pointer to name here.
 111  */
 112 struct tee_rpmb_fs_dirent {
 113         struct tee_fs_dirent entry;
 114         char name[TEE_RPMB_FS_FILENAME_LENGTH];
 115         /* */
 116         SIMPLEQ_ENTRY(tee_rpmb_fs_dirent) link;
 117 };
 118
 119 /**
 120  * The RPMB directory representation. It contains a queue of
 121  * RPMB directory entries: 'next'.
 122  * The current pointer points to the last directory entry
 123  * returned by readdir().
 124  */
 125 struct tee_fs_dir {
 126         struct tee_rpmb_fs_dirent *current;
 127         /* */
 128         SIMPLEQ_HEAD(next_head, tee_rpmb_fs_dirent) next;
 129 };
 130
 131 static struct rpmb_fs_parameters *fs_par;
 132
 133 /*
 134  * Lower interface to RPMB device
 135  */
 136
 137 #define RPMB_DATA_OFFSET            (RPMB_STUFF_DATA_SIZE + RPMB_KEY_MAC_SIZE)
 138 #define RPMB_MAC_PROTECT_DATA_SIZE  (RPMB_DATA_FRAME_SIZE - RPMB_DATA_OFFSET)
 139
 140 #define RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM          0x0001
 141 #define RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ    0x0002
 142 #define RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE           0x0003
 143 #define RPMB_MSG_TYPE_REQ_AUTH_DATA_READ            0x0004
 144 #define RPMB_MSG_TYPE_REQ_RESULT_READ               0x0005
 145 #define RPMB_MSG_TYPE_RESP_AUTH_KEY_PROGRAM         0x0100
 146 #define RPMB_MSG_TYPE_RESP_WRITE_COUNTER_VAL_READ   0x0200
 147 #define RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE          0x0300
 148 #define RPMB_MSG_TYPE_RESP_AUTH_DATA_READ           0x0400
 149
 150 #define RPMB_STUFF_DATA_SIZE                        196
 151 #define RPMB_KEY_MAC_SIZE                           32
 152 #define RPMB_DATA_SIZE                              256
 153 #define RPMB_NONCE_SIZE                             16
 154 #define RPMB_DATA_FRAME_SIZE                        512
 155
 156 #define RPMB_RESULT_OK                              0x00
 157 #define RPMB_RESULT_GENERAL_FAILURE                 0x01
 158 #define RPMB_RESULT_AUTH_FAILURE                    0x02
 159 #define RPMB_RESULT_COUNTER_FAILURE                 0x03
 160 #define RPMB_RESULT_ADDRESS_FAILURE                 0x04
 161 #define RPMB_RESULT_WRITE_FAILURE                   0x05
 162 #define RPMB_RESULT_READ_FAILURE                    0x06
 163 #define RPMB_RESULT_AUTH_KEY_NOT_PROGRAMMED         0x07
 164 #define RPMB_RESULT_MASK                            0x3F
 165 #define RPMB_RESULT_WR_CNT_EXPIRED                  0x80
 166
 167 /* RPMB internal commands */
 168 #define RPMB_CMD_DATA_REQ      0x00
 169 #define RPMB_CMD_GET_DEV_INFO  0x01
 170
 171 #define RPMB_SIZE_SINGLE (128 * 1024)
 172
 173 /* Error codes for get_dev_info request/response. */
 174 #define RPMB_CMD_GET_DEV_INFO_RET_OK     0x00
 175 #define RPMB_CMD_GET_DEV_INFO_RET_ERROR  0x01
 176
 177 struct rpmb_data_frame {
 178         uint8_t stuff_bytes[RPMB_STUFF_DATA_SIZE];
 179         uint8_t key_mac[RPMB_KEY_MAC_SIZE];
 180         uint8_t data[RPMB_DATA_SIZE];
 181         uint8_t nonce[RPMB_NONCE_SIZE];
 182         uint8_t write_counter[4];
 183         uint8_t address[2];
 184         uint8_t block_count[2];
 185         uint8_t op_result[2];
 186         uint8_t msg_type[2];
 187 };
 188
 189 struct rpmb_req {
 190         uint16_t cmd;
 191         uint16_t dev_id;
 192         uint16_t block_count;
 193         /* variable length of data */
 194         /* uint8_t data[]; REMOVED! */
 195 };
 196
 197 #define TEE_RPMB_REQ_DATA(req) \
 198                 ((void *)((struct rpmb_req *)(req) + 1))
 199
 200 struct rpmb_raw_data {
 201         uint16_t msg_type;
 202         uint16_t *op_result;
 203         uint16_t *block_count;
 204         uint16_t *blk_idx;
 205         uint32_t *write_counter;
 206         uint8_t *nonce;
 207         uint8_t *key_mac;
 208         uint8_t *data;
 209         /* data length to read or write */
 210         uint32_t len;
 211         /* Byte address offset in the first block involved */
 212         uint8_t byte_offset;
 213 };
 214
 215 #define RPMB_EMMC_CID_SIZE 16
 216 struct rpmb_dev_info {
 217         uint8_t cid[RPMB_EMMC_CID_SIZE];
 218         /* EXT CSD-slice 168 "RPMB Size" */
 219         uint8_t rpmb_size_mult;
 220         /* EXT CSD-slice 222 "Reliable Write Sector Count" */
 221         uint8_t rel_wr_sec_c;
 222         /* Check the ret code and accept the data only if it is OK. */
 223         uint8_t ret_code;
 224 };
 225
 226 /*
 227  * Struct for rpmb context data.
 228  *
 229  * @key              RPMB key.
 230  * @cid              eMMC card ID.
 231  * @hash_ctx_size    Hash context size
 232  * @wr_cnt           Current write counter.
 233  * @max_blk_idx      The highest block index supported by current device.
 234  * @rel_wr_blkcnt    Max number of data blocks for each reliable write.
 235  * @dev_id           Device ID of the eMMC device.
 236  * @wr_cnt_synced    Flag indicating if write counter is synced to RPMB.
 237  * @key_derived      Flag indicating if key has been generated.
 238  * @key_verified     Flag indicating the key generated is verified ok.
 239  * @dev_info_synced  Flag indicating if dev info has been retrieved from RPMB.
 240  */
 241 struct tee_rpmb_ctx {
 242         uint8_t key[RPMB_KEY_MAC_SIZE];
 243         uint8_t cid[RPMB_EMMC_CID_SIZE];
 244         size_t hash_ctx_size;
 245         uint32_t wr_cnt;
 246         uint16_t max_blk_idx;
 247         uint16_t rel_wr_blkcnt;
 248         uint16_t dev_id;
 249         bool wr_cnt_synced;
 250         bool key_derived;
 251         bool key_verified;
 252         bool dev_info_synced;
 253 };
 254
 255 static struct tee_rpmb_ctx *rpmb_ctx;
 256
 257 /*
 258  * Mutex to serialize the operations exported by this file.
 259  * It protects rpmb_ctx and prevents overlapping operations on eMMC devices with
 260  * different IDs.
 261  */
 262 static struct mutex rpmb_mutex = MUTEX_INITIALIZER;
 263
 264 #ifdef CFG_RPMB_TESTKEY
 265
 266 static const uint8_t rpmb_test_key[RPMB_KEY_MAC_SIZE] = {
 267         0xD3, 0xEB, 0x3E, 0xC3, 0x6E, 0x33, 0x4C, 0x9F,
 268         0x98, 0x8C, 0xE2, 0xC0, 0xB8, 0x59, 0x54, 0x61,
 269         0x0D, 0x2B, 0xCF, 0x86, 0x64, 0x84, 0x4D, 0xF2,
 270         0xAB, 0x56, 0xE6, 0xC6, 0x1B, 0xB7, 0x01, 0xE4
 271 };
 272
 273 static TEE_Result tee_rpmb_key_gen(uint16_t dev_id __unused,
 274                                    uint8_t *key, uint32_t len)
 275 {
 276         TEE_Result res = TEE_SUCCESS;
 277
 278         if (!key || RPMB_KEY_MAC_SIZE != len) {
 279                 res = TEE_ERROR_BAD_PARAMETERS;
 280                 goto out;
 281         }
 282
 283         DMSG("RPMB: Using test key");
 284         memcpy(key, rpmb_test_key, RPMB_KEY_MAC_SIZE);
 285
 286 out:
 287         return res;
 288 }
 289
 290 #else /* !CFG_RPMB_TESTKEY */
 291
 292 /*
 293  * NOTE: We need a common API to get hw unique key and it
 294  * should return error when the hw unique is not a valid
 295  * one as stated below.
 296  * We need to make sure the hw unique we get is valid by:
 297  * 1. In case of HUK is used, checking if OTP is hidden (in
 298  *    which case only zeros will be returned) or not;
 299  * 2. In case of SSK is used, checking if SSK in OTP is
 300  *    write_locked (which means a valid key is provisioned)
 301  *    or not.
 302  *
 303  * Maybe tee_get_hw_unique_key() should be exposed as
 304  * generic API for getting hw unique key!
 305  * We should change the API tee_otp_get_hw_unique_key()
 306  * to return error code!
 307  */
 308 static TEE_Result tee_get_hw_unique_key(struct tee_hw_unique_key *hwkey)
 309 {
 310         if (!hwkey)
 311                 return TEE_ERROR_BAD_PARAMETERS;
 312
 313         tee_otp_get_hw_unique_key(hwkey);
 314
 315         return TEE_SUCCESS;
 316 }
 317
 318 static TEE_Result tee_rpmb_key_gen(uint16_t dev_id __unused,
 319                                    uint8_t *key, uint32_t len)
 320 {
 321         TEE_Result res;
 322         struct tee_hw_unique_key hwkey;
 323         uint8_t *ctx = NULL;
 324
 325         if (!key || RPMB_KEY_MAC_SIZE != len) {
 326                 res = TEE_ERROR_BAD_PARAMETERS;
 327                 goto out;
 328         }
 329
 330         IMSG("RPMB: Using generated key");
 331         res = tee_get_hw_unique_key(&hwkey);
 332         if (res != TEE_SUCCESS)
 333                 goto out;
 334
 335         ctx = malloc(rpmb_ctx->hash_ctx_size);
 336         if (!ctx) {
 337                 res = TEE_ERROR_OUT_OF_MEMORY;
 338                 goto out;
 339         }
 340
 341         res = crypto_ops.mac.init(ctx, TEE_ALG_HMAC_SHA256, hwkey.data,
 342                                   HW_UNIQUE_KEY_LENGTH);
 343         if (res != TEE_SUCCESS)
 344                 goto out;
 345
 346         res = crypto_ops.mac.update(ctx, TEE_ALG_HMAC_SHA256,
 347                                     (uint8_t *)rpmb_ctx->cid,
 348                                     RPMB_EMMC_CID_SIZE);
 349         if (res != TEE_SUCCESS)
 350                 goto out;
 351
 352         res = crypto_ops.mac.final(ctx, TEE_ALG_HMAC_SHA256, key, len);
 353
 354 out:
 355         free(ctx);
 356         return res;
 357 }
 358
 359 #endif /* !CFG_RPMB_TESTKEY */
 360
 361 static void u32_to_bytes(uint32_t u32, uint8_t *bytes)
 362 {
 363         *bytes = (uint8_t) (u32 >> 24);
 364         *(bytes + 1) = (uint8_t) (u32 >> 16);
 365         *(bytes + 2) = (uint8_t) (u32 >> 8);
 366         *(bytes + 3) = (uint8_t) u32;
 367 }
 368
 369 static void bytes_to_u32(uint8_t *bytes, uint32_t *u32)
 370 {
 371         *u32 = (uint32_t) ((*(bytes) << 24) +
 372                            (*(bytes + 1) << 16) +
 373                            (*(bytes + 2) << 8) + (*(bytes + 3)));
 374 }
 375
 376 static void u16_to_bytes(uint16_t u16, uint8_t *bytes)
 377 {
 378         *bytes = (uint8_t) (u16 >> 8);
 379         *(bytes + 1) = (uint8_t) u16;
 380 }
 381
 382 static void bytes_to_u16(uint8_t *bytes, uint16_t *u16)
 383 {
 384         *u16 = (uint16_t) ((*bytes << 8) + *(bytes + 1));
 385 }
 386
 387 static TEE_Result tee_rpmb_mac_calc(uint8_t *mac, uint32_t macsize,
 388                                     uint8_t *key, uint32_t keysize,
 389                                     struct rpmb_data_frame *datafrms,
 390                                     uint16_t blkcnt)
 391 {
 392         TEE_Result res = TEE_ERROR_GENERIC;
 393         int i;
 394         uint8_t *ctx = NULL;
 395
 396         if (!mac || !key || !datafrms)
 397                 return TEE_ERROR_BAD_PARAMETERS;
 398
 399         ctx = malloc(rpmb_ctx->hash_ctx_size);
 400         if (!ctx)
 401                 return TEE_ERROR_OUT_OF_MEMORY;
 402
 403         res = crypto_ops.mac.init(ctx, TEE_ALG_HMAC_SHA256, key, keysize);
 404         if (res != TEE_SUCCESS)
 405                 goto func_exit;
 406
 407         for (i = 0; i < blkcnt; i++) {
 408                 res = crypto_ops.mac.update(ctx, TEE_ALG_HMAC_SHA256,
 409                                           datafrms[i].data,
 410                                           RPMB_MAC_PROTECT_DATA_SIZE);
 411                 if (res != TEE_SUCCESS)
 412                         goto func_exit;
 413         }
 414
 415         res = crypto_ops.mac.final(ctx, TEE_ALG_HMAC_SHA256, mac, macsize);
 416         if (res != TEE_SUCCESS)
 417                 goto func_exit;
 418
 419         res = TEE_SUCCESS;
 420
 421 func_exit:
 422         free(ctx);
 423         return res;
 424 }
 425
 426 struct tee_rpmb_mem {
 427         paddr_t phreq;
 428         uint64_t phreq_cookie;
 429         paddr_t phresp;
 430         uint64_t phresp_cookie;
 431         size_t req_size;
 432         size_t resp_size;
 433 };
 434
 435 static void tee_rpmb_free(struct tee_rpmb_mem *mem)
 436 {
 437         if (!mem)
 438                 return;
 439
 440         if (mem->phreq) {
 441                 thread_rpc_free_payload(mem->phreq_cookie);
 442                 mem->phreq_cookie = 0;
 443                 mem->phreq = 0;
 444         }
 445         if (mem->phresp) {
 446                 thread_rpc_free_payload(mem->phresp_cookie);
 447                 mem->phresp_cookie = 0;
 448                 mem->phresp = 0;
 449         }
 450 }
 451
 452
 453 static TEE_Result tee_rpmb_alloc(size_t req_size, size_t resp_size,
 454                 struct tee_rpmb_mem *mem, void **req, void **resp)
 455 {
 456         TEE_Result res = TEE_SUCCESS;
 457         size_t req_s = ROUNDUP(req_size, sizeof(uint32_t));
 458         size_t resp_s = ROUNDUP(resp_size, sizeof(uint32_t));
 459
 460         if (!mem)
 461                 return TEE_ERROR_BAD_PARAMETERS;
 462
 463         memset(mem, 0, sizeof(*mem));
 464         thread_rpc_alloc_payload(req_s, &mem->phreq, &mem->phreq_cookie);
 465         thread_rpc_alloc_payload(resp_s, &mem->phresp, &mem->phresp_cookie);
 466         if (!mem->phreq || !mem->phresp) {
 467                 res = TEE_ERROR_OUT_OF_MEMORY;
 468                 goto out;
 469         }
 470
 471         *req = phys_to_virt(mem->phreq, MEM_AREA_NSEC_SHM);
 472         *resp = phys_to_virt(mem->phresp, MEM_AREA_NSEC_SHM);
 473         if (!*req || !*resp) {
 474                 res = TEE_ERROR_GENERIC;
 475                 goto out;
 476         }
 477
 478         mem->req_size = req_size;
 479         mem->resp_size = resp_size;
 480
 481 out:
 482         if (res != TEE_SUCCESS)
 483                 tee_rpmb_free(mem);
 484         return res;
 485 }
 486
 487 static TEE_Result tee_rpmb_invoke(struct tee_rpmb_mem *mem)
 488 {
 489         struct optee_msg_param params[2];
 490
 491         memset(params, 0, sizeof(params));
 492         params[0].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
 493         params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT;
 494         params[0].u.tmem.buf_ptr = mem->phreq;
 495         params[0].u.tmem.size = mem->req_size;
 496         params[0].u.tmem.shm_ref = mem->phreq_cookie;
 497         params[1].u.tmem.buf_ptr = mem->phresp;
 498         params[1].u.tmem.size = mem->resp_size;
 499         params[1].u.tmem.shm_ref = mem->phresp_cookie;
 500
 501         return thread_rpc_cmd(OPTEE_MSG_RPC_CMD_RPMB, 2, params);
 502 }
 503
 504 static bool is_zero(const uint8_t *buf, size_t size)
 505 {
 506         size_t i;
 507
 508         for (i = 0; i < size; i++)
 509                 if (buf[i])
 510                         return false;
 511         return true;
 512 }
 513
 514 static TEE_Result encrypt_block(uint8_t *out, const uint8_t *in,
 515                                 uint16_t blk_idx, const uint8_t *fek)
 516 {
 517         return tee_fs_crypt_block(out, in, RPMB_DATA_SIZE, blk_idx, fek,
 518                                   TEE_MODE_ENCRYPT);
 519 }
 520
 521 static TEE_Result decrypt_block(uint8_t *out, const uint8_t *in,
 522                                 uint16_t blk_idx, const uint8_t *fek)
 523 {
 524         return tee_fs_crypt_block(out, in, RPMB_DATA_SIZE, blk_idx, fek,
 525                                   TEE_MODE_DECRYPT);
 526 }
 527
 528 /* Decrypt/copy at most one block of data */
 529 static TEE_Result decrypt(uint8_t *out, const struct rpmb_data_frame *frm,
 530                           size_t size, size_t offset,
 531                           uint16_t blk_idx __maybe_unused, const uint8_t *fek)
 532 {
 533         uint8_t *tmp __maybe_unused;
 534
 535
 536         if ((size + offset < size) || (size + offset > RPMB_DATA_SIZE))
 537                 panic("invalid size or offset");
 538
 539         if (!fek) {
 540                 /* Block is not encrypted (not a file data block) */
 541                 memcpy(out, frm->data + offset, size);
 542         } else if (is_zero(fek, TEE_FS_KM_FEK_SIZE)) {
 543                 /* The file was created with encryption disabled */
 544                 return TEE_ERROR_SECURITY;
 545         } else {
 546                 /* Block is encrypted */
 547                 if (size < RPMB_DATA_SIZE) {
 548                         /*
 549                          * Since output buffer is not large enough to hold one
 550                          * block we must allocate a temporary buffer.
 551                          */
 552                         tmp = malloc(RPMB_DATA_SIZE);
 553                         if (!tmp)
 554                                 return TEE_ERROR_OUT_OF_MEMORY;
 555                         decrypt_block(tmp, frm->data, blk_idx, fek);
 556                         memcpy(out, tmp + offset, size);
 557                         free(tmp);
 558                 } else {
 559                         decrypt_block(out, frm->data, blk_idx, fek);
 560                 }
 561         }
 562
 563         return TEE_SUCCESS;
 564 }
 565
 566 static TEE_Result tee_rpmb_req_pack(struct rpmb_req *req,
 567                                     struct rpmb_raw_data *rawdata,
 568                                     uint16_t nbr_frms, uint16_t dev_id,
 569                                     const uint8_t *fek)
 570 {
 571         TEE_Result res = TEE_ERROR_GENERIC;
 572         int i;
 573         struct rpmb_data_frame *datafrm;
 574
 575         if (!req || !rawdata || !nbr_frms)
 576                 return TEE_ERROR_BAD_PARAMETERS;
 577
 578         /*
 579          * Check write blockcount is not bigger than reliable write
 580          * blockcount.
 581          */
 582         if ((rawdata->msg_type == RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE) &&
 583             (nbr_frms > rpmb_ctx->rel_wr_blkcnt)) {
 584                 DMSG("wr_blkcnt(%d) > rel_wr_blkcnt(%d)", nbr_frms,
 585                      rpmb_ctx->rel_wr_blkcnt);
 586                 return TEE_ERROR_GENERIC;
 587         }
 588
 589         req->cmd = RPMB_CMD_DATA_REQ;
 590         req->dev_id = dev_id;
 591
 592         /* Allocate memory for construct all data packets and calculate MAC. */
 593         datafrm = calloc(nbr_frms, RPMB_DATA_FRAME_SIZE);
 594         if (!datafrm)
 595                 return TEE_ERROR_OUT_OF_MEMORY;
 596
 597         for (i = 0; i < nbr_frms; i++) {
 598                 u16_to_bytes(rawdata->msg_type, datafrm[i].msg_type);
 599
 600                 if (rawdata->block_count)
 601                         u16_to_bytes(*rawdata->block_count,
 602                                      datafrm[i].block_count);
 603
 604                 if (rawdata->blk_idx) {
 605                         /* Check the block index is within range. */
 606                         if ((*rawdata->blk_idx + nbr_frms) >
 607                             rpmb_ctx->max_blk_idx) {
 608                                 res = TEE_ERROR_GENERIC;
 609                                 goto func_exit;
 610                         }
 611                         u16_to_bytes(*rawdata->blk_idx, datafrm[i].address);
 612                 }
 613
 614                 if (rawdata->write_counter)
 615                         u32_to_bytes(*rawdata->write_counter,
 616                                      datafrm[i].write_counter);
 617
 618                 if (rawdata->nonce)
 619                         memcpy(datafrm[i].nonce, rawdata->nonce,
 620                                RPMB_NONCE_SIZE);
 621
 622                 if (rawdata->data) {
 623                         if (fek)
 624                                 encrypt_block(datafrm[i].data,
 625                                         rawdata->data + (i * RPMB_DATA_SIZE),
 626                                         *rawdata->blk_idx + i, fek);
 627                         else
 628                                 memcpy(datafrm[i].data,
 629                                        rawdata->data + (i * RPMB_DATA_SIZE),
 630                                        RPMB_DATA_SIZE);
 631                 }
 632         }
 633
 634         if (rawdata->key_mac) {
 635                 if (rawdata->msg_type == RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE) {
 636                         res =
 637                             tee_rpmb_mac_calc(rawdata->key_mac,
 638                                               RPMB_KEY_MAC_SIZE, rpmb_ctx->key,
 639                                               RPMB_KEY_MAC_SIZE, datafrm,
 640                                               nbr_frms);
 641                         if (res != TEE_SUCCESS)
 642                                 goto func_exit;
 643                 }
 644                 memcpy(datafrm[nbr_frms - 1].key_mac,
 645                        rawdata->key_mac, RPMB_KEY_MAC_SIZE);
 646         }
 647
 648         memcpy(TEE_RPMB_REQ_DATA(req), datafrm,
 649                nbr_frms * RPMB_DATA_FRAME_SIZE);
 650
 651 #ifdef CFG_RPMB_FS_DEBUG_DATA
 652         for (i = 0; i < nbr_frms; i++) {
 653                 DMSG("Dumping data frame %d:", i);
 654                 DHEXDUMP((uint8_t *)&datafrm[i] + RPMB_STUFF_DATA_SIZE,
 655                          512 - RPMB_STUFF_DATA_SIZE);
 656         }
 657 #endif
 658
 659         res = TEE_SUCCESS;
 660 func_exit:
 661         free(datafrm);
 662         return res;
 663 }
 664
 665 static TEE_Result data_cpy_mac_calc_1b(struct rpmb_raw_data *rawdata,
 666                                        struct rpmb_data_frame *frm,
 667                                        uint8_t *fek)
 668 {
 669         TEE_Result res;
 670         uint8_t *data;
 671         uint16_t idx;
 672
 673         if (rawdata->len + rawdata->byte_offset > RPMB_DATA_SIZE)
 674                 return TEE_ERROR_BAD_PARAMETERS;
 675
 676         res = tee_rpmb_mac_calc(rawdata->key_mac, RPMB_KEY_MAC_SIZE,
 677                                 rpmb_ctx->key, RPMB_KEY_MAC_SIZE, frm, 1);
 678         if (res != TEE_SUCCESS)
 679                 return res;
 680
 681         data = rawdata->data;
 682         bytes_to_u16(frm->address, &idx);
 683
 684         res = decrypt(data, frm, rawdata->len, rawdata->byte_offset, idx, fek);
 685         return res;
 686 }
 687
 688 static TEE_Result tee_rpmb_data_cpy_mac_calc(struct rpmb_data_frame *datafrm,
 689                                              struct rpmb_raw_data *rawdata,
 690                                              uint16_t nbr_frms,
 691                                              struct rpmb_data_frame *lastfrm,
 692                                              uint8_t *fek)
 693 {
 694         TEE_Result res = TEE_ERROR_GENERIC;
 695         int i;
 696         uint8_t *ctx = NULL;
 697         uint16_t offset;
 698         uint32_t size;
 699         uint8_t *data;
 700         uint16_t start_idx;
 701         struct rpmb_data_frame localfrm;
 702
 703         if (!datafrm || !rawdata || !nbr_frms || !lastfrm)
 704                 return TEE_ERROR_BAD_PARAMETERS;
 705
 706         if (nbr_frms == 1)
 707                 return data_cpy_mac_calc_1b(rawdata, lastfrm, fek);
 708
 709         /* nbr_frms > 1 */
 710
 711         data = rawdata->data;
 712
 713         ctx = malloc(rpmb_ctx->hash_ctx_size);
 714         if (!ctx) {
 715                 res = TEE_ERROR_OUT_OF_MEMORY;
 716                 goto func_exit;
 717         }
 718
 719         res = crypto_ops.mac.init(ctx, TEE_ALG_HMAC_SHA256, rpmb_ctx->key,
 720                                   RPMB_KEY_MAC_SIZE);
 721         if (res != TEE_SUCCESS)
 722                 goto func_exit;
 723
 724         /*
 725          * Note: JEDEC JESD84-B51: "In every packet the address is the start
 726          * address of the full access (not address of the individual half a
 727          * sector)"
 728          */
 729         bytes_to_u16(lastfrm->address, &start_idx);
 730
 731         for (i = 0; i < (nbr_frms - 1); i++) {
 732
 733                 /*
 734                  * By working on a local copy of the RPMB frame, we ensure that
 735                  * the data can not be modified after the MAC is computed but
 736                  * before the payload is decrypted/copied to the output buffer.
 737                  */
 738                 memcpy(&localfrm, &datafrm[i], RPMB_DATA_FRAME_SIZE);
 739
 740                 res = crypto_ops.mac.update(ctx, TEE_ALG_HMAC_SHA256,
 741                                             localfrm.data,
 742                                             RPMB_MAC_PROTECT_DATA_SIZE);
 743                 if (res != TEE_SUCCESS)
 744                         goto func_exit;
 745
 746                 if (i == 0) {
 747                         /* First block */
 748                         offset = rawdata->byte_offset;
 749                         size = RPMB_DATA_SIZE - offset;
 750                 } else {
 751                         /* Middle blocks */
 752                         size = RPMB_DATA_SIZE;
 753                         offset = 0;
 754                 }
 755
 756                 res = decrypt(data, &localfrm, size, offset, start_idx + i,
 757                               fek);
 758                 if (res != TEE_SUCCESS)
 759                         goto func_exit;
 760
 761                 data += size;
 762         }
 763
 764         /* Last block */
 765         size = (rawdata->len + rawdata->byte_offset) % RPMB_DATA_SIZE;
 766         if (size == 0)
 767                 size = RPMB_DATA_SIZE;
 768         res = decrypt(data, lastfrm, size, 0, start_idx + nbr_frms - 1, fek);
 769         if (res != TEE_SUCCESS)
 770                 goto func_exit;
 771
 772         /* Update MAC against the last block */
 773         res = crypto_ops.mac.update(ctx, TEE_ALG_HMAC_SHA256, lastfrm->data,
 774                                     RPMB_MAC_PROTECT_DATA_SIZE);
 775         if (res != TEE_SUCCESS)
 776                 goto func_exit;
 777
 778         res = crypto_ops.mac.final(ctx, TEE_ALG_HMAC_SHA256, rawdata->key_mac,
 779                                    RPMB_KEY_MAC_SIZE);
 780         if (res != TEE_SUCCESS)
 781                 goto func_exit;
 782
 783         res = TEE_SUCCESS;
 784
 785 func_exit:
 786         free(ctx);
 787         return res;
 788 }
 789
 790 static TEE_Result tee_rpmb_resp_unpack_verify(struct rpmb_data_frame *datafrm,
 791                                               struct rpmb_raw_data *rawdata,
 792                                               uint16_t nbr_frms, uint8_t *fek)
 793 {
 794         TEE_Result res = TEE_ERROR_GENERIC;
 795         uint16_t msg_type;
 796         uint32_t wr_cnt;
 797         uint16_t blk_idx;
 798         uint16_t op_result;
 799         struct rpmb_data_frame lastfrm;
 800
 801         if (!datafrm || !rawdata || !nbr_frms)
 802                 return TEE_ERROR_BAD_PARAMETERS;
 803
 804 #ifdef CFG_RPMB_FS_DEBUG_DATA
 805         for (uint32_t i = 0; i < nbr_frms; i++) {
 806                 DMSG("Dumping data frame %d:", i);
 807                 DHEXDUMP((uint8_t *)&datafrm[i] + RPMB_STUFF_DATA_SIZE,
 808                          512 - RPMB_STUFF_DATA_SIZE);
 809         }
 810 #endif
 811
 812         /* Make sure the last data packet can't be modified once verified */
 813         memcpy(&lastfrm, &datafrm[nbr_frms - 1], RPMB_DATA_FRAME_SIZE);
 814
 815         /* Handle operation result and translate to TEEC error code. */
 816         bytes_to_u16(lastfrm.op_result, &op_result);
 817         if (rawdata->op_result)
 818                 *rawdata->op_result = op_result;
 819         if (op_result != RPMB_RESULT_OK)
 820                 return TEE_ERROR_GENERIC;
 821
 822         /* Check the response msg_type. */
 823         bytes_to_u16(lastfrm.msg_type, &msg_type);
 824         if (msg_type != rawdata->msg_type) {
 825                 DMSG("Unexpected msg_type (0x%04x != 0x%04x)", msg_type,
 826                      rawdata->msg_type);
 827                 return TEE_ERROR_GENERIC;
 828         }
 829
 830         if (rawdata->blk_idx) {
 831                 bytes_to_u16(lastfrm.address, &blk_idx);
 832                 if (blk_idx != *rawdata->blk_idx) {
 833                         DMSG("Unexpected block index");
 834                         return TEE_ERROR_GENERIC;
 835                 }
 836         }
 837
 838         if (rawdata->write_counter) {
 839                 wr_cnt = *rawdata->write_counter;
 840                 bytes_to_u32(lastfrm.write_counter, rawdata->write_counter);
 841                 if (msg_type == RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE) {
 842                         /* Verify the write counter is incremented by 1 */
 843                         if (*rawdata->write_counter != wr_cnt + 1) {
 844                                 DMSG("Counter mismatched (0x%04x/0x%04x)",
 845                                      *rawdata->write_counter, wr_cnt + 1);
 846                                 return TEE_ERROR_SECURITY;
 847                         }
 848                         rpmb_ctx->wr_cnt++;
 849                 }
 850         }
 851
 852         if (rawdata->nonce) {
 853                 if (buf_compare_ct(rawdata->nonce, lastfrm.nonce,
 854                                    RPMB_NONCE_SIZE) != 0) {
 855                         DMSG("Nonce mismatched");
 856                         return TEE_ERROR_SECURITY;
 857                 }
 858         }
 859
 860         if (rawdata->key_mac) {
 861                 if (msg_type == RPMB_MSG_TYPE_RESP_AUTH_DATA_READ) {
 862                         if (!rawdata->data)
 863                                 return TEE_ERROR_GENERIC;
 864
 865                         res = tee_rpmb_data_cpy_mac_calc(datafrm, rawdata,
 866                                                          nbr_frms, &lastfrm,
 867                                                          fek);
 868
 869                         if (res != TEE_SUCCESS)
 870                                 return res;
 871                 } else {
 872                         /*
 873                          * There should be only one data frame for
 874                          * other msg types.
 875                          */
 876                         if (nbr_frms != 1)
 877                                 return TEE_ERROR_GENERIC;
 878
 879                         res = tee_rpmb_mac_calc(rawdata->key_mac,
 880                                                 RPMB_KEY_MAC_SIZE,
 881                                                 rpmb_ctx->key,
 882                                                 RPMB_KEY_MAC_SIZE,
 883                                                 &lastfrm, 1);
 884
 885                         if (res != TEE_SUCCESS)
 886                                 return res;
 887                 }
 888
 889 #ifndef CFG_RPMB_FS_NO_MAC
 890                 if (buf_compare_ct(rawdata->key_mac,
 891                                    (datafrm + nbr_frms - 1)->key_mac,
 892                                    RPMB_KEY_MAC_SIZE) != 0) {
 893                         DMSG("MAC mismatched:");
 894 #ifdef CFG_RPMB_FS_DEBUG_DATA
 895                         DHEXDUMP((uint8_t *)rawdata->key_mac, 32);
 896 #endif
 897                         return TEE_ERROR_SECURITY;
 898                 }
 899 #endif /* !CFG_RPMB_FS_NO_MAC */
 900         }
 901
 902         return TEE_SUCCESS;
 903 }
 904
 905 static TEE_Result tee_rpmb_get_dev_info(uint16_t dev_id,
 906                                         struct rpmb_dev_info *dev_info)
 907 {
 908         TEE_Result res = TEE_ERROR_GENERIC;
 909         struct tee_rpmb_mem mem;
 910         struct rpmb_dev_info *di;
 911         struct rpmb_req *req = NULL;
 912         uint8_t *resp = NULL;
 913         uint32_t req_size;
 914         uint32_t resp_size;
 915
 916         if (!dev_info)
 917                 return TEE_ERROR_BAD_PARAMETERS;
 918
 919         req_size = sizeof(struct rpmb_req);
 920         resp_size = sizeof(struct rpmb_dev_info);
 921         res = tee_rpmb_alloc(req_size, resp_size, &mem,
 922                              (void *)&req, (void *)&resp);
 923         if (res != TEE_SUCCESS)
 924                 goto func_exit;
 925
 926         req->cmd = RPMB_CMD_GET_DEV_INFO;
 927         req->dev_id = dev_id;
 928
 929         di = (struct rpmb_dev_info *)resp;
 930         di->ret_code = RPMB_CMD_GET_DEV_INFO_RET_ERROR;
 931
 932         res = tee_rpmb_invoke(&mem);
 933         if (res != TEE_SUCCESS)
 934                 goto func_exit;
 935
 936         if (di->ret_code != RPMB_CMD_GET_DEV_INFO_RET_OK) {
 937                 res = TEE_ERROR_GENERIC;
 938                 goto func_exit;
 939         }
 940
 941         memcpy((uint8_t *)dev_info, resp, sizeof(struct rpmb_dev_info));
 942
 943 #ifdef CFG_RPMB_FS_DEBUG_DATA
 944         DMSG("Dumping dev_info:");
 945         DHEXDUMP((uint8_t *)dev_info, sizeof(struct rpmb_dev_info));
 946 #endif
 947
 948         res = TEE_SUCCESS;
 949
 950 func_exit:
 951         tee_rpmb_free(&mem);
 952         return res;
 953 }
 954
 955 static TEE_Result tee_rpmb_init_read_wr_cnt(uint16_t dev_id,
 956                                             uint32_t *wr_cnt,
 957                                             uint16_t *op_result)
 958 {
 959         TEE_Result res = TEE_ERROR_GENERIC;
 960         struct tee_rpmb_mem mem;
 961         uint16_t msg_type;
 962         uint8_t nonce[RPMB_NONCE_SIZE];
 963         uint8_t hmac[RPMB_KEY_MAC_SIZE];
 964         struct rpmb_req *req = NULL;
 965         struct rpmb_data_frame *resp = NULL;
 966         struct rpmb_raw_data rawdata;
 967         uint32_t req_size;
 968         uint32_t resp_size;
 969
 970         if (!wr_cnt)
 971                 return TEE_ERROR_BAD_PARAMETERS;
 972
 973         req_size = sizeof(struct rpmb_req) + RPMB_DATA_FRAME_SIZE;
 974         resp_size = RPMB_DATA_FRAME_SIZE;
 975         res = tee_rpmb_alloc(req_size, resp_size, &mem,
 976                              (void *)&req, (void *)&resp);
 977         if (res != TEE_SUCCESS)
 978                 goto func_exit;
 979
 980         res = crypto_ops.prng.read(nonce, RPMB_NONCE_SIZE);
 981         if (res != TEE_SUCCESS)
 982                 goto func_exit;
 983
 984         msg_type = RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ;
 985
 986         memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
 987         rawdata.msg_type = msg_type;
 988         rawdata.nonce = nonce;
 989
 990         res = tee_rpmb_req_pack(req, &rawdata, 1, dev_id, NULL);
 991         if (res != TEE_SUCCESS)
 992                 goto func_exit;
 993
 994         res = tee_rpmb_invoke(&mem);
 995         if (res != TEE_SUCCESS)
 996                 goto func_exit;
 997
 998         msg_type = RPMB_MSG_TYPE_RESP_WRITE_COUNTER_VAL_READ;
 999
1000         memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
1001         rawdata.msg_type = msg_type;
1002         rawdata.op_result = op_result;
1003         rawdata.write_counter = wr_cnt;
1004         rawdata.nonce = nonce;
1005         rawdata.key_mac = hmac;
1006
1007         res = tee_rpmb_resp_unpack_verify(resp, &rawdata, 1, NULL);
1008         if (res != TEE_SUCCESS)
1009                 goto func_exit;
1010
1011         res = TEE_SUCCESS;
1012
1013 func_exit:
1014         tee_rpmb_free(&mem);
1015         return res;
1016 }
1017
1018 static TEE_Result tee_rpmb_verify_key_sync_counter(uint16_t dev_id)
1019 {
1020         uint16_t op_result = 0;
1021         TEE_Result res = TEE_ERROR_GENERIC;
1022
1023         res = tee_rpmb_init_read_wr_cnt(dev_id, &rpmb_ctx->wr_cnt,
1024                                         &op_result);
1025
1026         if (res == TEE_SUCCESS) {
1027                 rpmb_ctx->key_verified = true;
1028                 rpmb_ctx->wr_cnt_synced = true;
1029         }
1030
1031         DMSG("Verify key returning 0x%x\n", res);
1032         return res;
1033 }
1034
1035 #ifdef CFG_RPMB_WRITE_KEY
1036 static TEE_Result tee_rpmb_write_key(uint16_t dev_id)
1037 {
1038         TEE_Result res = TEE_ERROR_GENERIC;
1039         struct tee_rpmb_mem mem = { 0 };
1040         uint16_t msg_type;
1041         struct rpmb_req *req = NULL;
1042         struct rpmb_data_frame *resp = NULL;
1043         struct rpmb_raw_data rawdata;
1044         uint32_t req_size;
1045         uint32_t resp_size;
1046
1047         req_size = sizeof(struct rpmb_req) + RPMB_DATA_FRAME_SIZE;
1048         resp_size = RPMB_DATA_FRAME_SIZE;
1049         res = tee_rpmb_alloc(req_size, resp_size, &mem,
1050                              (void *)&req, (void *)&resp);
1051         if (res != TEE_SUCCESS)
1052                 goto func_exit;
1053
1054         msg_type = RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM;
1055
1056         memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
1057         rawdata.msg_type = msg_type;
1058         rawdata.key_mac = rpmb_ctx->key;
1059
1060         res = tee_rpmb_req_pack(req, &rawdata, 1, dev_id, NULL);
1061         if (res != TEE_SUCCESS)
1062                 goto func_exit;
1063
1064         res = tee_rpmb_invoke(&mem);
1065         if (res != TEE_SUCCESS)
1066                 goto func_exit;
1067
1068         msg_type = RPMB_MSG_TYPE_RESP_AUTH_KEY_PROGRAM;
1069
1070         memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
1071         rawdata.msg_type = msg_type;
1072
1073         res = tee_rpmb_resp_unpack_verify(resp, &rawdata, 1, NULL);
1074         if (res != TEE_SUCCESS)
1075                 goto func_exit;
1076
1077         res = TEE_SUCCESS;
1078
1079 func_exit:
1080         tee_rpmb_free(&mem);
1081         return res;
1082 }
1083
1084 static TEE_Result tee_rpmb_write_and_verify_key(uint16_t dev_id)
1085 {
1086         TEE_Result res;
1087
1088         DMSG("RPMB INIT: Writing Key");
1089         res = tee_rpmb_write_key(dev_id);
1090         if (res == TEE_SUCCESS) {
1091                 DMSG("RPMB INIT: Verifying Key");
1092                 res = tee_rpmb_verify_key_sync_counter(dev_id);
1093         }
1094         return res;
1095 }
1096 #else
1097 static TEE_Result tee_rpmb_write_and_verify_key(uint16_t dev_id __unused)
1098 {
1099         return TEE_ERROR_BAD_STATE;
1100 }
1101 #endif
1102
1103 /* True when all the required crypto functions are available */
1104 static bool have_crypto_ops(void)
1105 {
1106         return (crypto_ops.mac.init && crypto_ops.mac.update &&
1107                 crypto_ops.mac.final && crypto_ops.prng.read);
1108 }
1109
1110 /* This function must never return TEE_SUCCESS if rpmb_ctx == NULL */
1111 static TEE_Result tee_rpmb_init(uint16_t dev_id)
1112 {
1113         TEE_Result res = TEE_SUCCESS;
1114         struct rpmb_dev_info dev_info;
1115
1116         if (!have_crypto_ops())
1117                 return TEE_ERROR_NOT_SUPPORTED;
1118
1119         if (!rpmb_ctx) {
1120                 rpmb_ctx = calloc(1, sizeof(struct tee_rpmb_ctx));
1121                 if (!rpmb_ctx)
1122                         return TEE_ERROR_OUT_OF_MEMORY;
1123         } else if (rpmb_ctx->dev_id != dev_id) {
1124                 memset(rpmb_ctx, 0x00, sizeof(struct tee_rpmb_ctx));
1125         }
1126
1127         rpmb_ctx->dev_id = dev_id;
1128
1129         if (!rpmb_ctx->dev_info_synced) {
1130                 DMSG("RPMB: Syncing device information");
1131
1132                 dev_info.rpmb_size_mult = 0;
1133                 dev_info.rel_wr_sec_c = 0;
1134                 res = tee_rpmb_get_dev_info(dev_id, &dev_info);
1135                 if (res != TEE_SUCCESS)
1136                         goto func_exit;
1137
1138                 DMSG("RPMB: RPMB size is %d*128 KB", dev_info.rpmb_size_mult);
1139                 DMSG("RPMB: Reliable Write Sector Count is %d",
1140                      dev_info.rel_wr_sec_c);
1141
1142                 if (dev_info.rpmb_size_mult == 0) {
1143                         res = TEE_ERROR_GENERIC;
1144                         goto func_exit;
1145                 }
1146
1147                 rpmb_ctx->max_blk_idx = (dev_info.rpmb_size_mult *
1148                                          RPMB_SIZE_SINGLE / RPMB_DATA_SIZE) - 1;
1149
1150                 memcpy(rpmb_ctx->cid, dev_info.cid, RPMB_EMMC_CID_SIZE);
1151
1152                 if ((rpmb_ctx->hash_ctx_size == 0) &&
1153                     (crypto_ops.mac.get_ctx_size(
1154                             TEE_ALG_HMAC_SHA256,
1155                             &rpmb_ctx->hash_ctx_size))) {
1156                         rpmb_ctx->hash_ctx_size = 0;
1157                         res = TEE_ERROR_GENERIC;
1158                         goto func_exit;
1159                 }
1160
1161 #ifdef RPMB_DRIVER_MULTIPLE_WRITE_FIXED
1162                 rpmb_ctx->rel_wr_blkcnt = dev_info.rel_wr_sec_c * 2;
1163 #else
1164                 rpmb_ctx->rel_wr_blkcnt = 1;
1165 #endif
1166
1167                 rpmb_ctx->dev_info_synced = true;
1168         }
1169
1170         if (!rpmb_ctx->key_derived) {
1171                 DMSG("RPMB INIT: Deriving key");
1172
1173                 res = tee_rpmb_key_gen(dev_id, rpmb_ctx->key,
1174                                        RPMB_KEY_MAC_SIZE);
1175                 if (res != TEE_SUCCESS)
1176                         goto func_exit;
1177
1178                 rpmb_ctx->key_derived = true;
1179         }
1180
1181         /* Perform a write counter read to verify if the key is ok. */
1182         if (!rpmb_ctx->wr_cnt_synced || !rpmb_ctx->key_verified) {
1183                 DMSG("RPMB INIT: Verifying Key");
1184
1185                 res = tee_rpmb_verify_key_sync_counter(dev_id);
1186                 if (res != TEE_SUCCESS && !rpmb_ctx->key_verified) {
1187                         /*
1188                          * Need to write the key here and verify it.
1189                          */
1190                         res = tee_rpmb_write_and_verify_key(dev_id);
1191                 }
1192         }
1193
1194 func_exit:
1195         return res;
1196 }
1197
1198 /*
1199  * Read RPMB data in bytes.
1200  *
1201  * @dev_id     Device ID of the eMMC device.
1202  * @addr       Byte address of data.
1203  * @data       Pointer to the data.
1204  * @len        Size of data in bytes.
1205  * @fek        Encrypted File Encryption Key or NULL.
1206  */
1207 static TEE_Result tee_rpmb_read(uint16_t dev_id, uint32_t addr, uint8_t *data,
1208                                 uint32_t len, uint8_t *fek)
1209 {
1210         TEE_Result res = TEE_ERROR_GENERIC;
1211         struct tee_rpmb_mem mem = { 0 };
1212         uint16_t msg_type;
1213         uint8_t nonce[RPMB_NONCE_SIZE];
1214         uint8_t hmac[RPMB_KEY_MAC_SIZE];
1215         struct rpmb_req *req = NULL;
1216         struct rpmb_data_frame *resp = NULL;
1217         struct rpmb_raw_data rawdata;
1218         uint32_t req_size;
1219         uint32_t resp_size;
1220         uint16_t blk_idx;
1221         uint16_t blkcnt;
1222         uint8_t byte_offset;
1223
1224         if (!data || !len)
1225                 return TEE_ERROR_BAD_PARAMETERS;
1226
1227         blk_idx = addr / RPMB_DATA_SIZE;
1228         byte_offset = addr % RPMB_DATA_SIZE;
1229
1230         blkcnt =
1231             ROUNDUP(len + byte_offset, RPMB_DATA_SIZE) / RPMB_DATA_SIZE;
1232         res = tee_rpmb_init(dev_id);
1233         if (res != TEE_SUCCESS)
1234                 goto func_exit;
1235
1236         req_size = sizeof(struct rpmb_req) + RPMB_DATA_FRAME_SIZE;
1237         resp_size = RPMB_DATA_FRAME_SIZE * blkcnt;
1238         res = tee_rpmb_alloc(req_size, resp_size, &mem,
1239                              (void *)&req, (void *)&resp);
1240         if (res != TEE_SUCCESS)
1241                 goto func_exit;
1242
1243         msg_type = RPMB_MSG_TYPE_REQ_AUTH_DATA_READ;
1244         res = crypto_ops.prng.read(nonce, RPMB_NONCE_SIZE);
1245         if (res != TEE_SUCCESS)
1246                 goto func_exit;
1247
1248         memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
1249         rawdata.msg_type = msg_type;
1250         rawdata.nonce = nonce;
1251         rawdata.blk_idx = &blk_idx;
1252         res = tee_rpmb_req_pack(req, &rawdata, 1, dev_id, NULL);
1253         if (res != TEE_SUCCESS)
1254                 goto func_exit;
1255
1256         req->block_count = blkcnt;
1257
1258         DMSG("Read %u block%s at index %u", blkcnt, ((blkcnt > 1) ? "s" : ""),
1259              blk_idx);
1260
1261         res = tee_rpmb_invoke(&mem);
1262         if (res != TEE_SUCCESS)
1263                 goto func_exit;
1264
1265         msg_type = RPMB_MSG_TYPE_RESP_AUTH_DATA_READ;
1266
1267         memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
1268         rawdata.msg_type = msg_type;
1269         rawdata.block_count = &blkcnt;
1270         rawdata.blk_idx = &blk_idx;
1271         rawdata.nonce = nonce;
1272         rawdata.key_mac = hmac;
1273         rawdata.data = data;
1274
1275         rawdata.len = len;
1276         rawdata.byte_offset = byte_offset;
1277
1278         res = tee_rpmb_resp_unpack_verify(resp, &rawdata, blkcnt, fek);
1279         if (res != TEE_SUCCESS)
1280                 goto func_exit;
1281
1282         res = TEE_SUCCESS;
1283
1284 func_exit:
1285         tee_rpmb_free(&mem);
1286         return res;
1287 }
1288
1289 static TEE_Result tee_rpmb_write_blk(uint16_t dev_id, uint16_t blk_idx,
1290                                      const uint8_t *data_blks, uint16_t blkcnt,
1291                                      const uint8_t *fek)
1292 {
1293         TEE_Result res;
1294         struct tee_rpmb_mem mem;
1295         uint16_t msg_type;
1296         uint32_t wr_cnt;
1297         uint8_t hmac[RPMB_KEY_MAC_SIZE];
1298         struct rpmb_req *req = NULL;
1299         struct rpmb_data_frame *resp = NULL;
1300         struct rpmb_raw_data rawdata;
1301         uint32_t req_size;
1302         uint32_t resp_size;
1303         uint32_t nbr_writes;
1304         uint16_t tmp_blkcnt;
1305         uint16_t tmp_blk_idx;
1306         uint16_t i;
1307
1308         DMSG("Write %u block%s at index %u", blkcnt, ((blkcnt > 1) ? "s" : ""),
1309              blk_idx);
1310
1311         if (!data_blks || !blkcnt)
1312                 return TEE_ERROR_BAD_PARAMETERS;
1313
1314         res = tee_rpmb_init(dev_id);
1315         if (res != TEE_SUCCESS)
1316                 return res;
1317
1318         /*
1319          * We need to split data when block count
1320          * is bigger than reliable block write count.
1321          */
1322         if (blkcnt < rpmb_ctx->rel_wr_blkcnt)
1323                 req_size = sizeof(struct rpmb_req) +
1324                     RPMB_DATA_FRAME_SIZE * blkcnt;
1325         else
1326                 req_size = sizeof(struct rpmb_req) +
1327                     RPMB_DATA_FRAME_SIZE * rpmb_ctx->rel_wr_blkcnt;
1328
1329         resp_size = RPMB_DATA_FRAME_SIZE;
1330         res = tee_rpmb_alloc(req_size, resp_size, &mem,
1331                              (void *)&req, (void *)&resp);
1332         if (res != TEE_SUCCESS)
1333                 return res;
1334
1335         nbr_writes = blkcnt / rpmb_ctx->rel_wr_blkcnt;
1336         if (blkcnt % rpmb_ctx->rel_wr_blkcnt > 0)
1337                 nbr_writes += 1;
1338
1339         tmp_blkcnt = rpmb_ctx->rel_wr_blkcnt;
1340         tmp_blk_idx = blk_idx;
1341         for (i = 0; i < nbr_writes; i++) {
1342                 /*
1343                  * To handle the last write of block count which is
1344                  * equal or smaller than reliable write block count.
1345                  */
1346                 if (i == nbr_writes - 1)
1347                         tmp_blkcnt = blkcnt - rpmb_ctx->rel_wr_blkcnt *
1348                             (nbr_writes - 1);
1349
1350                 msg_type = RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE;
1351                 wr_cnt = rpmb_ctx->wr_cnt;
1352
1353                 memset(req, 0x00, req_size);
1354                 memset(resp, 0x00, resp_size);
1355
1356                 memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
1357                 rawdata.msg_type = msg_type;
1358                 rawdata.block_count = &tmp_blkcnt;
1359                 rawdata.blk_idx = &tmp_blk_idx;
1360                 rawdata.write_counter = &wr_cnt;
1361                 rawdata.key_mac = hmac;
1362                 rawdata.data = (uint8_t *)data_blks +
1363                                 i * rpmb_ctx->rel_wr_blkcnt * RPMB_DATA_SIZE;
1364
1365                 res = tee_rpmb_req_pack(req, &rawdata, tmp_blkcnt, dev_id,
1366                                         fek);
1367                 if (res != TEE_SUCCESS)
1368                         goto out;
1369
1370                 res = tee_rpmb_invoke(&mem);
1371                 if (res != TEE_SUCCESS) {
1372                         /*
1373                          * To force wr_cnt sync next time, as it might get
1374                          * out of sync due to inconsistent operation result!
1375                          */
1376                         rpmb_ctx->wr_cnt_synced = false;
1377                         goto out;
1378                 }
1379
1380                 msg_type = RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE;
1381
1382                 memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
1383                 rawdata.msg_type = msg_type;
1384                 rawdata.block_count = &tmp_blkcnt;
1385                 rawdata.blk_idx = &tmp_blk_idx;
1386                 rawdata.write_counter = &wr_cnt;
1387                 rawdata.key_mac = hmac;
1388
1389                 res = tee_rpmb_resp_unpack_verify(resp, &rawdata, 1, NULL);
1390                 if (res != TEE_SUCCESS) {
1391                         /*
1392                          * To force wr_cnt sync next time, as it might get
1393                          * out of sync due to inconsistent operation result!
1394                          */
1395                         rpmb_ctx->wr_cnt_synced = false;
1396                         goto out;
1397                 }
1398
1399                 tmp_blk_idx += tmp_blkcnt;
1400         }
1401
1402 out:
1403         tee_rpmb_free(&mem);
1404         return res;
1405 }
1406
1407 static bool tee_rpmb_write_is_atomic(uint16_t dev_id __unused, uint32_t addr,
1408                                      uint32_t len)
1409 {
1410         uint8_t byte_offset = addr % RPMB_DATA_SIZE;
1411         uint16_t blkcnt = ROUNDUP(len + byte_offset,
1412                                   RPMB_DATA_SIZE) / RPMB_DATA_SIZE;
1413
1414         return (blkcnt <= rpmb_ctx->rel_wr_blkcnt);
1415 }
1416
1417 /*
1418  * Write RPMB data in bytes.
1419  *
1420  * @dev_id     Device ID of the eMMC device.
1421  * @addr       Byte address of data.
1422  * @data       Pointer to the data.
1423  * @len        Size of data in bytes.
1424  * @fek        Encrypted File Encryption Key or NULL.
1425  */
1426 static TEE_Result tee_rpmb_write(uint16_t dev_id, uint32_t addr,
1427                                  const uint8_t *data, uint32_t len,
1428                                  uint8_t *fek)
1429 {
1430         TEE_Result res = TEE_ERROR_GENERIC;
1431         uint8_t *data_tmp = NULL;
1432         uint16_t blk_idx;
1433         uint16_t blkcnt;
1434         uint8_t byte_offset;
1435
1436         blk_idx = addr / RPMB_DATA_SIZE;
1437         byte_offset = addr % RPMB_DATA_SIZE;
1438
1439         blkcnt =
1440             ROUNDUP(len + byte_offset, RPMB_DATA_SIZE) / RPMB_DATA_SIZE;
1441
1442         if (byte_offset == 0 && (len % RPMB_DATA_SIZE) == 0) {
1443                 res = tee_rpmb_write_blk(dev_id, blk_idx, data, blkcnt, fek);
1444                 if (res != TEE_SUCCESS)
1445                         goto func_exit;
1446         } else {
1447                 data_tmp = calloc(blkcnt, RPMB_DATA_SIZE);
1448                 if (!data_tmp) {
1449                         res = TEE_ERROR_OUT_OF_MEMORY;
1450                         goto func_exit;
1451                 }
1452
1453                 /* Read the complete blocks */
1454                 res = tee_rpmb_read(dev_id, blk_idx * RPMB_DATA_SIZE, data_tmp,
1455                                     blkcnt * RPMB_DATA_SIZE, fek);
1456                 if (res != TEE_SUCCESS)
1457                         goto func_exit;
1458
1459                 /* Partial update of the data blocks */
1460                 memcpy(data_tmp + byte_offset, data, len);
1461
1462                 res = tee_rpmb_write_blk(dev_id, blk_idx, data_tmp, blkcnt,
1463                                          fek);
1464                 if (res != TEE_SUCCESS)
1465                         goto func_exit;
1466         }
1467
1468         res = TEE_SUCCESS;
1469
1470 func_exit:
1471         free(data_tmp);
1472         return res;
1473 }
1474
1475 /*
1476  * Read the RPMB write counter.
1477  *
1478  * @dev_id     Device ID of the eMMC device.
1479  * @counter    Pointer to the counter.
1480  */
1481 static TEE_Result tee_rpmb_get_write_counter(uint16_t dev_id,
1482                                              uint32_t *counter)
1483 {
1484         TEE_Result res = TEE_SUCCESS;
1485
1486         if (!counter)
1487                 return TEE_ERROR_BAD_PARAMETERS;
1488
1489         if (!rpmb_ctx || !rpmb_ctx->wr_cnt_synced) {
1490                 res = tee_rpmb_init(dev_id);
1491                 if (res != TEE_SUCCESS)
1492                         goto func_exit;
1493         }
1494
1495         *counter = rpmb_ctx->wr_cnt;
1496
1497 func_exit:
1498         return res;
1499 }
1500
1501 /*
1502  * Read the RPMB max block.
1503  *
1504  * @dev_id     Device ID of the eMMC device.
1505  * @counter    Pointer to receive the max block.
1506  */
1507 static TEE_Result tee_rpmb_get_max_block(uint16_t dev_id, uint32_t *max_block)
1508 {
1509         TEE_Result res = TEE_SUCCESS;
1510
1511         if (!max_block)
1512                 return TEE_ERROR_BAD_PARAMETERS;
1513
1514         if (!rpmb_ctx || !rpmb_ctx->dev_info_synced) {
1515                 res = tee_rpmb_init(dev_id);
1516                 if (res != TEE_SUCCESS)
1517                         goto func_exit;
1518         }
1519
1520         *max_block = rpmb_ctx->max_blk_idx;
1521
1522 func_exit:
1523         return res;
1524 }
1525
1526 /*
1527  * End of lower interface to RPMB device
1528  */
1529
1530 static TEE_Result get_fat_start_address(uint32_t *addr);
1531
1532 static void dump_fat(void)
1533 {
1534         TEE_Result res = TEE_ERROR_GENERIC;
1535         struct rpmb_fat_entry *fat_entries = NULL;
1536         uint32_t fat_address;
1537         size_t size;
1538         int i;
1539         bool last_entry_found = false;
1540
1541         res = get_fat_start_address(&fat_address);
1542         if (res != TEE_SUCCESS)
1543                 goto out;
1544
1545         size = N_ENTRIES * sizeof(struct rpmb_fat_entry);
1546         fat_entries = malloc(size);
1547         if (!fat_entries) {
1548                 res = TEE_ERROR_OUT_OF_MEMORY;
1549                 goto out;
1550         }
1551
1552         while (!last_entry_found) {
1553                 res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID, fat_address,
1554                                     (uint8_t *)fat_entries, size, NULL);
1555                 if (res != TEE_SUCCESS)
1556                         goto out;
1557
1558                 for (i = 0; i < N_ENTRIES; i++) {
1559
1560                         FMSG("flags 0x%x, size %d, address 0x%x, filename '%s'",
1561                                 fat_entries[i].flags,
1562                                 fat_entries[i].data_size,
1563                                 fat_entries[i].start_address,
1564                                 fat_entries[i].filename);
1565
1566                         if ((fat_entries[i].flags & FILE_IS_LAST_ENTRY) != 0) {
1567                                 last_entry_found = true;
1568                                 break;
1569                         }
1570
1571                         /* Move to next fat_entry. */
1572                         fat_address += sizeof(struct rpmb_fat_entry);
1573                 }
1574         }
1575
1576 out:
1577         free(fat_entries);
1578 }
1579
1580 #if (TRACE_LEVEL >= TRACE_DEBUG)
1581 static void dump_fh(struct rpmb_file_handle *fh)
1582 {
1583         DMSG("fh->filename=%s", fh->filename);
1584         DMSG("fh->pos=%u", fh->pos);
1585         DMSG("fh->rpmb_fat_address=%u", fh->rpmb_fat_address);
1586         DMSG("fh->fat_entry.start_address=%u", fh->fat_entry.start_address);
1587         DMSG("fh->fat_entry.data_size=%u", fh->fat_entry.data_size);
1588 }
1589 #else
1590 static void dump_fh(struct rpmb_file_handle *fh __unused)
1591 {
1592 }
1593 #endif
1594
1595 static struct rpmb_file_handle *alloc_file_handle(const char *filename)
1596 {
1597         struct rpmb_file_handle *fh = NULL;
1598
1599         fh = calloc(1, sizeof(struct rpmb_file_handle));
1600         if (!fh)
1601                 return NULL;
1602
1603         if (filename)
1604                 strlcpy(fh->filename, filename, sizeof(fh->filename));
1605
1606         return fh;
1607 }
1608
1609 /**
1610  * write_fat_entry: Store info in a fat_entry to RPMB.
1611  */
1612 static TEE_Result write_fat_entry(struct rpmb_file_handle *fh,
1613                                   bool update_write_counter)
1614 {
1615         TEE_Result res = TEE_ERROR_GENERIC;
1616
1617         /* Protect partition data. */
1618         if (fh->rpmb_fat_address < sizeof(struct rpmb_fs_partition)) {
1619                 res = TEE_ERROR_ACCESS_CONFLICT;
1620                 goto out;
1621         }
1622
1623         if (fh->rpmb_fat_address % sizeof(struct rpmb_fat_entry) != 0) {
1624                 res = TEE_ERROR_BAD_PARAMETERS;
1625                 goto out;
1626         }
1627
1628         if (update_write_counter) {
1629                 res = tee_rpmb_get_write_counter(CFG_RPMB_FS_DEV_ID,
1630                                                  &fh->fat_entry.write_counter);
1631                 if (res != TEE_SUCCESS)
1632                         goto out;
1633         }
1634
1635         res = tee_rpmb_write(CFG_RPMB_FS_DEV_ID, fh->rpmb_fat_address,
1636                              (uint8_t *)&fh->fat_entry,
1637                              sizeof(struct rpmb_fat_entry), NULL);
1638
1639         dump_fat();
1640
1641 out:
1642         return res;
1643 }
1644
1645 /**
1646  * rpmb_fs_setup: Setup rpmb fs.
1647  * Set initial partition and FS values and write to RPMB.
1648  * Store frequently used data in RAM.
1649  */
1650 static TEE_Result rpmb_fs_setup(void)
1651 {
1652         TEE_Result res = TEE_ERROR_GENERIC;
1653         struct rpmb_fs_partition *partition_data = NULL;
1654         struct rpmb_file_handle *fh = NULL;
1655         uint32_t max_rpmb_block = 0;
1656
1657         if (fs_par) {
1658                 res = TEE_SUCCESS;
1659                 goto out;
1660         }
1661
1662         res = tee_rpmb_get_max_block(CFG_RPMB_FS_DEV_ID, &max_rpmb_block);
1663         if (res != TEE_SUCCESS)
1664                 goto out;
1665
1666         partition_data = calloc(1, sizeof(struct rpmb_fs_partition));
1667         if (!partition_data) {
1668                 res = TEE_ERROR_OUT_OF_MEMORY;
1669                 goto out;
1670         }
1671
1672         res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID, RPMB_STORAGE_START_ADDRESS,
1673                             (uint8_t *)partition_data,
1674                             sizeof(struct rpmb_fs_partition), NULL);
1675         if (res != TEE_SUCCESS)
1676                 goto out;
1677
1678 #ifndef CFG_RPMB_RESET_FAT
1679         if (partition_data->rpmb_fs_magic == RPMB_FS_MAGIC) {
1680                 if (partition_data->fs_version == FS_VERSION) {
1681                         res = TEE_SUCCESS;
1682                         goto store_fs_par;
1683                 } else {
1684                         /* Wrong software is in use. */
1685                         res = TEE_ERROR_ACCESS_DENIED;
1686                         goto out;
1687                 }
1688         }
1689 #else
1690         EMSG("**** Clearing Storage ****");
1691 #endif
1692
1693         /* Setup new partition data. */
1694         partition_data->rpmb_fs_magic = RPMB_FS_MAGIC;
1695         partition_data->fs_version = FS_VERSION;
1696         partition_data->fat_start_address = RPMB_FS_FAT_START_ADDRESS;
1697
1698         /* Initial FAT entry with FILE_IS_LAST_ENTRY flag set. */
1699         fh = alloc_file_handle(NULL);
1700         if (!fh) {
1701                 res = TEE_ERROR_OUT_OF_MEMORY;
1702                 goto out;
1703         }
1704         fh->fat_entry.flags = FILE_IS_LAST_ENTRY;
1705         fh->rpmb_fat_address = partition_data->fat_start_address;
1706
1707         /* Write init FAT entry and partition data to RPMB. */
1708         res = write_fat_entry(fh, true);
1709         if (res != TEE_SUCCESS)
1710                 goto out;
1711
1712         res =
1713             tee_rpmb_get_write_counter(CFG_RPMB_FS_DEV_ID,
1714                                        &partition_data->write_counter);
1715         if (res != TEE_SUCCESS)
1716                 goto out;
1717         res = tee_rpmb_write(CFG_RPMB_FS_DEV_ID, RPMB_STORAGE_START_ADDRESS,
1718                              (uint8_t *)partition_data,
1719                              sizeof(struct rpmb_fs_partition), NULL);
1720
1721 #ifndef CFG_RPMB_RESET_FAT
1722 store_fs_par:
1723 #endif
1724
1725         /* Store FAT start address. */
1726         fs_par = calloc(1, sizeof(struct rpmb_fs_parameters));
1727         if (!fs_par) {
1728                 res = TEE_ERROR_OUT_OF_MEMORY;
1729                 goto out;
1730         }
1731
1732         fs_par->fat_start_address = partition_data->fat_start_address;
1733         fs_par->max_rpmb_address = max_rpmb_block << RPMB_BLOCK_SIZE_SHIFT;
1734
1735         dump_fat();
1736
1737 out:
1738         free(fh);
1739         free(partition_data);
1740         return res;
1741 }
1742
1743 /**
1744  * get_fat_start_address:
1745  * FAT start_address from fs_par.
1746  */
1747 static TEE_Result get_fat_start_address(uint32_t *addr)
1748 {
1749         if (!fs_par)
1750                 return TEE_ERROR_NO_DATA;
1751
1752         *addr = fs_par->fat_start_address;
1753
1754         return TEE_SUCCESS;
1755 }
1756
1757 /**
1758  * read_fat: Read FAT entries
1759  * Return matching FAT entry for read, rm rename and stat.
1760  * Build up memory pool and return matching entry for write operation.
1761  * "Last FAT entry" can be returned during write.
1762  */
1763 static TEE_Result read_fat(struct rpmb_file_handle *fh, tee_mm_pool_t *p)
1764 {
1765         TEE_Result res = TEE_ERROR_GENERIC;
1766         tee_mm_entry_t *mm = NULL;
1767         struct rpmb_fat_entry *fat_entries = NULL;
1768         uint32_t fat_address;
1769         size_t size;
1770         int i;
1771         bool entry_found = false;
1772         bool last_entry_found = false;
1773         bool expand_fat = false;
1774         struct rpmb_file_handle last_fh;
1775
1776         DMSG("fat_address %d", fh->rpmb_fat_address);
1777
1778         res = rpmb_fs_setup();
1779         if (res != TEE_SUCCESS)
1780                 goto out;
1781
1782         res = get_fat_start_address(&fat_address);
1783         if (res != TEE_SUCCESS)
1784                 goto out;
1785
1786         size = N_ENTRIES * sizeof(struct rpmb_fat_entry);
1787         fat_entries = malloc(size);
1788         if (!fat_entries) {
1789                 res = TEE_ERROR_OUT_OF_MEMORY;
1790                 goto out;
1791         }
1792
1793         /*
1794          * The pool is used to represent the current RPMB layout. To find
1795          * a slot for the file tee_mm_alloc is called on the pool. Thus
1796          * if it is not NULL the entire FAT must be traversed to fill in
1797          * the pool.
1798          */
1799         while (!last_entry_found && (!entry_found || p)) {
1800                 res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID, fat_address,
1801                                     (uint8_t *)fat_entries, size, NULL);
1802                 if (res != TEE_SUCCESS)
1803                         goto out;
1804
1805                 for (i = 0; i < N_ENTRIES; i++) {
1806                         /*
1807                          * Look for an entry, matching filenames. (read, rm,
1808                          * rename and stat.). Only store first filename match.
1809                          */
1810                         if (fh->filename &&
1811                             (strcmp(fh->filename,
1812                                     fat_entries[i].filename) == 0) &&
1813                             (fat_entries[i].flags & FILE_IS_ACTIVE) &&
1814                             (!entry_found)) {
1815                                 entry_found = true;
1816                                 fh->rpmb_fat_address = fat_address;
1817                                 memcpy(&fh->fat_entry, &fat_entries[i],
1818                                        sizeof(struct rpmb_fat_entry));
1819                                 if (!p)
1820                                         break;
1821                         }
1822
1823                         /* Add existing files to memory pool. (write) */
1824                         if (p) {
1825                                 if ((fat_entries[i].flags & FILE_IS_ACTIVE) &&
1826                                     (fat_entries[i].data_size > 0)) {
1827
1828                                         mm = tee_mm_alloc2
1829                                                 (p,
1830                                                  fat_entries[i].start_address,
1831                                                  fat_entries[i].data_size);
1832                                         if (!mm) {
1833                                                 res = TEE_ERROR_OUT_OF_MEMORY;
1834                                                 goto out;
1835                                         }
1836                                 }
1837
1838                                 /* Unused FAT entries can be reused (write) */
1839                                 if (((fat_entries[i].flags & FILE_IS_ACTIVE) ==
1840                                      0) && (fh->rpmb_fat_address == 0)) {
1841                                         fh->rpmb_fat_address = fat_address;
1842                                         memcpy(&fh->fat_entry, &fat_entries[i],
1843                                                sizeof(struct rpmb_fat_entry));
1844                                 }
1845                         }
1846
1847                         if ((fat_entries[i].flags & FILE_IS_LAST_ENTRY) != 0) {
1848                                 last_entry_found = true;
1849
1850                                 /*
1851                                  * If the last entry was reached and was chosen
1852                                  * by the previous check, then the FAT needs to
1853                                  * be expanded.
1854                                  * fh->rpmb_fat_address is the address chosen
1855                                  * to store the files FAT entry and fat_address
1856                                  * is the current FAT entry address being
1857                                  * compared.
1858                                  */
1859                                 if (p && fh->rpmb_fat_address == fat_address)
1860                                         expand_fat = true;
1861                                 break;
1862                         }
1863
1864                         /* Move to next fat_entry. */
1865                         fat_address += sizeof(struct rpmb_fat_entry);
1866                 }
1867         }
1868
1869         /*
1870          * Represent the FAT table in the pool.
1871          */
1872         if (p) {
1873                 /*
1874                  * Since fat_address is the start of the last entry it needs to
1875                  * be moved up by an entry.
1876                  */
1877                 fat_address += sizeof(struct rpmb_fat_entry);
1878
1879                 /* Make room for yet a FAT entry and add to memory pool. */
1880                 if (expand_fat)
1881                         fat_address += sizeof(struct rpmb_fat_entry);
1882
1883                 mm = tee_mm_alloc2(p, RPMB_STORAGE_START_ADDRESS, fat_address);
1884                 if (!mm) {
1885                         res = TEE_ERROR_OUT_OF_MEMORY;
1886                         goto out;
1887                 }
1888
1889                 if (expand_fat) {
1890                         /*
1891                          * Point fat_address to the beginning of the new
1892                          * entry.
1893                          */
1894                         fat_address -= sizeof(struct rpmb_fat_entry);
1895                         memset(&last_fh, 0, sizeof(last_fh));
1896                         last_fh.fat_entry.flags = FILE_IS_LAST_ENTRY;
1897                         last_fh.rpmb_fat_address = fat_address;
1898                         res = write_fat_entry(&last_fh, true);
1899                         if (res != TEE_SUCCESS)
1900                                 goto out;
1901                 }
1902         }
1903
1904         if (fh->filename && !fh->rpmb_fat_address)
1905                 res = TEE_ERROR_ITEM_NOT_FOUND;
1906
1907 out:
1908         free(fat_entries);
1909         return res;
1910 }
1911
1912 static TEE_Result generate_fek(struct rpmb_fat_entry *fe)
1913 {
1914         TEE_Result res;
1915
1916 again:
1917         res = tee_fs_generate_fek(fe->fek, sizeof(fe->fek));
1918         if (res != TEE_SUCCESS)
1919                 return res;
1920
1921         if (is_zero(fe->fek, sizeof(fe->fek)))
1922                 goto again;
1923
1924         return res;
1925 }
1926
1927 static TEE_Result rpmb_fs_open_internal(const char *file, bool create,
1928                                         struct tee_file_handle **ret_fh)
1929 {
1930         struct rpmb_file_handle *fh = NULL;
1931         size_t filelen;
1932         tee_mm_pool_t p;
1933         bool pool_result;
1934         TEE_Result res = TEE_ERROR_GENERIC;
1935
1936         mutex_lock(&rpmb_mutex);
1937
1938         filelen = strlen(file);
1939         if (filelen >= TEE_RPMB_FS_FILENAME_LENGTH - 1 || filelen == 0) {
1940                 res = TEE_ERROR_BAD_PARAMETERS;
1941                 goto out;
1942         }
1943
1944         if (file[filelen - 1] == '/') {
1945                 res = TEE_ERROR_BAD_PARAMETERS;
1946                 goto out;
1947         }
1948
1949         fh = alloc_file_handle(file);
1950         if (!fh) {
1951                 res = TEE_ERROR_OUT_OF_MEMORY;
1952                 goto out;
1953         }
1954
1955         /* We need to do setup in order to make sure fs_par is filled in */
1956         res = rpmb_fs_setup();
1957         if (res != TEE_SUCCESS)
1958                 goto out;
1959
1960         if (create) {
1961                 /* Upper memory allocation must be used for RPMB_FS. */
1962                 pool_result = tee_mm_init(&p,
1963                                           RPMB_STORAGE_START_ADDRESS,
1964                                           fs_par->max_rpmb_address,
1965                                           RPMB_BLOCK_SIZE_SHIFT,
1966                                           TEE_MM_POOL_HI_ALLOC);
1967
1968                 if (!pool_result) {
1969                         res = TEE_ERROR_OUT_OF_MEMORY;
1970                         goto out;
1971                 }
1972
1973                 res = read_fat(fh, &p);
1974                 tee_mm_final(&p);
1975                 if (res != TEE_SUCCESS)
1976                         goto out;
1977         } else {
1978                 res = read_fat(fh, NULL);
1979                 if (res != TEE_SUCCESS)
1980                         goto out;
1981         }
1982
1983         /*
1984          * If this is opened with create and the entry found was not active
1985          * then this is a new file and the FAT entry must be written
1986          */
1987         if (create) {
1988                 if ((fh->fat_entry.flags & FILE_IS_ACTIVE) == 0) {
1989                         memset(&fh->fat_entry, 0,
1990                                 sizeof(struct rpmb_fat_entry));
1991                         memcpy(fh->fat_entry.filename, file, strlen(file));
1992                         /* Start address and size are 0 */
1993                         fh->fat_entry.flags = FILE_IS_ACTIVE;
1994
1995                         res = generate_fek(&fh->fat_entry);
1996                         if (res != TEE_SUCCESS)
1997                                 goto out;
1998                         DMSG("GENERATE FEK key: %p",
1999                              (void *)fh->fat_entry.fek);
2000                         DHEXDUMP(fh->fat_entry.fek, sizeof(fh->fat_entry.fek));
2001
2002                         res = write_fat_entry(fh, true);
2003                         if (res != TEE_SUCCESS)
2004                                 goto out;
2005                 }
2006         }
2007
2008         res = TEE_SUCCESS;
2009
2010 out:
2011         if (res == TEE_SUCCESS)
2012                 *ret_fh = (struct tee_file_handle *)fh;
2013         else
2014                 free(fh);
2015
2016         mutex_unlock(&rpmb_mutex);
2017         return res;
2018 }
2019
2020 static void rpmb_fs_close(struct tee_file_handle **tfh)
2021 {
2022         struct rpmb_file_handle *fh = (struct rpmb_file_handle *)*tfh;
2023
2024         free(fh);
2025         *tfh = NULL;
2026 }
2027
2028 static TEE_Result rpmb_fs_read(struct tee_file_handle *tfh, void *buf,
2029                                size_t *len)
2030 {
2031         TEE_Result res;
2032         struct rpmb_file_handle *fh = (struct rpmb_file_handle *)tfh;
2033         size_t size = *len;
2034
2035         if (!size)
2036                 return TEE_SUCCESS;
2037
2038         mutex_lock(&rpmb_mutex);
2039
2040         dump_fh(fh);
2041
2042         res = read_fat(fh, NULL);
2043         if (res != TEE_SUCCESS)
2044                 goto out;
2045
2046         if (fh->pos >= fh->fat_entry.data_size) {
2047                 *len = 0;
2048                 goto out;
2049         }
2050
2051         size = MIN(size, fh->fat_entry.data_size - fh->pos);
2052         if (size) {
2053                 res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID,
2054                                     fh->fat_entry.start_address + fh->pos, buf,
2055                                     size, fh->fat_entry.fek);
2056                 if (res != TEE_SUCCESS)
2057                         goto out;
2058         }
2059         *len = size;
2060         fh->pos += size;
2061
2062 out:
2063         mutex_unlock(&rpmb_mutex);
2064         return res;
2065 }
2066
2067 static TEE_Result rpmb_fs_write(struct tee_file_handle *tfh, const void *buf,
2068                                 size_t size)
2069 {
2070         TEE_Result res;
2071         struct rpmb_file_handle *fh = (struct rpmb_file_handle *)tfh;
2072         tee_mm_pool_t p;
2073         bool pool_result = false;
2074         tee_mm_entry_t *mm;
2075         size_t end;
2076         size_t newsize;
2077         uint8_t *newbuf = NULL;
2078         uintptr_t newaddr;
2079         uint32_t start_addr;
2080
2081         if (!size)
2082                 return TEE_SUCCESS;
2083
2084         mutex_lock(&rpmb_mutex);
2085
2086         if (!fs_par) {
2087                 res = TEE_ERROR_GENERIC;
2088                 goto out;
2089         }
2090
2091         dump_fh(fh);
2092
2093         /* Upper memory allocation must be used for RPMB_FS. */
2094         pool_result = tee_mm_init(&p,
2095                                   RPMB_STORAGE_START_ADDRESS,
2096                                   fs_par->max_rpmb_address,
2097                                   RPMB_BLOCK_SIZE_SHIFT,
2098                                   TEE_MM_POOL_HI_ALLOC);
2099         if (!pool_result) {
2100                 res = TEE_ERROR_OUT_OF_MEMORY;
2101                 goto out;
2102         }
2103
2104         res = read_fat(fh, &p);
2105         if (res != TEE_SUCCESS)
2106                 goto out;
2107
2108         if (fh->fat_entry.flags & FILE_IS_LAST_ENTRY)
2109                 panic("invalid last entry flag");
2110
2111         end = fh->pos + size;
2112         start_addr = fh->fat_entry.start_address + fh->pos;
2113
2114         if (end <= fh->fat_entry.data_size &&
2115             tee_rpmb_write_is_atomic(CFG_RPMB_FS_DEV_ID, start_addr, size)) {
2116
2117                 DMSG("Updating data in-place");
2118                 res = tee_rpmb_write(CFG_RPMB_FS_DEV_ID, start_addr, buf,
2119                                      size, fh->fat_entry.fek);
2120                 if (res != TEE_SUCCESS)
2121                         goto out;
2122         } else {
2123                 /*
2124                  * File must be extended, or update cannot be atomic: allocate,
2125                  * read, update, write.
2126                  */
2127
2128                 DMSG("Need to re-allocate");
2129                 newsize = MAX(end, fh->fat_entry.data_size);
2130                 mm = tee_mm_alloc(&p, newsize);
2131                 newbuf = calloc(newsize, 1);
2132                 if (!mm || !newbuf) {
2133                         res = TEE_ERROR_OUT_OF_MEMORY;
2134                         goto out;
2135                 }
2136
2137                 if (fh->fat_entry.data_size) {
2138                         res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID,
2139                                             fh->fat_entry.start_address,
2140                                             newbuf, fh->fat_entry.data_size,
2141                                             fh->fat_entry.fek);
2142                         if (res != TEE_SUCCESS)
2143                                 goto out;
2144                 }
2145
2146                 memcpy(newbuf + fh->pos, buf, size);
2147
2148                 newaddr = tee_mm_get_smem(mm);
2149                 res = tee_rpmb_write(CFG_RPMB_FS_DEV_ID, newaddr, newbuf,
2150                                      newsize, fh->fat_entry.fek);
2151                 if (res != TEE_SUCCESS)
2152                         goto out;
2153
2154                 fh->fat_entry.data_size = newsize;
2155                 fh->fat_entry.start_address = newaddr;
2156                 res = write_fat_entry(fh, true);
2157                 if (res != TEE_SUCCESS)
2158                         goto out;
2159         }
2160
2161         fh->pos += size;
2162 out:
2163         mutex_unlock(&rpmb_mutex);
2164         if (pool_result)
2165                 tee_mm_final(&p);
2166         if (newbuf)
2167                 free(newbuf);
2168
2169         return res;
2170 }
2171
2172 static TEE_Result rpmb_fs_seek(struct tee_file_handle *tfh, int32_t offset,
2173                                TEE_Whence whence, int32_t *new_offs)
2174
2175 {
2176         struct rpmb_file_handle *fh = (struct rpmb_file_handle *)tfh;
2177         TEE_Result res;
2178         tee_fs_off_t new_pos;
2179
2180         mutex_lock(&rpmb_mutex);
2181
2182         res = read_fat(fh, NULL);
2183         if (res != TEE_SUCCESS)
2184                 goto out;
2185
2186         switch (whence) {
2187         case TEE_DATA_SEEK_SET:
2188                 new_pos = offset;
2189                 break;
2190
2191         case TEE_DATA_SEEK_CUR:
2192                 new_pos = fh->pos + offset;
2193                 break;
2194
2195         case TEE_DATA_SEEK_END:
2196                 new_pos = fh->fat_entry.data_size + offset;
2197                 break;
2198
2199         default:
2200                 res = TEE_ERROR_BAD_PARAMETERS;
2201                 goto out;
2202         }
2203
2204         if (new_pos < 0)
2205                 new_pos = 0;
2206
2207         if (new_pos > TEE_DATA_MAX_POSITION) {
2208                 EMSG("Position is beyond TEE_DATA_MAX_POSITION");
2209                 res = TEE_ERROR_BAD_PARAMETERS;
2210                 goto out;
2211         }
2212
2213         fh->pos = new_pos;
2214         if (new_offs)
2215                 *new_offs = new_pos;
2216 out:
2217         mutex_unlock(&rpmb_mutex);
2218         return res;
2219 }
2220
2221 static TEE_Result rpmb_fs_remove(const char *filename)
2222 {
2223         TEE_Result res = TEE_ERROR_GENERIC;
2224         struct rpmb_file_handle *fh = NULL;
2225
2226         mutex_lock(&rpmb_mutex);
2227
2228         if (!filename || strlen(filename) >= TEE_RPMB_FS_FILENAME_LENGTH - 1) {
2229                 res = TEE_ERROR_BAD_PARAMETERS;
2230                 goto out;
2231         }
2232
2233         fh = alloc_file_handle(filename);
2234         if (!fh) {
2235                 res = TEE_ERROR_OUT_OF_MEMORY;
2236                 goto out;
2237         }
2238
2239         res = read_fat(fh, NULL);
2240         if (res != TEE_SUCCESS)
2241                 goto out;
2242
2243         /* Clear this file entry. */
2244         memset(&fh->fat_entry, 0, sizeof(struct rpmb_fat_entry));
2245         res = write_fat_entry(fh, false);
2246
2247 out:
2248         mutex_unlock(&rpmb_mutex);
2249         free(fh);
2250         return res;
2251 }
2252
2253 static  TEE_Result rpmb_fs_rename(const char *old_name, const char *new_name,
2254                                   bool overwrite)
2255 {
2256         TEE_Result res = TEE_ERROR_GENERIC;
2257         struct rpmb_file_handle *fh_old = NULL;
2258         struct rpmb_file_handle *fh_new = NULL;
2259         uint32_t old_len;
2260         uint32_t new_len;
2261
2262         mutex_lock(&rpmb_mutex);
2263
2264         if (!old_name || !new_name) {
2265                 res = TEE_ERROR_BAD_PARAMETERS;
2266                 goto out;
2267         }
2268
2269         old_len = strlen(old_name);
2270         new_len = strlen(new_name);
2271
2272         if ((old_len >= TEE_RPMB_FS_FILENAME_LENGTH - 1) ||
2273             (new_len >= TEE_RPMB_FS_FILENAME_LENGTH - 1) || (new_len == 0)) {
2274
2275                 res = TEE_ERROR_BAD_PARAMETERS;
2276                 goto out;
2277         }
2278
2279         fh_old = alloc_file_handle(old_name);
2280         if (!fh_old) {
2281                 res = TEE_ERROR_OUT_OF_MEMORY;
2282                 goto out;
2283         }
2284
2285         fh_new = alloc_file_handle(new_name);
2286         if (!fh_new) {
2287                 res = TEE_ERROR_OUT_OF_MEMORY;
2288                 goto out;
2289         }
2290
2291         res = read_fat(fh_old, NULL);
2292         if (res != TEE_SUCCESS)
2293                 goto out;
2294
2295         res = read_fat(fh_new, NULL);
2296         if (res == TEE_SUCCESS) {
2297                 if (!overwrite) {
2298                         res = TEE_ERROR_BAD_PARAMETERS;
2299                         goto out;
2300                 }
2301
2302                 /* Clear this file entry. */
2303                 memset(&fh_new->fat_entry, 0, sizeof(struct rpmb_fat_entry));
2304                 res = write_fat_entry(fh_new, false);
2305                 if (res != TEE_SUCCESS)
2306                         goto out;
2307         }
2308
2309         memset(fh_old->fat_entry.filename, 0, TEE_RPMB_FS_FILENAME_LENGTH);
2310         memcpy(fh_old->fat_entry.filename, new_name, new_len);
2311
2312         res = write_fat_entry(fh_old, false);
2313
2314 out:
2315         mutex_unlock(&rpmb_mutex);
2316         free(fh_old);
2317         free(fh_new);
2318
2319         return res;
2320 }
2321
2322 static TEE_Result rpmb_fs_truncate(struct tee_file_handle *tfh, size_t length)
2323 {
2324         struct rpmb_file_handle *fh = (struct rpmb_file_handle *)tfh;
2325         tee_mm_pool_t p;
2326         bool pool_result = false;
2327         tee_mm_entry_t *mm;
2328         uint32_t newsize;
2329         uint8_t *newbuf = NULL;
2330         uintptr_t newaddr;
2331         TEE_Result res = TEE_ERROR_GENERIC;
2332
2333         mutex_lock(&rpmb_mutex);
2334
2335         if (length > INT32_MAX) {
2336                 res = TEE_ERROR_BAD_PARAMETERS;
2337                 goto out;
2338         }
2339         newsize = length;
2340
2341         res = read_fat(fh, NULL);
2342         if (res != TEE_SUCCESS)
2343                 goto out;
2344
2345         if (newsize > fh->fat_entry.data_size) {
2346                 /* Extend file */
2347
2348                 pool_result = tee_mm_init(&p,
2349                                           RPMB_STORAGE_START_ADDRESS,
2350                                           fs_par->max_rpmb_address,
2351                                           RPMB_BLOCK_SIZE_SHIFT,
2352                                           TEE_MM_POOL_HI_ALLOC);
2353                 if (!pool_result) {
2354                         res = TEE_ERROR_OUT_OF_MEMORY;
2355                         goto out;
2356                 }
2357                 res = read_fat(fh, &p);
2358                 if (res != TEE_SUCCESS)
2359                         goto out;
2360
2361                 mm = tee_mm_alloc(&p, newsize);
2362                 newbuf = calloc(newsize, 1);
2363                 if (!mm || !newbuf) {
2364                         res = TEE_ERROR_OUT_OF_MEMORY;
2365                         goto out;
2366                 }
2367
2368                 if (fh->fat_entry.data_size) {
2369                         res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID,
2370                                             fh->fat_entry.start_address,
2371                                             newbuf, fh->fat_entry.data_size,
2372                                             fh->fat_entry.fek);
2373                         if (res != TEE_SUCCESS)
2374                                 goto out;
2375                 }
2376
2377                 newaddr = tee_mm_get_smem(mm);
2378                 res = tee_rpmb_write(CFG_RPMB_FS_DEV_ID, newaddr, newbuf,
2379                                      newsize, fh->fat_entry.fek);
2380                 if (res != TEE_SUCCESS)
2381                         goto out;
2382
2383         } else {
2384                 /* Don't change file location */
2385                 newaddr = fh->fat_entry.start_address;
2386         }
2387
2388         /* fh->pos is unchanged */
2389         fh->fat_entry.data_size = newsize;
2390         fh->fat_entry.start_address = newaddr;
2391         res = write_fat_entry(fh, true);
2392
2393 out:
2394         mutex_unlock(&rpmb_mutex);
2395         if (pool_result)
2396                 tee_mm_final(&p);
2397         if (newbuf)
2398                 free(newbuf);
2399
2400         return res;
2401 }
2402
2403 static void rpmb_fs_dir_free(struct tee_fs_dir *dir)
2404 {
2405         struct tee_rpmb_fs_dirent *e;
2406
2407         if (!dir)
2408                 return;
2409
2410         free(dir->current);
2411
2412         while ((e = SIMPLEQ_FIRST(&dir->next))) {
2413                 SIMPLEQ_REMOVE_HEAD(&dir->next, link);
2414                 free(e);
2415         }
2416 }
2417
2418 static TEE_Result rpmb_fs_dir_populate(const char *path,
2419                                        struct tee_fs_dir *dir)
2420 {
2421         struct tee_rpmb_fs_dirent *current = NULL;
2422         struct rpmb_fat_entry *fat_entries = NULL;
2423         uint32_t fat_address;
2424         uint32_t filelen;
2425         char *filename;
2426         int i;
2427         bool last_entry_found = false;
2428         bool matched;
2429         struct tee_rpmb_fs_dirent *next = NULL;
2430         uint32_t pathlen;
2431         TEE_Result res = TEE_ERROR_GENERIC;
2432         uint32_t size;
2433         char temp;
2434
2435         mutex_lock(&rpmb_mutex);
2436
2437         res = rpmb_fs_setup();
2438         if (res != TEE_SUCCESS)
2439                 goto out;
2440
2441         res = get_fat_start_address(&fat_address);
2442         if (res != TEE_SUCCESS)
2443                 goto out;
2444
2445         size = N_ENTRIES * sizeof(struct rpmb_fat_entry);
2446         fat_entries = malloc(size);
2447         if (!fat_entries) {
2448                 res = TEE_ERROR_OUT_OF_MEMORY;
2449                 goto out;
2450         }
2451
2452         pathlen = strlen(path);
2453         while (!last_entry_found) {
2454                 res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID, fat_address,
2455                                     (uint8_t *)fat_entries, size, NULL);
2456                 if (res != TEE_SUCCESS)
2457                         goto out;
2458
2459                 for (i = 0; i < N_ENTRIES; i++) {
2460                         filename = fat_entries[i].filename;
2461                         if (fat_entries[i].flags & FILE_IS_ACTIVE) {
2462                                 matched = false;
2463                                 filelen = strlen(filename);
2464                                 if (filelen > pathlen) {
2465                                         temp = filename[pathlen];
2466                                         filename[pathlen] = '\0';
2467                                         if (strcmp(filename, path) == 0)
2468                                                 matched = true;
2469
2470                                         filename[pathlen] = temp;
2471                                 }
2472
2473                                 if (matched) {
2474                                         next = malloc(sizeof(*next));
2475                                         if (!next) {
2476                                                 res = TEE_ERROR_OUT_OF_MEMORY;
2477                                                 goto out;
2478                                         }
2479
2480                                         memset(next, 0, sizeof(*next));
2481                                         next->entry.d_name = next->name;
2482                                         memcpy(next->name,
2483                                                 &filename[pathlen],
2484                                                 filelen - pathlen);
2485
2486                                         SIMPLEQ_INSERT_TAIL(&dir->next, next,
2487                                                             link);
2488                                         current = next;
2489                                 }
2490                         }
2491
2492                         if (fat_entries[i].flags & FILE_IS_LAST_ENTRY) {
2493                                 last_entry_found = true;
2494                                 break;
2495                         }
2496
2497                         /* Move to next fat_entry. */
2498                         fat_address += sizeof(struct rpmb_fat_entry);
2499                 }
2500         }
2501
2502         if (current)
2503                 res = TEE_SUCCESS;
2504         else
2505                 res = TEE_ERROR_ITEM_NOT_FOUND; /* No directories were found. */
2506
2507 out:
2508         mutex_unlock(&rpmb_mutex);
2509         if (res != TEE_SUCCESS)
2510                 rpmb_fs_dir_free(dir);
2511         if (fat_entries)
2512                 free(fat_entries);
2513
2514         return res;
2515 }
2516
2517 static TEE_Result rpmb_fs_opendir(const char *path, struct tee_fs_dir **dir)
2518 {
2519         uint32_t len;
2520         uint32_t max_size;
2521         char path_local[TEE_RPMB_FS_FILENAME_LENGTH];
2522         TEE_Result res = TEE_ERROR_GENERIC;
2523         struct tee_fs_dir *rpmb_dir = NULL;
2524
2525         if (!path || !dir) {
2526                 res = TEE_ERROR_BAD_PARAMETERS;
2527                 goto out;
2528         }
2529
2530         /*
2531          * There must be room for at least the NULL char and a char for the
2532          * filename after the path.
2533          */
2534         max_size = TEE_RPMB_FS_FILENAME_LENGTH - 2;
2535         len = strlen(path);
2536         if (len > max_size || len == 0) {
2537                 res = TEE_ERROR_BAD_PARAMETERS;
2538                 goto out;
2539         }
2540
2541         memset(path_local, 0, sizeof(path_local));
2542         memcpy(path_local, path, len);
2543
2544         /* Add a slash to correctly match the full directory name. */
2545         if (path_local[len - 1] != '/')
2546                 path_local[len] = '/';
2547
2548         rpmb_dir = calloc(1, sizeof(*rpmb_dir));
2549         if (!rpmb_dir) {
2550                 res = TEE_ERROR_OUT_OF_MEMORY;
2551                 goto out;
2552         }
2553         SIMPLEQ_INIT(&rpmb_dir->next);
2554
2555         res = rpmb_fs_dir_populate(path_local, rpmb_dir);
2556         if (res != TEE_SUCCESS) {
2557                 free(rpmb_dir);
2558                 rpmb_dir = NULL;
2559                 goto out;
2560         }
2561
2562         *dir = rpmb_dir;
2563
2564 out:
2565         return res;
2566 }
2567
2568 static TEE_Result rpmb_fs_readdir(struct tee_fs_dir *dir,
2569                                   struct tee_fs_dirent **ent)
2570 {
2571         if (!dir)
2572                 return TEE_ERROR_GENERIC;
2573
2574         free(dir->current);
2575
2576         dir->current = SIMPLEQ_FIRST(&dir->next);
2577         if (!dir->current)
2578                 return TEE_ERROR_ITEM_NOT_FOUND;
2579
2580         SIMPLEQ_REMOVE_HEAD(&dir->next, link);
2581
2582         *ent = &dir->current->entry;
2583         return TEE_SUCCESS;
2584 }
2585
2586 static void rpmb_fs_closedir(struct tee_fs_dir *dir)
2587 {
2588         if (dir) {
2589                 rpmb_fs_dir_free(dir);
2590                 free(dir);
2591         }
2592 }
2593
2594 static TEE_Result rpmb_fs_open(const char *file, struct tee_file_handle **fh)
2595 {
2596         return rpmb_fs_open_internal(file, false, fh);
2597 }
2598
2599 static TEE_Result rpmb_fs_create(const char *file, struct tee_file_handle **fh)
2600 {
2601         return rpmb_fs_open_internal(file, true, fh);
2602 }
2603
2604 const struct tee_file_operations rpmb_fs_ops = {
2605         .open = rpmb_fs_open,
2606         .create = rpmb_fs_create,
2607         .close = rpmb_fs_close,
2608         .read = rpmb_fs_read,
2609         .write = rpmb_fs_write,
2610         .seek = rpmb_fs_seek,
2611         .truncate = rpmb_fs_truncate,
2612         .rename = rpmb_fs_rename,
2613         .remove = rpmb_fs_remove,
2614         .opendir = rpmb_fs_opendir,
2615         .closedir = rpmb_fs_closedir,
2616         .readdir = rpmb_fs_readdir,
2617 };