Revive from archive.

TCP/Makefile

@@ -0,0 +1,27 @@
+SOURCES = $(wildcard *.c)
+TARGET = tcp
+
+# top-level rule, to compile everything.
+all: $(TARGET)
+
+DEPS = $(wildcard *.d)
+OBJECTS = $(SOURCES:.c=.o)
+
+#link
+$(TARGET): $(OBJECTS)
+	gcc -o $@ $^ -lpthread -g
+
+#compile
+%.o : %.c
+	gcc -o $@ $< -c -g -MMD -Wall
+
+# If explicit dependencies exist, add them
+include $(DEPS)
+
+# rule for cleaning re-compilable files.
+clean:
+	rm -f $(TARGET) $(OBJECTS) $(DEPS)
+        
+rebuild: clean all
+
+.PHONY: rebuild clean all

TCP/main.c

@@ -0,0 +1,89 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <pthread.h>
+
+#include <sys/types.h>
+#include <sys/socket.h>
+
+#include "tcp.h"
+
+#define LINE_SIZE 256
+
+typedef void *(*thread_t)(void*);
+typedef void *thread_param_t;
+
+void *receive_thread(socket_fd_t socket)
+{
+	while (1){
+		int chr;
+
+		int s = recv(socket, &chr, 1, 0);
+
+		if (s < 0) {
+			perror("receive_thread (recv):");
+			close(socket);
+			exit(1);
+		}
+		else if (s == 0) {
+			exit(1);
+		}
+		else {
+			putchar(chr);
+			fflush(stdout);
+		}
+	}
+}
+
+
+
+int main(int argc, char *argv[])
+{
+	pthread_t recv_thread;
+	socket_fd_t socket;
+	char *tmp;
+	int port;
+	char line[LINE_SIZE];
+
+	if (argc < 2) {
+		printf("tcp: not enough input arguments\n");
+		return 1;
+	}
+
+	if ((tmp = strchr(argv[1], ':'))) {
+		port = atoi(tmp+1);
+		*tmp = '\0';
+		
+		printf("Assuming client mode: Host:%s, Port:%d\n", argv[1], port);
+
+		socket = tcp_connect(argv[1], port);
+	}
+	else {
+		port = atoi(argv[1]);
+
+		printf("Assuming server mode: Port:%d\n", port);
+
+		socket = tcp_accept(port);
+	}
+
+	if (socket < 0) {
+		return socket;
+	}
+
+				/*START*/
+		
+	pthread_create(&recv_thread, NULL, (thread_t)&receive_thread,
+			(thread_param_t)socket);
+	
+	while (1) {
+		fgets(line, sizeof(line), stdin);
+
+		if (write(socket, line, strlen(line)) != strlen(line)) {
+			perror("main (write):");
+			close(socket);
+			exit(1);
+		}
+	}
+
+}

TCP/tcp.c

@@ -0,0 +1,110 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <memory.h>
+//#include <pthread.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netdb.h> 
+
+#include "tcp.h"
+
+
+struct sockaddr_in init_sockaddr_in(int port) {
+	struct sockaddr_in r;
+
+	r.sin_family = AF_INET;
+	r.sin_addr.s_addr = INADDR_ANY;
+	r.sin_port = htons(port);
+	 
+	return r;
+}
+
+
+
+struct sockaddr *hostent2sockaddr(struct hostent *e, int port) {
+	static struct sockaddr_in sa;
+
+	sa.sin_family = AF_INET;
+	sa.sin_port = htons(port);
+	memcpy(&sa.sin_addr.s_addr, e->h_addr, e->h_length);
+	 
+	return (struct sockaddr*)&sa;
+}
+
+
+
+
+void my_setsockopt(int socket, int option, int value)
+{
+	setsockopt(socket, SOL_SOCKET, option, &value, sizeof(value));
+}
+
+
+
+socket_fd_t tcp_connect(const char *server_name, int port)
+{
+	socket_fd_t comm_socket;
+	struct sockaddr *serv_addr = NULL;
+	struct hostent *server = NULL;
+
+	if ((comm_socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP)) < 0) {
+		perror("connect (socket)");
+		return ERR_CANT_CREATE_SOCKET;
+	}
+	
+	if ((server = gethostbyname(server_name)) == NULL) {
+		perror("connect (gethostbyname)");
+		return ERR_INVALID_HOST;
+	}
+
+	printf("%s \n", *server);
+
+	serv_addr = hostent2sockaddr(server, port);
+
+	if (connect(comm_socket, serv_addr, sizeof(*serv_addr)) < 0) {
+		perror("connect (connect)");
+		return ERR_CANT_CONNECT;
+	}
+
+	return comm_socket;
+}
+
+
+
+socket_fd_t tcp_accept(int port)
+{
+	socket_fd_t accept_socket, comm_socket;
+	socklen_t addrlen = sizeof(struct sockaddr_in);
+	struct sockaddr_in serv_addr = init_sockaddr_in(port);
+	struct sockaddr_in cli_addr = init_sockaddr_in(0);
+	 
+	if ((accept_socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP)) < 0) {
+		perror("accept (socket)");
+		return ERR_CANT_CREATE_SOCKET;
+	}
+
+	my_setsockopt(accept_socket, SO_REUSEADDR, 1);
+
+	if (bind(accept_socket, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) {
+		perror("accept (bind)");
+		close(accept_socket);
+		return ERR_CANT_BIND_SOCKET;
+	}
+
+	if (listen(accept_socket, 255) < 0) {
+		perror("accept (listen)");
+		close(accept_socket);
+		return ERR_CANT_LISTEN;
+	}
+
+	if ((comm_socket = accept(accept_socket, (struct sockaddr *)&cli_addr, &addrlen)) < 0) {
+		perror("accept (accept)");
+		return ERR_CANT_ACCEPT;
+	}
+
+	close(accept_socket);
+	return comm_socket;
+}
+

TCP/tcp.h

@@ -0,0 +1,16 @@
+#define ERR_INVALID_HOST -1
+#define ERR_CANT_CREATE_SOCKET -2
+#define ERR_CANT_WRITE -3
+#define ERR_CANT_CONNECT -4
+#define ERR_CANT_CREATE_THREAD -5
+#define ERR_CANT_GET_BROADCAST_ADDR -6
+#define ERR_CANT_BIND_SOCKET -7
+#define ERR_INVALID_DEVICE -8
+#define ERR_CANT_LISTEN -9
+#define ERR_CANT_ACCEPT -10
+
+typedef int socket_fd_t;
+
+socket_fd_t tcp_connect(const char* server, int port);
+socket_fd_t tcp_accept(int port);
+

UDP/Makefile

@@ -0,0 +1,27 @@
+SOURCES = $(wildcard *.c)
+TARGET = udp
+
+# top-level rule, to compile everything.
+all: $(TARGET)
+
+DEPS = $(wildcard *.d)
+OBJECTS = $(SOURCES:.c=.o)
+
+#link
+$(TARGET): $(OBJECTS)
+	gcc -o $@ $^ -lpthread -g
+
+#compile
+%.o : %.c
+	gcc -o $@ $< -c -g -MMD -Wall
+
+# If explicit dependencies exist, add them
+include $(DEPS)
+
+# rule for cleaning re-compilable files.
+clean:
+	rm -f $(TARGET) $(OBJECTS) $(DEPS)
+        
+rebuild: clean all
+
+.PHONY: rebuild clean all

UDP/main.c

@@ -0,0 +1,67 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "udp.h"
+
+#define LINE_SIZE 256
+
+
+static void receive_function(udp_packet_t packet)
+{
+	putchar(packet);
+	fflush(stdout);
+}
+
+
+
+int main(int argc, char *argv[])
+{
+	int port;
+	char line[LINE_SIZE];
+	char server[100];
+
+	if (argc < 2) {
+		printf("udp: not enough input arguments\n");
+		return 1;
+	}
+	else if (!strchr(argv[1], ':')) {
+		printf(
+				"Usage:\n"
+			  	"  udp target:port\n"
+				"or:\n"
+			  	"  udp broadcast:port\n"
+		);
+
+		return 1;
+	}
+
+	port = atoi(strchr(argv[1], ':')+1);
+	*strchr(argv[1], ':') = '\0';
+	
+	if (!strcmp(argv[1], "broadcast")) {
+		if (udp_get_broadcast_address("eth1", server, sizeof(server)) < 0) {
+			printf("Error: Cannot determine broadcast address\n");
+			exit(1);
+		}
+	}
+	else {
+		strncpy(server, argv[1], sizeof(server));
+	}
+	
+	printf("Using %s:%d\n", server, port);
+	
+	udp_listen(port, &receive_function);
+
+	while (1) {
+		int i;
+		fgets(line, LINE_SIZE, stdin);
+
+		for (i = 0; line[i] != '\0'; i++) {
+			udp_packet_t packet = line[i];
+			udp_send_packet(server, port, packet);
+		}
+	}
+
+}

UDP/udp.c

@@ -0,0 +1,191 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <memory.h>
+#include <pthread.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netdb.h> 
+
+#include <string.h>
+
+#include <sys/ioctl.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <net/if.h>
+
+#include "udp.h"
+
+typedef void *(*thread_t)(void*);
+typedef void *thread_param_t;
+
+typedef struct tag_listen_info
+{
+	int port;
+	void (*receiving_function)(udp_packet_t);
+} listen_info_t;
+
+
+
+int udp_get_broadcast_address(const char *network_interface, char *broadcast_address, int address_buffer_length)
+{
+	int sockfd = -1;
+	struct ifreq ifr;
+
+	// Start referanse-socket
+	if ((sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) {
+		perror("get_broadcast_addr (socket)");
+		return ERR_CANT_CREATE_SOCKET;
+	}
+
+	strncpy (ifr.ifr_name, network_interface, sizeof(ifr.ifr_name));
+
+	// Hent broadcast adresse fra ioctl()
+	if (ioctl(sockfd, SIOCGIFBRDADDR, &ifr) != 0) {
+		perror("get_broadcast_addr (ioctl)");
+		return ERR_CANT_GET_BROADCAST_ADDR;
+	}
+	
+	if ( ifr.ifr_broadaddr.sa_family != AF_INET ) {
+		fputs("Not IPV4 network interface", stderr);
+		return ERR_INVALID_DEVICE;	
+	}
+	
+	strncpy( 
+		broadcast_address, 
+		inet_ntoa(((struct sockaddr_in *)&ifr.ifr_broadaddr)->sin_addr),
+		address_buffer_length);
+		
+	broadcast_address[address_buffer_length-1] = '\0';
+	
+	close(sockfd);
+
+	return 0;
+} 
+
+
+
+void udp_abort_listen(int thread) 
+{
+	pthread_cancel( thread );
+}
+
+
+
+static void *udp_listen_thread(listen_info_t *listen_info_ptr)
+{
+	int sockfd, n;
+	struct sockaddr_in serv_addr;
+	udp_packet_t packet;
+	listen_info_t li = *listen_info_ptr;
+	
+	if ((sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) {
+		perror("listen (socket)");
+		pthread_exit(NULL);
+	}
+
+	{
+		int dummy=1;
+		setsockopt(sockfd, SOL_SOCKET, SO_BROADCAST, &dummy, sizeof(dummy));
+		setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &dummy, sizeof(dummy));
+	}
+
+	memset(&serv_addr, 0, sizeof(serv_addr));
+	 
+	serv_addr.sin_family = AF_INET;
+	serv_addr.sin_addr.s_addr = INADDR_ANY;
+	serv_addr.sin_port = htons(li.port);
+	 
+	if (bind(sockfd, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0) {
+		perror("listen (bind)");
+		close(sockfd);
+		pthread_exit(NULL);
+	}
+
+	while (1) {	
+		n = recvfrom(sockfd, &packet, sizeof(packet), 0, 0, 0);
+			
+		if (n < 0) 
+			fprintf(stderr, "ERROR reading from socket\n");
+
+		li.receiving_function(packet);
+	
+	}
+					
+	pthread_exit(NULL);
+}
+
+
+
+int udp_listen(int port, void (*receiving_function)(udp_packet_t))
+{
+	static listen_info_t li;
+	pthread_t new_thread;
+	
+	li.port = port;
+	li.receiving_function = receiving_function;
+	
+	if (pthread_create(&new_thread, NULL, (thread_t)&udp_listen_thread,
+				(thread_param_t)&li) != 0) {
+		perror ("listen (pthread_create)");
+		return ERR_CANT_CREATE_THREAD;
+	}
+	else {
+		return new_thread;
+	}
+}
+
+
+
+int udp_send_packet(const char addr[256], int port, udp_packet_t packet)
+{
+	int sockfd;
+	struct sockaddr_in serv_addr;
+	struct hostent *server;
+
+	server = gethostbyname(addr);
+	
+	if (server == NULL) {
+		perror("send (gethostbyname)");
+		return ERR_INVALID_HOST;
+	}
+
+	memset(&serv_addr, 0, sizeof(serv_addr));
+	
+	serv_addr.sin_family = AF_INET;
+	
+	memcpy(&serv_addr.sin_addr.s_addr, server->h_addr, server->h_length);
+	
+	serv_addr.sin_port = htons(port);
+	
+	if ((sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) {
+		perror("send (socket)");
+		return ERR_CANT_CREATE_SOCKET;
+	}
+	
+	{
+		int dummy=1;
+		setsockopt(sockfd, SOL_SOCKET, SO_BROADCAST, &dummy, sizeof(dummy));
+		setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &dummy, sizeof(dummy));
+	}
+	
+	if (connect(sockfd, (void*)(&serv_addr), sizeof(serv_addr)) < 0) {	
+		perror("send (connect)");
+		close(sockfd);
+		return ERR_CANT_CONNECT;
+	}
+	
+	if (write(sockfd, &packet, sizeof(packet)) != sizeof(packet)) {
+		perror("send (write)");
+		close(sockfd);
+		return ERR_CANT_WRITE;
+	}
+
+	close(sockfd);
+
+	return 0;
+}
+
+
+

UDP/udp.h

@@ -0,0 +1,22 @@
+#ifndef __INCLUDE_COMM_UDP_H__
+#define __INCLUDE_COMM_UDP_H__
+
+#define ERR_INVALID_HOST -1
+#define ERR_CANT_CREATE_SOCKET -2
+#define ERR_CANT_WRITE -3
+#define ERR_CANT_CONNECT -4
+#define ERR_CANT_CREATE_THREAD -5
+#define ERR_CANT_GET_BROADCAST_ADDR -6
+#define ERR_INVALID_DEVICE -7
+
+typedef unsigned long int udp_packet_t;
+
+void udp_abort_listen(int thread);
+int udp_send_packet(const char addr[256], int port, udp_packet_t packet);
+int udp_listen(int port, void (*receiving_function)(udp_packet_t));
+
+int udp_get_broadcast_address(const char *network_interface, 
+  char *broadcast_address, int address_buffer_length);
+
+#endif // #ifndef __INCLUDE_COMM_UDP_H__
+

elev-threaded/Makefile

@@ -0,0 +1,27 @@
+SOURCES = $(wildcard *.c)
+TARGET = heis
+
+# top-level rule, to compile everything.
+all: $(TARGET)
+
+DEPS = $(wildcard *.d)
+OBJECTS = $(SOURCES:.c=.o)
+
+#link
+$(TARGET): $(OBJECTS)
+	gcc -o $@ $^ -lpthread -g -lcomedi -lm
+
+#compile
+%.o : %.c
+	gcc -o $@ $< -c -g -MMD -Wall
+
+# If explicit dependencies exist, add them
+include $(DEPS)
+
+# rule for cleaning re-compilable files.
+clean:
+	rm -f $(TARGET) $(OBJECTS) $(DEPS)
+        
+rebuild: clean all
+
+.PHONY: rebuild clean all

elev-threaded/channels.h

@@ -0,0 +1,62 @@
+// Channel definitions for elevator control using LibComedi
+//
+// 2006, Martin Korsgaard
+#ifndef __INCLUDE_DRIVER_CHANNELS_H__
+#define __INCLUDE_DRIVER_CHANNELS_H__
+
+//in port 4
+#define PORT4          3
+#define OBSTRUCTION    (0x300+23)
+#define STOP           (0x300+22)
+#define FLOOR_COMMAND1 (0x300+21)
+#define FLOOR_COMMAND2 (0x300+20)
+#define FLOOR_COMMAND3 (0x300+19)
+#define FLOOR_COMMAND4 (0x300+18)
+#define FLOOR_UP1      (0x300+17)
+#define FLOOR_UP2      (0x300+16)
+
+//in port 1
+#define PORT1          2
+#define FLOOR_DOWN2    (0x200+0)
+#define FLOOR_UP3      (0x200+1)
+#define FLOOR_DOWN3    (0x200+2)
+#define FLOOR_DOWN4    (0x200+3)
+#define SENSOR1        (0x200+4)
+#define SENSOR2        (0x200+5)
+#define SENSOR3        (0x200+6)
+#define SENSOR4        (0x200+7)
+
+//out port 3
+#define PORT3          3
+#define MOTORDIR       (0x300+15)
+#define LIGHT_STOP     (0x300+14)
+#define LIGHT_COMMAND1 (0x300+13)
+#define LIGHT_COMMAND2 (0x300+12)
+#define LIGHT_COMMAND3 (0x300+11)
+#define LIGHT_COMMAND4 (0x300+10)
+#define LIGHT_UP1      (0x300+9)
+#define LIGHT_UP2      (0x300+8)
+
+//out port 2
+#define PORT2 3
+#define LIGHT_DOWN2    (0x300+7)
+#define LIGHT_UP3      (0x300+6)
+#define LIGHT_DOWN3    (0x300+5)
+#define LIGHT_DOWN4    (0x300+4)
+#define DOOR_OPEN      (0x300+3)
+#define FLOOR_IND2     (0x300+1)
+#define FLOOR_IND1     (0x300+0)
+
+//out port 0
+#define PORT0          1
+#define MOTOR          (0x100+0)
+
+//non-existing ports (to achieve macro consistency)
+#define FLOOR_DOWN1    -1
+#define FLOOR_UP4      -1
+#define LIGHT_DOWN1    -1
+#define LIGHT_UP4      -1
+
+
+
+#endif //#ifndef __INCLUDE_DRIVER_CHANNELS_H__

elev-threaded/elev.c

@@ -0,0 +1,237 @@
+// Wrapper for libComedi Elevator control.
+// These functions provides an interface to the elevators in the real time lab
+//
+// 2006, Martin Korsgaard
+#define _XOPEN_SOURCE 500
+#include <assert.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <pthread.h>
+
+#include "channels.h"
+#include "elev.h"
+#include "io.h"
+
+#define POLLING_INTERVAL 50000
+
+// Number of signals and lamps per floor (signals includes stop, obstr)
+#define N_SIGNALS 6
+#define N_LAMPS 3
+
+
+// Array of callback associations.
+static void (*callbacks_g[N_SIGNALS])(int, int);
+
+
+
+// Matrix of lamp channels indexed by floor and lamp type.
+static const int lamp_channel_matrix[N_FLOORS][N_LAMPS] = {
+	{LIGHT_UP1, LIGHT_DOWN1, LIGHT_COMMAND1},
+	{LIGHT_UP2, LIGHT_DOWN2, LIGHT_COMMAND2},
+	{LIGHT_UP3, LIGHT_DOWN3, LIGHT_COMMAND3},
+	{LIGHT_UP4, LIGHT_DOWN4, LIGHT_COMMAND4}};
+
+
+
+// Matrix of elevator signals channels indexed by floor and signal type.
+static const int signal_channel_matrix[N_FLOORS][N_SIGNALS] = {
+	{FLOOR_UP1, FLOOR_DOWN1, FLOOR_COMMAND1, SENSOR1, STOP, OBSTRUCTION},
+	{FLOOR_UP2, FLOOR_DOWN2, FLOOR_COMMAND2, SENSOR2, STOP, OBSTRUCTION},
+	{FLOOR_UP3, FLOOR_DOWN3, FLOOR_COMMAND3, SENSOR3, STOP, OBSTRUCTION},
+	{FLOOR_UP4, FLOOR_DOWN4, FLOOR_COMMAND4, SENSOR4, STOP, OBSTRUCTION}};
+
+
+
+void elev_set_speed(int speed)
+{
+	// In order to sharply stop the elevator, the direction bit is toggled
+	// before setting speed to zero.
+	static int last_speed = 0;
+	
+	// If to start (speed > 0)
+	if (speed > 0) 
+		io_clear_bit(MOTORDIR);
+	else if (speed < 0) 
+		io_set_bit(MOTORDIR);
+
+	// If to stop (speed == 0)
+	else if (last_speed < 0)
+		io_clear_bit(MOTORDIR);
+	else if (last_speed > 0)
+		io_set_bit(MOTORDIR);
+
+	last_speed = speed ;
+
+	// Write new setting to motor.
+	io_write_analog(MOTOR, 2048 + 2*abs(speed));
+
+	//printf(__FILE__ ": Speed set to %d\n", speed);
+}
+
+
+
+void elev_set_door_open_lamp(int value)
+{
+	if (value)
+		io_set_bit(DOOR_OPEN);
+	else
+		io_clear_bit(DOOR_OPEN);
+}
+ 
+
+
+void elev_set_stop_lamp(int value)
+{
+	if (value)
+		io_set_bit(LIGHT_STOP);
+	else
+		io_clear_bit(LIGHT_STOP);
+}
+
+
+
+
+void elev_set_floor_indicator(int floor)
+{
+	assert(floor >= 0);
+	assert(floor < N_FLOORS);
+
+	if (floor & 0x02)
+		io_set_bit(FLOOR_IND1);
+	else
+		io_clear_bit(FLOOR_IND1);
+		
+	if (floor & 0x01)
+		io_set_bit(FLOOR_IND2);
+	else
+		io_clear_bit(FLOOR_IND2);
+}
+
+
+
+void elev_set_button_lamp(elev_direction_t lamp, int floor, int value)
+{
+	assert(floor >= 0);
+	assert(floor < N_FLOORS);
+
+	if (value == 1)
+		io_set_bit(lamp_channel_matrix[floor][lamp]);
+	else
+		io_clear_bit(lamp_channel_matrix[floor][lamp]);		
+}
+
+
+
+void elev_unregister_callback(elev_signal_t type)
+{
+	callbacks_g[type] = NULL;
+}
+
+
+
+void elev_register_callback(elev_signal_t type, void (*callback)(int, int))
+{
+	callbacks_g[type] = callback;
+}
+
+
+
+void elev_disable_callbacks(int thread)
+{
+	pthread_cancel(thread);
+}
+
+
+
+int elev_init(void)
+{
+	memset(callbacks_g, 0, sizeof(callbacks_g));
+	
+	return io_init();
+}
+
+
+
+static void *polling_thread(void *p)
+{
+	// Store previous values of inputs, raise signal if changed between
+	// two polls.
+	static int prev_values[N_FLOORS][N_SIGNALS];
+
+	memset(prev_values, 0, sizeof(prev_values));
+	
+	while (1) {
+		for (int type = 0; type < N_SIGNALS; type++){
+			for (int floor = 0; floor < N_FLOORS; floor++){
+				int value = io_read_bit(signal_channel_matrix[floor][type]);
+
+				// If no associated callback: ignore.
+				if (callbacks_g[type] == NULL) {
+					;
+				}
+				// If value has not changed, ignore.
+				else if (value == prev_values[floor][type]) {
+					;
+				}
+				// Obstruction is raised on any edge, with new value as param.
+				else if (type == SIGNAL_TYPE_OBSTR) {
+					callbacks_g[type](0, value);
+				}
+				// STOP is called with no arguments, only when pushed (not when
+				// released).
+				else if (type == SIGNAL_TYPE_STOP && value == 1) {
+					callbacks_g[type](0, 0);
+				}
+				// Floor sensor is raised on any edge, entering and leaving floor,
+				// with floor and value (edge) as param.
+				else if (type == SIGNAL_TYPE_SENSOR) {
+					callbacks_g[type](floor, value);
+				}
+				// Other signals (CALL_UP, CALL_DOWN, COMMAND) are raised on
+				// rising edge only, with floor and type as parameters.
+				else if (value == 1) {
+					callbacks_g[type](floor, type);
+				}
+				// Store current value of signal to avoid multiple calls on the
+				// same signal.
+				prev_values[floor][type] = value;
+				
+				// Obstr and stop is not floor dependent, so only loop once.
+				if (type == SIGNAL_TYPE_OBSTR || type == SIGNAL_TYPE_STOP) 
+					break;
+
+			}
+		}
+
+		usleep(POLLING_INTERVAL);
+	}
+    return NULL;
+}
+
+
+
+int elev_enable_callbacks(void)
+{
+	pthread_t new_thread ;
+	
+	if (pthread_create(&new_thread, NULL, polling_thread, NULL) != 0)
+		return -1;
+	else
+		return new_thread;	
+}
+
+
+void elev_reset_all_lamps(void) {
+	elev_set_speed(0);
+	elev_set_stop_lamp(0);
+	elev_set_door_open_lamp(0);
+
+	for (int floor = 0; floor < N_FLOORS; ++floor) {
+		elev_set_button_lamp(ELEV_DIR_DOWN, floor, 0);
+		elev_set_button_lamp(ELEV_DIR_UP, floor, 0);
+		elev_set_button_lamp(ELEV_DIR_COMMAND, floor, 0);
+	}
+}
+

elev-threaded/elev.d

@@ -0,0 +1 @@
+elev.o: elev.c channels.h elev.h io.h

elev-threaded/elev.h

@@ -0,0 +1,144 @@
+// Wrapper for libComedi Elevator control.
+// These functions provides an interface to the elevators in the real time lab
+//
+// 2006, Martin Korsgaard
+#ifndef __INCLUDE_DRIVER_ELEV_H__
+#define __INCLUDE_DRIVER_ELEV_H__
+
+
+// Number of floors
+#define N_FLOORS 4
+
+
+typedef enum elev_direction_t {ELEV_DIR_UP = 0, ELEV_DIR_DOWN = 1, ELEV_DIR_COMMAND = 2, ELEV_DIR_NONE = 2}
+	elev_direction_t;
+
+/**
+	Sets the speed of the elevator. 
+	@param speed New speed of elevator. Positive values denote upward movement
+		and vice versa. Set speed to 0 to stop the elevator.
+*/
+void elev_set_speed(int speed);
+
+
+
+/**
+	Turn door-open lamp on or off.
+	@param value Non-zero value turns lamp on, 0 turns lamp off.
+*/
+void elev_set_door_open_lamp(int value);
+
+
+
+/**
+	Turn stop lamp on or off.
+	@param value Non-zero value turns lamp on, 0 turns lamp off.
+*/
+void elev_set_stop_lamp(int value);
+
+
+
+/**
+	Set floor indicator lamp for a given floor.
+	@param floor Which floor lamp to turn on. Other floor lamps are turned off.
+*/
+void elev_set_floor_indicator(int floor);
+
+
+
+/**
+	Set a button lamp.
+	@param lamp Which type of lamp to set (call up, call down, or "inside"
+		command).
+	@param floor Floor of lamp to set.
+	@param value Non-zero value turns lamp on, 0 turns lamp off.
+*/
+void elev_set_button_lamp(elev_direction_t lamp, int floor, int value);
+
+
+
+/**
+	Signal type definitions for register_callback().
+*/
+typedef enum tag_elev_signal {
+	SIGNAL_TYPE_CALL_UP = 0,
+	SIGNAL_TYPE_CALL_DOWN = 1,
+	SIGNAL_TYPE_COMMAND = 2,
+	SIGNAL_TYPE_SENSOR = 3,
+	SIGNAL_TYPE_STOP = 4,
+	SIGNAL_TYPE_OBSTR = 5
+} elev_signal_t;
+
+
+
+/**
+	Register a callback function to handle signals from the elevator.
+	@param type Type of signal to register handler for.
+	@param Callback function to call when signal arrives. The callback function
+		must be on the following form: `void fun(int floor, int value)'.
+		SIGNAL_TYPE_STOP triggers on pushing the stop button, `floor' and
+			`value' is always passed 0.
+		SIGNAL_TYPE_OBSTR triggers on flipping the obstruction switch. `floor'
+			is always passed 0. `value' is one for obstruction enabled, 0 for
+			disabled.
+		SIGNAL_TYPE_SENSOR triggers on both entering and leaving a floor.
+			`floor' is the number of the floor, `value' is 1 for entering the
+			floor, 0 for leaving.
+		SIGNAL_TYPE_CALL_UP,
+		SIGNAL_TYPE_CALL_DOWN,
+		SIGNAL_TYPE_COMMAND triggers on button push. The command buttons are the
+			floor buttons "inside" the elevator. `floor' is the floor of the
+			button being pushed, `value' is the type of button, according to
+			elev_direction_t, defined above (ELEV_DIR_DOWN etc.).
+*/
+void elev_register_callback(elev_signal_t type, void (*callback)(int, int));
+
+
+
+/**
+	Remove an association from a callback function.
+	After calling this function, callback will not be called when the associated
+		signal arrives.
+	@param type Type of signal to register handler for.
+*/
+void elev_unregister_callback(elev_signal_t type);
+
+
+
+/**
+	Initialize elevator.
+	@return Non-zero on success, 0 on failure.
+*/
+int elev_init(void);
+
+
+
+/**
+	Start calling callbacks when signals arrive.
+	This function spawns a new thread that polls the elevator for signals (like 
+		the elevator arriving at a new floor, or a button is pushed), and calls
+		the associated callback functions (if registered). 
+	@return thread index of new thread. Needed to call elev_disable_callbacks().
+*/
+int elev_enable_callbacks(void);
+	
+	
+
+/**
+	Stops calling callbacks when signals arrive.
+	This function will kill off the thread that polls the elevator for signals.
+	@param thread Value returned from corresponding elev_enable_callbacks().
+*/
+void elev_disable_callbacks(int thread) ;
+
+
+/**
+	Clears all lamps. 
+	Clears all lamps. Useful on reset, as lamps from previous runs may still be
+	lit.
+*/
+void elev_reset_all_lamps(void);
+
+
+#endif // #ifndef __INCLUDE_DRIVER_ELEV_H__
+

elev-threaded/globals.h

@@ -0,0 +1,24 @@
+#ifndef __INCLUDE_GLOBALS_H__
+#define __INCLUDE_GLOBALS_H__
+
+
+#define ELEV_GAIN 200
+
+#define PORT 20202
+#define PORTSTR "20202"
+#define IPGROUP "129.241.187.255"
+#define TCP_LISTEN_TIMEOUT 2
+#define TCP_PING_INTERVAL 1
+#define PING_ATTEMPTS 3
+#define MAX_ELEV 10
+
+// Flags for network communication
+#define PING 1
+#define PONG 2
+#define ORDER 3
+#define REQUEST 4
+#define STATUS 5
+#define CANDIDATE 6
+
+
+#endif

elev-threaded/io.c

@@ -0,0 +1,81 @@
+// Wrapper for libComedi I/O.
+// These functions provide and interface to libComedi limited to use in
+// the real time lab.
+//
+// 2006, Martin Korsgaard
+#include <comedilib.h>
+#include "io.h"
+#include "channels.h"
+
+
+// Pointer to libComedi device.
+static comedi_t *it_g = NULL;
+
+
+
+int io_init()
+{
+    int i, status = 0;
+
+    it_g = comedi_open("/dev/comedi0");
+  
+    if (it_g == NULL)
+        return 0;
+
+    for (i = 0; i < 8; i++) {
+        // comedi_dio_config is supposed to return 1 on success and -1 on error,
+        // but seems to return 0 on success on newest versions. Anyway, do a
+        // bitwise or, so that a single -1 will result in final value of -1.
+        status |= comedi_dio_config(it_g, PORT1, i, COMEDI_INPUT);
+        status |= comedi_dio_config(it_g, PORT2, i, COMEDI_OUTPUT);
+        status |= comedi_dio_config(it_g, PORT3, i+8, COMEDI_OUTPUT);
+        status |= comedi_dio_config(it_g, PORT4, i+16, COMEDI_INPUT);
+    }
+
+    return (status != -1);
+}
+
+
+
+void io_set_bit(int channel)
+{
+    comedi_dio_write(it_g, channel >> 8, channel & 0xff, 1);
+}
+
+
+
+void io_clear_bit(int channel)
+{
+    comedi_dio_write(it_g, channel >> 8, channel & 0xff, 0);
+}
+
+
+
+void io_write_analog(int channel, int value)
+{
+    comedi_data_write(it_g, channel>>8, channel&0xff, 0, AREF_GROUND, value);
+}
+
+
+
+int io_read_bit(int channel)
+{
+    unsigned int data=0;
+    comedi_dio_read(it_g, channel>>8, channel&0xff, &data);
+
+    return (int)data;
+}
+
+
+
+int io_read_analog(int channel)
+{
+    lsampl_t data = 0;
+    comedi_data_read(it_g, channel>>8, channel&0xff, 0, AREF_GROUND, &data);
+
+    return (int)data;
+}
+
+
+
+

elev-threaded/io.d

@@ -0,0 +1 @@
+io.o: io.c io.h channels.h

elev-threaded/io.h

@@ -0,0 +1,62 @@
+// Wrapper for libComedi I/O.
+// These functions provide and interface to libComedi limited to use in
+// the real time lab.
+//
+// 2006, Martin Korsgaard
+#ifndef __INCLUDE_DRIVER_IO_H__
+#define __INCLUDE_DRIVER_IO_H__
+
+
+
+/**
+  Initialize libComedi in "Sanntidssalen"
+  @return Non-zero on success and 0 on failure
+*/
+int io_init();
+
+
+
+/**
+  Sets a digital channel bit.
+  @param channel Channel bit to set.
+*/
+void io_set_bit(int channel);
+
+
+
+/**
+  Clears a digital channel bit.
+  @param channel Channel bit to set.
+*/
+void io_clear_bit(int channel);
+
+
+
+/**
+  Writes a value to an analog channel.
+  @param channel Channel to write to.
+  @param value Value to write.
+*/
+void io_write_analog(int channel, int value);
+
+
+
+/**
+  Reads a bit value from a digital channel.
+  @param channel Channel to read from.
+  @return Value read.
+*/
+int io_read_bit(int channel);
+
+
+
+
+/**
+  Reads a bit value from an analog channel.
+  @param channel Channel to read from.
+  @return Value read.
+*/
+int io_read_analog(int channel);
+
+#endif // #ifndef __INCLUDE_DRIVER_IO_H__
+

elev-threaded/local_queue.c

@@ -0,0 +1,197 @@
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "local_queue.h"
+#include "localsys.h"
+
+
+
+typedef struct tag_queue {
+  elev_direction_t button;
+  int floor;
+  struct tag_queue *next;
+} queue_t;
+
+queue_t *head = NULL;
+queue_t *last = NULL;
+
+
+
+void print_queue(){
+	queue_t *it = head;
+	printf("Local queue: ");
+	while(it != NULL){
+		printf("floor %d, type %d  |  ",it->floor+1, it->button);
+		it = it->next;
+	}
+	printf("\n");
+}
+
+
+void move_to_front(queue_t *node){  	
+	if (node == head) return;
+	
+	queue_t *prev = head;
+	queue_t *it = head->next;
+	
+	while (it != NULL) {
+		if (it != node) continue;
+		
+		if(prev->next == last) {
+			prev->next = NULL;
+			last->next = head;
+			head = last;
+			last = prev;
+			return;
+		}
+		else if(prev->next == it){
+			prev->next = it->next;
+			it->next = head;
+			head = it;
+			it = prev; 
+			return;
+		}
+		it = it->next;
+		prev = prev->next;
+	}
+	
+}
+
+
+
+void passing_by(queue_t *node) {
+	int end_dest = get_end_dest();
+	int floor = get_floor();
+	int dir = end_dest-floor;
+	
+	if (dir > 0) {
+		if ((node->floor < end_dest) && (node->floor > floor) && (node->button != ELEV_DIR_DOWN)) {
+			move_to_front(node);
+			set_dest(node->floor);
+			printf("New destination: %d\n", node->floor+1);
+		}
+	}
+	else if (dir < 0) {
+		if ((node->floor > end_dest) && (node->floor < floor) && (node->button != ELEV_DIR_UP)) {
+			move_to_front(node);
+			set_dest(node->floor);
+			printf("New destination: %d\n", node->floor+1);
+		}
+	}
+}
+
+
+
+void find_end_dest() {
+	queue_t *it = head;
+	int dest = get_dest();
+	int floor = get_floor();
+	int dir = dest-floor;
+	
+	while (it != NULL) {
+		if (((dir > 0) && (it->floor > get_end_dest()) && (it->button != ELEV_DIR_DOWN)) || 
+			((dir < 0) && (it->floor < get_end_dest()) && (it->button != ELEV_DIR_UP))) {
+			set_end_dest(it->floor);
+		}
+		it = it->next;
+	}
+	printf("End destination: %d\n", get_end_dest()+1);
+	
+}
+
+
+
+void pop_order() {
+	if(head == NULL) return;
+	elev_set_button_lamp(head->button, head->floor, 0);
+	
+	queue_t *new_head = head->next;
+	free(head);
+	head = new_head;
+
+	if(head == NULL) {
+		set_dest(-1);
+		set_end_dest(-1);
+		last = NULL;
+	}
+	else {
+		set_dest(head->floor);
+		printf("Next destination: %d\n", head->floor+1);
+		set_end_dest(head->floor);
+		find_end_dest();
+	}
+	print_queue();
+}
+
+
+void add_to_queue(elev_direction_t b, int f){
+	queue_t *it = head;
+	while (it != NULL) {
+		if ((it->button == b) && (it->floor == f)) return;
+		it = it->next;
+	}
+	
+	queue_t *new_order;
+	new_order = malloc(sizeof(queue_t));
+	new_order->button = b;
+	new_order->floor = f;
+	new_order->next = NULL;
+	
+	if (head == NULL){
+		set_end_dest(f);
+		set_dest(f);
+		printf("Next destination: %d\n", f+1);
+		head = new_order;
+		last = new_order;
+		elev_set_button_lamp(b, f, 1);
+	}
+	else {
+		last->next = new_order;
+		last = new_order;
+		elev_set_button_lamp(b, f, 1);
+		
+		passing_by(last);
+		find_end_dest();
+	}
+	print_queue();
+}
+
+void remove_orders() {
+	if (head == NULL) return;
+	queue_t *it = head;	
+	int found_call = 0;
+	
+	int dest = get_dest();
+	int floor = get_floor();
+	int dir = dest-floor;
+	while(it != NULL){
+		if (floor == it->floor) {
+			if (((dir > 0) && (it->button != ELEV_DIR_DOWN)) || 
+				((dir < 0) && (it->button != ELEV_DIR_UP))) {
+				move_to_front(it);
+				pop_order();
+			}
+			else if (it->button == ELEV_DIR_COMMAND) {
+				move_to_front(it);
+				pop_order();
+				it = it->next;
+				continue;
+			}
+			else if ((dir == 0) && (!found_call)) {
+				found_call = 1;
+				move_to_front(it);
+				pop_order();
+			}
+		}
+		it = it->next;
+	}
+}
+
+void clear_queue() {
+	queue_t *it = head;
+	while (it != NULL) {
+		pop_order();
+	}
+}
+
+

elev-threaded/local_queue.d

@@ -0,0 +1 @@
+local_queue.o: local_queue.c local_queue.h elev.h localsys.h

elev-threaded/local_queue.h

@@ -0,0 +1,6 @@
+#include "elev.h"
+
+void add_to_queue(elev_direction_t b, int f);
+void clear_queue();
+void remove_orders();
+int get_head();

elev-threaded/localsys.c

@@ -0,0 +1,297 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <assert.h>
+
+#include "local_queue.h"
+#include "elev.h"
+#include "io.h"
+#include "channels.h"
+#include "globals.h"
+#include "localsys.h"
+#include "netmod.h"
+#include "mastermod.h"
+
+
+int dest = -1;
+int end_dest = -1;
+elev_state_t state;
+int current_floor = -1;
+char msg[BUFLEN_TCP];
+
+
+void set_dest(int f) {
+	dest = f;
+}
+int get_dest() {
+	return dest;
+}
+
+void set_end_dest(int f) {
+	end_dest = f;
+}
+
+int get_end_dest() {
+	return end_dest;
+}
+
+int get_state() {
+	return state;
+}
+
+int get_floor() {
+	return current_floor;
+}
+
+void status_str(int s, int f, int end) {
+	msg[0] = (char) STATUS;
+	msg[1] = (char) s;
+	msg[2] = (char) f;
+	msg[3] = (char) end;
+}
+
+int timeDelay(int sec) {
+	static int count;
+	static time_t start;
+	count ++;
+	if ( count == 1) {
+		time(&start);
+	}
+	time_t end;
+	time(&end);
+
+	if (difftime(end,start) >= sec) {
+		count = 0;
+		return 1;
+	}
+	else
+		return 0;
+}
+
+void* state_machine(){
+	while(1) {	  
+	   switch (state) {
+
+			case DOOROPEN:
+				elev_set_door_open_lamp(1);
+				if (io_read_bit(OBSTRUCTION)) break;
+				
+				if (timeDelay(3)) {
+					state = DOORCLOSED;
+				    elev_set_door_open_lamp(0);
+					break;
+				}
+				else {
+					usleep(1e4);
+					break;
+				}
+
+			case DOORCLOSED:
+				 if (dest < 0){
+				 	printf("State: IDLE\n");
+					state = IDLE;
+					status_str(state, current_floor, end_dest);
+					if (get_role() != MASTER) {
+						if (write(get_master_sock(), msg, BUFLEN_TCP) == -1) printf("Failed to send status\n");
+					}
+					else set_elev_state(-1, msg);
+					break;
+				 }
+
+				 else if (current_floor < dest){
+				    elev_set_speed(150);
+				    printf("State: MOVING_UP\n");
+				    state = MOVING_UP;
+				    status_str(state, current_floor, end_dest);
+				    if (get_role() != MASTER) {
+						if (write(get_master_sock(), msg, BUFLEN_TCP) == -1) printf("Failed to send status\n");
+					}
+					else set_elev_state(-1, msg);
+				    break;
+				 }
+
+				 else if (current_floor > dest) {
+				    elev_set_speed(-150);
+				    printf("State: MOVING_DOWN\n");
+				    state = MOVING_DOWN;
+				    status_str(state, current_floor, end_dest);
+				    if (get_role() != MASTER) {
+						if (write(get_master_sock(), msg, BUFLEN_TCP) == -1) printf("Failed to send status\n");
+					}
+					else set_elev_state(-1, msg);
+				    break;
+				 }
+
+				 else if (current_floor == dest){
+				 	remove_orders();
+				 	printf("State: DOOROPEN\n");
+					state = DOOROPEN;
+					break;
+				}	
+		
+			case IDLE:
+				if (dest > -1) {
+					state = DOORCLOSED;
+					status_str(state, current_floor, end_dest);
+					if (get_role() != MASTER) {
+						if (write(get_master_sock(), msg, BUFLEN_TCP) == -1) printf("Failed to send status\n");
+					}
+					else set_elev_state(-1, msg);
+				}
+				usleep(1e4);
+			
+			default:
+				usleep(1e4);
+			
+		}
+		usleep(100);
+		
+	}
+	return NULL;
+}
+
+void stop_func(int floor, int value) {
+	elev_set_speed(0);
+	elev_set_stop_lamp(1);
+	state = STOPPED;
+	status_str(state, current_floor, end_dest);
+	printf("STOPPED\n");
+	if (get_role() != MASTER) {
+		if (write(get_master_sock(), msg, BUFLEN_TCP) == -1) printf("Failed to send status\n");
+	}
+	else set_elev_state(-1, msg);
+	
+}
+
+void command_func(int floor, int value) {
+	if (state == STOPPED) {
+		elev_set_speed(ELEV_GAIN);
+		elev_set_stop_lamp(0);
+	}
+	
+	add_to_queue(value, floor);
+}
+
+void elev_order_func(int floor, int value) {
+	char send[BUFLEN_TCP];
+	if (get_role() != MASTER) {
+		// Make backup? Check with ping pong etc
+		send[0] = (char) ORDER;
+		send[1] = (char) value;
+		send[2] = (char) floor;
+		
+		if (tcp_write(get_master_sock(), send) == -1) printf("Cant write to master, do something something...\n");
+		
+	}
+	else {
+		int elev = find_opt_elev(value, floor);
+		printf("Optimal elevator has socketid %d\n", elev);
+		
+		if (elev != -1) {
+			send[0] = (char) REQUEST;
+			if (tcp_write(elev, send) == -1) printf("Failed to give order...\n");
+		}
+		else add_to_queue(value, floor);
+	}
+}
+
+void sensor_func(int floor, int value) {
+	if (value == 0) return;
+	
+	elev_set_floor_indicator(floor);
+	current_floor = floor;
+	printf("Current floor: %d\n", current_floor+1);
+	
+	if (floor == dest) {
+		printf("Arriving at floor %d\n", floor+1);
+		elev_set_speed(0);
+		remove_orders();
+		//Husk å fjerne alle aktuelle ordre ved en funksjon
+		state = DOOROPEN;
+		status_str(state, current_floor, end_dest);
+		if (get_role() != MASTER) {
+			if (write(get_master_sock(), msg, BUFLEN_TCP) == -1) printf("Failed to send status\n");
+		}
+		else set_elev_state(-1, msg);
+		printf("State: DOOROPEN\n");
+		return;
+	}
+	if ((current_floor == N_FLOORS-1) || (current_floor == 0)) {
+		printf("Reached end of shaft unexpectedly\n");
+		elev_set_speed(0);
+		state = DOOROPEN;
+		status_str(state, current_floor, end_dest);
+		if (get_role() != MASTER) {
+			if (write(get_master_sock(), msg, BUFLEN_TCP) == -1) printf("Failed to send status\n");
+		}
+		else set_elev_state(-1, msg);
+		printf("State: DOOROPEN\n");	
+	}
+}
+
+void sensor_func_init(int floor, int value) {
+	elev_set_speed(0);
+	current_floor = floor;
+	elev_unregister_callback(SIGNAL_TYPE_SENSOR);
+	elev_register_callback(SIGNAL_TYPE_SENSOR, &sensor_func);
+	elev_register_callback(SIGNAL_TYPE_COMMAND, &command_func);
+	elev_register_callback(SIGNAL_TYPE_CALL_UP, &elev_order_func);
+	elev_register_callback(SIGNAL_TYPE_CALL_DOWN, &elev_order_func);
+	elev_register_callback(SIGNAL_TYPE_STOP, &stop_func);
+	state = IDLE;
+	pthread_t state_th;
+	pthread_create(&state_th, NULL, state_machine, NULL);		
+	printf("Elevator initialized and ready for use.\n");
+	
+	status_str(state, current_floor, end_dest);
+	if (get_role() != MASTER) {
+		if (write(get_master_sock(), msg, BUFLEN_TCP) == -1) printf("Failed to send status\n");
+	}
+	else set_elev_state(-1, msg);
+}
+
+int localsys_init() {
+	
+	if (!elev_init()) {
+		printf(__FILE__ ": Unable to initialize elevator hardware\n");
+		return 1;
+	}
+
+	elev_reset_all_lamps();
+	clear_queue();
+	
+	if (io_read_bit(SENSOR1)) current_floor = 0;
+	if (io_read_bit(SENSOR2)) current_floor = 1;
+	if (io_read_bit(SENSOR3)) current_floor = 2;
+	if (io_read_bit(SENSOR4)) current_floor = 3;
+	
+	if (current_floor > 0) {
+		elev_register_callback(SIGNAL_TYPE_SENSOR, &sensor_func);
+		elev_register_callback(SIGNAL_TYPE_COMMAND, &command_func);
+		elev_register_callback(SIGNAL_TYPE_CALL_UP, &elev_order_func);
+		elev_register_callback(SIGNAL_TYPE_CALL_DOWN, &elev_order_func);
+		elev_register_callback(SIGNAL_TYPE_STOP, &stop_func);
+		state = IDLE;
+		pthread_t state_th;
+		pthread_create(&state_th, NULL, state_machine, NULL);		
+		printf("Elevator initialized and ready for use.\n");
+		
+		status_str(state, current_floor, end_dest);
+		if (get_role() != MASTER) {
+			if (write(get_master_sock(), msg, BUFLEN_TCP) == -1) printf("Failed to send status\n");
+		}
+		else set_elev_state(-1, msg);
+	}
+	else {
+		printf("Going to nearest floor...\n");
+		elev_register_callback(SIGNAL_TYPE_SENSOR, &sensor_func_init);
+		elev_set_speed(-ELEV_GAIN);
+	}
+	
+	int callback_th = elev_enable_callbacks();
+	return callback_th;
+}
+
+

elev-threaded/localsys.d

@@ -0,0 +1,2 @@
+localsys.o: localsys.c local_queue.h elev.h io.h channels.h globals.h \
+ localsys.h netmod.h mastermod.h

elev-threaded/localsys.h

@@ -0,0 +1,19 @@
+typedef enum tag_elev_state {
+	DOOROPEN = 0,
+	MOVING_UP = 1,
+	MOVING_DOWN = 2,
+	DOORCLOSED = 3,
+	STOPPED = 4,
+	IDLE = 5,
+} elev_state_t;
+
+
+void set_dest(int f);
+int get_dest();
+void set_end_dest(int f);
+int get_end_dest();
+int get_state();
+int get_floor();
+int localsys_init();
+int timeDelay(int sec);
+

elev-threaded/main.c

@@ -0,0 +1,37 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <errno.h>
+#include <string.h>
+#include <pthread.h>
+
+//#include "elev.h"
+#include "localsys.h"
+#include "netmod.h"
+#include "globals.h"
+
+
+int main(int argc, char* argv[]) {
+	if (argc > 1) {
+		printf("Usage: Just run 'heis' on Sanntidssalen.\n");
+		return 1;
+	}			
+	
+	printf("\n########################################\n");
+	printf("Group 20\nMichael Soukup & Petter Rossvoll\n");
+	printf("########################################\n\n");
+	
+	printf("Initializing network on subnet %s through port %d...\n", IPGROUP, PORT);
+	
+	net_init();	
+	
+	printf("Initializing elevator...\n");
+	int callback_th;
+	callback_th = localsys_init();
+	
+	
+	while (1) {
+		
+	}
+	return 0;
+}

elev-threaded/main.d

@@ -0,0 +1 @@
+main.o: main.c localsys.h netmod.h globals.h

elev-threaded/mastermod.c

@@ -0,0 +1,201 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <time.h>
+
+#include "localsys.h"
+#include "mastermod.h"
+#include "elev.h"
+#include "globals.h"
+#include "netmod.h"
+
+
+role_t role = MASTER;
+int master_sock;
+int master_cand_sock = -1;
+status_t *lhead = NULL;
+status_t *llast = NULL;
+
+
+void print_status(){
+	status_t *it = lhead;
+	printf("Online Elevators: ");
+	while(it != NULL){
+		printf("socket %d, role %d  |  ",it->sockid, it->role);
+		it = it->next;
+	}
+	printf("\n");
+}
+
+void add_to_list(int sock){
+	status_t *new_elev;
+	new_elev = malloc(sizeof(status_t));
+	new_elev->sockid = sock;
+	new_elev->next = NULL;
+
+	if (lhead == NULL){
+		lhead = new_elev;
+	}
+	else {
+		llast->next = new_elev;
+	}
+	llast = new_elev;
+	print_status();
+}
+
+int get_master_cand_sock() {
+	return master_cand_sock;
+}
+
+void set_alive(int sock, int b) {
+	status_t *it = lhead;
+	while(it != NULL){
+		if (it->sockid == sock) {
+			it->alive = b;
+			return;
+		}	
+		it = it->next;
+	}
+}
+
+role_t get_role() {
+	return role;
+}
+void set_role(role_t r) {
+	role = r;
+}
+
+int get_master_sock() {
+	return master_sock;
+}
+
+void set_master_flag() {
+	lhead->sockid = -1;
+}
+
+void set_master_sock(int sock) {
+	master_sock = sock;
+}
+
+void print_elevs() {
+	status_t *it = lhead;
+	printf("Elevator list: ");
+	while(it != NULL){
+		printf("Socket %d, alive %d\n",it->sockid, it->alive);
+		it = it->next;
+	}
+}
+
+void add_elev(int sock){	
+	status_t *new_elev;
+	new_elev = malloc(sizeof(status_t));
+	new_elev->sockid = sock;
+	new_elev->floor = 0;
+	new_elev->end_dest = 0;
+	new_elev->state = STOPPED;
+	new_elev->alive = 1;
+	new_elev->next = NULL;
+
+	if (lhead == NULL){
+		lhead = new_elev;
+		new_elev->role = MASTER;
+	}
+	else {
+		llast->next = new_elev;
+		new_elev->role = SLAVE;
+	}
+	llast = new_elev;
+	print_elevs();
+}
+
+void master_init(int s) {
+	add_elev(s);
+	lhead->role = MASTER;
+	lhead->alive = 1;
+}
+
+void add_slave(int sock) {
+	add_to_list(sock);
+	
+	llast->floor = 0;
+	llast->state = STOPPED;	
+	llast->alive = 1;
+	llast->end_dest = 0;
+	
+	if (master_cand_sock == -1) {
+		master_cand_sock = sock;
+		llast->role = MASTER_CAND;
+	}
+	else llast->role = SLAVE;
+}
+
+void set_elev_state(int sock, char str[]) {
+	status_t *it = lhead;
+	while(it != NULL){
+		if (it->sockid == sock) {
+			it->state = (int) str[1];
+			it->floor = (int) str[2];
+			it->end_dest = (int) str[3];
+			return;
+		}
+		
+		
+		it = it->next;
+	}
+}
+
+int find_opt_elev(int call, int floor) {
+	int best = abs(floor - lhead->floor);
+	status_t *res = lhead;
+	status_t *it = lhead->next;
+	int dir = lhead->end_dest-lhead->floor;
+	
+	int passing = 0;
+	int idle = 0;
+	
+	while (it != NULL) {
+		if (((!it->alive) || (it->end_dest == -1))) {
+			it = it->next;
+			continue;
+		}
+		dir = it->end_dest-it->floor;
+		
+		if (((dir > 0) && (floor > it->floor) && (floor <= it->end_dest) && (call != ELEV_DIR_DOWN)) || 
+			((dir < 0) && (floor < it->floor) && (floor >= it->end_dest) && (call != ELEV_DIR_UP))) {
+			if (abs(floor - it->floor) < best) {
+				res = it;
+				best = abs(floor - it->floor);
+				passing = 1;
+			}
+		}
+		it = it->next;
+	}
+	
+	if (!passing) {
+		it = lhead;
+		best = N_FLOORS+1;
+		while (it != NULL) {
+			if (!it->alive) {
+				it = it->next;
+				continue;
+			}
+			if ((it->state == IDLE) && (abs(floor - it->floor) < best)) {
+				best = abs(floor - it->floor);
+				res = it;
+				idle = 1;	
+			}
+			else if ((!idle) && (abs(floor - it->floor) < best)){
+				best = abs(floor - it->floor);
+				res = it;
+			}
+			it = it->next;
+		}
+		
+	}	
+	return res->sockid;
+}
+
+
+

elev-threaded/mastermod.d

@@ -0,0 +1 @@
+mastermod.o: mastermod.c localsys.h mastermod.h elev.h globals.h netmod.h

elev-threaded/mastermod.h

@@ -0,0 +1,30 @@
+
+typedef enum tag_role{
+	SLAVE = 0,
+	MASTER = 1,
+	MASTER_CAND = 2,
+} role_t;
+
+typedef struct tag_status {
+	int sockid;
+	int floor;
+	int end_dest;
+	elev_state_t state;
+	role_t role;
+	int alive;
+	struct tag_status *next;
+} status_t;
+
+int get_master_cand_sock();
+role_t get_role();
+void set_role(role_t r);
+void set_alive(int sock, int b);
+void set_master_flag();
+void set_master_sock(int sock);
+int get_master_sock();
+int get_master_cand_sock();
+void master_init();
+void add_slave(int sock);
+void set_elev_state(int sock, char str[]);
+int find_opt_elev(int call, int floor);
+

elev-threaded/netmod.c

@@ -0,0 +1,360 @@
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <netdb.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <pthread.h>
+#include <time.h>
+
+#include "localsys.h"
+#include "netmod.h"
+#include "local_queue.h"
+#include "globals.h"
+#include "mastermod.h"
+
+
+int ping_tries = 0;
+
+int tcp_write(int sock, char s[]) {
+	return send(sock, s, BUFLEN_TCP, 0);
+}
+
+void* tcp_read(void* s) {
+	int sock = (int) s;
+	char rec_buf[BUFLEN_TCP];
+	char sen_buf[BUFLEN_TCP];
+	//char order_buf[BUFLEN_TCP];
+	int flag = -1;
+	int cand_isset = 0;
+	int err;
+	
+	while (1) {
+		err = recv(sock, rec_buf, BUFLEN_TCP, 0);
+		if (err < 0) {
+			printf("Connection error recv from socket id %d\n", sock);	
+			if (get_role() != SLAVE) set_alive(sock, 0);
+			return NULL;	
+		}
+		else if (err == 0){
+			printf("Connection with socket id %d was shut down\n", sock);	
+			if (get_role() != SLAVE) set_alive(sock, 0);
+			return NULL;	
+		}
+		flag = (int) rec_buf[0];
+		
+		switch (flag) {
+			case PING:
+				if (!cand_isset) {
+					if (sock == get_master_cand_sock()) {
+						sen_buf[0] = (char) CANDIDATE;
+						sen_buf[1] = (char) 1;
+						cand_isset = 1;
+						if (write(sock, sen_buf, BUFLEN_TCP) == -1) printf("Couldnt set candidate!\n");
+					}
+					else {
+						sen_buf[0] = (char) CANDIDATE;
+						sen_buf[1] = (char) 0;
+						if (write(sock, sen_buf, BUFLEN_TCP) == -1) printf("Couldnt set candidate!\n");
+						cand_isset = 1;
+					}
+				}
+				sen_buf[0] = (char) PONG;
+				write(sock, sen_buf, BUFLEN_TCP);
+				break;
+			
+			case PONG:
+				ping_tries = 0;
+				break;
+			
+			case STATUS:
+				set_elev_state(sock, rec_buf);
+				printf("Status update: state %d, floor %d, end destination %d\n", rec_buf[1], rec_buf[2], rec_buf[3]);
+				
+				break;
+			
+			case ORDER:
+				printf("New order. Call %d, floor %d\n", rec_buf[1], rec_buf[2]);
+				/*order_buf[0] = (char) ORDER;
+				order_buf[1] = rec_buf[1];
+				order_buf[2] = rec_buf[2];*/
+				sen_buf[0] = (char) REQUEST;
+				sen_buf[1] = (char) rec_buf[1];
+				sen_buf[2] = (char) rec_buf[2];
+				
+				int elev = find_opt_elev((int) rec_buf[1], (int) rec_buf[2]);
+				printf("Optimal elevator has socketid %d\n", elev);	
+				
+				if (elev != -1) {
+					if (write(elev, sen_buf, BUFLEN_TCP) == -1) printf("Couldnt deliver order!\n");
+				}
+				else add_to_queue((int) rec_buf[1], (int) rec_buf[2]);
+				break;
+			
+			case REQUEST:
+				elev_set_button_lamp((int) rec_buf[1], (int) rec_buf[2], 1);
+				printf("New request\n");
+				add_to_queue((int) rec_buf[1], (int) rec_buf[2]);
+				break;
+			
+			case CANDIDATE:
+				if ((int) rec_buf[1]) {
+					printf("Taking role as master candidate\n");
+					master_init(sock);
+					set_role(MASTER_CAND);
+				}
+				else {
+					printf("Taking role as slave\n");
+					set_role(SLAVE);
+				}
+				break;
+						
+			case (-1):
+				printf("Flag is not set\n");
+				break;
+				
+		}
+		
+	}
+	return NULL;
+}
+
+void* ping_master(void* s) {	
+	int socket = (int) s;
+	char msg[BUFLEN_TCP];
+	msg[0] = (char) PING;
+	while(1) {
+		//printf("-> Ping...\n");
+		write(socket, msg, BUFLEN_TCP);	
+		if(ping_tries == PING_ATTEMPTS) {
+			printf("Lost connection to master\n");
+			break;
+		}
+		ping_tries++;
+		sleep(TCP_PING_INTERVAL);
+	}
+	
+	printf("No pong response!\n");
+	net_init();
+	return NULL;
+	
+}
+
+void* tcp_listen(void* timeout_id) {
+	struct sockaddr_in socklisten_addr;
+	struct sockaddr_in master_addr;
+	int sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
+	socklen_t addrlen;
+	
+	if (sock == -1) {
+		perror("socket");
+		exit(1);
+	}
+	
+	memset(&socklisten_addr, 0, sizeof socklisten_addr);
+	socklisten_addr.sin_family = AF_INET;
+	socklisten_addr.sin_port = htons(PORT);
+	socklisten_addr.sin_addr.s_addr = INADDR_ANY;
+	
+	int reuse = 1;
+	if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof reuse) == -1) {
+		perror("setsockopt");
+		close(sock);
+		exit(1);
+	}
+	
+	if (bind(sock, (struct sockaddr *)&socklisten_addr, sizeof socklisten_addr) == -1) {
+		perror("bind");
+		close(sock);
+		//exit(1);
+	}
+	
+	if (listen(sock, 1) < 0) {
+		perror("listen");
+		exit(1);
+	}
+	
+	printf("Waiting to accept...\n");	
+	addrlen = sizeof master_addr;		
+	int sock_master = accept(sock, (struct sockaddr *)&master_addr, &addrlen);
+	
+	printf("Connected to master\n");
+	set_master_sock(sock_master);
+	set_role(SLAVE);
+
+	pthread_t read_th, ping_th;
+	pthread_create(&read_th, NULL, tcp_read, (void*) sock_master);
+	pthread_create(&ping_th, NULL, ping_master, (void*) sock_master);
+
+	printf("Network initialization complete.\n");
+	
+	close(sock);
+	
+			
+	return NULL;
+	
+}
+
+
+void tcp_connect(char ip[]) {
+	struct sockaddr_in sockconn_addr;
+	int sockid = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
+
+	if (sockid == -1) {
+		perror("socket");
+		exit(1);
+	}
+	
+	memset(&sockconn_addr, 0, sizeof sockconn_addr);
+	sockconn_addr.sin_family = AF_INET;
+	sockconn_addr.sin_port = htons(PORT);
+	
+	if(inet_pton(AF_INET, ip, &sockconn_addr.sin_addr) == 0) {
+		perror("Not a valid ip");
+		close(sockid);
+		exit(1);
+	}
+	
+	printf("Connecting to %s ...\n", ip);
+	if (connect(sockid, (struct sockaddr *)&sockconn_addr, sizeof sockconn_addr) == -1) {
+		perror("connect");
+		close(sockid);
+		exit(1);
+	}
+	
+	printf("Connected to slave with socket ID %d\n", sockid);
+	add_slave(sockid);
+	
+	pthread_t read_th;
+	pthread_create(&read_th, NULL, tcp_read, (void*) sockid);
+}
+
+void udp_broadcast() {
+	int sock;
+	struct sockaddr_in broadcast_addr;
+	struct hostent *he;
+	int broadcast = 1;
+	
+	char dummymsg[BUFLEN_UDP] = "\0"; 
+	
+	if ((he = gethostbyname(IPGROUP)) == NULL) {
+		perror("gethostbyname");
+		exit(1);
+	}
+	
+	if ((sock = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
+		perror("socket");
+		exit(1);
+	}
+	
+	// Allow broadcast
+	if (setsockopt(sock, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof broadcast) == -1) {
+		perror("setsockopt (SO_BROADCAST)");
+		close(sock);
+		exit(1);
+	}
+	
+	broadcast_addr.sin_family = AF_INET;
+	broadcast_addr.sin_port = htons(PORT);
+	broadcast_addr.sin_addr = *((struct in_addr *)he->h_addr);
+	memset(broadcast_addr.sin_zero, '\0', sizeof broadcast_addr.sin_zero);
+	
+	
+	printf("Broadcasting to %s\n", IPGROUP);
+	if (sendto(sock, dummymsg, strlen(dummymsg), 0, (struct sockaddr *)&broadcast_addr, sizeof broadcast_addr) == -1) {
+		perror("sendto");
+		exit(1);
+	}
+	
+	close(sock);
+}
+
+void* udp_listen() {
+	int sock;
+	struct addrinfo hints;
+	struct addrinfo *result, *rp;
+	struct sockaddr_in slave_addr;
+	char buf[BUFLEN_UDP];
+	socklen_t addrlen = sizeof(struct sockaddr);
+	
+	memset(&hints, 0, sizeof hints);
+	hints.ai_family = AF_INET;
+	hints.ai_socktype = SOCK_DGRAM;
+	hints.ai_flags = AI_PASSIVE;
+	
+	if (getaddrinfo(NULL, PORTSTR, &hints, &result) != 0) {
+		perror("getaddrinfo");
+		exit(1);
+	}
+	
+	for (rp = result; rp != NULL; rp = rp->ai_next) {
+		sock = socket(rp->ai_family, rp->ai_socktype, 0);
+		
+		if (sock == -1)
+			continue;
+		
+		if (bind(sock, rp->ai_addr, rp->ai_addrlen) == 0)
+			break;
+			
+		close(sock);
+	}
+	
+	freeaddrinfo(result);
+	printf("Waiting for slaves...\n");
+	
+	while(1) {
+		
+		if (recvfrom(sock, buf, BUFLEN_UDP, 0, (struct sockaddr *)&slave_addr, &addrlen) == -1) {
+			perror("recvfrom");
+			exit(1);
+		}
+		
+		printf("Found new slave\n");
+		tcp_connect(inet_ntoa(slave_addr.sin_addr));
+	}
+	return NULL;	
+}
+
+void* timeout(void* tcp_listen_id) {
+	sleep(TCP_LISTEN_TIMEOUT);
+	
+	if (get_role() == MASTER) {
+		printf("Master is not present\nStarting as master...\n");
+		master_init(-1);
+		pthread_cancel((pthread_t) tcp_listen_id);
+		printf("Start listening for slave broadcasts through UDP\n");
+		pthread_t udp_listen_th;
+		pthread_create(&udp_listen_th, NULL, udp_listen, NULL);
+		printf("Network initialization complete.\n");
+		
+	}
+		
+	return NULL;	
+}
+
+void net_init() {
+	printf("Starting search for master\n");
+	
+	if (get_role() == MASTER_CAND) {
+		set_role(MASTER);
+		set_master_flag();
+		printf("I am master candidate. Taking role as master...\n");
+		pthread_t udp_listen_th;
+		pthread_create(&udp_listen_th, NULL, udp_listen, NULL);
+	}
+	else {
+		pthread_t tcp_listen_th, timeout_th;	
+		pthread_create(&tcp_listen_th, NULL, tcp_listen, (void*) timeout_th);
+		pthread_create(&timeout_th, NULL, timeout, (void*) tcp_listen_th);
+	
+		udp_broadcast();	
+
+		pthread_join(timeout_th, NULL);
+	}
+}
+
+

elev-threaded/netmod.d

@@ -0,0 +1,2 @@
+netmod.o: netmod.c localsys.h netmod.h local_queue.h elev.h globals.h \
+ mastermod.h

elev-threaded/netmod.h

@@ -0,0 +1,6 @@
+#define BUFLEN_TCP 16
+#define BUFLEN_UDP 16
+
+void net_init();
+int tcp_write(int sock, char s[]);
+