1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
|
/* Thus far used only by udpsvd.c */
void socket_want_pktinfo(int fd);
ssize_t send_to_from(int fd, void *buf, size_t len, int flags,
const struct sockaddr *from, const struct sockaddr *to,
socklen_t tolen);
ssize_t recv_from_to(int fd, void *buf, size_t len, int flags,
struct sockaddr *from, struct sockaddr *to,
socklen_t sa_size);
/*
* This asks kernel to let us know dst addr/port of incoming packets
* We don't check for errors here. Not supported == won't be used
*/
void
socket_want_pktinfo(int fd)
{
#ifdef IP_PKTINFO
setsockopt(fd, IPPROTO_IP, IP_PKTINFO, &const_int_1, sizeof(int));
#endif
#ifdef IPV6_PKTINFO
setsockopt(fd, IPPROTO_IPV6, IPV6_PKTINFO, &const_int_1, sizeof(int));
#endif
}
ssize_t
send_to_from(int fd, void *buf, size_t len, int flags,
const struct sockaddr *from, const struct sockaddr *to,
socklen_t tolen)
{
#ifndef IP_PKTINFO
return sendto(fd, buf, len, flags, to, tolen);
#else
struct iovec iov[1];
struct msghdr msg;
char cbuf[LSA_SIZEOF_SA];
/* actually, max(sizeof(in_pktinfo),sizeof(in6_pktinfo)) */
struct cmsghdr* cmsgptr;
if (from->sa_family != AF_INET
#if ENABLE_FEATURE_IPV6
&& from->sa_family != AF_INET6
#endif
) {
/* ANY local address */
return sendto(fd, buf, len, flags, to, tolen);
}
/* man recvmsg and man cmsg is needed to make sense of code below */
iov[0].iov_base = buf;
iov[0].iov_len = len;
memset(cbuf, 0, sizeof(cbuf));
memset(&msg, 0, sizeof(msg));
msg.msg_name = (void *)(struct sockaddr *)to; /* or compiler will annoy us */
msg.msg_namelen = tolen;
msg.msg_iov = iov;
msg.msg_iovlen = 1;
msg.msg_control = cbuf;
msg.msg_controllen = sizeof(cbuf);
msg.msg_flags = flags;
cmsgptr = CMSG_FIRSTHDR(&msg);
if (to->sa_family == AF_INET && from->sa_family == AF_INET) {
struct in_pktinfo *pktptr;
cmsgptr->cmsg_level = IPPROTO_IP;
cmsgptr->cmsg_type = IP_PKTINFO;
cmsgptr->cmsg_len = CMSG_LEN(sizeof(struct in_pktinfo));
pktptr = (struct in_pktinfo *)(CMSG_DATA(cmsgptr));
/* pktptr->ipi_ifindex = 0; -- already done by memset(cbuf...) */
pktptr->ipi_spec_dst = ((struct sockaddr_in*)from)->sin_addr;
}
#if ENABLE_FEATURE_IPV6 && defined(IP6_PKTINFO)
else if (to->sa_family == AF_INET6 && from->sa_family == AF_INET6) {
struct in6_pktinfo *pktptr;
cmsgptr->cmsg_level = IPPROTO_IPV6;
cmsgptr->cmsg_type = IP6_PKTINFO;
cmsgptr->cmsg_len = CMSG_LEN(sizeof(struct in6_pktinfo));
pktptr = (struct in6_pktinfo *)(CMSG_DATA(cmsgptr));
/* pktptr->ipi6_ifindex = 0; -- already done by memset(cbuf...) */
pktptr->ipi6_addr = ((struct sockaddr_in6*)from)->sin6_addr;
}
#endif
return sendmsg(fd, &msg, flags);
#endif
}
/* NB: this will never set port# in *to! */
ssize_t
recv_from_to(int fd, void *buf, size_t len, int flags,
struct sockaddr *from, struct sockaddr *to,
socklen_t sa_size)
{
#ifndef IP_PKTINFO
return recvfrom(fd, buf, len, flags, from, &sa_size);
#else
/* man recvmsg and man cmsg is needed to make sense of code below */
struct iovec iov[1];
union {
char cmsg[CMSG_SPACE(sizeof(struct in_pktinfo))];
char cmsg6[CMSG_SPACE(sizeof(struct in6_pktinfo))];
} u;
struct cmsghdr *cmsgptr;
struct msghdr msg;
socklen_t recv_length;
iov[0].iov_base = buf;
iov[0].iov_len = len;
memset(&msg, 0, sizeof(msg));
msg.msg_name = (struct sockaddr *)from;
msg.msg_namelen = sa_size;
msg.msg_iov = iov;
msg.msg_iovlen = 1;
msg.msg_control = &u;
msg.msg_controllen = sizeof(u);
recv_length = recvmsg(fd, &msg, flags);
if (recv_length < 0)
return recv_length;
/* Here we try to retrieve destination IP and memorize it */
memset(to, 0, sa_size);
for (cmsgptr = CMSG_FIRSTHDR(&msg);
cmsgptr != NULL;
cmsgptr = CMSG_NXTHDR(&msg, cmsgptr)
) {
if (cmsgptr->cmsg_level == IPPROTO_IP
&& cmsgptr->cmsg_type == IP_PKTINFO
) {
#define pktinfo(cmsgptr) ( (struct in_pktinfo*)(CMSG_DATA(cmsgptr)) )
to->sa_family = AF_INET;
((struct sockaddr_in*)to)->sin_addr = pktinfo(cmsgptr)->ipi_addr;
/* ((struct sockaddr_in*)to)->sin_port = 123; */
#undef pktinfo
break;
}
#if ENABLE_FEATURE_IPV6 && defined(IP6_PKTINFO)
if (cmsgptr->cmsg_level == IPPROTO_IPV6
&& cmsgptr->cmsg_type == IP6_PKTINFO
) {
#define pktinfo(cmsgptr) ( (struct in6_pktinfo*)(CMSG_DATA(cmsgptr)) )
to->sa_family = AF_INET6;
((struct sockaddr_in6*)to)->sin6_addr = pktinfo(cmsgptr)->ipi6_addr;
/* ((struct sockaddr_in6*)to)->sin6_port = 123; */
#undef pktinfo
break;
}
#endif
}
return recv_length;
#endif
}
|
