10 files changed, 282 insertions, 141 deletions

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')