[v3,2/4] soc: qcom: Add SoC sleep stats driver

diff --git a/drivers/soc/qcom/Kconfig b/drivers/soc/qcom/Kconfig
index d0a73e7..08bc0df3 100644
--- a/drivers/soc/qcom/Kconfig
+++ b/drivers/soc/qcom/Kconfig
@@ -185,6 +185,15 @@  config QCOM_SOCINFO
 	 Say yes here to support the Qualcomm socinfo driver, providing
 	 information about the SoC to user space.
 
+config QCOM_SOC_SLEEP_STATS
+	tristate "Qualcomm Technologies, Inc. (QTI) SoC sleep stats driver"
+	depends on ARCH_QCOM && DEBUG_FS || COMPILE_TEST
+	help
+	  Qualcomm Technologies, Inc. (QTI) SoC sleep stats driver to read
+	  the shared memory exported by the remote processor related to
+	  various SoC level low power modes statistics and export to debugfs
+	  interface.
+
 config QCOM_WCNSS_CTRL
 	tristate "Qualcomm WCNSS control driver"
 	depends on ARCH_QCOM || COMPILE_TEST
diff --git a/drivers/soc/qcom/Makefile b/drivers/soc/qcom/Makefile
index 9fb35c8..e6bd052 100644
--- a/drivers/soc/qcom/Makefile
+++ b/drivers/soc/qcom/Makefile
@@ -20,6 +20,7 @@  obj-$(CONFIG_QCOM_SMEM_STATE) += smem_state.o
 obj-$(CONFIG_QCOM_SMP2P)	+= smp2p.o
 obj-$(CONFIG_QCOM_SMSM)	+= smsm.o
 obj-$(CONFIG_QCOM_SOCINFO)	+= socinfo.o
+obj-$(CONFIG_QCOM_SOC_SLEEP_STATS)	+= soc_sleep_stats.o
 obj-$(CONFIG_QCOM_WCNSS_CTRL) += wcnss_ctrl.o
 obj-$(CONFIG_QCOM_APR) += apr.o
 obj-$(CONFIG_QCOM_LLCC) += llcc-qcom.o
diff --git a/drivers/soc/qcom/soc_sleep_stats.c b/drivers/soc/qcom/soc_sleep_stats.c
new file mode 100644
index 00000000..79a14d2
--- /dev/null
+++ b/drivers/soc/qcom/soc_sleep_stats.c
@@ -0,0 +1,248 @@ 
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2011-2020, The Linux Foundation. All rights reserved.
+ */
+
+#include <linux/debugfs.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/seq_file.h>
+
+#include <linux/soc/qcom/smem.h>
+#include <clocksource/arm_arch_timer.h>
+
+#define MODEM_ITEM_ID		605
+#define ADSP_ITEM_ID		606
+#define CDSP_ITEM_ID		607
+#define SLPI_ITEM_ID		608
+#define GPU_ITEM_ID		609
+#define DISPLAY_ITEM_ID		610
+
+#define PID_APSS		0
+#define PID_MODEM		1
+#define PID_ADSP		2
+#define PID_SLPI		3
+#define PID_CDSP		5
+
+#define NAME_LENGTH		5
+
+#define STAT_TYPE_ADDR		0x0
+#define COUNT_ADDR		0x4
+#define LAST_ENTERED_AT_ADDR	0x8
+#define LAST_EXITED_AT_ADDR	0x10
+#define ACCUMULATED_ADDR	0x18
+#define CLIENT_VOTES_ADDR	0x1c
+
+struct subsystem_data {
+	const char *name;
+	u32 item_id;
+	u32 pid;
+};
+
+static struct subsystem_data subsystems[] = {
+	{ "modem", MODEM_ITEM_ID, PID_MODEM },
+	{ "adsp", ADSP_ITEM_ID, PID_ADSP },
+	{ "cdsp", CDSP_ITEM_ID, PID_CDSP },
+	{ "slpi", SLPI_ITEM_ID, PID_SLPI },
+	{ "gpu", GPU_ITEM_ID, PID_APSS },
+	{ "display", DISPLAY_ITEM_ID, PID_APSS },
+};
+
+struct stats_config {
+	unsigned int offset_addr;
+	unsigned int num_records;
+	bool appended_stats_avail;
+};
+
+static const struct stats_config *config;
+
+struct soc_sleep_stats_data {
+	phys_addr_t stats_base;
+	resource_size_t stats_size;
+	void __iomem *reg;
+	struct dentry *root;
+};
+
+struct entry {
+	u32 stat_type;
+	u32 count;
+	u64 last_entered_at;
+	u64 last_exited_at;
+	u64 accumulated;
+};
+
+struct appended_entry {
+	u32 client_votes;
+	u32 reserved[3];
+};
+
+static void print_sleep_stats(struct seq_file *s, struct entry *e)
+{
+	seq_printf(s, "Count = %u\n", e->count);
+	seq_printf(s, "Last Entered At = %llu\n", e->last_entered_at);
+	seq_printf(s, "Last Exited At = %llu\n", e->last_exited_at);
+	seq_printf(s, "Accumulated Duration = %llu\n", e->accumulated);
+}
+
+static int subsystem_sleep_stats_show(struct seq_file *s, void *d)
+{
+	struct subsystem_data *subsystem = s->private;
+	struct entry *e;
+
+	e = qcom_smem_get(subsystem->pid, subsystem->item_id, NULL);
+	if (IS_ERR(e))
+		return PTR_ERR(e);
+
+	/*
+	 * If a subsystem is in sleep when reading the sleep stats adjust
+	 * the accumulated sleep duration to show actual sleep time.
+	 */
+	if (e->last_entered_at > e->last_exited_at)
+		e->accumulated += (arch_timer_read_counter()
+				   - e->last_entered_at);
+
+	print_sleep_stats(s, e);
+
+	return 0;
+}
+
+static int soc_sleep_stats_show(struct seq_file *s, void *d)
+{
+	void __iomem *reg = s->private;
+	struct entry e;
+
+	e.count = readl_relaxed(reg + COUNT_ADDR);
+	e.last_entered_at = readq_relaxed(reg + LAST_ENTERED_AT_ADDR);
+	e.last_exited_at = readq_relaxed(reg + LAST_EXITED_AT_ADDR);
+	e.accumulated = readq_relaxed(reg + ACCUMULATED_ADDR);
+
+	print_sleep_stats(s, &e);
+
+	if (config->appended_stats_avail) {
+		struct appended_entry ae;
+
+		ae.client_votes = readl_relaxed(reg + CLIENT_VOTES_ADDR);
+		seq_printf(s, "Client_votes = %#x\n", ae.client_votes);
+	}
+
+	return 0;
+}
+
+DEFINE_SHOW_ATTRIBUTE(soc_sleep_stats);
+DEFINE_SHOW_ATTRIBUTE(subsystem_sleep_stats);
+
+static void create_debugfs_entries(struct soc_sleep_stats_data *drv)
+{
+	struct entry *e;
+	char stat_type[NAME_LENGTH] = {0};
+	u32 offset, type;
+	int i;
+
+	drv->root = debugfs_create_dir("soc_sleep_stats", NULL);
+
+	for (i = 0; i < config->num_records; i++) {
+		offset = STAT_TYPE_ADDR + (i * sizeof(struct entry));
+
+		if (config->appended_stats_avail)
+			offset += i * sizeof(struct appended_entry);
+
+		type = le32_to_cpu(readl_relaxed(drv->reg + offset));
+		memcpy(stat_type, &type, sizeof(u32));
+		debugfs_create_file(stat_type, 0444, drv->root,
+				    drv->reg + offset,
+				    &soc_sleep_stats_fops);
+	}
+
+	for (i = 0; i < ARRAY_SIZE(subsystems); i++) {
+		e = qcom_smem_get(subsystems[i].pid, subsystems[i].item_id,
+				  NULL);
+
+		if (IS_ERR(e))
+			continue;
+
+		debugfs_create_file(subsystems[i].name, 0444, drv->root,
+				    &subsystems[i],
+				    &subsystem_sleep_stats_fops);
+	}
+}
+
+static int soc_sleep_stats_probe(struct platform_device *pdev)
+{
+	struct soc_sleep_stats_data *drv;
+	struct resource *res;
+	void __iomem *offset_addr;
+
+	drv = devm_kzalloc(&pdev->dev, sizeof(*drv), GFP_KERNEL);
+	if (!drv)
+		return -ENOMEM;
+
+	config = device_get_match_data(&pdev->dev);
+	if (!config)
+		return -ENODEV;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return PTR_ERR(res);
+
+	offset_addr = devm_ioremap(&pdev->dev,
+				   res->start + config->offset_addr,
+				   sizeof(u32));
+	if (IS_ERR(offset_addr))
+		return PTR_ERR(offset_addr);
+
+	drv->stats_base = res->start | readl_relaxed(offset_addr);
+	drv->stats_size = resource_size(res);
+	devm_iounmap(&pdev->dev, offset_addr);
+
+	drv->reg = devm_ioremap(&pdev->dev, drv->stats_base, drv->stats_size);
+	if (!drv->reg)
+		return -ENOMEM;
+
+	create_debugfs_entries(drv);
+	platform_set_drvdata(pdev, drv);
+
+	return 0;
+}
+
+static int soc_sleep_stats_remove(struct platform_device *pdev)
+{
+	struct soc_sleep_stats_data *drv = platform_get_drvdata(pdev);
+
+	debugfs_remove_recursive(drv->root);
+
+	return 0;
+}
+
+static const struct stats_config rpm_data = {
+	.offset_addr = 0x14,
+	.num_records = 2,
+	.appended_stats_avail = true,
+};
+
+static const struct stats_config rpmh_data = {
+	.offset_addr = 0x4,
+	.num_records = 3,
+	.appended_stats_avail = false,
+};
+
+static const struct of_device_id soc_sleep_stats_table[] = {
+	{ .compatible = "qcom,rpm-sleep-stats", .data = &rpm_data},
+	{ .compatible = "qcom,rpmh-sleep-stats", .data = &rpmh_data},
+	{ }
+};
+
+static struct platform_driver soc_sleep_stats_driver = {
+	.probe = soc_sleep_stats_probe,
+	.remove = soc_sleep_stats_remove,
+	.driver = {
+		.name = "soc_sleep_stats",
+		.of_match_table = soc_sleep_stats_table,
+	},
+};
+module_platform_driver(soc_sleep_stats_driver);
+
+MODULE_DESCRIPTION("Qualcomm Technologies, Inc. (QTI) SoC Sleep Stats driver");
+MODULE_LICENSE("GPL v2");