ASR_BASE
Change-Id: Icf3719cc0afe3eeb3edc7fa80a2eb5199ca9dda1
diff --git a/marvell/linux/kernel/power/user_qos.c b/marvell/linux/kernel/power/user_qos.c
new file mode 100644
index 0000000..b3c4222
--- /dev/null
+++ b/marvell/linux/kernel/power/user_qos.c
@@ -0,0 +1,218 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Support for asr spi controller
+ *
+ * Copyright (C) 2021 ASR Micro Limited
+ *
+ */
+
+#include <linux/ctype.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/hrtimer.h>
+#include <linux/list.h>
+#include <linux/rbtree.h>
+#include <linux/slab.h>
+#include <linux/pm_qos.h>
+
+static DEFINE_MUTEX(qos_lock);
+
+struct pmqos {
+ char *name;
+ struct rb_node node;
+ struct pm_qos_request qr;
+};
+
+/*1 to 1 mapped to the pm_qos_class_id in pm_qos.h*/
+static struct rb_root pmqos_tree[PM_QOS_NUM_CLASSES] = {
+ RB_ROOT,
+ RB_ROOT,
+ RB_ROOT,
+ RB_ROOT,
+ RB_ROOT,
+ RB_ROOT,
+
+ RB_ROOT,
+ RB_ROOT,
+
+ RB_ROOT,
+ RB_ROOT,
+};
+
+ssize_t pm_show_pmqos(char *buf, int pm_qos_class)
+{
+ struct rb_node *node;
+ struct pmqos *pq;
+ char *str = buf;
+ char *end = buf + PAGE_SIZE;
+
+ mutex_lock(&qos_lock);
+
+ for (node = rb_first(&pmqos_tree[pm_qos_class]); node; node = rb_next(node)) {
+ pq = rb_entry(node, struct pmqos, node);
+ if (pq->qr.pm_qos_class == pm_qos_class)
+ str += scnprintf(str, end - str, "%s ", pq->name);
+ }
+ if (str > buf)
+ str--;
+
+ str += scnprintf(str, end - str, "\n");
+
+ mutex_unlock(&qos_lock);
+ return (str - buf);
+}
+
+static struct pmqos *pmqos_lookup_add(const char *name, int pm_qos_class,
+ size_t len, bool add_if_not_found)
+{
+ struct rb_node **node = &pmqos_tree[pm_qos_class].rb_node;
+ struct rb_node *parent = *node;
+ struct pmqos *pq;
+
+ while (*node) {
+ int diff;
+
+ parent = *node;
+ pq = rb_entry(*node, struct pmqos, node);
+ diff = strncmp(name, pq->name, len);
+ if (diff == 0) {
+ if (pq->name[len])
+ diff = -1;
+ else
+ return pq;
+ }
+ if (diff < 0)
+ node = &(*node)->rb_left;
+ else
+ node = &(*node)->rb_right;
+ }
+ if (!add_if_not_found)
+ return ERR_PTR(-EINVAL);
+
+ /* Not found, we have to add a new one. */
+ pq = kzalloc(sizeof(*pq), GFP_KERNEL);
+ if (!pq)
+ return ERR_PTR(-ENOMEM);
+
+ pq->name = kstrndup(name, len, GFP_KERNEL);
+ if (!pq->name) {
+ kfree(pq);
+ return ERR_PTR(-ENOMEM);
+ }
+ pq->qr.name = pq->name;
+ pm_qos_add_request(&pq->qr,
+ pm_qos_class, PM_QOS_DEFAULT_VALUE);
+ rb_link_node(&pq->node, parent, node);
+ rb_insert_color(&pq->node, &pmqos_tree[pm_qos_class]);
+ return pq;
+}
+
+int pm_freq_qos(const char *buf, int pm_qos_class)
+{
+ const char *str = buf;
+ struct pmqos *pq;
+ u64 timeout_ns = 0;
+ unsigned long freq = 0;
+ size_t len;
+ int ret = 0;
+
+ while (*str && !isspace(*str))
+ str++;
+
+ len = str - buf;
+ if (!len)
+ return -EINVAL;
+
+ str = skip_spaces(str);
+ if (*str && *str != '\n') {
+ /*duplicate the rest of the string so that we can support multiple args*/
+ char *strdup = kzalloc((strlen(str) + 1), GFP_KERNEL);
+ char *strstart = strdup;
+ char *strpos = strdup;
+ char *strpos2 = NULL;
+
+ if (!strdup)
+ return -ENOMEM;
+ else
+ strcpy(strdup, str);
+
+ while (*strdup && !isspace(*strdup))
+ strdup++;
+ strpos2 = strdup;
+ strdup = skip_spaces(strdup);
+ if(strdup != strpos2) {
+ /*There is 2nd arg as timeout value*/
+ if (*strdup && *strdup != '\n') {
+ *strpos2 = '\0';
+ /* Find out if there's a valid timeout string appended. */
+ ret = kstrtou64(strdup, 10, &timeout_ns);
+ if (ret) {
+ kfree(strstart);
+ return -EINVAL;
+ }
+ }
+ }
+ /*Here is the 1st arg as freq value*/
+ ret = kstrtoul(strpos, 10, &freq);
+ if (ret) {
+ kfree(strstart);
+ return -EINVAL;
+ }
+ /* free strstart to avoid memleak */
+ kfree(strstart);
+ }
+
+ mutex_lock(&qos_lock);
+ pq = pmqos_lookup_add(buf, pm_qos_class, len, true);
+ if (IS_ERR(pq)) {
+ ret = PTR_ERR(pq);
+ goto out;
+ }
+ if (timeout_ns)
+ {
+ /* up bound to usecs, conver to us by do_div */
+ u64 timeout_us = timeout_ns + NSEC_PER_USEC - 1;
+
+ do_div(timeout_us, NSEC_PER_USEC);
+ pm_qos_update_request_timeout(&pq->qr, freq, timeout_us);
+ } else {
+ pm_qos_update_request(&pq->qr, freq);
+ }
+
+ out:
+ mutex_unlock(&qos_lock);
+ return ret;
+}
+
+int pm_freq_unqos(const char *buf, int pm_qos_class)
+{
+ struct pmqos *pq;
+ size_t len;
+ int ret = 0;
+
+ len = strlen(buf);
+ if (!len)
+ return -EINVAL;
+
+ if (buf[len-1] == '\n')
+ len--;
+
+ if (!len)
+ return -EINVAL;
+
+ mutex_lock(&qos_lock);
+
+ pq = pmqos_lookup_add(buf, pm_qos_class, len, false);
+ if (IS_ERR(pq)) {
+ ret = PTR_ERR(pq);
+ goto out;
+ }
+ pm_qos_remove_request(&pq->qr);
+ rb_erase(&pq->node, &pmqos_tree[pm_qos_class]);
+ kfree(pq->name);
+ kfree(pq);
+
+ out:
+ mutex_unlock(&qos_lock);
+ return ret;
+}