14b07b3f6SRemi Denis-Courmont /* 24b07b3f6SRemi Denis-Courmont * File: af_phonet.c 34b07b3f6SRemi Denis-Courmont * 44b07b3f6SRemi Denis-Courmont * Phonet protocols family 54b07b3f6SRemi Denis-Courmont * 64b07b3f6SRemi Denis-Courmont * Copyright (C) 2008 Nokia Corporation. 74b07b3f6SRemi Denis-Courmont * 84b07b3f6SRemi Denis-Courmont * Contact: Remi Denis-Courmont <remi.denis-courmont@nokia.com> 94b07b3f6SRemi Denis-Courmont * Original author: Sakari Ailus <sakari.ailus@nokia.com> 104b07b3f6SRemi Denis-Courmont * 114b07b3f6SRemi Denis-Courmont * This program is free software; you can redistribute it and/or 124b07b3f6SRemi Denis-Courmont * modify it under the terms of the GNU General Public License 134b07b3f6SRemi Denis-Courmont * version 2 as published by the Free Software Foundation. 144b07b3f6SRemi Denis-Courmont * 154b07b3f6SRemi Denis-Courmont * This program is distributed in the hope that it will be useful, but 164b07b3f6SRemi Denis-Courmont * WITHOUT ANY WARRANTY; without even the implied warranty of 174b07b3f6SRemi Denis-Courmont * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 184b07b3f6SRemi Denis-Courmont * General Public License for more details. 194b07b3f6SRemi Denis-Courmont * 204b07b3f6SRemi Denis-Courmont * You should have received a copy of the GNU General Public License 214b07b3f6SRemi Denis-Courmont * along with this program; if not, write to the Free Software 224b07b3f6SRemi Denis-Courmont * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 234b07b3f6SRemi Denis-Courmont * 02110-1301 USA 244b07b3f6SRemi Denis-Courmont */ 254b07b3f6SRemi Denis-Courmont 264b07b3f6SRemi Denis-Courmont #include <linux/kernel.h> 274b07b3f6SRemi Denis-Courmont #include <linux/module.h> 285a0e3ad6STejun Heo #include <linux/slab.h> 294b07b3f6SRemi Denis-Courmont #include <asm/unaligned.h> 304b07b3f6SRemi Denis-Courmont #include <net/sock.h> 314b07b3f6SRemi Denis-Courmont 324b07b3f6SRemi Denis-Courmont #include <linux/if_phonet.h> 334b07b3f6SRemi Denis-Courmont #include <linux/phonet.h> 344b07b3f6SRemi Denis-Courmont #include <net/phonet/phonet.h> 35f8ff6028SRemi Denis-Courmont #include <net/phonet/pn_dev.h> 364b07b3f6SRemi Denis-Courmont 37566521d6SAlexey Dobriyan /* Transport protocol registration */ 38566521d6SAlexey Dobriyan static struct phonet_protocol *proto_tab[PHONET_NPROTO] __read_mostly; 39566521d6SAlexey Dobriyan 40facb4edcSDan Carpenter static struct phonet_protocol *phonet_proto_get(unsigned int protocol) 41566521d6SAlexey Dobriyan { 42566521d6SAlexey Dobriyan struct phonet_protocol *pp; 43566521d6SAlexey Dobriyan 44566521d6SAlexey Dobriyan if (protocol >= PHONET_NPROTO) 45566521d6SAlexey Dobriyan return NULL; 46566521d6SAlexey Dobriyan 477ed0132fSRémi Denis-Courmont rcu_read_lock(); 48b2a5decdSRémi Denis-Courmont pp = rcu_dereference(proto_tab[protocol]); 49566521d6SAlexey Dobriyan if (pp && !try_module_get(pp->prot->owner)) 50566521d6SAlexey Dobriyan pp = NULL; 517ed0132fSRémi Denis-Courmont rcu_read_unlock(); 52566521d6SAlexey Dobriyan 53566521d6SAlexey Dobriyan return pp; 54566521d6SAlexey Dobriyan } 55566521d6SAlexey Dobriyan 56566521d6SAlexey Dobriyan static inline void phonet_proto_put(struct phonet_protocol *pp) 57566521d6SAlexey Dobriyan { 58566521d6SAlexey Dobriyan module_put(pp->prot->owner); 59566521d6SAlexey Dobriyan } 604b07b3f6SRemi Denis-Courmont 614b07b3f6SRemi Denis-Courmont /* protocol family functions */ 624b07b3f6SRemi Denis-Courmont 633f378b68SEric Paris static int pn_socket_create(struct net *net, struct socket *sock, int protocol, 643f378b68SEric Paris int kern) 654b07b3f6SRemi Denis-Courmont { 66ba113a94SRemi Denis-Courmont struct sock *sk; 67ba113a94SRemi Denis-Courmont struct pn_sock *pn; 684b07b3f6SRemi Denis-Courmont struct phonet_protocol *pnp; 694b07b3f6SRemi Denis-Courmont int err; 704b07b3f6SRemi Denis-Courmont 714b07b3f6SRemi Denis-Courmont if (!capable(CAP_SYS_ADMIN)) 724b07b3f6SRemi Denis-Courmont return -EPERM; 734b07b3f6SRemi Denis-Courmont 744b07b3f6SRemi Denis-Courmont if (protocol == 0) { 754b07b3f6SRemi Denis-Courmont /* Default protocol selection */ 764b07b3f6SRemi Denis-Courmont switch (sock->type) { 774b07b3f6SRemi Denis-Courmont case SOCK_DGRAM: 784b07b3f6SRemi Denis-Courmont protocol = PN_PROTO_PHONET; 794b07b3f6SRemi Denis-Courmont break; 809641458dSRémi Denis-Courmont case SOCK_SEQPACKET: 819641458dSRémi Denis-Courmont protocol = PN_PROTO_PIPE; 829641458dSRémi Denis-Courmont break; 834b07b3f6SRemi Denis-Courmont default: 844b07b3f6SRemi Denis-Courmont return -EPROTONOSUPPORT; 854b07b3f6SRemi Denis-Courmont } 864b07b3f6SRemi Denis-Courmont } 874b07b3f6SRemi Denis-Courmont 884b07b3f6SRemi Denis-Courmont pnp = phonet_proto_get(protocol); 8925532824SRémi Denis-Courmont if (pnp == NULL && 9025532824SRémi Denis-Courmont request_module("net-pf-%d-proto-%d", PF_PHONET, protocol) == 0) 9125532824SRémi Denis-Courmont pnp = phonet_proto_get(protocol); 9295a5afcaSJohannes Berg 934b07b3f6SRemi Denis-Courmont if (pnp == NULL) 944b07b3f6SRemi Denis-Courmont return -EPROTONOSUPPORT; 954b07b3f6SRemi Denis-Courmont if (sock->type != pnp->sock_type) { 964b07b3f6SRemi Denis-Courmont err = -EPROTONOSUPPORT; 974b07b3f6SRemi Denis-Courmont goto out; 984b07b3f6SRemi Denis-Courmont } 994b07b3f6SRemi Denis-Courmont 100ba113a94SRemi Denis-Courmont sk = sk_alloc(net, PF_PHONET, GFP_KERNEL, pnp->prot); 101ba113a94SRemi Denis-Courmont if (sk == NULL) { 102ba113a94SRemi Denis-Courmont err = -ENOMEM; 103ba113a94SRemi Denis-Courmont goto out; 104ba113a94SRemi Denis-Courmont } 105ba113a94SRemi Denis-Courmont 106ba113a94SRemi Denis-Courmont sock_init_data(sock, sk); 107ba113a94SRemi Denis-Courmont sock->state = SS_UNCONNECTED; 108ba113a94SRemi Denis-Courmont sock->ops = pnp->ops; 109ba113a94SRemi Denis-Courmont sk->sk_backlog_rcv = sk->sk_prot->backlog_rcv; 110ba113a94SRemi Denis-Courmont sk->sk_protocol = protocol; 111ba113a94SRemi Denis-Courmont pn = pn_sk(sk); 112ba113a94SRemi Denis-Courmont pn->sobject = 0; 113a8059512SRémi Denis-Courmont pn->dobject = 0; 114ba113a94SRemi Denis-Courmont pn->resource = 0; 115ba113a94SRemi Denis-Courmont sk->sk_prot->init(sk); 116ba113a94SRemi Denis-Courmont err = 0; 1174b07b3f6SRemi Denis-Courmont 1184b07b3f6SRemi Denis-Courmont out: 1194b07b3f6SRemi Denis-Courmont phonet_proto_put(pnp); 1204b07b3f6SRemi Denis-Courmont return err; 1214b07b3f6SRemi Denis-Courmont } 1224b07b3f6SRemi Denis-Courmont 123ec1b4cf7SStephen Hemminger static const struct net_proto_family phonet_proto_family = { 12425532824SRémi Denis-Courmont .family = PF_PHONET, 1254b07b3f6SRemi Denis-Courmont .create = pn_socket_create, 1264b07b3f6SRemi Denis-Courmont .owner = THIS_MODULE, 1274b07b3f6SRemi Denis-Courmont }; 1284b07b3f6SRemi Denis-Courmont 1295f77076dSRemi Denis-Courmont /* Phonet device header operations */ 1305f77076dSRemi Denis-Courmont static int pn_header_create(struct sk_buff *skb, struct net_device *dev, 1315f77076dSRemi Denis-Courmont unsigned short type, const void *daddr, 1325f77076dSRemi Denis-Courmont const void *saddr, unsigned len) 1335f77076dSRemi Denis-Courmont { 1345f77076dSRemi Denis-Courmont u8 *media = skb_push(skb, 1); 1355f77076dSRemi Denis-Courmont 1365f77076dSRemi Denis-Courmont if (type != ETH_P_PHONET) 1375f77076dSRemi Denis-Courmont return -1; 1385f77076dSRemi Denis-Courmont 1395f77076dSRemi Denis-Courmont if (!saddr) 1405f77076dSRemi Denis-Courmont saddr = dev->dev_addr; 1415f77076dSRemi Denis-Courmont *media = *(const u8 *)saddr; 1425f77076dSRemi Denis-Courmont return 1; 1435f77076dSRemi Denis-Courmont } 1445f77076dSRemi Denis-Courmont 1455f77076dSRemi Denis-Courmont static int pn_header_parse(const struct sk_buff *skb, unsigned char *haddr) 1465f77076dSRemi Denis-Courmont { 1475f77076dSRemi Denis-Courmont const u8 *media = skb_mac_header(skb); 1485f77076dSRemi Denis-Courmont *haddr = *media; 1495f77076dSRemi Denis-Courmont return 1; 1505f77076dSRemi Denis-Courmont } 1515f77076dSRemi Denis-Courmont 1525f77076dSRemi Denis-Courmont struct header_ops phonet_header_ops = { 1535f77076dSRemi Denis-Courmont .create = pn_header_create, 1545f77076dSRemi Denis-Courmont .parse = pn_header_parse, 1555f77076dSRemi Denis-Courmont }; 1565f77076dSRemi Denis-Courmont EXPORT_SYMBOL(phonet_header_ops); 1575f77076dSRemi Denis-Courmont 158107d0d9bSRemi Denis-Courmont /* 159107d0d9bSRemi Denis-Courmont * Prepends an ISI header and sends a datagram. 160107d0d9bSRemi Denis-Courmont */ 161107d0d9bSRemi Denis-Courmont static int pn_send(struct sk_buff *skb, struct net_device *dev, 162be0c52bfSRemi Denis-Courmont u16 dst, u16 src, u8 res, u8 irq) 163107d0d9bSRemi Denis-Courmont { 164107d0d9bSRemi Denis-Courmont struct phonethdr *ph; 165107d0d9bSRemi Denis-Courmont int err; 166107d0d9bSRemi Denis-Courmont 167ebfe92caSRémi Denis-Courmont if (skb->len + 2 > 0xffff /* Phonet length field limit */ || 168ebfe92caSRémi Denis-Courmont skb->len + sizeof(struct phonethdr) > dev->mtu) { 169107d0d9bSRemi Denis-Courmont err = -EMSGSIZE; 170107d0d9bSRemi Denis-Courmont goto drop; 171107d0d9bSRemi Denis-Courmont } 172107d0d9bSRemi Denis-Courmont 17318a1166dSRémi Denis-Courmont /* Broadcast sending is not implemented */ 17418a1166dSRémi Denis-Courmont if (pn_addr(dst) == PNADDR_BROADCAST) { 17518a1166dSRémi Denis-Courmont err = -EOPNOTSUPP; 17618a1166dSRémi Denis-Courmont goto drop; 17718a1166dSRémi Denis-Courmont } 17818a1166dSRémi Denis-Courmont 179107d0d9bSRemi Denis-Courmont skb_reset_transport_header(skb); 180107d0d9bSRemi Denis-Courmont WARN_ON(skb_headroom(skb) & 1); /* HW assumes word alignment */ 181107d0d9bSRemi Denis-Courmont skb_push(skb, sizeof(struct phonethdr)); 182107d0d9bSRemi Denis-Courmont skb_reset_network_header(skb); 183107d0d9bSRemi Denis-Courmont ph = pn_hdr(skb); 184107d0d9bSRemi Denis-Courmont ph->pn_rdev = pn_dev(dst); 185107d0d9bSRemi Denis-Courmont ph->pn_sdev = pn_dev(src); 186107d0d9bSRemi Denis-Courmont ph->pn_res = res; 187107d0d9bSRemi Denis-Courmont ph->pn_length = __cpu_to_be16(skb->len + 2 - sizeof(*ph)); 188107d0d9bSRemi Denis-Courmont ph->pn_robj = pn_obj(dst); 189107d0d9bSRemi Denis-Courmont ph->pn_sobj = pn_obj(src); 190107d0d9bSRemi Denis-Courmont 191107d0d9bSRemi Denis-Courmont skb->protocol = htons(ETH_P_PHONET); 192107d0d9bSRemi Denis-Courmont skb->priority = 0; 193107d0d9bSRemi Denis-Courmont skb->dev = dev; 194107d0d9bSRemi Denis-Courmont 195aa6c45f3SRémi Denis-Courmont if (skb->pkt_type == PACKET_LOOPBACK) { 196107d0d9bSRemi Denis-Courmont skb_reset_mac_header(skb); 197107d0d9bSRemi Denis-Courmont skb_orphan(skb); 198be0c52bfSRemi Denis-Courmont if (irq) 199be0c52bfSRemi Denis-Courmont netif_rx(skb); 200be0c52bfSRemi Denis-Courmont else 201107d0d9bSRemi Denis-Courmont netif_rx_ni(skb); 202107d0d9bSRemi Denis-Courmont err = 0; 203107d0d9bSRemi Denis-Courmont } else { 204107d0d9bSRemi Denis-Courmont err = dev_hard_header(skb, dev, ntohs(skb->protocol), 205107d0d9bSRemi Denis-Courmont NULL, NULL, skb->len); 206107d0d9bSRemi Denis-Courmont if (err < 0) { 207107d0d9bSRemi Denis-Courmont err = -EHOSTUNREACH; 208107d0d9bSRemi Denis-Courmont goto drop; 209107d0d9bSRemi Denis-Courmont } 210107d0d9bSRemi Denis-Courmont err = dev_queue_xmit(skb); 211107d0d9bSRemi Denis-Courmont } 212107d0d9bSRemi Denis-Courmont 213107d0d9bSRemi Denis-Courmont return err; 214107d0d9bSRemi Denis-Courmont drop: 215107d0d9bSRemi Denis-Courmont kfree_skb(skb); 216107d0d9bSRemi Denis-Courmont return err; 217107d0d9bSRemi Denis-Courmont } 218107d0d9bSRemi Denis-Courmont 219be0c52bfSRemi Denis-Courmont static int pn_raw_send(const void *data, int len, struct net_device *dev, 220be0c52bfSRemi Denis-Courmont u16 dst, u16 src, u8 res) 221be0c52bfSRemi Denis-Courmont { 222be0c52bfSRemi Denis-Courmont struct sk_buff *skb = alloc_skb(MAX_PHONET_HEADER + len, GFP_ATOMIC); 223be0c52bfSRemi Denis-Courmont if (skb == NULL) 224be0c52bfSRemi Denis-Courmont return -ENOMEM; 225be0c52bfSRemi Denis-Courmont 226aa6c45f3SRémi Denis-Courmont if (phonet_address_lookup(dev_net(dev), pn_addr(dst)) == 0) 227aa6c45f3SRémi Denis-Courmont skb->pkt_type = PACKET_LOOPBACK; 228aa6c45f3SRémi Denis-Courmont 229be0c52bfSRemi Denis-Courmont skb_reserve(skb, MAX_PHONET_HEADER); 230be0c52bfSRemi Denis-Courmont __skb_put(skb, len); 231be0c52bfSRemi Denis-Courmont skb_copy_to_linear_data(skb, data, len); 232be0c52bfSRemi Denis-Courmont return pn_send(skb, dev, dst, src, res, 1); 233be0c52bfSRemi Denis-Courmont } 234be0c52bfSRemi Denis-Courmont 235107d0d9bSRemi Denis-Courmont /* 236107d0d9bSRemi Denis-Courmont * Create a Phonet header for the skb and send it out. Returns 237107d0d9bSRemi Denis-Courmont * non-zero error code if failed. The skb is freed then. 238107d0d9bSRemi Denis-Courmont */ 239107d0d9bSRemi Denis-Courmont int pn_skb_send(struct sock *sk, struct sk_buff *skb, 240107d0d9bSRemi Denis-Courmont const struct sockaddr_pn *target) 241107d0d9bSRemi Denis-Courmont { 242aa6c45f3SRémi Denis-Courmont struct net *net = sock_net(sk); 243107d0d9bSRemi Denis-Courmont struct net_device *dev; 244107d0d9bSRemi Denis-Courmont struct pn_sock *pn = pn_sk(sk); 245107d0d9bSRemi Denis-Courmont int err; 246a8059512SRémi Denis-Courmont u16 src, dst; 247a8059512SRémi Denis-Courmont u8 daddr, saddr, res; 248a8059512SRémi Denis-Courmont 249a8059512SRémi Denis-Courmont src = pn->sobject; 250a8059512SRémi Denis-Courmont if (target != NULL) { 251a8059512SRémi Denis-Courmont dst = pn_sockaddr_get_object(target); 252a8059512SRémi Denis-Courmont res = pn_sockaddr_get_resource(target); 253a8059512SRémi Denis-Courmont } else { 254a8059512SRémi Denis-Courmont dst = pn->dobject; 255a8059512SRémi Denis-Courmont res = pn->resource; 256a8059512SRémi Denis-Courmont } 257a8059512SRémi Denis-Courmont daddr = pn_addr(dst); 258107d0d9bSRemi Denis-Courmont 259107d0d9bSRemi Denis-Courmont err = -EHOSTUNREACH; 260107d0d9bSRemi Denis-Courmont if (sk->sk_bound_dev_if) 261aa6c45f3SRémi Denis-Courmont dev = dev_get_by_index(net, sk->sk_bound_dev_if); 262aa6c45f3SRémi Denis-Courmont else if (phonet_address_lookup(net, daddr) == 0) { 263aa6c45f3SRémi Denis-Courmont dev = phonet_device_get(net); 264aa6c45f3SRémi Denis-Courmont skb->pkt_type = PACKET_LOOPBACK; 265c69d4407SRémi Denis-Courmont } else if (dst == 0) { 266b6a563b2SRémi Denis-Courmont /* Resource routing (small race until phonet_rcv()) */ 267c69d4407SRémi Denis-Courmont struct sock *sk = pn_find_sock_by_res(net, res); 268b6a563b2SRémi Denis-Courmont if (sk) { 269b6a563b2SRémi Denis-Courmont sock_put(sk); 270b6a563b2SRémi Denis-Courmont dev = phonet_device_get(net); 271b6a563b2SRémi Denis-Courmont skb->pkt_type = PACKET_LOOPBACK; 272b6a563b2SRémi Denis-Courmont } else 273b6a563b2SRémi Denis-Courmont dev = phonet_route_output(net, daddr); 274aa6c45f3SRémi Denis-Courmont } else 275aa6c45f3SRémi Denis-Courmont dev = phonet_route_output(net, daddr); 276aa6c45f3SRémi Denis-Courmont 277107d0d9bSRemi Denis-Courmont if (!dev || !(dev->flags & IFF_UP)) 278107d0d9bSRemi Denis-Courmont goto drop; 279107d0d9bSRemi Denis-Courmont 280107d0d9bSRemi Denis-Courmont saddr = phonet_address_get(dev, daddr); 281107d0d9bSRemi Denis-Courmont if (saddr == PN_NO_ADDR) 282107d0d9bSRemi Denis-Courmont goto drop; 283107d0d9bSRemi Denis-Courmont 284107d0d9bSRemi Denis-Courmont if (!pn_addr(src)) 285107d0d9bSRemi Denis-Courmont src = pn_object(saddr, pn_obj(src)); 286107d0d9bSRemi Denis-Courmont 287a8059512SRémi Denis-Courmont err = pn_send(skb, dev, dst, src, res, 0); 288107d0d9bSRemi Denis-Courmont dev_put(dev); 289107d0d9bSRemi Denis-Courmont return err; 290107d0d9bSRemi Denis-Courmont 291107d0d9bSRemi Denis-Courmont drop: 292107d0d9bSRemi Denis-Courmont kfree_skb(skb); 293107d0d9bSRemi Denis-Courmont if (dev) 294107d0d9bSRemi Denis-Courmont dev_put(dev); 295107d0d9bSRemi Denis-Courmont return err; 296107d0d9bSRemi Denis-Courmont } 297107d0d9bSRemi Denis-Courmont EXPORT_SYMBOL(pn_skb_send); 298107d0d9bSRemi Denis-Courmont 299be0c52bfSRemi Denis-Courmont /* Do not send an error message in response to an error message */ 300be0c52bfSRemi Denis-Courmont static inline int can_respond(struct sk_buff *skb) 301be0c52bfSRemi Denis-Courmont { 302be0c52bfSRemi Denis-Courmont const struct phonethdr *ph; 303be0c52bfSRemi Denis-Courmont const struct phonetmsg *pm; 304be0c52bfSRemi Denis-Courmont u8 submsg_id; 305be0c52bfSRemi Denis-Courmont 306be0c52bfSRemi Denis-Courmont if (!pskb_may_pull(skb, 3)) 307be0c52bfSRemi Denis-Courmont return 0; 308be0c52bfSRemi Denis-Courmont 309be0c52bfSRemi Denis-Courmont ph = pn_hdr(skb); 310be0c52bfSRemi Denis-Courmont if (ph->pn_res == PN_PREFIX && !pskb_may_pull(skb, 5)) 311be0c52bfSRemi Denis-Courmont return 0; 312c3a90c78SRemi Denis-Courmont if (ph->pn_res == PN_COMMGR) /* indications */ 313c3a90c78SRemi Denis-Courmont return 0; 314be0c52bfSRemi Denis-Courmont 315be0c52bfSRemi Denis-Courmont ph = pn_hdr(skb); /* re-acquires the pointer */ 316be0c52bfSRemi Denis-Courmont pm = pn_msg(skb); 317be0c52bfSRemi Denis-Courmont if (pm->pn_msg_id != PN_COMMON_MESSAGE) 318be0c52bfSRemi Denis-Courmont return 1; 319be0c52bfSRemi Denis-Courmont submsg_id = (ph->pn_res == PN_PREFIX) 320be0c52bfSRemi Denis-Courmont ? pm->pn_e_submsg_id : pm->pn_submsg_id; 321be0c52bfSRemi Denis-Courmont if (submsg_id != PN_COMM_ISA_ENTITY_NOT_REACHABLE_RESP && 322be0c52bfSRemi Denis-Courmont pm->pn_e_submsg_id != PN_COMM_SERVICE_NOT_IDENTIFIED_RESP) 323be0c52bfSRemi Denis-Courmont return 1; 324be0c52bfSRemi Denis-Courmont return 0; 325be0c52bfSRemi Denis-Courmont } 326be0c52bfSRemi Denis-Courmont 327be0c52bfSRemi Denis-Courmont static int send_obj_unreachable(struct sk_buff *rskb) 328be0c52bfSRemi Denis-Courmont { 329be0c52bfSRemi Denis-Courmont const struct phonethdr *oph = pn_hdr(rskb); 330be0c52bfSRemi Denis-Courmont const struct phonetmsg *opm = pn_msg(rskb); 331be0c52bfSRemi Denis-Courmont struct phonetmsg resp; 332be0c52bfSRemi Denis-Courmont 333be0c52bfSRemi Denis-Courmont memset(&resp, 0, sizeof(resp)); 334be0c52bfSRemi Denis-Courmont resp.pn_trans_id = opm->pn_trans_id; 335be0c52bfSRemi Denis-Courmont resp.pn_msg_id = PN_COMMON_MESSAGE; 336be0c52bfSRemi Denis-Courmont if (oph->pn_res == PN_PREFIX) { 337be0c52bfSRemi Denis-Courmont resp.pn_e_res_id = opm->pn_e_res_id; 338be0c52bfSRemi Denis-Courmont resp.pn_e_submsg_id = PN_COMM_ISA_ENTITY_NOT_REACHABLE_RESP; 339be0c52bfSRemi Denis-Courmont resp.pn_e_orig_msg_id = opm->pn_msg_id; 340be0c52bfSRemi Denis-Courmont resp.pn_e_status = 0; 341be0c52bfSRemi Denis-Courmont } else { 342be0c52bfSRemi Denis-Courmont resp.pn_submsg_id = PN_COMM_ISA_ENTITY_NOT_REACHABLE_RESP; 343be0c52bfSRemi Denis-Courmont resp.pn_orig_msg_id = opm->pn_msg_id; 344be0c52bfSRemi Denis-Courmont resp.pn_status = 0; 345be0c52bfSRemi Denis-Courmont } 346be0c52bfSRemi Denis-Courmont return pn_raw_send(&resp, sizeof(resp), rskb->dev, 347be0c52bfSRemi Denis-Courmont pn_object(oph->pn_sdev, oph->pn_sobj), 348be0c52bfSRemi Denis-Courmont pn_object(oph->pn_rdev, oph->pn_robj), 349be0c52bfSRemi Denis-Courmont oph->pn_res); 350be0c52bfSRemi Denis-Courmont } 351be0c52bfSRemi Denis-Courmont 352be0c52bfSRemi Denis-Courmont static int send_reset_indications(struct sk_buff *rskb) 353be0c52bfSRemi Denis-Courmont { 354be0c52bfSRemi Denis-Courmont struct phonethdr *oph = pn_hdr(rskb); 355be0c52bfSRemi Denis-Courmont static const u8 data[4] = { 356be0c52bfSRemi Denis-Courmont 0x00 /* trans ID */, 0x10 /* subscribe msg */, 357be0c52bfSRemi Denis-Courmont 0x00 /* subscription count */, 0x00 /* dummy */ 358be0c52bfSRemi Denis-Courmont }; 359be0c52bfSRemi Denis-Courmont 360be0c52bfSRemi Denis-Courmont return pn_raw_send(data, sizeof(data), rskb->dev, 361be0c52bfSRemi Denis-Courmont pn_object(oph->pn_sdev, 0x00), 362c3a90c78SRemi Denis-Courmont pn_object(oph->pn_rdev, oph->pn_robj), 363c3a90c78SRemi Denis-Courmont PN_COMMGR); 364be0c52bfSRemi Denis-Courmont } 365be0c52bfSRemi Denis-Courmont 366be0c52bfSRemi Denis-Courmont 3674b07b3f6SRemi Denis-Courmont /* packet type functions */ 3684b07b3f6SRemi Denis-Courmont 3694b07b3f6SRemi Denis-Courmont /* 3704b07b3f6SRemi Denis-Courmont * Stuff received packets to associated sockets. 3714b07b3f6SRemi Denis-Courmont * On error, returns non-zero and releases the skb. 3724b07b3f6SRemi Denis-Courmont */ 3734b07b3f6SRemi Denis-Courmont static int phonet_rcv(struct sk_buff *skb, struct net_device *dev, 3744b07b3f6SRemi Denis-Courmont struct packet_type *pkttype, 3754b07b3f6SRemi Denis-Courmont struct net_device *orig_dev) 3764b07b3f6SRemi Denis-Courmont { 3774b8f704bSremi.denis-courmont@nokia struct net *net = dev_net(dev); 3784b07b3f6SRemi Denis-Courmont struct phonethdr *ph; 3794b07b3f6SRemi Denis-Courmont struct sockaddr_pn sa; 3804b07b3f6SRemi Denis-Courmont u16 len; 3814b07b3f6SRemi Denis-Courmont 3824b07b3f6SRemi Denis-Courmont /* check we have at least a full Phonet header */ 3834b07b3f6SRemi Denis-Courmont if (!pskb_pull(skb, sizeof(struct phonethdr))) 3844b07b3f6SRemi Denis-Courmont goto out; 3854b07b3f6SRemi Denis-Courmont 3864b07b3f6SRemi Denis-Courmont /* check that the advertised length is correct */ 3874b07b3f6SRemi Denis-Courmont ph = pn_hdr(skb); 3884b07b3f6SRemi Denis-Courmont len = get_unaligned_be16(&ph->pn_length); 3894b07b3f6SRemi Denis-Courmont if (len < 2) 3904b07b3f6SRemi Denis-Courmont goto out; 3914b07b3f6SRemi Denis-Courmont len -= 2; 3924b07b3f6SRemi Denis-Courmont if ((len > skb->len) || pskb_trim(skb, len)) 3934b07b3f6SRemi Denis-Courmont goto out; 3944b07b3f6SRemi Denis-Courmont skb_reset_transport_header(skb); 3954b07b3f6SRemi Denis-Courmont 3964b07b3f6SRemi Denis-Courmont pn_skb_get_dst_sockaddr(skb, &sa); 3974b07b3f6SRemi Denis-Courmont 398f14001fcSRémi Denis-Courmont /* check if this is broadcasted */ 399f14001fcSRémi Denis-Courmont if (pn_sockaddr_get_addr(&sa) == PNADDR_BROADCAST) { 400f14001fcSRémi Denis-Courmont pn_deliver_sock_broadcast(net, skb); 401f14001fcSRémi Denis-Courmont goto out; 402f14001fcSRémi Denis-Courmont } 403f14001fcSRémi Denis-Courmont 404b6a563b2SRémi Denis-Courmont /* resource routing */ 405b6a563b2SRémi Denis-Courmont if (pn_sockaddr_get_object(&sa) == 0) { 406b6a563b2SRémi Denis-Courmont struct sock *sk = pn_find_sock_by_res(net, sa.spn_resource); 407b6a563b2SRémi Denis-Courmont if (sk) 408b6a563b2SRémi Denis-Courmont return sk_receive_skb(sk, skb, 0); 409b6a563b2SRémi Denis-Courmont } 410b6a563b2SRémi Denis-Courmont 4114b8f704bSremi.denis-courmont@nokia /* check if we are the destination */ 4124b8f704bSremi.denis-courmont@nokia if (phonet_address_lookup(net, pn_sockaddr_get_addr(&sa)) == 0) { 4134b8f704bSremi.denis-courmont@nokia /* Phonet packet input */ 4144b8f704bSremi.denis-courmont@nokia struct sock *sk = pn_find_sock_by_sa(net, &sa); 4154b8f704bSremi.denis-courmont@nokia 4164b8f704bSremi.denis-courmont@nokia if (sk) 4174b8f704bSremi.denis-courmont@nokia return sk_receive_skb(sk, skb, 0); 4184b8f704bSremi.denis-courmont@nokia 419be0c52bfSRemi Denis-Courmont if (can_respond(skb)) { 420be0c52bfSRemi Denis-Courmont send_obj_unreachable(skb); 421be0c52bfSRemi Denis-Courmont send_reset_indications(skb); 422be0c52bfSRemi Denis-Courmont } 42386a0a1e5SRémi Denis-Courmont } else if (unlikely(skb->pkt_type == PACKET_LOOPBACK)) 42486a0a1e5SRémi Denis-Courmont goto out; /* Race between address deletion and loopback */ 42586a0a1e5SRémi Denis-Courmont else { 42686a0a1e5SRémi Denis-Courmont /* Phonet packet routing */ 42786a0a1e5SRémi Denis-Courmont struct net_device *out_dev; 42886a0a1e5SRémi Denis-Courmont 42986a0a1e5SRémi Denis-Courmont out_dev = phonet_route_output(net, pn_sockaddr_get_addr(&sa)); 43086a0a1e5SRémi Denis-Courmont if (!out_dev) { 43186a0a1e5SRémi Denis-Courmont LIMIT_NETDEBUG(KERN_WARNING"No Phonet route to %02X\n", 43286a0a1e5SRémi Denis-Courmont pn_sockaddr_get_addr(&sa)); 43386a0a1e5SRémi Denis-Courmont goto out; 43486a0a1e5SRémi Denis-Courmont } 43586a0a1e5SRémi Denis-Courmont 43686a0a1e5SRémi Denis-Courmont __skb_push(skb, sizeof(struct phonethdr)); 43786a0a1e5SRémi Denis-Courmont skb->dev = out_dev; 43886a0a1e5SRémi Denis-Courmont if (out_dev == dev) { 43986a0a1e5SRémi Denis-Courmont LIMIT_NETDEBUG(KERN_ERR"Phonet loop to %02X on %s\n", 44086a0a1e5SRémi Denis-Courmont pn_sockaddr_get_addr(&sa), dev->name); 44186a0a1e5SRémi Denis-Courmont goto out_dev; 44286a0a1e5SRémi Denis-Courmont } 44386a0a1e5SRémi Denis-Courmont /* Some drivers (e.g. TUN) do not allocate HW header space */ 44486a0a1e5SRémi Denis-Courmont if (skb_cow_head(skb, out_dev->hard_header_len)) 44586a0a1e5SRémi Denis-Courmont goto out_dev; 44686a0a1e5SRémi Denis-Courmont 44786a0a1e5SRémi Denis-Courmont if (dev_hard_header(skb, out_dev, ETH_P_PHONET, NULL, NULL, 44886a0a1e5SRémi Denis-Courmont skb->len) < 0) 44986a0a1e5SRémi Denis-Courmont goto out_dev; 45086a0a1e5SRémi Denis-Courmont dev_queue_xmit(skb); 45186a0a1e5SRémi Denis-Courmont dev_put(out_dev); 45286a0a1e5SRémi Denis-Courmont return NET_RX_SUCCESS; 45386a0a1e5SRémi Denis-Courmont out_dev: 45486a0a1e5SRémi Denis-Courmont dev_put(out_dev); 455be0c52bfSRemi Denis-Courmont } 456ba113a94SRemi Denis-Courmont 4574b07b3f6SRemi Denis-Courmont out: 4584b07b3f6SRemi Denis-Courmont kfree_skb(skb); 4594b07b3f6SRemi Denis-Courmont return NET_RX_DROP; 4604b07b3f6SRemi Denis-Courmont } 4614b07b3f6SRemi Denis-Courmont 4627546dd97SStephen Hemminger static struct packet_type phonet_packet_type __read_mostly = { 46309640e63SHarvey Harrison .type = cpu_to_be16(ETH_P_PHONET), 4644b07b3f6SRemi Denis-Courmont .func = phonet_rcv, 4654b07b3f6SRemi Denis-Courmont }; 4664b07b3f6SRemi Denis-Courmont 4677ed0132fSRémi Denis-Courmont static DEFINE_MUTEX(proto_tab_lock); 4687ed0132fSRémi Denis-Courmont 469facb4edcSDan Carpenter int __init_or_module phonet_proto_register(unsigned int protocol, 4704b07b3f6SRemi Denis-Courmont struct phonet_protocol *pp) 4714b07b3f6SRemi Denis-Courmont { 4724b07b3f6SRemi Denis-Courmont int err = 0; 4734b07b3f6SRemi Denis-Courmont 4744b07b3f6SRemi Denis-Courmont if (protocol >= PHONET_NPROTO) 4754b07b3f6SRemi Denis-Courmont return -EINVAL; 4764b07b3f6SRemi Denis-Courmont 4774b07b3f6SRemi Denis-Courmont err = proto_register(pp->prot, 1); 4784b07b3f6SRemi Denis-Courmont if (err) 4794b07b3f6SRemi Denis-Courmont return err; 4804b07b3f6SRemi Denis-Courmont 4817ed0132fSRémi Denis-Courmont mutex_lock(&proto_tab_lock); 4824b07b3f6SRemi Denis-Courmont if (proto_tab[protocol]) 4834b07b3f6SRemi Denis-Courmont err = -EBUSY; 4844b07b3f6SRemi Denis-Courmont else 4857ed0132fSRémi Denis-Courmont rcu_assign_pointer(proto_tab[protocol], pp); 4867ed0132fSRémi Denis-Courmont mutex_unlock(&proto_tab_lock); 4874b07b3f6SRemi Denis-Courmont 4884b07b3f6SRemi Denis-Courmont return err; 4894b07b3f6SRemi Denis-Courmont } 4904b07b3f6SRemi Denis-Courmont EXPORT_SYMBOL(phonet_proto_register); 4914b07b3f6SRemi Denis-Courmont 492facb4edcSDan Carpenter void phonet_proto_unregister(unsigned int protocol, struct phonet_protocol *pp) 4934b07b3f6SRemi Denis-Courmont { 4947ed0132fSRémi Denis-Courmont mutex_lock(&proto_tab_lock); 4954b07b3f6SRemi Denis-Courmont BUG_ON(proto_tab[protocol] != pp); 4967ed0132fSRémi Denis-Courmont rcu_assign_pointer(proto_tab[protocol], NULL); 4977ed0132fSRémi Denis-Courmont mutex_unlock(&proto_tab_lock); 4987ed0132fSRémi Denis-Courmont synchronize_rcu(); 4994b07b3f6SRemi Denis-Courmont proto_unregister(pp->prot); 5004b07b3f6SRemi Denis-Courmont } 5014b07b3f6SRemi Denis-Courmont EXPORT_SYMBOL(phonet_proto_unregister); 5024b07b3f6SRemi Denis-Courmont 5034b07b3f6SRemi Denis-Courmont /* Module registration */ 5044b07b3f6SRemi Denis-Courmont static int __init phonet_init(void) 5054b07b3f6SRemi Denis-Courmont { 5064b07b3f6SRemi Denis-Courmont int err; 5074b07b3f6SRemi Denis-Courmont 50876e02cf6Sremi.denis-courmont@nokia err = phonet_device_init(); 50976e02cf6Sremi.denis-courmont@nokia if (err) 51076e02cf6Sremi.denis-courmont@nokia return err; 51176e02cf6Sremi.denis-courmont@nokia 5126b0d07baSRémi Denis-Courmont pn_sock_init(); 5134b07b3f6SRemi Denis-Courmont err = sock_register(&phonet_proto_family); 5144b07b3f6SRemi Denis-Courmont if (err) { 5154b07b3f6SRemi Denis-Courmont printk(KERN_ALERT 5164b07b3f6SRemi Denis-Courmont "phonet protocol family initialization failed\n"); 51776e02cf6Sremi.denis-courmont@nokia goto err_sock; 5184b07b3f6SRemi Denis-Courmont } 5194b07b3f6SRemi Denis-Courmont 5204b07b3f6SRemi Denis-Courmont dev_add_pack(&phonet_packet_type); 52187ab4e20SRemi Denis-Courmont phonet_sysctl_init(); 522107d0d9bSRemi Denis-Courmont 523107d0d9bSRemi Denis-Courmont err = isi_register(); 524107d0d9bSRemi Denis-Courmont if (err) 525107d0d9bSRemi Denis-Courmont goto err; 5264b07b3f6SRemi Denis-Courmont return 0; 527107d0d9bSRemi Denis-Courmont 528107d0d9bSRemi Denis-Courmont err: 52987ab4e20SRemi Denis-Courmont phonet_sysctl_exit(); 53025532824SRémi Denis-Courmont sock_unregister(PF_PHONET); 531107d0d9bSRemi Denis-Courmont dev_remove_pack(&phonet_packet_type); 53276e02cf6Sremi.denis-courmont@nokia err_sock: 533107d0d9bSRemi Denis-Courmont phonet_device_exit(); 534107d0d9bSRemi Denis-Courmont return err; 5354b07b3f6SRemi Denis-Courmont } 5364b07b3f6SRemi Denis-Courmont 5374b07b3f6SRemi Denis-Courmont static void __exit phonet_exit(void) 5384b07b3f6SRemi Denis-Courmont { 539107d0d9bSRemi Denis-Courmont isi_unregister(); 54087ab4e20SRemi Denis-Courmont phonet_sysctl_exit(); 54125532824SRémi Denis-Courmont sock_unregister(PF_PHONET); 5424b07b3f6SRemi Denis-Courmont dev_remove_pack(&phonet_packet_type); 543f8ff6028SRemi Denis-Courmont phonet_device_exit(); 5444b07b3f6SRemi Denis-Courmont } 5454b07b3f6SRemi Denis-Courmont 5464b07b3f6SRemi Denis-Courmont module_init(phonet_init); 5474b07b3f6SRemi Denis-Courmont module_exit(phonet_exit); 5484b07b3f6SRemi Denis-Courmont MODULE_DESCRIPTION("Phonet protocol stack for Linux"); 5494b07b3f6SRemi Denis-Courmont MODULE_LICENSE("GPL"); 55025532824SRémi Denis-Courmont MODULE_ALIAS_NETPROTO(PF_PHONET); 551