log: output TEE Errors to KMSG for debugging [2/2]

PD#SWPL-9652

Problem:
TEE logs are output to UART and not via kmsg.
Secure OS should support routing TEE logs into kmsg.

Solution:
1) reserve system share-mem as logger mem.
2) add API to support routing TEE logs to REE kmsg.

Verify:
Android P + U200

Change-Id: I7a43827f1d25a2e72ff15082751351bcad7c68f4
Signed-off-by: Liqiang Jin <liqiang.jin@amlogic.com>

6 files changed, 376 insertions, 3 deletions

diff --git a/Makefile b/Makefile
index f94a071..304db42 100644
--- a/Makefile
+++ b/Makefile
@@ -1,6 +1,7 @@
 ccflags-y+=-Werror
 ccflags-y+=-I$(M)/include/linux
 ccflags-y+=-I$(M)/include
+ccflags-y+=-I$(M)
 
 obj-m += optee.o
 obj-y += optee/
diff --git a/optee/Makefile b/optee/Makefile
index b104819..8865e15 100644
--- a/optee/Makefile
+++ b/optee/Makefile
@@ -13,3 +13,4 @@ optee_armtz-objs += smccc-call-a32.o
 else
 optee_armtz-objs += smccc-call.o
 endif
+optee_armtz-objs += log.o
diff --git a/optee/core.c b/optee/core.c
index efacf4f..5295c75 100644
--- a/optee/core.c
+++ b/optee/core.c
@@ -29,11 +29,14 @@
 #include "../include/linux/arm-smccc.h"
 #include "optee_private.h"
 #include "optee_smc.h"
+#include "../tee_private.h"
 
 #define DRIVER_NAME "optee"
 
 #define OPTEE_SHM_NUM_PRIV_PAGES	4
 
+#define LOGGER_SHM_SIZE             (256 * 1024)
+
 /**
  * optee_from_msg_param() - convert from OPTEE_MSG parameters to
  *			    struct tee_param
@@ -342,7 +345,8 @@ static bool optee_msg_exchange_capabilities(optee_invoke_fn *invoke_fn,
 }
 
 static struct tee_shm_pool *
-optee_config_shm_memremap(optee_invoke_fn *invoke_fn, void **memremaped_shm)
+optee_config_shm_memremap(optee_invoke_fn *invoke_fn, void **memremaped_shm,
+		phys_addr_t *logger_shm_pa)
 {
 	union {
 		struct arm_smccc_res smccc;
@@ -358,6 +362,11 @@ optee_config_shm_memremap(optee_invoke_fn *invoke_fn, void **memremaped_shm)
 	struct tee_shm_pool_mem_info priv_info;
 	struct tee_shm_pool_mem_info dmabuf_info;
 
+	if (!invoke_fn || !memremaped_shm || !logger_shm_pa) {
+		pr_err("Invalid parameters");
+		return ERR_PTR(-EINVAL);
+	}
+
 	invoke_fn(OPTEE_SMC_GET_SHM_CONFIG, 0, 0, 0, 0, 0, 0, 0, &res.smccc);
 	if (res.result.status != OPTEE_SMC_RETURN_OK) {
 		pr_info("shm service not available\n");
@@ -396,7 +405,9 @@ optee_config_shm_memremap(optee_invoke_fn *invoke_fn, void **memremaped_shm)
 	priv_info.size = OPTEE_SHM_NUM_PRIV_PAGES * PAGE_SIZE;
 	dmabuf_info.vaddr = vaddr + OPTEE_SHM_NUM_PRIV_PAGES * PAGE_SIZE;
 	dmabuf_info.paddr = paddr + OPTEE_SHM_NUM_PRIV_PAGES * PAGE_SIZE;
-	dmabuf_info.size = size - OPTEE_SHM_NUM_PRIV_PAGES * PAGE_SIZE;
+	dmabuf_info.size = size - OPTEE_SHM_NUM_PRIV_PAGES * PAGE_SIZE -
+		LOGGER_SHM_SIZE;
+	*logger_shm_pa = dmabuf_info.paddr + dmabuf_info.size;
 
 	pool = tee_shm_pool_alloc_res_mem(&priv_info, &dmabuf_info);
 	if (IS_ERR(pool)) {
@@ -431,6 +442,7 @@ static int optee_probe(struct platform_device *pdev)
 	struct tee_shm_pool *pool;
 	struct optee *optee = NULL;
 	void *memremaped_shm = NULL;
+	phys_addr_t logger_shm_pa = 0;
 	struct tee_device *teedev;
 	u32 sec_caps;
 	int rc;
@@ -461,7 +473,8 @@ static int optee_probe(struct platform_device *pdev)
 	if (!(sec_caps & OPTEE_SMC_SEC_CAP_HAVE_RESERVED_SHM))
 		return -EINVAL;
 
-	pool = optee_config_shm_memremap(invoke_fn, &memremaped_shm);
+	pool = optee_config_shm_memremap(invoke_fn, &memremaped_shm,
+			&logger_shm_pa);
 	if (IS_ERR(pool))
 		return PTR_ERR(pool);
 
@@ -495,6 +508,8 @@ static int optee_probe(struct platform_device *pdev)
 	if (rc)
 		goto err;
 
+	optee_log_init(optee->teedev, logger_shm_pa, LOGGER_SHM_SIZE);
+
 	mutex_init(&optee->call_queue.mutex);
 	INIT_LIST_HEAD(&optee->call_queue.waiters);
 	optee_wait_queue_init(&optee->wait_queue);
@@ -530,6 +545,8 @@ static int optee_remove(struct platform_device *pdev)
 {
 	struct optee *optee = platform_get_drvdata(pdev);
 
+	optee_log_exit(optee->teedev);
+
 	/*
 	 * Ask OP-TEE to free all cached shared memory objects to decrease
 	 * reference counters and also avoid wild pointers in secure world
diff --git a/optee/log.c b/optee/log.c
new file mode 100644
index 0000000..45f0373
--- /dev/null
+++ b/optee/log.c
@@ -0,0 +1,331 @@
+/*
+ * Copyright (c) 2017, Amlogic, Inc.
+ *
+ * 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/errno.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/types.h>
+#include <linux/uaccess.h>
+#include <linux/sysfs.h>
+#include <linux/kthread.h>
+
+#include "optee_smc.h"
+#include "optee_private.h"
+#include "../tee_private.h"
+
+//#define OPTEE_LOG_BUFFER_DEBUG      1
+
+#define OPTEE_LOG_BUFFER_MAGIC      0xAA00AA00
+#define OPTEE_LOG_BUFFER_OFFSET     0x00000080
+#define OPTEE_LOG_READ_MAX          PAGE_SIZE
+#define OPTEE_LOG_LINE_MAX          1024
+#define OPTEE_LOG_TIMER_INTERVAL    1
+
+#undef pr_fmt
+#define pr_fmt(fmt) "[TEE] " fmt
+
+struct optee_log_ctl_s {
+	unsigned int magic;
+	unsigned int inited;
+	unsigned int total_size;
+	unsigned int fill_size;
+	unsigned int mode;
+	unsigned int reader;
+	unsigned int writer;
+
+	unsigned char *buffer;
+};
+
+static struct optee_log_ctl_s *optee_log_ctl;
+static unsigned char *optee_log_buff;
+static uint32_t optee_log_mode = 1;
+static struct timer_list optee_log_timer;
+static uint8_t line_buff[OPTEE_LOG_LINE_MAX];
+static void *g_shm_va;
+
+static bool init_shm(phys_addr_t shm_pa, uint32_t shm_size)
+{
+	struct arm_smccc_res smccc;
+	uint32_t start = 1;
+
+	if (pfn_valid(__phys_to_pfn(shm_pa)))
+		g_shm_va = (void __iomem *)__phys_to_virt(shm_pa);
+	else
+		g_shm_va = ioremap_cache(shm_pa, shm_size);
+
+	if (!g_shm_va) {
+		pr_err("map logger share-mem failed\n");
+		return false;
+	}
+
+	arm_smccc_smc(OPTEE_SMC_SET_LOGGER, start, shm_pa, shm_size, 0, 0, 0, 0,
+			&smccc);
+
+	return (0 == smccc.a0);
+}
+
+static void uninit_shm(void)
+{
+	struct arm_smccc_res smccc;
+	uint32_t start = 0;
+
+	if (g_shm_va)
+		iounmap(g_shm_va);
+
+	arm_smccc_smc(OPTEE_SMC_SET_LOGGER, start, 0, 0, 0, 0, 0, 0, &smccc);
+}
+
+static ssize_t log_mode_show(struct class *cla,
+		struct class_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%d\n", optee_log_mode);
+}
+
+static ssize_t log_mode_store(struct class *cla,
+			       struct class_attribute *attr,
+			       const char *buf, size_t count)
+{
+	int res = 0;
+	int rc = 0;
+	struct optee_log_ctl_s *ctl = optee_log_ctl;
+
+	rc = kstrtoint(buf, 0, &res);
+	pr_notice("log_mode: %d->%d\n", optee_log_mode, res);
+	optee_log_mode = res;
+	if (ctl && ctl->mode != optee_log_mode)
+		ctl->mode = optee_log_mode;
+
+	return count;
+}
+
+static ssize_t log_buff_get_read_buff(char **buf, int len)
+{
+	int writer;
+	int reader;
+	int read_size = 0;
+	struct optee_log_ctl_s *ctl = optee_log_ctl;
+
+	if ((!ctl) || (len <= 0))
+		return 0;
+
+	writer = ctl->writer;
+	reader = ctl->reader;
+
+	if (reader == writer)
+		read_size = 0;
+	else if (reader < writer)
+		read_size = writer - reader;
+	else
+		read_size = ctl->total_size - reader;
+
+	if (read_size > len)
+		read_size = len;
+
+	*buf = optee_log_buff + reader;
+	ctl->reader += read_size;
+	if (ctl->reader == ctl->total_size)
+		ctl->reader = 0;
+
+	return read_size;
+}
+
+static size_t log_print_text(char *buf, size_t size)
+{
+	const char *text = buf;
+	size_t text_size = size;
+	size_t len = 0;
+	char *line = line_buff;
+
+	do {
+		const char *next = memchr(text, '\n', text_size);
+		size_t line_size;
+
+		if (next) {
+			line_size = next - text;
+			next++;
+			text_size -= next - text;
+		} else
+			line_size = text_size;
+
+		memcpy(line, text, line_size);
+		len += line_size;
+		if (next)
+			len++;
+		else if (line[line_size] == '\0')
+			len++;
+		line[line_size] = '\n';
+		line[line_size + 1] = '\0';
+		pr_notice("%s", line);
+		text = next;
+	} while (text && (len < size));
+
+	return len;
+}
+
+static ssize_t log_buff_dump(char *buf, size_t size)
+{
+	ssize_t len;
+	char *ptr = NULL;
+	struct optee_log_ctl_s *ctl = optee_log_ctl;
+
+	if (!ctl)
+		return 0;
+
+	len = ctl->reader;
+	if (len == 0)
+		return 0;
+
+	ptr = optee_log_buff;
+	if (len > size) {
+		ptr += len - size;
+		len = size;
+	}
+	memcpy(buf, ptr, len);
+
+	return len;
+}
+
+static void log_buff_reset(void)
+{
+	struct optee_log_ctl_s *ctl = optee_log_ctl;
+
+	if (!ctl)
+		return;
+
+	ctl->writer = 0;
+	ctl->reader = 0;
+
+	return;
+}
+
+static void log_buff_info(void)
+{
+#ifdef OPTEE_LOG_BUFFER_DEBUG
+	struct optee_log_ctl_s *ctl = optee_log_ctl;
+
+	if (!ctl)
+		return;
+
+	pr_notice("    total_size: %d\n", ctl->total_size);
+	pr_notice("     fill_size: %d\n", ctl->fill_size);
+	pr_notice("          mode: %d\n", ctl->mode);
+	pr_notice("        reader: %d\n", ctl->reader);
+	pr_notice("        writer: %d\n", ctl->writer);
+#endif
+}
+
+static ssize_t log_buff_store(struct class *cla, struct class_attribute *attr,
+		const char *buf, size_t count)
+{
+	int res = 0;
+	int rc = 0;
+
+	rc = kstrtoint(buf, 0, &res);
+	if (res == 0)
+		/* reset log buffer */
+		log_buff_reset();
+	else if (res == 1)
+		/* show log buffer info */
+		log_buff_info();
+	else
+		pr_notice("set 0 to reset tee log buffer\n");
+
+	return count;
+}
+
+static ssize_t log_buff_show(struct class *cla,
+		struct class_attribute *attr, char *buf)
+{
+	return log_buff_dump(buf, OPTEE_LOG_READ_MAX);
+}
+
+static struct class_attribute log_class_attrs[] = {
+	__ATTR(log_mode, S_IRUGO | S_IWUSR, log_mode_show, log_mode_store),
+	__ATTR(log_buff, S_IRUGO | S_IWUSR, log_buff_show, log_buff_store),
+};
+
+static void log_buff_output(void)
+{
+	size_t len;
+	char *read_buff = NULL;
+
+	if (optee_log_mode == 0)
+		return;
+
+	len = log_buff_get_read_buff(&read_buff, OPTEE_LOG_READ_MAX);
+	if (len > 0)
+		log_print_text(read_buff, len);
+}
+
+static void log_timer_func(unsigned long arg)
+{
+	log_buff_output();
+	mod_timer(&optee_log_timer, jiffies + OPTEE_LOG_TIMER_INTERVAL * HZ);
+}
+
+int optee_log_init(struct tee_device *tee_dev, phys_addr_t shm_pa,
+		uint32_t shm_size)
+{
+	int rc = 0;
+	int i = 0;
+	int n = 0;
+
+	if (!init_shm(shm_pa, shm_size))
+		return -1;
+
+	optee_log_buff = (unsigned char *)(g_shm_va + OPTEE_LOG_BUFFER_OFFSET);
+	optee_log_ctl = (struct optee_log_ctl_s *)g_shm_va;
+	if ((optee_log_ctl->magic != OPTEE_LOG_BUFFER_MAGIC)
+		|| (optee_log_ctl->inited != 1)) {
+		uninit_shm();
+		optee_log_ctl = NULL;
+		rc = -1;
+		pr_err("tee log buffer init failed\n");
+		goto err;
+	}
+	optee_log_ctl->mode = optee_log_mode;
+
+	/* create attr files */
+	n = sizeof(log_class_attrs) / sizeof(struct class_attribute);
+	for (i = 0; i < n; i++) {
+		rc = class_create_file(tee_dev->dev.class, &log_class_attrs[i]);
+		if (rc)
+			goto err;
+	}
+
+	/* init timer */
+	init_timer(&optee_log_timer);
+	optee_log_timer.data = 0L;
+	optee_log_timer.function = log_timer_func;
+	optee_log_timer.expires = jiffies + HZ;
+	add_timer(&optee_log_timer);
+
+err:
+	return rc;
+}
+
+void optee_log_exit(struct tee_device *tee_dev)
+{
+	int i = 0;
+	int n = 0;
+
+	del_timer_sync(&optee_log_timer);
+
+	n = sizeof(log_class_attrs) / sizeof(struct class_attribute);
+	for (i = 0; i < n; i++)
+		class_remove_file(tee_dev->dev.class, &log_class_attrs[i]);
+
+	uninit_shm();
+}
diff --git a/optee/optee_private.h b/optee/optee_private.h
index cc5e0b5..18a6f04 100644
--- a/optee/optee_private.h
+++ b/optee/optee_private.h
@@ -15,11 +15,13 @@
 #ifndef OPTEE_PRIVATE_H
 #define OPTEE_PRIVATE_H
 
+#include <linux/device.h>
 #include <linux/arm-smccc.h>
 #include <linux/semaphore.h>
 #include <linux/types.h>
 #include "tee_drv.h"
 #include "optee_msg.h"
+#include "../tee_private.h"
 
 #define OPTEE_MAX_ARG_SIZE	1024
 
@@ -154,6 +156,9 @@ int optee_from_msg_param(struct tee_param *params, size_t num_params,
 int optee_to_msg_param(struct optee_msg_param *msg_params, size_t num_params,
 		       const struct tee_param *params);
 
+int optee_log_init(struct tee_device *, phys_addr_t, uint32_t);
+void optee_log_exit(struct tee_device *);
+
 /*
  * Small helpers
  */
diff --git a/optee/optee_smc.h b/optee/optee_smc.h
index 13b7c98..bcf4ce2 100644
--- a/optee/optee_smc.h
+++ b/optee/optee_smc.h
@@ -426,6 +426,24 @@ struct optee_smc_disable_shm_cache_result {
 #define OPTEE_SMC_RETURN_RPC_CMD \
 	OPTEE_SMC_RPC_VAL(OPTEE_SMC_RPC_FUNC_CMD)
 
+/*
+ * set logger
+ *
+ * Call register usage:
+ * a0      SMC Function ID, OPTEE_SMC_SET_LOGGER
+ * a1      start logger: a1 > 0; stop logger: a1 = 0;
+ * a2      logger share-mem phy addr
+ * a3      logger share-mem size
+ * a4-7    Not used
+ *
+ * Normal return register usage:
+ * a0      set logger result
+ * a1-7    Preserved
+ */
+#define OPTEE_SMC_FUNCID_SET_LOGGER    0xE001
+#define OPTEE_SMC_SET_LOGGER \
+    OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_SET_LOGGER)
+
 /* Returned in a0 */
 #define OPTEE_SMC_RETURN_UNKNOWN_FUNCTION 0xFFFFFFFF