From: Kory Maincent (Dent Project) <kory.maincent@xxxxxxxxxxx>
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: Kory Maincent <kory.maincent@xxxxxxxxxxx>
---
Changes in v3:
- New patch
---
drivers/net/pse-pd/pd692x0.c | 183 +++++++++++++++++++++++++++++++++++++++++++
1 file changed, 183 insertions(+)
diff --git a/drivers/net/pse-pd/pd692x0.c b/drivers/net/pse-pd/pd692x0.c
index 0af7db80b2f8..be9fc4afaf97 100644
--- a/drivers/net/pse-pd/pd692x0.c
+++ b/drivers/net/pse-pd/pd692x0.c
@@ -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)
@@ -685,6 +701,8 @@ static int pd692x0_ethtool_get_status(struct pse_controller_dev *pcdev,
if (ret < 0)
return ret;
status->c33_avail_pw_limit = ret;
+ /* PSE core priority start at 0 */
+ status->c33_prio = buf.data[2] - 1;
memset(&buf, 0, sizeof(buf));
msg = pd692x0_msg_template_list[PD692X0_MSG_GET_PORT_CLASS];
@@ -731,6 +749,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;
};
@@ -822,6 +841,8 @@ pd692x0_of_get_managers(struct pd692x0_priv *priv,
if (ret)
goto out;
+ of_node_get(node);
+ manager[manager_id].node = node;
nmanagers++;
}
@@ -834,6 +855,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);
@@ -841,6 +864,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_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,
@@ -963,6 +1110,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;
@@ -973,8 +1128,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;
}
@@ -1061,6 +1222,25 @@ static int pd692x0_pi_set_current_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,
.ethtool_get_status = pd692x0_ethtool_get_status,
@@ -1070,6 +1250,7 @@ static const struct pse_controller_ops pd692x0_ops = {
.pi_get_voltage = pd692x0_pi_get_voltage,
.pi_get_current_limit = pd692x0_pi_get_current_limit,
.pi_set_current_limit = pd692x0_pi_set_current_limit,
+ .pi_set_prio = pd692x0_pi_set_prio,
};
#define PD692X0_FW_LINE_MAX_SZ 0xff
@@ -1486,6 +1667,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.port_prio_supp_modes = ETHTOOL_PSE_PORT_PRIO_DYNAMIC;
+ priv->pcdev.pis_prio_max = 2;
ret = devm_pse_controller_register(dev, &priv->pcdev);
if (ret)
return dev_err_probe(dev, ret,
--
2.34.1
