/*
* an experimental dsnattest server
* Waits for 27900:UDP, 27901:UDP and returns something
* Usage: $0
*/
#include <stdio.h>
#include <stdlib.h> /*atoi*/
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>	/*inet_aton*/
#include <string.h> /*memcpy*/
#include <sys/select.h>
#include <ctype.h> /* isprint */

void
xdump(FILE *o, const unsigned char *buf, size_t len)
{
	size_t i;
	for (i=0; i<len; i+=16) {
		size_t k;
		for (k=0; k<16 && i+k<len; k++) {
			fprintf(o," %02X", buf[i+k]&0xff);
		}
		for (k; k<16; k++) { fprintf(o, "   "); }
		fputc(' ', o);
		for (k=0; k<16 && i+k<len; k++) {
			register unsigned char c = buf[i+k]&0xff;
			fputc(isprint(c)?c:'.' ,o);
		}
		fputc('\n', o);
	}
}

int
main(int argc, char *argv[])
{
	int	rsock27900, rsock27901;
	struct sockaddr_in	in27900, in27901;
	unsigned char	rbuf[256];

	rsock27900 = socket(PF_INET, SOCK_DGRAM, 0);
	rsock27901 = socket(PF_INET, SOCK_DGRAM, 0);
	in27900.sin_family = in27901.sin_family = AF_INET;
	in27900.sin_port = htons(27900); in27901.sin_port = htons(27901);
	in27900.sin_addr.s_addr = in27901.sin_addr.s_addr = htonl(INADDR_ANY);
	if (bind(rsock27900, &in27900, sizeof(in27900)) <0) { perror("bind 27900"); exit(1); }
	if (bind(rsock27901, &in27901, sizeof(in27901)) <0) { perror("bind 27901"); exit(1); }

	while (1) {
	fd_set	fds;
	int	nfds = 0;
	int	rsock;
	const struct sockaddr_in	*rsock_in;
	struct sockaddr_in	from_addr;
	socklen_t	from_addr_len;
	ssize_t	recvlen;
	ssize_t	sendlen;

	FD_ZERO(&fds); nfds=0;
	FD_SET(rsock27900, &fds); if (nfds<rsock27900+1) nfds=rsock27900+1;
	FD_SET(rsock27901, &fds); if (nfds<rsock27901+1) nfds=rsock27901+1;
	select(nfds, &fds, NULL, NULL, NULL);
	if (FD_ISSET(rsock27900, &fds)) {
		rsock = rsock27900;
		rsock_in = &in27900;
	} else
	if (FD_ISSET(rsock27901, &fds)) {
		rsock = rsock27901;
		rsock_in = &in27901;
	} else {
		/* can't happen */
		continue;
	}

	from_addr_len = sizeof(from_addr);
	recvlen = recvfrom(rsock, rbuf, sizeof(rbuf), 0/*flags*/, &from_addr,&from_addr_len);

	if (recvlen < 0) { perror("recvfrom"); continue; }

	printf("Received port %d client %s:%d len %d\n", ntohs(rsock_in->sin_port), inet_ntoa(from_addr.sin_addr),ntohs(from_addr.sin_port), (int)recvlen);
	xdump(stdout, rbuf,(recvlen<32)?recvlen:32);

	if (rsock == rsock27900) {
		sendlen = 7;
		memcpy(rbuf, "\xfe\xfd\x09\x00\x00\x00\x00", 7);
	} else {

	sendlen = (recvlen<21)?recvlen:21;

	/* genuine natneg server doesn't reply
	 * if first 6 bytes wasn't the magic
	 * FD FC 1E 66 6A B2, but we do for now
	 */

	printf(" proto_ver=%d command=0x%02X msgid=0x%04X server_select=%d\n", rbuf[0x06], rbuf[0x07], rbuf[0x0a]*256+rbuf[0x0b], rbuf[0x0c]);

	/* genuine natneg server only responds if
	 * server_select matches itself,
	 * (natneg1:3, natneg2:2, natneg3:!2&!3)
	 * but we respond anyway for now
	 */

	/*rbuf[0x06] == 3; protocol ver? */
	switch (rbuf[0x07]) /*command*/ {
	  case 0x00: /* return fixed magic in payload */
	  	rbuf[0x07] = 0x01;
		rbuf[0x0d] = 0; /* reply flag */
		memcpy(&rbuf[14], "\xFF\xFF\x6D\x16\xB5\x7D\xEA", 7);
		break;
	  case 0x08: /* plain return */
	  	rbuf[0x07] = 0x09;
		break;
	  case 0x0a: /* return global addr/port seen */
	  	rbuf[0x07] = 0x0b;
		rbuf[0x0d] = 0; /* reply flag */
		memcpy(&rbuf[15], &from_addr.sin_addr, 4);
		memcpy(&rbuf[19], &from_addr.sin_port, 2);
		/*rbuf[15]^=0x55; /*changing the addr will result in 41212/61212 (5th digit 4 or 6)*/
		/*rbuf[20]^=0x55; /*changing the port will result in 21312/31312 (3rd digit 3)*/
		break;
	  case 0x0c: /* plain return */
	  	rbuf[0x07] = 0x02;
		rbuf[0x0d] = 0; /* reply flag */
		break;
	  default:
	  	/* genuine natneg server doesn't reply
		 * anything otherwise, but we will just
		 * echo back for now
		 */
		 break;
	}
	} /*endif*/

	printf("sending back %d bytes\n", sendlen);
	xdump(stdout, rbuf, sendlen);
	sendto(rsock, rbuf, sendlen, 0/*flags*/, &from_addr,from_addr_len);

	}

}