pn_netlink.c 7.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300
  1. /*
  2. * File: pn_netlink.c
  3. *
  4. * Phonet netlink interface
  5. *
  6. * Copyright (C) 2008 Nokia Corporation.
  7. *
  8. * Contact: Remi Denis-Courmont <remi.denis-courmont@nokia.com>
  9. * Original author: Sakari Ailus <sakari.ailus@nokia.com>
  10. *
  11. * This program is free software; you can redistribute it and/or
  12. * modify it under the terms of the GNU General Public License
  13. * version 2 as published by the Free Software Foundation.
  14. *
  15. * This program is distributed in the hope that it will be useful, but
  16. * WITHOUT ANY WARRANTY; without even the implied warranty of
  17. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  18. * General Public License for more details.
  19. *
  20. * You should have received a copy of the GNU General Public License
  21. * along with this program; if not, write to the Free Software
  22. * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
  23. * 02110-1301 USA
  24. */
  25. #include <linux/kernel.h>
  26. #include <linux/netlink.h>
  27. #include <linux/phonet.h>
  28. #include <net/sock.h>
  29. #include <net/phonet/pn_dev.h>
  30. /* Device address handling */
  31. static int fill_addr(struct sk_buff *skb, struct net_device *dev, u8 addr,
  32. u32 pid, u32 seq, int event);
  33. void phonet_address_notify(int event, struct net_device *dev, u8 addr)
  34. {
  35. struct sk_buff *skb;
  36. int err = -ENOBUFS;
  37. skb = nlmsg_new(NLMSG_ALIGN(sizeof(struct ifaddrmsg)) +
  38. nla_total_size(1), GFP_KERNEL);
  39. if (skb == NULL)
  40. goto errout;
  41. err = fill_addr(skb, dev, addr, 0, 0, event);
  42. if (err < 0) {
  43. WARN_ON(err == -EMSGSIZE);
  44. kfree_skb(skb);
  45. goto errout;
  46. }
  47. rtnl_notify(skb, dev_net(dev), 0,
  48. RTNLGRP_PHONET_IFADDR, NULL, GFP_KERNEL);
  49. return;
  50. errout:
  51. rtnl_set_sk_err(dev_net(dev), RTNLGRP_PHONET_IFADDR, err);
  52. }
  53. static const struct nla_policy ifa_phonet_policy[IFA_MAX+1] = {
  54. [IFA_LOCAL] = { .type = NLA_U8 },
  55. };
  56. static int addr_doit(struct sk_buff *skb, struct nlmsghdr *nlh, void *attr)
  57. {
  58. struct net *net = sock_net(skb->sk);
  59. struct nlattr *tb[IFA_MAX+1];
  60. struct net_device *dev;
  61. struct ifaddrmsg *ifm;
  62. int err;
  63. u8 pnaddr;
  64. if (!capable(CAP_SYS_ADMIN))
  65. return -EPERM;
  66. ASSERT_RTNL();
  67. err = nlmsg_parse(nlh, sizeof(*ifm), tb, IFA_MAX, ifa_phonet_policy);
  68. if (err < 0)
  69. return err;
  70. ifm = nlmsg_data(nlh);
  71. if (tb[IFA_LOCAL] == NULL)
  72. return -EINVAL;
  73. pnaddr = nla_get_u8(tb[IFA_LOCAL]);
  74. if (pnaddr & 3)
  75. /* Phonet addresses only have 6 high-order bits */
  76. return -EINVAL;
  77. dev = __dev_get_by_index(net, ifm->ifa_index);
  78. if (dev == NULL)
  79. return -ENODEV;
  80. if (nlh->nlmsg_type == RTM_NEWADDR)
  81. err = phonet_address_add(dev, pnaddr);
  82. else
  83. err = phonet_address_del(dev, pnaddr);
  84. if (!err)
  85. phonet_address_notify(nlh->nlmsg_type, dev, pnaddr);
  86. return err;
  87. }
  88. static int fill_addr(struct sk_buff *skb, struct net_device *dev, u8 addr,
  89. u32 pid, u32 seq, int event)
  90. {
  91. struct ifaddrmsg *ifm;
  92. struct nlmsghdr *nlh;
  93. nlh = nlmsg_put(skb, pid, seq, event, sizeof(*ifm), 0);
  94. if (nlh == NULL)
  95. return -EMSGSIZE;
  96. ifm = nlmsg_data(nlh);
  97. ifm->ifa_family = AF_PHONET;
  98. ifm->ifa_prefixlen = 0;
  99. ifm->ifa_flags = IFA_F_PERMANENT;
  100. ifm->ifa_scope = RT_SCOPE_LINK;
  101. ifm->ifa_index = dev->ifindex;
  102. NLA_PUT_U8(skb, IFA_LOCAL, addr);
  103. return nlmsg_end(skb, nlh);
  104. nla_put_failure:
  105. nlmsg_cancel(skb, nlh);
  106. return -EMSGSIZE;
  107. }
  108. static int getaddr_dumpit(struct sk_buff *skb, struct netlink_callback *cb)
  109. {
  110. struct phonet_device_list *pndevs;
  111. struct phonet_device *pnd;
  112. int dev_idx = 0, dev_start_idx = cb->args[0];
  113. int addr_idx = 0, addr_start_idx = cb->args[1];
  114. pndevs = phonet_device_list(sock_net(skb->sk));
  115. rcu_read_lock();
  116. list_for_each_entry_rcu(pnd, &pndevs->list, list) {
  117. u8 addr;
  118. if (dev_idx > dev_start_idx)
  119. addr_start_idx = 0;
  120. if (dev_idx++ < dev_start_idx)
  121. continue;
  122. addr_idx = 0;
  123. for_each_set_bit(addr, pnd->addrs, 64) {
  124. if (addr_idx++ < addr_start_idx)
  125. continue;
  126. if (fill_addr(skb, pnd->netdev, addr << 2,
  127. NETLINK_CB(cb->skb).pid,
  128. cb->nlh->nlmsg_seq, RTM_NEWADDR) < 0)
  129. goto out;
  130. }
  131. }
  132. out:
  133. rcu_read_unlock();
  134. cb->args[0] = dev_idx;
  135. cb->args[1] = addr_idx;
  136. return skb->len;
  137. }
  138. /* Routes handling */
  139. static int fill_route(struct sk_buff *skb, struct net_device *dev, u8 dst,
  140. u32 pid, u32 seq, int event)
  141. {
  142. struct rtmsg *rtm;
  143. struct nlmsghdr *nlh;
  144. nlh = nlmsg_put(skb, pid, seq, event, sizeof(*rtm), 0);
  145. if (nlh == NULL)
  146. return -EMSGSIZE;
  147. rtm = nlmsg_data(nlh);
  148. rtm->rtm_family = AF_PHONET;
  149. rtm->rtm_dst_len = 6;
  150. rtm->rtm_src_len = 0;
  151. rtm->rtm_tos = 0;
  152. rtm->rtm_table = RT_TABLE_MAIN;
  153. rtm->rtm_protocol = RTPROT_STATIC;
  154. rtm->rtm_scope = RT_SCOPE_UNIVERSE;
  155. rtm->rtm_type = RTN_UNICAST;
  156. rtm->rtm_flags = 0;
  157. NLA_PUT_U8(skb, RTA_DST, dst);
  158. NLA_PUT_U32(skb, RTA_OIF, dev->ifindex);
  159. return nlmsg_end(skb, nlh);
  160. nla_put_failure:
  161. nlmsg_cancel(skb, nlh);
  162. return -EMSGSIZE;
  163. }
  164. void rtm_phonet_notify(int event, struct net_device *dev, u8 dst)
  165. {
  166. struct sk_buff *skb;
  167. int err = -ENOBUFS;
  168. skb = nlmsg_new(NLMSG_ALIGN(sizeof(struct ifaddrmsg)) +
  169. nla_total_size(1) + nla_total_size(4), GFP_KERNEL);
  170. if (skb == NULL)
  171. goto errout;
  172. err = fill_route(skb, dev, dst, 0, 0, event);
  173. if (err < 0) {
  174. WARN_ON(err == -EMSGSIZE);
  175. kfree_skb(skb);
  176. goto errout;
  177. }
  178. rtnl_notify(skb, dev_net(dev), 0,
  179. RTNLGRP_PHONET_ROUTE, NULL, GFP_KERNEL);
  180. return;
  181. errout:
  182. rtnl_set_sk_err(dev_net(dev), RTNLGRP_PHONET_ROUTE, err);
  183. }
  184. static const struct nla_policy rtm_phonet_policy[RTA_MAX+1] = {
  185. [RTA_DST] = { .type = NLA_U8 },
  186. [RTA_OIF] = { .type = NLA_U32 },
  187. };
  188. static int route_doit(struct sk_buff *skb, struct nlmsghdr *nlh, void *attr)
  189. {
  190. struct net *net = sock_net(skb->sk);
  191. struct nlattr *tb[RTA_MAX+1];
  192. struct net_device *dev;
  193. struct rtmsg *rtm;
  194. int err;
  195. u8 dst;
  196. if (!capable(CAP_SYS_ADMIN))
  197. return -EPERM;
  198. ASSERT_RTNL();
  199. err = nlmsg_parse(nlh, sizeof(*rtm), tb, RTA_MAX, rtm_phonet_policy);
  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, addr_idx = 0, addr_start_idx = cb->args[0];
  225. for (addr = 0; addr < 64; addr++) {
  226. struct net_device *dev;
  227. dev = phonet_route_get(net, addr << 2);
  228. if (!dev)
  229. continue;
  230. if (addr_idx++ < addr_start_idx)
  231. continue;
  232. if (fill_route(skb, dev, addr << 2, NETLINK_CB(cb->skb).pid,
  233. cb->nlh->nlmsg_seq, RTM_NEWROUTE))
  234. goto out;
  235. }
  236. out:
  237. cb->args[0] = addr_idx;
  238. cb->args[1] = 0;
  239. return skb->len;
  240. }
  241. int __init phonet_netlink_register(void)
  242. {
  243. int err = __rtnl_register(PF_PHONET, RTM_NEWADDR, addr_doit, NULL);
  244. if (err)
  245. return err;
  246. /* Further __rtnl_register() cannot fail */
  247. __rtnl_register(PF_PHONET, RTM_DELADDR, addr_doit, NULL);
  248. __rtnl_register(PF_PHONET, RTM_GETADDR, NULL, getaddr_dumpit);
  249. __rtnl_register(PF_PHONET, RTM_NEWROUTE, route_doit, NULL);
  250. __rtnl_register(PF_PHONET, RTM_DELROUTE, route_doit, NULL);
  251. __rtnl_register(PF_PHONET, RTM_GETROUTE, NULL, route_dumpit);
  252. return 0;
  253. }