OP-TEE Benchmark **not for mainline**

Add Benchmark support

Reviewed-by: Joakim Bech <joakim.bech@linaro.org>
Signed-off-by: Igor Opaniuk <igor.opaniuk@linaro.org>

drivers/tee/optee/Kconfig

@@ -5,3 +5,10 @@ config OPTEE
 	help
 	  This implements the OP-TEE Trusted Execution Environment (TEE)
 	  driver.
+
+config OPTEE_BENCHMARK
+	bool "OP-TEE Benchmark (EXPERIMENTAL)"
+	depends on OPTEE
+	help
+	  This enables benchmarking feature in the OP-TEE Trusted
+	  Execution Environment (TEE) driver.

drivers/tee/optee/Makefile

@@ -3,3 +3,4 @@ optee-objs += core.o
 optee-objs += call.o
 optee-objs += rpc.o
 optee-objs += supp.o
+optee-objs += bench.o

drivers/tee/optee/bench.c

@@ -0,0 +1,157 @@
+/*
+ * Copyright (c) 2017, Linaro Limited
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/smp.h>
+#include "optee_bench.h"
+
+/*
+ * Specific defines for ARM performance timers
+ */
+/* aarch32 */
+#define OPTEE_BENCH_DEF_OPTS			(1 | 16)
+#define OPTEE_BENCH_DEF_OVER			0x8000000f
+/* enable 64 divider for CCNT */
+#define OPTEE_BENCH_DIVIDER_OPTS		(OPTEE_BENCH_DEF_OPTS | 8)
+
+/* aarch64 */
+#define OPTEE_BENCH_ARMV8_PMCR_MASK	0x3f
+#define OPTEE_BENCH_ARMV8_PMCR_E	(1 << 0) /* Enable all counters */
+#define OPTEE_BENCH_ARMV8_PMCR_P	(1 << 1) /* Reset all counters */
+#define OPTEE_BENCH_ARMV8_PMCR_C	(1 << 2) /* Cycle counter reset */
+#define OPTEE_BENCH_ARMV8_PMCR_D    (1 << 3) /* 64 divider */
+
+#define OPTEE_BENCH_ARMV8_PMUSERENR_EL0	(1 << 0) /* EL0 access enable */
+#define OPTEE_BENCH_ARMV8_PMUSERENR_CR	(1 << 2) /* CCNT read enable */
+
+struct optee_ts_global *optee_bench_ts_global;
+struct rw_semaphore optee_bench_ts_rwsem;
+
+#ifdef CONFIG_OPTEE_BENCHMARK
+static inline u32 armv8pmu_pmcr_read(void)
+{
+		u32 val = 0;
+
+		asm volatile("mrs %0, pmcr_el0" : "=r"(val));
+
+		return (u32)val;
+}
+
+static inline void armv8pmu_pmcr_write(u32 val)
+{
+		val &= OPTEE_BENCH_ARMV8_PMCR_MASK;
+		asm volatile("msr pmcr_el0, %0" :: "r"((u64)val));
+}
+
+static inline u64 read_ccounter(void)
+{
+	u64 ccounter;
+
+#ifdef __aarch64__
+	asm volatile("mrs %0, PMCCNTR_EL0" : "=r"(ccounter));
+#else
+	asm volatile("mrc p15, 0, %0, c9, c13, 0" : "=r"(ccounter));
+#endif
+
+	return ccounter * OPTEE_BENCH_DIVIDER;
+}
+
+static void optee_pmu_setup(void *data)
+{
+#ifdef __aarch64__
+	/* Enable EL0 access to PMU counters. */
+	asm volatile("msr pmuserenr_el0, %0" :: "r"((u64)
+			OPTEE_BENCH_ARMV8_PMUSERENR_EL0 |
+			OPTEE_BENCH_ARMV8_PMUSERENR_CR));
+	/* Enable PMU counters */
+	armv8pmu_pmcr_write(OPTEE_BENCH_ARMV8_PMCR_P |
+					OPTEE_BENCH_ARMV8_PMCR_C |
+					OPTEE_BENCH_ARMV8_PMCR_D);
+	asm volatile("msr pmcntenset_el0, %0" :: "r"((u64)(1 << 31)));
+	armv8pmu_pmcr_write(armv8pmu_pmcr_read() |
+					OPTEE_BENCH_ARMV8_PMCR_E);
+#else
+	/* Enable EL0 access to PMU counters */
+	asm volatile("mcr p15, 0, %0, c9, c14, 0" :: "r"(1));
+	/* Enable all PMU counters */
+	asm volatile("mcr p15, 0, %0, c9, c12, 0" :: "r"
+					(OPTEE_BENCH_DIVIDER_OPTS));
+	/* Disable counter overflow interrupts */
+	asm volatile("mcr p15, 0, %0, c9, c12, 1" :: "r"(OPTEE_BENCH_DEF_OVER));
+#endif
+}
+
+static void optee_pmu_disable(void *data)
+{
+#ifdef __aarch64__
+	/* Disable EL0 access */
+	asm volatile("msr pmuserenr_el0, %0" :: "r"((u64)0));
+	/* Disable PMU counters */
+	armv8pmu_pmcr_write(armv8pmu_pmcr_read() | ~OPTEE_BENCH_ARMV8_PMCR_E);
+#else
+	/* Disable all PMU counters */
+	asm volatile("mcr p15, 0, %0, c9, c12, 0" :: "r"(0));
+	/* Enable counter overflow interrupts */
+	asm volatile("mcr p15, 0, %0, c9, c12, 2" :: "r"(OPTEE_BENCH_DEF_OVER));
+	/* Disable EL0 access to PMU counters. */
+	asm volatile("mcr p15, 0, %0, c9, c14, 0" :: "r"(0));
+#endif
+}
+
+void optee_bm_enable(void)
+{
+	on_each_cpu(optee_pmu_setup, NULL, 1);
+}
+
+void optee_bm_disable(void)
+{
+	on_each_cpu(optee_pmu_disable, NULL, 1);
+}
+
+void optee_bm_timestamp(void)
+{
+	struct optee_ts_cpu_buf *cpu_buf;
+	struct optee_time_st ts_data;
+	uint64_t ts_i;
+	void *ret_addr;
+	int cur_cpu = 0;
+	int ret;
+
+	down_read(&optee_bench_ts_rwsem);
+
+	if (!optee_bench_ts_global) {
+		up_read(&optee_bench_ts_rwsem);
+		return;
+	}
+
+	cur_cpu = get_cpu();
+
+	if (cur_cpu >= optee_bench_ts_global->cores) {
+		put_cpu();
+		up_read(&optee_bench_ts_rwsem);
+		return;
+	}
+
+	ret_addr = __builtin_return_address(0);
+
+	cpu_buf = &optee_bench_ts_global->cpu_buf[cur_cpu];
+	ts_i = __sync_fetch_and_add(&cpu_buf->head, 1);
+	ts_data.cnt = read_ccounter();
+	ts_data.addr = (uintptr_t)ret_addr;
+	ts_data.src = OPTEE_BENCH_KMOD;
+	cpu_buf->stamps[ts_i & OPTEE_BENCH_MAX_MASK] = ts_data;
+
+	up_read(&optee_bench_ts_rwsem);
+
+	put_cpu();
+}
+#endif  /* CONFIG_OPTEE_BENCHMARK */

drivers/tee/optee/call.c

@@ -21,6 +21,7 @@
 #include <linux/uaccess.h>
 #include "optee_private.h"
 #include "optee_smc.h"
+#include "optee_bench.h"
 
 struct optee_call_waiter {
 	struct list_head list_node;
@@ -144,10 +145,14 @@ u32 optee_do_call_with_arg(struct tee_context *ctx, phys_addr_t parg)
 	while (true) {
 		struct arm_smccc_res res;
 
+		optee_bm_timestamp();
+
 		optee->invoke_fn(param.a0, param.a1, param.a2, param.a3,
 				 param.a4, param.a5, param.a6, param.a7,
 				 &res);
 
+		optee_bm_timestamp();
+
 		if (res.a0 == OPTEE_SMC_RETURN_ETHREAD_LIMIT) {
 			/*
 			 * Out of threads in secure world, wait for a thread

drivers/tee/optee/core.c

@@ -26,6 +26,7 @@
 #include <linux/tee_drv.h>
 #include <linux/types.h>
 #include <linux/uaccess.h>
+#include "optee_bench.h"
 #include "optee_private.h"
 #include "optee_smc.h"
 
@@ -598,6 +599,8 @@ static int __init optee_driver_init(void)
 
 	optee_svc = optee;
 
+	optee_bm_enable();
+
 	return 0;
 }
 module_init(optee_driver_init);
@@ -609,6 +612,8 @@ static void __exit optee_driver_exit(void)
 	optee_svc = NULL;
 	if (optee)
 		optee_remove(optee);
+
+	optee_bm_disable();
 }
 module_exit(optee_driver_exit);
 

drivers/tee/optee/optee_bench.h

@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _OPTEE_BENCH_H
+#define _OPTEE_BENCH_H
+
+#include <linux/rwsem.h>
+#include <linux/tee_drv.h>
+
+/*
+ * Cycle count divider is enabled (in PMCR),
+ * CCNT value is incremented every 64th clock cycle
+ */
+#define OPTEE_BENCH_DIVIDER			64
+
+/* max amount of timestamps */
+#define OPTEE_BENCH_MAX_STAMPS		32
+#define OPTEE_BENCH_MAX_MASK		(OPTEE_BENCH_MAX_STAMPS - 1)
+
+/* OP-TEE susbsystems ids */
+#define OPTEE_BENCH_KMOD	0x20000000
+
+#define OPTEE_MSG_RPC_CMD_BENCH_REG_NEW		0
+#define OPTEE_MSG_RPC_CMD_BENCH_REG_DEL		1
+
+/* storing timestamp */
+struct optee_time_st {
+	uint64_t cnt;	/* stores value from CNTPCT register */
+	uint64_t addr;	/* stores value from program counter register */
+	uint64_t src;	/* OP-TEE subsystem id */
+};
+
+/* per-cpu circular buffer for timestamps */
+struct optee_ts_cpu_buf {
+	uint64_t head;
+	uint64_t tail;
+	struct optee_time_st stamps[OPTEE_BENCH_MAX_STAMPS];
+};
+
+/* memory layout for shared memory, where timestamps will be stored */
+struct optee_ts_global {
+	uint64_t cores;
+	struct optee_ts_cpu_buf cpu_buf[];
+};
+
+extern struct optee_ts_global *optee_bench_ts_global;
+extern struct rw_semaphore optee_bench_ts_rwsem;
+
+#ifdef CONFIG_OPTEE_BENCHMARK
+void optee_bm_enable(void);
+void optee_bm_disable(void);
+void optee_bm_timestamp(void);
+#else
+static inline void optee_bm_enable(void) {}
+static inline void optee_bm_disable(void) {}
+static inline void optee_bm_timestamp(void) {}
+#endif  /* CONFIG_OPTEE_BENCHMARK */
+#endif /* _OPTEE_BENCH_H */

drivers/tee/optee/optee_msg.h

@@ -415,4 +415,12 @@ struct optee_msg_arg {
  */
 #define OPTEE_MSG_RPC_CMD_SHM_FREE	7
 
+/*
+ * Register timestamp buffer
+ *
+ * [in] param[0].u.value.a	Subcommand (register buffer, unregister buffer)
+ * [in] param[0].u.value.b	Physical address of timestamp buffer
+ * [in] param[0].u.value.c	Size of buffer
+ */
+#define OPTEE_MSG_RPC_CMD_BENCH_REG	20
 #endif /* _OPTEE_MSG_H */

drivers/tee/optee/rpc.c

@@ -16,8 +16,10 @@
 
 #include <linux/delay.h>
 #include <linux/device.h>
+#include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/tee_drv.h>
+#include "optee_bench.h"
 #include "optee_private.h"
 #include "optee_smc.h"
 
@@ -307,6 +309,50 @@ static void handle_rpc_func_cmd_shm_free(struct tee_context *ctx,
 	arg->ret = TEEC_SUCCESS;
 }
 
+static void handle_rpc_func_cmd_bm_reg(struct optee_msg_arg *arg)
+{
+	u64 size;
+	u64 type;
+	u64 paddr;
+
+	if (arg->num_params != 1)
+		goto bad;
+
+	if ((arg->params[0].attr & OPTEE_MSG_ATTR_TYPE_MASK) !=
+			OPTEE_MSG_ATTR_TYPE_VALUE_INPUT)
+		goto bad;
+
+	type = arg->params[0].u.value.a;
+	switch (type) {
+	case OPTEE_MSG_RPC_CMD_BENCH_REG_NEW:
+		size = arg->params[0].u.value.c;
+		paddr = arg->params[0].u.value.b;
+		down_write(&optee_bench_ts_rwsem);
+		optee_bench_ts_global =
+			memremap(paddr, size, MEMREMAP_WB);
+		if (!optee_bench_ts_global) {
+			up_write(&optee_bench_ts_rwsem);
+			goto bad;
+		}
+		up_write(&optee_bench_ts_rwsem);
+		break;
+	case OPTEE_MSG_RPC_CMD_BENCH_REG_DEL:
+		down_write(&optee_bench_ts_rwsem);
+		if (optee_bench_ts_global)
+			memunmap(optee_bench_ts_global);
+		optee_bench_ts_global = NULL;
+		up_write(&optee_bench_ts_rwsem);
+		break;
+	default:
+		goto bad;
+	}
+
+	arg->ret = TEEC_SUCCESS;
+	return;
+bad:
+	arg->ret = TEEC_ERROR_BAD_PARAMETERS;
+}
+
 static void handle_rpc_func_cmd(struct tee_context *ctx, struct optee *optee,
 				struct tee_shm *shm)
 {
@@ -334,6 +380,9 @@ static void handle_rpc_func_cmd(struct tee_context *ctx, struct optee *optee,
 	case OPTEE_MSG_RPC_CMD_SHM_FREE:
 		handle_rpc_func_cmd_shm_free(ctx, arg);
 		break;
+	case OPTEE_MSG_RPC_CMD_BENCH_REG:
+		handle_rpc_func_cmd_bm_reg(arg);
+		break;
 	default:
 		handle_rpc_supp_cmd(ctx, arg);
 	}