1 /* $OpenBSD: util.c,v 1.14 2023/12/21 12:43:31 martijn Exp $ */
2 /*
3 * Copyright (c) 2014 Bret Stephen Lambert <blambert@openbsd.org>
4 *
5 * Permission to use, copy, modify, and distribute this software for any
6 * purpose with or without fee is hereby granted, provided that the above
7 * copyright notice and this permission notice appear in all copies.
8 *
9 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
10 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
11 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
12 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
13 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
14 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
15 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
16 */
17
18 #include <sys/socket.h>
19
20 #include <netinet/in.h>
21
22 #include <ctype.h>
23 #include <stdlib.h>
24 #include <stdint.h>
25 #include <string.h>
26 #include <strings.h>
27 #include <netdb.h>
28
29 #include "snmpd.h"
30
31 ssize_t
sendtofrom(int s,void * buf,size_t len,int flags,struct sockaddr * to,socklen_t tolen,struct sockaddr * from,socklen_t fromlen)32 sendtofrom(int s, void *buf, size_t len, int flags, struct sockaddr *to,
33 socklen_t tolen, struct sockaddr *from, socklen_t fromlen)
34 {
35 struct iovec iov;
36 struct msghdr msg;
37 struct cmsghdr *cmsg;
38 struct in6_pktinfo *pkt6;
39 struct sockaddr_in *in;
40 struct sockaddr_in6 *in6;
41 union {
42 struct cmsghdr hdr;
43 char inbuf[CMSG_SPACE(sizeof(struct in_addr))];
44 char in6buf[CMSG_SPACE(sizeof(struct in6_pktinfo))];
45 } cmsgbuf;
46
47 bzero(&msg, sizeof(msg));
48 bzero(&cmsgbuf, sizeof(cmsgbuf));
49
50 iov.iov_base = buf;
51 iov.iov_len = len;
52 msg.msg_iov = &iov;
53 msg.msg_iovlen = 1;
54 msg.msg_name = to;
55 msg.msg_namelen = tolen;
56 msg.msg_control = &cmsgbuf;
57 msg.msg_controllen = sizeof(cmsgbuf);
58
59 cmsg = CMSG_FIRSTHDR(&msg);
60 switch (to->sa_family) {
61 case AF_INET:
62 msg.msg_controllen = sizeof(cmsgbuf.inbuf);
63 cmsg->cmsg_len = CMSG_LEN(sizeof(struct in_addr));
64 cmsg->cmsg_level = IPPROTO_IP;
65 cmsg->cmsg_type = IP_SENDSRCADDR;
66 in = (struct sockaddr_in *)from;
67 memcpy(CMSG_DATA(cmsg), &in->sin_addr, sizeof(struct in_addr));
68 break;
69 case AF_INET6:
70 msg.msg_controllen = sizeof(cmsgbuf.in6buf);
71 cmsg->cmsg_len = CMSG_LEN(sizeof(struct in6_pktinfo));
72 cmsg->cmsg_level = IPPROTO_IPV6;
73 cmsg->cmsg_type = IPV6_PKTINFO;
74 in6 = (struct sockaddr_in6 *)from;
75 pkt6 = (struct in6_pktinfo *)CMSG_DATA(cmsg);
76 pkt6->ipi6_addr = in6->sin6_addr;
77 break;
78 }
79
80 return sendmsg(s, &msg, flags);
81 }
82
83 ssize_t
recvfromto(int s,void * buf,size_t len,int flags,struct sockaddr * from,socklen_t * fromlen,struct sockaddr * to,socklen_t * tolen)84 recvfromto(int s, void *buf, size_t len, int flags, struct sockaddr *from,
85 socklen_t *fromlen, struct sockaddr *to, socklen_t *tolen)
86 {
87 struct iovec iov;
88 struct msghdr msg;
89 struct cmsghdr *cmsg;
90 struct in6_pktinfo *pkt6;
91 struct sockaddr_in *in;
92 struct sockaddr_in6 *in6;
93 ssize_t ret;
94 union {
95 struct cmsghdr hdr;
96 char buf[CMSG_SPACE(sizeof(struct sockaddr_storage))];
97 } cmsgbuf;
98
99 bzero(&msg, sizeof(msg));
100 bzero(&cmsgbuf.buf, sizeof(cmsgbuf.buf));
101
102 iov.iov_base = buf;
103 iov.iov_len = len;
104 msg.msg_iov = &iov;
105 msg.msg_iovlen = 1;
106 msg.msg_name = from;
107 msg.msg_namelen = *fromlen;
108 msg.msg_control = &cmsgbuf.buf;
109 msg.msg_controllen = sizeof(cmsgbuf.buf);
110
111 if ((ret = recvmsg(s, &msg, flags)) == -1)
112 return (-1);
113
114 *fromlen = from->sa_len;
115 *tolen = 0;
116
117 if (getsockname(s, to, tolen) != 0)
118 *tolen = 0;
119
120 for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
121 cmsg = CMSG_NXTHDR(&msg, cmsg)) {
122 switch (from->sa_family) {
123 case AF_INET:
124 if (cmsg->cmsg_level == IPPROTO_IP &&
125 cmsg->cmsg_type == IP_RECVDSTADDR) {
126 in = (struct sockaddr_in *)to;
127 in->sin_family = AF_INET;
128 in->sin_len = *tolen = sizeof(*in);
129 memcpy(&in->sin_addr, CMSG_DATA(cmsg),
130 sizeof(struct in_addr));
131 }
132 break;
133 case AF_INET6:
134 if (cmsg->cmsg_level == IPPROTO_IPV6 &&
135 cmsg->cmsg_type == IPV6_PKTINFO) {
136 in6 = (struct sockaddr_in6 *)to;
137 in6->sin6_family = AF_INET6;
138 in6->sin6_len = *tolen = sizeof(*in6);
139 pkt6 = (struct in6_pktinfo *)CMSG_DATA(cmsg);
140 memcpy(&in6->sin6_addr, &pkt6->ipi6_addr,
141 sizeof(struct in6_addr));
142 if (IN6_IS_ADDR_LINKLOCAL(&in6->sin6_addr))
143 in6->sin6_scope_id =
144 pkt6->ipi6_ifindex;
145 }
146 break;
147 }
148 }
149
150 return (ret);
151 }
152
153 const char *
print_host(struct sockaddr_storage * ss,char * buf,size_t len)154 print_host(struct sockaddr_storage *ss, char *buf, size_t len)
155 {
156 if (getnameinfo((struct sockaddr *)ss, ss->ss_len,
157 buf, len, NULL, 0, NI_NUMERICHOST) != 0) {
158 buf[0] = '\0';
159 return (NULL);
160 }
161 return (buf);
162 }
163
164 char *
tohexstr(uint8_t * bstr,int len)165 tohexstr(uint8_t *bstr, int len)
166 {
167 #define MAXHEXSTRLEN 256
168 static char hstr[2 * MAXHEXSTRLEN + 1];
169 static const char hex[] = "0123456789abcdef";
170 int i;
171
172 if (len > MAXHEXSTRLEN)
173 len = MAXHEXSTRLEN; /* truncate */
174 for (i = 0; i < len; i++) {
175 hstr[i + i] = hex[bstr[i] >> 4];
176 hstr[i + i + 1] = hex[bstr[i] & 0x0f];
177 }
178 hstr[i + i] = '\0';
179 return hstr;
180 }
181
182 uint8_t *
fromhexstr(uint8_t * bstr,const char * hstr,size_t len)183 fromhexstr(uint8_t *bstr, const char *hstr, size_t len)
184 {
185 size_t i;
186 char hex[3];
187
188 if (len % 2 != 0)
189 return NULL;
190
191 hex[2] = '\0';
192 for (i = 0; i < len; i += 2) {
193 if (!isxdigit(hstr[i]) || !isxdigit(hstr[i + 1]))
194 return NULL;
195 hex[0] = hstr[i];
196 hex[1] = hstr[i + 1];
197 bstr[i / 2] = strtol(hex, NULL, 16);
198 }
199
200 return bstr;
201 }
202