1 files changed, 93 insertions, 0 deletions
diff --git a/fw/signal.c b/fw/signal.c
new file mode 100644
index 0000000..30916e0
--- /dev/null
+++ b/fw/signal.c
@@ -0,0 +1,93 @@
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "types.h"
+#include "can.h"
+
+#include "signal.h"
+#include "signal_utestable.h"
+
+// Extract a little-endian value from a frame.
+// Assumes signal is within the frame's DATA FIELD.
+Status
+pluckLE(const SigFmt *sig, const CanFrame *frame, U32 *raw) {
+	// TODO
+}
+
+// Extract a big-endian value from a frame.
+// Assumes signal is within the frame's DATA FIELD.
+void
+pluckBE(const SigFmt *sig, const CanFrame *frame, U32 *raw) {
+	U8 i, end, mask;
+
+	*raw = 0ul;
+	end = sig->start+sig->size;
+
+	// First iteration starts at arbitrary bit: namely the (i%8)'th bit.
+	// Subsequent iterations start at bit 0 of each byte.
+
+	for (i = sig->start; i < end; i += 8u-(i%8u)) {
+		mask = 0xFF << (i%8u);
+		if (i/8u == end/8u) { // if end in this byte
+			mask >>= (8u - (end%8u)); // ignore top bits
+			*raw <<= (end%8u) - (i%8u); // include bits between i and end
+		} else {
+			*raw <<= 8u - (i%8u); // include bits between i and end of byte
+		}
+		*raw |= (frame->data[i/8u] & mask) >> (i%8u);
+	}
+}
+
+Status
+sigPluck(const SigFmt *sig, const CanFrame *frame, Number *raw) {
+	// Preconditions
+	if (
+		(sig->start >= (8u * frame->dlc)) // signal starts outside DATA FIELD
+		|| (sig->size > (8u * frame->dlc - sig->start)) // signal extends outside DATA FIELD
+		|| (sig->size < 1u) // signal is empty
+	) {
+		return FAIL;
+	}
+
+	// Read signal into U32
+	switch (sig->order) {
+	case LITTLE_ENDIAN:
+		pluckLE(sig, frame, &raw->u32);
+		break;
+	case BIG_ENDIAN:
+		pluckBE(sig, frame, &raw->u32);
+		break;
+	default:
+		return FAIL; // invalid byte order
+	}
+
+	// Shrink signal to appropriate size and set +/- sign
+	if (sig->size <= 8u) {
+		raw->u8 = raw->u32 & 0xFF;
+		if (sig->isSigned) {
+			raw->type = NUM_I8;
+			raw->i8 = *(I8 *)&raw->u8;
+		} else {
+			raw->type = NUM_U8;
+		}
+	} else if (sig->size <= 16u) {
+		raw->u16 = raw->u32 & 0xFFFF;
+		if (sig->isSigned) {
+			raw->type = NUM_I16;
+			raw->i16 = *(I16 *)&raw->u16;
+		} else {
+			raw->type = NUM_U16;
+		}
+	} else if (sig->size <= 32u) {
+		if (sig->isSigned) {
+			raw->type = NUM_I32;
+			raw->i32 = *(I32 *)&raw->u32;
+		} else {
+			raw->type = NUM_U32;
+		}
+	} else {
+		return FAIL; // signal too big
+	}
+
+	return OK;
+}