Orange Pi5 kernel

^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   1) // SPDX-License-Identifier: GPL-2.0-or-later
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   2) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   3)  * Marvell 88E6xxx Switch hardware timestamping support
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   4)  *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   5)  * Copyright (c) 2008 Marvell Semiconductor
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   6)  *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   7)  * Copyright (c) 2017 National Instruments
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   8)  *      Erik Hons <erik.hons@ni.com>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300   9)  *      Brandon Streiff <brandon.streiff@ni.com>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  10)  *      Dane Wagner <dane.wagner@ni.com>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  11)  */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  12) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  13) #include "chip.h"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  14) #include "global2.h"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  15) #include "hwtstamp.h"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  16) #include "ptp.h"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  17) #include <linux/ptp_classify.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  18) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  19) #define SKB_PTP_TYPE(__skb) (*(unsigned int *)((__skb)->cb))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  20) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  21) static int mv88e6xxx_port_ptp_read(struct mv88e6xxx_chip *chip, int port,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  22) 				   int addr, u16 *data, int len)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  23) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  24) 	if (!chip->info->ops->avb_ops->port_ptp_read)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  25) 		return -EOPNOTSUPP;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  26) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  27) 	return chip->info->ops->avb_ops->port_ptp_read(chip, port, addr,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  28) 						       data, len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  29) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  30) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  31) static int mv88e6xxx_port_ptp_write(struct mv88e6xxx_chip *chip, int port,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  32) 				    int addr, u16 data)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  33) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  34) 	if (!chip->info->ops->avb_ops->port_ptp_write)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  35) 		return -EOPNOTSUPP;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  36) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  37) 	return chip->info->ops->avb_ops->port_ptp_write(chip, port, addr,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  38) 							data);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  39) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  40) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  41) static int mv88e6xxx_ptp_write(struct mv88e6xxx_chip *chip, int addr,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  42) 			       u16 data)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  43) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  44) 	if (!chip->info->ops->avb_ops->ptp_write)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  45) 		return -EOPNOTSUPP;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  46) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  47) 	return chip->info->ops->avb_ops->ptp_write(chip, addr, data);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  48) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  49) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  50) static int mv88e6xxx_ptp_read(struct mv88e6xxx_chip *chip, int addr,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  51) 			      u16 *data)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  52) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  53) 	if (!chip->info->ops->avb_ops->ptp_read)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  54) 		return -EOPNOTSUPP;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  55) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  56) 	return chip->info->ops->avb_ops->ptp_read(chip, addr, data, 1);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  57) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  58) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  59) /* TX_TSTAMP_TIMEOUT: This limits the time spent polling for a TX
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  60)  * timestamp. When working properly, hardware will produce a timestamp
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  61)  * within 1ms. Software may enounter delays due to MDIO contention, so
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  62)  * the timeout is set accordingly.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  63)  */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  64) #define TX_TSTAMP_TIMEOUT	msecs_to_jiffies(40)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  65) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  66) int mv88e6xxx_get_ts_info(struct dsa_switch *ds, int port,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  67) 			  struct ethtool_ts_info *info)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  68) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  69) 	const struct mv88e6xxx_ptp_ops *ptp_ops;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  70) 	struct mv88e6xxx_chip *chip;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  71) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  72) 	chip = ds->priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  73) 	ptp_ops = chip->info->ops->ptp_ops;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  74) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  75) 	if (!chip->info->ptp_support)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  76) 		return -EOPNOTSUPP;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  77) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  78) 	info->so_timestamping =
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  79) 		SOF_TIMESTAMPING_TX_HARDWARE |
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  80) 		SOF_TIMESTAMPING_RX_HARDWARE |
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  81) 		SOF_TIMESTAMPING_RAW_HARDWARE;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  82) 	info->phc_index = ptp_clock_index(chip->ptp_clock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  83) 	info->tx_types =
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  84) 		(1 << HWTSTAMP_TX_OFF) |
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  85) 		(1 << HWTSTAMP_TX_ON);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  86) 	info->rx_filters = ptp_ops->rx_filters;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  87) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  88) 	return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  89) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  90) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  91) static int mv88e6xxx_set_hwtstamp_config(struct mv88e6xxx_chip *chip, int port,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  92) 					 struct hwtstamp_config *config)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  93) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  94) 	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  95) 	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  96) 	bool tstamp_enable = false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  97) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  98) 	/* Prevent the TX/RX paths from trying to interact with the
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  99) 	 * timestamp hardware while we reconfigure it.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 100) 	 */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 101) 	clear_bit_unlock(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 102) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 103) 	/* reserved for future extensions */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 104) 	if (config->flags)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 105) 		return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 106) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 107) 	switch (config->tx_type) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 108) 	case HWTSTAMP_TX_OFF:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 109) 		tstamp_enable = false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 110) 		break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 111) 	case HWTSTAMP_TX_ON:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 112) 		tstamp_enable = true;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 113) 		break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 114) 	default:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 115) 		return -ERANGE;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 116) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 117) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 118) 	/* The switch supports timestamping both L2 and L4; one cannot be
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 119) 	 * disabled independently of the other.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 120) 	 */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 121) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 122) 	if (!(BIT(config->rx_filter) & ptp_ops->rx_filters)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 123) 		config->rx_filter = HWTSTAMP_FILTER_NONE;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 124) 		dev_dbg(chip->dev, "Unsupported rx_filter %d\n",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 125) 			config->rx_filter);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 126) 		return -ERANGE;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 127) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 128) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 129) 	switch (config->rx_filter) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 130) 	case HWTSTAMP_FILTER_NONE:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 131) 		tstamp_enable = false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 132) 		break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 133) 	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 134) 	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 135) 	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 136) 	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 137) 	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 138) 	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 139) 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 140) 	case HWTSTAMP_FILTER_PTP_V2_SYNC:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 141) 	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 142) 		config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 143) 		break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 144) 	case HWTSTAMP_FILTER_ALL:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 145) 	default:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 146) 		config->rx_filter = HWTSTAMP_FILTER_NONE;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 147) 		return -ERANGE;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 148) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 149) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 150) 	mv88e6xxx_reg_lock(chip);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 151) 	if (tstamp_enable) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 152) 		chip->enable_count += 1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 153) 		if (chip->enable_count == 1 && ptp_ops->global_enable)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 154) 			ptp_ops->global_enable(chip);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 155) 		if (ptp_ops->port_enable)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 156) 			ptp_ops->port_enable(chip, port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 157) 	} else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 158) 		if (ptp_ops->port_disable)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 159) 			ptp_ops->port_disable(chip, port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 160) 		chip->enable_count -= 1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 161) 		if (chip->enable_count == 0 && ptp_ops->global_disable)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 162) 			ptp_ops->global_disable(chip);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 163) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 164) 	mv88e6xxx_reg_unlock(chip);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 165) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 166) 	/* Once hardware has been configured, enable timestamp checks
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 167) 	 * in the RX/TX paths.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 168) 	 */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 169) 	if (tstamp_enable)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 170) 		set_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 171) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 172) 	return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 173) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 174) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 175) int mv88e6xxx_port_hwtstamp_set(struct dsa_switch *ds, int port,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 176) 				struct ifreq *ifr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 177) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 178) 	struct mv88e6xxx_chip *chip = ds->priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 179) 	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 180) 	struct hwtstamp_config config;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 181) 	int err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 182) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 183) 	if (!chip->info->ptp_support)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 184) 		return -EOPNOTSUPP;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 185) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 186) 	if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 187) 		return -EFAULT;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 188) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 189) 	err = mv88e6xxx_set_hwtstamp_config(chip, port, &config);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 190) 	if (err)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 191) 		return err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 192) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 193) 	/* Save the chosen configuration to be returned later. */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 194) 	memcpy(&ps->tstamp_config, &config, sizeof(config));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 195) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 196) 	return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 197) 		-EFAULT : 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 198) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 199) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 200) int mv88e6xxx_port_hwtstamp_get(struct dsa_switch *ds, int port,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 201) 				struct ifreq *ifr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 202) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 203) 	struct mv88e6xxx_chip *chip = ds->priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 204) 	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 205) 	struct hwtstamp_config *config = &ps->tstamp_config;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 206) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 207) 	if (!chip->info->ptp_support)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 208) 		return -EOPNOTSUPP;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 209) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 210) 	return copy_to_user(ifr->ifr_data, config, sizeof(*config)) ?
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 211) 		-EFAULT : 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 212) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 213) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 214) /* Returns a pointer to the PTP header if the caller should time stamp,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 215)  * or NULL if the caller should not.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 216)  */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 217) static struct ptp_header *mv88e6xxx_should_tstamp(struct mv88e6xxx_chip *chip,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 218) 						  int port, struct sk_buff *skb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 219) 						  unsigned int type)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 220) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 221) 	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 222) 	struct ptp_header *hdr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 223) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 224) 	if (!chip->info->ptp_support)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 225) 		return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 226) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 227) 	hdr = ptp_parse_header(skb, type);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 228) 	if (!hdr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 229) 		return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 230) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 231) 	if (!test_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 232) 		return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 233) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 234) 	return hdr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 235) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 236) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 237) static int mv88e6xxx_ts_valid(u16 status)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 238) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 239) 	if (!(status & MV88E6XXX_PTP_TS_VALID))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 240) 		return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 241) 	if (status & MV88E6XXX_PTP_TS_STATUS_MASK)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 242) 		return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 243) 	return 1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 244) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 245) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 246) static int seq_match(struct sk_buff *skb, u16 ts_seqid)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 247) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 248) 	unsigned int type = SKB_PTP_TYPE(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 249) 	struct ptp_header *hdr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 250) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 251) 	hdr = ptp_parse_header(skb, type);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 252) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 253) 	return ts_seqid == ntohs(hdr->sequence_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 254) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 255) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 256) static void mv88e6xxx_get_rxts(struct mv88e6xxx_chip *chip,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 257) 			       struct mv88e6xxx_port_hwtstamp *ps,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 258) 			       struct sk_buff *skb, u16 reg,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 259) 			       struct sk_buff_head *rxq)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 260) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 261) 	u16 buf[4] = { 0 }, status, seq_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 262) 	struct skb_shared_hwtstamps *shwt;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 263) 	struct sk_buff_head received;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 264) 	u64 ns, timelo, timehi;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 265) 	unsigned long flags;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 266) 	int err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 267) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 268) 	/* The latched timestamp belongs to one of the received frames. */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 269) 	__skb_queue_head_init(&received);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 270) 	spin_lock_irqsave(&rxq->lock, flags);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 271) 	skb_queue_splice_tail_init(rxq, &received);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 272) 	spin_unlock_irqrestore(&rxq->lock, flags);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 273) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 274) 	mv88e6xxx_reg_lock(chip);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 275) 	err = mv88e6xxx_port_ptp_read(chip, ps->port_id,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 276) 				      reg, buf, ARRAY_SIZE(buf));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 277) 	mv88e6xxx_reg_unlock(chip);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 278) 	if (err)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 279) 		pr_err("failed to get the receive time stamp\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 280) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 281) 	status = buf[0];
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 282) 	timelo = buf[1];
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 283) 	timehi = buf[2];
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 284) 	seq_id = buf[3];
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 285) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 286) 	if (status & MV88E6XXX_PTP_TS_VALID) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 287) 		mv88e6xxx_reg_lock(chip);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 288) 		err = mv88e6xxx_port_ptp_write(chip, ps->port_id, reg, 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 289) 		mv88e6xxx_reg_unlock(chip);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 290) 		if (err)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 291) 			pr_err("failed to clear the receive status\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 292) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 293) 	/* Since the device can only handle one time stamp at a time,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 294) 	 * we purge any extra frames from the queue.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 295) 	 */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 296) 	for ( ; skb; skb = __skb_dequeue(&received)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 297) 		if (mv88e6xxx_ts_valid(status) && seq_match(skb, seq_id)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 298) 			ns = timehi << 16 | timelo;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 299) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 300) 			mv88e6xxx_reg_lock(chip);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 301) 			ns = timecounter_cyc2time(&chip->tstamp_tc, ns);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 302) 			mv88e6xxx_reg_unlock(chip);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 303) 			shwt = skb_hwtstamps(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 304) 			memset(shwt, 0, sizeof(*shwt));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 305) 			shwt->hwtstamp = ns_to_ktime(ns);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 306) 			status &= ~MV88E6XXX_PTP_TS_VALID;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 307) 		}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 308) 		netif_rx_ni(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 309) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 310) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 311) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 312) static void mv88e6xxx_rxtstamp_work(struct mv88e6xxx_chip *chip,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 313) 				    struct mv88e6xxx_port_hwtstamp *ps)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 314) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 315) 	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 316) 	struct sk_buff *skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 317) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 318) 	skb = skb_dequeue(&ps->rx_queue);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 319) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 320) 	if (skb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 321) 		mv88e6xxx_get_rxts(chip, ps, skb, ptp_ops->arr0_sts_reg,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 322) 				   &ps->rx_queue);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 323) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 324) 	skb = skb_dequeue(&ps->rx_queue2);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 325) 	if (skb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 326) 		mv88e6xxx_get_rxts(chip, ps, skb, ptp_ops->arr1_sts_reg,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 327) 				   &ps->rx_queue2);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 328) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 329) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 330) static int is_pdelay_resp(const struct ptp_header *hdr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 331) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 332) 	return (hdr->tsmt & 0xf) == 3;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 333) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 334) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 335) bool mv88e6xxx_port_rxtstamp(struct dsa_switch *ds, int port,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 336) 			     struct sk_buff *skb, unsigned int type)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 337) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 338) 	struct mv88e6xxx_port_hwtstamp *ps;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 339) 	struct mv88e6xxx_chip *chip;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 340) 	struct ptp_header *hdr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 341) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 342) 	chip = ds->priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 343) 	ps = &chip->port_hwtstamp[port];
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 344) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 345) 	if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 346) 		return false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 347) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 348) 	hdr = mv88e6xxx_should_tstamp(chip, port, skb, type);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 349) 	if (!hdr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 350) 		return false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 351) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 352) 	SKB_PTP_TYPE(skb) = type;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 353) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 354) 	if (is_pdelay_resp(hdr))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 355) 		skb_queue_tail(&ps->rx_queue2, skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 356) 	else
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 357) 		skb_queue_tail(&ps->rx_queue, skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 358) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 359) 	ptp_schedule_worker(chip->ptp_clock, 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 360) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 361) 	return true;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 362) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 363) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 364) static int mv88e6xxx_txtstamp_work(struct mv88e6xxx_chip *chip,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 365) 				   struct mv88e6xxx_port_hwtstamp *ps)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 366) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 367) 	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 368) 	struct skb_shared_hwtstamps shhwtstamps;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 369) 	u16 departure_block[4], status;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 370) 	struct sk_buff *tmp_skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 371) 	u32 time_raw;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 372) 	int err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 373) 	u64 ns;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 374) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 375) 	if (!ps->tx_skb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 376) 		return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 377) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 378) 	mv88e6xxx_reg_lock(chip);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 379) 	err = mv88e6xxx_port_ptp_read(chip, ps->port_id,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 380) 				      ptp_ops->dep_sts_reg,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 381) 				      departure_block,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 382) 				      ARRAY_SIZE(departure_block));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 383) 	mv88e6xxx_reg_unlock(chip);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 384) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 385) 	if (err)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 386) 		goto free_and_clear_skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 387) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 388) 	if (!(departure_block[0] & MV88E6XXX_PTP_TS_VALID)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 389) 		if (time_is_before_jiffies(ps->tx_tstamp_start +
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 390) 					   TX_TSTAMP_TIMEOUT)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 391) 			dev_warn(chip->dev, "p%d: clearing tx timestamp hang\n",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 392) 				 ps->port_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 393) 			goto free_and_clear_skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 394) 		}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 395) 		/* The timestamp should be available quickly, while getting it
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 396) 		 * is high priority and time bounded to only 10ms. A poll is
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 397) 		 * warranted so restart the work.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 398) 		 */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 399) 		return 1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 400) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 401) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 402) 	/* We have the timestamp; go ahead and clear valid now */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 403) 	mv88e6xxx_reg_lock(chip);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 404) 	mv88e6xxx_port_ptp_write(chip, ps->port_id, ptp_ops->dep_sts_reg, 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 405) 	mv88e6xxx_reg_unlock(chip);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 406) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 407) 	status = departure_block[0] & MV88E6XXX_PTP_TS_STATUS_MASK;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 408) 	if (status != MV88E6XXX_PTP_TS_STATUS_NORMAL) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 409) 		dev_warn(chip->dev, "p%d: tx timestamp overrun\n", ps->port_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 410) 		goto free_and_clear_skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 411) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 412) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 413) 	if (departure_block[3] != ps->tx_seq_id) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 414) 		dev_warn(chip->dev, "p%d: unexpected seq. id\n", ps->port_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 415) 		goto free_and_clear_skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 416) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 417) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 418) 	memset(&shhwtstamps, 0, sizeof(shhwtstamps));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 419) 	time_raw = ((u32)departure_block[2] << 16) | departure_block[1];
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 420) 	mv88e6xxx_reg_lock(chip);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 421) 	ns = timecounter_cyc2time(&chip->tstamp_tc, time_raw);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 422) 	mv88e6xxx_reg_unlock(chip);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 423) 	shhwtstamps.hwtstamp = ns_to_ktime(ns);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 424) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 425) 	dev_dbg(chip->dev,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 426) 		"p%d: txtstamp %llx status 0x%04x skb ID 0x%04x hw ID 0x%04x\n",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 427) 		ps->port_id, ktime_to_ns(shhwtstamps.hwtstamp),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 428) 		departure_block[0], ps->tx_seq_id, departure_block[3]);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 429) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 430) 	/* skb_complete_tx_timestamp() will free up the client to make
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 431) 	 * another timestamp-able transmit. We have to be ready for it
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 432) 	 * -- by clearing the ps->tx_skb "flag" -- beforehand.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 433) 	 */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 434) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 435) 	tmp_skb = ps->tx_skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 436) 	ps->tx_skb = NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 437) 	clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 438) 	skb_complete_tx_timestamp(tmp_skb, &shhwtstamps);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 439) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 440) 	return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 441) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 442) free_and_clear_skb:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 443) 	dev_kfree_skb_any(ps->tx_skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 444) 	ps->tx_skb = NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 445) 	clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 446) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 447) 	return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 448) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 449) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 450) long mv88e6xxx_hwtstamp_work(struct ptp_clock_info *ptp)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 451) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 452) 	struct mv88e6xxx_chip *chip = ptp_to_chip(ptp);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 453) 	struct dsa_switch *ds = chip->ds;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 454) 	struct mv88e6xxx_port_hwtstamp *ps;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 455) 	int i, restart = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 456) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 457) 	for (i = 0; i < ds->num_ports; i++) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 458) 		if (!dsa_is_user_port(ds, i))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 459) 			continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 460) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 461) 		ps = &chip->port_hwtstamp[i];
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 462) 		if (test_bit(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 463) 			restart |= mv88e6xxx_txtstamp_work(chip, ps);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 464) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 465) 		mv88e6xxx_rxtstamp_work(chip, ps);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 466) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 467) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 468) 	return restart ? 1 : -1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 469) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 470) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 471) bool mv88e6xxx_port_txtstamp(struct dsa_switch *ds, int port,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 472) 			     struct sk_buff *clone, unsigned int type)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 473) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 474) 	struct mv88e6xxx_chip *chip = ds->priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 475) 	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 476) 	struct ptp_header *hdr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 477) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 478) 	if (!(skb_shinfo(clone)->tx_flags & SKBTX_HW_TSTAMP))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 479) 		return false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 480) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 481) 	hdr = mv88e6xxx_should_tstamp(chip, port, clone, type);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 482) 	if (!hdr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 483) 		return false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 484) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 485) 	if (test_and_set_bit_lock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 486) 				  &ps->state))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 487) 		return false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 488) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 489) 	ps->tx_skb = clone;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 490) 	ps->tx_tstamp_start = jiffies;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 491) 	ps->tx_seq_id = be16_to_cpu(hdr->sequence_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 492) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 493) 	ptp_schedule_worker(chip->ptp_clock, 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 494) 	return true;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 495) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 496) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 497) int mv88e6165_global_disable(struct mv88e6xxx_chip *chip)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 498) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 499) 	u16 val;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 500) 	int err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 501) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 502) 	err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, &val);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 503) 	if (err)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 504) 		return err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 505) 	val |= MV88E6165_PTP_CFG_DISABLE_PTP;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 506) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 507) 	return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, val);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 508) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 509) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 510) int mv88e6165_global_enable(struct mv88e6xxx_chip *chip)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 511) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 512) 	u16 val;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 513) 	int err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 514) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 515) 	err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, &val);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 516) 	if (err)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 517) 		return err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 518) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 519) 	val &= ~(MV88E6165_PTP_CFG_DISABLE_PTP | MV88E6165_PTP_CFG_TSPEC_MASK);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 520) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 521) 	return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, val);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 522) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 523) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 524) int mv88e6352_hwtstamp_port_disable(struct mv88e6xxx_chip *chip, int port)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 525) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 526) 	return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 527) 					MV88E6XXX_PORT_PTP_CFG0_DISABLE_PTP);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 528) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 529) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 530) int mv88e6352_hwtstamp_port_enable(struct mv88e6xxx_chip *chip, int port)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 531) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 532) 	return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 533) 					MV88E6XXX_PORT_PTP_CFG0_DISABLE_TSPEC_MATCH);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 534) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 535) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 536) static int mv88e6xxx_hwtstamp_port_setup(struct mv88e6xxx_chip *chip, int port)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 537) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 538) 	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 539) 	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 540) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 541) 	ps->port_id = port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 542) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 543) 	skb_queue_head_init(&ps->rx_queue);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 544) 	skb_queue_head_init(&ps->rx_queue2);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 545) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 546) 	if (ptp_ops->port_disable)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 547) 		return ptp_ops->port_disable(chip, port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 548) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 549) 	return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 550) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 551) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 552) int mv88e6xxx_hwtstamp_setup(struct mv88e6xxx_chip *chip)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 553) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 554) 	const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 555) 	int err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 556) 	int i;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 557) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 558) 	/* Disable timestamping on all ports. */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 559) 	for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 560) 		err = mv88e6xxx_hwtstamp_port_setup(chip, i);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 561) 		if (err)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 562) 			return err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 563) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 564) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 565) 	/* Disable PTP globally */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 566) 	if (ptp_ops->global_disable) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 567) 		err = ptp_ops->global_disable(chip);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 568) 		if (err)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 569) 			return err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 570) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 571) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 572) 	/* Set the ethertype of L2 PTP messages */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 573) 	err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_GC_ETYPE, ETH_P_1588);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 574) 	if (err)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 575) 		return err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 576) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 577) 	/* MV88E6XXX_PTP_MSG_TYPE is a mask of PTP message types to
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 578) 	 * timestamp. This affects all ports that have timestamping enabled,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 579) 	 * but the timestamp config is per-port; thus we configure all events
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 580) 	 * here and only support the HWTSTAMP_FILTER_*_EVENT filter types.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 581) 	 */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 582) 	err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_MSGTYPE,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 583) 				  MV88E6XXX_PTP_MSGTYPE_ALL_EVENT);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 584) 	if (err)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 585) 		return err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 586) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 587) 	/* Use ARRIVAL1 for peer delay response messages. */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 588) 	err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_TS_ARRIVAL_PTR,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 589) 				  MV88E6XXX_PTP_MSGTYPE_PDLAY_RES);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 590) 	if (err)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 591) 		return err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 592) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 593) 	/* 88E6341 devices default to timestamping at the PHY, but this has
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 594) 	 * a hardware issue that results in unreliable timestamps. Force
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 595) 	 * these devices to timestamp at the MAC.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 596) 	 */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 597) 	if (chip->info->family == MV88E6XXX_FAMILY_6341) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 598) 		u16 val = MV88E6341_PTP_CFG_UPDATE |
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 599) 			  MV88E6341_PTP_CFG_MODE_IDX |
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 600) 			  MV88E6341_PTP_CFG_MODE_TS_AT_MAC;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 601) 		err = mv88e6xxx_ptp_write(chip, MV88E6341_PTP_CFG, val);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 602) 		if (err)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 603) 			return err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 604) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 605) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 606) 	return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 607) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 608) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 609) void mv88e6xxx_hwtstamp_free(struct mv88e6xxx_chip *chip)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 610) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 611) }