diff --git a/src/rp2040/Kconfig b/src/rp2040/Kconfig
index 481761b3d..97c31b859 100644
--- a/src/rp2040/Kconfig
+++ b/src/rp2040/Kconfig
@@ -14,8 +14,6 @@ config RPXXXX_SELECT
     select HAVE_GPIO_HARD_PWM
     select HAVE_STEPPER_OPTIMIZED_BOTH_EDGE
     select HAVE_BOOTLOADER_REQUEST
-    # Software divide needed on rp2040 for rate calculation in i2c.c
-    select HAVE_SOFTWARE_DIVIDE_REQUIRED if MACH_RP2040
 
 config BOARD_DIRECTORY
     string
diff --git a/src/rp2040/i2c.c b/src/rp2040/i2c.c
index 8ec2bd7a6..0bec61c29 100644
--- a/src/rp2040/i2c.c
+++ b/src/rp2040/i2c.c
@@ -101,10 +101,14 @@ i2c_setup(uint32_t bus, uint32_t rate, uint8_t addr)
 
         // See `i2c_set_baudrate` in the Pico SDK `hardware_i2c/i2c.c` file
         // for details on the calculations here.
-        if (rate > 1000000)
-            rate = 1000000; // Clamp the rate to 1Mbps
-        uint32_t period = (pclk + rate / 2) / rate;
-        uint32_t lcnt = period * 3 / 5;
+        uint32_t period;
+        if (rate >= 1000000)
+            period = DIV_ROUND_CLOSEST(pclk, 1000000);
+        else if (rate >= 400000)
+            period = DIV_ROUND_CLOSEST(pclk, 400000);
+        else
+            period = DIV_ROUND_CLOSEST(pclk, 100000);
+        uint32_t lcnt = DIV_ROUND_CLOSEST(period * ((3<<16) / 5), 1<<16); // 60%
         uint32_t hcnt = period - lcnt;
         uint32_t sda_tx_hold_count = ((pclk * 3) / 10000000) + 1;