Você não pode selecionar mais de 25 tópicos Os tópicos devem começar com uma letra ou um número, podem incluir traços ('-') e podem ter até 35 caracteres.
 
 
 
 
 
 

6245 linhas
200 KiB

  1. #include "plsr.h"
  2. #include "plsr_internal.h"
  3. #include "plsr_planner.h"
  4. #include "plsr_platform.h"
  5. #include <stddef.h>
  6. #include <string.h>
  7. #if defined(__ICCARM__)
  8. #include <intrinsics.h>
  9. #endif
  10. #define PLSR_COMMON_FIRST_ADDRESS (0x1000U)
  11. #define PLSR_COMMON_LAST_ADDRESS (0x10FFU)
  12. #define PLSR_SEGMENT_FIRST_ADDRESS (0x1100U)
  13. #define PLSR_SEGMENT_STRIDE (0x0010U)
  14. #define PLSR_SEGMENT_DEFINED_WORDS (8U)
  15. #define PLSR_WAIT_TIME (0U)
  16. #define PLSR_WAIT_SIGNAL (1U)
  17. #define PLSR_ACT_TIME (2U)
  18. #define PLSR_EXT_SIGNAL (3U)
  19. #define PLSR_EXT_OR_COMPLETE (4U)
  20. #define PLSR_SEND_COMPLETE (0U)
  21. #define PLSR_SEND_SUBSEQUENT (1U)
  22. #define PLSR_POSITION_RELATIVE (0U)
  23. #define PLSR_POSITION_ABSOLUTE (1U)
  24. #define PLSR_COMMAND_START (0x0001U)
  25. #define PLSR_COMMAND_STOP (0x0002U)
  26. #define PLSR_COMMAND_CLEAR (0x0004U)
  27. #define PLSR_DIAG_FLAG_MONITORING (0x0001U)
  28. #define PLSR_DIAG_FLAG_COUNT_CHECKED (0x0002U)
  29. #define PLSR_DIAG_FLAG_COUNT_PASS (0x0004U)
  30. #define PLSR_DIAG_FLAG_FREQUENCY_CHECKED (0x0008U)
  31. #define PLSR_DIAG_FLAG_FREQUENCY_PASS (0x0010U)
  32. #define PLSR_DIAG_FLAG_CURVE_CHECKED (0x0020U)
  33. #define PLSR_DIAG_FLAG_CURVE_PASS (0x0040U)
  34. #define PLSR_DIAG_FLAG_FAULT_LATCHED (0x0080U)
  35. #define PLSR_DIAG_REASON_NONE (0U)
  36. #define PLSR_DIAG_REASON_COUNT (1U)
  37. #define PLSR_DIAG_REASON_FREQUENCY (2U)
  38. #define PLSR_DIAG_REASON_CURVE (3U)
  39. #define PLSR_DIAG_REASON_INVALID_SAMPLE (4U)
  40. #define PLSR_CONFIG_SAVE_DELAY_MS (1000U)
  41. #define PLSR_POSITION_CHECKPOINT_MS (10U)
  42. #define PLSR_PROFILE_QUEUE_CAPACITY (1024U)
  43. #define PLSR_PROFILE_QUEUE_MASK (PLSR_PROFILE_QUEUE_CAPACITY - 1U)
  44. #if ((PLSR_PROFILE_QUEUE_CAPACITY == 0U) \
  45. || ((PLSR_PROFILE_QUEUE_CAPACITY \
  46. & (PLSR_PROFILE_QUEUE_CAPACITY - 1U)) != 0U) \
  47. || (PLSR_PROFILE_QUEUE_CAPACITY > 65535U))
  48. #error "PLSR profile queue capacity must be a power of two not exceeding 65535"
  49. #endif
  50. /* A profile item can represent only one pulse. At the 100 kHz limit these
  51. thresholds provide 4 ms of startup data, refill toward 8 ms, and cap each
  52. 1 ms producer pass at 2 ms worth of newly planned items. */
  53. #define PLSR_PROFILE_STARTUP_TARGET (400U)
  54. #define PLSR_PROFILE_STARTUP_BUDGET (400U)
  55. #define PLSR_PROFILE_REFILL_TARGET (800U)
  56. #define PLSR_PROFILE_REFILL_BUDGET (200U)
  57. #define PLSR_TIMED_START_BUILD_BUDGET (32U)
  58. #define PLSR_TIMED_START_READY_ITEMS PLSR_PROFILE_STARTUP_BUDGET
  59. #define PLSR_PROFILE_LOW_WATER (100U)
  60. #define PLSR_PROFILE_LOW_WATER_CLEAR (200U)
  61. #define PLSR_UNDERRUN_HOLD_TIME_US (2000UL)
  62. #define PLSR_UNDERRUN_HOLD_MAX_PULSES (200UL)
  63. #define PLSR_HANDOFF_WARMUP_ITEMS (8U)
  64. #define PLSR_REPLAN_PREFIX_ITEMS (512U)
  65. #define PLSR_REPLAN_PREFIX_TIME_US (5000ULL)
  66. #define PLSR_REPLAN_BUILD_ITEMS (400U)
  67. #define PLSR_BOUNDARY_EVENT_CAPACITY (2U)
  68. #if ((PLSR_BOUNDARY_EVENT_CAPACITY == 0U) \
  69. || ((PLSR_BOUNDARY_EVENT_CAPACITY \
  70. & (PLSR_BOUNDARY_EVENT_CAPACITY - 1U)) != 0U) \
  71. || (PLSR_BOUNDARY_EVENT_CAPACITY > 127U))
  72. #error "PLSR boundary event capacity must be a power of two up to 127"
  73. #endif
  74. #define PLSR_QUEUE_DIAG_LOW_WATER (0x0001U)
  75. #define PLSR_QUEUE_DIAG_UNDERRUN (0x0002U)
  76. #define PLSR_QUEUE_DIAG_PLANNER_CLIPPED (0x0004U)
  77. #define PLSR_RAMP_POLL_RESERVE_AREA_DIVISOR (20ULL)
  78. /* 功能诊断(计数/频率/曲线检查)总开关。
  79. * 0 = 全部关闭:诊断采集、结算与锁定均不执行,Modbus 诊断寄存器保持全 0,
  80. * 诊断控制字写入仍被接受(无操作)。运动核心先行跑通;曲线校准阶段再置 1。 */
  81. #define PLSR_DIAGNOSTIC_ENABLED (0U)
  82. /* 计数错误停机检查:暂时关闭。counted 流计数同步与执行器重构对齐期间,
  83. * 实际输出与预期的偏差只做数据钳制(delta=remaining),不再进 PLSR_ERROR_COUNT。 */
  84. #define PLSR_COUNT_ERROR_CHECK_ENABLED (0U)
  85. typedef struct
  86. {
  87. uint32_t fromHz;
  88. uint32_t toHz;
  89. uint32_t durationMs;
  90. uint32_t elapsedMs;
  91. uint8_t active;
  92. } PLSR_RAMP;
  93. typedef struct
  94. {
  95. PLSR_PLANNER_CONTEXT planner;
  96. PLSR_STREAM_ITEM pendingItem;
  97. uint32_t startHz;
  98. uint32_t peakHz;
  99. uint32_t endHz;
  100. uint32_t pulseCount;
  101. uint32_t entryPulses;
  102. uint32_t steadyPulses;
  103. uint32_t exitPulses;
  104. volatile uint32_t nextPeriod;
  105. uint32_t pendingRepeats;
  106. uint32_t lastRampFrequencyHz;
  107. volatile uint8_t active;
  108. } PLSR_SHORT_PROFILE;
  109. typedef struct
  110. {
  111. PLSR_PLATFORM_TIMER_SETTING setting;
  112. uint32_t requestedFrequencyHz;
  113. uint32_t repeatCount;
  114. uint8_t startsNextSegment;
  115. } PLSR_PROFILE_ENTRY;
  116. typedef struct
  117. {
  118. PLSR_PROFILE_ENTRY entries[PLSR_PROFILE_QUEUE_CAPACITY];
  119. PLSR_SHORT_PROFILE producerProfile;
  120. volatile uint32_t readIndex;
  121. volatile uint32_t writeIndex;
  122. volatile uint32_t generation;
  123. uint32_t producerEpoch;
  124. volatile uint32_t repeatRemaining;
  125. volatile uint32_t underrunDebtPulses;
  126. PLSR_PLATFORM_TIMER_SETTING lastSetting;
  127. uint8_t producerSegment;
  128. uint8_t preparedHandoffBank;
  129. volatile uint8_t haveLastSetting;
  130. volatile uint8_t active;
  131. volatile uint8_t generatorComplete;
  132. } PLSR_PROFILE_QUEUE;
  133. /* WAIT_TIME and WAIT_SIGNAL boundaries are cold starts, but their planner
  134. work is predictable while the source segment is still running or waiting.
  135. Build that next stream in the inactive queue bank and publish it only after
  136. the startup horizon is ready. The release path then has no planner loop. */
  137. typedef struct
  138. {
  139. PLSR_PROFILE_ENTRY firstRun;
  140. uint64_t magnitude;
  141. uint32_t sourceEpoch;
  142. uint32_t queueGeneration;
  143. uint16_t generatedItems;
  144. uint8_t sourceSegment;
  145. uint8_t nextSegment;
  146. uint8_t positive;
  147. uint8_t directionLevel;
  148. uint8_t directionChanged;
  149. uint8_t queueBank;
  150. volatile uint8_t building;
  151. volatile uint8_t valid;
  152. volatile uint8_t pendingActivation;
  153. } PLSR_TIMED_START;
  154. typedef struct
  155. {
  156. uint64_t magnitude;
  157. uint32_t firstFrequencyHz;
  158. uint32_t secondFrequencyHz;
  159. uint32_t firstRequestedFrequencyHz;
  160. uint32_t secondRequestedFrequencyHz;
  161. uint32_t firstRepeatCount;
  162. uint32_t secondRepeatCount;
  163. PLSR_PLATFORM_TIMER_SETTING firstSetting;
  164. PLSR_PLATFORM_TIMER_SETTING secondSetting;
  165. PLSR_PROFILE_ENTRY warmup[PLSR_HANDOFF_WARMUP_ITEMS];
  166. PLSR_SHORT_PROFILE profile;
  167. uint8_t warmupCount;
  168. uint8_t nextSegment;
  169. uint8_t positive;
  170. volatile uint8_t valid;
  171. } PLSR_HANDOFF_PLAN;
  172. typedef struct
  173. {
  174. uint64_t magnitude;
  175. uint32_t firstFrequencyHz;
  176. uint32_t completedPulses;
  177. uint8_t nextSegment;
  178. uint8_t positive;
  179. } PLSR_COUNTED_BOUNDARY_EVENT;
  180. typedef enum
  181. {
  182. PLSR_WORD_OK = 0,
  183. PLSR_WORD_ILLEGAL_ADDRESS,
  184. PLSR_WORD_ILLEGAL_VALUE
  185. } PLSR_WORD_RESULT;
  186. typedef enum
  187. {
  188. PLSR_COMMAND_MAILBOX_EMPTY = 0,
  189. PLSR_COMMAND_MAILBOX_PENDING,
  190. PLSR_COMMAND_MAILBOX_EXECUTING
  191. } PLSR_COMMAND_MAILBOX_STATE;
  192. typedef struct
  193. {
  194. PLSR_CONFIG startConfig;
  195. uint16_t command;
  196. volatile uint8_t state;
  197. } PLSR_COMMAND_MAILBOX;
  198. typedef struct
  199. {
  200. volatile PLSR_EXEC_MODE mode;
  201. volatile uint32_t generation;
  202. } PLSR_EXECUTOR;
  203. typedef struct
  204. {
  205. uint16_t flags;
  206. uint16_t reason;
  207. uint16_t lastSegment;
  208. uint16_t modeDirection;
  209. uint32_t expectedCycles;
  210. uint32_t observedCycles;
  211. int32_t countError;
  212. uint32_t requestedHz;
  213. uint32_t expectedTimerHz;
  214. uint32_t activeTimerHz;
  215. int32_t requestErrorHz;
  216. uint32_t curveSampleCount;
  217. uint32_t curveMismatchCount;
  218. uint32_t curveMaxAbsErrorHz;
  219. uint32_t firstMismatchSample;
  220. } PLSR_DIAGNOSTIC;
  221. static const uint16_t PlsrSineProgressQ16[65] =
  222. {
  223. 0U, 39U, 158U, 355U, 630U, 982U, 1411U, 1915U,
  224. 2494U, 3146U, 3869U, 4662U, 5522U, 6448U, 7438U, 8488U,
  225. 9597U, 10762U, 11980U, 13248U, 14563U, 15922U, 17321U,
  226. 18758U, 20228U, 21728U, 23256U, 24806U, 26375U, 27960U,
  227. 29556U, 31160U, 32767U, 34375U, 35979U, 37575U, 39160U,
  228. 40729U, 42279U, 43807U, 45307U, 46777U, 48214U, 49613U,
  229. 50972U, 52287U, 53555U, 54773U, 55938U, 57047U, 58097U,
  230. 59087U, 60013U, 60873U, 61666U, 62389U, 63041U, 63620U,
  231. 64124U, 64553U, 64905U, 65180U, 65377U, 65496U, 65535U
  232. };
  233. static PLSR_CONFIG PlsrShadowConfig;
  234. static PLSR_CONFIG PlsrActiveConfig;
  235. static PLSR_CONFIG PlsrCandidateConfig;
  236. static volatile int32_t PlsrPosition;
  237. static volatile uint64_t PlsrRemainingPulses;
  238. static volatile uint8_t PlsrPulseActive;
  239. static volatile uint8_t PlsrCutRequested;
  240. static volatile uint8_t PlsrBoundaryPending;
  241. static volatile uint8_t PlsrBoundaryWasCut;
  242. static volatile uint8_t PlsrCountPositive;
  243. static volatile uint8_t PlsrCountOverflowPending;
  244. static volatile uint8_t PlsrPositionValid;
  245. static volatile uint8_t PlsrPositionCheckpointDirty;
  246. static volatile uint8_t PlsrFrequencyUpdatePending;
  247. static volatile uint8_t PlsrFrequencyUpdateSegment;
  248. static volatile uint8_t PlsrSeamlessHandoffPending;
  249. static volatile uint8_t PlsrTimerErrorPending;
  250. static volatile uint8_t PlsrDeferredFrequencyPending;
  251. static volatile uint32_t PlsrCurrentFrequencyHz;
  252. static volatile uint32_t PlsrQueuedFrequencyHz;
  253. static volatile uint32_t PlsrBoundaryFrequencyHz;
  254. static volatile uint32_t PlsrFrequencyUpdateTargetHz;
  255. static volatile uint32_t PlsrDeferredFrequencyHz;
  256. static volatile uint32_t PlsrSegmentEpoch;
  257. static volatile PLSR_STATUS PlsrRunStatus = PLSR_STATUS_UNINITIALIZED;
  258. static PLSR_ERROR PlsrError = PLSR_ERROR_NONE;
  259. static PLSR_RAMP PlsrRamp;
  260. static PLSR_SHORT_PROFILE PlsrShortProfile;
  261. static PLSR_PROFILE_QUEUE PlsrProfileQueues[2];
  262. static volatile uint8_t PlsrProfileQueueBank;
  263. #define PlsrProfileQueue (PlsrProfileQueues[PlsrProfileQueueBank])
  264. static PLSR_TIMED_START PlsrTimedStart;
  265. static PLSR_HANDOFF_PLAN PlsrHandoffPlan;
  266. static volatile uint64_t PlsrCountedObservedPublished;
  267. static volatile uint8_t PlsrCountedHandoffStaged;
  268. /* 段边界/流故障事件:IRQ 只置事件并快照(handoff 计划 + 边界累计计数),
  269. 全部簿记由 1ms 任务消费事件时完成(IRQ 内不做状态转换与错误标志写入)。 */
  270. static PLSR_COUNTED_BOUNDARY_EVENT
  271. PlsrCountedBoundaryEvents[PLSR_BOUNDARY_EVENT_CAPACITY];
  272. static volatile uint8_t PlsrCountedBoundaryWriteIndex;
  273. static volatile uint8_t PlsrCountedBoundaryReadIndex;
  274. static volatile uint8_t PlsrCountedBoundaryBusy;
  275. static volatile uint8_t PlsrCountedFaultEvent;
  276. static PLSR_HANDOFF_PLAN
  277. PlsrPreparedHandoffPlans[2][PLSR_SEGMENT_COUNT_MAX];
  278. static volatile uint8_t PlsrPreparedHandoffBank;
  279. #ifdef PLSR_HOST_TEST
  280. static uint32_t PlsrHostTestEventSequence;
  281. static uint32_t PlsrHostTestLastGateEvent;
  282. static uint32_t PlsrHostTestLastDiagnosticFinishEvent;
  283. #endif
  284. static uint8_t PlsrInitialized;
  285. static volatile uint8_t PlsrCurrentSegment;
  286. static uint8_t PlsrDirectionDelayActive;
  287. static uint16_t PlsrDirectionDelayRemainingMs;
  288. static volatile uint8_t PlsrSegmentClockStarted;
  289. static volatile uint32_t PlsrSegmentElapsedMs;
  290. static volatile uint32_t PlsrWaitElapsedMs;
  291. static volatile uint8_t PlsrExtPreviousLevel;
  292. static volatile uint8_t PlsrExtEdgePending;
  293. static volatile uint8_t PlsrWaitSignalArmedMask;
  294. static volatile uint8_t PlsrWaitSignalPendingMask;
  295. static volatile uint8_t PlsrStopRequested;
  296. static volatile uint8_t PlsrStopPulsesRemaining;
  297. static volatile uint8_t PlsrAbStopArmed;
  298. static volatile uint8_t PlsrBoundaryRampStarted;
  299. static uint8_t PlsrLastDirectionValid;
  300. static uint8_t PlsrLastDirectionOutput;
  301. static uint8_t PlsrLastDirectionLevel;
  302. static uint8_t PlsrPersistenceDirty;
  303. static uint16_t PlsrPersistenceDelayMs;
  304. static uint8_t PlsrPositionCheckpointElapsedMs;
  305. static PLSR_COMMAND_MAILBOX PlsrCommandMailbox;
  306. static PLSR_EXECUTOR PlsrExecutor;
  307. static volatile PLSR_DIAGNOSTIC PlsrDiagnostic;
  308. static volatile uint64_t PlsrDiagnosticSegmentCounterBase;
  309. static volatile uint32_t PlsrDiagnosticQueuedExpectedHz;
  310. static volatile uint16_t PlsrDiagnosticFailedChecks;
  311. static volatile uint16_t PlsrProfileQueueMinimumDepth;
  312. static volatile uint16_t PlsrProfileQueueDiagnosticFlags;
  313. static volatile uint8_t PlsrProfileQueueLowWaterLatched;
  314. static volatile uint32_t PlsrProfileQueueLowWaterEvents;
  315. static volatile uint32_t PlsrProfileQueueUnderruns;
  316. static volatile uint32_t PlsrPlannerClippedCount;
  317. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0)
  318. volatile uint32_t PlsrProfileProducerItemCount;
  319. volatile uint32_t PlsrProfileProducerTotalCycles;
  320. volatile uint32_t PlsrProfileProducerMaxItemCycles;
  321. #endif
  322. static uint8_t PlsrIsBusy(void);
  323. static void PlsrSetDefaults(PLSR_CONFIG *config);
  324. static uint8_t PlsrConfigIsValid(const PLSR_CONFIG *config,
  325. uint8_t validateActivePath);
  326. static uint32_t PlsrResolvedSegmentFrequency(const PLSR_CONFIG *config,
  327. uint8_t segmentIndex);
  328. static uint16_t PlsrReadConfigWord(const PLSR_CONFIG *config,
  329. uint16_t address);
  330. static PLSR_WORD_RESULT PlsrWriteConfigWord(PLSR_CONFIG *config,
  331. uint16_t address,
  332. uint16_t value);
  333. static uint8_t PlsrAddressIsDwordHalf(uint16_t address,
  334. uint16_t *pairedAddress);
  335. static PLSR_MB_RESULT PlsrQueueCommand(uint16_t command);
  336. static uint8_t PlsrPollCommandMailbox(void);
  337. static void PlsrExecuteStart(void);
  338. static uint8_t PlsrExecuteStop(void);
  339. static void PlsrExecuteClear(void);
  340. static uint8_t PlsrStartSegment(uint8_t segmentNumber,
  341. uint8_t allowCarry,
  342. uint32_t carryFrequencyHz);
  343. static uint32_t PlsrEffectiveStartFrequency(uint32_t segmentFrequencyHz,
  344. uint8_t allowCarry,
  345. uint8_t directionChanged,
  346. uint32_t carryFrequencyHz);
  347. static uint32_t PlsrEffectiveStopFrequency(uint32_t segmentFrequencyHz);
  348. static uint8_t PlsrBeginSegmentOutput(uint32_t startFrequencyHz);
  349. static void PlsrHandleBoundary(uint8_t extEdge);
  350. static void PlsrTransitionToNext(uint8_t allowCarry);
  351. static void PlsrFinishCompleted(void);
  352. static void PlsrFinishStopped(void);
  353. static void PlsrEnterError(PLSR_ERROR error);
  354. static void PlsrEnterErrorIfEpoch(PLSR_ERROR error,
  355. uint32_t expectedEpoch);
  356. static void PlsrMarkPersistenceDirty(uint16_t delayMs);
  357. static void PlsrCheckpointPosition(uint8_t wasBusy);
  358. static void PlsrPollPositionCheckpoint(void);
  359. static void PlsrArmWaitSignal(uint8_t inputSelection);
  360. static void PlsrDisarmWaitSignal(void);
  361. static uint8_t PlsrWaitSignalDetected(uint8_t inputSelection);
  362. static PLSR_PLATFORM_QUEUE_RESULT PlsrTryContinuousHandoff(void);
  363. static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile,
  364. uint8_t segmentNumber,
  365. uint32_t startFrequencyHz,
  366. uint32_t targetFrequencyHz,
  367. uint64_t pulseCount,
  368. uint8_t currentPositive);
  369. static uint8_t PlsrShortProfileTakeRun(PLSR_SHORT_PROFILE *profile,
  370. PLSR_PROFILE_ENTRY *entry);
  371. static uint8_t PlsrShortProfileTakeRunLimited(
  372. PLSR_SHORT_PROFILE *profile,
  373. PLSR_PROFILE_ENTRY *entry,
  374. uint32_t maximumRepeats);
  375. static void PlsrProfileRecordPlannerStatus(
  376. const PLSR_SHORT_PROFILE *profile);
  377. static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir(
  378. uint32_t targetHz,
  379. uint32_t totalPulses);
  380. static void PlsrProfileQueueReset(void);
  381. static void PlsrProfileQueueResetBankLocked(uint8_t bank);
  382. static void PlsrInvalidateTimedStartLocked(void);
  383. static void PlsrServiceTimedStartPreparation(void);
  384. static PLSR_PLATFORM_SERVICE_RESULT PlsrTryStartPreparedTimedSegment(
  385. uint8_t segmentNumber);
  386. static uint8_t PlsrStartPreparedTimedOutput(void);
  387. static void PlsrProfileQueueInvalidateGeneration(void);
  388. static uint8_t PlsrProfileQueueBegin(
  389. const PLSR_SHORT_PROFILE *producerProfile,
  390. uint32_t producerEpoch,
  391. uint8_t producerSegment,
  392. uint8_t preparedHandoffBank);
  393. static uint8_t PlsrProfileQueueFill(uint16_t targetCount,
  394. uint16_t *itemBudget);
  395. static uint8_t PlsrTimerSettingsEqual(
  396. const PLSR_PLATFORM_TIMER_SETTING *first,
  397. const PLSR_PLATFORM_TIMER_SETTING *second);
  398. static uint8_t PlsrProfileQueueAppendLocked(
  399. PLSR_PROFILE_QUEUE *queue,
  400. const PLSR_PROFILE_ENTRY *entry);
  401. static uint8_t PlsrStageCountedHandoff(void);
  402. static PLSR_PLATFORM_QUEUE_RESULT PlsrProfileQueueCommitNext(
  403. uint8_t pulseOutput);
  404. static uint8_t PlsrPrepareFutureHandoffQueue(void);
  405. static void PlsrActivateHandoffQueueFromIrq(
  406. const PLSR_HANDOFF_PLAN *plan,
  407. uint32_t producerEpoch);
  408. static void PlsrCopyShortProfile(PLSR_SHORT_PROFILE *destination,
  409. const PLSR_SHORT_PROFILE *source);
  410. static void PlsrInvalidateHandoffPlans(void);
  411. static uint8_t PlsrBuildHandoffPlanBank(
  412. const PLSR_CONFIG *frequencyConfig);
  413. static uint8_t PlsrSelectPreparedHandoffPlan(PLSR_HANDOFF_PLAN *plan);
  414. static PLSR_PLATFORM_QUEUE_RESULT PlsrPrimeHandoff(void);
  415. static uint8_t PlsrBuildHandoffPlan(uint8_t sourceSegment,
  416. const PLSR_CONFIG *frequencyConfig,
  417. uint32_t carryFrequencyHz,
  418. PLSR_HANDOFF_PLAN *plan);
  419. static void PlsrRefreshCurrentHandoffPlan(uint32_t terminalFrequencyHz);
  420. static void PlsrDiagnosticReset(void);
  421. static void PlsrDiagnosticBeginSegment(uint8_t segmentNumber,
  422. uint64_t expectedCycles,
  423. uint8_t directionPositive);
  424. static void PlsrDiagnosticFinishSegment(uint8_t completedNormally);
  425. static uint8_t PlsrDiagnosticStartPulse(uint8_t pulseOutput,
  426. uint32_t requestedFirstHz,
  427. uint32_t requestedQueuedHz,
  428. uint32_t *actualFirstHz,
  429. uint32_t *actualQueuedHz);
  430. static PLSR_PLATFORM_QUEUE_RESULT PlsrDiagnosticQueueFrequency(
  431. uint8_t pulseOutput,
  432. uint32_t requestedHz,
  433. uint32_t *actualHz);
  434. static void PlsrDiagnosticCheckActiveFrequency(uint32_t activeHz);
  435. static uint8_t PlsrArmFinalAbBoundaryLocked(void);
  436. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
  437. && !defined(PLSR_HOST_TEST)
  438. static uint32_t PlsrProfileTimingNow(void);
  439. #endif
  440. static uint8_t PlsrQueueFinalAbBoundaryFromIrq(void);
  441. static uint8_t PlsrIsBusy(void)
  442. {
  443. return ((PlsrRunStatus == PLSR_STATUS_ACCELERATING)
  444. || (PlsrRunStatus == PLSR_STATUS_RUNNING)
  445. || (PlsrRunStatus == PLSR_STATUS_DECELERATING)
  446. || (PlsrRunStatus == PLSR_STATUS_WAITING)
  447. || (PlsrRunStatus == PLSR_STATUS_PAUSED)) ? 1U : 0U;
  448. }
  449. static uint32_t PlsrJoinU32(uint16_t lowWord, uint16_t highWord)
  450. {
  451. return (uint32_t)lowWord | ((uint32_t)highWord << 16U);
  452. }
  453. static uint16_t PlsrLowWord(uint32_t value)
  454. {
  455. return (uint16_t)(value & 0xFFFFUL);
  456. }
  457. static uint16_t PlsrHighWord(uint32_t value)
  458. {
  459. return (uint16_t)(value >> 16U);
  460. }
  461. static uint8_t PlsrFinalAbBoundaryRequired(void)
  462. {
  463. uint8_t finalBoundary;
  464. if (PlsrActiveConfig.outputMode != PLSR_OUTPUT_AB)
  465. {
  466. return 0U;
  467. }
  468. finalBoundary = ((PlsrCutRequested != 0U)
  469. || ((PlsrStopRequested != 0U)
  470. && (PlsrStopPulsesRemaining == 1U))
  471. || ((PlsrRemainingPulses == 1UL)
  472. && (PlsrHandoffPlan.valid == 0U))) ? 1U : 0U;
  473. return ((PlsrPulseActive != 0U) && (finalBoundary != 0U)) ? 1U : 0U;
  474. }
  475. static uint8_t PlsrCompleteFinalAbArmLocked(void)
  476. {
  477. PLSR_PLATFORM_STOP_RESULT result;
  478. uint8_t pulseOutput = (uint8_t)PlsrActiveConfig.pulseOutput;
  479. result = PlsrPlatformRequestStopLocked(pulseOutput, 1U);
  480. if (result == PLSR_PLATFORM_STOP_PENDING)
  481. {
  482. PlsrQueuedFrequencyHz = PlsrCurrentFrequencyHz;
  483. PlsrDiagnosticQueuedExpectedHz = PlsrCurrentFrequencyHz;
  484. PlsrDeferredFrequencyHz = PlsrCurrentFrequencyHz;
  485. PlsrDeferredFrequencyPending = 0U;
  486. PlsrFrequencyUpdatePending = 0U;
  487. PlsrShortProfile.active = 0U;
  488. PlsrHandoffPlan.valid = 0U;
  489. PlsrRamp.active = 0U;
  490. return 1U;
  491. }
  492. PlsrTimerErrorPending = 1U;
  493. PlsrPlatformStopPulse(pulseOutput);
  494. PlsrAbStopArmed = 0U;
  495. PlsrPulseActive = 0U;
  496. PlsrBoundaryFrequencyHz = PlsrCurrentFrequencyHz;
  497. PlsrBoundaryWasCut = (PlsrCutRequested != 0U) ? 1U : 0U;
  498. PlsrCutRequested = 0U;
  499. PlsrCurrentFrequencyHz = 0UL;
  500. PlsrQueuedFrequencyHz = 0UL;
  501. PlsrBoundaryPending = 1U;
  502. return 0U;
  503. }
  504. static uint8_t PlsrArmFinalAbBoundaryLocked(void)
  505. {
  506. if (PlsrFinalAbBoundaryRequired() == 0U)
  507. {
  508. return 1U;
  509. }
  510. if (PlsrAbStopArmed != 0U)
  511. {
  512. return 1U;
  513. }
  514. PlsrAbStopArmed = 1U;
  515. return PlsrCompleteFinalAbArmLocked();
  516. }
  517. static uint8_t PlsrQueueFinalAbBoundaryFromIrq(void)
  518. {
  519. uint32_t criticalState;
  520. if ((PlsrFinalAbBoundaryRequired() == 0U)
  521. || (PlsrAbStopArmed != 0U))
  522. {
  523. return 1U;
  524. }
  525. PlsrAbStopArmed = 1U;
  526. if (PlsrPlatformQueueFinalArmFromIrq(
  527. (uint8_t)PlsrActiveConfig.pulseOutput) != 0U)
  528. {
  529. return 1U;
  530. }
  531. criticalState = PlsrPlatformEnterCritical();
  532. if ((PlsrAbStopArmed == 0U)
  533. || (PlsrFinalAbBoundaryRequired() == 0U))
  534. {
  535. PlsrPlatformExitCritical(criticalState);
  536. return 1U;
  537. }
  538. {
  539. uint8_t result = PlsrCompleteFinalAbArmLocked();
  540. PlsrPlatformExitCritical(criticalState);
  541. return result;
  542. }
  543. }
  544. void PlsrFinalArmJobIrq(uint8_t pulseOutput)
  545. {
  546. uint32_t criticalState = PlsrPlatformEnterCritical();
  547. if ((PlsrAbStopArmed == 0U) || (PlsrPulseActive == 0U)
  548. || (pulseOutput != (uint8_t)PlsrActiveConfig.pulseOutput)
  549. || (PlsrFinalAbBoundaryRequired() == 0U))
  550. {
  551. if ((PlsrPulseActive != 0U)
  552. && (pulseOutput == (uint8_t)PlsrActiveConfig.pulseOutput))
  553. {
  554. PlsrAbStopArmed = 0U;
  555. }
  556. PlsrPlatformExitCritical(criticalState);
  557. return;
  558. }
  559. (void)PlsrCompleteFinalAbArmLocked();
  560. PlsrPlatformExitCritical(criticalState);
  561. }
  562. #if PLSR_DIAGNOSTIC_ENABLED
  563. static uint32_t PlsrAbsDifferenceU32(uint32_t first, uint32_t second)
  564. {
  565. return (first >= second) ? (first - second) : (second - first);
  566. }
  567. static int32_t PlsrDifferenceI32(uint32_t first, uint32_t second)
  568. {
  569. int64_t difference = (int64_t)first - (int64_t)second;
  570. if (difference > INT32_MAX)
  571. {
  572. return INT32_MAX;
  573. }
  574. if (difference < INT32_MIN)
  575. {
  576. return INT32_MIN;
  577. }
  578. return (int32_t)difference;
  579. }
  580. #endif
  581. #if PLSR_DIAGNOSTIC_ENABLED
  582. static void PlsrDiagnosticLatch(uint16_t reason)
  583. {
  584. PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_FAULT_LATCHED;
  585. if (PlsrDiagnostic.reason == PLSR_DIAG_REASON_NONE)
  586. {
  587. PlsrDiagnostic.reason = reason;
  588. }
  589. }
  590. #endif
  591. static void PlsrDiagnosticReset(void)
  592. {
  593. #if PLSR_DIAGNOSTIC_ENABLED
  594. (void)memset((void *)&PlsrDiagnostic, 0, sizeof(PlsrDiagnostic));
  595. PlsrDiagnostic.firstMismatchSample = 0xFFFFFFFFUL;
  596. PlsrDiagnosticSegmentCounterBase = 0UL;
  597. PlsrDiagnosticQueuedExpectedHz = 0UL;
  598. PlsrDiagnosticFailedChecks = 0U;
  599. #else
  600. /* 关闭状态保持诊断区清零,Modbus 读诊断寄存器恒为 0。 */
  601. (void)memset((void *)&PlsrDiagnostic, 0, sizeof(PlsrDiagnostic));
  602. #endif
  603. PlsrProfileQueueMinimumDepth = 0xFFFFU;
  604. PlsrProfileQueueDiagnosticFlags = 0U;
  605. PlsrProfileQueueLowWaterLatched = 0U;
  606. PlsrProfileQueueLowWaterEvents = 0UL;
  607. PlsrProfileQueueUnderruns = 0UL;
  608. PlsrPlannerClippedCount = 0UL;
  609. }
  610. static void PlsrDiagnosticBeginSegment(uint8_t segmentNumber,
  611. uint64_t expectedCycles,
  612. uint8_t directionPositive)
  613. {
  614. #if PLSR_DIAGNOSTIC_ENABLED
  615. if (expectedCycles > 0xFFFFFFFFUL)
  616. {
  617. PlsrDiagnostic.expectedCycles = 0xFFFFFFFFUL;
  618. PlsrDiagnosticLatch(PLSR_DIAG_REASON_INVALID_SAMPLE);
  619. }
  620. else
  621. {
  622. PlsrDiagnostic.expectedCycles = (uint32_t)expectedCycles;
  623. }
  624. PlsrDiagnostic.flags &= (uint16_t)~(PLSR_DIAG_FLAG_COUNT_CHECKED
  625. | PLSR_DIAG_FLAG_COUNT_PASS);
  626. PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_MONITORING;
  627. PlsrDiagnostic.lastSegment = segmentNumber;
  628. PlsrDiagnostic.modeDirection =
  629. (uint16_t)(PlsrActiveConfig.outputMode
  630. | ((uint16_t)directionPositive << 8U));
  631. PlsrDiagnostic.observedCycles = 0UL;
  632. PlsrDiagnostic.countError = 0L;
  633. PlsrDiagnosticSegmentCounterBase = PlsrPlatformObservedPulses(
  634. (uint8_t)PlsrActiveConfig.pulseOutput);
  635. #else
  636. (void)segmentNumber;
  637. (void)expectedCycles;
  638. (void)directionPositive;
  639. #endif
  640. }
  641. static void PlsrDiagnosticFinishSegment(uint8_t completedNormally)
  642. {
  643. #if PLSR_DIAGNOSTIC_ENABLED
  644. uint64_t now;
  645. uint64_t observed;
  646. uint32_t observedCycles;
  647. uint32_t expectedCycles;
  648. if ((PlsrDiagnostic.flags & PLSR_DIAG_FLAG_MONITORING) == 0U)
  649. {
  650. return;
  651. }
  652. #ifdef PLSR_HOST_TEST
  653. PlsrHostTestEventSequence++;
  654. PlsrHostTestLastDiagnosticFinishEvent = PlsrHostTestEventSequence;
  655. #endif
  656. now = PlsrPlatformObservedPulses(
  657. (uint8_t)PlsrActiveConfig.pulseOutput);
  658. observed = (now >= PlsrDiagnosticSegmentCounterBase)
  659. ? (now - PlsrDiagnosticSegmentCounterBase) : 0UL;
  660. if (observed > 0xFFFFFFFFUL)
  661. {
  662. PlsrDiagnostic.observedCycles = 0xFFFFFFFFUL;
  663. PlsrDiagnosticFailedChecks |= PLSR_DIAG_FLAG_COUNT_PASS;
  664. PlsrDiagnostic.flags &= (uint16_t)~PLSR_DIAG_FLAG_COUNT_PASS;
  665. PlsrDiagnosticLatch(PLSR_DIAG_REASON_INVALID_SAMPLE);
  666. }
  667. else
  668. {
  669. PlsrDiagnostic.observedCycles = (uint32_t)observed;
  670. }
  671. PlsrDiagnostic.flags &= (uint16_t)~PLSR_DIAG_FLAG_MONITORING;
  672. if (completedNormally == 0U)
  673. {
  674. PlsrDiagnostic.countError = 0L;
  675. PlsrDiagnostic.flags &= (uint16_t)~(PLSR_DIAG_FLAG_COUNT_CHECKED
  676. | PLSR_DIAG_FLAG_COUNT_PASS);
  677. return;
  678. }
  679. observedCycles = PlsrDiagnostic.observedCycles;
  680. expectedCycles = PlsrDiagnostic.expectedCycles;
  681. PlsrDiagnostic.countError =
  682. PlsrDifferenceI32(observedCycles, expectedCycles);
  683. PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_COUNT_CHECKED;
  684. if (PlsrDiagnostic.countError != 0L)
  685. {
  686. PlsrDiagnosticFailedChecks |= PLSR_DIAG_FLAG_COUNT_PASS;
  687. PlsrDiagnostic.flags &= (uint16_t)~PLSR_DIAG_FLAG_COUNT_PASS;
  688. PlsrDiagnosticLatch(PLSR_DIAG_REASON_COUNT);
  689. }
  690. else if ((PlsrDiagnosticFailedChecks & PLSR_DIAG_FLAG_COUNT_PASS) == 0U)
  691. {
  692. PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_COUNT_PASS;
  693. }
  694. #else
  695. (void)completedNormally;
  696. #endif
  697. }
  698. static void PlsrDiagnosticRecordFrequency(uint32_t requestedHz,
  699. uint32_t expectedHz,
  700. uint32_t actualHz)
  701. {
  702. #if PLSR_DIAGNOSTIC_ENABLED
  703. uint32_t error = PlsrAbsDifferenceU32(actualHz, expectedHz);
  704. PlsrDiagnostic.requestedHz = requestedHz;
  705. PlsrDiagnostic.expectedTimerHz = expectedHz;
  706. PlsrDiagnostic.activeTimerHz = actualHz;
  707. /* Request error is divider quantization, not a diagnostic deviation. */
  708. PlsrDiagnostic.requestErrorHz = PlsrDifferenceI32(expectedHz, requestedHz);
  709. PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_FREQUENCY_CHECKED;
  710. if (actualHz != expectedHz)
  711. {
  712. PlsrDiagnosticFailedChecks |= PLSR_DIAG_FLAG_FREQUENCY_PASS;
  713. PlsrDiagnostic.flags &= (uint16_t)~PLSR_DIAG_FLAG_FREQUENCY_PASS;
  714. PlsrDiagnosticLatch(PLSR_DIAG_REASON_FREQUENCY);
  715. }
  716. else if ((PlsrDiagnosticFailedChecks
  717. & PLSR_DIAG_FLAG_FREQUENCY_PASS) == 0U)
  718. {
  719. PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_FREQUENCY_PASS;
  720. }
  721. if (error > PlsrDiagnostic.curveMaxAbsErrorHz)
  722. {
  723. PlsrDiagnostic.curveMaxAbsErrorHz = error;
  724. }
  725. #else
  726. (void)requestedHz;
  727. (void)expectedHz;
  728. (void)actualHz;
  729. #endif
  730. }
  731. #if PLSR_DIAGNOSTIC_ENABLED
  732. static void PlsrDiagnosticRecordCurveSample(uint32_t expectedHz,
  733. uint32_t actualHz)
  734. {
  735. uint32_t error = PlsrAbsDifferenceU32(expectedHz, actualHz);
  736. if (PlsrDiagnostic.curveSampleCount != 0xFFFFFFFFUL)
  737. {
  738. PlsrDiagnostic.curveSampleCount++;
  739. }
  740. PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_CURVE_CHECKED;
  741. if (error == 0UL)
  742. {
  743. if ((PlsrDiagnosticFailedChecks & PLSR_DIAG_FLAG_CURVE_PASS) == 0U)
  744. {
  745. PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_CURVE_PASS;
  746. }
  747. }
  748. else
  749. {
  750. PlsrDiagnosticFailedChecks |= PLSR_DIAG_FLAG_CURVE_PASS;
  751. PlsrDiagnostic.flags &= (uint16_t)~PLSR_DIAG_FLAG_CURVE_PASS;
  752. if (PlsrDiagnostic.curveMismatchCount != 0xFFFFFFFFUL)
  753. {
  754. PlsrDiagnostic.curveMismatchCount++;
  755. }
  756. if (PlsrDiagnostic.firstMismatchSample == 0xFFFFFFFFUL)
  757. {
  758. PlsrDiagnostic.firstMismatchSample =
  759. PlsrDiagnostic.curveSampleCount;
  760. }
  761. if (error > PlsrDiagnostic.curveMaxAbsErrorHz)
  762. {
  763. PlsrDiagnostic.curveMaxAbsErrorHz = error;
  764. }
  765. PlsrDiagnosticLatch(PLSR_DIAG_REASON_CURVE);
  766. }
  767. }
  768. #endif
  769. static uint8_t PlsrDiagnosticStartPulse(uint8_t pulseOutput,
  770. uint32_t requestedFirstHz,
  771. uint32_t requestedQueuedHz,
  772. uint32_t *actualFirstHz,
  773. uint32_t *actualQueuedHz)
  774. {
  775. PLSR_PLATFORM_TIMER_SETTING firstSetting;
  776. PLSR_PLATFORM_TIMER_SETTING queuedSetting;
  777. if ((PlsrPlatformBuildTimerSetting(
  778. pulseOutput, (uint8_t)PlsrActiveConfig.outputMode,
  779. requestedFirstHz, &firstSetting) == 0U)
  780. || (PlsrPlatformBuildTimerSetting(
  781. pulseOutput, (uint8_t)PlsrActiveConfig.outputMode,
  782. requestedQueuedHz, &queuedSetting) == 0U)
  783. || (PlsrPlatformStartPrepared(pulseOutput, &firstSetting,
  784. &queuedSetting, actualFirstHz,
  785. actualQueuedHz) == 0U))
  786. {
  787. PlsrDiagnosticFinishSegment(0U);
  788. return 0U;
  789. }
  790. #if PLSR_DIAGNOSTIC_ENABLED
  791. PlsrDiagnosticRecordFrequency(requestedFirstHz,
  792. firstSetting.actualFrequencyHz,
  793. *actualFirstHz);
  794. PlsrDiagnosticRecordCurveSample(firstSetting.actualFrequencyHz,
  795. *actualFirstHz);
  796. PlsrDiagnosticQueuedExpectedHz = queuedSetting.actualFrequencyHz;
  797. #endif
  798. return 1U;
  799. }
  800. static PLSR_PLATFORM_QUEUE_RESULT PlsrDiagnosticQueueFrequency(
  801. uint8_t pulseOutput,
  802. uint32_t requestedHz,
  803. uint32_t *actualHz)
  804. {
  805. PLSR_PLATFORM_QUEUE_RESULT result;
  806. #if PLSR_DIAGNOSTIC_ENABLED
  807. uint32_t expectedHz;
  808. if (PlsrPlatformExpectedFrequency(
  809. pulseOutput, (uint8_t)PlsrActiveConfig.outputMode,
  810. requestedHz, &expectedHz) == 0U)
  811. {
  812. PlsrDiagnosticFinishSegment(0U);
  813. return PLSR_PLATFORM_QUEUE_FAILED;
  814. }
  815. #endif
  816. result = PlsrPlatformQueueFrequency(pulseOutput, requestedHz, actualHz);
  817. if (result == PLSR_PLATFORM_QUEUE_FAILED)
  818. {
  819. PlsrDiagnosticFinishSegment(0U);
  820. return result;
  821. }
  822. if (result == PLSR_PLATFORM_QUEUE_STALE)
  823. {
  824. return result;
  825. }
  826. #if PLSR_DIAGNOSTIC_ENABLED
  827. PlsrDiagnosticRecordFrequency(requestedHz, expectedHz, *actualHz);
  828. PlsrDiagnosticQueuedExpectedHz = expectedHz;
  829. #endif
  830. return PLSR_PLATFORM_QUEUE_APPLIED;
  831. }
  832. static void PlsrDiagnosticCheckActiveFrequency(uint32_t activeHz)
  833. {
  834. #if PLSR_DIAGNOSTIC_ENABLED
  835. uint16_t platformFault = PlsrPlatformDiagnosticFault();
  836. uint32_t observedHz = activeHz;
  837. if (platformFault == PLSR_DIAG_REASON_FREQUENCY)
  838. {
  839. observedHz = PlsrPlatformActiveFrequency(
  840. (uint8_t)PlsrActiveConfig.pulseOutput);
  841. if (observedHz == 0UL)
  842. {
  843. observedHz = activeHz;
  844. }
  845. PlsrDiagnosticFailedChecks |= PLSR_DIAG_FLAG_FREQUENCY_PASS;
  846. PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_FREQUENCY_CHECKED;
  847. PlsrDiagnostic.flags &= (uint16_t)~PLSR_DIAG_FLAG_FREQUENCY_PASS;
  848. PlsrDiagnosticLatch(PLSR_DIAG_REASON_FREQUENCY);
  849. }
  850. if ((platformFault != PLSR_DIAG_REASON_NONE)
  851. && (PlsrDiagnosticQueuedExpectedHz != 0UL))
  852. {
  853. PlsrDiagnostic.activeTimerHz = observedHz;
  854. if (platformFault == PLSR_DIAG_REASON_CURVE)
  855. {
  856. if (PlsrDiagnostic.curveSampleCount != 0xFFFFFFFFUL)
  857. {
  858. PlsrDiagnostic.curveSampleCount++;
  859. }
  860. PlsrDiagnostic.flags |= PLSR_DIAG_FLAG_CURVE_CHECKED;
  861. PlsrDiagnostic.flags &= (uint16_t)~PLSR_DIAG_FLAG_CURVE_PASS;
  862. PlsrDiagnosticFailedChecks |= PLSR_DIAG_FLAG_CURVE_PASS;
  863. if (PlsrDiagnostic.curveMismatchCount != 0xFFFFFFFFUL)
  864. {
  865. PlsrDiagnostic.curveMismatchCount++;
  866. }
  867. if (PlsrDiagnostic.firstMismatchSample == 0xFFFFFFFFUL)
  868. {
  869. PlsrDiagnostic.firstMismatchSample =
  870. PlsrDiagnostic.curveSampleCount;
  871. }
  872. PlsrDiagnosticLatch(PLSR_DIAG_REASON_CURVE);
  873. }
  874. else if (platformFault == PLSR_DIAG_REASON_FREQUENCY)
  875. {
  876. PlsrDiagnosticRecordCurveSample(PlsrDiagnosticQueuedExpectedHz,
  877. observedHz);
  878. }
  879. }
  880. #else
  881. (void)activeHz;
  882. #endif
  883. }
  884. static void PlsrSetDefaults(PLSR_CONFIG *config)
  885. {
  886. uint8_t index;
  887. (void)memset(config, 0, sizeof(*config));
  888. config->pulseOutput = 0U;
  889. config->directionOutput = 0U;
  890. config->waitInput = 0U;
  891. config->extInput = 0U;
  892. config->sendMode = PLSR_SEND_COMPLETE;
  893. config->directionDelayMs = 10U;
  894. config->directionNegativeLogic = 0U;
  895. config->curveMode = 0U;
  896. config->positionMode = PLSR_POSITION_RELATIVE;
  897. config->segmentCount = 1U;
  898. config->startSegment = 1U;
  899. config->defaultSpeedHz = 1000UL;
  900. config->startSpeedHz = 100UL;
  901. config->stopSpeedHz = 100UL;
  902. config->accelerationTimeMs = 100U;
  903. config->decelerationTimeMs = 100U;
  904. config->outputMode = PLSR_OUTPUT_PULSE_DIR;
  905. for (index = 0U; index < PLSR_SEGMENT_COUNT_MAX; index++)
  906. {
  907. config->segments[index].frequencyHz = 1000UL;
  908. config->segments[index].pulses = (index == 0U) ? 1000L : 0L;
  909. config->segments[index].waitType = PLSR_EXT_OR_COMPLETE;
  910. config->segments[index].waitTimeMs = 0U;
  911. config->segments[index].actTimeMs = 0U;
  912. config->segments[index].jumpSegment = 0U;
  913. }
  914. }
  915. static uint8_t PlsrConfigIsValid(const PLSR_CONFIG *config,
  916. uint8_t validateActivePath)
  917. {
  918. uint8_t index;
  919. if ((config->pulseOutput > 3U) || (config->directionOutput > 3U)
  920. || (config->waitInput > 1U) || (config->extInput > 1U)
  921. || (config->sendMode > PLSR_SEND_SUBSEQUENT)
  922. || (config->directionNegativeLogic > 1U)
  923. || (config->curveMode > 2U)
  924. || (config->positionMode > PLSR_POSITION_ABSOLUTE)
  925. || (config->outputMode > PLSR_OUTPUT_AB)
  926. || ((config->outputMode == PLSR_OUTPUT_AB)
  927. && ((config->pulseOutput & 1U) != 0U))
  928. || (config->segmentCount == 0U)
  929. || (config->segmentCount > PLSR_SEGMENT_COUNT_MAX)
  930. || (config->startSegment == 0U)
  931. || (config->startSegment > PLSR_SEGMENT_COUNT_MAX)
  932. || (config->defaultSpeedHz == 0UL)
  933. || (config->defaultSpeedHz > PLSR_FREQUENCY_MAX_HZ)
  934. || (config->startSpeedHz > PLSR_FREQUENCY_MAX_HZ)
  935. || (config->stopSpeedHz > PLSR_FREQUENCY_MAX_HZ))
  936. {
  937. return 0U;
  938. }
  939. if ((validateActivePath != 0U)
  940. && (config->startSegment > config->segmentCount))
  941. {
  942. return 0U;
  943. }
  944. for (index = 0U; index < PLSR_SEGMENT_COUNT_MAX; index++)
  945. {
  946. const PLSR_SEGMENT_CONFIG *segment = &config->segments[index];
  947. if ((segment->frequencyHz > PLSR_FREQUENCY_MAX_HZ)
  948. || (segment->waitType > PLSR_EXT_OR_COMPLETE)
  949. || (segment->jumpSegment > PLSR_SEGMENT_COUNT_MAX))
  950. {
  951. return 0U;
  952. }
  953. if ((validateActivePath != 0U)
  954. && (index < config->segmentCount)
  955. && (segment->jumpSegment > config->segmentCount))
  956. {
  957. return 0U;
  958. }
  959. }
  960. return 1U;
  961. }
  962. static uint16_t PlsrReadConfigWord(const PLSR_CONFIG *config,
  963. uint16_t address)
  964. {
  965. uint16_t offset;
  966. uint8_t segmentIndex;
  967. const PLSR_SEGMENT_CONFIG *segment;
  968. switch (address)
  969. {
  970. case 0x1000U: return config->pulseOutput;
  971. case 0x1001U: return config->directionOutput;
  972. case 0x1002U: return config->waitInput;
  973. case 0x1003U: return config->extInput;
  974. case 0x1004U: return config->sendMode;
  975. case 0x1005U: return config->directionDelayMs;
  976. case 0x1006U: return config->directionNegativeLogic;
  977. case 0x1007U: return config->curveMode;
  978. case 0x1008U: return config->positionMode;
  979. case 0x1009U: return config->segmentCount;
  980. case 0x100AU: return config->startSegment;
  981. case 0x100BU: return PlsrLowWord(config->defaultSpeedHz);
  982. case 0x100CU: return PlsrHighWord(config->defaultSpeedHz);
  983. case 0x100DU: return PlsrLowWord(config->startSpeedHz);
  984. case 0x100EU: return PlsrHighWord(config->startSpeedHz);
  985. case 0x1010U: return PlsrLowWord(config->stopSpeedHz);
  986. case 0x1011U: return PlsrHighWord(config->stopSpeedHz);
  987. case 0x1012U: return config->accelerationTimeMs;
  988. case 0x1013U: return config->decelerationTimeMs;
  989. default: break;
  990. }
  991. if ((address >= PLSR_SEGMENT_FIRST_ADDRESS)
  992. && (address <= PLSR_CONFIG_LAST_ADDRESS))
  993. {
  994. offset = (uint16_t)(address - PLSR_SEGMENT_FIRST_ADDRESS);
  995. segmentIndex = (uint8_t)(offset / PLSR_SEGMENT_STRIDE);
  996. offset = (uint16_t)(offset % PLSR_SEGMENT_STRIDE);
  997. segment = &config->segments[segmentIndex];
  998. switch (offset)
  999. {
  1000. case 0U: return PlsrLowWord(segment->frequencyHz);
  1001. case 1U: return PlsrHighWord(segment->frequencyHz);
  1002. case 2U: return PlsrLowWord((uint32_t)segment->pulses);
  1003. case 3U: return PlsrHighWord((uint32_t)segment->pulses);
  1004. case 4U: return segment->waitType;
  1005. case 5U: return segment->waitTimeMs;
  1006. case 6U: return segment->actTimeMs;
  1007. case 7U: return segment->jumpSegment;
  1008. default: return 0U;
  1009. }
  1010. }
  1011. /* 0x100F, 0x1014..0x10FF, and segment padding read as zero. */
  1012. return 0U;
  1013. }
  1014. static PLSR_WORD_RESULT PlsrWriteConfigWord(PLSR_CONFIG *config,
  1015. uint16_t address,
  1016. uint16_t value)
  1017. {
  1018. uint16_t offset;
  1019. uint8_t segmentIndex;
  1020. PLSR_SEGMENT_CONFIG *segment;
  1021. switch (address)
  1022. {
  1023. case 0x1000U: config->pulseOutput = value; return PLSR_WORD_OK;
  1024. case 0x1001U: config->directionOutput = value; return PLSR_WORD_OK;
  1025. case 0x1002U: config->waitInput = value; return PLSR_WORD_OK;
  1026. case 0x1003U: config->extInput = value; return PLSR_WORD_OK;
  1027. case 0x1004U: config->sendMode = value; return PLSR_WORD_OK;
  1028. case 0x1005U: config->directionDelayMs = value; return PLSR_WORD_OK;
  1029. case 0x1006U:
  1030. config->directionNegativeLogic = value;
  1031. return PLSR_WORD_OK;
  1032. case 0x1007U: config->curveMode = value; return PLSR_WORD_OK;
  1033. case 0x1008U: config->positionMode = value; return PLSR_WORD_OK;
  1034. case 0x1009U: config->segmentCount = value; return PLSR_WORD_OK;
  1035. case 0x100AU: config->startSegment = value; return PLSR_WORD_OK;
  1036. case 0x100BU:
  1037. config->defaultSpeedHz =
  1038. PlsrJoinU32(value, PlsrHighWord(config->defaultSpeedHz));
  1039. return PLSR_WORD_OK;
  1040. case 0x100CU:
  1041. config->defaultSpeedHz =
  1042. PlsrJoinU32(PlsrLowWord(config->defaultSpeedHz), value);
  1043. return PLSR_WORD_OK;
  1044. case 0x100DU:
  1045. config->startSpeedHz =
  1046. PlsrJoinU32(value, PlsrHighWord(config->startSpeedHz));
  1047. return PLSR_WORD_OK;
  1048. case 0x100EU:
  1049. config->startSpeedHz =
  1050. PlsrJoinU32(PlsrLowWord(config->startSpeedHz), value);
  1051. return PLSR_WORD_OK;
  1052. case 0x100FU:
  1053. return (value == 0U) ? PLSR_WORD_OK : PLSR_WORD_ILLEGAL_VALUE;
  1054. case 0x1010U:
  1055. config->stopSpeedHz =
  1056. PlsrJoinU32(value, PlsrHighWord(config->stopSpeedHz));
  1057. return PLSR_WORD_OK;
  1058. case 0x1011U:
  1059. config->stopSpeedHz =
  1060. PlsrJoinU32(PlsrLowWord(config->stopSpeedHz), value);
  1061. return PLSR_WORD_OK;
  1062. case 0x1012U:
  1063. config->accelerationTimeMs = value;
  1064. return PLSR_WORD_OK;
  1065. case 0x1013U:
  1066. config->decelerationTimeMs = value;
  1067. return PLSR_WORD_OK;
  1068. default: break;
  1069. }
  1070. if ((address >= 0x1014U) && (address <= PLSR_COMMON_LAST_ADDRESS))
  1071. {
  1072. return (value == 0U) ? PLSR_WORD_OK : PLSR_WORD_ILLEGAL_VALUE;
  1073. }
  1074. if ((address < PLSR_SEGMENT_FIRST_ADDRESS)
  1075. || (address > PLSR_CONFIG_LAST_ADDRESS))
  1076. {
  1077. return PLSR_WORD_ILLEGAL_ADDRESS;
  1078. }
  1079. offset = (uint16_t)(address - PLSR_SEGMENT_FIRST_ADDRESS);
  1080. segmentIndex = (uint8_t)(offset / PLSR_SEGMENT_STRIDE);
  1081. offset = (uint16_t)(offset % PLSR_SEGMENT_STRIDE);
  1082. segment = &config->segments[segmentIndex];
  1083. switch (offset)
  1084. {
  1085. case 0U:
  1086. segment->frequencyHz =
  1087. PlsrJoinU32(value, PlsrHighWord(segment->frequencyHz));
  1088. return PLSR_WORD_OK;
  1089. case 1U:
  1090. segment->frequencyHz =
  1091. PlsrJoinU32(PlsrLowWord(segment->frequencyHz), value);
  1092. return PLSR_WORD_OK;
  1093. case 2U:
  1094. segment->pulses = (int32_t)PlsrJoinU32(
  1095. value, PlsrHighWord((uint32_t)segment->pulses));
  1096. return PLSR_WORD_OK;
  1097. case 3U:
  1098. segment->pulses = (int32_t)PlsrJoinU32(
  1099. PlsrLowWord((uint32_t)segment->pulses), value);
  1100. return PLSR_WORD_OK;
  1101. case 4U: segment->waitType = value; return PLSR_WORD_OK;
  1102. case 5U: segment->waitTimeMs = value; return PLSR_WORD_OK;
  1103. case 6U: segment->actTimeMs = value; return PLSR_WORD_OK;
  1104. case 7U: segment->jumpSegment = value; return PLSR_WORD_OK;
  1105. default:
  1106. return (value == 0U) ? PLSR_WORD_OK : PLSR_WORD_ILLEGAL_VALUE;
  1107. }
  1108. }
  1109. static uint8_t PlsrAddressIsDwordHalf(uint16_t address,
  1110. uint16_t *pairedAddress)
  1111. {
  1112. uint16_t offset;
  1113. switch (address)
  1114. {
  1115. case 0x100BU: case 0x100DU: case 0x1010U:
  1116. *pairedAddress = (uint16_t)(address + 1U);
  1117. return 1U;
  1118. case 0x100CU: case 0x100EU: case 0x1011U:
  1119. *pairedAddress = (uint16_t)(address - 1U);
  1120. return 1U;
  1121. default: break;
  1122. }
  1123. if ((address >= PLSR_SEGMENT_FIRST_ADDRESS)
  1124. && (address <= PLSR_CONFIG_LAST_ADDRESS))
  1125. {
  1126. offset = (uint16_t)((address - PLSR_SEGMENT_FIRST_ADDRESS)
  1127. % PLSR_SEGMENT_STRIDE);
  1128. if ((offset == 0U) || (offset == 2U))
  1129. {
  1130. *pairedAddress = (uint16_t)(address + 1U);
  1131. return 1U;
  1132. }
  1133. if ((offset == 1U) || (offset == 3U))
  1134. {
  1135. *pairedAddress = (uint16_t)(address - 1U);
  1136. return 1U;
  1137. }
  1138. }
  1139. return 0U;
  1140. }
  1141. static uint8_t PlsrAddressIsProduct(uint16_t address)
  1142. {
  1143. return (((address >= PLSR_CONFIG_FIRST_ADDRESS)
  1144. && (address <= PLSR_CONFIG_LAST_ADDRESS))
  1145. || (address == PLSR_OUTPUT_MODE_ADDRESS)
  1146. || ((address >= PLSR_STATUS_FIRST_ADDRESS)
  1147. && (address <= PLSR_STATUS_LAST_ADDRESS))
  1148. || ((address >= PLSR_DIAGNOSTIC_FIRST_ADDRESS)
  1149. && (address <= PLSR_DIAGNOSTIC_LAST_ADDRESS))
  1150. || ((address >= PLSR_QUEUE_DIAGNOSTIC_FIRST_ADDRESS)
  1151. && (address <= PLSR_QUEUE_DIAGNOSTIC_LAST_ADDRESS))
  1152. || (address == PLSR_CONTROL_ADDRESS)
  1153. || (address == PLSR_DIAGNOSTIC_CONTROL_ADDRESS)) ? 1U : 0U;
  1154. }
  1155. static PLSR_MB_RESULT PlsrClassifyRange(uint16_t startAddress,
  1156. uint16_t quantity)
  1157. {
  1158. uint32_t address;
  1159. uint32_t endAddress;
  1160. uint8_t foundProduct = 0U;
  1161. uint8_t foundOther = 0U;
  1162. if (quantity == 0U)
  1163. {
  1164. return PLSR_MB_ILLEGAL_VALUE;
  1165. }
  1166. endAddress = (uint32_t)startAddress + (uint32_t)quantity - 1UL;
  1167. if (endAddress > 0xFFFFUL)
  1168. {
  1169. return PLSR_MB_ILLEGAL_ADDRESS;
  1170. }
  1171. for (address = startAddress; address <= endAddress; address++)
  1172. {
  1173. if (PlsrAddressIsProduct((uint16_t)address) != 0U)
  1174. {
  1175. foundProduct = 1U;
  1176. }
  1177. else
  1178. {
  1179. foundOther = 1U;
  1180. }
  1181. }
  1182. if (foundProduct == 0U)
  1183. {
  1184. return PLSR_MB_NOT_HANDLED;
  1185. }
  1186. return (foundOther != 0U) ? PLSR_MB_ILLEGAL_ADDRESS : PLSR_MB_OK;
  1187. }
  1188. static uint32_t PlsrCurveProgressQ16(uint32_t elapsed,
  1189. uint32_t duration,
  1190. uint16_t curveMode)
  1191. {
  1192. uint32_t linear;
  1193. if ((duration == 0UL) || (elapsed >= duration))
  1194. {
  1195. return 65535UL;
  1196. }
  1197. linear = (uint32_t)(((uint64_t)elapsed * 65535UL) / duration);
  1198. if (curveMode == 1U)
  1199. {
  1200. uint64_t x = linear;
  1201. uint64_t x2 = (x * x) / 65535UL;
  1202. return (uint32_t)((x2 * (196605UL - 2UL * x)) / 65535UL);
  1203. }
  1204. if (curveMode == 2U)
  1205. {
  1206. uint32_t scaled = linear * 64UL;
  1207. uint32_t index = scaled / 65535UL;
  1208. uint32_t fraction = scaled % 65535UL;
  1209. uint32_t first;
  1210. uint32_t second;
  1211. if (index >= 64UL)
  1212. {
  1213. return 65535UL;
  1214. }
  1215. first = PlsrSineProgressQ16[index];
  1216. second = PlsrSineProgressQ16[index + 1UL];
  1217. return first + (uint32_t)(((uint64_t)(second - first) * fraction)
  1218. / 65535UL);
  1219. }
  1220. return linear;
  1221. }
  1222. static uint32_t PlsrRampDurationMs(uint32_t fromHz, uint32_t toHz)
  1223. {
  1224. uint32_t gap;
  1225. uint32_t baseTimeMs;
  1226. uint64_t duration;
  1227. if (fromHz == toHz)
  1228. {
  1229. return 0UL;
  1230. }
  1231. gap = (fromHz > toHz) ? (fromHz - toHz) : (toHz - fromHz);
  1232. baseTimeMs = (toHz > fromHz) ? PlsrActiveConfig.accelerationTimeMs
  1233. : PlsrActiveConfig.decelerationTimeMs;
  1234. if (baseTimeMs == 0UL)
  1235. {
  1236. return 0UL;
  1237. }
  1238. duration = ((uint64_t)gap * baseTimeMs
  1239. + PlsrActiveConfig.defaultSpeedHz - 1UL)
  1240. / PlsrActiveConfig.defaultSpeedHz;
  1241. if (duration > 0xFFFFFFFFUL)
  1242. {
  1243. return 0xFFFFFFFFUL;
  1244. }
  1245. return (uint32_t)duration;
  1246. }
  1247. static uint32_t PlsrEffectiveStartFrequency(uint32_t segmentFrequencyHz,
  1248. uint8_t allowCarry,
  1249. uint8_t directionChanged,
  1250. uint32_t carryFrequencyHz)
  1251. {
  1252. if ((allowCarry != 0U) && (directionChanged == 0U)
  1253. && (carryFrequencyHz != 0UL))
  1254. {
  1255. return carryFrequencyHz;
  1256. }
  1257. if (PlsrActiveConfig.accelerationTimeMs == 0U)
  1258. {
  1259. return segmentFrequencyHz;
  1260. }
  1261. return PlsrActiveConfig.startSpeedHz;
  1262. }
  1263. static uint32_t PlsrEffectiveStopFrequency(uint32_t segmentFrequencyHz)
  1264. {
  1265. return (PlsrActiveConfig.decelerationTimeMs == 0U)
  1266. ? segmentFrequencyHz : PlsrActiveConfig.stopSpeedHz;
  1267. }
  1268. static void PlsrRampStart(uint32_t fromHz, uint32_t toHz)
  1269. {
  1270. PlsrRamp.fromHz = fromHz;
  1271. PlsrRamp.toHz = toHz;
  1272. PlsrRamp.durationMs = PlsrRampDurationMs(fromHz, toHz);
  1273. PlsrRamp.elapsedMs = 0UL;
  1274. PlsrRamp.active = (PlsrRamp.durationMs != 0UL) ? 1U : 0U;
  1275. if (toHz > fromHz)
  1276. {
  1277. PlsrRunStatus = PLSR_STATUS_ACCELERATING;
  1278. }
  1279. else if (toHz < fromHz)
  1280. {
  1281. PlsrRunStatus = PLSR_STATUS_DECELERATING;
  1282. }
  1283. else
  1284. {
  1285. PlsrRunStatus = PLSR_STATUS_RUNNING;
  1286. }
  1287. }
  1288. static uint8_t PlsrApplyFrequencyPair(uint32_t requestedFirstFrequencyHz,
  1289. uint32_t requestedQueuedFrequencyHz,
  1290. uint32_t expectedEpoch)
  1291. {
  1292. uint32_t criticalState;
  1293. uint32_t actualFirstFrequencyHz;
  1294. uint32_t actualQueuedFrequencyHz;
  1295. uint32_t hardwareFirstFrequencyHz = requestedFirstFrequencyHz;
  1296. uint32_t hardwareQueuedFrequencyHz = requestedQueuedFrequencyHz;
  1297. PLSR_HANDOFF_PLAN candidatePlan;
  1298. uint8_t haveCandidatePlan = 0U;
  1299. criticalState = PlsrPlatformEnterCritical();
  1300. if ((PlsrSegmentEpoch != expectedEpoch)
  1301. || (PlsrBoundaryPending != 0U) || (PlsrRemainingPulses == 0UL))
  1302. {
  1303. PlsrPlatformExitCritical(criticalState);
  1304. return 1U;
  1305. }
  1306. if (hardwareFirstFrequencyHz == 0UL)
  1307. {
  1308. if (PlsrPulseActive == 0U)
  1309. {
  1310. PlsrCurrentFrequencyHz = 0UL;
  1311. PlsrQueuedFrequencyHz = 0UL;
  1312. PlsrPlatformExitCritical(criticalState);
  1313. return 1U;
  1314. }
  1315. hardwareFirstFrequencyHz = 1UL;
  1316. }
  1317. if (hardwareQueuedFrequencyHz == 0UL)
  1318. {
  1319. hardwareQueuedFrequencyHz = 1UL;
  1320. }
  1321. if (PlsrPulseActive != 0U)
  1322. {
  1323. if (PlsrAbStopArmed != 0U)
  1324. {
  1325. PlsrPlatformExitCritical(criticalState);
  1326. return 1U;
  1327. }
  1328. if (PlsrHandoffPlan.valid != 0U)
  1329. {
  1330. PlsrPlatformExitCritical(criticalState);
  1331. return 1U;
  1332. }
  1333. PlsrDeferredFrequencyHz = hardwareQueuedFrequencyHz;
  1334. PlsrDeferredFrequencyPending = 1U;
  1335. PlsrPlatformExitCritical(criticalState);
  1336. return 1U;
  1337. }
  1338. else
  1339. {
  1340. if (PlsrSelectPreparedHandoffPlan(&candidatePlan) != 0U)
  1341. {
  1342. hardwareQueuedFrequencyHz = candidatePlan.firstFrequencyHz;
  1343. haveCandidatePlan = 1U;
  1344. }
  1345. if (PlsrDiagnosticStartPulse(
  1346. (uint8_t)PlsrActiveConfig.pulseOutput,
  1347. hardwareFirstFrequencyHz, hardwareQueuedFrequencyHz,
  1348. &actualFirstFrequencyHz, &actualQueuedFrequencyHz) == 0U)
  1349. {
  1350. PlsrPlatformExitCritical(criticalState);
  1351. return 0U;
  1352. }
  1353. PlsrPulseActive = 1U;
  1354. PlsrCurrentFrequencyHz = actualFirstFrequencyHz;
  1355. PlsrHandoffPlan.valid = 0U;
  1356. if (haveCandidatePlan != 0U)
  1357. {
  1358. PlsrHandoffPlan = candidatePlan;
  1359. PlsrHandoffPlan.firstFrequencyHz = actualQueuedFrequencyHz;
  1360. PlsrHandoffPlan.valid = 1U;
  1361. }
  1362. }
  1363. PlsrQueuedFrequencyHz = actualQueuedFrequencyHz;
  1364. if (PlsrArmFinalAbBoundaryLocked() == 0U)
  1365. {
  1366. PlsrPlatformExitCritical(criticalState);
  1367. return 0U;
  1368. }
  1369. PlsrPlatformExitCritical(criticalState);
  1370. return 1U;
  1371. }
  1372. static PLSR_PLATFORM_QUEUE_RESULT PlsrApplyFrequency(
  1373. uint32_t requestedFrequencyHz,
  1374. uint32_t expectedEpoch)
  1375. {
  1376. return (PlsrApplyFrequencyPair(requestedFrequencyHz,
  1377. requestedFrequencyHz,
  1378. expectedEpoch) != 0U)
  1379. ? PLSR_PLATFORM_QUEUE_APPLIED
  1380. : PLSR_PLATFORM_QUEUE_FAILED;
  1381. }
  1382. static uint8_t PlsrStopDrainPulseCount(void)
  1383. {
  1384. uint8_t deferredPending = PlsrDeferredFrequencyPending;
  1385. uint32_t deferredFrequencyHz = PlsrDeferredFrequencyHz;
  1386. uint32_t queuedFrequencyHz = PlsrQueuedFrequencyHz;
  1387. uint32_t currentFrequencyHz = PlsrCurrentFrequencyHz;
  1388. if ((deferredPending != 0U)
  1389. && (deferredFrequencyHz != queuedFrequencyHz))
  1390. {
  1391. return 3U;
  1392. }
  1393. PlsrDeferredFrequencyPending = 0U;
  1394. return (currentFrequencyHz == queuedFrequencyHz) ? 1U : 2U;
  1395. }
  1396. static uint8_t PlsrArmStopDrainLocked(void)
  1397. {
  1398. uint32_t drainPulses = PlsrStopDrainPulseCount();
  1399. PlsrStopPulsesRemaining = (uint8_t)drainPulses;
  1400. return PlsrArmFinalAbBoundaryLocked();
  1401. }
  1402. static PLSR_PLATFORM_QUEUE_RESULT PlsrCommitDeferredFrequency(
  1403. uint8_t pulseOutput)
  1404. {
  1405. uint32_t requestedFrequencyHz;
  1406. uint32_t actualFrequencyHz;
  1407. PLSR_PLATFORM_QUEUE_RESULT result;
  1408. if (PlsrDeferredFrequencyPending == 0U)
  1409. {
  1410. return PLSR_PLATFORM_QUEUE_APPLIED;
  1411. }
  1412. requestedFrequencyHz = PlsrDeferredFrequencyHz;
  1413. result = PlsrDiagnosticQueueFrequency(pulseOutput, requestedFrequencyHz,
  1414. &actualFrequencyHz);
  1415. if (result != PLSR_PLATFORM_QUEUE_APPLIED)
  1416. {
  1417. return result;
  1418. }
  1419. if ((PlsrDeferredFrequencyPending != 0U)
  1420. && (PlsrDeferredFrequencyHz == requestedFrequencyHz))
  1421. {
  1422. PlsrDeferredFrequencyPending = 0U;
  1423. }
  1424. PlsrQueuedFrequencyHz = actualFrequencyHz;
  1425. return PLSR_PLATFORM_QUEUE_APPLIED;
  1426. }
  1427. static uint8_t PlsrRampAdvance(uint32_t expectedEpoch)
  1428. {
  1429. uint32_t progress;
  1430. uint32_t frequency;
  1431. uint32_t gap;
  1432. uint32_t criticalState;
  1433. uint32_t fromHz;
  1434. uint32_t toHz;
  1435. uint32_t durationMs;
  1436. uint32_t elapsedMs;
  1437. PLSR_PLATFORM_QUEUE_RESULT applyResult;
  1438. criticalState = PlsrPlatformEnterCritical();
  1439. if ((PlsrSegmentEpoch != expectedEpoch) || (PlsrRamp.active == 0U))
  1440. {
  1441. PlsrPlatformExitCritical(criticalState);
  1442. return 1U;
  1443. }
  1444. if (PlsrRamp.elapsedMs < PlsrRamp.durationMs)
  1445. {
  1446. PlsrRamp.elapsedMs++;
  1447. }
  1448. fromHz = PlsrRamp.fromHz;
  1449. toHz = PlsrRamp.toHz;
  1450. durationMs = PlsrRamp.durationMs;
  1451. elapsedMs = PlsrRamp.elapsedMs;
  1452. PlsrPlatformExitCritical(criticalState);
  1453. progress = PlsrCurveProgressQ16(elapsedMs,
  1454. durationMs,
  1455. PlsrActiveConfig.curveMode);
  1456. if (toHz >= fromHz)
  1457. {
  1458. gap = toHz - fromHz;
  1459. frequency = fromHz
  1460. + (uint32_t)(((uint64_t)gap * progress) / 65535UL);
  1461. }
  1462. else
  1463. {
  1464. gap = fromHz - toHz;
  1465. frequency = fromHz
  1466. - (uint32_t)(((uint64_t)gap * progress) / 65535UL);
  1467. }
  1468. applyResult = PlsrApplyFrequency(frequency, expectedEpoch);
  1469. if (applyResult == PLSR_PLATFORM_QUEUE_FAILED)
  1470. {
  1471. return 0U;
  1472. }
  1473. if (applyResult == PLSR_PLATFORM_QUEUE_STALE)
  1474. {
  1475. return 1U;
  1476. }
  1477. criticalState = PlsrPlatformEnterCritical();
  1478. if (PlsrSegmentEpoch != expectedEpoch)
  1479. {
  1480. PlsrPlatformExitCritical(criticalState);
  1481. return 1U;
  1482. }
  1483. if (elapsedMs >= durationMs)
  1484. {
  1485. PlsrRamp.active = 0U;
  1486. if (PlsrStopRequested == 0U)
  1487. {
  1488. PlsrRunStatus = PLSR_STATUS_RUNNING;
  1489. }
  1490. else
  1491. {
  1492. (void)PlsrArmStopDrainLocked();
  1493. }
  1494. }
  1495. PlsrPlatformExitCritical(criticalState);
  1496. return 1U;
  1497. }
  1498. static uint8_t PlsrGetNextSegment(uint8_t *nextSegment)
  1499. {
  1500. const PLSR_SEGMENT_CONFIG *segment =
  1501. &PlsrActiveConfig.segments[PlsrCurrentSegment - 1U];
  1502. if (segment->jumpSegment != 0U)
  1503. {
  1504. *nextSegment = (uint8_t)segment->jumpSegment;
  1505. return 1U;
  1506. }
  1507. if (PlsrCurrentSegment < PlsrActiveConfig.segmentCount)
  1508. {
  1509. *nextSegment = (uint8_t)(PlsrCurrentSegment + 1U);
  1510. return 1U;
  1511. }
  1512. return 0U;
  1513. }
  1514. static uint32_t PlsrResolvedSegmentFrequency(const PLSR_CONFIG *config,
  1515. uint8_t segmentIndex)
  1516. {
  1517. uint32_t frequencyHz = config->segments[segmentIndex].frequencyHz;
  1518. return (frequencyHz == 0UL) ? config->defaultSpeedHz : frequencyHz;
  1519. }
  1520. static int64_t PlsrSegmentDisplacement(uint8_t segmentNumber,
  1521. int32_t referencePosition)
  1522. {
  1523. int32_t configured =
  1524. PlsrActiveConfig.segments[segmentNumber - 1U].pulses;
  1525. if (PlsrActiveConfig.positionMode == PLSR_POSITION_ABSOLUTE)
  1526. {
  1527. return (int64_t)configured - (int64_t)referencePosition;
  1528. }
  1529. return configured;
  1530. }
  1531. static uint8_t PlsrPredictNextDirection(uint8_t nextSegment,
  1532. uint8_t *positive)
  1533. {
  1534. uint32_t criticalState;
  1535. uint64_t remaining;
  1536. int32_t position;
  1537. uint32_t predictedBits;
  1538. int32_t predictedPosition;
  1539. int64_t displacement;
  1540. criticalState = PlsrPlatformEnterCritical();
  1541. remaining = PlsrRemainingPulses;
  1542. position = PlsrPosition;
  1543. PlsrPlatformExitCritical(criticalState);
  1544. predictedBits = (uint32_t)position;
  1545. if (PlsrCountPositive != 0U)
  1546. {
  1547. predictedBits += (uint32_t)remaining;
  1548. }
  1549. else
  1550. {
  1551. predictedBits -= (uint32_t)remaining;
  1552. }
  1553. predictedPosition = (int32_t)predictedBits;
  1554. displacement = PlsrSegmentDisplacement(nextSegment, predictedPosition);
  1555. if (displacement == 0)
  1556. {
  1557. return 0U;
  1558. }
  1559. *positive = (displacement > 0) ? 1U : 0U;
  1560. return 1U;
  1561. }
  1562. static uint64_t PlsrRemainingSnapshot(void)
  1563. {
  1564. uint32_t criticalState = PlsrPlatformEnterCritical();
  1565. uint64_t remaining = PlsrRemainingPulses;
  1566. PlsrPlatformExitCritical(criticalState);
  1567. return remaining;
  1568. }
  1569. static uint64_t PlsrRampPulseEstimate(uint32_t fromHz,
  1570. uint32_t toHz,
  1571. uint32_t rateBoundHz)
  1572. {
  1573. uint32_t duration = PlsrRampDurationMs(fromHz, toHz);
  1574. uint64_t sum = (uint64_t)fromHz + toHz;
  1575. uint64_t discreteArea = sum * duration;
  1576. uint64_t rampArea;
  1577. uint64_t pollReserve = 0ULL;
  1578. uint64_t candidateReserve =
  1579. ((uint64_t)rateBoundHz + 999UL) / 1000UL;
  1580. uint64_t endpointCorrection = 0ULL;
  1581. /* RampAdvance applies samples at 1/duration through duration/duration.
  1582. For all supported symmetric curves this right-endpoint sum differs
  1583. from the continuous trapezoid by half the endpoint delta. */
  1584. if (toHz >= fromHz)
  1585. {
  1586. discreteArea += (uint64_t)toHz - fromHz;
  1587. }
  1588. else
  1589. {
  1590. discreteArea -= (uint64_t)fromHz - toHz;
  1591. endpointCorrection =
  1592. ((uint64_t)fromHz - toHz + 1999UL) / 2000UL;
  1593. }
  1594. rampArea = (discreteArea + 1999UL) / 2000UL;
  1595. if (candidateReserve
  1596. <= rampArea / PLSR_RAMP_POLL_RESERVE_AREA_DIVISOR)
  1597. {
  1598. pollReserve = candidateReserve + endpointCorrection;
  1599. }
  1600. /* A full poll reserve on shorter ramps would dominate their area and
  1601. leave stop-speed tail pulses. */
  1602. return rampArea + pollReserve;
  1603. }
  1604. static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile,
  1605. uint8_t segmentNumber,
  1606. uint32_t startFrequencyHz,
  1607. uint32_t targetFrequencyHz,
  1608. uint64_t pulseCount,
  1609. uint8_t currentPositive)
  1610. {
  1611. PLSR_MOTION_BLOCK block;
  1612. PLSR_PLANNER_STATUS status;
  1613. const PLSR_SEGMENT_CONFIG *segment;
  1614. uint8_t nextSegment = 0U;
  1615. uint8_t nextPositive;
  1616. (void)memset(profile, 0, sizeof(*profile));
  1617. if ((PlsrActiveConfig.outputMode != PLSR_OUTPUT_PULSE_DIR)
  1618. || (pulseCount == 0ULL) || (pulseCount > 0xFFFFFFFFULL)
  1619. || (segmentNumber == 0U)
  1620. || (segmentNumber > PlsrActiveConfig.segmentCount))
  1621. {
  1622. return 0U;
  1623. }
  1624. block.entryHz = startFrequencyHz;
  1625. block.cruiseHz = targetFrequencyHz;
  1626. block.exitHz = PlsrEffectiveStopFrequency(targetFrequencyHz);
  1627. block.pulseBudget = (uint32_t)pulseCount;
  1628. block.referenceSpeedHz = PlsrActiveConfig.defaultSpeedHz;
  1629. block.accelerationTimeMs = PlsrActiveConfig.accelerationTimeMs;
  1630. block.decelerationTimeMs = PlsrActiveConfig.decelerationTimeMs;
  1631. block.curveMode = PlsrActiveConfig.curveMode;
  1632. block.pulseOutput = (uint8_t)PlsrActiveConfig.pulseOutput;
  1633. block.boundary = PLSR_BOUNDARY_STOP;
  1634. segment = &PlsrActiveConfig.segments[segmentNumber - 1U];
  1635. if (segment->waitType == PLSR_EXT_OR_COMPLETE)
  1636. {
  1637. nextSegment = (segment->jumpSegment != 0U)
  1638. ? (uint8_t)segment->jumpSegment
  1639. : ((segmentNumber < PlsrActiveConfig.segmentCount)
  1640. ? (uint8_t)(segmentNumber + 1U) : 0U);
  1641. }
  1642. if (nextSegment != 0U)
  1643. {
  1644. if (PlsrActiveConfig.positionMode == PLSR_POSITION_RELATIVE)
  1645. {
  1646. currentPositive = (segment->pulses >= 0L) ? 1U : 0U;
  1647. nextPositive =
  1648. (PlsrActiveConfig.segments[nextSegment - 1U].pulses >= 0L)
  1649. ? 1U : 0U;
  1650. }
  1651. else if ((int64_t)PlsrActiveConfig.segments[nextSegment - 1U].pulses
  1652. != (int64_t)segment->pulses)
  1653. {
  1654. nextPositive =
  1655. ((int64_t)PlsrActiveConfig.segments[nextSegment - 1U].pulses
  1656. > (int64_t)segment->pulses) ? 1U : 0U;
  1657. }
  1658. else
  1659. {
  1660. nextPositive = (uint8_t)(currentPositive ^ 1U);
  1661. }
  1662. if (nextPositive == currentPositive)
  1663. {
  1664. block.exitHz = targetFrequencyHz;
  1665. block.boundary = PLSR_BOUNDARY_NEXT_CARRY;
  1666. }
  1667. }
  1668. status = PlsrPlannerBegin(&profile->planner, &block,
  1669. startFrequencyHz, 0ULL);
  1670. if ((status != PLSR_PLANNER_OK)
  1671. && (status != PLSR_PLANNER_CLIPPED))
  1672. {
  1673. return 0U;
  1674. }
  1675. profile->startHz = profile->planner.startHz;
  1676. profile->peakHz = profile->planner.peakHz;
  1677. profile->endHz = profile->planner.endHz;
  1678. profile->pulseCount = profile->planner.block.pulseBudget;
  1679. profile->entryPulses = profile->planner.entryPulses;
  1680. profile->steadyPulses = profile->planner.steadyPulses;
  1681. profile->exitPulses = profile->planner.exitPulses;
  1682. profile->active = 1U;
  1683. return 1U;
  1684. }
  1685. static uint8_t PlsrShortProfileTakeRun(PLSR_SHORT_PROFILE *profile,
  1686. PLSR_PROFILE_ENTRY *entry)
  1687. {
  1688. return PlsrShortProfileTakeRunLimited(profile, entry, 0xFFFFFFFFUL);
  1689. }
  1690. static uint8_t PlsrShortProfileTakeRunLimited(
  1691. PLSR_SHORT_PROFILE *profile,
  1692. PLSR_PROFILE_ENTRY *entry,
  1693. uint32_t maximumRepeats)
  1694. {
  1695. uint32_t repeatCount;
  1696. if ((profile->nextPeriod >= profile->pulseCount)
  1697. || (entry == NULL) || (maximumRepeats == 0UL))
  1698. {
  1699. return 0U;
  1700. }
  1701. if (profile->pendingRepeats == 0UL)
  1702. {
  1703. if (PlsrPlannerGenerate(&profile->planner,
  1704. &profile->pendingItem, 1U) == 0U)
  1705. {
  1706. profile->active = 0U;
  1707. return 0U;
  1708. }
  1709. profile->pendingRepeats = profile->pendingItem.repeatCount;
  1710. }
  1711. repeatCount = profile->pendingRepeats;
  1712. if (repeatCount > profile->pulseCount - profile->nextPeriod)
  1713. {
  1714. repeatCount = profile->pulseCount - profile->nextPeriod;
  1715. }
  1716. if (repeatCount > maximumRepeats)
  1717. {
  1718. repeatCount = maximumRepeats;
  1719. }
  1720. if (repeatCount == 0UL)
  1721. {
  1722. profile->active = 0U;
  1723. return 0U;
  1724. }
  1725. entry->setting = profile->pendingItem.setting;
  1726. entry->requestedFrequencyHz =
  1727. profile->pendingItem.requestedFrequencyHz;
  1728. entry->repeatCount = repeatCount;
  1729. entry->startsNextSegment = 0U;
  1730. profile->pendingRepeats -= repeatCount;
  1731. profile->nextPeriod += repeatCount;
  1732. profile->lastRampFrequencyHz = entry->setting.actualFrequencyHz;
  1733. if (profile->nextPeriod >= profile->pulseCount)
  1734. {
  1735. profile->active = 0U;
  1736. }
  1737. return 1U;
  1738. }
  1739. /* Build the replacement stream in the inactive queue bank while the IRQ keeps
  1740. consuming the published bank. A bounded prefix from the old queue bridges
  1741. the construction interval; the final bank flip is the only critical part. */
  1742. static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir(
  1743. uint32_t targetHz,
  1744. uint32_t totalPulses)
  1745. {
  1746. PLSR_PROFILE_QUEUE *source;
  1747. PLSR_PROFILE_QUEUE *destination;
  1748. PLSR_SHORT_PROFILE replacement;
  1749. PLSR_SHORT_PROFILE candidate;
  1750. PLSR_PROFILE_ENTRY entry;
  1751. uint64_t prefixTimeUs = 0ULL;
  1752. uint64_t prefixPulses = 0ULL;
  1753. uint32_t committedPulses;
  1754. uint32_t tailFrequencyHz;
  1755. uint32_t sourceGeneration;
  1756. uint32_t snapshotReadIndex;
  1757. uint32_t snapshotWriteIndex;
  1758. uint32_t currentReadIndex;
  1759. uint32_t availablePlanPulses;
  1760. uint32_t replacementPulses;
  1761. uint32_t prefixCount = 0UL;
  1762. uint32_t replacementStartIndex;
  1763. uint32_t consumedPrefix;
  1764. uint32_t sourceIndex;
  1765. uint32_t criticalState;
  1766. uint32_t epoch;
  1767. uint8_t segmentNumber;
  1768. uint8_t handoffBank;
  1769. uint8_t sourceBank;
  1770. uint8_t destinationBank;
  1771. uint8_t boundaryWriteIndex;
  1772. uint8_t pipelineStartsNextSegment = 0U;
  1773. uint8_t tailWasClipped = 0U;
  1774. uint16_t fillBudget = PLSR_REPLAN_BUILD_ITEMS;
  1775. criticalState = PlsrPlatformEnterCritical();
  1776. sourceBank = PlsrProfileQueueBank;
  1777. destinationBank = (uint8_t)(sourceBank ^ 1U);
  1778. source = &PlsrProfileQueues[sourceBank];
  1779. destination = &PlsrProfileQueues[destinationBank];
  1780. sourceGeneration = source->generation;
  1781. snapshotReadIndex = source->readIndex;
  1782. snapshotWriteIndex = source->writeIndex;
  1783. epoch = PlsrSegmentEpoch;
  1784. segmentNumber = PlsrCurrentSegment;
  1785. handoffBank = PlsrPreparedHandoffBank;
  1786. boundaryWriteIndex = PlsrCountedBoundaryWriteIndex;
  1787. if ((PlsrPulseActive == 0U) || (segmentNumber == 0U)
  1788. || (segmentNumber > PlsrActiveConfig.segmentCount)
  1789. || (PlsrCountedFaultEvent != 0U))
  1790. {
  1791. PlsrPlatformExitCritical(criticalState);
  1792. return PLSR_PLATFORM_SERVICE_FAILED;
  1793. }
  1794. if (boundaryWriteIndex != PlsrCountedBoundaryReadIndex)
  1795. {
  1796. PlsrPlatformExitCritical(criticalState);
  1797. return PLSR_PLATFORM_SERVICE_DEFERRED;
  1798. }
  1799. if (PlsrPlatformFinitePipelineSnapshot(
  1800. (uint8_t)PlsrActiveConfig.pulseOutput,
  1801. &committedPulses, &tailFrequencyHz,
  1802. &pipelineStartsNextSegment) == 0U)
  1803. {
  1804. PlsrPlatformExitCritical(criticalState);
  1805. return PLSR_PLATFORM_SERVICE_FAILED;
  1806. }
  1807. if (pipelineStartsNextSegment != 0U)
  1808. {
  1809. PlsrPlatformExitCritical(criticalState);
  1810. return PLSR_PLATFORM_SERVICE_DEFERRED;
  1811. }
  1812. /* The replacement stream owns the inactive bank from this point. */
  1813. PlsrInvalidateTimedStartLocked();
  1814. PlsrPlatformExitCritical(criticalState);
  1815. if (totalPulses < committedPulses)
  1816. {
  1817. totalPulses = committedPulses;
  1818. }
  1819. availablePlanPulses = totalPulses - committedPulses;
  1820. destination->readIndex = 0UL;
  1821. destination->writeIndex = 0UL;
  1822. destination->repeatRemaining = 0UL;
  1823. destination->underrunDebtPulses = 0UL;
  1824. destination->haveLastSetting = 0U;
  1825. destination->producerEpoch = epoch;
  1826. destination->producerSegment = segmentNumber;
  1827. destination->preparedHandoffBank = handoffBank;
  1828. destination->generatorComplete = 0U;
  1829. destination->active = 0U;
  1830. destination->producerProfile.active = 0U;
  1831. sourceIndex = snapshotReadIndex;
  1832. while ((sourceIndex != snapshotWriteIndex)
  1833. && (prefixCount < PLSR_REPLAN_PREFIX_ITEMS)
  1834. && (prefixPulses < availablePlanPulses)
  1835. && (prefixTimeUs < PLSR_REPLAN_PREFIX_TIME_US))
  1836. {
  1837. uint32_t remainingPrefixPulses =
  1838. availablePlanPulses - (uint32_t)prefixPulses;
  1839. uint32_t frequencyHz;
  1840. uint64_t remainingPrefixTimeUs;
  1841. uint64_t timeLimitedPulses;
  1842. entry = source->entries[sourceIndex & PLSR_PROFILE_QUEUE_MASK];
  1843. if ((entry.repeatCount == 0UL)
  1844. || (entry.startsNextSegment != 0U))
  1845. {
  1846. break;
  1847. }
  1848. frequencyHz = entry.setting.actualFrequencyHz;
  1849. if (frequencyHz == 0UL)
  1850. {
  1851. return PLSR_PLATFORM_SERVICE_FAILED;
  1852. }
  1853. remainingPrefixTimeUs =
  1854. PLSR_REPLAN_PREFIX_TIME_US - prefixTimeUs;
  1855. timeLimitedPulses =
  1856. (remainingPrefixTimeUs * frequencyHz + 999999ULL) / 1000000ULL;
  1857. if (timeLimitedPulses == 0ULL)
  1858. {
  1859. timeLimitedPulses = 1ULL;
  1860. }
  1861. if (entry.repeatCount > timeLimitedPulses)
  1862. {
  1863. entry.repeatCount = (uint32_t)timeLimitedPulses;
  1864. tailWasClipped = 1U;
  1865. }
  1866. if (entry.repeatCount > remainingPrefixPulses)
  1867. {
  1868. entry.repeatCount = remainingPrefixPulses;
  1869. tailWasClipped = 1U;
  1870. }
  1871. destination->entries[prefixCount] = entry;
  1872. prefixCount++;
  1873. sourceIndex++;
  1874. prefixPulses += entry.repeatCount;
  1875. prefixTimeUs +=
  1876. ((uint64_t)entry.repeatCount * 1000000ULL
  1877. + frequencyHz - 1UL) / frequencyHz;
  1878. tailFrequencyHz = frequencyHz;
  1879. }
  1880. replacementPulses = availablePlanPulses - (uint32_t)prefixPulses;
  1881. replacementStartIndex = prefixCount;
  1882. if (replacementPulses != 0UL)
  1883. {
  1884. if ((tailFrequencyHz == 0UL)
  1885. || (PlsrPrepareShortProfile(&replacement, segmentNumber,
  1886. tailFrequencyHz, targetHz,
  1887. replacementPulses,
  1888. PlsrCountPositive) == 0U))
  1889. {
  1890. return PLSR_PLATFORM_SERVICE_FAILED;
  1891. }
  1892. PlsrCopyShortProfile(&destination->producerProfile, &replacement);
  1893. destination->active = 1U;
  1894. candidate = replacement;
  1895. while ((fillBudget != 0U)
  1896. && (prefixCount < PLSR_PROFILE_QUEUE_CAPACITY)
  1897. && (candidate.active != 0U))
  1898. {
  1899. fillBudget--;
  1900. if (PlsrShortProfileTakeRun(&candidate, &entry) == 0U)
  1901. {
  1902. return PLSR_PLATFORM_SERVICE_FAILED;
  1903. }
  1904. if ((prefixCount > replacementStartIndex)
  1905. && (destination->entries[prefixCount - 1UL]
  1906. .startsNextSegment == 0U)
  1907. && (PlsrTimerSettingsEqual(
  1908. &destination->entries[prefixCount - 1UL].setting,
  1909. &entry.setting) != 0U)
  1910. && (destination->entries[prefixCount - 1UL].repeatCount
  1911. <= (0xFFFFFFFFUL - entry.repeatCount)))
  1912. {
  1913. destination->entries[prefixCount - 1UL].repeatCount +=
  1914. entry.repeatCount;
  1915. }
  1916. else
  1917. {
  1918. destination->entries[prefixCount++] = entry;
  1919. }
  1920. }
  1921. PlsrCopyShortProfile(&destination->producerProfile, &candidate);
  1922. destination->generatorComplete =
  1923. ((candidate.active == 0U)
  1924. || (candidate.nextPeriod >= candidate.pulseCount)) ? 1U : 0U;
  1925. }
  1926. else
  1927. {
  1928. (void)memset(&replacement, 0, sizeof(replacement));
  1929. replacement.endHz = tailFrequencyHz;
  1930. destination->generatorComplete = 1U;
  1931. destination->active = (prefixCount != 0UL) ? 1U : 0U;
  1932. }
  1933. destination->writeIndex = prefixCount;
  1934. criticalState = PlsrPlatformEnterCritical();
  1935. source = &PlsrProfileQueues[sourceBank];
  1936. currentReadIndex = source->readIndex;
  1937. if ((PlsrProfileQueueBank != sourceBank)
  1938. || (source->generation != sourceGeneration)
  1939. || (source->writeIndex != snapshotWriteIndex)
  1940. || (PlsrSegmentEpoch != epoch)
  1941. || (PlsrCurrentSegment != segmentNumber)
  1942. || (PlsrCountedBoundaryWriteIndex != boundaryWriteIndex)
  1943. || (PlsrCountedFaultEvent != 0U)
  1944. || (currentReadIndex < snapshotReadIndex)
  1945. || ((currentReadIndex - snapshotReadIndex) > prefixCount)
  1946. || ((tailWasClipped != 0U)
  1947. && ((currentReadIndex - snapshotReadIndex) >= prefixCount)))
  1948. {
  1949. PlsrPlatformExitCritical(criticalState);
  1950. return PLSR_PLATFORM_SERVICE_DEFERRED;
  1951. }
  1952. consumedPrefix = currentReadIndex - snapshotReadIndex;
  1953. destination->readIndex = consumedPrefix;
  1954. destination->generation = sourceGeneration + 1UL;
  1955. PlsrRamp.active = 0U;
  1956. PlsrHandoffPlan.valid = 0U;
  1957. PlsrCountedHandoffStaged = 0U;
  1958. PlsrProfileQueueBank = destinationBank;
  1959. PlsrCopyShortProfile(&PlsrShortProfile,
  1960. &destination->producerProfile);
  1961. PlsrRemainingPulses = totalPulses;
  1962. PlsrProfileQueueMinimumDepth =
  1963. (uint16_t)(prefixCount - consumedPrefix);
  1964. PlsrProfileQueueLowWaterLatched = 0U;
  1965. PlsrExecutor.generation++;
  1966. PlsrExecutor.mode = (destination->generatorComplete != 0U)
  1967. ? PLSR_EXEC_STEP_TABLE : PLSR_EXEC_STREAM;
  1968. PlsrPlatformExitCritical(criticalState);
  1969. PlsrProfileRecordPlannerStatus(&replacement);
  1970. PlsrRefreshCurrentHandoffPlan(replacement.endHz);
  1971. return PLSR_PLATFORM_SERVICE_READY;
  1972. }
  1973. static void PlsrCopyShortProfile(PLSR_SHORT_PROFILE *destination,
  1974. const PLSR_SHORT_PROFILE *source)
  1975. {
  1976. *destination = *source;
  1977. }
  1978. static uint16_t PlsrProfileQueueCount(void)
  1979. {
  1980. uint32_t writeIndex = PlsrProfileQueue.writeIndex;
  1981. uint32_t readIndex = PlsrProfileQueue.readIndex;
  1982. return (uint16_t)(writeIndex - readIndex);
  1983. }
  1984. static void PlsrProfileQueueRecordDepth(uint16_t depth)
  1985. {
  1986. if (depth < PlsrProfileQueueMinimumDepth)
  1987. {
  1988. PlsrProfileQueueMinimumDepth = depth;
  1989. }
  1990. if (depth <= PLSR_PROFILE_LOW_WATER)
  1991. {
  1992. PlsrProfileQueueDiagnosticFlags |= PLSR_QUEUE_DIAG_LOW_WATER;
  1993. if (PlsrProfileQueueLowWaterLatched == 0U)
  1994. {
  1995. PlsrProfileQueueLowWaterLatched = 1U;
  1996. if (PlsrProfileQueueLowWaterEvents != 0xFFFFFFFFUL)
  1997. {
  1998. PlsrProfileQueueLowWaterEvents++;
  1999. }
  2000. }
  2001. }
  2002. else if (depth >= PLSR_PROFILE_LOW_WATER_CLEAR)
  2003. {
  2004. PlsrProfileQueueLowWaterLatched = 0U;
  2005. }
  2006. }
  2007. static void PlsrProfileQueueRecordUnderrun(void)
  2008. {
  2009. PlsrProfileQueueDiagnosticFlags |= PLSR_QUEUE_DIAG_UNDERRUN;
  2010. if (PlsrProfileQueueUnderruns != 0xFFFFFFFFUL)
  2011. {
  2012. PlsrProfileQueueUnderruns++;
  2013. }
  2014. }
  2015. static void PlsrProfileRecordPlannerStatus(
  2016. const PLSR_SHORT_PROFILE *profile)
  2017. {
  2018. if ((profile != NULL) && (profile->planner.clipped != 0U))
  2019. {
  2020. uint32_t criticalState = PlsrPlatformEnterCritical();
  2021. PlsrProfileQueueDiagnosticFlags |=
  2022. PLSR_QUEUE_DIAG_PLANNER_CLIPPED;
  2023. if (PlsrPlannerClippedCount != 0xFFFFFFFFUL)
  2024. {
  2025. PlsrPlannerClippedCount++;
  2026. }
  2027. PlsrPlatformExitCritical(criticalState);
  2028. }
  2029. }
  2030. static void PlsrInvalidateTimedStartLocked(void)
  2031. {
  2032. PlsrTimedStart.building = 0U;
  2033. PlsrTimedStart.valid = 0U;
  2034. PlsrTimedStart.pendingActivation = 0U;
  2035. }
  2036. static void PlsrProfileQueueResetBankLocked(uint8_t bank)
  2037. {
  2038. PLSR_PROFILE_QUEUE *queue = &PlsrProfileQueues[bank & 1U];
  2039. queue->generation++;
  2040. queue->readIndex = 0U;
  2041. queue->writeIndex = 0U;
  2042. queue->active = 0U;
  2043. queue->generatorComplete = 0U;
  2044. queue->producerEpoch = 0UL;
  2045. queue->repeatRemaining = 0UL;
  2046. queue->underrunDebtPulses = 0UL;
  2047. queue->producerSegment = 0U;
  2048. queue->preparedHandoffBank = 0U;
  2049. queue->haveLastSetting = 0U;
  2050. queue->producerProfile.active = 0U;
  2051. }
  2052. static void PlsrProfileQueueReset(void)
  2053. {
  2054. uint32_t criticalState = PlsrPlatformEnterCritical();
  2055. uint8_t bank;
  2056. PlsrInvalidateTimedStartLocked();
  2057. for (bank = 0U; bank < 2U; bank++)
  2058. {
  2059. PlsrProfileQueueResetBankLocked(bank);
  2060. }
  2061. PlsrProfileQueueBank = 0U;
  2062. PlsrProfileQueueLowWaterLatched = 0U;
  2063. PlsrPlatformExitCritical(criticalState);
  2064. }
  2065. static uint8_t PlsrProfileQueueBegin(
  2066. const PLSR_SHORT_PROFILE *producerProfile,
  2067. uint32_t producerEpoch,
  2068. uint8_t producerSegment,
  2069. uint8_t preparedHandoffBank)
  2070. {
  2071. uint32_t criticalState = PlsrPlatformEnterCritical();
  2072. PLSR_PROFILE_QUEUE *queue =
  2073. &PlsrProfileQueues[PlsrProfileQueueBank];
  2074. queue->generation++;
  2075. queue->readIndex = 0U;
  2076. queue->writeIndex = 0U;
  2077. queue->producerEpoch = producerEpoch;
  2078. queue->producerSegment = producerSegment;
  2079. queue->preparedHandoffBank = preparedHandoffBank;
  2080. queue->generatorComplete = 0U;
  2081. queue->repeatRemaining = 0UL;
  2082. queue->underrunDebtPulses = 0UL;
  2083. queue->haveLastSetting = 0U;
  2084. PlsrProfileQueueMinimumDepth = PLSR_PROFILE_QUEUE_CAPACITY;
  2085. PlsrProfileQueueLowWaterLatched = 0U;
  2086. PlsrCopyShortProfile(&queue->producerProfile, producerProfile);
  2087. PlsrProfileRecordPlannerStatus(producerProfile);
  2088. queue->active =
  2089. ((producerProfile->active != 0U)
  2090. && (producerProfile->nextPeriod < producerProfile->pulseCount))
  2091. ? 1U : 0U;
  2092. if (queue->active == 0U)
  2093. {
  2094. queue->generatorComplete = 1U;
  2095. }
  2096. PlsrPlatformExitCritical(criticalState);
  2097. return 1U;
  2098. }
  2099. static void PlsrProfileQueueInvalidateGeneration(void)
  2100. {
  2101. uint32_t criticalState = PlsrPlatformEnterCritical();
  2102. PLSR_PROFILE_QUEUE *queue =
  2103. &PlsrProfileQueues[PlsrProfileQueueBank];
  2104. queue->generation++;
  2105. queue->active = 0U;
  2106. queue->generatorComplete = 0U;
  2107. queue->readIndex = 0U;
  2108. queue->writeIndex = 0U;
  2109. queue->repeatRemaining = 0UL;
  2110. queue->underrunDebtPulses = 0UL;
  2111. queue->haveLastSetting = 0U;
  2112. PlsrPlatformExitCritical(criticalState);
  2113. }
  2114. /* Called with pulse IRQs masked. Quantized-equal adjacent settings form one
  2115. hardware run even if their requested ramp frequencies differ. Segment
  2116. markers remain separate because their completion owns the boundary event. */
  2117. static uint8_t PlsrTimerSettingsEqual(
  2118. const PLSR_PLATFORM_TIMER_SETTING *first,
  2119. const PLSR_PLATFORM_TIMER_SETTING *second)
  2120. {
  2121. return ((first->actualFrequencyHz == second->actualFrequencyHz)
  2122. && (first->prescaler == second->prescaler)
  2123. && (first->pairPrescaler == second->pairPrescaler)
  2124. && (first->period == second->period)
  2125. && (first->compare == second->compare)) ? 1U : 0U;
  2126. }
  2127. static uint8_t PlsrProfileQueueAppendLocked(
  2128. PLSR_PROFILE_QUEUE *queue,
  2129. const PLSR_PROFILE_ENTRY *entry)
  2130. {
  2131. PLSR_PROFILE_ENTRY *previous;
  2132. uint32_t writeIndex;
  2133. if ((queue == NULL) || (entry == NULL) || (entry->repeatCount == 0UL))
  2134. {
  2135. return 0U;
  2136. }
  2137. writeIndex = queue->writeIndex;
  2138. if (writeIndex != queue->readIndex)
  2139. {
  2140. previous = &queue->entries[
  2141. (writeIndex - 1UL) & PLSR_PROFILE_QUEUE_MASK];
  2142. if ((previous->startsNextSegment == 0U)
  2143. && (entry->startsNextSegment == 0U)
  2144. && (PlsrTimerSettingsEqual(
  2145. &previous->setting, &entry->setting) != 0U)
  2146. && (previous->repeatCount
  2147. <= (0xFFFFFFFFUL - entry->repeatCount)))
  2148. {
  2149. previous->repeatCount += entry->repeatCount;
  2150. return 1U;
  2151. }
  2152. }
  2153. if ((writeIndex - queue->readIndex) >= PLSR_PROFILE_QUEUE_CAPACITY)
  2154. {
  2155. return 0U;
  2156. }
  2157. queue->entries[writeIndex & PLSR_PROFILE_QUEUE_MASK] = *entry;
  2158. queue->writeIndex = writeIndex + 1UL;
  2159. return 1U;
  2160. }
  2161. static void PlsrServiceTimedStartPreparation(void)
  2162. {
  2163. PLSR_PROFILE_QUEUE *queue;
  2164. PLSR_SHORT_PROFILE profile;
  2165. PLSR_PROFILE_ENTRY entry;
  2166. PLSR_HANDOFF_PLAN *handoff;
  2167. const PLSR_SEGMENT_CONFIG *source;
  2168. uint32_t criticalState;
  2169. uint32_t sourceEpoch;
  2170. uint32_t queueGeneration;
  2171. uint32_t targetFrequencyHz;
  2172. uint32_t startFrequencyHz;
  2173. uint64_t remaining;
  2174. int64_t predictedPosition;
  2175. int64_t displacement;
  2176. uint64_t magnitude;
  2177. int32_t position;
  2178. uint16_t budget;
  2179. uint8_t sourceSegment;
  2180. uint8_t nextSegment;
  2181. uint8_t positive;
  2182. uint8_t directionLevel;
  2183. uint8_t directionChanged;
  2184. uint8_t queueBank;
  2185. if ((PlsrActiveConfig.outputMode != PLSR_OUTPUT_PULSE_DIR)
  2186. || (PlsrStopRequested != 0U)
  2187. || (PlsrTimedStart.pendingActivation != 0U))
  2188. {
  2189. return;
  2190. }
  2191. criticalState = PlsrPlatformEnterCritical();
  2192. sourceEpoch = PlsrSegmentEpoch;
  2193. sourceSegment = PlsrCurrentSegment;
  2194. if ((sourceSegment == 0U)
  2195. || (sourceSegment > PlsrActiveConfig.segmentCount)
  2196. || ((PlsrActiveConfig.segments[sourceSegment - 1U].waitType
  2197. != PLSR_WAIT_TIME)
  2198. && (PlsrActiveConfig.segments[sourceSegment - 1U].waitType
  2199. != PLSR_WAIT_SIGNAL)))
  2200. {
  2201. PlsrInvalidateTimedStartLocked();
  2202. PlsrPlatformExitCritical(criticalState);
  2203. return;
  2204. }
  2205. if ((PlsrTimedStart.building != 0U)
  2206. || (PlsrTimedStart.valid != 0U))
  2207. {
  2208. if ((PlsrTimedStart.sourceEpoch != sourceEpoch)
  2209. || (PlsrTimedStart.sourceSegment != sourceSegment)
  2210. || (PlsrTimedStart.queueBank == PlsrProfileQueueBank)
  2211. || (PlsrProfileQueues[PlsrTimedStart.queueBank].generation
  2212. != PlsrTimedStart.queueGeneration))
  2213. {
  2214. PlsrInvalidateTimedStartLocked();
  2215. }
  2216. else
  2217. {
  2218. PlsrPlatformExitCritical(criticalState);
  2219. if (PlsrTimedStart.valid != 0U)
  2220. {
  2221. return;
  2222. }
  2223. goto fill_timed_start;
  2224. }
  2225. }
  2226. position = PlsrPosition;
  2227. remaining = PlsrRemainingPulses;
  2228. positive = PlsrCountPositive;
  2229. PlsrPlatformExitCritical(criticalState);
  2230. if (PlsrGetNextSegment(&nextSegment) == 0U)
  2231. {
  2232. return;
  2233. }
  2234. source = &PlsrActiveConfig.segments[sourceSegment - 1U];
  2235. if (PlsrActiveConfig.positionMode == PLSR_POSITION_ABSOLUTE)
  2236. {
  2237. predictedPosition = source->pulses;
  2238. }
  2239. else if (positive != 0U)
  2240. {
  2241. predictedPosition = (int64_t)position + (int64_t)remaining;
  2242. }
  2243. else
  2244. {
  2245. predictedPosition = (int64_t)position - (int64_t)remaining;
  2246. }
  2247. if ((predictedPosition > (int64_t)INT32_MAX)
  2248. || (predictedPosition < (int64_t)INT32_MIN))
  2249. {
  2250. return;
  2251. }
  2252. displacement = PlsrSegmentDisplacement(
  2253. nextSegment, (int32_t)predictedPosition);
  2254. if (displacement == 0)
  2255. {
  2256. return;
  2257. }
  2258. positive = (displacement > 0) ? 1U : 0U;
  2259. magnitude = (displacement < 0) ? (uint64_t)(-displacement)
  2260. : (uint64_t)displacement;
  2261. if (magnitude > 0xFFFFFFFFULL)
  2262. {
  2263. return;
  2264. }
  2265. directionLevel = positive;
  2266. if (PlsrActiveConfig.directionNegativeLogic != 0U)
  2267. {
  2268. directionLevel ^= 1U;
  2269. }
  2270. directionChanged = ((PlsrLastDirectionValid == 0U)
  2271. || (PlsrLastDirectionOutput
  2272. != (uint8_t)PlsrActiveConfig.directionOutput)
  2273. || (PlsrLastDirectionLevel != directionLevel)) ? 1U : 0U;
  2274. targetFrequencyHz =
  2275. PlsrActiveConfig.segments[nextSegment - 1U].frequencyHz;
  2276. startFrequencyHz = PlsrEffectiveStartFrequency(
  2277. targetFrequencyHz, 0U, directionChanged, 0UL);
  2278. if ((PlsrPrepareShortProfile(&profile, nextSegment,
  2279. startFrequencyHz, targetFrequencyHz,
  2280. magnitude, positive) == 0U)
  2281. || (PlsrShortProfileTakeRun(&profile, &entry) == 0U))
  2282. {
  2283. return;
  2284. }
  2285. /* Keep the seamless plan for this future segment consistent with its
  2286. possibly clipped terminal frequency. */
  2287. handoff = &PlsrPreparedHandoffPlans[PlsrPreparedHandoffBank]
  2288. [nextSegment - 1U];
  2289. (void)PlsrBuildHandoffPlan(nextSegment, &PlsrActiveConfig,
  2290. profile.endHz, handoff);
  2291. criticalState = PlsrPlatformEnterCritical();
  2292. if ((PlsrSegmentEpoch != sourceEpoch)
  2293. || (PlsrCurrentSegment != sourceSegment))
  2294. {
  2295. PlsrPlatformExitCritical(criticalState);
  2296. return;
  2297. }
  2298. queueBank = (uint8_t)(PlsrProfileQueueBank ^ 1U);
  2299. PlsrProfileQueueResetBankLocked(queueBank);
  2300. queue = &PlsrProfileQueues[queueBank];
  2301. queue->producerEpoch = sourceEpoch + 1UL;
  2302. queue->producerSegment = nextSegment;
  2303. queue->preparedHandoffBank = PlsrPreparedHandoffBank;
  2304. queue->repeatRemaining = 0UL;
  2305. queue->underrunDebtPulses = 0UL;
  2306. queue->haveLastSetting = 0U;
  2307. PlsrCopyShortProfile(&queue->producerProfile, &profile);
  2308. queue->active = 1U;
  2309. queue->generatorComplete = (profile.active == 0U) ? 1U : 0U;
  2310. PlsrTimedStart.firstRun = entry;
  2311. PlsrTimedStart.magnitude = magnitude;
  2312. PlsrTimedStart.sourceEpoch = sourceEpoch;
  2313. PlsrTimedStart.queueGeneration = queue->generation;
  2314. PlsrTimedStart.generatedItems = 0U;
  2315. PlsrTimedStart.sourceSegment = sourceSegment;
  2316. PlsrTimedStart.nextSegment = nextSegment;
  2317. PlsrTimedStart.positive = positive;
  2318. PlsrTimedStart.directionLevel = directionLevel;
  2319. PlsrTimedStart.directionChanged = directionChanged;
  2320. PlsrTimedStart.queueBank = queueBank;
  2321. PlsrTimedStart.valid = (queue->generatorComplete != 0U) ? 1U : 0U;
  2322. PlsrTimedStart.building = (PlsrTimedStart.valid == 0U) ? 1U : 0U;
  2323. PlsrTimedStart.pendingActivation = 0U;
  2324. PlsrPlatformExitCritical(criticalState);
  2325. PlsrProfileRecordPlannerStatus(&profile);
  2326. fill_timed_start:
  2327. budget = PLSR_TIMED_START_BUILD_BUDGET;
  2328. while (budget != 0U)
  2329. {
  2330. criticalState = PlsrPlatformEnterCritical();
  2331. queueBank = PlsrTimedStart.queueBank;
  2332. queue = &PlsrProfileQueues[queueBank];
  2333. queueGeneration = PlsrTimedStart.queueGeneration;
  2334. if ((PlsrTimedStart.building == 0U)
  2335. || (PlsrTimedStart.sourceEpoch != PlsrSegmentEpoch)
  2336. || (PlsrTimedStart.sourceSegment != PlsrCurrentSegment)
  2337. || (queueBank == PlsrProfileQueueBank)
  2338. || (queue->generation != queueGeneration))
  2339. {
  2340. PlsrInvalidateTimedStartLocked();
  2341. PlsrPlatformExitCritical(criticalState);
  2342. return;
  2343. }
  2344. PlsrCopyShortProfile(&profile, &queue->producerProfile);
  2345. PlsrPlatformExitCritical(criticalState);
  2346. if (PlsrShortProfileTakeRun(&profile, &entry) == 0U)
  2347. {
  2348. criticalState = PlsrPlatformEnterCritical();
  2349. if ((PlsrTimedStart.building != 0U)
  2350. && (queue->generation == queueGeneration))
  2351. {
  2352. queue->generatorComplete = 1U;
  2353. PlsrTimedStart.building = 0U;
  2354. PlsrTimedStart.valid = 1U;
  2355. }
  2356. PlsrPlatformExitCritical(criticalState);
  2357. return;
  2358. }
  2359. criticalState = PlsrPlatformEnterCritical();
  2360. if ((PlsrTimedStart.building == 0U)
  2361. || (PlsrTimedStart.sourceEpoch != PlsrSegmentEpoch)
  2362. || (queueBank == PlsrProfileQueueBank)
  2363. || (queue->generation != queueGeneration))
  2364. {
  2365. PlsrInvalidateTimedStartLocked();
  2366. PlsrPlatformExitCritical(criticalState);
  2367. return;
  2368. }
  2369. if (PlsrProfileQueueAppendLocked(queue, &entry) == 0U)
  2370. {
  2371. PlsrInvalidateTimedStartLocked();
  2372. PlsrPlatformExitCritical(criticalState);
  2373. return;
  2374. }
  2375. PlsrCopyShortProfile(&queue->producerProfile, &profile);
  2376. if (PlsrTimedStart.generatedItems != 0xFFFFU)
  2377. {
  2378. PlsrTimedStart.generatedItems++;
  2379. }
  2380. if ((profile.active == 0U)
  2381. || (profile.nextPeriod >= profile.pulseCount))
  2382. {
  2383. queue->generatorComplete = 1U;
  2384. }
  2385. if ((PlsrTimedStart.generatedItems
  2386. >= PLSR_TIMED_START_READY_ITEMS)
  2387. || (queue->generatorComplete != 0U))
  2388. {
  2389. PlsrTimedStart.building = 0U;
  2390. PlsrTimedStart.valid = 1U;
  2391. PlsrPlatformExitCritical(criticalState);
  2392. return;
  2393. }
  2394. PlsrPlatformExitCritical(criticalState);
  2395. budget--;
  2396. }
  2397. }
  2398. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
  2399. && !defined(PLSR_HOST_TEST)
  2400. static uint32_t PlsrProfileTimingNow(void)
  2401. {
  2402. return *((volatile uint32_t *)0xE0001004UL);
  2403. }
  2404. static void PlsrProfileRecordProducerCycles(uint32_t startCycles)
  2405. {
  2406. uint32_t elapsed = PlsrProfileTimingNow() - startCycles;
  2407. uint32_t total = PlsrProfileProducerTotalCycles;
  2408. if (PlsrProfileProducerItemCount != 0xFFFFFFFFUL)
  2409. {
  2410. PlsrProfileProducerItemCount++;
  2411. }
  2412. PlsrProfileProducerTotalCycles =
  2413. (elapsed > (0xFFFFFFFFUL - total)) ? 0xFFFFFFFFUL
  2414. : total + elapsed;
  2415. if (elapsed > PlsrProfileProducerMaxItemCycles)
  2416. {
  2417. PlsrProfileProducerMaxItemCycles = elapsed;
  2418. }
  2419. }
  2420. #endif
  2421. static uint8_t PlsrProfileQueueFill(uint16_t targetCount,
  2422. uint16_t *itemBudget)
  2423. {
  2424. PLSR_SHORT_PROFILE candidate;
  2425. PLSR_PROFILE_ENTRY entry;
  2426. uint32_t generation;
  2427. uint32_t producerEpoch;
  2428. uint32_t criticalState;
  2429. uint32_t underrunDebt;
  2430. uint32_t queueReadIndex;
  2431. uint32_t queueWriteIndex;
  2432. uint32_t queueCount;
  2433. uint32_t currentGeneration;
  2434. uint32_t currentProducerEpoch;
  2435. PLSR_PROFILE_QUEUE *queue;
  2436. uint8_t queueBank;
  2437. uint8_t currentBank;
  2438. uint8_t queueActive;
  2439. uint8_t generatorComplete;
  2440. uint8_t payingUnderrunDebt;
  2441. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
  2442. && !defined(PLSR_HOST_TEST)
  2443. uint32_t startCycles;
  2444. #endif
  2445. if (itemBudget == NULL)
  2446. {
  2447. return 0U;
  2448. }
  2449. if ((targetCount == 0U)
  2450. || (targetCount > PLSR_PROFILE_QUEUE_CAPACITY))
  2451. {
  2452. targetCount = PLSR_PROFILE_QUEUE_CAPACITY;
  2453. }
  2454. while (1)
  2455. {
  2456. criticalState = PlsrPlatformEnterCritical();
  2457. queueBank = PlsrProfileQueueBank;
  2458. queue = &PlsrProfileQueues[queueBank];
  2459. queueActive = queue->active;
  2460. generatorComplete = queue->generatorComplete;
  2461. queueWriteIndex = queue->writeIndex;
  2462. queueReadIndex = queue->readIndex;
  2463. queueCount = queueWriteIndex - queueReadIndex;
  2464. if ((queueActive == 0U)
  2465. || (generatorComplete != 0U)
  2466. || (queueCount >= targetCount)
  2467. || (*itemBudget == 0U))
  2468. {
  2469. PlsrPlatformExitCritical(criticalState);
  2470. return 1U;
  2471. }
  2472. (*itemBudget)--;
  2473. generation = queue->generation;
  2474. producerEpoch = queue->producerEpoch;
  2475. underrunDebt = queue->underrunDebtPulses;
  2476. payingUnderrunDebt = (underrunDebt != 0UL) ? 1U : 0U;
  2477. PlsrCopyShortProfile(&candidate, &queue->producerProfile);
  2478. PlsrPlatformExitCritical(criticalState);
  2479. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
  2480. && !defined(PLSR_HOST_TEST)
  2481. startCycles = PlsrProfileTimingNow();
  2482. #endif
  2483. if (PlsrShortProfileTakeRunLimited(
  2484. &candidate, &entry,
  2485. (payingUnderrunDebt != 0U)
  2486. ? underrunDebt : 0xFFFFFFFFUL) == 0U)
  2487. {
  2488. return 0U;
  2489. }
  2490. criticalState = PlsrPlatformEnterCritical();
  2491. currentBank = PlsrProfileQueueBank;
  2492. queueActive = queue->active;
  2493. currentGeneration = queue->generation;
  2494. currentProducerEpoch = queue->producerEpoch;
  2495. queueWriteIndex = queue->writeIndex;
  2496. queueReadIndex = queue->readIndex;
  2497. queueCount = queueWriteIndex - queueReadIndex;
  2498. if ((currentBank != queueBank)
  2499. || (queueActive == 0U)
  2500. || (currentGeneration != generation)
  2501. || (currentProducerEpoch != producerEpoch)
  2502. || (queueCount >= PLSR_PROFILE_QUEUE_CAPACITY))
  2503. {
  2504. PlsrPlatformExitCritical(criticalState);
  2505. continue;
  2506. }
  2507. PlsrCopyShortProfile(&queue->producerProfile, &candidate);
  2508. if (payingUnderrunDebt != 0U)
  2509. {
  2510. underrunDebt = queue->underrunDebtPulses;
  2511. if (underrunDebt < entry.repeatCount)
  2512. {
  2513. PlsrPlatformExitCritical(criticalState);
  2514. continue;
  2515. }
  2516. queue->underrunDebtPulses =
  2517. underrunDebt - entry.repeatCount;
  2518. }
  2519. else if (PlsrProfileQueueAppendLocked(
  2520. queue, &entry) == 0U)
  2521. {
  2522. PlsrPlatformExitCritical(criticalState);
  2523. continue;
  2524. }
  2525. queueWriteIndex = queue->writeIndex;
  2526. queueReadIndex = queue->readIndex;
  2527. queueCount = queueWriteIndex - queueReadIndex;
  2528. if (queueCount >= PLSR_PROFILE_LOW_WATER_CLEAR)
  2529. {
  2530. PlsrProfileQueueLowWaterLatched = 0U;
  2531. }
  2532. if (((candidate.active == 0U)
  2533. || (candidate.nextPeriod >= candidate.pulseCount))
  2534. && (queue->underrunDebtPulses == 0UL))
  2535. {
  2536. queue->generatorComplete = 1U;
  2537. }
  2538. PlsrPlatformExitCritical(criticalState);
  2539. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
  2540. && !defined(PLSR_HOST_TEST)
  2541. PlsrProfileRecordProducerCycles(startCycles);
  2542. #endif
  2543. }
  2544. }
  2545. static PLSR_PLATFORM_QUEUE_RESULT PlsrProfileQueueCommitNext(
  2546. uint8_t pulseOutput)
  2547. {
  2548. const PLSR_PROFILE_ENTRY *entry;
  2549. uint32_t readIndex;
  2550. uint32_t nextReadIndex;
  2551. uint32_t writeIndex;
  2552. uint32_t actualFrequencyHz;
  2553. uint32_t repeatRemaining;
  2554. PLSR_PLATFORM_QUEUE_RESULT result;
  2555. repeatRemaining = PlsrProfileQueue.repeatRemaining;
  2556. if (repeatRemaining != 0UL)
  2557. {
  2558. PlsrProfileQueue.repeatRemaining = repeatRemaining - 1UL;
  2559. return PLSR_PLATFORM_QUEUE_APPLIED;
  2560. }
  2561. if (PlsrProfileQueue.active == 0U)
  2562. {
  2563. if (PlsrProfileQueue.generatorComplete != 0U)
  2564. {
  2565. PlsrShortProfile.active = 0U;
  2566. }
  2567. return PLSR_PLATFORM_QUEUE_APPLIED;
  2568. }
  2569. readIndex = PlsrProfileQueue.readIndex;
  2570. if (readIndex == PlsrProfileQueue.writeIndex)
  2571. {
  2572. if (PlsrProfileQueue.generatorComplete != 0U)
  2573. {
  2574. PlsrProfileQueue.active = 0U;
  2575. PlsrShortProfile.active = 0U;
  2576. return PLSR_PLATFORM_QUEUE_APPLIED;
  2577. }
  2578. PlsrProfileQueueRecordUnderrun();
  2579. return PLSR_PLATFORM_QUEUE_FAILED;
  2580. }
  2581. entry = &PlsrProfileQueue.entries[readIndex & PLSR_PROFILE_QUEUE_MASK];
  2582. result = PlsrPlatformLoadPreparedFromIrq(pulseOutput, &entry->setting,
  2583. &actualFrequencyHz);
  2584. if (result != PLSR_PLATFORM_QUEUE_APPLIED)
  2585. {
  2586. return result;
  2587. }
  2588. nextReadIndex = readIndex + 1UL;
  2589. PlsrProfileQueue.readIndex = nextReadIndex;
  2590. PlsrProfileQueue.repeatRemaining = entry->repeatCount - 1UL;
  2591. if (PlsrProfileQueue.generatorComplete == 0U)
  2592. {
  2593. PlsrProfileQueueRecordDepth(
  2594. (uint16_t)(PlsrProfileQueue.writeIndex - nextReadIndex));
  2595. }
  2596. PlsrDeferredFrequencyPending = 0U;
  2597. PlsrQueuedFrequencyHz = actualFrequencyHz;
  2598. PlsrDiagnosticRecordFrequency(entry->requestedFrequencyHz,
  2599. entry->setting.actualFrequencyHz,
  2600. actualFrequencyHz);
  2601. PlsrDiagnosticQueuedExpectedHz = entry->setting.actualFrequencyHz;
  2602. if (actualFrequencyHz > PlsrCurrentFrequencyHz)
  2603. {
  2604. PlsrRunStatus = PLSR_STATUS_ACCELERATING;
  2605. }
  2606. else if (actualFrequencyHz < PlsrCurrentFrequencyHz)
  2607. {
  2608. PlsrRunStatus = PLSR_STATUS_DECELERATING;
  2609. }
  2610. else
  2611. {
  2612. PlsrRunStatus = PLSR_STATUS_RUNNING;
  2613. }
  2614. writeIndex = PlsrProfileQueue.writeIndex;
  2615. if ((nextReadIndex == writeIndex)
  2616. && (PlsrProfileQueue.generatorComplete != 0U))
  2617. {
  2618. PlsrProfileQueue.active = 0U;
  2619. PlsrShortProfile.active = 0U;
  2620. }
  2621. return PLSR_PLATFORM_QUEUE_APPLIED;
  2622. }
  2623. static void PlsrInvalidateHandoffPlans(void)
  2624. {
  2625. uint8_t bank;
  2626. uint8_t index;
  2627. uint32_t criticalState = PlsrPlatformEnterCritical();
  2628. PlsrHandoffPlan.valid = 0U;
  2629. PlsrCountedHandoffStaged = 0U;
  2630. for (bank = 0U; bank < 2U; bank++)
  2631. {
  2632. for (index = 0U; index < PLSR_SEGMENT_COUNT_MAX; index++)
  2633. {
  2634. PlsrPreparedHandoffPlans[bank][index].valid = 0U;
  2635. }
  2636. }
  2637. PlsrPlatformExitCritical(criticalState);
  2638. }
  2639. static void PlsrMaybePlanBoundaryRamp(uint32_t expectedEpoch)
  2640. {
  2641. const PLSR_SEGMENT_CONFIG *segment;
  2642. uint8_t nextSegment;
  2643. uint8_t nextPositive;
  2644. uint8_t hasNext;
  2645. uint32_t targetHz;
  2646. uint32_t criticalState;
  2647. uint32_t currentHz;
  2648. uint32_t queuedHz;
  2649. uint32_t deferredHz;
  2650. uint32_t rateBoundHz;
  2651. uint8_t deferredPending;
  2652. uint64_t estimate;
  2653. uint64_t candidateEstimate;
  2654. uint64_t remaining;
  2655. uint8_t rampActive;
  2656. if ((PlsrPulseActive == 0U)
  2657. || (PlsrBoundaryRampStarted != 0U)
  2658. || (PlsrShortProfile.active != 0U)
  2659. || (PlsrHandoffPlan.valid != 0U)
  2660. || (PlsrStopRequested != 0U) || (PlsrCurrentSegment == 0U))
  2661. {
  2662. return;
  2663. }
  2664. segment = &PlsrActiveConfig.segments[PlsrCurrentSegment - 1U];
  2665. hasNext = PlsrGetNextSegment(&nextSegment);
  2666. targetHz = PlsrEffectiveStopFrequency(segment->frequencyHz);
  2667. if ((hasNext != 0U)
  2668. && (segment->waitType == PLSR_EXT_OR_COMPLETE))
  2669. {
  2670. if ((PlsrPredictNextDirection(nextSegment, &nextPositive) != 0U)
  2671. && (nextPositive == PlsrCountPositive))
  2672. {
  2673. targetHz = PlsrActiveConfig.segments[nextSegment - 1U].frequencyHz;
  2674. }
  2675. }
  2676. criticalState = PlsrPlatformEnterCritical();
  2677. currentHz = PlsrCurrentFrequencyHz;
  2678. queuedHz = PlsrQueuedFrequencyHz;
  2679. deferredHz = PlsrDeferredFrequencyHz;
  2680. deferredPending = PlsrDeferredFrequencyPending;
  2681. if ((PlsrSegmentEpoch != expectedEpoch)
  2682. || (PlsrBoundaryPending != 0U)
  2683. || (PlsrPulseActive == 0U)
  2684. || (PlsrBoundaryRampStarted != 0U)
  2685. || (PlsrShortProfile.active != 0U)
  2686. || (PlsrHandoffPlan.valid != 0U)
  2687. || (PlsrStopRequested != 0U)
  2688. || (PlsrAbStopArmed != 0U))
  2689. {
  2690. PlsrPlatformExitCritical(criticalState);
  2691. return;
  2692. }
  2693. PlsrPlatformExitCritical(criticalState);
  2694. rateBoundHz = (currentHz > queuedHz) ? currentHz : queuedHz;
  2695. if ((deferredPending != 0U) && (deferredHz > rateBoundHz))
  2696. {
  2697. rateBoundHz = deferredHz;
  2698. }
  2699. estimate = PlsrRampPulseEstimate(currentHz, targetHz, rateBoundHz);
  2700. candidateEstimate = PlsrRampPulseEstimate(queuedHz, targetHz,
  2701. rateBoundHz);
  2702. if (candidateEstimate > estimate)
  2703. {
  2704. estimate = candidateEstimate;
  2705. }
  2706. if (deferredPending != 0U)
  2707. {
  2708. candidateEstimate = PlsrRampPulseEstimate(deferredHz, targetHz,
  2709. rateBoundHz);
  2710. if (candidateEstimate > estimate)
  2711. {
  2712. estimate = candidateEstimate;
  2713. }
  2714. }
  2715. criticalState = PlsrPlatformEnterCritical();
  2716. remaining = PlsrRemainingPulses;
  2717. if ((PlsrSegmentEpoch != expectedEpoch)
  2718. || (PlsrBoundaryPending != 0U)
  2719. || (PlsrPulseActive == 0U)
  2720. || (PlsrBoundaryRampStarted != 0U)
  2721. || (PlsrShortProfile.active != 0U)
  2722. || (PlsrHandoffPlan.valid != 0U)
  2723. || (PlsrStopRequested != 0U)
  2724. || (PlsrAbStopArmed != 0U))
  2725. {
  2726. PlsrPlatformExitCritical(criticalState);
  2727. return;
  2728. }
  2729. if (remaining > estimate)
  2730. {
  2731. PlsrPlatformExitCritical(criticalState);
  2732. return;
  2733. }
  2734. PlsrBoundaryRampStarted = 1U;
  2735. PlsrRampStart(PlsrCurrentFrequencyHz, targetHz);
  2736. rampActive = PlsrRamp.active;
  2737. PlsrPlatformExitCritical(criticalState);
  2738. if (rampActive != 0U)
  2739. {
  2740. if (PlsrRampAdvance(expectedEpoch) == 0U)
  2741. {
  2742. PlsrEnterErrorIfEpoch(PLSR_ERROR_TIMER, expectedEpoch);
  2743. }
  2744. }
  2745. else
  2746. {
  2747. (void)PlsrApplyFrequency(targetHz, expectedEpoch);
  2748. }
  2749. }
  2750. static uint8_t PlsrStartPreparedTimedOutput(void)
  2751. {
  2752. PLSR_PROFILE_QUEUE *queue;
  2753. PLSR_PROFILE_ENTRY firstRun;
  2754. uint32_t criticalState;
  2755. uint32_t firstFrequencyHz;
  2756. uint32_t secondFrequencyHz;
  2757. uint32_t actualFrequencyHz;
  2758. uint32_t queueReadIndex;
  2759. uint32_t queueWriteIndex;
  2760. uint8_t queueBank;
  2761. criticalState = PlsrPlatformEnterCritical();
  2762. queueBank = PlsrTimedStart.queueBank;
  2763. queue = &PlsrProfileQueues[queueBank];
  2764. if ((PlsrTimedStart.pendingActivation == 0U)
  2765. || (PlsrTimedStart.valid == 0U)
  2766. || (PlsrCurrentSegment != PlsrTimedStart.nextSegment)
  2767. || (PlsrSegmentEpoch != PlsrTimedStart.sourceEpoch + 1UL)
  2768. || (queueBank == PlsrProfileQueueBank)
  2769. || (queue->generation != PlsrTimedStart.queueGeneration)
  2770. || (queue->producerEpoch != PlsrSegmentEpoch)
  2771. || (queue->producerSegment != PlsrCurrentSegment))
  2772. {
  2773. PlsrPlatformExitCritical(criticalState);
  2774. return 0U;
  2775. }
  2776. firstRun = PlsrTimedStart.firstRun;
  2777. PlsrProfileQueueResetBankLocked(PlsrProfileQueueBank);
  2778. PlsrProfileQueueBank = queueBank;
  2779. PlsrCopyShortProfile(&PlsrShortProfile, &queue->producerProfile);
  2780. PlsrTimedStart.pendingActivation = 0U;
  2781. PlsrTimedStart.valid = 0U;
  2782. PlsrTimedStart.building = 0U;
  2783. PlsrPlatformExitCritical(criticalState);
  2784. PlsrSegmentClockStarted = 1U;
  2785. PlsrSegmentElapsedMs = 0UL;
  2786. PlsrRamp.active = 0U;
  2787. PlsrCountedHandoffStaged = 0U;
  2788. if (PlsrStageCountedHandoff() == 0U)
  2789. {
  2790. return 0U;
  2791. }
  2792. firstFrequencyHz = firstRun.setting.actualFrequencyHz;
  2793. secondFrequencyHz = firstFrequencyHz;
  2794. criticalState = PlsrPlatformEnterCritical();
  2795. queueReadIndex = queue->readIndex;
  2796. queueWriteIndex = queue->writeIndex;
  2797. if (queueReadIndex != queueWriteIndex)
  2798. {
  2799. secondFrequencyHz = queue->entries[
  2800. queueReadIndex & PLSR_PROFILE_QUEUE_MASK]
  2801. .setting.actualFrequencyHz;
  2802. }
  2803. queue->lastSetting = firstRun.setting;
  2804. queue->haveLastSetting = 1U;
  2805. PlsrPlatformExitCritical(criticalState);
  2806. if (secondFrequencyHz > firstFrequencyHz)
  2807. {
  2808. PlsrRunStatus = PLSR_STATUS_ACCELERATING;
  2809. }
  2810. else if (secondFrequencyHz < firstFrequencyHz)
  2811. {
  2812. PlsrRunStatus = PLSR_STATUS_DECELERATING;
  2813. }
  2814. else
  2815. {
  2816. PlsrRunStatus = PLSR_STATUS_RUNNING;
  2817. }
  2818. PlsrCurrentFrequencyHz = firstFrequencyHz;
  2819. PlsrQueuedFrequencyHz = secondFrequencyHz;
  2820. PlsrDiagnosticQueuedExpectedHz = firstRun.setting.actualFrequencyHz;
  2821. PlsrCountedObservedPublished = PlsrPlatformObservedPulses(
  2822. (uint8_t)PlsrActiveConfig.pulseOutput);
  2823. PlsrProfileQueueMinimumDepth = PlsrProfileQueueCount();
  2824. PlsrProfileQueueLowWaterLatched = 0U;
  2825. PlsrPulseActive = 1U;
  2826. PlsrExecutor.generation++;
  2827. PlsrExecutor.mode = (queue->generatorComplete != 0U)
  2828. ? PLSR_EXEC_STEP_TABLE
  2829. : PLSR_EXEC_STREAM;
  2830. if (PlsrPlatformStartCountedStreamPrepared(
  2831. (uint8_t)PlsrActiveConfig.pulseOutput,
  2832. &firstRun.setting, firstRun.repeatCount,
  2833. &actualFrequencyHz) == 0U)
  2834. {
  2835. PlsrPulseActive = 0U;
  2836. PlsrExecutor.mode = PLSR_EXEC_IDLE;
  2837. PlsrProfileQueueReset();
  2838. PlsrShortProfile.active = 0U;
  2839. return 0U;
  2840. }
  2841. PlsrCurrentFrequencyHz = actualFrequencyHz;
  2842. PlsrDiagnosticRecordFrequency(firstRun.requestedFrequencyHz,
  2843. firstRun.setting.actualFrequencyHz,
  2844. actualFrequencyHz);
  2845. return 1U;
  2846. }
  2847. static PLSR_PLATFORM_SERVICE_RESULT PlsrTryStartPreparedTimedSegment(
  2848. uint8_t segmentNumber)
  2849. {
  2850. PLSR_PROFILE_QUEUE *queue;
  2851. uint32_t criticalState;
  2852. uint64_t magnitude;
  2853. uint8_t positive;
  2854. uint8_t directionLevel;
  2855. uint8_t directionChanged;
  2856. criticalState = PlsrPlatformEnterCritical();
  2857. queue = &PlsrProfileQueues[PlsrTimedStart.queueBank];
  2858. if ((PlsrTimedStart.valid == 0U)
  2859. || (PlsrTimedStart.pendingActivation != 0U)
  2860. || (PlsrTimedStart.sourceEpoch != PlsrSegmentEpoch)
  2861. || (PlsrTimedStart.sourceSegment != PlsrCurrentSegment)
  2862. || (PlsrTimedStart.nextSegment != segmentNumber)
  2863. || (PlsrTimedStart.queueBank == PlsrProfileQueueBank)
  2864. || (queue->generation != PlsrTimedStart.queueGeneration))
  2865. {
  2866. PlsrPlatformExitCritical(criticalState);
  2867. return PLSR_PLATFORM_SERVICE_DEFERRED;
  2868. }
  2869. magnitude = PlsrTimedStart.magnitude;
  2870. positive = PlsrTimedStart.positive;
  2871. directionLevel = PlsrTimedStart.directionLevel;
  2872. directionChanged = PlsrTimedStart.directionChanged;
  2873. PlsrSegmentEpoch++;
  2874. PlsrCurrentSegment = segmentNumber;
  2875. PlsrSegmentClockStarted = 0U;
  2876. PlsrSegmentElapsedMs = 0UL;
  2877. PlsrWaitElapsedMs = 0UL;
  2878. PlsrBoundaryRampStarted = 0U;
  2879. PlsrBoundaryPending = 0U;
  2880. PlsrBoundaryWasCut = 0U;
  2881. PlsrCutRequested = 0U;
  2882. PlsrStopPulsesRemaining = 0U;
  2883. PlsrAbStopArmed = 0U;
  2884. PlsrFrequencyUpdatePending = 0U;
  2885. PlsrDeferredFrequencyPending = 0U;
  2886. PlsrCurrentFrequencyHz = 0UL;
  2887. PlsrQueuedFrequencyHz = 0UL;
  2888. PlsrHandoffPlan.valid = 0U;
  2889. PlsrShortProfile.active = 0U;
  2890. PlsrShortProfile.nextPeriod = 0U;
  2891. PlsrDirectionDelayActive = 0U;
  2892. PlsrDirectionDelayRemainingMs = 0U;
  2893. PlsrExtEdgePending = 0U;
  2894. PlsrRemainingPulses = magnitude;
  2895. PlsrCountPositive = positive;
  2896. PlsrTimedStart.pendingActivation = 1U;
  2897. PlsrPlatformExitCritical(criticalState);
  2898. PlsrExtPreviousLevel =
  2899. PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.extInput);
  2900. if (PlsrPlatformPrepare((uint8_t)PlsrActiveConfig.pulseOutput,
  2901. (uint8_t)PlsrActiveConfig.directionOutput,
  2902. directionLevel,
  2903. (uint8_t)PlsrActiveConfig.outputMode,
  2904. positive) == 0U)
  2905. {
  2906. criticalState = PlsrPlatformEnterCritical();
  2907. PlsrInvalidateTimedStartLocked();
  2908. PlsrPlatformExitCritical(criticalState);
  2909. return PLSR_PLATFORM_SERVICE_FAILED;
  2910. }
  2911. PlsrDiagnosticBeginSegment(segmentNumber, magnitude, positive);
  2912. PlsrLastDirectionValid = 1U;
  2913. PlsrLastDirectionOutput = (uint8_t)PlsrActiveConfig.directionOutput;
  2914. PlsrLastDirectionLevel = directionLevel;
  2915. if ((directionChanged != 0U)
  2916. && (PlsrActiveConfig.directionDelayMs != 0U))
  2917. {
  2918. PlsrDirectionDelayActive = 1U;
  2919. PlsrDirectionDelayRemainingMs = PlsrActiveConfig.directionDelayMs;
  2920. PlsrRunStatus = PLSR_STATUS_ACCELERATING;
  2921. return PLSR_PLATFORM_SERVICE_READY;
  2922. }
  2923. if (PlsrStartPreparedTimedOutput() == 0U)
  2924. {
  2925. return PLSR_PLATFORM_SERVICE_FAILED;
  2926. }
  2927. return PLSR_PLATFORM_SERVICE_READY;
  2928. }
  2929. static uint8_t PlsrBeginSegmentOutput(uint32_t startFrequencyHz)
  2930. {
  2931. uint32_t targetFrequencyHz =
  2932. PlsrActiveConfig.segments[PlsrCurrentSegment - 1U].frequencyHz;
  2933. uint32_t firstFrequencyHz;
  2934. uint32_t secondFrequencyHz;
  2935. uint32_t actualFrequencyHz;
  2936. uint32_t profileEpoch;
  2937. uint64_t remainingPulses;
  2938. uint8_t profileSegment;
  2939. uint8_t profileHandoffBank;
  2940. uint8_t countPositive;
  2941. uint16_t fillBudget = PLSR_PROFILE_STARTUP_BUDGET;
  2942. PLSR_PROFILE_ENTRY firstRun;
  2943. PlsrSegmentClockStarted = 1U;
  2944. PlsrSegmentElapsedMs = 0UL;
  2945. profileSegment = PlsrCurrentSegment;
  2946. countPositive = PlsrCountPositive;
  2947. remainingPulses = PlsrRemainingSnapshot();
  2948. if (PlsrPrepareShortProfile(&PlsrShortProfile,
  2949. profileSegment, startFrequencyHz,
  2950. targetFrequencyHz,
  2951. remainingPulses,
  2952. countPositive) != 0U)
  2953. {
  2954. PlsrRamp.active = 0U;
  2955. PlsrRefreshCurrentHandoffPlan(PlsrShortProfile.endHz);
  2956. if (PlsrShortProfileTakeRun(&PlsrShortProfile, &firstRun) == 0U)
  2957. {
  2958. return 0U;
  2959. }
  2960. firstFrequencyHz = firstRun.setting.actualFrequencyHz;
  2961. secondFrequencyHz = firstFrequencyHz;
  2962. profileEpoch = PlsrSegmentEpoch;
  2963. profileHandoffBank = PlsrPreparedHandoffBank;
  2964. (void)PlsrProfileQueueBegin(&PlsrShortProfile,
  2965. profileEpoch,
  2966. profileSegment,
  2967. profileHandoffBank);
  2968. if (PlsrProfileQueueFill(PLSR_PROFILE_STARTUP_TARGET,
  2969. &fillBudget) == 0U)
  2970. {
  2971. PlsrProfileQueueReset();
  2972. PlsrShortProfile.active = 0U;
  2973. return 0U;
  2974. }
  2975. PlsrCountedHandoffStaged = 0U;
  2976. if ((PlsrStageCountedHandoff() == 0U)
  2977. || (PlsrProfileQueueFill(PLSR_PROFILE_STARTUP_TARGET,
  2978. &fillBudget) == 0U))
  2979. {
  2980. PlsrProfileQueueReset();
  2981. PlsrShortProfile.active = 0U;
  2982. return 0U;
  2983. }
  2984. {
  2985. uint32_t readIndex = PlsrProfileQueue.readIndex;
  2986. uint32_t writeIndex = PlsrProfileQueue.writeIndex;
  2987. if (readIndex != writeIndex)
  2988. {
  2989. secondFrequencyHz = PlsrProfileQueue.entries[
  2990. readIndex & PLSR_PROFILE_QUEUE_MASK]
  2991. .setting.actualFrequencyHz;
  2992. }
  2993. }
  2994. if (secondFrequencyHz > firstFrequencyHz)
  2995. {
  2996. PlsrRunStatus = PLSR_STATUS_ACCELERATING;
  2997. }
  2998. else if (secondFrequencyHz < firstFrequencyHz)
  2999. {
  3000. PlsrRunStatus = PLSR_STATUS_DECELERATING;
  3001. }
  3002. else
  3003. {
  3004. PlsrRunStatus = PLSR_STATUS_RUNNING;
  3005. }
  3006. PlsrCurrentFrequencyHz = firstFrequencyHz;
  3007. PlsrQueuedFrequencyHz = secondFrequencyHz;
  3008. PlsrProfileQueue.lastSetting = firstRun.setting;
  3009. PlsrProfileQueue.haveLastSetting = 1U;
  3010. PlsrDiagnosticQueuedExpectedHz = firstRun.setting.actualFrequencyHz;
  3011. PlsrCountedObservedPublished = PlsrPlatformObservedPulses(
  3012. (uint8_t)PlsrActiveConfig.pulseOutput);
  3013. PlsrProfileQueueMinimumDepth = PlsrProfileQueueCount();
  3014. PlsrProfileQueueLowWaterLatched = 0U;
  3015. PlsrPulseActive = 1U;
  3016. PlsrExecutor.generation++;
  3017. PlsrExecutor.mode = (PlsrProfileQueue.generatorComplete != 0U)
  3018. ? PLSR_EXEC_STEP_TABLE
  3019. : PLSR_EXEC_STREAM;
  3020. if (PlsrPlatformStartCountedStreamPrepared(
  3021. (uint8_t)PlsrActiveConfig.pulseOutput,
  3022. &firstRun.setting, firstRun.repeatCount,
  3023. &actualFrequencyHz) == 0U)
  3024. {
  3025. PlsrPulseActive = 0U;
  3026. PlsrExecutor.mode = PLSR_EXEC_IDLE;
  3027. PlsrProfileQueueReset();
  3028. PlsrShortProfile.active = 0U;
  3029. return 0U;
  3030. }
  3031. PlsrCurrentFrequencyHz = actualFrequencyHz;
  3032. PlsrDiagnosticRecordFrequency(firstRun.requestedFrequencyHz,
  3033. firstRun.setting.actualFrequencyHz,
  3034. actualFrequencyHz);
  3035. return 1U;
  3036. }
  3037. if (PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR)
  3038. {
  3039. return 0U;
  3040. }
  3041. PlsrProfileQueueReset();
  3042. PlsrRampStart(startFrequencyHz, targetFrequencyHz);
  3043. if (PlsrRamp.active == 0U)
  3044. {
  3045. if (PlsrApplyFrequency(targetFrequencyHz, PlsrSegmentEpoch) == 0U)
  3046. {
  3047. return 0U;
  3048. }
  3049. PlsrRunStatus = PLSR_STATUS_RUNNING;
  3050. }
  3051. else if ((startFrequencyHz != 0UL)
  3052. && (PlsrApplyFrequency(startFrequencyHz,
  3053. PlsrSegmentEpoch) == 0U))
  3054. {
  3055. return 0U;
  3056. }
  3057. if (PlsrPrepareFutureHandoffQueue() == 0U)
  3058. {
  3059. return 0U;
  3060. }
  3061. PlsrExecutor.generation++;
  3062. PlsrExecutor.mode = PLSR_EXEC_AB_LEGACY;
  3063. return 1U;
  3064. }
  3065. static uint8_t PlsrStartSegment(uint8_t segmentNumber,
  3066. uint8_t allowCarry,
  3067. uint32_t carryFrequencyHz)
  3068. {
  3069. uint32_t criticalState;
  3070. int32_t position;
  3071. int64_t displacement;
  3072. uint64_t magnitude;
  3073. uint8_t positive;
  3074. uint8_t directionLevel;
  3075. uint8_t directionChanged;
  3076. uint32_t startFrequencyHz;
  3077. PLSR_PROFILE_QUEUE *queue;
  3078. if ((segmentNumber == 0U)
  3079. || (segmentNumber > PlsrActiveConfig.segmentCount))
  3080. {
  3081. return 0U;
  3082. }
  3083. PlsrDisarmWaitSignal();
  3084. criticalState = PlsrPlatformEnterCritical();
  3085. position = PlsrPosition;
  3086. PlsrPlatformExitCritical(criticalState);
  3087. displacement = PlsrSegmentDisplacement(segmentNumber, position);
  3088. positive = (displacement >= 0) ? 1U : 0U;
  3089. magnitude = (displacement < 0) ? (uint64_t)(-displacement)
  3090. : (uint64_t)displacement;
  3091. PlsrSegmentEpoch++;
  3092. PlsrCurrentSegment = segmentNumber;
  3093. PlsrSegmentClockStarted = 0U;
  3094. PlsrSegmentElapsedMs = 0UL;
  3095. PlsrWaitElapsedMs = 0UL;
  3096. PlsrBoundaryRampStarted = 0U;
  3097. PlsrBoundaryPending = 0U;
  3098. PlsrBoundaryWasCut = 0U;
  3099. PlsrCutRequested = 0U;
  3100. PlsrStopPulsesRemaining = 0U;
  3101. PlsrAbStopArmed = 0U;
  3102. PlsrFrequencyUpdatePending = 0U;
  3103. PlsrDeferredFrequencyPending = 0U;
  3104. PlsrCurrentFrequencyHz = 0UL;
  3105. PlsrQueuedFrequencyHz = 0UL;
  3106. PlsrHandoffPlan.valid = 0U;
  3107. PlsrShortProfile.active = 0U;
  3108. PlsrShortProfile.nextPeriod = 0U;
  3109. queue = &PlsrProfileQueues[PlsrProfileQueueBank];
  3110. if ((allowCarry == 0U) || (queue->active == 0U)
  3111. || (queue->producerEpoch != PlsrSegmentEpoch)
  3112. || (queue->producerSegment != segmentNumber))
  3113. {
  3114. PlsrProfileQueueReset();
  3115. }
  3116. PlsrDirectionDelayActive = 0U;
  3117. PlsrDirectionDelayRemainingMs = 0U;
  3118. PlsrExtEdgePending = 0U;
  3119. PlsrExtPreviousLevel =
  3120. PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.extInput);
  3121. criticalState = PlsrPlatformEnterCritical();
  3122. PlsrRemainingPulses = magnitude;
  3123. PlsrCountPositive = positive;
  3124. PlsrPlatformExitCritical(criticalState);
  3125. if ((PlsrActiveConfig.segments[segmentNumber - 1U].waitType
  3126. == PLSR_ACT_TIME)
  3127. && (PlsrActiveConfig.segments[segmentNumber - 1U].actTimeMs == 0U))
  3128. {
  3129. PlsrSegmentClockStarted = 1U;
  3130. PlsrRunStatus = PLSR_STATUS_RUNNING;
  3131. PlsrBoundaryFrequencyHz = (allowCarry != 0U) ? carryFrequencyHz : 0UL;
  3132. PlsrBoundaryWasCut = 1U;
  3133. PlsrBoundaryPending = 1U;
  3134. PlsrDiagnosticBeginSegment(segmentNumber, 0UL, positive);
  3135. PlsrDiagnosticFinishSegment(1U);
  3136. return 1U;
  3137. }
  3138. if (magnitude == 0UL)
  3139. {
  3140. PlsrSegmentClockStarted = 1U;
  3141. PlsrRunStatus = PLSR_STATUS_RUNNING;
  3142. PlsrBoundaryFrequencyHz = 0UL;
  3143. PlsrBoundaryPending = 1U;
  3144. PlsrDiagnosticBeginSegment(segmentNumber, 0UL, positive);
  3145. PlsrDiagnosticFinishSegment(1U);
  3146. return 1U;
  3147. }
  3148. directionLevel = positive;
  3149. if ((PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR)
  3150. && (PlsrActiveConfig.directionNegativeLogic != 0U))
  3151. {
  3152. directionLevel ^= 1U;
  3153. }
  3154. directionChanged = ((PlsrLastDirectionValid == 0U)
  3155. || (PlsrLastDirectionOutput
  3156. != (uint8_t)PlsrActiveConfig.directionOutput)
  3157. || (PlsrLastDirectionLevel != directionLevel)) ? 1U : 0U;
  3158. if (PlsrPlatformPrepare((uint8_t)PlsrActiveConfig.pulseOutput,
  3159. (uint8_t)PlsrActiveConfig.directionOutput,
  3160. directionLevel,
  3161. (uint8_t)PlsrActiveConfig.outputMode,
  3162. positive) == 0U)
  3163. {
  3164. return 0U;
  3165. }
  3166. PlsrDiagnosticBeginSegment(segmentNumber, magnitude, positive);
  3167. PlsrLastDirectionValid = 1U;
  3168. PlsrLastDirectionOutput = (uint8_t)PlsrActiveConfig.directionOutput;
  3169. PlsrLastDirectionLevel = directionLevel;
  3170. startFrequencyHz = PlsrEffectiveStartFrequency(
  3171. PlsrActiveConfig.segments[segmentNumber - 1U].frequencyHz,
  3172. allowCarry, directionChanged, carryFrequencyHz);
  3173. if ((PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR)
  3174. && (directionChanged != 0U)
  3175. && (PlsrActiveConfig.directionDelayMs != 0U))
  3176. {
  3177. PlsrDirectionDelayActive = 1U;
  3178. PlsrDirectionDelayRemainingMs = PlsrActiveConfig.directionDelayMs;
  3179. PlsrRunStatus = PLSR_STATUS_ACCELERATING;
  3180. return 1U;
  3181. }
  3182. PlsrDirectionDelayActive = 0U;
  3183. return PlsrBeginSegmentOutput(startFrequencyHz);
  3184. }
  3185. static void PlsrMarkPersistenceDirty(uint16_t delayMs)
  3186. {
  3187. PlsrPersistenceDirty = 1U;
  3188. PlsrPersistenceDelayMs = delayMs;
  3189. }
  3190. static void PlsrCheckpointPosition(uint8_t wasBusy)
  3191. {
  3192. uint32_t criticalState;
  3193. int32_t position;
  3194. uint8_t positionValid;
  3195. criticalState = PlsrPlatformEnterCritical();
  3196. position = PlsrPosition;
  3197. positionValid = PlsrPositionValid;
  3198. PlsrPositionCheckpointDirty = 0U;
  3199. PlsrPlatformExitCritical(criticalState);
  3200. PlsrPlatformCheckpointPosition(position, positionValid, wasBusy);
  3201. PlsrPositionCheckpointElapsedMs = 0U;
  3202. }
  3203. static void PlsrPollPositionCheckpoint(void)
  3204. {
  3205. if (PlsrPositionCheckpointDirty == 0U)
  3206. {
  3207. PlsrPositionCheckpointElapsedMs = 0U;
  3208. return;
  3209. }
  3210. if (PlsrPositionCheckpointElapsedMs < PLSR_POSITION_CHECKPOINT_MS)
  3211. {
  3212. PlsrPositionCheckpointElapsedMs++;
  3213. }
  3214. if (PlsrPositionCheckpointElapsedMs >= PLSR_POSITION_CHECKPOINT_MS)
  3215. {
  3216. PlsrCheckpointPosition(1U);
  3217. }
  3218. }
  3219. static void PlsrFinishCompleted(void)
  3220. {
  3221. PlsrDisarmWaitSignal();
  3222. PlsrPlatformStopPulse((uint8_t)PlsrActiveConfig.pulseOutput);
  3223. PlsrRemainingPulses = 0UL;
  3224. PlsrPulseActive = 0U;
  3225. PlsrCutRequested = 0U;
  3226. PlsrBoundaryPending = 0U;
  3227. PlsrBoundaryWasCut = 0U;
  3228. PlsrCurrentFrequencyHz = 0UL;
  3229. PlsrQueuedFrequencyHz = 0UL;
  3230. PlsrCurrentSegment = 0U;
  3231. PlsrSegmentClockStarted = 0U;
  3232. PlsrDirectionDelayActive = 0U;
  3233. PlsrDirectionDelayRemainingMs = 0U;
  3234. PlsrExtEdgePending = 0U;
  3235. PlsrStopRequested = 0U;
  3236. PlsrStopPulsesRemaining = 0U;
  3237. PlsrAbStopArmed = 0U;
  3238. PlsrSeamlessHandoffPending = 0U;
  3239. PlsrDeferredFrequencyPending = 0U;
  3240. PlsrShortProfile.active = 0U;
  3241. PlsrProfileQueueReset();
  3242. PlsrHandoffPlan.valid = 0U;
  3243. PlsrCountedBoundaryWriteIndex = 0U;
  3244. PlsrCountedBoundaryReadIndex = 0U;
  3245. PlsrCountedBoundaryBusy = 0U;
  3246. PlsrCountedFaultEvent = 0U;
  3247. PlsrTimerErrorPending = 0U;
  3248. PlsrRamp.active = 0U;
  3249. PlsrExecutor.mode = PLSR_EXEC_IDLE;
  3250. PlsrInvalidateHandoffPlans();
  3251. PlsrRunStatus = PLSR_STATUS_COMPLETED;
  3252. PlsrError = PLSR_ERROR_NONE;
  3253. PlsrCheckpointPosition(0U);
  3254. PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS);
  3255. }
  3256. static void PlsrFinishStopped(void)
  3257. {
  3258. PlsrDisarmWaitSignal();
  3259. PlsrDiagnosticFinishSegment(0U);
  3260. PlsrPlatformStopPulse((uint8_t)PlsrActiveConfig.pulseOutput);
  3261. PlsrRemainingPulses = 0UL;
  3262. PlsrPulseActive = 0U;
  3263. PlsrCutRequested = 0U;
  3264. PlsrBoundaryPending = 0U;
  3265. PlsrBoundaryWasCut = 0U;
  3266. PlsrCurrentFrequencyHz = 0UL;
  3267. PlsrQueuedFrequencyHz = 0UL;
  3268. PlsrCurrentSegment = 0U;
  3269. PlsrSegmentClockStarted = 0U;
  3270. PlsrDirectionDelayActive = 0U;
  3271. PlsrDirectionDelayRemainingMs = 0U;
  3272. PlsrExtEdgePending = 0U;
  3273. PlsrStopRequested = 0U;
  3274. PlsrStopPulsesRemaining = 0U;
  3275. PlsrAbStopArmed = 0U;
  3276. PlsrSeamlessHandoffPending = 0U;
  3277. PlsrDeferredFrequencyPending = 0U;
  3278. PlsrShortProfile.active = 0U;
  3279. PlsrProfileQueueReset();
  3280. PlsrHandoffPlan.valid = 0U;
  3281. PlsrCountedBoundaryWriteIndex = 0U;
  3282. PlsrCountedBoundaryReadIndex = 0U;
  3283. PlsrCountedBoundaryBusy = 0U;
  3284. PlsrCountedFaultEvent = 0U;
  3285. PlsrTimerErrorPending = 0U;
  3286. PlsrRamp.active = 0U;
  3287. PlsrExecutor.mode = PLSR_EXEC_IDLE;
  3288. PlsrInvalidateHandoffPlans();
  3289. PlsrRunStatus = PLSR_STATUS_STOPPED;
  3290. PlsrError = PLSR_ERROR_NONE;
  3291. PlsrCheckpointPosition(0U);
  3292. PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS);
  3293. }
  3294. static void PlsrEnterError(PLSR_ERROR error)
  3295. {
  3296. PlsrDisarmWaitSignal();
  3297. PlsrDiagnosticFinishSegment(0U);
  3298. PlsrPlatformStopPulse((uint8_t)PlsrActiveConfig.pulseOutput);
  3299. PlsrRemainingPulses = 0UL;
  3300. PlsrPulseActive = 0U;
  3301. PlsrCutRequested = 0U;
  3302. PlsrBoundaryPending = 0U;
  3303. PlsrBoundaryWasCut = 0U;
  3304. PlsrCurrentFrequencyHz = 0UL;
  3305. PlsrQueuedFrequencyHz = 0UL;
  3306. PlsrCurrentSegment = 0U;
  3307. PlsrSegmentClockStarted = 0U;
  3308. PlsrDirectionDelayActive = 0U;
  3309. PlsrDirectionDelayRemainingMs = 0U;
  3310. PlsrExtEdgePending = 0U;
  3311. PlsrStopRequested = 0U;
  3312. PlsrStopPulsesRemaining = 0U;
  3313. PlsrAbStopArmed = 0U;
  3314. PlsrSeamlessHandoffPending = 0U;
  3315. PlsrDeferredFrequencyPending = 0U;
  3316. PlsrShortProfile.active = 0U;
  3317. PlsrProfileQueueReset();
  3318. PlsrHandoffPlan.valid = 0U;
  3319. PlsrCountedBoundaryWriteIndex = 0U;
  3320. PlsrCountedBoundaryReadIndex = 0U;
  3321. PlsrCountedBoundaryBusy = 0U;
  3322. PlsrCountedFaultEvent = 0U;
  3323. PlsrTimerErrorPending = 0U;
  3324. PlsrRamp.active = 0U;
  3325. PlsrExecutor.mode = PLSR_EXEC_IDLE;
  3326. PlsrInvalidateHandoffPlans();
  3327. PlsrRunStatus = PLSR_STATUS_ERROR;
  3328. PlsrError = error;
  3329. PlsrCheckpointPosition(0U);
  3330. PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS);
  3331. }
  3332. static void PlsrTransitionToNext(uint8_t allowCarry)
  3333. {
  3334. uint8_t nextSegment;
  3335. uint32_t carryFrequencyHz = PlsrBoundaryFrequencyHz;
  3336. PLSR_PLATFORM_SERVICE_RESULT preparedResult;
  3337. if (PlsrGetNextSegment(&nextSegment) == 0U)
  3338. {
  3339. PlsrFinishCompleted();
  3340. return;
  3341. }
  3342. if (allowCarry == 0U)
  3343. {
  3344. preparedResult = PlsrTryStartPreparedTimedSegment(nextSegment);
  3345. if (preparedResult == PLSR_PLATFORM_SERVICE_READY)
  3346. {
  3347. return;
  3348. }
  3349. if (preparedResult == PLSR_PLATFORM_SERVICE_FAILED)
  3350. {
  3351. PlsrEnterError(PLSR_ERROR_INVALID_RESOURCE);
  3352. return;
  3353. }
  3354. }
  3355. if (PlsrStartSegment(nextSegment, allowCarry, carryFrequencyHz) == 0U)
  3356. {
  3357. PlsrEnterError(PLSR_ERROR_INVALID_RESOURCE);
  3358. }
  3359. }
  3360. static uint8_t PlsrBuildHandoffPlan(uint8_t sourceSegment,
  3361. const PLSR_CONFIG *frequencyConfig,
  3362. uint32_t carryFrequencyHz,
  3363. PLSR_HANDOFF_PLAN *plan)
  3364. {
  3365. const PLSR_SEGMENT_CONFIG *segment;
  3366. uint8_t nextSegment;
  3367. int64_t displacement;
  3368. uint32_t nextFrequencyHz;
  3369. uint8_t positive;
  3370. uint8_t warmupIndex;
  3371. plan->valid = 0U;
  3372. if ((sourceSegment == 0U)
  3373. || (sourceSegment > PlsrActiveConfig.segmentCount))
  3374. {
  3375. return 0U;
  3376. }
  3377. segment = &PlsrActiveConfig.segments[sourceSegment - 1U];
  3378. if (segment->waitType != PLSR_EXT_OR_COMPLETE)
  3379. {
  3380. return 0U;
  3381. }
  3382. if (segment->jumpSegment != 0U)
  3383. {
  3384. nextSegment = (uint8_t)segment->jumpSegment;
  3385. }
  3386. else if (sourceSegment < PlsrActiveConfig.segmentCount)
  3387. {
  3388. nextSegment = (uint8_t)(sourceSegment + 1U);
  3389. }
  3390. else
  3391. {
  3392. return 0U;
  3393. }
  3394. if (PlsrActiveConfig.positionMode == PLSR_POSITION_ABSOLUTE)
  3395. {
  3396. displacement = (int64_t)PlsrActiveConfig.segments[nextSegment - 1U].pulses
  3397. - (int64_t)segment->pulses;
  3398. }
  3399. else
  3400. {
  3401. displacement = PlsrActiveConfig.segments[nextSegment - 1U].pulses;
  3402. }
  3403. if (displacement == 0)
  3404. {
  3405. return 0U;
  3406. }
  3407. positive = (displacement > 0) ? 1U : 0U;
  3408. plan->magnitude = (displacement < 0) ? (uint64_t)(-displacement)
  3409. : (uint64_t)displacement;
  3410. if (carryFrequencyHz == 0UL)
  3411. {
  3412. carryFrequencyHz =
  3413. PlsrResolvedSegmentFrequency(frequencyConfig, sourceSegment - 1U);
  3414. }
  3415. nextFrequencyHz =
  3416. PlsrResolvedSegmentFrequency(frequencyConfig, nextSegment - 1U);
  3417. if (PlsrPrepareShortProfile(&plan->profile, nextSegment,
  3418. carryFrequencyHz, nextFrequencyHz,
  3419. plan->magnitude, positive) == 0U)
  3420. {
  3421. return 0U;
  3422. }
  3423. {
  3424. PLSR_PROFILE_ENTRY firstEntry;
  3425. PLSR_PROFILE_ENTRY secondEntry;
  3426. if (PlsrShortProfileTakeRun(&plan->profile, &firstEntry) == 0U)
  3427. {
  3428. return 0U;
  3429. }
  3430. plan->firstSetting = firstEntry.setting;
  3431. plan->firstFrequencyHz = firstEntry.setting.actualFrequencyHz;
  3432. plan->firstRequestedFrequencyHz = firstEntry.requestedFrequencyHz;
  3433. plan->firstRepeatCount = firstEntry.repeatCount;
  3434. if (PlsrShortProfileTakeRun(&plan->profile, &secondEntry) != 0U)
  3435. {
  3436. plan->secondSetting = secondEntry.setting;
  3437. plan->secondFrequencyHz = secondEntry.setting.actualFrequencyHz;
  3438. plan->secondRequestedFrequencyHz =
  3439. secondEntry.requestedFrequencyHz;
  3440. plan->secondRepeatCount = secondEntry.repeatCount;
  3441. }
  3442. else
  3443. {
  3444. plan->secondSetting = firstEntry.setting;
  3445. plan->secondFrequencyHz = firstEntry.setting.actualFrequencyHz;
  3446. plan->secondRequestedFrequencyHz =
  3447. firstEntry.requestedFrequencyHz;
  3448. plan->secondRepeatCount = 0UL;
  3449. }
  3450. }
  3451. if ((plan->firstRepeatCount == 0UL)
  3452. || (plan->firstFrequencyHz == 0UL))
  3453. {
  3454. return 0U;
  3455. }
  3456. plan->warmupCount = 0U;
  3457. for (warmupIndex = 0U;
  3458. (warmupIndex < PLSR_HANDOFF_WARMUP_ITEMS)
  3459. && (plan->profile.nextPeriod < plan->profile.pulseCount);
  3460. warmupIndex++)
  3461. {
  3462. PLSR_PROFILE_ENTRY *entry = &plan->warmup[warmupIndex];
  3463. if (PlsrShortProfileTakeRun(&plan->profile, entry) == 0U)
  3464. {
  3465. return 0U;
  3466. }
  3467. plan->warmupCount++;
  3468. }
  3469. plan->firstFrequencyHz = plan->firstSetting.actualFrequencyHz;
  3470. plan->secondFrequencyHz = plan->secondSetting.actualFrequencyHz;
  3471. plan->nextSegment = nextSegment;
  3472. plan->positive = positive;
  3473. plan->valid = 1U;
  3474. return 1U;
  3475. }
  3476. static uint8_t PlsrBuildHandoffPlanBank(
  3477. const PLSR_CONFIG *frequencyConfig)
  3478. {
  3479. uint8_t buildBank = (uint8_t)(PlsrPreparedHandoffBank ^ 1U);
  3480. uint8_t sourceSegment;
  3481. PLSR_HANDOFF_PLAN *destination;
  3482. for (sourceSegment = 0U;
  3483. sourceSegment < PLSR_SEGMENT_COUNT_MAX;
  3484. sourceSegment++)
  3485. {
  3486. PlsrPreparedHandoffPlans[buildBank][sourceSegment].valid = 0U;
  3487. }
  3488. for (sourceSegment = 1U;
  3489. sourceSegment <= PlsrActiveConfig.segmentCount;
  3490. sourceSegment++)
  3491. {
  3492. destination =
  3493. &PlsrPreparedHandoffPlans[buildBank][sourceSegment - 1U];
  3494. (void)PlsrBuildHandoffPlan(sourceSegment, frequencyConfig, 0UL,
  3495. destination);
  3496. }
  3497. return buildBank;
  3498. }
  3499. static void PlsrRefreshCurrentHandoffPlan(uint32_t terminalFrequencyHz)
  3500. {
  3501. PLSR_PLATFORM_TIMER_SETTING terminalSetting;
  3502. PLSR_HANDOFF_PLAN *destination;
  3503. uint8_t currentSegment = PlsrCurrentSegment;
  3504. uint8_t preparedBank = PlsrPreparedHandoffBank;
  3505. if ((currentSegment == 0U)
  3506. || (currentSegment > PlsrActiveConfig.segmentCount)
  3507. || (PlsrPlatformBuildTimerSetting(
  3508. (uint8_t)PlsrActiveConfig.pulseOutput,
  3509. (uint8_t)PlsrActiveConfig.outputMode,
  3510. terminalFrequencyHz, &terminalSetting) == 0U))
  3511. {
  3512. return;
  3513. }
  3514. destination = &PlsrPreparedHandoffPlans[preparedBank]
  3515. [currentSegment - 1U];
  3516. (void)PlsrBuildHandoffPlan(currentSegment, &PlsrActiveConfig,
  3517. terminalSetting.actualFrequencyHz,
  3518. destination);
  3519. }
  3520. static uint8_t PlsrSelectPreparedHandoffPlan(PLSR_HANDOFF_PLAN *plan)
  3521. {
  3522. const PLSR_HANDOFF_PLAN *prepared;
  3523. uint8_t preparedBank;
  3524. uint8_t currentSegment;
  3525. plan->valid = 0U;
  3526. currentSegment = PlsrCurrentSegment;
  3527. if ((PlsrRemainingPulses != 1UL)
  3528. || (PlsrStopRequested != 0U)
  3529. || (PlsrCountOverflowPending != 0U)
  3530. || (PlsrCutRequested != 0U)
  3531. || (PlsrAbStopArmed != 0U)
  3532. || (currentSegment == 0U)
  3533. || (currentSegment > PlsrActiveConfig.segmentCount))
  3534. {
  3535. return 0U;
  3536. }
  3537. preparedBank = PlsrPreparedHandoffBank;
  3538. prepared = &PlsrPreparedHandoffPlans[preparedBank][currentSegment - 1U];
  3539. if ((prepared->valid == 0U)
  3540. || (prepared->positive != PlsrCountPositive))
  3541. {
  3542. return 0U;
  3543. }
  3544. *plan = *prepared;
  3545. return 1U;
  3546. }
  3547. static uint8_t PlsrStageCountedHandoff(void)
  3548. {
  3549. const PLSR_HANDOFF_PLAN *prepared;
  3550. PLSR_HANDOFF_PLAN candidate;
  3551. PLSR_PROFILE_ENTRY entry;
  3552. uint32_t criticalState;
  3553. uint32_t writeIndex;
  3554. uint16_t required;
  3555. uint8_t currentSegment;
  3556. uint8_t index;
  3557. PLSR_PROFILE_QUEUE *queue =
  3558. &PlsrProfileQueues[PlsrProfileQueueBank];
  3559. if ((PlsrActiveConfig.outputMode != PLSR_OUTPUT_PULSE_DIR)
  3560. || (PlsrCountedHandoffStaged != 0U)
  3561. || (PlsrStopRequested != 0U)
  3562. || (PlsrCutRequested != 0U)
  3563. || (queue->generatorComplete == 0U))
  3564. {
  3565. return 1U;
  3566. }
  3567. currentSegment = PlsrCurrentSegment;
  3568. if ((currentSegment == 0U)
  3569. || (currentSegment > PlsrActiveConfig.segmentCount))
  3570. {
  3571. return 1U;
  3572. }
  3573. prepared = &PlsrPreparedHandoffPlans[PlsrPreparedHandoffBank]
  3574. [currentSegment - 1U];
  3575. if ((prepared->valid == 0U)
  3576. || (prepared->positive != PlsrCountPositive))
  3577. {
  3578. return 1U;
  3579. }
  3580. candidate = *prepared;
  3581. required = (uint16_t)(1U + candidate.warmupCount
  3582. + ((candidate.secondRepeatCount != 0UL)
  3583. ? 1U : 0U));
  3584. criticalState = PlsrPlatformEnterCritical();
  3585. if ((PlsrCountedHandoffStaged != 0U)
  3586. || (queue->generatorComplete == 0U)
  3587. || ((uint16_t)(PLSR_PROFILE_QUEUE_CAPACITY
  3588. - PlsrProfileQueueCount()) < required))
  3589. {
  3590. PlsrPlatformExitCritical(criticalState);
  3591. return 1U;
  3592. }
  3593. writeIndex = queue->writeIndex;
  3594. entry.setting = candidate.firstSetting;
  3595. entry.requestedFrequencyHz = candidate.firstRequestedFrequencyHz;
  3596. entry.repeatCount = candidate.firstRepeatCount;
  3597. entry.startsNextSegment = 1U;
  3598. queue->entries[writeIndex & PLSR_PROFILE_QUEUE_MASK] = entry;
  3599. writeIndex++;
  3600. if (candidate.secondRepeatCount != 0UL)
  3601. {
  3602. entry.setting = candidate.secondSetting;
  3603. entry.requestedFrequencyHz =
  3604. candidate.secondRequestedFrequencyHz;
  3605. entry.repeatCount = candidate.secondRepeatCount;
  3606. entry.startsNextSegment = 0U;
  3607. queue->entries[writeIndex & PLSR_PROFILE_QUEUE_MASK] = entry;
  3608. writeIndex++;
  3609. }
  3610. for (index = 0U; index < candidate.warmupCount; index++)
  3611. {
  3612. entry = candidate.warmup[index];
  3613. entry.startsNextSegment = 0U;
  3614. queue->entries[writeIndex & PLSR_PROFILE_QUEUE_MASK] = entry;
  3615. writeIndex++;
  3616. }
  3617. queue->writeIndex = writeIndex;
  3618. PlsrCopyShortProfile(&queue->producerProfile, &candidate.profile);
  3619. queue->producerEpoch = PlsrSegmentEpoch + 1UL;
  3620. queue->producerSegment = candidate.nextSegment;
  3621. queue->generatorComplete =
  3622. ((candidate.profile.active == 0U)
  3623. || (candidate.profile.nextPeriod >= candidate.profile.pulseCount))
  3624. ? 1U : 0U;
  3625. queue->active = 1U;
  3626. PlsrHandoffPlan = candidate;
  3627. PlsrHandoffPlan.valid = 1U;
  3628. PlsrCountedHandoffStaged = 1U;
  3629. PlsrPlatformExitCritical(criticalState);
  3630. return 1U;
  3631. }
  3632. static uint8_t PlsrSyncCountedProgress(void)
  3633. {
  3634. uint64_t observed;
  3635. uint64_t delta;
  3636. uint64_t remaining;
  3637. int64_t nextPosition;
  3638. observed = PlsrPlatformObservedPulses(
  3639. (uint8_t)PlsrActiveConfig.pulseOutput);
  3640. if (observed < PlsrCountedObservedPublished)
  3641. {
  3642. return 0U;
  3643. }
  3644. delta = observed - PlsrCountedObservedPublished;
  3645. if (delta == 0ULL)
  3646. {
  3647. return 1U;
  3648. }
  3649. remaining = PlsrRemainingPulses;
  3650. if (delta > remaining)
  3651. {
  3652. delta = remaining;
  3653. #if PLSR_COUNT_ERROR_CHECK_ENABLED
  3654. PlsrCountOverflowPending = 1U;
  3655. #endif
  3656. }
  3657. if (PlsrCountPositive != 0U)
  3658. {
  3659. nextPosition = (int64_t)PlsrPosition + (int64_t)delta;
  3660. }
  3661. else
  3662. {
  3663. nextPosition = (int64_t)PlsrPosition - (int64_t)delta;
  3664. }
  3665. if ((nextPosition > (int64_t)INT32_MAX)
  3666. || (nextPosition < (int64_t)INT32_MIN))
  3667. {
  3668. PlsrPositionValid = 0U;
  3669. #if PLSR_COUNT_ERROR_CHECK_ENABLED
  3670. PlsrCountOverflowPending = 1U;
  3671. #endif
  3672. }
  3673. else
  3674. {
  3675. PlsrPosition = (int32_t)nextPosition;
  3676. }
  3677. PlsrRemainingPulses = remaining - delta;
  3678. PlsrCountedObservedPublished = observed;
  3679. PlsrPositionCheckpointDirty = 1U;
  3680. return 1U;
  3681. }
  3682. uint8_t PlsrExecTakeCountedRunIrq(
  3683. uint8_t pulseOutput,
  3684. PLSR_PLATFORM_TIMER_SETTING *setting,
  3685. uint32_t *pulseCount,
  3686. uint8_t *startsNextSegment)
  3687. {
  3688. const PLSR_PROFILE_ENTRY *entry;
  3689. PLSR_PROFILE_QUEUE *queue;
  3690. uint32_t readIndex;
  3691. uint32_t nextReadIndex;
  3692. if ((pulseOutput != (uint8_t)PlsrActiveConfig.pulseOutput)
  3693. || (setting == NULL) || (pulseCount == NULL)
  3694. || (startsNextSegment == NULL)
  3695. || (PlsrPulseActive == 0U))
  3696. {
  3697. return PLSR_EXEC_RUN_FAILED;
  3698. }
  3699. queue = &PlsrProfileQueues[PlsrProfileQueueBank];
  3700. readIndex = queue->readIndex;
  3701. if (readIndex == queue->writeIndex)
  3702. {
  3703. uint32_t availablePulses;
  3704. uint32_t holdPulses;
  3705. uint32_t holdFrequencyHz;
  3706. if (queue->generatorComplete != 0U)
  3707. {
  3708. return PLSR_EXEC_RUN_DONE;
  3709. }
  3710. PlsrProfileQueueRecordUnderrun();
  3711. /* A producer miss must not immediately turn the last scheduled edge
  3712. into an abrupt stop. Borrow at most 2 ms of still-unplanned logical
  3713. pulses and hold the last applied timer setting. The task repays
  3714. this debt by advancing (without enqueueing) the same number of
  3715. planner pulses, so segment pulse count remains exact. */
  3716. availablePulses =
  3717. (queue->producerProfile.pulseCount
  3718. > queue->producerProfile.nextPeriod)
  3719. ? queue->producerProfile.pulseCount
  3720. - queue->producerProfile.nextPeriod
  3721. : 0UL;
  3722. if (availablePulses > queue->underrunDebtPulses)
  3723. {
  3724. availablePulses -= queue->underrunDebtPulses;
  3725. }
  3726. else
  3727. {
  3728. availablePulses = 0UL;
  3729. }
  3730. holdFrequencyHz = queue->lastSetting.actualFrequencyHz;
  3731. holdPulses = (uint32_t)(
  3732. ((uint64_t)holdFrequencyHz * PLSR_UNDERRUN_HOLD_TIME_US
  3733. + 999999ULL) / 1000000ULL);
  3734. if (holdPulses == 0UL)
  3735. {
  3736. holdPulses = 1UL;
  3737. }
  3738. if (holdPulses > PLSR_UNDERRUN_HOLD_MAX_PULSES)
  3739. {
  3740. holdPulses = PLSR_UNDERRUN_HOLD_MAX_PULSES;
  3741. }
  3742. if (holdPulses > availablePulses)
  3743. {
  3744. holdPulses = availablePulses;
  3745. }
  3746. if ((queue->active == 0U) || (queue->haveLastSetting == 0U)
  3747. || (holdFrequencyHz == 0UL) || (holdPulses == 0UL))
  3748. {
  3749. return PLSR_EXEC_RUN_FAILED;
  3750. }
  3751. queue->underrunDebtPulses += holdPulses;
  3752. queue->generation++;
  3753. *setting = queue->lastSetting;
  3754. *pulseCount = holdPulses;
  3755. *startsNextSegment = 0U;
  3756. PlsrQueuedFrequencyHz = holdFrequencyHz;
  3757. return PLSR_EXEC_RUN_READY;
  3758. }
  3759. entry = &queue->entries[readIndex & PLSR_PROFILE_QUEUE_MASK];
  3760. if (entry->repeatCount == 0UL)
  3761. {
  3762. return PLSR_EXEC_RUN_FAILED;
  3763. }
  3764. /* 先校验 handoff 前置条件,再推进读索引:失败时该项不被消费。
  3765. 不在此处更新任何执行状态(Pending/active 等),全部由任务消费事件时做。 */
  3766. if (entry->startsNextSegment != 0U)
  3767. {
  3768. if ((PlsrCountedHandoffStaged == 0U)
  3769. || (PlsrHandoffPlan.valid == 0U))
  3770. {
  3771. return PLSR_EXEC_RUN_FAILED;
  3772. }
  3773. }
  3774. *setting = entry->setting;
  3775. *pulseCount = entry->repeatCount;
  3776. *startsNextSegment = entry->startsNextSegment;
  3777. nextReadIndex = readIndex + 1UL;
  3778. queue->readIndex = nextReadIndex;
  3779. queue->lastSetting = entry->setting;
  3780. queue->haveLastSetting = 1U;
  3781. if (queue->generatorComplete == 0U)
  3782. {
  3783. PlsrProfileQueueRecordDepth(
  3784. (uint16_t)(queue->writeIndex - nextReadIndex));
  3785. }
  3786. /* 记录硬件即将生效的频率(事实镜像),不做任何状态转换。 */
  3787. PlsrQueuedFrequencyHz = entry->setting.actualFrequencyHz;
  3788. return PLSR_EXEC_RUN_READY;
  3789. }
  3790. void PlsrExecCountedSegmentBoundaryIrq(uint8_t pulseOutput,
  3791. uint32_t completedPulses)
  3792. {
  3793. PLSR_COUNTED_BOUNDARY_EVENT *event;
  3794. uint8_t writeIndex;
  3795. if (pulseOutput != (uint8_t)PlsrActiveConfig.pulseOutput)
  3796. {
  3797. return;
  3798. }
  3799. if (PlsrHandoffPlan.valid == 0U)
  3800. {
  3801. PlsrCountedFaultEvent = 1U;
  3802. return;
  3803. }
  3804. writeIndex = PlsrCountedBoundaryWriteIndex;
  3805. if ((PlsrCountedBoundaryBusy != 0U)
  3806. || ((uint8_t)(writeIndex - PlsrCountedBoundaryReadIndex)
  3807. >= PLSR_BOUNDARY_EVENT_CAPACITY))
  3808. {
  3809. PlsrCountedFaultEvent = 1U;
  3810. return;
  3811. }
  3812. event = &PlsrCountedBoundaryEvents[
  3813. writeIndex & (PLSR_BOUNDARY_EVENT_CAPACITY - 1U)];
  3814. event->magnitude = PlsrHandoffPlan.magnitude;
  3815. event->firstFrequencyHz = PlsrHandoffPlan.firstFrequencyHz;
  3816. event->completedPulses = completedPulses;
  3817. event->nextSegment = PlsrHandoffPlan.nextSegment;
  3818. event->positive = PlsrHandoffPlan.positive;
  3819. #if defined(__ICCARM__)
  3820. __DMB();
  3821. #endif
  3822. PlsrCountedBoundaryWriteIndex = (uint8_t)(writeIndex + 1U);
  3823. }
  3824. static void PlsrExecServiceCountedBoundaryEvent(void);
  3825. /* 任务上下文:消费 counted 流的全部 IRQ 事件(流故障 + 段边界)。
  3826. 段切换簿记与错误标志写入均在此完成,IRQ 内不再做状态转换。 */
  3827. static void PlsrExecServiceCountedEvents(void)
  3828. {
  3829. if (PlsrCountedFaultEvent != 0U)
  3830. {
  3831. PlsrCountedFaultEvent = 0U;
  3832. PlsrTimerErrorPending = 1U;
  3833. return;
  3834. }
  3835. PlsrExecServiceCountedBoundaryEvent();
  3836. }
  3837. /* 任务上下文:消费段边界事件,完成全部段切换簿记。
  3838. 硬件连续性不依赖本函数(下一段首 run 已由队列预装载),晚 1ms 消费无影响。 */
  3839. static void PlsrExecServiceCountedBoundaryEvent(void)
  3840. {
  3841. PLSR_COUNTED_BOUNDARY_EVENT event;
  3842. uint64_t observed;
  3843. uint64_t delta;
  3844. int64_t nextPosition;
  3845. uint32_t currentFrequencyHz;
  3846. uint32_t queuedFrequencyHz;
  3847. uint8_t nextSegment;
  3848. uint32_t criticalState;
  3849. uint8_t readIndex;
  3850. criticalState = PlsrPlatformEnterCritical();
  3851. readIndex = PlsrCountedBoundaryReadIndex;
  3852. if (readIndex == PlsrCountedBoundaryWriteIndex)
  3853. {
  3854. PlsrPlatformExitCritical(criticalState);
  3855. return;
  3856. }
  3857. event = PlsrCountedBoundaryEvents[
  3858. readIndex & (PLSR_BOUNDARY_EVENT_CAPACITY - 1U)];
  3859. PlsrCountedBoundaryReadIndex = (uint8_t)(readIndex + 1U);
  3860. PlsrCountedBoundaryBusy = 1U;
  3861. PlsrPlatformExitCritical(criticalState);
  3862. observed = event.completedPulses;
  3863. nextSegment = event.nextSegment;
  3864. if ((nextSegment == 0U)
  3865. || (nextSegment > PlsrActiveConfig.segmentCount))
  3866. {
  3867. PlsrTimerErrorPending = 1U;
  3868. return;
  3869. }
  3870. /* 用 IRQ 边界时刻的计数快照结算旧段:任务消费时下一 run 可能已发出
  3871. 若干脉冲,直接读当前硬件计数会把下一段脉冲算进旧段。 */
  3872. if (observed < PlsrCountedObservedPublished)
  3873. {
  3874. PlsrTimerErrorPending = 1U;
  3875. return;
  3876. }
  3877. delta = observed - PlsrCountedObservedPublished;
  3878. if (delta > PlsrRemainingPulses)
  3879. {
  3880. delta = PlsrRemainingPulses;
  3881. #if PLSR_COUNT_ERROR_CHECK_ENABLED
  3882. PlsrCountOverflowPending = 1U;
  3883. #endif
  3884. }
  3885. if (PlsrCountPositive != 0U)
  3886. {
  3887. nextPosition = (int64_t)PlsrPosition + (int64_t)delta;
  3888. }
  3889. else
  3890. {
  3891. nextPosition = (int64_t)PlsrPosition - (int64_t)delta;
  3892. }
  3893. if ((nextPosition > (int64_t)INT32_MAX)
  3894. || (nextPosition < (int64_t)INT32_MIN))
  3895. {
  3896. PlsrPositionValid = 0U;
  3897. #if PLSR_COUNT_ERROR_CHECK_ENABLED
  3898. PlsrCountOverflowPending = 1U;
  3899. #endif
  3900. }
  3901. else
  3902. {
  3903. PlsrPosition = (int32_t)nextPosition;
  3904. }
  3905. PlsrRemainingPulses -= delta;
  3906. PlsrCountedObservedPublished = observed;
  3907. PlsrPositionCheckpointDirty = 1U;
  3908. if (PlsrRemainingPulses != 0ULL)
  3909. {
  3910. PlsrTimerErrorPending = 1U;
  3911. return;
  3912. }
  3913. PlsrDiagnosticFinishSegment(1U);
  3914. PlsrSegmentEpoch++;
  3915. PlsrCurrentSegment = nextSegment;
  3916. PlsrRemainingPulses = event.magnitude;
  3917. PlsrCountPositive = event.positive;
  3918. PlsrCurrentFrequencyHz = event.firstFrequencyHz;
  3919. PlsrBoundaryFrequencyHz = event.firstFrequencyHz;
  3920. PlsrSegmentClockStarted = 1U;
  3921. PlsrSegmentElapsedMs = 0UL;
  3922. PlsrWaitElapsedMs = 0UL;
  3923. PlsrBoundaryRampStarted = 0U;
  3924. PlsrBoundaryPending = 0U;
  3925. PlsrBoundaryWasCut = 0U;
  3926. PlsrCutRequested = 0U;
  3927. if (PlsrFrequencyUpdatePending != 0U)
  3928. {
  3929. /* An online frequency transaction that met a latched segment marker
  3930. follows the hardware into the new segment instead of invalidating
  3931. the old handoff metadata. */
  3932. PlsrFrequencyUpdateSegment = nextSegment;
  3933. PlsrFrequencyUpdateTargetHz =
  3934. PlsrActiveConfig.segments[nextSegment - 1U].frequencyHz;
  3935. }
  3936. PlsrDeferredFrequencyPending = 0U;
  3937. PlsrExtEdgePending = 0U;
  3938. PlsrExtPreviousLevel =
  3939. PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.extInput);
  3940. PlsrCopyShortProfile(&PlsrShortProfile,
  3941. &PlsrProfileQueue.producerProfile);
  3942. PlsrDiagnosticBeginSegment(nextSegment, event.magnitude, event.positive);
  3943. PlsrSeamlessHandoffPending = 1U;
  3944. PlsrCountedHandoffStaged = 0U;
  3945. PlsrHandoffPlan.valid = 0U;
  3946. PlsrExecutor.generation++;
  3947. PlsrExecutor.mode = (PlsrProfileQueue.generatorComplete != 0U)
  3948. ? PLSR_EXEC_STEP_TABLE
  3949. : PLSR_EXEC_STREAM;
  3950. currentFrequencyHz = PlsrCurrentFrequencyHz;
  3951. queuedFrequencyHz = PlsrQueuedFrequencyHz;
  3952. if (queuedFrequencyHz > currentFrequencyHz)
  3953. {
  3954. PlsrRunStatus = PLSR_STATUS_ACCELERATING;
  3955. }
  3956. else if (queuedFrequencyHz < currentFrequencyHz)
  3957. {
  3958. PlsrRunStatus = PLSR_STATUS_DECELERATING;
  3959. }
  3960. else
  3961. {
  3962. PlsrRunStatus = PLSR_STATUS_RUNNING;
  3963. }
  3964. criticalState = PlsrPlatformEnterCritical();
  3965. PlsrCountedBoundaryBusy = 0U;
  3966. PlsrPlatformExitCritical(criticalState);
  3967. }
  3968. void PlsrExecCountedStreamFaultIrq(uint8_t pulseOutput)
  3969. {
  3970. if (pulseOutput == (uint8_t)PlsrActiveConfig.pulseOutput)
  3971. {
  3972. /* 只置事件:任务消费时统一置 PlsrTimerErrorPending。 */
  3973. PlsrCountedFaultEvent = 1U;
  3974. }
  3975. }
  3976. static uint8_t PlsrPrepareFutureHandoffQueue(void)
  3977. {
  3978. const PLSR_HANDOFF_PLAN *prepared;
  3979. uint8_t currentSegment = PlsrCurrentSegment;
  3980. uint8_t preparedBank = PlsrPreparedHandoffBank;
  3981. uint16_t fillBudget = PLSR_PROFILE_STARTUP_BUDGET;
  3982. PlsrProfileQueueReset();
  3983. if ((currentSegment == 0U)
  3984. || (currentSegment > PlsrActiveConfig.segmentCount))
  3985. {
  3986. return 1U;
  3987. }
  3988. prepared = &PlsrPreparedHandoffPlans[preparedBank][currentSegment - 1U];
  3989. if ((prepared->valid == 0U) || (prepared->profile.active == 0U)
  3990. || (prepared->profile.nextPeriod >= prepared->profile.pulseCount))
  3991. {
  3992. return 1U;
  3993. }
  3994. (void)PlsrProfileQueueBegin(&prepared->profile,
  3995. PlsrSegmentEpoch + 1UL,
  3996. prepared->nextSegment,
  3997. preparedBank);
  3998. if (PlsrProfileQueueFill(PLSR_PROFILE_STARTUP_TARGET,
  3999. &fillBudget) == 0U)
  4000. {
  4001. PlsrProfileQueueReset();
  4002. return 0U;
  4003. }
  4004. return 1U;
  4005. }
  4006. /* The first two periods are already in the timer pipeline. Seed a small
  4007. precomputed tail here so the 1 ms producer has time to resume without
  4008. doing planner math in the pulse IRQ. */
  4009. static void PlsrActivateHandoffQueueFromIrq(
  4010. const PLSR_HANDOFF_PLAN *plan,
  4011. uint32_t producerEpoch)
  4012. {
  4013. uint8_t index;
  4014. PLSR_PROFILE_QUEUE *queue =
  4015. &PlsrProfileQueues[PlsrProfileQueueBank];
  4016. uint32_t readIndex = queue->readIndex;
  4017. uint32_t writeIndex = queue->writeIndex;
  4018. uint32_t queuedEpoch = queue->producerEpoch;
  4019. uint8_t queuedSegment = queue->producerSegment;
  4020. uint8_t queueActive = queue->active;
  4021. if ((queueActive != 0U) && (queuedEpoch == producerEpoch)
  4022. && (queuedSegment == plan->nextSegment)
  4023. && (readIndex != writeIndex))
  4024. {
  4025. PlsrCopyShortProfile(&PlsrShortProfile,
  4026. &queue->producerProfile);
  4027. PlsrShortProfile.active = 1U;
  4028. PlsrProfileQueueMinimumDepth = PlsrProfileQueueCount();
  4029. PlsrProfileQueueLowWaterLatched = 0U;
  4030. return;
  4031. }
  4032. /* Fallback for a stale/missing prepared bank. This path is fault
  4033. containment only; normal AB handoff uses the 400-item task-built queue. */
  4034. queue->generation++;
  4035. queue->readIndex = 0U;
  4036. queue->writeIndex = plan->warmupCount;
  4037. for (index = 0U; index < plan->warmupCount; index++)
  4038. {
  4039. queue->entries[index] = plan->warmup[index];
  4040. }
  4041. PlsrCopyShortProfile(&queue->producerProfile, &plan->profile);
  4042. queue->producerEpoch = producerEpoch;
  4043. queue->producerSegment = plan->nextSegment;
  4044. queue->preparedHandoffBank = PlsrPreparedHandoffBank;
  4045. queue->generatorComplete =
  4046. (plan->profile.active == 0U) ? 1U : 0U;
  4047. queue->active =
  4048. ((plan->warmupCount != 0U) || (plan->profile.active != 0U))
  4049. ? 1U : 0U;
  4050. PlsrCopyShortProfile(&PlsrShortProfile, &plan->profile);
  4051. if (queue->active != 0U)
  4052. {
  4053. PlsrShortProfile.active = 1U;
  4054. }
  4055. }
  4056. static PLSR_PLATFORM_QUEUE_RESULT PlsrPrimeHandoff(void)
  4057. {
  4058. PLSR_HANDOFF_PLAN candidatePlan;
  4059. uint32_t actualFrequencyHz;
  4060. PLSR_PLATFORM_QUEUE_RESULT result;
  4061. PlsrHandoffPlan.valid = 0U;
  4062. if (PlsrAbStopArmed != 0U)
  4063. {
  4064. return PLSR_PLATFORM_QUEUE_FAILED;
  4065. }
  4066. if (PlsrSelectPreparedHandoffPlan(&candidatePlan) == 0U)
  4067. {
  4068. return PLSR_PLATFORM_QUEUE_FAILED;
  4069. }
  4070. PlsrDeferredFrequencyPending = 0U;
  4071. result = PlsrPlatformLoadPreparedFromIrq(
  4072. (uint8_t)PlsrActiveConfig.pulseOutput,
  4073. &candidatePlan.firstSetting, &actualFrequencyHz);
  4074. if (result != PLSR_PLATFORM_QUEUE_APPLIED)
  4075. {
  4076. if (result == PLSR_PLATFORM_QUEUE_FAILED)
  4077. {
  4078. PlsrTimerErrorPending = 1U;
  4079. }
  4080. return result;
  4081. }
  4082. PlsrDiagnosticRecordFrequency(candidatePlan.firstRequestedFrequencyHz,
  4083. candidatePlan.firstSetting.actualFrequencyHz,
  4084. actualFrequencyHz);
  4085. PlsrDiagnosticQueuedExpectedHz =
  4086. candidatePlan.firstSetting.actualFrequencyHz;
  4087. PlsrQueuedFrequencyHz = actualFrequencyHz;
  4088. PlsrHandoffPlan = candidatePlan;
  4089. PlsrHandoffPlan.firstFrequencyHz = actualFrequencyHz;
  4090. PlsrHandoffPlan.valid = 1U;
  4091. return PLSR_PLATFORM_QUEUE_APPLIED;
  4092. }
  4093. static PLSR_PLATFORM_QUEUE_RESULT PlsrTryContinuousHandoff(void)
  4094. {
  4095. uint32_t actualQueuedFrequencyHz;
  4096. uint32_t currentFrequencyHz;
  4097. uint32_t queuedFrequencyHz;
  4098. uint8_t nextSegment = PlsrHandoffPlan.nextSegment;
  4099. uint64_t magnitude = PlsrHandoffPlan.magnitude;
  4100. uint8_t positive = PlsrHandoffPlan.positive;
  4101. PLSR_PLATFORM_QUEUE_RESULT result;
  4102. if ((PlsrHandoffPlan.valid == 0U)
  4103. || (PlsrStopRequested != 0U)
  4104. || (PlsrCountOverflowPending != 0U)
  4105. || (PlsrCutRequested != 0U)
  4106. || (PlsrAbStopArmed != 0U)
  4107. || (PlsrCurrentFrequencyHz != PlsrHandoffPlan.firstFrequencyHz)
  4108. || (nextSegment == 0U)
  4109. || (nextSegment > PlsrActiveConfig.segmentCount))
  4110. {
  4111. return PLSR_PLATFORM_QUEUE_FAILED;
  4112. }
  4113. PlsrDeferredFrequencyPending = 0U;
  4114. result = PlsrPlatformLoadPreparedFromIrq(
  4115. (uint8_t)PlsrActiveConfig.pulseOutput,
  4116. &PlsrHandoffPlan.secondSetting, &actualQueuedFrequencyHz);
  4117. if (result == PLSR_PLATFORM_QUEUE_STALE)
  4118. {
  4119. return result;
  4120. }
  4121. if (result == PLSR_PLATFORM_QUEUE_FAILED)
  4122. {
  4123. PlsrShortProfile.active = 0U;
  4124. PlsrHandoffPlan.valid = 0U;
  4125. PlsrTimerErrorPending = 1U;
  4126. return result;
  4127. }
  4128. PlsrDiagnosticRecordFrequency(
  4129. PlsrHandoffPlan.secondRequestedFrequencyHz,
  4130. PlsrHandoffPlan.secondSetting.actualFrequencyHz,
  4131. actualQueuedFrequencyHz);
  4132. PlsrDiagnosticQueuedExpectedHz =
  4133. PlsrHandoffPlan.secondSetting.actualFrequencyHz;
  4134. PlsrQueuedFrequencyHz = actualQueuedFrequencyHz;
  4135. PlsrDiagnosticFinishSegment(1U);
  4136. PlsrSegmentEpoch++;
  4137. PlsrActivateHandoffQueueFromIrq(&PlsrHandoffPlan,
  4138. PlsrSegmentEpoch);
  4139. PlsrCurrentSegment = nextSegment;
  4140. PlsrRemainingPulses = magnitude;
  4141. PlsrCountPositive = positive;
  4142. PlsrBoundaryFrequencyHz = PlsrCurrentFrequencyHz;
  4143. PlsrSegmentClockStarted = 1U;
  4144. PlsrSegmentElapsedMs = 0UL;
  4145. PlsrWaitElapsedMs = 0UL;
  4146. PlsrBoundaryRampStarted = 0U;
  4147. PlsrBoundaryPending = 0U;
  4148. PlsrBoundaryWasCut = 0U;
  4149. PlsrCutRequested = 0U;
  4150. PlsrFrequencyUpdatePending = 0U;
  4151. PlsrDeferredFrequencyPending = 0U;
  4152. PlsrExtEdgePending = 0U;
  4153. PlsrExtPreviousLevel =
  4154. PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.extInput);
  4155. PlsrDiagnosticBeginSegment(nextSegment, magnitude, positive);
  4156. PlsrSeamlessHandoffPending = 1U;
  4157. PlsrHandoffPlan.valid = 0U;
  4158. if (PlsrRemainingPulses == 1UL)
  4159. {
  4160. result = PlsrPrimeHandoff();
  4161. if (result == PLSR_PLATFORM_QUEUE_STALE)
  4162. {
  4163. return result;
  4164. }
  4165. }
  4166. (void)PlsrQueueFinalAbBoundaryFromIrq();
  4167. queuedFrequencyHz = PlsrQueuedFrequencyHz;
  4168. currentFrequencyHz = PlsrCurrentFrequencyHz;
  4169. if (PlsrShortProfile.active != 0U)
  4170. {
  4171. if (queuedFrequencyHz > currentFrequencyHz)
  4172. {
  4173. PlsrRunStatus = PLSR_STATUS_ACCELERATING;
  4174. }
  4175. else if (queuedFrequencyHz < currentFrequencyHz)
  4176. {
  4177. PlsrRunStatus = PLSR_STATUS_DECELERATING;
  4178. }
  4179. else
  4180. {
  4181. PlsrRunStatus = PLSR_STATUS_RUNNING;
  4182. }
  4183. }
  4184. else
  4185. {
  4186. PlsrRunStatus = PLSR_STATUS_RUNNING;
  4187. }
  4188. return PLSR_PLATFORM_QUEUE_APPLIED;
  4189. }
  4190. /* EXTI only records a WAIT edge. Motion state transitions and timer startup
  4191. remain in the task context. The armed mask prevents an edge from an idle
  4192. or unrelated input from releasing a later segment. */
  4193. void PlsrWaitInputExtiIrq(uint8_t inputSelection)
  4194. {
  4195. uint8_t bit;
  4196. if (inputSelection > 1U)
  4197. {
  4198. return;
  4199. }
  4200. bit = (uint8_t)(1U << inputSelection);
  4201. if ((PlsrWaitSignalArmedMask & bit) != 0U)
  4202. {
  4203. PlsrWaitSignalPendingMask |= bit;
  4204. }
  4205. }
  4206. static void PlsrArmWaitSignal(uint8_t inputSelection)
  4207. {
  4208. uint32_t criticalState;
  4209. uint8_t bit;
  4210. if (inputSelection > 1U)
  4211. {
  4212. return;
  4213. }
  4214. bit = (uint8_t)(1U << inputSelection);
  4215. criticalState = PlsrPlatformEnterCritical();
  4216. PlsrWaitSignalPendingMask = 0U;
  4217. PlsrWaitSignalArmedMask = bit;
  4218. PlsrPlatformExitCritical(criticalState);
  4219. }
  4220. static void PlsrDisarmWaitSignal(void)
  4221. {
  4222. uint32_t criticalState = PlsrPlatformEnterCritical();
  4223. PlsrWaitSignalArmedMask = 0U;
  4224. PlsrWaitSignalPendingMask = 0U;
  4225. PlsrPlatformExitCritical(criticalState);
  4226. }
  4227. static uint8_t PlsrWaitSignalDetected(uint8_t inputSelection)
  4228. {
  4229. uint32_t criticalState;
  4230. uint8_t bit;
  4231. if (inputSelection > 1U)
  4232. {
  4233. return 0U;
  4234. }
  4235. bit = (uint8_t)(1U << inputSelection);
  4236. criticalState = PlsrPlatformEnterCritical();
  4237. if ((PlsrWaitSignalPendingMask & bit) != 0U)
  4238. {
  4239. PlsrWaitSignalPendingMask &= (uint8_t)~bit;
  4240. PlsrWaitSignalArmedMask &= (uint8_t)~bit;
  4241. PlsrPlatformExitCritical(criticalState);
  4242. return 1U;
  4243. }
  4244. PlsrPlatformExitCritical(criticalState);
  4245. /* Preserve the original level semantics: a signal already high when the
  4246. segment enters WAIT must release it even though no new EXTI edge occurs. */
  4247. if (PlsrPlatformReadInput(inputSelection) != 0U)
  4248. {
  4249. criticalState = PlsrPlatformEnterCritical();
  4250. PlsrWaitSignalPendingMask &= (uint8_t)~bit;
  4251. PlsrWaitSignalArmedMask &= (uint8_t)~bit;
  4252. PlsrPlatformExitCritical(criticalState);
  4253. return 1U;
  4254. }
  4255. return 0U;
  4256. }
  4257. static void PlsrHandleBoundary(uint8_t extEdge)
  4258. {
  4259. const PLSR_SEGMENT_CONFIG *segment;
  4260. uint32_t criticalState;
  4261. uint8_t wasCut = PlsrBoundaryWasCut;
  4262. uint8_t preserveTimedStart;
  4263. PlsrPlatformStopPulse((uint8_t)PlsrActiveConfig.pulseOutput);
  4264. PlsrBoundaryPending = 0U;
  4265. PlsrBoundaryWasCut = 0U;
  4266. PlsrPulseActive = 0U;
  4267. PlsrCurrentFrequencyHz = 0UL;
  4268. PlsrQueuedFrequencyHz = 0UL;
  4269. PlsrShortProfile.active = 0U;
  4270. criticalState = PlsrPlatformEnterCritical();
  4271. preserveTimedStart =
  4272. (((PlsrTimedStart.valid != 0U)
  4273. || (PlsrTimedStart.building != 0U))
  4274. && (PlsrTimedStart.sourceEpoch == PlsrSegmentEpoch)
  4275. && (PlsrTimedStart.sourceSegment == PlsrCurrentSegment)
  4276. && (PlsrTimedStart.queueBank != PlsrProfileQueueBank)) ? 1U : 0U;
  4277. if (preserveTimedStart != 0U)
  4278. {
  4279. PlsrProfileQueueResetBankLocked(PlsrProfileQueueBank);
  4280. PlsrProfileQueueLowWaterLatched = 0U;
  4281. }
  4282. PlsrPlatformExitCritical(criticalState);
  4283. if (preserveTimedStart == 0U)
  4284. {
  4285. PlsrProfileQueueReset();
  4286. }
  4287. PlsrHandoffPlan.valid = 0U;
  4288. PlsrDeferredFrequencyPending = 0U;
  4289. PlsrAbStopArmed = 0U;
  4290. PlsrCheckpointPosition(1U);
  4291. if (PlsrTimerErrorPending != 0U)
  4292. {
  4293. PlsrTimerErrorPending = 0U;
  4294. PlsrEnterError(PLSR_ERROR_TIMER);
  4295. return;
  4296. }
  4297. #if PLSR_COUNT_ERROR_CHECK_ENABLED
  4298. if (PlsrCountOverflowPending != 0U)
  4299. {
  4300. PlsrCountOverflowPending = 0U;
  4301. PlsrEnterError(PLSR_ERROR_COUNT);
  4302. return;
  4303. }
  4304. #else
  4305. /* 计数错误检查暂时关闭:偏差已由同步路径钳制,标志直接清除。 */
  4306. PlsrCountOverflowPending = 0U;
  4307. #endif
  4308. if (PlsrStopRequested != 0U)
  4309. {
  4310. PlsrFinishStopped();
  4311. return;
  4312. }
  4313. if ((PlsrCurrentSegment == 0U)
  4314. || (PlsrCurrentSegment > PlsrActiveConfig.segmentCount))
  4315. {
  4316. PlsrEnterError(PLSR_ERROR_INTERNAL);
  4317. return;
  4318. }
  4319. segment = &PlsrActiveConfig.segments[PlsrCurrentSegment - 1U];
  4320. if (wasCut != 0U)
  4321. {
  4322. PlsrTransitionToNext(
  4323. (PlsrActiveConfig.sendMode == PLSR_SEND_SUBSEQUENT) ? 1U : 0U);
  4324. return;
  4325. }
  4326. switch (segment->waitType)
  4327. {
  4328. case PLSR_WAIT_TIME:
  4329. PlsrWaitElapsedMs = 0UL;
  4330. if (segment->waitTimeMs == 0UL)
  4331. {
  4332. PlsrTransitionToNext(0U);
  4333. }
  4334. else
  4335. {
  4336. PlsrRunStatus = PLSR_STATUS_WAITING;
  4337. }
  4338. break;
  4339. case PLSR_WAIT_SIGNAL:
  4340. PlsrRunStatus = PLSR_STATUS_WAITING;
  4341. PlsrArmWaitSignal((uint8_t)PlsrActiveConfig.waitInput);
  4342. if (PlsrWaitSignalDetected(
  4343. (uint8_t)PlsrActiveConfig.waitInput) != 0U)
  4344. {
  4345. PlsrTransitionToNext(0U);
  4346. }
  4347. break;
  4348. case PLSR_ACT_TIME:
  4349. if (PlsrSegmentElapsedMs >= segment->actTimeMs)
  4350. {
  4351. PlsrTransitionToNext(0U);
  4352. }
  4353. else
  4354. {
  4355. PlsrRunStatus = PLSR_STATUS_WAITING;
  4356. }
  4357. break;
  4358. case PLSR_EXT_SIGNAL:
  4359. if (extEdge != 0U)
  4360. {
  4361. PlsrTransitionToNext(0U);
  4362. }
  4363. else
  4364. {
  4365. PlsrRunStatus = PLSR_STATUS_WAITING;
  4366. }
  4367. break;
  4368. case PLSR_EXT_OR_COMPLETE:
  4369. PlsrTransitionToNext(1U);
  4370. break;
  4371. default:
  4372. PlsrEnterError(PLSR_ERROR_INTERNAL);
  4373. break;
  4374. }
  4375. }
  4376. static void PlsrRequestCut(uint32_t expectedEpoch)
  4377. {
  4378. uint32_t criticalState = PlsrPlatformEnterCritical();
  4379. if (PlsrSegmentEpoch != expectedEpoch)
  4380. {
  4381. PlsrPlatformExitCritical(criticalState);
  4382. return;
  4383. }
  4384. if (PlsrBoundaryPending != 0U)
  4385. {
  4386. PlsrBoundaryWasCut = 1U;
  4387. }
  4388. else if (PlsrPulseActive != 0U)
  4389. {
  4390. PlsrCutRequested = 1U;
  4391. (void)PlsrArmFinalAbBoundaryLocked();
  4392. }
  4393. else
  4394. {
  4395. PlsrBoundaryFrequencyHz = PlsrCurrentFrequencyHz;
  4396. PlsrBoundaryWasCut = 1U;
  4397. PlsrBoundaryPending = 1U;
  4398. }
  4399. PlsrPlatformExitCritical(criticalState);
  4400. }
  4401. static void PlsrPollWaiting(uint8_t extEdge)
  4402. {
  4403. const PLSR_SEGMENT_CONFIG *segment =
  4404. &PlsrActiveConfig.segments[PlsrCurrentSegment - 1U];
  4405. switch (segment->waitType)
  4406. {
  4407. case PLSR_WAIT_TIME:
  4408. PlsrWaitElapsedMs++;
  4409. if (PlsrWaitElapsedMs >= segment->waitTimeMs)
  4410. {
  4411. PlsrTransitionToNext(0U);
  4412. }
  4413. break;
  4414. case PLSR_WAIT_SIGNAL:
  4415. if (PlsrWaitSignalDetected(
  4416. (uint8_t)PlsrActiveConfig.waitInput) != 0U)
  4417. {
  4418. PlsrTransitionToNext(0U);
  4419. }
  4420. break;
  4421. case PLSR_ACT_TIME:
  4422. if (PlsrSegmentElapsedMs >= segment->actTimeMs)
  4423. {
  4424. PlsrTransitionToNext(0U);
  4425. }
  4426. break;
  4427. case PLSR_EXT_SIGNAL:
  4428. if (extEdge != 0U)
  4429. {
  4430. PlsrTransitionToNext(0U);
  4431. }
  4432. break;
  4433. default:
  4434. PlsrEnterError(PLSR_ERROR_INTERNAL);
  4435. break;
  4436. }
  4437. }
  4438. static void PlsrPollPersistenceDelay(void)
  4439. {
  4440. if ((PlsrPersistenceDirty != 0U)
  4441. && (PlsrIsBusy() == 0U)
  4442. && (PlsrPersistenceDelayMs != 0U))
  4443. {
  4444. PlsrPersistenceDelayMs--;
  4445. }
  4446. }
  4447. void PlsrServicePersistence(void)
  4448. {
  4449. PLSR_PERSIST_PAYLOAD payload;
  4450. PLSR_PLATFORM_SERVICE_RESULT serviceResult;
  4451. uint32_t criticalState;
  4452. if (PlsrIsBusy() != 0U)
  4453. {
  4454. return;
  4455. }
  4456. serviceResult = PlsrPlatformServicePersistence();
  4457. if (serviceResult == PLSR_PLATFORM_SERVICE_FAILED)
  4458. {
  4459. PlsrRunStatus = PLSR_STATUS_ERROR;
  4460. PlsrError = PLSR_ERROR_INTERNAL;
  4461. return;
  4462. }
  4463. if ((serviceResult == PLSR_PLATFORM_SERVICE_DEFERRED)
  4464. || (PlsrPersistenceDirty == 0U))
  4465. {
  4466. return;
  4467. }
  4468. if (PlsrPersistenceDelayMs != 0U)
  4469. {
  4470. return;
  4471. }
  4472. payload.config = PlsrShadowConfig;
  4473. criticalState = PlsrPlatformEnterCritical();
  4474. payload.position = PlsrPosition;
  4475. payload.positionValid = PlsrPositionValid;
  4476. PlsrPlatformExitCritical(criticalState);
  4477. payload.wasBusy = 0U;
  4478. payload.reserved = 0U;
  4479. if (PlsrPlatformSave(&payload) != 0U)
  4480. {
  4481. PlsrPersistenceDirty = 0U;
  4482. }
  4483. else
  4484. {
  4485. PlsrRunStatus = PLSR_STATUS_ERROR;
  4486. PlsrError = PLSR_ERROR_INTERNAL;
  4487. }
  4488. }
  4489. uint8_t PlsrInit(void)
  4490. {
  4491. PLSR_PERSIST_PAYLOAD payload;
  4492. PlsrInitialized = 0U;
  4493. PlsrRunStatus = PLSR_STATUS_UNINITIALIZED;
  4494. if (PlsrPlatformInit() == 0U)
  4495. {
  4496. return 0U;
  4497. }
  4498. if ((PlsrPlatformLoad(&payload) == 0U)
  4499. || (PlsrConfigIsValid(&payload.config, 0U) == 0U)
  4500. || (payload.positionValid > 1U) || (payload.wasBusy > 1U))
  4501. {
  4502. PlsrSetDefaults(&PlsrShadowConfig);
  4503. PlsrPosition = 0L;
  4504. PlsrPositionValid = 1U;
  4505. PlsrPlatformCheckpointConfig(&PlsrShadowConfig);
  4506. PlsrPlatformCheckpointPosition(0L, 1U, 0U);
  4507. PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS);
  4508. }
  4509. else
  4510. {
  4511. PlsrShadowConfig = payload.config;
  4512. PlsrPosition = payload.position;
  4513. PlsrPositionValid = ((payload.positionValid != 0U)
  4514. && (payload.wasBusy == 0U)) ? 1U : 0U;
  4515. PlsrPersistenceDirty = 0U;
  4516. PlsrPersistenceDelayMs = 0U;
  4517. }
  4518. (void)memset(&PlsrActiveConfig, 0, sizeof(PlsrActiveConfig));
  4519. (void)memset(&PlsrRamp, 0, sizeof(PlsrRamp));
  4520. (void)memset(&PlsrShortProfile, 0, sizeof(PlsrShortProfile));
  4521. (void)memset(PlsrProfileQueues, 0, sizeof(PlsrProfileQueues));
  4522. PlsrProfileQueueBank = 0U;
  4523. (void)memset(&PlsrHandoffPlan, 0, sizeof(PlsrHandoffPlan));
  4524. (void)memset(&PlsrExecutor, 0, sizeof(PlsrExecutor));
  4525. (void)memset(PlsrPreparedHandoffPlans, 0,
  4526. sizeof(PlsrPreparedHandoffPlans));
  4527. PlsrPreparedHandoffBank = 0U;
  4528. PlsrRemainingPulses = 0UL;
  4529. PlsrPulseActive = 0U;
  4530. PlsrCutRequested = 0U;
  4531. PlsrBoundaryPending = 0U;
  4532. PlsrBoundaryWasCut = 0U;
  4533. PlsrCountOverflowPending = 0U;
  4534. PlsrCountedObservedPublished = 0ULL;
  4535. PlsrCountedHandoffStaged = 0U;
  4536. PlsrCountedBoundaryWriteIndex = 0U;
  4537. PlsrCountedBoundaryReadIndex = 0U;
  4538. PlsrCountedBoundaryBusy = 0U;
  4539. (void)memset(PlsrCountedBoundaryEvents, 0,
  4540. sizeof(PlsrCountedBoundaryEvents));
  4541. PlsrCountedFaultEvent = 0U;
  4542. PlsrPositionCheckpointDirty = 0U;
  4543. PlsrFrequencyUpdatePending = 0U;
  4544. PlsrDeferredFrequencyPending = 0U;
  4545. PlsrFrequencyUpdateSegment = 0U;
  4546. PlsrSeamlessHandoffPending = 0U;
  4547. PlsrTimerErrorPending = 0U;
  4548. PlsrCurrentFrequencyHz = 0UL;
  4549. PlsrQueuedFrequencyHz = 0UL;
  4550. PlsrBoundaryFrequencyHz = 0UL;
  4551. PlsrFrequencyUpdateTargetHz = 0UL;
  4552. PlsrDeferredFrequencyHz = 0UL;
  4553. PlsrSegmentEpoch = 0UL;
  4554. PlsrCurrentSegment = 0U;
  4555. PlsrDirectionDelayActive = 0U;
  4556. PlsrDirectionDelayRemainingMs = 0U;
  4557. PlsrSegmentClockStarted = 0U;
  4558. PlsrExtPreviousLevel = 0U;
  4559. PlsrExtEdgePending = 0U;
  4560. PlsrWaitSignalArmedMask = 0U;
  4561. PlsrWaitSignalPendingMask = 0U;
  4562. PlsrStopRequested = 0U;
  4563. PlsrStopPulsesRemaining = 0U;
  4564. PlsrAbStopArmed = 0U;
  4565. PlsrLastDirectionValid = 0U;
  4566. PlsrPositionCheckpointElapsedMs = 0U;
  4567. PlsrCommandMailbox.command = 0U;
  4568. PlsrCommandMailbox.state = PLSR_COMMAND_MAILBOX_EMPTY;
  4569. PlsrDiagnosticReset();
  4570. #ifdef PLSR_HOST_TEST
  4571. PlsrHostTestEventSequence = 0UL;
  4572. PlsrHostTestLastGateEvent = 0UL;
  4573. PlsrHostTestLastDiagnosticFinishEvent = 0UL;
  4574. #endif
  4575. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0)
  4576. PlsrProfileProducerItemCount = 0UL;
  4577. PlsrProfileProducerTotalCycles = 0UL;
  4578. PlsrProfileProducerMaxItemCycles = 0UL;
  4579. #endif
  4580. PlsrError = PLSR_ERROR_NONE;
  4581. PlsrRunStatus = PLSR_STATUS_IDLE;
  4582. PlsrInitialized = 1U;
  4583. return 1U;
  4584. }
  4585. static PLSR_MB_RESULT PlsrQueueCommand(uint16_t command)
  4586. {
  4587. uint32_t criticalState;
  4588. PLSR_MB_RESULT result = PLSR_MB_OK;
  4589. if (PlsrInitialized == 0U)
  4590. {
  4591. return PLSR_MB_SERVER_FAILURE;
  4592. }
  4593. criticalState = PlsrPlatformEnterCritical();
  4594. /* Capacity one: repeats acknowledge the first command; conflicts wait. */
  4595. if (PlsrCommandMailbox.state != PLSR_COMMAND_MAILBOX_EMPTY)
  4596. {
  4597. result = (PlsrCommandMailbox.command == command)
  4598. ? PLSR_MB_OK : PLSR_MB_DEVICE_BUSY;
  4599. PlsrPlatformExitCritical(criticalState);
  4600. return result;
  4601. }
  4602. if (command == PLSR_COMMAND_START)
  4603. {
  4604. if ((PlsrIsBusy() != 0U)
  4605. || (PlsrRunStatus == PLSR_STATUS_ERROR))
  4606. {
  4607. result = PLSR_MB_DEVICE_BUSY;
  4608. }
  4609. else if ((PlsrRunStatus != PLSR_STATUS_IDLE)
  4610. && (PlsrRunStatus != PLSR_STATUS_COMPLETED)
  4611. && (PlsrRunStatus != PLSR_STATUS_STOPPED))
  4612. {
  4613. result = PLSR_MB_ILLEGAL_VALUE;
  4614. }
  4615. else if ((PlsrConfigIsValid(&PlsrShadowConfig, 1U) == 0U)
  4616. || ((PlsrShadowConfig.positionMode
  4617. == PLSR_POSITION_ABSOLUTE)
  4618. && (PlsrPositionValid == 0U)))
  4619. {
  4620. result = PLSR_MB_ILLEGAL_VALUE;
  4621. }
  4622. else
  4623. {
  4624. PlsrCommandMailbox.startConfig = PlsrShadowConfig;
  4625. }
  4626. }
  4627. else if ((command == PLSR_COMMAND_CLEAR) && (PlsrIsBusy() != 0U))
  4628. {
  4629. result = PLSR_MB_DEVICE_BUSY;
  4630. }
  4631. if (result == PLSR_MB_OK)
  4632. {
  4633. PlsrCommandMailbox.command = command;
  4634. PlsrCommandMailbox.state = PLSR_COMMAND_MAILBOX_PENDING;
  4635. }
  4636. PlsrPlatformExitCritical(criticalState);
  4637. return result;
  4638. }
  4639. static void PlsrExecuteStart(void)
  4640. {
  4641. uint32_t criticalState;
  4642. uint8_t handoffBank;
  4643. uint8_t index;
  4644. PlsrActiveConfig = PlsrCommandMailbox.startConfig;
  4645. for (index = 0U; index < PlsrActiveConfig.segmentCount; index++)
  4646. {
  4647. PlsrActiveConfig.segments[index].frequencyHz =
  4648. PlsrResolvedSegmentFrequency(&PlsrActiveConfig, index);
  4649. }
  4650. handoffBank = PlsrBuildHandoffPlanBank(&PlsrActiveConfig);
  4651. criticalState = PlsrPlatformEnterCritical();
  4652. PlsrPreparedHandoffBank = handoffBank;
  4653. PlsrHandoffPlan.valid = 0U;
  4654. PlsrPlatformExitCritical(criticalState);
  4655. PlsrStopRequested = 0U;
  4656. PlsrTimerErrorPending = 0U;
  4657. PlsrShortProfile.active = 0U;
  4658. PlsrError = PLSR_ERROR_NONE;
  4659. PlsrLastDirectionValid = 0U;
  4660. PlsrCheckpointPosition(1U);
  4661. if (PlsrStartSegment((uint8_t)PlsrActiveConfig.startSegment, 0U, 0UL)
  4662. == 0U)
  4663. {
  4664. PlsrEnterError((PlsrTimerErrorPending != 0U)
  4665. ? PLSR_ERROR_TIMER
  4666. : PLSR_ERROR_INVALID_RESOURCE);
  4667. }
  4668. }
  4669. static uint8_t PlsrExecuteStop(void)
  4670. {
  4671. uint32_t criticalState;
  4672. uint32_t stopTargetHz;
  4673. uint32_t appliedHz;
  4674. uint64_t drainPulses;
  4675. if (PlsrIsBusy() == 0U)
  4676. {
  4677. return 0U;
  4678. }
  4679. criticalState = PlsrPlatformEnterCritical();
  4680. if (PlsrStopRequested != 0U)
  4681. {
  4682. PlsrPlatformExitCritical(criticalState);
  4683. return 0U;
  4684. }
  4685. PlsrStopRequested = 1U;
  4686. PlsrStopPulsesRemaining = 0U;
  4687. if (PlsrBoundaryPending != 0U)
  4688. {
  4689. PlsrPlatformExitCritical(criticalState);
  4690. return 1U;
  4691. }
  4692. if (PlsrPulseActive == 0U)
  4693. {
  4694. PlsrPlatformExitCritical(criticalState);
  4695. PlsrFinishStopped();
  4696. return 1U;
  4697. }
  4698. if (PlsrActiveConfig.outputMode != PLSR_OUTPUT_PULSE_DIR)
  4699. {
  4700. PlsrShortProfile.active = 0U;
  4701. PlsrProfileQueueInvalidateGeneration();
  4702. PlsrHandoffPlan.valid = 0U;
  4703. PlsrInvalidateHandoffPlans();
  4704. }
  4705. stopTargetHz = PlsrEffectiveStopFrequency(
  4706. PlsrActiveConfig.segments[PlsrCurrentSegment - 1U].frequencyHz);
  4707. appliedHz = (PlsrQueuedFrequencyHz != 0UL)
  4708. ? PlsrQueuedFrequencyHz : PlsrCurrentFrequencyHz;
  4709. if (stopTargetHz > appliedHz)
  4710. {
  4711. stopTargetHz = appliedHz;
  4712. }
  4713. if (PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR)
  4714. {
  4715. drainPulses = PlsrRampPulseEstimate(appliedHz, stopTargetHz,
  4716. appliedHz) + 1ULL;
  4717. if (drainPulses > PlsrRemainingPulses)
  4718. {
  4719. drainPulses = PlsrRemainingPulses;
  4720. }
  4721. if (drainPulses == 0ULL)
  4722. {
  4723. drainPulses = 1ULL;
  4724. }
  4725. PlsrRemainingPulses = drainPulses;
  4726. PlsrPlatformExitCritical(criticalState);
  4727. if (PlsrReplanPulseDir(stopTargetHz,
  4728. (uint32_t)drainPulses)
  4729. != PLSR_PLATFORM_SERVICE_READY)
  4730. {
  4731. PlsrEnterError(PLSR_ERROR_TIMER);
  4732. return 1U;
  4733. }
  4734. PlsrInvalidateHandoffPlans();
  4735. criticalState = PlsrPlatformEnterCritical();
  4736. PlsrRunStatus = PLSR_STATUS_DECELERATING;
  4737. PlsrPlatformExitCritical(criticalState);
  4738. return 1U;
  4739. }
  4740. PlsrRampStart(PlsrCurrentFrequencyHz, stopTargetHz);
  4741. PlsrRunStatus = PLSR_STATUS_DECELERATING;
  4742. if (PlsrRamp.active == 0U)
  4743. {
  4744. if (PlsrApplyFrequency(stopTargetHz, PlsrSegmentEpoch) == 0U)
  4745. {
  4746. PlsrPlatformExitCritical(criticalState);
  4747. PlsrEnterError(PLSR_ERROR_TIMER);
  4748. return 1U;
  4749. }
  4750. (void)PlsrArmStopDrainLocked();
  4751. }
  4752. PlsrPlatformExitCritical(criticalState);
  4753. return 1U;
  4754. }
  4755. static void PlsrExecuteClear(void)
  4756. {
  4757. uint32_t criticalState;
  4758. PlsrDisarmWaitSignal();
  4759. criticalState = PlsrPlatformEnterCritical();
  4760. PlsrPosition = 0L;
  4761. PlsrPositionValid = 1U;
  4762. PlsrRemainingPulses = 0UL;
  4763. PlsrPlatformExitCritical(criticalState);
  4764. PlsrPlatformCheckpointPosition(0L, 1U, 0U);
  4765. PlsrCountOverflowPending = 0U;
  4766. PlsrPositionCheckpointDirty = 0U;
  4767. PlsrPositionCheckpointElapsedMs = 0U;
  4768. PlsrCurrentSegment = 0U;
  4769. PlsrCurrentFrequencyHz = 0UL;
  4770. PlsrQueuedFrequencyHz = 0UL;
  4771. PlsrCutRequested = 0U;
  4772. PlsrBoundaryPending = 0U;
  4773. PlsrBoundaryWasCut = 0U;
  4774. PlsrDirectionDelayActive = 0U;
  4775. PlsrDirectionDelayRemainingMs = 0U;
  4776. PlsrExtEdgePending = 0U;
  4777. PlsrShortProfile.active = 0U;
  4778. PlsrProfileQueueReset();
  4779. PlsrHandoffPlan.valid = 0U;
  4780. PlsrInvalidateHandoffPlans();
  4781. PlsrDeferredFrequencyPending = 0U;
  4782. PlsrTimerErrorPending = 0U;
  4783. PlsrRamp.active = 0U;
  4784. PlsrExecutor.mode = PLSR_EXEC_IDLE;
  4785. PlsrError = PLSR_ERROR_NONE;
  4786. PlsrRunStatus = PLSR_STATUS_IDLE;
  4787. PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS);
  4788. }
  4789. static uint8_t PlsrPollCommandMailbox(void)
  4790. {
  4791. uint16_t command;
  4792. uint8_t endPoll = 1U;
  4793. uint32_t criticalState;
  4794. criticalState = PlsrPlatformEnterCritical();
  4795. if (PlsrCommandMailbox.state != PLSR_COMMAND_MAILBOX_PENDING)
  4796. {
  4797. PlsrPlatformExitCritical(criticalState);
  4798. return 0U;
  4799. }
  4800. PlsrCommandMailbox.state = PLSR_COMMAND_MAILBOX_EXECUTING;
  4801. command = PlsrCommandMailbox.command;
  4802. PlsrPlatformExitCritical(criticalState);
  4803. switch (command)
  4804. {
  4805. case PLSR_COMMAND_START: PlsrExecuteStart(); break;
  4806. case PLSR_COMMAND_STOP: endPoll = PlsrExecuteStop(); break;
  4807. case PLSR_COMMAND_CLEAR: PlsrExecuteClear(); break;
  4808. default: PlsrEnterError(PLSR_ERROR_INTERNAL); break;
  4809. }
  4810. criticalState = PlsrPlatformEnterCritical();
  4811. PlsrCommandMailbox.command = 0U;
  4812. PlsrCommandMailbox.state = PLSR_COMMAND_MAILBOX_EMPTY;
  4813. PlsrPlatformExitCritical(criticalState);
  4814. return endPoll;
  4815. }
  4816. static void PlsrEnterErrorIfEpoch(PLSR_ERROR error, uint32_t expectedEpoch)
  4817. {
  4818. uint32_t criticalState = PlsrPlatformEnterCritical();
  4819. if (PlsrSegmentEpoch == expectedEpoch)
  4820. {
  4821. PlsrEnterError(error);
  4822. }
  4823. PlsrPlatformExitCritical(criticalState);
  4824. }
  4825. static uint8_t PlsrServiceCountedExecutor(void)
  4826. {
  4827. uint32_t completedPulses;
  4828. uint32_t completedFrequencyHz;
  4829. uint32_t activeFrequencyHz;
  4830. uint32_t queuedFrequencyHz;
  4831. uint8_t completedNormally;
  4832. if ((PlsrPulseActive == 0U)
  4833. || ((PlsrExecutor.mode != PLSR_EXEC_STEP_TABLE)
  4834. && (PlsrExecutor.mode != PLSR_EXEC_STREAM)
  4835. && (PlsrExecutor.mode != PLSR_EXEC_STOPPING)))
  4836. {
  4837. return 0U;
  4838. }
  4839. if (PlsrSyncCountedProgress() == 0U)
  4840. {
  4841. #if PLSR_COUNT_ERROR_CHECK_ENABLED
  4842. PlsrEnterError(PLSR_ERROR_COUNT);
  4843. return 1U;
  4844. #else
  4845. /* 计数错误检查暂时关闭:observed 回退(平台计数器基准重置造成的
  4846. 视图回退)时重新校准发布点,继续运行,不进错误。 */
  4847. PlsrCountedObservedPublished = PlsrPlatformObservedPulses(
  4848. (uint8_t)PlsrActiveConfig.pulseOutput);
  4849. return 0U;
  4850. #endif
  4851. }
  4852. activeFrequencyHz = PlsrPlatformActiveFrequency(
  4853. (uint8_t)PlsrActiveConfig.pulseOutput);
  4854. if (activeFrequencyHz != 0UL)
  4855. {
  4856. PlsrCurrentFrequencyHz = activeFrequencyHz;
  4857. PlsrDiagnosticCheckActiveFrequency(activeFrequencyHz);
  4858. queuedFrequencyHz = PlsrQueuedFrequencyHz;
  4859. if (queuedFrequencyHz > activeFrequencyHz)
  4860. {
  4861. PlsrRunStatus = PLSR_STATUS_ACCELERATING;
  4862. }
  4863. else if (queuedFrequencyHz < activeFrequencyHz)
  4864. {
  4865. PlsrRunStatus = PLSR_STATUS_DECELERATING;
  4866. }
  4867. else
  4868. {
  4869. PlsrRunStatus = PLSR_STATUS_RUNNING;
  4870. }
  4871. }
  4872. if (PlsrPlatformTakeFiniteCompletion(
  4873. (uint8_t)PlsrActiveConfig.pulseOutput,
  4874. &completedPulses) == 0U)
  4875. {
  4876. return 0U;
  4877. }
  4878. (void)completedPulses;
  4879. (void)PlsrSyncCountedProgress();
  4880. completedFrequencyHz = PlsrCurrentFrequencyHz;
  4881. completedNormally = ((PlsrRemainingPulses == 0ULL)
  4882. && (PlsrCutRequested == 0U)
  4883. && (PlsrCountOverflowPending == 0U)
  4884. && (PlsrTimerErrorPending == 0U)) ? 1U : 0U;
  4885. PlsrDiagnosticFinishSegment(completedNormally);
  4886. PlsrShortProfile.active = 0U;
  4887. PlsrProfileQueue.active = 0U;
  4888. PlsrHandoffPlan.valid = 0U;
  4889. PlsrCountedHandoffStaged = 0U;
  4890. PlsrBoundaryFrequencyHz = completedFrequencyHz;
  4891. PlsrBoundaryWasCut = (PlsrCutRequested != 0U) ? 1U : 0U;
  4892. PlsrCutRequested = 0U;
  4893. PlsrPulseActive = 0U;
  4894. PlsrCurrentFrequencyHz = 0UL;
  4895. PlsrQueuedFrequencyHz = 0UL;
  4896. PlsrBoundaryPending = 1U;
  4897. PlsrExecutor.mode = PLSR_EXEC_IDLE;
  4898. return 1U;
  4899. }
  4900. void PlsrPoll1ms(void)
  4901. {
  4902. uint8_t extLevel;
  4903. uint8_t extEdge;
  4904. uint8_t activeSegmentNumber;
  4905. uint8_t applyDynamicFrequency = 0U;
  4906. uint8_t pulseDirReplanRequested = 0U;
  4907. uint8_t pulseDirReplanFailed = 0U;
  4908. PLSR_SEGMENT_CONFIG *activeSegment;
  4909. uint32_t criticalState;
  4910. uint32_t newTargetHz;
  4911. uint32_t pulseDirReplanPulses = 0UL;
  4912. uint32_t pollEpoch;
  4913. uint16_t fillBudget = PLSR_PROFILE_REFILL_BUDGET;
  4914. PLSR_PLATFORM_SERVICE_RESULT pulseDirReplanResult =
  4915. PLSR_PLATFORM_SERVICE_READY;
  4916. if (PlsrInitialized == 0U)
  4917. {
  4918. return;
  4919. }
  4920. if (PlsrPollCommandMailbox() != 0U)
  4921. {
  4922. return;
  4923. }
  4924. /* 段边界事件必须在任何计数同步之前消费:快照是 IRQ 边界时刻的
  4925. 硬件计数,晚消费会把下一 run 的脉冲算进旧段。 */
  4926. PlsrExecServiceCountedEvents();
  4927. if (PlsrTimerErrorPending != 0U)
  4928. {
  4929. PlsrEnterError(PLSR_ERROR_TIMER);
  4930. return;
  4931. }
  4932. PlsrPollPositionCheckpoint();
  4933. if (PlsrServiceCountedExecutor() != 0U)
  4934. {
  4935. if (PlsrBoundaryPending != 0U)
  4936. {
  4937. PlsrHandleBoundary(0U);
  4938. }
  4939. PlsrPollPersistenceDelay();
  4940. return;
  4941. }
  4942. if (PlsrProfileQueueCount() <= PLSR_PROFILE_LOW_WATER)
  4943. {
  4944. fillBudget = PLSR_PROFILE_STARTUP_BUDGET;
  4945. }
  4946. if (((PlsrExecutor.mode == PLSR_EXEC_STEP_TABLE)
  4947. || (PlsrExecutor.mode == PLSR_EXEC_STREAM)
  4948. || (PlsrExecutor.mode == PLSR_EXEC_AB_LEGACY))
  4949. && (PlsrProfileQueue.active != 0U)
  4950. && (PlsrProfileQueue.generatorComplete == 0U)
  4951. && (PlsrProfileQueueFill(PLSR_PROFILE_REFILL_TARGET,
  4952. &fillBudget) == 0U))
  4953. {
  4954. PlsrEnterError(PLSR_ERROR_TIMER);
  4955. return;
  4956. }
  4957. if (((PlsrExecutor.mode == PLSR_EXEC_STEP_TABLE)
  4958. || (PlsrExecutor.mode == PLSR_EXEC_STREAM))
  4959. && ((PlsrStageCountedHandoff() == 0U)
  4960. || (PlsrProfileQueueFill(PLSR_PROFILE_REFILL_TARGET,
  4961. &fillBudget) == 0U)))
  4962. {
  4963. PlsrEnterError(PLSR_ERROR_TIMER);
  4964. return;
  4965. }
  4966. /* This work is bounded and targets only the inactive bank. It is done
  4967. after servicing the live queue so preparation cannot starve output. */
  4968. PlsrServiceTimedStartPreparation();
  4969. criticalState = PlsrPlatformEnterCritical();
  4970. pollEpoch = PlsrSegmentEpoch;
  4971. extLevel = PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.extInput);
  4972. extEdge = ((extLevel != 0U) && (PlsrExtPreviousLevel == 0U)) ? 1U : 0U;
  4973. PlsrExtPreviousLevel = extLevel;
  4974. if (PlsrExtEdgePending != 0U)
  4975. {
  4976. extEdge = 1U;
  4977. }
  4978. PlsrPlatformExitCritical(criticalState);
  4979. if (PlsrBoundaryPending != 0U)
  4980. {
  4981. PlsrExtEdgePending = 0U;
  4982. PlsrHandleBoundary(extEdge);
  4983. PlsrPollPersistenceDelay();
  4984. return;
  4985. }
  4986. if (PlsrIsBusy() == 0U)
  4987. {
  4988. PlsrExtEdgePending = 0U;
  4989. PlsrPollPersistenceDelay();
  4990. return;
  4991. }
  4992. if (PlsrSegmentEpoch != pollEpoch)
  4993. {
  4994. return;
  4995. }
  4996. if (PlsrDirectionDelayActive != 0U)
  4997. {
  4998. if (extEdge != 0U)
  4999. {
  5000. PlsrExtEdgePending = 1U;
  5001. }
  5002. if (PlsrDirectionDelayRemainingMs != 0U)
  5003. {
  5004. PlsrDirectionDelayRemainingMs--;
  5005. }
  5006. if (PlsrDirectionDelayRemainingMs == 0U)
  5007. {
  5008. PlsrDirectionDelayActive = 0U;
  5009. if (PlsrTimedStart.pendingActivation != 0U)
  5010. {
  5011. if (PlsrStartPreparedTimedOutput() == 0U)
  5012. {
  5013. PlsrEnterError(PLSR_ERROR_TIMER);
  5014. }
  5015. }
  5016. else if (PlsrBeginSegmentOutput(PlsrEffectiveStartFrequency(
  5017. PlsrActiveConfig.segments[PlsrCurrentSegment - 1U]
  5018. .frequencyHz,
  5019. 0U, 1U, 0UL)) == 0U)
  5020. {
  5021. PlsrEnterError(PLSR_ERROR_TIMER);
  5022. }
  5023. }
  5024. return;
  5025. }
  5026. criticalState = PlsrPlatformEnterCritical();
  5027. if (PlsrSegmentEpoch != pollEpoch)
  5028. {
  5029. PlsrPlatformExitCritical(criticalState);
  5030. return;
  5031. }
  5032. PlsrExtEdgePending = 0U;
  5033. if (PlsrSegmentClockStarted != 0U)
  5034. {
  5035. PlsrSegmentElapsedMs++;
  5036. }
  5037. if (PlsrRunStatus == PLSR_STATUS_WAITING)
  5038. {
  5039. PlsrPlatformExitCritical(criticalState);
  5040. PlsrPollWaiting(extEdge);
  5041. return;
  5042. }
  5043. activeSegmentNumber = PlsrCurrentSegment;
  5044. if ((activeSegmentNumber == 0U)
  5045. || (activeSegmentNumber > PlsrActiveConfig.segmentCount))
  5046. {
  5047. PlsrPlatformExitCritical(criticalState);
  5048. PlsrEnterError(PLSR_ERROR_INTERNAL);
  5049. return;
  5050. }
  5051. activeSegment = &PlsrActiveConfig.segments[activeSegmentNumber - 1U];
  5052. if (PlsrSeamlessHandoffPending != 0U)
  5053. {
  5054. PlsrSeamlessHandoffPending = 0U;
  5055. if ((PlsrStopRequested == 0U)
  5056. && (PlsrShortProfile.active == 0U)
  5057. && (PlsrFrequencyUpdatePending == 0U))
  5058. {
  5059. PlsrRamp.active = 0U;
  5060. PlsrRunStatus = PLSR_STATUS_RUNNING;
  5061. }
  5062. }
  5063. newTargetHz = PlsrFrequencyUpdateTargetHz;
  5064. if ((PlsrStopRequested == 0U)
  5065. && (PlsrFrequencyUpdatePending != 0U)
  5066. && (PlsrAbStopArmed == 0U)
  5067. && (PlsrFrequencyUpdateSegment == activeSegmentNumber))
  5068. {
  5069. activeSegment->frequencyHz = newTargetHz;
  5070. PlsrBoundaryRampStarted = 0U;
  5071. if (PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR)
  5072. {
  5073. uint64_t remaining = PlsrRemainingPulses;
  5074. if (PlsrCountedHandoffStaged != 0U)
  5075. {
  5076. /* The marker and its metadata are one published transaction.
  5077. Leave both untouched; the boundary consumer retargets this
  5078. pending update to the segment that hardware enters. */
  5079. }
  5080. else if ((remaining == 0ULL) || (remaining > 0xFFFFFFFFULL))
  5081. {
  5082. PlsrFrequencyUpdatePending = 0U;
  5083. pulseDirReplanFailed = 1U;
  5084. }
  5085. else
  5086. {
  5087. PlsrFrequencyUpdatePending = 0U;
  5088. pulseDirReplanPulses = (uint32_t)remaining;
  5089. pulseDirReplanRequested = 1U;
  5090. }
  5091. }
  5092. else
  5093. {
  5094. PlsrFrequencyUpdatePending = 0U;
  5095. PlsrShortProfile.active = 0U;
  5096. PlsrProfileQueueInvalidateGeneration();
  5097. PlsrRampStart(PlsrCurrentFrequencyHz, newTargetHz);
  5098. if (PlsrRamp.active == 0U)
  5099. {
  5100. applyDynamicFrequency = 1U;
  5101. }
  5102. }
  5103. }
  5104. PlsrPlatformExitCritical(criticalState);
  5105. if (pulseDirReplanRequested != 0U)
  5106. {
  5107. pulseDirReplanResult = PlsrReplanPulseDir(
  5108. newTargetHz, pulseDirReplanPulses);
  5109. if (pulseDirReplanResult == PLSR_PLATFORM_SERVICE_FAILED)
  5110. {
  5111. pulseDirReplanFailed = 1U;
  5112. }
  5113. else if (pulseDirReplanResult == PLSR_PLATFORM_SERVICE_DEFERRED)
  5114. {
  5115. /* A segment-marker run may already be latched. Keep the old
  5116. published stream intact and retry against whichever segment is
  5117. active after the boundary event is settled. */
  5118. criticalState = PlsrPlatformEnterCritical();
  5119. if ((PlsrSegmentEpoch == pollEpoch)
  5120. && (PlsrStopRequested == 0U))
  5121. {
  5122. PlsrFrequencyUpdateTargetHz = newTargetHz;
  5123. PlsrFrequencyUpdateSegment = activeSegmentNumber;
  5124. PlsrFrequencyUpdatePending = 1U;
  5125. }
  5126. PlsrPlatformExitCritical(criticalState);
  5127. }
  5128. }
  5129. if (pulseDirReplanFailed != 0U)
  5130. {
  5131. PlsrEnterErrorIfEpoch(PLSR_ERROR_TIMER, pollEpoch);
  5132. return;
  5133. }
  5134. if (applyDynamicFrequency != 0U)
  5135. {
  5136. if (PlsrApplyFrequency(newTargetHz, pollEpoch) == 0U)
  5137. {
  5138. PlsrEnterErrorIfEpoch(PLSR_ERROR_TIMER, pollEpoch);
  5139. return;
  5140. }
  5141. criticalState = PlsrPlatformEnterCritical();
  5142. if (PlsrSegmentEpoch == pollEpoch)
  5143. {
  5144. PlsrRunStatus = PLSR_STATUS_RUNNING;
  5145. }
  5146. PlsrPlatformExitCritical(criticalState);
  5147. }
  5148. if ((PlsrActiveConfig.outputMode == PLSR_OUTPUT_AB)
  5149. && (PlsrShortProfile.active == 0U)
  5150. && (PlsrRamp.active != 0U)
  5151. && (PlsrRampAdvance(pollEpoch) == 0U))
  5152. {
  5153. PlsrEnterErrorIfEpoch(PLSR_ERROR_TIMER, pollEpoch);
  5154. return;
  5155. }
  5156. criticalState = PlsrPlatformEnterCritical();
  5157. if (PlsrSegmentEpoch != pollEpoch)
  5158. {
  5159. PlsrPlatformExitCritical(criticalState);
  5160. return;
  5161. }
  5162. if ((PlsrActiveConfig.outputMode == PLSR_OUTPUT_AB)
  5163. && (PlsrStopRequested != 0U) && (PlsrRamp.active == 0U))
  5164. {
  5165. if (PlsrStopPulsesRemaining == 0U)
  5166. {
  5167. if (PlsrArmStopDrainLocked() == 0U)
  5168. {
  5169. PlsrPlatformExitCritical(criticalState);
  5170. PlsrEnterError(PLSR_ERROR_TIMER);
  5171. return;
  5172. }
  5173. }
  5174. else
  5175. {
  5176. (void)PlsrArmFinalAbBoundaryLocked();
  5177. }
  5178. PlsrPlatformExitCritical(criticalState);
  5179. return;
  5180. }
  5181. PlsrPlatformExitCritical(criticalState);
  5182. if ((activeSegment->waitType == PLSR_ACT_TIME)
  5183. && (PlsrSegmentElapsedMs >= activeSegment->actTimeMs))
  5184. {
  5185. PlsrRequestCut(pollEpoch);
  5186. return;
  5187. }
  5188. if (((activeSegment->waitType == PLSR_EXT_SIGNAL)
  5189. || (activeSegment->waitType == PLSR_EXT_OR_COMPLETE))
  5190. && (extEdge != 0U))
  5191. {
  5192. PlsrRequestCut(pollEpoch);
  5193. return;
  5194. }
  5195. if (PlsrActiveConfig.outputMode == PLSR_OUTPUT_AB)
  5196. {
  5197. PlsrMaybePlanBoundaryRamp(pollEpoch);
  5198. }
  5199. }
  5200. void PlsrPulseTimerIrq(uint8_t pulseOutput)
  5201. {
  5202. uint32_t positionBits;
  5203. uint32_t completedFrequencyHz;
  5204. uint64_t remainingPulses;
  5205. PLSR_PLATFORM_QUEUE_RESULT queueResult;
  5206. if ((PlsrPulseActive == 0U)
  5207. || (pulseOutput != (uint8_t)PlsrActiveConfig.pulseOutput))
  5208. {
  5209. return;
  5210. }
  5211. if ((PlsrExecutor.mode == PLSR_EXEC_STEP_TABLE)
  5212. || (PlsrExecutor.mode == PLSR_EXEC_STREAM)
  5213. || (PlsrExecutor.mode == PLSR_EXEC_STOPPING))
  5214. {
  5215. return;
  5216. }
  5217. completedFrequencyHz = PlsrCurrentFrequencyHz;
  5218. PlsrDiagnosticCheckActiveFrequency(completedFrequencyHz);
  5219. PlsrCurrentFrequencyHz = PlsrPlatformActiveFrequency(pulseOutput);
  5220. if (PlsrCurrentFrequencyHz == 0UL)
  5221. {
  5222. PlsrCurrentFrequencyHz = PlsrQueuedFrequencyHz;
  5223. }
  5224. if ((PlsrStopRequested != 0U) && (PlsrRamp.active == 0U)
  5225. && (PlsrStopPulsesRemaining != 0U))
  5226. {
  5227. PlsrStopPulsesRemaining--;
  5228. if (PlsrStopPulsesRemaining == 0U)
  5229. {
  5230. PlsrCutRequested = 1U;
  5231. }
  5232. }
  5233. positionBits = (uint32_t)PlsrPosition;
  5234. if (PlsrCountPositive != 0U)
  5235. {
  5236. if (positionBits == (uint32_t)INT32_MAX)
  5237. {
  5238. PlsrPositionValid = 0U;
  5239. #if PLSR_COUNT_ERROR_CHECK_ENABLED
  5240. PlsrCountOverflowPending = 1U;
  5241. #endif
  5242. }
  5243. positionBits++;
  5244. }
  5245. else
  5246. {
  5247. if (positionBits == (uint32_t)INT32_MIN)
  5248. {
  5249. PlsrPositionValid = 0U;
  5250. #if PLSR_COUNT_ERROR_CHECK_ENABLED
  5251. PlsrCountOverflowPending = 1U;
  5252. #endif
  5253. }
  5254. positionBits--;
  5255. }
  5256. PlsrPosition = (int32_t)positionBits;
  5257. PlsrPositionCheckpointDirty = 1U;
  5258. remainingPulses = PlsrRemainingPulses;
  5259. if (remainingPulses != 0UL)
  5260. {
  5261. remainingPulses--;
  5262. PlsrRemainingPulses = remainingPulses;
  5263. }
  5264. if ((remainingPulses == 0UL) || (PlsrCutRequested != 0U)
  5265. || (PlsrCountOverflowPending != 0U))
  5266. {
  5267. if (remainingPulses == 0UL)
  5268. {
  5269. queueResult = PlsrTryContinuousHandoff();
  5270. if (queueResult != PLSR_PLATFORM_QUEUE_FAILED)
  5271. {
  5272. return;
  5273. }
  5274. }
  5275. PlsrPlatformGateFromIrq(pulseOutput);
  5276. #ifdef PLSR_HOST_TEST
  5277. PlsrHostTestEventSequence++;
  5278. PlsrHostTestLastGateEvent = PlsrHostTestEventSequence;
  5279. #endif
  5280. PlsrDiagnosticFinishSegment(
  5281. ((remainingPulses == 0UL)
  5282. && (PlsrCutRequested == 0U)
  5283. && (PlsrCountOverflowPending == 0U)) ? 1U : 0U);
  5284. PlsrShortProfile.active = 0U;
  5285. PlsrHandoffPlan.valid = 0U;
  5286. PlsrBoundaryFrequencyHz = completedFrequencyHz;
  5287. PlsrBoundaryWasCut = (PlsrCutRequested != 0U) ? 1U : 0U;
  5288. PlsrCutRequested = 0U;
  5289. PlsrAbStopArmed = 0U;
  5290. PlsrPulseActive = 0U;
  5291. PlsrCurrentFrequencyHz = 0UL;
  5292. PlsrQueuedFrequencyHz = 0UL;
  5293. PlsrBoundaryPending = 1U;
  5294. return;
  5295. }
  5296. if (PlsrShortProfile.active != 0U)
  5297. {
  5298. queueResult = PlsrProfileQueueCommitNext(pulseOutput);
  5299. if (queueResult == PLSR_PLATFORM_QUEUE_STALE)
  5300. {
  5301. return;
  5302. }
  5303. if (queueResult == PLSR_PLATFORM_QUEUE_FAILED)
  5304. {
  5305. PlsrDiagnosticFinishSegment(0U);
  5306. PlsrTimerErrorPending = 1U;
  5307. PlsrShortProfile.active = 0U;
  5308. PlsrHandoffPlan.valid = 0U;
  5309. PlsrBoundaryFrequencyHz = completedFrequencyHz;
  5310. PlsrBoundaryWasCut = 0U;
  5311. PlsrCutRequested = 0U;
  5312. PlsrPlatformGateFromIrq(pulseOutput);
  5313. PlsrAbStopArmed = 0U;
  5314. PlsrPulseActive = 0U;
  5315. PlsrCurrentFrequencyHz = 0UL;
  5316. PlsrQueuedFrequencyHz = 0UL;
  5317. PlsrBoundaryPending = 1U;
  5318. return;
  5319. }
  5320. }
  5321. if ((PlsrShortProfile.active == 0U)
  5322. && (remainingPulses == 1UL))
  5323. {
  5324. if (PlsrPrimeHandoff() == PLSR_PLATFORM_QUEUE_STALE)
  5325. {
  5326. return;
  5327. }
  5328. }
  5329. if ((PlsrShortProfile.active == 0U)
  5330. && (PlsrHandoffPlan.valid == 0U)
  5331. && (PlsrDeferredFrequencyPending != 0U))
  5332. {
  5333. queueResult = PlsrCommitDeferredFrequency(pulseOutput);
  5334. if (queueResult == PLSR_PLATFORM_QUEUE_STALE)
  5335. {
  5336. return;
  5337. }
  5338. if (queueResult == PLSR_PLATFORM_QUEUE_FAILED)
  5339. {
  5340. PlsrDiagnosticFinishSegment(0U);
  5341. PlsrTimerErrorPending = 1U;
  5342. PlsrBoundaryFrequencyHz = completedFrequencyHz;
  5343. PlsrBoundaryWasCut = 0U;
  5344. PlsrCutRequested = 0U;
  5345. PlsrPlatformGateFromIrq(pulseOutput);
  5346. PlsrAbStopArmed = 0U;
  5347. PlsrPulseActive = 0U;
  5348. PlsrCurrentFrequencyHz = 0UL;
  5349. PlsrQueuedFrequencyHz = 0UL;
  5350. PlsrBoundaryPending = 1U;
  5351. return;
  5352. }
  5353. }
  5354. if ((PlsrActiveConfig.outputMode == PLSR_OUTPUT_AB)
  5355. && (PlsrQueueFinalAbBoundaryFromIrq() == 0U))
  5356. {
  5357. return;
  5358. }
  5359. }
  5360. PLSR_MB_RESULT PlsrModbusReadHolding(uint16_t startAddress,
  5361. uint16_t quantity,
  5362. uint16_t *values)
  5363. {
  5364. PLSR_MB_RESULT classification;
  5365. uint16_t index;
  5366. uint32_t criticalState;
  5367. int32_t position;
  5368. uint32_t frequency;
  5369. uint16_t statusWords[7];
  5370. uint16_t diagnosticWords[26];
  5371. uint16_t queueDiagnosticWords[9];
  5372. if (values == NULL)
  5373. {
  5374. return PLSR_MB_ILLEGAL_VALUE;
  5375. }
  5376. classification = PlsrClassifyRange(startAddress, quantity);
  5377. if (classification != PLSR_MB_OK)
  5378. {
  5379. return classification;
  5380. }
  5381. criticalState = PlsrPlatformEnterCritical();
  5382. position = PlsrPosition;
  5383. frequency = PlsrCurrentFrequencyHz;
  5384. statusWords[0] = PlsrLowWord((uint32_t)position);
  5385. statusWords[1] = PlsrHighWord((uint32_t)position);
  5386. statusWords[2] = PlsrLowWord(frequency);
  5387. statusWords[3] = PlsrHighWord(frequency);
  5388. statusWords[4] = (uint16_t)PlsrRunStatus;
  5389. statusWords[5] = PlsrCurrentSegment;
  5390. statusWords[6] = (uint16_t)PlsrError;
  5391. diagnosticWords[0] = PlsrDiagnostic.flags;
  5392. diagnosticWords[1] = PlsrDiagnostic.reason;
  5393. diagnosticWords[2] = PlsrDiagnostic.lastSegment;
  5394. diagnosticWords[3] = PlsrDiagnostic.modeDirection;
  5395. diagnosticWords[4] = PlsrLowWord(PlsrDiagnostic.expectedCycles);
  5396. diagnosticWords[5] = PlsrHighWord(PlsrDiagnostic.expectedCycles);
  5397. diagnosticWords[6] = PlsrLowWord(PlsrDiagnostic.observedCycles);
  5398. diagnosticWords[7] = PlsrHighWord(PlsrDiagnostic.observedCycles);
  5399. diagnosticWords[8] = PlsrLowWord((uint32_t)PlsrDiagnostic.countError);
  5400. diagnosticWords[9] = PlsrHighWord((uint32_t)PlsrDiagnostic.countError);
  5401. diagnosticWords[10] = PlsrLowWord(PlsrDiagnostic.requestedHz);
  5402. diagnosticWords[11] = PlsrHighWord(PlsrDiagnostic.requestedHz);
  5403. diagnosticWords[12] = PlsrLowWord(PlsrDiagnostic.expectedTimerHz);
  5404. diagnosticWords[13] = PlsrHighWord(PlsrDiagnostic.expectedTimerHz);
  5405. diagnosticWords[14] = PlsrLowWord(PlsrDiagnostic.activeTimerHz);
  5406. diagnosticWords[15] = PlsrHighWord(PlsrDiagnostic.activeTimerHz);
  5407. diagnosticWords[16] =
  5408. PlsrLowWord((uint32_t)PlsrDiagnostic.requestErrorHz);
  5409. diagnosticWords[17] =
  5410. PlsrHighWord((uint32_t)PlsrDiagnostic.requestErrorHz);
  5411. diagnosticWords[18] = PlsrLowWord(PlsrDiagnostic.curveSampleCount);
  5412. diagnosticWords[19] = PlsrHighWord(PlsrDiagnostic.curveSampleCount);
  5413. diagnosticWords[20] = PlsrLowWord(PlsrDiagnostic.curveMismatchCount);
  5414. diagnosticWords[21] = PlsrHighWord(PlsrDiagnostic.curveMismatchCount);
  5415. diagnosticWords[22] = PlsrLowWord(PlsrDiagnostic.curveMaxAbsErrorHz);
  5416. diagnosticWords[23] = PlsrHighWord(PlsrDiagnostic.curveMaxAbsErrorHz);
  5417. diagnosticWords[24] = PlsrLowWord(PlsrDiagnostic.firstMismatchSample);
  5418. diagnosticWords[25] = PlsrHighWord(PlsrDiagnostic.firstMismatchSample);
  5419. queueDiagnosticWords[0] = PlsrProfileQueueDiagnosticFlags;
  5420. queueDiagnosticWords[1] = PlsrProfileQueueMinimumDepth;
  5421. queueDiagnosticWords[2] = PlsrProfileQueueCount();
  5422. queueDiagnosticWords[3] = PlsrLowWord(PlsrProfileQueueLowWaterEvents);
  5423. queueDiagnosticWords[4] = PlsrHighWord(PlsrProfileQueueLowWaterEvents);
  5424. queueDiagnosticWords[5] = PlsrLowWord(PlsrProfileQueueUnderruns);
  5425. queueDiagnosticWords[6] = PlsrHighWord(PlsrProfileQueueUnderruns);
  5426. queueDiagnosticWords[7] = PlsrLowWord(PlsrPlannerClippedCount);
  5427. queueDiagnosticWords[8] = PlsrHighWord(PlsrPlannerClippedCount);
  5428. PlsrPlatformExitCritical(criticalState);
  5429. for (index = 0U; index < quantity; index++)
  5430. {
  5431. uint16_t address = (uint16_t)(startAddress + index);
  5432. if ((address >= PLSR_CONFIG_FIRST_ADDRESS)
  5433. && (address <= PLSR_CONFIG_LAST_ADDRESS))
  5434. {
  5435. values[index] = PlsrReadConfigWord(&PlsrShadowConfig, address);
  5436. }
  5437. else if (address == PLSR_OUTPUT_MODE_ADDRESS)
  5438. {
  5439. values[index] = PlsrShadowConfig.outputMode;
  5440. }
  5441. else if ((address >= PLSR_STATUS_FIRST_ADDRESS)
  5442. && (address <= PLSR_STATUS_LAST_ADDRESS))
  5443. {
  5444. values[index] = statusWords[address - PLSR_STATUS_FIRST_ADDRESS];
  5445. }
  5446. else if ((address >= PLSR_DIAGNOSTIC_FIRST_ADDRESS)
  5447. && (address <= PLSR_DIAGNOSTIC_LAST_ADDRESS))
  5448. {
  5449. values[index] =
  5450. diagnosticWords[address - PLSR_DIAGNOSTIC_FIRST_ADDRESS];
  5451. }
  5452. else if ((address >= PLSR_QUEUE_DIAGNOSTIC_FIRST_ADDRESS)
  5453. && (address <= PLSR_QUEUE_DIAGNOSTIC_LAST_ADDRESS))
  5454. {
  5455. values[index] = queueDiagnosticWords[
  5456. address - PLSR_QUEUE_DIAGNOSTIC_FIRST_ADDRESS];
  5457. }
  5458. else if (address == PLSR_CONTROL_ADDRESS)
  5459. {
  5460. values[index] = 0U;
  5461. }
  5462. else if (address == PLSR_DIAGNOSTIC_CONTROL_ADDRESS)
  5463. {
  5464. values[index] = 0U;
  5465. }
  5466. else
  5467. {
  5468. return PLSR_MB_ILLEGAL_ADDRESS;
  5469. }
  5470. }
  5471. return PLSR_MB_OK;
  5472. }
  5473. PLSR_MB_RESULT PlsrModbusWriteHolding(uint16_t startAddress,
  5474. uint16_t quantity,
  5475. const uint16_t *values)
  5476. {
  5477. PLSR_MB_RESULT classification;
  5478. PLSR_WORD_RESULT wordResult;
  5479. uint16_t index;
  5480. uint16_t pairedAddress;
  5481. uint32_t requestEnd;
  5482. uint32_t criticalState;
  5483. uint8_t handoffBank = 0U;
  5484. uint8_t updateActiveFrequencies = 0U;
  5485. uint8_t drainedToDifferentSegment = 0U;
  5486. uint8_t segmentBeforeDrain;
  5487. uint32_t drainedSegmentTargetHz = 0UL;
  5488. PLSR_PLATFORM_QUEUE_RESULT queueResult;
  5489. if (values == NULL)
  5490. {
  5491. return PLSR_MB_ILLEGAL_VALUE;
  5492. }
  5493. classification = PlsrClassifyRange(startAddress, quantity);
  5494. if (classification != PLSR_MB_OK)
  5495. {
  5496. return classification;
  5497. }
  5498. if ((startAddress >= PLSR_STATUS_FIRST_ADDRESS)
  5499. && (startAddress <= PLSR_STATUS_LAST_ADDRESS))
  5500. {
  5501. return PLSR_MB_ILLEGAL_ADDRESS;
  5502. }
  5503. if ((startAddress >= PLSR_DIAGNOSTIC_FIRST_ADDRESS)
  5504. && (startAddress <= PLSR_DIAGNOSTIC_LAST_ADDRESS))
  5505. {
  5506. return PLSR_MB_ILLEGAL_ADDRESS;
  5507. }
  5508. if ((startAddress >= PLSR_QUEUE_DIAGNOSTIC_FIRST_ADDRESS)
  5509. && (startAddress <= PLSR_QUEUE_DIAGNOSTIC_LAST_ADDRESS))
  5510. {
  5511. return PLSR_MB_ILLEGAL_ADDRESS;
  5512. }
  5513. if (startAddress == PLSR_DIAGNOSTIC_CONTROL_ADDRESS)
  5514. {
  5515. if (quantity != 1U)
  5516. {
  5517. return PLSR_MB_ILLEGAL_ADDRESS;
  5518. }
  5519. if ((values[0] != 0U) && (values[0] != 1U))
  5520. {
  5521. return PLSR_MB_ILLEGAL_VALUE;
  5522. }
  5523. if ((values[0] != 0U) && (PlsrIsBusy() != 0U))
  5524. {
  5525. return PLSR_MB_DEVICE_BUSY;
  5526. }
  5527. if (values[0] != 0U)
  5528. {
  5529. criticalState = PlsrPlatformEnterCritical();
  5530. PlsrDiagnosticReset();
  5531. PlsrPlatformExitCritical(criticalState);
  5532. }
  5533. return PLSR_MB_OK;
  5534. }
  5535. if (startAddress == PLSR_CONTROL_ADDRESS)
  5536. {
  5537. if (quantity != 1U)
  5538. {
  5539. return PLSR_MB_ILLEGAL_ADDRESS;
  5540. }
  5541. switch (values[0])
  5542. {
  5543. case 0U: return PLSR_MB_OK;
  5544. case PLSR_COMMAND_START:
  5545. case PLSR_COMMAND_STOP:
  5546. case PLSR_COMMAND_CLEAR:
  5547. return PlsrQueueCommand(values[0]);
  5548. default: return PLSR_MB_ILLEGAL_VALUE;
  5549. }
  5550. }
  5551. if (startAddress == PLSR_OUTPUT_MODE_ADDRESS)
  5552. {
  5553. if (quantity != 1U)
  5554. {
  5555. return PLSR_MB_ILLEGAL_ADDRESS;
  5556. }
  5557. if (PlsrIsBusy() != 0U)
  5558. {
  5559. return PLSR_MB_DEVICE_BUSY;
  5560. }
  5561. PlsrCandidateConfig = PlsrShadowConfig;
  5562. PlsrCandidateConfig.outputMode = values[0];
  5563. if (PlsrConfigIsValid(&PlsrCandidateConfig, 0U) == 0U)
  5564. {
  5565. return PLSR_MB_ILLEGAL_VALUE;
  5566. }
  5567. criticalState = PlsrPlatformEnterCritical();
  5568. PlsrShadowConfig = PlsrCandidateConfig;
  5569. PlsrPlatformExitCritical(criticalState);
  5570. PlsrPlatformCheckpointConfig(&PlsrShadowConfig);
  5571. PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS);
  5572. return PLSR_MB_OK;
  5573. }
  5574. if ((startAddress < PLSR_CONFIG_FIRST_ADDRESS)
  5575. || ((uint32_t)startAddress + quantity - 1UL
  5576. > PLSR_CONFIG_LAST_ADDRESS))
  5577. {
  5578. return PLSR_MB_ILLEGAL_ADDRESS;
  5579. }
  5580. if ((PlsrIsBusy() != 0U)
  5581. && (startAddress <= 0x1001U)
  5582. && ((uint32_t)startAddress + quantity - 1UL >= 0x1000UL))
  5583. {
  5584. return PLSR_MB_DEVICE_BUSY;
  5585. }
  5586. requestEnd = (uint32_t)startAddress + quantity;
  5587. for (index = 0U; index < quantity; index++)
  5588. {
  5589. uint16_t address = (uint16_t)(startAddress + index);
  5590. if (PlsrAddressIsDwordHalf(address, &pairedAddress) != 0U)
  5591. {
  5592. if (((uint32_t)pairedAddress < startAddress)
  5593. || ((uint32_t)pairedAddress >= requestEnd))
  5594. {
  5595. return PLSR_MB_ILLEGAL_ADDRESS;
  5596. }
  5597. }
  5598. }
  5599. PlsrCandidateConfig = PlsrShadowConfig;
  5600. for (index = 0U; index < quantity; index++)
  5601. {
  5602. wordResult = PlsrWriteConfigWord(&PlsrCandidateConfig,
  5603. (uint16_t)(startAddress + index),
  5604. values[index]);
  5605. if (wordResult == PLSR_WORD_ILLEGAL_ADDRESS)
  5606. {
  5607. return PLSR_MB_ILLEGAL_ADDRESS;
  5608. }
  5609. if (wordResult == PLSR_WORD_ILLEGAL_VALUE)
  5610. {
  5611. return PLSR_MB_ILLEGAL_VALUE;
  5612. }
  5613. }
  5614. if (PlsrConfigIsValid(&PlsrCandidateConfig, 0U) == 0U)
  5615. {
  5616. return PLSR_MB_ILLEGAL_VALUE;
  5617. }
  5618. if (PlsrIsBusy() != 0U)
  5619. {
  5620. for (index = 0U; index < PlsrActiveConfig.segmentCount; index++)
  5621. {
  5622. if (PlsrCandidateConfig.segments[index].frequencyHz
  5623. != PlsrShadowConfig.segments[index].frequencyHz)
  5624. {
  5625. updateActiveFrequencies = 1U;
  5626. }
  5627. }
  5628. }
  5629. if (updateActiveFrequencies != 0U)
  5630. {
  5631. handoffBank = PlsrBuildHandoffPlanBank(&PlsrCandidateConfig);
  5632. }
  5633. criticalState = PlsrPlatformEnterCritical();
  5634. segmentBeforeDrain = PlsrCurrentSegment;
  5635. if ((updateActiveFrequencies != 0U) && (PlsrPulseActive != 0U)
  5636. && (PlsrActiveConfig.outputMode != PLSR_OUTPUT_PULSE_DIR))
  5637. {
  5638. PlsrPlatformDrainPendingPulse(
  5639. (uint8_t)PlsrActiveConfig.pulseOutput);
  5640. drainedToDifferentSegment =
  5641. (PlsrCurrentSegment != segmentBeforeDrain) ? 1U : 0U;
  5642. if ((drainedToDifferentSegment != 0U)
  5643. && (PlsrCurrentSegment != 0U)
  5644. && (PlsrCurrentSegment <= PlsrCandidateConfig.segmentCount))
  5645. {
  5646. drainedSegmentTargetHz = PlsrResolvedSegmentFrequency(
  5647. &PlsrCandidateConfig, PlsrCurrentSegment - 1U);
  5648. }
  5649. }
  5650. PlsrShadowConfig = PlsrCandidateConfig;
  5651. if (updateActiveFrequencies != 0U)
  5652. {
  5653. PlsrInvalidateTimedStartLocked();
  5654. for (index = 0U; index < PlsrActiveConfig.segmentCount; index++)
  5655. {
  5656. uint8_t frequencyChanged =
  5657. (PlsrActiveConfig.segments[index].frequencyHz
  5658. != PlsrResolvedSegmentFrequency(&PlsrCandidateConfig, index))
  5659. ? 1U : 0U;
  5660. PlsrActiveConfig.segments[index].frequencyHz =
  5661. PlsrResolvedSegmentFrequency(&PlsrCandidateConfig, index);
  5662. if ((frequencyChanged != 0U)
  5663. && (index + 1U == PlsrCurrentSegment)
  5664. && (PlsrStopRequested == 0U)
  5665. && (PlsrAbStopArmed == 0U)
  5666. && (PlsrRunStatus != PLSR_STATUS_WAITING))
  5667. {
  5668. PlsrFrequencyUpdateTargetHz =
  5669. PlsrResolvedSegmentFrequency(&PlsrCandidateConfig,
  5670. index);
  5671. PlsrFrequencyUpdateSegment = (uint8_t)(index + 1U);
  5672. PlsrFrequencyUpdatePending = 1U;
  5673. }
  5674. }
  5675. PlsrPreparedHandoffBank = handoffBank;
  5676. if (PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR)
  5677. {
  5678. /* Publish the new prepared-plan bank without touching the stream
  5679. or handoff metadata already visible to the counter IRQ. A
  5680. task-context replan replaces the current tail atomically. If
  5681. the segment marker was already latched, the request remains
  5682. pending and is retargeted by the boundary event. */
  5683. if ((PlsrPulseActive != 0U)
  5684. && (PlsrStopRequested == 0U)
  5685. && (PlsrAbStopArmed == 0U)
  5686. && (PlsrRunStatus != PLSR_STATUS_WAITING)
  5687. && (PlsrCurrentSegment != 0U)
  5688. && (PlsrCurrentSegment
  5689. <= PlsrActiveConfig.segmentCount))
  5690. {
  5691. PlsrFrequencyUpdateSegment = PlsrCurrentSegment;
  5692. PlsrFrequencyUpdateTargetHz =
  5693. PlsrActiveConfig.segments[
  5694. PlsrCurrentSegment - 1U].frequencyHz;
  5695. PlsrFrequencyUpdatePending = 1U;
  5696. }
  5697. }
  5698. else
  5699. {
  5700. PlsrHandoffPlan.valid = 0U;
  5701. PlsrProfileQueueInvalidateGeneration();
  5702. if ((drainedToDifferentSegment == 0U)
  5703. && (PlsrPulseActive != 0U)
  5704. && (PlsrAbStopArmed == 0U)
  5705. && (PlsrShortProfile.active == 0U)
  5706. && (PlsrRemainingPulses == 1UL))
  5707. {
  5708. (void)PlsrPrimeHandoff();
  5709. }
  5710. }
  5711. if ((drainedToDifferentSegment != 0U)
  5712. && (PlsrPulseActive != 0U)
  5713. && (PlsrCurrentSegment != 0U)
  5714. && (PlsrCurrentSegment <= PlsrActiveConfig.segmentCount)
  5715. && (PlsrStopRequested == 0U)
  5716. && (PlsrAbStopArmed == 0U)
  5717. && (PlsrRunStatus != PLSR_STATUS_WAITING)
  5718. && (PlsrCurrentFrequencyHz != drainedSegmentTargetHz))
  5719. {
  5720. PlsrFrequencyUpdateTargetHz =
  5721. drainedSegmentTargetHz;
  5722. PlsrFrequencyUpdateSegment = PlsrCurrentSegment;
  5723. PlsrFrequencyUpdatePending = 1U;
  5724. }
  5725. }
  5726. PlsrPlatformExitCritical(criticalState);
  5727. if ((updateActiveFrequencies != 0U)
  5728. && (PlsrPulseActive != 0U)
  5729. && (PlsrActiveConfig.outputMode != PLSR_OUTPUT_PULSE_DIR)
  5730. && (PlsrAbStopArmed == 0U)
  5731. && (PlsrFrequencyUpdatePending == 0U)
  5732. && (PlsrPrepareFutureHandoffQueue() == 0U))
  5733. {
  5734. PlsrTimerErrorPending = 1U;
  5735. }
  5736. if ((drainedToDifferentSegment != 0U)
  5737. && (PlsrAbStopArmed == 0U)
  5738. && (PlsrFrequencyUpdatePending != 0U))
  5739. {
  5740. uint32_t drainEpoch = PlsrSegmentEpoch;
  5741. uint32_t actualDrainFrequencyHz;
  5742. queueResult = PlsrDiagnosticQueueFrequency(
  5743. (uint8_t)PlsrActiveConfig.pulseOutput,
  5744. drainedSegmentTargetHz, &actualDrainFrequencyHz);
  5745. if (queueResult == PLSR_PLATFORM_QUEUE_FAILED)
  5746. {
  5747. PlsrTimerErrorPending = 1U;
  5748. }
  5749. else if (queueResult == PLSR_PLATFORM_QUEUE_APPLIED)
  5750. {
  5751. uint32_t currentEpoch;
  5752. uint8_t updateSegment;
  5753. uint8_t currentSegment;
  5754. criticalState = PlsrPlatformEnterCritical();
  5755. currentEpoch = PlsrSegmentEpoch;
  5756. updateSegment = PlsrFrequencyUpdateSegment;
  5757. currentSegment = PlsrCurrentSegment;
  5758. if ((currentEpoch == drainEpoch)
  5759. && (updateSegment == currentSegment))
  5760. {
  5761. PlsrQueuedFrequencyHz = actualDrainFrequencyHz;
  5762. PlsrFrequencyUpdatePending = 0U;
  5763. PlsrRamp.active = 0U;
  5764. PlsrRunStatus = PLSR_STATUS_RUNNING;
  5765. }
  5766. PlsrPlatformExitCritical(criticalState);
  5767. }
  5768. }
  5769. PlsrPlatformCheckpointConfig(&PlsrShadowConfig);
  5770. PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS);
  5771. return PLSR_MB_OK;
  5772. }
  5773. #ifdef PLSR_HOST_TEST
  5774. void PlsrTestSetPosition(int32_t position, uint8_t positionValid)
  5775. {
  5776. uint32_t criticalState = PlsrPlatformEnterCritical();
  5777. PlsrPosition = position;
  5778. PlsrPositionValid = (positionValid != 0U) ? 1U : 0U;
  5779. PlsrRemainingPulses = 0UL;
  5780. PlsrPositionCheckpointDirty = 0U;
  5781. PlsrPlatformExitCritical(criticalState);
  5782. PlsrPlatformCheckpointPosition(position, PlsrPositionValid, 0U);
  5783. }
  5784. void PlsrTestSetDiagnosticCurveCounts(uint32_t sampleCount,
  5785. uint32_t mismatchCount)
  5786. {
  5787. PlsrDiagnostic.curveSampleCount = sampleCount;
  5788. PlsrDiagnostic.curveMismatchCount = mismatchCount;
  5789. }
  5790. void PlsrTestForceProfileQueueEmpty(void)
  5791. {
  5792. PlsrProfileQueue.readIndex = PlsrProfileQueue.writeIndex;
  5793. PlsrProfileQueue.generatorComplete = 0U;
  5794. PlsrProfileQueue.active = 1U;
  5795. }
  5796. uint16_t PlsrTestProfileQueueCount(void)
  5797. {
  5798. return PlsrProfileQueueCount();
  5799. }
  5800. uint32_t PlsrTestProfileQueueGeneration(void)
  5801. {
  5802. return PlsrProfileQueue.generation;
  5803. }
  5804. uint32_t PlsrTestProfileQueueReadIndex(void)
  5805. {
  5806. return PlsrProfileQueue.readIndex;
  5807. }
  5808. uint32_t PlsrTestProfileQueueWriteIndex(void)
  5809. {
  5810. return PlsrProfileQueue.writeIndex;
  5811. }
  5812. uint32_t PlsrTestProfileQueueHeadFrequency(void)
  5813. {
  5814. uint32_t readIndex = PlsrProfileQueue.readIndex;
  5815. if (readIndex == PlsrProfileQueue.writeIndex)
  5816. {
  5817. return 0UL;
  5818. }
  5819. return PlsrProfileQueue
  5820. .entries[readIndex & PLSR_PROFILE_QUEUE_MASK].requestedFrequencyHz;
  5821. }
  5822. uint16_t PlsrTestProfileProducerNextPeriod(void)
  5823. {
  5824. return PlsrProfileQueue.producerProfile.nextPeriod;
  5825. }
  5826. uint16_t PlsrTestActiveProfileNextPeriod(void)
  5827. {
  5828. return PlsrShortProfile.nextPeriod;
  5829. }
  5830. uint32_t PlsrTestLastGateEvent(void)
  5831. {
  5832. return PlsrHostTestLastGateEvent;
  5833. }
  5834. uint32_t PlsrTestLastDiagnosticFinishEvent(void)
  5835. {
  5836. return PlsrHostTestLastDiagnosticFinishEvent;
  5837. }
  5838. #endif