diff mbox series

[6/6] scsi: ufs: exynos: Add support for Flash Memory Protector (FMP)

Message ID 20240611223419.239466-7-ebiggers@kernel.org (mailing list archive)
State New
Headers show
Series Basic inline encryption support for ufs-exynos | expand

Commit Message

Eric Biggers June 11, 2024, 10:34 p.m. UTC 
From: Eric Biggers <ebiggers@google.com>

Add support for Flash Memory Protector (FMP), which is the inline
encryption hardware on Exynos and Exynos-based SoCs.

Specifically, add support for the "traditional FMP mode" that works on
many Exynos-based SoCs including gs101.  This is the mode that uses
"software keys" and is compatible with the upstream kernel's existing
inline encryption framework in the block and filesystem layers.  I plan
to add support for the wrapped key support on gs101 at a later time.

Tested on gs101 (specifically Pixel 6) by running the 'encrypt' group of
xfstests on a filesystem mounted with the 'inlinecrypt' mount option.

Signed-off-by: Eric Biggers <ebiggers@google.com>
---
 drivers/ufs/host/ufs-exynos.c | 219 +++++++++++++++++++++++++++++++++-
 1 file changed, 218 insertions(+), 1 deletion(-)

Comments

Bart Van Assche June 14, 2024, 5:08 p.m. UTC | #1 
On 6/11/24 3:34 PM, Eric Biggers wrote:
> +#define FMP_DATA_UNIT_SIZE	SZ_4K

A Samsung employee told me that the Exynos encryption data unit size is configurable
and also that it is set by the following code:

	hci_writel(ufs, PRDT_SET_SIZE(12), HCI_TXPRDT_ENTRY_SIZE);
	hci_writel(ufs, PRDT_SET_SIZE(12), HCI_RXPRDT_ENTRY_SIZE);

How about introducing a new macro that represents the TX PRDT entry size, the RX PRDT
entry size and the encryption data unit size?

Thanks,

Bart.
Sam Protsenko June 14, 2024, 11 p.m. UTC | #2 
On Tue, Jun 11, 2024 at 5:36 PM Eric Biggers <ebiggers@kernel.org> wrote:
>
> From: Eric Biggers <ebiggers@google.com>
>
> Add support for Flash Memory Protector (FMP), which is the inline
> encryption hardware on Exynos and Exynos-based SoCs.
>
> Specifically, add support for the "traditional FMP mode" that works on
> many Exynos-based SoCs including gs101.  This is the mode that uses
> "software keys" and is compatible with the upstream kernel's existing
> inline encryption framework in the block and filesystem layers.  I plan
> to add support for the wrapped key support on gs101 at a later time.
>
> Tested on gs101 (specifically Pixel 6) by running the 'encrypt' group of
> xfstests on a filesystem mounted with the 'inlinecrypt' mount option.
>
> Signed-off-by: Eric Biggers <ebiggers@google.com>
> ---
>  drivers/ufs/host/ufs-exynos.c | 219 +++++++++++++++++++++++++++++++++-
>  1 file changed, 218 insertions(+), 1 deletion(-)
>
> diff --git a/drivers/ufs/host/ufs-exynos.c b/drivers/ufs/host/ufs-exynos.c
> index 88d125d1ee3c..969c4eedbe2d 100644
> --- a/drivers/ufs/host/ufs-exynos.c
> +++ b/drivers/ufs/host/ufs-exynos.c
> @@ -6,10 +6,13 @@
>   * Author: Seungwon Jeon  <essuuj@gmail.com>
>   * Author: Alim Akhtar <alim.akhtar@samsung.com>
>   *
>   */
>
> +#include <asm/unaligned.h>
> +#include <crypto/aes.h>
> +#include <linux/arm-smccc.h>
>  #include <linux/clk.h>
>  #include <linux/delay.h>
>  #include <linux/module.h>
>  #include <linux/of.h>
>  #include <linux/of_address.h>
> @@ -1149,10 +1152,221 @@ static inline void exynos_ufs_priv_init(struct ufs_hba *hba,
>                 ufs->rx_sel_idx = 0;
>         hba->priv = (void *)ufs;
>         hba->quirks = ufs->drv_data->quirks;
>  }
>
> +#ifdef CONFIG_SCSI_UFS_CRYPTO
> +
> +/*
> + * Support for Flash Memory Protector (FMP), which is the inline encryption
> + * hardware on Exynos and Exynos-based SoCs.  The interface to this hardware is
> + * not compatible with the standard UFS crypto.  It requires that encryption be
> + * configured in the PRDT using a nonstandard extension.
> + */
> +
> +enum fmp_crypto_algo_mode {
> +       FMP_BYPASS_MODE = 0,
> +       FMP_ALGO_MODE_AES_CBC = 1,
> +       FMP_ALGO_MODE_AES_XTS = 2,
> +};
> +enum fmp_crypto_key_length {
> +       FMP_KEYLEN_256BIT = 1,
> +};
> +#define FMP_DATA_UNIT_SIZE     SZ_4K
> +
> +/* This is the nonstandard format of PRDT entries when FMP is enabled. */
> +struct fmp_sg_entry {
> +
> +       /*
> +        * This is the standard PRDT entry, but with nonstandard bitfields in
> +        * the high bits of the 'size' field, i.e. the last 32-bit word.  When
> +        * these nonstandard bitfields are zero, the data segment won't be
> +        * encrypted or decrypted.  Otherwise they specify the algorithm and key
> +        * length with which the data segment will be encrypted or decrypted.
> +        */

Minor suggestion: create a kernel-doc comment for the structure and
pull all fields documentation there.

> +       struct ufshcd_sg_entry base;
> +
> +       /* The initialization vector (IV) with all bytes reversed */
> +       __be64 file_iv[2];
> +
> +       /*
> +        * The key with all bytes reversed.  For XTS, the two halves of the key
> +        * are given separately and are byte-reversed separately.
> +        */
> +       __be64 file_enckey[4];
> +       __be64 file_twkey[4];
> +
> +       /* Unused */
> +       __be64 disk_iv[2];
> +       __be64 reserved[2];
> +};
> +
> +#define SMC_CMD_FMP_SECURITY           0xC2001810
> +#define SMC_CMD_SMU                    0xC2001850
> +#define SMC_CMD_FMP_SMU_RESUME         0xC2001860

Suggest to use ARM_SMCCC_CALL_VAL() macro to define above values.

> +#define SMU_EMBEDDED                   0
> +#define SMU_INIT                       0
> +#define CFG_DESCTYPE_3                 3
> +
> +static inline long exynos_smc(unsigned long cmd, unsigned long arg0,
> +                             unsigned long arg1, unsigned long arg2)
> +{
> +       struct arm_smccc_res res;
> +
> +       arm_smccc_smc(cmd, arg0, arg1, arg2, 0, 0, 0, 0, &res);
> +       return res.a0;
> +}

This wrapper looks like it was borrowed from the downstream Samsung
code. Not sure if it brings any value nowadays. Maybe it would be
clearer to just use arm_smccc_smc() directly and remove this wrapper?

> +
> +static void exynos_ufs_fmp_init(struct ufs_hba *hba)
> +{
> +       struct blk_crypto_profile *profile = &hba->crypto_profile;
> +       long ret;
> +
> +       /*
> +        * Check for the standard crypto support bit, since it's available even
> +        * though the rest of the interface to FMP is nonstandard.
> +        *
> +        * This check should have the effect of preventing the driver from
> +        * trying to use FMP on old Exynos SoCs that don't have FMP.
> +        */
> +       if (!(ufshcd_readl(hba, REG_CONTROLLER_CAPABILITIES) &
> +             MASK_CRYPTO_SUPPORT))
> +               return;
> +
> +       /*
> +        * This call (which sets DESCTYPE to 0x3 in the FMPSECURITY0 register)
> +        * is needed to make the hardware use the larger PRDT entry size.
> +        */
> +       BUILD_BUG_ON(sizeof(struct fmp_sg_entry) != 128);
> +       ret = exynos_smc(SMC_CMD_FMP_SECURITY, 0, SMU_EMBEDDED, CFG_DESCTYPE_3);
> +       if (ret) {
> +               dev_warn(hba->dev,
> +                        "SMC_CMD_FMP_SECURITY failed on init: %ld.  Disabling FMP support.\n",
> +                        ret);
> +               return;
> +       }
> +       ufshcd_set_sg_entry_size(hba, sizeof(struct fmp_sg_entry));
> +
> +       /*
> +        * This is needed to initialize FMP.  Without it, errors occur when
> +        * inline encryption is used.
> +        */
> +       ret = exynos_smc(SMC_CMD_SMU, SMU_INIT, SMU_EMBEDDED, 0);
> +       if (ret) {
> +               dev_err(hba->dev,
> +                       "SMC_CMD_SMU(SMU_INIT) failed: %ld.  Disabling FMP support.\n",
> +                       ret);
> +               return;
> +       }
> +
> +       /* Advertise crypto capabilities to the block layer. */
> +       ret = devm_blk_crypto_profile_init(hba->dev, profile, 0);
> +       if (ret) {
> +               /* Only ENOMEM should be possible here. */
> +               dev_err(hba->dev, "Failed to initialize crypto profile: %ld\n",
> +                       ret);
> +               return;
> +       }
> +       profile->max_dun_bytes_supported = AES_BLOCK_SIZE;
> +       profile->dev = hba->dev;
> +       profile->modes_supported[BLK_ENCRYPTION_MODE_AES_256_XTS] =
> +               FMP_DATA_UNIT_SIZE;
> +
> +       /* Advertise crypto support to ufshcd-core. */
> +       hba->caps |= UFSHCD_CAP_CRYPTO;
> +
> +       /* Advertise crypto quirks to ufshcd-core. */
> +       hba->quirks |= UFSHCD_QUIRK_CUSTOM_CRYPTO_PROFILE |
> +                      UFSHCD_QUIRK_BROKEN_CRYPTO_ENABLE |
> +                      UFSHCD_QUIRK_KEYS_IN_PRDT;
> +
> +}
> +
> +static void exynos_ufs_fmp_resume(struct ufs_hba *hba)
> +{
> +       long ret;
> +
> +       ret = exynos_smc(SMC_CMD_FMP_SECURITY, 0, SMU_EMBEDDED, CFG_DESCTYPE_3);
> +       if (ret)
> +               dev_err(hba->dev,
> +                       "SMC_CMD_FMP_SECURITY failed on resume: %ld\n", ret);
> +
> +       ret = exynos_smc(SMC_CMD_FMP_SMU_RESUME, 0, SMU_EMBEDDED, 0);
> +       if (ret)
> +               dev_err(hba->dev, "SMC_CMD_FMP_SMU_RESUME failed: %ld\n", ret);
> +}
> +
> +static inline __be64 fmp_key_word(const u8 *key, int j)
> +{
> +       return cpu_to_be64(get_unaligned_le64(
> +                       key + AES_KEYSIZE_256 - (j + 1) * sizeof(u64)));
> +}
> +
> +/* Fill the PRDT for a request according to the given encryption context. */
> +static int exynos_ufs_fmp_fill_prdt(struct ufs_hba *hba,
> +                                   const struct bio_crypt_ctx *crypt_ctx,
> +                                   void *prdt, unsigned int num_segments)
> +{
> +       struct fmp_sg_entry *fmp_prdt = prdt;
> +       const u8 *enckey = crypt_ctx->bc_key->raw;
> +       const u8 *twkey = enckey + AES_KEYSIZE_256;
> +       u64 dun_lo = crypt_ctx->bc_dun[0];
> +       u64 dun_hi = crypt_ctx->bc_dun[1];
> +       unsigned int i;
> +
> +       /* If FMP wasn't enabled, we shouldn't get any encrypted requests. */
> +       if (WARN_ON_ONCE(!(hba->caps & UFSHCD_CAP_CRYPTO)))
> +               return -EIO;
> +
> +       /* Configure FMP on each segment of the request. */
> +       for (i = 0; i < num_segments; i++) {
> +               struct fmp_sg_entry *prd = &fmp_prdt[i];
> +               int j;
> +
> +               /* Each segment must be exactly one data unit. */
> +               if (prd->base.size != cpu_to_le32(FMP_DATA_UNIT_SIZE - 1)) {
> +                       dev_err(hba->dev,
> +                               "data segment is misaligned for FMP\n");
> +                       return -EIO;
> +               }
> +
> +               /* Set the algorithm and key length. */
> +               prd->base.size |= cpu_to_le32((FMP_ALGO_MODE_AES_XTS << 28) |
> +                                             (FMP_KEYLEN_256BIT << 26));
> +
> +               /* Set the IV. */
> +               prd->file_iv[0] = cpu_to_be64(dun_hi);
> +               prd->file_iv[1] = cpu_to_be64(dun_lo);
> +
> +               /* Set the key. */
> +               for (j = 0; j < AES_KEYSIZE_256 / sizeof(u64); j++) {
> +                       prd->file_enckey[j] = fmp_key_word(enckey, j);
> +                       prd->file_twkey[j] = fmp_key_word(twkey, j);
> +               }
> +
> +               /* Increment the data unit number. */
> +               dun_lo++;
> +               if (dun_lo == 0)
> +                       dun_hi++;
> +       }
> +       return 0;
> +}
> +
> +#else /* CONFIG_SCSI_UFS_CRYPTO */
> +
> +static void exynos_ufs_fmp_init(struct ufs_hba *hba)
> +{
> +}
> +
> +static void exynos_ufs_fmp_resume(struct ufs_hba *hba)
> +{
> +}
> +
> +#define exynos_ufs_fmp_fill_prdt NULL
> +
> +#endif /* !CONFIG_SCSI_UFS_CRYPTO */
> +
>  static int exynos_ufs_init(struct ufs_hba *hba)
>  {
>         struct device *dev = hba->dev;
>         struct platform_device *pdev = to_platform_device(dev);
>         struct exynos_ufs *ufs;
> @@ -1196,10 +1410,12 @@ static int exynos_ufs_init(struct ufs_hba *hba)
>                 goto out;
>         }
>
>         exynos_ufs_priv_init(hba, ufs);
>
> +       exynos_ufs_fmp_init(hba);
> +
>         if (ufs->drv_data->drv_init) {
>                 ret = ufs->drv_data->drv_init(dev, ufs);
>                 if (ret) {
>                         dev_err(dev, "failed to init drv-data\n");
>                         goto out;
> @@ -1430,11 +1646,11 @@ static int exynos_ufs_resume(struct ufs_hba *hba, enum ufs_pm_op pm_op)
>
>         if (!ufshcd_is_link_active(hba))
>                 phy_power_on(ufs->phy);
>
>         exynos_ufs_config_smu(ufs);
> -
> +       exynos_ufs_fmp_resume(hba);
>         return 0;
>  }
>
>  static int exynosauto_ufs_vh_link_startup_notify(struct ufs_hba *hba,
>                                                  enum ufs_notify_change_status status)
> @@ -1696,10 +1912,11 @@ static const struct ufs_hba_variant_ops ufs_hba_exynos_ops = {
>         .setup_xfer_req                 = exynos_ufs_specify_nexus_t_xfer_req,
>         .setup_task_mgmt                = exynos_ufs_specify_nexus_t_tm_req,
>         .hibern8_notify                 = exynos_ufs_hibern8_notify,
>         .suspend                        = exynos_ufs_suspend,
>         .resume                         = exynos_ufs_resume,
> +       .fill_crypto_prdt               = exynos_ufs_fmp_fill_prdt,
>  };
>
>  static struct ufs_hba_variant_ops ufs_hba_exynosauto_vh_ops = {
>         .name                           = "exynosauto_ufs_vh",
>         .init                           = exynosauto_ufs_vh_init,
> --
> 2.45.2
>
>
Eric Biggers July 2, 2024, 7:28 a.m. UTC | #3 
On Fri, Jun 14, 2024 at 10:08:49AM -0700, Bart Van Assche wrote:
> On 6/11/24 3:34 PM, Eric Biggers wrote:
> > +#define FMP_DATA_UNIT_SIZE	SZ_4K
> 
> A Samsung employee told me that the Exynos encryption data unit size is configurable
> and also that it is set by the following code:
> 
> 	hci_writel(ufs, PRDT_SET_SIZE(12), HCI_TXPRDT_ENTRY_SIZE);
> 	hci_writel(ufs, PRDT_SET_SIZE(12), HCI_RXPRDT_ENTRY_SIZE);
> 
> How about introducing a new macro that represents the TX PRDT entry size, the RX PRDT
> entry size and the encryption data unit size?
> 

Done in v2.

- Eric
Eric Biggers July 2, 2024, 7:28 a.m. UTC | #4 
On Fri, Jun 14, 2024 at 06:00:57PM -0500, Sam Protsenko wrote:
> On Tue, Jun 11, 2024 at 5:36 PM Eric Biggers <ebiggers@kernel.org> wrote:
> >
> > From: Eric Biggers <ebiggers@google.com>
> >
> > Add support for Flash Memory Protector (FMP), which is the inline
> > encryption hardware on Exynos and Exynos-based SoCs.
> >
> > Specifically, add support for the "traditional FMP mode" that works on
> > many Exynos-based SoCs including gs101.  This is the mode that uses
> > "software keys" and is compatible with the upstream kernel's existing
> > inline encryption framework in the block and filesystem layers.  I plan
> > to add support for the wrapped key support on gs101 at a later time.
> >
> > Tested on gs101 (specifically Pixel 6) by running the 'encrypt' group of
> > xfstests on a filesystem mounted with the 'inlinecrypt' mount option.
> >
> > Signed-off-by: Eric Biggers <ebiggers@google.com>
> > ---
> >  drivers/ufs/host/ufs-exynos.c | 219 +++++++++++++++++++++++++++++++++-
> >  1 file changed, 218 insertions(+), 1 deletion(-)
> >
> > diff --git a/drivers/ufs/host/ufs-exynos.c b/drivers/ufs/host/ufs-exynos.c
> > index 88d125d1ee3c..969c4eedbe2d 100644
> > --- a/drivers/ufs/host/ufs-exynos.c
> > +++ b/drivers/ufs/host/ufs-exynos.c
> > @@ -6,10 +6,13 @@
> >   * Author: Seungwon Jeon  <essuuj@gmail.com>
> >   * Author: Alim Akhtar <alim.akhtar@samsung.com>
> >   *
> >   */
> >
> > +#include <asm/unaligned.h>
> > +#include <crypto/aes.h>
> > +#include <linux/arm-smccc.h>
> >  #include <linux/clk.h>
> >  #include <linux/delay.h>
> >  #include <linux/module.h>
> >  #include <linux/of.h>
> >  #include <linux/of_address.h>
> > @@ -1149,10 +1152,221 @@ static inline void exynos_ufs_priv_init(struct ufs_hba *hba,
> >                 ufs->rx_sel_idx = 0;
> >         hba->priv = (void *)ufs;
> >         hba->quirks = ufs->drv_data->quirks;
> >  }
> >
> > +#ifdef CONFIG_SCSI_UFS_CRYPTO
> > +
> > +/*
> > + * Support for Flash Memory Protector (FMP), which is the inline encryption
> > + * hardware on Exynos and Exynos-based SoCs.  The interface to this hardware is
> > + * not compatible with the standard UFS crypto.  It requires that encryption be
> > + * configured in the PRDT using a nonstandard extension.
> > + */
> > +
> > +enum fmp_crypto_algo_mode {
> > +       FMP_BYPASS_MODE = 0,
> > +       FMP_ALGO_MODE_AES_CBC = 1,
> > +       FMP_ALGO_MODE_AES_XTS = 2,
> > +};
> > +enum fmp_crypto_key_length {
> > +       FMP_KEYLEN_256BIT = 1,
> > +};
> > +#define FMP_DATA_UNIT_SIZE     SZ_4K
> > +
> > +/* This is the nonstandard format of PRDT entries when FMP is enabled. */
> > +struct fmp_sg_entry {
> > +
> > +       /*
> > +        * This is the standard PRDT entry, but with nonstandard bitfields in
> > +        * the high bits of the 'size' field, i.e. the last 32-bit word.  When
> > +        * these nonstandard bitfields are zero, the data segment won't be
> > +        * encrypted or decrypted.  Otherwise they specify the algorithm and key
> > +        * length with which the data segment will be encrypted or decrypted.
> > +        */
> 
> Minor suggestion: create a kernel-doc comment for the structure and
> pull all fields documentation there.
> 
> > +       struct ufshcd_sg_entry base;
> > +
> > +       /* The initialization vector (IV) with all bytes reversed */
> > +       __be64 file_iv[2];
> > +
> > +       /*
> > +        * The key with all bytes reversed.  For XTS, the two halves of the key
> > +        * are given separately and are byte-reversed separately.
> > +        */
> > +       __be64 file_enckey[4];
> > +       __be64 file_twkey[4];
> > +
> > +       /* Unused */
> > +       __be64 disk_iv[2];
> > +       __be64 reserved[2];
> > +};
> > +
> > +#define SMC_CMD_FMP_SECURITY           0xC2001810
> > +#define SMC_CMD_SMU                    0xC2001850
> > +#define SMC_CMD_FMP_SMU_RESUME         0xC2001860
> 
> Suggest to use ARM_SMCCC_CALL_VAL() macro to define above values.
> 
> > +#define SMU_EMBEDDED                   0
> > +#define SMU_INIT                       0
> > +#define CFG_DESCTYPE_3                 3
> > +
> > +static inline long exynos_smc(unsigned long cmd, unsigned long arg0,
> > +                             unsigned long arg1, unsigned long arg2)
> > +{
> > +       struct arm_smccc_res res;
> > +
> > +       arm_smccc_smc(cmd, arg0, arg1, arg2, 0, 0, 0, 0, &res);
> > +       return res.a0;
> > +}
> 
> This wrapper looks like it was borrowed from the downstream Samsung
> code. Not sure if it brings any value nowadays. Maybe it would be
> clearer to just use arm_smccc_smc() directly and remove this wrapper?
> 

All done in v2.  Thanks.

- Eric
diff mbox series

Patch

diff --git a/drivers/ufs/host/ufs-exynos.c b/drivers/ufs/host/ufs-exynos.c
index 88d125d1ee3c..969c4eedbe2d 100644
--- a/drivers/ufs/host/ufs-exynos.c
+++ b/drivers/ufs/host/ufs-exynos.c
@@ -6,10 +6,13 @@ 
  * Author: Seungwon Jeon  <essuuj@gmail.com>
  * Author: Alim Akhtar <alim.akhtar@samsung.com>
  *
  */
 
+#include <asm/unaligned.h>
+#include <crypto/aes.h>
+#include <linux/arm-smccc.h>
 #include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
@@ -1149,10 +1152,221 @@  static inline void exynos_ufs_priv_init(struct ufs_hba *hba,
 		ufs->rx_sel_idx = 0;
 	hba->priv = (void *)ufs;
 	hba->quirks = ufs->drv_data->quirks;
 }
 
+#ifdef CONFIG_SCSI_UFS_CRYPTO
+
+/*
+ * Support for Flash Memory Protector (FMP), which is the inline encryption
+ * hardware on Exynos and Exynos-based SoCs.  The interface to this hardware is
+ * not compatible with the standard UFS crypto.  It requires that encryption be
+ * configured in the PRDT using a nonstandard extension.
+ */
+
+enum fmp_crypto_algo_mode {
+	FMP_BYPASS_MODE = 0,
+	FMP_ALGO_MODE_AES_CBC = 1,
+	FMP_ALGO_MODE_AES_XTS = 2,
+};
+enum fmp_crypto_key_length {
+	FMP_KEYLEN_256BIT = 1,
+};
+#define FMP_DATA_UNIT_SIZE	SZ_4K
+
+/* This is the nonstandard format of PRDT entries when FMP is enabled. */
+struct fmp_sg_entry {
+
+	/*
+	 * This is the standard PRDT entry, but with nonstandard bitfields in
+	 * the high bits of the 'size' field, i.e. the last 32-bit word.  When
+	 * these nonstandard bitfields are zero, the data segment won't be
+	 * encrypted or decrypted.  Otherwise they specify the algorithm and key
+	 * length with which the data segment will be encrypted or decrypted.
+	 */
+	struct ufshcd_sg_entry base;
+
+	/* The initialization vector (IV) with all bytes reversed */
+	__be64 file_iv[2];
+
+	/*
+	 * The key with all bytes reversed.  For XTS, the two halves of the key
+	 * are given separately and are byte-reversed separately.
+	 */
+	__be64 file_enckey[4];
+	__be64 file_twkey[4];
+
+	/* Unused */
+	__be64 disk_iv[2];
+	__be64 reserved[2];
+};
+
+#define SMC_CMD_FMP_SECURITY		0xC2001810
+#define SMC_CMD_SMU			0xC2001850
+#define SMC_CMD_FMP_SMU_RESUME		0xC2001860
+#define SMU_EMBEDDED			0
+#define SMU_INIT			0
+#define CFG_DESCTYPE_3			3
+
+static inline long exynos_smc(unsigned long cmd, unsigned long arg0,
+			      unsigned long arg1, unsigned long arg2)
+{
+	struct arm_smccc_res res;
+
+	arm_smccc_smc(cmd, arg0, arg1, arg2, 0, 0, 0, 0, &res);
+	return res.a0;
+}
+
+static void exynos_ufs_fmp_init(struct ufs_hba *hba)
+{
+	struct blk_crypto_profile *profile = &hba->crypto_profile;
+	long ret;
+
+	/*
+	 * Check for the standard crypto support bit, since it's available even
+	 * though the rest of the interface to FMP is nonstandard.
+	 *
+	 * This check should have the effect of preventing the driver from
+	 * trying to use FMP on old Exynos SoCs that don't have FMP.
+	 */
+	if (!(ufshcd_readl(hba, REG_CONTROLLER_CAPABILITIES) &
+	      MASK_CRYPTO_SUPPORT))
+		return;
+
+	/*
+	 * This call (which sets DESCTYPE to 0x3 in the FMPSECURITY0 register)
+	 * is needed to make the hardware use the larger PRDT entry size.
+	 */
+	BUILD_BUG_ON(sizeof(struct fmp_sg_entry) != 128);
+	ret = exynos_smc(SMC_CMD_FMP_SECURITY, 0, SMU_EMBEDDED, CFG_DESCTYPE_3);
+	if (ret) {
+		dev_warn(hba->dev,
+			 "SMC_CMD_FMP_SECURITY failed on init: %ld.  Disabling FMP support.\n",
+			 ret);
+		return;
+	}
+	ufshcd_set_sg_entry_size(hba, sizeof(struct fmp_sg_entry));
+
+	/*
+	 * This is needed to initialize FMP.  Without it, errors occur when
+	 * inline encryption is used.
+	 */
+	ret = exynos_smc(SMC_CMD_SMU, SMU_INIT, SMU_EMBEDDED, 0);
+	if (ret) {
+		dev_err(hba->dev,
+			"SMC_CMD_SMU(SMU_INIT) failed: %ld.  Disabling FMP support.\n",
+			ret);
+		return;
+	}
+
+	/* Advertise crypto capabilities to the block layer. */
+	ret = devm_blk_crypto_profile_init(hba->dev, profile, 0);
+	if (ret) {
+		/* Only ENOMEM should be possible here. */
+		dev_err(hba->dev, "Failed to initialize crypto profile: %ld\n",
+			ret);
+		return;
+	}
+	profile->max_dun_bytes_supported = AES_BLOCK_SIZE;
+	profile->dev = hba->dev;
+	profile->modes_supported[BLK_ENCRYPTION_MODE_AES_256_XTS] =
+		FMP_DATA_UNIT_SIZE;
+
+	/* Advertise crypto support to ufshcd-core. */
+	hba->caps |= UFSHCD_CAP_CRYPTO;
+
+	/* Advertise crypto quirks to ufshcd-core. */
+	hba->quirks |= UFSHCD_QUIRK_CUSTOM_CRYPTO_PROFILE |
+		       UFSHCD_QUIRK_BROKEN_CRYPTO_ENABLE |
+		       UFSHCD_QUIRK_KEYS_IN_PRDT;
+
+}
+
+static void exynos_ufs_fmp_resume(struct ufs_hba *hba)
+{
+	long ret;
+
+	ret = exynos_smc(SMC_CMD_FMP_SECURITY, 0, SMU_EMBEDDED, CFG_DESCTYPE_3);
+	if (ret)
+		dev_err(hba->dev,
+			"SMC_CMD_FMP_SECURITY failed on resume: %ld\n", ret);
+
+	ret = exynos_smc(SMC_CMD_FMP_SMU_RESUME, 0, SMU_EMBEDDED, 0);
+	if (ret)
+		dev_err(hba->dev, "SMC_CMD_FMP_SMU_RESUME failed: %ld\n", ret);
+}
+
+static inline __be64 fmp_key_word(const u8 *key, int j)
+{
+	return cpu_to_be64(get_unaligned_le64(
+			key + AES_KEYSIZE_256 - (j + 1) * sizeof(u64)));
+}
+
+/* Fill the PRDT for a request according to the given encryption context. */
+static int exynos_ufs_fmp_fill_prdt(struct ufs_hba *hba,
+				    const struct bio_crypt_ctx *crypt_ctx,
+				    void *prdt, unsigned int num_segments)
+{
+	struct fmp_sg_entry *fmp_prdt = prdt;
+	const u8 *enckey = crypt_ctx->bc_key->raw;
+	const u8 *twkey = enckey + AES_KEYSIZE_256;
+	u64 dun_lo = crypt_ctx->bc_dun[0];
+	u64 dun_hi = crypt_ctx->bc_dun[1];
+	unsigned int i;
+
+	/* If FMP wasn't enabled, we shouldn't get any encrypted requests. */
+	if (WARN_ON_ONCE(!(hba->caps & UFSHCD_CAP_CRYPTO)))
+		return -EIO;
+
+	/* Configure FMP on each segment of the request. */
+	for (i = 0; i < num_segments; i++) {
+		struct fmp_sg_entry *prd = &fmp_prdt[i];
+		int j;
+
+		/* Each segment must be exactly one data unit. */
+		if (prd->base.size != cpu_to_le32(FMP_DATA_UNIT_SIZE - 1)) {
+			dev_err(hba->dev,
+				"data segment is misaligned for FMP\n");
+			return -EIO;
+		}
+
+		/* Set the algorithm and key length. */
+		prd->base.size |= cpu_to_le32((FMP_ALGO_MODE_AES_XTS << 28) |
+					      (FMP_KEYLEN_256BIT << 26));
+
+		/* Set the IV. */
+		prd->file_iv[0] = cpu_to_be64(dun_hi);
+		prd->file_iv[1] = cpu_to_be64(dun_lo);
+
+		/* Set the key. */
+		for (j = 0; j < AES_KEYSIZE_256 / sizeof(u64); j++) {
+			prd->file_enckey[j] = fmp_key_word(enckey, j);
+			prd->file_twkey[j] = fmp_key_word(twkey, j);
+		}
+
+		/* Increment the data unit number. */
+		dun_lo++;
+		if (dun_lo == 0)
+			dun_hi++;
+	}
+	return 0;
+}
+
+#else /* CONFIG_SCSI_UFS_CRYPTO */
+
+static void exynos_ufs_fmp_init(struct ufs_hba *hba)
+{
+}
+
+static void exynos_ufs_fmp_resume(struct ufs_hba *hba)
+{
+}
+
+#define exynos_ufs_fmp_fill_prdt NULL
+
+#endif /* !CONFIG_SCSI_UFS_CRYPTO */
+
 static int exynos_ufs_init(struct ufs_hba *hba)
 {
 	struct device *dev = hba->dev;
 	struct platform_device *pdev = to_platform_device(dev);
 	struct exynos_ufs *ufs;
@@ -1196,10 +1410,12 @@  static int exynos_ufs_init(struct ufs_hba *hba)
 		goto out;
 	}
 
 	exynos_ufs_priv_init(hba, ufs);
 
+	exynos_ufs_fmp_init(hba);
+
 	if (ufs->drv_data->drv_init) {
 		ret = ufs->drv_data->drv_init(dev, ufs);
 		if (ret) {
 			dev_err(dev, "failed to init drv-data\n");
 			goto out;
@@ -1430,11 +1646,11 @@  static int exynos_ufs_resume(struct ufs_hba *hba, enum ufs_pm_op pm_op)
 
 	if (!ufshcd_is_link_active(hba))
 		phy_power_on(ufs->phy);
 
 	exynos_ufs_config_smu(ufs);
-
+	exynos_ufs_fmp_resume(hba);
 	return 0;
 }
 
 static int exynosauto_ufs_vh_link_startup_notify(struct ufs_hba *hba,
 						 enum ufs_notify_change_status status)
@@ -1696,10 +1912,11 @@  static const struct ufs_hba_variant_ops ufs_hba_exynos_ops = {
 	.setup_xfer_req			= exynos_ufs_specify_nexus_t_xfer_req,
 	.setup_task_mgmt		= exynos_ufs_specify_nexus_t_tm_req,
 	.hibern8_notify			= exynos_ufs_hibern8_notify,
 	.suspend			= exynos_ufs_suspend,
 	.resume				= exynos_ufs_resume,
+	.fill_crypto_prdt		= exynos_ufs_fmp_fill_prdt,
 };
 
 static struct ufs_hba_variant_ops ufs_hba_exynosauto_vh_ops = {
 	.name				= "exynosauto_ufs_vh",
 	.init				= exynosauto_ufs_vh_init,