@@ -97,8 +97,12 @@ struct qrtr_node {
97
97
struct list_head item ;
98
98
};
99
99
100
- static int qrtr_local_enqueue (struct qrtr_node * node , struct sk_buff * skb );
101
- static int qrtr_bcast_enqueue (struct qrtr_node * node , struct sk_buff * skb );
100
+ static int qrtr_local_enqueue (struct qrtr_node * node , struct sk_buff * skb ,
101
+ int type , struct sockaddr_qrtr * from ,
102
+ struct sockaddr_qrtr * to );
103
+ static int qrtr_bcast_enqueue (struct qrtr_node * node , struct sk_buff * skb ,
104
+ int type , struct sockaddr_qrtr * from ,
105
+ struct sockaddr_qrtr * to );
102
106
103
107
/* Release node resources and free the node.
104
108
*
@@ -136,10 +140,27 @@ static void qrtr_node_release(struct qrtr_node *node)
136
140
}
137
141
138
142
/* Pass an outgoing packet socket buffer to the endpoint driver. */
139
- static int qrtr_node_enqueue (struct qrtr_node * node , struct sk_buff * skb )
143
+ static int qrtr_node_enqueue (struct qrtr_node * node , struct sk_buff * skb ,
144
+ int type , struct sockaddr_qrtr * from ,
145
+ struct sockaddr_qrtr * to )
140
146
{
147
+ struct qrtr_hdr * hdr ;
148
+ size_t len = skb -> len ;
141
149
int rc = - ENODEV ;
142
150
151
+ hdr = skb_push (skb , QRTR_HDR_SIZE );
152
+ hdr -> version = cpu_to_le32 (QRTR_PROTO_VER );
153
+ hdr -> type = cpu_to_le32 (type );
154
+ hdr -> src_node_id = cpu_to_le32 (from -> sq_node );
155
+ hdr -> src_port_id = cpu_to_le32 (from -> sq_port );
156
+ hdr -> dst_node_id = cpu_to_le32 (to -> sq_node );
157
+ hdr -> dst_port_id = cpu_to_le32 (to -> sq_port );
158
+
159
+ hdr -> size = cpu_to_le32 (len );
160
+ hdr -> confirm_rx = 0 ;
161
+
162
+ skb_put_padto (skb , ALIGN (len , 4 ));
163
+
143
164
mutex_lock (& node -> ep_lock );
144
165
if (node -> ep )
145
166
rc = node -> ep -> xmit (node -> ep , skb );
@@ -237,23 +258,13 @@ EXPORT_SYMBOL_GPL(qrtr_endpoint_post);
237
258
static struct sk_buff * qrtr_alloc_ctrl_packet (u32 type , size_t pkt_len ,
238
259
u32 src_node , u32 dst_node )
239
260
{
240
- struct qrtr_hdr * hdr ;
241
261
struct sk_buff * skb ;
242
262
243
263
skb = alloc_skb (QRTR_HDR_SIZE + pkt_len , GFP_KERNEL );
244
264
if (!skb )
245
265
return NULL ;
246
- skb_reset_transport_header (skb );
247
266
248
- hdr = skb_put (skb , QRTR_HDR_SIZE );
249
- hdr -> version = cpu_to_le32 (QRTR_PROTO_VER );
250
- hdr -> type = cpu_to_le32 (type );
251
- hdr -> src_node_id = cpu_to_le32 (src_node );
252
- hdr -> src_port_id = cpu_to_le32 (QRTR_PORT_CTRL );
253
- hdr -> confirm_rx = cpu_to_le32 (0 );
254
- hdr -> size = cpu_to_le32 (pkt_len );
255
- hdr -> dst_node_id = cpu_to_le32 (dst_node );
256
- hdr -> dst_port_id = cpu_to_le32 (QRTR_PORT_CTRL );
267
+ skb_reserve (skb , QRTR_HDR_SIZE );
257
268
258
269
return skb ;
259
270
}
@@ -326,6 +337,8 @@ static void qrtr_port_put(struct qrtr_sock *ipc);
326
337
static void qrtr_node_rx_work (struct work_struct * work )
327
338
{
328
339
struct qrtr_node * node = container_of (work , struct qrtr_node , work );
340
+ struct sockaddr_qrtr dst ;
341
+ struct sockaddr_qrtr src ;
329
342
struct sk_buff * skb ;
330
343
331
344
while ((skb = skb_dequeue (& node -> rx_queue )) != NULL ) {
@@ -341,6 +354,11 @@ static void qrtr_node_rx_work(struct work_struct *work)
341
354
dst_port = le32_to_cpu (phdr -> dst_port_id );
342
355
confirm = !!phdr -> confirm_rx ;
343
356
357
+ src .sq_node = src_node ;
358
+ src .sq_port = le32_to_cpu (phdr -> src_port_id );
359
+ dst .sq_node = dst_node ;
360
+ dst .sq_port = dst_port ;
361
+
344
362
qrtr_node_assign (node , src_node );
345
363
346
364
ipc = qrtr_port_lookup (dst_port );
@@ -357,7 +375,9 @@ static void qrtr_node_rx_work(struct work_struct *work)
357
375
skb = qrtr_alloc_resume_tx (dst_node , node -> nid , dst_port );
358
376
if (!skb )
359
377
break ;
360
- if (qrtr_node_enqueue (node , skb ))
378
+
379
+ if (qrtr_node_enqueue (node , skb , QRTR_TYPE_RESUME_TX ,
380
+ & dst , & src ))
361
381
break ;
362
382
}
363
383
}
@@ -407,6 +427,8 @@ EXPORT_SYMBOL_GPL(qrtr_endpoint_register);
407
427
void qrtr_endpoint_unregister (struct qrtr_endpoint * ep )
408
428
{
409
429
struct qrtr_node * node = ep -> node ;
430
+ struct sockaddr_qrtr src = {AF_QIPCRTR , node -> nid , QRTR_PORT_CTRL };
431
+ struct sockaddr_qrtr dst = {AF_QIPCRTR , qrtr_local_nid , QRTR_PORT_CTRL };
410
432
struct sk_buff * skb ;
411
433
412
434
mutex_lock (& node -> ep_lock );
@@ -416,7 +438,7 @@ void qrtr_endpoint_unregister(struct qrtr_endpoint *ep)
416
438
/* Notify the local controller about the event */
417
439
skb = qrtr_alloc_local_bye (node -> nid );
418
440
if (skb )
419
- qrtr_local_enqueue (NULL , skb );
441
+ qrtr_local_enqueue (NULL , skb , QRTR_TYPE_BYE , & src , & dst );
420
442
421
443
qrtr_node_release (node );
422
444
ep -> node = NULL ;
@@ -454,11 +476,17 @@ static void qrtr_port_remove(struct qrtr_sock *ipc)
454
476
{
455
477
struct sk_buff * skb ;
456
478
int port = ipc -> us .sq_port ;
479
+ struct sockaddr_qrtr to ;
480
+
481
+ to .sq_family = AF_QIPCRTR ;
482
+ to .sq_node = QRTR_NODE_BCAST ;
483
+ to .sq_port = QRTR_PORT_CTRL ;
457
484
458
485
skb = qrtr_alloc_del_client (& ipc -> us );
459
486
if (skb ) {
460
487
skb_set_owner_w (skb , & ipc -> sk );
461
- qrtr_bcast_enqueue (NULL , skb );
488
+ qrtr_bcast_enqueue (NULL , skb , QRTR_TYPE_DEL_CLIENT , & ipc -> us ,
489
+ & to );
462
490
}
463
491
464
492
if (port == QRTR_PORT_CTRL )
@@ -606,19 +634,25 @@ static int qrtr_bind(struct socket *sock, struct sockaddr *saddr, int len)
606
634
}
607
635
608
636
/* Queue packet to local peer socket. */
609
- static int qrtr_local_enqueue (struct qrtr_node * node , struct sk_buff * skb )
637
+ static int qrtr_local_enqueue (struct qrtr_node * node , struct sk_buff * skb ,
638
+ int type , struct sockaddr_qrtr * from ,
639
+ struct sockaddr_qrtr * to )
610
640
{
611
- const struct qrtr_hdr * phdr ;
612
641
struct qrtr_sock * ipc ;
642
+ struct qrtr_hdr * phdr ;
613
643
614
- phdr = (const struct qrtr_hdr * )skb_transport_header (skb );
615
-
616
- ipc = qrtr_port_lookup (le32_to_cpu (phdr -> dst_port_id ));
644
+ ipc = qrtr_port_lookup (to -> sq_port );
617
645
if (!ipc || & ipc -> sk == skb -> sk ) { /* do not send to self */
618
646
kfree_skb (skb );
619
647
return - ENODEV ;
620
648
}
621
649
650
+ phdr = skb_push (skb , QRTR_HDR_SIZE );
651
+ skb_reset_transport_header (skb );
652
+
653
+ phdr -> src_node_id = cpu_to_le32 (from -> sq_node );
654
+ phdr -> src_port_id = cpu_to_le32 (from -> sq_port );
655
+
622
656
if (sock_queue_rcv_skb (& ipc -> sk , skb )) {
623
657
qrtr_port_put (ipc );
624
658
kfree_skb (skb );
@@ -631,7 +665,9 @@ static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb)
631
665
}
632
666
633
667
/* Queue packet for broadcast. */
634
- static int qrtr_bcast_enqueue (struct qrtr_node * node , struct sk_buff * skb )
668
+ static int qrtr_bcast_enqueue (struct qrtr_node * node , struct sk_buff * skb ,
669
+ int type , struct sockaddr_qrtr * from ,
670
+ struct sockaddr_qrtr * to )
635
671
{
636
672
struct sk_buff * skbn ;
637
673
@@ -641,25 +677,26 @@ static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb)
641
677
if (!skbn )
642
678
break ;
643
679
skb_set_owner_w (skbn , skb -> sk );
644
- qrtr_node_enqueue (node , skbn );
680
+ qrtr_node_enqueue (node , skbn , type , from , to );
645
681
}
646
682
mutex_unlock (& qrtr_node_lock );
647
683
648
- qrtr_local_enqueue (node , skb );
684
+ qrtr_local_enqueue (node , skb , type , from , to );
649
685
650
686
return 0 ;
651
687
}
652
688
653
689
static int qrtr_sendmsg (struct socket * sock , struct msghdr * msg , size_t len )
654
690
{
655
691
DECLARE_SOCKADDR (struct sockaddr_qrtr * , addr , msg -> msg_name );
656
- int (* enqueue_fn )(struct qrtr_node * , struct sk_buff * );
692
+ int (* enqueue_fn )(struct qrtr_node * , struct sk_buff * , int ,
693
+ struct sockaddr_qrtr * , struct sockaddr_qrtr * );
657
694
struct qrtr_sock * ipc = qrtr_sk (sock -> sk );
658
695
struct sock * sk = sock -> sk ;
659
696
struct qrtr_node * node ;
660
- struct qrtr_hdr * hdr ;
661
697
struct sk_buff * skb ;
662
698
size_t plen ;
699
+ u32 type = QRTR_TYPE_DATA ;
663
700
int rc ;
664
701
665
702
if (msg -> msg_flags & ~(MSG_DONTWAIT ))
@@ -713,32 +750,14 @@ static int qrtr_sendmsg(struct socket *sock, struct msghdr *msg, size_t len)
713
750
if (!skb )
714
751
goto out_node ;
715
752
716
- skb_reset_transport_header (skb );
717
- skb_put (skb , len + QRTR_HDR_SIZE );
718
-
719
- hdr = (struct qrtr_hdr * )skb_transport_header (skb );
720
- hdr -> version = cpu_to_le32 (QRTR_PROTO_VER );
721
- hdr -> src_node_id = cpu_to_le32 (ipc -> us .sq_node );
722
- hdr -> src_port_id = cpu_to_le32 (ipc -> us .sq_port );
723
- hdr -> confirm_rx = cpu_to_le32 (0 );
724
- hdr -> size = cpu_to_le32 (len );
725
- hdr -> dst_node_id = cpu_to_le32 (addr -> sq_node );
726
- hdr -> dst_port_id = cpu_to_le32 (addr -> sq_port );
753
+ skb_reserve (skb , QRTR_HDR_SIZE );
727
754
728
- rc = skb_copy_datagram_from_iter (skb , QRTR_HDR_SIZE ,
729
- & msg -> msg_iter , len );
755
+ rc = memcpy_from_msg (skb_put (skb , len ), msg , len );
730
756
if (rc ) {
731
757
kfree_skb (skb );
732
758
goto out_node ;
733
759
}
734
760
735
- if (plen != len ) {
736
- rc = skb_pad (skb , plen - len );
737
- if (rc )
738
- goto out_node ;
739
- skb_put (skb , plen - len );
740
- }
741
-
742
761
if (ipc -> us .sq_port == QRTR_PORT_CTRL ) {
743
762
if (len < 4 ) {
744
763
rc = - EINVAL ;
@@ -747,12 +766,11 @@ static int qrtr_sendmsg(struct socket *sock, struct msghdr *msg, size_t len)
747
766
}
748
767
749
768
/* control messages already require the type as 'command' */
750
- skb_copy_bits (skb , QRTR_HDR_SIZE , & hdr -> type , 4 );
751
- } else {
752
- hdr -> type = cpu_to_le32 (QRTR_TYPE_DATA );
769
+ skb_copy_bits (skb , 0 , & type , 4 );
770
+ type = le32_to_cpu (type );
753
771
}
754
772
755
- rc = enqueue_fn (node , skb );
773
+ rc = enqueue_fn (node , skb , type , & ipc -> us , addr );
756
774
if (rc >= 0 )
757
775
rc = len ;
758
776
0 commit comments