1 files changed, 37 insertions, 0 deletions
diff --git a/ports/stm32f4/src/stream_i2c.c b/ports/stm32f4/src/stream_i2c.c
new file mode 100644
index 0000000..621e11e
--- /dev/null
+++ b/ports/stm32f4/src/stream_i2c.c
@@ -0,0 +1,37 @@
+#include "stream_i2c.h"
+#include "port.h"
+#include "stream.h"
+
+int read_I2C(uint8_t* buf, size_t count, void **vptr, void *sptr)
+{
+    p_stream_t *stream = (p_stream_t*)sptr;
+    I2C_HandleTypeDef dev = *(I2C_HandleTypeDef*)stream->props[0];
+    
+    uint16_t addr         = *(uint16_t*)vptr[0];
+    uint32_t timeout      = *(uint32_t*)vptr[1];
+    uint16_t AF_limit     = *(uint32_t*)vptr[2];
+    
+    int error, AF_counter = 0; 
+    while (HAL_I2C_Master_Receive(&dev, addr, buf, count, timeout) != HAL_OK) {
+	if ((error = HAL_I2C_GetError(&dev)) != HAL_I2C_ERROR_AF) {
+	    return error;
+	}
+	else if (++AF_counter > AF_limit) {
+	    return HAL_I2C_ERROR_AF;
+	}
+    }
+    return 0;
+}
+
+int write_I2C(uint8_t* buf, size_t count, void **vptr, void *sptr)
+{
+    p_stream_t *stream = (p_stream_t*)sptr;
+    I2C_HandleTypeDef dev = *(I2C_HandleTypeDef*)stream->props[0];
+    uint16_t addr         = *(uint16_t*)vptr[1];
+    uint32_t timeout      = *(uint32_t*)vptr[2];
+    
+    while (HAL_I2C_Master_Transmit(&dev, addr, buf, count, timeout) != HAL_OK) {
+	return HAL_I2C_GetError(&dev);
+    }
+    return 0;
+}