1/*-
2 * Copyright (c) 2017 Mark Johnston <markj@FreeBSD.org>
3 *
4 * Redistribution and use in source and binary forms, with or without
5 * modification, are permitted provided that the following conditions
6 * are met:
7 * 1. Redistributions of source code must retain the above copyright
8 *    notice unmodified, this list of conditions, and the following
9 *    disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 *    notice, this list of conditions and the following disclaimer in the
12 *    documentation and/or other materials provided with the distribution.
13 *
14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
15 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
16 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
17 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
18 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
19 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
20 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
21 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
22 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
23 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
24 */
25
26#include "linux/hrtimer.h"
27
28#include "los_task_pri.h"
29#include "los_spinlock.h"
30#include "target_config.h"
31#include "los_init.h"
32
33#define US_PER_SECOND 1000000
34
35/* spinlock for hrtimer module only available on SMP mode */
36LITE_OS_SEC_BSS SPIN_LOCK_INIT(g_hrtimerSpin);
37
38LITE_OS_SEC_BSS struct hrtimer_list_node *g_hrtimerList;
39
40STATIC VOID HandlerNodeAdd(struct hrtimer_list_node *htrimer, struct handler_list_node *handlerNode)
41{
42    struct handler_list_node *tmpNode = NULL;
43
44    if (htrimer == NULL) {
45        return;
46    }
47
48    tmpNode = htrimer->HandlerHead;
49    if (tmpNode == NULL) {
50        htrimer->HandlerHead = handlerNode;
51    } else {
52        while (tmpNode->pstNext != NULL) {
53            tmpNode = tmpNode->pstNext;
54        }
55        tmpNode->pstNext = handlerNode;
56    } /* FIFO */
57}
58
59STATIC VOID HrtimerNodeAdd(struct hrtimer_list_node *htrimer, struct handler_list_node *handlerNode)
60{
61    struct hrtimer_list_node *prevNode = NULL;
62    struct hrtimer_list_node *curNode = NULL;
63    struct hrtimer_list_node *tmpNode = NULL;
64    UINT32 temp;
65
66    if (g_hrtimerList == NULL) {
67        g_hrtimerList = htrimer;
68        HrtimerClockStart(htrimer->set_time_reload);
69    } else {
70        temp = HrtimerClockValueGet();
71        g_hrtimerList->set_time_reload = temp;
72        curNode = g_hrtimerList;
73        while (curNode != NULL) {
74            if (curNode->set_time_reload > htrimer->set_time_reload) {
75                break;
76            }
77            if (curNode->set_time_reload == htrimer->set_time_reload) {
78                HandlerNodeAdd(curNode, handlerNode);
79                (VOID)LOS_MemFree(m_aucSysMem0, htrimer);
80                return;
81            }
82            htrimer->set_time_reload -= curNode->set_time_reload;
83            prevNode = curNode;
84            curNode = curNode->pstNext;
85        }
86        if (curNode == g_hrtimerList) {
87            tmpNode = g_hrtimerList;
88            HrtimerClockStop();
89            HrtimerClockStart(htrimer->set_time_reload);
90
91            tmpNode->set_time_reload -= htrimer->set_time_reload;
92            htrimer->pstNext = curNode;
93            g_hrtimerList = htrimer;
94        } else if (curNode == NULL) {
95            prevNode->pstNext = htrimer;
96        } else {
97            htrimer->pstNext = curNode;
98            prevNode->pstNext = htrimer;
99            curNode->set_time_reload -= htrimer->set_time_reload;
100        }
101    }
102    if (handlerNode != NULL) {
103        HandlerNodeAdd(htrimer, handlerNode);
104    }
105}
106
107STATIC VOID HrtimerHandlerRunAddNode(struct hrtimer_list_node *hrtimer, struct handler_list_node *handlerHead)
108{
109    if (handlerHead == NULL) {
110        (VOID)LOS_MemFree(m_aucSysMem0, hrtimer);
111    } else {
112        hrtimer->set_time_reload = (UINT32)((hrtimer->_softexpires.tv.sec * US_PER_SECOND +
113                                    hrtimer->_softexpires.tv.usec) * HRTIMER_PERUS);
114        LOS_SpinLock(&g_hrtimerSpin);
115        HrtimerNodeAdd(hrtimer, handlerHead);
116        LOS_SpinUnlock(&g_hrtimerSpin);
117    }
118}
119
120STATIC VOID HrtimerHandlerRun(VOID)
121{
122    struct hrtimer_list_node *hrtimer = NULL;
123    struct handler_list_node *curNode = NULL;
124    struct handler_list_node *handler = NULL;
125    struct handler_list_node *handlerTail = NULL;
126    struct handler_list_node *handlerHead = NULL;
127    struct hrtimer timer;
128    enum hrtimer_restart restart;
129
130    LOS_SpinLock(&g_hrtimerSpin);
131    if (g_hrtimerList == NULL) {
132        LOS_SpinUnlock(&g_hrtimerSpin);
133        return;
134    }
135    hrtimer = g_hrtimerList;
136    g_hrtimerList = hrtimer->pstNext;
137    if (g_hrtimerList != NULL) {
138        HrtimerClockStop();
139        HrtimerClockStart(g_hrtimerList->set_time_reload);
140    }
141    LOS_SpinUnlock(&g_hrtimerSpin);
142
143    timer._softexpires.tv.usec = hrtimer->_softexpires.tv.usec;
144    timer._softexpires.tv.sec = hrtimer->_softexpires.tv.sec;
145    handler = hrtimer->HandlerHead;
146    hrtimer->pstNext = NULL;
147    hrtimer->HandlerHead = NULL;
148
149    while ((handler != NULL) && (handler->pfnHandler != NULL)) {
150        timer.function = handler->pfnHandler;
151        restart = handler->pfnHandler(&timer);
152        curNode = handler;
153        handler = handler->pstNext;
154        curNode->pstNext = NULL;
155
156        if (restart == HRTIMER_NORESTART) {
157            (VOID)LOS_MemFree(m_aucSysMem0, curNode);
158        } else if (restart == HRTIMER_RESTART) {
159            if (handlerHead != NULL) {
160                handlerTail->pstNext = curNode;
161                handlerTail = curNode;
162            } else {
163                handlerHead = curNode;
164                handlerTail = curNode;
165            }
166        } else {
167            PRINT_ERR("The return value of hrtimer function is not defined!\n");
168        }
169    }
170
171    HrtimerHandlerRunAddNode(hrtimer, handlerHead);
172}
173
174STATIC VOID HrtimerListScan(VOID)
175{
176    HrtimerClockIrqClear();
177    HrtimerHandlerRun();
178}
179
180STATIC VOID GetHandlerNodePosition(const struct hrtimer *timer, struct hrtimer_list_node *hrtimerNode,
181                                   struct handler_list_node **prevNode, struct handler_list_node **curNode)
182{
183    struct handler_list_node *curHandler = NULL;
184    struct handler_list_node *prevHandler = NULL;
185
186    curHandler = hrtimerNode->HandlerHead;
187    while (curHandler != NULL) {
188        if ((curHandler->pfnHandler == timer->function) &&
189            (curHandler->_softexpires.tv.sec == timer->_softexpires.tv.sec) &&
190            (curHandler->_softexpires.tv.usec == timer->_softexpires.tv.usec)) {
191            *prevNode = prevHandler;
192            *curNode = curHandler;
193            return;
194        }
195        prevHandler = curHandler;
196        curHandler = curHandler->pstNext;
197    }
198}
199
200STATIC VOID GetHrtimerNodePosition(const struct hrtimer *timer, struct hrtimer_list_node **prevNode,
201                                   struct hrtimer_list_node **curNode)
202{
203    struct handler_list_node *curHandler = NULL;
204
205    *curNode = g_hrtimerList;
206    while (*curNode != NULL) {
207        curHandler = (*curNode)->HandlerHead;
208        while (curHandler != NULL) {
209            if ((curHandler->pfnHandler == timer->function) &&
210                (curHandler->_softexpires.tv.sec == timer->_softexpires.tv.sec) &&
211                (curHandler->_softexpires.tv.usec == timer->_softexpires.tv.usec)) {
212                return;
213            }
214            curHandler = curHandler->pstNext;
215        }
216        *prevNode = *curNode;
217        *curNode = (*curNode)->pstNext;
218    }
219}
220
221STATIC struct hrtimer_list_node *HrtimerListNodeInit(union ktime time)
222{
223    struct hrtimer_list_node *hrtimer = (struct hrtimer_list_node *)LOS_MemAlloc(m_aucSysMem0,
224                                                                                 sizeof(struct hrtimer_list_node));
225    if (hrtimer == NULL) {
226        return NULL;
227    }
228    hrtimer->_softexpires = time;
229    hrtimer->set_time_reload = (UINT32)((time.tv.sec * US_PER_SECOND + time.tv.usec) * HRTIMER_PERUS);
230    hrtimer->HandlerHead = NULL;
231    hrtimer->pstNext = NULL;
232    return hrtimer;
233}
234
235STATIC UINT32 ChangeNodePosition(struct hrtimer_list_node *prevNode, struct hrtimer_list_node *curNode,
236                                 struct handler_list_node *prevHandler, struct handler_list_node *curHandler,
237                                 union ktime time)
238{
239    struct hrtimer_list_node *htrimer = NULL;
240
241    if ((prevHandler != NULL) || (curHandler->pstNext != NULL)) {
242        if (prevHandler == NULL) {
243            curNode->HandlerHead = curHandler->pstNext;
244        } else {
245            prevHandler->pstNext = curHandler->pstNext;
246        }
247
248        curHandler->pstNext = NULL;
249        curHandler->_softexpires = time;
250
251        htrimer = HrtimerListNodeInit(time);
252        if (htrimer == NULL) {
253            return LOS_NOK;
254        }
255
256        HrtimerNodeAdd(htrimer, curHandler);
257    } else {
258        if (curNode->pstNext != NULL) {
259            if (curNode == g_hrtimerList) {
260                g_hrtimerList = curNode->pstNext;
261                g_hrtimerList->set_time_reload += HrtimerClockValueGet();
262                HrtimerClockStop();
263                HrtimerClockStart(g_hrtimerList->set_time_reload);
264            } else {
265                prevNode->pstNext = curNode->pstNext;
266                curNode->pstNext->set_time_reload += curNode->set_time_reload;
267            }
268        } else {
269            if (curNode == g_hrtimerList) {
270                g_hrtimerList = NULL;
271                HrtimerClockStop();
272            } else {
273                prevNode->pstNext = NULL;
274            }
275        }
276        curNode->pstNext = NULL;
277        curNode->_softexpires = time;
278        curNode->set_time_reload = (UINT32)((time.tv.sec * US_PER_SECOND + time.tv.usec) * HRTIMER_PERUS);
279        curNode->HandlerHead->_softexpires = time;
280        HrtimerNodeAdd(curNode, NULL);
281    }
282
283    return LOS_OK;
284}
285
286STATIC VOID CancelHandlerNode(const struct hrtimer *timer, struct hrtimer_list_node *curNode)
287{
288    struct handler_list_node *curHandler = curNode->HandlerHead;
289    struct handler_list_node *prevHandler = curHandler;
290
291    while (curHandler != NULL) {
292        if ((curHandler->pfnHandler == timer->function) &&
293            (curHandler->_softexpires.tv.sec == timer->_softexpires.tv.sec) &&
294            (curHandler->_softexpires.tv.usec == timer->_softexpires.tv.usec)) {
295            if (curHandler == curNode->HandlerHead) {
296                curNode->HandlerHead = curHandler->pstNext;
297            } else {
298                prevHandler->pstNext = curHandler->pstNext;
299            }
300            curHandler->pstNext = NULL;
301            (VOID)LOS_MemFree(m_aucSysMem0, curHandler);
302            break;
303        }
304        prevHandler = curHandler;
305        curHandler = curHandler->pstNext;
306    }
307}
308
309STATIC UINT32 CheckTime(union ktime *time)
310{
311    if ((time->tv.sec == 0) && (time->tv.usec == 0)) {
312        return LOS_NOK;
313    }
314
315    if (time->tv.usec >= US_PER_SECOND) {
316        time->tv.sec += time->tv.usec / US_PER_SECOND;
317        time->tv.usec = time->tv.usec % US_PER_SECOND;
318    }
319
320    return LOS_OK;
321}
322
323void linux_hrtimer_init(struct hrtimer *timer, clockid_t clockID, enum hrtimer_mode mode)
324{
325    (VOID)clockID;
326    if ((timer == NULL) || (mode != HRTIMER_MODE_REL)) {
327        PRINT_ERR("The timer is NULL OR The mode is not HRTIMER_MODE_REL!\n");
328    }
329    return;
330}
331
332int linux_hrtimer_create(struct hrtimer *timer, union ktime time, Handler handler)
333{
334    UINT32 ret;
335
336    if ((timer == NULL) || (handler == NULL)) {
337        return -1;
338    }
339
340    ret = CheckTime(&time);
341    if (ret != LOS_OK) {
342        return -1;
343    }
344
345    timer->_softexpires.tv.sec = time.tv.sec;
346    timer->_softexpires.tv.usec = time.tv.usec;
347    timer->function = handler;
348
349    return 0;
350}
351
352STATIC struct handler_list_node *HandleNodeInit(union ktime time, struct hrtimer *timer)
353{
354    struct handler_list_node *handler = NULL;
355    handler = (struct handler_list_node *)LOS_MemAlloc(m_aucSysMem0, sizeof(struct handler_list_node));
356    if (handler == NULL) {
357        return NULL;
358    }
359    handler->_softexpires = time;
360    handler->pfnHandler = timer->function;
361    handler->pstNext = NULL;
362    return handler;
363}
364
365int linux_hrtimer_start(struct hrtimer *timer, union ktime time, const enum hrtimer_mode mode)
366{
367    struct hrtimer_list_node *hrtimer = NULL;
368    struct hrtimer_list_node *prevNode = NULL;
369    struct hrtimer_list_node *curNode = NULL;
370    struct handler_list_node *handler = NULL;
371    struct handler_list_node *prevHandler = NULL;
372    struct handler_list_node *curHandler = NULL;
373    UINT32 intSave;
374
375    if ((timer == NULL) || (mode != HRTIMER_MODE_REL)) {
376        return -1;
377    }
378
379    if (CheckTime(&time) != LOS_OK) {
380        return -1;
381    }
382
383    LOS_SpinLockSave(&g_hrtimerSpin, &intSave);
384    GetHrtimerNodePosition(timer, &prevNode, &curNode);
385    if (curNode == NULL) {
386        LOS_SpinUnlockRestore(&g_hrtimerSpin, intSave);
387        if (timer->function == NULL) {
388            return -1;
389        }
390        hrtimer = HrtimerListNodeInit(time);
391        if (hrtimer == NULL) {
392            return -1;
393        }
394        handler = HandleNodeInit(time, timer);
395        if (handler == NULL) {
396            (VOID)LOS_MemFree(m_aucSysMem0, hrtimer);
397            return -1;
398        }
399
400        LOS_SpinLockSave(&g_hrtimerSpin, &intSave);
401        HrtimerNodeAdd(hrtimer, handler);
402        LOS_SpinUnlockRestore(&g_hrtimerSpin, intSave);
403        return 0;
404    } else {
405        GetHandlerNodePosition(timer, curNode, &prevHandler, &curHandler);
406        if (ChangeNodePosition(prevNode, curNode, prevHandler, curHandler, time) == LOS_OK) {
407            LOS_SpinUnlockRestore(&g_hrtimerSpin, intSave);
408            return 1;
409        }
410    }
411
412    LOS_SpinUnlockRestore(&g_hrtimerSpin, intSave);
413    return -1;
414}
415
416int linux_hrtimer_cancel(struct hrtimer *timer)
417{
418    struct hrtimer_list_node *prevNode = NULL;
419    struct hrtimer_list_node *curNode = NULL;
420    UINT32 intSave;
421
422    if (timer == NULL) {
423        return -1;
424    }
425
426    LOS_SpinLockSave(&g_hrtimerSpin, &intSave);
427    curNode = g_hrtimerList;
428    if (curNode == NULL) {
429        LOS_SpinUnlockRestore(&g_hrtimerSpin, intSave);
430        return 0;
431    }
432
433    GetHrtimerNodePosition(timer, &prevNode, &curNode);
434
435    if (curNode == NULL) {
436        LOS_SpinUnlockRestore(&g_hrtimerSpin, intSave);
437        return 0;
438    } else if (curNode == g_hrtimerList) {
439        CancelHandlerNode(timer, curNode);
440
441        if (curNode->HandlerHead == NULL) {
442            g_hrtimerList = curNode->pstNext;
443            if (g_hrtimerList != NULL) {
444                g_hrtimerList->set_time_reload += HrtimerClockValueGet();
445                HrtimerClockStop();
446                HrtimerClockStart(g_hrtimerList->set_time_reload);
447            } else {
448                HrtimerClockStop();
449            }
450            curNode->pstNext = NULL;
451            (VOID)LOS_MemFree(m_aucSysMem0, curNode);
452        }
453    } else {
454        CancelHandlerNode(timer, curNode);
455
456        if (curNode->HandlerHead == NULL) {
457            if (curNode->pstNext == NULL) {
458                prevNode->pstNext = NULL;
459            } else {
460                prevNode->pstNext = curNode->pstNext;
461                prevNode->pstNext->set_time_reload += curNode->set_time_reload;
462            }
463            curNode->pstNext = NULL;
464            (VOID)LOS_MemFree(m_aucSysMem0, curNode);
465        }
466    }
467    LOS_SpinUnlockRestore(&g_hrtimerSpin, intSave);
468    return 1;
469}
470
471u64 linux_hrtimer_forward(struct hrtimer *timer, union ktime interval)
472{
473    struct hrtimer_list_node *prevNode = NULL;
474    struct hrtimer_list_node *curNode = NULL;
475    struct handler_list_node *prevHandler = NULL;
476    struct handler_list_node *curHandler = NULL;
477    UINT32 intSave;
478    UINT32 ret;
479
480    if (timer == NULL) {
481        return 0;
482    }
483
484    ret = CheckTime(&interval);
485    if (ret != LOS_OK) {
486        return 0;
487    }
488
489    LOS_SpinLockSave(&g_hrtimerSpin, &intSave);
490    GetHrtimerNodePosition(timer, &prevNode, &curNode);
491    if (curNode == NULL) {
492        LOS_SpinUnlockRestore(&g_hrtimerSpin, intSave);
493        return 0;
494    }
495    GetHandlerNodePosition(timer, curNode, &prevHandler, &curHandler);
496    timer->_softexpires = interval;
497    ret = ChangeNodePosition(prevNode, curNode, prevHandler, curHandler, interval);
498    LOS_SpinUnlockRestore(&g_hrtimerSpin, intSave);
499    if (ret != LOS_OK) {
500        return 0;
501    } else {
502        return (u64)((interval.tv.sec * US_PER_SECOND + interval.tv.usec) * HRTIMER_PERUS);
503    }
504}
505
506int linux_hrtimer_is_queued(struct hrtimer *timer)
507{
508    struct hrtimer_list_node *curNode = NULL;
509    struct handler_list_node *handler = NULL;
510    INT32 ret = LOS_NOK;
511    UINT32 intSave;
512
513    if (timer == NULL) {
514        return -1;
515    }
516
517    LOS_SpinLockSave(&g_hrtimerSpin, &intSave);
518    curNode = g_hrtimerList;
519    while (curNode != NULL) {
520        handler = curNode->HandlerHead;
521        while (handler != NULL) {
522            if (handler->pfnHandler == timer->function) {
523                break;
524            }
525            handler = handler->pstNext;
526        }
527
528        if ((handler != NULL) && (handler->pfnHandler == timer->function) &&
529            (handler->_softexpires.tv.sec == timer->_softexpires.tv.sec) &&
530            (handler->_softexpires.tv.usec == timer->_softexpires.tv.usec)) {
531            ret = LOS_OK;
532            break;
533        }
534        curNode = curNode->pstNext;
535    }
536    LOS_SpinUnlockRestore(&g_hrtimerSpin, intSave);
537
538    return ret;
539}
540
541UINT32 HrtimersInit(VOID)
542{
543    UINT32 ret;
544
545    g_hrtimerList = NULL;
546    /* Initialize the timer */
547    HrtimerClockInit();
548    /* Create interrupt of the timer */
549    ret = LOS_HwiCreate(NUM_HAL_INTERRUPT_HRTIMER, 0, 0, HrtimerListScan, 0);
550    if (ret != LOS_OK) {
551        return LOS_NOK;
552    }
553    HalIrqUnmask(NUM_HAL_INTERRUPT_HRTIMER);
554
555    return LOS_OK;
556}
557
558LOS_MODULE_INIT(HrtimersInit, LOS_INIT_LEVEL_PLATFROM_EARLY);
559