dp83640.c 37 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535
  1. /*
  2. * Driver for the National Semiconductor DP83640 PHYTER
  3. *
  4. * Copyright (C) 2010 OMICRON electronics GmbH
  5. *
  6. * This program is free software; you can redistribute it and/or modify
  7. * it under the terms of the GNU General Public License as published by
  8. * the Free Software Foundation; either version 2 of the License, or
  9. * (at your option) any later version.
  10. *
  11. * This program is distributed in the hope that it will be useful,
  12. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  14. * GNU General Public License for more details.
  15. *
  16. * You should have received a copy of the GNU General Public License
  17. * along with this program; if not, write to the Free Software
  18. * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  19. */
  20. #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
  21. #include <linux/crc32.h>
  22. #include <linux/ethtool.h>
  23. #include <linux/kernel.h>
  24. #include <linux/list.h>
  25. #include <linux/mii.h>
  26. #include <linux/module.h>
  27. #include <linux/net_tstamp.h>
  28. #include <linux/netdevice.h>
  29. #include <linux/if_vlan.h>
  30. #include <linux/phy.h>
  31. #include <linux/ptp_classify.h>
  32. #include <linux/ptp_clock_kernel.h>
  33. #include "dp83640_reg.h"
  34. #define DP83640_PHY_ID 0x20005ce1
  35. #define PAGESEL 0x13
  36. #define MAX_RXTS 64
  37. #define N_EXT_TS 6
  38. #define N_PER_OUT 7
  39. #define PSF_PTPVER 2
  40. #define PSF_EVNT 0x4000
  41. #define PSF_RX 0x2000
  42. #define PSF_TX 0x1000
  43. #define EXT_EVENT 1
  44. #define CAL_EVENT 7
  45. #define CAL_TRIGGER 1
  46. #define DP83640_N_PINS 12
  47. #define MII_DP83640_MICR 0x11
  48. #define MII_DP83640_MISR 0x12
  49. #define MII_DP83640_MICR_OE 0x1
  50. #define MII_DP83640_MICR_IE 0x2
  51. #define MII_DP83640_MISR_RHF_INT_EN 0x01
  52. #define MII_DP83640_MISR_FHF_INT_EN 0x02
  53. #define MII_DP83640_MISR_ANC_INT_EN 0x04
  54. #define MII_DP83640_MISR_DUP_INT_EN 0x08
  55. #define MII_DP83640_MISR_SPD_INT_EN 0x10
  56. #define MII_DP83640_MISR_LINK_INT_EN 0x20
  57. #define MII_DP83640_MISR_ED_INT_EN 0x40
  58. #define MII_DP83640_MISR_LQ_INT_EN 0x80
  59. /* phyter seems to miss the mark by 16 ns */
  60. #define ADJTIME_FIX 16
  61. #define SKB_TIMESTAMP_TIMEOUT 2 /* jiffies */
  62. #if defined(__BIG_ENDIAN)
  63. #define ENDIAN_FLAG 0
  64. #elif defined(__LITTLE_ENDIAN)
  65. #define ENDIAN_FLAG PSF_ENDIAN
  66. #endif
  67. struct dp83640_skb_info {
  68. int ptp_type;
  69. unsigned long tmo;
  70. };
  71. struct phy_rxts {
  72. u16 ns_lo; /* ns[15:0] */
  73. u16 ns_hi; /* overflow[1:0], ns[29:16] */
  74. u16 sec_lo; /* sec[15:0] */
  75. u16 sec_hi; /* sec[31:16] */
  76. u16 seqid; /* sequenceId[15:0] */
  77. u16 msgtype; /* messageType[3:0], hash[11:0] */
  78. };
  79. struct phy_txts {
  80. u16 ns_lo; /* ns[15:0] */
  81. u16 ns_hi; /* overflow[1:0], ns[29:16] */
  82. u16 sec_lo; /* sec[15:0] */
  83. u16 sec_hi; /* sec[31:16] */
  84. };
  85. struct rxts {
  86. struct list_head list;
  87. unsigned long tmo;
  88. u64 ns;
  89. u16 seqid;
  90. u8 msgtype;
  91. u16 hash;
  92. };
  93. struct dp83640_clock;
  94. struct dp83640_private {
  95. struct list_head list;
  96. struct dp83640_clock *clock;
  97. struct phy_device *phydev;
  98. struct delayed_work ts_work;
  99. int hwts_tx_en;
  100. int hwts_rx_en;
  101. int layer;
  102. int version;
  103. /* remember state of cfg0 during calibration */
  104. int cfg0;
  105. /* remember the last event time stamp */
  106. struct phy_txts edata;
  107. /* list of rx timestamps */
  108. struct list_head rxts;
  109. struct list_head rxpool;
  110. struct rxts rx_pool_data[MAX_RXTS];
  111. /* protects above three fields from concurrent access */
  112. spinlock_t rx_lock;
  113. /* queues of incoming and outgoing packets */
  114. struct sk_buff_head rx_queue;
  115. struct sk_buff_head tx_queue;
  116. };
  117. struct dp83640_clock {
  118. /* keeps the instance in the 'phyter_clocks' list */
  119. struct list_head list;
  120. /* we create one clock instance per MII bus */
  121. struct mii_bus *bus;
  122. /* protects extended registers from concurrent access */
  123. struct mutex extreg_lock;
  124. /* remembers which page was last selected */
  125. int page;
  126. /* our advertised capabilities */
  127. struct ptp_clock_info caps;
  128. /* protects the three fields below from concurrent access */
  129. struct mutex clock_lock;
  130. /* the one phyter from which we shall read */
  131. struct dp83640_private *chosen;
  132. /* list of the other attached phyters, not chosen */
  133. struct list_head phylist;
  134. /* reference to our PTP hardware clock */
  135. struct ptp_clock *ptp_clock;
  136. };
  137. /* globals */
  138. enum {
  139. CALIBRATE_GPIO,
  140. PEROUT_GPIO,
  141. EXTTS0_GPIO,
  142. EXTTS1_GPIO,
  143. EXTTS2_GPIO,
  144. EXTTS3_GPIO,
  145. EXTTS4_GPIO,
  146. EXTTS5_GPIO,
  147. GPIO_TABLE_SIZE
  148. };
  149. static int chosen_phy = -1;
  150. static ushort gpio_tab[GPIO_TABLE_SIZE] = {
  151. 1, 2, 3, 4, 8, 9, 10, 11
  152. };
  153. module_param(chosen_phy, int, 0444);
  154. module_param_array(gpio_tab, ushort, NULL, 0444);
  155. MODULE_PARM_DESC(chosen_phy, \
  156. "The address of the PHY to use for the ancillary clock features");
  157. MODULE_PARM_DESC(gpio_tab, \
  158. "Which GPIO line to use for which purpose: cal,perout,extts1,...,extts6");
  159. static void dp83640_gpio_defaults(struct ptp_pin_desc *pd)
  160. {
  161. int i, index;
  162. for (i = 0; i < DP83640_N_PINS; i++) {
  163. snprintf(pd[i].name, sizeof(pd[i].name), "GPIO%d", 1 + i);
  164. pd[i].index = i;
  165. }
  166. for (i = 0; i < GPIO_TABLE_SIZE; i++) {
  167. if (gpio_tab[i] < 1 || gpio_tab[i] > DP83640_N_PINS) {
  168. pr_err("gpio_tab[%d]=%hu out of range", i, gpio_tab[i]);
  169. return;
  170. }
  171. }
  172. index = gpio_tab[CALIBRATE_GPIO] - 1;
  173. pd[index].func = PTP_PF_PHYSYNC;
  174. pd[index].chan = 0;
  175. index = gpio_tab[PEROUT_GPIO] - 1;
  176. pd[index].func = PTP_PF_PEROUT;
  177. pd[index].chan = 0;
  178. for (i = EXTTS0_GPIO; i < GPIO_TABLE_SIZE; i++) {
  179. index = gpio_tab[i] - 1;
  180. pd[index].func = PTP_PF_EXTTS;
  181. pd[index].chan = i - EXTTS0_GPIO;
  182. }
  183. }
  184. /* a list of clocks and a mutex to protect it */
  185. static LIST_HEAD(phyter_clocks);
  186. static DEFINE_MUTEX(phyter_clocks_lock);
  187. static void rx_timestamp_work(struct work_struct *work);
  188. /* extended register access functions */
  189. #define BROADCAST_ADDR 31
  190. static inline int broadcast_write(struct phy_device *phydev, u32 regnum,
  191. u16 val)
  192. {
  193. return mdiobus_write(phydev->mdio.bus, BROADCAST_ADDR, regnum, val);
  194. }
  195. /* Caller must hold extreg_lock. */
  196. static int ext_read(struct phy_device *phydev, int page, u32 regnum)
  197. {
  198. struct dp83640_private *dp83640 = phydev->priv;
  199. int val;
  200. if (dp83640->clock->page != page) {
  201. broadcast_write(phydev, PAGESEL, page);
  202. dp83640->clock->page = page;
  203. }
  204. val = phy_read(phydev, regnum);
  205. return val;
  206. }
  207. /* Caller must hold extreg_lock. */
  208. static void ext_write(int broadcast, struct phy_device *phydev,
  209. int page, u32 regnum, u16 val)
  210. {
  211. struct dp83640_private *dp83640 = phydev->priv;
  212. if (dp83640->clock->page != page) {
  213. broadcast_write(phydev, PAGESEL, page);
  214. dp83640->clock->page = page;
  215. }
  216. if (broadcast)
  217. broadcast_write(phydev, regnum, val);
  218. else
  219. phy_write(phydev, regnum, val);
  220. }
  221. /* Caller must hold extreg_lock. */
  222. static int tdr_write(int bc, struct phy_device *dev,
  223. const struct timespec64 *ts, u16 cmd)
  224. {
  225. ext_write(bc, dev, PAGE4, PTP_TDR, ts->tv_nsec & 0xffff);/* ns[15:0] */
  226. ext_write(bc, dev, PAGE4, PTP_TDR, ts->tv_nsec >> 16); /* ns[31:16] */
  227. ext_write(bc, dev, PAGE4, PTP_TDR, ts->tv_sec & 0xffff); /* sec[15:0] */
  228. ext_write(bc, dev, PAGE4, PTP_TDR, ts->tv_sec >> 16); /* sec[31:16]*/
  229. ext_write(bc, dev, PAGE4, PTP_CTL, cmd);
  230. return 0;
  231. }
  232. /* convert phy timestamps into driver timestamps */
  233. static void phy2rxts(struct phy_rxts *p, struct rxts *rxts)
  234. {
  235. u32 sec;
  236. sec = p->sec_lo;
  237. sec |= p->sec_hi << 16;
  238. rxts->ns = p->ns_lo;
  239. rxts->ns |= (p->ns_hi & 0x3fff) << 16;
  240. rxts->ns += ((u64)sec) * 1000000000ULL;
  241. rxts->seqid = p->seqid;
  242. rxts->msgtype = (p->msgtype >> 12) & 0xf;
  243. rxts->hash = p->msgtype & 0x0fff;
  244. rxts->tmo = jiffies + SKB_TIMESTAMP_TIMEOUT;
  245. }
  246. static u64 phy2txts(struct phy_txts *p)
  247. {
  248. u64 ns;
  249. u32 sec;
  250. sec = p->sec_lo;
  251. sec |= p->sec_hi << 16;
  252. ns = p->ns_lo;
  253. ns |= (p->ns_hi & 0x3fff) << 16;
  254. ns += ((u64)sec) * 1000000000ULL;
  255. return ns;
  256. }
  257. static int periodic_output(struct dp83640_clock *clock,
  258. struct ptp_clock_request *clkreq, bool on,
  259. int trigger)
  260. {
  261. struct dp83640_private *dp83640 = clock->chosen;
  262. struct phy_device *phydev = dp83640->phydev;
  263. u32 sec, nsec, pwidth;
  264. u16 gpio, ptp_trig, val;
  265. if (on) {
  266. gpio = 1 + ptp_find_pin(clock->ptp_clock, PTP_PF_PEROUT,
  267. trigger);
  268. if (gpio < 1)
  269. return -EINVAL;
  270. } else {
  271. gpio = 0;
  272. }
  273. ptp_trig = TRIG_WR |
  274. (trigger & TRIG_CSEL_MASK) << TRIG_CSEL_SHIFT |
  275. (gpio & TRIG_GPIO_MASK) << TRIG_GPIO_SHIFT |
  276. TRIG_PER |
  277. TRIG_PULSE;
  278. val = (trigger & TRIG_SEL_MASK) << TRIG_SEL_SHIFT;
  279. if (!on) {
  280. val |= TRIG_DIS;
  281. mutex_lock(&clock->extreg_lock);
  282. ext_write(0, phydev, PAGE5, PTP_TRIG, ptp_trig);
  283. ext_write(0, phydev, PAGE4, PTP_CTL, val);
  284. mutex_unlock(&clock->extreg_lock);
  285. return 0;
  286. }
  287. sec = clkreq->perout.start.sec;
  288. nsec = clkreq->perout.start.nsec;
  289. pwidth = clkreq->perout.period.sec * 1000000000UL;
  290. pwidth += clkreq->perout.period.nsec;
  291. pwidth /= 2;
  292. mutex_lock(&clock->extreg_lock);
  293. ext_write(0, phydev, PAGE5, PTP_TRIG, ptp_trig);
  294. /*load trigger*/
  295. val |= TRIG_LOAD;
  296. ext_write(0, phydev, PAGE4, PTP_CTL, val);
  297. ext_write(0, phydev, PAGE4, PTP_TDR, nsec & 0xffff); /* ns[15:0] */
  298. ext_write(0, phydev, PAGE4, PTP_TDR, nsec >> 16); /* ns[31:16] */
  299. ext_write(0, phydev, PAGE4, PTP_TDR, sec & 0xffff); /* sec[15:0] */
  300. ext_write(0, phydev, PAGE4, PTP_TDR, sec >> 16); /* sec[31:16] */
  301. ext_write(0, phydev, PAGE4, PTP_TDR, pwidth & 0xffff); /* ns[15:0] */
  302. ext_write(0, phydev, PAGE4, PTP_TDR, pwidth >> 16); /* ns[31:16] */
  303. /* Triggers 0 and 1 has programmable pulsewidth2 */
  304. if (trigger < 2) {
  305. ext_write(0, phydev, PAGE4, PTP_TDR, pwidth & 0xffff);
  306. ext_write(0, phydev, PAGE4, PTP_TDR, pwidth >> 16);
  307. }
  308. /*enable trigger*/
  309. val &= ~TRIG_LOAD;
  310. val |= TRIG_EN;
  311. ext_write(0, phydev, PAGE4, PTP_CTL, val);
  312. mutex_unlock(&clock->extreg_lock);
  313. return 0;
  314. }
  315. /* ptp clock methods */
  316. static int ptp_dp83640_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
  317. {
  318. struct dp83640_clock *clock =
  319. container_of(ptp, struct dp83640_clock, caps);
  320. struct phy_device *phydev = clock->chosen->phydev;
  321. u64 rate;
  322. int neg_adj = 0;
  323. u16 hi, lo;
  324. if (ppb < 0) {
  325. neg_adj = 1;
  326. ppb = -ppb;
  327. }
  328. rate = ppb;
  329. rate <<= 26;
  330. rate = div_u64(rate, 1953125);
  331. hi = (rate >> 16) & PTP_RATE_HI_MASK;
  332. if (neg_adj)
  333. hi |= PTP_RATE_DIR;
  334. lo = rate & 0xffff;
  335. mutex_lock(&clock->extreg_lock);
  336. ext_write(1, phydev, PAGE4, PTP_RATEH, hi);
  337. ext_write(1, phydev, PAGE4, PTP_RATEL, lo);
  338. mutex_unlock(&clock->extreg_lock);
  339. return 0;
  340. }
  341. static int ptp_dp83640_adjtime(struct ptp_clock_info *ptp, s64 delta)
  342. {
  343. struct dp83640_clock *clock =
  344. container_of(ptp, struct dp83640_clock, caps);
  345. struct phy_device *phydev = clock->chosen->phydev;
  346. struct timespec64 ts;
  347. int err;
  348. delta += ADJTIME_FIX;
  349. ts = ns_to_timespec64(delta);
  350. mutex_lock(&clock->extreg_lock);
  351. err = tdr_write(1, phydev, &ts, PTP_STEP_CLK);
  352. mutex_unlock(&clock->extreg_lock);
  353. return err;
  354. }
  355. static int ptp_dp83640_gettime(struct ptp_clock_info *ptp,
  356. struct timespec64 *ts)
  357. {
  358. struct dp83640_clock *clock =
  359. container_of(ptp, struct dp83640_clock, caps);
  360. struct phy_device *phydev = clock->chosen->phydev;
  361. unsigned int val[4];
  362. mutex_lock(&clock->extreg_lock);
  363. ext_write(0, phydev, PAGE4, PTP_CTL, PTP_RD_CLK);
  364. val[0] = ext_read(phydev, PAGE4, PTP_TDR); /* ns[15:0] */
  365. val[1] = ext_read(phydev, PAGE4, PTP_TDR); /* ns[31:16] */
  366. val[2] = ext_read(phydev, PAGE4, PTP_TDR); /* sec[15:0] */
  367. val[3] = ext_read(phydev, PAGE4, PTP_TDR); /* sec[31:16] */
  368. mutex_unlock(&clock->extreg_lock);
  369. ts->tv_nsec = val[0] | (val[1] << 16);
  370. ts->tv_sec = val[2] | (val[3] << 16);
  371. return 0;
  372. }
  373. static int ptp_dp83640_settime(struct ptp_clock_info *ptp,
  374. const struct timespec64 *ts)
  375. {
  376. struct dp83640_clock *clock =
  377. container_of(ptp, struct dp83640_clock, caps);
  378. struct phy_device *phydev = clock->chosen->phydev;
  379. int err;
  380. mutex_lock(&clock->extreg_lock);
  381. err = tdr_write(1, phydev, ts, PTP_LOAD_CLK);
  382. mutex_unlock(&clock->extreg_lock);
  383. return err;
  384. }
  385. static int ptp_dp83640_enable(struct ptp_clock_info *ptp,
  386. struct ptp_clock_request *rq, int on)
  387. {
  388. struct dp83640_clock *clock =
  389. container_of(ptp, struct dp83640_clock, caps);
  390. struct phy_device *phydev = clock->chosen->phydev;
  391. unsigned int index;
  392. u16 evnt, event_num, gpio_num;
  393. switch (rq->type) {
  394. case PTP_CLK_REQ_EXTTS:
  395. index = rq->extts.index;
  396. if (index >= N_EXT_TS)
  397. return -EINVAL;
  398. event_num = EXT_EVENT + index;
  399. evnt = EVNT_WR | (event_num & EVNT_SEL_MASK) << EVNT_SEL_SHIFT;
  400. if (on) {
  401. gpio_num = 1 + ptp_find_pin(clock->ptp_clock,
  402. PTP_PF_EXTTS, index);
  403. if (gpio_num < 1)
  404. return -EINVAL;
  405. evnt |= (gpio_num & EVNT_GPIO_MASK) << EVNT_GPIO_SHIFT;
  406. if (rq->extts.flags & PTP_FALLING_EDGE)
  407. evnt |= EVNT_FALL;
  408. else
  409. evnt |= EVNT_RISE;
  410. }
  411. mutex_lock(&clock->extreg_lock);
  412. ext_write(0, phydev, PAGE5, PTP_EVNT, evnt);
  413. mutex_unlock(&clock->extreg_lock);
  414. return 0;
  415. case PTP_CLK_REQ_PEROUT:
  416. if (rq->perout.index >= N_PER_OUT)
  417. return -EINVAL;
  418. return periodic_output(clock, rq, on, rq->perout.index);
  419. default:
  420. break;
  421. }
  422. return -EOPNOTSUPP;
  423. }
  424. static int ptp_dp83640_verify(struct ptp_clock_info *ptp, unsigned int pin,
  425. enum ptp_pin_function func, unsigned int chan)
  426. {
  427. struct dp83640_clock *clock =
  428. container_of(ptp, struct dp83640_clock, caps);
  429. if (clock->caps.pin_config[pin].func == PTP_PF_PHYSYNC &&
  430. !list_empty(&clock->phylist))
  431. return 1;
  432. if (func == PTP_PF_PHYSYNC)
  433. return 1;
  434. return 0;
  435. }
  436. static u8 status_frame_dst[6] = { 0x01, 0x1B, 0x19, 0x00, 0x00, 0x00 };
  437. static u8 status_frame_src[6] = { 0x08, 0x00, 0x17, 0x0B, 0x6B, 0x0F };
  438. static void enable_status_frames(struct phy_device *phydev, bool on)
  439. {
  440. struct dp83640_private *dp83640 = phydev->priv;
  441. struct dp83640_clock *clock = dp83640->clock;
  442. u16 cfg0 = 0, ver;
  443. if (on)
  444. cfg0 = PSF_EVNT_EN | PSF_RXTS_EN | PSF_TXTS_EN | ENDIAN_FLAG;
  445. ver = (PSF_PTPVER & VERSIONPTP_MASK) << VERSIONPTP_SHIFT;
  446. mutex_lock(&clock->extreg_lock);
  447. ext_write(0, phydev, PAGE5, PSF_CFG0, cfg0);
  448. ext_write(0, phydev, PAGE6, PSF_CFG1, ver);
  449. mutex_unlock(&clock->extreg_lock);
  450. if (!phydev->attached_dev) {
  451. pr_warn("expected to find an attached netdevice\n");
  452. return;
  453. }
  454. if (on) {
  455. if (dev_mc_add(phydev->attached_dev, status_frame_dst))
  456. pr_warn("failed to add mc address\n");
  457. } else {
  458. if (dev_mc_del(phydev->attached_dev, status_frame_dst))
  459. pr_warn("failed to delete mc address\n");
  460. }
  461. }
  462. static bool is_status_frame(struct sk_buff *skb, int type)
  463. {
  464. struct ethhdr *h = eth_hdr(skb);
  465. if (PTP_CLASS_V2_L2 == type &&
  466. !memcmp(h->h_source, status_frame_src, sizeof(status_frame_src)))
  467. return true;
  468. else
  469. return false;
  470. }
  471. static int expired(struct rxts *rxts)
  472. {
  473. return time_after(jiffies, rxts->tmo);
  474. }
  475. /* Caller must hold rx_lock. */
  476. static void prune_rx_ts(struct dp83640_private *dp83640)
  477. {
  478. struct list_head *this, *next;
  479. struct rxts *rxts;
  480. list_for_each_safe(this, next, &dp83640->rxts) {
  481. rxts = list_entry(this, struct rxts, list);
  482. if (expired(rxts)) {
  483. list_del_init(&rxts->list);
  484. list_add(&rxts->list, &dp83640->rxpool);
  485. }
  486. }
  487. }
  488. /* synchronize the phyters so they act as one clock */
  489. static void enable_broadcast(struct phy_device *phydev, int init_page, int on)
  490. {
  491. int val;
  492. phy_write(phydev, PAGESEL, 0);
  493. val = phy_read(phydev, PHYCR2);
  494. if (on)
  495. val |= BC_WRITE;
  496. else
  497. val &= ~BC_WRITE;
  498. phy_write(phydev, PHYCR2, val);
  499. phy_write(phydev, PAGESEL, init_page);
  500. }
  501. static void recalibrate(struct dp83640_clock *clock)
  502. {
  503. s64 now, diff;
  504. struct phy_txts event_ts;
  505. struct timespec64 ts;
  506. struct list_head *this;
  507. struct dp83640_private *tmp;
  508. struct phy_device *master = clock->chosen->phydev;
  509. u16 cal_gpio, cfg0, evnt, ptp_trig, trigger, val;
  510. trigger = CAL_TRIGGER;
  511. cal_gpio = 1 + ptp_find_pin(clock->ptp_clock, PTP_PF_PHYSYNC, 0);
  512. if (cal_gpio < 1) {
  513. pr_err("PHY calibration pin not available - PHY is not calibrated.");
  514. return;
  515. }
  516. mutex_lock(&clock->extreg_lock);
  517. /*
  518. * enable broadcast, disable status frames, enable ptp clock
  519. */
  520. list_for_each(this, &clock->phylist) {
  521. tmp = list_entry(this, struct dp83640_private, list);
  522. enable_broadcast(tmp->phydev, clock->page, 1);
  523. tmp->cfg0 = ext_read(tmp->phydev, PAGE5, PSF_CFG0);
  524. ext_write(0, tmp->phydev, PAGE5, PSF_CFG0, 0);
  525. ext_write(0, tmp->phydev, PAGE4, PTP_CTL, PTP_ENABLE);
  526. }
  527. enable_broadcast(master, clock->page, 1);
  528. cfg0 = ext_read(master, PAGE5, PSF_CFG0);
  529. ext_write(0, master, PAGE5, PSF_CFG0, 0);
  530. ext_write(0, master, PAGE4, PTP_CTL, PTP_ENABLE);
  531. /*
  532. * enable an event timestamp
  533. */
  534. evnt = EVNT_WR | EVNT_RISE | EVNT_SINGLE;
  535. evnt |= (CAL_EVENT & EVNT_SEL_MASK) << EVNT_SEL_SHIFT;
  536. evnt |= (cal_gpio & EVNT_GPIO_MASK) << EVNT_GPIO_SHIFT;
  537. list_for_each(this, &clock->phylist) {
  538. tmp = list_entry(this, struct dp83640_private, list);
  539. ext_write(0, tmp->phydev, PAGE5, PTP_EVNT, evnt);
  540. }
  541. ext_write(0, master, PAGE5, PTP_EVNT, evnt);
  542. /*
  543. * configure a trigger
  544. */
  545. ptp_trig = TRIG_WR | TRIG_IF_LATE | TRIG_PULSE;
  546. ptp_trig |= (trigger & TRIG_CSEL_MASK) << TRIG_CSEL_SHIFT;
  547. ptp_trig |= (cal_gpio & TRIG_GPIO_MASK) << TRIG_GPIO_SHIFT;
  548. ext_write(0, master, PAGE5, PTP_TRIG, ptp_trig);
  549. /* load trigger */
  550. val = (trigger & TRIG_SEL_MASK) << TRIG_SEL_SHIFT;
  551. val |= TRIG_LOAD;
  552. ext_write(0, master, PAGE4, PTP_CTL, val);
  553. /* enable trigger */
  554. val &= ~TRIG_LOAD;
  555. val |= TRIG_EN;
  556. ext_write(0, master, PAGE4, PTP_CTL, val);
  557. /* disable trigger */
  558. val = (trigger & TRIG_SEL_MASK) << TRIG_SEL_SHIFT;
  559. val |= TRIG_DIS;
  560. ext_write(0, master, PAGE4, PTP_CTL, val);
  561. /*
  562. * read out and correct offsets
  563. */
  564. val = ext_read(master, PAGE4, PTP_STS);
  565. pr_info("master PTP_STS 0x%04hx\n", val);
  566. val = ext_read(master, PAGE4, PTP_ESTS);
  567. pr_info("master PTP_ESTS 0x%04hx\n", val);
  568. event_ts.ns_lo = ext_read(master, PAGE4, PTP_EDATA);
  569. event_ts.ns_hi = ext_read(master, PAGE4, PTP_EDATA);
  570. event_ts.sec_lo = ext_read(master, PAGE4, PTP_EDATA);
  571. event_ts.sec_hi = ext_read(master, PAGE4, PTP_EDATA);
  572. now = phy2txts(&event_ts);
  573. list_for_each(this, &clock->phylist) {
  574. tmp = list_entry(this, struct dp83640_private, list);
  575. val = ext_read(tmp->phydev, PAGE4, PTP_STS);
  576. pr_info("slave PTP_STS 0x%04hx\n", val);
  577. val = ext_read(tmp->phydev, PAGE4, PTP_ESTS);
  578. pr_info("slave PTP_ESTS 0x%04hx\n", val);
  579. event_ts.ns_lo = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
  580. event_ts.ns_hi = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
  581. event_ts.sec_lo = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
  582. event_ts.sec_hi = ext_read(tmp->phydev, PAGE4, PTP_EDATA);
  583. diff = now - (s64) phy2txts(&event_ts);
  584. pr_info("slave offset %lld nanoseconds\n", diff);
  585. diff += ADJTIME_FIX;
  586. ts = ns_to_timespec64(diff);
  587. tdr_write(0, tmp->phydev, &ts, PTP_STEP_CLK);
  588. }
  589. /*
  590. * restore status frames
  591. */
  592. list_for_each(this, &clock->phylist) {
  593. tmp = list_entry(this, struct dp83640_private, list);
  594. ext_write(0, tmp->phydev, PAGE5, PSF_CFG0, tmp->cfg0);
  595. }
  596. ext_write(0, master, PAGE5, PSF_CFG0, cfg0);
  597. mutex_unlock(&clock->extreg_lock);
  598. }
  599. /* time stamping methods */
  600. static inline u16 exts_chan_to_edata(int ch)
  601. {
  602. return 1 << ((ch + EXT_EVENT) * 2);
  603. }
  604. static int decode_evnt(struct dp83640_private *dp83640,
  605. void *data, int len, u16 ests)
  606. {
  607. struct phy_txts *phy_txts;
  608. struct ptp_clock_event event;
  609. int i, parsed;
  610. int words = (ests >> EVNT_TS_LEN_SHIFT) & EVNT_TS_LEN_MASK;
  611. u16 ext_status = 0;
  612. /* calculate length of the event timestamp status message */
  613. if (ests & MULT_EVNT)
  614. parsed = (words + 2) * sizeof(u16);
  615. else
  616. parsed = (words + 1) * sizeof(u16);
  617. /* check if enough data is available */
  618. if (len < parsed)
  619. return len;
  620. if (ests & MULT_EVNT) {
  621. ext_status = *(u16 *) data;
  622. data += sizeof(ext_status);
  623. }
  624. phy_txts = data;
  625. switch (words) { /* fall through in every case */
  626. case 3:
  627. dp83640->edata.sec_hi = phy_txts->sec_hi;
  628. case 2:
  629. dp83640->edata.sec_lo = phy_txts->sec_lo;
  630. case 1:
  631. dp83640->edata.ns_hi = phy_txts->ns_hi;
  632. case 0:
  633. dp83640->edata.ns_lo = phy_txts->ns_lo;
  634. }
  635. if (!ext_status) {
  636. i = ((ests >> EVNT_NUM_SHIFT) & EVNT_NUM_MASK) - EXT_EVENT;
  637. ext_status = exts_chan_to_edata(i);
  638. }
  639. event.type = PTP_CLOCK_EXTTS;
  640. event.timestamp = phy2txts(&dp83640->edata);
  641. /* Compensate for input path and synchronization delays */
  642. event.timestamp -= 35;
  643. for (i = 0; i < N_EXT_TS; i++) {
  644. if (ext_status & exts_chan_to_edata(i)) {
  645. event.index = i;
  646. ptp_clock_event(dp83640->clock->ptp_clock, &event);
  647. }
  648. }
  649. return parsed;
  650. }
  651. #define DP83640_PACKET_HASH_OFFSET 20
  652. #define DP83640_PACKET_HASH_LEN 10
  653. static int match(struct sk_buff *skb, unsigned int type, struct rxts *rxts)
  654. {
  655. u16 *seqid, hash;
  656. unsigned int offset = 0;
  657. u8 *msgtype, *data = skb_mac_header(skb);
  658. /* check sequenceID, messageType, 12 bit hash of offset 20-29 */
  659. if (type & PTP_CLASS_VLAN)
  660. offset += VLAN_HLEN;
  661. switch (type & PTP_CLASS_PMASK) {
  662. case PTP_CLASS_IPV4:
  663. offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
  664. break;
  665. case PTP_CLASS_IPV6:
  666. offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
  667. break;
  668. case PTP_CLASS_L2:
  669. offset += ETH_HLEN;
  670. break;
  671. default:
  672. return 0;
  673. }
  674. if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid))
  675. return 0;
  676. if (unlikely(type & PTP_CLASS_V1))
  677. msgtype = data + offset + OFF_PTP_CONTROL;
  678. else
  679. msgtype = data + offset;
  680. if (rxts->msgtype != (*msgtype & 0xf))
  681. return 0;
  682. seqid = (u16 *)(data + offset + OFF_PTP_SEQUENCE_ID);
  683. if (rxts->seqid != ntohs(*seqid))
  684. return 0;
  685. hash = ether_crc(DP83640_PACKET_HASH_LEN,
  686. data + offset + DP83640_PACKET_HASH_OFFSET) >> 20;
  687. if (rxts->hash != hash)
  688. return 0;
  689. return 1;
  690. }
  691. static void decode_rxts(struct dp83640_private *dp83640,
  692. struct phy_rxts *phy_rxts)
  693. {
  694. struct rxts *rxts;
  695. struct skb_shared_hwtstamps *shhwtstamps = NULL;
  696. struct sk_buff *skb;
  697. unsigned long flags;
  698. u8 overflow;
  699. overflow = (phy_rxts->ns_hi >> 14) & 0x3;
  700. if (overflow)
  701. pr_debug("rx timestamp queue overflow, count %d\n", overflow);
  702. spin_lock_irqsave(&dp83640->rx_lock, flags);
  703. prune_rx_ts(dp83640);
  704. if (list_empty(&dp83640->rxpool)) {
  705. pr_debug("rx timestamp pool is empty\n");
  706. goto out;
  707. }
  708. rxts = list_first_entry(&dp83640->rxpool, struct rxts, list);
  709. list_del_init(&rxts->list);
  710. phy2rxts(phy_rxts, rxts);
  711. spin_lock(&dp83640->rx_queue.lock);
  712. skb_queue_walk(&dp83640->rx_queue, skb) {
  713. struct dp83640_skb_info *skb_info;
  714. skb_info = (struct dp83640_skb_info *)skb->cb;
  715. if (match(skb, skb_info->ptp_type, rxts)) {
  716. __skb_unlink(skb, &dp83640->rx_queue);
  717. shhwtstamps = skb_hwtstamps(skb);
  718. memset(shhwtstamps, 0, sizeof(*shhwtstamps));
  719. shhwtstamps->hwtstamp = ns_to_ktime(rxts->ns);
  720. netif_rx_ni(skb);
  721. list_add(&rxts->list, &dp83640->rxpool);
  722. break;
  723. }
  724. }
  725. spin_unlock(&dp83640->rx_queue.lock);
  726. if (!shhwtstamps)
  727. list_add_tail(&rxts->list, &dp83640->rxts);
  728. out:
  729. spin_unlock_irqrestore(&dp83640->rx_lock, flags);
  730. }
  731. static void decode_txts(struct dp83640_private *dp83640,
  732. struct phy_txts *phy_txts)
  733. {
  734. struct skb_shared_hwtstamps shhwtstamps;
  735. struct sk_buff *skb;
  736. u64 ns;
  737. u8 overflow;
  738. /* We must already have the skb that triggered this. */
  739. skb = skb_dequeue(&dp83640->tx_queue);
  740. if (!skb) {
  741. pr_debug("have timestamp but tx_queue empty\n");
  742. return;
  743. }
  744. overflow = (phy_txts->ns_hi >> 14) & 0x3;
  745. if (overflow) {
  746. pr_debug("tx timestamp queue overflow, count %d\n", overflow);
  747. while (skb) {
  748. kfree_skb(skb);
  749. skb = skb_dequeue(&dp83640->tx_queue);
  750. }
  751. return;
  752. }
  753. ns = phy2txts(phy_txts);
  754. memset(&shhwtstamps, 0, sizeof(shhwtstamps));
  755. shhwtstamps.hwtstamp = ns_to_ktime(ns);
  756. skb_complete_tx_timestamp(skb, &shhwtstamps);
  757. }
  758. static void decode_status_frame(struct dp83640_private *dp83640,
  759. struct sk_buff *skb)
  760. {
  761. struct phy_rxts *phy_rxts;
  762. struct phy_txts *phy_txts;
  763. u8 *ptr;
  764. int len, size;
  765. u16 ests, type;
  766. ptr = skb->data + 2;
  767. for (len = skb_headlen(skb) - 2; len > sizeof(type); len -= size) {
  768. type = *(u16 *)ptr;
  769. ests = type & 0x0fff;
  770. type = type & 0xf000;
  771. len -= sizeof(type);
  772. ptr += sizeof(type);
  773. if (PSF_RX == type && len >= sizeof(*phy_rxts)) {
  774. phy_rxts = (struct phy_rxts *) ptr;
  775. decode_rxts(dp83640, phy_rxts);
  776. size = sizeof(*phy_rxts);
  777. } else if (PSF_TX == type && len >= sizeof(*phy_txts)) {
  778. phy_txts = (struct phy_txts *) ptr;
  779. decode_txts(dp83640, phy_txts);
  780. size = sizeof(*phy_txts);
  781. } else if (PSF_EVNT == type) {
  782. size = decode_evnt(dp83640, ptr, len, ests);
  783. } else {
  784. size = 0;
  785. break;
  786. }
  787. ptr += size;
  788. }
  789. }
  790. static int is_sync(struct sk_buff *skb, int type)
  791. {
  792. u8 *data = skb->data, *msgtype;
  793. unsigned int offset = 0;
  794. if (type & PTP_CLASS_VLAN)
  795. offset += VLAN_HLEN;
  796. switch (type & PTP_CLASS_PMASK) {
  797. case PTP_CLASS_IPV4:
  798. offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
  799. break;
  800. case PTP_CLASS_IPV6:
  801. offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
  802. break;
  803. case PTP_CLASS_L2:
  804. offset += ETH_HLEN;
  805. break;
  806. default:
  807. return 0;
  808. }
  809. if (type & PTP_CLASS_V1)
  810. offset += OFF_PTP_CONTROL;
  811. if (skb->len < offset + 1)
  812. return 0;
  813. msgtype = data + offset;
  814. return (*msgtype & 0xf) == 0;
  815. }
  816. static void dp83640_free_clocks(void)
  817. {
  818. struct dp83640_clock *clock;
  819. struct list_head *this, *next;
  820. mutex_lock(&phyter_clocks_lock);
  821. list_for_each_safe(this, next, &phyter_clocks) {
  822. clock = list_entry(this, struct dp83640_clock, list);
  823. if (!list_empty(&clock->phylist)) {
  824. pr_warn("phy list non-empty while unloading\n");
  825. BUG();
  826. }
  827. list_del(&clock->list);
  828. mutex_destroy(&clock->extreg_lock);
  829. mutex_destroy(&clock->clock_lock);
  830. put_device(&clock->bus->dev);
  831. kfree(clock->caps.pin_config);
  832. kfree(clock);
  833. }
  834. mutex_unlock(&phyter_clocks_lock);
  835. }
  836. static void dp83640_clock_init(struct dp83640_clock *clock, struct mii_bus *bus)
  837. {
  838. INIT_LIST_HEAD(&clock->list);
  839. clock->bus = bus;
  840. mutex_init(&clock->extreg_lock);
  841. mutex_init(&clock->clock_lock);
  842. INIT_LIST_HEAD(&clock->phylist);
  843. clock->caps.owner = THIS_MODULE;
  844. sprintf(clock->caps.name, "dp83640 timer");
  845. clock->caps.max_adj = 1953124;
  846. clock->caps.n_alarm = 0;
  847. clock->caps.n_ext_ts = N_EXT_TS;
  848. clock->caps.n_per_out = N_PER_OUT;
  849. clock->caps.n_pins = DP83640_N_PINS;
  850. clock->caps.pps = 0;
  851. clock->caps.adjfreq = ptp_dp83640_adjfreq;
  852. clock->caps.adjtime = ptp_dp83640_adjtime;
  853. clock->caps.gettime64 = ptp_dp83640_gettime;
  854. clock->caps.settime64 = ptp_dp83640_settime;
  855. clock->caps.enable = ptp_dp83640_enable;
  856. clock->caps.verify = ptp_dp83640_verify;
  857. /*
  858. * Convert the module param defaults into a dynamic pin configuration.
  859. */
  860. dp83640_gpio_defaults(clock->caps.pin_config);
  861. /*
  862. * Get a reference to this bus instance.
  863. */
  864. get_device(&bus->dev);
  865. }
  866. static int choose_this_phy(struct dp83640_clock *clock,
  867. struct phy_device *phydev)
  868. {
  869. if (chosen_phy == -1 && !clock->chosen)
  870. return 1;
  871. if (chosen_phy == phydev->mdio.addr)
  872. return 1;
  873. return 0;
  874. }
  875. static struct dp83640_clock *dp83640_clock_get(struct dp83640_clock *clock)
  876. {
  877. if (clock)
  878. mutex_lock(&clock->clock_lock);
  879. return clock;
  880. }
  881. /*
  882. * Look up and lock a clock by bus instance.
  883. * If there is no clock for this bus, then create it first.
  884. */
  885. static struct dp83640_clock *dp83640_clock_get_bus(struct mii_bus *bus)
  886. {
  887. struct dp83640_clock *clock = NULL, *tmp;
  888. struct list_head *this;
  889. mutex_lock(&phyter_clocks_lock);
  890. list_for_each(this, &phyter_clocks) {
  891. tmp = list_entry(this, struct dp83640_clock, list);
  892. if (tmp->bus == bus) {
  893. clock = tmp;
  894. break;
  895. }
  896. }
  897. if (clock)
  898. goto out;
  899. clock = kzalloc(sizeof(struct dp83640_clock), GFP_KERNEL);
  900. if (!clock)
  901. goto out;
  902. clock->caps.pin_config = kzalloc(sizeof(struct ptp_pin_desc) *
  903. DP83640_N_PINS, GFP_KERNEL);
  904. if (!clock->caps.pin_config) {
  905. kfree(clock);
  906. clock = NULL;
  907. goto out;
  908. }
  909. dp83640_clock_init(clock, bus);
  910. list_add_tail(&phyter_clocks, &clock->list);
  911. out:
  912. mutex_unlock(&phyter_clocks_lock);
  913. return dp83640_clock_get(clock);
  914. }
  915. static void dp83640_clock_put(struct dp83640_clock *clock)
  916. {
  917. mutex_unlock(&clock->clock_lock);
  918. }
  919. static int dp83640_probe(struct phy_device *phydev)
  920. {
  921. struct dp83640_clock *clock;
  922. struct dp83640_private *dp83640;
  923. int err = -ENOMEM, i;
  924. if (phydev->mdio.addr == BROADCAST_ADDR)
  925. return 0;
  926. clock = dp83640_clock_get_bus(phydev->mdio.bus);
  927. if (!clock)
  928. goto no_clock;
  929. dp83640 = kzalloc(sizeof(struct dp83640_private), GFP_KERNEL);
  930. if (!dp83640)
  931. goto no_memory;
  932. dp83640->phydev = phydev;
  933. INIT_DELAYED_WORK(&dp83640->ts_work, rx_timestamp_work);
  934. INIT_LIST_HEAD(&dp83640->rxts);
  935. INIT_LIST_HEAD(&dp83640->rxpool);
  936. for (i = 0; i < MAX_RXTS; i++)
  937. list_add(&dp83640->rx_pool_data[i].list, &dp83640->rxpool);
  938. phydev->priv = dp83640;
  939. spin_lock_init(&dp83640->rx_lock);
  940. skb_queue_head_init(&dp83640->rx_queue);
  941. skb_queue_head_init(&dp83640->tx_queue);
  942. dp83640->clock = clock;
  943. if (choose_this_phy(clock, phydev)) {
  944. clock->chosen = dp83640;
  945. clock->ptp_clock = ptp_clock_register(&clock->caps,
  946. &phydev->mdio.dev);
  947. if (IS_ERR(clock->ptp_clock)) {
  948. err = PTR_ERR(clock->ptp_clock);
  949. goto no_register;
  950. }
  951. } else
  952. list_add_tail(&dp83640->list, &clock->phylist);
  953. dp83640_clock_put(clock);
  954. return 0;
  955. no_register:
  956. clock->chosen = NULL;
  957. kfree(dp83640);
  958. no_memory:
  959. dp83640_clock_put(clock);
  960. no_clock:
  961. return err;
  962. }
  963. static void dp83640_remove(struct phy_device *phydev)
  964. {
  965. struct dp83640_clock *clock;
  966. struct list_head *this, *next;
  967. struct dp83640_private *tmp, *dp83640 = phydev->priv;
  968. if (phydev->mdio.addr == BROADCAST_ADDR)
  969. return;
  970. enable_status_frames(phydev, false);
  971. cancel_delayed_work_sync(&dp83640->ts_work);
  972. skb_queue_purge(&dp83640->rx_queue);
  973. skb_queue_purge(&dp83640->tx_queue);
  974. clock = dp83640_clock_get(dp83640->clock);
  975. if (dp83640 == clock->chosen) {
  976. ptp_clock_unregister(clock->ptp_clock);
  977. clock->chosen = NULL;
  978. } else {
  979. list_for_each_safe(this, next, &clock->phylist) {
  980. tmp = list_entry(this, struct dp83640_private, list);
  981. if (tmp == dp83640) {
  982. list_del_init(&tmp->list);
  983. break;
  984. }
  985. }
  986. }
  987. dp83640_clock_put(clock);
  988. kfree(dp83640);
  989. }
  990. static int dp83640_config_init(struct phy_device *phydev)
  991. {
  992. struct dp83640_private *dp83640 = phydev->priv;
  993. struct dp83640_clock *clock = dp83640->clock;
  994. if (clock->chosen && !list_empty(&clock->phylist))
  995. recalibrate(clock);
  996. else {
  997. mutex_lock(&clock->extreg_lock);
  998. enable_broadcast(phydev, clock->page, 1);
  999. mutex_unlock(&clock->extreg_lock);
  1000. }
  1001. enable_status_frames(phydev, true);
  1002. mutex_lock(&clock->extreg_lock);
  1003. ext_write(0, phydev, PAGE4, PTP_CTL, PTP_ENABLE);
  1004. mutex_unlock(&clock->extreg_lock);
  1005. return 0;
  1006. }
  1007. static int dp83640_ack_interrupt(struct phy_device *phydev)
  1008. {
  1009. int err = phy_read(phydev, MII_DP83640_MISR);
  1010. if (err < 0)
  1011. return err;
  1012. return 0;
  1013. }
  1014. static int dp83640_config_intr(struct phy_device *phydev)
  1015. {
  1016. int micr;
  1017. int misr;
  1018. int err;
  1019. if (phydev->interrupts == PHY_INTERRUPT_ENABLED) {
  1020. misr = phy_read(phydev, MII_DP83640_MISR);
  1021. if (misr < 0)
  1022. return misr;
  1023. misr |=
  1024. (MII_DP83640_MISR_ANC_INT_EN |
  1025. MII_DP83640_MISR_DUP_INT_EN |
  1026. MII_DP83640_MISR_SPD_INT_EN |
  1027. MII_DP83640_MISR_LINK_INT_EN);
  1028. err = phy_write(phydev, MII_DP83640_MISR, misr);
  1029. if (err < 0)
  1030. return err;
  1031. micr = phy_read(phydev, MII_DP83640_MICR);
  1032. if (micr < 0)
  1033. return micr;
  1034. micr |=
  1035. (MII_DP83640_MICR_OE |
  1036. MII_DP83640_MICR_IE);
  1037. return phy_write(phydev, MII_DP83640_MICR, micr);
  1038. } else {
  1039. micr = phy_read(phydev, MII_DP83640_MICR);
  1040. if (micr < 0)
  1041. return micr;
  1042. micr &=
  1043. ~(MII_DP83640_MICR_OE |
  1044. MII_DP83640_MICR_IE);
  1045. err = phy_write(phydev, MII_DP83640_MICR, micr);
  1046. if (err < 0)
  1047. return err;
  1048. misr = phy_read(phydev, MII_DP83640_MISR);
  1049. if (misr < 0)
  1050. return misr;
  1051. misr &=
  1052. ~(MII_DP83640_MISR_ANC_INT_EN |
  1053. MII_DP83640_MISR_DUP_INT_EN |
  1054. MII_DP83640_MISR_SPD_INT_EN |
  1055. MII_DP83640_MISR_LINK_INT_EN);
  1056. return phy_write(phydev, MII_DP83640_MISR, misr);
  1057. }
  1058. }
  1059. static int dp83640_hwtstamp(struct phy_device *phydev, struct ifreq *ifr)
  1060. {
  1061. struct dp83640_private *dp83640 = phydev->priv;
  1062. struct hwtstamp_config cfg;
  1063. u16 txcfg0, rxcfg0;
  1064. if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
  1065. return -EFAULT;
  1066. if (cfg.flags) /* reserved for future extensions */
  1067. return -EINVAL;
  1068. if (cfg.tx_type < 0 || cfg.tx_type > HWTSTAMP_TX_ONESTEP_SYNC)
  1069. return -ERANGE;
  1070. dp83640->hwts_tx_en = cfg.tx_type;
  1071. switch (cfg.rx_filter) {
  1072. case HWTSTAMP_FILTER_NONE:
  1073. dp83640->hwts_rx_en = 0;
  1074. dp83640->layer = 0;
  1075. dp83640->version = 0;
  1076. break;
  1077. case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
  1078. case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
  1079. case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
  1080. dp83640->hwts_rx_en = 1;
  1081. dp83640->layer = PTP_CLASS_L4;
  1082. dp83640->version = PTP_CLASS_V1;
  1083. break;
  1084. case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
  1085. case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
  1086. case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
  1087. dp83640->hwts_rx_en = 1;
  1088. dp83640->layer = PTP_CLASS_L4;
  1089. dp83640->version = PTP_CLASS_V2;
  1090. break;
  1091. case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
  1092. case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
  1093. case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
  1094. dp83640->hwts_rx_en = 1;
  1095. dp83640->layer = PTP_CLASS_L2;
  1096. dp83640->version = PTP_CLASS_V2;
  1097. break;
  1098. case HWTSTAMP_FILTER_PTP_V2_EVENT:
  1099. case HWTSTAMP_FILTER_PTP_V2_SYNC:
  1100. case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
  1101. dp83640->hwts_rx_en = 1;
  1102. dp83640->layer = PTP_CLASS_L4 | PTP_CLASS_L2;
  1103. dp83640->version = PTP_CLASS_V2;
  1104. break;
  1105. default:
  1106. return -ERANGE;
  1107. }
  1108. txcfg0 = (dp83640->version & TX_PTP_VER_MASK) << TX_PTP_VER_SHIFT;
  1109. rxcfg0 = (dp83640->version & TX_PTP_VER_MASK) << TX_PTP_VER_SHIFT;
  1110. if (dp83640->layer & PTP_CLASS_L2) {
  1111. txcfg0 |= TX_L2_EN;
  1112. rxcfg0 |= RX_L2_EN;
  1113. }
  1114. if (dp83640->layer & PTP_CLASS_L4) {
  1115. txcfg0 |= TX_IPV6_EN | TX_IPV4_EN;
  1116. rxcfg0 |= RX_IPV6_EN | RX_IPV4_EN;
  1117. }
  1118. if (dp83640->hwts_tx_en)
  1119. txcfg0 |= TX_TS_EN;
  1120. if (dp83640->hwts_tx_en == HWTSTAMP_TX_ONESTEP_SYNC)
  1121. txcfg0 |= SYNC_1STEP | CHK_1STEP;
  1122. if (dp83640->hwts_rx_en)
  1123. rxcfg0 |= RX_TS_EN;
  1124. mutex_lock(&dp83640->clock->extreg_lock);
  1125. ext_write(0, phydev, PAGE5, PTP_TXCFG0, txcfg0);
  1126. ext_write(0, phydev, PAGE5, PTP_RXCFG0, rxcfg0);
  1127. mutex_unlock(&dp83640->clock->extreg_lock);
  1128. return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
  1129. }
  1130. static void rx_timestamp_work(struct work_struct *work)
  1131. {
  1132. struct dp83640_private *dp83640 =
  1133. container_of(work, struct dp83640_private, ts_work.work);
  1134. struct sk_buff *skb;
  1135. /* Deliver expired packets. */
  1136. while ((skb = skb_dequeue(&dp83640->rx_queue))) {
  1137. struct dp83640_skb_info *skb_info;
  1138. skb_info = (struct dp83640_skb_info *)skb->cb;
  1139. if (!time_after(jiffies, skb_info->tmo)) {
  1140. skb_queue_head(&dp83640->rx_queue, skb);
  1141. break;
  1142. }
  1143. netif_rx_ni(skb);
  1144. }
  1145. if (!skb_queue_empty(&dp83640->rx_queue))
  1146. schedule_delayed_work(&dp83640->ts_work, SKB_TIMESTAMP_TIMEOUT);
  1147. }
  1148. static bool dp83640_rxtstamp(struct phy_device *phydev,
  1149. struct sk_buff *skb, int type)
  1150. {
  1151. struct dp83640_private *dp83640 = phydev->priv;
  1152. struct dp83640_skb_info *skb_info = (struct dp83640_skb_info *)skb->cb;
  1153. struct list_head *this, *next;
  1154. struct rxts *rxts;
  1155. struct skb_shared_hwtstamps *shhwtstamps = NULL;
  1156. unsigned long flags;
  1157. if (is_status_frame(skb, type)) {
  1158. decode_status_frame(dp83640, skb);
  1159. kfree_skb(skb);
  1160. return true;
  1161. }
  1162. if (!dp83640->hwts_rx_en)
  1163. return false;
  1164. if ((type & dp83640->version) == 0 || (type & dp83640->layer) == 0)
  1165. return false;
  1166. spin_lock_irqsave(&dp83640->rx_lock, flags);
  1167. prune_rx_ts(dp83640);
  1168. list_for_each_safe(this, next, &dp83640->rxts) {
  1169. rxts = list_entry(this, struct rxts, list);
  1170. if (match(skb, type, rxts)) {
  1171. shhwtstamps = skb_hwtstamps(skb);
  1172. memset(shhwtstamps, 0, sizeof(*shhwtstamps));
  1173. shhwtstamps->hwtstamp = ns_to_ktime(rxts->ns);
  1174. netif_rx_ni(skb);
  1175. list_del_init(&rxts->list);
  1176. list_add(&rxts->list, &dp83640->rxpool);
  1177. break;
  1178. }
  1179. }
  1180. spin_unlock_irqrestore(&dp83640->rx_lock, flags);
  1181. if (!shhwtstamps) {
  1182. skb_info->ptp_type = type;
  1183. skb_info->tmo = jiffies + SKB_TIMESTAMP_TIMEOUT;
  1184. skb_queue_tail(&dp83640->rx_queue, skb);
  1185. schedule_delayed_work(&dp83640->ts_work, SKB_TIMESTAMP_TIMEOUT);
  1186. }
  1187. return true;
  1188. }
  1189. static void dp83640_txtstamp(struct phy_device *phydev,
  1190. struct sk_buff *skb, int type)
  1191. {
  1192. struct dp83640_private *dp83640 = phydev->priv;
  1193. switch (dp83640->hwts_tx_en) {
  1194. case HWTSTAMP_TX_ONESTEP_SYNC:
  1195. if (is_sync(skb, type)) {
  1196. kfree_skb(skb);
  1197. return;
  1198. }
  1199. /* fall through */
  1200. case HWTSTAMP_TX_ON:
  1201. skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
  1202. skb_queue_tail(&dp83640->tx_queue, skb);
  1203. break;
  1204. case HWTSTAMP_TX_OFF:
  1205. default:
  1206. kfree_skb(skb);
  1207. break;
  1208. }
  1209. }
  1210. static int dp83640_ts_info(struct phy_device *dev, struct ethtool_ts_info *info)
  1211. {
  1212. struct dp83640_private *dp83640 = dev->priv;
  1213. info->so_timestamping =
  1214. SOF_TIMESTAMPING_TX_HARDWARE |
  1215. SOF_TIMESTAMPING_RX_HARDWARE |
  1216. SOF_TIMESTAMPING_RAW_HARDWARE;
  1217. info->phc_index = ptp_clock_index(dp83640->clock->ptp_clock);
  1218. info->tx_types =
  1219. (1 << HWTSTAMP_TX_OFF) |
  1220. (1 << HWTSTAMP_TX_ON) |
  1221. (1 << HWTSTAMP_TX_ONESTEP_SYNC);
  1222. info->rx_filters =
  1223. (1 << HWTSTAMP_FILTER_NONE) |
  1224. (1 << HWTSTAMP_FILTER_PTP_V1_L4_EVENT) |
  1225. (1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT) |
  1226. (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
  1227. (1 << HWTSTAMP_FILTER_PTP_V2_EVENT);
  1228. return 0;
  1229. }
  1230. static struct phy_driver dp83640_driver = {
  1231. .phy_id = DP83640_PHY_ID,
  1232. .phy_id_mask = 0xfffffff0,
  1233. .name = "NatSemi DP83640",
  1234. .features = PHY_BASIC_FEATURES,
  1235. .flags = PHY_HAS_INTERRUPT,
  1236. .probe = dp83640_probe,
  1237. .remove = dp83640_remove,
  1238. .config_init = dp83640_config_init,
  1239. .config_aneg = genphy_config_aneg,
  1240. .read_status = genphy_read_status,
  1241. .ack_interrupt = dp83640_ack_interrupt,
  1242. .config_intr = dp83640_config_intr,
  1243. .ts_info = dp83640_ts_info,
  1244. .hwtstamp = dp83640_hwtstamp,
  1245. .rxtstamp = dp83640_rxtstamp,
  1246. .txtstamp = dp83640_txtstamp,
  1247. };
  1248. static int __init dp83640_init(void)
  1249. {
  1250. return phy_driver_register(&dp83640_driver, THIS_MODULE);
  1251. }
  1252. static void __exit dp83640_exit(void)
  1253. {
  1254. dp83640_free_clocks();
  1255. phy_driver_unregister(&dp83640_driver);
  1256. }
  1257. MODULE_DESCRIPTION("National Semiconductor DP83640 PHY driver");
  1258. MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>");
  1259. MODULE_LICENSE("GPL");
  1260. module_init(dp83640_init);
  1261. module_exit(dp83640_exit);
  1262. static struct mdio_device_id __maybe_unused dp83640_tbl[] = {
  1263. { DP83640_PHY_ID, 0xfffffff0 },
  1264. { }
  1265. };
  1266. MODULE_DEVICE_TABLE(mdio, dp83640_tbl);