MLK-18744 staging: typec: tcpm: qos handling fix

Commit 661b7ec2359e ("MLK-17921-2 staging: typec: tcpm: add qos for
PD transfer") introduce qos for tcpm transfer between typec controller
and CPU, but the request and remove are not balanced well, the initial
request should be moved to tcpm_port_register as tcpm_init will be called
for every hard reset, and also move qos hold&release to where port state
changes.

Fixes: 661b7ec2359e ("MLK-17921-2 staging: typec: tcpm: add qos for PD transfer")
Acked-by: Peter Chen <peter.chen@nxp.com>
Signed-off-by: Li Jun <jun.li@nxp.com>

1 files changed, 32 insertions, 33 deletions

diff --git a/drivers/staging/typec/tcpm.c b/drivers/staging/typec/tcpm.c
index 14d6696708e5..7e167bdaf7da 100644
--- a/drivers/staging/typec/tcpm.c
+++ b/drivers/staging/typec/tcpm.c
@@ -819,6 +819,33 @@ static int tcpm_pd_send_sink_caps(struct tcpm_port *port)
 	return tcpm_pd_transmit(port, TCPC_TX_SOP, &msg);
 }
 
+static void tcpm_qos_handling(struct tcpm_port *port)
+{
+	enum tcpm_state idle_state;
+
+	if (port->typec_caps.type == TYPEC_PORT_UFP)
+		idle_state = SNK_UNATTACHED;
+	else if (port->typec_caps.type == TYPEC_PORT_DFP)
+		idle_state = SNK_UNATTACHED;
+	else if (port->typec_caps.type == TYPEC_PORT_DRP)
+		idle_state = DRP_TOGGLING;
+	else
+		return;
+
+	if ((port->prev_state == SNK_READY || port->prev_state == SRC_READY ||
+	     port->prev_state == idle_state)) {
+		/* Hold high bus before leave those states */
+		request_bus_freq(BUS_FREQ_HIGH);
+		pm_qos_add_request(&port->pm_qos_req,
+				   PM_QOS_CPU_DMA_LATENCY, 0);
+	} else if ((port->state == SNK_READY || port->state == SRC_READY ||
+		    port->state == idle_state)) {
+		/* Release high bus after enter those states */
+		pm_qos_remove_request(&port->pm_qos_req);
+		release_bus_freq(BUS_FREQ_HIGH);
+	}
+}
+
 static void tcpm_set_state(struct tcpm_port *port, enum tcpm_state state,
 			   unsigned int delay_ms)
 {
@@ -837,6 +864,7 @@ static void tcpm_set_state(struct tcpm_port *port, enum tcpm_state state,
 		port->delayed_state = INVALID_STATE;
 		port->prev_state = port->state;
 		port->state = state;
+		tcpm_qos_handling(port);
 		/*
 		 * Don't re-queue the state machine work item if we're currently
 		 * in the state machine and we're immediately changing states.
@@ -2761,33 +2789,6 @@ static void run_state_machine(struct tcpm_port *port)
 	}
 }
 
-static void tcpm_qos_active(struct tcpm_port *port, bool on)
-{
-	enum tcpm_state idle_state;
-
-	if (port->typec_caps.type == TYPEC_PORT_UFP)
-		idle_state = SNK_UNATTACHED;
-	else if (port->typec_caps.type == TYPEC_PORT_DFP)
-		idle_state = SNK_UNATTACHED;
-	else if (port->typec_caps.type == TYPEC_PORT_DRP)
-		idle_state = DRP_TOGGLING;
-	else
-		return;
-
-	if ((port->prev_state == SNK_READY || port->prev_state == SRC_READY ||
-	     port->prev_state == idle_state) && on) {
-		/* Hold high bus before leave those states */
-		request_bus_freq(BUS_FREQ_HIGH);
-		pm_qos_add_request(&port->pm_qos_req,
-				   PM_QOS_CPU_DMA_LATENCY, 0);
-	} else if ((port->state == SNK_READY || port->state == SRC_READY ||
-		    port->state == idle_state) && !on) {
-		/* Release high bus after enter those states */
-		pm_qos_remove_request(&port->pm_qos_req);
-		release_bus_freq(BUS_FREQ_HIGH);
-	}
-}
-
 static void tcpm_state_machine_work(struct work_struct *work)
 {
 	struct tcpm_port *port = container_of(work, struct tcpm_port,
@@ -2808,9 +2809,9 @@ static void tcpm_state_machine_work(struct work_struct *work)
 		port->prev_state = port->state;
 		port->state = port->delayed_state;
 		port->delayed_state = INVALID_STATE;
+		tcpm_qos_handling(port);
 	}
 
-	tcpm_qos_active(port, true);
 	/*
 	 * Continue running as long as we have (non-delayed) state changes
 	 * to make.
@@ -2821,8 +2822,6 @@ static void tcpm_state_machine_work(struct work_struct *work)
 		if (port->queued_message)
 			tcpm_send_queued_message(port);
 	} while (port->state != prev_state && !port->delayed_state);
-
-	tcpm_qos_active(port, false);
 done:
 	port->state_machine_running = false;
 	mutex_unlock(&port->lock);
@@ -3423,9 +3422,6 @@ static void tcpm_init(struct tcpm_port *port)
 
 	tcpm_reset_port(port);
 
-	request_bus_freq(BUS_FREQ_HIGH);
-	pm_qos_add_request(&port->pm_qos_req, PM_QOS_CPU_DMA_LATENCY, 0);
-
 	tcpm_set_state(port, tcpm_default_state(port), 0);
 
 	_tcpm_cc_change(port, cc1, cc2);
@@ -3609,6 +3605,9 @@ struct tcpm_port *tcpm_register_port(struct device *dev, struct tcpc_dev *tcpc)
 
 	tcpm_debugfs_init(port);
 	mutex_lock(&port->lock);
+	request_bus_freq(BUS_FREQ_HIGH);
+	pm_qos_add_request(&port->pm_qos_req, PM_QOS_CPU_DMA_LATENCY, 0);
+
 	tcpm_init(port);
 	mutex_unlock(&port->lock);