2 files changed, 218 insertions, 22 deletions

diff --git a/common/socket.c b/common/socket.c
index 777b23e..cf6e9eb 100644
--- a/common/socket.c
+++ b/common/socket.c
@@ -1,7 +1,7 @@
 /*
  * socket.c
  *
- * Copyright (C) 2012-2018 Nikias Bassen <nikias@gmx.li>
+ * Copyright (C) 2012-2020 Nikias Bassen <nikias@gmx.li>
  * Copyright (C) 2012 Martin Szulecki <m.szulecki@libimobiledevice.org>
  *
  * This library is free software; you can redistribute it and/or
@@ -49,6 +49,9 @@ static int wsa_init = 0;
 #define RECV_TIMEOUT 20000
 #define CONNECT_TIMEOUT 5000
 
+#ifndef EAFNOSUPPORT
+#define EAFNOSUPPORT 102
+#endif
 #ifndef ECONNRESET
 #define ECONNRESET 108
 #endif
@@ -63,6 +66,59 @@ void socket_set_verbose(int level)
         verbose = level;
 }
 
+const char *socket_addr_to_string(struct sockaddr *addr, char *addr_out, size_t addr_out_size)
+{
+#ifdef WIN32
+        WSADATA wsa_data;
+        if (!wsa_init) {
+                if (WSAStartup(MAKEWORD(2,2), &wsa_data) != ERROR_SUCCESS) {
+                        fprintf(stderr, "WSAStartup failed!\n");
+                        ExitProcess(-1);
+                }
+                wsa_init = 1;
+        }
+        DWORD addr_out_len = addr_out_size;
+        DWORD addrlen = 0;
+
+        if (addr->sa_family == AF_INET) {
+                addrlen = sizeof(struct sockaddr_in);
+        }
+#ifdef AF_INET6
+        else if (addr->sa_family == AF_INET6) {
+                addrlen = sizeof(struct sockaddr_in6);
+        }
+#endif
+        else {
+                errno = EAFNOSUPPORT;
+                return NULL;
+        }
+
+        if (WSAAddressToString(addr, addrlen, NULL, addr_out, &addr_out_len) == 0) {
+                return addr_out;
+        }
+#else
+        const void *addrdata = NULL;
+
+        if (addr->sa_family == AF_INET) {
+                addrdata = &((struct sockaddr_in*)addr)->sin_addr;
+        }
+#ifdef AF_INET6
+        else if (addr->sa_family == AF_INET6) {
+                addrdata = &((struct sockaddr_in6*)addr)->sin6_addr;
+        }
+#endif
+        else {
+                errno = EAFNOSUPPORT;
+                return NULL;
+        }
+
+        if (inet_ntop(addr->sa_family, addrdata, addr_out, addr_out_size)) {
+                return addr_out;
+        }
+#endif
+        return NULL;
+}
+
 #ifndef WIN32
 int socket_create_unix(const char *filename)
 {
@@ -161,11 +217,37 @@ int socket_connect_unix(const char *filename)
         strncpy(name.sun_path, filename, sizeof(name.sun_path));
         name.sun_path[sizeof(name.sun_path) - 1] = 0;
 
-        if (connect(sfd, (struct sockaddr*)&name, sizeof(name)) < 0) {
+        int flags = fcntl(sfd, F_GETFL, 0);
+        fcntl(sfd, F_SETFL, flags | O_NONBLOCK);
+
+        do {
+                if (connect(sfd, (struct sockaddr*)&name, sizeof(name)) != -1) {
+                        break;
+                }
+                if (errno == EINPROGRESS) {
+                        fd_set fds;
+                        FD_ZERO(&fds);
+                        FD_SET(sfd, &fds);
+
+                        struct timeval timeout;
+                        timeout.tv_sec = CONNECT_TIMEOUT / 1000;
+                        timeout.tv_usec = (CONNECT_TIMEOUT - (timeout.tv_sec * 1000)) * 1000;
+                        if (select(sfd + 1, NULL, &fds, NULL, &timeout) == 1) {
+                                int so_error;
+                                socklen_t len = sizeof(so_error);
+                                getsockopt(sfd, SOL_SOCKET, SO_ERROR, (void*)&so_error, &len);
+                                if (so_error == 0) {
+                                        break;
+                                }
+                        }
+                }
                 socket_close(sfd);
+                sfd = -1;
+        } while (0);
+
+        if (sfd < 0) {
                 if (verbose >= 2)
-                        fprintf(stderr, "%s: connect: %s\n", __func__,
-                                        strerror(errno));
+                        fprintf(stderr, "%s: connect: %s\n", __func__, strerror(errno));
                 return -1;
         }
 
@@ -228,6 +310,124 @@ int socket_create(uint16_t port)
         return sfd;
 }
 
+int socket_connect_addr(struct sockaddr* addr, uint16_t port)
+{
+        int sfd = -1;
+        int yes = 1;
+        int bufsize = 0x20000;
+        int addrlen = 0;
+#ifdef WIN32
+        u_long l_yes = 1;
+        WSADATA wsa_data;
+        if (!wsa_init) {
+                if (WSAStartup(MAKEWORD(2,2), &wsa_data) != ERROR_SUCCESS) {
+                        fprintf(stderr, "WSAStartup failed!\n");
+                        ExitProcess(-1);
+                }
+                wsa_init = 1;
+        }
+#endif
+
+        if (addr->sa_family == AF_INET) {
+                struct sockaddr_in* addr_in = (struct sockaddr_in*)addr;
+                addr_in->sin_port = htons(port);
+                addrlen = sizeof(struct sockaddr_in);
+        }
+#ifdef AF_INET6
+        else if (addr->sa_family == AF_INET6) {
+                struct sockaddr_in6* addr_in = (struct sockaddr_in6*)addr;
+                addr_in->sin6_port = htons(port);
+                addrlen = sizeof(struct sockaddr_in6);
+        }
+#endif
+        else {
+                fprintf(stderr, "ERROR: Unsupported address family");
+                return -1;
+        }
+
+        sfd = socket(addr->sa_family, SOCK_STREAM, IPPROTO_TCP);
+        if (sfd == -1) {
+                perror("socket()");
+                return -1;
+        }
+
+#ifdef SO_NOSIGPIPE
+        if (setsockopt(sfd, SOL_SOCKET, SO_NOSIGPIPE, (void*)&yes, sizeof(int)) == -1) {
+                perror("setsockopt()");
+                socket_close(sfd);
+                return -1;
+        }
+#endif
+
+        if (setsockopt(sfd, SOL_SOCKET, SO_REUSEADDR, (void*)&yes, sizeof(int)) == -1) {
+                perror("setsockopt()");
+                socket_close(sfd);
+                return -1;
+        }
+
+#ifdef WIN32
+        ioctlsocket(sfd, FIONBIO, &l_yes);
+#else
+        int flags = fcntl(sfd, F_GETFL, 0);
+        fcntl(sfd, F_SETFL, flags | O_NONBLOCK);
+#endif
+
+        do {
+                if (connect(sfd, addr, addrlen) != -1) {
+                        break;
+                }
+#ifdef WIN32
+                if (WSAGetLastError() == WSAEWOULDBLOCK)
+#else
+                if (errno == EINPROGRESS)
+#endif
+                {
+                        fd_set fds;
+                        FD_ZERO(&fds);
+                        FD_SET(sfd, &fds);
+
+                        struct timeval timeout;
+                        timeout.tv_sec = CONNECT_TIMEOUT / 1000;
+                        timeout.tv_usec = (CONNECT_TIMEOUT - (timeout.tv_sec * 1000)) * 1000;
+                        if (select(sfd + 1, NULL, &fds, NULL, &timeout) == 1) {
+                                int so_error;
+                                socklen_t len = sizeof(so_error);
+                                getsockopt(sfd, SOL_SOCKET, SO_ERROR, (void*)&so_error, &len);
+                                if (so_error == 0) {
+                                        errno = 0;
+                                        break;
+                                }
+                                errno = so_error;
+                        }
+                }
+                socket_close(sfd);
+                sfd = -1;
+        } while (0);
+
+        if (sfd < 0) {
+                if (verbose >= 2) {
+                        char addrtxt[48];
+                        socket_addr_to_string(addr, addrtxt, sizeof(addrtxt));
+                        fprintf(stderr, "%s: Could not connect to %s port %d\n", __func__, addrtxt, port);
+                }
+                return -1;
+        }
+
+        if (setsockopt(sfd, IPPROTO_TCP, TCP_NODELAY, (void*)&yes, sizeof(int)) == -1) {
+                perror("Could not set TCP_NODELAY on socket");
+        }
+
+        if (setsockopt(sfd, SOL_SOCKET, SO_SNDBUF, (void*)&bufsize, sizeof(int)) == -1) {
+                perror("Could not set send buffer for socket");
+        }
+
+        if (setsockopt(sfd, SOL_SOCKET, SO_RCVBUF, (void*)&bufsize, sizeof(int)) == -1) {
+                perror("Could not set receive buffer for socket");
+        }
+
+        return sfd;
+}
+
 int socket_connect(const char *addr, uint16_t port)
 {
         int sfd = -1;
@@ -239,7 +439,6 @@ int socket_connect(const char *addr, uint16_t port)
         int res;
 #ifdef WIN32
         u_long l_yes = 1;
-        u_long l_no = 0;
         WSADATA wsa_data;
         if (!wsa_init) {
                 if (WSAStartup(MAKEWORD(2,2), &wsa_data) != ERROR_SUCCESS) {
@@ -277,6 +476,14 @@ int socket_connect(const char *addr, uint16_t port)
                         continue;
                 }
 
+#ifdef SO_NOSIGPIPE
+                if (setsockopt(sfd, SOL_SOCKET, SO_NOSIGPIPE, (void*)&yes, sizeof(int)) == -1) {
+                        perror("setsockopt()");
+                        socket_close(sfd);
+                        return -1;
+                }
+#endif
+
                 if (setsockopt(sfd, SOL_SOCKET, SO_REUSEADDR, (void*)&yes, sizeof(int)) == -1) {
                         perror("setsockopt()");
                         socket_close(sfd);
@@ -286,7 +493,8 @@ int socket_connect(const char *addr, uint16_t port)
 #ifdef WIN32
                 ioctlsocket(sfd, FIONBIO, &l_yes);
 #else
-                fcntl(sfd, F_SETFL, O_NONBLOCK);
+                flags = fcntl(sfd, F_GETFL, 0);
+                fcntl(sfd, F_SETFL, flags | O_NONBLOCK);
 #endif
 
                 if (connect(sfd, rp->ai_addr, rp->ai_addrlen) != -1) {
@@ -325,21 +533,6 @@ int socket_connect(const char *addr, uint16_t port)
                 return -1;
         }
 
-#ifdef WIN32
-        ioctlsocket(sfd, FIONBIO, &l_no);
-#else
-        flags = fcntl(sfd, F_GETFL, 0);
-        fcntl(sfd, F_SETFL, flags & (~O_NONBLOCK));
-#endif
-
-#ifdef SO_NOSIGPIPE
-        if (setsockopt(sfd, SOL_SOCKET, SO_NOSIGPIPE, (void*)&yes, sizeof(int)) == -1) {
-                perror("setsockopt()");
-                socket_close(sfd);
-                return -1;
-        }
-#endif
-
         if (setsockopt(sfd, IPPROTO_TCP, TCP_NODELAY, (void*)&yes, sizeof(int)) == -1) {
                 perror("Could not set TCP_NODELAY on socket");
         }
diff --git a/common/socket.h b/common/socket.h
index e31de6b..38eeddf 100644
--- a/common/socket.h
+++ b/common/socket.h
@@ -1,8 +1,8 @@
 /*
  * socket.h
  *
+ * Copyright (C) 2012-2020 Nikias Bassen <nikias@gmx.li>
  * Copyright (C) 2012 Martin Szulecki <m.szulecki@libimobiledevice.org>
- * Copyright (C) 2012 Nikias Bassen <nikias@gmx.li>
  *
  * This library is free software; you can redistribute it and/or
  * modify it under the terms of the GNU Lesser General Public
@@ -46,6 +46,7 @@ int socket_create_unix(const char *filename);
 int socket_connect_unix(const char *filename);
 #endif
 int socket_create(uint16_t port);
+int socket_connect_addr(struct sockaddr *addr, uint16_t port);
 int socket_connect(const char *addr, uint16_t port);
 int socket_check_fd(int fd, fd_mode fdm, unsigned int timeout);
 int socket_accept(int fd, uint16_t port);
@@ -62,4 +63,6 @@ int socket_send(int fd, void *data, size_t size);
 
 void socket_set_verbose(int level);
 
+const char *socket_addr_to_string(struct sockaddr *addr, char *addr_out, size_t addr_out_size);
+
 #endif  /* SOCKET_SOCKET_H */