C++ MSM_BUS_DBG函数代码示例

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

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

	MSM_BUS_DBG("Device = %d buswidth %u", bus_node->node_info->id,
				bus_node->node_info->buswidth);
	for (j = 0; j < bus_node->node_info->num_connections; j++) {
		struct msm_bus_node_device_type *bdev =
			(struct msm_bus_node_device_type *)
			bus_node->node_info->dev_connections[j]->platform_data;
		MSM_BUS_DBG("\n\t Connection[%d] %d", j, bdev->node_info->id);
	}

exit_node_debug:
	return ret;
}

bool msm_bus_rpm_is_mem_interleaved(void)
{
	int status = 0;
	struct msm_rpm_iv_pair il[2];
	uint16_t id[2];

	il[0].value = 0;
	il[1].value = 0;
	status = msm_bus_board_rpm_get_il_ids(id);
	if (status) {
		MSM_BUS_DBG("Dynamic check not supported, "
			"default: Interleaved memory\n");
		goto inter;
	}

	il[0].id = id[0];
	il[1].id = id[1];
	status = msm_rpm_get_status(il, ARRAY_SIZE(il));
	if (status) {
		MSM_BUS_ERR("Status read for interleaving returned: %d\n"
			"Using interleaved memory by default\n",
			status);
		goto inter;
	}

	if ((il[0].value & 0xFFFF0000) != (il[1].value & 0xFFFF0000)) {
		MSM_BUS_DBG("Non-interleaved memory\n");
		return false;
	}

inter:
	MSM_BUS_DBG("Interleaved memory\n");
	return true;
}

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 void msm_bus_bimc_config_limiter(
	struct msm_bus_fabric_registration *fab_pdata,
	struct msm_bus_inode_info *info)
{
	struct msm_bus_bimc_info *binfo;
	int mode, i, ports;

	binfo = (struct msm_bus_bimc_info *)fab_pdata->hw_data;
	ports = info->node_info->num_mports;

	if (!info->node_info->qport) {
		MSM_BUS_DBG("No QoS Ports to init\n");
		return;
	}

	if (info->cur_lim_bw)
		mode = BIMC_QOS_MODE_LIMITER;
	else
		mode = info->node_info->mode;

	switch (mode) {
	case BIMC_QOS_MODE_BYPASS:
	case BIMC_QOS_MODE_FIXED:
		for (i = 0; i < ports; i++)
			bke_switch(binfo->base, info->node_info->qport[i],
				BKE_OFF, mode);
		break;
	case BIMC_QOS_MODE_REGULATOR:
	case BIMC_QOS_MODE_LIMITER:
		if (info->cur_lim_bw != info->cur_prg_bw) {
			MSM_BUS_DBG("Enabled BKE throttling node %d to %llu\n",
				info->node_info->id, info->cur_lim_bw);
			trace_bus_bimc_config_limiter(info->node_info->id,
				info->cur_lim_bw);
			for (i = 0; i < ports; i++) {
				/* If not in fixed mode, update bandwidth */
				struct msm_bus_bimc_qos_bw qbw;

				qbw.ws = info->node_info->ws;
				qbw.bw = info->cur_lim_bw;
				qbw.gp = info->node_info->bimc_gp;
				qbw.thmp = info->node_info->bimc_thmp;
				bimc_set_static_qos_bw(binfo->base,
					binfo->qos_freq,
					info->node_info->qport[i], &qbw);
				bke_switch(binfo->base,
					info->node_info->qport[i],
					BKE_ON, mode);
				info->cur_prg_bw = qbw.bw;
			}
		}
		break;
	default:
		break;
	}
}

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_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 u64 *get_th_params(struct platform_device *pdev,
		const struct device_node *node, const char *prop,
		int *nports)
{
	int size = 0, ret;
	u64 *ret_arr = NULL;
	int *arr = NULL;
	int i;

	if (of_get_property(node, prop, &size)) {
		*nports = size / sizeof(int);
	} else {
		pr_debug("Property %s not available\n", prop);
		*nports = 0;
		return NULL;
	}

	ret_arr = devm_kzalloc(&pdev->dev, (*nports * sizeof(u64)),
							GFP_KERNEL);
	arr = kzalloc(size, GFP_KERNEL);
	if ((size > 0) && (ZERO_OR_NULL_PTR(arr)
			|| ZERO_OR_NULL_PTR(ret_arr))) {
		if (arr)
			kfree(arr);
		else if (ret_arr)
			devm_kfree(&pdev->dev, ret_arr);
		pr_err("Error: Failed to alloc mem for %s\n", prop);
		return NULL;
	}

	ret = of_property_read_u32_array(node, prop, (u32 *)arr, *nports);
	if (ret) {
		pr_err("Error in reading property: %s\n", prop);
		goto err;
	}

	for (i = 0; i < *nports; i++)
		ret_arr[i] = (uint64_t)KBTOB(arr[i]);

	MSM_BUS_DBG("%s: num entries %d prop %s", __func__, *nports, prop);

	for (i = 0; i < *nports; i++)
		MSM_BUS_DBG("Th %d val %llu", i, ret_arr[i]);

	kfree(arr);
	return ret_arr;
err:
	kfree(arr);
	devm_kfree(&pdev->dev, ret_arr);
	return NULL;
}

static void bimc_set_static_qos_bw(void __iomem *base, unsigned int qos_freq,
	int mport, struct msm_bus_bimc_qos_bw *qbw)
{
	int32_t bw_mbps, thh = 0, thm, thl, gc;
	int32_t gp;
	u64 temp;

	if (qos_freq == 0) {
		MSM_BUS_DBG("No QoS Frequency.\n");
		return;
	}

	if (!(qbw->bw && qbw->gp)) {
		MSM_BUS_DBG("No QoS Bandwidth or Window size\n");
		return;
	}

	/* Convert bandwidth to MBPS */
	temp = qbw->bw;
	bimc_div(&temp, 1000000);
	bw_mbps = temp;

	/* Grant period in clock cycles
	 * Grant period from bandwidth structure
	 * is in nano seconds, QoS freq is in KHz.
	 * Divide by 1000 to get clock cycles.
	 */
	gp = (qos_freq * qbw->gp) / (1000 * NSEC_PER_USEC);

	/* Grant count = BW in MBps * Grant period
	 * in micro seconds
	 */
	gc = bw_mbps * (qbw->gp / NSEC_PER_USEC);
	gc = min(gc, MAX_GC);

	/* Medium threshold = -((Medium Threshold percentage *
	 * Grant count) / 100)
	 */
	thm = -((qbw->thmp * gc) / 100);
	qbw->thm = thm;

	/* Low threshold = -(Grant count) */
	thl = -gc;
	qbw->thl = thl;

	MSM_BUS_DBG("%s: BKE parameters: gp %d, gc %d, thm %d thl %d thh %d",
			__func__, gp, gc, thm, thl, thh);

	trace_bus_bke_params(gc, gp, thl, thm, thl);
	set_qos_bw_regs(base, mport, thh, thm, thl, gp, gc);
}

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

static int msm_bus_bimc_commit(struct msm_bus_fabric_registration
	*fab_pdata, void *hw_data, void **cdata)
{
	MSM_BUS_DBG("\nReached BIMC Commit\n");
	msm_bus_remote_hw_commit(fab_pdata, hw_data, cdata);
	return 0;
}

static uint32_t noc_sat_field(uint64_t bw, uint32_t ws, uint32_t qos_freq)
{
	uint32_t sat_field = 0, win;

	if (bw) {
		/* Limit to max bw and scale bw to 100 KB increments */
		uint64_t tbw, tscale;
		uint64_t bw_scaled = min_t(uint64_t, bw, MAX_BW(qos_freq));
		uint32_t rem = noc_div(&bw_scaled, 100000);

		/**
		 * Calculate saturation from windows size.
		 * WS must be at least one arb period.
		 * Saturation must not exceed max field size
		 *
		 * Bandwidth is in 100KB increments
		 * Window size is in ns
		 * qos_freq is in KHz
		 */
		win = max(ws, 1000000 / qos_freq);
		tbw = bw_scaled * win * qos_freq;
		tscale = 10000000ULL * BW_SCALE * SAT_SCALE;
		rem = noc_div(&tbw, tscale);
		sat_field = (uint32_t)min_t(uint64_t, tbw, MAX_SAT_FIELD);
	}

	MSM_BUS_DBG("NOC: sat_field: %d\n", sat_field);
	return sat_field;
}

static void init_health_regs(struct msm_bus_bimc_info *binfo,
				struct msm_bus_inode_info *info,
				struct msm_bus_bimc_qos_mode *qmode,
				int mode)
{
	int i;

	if (mode == BIMC_QOS_MODE_LIMITER) {
		qmode->rl.qhealth[0].limit_commands = 1;
		qmode->rl.qhealth[1].limit_commands = 0;
		qmode->rl.qhealth[2].limit_commands = 0;
		qmode->rl.qhealth[3].limit_commands = 0;

		if (!info->node_info->qport) {
			MSM_BUS_DBG("No QoS Ports to init\n");
			return;
		}

		for (i = 0; i < info->node_info->num_mports; i++) {
			/* If not in bypass mode, update priority */
			if (mode != BIMC_QOS_MODE_BYPASS)
				msm_bus_bimc_set_qos_prio(binfo->base,
				info->node_info->qport[i], mode, qmode);
		}
	}
}

/**
 * msm_bus_scale_unregister_client() - Unregister the client from the bus driver
 * @cl: Handle to the client
 */
void msm_bus_scale_unregister_client(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);
#ifdef SEC_FEATURE_USE_RT_MUTEX
	rt_mutex_lock(&msm_bus_lock);
#else
	mutex_lock(&msm_bus_lock);
#endif
	msm_bus_scale_client_reset_pnodes(cl);
	msm_bus_dbg_client_data(client->pdata, MSM_BUS_DBG_UNREGISTER, cl);
#ifdef SEC_FEATURE_USE_RT_MUTEX
	rt_mutex_unlock(&msm_bus_lock);
#else
	mutex_unlock(&msm_bus_lock);
#endif
	kfree(client->src_pnode);
	kfree(client);
}