From: "Kory Maincent (Dent Project)" <[email protected]>
This patch expands PSE callbacks with newly introduced
pi_get/set_current_limit() and pi_get_voltage() callback.
The only way to set ps692x0 port power limit is by configure the power
class plus a small power supplement which maximum depends on each class.
Signed-off-by: Kory Maincent <[email protected]>
---
Change in v2:
- Use uA and uV instead of mA and mV to have more precision in the power
calculation. Need to use 64bit variables for the calculation.
- Modify the behavior in case of setting the current out of the available
ranges. Report an error now.
---
drivers/net/pse-pd/pd692x0.c | 193 ++++++++++++++++++++++++++++++++++++++++++-
1 file changed, 191 insertions(+), 2 deletions(-)
diff --git a/drivers/net/pse-pd/pd692x0.c b/drivers/net/pse-pd/pd692x0.c
index 6b7367d009d6..4425e4ba2cfb 100644
--- a/drivers/net/pse-pd/pd692x0.c
+++ b/drivers/net/pse-pd/pd692x0.c
@@ -74,6 +74,8 @@ enum {
PD692X0_MSG_GET_PORT_STATUS,
PD692X0_MSG_DOWNLOAD_CMD,
PD692X0_MSG_GET_PORT_CLASS,
+ PD692X0_MSG_GET_PORT_MEAS,
+ PD692X0_MSG_GET_PORT_PARAM,
/* add new message above here */
PD692X0_MSG_CNT
@@ -135,7 +137,7 @@ static const struct pd692x0_msg pd692x0_msg_template_list[PD692X0_MSG_CNT] = {
[PD692X0_MSG_SET_PORT_PARAM] = {
.key = PD692X0_KEY_CMD,
.sub = {0x05, 0xc0},
- .data = { 0, 0xff, 0xff, 0xff,
+ .data = { 0xf, 0xff, 0xff, 0xff,
0x4e, 0x4e, 0x4e, 0x4e},
},
[PD692X0_MSG_GET_PORT_STATUS] = {
@@ -156,6 +158,18 @@ static const struct pd692x0_msg pd692x0_msg_template_list[PD692X0_MSG_CNT] = {
.data = {0x4e, 0x4e, 0x4e, 0x4e,
0x4e, 0x4e, 0x4e, 0x4e},
},
+ [PD692X0_MSG_GET_PORT_MEAS] = {
+ .key = PD692X0_KEY_REQ,
+ .sub = {0x05, 0xc5},
+ .data = {0x4e, 0x4e, 0x4e, 0x4e,
+ 0x4e, 0x4e, 0x4e, 0x4e},
+ },
+ [PD692X0_MSG_GET_PORT_PARAM] = {
+ .key = PD692X0_KEY_REQ,
+ .sub = {0x05, 0xc0},
+ .data = {0x4e, 0x4e, 0x4e, 0x4e,
+ 0x4e, 0x4e, 0x4e, 0x4e},
+ },
};
static u8 pd692x0_build_msg(struct pd692x0_msg *msg, u8 echo)
@@ -511,6 +525,85 @@ pd692x0_get_ext_state(struct ethtool_c33_pse_ext_state_info *c33_ext_state_info,
}
}
+struct pd692x0_class_pw {
+ int class;
+ int class_cfg_value;
+ int class_pw;
+ int max_added_class_pw;
+};
+
+/* 4/2 pairs class configuration power table in compliance mode.
+ * Need to be arranged in ascending order of power support.
+ */
+static const struct pd692x0_class_pw pd692x0_class_pw_table[] = {
+ {.class = 3, .class_cfg_value = 0x3, .class_pw = 15000, .max_added_class_pw = 3100},
+ {.class = 4, .class_cfg_value = 0x2, .class_pw = 30000, .max_added_class_pw = 8000},
+ {.class = 6, .class_cfg_value = 0x1, .class_pw = 60000, .max_added_class_pw = 5000},
+ {.class = 8, .class_cfg_value = 0x0, .class_pw = 90000, .max_added_class_pw = 7500},
+ { /* sentinel */ }
+};
+
+static int pd692x0_pi_get_pw_from_table(int op_mode, int added_pw)
+{
+ const struct pd692x0_class_pw *pw_table;
+
+ pw_table = pd692x0_class_pw_table;
+ while (pw_table->class) {
+ if (pw_table->class_cfg_value == op_mode)
+ return pw_table->class_pw + added_pw * 100;
+
+ pw_table++;
+ }
+
+ return -ERANGE;
+}
+
+static int pd692x0_pi_set_pw_from_table(struct device *dev,
+ struct pd692x0_msg *msg, int pw)
+{
+ const struct pd692x0_class_pw *pw_table;
+
+ pw_table = pd692x0_class_pw_table;
+ if (pw < pw_table->class_pw) {
+ dev_err(dev,
+ "Power limit %dmW not supported. Ranges minimal available: [%d-%d]\n",
+ pw,
+ pw_table->class_pw,
+ pw_table->class_pw + pw_table->max_added_class_pw);
+ return -ERANGE;
+ }
+
+ while (pw_table->class) {
+ if (pw > (pw_table->class_pw + pw_table->max_added_class_pw)) {
+ pw_table++;
+ continue;
+ }
+
+ if (pw < pw_table->class_pw) {
+ dev_err(dev,
+ "Power limit %dmW not supported. Ranges availables: [%d-%d] or [%d-%d]\n",
+ pw,
+ (pw_table - 1)->class_pw,
+ (pw_table - 1)->class_pw + (pw_table - 1)->max_added_class_pw,
+ pw_table->class_pw,
+ pw_table->class_pw + pw_table->max_added_class_pw);
+ return -ERANGE;
+ }
+
+ msg->data[2] = pw_table->class_cfg_value;
+ msg->data[3] = (pw - pw_table->class_pw) / 100;
+ return 0;
+ }
+
+ pw_table--;
+ dev_warn(dev,
+ "Power limit %dmW not supported. Set to highest power limit %dmW\n",
+ pw, pw_table->class_pw + pw_table->max_added_class_pw);
+ msg->data[2] = pw_table->class_cfg_value;
+ msg->data[3] = pw_table->max_added_class_pw / 100;
+ return 0;
+}
+
static int pd692x0_ethtool_get_status(struct pse_controller_dev *pcdev,
unsigned long id,
struct netlink_ext_ack *extack,
@@ -549,9 +642,20 @@ static int pd692x0_ethtool_get_status(struct pse_controller_dev *pcdev,
priv->admin_state[id] = status->c33_admin_state;
pd692x0_get_ext_state(&status->c33_ext_state_info, buf.sub[0]);
-
status->c33_actual_pw = (buf.data[0] << 4 | buf.data[1]) * 100;
+ msg = pd692x0_msg_template_list[PD692X0_MSG_GET_PORT_PARAM];
+ msg.sub[2] = id;
+ memset(&buf, 0, sizeof(buf));
+ ret = pd692x0_sendrecv_msg(priv, &msg, &buf);
+ if (ret < 0)
+ return ret;
+
+ ret = pd692x0_pi_get_pw_from_table(buf.data[0], buf.data[1]);
+ if (ret < 0)
+ return ret;
+ status->c33_pw_limit = ret;
+
memset(&buf, 0, sizeof(buf));
msg = pd692x0_msg_template_list[PD692X0_MSG_GET_PORT_CLASS];
msg.sub[2] = id;
@@ -841,12 +945,97 @@ static int pd692x0_setup_pi_matrix(struct pse_controller_dev *pcdev)
return ret;
}
+static int pd692x0_pi_get_voltage(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_MEAS];
+ msg.sub[2] = id;
+ ret = pd692x0_sendrecv_msg(priv, &msg, &buf);
+ if (ret < 0)
+ return ret;
+
+ /* Convert 0.1V unit to uV */
+ return (buf.sub[0] << 8 | buf.sub[1]) * 100000;
+}
+
+static int pd692x0_pi_get_current_limit(struct pse_controller_dev *pcdev,
+ int id)
+{
+ struct pd692x0_priv *priv = to_pd692x0_priv(pcdev);
+ struct pd692x0_msg msg, buf = {0};
+ int mW, uV, uA, ret;
+ s64 tmp_64;
+
+ 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;
+
+ ret = pd692x0_pi_get_pw_from_table(buf.data[2], buf.data[3]);
+ if (ret < 0)
+ return ret;
+ mW = ret;
+
+ ret = pd692x0_pi_get_voltage(pcdev, id);
+ if (ret < 0)
+ return ret;
+ uV = ret;
+
+ tmp_64 = mW;
+ tmp_64 *= 1000000000ull;
+ /* uA = mW * 1000000000 / uV */
+ uA = DIV_ROUND_CLOSEST_ULL(tmp_64, uV);
+ return uA;
+}
+
+static int pd692x0_pi_set_current_limit(struct pse_controller_dev *pcdev,
+ int id, int max_uA)
+{
+ struct pd692x0_priv *priv = to_pd692x0_priv(pcdev);
+ struct device *dev = &priv->client->dev;
+ struct pd692x0_msg msg, buf = {0};
+ int uV, ret, mW;
+ s64 tmp_64;
+
+ ret = pd692x0_fw_unavailable(priv);
+ if (ret)
+ return ret;
+
+ ret = pd692x0_pi_get_voltage(pcdev, id);
+ if (ret < 0)
+ return ret;
+ uV = ret;
+
+ msg = pd692x0_msg_template_list[PD692X0_MSG_SET_PORT_PARAM];
+ msg.sub[2] = id;
+ tmp_64 = uV;
+ tmp_64 *= max_uA;
+ /* mW = uV * uA / 1000000000 */
+ mW = DIV_ROUND_CLOSEST_ULL(tmp_64, 1000000000);
+ ret = pd692x0_pi_set_pw_from_table(dev, &msg, mW);
+ if (ret)
+ return ret;
+
+ 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,
.pi_enable = pd692x0_pi_enable,
.pi_disable = pd692x0_pi_disable,
.pi_is_enabled = pd692x0_pi_is_enabled,
+ .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,
};
#define PD692X0_FW_LINE_MAX_SZ 0xff
--
2.34.1