Merge branch 'command' into ui

Conflicts:
	Makefile
	console/console/Makefile
	controller/block/il2c/il2c.c
	controller/build.mk
	controller/controller/Makefile
	controller/controller/controller_setpoint_command.c

Makefile

@@ -7,6 +7,8 @@ all:
 	$(MAKE) -C common/utils
 	$(MAKE) -C common/log
 	$(MAKE) -C common/trace
+	$(MAKE) -C common/command
+	$(MAKE) -C common/trace
 	$(MAKE) -C controller
 	cd libnova-0.13.0 ; ./configure -enable-static -disable-shared --prefix=${CURDIR} ; make; make install
 	$(MAKE) -C console
@@ -37,12 +39,14 @@ controller:
 	$(MAKE) -C common/utils
 	$(MAKE) -C common/log
 	$(MAKE) -C common/trace
+	$(MAKE) -C common/command
 	$(MAKE) -C controller
 
 console:
 	$(MAKE) -C common/utils
 	$(MAKE) -C common/log
 	$(MAKE) -C common/trace
+	$(MAKE) -C common/command
 	cd libnova-0.13.0 ; ./configure -enable-static -disable-shared --prefix=${CURDIR} ; make; make install
 	$(MAKE) -C console
 
@@ -51,6 +55,7 @@ clean:
 	$(MAKE) -C common/log clean
 	$(MAKE) -C common/utils clean
 	$(MAKE) -C common/trace clean
+	$(MAKE) -C common/command clean
 	$(MAKE) -C controller clean
 	cd libnova-0.13.0 ; make clean || true
 	$(MAKE) -C console clean
@@ -76,12 +81,17 @@ rel_trace: rel_utils
 	$(MAKE) -C $(REL_BUILD_DIR)/common/trace >/dev/null; \
 	echo "       Done"
 
+rel_command: rel_utils
+	@echo "****** Building common/command"; \
+	$(MAKE) -C $(REL_BUILD_DIR)/common/command >/dev/null; \
+	echo "       Done"
+
 rel_log:
 	@echo "****** Building common/log"; \
 	$(MAKE) -C $(REL_BUILD_DIR)/common/log >/dev/null; \
 	echo "       Done"
 
-rel_ctrl: rel_trace rel_log
+rel_ctrl: rel_trace rel_log rel_command
 	@echo "****** Building controller"; \
 	$(MAKE) -C $(REL_BUILD_DIR)/controller >/dev/null; \
 	echo "       Done"
@@ -93,7 +103,7 @@ rel_libnova: rel_clone_dir
 	echo "       Done"
 
 
-rel_cons: rel_trace rel_libnova rel_log
+rel_cons: rel_trace rel_libnova rel_log rel_command
 	@echo "****** Building console"; \
 	$(MAKE) -C $(REL_BUILD_DIR)/console >/dev/null; \
 	echo "       Done"

common/command/.gitignore

+command_list

common/command/Makefile

+
+CFLAGS+= -Wall -O3 -I../utils/ -I.. -I../include
+LDFLAGS+= -L../lib
+
+LOBJS= command.lo
+#SRCS= trace_dump.c trace_dumpdiff.c trace_fft.c
+
+all: libs bins libs_install
+
+
+libs: libcommand.la
+
+libs_install: libcommand.la_install
+
+bins: command_list
+#bins: trace_dump trace_dumpdiff trace_fft trace_view trace_list
+
+${CURDIR}/../lib:
+	@mkdir ${CURDIR}/../lib
+
+libcommand.la_LDFLAGS= -lutils -lm -llog -rpath ${CURDIR}/../lib
+libcommand.la: command.lo command_tcp.lo
+
+libcommand.la_install: libcommand.la ${CURDIR}/../lib
+	@echo "CP $^ to libdir"
+	@${LIBTOOL} --quiet --mode=install install libcommand.la ${CURDIR}/../lib
+
+#trace_dump_LDFLAGS= -ltrace -lutils
+#trace_dump: trace_dump.o libs_install
+
+#trace_dumpdiff_LDFLAGS= -ltrace -lutils
+#trace_dumpdiff: trace_dumpdiff.o libs_install
+
+#trace_fft_LDFLAGS= -lm -lutils -ltrace -lfftw3
+#trace_fft: trace_fft.o libs_install
+
+command_list_LDFLAGS= -lcommand
+command_list: command_list.o libs_install
+
+#trace_view_LDFLAGS= -ltrace
+#trace_view: trace_view.o libs_install
+
+
+clean:
+	rm -rf *.o *.a *.d *.lo *.la .libs 
+#	    trace_dump trace_dumpdiff trace_fft trace_list trace_view
+
+include ../build.mk

common/command/command.c

+/*
+	Command support
+
+	Copyright Jeroen Vreeken (jeroen@vreeken.net), 2014
+
+	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 3 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.
+
+	You should have received a copy of the GNU General Public License
+	along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+*/
+
+#include <command/command.h>
+#include <command/command_def.h>
+
+#include <tcp_connect.h>
+#include <log/log.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <inttypes.h>
+#include <sys/ioctl.h>
+#include <pthread.h>
+
+
+void command_initialize(struct command *command, size_t buffer)
+{
+	memset(command, 0, sizeof(struct command));
+	command->rx_buffer = malloc(buffer);
+	command->rx_buf_size = buffer;
+	command->fd = -1;
+}
+
+void command_packet_initialize(struct command_pkt *pkt)
+{
+	memset(pkt, 0, sizeof(struct command_pkt));
+}
+
+
+
+#define PACKET_BUFLEN 256
+#define PACKET_POOL_CHUNK 16
+
+static struct command_pkt **packet_pool;
+static int packet_pool_size = 0;
+static int packet_pool_cur = 0;
+
+static pthread_mutex_t command_pkt_mutex = PTHREAD_MUTEX_INITIALIZER;
+
+struct command_pkt *command_packet_new(void)
+{
+	struct command_pkt *pkt = NULL;
+	
+	pthread_mutex_lock(&command_pkt_mutex);
+	if (packet_pool_cur) {
+		pkt = packet_pool[packet_pool_cur-1];
+		packet_pool_cur--;
+	}
+	pthread_mutex_unlock(&command_pkt_mutex);
+	
+	if (!pkt) {
+		pkt = calloc(1, sizeof(struct command_pkt));
+		pkt->data = malloc(PACKET_BUFLEN);
+		pkt->buflen = PACKET_BUFLEN;
+	}
+	
+	return pkt;
+}
+
+int command_packet_resize(struct command_pkt *pkt, size_t len)
+{
+	if (len <= pkt->buflen)
+		return 0;
+	
+	pkt->data = realloc(pkt->data, len);
+	pkt->buflen = len;
+
+	return 0;
+}
+
+void command_packet_put(struct command_pkt *pkt)
+{
+	if (pkt->buflen > PACKET_BUFLEN) {
+		free(pkt->data);
+		free(pkt);
+		return;
+	}
+	
+	pthread_mutex_lock(&command_pkt_mutex);
+
+	if (packet_pool_cur >= packet_pool_size) {
+		packet_pool_size += PACKET_POOL_CHUNK;
+		packet_pool = realloc(packet_pool,
+		    sizeof(struct command_pkt *) * packet_pool_size);
+	}
+	packet_pool[packet_pool_cur] = pkt;
+	pkt->len = 0;
+	packet_pool_cur++;
+
+	pthread_mutex_unlock(&command_pkt_mutex);
+}
+
+int command_packet_list_add(struct command_pkt *pkt, 
+    char *name, enum command_value_type type, char *unit,
+    int typec, enum command_ptype typev[])
+ {
+	size_t entry_size = strlen(name)+1 + 1 + strlen(unit) + 1 + typec + 1;
+	unsigned char *entry;
+	int i;
+	
+	if (!pkt->len) {
+		command_packet_resize(pkt, 1);
+		pkt->len = 1;
+		pkt->data[0] = COMMAND_PTYPE_LIST;
+	}
+	
+	command_packet_resize(pkt, pkt->len + entry_size);
+	entry = pkt->data + pkt->len;
+
+	strcpy((char *)entry, name);
+	entry += strlen(name) + 1;
+
+	*entry = type;
+	entry++;
+
+	strcpy((char *)entry, unit);
+	entry += strlen(unit) + 1;
+	
+	for (i = 0; i < typec; i++) {
+	   *entry = typev[i];
+	   entry++;
+	}
+	*entry = COMMAND_PTYPE_NULL;
+	entry++;
+	
+	pkt->len += entry_size;
+	
+	return 0;
+}
+
+int command_packet_name_set(struct command_pkt *pkt, char *name)
+{
+	if (pkt->len < sizeof(struct command_header) + strlen(name) + 1) {
+		pkt->len = strlen(name) + 1 + sizeof(struct command_header);
+		command_packet_resize(pkt, pkt->len);
+		pkt->data[0] = COMMAND_PTYPE_NAME;
+	}
+	strcpy((char *)pkt->data + 1, name);
+	
+	return 0;
+}
+
+int command_packet_id_set(struct command_pkt *pkt, char *id)
+{
+	if (pkt->len < sizeof(struct command_header) + strlen(id) + 1) {
+		pkt->len = strlen(id) + 1 + sizeof(struct command_header);
+		command_packet_resize(pkt, pkt->len);
+		pkt->data[0] = COMMAND_PTYPE_ID;
+	}
+	strcpy((char *)pkt->data + 1, id);
+	
+	return 0;
+}
+
+int command_packet_entry_set(struct command_pkt *pkt,
+    struct command_entry *entry, enum command_value_type type)
+{
+	size_t vsize;
+	size_t size;
+	unsigned char *p;
+	
+	switch (type) {
+		default:
+		case COMMAND_VALUE_TYPE_FLOAT:
+		case COMMAND_VALUE_TYPE_UINT32:
+		case COMMAND_VALUE_TYPE_SINT32:
+			vsize = 4;
+			break;
+		case COMMAND_VALUE_TYPE_BOOL:
+		case COMMAND_VALUE_TYPE_UINT8:
+		case COMMAND_VALUE_TYPE_SINT8:
+			vsize = 1;
+			break;
+		case COMMAND_VALUE_TYPE_UINT16:
+		case COMMAND_VALUE_TYPE_SINT16:
+			vsize = 2;
+			break;
+	}
+	
+	if (entry->type == COMMAND_PTYPE_SETPOINT_TIME) {
+		size = 8 + 4 + vsize;
+	} else {
+		size = vsize;
+	}
+	
+	pkt->len = size + sizeof(struct command_header);
+	command_packet_resize(pkt, pkt->len);
+	pkt->data[0] = entry->type;
+	
+	p = &pkt->data[1];
+	
+	if (entry->type == COMMAND_PTYPE_SETPOINT_TIME) {
+		struct command_timestamp *t = (void *)p;
+		
+		t->sec = htobe64(entry->t.tv_sec);
+		t->nsec = htobe32(entry->t.tv_nsec);
+		
+		p += sizeof(struct command_timestamp);
+	}
+	
+	switch (vsize) {
+		case 1:
+			*p = entry->value.u8;
+			break;
+		case 2: {
+			struct command_ptype_value *pv;
+			
+			pv = (void *)p;
+			pv->u.u16 = htobe16(entry->value.u16);
+			break;
+		}
+		case 4: {
+			struct command_ptype_value *pv;
+			
+			pv = (void *)p;
+			pv->u.u32 = htobe32(entry->value.u32);
+			break;
+		}
+	}
+	
+	return 0;
+}
+
+bool command_fd_set(struct command *command, fd_set *set, int *high)
+{
+	if (command->fd < 0) {
+		if (!command->recover)
+			return false;
+
+		if (command->recovertime >= time(NULL))
+			return false;
+		command->recovertime = time(NULL);
+
+		log_send(LOG_T_DEBUG, "Attempt to recover command %s", 
+		    command->name);
+		command->fd = tcp_connect(command->host, command->port);
+		if (command->fd >= 0) {
+			struct command_pkt *pkt;
+		
+			ioctl(command->fd, FIONBIO, &(int){1});
+
+			if (command->name) {
+				pkt = command_packet_new();
+				command_packet_name_set(pkt, command->name);
+				command_packet_write(command, pkt);
+				command_packet_put(pkt);
+			}
+			if (command->id) {
+				pkt = command_packet_new();
+				command_packet_id_set(pkt, command->id);
+				command_packet_write(command, pkt);
+				command_packet_put(pkt);
+			}
+		}
+		if (command->fd < 0) {
+			log_send(LOG_T_DEBUG, "Failed to recover command connection");
+			return false;
+		}
+		log_send(LOG_T_DEBUG, "Reconnected command");
+	}
+
+	FD_SET(command->fd, set);
+
+	if (command->fd > *high)
+		*high = command->fd;
+	
+	return true;
+}
+
+int command_handle(struct command *command, fd_set *set)
+{
+	if (FD_ISSET(command->fd, set)) {
+		while (command_fd_read(command)) {
+			struct command_pkt *pkt;
+			struct command_entry entry;
+			size_t pos = 1;
+			
+			pkt = command_packet_get(command);
+			if (!pkt)
+				break;
+
+			switch (pkt->data[0]) {
+				case COMMAND_PTYPE_LIST: {
+					unsigned char *listentry;
+					
+					while (pos < pkt->len) {
+						char *name;
+						enum command_value_type type;
+						char *unit;
+						enum command_ptype ptypev[COMMAND_PTYPE_MAX] = { 0 };
+						int i;
+						int j;
+						
+						listentry = pkt->data + pos;
+						
+						name = (char *)listentry;
+						type = listentry[strlen(name) + 1];
+						unit = (char *)listentry+strlen(name) + 2;
+						for (i = strlen(name) + strlen(unit) + 3, j = 0;
+						    j < COMMAND_PTYPE_MAX && listentry[i] != COMMAND_PTYPE_NULL;
+						    i++, j++)
+							ptypev[j] = listentry[i];
+						
+						if (command->handler_list_entry) {
+							command->handler_list_entry(command,
+							    name, type, unit, j, ptypev);
+						}
+						pos += strlen(name) + strlen(unit) + 3 + j + 1;
+					}
+					command->list_received = true;
+					break;
+				}
+				case COMMAND_PTYPE_SETPOINT_TIME: {
+					struct command_timestamp *t;
+					
+					if (pos < pkt->len) {
+						t = (void *)pkt->data + pos;
+						entry.t.tv_sec = be64toh(t->sec);
+						entry.t.tv_nsec = be32toh(t->nsec);
+					}
+					pos += sizeof(struct command_timestamp);
+				}
+				/* fall through */
+				case COMMAND_PTYPE_SPEED:
+				case COMMAND_PTYPE_SETPOINT: {
+					struct command_ptype_value *v;
+					
+					entry.type = pkt->data[0];
+					
+					if (pos < pkt->len) {
+						v = (void *)pkt->data + pos;
+						switch (command->type) {
+							case COMMAND_VALUE_TYPE_FLOAT:
+							case COMMAND_VALUE_TYPE_UINT32:
+							case COMMAND_VALUE_TYPE_SINT32:
+								entry.value.u32 = be32toh(v->u.u32);
+								pos+=4;
+								break;
+							case COMMAND_VALUE_TYPE_UINT16:
+							case COMMAND_VALUE_TYPE_SINT16:
+								entry.value.u16 = be32toh(v->u.u16);
+								pos+=2;
+								break;
+							case COMMAND_VALUE_TYPE_BOOL:
+							case COMMAND_VALUE_TYPE_UINT8:
+							case COMMAND_VALUE_TYPE_SINT8:
+								entry.value.u8 = v->u.u8;
+								pos++;
+								break;
+						}
+					}
+					if (command->handler_entry)
+						command->handler_entry(command, &entry);
+					break;
+				}
+				case COMMAND_PTYPE_NAME: {
+					char *name;
+					
+					name = (void*)pkt->data + 1;
+					
+					free(command->name);
+					command->name = strdup(name);
+
+					if (command->handler_name) {
+						command->handler_name(command, name);
+					}
+					break;
+				}
+				case COMMAND_PTYPE_ID: {
+					char *id;
+					
+					id = (void*)pkt->data + 1;
+					
+					free(command->id);
+					command->id = strdup(id);
+
+					if (command->handler_id) {
+						command->handler_id(command, id);
+					}
+					break;
+				}
+			}
+			
+			command_packet_put(pkt);
+		}
+		return 0;
+	}
+
+	return 0;
+}
+
+enum command_state command_state_get(struct command *command)
+{
+	if (command->fd < 0)
+		return COMMAND_STATE_DISCONNECTED;
+	
+	if (command->list_received)
+		return COMMAND_STATE_READY;
+	
+	return COMMAND_STATE_CONNECTED;
+}
+
+void command_autorecover(struct command *command, bool value)
+{
+	command->recover = value;
+}
+
+struct command *command_open_simple(char *host, int port, char *name, char *id,
+    enum command_value_type type)
+{
+	struct command *command = command_open(host, port);
+	
+	if (!command)
+		goto err;
+
+	command->type = type;
+
+	command_id_set(command, id);
+	command_name_set(command, name);
+	command_autorecover(command, true);
+
+	return command;
+
+err:
+	return NULL;
+}
+
+int command_name_set(struct command *command, char *name)
+{
+	struct command_pkt *pkt;
+
+	free(command->name);
+	command->name = strdup(name);
+	
+	pkt = command_packet_new();
+	command_packet_name_set(pkt, name);
+	command_packet_write(command, pkt);
+	command_packet_put(pkt);
+	
+	return 0;
+}
+
+int command_id_set(struct command *command, char *id)
+{
+	struct command_pkt *pkt;
+
+	free(command->id);
+	command->id = strdup(id);
+	
+	pkt = command_packet_new();
+	command_packet_id_set(pkt, id);
+	command_packet_write(command, pkt);
+	command_packet_put(pkt);
+	
+	return 0;
+}
+
+int command_send(struct command *command, struct command_entry *entry)
+{
+	fd_set dummyset;
+	int dummyhigh;
+	struct command_pkt *pkt;
+	
+	if (!command_fd_set(command, &dummyset, &dummyhigh))
+		return -1;
+
+	pkt = command_packet_new();
+	command_packet_entry_set(pkt, entry, command->type);
+	command_packet_write(command, pkt);
+	command_packet_put(pkt);
+	
+	return 0;
+}
+
+char *enum_command_ptype2str(enum command_ptype ptype)
+{
+	switch(ptype) {
+		case COMMAND_PTYPE_NULL:
+			return "NULL";
+		case COMMAND_PTYPE_LIST:
+			return "LIST";
+		case COMMAND_PTYPE_NAME:
+			return "NAME";
+		case COMMAND_PTYPE_ID:
+			return "ID";
+		case COMMAND_PTYPE_SETPOINT:
+			return "SETPOINT";
+		case COMMAND_PTYPE_SPEED:
+			return "SPEED";
+		case COMMAND_PTYPE_SETPOINT_TIME:
+			return "SETPOINT_TIME";
+		case COMMAND_PTYPE_MAX:
+			return "MAX";
+		default:
+			return "unknown";