[PATCH net-next 1/4] net: pse-pd: realtek-pse-mcu: ensure MCU does dynamic power management

[Jonas Jelonek <jonas@jonasjelonek.de>]

From: Russell Senior <russell@personaltelco.net>

The MCU may have any power management mode configuration by default
but the driver should ensure that the MCU performs dynamic power
management with priorities as advertised. On certain devices, the
default MCU configuration ships with another default, possibly
preventing any power delivery.

Select the mode before deasserting the hardware port gate, so a failure
to establish the management policy does not ungate the ports.

Signed-off-by: Russell Senior <russell@personaltelco.net>
[jonas@jonasjelonek.de: adjust commit message, macro naming and comment;
 change error handling and select the mode before ungating ports]
Signed-off-by: Jonas Jelonek <jonas@jonasjelonek.de>
---
 drivers/net/pse-pd/realtek-pse-mcu-core.c | 32 +++++++++++++++++++++++
 1 file changed, 32 insertions(+)

diff --git a/drivers/net/pse-pd/realtek-pse-mcu-core.c b/drivers/net/pse-pd/realtek-pse-mcu-core.c
index c70b09ca2012..f3fc67fe29d3 100644
--- a/drivers/net/pse-pd/realtek-pse-mcu-core.c
+++ b/drivers/net/pse-pd/realtek-pse-mcu-core.c
@@ -68,10 +68,14 @@
 /* Nominal PSE rail; 802.3at/bt operating range. */
 #define RTPSE_MCU_PSE_VOLTAGE_UV		54000000
 
+/* RTPSE_MCU_CMD_SET_POWER_MANAGEMENT_MODE values */
+#define RTPSE_MCU_POWER_MGMT_DYNAMIC_PRIO	0x02	/* dynamic, with port priority */
+
 enum rtpse_mcu_cmd {
 	RTPSE_MCU_CMD_SET_GLOBAL_STATE,
 	RTPSE_MCU_CMD_GET_SYSTEM_INFO,
 	RTPSE_MCU_CMD_GET_EXT_CONFIG,
+	RTPSE_MCU_CMD_SET_POWER_MANAGEMENT_MODE,
 
 	RTPSE_MCU_CMD_PORT_ENABLE,
 	RTPSE_MCU_CMD_PORT_SET_POWER_LIMIT_TYPE,
@@ -369,6 +373,26 @@ static int rtpse_mcu_set_global_state(struct rtpse_mcu_ctrl *pse, bool enable)
 	return (resp.payload[0] == 0x0) ? 0 : -EIO;
 }
 
+static int rtpse_mcu_set_power_management_mode(struct rtpse_mcu_ctrl *pse, u8 value)
+{
+	struct rtpse_mcu_msg req, resp;
+	const struct rtpse_mcu_opcode *opc;
+	int ret;
+
+	opc = &pse->dialect->opcode[RTPSE_MCU_CMD_SET_POWER_MANAGEMENT_MODE];
+	if (!opc->valid)
+		return -EOPNOTSUPP;
+
+	rtpse_mcu_msg_init(&req, opc->op);
+	req.payload[0] = value;
+
+	ret = rtpse_mcu_do_xfer(pse, &req, &resp);
+	if (ret)
+		return ret;
+
+	return (resp.payload[0] == 0x0) ? 0 : -EIO;
+}
+
 /* Port operations */
 
 static int rtpse_mcu_port_get_status(struct rtpse_mcu_ctrl *pse, unsigned int port,
@@ -826,6 +850,12 @@ int rtpse_mcu_register(struct rtpse_mcu_ctrl *pse)
 	if (ret)
 		return ret;
 
+	/* ensure the MCU does dynamic power management with priorities */
+	ret = rtpse_mcu_set_power_management_mode(pse, RTPSE_MCU_POWER_MGMT_DYNAMIC_PRIO);
+	if (ret)
+		return dev_err_probe(pse->dev, ret,
+				     "failed to set power management mode\n");
+
 	/*
 	 * Some boards gate all ports through a hardware line; deassert it only
 	 * after the MCU is confirmed, so a discover failure never ungates the
@@ -944,6 +974,7 @@ static const struct rtpse_mcu_dialect rtpse_mcu_dialect_gen2 = {
 		[RTPSE_MCU_CMD_SET_GLOBAL_STATE]	= RTPSE_MCU_OP(0x00),
 		[RTPSE_MCU_CMD_GET_SYSTEM_INFO]		= RTPSE_MCU_OP(0x40),
 		[RTPSE_MCU_CMD_GET_EXT_CONFIG]		= RTPSE_MCU_OP(0x4a),
+		[RTPSE_MCU_CMD_SET_POWER_MANAGEMENT_MODE] = RTPSE_MCU_OP(0x10),
 
 		[RTPSE_MCU_CMD_PORT_ENABLE]		= RTPSE_MCU_OP(0x01),
 		[RTPSE_MCU_CMD_PORT_SET_POWER_LIMIT_TYPE] = RTPSE_MCU_OP(0x12),
@@ -964,6 +995,7 @@ static const struct rtpse_mcu_dialect rtpse_mcu_dialect_gen1 = {
 	.opcode = {
 		[RTPSE_MCU_CMD_GET_SYSTEM_INFO]		= RTPSE_MCU_OP(0x20),
 		[RTPSE_MCU_CMD_GET_EXT_CONFIG]		= RTPSE_MCU_OP(0x2b),
+		[RTPSE_MCU_CMD_SET_POWER_MANAGEMENT_MODE] = RTPSE_MCU_OP(0x17),
 
 		[RTPSE_MCU_CMD_PORT_ENABLE]		= RTPSE_MCU_OP(0x00),
 		[RTPSE_MCU_CMD_PORT_SET_POWER_LIMIT_TYPE] = RTPSE_MCU_OP(0x15),
-- 
2.53.0