efi/optee: support SetVariablesetvar2

This is a very rough PoC that works.
It's still early work and I probaly have to change the logic of swaping
the efivar callbacks. We are probably better of registering a config
table from the bootloader and swapping the functions directly in
drivers/firmware/efi/efi.c, during efivars_register().

It still needs some thinking since we have to at least:
1. Make sure op-tee is up before poking the variables
2. Disable EFI pstore

Signed-off-by: Ilias Apalodimas <ilias.apalodimas@linaro.org>

9 files changed, 925 insertions, 1 deletions

diff --git a/drivers/firmware/efi/vars.c b/drivers/firmware/efi/vars.c
index 41c1d00bf933..86909fae2f95 100644
--- a/drivers/firmware/efi/vars.c
+++ b/drivers/firmware/efi/vars.c
@@ -1208,6 +1208,21 @@ out:
 }
 EXPORT_SYMBOL_GPL(efivars_unregister);
 
+int efivars_swap_ops(const struct efivar_operations *ops)
+{
+	if (down_interruptible(&efivars_lock))
+		return -EINTR;
+
+	__efivars->ops = ops;
+
+	pr_info("Changed efivars operations\n");
+
+	up(&efivars_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(efivars_swap_ops);
+
 int efivar_supports_writes(void)
 {
 	return __efivars && __efivars->ops->set_variable;
diff --git a/drivers/tee/Kconfig b/drivers/tee/Kconfig
index e99d840c2511..668f6e227c54 100644
--- a/drivers/tee/Kconfig
+++ b/drivers/tee/Kconfig
@@ -16,6 +16,7 @@ if TEE
 menu "TEE drivers"
 
 source "drivers/tee/optee/Kconfig"
+source "drivers/tee/optee/stmm/Kconfig"
 source "drivers/tee/amdtee/Kconfig"
 endmenu
 
diff --git a/drivers/tee/optee/Makefile b/drivers/tee/optee/Makefile
index 56263ae3b1d7..3872fd49e85d 100644
--- a/drivers/tee/optee/Makefile
+++ b/drivers/tee/optee/Makefile
@@ -1,8 +1,10 @@
 # SPDX-License-Identifier: GPL-2.0
 obj-$(CONFIG_OPTEE) += optee.o
+obj-y += stmm/
 optee-objs += core.o
 optee-objs += call.o
 optee-objs += rpc.o
 optee-objs += supp.o
 optee-objs += shm_pool.o
 optee-objs += device.o
+optee-objs += stmm/optee_stmm_efi.o
diff --git a/drivers/tee/optee/core.c b/drivers/tee/optee/core.c
index cf4718c6d35d..ce3010c23ce4 100644
--- a/drivers/tee/optee/core.c
+++ b/drivers/tee/optee/core.c
@@ -709,6 +709,14 @@ static int optee_probe(struct platform_device *pdev)
 		return rc;
 	}
 
+	/*
+	 * Check if OP-TEE can provide EFI runtime variable support and
+	 * switch
+	 */
+	rc = efi_init_variables();
+	if (rc)
+		printk("Can't use OP-TEE backed EFI variables\n");
+
 	pr_info("initialized driver\n");
 	return 0;
 err:
@@ -726,6 +734,7 @@ err:
 		tee_shm_pool_free(pool);
 	if (memremaped_shm)
 		memunmap(memremaped_shm);
+
 	return rc;
 }
 
diff --git a/drivers/tee/optee/optee_private.h b/drivers/tee/optee/optee_private.h
index e25b216a14ef..07d3b7981679 100644
--- a/drivers/tee/optee/optee_private.h
+++ b/drivers/tee/optee/optee_private.h
@@ -41,6 +41,7 @@ struct optee_wait_queue {
 	struct list_head db;
 };
 
+int efi_init_variables(void);
 /**
  * struct optee_supp - supplicant synchronization struct
  * @ctx			the context of current connected supplicant.
diff --git a/drivers/tee/optee/stmm/Kconfig b/drivers/tee/optee/stmm/Kconfig
new file mode 100644
index 000000000000..02bed8fb3edf
--- /dev/null
+++ b/drivers/tee/optee/stmm/Kconfig
@@ -0,0 +1,13 @@
+# SPDX-License-Identifier: GPL-2.0
+# Kconfig for OPTEE backed EFI variables
+
+config OPTEE_EFI_VARS
+	bool "OP-TEE backed EFI variables storage"
+	help
+	  If OP-TEE is compiled to include StandAloneMM as a separate 
+	  secure partition it has the ability to check and store EFI 
+	  variables on an RPMB or any other non-volatile medium used by
+	  StandAloneMM.
+	  
+	  Enabling this will change the EFI runtime services from the firmware
+	  provided functions to OP-TEE calls.
diff --git a/drivers/tee/optee/stmm/mm_communication.h b/drivers/tee/optee/stmm/mm_communication.h
new file mode 100644
index 000000000000..46d177696949
--- /dev/null
+++ b/drivers/tee/optee/stmm/mm_communication.h
@@ -0,0 +1,248 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ *  Headers for EFI variable service via StandAloneMM, EDK2 application running
+ *  in OP-TEE
+ *
+ *  Copyright (c) 2017, Intel Corporation. All rights reserved.
+ *  Copyright (C) 2020 Linaro Ltd.
+ */
+
+#ifndef _MM_COMMUNICATION_H_
+#define _MM_COMMUNICATION_H_
+
+/*
+ * Interface to the pseudo Trusted Application (TA), which provides a
+ * communication channel with the Standalone MM (Management Mode)
+ * Secure Partition running at Secure-EL0
+ */
+
+#define PTA_STMM_CMDID_COMMUNICATE 0
+
+/* OP-TEE is using big endian GUIDs while UEFI uses little endian ones */
+#define PTA_STMM_UUID { 0xed32d533, 0x99e6, 0x4209, {\
+			0x9c, 0xc0, 0x2d, 0x72, 0xcd, 0xd9, 0x98, 0xa7 } }
+
+#define EFI_MM_VARIABLE_GUID \
+	EFI_GUID(0xed32d533, 0x99e6, 0x4209, \
+		 0x9c, 0xc0, 0x2d, 0x72, 0xcd, 0xd9, 0x98, 0xa7)
+
+/* Defined in EDK2 MdePkg/Include/Protocol/MmCommunication.h */
+
+/**
+ * struct efi_mm_communicate_header - Header used for SMM variable communication
+
+ * @header_guid:  header use for disambiguation of content
+ * @message_len:  length of the message. Does not include the size of the
+ *                header
+ * @data:         payload of the message
+ *
+ * Defined in EDK2 as EFI_MM_COMMUNICATE_HEADER.
+ * To avoid confusion in interpreting frames, the communication buffer should
+ * always begin with efi_mm_communicate_header.
+ */
+struct efi_mm_communicate_header {
+	efi_guid_t header_guid;
+	size_t     message_len;
+	u8         data[];
+} __packed;
+
+#define MM_COMMUNICATE_HEADER_SIZE \
+	(sizeof(struct efi_mm_communicate_header))
+
+/* Defined in EDK2 ArmPkg/Include/IndustryStandard/ArmMmSvc.h */
+
+/* SPM return error codes */
+#define ARM_SVC_SPM_RET_SUCCESS               0
+#define ARM_SVC_SPM_RET_NOT_SUPPORTED        -1
+#define ARM_SVC_SPM_RET_INVALID_PARAMS       -2
+#define ARM_SVC_SPM_RET_DENIED               -3
+#define ARM_SVC_SPM_RET_NO_MEMORY            -5
+
+/* Defined in EDK2 MdeModulePkg/Include/Guid/SmmVariableCommon.h */
+
+#define SMM_VARIABLE_FUNCTION_GET_VARIABLE  1
+/*
+ * The payload for this function is
+ * SMM_VARIABLE_COMMUNICATE_GET_NEXT_VARIABLE_NAME.
+ */
+#define SMM_VARIABLE_FUNCTION_GET_NEXT_VARIABLE_NAME  2
+/*
+ * The payload for this function is SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE.
+ */
+#define SMM_VARIABLE_FUNCTION_SET_VARIABLE  3
+/*
+ * The payload for this function is
+ * SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO.
+ */
+#define SMM_VARIABLE_FUNCTION_QUERY_VARIABLE_INFO  4
+/*
+ * It is a notify event, no extra payload for this function.
+ */
+#define SMM_VARIABLE_FUNCTION_READY_TO_BOOT  5
+/*
+ * It is a notify event, no extra payload for this function.
+ */
+#define SMM_VARIABLE_FUNCTION_EXIT_BOOT_SERVICE  6
+/*
+ * The payload for this function is VARIABLE_INFO_ENTRY.
+ * The GUID in EFI_SMM_COMMUNICATE_HEADER is gEfiSmmVariableProtocolGuid.
+ */
+#define SMM_VARIABLE_FUNCTION_GET_STATISTICS  7
+/*
+ * The payload for this function is SMM_VARIABLE_COMMUNICATE_LOCK_VARIABLE
+ */
+#define SMM_VARIABLE_FUNCTION_LOCK_VARIABLE   8
+
+#define SMM_VARIABLE_FUNCTION_VAR_CHECK_VARIABLE_PROPERTY_SET  9
+
+#define SMM_VARIABLE_FUNCTION_VAR_CHECK_VARIABLE_PROPERTY_GET  10
+
+#define SMM_VARIABLE_FUNCTION_GET_PAYLOAD_SIZE  11
+/*
+ * The payload for this function is
+ * SMM_VARIABLE_COMMUNICATE_RUNTIME_VARIABLE_CACHE_CONTEXT
+ */
+#define SMM_VARIABLE_FUNCTION_INIT_RUNTIME_VARIABLE_CACHE_CONTEXT 12
+
+#define SMM_VARIABLE_FUNCTION_SYNC_RUNTIME_CACHE  13
+/*
+ * The payload for this function is
+ * SMM_VARIABLE_COMMUNICATE_GET_RUNTIME_CACHE_INFO
+ */
+#define SMM_VARIABLE_FUNCTION_GET_RUNTIME_CACHE_INFO  14
+
+/**
+ * struct smm_variable_communicate_header - Used for SMM variable communication
+
+ * @function:     function to call in Smm.
+ * @ret_status:   return status
+ * @data:         payload
+ *
+ * Defined in EDK2 as SMM_VARIABLE_COMMUNICATE_HEADER.
+ */
+struct smm_variable_communicate_header {
+	size_t  function;
+	efi_status_t ret_status;
+	u8 data[];
+};
+
+#define MM_VARIABLE_COMMUNICATE_SIZE \
+	(sizeof(struct smm_variable_communicate_header))
+
+/**
+ * struct smm_variable_access - Used to communicate with StMM by
+ *                              SetVariable and GetVariable.
+
+ * @guid:         vendor GUID
+ * @data_size:    size of EFI variable data
+ * @name_size:    size of EFI name
+ * @attr:         attributes
+ * @name:         variable name
+ *
+ * Defined in EDK2 as SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE.
+ *
+ */
+struct smm_variable_access {
+	efi_guid_t  guid;
+	size_t data_size;
+	size_t name_size;
+	u32 attr;
+	u16 name[];
+};
+
+#define MM_VARIABLE_ACCESS_HEADER_SIZE \
+	(sizeof(struct smm_variable_access))
+/**
+ * struct smm_variable_payload_size - Used to get the max allowed
+ *                                    payload used in StMM.
+ *
+ * @size:  size to fill in
+ *
+ * Defined in EDK2 as SMM_VARIABLE_COMMUNICATE_GET_PAYLOAD_SIZE.
+ *
+ */
+struct smm_variable_payload_size {
+	size_t size;
+};
+
+/**
+ * struct smm_variable_getnext - Used to communicate with StMM for
+ *                               GetNextVariableName.
+ *
+ * @guid:       vendor GUID
+ * @name_size:  size of the name of the variable
+ * @name:       variable name
+ *
+ * Defined in EDK2 as SMM_VARIABLE_COMMUNICATE_GET_NEXT_VARIABLE_NAME.
+ */
+struct smm_variable_getnext {
+	efi_guid_t  guid;
+	size_t name_size;
+	u16         name[];
+};
+
+#define MM_VARIABLE_GET_NEXT_HEADER_SIZE \
+	(sizeof(struct smm_variable_getnext))
+
+/**
+ * struct smm_variable_query_info - Used to communicate with StMM for
+ *                                  QueryVariableInfo.
+ *
+ * @max_variable_storage:        max available storage
+ * @remaining_variable_storage:  remaining available storage
+ * @max_variable_size:           max variable supported size
+ * @attr:                        attributes to query storage for
+ *
+ * Defined in EDK2 as SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO.
+ */
+struct smm_variable_query_info {
+	u64 max_variable_storage;
+	u64 remaining_variable_storage;
+	u64 max_variable_size;
+	u32 attr;
+};
+
+#define VAR_CHECK_VARIABLE_PROPERTY_REVISION 0x0001
+#define VAR_CHECK_VARIABLE_PROPERTY_READ_ONLY BIT(0)
+/**
+ * struct var_check_property - Used to store variable properties in StMM
+ *
+ * @revision:   magic revision number for variable property checking
+ * @property:   properties mask for the variable used in StMM.
+ *              Currently RO flag is supported
+ * @attributes: variable attributes used in StMM checking when properties
+ *              for a variable are enabled
+ * @minsize:    minimum allowed size for variable payload checked against
+ *              smm_variable_access->datasize in StMM
+ * @maxsize:    maximum allowed size for variable payload checked against
+ *              smm_variable_access->datasize in StMM
+ *
+ * Defined in EDK2 as VAR_CHECK_VARIABLE_PROPERTY.
+ */
+struct var_check_property {
+	u16 revision;
+	u16 property;
+	u32 attributes;
+	size_t minsize;
+	size_t maxsize;
+};
+
+/**
+ * struct smm_variable_var_check_property - Used to communicate variable
+ *                                          properties with StMM
+ *
+ * @guid:       vendor GUID
+ * @name_size:  size of EFI name
+ * @property:   variable properties struct
+ * @name:       variable name
+ *
+ * Defined in EDK2 as SMM_VARIABLE_COMMUNICATE_VAR_CHECK_VARIABLE_PROPERTY.
+ */
+struct smm_variable_var_check_property {
+	efi_guid_t guid;
+	size_t name_size;
+	struct var_check_property property;
+	u16 name[];
+};
+
+#endif /* _MM_COMMUNICATION_H_ */
diff --git a/drivers/tee/optee/stmm/optee_stmm_efi.c b/drivers/tee/optee/stmm/optee_stmm_efi.c
new file mode 100644
index 000000000000..e4c44851b1d0
--- /dev/null
+++ b/drivers/tee/optee/stmm/optee_stmm_efi.c
@@ -0,0 +1,633 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ *  EFI variable service via OP-TEE
+ *
+ *  Copyright (C) 2021 Linaro
+ */
+
+// new
+#include <linux/efi.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/tee.h>
+#include <linux/tee_drv.h>
+#include <linux/ucs2_string.h>
+#include "mm_communication.h"
+#include "../optee_private.h"
+
+#define OPTEE_PAGE_SIZE BIT(12)
+static size_t max_buffer_size;	/* comm + var + func + data */
+static size_t max_payload_size;	/* func + data */
+static struct efivar_operations optee_ops;
+
+struct mm_connection {
+	struct tee_context *ctx;
+	u32 session;
+};
+
+//FIXME
+static int optee_ctx_match(struct tee_ioctl_version_data *ver, const void *data)
+{
+	if (ver->impl_id == TEE_IMPL_ID_OPTEE)
+		return 1;
+	else
+		return 0;
+}
+
+struct tee_optee_ta_uuid {
+	u32 time_low;
+	u16 time_mid;
+	u16 time_hi_and_version;
+	u8 clock_seq_and_node[8];
+};
+
+static void tee_optee_ta_uuid_to_octets(u8 d[16],
+                                 const struct tee_optee_ta_uuid *s)
+{
+	d[0] = s->time_low >> 24;
+	d[1] = s->time_low >> 16;
+	d[2] = s->time_low >> 8;
+	d[3] = s->time_low;
+	d[4] = s->time_mid >> 8;
+	d[5] = s->time_mid;
+	d[6] = s->time_hi_and_version >> 8;
+	d[7] = s->time_hi_and_version;
+	memcpy(d + 8, s->clock_seq_and_node, sizeof(s->clock_seq_and_node));
+}
+//FIXME END
+
+/**
+ * get_connection() - Retrieve OP-TEE session for a specific UUID.
+ *
+ * @conn:   session buffer to fill
+ * Return:  status code
+ */
+static int get_connection(struct mm_connection *conn)
+{
+	const struct tee_optee_ta_uuid uuid = PTA_STMM_UUID;
+	struct tee_context *ctx = NULL;
+	struct tee_ioctl_open_session_arg arg;
+	int rc;
+
+	memset(&arg, 0, sizeof(arg));
+
+	/* Open context with OP-TEE driver */
+	ctx = tee_client_open_context(NULL, optee_ctx_match, NULL, NULL);
+	if (IS_ERR(ctx))
+		return -ENODEV;
+
+	tee_optee_ta_uuid_to_octets(arg.uuid, &uuid);
+	rc = tee_client_open_session(ctx, &arg, NULL);
+	if (!rc) {
+		conn->ctx = ctx;
+		conn->session = arg.session;
+	}
+
+	return rc;
+}
+
+/**
+ * optee_mm_communicate() - Pass a buffer to StandaloneMM running in OP-TEE
+ *
+ * @comm_buf:		locally allocted communcation buffer
+ * @dsize:		buffer size
+ * Return:		status code
+ */
+static efi_status_t optee_mm_communicate(void *comm_buf, size_t dsize)
+{
+	size_t buf_size;
+	efi_status_t ret;
+	struct efi_mm_communicate_header *mm_hdr;
+	struct mm_connection conn = { NULL, 0 };
+	struct tee_ioctl_invoke_arg arg;
+	struct tee_param param[4];
+	struct tee_shm *shm = NULL;
+	int rc;
+
+	if (!comm_buf)
+		return EFI_INVALID_PARAMETER;
+
+	mm_hdr = (struct efi_mm_communicate_header *)comm_buf;
+	buf_size = mm_hdr->message_len + sizeof(efi_guid_t) + sizeof(size_t);
+
+	if (dsize != buf_size)
+		return EFI_INVALID_PARAMETER;
+
+	rc = get_connection(&conn);
+	if (rc) {
+		printk("Unable to open OP-TEE session (err=%d)\n", rc);
+		return EFI_UNSUPPORTED;
+	}
+
+	shm = tee_shm_register(conn.ctx, (unsigned long)comm_buf, buf_size,
+			       TEE_SHM_DMA_BUF | TEE_SHM_KERNEL_MAPPED);
+	if (IS_ERR(shm)) {
+		printk("Unable to register shared memory\n");
+		tee_client_close_session(conn.ctx, conn.session);
+		return EFI_UNSUPPORTED;
+	}
+
+	memset(&arg, 0, sizeof(arg));
+	arg.func = PTA_STMM_CMDID_COMMUNICATE;
+	arg.session = conn.session;
+	arg.num_params = 4;
+
+	memset(param, 0, sizeof(param));
+	param[0].attr = TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INOUT;
+	param[0].u.memref.size = buf_size;
+	param[0].u.memref.shm = shm;
+	param[1].attr = TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_OUTPUT;
+	param[2].attr = TEE_IOCTL_PARAM_ATTR_TYPE_NONE;
+	param[3].attr = TEE_IOCTL_PARAM_ATTR_TYPE_NONE;
+
+	rc = tee_client_invoke_func(conn.ctx, &arg, param);
+	tee_shm_free(shm);
+	/* even if close session fails the session will be invalidaded */
+	tee_client_close_session(conn.ctx, conn.session);
+	if (rc || arg.ret != 0) { // TEE_SUCCESS? FIXME
+		printk("ERR: %d %x\n", rc, arg.ret);
+		return EFI_DEVICE_ERROR;
+	}
+
+
+	switch (param[1].u.value.a) {
+	case ARM_SVC_SPM_RET_SUCCESS:
+		ret = EFI_SUCCESS;
+		break;
+
+	case ARM_SVC_SPM_RET_INVALID_PARAMS:
+		ret = EFI_INVALID_PARAMETER;
+		break;
+
+	case ARM_SVC_SPM_RET_DENIED:
+		ret = EFI_ACCESS_DENIED;
+		break;
+
+	case ARM_SVC_SPM_RET_NO_MEMORY:
+		ret = EFI_OUT_OF_RESOURCES;
+		break;
+
+	default:
+		ret = EFI_ACCESS_DENIED;
+	}
+
+	return ret;
+}
+
+/**
+ * mm_communicate() - Adjust the cmonnucation buffer to StandAlonneMM and send
+ * it to OP-TEE
+ *
+ * @comm_buf:		locally allocted communcation buffer
+ * @dsize:		buffer size
+ * Return:		status code
+ */
+static efi_status_t mm_communicate(u8 *comm_buf, size_t dsize)
+{
+	efi_status_t ret;
+	struct efi_mm_communicate_header *mm_hdr;
+	struct smm_variable_communicate_header *var_hdr;
+
+	dsize += MM_COMMUNICATE_HEADER_SIZE + MM_VARIABLE_COMMUNICATE_SIZE;
+	mm_hdr = (struct efi_mm_communicate_header *)comm_buf;
+	var_hdr = (struct smm_variable_communicate_header *)mm_hdr->data;
+
+	ret = optee_mm_communicate(comm_buf, dsize);
+	if (ret != EFI_SUCCESS) {
+		printk("%s failed!\n", __func__);
+		return ret;
+	}
+
+	return var_hdr->ret_status;
+}
+
+/**
+ * setup_mm_hdr() -	Allocate a buffer for StandAloneMM and initialize the
+ *			header data.
+ *
+ * @dptr:		pointer address of the corresponding StandAloneMM
+ *			function
+ * @payload_size:	buffer size
+ * @func:		standAloneMM function number
+ * @ret:		EFI return code
+ * Return:		buffer or NULL
+ */
+static u8 *setup_mm_hdr(void **dptr, size_t payload_size,
+			size_t func, efi_status_t *ret)
+{
+	const efi_guid_t mm_var_guid = EFI_MM_VARIABLE_GUID;
+	struct efi_mm_communicate_header *mm_hdr;
+	struct smm_variable_communicate_header *var_hdr;
+	u8 *comm_buf;
+
+	/* In the init function we initialize max_buffer_size with
+	 * get_max_payload(). So skip the test if max_buffer_size is initialized
+	 * StandAloneMM will perform similar checks and drop the buffer if it's
+	 * too long
+	 */
+	if (max_buffer_size && max_buffer_size <
+			(MM_COMMUNICATE_HEADER_SIZE +
+			 MM_VARIABLE_COMMUNICATE_SIZE +
+			 payload_size)) {
+		*ret = EFI_INVALID_PARAMETER;
+		return NULL;
+	}
+
+	comm_buf = kzalloc(MM_COMMUNICATE_HEADER_SIZE +
+			   MM_VARIABLE_COMMUNICATE_SIZE + payload_size,
+			   GFP_KERNEL);
+	if (!comm_buf) {
+		*ret = EFI_OUT_OF_RESOURCES;
+		return NULL;
+	}
+
+	mm_hdr = (struct efi_mm_communicate_header *)comm_buf;
+	memcpy(&mm_hdr->header_guid, &mm_var_guid, sizeof(mm_hdr->header_guid));
+	mm_hdr->message_len = MM_VARIABLE_COMMUNICATE_SIZE + payload_size;
+
+	var_hdr = (struct smm_variable_communicate_header *)mm_hdr->data;
+	var_hdr->function = func;
+	if (dptr)
+		*dptr = var_hdr->data;
+	*ret = EFI_SUCCESS;
+
+	return comm_buf;
+}
+
+/**
+ * get_max_payload() - Get variable payload size from StandAloneMM.
+ *
+ * @size:    size of the variable in storage
+ * Return:   status code
+ */
+static efi_status_t get_max_payload(size_t *size)
+{
+	struct smm_variable_payload_size *var_payload = NULL;
+	size_t payload_size;
+	u8 *comm_buf = NULL;
+	efi_status_t ret;
+
+	if (!size) {
+		ret = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+
+	payload_size = sizeof(*var_payload);
+	comm_buf = setup_mm_hdr((void **)&var_payload, payload_size,
+				SMM_VARIABLE_FUNCTION_GET_PAYLOAD_SIZE, &ret);
+	if (!comm_buf)
+		goto out;
+
+	ret = mm_communicate(comm_buf, payload_size);
+	if (ret != EFI_SUCCESS)
+		goto out;
+
+	/* Make sure the buffer is big enough for storing variables */
+	if (var_payload->size < MM_VARIABLE_ACCESS_HEADER_SIZE + 0x20) {
+		ret = EFI_DEVICE_ERROR;
+		goto out;
+	}
+	*size = var_payload->size;
+	/*
+	 * Although the max payload is configurable on StMM, we only share a
+	 * single page from OP-TEE for the non-secure buffer used to communicate
+	 * with StMM. Since OP-TEE will reject to map anything bigger than that,
+	 * make sure we are in bounds.
+	 */
+	if (*size > OPTEE_PAGE_SIZE)
+		*size = OPTEE_PAGE_SIZE - MM_COMMUNICATE_HEADER_SIZE  -
+			MM_VARIABLE_COMMUNICATE_SIZE;
+	/*
+	 * There seems to be a bug in EDK2 miscalculating the boundaries and
+	 * size checks, so deduct 2 more bytes to fulfill this requirement. Fix
+	 * it up here to ensure backwards compatibility with older versions
+	 * (cf. StandaloneMmPkg/Drivers/StandaloneMmCpu/AArch64/EventHandle.c.
+	 * sizeof (EFI_MM_COMMUNICATE_HEADER) instead the size minus the
+	 * flexible array member).
+	 *
+	 * size is guaranteed to be > 2 due to checks on the beginning.
+	 */
+	*size -= 2;
+out:
+	kfree(comm_buf);
+	return ret;
+}
+
+static efi_status_t get_property_int(u16 *name, size_t name_size,
+				     const efi_guid_t *vendor,
+				     struct var_check_property *var_property)
+{
+	struct smm_variable_var_check_property *smm_property;
+	size_t payload_size;
+	u8 *comm_buf = NULL;
+	efi_status_t ret;
+
+	memset(var_property, 0, sizeof(*var_property));
+	payload_size = sizeof(*smm_property) + name_size;
+	if (payload_size > max_payload_size) {
+		ret = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+	comm_buf = setup_mm_hdr((void **)&smm_property, payload_size,
+				SMM_VARIABLE_FUNCTION_VAR_CHECK_VARIABLE_PROPERTY_GET,
+				&ret);
+	if (!comm_buf)
+		goto out;
+
+	memcpy(&smm_property->guid, vendor, sizeof(smm_property->guid));
+	smm_property->name_size = name_size;
+	memcpy(smm_property->name, name, name_size);
+
+	ret = mm_communicate(comm_buf, payload_size);
+	/*
+	 * Currently only R/O property is supported in StMM.
+	 * Variables that are not set to R/O will not set the property in StMM
+	 * and the call will return EFI_NOT_FOUND. We are setting the
+	 * properties to 0x0 so checking against that is enough for the
+	 * EFI_NOT_FOUND case.
+	 */
+	if (ret == EFI_NOT_FOUND)
+		ret = EFI_SUCCESS;
+	if (ret != EFI_SUCCESS)
+		goto out;
+	memcpy(var_property, &smm_property->property, sizeof(*var_property));
+
+out:
+	kfree(comm_buf);
+	return ret;
+}
+
+static efi_status_t optee_get_variable(u16 *name, efi_guid_t *vendor, u32 *attributes,
+				unsigned long *data_size, void *data)
+{
+	struct var_check_property var_property;
+	struct smm_variable_access *var_acc;
+	size_t payload_size;
+	size_t name_size;
+	size_t tmp_dsize;
+	u8 *comm_buf = NULL;
+	efi_status_t ret;
+
+	if (!name || !vendor || !data_size) {
+		ret = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+	//FIXME function enters 3 times??
+	/* Check payload size */
+	//FIXME
+	name_size = (ucs2_strnlen(name, EFI_VAR_NAME_LEN) + 1) * sizeof(u16);
+
+	if (name_size > max_payload_size - MM_VARIABLE_ACCESS_HEADER_SIZE) {
+		ret = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+
+	/* Trim output buffer size */
+	tmp_dsize = *data_size;
+	if (name_size + tmp_dsize >
+			max_payload_size - MM_VARIABLE_ACCESS_HEADER_SIZE) {
+		tmp_dsize = max_payload_size -
+				MM_VARIABLE_ACCESS_HEADER_SIZE -
+				name_size;
+	}
+
+	/* Get communication buffer and initialize header */
+	payload_size = MM_VARIABLE_ACCESS_HEADER_SIZE + name_size + tmp_dsize;
+	comm_buf = setup_mm_hdr((void **)&var_acc, payload_size,
+				SMM_VARIABLE_FUNCTION_GET_VARIABLE, &ret);
+	if (!comm_buf)
+		goto out;
+
+
+	/* Fill in contents */
+	memcpy(&var_acc->guid, vendor, sizeof(var_acc->guid));
+	var_acc->data_size = tmp_dsize;
+	var_acc->name_size = name_size;
+	var_acc->attr = attributes ? *attributes : 0;
+	memcpy(var_acc->name, name, name_size);
+
+	/* Communicate */
+	ret = mm_communicate(comm_buf, payload_size);
+	if (ret == EFI_SUCCESS || ret == EFI_BUFFER_TOO_SMALL)
+		/* Update with reported data size for trimmed case */
+		*data_size = var_acc->data_size;
+	if (ret != EFI_SUCCESS)
+		goto out;
+
+	ret = get_property_int(name, name_size, vendor, &var_property);
+	if (ret != EFI_SUCCESS) {
+		printk("AA %d\n", __LINE__);
+		goto out;
+	}
+
+	if (attributes)
+		*attributes = var_acc->attr;
+
+	if (data)
+		memcpy(data, (u8 *)var_acc->name + var_acc->name_size,
+		       var_acc->data_size);
+	else
+		ret = EFI_INVALID_PARAMETER;
+
+out:
+	kfree(comm_buf);
+	return ret;
+}
+
+static efi_status_t optee_get_next_variable(unsigned long *name_size,
+				     efi_char16_t *name, efi_guid_t *guid)
+{
+	struct smm_variable_getnext *var_getnext;
+	size_t payload_size;
+	size_t out_name_size;
+	size_t in_name_size;
+	u8 *comm_buf = NULL;
+	efi_status_t ret;
+
+	if (!name_size || !name || !guid) {
+		ret = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+
+	out_name_size = *name_size;
+	in_name_size = (ucs2_strnlen(name, EFI_VAR_NAME_LEN) + 1) * sizeof(u16);
+
+	//if (out_name_size < in_name_size) {
+		//ret = EFI_INVALID_PARAMETER;
+		//goto out;
+	//}
+
+	if (in_name_size > max_payload_size - MM_VARIABLE_GET_NEXT_HEADER_SIZE) {
+		ret = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+
+	/* Trim output buffer size */
+	if (out_name_size > max_payload_size - MM_VARIABLE_GET_NEXT_HEADER_SIZE)
+		out_name_size = max_payload_size - MM_VARIABLE_GET_NEXT_HEADER_SIZE;
+
+	payload_size = MM_VARIABLE_GET_NEXT_HEADER_SIZE + out_name_size;
+	comm_buf = setup_mm_hdr((void **)&var_getnext, payload_size,
+				SMM_VARIABLE_FUNCTION_GET_NEXT_VARIABLE_NAME,
+				&ret);
+	if (!comm_buf)
+		goto out;
+
+	/* Fill in contents */
+	//FIXME overflow?
+	memcpy(&var_getnext->guid, guid, sizeof(var_getnext->guid));
+	var_getnext->name_size = out_name_size;
+	memcpy(var_getnext->name, name, in_name_size);
+
+	if (in_name_size < out_name_size)
+		memset((u8 *)var_getnext->name + in_name_size, 0x0,
+		       out_name_size - in_name_size);
+
+	/* Communicate */
+	ret = mm_communicate(comm_buf, payload_size);
+	if (ret == EFI_SUCCESS || ret == EFI_BUFFER_TOO_SMALL) {
+		/* Update with reported data size for trimmed case */
+		*name_size = var_getnext->name_size;
+	}
+	if (ret != EFI_SUCCESS)
+		goto out;
+
+	memcpy(guid, &var_getnext->guid, sizeof(*guid));
+	memcpy(name, var_getnext->name, var_getnext->name_size);
+
+out:
+	kfree(comm_buf);
+	return ret;
+}
+
+static efi_status_t optee_set_variable(efi_char16_t *name, efi_guid_t *vendor,
+				u32 attributes, unsigned long data_size,
+				void *data)
+{
+	efi_status_t ret;
+	struct var_check_property var_property;
+	struct smm_variable_access *var_acc;
+	size_t payload_size;
+	size_t name_size;
+	u8 *comm_buf = NULL;
+
+	if (!name || name[0] == 0 || !vendor) {
+		ret = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+	if (data_size > 0 && !data) {
+		ret = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+	/* Check payload size */
+	name_size = (ucs2_strnlen(name, EFI_VAR_NAME_LEN) + 1) * sizeof(u16);
+	payload_size = MM_VARIABLE_ACCESS_HEADER_SIZE + name_size + data_size;
+	if (payload_size > max_payload_size) {
+		ret = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+
+	/*
+	 * Allocate the buffer early, before switching to RW (if needed)
+	 * so we won't need to account for any failures in reading/setting
+	 * the properties, if the allocation fails
+	 */
+	comm_buf = setup_mm_hdr((void **)&var_acc, payload_size,
+				SMM_VARIABLE_FUNCTION_SET_VARIABLE, &ret);
+	if (!comm_buf)
+		goto out;
+
+	/*
+	 * The API has the ability to override RO flags. If no RO check was
+	 * requested switch the variable to RW for the duration of this call
+	 */
+	ret = get_property_int(name, name_size, vendor,
+			       &var_property);
+	if (ret != EFI_SUCCESS) {
+		printk("Getting variable property failed\n");
+		goto out;
+	}
+
+	if (var_property.property & VAR_CHECK_VARIABLE_PROPERTY_READ_ONLY) {
+		ret = EFI_WRITE_PROTECTED;
+		goto out;
+	}
+
+	/* Fill in contents */
+	memcpy(&var_acc->guid, vendor, sizeof(var_acc->guid));
+	var_acc->data_size = data_size;
+	var_acc->name_size = name_size;
+	var_acc->attr = attributes;
+	memcpy(var_acc->name, name, name_size);
+	memcpy((u8 *)var_acc->name + name_size, data, data_size);
+
+	/* Communicate */
+	ret = mm_communicate(comm_buf, payload_size);
+	printk("Set Variable %s %d %lx\n", __FILE__, __LINE__, ret);
+out:
+	kfree(comm_buf);
+	return ret;
+}
+
+static efi_status_t optee_query_variable_info(u32 attributes,
+				       u64 *max_variable_storage_size,
+				       u64 *remain_variable_storage_size,
+				       u64 *max_variable_size)
+{
+	struct smm_variable_query_info *mm_query_info;
+	size_t payload_size;
+	efi_status_t ret;
+	u8 *comm_buf;
+
+	payload_size = sizeof(*mm_query_info);
+	comm_buf = setup_mm_hdr((void **)&mm_query_info, payload_size,
+				SMM_VARIABLE_FUNCTION_QUERY_VARIABLE_INFO,
+				&ret);
+	if (!comm_buf)
+		goto out;
+
+	mm_query_info->attr = attributes;
+	ret = mm_communicate(comm_buf, payload_size);
+	if (ret != EFI_SUCCESS)
+		goto out;
+	*max_variable_storage_size = mm_query_info->max_variable_storage;
+	*remain_variable_storage_size =
+			mm_query_info->remaining_variable_storage;
+	*max_variable_size = mm_query_info->max_variable_size;
+
+out:
+	kfree(comm_buf);
+	return ret;
+}
+
+/**
+ * efi_init_variables() - initialize variable services
+ *
+ * Return:	0 on success
+ */
+int efi_init_variables(void)
+{
+	efi_status_t ret;
+	int rc = 0;
+
+	printk("AA: Enter variables\n");
+	/* FIXME convert ret tro int and map errors to EFI errors */
+	ret = get_max_payload(&max_payload_size);
+	if (ret != EFI_SUCCESS)
+		return -EINVAL;
+	printk("AA: Got max payload %zu variables\n", max_payload_size);
+
+	max_buffer_size = MM_COMMUNICATE_HEADER_SIZE +
+			  MM_VARIABLE_COMMUNICATE_SIZE +
+			  max_payload_size;
+
+	optee_ops.get_variable = optee_get_variable;
+	optee_ops.get_next_variable = optee_get_next_variable;
+	optee_ops.set_variable = optee_set_variable;
+	optee_ops.set_variable_nonblocking = optee_set_variable;
+	rc = efivars_swap_ops(&optee_ops);
+	printk("Swapped var ops\n");
+
+	return rc;
+}
+EXPORT_SYMBOL(efi_init_variables);
diff --git a/include/linux/efi.h b/include/linux/efi.h
index d7c0e73af2b9..715b93f3fcc3 100644
--- a/include/linux/efi.h
+++ b/include/linux/efi.h
@@ -39,6 +39,7 @@
 #define EFI_WRITE_PROTECTED	( 8 | (1UL << (BITS_PER_LONG-1)))
 #define EFI_OUT_OF_RESOURCES	( 9 | (1UL << (BITS_PER_LONG-1)))
 #define EFI_NOT_FOUND		(14 | (1UL << (BITS_PER_LONG-1)))
+#define EFI_ACCESS_DENIED	(15 | (1UL << (BITS_PER_LONG-1)))
 #define EFI_TIMEOUT		(18 | (1UL << (BITS_PER_LONG-1)))
 #define EFI_ABORTED		(21 | (1UL << (BITS_PER_LONG-1)))
 #define EFI_SECURITY_VIOLATION	(26 | (1UL << (BITS_PER_LONG-1)))
@@ -238,7 +239,7 @@ typedef efi_status_t efi_get_variable_t (efi_char16_t *name, efi_guid_t *vendor,
 					 unsigned long *data_size, void *data);
 typedef efi_status_t efi_get_next_variable_t (unsigned long *name_size, efi_char16_t *name,
 					      efi_guid_t *vendor);
-typedef efi_status_t efi_set_variable_t (efi_char16_t *name, efi_guid_t *vendor, 
+typedef efi_status_t efi_set_variable_t (efi_char16_t *name, efi_guid_t *vendor,
 					 u32 attr, unsigned long data_size,
 					 void *data);
 typedef efi_status_t efi_get_next_high_mono_count_t (u32 *count);
@@ -997,6 +998,7 @@ int efivars_register(struct efivars *efivars,
 		     const struct efivar_operations *ops,
 		     struct kobject *kobject);
 int efivars_unregister(struct efivars *efivars);
+int efivars_swap_ops(const struct efivar_operations *ops);
 struct kobject *efivars_kobject(void);
 
 int efivar_supports_writes(void);