drivers/soc/qcom/msm_bus/msm_bus_rules.c

/* Copyright (c) 2014-2017, The Linux Foundation. All rights reserved.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 and
 * only version 2 as published by the Free Software Foundation.
 *
 * 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/list_sort.h>
#include <linux/msm-bus-board.h>
#include <linux/msm_bus_rules.h>
#include <linux/slab.h>
#include <linux/types.h>
#include <linux/msm-bus.h>
#include <trace/events/trace_msm_bus.h>

struct node_vote_info {
	int id;
	u64 ib;
	u64 ab;
	u64 clk;
};

struct rules_def {
	int rule_id;
	int num_src;
	int state;
	struct node_vote_info *src_info;
	struct bus_rule_type rule_ops;
	bool state_change;
	struct list_head link;
};

struct rule_node_info {
	int id;
	void *data;
	struct raw_notifier_head rule_notify_list;
	struct rules_def *cur_rule;
	int num_rules;
	struct list_head node_rules;
	struct list_head link;
	struct rule_apply_rcm_info apply;
};

DEFINE_MUTEX(msm_bus_rules_lock);
static LIST_HEAD(node_list);
static struct rule_node_info *get_node(u32 id, void *data);
static int node_rules_compare(void *priv, struct list_head *a,
					struct list_head *b);

#define LE(op1, op2)	(op1 <= op2)
#define LT(op1, op2)	(op1 < op2)
#define GE(op1, op2)	(op1 >= op2)
#define GT(op1, op2)	(op1 > op2)
#define NB_ID		(0x201)

static struct rule_node_info *get_node(u32 id, void *data)
{
	struct rule_node_info *node_it = NULL;
	struct rule_node_info *node_match = NULL;

	list_for_each_entry(node_it, &node_list, link) {
		if (node_it->id == id) {
			if (id == NB_ID) {
				if (node_it->data == data) {
					node_match = node_it;
					break;
				}
			} else {
				node_match = node_it;
				break;
			}
		}
	}
	return node_match;
}

static struct rule_node_info *gen_node(u32 id, void *data)
{
	struct rule_node_info *node_it = NULL;
	struct rule_node_info *node_match = NULL;

	list_for_each_entry(node_it, &node_list, link) {
		if (node_it->id == id) {
			node_match = node_it;
			break;
		}
	}

	if (!node_match) {
		node_match = kzalloc(sizeof(struct rule_node_info), GFP_KERNEL);
			goto exit_node_match;

		node_match->id = id;
		node_match->cur_rule = NULL;
		node_match->num_rules = 0;
		node_match->data = data;
		list_add_tail(&node_match->link, &node_list);
		INIT_LIST_HEAD(&node_match->node_rules);
		RAW_INIT_NOTIFIER_HEAD(&node_match->rule_notify_list);
		pr_debug("Added new node %d to list\n", id);
	}
exit_node_match:
	return node_match;
}

static bool do_compare_op(u64 op1, u64 op2, int op)
{
	bool ret = false;

	switch (op) {
	case OP_LE:
		ret = LE(op1, op2);
		break;
	case OP_LT:
		ret = LT(op1, op2);
		break;
	case OP_GT:
		ret = GT(op1, op2);
		break;
	case OP_GE:
		ret = GE(op1, op2);
		break;
	case OP_NOOP:
		ret = true;
		break;
	default:
		pr_info("Invalid OP %d", op);
		break;
	}
	return ret;
}

static void update_src_id_vote(struct rule_update_path_info *inp_node,
				struct rule_node_info *rule_node)
{
	struct rules_def *rule;
	int i;

	list_for_each_entry(rule, &rule_node->node_rules, link) {
		for (i = 0; i < rule->num_src; i++) {
			if (rule->src_info[i].id == inp_node->id) {
				rule->src_info[i].ib = inp_node->ib;
				rule->src_info[i].ab = inp_node->ab;
				rule->src_info[i].clk = inp_node->clk;
			}
		}
	}
}

static u64 get_field(struct rules_def *rule, int src_id)
{
	u64 field = 0;
	int i;

	for (i = 0; i < rule->num_src; i++) {
		switch (rule->rule_ops.src_field) {
		case FLD_IB:
			field += rule->src_info[i].ib;
			break;
		case FLD_AB:
			field += rule->src_info[i].ab;
			break;
		case FLD_CLK:
			field += rule->src_info[i].clk;
			break;
		}
	}

	return field;
}

static bool check_rule(struct rules_def *rule,
			struct rule_update_path_info *inp)
{
	bool ret = false;

	if (!rule)
		return ret;

	switch (rule->rule_ops.op) {
	case OP_LE:
	case OP_LT:
	case OP_GT:
	case OP_GE:
	{
		u64 src_field = get_field(rule, inp->id);

		ret = do_compare_op(src_field, rule->rule_ops.thresh,
							rule->rule_ops.op);
		break;
	}
	default:
		pr_err("Unsupported op %d", rule->rule_ops.op);
		break;
	}
	return ret;
}

static void match_rule(struct rule_update_path_info *inp_node,
			struct rule_node_info *node)
{
	struct rules_def *rule;
	int i;

	list_for_each_entry(rule, &node->node_rules, link) {
		for (i = 0; i < rule->num_src; i++) {
			if (rule->src_info[i].id != inp_node->id)
				continue;

			if (check_rule(rule, inp_node)) {
				trace_bus_rules_matches(
				(node->cur_rule ?
					node->cur_rule->rule_id : -1),
				inp_node->id, inp_node->ab,
				inp_node->ib, inp_node->clk);
				if (rule->state ==
					RULE_STATE_NOT_APPLIED)
					rule->state_change = true;
				rule->state = RULE_STATE_APPLIED;
			} else {
				if (rule->state ==
					RULE_STATE_APPLIED)
					rule->state_change = true;
				rule->state = RULE_STATE_NOT_APPLIED;
			}
		}
	}
}

static void apply_rule(struct rule_node_info *node,
			struct list_head *output_list)
{
	struct rules_def *rule;
	struct rules_def *last_rule;

	last_rule = node->cur_rule;
	node->cur_rule = NULL;
	list_for_each_entry(rule, &node->node_rules, link) {
		if ((rule->state == RULE_STATE_APPLIED) &&
						!node->cur_rule)
			node->cur_rule = rule;

		if (node->id == NB_ID) {
			if (rule->state_change) {
				rule->state_change = false;
				raw_notifier_call_chain(&node->rule_notify_list,
					rule->state, (void *)&rule->rule_ops);
			}
		} else {
			if ((rule->state == RULE_STATE_APPLIED) &&
			     (node->cur_rule &&
				(node->cur_rule->rule_id == rule->rule_id))) {
				node->apply.id = rule->rule_ops.dst_node[0];
				node->apply.throttle = rule->rule_ops.mode;
				node->apply.lim_bw = rule->rule_ops.dst_bw;
				node->apply.after_clk_commit = false;
				if (last_rule != node->cur_rule)
					list_add_tail(&node->apply.link,
								output_list);
				if (last_rule) {
					if (node_rules_compare(NULL,
						&last_rule->link,
						&node->cur_rule->link) == -1)
						node->apply.after_clk_commit =
									true;
				}
			}
			rule->state_change = false;
		}
	}

}

int msm_rules_update_path(struct list_head *input_list,
			struct list_head *output_list)
{
	int ret = 0;
	struct rule_update_path_info  *inp_node;
	struct rule_node_info *node_it = NULL;

	mutex_lock(&msm_bus_rules_lock);
	list_for_each_entry(inp_node, input_list, link) {
		list_for_each_entry(node_it, &node_list, link) {
			update_src_id_vote(inp_node, node_it);
			match_rule(inp_node, node_it);
		}
	}

	list_for_each_entry(node_it, &node_list, link)
		apply_rule(node_it, output_list);
	mutex_unlock(&msm_bus_rules_lock);
	return ret;
}

static bool ops_equal(int op1, int op2)
{
	bool ret = false;

	switch (op1) {
	case OP_GT:
	case OP_GE:
	case OP_LT:
	case OP_LE:
		if (abs(op1 - op2) <= 1)
			ret = true;
		break;
	default:
		ret = (op1 == op2);
	}

	return ret;
}

static bool is_throttle_rule(int mode)
{
	bool ret = true;

	if (mode == THROTTLE_OFF)
		ret = false;

	return ret;
}

static int node_rules_compare(void *priv, struct list_head *a,
					struct list_head *b)
{
	struct rules_def *ra = container_of(a, struct rules_def, link);
	struct rules_def *rb = container_of(b, struct rules_def, link);
	int ret = -1;
	int64_t th_diff = 0;


	if (ra->rule_ops.mode == rb->rule_ops.mode) {
		if (ops_equal(ra->rule_ops.op, rb->rule_ops.op)) {
			if ((ra->rule_ops.op == OP_LT) ||
				(ra->rule_ops.op == OP_LE)) {
				th_diff = ra->rule_ops.thresh -
						rb->rule_ops.thresh;
				if (th_diff > 0)
					ret = 1;
				else
					ret = -1;
			} else if ((ra->rule_ops.op == OP_GT) ||
					(ra->rule_ops.op == OP_GE)) {
				th_diff = rb->rule_ops.thresh -
							ra->rule_ops.thresh;
				if (th_diff > 0)
					ret = 1;
				else
					ret = -1;
			}
		} else {
			ret = ra->rule_ops.op - rb->rule_ops.op;
		}
	} else if (is_throttle_rule(ra->rule_ops.mode) &&
				is_throttle_rule(rb->rule_ops.mode)) {
		if (ra->rule_ops.mode == THROTTLE_ON)
			ret = -1;
		else
			ret = 1;
	} else if ((ra->rule_ops.mode == THROTTLE_OFF) &&
		is_throttle_rule(rb->rule_ops.mode)) {
		ret = 1;
	} else if (is_throttle_rule(ra->rule_ops.mode) &&
		(rb->rule_ops.mode == THROTTLE_OFF)) {
		ret = -1;
	}

	return ret;
}

static void print_rules(struct rule_node_info *node_it)
{
	struct rules_def *node_rule = NULL;
	int i;

	if (!node_it) {
		pr_err("%s: no node for found", __func__);
		return;
	}

	pr_info("\n Now printing rules for Node %d  cur rule %d\n",
			node_it->id,
			(node_it->cur_rule ? node_it->cur_rule->rule_id : -1));
	list_for_each_entry(node_rule, &node_it->node_rules, link) {
		pr_info("\n num Rules %d  rule Id %d\n",
				node_it->num_rules, node_rule->rule_id);
		pr_info("Rule: src_field %d\n", node_rule->rule_ops.src_field);
		for (i = 0; i < node_rule->rule_ops.num_src; i++)
			pr_info("Rule: src %d\n",
					node_rule->rule_ops.src_id[i]);
		for (i = 0; i < node_rule->rule_ops.num_dst; i++)
			pr_info("Rule: dst %d dst_bw %llu\n",
						node_rule->rule_ops.dst_node[i],
						node_rule->rule_ops.dst_bw);
		pr_info("Rule: thresh %llu op %d mode %d State %d\n",
					node_rule->rule_ops.thresh,
					node_rule->rule_ops.op,
					node_rule->rule_ops.mode,
					node_rule->state);
	}
}

void print_all_rules(void)
{
	struct rule_node_info *node_it = NULL;

	list_for_each_entry(node_it, &node_list, link)
		print_rules(node_it);
}

void print_rules_buf(char *buf, int max_buf)
{
	struct rule_node_info *node_it = NULL;
	struct rules_def *node_rule = NULL;
	int i;
	int cnt = 0;

	list_for_each_entry(node_it, &node_list, link) {
		cnt += scnprintf(buf + cnt, max_buf - cnt,
			"\n Now printing rules for Node %d cur_rule %d\n",
			node_it->id,
			(node_it->cur_rule ? node_it->cur_rule->rule_id : -1));
		list_for_each_entry(node_rule, &node_it->node_rules, link) {
			cnt += scnprintf(buf + cnt, max_buf - cnt,
				"\nNum Rules:%d ruleId %d STATE:%d change:%d\n",
				node_it->num_rules, node_rule->rule_id,
				node_rule->state, node_rule->state_change);
			cnt += scnprintf(buf + cnt, max_buf - cnt,
				"Src_field %d\n",
				node_rule->rule_ops.src_field);
			for (i = 0; i < node_rule->rule_ops.num_src; i++)
				cnt += scnprintf(buf + cnt, max_buf - cnt,
					"Src %d Cur Ib %llu Ab %llu\n",
					node_rule->rule_ops.src_id[i],
					node_rule->src_info[i].ib,
					node_rule->src_info[i].ab);
			for (i = 0; i < node_rule->rule_ops.num_dst; i++)
				cnt += scnprintf(buf + cnt, max_buf - cnt,
					"Dst %d dst_bw %llu\n",
					node_rule->rule_ops.dst_node[0],
					node_rule->rule_ops.dst_bw);
			cnt += scnprintf(buf + cnt, max_buf - cnt,
					"Thresh %llu op %d mode %d\n",
					node_rule->rule_ops.thresh,
					node_rule->rule_ops.op,
					node_rule->rule_ops.mode);
		}
	}
}

static int copy_rule(struct bus_rule_type *src, struct rules_def *node_rule,
			struct notifier_block *nb)
{
	int i;
	int ret = 0;

	memcpy(&node_rule->rule_ops, src,
				sizeof(struct bus_rule_type));
	node_rule->rule_ops.src_id = kzalloc(
			(sizeof(int) * node_rule->rule_ops.num_src),
							GFP_KERNEL);
	if (!node_rule->rule_ops.src_id) {
		pr_err("%s:Failed to allocate for src_id",
					__func__);
		return -ENOMEM;
	}
	memcpy(node_rule->rule_ops.src_id, src->src_id,
				sizeof(int) * src->num_src);


	if (!nb) {
		node_rule->rule_ops.dst_node = kzalloc(
			(sizeof(int) * node_rule->rule_ops.num_dst),
						GFP_KERNEL);
		if (!node_rule->rule_ops.dst_node)
			return -ENOMEM;
		memcpy(node_rule->rule_ops.dst_node, src->dst_node,
						sizeof(int) * src->num_dst);
	}

	node_rule->num_src = src->num_src;
	node_rule->src_info = kzalloc(
		(sizeof(struct node_vote_info) * node_rule->rule_ops.num_src),
							GFP_KERNEL);
	if (!node_rule->src_info) {
		pr_err("%s:Failed to allocate for src_id",
						__func__);
		return -ENOMEM;
	}
	for (i = 0; i < src->num_src; i++)
		node_rule->src_info[i].id = src->src_id[i];

	return ret;
}

static bool __rule_register(int num_rules, struct bus_rule_type *rule,
					struct notifier_block *nb)
{
	struct rule_node_info *node = NULL;
	int i, j;
	struct rules_def *node_rule = NULL;
	int num_dst = 0;
	bool reg_success = true;

	if (num_rules <= 0)
		return false;

	for (i = 0; i < num_rules; i++) {
		if (nb)
			num_dst = 1;
		else
			num_dst = rule[i].num_dst;

		for (j = 0; j < num_dst; j++) {
			int id = 0;

			if (nb)
				id = NB_ID;
			else
				id = rule[i].dst_node[j];

			node = gen_node(id, nb);
			if (!node) {
				pr_info("Error getting rule");
				reg_success = false;
				goto exit_rule_register;
			}
			node_rule = kzalloc(sizeof(struct rules_def),
						GFP_KERNEL);
			if (!node_rule) {
				reg_success = false;
				goto exit_rule_register;
			}

			if (copy_rule(&rule[i], node_rule, nb)) {
				pr_err("Error copying rule");
				reg_success = false;
				goto exit_rule_register;
			}

			node_rule->rule_id = node->num_rules++;
			if (nb)
				node->data = nb;

			list_add_tail(&node_rule->link, &node->node_rules);
		}
	}
	list_sort(NULL, &node->node_rules, node_rules_compare);
	if (nb && nb != node->rule_notify_list.head)
		raw_notifier_chain_register(&node->rule_notify_list, nb);
exit_rule_register:
	return reg_success;
}

static int comp_rules(struct bus_rule_type *rulea, struct bus_rule_type *ruleb)
{
	int ret = 1;

	if (rulea->num_src == ruleb->num_src)
		ret = memcmp(rulea->src_id, ruleb->src_id,
				(sizeof(int) * rulea->num_src));
	if (!ret && (rulea->num_dst == ruleb->num_dst))
		ret = memcmp(rulea->dst_node, ruleb->dst_node,
				(sizeof(int) * rulea->num_dst));
	if (ret || (rulea->dst_bw != ruleb->dst_bw) ||
		(rulea->op != ruleb->op) || (rulea->thresh != ruleb->thresh))
		ret = 1;
	return ret;
}

void msm_rule_register(int num_rules, struct bus_rule_type *rule,
					struct notifier_block *nb)
{
	if (!rule || num_rules <= 0)
		return;

	mutex_lock(&msm_bus_rules_lock);
	__rule_register(num_rules, rule, nb);
	mutex_unlock(&msm_bus_rules_lock);
}

static bool __rule_unregister(int num_rules, struct bus_rule_type *rule,
					struct notifier_block *nb)
{
	int i = 0;
	struct rule_node_info *node = NULL;
	struct rule_node_info *node_tmp = NULL;
	struct rules_def *node_rule;
	struct rules_def *node_rule_tmp;
	bool match_found = false;

	if (num_rules <= 0)
		return false;

	if (nb) {
		node = get_node(NB_ID, nb);
		if (!node) {
			pr_err("%s: Can't find node", __func__);
			goto exit_unregister_rule;
		}
		match_found = true;
		list_for_each_entry_safe(node_rule, node_rule_tmp,
					&node->node_rules, link) {
			if (comp_rules(&node_rule->rule_ops,
					&rule[i]) == 0) {
				list_del(&node_rule->link);
				kfree(node_rule);
				match_found = true;
				node->num_rules--;
				list_sort(NULL,
					&node->node_rules,
					node_rules_compare);
				break;
			}
		}
		if (!node->num_rules)
			raw_notifier_chain_unregister(
					&node->rule_notify_list, nb);
	} else {
		for (i = 0; i < num_rules; i++) {
			match_found = false;

			list_for_each_entry(node, &node_list, link) {
				list_for_each_entry_safe(node_rule,
				node_rule_tmp, &node->node_rules, link) {
					if (comp_rules(&node_rule->rule_ops,
						&rule[i]) != 0)
						continue;
					list_del(&node_rule->link);
					kfree(node_rule);
					match_found = true;
					node->num_rules--;
					list_sort(NULL,
						&node->node_rules,
						node_rules_compare);
					break;
				}
			}
		}
	}

	list_for_each_entry_safe(node, node_tmp,
					&node_list, link) {
		if (!node->num_rules) {
			pr_debug("Deleting Rule node %d", node->id);
			list_del(&node->link);
			kfree(node);
		}
	}
exit_unregister_rule:
	return match_found;
}

void msm_rule_unregister(int num_rules, struct bus_rule_type *rule,
					struct notifier_block *nb)
{
	if (!rule || num_rules <= 0)
		return;

	mutex_lock(&msm_bus_rules_lock);
	__rule_unregister(num_rules, rule, nb);
	mutex_unlock(&msm_bus_rules_lock);
}

bool msm_rule_update(struct bus_rule_type *old_rule,
			struct bus_rule_type *new_rule,
			struct notifier_block *nb)
{
	bool rc = true;

	if (!old_rule || !new_rule) {
		pr_err("%s:msm_rule_update: void rules, error\n", __func__);
		return false;
	}
	mutex_lock(&msm_bus_rules_lock);
	if (!__rule_unregister(1, old_rule, nb)) {
		pr_err("%s:msm_rule_update: failed to unregister old rule\n",
				__func__);
		rc = false;
		goto exit_rule_update;
	}

	if (!__rule_register(1, new_rule, nb)) {
		/*
		 * Registering new rule has failed for some reason, attempt
		 * to re-register the old rule and return error.
		 */
		pr_err("%s:msm_rule_update: failed to register new rule\n",
				__func__);
		__rule_register(1, old_rule, nb);
		rc = false;
	}
exit_rule_update:
	mutex_unlock(&msm_bus_rules_lock);
	return rc;
}

void msm_rule_evaluate_rules(int node)
{
	struct msm_bus_client_handle *handle;

	handle = msm_bus_scale_register(node, node, "tmp-rm", false);
	if (!handle)
		return;
	msm_bus_scale_update_bw(handle, 0, 0);
	msm_bus_scale_unregister(handle);
}

bool msm_rule_are_rules_registered(void)
{
	bool ret = false;

	if (list_empty(&node_list))
		ret = false;
	else
		ret = true;

	return ret;
}