1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* |
3 | * File: af_phonet.c |
4 | * |
5 | * Phonet protocols family |
6 | * |
7 | * Copyright (C) 2008 Nokia Corporation. |
8 | * |
9 | * Authors: Sakari Ailus <sakari.ailus@nokia.com> |
10 | * RĂ©mi Denis-Courmont |
11 | */ |
12 | |
13 | #include <linux/kernel.h> |
14 | #include <linux/module.h> |
15 | #include <linux/slab.h> |
16 | #include <asm/unaligned.h> |
17 | #include <net/sock.h> |
18 | |
19 | #include <linux/if_phonet.h> |
20 | #include <linux/phonet.h> |
21 | #include <net/phonet/phonet.h> |
22 | #include <net/phonet/pn_dev.h> |
23 | |
24 | /* Transport protocol registration */ |
25 | static const struct phonet_protocol *proto_tab[PHONET_NPROTO] __read_mostly; |
26 | |
27 | static const struct phonet_protocol *phonet_proto_get(unsigned int protocol) |
28 | { |
29 | const struct phonet_protocol *pp; |
30 | |
31 | if (protocol >= PHONET_NPROTO) |
32 | return NULL; |
33 | |
34 | rcu_read_lock(); |
35 | pp = rcu_dereference(proto_tab[protocol]); |
36 | if (pp && !try_module_get(module: pp->prot->owner)) |
37 | pp = NULL; |
38 | rcu_read_unlock(); |
39 | |
40 | return pp; |
41 | } |
42 | |
43 | static inline void phonet_proto_put(const struct phonet_protocol *pp) |
44 | { |
45 | module_put(module: pp->prot->owner); |
46 | } |
47 | |
48 | /* protocol family functions */ |
49 | |
50 | static int pn_socket_create(struct net *net, struct socket *sock, int protocol, |
51 | int kern) |
52 | { |
53 | struct sock *sk; |
54 | struct pn_sock *pn; |
55 | const struct phonet_protocol *pnp; |
56 | int err; |
57 | |
58 | if (!capable(CAP_SYS_ADMIN)) |
59 | return -EPERM; |
60 | |
61 | if (protocol == 0) { |
62 | /* Default protocol selection */ |
63 | switch (sock->type) { |
64 | case SOCK_DGRAM: |
65 | protocol = PN_PROTO_PHONET; |
66 | break; |
67 | case SOCK_SEQPACKET: |
68 | protocol = PN_PROTO_PIPE; |
69 | break; |
70 | default: |
71 | return -EPROTONOSUPPORT; |
72 | } |
73 | } |
74 | |
75 | pnp = phonet_proto_get(protocol); |
76 | if (pnp == NULL && |
77 | request_module("net-pf-%d-proto-%d" , PF_PHONET, protocol) == 0) |
78 | pnp = phonet_proto_get(protocol); |
79 | |
80 | if (pnp == NULL) |
81 | return -EPROTONOSUPPORT; |
82 | if (sock->type != pnp->sock_type) { |
83 | err = -EPROTONOSUPPORT; |
84 | goto out; |
85 | } |
86 | |
87 | sk = sk_alloc(net, PF_PHONET, GFP_KERNEL, prot: pnp->prot, kern); |
88 | if (sk == NULL) { |
89 | err = -ENOMEM; |
90 | goto out; |
91 | } |
92 | |
93 | sock_init_data(sock, sk); |
94 | sock->state = SS_UNCONNECTED; |
95 | sock->ops = pnp->ops; |
96 | sk->sk_backlog_rcv = sk->sk_prot->backlog_rcv; |
97 | sk->sk_protocol = protocol; |
98 | pn = pn_sk(sk); |
99 | pn->sobject = 0; |
100 | pn->dobject = 0; |
101 | pn->resource = 0; |
102 | sk->sk_prot->init(sk); |
103 | err = 0; |
104 | |
105 | out: |
106 | phonet_proto_put(pp: pnp); |
107 | return err; |
108 | } |
109 | |
110 | static const struct net_proto_family phonet_proto_family = { |
111 | .family = PF_PHONET, |
112 | .create = pn_socket_create, |
113 | .owner = THIS_MODULE, |
114 | }; |
115 | |
116 | /* Phonet device header operations */ |
117 | static int (struct sk_buff *skb, struct net_device *dev, |
118 | unsigned short type, const void *daddr, |
119 | const void *saddr, unsigned int len) |
120 | { |
121 | u8 *media = skb_push(skb, len: 1); |
122 | |
123 | if (type != ETH_P_PHONET) |
124 | return -1; |
125 | |
126 | if (!saddr) |
127 | saddr = dev->dev_addr; |
128 | *media = *(const u8 *)saddr; |
129 | return 1; |
130 | } |
131 | |
132 | static int (const struct sk_buff *skb, unsigned char *haddr) |
133 | { |
134 | const u8 *media = skb_mac_header(skb); |
135 | *haddr = *media; |
136 | return 1; |
137 | } |
138 | |
139 | const struct header_ops = { |
140 | .create = pn_header_create, |
141 | .parse = pn_header_parse, |
142 | }; |
143 | EXPORT_SYMBOL(phonet_header_ops); |
144 | |
145 | /* |
146 | * Prepends an ISI header and sends a datagram. |
147 | */ |
148 | static int pn_send(struct sk_buff *skb, struct net_device *dev, |
149 | u16 dst, u16 src, u8 res) |
150 | { |
151 | struct phonethdr *ph; |
152 | int err; |
153 | |
154 | if (skb->len + 2 > 0xffff /* Phonet length field limit */ || |
155 | skb->len + sizeof(struct phonethdr) > dev->mtu) { |
156 | err = -EMSGSIZE; |
157 | goto drop; |
158 | } |
159 | |
160 | /* Broadcast sending is not implemented */ |
161 | if (pn_addr(handle: dst) == PNADDR_BROADCAST) { |
162 | err = -EOPNOTSUPP; |
163 | goto drop; |
164 | } |
165 | |
166 | skb_reset_transport_header(skb); |
167 | WARN_ON(skb_headroom(skb) & 1); /* HW assumes word alignment */ |
168 | skb_push(skb, len: sizeof(struct phonethdr)); |
169 | skb_reset_network_header(skb); |
170 | ph = pn_hdr(skb); |
171 | ph->pn_rdev = pn_dev(handle: dst); |
172 | ph->pn_sdev = pn_dev(handle: src); |
173 | ph->pn_res = res; |
174 | ph->pn_length = __cpu_to_be16(skb->len + 2 - sizeof(*ph)); |
175 | ph->pn_robj = pn_obj(handle: dst); |
176 | ph->pn_sobj = pn_obj(handle: src); |
177 | |
178 | skb->protocol = htons(ETH_P_PHONET); |
179 | skb->priority = 0; |
180 | skb->dev = dev; |
181 | |
182 | if (skb->pkt_type == PACKET_LOOPBACK) { |
183 | skb_reset_mac_header(skb); |
184 | skb_orphan(skb); |
185 | err = netif_rx(skb) ? -ENOBUFS : 0; |
186 | } else { |
187 | err = dev_hard_header(skb, dev, ntohs(skb->protocol), |
188 | NULL, NULL, len: skb->len); |
189 | if (err < 0) { |
190 | err = -EHOSTUNREACH; |
191 | goto drop; |
192 | } |
193 | err = dev_queue_xmit(skb); |
194 | if (unlikely(err > 0)) |
195 | err = net_xmit_errno(err); |
196 | } |
197 | |
198 | return err; |
199 | drop: |
200 | kfree_skb(skb); |
201 | return err; |
202 | } |
203 | |
204 | static int pn_raw_send(const void *data, int len, struct net_device *dev, |
205 | u16 dst, u16 src, u8 res) |
206 | { |
207 | struct sk_buff *skb = alloc_skb(MAX_PHONET_HEADER + len, GFP_ATOMIC); |
208 | if (skb == NULL) |
209 | return -ENOMEM; |
210 | |
211 | if (phonet_address_lookup(net: dev_net(dev), addr: pn_addr(handle: dst)) == 0) |
212 | skb->pkt_type = PACKET_LOOPBACK; |
213 | |
214 | skb_reserve(skb, MAX_PHONET_HEADER); |
215 | __skb_put(skb, len); |
216 | skb_copy_to_linear_data(skb, from: data, len); |
217 | return pn_send(skb, dev, dst, src, res); |
218 | } |
219 | |
220 | /* |
221 | * Create a Phonet header for the skb and send it out. Returns |
222 | * non-zero error code if failed. The skb is freed then. |
223 | */ |
224 | int pn_skb_send(struct sock *sk, struct sk_buff *skb, |
225 | const struct sockaddr_pn *target) |
226 | { |
227 | struct net *net = sock_net(sk); |
228 | struct net_device *dev; |
229 | struct pn_sock *pn = pn_sk(sk); |
230 | int err; |
231 | u16 src, dst; |
232 | u8 daddr, saddr, res; |
233 | |
234 | src = pn->sobject; |
235 | if (target != NULL) { |
236 | dst = pn_sockaddr_get_object(spn: target); |
237 | res = pn_sockaddr_get_resource(spn: target); |
238 | } else { |
239 | dst = pn->dobject; |
240 | res = pn->resource; |
241 | } |
242 | daddr = pn_addr(handle: dst); |
243 | |
244 | err = -EHOSTUNREACH; |
245 | if (sk->sk_bound_dev_if) |
246 | dev = dev_get_by_index(net, ifindex: sk->sk_bound_dev_if); |
247 | else if (phonet_address_lookup(net, addr: daddr) == 0) { |
248 | dev = phonet_device_get(net); |
249 | skb->pkt_type = PACKET_LOOPBACK; |
250 | } else if (dst == 0) { |
251 | /* Resource routing (small race until phonet_rcv()) */ |
252 | struct sock *sk = pn_find_sock_by_res(net, res); |
253 | if (sk) { |
254 | sock_put(sk); |
255 | dev = phonet_device_get(net); |
256 | skb->pkt_type = PACKET_LOOPBACK; |
257 | } else |
258 | dev = phonet_route_output(net, daddr); |
259 | } else |
260 | dev = phonet_route_output(net, daddr); |
261 | |
262 | if (!dev || !(dev->flags & IFF_UP)) |
263 | goto drop; |
264 | |
265 | saddr = phonet_address_get(dev, addr: daddr); |
266 | if (saddr == PN_NO_ADDR) |
267 | goto drop; |
268 | |
269 | if (!pn_addr(handle: src)) |
270 | src = pn_object(addr: saddr, port: pn_obj(handle: src)); |
271 | |
272 | err = pn_send(skb, dev, dst, src, res); |
273 | dev_put(dev); |
274 | return err; |
275 | |
276 | drop: |
277 | kfree_skb(skb); |
278 | dev_put(dev); |
279 | return err; |
280 | } |
281 | EXPORT_SYMBOL(pn_skb_send); |
282 | |
283 | /* Do not send an error message in response to an error message */ |
284 | static inline int can_respond(struct sk_buff *skb) |
285 | { |
286 | const struct phonethdr *ph; |
287 | const struct phonetmsg *pm; |
288 | u8 submsg_id; |
289 | |
290 | if (!pskb_may_pull(skb, len: 3)) |
291 | return 0; |
292 | |
293 | ph = pn_hdr(skb); |
294 | if (ph->pn_res == PN_PREFIX && !pskb_may_pull(skb, len: 5)) |
295 | return 0; |
296 | if (ph->pn_res == PN_COMMGR) /* indications */ |
297 | return 0; |
298 | |
299 | ph = pn_hdr(skb); /* re-acquires the pointer */ |
300 | pm = pn_msg(skb); |
301 | if (pm->pn_msg_id != PN_COMMON_MESSAGE) |
302 | return 1; |
303 | submsg_id = (ph->pn_res == PN_PREFIX) |
304 | ? pm->pn_e_submsg_id : pm->pn_submsg_id; |
305 | if (submsg_id != PN_COMM_ISA_ENTITY_NOT_REACHABLE_RESP && |
306 | pm->pn_e_submsg_id != PN_COMM_SERVICE_NOT_IDENTIFIED_RESP) |
307 | return 1; |
308 | return 0; |
309 | } |
310 | |
311 | static int send_obj_unreachable(struct sk_buff *rskb) |
312 | { |
313 | const struct phonethdr *oph = pn_hdr(skb: rskb); |
314 | const struct phonetmsg *opm = pn_msg(skb: rskb); |
315 | struct phonetmsg resp; |
316 | |
317 | memset(&resp, 0, sizeof(resp)); |
318 | resp.pn_trans_id = opm->pn_trans_id; |
319 | resp.pn_msg_id = PN_COMMON_MESSAGE; |
320 | if (oph->pn_res == PN_PREFIX) { |
321 | resp.pn_e_res_id = opm->pn_e_res_id; |
322 | resp.pn_e_submsg_id = PN_COMM_ISA_ENTITY_NOT_REACHABLE_RESP; |
323 | resp.pn_e_orig_msg_id = opm->pn_msg_id; |
324 | resp.pn_e_status = 0; |
325 | } else { |
326 | resp.pn_submsg_id = PN_COMM_ISA_ENTITY_NOT_REACHABLE_RESP; |
327 | resp.pn_orig_msg_id = opm->pn_msg_id; |
328 | resp.pn_status = 0; |
329 | } |
330 | return pn_raw_send(data: &resp, len: sizeof(resp), dev: rskb->dev, |
331 | dst: pn_object(addr: oph->pn_sdev, port: oph->pn_sobj), |
332 | src: pn_object(addr: oph->pn_rdev, port: oph->pn_robj), |
333 | res: oph->pn_res); |
334 | } |
335 | |
336 | static int send_reset_indications(struct sk_buff *rskb) |
337 | { |
338 | struct phonethdr *oph = pn_hdr(skb: rskb); |
339 | static const u8 data[4] = { |
340 | 0x00 /* trans ID */, 0x10 /* subscribe msg */, |
341 | 0x00 /* subscription count */, 0x00 /* dummy */ |
342 | }; |
343 | |
344 | return pn_raw_send(data, len: sizeof(data), dev: rskb->dev, |
345 | dst: pn_object(addr: oph->pn_sdev, port: 0x00), |
346 | src: pn_object(addr: oph->pn_rdev, port: oph->pn_robj), |
347 | PN_COMMGR); |
348 | } |
349 | |
350 | |
351 | /* packet type functions */ |
352 | |
353 | /* |
354 | * Stuff received packets to associated sockets. |
355 | * On error, returns non-zero and releases the skb. |
356 | */ |
357 | static int phonet_rcv(struct sk_buff *skb, struct net_device *dev, |
358 | struct packet_type *pkttype, |
359 | struct net_device *orig_dev) |
360 | { |
361 | struct net *net = dev_net(dev); |
362 | struct phonethdr *ph; |
363 | struct sockaddr_pn sa; |
364 | u16 len; |
365 | |
366 | skb = skb_share_check(skb, GFP_ATOMIC); |
367 | if (!skb) |
368 | return NET_RX_DROP; |
369 | |
370 | /* check we have at least a full Phonet header */ |
371 | if (!pskb_pull(skb, len: sizeof(struct phonethdr))) |
372 | goto out; |
373 | |
374 | /* check that the advertised length is correct */ |
375 | ph = pn_hdr(skb); |
376 | len = get_unaligned_be16(p: &ph->pn_length); |
377 | if (len < 2) |
378 | goto out; |
379 | len -= 2; |
380 | if ((len > skb->len) || pskb_trim(skb, len)) |
381 | goto out; |
382 | skb_reset_transport_header(skb); |
383 | |
384 | pn_skb_get_dst_sockaddr(skb, sa: &sa); |
385 | |
386 | /* check if this is broadcasted */ |
387 | if (pn_sockaddr_get_addr(spn: &sa) == PNADDR_BROADCAST) { |
388 | pn_deliver_sock_broadcast(net, skb); |
389 | goto out; |
390 | } |
391 | |
392 | /* resource routing */ |
393 | if (pn_sockaddr_get_object(spn: &sa) == 0) { |
394 | struct sock *sk = pn_find_sock_by_res(net, res: sa.spn_resource); |
395 | if (sk) |
396 | return sk_receive_skb(sk, skb, nested: 0); |
397 | } |
398 | |
399 | /* check if we are the destination */ |
400 | if (phonet_address_lookup(net, addr: pn_sockaddr_get_addr(spn: &sa)) == 0) { |
401 | /* Phonet packet input */ |
402 | struct sock *sk = pn_find_sock_by_sa(net, sa: &sa); |
403 | |
404 | if (sk) |
405 | return sk_receive_skb(sk, skb, nested: 0); |
406 | |
407 | if (can_respond(skb)) { |
408 | send_obj_unreachable(rskb: skb); |
409 | send_reset_indications(rskb: skb); |
410 | } |
411 | } else if (unlikely(skb->pkt_type == PACKET_LOOPBACK)) |
412 | goto out; /* Race between address deletion and loopback */ |
413 | else { |
414 | /* Phonet packet routing */ |
415 | struct net_device *out_dev; |
416 | |
417 | out_dev = phonet_route_output(net, daddr: pn_sockaddr_get_addr(spn: &sa)); |
418 | if (!out_dev) { |
419 | net_dbg_ratelimited("No Phonet route to %02X\n" , |
420 | pn_sockaddr_get_addr(&sa)); |
421 | goto out; |
422 | } |
423 | |
424 | __skb_push(skb, len: sizeof(struct phonethdr)); |
425 | skb->dev = out_dev; |
426 | if (out_dev == dev) { |
427 | net_dbg_ratelimited("Phonet loop to %02X on %s\n" , |
428 | pn_sockaddr_get_addr(&sa), |
429 | dev->name); |
430 | goto out_dev; |
431 | } |
432 | /* Some drivers (e.g. TUN) do not allocate HW header space */ |
433 | if (skb_cow_head(skb, headroom: out_dev->hard_header_len)) |
434 | goto out_dev; |
435 | |
436 | if (dev_hard_header(skb, dev: out_dev, ETH_P_PHONET, NULL, NULL, |
437 | len: skb->len) < 0) |
438 | goto out_dev; |
439 | dev_queue_xmit(skb); |
440 | dev_put(dev: out_dev); |
441 | return NET_RX_SUCCESS; |
442 | out_dev: |
443 | dev_put(dev: out_dev); |
444 | } |
445 | |
446 | out: |
447 | kfree_skb(skb); |
448 | return NET_RX_DROP; |
449 | } |
450 | |
451 | static struct packet_type phonet_packet_type __read_mostly = { |
452 | .type = cpu_to_be16(ETH_P_PHONET), |
453 | .func = phonet_rcv, |
454 | }; |
455 | |
456 | static DEFINE_MUTEX(proto_tab_lock); |
457 | |
458 | int __init_or_module phonet_proto_register(unsigned int protocol, |
459 | const struct phonet_protocol *pp) |
460 | { |
461 | int err = 0; |
462 | |
463 | if (protocol >= PHONET_NPROTO) |
464 | return -EINVAL; |
465 | |
466 | err = proto_register(prot: pp->prot, alloc_slab: 1); |
467 | if (err) |
468 | return err; |
469 | |
470 | mutex_lock(&proto_tab_lock); |
471 | if (proto_tab[protocol]) |
472 | err = -EBUSY; |
473 | else |
474 | rcu_assign_pointer(proto_tab[protocol], pp); |
475 | mutex_unlock(lock: &proto_tab_lock); |
476 | |
477 | return err; |
478 | } |
479 | EXPORT_SYMBOL(phonet_proto_register); |
480 | |
481 | void phonet_proto_unregister(unsigned int protocol, |
482 | const struct phonet_protocol *pp) |
483 | { |
484 | mutex_lock(&proto_tab_lock); |
485 | BUG_ON(proto_tab[protocol] != pp); |
486 | RCU_INIT_POINTER(proto_tab[protocol], NULL); |
487 | mutex_unlock(lock: &proto_tab_lock); |
488 | synchronize_rcu(); |
489 | proto_unregister(prot: pp->prot); |
490 | } |
491 | EXPORT_SYMBOL(phonet_proto_unregister); |
492 | |
493 | /* Module registration */ |
494 | static int __init phonet_init(void) |
495 | { |
496 | int err; |
497 | |
498 | err = phonet_device_init(); |
499 | if (err) |
500 | return err; |
501 | |
502 | pn_sock_init(); |
503 | err = sock_register(fam: &phonet_proto_family); |
504 | if (err) { |
505 | printk(KERN_ALERT |
506 | "phonet protocol family initialization failed\n" ); |
507 | goto err_sock; |
508 | } |
509 | |
510 | dev_add_pack(pt: &phonet_packet_type); |
511 | phonet_sysctl_init(); |
512 | |
513 | err = isi_register(); |
514 | if (err) |
515 | goto err; |
516 | return 0; |
517 | |
518 | err: |
519 | phonet_sysctl_exit(); |
520 | sock_unregister(PF_PHONET); |
521 | dev_remove_pack(pt: &phonet_packet_type); |
522 | err_sock: |
523 | phonet_device_exit(); |
524 | return err; |
525 | } |
526 | |
527 | static void __exit phonet_exit(void) |
528 | { |
529 | isi_unregister(); |
530 | phonet_sysctl_exit(); |
531 | sock_unregister(PF_PHONET); |
532 | dev_remove_pack(pt: &phonet_packet_type); |
533 | phonet_device_exit(); |
534 | } |
535 | |
536 | module_init(phonet_init); |
537 | module_exit(phonet_exit); |
538 | MODULE_DESCRIPTION("Phonet protocol stack for Linux" ); |
539 | MODULE_LICENSE("GPL" ); |
540 | MODULE_ALIAS_NETPROTO(PF_PHONET); |
541 | |