diff --git a/main/TPS546.c b/main/TPS546.c
index 2f9301d2..e139ec23 100644
--- a/main/TPS546.c
+++ b/main/TPS546.c
@@ -32,7 +32,7 @@ static const char *TAG = "TPS546.c";
 /**
  * @brief SMBus read byte
  */
-esp_err_t smb_read_byte(uint8_t command, uint8_t *data)
+static esp_err_t smb_read_byte(uint8_t command, uint8_t *data)
 {
     esp_err_t err = ESP_FAIL;
 
@@ -54,7 +54,7 @@ esp_err_t smb_read_byte(uint8_t command, uint8_t *data)
 /**
  * @brief SMBus write byte
  */
-esp_err_t smb_write_byte(uint8_t command, uint8_t data)
+static esp_err_t smb_write_byte(uint8_t command, uint8_t data)
 {
     esp_err_t err = ESP_FAIL;
 
@@ -74,7 +74,7 @@ esp_err_t smb_write_byte(uint8_t command, uint8_t data)
 /**
  * @brief SMBus read word
  */
-esp_err_t smb_read_word(uint8_t command, uint16_t *result)
+static esp_err_t smb_read_word(uint8_t command, uint16_t *result)
 {
     uint8_t data[2];
     esp_err_t err = ESP_FAIL;
@@ -99,7 +99,7 @@ esp_err_t smb_read_word(uint8_t command, uint16_t *result)
 /**
  * @brief SMBus write word
  */
-esp_err_t smb_write_word(uint8_t command, uint16_t data)
+static esp_err_t smb_write_word(uint8_t command, uint16_t data)
 {
     esp_err_t err = ESP_FAIL;
 
@@ -149,32 +149,9 @@ static esp_err_t smb_read_block(uint8_t command, uint8_t * data, uint8_t len)
 
 
 /**
- * @brief Read a sequence of I2C bytes
+ * @brief Convert an SLINEAR11 value into an int
  */
-static esp_err_t register_read(uint8_t reg_addr, uint8_t * data, size_t len)
-{
-    return i2c_master_write_read_device(I2C_MASTER_NUM, TPS546_I2CADDR, &reg_addr, 1, data, len,
-                                        I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
-}
-
-/**
- * @brief Write a byte to a I2C register
- */
-static esp_err_t register_write_byte(uint8_t reg_addr, uint8_t data)
-{
-    int ret;
-    uint8_t write_buf[2] = {reg_addr, data};
-
-    ret = i2c_master_write_to_device(I2C_MASTER_NUM, TPS546_I2CADDR, write_buf, sizeof(write_buf),
-                                     I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
-
-    return ret;
-}
-
-/**
- * @brief Convert an SLINEAR11 value into a float
- */
-float slinear11_2_float(uint16_t value)
+static int slinear11_2_int(uint16_t value)
 {
     int exponent, mantissa;
     float result;
@@ -198,13 +175,56 @@ float slinear11_2_float(uint16_t value)
 
     // calculate result (mantissa * 2^exponent)
     result = mantissa * powf(2.0, exponent);
+    ESP_LOGI(TAG, "result: %f", result);
+    return (int)result;
+}
+
+/**
+ * @brief Convert an int value into an SLINEAR11
+ */
+static uint16_t int_2_slinear11(int value)
+{
+    float mantissa;
+    int exponent;
+    uint16_t result = 0;
+
+    mantissa = frexp(value, &exponent);
+    ESP_LOGI(TAG, "mantissa: %f, exponent: %d", mantissa, exponent);
+
+    result = (mantissa * 1024);
+//  ESP_LOGI(TAG, "result: %04x", result);
+    // TPS546.c: Writing new frequency: 550
+    // TPS546.c: result: 0.537109, exponent: 10
+
+    // First 5 bits is exponent in twos-complement
+    // check the first bit of the exponent to see if its negative
+//    if (value & 0x8000) {
+        // exponent is negative
+//        exponent = -1 * (((~value >> 11) & 0x001F) + 1);
+//    } else {
+//        exponent = (value >> 11);
+//    }
+    // last 11 bits is the mantissa in twos-complement
+    // check the first bit of the mantissa to see if its negative
+//    if (value & 0x400) {
+        // mantissa is negative
+//        mantissa = -1 * ((~value & 0x07FF) + 1);
+//    } else {
+//        mantissa = (value & 0x07FF);
+//    }
+
+    // TPS546.c: Read Freq: 1234
+    // TPS546.c: result: 2256.000000
+
+    // calculate result (mantissa * 2^exponent)
+//    result = mantissa * powf(2.0, exponent);
     return result;
 }
 
 /**
  * @brief Convert a ULINEAR16 value into an int
  */
-int ulinear16_2_int(uint16_t value)
+static int ulinear16_2_int(uint16_t value)
 {
     float exponent;
     int result;
@@ -227,7 +247,7 @@ int ulinear16_2_int(uint16_t value)
 /**
  * @brief Convert an int value into a ULINEAR16
  */
-uint16_t int_2_ulinear16(int value)
+static uint16_t int_2_ulinear16(int value)
 {
     uint8_t voutmode;
     float exponent;
@@ -262,8 +282,7 @@ void TPS546_init(void)
     int millivolts;
     int vmax;
     float f_value;
-    float iout;
-
+    int iout;
     ESP_LOGI(TAG, "Initializing the core voltage regulator");
 
     smb_read_block(PMBUS_IC_DEVICE_ID, data, 6);
@@ -273,11 +292,13 @@ void TPS546_init(void)
     smb_read_byte(PMBUS_REVISION, &u8_value);
     ESP_LOGI(TAG, "PMBus revision: %02x", u8_value);
 
-
-    /* Show temperature (SLINEAR11) */
+    /* Show temperature */
     ESP_LOGI(TAG, "--------------------------------");
     ESP_LOGI(TAG, "Temp: %d", TPS546_get_temperature());
 
+    /* Show switching frequency */
+    TPS546_get_frequency();
+    TPS546_set_frequency(650);
 
     /* Show voltage settings */
     TPS546_show_voltage_settings();
@@ -293,14 +314,10 @@ void TPS546_init(void)
 //    ESP_LOGI(TAG, "Vout Max set to: %d mV", vmax);
 
     ESP_LOGI(TAG, "-----------CURRENT---------------------");
-
     /* Get output current (SLINEAR11) */
     smb_read_word(PMBUS_READ_IOUT, &u16_value);
-    iout = slinear11_2_float(u16_value);
-    ESP_LOGI(TAG, "Iout measured: %2.2f", iout);
-
-
-
+    iout = slinear11_2_int(u16_value);
+    ESP_LOGI(TAG, "Iout measured: %d", iout);
 
     /* Get voltage output (ULINEAR16) */
     // This gets a timeout, don't know why.  clock stretching?
@@ -311,14 +328,46 @@ void TPS546_init(void)
 
 }
 
+int TPS546_get_frequency(void)
+{
+    uint16_t value;
+    int freq;
+
+    smb_read_word(PMBUS_FREQUENCY_SWITCH, &value);
+    ESP_LOGI(TAG, "Read Freq: %04x", value);
+    freq = slinear11_2_int(value);
+
+    ESP_LOGI(TAG, "Frequency: %d", freq);
+    return (int)freq;
+}
+
+void TPS546_set_frequency(int newfreq)
+{
+    uint16_t value;
+    int freq;
+
+    ESP_LOGI(TAG, "Writing new frequency: %d", newfreq);
+    value = int_2_slinear11(newfreq);
+    //value = 0xC999;
+    ESP_LOGI(TAG, "New value: 0x%04x", value);
+    //smb_write_word(PMBUS_FREQUENCY_SWITCH, value);
+
+    ESP_LOGI(TAG, "Checking conversion...");
+    freq = slinear11_2_int(value);
+    ESP_LOGI(TAG, "Converted value: %d", freq);
+
+    //ESP_LOGI(TAG, "Frequency: %d", (int)freq);
+    //return (int)freq;
+}
+
 int TPS546_get_temperature(void)
 {
     uint16_t value;
-    float temp;
+    int temp;
 
     smb_read_word(PMBUS_READ_TEMPERATURE_1, &value);
-    temp = slinear11_2_float(value);
-    return (int)temp;
+    temp = slinear11_2_int(value);
+    return temp;
 }
 
 void TPS546_set_vout(int millivolts)
@@ -340,13 +389,13 @@ void TPS546_show_voltage_settings(void)
     ESP_LOGI(TAG, "-----------VOLTAGE---------------------");
     /* VIN_ON SLINEAR11 */
     smb_read_word(PMBUS_VIN_ON, &u16_value);
-    f_value = slinear11_2_float(u16_value);
-    ESP_LOGI(TAG, "VIN ON set to: %f", f_value);
+    i_value = slinear11_2_int(u16_value);
+    ESP_LOGI(TAG, "VIN ON set to: %d", i_value);
 
     /* VIN_OFF SLINEAR11 */
     smb_read_word(PMBUS_VIN_OFF, &u16_value);
-    f_value = slinear11_2_float(u16_value);
-    ESP_LOGI(TAG, "VIN OFF set to: %f", f_value);
+    i_value = slinear11_2_int(u16_value);
+    ESP_LOGI(TAG, "VIN OFF set to: %d", i_value);
 
     /* VOUT_MAX */
     smb_read_word(PMBUS_VOUT_MAX, &u16_value);
diff --git a/main/TPS546.h b/main/TPS546.h
index 84b697ec..daf90522 100644
--- a/main/TPS546.h
+++ b/main/TPS546.h
@@ -53,6 +53,8 @@ struct tps546_settings_t
 };
 
 void TPS546_init(void);
+int TPS546_get_frequency(void);
+void TPS546_set_frequency(int);
 int TPS546_get_temperature(void);
 void TPS546_set_vout(int millivolts);
 void TPS546_show_voltage_settings(void);