net: mscc: ocelot: perform error cleanup in ocelot_hwstamp_set()

An unsupported RX filter will leave the port with TX timestamping still
applied as per the new request, rather than the old setting. When
parsing the tx_type, don't apply it just yet, but delay that until after
we've parsed the rx_filter as well (and potentially returned -ERANGE for
that).

Similarly, copy_to_user() may fail, which is a rare occurrence, but
should still be treated by unwinding what was done.

Fixes: 96ca08c05838 ("net: mscc: ocelot: set up traps for PTP packets")
Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
Link: https://patch.msgid.link/20241205145519.1236778-6-vladimir.oltean@nxp.com
Signed-off-by: Jakub Kicinski <kuba@kernel.org>

diff --git a/drivers/net/ethernet/mscc/ocelot_ptp.c b/drivers/net/ethernet/mscc/ocelot_ptp.c
index 7eb01d1e1ecd650a7d06cb1a9c7859fddde31425..808ce8e68d3937a2f19a34ae6a7086051cfcdabe 100644 (file)
--- a/drivers/net/ethernet/mscc/ocelot_ptp.c
+++ b/drivers/net/ethernet/mscc/ocelot_ptp.c
@@ -497,6 +497,28 @@ static int ocelot_traps_to_ptp_rx_filter(unsigned int proto)
        return HWTSTAMP_FILTER_NONE;
 }
 
+static int ocelot_ptp_tx_type_to_cmd(int tx_type, int *ptp_cmd)
+{
+       switch (tx_type) {
+       case HWTSTAMP_TX_ON:
+               *ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
+               break;
+       case HWTSTAMP_TX_ONESTEP_SYNC:
+               /* IFH_REW_OP_ONE_STEP_PTP updates the correctionField,
+                * what we need to update is the originTimestamp.
+                */
+               *ptp_cmd = IFH_REW_OP_ORIGIN_PTP;
+               break;
+       case HWTSTAMP_TX_OFF:
+               *ptp_cmd = 0;
+               break;
+       default:
+               return -ERANGE;
+       }
+
+       return 0;
+}
+
 int ocelot_hwstamp_get(struct ocelot *ocelot, int port, struct ifreq *ifr)
 {
        struct ocelot_port *ocelot_port = ocelot->ports[port];
@@ -523,30 +545,19 @@ EXPORT_SYMBOL(ocelot_hwstamp_get);
 int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr)
 {
        struct ocelot_port *ocelot_port = ocelot->ports[port];
+       int ptp_cmd, old_ptp_cmd = ocelot_port->ptp_cmd;
        bool l2 = false, l4 = false;
        struct hwtstamp_config cfg;
+       bool old_l2, old_l4;
        int err;
 
        if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
                return -EFAULT;
 
        /* Tx type sanity check */
-       switch (cfg.tx_type) {
-       case HWTSTAMP_TX_ON:
-               ocelot_port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
-               break;
-       case HWTSTAMP_TX_ONESTEP_SYNC:
-               /* IFH_REW_OP_ONE_STEP_PTP updates the correctional field, we
-                * need to update the origin time.
-                */
-               ocelot_port->ptp_cmd = IFH_REW_OP_ORIGIN_PTP;
-               break;
-       case HWTSTAMP_TX_OFF:
-               ocelot_port->ptp_cmd = 0;
-               break;
-       default:
-               return -ERANGE;
-       }
+       err = ocelot_ptp_tx_type_to_cmd(cfg.tx_type, &ptp_cmd);
+       if (err)
+               return err;
 
        switch (cfg.rx_filter) {
        case HWTSTAMP_FILTER_NONE:
@@ -571,13 +582,27 @@ int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr)
                return -ERANGE;
        }
 
+       old_l2 = ocelot_port->trap_proto & OCELOT_PROTO_PTP_L2;
+       old_l4 = ocelot_port->trap_proto & OCELOT_PROTO_PTP_L4;
+
        err = ocelot_setup_ptp_traps(ocelot, port, l2, l4);
        if (err)
                return err;
 
+       ocelot_port->ptp_cmd = ptp_cmd;
+
        cfg.rx_filter = ocelot_traps_to_ptp_rx_filter(ocelot_port->trap_proto);
 
-       return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
+       if (copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg))) {
+               err = -EFAULT;
+               goto out_restore_ptp_traps;
+       }
+
+       return 0;
+out_restore_ptp_traps:
+       ocelot_setup_ptp_traps(ocelot, port, old_l2, old_l4);
+       ocelot_port->ptp_cmd = old_ptp_cmd;
+       return err;
 }
 EXPORT_SYMBOL(ocelot_hwstamp_set);