1 // SPDX-License-Identifier: GPL-2.0 2 /* Microchip KSZ PTP Implementation 3 * 4 * Copyright (C) 2020 ARRI Lighting 5 * Copyright (C) 2022 Microchip Technology Inc. 6 */ 7 8 #include <linux/dsa/ksz_common.h> 9 #include <linux/kernel.h> 10 #include <linux/ptp_classify.h> 11 #include <linux/ptp_clock_kernel.h> 12 13 #include "ksz_common.h" 14 #include "ksz_ptp.h" 15 #include "ksz_ptp_reg.h" 16 17 #define ptp_caps_to_data(d) container_of((d), struct ksz_ptp_data, caps) 18 #define ptp_data_to_ksz_dev(d) container_of((d), struct ksz_device, ptp_data) 19 20 /* Sub-nanoseconds-adj,max * sub-nanoseconds / 40ns * 1ns 21 * = (2^30-1) * (2 ^ 32) / 40 ns * 1 ns = 6249999 22 */ 23 #define KSZ_MAX_DRIFT_CORR 6249999 24 25 #define KSZ_PTP_INC_NS 40ULL /* HW clock is incremented every 40 ns (by 40) */ 26 #define KSZ_PTP_SUBNS_BITS 32 27 28 static int ksz_ptp_enable_mode(struct ksz_device *dev) 29 { 30 struct ksz_tagger_data *tagger_data = ksz_tagger_data(dev->ds); 31 struct ksz_port *prt; 32 struct dsa_port *dp; 33 bool tag_en = false; 34 35 dsa_switch_for_each_user_port(dp, dev->ds) { 36 prt = &dev->ports[dp->index]; 37 if (prt->hwts_tx_en || prt->hwts_rx_en) { 38 tag_en = true; 39 break; 40 } 41 } 42 43 tagger_data->hwtstamp_set_state(dev->ds, tag_en); 44 45 return ksz_rmw16(dev, REG_PTP_MSG_CONF1, PTP_ENABLE, 46 tag_en ? PTP_ENABLE : 0); 47 } 48 49 /* The function is return back the capability of timestamping feature when 50 * requested through ethtool -T <interface> utility 51 */ 52 int ksz_get_ts_info(struct dsa_switch *ds, int port, struct ethtool_ts_info *ts) 53 { 54 struct ksz_device *dev = ds->priv; 55 struct ksz_ptp_data *ptp_data; 56 57 ptp_data = &dev->ptp_data; 58 59 if (!ptp_data->clock) 60 return -ENODEV; 61 62 ts->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE | 63 SOF_TIMESTAMPING_RX_HARDWARE | 64 SOF_TIMESTAMPING_RAW_HARDWARE; 65 66 ts->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ONESTEP_P2P); 67 68 ts->rx_filters = BIT(HWTSTAMP_FILTER_NONE) | 69 BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT) | 70 BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) | 71 BIT(HWTSTAMP_FILTER_PTP_V2_EVENT); 72 73 ts->phc_index = ptp_clock_index(ptp_data->clock); 74 75 return 0; 76 } 77 78 int ksz_hwtstamp_get(struct dsa_switch *ds, int port, struct ifreq *ifr) 79 { 80 struct ksz_device *dev = ds->priv; 81 struct hwtstamp_config *config; 82 struct ksz_port *prt; 83 84 prt = &dev->ports[port]; 85 config = &prt->tstamp_config; 86 87 return copy_to_user(ifr->ifr_data, config, sizeof(*config)) ? 88 -EFAULT : 0; 89 } 90 91 static int ksz_set_hwtstamp_config(struct ksz_device *dev, 92 struct ksz_port *prt, 93 struct hwtstamp_config *config) 94 { 95 if (config->flags) 96 return -EINVAL; 97 98 switch (config->tx_type) { 99 case HWTSTAMP_TX_OFF: 100 prt->hwts_tx_en = false; 101 break; 102 case HWTSTAMP_TX_ONESTEP_P2P: 103 prt->hwts_tx_en = true; 104 break; 105 default: 106 return -ERANGE; 107 } 108 109 switch (config->rx_filter) { 110 case HWTSTAMP_FILTER_NONE: 111 prt->hwts_rx_en = false; 112 break; 113 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: 114 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: 115 config->rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT; 116 prt->hwts_rx_en = true; 117 break; 118 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: 119 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: 120 config->rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT; 121 prt->hwts_rx_en = true; 122 break; 123 case HWTSTAMP_FILTER_PTP_V2_EVENT: 124 case HWTSTAMP_FILTER_PTP_V2_SYNC: 125 config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; 126 prt->hwts_rx_en = true; 127 break; 128 default: 129 config->rx_filter = HWTSTAMP_FILTER_NONE; 130 return -ERANGE; 131 } 132 133 return ksz_ptp_enable_mode(dev); 134 } 135 136 int ksz_hwtstamp_set(struct dsa_switch *ds, int port, struct ifreq *ifr) 137 { 138 struct ksz_device *dev = ds->priv; 139 struct hwtstamp_config config; 140 struct ksz_port *prt; 141 int ret; 142 143 prt = &dev->ports[port]; 144 145 ret = copy_from_user(&config, ifr->ifr_data, sizeof(config)); 146 if (ret) 147 return ret; 148 149 ret = ksz_set_hwtstamp_config(dev, prt, &config); 150 if (ret) 151 return ret; 152 153 memcpy(&prt->tstamp_config, &config, sizeof(config)); 154 155 return copy_to_user(ifr->ifr_data, &config, sizeof(config)); 156 } 157 158 static int _ksz_ptp_gettime(struct ksz_device *dev, struct timespec64 *ts) 159 { 160 u32 nanoseconds; 161 u32 seconds; 162 u8 phase; 163 int ret; 164 165 /* Copy current PTP clock into shadow registers and read */ 166 ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_READ_TIME, PTP_READ_TIME); 167 if (ret) 168 return ret; 169 170 ret = ksz_read8(dev, REG_PTP_RTC_SUB_NANOSEC__2, &phase); 171 if (ret) 172 return ret; 173 174 ret = ksz_read32(dev, REG_PTP_RTC_NANOSEC, &nanoseconds); 175 if (ret) 176 return ret; 177 178 ret = ksz_read32(dev, REG_PTP_RTC_SEC, &seconds); 179 if (ret) 180 return ret; 181 182 ts->tv_sec = seconds; 183 ts->tv_nsec = nanoseconds + phase * 8; 184 185 return 0; 186 } 187 188 static int ksz_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts) 189 { 190 struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp); 191 struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data); 192 int ret; 193 194 mutex_lock(&ptp_data->lock); 195 ret = _ksz_ptp_gettime(dev, ts); 196 mutex_unlock(&ptp_data->lock); 197 198 return ret; 199 } 200 201 static int ksz_ptp_settime(struct ptp_clock_info *ptp, 202 const struct timespec64 *ts) 203 { 204 struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp); 205 struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data); 206 int ret; 207 208 mutex_lock(&ptp_data->lock); 209 210 /* Write to shadow registers and Load PTP clock */ 211 ret = ksz_write16(dev, REG_PTP_RTC_SUB_NANOSEC__2, PTP_RTC_0NS); 212 if (ret) 213 goto unlock; 214 215 ret = ksz_write32(dev, REG_PTP_RTC_NANOSEC, ts->tv_nsec); 216 if (ret) 217 goto unlock; 218 219 ret = ksz_write32(dev, REG_PTP_RTC_SEC, ts->tv_sec); 220 if (ret) 221 goto unlock; 222 223 ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_LOAD_TIME, PTP_LOAD_TIME); 224 225 unlock: 226 mutex_unlock(&ptp_data->lock); 227 228 return ret; 229 } 230 231 static int ksz_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) 232 { 233 struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp); 234 struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data); 235 u64 base, adj; 236 bool negative; 237 u32 data32; 238 int ret; 239 240 mutex_lock(&ptp_data->lock); 241 242 if (scaled_ppm) { 243 base = KSZ_PTP_INC_NS << KSZ_PTP_SUBNS_BITS; 244 negative = diff_by_scaled_ppm(base, scaled_ppm, &adj); 245 246 data32 = (u32)adj; 247 data32 &= PTP_SUBNANOSEC_M; 248 if (!negative) 249 data32 |= PTP_RATE_DIR; 250 251 ret = ksz_write32(dev, REG_PTP_SUBNANOSEC_RATE, data32); 252 if (ret) 253 goto unlock; 254 255 ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_CLK_ADJ_ENABLE, 256 PTP_CLK_ADJ_ENABLE); 257 if (ret) 258 goto unlock; 259 } else { 260 ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_CLK_ADJ_ENABLE, 0); 261 if (ret) 262 goto unlock; 263 } 264 265 unlock: 266 mutex_unlock(&ptp_data->lock); 267 return ret; 268 } 269 270 static int ksz_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) 271 { 272 struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp); 273 struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data); 274 s32 sec, nsec; 275 u16 data16; 276 int ret; 277 278 mutex_lock(&ptp_data->lock); 279 280 /* do not use ns_to_timespec64(), 281 * both sec and nsec are subtracted by hw 282 */ 283 sec = div_s64_rem(delta, NSEC_PER_SEC, &nsec); 284 285 ret = ksz_write32(dev, REG_PTP_RTC_NANOSEC, abs(nsec)); 286 if (ret) 287 goto unlock; 288 289 ret = ksz_write32(dev, REG_PTP_RTC_SEC, abs(sec)); 290 if (ret) 291 goto unlock; 292 293 ret = ksz_read16(dev, REG_PTP_CLK_CTRL, &data16); 294 if (ret) 295 goto unlock; 296 297 data16 |= PTP_STEP_ADJ; 298 299 /* PTP_STEP_DIR -- 0: subtract, 1: add */ 300 if (delta < 0) 301 data16 &= ~PTP_STEP_DIR; 302 else 303 data16 |= PTP_STEP_DIR; 304 305 ret = ksz_write16(dev, REG_PTP_CLK_CTRL, data16); 306 307 unlock: 308 mutex_unlock(&ptp_data->lock); 309 return ret; 310 } 311 312 static int ksz_ptp_start_clock(struct ksz_device *dev) 313 { 314 return ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_CLK_ENABLE, PTP_CLK_ENABLE); 315 } 316 317 int ksz_ptp_clock_register(struct dsa_switch *ds) 318 { 319 struct ksz_device *dev = ds->priv; 320 struct ksz_ptp_data *ptp_data; 321 int ret; 322 323 ptp_data = &dev->ptp_data; 324 mutex_init(&ptp_data->lock); 325 326 ptp_data->caps.owner = THIS_MODULE; 327 snprintf(ptp_data->caps.name, 16, "Microchip Clock"); 328 ptp_data->caps.max_adj = KSZ_MAX_DRIFT_CORR; 329 ptp_data->caps.gettime64 = ksz_ptp_gettime; 330 ptp_data->caps.settime64 = ksz_ptp_settime; 331 ptp_data->caps.adjfine = ksz_ptp_adjfine; 332 ptp_data->caps.adjtime = ksz_ptp_adjtime; 333 334 ret = ksz_ptp_start_clock(dev); 335 if (ret) 336 return ret; 337 338 /* Currently only P2P mode is supported. When 802_1AS bit is set, it 339 * forwards all PTP packets to host port and none to other ports. 340 */ 341 ret = ksz_rmw16(dev, REG_PTP_MSG_CONF1, PTP_TC_P2P | PTP_802_1AS, 342 PTP_TC_P2P | PTP_802_1AS); 343 if (ret) 344 return ret; 345 346 ptp_data->clock = ptp_clock_register(&ptp_data->caps, dev->dev); 347 if (IS_ERR_OR_NULL(ptp_data->clock)) 348 return PTR_ERR(ptp_data->clock); 349 350 return 0; 351 } 352 353 void ksz_ptp_clock_unregister(struct dsa_switch *ds) 354 { 355 struct ksz_device *dev = ds->priv; 356 struct ksz_ptp_data *ptp_data; 357 358 ptp_data = &dev->ptp_data; 359 360 if (ptp_data->clock) 361 ptp_clock_unregister(ptp_data->clock); 362 } 363 364 MODULE_AUTHOR("Christian Eggers <ceggers@arri.de>"); 365 MODULE_AUTHOR("Arun Ramadoss <arun.ramadoss@microchip.com>"); 366 MODULE_DESCRIPTION("PTP support for KSZ switch"); 367 MODULE_LICENSE("GPL"); 368