Merge branch '35-Message-Output'

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

13 files changed, 480 insertions, 279 deletions

diff --git a/include/meson.build b/include/meson.build
index 33e9426..77a8d53 100644
--- a/include/meson.build
+++ b/include/meson.build
@@ -1,7 +1,7 @@
 header_filenames = [
 	'utils.h',
 	'net.h',
-	'protocol.h'
+	'tlv.h'
 ]
 
 fw_headers = files(header_filenames)
diff --git a/include/protocol.h b/include/protocol.h
deleted file mode 100644
index 34ad4a3..0000000
--- a/include/protocol.h
+++ /dev/null
@@ -1,137 +0,0 @@
-/*
- * Usurpataion --- client-server protocol interface.
- *
- * Copyright (C) 2019 Ramūnas Mažeikis
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; version 2.1
- * of the License.
- *
- * 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
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA
- */
-
-#ifndef USURPATION_PROTOCOL_H_INCLUDED
-#define USURPATION_PROTOCOL_H_INCLUDED
-
-#include <errno.h>
-
-#define	E_TLV_OVERFLOW		(1 << 0)
-#define E_UNKNOWN_TYPE		(1 << 1)
-#define E_IVALID_DESCRIPTOR	(1 << 2)
-#define END_OF_PACKET		(1 << 3)
-
-/**
- * Regular packets contain tlv's defined by tlv_type.
- *
- * Hearbeat packet tell daemon that device is still alive and listening.
- *
- * Discovery packets are used for what they say.
- */
-enum packet_type {
-	REGURAL,
-	HEARTBEAT,
-	DISCOVERY
-};
-
-/**
- * Message sequence number since beggining of sesssion.
- *
- * Mainly used for identifying lost messages.
- */
-typedef unsigned int msg_idx_t;
-
-enum tlv_type {
-	/**
-	 * NULL-terminated string. To be put in a queue to display on the
-	 * screen.
-	 */
-	TEXT,
-
-	/** Fixed point. 1 decimal digit of precision. */
-	FPI1,
-
-	/** Literally time_t */
-	TIMESTAMP,
-
-	/** Represents a request for lost message. Data is unsigned integer
-	 * that uniquely identifies the message.
-	 */
-	REQUEST,
-
-	/**
-	 * Response to request. Begins with unsigned integer that represents
-	 * which message is being repeated and the actual null-terminated
-	 * message after that.
-	 */
-	REPLY,
-
-	/**
-	 * UUID that represents a particular device.
-	 */
-	UUID
-};
-
-/**
- * Packet data itself is a special type of tlv. A packet is either regular,
- * hearbeat or discovery.
- *
- * May be used to send data.
- */
-struct tlv_packet {
-	enum packet_type type;
-	size_t size;
-	size_t offset;
-	char *data;	/* Bytes representing tlv's */
-};
-
-/**
- * Literally type-length-value
- * */
-struct tlv {
-	enum tlv_type type;
-	size_t length;
-	void *data;
-};
-
-struct tlv_parser {
-	char *data;
-	size_t offset;
-	size_t size;
-};
-
-int get_tlv(struct tlv_parser *parser, struct tlv *ret);
-
-/**
- * Appends data to the next packet to be sent. Type of data is determined by
- * enum tlv_type.
- *
- * In case of overflow return E_TLV_OVERFLOW.
- *
- * On next call after retreiving last packet returns END_OF_PACKET.
- *
- * Overflow can be detected after forming tlv header. This means that the
- * packet may have changes.
- * */
-int push_data(struct tlv_packet *packet, enum tlv_type type, char *data);
-
-
-/**
- * Resets offset to 0 and set entire buffer to 0.
- */
-void clear_data(struct tlv_packet *packet);
-
-
-/**
- * Tells what size of buffer is needed for next tlv.
- */
-size_t tlv_data_size(struct tlv_parser *parser);
-
-#endif /* USURPATION_PROTOCOL_H_INCLUDED */
diff --git a/include/tlv.h b/include/tlv.h
new file mode 100644
index 0000000..b52763a
--- /dev/null
+++ b/include/tlv.h
@@ -0,0 +1,197 @@
+/*
+ * Usurpataion --- client-server protocol interface.
+ *
+ * Copyright (C) 2019 Ramūnas Mažeikis
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; version 2.1
+ * of the License.
+ *
+ * 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
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA
+ */
+
+#ifndef USURPATION_PROTOCOL_H_INCLUDED
+#define USURPATION_PROTOCOL_H_INCLUDED
+
+#if 0
+
+How do we use this API(?), I hear you say. Well, here is a self-contained
+example complete with tedious comments, allocation of buffers and cleanup:
+
+-------------------------------------------------------------------------------
+  1 |	void functy_the_function(void)
+  2 |	{
+  3 |		struct tlv tlv_packet;				/* A packet made of tlv's */
+  4 |		struct tlv text;				/* Actually useful data */
+  5 |		char *packet;					/* Raw bytes to be pushed via UDP */
+  6 |		size_t packet_size;				/* Raw packet size */
+  7 |		char message[] = "Get jacked!";			/* Duh! */
+  9 |
+ 10 |		tlv_init(&tlv_packet, REGURAL);			/* Initialise tlv_packet to regular */
+ 11 |		tlv_init(&text, TEXT);				/* Initialise text packet */
+ 12 |		tlv_push_data(&text, message, sizeof(message));	/* Push text to appropriate tlv */
+ 13 |		tlv_push_tlv(&tlv_packet, &text);		/* Push the tlv into tlv packet */
+ 14 |
+ 15 |		packet_size = tlv_raw_size(&tlv_packet);	/* Get raw size required to push entire tlv_packet as bytes */
+ 16 |		packet = (char *)malloc(packet_size);		/* Allocate buffer for outgoing packet */
+ 17 |		tlv_get_raw(&tlv_packet, packet);		/* Flash tlv_packet to buffer */
+ 19 |		udp_push(packet, packet_size);			/* Push packet via UDP */
+ 20 |		udp_flush();					/* Flush to the other side*/
+ 21 |
+ 22 |		tlv_destroy(&tlv_packet);			/* Free tlv_packet */
+ 23 |		free(packet);					/* Free packet itself */
+ 24 |		return;						/* GTFO and save stack space */
+ 25 |	}
+-------------------------------------------------------------------------------
+
+A few things to note:
+	* Calling tlv_destroy() on a tlv of type REGURAL or HEARTBEAT destorys
+	  all sub-tlv's too.
+	* Tlv's get copies of data, which means the original buffer can be
+	  freed immediately.
+
+#endif
+
+
+#include <errno.h>
+
+#define E_TLV_OVERFLOW		(1 << 0)
+#define E_UNKNOWN_TYPE		(1 << 1)
+#define END_OF_PACKET		(1 << 2)
+
+#define TLV_SZ_MAX_RAW		(MTU - 64)
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+/**
+ * Message sequence number since beggining of sesssion.
+ *
+ * Mainly used for identifying lost messages.
+ */
+typedef unsigned int msg_idx_t;
+
+enum tlv_type {
+	/**
+	 * NULL-terminated string. To be put in a queue to display on the
+	 * screen.
+	 */
+	TEXT,
+
+	/** Fixed point. 1 decimal digit of precision. */
+	FPI1,
+
+	/** Literally time_t */
+	TIMESTAMP,
+
+	/** Represents a request for lost message. Data is unsigned integer
+	 * that uniquely identifies the message.
+	 */
+	REQUEST,
+
+	/**
+	 * Response to request. Begins with unsigned integer that represents
+	 * which message is being repeated and the actual null-terminated
+	 * message after that.
+	 */
+	REPLY,
+
+	/**
+	 * UUID that represents a particular device.
+	 */
+	UUID,
+
+/* Data of the following types are other tlv's! */
+
+	/**
+	 * Just a TLV container.
+	 */
+	REGURAL,
+
+	/**
+	 * Says "I'm not dead yet.".
+	 */
+	HEARTBEAT,
+
+	/**
+	 * Says "Bring out yer dead!".
+	 */
+	DISCOVERY
+};
+
+/**
+ * Literally type-length-value
+ * */
+struct tlv {
+	enum tlv_type type;
+	size_t length;
+	void *data;
+};
+
+/**
+ * Keeps state of the parsing process.
+ *
+ * Related functions return one tlv at a time.
+ */
+struct tlv_parser {
+	char *data;
+	size_t offset;
+	size_t size;
+};
+
+/**
+ * Fills tlv structure to represent the next tlv in the packet.
+ *
+ * Returns END_OF_PACKET if all tlv's were read of E_TLV_OVERFLOW, if the last
+ * tlv, according to its declared size should not fit in a packet.
+ */
+int tlv_get(struct tlv_parser *parser, struct tlv *ret);
+
+/**
+ * Initialises tlv to sane values.
+ */
+void tlv_init(struct tlv *t, enum tlv_type type);
+
+/**
+ * Frees data held in the tlv structure. The structure itself shall be freed as
+ * needed by calling code.
+ */
+void tlv_destroy(struct tlv *t);
+
+/**
+ * Tells amount of bytes needed to push tlv to a buffer
+ */
+size_t tlv_raw_size(const struct tlv *t);
+
+/**
+ * Pushes tlv to buffer as contiguous data. Check tlv size with tlv_raw_size
+ * beforehand. If you don't do that and overflow --- tough tiddy.
+ */
+int tlv_get_raw(struct tlv *t, char *buf);
+
+/**
+ * Pushes data to tlv. Returns E_TLV_OVERFLOW if pushing data would cause the
+ * final size to be greater than TLV_SZ_MAX_RAW. In case of such error the data is left
+ * untouched.
+ */
+int tlv_push_data(struct tlv *t, const char *data, size_t size);
+
+/**
+ * Pushes a sub-tlv into the packet. 't' can only be REGURAL, HEARTBEAT or
+ * DISCOVERY.
+ */
+int tlv_push_tlv(struct tlv *t, const struct tlv *other);
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* USURPATION_PROTOCOL_H_INCLUDED */
diff --git a/src/common/meson.build b/src/common/meson.build
index b0f0c82..da8b656 100644
--- a/src/common/meson.build
+++ b/src/common/meson.build
@@ -1,4 +1,5 @@
 common_filenames = [
+	'tlv.c'
 ]
 
 common_sources = files(common_filenames)
diff --git a/src/common/protocol_private.h b/src/common/protocol_private.h
deleted file mode 100644
index 51d5431..0000000
--- a/src/common/protocol_private.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*
- * Usurpataion --- clinet-server protocol implementation.
- *
- * Copyright (C) 2019 Ramūnas Mažeikis
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; version 2.1
- * of the License.
- *
- * 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
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA
- */
-
-/**
- * Common parts of protocol implementation. Handling of anything that actually
- * deals with connection descriptor has to be implemented by device and daemon
- * separately.
- */
-
-#ifndef PROTOCOL_PRIVATE_H
-#define PROTOCOL_PRIVATE_H
-
-#include "protocol_private.h"
-
-/**
- * Convenience function that pushes bytes to the end of a packet and reports
- * potential overflow.
- *
- * In case of detected overflow nothing is done to the packet.
- */
-int push_bytes(struct tlv_packet *packet, char *data, size_t size);
-
-/**
- * Convenience function that forms a tlv header at the end of a packet. Reports
- * potential overflow.
- *
- * In case of detected overflow nothing is done to the packet.
- */
-int push_tlv_header(struct tlv_packet *packet, enum tlv_type type, size_t size);
-
-#endif /* PROTOCOL_PRIVATE_H */
diff --git a/src/common/protocol.c b/src/common/tlv.c
index b2e0d40..c25f008 100644
--- a/src/common/protocol.c
+++ b/src/common/tlv.c
@@ -27,107 +27,106 @@
 #include <stdlib.h>
 #include <string.h>
 #include <time.h>
-#include "protocol.h"
-#include "protocol_private.h"
+#include "tlv.h"
 #include "net.h"
 #include "utils.h"
 
-int push_tlv(struct tlv_packet *packet, enum tlv_type type, char *data)
+int tlv_get(struct tlv_parser *parser, struct tlv *ret)
 {
-	int ret = 0;
-	size_t size;
+	int retval = 0;
 
-	switch (type) {
-	case TEXT:
-		size = strlen(data) + 1;
-		break;
-	case FPI1:
-		size = sizeof(fpi1_t);
-		break;
-	case TIMESTAMP:
-		size = sizeof(time_t);
-		break;
-	case REQUEST:
-		size = sizeof(msg_idx_t);
-		break;
-	case REPLY:
-		size = sizeof(msg_idx_t) + strlen(data + sizeof(msg_idx_t));
-		break;
-	case UUID:
-		size = sizeof(uuid_t);
-		break;
-	default:
-		ret = E_UNKNOWN_TYPE;
+	if (parser->offset + sizeof(ret->type) + sizeof(ret->length) >= parser->size) {
+		retval = E_TLV_OVERFLOW;
+	} else if (parser -> offset == parser->size) {
+		retval = END_OF_PACKET;
+	} else {
+		memcpy(&ret->type, parser->data + parser->offset, sizeof(ret->type));
+		parser->size += sizeof(ret->type);
+		ret->length = memcpy(&ret->length, parser->data + parser->offset, sizeof(ret->length));
+		parser->offset += sizeof(ret->length);
+		if (parser->offset + ret->length >= parser->size) {
+			retval = E_TLV_OVERFLOW;
+		} else {
+			memcpy(ret->data, parser->data, ret->length);
+		}
 	}
-	ret |= push_tlv_header(packet, type, size);
-	ret |= push_bytes(packet, data, size);
-	return ret;
-}
-
-void clear_data(struct tlv_packet *packet)
-{
-	packet->offset = 0;
-	packet->type = 0;
-	memset(packet->data, 0, packet->size);
+	return retval;
 }
 
-static int push_bytes(struct tlv_packet *packet, char *data, size_t size)
+size_t tlv_data_size(struct tlv_parser *parser)
 {
-	int ret = 0;
+	size_t size;
 
-	if (packet->offset + size >= packet->size) {
-		ret = E_TLV_OVERFLOW;
+	if (parser->offset + sizeof(enum tlv_type) + sizeof(size_t) >= parser->size) {
+		size = 0;
 	} else {
-		memcpy(packet->data + packet->offset, data, size);
-		packet->offset += size;
+		memcpy(&size, parser->data + parser->offset + sizeof(enum tlv_type), sizeof(size_t));
 	}
+	return size;
 }
 
-static int push_tlv_header(struct tlv_packet *packet, enum tlv_type type, size_t size)
+void tlv_destroy(struct tlv *t)
 {
-	int ret = 0;
-
-	if (packet->offset + sizeof(type) + sizeof(size) >= packet->size) {
-		ret = E_TLV_OVERFLOW;
-	} else {
-		memcpy(packet->data + packet->size, type, sizeof(type));
-		packet->offset += sizeof(type);
-		memcpy(packet->data + packet->offset, size, sizeof(size));
-		packet->offset += sizeof(size);
+	size_t i = 0;
+	size_t tlv_count;
+	struct tlv *arr;
+	switch (t->type)
+	{
+	case REGURAL:
+	case HEARTBEAT:
+	case DISCOVERY:
+		tlv_count = t->length / sizeof(struct tlv);
+		arr = t->data;
+		for (i = 0; i < tlv_count; i++) {
+			tlv_destroy(&arr[i]);
+		}
+	default:
+		free(t->data);
+		t->length = 0;
+		break;
 	}
-	return ret;
 }
 
-int get_tlv(struct tlv_parser *parser, struct tlv *ret)
+size_t tlv_raw_size(const struct tlv *t)
 {
-	int ret = 0;
+	return sizeof(*t) + t->length;
+}
 
-	if (parser->offset + sizeof(ret->type) + sizeof(ret->length) >= parser->size) {
+int tlv_push_data(struct tlv *t, const char *data, size_t size)
+{
+	int ret = 0;
+	size_t final_size = tlv_raw_size(t) + size;
+	if (final_size > TLV_SZ_MAX_RAW) {
 		ret = E_TLV_OVERFLOW;
-	} else if (parser -> offset == parser->size) {
-		ret = END_OF_PACKET;
 	} else {
-		ret->type = memcpy(&ret->type, parser->data + parser->offset, sizeof(ret->type));
-		parser->size += sizeof(ret->type);
-		ret->length = memcpy(&ret->length, parser->data + parser->offset, sizeof(ret->length));
-		parser->offset += sizeof(ret->length);
-		if (parser->offset + ret->length >= parser->size) {
-			ret = E_TLV_OVERFLOW;
-		} else {
-			memcpy(ret->data, parser->data, ret->length);
-		}
+		t->data = realloc(t->data, final_size);
+		memcpy(t->data + t->length, data, size);
+		t->length = final_size;
 	}
 	return ret;
 }
 
-size_t tlv_data_size(struct tlv_parser *parser)
+void tlv_init(struct tlv *t, enum tlv_type type)
 {
-	size_t size;
+	t->type = type;
+	t->length = 0;
+	t->data = NULL;
+}
 
-	if (parser->offset + sizeof(enum tlv_type) + sizeof(size_t) >= parser->size) {
-		size = 0;
+int tlv_push_tlv(struct tlv *t, const struct tlv *other)
+{
+	int ret = 0;
+	size_t other_size;
+	size_t final_size;
+
+	other_size = tlv_raw_size(other);
+	final_size = tlv_raw_size(t) + other_size;
+	if (final_size > TLV_SZ_MAX_RAW) {
+		ret = E_TLV_OVERFLOW;
 	} else {
-		memcpy(&size, parser->data + parser->offset + sizeof(enum tlv_type), sizeof(size_t));
+		tlv_get_raw(other, t->data + t->length);
+		t->length = final_size;
 	}
-	return size;
+
+	return ret;
 }
diff --git a/src/device/device_network.cpp b/src/device/device_network.cpp
index d7781a0..f2456d9 100644
--- a/src/device/device_network.cpp
+++ b/src/device/device_network.cpp
@@ -26,7 +26,7 @@
 
 static struct netstate {
 	WiFiUDP udp;
-	char udppacketbuffer[1500];
+	char udppacketbuffer[MTU];
 	char *udppacketcursor;
 	IPAddress daemon_ip;
 	bool acquired;
@@ -59,6 +59,17 @@ int udp_flush(void)
 	return state.udp.endPacket();
 }
 
+size_t udp_get_data(char *buf, size_t size)
+{
+	size_t ret;
+	if (state.udp.available() != 0) {
+		ret = state.udp.read(buf, size);
+	} else {
+		ret = 0;
+	}
+	return ret;
+}
+
 void discover_client(const int port)
 {
 	IPAddress bcastip(255, 255, 255, 255);
@@ -100,4 +111,3 @@ static void udp_init_packet_expaddr(IPAddress ip, const int port)
 	memset(state.udppacketbuffer, 0, sizeof(state.udppacketbuffer));
 	state.udppacketcursor = state.udppacketbuffer;
 }
-
diff --git a/src/device/device_network.h b/src/device/device_network.h
index d8f41a1..92af429 100755..100644
--- a/src/device/device_network.h
+++ b/src/device/device_network.h
@@ -31,6 +31,7 @@ void udp_init(const int port);
 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);
 
diff --git a/src/device/main.ino b/src/device/main.ino
index 66e52e8..24cfb9c 100644
--- a/src/device/main.ino
+++ b/src/device/main.ino
@@ -27,6 +27,9 @@
 #include "SSD1306Wire.h"
 #include "DejaVu_Sans_Mono_13.h"
 #include "device_network.h"
+#include "screen.h"
+#include "net.h"
+#include "tlv.h"
 
 static const unsigned int internal_led = 2;
 static unsigned int led_state = 0;
@@ -36,6 +39,16 @@ static void init_OLED(void);
 unsigned int toggle_led(const int pin);
 static int wifi_connect(const char * const ssid, const char * const password, const char doblink, const int ledpin);
 static void blink_led(const int pin, const int ontime, const int offtime);
+void handle_tlv(const struct tlv *data);
+
+static struct progstate_t {
+	int ip_print_count = 5;
+	struct display_status ds = {0};
+	struct tlv_parser parser = {0};
+	struct tlv crr_data;
+	size_t bytes_read = 0;
+	char in_packet_buf[MTU];
+} progstate;
 
 void setup(void)
 {
@@ -64,27 +77,55 @@ void loop(void)
 	static String prefix;
 	static IPAddress ip_to_print;
 	static IPAddress *daemon_ip = NULL;
-	static int print_dev_ip = 0;
 
 	static unsigned int delta = 2000; /* sleep length to use (ms) */
-	/* static int dot_idx = 0; */
 
 	delay(delta);
-	udp_init_packet(com_port);
-	udp_push(clientmagic, sizeof(clientmagic));
-	udp_flush();
+	/* Initial display of ip's. */
+	if (progstate.ip_print_count > 0) {
+		udp_init_packet(com_port);
+		udp_push(clientmagic, sizeof(clientmagic));
+		udp_flush();
+
+		if (!daemon_ip) {
+			daemon_ip = get_daemon_address();
+		}
 
-	if (!daemon_ip) {
-		daemon_ip = get_daemon_address();
+		prefix = (progstate.ip_print_count % 2) ? devstr : daemonstr;
+		ip_to_print = (progstate.ip_print_count) ? 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 {	/* Dealing with tlv's one at a time. */
+		progstate.bytes_read = udp_get_data(progstate.in_packet_buf, sizeof(progstate.in_packet_buf));
+		if (progstate.bytes_read > 0) {
+			progstate.parser.data = progstate.in_packet_buf;
+			progstate.parser.offset = 0;
+			progstate.parser.size = progstate.bytes_read;
+			/* Ignore errors for now. */
+			while (tlv_get(&progstate.parser, &progstate.crr_data) == 0) {
+				handle_tlv(&progstate.crr_data);
+			}
+		}
+		display_update_scroll(&progstate.ds);
 	}
+}
 
-	prefix = (print_dev_ip) ? devstr : daemonstr;
-	ip_to_print = (print_dev_ip) ? WiFi.localIP() : *daemon_ip;
-	display.clear();
-	display.drawString(0, 0, prefix);
-	display.drawString(0, 16, ip_to_print.toString());
-	display.display();
-	print_dev_ip = !print_dev_ip;
+void handle_tlv(const struct tlv *t)
+{
+	/* Currently just dealing with text.
+	 * */
+	switch (t->type) {
+	case TEXT:
+		display_status_init(&display, &progstate.ds, (char *)t->data);
+		break;
+	default:
+		display.clear();
+		display.drawString(0, 0, "Fugg :DDD");
+		break;
+	}
 }
 
 static void init_OLED(void)
diff --git a/src/device/meson.build b/src/device/meson.build
index 625bd2d..7a0665c 100644
--- a/src/device/meson.build
+++ b/src/device/meson.build
@@ -35,10 +35,14 @@ if get_option('fwbuild')
 		cpus = '2'
 	endif
 
-	fw_filenames = ['main.ino',
+	fw_filenames = [
+			'main.ino',
 			'DejaVu_Sans_Mono_13.h',
 			'device_network.cpp',
-			'device_network.h']
+			'device_network.h',
+			'screen.cpp',
+			'screen.h'
+			]
 	fw_true_sources += files(fw_filenames)
 	fw_filenames += oledlibnames
 	fw_true_sources += oledlib
diff --git a/src/device/screen.cpp b/src/device/screen.cpp
new file mode 100644
index 0000000..2857161
--- /dev/null
+++ b/src/device/screen.cpp
@@ -0,0 +1,83 @@
+#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);
+
+/* Effectively const. For type safety reasons. */
+static char NOTHING[] = {'\0'};
+
+void display_status_init(OLEDDisplay *screen, struct display_status *status, char *msg)
+{
+	status->delta = 2;	/* Currently default */
+	status->screen = screen;
+	init_msg(msg, strlen(msg));
+	status->message = msg;
+	status->line_cursor = 0;
+	status->last_scroll_time = time(NULL);
+	update_lines(status);
+}
+
+/**
+ * Turns all whitespace into literal spaces to save screen real-estate and
+ * possible misinterpretation.
+ */
+void init_msg(char *msg, size_t size)
+{
+	size_t i;
+
+	for (i = 0; i < size; i++) {
+		switch (msg[i]) {
+		case '\n':
+		case '\t':
+		case '\r':
+			msg[i] = ' ';
+			break;
+		case '\0':
+			goto end;
+		default:
+			break;
+		}
+	}
+end:
+	return;
+}
+
+int display_update_scroll(struct display_status *status)
+{
+	time_t crr_time = time(NULL);
+	/* 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 == NOTHING && status->second_line == NOTHING) {
+		return END_OF_MESSAGE;
+	} else {
+		return 0;
+	}
+}
+
+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);
+}
+
+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
+			: NOTHING;
+	status->second_line = (status->line_cursor * SCREEN_MAX_CHARS < status->message_len)
+			? status->message + (status->line_cursor + 1) * SCREEN_MAX_CHARS
+			: NOTHING;
+}
diff --git a/src/device/screen.h b/src/device/screen.h
new file mode 100644
index 0000000..5d0e3b3
--- /dev/null
+++ b/src/device/screen.h
@@ -0,0 +1,50 @@
+/**
+ * Simple API for scrolling lines. Keeps things simple by not even assuming
+ * which screen is being drawn on.
+ */
+
+#ifndef DEVICE_SCREEN_H
+#define DEVICE_SCREEN_H
+
+#include <time.h>
+#include <OLEDDisplay.h>
+#include <Wire.h>
+
+#define SCREEN_WIDTH		(128)
+#define SCREEN_HEIGHT		(32)
+#define FONT_WIDTH		(8)
+#define SCREEN_MAX_CHARS	(SCREEN_WIDTH / FONT_WIDTH)
+
+/**
+ * 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. */
+};
+
+/**
+ * Displays scrolling text on the screen.
+ */
+int display_update_scroll(struct display_status *status);
+
+#define END_OF_MESSAGE	(1 << 0)
+/**
+ * Initialises display_status structure so it can be used for
+ * display_update_scroll.
+ *
+ * screen	- screen to draw on
+ *
+ * status	- structure to Initialise
+ *
+ * msg		- message to scroll on the screen
+ */
+void display_status_init(OLEDDisplay *screen, struct display_status *status, char *msg);
+
+#endif /* DEVICE_SCREEN_H */
\ No newline at end of file
diff --git a/src/meson.build b/src/meson.build
index 12b583a..38a1f57 100644
--- a/src/meson.build
+++ b/src/meson.build
@@ -1,6 +1,6 @@
-#subdir('common')
+subdir('common')
 
-fw_true_sources = [fw_headers]
+fw_true_sources = [fw_headers, common_sources]
 
 subdir('daemon')
 subdir('device')