Commit 35975401 authored by Kory Maincent (Dent Project)'s avatar Kory Maincent (Dent Project) Committed by Jakub Kicinski
Browse files

net: pse-pd: pd692x0: Add support for PSE PI priority feature 



This patch extends the PSE callbacks by adding support for the newly
introduced pi_set_prio() callback, enabling the configuration of PSE PI
priorities. The current port priority is now also included in the status
information returned to users.

Signed-off-by: default avatarKory Maincent (Dent Project) <kory.maincent@bootlin.com>
Reviewed-by: default avatarOleksij Rempel <o.rempel@pengutronix.de>
Link: https://patch.msgid.link/20250617-feature_poe_port_prio-v14-9-78a1a645e2ee@bootlin.com


Signed-off-by: default avatarJakub Kicinski <kuba@kernel.org>
parent eeb0c8f7

drivers/net/pse-pd/pd692x0.c

+205 −0
Original line number Diff line number Diff line
@@ -12,6 +12,8 @@ 
#include <linux/of.h>

#include <linux/platform_device.h>

#include <linux/pse-pd/pse.h>

#include <linux/regulator/driver.h>

#include <linux/regulator/machine.h>



#define PD692X0_PSE_NAME "pd692x0_pse"
@@ -76,6 +78,8 @@ enum { 
	PD692X0_MSG_GET_PORT_CLASS,

	PD692X0_MSG_GET_PORT_MEAS,

	PD692X0_MSG_GET_PORT_PARAM,

	PD692X0_MSG_GET_POWER_BANK,

	PD692X0_MSG_SET_POWER_BANK,



	/* add new message above here */

	PD692X0_MSG_CNT
@@ -95,6 +99,8 @@ struct pd692x0_priv { 
	unsigned long last_cmd_key_time;



	enum ethtool_c33_pse_admin_state admin_state[PD692X0_MAX_PIS];

	struct regulator_dev *manager_reg[PD692X0_MAX_MANAGERS];

	int manager_pw_budget[PD692X0_MAX_MANAGERS];

};



/* Template list of communication messages. The non-null bytes defined here
@@ -170,6 +176,16 @@ static const struct pd692x0_msg pd692x0_msg_template_list[PD692X0_MSG_CNT] = { 
		.data = {0x4e, 0x4e, 0x4e, 0x4e,

			 0x4e, 0x4e, 0x4e, 0x4e},

	},

	[PD692X0_MSG_GET_POWER_BANK] = {

		.key = PD692X0_KEY_REQ,

		.sub = {0x07, 0x0b, 0x57},

		.data = {   0, 0x4e, 0x4e, 0x4e,

			 0x4e, 0x4e, 0x4e, 0x4e},

	},

	[PD692X0_MSG_SET_POWER_BANK] = {

		.key = PD692X0_KEY_CMD,

		.sub = {0x07, 0x0b, 0x57},

	},

};



static u8 pd692x0_build_msg(struct pd692x0_msg *msg, u8 echo)
@@ -739,6 +755,29 @@ pd692x0_pi_get_actual_pw(struct pse_controller_dev *pcdev, int id) 
	return (buf.data[0] << 4 | buf.data[1]) * 100;

}



static int

pd692x0_pi_get_prio(struct pse_controller_dev *pcdev, int id)

{

	struct pd692x0_priv *priv = to_pd692x0_priv(pcdev);

	struct pd692x0_msg msg, buf = {0};

	int ret;



	ret = pd692x0_fw_unavailable(priv);

	if (ret)

		return ret;



	msg = pd692x0_msg_template_list[PD692X0_MSG_GET_PORT_PARAM];

	msg.sub[2] = id;

	ret = pd692x0_sendrecv_msg(priv, &msg, &buf);

	if (ret < 0)

		return ret;

	if (!buf.data[2] || buf.data[2] > pcdev->pis_prio_max + 1)

		return -ERANGE;



	/* PSE core priority start at 0 */

	return buf.data[2] - 1;

}



static struct pd692x0_msg_ver pd692x0_get_sw_version(struct pd692x0_priv *priv)

{

	struct device *dev = &priv->client->dev;
@@ -766,6 +805,7 @@ static struct pd692x0_msg_ver pd692x0_get_sw_version(struct pd692x0_priv *priv) 


struct pd692x0_manager {

	struct device_node *port_node[PD692X0_MAX_MANAGER_PORTS];

	struct device_node *node;

	int nports;

};
@@ -857,6 +897,8 @@ pd692x0_of_get_managers(struct pd692x0_priv *priv, 
		if (ret)

			goto out;



		of_node_get(node);

		manager[manager_id].node = node;

		nmanagers++;

	}
@@ -869,6 +911,8 @@ pd692x0_of_get_managers(struct pd692x0_priv *priv, 
			of_node_put(manager[i].port_node[j]);

			manager[i].port_node[j] = NULL;

		}

		of_node_put(manager[i].node);

		manager[i].node = NULL;

	}



	of_node_put(node);
@@ -876,6 +920,130 @@ pd692x0_of_get_managers(struct pd692x0_priv *priv, 
	return ret;

}



static const struct regulator_ops dummy_ops;



static struct regulator_dev *

pd692x0_register_manager_regulator(struct device *dev, char *reg_name,

				   struct device_node *node)

{

	struct regulator_init_data *rinit_data;

	struct regulator_config rconfig = {0};

	struct regulator_desc *rdesc;

	struct regulator_dev *rdev;



	rinit_data = devm_kzalloc(dev, sizeof(*rinit_data),

				  GFP_KERNEL);

	if (!rinit_data)

		return ERR_PTR(-ENOMEM);



	rdesc = devm_kzalloc(dev, sizeof(*rdesc), GFP_KERNEL);

	if (!rdesc)

		return ERR_PTR(-ENOMEM);



	rdesc->name = reg_name;

	rdesc->type = REGULATOR_VOLTAGE;

	rdesc->ops = &dummy_ops;

	rdesc->owner = THIS_MODULE;



	rinit_data->supply_regulator = "vmain";



	rconfig.dev = dev;

	rconfig.init_data = rinit_data;

	rconfig.of_node = node;



	rdev = devm_regulator_register(dev, rdesc, &rconfig);

	if (IS_ERR(rdev)) {

		dev_err_probe(dev, PTR_ERR(rdev),

			      "Failed to register regulator\n");

		return rdev;

	}



	return rdev;

}



static int

pd692x0_register_managers_regulator(struct pd692x0_priv *priv,

				    const struct pd692x0_manager *manager,

				    int nmanagers)

{

	struct device *dev = &priv->client->dev;

	size_t reg_name_len;

	int i;



	/* Each regulator name len is dev name + 12 char +

	 * int max digit number (10) + 1

	 */

	reg_name_len = strlen(dev_name(dev)) + 23;



	for (i = 0; i < nmanagers; i++) {

		struct regulator_dev *rdev;

		char *reg_name;



		reg_name = devm_kzalloc(dev, reg_name_len, GFP_KERNEL);

		if (!reg_name)

			return -ENOMEM;

		snprintf(reg_name, 26, "pse-%s-manager%d", dev_name(dev), i);

		rdev = pd692x0_register_manager_regulator(dev, reg_name,

							  manager[i].node);

		if (IS_ERR(rdev))

			return PTR_ERR(rdev);



		priv->manager_reg[i] = rdev;

	}



	return 0;

}



static int

pd692x0_conf_manager_power_budget(struct pd692x0_priv *priv, int id, int pw)

{

	struct pd692x0_msg msg, buf;

	int ret, pw_mW = pw / 1000;



	msg = pd692x0_msg_template_list[PD692X0_MSG_GET_POWER_BANK];

	msg.data[0] = id;

	ret = pd692x0_sendrecv_msg(priv, &msg, &buf);

	if (ret < 0)

		return ret;



	msg = pd692x0_msg_template_list[PD692X0_MSG_SET_POWER_BANK];

	msg.data[0] = id;

	msg.data[1] = pw_mW >> 8;

	msg.data[2] = pw_mW & 0xff;

	msg.data[3] = buf.sub[2];

	msg.data[4] = buf.data[0];

	msg.data[5] = buf.data[1];

	msg.data[6] = buf.data[2];

	msg.data[7] = buf.data[3];

	return pd692x0_sendrecv_msg(priv, &msg, &buf);

}



static int

pd692x0_configure_managers(struct pd692x0_priv *priv, int nmanagers)

{

	int i, ret;



	for (i = 0; i < nmanagers; i++) {

		struct regulator *supply = priv->manager_reg[i]->supply;

		int pw_budget;



		pw_budget = regulator_get_unclaimed_power_budget(supply);

		/* Max power budget per manager */

		if (pw_budget > 6000000)

			pw_budget = 6000000;

		ret = regulator_request_power_budget(supply, pw_budget);

		if (ret < 0)

			return ret;



		priv->manager_pw_budget[i] = pw_budget;

		ret = pd692x0_conf_manager_power_budget(priv, i, pw_budget);

		if (ret < 0)

			return ret;

	}



	return 0;

}



static int

pd692x0_set_port_matrix(const struct pse_pi_pairset *pairset,

			const struct pd692x0_manager *manager,
@@ -998,6 +1166,14 @@ static int pd692x0_setup_pi_matrix(struct pse_controller_dev *pcdev) 
		return ret;



	nmanagers = ret;

	ret = pd692x0_register_managers_regulator(priv, manager, nmanagers);

	if (ret)

		goto out;



	ret = pd692x0_configure_managers(priv, nmanagers);

	if (ret)

		goto out;



	ret = pd692x0_set_ports_matrix(priv, manager, nmanagers, port_matrix);

	if (ret)

		goto out;
@@ -1008,8 +1184,14 @@ static int pd692x0_setup_pi_matrix(struct pse_controller_dev *pcdev) 


out:

	for (i = 0; i < nmanagers; i++) {

		struct regulator *supply = priv->manager_reg[i]->supply;



		regulator_free_power_budget(supply,

					    priv->manager_pw_budget[i]);



		for (j = 0; j < manager[i].nports; j++)

			of_node_put(manager[i].port_node[j]);

		of_node_put(manager[i].node);

	}

	return ret;

}
@@ -1071,6 +1253,25 @@ static int pd692x0_pi_set_pw_limit(struct pse_controller_dev *pcdev, 
	return pd692x0_sendrecv_msg(priv, &msg, &buf);

}



static int pd692x0_pi_set_prio(struct pse_controller_dev *pcdev, int id,

			       unsigned int prio)

{

	struct pd692x0_priv *priv = to_pd692x0_priv(pcdev);

	struct pd692x0_msg msg, buf = {0};

	int ret;



	ret = pd692x0_fw_unavailable(priv);

	if (ret)

		return ret;



	msg = pd692x0_msg_template_list[PD692X0_MSG_SET_PORT_PARAM];

	msg.sub[2] = id;

	/* Controller priority from 1 to 3 */

	msg.data[4] = prio + 1;



	return pd692x0_sendrecv_msg(priv, &msg, &buf);

}



static const struct pse_controller_ops pd692x0_ops = {

	.setup_pi_matrix = pd692x0_setup_pi_matrix,

	.pi_get_admin_state = pd692x0_pi_get_admin_state,
@@ -1084,6 +1285,8 @@ static const struct pse_controller_ops pd692x0_ops = { 
	.pi_get_pw_limit = pd692x0_pi_get_pw_limit,

	.pi_set_pw_limit = pd692x0_pi_set_pw_limit,

	.pi_get_pw_limit_ranges = pd692x0_pi_get_pw_limit_ranges,

	.pi_get_prio = pd692x0_pi_get_prio,

	.pi_set_prio = pd692x0_pi_set_prio,

};



#define PD692X0_FW_LINE_MAX_SZ 0xff
@@ -1500,6 +1703,8 @@ static int pd692x0_i2c_probe(struct i2c_client *client) 
	priv->pcdev.ops = &pd692x0_ops;

	priv->pcdev.dev = dev;

	priv->pcdev.types = ETHTOOL_PSE_C33;

	priv->pcdev.supp_budget_eval_strategies = PSE_BUDGET_EVAL_STRAT_DYNAMIC;

	priv->pcdev.pis_prio_max = 2;

	ret = devm_pse_controller_register(dev, &priv->pcdev);

	if (ret)

		return dev_err_probe(dev, ret,