diff --git a/app/cmake/incp.cmake b/app/cmake/incp.cmake
index 4bab6ea..731b23c 100644
--- a/app/cmake/incp.cmake
+++ b/app/cmake/incp.cmake
@@ -1,7 +1,7 @@
 FetchContent_Declare(
 	pyincp
     GIT_REPOSITORY git@repo.dsi.tecnico.ulisboa.pt:nanosatlab/istsat-2/incp.git
-    GIT_TAG feat/cli-rework
+    GIT_TAG develop
 )
 
 FetchContent_MakeAvailable(pyincp)
diff --git a/app/incp/schema.yml b/app/incp/schema.yml
index 4fb287b..c29bf62 100644
--- a/app/incp/schema.yml
+++ b/app/incp/schema.yml
@@ -1,10 +1,67 @@
 modules:
-  - name: mod_a
-    enums: []
+  - name: calc
+    enums:
+      - name: enum_a
+        doc: Enum a
+        values: [a, b]
     variables:
-      - name: var_a
-        doc: Var a
-        type: I32
-    reports: []
-    commands: []
-    events: []
+      - name: time_boot
+        doc: Time in seconds since last reboot
+        type: U32
+
+      - name: time_real
+        doc: Time in seconds since epoch
+        type: U32
+
+      - name: test_str
+        doc: A test string with the fixed value TEST
+        type: S16
+
+      - name: test_enum
+        doc: Tests enums
+        type: enum_a
+
+    reports:
+      - name: time
+        doc: Collection of all time variables
+        variables:
+          - time_boot
+          - time_real
+
+    commands:
+      - name: add
+        doc: Sums two numbers
+        args:
+          - name: left
+            doc: First operand
+            type: I32
+          - name: right
+            doc: Second operand
+            type: I32
+        rets:
+          - name: val
+            doc: Result
+            type: I32
+
+      - name: div
+        doc: Divides two numbers
+        args:
+          - name: left
+            doc: First operand
+            type: I32
+          - name: right
+            doc: Second operand
+            type: I32
+        rets:
+          - name: val
+            doc: Result
+            type: I32
+          - name: err
+            doc: Error flag, set on error
+            type: Bool
+
+    events:
+      - name: event_a
+        doc: Event a
+      - name: event_b
+        doc: Event b
diff --git a/app/src/CMakeLists.txt b/app/src/CMakeLists.txt
index 97966ec..25c4e35 100644
--- a/app/src/CMakeLists.txt
+++ b/app/src/CMakeLists.txt
@@ -1,3 +1,5 @@
 target_sources(${EXECUTABLE} PRIVATE
 	main.c
+	incp_handler.c
+	msg_buf.c
 )
diff --git a/app/src/circular_buffer.h b/app/src/circular_buffer.h
deleted file mode 100644
index 9fc22c1..0000000
--- a/app/src/circular_buffer.h
+++ /dev/null
@@ -1,31 +0,0 @@
-
-#ifndef _CIRCULAR_BUFF_H_
-#define _CIRCULAR_BUFF_H_
-
-#include <stdbool.h>
-
-#define _CB_COUNT(buffer) (sizeof((buffer)->buf) / sizeof((buffer)->buf[0]))
-#define _CB_WRAP(buffer, idx) ((idx) % (_CB_COUNT(buffer)))
-
-#define CIRCULAR_BUFFER_ALL(name, type, capacity) \
-typedef struct {  \
-	volatile uint32_t tail, count; \
-	volatile type buf[capacity]; \
-} name##_t; \
-void name##_init(volatile name##_t *buffer) { buffer->tail = buffer->count = 0; } \
-bool name##_append(volatile name##_t *buffer, type element) { if (buffer->count == _CB_COUNT(buffer)) return false; buffer->buf[_CB_WRAP(buffer, buffer->tail + buffer->count)] = element; ++buffer->count; return true; } \
-bool name##_append_range(volatile name##_t *buffer, type *elements, int count) { \
-	for (int i = 0; i < count; ++i) \
-		if (!name##_append(buffer, elements[i])) return false; \
-	return true; \
-} \
-int name##_take(volatile name##_t *buffer, type *dest, uint32_t count) { \
-	count = count > buffer->count ? buffer->count : count; \
-	for (uint32_t i = 0; i < count; ++i) { \
-		dest[i] = buffer->buf[_CB_WRAP(buffer, buffer->tail)]; \
-		++buffer->tail; --buffer->count; \
-	} \
-	return count; \
-} \
-
-#endif
diff --git a/app/src/incp_handler.c b/app/src/incp_handler.c
new file mode 100644
index 0000000..a26a685
--- /dev/null
+++ b/app/src/incp_handler.c
@@ -0,0 +1,22 @@
+#include "incp_handler.h"
+#include "main.h"
+
+#define INCP_IMPL_CALC
+#include "gen_wrapper.h"
+
+
+void handle_incp(msg_buf_t *msgs)
+{
+	uint8_t *bytes;
+	uint32_t len;
+	err_t err;
+
+	err = msg_buf_peek(msgs, (void**)&bytes, &len);
+	if (err == ERR_EMPTY)
+		return;
+
+	if (err != ERR_OK)
+		Error_Handler();
+
+	//TODO: React to INCP
+}
diff --git a/app/src/incp_handler.h b/app/src/incp_handler.h
new file mode 100644
index 0000000..d12bcb8
--- /dev/null
+++ b/app/src/incp_handler.h
@@ -0,0 +1,8 @@
+#ifndef INCP_HANDLER_H_
+#define INCP_HANDLER_H_
+
+#include "msg_buf.h"
+
+void handle_incp(msg_buf_t *msgs);
+
+#endif
diff --git a/app/src/main.c b/app/src/main.c
index 1407059..0aafd0f 100644
--- a/app/src/main.c
+++ b/app/src/main.c
@@ -6,14 +6,15 @@
 #include "stm32f4xx_hal_gpio.h"
 #include "stm32f4xx_ll_i2c.h"
 
-#include "gen_wrapper.h"
+#include "incp_handler.h"
+#include "msg_buf.h"
 
-#include "circular_buffer.h"
+#define I2C_BUF_SIZE 512
+uint8_t i2c_buf[I2C_BUF_SIZE];
 
-#define CIRC_BUF_SIZE (2*1024)
-CIRCULAR_BUFFER_ALL(circ_buff, uint8_t, CIRC_BUF_SIZE)
-
-static volatile circ_buff_t i2c_buf;
+#define MSG_BUF_CAPACITY (2*1024)
+uint8_t i2c_msg_store[MSG_BUF_CAPACITY];
+msg_buf_t i2c_msgs;
 
 void I2C1_EV_IRQHandler()
 {
@@ -24,8 +25,10 @@ void I2C1_EV_IRQHandler()
 	//Skip checking if receiving or transmitting (should always be false)
 	LL_I2C_ClearFlag_ADDR(I2C1);
 
-	//Allow buffer to grow until full
-	while (i2c_buf.count < _CB_COUNT(&i2c_buf))
+	int head = 0;
+
+	//Allow buffer to grow until full (and inclusive)
+	while (head <= I2C_BUF_SIZE)
 	{
 		//Wait for data or stop flag
 		while (!LL_I2C_IsActiveFlag_STOP(I2C1) && !LL_I2C_IsActiveFlag_RXNE(I2C1));
@@ -37,14 +40,16 @@ void I2C1_EV_IRQHandler()
 		}
 		else //Data received
 		{
+			if (head >= I2C_BUF_SIZE) //Overflow for sure
+				Error_Handler();
+
 			uint8_t data = LL_I2C_ReceiveData8(I2C1);
-			circ_buff_append_range(&i2c_buf, &data, 1);
+			i2c_buf[head++] = data;
 		}
 	}
 
-	//Overflow or stop condition reached
-	if (i2c_buf.count >= _CB_COUNT(&i2c_buf))
-		Error_Handler();
+	//Stop condition reached, save msg
+	msg_buf_push(&i2c_msgs, i2c_buf, head);
 }
 
 bool I2C1_transmit_master(uint8_t addr, uint8_t *buf, uint8_t count)
@@ -85,10 +90,16 @@ bool I2C1_transmit_master(uint8_t addr, uint8_t *buf, uint8_t count)
 
 int my_main()
 {
+	msg_buf_init(&i2c_msgs, i2c_msg_store, sizeof(i2c_msg_store));
+
 	while (1)
 	{
 		HAL_GPIO_TogglePin(LD2_GPIO_Port, LD2_Pin);
-		HAL_Delay(500);
+		for (int i = 0; i < 500; ++i)
+		{
+			HAL_Delay(1);
+			handle_incp(&i2c_msgs);
+		}
 	}
 
 	return 0;
diff --git a/app/src/msg_buf.c b/app/src/msg_buf.c
new file mode 100644
index 0000000..9a66adc
--- /dev/null
+++ b/app/src/msg_buf.c
@@ -0,0 +1,113 @@
+#include "msg_buf.h"
+
+typedef uint16_t len_prefix_t;
+#define _PADDING_BYTE (0xFF)
+#define _PADDING_VALUE ((len_prefix_t)0xFFFF)
+#define _LENGTH_SIZE (sizeof(len_prefix_t))
+
+
+
+int msg_buf_init(msg_buf_t *buf, void *storage, uint32_t capacity)
+{
+	if ((capacity % _LENGTH_SIZE) != 0)
+		return ERR_ARGS;
+
+	buf->storage = storage;
+	buf->capacity = capacity;
+	buf->head = buf->count = 0;
+
+	return ERR_OK;
+}
+
+static inline void pad_to_end(msg_buf_t *buf)
+{
+	//Pad to end, advancing head and count
+	uint32_t len = buf->capacity - buf->head;
+	memset(buf->storage + buf->head, _PADDING_BYTE, len);
+	buf->count += len;
+	buf->head = 0;
+}
+
+int msg_buf_push(msg_buf_t *buf, void *data, uint32_t len)
+{
+	if (len > MAX_LEN)
+		return ERR_ARGS;
+
+	//Length of prefix+message
+	uint32_t len_f = _LENGTH_SIZE + len;
+
+	if (buf->capacity - buf->head < len_f) //No space ahead
+	{
+		if (buf->head - buf->count < len_f) //No space behind
+			return ERR_FULL;
+
+		pad_to_end(buf);
+
+		//Reset head
+		buf->head = 0;
+	}
+
+	//At this point head is at the first free location and there is enough space after it
+	*(len_prefix_t*)(buf->storage + buf->head) = len;
+	memcpy(buf->storage + buf->head + _LENGTH_SIZE, buf, len);
+	buf->head += len_f;
+	buf->count += len_f;
+
+	return ERR_OK;
+}
+
+static inline uint32_t buf_tail(msg_buf_t *buf)
+{
+	if (buf->head >= buf->count)
+		return buf->head - buf->count;
+
+	return buf->capacity + buf->head - buf->count;
+}
+
+int msg_buf_peek(msg_buf_t *buf, void **ptr, uint32_t *len)
+{
+	if (buf->count == 0)
+		return ERR_EMPTY;
+
+	uint32_t tail = buf_tail(buf);
+	//No need to worry about padding, pop will do cleanups
+	*len = *(uint16_t*)(buf->storage + tail);
+	*ptr = buf->storage + tail + _LENGTH_SIZE;
+
+	return ERR_OK;
+}
+
+int msg_buf_pop(msg_buf_t *buf)
+{
+	if (buf->count == 0)
+		return ERR_EMPTY;
+
+	uint32_t tail = buf_tail(buf);
+	uint32_t len = *(uint16_t*)(buf->storage + tail);
+	buf->count -= _LENGTH_SIZE + len;
+
+	//If empty, reset head to start (minimize fragmentation)
+	if (buf->count == 0)
+	{
+		buf->head = 0;
+		return ERR_OK;
+	}
+
+	//At this point tail can either be at thestart of padding
+	//  -> Clear padding (wraps back to 0)
+
+	tail = buf_tail(buf);
+	len = *(uint16_t*)(buf->storage + tail);
+
+	//Leave as is if not padding
+	if (len != _PADDING_VALUE)
+		return ERR_OK;
+
+	//Clear padding
+	uint32_t padding_len = buf->capacity - tail;
+	buf->count -= padding_len;
+
+	//Tail is now at storage start.
+	//If the buffer is empty, it is also aligned as a consequence.
+	return ERR_OK;
+}
diff --git a/app/src/msg_buf.h b/app/src/msg_buf.h
new file mode 100644
index 0000000..9ef5e3a
--- /dev/null
+++ b/app/src/msg_buf.h
@@ -0,0 +1,36 @@
+#ifndef MSG_BUF_H_
+#define MSG_BUF_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+
+
+#define MAX_LEN (0xFFFE)
+
+
+typedef uint32_t err_t;
+#define ERR_OK ((err_t)0)
+#define ERR_ARGS ((err_t)1)
+#define ERR_FULL ((err_t)2)
+#define ERR_EMPTY ((err_t)3)
+
+
+// Message buffer
+// Variable-sized entry circular buffer
+// No modulo, contiguous memory read/write
+// Maximum message size 0xFFFE (0xFFFF is padding)
+// Minimum message size 0
+// NOT thread/task safe
+typedef struct msg_buf {
+	uint8_t *storage;
+	uint32_t capacity;
+	uint32_t head, count;
+} msg_buf_t;
+
+int msg_buf_init(msg_buf_t *buf, void *storage, uint32_t capacity);
+int msg_buf_push(msg_buf_t *buf, void *data, uint32_t len);
+int msg_buf_peek(msg_buf_t *buf, void **ptr, uint32_t *len);
+int msg_buf_pop(msg_buf_t *buf);
+
+#endif