[v2,13/15] optee: support routing of rpmb data frames to mmc

Message ID 20180823104334.16083-14-jens.wiklander@linaro.org
State Superseded
Headers show
Series
  • AVB using OP-TEE
Related show

Commit Message

Jens Wiklander Aug. 23, 2018, 10:43 a.m.
Adds support in optee supplicant to route signed (MACed) RPMB frames
from OP-TEE Secure OS to MMC and vice versa to manipulate the RPMB
partition.

Tested-by: Igor Opaniuk <igor.opaniuk@linaro.org>

Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>

---
 drivers/tee/optee/Makefile        |   1 +
 drivers/tee/optee/core.c          |   8 ++
 drivers/tee/optee/optee_private.h |  31 ++++-
 drivers/tee/optee/rpmb.c          | 184 ++++++++++++++++++++++++++++++
 drivers/tee/optee/supplicant.c    |   3 +
 5 files changed, 226 insertions(+), 1 deletion(-)
 create mode 100644 drivers/tee/optee/rpmb.c

-- 
2.17.1

Comments

Simon Glass Aug. 30, 2018, 12:29 a.m. | #1
Hi Jens,

On 23 August 2018 at 04:43, Jens Wiklander <jens.wiklander@linaro.org> wrote:
> Adds support in optee supplicant to route signed (MACed) RPMB frames
> from OP-TEE Secure OS to MMC and vice versa to manipulate the RPMB
> partition.
>
> Tested-by: Igor Opaniuk <igor.opaniuk@linaro.org>
> Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>
> ---
>  drivers/tee/optee/Makefile        |   1 +
>  drivers/tee/optee/core.c          |   8 ++
>  drivers/tee/optee/optee_private.h |  31 ++++-
>  drivers/tee/optee/rpmb.c          | 184 ++++++++++++++++++++++++++++++
>  drivers/tee/optee/supplicant.c    |   3 +
>  5 files changed, 226 insertions(+), 1 deletion(-)
>  create mode 100644 drivers/tee/optee/rpmb.c
>
> diff --git a/drivers/tee/optee/Makefile b/drivers/tee/optee/Makefile
> index 6148feb474a5..928d3f80027f 100644
> --- a/drivers/tee/optee/Makefile
> +++ b/drivers/tee/optee/Makefile
> @@ -2,3 +2,4 @@
>
>  obj-y += core.o
>  obj-y += supplicant.o
> +obj-$(CONFIG_SUPPORT_EMMC_RPMB) += rpmb.o
> diff --git a/drivers/tee/optee/core.c b/drivers/tee/optee/core.c
> index f2d92d96551b..e297d206af3a 100644
> --- a/drivers/tee/optee/core.c
> +++ b/drivers/tee/optee/core.c
> @@ -280,6 +280,13 @@ static u32 do_call_with_arg(struct udevice *dev, struct optee_msg_arg *arg)
>                         param.a3 = res.a3;
>                         handle_rpc(dev, &param, &page_list);
>                 } else {
> +                       /*
> +                        * In case we've accessed RPMB to serve an RPC
> +                        * request we need to restore the previously
> +                        * selected partition as the caller may expect it
> +                        * to remain unchanged.
> +                        */
> +                       optee_suppl_rpmb_release(dev);
>                         return call_err_to_res(res.a0);
>                 }
>         }
> @@ -611,4 +618,5 @@ U_BOOT_DRIVER(optee) = {
>         .probe = optee_probe,
>         .ops = &optee_ops,
>         .platdata_auto_alloc_size = sizeof(struct optee_pdata),
> +       .priv_auto_alloc_size = sizeof(struct optee_private),
>  };
> diff --git a/drivers/tee/optee/optee_private.h b/drivers/tee/optee/optee_private.h
> index daa470f812a9..b76979d21011 100644
> --- a/drivers/tee/optee/optee_private.h
> +++ b/drivers/tee/optee/optee_private.h
> @@ -6,7 +6,36 @@
>  #ifndef __OPTEE_PRIVATE_H
>  #define __OPTEE_PRIVATE_H
>
> +#include <tee.h>
> +#include <log.h>
> +
> +struct optee_private {

doc comment

> +       struct mmc *rpmb_mmc;
> +       int rpmb_dev_id;
> +       char rpmb_original_part;

Why is this char? Are you trying to save memory? I doubt it will work :-)


> +};
> +
> +struct optee_msg_arg;
> +
> +void optee_suppl_cmd(struct udevice *dev, struct tee_shm *shm_arg,
> +                    void **page_list);

Function comments.

> +
> +#ifdef CONFIG_SUPPORT_EMMC_RPMB
> +void optee_suppl_cmd_rpmb(struct udevice *dev, struct optee_msg_arg *arg);
> +void optee_suppl_rpmb_release(struct udevice *dev);
> +#else
> +static inline void optee_suppl_cmd_rpmb(struct udevice *dev,
> +                                       struct optee_msg_arg *arg)
> +{
> +       debug("OPTEE_MSG_RPC_CMD_RPMB not implemented\n");
> +       arg->ret = TEE_ERROR_NOT_IMPLEMENTED;
> +}
> +
> +static inline void optee_suppl_rpmb_release(struct udevice *dev)
> +{
> +}
> +#endif
> +
>  void *optee_alloc_and_init_page_list(void *buf, ulong len, u64 *phys_buf_ptr);
> -void optee_suppl_cmd(struct udevice *dev, void *shm, void **page_list);
>
>  #endif /*__OPTEE_PRIVATE_H*/
> diff --git a/drivers/tee/optee/rpmb.c b/drivers/tee/optee/rpmb.c
> new file mode 100644
> index 000000000000..c1447a5561c2
> --- /dev/null
> +++ b/drivers/tee/optee/rpmb.c
> @@ -0,0 +1,184 @@
> +// SPDX-License-Identifier: BSD-2-Clause
> +/*
> + * Copyright (c) 2018 Linaro Limited
> + */
> +
> +#include <common.h>
> +#include <log.h>
> +#include <tee.h>
> +#include <mmc.h>
> +
> +#include "optee_msg.h"
> +#include "optee_private.h"
> +
> +/*
> + * Request and response definitions must be in sync with the secure side of
> + * OP-TEE.
> + */
> +
> +/* Request */
> +struct rpmb_req {
> +       u16 cmd;
> +#define RPMB_CMD_DATA_REQ      0x00
> +#define RPMB_CMD_GET_DEV_INFO  0x01
> +       u16 dev_id;
> +       u16 block_count;
> +       /* Optional data frames (rpmb_data_frame) follow */
> +};
> +
> +#define RPMB_REQ_DATA(req) ((void *)((struct rpmb_req *)(req) + 1))
> +
> +/* Response to device info request */
> +struct rpmb_dev_info {
> +       u8 cid[16];
> +       u8 rpmb_size_mult;      /* EXT CSD-slice 168: RPMB Size */
> +       u8 rel_wr_sec_c;        /* EXT CSD-slice 222: Reliable Write Sector */
> +                               /*                    Count */
> +       u8 ret_code;
> +#define RPMB_CMD_GET_DEV_INFO_RET_OK     0x00
> +#define RPMB_CMD_GET_DEV_INFO_RET_ERROR  0x01
> +};
> +
> +static void release_mmc(struct optee_private *priv)
> +{
> +       int rc;
> +
> +       if (!priv->rpmb_mmc)
> +               return;
> +
> +       rc = blk_select_hwpart_devnum(IF_TYPE_MMC, priv->rpmb_dev_id,
> +                                     priv->rpmb_original_part);
> +       if (rc)
> +               debug("%s: blk_select_hwpart_devnum() failed: %d\n",
> +                     __func__, rc);
> +
> +       priv->rpmb_mmc = NULL;
> +}
> +
> +static struct mmc *get_mmc(struct optee_private *priv, int dev_id)
> +{
> +       struct mmc *mmc;
> +       int rc;
> +
> +       if (priv->rpmb_mmc && priv->rpmb_dev_id == dev_id)
> +               return priv->rpmb_mmc;
> +
> +       release_mmc(priv);
> +
> +       mmc = find_mmc_device(dev_id);
> +       if (!mmc) {
> +               debug("Cannot find RPMB device\n");
> +               return NULL;
> +       }
> +       if (!(mmc->version & MMC_VERSION_MMC)) {
> +               debug("Device id %d is not an eMMC device\n", dev_id);
> +               return NULL;
> +       }
> +       if (mmc->version < MMC_VERSION_4_41) {
> +               debug("Device id %d: RPMB not supported before version 4.41\n",
> +                     dev_id);
> +               return NULL;
> +       }
> +
> +#ifdef CONFIG_BLK

You shouldn't support !CONFIG_BLK. It is going away in the next release.

> +       priv->rpmb_original_part = mmc_get_blk_desc(mmc)->hwpart;
> +#else
> +       priv->rpmb_original_part = mmc->block_dev.hwpart;
> +#endif
> +
> +       rc = blk_select_hwpart_devnum(IF_TYPE_MMC, dev_id, MMC_PART_RPMB);
> +       if (rc) {
> +               debug("Device id %d: cannot select RPMB partition: %d\n",
> +                     dev_id, rc);
> +               return NULL;
> +       }
> +
> +       priv->rpmb_mmc = mmc;
> +       priv->rpmb_dev_id = dev_id;
> +       return mmc;
> +}
> +
> +static u32 rpmb_get_dev_info(u16 dev_id, struct rpmb_dev_info *info)
> +{
> +       struct mmc *mmc = find_mmc_device(dev_id);
> +
> +       if (!mmc)
> +               return TEE_ERROR_ITEM_NOT_FOUND;
> +
> +       if (!mmc->ext_csd)
> +               return TEE_ERROR_GENERIC;
> +
> +       memcpy(info->cid, mmc->cid, sizeof(info->cid));
> +       info->rel_wr_sec_c = mmc->ext_csd[222];
> +       info->rpmb_size_mult = mmc->ext_csd[168];
> +       info->ret_code = RPMB_CMD_GET_DEV_INFO_RET_OK;
> +
> +       return TEE_SUCCESS;
> +}
> +
> +static u32 rpmb_process_request(struct optee_private *priv, void *req,
> +                               ulong req_size, void *rsp, ulong rsp_size)
> +{
> +       struct rpmb_req *sreq = req;
> +       struct mmc *mmc;
> +
> +       if (req_size < sizeof(*sreq))
> +               return TEE_ERROR_BAD_PARAMETERS;
> +
> +       switch (sreq->cmd) {
> +       case RPMB_CMD_DATA_REQ:
> +               mmc = get_mmc(priv, sreq->dev_id);
> +               if (!mmc)
> +                       return TEE_ERROR_ITEM_NOT_FOUND;
> +               if (mmc_rpmb_route_frames(mmc, RPMB_REQ_DATA(req),
> +                                         req_size - sizeof(struct rpmb_req),
> +                                         rsp, rsp_size))
> +                       return TEE_ERROR_BAD_PARAMETERS;
> +               return TEE_SUCCESS;
> +
> +       case RPMB_CMD_GET_DEV_INFO:
> +               if (req_size != sizeof(struct rpmb_req) ||
> +                   rsp_size != sizeof(struct rpmb_dev_info)) {
> +                       debug("Invalid req/rsp size\n");
> +                       return TEE_ERROR_BAD_PARAMETERS;
> +               }
> +               return rpmb_get_dev_info(sreq->dev_id, rsp);
> +
> +       default:
> +               debug("Unsupported RPMB command: %d\n", sreq->cmd);
> +               return TEE_ERROR_BAD_PARAMETERS;
> +       }
> +}
> +
> +void optee_suppl_cmd_rpmb(struct udevice *dev, struct optee_msg_arg *arg)
> +{
> +       struct tee_shm *req_shm;
> +       struct tee_shm *rsp_shm;
> +       void *req_buf;
> +       void *rsp_buf;
> +       ulong req_size;
> +       ulong rsp_size;
> +
> +       if (arg->num_params != 2 ||
> +           arg->params[0].attr != OPTEE_MSG_ATTR_TYPE_RMEM_INPUT ||
> +           arg->params[1].attr != OPTEE_MSG_ATTR_TYPE_RMEM_OUTPUT) {
> +               arg->ret = TEE_ERROR_BAD_PARAMETERS;
> +               return;
> +       }
> +
> +       req_shm = (struct tee_shm *)(ulong)arg->params[0].u.rmem.shm_ref;
> +       req_buf = (u8 *)req_shm->addr + arg->params[0].u.rmem.offs;
> +       req_size = arg->params[0].u.rmem.size;
> +
> +       rsp_shm = (struct tee_shm *)(ulong)arg->params[1].u.rmem.shm_ref;
> +       rsp_buf = (u8 *)rsp_shm->addr + arg->params[1].u.rmem.offs;
> +       rsp_size = arg->params[1].u.rmem.size;
> +
> +       arg->ret = rpmb_process_request(dev_get_priv(dev), req_buf, req_size,
> +                                       rsp_buf, rsp_size);
> +}
> +
> +void optee_suppl_rpmb_release(struct udevice *dev)
> +{
> +       release_mmc(dev_get_priv(dev));
> +}
> diff --git a/drivers/tee/optee/supplicant.c b/drivers/tee/optee/supplicant.c
> index 6965055bd1b5..14cb8717522c 100644
> --- a/drivers/tee/optee/supplicant.c
> +++ b/drivers/tee/optee/supplicant.c
> @@ -81,6 +81,9 @@ void optee_suppl_cmd(struct udevice *dev, struct tee_shm *shm_arg,
>                 debug("OPTEE_MSG_RPC_CMD_FS not implemented\n");
>                 arg->ret = TEE_ERROR_NOT_IMPLEMENTED;
>                 break;
> +       case OPTEE_MSG_RPC_CMD_RPMB:
> +               optee_suppl_cmd_rpmb(dev, arg);
> +               break;
>         default:
>                 arg->ret = TEE_ERROR_NOT_IMPLEMENTED;
>         }
> --
> 2.17.1
>

Regards,
Simon
Jens Wiklander Aug. 30, 2018, 2:41 p.m. | #2
Hi Simon,

On Wed, Aug 29, 2018 at 06:29:12PM -0600, Simon Glass wrote:
> Hi Jens,
> 
> On 23 August 2018 at 04:43, Jens Wiklander <jens.wiklander@linaro.org> wrote:
> > Adds support in optee supplicant to route signed (MACed) RPMB frames
> > from OP-TEE Secure OS to MMC and vice versa to manipulate the RPMB
> > partition.
> >
> > Tested-by: Igor Opaniuk <igor.opaniuk@linaro.org>
> > Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>
> > ---
> >  drivers/tee/optee/Makefile        |   1 +
> >  drivers/tee/optee/core.c          |   8 ++
> >  drivers/tee/optee/optee_private.h |  31 ++++-
> >  drivers/tee/optee/rpmb.c          | 184 ++++++++++++++++++++++++++++++
> >  drivers/tee/optee/supplicant.c    |   3 +
> >  5 files changed, 226 insertions(+), 1 deletion(-)
> >  create mode 100644 drivers/tee/optee/rpmb.c
> >
> > diff --git a/drivers/tee/optee/Makefile b/drivers/tee/optee/Makefile
> > index 6148feb474a5..928d3f80027f 100644
> > --- a/drivers/tee/optee/Makefile
> > +++ b/drivers/tee/optee/Makefile
> > @@ -2,3 +2,4 @@
> >
> >  obj-y += core.o
> >  obj-y += supplicant.o
> > +obj-$(CONFIG_SUPPORT_EMMC_RPMB) += rpmb.o
> > diff --git a/drivers/tee/optee/core.c b/drivers/tee/optee/core.c
> > index f2d92d96551b..e297d206af3a 100644
> > --- a/drivers/tee/optee/core.c
> > +++ b/drivers/tee/optee/core.c
> > @@ -280,6 +280,13 @@ static u32 do_call_with_arg(struct udevice *dev, struct optee_msg_arg *arg)
> >                         param.a3 = res.a3;
> >                         handle_rpc(dev, &param, &page_list);
> >                 } else {
> > +                       /*
> > +                        * In case we've accessed RPMB to serve an RPC
> > +                        * request we need to restore the previously
> > +                        * selected partition as the caller may expect it
> > +                        * to remain unchanged.
> > +                        */
> > +                       optee_suppl_rpmb_release(dev);
> >                         return call_err_to_res(res.a0);
> >                 }
> >         }
> > @@ -611,4 +618,5 @@ U_BOOT_DRIVER(optee) = {
> >         .probe = optee_probe,
> >         .ops = &optee_ops,
> >         .platdata_auto_alloc_size = sizeof(struct optee_pdata),
> > +       .priv_auto_alloc_size = sizeof(struct optee_private),
> >  };
> > diff --git a/drivers/tee/optee/optee_private.h b/drivers/tee/optee/optee_private.h
> > index daa470f812a9..b76979d21011 100644
> > --- a/drivers/tee/optee/optee_private.h
> > +++ b/drivers/tee/optee/optee_private.h
> > @@ -6,7 +6,36 @@
> >  #ifndef __OPTEE_PRIVATE_H
> >  #define __OPTEE_PRIVATE_H
> >
> > +#include <tee.h>
> > +#include <log.h>
> > +
> > +struct optee_private {
> 
> doc comment

OK, I'll fix.

> 
> > +       struct mmc *rpmb_mmc;
> > +       int rpmb_dev_id;
> > +       char rpmb_original_part;
> 
> Why is this char? Are you trying to save memory? I doubt it will work :-)

Good question. I think I got it from struct blk_desc::hwpart, but still
got it slightly wrong. I'll change it into an int instead.

> 
> 
> > +};
> > +
> > +struct optee_msg_arg;
> > +
> > +void optee_suppl_cmd(struct udevice *dev, struct tee_shm *shm_arg,
> > +                    void **page_list);
> 
> Function comments.
> 
> > +
> > +#ifdef CONFIG_SUPPORT_EMMC_RPMB
> > +void optee_suppl_cmd_rpmb(struct udevice *dev, struct optee_msg_arg *arg);
> > +void optee_suppl_rpmb_release(struct udevice *dev);
> > +#else
> > +static inline void optee_suppl_cmd_rpmb(struct udevice *dev,
> > +                                       struct optee_msg_arg *arg)
> > +{
> > +       debug("OPTEE_MSG_RPC_CMD_RPMB not implemented\n");
> > +       arg->ret = TEE_ERROR_NOT_IMPLEMENTED;
> > +}
> > +
> > +static inline void optee_suppl_rpmb_release(struct udevice *dev)
> > +{
> > +}
> > +#endif
> > +
> >  void *optee_alloc_and_init_page_list(void *buf, ulong len, u64 *phys_buf_ptr);
> > -void optee_suppl_cmd(struct udevice *dev, void *shm, void **page_list);
> >
> >  #endif /*__OPTEE_PRIVATE_H*/
> > diff --git a/drivers/tee/optee/rpmb.c b/drivers/tee/optee/rpmb.c
> > new file mode 100644
> > index 000000000000..c1447a5561c2
> > --- /dev/null
> > +++ b/drivers/tee/optee/rpmb.c
> > @@ -0,0 +1,184 @@
> > +// SPDX-License-Identifier: BSD-2-Clause
> > +/*
> > + * Copyright (c) 2018 Linaro Limited
> > + */
> > +
> > +#include <common.h>
> > +#include <log.h>
> > +#include <tee.h>
> > +#include <mmc.h>
> > +
> > +#include "optee_msg.h"
> > +#include "optee_private.h"
> > +
> > +/*
> > + * Request and response definitions must be in sync with the secure side of
> > + * OP-TEE.
> > + */
> > +
> > +/* Request */
> > +struct rpmb_req {
> > +       u16 cmd;
> > +#define RPMB_CMD_DATA_REQ      0x00
> > +#define RPMB_CMD_GET_DEV_INFO  0x01
> > +       u16 dev_id;
> > +       u16 block_count;
> > +       /* Optional data frames (rpmb_data_frame) follow */
> > +};
> > +
> > +#define RPMB_REQ_DATA(req) ((void *)((struct rpmb_req *)(req) + 1))
> > +
> > +/* Response to device info request */
> > +struct rpmb_dev_info {
> > +       u8 cid[16];
> > +       u8 rpmb_size_mult;      /* EXT CSD-slice 168: RPMB Size */
> > +       u8 rel_wr_sec_c;        /* EXT CSD-slice 222: Reliable Write Sector */
> > +                               /*                    Count */
> > +       u8 ret_code;
> > +#define RPMB_CMD_GET_DEV_INFO_RET_OK     0x00
> > +#define RPMB_CMD_GET_DEV_INFO_RET_ERROR  0x01
> > +};
> > +
> > +static void release_mmc(struct optee_private *priv)
> > +{
> > +       int rc;
> > +
> > +       if (!priv->rpmb_mmc)
> > +               return;
> > +
> > +       rc = blk_select_hwpart_devnum(IF_TYPE_MMC, priv->rpmb_dev_id,
> > +                                     priv->rpmb_original_part);
> > +       if (rc)
> > +               debug("%s: blk_select_hwpart_devnum() failed: %d\n",
> > +                     __func__, rc);
> > +
> > +       priv->rpmb_mmc = NULL;
> > +}
> > +
> > +static struct mmc *get_mmc(struct optee_private *priv, int dev_id)
> > +{
> > +       struct mmc *mmc;
> > +       int rc;
> > +
> > +       if (priv->rpmb_mmc && priv->rpmb_dev_id == dev_id)
> > +               return priv->rpmb_mmc;
> > +
> > +       release_mmc(priv);
> > +
> > +       mmc = find_mmc_device(dev_id);
> > +       if (!mmc) {
> > +               debug("Cannot find RPMB device\n");
> > +               return NULL;
> > +       }
> > +       if (!(mmc->version & MMC_VERSION_MMC)) {
> > +               debug("Device id %d is not an eMMC device\n", dev_id);
> > +               return NULL;
> > +       }
> > +       if (mmc->version < MMC_VERSION_4_41) {
> > +               debug("Device id %d: RPMB not supported before version 4.41\n",
> > +                     dev_id);
> > +               return NULL;
> > +       }
> > +
> > +#ifdef CONFIG_BLK
> 
> You shouldn't support !CONFIG_BLK. It is going away in the next release.

OK, I'll remove the ifdef and just keep the
mmc_get_blk_desc(mmc)->hwpart line then.

> 
> > +       priv->rpmb_original_part = mmc_get_blk_desc(mmc)->hwpart;
> > +#else
> > +       priv->rpmb_original_part = mmc->block_dev.hwpart;
> > +#endif
> > +
> > +       rc = blk_select_hwpart_devnum(IF_TYPE_MMC, dev_id, MMC_PART_RPMB);
> > +       if (rc) {
> > +               debug("Device id %d: cannot select RPMB partition: %d\n",
> > +                     dev_id, rc);
> > +               return NULL;
> > +       }
> > +
> > +       priv->rpmb_mmc = mmc;
> > +       priv->rpmb_dev_id = dev_id;
> > +       return mmc;
> > +}
> > +
> > +static u32 rpmb_get_dev_info(u16 dev_id, struct rpmb_dev_info *info)
> > +{
> > +       struct mmc *mmc = find_mmc_device(dev_id);
> > +
> > +       if (!mmc)
> > +               return TEE_ERROR_ITEM_NOT_FOUND;
> > +
> > +       if (!mmc->ext_csd)
> > +               return TEE_ERROR_GENERIC;
> > +
> > +       memcpy(info->cid, mmc->cid, sizeof(info->cid));
> > +       info->rel_wr_sec_c = mmc->ext_csd[222];
> > +       info->rpmb_size_mult = mmc->ext_csd[168];
> > +       info->ret_code = RPMB_CMD_GET_DEV_INFO_RET_OK;
> > +
> > +       return TEE_SUCCESS;
> > +}
> > +
> > +static u32 rpmb_process_request(struct optee_private *priv, void *req,
> > +                               ulong req_size, void *rsp, ulong rsp_size)
> > +{
> > +       struct rpmb_req *sreq = req;
> > +       struct mmc *mmc;
> > +
> > +       if (req_size < sizeof(*sreq))
> > +               return TEE_ERROR_BAD_PARAMETERS;
> > +
> > +       switch (sreq->cmd) {
> > +       case RPMB_CMD_DATA_REQ:
> > +               mmc = get_mmc(priv, sreq->dev_id);
> > +               if (!mmc)
> > +                       return TEE_ERROR_ITEM_NOT_FOUND;
> > +               if (mmc_rpmb_route_frames(mmc, RPMB_REQ_DATA(req),
> > +                                         req_size - sizeof(struct rpmb_req),
> > +                                         rsp, rsp_size))
> > +                       return TEE_ERROR_BAD_PARAMETERS;
> > +               return TEE_SUCCESS;
> > +
> > +       case RPMB_CMD_GET_DEV_INFO:
> > +               if (req_size != sizeof(struct rpmb_req) ||
> > +                   rsp_size != sizeof(struct rpmb_dev_info)) {
> > +                       debug("Invalid req/rsp size\n");
> > +                       return TEE_ERROR_BAD_PARAMETERS;
> > +               }
> > +               return rpmb_get_dev_info(sreq->dev_id, rsp);
> > +
> > +       default:
> > +               debug("Unsupported RPMB command: %d\n", sreq->cmd);
> > +               return TEE_ERROR_BAD_PARAMETERS;
> > +       }
> > +}
> > +
> > +void optee_suppl_cmd_rpmb(struct udevice *dev, struct optee_msg_arg *arg)
> > +{
> > +       struct tee_shm *req_shm;
> > +       struct tee_shm *rsp_shm;
> > +       void *req_buf;
> > +       void *rsp_buf;
> > +       ulong req_size;
> > +       ulong rsp_size;
> > +
> > +       if (arg->num_params != 2 ||
> > +           arg->params[0].attr != OPTEE_MSG_ATTR_TYPE_RMEM_INPUT ||
> > +           arg->params[1].attr != OPTEE_MSG_ATTR_TYPE_RMEM_OUTPUT) {
> > +               arg->ret = TEE_ERROR_BAD_PARAMETERS;
> > +               return;
> > +       }
> > +
> > +       req_shm = (struct tee_shm *)(ulong)arg->params[0].u.rmem.shm_ref;
> > +       req_buf = (u8 *)req_shm->addr + arg->params[0].u.rmem.offs;
> > +       req_size = arg->params[0].u.rmem.size;
> > +
> > +       rsp_shm = (struct tee_shm *)(ulong)arg->params[1].u.rmem.shm_ref;
> > +       rsp_buf = (u8 *)rsp_shm->addr + arg->params[1].u.rmem.offs;
> > +       rsp_size = arg->params[1].u.rmem.size;
> > +
> > +       arg->ret = rpmb_process_request(dev_get_priv(dev), req_buf, req_size,
> > +                                       rsp_buf, rsp_size);
> > +}
> > +
> > +void optee_suppl_rpmb_release(struct udevice *dev)
> > +{
> > +       release_mmc(dev_get_priv(dev));
> > +}
> > diff --git a/drivers/tee/optee/supplicant.c b/drivers/tee/optee/supplicant.c
> > index 6965055bd1b5..14cb8717522c 100644
> > --- a/drivers/tee/optee/supplicant.c
> > +++ b/drivers/tee/optee/supplicant.c
> > @@ -81,6 +81,9 @@ void optee_suppl_cmd(struct udevice *dev, struct tee_shm *shm_arg,
> >                 debug("OPTEE_MSG_RPC_CMD_FS not implemented\n");
> >                 arg->ret = TEE_ERROR_NOT_IMPLEMENTED;
> >                 break;
> > +       case OPTEE_MSG_RPC_CMD_RPMB:
> > +               optee_suppl_cmd_rpmb(dev, arg);
> > +               break;
> >         default:
> >                 arg->ret = TEE_ERROR_NOT_IMPLEMENTED;
> >         }
> > --
> > 2.17.1
> >

Thanks for the review,
Jens

Patch

diff --git a/drivers/tee/optee/Makefile b/drivers/tee/optee/Makefile
index 6148feb474a5..928d3f80027f 100644
--- a/drivers/tee/optee/Makefile
+++ b/drivers/tee/optee/Makefile
@@ -2,3 +2,4 @@ 
 
 obj-y += core.o
 obj-y += supplicant.o
+obj-$(CONFIG_SUPPORT_EMMC_RPMB) += rpmb.o
diff --git a/drivers/tee/optee/core.c b/drivers/tee/optee/core.c
index f2d92d96551b..e297d206af3a 100644
--- a/drivers/tee/optee/core.c
+++ b/drivers/tee/optee/core.c
@@ -280,6 +280,13 @@  static u32 do_call_with_arg(struct udevice *dev, struct optee_msg_arg *arg)
 			param.a3 = res.a3;
 			handle_rpc(dev, &param, &page_list);
 		} else {
+			/*
+			 * In case we've accessed RPMB to serve an RPC
+			 * request we need to restore the previously
+			 * selected partition as the caller may expect it
+			 * to remain unchanged.
+			 */
+			optee_suppl_rpmb_release(dev);
 			return call_err_to_res(res.a0);
 		}
 	}
@@ -611,4 +618,5 @@  U_BOOT_DRIVER(optee) = {
 	.probe = optee_probe,
 	.ops = &optee_ops,
 	.platdata_auto_alloc_size = sizeof(struct optee_pdata),
+	.priv_auto_alloc_size = sizeof(struct optee_private),
 };
diff --git a/drivers/tee/optee/optee_private.h b/drivers/tee/optee/optee_private.h
index daa470f812a9..b76979d21011 100644
--- a/drivers/tee/optee/optee_private.h
+++ b/drivers/tee/optee/optee_private.h
@@ -6,7 +6,36 @@ 
 #ifndef __OPTEE_PRIVATE_H
 #define __OPTEE_PRIVATE_H
 
+#include <tee.h>
+#include <log.h>
+
+struct optee_private {
+	struct mmc *rpmb_mmc;
+	int rpmb_dev_id;
+	char rpmb_original_part;
+};
+
+struct optee_msg_arg;
+
+void optee_suppl_cmd(struct udevice *dev, struct tee_shm *shm_arg,
+		     void **page_list);
+
+#ifdef CONFIG_SUPPORT_EMMC_RPMB
+void optee_suppl_cmd_rpmb(struct udevice *dev, struct optee_msg_arg *arg);
+void optee_suppl_rpmb_release(struct udevice *dev);
+#else
+static inline void optee_suppl_cmd_rpmb(struct udevice *dev,
+					struct optee_msg_arg *arg)
+{
+	debug("OPTEE_MSG_RPC_CMD_RPMB not implemented\n");
+	arg->ret = TEE_ERROR_NOT_IMPLEMENTED;
+}
+
+static inline void optee_suppl_rpmb_release(struct udevice *dev)
+{
+}
+#endif
+
 void *optee_alloc_and_init_page_list(void *buf, ulong len, u64 *phys_buf_ptr);
-void optee_suppl_cmd(struct udevice *dev, void *shm, void **page_list);
 
 #endif /*__OPTEE_PRIVATE_H*/
diff --git a/drivers/tee/optee/rpmb.c b/drivers/tee/optee/rpmb.c
new file mode 100644
index 000000000000..c1447a5561c2
--- /dev/null
+++ b/drivers/tee/optee/rpmb.c
@@ -0,0 +1,184 @@ 
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (c) 2018 Linaro Limited
+ */
+
+#include <common.h>
+#include <log.h>
+#include <tee.h>
+#include <mmc.h>
+
+#include "optee_msg.h"
+#include "optee_private.h"
+
+/*
+ * Request and response definitions must be in sync with the secure side of
+ * OP-TEE.
+ */
+
+/* Request */
+struct rpmb_req {
+	u16 cmd;
+#define RPMB_CMD_DATA_REQ      0x00
+#define RPMB_CMD_GET_DEV_INFO  0x01
+	u16 dev_id;
+	u16 block_count;
+	/* Optional data frames (rpmb_data_frame) follow */
+};
+
+#define RPMB_REQ_DATA(req) ((void *)((struct rpmb_req *)(req) + 1))
+
+/* Response to device info request */
+struct rpmb_dev_info {
+	u8 cid[16];
+	u8 rpmb_size_mult;	/* EXT CSD-slice 168: RPMB Size */
+	u8 rel_wr_sec_c;	/* EXT CSD-slice 222: Reliable Write Sector */
+				/*                    Count */
+	u8 ret_code;
+#define RPMB_CMD_GET_DEV_INFO_RET_OK     0x00
+#define RPMB_CMD_GET_DEV_INFO_RET_ERROR  0x01
+};
+
+static void release_mmc(struct optee_private *priv)
+{
+	int rc;
+
+	if (!priv->rpmb_mmc)
+		return;
+
+	rc = blk_select_hwpart_devnum(IF_TYPE_MMC, priv->rpmb_dev_id,
+				      priv->rpmb_original_part);
+	if (rc)
+		debug("%s: blk_select_hwpart_devnum() failed: %d\n",
+		      __func__, rc);
+
+	priv->rpmb_mmc = NULL;
+}
+
+static struct mmc *get_mmc(struct optee_private *priv, int dev_id)
+{
+	struct mmc *mmc;
+	int rc;
+
+	if (priv->rpmb_mmc && priv->rpmb_dev_id == dev_id)
+		return priv->rpmb_mmc;
+
+	release_mmc(priv);
+
+	mmc = find_mmc_device(dev_id);
+	if (!mmc) {
+		debug("Cannot find RPMB device\n");
+		return NULL;
+	}
+	if (!(mmc->version & MMC_VERSION_MMC)) {
+		debug("Device id %d is not an eMMC device\n", dev_id);
+		return NULL;
+	}
+	if (mmc->version < MMC_VERSION_4_41) {
+		debug("Device id %d: RPMB not supported before version 4.41\n",
+		      dev_id);
+		return NULL;
+	}
+
+#ifdef CONFIG_BLK
+	priv->rpmb_original_part = mmc_get_blk_desc(mmc)->hwpart;
+#else
+	priv->rpmb_original_part = mmc->block_dev.hwpart;
+#endif
+
+	rc = blk_select_hwpart_devnum(IF_TYPE_MMC, dev_id, MMC_PART_RPMB);
+	if (rc) {
+		debug("Device id %d: cannot select RPMB partition: %d\n",
+		      dev_id, rc);
+		return NULL;
+	}
+
+	priv->rpmb_mmc = mmc;
+	priv->rpmb_dev_id = dev_id;
+	return mmc;
+}
+
+static u32 rpmb_get_dev_info(u16 dev_id, struct rpmb_dev_info *info)
+{
+	struct mmc *mmc = find_mmc_device(dev_id);
+
+	if (!mmc)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	if (!mmc->ext_csd)
+		return TEE_ERROR_GENERIC;
+
+	memcpy(info->cid, mmc->cid, sizeof(info->cid));
+	info->rel_wr_sec_c = mmc->ext_csd[222];
+	info->rpmb_size_mult = mmc->ext_csd[168];
+	info->ret_code = RPMB_CMD_GET_DEV_INFO_RET_OK;
+
+	return TEE_SUCCESS;
+}
+
+static u32 rpmb_process_request(struct optee_private *priv, void *req,
+				ulong req_size, void *rsp, ulong rsp_size)
+{
+	struct rpmb_req *sreq = req;
+	struct mmc *mmc;
+
+	if (req_size < sizeof(*sreq))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	switch (sreq->cmd) {
+	case RPMB_CMD_DATA_REQ:
+		mmc = get_mmc(priv, sreq->dev_id);
+		if (!mmc)
+			return TEE_ERROR_ITEM_NOT_FOUND;
+		if (mmc_rpmb_route_frames(mmc, RPMB_REQ_DATA(req),
+					  req_size - sizeof(struct rpmb_req),
+					  rsp, rsp_size))
+			return TEE_ERROR_BAD_PARAMETERS;
+		return TEE_SUCCESS;
+
+	case RPMB_CMD_GET_DEV_INFO:
+		if (req_size != sizeof(struct rpmb_req) ||
+		    rsp_size != sizeof(struct rpmb_dev_info)) {
+			debug("Invalid req/rsp size\n");
+			return TEE_ERROR_BAD_PARAMETERS;
+		}
+		return rpmb_get_dev_info(sreq->dev_id, rsp);
+
+	default:
+		debug("Unsupported RPMB command: %d\n", sreq->cmd);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+}
+
+void optee_suppl_cmd_rpmb(struct udevice *dev, struct optee_msg_arg *arg)
+{
+	struct tee_shm *req_shm;
+	struct tee_shm *rsp_shm;
+	void *req_buf;
+	void *rsp_buf;
+	ulong req_size;
+	ulong rsp_size;
+
+	if (arg->num_params != 2 ||
+	    arg->params[0].attr != OPTEE_MSG_ATTR_TYPE_RMEM_INPUT ||
+	    arg->params[1].attr != OPTEE_MSG_ATTR_TYPE_RMEM_OUTPUT) {
+		arg->ret = TEE_ERROR_BAD_PARAMETERS;
+		return;
+	}
+
+	req_shm = (struct tee_shm *)(ulong)arg->params[0].u.rmem.shm_ref;
+	req_buf = (u8 *)req_shm->addr + arg->params[0].u.rmem.offs;
+	req_size = arg->params[0].u.rmem.size;
+
+	rsp_shm = (struct tee_shm *)(ulong)arg->params[1].u.rmem.shm_ref;
+	rsp_buf = (u8 *)rsp_shm->addr + arg->params[1].u.rmem.offs;
+	rsp_size = arg->params[1].u.rmem.size;
+
+	arg->ret = rpmb_process_request(dev_get_priv(dev), req_buf, req_size,
+					rsp_buf, rsp_size);
+}
+
+void optee_suppl_rpmb_release(struct udevice *dev)
+{
+	release_mmc(dev_get_priv(dev));
+}
diff --git a/drivers/tee/optee/supplicant.c b/drivers/tee/optee/supplicant.c
index 6965055bd1b5..14cb8717522c 100644
--- a/drivers/tee/optee/supplicant.c
+++ b/drivers/tee/optee/supplicant.c
@@ -81,6 +81,9 @@  void optee_suppl_cmd(struct udevice *dev, struct tee_shm *shm_arg,
 		debug("OPTEE_MSG_RPC_CMD_FS not implemented\n");
 		arg->ret = TEE_ERROR_NOT_IMPLEMENTED;
 		break;
+	case OPTEE_MSG_RPC_CMD_RPMB:
+		optee_suppl_cmd_rpmb(dev, arg);
+		break;
 	default:
 		arg->ret = TEE_ERROR_NOT_IMPLEMENTED;
 	}