pn_dev.c 9.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * File: pn_dev.c
  4. *
  5. * Phonet network device
  6. *
  7. * Copyright (C) 2008 Nokia Corporation.
  8. *
  9. * Authors: Sakari Ailus <sakari.ailus@nokia.com>
  10. * Rémi Denis-Courmont
  11. */
  12. #include <linux/kernel.h>
  13. #include <linux/net.h>
  14. #include <linux/slab.h>
  15. #include <linux/netdevice.h>
  16. #include <linux/phonet.h>
  17. #include <linux/proc_fs.h>
  18. #include <linux/if_arp.h>
  19. #include <net/sock.h>
  20. #include <net/netns/generic.h>
  21. #include <net/phonet/pn_dev.h>
  22. struct phonet_routes {
  23. struct mutex lock;
  24. struct net_device __rcu *table[64];
  25. };
  26. struct phonet_net {
  27. struct phonet_device_list pndevs;
  28. struct phonet_routes routes;
  29. };
  30. static unsigned int phonet_net_id __read_mostly;
  31. static struct phonet_net *phonet_pernet(struct net *net)
  32. {
  33. return net_generic(net, phonet_net_id);
  34. }
  35. struct phonet_device_list *phonet_device_list(struct net *net)
  36. {
  37. struct phonet_net *pnn = phonet_pernet(net);
  38. return &pnn->pndevs;
  39. }
  40. /* Allocate new Phonet device. */
  41. static struct phonet_device *__phonet_device_alloc(struct net_device *dev)
  42. {
  43. struct phonet_device_list *pndevs = phonet_device_list(dev_net(dev));
  44. struct phonet_device *pnd = kmalloc(sizeof(*pnd), GFP_ATOMIC);
  45. if (pnd == NULL)
  46. return NULL;
  47. pnd->netdev = dev;
  48. bitmap_zero(pnd->addrs, 64);
  49. BUG_ON(!mutex_is_locked(&pndevs->lock));
  50. list_add_rcu(&pnd->list, &pndevs->list);
  51. return pnd;
  52. }
  53. static struct phonet_device *__phonet_get(struct net_device *dev)
  54. {
  55. struct phonet_device_list *pndevs = phonet_device_list(dev_net(dev));
  56. struct phonet_device *pnd;
  57. BUG_ON(!mutex_is_locked(&pndevs->lock));
  58. list_for_each_entry(pnd, &pndevs->list, list) {
  59. if (pnd->netdev == dev)
  60. return pnd;
  61. }
  62. return NULL;
  63. }
  64. static struct phonet_device *__phonet_get_rcu(struct net_device *dev)
  65. {
  66. struct phonet_device_list *pndevs = phonet_device_list(dev_net(dev));
  67. struct phonet_device *pnd;
  68. list_for_each_entry_rcu(pnd, &pndevs->list, list) {
  69. if (pnd->netdev == dev)
  70. return pnd;
  71. }
  72. return NULL;
  73. }
  74. static void phonet_device_destroy(struct net_device *dev)
  75. {
  76. struct phonet_device_list *pndevs = phonet_device_list(dev_net(dev));
  77. struct phonet_device *pnd;
  78. ASSERT_RTNL();
  79. mutex_lock(&pndevs->lock);
  80. pnd = __phonet_get(dev);
  81. if (pnd)
  82. list_del_rcu(&pnd->list);
  83. mutex_unlock(&pndevs->lock);
  84. if (pnd) {
  85. u8 addr;
  86. for_each_set_bit(addr, pnd->addrs, 64)
  87. phonet_address_notify(RTM_DELADDR, dev, addr);
  88. kfree(pnd);
  89. }
  90. }
  91. struct net_device *phonet_device_get(struct net *net)
  92. {
  93. struct phonet_device_list *pndevs = phonet_device_list(net);
  94. struct phonet_device *pnd;
  95. struct net_device *dev = NULL;
  96. rcu_read_lock();
  97. list_for_each_entry_rcu(pnd, &pndevs->list, list) {
  98. dev = pnd->netdev;
  99. BUG_ON(!dev);
  100. if ((dev->reg_state == NETREG_REGISTERED) &&
  101. ((pnd->netdev->flags & IFF_UP)) == IFF_UP)
  102. break;
  103. dev = NULL;
  104. }
  105. if (dev)
  106. dev_hold(dev);
  107. rcu_read_unlock();
  108. return dev;
  109. }
  110. int phonet_address_add(struct net_device *dev, u8 addr)
  111. {
  112. struct phonet_device_list *pndevs = phonet_device_list(dev_net(dev));
  113. struct phonet_device *pnd;
  114. int err = 0;
  115. mutex_lock(&pndevs->lock);
  116. /* Find or create Phonet-specific device data */
  117. pnd = __phonet_get(dev);
  118. if (pnd == NULL)
  119. pnd = __phonet_device_alloc(dev);
  120. if (unlikely(pnd == NULL))
  121. err = -ENOMEM;
  122. else if (test_and_set_bit(addr >> 2, pnd->addrs))
  123. err = -EEXIST;
  124. mutex_unlock(&pndevs->lock);
  125. return err;
  126. }
  127. int phonet_address_del(struct net_device *dev, u8 addr)
  128. {
  129. struct phonet_device_list *pndevs = phonet_device_list(dev_net(dev));
  130. struct phonet_device *pnd;
  131. int err = 0;
  132. mutex_lock(&pndevs->lock);
  133. pnd = __phonet_get(dev);
  134. if (!pnd || !test_and_clear_bit(addr >> 2, pnd->addrs)) {
  135. err = -EADDRNOTAVAIL;
  136. pnd = NULL;
  137. } else if (bitmap_empty(pnd->addrs, 64))
  138. list_del_rcu(&pnd->list);
  139. else
  140. pnd = NULL;
  141. mutex_unlock(&pndevs->lock);
  142. if (pnd)
  143. kfree_rcu(pnd, rcu);
  144. return err;
  145. }
  146. /* Gets a source address toward a destination, through a interface. */
  147. u8 phonet_address_get(struct net_device *dev, u8 daddr)
  148. {
  149. struct phonet_device *pnd;
  150. u8 saddr;
  151. rcu_read_lock();
  152. pnd = __phonet_get_rcu(dev);
  153. if (pnd) {
  154. BUG_ON(bitmap_empty(pnd->addrs, 64));
  155. /* Use same source address as destination, if possible */
  156. if (test_bit(daddr >> 2, pnd->addrs))
  157. saddr = daddr;
  158. else
  159. saddr = find_first_bit(pnd->addrs, 64) << 2;
  160. } else
  161. saddr = PN_NO_ADDR;
  162. rcu_read_unlock();
  163. if (saddr == PN_NO_ADDR) {
  164. /* Fallback to another device */
  165. struct net_device *def_dev;
  166. def_dev = phonet_device_get(dev_net(dev));
  167. if (def_dev) {
  168. if (def_dev != dev)
  169. saddr = phonet_address_get(def_dev, daddr);
  170. dev_put(def_dev);
  171. }
  172. }
  173. return saddr;
  174. }
  175. int phonet_address_lookup(struct net *net, u8 addr)
  176. {
  177. struct phonet_device_list *pndevs = phonet_device_list(net);
  178. struct phonet_device *pnd;
  179. int err = -EADDRNOTAVAIL;
  180. rcu_read_lock();
  181. list_for_each_entry_rcu(pnd, &pndevs->list, list) {
  182. /* Don't allow unregistering devices! */
  183. if ((pnd->netdev->reg_state != NETREG_REGISTERED) ||
  184. ((pnd->netdev->flags & IFF_UP)) != IFF_UP)
  185. continue;
  186. if (test_bit(addr >> 2, pnd->addrs)) {
  187. err = 0;
  188. goto found;
  189. }
  190. }
  191. found:
  192. rcu_read_unlock();
  193. return err;
  194. }
  195. /* automatically configure a Phonet device, if supported */
  196. static int phonet_device_autoconf(struct net_device *dev)
  197. {
  198. struct if_phonet_req req;
  199. int ret;
  200. if (!dev->netdev_ops->ndo_do_ioctl)
  201. return -EOPNOTSUPP;
  202. ret = dev->netdev_ops->ndo_do_ioctl(dev, (struct ifreq *)&req,
  203. SIOCPNGAUTOCONF);
  204. if (ret < 0)
  205. return ret;
  206. ASSERT_RTNL();
  207. ret = phonet_address_add(dev, req.ifr_phonet_autoconf.device);
  208. if (ret)
  209. return ret;
  210. phonet_address_notify(RTM_NEWADDR, dev,
  211. req.ifr_phonet_autoconf.device);
  212. return 0;
  213. }
  214. static void phonet_route_autodel(struct net_device *dev)
  215. {
  216. struct phonet_net *pnn = phonet_pernet(dev_net(dev));
  217. unsigned int i;
  218. DECLARE_BITMAP(deleted, 64);
  219. /* Remove left-over Phonet routes */
  220. bitmap_zero(deleted, 64);
  221. mutex_lock(&pnn->routes.lock);
  222. for (i = 0; i < 64; i++)
  223. if (rcu_access_pointer(pnn->routes.table[i]) == dev) {
  224. RCU_INIT_POINTER(pnn->routes.table[i], NULL);
  225. set_bit(i, deleted);
  226. }
  227. mutex_unlock(&pnn->routes.lock);
  228. if (bitmap_empty(deleted, 64))
  229. return; /* short-circuit RCU */
  230. synchronize_rcu();
  231. for_each_set_bit(i, deleted, 64) {
  232. rtm_phonet_notify(RTM_DELROUTE, dev, i);
  233. dev_put(dev);
  234. }
  235. }
  236. /* notify Phonet of device events */
  237. static int phonet_device_notify(struct notifier_block *me, unsigned long what,
  238. void *ptr)
  239. {
  240. struct net_device *dev = netdev_notifier_info_to_dev(ptr);
  241. switch (what) {
  242. case NETDEV_REGISTER:
  243. if (dev->type == ARPHRD_PHONET)
  244. phonet_device_autoconf(dev);
  245. break;
  246. case NETDEV_UNREGISTER:
  247. phonet_device_destroy(dev);
  248. phonet_route_autodel(dev);
  249. break;
  250. }
  251. return 0;
  252. }
  253. static struct notifier_block phonet_device_notifier = {
  254. .notifier_call = phonet_device_notify,
  255. .priority = 0,
  256. };
  257. /* Per-namespace Phonet devices handling */
  258. static int __net_init phonet_init_net(struct net *net)
  259. {
  260. struct phonet_net *pnn = phonet_pernet(net);
  261. if (!proc_create_net("phonet", 0, net->proc_net, &pn_sock_seq_ops,
  262. sizeof(struct seq_net_private)))
  263. return -ENOMEM;
  264. INIT_LIST_HEAD(&pnn->pndevs.list);
  265. mutex_init(&pnn->pndevs.lock);
  266. mutex_init(&pnn->routes.lock);
  267. return 0;
  268. }
  269. static void __net_exit phonet_exit_net(struct net *net)
  270. {
  271. struct phonet_net *pnn = phonet_pernet(net);
  272. remove_proc_entry("phonet", net->proc_net);
  273. WARN_ON_ONCE(!list_empty(&pnn->pndevs.list));
  274. }
  275. static struct pernet_operations phonet_net_ops = {
  276. .init = phonet_init_net,
  277. .exit = phonet_exit_net,
  278. .id = &phonet_net_id,
  279. .size = sizeof(struct phonet_net),
  280. };
  281. /* Initialize Phonet devices list */
  282. int __init phonet_device_init(void)
  283. {
  284. int err = register_pernet_subsys(&phonet_net_ops);
  285. if (err)
  286. return err;
  287. proc_create_net("pnresource", 0, init_net.proc_net, &pn_res_seq_ops,
  288. sizeof(struct seq_net_private));
  289. register_netdevice_notifier(&phonet_device_notifier);
  290. err = phonet_netlink_register();
  291. if (err)
  292. phonet_device_exit();
  293. return err;
  294. }
  295. void phonet_device_exit(void)
  296. {
  297. rtnl_unregister_all(PF_PHONET);
  298. unregister_netdevice_notifier(&phonet_device_notifier);
  299. unregister_pernet_subsys(&phonet_net_ops);
  300. remove_proc_entry("pnresource", init_net.proc_net);
  301. }
  302. int phonet_route_add(struct net_device *dev, u8 daddr)
  303. {
  304. struct phonet_net *pnn = phonet_pernet(dev_net(dev));
  305. struct phonet_routes *routes = &pnn->routes;
  306. int err = -EEXIST;
  307. daddr = daddr >> 2;
  308. mutex_lock(&routes->lock);
  309. if (routes->table[daddr] == NULL) {
  310. rcu_assign_pointer(routes->table[daddr], dev);
  311. dev_hold(dev);
  312. err = 0;
  313. }
  314. mutex_unlock(&routes->lock);
  315. return err;
  316. }
  317. int phonet_route_del(struct net_device *dev, u8 daddr)
  318. {
  319. struct phonet_net *pnn = phonet_pernet(dev_net(dev));
  320. struct phonet_routes *routes = &pnn->routes;
  321. daddr = daddr >> 2;
  322. mutex_lock(&routes->lock);
  323. if (rcu_access_pointer(routes->table[daddr]) == dev)
  324. RCU_INIT_POINTER(routes->table[daddr], NULL);
  325. else
  326. dev = NULL;
  327. mutex_unlock(&routes->lock);
  328. if (!dev)
  329. return -ENOENT;
  330. synchronize_rcu();
  331. dev_put(dev);
  332. return 0;
  333. }
  334. struct net_device *phonet_route_get_rcu(struct net *net, u8 daddr)
  335. {
  336. struct phonet_net *pnn = phonet_pernet(net);
  337. struct phonet_routes *routes = &pnn->routes;
  338. struct net_device *dev;
  339. daddr >>= 2;
  340. dev = rcu_dereference(routes->table[daddr]);
  341. return dev;
  342. }
  343. struct net_device *phonet_route_output(struct net *net, u8 daddr)
  344. {
  345. struct phonet_net *pnn = phonet_pernet(net);
  346. struct phonet_routes *routes = &pnn->routes;
  347. struct net_device *dev;
  348. daddr >>= 2;
  349. rcu_read_lock();
  350. dev = rcu_dereference(routes->table[daddr]);
  351. if (dev)
  352. dev_hold(dev);
  353. rcu_read_unlock();
  354. if (!dev)
  355. dev = phonet_device_get(net); /* Default route */
  356. return dev;
  357. }