From b3c3e4e3840e45275f199385ab0add7f1f10d327 Mon Sep 17 00:00:00 2001
From: "Ben V. Brown" <Ralim@Ralimtek.com>
Date: Mon, 5 Apr 2021 14:37:50 +1000
Subject: [PATCH] Spawn PD earlier

---
 source/Core/BSP/Miniware/postRTOS.cpp         |  9 +--------
 source/Core/BSP/Pine64/postRTOS.cpp           |  4 ----
 source/Core/Drivers/FUSB302/int_n.cpp         | 12 +++++-------
 source/Core/Drivers/FUSB302/int_n.h           |  2 +-
 source/Core/Drivers/FUSB302/pdb_conf.h        |  4 ++--
 source/Core/Drivers/FUSB302/policy_engine.cpp | 10 +++++-----
 source/Core/Src/main.cpp                      |  8 +++++++-
 7 files changed, 21 insertions(+), 28 deletions(-)

diff --git a/source/Core/BSP/Miniware/postRTOS.cpp b/source/Core/BSP/Miniware/postRTOS.cpp
index 34cd8e33..2daab8b6 100644
--- a/source/Core/BSP/Miniware/postRTOS.cpp
+++ b/source/Core/BSP/Miniware/postRTOS.cpp
@@ -11,11 +11,4 @@
 #include "task.h"
 
 // Initialisation to be performed with scheduler active
-void postRToSInit() {
-#ifdef POW_PD
-  if (usb_pd_detect() == true) {
-    // Spawn all of the USB-C processors
-    fusb302_start_processing();
-  }
-#endif
-}
+void postRToSInit() {}
diff --git a/source/Core/BSP/Pine64/postRTOS.cpp b/source/Core/BSP/Pine64/postRTOS.cpp
index 0d684f3f..107d2597 100644
--- a/source/Core/BSP/Pine64/postRTOS.cpp
+++ b/source/Core/BSP/Pine64/postRTOS.cpp
@@ -19,10 +19,6 @@ void postRToSInit() {
     hall_effect_present = Si7210::init();
   }
 #endif
-#ifdef POW_PD
-  // Spawn all of the USB-C processors
-  fusb302_start_processing();
-#endif
 }
 int16_t getRawHallEffect() {
   if (hall_effect_present) {
diff --git a/source/Core/Drivers/FUSB302/int_n.cpp b/source/Core/Drivers/FUSB302/int_n.cpp
index e9d8eab5..3fc3e246 100644
--- a/source/Core/Drivers/FUSB302/int_n.cpp
+++ b/source/Core/Drivers/FUSB302/int_n.cpp
@@ -25,7 +25,7 @@
 #include "task.h"
 #include <pd.h>
 
-osThreadId          InterruptHandler::TaskHandle = NULL;
+volatile osThreadId InterruptHandler::TaskHandle = NULL;
 uint32_t            InterruptHandler::TaskBuffer[InterruptHandler::TaskStackSize];
 osStaticThreadDef_t InterruptHandler::TaskControlBlock;
 
@@ -68,11 +68,9 @@ void InterruptHandler::Thread(const void *arg) {
   }
 }
 void InterruptHandler::irqCallback() {
-  if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
-    if (TaskHandle != NULL) {
-      BaseType_t taskWoke = pdFALSE;
-      xTaskNotifyFromISR(TaskHandle, 0x01, eNotifyAction::eSetBits, &taskWoke);
-      portYIELD_FROM_ISR(taskWoke);
-    }
+  if (TaskHandle != NULL) {
+    BaseType_t taskWoke = pdFALSE;
+    xTaskNotifyFromISR(TaskHandle, 0, eNotifyAction::eNoAction, &taskWoke);
+    portYIELD_FROM_ISR(taskWoke);
   }
 }
diff --git a/source/Core/Drivers/FUSB302/int_n.h b/source/Core/Drivers/FUSB302/int_n.h
index 4b291a5e..fb5dc1d8 100644
--- a/source/Core/Drivers/FUSB302/int_n.h
+++ b/source/Core/Drivers/FUSB302/int_n.h
@@ -29,7 +29,7 @@ public:
 
 private:
   static void                Thread(const void *arg);
-  static osThreadId          TaskHandle;
+  static volatile osThreadId TaskHandle;
   static const size_t        TaskStackSize = 1536 / 3;
   static uint32_t            TaskBuffer[TaskStackSize];
   static osStaticThreadDef_t TaskControlBlock;
diff --git a/source/Core/Drivers/FUSB302/pdb_conf.h b/source/Core/Drivers/FUSB302/pdb_conf.h
index 640ea233..181eb669 100644
--- a/source/Core/Drivers/FUSB302/pdb_conf.h
+++ b/source/Core/Drivers/FUSB302/pdb_conf.h
@@ -24,8 +24,8 @@
 #define EVENT_MASK(x) (1 << x)
 
 /* PD Buddy thread priorities */
-#define PDB_PRIO_PE        (osPriorityBelowNormal)
+#define PDB_PRIO_PE        (osPriorityAboveNormal)
 #define PDB_PRIO_PRL       (osPriorityNormal)
-#define PDB_PRIO_PRL_INT_N (osPriorityAboveNormal)
+#define PDB_PRIO_PRL_INT_N (osPriorityBelowNormal)
 
 #endif /* PDB_CONF_H */
diff --git a/source/Core/Drivers/FUSB302/policy_engine.cpp b/source/Core/Drivers/FUSB302/policy_engine.cpp
index c22ff329..8943a986 100644
--- a/source/Core/Drivers/FUSB302/policy_engine.cpp
+++ b/source/Core/Drivers/FUSB302/policy_engine.cpp
@@ -622,10 +622,10 @@ void PolicyEngine::PPSTimerCallback() {
   if (PPSTimerEnabled && state == policy_engine_state::PESinkReady) {
     // I believe even once per second is totally fine, but leaning on faster since everything seems cool with faster
     // Have seen everything from 10ms to 1 second :D
-    // if ((xTaskGetTickCount() - PPSTimeLastEvent) > (100)) {
-    // Send a new PPS message
-    PolicyEngine::notify(Notifications::PDB_EVT_PE_PPS_REQUEST);
-    PPSTimeLastEvent = xTaskGetTickCount();
-    // }
+    if ((xTaskGetTickCount() - PPSTimeLastEvent) > (TICKS_SECOND)) {
+      // Send a new PPS message
+      PolicyEngine::notify(Notifications::PDB_EVT_PE_PPS_REQUEST);
+      PPSTimeLastEvent = xTaskGetTickCount();
+    }
   }
 }
diff --git a/source/Core/Src/main.cpp b/source/Core/Src/main.cpp
index 389005ca..dc881a60 100644
--- a/source/Core/Src/main.cpp
+++ b/source/Core/Src/main.cpp
@@ -9,6 +9,7 @@
 #include "Settings.h"
 #include "cmsis_os.h"
 #include <MMA8652FC.hpp>
+#include <fusbpd.h>
 #include <main.hpp>
 #include <power.hpp>
 uint8_t DetectedAccelerometerVersion = 0;
@@ -61,7 +62,12 @@ int main(void) {
   osThreadStaticDef(MOVTask, startMOVTask, osPriorityNormal, 0, MOVTaskStackSize, MOVTaskBuffer, &MOVTaskControlBlock);
   MOVTaskHandle = osThreadCreate(osThread(MOVTask), NULL);
   resetWatchdog();
-
+#ifdef POW_PD
+  if (usb_pd_detect() == true) {
+    // Spawn all of the USB-C processors
+    fusb302_start_processing();
+  }
+#endif
   /* Start scheduler */
   osKernelStart();
   /* We should never get here as control is now taken by the scheduler */