ethstream/nerdjack.c

/*
 * Labjack Tools
 * Copyright (c) 2003-2007 Jim Paris <jim@jtan.com>
 *
 * This is free software; you can redistribute it and/or modify it and
 * it is provided under the terms of version 2 of the GNU General Public
 * License as published by the Free Software Foundation; see COPYING.
 */

#include <errno.h>
#include <stdint.h>
#include <stdlib.h>
#include <unistd.h>
#include <stdio.h>
#include <string.h>

#include <math.h>

#include "netutil.h"
#include "compat.h"

#include "debug.h"
#include "nerdjack.h"
#include "util.h"
#include "netutil.h"
#include "ethstream.h"

#define NERD_HEADER_SIZE 8
#define MAX_SOCKETS 32

typedef struct __attribute__ ((__packed__)) {
	unsigned char headerone;
	unsigned char headertwo;
	unsigned short packetNumber;
	unsigned short adcused;
	unsigned short packetsready;
	signed short data[NERDJACK_NUM_SAMPLES];
} dataPacket;

struct discovered_socket {
	int sock;
	uint32_t local_ip;
	uint32_t subnet_mask;
};

struct discover_t {
	struct discovered_socket socks[MAX_SOCKETS];
	unsigned int sock_count;
};

/* Choose the best ScanConfig and ScanInterval parameters for the
   desired scanrate.  Returns -1 if no valid config found */
int
nerdjack_choose_scan(double desired_rate, double *actual_rate,
		     unsigned long *period)
{
	//The ffffe is because of a silicon bug.  The last bit is unusable in all
	//devices so far.  It is worked around on the chip, but giving it exactly
	//0xfffff would cause the workaround code to roll over.
	*period = floor((double)NERDJACK_CLOCK_RATE / desired_rate);
	if (*period > 0x0ffffe) {
		info("Cannot sample that slowly\n");
		*actual_rate = (double)NERDJACK_CLOCK_RATE / (double)0x0ffffe;
		*period = 0x0ffffe;
		return -1;
	}
	//Period holds the period register for the NerdJack, so it needs to be right
	*actual_rate = (double)NERDJACK_CLOCK_RATE / (double)*period;
	if (*actual_rate != desired_rate) {
		return -1;
	}
	return 0;
}

/**
* Create a discovered socket and add it to the socket list structure.
* All sockets in the structure should be created, bound, and ready for broadcasting
*/
static int discovered_sock_create(struct discover_t *ds, uint32_t local_ip,
				  uint32_t subnet_mask)
{
	if (ds->sock_count >= MAX_SOCKETS) {
		return 0;
	}

	/* Create socket. */
	int sock = (int)socket(AF_INET, SOCK_DGRAM, 0);
	if (sock == -1) {
		return 0;
	}

	/* Allow broadcast. */
	int sock_opt = 1;
	setsockopt(sock, SOL_SOCKET, SO_BROADCAST, (char *)&sock_opt,
		   sizeof(sock_opt));

	/* Set nonblocking */
	if (soblock(sock, 0) < 0) {
		verb("can't set nonblocking\n");
		close(sock);
		return 0;
	}

	/* Bind socket. */
	struct sockaddr_in sock_addr;
	memset(&sock_addr, 0, sizeof(sock_addr));
	sock_addr.sin_family = AF_INET;
	sock_addr.sin_addr.s_addr = htonl(local_ip);
	sock_addr.sin_port = htons(0);
	if (bind(sock, (struct sockaddr *)&sock_addr, sizeof(sock_addr)) != 0) {
		close(sock);
		return 0;
	}

	/* Write sock entry. */
	struct discovered_socket *dss = &ds->socks[ds->sock_count++];
	dss->sock = sock;
	dss->local_ip = local_ip;
	dss->subnet_mask = subnet_mask;

	return 1;
}

/**
 * Enumerate all interfaces we can find and open sockets on each
 */
#if defined(USE_IPHLPAPI)
static void enumerate_interfaces(struct discover_t *ds)
{
	PIP_ADAPTER_INFO pAdapterInfo =
	    (IP_ADAPTER_INFO *) malloc(sizeof(IP_ADAPTER_INFO));
	ULONG ulOutBufLen = sizeof(IP_ADAPTER_INFO);

	DWORD Ret = GetAdaptersInfo(pAdapterInfo, &ulOutBufLen);
	if (Ret != NO_ERROR) {
		free(pAdapterInfo);
		if (Ret != ERROR_BUFFER_OVERFLOW) {
			return;
		}
		pAdapterInfo = (IP_ADAPTER_INFO *) malloc(ulOutBufLen);
		Ret = GetAdaptersInfo(pAdapterInfo, &ulOutBufLen);
		if (Ret != NO_ERROR) {
			free(pAdapterInfo);
			return;
		}
	}

	PIP_ADAPTER_INFO pAdapter = pAdapterInfo;
	while (pAdapter) {
		IP_ADDR_STRING *pIPAddr = &pAdapter->IpAddressList;
		while (pIPAddr) {
			uint32_t local_ip =
			    ntohl(inet_addr(pIPAddr->IpAddress.String));
			uint32_t mask =
			    ntohl(inet_addr(pIPAddr->IpMask.String));

			if (local_ip == 0) {
				pIPAddr = pIPAddr->Next;
				continue;
			}

			discovered_sock_create(ds, local_ip, mask);
			pIPAddr = pIPAddr->Next;
		}

		pAdapter = pAdapter->Next;
	}

	free(pAdapterInfo);
}

#else
static void enumerate_interfaces(struct discover_t *ds)
{
	int fd = socket(AF_INET, SOCK_DGRAM, 0);
	if (fd == -1) {
		return;
	}

	struct ifconf ifc;
	uint8_t buf[8192];
	ifc.ifc_len = sizeof(buf);
	ifc.ifc_buf = (char *)buf;

	memset(buf, 0, sizeof(buf));

	if (ioctl(fd, SIOCGIFCONF, &ifc) != 0) {
		close(fd);
		return;
	}

	uint8_t *ptr = (uint8_t *) ifc.ifc_req;
	uint8_t *end = (uint8_t *) & ifc.ifc_buf[ifc.ifc_len];

	while (ptr <= end) {
		struct ifreq *ifr = (struct ifreq *)ptr;
		ptr += _SIZEOF_ADDR_IFREQ(*ifr);

		if (ioctl(fd, SIOCGIFADDR, ifr) != 0) {
			continue;
		}
		struct sockaddr_in *addr_in =
		    (struct sockaddr_in *)&(ifr->ifr_addr);
		uint32_t local_ip = ntohl(addr_in->sin_addr.s_addr);
		if (local_ip == 0) {
			continue;
		}

		if (ioctl(fd, SIOCGIFNETMASK, ifr) != 0) {
			continue;
		}

		struct sockaddr_in *mask_in =
		    (struct sockaddr_in *)&(ifr->ifr_addr);
		uint32_t mask = ntohl(mask_in->sin_addr.s_addr);

		discovered_sock_create(ds, local_ip, mask);
	}
}
#endif
/**
 * Close all sockets previously enumerated and free the struct
 */
static void destroy_socks(struct discover_t *ds)
{
	unsigned int i;
	for (i = 0; i < ds->sock_count; i++) {
		struct discovered_socket *dss = &ds->socks[i];
		close(dss->sock);
	}

	free(ds);
}

/* Perform autodetection.  Returns 0 on success, -1 on error
 * Sets ipAddress to the detected address
 */
int nerdjack_detect(char *ipAddress)
{
	int32_t receivesock;
	struct sockaddr_in sa, receiveaddr, sFromAddr;
	int buffer_length;
	char buffer[200];
	char incomingData[10];
	socklen_t lFromLen;

	sprintf(buffer, "TEST");
	buffer_length = strlen(buffer) + 1;

	net_init();

	receivesock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);

	/* Set nonblocking */
	if (soblock(receivesock, 0) < 0) {
		verb("can't set nonblocking\n");
		return -1;
	}

	if (-1 == receivesock) {	/* if socket failed to initialize, exit */
		verb("Error Creating Socket\n");
		return -1;
	}
	//Setup family for both sockets
	sa.sin_family = PF_INET;
	receiveaddr.sin_family = PF_INET;

	//Setup ports to send on DATA and receive on RECEIVE
	receiveaddr.sin_port = htons(NERDJACK_UDP_RECEIVE_PORT);
	sa.sin_port = htons(NERDJACK_DATA_PORT);

	//Receive from any IP address
	receiveaddr.sin_addr.s_addr = INADDR_ANY;

	bind(receivesock, (struct sockaddr *)&receiveaddr,
	     sizeof(struct sockaddr_in));

	struct discover_t *ds =
	    (struct discover_t *)calloc(1, sizeof(struct discover_t));
	if (!ds) {
		return -1;
	}

	/* Create a routable broadcast socket. */
	if (!discovered_sock_create(ds, 0, 0)) {
		free(ds);
		return -1;
	}

	/* Detect & create local sockets. */
	enumerate_interfaces(ds);

	/*
	 * Send subnet broadcast using each local ip socket.
	 * This will work with multiple separate 169.254.x.x interfaces.
	 */
	unsigned int i;
	for (i = 0; i < ds->sock_count; i++) {
		struct discovered_socket *dss = &ds->socks[i];
		uint32_t target_ip = dss->local_ip | ~dss->subnet_mask;
		sa.sin_addr.s_addr = htonl(target_ip);
		sendto(dss->sock, buffer, buffer_length, 0,
		       (struct sockaddr *)&sa, sizeof(struct sockaddr_in));
	}

	destroy_socks(ds);

	lFromLen = sizeof(sFromAddr);

	if (0 >
	    recvfrom_timeout(receivesock, incomingData, sizeof(incomingData), 0,
			     (struct sockaddr *)&sFromAddr, &lFromLen,
			     &(struct timeval) {
			     .tv_sec = TIMEOUT})) {
		close(receivesock);
		return -1;
	}

	ipAddress = malloc(INET_ADDRSTRLEN);

	//It isn't ipv6 friendly, but inet_ntop isn't on Windows...
	strcpy(ipAddress, inet_ntoa(sFromAddr.sin_addr));

	close(receivesock);
	return 0;
}

 /*
  * Get the NerdJack version string and print it
  */
int nerd_get_version(const char *address)
{
	int ret, fd_command;
	char buf[200];
	fd_command = nerd_open(address, NERDJACK_COMMAND_PORT);
	if (fd_command < 0) {
		info("Connect failed: %s:%d\n", address, NERDJACK_COMMAND_PORT);
		return -2;
	}

	/* Send request */
	ret = send_all_timeout(fd_command, "VERS", 4, 0, &(struct timeval) {
			       .tv_sec = TIMEOUT});
	if (ret < 0) {
		verb("short send %d\n", (int)ret);
		return -1;
	}

	ret = recv_all_timeout(fd_command, buf, 200, 0, &(struct timeval) {
			       .tv_sec = TIMEOUT});

	nerd_close_conn(fd_command);

	if (ret < 0) {
		verb("Error receiving command\n");
		return -1;
	}
	//Slice off the "OK" from the string
	buf[strlen(buf) - 2] = '\0';

	printf("%s\n", buf);

	return 0;
}

/* Send the given command to address.  The command should be something
 * of the specified length.  This expects the NerdJack to reply with OK
 * or NO
 */
int nerd_send_command(const char *address, void *command, int length)
{
	int ret, fd_command;
	char buf[3];
	fd_command = nerd_open(address, NERDJACK_COMMAND_PORT);
	if (fd_command < 0) {
		info("Connect failed: %s:%d\n", address, NERDJACK_COMMAND_PORT);
		return -2;
	}

	/* Send request */
	ret = send_all_timeout(fd_command, command, length, 0, &(struct timeval) {
			       .tv_sec = TIMEOUT});
	if (ret < 0 || ret != length) {
		verb("short send %d\n", (int)ret);
		return -1;
	}

	ret = recv_all_timeout(fd_command, buf, 3, 0, &(struct timeval) {
			       .tv_sec = TIMEOUT});

	nerd_close_conn(fd_command);

	if (ret < 0 || ret != 3) {
		verb("Error receiving OK for command\n");
		return -1;
	}

	if (0 != strcmp("OK", buf)) {
		verb("Did not receive OK.  Received %s\n", buf);
		return -4;
	}

	return 0;
}

int
nerd_data_stream(int data_fd, int numChannels, int *channel_list,
		 int precision, int convert, int lines, int showmem,
		 unsigned short *currentcount, unsigned int period,
		 int wasreset)
{
	//Variables that should persist across retries
	static dataPacket buf;
	static int linesleft = 0;
	static int linesdumped = 0;

	//Variables essential to packet processing
	signed short datapoint = 0;
	int i;

	int numChannelsSampled = channel_list[0] + 1;

	//The number sampled will be the highest channel requested plus 1
	//(i.e. channel 0 requested means 1 sampled)
	for (i = 0; i < numChannels; i++) {
		if (channel_list[i] + 1 > numChannelsSampled)
			numChannelsSampled = channel_list[i] + 1;
	}

	double voltline[numChannels];

	unsigned short dataline[numChannels];

	unsigned short packetsready = 0;
	unsigned short adcused = 0;
	unsigned short tempshort = 0;
	int charsread = 0;

	int numgroupsProcessed = 0;
	double volts;

	//The timeout should be the expected time plus 60 seconds
	//This permits slower speeds to work properly
	unsigned int expectedtimeout =
	    (period * NERDJACK_NUM_SAMPLES / NERDJACK_CLOCK_RATE) + 60;

	//Check to see if we're trying to resume
	//Don't blow away linesleft in that case
	if (lines != 0 && linesleft == 0) {
		linesleft = lines;
	}
	//If there was a reset, we still need to dump a line because of faulty PDCA start
	if (wasreset) {
		linesdumped = 0;
	}
	//If this is the first time called, warn the user if we're too fast
	if (linesdumped == 0) {
		if (period < (numChannelsSampled * 200 + 600)) {
			info("You are sampling close to the limit of NerdJack\n");
			info("Sample fewer channels or sample slower\n");
		}
	}
	//Now destination structure array is set as well as numDuplicates.

	int totalGroups = NERDJACK_NUM_SAMPLES / numChannelsSampled;

	//Loop forever to grab data
	while ((charsread =
		recv_all_timeout(data_fd, &buf, NERDJACK_PACKET_SIZE, 0,
				 &(struct timeval) {
				 .tv_sec = expectedtimeout}))) {

		if (charsread != NERDJACK_PACKET_SIZE) {
			//There was a problem getting data.  Probably a closed
			//connection.
			info("Packet timed out or was too short\n");
			return -2;
		}
		//First check the header info
		if (buf.headerone != 0xF0 || buf.headertwo != 0xAA) {
			info("No Header info\n");
			return -1;
		}
		//Check counter info to make sure not out of order
		tempshort = ntohs(buf.packetNumber);
		if (tempshort != *currentcount) {
			info("Count wrong. Expected %hd but got %hd\n",
			     *currentcount, tempshort);
			return -1;
		}
		//Increment number of packets received
		*currentcount = *currentcount + 1;

		adcused = ntohs(buf.adcused);
		packetsready = ntohs(buf.packetsready);
		numgroupsProcessed = 0;

		if (showmem) {
			printf("%hd %hd\n", adcused, packetsready);
			continue;
		}
		//While there is still more data in the packet, process it
		while (numgroupsProcessed < totalGroups) {
			//Poison the data structure
			switch (convert) {
			case CONVERT_VOLTS:
				memset(voltline, 0,
				       numChannels * sizeof(double));
				break;
			default:
			case CONVERT_HEX:
			case CONVERT_DEC:
				memset(dataline, 0,
				       numChannels * sizeof(unsigned char));
			}

			//Read in each group
			for (i = 0; i < numChannels; i++) {
				//Get the datapoint associated with the desired channel
				datapoint =
				    ntohs(buf.data[channel_list[i] +
						   numgroupsProcessed *
						   numChannelsSampled]);

				//Place it into the line
				switch (convert) {
				case CONVERT_VOLTS:
					if (channel_list[i] <= 5) {
						volts =
						    (double)(datapoint /
							     32767.0) *
						    ((precision & 0x01) ? 5.0 :
						     10.0);
					} else {
						volts =
						    (double)(datapoint /
							     32767.0) *
						    ((precision & 0x02) ? 5.0 :
						     10.0);
					}
					voltline[i] = volts;
					break;
				default:
				case CONVERT_HEX:
				case CONVERT_DEC:
					dataline[i] =
					    (unsigned short)(datapoint -
							     INT16_MIN);
					break;
				}
			}
			//We want to dump the first line because it's usually spurious
			if (linesdumped != 0) {
				//Now print the group
				switch (convert) {
				case CONVERT_VOLTS:
					for (i = 0; i < numChannels; i++) {
						if (printf("%lf ", voltline[i])
						    < 0)
							goto bad;
					}
					break;
				case CONVERT_HEX:
					for (i = 0; i < numChannels; i++) {
						if (printf("%04hX", dataline[i])
						    < 0)
							goto bad;
					}
					break;
				default:
				case CONVERT_DEC:
					for (i = 0; i < numChannels; i++) {
						if (printf("%hu ", dataline[i])
						    < 0)
							goto bad;
					}
					break;
				}
				if (printf("\n") < 0)
					goto bad;

				//If we're counting lines, decrement them
				if (lines != 0) {
					linesleft--;
					if (linesleft == 0) {
						return 0;
					}
				}

			} else {
				linesdumped = linesdumped + 1;
			}

			//We've processed this group, so advance the counter
			numgroupsProcessed++;

		}
	}

	return 0;

 bad:
	info("Output error (disk full?)\n");
	return -3;

}

/* Open a connection to the NerdJack */
int nerd_open(const char *address, int port)
{

	struct hostent *he;

	net_init();

	int32_t i32SocketFD = socket(PF_INET, SOCK_STREAM, 0);

	if (-1 == i32SocketFD) {
		verb("cannot create socket");
		return -1;
	}

	/* Set nonblocking */
	if (soblock(i32SocketFD, 0) < 0) {
		verb("can't set nonblocking\n");
		return -1;
	}

	struct sockaddr_in stSockAddr;
	memset(&stSockAddr, 0, sizeof(stSockAddr));

	stSockAddr.sin_family = AF_INET;
	stSockAddr.sin_port = htons(port);

	he = gethostbyname(address);
	if (he == NULL) {
		verb("gethostbyname(\"%s\") failed\n", address);
		return -1;
	}
	stSockAddr.sin_addr = *((struct in_addr *)he->h_addr);

	debug("Resolved %s -> %s\n", address, inet_ntoa(stSockAddr.sin_addr));

	/* Connect */
	if (connect_timeout
	    (i32SocketFD, (struct sockaddr *)&stSockAddr, sizeof(stSockAddr),
	     &(struct timeval) {
	     .tv_sec = 3}) < 0) {
		verb("connection to %s:%d failed: %s\n",
		     inet_ntoa(stSockAddr.sin_addr), port,
		     compat_strerror(errno));
		return -1;
	}

	return i32SocketFD;
}

//Generate an appropriate sample initiation command
int
nerd_generate_command(getPacket * command, int *channel_list,
		      int channel_count, int precision, unsigned long period)
{

	short channelbit = 0;
	int i;
	int highestchannel = 0;

	for (i = 0; i < channel_count; i++) {
		if (channel_list[i] > highestchannel) {
			highestchannel = channel_list[i];
		}
		//channelbit = channelbit | (0x1 << channel_list[i]);
	}

	for (i = 0; i <= highestchannel; i++) {
		channelbit = channelbit | (0x01 << i);
	}

	command->word[0] = 'G';
	command->word[1] = 'E';
	command->word[2] = 'T';
	command->word[3] = 'D';
	command->channelbit = htons(channelbit);
	command->precision = precision;
	command->period = htonl(period);
	command->prescaler = 0;

	return 0;

}

int nerd_close_conn(int data_fd)
{
	shutdown(data_fd, 2);
	close(data_fd);
	return 0;
}