USB-PD Support for Sequre S60P (#1883)

* Basic Init

* Rought implementation of fs2711 usb pd interface

* Rought implementation of fs2711 usb pd interface

* Still needs work overcurrent protection keeps getting tripped

* New pdo selection logic

* Update push.yml

* Update push.yml

* Update push.yml

* Update Makefile

* Adds PPS

* Removed unused define

* Adds PPS

* Apply suggestions from code review

Co-authored-by: Ben V. Brown <[email protected]>

* Code review changes

* Added osDelay include

* New line alignment for S60 softwarei2c

* Code review

* Fixes code review stuff

* code review changes

* Change voltage limit to 20 as that's what the device is rated for

* Shortened wait time for usb pd

* Fixed issues that cuase S60P to restart constantly

* fixing minimal OLED brightness

With the current settings, the OLED turns off if the first level is selected.

* Adds protocol to s60p debug menu

* loosened fs2711 protocol selection timing

* Adds PDO register reading to negotiation logic

* Fixes FS2711 timeout issue and cleans up driver

* Adds FS2711 protocol negotiation to power loop

* Removed uneeded define

* Reverts changes to Font.h and adds clang-format comments

---------

Co-authored-by: Ben V. Brown <[email protected]>
Co-authored-by: discip <[email protected]>
Co-authored-by: Ben V. Brown <[email protected]>

12 files changed, 507 insertions, 14 deletions

diff --git a/source/Core/BSP/Sequre_S60/BSP.cpp b/source/Core/BSP/Sequre_S60/BSP.cpp
index 3dbefcaf..b32f239e 100644
--- a/source/Core/BSP/Sequre_S60/BSP.cpp
+++ b/source/Core/BSP/Sequre_S60/BSP.cpp
@@ -2,6 +2,7 @@
 

 #include "BSP.h"

 #include "BootLogo.h"

+#include "FS2711.hpp"

 #include "HUB238.hpp"

 #include "I2C_Wrapper.hpp"

 #include "Pins.h"

@@ -210,13 +211,23 @@ bool isTipDisconnected() {
 

 void    setStatusLED(const enum StatusLED state) {}

 uint8_t preStartChecks() {

+#if POW_PD_EXT == 1

   if (!hub238_has_run_selection() && (xTaskGetTickCount() < TICKS_SECOND * 5)) {

     return 0;

   }

   // We check if we are in a "Limited" mode; where we have to run the PWM really fast

   // Where as if we are on 9V for example, the tip resistance is enough

-  uint16_t voltage                = hub238_source_voltage();

-  uint16_t currentx100            = hub238_source_currentX100();

+  uint16_t voltage     = hub238_source_voltage();

+  uint16_t currentx100 = hub238_source_currentX100();

+#endif

+#if POW_PD_EXT == 2

+  if (!FS2711::has_run_selection() && (xTaskGetTickCount() < TICKS_SECOND * 5)) {

+    return 0;

+  }

+  uint16_t voltage     = FS2711::source_voltage();

+  uint16_t currentx100 = FS2711::source_currentx100();

+#endif

+

   uint16_t thresholdResistancex10 = ((voltage * 1000) / currentx100) + 5;

 

   if (getTipResistanceX10() <= thresholdResistancex10) {

diff --git a/source/Core/BSP/Sequre_S60/Pins.h b/source/Core/BSP/Sequre_S60/Pins.h
index 9f0cfb56..24b65bb5 100644
--- a/source/Core/BSP/Sequre_S60/Pins.h
+++ b/source/Core/BSP/Sequre_S60/Pins.h
@@ -40,7 +40,6 @@
 

 #endif

 

-

 #ifdef MODEL_S60P

 

 #define KEY_B_Pin             GPIO_PIN_1

diff --git a/source/Core/BSP/Sequre_S60/Software_I2C.h b/source/Core/BSP/Sequre_S60/Software_I2C.h
index 65a51fad..d80bbfcb 100644
--- a/source/Core/BSP/Sequre_S60/Software_I2C.h
+++ b/source/Core/BSP/Sequre_S60/Software_I2C.h
@@ -18,12 +18,14 @@
 #define SOFT_SDA2_LOW()  HAL_GPIO_WritePin(SDA2_GPIO_Port, SDA2_Pin, GPIO_PIN_RESET)

 #define SOFT_SDA2_READ() (HAL_GPIO_ReadPin(SDA2_GPIO_Port, SDA2_Pin) == GPIO_PIN_SET ? 1 : 0)

 #define SOFT_SCL2_READ() (HAL_GPIO_ReadPin(SCL2_GPIO_Port, SCL2_Pin) == GPIO_PIN_SET ? 1 : 0)

+// clang-format off

 #define SOFT_I2C_DELAY()              \

   {                                   \

     for (int xx = 0; xx < 12; xx++) { \

       asm("nop");                     \

     }                                 \

   }

+// clang-format on

 

 #endif

 // 40 ~= 100kHz; 15 gives around 250kHz or so which is fast _and_ stable

diff --git a/source/Core/BSP/Sequre_S60/configuration.h b/source/Core/BSP/Sequre_S60/configuration.h
index 399848b2..008ce6b5 100644
--- a/source/Core/BSP/Sequre_S60/configuration.h
+++ b/source/Core/BSP/Sequre_S60/configuration.h
@@ -24,8 +24,8 @@
  * OLED Brightness
  *
  */
-#define MIN_BRIGHTNESS     0   // Min OLED brightness selectable
-#define MAX_BRIGHTNESS     100 // Max OLED brightness selectable
+#define MIN_BRIGHTNESS     1   // Min OLED brightness selectable
+#define MAX_BRIGHTNESS     101 // Max OLED brightness selectable
 #define BRIGHTNESS_STEP    25  // OLED brightness increment
 #define DEFAULT_BRIGHTNESS 25  // default OLED brightness
 
@@ -166,7 +166,6 @@
 #define MODEL_HAS_DCDC // We dont have DC/DC but have reallly fast PWM that gets us roughly the same place
 #endif                 /* S60 */
 
-
 #ifdef MODEL_S60P
 #define VOLTAGE_DIV        460 // Default divider scaler
 #define CALIBRATION_OFFSET 200 // Default adc offset in uV
@@ -176,7 +175,7 @@
 #define POWER_LIMIT_STEPS  5
 #define OP_AMP_GAIN_STAGE  536
 #define TEMP_uV_LOOKUP_S60
-#define USB_PD_VMAX 12 // Maximum voltage for PD to negotiate
+#define USB_PD_VMAX 20 // Maximum voltage for PD to negotiate
 
 #define HARDWARE_MAX_WATTAGE_X10 600
 
@@ -187,7 +186,7 @@
 
 #define OLED_128x32
 #define GPIO_VIBRATION
-#define POW_PD_EXT                1
+#define POW_PD_EXT                2
 #define USB_PD_EPR_WATTAGE        0 /*No EPR*/
 #define DEBUG_POWER_MENU_BUTTON_B 1
 #define HAS_POWER_DEBUG_MENU
@@ -197,6 +196,7 @@
 
 #define MODEL_HAS_DCDC // We dont have DC/DC but have reallly fast PWM that gets us roughly the same place
 #endif                 /* S60P */
+
 #define FLASH_LOGOADDR      (0x08000000 + (62 * 1024))
 #define SETTINGS_START_PAGE (0x08000000 + (63 * 1024))
 
diff --git a/source/Core/Drivers/FS2711.cpp b/source/Core/Drivers/FS2711.cpp
new file mode 100644
index 00000000..9b192d03
--- /dev/null
+++ b/source/Core/Drivers/FS2711.cpp
@@ -0,0 +1,239 @@
+#include "FS2711.hpp"
+#include "FS2711_defines.h"
+#include "I2CBB2.hpp"
+#include "configuration.h"
+#if POW_PD_EXT == 2
+#include "BSP.h"
+#include "cmsis_os.h"
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#ifndef USB_PD_VMAX
+#error Max PD Voltage must be defined
+#endif
+
+#define PROTOCOL_TIMEOUT 100 // ms
+
+extern int32_t powerSupplyWattageLimit;
+
+fs2711_state_t FS2711::state;
+
+inline void i2c_write(uint8_t addr, uint8_t data) { I2CBB2::Mem_Write(FS2711_ADDR, addr, &data, 1); }
+
+inline uint8_t i2c_read(uint8_t addr) {
+  uint8_t data = 0;
+  I2CBB2::Mem_Read(FS2711_ADDR, addr, &data, 1);
+  return data;
+}
+
+inline bool i2c_probe(uint8_t addr) { return I2CBB2::probe(addr); }
+
+void FS2711::start() {
+  memset(&state, 0, sizeof(fs2711_state_t));
+  state.req_pdo_num = 0xFF;
+
+  enable_protocol(false);
+  osDelay(PROTOCOL_TIMEOUT);
+  select_protocol(FS2711_PROTOCOL_PD);
+  enable_protocol(true);
+  osDelay(PROTOCOL_TIMEOUT);
+}
+
+uint8_t FS2711::selected_protocol() { return i2c_read(FS2711_REG_SELECT_PROTOCOL); }
+
+void FS2711::enable_protocol(bool enable) { i2c_write(FS2711_REG_ENABLE_PROTOCOL, enable ? FS2711_ENABLE : FS2711_DISABLE); }
+
+void FS2711::select_protocol(uint8_t protocol) { i2c_write(FS2711_REG_SELECT_PROTOCOL, protocol); }
+
+void FS2711::enable_voltage() { i2c_write(FS2711_REG_ENABLE_VOLTAGE, FS2711_ENABLE); }
+
+bool FS2711::probe() { return i2c_probe(FS2711_ADDR); }
+
+void FS2711::pdo_update() {
+  uint8_t pdo_b0 = 0, pdo_b1 = 0, pdo_b2 = 0, pdo_b3 = 0;
+
+  state.pdo_num = 0;
+  memset(state.pdo_type, 0, 7);
+  memset(state.pdo_min_volt, 0, 7);
+  memset(state.pdo_max_volt, 0, 7);
+  memset(state.pdo_max_curr, 0, 7);
+
+  for (uint8_t i = 0; i < 7; i++) {
+    pdo_b0 = i2c_read(FS2711_REG_PDO_B0 + i * 4);
+    pdo_b1 = i2c_read(FS2711_REG_PDO_B1 + i * 4);
+    pdo_b2 = i2c_read(FS2711_REG_PDO_B2 + i * 4);
+    pdo_b3 = i2c_read(FS2711_REG_PDO_B3 + i * 4);
+
+    if (pdo_b0) {
+      if ((pdo_b3 & FS2711_REG_PDO_B0) == FS2711_REG_PDO_B0) {
+        state.pdo_type[i]     = FS2711_PDO_PPS;
+        state.pdo_min_volt[i] = pdo_b1 * 100;
+        state.pdo_max_volt[i] = ((pdo_b2 >> 1) + ((pdo_b3 & 0x1) << 7)) * 100;
+        state.pdo_max_curr[i] = (pdo_b0 & 0x7F) * 50;
+      } else {
+        state.pdo_type[i]     = FS2711_PDO_FIX;
+        state.pdo_min_volt[i] = ((pdo_b1 >> 2) + ((pdo_b2 & 0xF) << 6)) * 50;
+        state.pdo_max_volt[i] = state.pdo_min_volt[i];
+        state.pdo_max_curr[i] = (pdo_b0 + ((pdo_b1 & 0x3) << 8)) * 10;
+      }
+      state.pdo_num++;
+    }
+  }
+}
+
+bool FS2711::open_pps(uint8_t pdoid, uint16_t volt, uint16_t max_curr) {
+  uint16_t wr;
+
+  if (pdoid > state.pdo_num)
+    return false;
+  if ((volt > state.pdo_max_volt[pdoid]) || (volt < state.pdo_min_volt[pdoid]))
+    return false;
+  if ((volt > state.pdo_max_volt[pdoid]) || (volt < state.pdo_min_volt[pdoid]))
+    return false;
+  if (max_curr > state.pdo_max_curr[pdoid])
+    return false;
+  if (state.pdo_type[pdoid] != FS2711_PDO_PPS)
+    return false;
+
+  if (FS2711::selected_protocol() == FS2711_PROTOCOL_PD) {
+    select_protocol(FS2711_PROTOCOL_PPS);
+    enable_protocol(true);
+  }
+
+  if (FS2711::selected_protocol() != FS2711_PROTOCOL_PPS) {
+    return false;
+  }
+
+  i2c_write(FS2711_REG_PDO_IDX, pdoid + (pdoid << 4));
+  wr = (volt - state.pdo_min_volt[pdoid]) / 20;
+  i2c_write(FS2711_PROTOCOL_PPS_CURRENT, max_curr / 50);
+
+  i2c_write(FS2711_REG_VOLT_CFG_B0, wr & 0xFF);
+  i2c_write(FS2711_REG_VOLT_CFG_B1, (wr >> 8) & 0xFF);
+  i2c_write(FS2711_REG_VOLT_CFG_B2, wr & 0xFF);
+  i2c_write(FS2711_REG_VOLT_CFG_B3, (wr >> 8) & 0xFF);
+
+  enable_voltage();
+
+  state.source_voltage    = volt;
+  state.source_current    = max_curr;
+  state.req_pdo_num       = pdoid;
+  powerSupplyWattageLimit = ((volt * max_curr) / 1000000) - 2;
+  return true;
+}
+
+bool FS2711::open_pd(uint8_t pdoid) {
+  if (pdoid >= state.pdo_num) {
+    return false;
+  }
+  if (state.pdo_type[pdoid] != FS2711_PDO_FIX) {
+    return false;
+  }
+
+  if (FS2711::selected_protocol() != FS2711_PROTOCOL_PD) {
+    return false;
+  }
+
+  i2c_write(FS2711_REG_PDO_IDX, pdoid + (pdoid << 4));
+
+  enable_voltage();
+
+  state.source_voltage = state.pdo_max_volt[pdoid];
+  state.source_current = state.pdo_max_curr[pdoid];
+  state.req_pdo_num    = pdoid;
+
+  powerSupplyWattageLimit = ((state.source_voltage * state.source_current) / 1000000) - 2;
+  return true;
+}
+
+void FS2711::negotiate() {
+  uint16_t best_voltage = 0;
+  uint16_t best_current = 0;
+  uint8_t  best_pdoid   = 0xFF;
+  bool     pps          = false;
+
+  int min_resistance_omhsx10 = 0;
+
+  // FS2711 uses mV instead of V
+  const uint16_t vmax           = USB_PD_VMAX * 1000;
+  const uint8_t  tip_resistance = getTipResistanceX10() + 5;
+
+  uint16_t pdo_min_mv = 0, pdo_max_mv = 0, pdo_max_curr = 0, pdo_type = 0;
+
+  FS2711::pdo_update();
+
+  for (int i = 0; state.pdo_num > i; i++) {
+    pdo_min_mv   = state.pdo_min_volt[i];
+    pdo_max_mv   = state.pdo_max_volt[i];
+    pdo_max_curr = state.pdo_max_curr[i];
+    pdo_type     = state.pdo_type[i];
+
+    min_resistance_omhsx10 = (pdo_max_mv / pdo_max_curr) * 10;
+
+    switch (pdo_type) {
+    case FS2711_PDO_FIX:
+      if (pdo_max_mv > 0 && vmax >= pdo_max_mv) {
+        if (min_resistance_omhsx10 <= tip_resistance) {
+          if (pdo_max_mv > best_voltage) {
+            pps          = false;
+            best_pdoid   = i;
+            best_voltage = pdo_max_mv;
+            best_current = pdo_max_curr;
+          }
+        }
+      }
+      break;
+
+    case FS2711_PDO_PPS: {
+      int ideal_mv = tip_resistance * (pdo_max_curr / 10);
+      if (ideal_mv > pdo_max_mv) {
+        ideal_mv = pdo_max_mv;
+      }
+
+      if (ideal_mv > vmax) {
+        ideal_mv = vmax;
+      }
+
+      if (ideal_mv > best_voltage) {
+        best_pdoid   = i;
+        best_voltage = ideal_mv;
+        best_current = pdo_max_curr;
+        pps          = true;
+      }
+    }
+
+    break;
+
+    default:
+      break;
+    }
+  }
+
+  if (best_pdoid != 0xFF && best_pdoid != state.req_pdo_num) {
+    if (pps) {
+      FS2711::open_pps(best_pdoid, best_voltage, best_current);
+    } else {
+      FS2711::open_pd(best_pdoid);
+    }
+  }
+}
+
+bool FS2711::has_run_selection() { return state.req_pdo_num != 0xFF; }
+
+uint16_t FS2711::source_voltage() { return state.source_voltage / 1000; }
+
+// FS2711 does current in mV so it needs to be converted to x100 intead of x1000
+uint16_t FS2711::source_currentx100() { return state.source_current / 10; }
+
+uint16_t FS2711::debug_pdo_max_voltage(uint8_t pdoid) { return state.pdo_max_volt[pdoid]; }
+
+uint16_t FS2711::debug_pdo_min_voltage(uint8_t pdoid) { return state.pdo_min_volt[pdoid]; }
+
+uint16_t FS2711::debug_pdo_source_current(uint8_t pdoid) { return state.pdo_max_curr[pdoid]; }
+
+uint16_t FS2711::debug_pdo_type(uint8_t pdoid) { return state.pdo_type[pdoid]; }
+
+fs2711_state_t FS2711::debug_get_state() { return state; }
+
+#endif
diff --git a/source/Core/Drivers/FS2711.hpp b/source/Core/Drivers/FS2711.hpp
new file mode 100644
index 00000000..9fb2ae83
--- /dev/null
+++ b/source/Core/Drivers/FS2711.hpp
@@ -0,0 +1,61 @@
+#include "configuration.h"
+#ifndef _DRIVERS_FS2711_HPP_
+#define _DRIVERS_FS2711_HPP_
+// #define POW_PD_EXT 2
+#if POW_PD_EXT == 2
+#include <stdbool.h>
+#include <stdint.h>
+
+typedef struct {
+  uint8_t  pdo_num; // Nums of USB-PD Objects max of 7
+  uint16_t source_current;
+  uint16_t source_voltage;
+  uint16_t req_pdo_num;
+  uint16_t pdo_type[7];
+  uint16_t pdo_min_volt[7];
+  uint16_t pdo_max_volt[7];
+  uint16_t pdo_max_curr[7];
+} fs2711_state_t;
+
+class FS2711 {
+public:
+  static bool probe();
+
+  static void start();
+
+  static bool open_pps(uint8_t PDOID, uint16_t volt, uint16_t max_curr);
+
+  static bool open_pd(uint8_t PDOID);
+
+  static void negotiate();
+
+  static bool has_run_selection();
+
+  static uint16_t source_voltage();
+
+  static uint16_t source_currentx100();
+
+  static uint8_t selected_protocol();
+
+  static void pdo_update();
+
+  static uint8_t        debug_protocol();
+  static uint16_t       debug_pdo_max_voltage(uint8_t pdoid);
+  static uint16_t       debug_pdo_min_voltage(uint8_t pdoid);
+  static uint16_t       debug_pdo_source_current(uint8_t pdoid);
+  static uint16_t       debug_pdo_type(uint8_t pdoid);
+  static fs2711_state_t debug_get_state();
+
+private:
+  // Internal state of IC
+  static fs2711_state_t state;
+
+  static void enable_protocol(bool enable);
+
+  static void select_protocol(uint8_t protocol);
+
+  static void enable_voltage();
+};
+
+#endif
+#endif
diff --git a/source/Core/Drivers/FS2711_defines.h b/source/Core/Drivers/FS2711_defines.h
new file mode 100644
index 00000000..d7594b75
--- /dev/null
+++ b/source/Core/Drivers/FS2711_defines.h
@@ -0,0 +1,73 @@
+#ifndef _FS2711_DEFINE_HPP_
+#define _FS2711_DEFINE_HPP_
+
+#define FS2711_WRITE_ADDR 0x5A
+#define FS2711_READ_ADDR  0x5B
+
+#define FS2711_ADDR 0x5A
+
+#define FS2711_PDO_FIX 0
+#define FS2711_PDO_PPS 1
+
+#define FS2711_MAX_5V  1
+#define FS2711_MAX_9V  3
+#define FS2711_MAX_12V 7
+#define FS2711_MAX_20V 15
+
+#define FS2711_ENABLE  0x1
+#define FS2711_DISABLE 0x2
+
+// Protocol Selection
+#define FS2711_PROTOCOL_QC2A 4
+#define FS2711_PROTOCOL_QC2B 5
+#define FS2711_PROTOCOL_QC3A 6
+#define FS2711_PROTOCOL_QC3B 7
+#define FS2711_PROTOCOL_PPS  20
+#define FS2711_PROTOCOL_PD   21
+
+#define FS2711_PROTOCOL_PPS_CURRENT 0xDE
+
+#define FS2711_PROTOCOL_QC_MAX_VOLT 0xC0
+
+#define FS2711_REG_SCAN_START      0x40 // Protocol Scan
+#define FS2711_REG_ENABLE_PROTOCOL 0x41 // Enable Protocol
+#define FS2711_REG_SELECT_PROTOCOL 0x42 // Select Protocol
+#define FS2711_REG_ENABLE_VOLTAGE  0x43 // Enable Voltage
+#define FS2711_REG_PDO_IDX         0x46 // Requests Protocol Index
+#define FS2711_REG_SWEEP           0x47 // Requests a voltage sweep?
+#define FS2711_REG_PORT_RESET      0x49 // Port Reset
+#define FS2711_REG_SYSTEM_RESET    0x4A // System Reset
+#define FS2711_REG_DPDM            0x51 // DPDM
+#define FS2711_REG_MODE_SET        0xA0 // Mode set
+#define FS2711_REG_STATE0          0xB1 // PD:A_SNK PD:A_SRC (PD:pe_ready POM:crc_success) (PD:soft_reset POM:crc_fail) (PD:hard_rest POM:resp_fail) PD:hardreset_found VIVO
+#define FS2711_REG_STATE1          0xB2 // scan_done pdo_updated vooc_recv_cmd vivo_pom_tx_finish vivo_pom_rx_finish huawei_comm_fail huawei_op_finish
+
+// Used to calculate PDO Objects
+#define FS2711_REG_PDO_B0 0xC0
+#define FS2711_REG_PDO_B1 0xC1
+#define FS2711_REG_PDO_B2 0xC2
+#define FS2711_REG_PDO_B3 0xC3
+
+#define FS2711_REG_VOLT_CFG_B0 0xF4
+#define FS2711_REG_VOLT_CFG_B1 0xF5
+#define FS2711_REG_VOLT_CFG_B2 0xF6
+#define FS2711_REG_VOLT_CFG_B3 0xF7
+
+// 0xF0 ~ 0xF1 16 bits
+#define FS2711_REG_MASK 0xF0
+// 0xF4 ~ 0xF7 32 bits
+#define FS2711_REG_PROTOCOL_VOLT 0xF4
+// 0xF8 ~ 0xFB 24 bits
+#define FS2711_REG_PROTOCOL_EXISTS 0xF8
+
+#define FS2711_SWEEP_SAW  0
+#define FS2711_SWEEP_TRI  1
+#define FS2711_SWEEP_STEP 2
+
+#define FS2711_STATE_SCAN_DONE   0x01
+#define FS2711_STATE_PDO_UPDATE  0x02
+#define FS2711_STATE_PD_SNK      0x40
+#define FS2711_STATE_PD_SRC      0x80
+#define FS2711_STATE_PD_PE_READY 0x100
+#define FS2711_STATE_DISABLE     0x800
+#endif
diff --git a/source/Core/Drivers/Font.h b/source/Core/Drivers/Font.h
index 98946449..40f15863 100644
--- a/source/Core/Drivers/Font.h
+++ b/source/Core/Drivers/Font.h
@@ -14,7 +14,7 @@
 #define FONT_12_WIDTH 12

 // THE MAIN FONTS ARE NO LONGER HERE, MOVED TO PYTHON AUTO GEN

 // THESE ARE ONLY THE SYMBOL FONTS

-

+// clang-format off

 const uint8_t ExtraFontChars[] = {

     // width = 12

     // height = 16

@@ -872,5 +872,6 @@ const uint8_t SettingsMenuIcons[][32 * 3] = {
     }

 #endif

 };

+// clang-format on

 

 #endif /* FONT_H_ */

diff --git a/source/Core/Threads/OperatingModes/ShowStartupWarnings.cpp b/source/Core/Threads/OperatingModes/ShowStartupWarnings.cpp
index 4e4a3aa7..b4f8355b 100644
--- a/source/Core/Threads/OperatingModes/ShowStartupWarnings.cpp
+++ b/source/Core/Threads/OperatingModes/ShowStartupWarnings.cpp
@@ -1,3 +1,4 @@
+#include "FS2711.hpp"
 #include "HUB238.hpp"
 #include "OperatingModes.h"
 OperatingMode showWarnings(const ButtonState buttons, guiContext *cxt) {
@@ -81,8 +82,24 @@ OperatingMode showWarnings(const ButtonState buttons, guiContext *cxt) {
       cxt->scratch_state.state1 = 4;
     }
 #else
+#if POW_PD_EXT == 2
+    if (!FS2711::probe()) {
+      if (getSettingValue(SettingsOptions::PDMissingWarningCounter) < 2) {
+        if (warnUser(translatedString(Tr->NoPowerDeliveryMessage), buttons)) {
+          cxt->scratch_state.state1 = 4;
+          nextSettingValue(SettingsOptions::PDMissingWarningCounter);
+          saveSettings();
+        }
+      } else {
+        cxt->scratch_state.state1 = 4;
+      }
+    } else {
+      cxt->scratch_state.state1 = 4;
+    }
+#else
     cxt->scratch_state.state1 = 4;
 #endif /*POW_PD_EXT==1*/
+#endif /*POW_PD_EXT==2*/
 #endif /*POW_PD*/
 
     break;
diff --git a/source/Core/Threads/OperatingModes/USBPDDebug_FS2711.cpp b/source/Core/Threads/OperatingModes/USBPDDebug_FS2711.cpp
new file mode 100644
index 00000000..6d5faea8
--- /dev/null
+++ b/source/Core/Threads/OperatingModes/USBPDDebug_FS2711.cpp
@@ -0,0 +1,81 @@
+#include "FS2711.hpp"
+#include "OperatingModes.h"
+#include "stdbool.h"
+#if POW_PD_EXT == 2
+#ifdef HAS_POWER_DEBUG_MENU
+
+OperatingMode showPDDebug(const ButtonState buttons, guiContext *cxt) {
+  // Print out the USB-PD state
+  // Basically this is like the Debug menu, but instead we want to print out the PD status
+  uint8_t     screen = 0;
+  ButtonState b;
+  for (;;) {
+    OLED::clearScreen();                               // Ensure the buffer starts clean
+    OLED::setCursor(0, 0);                             // Position the cursor at the 0,0 (top left)
+    OLED::print(SmallSymbolPDDebug, FontStyle::SMALL); // Print Title
+    OLED::setCursor(0, 8);                             // second line
+    if (screen > 7) {
+      screen = 0;
+    }
+    if (screen == 0) {
+      // Print the PD Debug state
+      OLED::print(SmallSymbolState, FontStyle::SMALL);
+      OLED::print(SmallSymbolSpace, FontStyle::SMALL);
+
+      fs2711_state_t state = FS2711::debug_get_state();
+
+      OLED::printNumber(state.pdo_num, 1, FontStyle::SMALL);
+      OLED::print(SmallSymbolSpace, FontStyle::SMALL);
+
+      //     OLED::drawHex(state.req_pdo_num, FontStyle::SMALL, 4);
+      OLED::printNumber(state.req_pdo_num > 7 ? 0 : state.req_pdo_num + 1, 1, FontStyle::SMALL, true);
+      OLED::print(SmallSymbolSpace, FontStyle::SMALL);
+      uint8_t protocol = FS2711::selected_protocol();
+      OLED::printNumber(protocol, 2, FontStyle::SMALL);
+      OLED::print(SmallSymbolSpace, FontStyle::SMALL);
+    } else {
+
+      // Print out the Proposed power options one by one
+      uint16_t max_voltage = FS2711::debug_pdo_max_voltage(screen - 1);
+      if (max_voltage == 0) {
+        screen += 1;
+      } else {
+        uint16_t min_voltage = FS2711::debug_pdo_min_voltage(screen - 1);
+        uint16_t current     = FS2711::debug_pdo_source_current(screen - 1);
+        uint16_t pdo_type    = FS2711::debug_pdo_type(screen - 1);
+
+        OLED::printNumber(screen, 1, FontStyle::SMALL, true);
+        OLED::print(SmallSymbolSpace, FontStyle::SMALL);
+
+        if (pdo_type == 1) {
+          OLED::printNumber(min_voltage / 1000, 2, FontStyle::SMALL, true);
+          OLED::print(SmallSymbolMinus, FontStyle::SMALL);
+          OLED::printNumber(max_voltage / 1000, 2, FontStyle::SMALL, false);
+        } else {
+          OLED::printNumber(max_voltage / 1000, 2, FontStyle::SMALL, true);
+        }
+        OLED::print(SmallSymbolVolts, FontStyle::SMALL);
+        OLED::print(SmallSymbolSpace, FontStyle::SMALL);
+
+        OLED::printNumber(current / 1000, 2, FontStyle::SMALL, true);
+        OLED::print(SmallSymbolDot, FontStyle::SMALL);
+        OLED::printNumber(current % 1000, 1, FontStyle::SMALL, false);
+        OLED::print(SmallSymbolAmps, FontStyle::SMALL);
+        // OLED::printNumber(currentx100 % 100, 2, FontStyle::SMALL, true);
+      }
+    }
+
+    OLED::refresh();
+    b = getButtonState();
+    if (b == BUTTON_B_SHORT) {
+      return OperatingMode::InitialisationDone;
+    } else if (b == BUTTON_F_SHORT) {
+      screen++;
+    }
+
+    GUIDelay();
+  }
+  return OperatingMode::UsbPDDebug;
+}
+#endif
+#endif
diff --git a/source/Core/Threads/POWThread.cpp b/source/Core/Threads/POWThread.cpp
index bb816ad7..27b1719b 100644
--- a/source/Core/Threads/POWThread.cpp
+++ b/source/Core/Threads/POWThread.cpp
@@ -6,6 +6,7 @@
  */

 

 #include "BSP.h"

+#include "FS2711.hpp"

 #include "FreeRTOS.h"

 #include "HUB238.hpp"

 #include "QC3.h"

@@ -14,6 +15,7 @@
 #include "cmsis_os.h"

 #include "configuration.h"

 #include "main.hpp"

+#include "stdbool.h"

 #include "stdlib.h"

 #include "task.h"

 

@@ -32,8 +34,12 @@ void startPOWTask(void const *argument __unused) {
   USBPowerDelivery::start();

   // Crank the handle at boot until we are stable and waiting for IRQ

   USBPowerDelivery::step();

-

 #endif

+#if POW_PD_EXT == 2

+  FS2711::start();

+  FS2711::negotiate();

+#endif

+

   BaseType_t res;

   for (;;) {

     res = pdFALSE;

@@ -61,6 +67,9 @@ void startPOWTask(void const *argument __unused) {
 #if POW_PD_EXT == 1

     hub238_check_negotiation();

 #endif

+#if POW_PD_EXT == 2

+    FS2711::negotiate();

+#endif

     power_check();

   }

 }

diff --git a/source/Makefile b/source/Makefile
index a888def1..f0b0f9f1 100644
--- a/source/Makefile
+++ b/source/Makefile
@@ -142,12 +142,12 @@ CPUFLAGS=-mcpu=cortex-m3  \
          -mfloat-abi=soft

 

 flash_size=62k

-ifeq ($(model),S60)

-bootldr_size=0x4400

-DEVICE_DFU_ADDRESS=0x08004400

-else 

+ifeq ($(model), S60P)

 bootldr_size=0x5000

 DEVICE_DFU_ADDRESS=0x08005000

+else

+bootldr_size=0x4400

+DEVICE_DFU_ADDRESS=0x08004400

 endif

 DEVICE_DFU_VID_PID=0x1209:0xDB42

 endif # ALL_SEQURE_MODELS