1 files changed, 72 insertions, 0 deletions
diff --git a/fw/table.c b/fw/table.c
new file mode 100644
index 0000000..a509082
--- /dev/null
+++ b/fw/table.c
@@ -0,0 +1,72 @@
+#include <xc.h>
+
+#include <stdint.h>
+
+#include "types.h"
+#include "eeprom.h"
+
+#include "table.h"
+
+Status
+tabWrite(const Table *tab, U8 k, U16 key, U16 val) {
+	if (k >= TAB_ROWS) {
+		return FAIL;
+	}
+
+	U16 addr = addU16U8(tab->offset, k*TAB_ROW_SIZE);
+	U8 row[4u] = {key.lo, key.hi, val.lo, val.hi};
+	return eepromWrite(addr, row, sizeof(row));
+}
+
+Status
+tabRead(const Table *tab, U8 k, U16 *key, U16 *val) {
+	U16 addr;
+	U8 row[4u];
+	Status status;
+
+	if (k >= TAB_ROWS) {
+		return FAIL;
+	}
+
+	addr = addU16U8(tab->offset, k*TAB_ROW_SIZE);
+	status = eepromRead(addr, row, sizeof(row));
+	*key = (U16){.lo=row[0u], .hi=row[1u]};
+	*val = (U16){.lo=row[2u], .hi=row[3u]};
+	return status;
+}
+
+Status
+tabLookup(const Table *tab, U16 key, U16 *val) {
+	U8 k;
+	U16 tkey, tval1, tval2;
+	Status status;
+	I8 ord;
+
+	// Search for key
+	for (k = 0u; (k < TAB_ROWS-1u); k++) {
+		status = tabRead(tab, k, &tkey, &tval1);
+		if (status != OK) {
+			return FAIL;
+		}
+		ord = cmpU16(key, tkey);
+		if (ord == 0u) { // found exact key
+			*val = tval1; 
+			return OK;
+		} else if (ord > 0u) { // interpolate
+			status = tabRead(tab, k+1u, &tkey, &tval2);
+			if (status != OK) {
+				return FAIL;
+			}
+			// Mean: (tval1+tval2)/2
+			*val = rshiftU16(addU16(tval1, tval2), 1u); 
+			return OK;
+		} else { // less
+			// continue
+		}
+	}
+
+	// Reached last row
+	*val = tval1; // last value in table
+
+	return OK;
+}