path: root/drivers/soc/qcom/msm_bus/msm_bus_arb.c

/* Copyright (c) 2011-2014, 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.
 */

#define pr_fmt(fmt) "AXI: %s(): " fmt, __func__

#include <linux/kernel.h>
#include <linux/init.h>
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/mutex.h>
#include <linux/radix-tree.h>
#include <linux/clk.h>
#include <linux/msm-bus.h>
#include "msm_bus_core.h"
#include <trace/events/trace_msm_bus.h>

#define INDEX_MASK 0x0000FFFF
#define PNODE_MASK 0xFFFF0000
#define SHIFT_VAL 16
#define CREATE_PNODE_ID(n, i) (((n) << SHIFT_VAL) | (i))
#define GET_INDEX(n) ((n) & INDEX_MASK)
#define GET_NODE(n) ((n) >> SHIFT_VAL)
#define IS_NODE(n) ((n) % FABRIC_ID_KEY)
#define SEL_FAB_CLK 1
#define SEL_SLAVE_CLK 0
/*
 * To get to BIMC BW convert Hz to bytes by multiplying bus width(8),
 * double-data-rate(2) * ddr-channels(2).
 */
#define GET_BIMC_BW(clk)	(clk * 8 * 2 * 2)

#define BW_TO_CLK_FREQ_HZ(width, bw) \
	msm_bus_div64(width, bw)

#define IS_MASTER_VALID(mas) \
	(((mas >= MSM_BUS_MASTER_FIRST) && (mas <= MSM_BUS_MASTER_LAST)) \
	 ? 1 : 0)

#define IS_SLAVE_VALID(slv) \
	(((slv >= MSM_BUS_SLAVE_FIRST) && (slv <= MSM_BUS_SLAVE_LAST)) ? 1 : 0)

static DEFINE_MUTEX(msm_bus_lock);

/* This function uses shift operations to divide 64 bit value for higher
 * efficiency. The divisor expected are number of ports or bus-width.
 * These are expected to be 1, 2, 4, 8, 16 and 32 in most cases.
 *
 * To account for exception to the above divisor values, the standard
 * do_div function is used.
 * */
uint64_t msm_bus_div64(unsigned int w, uint64_t bw)
{
	uint64_t *b = &bw;

	if ((bw > 0) && (bw < w))
		return 1;

	switch (w) {
	case 0:
		WARN(1, "AXI: Divide by 0 attempted\n");
	case 1: return bw;
	case 2:	return (bw >> 1);
	case 4:	return (bw >> 2);
	case 8:	return (bw >> 3);
	case 16: return (bw >> 4);
	case 32: return (bw >> 5);
	}

	do_div(*b, w);
	return *b;
}

/**
 * add_path_node: Adds the path information to the current node
 * @info: Internal node info structure
 * @next: Combination of the id and index of the next node
 * Function returns: Number of pnodes (path_nodes) on success,
 * error on failure.
 *
 * Every node maintains the list of path nodes. A path node is
 * reached by finding the node-id and index stored at the current
 * node. This makes updating the paths with requested bw and clock
 * values efficient, as it avoids lookup for each update-path request.
 */
static int add_path_node(struct msm_bus_inode_info *info, int next)
{
	struct path_node *pnode;
	int i;
	if (ZERO_OR_NULL_PTR(info)) {
		MSM_BUS_ERR("Cannot find node info!: id :%d\n",
				info->node_info->priv_id);
		return -ENXIO;
	}

	for (i = 0; i <= info->num_pnodes; i++) {
		if (info->pnode[i].next == -2) {
			MSM_BUS_DBG("Reusing pnode for info: %d at index: %d\n",
				info->node_info->priv_id, i);
			info->pnode[i].clk[DUAL_CTX] = 0;
			info->pnode[i].clk[ACTIVE_CTX] = 0;
			info->pnode[i].bw[DUAL_CTX] = 0;
			info->pnode[i].bw[ACTIVE_CTX] = 0;
			info->pnode[i].next = next;
			MSM_BUS_DBG("%d[%d] : (%d, %d)\n",
				info->node_info->priv_id, i, GET_NODE(next),
				GET_INDEX(next));
			return i;
		}
	}

	info->num_pnodes++;
	pnode = krealloc(info->pnode,
		((info->num_pnodes + 1) * sizeof(struct path_node))
		, GFP_KERNEL);
	if (ZERO_OR_NULL_PTR(pnode)) {
		MSM_BUS_ERR("Error creating path node!\n");
		info->num_pnodes--;
		return -ENOMEM;
	}
	info->pnode = pnode;
	info->pnode[info->num_pnodes].clk[DUAL_CTX] = 0;
	info->pnode[info->num_pnodes].clk[ACTIVE_CTX] = 0;
	info->pnode[info->num_pnodes].bw[DUAL_CTX] = 0;
	info->pnode[info->num_pnodes].bw[ACTIVE_CTX] = 0;
	info->pnode[info->num_pnodes].next = next;
	MSM_BUS_DBG("%d[%d] : (%d, %d)\n", info->node_info->priv_id,
		info->num_pnodes, GET_NODE(next), GET_INDEX(next));
	return info->num_pnodes;
}

static int clearvisitedflag(struct device *dev, void *data)
{
	struct msm_bus_fabric_device *fabdev = to_msm_bus_fabric_device(dev);
	fabdev->visited = false;
	return 0;
}

/**
 * getpath() - Finds the path from the topology between src and dest
 * @src: Source. This is the master from which the request originates
 * @dest: Destination. This is the slave to which we're trying to reach
 *
 * Function returns: next_pnode_id. The higher 16 bits of the next_pnode_id
 * represent the src id of the  next node on path. The lower 16 bits of the
 * next_pnode_id represent the "index", which is the next entry in the array
 * of pnodes for that node to fill in clk and bw values. This is created using
 * CREATE_PNODE_ID. The return value is stored in ret_pnode, and this is added
 * to the list of path nodes.
 *
 * This function recursively finds the path by updating the src to the
 * closest possible node to dest.
 */
static int getpath(int src, int dest)
{
	int pnode_num = -1, i;
	struct msm_bus_fabnodeinfo *fabnodeinfo;
	struct msm_bus_fabric_device *fabdev;
	int next_pnode_id = -1;
	struct msm_bus_inode_info *info = NULL;
	int _src = src/FABRIC_ID_KEY;
	int _dst = dest/FABRIC_ID_KEY;
	int ret_pnode = -1;
	int fabid = GET_FABID(src);

	/* Find the location of fabric for the src */
	MSM_BUS_DBG("%d --> %d\n", src, dest);

	fabdev = msm_bus_get_fabric_device(fabid);
	if (!fabdev) {
		MSM_BUS_WARN("Fabric Not yet registered. Try again\n");
		return -ENXIO;
	}

	/* Are we there yet? */
	if (src == dest) {
		info = fabdev->algo->find_node(fabdev, src);
		if (ZERO_OR_NULL_PTR(info)) {
			MSM_BUS_ERR("Node %d not found\n", dest);
			return -ENXIO;
		}

		for (i = 0; i <= info->num_pnodes; i++) {
			if (info->pnode[i].next == -2) {
				MSM_BUS_DBG("src = dst  Reusing pnode for"
				" info: %d at index: %d\n",
				info->node_info->priv_id, i);
				next_pnode_id = CREATE_PNODE_ID(src, i);
				info->pnode[i].clk[DUAL_CTX] = 0;
				info->pnode[i].bw[DUAL_CTX] = 0;
				info->pnode[i].next = next_pnode_id;
				MSM_BUS_DBG("returning: %d, %d\n", GET_NODE
				(next_pnode_id), GET_INDEX(next_pnode_id));
				return next_pnode_id;
			}
		}
		next_pnode_id = CREATE_PNODE_ID(src, (info->num_pnodes + 1));
		pnode_num = add_path_node(info, next_pnode_id);
		if (pnode_num < 0) {
			MSM_BUS_ERR("Error adding path node\n");
			return -ENXIO;
		}
		MSM_BUS_DBG("returning: %d, %d\n", GET_NODE(next_pnode_id),
			GET_INDEX(next_pnode_id));
		return next_pnode_id;
	} else if (_src == _dst) {
		/*
		 * src and dest belong to same fabric, find the destination
		 * from the radix tree
		 */
		info = fabdev->algo->find_node(fabdev, dest);
		if (ZERO_OR_NULL_PTR(info)) {
			MSM_BUS_ERR("Node %d not found\n", dest);
			return -ENXIO;
		}

		ret_pnode = getpath(info->node_info->priv_id, dest);
		next_pnode_id = ret_pnode;
	} else {
		/* find the dest fabric */
		int trynextgw = true;
		struct list_head *gateways = fabdev->algo->get_gw_list(fabdev);
		list_for_each_entry(fabnodeinfo, gateways, list) {
		/* see if the destination is at a connected fabric */
			if (_dst == (fabnodeinfo->info->node_info->priv_id /
				FABRIC_ID_KEY)) {
				/* Found the fab on which the device exists */
				info = fabnodeinfo->info;
				trynextgw = false;
				ret_pnode = getpath(info->node_info->priv_id,
					dest);
				pnode_num = add_path_node(info, ret_pnode);
				if (pnode_num < 0) {
					MSM_BUS_ERR("Error adding path node\n");
					return -ENXIO;
				}
				next_pnode_id = CREATE_PNODE_ID(
					info->node_info->priv_id, pnode_num);
				break;
			}
		}

		/* find the gateway */
		if (trynextgw) {
			gateways = fabdev->algo->get_gw_list(fabdev);
			list_for_each_entry(fabnodeinfo, gateways, list) {
				struct msm_bus_fabric_device *gwfab =
					msm_bus_get_fabric_device(fabnodeinfo->
						info->node_info->priv_id);
				if (!gwfab) {
					MSM_BUS_ERR("Err: No gateway found\n");
					return -ENXIO;
				}

				if (!gwfab->visited) {
					MSM_BUS_DBG("VISITED ID: %d\n",
						gwfab->id);
					gwfab->visited = true;
					info = fabnodeinfo->info;
					ret_pnode = getpath(info->
						node_info->priv_id, dest);
					pnode_num = add_path_node(info,
						ret_pnode);
					if (pnode_num < 0) {
						MSM_BUS_ERR("Malloc failure in"
						" adding path node\n");
						return -ENXIO;
					}
					next_pnode_id = CREATE_PNODE_ID(
					info->node_info->priv_id, pnode_num);
					break;
				}
			}
			if (next_pnode_id < 0)
				return -ENXIO;
		}
	}

	if (!IS_NODE(src)) {
		MSM_BUS_DBG("Returning next_pnode_id:%d[%d]\n", GET_NODE(
			next_pnode_id), GET_INDEX(next_pnode_id));
		return next_pnode_id;
	}
	info = fabdev->algo->find_node(fabdev, src);
	if (!info) {
		MSM_BUS_ERR("Node info not found.\n");
		return -ENXIO;
	}

	pnode_num = add_path_node(info, next_pnode_id);
	MSM_BUS_DBG(" Last: %d[%d] = (%d, %d)\n",
		src, info->num_pnodes, GET_NODE(next_pnode_id),
		GET_INDEX(next_pnode_id));
	MSM_BUS_DBG("returning: %d, %d\n", src, pnode_num);
	return CREATE_PNODE_ID(src, pnode_num);
}

static uint64_t get_node_maxib(struct msm_bus_inode_info *info)
{
	int i, ctx;
	uint64_t maxib = 0;

	for (i = 0; i <= info->num_pnodes; i++) {
		for (ctx = 0; ctx < NUM_CTX; ctx++)
			maxib = max(info->pnode[i].clk[ctx], maxib);
	}

	MSM_BUS_DBG("%s: Node %d numpnodes %d maxib %llu", __func__,
		info->num_pnodes, info->node_info->id, maxib);
	return maxib;
}


static uint64_t get_node_sumab(struct msm_bus_inode_info *info)
{
	int i;
	uint64_t maxab = 0;

	for (i = 0; i <= info->num_pnodes; i++)
		maxab += info->pnode[i].bw[DUAL_CTX];

	MSM_BUS_DBG("%s: Node %d numpnodes %d maxib %llu", __func__,
		info->num_pnodes, info->node_info->id, maxab);
	return maxab;
}

static uint64_t get_vfe_bw(void)
{
	int vfe_id = MSM_BUS_MASTER_VFE;
	int iid = msm_bus_board_get_iid(vfe_id);
	int fabid;
	struct msm_bus_fabric_device *fabdev;
	struct msm_bus_inode_info *info;
	uint64_t vfe_bw = 0;

	fabid = GET_FABID(iid);
	fabdev = msm_bus_get_fabric_device(fabid);
	info = fabdev->algo->find_node(fabdev, iid);

	if (!info) {
		MSM_BUS_ERR("%s: Can't find node %d", __func__,
						vfe_id);
		goto exit_get_vfe_bw;
	}

	vfe_bw = get_node_sumab(info);
	MSM_BUS_DBG("vfe_ab %llu", vfe_bw);

exit_get_vfe_bw:
	return vfe_bw;
}

static uint64_t get_mdp_bw(void)
{
	int ids[] = {MSM_BUS_MASTER_MDP_PORT0, MSM_BUS_MASTER_MDP_PORT1};
	int i;
	uint64_t mdp_ab = 0;
	uint32_t ff = 0;

	for (i = 0; i < ARRAY_SIZE(ids); i++) {
		int iid = msm_bus_board_get_iid(ids[i]);
		int fabid;
		struct msm_bus_fabric_device *fabdev;
		struct msm_bus_inode_info *info;

		fabid = GET_FABID(iid);
		fabdev = msm_bus_get_fabric_device(fabid);
		info = fabdev->algo->find_node(fabdev, iid);

		if (!info) {
			MSM_BUS_ERR("%s: Can't find node %d", __func__,
								ids[i]);
			continue;
		}

		mdp_ab += get_node_sumab(info);
		MSM_BUS_DBG("mdp_ab %llu", mdp_ab);
		ff = info->node_info->ff;
	}

	if (ff) {
		mdp_ab = msm_bus_div64(2 * ff, 100 * mdp_ab);
	} else {
		MSM_BUS_ERR("MDP FF is 0");
		mdp_ab = 0;
	}


	MSM_BUS_DBG("MDP BW %llu\n", mdp_ab);
	return mdp_ab;
}

static uint64_t get_rt_bw(void)
{
	uint64_t rt_bw = 0;

	rt_bw += get_mdp_bw();
	rt_bw += get_vfe_bw();

	return rt_bw;
}

static uint64_t get_avail_bw(struct msm_bus_fabric_device *fabdev)
{
	uint64_t fabclk_rate = 0;
	int i;
	uint64_t avail_bw = 0;
	uint64_t rt_bw = get_rt_bw();
	struct msm_bus_fabric *fabric = to_msm_bus_fabric(fabdev);

	if (!rt_bw)
		goto exit_get_avail_bw;

	for (i = 0; i < NUM_CTX; i++) {
		uint64_t ctx_rate;
		ctx_rate =
			fabric->info.nodeclk[i].rate;
		fabclk_rate = max(ctx_rate, fabclk_rate);
	}

	if (!fabdev->eff_fact || !fabdev->nr_lim_thresh) {
		MSM_BUS_ERR("Error: Eff-fact %d; nr_thresh %llu",
				fabdev->eff_fact, fabdev->nr_lim_thresh);
		return 0;
	}

	avail_bw = msm_bus_div64(100,
				(GET_BIMC_BW(fabclk_rate) * fabdev->eff_fact));

	if (avail_bw >= fabdev->nr_lim_thresh)
		return 0;

	MSM_BUS_DBG("%s: Total_avail_bw %llu, rt_bw %llu\n",
		__func__, avail_bw, rt_bw);
	trace_bus_avail_bw(avail_bw, rt_bw);

	if (avail_bw < rt_bw) {
		MSM_BUS_ERR("\n%s: ERROR avail BW %llu < MDP %llu",
			__func__, avail_bw, rt_bw);
		avail_bw = 0;
		goto exit_get_avail_bw;
	}
	avail_bw -= rt_bw;

exit_get_avail_bw:
	return avail_bw;
}

static void program_nr_limits(struct msm_bus_fabric_device *fabdev)
{
	int num_nr_lim = 0;
	int i;
	struct msm_bus_inode_info *info[fabdev->num_nr_lim];
	struct msm_bus_fabric *fabric = to_msm_bus_fabric(fabdev);

	num_nr_lim = radix_tree_gang_lookup_tag(&fabric->fab_tree,
			(void **)&info, fabric->fabdev.id, fabdev->num_nr_lim,
			MASTER_NODE);

	for (i = 0; i < num_nr_lim; i++)
		fabdev->algo->config_limiter(fabdev, info[i]);
}

static int msm_bus_commit_limiter(struct device *dev, void *data)
{
	int ret = 0;
	struct msm_bus_fabric_device *fabdev = to_msm_bus_fabric_device(dev);

	MSM_BUS_DBG("fabid: %d\n", fabdev->id);
	program_nr_limits(fabdev);
	return ret;
}

static void compute_nr_limits(struct msm_bus_fabric_device *fabdev, int pnode)
{
	uint64_t total_ib = 0;
	int num_nr_lim = 0;
	uint64_t avail_bw = 0;
	struct msm_bus_inode_info *info[fabdev->num_nr_lim];
	struct msm_bus_fabric *fabric = to_msm_bus_fabric(fabdev);
	int i;

	num_nr_lim = radix_tree_gang_lookup_tag(&fabric->fab_tree,
			(void **)&info, fabric->fabdev.id, fabdev->num_nr_lim,
			MASTER_NODE);

	MSM_BUS_DBG("%s: Found %d NR LIM nodes", __func__, num_nr_lim);
	for (i = 0; i < num_nr_lim; i++)
		total_ib += get_node_maxib(info[i]);

	avail_bw = get_avail_bw(fabdev);
	MSM_BUS_DBG("\n %s: Avail BW %llu", __func__, avail_bw);

	for (i = 0; i < num_nr_lim; i++) {
		uint32_t node_pct = 0;
		uint64_t new_lim_bw = 0;
		uint64_t node_max_ib = 0;
		uint32_t node_max_ib_kB = 0;
		uint32_t total_ib_kB = 0;
		uint64_t bw_node;

		node_max_ib = get_node_maxib(info[i]);
		node_max_ib_kB = msm_bus_div64(1024, node_max_ib);
		total_ib_kB = msm_bus_div64(1024, total_ib);
		node_pct = (node_max_ib_kB * 100) / total_ib_kB;
		bw_node = node_pct * avail_bw;
		new_lim_bw = msm_bus_div64(100, bw_node);

		/*
		 * if limiter bw is more than the requested IB clip to
		   requested IB.
		*/
		if (new_lim_bw >= node_max_ib)
			new_lim_bw = node_max_ib;

		/*
		 * if there is a floor bw for this nr lim node and
		 *  if there is available bw to divy up among the nr masters
		 *  and if the nr lim masters have a non zero vote and
		 *  if the limited bw is below the floor for this node.
		 *    then limit this node to the floor bw.
		 */
		if (info[i]->node_info->floor_bw && node_max_ib && avail_bw &&
			(new_lim_bw <= info[i]->node_info->floor_bw)) {
			MSM_BUS_ERR("\nNode %d:Limiting BW:%llu < floor:%llu",
				info[i]->node_info->id,	new_lim_bw,
						info[i]->node_info->floor_bw);
			new_lim_bw = info[i]->node_info->floor_bw;
		}

		if (new_lim_bw != info[i]->cur_lim_bw) {
			info[i]->cur_lim_bw = new_lim_bw;
			MSM_BUS_DBG("NodeId %d: Requested IB %llu",
					info[i]->node_info->id, node_max_ib);
			MSM_BUS_DBG("Limited to %llu(%d pct of Avail %llu )\n",
					new_lim_bw, node_pct, avail_bw);
		} else {
			MSM_BUS_DBG("NodeId %d: No change Limited to %llu\n",
				info[i]->node_info->id, info[i]->cur_lim_bw);
		}
	}
}

static void setup_nr_limits(int curr, int pnode)
{
	struct msm_bus_fabric_device *fabdev =
		msm_bus_get_fabric_device(GET_FABID(curr));
	struct msm_bus_inode_info *info;

	if (!fabdev) {
		MSM_BUS_WARN("Fabric Not yet registered. Try again\n");
		goto exit_setup_nr_limits;
	}

	/* This logic is currently applicable to BIMC masters only */
	if (fabdev->id != MSM_BUS_FAB_DEFAULT) {
		MSM_BUS_ERR("Static limiting of NR masters only for BIMC\n");
		goto exit_setup_nr_limits;
	}

	info = fabdev->algo->find_node(fabdev, curr);
	if (!info) {
		MSM_BUS_ERR("Cannot find node info!\n");
		goto exit_setup_nr_limits;
	}

	compute_nr_limits(fabdev, pnode);
exit_setup_nr_limits:
	return;
}

static bool is_nr_lim(int id)
{
	struct msm_bus_fabric_device *fabdev = msm_bus_get_fabric_device
		(GET_FABID(id));
	struct msm_bus_inode_info *info;
	bool ret = false;

	if (!fabdev) {
		MSM_BUS_ERR("Bus device for bus ID: %d not found!\n",
			GET_FABID(id));
		goto exit_is_nr_lim;
	}

	info = fabdev->algo->find_node(fabdev, id);
	if (!info)
		MSM_BUS_ERR("Cannot find node info %d!\n", id);
	else if ((info->node_info->nr_lim || info->node_info->rt_mas))
		ret = true;
exit_is_nr_lim:
	return ret;
}

/**
 * update_path() - Update the path with the bandwidth and clock values, as
 * requested by the client.
 *
 * @curr: Current source node, as specified in the client vector (master)
 * @pnode: The first-hop node on the path, stored in the internal client struct
 * @req_clk: Requested clock value from the vector
 * @req_bw: Requested bandwidth value from the vector
 * @curr_clk: Current clock frequency
 * @curr_bw: Currently allocated bandwidth
 *
 * This function updates the nodes on the path calculated using getpath(), with
 * clock and bandwidth values. The sum of bandwidths, and the max of clock
 * frequencies is calculated at each node on the path. Commit data to be sent
 * to RPM for each master and slave is also calculated here.
 */
static int update_path(int curr, int pnode, uint64_t req_clk, uint64_t req_bw,
	uint64_t curr_clk, uint64_t curr_bw, unsigned int ctx, unsigned int
	cl_active_flag)
{
	int index, ret = 0;
	struct msm_bus_inode_info *info;
	struct msm_bus_inode_info *src_info;
	int next_pnode;
	int64_t add_bw = req_bw - curr_bw;
	uint64_t bwsum = 0;
	uint64_t req_clk_hz, curr_clk_hz, bwsum_hz;
	int *master_tiers;
	struct msm_bus_fabric_device *fabdev = msm_bus_get_fabric_device
		(GET_FABID(curr));

	if (!fabdev) {
		MSM_BUS_ERR("Bus device for bus ID: %d not found!\n",
			GET_FABID(curr));
		return -ENXIO;
	}

	MSM_BUS_DBG("args: %d %d %d %llu %llu %llu %llu %u\n",
		curr, GET_NODE(pnode), GET_INDEX(pnode), req_clk, req_bw,
		curr_clk, curr_bw, ctx);
	index = GET_INDEX(pnode);
	MSM_BUS_DBG("Client passed index :%d\n", index);
	info = fabdev->algo->find_node(fabdev, curr);
	if (!info) {
		MSM_BUS_ERR("Cannot find node info!\n");
		return -ENXIO;
	}
	src_info = info;

	info->link_info.sel_bw = &info->link_info.bw[ctx];
	info->link_info.sel_clk = &info->link_info.clk[ctx];
	*info->link_info.sel_bw += add_bw;

	info->pnode[index].sel_bw = &info->pnode[index].bw[ctx];

	/**
	 * To select the right clock, AND the context with
	 * client active flag.
	 */
	info->pnode[index].sel_clk = &info->pnode[index].clk[ctx &
		cl_active_flag];
	*info->pnode[index].sel_bw += add_bw;
	*info->pnode[index].sel_clk = req_clk;

	/**
	 * If master supports dual configuration, check if
	 * the configuration needs to be changed based on
	 * incoming requests
	 */
	if (info->node_info->dual_conf) {
		uint64_t node_maxib = 0;
		node_maxib = get_node_maxib(info);
		fabdev->algo->config_master(fabdev, info,
			node_maxib, req_bw);
	}

	info->link_info.num_tiers = info->node_info->num_tiers;
	info->link_info.tier = info->node_info->tier;
	master_tiers = info->node_info->tier;

	do {
		struct msm_bus_inode_info *hop;
		fabdev = msm_bus_get_fabric_device(GET_FABID(curr));
		if (!fabdev) {
			MSM_BUS_ERR("Fabric not found\n");
			return -ENXIO;
		}
		MSM_BUS_DBG("id: %d\n", info->node_info->priv_id);

		/* find next node and index */
		next_pnode = info->pnode[index].next;
		curr = GET_NODE(next_pnode);
		index = GET_INDEX(next_pnode);
		MSM_BUS_DBG("id:%d, next: %d\n", info->
		    node_info->priv_id, curr);

		/* Get hop */
		/* check if we are here as gateway, or does the hop belong to
		 * this fabric */
		if (IS_NODE(curr))
			hop = fabdev->algo->find_node(fabdev, curr);
		else
			hop = fabdev->algo->find_gw_node(fabdev, curr);
		if (!hop) {
			MSM_BUS_ERR("Null Info found for hop\n");
			return -ENXIO;
		}

		hop->link_info.sel_bw = &hop->link_info.bw[ctx];
		hop->link_info.sel_clk = &hop->link_info.clk[ctx];
		*hop->link_info.sel_bw += add_bw;

		hop->pnode[index].sel_bw = &hop->pnode[index].bw[ctx];
		hop->pnode[index].sel_clk = &hop->pnode[index].clk[ctx &
			cl_active_flag];

		if (!hop->node_info->buswidth) {
			MSM_BUS_WARN("No bus width found. Using default\n");
			hop->node_info->buswidth = 8;
		}
		*hop->pnode[index].sel_clk = BW_TO_CLK_FREQ_HZ(hop->node_info->
			buswidth, req_clk);
		*hop->pnode[index].sel_bw += add_bw;
		MSM_BUS_DBG("fabric: %d slave: %d, slave-width: %d info: %d\n",
			fabdev->id, hop->node_info->priv_id, hop->node_info->
			buswidth, info->node_info->priv_id);
		/* Update Bandwidth */
		fabdev->algo->update_bw(fabdev, hop, info, add_bw,
			master_tiers, ctx);
		bwsum = *hop->link_info.sel_bw;
		/* Update Fabric clocks */
		curr_clk_hz = BW_TO_CLK_FREQ_HZ(hop->node_info->buswidth,
			curr_clk);
		req_clk_hz = BW_TO_CLK_FREQ_HZ(hop->node_info->buswidth,
			req_clk);
		bwsum_hz = BW_TO_CLK_FREQ_HZ(hop->node_info->buswidth,
			bwsum);
		/* Account for multiple channels if any */
		if (hop->node_info->num_sports > 1)
			bwsum_hz = msm_bus_div64(hop->node_info->num_sports,
				bwsum_hz);
		MSM_BUS_DBG("AXI: Hop: %d, ports: %d, bwsum_hz: %llu\n",
				hop->node_info->id, hop->node_info->num_sports,
				bwsum_hz);
		MSM_BUS_DBG("up-clk: curr_hz: %llu, req_hz: %llu, bw_hz %llu\n",
			curr_clk, req_clk, bwsum_hz);
		ret = fabdev->algo->update_clks(fabdev, hop, index,
			curr_clk_hz, req_clk_hz, bwsum_hz, SEL_FAB_CLK,
			ctx, cl_active_flag);
		if (ret)
			MSM_BUS_WARN("Failed to update clk\n");
		info = hop;
	} while (GET_NODE(info->pnode[index].next) != info->node_info->priv_id);

	/* Update BW, clk after exiting the loop for the last one */
	if (!info) {
		MSM_BUS_ERR("Cannot find node info!\n");
		return -ENXIO;
	}

	/* Update slave clocks */
	ret = fabdev->algo->update_clks(fabdev, info, index, curr_clk_hz,
	    req_clk_hz, bwsum_hz, SEL_SLAVE_CLK, ctx, cl_active_flag);
	if (ret)
		MSM_BUS_ERR("Failed to update clk\n");

	if ((ctx == cl_active_flag) &&
		((src_info->node_info->nr_lim || src_info->node_info->rt_mas)))
		setup_nr_limits(curr, pnode);

	/* If freq is going down , apply the changes now before
	 * we commit clk data.
	 */
	if ((req_clk < curr_clk) || (req_bw < curr_bw))
		bus_for_each_dev(&msm_bus_type, NULL, NULL,
					msm_bus_commit_limiter);
	return ret;
}

/**
 * msm_bus_commit_fn() - Commits the data for fabric to rpm
 * @dev: fabric device
 * @data: NULL
 */
static int msm_bus_commit_fn(struct device *dev, void *data)
{
	int ret = 0;
	struct msm_bus_fabric_device *fabdev = to_msm_bus_fabric_device(dev);
	MSM_BUS_DBG("Committing: fabid: %d\n", fabdev->id);
	ret = fabdev->algo->commit(fabdev);
	return ret;
}

static uint32_t register_client_legacy(struct msm_bus_scale_pdata *pdata)
{
	struct msm_bus_client *client = NULL;
	int i;
	int src, dest, nfab;
	struct msm_bus_fabric_device *deffab;

	deffab = msm_bus_get_fabric_device(MSM_BUS_FAB_DEFAULT);
	if (!deffab) {
		MSM_BUS_ERR("Error finding default fabric\n");
		return 0;
	}

	nfab = msm_bus_get_num_fab();
	if (nfab < deffab->board_algo->board_nfab) {
		MSM_BUS_ERR("Can't register client!\n"
				"Num of fabrics up: %d\n",
				nfab);
		return 0;
	}

	if ((!pdata) || (pdata->usecase->num_paths == 0) || IS_ERR(pdata)) {
		MSM_BUS_ERR("Cannot register client with null data\n");
		return 0;
	}

	client = kzalloc(sizeof(struct msm_bus_client), GFP_KERNEL);
	if (!client) {
		MSM_BUS_ERR("Error allocating client\n");
		return 0;
	}

	mutex_lock(&msm_bus_lock);
	client->pdata = pdata;
	client->curr = -1;
	for (i = 0; i < pdata->usecase->num_paths; i++) {
		int *pnode;
		struct msm_bus_fabric_device *srcfab;
		pnode = krealloc(client->src_pnode, ((i + 1) * sizeof(int)),
			GFP_KERNEL);
		if (ZERO_OR_NULL_PTR(pnode)) {
			MSM_BUS_ERR("Invalid Pnode ptr!\n");
			continue;
		} else
			client->src_pnode = pnode;

		if (!IS_MASTER_VALID(pdata->usecase->vectors[i].src)) {
			MSM_BUS_ERR("Invalid Master ID %d in request!\n",
				pdata->usecase->vectors[i].src);
			goto err;
		}

		if (!IS_SLAVE_VALID(pdata->usecase->vectors[i].dst)) {
			MSM_BUS_ERR("Invalid Slave ID %d in request!\n",
				pdata->usecase->vectors[i].dst);
			goto err;
		}

		src = msm_bus_board_get_iid(pdata->usecase->vectors[i].src);
		if (src == -ENXIO) {
			MSM_BUS_ERR("Master %d not supported. Client cannot be"
				" registered\n",
				pdata->usecase->vectors[i].src);
			goto err;
		}
		dest = msm_bus_board_get_iid(pdata->usecase->vectors[i].dst);
		if (dest == -ENXIO) {
			MSM_BUS_ERR("Slave %d not supported. Client cannot be"
				" registered\n",
				pdata->usecase->vectors[i].dst);
			goto err;
		}
		srcfab = msm_bus_get_fabric_device(GET_FABID(src));
		if (!srcfab) {
			MSM_BUS_ERR("Fabric not found\n");
			goto err;
		}

		srcfab->visited = true;
		pnode[i] = getpath(src, dest);
		bus_for_each_dev(&msm_bus_type, NULL, NULL, clearvisitedflag);
		if (pnode[i] == -ENXIO) {
			MSM_BUS_ERR("Cannot register client now! Try again!\n");
			goto err;
		}
	}
	msm_bus_dbg_client_data(client->pdata, MSM_BUS_DBG_REGISTER,
		(uint32_t)client);
	mutex_unlock(&msm_bus_lock);
	MSM_BUS_DBG("ret: %u num_paths: %d\n", (uint32_t)client,
		pdata->usecase->num_paths);
	return (uint32_t)(client);
err:
	kfree(client->src_pnode);
	kfree(client);
	mutex_unlock(&msm_bus_lock);
	return 0;
}

static int update_request_legacy(uint32_t cl, unsigned index)
{
	int i, ret = 0;
	struct msm_bus_scale_pdata *pdata;
	int pnode, src = 0, curr, ctx;
	uint64_t req_clk = 0, req_bw = 0, curr_clk = 0, curr_bw = 0;
	struct msm_bus_client *client = (struct msm_bus_client *)cl;
	if (IS_ERR_OR_NULL(client)) {
		MSM_BUS_ERR("msm_bus_scale_client update req error %d\n",
				(uint32_t)client);
		return -ENXIO;
	}

	mutex_lock(&msm_bus_lock);
	if (client->curr == index)
		goto err;

	curr = client->curr;
	pdata = client->pdata;
	if (!pdata) {
		MSM_BUS_ERR("Null pdata passed to update-request\n");
		ret = -ENXIO;
		goto err;
	}

	if (index >= pdata->num_usecases) {
		MSM_BUS_ERR("Client %u passed invalid index: %d\n",
			(uint32_t)client, index);
		ret = -ENXIO;
		goto err;
	}

	MSM_BUS_DBG("cl: %u index: %d curr: %d num_paths: %d\n",
		cl, index, client->curr, client->pdata->usecase->num_paths);

	for (i = 0; i < pdata->usecase->num_paths; i++) {
		src = msm_bus_board_get_iid(client->pdata->usecase[index].
			vectors[i].src);
		if (src == -ENXIO) {
			MSM_BUS_ERR("Master %d not supported. Request cannot"
				" be updated\n", client->pdata->usecase->
				vectors[i].src);
			goto err;
		}

		if (msm_bus_board_get_iid(client->pdata->usecase[index].
			vectors[i].dst) == -ENXIO) {
			MSM_BUS_ERR("Slave %d not supported. Request cannot"
				" be updated\n", client->pdata->usecase->
				vectors[i].dst);
		}

		pnode = client->src_pnode[i];
		req_clk = client->pdata->usecase[index].vectors[i].ib;
		req_bw = client->pdata->usecase[index].vectors[i].ab;
		if (curr < 0) {
			curr_clk = 0;
			curr_bw = 0;
		} else {
			curr_clk = client->pdata->usecase[curr].vectors[i].ib;
			curr_bw = client->pdata->usecase[curr].vectors[i].ab;
			MSM_BUS_DBG("ab: %llu ib: %llu\n", curr_bw, curr_clk);
		}

		if (!pdata->active_only) {
			ret = update_path(src, pnode, req_clk, req_bw,
				curr_clk, curr_bw, 0, pdata->active_only);
			if (ret) {
				MSM_BUS_ERR("Update path failed! %d\n", ret);
				goto err;
			}
		}

		ret = update_path(src, pnode, req_clk, req_bw, curr_clk,
				curr_bw, ACTIVE_CTX, pdata->active_only);
		if (ret) {
			MSM_BUS_ERR("Update Path failed! %d\n", ret);
			goto err;
		}
	}

	client->curr = index;
	ctx = ACTIVE_CTX;
	msm_bus_dbg_client_data(client->pdata, index, cl);
	bus_for_each_dev(&msm_bus_type, NULL, NULL, msm_bus_commit_fn);

	/* For NR/RT limited masters, if freq is going up , apply the changes
	 * after we commit clk data.
	 */
	if (is_nr_lim(src) && ((req_clk > curr_clk) || (req_bw > curr_bw)))
		bus_for_each_dev(&msm_bus_type, NULL, NULL,
					msm_bus_commit_limiter);

err:
	mutex_unlock(&msm_bus_lock);
	return ret;
}

static int reset_pnodes(int curr, int pnode)
{
	struct msm_bus_inode_info *info;
	struct msm_bus_fabric_device *fabdev;
	int index, next_pnode;
	fabdev = msm_bus_get_fabric_device(GET_FABID(curr));
	if (!fabdev) {
		MSM_BUS_ERR("Fabric not found for: %d\n",
			(GET_FABID(curr)));
			return -ENXIO;
	}

	index = GET_INDEX(pnode);
	info = fabdev->algo->find_node(fabdev, curr);
	if (!info) {
		MSM_BUS_ERR("Cannot find node info!\n");
		return -ENXIO;
	}

	MSM_BUS_DBG("Starting the loop--remove\n");
	do {
		struct msm_bus_inode_info *hop;
		fabdev = msm_bus_get_fabric_device(GET_FABID(curr));
		if (!fabdev) {
			MSM_BUS_ERR("Fabric not found\n");
				return -ENXIO;
		}

		next_pnode = info->pnode[index].next;
		info->pnode[index].next = -2;
		curr = GET_NODE(next_pnode);
		index = GET_INDEX(next_pnode);
		if (IS_NODE(curr))
			hop = fabdev->algo->find_node(fabdev, curr);
		else
			hop = fabdev->algo->find_gw_node(fabdev, curr);
		if (!hop) {
			MSM_BUS_ERR("Null Info found for hop\n");
			return -ENXIO;
		}

		MSM_BUS_DBG("%d[%d] = %d\n", info->node_info->priv_id, index,
			info->pnode[index].next);
		MSM_BUS_DBG("num_pnodes: %d: %d\n", info->node_info->priv_id,
			info->num_pnodes);
		info = hop;
	} while (GET_NODE(info->pnode[index].next) != info->node_info->priv_id);

	info->pnode[index].next = -2;
	MSM_BUS_DBG("%d[%d] = %d\n", info->node_info->priv_id, index,
		info->pnode[index].next);
	MSM_BUS_DBG("num_pnodes: %d: %d\n", info->node_info->priv_id,
		info->num_pnodes);
	return 0;
}

int msm_bus_board_get_iid(int id)
{
	struct msm_bus_fabric_device *deffab;

	deffab = msm_bus_get_fabric_device(MSM_BUS_FAB_DEFAULT);
	if (!deffab) {
		MSM_BUS_ERR("Error finding default fabric\n");
		return -ENXIO;
	}

	return deffab->board_algo->get_iid(id);
}

void msm_bus_scale_client_reset_pnodes(uint32_t cl)
{
	int i, src, pnode, index;
	struct msm_bus_client *client = (struct msm_bus_client *)(cl);
	if (IS_ERR_OR_NULL(client)) {
		MSM_BUS_ERR("msm_bus_scale_reset_pnodes error\n");
		return;
	}
	index = 0;
	for (i = 0; i < client->pdata->usecase->num_paths; i++) {
		src = msm_bus_board_get_iid(
			client->pdata->usecase[index].vectors[i].src);
		pnode = client->src_pnode[i];
		MSM_BUS_DBG("(%d, %d)\n", GET_NODE(pnode), GET_INDEX(pnode));
		reset_pnodes(src, pnode);
	}
}

static void unregister_client_legacy(uint32_t cl)
{
	int i;
	struct msm_bus_client *client = (struct msm_bus_client *)(cl);
	bool warn = false;
	if (IS_ERR_OR_NULL(client))
		return;

	for (i = 0; i < client->pdata->usecase->num_paths; i++) {
		if ((client->pdata->usecase[0].vectors[i].ab) ||
			(client->pdata->usecase[0].vectors[i].ib)) {
			warn = true;
			break;
		}
	}

	if (warn) {
		int num_paths = client->pdata->usecase->num_paths;
		int ab[num_paths], ib[num_paths];
		WARN(1, "%s called unregister with non-zero vectors\n",
			client->pdata->name);

		/*
		 * Save client values and zero them out to
		 * cleanly unregister
		 */
		for (i = 0; i < num_paths; i++) {
			ab[i] = client->pdata->usecase[0].vectors[i].ab;
			ib[i] = client->pdata->usecase[0].vectors[i].ib;
			client->pdata->usecase[0].vectors[i].ab = 0;
			client->pdata->usecase[0].vectors[i].ib = 0;
		}

		msm_bus_scale_client_update_request(cl, 0);

		/* Restore client vectors if required for re-registering. */
		for (i = 0; i < num_paths; i++) {
			client->pdata->usecase[0].vectors[i].ab = ab[i];
			client->pdata->usecase[0].vectors[i].ib = ib[i];
		}
	} else if (client->curr != 0)
		msm_bus_scale_client_update_request(cl, 0);

	MSM_BUS_DBG("Unregistering client %d\n", cl);
	mutex_lock(&msm_bus_lock);
	msm_bus_scale_client_reset_pnodes(cl);
	msm_bus_dbg_client_data(client->pdata, MSM_BUS_DBG_UNREGISTER, cl);
	mutex_unlock(&msm_bus_lock);
	kfree(client->src_pnode);
	kfree(client);
}

void msm_bus_arb_setops_legacy(struct msm_bus_arb_ops *arb_ops)
{
	arb_ops->register_client = register_client_legacy;
	arb_ops->update_request = update_request_legacy;
	arb_ops->unregister_client = unregister_client_legacy;
}