~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~

TOMOYO Linux Cross Reference
Linux/net/qrtr/qrtr.c

Version: ~ [ linux-5.15-rc7 ] ~ [ linux-5.14.14 ] ~ [ linux-5.13.19 ] ~ [ linux-5.12.19 ] ~ [ linux-5.11.22 ] ~ [ linux-5.10.75 ] ~ [ linux-5.9.16 ] ~ [ linux-5.8.18 ] ~ [ linux-5.7.19 ] ~ [ linux-5.6.19 ] ~ [ linux-5.5.19 ] ~ [ linux-5.4.155 ] ~ [ linux-5.3.18 ] ~ [ linux-5.2.21 ] ~ [ linux-5.1.21 ] ~ [ linux-5.0.21 ] ~ [ linux-4.20.17 ] ~ [ linux-4.19.213 ] ~ [ linux-4.18.20 ] ~ [ linux-4.17.19 ] ~ [ linux-4.16.18 ] ~ [ linux-4.15.18 ] ~ [ linux-4.14.252 ] ~ [ linux-4.13.16 ] ~ [ linux-4.12.14 ] ~ [ linux-4.11.12 ] ~ [ linux-4.10.17 ] ~ [ linux-4.9.287 ] ~ [ linux-4.8.17 ] ~ [ linux-4.7.10 ] ~ [ linux-4.6.7 ] ~ [ linux-4.5.7 ] ~ [ linux-4.4.289 ] ~ [ linux-4.3.6 ] ~ [ linux-4.2.8 ] ~ [ linux-4.1.52 ] ~ [ linux-4.0.9 ] ~ [ linux-3.18.140 ] ~ [ linux-3.16.85 ] ~ [ linux-3.14.79 ] ~ [ linux-3.12.74 ] ~ [ linux-3.10.108 ] ~ [ linux-2.6.32.71 ] ~ [ linux-2.6.0 ] ~ [ linux-2.4.37.11 ] ~ [ unix-v6-master ] ~ [ ccs-tools-1.8.5 ] ~ [ policy-sample ] ~
Architecture: ~ [ i386 ] ~ [ alpha ] ~ [ m68k ] ~ [ mips ] ~ [ ppc ] ~ [ sparc ] ~ [ sparc64 ] ~

  1 /*
  2  * Copyright (c) 2015, Sony Mobile Communications Inc.
  3  * Copyright (c) 2013, The Linux Foundation. All rights reserved.
  4  *
  5  * This program is free software; you can redistribute it and/or modify
  6  * it under the terms of the GNU General Public License version 2 and
  7  * only version 2 as published by the Free Software Foundation.
  8  *
  9  * This program is distributed in the hope that it will be useful,
 10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 12  * GNU General Public License for more details.
 13  */
 14 #include <linux/module.h>
 15 #include <linux/netlink.h>
 16 #include <linux/qrtr.h>
 17 #include <linux/termios.h>      /* For TIOCINQ/OUTQ */
 18 
 19 #include <net/sock.h>
 20 
 21 #include "qrtr.h"
 22 
 23 #define QRTR_PROTO_VER 1
 24 
 25 /* auto-bind range */
 26 #define QRTR_MIN_EPH_SOCKET 0x4000
 27 #define QRTR_MAX_EPH_SOCKET 0x7fff
 28 
 29 enum qrtr_pkt_type {
 30         QRTR_TYPE_DATA          = 1,
 31         QRTR_TYPE_HELLO         = 2,
 32         QRTR_TYPE_BYE           = 3,
 33         QRTR_TYPE_NEW_SERVER    = 4,
 34         QRTR_TYPE_DEL_SERVER    = 5,
 35         QRTR_TYPE_DEL_CLIENT    = 6,
 36         QRTR_TYPE_RESUME_TX     = 7,
 37         QRTR_TYPE_EXIT          = 8,
 38         QRTR_TYPE_PING          = 9,
 39 };
 40 
 41 /**
 42  * struct qrtr_hdr - (I|R)PCrouter packet header
 43  * @version: protocol version
 44  * @type: packet type; one of QRTR_TYPE_*
 45  * @src_node_id: source node
 46  * @src_port_id: source port
 47  * @confirm_rx: boolean; whether a resume-tx packet should be send in reply
 48  * @size: length of packet, excluding this header
 49  * @dst_node_id: destination node
 50  * @dst_port_id: destination port
 51  */
 52 struct qrtr_hdr {
 53         __le32 version;
 54         __le32 type;
 55         __le32 src_node_id;
 56         __le32 src_port_id;
 57         __le32 confirm_rx;
 58         __le32 size;
 59         __le32 dst_node_id;
 60         __le32 dst_port_id;
 61 } __packed;
 62 
 63 #define QRTR_HDR_SIZE sizeof(struct qrtr_hdr)
 64 #define QRTR_NODE_BCAST ((unsigned int)-1)
 65 #define QRTR_PORT_CTRL ((unsigned int)-2)
 66 
 67 struct qrtr_sock {
 68         /* WARNING: sk must be the first member */
 69         struct sock sk;
 70         struct sockaddr_qrtr us;
 71         struct sockaddr_qrtr peer;
 72 };
 73 
 74 static inline struct qrtr_sock *qrtr_sk(struct sock *sk)
 75 {
 76         BUILD_BUG_ON(offsetof(struct qrtr_sock, sk) != 0);
 77         return container_of(sk, struct qrtr_sock, sk);
 78 }
 79 
 80 static unsigned int qrtr_local_nid = -1;
 81 
 82 /* for node ids */
 83 static RADIX_TREE(qrtr_nodes, GFP_KERNEL);
 84 /* broadcast list */
 85 static LIST_HEAD(qrtr_all_nodes);
 86 /* lock for qrtr_nodes, qrtr_all_nodes and node reference */
 87 static DEFINE_MUTEX(qrtr_node_lock);
 88 
 89 /* local port allocation management */
 90 static DEFINE_IDR(qrtr_ports);
 91 static DEFINE_MUTEX(qrtr_port_lock);
 92 
 93 /**
 94  * struct qrtr_node - endpoint node
 95  * @ep_lock: lock for endpoint management and callbacks
 96  * @ep: endpoint
 97  * @ref: reference count for node
 98  * @nid: node id
 99  * @rx_queue: receive queue
100  * @work: scheduled work struct for recv work
101  * @item: list item for broadcast list
102  */
103 struct qrtr_node {
104         struct mutex ep_lock;
105         struct qrtr_endpoint *ep;
106         struct kref ref;
107         unsigned int nid;
108 
109         struct sk_buff_head rx_queue;
110         struct work_struct work;
111         struct list_head item;
112 };
113 
114 static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb);
115 static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb);
116 
117 /* Release node resources and free the node.
118  *
119  * Do not call directly, use qrtr_node_release.  To be used with
120  * kref_put_mutex.  As such, the node mutex is expected to be locked on call.
121  */
122 static void __qrtr_node_release(struct kref *kref)
123 {
124         struct qrtr_node *node = container_of(kref, struct qrtr_node, ref);
125 
126         if (node->nid != QRTR_EP_NID_AUTO)
127                 radix_tree_delete(&qrtr_nodes, node->nid);
128 
129         list_del(&node->item);
130         mutex_unlock(&qrtr_node_lock);
131 
132         skb_queue_purge(&node->rx_queue);
133         kfree(node);
134 }
135 
136 /* Increment reference to node. */
137 static struct qrtr_node *qrtr_node_acquire(struct qrtr_node *node)
138 {
139         if (node)
140                 kref_get(&node->ref);
141         return node;
142 }
143 
144 /* Decrement reference to node and release as necessary. */
145 static void qrtr_node_release(struct qrtr_node *node)
146 {
147         if (!node)
148                 return;
149         kref_put_mutex(&node->ref, __qrtr_node_release, &qrtr_node_lock);
150 }
151 
152 /* Pass an outgoing packet socket buffer to the endpoint driver. */
153 static int qrtr_node_enqueue(struct qrtr_node *node, struct sk_buff *skb)
154 {
155         int rc = -ENODEV;
156 
157         mutex_lock(&node->ep_lock);
158         if (node->ep)
159                 rc = node->ep->xmit(node->ep, skb);
160         else
161                 kfree_skb(skb);
162         mutex_unlock(&node->ep_lock);
163 
164         return rc;
165 }
166 
167 /* Lookup node by id.
168  *
169  * callers must release with qrtr_node_release()
170  */
171 static struct qrtr_node *qrtr_node_lookup(unsigned int nid)
172 {
173         struct qrtr_node *node;
174 
175         mutex_lock(&qrtr_node_lock);
176         node = radix_tree_lookup(&qrtr_nodes, nid);
177         node = qrtr_node_acquire(node);
178         mutex_unlock(&qrtr_node_lock);
179 
180         return node;
181 }
182 
183 /* Assign node id to node.
184  *
185  * This is mostly useful for automatic node id assignment, based on
186  * the source id in the incoming packet.
187  */
188 static void qrtr_node_assign(struct qrtr_node *node, unsigned int nid)
189 {
190         if (node->nid != QRTR_EP_NID_AUTO || nid == QRTR_EP_NID_AUTO)
191                 return;
192 
193         mutex_lock(&qrtr_node_lock);
194         radix_tree_insert(&qrtr_nodes, nid, node);
195         node->nid = nid;
196         mutex_unlock(&qrtr_node_lock);
197 }
198 
199 /**
200  * qrtr_endpoint_post() - post incoming data
201  * @ep: endpoint handle
202  * @data: data pointer
203  * @len: size of data in bytes
204  *
205  * Return: 0 on success; negative error code on failure
206  */
207 int qrtr_endpoint_post(struct qrtr_endpoint *ep, const void *data, size_t len)
208 {
209         struct qrtr_node *node = ep->node;
210         const struct qrtr_hdr *phdr = data;
211         struct sk_buff *skb;
212         unsigned int psize;
213         unsigned int size;
214         unsigned int type;
215         unsigned int ver;
216         unsigned int dst;
217 
218         if (len < QRTR_HDR_SIZE || len & 3)
219                 return -EINVAL;
220 
221         ver = le32_to_cpu(phdr->version);
222         size = le32_to_cpu(phdr->size);
223         type = le32_to_cpu(phdr->type);
224         dst = le32_to_cpu(phdr->dst_port_id);
225 
226         psize = (size + 3) & ~3;
227 
228         if (ver != QRTR_PROTO_VER)
229                 return -EINVAL;
230 
231         if (len != psize + QRTR_HDR_SIZE)
232                 return -EINVAL;
233 
234         if (dst != QRTR_PORT_CTRL && type != QRTR_TYPE_DATA)
235                 return -EINVAL;
236 
237         skb = netdev_alloc_skb(NULL, len);
238         if (!skb)
239                 return -ENOMEM;
240 
241         skb_reset_transport_header(skb);
242         skb_put_data(skb, data, len);
243 
244         skb_queue_tail(&node->rx_queue, skb);
245         schedule_work(&node->work);
246 
247         return 0;
248 }
249 EXPORT_SYMBOL_GPL(qrtr_endpoint_post);
250 
251 static struct sk_buff *qrtr_alloc_ctrl_packet(u32 type, size_t pkt_len,
252                                               u32 src_node, u32 dst_node)
253 {
254         struct qrtr_hdr *hdr;
255         struct sk_buff *skb;
256 
257         skb = alloc_skb(QRTR_HDR_SIZE + pkt_len, GFP_KERNEL);
258         if (!skb)
259                 return NULL;
260         skb_reset_transport_header(skb);
261 
262         hdr = skb_put(skb, QRTR_HDR_SIZE);
263         hdr->version = cpu_to_le32(QRTR_PROTO_VER);
264         hdr->type = cpu_to_le32(type);
265         hdr->src_node_id = cpu_to_le32(src_node);
266         hdr->src_port_id = cpu_to_le32(QRTR_PORT_CTRL);
267         hdr->confirm_rx = cpu_to_le32(0);
268         hdr->size = cpu_to_le32(pkt_len);
269         hdr->dst_node_id = cpu_to_le32(dst_node);
270         hdr->dst_port_id = cpu_to_le32(QRTR_PORT_CTRL);
271 
272         return skb;
273 }
274 
275 /* Allocate and construct a resume-tx packet. */
276 static struct sk_buff *qrtr_alloc_resume_tx(u32 src_node,
277                                             u32 dst_node, u32 port)
278 {
279         const int pkt_len = 20;
280         struct sk_buff *skb;
281         __le32 *buf;
282 
283         skb = qrtr_alloc_ctrl_packet(QRTR_TYPE_RESUME_TX, pkt_len,
284                                      src_node, dst_node);
285         if (!skb)
286                 return NULL;
287 
288         buf = skb_put_zero(skb, pkt_len);
289         buf[0] = cpu_to_le32(QRTR_TYPE_RESUME_TX);
290         buf[1] = cpu_to_le32(src_node);
291         buf[2] = cpu_to_le32(port);
292 
293         return skb;
294 }
295 
296 /* Allocate and construct a BYE message to signal remote termination */
297 static struct sk_buff *qrtr_alloc_local_bye(u32 src_node)
298 {
299         const int pkt_len = 20;
300         struct sk_buff *skb;
301         __le32 *buf;
302 
303         skb = qrtr_alloc_ctrl_packet(QRTR_TYPE_BYE, pkt_len,
304                                      src_node, qrtr_local_nid);
305         if (!skb)
306                 return NULL;
307 
308         buf = skb_put_zero(skb, pkt_len);
309         buf[0] = cpu_to_le32(QRTR_TYPE_BYE);
310 
311         return skb;
312 }
313 
314 static struct sk_buff *qrtr_alloc_del_client(struct sockaddr_qrtr *sq)
315 {
316         const int pkt_len = 20;
317         struct sk_buff *skb;
318         __le32 *buf;
319 
320         skb = qrtr_alloc_ctrl_packet(QRTR_TYPE_DEL_CLIENT, pkt_len,
321                                      sq->sq_node, QRTR_NODE_BCAST);
322         if (!skb)
323                 return NULL;
324 
325         buf = skb_put_zero(skb, pkt_len);
326         buf[0] = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);
327         buf[1] = cpu_to_le32(sq->sq_node);
328         buf[2] = cpu_to_le32(sq->sq_port);
329 
330         return skb;
331 }
332 
333 static struct qrtr_sock *qrtr_port_lookup(int port);
334 static void qrtr_port_put(struct qrtr_sock *ipc);
335 
336 /* Handle and route a received packet.
337  *
338  * This will auto-reply with resume-tx packet as necessary.
339  */
340 static void qrtr_node_rx_work(struct work_struct *work)
341 {
342         struct qrtr_node *node = container_of(work, struct qrtr_node, work);
343         struct sk_buff *skb;
344 
345         while ((skb = skb_dequeue(&node->rx_queue)) != NULL) {
346                 const struct qrtr_hdr *phdr;
347                 u32 dst_node, dst_port;
348                 struct qrtr_sock *ipc;
349                 u32 src_node;
350                 int confirm;
351 
352                 phdr = (const struct qrtr_hdr *)skb_transport_header(skb);
353                 src_node = le32_to_cpu(phdr->src_node_id);
354                 dst_node = le32_to_cpu(phdr->dst_node_id);
355                 dst_port = le32_to_cpu(phdr->dst_port_id);
356                 confirm = !!phdr->confirm_rx;
357 
358                 qrtr_node_assign(node, src_node);
359 
360                 ipc = qrtr_port_lookup(dst_port);
361                 if (!ipc) {
362                         kfree_skb(skb);
363                 } else {
364                         if (sock_queue_rcv_skb(&ipc->sk, skb))
365                                 kfree_skb(skb);
366 
367                         qrtr_port_put(ipc);
368                 }
369 
370                 if (confirm) {
371                         skb = qrtr_alloc_resume_tx(dst_node, node->nid, dst_port);
372                         if (!skb)
373                                 break;
374                         if (qrtr_node_enqueue(node, skb))
375                                 break;
376                 }
377         }
378 }
379 
380 /**
381  * qrtr_endpoint_register() - register a new endpoint
382  * @ep: endpoint to register
383  * @nid: desired node id; may be QRTR_EP_NID_AUTO for auto-assignment
384  * Return: 0 on success; negative error code on failure
385  *
386  * The specified endpoint must have the xmit function pointer set on call.
387  */
388 int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int nid)
389 {
390         struct qrtr_node *node;
391 
392         if (!ep || !ep->xmit)
393                 return -EINVAL;
394 
395         node = kzalloc(sizeof(*node), GFP_KERNEL);
396         if (!node)
397                 return -ENOMEM;
398 
399         INIT_WORK(&node->work, qrtr_node_rx_work);
400         kref_init(&node->ref);
401         mutex_init(&node->ep_lock);
402         skb_queue_head_init(&node->rx_queue);
403         node->nid = QRTR_EP_NID_AUTO;
404         node->ep = ep;
405 
406         qrtr_node_assign(node, nid);
407 
408         mutex_lock(&qrtr_node_lock);
409         list_add(&node->item, &qrtr_all_nodes);
410         mutex_unlock(&qrtr_node_lock);
411         ep->node = node;
412 
413         return 0;
414 }
415 EXPORT_SYMBOL_GPL(qrtr_endpoint_register);
416 
417 /**
418  * qrtr_endpoint_unregister - unregister endpoint
419  * @ep: endpoint to unregister
420  */
421 void qrtr_endpoint_unregister(struct qrtr_endpoint *ep)
422 {
423         struct qrtr_node *node = ep->node;
424         struct sk_buff *skb;
425 
426         mutex_lock(&node->ep_lock);
427         node->ep = NULL;
428         mutex_unlock(&node->ep_lock);
429 
430         /* Notify the local controller about the event */
431         skb = qrtr_alloc_local_bye(node->nid);
432         if (skb)
433                 qrtr_local_enqueue(NULL, skb);
434 
435         qrtr_node_release(node);
436         ep->node = NULL;
437 }
438 EXPORT_SYMBOL_GPL(qrtr_endpoint_unregister);
439 
440 /* Lookup socket by port.
441  *
442  * Callers must release with qrtr_port_put()
443  */
444 static struct qrtr_sock *qrtr_port_lookup(int port)
445 {
446         struct qrtr_sock *ipc;
447 
448         if (port == QRTR_PORT_CTRL)
449                 port = 0;
450 
451         mutex_lock(&qrtr_port_lock);
452         ipc = idr_find(&qrtr_ports, port);
453         if (ipc)
454                 sock_hold(&ipc->sk);
455         mutex_unlock(&qrtr_port_lock);
456 
457         return ipc;
458 }
459 
460 /* Release acquired socket. */
461 static void qrtr_port_put(struct qrtr_sock *ipc)
462 {
463         sock_put(&ipc->sk);
464 }
465 
466 /* Remove port assignment. */
467 static void qrtr_port_remove(struct qrtr_sock *ipc)
468 {
469         struct sk_buff *skb;
470         int port = ipc->us.sq_port;
471 
472         skb = qrtr_alloc_del_client(&ipc->us);
473         if (skb) {
474                 skb_set_owner_w(skb, &ipc->sk);
475                 qrtr_bcast_enqueue(NULL, skb);
476         }
477 
478         if (port == QRTR_PORT_CTRL)
479                 port = 0;
480 
481         __sock_put(&ipc->sk);
482 
483         mutex_lock(&qrtr_port_lock);
484         idr_remove(&qrtr_ports, port);
485         mutex_unlock(&qrtr_port_lock);
486 }
487 
488 /* Assign port number to socket.
489  *
490  * Specify port in the integer pointed to by port, and it will be adjusted
491  * on return as necesssary.
492  *
493  * Port may be:
494  *   0: Assign ephemeral port in [QRTR_MIN_EPH_SOCKET, QRTR_MAX_EPH_SOCKET]
495  *   <QRTR_MIN_EPH_SOCKET: Specified; requires CAP_NET_ADMIN
496  *   >QRTR_MIN_EPH_SOCKET: Specified; available to all
497  */
498 static int qrtr_port_assign(struct qrtr_sock *ipc, int *port)
499 {
500         int rc;
501 
502         mutex_lock(&qrtr_port_lock);
503         if (!*port) {
504                 rc = idr_alloc(&qrtr_ports, ipc,
505                                QRTR_MIN_EPH_SOCKET, QRTR_MAX_EPH_SOCKET + 1,
506                                GFP_ATOMIC);
507                 if (rc >= 0)
508                         *port = rc;
509         } else if (*port < QRTR_MIN_EPH_SOCKET && !capable(CAP_NET_ADMIN)) {
510                 rc = -EACCES;
511         } else if (*port == QRTR_PORT_CTRL) {
512                 rc = idr_alloc(&qrtr_ports, ipc, 0, 1, GFP_ATOMIC);
513         } else {
514                 rc = idr_alloc(&qrtr_ports, ipc, *port, *port + 1, GFP_ATOMIC);
515                 if (rc >= 0)
516                         *port = rc;
517         }
518         mutex_unlock(&qrtr_port_lock);
519 
520         if (rc == -ENOSPC)
521                 return -EADDRINUSE;
522         else if (rc < 0)
523                 return rc;
524 
525         sock_hold(&ipc->sk);
526 
527         return 0;
528 }
529 
530 /* Reset all non-control ports */
531 static void qrtr_reset_ports(void)
532 {
533         struct qrtr_sock *ipc;
534         int id;
535 
536         mutex_lock(&qrtr_port_lock);
537         idr_for_each_entry(&qrtr_ports, ipc, id) {
538                 /* Don't reset control port */
539                 if (id == 0)
540                         continue;
541 
542                 sock_hold(&ipc->sk);
543                 ipc->sk.sk_err = ENETRESET;
544                 wake_up_interruptible(sk_sleep(&ipc->sk));
545                 sock_put(&ipc->sk);
546         }
547         mutex_unlock(&qrtr_port_lock);
548 }
549 
550 /* Bind socket to address.
551  *
552  * Socket should be locked upon call.
553  */
554 static int __qrtr_bind(struct socket *sock,
555                        const struct sockaddr_qrtr *addr, int zapped)
556 {
557         struct qrtr_sock *ipc = qrtr_sk(sock->sk);
558         struct sock *sk = sock->sk;
559         int port;
560         int rc;
561 
562         /* rebinding ok */
563         if (!zapped && addr->sq_port == ipc->us.sq_port)
564                 return 0;
565 
566         port = addr->sq_port;
567         rc = qrtr_port_assign(ipc, &port);
568         if (rc)
569                 return rc;
570 
571         /* unbind previous, if any */
572         if (!zapped)
573                 qrtr_port_remove(ipc);
574         ipc->us.sq_port = port;
575 
576         sock_reset_flag(sk, SOCK_ZAPPED);
577 
578         /* Notify all open ports about the new controller */
579         if (port == QRTR_PORT_CTRL)
580                 qrtr_reset_ports();
581 
582         return 0;
583 }
584 
585 /* Auto bind to an ephemeral port. */
586 static int qrtr_autobind(struct socket *sock)
587 {
588         struct sock *sk = sock->sk;
589         struct sockaddr_qrtr addr;
590 
591         if (!sock_flag(sk, SOCK_ZAPPED))
592                 return 0;
593 
594         addr.sq_family = AF_QIPCRTR;
595         addr.sq_node = qrtr_local_nid;
596         addr.sq_port = 0;
597 
598         return __qrtr_bind(sock, &addr, 1);
599 }
600 
601 /* Bind socket to specified sockaddr. */
602 static int qrtr_bind(struct socket *sock, struct sockaddr *saddr, int len)
603 {
604         DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, saddr);
605         struct qrtr_sock *ipc = qrtr_sk(sock->sk);
606         struct sock *sk = sock->sk;
607         int rc;
608 
609         if (len < sizeof(*addr) || addr->sq_family != AF_QIPCRTR)
610                 return -EINVAL;
611 
612         if (addr->sq_node != ipc->us.sq_node)
613                 return -EINVAL;
614 
615         lock_sock(sk);
616         rc = __qrtr_bind(sock, addr, sock_flag(sk, SOCK_ZAPPED));
617         release_sock(sk);
618 
619         return rc;
620 }
621 
622 /* Queue packet to local peer socket. */
623 static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb)
624 {
625         const struct qrtr_hdr *phdr;
626         struct qrtr_sock *ipc;
627 
628         phdr = (const struct qrtr_hdr *)skb_transport_header(skb);
629 
630         ipc = qrtr_port_lookup(le32_to_cpu(phdr->dst_port_id));
631         if (!ipc || &ipc->sk == skb->sk) { /* do not send to self */
632                 kfree_skb(skb);
633                 return -ENODEV;
634         }
635 
636         if (sock_queue_rcv_skb(&ipc->sk, skb)) {
637                 qrtr_port_put(ipc);
638                 kfree_skb(skb);
639                 return -ENOSPC;
640         }
641 
642         qrtr_port_put(ipc);
643 
644         return 0;
645 }
646 
647 /* Queue packet for broadcast. */
648 static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb)
649 {
650         struct sk_buff *skbn;
651 
652         mutex_lock(&qrtr_node_lock);
653         list_for_each_entry(node, &qrtr_all_nodes, item) {
654                 skbn = skb_clone(skb, GFP_KERNEL);
655                 if (!skbn)
656                         break;
657                 skb_set_owner_w(skbn, skb->sk);
658                 qrtr_node_enqueue(node, skbn);
659         }
660         mutex_unlock(&qrtr_node_lock);
661 
662         qrtr_local_enqueue(node, skb);
663 
664         return 0;
665 }
666 
667 static int qrtr_sendmsg(struct socket *sock, struct msghdr *msg, size_t len)
668 {
669         DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, msg->msg_name);
670         int (*enqueue_fn)(struct qrtr_node *, struct sk_buff *);
671         struct qrtr_sock *ipc = qrtr_sk(sock->sk);
672         struct sock *sk = sock->sk;
673         struct qrtr_node *node;
674         struct qrtr_hdr *hdr;
675         struct sk_buff *skb;
676         size_t plen;
677         int rc;
678 
679         if (msg->msg_flags & ~(MSG_DONTWAIT))
680                 return -EINVAL;
681 
682         if (len > 65535)
683                 return -EMSGSIZE;
684 
685         lock_sock(sk);
686 
687         if (addr) {
688                 if (msg->msg_namelen < sizeof(*addr)) {
689                         release_sock(sk);
690                         return -EINVAL;
691                 }
692 
693                 if (addr->sq_family != AF_QIPCRTR) {
694                         release_sock(sk);
695                         return -EINVAL;
696                 }
697 
698                 rc = qrtr_autobind(sock);
699                 if (rc) {
700                         release_sock(sk);
701                         return rc;
702                 }
703         } else if (sk->sk_state == TCP_ESTABLISHED) {
704                 addr = &ipc->peer;
705         } else {
706                 release_sock(sk);
707                 return -ENOTCONN;
708         }
709 
710         node = NULL;
711         if (addr->sq_node == QRTR_NODE_BCAST) {
712                 enqueue_fn = qrtr_bcast_enqueue;
713         } else if (addr->sq_node == ipc->us.sq_node) {
714                 enqueue_fn = qrtr_local_enqueue;
715         } else {
716                 enqueue_fn = qrtr_node_enqueue;
717                 node = qrtr_node_lookup(addr->sq_node);
718                 if (!node) {
719                         release_sock(sk);
720                         return -ECONNRESET;
721                 }
722         }
723 
724         plen = (len + 3) & ~3;
725         skb = sock_alloc_send_skb(sk, plen + QRTR_HDR_SIZE,
726                                   msg->msg_flags & MSG_DONTWAIT, &rc);
727         if (!skb)
728                 goto out_node;
729 
730         skb_reset_transport_header(skb);
731         skb_put(skb, len + QRTR_HDR_SIZE);
732 
733         hdr = (struct qrtr_hdr *)skb_transport_header(skb);
734         hdr->version = cpu_to_le32(QRTR_PROTO_VER);
735         hdr->src_node_id = cpu_to_le32(ipc->us.sq_node);
736         hdr->src_port_id = cpu_to_le32(ipc->us.sq_port);
737         hdr->confirm_rx = cpu_to_le32(0);
738         hdr->size = cpu_to_le32(len);
739         hdr->dst_node_id = cpu_to_le32(addr->sq_node);
740         hdr->dst_port_id = cpu_to_le32(addr->sq_port);
741 
742         rc = skb_copy_datagram_from_iter(skb, QRTR_HDR_SIZE,
743                                          &msg->msg_iter, len);
744         if (rc) {
745                 kfree_skb(skb);
746                 goto out_node;
747         }
748 
749         if (plen != len) {
750                 rc = skb_pad(skb, plen - len);
751                 if (rc)
752                         goto out_node;
753                 skb_put(skb, plen - len);
754         }
755 
756         if (ipc->us.sq_port == QRTR_PORT_CTRL) {
757                 if (len < 4) {
758                         rc = -EINVAL;
759                         kfree_skb(skb);
760                         goto out_node;
761                 }
762 
763                 /* control messages already require the type as 'command' */
764                 skb_copy_bits(skb, QRTR_HDR_SIZE, &hdr->type, 4);
765         } else {
766                 hdr->type = cpu_to_le32(QRTR_TYPE_DATA);
767         }
768 
769         rc = enqueue_fn(node, skb);
770         if (rc >= 0)
771                 rc = len;
772 
773 out_node:
774         qrtr_node_release(node);
775         release_sock(sk);
776 
777         return rc;
778 }
779 
780 static int qrtr_recvmsg(struct socket *sock, struct msghdr *msg,
781                         size_t size, int flags)
782 {
783         DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, msg->msg_name);
784         const struct qrtr_hdr *phdr;
785         struct sock *sk = sock->sk;
786         struct sk_buff *skb;
787         int copied, rc;
788 
789         lock_sock(sk);
790 
791         if (sock_flag(sk, SOCK_ZAPPED)) {
792                 release_sock(sk);
793                 return -EADDRNOTAVAIL;
794         }
795 
796         skb = skb_recv_datagram(sk, flags & ~MSG_DONTWAIT,
797                                 flags & MSG_DONTWAIT, &rc);
798         if (!skb) {
799                 release_sock(sk);
800                 return rc;
801         }
802 
803         phdr = (const struct qrtr_hdr *)skb_transport_header(skb);
804         copied = le32_to_cpu(phdr->size);
805         if (copied > size) {
806                 copied = size;
807                 msg->msg_flags |= MSG_TRUNC;
808         }
809 
810         rc = skb_copy_datagram_msg(skb, QRTR_HDR_SIZE, msg, copied);
811         if (rc < 0)
812                 goto out;
813         rc = copied;
814 
815         if (addr) {
816                 addr->sq_family = AF_QIPCRTR;
817                 addr->sq_node = le32_to_cpu(phdr->src_node_id);
818                 addr->sq_port = le32_to_cpu(phdr->src_port_id);
819                 msg->msg_namelen = sizeof(*addr);
820         }
821 
822 out:
823         skb_free_datagram(sk, skb);
824         release_sock(sk);
825 
826         return rc;
827 }
828 
829 static int qrtr_connect(struct socket *sock, struct sockaddr *saddr,
830                         int len, int flags)
831 {
832         DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, saddr);
833         struct qrtr_sock *ipc = qrtr_sk(sock->sk);
834         struct sock *sk = sock->sk;
835         int rc;
836 
837         if (len < sizeof(*addr) || addr->sq_family != AF_QIPCRTR)
838                 return -EINVAL;
839 
840         lock_sock(sk);
841 
842         sk->sk_state = TCP_CLOSE;
843         sock->state = SS_UNCONNECTED;
844 
845         rc = qrtr_autobind(sock);
846         if (rc) {
847                 release_sock(sk);
848                 return rc;
849         }
850 
851         ipc->peer = *addr;
852         sock->state = SS_CONNECTED;
853         sk->sk_state = TCP_ESTABLISHED;
854 
855         release_sock(sk);
856 
857         return 0;
858 }
859 
860 static int qrtr_getname(struct socket *sock, struct sockaddr *saddr,
861                         int *len, int peer)
862 {
863         struct qrtr_sock *ipc = qrtr_sk(sock->sk);
864         struct sockaddr_qrtr qaddr;
865         struct sock *sk = sock->sk;
866 
867         lock_sock(sk);
868         if (peer) {
869                 if (sk->sk_state != TCP_ESTABLISHED) {
870                         release_sock(sk);
871                         return -ENOTCONN;
872                 }
873 
874                 qaddr = ipc->peer;
875         } else {
876                 qaddr = ipc->us;
877         }
878         release_sock(sk);
879 
880         *len = sizeof(qaddr);
881         qaddr.sq_family = AF_QIPCRTR;
882 
883         memcpy(saddr, &qaddr, sizeof(qaddr));
884 
885         return 0;
886 }
887 
888 static int qrtr_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg)
889 {
890         void __user *argp = (void __user *)arg;
891         struct qrtr_sock *ipc = qrtr_sk(sock->sk);
892         struct sock *sk = sock->sk;
893         struct sockaddr_qrtr *sq;
894         struct sk_buff *skb;
895         struct ifreq ifr;
896         long len = 0;
897         int rc = 0;
898 
899         lock_sock(sk);
900 
901         switch (cmd) {
902         case TIOCOUTQ:
903                 len = sk->sk_sndbuf - sk_wmem_alloc_get(sk);
904                 if (len < 0)
905                         len = 0;
906                 rc = put_user(len, (int __user *)argp);
907                 break;
908         case TIOCINQ:
909                 skb = skb_peek(&sk->sk_receive_queue);
910                 if (skb)
911                         len = skb->len - QRTR_HDR_SIZE;
912                 rc = put_user(len, (int __user *)argp);
913                 break;
914         case SIOCGIFADDR:
915                 if (copy_from_user(&ifr, argp, sizeof(ifr))) {
916                         rc = -EFAULT;
917                         break;
918                 }
919 
920                 sq = (struct sockaddr_qrtr *)&ifr.ifr_addr;
921                 *sq = ipc->us;
922                 if (copy_to_user(argp, &ifr, sizeof(ifr))) {
923                         rc = -EFAULT;
924                         break;
925                 }
926                 break;
927         case SIOCGSTAMP:
928                 rc = sock_get_timestamp(sk, argp);
929                 break;
930         case SIOCADDRT:
931         case SIOCDELRT:
932         case SIOCSIFADDR:
933         case SIOCGIFDSTADDR:
934         case SIOCSIFDSTADDR:
935         case SIOCGIFBRDADDR:
936         case SIOCSIFBRDADDR:
937         case SIOCGIFNETMASK:
938         case SIOCSIFNETMASK:
939                 rc = -EINVAL;
940                 break;
941         default:
942                 rc = -ENOIOCTLCMD;
943                 break;
944         }
945 
946         release_sock(sk);
947 
948         return rc;
949 }
950 
951 static int qrtr_release(struct socket *sock)
952 {
953         struct sock *sk = sock->sk;
954         struct qrtr_sock *ipc;
955 
956         if (!sk)
957                 return 0;
958 
959         lock_sock(sk);
960 
961         ipc = qrtr_sk(sk);
962         sk->sk_shutdown = SHUTDOWN_MASK;
963         if (!sock_flag(sk, SOCK_DEAD))
964                 sk->sk_state_change(sk);
965 
966         sock_set_flag(sk, SOCK_DEAD);
967         sock->sk = NULL;
968 
969         if (!sock_flag(sk, SOCK_ZAPPED))
970                 qrtr_port_remove(ipc);
971 
972         skb_queue_purge(&sk->sk_receive_queue);
973 
974         release_sock(sk);
975         sock_put(sk);
976 
977         return 0;
978 }
979 
980 static const struct proto_ops qrtr_proto_ops = {
981         .owner          = THIS_MODULE,
982         .family         = AF_QIPCRTR,
983         .bind           = qrtr_bind,
984         .connect        = qrtr_connect,
985         .socketpair     = sock_no_socketpair,
986         .accept         = sock_no_accept,
987         .listen         = sock_no_listen,
988         .sendmsg        = qrtr_sendmsg,
989         .recvmsg        = qrtr_recvmsg,
990         .getname        = qrtr_getname,
991         .ioctl          = qrtr_ioctl,
992         .poll           = datagram_poll,
993         .shutdown       = sock_no_shutdown,
994         .setsockopt     = sock_no_setsockopt,
995         .getsockopt     = sock_no_getsockopt,
996         .release        = qrtr_release,
997         .mmap           = sock_no_mmap,
998         .sendpage       = sock_no_sendpage,
999 };
1000 
1001 static struct proto qrtr_proto = {
1002         .name           = "QIPCRTR",
1003         .owner          = THIS_MODULE,
1004         .obj_size       = sizeof(struct qrtr_sock),
1005 };
1006 
1007 static int qrtr_create(struct net *net, struct socket *sock,
1008                        int protocol, int kern)
1009 {
1010         struct qrtr_sock *ipc;
1011         struct sock *sk;
1012 
1013         if (sock->type != SOCK_DGRAM)
1014                 return -EPROTOTYPE;
1015 
1016         sk = sk_alloc(net, AF_QIPCRTR, GFP_KERNEL, &qrtr_proto, kern);
1017         if (!sk)
1018                 return -ENOMEM;
1019 
1020         sock_set_flag(sk, SOCK_ZAPPED);
1021 
1022         sock_init_data(sock, sk);
1023         sock->ops = &qrtr_proto_ops;
1024 
1025         ipc = qrtr_sk(sk);
1026         ipc->us.sq_family = AF_QIPCRTR;
1027         ipc->us.sq_node = qrtr_local_nid;
1028         ipc->us.sq_port = 0;
1029 
1030         return 0;
1031 }
1032 
1033 static const struct nla_policy qrtr_policy[IFA_MAX + 1] = {
1034         [IFA_LOCAL] = { .type = NLA_U32 },
1035 };
1036 
1037 static int qrtr_addr_doit(struct sk_buff *skb, struct nlmsghdr *nlh,
1038                           struct netlink_ext_ack *extack)
1039 {
1040         struct nlattr *tb[IFA_MAX + 1];
1041         struct ifaddrmsg *ifm;
1042         int rc;
1043 
1044         if (!netlink_capable(skb, CAP_NET_ADMIN))
1045                 return -EPERM;
1046 
1047         if (!netlink_capable(skb, CAP_SYS_ADMIN))
1048                 return -EPERM;
1049 
1050         ASSERT_RTNL();
1051 
1052         rc = nlmsg_parse(nlh, sizeof(*ifm), tb, IFA_MAX, qrtr_policy, extack);
1053         if (rc < 0)
1054                 return rc;
1055 
1056         ifm = nlmsg_data(nlh);
1057         if (!tb[IFA_LOCAL])
1058                 return -EINVAL;
1059 
1060         qrtr_local_nid = nla_get_u32(tb[IFA_LOCAL]);
1061         return 0;
1062 }
1063 
1064 static const struct net_proto_family qrtr_family = {
1065         .owner  = THIS_MODULE,
1066         .family = AF_QIPCRTR,
1067         .create = qrtr_create,
1068 };
1069 
1070 static int __init qrtr_proto_init(void)
1071 {
1072         int rc;
1073 
1074         rc = proto_register(&qrtr_proto, 1);
1075         if (rc)
1076                 return rc;
1077 
1078         rc = sock_register(&qrtr_family);
1079         if (rc) {
1080                 proto_unregister(&qrtr_proto);
1081                 return rc;
1082         }
1083 
1084         rtnl_register(PF_QIPCRTR, RTM_NEWADDR, qrtr_addr_doit, NULL, NULL);
1085 
1086         return 0;
1087 }
1088 module_init(qrtr_proto_init);
1089 
1090 static void __exit qrtr_proto_fini(void)
1091 {
1092         rtnl_unregister(PF_QIPCRTR, RTM_NEWADDR);
1093         sock_unregister(qrtr_family.family);
1094         proto_unregister(&qrtr_proto);
1095 }
1096 module_exit(qrtr_proto_fini);
1097 
1098 MODULE_DESCRIPTION("Qualcomm IPC-router driver");
1099 MODULE_LICENSE("GPL v2");
1100 

~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~

kernel.org | git.kernel.org | LWN.net | Project Home | Wiki (Japanese) | Wiki (English) | SVN repository | Mail admin

Linux® is a registered trademark of Linus Torvalds in the United States and other countries.
TOMOYO® is a registered trademark of NTT DATA CORPORATION.

osdn.jp