ptp_ines.c 19 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807
  1. // SPDX-License-Identifier: GPL-2.0
  2. //
  3. // Copyright (C) 2018 MOSER-BAER AG
  4. //
  5. #define pr_fmt(fmt) "InES_PTP: " fmt
  6. #include <linux/ethtool.h>
  7. #include <linux/export.h>
  8. #include <linux/if_vlan.h>
  9. #include <linux/mii_timestamper.h>
  10. #include <linux/module.h>
  11. #include <linux/net_tstamp.h>
  12. #include <linux/of.h>
  13. #include <linux/of_address.h>
  14. #include <linux/of_irq.h>
  15. #include <linux/phy.h>
  16. #include <linux/platform_device.h>
  17. #include <linux/ptp_classify.h>
  18. #include <linux/ptp_clock_kernel.h>
  19. #include <linux/stddef.h>
  20. MODULE_DESCRIPTION("Driver for the ZHAW InES PTP time stamping IP core");
  21. MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>");
  22. MODULE_VERSION("1.0");
  23. MODULE_LICENSE("GPL");
  24. /* GLOBAL register */
  25. #define MCAST_MAC_SELECT_SHIFT 2
  26. #define MCAST_MAC_SELECT_MASK 0x3
  27. #define IO_RESET BIT(1)
  28. #define PTP_RESET BIT(0)
  29. /* VERSION register */
  30. #define IF_MAJOR_VER_SHIFT 12
  31. #define IF_MAJOR_VER_MASK 0xf
  32. #define IF_MINOR_VER_SHIFT 8
  33. #define IF_MINOR_VER_MASK 0xf
  34. #define FPGA_MAJOR_VER_SHIFT 4
  35. #define FPGA_MAJOR_VER_MASK 0xf
  36. #define FPGA_MINOR_VER_SHIFT 0
  37. #define FPGA_MINOR_VER_MASK 0xf
  38. /* INT_STAT register */
  39. #define RX_INTR_STATUS_3 BIT(5)
  40. #define RX_INTR_STATUS_2 BIT(4)
  41. #define RX_INTR_STATUS_1 BIT(3)
  42. #define TX_INTR_STATUS_3 BIT(2)
  43. #define TX_INTR_STATUS_2 BIT(1)
  44. #define TX_INTR_STATUS_1 BIT(0)
  45. /* INT_MSK register */
  46. #define RX_INTR_MASK_3 BIT(5)
  47. #define RX_INTR_MASK_2 BIT(4)
  48. #define RX_INTR_MASK_1 BIT(3)
  49. #define TX_INTR_MASK_3 BIT(2)
  50. #define TX_INTR_MASK_2 BIT(1)
  51. #define TX_INTR_MASK_1 BIT(0)
  52. /* BUF_STAT register */
  53. #define RX_FIFO_NE_3 BIT(5)
  54. #define RX_FIFO_NE_2 BIT(4)
  55. #define RX_FIFO_NE_1 BIT(3)
  56. #define TX_FIFO_NE_3 BIT(2)
  57. #define TX_FIFO_NE_2 BIT(1)
  58. #define TX_FIFO_NE_1 BIT(0)
  59. /* PORT_CONF register */
  60. #define CM_ONE_STEP BIT(6)
  61. #define PHY_SPEED_SHIFT 4
  62. #define PHY_SPEED_MASK 0x3
  63. #define P2P_DELAY_WR_POS_SHIFT 2
  64. #define P2P_DELAY_WR_POS_MASK 0x3
  65. #define PTP_MODE_SHIFT 0
  66. #define PTP_MODE_MASK 0x3
  67. /* TS_STAT_TX register */
  68. #define TS_ENABLE BIT(15)
  69. #define DATA_READ_POS_SHIFT 8
  70. #define DATA_READ_POS_MASK 0x1f
  71. #define DISCARDED_EVENTS_SHIFT 4
  72. #define DISCARDED_EVENTS_MASK 0xf
  73. #define INES_N_PORTS 3
  74. #define INES_REGISTER_SIZE 0x80
  75. #define INES_PORT_OFFSET 0x20
  76. #define INES_PORT_SIZE 0x20
  77. #define INES_FIFO_DEPTH 90
  78. #define INES_MAX_EVENTS 100
  79. #define BC_PTP_V1 0
  80. #define BC_PTP_V2 1
  81. #define TC_E2E_PTP_V2 2
  82. #define TC_P2P_PTP_V2 3
  83. #define PHY_SPEED_10 0
  84. #define PHY_SPEED_100 1
  85. #define PHY_SPEED_1000 2
  86. #define PORT_CONF \
  87. ((PHY_SPEED_1000 << PHY_SPEED_SHIFT) | (BC_PTP_V2 << PTP_MODE_SHIFT))
  88. #define ines_read32(s, r) __raw_readl((void __iomem *)&s->regs->r)
  89. #define ines_write32(s, v, r) __raw_writel(v, (void __iomem *)&s->regs->r)
  90. #define MESSAGE_TYPE_SYNC 1
  91. #define MESSAGE_TYPE_P_DELAY_REQ 2
  92. #define MESSAGE_TYPE_P_DELAY_RESP 3
  93. #define MESSAGE_TYPE_DELAY_REQ 4
  94. #define SYNC 0x0
  95. #define DELAY_REQ 0x1
  96. #define PDELAY_REQ 0x2
  97. #define PDELAY_RESP 0x3
  98. static LIST_HEAD(ines_clocks);
  99. static DEFINE_MUTEX(ines_clocks_lock);
  100. struct ines_global_regs {
  101. u32 id;
  102. u32 test;
  103. u32 global;
  104. u32 version;
  105. u32 test2;
  106. u32 int_stat;
  107. u32 int_msk;
  108. u32 buf_stat;
  109. };
  110. struct ines_port_registers {
  111. u32 port_conf;
  112. u32 p_delay;
  113. u32 ts_stat_tx;
  114. u32 ts_stat_rx;
  115. u32 ts_tx;
  116. u32 ts_rx;
  117. };
  118. struct ines_timestamp {
  119. struct list_head list;
  120. unsigned long tmo;
  121. u16 tag;
  122. u64 sec;
  123. u64 nsec;
  124. u64 clkid;
  125. u16 portnum;
  126. u16 seqid;
  127. };
  128. struct ines_port {
  129. struct ines_port_registers *regs;
  130. struct mii_timestamper mii_ts;
  131. struct ines_clock *clock;
  132. bool rxts_enabled;
  133. bool txts_enabled;
  134. unsigned int index;
  135. struct delayed_work ts_work;
  136. /* lock protects event list and tx_skb */
  137. spinlock_t lock;
  138. struct sk_buff *tx_skb;
  139. struct list_head events;
  140. struct list_head pool;
  141. struct ines_timestamp pool_data[INES_MAX_EVENTS];
  142. };
  143. struct ines_clock {
  144. struct ines_port port[INES_N_PORTS];
  145. struct ines_global_regs __iomem *regs;
  146. void __iomem *base;
  147. struct device_node *node;
  148. struct device *dev;
  149. struct list_head list;
  150. };
  151. static bool ines_match(struct sk_buff *skb, unsigned int ptp_class,
  152. struct ines_timestamp *ts, struct device *dev);
  153. static int ines_rxfifo_read(struct ines_port *port);
  154. static u64 ines_rxts64(struct ines_port *port, unsigned int words);
  155. static bool ines_timestamp_expired(struct ines_timestamp *ts);
  156. static u64 ines_txts64(struct ines_port *port, unsigned int words);
  157. static void ines_txtstamp_work(struct work_struct *work);
  158. static bool is_sync_pdelay_resp(struct sk_buff *skb, int type);
  159. static u8 tag_to_msgtype(u8 tag);
  160. static void ines_clock_cleanup(struct ines_clock *clock)
  161. {
  162. struct ines_port *port;
  163. int i;
  164. for (i = 0; i < INES_N_PORTS; i++) {
  165. port = &clock->port[i];
  166. cancel_delayed_work_sync(&port->ts_work);
  167. }
  168. }
  169. static int ines_clock_init(struct ines_clock *clock, struct device *device,
  170. void __iomem *addr)
  171. {
  172. struct device_node *node = device->of_node;
  173. unsigned long port_addr;
  174. struct ines_port *port;
  175. int i, j;
  176. INIT_LIST_HEAD(&clock->list);
  177. clock->node = node;
  178. clock->dev = device;
  179. clock->base = addr;
  180. clock->regs = clock->base;
  181. for (i = 0; i < INES_N_PORTS; i++) {
  182. port = &clock->port[i];
  183. port_addr = (unsigned long) clock->base +
  184. INES_PORT_OFFSET + i * INES_PORT_SIZE;
  185. port->regs = (struct ines_port_registers *) port_addr;
  186. port->clock = clock;
  187. port->index = i;
  188. INIT_DELAYED_WORK(&port->ts_work, ines_txtstamp_work);
  189. spin_lock_init(&port->lock);
  190. INIT_LIST_HEAD(&port->events);
  191. INIT_LIST_HEAD(&port->pool);
  192. for (j = 0; j < INES_MAX_EVENTS; j++)
  193. list_add(&port->pool_data[j].list, &port->pool);
  194. }
  195. ines_write32(clock, 0xBEEF, test);
  196. ines_write32(clock, 0xBEEF, test2);
  197. dev_dbg(device, "ID 0x%x\n", ines_read32(clock, id));
  198. dev_dbg(device, "TEST 0x%x\n", ines_read32(clock, test));
  199. dev_dbg(device, "VERSION 0x%x\n", ines_read32(clock, version));
  200. dev_dbg(device, "TEST2 0x%x\n", ines_read32(clock, test2));
  201. for (i = 0; i < INES_N_PORTS; i++) {
  202. port = &clock->port[i];
  203. ines_write32(port, PORT_CONF, port_conf);
  204. }
  205. return 0;
  206. }
  207. static struct ines_port *ines_find_port(struct device_node *node, u32 index)
  208. {
  209. struct ines_port *port = NULL;
  210. struct ines_clock *clock;
  211. struct list_head *this;
  212. mutex_lock(&ines_clocks_lock);
  213. list_for_each(this, &ines_clocks) {
  214. clock = list_entry(this, struct ines_clock, list);
  215. if (clock->node == node) {
  216. port = &clock->port[index];
  217. break;
  218. }
  219. }
  220. mutex_unlock(&ines_clocks_lock);
  221. return port;
  222. }
  223. static u64 ines_find_rxts(struct ines_port *port, struct sk_buff *skb, int type)
  224. {
  225. struct list_head *this, *next;
  226. struct ines_timestamp *ts;
  227. unsigned long flags;
  228. u64 ns = 0;
  229. if (type == PTP_CLASS_NONE)
  230. return 0;
  231. spin_lock_irqsave(&port->lock, flags);
  232. ines_rxfifo_read(port);
  233. list_for_each_safe(this, next, &port->events) {
  234. ts = list_entry(this, struct ines_timestamp, list);
  235. if (ines_timestamp_expired(ts)) {
  236. list_del_init(&ts->list);
  237. list_add(&ts->list, &port->pool);
  238. continue;
  239. }
  240. if (ines_match(skb, type, ts, port->clock->dev)) {
  241. ns = ts->sec * 1000000000ULL + ts->nsec;
  242. list_del_init(&ts->list);
  243. list_add(&ts->list, &port->pool);
  244. break;
  245. }
  246. }
  247. spin_unlock_irqrestore(&port->lock, flags);
  248. return ns;
  249. }
  250. static u64 ines_find_txts(struct ines_port *port, struct sk_buff *skb)
  251. {
  252. unsigned int class = ptp_classify_raw(skb), i;
  253. u32 data_rd_pos, buf_stat, mask, ts_stat_tx;
  254. struct ines_timestamp ts;
  255. unsigned long flags;
  256. u64 ns = 0;
  257. mask = TX_FIFO_NE_1 << port->index;
  258. spin_lock_irqsave(&port->lock, flags);
  259. for (i = 0; i < INES_FIFO_DEPTH; i++) {
  260. buf_stat = ines_read32(port->clock, buf_stat);
  261. if (!(buf_stat & mask)) {
  262. dev_dbg(port->clock->dev,
  263. "Tx timestamp FIFO unexpectedly empty\n");
  264. break;
  265. }
  266. ts_stat_tx = ines_read32(port, ts_stat_tx);
  267. data_rd_pos = (ts_stat_tx >> DATA_READ_POS_SHIFT) &
  268. DATA_READ_POS_MASK;
  269. if (data_rd_pos) {
  270. dev_err(port->clock->dev,
  271. "unexpected Tx read pos %u\n", data_rd_pos);
  272. break;
  273. }
  274. ts.tag = ines_read32(port, ts_tx);
  275. ts.sec = ines_txts64(port, 3);
  276. ts.nsec = ines_txts64(port, 2);
  277. ts.clkid = ines_txts64(port, 4);
  278. ts.portnum = ines_read32(port, ts_tx);
  279. ts.seqid = ines_read32(port, ts_tx);
  280. if (ines_match(skb, class, &ts, port->clock->dev)) {
  281. ns = ts.sec * 1000000000ULL + ts.nsec;
  282. break;
  283. }
  284. }
  285. spin_unlock_irqrestore(&port->lock, flags);
  286. return ns;
  287. }
  288. static int ines_hwtstamp(struct mii_timestamper *mii_ts, struct ifreq *ifr)
  289. {
  290. struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
  291. u32 cm_one_step = 0, port_conf, ts_stat_rx, ts_stat_tx;
  292. struct hwtstamp_config cfg;
  293. unsigned long flags;
  294. if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
  295. return -EFAULT;
  296. /* reserved for future extensions */
  297. if (cfg.flags)
  298. return -EINVAL;
  299. switch (cfg.tx_type) {
  300. case HWTSTAMP_TX_OFF:
  301. ts_stat_tx = 0;
  302. break;
  303. case HWTSTAMP_TX_ON:
  304. ts_stat_tx = TS_ENABLE;
  305. break;
  306. case HWTSTAMP_TX_ONESTEP_P2P:
  307. ts_stat_tx = TS_ENABLE;
  308. cm_one_step = CM_ONE_STEP;
  309. break;
  310. default:
  311. return -ERANGE;
  312. }
  313. switch (cfg.rx_filter) {
  314. case HWTSTAMP_FILTER_NONE:
  315. ts_stat_rx = 0;
  316. break;
  317. case HWTSTAMP_FILTER_ALL:
  318. case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
  319. case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
  320. case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
  321. return -ERANGE;
  322. case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
  323. case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
  324. case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
  325. case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
  326. case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
  327. case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
  328. case HWTSTAMP_FILTER_PTP_V2_EVENT:
  329. case HWTSTAMP_FILTER_PTP_V2_SYNC:
  330. case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
  331. ts_stat_rx = TS_ENABLE;
  332. cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
  333. break;
  334. default:
  335. return -ERANGE;
  336. }
  337. spin_lock_irqsave(&port->lock, flags);
  338. port_conf = ines_read32(port, port_conf);
  339. port_conf &= ~CM_ONE_STEP;
  340. port_conf |= cm_one_step;
  341. ines_write32(port, port_conf, port_conf);
  342. ines_write32(port, ts_stat_rx, ts_stat_rx);
  343. ines_write32(port, ts_stat_tx, ts_stat_tx);
  344. port->rxts_enabled = ts_stat_rx == TS_ENABLE;
  345. port->txts_enabled = ts_stat_tx == TS_ENABLE;
  346. spin_unlock_irqrestore(&port->lock, flags);
  347. return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
  348. }
  349. static void ines_link_state(struct mii_timestamper *mii_ts,
  350. struct phy_device *phydev)
  351. {
  352. struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
  353. u32 port_conf, speed_conf;
  354. unsigned long flags;
  355. switch (phydev->speed) {
  356. case SPEED_10:
  357. speed_conf = PHY_SPEED_10 << PHY_SPEED_SHIFT;
  358. break;
  359. case SPEED_100:
  360. speed_conf = PHY_SPEED_100 << PHY_SPEED_SHIFT;
  361. break;
  362. case SPEED_1000:
  363. speed_conf = PHY_SPEED_1000 << PHY_SPEED_SHIFT;
  364. break;
  365. default:
  366. dev_err(port->clock->dev, "bad speed: %d\n", phydev->speed);
  367. return;
  368. }
  369. spin_lock_irqsave(&port->lock, flags);
  370. port_conf = ines_read32(port, port_conf);
  371. port_conf &= ~(0x3 << PHY_SPEED_SHIFT);
  372. port_conf |= speed_conf;
  373. ines_write32(port, port_conf, port_conf);
  374. spin_unlock_irqrestore(&port->lock, flags);
  375. }
  376. static bool ines_match(struct sk_buff *skb, unsigned int ptp_class,
  377. struct ines_timestamp *ts, struct device *dev)
  378. {
  379. struct ptp_header *hdr;
  380. u16 portn, seqid;
  381. u8 msgtype;
  382. u64 clkid;
  383. if (unlikely(ptp_class & PTP_CLASS_V1))
  384. return false;
  385. hdr = ptp_parse_header(skb, ptp_class);
  386. if (!hdr)
  387. return false;
  388. msgtype = ptp_get_msgtype(hdr, ptp_class);
  389. clkid = be64_to_cpup((__be64 *)&hdr->source_port_identity.clock_identity.id[0]);
  390. portn = be16_to_cpu(hdr->source_port_identity.port_number);
  391. seqid = be16_to_cpu(hdr->sequence_id);
  392. if (tag_to_msgtype(ts->tag & 0x7) != msgtype) {
  393. dev_dbg(dev, "msgtype mismatch ts %hhu != skb %hhu\n",
  394. tag_to_msgtype(ts->tag & 0x7), msgtype);
  395. return false;
  396. }
  397. if (ts->clkid != clkid) {
  398. dev_dbg(dev, "clkid mismatch ts %llx != skb %llx\n",
  399. ts->clkid, clkid);
  400. return false;
  401. }
  402. if (ts->portnum != portn) {
  403. dev_dbg(dev, "portn mismatch ts %hu != skb %hu\n",
  404. ts->portnum, portn);
  405. return false;
  406. }
  407. if (ts->seqid != seqid) {
  408. dev_dbg(dev, "seqid mismatch ts %hu != skb %hu\n",
  409. ts->seqid, seqid);
  410. return false;
  411. }
  412. return true;
  413. }
  414. static bool ines_rxtstamp(struct mii_timestamper *mii_ts,
  415. struct sk_buff *skb, int type)
  416. {
  417. struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
  418. struct skb_shared_hwtstamps *ssh;
  419. u64 ns;
  420. if (!port->rxts_enabled)
  421. return false;
  422. ns = ines_find_rxts(port, skb, type);
  423. if (!ns)
  424. return false;
  425. ssh = skb_hwtstamps(skb);
  426. ssh->hwtstamp = ns_to_ktime(ns);
  427. netif_rx(skb);
  428. return true;
  429. }
  430. static int ines_rxfifo_read(struct ines_port *port)
  431. {
  432. u32 data_rd_pos, buf_stat, mask, ts_stat_rx;
  433. struct ines_timestamp *ts;
  434. unsigned int i;
  435. mask = RX_FIFO_NE_1 << port->index;
  436. for (i = 0; i < INES_FIFO_DEPTH; i++) {
  437. if (list_empty(&port->pool)) {
  438. dev_err(port->clock->dev, "event pool is empty\n");
  439. return -1;
  440. }
  441. buf_stat = ines_read32(port->clock, buf_stat);
  442. if (!(buf_stat & mask))
  443. break;
  444. ts_stat_rx = ines_read32(port, ts_stat_rx);
  445. data_rd_pos = (ts_stat_rx >> DATA_READ_POS_SHIFT) &
  446. DATA_READ_POS_MASK;
  447. if (data_rd_pos) {
  448. dev_err(port->clock->dev, "unexpected Rx read pos %u\n",
  449. data_rd_pos);
  450. break;
  451. }
  452. ts = list_first_entry(&port->pool, struct ines_timestamp, list);
  453. ts->tmo = jiffies + HZ;
  454. ts->tag = ines_read32(port, ts_rx);
  455. ts->sec = ines_rxts64(port, 3);
  456. ts->nsec = ines_rxts64(port, 2);
  457. ts->clkid = ines_rxts64(port, 4);
  458. ts->portnum = ines_read32(port, ts_rx);
  459. ts->seqid = ines_read32(port, ts_rx);
  460. list_del_init(&ts->list);
  461. list_add_tail(&ts->list, &port->events);
  462. }
  463. return 0;
  464. }
  465. static u64 ines_rxts64(struct ines_port *port, unsigned int words)
  466. {
  467. unsigned int i;
  468. u64 result;
  469. u16 word;
  470. word = ines_read32(port, ts_rx);
  471. result = word;
  472. words--;
  473. for (i = 0; i < words; i++) {
  474. word = ines_read32(port, ts_rx);
  475. result <<= 16;
  476. result |= word;
  477. }
  478. return result;
  479. }
  480. static bool ines_timestamp_expired(struct ines_timestamp *ts)
  481. {
  482. return time_after(jiffies, ts->tmo);
  483. }
  484. static int ines_ts_info(struct mii_timestamper *mii_ts,
  485. struct ethtool_ts_info *info)
  486. {
  487. info->so_timestamping =
  488. SOF_TIMESTAMPING_TX_HARDWARE |
  489. SOF_TIMESTAMPING_TX_SOFTWARE |
  490. SOF_TIMESTAMPING_RX_HARDWARE |
  491. SOF_TIMESTAMPING_RX_SOFTWARE |
  492. SOF_TIMESTAMPING_SOFTWARE |
  493. SOF_TIMESTAMPING_RAW_HARDWARE;
  494. info->phc_index = -1;
  495. info->tx_types =
  496. (1 << HWTSTAMP_TX_OFF) |
  497. (1 << HWTSTAMP_TX_ON) |
  498. (1 << HWTSTAMP_TX_ONESTEP_P2P);
  499. info->rx_filters =
  500. (1 << HWTSTAMP_FILTER_NONE) |
  501. (1 << HWTSTAMP_FILTER_PTP_V2_EVENT);
  502. return 0;
  503. }
  504. static u64 ines_txts64(struct ines_port *port, unsigned int words)
  505. {
  506. unsigned int i;
  507. u64 result;
  508. u16 word;
  509. word = ines_read32(port, ts_tx);
  510. result = word;
  511. words--;
  512. for (i = 0; i < words; i++) {
  513. word = ines_read32(port, ts_tx);
  514. result <<= 16;
  515. result |= word;
  516. }
  517. return result;
  518. }
  519. static bool ines_txts_onestep(struct ines_port *port, struct sk_buff *skb, int type)
  520. {
  521. unsigned long flags;
  522. u32 port_conf;
  523. spin_lock_irqsave(&port->lock, flags);
  524. port_conf = ines_read32(port, port_conf);
  525. spin_unlock_irqrestore(&port->lock, flags);
  526. if (port_conf & CM_ONE_STEP)
  527. return is_sync_pdelay_resp(skb, type);
  528. return false;
  529. }
  530. static void ines_txtstamp(struct mii_timestamper *mii_ts,
  531. struct sk_buff *skb, int type)
  532. {
  533. struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
  534. struct sk_buff *old_skb = NULL;
  535. unsigned long flags;
  536. if (!port->txts_enabled || ines_txts_onestep(port, skb, type)) {
  537. kfree_skb(skb);
  538. return;
  539. }
  540. spin_lock_irqsave(&port->lock, flags);
  541. if (port->tx_skb)
  542. old_skb = port->tx_skb;
  543. port->tx_skb = skb;
  544. spin_unlock_irqrestore(&port->lock, flags);
  545. kfree_skb(old_skb);
  546. schedule_delayed_work(&port->ts_work, 1);
  547. }
  548. static void ines_txtstamp_work(struct work_struct *work)
  549. {
  550. struct ines_port *port =
  551. container_of(work, struct ines_port, ts_work.work);
  552. struct skb_shared_hwtstamps ssh;
  553. struct sk_buff *skb;
  554. unsigned long flags;
  555. u64 ns;
  556. spin_lock_irqsave(&port->lock, flags);
  557. skb = port->tx_skb;
  558. port->tx_skb = NULL;
  559. spin_unlock_irqrestore(&port->lock, flags);
  560. ns = ines_find_txts(port, skb);
  561. if (!ns) {
  562. kfree_skb(skb);
  563. return;
  564. }
  565. ssh.hwtstamp = ns_to_ktime(ns);
  566. skb_complete_tx_timestamp(skb, &ssh);
  567. }
  568. static bool is_sync_pdelay_resp(struct sk_buff *skb, int type)
  569. {
  570. struct ptp_header *hdr;
  571. u8 msgtype;
  572. hdr = ptp_parse_header(skb, type);
  573. if (!hdr)
  574. return false;
  575. msgtype = ptp_get_msgtype(hdr, type);
  576. switch ((msgtype & 0xf)) {
  577. case SYNC:
  578. case PDELAY_RESP:
  579. return true;
  580. default:
  581. return false;
  582. }
  583. }
  584. static u8 tag_to_msgtype(u8 tag)
  585. {
  586. switch (tag) {
  587. case MESSAGE_TYPE_SYNC:
  588. return SYNC;
  589. case MESSAGE_TYPE_P_DELAY_REQ:
  590. return PDELAY_REQ;
  591. case MESSAGE_TYPE_P_DELAY_RESP:
  592. return PDELAY_RESP;
  593. case MESSAGE_TYPE_DELAY_REQ:
  594. return DELAY_REQ;
  595. }
  596. return 0xf;
  597. }
  598. static struct mii_timestamper *ines_ptp_probe_channel(struct device *device,
  599. unsigned int index)
  600. {
  601. struct device_node *node = device->of_node;
  602. struct ines_port *port;
  603. if (index > INES_N_PORTS - 1) {
  604. dev_err(device, "bad port index %u\n", index);
  605. return ERR_PTR(-EINVAL);
  606. }
  607. port = ines_find_port(node, index);
  608. if (!port) {
  609. dev_err(device, "missing port index %u\n", index);
  610. return ERR_PTR(-ENODEV);
  611. }
  612. port->mii_ts.rxtstamp = ines_rxtstamp;
  613. port->mii_ts.txtstamp = ines_txtstamp;
  614. port->mii_ts.hwtstamp = ines_hwtstamp;
  615. port->mii_ts.link_state = ines_link_state;
  616. port->mii_ts.ts_info = ines_ts_info;
  617. return &port->mii_ts;
  618. }
  619. static void ines_ptp_release_channel(struct device *device,
  620. struct mii_timestamper *mii_ts)
  621. {
  622. }
  623. static struct mii_timestamping_ctrl ines_ctrl = {
  624. .probe_channel = ines_ptp_probe_channel,
  625. .release_channel = ines_ptp_release_channel,
  626. };
  627. static int ines_ptp_ctrl_probe(struct platform_device *pld)
  628. {
  629. struct ines_clock *clock;
  630. void __iomem *addr;
  631. int err = 0;
  632. addr = devm_platform_ioremap_resource(pld, 0);
  633. if (IS_ERR(addr)) {
  634. err = PTR_ERR(addr);
  635. goto out;
  636. }
  637. clock = kzalloc(sizeof(*clock), GFP_KERNEL);
  638. if (!clock) {
  639. err = -ENOMEM;
  640. goto out;
  641. }
  642. if (ines_clock_init(clock, &pld->dev, addr)) {
  643. kfree(clock);
  644. err = -ENOMEM;
  645. goto out;
  646. }
  647. err = register_mii_tstamp_controller(&pld->dev, &ines_ctrl);
  648. if (err) {
  649. kfree(clock);
  650. goto out;
  651. }
  652. mutex_lock(&ines_clocks_lock);
  653. list_add_tail(&ines_clocks, &clock->list);
  654. mutex_unlock(&ines_clocks_lock);
  655. dev_set_drvdata(&pld->dev, clock);
  656. out:
  657. return err;
  658. }
  659. static int ines_ptp_ctrl_remove(struct platform_device *pld)
  660. {
  661. struct ines_clock *clock = dev_get_drvdata(&pld->dev);
  662. unregister_mii_tstamp_controller(&pld->dev);
  663. mutex_lock(&ines_clocks_lock);
  664. list_del(&clock->list);
  665. mutex_unlock(&ines_clocks_lock);
  666. ines_clock_cleanup(clock);
  667. kfree(clock);
  668. return 0;
  669. }
  670. static const struct of_device_id ines_ptp_ctrl_of_match[] = {
  671. { .compatible = "ines,ptp-ctrl" },
  672. { }
  673. };
  674. MODULE_DEVICE_TABLE(of, ines_ptp_ctrl_of_match);
  675. static struct platform_driver ines_ptp_ctrl_driver = {
  676. .probe = ines_ptp_ctrl_probe,
  677. .remove = ines_ptp_ctrl_remove,
  678. .driver = {
  679. .name = "ines_ptp_ctrl",
  680. .of_match_table = of_match_ptr(ines_ptp_ctrl_of_match),
  681. },
  682. };
  683. module_platform_driver(ines_ptp_ctrl_driver);