chiark / gitweb /
Merge commit 'd817b49007e60b0e7392f23caf08dc0328eb32de' into HEAD
[secnet.git] / stest / udp-preload.c
1 /*
2  *  udp-preload.c - testing mock library for secnet udp
3  *  This file is part of secnet.
4  *
5  *  Copyright (C) 1998,2003-2004,2012,2017,2019 Ian Jackson
6  * 
7  *  This program is free software; you can redistribute it and/or modify
8  *  it under the terms of the GNU General Public License as published by
9  *  the Free Software Foundation; either version 3, or (at your option)
10  *  any later version.
11  * 
12  *  This program is distributed in the hope that it will be useful,
13  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
14  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15  *  GNU General Public License for more details.
16  * 
17  * You should have received a copy of the GNU General Public License
18  * version 3 along with secnet; if not, see
19  * https://www.gnu.org/licenses/gpl.html.
20  */
21
22 #define _GNU_SOURCE
23
24 #include <dlfcn.h>
25 #include <stdio.h>
26 #include <sys/types.h>
27 #include <unistd.h>
28 #include <errno.h>
29 #include <stdlib.h>
30 #include <string.h>
31 #include <sys/socket.h>
32 #include <netinet/in.h>
33 #include <sys/un.h>
34 #include <arpa/inet.h>
35
36 #define STDERRSTR_CONST(m) write(2,m,sizeof(m)-1)
37 #define STDERRSTR_STRING(m) write(2,m,strlen(m))
38
39 typedef void anyfn_type(void);
40
41 static anyfn_type *find_any(const char *name) {
42   static const char *dlerr;
43   anyfn_type *kv;
44
45   kv= dlsym(RTLD_NEXT,name); if (kv) return kv;
46   dlerr= dlerror(); if (!dlerr) dlerr= "dlsym() failed for no reason";
47   STDERRSTR_CONST("udp-preload: error finding original version of ");
48   STDERRSTR_STRING(name);
49   STDERRSTR_CONST(": ");
50   STDERRSTR_STRING(dlerr);
51   STDERRSTR_STRING("\n");
52   errno= ENOSYS;
53   return 0;
54 }
55
56 #define socket_args int domain, int type, int protocol
57 #define close_args  int fd
58 #define bind_args   int fd, const struct sockaddr *addr, socklen_t addrlen
59 #define sendto_args int fd, const void *buf, size_t len, int flags, \
60                     const struct sockaddr *addr, socklen_t addrlen
61 #define recvfrom_args  int fd, void *buf, size_t len, int flags, \
62                        struct sockaddr *addr, socklen_t *addrlen
63 #define setsockopt_args  int fd, int level, int optname, \
64                          const void *optval, socklen_t optlen
65 #define getsockname_args int fd, struct sockaddr *addr, socklen_t *addrlen
66 #define WRAPS(X)                                                \
67     X(socket,     int,     (domain,type,protocol))              \
68     X(close,      int,     (fd))                                \
69     X(bind,       int,     (fd,addr,addrlen))                   \
70     X(sendto,     ssize_t, (fd,buf,len,flags,addr,addrlen))     \
71     X(recvfrom,   ssize_t, (fd,buf,len,flags,addr,addrlen))     \
72     X(setsockopt, int,     (fd,level,optname,optval,optlen))    \
73     X(getsockname,int,     (fd,addr,addrlen))
74
75 #define DEF_OLD(fn,rt,args)                             \
76   typedef rt fn##_fn_type(fn##_args);                   \
77   static fn##_fn_type find_##fn, *old_##fn=find_##fn;   \
78   static rt find_##fn(fn##_args) {                      \
79     anyfn_type *anyfn;                                  \
80     anyfn= find_any(#fn); if (!anyfn) return -1;        \
81     old_##fn= (fn##_fn_type*)anyfn;                     \
82     return old_##fn args;                               \
83   }
84
85 WRAPS(DEF_OLD)
86
87 #define WRAP(fn) int fn(fn##_args)
88 #define TWRAP(fn) fn(fn##_args)
89
90 typedef struct{
91     int af;
92 } fdinfo;
93 static fdinfo **table;
94 static int tablesz;
95
96 static fdinfo *lookup(int fd) {
97     if (fd<0 || fd>=tablesz) return 0;
98     return table[fd];
99 }
100
101 #define ADDRPORTSTRLEN (INET6_ADDRSTRLEN+1+5) /* not including nul */
102
103 static int addrport2str(char buf[ADDRPORTSTRLEN+1],
104                         const struct sockaddr *addr, socklen_t addrlen) {
105     const void *addrv=addr;
106     const void *iav;
107     const struct sockaddr_in  *sin;
108     const struct sockaddr_in6 *sin6;
109     uint16_t port;
110     socklen_t el;
111     switch (addr->sa_family) {
112     case AF_INET:  sin =addrv; el=sizeof(*sin ); iav=&sin ->sin_addr ; port=sin ->sin_port ; break;
113     case AF_INET6: sin6=addrv; el=sizeof(*sin6); iav=&sin6->sin6_addr; port=sin6->sin6_port; break;
114     default: errno=ESRCH; return -1;
115     }
116 //fprintf(stderr,"af=%lu el=%lu addrlen=%lu\n",
117 //      (unsigned long)addr->sa_family,
118 //      (unsigned long)el,
119 //      (unsigned long)addrlen);
120     if (addrlen!=el) { errno=EINVAL; return -1; }
121     char *p=buf;
122     if (!inet_ntop(addr->sa_family,iav,p,INET6_ADDRSTRLEN)) return -1;
123     p+=strlen(p);
124     sprintf(p,",%u",(unsigned)ntohs(port));
125     return 0;
126 }
127
128 static int str2addrport(char *str,
129                         struct sockaddr *addr, socklen_t *addrlen) {
130     union {
131         struct sockaddr_in  sin;
132         struct sockaddr_in6 sin6;
133     } si;
134
135     memset(&si,0,sizeof(si));
136
137     int af;
138     void *iav;
139     uint16_t *portp;
140     socklen_t al;
141     switch (str[strcspn(str,".:")]) {
142     case '.': af=AF_INET ; iav=&si.sin .sin_addr ; al=sizeof(si.sin ); portp=&si.sin .sin_port ; break;
143     case ':': af=AF_INET6; iav=&si.sin6.sin6_addr; al=sizeof(si.sin6); portp=&si.sin6.sin6_port; break;
144     default: errno=ESRCH; return -1;
145     }
146     si.sin.sin_family=af;
147
148     char *comma=strchr(str,',');
149     if (!comma) { errno=ESRCH; return -1; }
150     *comma++=0;
151     int r=inet_pton(af,str,iav);
152 //fprintf(stderr,"inet_pton(%d,\"%s\",)=%d\n",af,str,r);
153     if (r<0) return -1;
154     if (r==0) { errno=ENOTTY; return -1; }
155
156     char *ep;
157     errno=0;
158     unsigned long port=strtoul(comma,&ep,10);
159     if (ep==comma || *ep || errno || port>65536) { errno=ESRCH; return -1; }
160     *portp= htons(port);
161
162     if (addr) memcpy(addr,&si, *addrlen<al ? *addrlen : al);
163     *addrlen=al;
164     return 0;
165 }
166
167 static char *sun_prep(struct sockaddr_un *sun) {
168     const char *dir=getenv("UDP_PRELOAD_DIR");
169     if (!dir) { errno=ECHILD; return 0; }
170
171     memset(sun,0,sizeof(*sun));
172     sun->sun_family=AF_UNIX;
173     size_t dl = strlen(dir);
174     if (dl + 1 + ADDRPORTSTRLEN + 1 > sizeof(sun->sun_path)) {
175         errno=ENAMETOOLONG; return 0;
176     }
177     strcpy(sun->sun_path,dir);
178     char *p=sun->sun_path+dl;
179     *p++='/';
180     return p;
181 }
182
183 WRAP(socket) {
184     if (!((domain==AF_INET || domain==AF_INET6) &&
185           type==SOCK_DGRAM))
186         return old_socket(domain,type,protocol);
187     int fd=socket(AF_UNIX,SOCK_DGRAM,0);
188     if (fd<0) return fd;
189     if (fd>=tablesz) {
190         int newsz=(fd+1)*2;
191         table=realloc(table,newsz*sizeof(*table));
192         if (!table) goto fail;
193         while (tablesz<newsz) table[tablesz++]=0;
194     }
195     free(table[fd]);
196     table[fd]=malloc(sizeof(*table[fd]));
197     if (!table[fd]) goto fail;
198     table[fd]->af=domain;
199     return fd;
200
201  fail:
202     close(fd);
203     return -1;
204 }
205
206 WRAP(close) {
207     if (fd>=0 && fd<tablesz) {
208         free(table[fd]);
209         table[fd]=0;
210     }
211     return old_close(fd);
212 }
213
214 WRAP(bind) {
215     fdinfo *ent=lookup(fd);
216     if (!ent) return old_bind(fd,addr,addrlen);
217     struct sockaddr_un sun;
218     char *p=sun_prep(&sun);
219     if (addrport2str(p,addr,addrlen)) return -1;
220 //fprintf(stderr,"binding %s\n",sun.sun_path);
221     if (unlink(sun.sun_path) && errno!=ENOENT) return -1;
222     return old_bind(fd,(const void*)&sun,sizeof(sun));
223 }
224
225 WRAP(setsockopt) {
226     fdinfo *ent=lookup(fd);
227     if (!ent) return old_setsockopt(fd,level,optname,optval,optlen);
228     if (ent->af==AF_INET6 && level==IPPROTO_IPV6 && optname==IPV6_V6ONLY
229         && optlen==sizeof(int) && *(int*)optval==1) {
230         return 0;
231     }
232     errno=ENOTTY;
233     return -1;
234 }
235
236 WRAP(getsockname) {
237     fdinfo *ent=lookup(fd);
238     if (!ent) return old_getsockname(fd,addr,addrlen);
239     struct sockaddr_un sun;
240     socklen_t sunlen=sizeof(sun);
241     if (old_getsockname(fd,(void*)&sun,&sunlen)) return -1;
242     if (sun.sun_family!=AF_UNIX || sunlen>sizeof(sun)) {
243 //fprintf(stderr,"old_getsockname af=%lu sunlen=%lu\n",
244 //      (unsigned long)sun.sun_family,
245 //      (unsigned long)sunlen);
246         errno=EDOM; return -1;
247     }
248     char *slash=strrchr(sun.sun_path,'/');
249     if (str2addrport(slash ? slash+1 : sun.sun_path,
250                      addr,addrlen)) return -1;
251     return 0;
252 }
253
254 ssize_t TWRAP(sendto) {
255     fdinfo *ent=lookup(fd);
256     if (!ent) return old_sendto(fd,buf,len,flags,addr,addrlen);
257
258     if (flags) { errno=ENOEXEC; return -1; }
259
260     const char *leaf=getenv("UDP_PRELOAD_SERVER");
261     if (!leaf) leaf="udp";
262     if (strlen(leaf) > ADDRPORTSTRLEN) { errno=ENAMETOOLONG; return -1; }
263     struct sockaddr_un sun;
264     char *p=sun_prep(&sun);
265     strcpy(p,leaf);
266
267     char tbuf[ADDRPORTSTRLEN+1];
268     memset(tbuf,0,sizeof(tbuf));
269     if (addrport2str(tbuf,addr,addrlen)) return -1;
270
271     struct iovec iov[2];
272     iov[0].iov_base=tbuf;
273     iov[0].iov_len=sizeof(tbuf);
274     iov[1].iov_base=(void*)buf;
275     iov[1].iov_len=len;
276     
277     struct msghdr m;
278     memset(&m,0,sizeof(m));
279     m.msg_name=&sun;
280     m.msg_namelen=sizeof(sun);
281     m.msg_iov=iov;
282     m.msg_iovlen=2;
283
284     return sendmsg(fd,&m,0);
285 }
286
287 ssize_t TWRAP(recvfrom) {
288     fdinfo *ent=lookup(fd);
289     if (!ent) return old_recvfrom(fd,buf,len,flags,addr,addrlen);
290
291 //fprintf(stderr,"recvfrom %d len=%lu flags=%d al=%lu\n",
292 //      fd, (unsigned long)len, flags, (unsigned long)*addrlen);
293
294     if (flags) { errno=ENOEXEC; return -1; }
295
296     char tbuf[ADDRPORTSTRLEN+1];
297
298     struct iovec iov[2];
299     iov[0].iov_base=tbuf;
300     iov[0].iov_len=sizeof(tbuf);
301     iov[1].iov_base=buf;
302     iov[1].iov_len=len;
303
304     struct msghdr m;
305     memset(&m,0,sizeof(m));
306     m.msg_iov=iov;
307     m.msg_iovlen=2;
308
309     ssize_t rr=recvmsg(fd,&m,0);
310     if (rr==-1) return rr;
311     if ((size_t)rr<sizeof(tbuf)) { errno=ENXIO; return -1; }
312     if (tbuf[ADDRPORTSTRLEN]) { errno=E2BIG; return -1; }
313     if (str2addrport(tbuf,addr,addrlen)) {
314         fprintf(stderr, "recvfrom str2addrport `%s' %s\n",tbuf,
315                 strerror(errno));
316         return -1;
317     }
318
319     rr -= sizeof(tbuf);
320 //fprintf(stderr,"recvfrom %s %lu ok\n",tbuf,(unsigned long)rr);
321     return rr;
322 }