Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0
0002 /* Copyright (c) 2019, Vladimir Oltean <olteanv@gmail.com>
0003  */
0004 #include <linux/spi/spi.h>
0005 #include "sja1105.h"
0006 
0007 /* The adjfine API clamps ppb between [-32,768,000, 32,768,000], and
0008  * therefore scaled_ppm between [-2,147,483,648, 2,147,483,647].
0009  * Set the maximum supported ppb to a round value smaller than the maximum.
0010  *
0011  * Percentually speaking, this is a +/- 0.032x adjustment of the
0012  * free-running counter (0.968x to 1.032x).
0013  */
0014 #define SJA1105_MAX_ADJ_PPB     32000000
0015 #define SJA1105_SIZE_PTP_CMD        4
0016 
0017 /* PTPSYNCTS has no interrupt or update mechanism, because the intended
0018  * hardware use case is for the timestamp to be collected synchronously,
0019  * immediately after the CAS_MASTER SJA1105 switch has performed a CASSYNC
0020  * one-shot toggle (no return to level) on the PTP_CLK pin. When used as a
0021  * generic extts source, the PTPSYNCTS register needs polling and a comparison
0022  * with the old value. The polling interval is configured as the Nyquist rate
0023  * of a signal with 50% duty cycle and 1Hz frequency, which is sadly all that
0024  * this hardware can do (but may be enough for some setups). Anything of higher
0025  * frequency than 1 Hz will be lost, since there is no timestamp FIFO.
0026  */
0027 #define SJA1105_EXTTS_INTERVAL      (HZ / 6)
0028 
0029 /*            This range is actually +/- SJA1105_MAX_ADJ_PPB
0030  *            divided by 1000 (ppb -> ppm) and with a 16-bit
0031  *            "fractional" part (actually fixed point).
0032  *                                    |
0033  *                                    v
0034  * Convert scaled_ppm from the +/- ((10^6) << 16) range
0035  * into the +/- (1 << 31) range.
0036  *
0037  * This forgoes a "ppb" numeric representation (up to NSEC_PER_SEC)
0038  * and defines the scaling factor between scaled_ppm and the actual
0039  * frequency adjustments of the PHC.
0040  *
0041  *   ptpclkrate = scaled_ppm * 2^31 / (10^6 * 2^16)
0042  *   simplifies to
0043  *   ptpclkrate = scaled_ppm * 2^9 / 5^6
0044  */
0045 #define SJA1105_CC_MULT_NUM     (1 << 9)
0046 #define SJA1105_CC_MULT_DEM     15625
0047 #define SJA1105_CC_MULT         0x80000000
0048 
0049 enum sja1105_ptp_clk_mode {
0050     PTP_ADD_MODE = 1,
0051     PTP_SET_MODE = 0,
0052 };
0053 
0054 #define extts_to_data(t) \
0055         container_of((t), struct sja1105_ptp_data, extts_timer)
0056 #define ptp_caps_to_data(d) \
0057         container_of((d), struct sja1105_ptp_data, caps)
0058 #define ptp_data_to_sja1105(d) \
0059         container_of((d), struct sja1105_private, ptp_data)
0060 
0061 /* Must be called only while the RX timestamping state of the tagger
0062  * is turned off
0063  */
0064 static int sja1105_change_rxtstamping(struct sja1105_private *priv,
0065                       bool on)
0066 {
0067     struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
0068     struct sja1105_general_params_entry *general_params;
0069     struct sja1105_table *table;
0070 
0071     table = &priv->static_config.tables[BLK_IDX_GENERAL_PARAMS];
0072     general_params = table->entries;
0073     general_params->send_meta1 = on;
0074     general_params->send_meta0 = on;
0075 
0076     ptp_cancel_worker_sync(ptp_data->clock);
0077     skb_queue_purge(&ptp_data->skb_txtstamp_queue);
0078     skb_queue_purge(&ptp_data->skb_rxtstamp_queue);
0079 
0080     return sja1105_static_config_reload(priv, SJA1105_RX_HWTSTAMPING);
0081 }
0082 
0083 int sja1105_hwtstamp_set(struct dsa_switch *ds, int port, struct ifreq *ifr)
0084 {
0085     struct sja1105_tagger_data *tagger_data = sja1105_tagger_data(ds);
0086     struct sja1105_private *priv = ds->priv;
0087     struct hwtstamp_config config;
0088     bool rx_on;
0089     int rc;
0090 
0091     if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
0092         return -EFAULT;
0093 
0094     switch (config.tx_type) {
0095     case HWTSTAMP_TX_OFF:
0096         priv->hwts_tx_en &= ~BIT(port);
0097         break;
0098     case HWTSTAMP_TX_ON:
0099         priv->hwts_tx_en |= BIT(port);
0100         break;
0101     default:
0102         return -ERANGE;
0103     }
0104 
0105     switch (config.rx_filter) {
0106     case HWTSTAMP_FILTER_NONE:
0107         rx_on = false;
0108         break;
0109     default:
0110         rx_on = true;
0111         break;
0112     }
0113 
0114     if (rx_on != tagger_data->rxtstamp_get_state(ds)) {
0115         tagger_data->rxtstamp_set_state(ds, false);
0116 
0117         rc = sja1105_change_rxtstamping(priv, rx_on);
0118         if (rc < 0) {
0119             dev_err(ds->dev,
0120                 "Failed to change RX timestamping: %d\n", rc);
0121             return rc;
0122         }
0123         if (rx_on)
0124             tagger_data->rxtstamp_set_state(ds, true);
0125     }
0126 
0127     if (copy_to_user(ifr->ifr_data, &config, sizeof(config)))
0128         return -EFAULT;
0129     return 0;
0130 }
0131 
0132 int sja1105_hwtstamp_get(struct dsa_switch *ds, int port, struct ifreq *ifr)
0133 {
0134     struct sja1105_tagger_data *tagger_data = sja1105_tagger_data(ds);
0135     struct sja1105_private *priv = ds->priv;
0136     struct hwtstamp_config config;
0137 
0138     config.flags = 0;
0139     if (priv->hwts_tx_en & BIT(port))
0140         config.tx_type = HWTSTAMP_TX_ON;
0141     else
0142         config.tx_type = HWTSTAMP_TX_OFF;
0143     if (tagger_data->rxtstamp_get_state(ds))
0144         config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
0145     else
0146         config.rx_filter = HWTSTAMP_FILTER_NONE;
0147 
0148     return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
0149         -EFAULT : 0;
0150 }
0151 
0152 int sja1105_get_ts_info(struct dsa_switch *ds, int port,
0153             struct ethtool_ts_info *info)
0154 {
0155     struct sja1105_private *priv = ds->priv;
0156     struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
0157 
0158     /* Called during cleanup */
0159     if (!ptp_data->clock)
0160         return -ENODEV;
0161 
0162     info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
0163                 SOF_TIMESTAMPING_RX_HARDWARE |
0164                 SOF_TIMESTAMPING_RAW_HARDWARE;
0165     info->tx_types = (1 << HWTSTAMP_TX_OFF) |
0166              (1 << HWTSTAMP_TX_ON);
0167     info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) |
0168                (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT);
0169     info->phc_index = ptp_clock_index(ptp_data->clock);
0170     return 0;
0171 }
0172 
0173 void sja1105et_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd,
0174                    enum packing_op op)
0175 {
0176     const int size = SJA1105_SIZE_PTP_CMD;
0177     /* No need to keep this as part of the structure */
0178     u64 valid = 1;
0179 
0180     sja1105_packing(buf, &valid,           31, 31, size, op);
0181     sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op);
0182     sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op);
0183     sja1105_packing(buf, &cmd->startptpcp, 28, 28, size, op);
0184     sja1105_packing(buf, &cmd->stopptpcp,  27, 27, size, op);
0185     sja1105_packing(buf, &cmd->resptp,      2,  2, size, op);
0186     sja1105_packing(buf, &cmd->corrclk4ts,  1,  1, size, op);
0187     sja1105_packing(buf, &cmd->ptpclkadd,   0,  0, size, op);
0188 }
0189 
0190 void sja1105pqrs_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd,
0191                  enum packing_op op)
0192 {
0193     const int size = SJA1105_SIZE_PTP_CMD;
0194     /* No need to keep this as part of the structure */
0195     u64 valid = 1;
0196 
0197     sja1105_packing(buf, &valid,           31, 31, size, op);
0198     sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op);
0199     sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op);
0200     sja1105_packing(buf, &cmd->startptpcp, 28, 28, size, op);
0201     sja1105_packing(buf, &cmd->stopptpcp,  27, 27, size, op);
0202     sja1105_packing(buf, &cmd->resptp,      3,  3, size, op);
0203     sja1105_packing(buf, &cmd->corrclk4ts,  2,  2, size, op);
0204     sja1105_packing(buf, &cmd->ptpclkadd,   0,  0, size, op);
0205 }
0206 
0207 int sja1105_ptp_commit(struct dsa_switch *ds, struct sja1105_ptp_cmd *cmd,
0208                sja1105_spi_rw_mode_t rw)
0209 {
0210     const struct sja1105_private *priv = ds->priv;
0211     const struct sja1105_regs *regs = priv->info->regs;
0212     u8 buf[SJA1105_SIZE_PTP_CMD] = {0};
0213     int rc;
0214 
0215     if (rw == SPI_WRITE)
0216         priv->info->ptp_cmd_packing(buf, cmd, PACK);
0217 
0218     rc = sja1105_xfer_buf(priv, rw, regs->ptp_control, buf,
0219                   SJA1105_SIZE_PTP_CMD);
0220 
0221     if (rw == SPI_READ)
0222         priv->info->ptp_cmd_packing(buf, cmd, UNPACK);
0223 
0224     return rc;
0225 }
0226 
0227 /* The switch returns partial timestamps (24 bits for SJA1105 E/T, which wrap
0228  * around in 0.135 seconds, and 32 bits for P/Q/R/S, wrapping around in 34.35
0229  * seconds).
0230  *
0231  * This receives the RX or TX MAC timestamps, provided by hardware as
0232  * the lower bits of the cycle counter, sampled at the time the timestamp was
0233  * collected.
0234  *
0235  * To reconstruct into a full 64-bit-wide timestamp, the cycle counter is
0236  * read and the high-order bits are filled in.
0237  *
0238  * Must be called within one wraparound period of the partial timestamp since
0239  * it was generated by the MAC.
0240  */
0241 static u64 sja1105_tstamp_reconstruct(struct dsa_switch *ds, u64 now,
0242                       u64 ts_partial)
0243 {
0244     struct sja1105_private *priv = ds->priv;
0245     u64 partial_tstamp_mask = CYCLECOUNTER_MASK(priv->info->ptp_ts_bits);
0246     u64 ts_reconstructed;
0247 
0248     ts_reconstructed = (now & ~partial_tstamp_mask) | ts_partial;
0249 
0250     /* Check lower bits of current cycle counter against the timestamp.
0251      * If the current cycle counter is lower than the partial timestamp,
0252      * then wraparound surely occurred and must be accounted for.
0253      */
0254     if ((now & partial_tstamp_mask) <= ts_partial)
0255         ts_reconstructed -= (partial_tstamp_mask + 1);
0256 
0257     return ts_reconstructed;
0258 }
0259 
0260 /* Reads the SPI interface for an egress timestamp generated by the switch
0261  * for frames sent using management routes.
0262  *
0263  * SJA1105 E/T layout of the 4-byte SPI payload:
0264  *
0265  * 31    23    15    7     0
0266  * |     |     |     |     |
0267  * +-----+-----+-----+     ^
0268  *          ^              |
0269  *          |              |
0270  *  24-bit timestamp   Update bit
0271  *
0272  *
0273  * SJA1105 P/Q/R/S layout of the 8-byte SPI payload:
0274  *
0275  * 31    23    15    7     0     63    55    47    39    32
0276  * |     |     |     |     |     |     |     |     |     |
0277  *                         ^     +-----+-----+-----+-----+
0278  *                         |                 ^
0279  *                         |                 |
0280  *                    Update bit    32-bit timestamp
0281  *
0282  * Notice that the update bit is in the same place.
0283  * To have common code for E/T and P/Q/R/S for reading the timestamp,
0284  * we need to juggle with the offset and the bit indices.
0285  */
0286 static int sja1105_ptpegr_ts_poll(struct dsa_switch *ds, int port, u64 *ts)
0287 {
0288     struct sja1105_private *priv = ds->priv;
0289     const struct sja1105_regs *regs = priv->info->regs;
0290     int tstamp_bit_start, tstamp_bit_end;
0291     int timeout = 10;
0292     u8 packed_buf[8];
0293     u64 update;
0294     int rc;
0295 
0296     do {
0297         rc = sja1105_xfer_buf(priv, SPI_READ, regs->ptpegr_ts[port],
0298                       packed_buf, priv->info->ptpegr_ts_bytes);
0299         if (rc < 0)
0300             return rc;
0301 
0302         sja1105_unpack(packed_buf, &update, 0, 0,
0303                    priv->info->ptpegr_ts_bytes);
0304         if (update)
0305             break;
0306 
0307         usleep_range(10, 50);
0308     } while (--timeout);
0309 
0310     if (!timeout)
0311         return -ETIMEDOUT;
0312 
0313     /* Point the end bit to the second 32-bit word on P/Q/R/S,
0314      * no-op on E/T.
0315      */
0316     tstamp_bit_end = (priv->info->ptpegr_ts_bytes - 4) * 8;
0317     /* Shift the 24-bit timestamp on E/T to be collected from 31:8.
0318      * No-op on P/Q/R/S.
0319      */
0320     tstamp_bit_end += 32 - priv->info->ptp_ts_bits;
0321     tstamp_bit_start = tstamp_bit_end + priv->info->ptp_ts_bits - 1;
0322 
0323     *ts = 0;
0324 
0325     sja1105_unpack(packed_buf, ts, tstamp_bit_start, tstamp_bit_end,
0326                priv->info->ptpegr_ts_bytes);
0327 
0328     return 0;
0329 }
0330 
0331 /* Caller must hold ptp_data->lock */
0332 static int sja1105_ptpclkval_read(struct sja1105_private *priv, u64 *ticks,
0333                   struct ptp_system_timestamp *ptp_sts)
0334 {
0335     const struct sja1105_regs *regs = priv->info->regs;
0336 
0337     return sja1105_xfer_u64(priv, SPI_READ, regs->ptpclkval, ticks,
0338                 ptp_sts);
0339 }
0340 
0341 /* Caller must hold ptp_data->lock */
0342 static int sja1105_ptpclkval_write(struct sja1105_private *priv, u64 ticks,
0343                    struct ptp_system_timestamp *ptp_sts)
0344 {
0345     const struct sja1105_regs *regs = priv->info->regs;
0346 
0347     return sja1105_xfer_u64(priv, SPI_WRITE, regs->ptpclkval, &ticks,
0348                 ptp_sts);
0349 }
0350 
0351 static void sja1105_extts_poll(struct sja1105_private *priv)
0352 {
0353     struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
0354     const struct sja1105_regs *regs = priv->info->regs;
0355     struct ptp_clock_event event;
0356     u64 ptpsyncts = 0;
0357     int rc;
0358 
0359     rc = sja1105_xfer_u64(priv, SPI_READ, regs->ptpsyncts, &ptpsyncts,
0360                   NULL);
0361     if (rc < 0)
0362         dev_err_ratelimited(priv->ds->dev,
0363                     "Failed to read PTPSYNCTS: %d\n", rc);
0364 
0365     if (ptpsyncts && ptp_data->ptpsyncts != ptpsyncts) {
0366         event.index = 0;
0367         event.type = PTP_CLOCK_EXTTS;
0368         event.timestamp = ns_to_ktime(sja1105_ticks_to_ns(ptpsyncts));
0369         ptp_clock_event(ptp_data->clock, &event);
0370 
0371         ptp_data->ptpsyncts = ptpsyncts;
0372     }
0373 }
0374 
0375 static long sja1105_rxtstamp_work(struct ptp_clock_info *ptp)
0376 {
0377     struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
0378     struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
0379     struct dsa_switch *ds = priv->ds;
0380     struct sk_buff *skb;
0381 
0382     mutex_lock(&ptp_data->lock);
0383 
0384     while ((skb = skb_dequeue(&ptp_data->skb_rxtstamp_queue)) != NULL) {
0385         struct skb_shared_hwtstamps *shwt = skb_hwtstamps(skb);
0386         u64 ticks, ts;
0387         int rc;
0388 
0389         rc = sja1105_ptpclkval_read(priv, &ticks, NULL);
0390         if (rc < 0) {
0391             dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
0392             kfree_skb(skb);
0393             continue;
0394         }
0395 
0396         *shwt = (struct skb_shared_hwtstamps) {0};
0397 
0398         ts = SJA1105_SKB_CB(skb)->tstamp;
0399         ts = sja1105_tstamp_reconstruct(ds, ticks, ts);
0400 
0401         shwt->hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
0402         netif_rx(skb);
0403     }
0404 
0405     if (ptp_data->extts_enabled)
0406         sja1105_extts_poll(priv);
0407 
0408     mutex_unlock(&ptp_data->lock);
0409 
0410     /* Don't restart */
0411     return -1;
0412 }
0413 
0414 bool sja1105_rxtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
0415 {
0416     struct sja1105_tagger_data *tagger_data = sja1105_tagger_data(ds);
0417     struct sja1105_private *priv = ds->priv;
0418     struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
0419 
0420     if (!tagger_data->rxtstamp_get_state(ds))
0421         return false;
0422 
0423     /* We need to read the full PTP clock to reconstruct the Rx
0424      * timestamp. For that we need a sleepable context.
0425      */
0426     skb_queue_tail(&ptp_data->skb_rxtstamp_queue, skb);
0427     ptp_schedule_worker(ptp_data->clock, 0);
0428     return true;
0429 }
0430 
0431 bool sja1110_rxtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
0432 {
0433     struct skb_shared_hwtstamps *shwt = skb_hwtstamps(skb);
0434     u64 ts = SJA1105_SKB_CB(skb)->tstamp;
0435 
0436     *shwt = (struct skb_shared_hwtstamps) {0};
0437 
0438     shwt->hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
0439 
0440     /* Don't defer */
0441     return false;
0442 }
0443 
0444 /* Called from dsa_skb_defer_rx_timestamp */
0445 bool sja1105_port_rxtstamp(struct dsa_switch *ds, int port,
0446                struct sk_buff *skb, unsigned int type)
0447 {
0448     struct sja1105_private *priv = ds->priv;
0449 
0450     return priv->info->rxtstamp(ds, port, skb);
0451 }
0452 
0453 void sja1110_process_meta_tstamp(struct dsa_switch *ds, int port, u8 ts_id,
0454                  enum sja1110_meta_tstamp dir, u64 tstamp)
0455 {
0456     struct sja1105_private *priv = ds->priv;
0457     struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
0458     struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
0459     struct skb_shared_hwtstamps shwt = {0};
0460 
0461     /* We don't care about RX timestamps on the CPU port */
0462     if (dir == SJA1110_META_TSTAMP_RX)
0463         return;
0464 
0465     spin_lock(&ptp_data->skb_txtstamp_queue.lock);
0466 
0467     skb_queue_walk_safe(&ptp_data->skb_txtstamp_queue, skb, skb_tmp) {
0468         if (SJA1105_SKB_CB(skb)->ts_id != ts_id)
0469             continue;
0470 
0471         __skb_unlink(skb, &ptp_data->skb_txtstamp_queue);
0472         skb_match = skb;
0473 
0474         break;
0475     }
0476 
0477     spin_unlock(&ptp_data->skb_txtstamp_queue.lock);
0478 
0479     if (WARN_ON(!skb_match))
0480         return;
0481 
0482     shwt.hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(tstamp));
0483     skb_complete_tx_timestamp(skb_match, &shwt);
0484 }
0485 
0486 /* In addition to cloning the skb which is done by the common
0487  * sja1105_port_txtstamp, we need to generate a timestamp ID and save the
0488  * packet to the TX timestamping queue.
0489  */
0490 void sja1110_txtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
0491 {
0492     struct sk_buff *clone = SJA1105_SKB_CB(skb)->clone;
0493     struct sja1105_private *priv = ds->priv;
0494     struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
0495     u8 ts_id;
0496 
0497     skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
0498 
0499     spin_lock(&priv->ts_id_lock);
0500 
0501     ts_id = priv->ts_id;
0502     /* Deal automatically with 8-bit wraparound */
0503     priv->ts_id++;
0504 
0505     SJA1105_SKB_CB(clone)->ts_id = ts_id;
0506 
0507     spin_unlock(&priv->ts_id_lock);
0508 
0509     skb_queue_tail(&ptp_data->skb_txtstamp_queue, clone);
0510 }
0511 
0512 /* Called from dsa_skb_tx_timestamp. This callback is just to clone
0513  * the skb and have it available in SJA1105_SKB_CB in the .port_deferred_xmit
0514  * callback, where we will timestamp it synchronously.
0515  */
0516 void sja1105_port_txtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
0517 {
0518     struct sja1105_private *priv = ds->priv;
0519     struct sk_buff *clone;
0520 
0521     if (!(priv->hwts_tx_en & BIT(port)))
0522         return;
0523 
0524     clone = skb_clone_sk(skb);
0525     if (!clone)
0526         return;
0527 
0528     SJA1105_SKB_CB(skb)->clone = clone;
0529 
0530     if (priv->info->txtstamp)
0531         priv->info->txtstamp(ds, port, skb);
0532 }
0533 
0534 static int sja1105_ptp_reset(struct dsa_switch *ds)
0535 {
0536     struct sja1105_private *priv = ds->priv;
0537     struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
0538     struct sja1105_ptp_cmd cmd = ptp_data->cmd;
0539     int rc;
0540 
0541     mutex_lock(&ptp_data->lock);
0542 
0543     cmd.resptp = 1;
0544 
0545     dev_dbg(ds->dev, "Resetting PTP clock\n");
0546     rc = sja1105_ptp_commit(ds, &cmd, SPI_WRITE);
0547 
0548     sja1105_tas_clockstep(priv->ds);
0549 
0550     mutex_unlock(&ptp_data->lock);
0551 
0552     return rc;
0553 }
0554 
0555 /* Caller must hold ptp_data->lock */
0556 int __sja1105_ptp_gettimex(struct dsa_switch *ds, u64 *ns,
0557                struct ptp_system_timestamp *ptp_sts)
0558 {
0559     struct sja1105_private *priv = ds->priv;
0560     u64 ticks;
0561     int rc;
0562 
0563     rc = sja1105_ptpclkval_read(priv, &ticks, ptp_sts);
0564     if (rc < 0) {
0565         dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
0566         return rc;
0567     }
0568 
0569     *ns = sja1105_ticks_to_ns(ticks);
0570 
0571     return 0;
0572 }
0573 
0574 static int sja1105_ptp_gettimex(struct ptp_clock_info *ptp,
0575                 struct timespec64 *ts,
0576                 struct ptp_system_timestamp *ptp_sts)
0577 {
0578     struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
0579     struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
0580     u64 now = 0;
0581     int rc;
0582 
0583     mutex_lock(&ptp_data->lock);
0584 
0585     rc = __sja1105_ptp_gettimex(priv->ds, &now, ptp_sts);
0586     *ts = ns_to_timespec64(now);
0587 
0588     mutex_unlock(&ptp_data->lock);
0589 
0590     return rc;
0591 }
0592 
0593 /* Caller must hold ptp_data->lock */
0594 static int sja1105_ptp_mode_set(struct sja1105_private *priv,
0595                 enum sja1105_ptp_clk_mode mode)
0596 {
0597     struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
0598 
0599     if (ptp_data->cmd.ptpclkadd == mode)
0600         return 0;
0601 
0602     ptp_data->cmd.ptpclkadd = mode;
0603 
0604     return sja1105_ptp_commit(priv->ds, &ptp_data->cmd, SPI_WRITE);
0605 }
0606 
0607 /* Write to PTPCLKVAL while PTPCLKADD is 0 */
0608 int __sja1105_ptp_settime(struct dsa_switch *ds, u64 ns,
0609               struct ptp_system_timestamp *ptp_sts)
0610 {
0611     struct sja1105_private *priv = ds->priv;
0612     u64 ticks = ns_to_sja1105_ticks(ns);
0613     int rc;
0614 
0615     rc = sja1105_ptp_mode_set(priv, PTP_SET_MODE);
0616     if (rc < 0) {
0617         dev_err(priv->ds->dev, "Failed to put PTPCLK in set mode\n");
0618         return rc;
0619     }
0620 
0621     rc = sja1105_ptpclkval_write(priv, ticks, ptp_sts);
0622 
0623     sja1105_tas_clockstep(priv->ds);
0624 
0625     return rc;
0626 }
0627 
0628 static int sja1105_ptp_settime(struct ptp_clock_info *ptp,
0629                    const struct timespec64 *ts)
0630 {
0631     struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
0632     struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
0633     u64 ns = timespec64_to_ns(ts);
0634     int rc;
0635 
0636     mutex_lock(&ptp_data->lock);
0637 
0638     rc = __sja1105_ptp_settime(priv->ds, ns, NULL);
0639 
0640     mutex_unlock(&ptp_data->lock);
0641 
0642     return rc;
0643 }
0644 
0645 static int sja1105_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
0646 {
0647     struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
0648     struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
0649     const struct sja1105_regs *regs = priv->info->regs;
0650     u32 clkrate32;
0651     s64 clkrate;
0652     int rc;
0653 
0654     clkrate = (s64)scaled_ppm * SJA1105_CC_MULT_NUM;
0655     clkrate = div_s64(clkrate, SJA1105_CC_MULT_DEM);
0656 
0657     /* Take a +/- value and re-center it around 2^31. */
0658     clkrate = SJA1105_CC_MULT + clkrate;
0659     WARN_ON(abs(clkrate) >= GENMASK_ULL(31, 0));
0660     clkrate32 = clkrate;
0661 
0662     mutex_lock(&ptp_data->lock);
0663 
0664     rc = sja1105_xfer_u32(priv, SPI_WRITE, regs->ptpclkrate, &clkrate32,
0665                   NULL);
0666 
0667     sja1105_tas_adjfreq(priv->ds);
0668 
0669     mutex_unlock(&ptp_data->lock);
0670 
0671     return rc;
0672 }
0673 
0674 /* Write to PTPCLKVAL while PTPCLKADD is 1 */
0675 int __sja1105_ptp_adjtime(struct dsa_switch *ds, s64 delta)
0676 {
0677     struct sja1105_private *priv = ds->priv;
0678     s64 ticks = ns_to_sja1105_ticks(delta);
0679     int rc;
0680 
0681     rc = sja1105_ptp_mode_set(priv, PTP_ADD_MODE);
0682     if (rc < 0) {
0683         dev_err(priv->ds->dev, "Failed to put PTPCLK in add mode\n");
0684         return rc;
0685     }
0686 
0687     rc = sja1105_ptpclkval_write(priv, ticks, NULL);
0688 
0689     sja1105_tas_clockstep(priv->ds);
0690 
0691     return rc;
0692 }
0693 
0694 static int sja1105_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
0695 {
0696     struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
0697     struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
0698     int rc;
0699 
0700     mutex_lock(&ptp_data->lock);
0701 
0702     rc = __sja1105_ptp_adjtime(priv->ds, delta);
0703 
0704     mutex_unlock(&ptp_data->lock);
0705 
0706     return rc;
0707 }
0708 
0709 static void sja1105_ptp_extts_setup_timer(struct sja1105_ptp_data *ptp_data)
0710 {
0711     unsigned long expires = ((jiffies / SJA1105_EXTTS_INTERVAL) + 1) *
0712                 SJA1105_EXTTS_INTERVAL;
0713 
0714     mod_timer(&ptp_data->extts_timer, expires);
0715 }
0716 
0717 static void sja1105_ptp_extts_timer(struct timer_list *t)
0718 {
0719     struct sja1105_ptp_data *ptp_data = extts_to_data(t);
0720 
0721     ptp_schedule_worker(ptp_data->clock, 0);
0722 
0723     sja1105_ptp_extts_setup_timer(ptp_data);
0724 }
0725 
0726 static int sja1105_change_ptp_clk_pin_func(struct sja1105_private *priv,
0727                        enum ptp_pin_function func)
0728 {
0729     struct sja1105_avb_params_entry *avb;
0730     enum ptp_pin_function old_func;
0731 
0732     avb = priv->static_config.tables[BLK_IDX_AVB_PARAMS].entries;
0733 
0734     if (priv->info->device_id == SJA1105E_DEVICE_ID ||
0735         priv->info->device_id == SJA1105T_DEVICE_ID ||
0736         avb->cas_master)
0737         old_func = PTP_PF_PEROUT;
0738     else
0739         old_func = PTP_PF_EXTTS;
0740 
0741     if (func == old_func)
0742         return 0;
0743 
0744     avb->cas_master = (func == PTP_PF_PEROUT);
0745 
0746     return sja1105_dynamic_config_write(priv, BLK_IDX_AVB_PARAMS, 0, avb,
0747                         true);
0748 }
0749 
0750 /* The PTP_CLK pin may be configured to toggle with a 50% duty cycle and a
0751  * frequency f:
0752  *
0753  *           NSEC_PER_SEC
0754  * f = ----------------------
0755  *     (PTPPINDUR * 8 ns) * 2
0756  */
0757 static int sja1105_per_out_enable(struct sja1105_private *priv,
0758                   struct ptp_perout_request *perout,
0759                   bool on)
0760 {
0761     struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
0762     const struct sja1105_regs *regs = priv->info->regs;
0763     struct sja1105_ptp_cmd cmd = ptp_data->cmd;
0764     int rc;
0765 
0766     /* We only support one channel */
0767     if (perout->index != 0)
0768         return -EOPNOTSUPP;
0769 
0770     /* Reject requests with unsupported flags */
0771     if (perout->flags)
0772         return -EOPNOTSUPP;
0773 
0774     mutex_lock(&ptp_data->lock);
0775 
0776     rc = sja1105_change_ptp_clk_pin_func(priv, PTP_PF_PEROUT);
0777     if (rc)
0778         goto out;
0779 
0780     if (on) {
0781         struct timespec64 pin_duration_ts = {
0782             .tv_sec = perout->period.sec,
0783             .tv_nsec = perout->period.nsec,
0784         };
0785         struct timespec64 pin_start_ts = {
0786             .tv_sec = perout->start.sec,
0787             .tv_nsec = perout->start.nsec,
0788         };
0789         u64 pin_duration = timespec64_to_ns(&pin_duration_ts);
0790         u64 pin_start = timespec64_to_ns(&pin_start_ts);
0791         u32 pin_duration32;
0792         u64 now;
0793 
0794         /* ptppindur: 32 bit register which holds the interval between
0795          * 2 edges on PTP_CLK. So check for truncation which happens
0796          * at periods larger than around 68.7 seconds.
0797          */
0798         pin_duration = ns_to_sja1105_ticks(pin_duration / 2);
0799         if (pin_duration > U32_MAX) {
0800             rc = -ERANGE;
0801             goto out;
0802         }
0803         pin_duration32 = pin_duration;
0804 
0805         /* ptppins: 64 bit register which needs to hold a PTP time
0806          * larger than the current time, otherwise the startptpcp
0807          * command won't do anything. So advance the current time
0808          * by a number of periods in a way that won't alter the
0809          * phase offset.
0810          */
0811         rc = __sja1105_ptp_gettimex(priv->ds, &now, NULL);
0812         if (rc < 0)
0813             goto out;
0814 
0815         pin_start = future_base_time(pin_start, pin_duration,
0816                          now + 1ull * NSEC_PER_SEC);
0817         pin_start = ns_to_sja1105_ticks(pin_start);
0818 
0819         rc = sja1105_xfer_u64(priv, SPI_WRITE, regs->ptppinst,
0820                       &pin_start, NULL);
0821         if (rc < 0)
0822             goto out;
0823 
0824         rc = sja1105_xfer_u32(priv, SPI_WRITE, regs->ptppindur,
0825                       &pin_duration32, NULL);
0826         if (rc < 0)
0827             goto out;
0828     }
0829 
0830     if (on)
0831         cmd.startptpcp = true;
0832     else
0833         cmd.stopptpcp = true;
0834 
0835     rc = sja1105_ptp_commit(priv->ds, &cmd, SPI_WRITE);
0836 
0837 out:
0838     mutex_unlock(&ptp_data->lock);
0839 
0840     return rc;
0841 }
0842 
0843 static int sja1105_extts_enable(struct sja1105_private *priv,
0844                 struct ptp_extts_request *extts,
0845                 bool on)
0846 {
0847     int rc;
0848 
0849     /* We only support one channel */
0850     if (extts->index != 0)
0851         return -EOPNOTSUPP;
0852 
0853     /* Reject requests with unsupported flags */
0854     if (extts->flags & ~(PTP_ENABLE_FEATURE |
0855                  PTP_RISING_EDGE |
0856                  PTP_FALLING_EDGE |
0857                  PTP_STRICT_FLAGS))
0858         return -EOPNOTSUPP;
0859 
0860     /* We can only enable time stamping on both edges, sadly. */
0861     if ((extts->flags & PTP_STRICT_FLAGS) &&
0862         (extts->flags & PTP_ENABLE_FEATURE) &&
0863         (extts->flags & PTP_EXTTS_EDGES) != PTP_EXTTS_EDGES)
0864         return -EOPNOTSUPP;
0865 
0866     rc = sja1105_change_ptp_clk_pin_func(priv, PTP_PF_EXTTS);
0867     if (rc)
0868         return rc;
0869 
0870     priv->ptp_data.extts_enabled = on;
0871 
0872     if (on)
0873         sja1105_ptp_extts_setup_timer(&priv->ptp_data);
0874     else
0875         del_timer_sync(&priv->ptp_data.extts_timer);
0876 
0877     return 0;
0878 }
0879 
0880 static int sja1105_ptp_enable(struct ptp_clock_info *ptp,
0881                   struct ptp_clock_request *req, int on)
0882 {
0883     struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
0884     struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
0885     int rc = -EOPNOTSUPP;
0886 
0887     if (req->type == PTP_CLK_REQ_PEROUT)
0888         rc = sja1105_per_out_enable(priv, &req->perout, on);
0889     else if (req->type == PTP_CLK_REQ_EXTTS)
0890         rc = sja1105_extts_enable(priv, &req->extts, on);
0891 
0892     return rc;
0893 }
0894 
0895 static int sja1105_ptp_verify_pin(struct ptp_clock_info *ptp, unsigned int pin,
0896                   enum ptp_pin_function func, unsigned int chan)
0897 {
0898     struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
0899     struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
0900 
0901     if (chan != 0 || pin != 0)
0902         return -1;
0903 
0904     switch (func) {
0905     case PTP_PF_NONE:
0906     case PTP_PF_PEROUT:
0907         break;
0908     case PTP_PF_EXTTS:
0909         if (priv->info->device_id == SJA1105E_DEVICE_ID ||
0910             priv->info->device_id == SJA1105T_DEVICE_ID)
0911             return -1;
0912         break;
0913     default:
0914         return -1;
0915     }
0916     return 0;
0917 }
0918 
0919 static struct ptp_pin_desc sja1105_ptp_pin = {
0920     .name = "ptp_clk",
0921     .index = 0,
0922     .func = PTP_PF_NONE,
0923 };
0924 
0925 int sja1105_ptp_clock_register(struct dsa_switch *ds)
0926 {
0927     struct sja1105_private *priv = ds->priv;
0928     struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
0929 
0930     ptp_data->caps = (struct ptp_clock_info) {
0931         .owner      = THIS_MODULE,
0932         .name       = "SJA1105 PHC",
0933         .adjfine    = sja1105_ptp_adjfine,
0934         .adjtime    = sja1105_ptp_adjtime,
0935         .gettimex64 = sja1105_ptp_gettimex,
0936         .settime64  = sja1105_ptp_settime,
0937         .enable     = sja1105_ptp_enable,
0938         .verify     = sja1105_ptp_verify_pin,
0939         .do_aux_work    = sja1105_rxtstamp_work,
0940         .max_adj    = SJA1105_MAX_ADJ_PPB,
0941         .pin_config = &sja1105_ptp_pin,
0942         .n_pins     = 1,
0943         .n_ext_ts   = 1,
0944         .n_per_out  = 1,
0945     };
0946 
0947     /* Only used on SJA1105 */
0948     skb_queue_head_init(&ptp_data->skb_rxtstamp_queue);
0949     /* Only used on SJA1110 */
0950     skb_queue_head_init(&ptp_data->skb_txtstamp_queue);
0951 
0952     ptp_data->clock = ptp_clock_register(&ptp_data->caps, ds->dev);
0953     if (IS_ERR_OR_NULL(ptp_data->clock))
0954         return PTR_ERR(ptp_data->clock);
0955 
0956     ptp_data->cmd.corrclk4ts = true;
0957     ptp_data->cmd.ptpclkadd = PTP_SET_MODE;
0958 
0959     timer_setup(&ptp_data->extts_timer, sja1105_ptp_extts_timer, 0);
0960 
0961     return sja1105_ptp_reset(ds);
0962 }
0963 
0964 void sja1105_ptp_clock_unregister(struct dsa_switch *ds)
0965 {
0966     struct sja1105_private *priv = ds->priv;
0967     struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
0968 
0969     if (IS_ERR_OR_NULL(ptp_data->clock))
0970         return;
0971 
0972     del_timer_sync(&ptp_data->extts_timer);
0973     ptp_cancel_worker_sync(ptp_data->clock);
0974     skb_queue_purge(&ptp_data->skb_txtstamp_queue);
0975     skb_queue_purge(&ptp_data->skb_rxtstamp_queue);
0976     ptp_clock_unregister(ptp_data->clock);
0977     ptp_data->clock = NULL;
0978 }
0979 
0980 void sja1105_ptp_txtstamp_skb(struct dsa_switch *ds, int port,
0981                   struct sk_buff *skb)
0982 {
0983     struct sja1105_private *priv = ds->priv;
0984     struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
0985     struct skb_shared_hwtstamps shwt = {0};
0986     u64 ticks, ts;
0987     int rc;
0988 
0989     skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
0990 
0991     mutex_lock(&ptp_data->lock);
0992 
0993     rc = sja1105_ptpegr_ts_poll(ds, port, &ts);
0994     if (rc < 0) {
0995         dev_err(ds->dev, "timed out polling for tstamp\n");
0996         kfree_skb(skb);
0997         goto out;
0998     }
0999 
1000     rc = sja1105_ptpclkval_read(priv, &ticks, NULL);
1001     if (rc < 0) {
1002         dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
1003         kfree_skb(skb);
1004         goto out;
1005     }
1006 
1007     ts = sja1105_tstamp_reconstruct(ds, ticks, ts);
1008 
1009     shwt.hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
1010     skb_complete_tx_timestamp(skb, &shwt);
1011 
1012 out:
1013     mutex_unlock(&ptp_data->lock);
1014 }
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!