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