C++ MSM_BUS_ERR函数代码示例

static int msm_bus_agg_fab_clks(struct device *bus_dev, void *data)
{
	struct msm_bus_node_device_type *node = NULL;
	int ret = 0;
	int ctx = *(int *)data;

	if (ctx >= NUM_CTX) {
		MSM_BUS_ERR("%s: Invalid Context %d", __func__, ctx);
		goto exit_agg_fab_clks;
	}

	node = bus_dev->platform_data;
	if (!node) {
		MSM_BUS_ERR("%s: Can't get device info", __func__);
		goto exit_agg_fab_clks;
	}

	if (!node->node_info->is_fab_dev) {
		struct msm_bus_node_device_type *bus_dev = NULL;

		bus_dev = node->node_info->bus_device->platform_data;

		if (node->cur_clk_hz[ctx] >= bus_dev->cur_clk_hz[ctx])
			bus_dev->cur_clk_hz[ctx] = node->cur_clk_hz[ctx];
	}

exit_agg_fab_clks:
	return ret;
}

static int msm_bus_dev_init_qos(struct device *dev, void *data)
{
	int ret = 0;
	struct msm_bus_node_device_type *node_dev = NULL;

	node_dev = dev->platform_data;

	if (!node_dev) {
		MSM_BUS_ERR("%s: Unable to get node device info" , __func__);
		ret = -ENXIO;
		goto exit_init_qos;
	}

	MSM_BUS_DBG("Device = %d", node_dev->node_info->id);

	if (node_dev->ap_owned) {
		struct msm_bus_node_device_type *bus_node_info;

		bus_node_info = node_dev->node_info->bus_device->platform_data;

		if (!bus_node_info) {
			MSM_BUS_ERR("%s: Unable to get bus device infofor %d",
				__func__,
				node_dev->node_info->id);
			ret = -ENXIO;
			goto exit_init_qos;
		}

		if (bus_node_info->fabdev &&
			bus_node_info->fabdev->noc_ops.qos_init) {
			int ret = 0;

			if (node_dev->ap_owned &&
				(node_dev->node_info->qos_params.mode) != -1) {

				if (bus_node_info->fabdev->bypass_qos_prg)
					goto exit_init_qos;

				ret = msm_bus_qos_enable_clk(node_dev);
				if (ret < 0) {
					MSM_BUS_ERR("Can't Enable QoS clk %d",
					node_dev->node_info->id);
					goto exit_init_qos;
				}

				bus_node_info->fabdev->noc_ops.qos_init(
					node_dev,
					bus_node_info->fabdev->qos_base,
					bus_node_info->fabdev->base_offset,
					bus_node_info->fabdev->qos_off,
					bus_node_info->fabdev->qos_freq);
				msm_bus_qos_disable_clk(node_dev, ret);
			}
		} else
			MSM_BUS_ERR("%s: Skipping QOS init for %d",
				__func__, node_dev->node_info->id);
	}
exit_init_qos:
	return ret;
}

static int flush_clk_data(struct device *node_device, int ctx)
{
	struct msm_bus_node_device_type *node;
	struct nodeclk *nodeclk = NULL;
	int ret = 0;

	node = node_device->platform_data;
	if (!node) {
		MSM_BUS_ERR("%s: Unable to find bus device for device %d",
			__func__, node->node_info->id);
		ret = -ENODEV;
		goto exit_flush_clk_data;
	}

	nodeclk = &node->clk[ctx];
	if (node->node_info->is_fab_dev) {
		if (nodeclk->rate != node->cur_clk_hz[ctx]) {
			nodeclk->rate = node->cur_clk_hz[ctx];
			nodeclk->dirty = true;
		}
	}

	if (nodeclk && nodeclk->clk && nodeclk->dirty) {
		long rounded_rate;

		if (nodeclk->rate) {
			rounded_rate = clk_round_rate(nodeclk->clk,
							nodeclk->rate);
			ret = setrate_nodeclk(nodeclk, rounded_rate);

			if (ret) {
				MSM_BUS_ERR("%s: Failed to set_rate %lu for %d",
					__func__, rounded_rate,
						node->node_info->id);
				ret = -ENODEV;
				goto exit_flush_clk_data;
			}

			ret = enable_nodeclk(nodeclk);
		} else
			ret = disable_nodeclk(nodeclk);

		if (ret) {
			MSM_BUS_ERR("%s: Failed to enable for %d", __func__,
						node->node_info->id);
			ret = -ENODEV;
			goto exit_flush_clk_data;
		}
		MSM_BUS_DBG("%s: Updated %d clk to %llu", __func__,
				node->node_info->id, nodeclk->rate);

	}
exit_flush_clk_data:
	/* Reset the aggregated clock rate for fab devices*/
	if (node->node_info->is_fab_dev)
		node->cur_clk_hz[ctx] = 0;

	nodeclk->dirty = 0;
	return ret;
}

int msm_bus_commit_data(int *dirty_nodes, int ctx, int num_dirty)
{
	int ret = 0;
	int i = 0;

	/* Aggregate the bus clocks */
	bus_for_each_dev(&msm_bus_type, NULL, (void *)&ctx,
				msm_bus_agg_fab_clks);

	for (i = 0; i < num_dirty; i++) {
		struct device *node_device =
					bus_find_device(&msm_bus_type, NULL,
						(void *)&dirty_nodes[i],
						msm_bus_device_match_adhoc);

		ret = flush_bw_data(node_device, ctx);
		if (ret)
			MSM_BUS_ERR("%s: Error flushing bw data for node %d",
					__func__, dirty_nodes[i]);

		ret = flush_clk_data(node_device, ctx);
		if (ret)
			MSM_BUS_ERR("%s: Error flushing clk data for node %d",
					__func__, dirty_nodes[i]);
	}
	kfree(dirty_nodes);
	/* Aggregate the bus clocks */
	bus_for_each_dev(&msm_bus_type, NULL, (void *)&ctx,
				msm_bus_reset_fab_clks);
	return ret;
}

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);
	if (!fabdev) {
		MSM_BUS_ERR("Fabric not found for: %d\n", fabid);
		goto exit_get_vfe_bw;
	}
	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 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 int msm_bus_init_clk(struct device *bus_dev,
				struct msm_bus_node_device_type *pdata)
{
	unsigned int ctx;
	int ret = 0;
	struct msm_bus_node_device_type *node_dev = bus_dev->platform_data;

	for (ctx = 0; ctx < NUM_CTX; ctx++) {
		if (!IS_ERR_OR_NULL(pdata->clk[ctx].clk)) {
			node_dev->clk[ctx].clk = pdata->clk[ctx].clk;
			node_dev->clk[ctx].enable = false;
			node_dev->clk[ctx].dirty = false;
			MSM_BUS_ERR("%s: Valid node clk node %d ctx %d",
				__func__, node_dev->node_info->id, ctx);
		}
	}

	if (!IS_ERR_OR_NULL(pdata->qos_clk.clk)) {
		node_dev->qos_clk.clk = pdata->qos_clk.clk;
		node_dev->qos_clk.enable = false;
		MSM_BUS_ERR("%s: Valid Iface clk node %d", __func__,
						node_dev->node_info->id);
	}

	return ret;
}

static int send_rpm_msg(struct device *device)
{
	int ret = 0;
	int ctx;
	int rsc_type;
	struct msm_bus_node_device_type *ndev =
					device->platform_data;
	struct msm_rpm_kvp rpm_kvp;

	if (!ndev) {
		MSM_BUS_ERR("%s: Error getting node info.", __func__);
		ret = -ENODEV;
		goto exit_send_rpm_msg;
	}

	rpm_kvp.length = sizeof(uint64_t);
	rpm_kvp.key = RPM_MASTER_FIELD_BW;

	for (ctx = MSM_RPM_CTX_ACTIVE_SET; ctx <= MSM_RPM_CTX_SLEEP_SET;
					ctx++) {
		if (ctx == MSM_RPM_CTX_ACTIVE_SET)
			rpm_kvp.data =
			(uint8_t *)&ndev->node_ab.ab[MSM_RPM_CTX_ACTIVE_SET];
		else {
			rpm_kvp.data =
			(uint8_t *) &ndev->node_ab.ab[MSM_RPM_CTX_SLEEP_SET];
		}

		if (ndev->node_info->mas_rpm_id != -1) {
			rsc_type = RPM_BUS_MASTER_REQ;
			ret = msm_rpm_send_message(ctx, rsc_type,
				ndev->node_info->mas_rpm_id, &rpm_kvp, 1);
			if (ret) {
				MSM_BUS_ERR("%s: Failed to send RPM message:",
						__func__);
				MSM_BUS_ERR("%s:Node Id %d RPM id %d",
				__func__, ndev->node_info->id,
					 ndev->node_info->mas_rpm_id);
				goto exit_send_rpm_msg;
			}
		}

		if (ndev->node_info->slv_rpm_id != -1) {
			rsc_type = RPM_BUS_SLAVE_REQ;
			ret = msm_rpm_send_message(ctx, rsc_type,
				ndev->node_info->slv_rpm_id, &rpm_kvp, 1);
			if (ret) {
				MSM_BUS_ERR("%s: Failed to send RPM message:",
							__func__);
				MSM_BUS_ERR("%s: Node Id %d RPM id %d",
				__func__, ndev->node_info->id,
					ndev->node_info->slv_rpm_id);
				goto exit_send_rpm_msg;
			}
		}
	}
exit_send_rpm_msg:
	return ret;
}

int msm_bus_update_clks(struct msm_bus_node_device_type *nodedev,
		int ctx, int **dirty_nodes, int *num_dirty)
{
	int status = 0;
	struct nodeclk *nodeclk;
	struct nodeclk *busclk;
	struct msm_bus_node_device_type *bus_info = NULL;
	uint64_t req_clk;

	bus_info = nodedev->node_info->bus_device->platform_data;

	if (!bus_info) {
		MSM_BUS_ERR("%s: Unable to find bus device for device %d",
			__func__, nodedev->node_info->id);
		status = -ENODEV;
		goto exit_set_clks;
	}

	req_clk = nodedev->cur_clk_hz[ctx];
	busclk = &bus_info->clk[ctx];

	if (busclk->rate != req_clk) {
		busclk->rate = req_clk;
		busclk->dirty = 1;
		MSM_BUS_DBG("%s: Modifying bus clk %d Rate %llu", __func__,
					bus_info->node_info->id, req_clk);
		status = add_dirty_node(dirty_nodes, bus_info->node_info->id,
								num_dirty);

		if (status) {
			MSM_BUS_ERR("%s: Failed to add dirty node %d", __func__,
						bus_info->node_info->id);
			goto exit_set_clks;
		}
	}

	req_clk = nodedev->cur_clk_hz[ctx];
	nodeclk = &nodedev->clk[ctx];

	if (IS_ERR_OR_NULL(nodeclk))
		goto exit_set_clks;

	if (!nodeclk->dirty || (nodeclk->dirty && (nodeclk->rate < req_clk))) {
		nodeclk->rate = req_clk;
		nodeclk->dirty = 1;
		MSM_BUS_DBG("%s: Modifying node clk %d Rate %llu", __func__,
					nodedev->node_info->id, req_clk);
		status = add_dirty_node(dirty_nodes, nodedev->node_info->id,
								num_dirty);
		if (status) {
			MSM_BUS_ERR("%s: Failed to add dirty node %d", __func__,
						nodedev->node_info->id);
			goto exit_set_clks;
		}
	}

exit_set_clks:
	return status;
}

static int msm_bus_qos_enable_clk(struct msm_bus_node_device_type *node)
{
	struct msm_bus_node_device_type *bus_node = NULL;
	long rounded_rate;
	int ret = 0;
	int bus_qos_enabled = 0;

	if (!node) {
		ret = -ENXIO;
		goto exit_enable_qos_clk;
	}

	bus_node = node->node_info->bus_device->platform_data;

	if (!bus_node) {
		ret = -ENXIO;
		goto exit_enable_qos_clk;
	}

	if (!clk_get_rate(bus_node->clk[DUAL_CTX].clk)) {
		rounded_rate = clk_round_rate(bus_node->clk[DUAL_CTX].clk, 1);
		ret = setrate_nodeclk(&bus_node->clk[DUAL_CTX], rounded_rate);
		if (ret) {
			MSM_BUS_ERR("%s: Failed to set bus clk, node %d",
				__func__, node->node_info->id);
			goto exit_enable_qos_clk;
		}

		ret = enable_nodeclk(&bus_node->clk[DUAL_CTX]);
		if (ret) {
			MSM_BUS_ERR("%s: Failed to enable bus clk, node %d",
				__func__, node->node_info->id);
			goto exit_enable_qos_clk;
		}
		bus_qos_enabled = 1;
	}

	if (!IS_ERR_OR_NULL(node->qos_clk.clk)) {
		rounded_rate = clk_round_rate(node->qos_clk.clk, 1);
		ret = setrate_nodeclk(&node->qos_clk, rounded_rate);
		if (ret) {
			MSM_BUS_ERR("Failed to set bus clk, node %d",
			 node->node_info->id);
			goto exit_enable_qos_clk;
		}

		ret = enable_nodeclk(&node->qos_clk);
		if (ret) {
			MSM_BUS_ERR("Err enable mas qos clk, node %d ret %d",
				node->node_info->id, ret);
			goto exit_enable_qos_clk;
		}
	}
	ret = bus_qos_enabled;

exit_enable_qos_clk:
	return ret;
}

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

static int msm_bus_fabric_init(struct device *dev,
			struct msm_bus_node_device_type *pdata)
{
	struct msm_bus_fab_device_type *fabdev;
	struct msm_bus_node_device_type *node_dev = NULL;
	int ret = 0;

	node_dev = dev->platform_data;
	if (!node_dev) {
		MSM_BUS_ERR("%s: Unable to get bus device info" , __func__);
		ret = -ENXIO;
		goto exit_fabric_init;
	}

	if (node_dev->node_info->virt_dev) {
		MSM_BUS_ERR("%s: Skip Fab init for virtual device %d", __func__,
						node_dev->node_info->id);
		goto exit_fabric_init;
	}

	fabdev = devm_kzalloc(dev, sizeof(struct msm_bus_fab_device_type),
								GFP_KERNEL);
	if (!fabdev) {
		MSM_BUS_ERR("Fabric alloc failed\n");
		ret = -ENOMEM;
		goto exit_fabric_init;
	}

	node_dev->fabdev = fabdev;
	fabdev->pqos_base = pdata->fabdev->pqos_base;
	fabdev->qos_range = pdata->fabdev->qos_range;
	fabdev->base_offset = pdata->fabdev->base_offset;
	fabdev->qos_off = pdata->fabdev->qos_off;
	fabdev->qos_freq = pdata->fabdev->qos_freq;
	fabdev->bus_type = pdata->fabdev->bus_type;
	fabdev->bypass_qos_prg = pdata->fabdev->bypass_qos_prg;
	msm_bus_fab_init_noc_ops(node_dev);

	fabdev->qos_base = devm_ioremap(dev,
				fabdev->pqos_base, fabdev->qos_range);
	if (!fabdev->qos_base) {
		MSM_BUS_ERR("%s: Error remapping address 0x%zx :bus device %d",
			__func__,
			 (size_t)fabdev->pqos_base, node_dev->node_info->id);
		ret = -ENOMEM;
		goto exit_fabric_init;
	}

	/*if (msmbus_coresight_init(pdev))
		pr_warn("Coresight support absent for bus: %d\n", pdata->id);*/
exit_fabric_init:
	return ret;
}

int __init msm_bus_device_init_driver(void)
{
	int rc;

	MSM_BUS_ERR("msm_bus_fabric_init_driver\n");
	rc =  platform_driver_register(&msm_bus_device_driver);

	if (rc) {
		MSM_BUS_ERR("Failed to register bus device driver");
		return rc;
	}
	return platform_driver_register(&msm_bus_rules_driver);
}

static int msm_bus_rpm_commit_arb(struct msm_bus_fabric_registration
	*fab_pdata, int ctx, void *rpm_data,
	struct commit_data *cd, bool valid)
{
	int i, status = 0, rsc_type, key;

	MSM_BUS_DBG("Context: %d\n", ctx);
	rsc_type = RPM_BUS_MASTER_REQ;
	key = RPM_MASTER_FIELD_BW;
	for (i = 0; i < fab_pdata->nmasters; i++) {
		if (cd->mas_arb[i].dirty) {
			MSM_BUS_DBG("MAS HWID: %d, BW: %llu DIRTY: %d\n",
				cd->mas_arb[i].hw_id,
				cd->mas_arb[i].bw,
				cd->mas_arb[i].dirty);
			status = msm_bus_rpm_req(ctx, rsc_type, key,
				&cd->mas_arb[i], valid);
			if (status) {
				MSM_BUS_ERR("RPM: Req fail: mas:%d, bw:%llu\n",
					cd->mas_arb[i].hw_id,
					cd->mas_arb[i].bw);
				break;
			} else {
				cd->mas_arb[i].dirty = false;
			}
		}
	}

	rsc_type = RPM_BUS_SLAVE_REQ;
	key = RPM_SLAVE_FIELD_BW;
	for (i = 0; i < fab_pdata->nslaves; i++) {
		if (cd->slv_arb[i].dirty) {
			MSM_BUS_DBG("SLV HWID: %d, BW: %llu DIRTY: %d\n",
				cd->slv_arb[i].hw_id,
				cd->slv_arb[i].bw,
				cd->slv_arb[i].dirty);
			status = msm_bus_rpm_req(ctx, rsc_type, key,
				&cd->slv_arb[i], valid);
			if (status) {
				MSM_BUS_ERR("RPM: Req fail: slv:%d, bw:%llu\n",
					cd->slv_arb[i].hw_id,
					cd->slv_arb[i].bw);
				break;
			} else {
				cd->slv_arb[i].dirty = false;
			}
		}
	}

	return status;
}

static int msm_bus_setup_dev_conn(struct device *bus_dev, void *data)
{
	struct msm_bus_node_device_type *bus_node = NULL;
	int ret = 0;
	int j;

	bus_node = bus_dev->platform_data;
	if (!bus_node) {
		MSM_BUS_ERR("%s: Can't get device info", __func__);
		ret = -ENODEV;
		goto exit_setup_dev_conn;
	}

	/* Setup parent bus device for this node */
	if (!bus_node->node_info->is_fab_dev) {
		struct device *bus_parent_device =
			bus_find_device(&msm_bus_type, NULL,
				(void *)&bus_node->node_info->bus_device_id,
				msm_bus_device_match_adhoc);

		if (!bus_parent_device) {
			MSM_BUS_ERR("%s: Error finding parentdev %d parent %d",
				__func__,
				bus_node->node_info->id,
				bus_node->node_info->bus_device_id);
			ret = -ENXIO;
			goto exit_setup_dev_conn;
		}
		bus_node->node_info->bus_device = bus_parent_device;
	}

	for (j = 0; j < bus_node->node_info->num_connections; j++) {
		bus_node->node_info->dev_connections[j] =
			bus_find_device(&msm_bus_type, NULL,
				(void *)&bus_node->node_info->connections[j],
				msm_bus_device_match_adhoc);

		if (!bus_node->node_info->dev_connections[j]) {
			MSM_BUS_ERR("%s: Error finding conn %d for device %d",
				__func__, bus_node->node_info->connections[j],
				 bus_node->node_info->id);
			ret = -ENODEV;
			goto exit_setup_dev_conn;
		}
	}

exit_setup_dev_conn:
	return ret;
}