caif_hsi.c 34 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453
  1. /*
  2. * Copyright (C) ST-Ericsson AB 2010
  3. * Contact: Sjur Brendeland / sjur.brandeland@stericsson.com
  4. * Author: Daniel Martensson / daniel.martensson@stericsson.com
  5. * Dmitry.Tarnyagin / dmitry.tarnyagin@stericsson.com
  6. * License terms: GNU General Public License (GPL) version 2.
  7. */
  8. #define pr_fmt(fmt) KBUILD_MODNAME fmt
  9. #include <linux/init.h>
  10. #include <linux/module.h>
  11. #include <linux/device.h>
  12. #include <linux/platform_device.h>
  13. #include <linux/netdevice.h>
  14. #include <linux/string.h>
  15. #include <linux/list.h>
  16. #include <linux/interrupt.h>
  17. #include <linux/delay.h>
  18. #include <linux/sched.h>
  19. #include <linux/if_arp.h>
  20. #include <linux/timer.h>
  21. #include <linux/rtnetlink.h>
  22. #include <linux/pkt_sched.h>
  23. #include <net/caif/caif_layer.h>
  24. #include <net/caif/caif_hsi.h>
  25. MODULE_LICENSE("GPL");
  26. MODULE_AUTHOR("Daniel Martensson<daniel.martensson@stericsson.com>");
  27. MODULE_DESCRIPTION("CAIF HSI driver");
  28. /* Returns the number of padding bytes for alignment. */
  29. #define PAD_POW2(x, pow) ((((x)&((pow)-1)) == 0) ? 0 :\
  30. (((pow)-((x)&((pow)-1)))))
  31. static int inactivity_timeout = 1000;
  32. module_param(inactivity_timeout, int, S_IRUGO | S_IWUSR);
  33. MODULE_PARM_DESC(inactivity_timeout, "Inactivity timeout on HSI, ms.");
  34. static int aggregation_timeout = 1;
  35. module_param(aggregation_timeout, int, S_IRUGO | S_IWUSR);
  36. MODULE_PARM_DESC(aggregation_timeout, "Aggregation timeout on HSI, ms.");
  37. /*
  38. * HSI padding options.
  39. * Warning: must be a base of 2 (& operation used) and can not be zero !
  40. */
  41. static int hsi_head_align = 4;
  42. module_param(hsi_head_align, int, S_IRUGO);
  43. MODULE_PARM_DESC(hsi_head_align, "HSI head alignment.");
  44. static int hsi_tail_align = 4;
  45. module_param(hsi_tail_align, int, S_IRUGO);
  46. MODULE_PARM_DESC(hsi_tail_align, "HSI tail alignment.");
  47. /*
  48. * HSI link layer flowcontrol thresholds.
  49. * Warning: A high threshold value migth increase throughput but it will at
  50. * the same time prevent channel prioritization and increase the risk of
  51. * flooding the modem. The high threshold should be above the low.
  52. */
  53. static int hsi_high_threshold = 100;
  54. module_param(hsi_high_threshold, int, S_IRUGO);
  55. MODULE_PARM_DESC(hsi_high_threshold, "HSI high threshold (FLOW OFF).");
  56. static int hsi_low_threshold = 50;
  57. module_param(hsi_low_threshold, int, S_IRUGO);
  58. MODULE_PARM_DESC(hsi_low_threshold, "HSI high threshold (FLOW ON).");
  59. #define ON 1
  60. #define OFF 0
  61. /*
  62. * Threshold values for the HSI packet queue. Flowcontrol will be asserted
  63. * when the number of packets exceeds HIGH_WATER_MARK. It will not be
  64. * de-asserted before the number of packets drops below LOW_WATER_MARK.
  65. */
  66. #define LOW_WATER_MARK hsi_low_threshold
  67. #define HIGH_WATER_MARK hsi_high_threshold
  68. static LIST_HEAD(cfhsi_list);
  69. static spinlock_t cfhsi_list_lock;
  70. static void cfhsi_inactivity_tout(unsigned long arg)
  71. {
  72. struct cfhsi *cfhsi = (struct cfhsi *)arg;
  73. dev_dbg(&cfhsi->ndev->dev, "%s.\n",
  74. __func__);
  75. /* Schedule power down work queue. */
  76. if (!test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
  77. queue_work(cfhsi->wq, &cfhsi->wake_down_work);
  78. }
  79. static void cfhsi_update_aggregation_stats(struct cfhsi *cfhsi,
  80. const struct sk_buff *skb,
  81. int direction)
  82. {
  83. struct caif_payload_info *info;
  84. int hpad, tpad, len;
  85. info = (struct caif_payload_info *)&skb->cb;
  86. hpad = 1 + PAD_POW2((info->hdr_len + 1), hsi_head_align);
  87. tpad = PAD_POW2((skb->len + hpad), hsi_tail_align);
  88. len = skb->len + hpad + tpad;
  89. if (direction > 0)
  90. cfhsi->aggregation_len += len;
  91. else if (direction < 0)
  92. cfhsi->aggregation_len -= len;
  93. }
  94. static bool cfhsi_can_send_aggregate(struct cfhsi *cfhsi)
  95. {
  96. int i;
  97. if (cfhsi->aggregation_timeout < 0)
  98. return true;
  99. for (i = 0; i < CFHSI_PRIO_BEBK; ++i) {
  100. if (cfhsi->qhead[i].qlen)
  101. return true;
  102. }
  103. /* TODO: Use aggregation_len instead */
  104. if (cfhsi->qhead[CFHSI_PRIO_BEBK].qlen >= CFHSI_MAX_PKTS)
  105. return true;
  106. return false;
  107. }
  108. static struct sk_buff *cfhsi_dequeue(struct cfhsi *cfhsi)
  109. {
  110. struct sk_buff *skb;
  111. int i;
  112. for (i = 0; i < CFHSI_PRIO_LAST; ++i) {
  113. skb = skb_dequeue(&cfhsi->qhead[i]);
  114. if (skb)
  115. break;
  116. }
  117. return skb;
  118. }
  119. static int cfhsi_tx_queue_len(struct cfhsi *cfhsi)
  120. {
  121. int i, len = 0;
  122. for (i = 0; i < CFHSI_PRIO_LAST; ++i)
  123. len += skb_queue_len(&cfhsi->qhead[i]);
  124. return len;
  125. }
  126. static void cfhsi_abort_tx(struct cfhsi *cfhsi)
  127. {
  128. struct sk_buff *skb;
  129. for (;;) {
  130. spin_lock_bh(&cfhsi->lock);
  131. skb = cfhsi_dequeue(cfhsi);
  132. if (!skb)
  133. break;
  134. cfhsi->ndev->stats.tx_errors++;
  135. cfhsi->ndev->stats.tx_dropped++;
  136. cfhsi_update_aggregation_stats(cfhsi, skb, -1);
  137. spin_unlock_bh(&cfhsi->lock);
  138. kfree_skb(skb);
  139. }
  140. cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
  141. if (!test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
  142. mod_timer(&cfhsi->inactivity_timer,
  143. jiffies + cfhsi->inactivity_timeout);
  144. spin_unlock_bh(&cfhsi->lock);
  145. }
  146. static int cfhsi_flush_fifo(struct cfhsi *cfhsi)
  147. {
  148. char buffer[32]; /* Any reasonable value */
  149. size_t fifo_occupancy;
  150. int ret;
  151. dev_dbg(&cfhsi->ndev->dev, "%s.\n",
  152. __func__);
  153. do {
  154. ret = cfhsi->dev->cfhsi_fifo_occupancy(cfhsi->dev,
  155. &fifo_occupancy);
  156. if (ret) {
  157. dev_warn(&cfhsi->ndev->dev,
  158. "%s: can't get FIFO occupancy: %d.\n",
  159. __func__, ret);
  160. break;
  161. } else if (!fifo_occupancy)
  162. /* No more data, exitting normally */
  163. break;
  164. fifo_occupancy = min(sizeof(buffer), fifo_occupancy);
  165. set_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits);
  166. ret = cfhsi->dev->cfhsi_rx(buffer, fifo_occupancy,
  167. cfhsi->dev);
  168. if (ret) {
  169. clear_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits);
  170. dev_warn(&cfhsi->ndev->dev,
  171. "%s: can't read data: %d.\n",
  172. __func__, ret);
  173. break;
  174. }
  175. ret = 5 * HZ;
  176. ret = wait_event_interruptible_timeout(cfhsi->flush_fifo_wait,
  177. !test_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits), ret);
  178. if (ret < 0) {
  179. dev_warn(&cfhsi->ndev->dev,
  180. "%s: can't wait for flush complete: %d.\n",
  181. __func__, ret);
  182. break;
  183. } else if (!ret) {
  184. ret = -ETIMEDOUT;
  185. dev_warn(&cfhsi->ndev->dev,
  186. "%s: timeout waiting for flush complete.\n",
  187. __func__);
  188. break;
  189. }
  190. } while (1);
  191. return ret;
  192. }
  193. static int cfhsi_tx_frm(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
  194. {
  195. int nfrms = 0;
  196. int pld_len = 0;
  197. struct sk_buff *skb;
  198. u8 *pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ;
  199. skb = cfhsi_dequeue(cfhsi);
  200. if (!skb)
  201. return 0;
  202. /* Clear offset. */
  203. desc->offset = 0;
  204. /* Check if we can embed a CAIF frame. */
  205. if (skb->len < CFHSI_MAX_EMB_FRM_SZ) {
  206. struct caif_payload_info *info;
  207. int hpad = 0;
  208. int tpad = 0;
  209. /* Calculate needed head alignment and tail alignment. */
  210. info = (struct caif_payload_info *)&skb->cb;
  211. hpad = 1 + PAD_POW2((info->hdr_len + 1), hsi_head_align);
  212. tpad = PAD_POW2((skb->len + hpad), hsi_tail_align);
  213. /* Check if frame still fits with added alignment. */
  214. if ((skb->len + hpad + tpad) <= CFHSI_MAX_EMB_FRM_SZ) {
  215. u8 *pemb = desc->emb_frm;
  216. desc->offset = CFHSI_DESC_SHORT_SZ;
  217. *pemb = (u8)(hpad - 1);
  218. pemb += hpad;
  219. /* Update network statistics. */
  220. spin_lock_bh(&cfhsi->lock);
  221. cfhsi->ndev->stats.tx_packets++;
  222. cfhsi->ndev->stats.tx_bytes += skb->len;
  223. cfhsi_update_aggregation_stats(cfhsi, skb, -1);
  224. spin_unlock_bh(&cfhsi->lock);
  225. /* Copy in embedded CAIF frame. */
  226. skb_copy_bits(skb, 0, pemb, skb->len);
  227. /* Consume the SKB */
  228. consume_skb(skb);
  229. skb = NULL;
  230. }
  231. }
  232. /* Create payload CAIF frames. */
  233. pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ;
  234. while (nfrms < CFHSI_MAX_PKTS) {
  235. struct caif_payload_info *info;
  236. int hpad = 0;
  237. int tpad = 0;
  238. if (!skb)
  239. skb = cfhsi_dequeue(cfhsi);
  240. if (!skb)
  241. break;
  242. /* Calculate needed head alignment and tail alignment. */
  243. info = (struct caif_payload_info *)&skb->cb;
  244. hpad = 1 + PAD_POW2((info->hdr_len + 1), hsi_head_align);
  245. tpad = PAD_POW2((skb->len + hpad), hsi_tail_align);
  246. /* Fill in CAIF frame length in descriptor. */
  247. desc->cffrm_len[nfrms] = hpad + skb->len + tpad;
  248. /* Fill head padding information. */
  249. *pfrm = (u8)(hpad - 1);
  250. pfrm += hpad;
  251. /* Update network statistics. */
  252. spin_lock_bh(&cfhsi->lock);
  253. cfhsi->ndev->stats.tx_packets++;
  254. cfhsi->ndev->stats.tx_bytes += skb->len;
  255. cfhsi_update_aggregation_stats(cfhsi, skb, -1);
  256. spin_unlock_bh(&cfhsi->lock);
  257. /* Copy in CAIF frame. */
  258. skb_copy_bits(skb, 0, pfrm, skb->len);
  259. /* Update payload length. */
  260. pld_len += desc->cffrm_len[nfrms];
  261. /* Update frame pointer. */
  262. pfrm += skb->len + tpad;
  263. /* Consume the SKB */
  264. consume_skb(skb);
  265. skb = NULL;
  266. /* Update number of frames. */
  267. nfrms++;
  268. }
  269. /* Unused length fields should be zero-filled (according to SPEC). */
  270. while (nfrms < CFHSI_MAX_PKTS) {
  271. desc->cffrm_len[nfrms] = 0x0000;
  272. nfrms++;
  273. }
  274. /* Check if we can piggy-back another descriptor. */
  275. if (cfhsi_can_send_aggregate(cfhsi))
  276. desc->header |= CFHSI_PIGGY_DESC;
  277. else
  278. desc->header &= ~CFHSI_PIGGY_DESC;
  279. return CFHSI_DESC_SZ + pld_len;
  280. }
  281. static void cfhsi_start_tx(struct cfhsi *cfhsi)
  282. {
  283. struct cfhsi_desc *desc = (struct cfhsi_desc *)cfhsi->tx_buf;
  284. int len, res;
  285. dev_dbg(&cfhsi->ndev->dev, "%s.\n", __func__);
  286. if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
  287. return;
  288. do {
  289. /* Create HSI frame. */
  290. len = cfhsi_tx_frm(desc, cfhsi);
  291. if (!len) {
  292. spin_lock_bh(&cfhsi->lock);
  293. if (unlikely(cfhsi_tx_queue_len(cfhsi))) {
  294. spin_unlock_bh(&cfhsi->lock);
  295. res = -EAGAIN;
  296. continue;
  297. }
  298. cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
  299. /* Start inactivity timer. */
  300. mod_timer(&cfhsi->inactivity_timer,
  301. jiffies + cfhsi->inactivity_timeout);
  302. spin_unlock_bh(&cfhsi->lock);
  303. break;
  304. }
  305. /* Set up new transfer. */
  306. res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev);
  307. if (WARN_ON(res < 0))
  308. dev_err(&cfhsi->ndev->dev, "%s: TX error %d.\n",
  309. __func__, res);
  310. } while (res < 0);
  311. }
  312. static void cfhsi_tx_done(struct cfhsi *cfhsi)
  313. {
  314. dev_dbg(&cfhsi->ndev->dev, "%s.\n", __func__);
  315. if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
  316. return;
  317. /*
  318. * Send flow on if flow off has been previously signalled
  319. * and number of packets is below low water mark.
  320. */
  321. spin_lock_bh(&cfhsi->lock);
  322. if (cfhsi->flow_off_sent &&
  323. cfhsi_tx_queue_len(cfhsi) <= cfhsi->q_low_mark &&
  324. cfhsi->cfdev.flowctrl) {
  325. cfhsi->flow_off_sent = 0;
  326. cfhsi->cfdev.flowctrl(cfhsi->ndev, ON);
  327. }
  328. if (cfhsi_can_send_aggregate(cfhsi)) {
  329. spin_unlock_bh(&cfhsi->lock);
  330. cfhsi_start_tx(cfhsi);
  331. } else {
  332. mod_timer(&cfhsi->aggregation_timer,
  333. jiffies + cfhsi->aggregation_timeout);
  334. spin_unlock_bh(&cfhsi->lock);
  335. }
  336. return;
  337. }
  338. static void cfhsi_tx_done_cb(struct cfhsi_drv *drv)
  339. {
  340. struct cfhsi *cfhsi;
  341. cfhsi = container_of(drv, struct cfhsi, drv);
  342. dev_dbg(&cfhsi->ndev->dev, "%s.\n",
  343. __func__);
  344. if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
  345. return;
  346. cfhsi_tx_done(cfhsi);
  347. }
  348. static int cfhsi_rx_desc(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
  349. {
  350. int xfer_sz = 0;
  351. int nfrms = 0;
  352. u16 *plen = NULL;
  353. u8 *pfrm = NULL;
  354. if ((desc->header & ~CFHSI_PIGGY_DESC) ||
  355. (desc->offset > CFHSI_MAX_EMB_FRM_SZ)) {
  356. dev_err(&cfhsi->ndev->dev, "%s: Invalid descriptor.\n",
  357. __func__);
  358. return -EPROTO;
  359. }
  360. /* Check for embedded CAIF frame. */
  361. if (desc->offset) {
  362. struct sk_buff *skb;
  363. u8 *dst = NULL;
  364. int len = 0;
  365. pfrm = ((u8 *)desc) + desc->offset;
  366. /* Remove offset padding. */
  367. pfrm += *pfrm + 1;
  368. /* Read length of CAIF frame (little endian). */
  369. len = *pfrm;
  370. len |= ((*(pfrm+1)) << 8) & 0xFF00;
  371. len += 2; /* Add FCS fields. */
  372. /* Sanity check length of CAIF frame. */
  373. if (unlikely(len > CFHSI_MAX_CAIF_FRAME_SZ)) {
  374. dev_err(&cfhsi->ndev->dev, "%s: Invalid length.\n",
  375. __func__);
  376. return -EPROTO;
  377. }
  378. /* Allocate SKB (OK even in IRQ context). */
  379. skb = alloc_skb(len + 1, GFP_ATOMIC);
  380. if (!skb) {
  381. dev_err(&cfhsi->ndev->dev, "%s: Out of memory !\n",
  382. __func__);
  383. return -ENOMEM;
  384. }
  385. caif_assert(skb != NULL);
  386. dst = skb_put(skb, len);
  387. memcpy(dst, pfrm, len);
  388. skb->protocol = htons(ETH_P_CAIF);
  389. skb_reset_mac_header(skb);
  390. skb->dev = cfhsi->ndev;
  391. /*
  392. * We are called from a arch specific platform device.
  393. * Unfortunately we don't know what context we're
  394. * running in.
  395. */
  396. if (in_interrupt())
  397. netif_rx(skb);
  398. else
  399. netif_rx_ni(skb);
  400. /* Update network statistics. */
  401. cfhsi->ndev->stats.rx_packets++;
  402. cfhsi->ndev->stats.rx_bytes += len;
  403. }
  404. /* Calculate transfer length. */
  405. plen = desc->cffrm_len;
  406. while (nfrms < CFHSI_MAX_PKTS && *plen) {
  407. xfer_sz += *plen;
  408. plen++;
  409. nfrms++;
  410. }
  411. /* Check for piggy-backed descriptor. */
  412. if (desc->header & CFHSI_PIGGY_DESC)
  413. xfer_sz += CFHSI_DESC_SZ;
  414. if ((xfer_sz % 4) || (xfer_sz > (CFHSI_BUF_SZ_RX - CFHSI_DESC_SZ))) {
  415. dev_err(&cfhsi->ndev->dev,
  416. "%s: Invalid payload len: %d, ignored.\n",
  417. __func__, xfer_sz);
  418. return -EPROTO;
  419. }
  420. return xfer_sz;
  421. }
  422. static int cfhsi_rx_desc_len(struct cfhsi_desc *desc)
  423. {
  424. int xfer_sz = 0;
  425. int nfrms = 0;
  426. u16 *plen;
  427. if ((desc->header & ~CFHSI_PIGGY_DESC) ||
  428. (desc->offset > CFHSI_MAX_EMB_FRM_SZ)) {
  429. pr_err("Invalid descriptor. %x %x\n", desc->header,
  430. desc->offset);
  431. return -EPROTO;
  432. }
  433. /* Calculate transfer length. */
  434. plen = desc->cffrm_len;
  435. while (nfrms < CFHSI_MAX_PKTS && *plen) {
  436. xfer_sz += *plen;
  437. plen++;
  438. nfrms++;
  439. }
  440. if (xfer_sz % 4) {
  441. pr_err("Invalid payload len: %d, ignored.\n", xfer_sz);
  442. return -EPROTO;
  443. }
  444. return xfer_sz;
  445. }
  446. static int cfhsi_rx_pld(struct cfhsi_desc *desc, struct cfhsi *cfhsi)
  447. {
  448. int rx_sz = 0;
  449. int nfrms = 0;
  450. u16 *plen = NULL;
  451. u8 *pfrm = NULL;
  452. /* Sanity check header and offset. */
  453. if (WARN_ON((desc->header & ~CFHSI_PIGGY_DESC) ||
  454. (desc->offset > CFHSI_MAX_EMB_FRM_SZ))) {
  455. dev_err(&cfhsi->ndev->dev, "%s: Invalid descriptor.\n",
  456. __func__);
  457. return -EPROTO;
  458. }
  459. /* Set frame pointer to start of payload. */
  460. pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ;
  461. plen = desc->cffrm_len;
  462. /* Skip already processed frames. */
  463. while (nfrms < cfhsi->rx_state.nfrms) {
  464. pfrm += *plen;
  465. rx_sz += *plen;
  466. plen++;
  467. nfrms++;
  468. }
  469. /* Parse payload. */
  470. while (nfrms < CFHSI_MAX_PKTS && *plen) {
  471. struct sk_buff *skb;
  472. u8 *dst = NULL;
  473. u8 *pcffrm = NULL;
  474. int len = 0;
  475. /* CAIF frame starts after head padding. */
  476. pcffrm = pfrm + *pfrm + 1;
  477. /* Read length of CAIF frame (little endian). */
  478. len = *pcffrm;
  479. len |= ((*(pcffrm + 1)) << 8) & 0xFF00;
  480. len += 2; /* Add FCS fields. */
  481. /* Sanity check length of CAIF frames. */
  482. if (unlikely(len > CFHSI_MAX_CAIF_FRAME_SZ)) {
  483. dev_err(&cfhsi->ndev->dev, "%s: Invalid length.\n",
  484. __func__);
  485. return -EPROTO;
  486. }
  487. /* Allocate SKB (OK even in IRQ context). */
  488. skb = alloc_skb(len + 1, GFP_ATOMIC);
  489. if (!skb) {
  490. dev_err(&cfhsi->ndev->dev, "%s: Out of memory !\n",
  491. __func__);
  492. cfhsi->rx_state.nfrms = nfrms;
  493. return -ENOMEM;
  494. }
  495. caif_assert(skb != NULL);
  496. dst = skb_put(skb, len);
  497. memcpy(dst, pcffrm, len);
  498. skb->protocol = htons(ETH_P_CAIF);
  499. skb_reset_mac_header(skb);
  500. skb->dev = cfhsi->ndev;
  501. /*
  502. * We're called from a platform device,
  503. * and don't know the context we're running in.
  504. */
  505. if (in_interrupt())
  506. netif_rx(skb);
  507. else
  508. netif_rx_ni(skb);
  509. /* Update network statistics. */
  510. cfhsi->ndev->stats.rx_packets++;
  511. cfhsi->ndev->stats.rx_bytes += len;
  512. pfrm += *plen;
  513. rx_sz += *plen;
  514. plen++;
  515. nfrms++;
  516. }
  517. return rx_sz;
  518. }
  519. static void cfhsi_rx_done(struct cfhsi *cfhsi)
  520. {
  521. int res;
  522. int desc_pld_len = 0, rx_len, rx_state;
  523. struct cfhsi_desc *desc = NULL;
  524. u8 *rx_ptr, *rx_buf;
  525. struct cfhsi_desc *piggy_desc = NULL;
  526. desc = (struct cfhsi_desc *)cfhsi->rx_buf;
  527. dev_dbg(&cfhsi->ndev->dev, "%s\n", __func__);
  528. if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
  529. return;
  530. /* Update inactivity timer if pending. */
  531. spin_lock_bh(&cfhsi->lock);
  532. mod_timer_pending(&cfhsi->inactivity_timer,
  533. jiffies + cfhsi->inactivity_timeout);
  534. spin_unlock_bh(&cfhsi->lock);
  535. if (cfhsi->rx_state.state == CFHSI_RX_STATE_DESC) {
  536. desc_pld_len = cfhsi_rx_desc_len(desc);
  537. if (desc_pld_len < 0)
  538. goto out_of_sync;
  539. rx_buf = cfhsi->rx_buf;
  540. rx_len = desc_pld_len;
  541. if (desc_pld_len > 0 && (desc->header & CFHSI_PIGGY_DESC))
  542. rx_len += CFHSI_DESC_SZ;
  543. if (desc_pld_len == 0)
  544. rx_buf = cfhsi->rx_flip_buf;
  545. } else {
  546. rx_buf = cfhsi->rx_flip_buf;
  547. rx_len = CFHSI_DESC_SZ;
  548. if (cfhsi->rx_state.pld_len > 0 &&
  549. (desc->header & CFHSI_PIGGY_DESC)) {
  550. piggy_desc = (struct cfhsi_desc *)
  551. (desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ +
  552. cfhsi->rx_state.pld_len);
  553. cfhsi->rx_state.piggy_desc = true;
  554. /* Extract payload len from piggy-backed descriptor. */
  555. desc_pld_len = cfhsi_rx_desc_len(piggy_desc);
  556. if (desc_pld_len < 0)
  557. goto out_of_sync;
  558. if (desc_pld_len > 0)
  559. rx_len = desc_pld_len;
  560. if (desc_pld_len > 0 &&
  561. (piggy_desc->header & CFHSI_PIGGY_DESC))
  562. rx_len += CFHSI_DESC_SZ;
  563. /*
  564. * Copy needed information from the piggy-backed
  565. * descriptor to the descriptor in the start.
  566. */
  567. memcpy(rx_buf, (u8 *)piggy_desc,
  568. CFHSI_DESC_SHORT_SZ);
  569. if (desc_pld_len == -EPROTO)
  570. goto out_of_sync;
  571. }
  572. }
  573. if (desc_pld_len) {
  574. rx_state = CFHSI_RX_STATE_PAYLOAD;
  575. rx_ptr = rx_buf + CFHSI_DESC_SZ;
  576. } else {
  577. rx_state = CFHSI_RX_STATE_DESC;
  578. rx_ptr = rx_buf;
  579. rx_len = CFHSI_DESC_SZ;
  580. }
  581. /* Initiate next read */
  582. if (test_bit(CFHSI_AWAKE, &cfhsi->bits)) {
  583. /* Set up new transfer. */
  584. dev_dbg(&cfhsi->ndev->dev, "%s: Start RX.\n",
  585. __func__);
  586. res = cfhsi->dev->cfhsi_rx(rx_ptr, rx_len,
  587. cfhsi->dev);
  588. if (WARN_ON(res < 0)) {
  589. dev_err(&cfhsi->ndev->dev, "%s: RX error %d.\n",
  590. __func__, res);
  591. cfhsi->ndev->stats.rx_errors++;
  592. cfhsi->ndev->stats.rx_dropped++;
  593. }
  594. }
  595. if (cfhsi->rx_state.state == CFHSI_RX_STATE_DESC) {
  596. /* Extract payload from descriptor */
  597. if (cfhsi_rx_desc(desc, cfhsi) < 0)
  598. goto out_of_sync;
  599. } else {
  600. /* Extract payload */
  601. if (cfhsi_rx_pld(desc, cfhsi) < 0)
  602. goto out_of_sync;
  603. if (piggy_desc) {
  604. /* Extract any payload in piggyback descriptor. */
  605. if (cfhsi_rx_desc(piggy_desc, cfhsi) < 0)
  606. goto out_of_sync;
  607. /* Mark no embedded frame after extracting it */
  608. piggy_desc->offset = 0;
  609. }
  610. }
  611. /* Update state info */
  612. memset(&cfhsi->rx_state, 0, sizeof(cfhsi->rx_state));
  613. cfhsi->rx_state.state = rx_state;
  614. cfhsi->rx_ptr = rx_ptr;
  615. cfhsi->rx_len = rx_len;
  616. cfhsi->rx_state.pld_len = desc_pld_len;
  617. cfhsi->rx_state.piggy_desc = desc->header & CFHSI_PIGGY_DESC;
  618. if (rx_buf != cfhsi->rx_buf)
  619. swap(cfhsi->rx_buf, cfhsi->rx_flip_buf);
  620. return;
  621. out_of_sync:
  622. dev_err(&cfhsi->ndev->dev, "%s: Out of sync.\n", __func__);
  623. print_hex_dump_bytes("--> ", DUMP_PREFIX_NONE,
  624. cfhsi->rx_buf, CFHSI_DESC_SZ);
  625. schedule_work(&cfhsi->out_of_sync_work);
  626. }
  627. static void cfhsi_rx_slowpath(unsigned long arg)
  628. {
  629. struct cfhsi *cfhsi = (struct cfhsi *)arg;
  630. dev_dbg(&cfhsi->ndev->dev, "%s.\n",
  631. __func__);
  632. cfhsi_rx_done(cfhsi);
  633. }
  634. static void cfhsi_rx_done_cb(struct cfhsi_drv *drv)
  635. {
  636. struct cfhsi *cfhsi;
  637. cfhsi = container_of(drv, struct cfhsi, drv);
  638. dev_dbg(&cfhsi->ndev->dev, "%s.\n",
  639. __func__);
  640. if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
  641. return;
  642. if (test_and_clear_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits))
  643. wake_up_interruptible(&cfhsi->flush_fifo_wait);
  644. else
  645. cfhsi_rx_done(cfhsi);
  646. }
  647. static void cfhsi_wake_up(struct work_struct *work)
  648. {
  649. struct cfhsi *cfhsi = NULL;
  650. int res;
  651. int len;
  652. long ret;
  653. cfhsi = container_of(work, struct cfhsi, wake_up_work);
  654. if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
  655. return;
  656. if (unlikely(test_bit(CFHSI_AWAKE, &cfhsi->bits))) {
  657. /* It happenes when wakeup is requested by
  658. * both ends at the same time. */
  659. clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
  660. clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
  661. return;
  662. }
  663. /* Activate wake line. */
  664. cfhsi->dev->cfhsi_wake_up(cfhsi->dev);
  665. dev_dbg(&cfhsi->ndev->dev, "%s: Start waiting.\n",
  666. __func__);
  667. /* Wait for acknowledge. */
  668. ret = CFHSI_WAKE_TOUT;
  669. ret = wait_event_interruptible_timeout(cfhsi->wake_up_wait,
  670. test_and_clear_bit(CFHSI_WAKE_UP_ACK,
  671. &cfhsi->bits), ret);
  672. if (unlikely(ret < 0)) {
  673. /* Interrupted by signal. */
  674. dev_err(&cfhsi->ndev->dev, "%s: Signalled: %ld.\n",
  675. __func__, ret);
  676. clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
  677. cfhsi->dev->cfhsi_wake_down(cfhsi->dev);
  678. return;
  679. } else if (!ret) {
  680. bool ca_wake = false;
  681. size_t fifo_occupancy = 0;
  682. /* Wakeup timeout */
  683. dev_dbg(&cfhsi->ndev->dev, "%s: Timeout.\n",
  684. __func__);
  685. /* Check FIFO to check if modem has sent something. */
  686. WARN_ON(cfhsi->dev->cfhsi_fifo_occupancy(cfhsi->dev,
  687. &fifo_occupancy));
  688. dev_dbg(&cfhsi->ndev->dev, "%s: Bytes in FIFO: %u.\n",
  689. __func__, (unsigned) fifo_occupancy);
  690. /* Check if we misssed the interrupt. */
  691. WARN_ON(cfhsi->dev->cfhsi_get_peer_wake(cfhsi->dev,
  692. &ca_wake));
  693. if (ca_wake) {
  694. dev_err(&cfhsi->ndev->dev, "%s: CA Wake missed !.\n",
  695. __func__);
  696. /* Clear the CFHSI_WAKE_UP_ACK bit to prevent race. */
  697. clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
  698. /* Continue execution. */
  699. goto wake_ack;
  700. }
  701. clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
  702. cfhsi->dev->cfhsi_wake_down(cfhsi->dev);
  703. return;
  704. }
  705. wake_ack:
  706. dev_dbg(&cfhsi->ndev->dev, "%s: Woken.\n",
  707. __func__);
  708. /* Clear power up bit. */
  709. set_bit(CFHSI_AWAKE, &cfhsi->bits);
  710. clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
  711. /* Resume read operation. */
  712. dev_dbg(&cfhsi->ndev->dev, "%s: Start RX.\n", __func__);
  713. res = cfhsi->dev->cfhsi_rx(cfhsi->rx_ptr, cfhsi->rx_len, cfhsi->dev);
  714. if (WARN_ON(res < 0))
  715. dev_err(&cfhsi->ndev->dev, "%s: RX err %d.\n", __func__, res);
  716. /* Clear power up acknowledment. */
  717. clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
  718. spin_lock_bh(&cfhsi->lock);
  719. /* Resume transmit if queues are not empty. */
  720. if (!cfhsi_tx_queue_len(cfhsi)) {
  721. dev_dbg(&cfhsi->ndev->dev, "%s: Peer wake, start timer.\n",
  722. __func__);
  723. /* Start inactivity timer. */
  724. mod_timer(&cfhsi->inactivity_timer,
  725. jiffies + cfhsi->inactivity_timeout);
  726. spin_unlock_bh(&cfhsi->lock);
  727. return;
  728. }
  729. dev_dbg(&cfhsi->ndev->dev, "%s: Host wake.\n",
  730. __func__);
  731. spin_unlock_bh(&cfhsi->lock);
  732. /* Create HSI frame. */
  733. len = cfhsi_tx_frm((struct cfhsi_desc *)cfhsi->tx_buf, cfhsi);
  734. if (likely(len > 0)) {
  735. /* Set up new transfer. */
  736. res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev);
  737. if (WARN_ON(res < 0)) {
  738. dev_err(&cfhsi->ndev->dev, "%s: TX error %d.\n",
  739. __func__, res);
  740. cfhsi_abort_tx(cfhsi);
  741. }
  742. } else {
  743. dev_err(&cfhsi->ndev->dev,
  744. "%s: Failed to create HSI frame: %d.\n",
  745. __func__, len);
  746. }
  747. }
  748. static void cfhsi_wake_down(struct work_struct *work)
  749. {
  750. long ret;
  751. struct cfhsi *cfhsi = NULL;
  752. size_t fifo_occupancy = 0;
  753. int retry = CFHSI_WAKE_TOUT;
  754. cfhsi = container_of(work, struct cfhsi, wake_down_work);
  755. dev_dbg(&cfhsi->ndev->dev, "%s.\n", __func__);
  756. if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
  757. return;
  758. /* Deactivate wake line. */
  759. cfhsi->dev->cfhsi_wake_down(cfhsi->dev);
  760. /* Wait for acknowledge. */
  761. ret = CFHSI_WAKE_TOUT;
  762. ret = wait_event_interruptible_timeout(cfhsi->wake_down_wait,
  763. test_and_clear_bit(CFHSI_WAKE_DOWN_ACK,
  764. &cfhsi->bits), ret);
  765. if (ret < 0) {
  766. /* Interrupted by signal. */
  767. dev_err(&cfhsi->ndev->dev, "%s: Signalled: %ld.\n",
  768. __func__, ret);
  769. return;
  770. } else if (!ret) {
  771. bool ca_wake = true;
  772. /* Timeout */
  773. dev_err(&cfhsi->ndev->dev, "%s: Timeout.\n", __func__);
  774. /* Check if we misssed the interrupt. */
  775. WARN_ON(cfhsi->dev->cfhsi_get_peer_wake(cfhsi->dev,
  776. &ca_wake));
  777. if (!ca_wake)
  778. dev_err(&cfhsi->ndev->dev, "%s: CA Wake missed !.\n",
  779. __func__);
  780. }
  781. /* Check FIFO occupancy. */
  782. while (retry) {
  783. WARN_ON(cfhsi->dev->cfhsi_fifo_occupancy(cfhsi->dev,
  784. &fifo_occupancy));
  785. if (!fifo_occupancy)
  786. break;
  787. set_current_state(TASK_INTERRUPTIBLE);
  788. schedule_timeout(1);
  789. retry--;
  790. }
  791. if (!retry)
  792. dev_err(&cfhsi->ndev->dev, "%s: FIFO Timeout.\n", __func__);
  793. /* Clear AWAKE condition. */
  794. clear_bit(CFHSI_AWAKE, &cfhsi->bits);
  795. /* Cancel pending RX requests. */
  796. cfhsi->dev->cfhsi_rx_cancel(cfhsi->dev);
  797. }
  798. static void cfhsi_out_of_sync(struct work_struct *work)
  799. {
  800. struct cfhsi *cfhsi = NULL;
  801. cfhsi = container_of(work, struct cfhsi, out_of_sync_work);
  802. rtnl_lock();
  803. dev_close(cfhsi->ndev);
  804. rtnl_unlock();
  805. }
  806. static void cfhsi_wake_up_cb(struct cfhsi_drv *drv)
  807. {
  808. struct cfhsi *cfhsi = NULL;
  809. cfhsi = container_of(drv, struct cfhsi, drv);
  810. dev_dbg(&cfhsi->ndev->dev, "%s.\n",
  811. __func__);
  812. set_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
  813. wake_up_interruptible(&cfhsi->wake_up_wait);
  814. if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))
  815. return;
  816. /* Schedule wake up work queue if the peer initiates. */
  817. if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits))
  818. queue_work(cfhsi->wq, &cfhsi->wake_up_work);
  819. }
  820. static void cfhsi_wake_down_cb(struct cfhsi_drv *drv)
  821. {
  822. struct cfhsi *cfhsi = NULL;
  823. cfhsi = container_of(drv, struct cfhsi, drv);
  824. dev_dbg(&cfhsi->ndev->dev, "%s.\n",
  825. __func__);
  826. /* Initiating low power is only permitted by the host (us). */
  827. set_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits);
  828. wake_up_interruptible(&cfhsi->wake_down_wait);
  829. }
  830. static void cfhsi_aggregation_tout(unsigned long arg)
  831. {
  832. struct cfhsi *cfhsi = (struct cfhsi *)arg;
  833. dev_dbg(&cfhsi->ndev->dev, "%s.\n",
  834. __func__);
  835. cfhsi_start_tx(cfhsi);
  836. }
  837. static int cfhsi_xmit(struct sk_buff *skb, struct net_device *dev)
  838. {
  839. struct cfhsi *cfhsi = NULL;
  840. int start_xfer = 0;
  841. int timer_active;
  842. int prio;
  843. if (!dev)
  844. return -EINVAL;
  845. cfhsi = netdev_priv(dev);
  846. switch (skb->priority) {
  847. case TC_PRIO_BESTEFFORT:
  848. case TC_PRIO_FILLER:
  849. case TC_PRIO_BULK:
  850. prio = CFHSI_PRIO_BEBK;
  851. break;
  852. case TC_PRIO_INTERACTIVE_BULK:
  853. prio = CFHSI_PRIO_VI;
  854. break;
  855. case TC_PRIO_INTERACTIVE:
  856. prio = CFHSI_PRIO_VO;
  857. break;
  858. case TC_PRIO_CONTROL:
  859. default:
  860. prio = CFHSI_PRIO_CTL;
  861. break;
  862. }
  863. spin_lock_bh(&cfhsi->lock);
  864. /* Update aggregation statistics */
  865. cfhsi_update_aggregation_stats(cfhsi, skb, 1);
  866. /* Queue the SKB */
  867. skb_queue_tail(&cfhsi->qhead[prio], skb);
  868. /* Sanity check; xmit should not be called after unregister_netdev */
  869. if (WARN_ON(test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))) {
  870. spin_unlock_bh(&cfhsi->lock);
  871. cfhsi_abort_tx(cfhsi);
  872. return -EINVAL;
  873. }
  874. /* Send flow off if number of packets is above high water mark. */
  875. if (!cfhsi->flow_off_sent &&
  876. cfhsi_tx_queue_len(cfhsi) > cfhsi->q_high_mark &&
  877. cfhsi->cfdev.flowctrl) {
  878. cfhsi->flow_off_sent = 1;
  879. cfhsi->cfdev.flowctrl(cfhsi->ndev, OFF);
  880. }
  881. if (cfhsi->tx_state == CFHSI_TX_STATE_IDLE) {
  882. cfhsi->tx_state = CFHSI_TX_STATE_XFER;
  883. start_xfer = 1;
  884. }
  885. if (!start_xfer) {
  886. /* Send aggregate if it is possible */
  887. bool aggregate_ready =
  888. cfhsi_can_send_aggregate(cfhsi) &&
  889. del_timer(&cfhsi->aggregation_timer) > 0;
  890. spin_unlock_bh(&cfhsi->lock);
  891. if (aggregate_ready)
  892. cfhsi_start_tx(cfhsi);
  893. return 0;
  894. }
  895. /* Delete inactivity timer if started. */
  896. timer_active = del_timer_sync(&cfhsi->inactivity_timer);
  897. spin_unlock_bh(&cfhsi->lock);
  898. if (timer_active) {
  899. struct cfhsi_desc *desc = (struct cfhsi_desc *)cfhsi->tx_buf;
  900. int len;
  901. int res;
  902. /* Create HSI frame. */
  903. len = cfhsi_tx_frm(desc, cfhsi);
  904. WARN_ON(!len);
  905. /* Set up new transfer. */
  906. res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev);
  907. if (WARN_ON(res < 0)) {
  908. dev_err(&cfhsi->ndev->dev, "%s: TX error %d.\n",
  909. __func__, res);
  910. cfhsi_abort_tx(cfhsi);
  911. }
  912. } else {
  913. /* Schedule wake up work queue if the we initiate. */
  914. if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits))
  915. queue_work(cfhsi->wq, &cfhsi->wake_up_work);
  916. }
  917. return 0;
  918. }
  919. static const struct net_device_ops cfhsi_ops;
  920. static void cfhsi_setup(struct net_device *dev)
  921. {
  922. int i;
  923. struct cfhsi *cfhsi = netdev_priv(dev);
  924. dev->features = 0;
  925. dev->netdev_ops = &cfhsi_ops;
  926. dev->type = ARPHRD_CAIF;
  927. dev->flags = IFF_POINTOPOINT | IFF_NOARP;
  928. dev->mtu = CFHSI_MAX_CAIF_FRAME_SZ;
  929. dev->tx_queue_len = 0;
  930. dev->destructor = free_netdev;
  931. for (i = 0; i < CFHSI_PRIO_LAST; ++i)
  932. skb_queue_head_init(&cfhsi->qhead[i]);
  933. cfhsi->cfdev.link_select = CAIF_LINK_HIGH_BANDW;
  934. cfhsi->cfdev.use_frag = false;
  935. cfhsi->cfdev.use_stx = false;
  936. cfhsi->cfdev.use_fcs = false;
  937. cfhsi->ndev = dev;
  938. }
  939. int cfhsi_probe(struct platform_device *pdev)
  940. {
  941. struct cfhsi *cfhsi = NULL;
  942. struct net_device *ndev;
  943. int res;
  944. ndev = alloc_netdev(sizeof(struct cfhsi), "cfhsi%d", cfhsi_setup);
  945. if (!ndev)
  946. return -ENODEV;
  947. cfhsi = netdev_priv(ndev);
  948. cfhsi->ndev = ndev;
  949. cfhsi->pdev = pdev;
  950. /* Assign the HSI device. */
  951. cfhsi->dev = pdev->dev.platform_data;
  952. /* Assign the driver to this HSI device. */
  953. cfhsi->dev->drv = &cfhsi->drv;
  954. /* Register network device. */
  955. res = register_netdev(ndev);
  956. if (res) {
  957. dev_err(&ndev->dev, "%s: Registration error: %d.\n",
  958. __func__, res);
  959. free_netdev(ndev);
  960. return -ENODEV;
  961. }
  962. /* Add CAIF HSI device to list. */
  963. spin_lock(&cfhsi_list_lock);
  964. list_add_tail(&cfhsi->list, &cfhsi_list);
  965. spin_unlock(&cfhsi_list_lock);
  966. return res;
  967. }
  968. static int cfhsi_open(struct net_device *ndev)
  969. {
  970. struct cfhsi *cfhsi = netdev_priv(ndev);
  971. int res;
  972. clear_bit(CFHSI_SHUTDOWN, &cfhsi->bits);
  973. /* Initialize state vaiables. */
  974. cfhsi->tx_state = CFHSI_TX_STATE_IDLE;
  975. cfhsi->rx_state.state = CFHSI_RX_STATE_DESC;
  976. /* Set flow info */
  977. cfhsi->flow_off_sent = 0;
  978. cfhsi->q_low_mark = LOW_WATER_MARK;
  979. cfhsi->q_high_mark = HIGH_WATER_MARK;
  980. /*
  981. * Allocate a TX buffer with the size of a HSI packet descriptors
  982. * and the necessary room for CAIF payload frames.
  983. */
  984. cfhsi->tx_buf = kzalloc(CFHSI_BUF_SZ_TX, GFP_KERNEL);
  985. if (!cfhsi->tx_buf) {
  986. res = -ENODEV;
  987. goto err_alloc_tx;
  988. }
  989. /*
  990. * Allocate a RX buffer with the size of two HSI packet descriptors and
  991. * the necessary room for CAIF payload frames.
  992. */
  993. cfhsi->rx_buf = kzalloc(CFHSI_BUF_SZ_RX, GFP_KERNEL);
  994. if (!cfhsi->rx_buf) {
  995. res = -ENODEV;
  996. goto err_alloc_rx;
  997. }
  998. cfhsi->rx_flip_buf = kzalloc(CFHSI_BUF_SZ_RX, GFP_KERNEL);
  999. if (!cfhsi->rx_flip_buf) {
  1000. res = -ENODEV;
  1001. goto err_alloc_rx_flip;
  1002. }
  1003. /* Pre-calculate inactivity timeout. */
  1004. if (inactivity_timeout != -1) {
  1005. cfhsi->inactivity_timeout =
  1006. inactivity_timeout * HZ / 1000;
  1007. if (!cfhsi->inactivity_timeout)
  1008. cfhsi->inactivity_timeout = 1;
  1009. else if (cfhsi->inactivity_timeout > NEXT_TIMER_MAX_DELTA)
  1010. cfhsi->inactivity_timeout = NEXT_TIMER_MAX_DELTA;
  1011. } else {
  1012. cfhsi->inactivity_timeout = NEXT_TIMER_MAX_DELTA;
  1013. }
  1014. /* Initialize aggregation timeout */
  1015. cfhsi->aggregation_timeout = aggregation_timeout;
  1016. /* Initialize recieve vaiables. */
  1017. cfhsi->rx_ptr = cfhsi->rx_buf;
  1018. cfhsi->rx_len = CFHSI_DESC_SZ;
  1019. /* Initialize spin locks. */
  1020. spin_lock_init(&cfhsi->lock);
  1021. /* Set up the driver. */
  1022. cfhsi->drv.tx_done_cb = cfhsi_tx_done_cb;
  1023. cfhsi->drv.rx_done_cb = cfhsi_rx_done_cb;
  1024. cfhsi->drv.wake_up_cb = cfhsi_wake_up_cb;
  1025. cfhsi->drv.wake_down_cb = cfhsi_wake_down_cb;
  1026. /* Initialize the work queues. */
  1027. INIT_WORK(&cfhsi->wake_up_work, cfhsi_wake_up);
  1028. INIT_WORK(&cfhsi->wake_down_work, cfhsi_wake_down);
  1029. INIT_WORK(&cfhsi->out_of_sync_work, cfhsi_out_of_sync);
  1030. /* Clear all bit fields. */
  1031. clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits);
  1032. clear_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits);
  1033. clear_bit(CFHSI_WAKE_UP, &cfhsi->bits);
  1034. clear_bit(CFHSI_AWAKE, &cfhsi->bits);
  1035. /* Create work thread. */
  1036. cfhsi->wq = create_singlethread_workqueue(cfhsi->pdev->name);
  1037. if (!cfhsi->wq) {
  1038. dev_err(&cfhsi->ndev->dev, "%s: Failed to create work queue.\n",
  1039. __func__);
  1040. res = -ENODEV;
  1041. goto err_create_wq;
  1042. }
  1043. /* Initialize wait queues. */
  1044. init_waitqueue_head(&cfhsi->wake_up_wait);
  1045. init_waitqueue_head(&cfhsi->wake_down_wait);
  1046. init_waitqueue_head(&cfhsi->flush_fifo_wait);
  1047. /* Setup the inactivity timer. */
  1048. init_timer(&cfhsi->inactivity_timer);
  1049. cfhsi->inactivity_timer.data = (unsigned long)cfhsi;
  1050. cfhsi->inactivity_timer.function = cfhsi_inactivity_tout;
  1051. /* Setup the slowpath RX timer. */
  1052. init_timer(&cfhsi->rx_slowpath_timer);
  1053. cfhsi->rx_slowpath_timer.data = (unsigned long)cfhsi;
  1054. cfhsi->rx_slowpath_timer.function = cfhsi_rx_slowpath;
  1055. /* Setup the aggregation timer. */
  1056. init_timer(&cfhsi->aggregation_timer);
  1057. cfhsi->aggregation_timer.data = (unsigned long)cfhsi;
  1058. cfhsi->aggregation_timer.function = cfhsi_aggregation_tout;
  1059. /* Activate HSI interface. */
  1060. res = cfhsi->dev->cfhsi_up(cfhsi->dev);
  1061. if (res) {
  1062. dev_err(&cfhsi->ndev->dev,
  1063. "%s: can't activate HSI interface: %d.\n",
  1064. __func__, res);
  1065. goto err_activate;
  1066. }
  1067. /* Flush FIFO */
  1068. res = cfhsi_flush_fifo(cfhsi);
  1069. if (res) {
  1070. dev_err(&cfhsi->ndev->dev, "%s: Can't flush FIFO: %d.\n",
  1071. __func__, res);
  1072. goto err_net_reg;
  1073. }
  1074. return res;
  1075. err_net_reg:
  1076. cfhsi->dev->cfhsi_down(cfhsi->dev);
  1077. err_activate:
  1078. destroy_workqueue(cfhsi->wq);
  1079. err_create_wq:
  1080. kfree(cfhsi->rx_flip_buf);
  1081. err_alloc_rx_flip:
  1082. kfree(cfhsi->rx_buf);
  1083. err_alloc_rx:
  1084. kfree(cfhsi->tx_buf);
  1085. err_alloc_tx:
  1086. return res;
  1087. }
  1088. static int cfhsi_close(struct net_device *ndev)
  1089. {
  1090. struct cfhsi *cfhsi = netdev_priv(ndev);
  1091. u8 *tx_buf, *rx_buf, *flip_buf;
  1092. /* going to shutdown driver */
  1093. set_bit(CFHSI_SHUTDOWN, &cfhsi->bits);
  1094. /* Flush workqueue */
  1095. flush_workqueue(cfhsi->wq);
  1096. /* Delete timers if pending */
  1097. del_timer_sync(&cfhsi->inactivity_timer);
  1098. del_timer_sync(&cfhsi->rx_slowpath_timer);
  1099. del_timer_sync(&cfhsi->aggregation_timer);
  1100. /* Cancel pending RX request (if any) */
  1101. cfhsi->dev->cfhsi_rx_cancel(cfhsi->dev);
  1102. /* Destroy workqueue */
  1103. destroy_workqueue(cfhsi->wq);
  1104. /* Store bufferes: will be freed later. */
  1105. tx_buf = cfhsi->tx_buf;
  1106. rx_buf = cfhsi->rx_buf;
  1107. flip_buf = cfhsi->rx_flip_buf;
  1108. /* Flush transmit queues. */
  1109. cfhsi_abort_tx(cfhsi);
  1110. /* Deactivate interface */
  1111. cfhsi->dev->cfhsi_down(cfhsi->dev);
  1112. /* Free buffers. */
  1113. kfree(tx_buf);
  1114. kfree(rx_buf);
  1115. kfree(flip_buf);
  1116. return 0;
  1117. }
  1118. static const struct net_device_ops cfhsi_ops = {
  1119. .ndo_open = cfhsi_open,
  1120. .ndo_stop = cfhsi_close,
  1121. .ndo_start_xmit = cfhsi_xmit
  1122. };
  1123. int cfhsi_remove(struct platform_device *pdev)
  1124. {
  1125. struct list_head *list_node;
  1126. struct list_head *n;
  1127. struct cfhsi *cfhsi = NULL;
  1128. struct cfhsi_dev *dev;
  1129. dev = (struct cfhsi_dev *)pdev->dev.platform_data;
  1130. spin_lock(&cfhsi_list_lock);
  1131. list_for_each_safe(list_node, n, &cfhsi_list) {
  1132. cfhsi = list_entry(list_node, struct cfhsi, list);
  1133. /* Find the corresponding device. */
  1134. if (cfhsi->dev == dev) {
  1135. /* Remove from list. */
  1136. list_del(list_node);
  1137. spin_unlock(&cfhsi_list_lock);
  1138. return 0;
  1139. }
  1140. }
  1141. spin_unlock(&cfhsi_list_lock);
  1142. return -ENODEV;
  1143. }
  1144. struct platform_driver cfhsi_plat_drv = {
  1145. .probe = cfhsi_probe,
  1146. .remove = cfhsi_remove,
  1147. .driver = {
  1148. .name = "cfhsi",
  1149. .owner = THIS_MODULE,
  1150. },
  1151. };
  1152. static void __exit cfhsi_exit_module(void)
  1153. {
  1154. struct list_head *list_node;
  1155. struct list_head *n;
  1156. struct cfhsi *cfhsi = NULL;
  1157. spin_lock(&cfhsi_list_lock);
  1158. list_for_each_safe(list_node, n, &cfhsi_list) {
  1159. cfhsi = list_entry(list_node, struct cfhsi, list);
  1160. /* Remove from list. */
  1161. list_del(list_node);
  1162. spin_unlock(&cfhsi_list_lock);
  1163. unregister_netdevice(cfhsi->ndev);
  1164. spin_lock(&cfhsi_list_lock);
  1165. }
  1166. spin_unlock(&cfhsi_list_lock);
  1167. /* Unregister platform driver. */
  1168. platform_driver_unregister(&cfhsi_plat_drv);
  1169. }
  1170. static int __init cfhsi_init_module(void)
  1171. {
  1172. int result;
  1173. /* Initialize spin lock. */
  1174. spin_lock_init(&cfhsi_list_lock);
  1175. /* Register platform driver. */
  1176. result = platform_driver_register(&cfhsi_plat_drv);
  1177. if (result) {
  1178. printk(KERN_ERR "Could not register platform HSI driver: %d.\n",
  1179. result);
  1180. goto err_dev_register;
  1181. }
  1182. err_dev_register:
  1183. return result;
  1184. }
  1185. module_init(cfhsi_init_module);
  1186. module_exit(cfhsi_exit_module);