1 | /* |
---|
2 | * Copyright (c) 2016, 2017 embedded brains GmbH |
---|
3 | * |
---|
4 | * The license and distribution terms for this file may be |
---|
5 | * found in the file LICENSE in this distribution or at |
---|
6 | * http://www.rtems.org/license/LICENSE. |
---|
7 | */ |
---|
8 | |
---|
9 | #if HAVE_CONFIG_H |
---|
10 | #include "config.h" |
---|
11 | #endif |
---|
12 | |
---|
13 | #include <rtems/score/threadqimpl.h> |
---|
14 | #include <rtems/score/threadimpl.h> |
---|
15 | #include <rtems/score/watchdogimpl.h> |
---|
16 | |
---|
17 | void _Thread_queue_Add_timeout_ticks( |
---|
18 | Thread_queue_Queue *queue, |
---|
19 | Thread_Control *the_thread, |
---|
20 | Per_CPU_Control *cpu_self, |
---|
21 | Thread_queue_Context *queue_context |
---|
22 | ) |
---|
23 | { |
---|
24 | Watchdog_Interval ticks; |
---|
25 | |
---|
26 | ticks = queue_context->Timeout.ticks; |
---|
27 | |
---|
28 | if ( ticks != WATCHDOG_NO_TIMEOUT ) { |
---|
29 | _Thread_Add_timeout_ticks( |
---|
30 | the_thread, |
---|
31 | cpu_self, |
---|
32 | queue_context->Timeout.ticks |
---|
33 | ); |
---|
34 | } |
---|
35 | } |
---|
36 | |
---|
37 | static bool _Thread_queue_Lazy_insert_monotonic_timespec( |
---|
38 | Thread_Control *the_thread, |
---|
39 | Per_CPU_Control *cpu_self, |
---|
40 | const struct timespec *abstime |
---|
41 | ) |
---|
42 | { |
---|
43 | uint64_t expire; |
---|
44 | ISR_lock_Context lock_context; |
---|
45 | bool insert; |
---|
46 | |
---|
47 | if ( abstime->tv_sec < 0 ) { |
---|
48 | expire = 0; |
---|
49 | } else if ( _Watchdog_Is_far_future_monotonic_timespec( abstime ) ) { |
---|
50 | expire = WATCHDOG_MAXIMUM_TICKS; |
---|
51 | } else { |
---|
52 | expire = _Watchdog_Monotonic_from_timespec( abstime ); |
---|
53 | } |
---|
54 | |
---|
55 | _ISR_lock_ISR_disable_and_acquire( |
---|
56 | &the_thread->Timer.Lock, |
---|
57 | &lock_context |
---|
58 | ); |
---|
59 | |
---|
60 | the_thread->Timer.header = |
---|
61 | &cpu_self->Watchdog.Header[ PER_CPU_WATCHDOG_MONOTONIC ]; |
---|
62 | the_thread->Timer.Watchdog.routine = _Thread_Timeout; |
---|
63 | insert = _Watchdog_Per_CPU_lazy_insert_monotonic( |
---|
64 | &the_thread->Timer.Watchdog, |
---|
65 | cpu_self, |
---|
66 | expire |
---|
67 | ); |
---|
68 | |
---|
69 | _ISR_lock_Release_and_ISR_enable( |
---|
70 | &the_thread->Timer.Lock, |
---|
71 | &lock_context |
---|
72 | ); |
---|
73 | return insert; |
---|
74 | } |
---|
75 | |
---|
76 | void _Thread_queue_Add_timeout_monotonic_timespec( |
---|
77 | Thread_queue_Queue *queue, |
---|
78 | Thread_Control *the_thread, |
---|
79 | Per_CPU_Control *cpu_self, |
---|
80 | Thread_queue_Context *queue_context |
---|
81 | ) |
---|
82 | { |
---|
83 | const struct timespec *abstime; |
---|
84 | |
---|
85 | abstime = queue_context->Timeout.arg; |
---|
86 | |
---|
87 | if ( _Watchdog_Is_valid_timespec( abstime ) ) { |
---|
88 | if ( |
---|
89 | !_Thread_queue_Lazy_insert_monotonic_timespec( |
---|
90 | the_thread, |
---|
91 | cpu_self, |
---|
92 | abstime |
---|
93 | ) |
---|
94 | ) { |
---|
95 | _Thread_Continue( the_thread, STATUS_TIMEOUT ); |
---|
96 | } |
---|
97 | } else { |
---|
98 | _Thread_Continue( the_thread, STATUS_INVALID_NUMBER ); |
---|
99 | } |
---|
100 | } |
---|
101 | |
---|
102 | void _Thread_queue_Add_timeout_realtime_timespec( |
---|
103 | Thread_queue_Queue *queue, |
---|
104 | Thread_Control *the_thread, |
---|
105 | Per_CPU_Control *cpu_self, |
---|
106 | Thread_queue_Context *queue_context |
---|
107 | ) |
---|
108 | { |
---|
109 | const struct timespec *abstime; |
---|
110 | |
---|
111 | abstime = queue_context->Timeout.arg; |
---|
112 | |
---|
113 | if ( _Watchdog_Is_valid_timespec( abstime ) ) { |
---|
114 | uint64_t expire; |
---|
115 | struct timespec now; |
---|
116 | |
---|
117 | if ( abstime->tv_sec < 0 ) { |
---|
118 | expire = 0; |
---|
119 | } else if ( _Watchdog_Is_far_future_realtime_timespec( abstime ) ) { |
---|
120 | expire = WATCHDOG_MAXIMUM_TICKS; |
---|
121 | } else { |
---|
122 | expire = _Watchdog_Realtime_from_timespec( abstime ); |
---|
123 | } |
---|
124 | |
---|
125 | _Timecounter_Getnanotime( &now ); |
---|
126 | |
---|
127 | if ( expire > _Watchdog_Realtime_from_timespec( &now ) ) { |
---|
128 | ISR_lock_Context lock_context; |
---|
129 | |
---|
130 | _ISR_lock_ISR_disable_and_acquire( |
---|
131 | &the_thread->Timer.Lock, |
---|
132 | &lock_context |
---|
133 | ); |
---|
134 | |
---|
135 | the_thread->Timer.header = |
---|
136 | &cpu_self->Watchdog.Header[ PER_CPU_WATCHDOG_REALTIME ]; |
---|
137 | the_thread->Timer.Watchdog.routine = _Thread_Timeout; |
---|
138 | _Watchdog_Per_CPU_insert_realtime( |
---|
139 | &the_thread->Timer.Watchdog, |
---|
140 | cpu_self, |
---|
141 | expire |
---|
142 | ); |
---|
143 | |
---|
144 | _ISR_lock_Release_and_ISR_enable( |
---|
145 | &the_thread->Timer.Lock, |
---|
146 | &lock_context |
---|
147 | ); |
---|
148 | } else { |
---|
149 | _Thread_Continue( the_thread, STATUS_TIMEOUT ); |
---|
150 | } |
---|
151 | } else { |
---|
152 | _Thread_Continue( the_thread, STATUS_INVALID_NUMBER ); |
---|
153 | } |
---|
154 | } |
---|