USB PD EPR Basics

source/Core/BSP/BSP_Power.h

@@ -16,6 +16,9 @@ extern "C" {
 // Can be used to check any details for the power system
 void power_check();
 
+// Return the tip resistance in x10 ohms (8.5 -> 85)
+uint8_t getTipResistanceX10();
+
 #ifdef __cplusplus
 }
 #endif

source/Core/BSP/MHP30/Power.cpp

@@ -17,3 +17,5 @@ void power_check() {
 }
 
 bool getIsPoweredByDCIN() { return false; }
+
+uint8_t getTipResistanceX10() { return TIP_RESISTANCE; }

source/Core/BSP/Miniware/BSP.cpp

@@ -288,3 +288,5 @@ uint64_t getDeviceID() {
   //
   return HAL_GetUIDw0() | ((uint64_t)HAL_GetUIDw1() << 32);
 }
+
+uint8_t getTipResistanceX10() { return TIP_RESISTANCE; }

source/Core/BSP/Pine64/BSP.cpp

@@ -93,3 +93,5 @@ void setStatusLED(const enum StatusLED state) {}
 
 uint8_t  preStartChecks() { return 0; }
 uint64_t getDeviceID() { return dbg_id_get(); }
+
+uint8_t getTipResistanceX10() { return TIP_RESISTANCE; }

source/Core/Drivers/USBPD.cpp

@@ -12,9 +12,6 @@
 #ifndef USB_PD_VMAX
 #error Max PD Voltage must be defined
 #endif
-#ifndef TIP_RESISTANCE
-#error Tip resistance must be defined
-#endif
 
 void ms_delay(uint32_t delayms) {
   // Convert ms -> ticks
@@ -28,8 +25,9 @@ uint32_t get_ms_timestamp() {
 }
 bool         pdbs_dpm_evaluate_capability(const pd_msg *capabilities, pd_msg *request);
 void         pdbs_dpm_get_sink_capability(pd_msg *cap, const bool isPD3);
+bool         EPREvaluateCapabilityFunc(const epr_pd_msg *capabilities, pd_msg *request);
 FUSB302      fusb((0x22 << 1), fusb_read_buf, fusb_write_buf, ms_delay); // Create FUSB driver
-PolicyEngine pe(fusb, get_ms_timestamp, ms_delay, pdbs_dpm_get_sink_capability, pdbs_dpm_evaluate_capability);
+PolicyEngine pe(fusb, get_ms_timestamp, ms_delay, pdbs_dpm_get_sink_capability, pdbs_dpm_evaluate_capability, EPREvaluateCapabilityFunc, 140);
 int          USBPowerDelivery::detectionState = 0;
 uint16_t     requested_voltage_mv             = 0;
 
@@ -48,10 +46,10 @@ void    USBPowerDelivery::IRQOccured() { pe.IRQOccured(); }
 bool    USBPowerDelivery::negotiationHasWorked() { return pe.pdHasNegotiated(); }
 uint8_t USBPowerDelivery::getStateNumber() { return pe.currentStateCode(true); }
 void    USBPowerDelivery::step() {
-  while (pe.thread()) {}
+     while (pe.thread()) {}
 }
 
-void USBPowerDelivery::PPSTimerCallback() { pe.PPSTimerCallback(); }
+void USBPowerDelivery::PPSTimerCallback() { pe.TimersCallback(); }
 bool USBPowerDelivery::negotiationComplete() {
   if (!fusbPresent()) {
     return true;
@@ -72,6 +70,10 @@ bool USBPowerDelivery::isVBUSConnected() {
   if (state) {
     return state == 1;
   }
+  // Dont run if we havent negotiated
+  if (!negotiationComplete()) {
+    return true;
+  }
   if (fusb.isVBUSConnected()) {
     state = 1;
     return true;
@@ -80,84 +82,191 @@ bool USBPowerDelivery::isVBUSConnected() {
     return false;
   }
 }
-pd_msg  lastCapabilities;
-pd_msg *USBPowerDelivery::getLastSeenCapabilities() { return &lastCapabilities; }
+uint32_t  lastCapabilities[11];
+uint32_t *USBPowerDelivery::getLastSeenCapabilities() { return lastCapabilities; }
 
-bool pdbs_dpm_evaluate_capability(const pd_msg *capabilities, pd_msg *request) {
-  memcpy(&lastCapabilities, capabilities, sizeof(pd_msg));
-  /* Get the number of PDOs */
-  uint8_t numobj = PD_NUMOBJ_GET(capabilities);
+#ifdef POW_EPR
+static unsigned int sqrtI(unsigned long sqrtArg) {
+  unsigned int  answer, x;
+  unsigned long temp;
+  if (sqrtArg == 0)
+    return 0; // undefined result
+  if (sqrtArg == 1)
+    return 1;                           // identity
+  answer = 0;                           // integer square root
+  for (x = 0x8000; x > 0; x = x >> 1) { // 16 bit shift
+    answer |= x;                        // possible bit in root
+    temp = answer * answer;             //
+    if (temp == sqrtArg)
+      break; // exact, found it
+    if (temp > sqrtArg)
+      answer ^= x; // too large, reverse bit
+  }
+  return answer; // approximate root
+}
+#endif
 
-  /* Make sure we have configuration */
-  /* Look at the PDOs to see if one matches our desires */
-  // Look against USB_PD_Desired_Levels to select in order of preference
-  uint8_t bestIndex        = 0xFF;
-  int     bestIndexVoltage = 0;
-  int     bestIndexCurrent = 0;
-  bool    bestIsPPS        = false;
-  powerSupplyWattageLimit  = 0;
-  for (uint8_t i = 0; i < numobj; i++) {
-    /* If we have a fixed PDO, its V equals our desired V, and its I is
-     * at least our desired I */
-    if ((capabilities->obj[i] & PD_PDO_TYPE) == PD_PDO_TYPE_FIXED) {
+// parseCapabilitiesArray returns true if a valid capability was found
+// caps is the array of capabilities objects
+// best* are output references
+bool parseCapabilitiesArray(const uint8_t numCaps, uint8_t *bestIndex, uint16_t *bestVoltage, uint16_t *bestCurrent, bool *bestIsPPS, bool *bestIsAVO) {
+  // Walk the given capabilities array; and select the best option
+  // Given assumption of fixed tip resistance; this can be simplified to highest voltage selection
+  *bestIndex   = 0xFF; // Mark unselected
+  *bestVoltage = 5000; // Default 5V
+
+  // Fudge of 0.5 ohms to round up a little to account for us always having off periods in PWM
+  uint8_t tipResistance = getTipResistanceX10() + 5;
+#ifdef MODEL_HAS_DCDC
+  // If this device has step down DC/DC inductor to smooth out current spikes
+  // We can instead ignore resistance and go for max voltage we can accept; and rely on the DC/DC regulation to keep under current limit
+  tipResistance = 255; // (Push to 25.5 ohms to effectively disable this check)
+#endif
+
+  for (uint8_t i = 0; i < numCaps; i++) {
+    if ((lastCapabilities[i] & PD_PDO_TYPE) == PD_PDO_TYPE_FIXED) {
       // This is a fixed PDO entry
       // Evaluate if it can produve sufficient current based on the TIP_RESISTANCE (ohms*10)
       // V=I*R -> V/I => minimum resistance, if our tip resistance is >= this then we can use this supply
 
-      int voltage_mv             = PD_PDV2MV(PD_PDO_SRC_FIXED_VOLTAGE_GET(capabilities->obj[i])); // voltage in mV units
-      int current_a_x100         = PD_PDO_SRC_FIXED_CURRENT_GET(capabilities->obj[i]);            // current in 10mA units
+      int voltage_mv             = PD_PDV2MV(PD_PDO_SRC_FIXED_VOLTAGE_GET(lastCapabilities[i])); // voltage in mV units
+      int current_a_x100         = PD_PDO_SRC_FIXED_CURRENT_GET(lastCapabilities[i]);            // current in 10mA units
       int min_resistance_ohmsx10 = voltage_mv / current_a_x100;
       if (voltage_mv <= (USB_PD_VMAX * 1000)) {
-#ifdef MODEL_HAS_DCDC
-        // If this device has step down DC/DC inductor to smooth out current spikes
-        // We can instead ignore resistance and go for max voltage we can accept
-        min_resistance_ohmsx10 = TIP_RESISTANCE;
-#endif
-        // Fudge of 0.5 ohms to round up a little to account for other losses
-        if (min_resistance_ohmsx10 <= (TIP_RESISTANCE + 5)) {
+        if (min_resistance_ohmsx10 <= tipResistance) {
           // This is a valid power source we can select as
-          if ((voltage_mv > bestIndexVoltage) || bestIndex == 0xFF) {
+          if (voltage_mv > *bestVoltage) {
             // Higher voltage and valid, select this instead
-            bestIndex        = i;
-            bestIndexVoltage = voltage_mv;
-            bestIndexCurrent = current_a_x100;
-            bestIsPPS        = false;
-#ifdef MODEL_HAS_DCDC
-            // set limiter for wattage
-            powerSupplyWattageLimit = ((voltage_mv * current_a_x100) / 100 / 1000);
-#endif
+            *bestIndex   = i;
+            *bestVoltage = voltage_mv;
+            *bestCurrent = current_a_x100;
+            *bestIsPPS   = false;
+            *bestIsAVO   = false;
           }
         }
       }
-    } else if ((capabilities->obj[i] & PD_PDO_TYPE) == PD_PDO_TYPE_AUGMENTED && (capabilities->obj[i] & PD_APDO_TYPE) == PD_APDO_TYPE_PPS) {
+    } else if ((lastCapabilities[i] & PD_PDO_TYPE) == PD_PDO_TYPE_AUGMENTED && (((lastCapabilities[i] & PD_APDO_TYPE) == PD_APDO_TYPE_PPS))) {
       // If this is a PPS slot, calculate the max voltage in the PPS range that can we be used and maintain
-      uint16_t max_voltage = PD_PAV2MV(PD_APDO_PPS_MAX_VOLTAGE_GET(capabilities->obj[i]));
-      // uint16_t min_voltage = PD_PAV2MV(PD_APDO_PPS_MIN_VOLTAGE_GET(capabilities->obj[i]));
-      uint16_t max_current = PD_PAI2CA(PD_APDO_PPS_CURRENT_GET(capabilities->obj[i])); // max current in 10mA units
+      uint16_t max_voltage = PD_PAV2MV(PD_APDO_PPS_MAX_VOLTAGE_GET(lastCapabilities[i]));
+      // uint16_t min_voltage = PD_PAV2MV(PD_APDO_PPS_MIN_VOLTAGE_GET(lastCapabilities[i]));
+      uint16_t max_current = PD_PAI2CA(PD_APDO_PPS_CURRENT_GET(lastCapabilities[i])); // max current in 10mA units
       // Using the current and tip resistance, calculate the ideal max voltage
       // if this is range, then we will work with this voltage
       // if this is not in range; then max_voltage can be safely selected
-      int ideal_voltage_mv = (TIP_RESISTANCE * max_current);
+      int ideal_voltage_mv = (tipResistance * max_current);
       if (ideal_voltage_mv > max_voltage) {
-        ideal_voltage_mv = max_voltage; // constrain
+        ideal_voltage_mv = max_voltage; // constrain to what this PDO offers
+      }
+      if (ideal_voltage_mv > 20000) {
+        ideal_voltage_mv = 20000; // Limit to 20V as some advertise 21 but are not stable at 21
       }
       if (ideal_voltage_mv > (USB_PD_VMAX * 1000)) {
-        ideal_voltage_mv = (USB_PD_VMAX * 1000); // constrain to model max
+        ideal_voltage_mv = (USB_PD_VMAX * 1000); // constrain to model max voltage safe to select
       }
-      if (ideal_voltage_mv > bestIndexVoltage || bestIndex == 0xFF) {
-        bestIndex        = i;
-        bestIndexVoltage = ideal_voltage_mv;
-        bestIndexCurrent = max_current;
-        bestIsPPS        = true;
-#ifdef MODEL_HAS_DCDC
-        // set limiter for wattage
-        powerSupplyWattageLimit = ((ideal_voltage_mv * max_current) / 100 / 1000);
-#endif
+      if (ideal_voltage_mv > *bestVoltage) {
+        *bestIndex   = i;
+        *bestVoltage = ideal_voltage_mv;
+        *bestCurrent = max_current;
+        *bestIsPPS   = true;
+        *bestIsAVO   = false;
+      }
+    }
+#ifdef POW_EPR
+    else if ((lastCapabilities[i] & PD_PDO_TYPE) == PD_PDO_TYPE_AUGMENTED && (((lastCapabilities[i] & PD_APDO_TYPE) == PD_APDO_TYPE_AVS))) {
+      *bestIsAVO           = true;
+      uint16_t max_voltage = PD_PAV2MV(PD_APDO_AVS_MAX_VOLTAGE_GET(lastCapabilities[i]));
+      uint8_t  max_wattage = PD_APDO_AVS_MAX_POWER_GET(lastCapabilities[i]);
+
+      // W = v^2/tip_resistance => Wattage*tip_resistance == Max_voltage^2
+      auto ideal_max_voltage = sqrtI((max_wattage * tipResistance) / 10) * 1000;
+      if (ideal_max_voltage > (USB_PD_VMAX * 1000)) {
+        ideal_max_voltage = (USB_PD_VMAX * 1000); // constrain to model max voltage safe to select
+      }
+      if (ideal_max_voltage > (max_voltage)) {
+        ideal_max_voltage = (max_voltage); // constrain to model max voltage safe to select
+      }
+      auto operating_current = (ideal_max_voltage / tipResistance); // Current in centiamps
+
+      if (ideal_max_voltage > *bestVoltage) {
+        *bestIndex   = i;
+        *bestVoltage = ideal_max_voltage;
+        *bestCurrent = operating_current;
+        *bestIsAVO   = true;
       }
     }
+#endif
+  }
+  // Now that the best index is known, set the current values
+  return *bestIndex != 0xFF; // have we selected one
+}
+
+bool EPREvaluateCapabilityFunc(const epr_pd_msg *capabilities, pd_msg *request) {
+#ifdef POW_EPR
+  // Select any EPR slots up to USB_PD_VMAX
+  memset(lastCapabilities, 0, sizeof(lastCapabilities));
+  memcpy(lastCapabilities, capabilities->obj, sizeof(lastCapabilities));
+  // PDO slots 1-7 shall be the standard PDO's
+  // PDO slots 8-11 shall be the >20V slots
+  uint8_t  numobj           = 11;
+  uint8_t  bestIndex        = 0xFF;
+  uint16_t bestIndexVoltage = 0;
+  uint16_t bestIndexCurrent = 0;
+  bool     bestIsPPS        = false;
+  bool     bestIsAVO        = false;
+
+  if (parseCapabilitiesArray(numobj, &bestIndex, &bestIndexVoltage, &bestIndexCurrent, &bestIsPPS, &bestIsAVO)) {
+    /* We got what we wanted, so build a request for that */
+    request->hdr    = PD_MSGTYPE_EPR_REQUEST | PD_NUMOBJ(2);
+    request->obj[1] = lastCapabilities[bestIndex]; // Copy PDO into slot 2
+
+    if (bestIsAVO) {
+      request->obj[0] = PD_RDO_PROG_CURRENT_SET(PD_CA2PAI(bestIndexCurrent)) | PD_RDO_PROG_VOLTAGE_SET(PD_MV2APS(bestIndexVoltage)) | PD_RDO_NO_USB_SUSPEND | PD_RDO_OBJPOS_SET(bestIndex + 1);
+    } else if (bestIsPPS) {
+      request->obj[0] = PD_RDO_PROG_CURRENT_SET(PD_CA2PAI(bestIndexCurrent)) | PD_RDO_PROG_VOLTAGE_SET(PD_MV2PRV(bestIndexVoltage)) | PD_RDO_NO_USB_SUSPEND | PD_RDO_OBJPOS_SET(bestIndex + 1);
+    } else {
+      request->obj[0] = PD_RDO_FV_MAX_CURRENT_SET(bestIndexCurrent) | PD_RDO_FV_CURRENT_SET(bestIndexCurrent) | PD_RDO_NO_USB_SUSPEND | PD_RDO_OBJPOS_SET(bestIndex + 1);
+    }
+    request->obj[0] |= PD_RDO_EPR_CAPABLE;
+
+    // We dont do usb
+    // request->obj[0] |= PD_RDO_USB_COMMS;
+
+    /* Update requested voltage */
+    requested_voltage_mv    = bestIndexVoltage;
+    powerSupplyWattageLimit = bestIndexVoltage * bestIndexCurrent / 100 / 1000; // Set watts for limit from PSU limit
+
+  } else {
+    /* Nothing matched (or no configuration), so get 5 V at low current */
+    request->hdr    = PD_MSGTYPE_EPR_REQUEST | PD_NUMOBJ(2);
+    request->obj[1] = lastCapabilities[0];
+    request->obj[0] = PD_RDO_FV_MAX_CURRENT_SET(100) | PD_RDO_FV_CURRENT_SET(100) | PD_RDO_NO_USB_SUSPEND | PD_RDO_OBJPOS_SET(1);
+    // We dont do usb
+    // request->obj[0] |= PD_RDO_USB_COMMS;
+
+    /* Update requested voltage */
+    requested_voltage_mv = 5000;
   }
+  return true;
+#endif
+  return false;
+}
+
+bool pdbs_dpm_evaluate_capability(const pd_msg *capabilities, pd_msg *request) {
+  memset(lastCapabilities, 0, sizeof(lastCapabilities));
+  memcpy(lastCapabilities, capabilities->obj, sizeof(uint32_t) * 7);
+  /* Get the number of PDOs */
+  uint8_t numobj = PD_NUMOBJ_GET(capabilities);
+
+  /* Make sure we have configuration */
+  /* Look at the PDOs to see if one matches our desires */
+  // Look against USB_PD_Desired_Levels to select in order of preference
+  uint8_t  bestIndex        = 0xFF;
+  uint16_t bestIndexVoltage = 0;
+  uint16_t bestIndexCurrent = 0;
+  bool     bestIsPPS        = false;
+  bool     bestIsAVO        = false;
 
-  if (bestIndex != 0xFF) {
+  if (parseCapabilitiesArray(numobj, &bestIndex, &bestIndexVoltage, &bestIndexCurrent, &bestIsPPS, &bestIsAVO)) {
     /* We got what we wanted, so build a request for that */
     request->hdr = PD_MSGTYPE_REQUEST | PD_NUMOBJ(1);
     if (bestIsPPS) {
@@ -167,14 +276,18 @@ bool pdbs_dpm_evaluate_capability(const pd_msg *capabilities, pd_msg *request) {
     }
     // We dont do usb
     // request->obj[0] |= PD_RDO_USB_COMMS;
+#ifdef POW_EPR
+    request->obj[0] |= PD_RDO_EPR_CAPABLE;
+#endif
 
     /* Update requested voltage */
-    requested_voltage_mv = bestIndexVoltage;
+    requested_voltage_mv    = bestIndexVoltage;
+    powerSupplyWattageLimit = bestIndexVoltage * bestIndexCurrent / 100 / 1000; // Set watts for limit from PSU limit
 
   } else {
     /* Nothing matched (or no configuration), so get 5 V at low current */
     request->hdr    = PD_MSGTYPE_REQUEST | PD_NUMOBJ(1);
-    request->obj[0] = PD_RDO_FV_MAX_CURRENT_SET(DPM_MIN_CURRENT) | PD_RDO_FV_CURRENT_SET(DPM_MIN_CURRENT) | PD_RDO_NO_USB_SUSPEND | PD_RDO_OBJPOS_SET(1);
+    request->obj[0] = PD_RDO_FV_MAX_CURRENT_SET(100) | PD_RDO_FV_CURRENT_SET(100) | PD_RDO_NO_USB_SUSPEND | PD_RDO_OBJPOS_SET(1);
     // We dont do usb
     // request->obj[0] |= PD_RDO_USB_COMMS;
 
@@ -199,7 +312,7 @@ void pdbs_dpm_get_sink_capability(pd_msg *cap, const bool isPD3) {
   // if (requested_voltage_mv != 5000) {
   //   voltage = requested_voltage_mv;
   // }
-  // uint16_t current = (voltage) / TIP_RESISTANCE; // In centi-amps
+  // uint16_t current = (voltage) / getTipResistanceX10(); // In centi-amps
 
   // /* Add a PDO for the desired power. */
   // cap->obj[numobj++] = PD_PDO_TYPE_FIXED | PD_PDO_SNK_FIXED_VOLTAGE_SET(PD_MV2PDV(voltage)) | PD_PDO_SNK_FIXED_CURRENT_SET(current);

source/Core/Drivers/USBPD.h

@@ -2,25 +2,24 @@
 #ifndef DRIVERS_USBPD_H_
 #define DRIVERS_USBPD_H_
 #include "configuration.h"
-#include "pdb_msg.h"
 #include <stdbool.h>
 #include <stdint.h>
 
 #ifdef __cplusplus
 #if POW_PD
 class USBPowerDelivery {
 public:
-  static bool    start();                   // Start the PD stack
-  static bool    negotiationComplete();     // Has negotiation completed to a voltage > 5v
-  static bool    negotiationInProgress();   // Is negotiation ongoing
-  static bool    fusbPresent();             // Is the FUSB302 present on the bus
-  static void    PPSTimerCallback();        // PPS Timer
-  static void    IRQOccured();              // Thread callback that an irq occured
-  static void    step();                    // Iterate the step machine
-  static bool    negotiationHasWorked();    // Has PD negotiation worked (are we in a PD contract)
-  static uint8_t getStateNumber();          // Debugging - Get the internal state number
-  static bool    isVBUSConnected();         // Is the VBus pin connected on the FUSB302
-  static pd_msg *getLastSeenCapabilities(); // returns pointer to the last seen capabilities from the powersource
+  static bool      start();                   // Start the PD stack
+  static bool      negotiationComplete();     // Has negotiation completed to a voltage > 5v
+  static bool      negotiationInProgress();   // Is negotiation ongoing
+  static bool      fusbPresent();             // Is the FUSB302 present on the bus
+  static void      PPSTimerCallback();        // PPS Timer
+  static void      IRQOccured();              // Thread callback that an irq occured
+  static void      step();                    // Iterate the step machine
+  static bool      negotiationHasWorked();    // Has PD negotiation worked (are we in a PD contract)
+  static uint8_t   getStateNumber();          // Debugging - Get the internal state number
+  static bool      isVBUSConnected();         // Is the VBus pin connected on the FUSB302
+  static uint32_t *getLastSeenCapabilities(); // returns pointer to the last seen capabilities from the powersource
 private:
   //
   static int detectionState;

source/Core/Src/power.cpp

@@ -49,7 +49,7 @@ uint32_t availableW10(uint8_t sample) {
   //				R = R*10
   // P therefore is in V^2*100/R*10 = W*10.
   uint32_t v                 = getInputVoltageX10(getSettingValue(SettingsOptions::VoltageDiv), sample); // 100 = 10v
-  uint32_t availableWattsX10 = (v * v) / TIP_RESISTANCE;
+  uint32_t availableWattsX10 = (v * v) / getTipResistanceX10();
   // However, 100% duty cycle is not possible as there is a dead time while the ADC takes a reading
   // Therefore need to scale available milliwats by this