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: Kory Maincent (Dent Project) <kory.maincent@bootlin.com>
Reviewed-by: Oleksij Rempel <o.rempel@pengutronix.de>
Link: https://patch.msgid.link/20250617-feature_poe_port_prio-v14-9-78a1a645e2ee@bootlin.com
Signed-off-by: Jakub Kicinski <kuba@kernel.org>

diff --git a/drivers/net/pse-pd/pd692x0.c b/drivers/net/pse-pd/pd692x0.c
index 7d60a714ca536175b6c2befa6552f9d746f72d37..a4766c18f3338a83d8b87127aa36b4361f816ff0 100644 (file)
--- 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)
@@ -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 @@ out:
                        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 @@ out:
        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,