caif_virtio.c 21 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * Copyright (C) ST-Ericsson AB 2013
  4. * Authors: Vicram Arv
  5. * Dmitry Tarnyagin <dmitry.tarnyagin@lockless.no>
  6. * Sjur Brendeland
  7. */
  8. #include <linux/module.h>
  9. #include <linux/if_arp.h>
  10. #include <linux/virtio.h>
  11. #include <linux/vringh.h>
  12. #include <linux/debugfs.h>
  13. #include <linux/spinlock.h>
  14. #include <linux/genalloc.h>
  15. #include <linux/interrupt.h>
  16. #include <linux/netdevice.h>
  17. #include <linux/rtnetlink.h>
  18. #include <linux/virtio_ids.h>
  19. #include <linux/virtio_caif.h>
  20. #include <linux/virtio_ring.h>
  21. #include <linux/dma-mapping.h>
  22. #include <net/caif/caif_dev.h>
  23. #include <linux/virtio_config.h>
  24. MODULE_LICENSE("GPL v2");
  25. MODULE_AUTHOR("Vicram Arv");
  26. MODULE_AUTHOR("Sjur Brendeland");
  27. MODULE_DESCRIPTION("Virtio CAIF Driver");
  28. /* NAPI schedule quota */
  29. #define CFV_DEFAULT_QUOTA 32
  30. /* Defaults used if virtio config space is unavailable */
  31. #define CFV_DEF_MTU_SIZE 4096
  32. #define CFV_DEF_HEADROOM 32
  33. #define CFV_DEF_TAILROOM 32
  34. /* Required IP header alignment */
  35. #define IP_HDR_ALIGN 4
  36. /* struct cfv_napi_contxt - NAPI context info
  37. * @riov: IOV holding data read from the ring. Note that riov may
  38. * still hold data when cfv_rx_poll() returns.
  39. * @head: Last descriptor ID we received from vringh_getdesc_kern.
  40. * We use this to put descriptor back on the used ring. USHRT_MAX is
  41. * used to indicate invalid head-id.
  42. */
  43. struct cfv_napi_context {
  44. struct vringh_kiov riov;
  45. unsigned short head;
  46. };
  47. /* struct cfv_stats - statistics for debugfs
  48. * @rx_napi_complete: Number of NAPI completions (RX)
  49. * @rx_napi_resched: Number of calls where the full quota was used (RX)
  50. * @rx_nomem: Number of SKB alloc failures (RX)
  51. * @rx_kicks: Number of RX kicks
  52. * @tx_full_ring: Number times TX ring was full
  53. * @tx_no_mem: Number of times TX went out of memory
  54. * @tx_flow_on: Number of flow on (TX)
  55. * @tx_kicks: Number of TX kicks
  56. */
  57. struct cfv_stats {
  58. u32 rx_napi_complete;
  59. u32 rx_napi_resched;
  60. u32 rx_nomem;
  61. u32 rx_kicks;
  62. u32 tx_full_ring;
  63. u32 tx_no_mem;
  64. u32 tx_flow_on;
  65. u32 tx_kicks;
  66. };
  67. /* struct cfv_info - Caif Virtio control structure
  68. * @cfdev: caif common header
  69. * @vdev: Associated virtio device
  70. * @vr_rx: rx/downlink host vring
  71. * @vq_tx: tx/uplink virtqueue
  72. * @ndev: CAIF link layer device
  73. * @watermark_tx: indicates number of free descriptors we need
  74. * to reopen the tx-queues after overload.
  75. * @tx_lock: protects vq_tx from concurrent use
  76. * @tx_release_tasklet: Tasklet for freeing consumed TX buffers
  77. * @napi: Napi context used in cfv_rx_poll()
  78. * @ctx: Context data used in cfv_rx_poll()
  79. * @tx_hr: transmit headroom
  80. * @rx_hr: receive headroom
  81. * @tx_tr: transmit tail room
  82. * @rx_tr: receive tail room
  83. * @mtu: transmit max size
  84. * @mru: receive max size
  85. * @allocsz: size of dma memory reserved for TX buffers
  86. * @alloc_addr: virtual address to dma memory for TX buffers
  87. * @alloc_dma: dma address to dma memory for TX buffers
  88. * @genpool: Gen Pool used for allocating TX buffers
  89. * @reserved_mem: Pointer to memory reserve allocated from genpool
  90. * @reserved_size: Size of memory reserve allocated from genpool
  91. * @stats: Statistics exposed in sysfs
  92. * @debugfs: Debugfs dentry for statistic counters
  93. */
  94. struct cfv_info {
  95. struct caif_dev_common cfdev;
  96. struct virtio_device *vdev;
  97. struct vringh *vr_rx;
  98. struct virtqueue *vq_tx;
  99. struct net_device *ndev;
  100. unsigned int watermark_tx;
  101. /* Protect access to vq_tx */
  102. spinlock_t tx_lock;
  103. struct tasklet_struct tx_release_tasklet;
  104. struct napi_struct napi;
  105. struct cfv_napi_context ctx;
  106. u16 tx_hr;
  107. u16 rx_hr;
  108. u16 tx_tr;
  109. u16 rx_tr;
  110. u32 mtu;
  111. u32 mru;
  112. size_t allocsz;
  113. void *alloc_addr;
  114. dma_addr_t alloc_dma;
  115. struct gen_pool *genpool;
  116. unsigned long reserved_mem;
  117. size_t reserved_size;
  118. struct cfv_stats stats;
  119. struct dentry *debugfs;
  120. };
  121. /* struct buf_info - maintains transmit buffer data handle
  122. * @size: size of transmit buffer
  123. * @dma_handle: handle to allocated dma device memory area
  124. * @vaddr: virtual address mapping to allocated memory area
  125. */
  126. struct buf_info {
  127. size_t size;
  128. u8 *vaddr;
  129. };
  130. /* Called from virtio device, in IRQ context */
  131. static void cfv_release_cb(struct virtqueue *vq_tx)
  132. {
  133. struct cfv_info *cfv = vq_tx->vdev->priv;
  134. ++cfv->stats.tx_kicks;
  135. tasklet_schedule(&cfv->tx_release_tasklet);
  136. }
  137. static void free_buf_info(struct cfv_info *cfv, struct buf_info *buf_info)
  138. {
  139. if (!buf_info)
  140. return;
  141. gen_pool_free(cfv->genpool, (unsigned long) buf_info->vaddr,
  142. buf_info->size);
  143. kfree(buf_info);
  144. }
  145. /* This is invoked whenever the remote processor completed processing
  146. * a TX msg we just sent, and the buffer is put back to the used ring.
  147. */
  148. static void cfv_release_used_buf(struct virtqueue *vq_tx)
  149. {
  150. struct cfv_info *cfv = vq_tx->vdev->priv;
  151. unsigned long flags;
  152. BUG_ON(vq_tx != cfv->vq_tx);
  153. for (;;) {
  154. unsigned int len;
  155. struct buf_info *buf_info;
  156. /* Get used buffer from used ring to recycle used descriptors */
  157. spin_lock_irqsave(&cfv->tx_lock, flags);
  158. buf_info = virtqueue_get_buf(vq_tx, &len);
  159. spin_unlock_irqrestore(&cfv->tx_lock, flags);
  160. /* Stop looping if there are no more buffers to free */
  161. if (!buf_info)
  162. break;
  163. free_buf_info(cfv, buf_info);
  164. /* watermark_tx indicates if we previously stopped the tx
  165. * queues. If we have enough free stots in the virtio ring,
  166. * re-establish memory reserved and open up tx queues.
  167. */
  168. if (cfv->vq_tx->num_free <= cfv->watermark_tx)
  169. continue;
  170. /* Re-establish memory reserve */
  171. if (cfv->reserved_mem == 0 && cfv->genpool)
  172. cfv->reserved_mem =
  173. gen_pool_alloc(cfv->genpool,
  174. cfv->reserved_size);
  175. /* Open up the tx queues */
  176. if (cfv->reserved_mem) {
  177. cfv->watermark_tx =
  178. virtqueue_get_vring_size(cfv->vq_tx);
  179. netif_tx_wake_all_queues(cfv->ndev);
  180. /* Buffers are recycled in cfv_netdev_tx, so
  181. * disable notifications when queues are opened.
  182. */
  183. virtqueue_disable_cb(cfv->vq_tx);
  184. ++cfv->stats.tx_flow_on;
  185. } else {
  186. /* if no memory reserve, wait for more free slots */
  187. WARN_ON(cfv->watermark_tx >
  188. virtqueue_get_vring_size(cfv->vq_tx));
  189. cfv->watermark_tx +=
  190. virtqueue_get_vring_size(cfv->vq_tx) / 4;
  191. }
  192. }
  193. }
  194. /* Allocate a SKB and copy packet data to it */
  195. static struct sk_buff *cfv_alloc_and_copy_skb(int *err,
  196. struct cfv_info *cfv,
  197. u8 *frm, u32 frm_len)
  198. {
  199. struct sk_buff *skb;
  200. u32 cfpkt_len, pad_len;
  201. *err = 0;
  202. /* Verify that packet size with down-link header and mtu size */
  203. if (frm_len > cfv->mru || frm_len <= cfv->rx_hr + cfv->rx_tr) {
  204. netdev_err(cfv->ndev,
  205. "Invalid frmlen:%u mtu:%u hr:%d tr:%d\n",
  206. frm_len, cfv->mru, cfv->rx_hr,
  207. cfv->rx_tr);
  208. *err = -EPROTO;
  209. return NULL;
  210. }
  211. cfpkt_len = frm_len - (cfv->rx_hr + cfv->rx_tr);
  212. pad_len = (unsigned long)(frm + cfv->rx_hr) & (IP_HDR_ALIGN - 1);
  213. skb = netdev_alloc_skb(cfv->ndev, frm_len + pad_len);
  214. if (!skb) {
  215. *err = -ENOMEM;
  216. return NULL;
  217. }
  218. skb_reserve(skb, cfv->rx_hr + pad_len);
  219. skb_put_data(skb, frm + cfv->rx_hr, cfpkt_len);
  220. return skb;
  221. }
  222. /* Get packets from the host vring */
  223. static int cfv_rx_poll(struct napi_struct *napi, int quota)
  224. {
  225. struct cfv_info *cfv = container_of(napi, struct cfv_info, napi);
  226. int rxcnt = 0;
  227. int err = 0;
  228. void *buf;
  229. struct sk_buff *skb;
  230. struct vringh_kiov *riov = &cfv->ctx.riov;
  231. unsigned int skb_len;
  232. do {
  233. skb = NULL;
  234. /* Put the previous iovec back on the used ring and
  235. * fetch a new iovec if we have processed all elements.
  236. */
  237. if (riov->i == riov->used) {
  238. if (cfv->ctx.head != USHRT_MAX) {
  239. vringh_complete_kern(cfv->vr_rx,
  240. cfv->ctx.head,
  241. 0);
  242. cfv->ctx.head = USHRT_MAX;
  243. }
  244. err = vringh_getdesc_kern(
  245. cfv->vr_rx,
  246. riov,
  247. NULL,
  248. &cfv->ctx.head,
  249. GFP_ATOMIC);
  250. if (err <= 0)
  251. goto exit;
  252. }
  253. buf = phys_to_virt((unsigned long) riov->iov[riov->i].iov_base);
  254. /* TODO: Add check on valid buffer address */
  255. skb = cfv_alloc_and_copy_skb(&err, cfv, buf,
  256. riov->iov[riov->i].iov_len);
  257. if (unlikely(err))
  258. goto exit;
  259. /* Push received packet up the stack. */
  260. skb_len = skb->len;
  261. skb->protocol = htons(ETH_P_CAIF);
  262. skb_reset_mac_header(skb);
  263. skb->dev = cfv->ndev;
  264. err = netif_receive_skb(skb);
  265. if (unlikely(err)) {
  266. ++cfv->ndev->stats.rx_dropped;
  267. } else {
  268. ++cfv->ndev->stats.rx_packets;
  269. cfv->ndev->stats.rx_bytes += skb_len;
  270. }
  271. ++riov->i;
  272. ++rxcnt;
  273. } while (rxcnt < quota);
  274. ++cfv->stats.rx_napi_resched;
  275. goto out;
  276. exit:
  277. switch (err) {
  278. case 0:
  279. ++cfv->stats.rx_napi_complete;
  280. /* Really out of patckets? (stolen from virtio_net)*/
  281. napi_complete(napi);
  282. if (unlikely(!vringh_notify_enable_kern(cfv->vr_rx)) &&
  283. napi_schedule_prep(napi)) {
  284. vringh_notify_disable_kern(cfv->vr_rx);
  285. __napi_schedule(napi);
  286. }
  287. break;
  288. case -ENOMEM:
  289. ++cfv->stats.rx_nomem;
  290. dev_kfree_skb(skb);
  291. /* Stop NAPI poll on OOM, we hope to be polled later */
  292. napi_complete(napi);
  293. vringh_notify_enable_kern(cfv->vr_rx);
  294. break;
  295. default:
  296. /* We're doomed, any modem fault is fatal */
  297. netdev_warn(cfv->ndev, "Bad ring, disable device\n");
  298. cfv->ndev->stats.rx_dropped = riov->used - riov->i;
  299. napi_complete(napi);
  300. vringh_notify_disable_kern(cfv->vr_rx);
  301. netif_carrier_off(cfv->ndev);
  302. break;
  303. }
  304. out:
  305. if (rxcnt && vringh_need_notify_kern(cfv->vr_rx) > 0)
  306. vringh_notify(cfv->vr_rx);
  307. return rxcnt;
  308. }
  309. static void cfv_recv(struct virtio_device *vdev, struct vringh *vr_rx)
  310. {
  311. struct cfv_info *cfv = vdev->priv;
  312. ++cfv->stats.rx_kicks;
  313. vringh_notify_disable_kern(cfv->vr_rx);
  314. napi_schedule(&cfv->napi);
  315. }
  316. static void cfv_destroy_genpool(struct cfv_info *cfv)
  317. {
  318. if (cfv->alloc_addr)
  319. dma_free_coherent(cfv->vdev->dev.parent->parent,
  320. cfv->allocsz, cfv->alloc_addr,
  321. cfv->alloc_dma);
  322. if (!cfv->genpool)
  323. return;
  324. gen_pool_free(cfv->genpool, cfv->reserved_mem,
  325. cfv->reserved_size);
  326. gen_pool_destroy(cfv->genpool);
  327. cfv->genpool = NULL;
  328. }
  329. static int cfv_create_genpool(struct cfv_info *cfv)
  330. {
  331. int err;
  332. /* dma_alloc can only allocate whole pages, and we need a more
  333. * fine graned allocation so we use genpool. We ask for space needed
  334. * by IP and a full ring. If the dma allcoation fails we retry with a
  335. * smaller allocation size.
  336. */
  337. err = -ENOMEM;
  338. cfv->allocsz = (virtqueue_get_vring_size(cfv->vq_tx) *
  339. (ETH_DATA_LEN + cfv->tx_hr + cfv->tx_tr) * 11)/10;
  340. if (cfv->allocsz <= (num_possible_cpus() + 1) * cfv->ndev->mtu)
  341. return -EINVAL;
  342. for (;;) {
  343. if (cfv->allocsz <= num_possible_cpus() * cfv->ndev->mtu) {
  344. netdev_info(cfv->ndev, "Not enough device memory\n");
  345. return -ENOMEM;
  346. }
  347. cfv->alloc_addr = dma_alloc_coherent(
  348. cfv->vdev->dev.parent->parent,
  349. cfv->allocsz, &cfv->alloc_dma,
  350. GFP_ATOMIC);
  351. if (cfv->alloc_addr)
  352. break;
  353. cfv->allocsz = (cfv->allocsz * 3) >> 2;
  354. }
  355. netdev_dbg(cfv->ndev, "Allocated %zd bytes from dma-memory\n",
  356. cfv->allocsz);
  357. /* Allocate on 128 bytes boundaries (1 << 7)*/
  358. cfv->genpool = gen_pool_create(7, -1);
  359. if (!cfv->genpool)
  360. goto err;
  361. err = gen_pool_add_virt(cfv->genpool, (unsigned long)cfv->alloc_addr,
  362. (phys_addr_t)virt_to_phys(cfv->alloc_addr),
  363. cfv->allocsz, -1);
  364. if (err)
  365. goto err;
  366. /* Reserve some memory for low memory situations. If we hit the roof
  367. * in the memory pool, we stop TX flow and release the reserve.
  368. */
  369. cfv->reserved_size = num_possible_cpus() * cfv->ndev->mtu;
  370. cfv->reserved_mem = gen_pool_alloc(cfv->genpool,
  371. cfv->reserved_size);
  372. if (!cfv->reserved_mem) {
  373. err = -ENOMEM;
  374. goto err;
  375. }
  376. cfv->watermark_tx = virtqueue_get_vring_size(cfv->vq_tx);
  377. return 0;
  378. err:
  379. cfv_destroy_genpool(cfv);
  380. return err;
  381. }
  382. /* Enable the CAIF interface and allocate the memory-pool */
  383. static int cfv_netdev_open(struct net_device *netdev)
  384. {
  385. struct cfv_info *cfv = netdev_priv(netdev);
  386. if (cfv_create_genpool(cfv))
  387. return -ENOMEM;
  388. netif_carrier_on(netdev);
  389. napi_enable(&cfv->napi);
  390. /* Schedule NAPI to read any pending packets */
  391. napi_schedule(&cfv->napi);
  392. return 0;
  393. }
  394. /* Disable the CAIF interface and free the memory-pool */
  395. static int cfv_netdev_close(struct net_device *netdev)
  396. {
  397. struct cfv_info *cfv = netdev_priv(netdev);
  398. unsigned long flags;
  399. struct buf_info *buf_info;
  400. /* Disable interrupts, queues and NAPI polling */
  401. netif_carrier_off(netdev);
  402. virtqueue_disable_cb(cfv->vq_tx);
  403. vringh_notify_disable_kern(cfv->vr_rx);
  404. napi_disable(&cfv->napi);
  405. /* Release any TX buffers on both used and avilable rings */
  406. cfv_release_used_buf(cfv->vq_tx);
  407. spin_lock_irqsave(&cfv->tx_lock, flags);
  408. while ((buf_info = virtqueue_detach_unused_buf(cfv->vq_tx)))
  409. free_buf_info(cfv, buf_info);
  410. spin_unlock_irqrestore(&cfv->tx_lock, flags);
  411. /* Release all dma allocated memory and destroy the pool */
  412. cfv_destroy_genpool(cfv);
  413. return 0;
  414. }
  415. /* Allocate a buffer in dma-memory and copy skb to it */
  416. static struct buf_info *cfv_alloc_and_copy_to_shm(struct cfv_info *cfv,
  417. struct sk_buff *skb,
  418. struct scatterlist *sg)
  419. {
  420. struct caif_payload_info *info = (void *)&skb->cb;
  421. struct buf_info *buf_info = NULL;
  422. u8 pad_len, hdr_ofs;
  423. if (!cfv->genpool)
  424. goto err;
  425. if (unlikely(cfv->tx_hr + skb->len + cfv->tx_tr > cfv->mtu)) {
  426. netdev_warn(cfv->ndev, "Invalid packet len (%d > %d)\n",
  427. cfv->tx_hr + skb->len + cfv->tx_tr, cfv->mtu);
  428. goto err;
  429. }
  430. buf_info = kmalloc(sizeof(struct buf_info), GFP_ATOMIC);
  431. if (unlikely(!buf_info))
  432. goto err;
  433. /* Make the IP header aligned in tbe buffer */
  434. hdr_ofs = cfv->tx_hr + info->hdr_len;
  435. pad_len = hdr_ofs & (IP_HDR_ALIGN - 1);
  436. buf_info->size = cfv->tx_hr + skb->len + cfv->tx_tr + pad_len;
  437. /* allocate dma memory buffer */
  438. buf_info->vaddr = (void *)gen_pool_alloc(cfv->genpool, buf_info->size);
  439. if (unlikely(!buf_info->vaddr))
  440. goto err;
  441. /* copy skbuf contents to send buffer */
  442. skb_copy_bits(skb, 0, buf_info->vaddr + cfv->tx_hr + pad_len, skb->len);
  443. sg_init_one(sg, buf_info->vaddr + pad_len,
  444. skb->len + cfv->tx_hr + cfv->rx_hr);
  445. return buf_info;
  446. err:
  447. kfree(buf_info);
  448. return NULL;
  449. }
  450. /* Put the CAIF packet on the virtio ring and kick the receiver */
  451. static int cfv_netdev_tx(struct sk_buff *skb, struct net_device *netdev)
  452. {
  453. struct cfv_info *cfv = netdev_priv(netdev);
  454. struct buf_info *buf_info;
  455. struct scatterlist sg;
  456. unsigned long flags;
  457. bool flow_off = false;
  458. int ret;
  459. /* garbage collect released buffers */
  460. cfv_release_used_buf(cfv->vq_tx);
  461. spin_lock_irqsave(&cfv->tx_lock, flags);
  462. /* Flow-off check takes into account number of cpus to make sure
  463. * virtqueue will not be overfilled in any possible smp conditions.
  464. *
  465. * Flow-on is triggered when sufficient buffers are freed
  466. */
  467. if (unlikely(cfv->vq_tx->num_free <= num_present_cpus())) {
  468. flow_off = true;
  469. cfv->stats.tx_full_ring++;
  470. }
  471. /* If we run out of memory, we release the memory reserve and retry
  472. * allocation.
  473. */
  474. buf_info = cfv_alloc_and_copy_to_shm(cfv, skb, &sg);
  475. if (unlikely(!buf_info)) {
  476. cfv->stats.tx_no_mem++;
  477. flow_off = true;
  478. if (cfv->reserved_mem && cfv->genpool) {
  479. gen_pool_free(cfv->genpool, cfv->reserved_mem,
  480. cfv->reserved_size);
  481. cfv->reserved_mem = 0;
  482. buf_info = cfv_alloc_and_copy_to_shm(cfv, skb, &sg);
  483. }
  484. }
  485. if (unlikely(flow_off)) {
  486. /* Turn flow on when a 1/4 of the descriptors are released */
  487. cfv->watermark_tx = virtqueue_get_vring_size(cfv->vq_tx) / 4;
  488. /* Enable notifications of recycled TX buffers */
  489. virtqueue_enable_cb(cfv->vq_tx);
  490. netif_tx_stop_all_queues(netdev);
  491. }
  492. if (unlikely(!buf_info)) {
  493. /* If the memory reserve does it's job, this shouldn't happen */
  494. netdev_warn(cfv->ndev, "Out of gen_pool memory\n");
  495. goto err;
  496. }
  497. ret = virtqueue_add_outbuf(cfv->vq_tx, &sg, 1, buf_info, GFP_ATOMIC);
  498. if (unlikely((ret < 0))) {
  499. /* If flow control works, this shouldn't happen */
  500. netdev_warn(cfv->ndev, "Failed adding buffer to TX vring:%d\n",
  501. ret);
  502. goto err;
  503. }
  504. /* update netdev statistics */
  505. cfv->ndev->stats.tx_packets++;
  506. cfv->ndev->stats.tx_bytes += skb->len;
  507. spin_unlock_irqrestore(&cfv->tx_lock, flags);
  508. /* tell the remote processor it has a pending message to read */
  509. virtqueue_kick(cfv->vq_tx);
  510. dev_kfree_skb(skb);
  511. return NETDEV_TX_OK;
  512. err:
  513. spin_unlock_irqrestore(&cfv->tx_lock, flags);
  514. cfv->ndev->stats.tx_dropped++;
  515. free_buf_info(cfv, buf_info);
  516. dev_kfree_skb(skb);
  517. return NETDEV_TX_OK;
  518. }
  519. static void cfv_tx_release_tasklet(unsigned long drv)
  520. {
  521. struct cfv_info *cfv = (struct cfv_info *)drv;
  522. cfv_release_used_buf(cfv->vq_tx);
  523. }
  524. static const struct net_device_ops cfv_netdev_ops = {
  525. .ndo_open = cfv_netdev_open,
  526. .ndo_stop = cfv_netdev_close,
  527. .ndo_start_xmit = cfv_netdev_tx,
  528. };
  529. static void cfv_netdev_setup(struct net_device *netdev)
  530. {
  531. netdev->netdev_ops = &cfv_netdev_ops;
  532. netdev->type = ARPHRD_CAIF;
  533. netdev->tx_queue_len = 100;
  534. netdev->flags = IFF_POINTOPOINT | IFF_NOARP;
  535. netdev->mtu = CFV_DEF_MTU_SIZE;
  536. netdev->needs_free_netdev = true;
  537. }
  538. /* Create debugfs counters for the device */
  539. static inline void debugfs_init(struct cfv_info *cfv)
  540. {
  541. cfv->debugfs = debugfs_create_dir(netdev_name(cfv->ndev), NULL);
  542. debugfs_create_u32("rx-napi-complete", 0400, cfv->debugfs,
  543. &cfv->stats.rx_napi_complete);
  544. debugfs_create_u32("rx-napi-resched", 0400, cfv->debugfs,
  545. &cfv->stats.rx_napi_resched);
  546. debugfs_create_u32("rx-nomem", 0400, cfv->debugfs,
  547. &cfv->stats.rx_nomem);
  548. debugfs_create_u32("rx-kicks", 0400, cfv->debugfs,
  549. &cfv->stats.rx_kicks);
  550. debugfs_create_u32("tx-full-ring", 0400, cfv->debugfs,
  551. &cfv->stats.tx_full_ring);
  552. debugfs_create_u32("tx-no-mem", 0400, cfv->debugfs,
  553. &cfv->stats.tx_no_mem);
  554. debugfs_create_u32("tx-kicks", 0400, cfv->debugfs,
  555. &cfv->stats.tx_kicks);
  556. debugfs_create_u32("tx-flow-on", 0400, cfv->debugfs,
  557. &cfv->stats.tx_flow_on);
  558. }
  559. /* Setup CAIF for the a virtio device */
  560. static int cfv_probe(struct virtio_device *vdev)
  561. {
  562. vq_callback_t *vq_cbs = cfv_release_cb;
  563. vrh_callback_t *vrh_cbs = cfv_recv;
  564. const char *names = "output";
  565. const char *cfv_netdev_name = "cfvrt";
  566. struct net_device *netdev;
  567. struct cfv_info *cfv;
  568. int err = -EINVAL;
  569. netdev = alloc_netdev(sizeof(struct cfv_info), cfv_netdev_name,
  570. NET_NAME_UNKNOWN, cfv_netdev_setup);
  571. if (!netdev)
  572. return -ENOMEM;
  573. cfv = netdev_priv(netdev);
  574. cfv->vdev = vdev;
  575. cfv->ndev = netdev;
  576. spin_lock_init(&cfv->tx_lock);
  577. /* Get the RX virtio ring. This is a "host side vring". */
  578. err = -ENODEV;
  579. if (!vdev->vringh_config || !vdev->vringh_config->find_vrhs)
  580. goto err;
  581. err = vdev->vringh_config->find_vrhs(vdev, 1, &cfv->vr_rx, &vrh_cbs);
  582. if (err)
  583. goto err;
  584. /* Get the TX virtio ring. This is a "guest side vring". */
  585. err = virtio_find_vqs(vdev, 1, &cfv->vq_tx, &vq_cbs, &names, NULL);
  586. if (err)
  587. goto err;
  588. /* Get the CAIF configuration from virtio config space, if available */
  589. if (vdev->config->get) {
  590. virtio_cread(vdev, struct virtio_caif_transf_config, headroom,
  591. &cfv->tx_hr);
  592. virtio_cread(vdev, struct virtio_caif_transf_config, headroom,
  593. &cfv->rx_hr);
  594. virtio_cread(vdev, struct virtio_caif_transf_config, tailroom,
  595. &cfv->tx_tr);
  596. virtio_cread(vdev, struct virtio_caif_transf_config, tailroom,
  597. &cfv->rx_tr);
  598. virtio_cread(vdev, struct virtio_caif_transf_config, mtu,
  599. &cfv->mtu);
  600. virtio_cread(vdev, struct virtio_caif_transf_config, mtu,
  601. &cfv->mru);
  602. } else {
  603. cfv->tx_hr = CFV_DEF_HEADROOM;
  604. cfv->rx_hr = CFV_DEF_HEADROOM;
  605. cfv->tx_tr = CFV_DEF_TAILROOM;
  606. cfv->rx_tr = CFV_DEF_TAILROOM;
  607. cfv->mtu = CFV_DEF_MTU_SIZE;
  608. cfv->mru = CFV_DEF_MTU_SIZE;
  609. }
  610. netdev->needed_headroom = cfv->tx_hr;
  611. netdev->needed_tailroom = cfv->tx_tr;
  612. /* Disable buffer release interrupts unless we have stopped TX queues */
  613. virtqueue_disable_cb(cfv->vq_tx);
  614. netdev->mtu = cfv->mtu - cfv->tx_tr;
  615. vdev->priv = cfv;
  616. /* Initialize NAPI poll context data */
  617. vringh_kiov_init(&cfv->ctx.riov, NULL, 0);
  618. cfv->ctx.head = USHRT_MAX;
  619. netif_napi_add(netdev, &cfv->napi, cfv_rx_poll, CFV_DEFAULT_QUOTA);
  620. tasklet_init(&cfv->tx_release_tasklet,
  621. cfv_tx_release_tasklet,
  622. (unsigned long)cfv);
  623. /* Carrier is off until netdevice is opened */
  624. netif_carrier_off(netdev);
  625. /* register Netdev */
  626. err = register_netdev(netdev);
  627. if (err) {
  628. dev_err(&vdev->dev, "Unable to register netdev (%d)\n", err);
  629. goto err;
  630. }
  631. debugfs_init(cfv);
  632. return 0;
  633. err:
  634. netdev_warn(cfv->ndev, "CAIF Virtio probe failed:%d\n", err);
  635. if (cfv->vr_rx)
  636. vdev->vringh_config->del_vrhs(cfv->vdev);
  637. if (cfv->vdev)
  638. vdev->config->del_vqs(cfv->vdev);
  639. free_netdev(netdev);
  640. return err;
  641. }
  642. static void cfv_remove(struct virtio_device *vdev)
  643. {
  644. struct cfv_info *cfv = vdev->priv;
  645. rtnl_lock();
  646. dev_close(cfv->ndev);
  647. rtnl_unlock();
  648. tasklet_kill(&cfv->tx_release_tasklet);
  649. debugfs_remove_recursive(cfv->debugfs);
  650. vringh_kiov_cleanup(&cfv->ctx.riov);
  651. vdev->config->reset(vdev);
  652. vdev->vringh_config->del_vrhs(cfv->vdev);
  653. cfv->vr_rx = NULL;
  654. vdev->config->del_vqs(cfv->vdev);
  655. unregister_netdev(cfv->ndev);
  656. }
  657. static struct virtio_device_id id_table[] = {
  658. { VIRTIO_ID_CAIF, VIRTIO_DEV_ANY_ID },
  659. { 0 },
  660. };
  661. static unsigned int features[] = {
  662. };
  663. static struct virtio_driver caif_virtio_driver = {
  664. .feature_table = features,
  665. .feature_table_size = ARRAY_SIZE(features),
  666. .driver.name = KBUILD_MODNAME,
  667. .driver.owner = THIS_MODULE,
  668. .id_table = id_table,
  669. .probe = cfv_probe,
  670. .remove = cfv_remove,
  671. };
  672. module_virtio_driver(caif_virtio_driver);
  673. MODULE_DEVICE_TABLE(virtio, id_table);