marvell/linux/kernel/power/user_qos.c

// 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;
}