/*
 *      Wi-Fi Detector
 *      ZD1211 UART debugging utility
 *
 *      Utility to read the ZD1211 scan data through a
 *      standard PC serial port.
 *      The ZD1211 has to be connected through a TTL
 *      voltage level converter!
 *
 *   Copyright (C) 2007 Michael Buesch <mb@bu3sch.de>
 *
 *   This program is free software; you can redistribute it and/or
 *   modify it under the terms of the GNU General Public License
 *   as published by the Free Software Foundation; either version 2
 *   of the License, or (at your option) any later version.
 *
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdint.h>
#include <termios.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <getopt.h>
#include <time.h>
#include <stdint.h>


#define BAUDRATE	B115200
#define DEVICE		"/dev/ttyUSB0"

#define PACKET_LENGTH	127

#define MAC_FMT		"%02x:%02x:%02x:%02x:%02x:%02x"
#define MAC_ARG(x)	((uint8_t*)(x))[0],((uint8_t*)(x))[1],\
			((uint8_t*)(x))[2],((uint8_t*)(x))[3],\
			((uint8_t*)(x))[4],((uint8_t*)(x))[5]
#define offsetof(type, member) ((size_t) &((type *)0)->member)


typedef uint16_t	le16_t;
typedef uint64_t	le64_t;

struct data_header {
	uint8_t unknown;
	uint8_t channel;
	uint8_t rssi;
} __attribute__((packed));

struct data_packet {
	struct data_header hdr;
	uint8_t __padding;
	uint8_t bssid[6];
	le16_t seqctl;
	struct {
		le64_t timestamp;
		le16_t beaconint;
		le16_t capab;
		uint8_t ie[0];
	} __attribute__((packed)) beacon;
} __attribute__((packed));


static int fd;


/* Convert a Little-Endian 16bit integer to CPU-endian */
static uint16_t from_le16(le16_t v)
{
	uint16_t ret = 0;

	ret |= (uint16_t)(((uint8_t *)&v)[0]);
	ret |= (uint16_t)(((uint8_t *)&v)[1]) << 8;

	return ret;
}

static int uart_read(uint8_t *data)
{
	int err = 0;
	ssize_t res;

	res = read(fd, data, sizeof(*data));
	if (res == -1) {
		err = errno;
		perror("uart_read");
	}
	if (res == 0) {
		/* EOF */
		err = -1;
	}

	return err;
}

#if 0
static int uart_write(uint8_t data)
{
	int err = 0;
	ssize_t res;

	res = write(fd, &data, sizeof(data));
	if (res != sizeof(data)) {
		err = errno;
		perror("uart_write");
	}

	return err;
}
#endif

static int uart_init(void)
{
	int err = -1;
	struct termios ios;

	fd = open(DEVICE, O_RDWR | O_NOCTTY);
	if (fd < 0) {
		fprintf(stderr, "Open %s: %s\n",
			DEVICE, strerror(errno));
		goto out;
	}

	tcgetattr(fd, &ios);
	cfsetispeed(&ios, BAUDRATE);
	cfsetospeed(&ios, BAUDRATE);
	cfmakeraw(&ios);
	ios.c_cflag &= ~(CSIZE | CLOCAL | CREAD | CSTOPB |
			 PARENB | PARODD);
	ios.c_cflag |= CS8 | CLOCAL | CREAD;
	ios.c_iflag &= ~(INPCK | PARMRK | IXON | IXOFF | BRKINT |
			 INLCR | IGNCR | ICRNL | IUCLC | IMAXBEL |
			 ISTRIP | IGNBRK | IGNPAR);
	ios.c_iflag |= IGNPAR;
	ios.c_lflag &= ~(NOFLSH | ECHO | ECHOE | ECHOK | ECHONL |
			 XCASE | ECHOCTL | ECHOPRT | ECHOKE |
			 PENDIN | ICANON | ISIG);
	ios.c_lflag |= 0;
	ios.c_cc[VINTR]    = 0;     /* Ctrl-c */ 
	ios.c_cc[VQUIT]    = 0;     /* Ctrl-\ */
	ios.c_cc[VERASE]   = 0;     /* del */
	ios.c_cc[VKILL]    = 0;     /* @ */
	ios.c_cc[VEOF]     = 4;     /* Ctrl-d */
	ios.c_cc[VTIME]    = 0;     /* inter-character timer unused */
	ios.c_cc[VMIN]     = 0;     /* blocking read until 1 character arrives */
	ios.c_cc[VSWTC]    = 0;     /* '\0' */
	ios.c_cc[VSTART]   = 0;     /* Ctrl-q */ 
	ios.c_cc[VSTOP]    = 0;     /* Ctrl-s */
	ios.c_cc[VSUSP]    = 0;     /* Ctrl-z */
	ios.c_cc[VEOL]     = 0;     /* '\0' */
	ios.c_cc[VREPRINT] = 0;     /* Ctrl-r */
	ios.c_cc[VDISCARD] = 0;     /* Ctrl-u */
	ios.c_cc[VWERASE]  = 0;     /* Ctrl-w */
	ios.c_cc[VLNEXT]   = 0;     /* Ctrl-v */
	ios.c_cc[VEOL2]    = 0;     /* '\0' */

	tcflush(fd, TCIFLUSH);
	err = tcsetattr(fd, TCSANOW, &ios);
	if (err < 0) {
		fprintf(stderr, "Set tty attributes on %s: %s\n",
			DEVICE, strerror(errno));
		goto err_close;
	}
	err = tcflow(fd, TCION);
	if (err) {
		fprintf(stderr, "Enable input on %s: %s\n",
			DEVICE, strerror(errno));
		goto err_close;
	}
	err = tcflow(fd, TCOON);
	if (err) {
		fprintf(stderr, "Enable output on %s: %s\n",
			DEVICE, strerror(errno));
		goto err_close;
	}

	err = 0;
out:
	return err;
err_close:
	close(fd);
	return err;
}

static void uart_exit(void)
{
	close(fd);
}

static void relax(void)
{
	struct timespec ts;

	ts.tv_sec = 0;
	ts.tv_nsec = 100000000;
	nanosleep(&ts, NULL);
}

static int wait_for_packet(int infinite)
{
	uint8_t c;
	int res;
	int nr_loops = infinite ? -1 : 10;

	printf("Waiting for next packet... ");
	fflush(stdout);
	while (1) {
		res = uart_read(&c);
		if (res > 0)
			exit(1);
		if (res == 0) {
			if (c == 0xAA)
				break;
		}
		if (nr_loops == 0) {
			printf("none.\n");
			return -1;
		}
		relax();
		if (nr_loops >= 0)
			nr_loops--;
	}
	printf("Ok.\n");
	return 0;
}

static size_t find_ie(const struct data_packet *p, uint8_t id, uint8_t *buf)
{
	size_t nrbytes;
	const uint8_t *d;
	uint8_t c;
	uint8_t len;
	size_t i = 0;
	int readit = 0;

	nrbytes = PACKET_LENGTH - offsetof(struct data_packet, beacon.ie);
	d = p->beacon.ie;

	while (1) {
		if (!nrbytes)
			break;
		c = *d;
		d++;
		nrbytes--;
		if (c == id)
			readit = 1;
		if (!nrbytes)
			break;
		len = *d;
		d++;
		nrbytes--;
		for (i = 0; i < len; i++) {
			if (!nrbytes)
				break;
			c = *d;
			d++;
			nrbytes--;
			if (readit)
				buf[i] = c;
		}
		if (readit)
			break;
		readit = 0;
	}

	return i;
}

static void print_packet(const struct data_packet *p)
{
	uint16_t capab;
	uint8_t buf[PACKET_LENGTH];
	size_t i, len;

	printf("\n== Found WLAN ==\n");
	len = find_ie(p, 0, buf);
	if (len) {
		printf("SSID:        \"");
		for (i = 0; i < len; i++)
			printf("%c", buf[i]);
		printf("\"\n");
	}
	printf("Channel:     %u\n", p->hdr.channel);
	printf("RSSI:        %u\n", p->hdr.rssi);
	printf("BSSID:       " MAC_FMT "\n", MAC_ARG(p->bssid));
	capab = from_le16(p->beacon.capab);
	printf("Capability:  %s%s%s%s%s\n",
	       (capab & 0x0001) ? "ESS, " : "",
	       (capab & 0x0002) ? "IBSS, " : "",
	       (capab & 0x0004) ? "CF_pollable, " : "",
	       (capab & 0x0008) ? "CF_poll_req, " : "",
	       (capab & 0x0010) ? "Encrypted, " : "Open, ");
	printf("\n");
}

static void do_work(void)
{
	uint8_t c;
	int i;
	int infinite = 1;
	uint8_t buf[PACKET_LENGTH];

	while (1) {
		if (wait_for_packet(infinite))
			break;
		infinite = 0;
		for (i = 0; i < PACKET_LENGTH; i++) {
			while (uart_read(&c) < 0)
				relax();
			buf[i] = c;
		}
		print_packet((const struct data_packet *)buf);
	}
	printf("\nDone.\n");
}

int main(int argc, char **argv)
{
	int ret = 1;
	int err;

	err = uart_init();
	if (err)
		goto out;

	do_work();

	ret = 0;
	uart_exit();
out:
	return ret;
}