From 7b32859c88b732ae8382b6aabe5e3be4ee2818e3 Mon Sep 17 00:00:00 2001
From: Ea-r-th <39779954+Ea-r-th@users.noreply.github.com>
Date: Tue, 16 Sep 2025 00:38:36 -0700
Subject: [PATCH] Added I2C clock config

---
 SHAL/Include/Peripheral/I2C/SHAL_I2C.h |  2 ++
 SHAL/Src/Core/SHAL_CORE.cpp            | 27 ++++++++++++++++++--------
 SHAL/Src/Peripheral/I2C/SHAL_I2C.cpp   |  6 ++++++
 SHAL/Src/main.cpp                      | 25 ++++++++++++++++++++----
 4 files changed, 48 insertions(+), 12 deletions(-)

diff --git a/SHAL/Include/Peripheral/I2C/SHAL_I2C.h b/SHAL/Include/Peripheral/I2C/SHAL_I2C.h
index a37f656..a765d45 100644
--- a/SHAL/Include/Peripheral/I2C/SHAL_I2C.h
+++ b/SHAL/Include/Peripheral/I2C/SHAL_I2C.h
@@ -27,6 +27,8 @@ public:
     /// \param readLen number of bytes to be read
     void masterWriteRead(uint8_t addr,const uint8_t* writeData, size_t writeLen, uint8_t* readData, size_t readLen);
 
+    uint8_t masterWriteReadByte(uint8_t addr, const uint8_t* writeData, size_t writeLen);
+
     /// Function to write an array of commands to an I2C device
     /// \param addr Address of slave device
     /// \param writeData Pointer to array of commands
diff --git a/SHAL/Src/Core/SHAL_CORE.cpp b/SHAL/Src/Core/SHAL_CORE.cpp
index 550c317..2f6f1b3 100644
--- a/SHAL/Src/Core/SHAL_CORE.cpp
+++ b/SHAL/Src/Core/SHAL_CORE.cpp
@@ -10,18 +10,29 @@ void SHAL_init(){
 
 
 void systick_init(){
-    SysTick->CTRL = 0;           //disable first
-    SysTick->LOAD = 0xFFFFFF;    //max 24-bit
-    SysTick->VAL  = 0;           //clear
+    SysTick->CTRL = 0;           //Disable first
+    SysTick->LOAD = 0xFFFFFF;    //Max 24-bit
+    SysTick->VAL  = 0;           //Clear
     SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_ENABLE_Msk;
 }
 
-void SHAL_delay_us(uint32_t us){
-    uint32_t start = SysTick->VAL;
-    uint32_t ticks = us * (SystemCoreClock / 1000000U);
 
-    //handle wraparound with 24-bit mask
-    while (((start - SysTick->VAL) & 0x00FFFFFF) < ticks) { }
+void SHAL_delay_us(uint32_t us){
+    uint32_t ticks = us * (SystemCoreClock / 1000000U);
+    uint32_t start = SysTick->VAL;
+
+    //Calculate target value (may wrap around)
+    uint32_t target = (start >= ticks) ? (start - ticks) : (start + 0x01000000 - ticks);
+    target &= 0x00FFFFFF;
+
+    //Wait until we reach the target
+    if (start >= ticks) {
+        //No wraparound case
+        while (SysTick->VAL > target) {}
+    } else {
+        while (SysTick->VAL <= start) {} //Wait for wraparound
+        while (SysTick->VAL > target) {} //Wait for target
+    }
 }
 
 void SHAL_delay_ms(uint32_t ms){
diff --git a/SHAL/Src/Peripheral/I2C/SHAL_I2C.cpp b/SHAL/Src/Peripheral/I2C/SHAL_I2C.cpp
index 62ec75c..722870f 100644
--- a/SHAL/Src/Peripheral/I2C/SHAL_I2C.cpp
+++ b/SHAL/Src/Peripheral/I2C/SHAL_I2C.cpp
@@ -98,6 +98,12 @@ void SHAL_I2C::masterRead(uint8_t addr, uint8_t *readBuffer, uint8_t bytesToRead
     masterWriteRead(addr,nullptr,0,readBuffer,bytesToRead);
 }
 
+uint8_t SHAL_I2C::masterWriteReadByte(uint8_t addr, const uint8_t *writeData, size_t writeLen) {
+    uint8_t val = 0;
+    masterWriteRead(addr, writeData, writeLen, &val, 1);
+    return val;
+}
+
 SHAL_I2C& I2CManager::get(uint8_t I2CBus) {
 
     if(I2CBus > NUM_I2C_BUSES - 1){
diff --git a/SHAL/Src/main.cpp b/SHAL/Src/main.cpp
index 32c6754..eaa702e 100644
--- a/SHAL/Src/main.cpp
+++ b/SHAL/Src/main.cpp
@@ -4,7 +4,7 @@
 
 void c3Interrupt(){
     PIN(A5).toggle();
-    UART(2).sendString("New test");
+    SHAL_UART2.sendString("New test");
 }
 
 void tim2Handler(){
@@ -20,6 +20,7 @@ int main() {
     SHAL_UART2.begin(115200);
 
     SHAL_I2C1.init(I2C_Pair::SCL1B6_SDA1B7);
+    SHAL_I2C1.setClockConfig(0x2000090E);
 
     //Use pin C3 to trigger a function on external interrupt
     PIN(C3).useAsExternalInterrupt(TriggerMode::RISING_EDGE,c3Interrupt);
@@ -31,11 +32,27 @@ int main() {
     PIN(A4).setPinMode(PinMode::OUTPUT_MODE);
     PIN(A5).setPinMode(PinMode::OUTPUT_MODE);
 
-    c3Interrupt();
+    //Temporary setup for DHT20
 
-    SHAL_delay_ms(3000);
+    PIN(A5).setLow();
 
-    c3Interrupt(); //test
+    SHAL_delay_ms(5000); //Wait 100 ms from datasheet
+
+    PIN(A5).setHigh();
+
+    uint8_t initByte[1] = {0x71};
+
+    uint8_t status = SHAL_I2C1.masterWriteReadByte(0x38,initByte,1);
+
+    if ((status & 0x18) != 0x18) {
+        SHAL_UART2.sendString("DHT ready");
+    } else {
+        SHAL_UART2.sendString("DHT broke");
+    }
+
+    PIN(A5).setLow();
+
+    //End setup
 
     while (true) {
     	__WFI();