1 /*
2  * Copyright (c) 2022-2023, Texas Instruments Incorporated
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * *  Redistributions of source code must retain the above copyright
10  *    notice, this list of conditions and the following disclaimer.
11  *
12  * *  Redistributions in binary form must reproduce the above copyright
13  *    notice, this list of conditions and the following disclaimer in the
14  *    documentation and/or other materials provided with the distribution.
15  *
16  * *  Neither the name of Texas Instruments Incorporated nor the names of
17  *    its contributors may be used to endorse or promote products derived
18  *    from this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
22  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
23  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
24  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
27  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
28  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
29  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
30  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31  */
32 /*
33  *  ======== RCL_Scheduler.c ========
34  */
35 
36 #include <stdint.h>
37 #include <stddef.h>
38 #include <stdbool.h>
39 
40 #include <ti/log/Log.h>
41 
42 #include <ti/drivers/dpl/HwiP.h>
43 
44 #include <ti/drivers/rcl/hal/hal.h>
45 #include <ti/drivers/rcl/RCL_Command.h>
46 #include <ti/drivers/rcl/RCL_Scheduler.h>
47 #include <ti/drivers/rcl/RCL_Debug.h>
48 
49 #include <ti/drivers/rcl/LRF.h>
50 
51 RCL_SchedulerState rclSchedulerState;
52 
53 
54 typedef enum
55 {
56     SchedulerNoStart = 0,
57     SchedulerStartNow = 1,
58     SchedulerStartAbsTimeAllowDelay = 2,
59     SchedulerStartAbsTimeNoDelay = 3,
60 } SchedulerStartType;
61 
62 static RCL_CommandStatus rclSchedulerProcessCmdStartStopTime(const RCL_CommandTiming *timing, uint32_t startTime, SchedulerStartType startType);
63 static void rclSchedulerFindEarliestStopTime(RCL_SchedulerStopInfo *stopInfo);
64 static RCL_StopType rclSchedulerSetNewStopTime(RCL_SchedulerStopInfo *stopInfo, uint32_t newStopTime, bool sched);
65 static RCL_StopType rclSchedulerCancelStopTime(RCL_SchedulerStopInfo *stopInfo, bool sched);
66 
67 /*
68  *  ======== RCL_Scheduler_findStopStatus ========
69  */
RCL_Scheduler_findStopStatus(RCL_StopType stopType)70 RCL_CommandStatus RCL_Scheduler_findStopStatus(RCL_StopType stopType)
71 {
72     RCL_CommandStatus status;
73     switch (stopType)
74     {
75         case RCL_StopType_DescheduleOnly:
76             switch (rclSchedulerState.descheduleReason)
77             {
78                 case RCL_SchedulerStopReason_Scheduling:
79                     status = RCL_CommandStatus_DescheduledScheduling;
80                     break;
81                 case RCL_SchedulerStopReason_Api:
82                     status = RCL_CommandStatus_DescheduledApi;
83                     break;
84                 default:
85                     /* Other values should not occur */
86                     status = RCL_CommandStatus_Error;
87                     break;
88             }
89             break;
90         case RCL_StopType_Graceful:
91             switch (rclSchedulerState.gracefulStopInfo.stopReason)
92             {
93                 case RCL_SchedulerStopReason_Timeout:
94                     status = RCL_CommandStatus_GracefulStopTimeout;
95                     break;
96                 case RCL_SchedulerStopReason_Scheduling:
97                     status = RCL_CommandStatus_GracefulStopScheduling;
98                     break;
99                 case RCL_SchedulerStopReason_Api:
100                     status = RCL_CommandStatus_GracefulStopApi;
101                     break;
102                 default:
103                     /* Other values should not occur */
104                     status = RCL_CommandStatus_Error;
105                     break;
106             }
107             break;
108         case RCL_StopType_Hard:
109             switch (rclSchedulerState.hardStopInfo.stopReason)
110             {
111                 case RCL_SchedulerStopReason_Timeout:
112                     status = RCL_CommandStatus_HardStopTimeout;
113                     break;
114                 case RCL_SchedulerStopReason_Scheduling:
115                     status = RCL_CommandStatus_HardStopScheduling;
116                     break;
117                 case RCL_SchedulerStopReason_Api:
118                     status = RCL_CommandStatus_HardStopApi;
119                     break;
120                 default:
121                     /* Other values should not occur */
122                     status = RCL_CommandStatus_Error;
123                     break;
124             }
125             break;
126         default:
127             /* Other stop types not allowed */
128             status = RCL_CommandStatus_Error;
129     }
130     /* Error status should not be produced if function is used correctly */
131     RCL_Debug_assert(status < RCL_CommandStatus_Error);
132 
133     return status;
134 }
135 
136 /*
137  *  ======== RCL_Scheduler_setStartStopTime ========
138  */
RCL_Scheduler_setStartStopTime(const RCL_Command * cmd)139 RCL_CommandStatus RCL_Scheduler_setStartStopTime(const RCL_Command *cmd)
140 {
141     uint32_t startTime;
142     SchedulerStartType startType;
143 
144     RCL_Debug_assert(cmd != NULL);
145 
146     if (cmd->scheduling == RCL_Schedule_AbsTime)
147     {
148         startType = cmd->allowDelay ? SchedulerStartAbsTimeAllowDelay : SchedulerStartAbsTimeNoDelay;
149         startTime = cmd->timing.absStartTime;
150     }
151     else
152     {
153         /* For schedule now, we don't need to check allowDelay here, as delays at this stage will be small */
154         startType = SchedulerStartNow;
155         startTime = 0;
156     }
157 
158     return rclSchedulerProcessCmdStartStopTime(&cmd->timing, startTime, startType);
159 }
160 
161 /*
162  *  ======== RCL_Scheduler_setStartStopTimeEarliestStart ========
163  */
RCL_Scheduler_setStartStopTimeEarliestStart(const RCL_Command * cmd,uint32_t earliestStartTime)164 RCL_CommandStatus RCL_Scheduler_setStartStopTimeEarliestStart(const RCL_Command *cmd, uint32_t earliestStartTime)
165 {
166     uint32_t startTime;
167     SchedulerStartType startType;
168 
169     RCL_Debug_assert(cmd != NULL);
170 
171     if (cmd->scheduling == RCL_Schedule_AbsTime)
172     {
173         startType = cmd->allowDelay ? SchedulerStartAbsTimeAllowDelay : SchedulerStartAbsTimeNoDelay;
174         startTime = cmd->timing.absStartTime;
175         if (!RCL_Scheduler_isLater(earliestStartTime, startTime))
176         {
177             /* Start time is earlier than indicated - delay start if allowed */
178             if (startType == SchedulerStartAbsTimeNoDelay)
179             {
180                 /* Delay not allowed */
181                 return RCL_CommandStatus_Error_StartTooLate;
182             }
183             else
184             {
185                 startTime = earliestStartTime;
186             }
187         }
188     }
189     else
190     {
191         /* For schedule now, we don't need to check allowDelay here, as delays at this stage will be small */
192         startType = SchedulerStartAbsTimeAllowDelay;
193         startTime = earliestStartTime;
194     }
195 
196     return rclSchedulerProcessCmdStartStopTime(&cmd->timing, startTime, startType);
197 }
198 
199 /*
200  *  ======== RCL_Scheduler_setCustomStartStopTime ========
201  */
RCL_Scheduler_setCustomStartStopTime(const RCL_CommandTiming * timing,RCL_ScheduleType scheduling,bool allowDelay)202 RCL_CommandStatus RCL_Scheduler_setCustomStartStopTime(const RCL_CommandTiming *timing, RCL_ScheduleType scheduling, bool allowDelay)
203 {
204     uint32_t startTime;
205     SchedulerStartType startType;
206 
207     RCL_Debug_assert(timing != NULL);
208 
209     if (scheduling == RCL_Schedule_AbsTime)
210     {
211         startType = allowDelay ? SchedulerStartAbsTimeAllowDelay : SchedulerStartAbsTimeNoDelay;
212         startTime = timing->absStartTime;
213     }
214     else
215     {
216         /* For schedule now, we don't need to check allowDelay here, as delays at this stage will be small */
217         startType = SchedulerStartNow;
218         startTime = 0;
219     }
220 
221     return rclSchedulerProcessCmdStartStopTime(timing, startTime, startType);
222 }
223 
224 /*
225  *  ======== RCL_Scheduler_setCustomStartStopTimeEarliestStart ========
226  */
RCL_Scheduler_setCustomStartStopTimeEarliestStart(const RCL_CommandTiming * timing,RCL_ScheduleType scheduling,bool allowDelay,uint32_t earliestStartTime)227 RCL_CommandStatus RCL_Scheduler_setCustomStartStopTimeEarliestStart(const RCL_CommandTiming *timing, RCL_ScheduleType scheduling, bool allowDelay, uint32_t earliestStartTime)
228 {
229     uint32_t startTime;
230     SchedulerStartType startType;
231 
232     RCL_Debug_assert(timing != NULL);
233 
234     if (scheduling == RCL_Schedule_AbsTime)
235     {
236         startType = allowDelay ? SchedulerStartAbsTimeAllowDelay : SchedulerStartAbsTimeNoDelay;
237         startTime = timing->absStartTime;
238         if (!RCL_Scheduler_isLater(earliestStartTime, startTime))
239         {
240             /* Start time is earlier than indicated - delay start if allowed */
241             if (startType == SchedulerStartAbsTimeNoDelay)
242             {
243                 /* Delay not allowed */
244                 return RCL_CommandStatus_Error_StartTooLate;
245             }
246             else
247             {
248                 startTime = earliestStartTime;
249             }
250         }
251     }
252     else
253     {
254         /* For schedule now, we don't need to check allowDelay here, as delays at this stage will be small */
255         startType = SchedulerStartAbsTimeAllowDelay;
256         startTime = earliestStartTime;
257     }
258 
259     return rclSchedulerProcessCmdStartStopTime(timing, startTime, startType);
260 }
261 
262 /*
263  *  ======== RCL_Scheduler_setCmdStopTimeNoStartTrigger ========
264  */
RCL_Scheduler_setCmdStopTimeNoStartTrigger(const RCL_Command * cmd)265 RCL_CommandStatus RCL_Scheduler_setCmdStopTimeNoStartTrigger(const RCL_Command *cmd)
266 {
267     RCL_Debug_assert(cmd != NULL);
268 
269     return rclSchedulerProcessCmdStartStopTime(&cmd->timing, 0, SchedulerNoStart);
270 }
271 
272 /*
273  *  ======== RCL_Scheduler_setNewStartNow ========
274  */
RCL_Scheduler_setNewStartNow(void)275 RCL_CommandStatus RCL_Scheduler_setNewStartNow(void)
276 {
277     return rclSchedulerProcessCmdStartStopTime(NULL, 0, SchedulerStartNow);
278 }
279 
280 /*
281  *  ======== RCL_Scheduler_setNewStartAbsTime ========
282  */
RCL_Scheduler_setNewStartAbsTime(uint32_t startTime,bool allowDelay)283 RCL_CommandStatus RCL_Scheduler_setNewStartAbsTime(uint32_t startTime, bool allowDelay)
284 {
285     return rclSchedulerProcessCmdStartStopTime(NULL, startTime,
286         allowDelay ? SchedulerStartAbsTimeAllowDelay : SchedulerStartAbsTimeNoDelay);
287 }
288 
289 /*
290  *  ======== RCL_Scheduler_setNewStartRelTime ========
291  */
RCL_Scheduler_setNewStartRelTime(uint32_t relStartTime)292 RCL_CommandStatus RCL_Scheduler_setNewStartRelTime(uint32_t relStartTime)
293 {
294     uint32_t startTime = rclSchedulerState.actualStartTime + relStartTime;
295 
296     return rclSchedulerProcessCmdStartStopTime(NULL, startTime, SchedulerStartAbsTimeAllowDelay);
297 }
298 
299 /*
300  *  ======== rclSchedulerProcessCmdStartStopTime ========
301  */
rclSchedulerProcessCmdStartStopTime(const RCL_CommandTiming * timing,uint32_t startTime,SchedulerStartType startType)302 static RCL_CommandStatus rclSchedulerProcessCmdStartStopTime(const RCL_CommandTiming *timing, uint32_t startTime, SchedulerStartType startType)
303 {
304     uintptr_t key;
305     uint32_t currentTime;
306     uint32_t actualStartTime = startTime;
307 
308     key = HwiP_disable();
309 
310     if (startType == SchedulerNoStart)
311     {
312         actualStartTime = hal_get_current_time();
313     }
314     else if (startType == SchedulerStartNow)
315     {
316         /* Find start time to trig now; add a delay to allow trigger to be in the future */
317         actualStartTime = hal_get_current_time() + RCL_SCHEDULER_TRIG_NOW_DELAY;
318     }
319     else
320     {
321         /* Check if there is enough time for start */
322         currentTime = hal_get_current_time();
323         if (!RCL_Scheduler_isLater(currentTime + RCL_SCHEDULER_TRIG_NOW_DELAY, startTime))
324         {
325             if (startType == SchedulerStartAbsTimeAllowDelay)
326             {
327                 actualStartTime = currentTime + RCL_SCHEDULER_TRIG_NOW_DELAY;
328             }
329             else {
330                 /* Error - start is too late */
331                 HwiP_restore(key);
332                 return RCL_CommandStatus_Error_StartTooLate;
333             }
334         }
335     }
336 
337     rclSchedulerState.actualStartTime = actualStartTime;
338     if (startType != SchedulerNoStart)
339     {
340         hal_setup_start_time(actualStartTime);
341         /* Due to protected area and added delay, the programmed start time is not expected to be in the future. */
342         RCL_Debug_assert(RCL_Scheduler_isLater(RCL_Scheduler_getCurrentTime(), actualStartTime));
343     }
344 
345     /* Set up stop times from command if not already done */
346     if (timing != NULL && (rclSchedulerState.stopTimeState < RCL_SchedulerStopTimeState_Found))
347     {
348         uint32_t relHardStopTime = timing->relHardStopTime;
349         int32_t relGracefulStopTime = timing->relGracefulStopTime;
350         if (relHardStopTime != 0)
351         {
352             rclSchedulerState.hardStopInfo.cmdStopTime = startTime + relHardStopTime;
353             rclSchedulerState.hardStopInfo.cmdStopEnabled = true;
354         }
355 
356         if (relGracefulStopTime < 0 && relHardStopTime != 0)
357         {
358             /* Graceful stop time relative to hard stop time */
359             relGracefulStopTime += relHardStopTime;
360             if (relGracefulStopTime <= 0)
361             {
362                 /* Stop immediately */
363                 relGracefulStopTime = 1;
364             }
365         }
366         if (relGracefulStopTime > 0)
367         {
368             rclSchedulerState.gracefulStopInfo.cmdStopTime = actualStartTime + relGracefulStopTime;
369             rclSchedulerState.gracefulStopInfo.cmdStopEnabled = true;
370         }
371 
372         /* Find earliest stop times */
373         rclSchedulerFindEarliestStopTime(&rclSchedulerState.hardStopInfo);
374         rclSchedulerFindEarliestStopTime(&rclSchedulerState.gracefulStopInfo);
375 
376         rclSchedulerState.stopTimeState = RCL_SchedulerStopTimeState_Found;
377     }
378 
379     /* Check if stop has been requested; at this time, radio may not have started */
380     if (rclSchedulerState.hardStopInfo.apiStopEnabled)
381     {
382         /* Hard stop before modem start */
383         HwiP_restore(key);
384         return RCL_Scheduler_findStopStatus(RCL_StopType_Hard);
385     }
386     else if (rclSchedulerState.hardStopInfo.stopReason != RCL_SchedulerStopReason_None)
387     {
388         uint32_t stopTime = (rclSchedulerState.hardStopInfo.stopReason == RCL_SchedulerStopReason_Timeout) ?
389             rclSchedulerState.hardStopInfo.cmdStopTime : rclSchedulerState.hardStopInfo.schedStopTime;
390 
391         if (!RCL_Scheduler_isLater(actualStartTime, stopTime))
392         {
393             /* Hard stop before modem start */
394             HwiP_restore(key);
395             return RCL_Scheduler_findStopStatus(RCL_StopType_Hard);
396         }
397     }
398 
399     if (rclSchedulerState.gracefulStopInfo.apiStopEnabled)
400     {
401         /* Graceful stop before modem start */
402         HwiP_restore(key);
403         return RCL_Scheduler_findStopStatus(RCL_StopType_Graceful);
404     }
405     else if (rclSchedulerState.gracefulStopInfo.stopReason != RCL_SchedulerStopReason_None)
406     {
407         uint32_t stopTime = (rclSchedulerState.gracefulStopInfo.stopReason == RCL_SchedulerStopReason_Timeout) ?
408             rclSchedulerState.gracefulStopInfo.cmdStopTime : rclSchedulerState.gracefulStopInfo.schedStopTime;
409 
410         if (!RCL_Scheduler_isLater(actualStartTime, stopTime))
411         {
412             /* Graceful stop before modem start */
413             HwiP_restore(key);
414             return RCL_Scheduler_findStopStatus(RCL_StopType_Graceful);
415         }
416     }
417 
418     HwiP_restore(key);
419 
420     Log_printf(RclCore, Log_DEBUG, "Using PBE start time 0x%08X (current time 0x%08X)", actualStartTime, RCL_Scheduler_getCurrentTime());
421 
422     return RCL_CommandStatus_Active;
423 }
424 
425 /*
426  *  ======== RCL_Scheduler_setStopTimes ========
427  */
RCL_Scheduler_setStopTimes(void)428 RCL_StopType RCL_Scheduler_setStopTimes(void)
429 {
430     RCL_StopType stopType = RCL_StopType_None;
431 
432     /* If stop times have not been found, do not program stop. Note that this
433        should only be done for very fast commands that are over as soon as
434        they start */
435     if (rclSchedulerState.stopTimeState >= RCL_SchedulerStopTimeState_Found)
436     {
437         uintptr_t key = HwiP_disable();
438 
439         if (rclSchedulerState.hardStopInfo.stopReason != RCL_SchedulerStopReason_None)
440         {
441             uint32_t stopTime = (rclSchedulerState.hardStopInfo.stopReason == RCL_SchedulerStopReason_Timeout) ?
442                 rclSchedulerState.hardStopInfo.cmdStopTime : rclSchedulerState.hardStopInfo.schedStopTime;
443 
444             hal_setup_hard_stop_time(stopTime);
445 
446             if (!RCL_Scheduler_isLater(RCL_Scheduler_getCurrentTime(), stopTime))
447             {
448                 /* Hard stop already occurred */
449                 hal_cancel_hard_stop_time();
450                 stopType = RCL_StopType_Hard;
451             }
452         }
453 
454         if (rclSchedulerState.gracefulStopInfo.stopReason != RCL_SchedulerStopReason_None)
455         {
456             uint32_t stopTime = (rclSchedulerState.gracefulStopInfo.stopReason == RCL_SchedulerStopReason_Timeout) ?
457                 rclSchedulerState.gracefulStopInfo.cmdStopTime : rclSchedulerState.gracefulStopInfo.schedStopTime;
458 
459             hal_setup_graceful_stop_time(stopTime);
460 
461             if (!RCL_Scheduler_isLater(RCL_Scheduler_getCurrentTime(), stopTime))
462             {
463                 /* Graceful stop already occurred */
464                 hal_cancel_graceful_stop_time();
465                 stopType = RCL_StopType_Graceful;
466             }
467         }
468 
469         rclSchedulerState.stopTimeState = RCL_SchedulerStopTimeState_Programmed;
470 
471         HwiP_restore(key);
472     }
473     return stopType;
474 }
475 
476 /*
477  *  ======== RCL_Scheduler_isLater ========
478  */
RCL_Scheduler_isLater(uint32_t refTime,uint32_t chkTime)479 bool RCL_Scheduler_isLater(uint32_t refTime, uint32_t chkTime)
480 {
481     uint32_t timediff = refTime - chkTime;
482     if (timediff >= 0x20000000)
483     {
484         /* chkTime is later than refTime */
485         return true;
486     }
487     else
488     {
489         /* chkTime is earlier than or same time as refTime */
490         return false;
491     }
492 }
493 
494 /*
495  *  ======== RCL_Scheduler_delta ========
496  */
RCL_Scheduler_delta(uint32_t refTime,uint32_t chkTime)497 int32_t RCL_Scheduler_delta(uint32_t refTime, uint32_t chkTime)
498 {
499     uint32_t timediff = refTime - chkTime;
500     if (timediff >= 0x20000000)
501     {
502         /* chkTime is later than refTime. Get difference as a positive number */
503         timediff = chkTime - refTime;
504         if (timediff >= 0x80000000)
505         {
506             /* Large difference - saturate */
507             return (int32_t) 0x7FFFFFFF;
508         }
509         else
510         {
511             /* Difference is a positive value also as signed */
512             return (int32_t) timediff;
513         }
514     }
515     else
516     {
517         /* chkTime is earlier than or same time as refTime. Return negative result or 0 */
518         return (int32_t)(-timediff);
519     }
520 }
521 
522 /*
523  *  ======== RCL_Scheduler_setSchedStopTime ========
524  */
RCL_Scheduler_setSchedStopTime(RCL_SchedulerStopInfo * stopInfo,uint32_t schedStopTime)525 RCL_StopType RCL_Scheduler_setSchedStopTime(RCL_SchedulerStopInfo *stopInfo, uint32_t schedStopTime)
526 {
527     return rclSchedulerSetNewStopTime(stopInfo, schedStopTime, true);
528 }
529 
530 /*
531  *  ======== RCL_Scheduler_setCmdStopTime ========
532  */
RCL_Scheduler_setCmdStopTime(RCL_SchedulerStopInfo * stopInfo,uint32_t cmdStopTime)533 RCL_StopType RCL_Scheduler_setCmdStopTime(RCL_SchedulerStopInfo *stopInfo, uint32_t cmdStopTime)
534 {
535     return rclSchedulerSetNewStopTime(stopInfo, cmdStopTime, false);
536 }
537 
538 /*
539  *  ======== RCL_Scheduler_cancelSchedStopTime ========
540  */
RCL_Scheduler_cancelSchedStopTime(RCL_SchedulerStopInfo * stopInfo)541 RCL_StopType RCL_Scheduler_cancelSchedStopTime(RCL_SchedulerStopInfo *stopInfo)
542 {
543     return rclSchedulerCancelStopTime(stopInfo, true);
544 }
545 
546 /*
547  *  ======== RCL_Scheduler_cancelCmdStopTime ========
548  */
RCL_Scheduler_cancelCmdStopTime(RCL_SchedulerStopInfo * stopInfo)549 RCL_StopType RCL_Scheduler_cancelCmdStopTime(RCL_SchedulerStopInfo *stopInfo)
550 {
551     return rclSchedulerCancelStopTime(stopInfo, false);
552 }
553 
rclSchedulerSetNewStopTime(RCL_SchedulerStopInfo * stopInfo,uint32_t newStopTime,bool sched)554 static RCL_StopType rclSchedulerSetNewStopTime(RCL_SchedulerStopInfo *stopInfo, uint32_t newStopTime, bool sched)
555 {
556     RCL_StopType immediateStop = RCL_StopType_None;
557 
558     /* Store current state of the stop info */
559     RCL_SchedulerStopReason oldStopReason = stopInfo->stopReason;
560     uint32_t oldStopTime;
561     switch (stopInfo->stopReason)
562     {
563         case RCL_SchedulerStopReason_Timeout:
564             oldStopTime = stopInfo->cmdStopTime;
565             break;
566 
567         case RCL_SchedulerStopReason_Scheduling:
568             oldStopTime = stopInfo->schedStopTime;
569             break;
570         default:
571             oldStopTime = 0;
572     }
573 
574     /* Set new stop time and enable it */
575     if (sched)
576     {
577         stopInfo->schedStopTime = newStopTime;
578         stopInfo->schedStopEnabled = 1;
579     }
580     else
581     {
582         stopInfo->cmdStopTime = newStopTime;
583         stopInfo->cmdStopEnabled = 1;
584     }
585 
586     /* Find updated earliest stop time */
587     rclSchedulerFindEarliestStopTime(stopInfo);
588 
589     uint32_t stopTime;
590     /* One of the stop times will be set, since we just set it */
591     stopTime = (stopInfo->stopReason == RCL_SchedulerStopReason_Timeout) ?
592         stopInfo->cmdStopTime : stopInfo->schedStopTime;
593 
594     /* Check if stop time has changed */
595     if (oldStopReason != RCL_SchedulerStopReason_None || stopTime != oldStopTime)
596     {
597         /* Check if stop has been activated */
598         if (rclSchedulerState.stopTimeState == RCL_SchedulerStopTimeState_Programmed)
599         {
600             /* Modify stop time and see if immediate stop is needed */
601             immediateStop = RCL_Scheduler_setStopTimes();
602             /* Notify handler that stop time has been changed */
603             RCL_Scheduler_postEvent(rclSchedulerState.currCmd, RCL_EventStopTimesUpdated);
604         }
605     }
606 
607     return immediateStop;
608 }
609 
rclSchedulerCancelStopTime(RCL_SchedulerStopInfo * stopInfo,bool sched)610 static RCL_StopType rclSchedulerCancelStopTime(RCL_SchedulerStopInfo *stopInfo, bool sched)
611 {
612     RCL_StopType immediateStop = RCL_StopType_None;
613 
614     /* Store current state of the stop info */
615     RCL_SchedulerStopReason oldStopReason = stopInfo->stopReason;
616     uint32_t oldStopTime;
617     switch (stopInfo->stopReason)
618     {
619         case RCL_SchedulerStopReason_Timeout:
620             oldStopTime = stopInfo->cmdStopTime;
621             break;
622 
623         case RCL_SchedulerStopReason_Scheduling:
624             oldStopTime = stopInfo->schedStopTime;
625             break;
626         default:
627             oldStopTime = 0;
628     }
629 
630     /* Disable applicable stop time */
631     if (sched)
632     {
633         stopInfo->schedStopEnabled = 0;
634     }
635     else
636     {
637         stopInfo->cmdStopEnabled = 0;
638     }
639 
640     /* Find updated earliest stop time */
641     rclSchedulerFindEarliestStopTime(stopInfo);
642 
643     if (stopInfo->stopReason == RCL_SchedulerStopReason_None)
644     {
645         if (oldStopReason != RCL_SchedulerStopReason_None && rclSchedulerState.stopTimeState == RCL_SchedulerStopTimeState_Programmed)
646         {
647             /* Cancel stop time */
648             if (stopInfo == &rclSchedulerState.hardStopInfo)
649             {
650                 hal_cancel_hard_stop_time();
651             }
652             else
653             {
654                 hal_cancel_graceful_stop_time();
655             }
656         }
657     }
658     else
659     {
660         uint32_t stopTime = (stopInfo->stopReason == RCL_SchedulerStopReason_Timeout) ?
661             stopInfo->cmdStopTime : stopInfo->schedStopTime;
662 
663         /* Check if stop time has changed */
664         if (stopTime != oldStopTime)
665         {
666             /* Check if stop has been activated */
667             if (rclSchedulerState.stopTimeState == RCL_SchedulerStopTimeState_Programmed)
668             {
669                 /* Modify stop time and see if immediate stop is needed */
670                 immediateStop = RCL_Scheduler_setStopTimes();
671                 /* Notify handler that stop time has been changed */
672                 RCL_Scheduler_postEvent(rclSchedulerState.currCmd, RCL_EventStopTimesUpdated);
673             }
674         }
675     }
676 
677     return immediateStop;
678 }
679 
680 /*
681  *  ======== rclSchedulerFindEarliestStopTime ========
682  */
rclSchedulerFindEarliestStopTime(RCL_SchedulerStopInfo * stopInfo)683 static void rclSchedulerFindEarliestStopTime(RCL_SchedulerStopInfo *stopInfo)
684 {
685     /* Find which stop time comes first */
686     if (stopInfo->cmdStopEnabled)
687     {
688         if (stopInfo->schedStopEnabled)
689         {
690             uint32_t currentTime = RCL_Scheduler_getCurrentTime();
691             if (RCL_Scheduler_isLater(currentTime, stopInfo->cmdStopTime))
692             {
693                 if (RCL_Scheduler_isLater(currentTime, stopInfo->schedStopTime))
694                 {
695                     /* Find difference from current time for each stop time */
696                     uint32_t timeDiffSched = stopInfo->schedStopTime - currentTime;
697                     uint32_t timeDiffCmd = stopInfo->cmdStopTime - currentTime;
698                     if (timeDiffSched < timeDiffCmd)
699                     {
700                         /* Scheduler stop time is first */
701                         stopInfo->stopReason = RCL_SchedulerStopReason_Scheduling;
702                     }
703                     else
704                     {
705                         /* Command stop time is first */
706                         stopInfo->stopReason = RCL_SchedulerStopReason_Timeout;
707                     }
708                 }
709                 else
710                 {
711                     /* Scheduler stop time is already in the past */
712                     stopInfo->stopReason = RCL_SchedulerStopReason_Scheduling;
713                 }
714             }
715             else
716             {
717                 /* Command stop time is already in the past */
718                 stopInfo->stopReason = RCL_SchedulerStopReason_Timeout;
719             }
720         }
721         else
722         {
723             /* Command stop time is the only one */
724             stopInfo->stopReason = RCL_SchedulerStopReason_Timeout;
725         }
726     }
727     else if (stopInfo->schedStopEnabled)
728     {
729         /* Scheduler stop time is the only one */
730         stopInfo->stopReason = RCL_SchedulerStopReason_Scheduling;
731     }
732     else
733     {
734         stopInfo->stopReason = RCL_SchedulerStopReason_None;
735     }
736 }
737 
738 /*
739  *  ======== RCL_Scheduler_postEvent ========
740  */
RCL_Scheduler_postEvent(RCL_Command_Handle c,RCL_Events e)741 bool RCL_Scheduler_postEvent(RCL_Command_Handle c, RCL_Events e)
742 {
743     RCL_Command *cmd = (RCL_Command *)c;
744     bool result = false;
745     if (cmd != NULL)
746     {
747         uintptr_t key = HwiP_disable();
748         if (cmd->status > RCL_CommandStatus_Queued && cmd->status < RCL_CommandStatus_Finished)
749         {
750             rclSchedulerState.postedRclEvents.value |= e.value | RCL_EventSoftwareTriggered.value;
751             hal_trigger_command_fsm();
752             result = true;
753         }
754         HwiP_restore(key);
755     }
756     return result;
757 }
758