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 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 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 * 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 * 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 * 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