Split out the code which sets up the upstream CPU port for PTP. This
will be required when converted to the generic Marvell PTP library.

Signed-off-by: Russell King (Oracle) <rmk+kernel@armlinux.org.uk>
---
 drivers/net/dsa/mv88e6xxx/ptp.c | 42 ++++++++++++++++++++-------------
 1 file changed, 25 insertions(+), 17 deletions(-)

diff --git a/drivers/net/dsa/mv88e6xxx/ptp.c b/drivers/net/dsa/mv88e6xxx/ptp.c
index f7603573d3a9..b60e4f02c256 100644
--- a/drivers/net/dsa/mv88e6xxx/ptp.c
+++ b/drivers/net/dsa/mv88e6xxx/ptp.c
@@ -444,6 +444,27 @@ const struct mv88e6xxx_ptp_ops mv88e6390_ptp_ops = {
 		(1 << HWTSTAMP_FILTER_PTP_V2_DELAY_REQ),
 };
 
+static int mv88e6xxx_set_ptp_cpu_port(struct mv88e6xxx_chip *chip)
+{
+	struct dsa_port *dp;
+	int upstream = 0;
+	int err;
+
+	if (!chip->info->ops->ptp_ops->set_ptp_cpu_port)
+		return 0;
+
+	dsa_switch_for_each_user_port(dp, chip->ds) {
+		upstream = dsa_upstream_port(chip->ds, dp->index);
+		break;
+	}
+
+	err = chip->info->ops->ptp_ops->set_ptp_cpu_port(chip, upstream);
+	if (err)
+		dev_err(chip->dev, "Failed to set PTP CPU destination port!\n");
+
+	return err;
+}
+
 static u64 mv88e6xxx_ptp_clock_read(struct cyclecounter *cc)
 {
 	struct mv88e6xxx_chip *chip = cc_to_chip(cc);
@@ -473,7 +494,7 @@ static void mv88e6xxx_ptp_overflow_check(struct work_struct *work)
 int mv88e6xxx_ptp_setup(struct mv88e6xxx_chip *chip)
 {
 	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
-	int i;
+	int err, i;
 
 	/* Set up the cycle counter */
 	chip->cc_coeffs = mv88e6xxx_cc_coeff_get(chip);
@@ -524,22 +545,9 @@ int mv88e6xxx_ptp_setup(struct mv88e6xxx_chip *chip)
 						     PTP_FALLING_EDGE |
 						     PTP_STRICT_FLAGS;
 
-	if (ptp_ops->set_ptp_cpu_port) {
-		struct dsa_port *dp;
-		int upstream = 0;
-		int err;
-
-		dsa_switch_for_each_user_port(dp, chip->ds) {
-			upstream = dsa_upstream_port(chip->ds, dp->index);
-			break;
-		}
-
-		err = ptp_ops->set_ptp_cpu_port(chip, upstream);
-		if (err) {
-			dev_err(chip->dev, "Failed to set PTP CPU destination port!\n");
-			return err;
-		}
-	}
+	err = mv88e6xxx_set_ptp_cpu_port(chip);
+	if (err)
+		return err;
 
 	chip->ptp_clock = ptp_clock_register(&chip->ptp_clock_info, chip->dev);
 	if (IS_ERR(chip->ptp_clock))
-- 
2.47.3