1 files changed, 37 insertions, 0 deletions
diff --git a/src/drivers/i2c.cpp b/src/drivers/i2c.cpp
index d3378df6..0c195a25 100644
--- a/src/drivers/i2c.cpp
+++ b/src/drivers/i2c.cpp
@@ -3,11 +3,48 @@
 
 #include "assert.h"
 #include "driver/i2c.h"
+#include "esp_err.h"
 
 namespace drivers {
 
+static const i2c_port_t kI2CPort = I2C_NUM_0;
+static const gpio_num_t kI2CSdaPin = GPIO_NUM_2;
+static const gpio_num_t kI2CSclPin = GPIO_NUM_4;
+static const uint32_t kI2CClkSpeed = 400'000;
+
 static constexpr int kCmdLinkSize = I2C_LINK_RECOMMENDED_SIZE(12);
 
+esp_err_t init_i2c(void) {
+  i2c_config_t config = {
+      .mode = I2C_MODE_MASTER,
+      .sda_io_num = kI2CSdaPin,
+      .scl_io_num = kI2CSclPin,
+      .sda_pullup_en = GPIO_PULLUP_ENABLE,
+      .scl_pullup_en = GPIO_PULLUP_ENABLE,
+      .master =
+          {
+              .clk_speed = kI2CClkSpeed,
+          },
+      // No requirements for the clock.
+      .clk_flags = 0,
+  };
+
+  if (esp_err_t err = i2c_param_config(kI2CPort, &config)) {
+    return err;
+  }
+  if (esp_err_t err = i2c_driver_install(kI2CPort, config.mode, 0, 0, 0)) {
+    return err;
+  }
+
+  // TODO: INT line
+
+  return ESP_OK;
+}
+
+esp_err_t deinit_i2c(void) {
+  return i2c_driver_delete(kI2CPort);
+}
+
 I2CTransaction::I2CTransaction() {
   // Use a fixed size buffer to avoid many many tiny allocations.
   buffer_ = (uint8_t*)calloc(sizeof(uint8_t), kCmdLinkSize);