hwtstamp.c 17 KB

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