1 /* 2 * File: af_phonet.c 3 * 4 * Phonet protocols family 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 26 #include <linux/kernel.h> 27 #include <linux/module.h> 28 #include <asm/unaligned.h> 29 #include <net/sock.h> 30 31 #include <linux/if_phonet.h> 32 #include <linux/phonet.h> 33 #include <net/phonet/phonet.h> 34 #include <net/phonet/pn_dev.h> 35 36 static struct net_proto_family phonet_proto_family; 37 static struct phonet_protocol *phonet_proto_get(int protocol); 38 static inline void phonet_proto_put(struct phonet_protocol *pp); 39 40 /* protocol family functions */ 41 42 static int pn_socket_create(struct net *net, struct socket *sock, int protocol) 43 { 44 struct sock *sk; 45 struct pn_sock *pn; 46 struct phonet_protocol *pnp; 47 int err; 48 49 if (net != &init_net) 50 return -EAFNOSUPPORT; 51 52 if (!capable(CAP_SYS_ADMIN)) 53 return -EPERM; 54 55 if (protocol == 0) { 56 /* Default protocol selection */ 57 switch (sock->type) { 58 case SOCK_DGRAM: 59 protocol = PN_PROTO_PHONET; 60 break; 61 case SOCK_SEQPACKET: 62 protocol = PN_PROTO_PIPE; 63 break; 64 default: 65 return -EPROTONOSUPPORT; 66 } 67 } 68 69 pnp = phonet_proto_get(protocol); 70 if (pnp == NULL && 71 request_module("net-pf-%d-proto-%d", PF_PHONET, protocol) == 0) 72 pnp = phonet_proto_get(protocol); 73 74 if (pnp == NULL) 75 return -EPROTONOSUPPORT; 76 if (sock->type != pnp->sock_type) { 77 err = -EPROTONOSUPPORT; 78 goto out; 79 } 80 81 sk = sk_alloc(net, PF_PHONET, GFP_KERNEL, pnp->prot); 82 if (sk == NULL) { 83 err = -ENOMEM; 84 goto out; 85 } 86 87 sock_init_data(sock, sk); 88 sock->state = SS_UNCONNECTED; 89 sock->ops = pnp->ops; 90 sk->sk_backlog_rcv = sk->sk_prot->backlog_rcv; 91 sk->sk_protocol = protocol; 92 pn = pn_sk(sk); 93 pn->sobject = 0; 94 pn->resource = 0; 95 sk->sk_prot->init(sk); 96 err = 0; 97 98 out: 99 phonet_proto_put(pnp); 100 return err; 101 } 102 103 static struct net_proto_family phonet_proto_family = { 104 .family = PF_PHONET, 105 .create = pn_socket_create, 106 .owner = THIS_MODULE, 107 }; 108 109 /* Phonet device header operations */ 110 static int pn_header_create(struct sk_buff *skb, struct net_device *dev, 111 unsigned short type, const void *daddr, 112 const void *saddr, unsigned len) 113 { 114 u8 *media = skb_push(skb, 1); 115 116 if (type != ETH_P_PHONET) 117 return -1; 118 119 if (!saddr) 120 saddr = dev->dev_addr; 121 *media = *(const u8 *)saddr; 122 return 1; 123 } 124 125 static int pn_header_parse(const struct sk_buff *skb, unsigned char *haddr) 126 { 127 const u8 *media = skb_mac_header(skb); 128 *haddr = *media; 129 return 1; 130 } 131 132 struct header_ops phonet_header_ops = { 133 .create = pn_header_create, 134 .parse = pn_header_parse, 135 }; 136 EXPORT_SYMBOL(phonet_header_ops); 137 138 /* 139 * Prepends an ISI header and sends a datagram. 140 */ 141 static int pn_send(struct sk_buff *skb, struct net_device *dev, 142 u16 dst, u16 src, u8 res, u8 irq) 143 { 144 struct phonethdr *ph; 145 int err; 146 147 if (skb->len + 2 > 0xffff) { 148 /* Phonet length field would overflow */ 149 err = -EMSGSIZE; 150 goto drop; 151 } 152 153 skb_reset_transport_header(skb); 154 WARN_ON(skb_headroom(skb) & 1); /* HW assumes word alignment */ 155 skb_push(skb, sizeof(struct phonethdr)); 156 skb_reset_network_header(skb); 157 ph = pn_hdr(skb); 158 ph->pn_rdev = pn_dev(dst); 159 ph->pn_sdev = pn_dev(src); 160 ph->pn_res = res; 161 ph->pn_length = __cpu_to_be16(skb->len + 2 - sizeof(*ph)); 162 ph->pn_robj = pn_obj(dst); 163 ph->pn_sobj = pn_obj(src); 164 165 skb->protocol = htons(ETH_P_PHONET); 166 skb->priority = 0; 167 skb->dev = dev; 168 169 if (pn_addr(src) == pn_addr(dst)) { 170 skb_reset_mac_header(skb); 171 skb->pkt_type = PACKET_LOOPBACK; 172 skb_orphan(skb); 173 if (irq) 174 netif_rx(skb); 175 else 176 netif_rx_ni(skb); 177 err = 0; 178 } else { 179 err = dev_hard_header(skb, dev, ntohs(skb->protocol), 180 NULL, NULL, skb->len); 181 if (err < 0) { 182 err = -EHOSTUNREACH; 183 goto drop; 184 } 185 err = dev_queue_xmit(skb); 186 } 187 188 return err; 189 drop: 190 kfree_skb(skb); 191 return err; 192 } 193 194 static int pn_raw_send(const void *data, int len, struct net_device *dev, 195 u16 dst, u16 src, u8 res) 196 { 197 struct sk_buff *skb = alloc_skb(MAX_PHONET_HEADER + len, GFP_ATOMIC); 198 if (skb == NULL) 199 return -ENOMEM; 200 201 skb_reserve(skb, MAX_PHONET_HEADER); 202 __skb_put(skb, len); 203 skb_copy_to_linear_data(skb, data, len); 204 return pn_send(skb, dev, dst, src, res, 1); 205 } 206 207 /* 208 * Create a Phonet header for the skb and send it out. Returns 209 * non-zero error code if failed. The skb is freed then. 210 */ 211 int pn_skb_send(struct sock *sk, struct sk_buff *skb, 212 const struct sockaddr_pn *target) 213 { 214 struct net_device *dev; 215 struct pn_sock *pn = pn_sk(sk); 216 int err; 217 u16 src; 218 u8 daddr = pn_sockaddr_get_addr(target), saddr = PN_NO_ADDR; 219 220 err = -EHOSTUNREACH; 221 if (sk->sk_bound_dev_if) 222 dev = dev_get_by_index(sock_net(sk), sk->sk_bound_dev_if); 223 else 224 dev = phonet_device_get(sock_net(sk)); 225 if (!dev || !(dev->flags & IFF_UP)) 226 goto drop; 227 228 saddr = phonet_address_get(dev, daddr); 229 if (saddr == PN_NO_ADDR) 230 goto drop; 231 232 src = pn->sobject; 233 if (!pn_addr(src)) 234 src = pn_object(saddr, pn_obj(src)); 235 236 err = pn_send(skb, dev, pn_sockaddr_get_object(target), 237 src, pn_sockaddr_get_resource(target), 0); 238 dev_put(dev); 239 return err; 240 241 drop: 242 kfree_skb(skb); 243 if (dev) 244 dev_put(dev); 245 return err; 246 } 247 EXPORT_SYMBOL(pn_skb_send); 248 249 /* Do not send an error message in response to an error message */ 250 static inline int can_respond(struct sk_buff *skb) 251 { 252 const struct phonethdr *ph; 253 const struct phonetmsg *pm; 254 u8 submsg_id; 255 256 if (!pskb_may_pull(skb, 3)) 257 return 0; 258 259 ph = pn_hdr(skb); 260 if (phonet_address_get(skb->dev, ph->pn_rdev) != ph->pn_rdev) 261 return 0; /* we are not the destination */ 262 if (ph->pn_res == PN_PREFIX && !pskb_may_pull(skb, 5)) 263 return 0; 264 if (ph->pn_res == PN_COMMGR) /* indications */ 265 return 0; 266 267 ph = pn_hdr(skb); /* re-acquires the pointer */ 268 pm = pn_msg(skb); 269 if (pm->pn_msg_id != PN_COMMON_MESSAGE) 270 return 1; 271 submsg_id = (ph->pn_res == PN_PREFIX) 272 ? pm->pn_e_submsg_id : pm->pn_submsg_id; 273 if (submsg_id != PN_COMM_ISA_ENTITY_NOT_REACHABLE_RESP && 274 pm->pn_e_submsg_id != PN_COMM_SERVICE_NOT_IDENTIFIED_RESP) 275 return 1; 276 return 0; 277 } 278 279 static int send_obj_unreachable(struct sk_buff *rskb) 280 { 281 const struct phonethdr *oph = pn_hdr(rskb); 282 const struct phonetmsg *opm = pn_msg(rskb); 283 struct phonetmsg resp; 284 285 memset(&resp, 0, sizeof(resp)); 286 resp.pn_trans_id = opm->pn_trans_id; 287 resp.pn_msg_id = PN_COMMON_MESSAGE; 288 if (oph->pn_res == PN_PREFIX) { 289 resp.pn_e_res_id = opm->pn_e_res_id; 290 resp.pn_e_submsg_id = PN_COMM_ISA_ENTITY_NOT_REACHABLE_RESP; 291 resp.pn_e_orig_msg_id = opm->pn_msg_id; 292 resp.pn_e_status = 0; 293 } else { 294 resp.pn_submsg_id = PN_COMM_ISA_ENTITY_NOT_REACHABLE_RESP; 295 resp.pn_orig_msg_id = opm->pn_msg_id; 296 resp.pn_status = 0; 297 } 298 return pn_raw_send(&resp, sizeof(resp), rskb->dev, 299 pn_object(oph->pn_sdev, oph->pn_sobj), 300 pn_object(oph->pn_rdev, oph->pn_robj), 301 oph->pn_res); 302 } 303 304 static int send_reset_indications(struct sk_buff *rskb) 305 { 306 struct phonethdr *oph = pn_hdr(rskb); 307 static const u8 data[4] = { 308 0x00 /* trans ID */, 0x10 /* subscribe msg */, 309 0x00 /* subscription count */, 0x00 /* dummy */ 310 }; 311 312 return pn_raw_send(data, sizeof(data), rskb->dev, 313 pn_object(oph->pn_sdev, 0x00), 314 pn_object(oph->pn_rdev, oph->pn_robj), 315 PN_COMMGR); 316 } 317 318 319 /* packet type functions */ 320 321 /* 322 * Stuff received packets to associated sockets. 323 * On error, returns non-zero and releases the skb. 324 */ 325 static int phonet_rcv(struct sk_buff *skb, struct net_device *dev, 326 struct packet_type *pkttype, 327 struct net_device *orig_dev) 328 { 329 struct phonethdr *ph; 330 struct sock *sk; 331 struct sockaddr_pn sa; 332 u16 len; 333 334 if (dev_net(dev) != &init_net) 335 goto out; 336 337 /* check we have at least a full Phonet header */ 338 if (!pskb_pull(skb, sizeof(struct phonethdr))) 339 goto out; 340 341 /* check that the advertised length is correct */ 342 ph = pn_hdr(skb); 343 len = get_unaligned_be16(&ph->pn_length); 344 if (len < 2) 345 goto out; 346 len -= 2; 347 if ((len > skb->len) || pskb_trim(skb, len)) 348 goto out; 349 skb_reset_transport_header(skb); 350 351 pn_skb_get_dst_sockaddr(skb, &sa); 352 if (pn_sockaddr_get_addr(&sa) == 0) 353 goto out; /* currently, we cannot be device 0 */ 354 355 sk = pn_find_sock_by_sa(&sa); 356 if (sk == NULL) { 357 if (can_respond(skb)) { 358 send_obj_unreachable(skb); 359 send_reset_indications(skb); 360 } 361 goto out; 362 } 363 364 /* Push data to the socket (or other sockets connected to it). */ 365 return sk_receive_skb(sk, skb, 0); 366 367 out: 368 kfree_skb(skb); 369 return NET_RX_DROP; 370 } 371 372 static struct packet_type phonet_packet_type = { 373 .type = __constant_htons(ETH_P_PHONET), 374 .dev = NULL, 375 .func = phonet_rcv, 376 }; 377 378 /* Transport protocol registration */ 379 static struct phonet_protocol *proto_tab[PHONET_NPROTO] __read_mostly; 380 static DEFINE_SPINLOCK(proto_tab_lock); 381 382 int __init_or_module phonet_proto_register(int protocol, 383 struct phonet_protocol *pp) 384 { 385 int err = 0; 386 387 if (protocol >= PHONET_NPROTO) 388 return -EINVAL; 389 390 err = proto_register(pp->prot, 1); 391 if (err) 392 return err; 393 394 spin_lock(&proto_tab_lock); 395 if (proto_tab[protocol]) 396 err = -EBUSY; 397 else 398 proto_tab[protocol] = pp; 399 spin_unlock(&proto_tab_lock); 400 401 return err; 402 } 403 EXPORT_SYMBOL(phonet_proto_register); 404 405 void phonet_proto_unregister(int protocol, struct phonet_protocol *pp) 406 { 407 spin_lock(&proto_tab_lock); 408 BUG_ON(proto_tab[protocol] != pp); 409 proto_tab[protocol] = NULL; 410 spin_unlock(&proto_tab_lock); 411 proto_unregister(pp->prot); 412 } 413 EXPORT_SYMBOL(phonet_proto_unregister); 414 415 static struct phonet_protocol *phonet_proto_get(int protocol) 416 { 417 struct phonet_protocol *pp; 418 419 if (protocol >= PHONET_NPROTO) 420 return NULL; 421 422 spin_lock(&proto_tab_lock); 423 pp = proto_tab[protocol]; 424 if (pp && !try_module_get(pp->prot->owner)) 425 pp = NULL; 426 spin_unlock(&proto_tab_lock); 427 428 return pp; 429 } 430 431 static inline void phonet_proto_put(struct phonet_protocol *pp) 432 { 433 module_put(pp->prot->owner); 434 } 435 436 /* Module registration */ 437 static int __init phonet_init(void) 438 { 439 int err; 440 441 err = sock_register(&phonet_proto_family); 442 if (err) { 443 printk(KERN_ALERT 444 "phonet protocol family initialization failed\n"); 445 return err; 446 } 447 448 phonet_device_init(); 449 dev_add_pack(&phonet_packet_type); 450 phonet_netlink_register(); 451 phonet_sysctl_init(); 452 453 err = isi_register(); 454 if (err) 455 goto err; 456 return 0; 457 458 err: 459 phonet_sysctl_exit(); 460 sock_unregister(PF_PHONET); 461 dev_remove_pack(&phonet_packet_type); 462 phonet_device_exit(); 463 return err; 464 } 465 466 static void __exit phonet_exit(void) 467 { 468 isi_unregister(); 469 phonet_sysctl_exit(); 470 sock_unregister(PF_PHONET); 471 dev_remove_pack(&phonet_packet_type); 472 phonet_device_exit(); 473 } 474 475 module_init(phonet_init); 476 module_exit(phonet_exit); 477 MODULE_DESCRIPTION("Phonet protocol stack for Linux"); 478 MODULE_LICENSE("GPL"); 479 MODULE_ALIAS_NETPROTO(PF_PHONET); 480