From 2466b7a2c6544fb585acb47871c75f2816629a2b Mon Sep 17 00:00:00 2001
From: "Dr. Matthew Swabey" <matthew@swabey.org>
Date: Tue, 21 Mar 2023 15:02:58 -0400
Subject: avr: Allow I2C bus speed to be set by Klipper (#6111)

Allow I2C bus speed to be set by appropriate Klipper commands.

Signed-off-by: Matthew Swabey <matthew@swabey.org>
---
 src/avr/i2c.c | 36 ++++++++++++++++--------------------
 1 file changed, 16 insertions(+), 20 deletions(-)

(limited to 'src')

diff --git a/src/avr/i2c.c b/src/avr/i2c.c
index 370582cc..658a30a1 100644
--- a/src/avr/i2c.c
+++ b/src/avr/i2c.c
@@ -27,31 +27,27 @@ static const uint8_t SCL = GPIO('D', 0), SDA = GPIO('D', 1);
 DECL_CONSTANT_STR("BUS_PINS_twi", "PD0,PD1");
 #endif
 
-static void
-i2c_init(void)
-{
-    if (TWCR & (1<<TWEN))
-        // Already setup
-        return;
-
-    // Setup output pins and enable pullups
-    gpio_out_setup(SDA, 1);
-    gpio_out_setup(SCL, 1);
-
-    // Set 100Khz frequency
-    TWSR = 0;
-    TWBR = ((CONFIG_CLOCK_FREQ / 100000) - 16) / 2;
-
-    // Enable interface
-    TWCR = (1<<TWEN);
-}
-
 struct i2c_config
 i2c_setup(uint32_t bus, uint32_t rate, uint8_t addr)
 {
     if (bus)
         shutdown("Unsupported i2c bus");
-    i2c_init();
+
+    if (!(TWCR & (1<<TWEN))) {
+        // Setup output pins and enable pullups
+        gpio_out_setup(SDA, 1);
+        gpio_out_setup(SCL, 1);
+
+        // Set frequency avoiding pulling in integer divide
+        TWSR = 0;
+        if (rate >= 400000)
+            TWBR = ((CONFIG_CLOCK_FREQ / 400000) - 16) / 2;
+        else
+            TWBR = ((CONFIG_CLOCK_FREQ / 100000) - 16) / 2;
+
+        // Enable interface
+        TWCR = (1<<TWEN);
+    }
     return (struct i2c_config){ .addr=addr<<1 };
 }
 
-- 
cgit v1.2.3-70-g09d2