4 files changed, 69 insertions, 1 deletions
diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt
index cd90499..88abf60 100644
--- a/main/CMakeLists.txt
+++ b/main/CMakeLists.txt
@@ -17,6 +17,7 @@ add_executable(main
 	main.c
 	init.c
 	sock.c
+	i2c.c
 	)
 
 pico_enable_stdio_usb(main 1)
@@ -29,6 +30,7 @@ target_include_directories(main PRIVATE ${CMAKE_CURRENT_LIST_DIR})
 target_link_libraries(main
 	pico_cyw43_arch_lwip_sys_freertos
 	pico_stdlib
+	hardware_i2c
 	FreeRTOS-Kernel
 	FreeRTOS-Kernel-Heap4
 	mpack
diff --git a/main/i2c.c b/main/i2c.c
new file mode 100644
index 0000000..7ab1d14
--- /dev/null
+++ b/main/i2c.c
@@ -0,0 +1,29 @@
+#include "i2c.h"
+
+#include <stdio.h>
+#include <stddef.h>
+#include <stdint.h>
+#include <pico/stdlib.h>
+#include <hardware/i2c.h>
+
+void init_i2c() {
+    stdio_init_all();
+	i2c_init(i2c_default, 100 * 1000);
+
+	// Initialize I2C pins - sda(16), scl(17)
+    gpio_set_function(SDA_PIN, GPIO_FUNC_I2C);
+    gpio_set_function(SCL_PIN, GPIO_FUNC_I2C);
+
+    gpio_pull_up(SDA_PIN);
+    gpio_pull_up(SCL_PIN);
+}
+
+int read_i2c(uint8_t addr, uint8_t *output, size_t len) {
+    // false - finished with bus
+    return i2c_read_blocking (i2c_default, addr, output, len, false);
+}
+
+int write_i2c(uint8_t addr, uint8_t *input, size_t len) {
+    // true to keep master control of bus
+    return i2c_write_blocking (i2c_default, addr, input, len, true);
+}
diff --git a/main/i2c.h b/main/i2c.h
new file mode 100644
index 0000000..d12bca1
--- /dev/null
+++ b/main/i2c.h
@@ -0,0 +1,36 @@
+#pragma once
+// https://github.com/raspberrypi/pico-examples/tree/master/i2c
+
+#include <stddef.h>
+#include <stdint.h>
+
+#define SDA_PIN 16
+#define SCL_PIN 17
+
+/**
+ * \brief initialize all required gpio for i2c usage on the pico
+ * 
+ * This functions only initializes the standard gpio required to start i2c
+ * communications.
+ *
+ * \note Tasks shouldn't depend on any other module in the main controller
+ */
+void init_i2c();
+
+/**
+ * \brief read data from addr with length len from i2c bus.
+ * 
+ * This functions reads data from a specific address on the i2c bus,
+ * the output var will hold the data which was read from said address with
+ * length len.
+ */
+int read_i2c(uint8_t addr, uint8_t *output, size_t len);
+
+/**
+ * \brief write data to addr with length len from i2c bus.
+ * 
+ * This functions writes data to a specific address on the i2c bus,
+ * the input var holds the data which will be written to the given 
+ * address with length len.
+ */
+int write_i2c(uint8_t addr, uint8_t *input, size_t len);
diff --git a/main/init.c b/main/init.c
index dcd3d54..f336ad0 100644
--- a/main/init.c
+++ b/main/init.c
@@ -1,5 +1,6 @@
 #include "config.h"
 #include "init.h"
+#include "i2c.h"
 
 #include <FreeRTOS.h>
 #include <task.h>
@@ -33,7 +34,7 @@ static void init_wifi() {
 
 static void async_init() {
 	init_cyw34();
-	// TODO: initialize i2c
+	init_i2c();
 	init_wifi();
 
 	xEventGroupSetBits(init_complete, 1);