hook various previously unused modules together.

This now allows us to send messages to the device and such. Yay? Yay.

Signed-off-by: Gediminas Jakutis <gediminas@varciai.lt>

14 files changed, 258 insertions, 208 deletions

diff --git a/include/net.h b/include/net.h
index 3fbaf5c..dd02999 100644
--- a/include/net.h
+++ b/include/net.h
@@ -47,6 +47,5 @@ int net_getlastdata(int nd, char ** const data, size_t *recvsize);
 int net_send(int nd, const char * const buf, size_t buf_size);
 
 int net_send_addr(int nd, const char * const buf, size_t buf_size, const struct sockaddr_in * const addr);
-void net_flush_read_buffer(int nd);
 
 #endif /* USURPATION_NET_H_INCLUDED */
diff --git a/include/proto_stdio.h b/include/proto_stdio.h
index c54ef9c..c7b9b25 100644
--- a/include/proto_stdio.h
+++ b/include/proto_stdio.h
@@ -22,9 +22,11 @@
 #ifndef USURPATION_PROTO_STDIO_H
 #define USURPATION_PROTO_STDIO_H
 
+#include "tlv.h"
+
 int proto_stdio_init(void);
 void proto_stdio_close(void);
 void message_receive(char *);
-char *message_send(void);
+struct tlv *message_send(struct tlv *in);
 
 #endif /* USURPATION_PROTO_STDIO_H */
diff --git a/meson.build b/meson.build
index 035f7a4..eae97a1 100644
--- a/meson.build
+++ b/meson.build
@@ -16,15 +16,16 @@ resource_dir_arg = 'DATA_DIR=' + '"' + join_paths(get_option('prefix'), resource
 add_project_arguments('-D', resource_dir_arg, language : 'c')
 add_project_link_arguments('-rdynamic', language : 'c')
 
-subdir('src')
-verb = '1'
+debugbuild = '0'
 if get_option('buildtype') == 'debug'
-	verb = '3'
+	debugbuild = '1'
 elif get_option('buildtype') == 'debugoptimized'
-	verb = '3'
+	debugbuild = '1'
 endif
 
-daemon = executable(progname, [d_sources, common_sources], version, include_directories : inc, install : true, dependencies : deps, extra_files : d_conf, c_args : ['-DUSURP_VERBOSITY=' + verb, '-DSYSCONFDIR=' + get_option('sysconfdir')])
+subdir('src')
+
+daemon = executable(progname, [d_sources, common_sources], version, include_directories : inc, install : true, dependencies : deps, extra_files : d_conf, c_args : ['-DDEBUG_BUILD=' + debugbuild, '-DSYSCONFDIR=' + get_option('sysconfdir')])
 #install_data(extra, install_dir : resource_dir)
 install_data(d_conf, install_dir : get_option('sysconfdir'))
 
diff --git a/src/common/tlv.c b/src/common/tlv.c
index 84b20ca..0e800ca 100755
--- a/src/common/tlv.c
+++ b/src/common/tlv.c
@@ -45,17 +45,17 @@ int tlv_get(char *in, struct tlv *tlv, char **saveptr)
 	memcpy(&head, in, sizeof(head));
 	head = ntoh_tlvh(&head);
 
+	tlv->head = head;
 	if (head.type == INVALID || !head.size) {
 		ret = NONEWDATA;
-		goto out;
+	} else {
+		tlv->data = in + sizeof(head);
 	}
 
-	tlv->head = head;
-	tlv->data = in + sizeof(head);
 	if (saveptr) {
 		*saveptr = in + sizeof(head) + head.size;
 	}
-out:
+
 	return ret;
 }
 
diff --git a/src/daemon/main.c b/src/daemon/main.c
index 07da36d..fd7932e 100644
--- a/src/daemon/main.c
+++ b/src/daemon/main.c
@@ -37,6 +37,7 @@ static struct _state {
 
 static void cleanup(void);
 static int __main_loop(const struct timespec * const iter_len);
+static int does_my_kokoro_go_doki_doki(struct tlv *tlv);
 
 /* the logic is a placeholder right now */
 int main(int argc, char **argv)
@@ -79,40 +80,49 @@ int main(int argc, char **argv)
 /* TODO: semi-stub */
 static int __main_loop(const struct timespec * const iter_len)
 {
-	struct tlv field;
 	struct tlv_packet pack;
+	struct tlv field;
 	struct timespec t;
 	char *buf = NULL;
 	size_t recvsize;
 	int ret = A_OK;
 
 	t = *iter_len;
-	tlv_init(&field, HEARTBEAT);
-	field.data = strdup(heartbeat_server);
-	field.head.size = sizeof(heartbeat_server);
 	tlv_packet_init(&pack);
-	tlv_pack(&pack, &field);
-	tlv_packet_finalize(&pack);
-	tlv_destroy(&field);
-
-	/* flush any pending discovery packets from the socker,
-	 * as those can be quite numerous on startup.
-	 */
-	net_flush_read_buffer(__progstate.nd);
 
 	while (ret != ERROR) {
 		ret = net_getlastdata(__progstate.nd, &buf, &recvsize);
 
-		if (ret == A_OK) {
-			/* we're only sending one TLV per packet as of now,
-			 * so no need to save the pointer for further parsing
-			 */
-			ret = tlv_get(buf, &field, NULL);
-			if (ret == A_OK && field.head.type == HEARTBEAT && !(strcmp(field.data, heartbeat_device))) {
-				/* error checking? what error checking? */
-				ret = net_send(__progstate.nd, pack.data, pack.cursor + 1);
+		if (ret != A_OK) {
+			goto skiprecv;
+		}
+		/* we're only sending one TLV per packet as of now,
+		 * so no need to save the pointer for further parsing
+		 */
+		ret = tlv_get(buf, &field, NULL);
+
+		/* handles discovery */
+		if (ret != A_OK || !(does_my_kokoro_go_doki_doki(&field))) {
+			goto skiprecv;
+		}
+
+		/* handles receiving messages */
+		if (field.head.type == TEXT) {
+			message_receive(field.data);
+		}
+/* jumping here if did not receive any data */
+skiprecv:
+		/* handles sending messages */
+		if (message_send(&field)) {
+			if (field.head.size) {
+				tlv_pack(&pack, &field);
+				tlv_packet_finalize(&pack);
+				tlv_destroy(&field);
+				net_send(__progstate.nd, pack.data, pack.cursor + 1);
+				tlv_packet_reset(&pack);
 			}
 		}
+
 		nanosleep(&t, NULL);
 	}
 
@@ -121,6 +131,35 @@ static int __main_loop(const struct timespec * const iter_len)
 	return ret;
 }
 
+static int does_my_kokoro_go_doki_doki(struct tlv *tlv)
+{
+	int ret = ERROR;
+	static struct tlv_packet pack;
+	static size_t firstrun = 1;
+
+	if (firstrun) {
+		struct tlv field;
+		tlv_init(&field, HEARTBEAT);
+		field.data = strdup(heartbeat_server);
+		field.head.size = sizeof(heartbeat_server);
+		tlv_packet_init(&pack);
+		tlv_pack(&pack, &field);
+		tlv_packet_finalize(&pack);
+		tlv_destroy(&field);
+		firstrun = 0;
+	}
+
+	if (tlv->head.type == HEARTBEAT && !(strcmp(tlv->data, heartbeat_device))) {
+		if (setting_verbose() >= INFO) {
+			fprintf(stderr, "heartbeat.\n");
+		}
+
+		ret = net_send(__progstate.nd, pack.data, pack.cursor + 1);
+	}
+
+	return ret;
+}
+
 static void cleanup(void)
 {
 	purple_close();
diff --git a/src/daemon/net.c b/src/daemon/net.c
index 640bd28..11a6a21 100644
--- a/src/daemon/net.c
+++ b/src/daemon/net.c
@@ -39,6 +39,7 @@
 
 struct netstate {
 	struct timespec lastreply;
+	struct timespec start_time;
 	int nd;
 	int sock;
 	unsigned short int port;
@@ -95,7 +96,6 @@ int net_init(const unsigned short int port)
 		goto out;
 	}
 
-	clock_gettime(CLOCK_MONOTONIC_RAW, &state[i].lastreply);
 	state[i].lastreply.tv_sec -= 5;
 	state[i].nd = i + 1;
 	state[i].port = port;
@@ -104,6 +104,8 @@ int net_init(const unsigned short int port)
 	state[i].bufsize = MTU;
 	state[i].sock_status = NONEWDATA;
 	state[i].buffer_status = NONEWDATA;
+	clock_gettime(CLOCK_MONOTONIC_RAW, &state[i].lastreply);
+	state[i].start_time = state[i].lastreply;
 	pthread_mutex_init(&state[i].mutex, NULL);
 
 	if (pthread_create(&state[i].listner, NULL, dolisten, state + i)) {
@@ -185,14 +187,15 @@ int net_send_addr(int nd, const char * const buf, size_t buf_size, const struct
 {
 	int ret = A_OK;
 	ssize_t sent;
-	struct timespec now;
+	struct timespec t;
 
 	if (nd > count || nd < 1 || state[--nd].available) {
 		ret = ERROR;
 	} else {
 		if (setting_verbose() >= INFO) {
-			clock_gettime(CLOCK_MONOTONIC_RAW, &now);
-			fprintf(stderr, "Sending packet to %s, timestap: %li \n", inet_ntoa(state[nd].clientaddr.sin_addr), now.tv_sec);
+			clock_gettime(CLOCK_MONOTONIC_RAW, &t);
+			t = rin_time_sub(&t, &state[nd].start_time);
+			fprintf(stderr, "Sending packet to %s, timestap: %li.%06lis \n", inet_ntoa(state[nd].clientaddr.sin_addr), t.tv_sec, t.tv_nsec / 1000);
 		}
 
 		sent = sendto(	state[nd].sock, buf, buf_size, MSG_DONTWAIT,
@@ -210,51 +213,16 @@ int net_send_addr(int nd, const char * const buf, size_t buf_size, const struct
 	return ret;
 }
 
-void net_flush_read_buffer(int nd)
-{
-	struct netstate *st;
-	int cancelstate;
-	struct timespec now;
-	struct timespec later;
-	struct timespec delta;
-
-	if (setting_verbose() >= INFO) {
-		fprintf(stderr, "acquiring net mutex\n");
-	}
-
-	if (!(nd > count || nd < 1 || state[--nd].available || state[nd].sock_status != A_OK)) {
-		st = state + nd;
-		pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, &cancelstate);
-		pthread_mutex_lock(&st->mutex);
-
-		if (setting_verbose() >= INFO) {
-			clock_gettime(CLOCK_MONOTONIC_RAW, &now);
-			fprintf(stderr, "flushing netsocket\n");
-		}
-
-		do {
-			st->sock_status = getpacket(st->data, st->bufsize, &st->recvsize, st->sock, &st->clientaddr);
-		} while(st->sock_status);
-
-		if (setting_verbose() >= INFO) {
-			clock_gettime(CLOCK_MONOTONIC_RAW, &later);
-			delta = rin_time_sub(&later, &now);
-			fprintf(stderr, "finished flushing netsocket in %lis%lins\n", delta.tv_sec, delta.tv_nsec);
-		}
-
-		pthread_mutex_unlock(&st->mutex);
-		pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, &cancelstate);
-		pthread_testcancel();
-	}
-}
-
 static void *dolisten(void * state)
 {
 	struct timespec now;
-	struct timespec wait = {0, 1 * 1000 * 1000}; /* 1 ms */
+	struct timespec delta;
+	struct timespec wait = {0, 1 * 1000 * 1000};	/* 1 ms */
+	struct timespec timeout = {60, 0};		/* 1 minute */
 	struct netstate *st;
 	int cancelstate;
 	int oldstatus = DEAD;
+	char *ipstring;
 
 	st = state;
 
@@ -267,18 +235,25 @@ static void *dolisten(void * state)
 		if (!st->sock_status) {
 			st->lastreply = now;
 			st->buffer_status = A_OK;
+
+			if (setting_verbose() >= INFO) {
+				ipstring = inet_ntoa(st->clientaddr.sin_addr);
+				delta = rin_time_sub(&now, &st->start_time);
+				fprintf(stderr, "packet from %s received at %li.%06li\n", ipstring, delta.tv_sec, delta.tv_nsec / 1000);
+			}
 		}
 
-		/* no packets in five seconds */
-		if ((now.tv_sec - st->lastreply.tv_sec) >= 20) {
+		/* timeout if no packets in over minute */
+		delta = rin_time_sub(&now, &st->lastreply);
+		if (rin_time_cmp_more(&delta, &timeout)) {
 			st->sock_status = DEAD;
 		};
 
 		if (oldstatus != st->sock_status && st->sock_status != NONEWDATA) {
 			oldstatus = st->sock_status;
-			if (st->sock_status == DEAD) {
-				/* this timestamp is arbitraty */
-				setting_verbose() >= INFO ? fprintf(stderr, "Connection with the client has been lost. Last reply since: %li \n", st->lastreply.tv_sec) : 0;
+			if (st->sock_status == DEAD && setting_verbose() >= INFO) {
+				delta = rin_time_sub(&now, &st->start_time);
+				fprintf(stderr, "Connection with the client has been lost. Last reply was: %li.%06lis ago\n", delta.tv_sec, delta.tv_nsec / 1000);
 			}
 		}
 
@@ -297,7 +272,6 @@ static int getpacket(char *data, size_t buffsize, ssize_t *recvbufsize, int sock
 {
 	static struct pollfd pfd = {0};
 	int ret;
-	char *ipstring;
 	socklen_t sender_len = sizeof(*sender);
 
 	if (!pfd.fd) {
@@ -316,8 +290,6 @@ static int getpacket(char *data, size_t buffsize, ssize_t *recvbufsize, int sock
 		if (*recvbufsize < 0) {
 			ret = ERR;
 		} else {
-			ipstring = inet_ntoa(sender->sin_addr);
-			setting_verbose() >= INFO ? fprintf(stderr, "packet from %s received\n", ipstring) : 0;
 			ret = A_OK;
 		}
 	} else {
diff --git a/src/daemon/proto_stdio.c b/src/daemon/proto_stdio.c
index 9aecdf0..6735746 100644
--- a/src/daemon/proto_stdio.c
+++ b/src/daemon/proto_stdio.c
@@ -23,7 +23,6 @@
 #include "proto_stdio.h"
 #include "proto_stdio_private.h"
 
-
 void message_receive(char *arg)
 {
 	int cancelstate;
@@ -37,19 +36,25 @@ void message_receive(char *arg)
 	nanosleep(&respite, NULL);
 }
 
-char *message_send(void)
+struct tlv *message_send(struct tlv *in)
 {
-	char *ret = NULL;
+	struct tlv *ret = in;
 	int cancelstate;
 
 	pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, &cancelstate);
 	pthread_mutex_lock(&state.out_m);
 
 	if (state.readbuf) {
-		ret = strdup(state.readbuf);
+		tlv_init(ret, TEXT);
+		ret->data = malloc(state.bytes_read);
+		ret->head.size = state.bytes_read;
+		memcpy(ret->data, state.readbuf, state.bytes_read);
 		free(state.readbuf);
 		state.readbuf = NULL;
 		state.readbufsize = 0;
+		state.bytes_read = 0;
+	} else {
+		ret = NULL;
 	}
 
 	pthread_mutex_unlock(&state.out_m);
@@ -70,7 +75,7 @@ static void *read_stdin(void *arg)
 		pthread_mutex_lock(&state.in_m);
 
 		if (!state.readbuf) {
-			state.readbufsize = getline(&state.readbuf, NULL, stdin);
+			state.bytes_read = getline(&state.readbuf, &state.readbufsize, stdin);
 		}
 
 		pthread_mutex_unlock(&state.in_m);
diff --git a/src/daemon/proto_stdio_private.h b/src/daemon/proto_stdio_private.h
index 209d191..a0b891a 100644
--- a/src/daemon/proto_stdio_private.h
+++ b/src/daemon/proto_stdio_private.h
@@ -35,9 +35,10 @@ static struct state {
 	pthread_mutex_t out_m;
 	char *readbuf;
 	char *writebuf;
-	ssize_t readbufsize;
+	size_t readbufsize;
+	ssize_t bytes_read;
 	ssize_t writebufsize;
-} state = {PTHREAD_MUTEX_INITIALIZER, 0, 0, PTHREAD_MUTEX_INITIALIZER, PTHREAD_MUTEX_INITIALIZER, NULL, NULL, 0, 0};
+} state = {PTHREAD_MUTEX_INITIALIZER, 0, 0, PTHREAD_MUTEX_INITIALIZER, PTHREAD_MUTEX_INITIALIZER, NULL, NULL, 0, 0, 0};
 
 static void *read_stdin(void *arg);
 static void *write_stdout(void *arg);
diff --git a/src/daemon/settings_private.h b/src/daemon/settings_private.h
index 69ed25a..f345a74 100644
--- a/src/daemon/settings_private.h
+++ b/src/daemon/settings_private.h
@@ -22,7 +22,9 @@
 #ifndef USURPATION_SETTINGS_PRIVATE_H
 #define USURPATION_SETTINGS_PRIVATE_H
 
-#ifndef USURP_VERBOSITY
+#if DEBUG_BUILD > 0
+	#define USURP_VERBOSITY INFO
+#else
 	#define USURP_VERBOSITY ERR
 #endif
 
diff --git a/src/device/device_network.cpp b/src/device/device_network.cpp
index e526519..bd8467b 100644
--- a/src/device/device_network.cpp
+++ b/src/device/device_network.cpp
@@ -63,13 +63,17 @@ int udp_flush(void)
 size_t udp_get_data(char *buf, size_t size)
 {
 	size_t ret;
+
+	state.udp.parsePacket();
+
 	if ((ret = state.udp.available())) {
 		state.udp.read(buf, size);
 	}
+
 	return ret;
 }
 
-void discover_client(const int port)
+IPAddress *discover_client(const int port)
 {
 	IPAddress bcastip(255, 255, 255, 255);
 	char buffer[128] = {0};
@@ -92,20 +96,19 @@ void discover_client(const int port)
 		delay(5);
 
 		expected_s = sizeof(field.head) + sizeof(heartbeat_server);
-		while (state.udp.parsePacket()) {
-			if (state.udp.available() >= expected_s) {
-				state.udp.read(buffer, sizeof(buffer));
-				tlv_get(buffer, &field, NULL);
-				if (field.head.type == HEARTBEAT && !(strcmp(heartbeat_server, field.data))) {
-					state.daemon_ip = state.udp.remoteIP();
-					++state.acquired;
-				}
+		if (udp_get_data(buffer, sizeof(buffer)) >= expected_s) {
+			tlv_get(buffer, &field, NULL);
+			if (field.head.type == HEARTBEAT && !(strcmp(heartbeat_server, field.data))) {
+				state.daemon_ip = state.udp.remoteIP();
+				++state.acquired;
 			}
 		}
 		delay(95);
 	} while (!state.acquired);
 
 	tlv_packet_destroy(&pack);
+
+	return &state.daemon_ip;
 }
 
 IPAddress *get_daemon_address(void)
diff --git a/src/device/device_network.h b/src/device/device_network.h
index ae719f0..dbe77c2 100644
--- a/src/device/device_network.h
+++ b/src/device/device_network.h
@@ -32,7 +32,6 @@ void udp_init_packet(const int port);
 void udp_push(const void * const data, const size_t size);
 int udp_flush(void);
 size_t udp_get_data(char *buf, size_t size);
-void discover_client(const int port);
-IPAddress *get_daemon_address(void);
+IPAddress *discover_client(const int port);
 
 #endif /* DEVICE_UDP_H */
diff --git a/src/device/main.ino b/src/device/main.ino
index fc307f9..49cb0e7 100644
--- a/src/device/main.ino
+++ b/src/device/main.ino
@@ -31,9 +31,16 @@
 #include "utils.h"
 #include "tlv.h"
 
+#define seconds(a) (((a) * 1000) / (period))
+
 static const unsigned int internal_led = 2;
 static unsigned int led_state = 0;
-static char *fugg = "O Fugg, ids an errror :DDDDD";
+static char fugg[] = "error :3ccc";
+static const char devstr[] = "Device IP:";
+static const char daemonstr[] = "Daemon IP:";
+static char ipstr[] = "                                ";
+static unsigned int period = 1000;
+
 SSD1306Wire display(0x3c, 4, 5, GEOMETRY_128_32);
 
 static void init_OLED(void);
@@ -47,9 +54,9 @@ static struct progstate_t {
 	struct display_status ds;
 	struct tlv field;
 	struct tlv_packet heartbeat;
-	size_t bytes_read;
 	char buf[MTU];
 	char hbcounter;
+	IPAddress *daemon_ip;
 } progstate;
 
 void setup(void)
@@ -61,93 +68,84 @@ void setup(void)
 	pinMode(internal_led, OUTPUT);
 	toggle_led(internal_led);
 	init_OLED();
+
 	display.fillCircle(32, 16, 12);
 	display.display();
+
 	wifi_connect(ssid, password, 1, internal_led);
 	udp_init(com_port);
+
 	display.fillCircle(64, 16, 12);
 	display.display();
-	discover_client(com_port);
+
+	progstate.daemon_ip = discover_client(com_port);
+
 	display.fillCircle(92, 16, 12);
 	display.display();
-	progstate.ip_print_count = 5;
+
 	tlv_init(&field, HEARTBEAT);
 	field.data = strdup(heartbeat_device);
 	field.head.size = sizeof(heartbeat_device);
+
 	tlv_packet_init(&progstate.heartbeat);
 	tlv_pack(&progstate.heartbeat, &field);
 	tlv_packet_finalize(&progstate.heartbeat);
 	tlv_destroy(&field);
-	display_status_init(&display, &progstate.ds, fugg);
+
+	progstate.ip_print_count = 5;
 }
 
 void loop(void)
 {
-	static const String devstr = "Device IP:";
-	static const String daemonstr = "Daemon IP:";
-	static String prefix;
-	static IPAddress ip_to_print;
-	static IPAddress *daemon_ip = NULL;
-#if 0
-	char *saveptr;
-#endif
-
-	static unsigned int delta = 2000; /* sleep length to use (ms) */
-
-	delay(delta);
+	static int displayed = END_OF_MESSAGE;
+	String current_ip;
+
+	delay(period);
+
 	if (!progstate.hbcounter) {
 		udp_init_packet(com_port);
 		udp_push(progstate.heartbeat.data, progstate.heartbeat.cursor + 1);
 		udp_flush();
-		progstate.hbcounter = 5;
+		progstate.hbcounter = seconds(30);
 	}
 
-	if (progstate.ip_print_count) {
-		/* Initial display of ip's. */
-		if (!daemon_ip) {
-			daemon_ip = get_daemon_address();
-		}
-
-		prefix = (progstate.ip_print_count % 2) ? devstr : daemonstr;
-		ip_to_print = (progstate.ip_print_count % 2) ? WiFi.localIP() : *daemon_ip;
-		display.clear();
-		display.drawString(0, 0, prefix);
-		display.drawString(0, 16, ip_to_print.toString());
-		display.display();
-		progstate.ip_print_count--;
-	} else {
-		progstate.bytes_read = udp_get_data(progstate.buf, sizeof(progstate.buf));
-		if (progstate.bytes_read > 0) {
+	/* Initial display of ips. */
+	if (!progstate.ip_print_count) {
+		if (udp_get_data(progstate.buf, sizeof(progstate.buf))) {
 			tlv_get(progstate.buf, &progstate.field, NULL);
 			handle_tlv(&progstate.field);
-#if 0
-			/* Dealing with tlv's one at a time. */
-			saveptr = progstate.buf;
-			while (!(tlv_get(saveptr, &progstate.field, &saveptr))) {
-				handle_tlv(&progstate.field);
-			}
-#endif
 		}
-
-		display_update_scroll(&progstate.ds);
+	} else if (displayed == END_OF_MESSAGE) {
+		current_ip = (progstate.ip_print_count % 2) ? WiFi.localIP().toString() : progstate.daemon_ip->toString();
+		memcpy(ipstr, (progstate.ip_print_count % 2) ? devstr : daemonstr, strlen(devstr));
+		memcpy(ipstr + 16, current_ip.c_str(), strlen(current_ip.c_str()));
+		display_status_init(&display, &progstate.ds, ipstr, strlen(ipstr), seconds(1));
+		progstate.ip_print_count--;
 	}
 
+	displayed = display_update_scroll(&progstate.ds);
 	--progstate.hbcounter;
 }
 
 void handle_tlv(const struct tlv *in)
 {
-	static char *fugg = "O Fugg, ids an errror :DDDDD";
 	/* Currently just dealing with text.
 	 * */
 	switch (in->head.type) {
 	case TEXT:
-		display_status_init(&display, &progstate.ds, in->data);
+		if (in->head.size) {
+			display_status_init(&display, &progstate.ds, in->data, in->head.size, seconds(3));
+		}
+	case INVALID:
+	case HEARTBEAT:
 		break;
 	default:
-		display_status_init(&display, &progstate.ds, fugg);
+		display_status_init(&display, &progstate.ds, fugg, sizeof(fugg), seconds(5));
 		break;
 	}
+
+	/* clear the tlv after handling it */
+	progstate.field = tlv_none;
 }
 
 static void init_OLED(void)
diff --git a/src/device/screen.cpp b/src/device/screen.cpp
index cf1428a..bc25d65 100644
--- a/src/device/screen.cpp
+++ b/src/device/screen.cpp
@@ -1,83 +1,104 @@
-#include <time.h>
 #include <stdlib.h>
 #include <string.h>
 #include <OLEDDisplay.h>
 #include <Wire.h>
 #include "screen.h"
 
-void draw_lines(OLEDDisplay *screen, struct display_status *status);
-void update_lines(struct display_status *status);
-void init_msg(char *msg, size_t size);
+static void draw_lines(OLEDDisplay *screen, struct display_status *status);
+static void update_lines(struct display_status *status);
+static void sanitize_msg(char *msg);
 
-static char empty_string[] = "";
-
-void display_status_init(OLEDDisplay *screen, struct display_status *status, char *msg)
+void display_status_init(OLEDDisplay *screen, struct display_status *status, char *msg, size_t size, unsigned long int period)
 {
-	status->delta = 2000;	/* Currently default */
+	status->period = period;
 	status->screen = screen;
-	init_msg(msg, strlen(msg));
-	status->message = msg;
-	status->line_cursor = 0;
-	status->last_scroll_time = time(NULL);
+	size = MESSAGE_MAX < size ? MESSAGE_MAX : size;
+	memset(status->message, 0, size + 1);
+	memcpy(status->message, msg, size);
+	status->msg_len = size;
+	sanitize_msg(status->message);
+	status->cursor = status->message;
+	status->remaining = status->msg_len;
 	update_lines(status);
+	draw_lines(status->screen, status);
+	status->last_scroll_time = millis();
+}
+
+int display_update_scroll(struct display_status *status)
+{
+	unsigned long crr_time;
+	int ret = END_OF_MESSAGE;
+
+	if (status->remaining) {
+		crr_time = millis();
+		/* Only scroll lines once a period, because --- duh! */
+		if ((crr_time - status->last_scroll_time) > status->period) {
+			status->last_scroll_time = crr_time;
+			update_lines(status);
+			draw_lines(status->screen, status);
+		}
+		ret = 0;
+	}
+
+	return ret;
 }
 
 /**
- * Turns all whitespace into literal spaces to save screen real-estate and
- * possible misinterpretation.
+ * Turns all whitespace and special characters into literal spaces to save
+ * screen real-estate and possible misinterpretation.
  */
-void init_msg(char *msg, size_t size)
+static void sanitize_msg(char *msg)
 {
-	size_t i;
-
-	for (i = 0; i < size; i++) {
-		switch (msg[i]) {
+	do {
+		switch (*msg) {
+		case '\f':
 		case '\n':
-		case '\t':
 		case '\r':
-			msg[i] = ' ';
+		case '\t':
+		case '\v':
+			*msg = ' ';
 			break;
-		case '\0':
-			goto end;
 		default:
+			if (*msg < 0x20 || *msg > 0x7f) {
+				*msg = '?';
+			}
 			break;
 		}
-	}
+	} while (*(++msg));
 end:
 	return;
 }
 
-int display_update_scroll(struct display_status *status)
+static void draw_lines(OLEDDisplay *screen, struct display_status *status)
 {
-	unsigned long crr_time = millis();
-	/* Only scroll lines once a delta, because --- duh! */
-	if (status->last_scroll_time - crr_time > status->delta) {
-		status->last_scroll_time += status->delta;
-		status->line_cursor++;
-		update_lines(status);
-		draw_lines(status->screen, status);
-	}
-	if (!(*status->first_line) && !(*(status->second_line))) {
-		return END_OF_MESSAGE;
-	} else {
-		return 0;
+	size_t i;
+	screen->clear();
+
+	for (i = 0; i < SCREEN_LINE_COUNT; ++i) {
+		screen->drawString(0, i * FONT_HEIGHT, status->lines[i]);
 	}
-}
 
-void draw_lines(OLEDDisplay *screen, struct display_status *status)
-{
-	screen->clear();
-	screen->drawString(0, 0, status->first_line);
-	screen->drawString(0, SCREEN_HEIGHT / 2, status->second_line);
 	screen->display();
 }
 
-void update_lines(struct display_status *status)
+static void update_lines(struct display_status *status)
 {
-	status->first_line = (status->line_cursor * SCREEN_MAX_CHARS < status->message_len)
-			? status->message + status->line_cursor * SCREEN_MAX_CHARS
-			: empty_string;
-	status->second_line = (status->line_cursor * SCREEN_MAX_CHARS < status->message_len)
-			? status->message + (status->line_cursor + 1) * SCREEN_MAX_CHARS
-			: empty_string;
+	size_t i;
+
+	if (status->remaining) {
+
+		memset(status->lines, 0, sizeof(status->lines));
+
+		for (i = 0; i < SCREEN_LINE_COUNT; ++i) {
+			if (status->remaining > SCREEN_LINE_CHARS) {
+				memcpy(status->lines[i], status->cursor, SCREEN_LINE_CHARS);
+				status->cursor += SCREEN_LINE_CHARS;
+				status->remaining -= SCREEN_LINE_CHARS;
+			} else if (status->remaining) {
+				memcpy(status->lines[i], status->cursor, status->remaining);
+				status->cursor += status->remaining;
+				status->remaining = 0;
+			}
+		}
+	}
 }
diff --git a/src/device/screen.h b/src/device/screen.h
index 5d0e3b3..3efff6d 100644
--- a/src/device/screen.h
+++ b/src/device/screen.h
@@ -13,20 +13,24 @@
 #define SCREEN_WIDTH		(128)
 #define SCREEN_HEIGHT		(32)
 #define FONT_WIDTH		(8)
-#define SCREEN_MAX_CHARS	(SCREEN_WIDTH / FONT_WIDTH)
+#define FONT_HEIGHT		(16)
+#define SCREEN_LINE_COUNT	(SCREEN_HEIGHT / FONT_HEIGHT)
+#define SCREEN_LINE_CHARS	(SCREEN_WIDTH / FONT_WIDTH)
+#define SCREEN_TOTAL_CHARS	(SCREEN_LINE_COUNT * SCREEN_LINE_CHARS)
+#define MESSAGE_MAX		(1023)
 
 /**
  * Struct that keeps track of the lines on the screen.
  */
 struct display_status {
-	OLEDDisplay *screen;		/* Screen to draw on. */
-	time_t delta;			/* Seconds/Line */
-	time_t last_scroll_time;	/* Last second the line was scrolled */
-	char *message;			/* Entire message to be shown */
-	char *first_line;		/* First line on display */
-	char *second_line;		/* Second line on display */
-	size_t message_len;		/* Length of the message */
-	size_t line_cursor;		/* Index of the first line being displayed. */
+	OLEDDisplay *screen;
+	unsigned long period;		/* milliseconds/Line */
+	unsigned long last_scroll_time;
+	char message[MESSAGE_MAX + 1];	/* Entire message to be shown */
+	char lines[SCREEN_LINE_COUNT][SCREEN_LINE_CHARS  + 1];
+	char *cursor;			/* the begining of the *next* location in the message to be shown*/
+	size_t msg_len;
+	size_t remaining;		/* offset of the next part of text to be shown */
 };
 
 /**
@@ -44,7 +48,11 @@ int display_update_scroll(struct display_status *status);
  * status	- structure to Initialise
  *
  * msg		- message to scroll on the screen
+ *
+ * size		- length of the message
+ *
+ * period	- time between [elder] scrolls
  */
-void display_status_init(OLEDDisplay *screen, struct display_status *status, char *msg);
+void display_status_init(OLEDDisplay *screen, struct display_status *status, char *msg, size_t size, unsigned long int period);
 
-#endif /* DEVICE_SCREEN_H */
\ No newline at end of file
+#endif /* DEVICE_SCREEN_H */