2 files changed, 44 insertions, 8 deletions

diff --git a/lib/pbdrv/drv/arduino/mod.cpp b/lib/pbdrv/drv/arduino/mod.cpp
index ac051db..d281495 100644
--- a/lib/pbdrv/drv/arduino/mod.cpp
+++ b/lib/pbdrv/drv/arduino/mod.cpp
@@ -4,24 +4,43 @@
 
 #include <Arduino.h>
 #include <Wire.h>
+#include <avr/delay.h>
+
+#include <FreeRTOS.h>
+#include <timers.h>
+#include <task.h>
 
 #include <stdlib.h>
 #include <stdint.h>
 
 #include "../../pb.h"
 #include "../../pb-mod.h"
+#include "../../pb-buf.h"
 #include "mod.h"
 
+//! Arduino setup function
+extern void setup(void);
+void loop(void) {}
+
+static void async_pb_i2c_recv(void * _msg, uint32_t _) {
+	pb_buf_t * msg = (pb_buf_t *) _msg;
+	pb_i2c_recv((uint8_t *) msg->data, msg->size);
+	pb_buf_free(msg);
+	free(msg);
+}
+
 static void recv_event(int bytes) {
-	uint8_t * data = (uint8_t *) malloc(bytes);
-	size_t size = 0;
+	pb_buf_t * msg = (pb_buf_t *) malloc(sizeof(pb_buf_t));
+	msg->data = (char *) malloc(bytes);
+	msg->size = 0;
 	while (Wire.available())
-		data[size++] = Wire.read();
+		msg->data[msg->size++] = Wire.read();
 
-	pb_i2c_recv(data, size);
+	// defer pb_i2c_recv call
+	xTimerPendFunctionCallFromISR(async_pb_i2c_recv, msg, 0, NULL);
 }
 
-void pb_setup() {
+static void pb_setup() {
 	Wire.begin((int) PB_MOD_ADDR);
 	Wire.setWireTimeout(PB_TIMEOUT_US, true);
 	Wire.setClock(PB_CLOCK_SPEED_HZ);
@@ -29,6 +48,18 @@ void pb_setup() {
 	Wire.onReceive(recv_event);
 }
 
+void initVariant(void) {
+	// call regular arduino setup
+	setup();
+	Serial.print("regular setup done and in initVariant\r\n"); // DEBUG
+
+	// call pbdrv-mod setup
+	pb_setup();
+
+	// start freertos scheduler
+	vTaskStartScheduler();
+}
+
 __weak void pb_i2c_send(i2c_addr_t addr, const uint8_t * buf, size_t sz) {
 	Wire.beginTransmission((int) addr);
 	Wire.write(buf, sz);
diff --git a/lib/pbdrv/drv/arduino/mod.h b/lib/pbdrv/drv/arduino/mod.h
index c4cb9ce..87b5724 100644
--- a/lib/pbdrv/drv/arduino/mod.h
+++ b/lib/pbdrv/drv/arduino/mod.h
@@ -4,12 +4,17 @@
 extern "C" {
 #endif
 
+//! Arduino init variant (called before user setup)
+void initVariant(void);
+
 /**
- * \brief puzzle bus driver setup
+ * \brief Arduino loop function
  *
- * This function should be called from the Arduino \c setup() function.
+ * Loop won't run because everything is handled by the freertos scheduler. It
+ * is defined in this driver to cause compiler warnings if the user has defined
+ * the loop() function anyways.
  */
-void pb_setup();
+extern void loop(void);
 
 #ifdef __cplusplus
 }