Add "listen for WOL" mode

1 files changed, 229 insertions, 14 deletions

diff --git a/jawol.c b/jawol.c
index 8c06878..481fafe 100644
--- a/jawol.c
+++ b/jawol.c
@@ -38,6 +38,13 @@ static const char usage_msg[] =
 "   --destination IPADDR\n"
 "   -d IPADDR   Destination (broadcast) IP address.\n"
 "\n"
+"Options for listening for WOL packets:\n"
+"   --listen\n"
+"   -l          Listen for incoming WOL packets and print their contents.\n"
+"               Pass twice to force listening for UDP and RAW packets and\n"
+"               bail out if this fails. Otherwise, continue as long as at\n"
+"               least one of the sockets could be bound successfully.\n"
+"\n"
 "Common options:\n"
 "   --verbose\n"
 "   -v          Verbose mode, print some additional information.\n"
@@ -69,6 +76,8 @@ static const char usage_msg[] =
 #include <linux/if.h>
 #include <sys/ioctl.h>
 #include <getopt.h>
+#include <poll.h>
+#include <fcntl.h>
 
 #define PACKET_SIZE (200)
 #define CHECK_PACKET_SIZE(x) do { \
@@ -96,6 +105,12 @@ static int rawsocket();
 static int udpsocket();
 static int etherwake(int count, char **macs);
 static int wakeonlan(int count, char **maclist);
+static int wolisten(bool force);
+static int rawrecv(int sock);
+static int udprecv(int sock);
+static int find_wol(const char *addr, const char *buffer, size_t buflen, const char *type);
+static void formatpw(const uint8_t *pw, int len, char *output, size_t outlen);
+static void sock_nonblock(int sock);
 
 static void print_usage(const char *a0)
 {
@@ -104,9 +119,9 @@ static void print_usage(const char *a0)
 
 int main(int argc, char **argv)
 {
-	int opt;
+	int opt, dolisten = 0;
 	bool gotip = false, gotif = false;
-	static const char *optString = "bDi:p:vhd:";
+	static const char *optString = "bDi:p:vhd:l";
 	static const struct option longOpts[] = {
 		{ "broadcast", no_argument, NULL, 'b' },
 		{ "debug", no_argument, NULL, 'D' },
@@ -115,6 +130,7 @@ int main(int argc, char **argv)
 		{ "verbose", no_argument, NULL, 'v' },
 		{ "help", no_argument, NULL, 'h' },
 		{ "destination", required_argument, NULL, 'd' },
+		{ "listen", no_argument, NULL, 'l' },
 		{ 0, 0, 0, 0 }
 	};
 	// Conveinience: Support MAC addr with dashes
@@ -134,9 +150,17 @@ int main(int argc, char **argv)
 		case 'v': options.verbose = true;                  break;
 		case 'h': print_usage( argv[0] );                  return 0;
 		case 'd': options.dest = get_ip( optarg ); gotip = true; break;
+		case 'l': dolisten++;                              break;
 		case '?': parse_errors++;                          break;
 		}
 	}
+	if ( dolisten && ( gotip || gotif || optind < argc ) ) {
+		fprintf( stderr, "Cannot combine -l with any other options.\n" );
+		return 1;
+	}
+	if ( dolisten ) {
+		return wolisten( dolisten > 1 );
+	}
 	if ( gotip && gotif ) {
 		fprintf( stderr, "Cannot use -d and -b/-i at the same time.\n" );
 		parse_errors++;
@@ -252,13 +276,13 @@ static int rawsocket()
 		if (errno == EPERM) {
 			fprintf(stderr, "This program must be run as root for raw mode to work.\n");
 		} else {
-			perror("socket");
+			perror("RAW socket()");
 		}
 		return -1;
 	}
 	/* This is necessary for broadcasts to work */
 	if ( setsockopt( s, SOL_SOCKET, SO_BROADCAST, (char *)&one, sizeof(one) ) == -1 ) {
-		perror( "setsockopt: SO_BROADCAST" );
+		perror( "RAW setsockopt: SO_BROADCAST" );
 		return -1;
 	}
 	return s;
@@ -271,11 +295,11 @@ int udpsocket()
 
 	sock = socket( AF_INET, SOCK_DGRAM, IPPROTO_UDP );
 	if ( sock == -1 ) {
-		perror( "socket" );
+		perror( "UDP socket()" );
 		return -1;
 	}
 	if ( setsockopt( sock, SOL_SOCKET, SO_BROADCAST, (char *) &optval, sizeof( optval ) ) == -1 ) {
-		perror( "setsockopt: SO_BROADCAST" );
+		perror( "UDP setsockopt: SO_BROADCAST" );
 		return -1;
 	}
 
@@ -377,12 +401,20 @@ static void get_wol_pw(const char *optarg)
 		fprintf(stderr, "The impossible happened.\n");
 		abort();
 	}
-	printf(" The Magic packet password is %2.2x %2.2x %2.2x %2.2x (%d).\n",
-		   passwd[0], passwd[1], passwd[2], passwd[3], byte_cnt);
 	options.wolpwlen = byte_cnt;
 	for (i = 0; i < byte_cnt; i++) {
+		if ( passwd[i] > 255 ) {
+			fprintf( stderr, "Password has invalid value for byte %d: %u.\n",
+					i, passwd[i] );
+			parse_errors++;
+		}
 		options.wolpw[i] = passwd[i];
 	}
+	if ( options.verbose ) {
+		char pw[20];
+		formatpw( (uint8_t*)options.wolpw + i, byte_cnt, pw, sizeof(pw) );
+		printf("The Magic packet password is %s.\n", pw);
+	}
 }
 
 /**
@@ -406,11 +438,10 @@ static int get_source_addr(int s, const char *ifname, unsigned char *output)
 	memcpy(output, if_hwaddr.ifr_hwaddr.sa_data, 6);
 
 	if ( options.verbose ) {
-		unsigned char *hwaddr = (unsigned char*)if_hwaddr.ifr_hwaddr.sa_data;
-		printf("The hardware address (SIOCGIFHWADDR) of %s is type %d  "
-				"%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x.\n", ifname,
-				if_hwaddr.ifr_hwaddr.sa_family, hwaddr[0], hwaddr[1],
-				hwaddr[2], hwaddr[3], hwaddr[4], hwaddr[5]);
+		char mac[20];
+		formatpw( (uint8_t*)if_hwaddr.ifr_hwaddr.sa_data, 6, mac, sizeof(mac) );
+		printf("The hardware address (SIOCGIFHWADDR) of %s is type %d"
+				" (%s).\n", ifname, if_hwaddr.ifr_hwaddr.sa_family, mac);
 	}
 	return 0;
 }
@@ -440,7 +471,7 @@ static int get_dest_addr(const char *hostid, struct ether_addr *dest_ether)
 		}
 	} else {
 		fprintf(stderr,
-					  "ether-wake: The Magic Packet host address must be "
+					  "The Magic Packet host address must be "
 					  "specified as\n"
 					  "  - a station address, 00:11:22:33:44:55, or\n"
 					  "  - a station address, 00-11-22-33-44-55, or\n"
@@ -460,3 +491,187 @@ static struct in_addr get_ip(const char *arg)
 	return tmp;
 }
 
+static int wolisten(bool force)
+{
+	// TODO: RAW
+	struct pollfd socks[2] = {{.events = POLLIN},{.events = POLLIN}};
+	int num = 0;
+	// UDP setup
+	int su = udpsocket();
+	if ( su == -1 ) {
+		if ( force )
+			return 1;
+	} else {
+		struct sockaddr_in addr = {
+			.sin_family = AF_INET,
+			.sin_port = htons( 9 ),
+			.sin_addr = { INADDR_ANY },
+		};
+		socklen_t addrlen = sizeof(addr);
+		sock_nonblock( su );
+		if ( bind( su, (struct sockaddr*)&addr, addrlen ) == -1 ) {
+			perror( "bind on UDP socket" );
+			close( su );
+			if ( force )
+				return 2;
+		} else {
+			socks[num++].fd = su;
+			if ( options.verbose ) {
+				printf( "Listening for UDP WOL packets\n" );
+			}
+		}
+	}
+	// RAW setup
+	int sr = rawsocket();
+	if ( sr == -1 ) {
+		if ( force )
+			return 1;
+	} else {
+		struct sockaddr_ll addr = {
+			.sll_protocol = htons( 0x0842 ),
+			.sll_family = AF_PACKET,
+		};
+		socklen_t addrlen = sizeof(addr);
+		sock_nonblock( sr );
+		if ( bind( sr, (struct sockaddr*)&addr, addrlen ) == -1 ) {
+			perror( "bind on RAW socket" );
+			close( sr );
+			if ( force )
+				return 2;
+		} else {
+			socks[num++].fd = sr;
+			if ( options.verbose ) {
+				printf( "Listening for RAW WOL packets\n" );
+			}
+		}
+	}
+	// Ready to go?
+	if ( num == 0 )
+		return 1;
+	for ( ;; ) {
+		if ( poll( socks, num, -1 ) == -1 ) {
+			if ( errno == EINTR || errno == EAGAIN )
+				continue;
+			perror( "poll" );
+			break;
+		}
+		for ( int i = 0; i < num; ++i ) {
+			if ( socks[i].revents == 0 )
+				continue;
+			if ( socks[i].fd == su ) {
+				udprecv( su );
+			} else if ( socks[i].fd == sr ) {
+				rawrecv( sr );
+			} else {
+				abort();
+			}
+		}
+	}
+	close( su );
+	return 1;
+}
+
+static const uint8_t ETHER_HEADER_LEN = 6 + 6 + 2; // 2x MAC + type
+
+static int rawrecv(int sock)
+{
+	char buffer[PACKET_SIZE], addrstr[50];
+	struct sockaddr_ll src = {0};
+	socklen_t srclen = sizeof(src);
+	ssize_t ret = recvfrom( sock, buffer, sizeof(buffer), 0, (struct sockaddr*)&src, &srclen );
+	if ( ret < 0 ) {
+		if ( errno == EINTR || errno == EAGAIN )
+			return 0;
+		perror( "recvfrom for RAW socket" );
+		return 1;
+	}
+	if ( ret < ETHER_HEADER_LEN ) {
+		printf( "Short raw packet received (%d bytes)\n", (int)ret );
+		return 0;
+	}
+	formatpw( (uint8_t*)src.sll_addr, src.sll_halen, addrstr, sizeof(addrstr) );
+	if ( options.debug ) {
+		printf( "Received RAW packet of size %d from %s!\n", (int)ret, addrstr );
+	}
+	return find_wol( addrstr, buffer + ETHER_HEADER_LEN, (size_t)ret - ETHER_HEADER_LEN, "RAW" );
+}
+
+static int udprecv(int sock)
+{
+	char buffer[PACKET_SIZE], addrstr[50];
+	struct sockaddr_in src = {0};
+	socklen_t srclen = sizeof(src);
+	ssize_t ret = recvfrom( sock, buffer, sizeof(buffer), 0, (struct sockaddr*)&src, &srclen );
+	if ( ret < 0 ) {
+		if ( errno == EINTR || errno == EAGAIN )
+			return 0;
+		perror( "recvfrom for UDP socket" );
+		return 1;
+	}
+	if ( inet_ntop( AF_INET, &src.sin_addr, addrstr, sizeof(addrstr) ) == NULL ) {
+		perror( "Cannot determine source address of received UDP packet" );
+		return 0;
+	}
+	if ( options.debug ) {
+		printf( "Received UDP datagram of size %d from %s!\n", (int)ret, addrstr );
+	}
+	return find_wol( addrstr, buffer, (size_t)ret, "UDP" );
+}
+
+static int find_wol(const char *addrstr, const char *buffer, size_t buflen, const char *type)
+{
+	const uint8_t *sync = (uint8_t*)memmem( buffer, buflen, "\xff\xff\xff\xff\xff\xff", 6 );
+	if ( sync == NULL ) {
+		if ( options.debug ) {
+			printf( "... no sync bytes found\n" );
+		}
+		return 0;
+	}
+	sync += 6;
+	const uint8_t *end = (uint8_t*)buffer + buflen;
+	const uint8_t *pos = sync;
+	int count = 0;
+	while ( count < 16 && pos + 6 <= end ) {
+		if ( memcmp( sync, pos, 6 ) != 0 )
+			break;
+		pos += 6;
+		count++;
+	}
+	if ( count == 16 ) {
+		char passwd[20];
+		char mac[20];
+		formatpw( sync, 6, mac, sizeof(mac) );
+		formatpw( pos, end - pos, passwd, sizeof(passwd) );
+		printf( "MAGIC source=%s dest=%s type=%s passwd=%s\n", addrstr, mac, type, passwd );
+	} else if ( options.verbose ) {
+		printf( "Sync bytes found, with %d repetitions of the following 6 bytes.\n", count );
+	}
+	return 0;
+}
+
+static void formatpw(const uint8_t *pw, int len, char *output, size_t outlen)
+{
+	if ( outlen <= 0 )
+		return;
+	if ( len == 4 ) {
+		snprintf( output, outlen, "%u.%u.%u.%u", pw[0], pw[1], pw[2], pw[3] );
+	} else if ( len == 6 ) {
+		snprintf( output, outlen, "%02x:%02x:%02x:%02x:%02x:%02x", pw[0], pw[1], pw[2], pw[3], pw[4], pw[5] );
+	} else {
+		*output = '\0';
+	}
+}
+
+static void sock_nonblock(int sock)
+{
+	errno = 0;
+	int f = fcntl( sock, F_GETFL );
+	if ( errno != 0 ) {
+		perror( "fcntl(F_GETFL)" );
+		return;
+	}
+	if ( fcntl( sock, F_SETFL, f | O_NONBLOCK ) == -1 ) {
+		perror( "fcntl(F_SETFL, ... | O_NONBLOCK)" );
+	}
+}
+