diff -drupN a/drivers/tee/optee/bench.c b/drivers/tee/optee/bench.c
--- a/drivers/tee/optee/bench.c	1970-01-01 03:00:00.000000000 +0300
+++ b/drivers/tee/optee/bench.c	2022-06-12 05:28:14.000000000 +0300
@@ -0,0 +1,156 @@
+/*
+ * 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;
+
+	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 */