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

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

Version: ~ [ linux-5.12-rc1 ] ~ [ linux-5.11.2 ] ~ [ linux-5.10.19 ] ~ [ linux-5.9.16 ] ~ [ linux-5.8.18 ] ~ [ linux-5.7.19 ] ~ [ linux-5.6.19 ] ~ [ linux-5.5.19 ] ~ [ linux-5.4.101 ] ~ [ linux-5.3.18 ] ~ [ linux-5.2.21 ] ~ [ linux-5.1.21 ] ~ [ linux-5.0.21 ] ~ [ linux-4.20.17 ] ~ [ linux-4.19.177 ] ~ [ linux-4.18.20 ] ~ [ linux-4.17.19 ] ~ [ linux-4.16.18 ] ~ [ linux-4.15.18 ] ~ [ linux-4.14.222 ] ~ [ linux-4.13.16 ] ~ [ linux-4.12.14 ] ~ [ linux-4.11.12 ] ~ [ linux-4.10.17 ] ~ [ linux-4.9.258 ] ~ [ linux-4.8.17 ] ~ [ linux-4.7.10 ] ~ [ linux-4.6.7 ] ~ [ linux-4.5.7 ] ~ [ linux-4.4.258 ] ~ [ 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 /* Release node resources and free the node.
115  *
116  * Do not call directly, use qrtr_node_release.  To be used with
117  * kref_put_mutex.  As such, the node mutex is expected to be locked on call.
118  */
119 static void __qrtr_node_release(struct kref *kref)
120 {
121         struct qrtr_node *node = container_of(kref, struct qrtr_node, ref);
122 
123         if (node->nid != QRTR_EP_NID_AUTO)
124                 radix_tree_delete(&qrtr_nodes, node->nid);
125 
126         list_del(&node->item);
127         mutex_unlock(&qrtr_node_lock);
128 
129         skb_queue_purge(&node->rx_queue);
130         kfree(node);
131 }
132 
133 /* Increment reference to node. */
134 static struct qrtr_node *qrtr_node_acquire(struct qrtr_node *node)
135 {
136         if (node)
137                 kref_get(&node->ref);
138         return node;
139 }
140 
141 /* Decrement reference to node and release as necessary. */
142 static void qrtr_node_release(struct qrtr_node *node)
143 {
144         if (!node)
145                 return;
146         kref_put_mutex(&node->ref, __qrtr_node_release, &qrtr_node_lock);
147 }
148 
149 /* Pass an outgoing packet socket buffer to the endpoint driver. */
150 static int qrtr_node_enqueue(struct qrtr_node *node, struct sk_buff *skb)
151 {
152         int rc = -ENODEV;
153 
154         mutex_lock(&node->ep_lock);
155         if (node->ep)
156                 rc = node->ep->xmit(node->ep, skb);
157         else
158                 kfree_skb(skb);
159         mutex_unlock(&node->ep_lock);
160 
161         return rc;
162 }
163 
164 /* Lookup node by id.
165  *
166  * callers must release with qrtr_node_release()
167  */
168 static struct qrtr_node *qrtr_node_lookup(unsigned int nid)
169 {
170         struct qrtr_node *node;
171 
172         mutex_lock(&qrtr_node_lock);
173         node = radix_tree_lookup(&qrtr_nodes, nid);
174         node = qrtr_node_acquire(node);
175         mutex_unlock(&qrtr_node_lock);
176 
177         return node;
178 }
179 
180 /* Assign node id to node.
181  *
182  * This is mostly useful for automatic node id assignment, based on
183  * the source id in the incoming packet.
184  */
185 static void qrtr_node_assign(struct qrtr_node *node, unsigned int nid)
186 {
187         if (node->nid != QRTR_EP_NID_AUTO || nid == QRTR_EP_NID_AUTO)
188                 return;
189 
190         mutex_lock(&qrtr_node_lock);
191         radix_tree_insert(&qrtr_nodes, nid, node);
192         node->nid = nid;
193         mutex_unlock(&qrtr_node_lock);
194 }
195 
196 /**
197  * qrtr_endpoint_post() - post incoming data
198  * @ep: endpoint handle
199  * @data: data pointer
200  * @len: size of data in bytes
201  *
202  * Return: 0 on success; negative error code on failure
203  */
204 int qrtr_endpoint_post(struct qrtr_endpoint *ep, const void *data, size_t len)
205 {
206         struct qrtr_node *node = ep->node;
207         const struct qrtr_hdr *phdr = data;
208         struct sk_buff *skb;
209         unsigned int psize;
210         unsigned int size;
211         unsigned int type;
212         unsigned int ver;
213         unsigned int dst;
214 
215         if (len < QRTR_HDR_SIZE || len & 3)
216                 return -EINVAL;
217 
218         ver = le32_to_cpu(phdr->version);
219         size = le32_to_cpu(phdr->size);
220         type = le32_to_cpu(phdr->type);
221         dst = le32_to_cpu(phdr->dst_port_id);
222 
223         psize = (size + 3) & ~3;
224 
225         if (ver != QRTR_PROTO_VER)
226                 return -EINVAL;
227 
228         if (len != psize + QRTR_HDR_SIZE)
229                 return -EINVAL;
230 
231         if (dst != QRTR_PORT_CTRL && type != QRTR_TYPE_DATA)
232                 return -EINVAL;
233 
234         skb = netdev_alloc_skb(NULL, len);
235         if (!skb)
236                 return -ENOMEM;
237 
238         skb_reset_transport_header(skb);
239         memcpy(skb_put(skb, len), data, len);
240 
241         skb_queue_tail(&node->rx_queue, skb);
242         schedule_work(&node->work);
243 
244         return 0;
245 }
246 EXPORT_SYMBOL_GPL(qrtr_endpoint_post);
247 
248 /* Allocate and construct a resume-tx packet. */
249 static struct sk_buff *qrtr_alloc_resume_tx(u32 src_node,
250                                             u32 dst_node, u32 port)
251 {
252         const int pkt_len = 20;
253         struct qrtr_hdr *hdr;
254         struct sk_buff *skb;
255         u32 *buf;
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 = (struct qrtr_hdr *)skb_put(skb, QRTR_HDR_SIZE);
263         hdr->version = cpu_to_le32(QRTR_PROTO_VER);
264         hdr->type = cpu_to_le32(QRTR_TYPE_RESUME_TX);
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         buf = (u32 *)skb_put(skb, pkt_len);
273         memset(buf, 0, pkt_len);
274         buf[0] = cpu_to_le32(QRTR_TYPE_RESUME_TX);
275         buf[1] = cpu_to_le32(src_node);
276         buf[2] = cpu_to_le32(port);
277 
278         return skb;
279 }
280 
281 static struct qrtr_sock *qrtr_port_lookup(int port);
282 static void qrtr_port_put(struct qrtr_sock *ipc);
283 
284 /* Handle and route a received packet.
285  *
286  * This will auto-reply with resume-tx packet as necessary.
287  */
288 static void qrtr_node_rx_work(struct work_struct *work)
289 {
290         struct qrtr_node *node = container_of(work, struct qrtr_node, work);
291         struct sk_buff *skb;
292 
293         while ((skb = skb_dequeue(&node->rx_queue)) != NULL) {
294                 const struct qrtr_hdr *phdr;
295                 u32 dst_node, dst_port;
296                 struct qrtr_sock *ipc;
297                 u32 src_node;
298                 int confirm;
299 
300                 phdr = (const struct qrtr_hdr *)skb_transport_header(skb);
301                 src_node = le32_to_cpu(phdr->src_node_id);
302                 dst_node = le32_to_cpu(phdr->dst_node_id);
303                 dst_port = le32_to_cpu(phdr->dst_port_id);
304                 confirm = !!phdr->confirm_rx;
305 
306                 qrtr_node_assign(node, src_node);
307 
308                 ipc = qrtr_port_lookup(dst_port);
309                 if (!ipc) {
310                         kfree_skb(skb);
311                 } else {
312                         if (sock_queue_rcv_skb(&ipc->sk, skb))
313                                 kfree_skb(skb);
314 
315                         qrtr_port_put(ipc);
316                 }
317 
318                 if (confirm) {
319                         skb = qrtr_alloc_resume_tx(dst_node, node->nid, dst_port);
320                         if (!skb)
321                                 break;
322                         if (qrtr_node_enqueue(node, skb))
323                                 break;
324                 }
325         }
326 }
327 
328 /**
329  * qrtr_endpoint_register() - register a new endpoint
330  * @ep: endpoint to register
331  * @nid: desired node id; may be QRTR_EP_NID_AUTO for auto-assignment
332  * Return: 0 on success; negative error code on failure
333  *
334  * The specified endpoint must have the xmit function pointer set on call.
335  */
336 int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int nid)
337 {
338         struct qrtr_node *node;
339 
340         if (!ep || !ep->xmit)
341                 return -EINVAL;
342 
343         node = kzalloc(sizeof(*node), GFP_KERNEL);
344         if (!node)
345                 return -ENOMEM;
346 
347         INIT_WORK(&node->work, qrtr_node_rx_work);
348         kref_init(&node->ref);
349         mutex_init(&node->ep_lock);
350         skb_queue_head_init(&node->rx_queue);
351         node->nid = QRTR_EP_NID_AUTO;
352         node->ep = ep;
353 
354         qrtr_node_assign(node, nid);
355 
356         mutex_lock(&qrtr_node_lock);
357         list_add(&node->item, &qrtr_all_nodes);
358         mutex_unlock(&qrtr_node_lock);
359         ep->node = node;
360 
361         return 0;
362 }
363 EXPORT_SYMBOL_GPL(qrtr_endpoint_register);
364 
365 /**
366  * qrtr_endpoint_unregister - unregister endpoint
367  * @ep: endpoint to unregister
368  */
369 void qrtr_endpoint_unregister(struct qrtr_endpoint *ep)
370 {
371         struct qrtr_node *node = ep->node;
372 
373         mutex_lock(&node->ep_lock);
374         node->ep = NULL;
375         mutex_unlock(&node->ep_lock);
376 
377         qrtr_node_release(node);
378         ep->node = NULL;
379 }
380 EXPORT_SYMBOL_GPL(qrtr_endpoint_unregister);
381 
382 /* Lookup socket by port.
383  *
384  * Callers must release with qrtr_port_put()
385  */
386 static struct qrtr_sock *qrtr_port_lookup(int port)
387 {
388         struct qrtr_sock *ipc;
389 
390         if (port == QRTR_PORT_CTRL)
391                 port = 0;
392 
393         mutex_lock(&qrtr_port_lock);
394         ipc = idr_find(&qrtr_ports, port);
395         if (ipc)
396                 sock_hold(&ipc->sk);
397         mutex_unlock(&qrtr_port_lock);
398 
399         return ipc;
400 }
401 
402 /* Release acquired socket. */
403 static void qrtr_port_put(struct qrtr_sock *ipc)
404 {
405         sock_put(&ipc->sk);
406 }
407 
408 /* Remove port assignment. */
409 static void qrtr_port_remove(struct qrtr_sock *ipc)
410 {
411         int port = ipc->us.sq_port;
412 
413         if (port == QRTR_PORT_CTRL)
414                 port = 0;
415 
416         __sock_put(&ipc->sk);
417 
418         mutex_lock(&qrtr_port_lock);
419         idr_remove(&qrtr_ports, port);
420         mutex_unlock(&qrtr_port_lock);
421 }
422 
423 /* Assign port number to socket.
424  *
425  * Specify port in the integer pointed to by port, and it will be adjusted
426  * on return as necesssary.
427  *
428  * Port may be:
429  *   0: Assign ephemeral port in [QRTR_MIN_EPH_SOCKET, QRTR_MAX_EPH_SOCKET]
430  *   <QRTR_MIN_EPH_SOCKET: Specified; requires CAP_NET_ADMIN
431  *   >QRTR_MIN_EPH_SOCKET: Specified; available to all
432  */
433 static int qrtr_port_assign(struct qrtr_sock *ipc, int *port)
434 {
435         int rc;
436 
437         mutex_lock(&qrtr_port_lock);
438         if (!*port) {
439                 rc = idr_alloc(&qrtr_ports, ipc,
440                                QRTR_MIN_EPH_SOCKET, QRTR_MAX_EPH_SOCKET + 1,
441                                GFP_ATOMIC);
442                 if (rc >= 0)
443                         *port = rc;
444         } else if (*port < QRTR_MIN_EPH_SOCKET && !capable(CAP_NET_ADMIN)) {
445                 rc = -EACCES;
446         } else if (*port == QRTR_PORT_CTRL) {
447                 rc = idr_alloc(&qrtr_ports, ipc, 0, 1, GFP_ATOMIC);
448         } else {
449                 rc = idr_alloc(&qrtr_ports, ipc, *port, *port + 1, GFP_ATOMIC);
450                 if (rc >= 0)
451                         *port = rc;
452         }
453         mutex_unlock(&qrtr_port_lock);
454 
455         if (rc == -ENOSPC)
456                 return -EADDRINUSE;
457         else if (rc < 0)
458                 return rc;
459 
460         sock_hold(&ipc->sk);
461 
462         return 0;
463 }
464 
465 /* Bind socket to address.
466  *
467  * Socket should be locked upon call.
468  */
469 static int __qrtr_bind(struct socket *sock,
470                        const struct sockaddr_qrtr *addr, int zapped)
471 {
472         struct qrtr_sock *ipc = qrtr_sk(sock->sk);
473         struct sock *sk = sock->sk;
474         int port;
475         int rc;
476 
477         /* rebinding ok */
478         if (!zapped && addr->sq_port == ipc->us.sq_port)
479                 return 0;
480 
481         port = addr->sq_port;
482         rc = qrtr_port_assign(ipc, &port);
483         if (rc)
484                 return rc;
485 
486         /* unbind previous, if any */
487         if (!zapped)
488                 qrtr_port_remove(ipc);
489         ipc->us.sq_port = port;
490 
491         sock_reset_flag(sk, SOCK_ZAPPED);
492 
493         return 0;
494 }
495 
496 /* Auto bind to an ephemeral port. */
497 static int qrtr_autobind(struct socket *sock)
498 {
499         struct sock *sk = sock->sk;
500         struct sockaddr_qrtr addr;
501 
502         if (!sock_flag(sk, SOCK_ZAPPED))
503                 return 0;
504 
505         addr.sq_family = AF_QIPCRTR;
506         addr.sq_node = qrtr_local_nid;
507         addr.sq_port = 0;
508 
509         return __qrtr_bind(sock, &addr, 1);
510 }
511 
512 /* Bind socket to specified sockaddr. */
513 static int qrtr_bind(struct socket *sock, struct sockaddr *saddr, int len)
514 {
515         DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, saddr);
516         struct qrtr_sock *ipc = qrtr_sk(sock->sk);
517         struct sock *sk = sock->sk;
518         int rc;
519 
520         if (len < sizeof(*addr) || addr->sq_family != AF_QIPCRTR)
521                 return -EINVAL;
522 
523         if (addr->sq_node != ipc->us.sq_node)
524                 return -EINVAL;
525 
526         lock_sock(sk);
527         rc = __qrtr_bind(sock, addr, sock_flag(sk, SOCK_ZAPPED));
528         release_sock(sk);
529 
530         return rc;
531 }
532 
533 /* Queue packet to local peer socket. */
534 static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb)
535 {
536         const struct qrtr_hdr *phdr;
537         struct qrtr_sock *ipc;
538 
539         phdr = (const struct qrtr_hdr *)skb_transport_header(skb);
540 
541         ipc = qrtr_port_lookup(le32_to_cpu(phdr->dst_port_id));
542         if (!ipc || &ipc->sk == skb->sk) { /* do not send to self */
543                 kfree_skb(skb);
544                 return -ENODEV;
545         }
546 
547         if (sock_queue_rcv_skb(&ipc->sk, skb)) {
548                 qrtr_port_put(ipc);
549                 kfree_skb(skb);
550                 return -ENOSPC;
551         }
552 
553         qrtr_port_put(ipc);
554 
555         return 0;
556 }
557 
558 /* Queue packet for broadcast. */
559 static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb)
560 {
561         struct sk_buff *skbn;
562 
563         mutex_lock(&qrtr_node_lock);
564         list_for_each_entry(node, &qrtr_all_nodes, item) {
565                 skbn = skb_clone(skb, GFP_KERNEL);
566                 if (!skbn)
567                         break;
568                 skb_set_owner_w(skbn, skb->sk);
569                 qrtr_node_enqueue(node, skbn);
570         }
571         mutex_unlock(&qrtr_node_lock);
572 
573         qrtr_local_enqueue(node, skb);
574 
575         return 0;
576 }
577 
578 static int qrtr_sendmsg(struct socket *sock, struct msghdr *msg, size_t len)
579 {
580         DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, msg->msg_name);
581         int (*enqueue_fn)(struct qrtr_node *, struct sk_buff *);
582         struct qrtr_sock *ipc = qrtr_sk(sock->sk);
583         struct sock *sk = sock->sk;
584         struct qrtr_node *node;
585         struct qrtr_hdr *hdr;
586         struct sk_buff *skb;
587         size_t plen;
588         int rc;
589 
590         if (msg->msg_flags & ~(MSG_DONTWAIT))
591                 return -EINVAL;
592 
593         if (len > 65535)
594                 return -EMSGSIZE;
595 
596         lock_sock(sk);
597 
598         if (addr) {
599                 if (msg->msg_namelen < sizeof(*addr)) {
600                         release_sock(sk);
601                         return -EINVAL;
602                 }
603 
604                 if (addr->sq_family != AF_QIPCRTR) {
605                         release_sock(sk);
606                         return -EINVAL;
607                 }
608 
609                 rc = qrtr_autobind(sock);
610                 if (rc) {
611                         release_sock(sk);
612                         return rc;
613                 }
614         } else if (sk->sk_state == TCP_ESTABLISHED) {
615                 addr = &ipc->peer;
616         } else {
617                 release_sock(sk);
618                 return -ENOTCONN;
619         }
620 
621         node = NULL;
622         if (addr->sq_node == QRTR_NODE_BCAST) {
623                 enqueue_fn = qrtr_bcast_enqueue;
624         } else if (addr->sq_node == ipc->us.sq_node) {
625                 enqueue_fn = qrtr_local_enqueue;
626         } else {
627                 enqueue_fn = qrtr_node_enqueue;
628                 node = qrtr_node_lookup(addr->sq_node);
629                 if (!node) {
630                         release_sock(sk);
631                         return -ECONNRESET;
632                 }
633         }
634 
635         plen = (len + 3) & ~3;
636         skb = sock_alloc_send_skb(sk, plen + QRTR_HDR_SIZE,
637                                   msg->msg_flags & MSG_DONTWAIT, &rc);
638         if (!skb)
639                 goto out_node;
640 
641         skb_reset_transport_header(skb);
642         skb_put(skb, len + QRTR_HDR_SIZE);
643 
644         hdr = (struct qrtr_hdr *)skb_transport_header(skb);
645         hdr->version = cpu_to_le32(QRTR_PROTO_VER);
646         hdr->src_node_id = cpu_to_le32(ipc->us.sq_node);
647         hdr->src_port_id = cpu_to_le32(ipc->us.sq_port);
648         hdr->confirm_rx = cpu_to_le32(0);
649         hdr->size = cpu_to_le32(len);
650         hdr->dst_node_id = cpu_to_le32(addr->sq_node);
651         hdr->dst_port_id = cpu_to_le32(addr->sq_port);
652 
653         rc = skb_copy_datagram_from_iter(skb, QRTR_HDR_SIZE,
654                                          &msg->msg_iter, len);
655         if (rc) {
656                 kfree_skb(skb);
657                 goto out_node;
658         }
659 
660         if (plen != len) {
661                 skb_pad(skb, plen - len);
662                 skb_put(skb, plen - len);
663         }
664 
665         if (ipc->us.sq_port == QRTR_PORT_CTRL) {
666                 if (len < 4) {
667                         rc = -EINVAL;
668                         kfree_skb(skb);
669                         goto out_node;
670                 }
671 
672                 /* control messages already require the type as 'command' */
673                 skb_copy_bits(skb, QRTR_HDR_SIZE, &hdr->type, 4);
674         } else {
675                 hdr->type = cpu_to_le32(QRTR_TYPE_DATA);
676         }
677 
678         rc = enqueue_fn(node, skb);
679         if (rc >= 0)
680                 rc = len;
681 
682 out_node:
683         qrtr_node_release(node);
684         release_sock(sk);
685 
686         return rc;
687 }
688 
689 static int qrtr_recvmsg(struct socket *sock, struct msghdr *msg,
690                         size_t size, int flags)
691 {
692         DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, msg->msg_name);
693         const struct qrtr_hdr *phdr;
694         struct sock *sk = sock->sk;
695         struct sk_buff *skb;
696         int copied, rc;
697 
698         lock_sock(sk);
699 
700         if (sock_flag(sk, SOCK_ZAPPED)) {
701                 release_sock(sk);
702                 return -EADDRNOTAVAIL;
703         }
704 
705         skb = skb_recv_datagram(sk, flags & ~MSG_DONTWAIT,
706                                 flags & MSG_DONTWAIT, &rc);
707         if (!skb) {
708                 release_sock(sk);
709                 return rc;
710         }
711 
712         phdr = (const struct qrtr_hdr *)skb_transport_header(skb);
713         copied = le32_to_cpu(phdr->size);
714         if (copied > size) {
715                 copied = size;
716                 msg->msg_flags |= MSG_TRUNC;
717         }
718 
719         rc = skb_copy_datagram_msg(skb, QRTR_HDR_SIZE, msg, copied);
720         if (rc < 0)
721                 goto out;
722         rc = copied;
723 
724         if (addr) {
725                 addr->sq_family = AF_QIPCRTR;
726                 addr->sq_node = le32_to_cpu(phdr->src_node_id);
727                 addr->sq_port = le32_to_cpu(phdr->src_port_id);
728                 msg->msg_namelen = sizeof(*addr);
729         }
730 
731 out:
732         skb_free_datagram(sk, skb);
733         release_sock(sk);
734 
735         return rc;
736 }
737 
738 static int qrtr_connect(struct socket *sock, struct sockaddr *saddr,
739                         int len, int flags)
740 {
741         DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, saddr);
742         struct qrtr_sock *ipc = qrtr_sk(sock->sk);
743         struct sock *sk = sock->sk;
744         int rc;
745 
746         if (len < sizeof(*addr) || addr->sq_family != AF_QIPCRTR)
747                 return -EINVAL;
748 
749         lock_sock(sk);
750 
751         sk->sk_state = TCP_CLOSE;
752         sock->state = SS_UNCONNECTED;
753 
754         rc = qrtr_autobind(sock);
755         if (rc) {
756                 release_sock(sk);
757                 return rc;
758         }
759 
760         ipc->peer = *addr;
761         sock->state = SS_CONNECTED;
762         sk->sk_state = TCP_ESTABLISHED;
763 
764         release_sock(sk);
765 
766         return 0;
767 }
768 
769 static int qrtr_getname(struct socket *sock, struct sockaddr *saddr,
770                         int *len, int peer)
771 {
772         struct qrtr_sock *ipc = qrtr_sk(sock->sk);
773         struct sockaddr_qrtr qaddr;
774         struct sock *sk = sock->sk;
775 
776         lock_sock(sk);
777         if (peer) {
778                 if (sk->sk_state != TCP_ESTABLISHED) {
779                         release_sock(sk);
780                         return -ENOTCONN;
781                 }
782 
783                 qaddr = ipc->peer;
784         } else {
785                 qaddr = ipc->us;
786         }
787         release_sock(sk);
788 
789         *len = sizeof(qaddr);
790         qaddr.sq_family = AF_QIPCRTR;
791 
792         memcpy(saddr, &qaddr, sizeof(qaddr));
793 
794         return 0;
795 }
796 
797 static int qrtr_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg)
798 {
799         void __user *argp = (void __user *)arg;
800         struct qrtr_sock *ipc = qrtr_sk(sock->sk);
801         struct sock *sk = sock->sk;
802         struct sockaddr_qrtr *sq;
803         struct sk_buff *skb;
804         struct ifreq ifr;
805         long len = 0;
806         int rc = 0;
807 
808         lock_sock(sk);
809 
810         switch (cmd) {
811         case TIOCOUTQ:
812                 len = sk->sk_sndbuf - sk_wmem_alloc_get(sk);
813                 if (len < 0)
814                         len = 0;
815                 rc = put_user(len, (int __user *)argp);
816                 break;
817         case TIOCINQ:
818                 skb = skb_peek(&sk->sk_receive_queue);
819                 if (skb)
820                         len = skb->len - QRTR_HDR_SIZE;
821                 rc = put_user(len, (int __user *)argp);
822                 break;
823         case SIOCGIFADDR:
824                 if (copy_from_user(&ifr, argp, sizeof(ifr))) {
825                         rc = -EFAULT;
826                         break;
827                 }
828 
829                 sq = (struct sockaddr_qrtr *)&ifr.ifr_addr;
830                 *sq = ipc->us;
831                 if (copy_to_user(argp, &ifr, sizeof(ifr))) {
832                         rc = -EFAULT;
833                         break;
834                 }
835                 break;
836         case SIOCGSTAMP:
837                 rc = sock_get_timestamp(sk, argp);
838                 break;
839         case SIOCADDRT:
840         case SIOCDELRT:
841         case SIOCSIFADDR:
842         case SIOCGIFDSTADDR:
843         case SIOCSIFDSTADDR:
844         case SIOCGIFBRDADDR:
845         case SIOCSIFBRDADDR:
846         case SIOCGIFNETMASK:
847         case SIOCSIFNETMASK:
848                 rc = -EINVAL;
849                 break;
850         default:
851                 rc = -ENOIOCTLCMD;
852                 break;
853         }
854 
855         release_sock(sk);
856 
857         return rc;
858 }
859 
860 static int qrtr_release(struct socket *sock)
861 {
862         struct sock *sk = sock->sk;
863         struct qrtr_sock *ipc;
864 
865         if (!sk)
866                 return 0;
867 
868         lock_sock(sk);
869 
870         ipc = qrtr_sk(sk);
871         sk->sk_shutdown = SHUTDOWN_MASK;
872         if (!sock_flag(sk, SOCK_DEAD))
873                 sk->sk_state_change(sk);
874 
875         sock_set_flag(sk, SOCK_DEAD);
876         sock->sk = NULL;
877 
878         if (!sock_flag(sk, SOCK_ZAPPED))
879                 qrtr_port_remove(ipc);
880 
881         skb_queue_purge(&sk->sk_receive_queue);
882 
883         release_sock(sk);
884         sock_put(sk);
885 
886         return 0;
887 }
888 
889 static const struct proto_ops qrtr_proto_ops = {
890         .owner          = THIS_MODULE,
891         .family         = AF_QIPCRTR,
892         .bind           = qrtr_bind,
893         .connect        = qrtr_connect,
894         .socketpair     = sock_no_socketpair,
895         .accept         = sock_no_accept,
896         .listen         = sock_no_listen,
897         .sendmsg        = qrtr_sendmsg,
898         .recvmsg        = qrtr_recvmsg,
899         .getname        = qrtr_getname,
900         .ioctl          = qrtr_ioctl,
901         .poll           = datagram_poll,
902         .shutdown       = sock_no_shutdown,
903         .setsockopt     = sock_no_setsockopt,
904         .getsockopt     = sock_no_getsockopt,
905         .release        = qrtr_release,
906         .mmap           = sock_no_mmap,
907         .sendpage       = sock_no_sendpage,
908 };
909 
910 static struct proto qrtr_proto = {
911         .name           = "QIPCRTR",
912         .owner          = THIS_MODULE,
913         .obj_size       = sizeof(struct qrtr_sock),
914 };
915 
916 static int qrtr_create(struct net *net, struct socket *sock,
917                        int protocol, int kern)
918 {
919         struct qrtr_sock *ipc;
920         struct sock *sk;
921 
922         if (sock->type != SOCK_DGRAM)
923                 return -EPROTOTYPE;
924 
925         sk = sk_alloc(net, AF_QIPCRTR, GFP_KERNEL, &qrtr_proto, kern);
926         if (!sk)
927                 return -ENOMEM;
928 
929         sock_set_flag(sk, SOCK_ZAPPED);
930 
931         sock_init_data(sock, sk);
932         sock->ops = &qrtr_proto_ops;
933 
934         ipc = qrtr_sk(sk);
935         ipc->us.sq_family = AF_QIPCRTR;
936         ipc->us.sq_node = qrtr_local_nid;
937         ipc->us.sq_port = 0;
938 
939         return 0;
940 }
941 
942 static const struct nla_policy qrtr_policy[IFA_MAX + 1] = {
943         [IFA_LOCAL] = { .type = NLA_U32 },
944 };
945 
946 static int qrtr_addr_doit(struct sk_buff *skb, struct nlmsghdr *nlh)
947 {
948         struct nlattr *tb[IFA_MAX + 1];
949         struct ifaddrmsg *ifm;
950         int rc;
951 
952         if (!netlink_capable(skb, CAP_NET_ADMIN))
953                 return -EPERM;
954 
955         if (!netlink_capable(skb, CAP_SYS_ADMIN))
956                 return -EPERM;
957 
958         ASSERT_RTNL();
959 
960         rc = nlmsg_parse(nlh, sizeof(*ifm), tb, IFA_MAX, qrtr_policy);
961         if (rc < 0)
962                 return rc;
963 
964         ifm = nlmsg_data(nlh);
965         if (!tb[IFA_LOCAL])
966                 return -EINVAL;
967 
968         qrtr_local_nid = nla_get_u32(tb[IFA_LOCAL]);
969         return 0;
970 }
971 
972 static const struct net_proto_family qrtr_family = {
973         .owner  = THIS_MODULE,
974         .family = AF_QIPCRTR,
975         .create = qrtr_create,
976 };
977 
978 static int __init qrtr_proto_init(void)
979 {
980         int rc;
981 
982         rc = proto_register(&qrtr_proto, 1);
983         if (rc)
984                 return rc;
985 
986         rc = sock_register(&qrtr_family);
987         if (rc) {
988                 proto_unregister(&qrtr_proto);
989                 return rc;
990         }
991 
992         rtnl_register(PF_QIPCRTR, RTM_NEWADDR, qrtr_addr_doit, NULL, NULL);
993 
994         return 0;
995 }
996 module_init(qrtr_proto_init);
997 
998 static void __exit qrtr_proto_fini(void)
999 {
1000         rtnl_unregister(PF_QIPCRTR, RTM_NEWADDR);
1001         sock_unregister(qrtr_family.family);
1002         proto_unregister(&qrtr_proto);
1003 }
1004 module_exit(qrtr_proto_fini);
1005 
1006 MODULE_DESCRIPTION("Qualcomm IPC-router driver");
1007 MODULE_LICENSE("GPL v2");
1008 

~ [ 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