1 /*
2 * Copyright (C) 2018 Alibaba Group Holding Limited
3 */
4
5 #include "lwip/opt.h"
6
7 #if LWIP_PACKET
8 #include "lwip/sockets.h"
9 #include "lwip/af_packet.h"
10 #include "lwip/pbuf.h"
11 #include "netif/ethernet.h"
12 #include "lwip/netif.h"
13 #include "lwip/priv/tcpip_priv.h"
14 #include "lwip/debug.h"
15
16 #include <string.h>
17
18 #define PACKET_THREAD_PRIO ( tskIDLE_PRIORITY + 3 )
19
20 #define ARPHRD_ETHER 1 /* ethernet hardware format */
21
22 #ifndef ETH_HWADDR_LEN
23 #define ETH_HWADDR_LEN 6
24 #endif
25
26 #ifndef ETHARP_HWADDR_LEN
27 #define ETHARP_HWADDR_LEN ETH_HWADDR_LEN
28 #endif
29
30 #define PACKET_HOST 0 /* To us */
31 #define PACKET_BROADCAST 1 /* To all */
32 #define PACKET_MULTICAST 2 /* To group */
33 #define PACKET_OTHERHOST 3 /* To someone else */
34 #define PACKET_OUTGOING 4 /* Originated by us */
35
36 #define DEFAULT_PKTMBOX_SIZE 8
37
38 #define ETH_HLEN 14
39 #define ETH_FRAME_LEN 1514
40 #define ETH_DATA_LEN 1500
41
42 #define PACKETS_DEBUG LWIP_DBG_OFF
43 //#define PACKETS_ERR LWIP_DBG_ON
44
45 /** The global array of available sockets */
46 static struct packet_sock psockets[NUM_PACKET_SOCKETS];
47
48 #if LWIP_SOCKET_SET_ERRNO
49 #ifndef set_errno
50 #define set_errno(err) do { if (err) { errno = (err); } } while(0)
51 #endif
52 #else /* LWIP_SOCKET_SET_ERRNO */
53 #define set_errno(err)
54 #endif /* LWIP_SOCKET_SET_ERRNO */
55
56 #define sock_set_errno(sk, e) do { \
57 const int sockerr = (e); \
58 sk->err = (u8_t)sockerr; \
59 set_errno(sockerr); \
60 } while (0)
61
62 int
63 packet_eth_hdr_info(struct pbuf* p, struct sockaddr_ll *sll, int ifindex, pkt_direct direct);
64
65 err_t
66 packet_input_hook (struct pbuf* p, struct netif *inp);
67
68 err_t
69 packet_output_hook (struct pbuf* p, struct netif *inp);
70
71 struct packet_sock*
tryget_pkt_socket(int s)72 tryget_pkt_socket(int s)
73 {
74 s -= LWIP_PACKET_SOCKET_OFFSET;
75 if ((s < 0) || (s >= NUM_PACKET_SOCKETS)) {
76 set_errno(EBADF);
77 return NULL;
78 }
79
80 if (!psockets[s].is_used) {
81 set_errno(EBADF);
82 return NULL;
83 }
84 return &psockets[s];
85 }
86
87 struct packet_sock*
get_pkt_socket(int s)88 get_pkt_socket(int s)
89 {
90 struct packet_sock *sock;
91
92 LWIP_DEBUGF(PACKETS_DEBUG, ("get_pkt_socket(%d) LWIP_PACKET_SOCKET_OFFSET(%d)\n", s, LWIP_PACKET_SOCKET_OFFSET));
93 s -= LWIP_PACKET_SOCKET_OFFSET;
94
95 if ((s < 0) || (s >= NUM_PACKET_SOCKETS)) {
96 LWIP_DEBUGF(PACKETS_DEBUG, ("get_pkt_socket(%d): invalid\n", s + LWIP_SOCKET_OFFSET));
97 set_errno(EBADF);
98 return NULL;
99 }
100
101 sock = &psockets[s];
102
103 if (!sock->is_used) {
104 LWIP_DEBUGF(PACKETS_DEBUG, ("get_socket(%d): not active\n", s + LWIP_SOCKET_OFFSET));
105 set_errno(EBADF);
106 return NULL;
107 }
108
109 return sock;
110 }
111
112 /* This function maybe have some risk */
113 int
get_pkt_socket_by_netif(struct netif * netif)114 get_pkt_socket_by_netif(struct netif* netif)
115 {
116 int i;
117
118 /* allocate a new socket identifier */
119 for (i = 0; i < NUM_PACKET_SOCKETS; ++i)
120 {
121 LWIP_DEBUGF(PACKETS_DEBUG, ("get_pkt_socket_by_netif netif(%c%c) %p netif_num %d"
122 "psockets[%d].ifindex %d is_used[%d] netif_find_by_index %p\n",
123 netif->name[0], netif->name[1],(void*)netif, netif->num, i,
124 (int) psockets[i].ifindex, psockets[i].is_used,
125 (void*)netif_find_by_index(psockets[i].ifindex)));
126 if (psockets[i].is_used)
127 {
128 LOCK_TCPIP_CORE();
129 if((netif == netif_find_by_index(psockets[i].ifindex)))
130 {
131 UNLOCK_TCPIP_CORE();
132 return i + LWIP_PACKET_SOCKET_OFFSET;
133 }
134 UNLOCK_TCPIP_CORE();
135 }
136 }
137 set_errno(EBADF);
138 return -1;
139 }
140
141 int
alloc_pkt_socket(int type,int protocol)142 alloc_pkt_socket(int type, int protocol)
143 {
144 int i;
145
146 LWIP_DEBUGF(PACKETS_DEBUG, ("alloc_pkt_socket (type=%d, protocol=%d)\n", type, protocol));
147
148 for (i = 0; i < NUM_PACKET_SOCKETS; ++i)
149 {
150 if(psockets[i].is_used == 1)
151 {
152 continue;
153 }
154
155 /* choose the right psockets */
156 psockets[i].flag = 0;
157 psockets[i].ifindex = 0;
158 psockets[i].hdrlen = 0;
159 psockets[i].reserve = 0;
160
161 if(sys_mbox_new(&psockets[i].pktmbox, DEFAULT_PKTMBOX_SIZE) != ERR_OK )
162 {
163 LWIP_DEBUGF(PACKETS_DEBUG, ("alloc_pkt_socket pktmbox(%d) new failed\n", i));
164 return -1;
165 }
166
167 psockets[i].is_used = 1;
168 psockets[i].recv_output = false;
169 psockets[i].type = type;
170 psockets[i].protocol = protocol;
171 return i+ LWIP_PACKET_SOCKET_OFFSET;
172 }
173
174 return -1;
175 }
176
177 int
free_pkt_socket(int s)178 free_pkt_socket(int s)
179 {
180 struct packet_sock* sock;
181
182 LWIP_DEBUGF(PACKETS_DEBUG, ("free_pkt_socket %d\n", s));
183 sock = tryget_pkt_socket(s);
184 if (!sock)
185 {
186 LWIP_DEBUGF(PACKETS_DEBUG, ("free_pkt_socket %d failed\n", s));
187 return -1;
188 }
189
190 sys_mbox_free(&sock->pktmbox);
191 sock->is_used = 0;
192 sock->ifindex = 0;
193 sock->type = 0;
194 sock->protocol= 0;
195 sock->recv_output = false;
196 sock->err = 0;
197
198 return 0;
199 }
200
201 int
packet_socket(int domain,int type,int protocol)202 packet_socket(int domain, int type, int protocol)
203 {
204 int s;
205
206 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_socket (%d, %d, %d)\n", domain, type, protocol));
207 if(domain != AF_PACKET)
208 {
209 return -1;
210 }
211
212 if((type != SOCK_DGRAM)&&
213 (type != SOCK_RAW))
214 {
215 return -1;
216 }
217
218 s = alloc_pkt_socket(type, protocol);
219 if(s == -1)
220 {
221 return -1;
222 }
223
224 return s;
225 }
226
227 int
packet_bind(int s,const struct sockaddr * name,socklen_t namelen)228 packet_bind(int s, const struct sockaddr *name, socklen_t namelen)
229 {
230 struct packet_sock *sock;
231 struct sockaddr_ll* addrll = (struct sockaddr_ll *) name;
232
233 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_bind (%d, 0x%x, %d)\n", s, (unsigned int)name, (int)namelen));
234 sock = get_pkt_socket(s);
235 if (!sock) {
236 return -1;
237 }
238
239 if(!name ||(namelen < sizeof(struct sockaddr_ll)))
240 {
241 return -1;
242 }
243
244 sock->ifindex = addrll->sll_ifindex;
245
246 return 0;
247 }
248
249 int
packet_listen(int s,int backlog)250 packet_listen(int s, int backlog)
251 {
252 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_listen is not supported\n"));
253 /* not support */
254 return -1;
255 }
256
257 int
packet_accept(int s,struct sockaddr * addr,socklen_t * addrlen)258 packet_accept(int s, struct sockaddr *addr, socklen_t *addrlen)
259 {
260 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_accept is not supported\n"));
261 /* not support */
262 return -1;
263 }
264
265 int
packet_connect(int s,const struct sockaddr * name,socklen_t namelen)266 packet_connect(int s, const struct sockaddr *name, socklen_t namelen)
267 {
268 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_connect is not supported\n"));
269 /* not support */
270 return -1;
271 }
272
273 int
packet_recvfrom(int s,void * mem,size_t len,int flags,struct sockaddr * from,socklen_t * fromlen)274 packet_recvfrom(int s, void *mem, size_t len, int flags,
275 struct sockaddr *from, socklen_t *fromlen)
276 {
277 struct packet_sock *sock;
278 struct pbuf *p;
279 void *recv_mem = NULL;
280 int ret_len = -1;
281 struct sockaddr_ll *sll = (struct sockaddr_ll *) from;
282 int hdr_len;
283 int msg_len;
284 int copy_offset;
285 struct tcpip_msg *msg;
286 pkt_direct direct;
287
288 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_recvfrom(%d, %p, %"SZT_F", 0x%x, ..)\n", s, mem, len, flags));
289 sock = get_pkt_socket(s);
290
291 if (!sock)
292 {
293 return -1;
294 }
295
296 if (!mem || !len)
297 {
298 return -1;
299 }
300
301 if (from && fromlen && (*fromlen < sizeof(struct sockaddr_ll)))
302 {
303 return -1;
304 }
305
306 /* check if got rcvevent */
307 if ((flags & MSG_DONTWAIT) && (sock->rcvevent <= 0))
308 {
309 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_recvfrom(%d): returning EWOULDBLOCK\n", s));
310 sock_set_errno(sock, EWOULDBLOCK);
311 return -1;
312 }
313
314 while(sys_mbox_valid(&sock->pktmbox))
315 {
316 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_recvfrom try to fetch data from sock %p pktmbox %p\n",
317 (void*)sock, (void*)&sock->pktmbox));
318 #if 0
319 if(SYS_MBOX_EMPTY == sys_arch_mbox_tryfetch(&sock->pktmbox, &recv_mem))
320 {
321 sock_set_errno(sock, EWOULDBLOCK);
322 return -1;
323 }
324 #endif
325 sys_arch_mbox_fetch(&sock->pktmbox, &recv_mem, 0);
326 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_recvfrom recvmem %p\n", (void*)recv_mem));
327
328 if ((mem != NULL) && (recv_mem != NULL))
329 {
330 msg = (struct tcpip_msg*)recv_mem;
331 packet_select_action(s, PACKET_ACTION_EVTRCVMINUS);
332 if(msg->type != TCPIP_MSG_INPKT)
333 {
334 memp_free(MEMP_TCPIP_MSG_INPKT, msg);
335 return -1;
336 }
337 p = msg->msg.inp.p;
338 if(msg->msg.inp.input_fn == packet_input_hook)
339 {
340 direct = INCOMING;
341 }
342 else if(msg->msg.inp.input_fn == packet_output_hook)
343 {
344 direct = OUTGOING;
345 }
346 else
347 {
348 pbuf_free(p);
349 memp_free(MEMP_TCPIP_MSG_INPKT, msg);
350 return -1;
351 }
352
353 hdr_len = packet_eth_hdr_info(p, sll, sock->ifindex, direct);
354 if(sock->type == SOCK_DGRAM)
355 {
356 msg_len = p->tot_len - hdr_len - ETH_PAD_SIZE;
357 copy_offset = (u16_t)(hdr_len + ETH_PAD_SIZE);
358 }
359 else
360 {
361 msg_len = p->tot_len - ETH_PAD_SIZE;
362 copy_offset = ETH_PAD_SIZE;
363 }
364 if(msg_len > len)
365 {
366 pbuf_copy_partial(p, mem, (u16_t)len, copy_offset);
367 ret_len = len;
368 }
369 else
370 {
371 pbuf_copy_partial(p, mem, (u16_t)msg_len, copy_offset);
372 ret_len = msg_len;
373 }
374 pbuf_free(p);
375 memp_free(MEMP_TCPIP_MSG_INPKT, msg);
376 break;
377 }
378 }
379
380 if(ret_len > 0)
381 {
382 if(fromlen)
383 {
384 sock_set_errno(sock, 0);
385 *fromlen = sizeof(struct sockaddr_ll);
386 }
387 }
388
389 return ret_len;
390 }
391
392 err_t
packet_select(int maxfdp1,fd_set * readset,fd_set * writeset,fd_set * exceptset,struct timeval * timeout)393 packet_select(int maxfdp1, fd_set *readset, fd_set *writeset, fd_set *exceptset,
394 struct timeval *timeout)
395 {
396 return -1;;
397 }
398
packet_select_action(int s,packet_action_t action)399 int packet_select_action(int s, packet_action_t action)
400 {
401 packet_sock *sock;
402 int err = 0;
403
404 sock = get_pkt_socket(s);
405 if (!sock)
406 {
407 return -1;
408 }
409
410 SYS_ARCH_DECL_PROTECT(lev);
411 SYS_ARCH_PROTECT(lev);
412 switch(action)
413 {
414 case PACKET_ACTION_EVTRCVPLUS:
415 sock->rcvevent++;
416 break;
417 case PACKET_ACTION_EVTRCVMINUS:
418 sock->rcvevent--;
419 break;
420 case PACKET_ACTION_SELWAITPLUS:
421 sock->select_waiting++;
422 break;
423 case PACKET_ACTION_SELWAITMINUS:
424 sock->select_waiting--;
425 break;
426 default:
427 err = -1;
428 break;
429 }
430
431 if (sock->select_waiting == 0) {
432 SYS_ARCH_UNPROTECT(lev);
433 /* noone is waiting for this socket, no need to check select_cb_list */
434 return 0;
435 }
436 SYS_ARCH_UNPROTECT(lev);
437 /* af_packet not support sendevent and errevent */
438 lwip_try_wakeup(s, sock->rcvevent, 0, 0);
439
440 return err;
441 }
442
443 int
packet_selscan(int maxfdp1,fd_set * readset_in,fd_set * writeset_in,fd_set * exceptset_in,fd_set * readset_out,fd_set * writeset_out,fd_set * exceptset_out)444 packet_selscan(int maxfdp1, fd_set *readset_in, fd_set *writeset_in, fd_set *exceptset_in,
445 fd_set *readset_out, fd_set *writeset_out, fd_set *exceptset_out)
446 {
447 int i, nready = 0;
448 fd_set lreadset;
449 struct packet_sock *sock;
450
451 /* packet_selscan do not support write or except event, just warning. */
452 if(writeset_in != NULL || exceptset_in != NULL)
453 {
454 LWIP_DEBUGF(PACKETS_DEBUG, ("af_packet do not support write or except sets."));
455 }
456
457 SYS_ARCH_DECL_PROTECT(lev);
458
459 FD_ZERO(&lreadset);
460
461 /* Go through each socket in each list to count number of sockets which
462 currently match */
463 for (i = LWIP_PACKET_SOCKET_OFFSET; i < maxfdp1; i++) {
464 /* if this FD is not in the set, continue */
465 if (!(readset_in && FD_ISSET(i, readset_in))) {
466 continue;
467 }
468 /* First get the socket's status (protected)... */
469 SYS_ARCH_PROTECT(lev);
470 sock = tryget_pkt_socket(i);
471 if (sock != NULL) {
472 s16_t rcvevent = sock->rcvevent;
473 SYS_ARCH_UNPROTECT(lev);
474 /* See if this socket is ready for read */
475 if (readset_in && FD_ISSET(i, readset_in) && (rcvevent > 0)) {
476 FD_SET(i, &lreadset);
477 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_selscan: fd=%d ready for reading\n", i));
478 nready++;
479 }
480 } else {
481 SYS_ARCH_UNPROTECT(lev);
482 /* continue on to next FD in list */
483 }
484 }
485
486 /* copy local sets to the ones provided as arguments */
487 *readset_out = lreadset;
488
489 LWIP_ASSERT("nready >= 0", nready >= 0);
490 return nready;
491 }
492
493 int
packet_recv(int s,void * mem,size_t len,int flags)494 packet_recv(int s, void *mem, size_t len, int flags)
495 {
496 return (packet_recvfrom(s, mem, len, flags, NULL, NULL));
497 }
498
499 err_t
packet_send(int s,const void * data,size_t size,int flags)500 packet_send(int s, const void *data, size_t size, int flags)
501 {
502 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_send is not supported\n"));
503 //not support
504 return -1;
505 }
506
507 /* check more about the details */
508 int
packet_eth_hdr_info(struct pbuf * p,struct sockaddr_ll * sll,int ifindex,pkt_direct direct)509 packet_eth_hdr_info(struct pbuf* p, struct sockaddr_ll *sll, int ifindex, pkt_direct direct)
510 {
511 struct eth_hdr *eth_hdr = (struct eth_hdr *) p->payload;
512 struct netif * netif;
513
514 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_eth_hdr_info p=0x%x, ssl=0x%x, ifindex=%d\n", (unsigned int)p, (unsigned int)sll, ifindex));
515 if(sll)
516 {
517 sll->sll_len = sizeof(struct sockaddr_ll);
518 sll->sll_family = AF_PACKET;
519 sll->sll_protocol = eth_hdr->type;
520 sll->sll_ifindex = ifindex;
521 sll->sll_hatype = ARPHRD_ETHER;
522 sll->sll_halen = ETHARP_HWADDR_LEN;
523
524 if(direct == OUTGOING)
525 {
526 sll->sll_addr[0] = eth_hdr->dest.addr[0];
527 sll->sll_addr[1] = eth_hdr->dest.addr[1];
528 sll->sll_addr[2] = eth_hdr->dest.addr[2];
529 sll->sll_addr[3] = eth_hdr->dest.addr[3];
530 sll->sll_addr[4] = eth_hdr->dest.addr[4];
531 sll->sll_addr[5] = eth_hdr->dest.addr[5];
532 }
533 else
534 {
535 /* direction is INCOMING */
536 sll->sll_addr[0] = eth_hdr->src.addr[0];
537 sll->sll_addr[1] = eth_hdr->src.addr[1];
538 sll->sll_addr[2] = eth_hdr->src.addr[2];
539 sll->sll_addr[3] = eth_hdr->src.addr[3];
540 sll->sll_addr[4] = eth_hdr->src.addr[4];
541 sll->sll_addr[5] = eth_hdr->src.addr[5];
542 }
543
544 netif = netif_find_by_index(ifindex);
545 if(netif == NULL)
546 {
547 return -1;
548 }
549
550 if(direct == OUTGOING)
551 {
552 sll->sll_pkttype = PACKET_OUTGOING;
553 }
554 else if (eth_hdr->dest.addr[0] & 0x01) {
555 if (eth_addr_cmp(ð_hdr->dest, ðbroadcast)) {
556 sll->sll_pkttype = PACKET_BROADCAST;
557 } else {
558 sll->sll_pkttype = PACKET_MULTICAST;
559 }
560 }
561 else if (memcmp(eth_hdr->dest.addr, netif->hwaddr,
562 ETHARP_HWADDR_LEN) == 0)
563 {
564 sll->sll_pkttype = PACKET_HOST;
565 }
566 else {
567 sll->sll_pkttype = PACKET_OUTGOING;
568 }
569 }
570
571 if (eth_hdr->type == PP_HTONS(ETHTYPE_VLAN)) {
572 return (ETH_HLEN + 4);
573 } else {
574 return (ETH_HLEN);
575 }
576 }
577
578 err_t
packet_eth_datagram_sendto(const void * data,size_t size,struct sockaddr_ll * sll)579 packet_eth_datagram_sendto(const void * data, size_t size, struct sockaddr_ll* sll)
580 {
581 struct netif* netif;
582 struct pbuf* pbuf_hdr;
583 struct pbuf* pbuf_payload;
584 struct eth_hdr * eth_hdr;
585 err_t err;
586
587 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_eth_datagram_sendto data=%p, size=%d, sll=%p\n", (void*)data, size, (void*)sll));
588 if( size > ETH_DATA_LEN)
589 {
590 return -1;
591 }
592 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_eth_datagram_sendto sll_ifindex(%d)\n", sll->sll_ifindex));
593 LOCK_TCPIP_CORE();
594 netif = netif_find_by_index(sll->sll_ifindex);
595 if(netif == NULL)
596 {
597 UNLOCK_TCPIP_CORE();
598 return -1;
599 }
600
601 if(!netif_is_up(netif) || !netif_is_link_up(netif))
602 {
603 UNLOCK_TCPIP_CORE();
604 return -1;
605 }
606
607 pbuf_hdr = pbuf_alloc(PBUF_RAW, ETH_HLEN + ETH_PAD_SIZE, PBUF_RAM); /* alloc pkt header with pad */
608 if (pbuf_hdr == NULL) {
609 UNLOCK_TCPIP_CORE();
610 set_errno(ENFILE);
611 return -1;
612 }
613
614 eth_hdr = (struct eth_hdr *)pbuf_hdr->payload;
615 eth_hdr->dest.addr[0] = sll->sll_addr[0];
616 eth_hdr->dest.addr[1] = sll->sll_addr[1];
617 eth_hdr->dest.addr[2] = sll->sll_addr[2];
618 eth_hdr->dest.addr[3] = sll->sll_addr[3];
619 eth_hdr->dest.addr[4] = sll->sll_addr[4];
620 eth_hdr->dest.addr[5] = sll->sll_addr[5];
621
622 eth_hdr->src.addr[0] = netif->hwaddr[0];
623 eth_hdr->src.addr[1] = netif->hwaddr[1];
624 eth_hdr->src.addr[2] = netif->hwaddr[2];
625 eth_hdr->src.addr[3] = netif->hwaddr[3];
626 eth_hdr->src.addr[4] = netif->hwaddr[4];
627 eth_hdr->src.addr[5] = netif->hwaddr[5];
628
629 eth_hdr->type = sll->sll_protocol;
630
631 pbuf_payload = pbuf_alloc(PBUF_RAW, (u16_t)size, PBUF_REF);
632 if (pbuf_payload == NULL) {
633 pbuf_free(pbuf_hdr);
634 UNLOCK_TCPIP_CORE();
635 set_errno(ENFILE);
636 return -1;
637 }
638
639 pbuf_payload->payload = (void *)data;
640
641 pbuf_cat(pbuf_hdr, pbuf_payload);
642
643 err = packet_output_hook(pbuf_hdr, netif);
644
645 pbuf_free(pbuf_hdr);
646
647 UNLOCK_TCPIP_CORE();
648 if (err) {
649 return -1;
650 } else {
651 return 0;
652 }
653 }
654
655 err_t
packet_eth_raw_sendto(const void * data,size_t size,struct sockaddr_ll * sll)656 packet_eth_raw_sendto(const void * data, size_t size, struct sockaddr_ll* sll)
657 {
658 struct netif* netif;
659 struct pbuf* pbuf_hdr;
660 struct pbuf* pbuf_payload;
661 err_t err;
662
663 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_eth_raw_sendto data=%p, size=%d, sll=%p\n", (void*)data, size, (void*)sll));
664 if((size < ETH_HLEN) || (size > ETH_FRAME_LEN))
665 {
666 return -1;
667 }
668
669 LOCK_TCPIP_CORE();
670 netif = netif_find_by_index(sll->sll_ifindex);
671 if(netif == NULL)
672 {
673 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_eth_raw_sendto invalid netif index %d\n", (int)sll->sll_ifindex));
674 UNLOCK_TCPIP_CORE();
675 return -1;
676 }
677
678 if (!(netif->flags & (NETIF_FLAG_ETHARP | NETIF_FLAG_ETHERNET)))
679 {
680 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_eth_raw_sendto invalid netif flags %d\n", (int)netif->flags));
681 UNLOCK_TCPIP_CORE();
682 return -1;
683 }
684
685 if (!netif_is_up(netif) || !netif_is_link_up(netif))
686 {
687 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_eth_raw_sendto netif_is_up(%d) netif_is_link_up(%d)\n",
688 netif_is_up(netif), netif_is_link_up(netif)));
689 UNLOCK_TCPIP_CORE();
690 return -1;
691 }
692
693 #if ETH_PAD_SIZE
694 pbuf_hdr = pbuf_alloc(PBUF_RAW, ETH_HLEN + ETH_PAD_SIZE, PBUF_RAM);
695 if (pbuf_hdr == NULL) {
696 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_eth_raw_sendto eth_pad alloc pbuf hdr failed\n"));
697 UNLOCK_TCPIP_CORE();
698 return -1;
699 }
700 memcpy(((u8_t *)pbuf_hdr->payload) + ETH_PAD_SIZE, pvPacket, ETH_HLEN);
701
702 pbuf_payload = pbuf_alloc(PBUF_RAW, (u16_t)(size - ETH_HLEN), PBUF_REF);
703 if (pbuf_payload == NULL) {
704 pbuf_free(pbuf_hdr);
705 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_eth_raw_sendto eth_pad alloc pbuf payload failed\n"));
706 UNLOCK_TCPIP_CORE();
707 return -1;
708 }
709 pbuf_payload->payload = (void *)((u8_t *)data + ETH_HLEN);
710
711 pbuf_cat(pbuf_hdr, pbuf_dat);
712
713 #else
714 (void)pbuf_payload;
715
716 pbuf_hdr = pbuf_alloc(PBUF_RAW, (u16_t)size, PBUF_REF);
717 if (pbuf_hdr == NULL) {
718 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_eth_raw_sendto alloc pbuf hdr failed\n"));
719 UNLOCK_TCPIP_CORE();
720 return -1;
721 }
722 pbuf_hdr->payload = (void *)data;
723 #endif
724
725 err = packet_output_hook (pbuf_hdr, netif);
726
727 pbuf_free(pbuf_hdr);
728 UNLOCK_TCPIP_CORE();
729
730 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_eth_raw_sendto err(%d)\n", err));
731 if(err)
732 {
733 return -1;
734 }
735 else
736 {
737 return 0;
738 }
739 }
740
741 int
packet_sendto(int s,const void * data,size_t size,int flags,const struct sockaddr * to,socklen_t tolen)742 packet_sendto(int s, const void *data, size_t size, int flags,
743 const struct sockaddr *to, socklen_t tolen)
744 {
745 struct packet_sock *sock;
746 struct sockaddr_ll* sll;
747 int err;
748
749 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_sendto s(%d) data(0x%x) size(%d) flags(%d)"
750 "to(0x%x) tolen(%d)\n", s, (int)data, (int)size, flags, (int)to, (int)tolen));
751 sock = get_pkt_socket(s);
752 if (!sock) {
753 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_sendto invalid s(%d)\n", s));
754 return -1;
755 }
756
757 if(!data || !size)
758 {
759 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_sendto invalid data or size\n"));
760 return -1;
761 }
762
763 if(to && (tolen >= sizeof(struct sockaddr_ll)))
764 {
765 sll = (struct sockaddr_ll*) to;
766 }
767 else
768 {
769 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_sendto invalid tolen(%d) no more than sll size(%d)\n",
770 (int)tolen, sizeof(struct sockaddr_ll)));
771 return -1;
772 }
773
774 /* set sock ifindex */
775 sock->ifindex = sll->sll_ifindex;
776
777 if(sock->type == SOCK_RAW)
778 {
779 err = packet_eth_raw_sendto(data, size, sll);
780 }
781 else
782 {
783 err = packet_eth_datagram_sendto(data, size, sll);
784 }
785
786 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_sendto err(%d)\n", err));
787 if(err)
788 {
789 /* invalid sock ifindex */
790 sock->ifindex = 0;
791 return -1;
792 }
793 else
794 {
795 return (size);
796 }
797 }
798
799 int
packet_sendmsg(int s,const struct msghdr * msg,int flags)800 packet_sendmsg(int s, const struct msghdr *msg, int flags)
801 {
802 return -1;
803 }
804
packet_close(int s)805 int packet_close(int s)
806 {
807 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_close(%d)\n", s));
808
809 if(free_pkt_socket(s) == -1)
810 {
811 return -1;
812 }
813
814 set_errno(0);
815 return 0;
816 }
817
packet_shutdown(int s,int how)818 int packet_shutdown(int s, int how)
819 {
820 return 0;
821 }
822
packet_getsockname(int s,struct sockaddr * name,socklen_t * namelen)823 int packet_getsockname (int s, struct sockaddr *name, socklen_t *namelen)
824 {
825 return 0;
826 }
827
packet_getpeername(int s,struct sockaddr * name,socklen_t * namelen)828 int packet_getpeername (int s, struct sockaddr *name, socklen_t *namelen)
829 {
830 return 0;
831 }
832
packet_setsockopt(int s,int level,int optname,const void * optval,socklen_t optlen)833 int packet_setsockopt (int s, int level, int optname,
834 const void *optval, socklen_t optlen)
835 {
836 int err = 0;
837 struct packet_sock *sock;
838
839 if(!optval || optlen < sizeof(int))
840 {
841 return -1;
842 }
843
844 sock = get_pkt_socket(s);
845 if (!sock) {
846 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_setsockopt invalid s(%d)\n", s));
847 return -1;
848 }
849
850 switch(level)
851 {
852 case SOL_PACKET:
853 switch(optname)
854 {
855 case PACKET_ADD_MEMBERSHIP:
856 case PACKET_DROP_MEMBERSHIP:
857 //to do
858 err = -1;
859 break;
860 case PACKET_RECV_OUTPUT:
861 if(*(int*)optval)
862 {
863 sock->recv_output = true;
864 }
865 else
866 {
867 sock->recv_output = false;
868 }
869 break;
870 default:
871 LWIP_DEBUGF(PACKETS_DEBUG, ("optname %d is not support\n", optname));
872 err = -1;
873 break;
874 }
875 break;
876 default:
877 LWIP_DEBUGF(PACKETS_DEBUG, ("level %d is not support\n", level));
878 err = -1;
879 break;
880 }
881
882 return err;
883 }
884
packet_getsockopt(int s,int level,int optname,void * optval,socklen_t * optlen)885 int packet_getsockopt (int s, int level, int optname, void *optval, socklen_t *optlen)
886 {
887 return -1;
888 }
889
packet_ioctl(int s,int cmd,void * arg)890 int packet_ioctl (int s, int cmd, void* arg)
891 {
892 return -1;
893 }
894
895 err_t
packet_input(struct pbuf * p,struct netif * inp,pkt_direct direct)896 packet_input(struct pbuf *p, struct netif *inp, pkt_direct direct)
897 {
898 struct eth_hdr *eth_hdr = NULL;
899 struct eth_vlan_hdr *eth_vlan_hdr = NULL;
900 int i;
901 struct packet_sock *sock;
902 struct pbuf *q;
903 struct tcpip_msg *msg;
904 #if BYTE_ORDER == LITTLE_ENDIAN
905 static uint16_t pall = 0x0300; /* ETH_P_ALL */
906 #else
907 static uint16_t pall = 0x0003;
908 #endif
909 bool is_recv = false;
910
911 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_input %p/%p\n", (void*)p, (void*)inp));
912 if(direct == OUTGOING)
913 {
914 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_input %p netif %p direct %d\n",(void*)p, (void*)inp, direct));
915 }
916 if((p->tot_len < (ETH_HLEN + ETH_PAD_SIZE)) ||
917 !(inp->flags & (NETIF_FLAG_ETHARP | NETIF_FLAG_ETHERNET)))
918 {
919 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_input tot_len %d flags %d \n",(int)p->tot_len, (int)inp->flags));
920 return -1;
921 }
922
923 eth_hdr = (struct eth_hdr *)p->payload;
924
925 if(eth_hdr->type == htons(ETHTYPE_VLAN))
926 {
927 eth_vlan_hdr = (struct eth_vlan_hdr *)(((char *)eth_hdr) + SIZEOF_ETH_HDR);
928 } else {
929 eth_vlan_hdr = NULL;
930 }
931
932 /* get the packet_sock that binding to the netif */
933 i = get_pkt_socket_by_netif(inp);
934 if(i == -1)
935 {
936 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_input invalid netif %p \n", (void*) inp));
937 return -1;
938 }
939
940 //LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_recvfrom(%d, %p, %"SZT_F", 0x%x, ..)\n", s, mem, len, flags));
941 sock = get_pkt_socket(i);
942
943 if (!sock) {
944 return -1;
945 }
946
947 if((direct == OUTGOING) && (sock->recv_output == false))
948 {
949 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_input sock->recv_output(%d)\n", (int) sock->recv_output));
950 return -1;
951 }
952 if(eth_vlan_hdr)
953 {
954 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_input eth_vlan_hdr 0x%x \n", (int) eth_vlan_hdr));
955 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_input left(%d, %d , %d) right (%d) \n",
956 eth_hdr->type, eth_vlan_hdr->tpid, pall, sock->protocol));
957 if((eth_hdr->type == sock->protocol)
958 || (eth_vlan_hdr->tpid == sock->protocol)
959 || (pall == sock->protocol))
960 {
961 is_recv = true;
962 }
963 }
964 else
965 {
966 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_input left(%d, %d) right (%d) \n", eth_hdr->type, pall, sock->protocol));
967 if((eth_hdr->type == sock->protocol)
968 ||(pall == sock->protocol))
969 {
970 is_recv = true;
971 }
972 }
973
974 if(is_recv == false)
975 {
976 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_input dont recv \n"));
977 return -1;
978 }
979
980 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_input p %p tot_len %d len %d\n", (void*)p, p->tot_len, p->len));
981 /* alloc a new buffer */
982 q = pbuf_alloc(PBUF_RAW, p->tot_len, PBUF_POOL);
983 if(!q)
984 {
985 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_input pbuf alloc failed \n"));
986 return -1;
987 }else
988 {
989 if (pbuf_copy(q, p) != ERR_OK)
990 {
991 pbuf_free(q);
992 q = NULL;
993 return -1;
994 }
995 }
996
997 msg = (struct tcpip_msg *)memp_malloc(MEMP_TCPIP_MSG_INPKT);
998 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_input alloc msg %p \n", msg));
999 if (msg == NULL) {
1000 pbuf_free(q);
1001 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_input msg alloc failed \n"));
1002 return ERR_MEM;
1003 }
1004
1005 msg->type = TCPIP_MSG_INPKT;
1006 msg->msg.inp.p = q;
1007 msg->msg.inp.netif = inp;
1008
1009 if(direct == INCOMING)
1010 {
1011 msg->msg.inp.input_fn = packet_input_hook;
1012 }
1013 else
1014 {
1015 msg->msg.inp.input_fn = packet_output_hook;
1016 }
1017 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_input sock %p pktmbox %p\n",
1018 (void*)sock, (void*)&sock->pktmbox));
1019 if (sys_mbox_trypost(&sock->pktmbox, msg) != ERR_OK) {
1020 pbuf_free(q);
1021 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_input trypost failed \n"));
1022 memp_free(MEMP_TCPIP_MSG_INPKT, msg);
1023 return ERR_MEM;
1024 }
1025
1026 packet_select_action(i, PACKET_ACTION_EVTRCVPLUS);
1027
1028 return 0;
1029 }
1030
1031 err_t
packet_input_hook(struct pbuf * p,struct netif * inp)1032 packet_input_hook (struct pbuf* p, struct netif* inp)
1033 {
1034 if (!netif_is_up(inp)) {
1035 return -1;
1036 }
1037 return packet_input(p, inp, INCOMING);
1038 }
1039
1040 err_t
packet_output_hook(struct pbuf * p,struct netif * inp)1041 packet_output_hook (struct pbuf*p, struct netif* inp)
1042 {
1043 #if 0
1044 unsigned char *data = (unsigned char*)p->payload;
1045 LWIP_DEBUGF(PACKETS_DEBUG, ("===================dump output data start=========================\n"));
1046 LWIP_DEBUGF(PACKETS_DEBUG, ("af_packet_test send data len = %d\n", p->len));
1047 for(int j = 0; j <= p->len; j++){
1048 LWIP_DEBUGF(PACKETS_DEBUG, ("%02x", (unsigned char)data[j]));
1049 }
1050 LWIP_DEBUGF(PACKETS_DEBUG, ("\n====================dump output data end==========================\n"));
1051 #endif
1052 LWIP_DEBUGF(PACKETS_DEBUG, ("packet_output_hook %p/%p\n", (void*)p, (void*)inp));
1053 packet_input(p, inp, OUTGOING);
1054
1055 return inp->linkoutput(inp, p);
1056 }
1057
1058 #endif /* LWIP_PACKET */
1059