0001
0002
0003
0004
0005
0006 #include <linux/module.h>
0007 #include <linux/netlink.h>
0008 #include <linux/qrtr.h>
0009 #include <linux/termios.h> /* For TIOCINQ/OUTQ */
0010 #include <linux/spinlock.h>
0011 #include <linux/wait.h>
0012
0013 #include <net/sock.h>
0014
0015 #include "qrtr.h"
0016
0017 #define QRTR_PROTO_VER_1 1
0018 #define QRTR_PROTO_VER_2 3
0019
0020
0021 #define QRTR_MIN_EPH_SOCKET 0x4000
0022 #define QRTR_MAX_EPH_SOCKET 0x7fff
0023 #define QRTR_EPH_PORT_RANGE \
0024 XA_LIMIT(QRTR_MIN_EPH_SOCKET, QRTR_MAX_EPH_SOCKET)
0025
0026
0027
0028
0029
0030
0031
0032
0033
0034
0035
0036
0037 struct qrtr_hdr_v1 {
0038 __le32 version;
0039 __le32 type;
0040 __le32 src_node_id;
0041 __le32 src_port_id;
0042 __le32 confirm_rx;
0043 __le32 size;
0044 __le32 dst_node_id;
0045 __le32 dst_port_id;
0046 } __packed;
0047
0048
0049
0050
0051
0052
0053
0054
0055
0056
0057
0058
0059
0060 struct qrtr_hdr_v2 {
0061 u8 version;
0062 u8 type;
0063 u8 flags;
0064 u8 optlen;
0065 __le32 size;
0066 __le16 src_node_id;
0067 __le16 src_port_id;
0068 __le16 dst_node_id;
0069 __le16 dst_port_id;
0070 };
0071
0072 #define QRTR_FLAGS_CONFIRM_RX BIT(0)
0073
0074 struct qrtr_cb {
0075 u32 src_node;
0076 u32 src_port;
0077 u32 dst_node;
0078 u32 dst_port;
0079
0080 u8 type;
0081 u8 confirm_rx;
0082 };
0083
0084 #define QRTR_HDR_MAX_SIZE max_t(size_t, sizeof(struct qrtr_hdr_v1), \
0085 sizeof(struct qrtr_hdr_v2))
0086
0087 struct qrtr_sock {
0088
0089 struct sock sk;
0090 struct sockaddr_qrtr us;
0091 struct sockaddr_qrtr peer;
0092 };
0093
0094 static inline struct qrtr_sock *qrtr_sk(struct sock *sk)
0095 {
0096 BUILD_BUG_ON(offsetof(struct qrtr_sock, sk) != 0);
0097 return container_of(sk, struct qrtr_sock, sk);
0098 }
0099
0100 static unsigned int qrtr_local_nid = 1;
0101
0102
0103 static RADIX_TREE(qrtr_nodes, GFP_ATOMIC);
0104 static DEFINE_SPINLOCK(qrtr_nodes_lock);
0105
0106 static LIST_HEAD(qrtr_all_nodes);
0107
0108 static DEFINE_MUTEX(qrtr_node_lock);
0109
0110
0111 static DEFINE_XARRAY_ALLOC(qrtr_ports);
0112
0113
0114
0115
0116
0117
0118
0119
0120
0121
0122
0123
0124 struct qrtr_node {
0125 struct mutex ep_lock;
0126 struct qrtr_endpoint *ep;
0127 struct kref ref;
0128 unsigned int nid;
0129
0130 struct radix_tree_root qrtr_tx_flow;
0131 struct mutex qrtr_tx_lock;
0132
0133 struct sk_buff_head rx_queue;
0134 struct list_head item;
0135 };
0136
0137
0138
0139
0140
0141
0142
0143 struct qrtr_tx_flow {
0144 struct wait_queue_head resume_tx;
0145 int pending;
0146 int tx_failed;
0147 };
0148
0149 #define QRTR_TX_FLOW_HIGH 10
0150 #define QRTR_TX_FLOW_LOW 5
0151
0152 static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb,
0153 int type, struct sockaddr_qrtr *from,
0154 struct sockaddr_qrtr *to);
0155 static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb,
0156 int type, struct sockaddr_qrtr *from,
0157 struct sockaddr_qrtr *to);
0158 static struct qrtr_sock *qrtr_port_lookup(int port);
0159 static void qrtr_port_put(struct qrtr_sock *ipc);
0160
0161
0162
0163
0164
0165
0166 static void __qrtr_node_release(struct kref *kref)
0167 {
0168 struct qrtr_node *node = container_of(kref, struct qrtr_node, ref);
0169 struct radix_tree_iter iter;
0170 struct qrtr_tx_flow *flow;
0171 unsigned long flags;
0172 void __rcu **slot;
0173
0174 spin_lock_irqsave(&qrtr_nodes_lock, flags);
0175
0176
0177
0178 radix_tree_for_each_slot(slot, &qrtr_nodes, &iter, 0) {
0179 if (*slot == node)
0180 radix_tree_iter_delete(&qrtr_nodes, &iter, slot);
0181 }
0182 spin_unlock_irqrestore(&qrtr_nodes_lock, flags);
0183
0184 list_del(&node->item);
0185 mutex_unlock(&qrtr_node_lock);
0186
0187 skb_queue_purge(&node->rx_queue);
0188
0189
0190 radix_tree_for_each_slot(slot, &node->qrtr_tx_flow, &iter, 0) {
0191 flow = *slot;
0192 radix_tree_iter_delete(&node->qrtr_tx_flow, &iter, slot);
0193 kfree(flow);
0194 }
0195 kfree(node);
0196 }
0197
0198
0199 static struct qrtr_node *qrtr_node_acquire(struct qrtr_node *node)
0200 {
0201 if (node)
0202 kref_get(&node->ref);
0203 return node;
0204 }
0205
0206
0207 static void qrtr_node_release(struct qrtr_node *node)
0208 {
0209 if (!node)
0210 return;
0211 kref_put_mutex(&node->ref, __qrtr_node_release, &qrtr_node_lock);
0212 }
0213
0214
0215
0216
0217
0218
0219 static void qrtr_tx_resume(struct qrtr_node *node, struct sk_buff *skb)
0220 {
0221 struct qrtr_ctrl_pkt *pkt = (struct qrtr_ctrl_pkt *)skb->data;
0222 u64 remote_node = le32_to_cpu(pkt->client.node);
0223 u32 remote_port = le32_to_cpu(pkt->client.port);
0224 struct qrtr_tx_flow *flow;
0225 unsigned long key;
0226
0227 key = remote_node << 32 | remote_port;
0228
0229 rcu_read_lock();
0230 flow = radix_tree_lookup(&node->qrtr_tx_flow, key);
0231 rcu_read_unlock();
0232 if (flow) {
0233 spin_lock(&flow->resume_tx.lock);
0234 flow->pending = 0;
0235 spin_unlock(&flow->resume_tx.lock);
0236 wake_up_interruptible_all(&flow->resume_tx);
0237 }
0238
0239 consume_skb(skb);
0240 }
0241
0242
0243
0244
0245
0246
0247
0248
0249
0250
0251
0252
0253
0254
0255
0256
0257 static int qrtr_tx_wait(struct qrtr_node *node, int dest_node, int dest_port,
0258 int type)
0259 {
0260 unsigned long key = (u64)dest_node << 32 | dest_port;
0261 struct qrtr_tx_flow *flow;
0262 int confirm_rx = 0;
0263 int ret;
0264
0265
0266 if (type != QRTR_TYPE_DATA)
0267 return 0;
0268
0269 mutex_lock(&node->qrtr_tx_lock);
0270 flow = radix_tree_lookup(&node->qrtr_tx_flow, key);
0271 if (!flow) {
0272 flow = kzalloc(sizeof(*flow), GFP_KERNEL);
0273 if (flow) {
0274 init_waitqueue_head(&flow->resume_tx);
0275 if (radix_tree_insert(&node->qrtr_tx_flow, key, flow)) {
0276 kfree(flow);
0277 flow = NULL;
0278 }
0279 }
0280 }
0281 mutex_unlock(&node->qrtr_tx_lock);
0282
0283
0284 if (!flow)
0285 return 1;
0286
0287 spin_lock_irq(&flow->resume_tx.lock);
0288 ret = wait_event_interruptible_locked_irq(flow->resume_tx,
0289 flow->pending < QRTR_TX_FLOW_HIGH ||
0290 flow->tx_failed ||
0291 !node->ep);
0292 if (ret < 0) {
0293 confirm_rx = ret;
0294 } else if (!node->ep) {
0295 confirm_rx = -EPIPE;
0296 } else if (flow->tx_failed) {
0297 flow->tx_failed = 0;
0298 confirm_rx = 1;
0299 } else {
0300 flow->pending++;
0301 confirm_rx = flow->pending == QRTR_TX_FLOW_LOW;
0302 }
0303 spin_unlock_irq(&flow->resume_tx.lock);
0304
0305 return confirm_rx;
0306 }
0307
0308
0309
0310
0311
0312
0313
0314
0315
0316
0317
0318
0319
0320
0321 static void qrtr_tx_flow_failed(struct qrtr_node *node, int dest_node,
0322 int dest_port)
0323 {
0324 unsigned long key = (u64)dest_node << 32 | dest_port;
0325 struct qrtr_tx_flow *flow;
0326
0327 rcu_read_lock();
0328 flow = radix_tree_lookup(&node->qrtr_tx_flow, key);
0329 rcu_read_unlock();
0330 if (flow) {
0331 spin_lock_irq(&flow->resume_tx.lock);
0332 flow->tx_failed = 1;
0333 spin_unlock_irq(&flow->resume_tx.lock);
0334 }
0335 }
0336
0337
0338 static int qrtr_node_enqueue(struct qrtr_node *node, struct sk_buff *skb,
0339 int type, struct sockaddr_qrtr *from,
0340 struct sockaddr_qrtr *to)
0341 {
0342 struct qrtr_hdr_v1 *hdr;
0343 size_t len = skb->len;
0344 int rc, confirm_rx;
0345
0346 confirm_rx = qrtr_tx_wait(node, to->sq_node, to->sq_port, type);
0347 if (confirm_rx < 0) {
0348 kfree_skb(skb);
0349 return confirm_rx;
0350 }
0351
0352 hdr = skb_push(skb, sizeof(*hdr));
0353 hdr->version = cpu_to_le32(QRTR_PROTO_VER_1);
0354 hdr->type = cpu_to_le32(type);
0355 hdr->src_node_id = cpu_to_le32(from->sq_node);
0356 hdr->src_port_id = cpu_to_le32(from->sq_port);
0357 if (to->sq_port == QRTR_PORT_CTRL) {
0358 hdr->dst_node_id = cpu_to_le32(node->nid);
0359 hdr->dst_port_id = cpu_to_le32(QRTR_PORT_CTRL);
0360 } else {
0361 hdr->dst_node_id = cpu_to_le32(to->sq_node);
0362 hdr->dst_port_id = cpu_to_le32(to->sq_port);
0363 }
0364
0365 hdr->size = cpu_to_le32(len);
0366 hdr->confirm_rx = !!confirm_rx;
0367
0368 rc = skb_put_padto(skb, ALIGN(len, 4) + sizeof(*hdr));
0369
0370 if (!rc) {
0371 mutex_lock(&node->ep_lock);
0372 rc = -ENODEV;
0373 if (node->ep)
0374 rc = node->ep->xmit(node->ep, skb);
0375 else
0376 kfree_skb(skb);
0377 mutex_unlock(&node->ep_lock);
0378 }
0379
0380
0381 if (rc && confirm_rx)
0382 qrtr_tx_flow_failed(node, to->sq_node, to->sq_port);
0383
0384 return rc;
0385 }
0386
0387
0388
0389
0390
0391 static struct qrtr_node *qrtr_node_lookup(unsigned int nid)
0392 {
0393 struct qrtr_node *node;
0394 unsigned long flags;
0395
0396 spin_lock_irqsave(&qrtr_nodes_lock, flags);
0397 node = radix_tree_lookup(&qrtr_nodes, nid);
0398 node = qrtr_node_acquire(node);
0399 spin_unlock_irqrestore(&qrtr_nodes_lock, flags);
0400
0401 return node;
0402 }
0403
0404
0405
0406
0407
0408
0409 static void qrtr_node_assign(struct qrtr_node *node, unsigned int nid)
0410 {
0411 unsigned long flags;
0412
0413 if (nid == QRTR_EP_NID_AUTO)
0414 return;
0415
0416 spin_lock_irqsave(&qrtr_nodes_lock, flags);
0417 radix_tree_insert(&qrtr_nodes, nid, node);
0418 if (node->nid == QRTR_EP_NID_AUTO)
0419 node->nid = nid;
0420 spin_unlock_irqrestore(&qrtr_nodes_lock, flags);
0421 }
0422
0423
0424
0425
0426
0427
0428
0429
0430
0431 int qrtr_endpoint_post(struct qrtr_endpoint *ep, const void *data, size_t len)
0432 {
0433 struct qrtr_node *node = ep->node;
0434 const struct qrtr_hdr_v1 *v1;
0435 const struct qrtr_hdr_v2 *v2;
0436 struct qrtr_sock *ipc;
0437 struct sk_buff *skb;
0438 struct qrtr_cb *cb;
0439 size_t size;
0440 unsigned int ver;
0441 size_t hdrlen;
0442
0443 if (len == 0 || len & 3)
0444 return -EINVAL;
0445
0446 skb = __netdev_alloc_skb(NULL, len, GFP_ATOMIC | __GFP_NOWARN);
0447 if (!skb)
0448 return -ENOMEM;
0449
0450 cb = (struct qrtr_cb *)skb->cb;
0451
0452
0453 ver = *(u8*)data;
0454
0455 switch (ver) {
0456 case QRTR_PROTO_VER_1:
0457 if (len < sizeof(*v1))
0458 goto err;
0459 v1 = data;
0460 hdrlen = sizeof(*v1);
0461
0462 cb->type = le32_to_cpu(v1->type);
0463 cb->src_node = le32_to_cpu(v1->src_node_id);
0464 cb->src_port = le32_to_cpu(v1->src_port_id);
0465 cb->confirm_rx = !!v1->confirm_rx;
0466 cb->dst_node = le32_to_cpu(v1->dst_node_id);
0467 cb->dst_port = le32_to_cpu(v1->dst_port_id);
0468
0469 size = le32_to_cpu(v1->size);
0470 break;
0471 case QRTR_PROTO_VER_2:
0472 if (len < sizeof(*v2))
0473 goto err;
0474 v2 = data;
0475 hdrlen = sizeof(*v2) + v2->optlen;
0476
0477 cb->type = v2->type;
0478 cb->confirm_rx = !!(v2->flags & QRTR_FLAGS_CONFIRM_RX);
0479 cb->src_node = le16_to_cpu(v2->src_node_id);
0480 cb->src_port = le16_to_cpu(v2->src_port_id);
0481 cb->dst_node = le16_to_cpu(v2->dst_node_id);
0482 cb->dst_port = le16_to_cpu(v2->dst_port_id);
0483
0484 if (cb->src_port == (u16)QRTR_PORT_CTRL)
0485 cb->src_port = QRTR_PORT_CTRL;
0486 if (cb->dst_port == (u16)QRTR_PORT_CTRL)
0487 cb->dst_port = QRTR_PORT_CTRL;
0488
0489 size = le32_to_cpu(v2->size);
0490 break;
0491 default:
0492 pr_err("qrtr: Invalid version %d\n", ver);
0493 goto err;
0494 }
0495
0496 if (!size || len != ALIGN(size, 4) + hdrlen)
0497 goto err;
0498
0499 if (cb->dst_port != QRTR_PORT_CTRL && cb->type != QRTR_TYPE_DATA &&
0500 cb->type != QRTR_TYPE_RESUME_TX)
0501 goto err;
0502
0503 skb_put_data(skb, data + hdrlen, size);
0504
0505 qrtr_node_assign(node, cb->src_node);
0506
0507 if (cb->type == QRTR_TYPE_NEW_SERVER) {
0508
0509 const struct qrtr_ctrl_pkt *pkt;
0510
0511 if (size < sizeof(*pkt))
0512 goto err;
0513
0514 pkt = data + hdrlen;
0515 qrtr_node_assign(node, le32_to_cpu(pkt->server.node));
0516 }
0517
0518 if (cb->type == QRTR_TYPE_RESUME_TX) {
0519 qrtr_tx_resume(node, skb);
0520 } else {
0521 ipc = qrtr_port_lookup(cb->dst_port);
0522 if (!ipc)
0523 goto err;
0524
0525 if (sock_queue_rcv_skb(&ipc->sk, skb)) {
0526 qrtr_port_put(ipc);
0527 goto err;
0528 }
0529
0530 qrtr_port_put(ipc);
0531 }
0532
0533 return 0;
0534
0535 err:
0536 kfree_skb(skb);
0537 return -EINVAL;
0538
0539 }
0540 EXPORT_SYMBOL_GPL(qrtr_endpoint_post);
0541
0542
0543
0544
0545
0546
0547
0548
0549
0550
0551
0552 static struct sk_buff *qrtr_alloc_ctrl_packet(struct qrtr_ctrl_pkt **pkt,
0553 gfp_t flags)
0554 {
0555 const int pkt_len = sizeof(struct qrtr_ctrl_pkt);
0556 struct sk_buff *skb;
0557
0558 skb = alloc_skb(QRTR_HDR_MAX_SIZE + pkt_len, flags);
0559 if (!skb)
0560 return NULL;
0561
0562 skb_reserve(skb, QRTR_HDR_MAX_SIZE);
0563 *pkt = skb_put_zero(skb, pkt_len);
0564
0565 return skb;
0566 }
0567
0568
0569
0570
0571
0572
0573
0574
0575
0576 int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int nid)
0577 {
0578 struct qrtr_node *node;
0579
0580 if (!ep || !ep->xmit)
0581 return -EINVAL;
0582
0583 node = kzalloc(sizeof(*node), GFP_KERNEL);
0584 if (!node)
0585 return -ENOMEM;
0586
0587 kref_init(&node->ref);
0588 mutex_init(&node->ep_lock);
0589 skb_queue_head_init(&node->rx_queue);
0590 node->nid = QRTR_EP_NID_AUTO;
0591 node->ep = ep;
0592
0593 INIT_RADIX_TREE(&node->qrtr_tx_flow, GFP_KERNEL);
0594 mutex_init(&node->qrtr_tx_lock);
0595
0596 qrtr_node_assign(node, nid);
0597
0598 mutex_lock(&qrtr_node_lock);
0599 list_add(&node->item, &qrtr_all_nodes);
0600 mutex_unlock(&qrtr_node_lock);
0601 ep->node = node;
0602
0603 return 0;
0604 }
0605 EXPORT_SYMBOL_GPL(qrtr_endpoint_register);
0606
0607
0608
0609
0610
0611 void qrtr_endpoint_unregister(struct qrtr_endpoint *ep)
0612 {
0613 struct qrtr_node *node = ep->node;
0614 struct sockaddr_qrtr src = {AF_QIPCRTR, node->nid, QRTR_PORT_CTRL};
0615 struct sockaddr_qrtr dst = {AF_QIPCRTR, qrtr_local_nid, QRTR_PORT_CTRL};
0616 struct radix_tree_iter iter;
0617 struct qrtr_ctrl_pkt *pkt;
0618 struct qrtr_tx_flow *flow;
0619 struct sk_buff *skb;
0620 unsigned long flags;
0621 void __rcu **slot;
0622
0623 mutex_lock(&node->ep_lock);
0624 node->ep = NULL;
0625 mutex_unlock(&node->ep_lock);
0626
0627
0628 spin_lock_irqsave(&qrtr_nodes_lock, flags);
0629 radix_tree_for_each_slot(slot, &qrtr_nodes, &iter, 0) {
0630 if (*slot != node)
0631 continue;
0632 src.sq_node = iter.index;
0633 skb = qrtr_alloc_ctrl_packet(&pkt, GFP_ATOMIC);
0634 if (skb) {
0635 pkt->cmd = cpu_to_le32(QRTR_TYPE_BYE);
0636 qrtr_local_enqueue(NULL, skb, QRTR_TYPE_BYE, &src, &dst);
0637 }
0638 }
0639 spin_unlock_irqrestore(&qrtr_nodes_lock, flags);
0640
0641
0642 mutex_lock(&node->qrtr_tx_lock);
0643 radix_tree_for_each_slot(slot, &node->qrtr_tx_flow, &iter, 0) {
0644 flow = *slot;
0645 wake_up_interruptible_all(&flow->resume_tx);
0646 }
0647 mutex_unlock(&node->qrtr_tx_lock);
0648
0649 qrtr_node_release(node);
0650 ep->node = NULL;
0651 }
0652 EXPORT_SYMBOL_GPL(qrtr_endpoint_unregister);
0653
0654
0655
0656
0657
0658 static struct qrtr_sock *qrtr_port_lookup(int port)
0659 {
0660 struct qrtr_sock *ipc;
0661
0662 if (port == QRTR_PORT_CTRL)
0663 port = 0;
0664
0665 rcu_read_lock();
0666 ipc = xa_load(&qrtr_ports, port);
0667 if (ipc)
0668 sock_hold(&ipc->sk);
0669 rcu_read_unlock();
0670
0671 return ipc;
0672 }
0673
0674
0675 static void qrtr_port_put(struct qrtr_sock *ipc)
0676 {
0677 sock_put(&ipc->sk);
0678 }
0679
0680
0681 static void qrtr_port_remove(struct qrtr_sock *ipc)
0682 {
0683 struct qrtr_ctrl_pkt *pkt;
0684 struct sk_buff *skb;
0685 int port = ipc->us.sq_port;
0686 struct sockaddr_qrtr to;
0687
0688 to.sq_family = AF_QIPCRTR;
0689 to.sq_node = QRTR_NODE_BCAST;
0690 to.sq_port = QRTR_PORT_CTRL;
0691
0692 skb = qrtr_alloc_ctrl_packet(&pkt, GFP_KERNEL);
0693 if (skb) {
0694 pkt->cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);
0695 pkt->client.node = cpu_to_le32(ipc->us.sq_node);
0696 pkt->client.port = cpu_to_le32(ipc->us.sq_port);
0697
0698 skb_set_owner_w(skb, &ipc->sk);
0699 qrtr_bcast_enqueue(NULL, skb, QRTR_TYPE_DEL_CLIENT, &ipc->us,
0700 &to);
0701 }
0702
0703 if (port == QRTR_PORT_CTRL)
0704 port = 0;
0705
0706 __sock_put(&ipc->sk);
0707
0708 xa_erase(&qrtr_ports, port);
0709
0710
0711
0712 synchronize_rcu();
0713 }
0714
0715
0716
0717
0718
0719
0720
0721
0722
0723
0724
0725 static int qrtr_port_assign(struct qrtr_sock *ipc, int *port)
0726 {
0727 int rc;
0728
0729 if (!*port) {
0730 rc = xa_alloc(&qrtr_ports, port, ipc, QRTR_EPH_PORT_RANGE,
0731 GFP_KERNEL);
0732 } else if (*port < QRTR_MIN_EPH_SOCKET && !capable(CAP_NET_ADMIN)) {
0733 rc = -EACCES;
0734 } else if (*port == QRTR_PORT_CTRL) {
0735 rc = xa_insert(&qrtr_ports, 0, ipc, GFP_KERNEL);
0736 } else {
0737 rc = xa_insert(&qrtr_ports, *port, ipc, GFP_KERNEL);
0738 }
0739
0740 if (rc == -EBUSY)
0741 return -EADDRINUSE;
0742 else if (rc < 0)
0743 return rc;
0744
0745 sock_hold(&ipc->sk);
0746
0747 return 0;
0748 }
0749
0750
0751 static void qrtr_reset_ports(void)
0752 {
0753 struct qrtr_sock *ipc;
0754 unsigned long index;
0755
0756 rcu_read_lock();
0757 xa_for_each_start(&qrtr_ports, index, ipc, 1) {
0758 sock_hold(&ipc->sk);
0759 ipc->sk.sk_err = ENETRESET;
0760 sk_error_report(&ipc->sk);
0761 sock_put(&ipc->sk);
0762 }
0763 rcu_read_unlock();
0764 }
0765
0766
0767
0768
0769
0770 static int __qrtr_bind(struct socket *sock,
0771 const struct sockaddr_qrtr *addr, int zapped)
0772 {
0773 struct qrtr_sock *ipc = qrtr_sk(sock->sk);
0774 struct sock *sk = sock->sk;
0775 int port;
0776 int rc;
0777
0778
0779 if (!zapped && addr->sq_port == ipc->us.sq_port)
0780 return 0;
0781
0782 port = addr->sq_port;
0783 rc = qrtr_port_assign(ipc, &port);
0784 if (rc)
0785 return rc;
0786
0787
0788 if (!zapped)
0789 qrtr_port_remove(ipc);
0790 ipc->us.sq_port = port;
0791
0792 sock_reset_flag(sk, SOCK_ZAPPED);
0793
0794
0795 if (port == QRTR_PORT_CTRL)
0796 qrtr_reset_ports();
0797
0798 return 0;
0799 }
0800
0801
0802 static int qrtr_autobind(struct socket *sock)
0803 {
0804 struct sock *sk = sock->sk;
0805 struct sockaddr_qrtr addr;
0806
0807 if (!sock_flag(sk, SOCK_ZAPPED))
0808 return 0;
0809
0810 addr.sq_family = AF_QIPCRTR;
0811 addr.sq_node = qrtr_local_nid;
0812 addr.sq_port = 0;
0813
0814 return __qrtr_bind(sock, &addr, 1);
0815 }
0816
0817
0818 static int qrtr_bind(struct socket *sock, struct sockaddr *saddr, int len)
0819 {
0820 DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, saddr);
0821 struct qrtr_sock *ipc = qrtr_sk(sock->sk);
0822 struct sock *sk = sock->sk;
0823 int rc;
0824
0825 if (len < sizeof(*addr) || addr->sq_family != AF_QIPCRTR)
0826 return -EINVAL;
0827
0828 if (addr->sq_node != ipc->us.sq_node)
0829 return -EINVAL;
0830
0831 lock_sock(sk);
0832 rc = __qrtr_bind(sock, addr, sock_flag(sk, SOCK_ZAPPED));
0833 release_sock(sk);
0834
0835 return rc;
0836 }
0837
0838
0839 static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb,
0840 int type, struct sockaddr_qrtr *from,
0841 struct sockaddr_qrtr *to)
0842 {
0843 struct qrtr_sock *ipc;
0844 struct qrtr_cb *cb;
0845
0846 ipc = qrtr_port_lookup(to->sq_port);
0847 if (!ipc || &ipc->sk == skb->sk) {
0848 if (ipc)
0849 qrtr_port_put(ipc);
0850 kfree_skb(skb);
0851 return -ENODEV;
0852 }
0853
0854 cb = (struct qrtr_cb *)skb->cb;
0855 cb->src_node = from->sq_node;
0856 cb->src_port = from->sq_port;
0857
0858 if (sock_queue_rcv_skb(&ipc->sk, skb)) {
0859 qrtr_port_put(ipc);
0860 kfree_skb(skb);
0861 return -ENOSPC;
0862 }
0863
0864 qrtr_port_put(ipc);
0865
0866 return 0;
0867 }
0868
0869
0870 static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb,
0871 int type, struct sockaddr_qrtr *from,
0872 struct sockaddr_qrtr *to)
0873 {
0874 struct sk_buff *skbn;
0875
0876 mutex_lock(&qrtr_node_lock);
0877 list_for_each_entry(node, &qrtr_all_nodes, item) {
0878 skbn = skb_clone(skb, GFP_KERNEL);
0879 if (!skbn)
0880 break;
0881 skb_set_owner_w(skbn, skb->sk);
0882 qrtr_node_enqueue(node, skbn, type, from, to);
0883 }
0884 mutex_unlock(&qrtr_node_lock);
0885
0886 qrtr_local_enqueue(NULL, skb, type, from, to);
0887
0888 return 0;
0889 }
0890
0891 static int qrtr_sendmsg(struct socket *sock, struct msghdr *msg, size_t len)
0892 {
0893 DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, msg->msg_name);
0894 int (*enqueue_fn)(struct qrtr_node *, struct sk_buff *, int,
0895 struct sockaddr_qrtr *, struct sockaddr_qrtr *);
0896 __le32 qrtr_type = cpu_to_le32(QRTR_TYPE_DATA);
0897 struct qrtr_sock *ipc = qrtr_sk(sock->sk);
0898 struct sock *sk = sock->sk;
0899 struct qrtr_node *node;
0900 struct sk_buff *skb;
0901 size_t plen;
0902 u32 type;
0903 int rc;
0904
0905 if (msg->msg_flags & ~(MSG_DONTWAIT))
0906 return -EINVAL;
0907
0908 if (len > 65535)
0909 return -EMSGSIZE;
0910
0911 lock_sock(sk);
0912
0913 if (addr) {
0914 if (msg->msg_namelen < sizeof(*addr)) {
0915 release_sock(sk);
0916 return -EINVAL;
0917 }
0918
0919 if (addr->sq_family != AF_QIPCRTR) {
0920 release_sock(sk);
0921 return -EINVAL;
0922 }
0923
0924 rc = qrtr_autobind(sock);
0925 if (rc) {
0926 release_sock(sk);
0927 return rc;
0928 }
0929 } else if (sk->sk_state == TCP_ESTABLISHED) {
0930 addr = &ipc->peer;
0931 } else {
0932 release_sock(sk);
0933 return -ENOTCONN;
0934 }
0935
0936 node = NULL;
0937 if (addr->sq_node == QRTR_NODE_BCAST) {
0938 if (addr->sq_port != QRTR_PORT_CTRL &&
0939 qrtr_local_nid != QRTR_NODE_BCAST) {
0940 release_sock(sk);
0941 return -ENOTCONN;
0942 }
0943 enqueue_fn = qrtr_bcast_enqueue;
0944 } else if (addr->sq_node == ipc->us.sq_node) {
0945 enqueue_fn = qrtr_local_enqueue;
0946 } else {
0947 node = qrtr_node_lookup(addr->sq_node);
0948 if (!node) {
0949 release_sock(sk);
0950 return -ECONNRESET;
0951 }
0952 enqueue_fn = qrtr_node_enqueue;
0953 }
0954
0955 plen = (len + 3) & ~3;
0956 skb = sock_alloc_send_skb(sk, plen + QRTR_HDR_MAX_SIZE,
0957 msg->msg_flags & MSG_DONTWAIT, &rc);
0958 if (!skb) {
0959 rc = -ENOMEM;
0960 goto out_node;
0961 }
0962
0963 skb_reserve(skb, QRTR_HDR_MAX_SIZE);
0964
0965 rc = memcpy_from_msg(skb_put(skb, len), msg, len);
0966 if (rc) {
0967 kfree_skb(skb);
0968 goto out_node;
0969 }
0970
0971 if (ipc->us.sq_port == QRTR_PORT_CTRL) {
0972 if (len < 4) {
0973 rc = -EINVAL;
0974 kfree_skb(skb);
0975 goto out_node;
0976 }
0977
0978
0979 skb_copy_bits(skb, 0, &qrtr_type, 4);
0980 }
0981
0982 type = le32_to_cpu(qrtr_type);
0983 rc = enqueue_fn(node, skb, type, &ipc->us, addr);
0984 if (rc >= 0)
0985 rc = len;
0986
0987 out_node:
0988 qrtr_node_release(node);
0989 release_sock(sk);
0990
0991 return rc;
0992 }
0993
0994 static int qrtr_send_resume_tx(struct qrtr_cb *cb)
0995 {
0996 struct sockaddr_qrtr remote = { AF_QIPCRTR, cb->src_node, cb->src_port };
0997 struct sockaddr_qrtr local = { AF_QIPCRTR, cb->dst_node, cb->dst_port };
0998 struct qrtr_ctrl_pkt *pkt;
0999 struct qrtr_node *node;
1000 struct sk_buff *skb;
1001 int ret;
1002
1003 node = qrtr_node_lookup(remote.sq_node);
1004 if (!node)
1005 return -EINVAL;
1006
1007 skb = qrtr_alloc_ctrl_packet(&pkt, GFP_KERNEL);
1008 if (!skb)
1009 return -ENOMEM;
1010
1011 pkt->cmd = cpu_to_le32(QRTR_TYPE_RESUME_TX);
1012 pkt->client.node = cpu_to_le32(cb->dst_node);
1013 pkt->client.port = cpu_to_le32(cb->dst_port);
1014
1015 ret = qrtr_node_enqueue(node, skb, QRTR_TYPE_RESUME_TX, &local, &remote);
1016
1017 qrtr_node_release(node);
1018
1019 return ret;
1020 }
1021
1022 static int qrtr_recvmsg(struct socket *sock, struct msghdr *msg,
1023 size_t size, int flags)
1024 {
1025 DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, msg->msg_name);
1026 struct sock *sk = sock->sk;
1027 struct sk_buff *skb;
1028 struct qrtr_cb *cb;
1029 int copied, rc;
1030
1031 lock_sock(sk);
1032
1033 if (sock_flag(sk, SOCK_ZAPPED)) {
1034 release_sock(sk);
1035 return -EADDRNOTAVAIL;
1036 }
1037
1038 skb = skb_recv_datagram(sk, flags, &rc);
1039 if (!skb) {
1040 release_sock(sk);
1041 return rc;
1042 }
1043 cb = (struct qrtr_cb *)skb->cb;
1044
1045 copied = skb->len;
1046 if (copied > size) {
1047 copied = size;
1048 msg->msg_flags |= MSG_TRUNC;
1049 }
1050
1051 rc = skb_copy_datagram_msg(skb, 0, msg, copied);
1052 if (rc < 0)
1053 goto out;
1054 rc = copied;
1055
1056 if (addr) {
1057
1058
1059
1060 memset(addr, 0, sizeof(*addr));
1061
1062 addr->sq_family = AF_QIPCRTR;
1063 addr->sq_node = cb->src_node;
1064 addr->sq_port = cb->src_port;
1065 msg->msg_namelen = sizeof(*addr);
1066 }
1067
1068 out:
1069 if (cb->confirm_rx)
1070 qrtr_send_resume_tx(cb);
1071
1072 skb_free_datagram(sk, skb);
1073 release_sock(sk);
1074
1075 return rc;
1076 }
1077
1078 static int qrtr_connect(struct socket *sock, struct sockaddr *saddr,
1079 int len, int flags)
1080 {
1081 DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, saddr);
1082 struct qrtr_sock *ipc = qrtr_sk(sock->sk);
1083 struct sock *sk = sock->sk;
1084 int rc;
1085
1086 if (len < sizeof(*addr) || addr->sq_family != AF_QIPCRTR)
1087 return -EINVAL;
1088
1089 lock_sock(sk);
1090
1091 sk->sk_state = TCP_CLOSE;
1092 sock->state = SS_UNCONNECTED;
1093
1094 rc = qrtr_autobind(sock);
1095 if (rc) {
1096 release_sock(sk);
1097 return rc;
1098 }
1099
1100 ipc->peer = *addr;
1101 sock->state = SS_CONNECTED;
1102 sk->sk_state = TCP_ESTABLISHED;
1103
1104 release_sock(sk);
1105
1106 return 0;
1107 }
1108
1109 static int qrtr_getname(struct socket *sock, struct sockaddr *saddr,
1110 int peer)
1111 {
1112 struct qrtr_sock *ipc = qrtr_sk(sock->sk);
1113 struct sockaddr_qrtr qaddr;
1114 struct sock *sk = sock->sk;
1115
1116 lock_sock(sk);
1117 if (peer) {
1118 if (sk->sk_state != TCP_ESTABLISHED) {
1119 release_sock(sk);
1120 return -ENOTCONN;
1121 }
1122
1123 qaddr = ipc->peer;
1124 } else {
1125 qaddr = ipc->us;
1126 }
1127 release_sock(sk);
1128
1129 qaddr.sq_family = AF_QIPCRTR;
1130
1131 memcpy(saddr, &qaddr, sizeof(qaddr));
1132
1133 return sizeof(qaddr);
1134 }
1135
1136 static int qrtr_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg)
1137 {
1138 void __user *argp = (void __user *)arg;
1139 struct qrtr_sock *ipc = qrtr_sk(sock->sk);
1140 struct sock *sk = sock->sk;
1141 struct sockaddr_qrtr *sq;
1142 struct sk_buff *skb;
1143 struct ifreq ifr;
1144 long len = 0;
1145 int rc = 0;
1146
1147 lock_sock(sk);
1148
1149 switch (cmd) {
1150 case TIOCOUTQ:
1151 len = sk->sk_sndbuf - sk_wmem_alloc_get(sk);
1152 if (len < 0)
1153 len = 0;
1154 rc = put_user(len, (int __user *)argp);
1155 break;
1156 case TIOCINQ:
1157 skb = skb_peek(&sk->sk_receive_queue);
1158 if (skb)
1159 len = skb->len;
1160 rc = put_user(len, (int __user *)argp);
1161 break;
1162 case SIOCGIFADDR:
1163 if (get_user_ifreq(&ifr, NULL, argp)) {
1164 rc = -EFAULT;
1165 break;
1166 }
1167
1168 sq = (struct sockaddr_qrtr *)&ifr.ifr_addr;
1169 *sq = ipc->us;
1170 if (put_user_ifreq(&ifr, argp)) {
1171 rc = -EFAULT;
1172 break;
1173 }
1174 break;
1175 case SIOCADDRT:
1176 case SIOCDELRT:
1177 case SIOCSIFADDR:
1178 case SIOCGIFDSTADDR:
1179 case SIOCSIFDSTADDR:
1180 case SIOCGIFBRDADDR:
1181 case SIOCSIFBRDADDR:
1182 case SIOCGIFNETMASK:
1183 case SIOCSIFNETMASK:
1184 rc = -EINVAL;
1185 break;
1186 default:
1187 rc = -ENOIOCTLCMD;
1188 break;
1189 }
1190
1191 release_sock(sk);
1192
1193 return rc;
1194 }
1195
1196 static int qrtr_release(struct socket *sock)
1197 {
1198 struct sock *sk = sock->sk;
1199 struct qrtr_sock *ipc;
1200
1201 if (!sk)
1202 return 0;
1203
1204 lock_sock(sk);
1205
1206 ipc = qrtr_sk(sk);
1207 sk->sk_shutdown = SHUTDOWN_MASK;
1208 if (!sock_flag(sk, SOCK_DEAD))
1209 sk->sk_state_change(sk);
1210
1211 sock_set_flag(sk, SOCK_DEAD);
1212 sock_orphan(sk);
1213 sock->sk = NULL;
1214
1215 if (!sock_flag(sk, SOCK_ZAPPED))
1216 qrtr_port_remove(ipc);
1217
1218 skb_queue_purge(&sk->sk_receive_queue);
1219
1220 release_sock(sk);
1221 sock_put(sk);
1222
1223 return 0;
1224 }
1225
1226 static const struct proto_ops qrtr_proto_ops = {
1227 .owner = THIS_MODULE,
1228 .family = AF_QIPCRTR,
1229 .bind = qrtr_bind,
1230 .connect = qrtr_connect,
1231 .socketpair = sock_no_socketpair,
1232 .accept = sock_no_accept,
1233 .listen = sock_no_listen,
1234 .sendmsg = qrtr_sendmsg,
1235 .recvmsg = qrtr_recvmsg,
1236 .getname = qrtr_getname,
1237 .ioctl = qrtr_ioctl,
1238 .gettstamp = sock_gettstamp,
1239 .poll = datagram_poll,
1240 .shutdown = sock_no_shutdown,
1241 .release = qrtr_release,
1242 .mmap = sock_no_mmap,
1243 .sendpage = sock_no_sendpage,
1244 };
1245
1246 static struct proto qrtr_proto = {
1247 .name = "QIPCRTR",
1248 .owner = THIS_MODULE,
1249 .obj_size = sizeof(struct qrtr_sock),
1250 };
1251
1252 static int qrtr_create(struct net *net, struct socket *sock,
1253 int protocol, int kern)
1254 {
1255 struct qrtr_sock *ipc;
1256 struct sock *sk;
1257
1258 if (sock->type != SOCK_DGRAM)
1259 return -EPROTOTYPE;
1260
1261 sk = sk_alloc(net, AF_QIPCRTR, GFP_KERNEL, &qrtr_proto, kern);
1262 if (!sk)
1263 return -ENOMEM;
1264
1265 sock_set_flag(sk, SOCK_ZAPPED);
1266
1267 sock_init_data(sock, sk);
1268 sock->ops = &qrtr_proto_ops;
1269
1270 ipc = qrtr_sk(sk);
1271 ipc->us.sq_family = AF_QIPCRTR;
1272 ipc->us.sq_node = qrtr_local_nid;
1273 ipc->us.sq_port = 0;
1274
1275 return 0;
1276 }
1277
1278 static const struct net_proto_family qrtr_family = {
1279 .owner = THIS_MODULE,
1280 .family = AF_QIPCRTR,
1281 .create = qrtr_create,
1282 };
1283
1284 static int __init qrtr_proto_init(void)
1285 {
1286 int rc;
1287
1288 rc = proto_register(&qrtr_proto, 1);
1289 if (rc)
1290 return rc;
1291
1292 rc = sock_register(&qrtr_family);
1293 if (rc)
1294 goto err_proto;
1295
1296 rc = qrtr_ns_init();
1297 if (rc)
1298 goto err_sock;
1299
1300 return 0;
1301
1302 err_sock:
1303 sock_unregister(qrtr_family.family);
1304 err_proto:
1305 proto_unregister(&qrtr_proto);
1306 return rc;
1307 }
1308 postcore_initcall(qrtr_proto_init);
1309
1310 static void __exit qrtr_proto_fini(void)
1311 {
1312 qrtr_ns_remove();
1313 sock_unregister(qrtr_family.family);
1314 proto_unregister(&qrtr_proto);
1315 }
1316 module_exit(qrtr_proto_fini);
1317
1318 MODULE_DESCRIPTION("Qualcomm IPC-router driver");
1319 MODULE_LICENSE("GPL v2");
1320 MODULE_ALIAS_NETPROTO(PF_QIPCRTR);