rionet.c 18 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747
  1. // SPDX-License-Identifier: GPL-2.0-or-later
  2. /*
  3. * rionet - Ethernet driver over RapidIO messaging services
  4. *
  5. * Copyright 2005 MontaVista Software, Inc.
  6. * Matt Porter <mporter@kernel.crashing.org>
  7. */
  8. #include <linux/module.h>
  9. #include <linux/kernel.h>
  10. #include <linux/dma-mapping.h>
  11. #include <linux/delay.h>
  12. #include <linux/rio.h>
  13. #include <linux/rio_drv.h>
  14. #include <linux/slab.h>
  15. #include <linux/rio_ids.h>
  16. #include <linux/netdevice.h>
  17. #include <linux/etherdevice.h>
  18. #include <linux/skbuff.h>
  19. #include <linux/crc32.h>
  20. #include <linux/ethtool.h>
  21. #include <linux/reboot.h>
  22. #define DRV_NAME "rionet"
  23. #define DRV_VERSION "0.3"
  24. #define DRV_AUTHOR "Matt Porter <mporter@kernel.crashing.org>"
  25. #define DRV_DESC "Ethernet over RapidIO"
  26. MODULE_AUTHOR(DRV_AUTHOR);
  27. MODULE_DESCRIPTION(DRV_DESC);
  28. MODULE_LICENSE("GPL");
  29. #define RIONET_DEFAULT_MSGLEVEL \
  30. (NETIF_MSG_DRV | \
  31. NETIF_MSG_LINK | \
  32. NETIF_MSG_RX_ERR | \
  33. NETIF_MSG_TX_ERR)
  34. #define RIONET_DOORBELL_JOIN 0x1000
  35. #define RIONET_DOORBELL_LEAVE 0x1001
  36. #define RIONET_MAILBOX 0
  37. #define RIONET_TX_RING_SIZE CONFIG_RIONET_TX_SIZE
  38. #define RIONET_RX_RING_SIZE CONFIG_RIONET_RX_SIZE
  39. #define RIONET_MAX_NETS 8
  40. #define RIONET_MSG_SIZE RIO_MAX_MSG_SIZE
  41. #define RIONET_MAX_MTU (RIONET_MSG_SIZE - ETH_HLEN)
  42. struct rionet_private {
  43. struct rio_mport *mport;
  44. struct sk_buff *rx_skb[RIONET_RX_RING_SIZE];
  45. struct sk_buff *tx_skb[RIONET_TX_RING_SIZE];
  46. int rx_slot;
  47. int tx_slot;
  48. int tx_cnt;
  49. int ack_slot;
  50. spinlock_t lock;
  51. spinlock_t tx_lock;
  52. u32 msg_enable;
  53. bool open;
  54. };
  55. struct rionet_peer {
  56. struct list_head node;
  57. struct rio_dev *rdev;
  58. struct resource *res;
  59. };
  60. struct rionet_net {
  61. struct net_device *ndev;
  62. struct list_head peers;
  63. spinlock_t lock; /* net info access lock */
  64. struct rio_dev **active;
  65. int nact; /* number of active peers */
  66. };
  67. static struct rionet_net nets[RIONET_MAX_NETS];
  68. #define is_rionet_capable(src_ops, dst_ops) \
  69. ((src_ops & RIO_SRC_OPS_DATA_MSG) && \
  70. (dst_ops & RIO_DST_OPS_DATA_MSG) && \
  71. (src_ops & RIO_SRC_OPS_DOORBELL) && \
  72. (dst_ops & RIO_DST_OPS_DOORBELL))
  73. #define dev_rionet_capable(dev) \
  74. is_rionet_capable(dev->src_ops, dev->dst_ops)
  75. #define RIONET_MAC_MATCH(x) (!memcmp((x), "\00\01\00\01", 4))
  76. #define RIONET_GET_DESTID(x) ((*((u8 *)x + 4) << 8) | *((u8 *)x + 5))
  77. static int rionet_rx_clean(struct net_device *ndev)
  78. {
  79. int i;
  80. int error = 0;
  81. struct rionet_private *rnet = netdev_priv(ndev);
  82. void *data;
  83. i = rnet->rx_slot;
  84. do {
  85. if (!rnet->rx_skb[i])
  86. continue;
  87. if (!(data = rio_get_inb_message(rnet->mport, RIONET_MAILBOX)))
  88. break;
  89. rnet->rx_skb[i]->data = data;
  90. skb_put(rnet->rx_skb[i], RIO_MAX_MSG_SIZE);
  91. rnet->rx_skb[i]->protocol =
  92. eth_type_trans(rnet->rx_skb[i], ndev);
  93. error = netif_rx(rnet->rx_skb[i]);
  94. if (error == NET_RX_DROP) {
  95. ndev->stats.rx_dropped++;
  96. } else {
  97. ndev->stats.rx_packets++;
  98. ndev->stats.rx_bytes += RIO_MAX_MSG_SIZE;
  99. }
  100. } while ((i = (i + 1) % RIONET_RX_RING_SIZE) != rnet->rx_slot);
  101. return i;
  102. }
  103. static void rionet_rx_fill(struct net_device *ndev, int end)
  104. {
  105. int i;
  106. struct rionet_private *rnet = netdev_priv(ndev);
  107. i = rnet->rx_slot;
  108. do {
  109. rnet->rx_skb[i] = dev_alloc_skb(RIO_MAX_MSG_SIZE);
  110. if (!rnet->rx_skb[i])
  111. break;
  112. rio_add_inb_buffer(rnet->mport, RIONET_MAILBOX,
  113. rnet->rx_skb[i]->data);
  114. } while ((i = (i + 1) % RIONET_RX_RING_SIZE) != end);
  115. rnet->rx_slot = i;
  116. }
  117. static int rionet_queue_tx_msg(struct sk_buff *skb, struct net_device *ndev,
  118. struct rio_dev *rdev)
  119. {
  120. struct rionet_private *rnet = netdev_priv(ndev);
  121. rio_add_outb_message(rnet->mport, rdev, 0, skb->data, skb->len);
  122. rnet->tx_skb[rnet->tx_slot] = skb;
  123. ndev->stats.tx_packets++;
  124. ndev->stats.tx_bytes += skb->len;
  125. if (++rnet->tx_cnt == RIONET_TX_RING_SIZE)
  126. netif_stop_queue(ndev);
  127. ++rnet->tx_slot;
  128. rnet->tx_slot &= (RIONET_TX_RING_SIZE - 1);
  129. if (netif_msg_tx_queued(rnet))
  130. printk(KERN_INFO "%s: queued skb len %8.8x\n", DRV_NAME,
  131. skb->len);
  132. return 0;
  133. }
  134. static netdev_tx_t rionet_start_xmit(struct sk_buff *skb,
  135. struct net_device *ndev)
  136. {
  137. int i;
  138. struct rionet_private *rnet = netdev_priv(ndev);
  139. struct ethhdr *eth = (struct ethhdr *)skb->data;
  140. u16 destid;
  141. unsigned long flags;
  142. int add_num = 1;
  143. spin_lock_irqsave(&rnet->tx_lock, flags);
  144. if (is_multicast_ether_addr(eth->h_dest))
  145. add_num = nets[rnet->mport->id].nact;
  146. if ((rnet->tx_cnt + add_num) > RIONET_TX_RING_SIZE) {
  147. netif_stop_queue(ndev);
  148. spin_unlock_irqrestore(&rnet->tx_lock, flags);
  149. printk(KERN_ERR "%s: BUG! Tx Ring full when queue awake!\n",
  150. ndev->name);
  151. return NETDEV_TX_BUSY;
  152. }
  153. if (is_multicast_ether_addr(eth->h_dest)) {
  154. int count = 0;
  155. for (i = 0; i < RIO_MAX_ROUTE_ENTRIES(rnet->mport->sys_size);
  156. i++)
  157. if (nets[rnet->mport->id].active[i]) {
  158. rionet_queue_tx_msg(skb, ndev,
  159. nets[rnet->mport->id].active[i]);
  160. if (count)
  161. refcount_inc(&skb->users);
  162. count++;
  163. }
  164. } else if (RIONET_MAC_MATCH(eth->h_dest)) {
  165. destid = RIONET_GET_DESTID(eth->h_dest);
  166. if (nets[rnet->mport->id].active[destid])
  167. rionet_queue_tx_msg(skb, ndev,
  168. nets[rnet->mport->id].active[destid]);
  169. else {
  170. /*
  171. * If the target device was removed from the list of
  172. * active peers but we still have TX packets targeting
  173. * it just report sending a packet to the target
  174. * (without actual packet transfer).
  175. */
  176. ndev->stats.tx_packets++;
  177. ndev->stats.tx_bytes += skb->len;
  178. dev_kfree_skb_any(skb);
  179. }
  180. }
  181. spin_unlock_irqrestore(&rnet->tx_lock, flags);
  182. return NETDEV_TX_OK;
  183. }
  184. static void rionet_dbell_event(struct rio_mport *mport, void *dev_id, u16 sid, u16 tid,
  185. u16 info)
  186. {
  187. struct net_device *ndev = dev_id;
  188. struct rionet_private *rnet = netdev_priv(ndev);
  189. struct rionet_peer *peer;
  190. unsigned char netid = rnet->mport->id;
  191. if (netif_msg_intr(rnet))
  192. printk(KERN_INFO "%s: doorbell sid %4.4x tid %4.4x info %4.4x",
  193. DRV_NAME, sid, tid, info);
  194. if (info == RIONET_DOORBELL_JOIN) {
  195. if (!nets[netid].active[sid]) {
  196. spin_lock(&nets[netid].lock);
  197. list_for_each_entry(peer, &nets[netid].peers, node) {
  198. if (peer->rdev->destid == sid) {
  199. nets[netid].active[sid] = peer->rdev;
  200. nets[netid].nact++;
  201. }
  202. }
  203. spin_unlock(&nets[netid].lock);
  204. rio_mport_send_doorbell(mport, sid,
  205. RIONET_DOORBELL_JOIN);
  206. }
  207. } else if (info == RIONET_DOORBELL_LEAVE) {
  208. spin_lock(&nets[netid].lock);
  209. if (nets[netid].active[sid]) {
  210. nets[netid].active[sid] = NULL;
  211. nets[netid].nact--;
  212. }
  213. spin_unlock(&nets[netid].lock);
  214. } else {
  215. if (netif_msg_intr(rnet))
  216. printk(KERN_WARNING "%s: unhandled doorbell\n",
  217. DRV_NAME);
  218. }
  219. }
  220. static void rionet_inb_msg_event(struct rio_mport *mport, void *dev_id, int mbox, int slot)
  221. {
  222. int n;
  223. struct net_device *ndev = dev_id;
  224. struct rionet_private *rnet = netdev_priv(ndev);
  225. if (netif_msg_intr(rnet))
  226. printk(KERN_INFO "%s: inbound message event, mbox %d slot %d\n",
  227. DRV_NAME, mbox, slot);
  228. spin_lock(&rnet->lock);
  229. if ((n = rionet_rx_clean(ndev)) != rnet->rx_slot)
  230. rionet_rx_fill(ndev, n);
  231. spin_unlock(&rnet->lock);
  232. }
  233. static void rionet_outb_msg_event(struct rio_mport *mport, void *dev_id, int mbox, int slot)
  234. {
  235. struct net_device *ndev = dev_id;
  236. struct rionet_private *rnet = netdev_priv(ndev);
  237. spin_lock(&rnet->tx_lock);
  238. if (netif_msg_intr(rnet))
  239. printk(KERN_INFO
  240. "%s: outbound message event, mbox %d slot %d\n",
  241. DRV_NAME, mbox, slot);
  242. while (rnet->tx_cnt && (rnet->ack_slot != slot)) {
  243. /* dma unmap single */
  244. dev_kfree_skb_irq(rnet->tx_skb[rnet->ack_slot]);
  245. rnet->tx_skb[rnet->ack_slot] = NULL;
  246. ++rnet->ack_slot;
  247. rnet->ack_slot &= (RIONET_TX_RING_SIZE - 1);
  248. rnet->tx_cnt--;
  249. }
  250. if (rnet->tx_cnt < RIONET_TX_RING_SIZE)
  251. netif_wake_queue(ndev);
  252. spin_unlock(&rnet->tx_lock);
  253. }
  254. static int rionet_open(struct net_device *ndev)
  255. {
  256. int i, rc = 0;
  257. struct rionet_peer *peer;
  258. struct rionet_private *rnet = netdev_priv(ndev);
  259. unsigned char netid = rnet->mport->id;
  260. unsigned long flags;
  261. if (netif_msg_ifup(rnet))
  262. printk(KERN_INFO "%s: open\n", DRV_NAME);
  263. if ((rc = rio_request_inb_dbell(rnet->mport,
  264. (void *)ndev,
  265. RIONET_DOORBELL_JOIN,
  266. RIONET_DOORBELL_LEAVE,
  267. rionet_dbell_event)) < 0)
  268. goto out;
  269. if ((rc = rio_request_inb_mbox(rnet->mport,
  270. (void *)ndev,
  271. RIONET_MAILBOX,
  272. RIONET_RX_RING_SIZE,
  273. rionet_inb_msg_event)) < 0)
  274. goto out;
  275. if ((rc = rio_request_outb_mbox(rnet->mport,
  276. (void *)ndev,
  277. RIONET_MAILBOX,
  278. RIONET_TX_RING_SIZE,
  279. rionet_outb_msg_event)) < 0)
  280. goto out;
  281. /* Initialize inbound message ring */
  282. for (i = 0; i < RIONET_RX_RING_SIZE; i++)
  283. rnet->rx_skb[i] = NULL;
  284. rnet->rx_slot = 0;
  285. rionet_rx_fill(ndev, 0);
  286. rnet->tx_slot = 0;
  287. rnet->tx_cnt = 0;
  288. rnet->ack_slot = 0;
  289. netif_carrier_on(ndev);
  290. netif_start_queue(ndev);
  291. spin_lock_irqsave(&nets[netid].lock, flags);
  292. list_for_each_entry(peer, &nets[netid].peers, node) {
  293. /* Send a join message */
  294. rio_send_doorbell(peer->rdev, RIONET_DOORBELL_JOIN);
  295. }
  296. spin_unlock_irqrestore(&nets[netid].lock, flags);
  297. rnet->open = true;
  298. out:
  299. return rc;
  300. }
  301. static int rionet_close(struct net_device *ndev)
  302. {
  303. struct rionet_private *rnet = netdev_priv(ndev);
  304. struct rionet_peer *peer;
  305. unsigned char netid = rnet->mport->id;
  306. unsigned long flags;
  307. int i;
  308. if (netif_msg_ifup(rnet))
  309. printk(KERN_INFO "%s: close %s\n", DRV_NAME, ndev->name);
  310. netif_stop_queue(ndev);
  311. netif_carrier_off(ndev);
  312. rnet->open = false;
  313. for (i = 0; i < RIONET_RX_RING_SIZE; i++)
  314. kfree_skb(rnet->rx_skb[i]);
  315. spin_lock_irqsave(&nets[netid].lock, flags);
  316. list_for_each_entry(peer, &nets[netid].peers, node) {
  317. if (nets[netid].active[peer->rdev->destid]) {
  318. rio_send_doorbell(peer->rdev, RIONET_DOORBELL_LEAVE);
  319. nets[netid].active[peer->rdev->destid] = NULL;
  320. }
  321. if (peer->res)
  322. rio_release_outb_dbell(peer->rdev, peer->res);
  323. }
  324. spin_unlock_irqrestore(&nets[netid].lock, flags);
  325. rio_release_inb_dbell(rnet->mport, RIONET_DOORBELL_JOIN,
  326. RIONET_DOORBELL_LEAVE);
  327. rio_release_inb_mbox(rnet->mport, RIONET_MAILBOX);
  328. rio_release_outb_mbox(rnet->mport, RIONET_MAILBOX);
  329. return 0;
  330. }
  331. static void rionet_remove_dev(struct device *dev, struct subsys_interface *sif)
  332. {
  333. struct rio_dev *rdev = to_rio_dev(dev);
  334. unsigned char netid = rdev->net->hport->id;
  335. struct rionet_peer *peer;
  336. int state, found = 0;
  337. unsigned long flags;
  338. if (!dev_rionet_capable(rdev))
  339. return;
  340. spin_lock_irqsave(&nets[netid].lock, flags);
  341. list_for_each_entry(peer, &nets[netid].peers, node) {
  342. if (peer->rdev == rdev) {
  343. list_del(&peer->node);
  344. if (nets[netid].active[rdev->destid]) {
  345. state = atomic_read(&rdev->state);
  346. if (state != RIO_DEVICE_GONE &&
  347. state != RIO_DEVICE_INITIALIZING) {
  348. rio_send_doorbell(rdev,
  349. RIONET_DOORBELL_LEAVE);
  350. }
  351. nets[netid].active[rdev->destid] = NULL;
  352. nets[netid].nact--;
  353. }
  354. found = 1;
  355. break;
  356. }
  357. }
  358. spin_unlock_irqrestore(&nets[netid].lock, flags);
  359. if (found) {
  360. if (peer->res)
  361. rio_release_outb_dbell(rdev, peer->res);
  362. kfree(peer);
  363. }
  364. }
  365. static void rionet_get_drvinfo(struct net_device *ndev,
  366. struct ethtool_drvinfo *info)
  367. {
  368. struct rionet_private *rnet = netdev_priv(ndev);
  369. strlcpy(info->driver, DRV_NAME, sizeof(info->driver));
  370. strlcpy(info->version, DRV_VERSION, sizeof(info->version));
  371. strlcpy(info->fw_version, "n/a", sizeof(info->fw_version));
  372. strlcpy(info->bus_info, rnet->mport->name, sizeof(info->bus_info));
  373. }
  374. static u32 rionet_get_msglevel(struct net_device *ndev)
  375. {
  376. struct rionet_private *rnet = netdev_priv(ndev);
  377. return rnet->msg_enable;
  378. }
  379. static void rionet_set_msglevel(struct net_device *ndev, u32 value)
  380. {
  381. struct rionet_private *rnet = netdev_priv(ndev);
  382. rnet->msg_enable = value;
  383. }
  384. static const struct ethtool_ops rionet_ethtool_ops = {
  385. .get_drvinfo = rionet_get_drvinfo,
  386. .get_msglevel = rionet_get_msglevel,
  387. .set_msglevel = rionet_set_msglevel,
  388. .get_link = ethtool_op_get_link,
  389. };
  390. static const struct net_device_ops rionet_netdev_ops = {
  391. .ndo_open = rionet_open,
  392. .ndo_stop = rionet_close,
  393. .ndo_start_xmit = rionet_start_xmit,
  394. .ndo_validate_addr = eth_validate_addr,
  395. .ndo_set_mac_address = eth_mac_addr,
  396. };
  397. static int rionet_setup_netdev(struct rio_mport *mport, struct net_device *ndev)
  398. {
  399. int rc = 0;
  400. struct rionet_private *rnet;
  401. u16 device_id;
  402. const size_t rionet_active_bytes = sizeof(void *) *
  403. RIO_MAX_ROUTE_ENTRIES(mport->sys_size);
  404. nets[mport->id].active = (struct rio_dev **)__get_free_pages(GFP_KERNEL,
  405. get_order(rionet_active_bytes));
  406. if (!nets[mport->id].active) {
  407. rc = -ENOMEM;
  408. goto out;
  409. }
  410. memset((void *)nets[mport->id].active, 0, rionet_active_bytes);
  411. /* Set up private area */
  412. rnet = netdev_priv(ndev);
  413. rnet->mport = mport;
  414. rnet->open = false;
  415. /* Set the default MAC address */
  416. device_id = rio_local_get_device_id(mport);
  417. ndev->dev_addr[0] = 0x00;
  418. ndev->dev_addr[1] = 0x01;
  419. ndev->dev_addr[2] = 0x00;
  420. ndev->dev_addr[3] = 0x01;
  421. ndev->dev_addr[4] = device_id >> 8;
  422. ndev->dev_addr[5] = device_id & 0xff;
  423. ndev->netdev_ops = &rionet_netdev_ops;
  424. ndev->mtu = RIONET_MAX_MTU;
  425. /* MTU range: 68 - 4082 */
  426. ndev->min_mtu = ETH_MIN_MTU;
  427. ndev->max_mtu = RIONET_MAX_MTU;
  428. ndev->features = NETIF_F_LLTX;
  429. SET_NETDEV_DEV(ndev, &mport->dev);
  430. ndev->ethtool_ops = &rionet_ethtool_ops;
  431. spin_lock_init(&rnet->lock);
  432. spin_lock_init(&rnet->tx_lock);
  433. rnet->msg_enable = RIONET_DEFAULT_MSGLEVEL;
  434. rc = register_netdev(ndev);
  435. if (rc != 0) {
  436. free_pages((unsigned long)nets[mport->id].active,
  437. get_order(rionet_active_bytes));
  438. goto out;
  439. }
  440. printk(KERN_INFO "%s: %s %s Version %s, MAC %pM, %s\n",
  441. ndev->name,
  442. DRV_NAME,
  443. DRV_DESC,
  444. DRV_VERSION,
  445. ndev->dev_addr,
  446. mport->name);
  447. out:
  448. return rc;
  449. }
  450. static int rionet_add_dev(struct device *dev, struct subsys_interface *sif)
  451. {
  452. int rc = -ENODEV;
  453. u32 lsrc_ops, ldst_ops;
  454. struct rionet_peer *peer;
  455. struct net_device *ndev = NULL;
  456. struct rio_dev *rdev = to_rio_dev(dev);
  457. unsigned char netid = rdev->net->hport->id;
  458. if (netid >= RIONET_MAX_NETS)
  459. return rc;
  460. /*
  461. * If first time through this net, make sure local device is rionet
  462. * capable and setup netdev (this step will be skipped in later probes
  463. * on the same net).
  464. */
  465. if (!nets[netid].ndev) {
  466. rio_local_read_config_32(rdev->net->hport, RIO_SRC_OPS_CAR,
  467. &lsrc_ops);
  468. rio_local_read_config_32(rdev->net->hport, RIO_DST_OPS_CAR,
  469. &ldst_ops);
  470. if (!is_rionet_capable(lsrc_ops, ldst_ops)) {
  471. printk(KERN_ERR
  472. "%s: local device %s is not network capable\n",
  473. DRV_NAME, rdev->net->hport->name);
  474. goto out;
  475. }
  476. /* Allocate our net_device structure */
  477. ndev = alloc_etherdev(sizeof(struct rionet_private));
  478. if (ndev == NULL) {
  479. rc = -ENOMEM;
  480. goto out;
  481. }
  482. rc = rionet_setup_netdev(rdev->net->hport, ndev);
  483. if (rc) {
  484. printk(KERN_ERR "%s: failed to setup netdev (rc=%d)\n",
  485. DRV_NAME, rc);
  486. free_netdev(ndev);
  487. goto out;
  488. }
  489. INIT_LIST_HEAD(&nets[netid].peers);
  490. spin_lock_init(&nets[netid].lock);
  491. nets[netid].nact = 0;
  492. nets[netid].ndev = ndev;
  493. }
  494. /*
  495. * If the remote device has mailbox/doorbell capabilities,
  496. * add it to the peer list.
  497. */
  498. if (dev_rionet_capable(rdev)) {
  499. struct rionet_private *rnet;
  500. unsigned long flags;
  501. rnet = netdev_priv(nets[netid].ndev);
  502. peer = kzalloc(sizeof(*peer), GFP_KERNEL);
  503. if (!peer) {
  504. rc = -ENOMEM;
  505. goto out;
  506. }
  507. peer->rdev = rdev;
  508. peer->res = rio_request_outb_dbell(peer->rdev,
  509. RIONET_DOORBELL_JOIN,
  510. RIONET_DOORBELL_LEAVE);
  511. if (!peer->res) {
  512. pr_err("%s: error requesting doorbells\n", DRV_NAME);
  513. kfree(peer);
  514. rc = -ENOMEM;
  515. goto out;
  516. }
  517. spin_lock_irqsave(&nets[netid].lock, flags);
  518. list_add_tail(&peer->node, &nets[netid].peers);
  519. spin_unlock_irqrestore(&nets[netid].lock, flags);
  520. pr_debug("%s: %s add peer %s\n",
  521. DRV_NAME, __func__, rio_name(rdev));
  522. /* If netdev is already opened, send join request to new peer */
  523. if (rnet->open)
  524. rio_send_doorbell(peer->rdev, RIONET_DOORBELL_JOIN);
  525. }
  526. return 0;
  527. out:
  528. return rc;
  529. }
  530. static int rionet_shutdown(struct notifier_block *nb, unsigned long code,
  531. void *unused)
  532. {
  533. struct rionet_peer *peer;
  534. unsigned long flags;
  535. int i;
  536. pr_debug("%s: %s\n", DRV_NAME, __func__);
  537. for (i = 0; i < RIONET_MAX_NETS; i++) {
  538. if (!nets[i].ndev)
  539. continue;
  540. spin_lock_irqsave(&nets[i].lock, flags);
  541. list_for_each_entry(peer, &nets[i].peers, node) {
  542. if (nets[i].active[peer->rdev->destid]) {
  543. rio_send_doorbell(peer->rdev,
  544. RIONET_DOORBELL_LEAVE);
  545. nets[i].active[peer->rdev->destid] = NULL;
  546. }
  547. }
  548. spin_unlock_irqrestore(&nets[i].lock, flags);
  549. }
  550. return NOTIFY_DONE;
  551. }
  552. static void rionet_remove_mport(struct device *dev,
  553. struct class_interface *class_intf)
  554. {
  555. struct rio_mport *mport = to_rio_mport(dev);
  556. struct net_device *ndev;
  557. int id = mport->id;
  558. pr_debug("%s %s\n", __func__, mport->name);
  559. WARN(nets[id].nact, "%s called when connected to %d peers\n",
  560. __func__, nets[id].nact);
  561. WARN(!nets[id].ndev, "%s called for mport without NDEV\n",
  562. __func__);
  563. if (nets[id].ndev) {
  564. ndev = nets[id].ndev;
  565. netif_stop_queue(ndev);
  566. unregister_netdev(ndev);
  567. free_pages((unsigned long)nets[id].active,
  568. get_order(sizeof(void *) *
  569. RIO_MAX_ROUTE_ENTRIES(mport->sys_size)));
  570. nets[id].active = NULL;
  571. free_netdev(ndev);
  572. nets[id].ndev = NULL;
  573. }
  574. }
  575. #ifdef MODULE
  576. static struct rio_device_id rionet_id_table[] = {
  577. {RIO_DEVICE(RIO_ANY_ID, RIO_ANY_ID)},
  578. { 0, } /* terminate list */
  579. };
  580. MODULE_DEVICE_TABLE(rapidio, rionet_id_table);
  581. #endif
  582. static struct subsys_interface rionet_interface = {
  583. .name = "rionet",
  584. .subsys = &rio_bus_type,
  585. .add_dev = rionet_add_dev,
  586. .remove_dev = rionet_remove_dev,
  587. };
  588. static struct notifier_block rionet_notifier = {
  589. .notifier_call = rionet_shutdown,
  590. };
  591. /* the rio_mport_interface is used to handle local mport devices */
  592. static struct class_interface rio_mport_interface __refdata = {
  593. .class = &rio_mport_class,
  594. .add_dev = NULL,
  595. .remove_dev = rionet_remove_mport,
  596. };
  597. static int __init rionet_init(void)
  598. {
  599. int ret;
  600. ret = register_reboot_notifier(&rionet_notifier);
  601. if (ret) {
  602. pr_err("%s: failed to register reboot notifier (err=%d)\n",
  603. DRV_NAME, ret);
  604. return ret;
  605. }
  606. ret = class_interface_register(&rio_mport_interface);
  607. if (ret) {
  608. pr_err("%s: class_interface_register error: %d\n",
  609. DRV_NAME, ret);
  610. return ret;
  611. }
  612. return subsys_interface_register(&rionet_interface);
  613. }
  614. static void __exit rionet_exit(void)
  615. {
  616. unregister_reboot_notifier(&rionet_notifier);
  617. subsys_interface_unregister(&rionet_interface);
  618. class_interface_unregister(&rio_mport_interface);
  619. }
  620. late_initcall(rionet_init);
  621. module_exit(rionet_exit);