pn_netlink.c 7.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * File: pn_netlink.c
  4. *
  5. * Phonet netlink interface
  6. *
  7. * Copyright (C) 2008 Nokia Corporation.
  8. *
  9. * Authors: Sakari Ailus <sakari.ailus@nokia.com>
  10. * Remi Denis-Courmont
  11. */
  12. #include <linux/kernel.h>
  13. #include <linux/netlink.h>
  14. #include <linux/phonet.h>
  15. #include <linux/slab.h>
  16. #include <net/sock.h>
  17. #include <net/phonet/pn_dev.h>
  18. /* Device address handling */
  19. static int fill_addr(struct sk_buff *skb, struct net_device *dev, u8 addr,
  20. u32 portid, u32 seq, int event);
  21. void phonet_address_notify(int event, struct net_device *dev, u8 addr)
  22. {
  23. struct sk_buff *skb;
  24. int err = -ENOBUFS;
  25. skb = nlmsg_new(NLMSG_ALIGN(sizeof(struct ifaddrmsg)) +
  26. nla_total_size(1), GFP_KERNEL);
  27. if (skb == NULL)
  28. goto errout;
  29. err = fill_addr(skb, dev, addr, 0, 0, event);
  30. if (err < 0) {
  31. WARN_ON(err == -EMSGSIZE);
  32. kfree_skb(skb);
  33. goto errout;
  34. }
  35. rtnl_notify(skb, dev_net(dev), 0,
  36. RTNLGRP_PHONET_IFADDR, NULL, GFP_KERNEL);
  37. return;
  38. errout:
  39. rtnl_set_sk_err(dev_net(dev), RTNLGRP_PHONET_IFADDR, err);
  40. }
  41. static const struct nla_policy ifa_phonet_policy[IFA_MAX+1] = {
  42. [IFA_LOCAL] = { .type = NLA_U8 },
  43. };
  44. static int addr_doit(struct sk_buff *skb, struct nlmsghdr *nlh,
  45. struct netlink_ext_ack *extack)
  46. {
  47. struct net *net = sock_net(skb->sk);
  48. struct nlattr *tb[IFA_MAX+1];
  49. struct net_device *dev;
  50. struct ifaddrmsg *ifm;
  51. int err;
  52. u8 pnaddr;
  53. if (!netlink_capable(skb, CAP_NET_ADMIN))
  54. return -EPERM;
  55. if (!netlink_capable(skb, CAP_SYS_ADMIN))
  56. return -EPERM;
  57. ASSERT_RTNL();
  58. err = nlmsg_parse_deprecated(nlh, sizeof(*ifm), tb, IFA_MAX,
  59. ifa_phonet_policy, extack);
  60. if (err < 0)
  61. return err;
  62. ifm = nlmsg_data(nlh);
  63. if (tb[IFA_LOCAL] == NULL)
  64. return -EINVAL;
  65. pnaddr = nla_get_u8(tb[IFA_LOCAL]);
  66. if (pnaddr & 3)
  67. /* Phonet addresses only have 6 high-order bits */
  68. return -EINVAL;
  69. dev = __dev_get_by_index(net, ifm->ifa_index);
  70. if (dev == NULL)
  71. return -ENODEV;
  72. if (nlh->nlmsg_type == RTM_NEWADDR)
  73. err = phonet_address_add(dev, pnaddr);
  74. else
  75. err = phonet_address_del(dev, pnaddr);
  76. if (!err)
  77. phonet_address_notify(nlh->nlmsg_type, dev, pnaddr);
  78. return err;
  79. }
  80. static int fill_addr(struct sk_buff *skb, struct net_device *dev, u8 addr,
  81. u32 portid, u32 seq, int event)
  82. {
  83. struct ifaddrmsg *ifm;
  84. struct nlmsghdr *nlh;
  85. nlh = nlmsg_put(skb, portid, seq, event, sizeof(*ifm), 0);
  86. if (nlh == NULL)
  87. return -EMSGSIZE;
  88. ifm = nlmsg_data(nlh);
  89. ifm->ifa_family = AF_PHONET;
  90. ifm->ifa_prefixlen = 0;
  91. ifm->ifa_flags = IFA_F_PERMANENT;
  92. ifm->ifa_scope = RT_SCOPE_LINK;
  93. ifm->ifa_index = dev->ifindex;
  94. if (nla_put_u8(skb, IFA_LOCAL, addr))
  95. goto nla_put_failure;
  96. nlmsg_end(skb, nlh);
  97. return 0;
  98. nla_put_failure:
  99. nlmsg_cancel(skb, nlh);
  100. return -EMSGSIZE;
  101. }
  102. static int getaddr_dumpit(struct sk_buff *skb, struct netlink_callback *cb)
  103. {
  104. struct phonet_device_list *pndevs;
  105. struct phonet_device *pnd;
  106. int dev_idx = 0, dev_start_idx = cb->args[0];
  107. int addr_idx = 0, addr_start_idx = cb->args[1];
  108. pndevs = phonet_device_list(sock_net(skb->sk));
  109. rcu_read_lock();
  110. list_for_each_entry_rcu(pnd, &pndevs->list, list) {
  111. u8 addr;
  112. if (dev_idx > dev_start_idx)
  113. addr_start_idx = 0;
  114. if (dev_idx++ < dev_start_idx)
  115. continue;
  116. addr_idx = 0;
  117. for_each_set_bit(addr, pnd->addrs, 64) {
  118. if (addr_idx++ < addr_start_idx)
  119. continue;
  120. if (fill_addr(skb, pnd->netdev, addr << 2,
  121. NETLINK_CB(cb->skb).portid,
  122. cb->nlh->nlmsg_seq, RTM_NEWADDR) < 0)
  123. goto out;
  124. }
  125. }
  126. out:
  127. rcu_read_unlock();
  128. cb->args[0] = dev_idx;
  129. cb->args[1] = addr_idx;
  130. return skb->len;
  131. }
  132. /* Routes handling */
  133. static int fill_route(struct sk_buff *skb, struct net_device *dev, u8 dst,
  134. u32 portid, u32 seq, int event)
  135. {
  136. struct rtmsg *rtm;
  137. struct nlmsghdr *nlh;
  138. nlh = nlmsg_put(skb, portid, seq, event, sizeof(*rtm), 0);
  139. if (nlh == NULL)
  140. return -EMSGSIZE;
  141. rtm = nlmsg_data(nlh);
  142. rtm->rtm_family = AF_PHONET;
  143. rtm->rtm_dst_len = 6;
  144. rtm->rtm_src_len = 0;
  145. rtm->rtm_tos = 0;
  146. rtm->rtm_table = RT_TABLE_MAIN;
  147. rtm->rtm_protocol = RTPROT_STATIC;
  148. rtm->rtm_scope = RT_SCOPE_UNIVERSE;
  149. rtm->rtm_type = RTN_UNICAST;
  150. rtm->rtm_flags = 0;
  151. if (nla_put_u8(skb, RTA_DST, dst) ||
  152. nla_put_u32(skb, RTA_OIF, dev->ifindex))
  153. goto nla_put_failure;
  154. nlmsg_end(skb, nlh);
  155. return 0;
  156. nla_put_failure:
  157. nlmsg_cancel(skb, nlh);
  158. return -EMSGSIZE;
  159. }
  160. void rtm_phonet_notify(int event, struct net_device *dev, u8 dst)
  161. {
  162. struct sk_buff *skb;
  163. int err = -ENOBUFS;
  164. skb = nlmsg_new(NLMSG_ALIGN(sizeof(struct ifaddrmsg)) +
  165. nla_total_size(1) + nla_total_size(4), GFP_KERNEL);
  166. if (skb == NULL)
  167. goto errout;
  168. err = fill_route(skb, dev, dst, 0, 0, event);
  169. if (err < 0) {
  170. WARN_ON(err == -EMSGSIZE);
  171. kfree_skb(skb);
  172. goto errout;
  173. }
  174. rtnl_notify(skb, dev_net(dev), 0,
  175. RTNLGRP_PHONET_ROUTE, NULL, GFP_KERNEL);
  176. return;
  177. errout:
  178. rtnl_set_sk_err(dev_net(dev), RTNLGRP_PHONET_ROUTE, err);
  179. }
  180. static const struct nla_policy rtm_phonet_policy[RTA_MAX+1] = {
  181. [RTA_DST] = { .type = NLA_U8 },
  182. [RTA_OIF] = { .type = NLA_U32 },
  183. };
  184. static int route_doit(struct sk_buff *skb, struct nlmsghdr *nlh,
  185. struct netlink_ext_ack *extack)
  186. {
  187. struct net *net = sock_net(skb->sk);
  188. struct nlattr *tb[RTA_MAX+1];
  189. struct net_device *dev;
  190. struct rtmsg *rtm;
  191. int err;
  192. u8 dst;
  193. if (!netlink_capable(skb, CAP_NET_ADMIN))
  194. return -EPERM;
  195. if (!netlink_capable(skb, CAP_SYS_ADMIN))
  196. return -EPERM;
  197. ASSERT_RTNL();
  198. err = nlmsg_parse_deprecated(nlh, sizeof(*rtm), tb, RTA_MAX,
  199. rtm_phonet_policy, extack);
  200. if (err < 0)
  201. return err;
  202. rtm = nlmsg_data(nlh);
  203. if (rtm->rtm_table != RT_TABLE_MAIN || rtm->rtm_type != RTN_UNICAST)
  204. return -EINVAL;
  205. if (tb[RTA_DST] == NULL || tb[RTA_OIF] == NULL)
  206. return -EINVAL;
  207. dst = nla_get_u8(tb[RTA_DST]);
  208. if (dst & 3) /* Phonet addresses only have 6 high-order bits */
  209. return -EINVAL;
  210. dev = __dev_get_by_index(net, nla_get_u32(tb[RTA_OIF]));
  211. if (dev == NULL)
  212. return -ENODEV;
  213. if (nlh->nlmsg_type == RTM_NEWROUTE)
  214. err = phonet_route_add(dev, dst);
  215. else
  216. err = phonet_route_del(dev, dst);
  217. if (!err)
  218. rtm_phonet_notify(nlh->nlmsg_type, dev, dst);
  219. return err;
  220. }
  221. static int route_dumpit(struct sk_buff *skb, struct netlink_callback *cb)
  222. {
  223. struct net *net = sock_net(skb->sk);
  224. u8 addr;
  225. rcu_read_lock();
  226. for (addr = cb->args[0]; addr < 64; addr++) {
  227. struct net_device *dev = phonet_route_get_rcu(net, addr << 2);
  228. if (!dev)
  229. continue;
  230. if (fill_route(skb, dev, addr << 2, NETLINK_CB(cb->skb).portid,
  231. cb->nlh->nlmsg_seq, RTM_NEWROUTE) < 0)
  232. goto out;
  233. }
  234. out:
  235. rcu_read_unlock();
  236. cb->args[0] = addr;
  237. return skb->len;
  238. }
  239. int __init phonet_netlink_register(void)
  240. {
  241. int err = rtnl_register_module(THIS_MODULE, PF_PHONET, RTM_NEWADDR,
  242. addr_doit, NULL, 0);
  243. if (err)
  244. return err;
  245. /* Further rtnl_register_module() cannot fail */
  246. rtnl_register_module(THIS_MODULE, PF_PHONET, RTM_DELADDR,
  247. addr_doit, NULL, 0);
  248. rtnl_register_module(THIS_MODULE, PF_PHONET, RTM_GETADDR,
  249. NULL, getaddr_dumpit, 0);
  250. rtnl_register_module(THIS_MODULE, PF_PHONET, RTM_NEWROUTE,
  251. route_doit, NULL, 0);
  252. rtnl_register_module(THIS_MODULE, PF_PHONET, RTM_DELROUTE,
  253. route_doit, NULL, 0);
  254. rtnl_register_module(THIS_MODULE, PF_PHONET, RTM_GETROUTE,
  255. NULL, route_dumpit, 0);
  256. return 0;
  257. }