1 #ifndef PT_EI
2 #define PT_EI inline
3 #endif
4 
5 #include "internals.h"
6 
7 #include <l4/sys/capability>
8 #include <l4/sys/thread>
9 #include <l4/re/env>
10 #include <l4/sys/factory>
11 #include <l4/re/util/cap_alloc>
12 #include <l4/sys/kdebug.h>
13 #include <l4/sys/scheduler>
14 
15 #include <pthread-l4.h>
16 #include <errno.h>
17 #include "spinlock.h"
18 
19 #include "l4.h"
20 
21 
pthread_l4_cap(pthread_t thread_id)22 l4_cap_idx_t pthread_l4_cap(pthread_t thread_id)
23 {
24   __volatile__ pthread_descr self = thread_self();
25   pthread_handle handle = thread_handle(thread_id);
26   pthread_descr th;
27 
28   __pthread_lock(handle_to_lock(handle), self);
29   if (nonexisting_handle(handle, thread_id)) {
30     __pthread_unlock(handle_to_lock(handle));
31     return L4_INVALID_CAP;
32   }
33   l4_cap_idx_t c;
34   th = handle_to_descr(handle);
35   c = th->p_th_cap;
36   __pthread_unlock(handle_to_lock(handle));
37   return c;
38 }
39 
cb(void * arg,pthread_descr th)40 static void cb(void *arg, pthread_descr th)
41 {
42   void (*fn)(pthread_t) = (void (*)(pthread_t))arg;
43 
44   fn(th->p_tid);
45 }
46 
pthread_l4_for_each_thread(void (* fn)(pthread_t))47 void pthread_l4_for_each_thread(void (*fn)(pthread_t))
48 {
49   struct pthread_request request;
50 
51   request.req_thread = thread_self();
52   request.req_kind = REQ_FOR_EACH_THREAD;
53   request.req_args.for_each.arg = (void *)fn;
54   request.req_args.for_each.fn = cb;
55 
56   __pthread_send_manager_rq(&request, 1);
57 }
58 
59 /**
60  * This function is called during libpthread initialization. That is, the main
61  * thread is running, and it runs this function, which sets up the pthread
62  * data structures for the main thread.
63  */
__pthread_l4_initialize_main_thread(pthread_descr th)64 int __pthread_l4_initialize_main_thread(pthread_descr th)
65 {
66   L4Re::Env *env = const_cast<L4Re::Env*>(L4Re::Env::env());
67   if (!env)
68     return -L4_ENODEV;
69 
70   L4::Cap<Th_sem_cap> s(env->first_free_cap() << L4_CAP_SHIFT);
71   if (!s.is_valid() || !s.cap())
72     return -L4_ENOMEM;
73 
74   // needed by __alloc_thread_sem
75   th->p_th_cap = env->main_thread().cap();
76 
77   int err = __alloc_thread_sem(th, s);
78   if (err < 0)
79     return err;
80 
81   env->first_free_cap((s.cap() + L4_CAP_OFFSET) >> L4_CAP_SHIFT);
82 
83   th->p_thsem_cap = s.cap();
84 
85   // ideally we would try to get the current prio of the main thread, but the
86   // API doesn't currently allow us to
87   th->p_sched_policy = SCHED_L4;
88   th->p_priority = L4RE_MAIN_THREAD_PRIO;
89   th->p_affinity_mask[0] = ~0ul;
90 
91   th->p_lock = handle_to_lock(l4_utcb());
92   th->p_tid  = l4_utcb();
93   l4_utcb_tcr()->user[0] = l4_addr_t(th);
94 
95   return 0;
96 }
97 
98 
99 int __attribute__((weak)) __pthread_sched_idle_prio    = 0x01;
100 int __attribute__((weak)) __pthread_sched_other_prio   = 0x02;
101 int __attribute__((weak)) __pthread_sched_rr_prio_min  = 0x40;
102 int __attribute__((weak)) __pthread_sched_rr_prio_max  = 0xf0;
103 
104 
__pthread_setschedparam(pthread_t thread,int policy,const struct sched_param * param)105 int __pthread_setschedparam(pthread_t thread, int policy,
106                             const struct sched_param *param) throw()
107 {
108   pthread_handle handle = thread_handle(thread);
109   pthread_descr th;
110 
111   __pthread_lock(handle_to_lock(handle), NULL);
112   if (__builtin_expect (invalid_handle(handle, thread), 0)) {
113     __pthread_unlock(handle_to_lock(handle));
114     return ESRCH;
115   }
116   th = handle_to_descr(handle);
117   int prio = __pthread_l4_getprio(policy, param->sched_priority);
118   if (prio < 0)
119     {
120       __pthread_unlock(handle_to_lock(handle));
121       return EINVAL;
122     }
123 
124   th->p_sched_policy = policy;
125   th->p_priority = param->sched_priority;
126 
127     {
128       L4::Cap<L4::Thread> t(th->p_th_cap);
129       l4_sched_param_t sp = l4_sched_param(prio, 0);
130       L4Re::Env::env()->scheduler()->run_thread(t, sp);
131     }
132   __pthread_unlock(handle_to_lock(handle));
133 
134   if (__pthread_manager_request > 0)
135     __pthread_manager_adjust_prio(prio);
136 
137   return 0;
138 }
strong_alias(__pthread_setschedparam,pthread_setschedparam)139 strong_alias (__pthread_setschedparam, pthread_setschedparam)
140 
141 int __pthread_getschedparam(pthread_t thread, int *policy,
142                             struct sched_param *param) throw()
143 {
144   pthread_handle handle = thread_handle(thread);
145   int pol, prio;
146 
147   __pthread_lock(handle_to_lock(handle), NULL);
148   if (__builtin_expect (invalid_handle(handle, thread), 0)) {
149     __pthread_unlock(handle_to_lock(handle));
150     return ESRCH;
151   }
152 
153   pol = handle_to_descr(handle)->p_sched_policy;
154   prio = handle_to_descr(handle)->p_priority;
155   __pthread_unlock(handle_to_lock(handle));
156 
157   *policy = pol;
158   param->sched_priority = prio;
159 
160   return 0;
161 }
strong_alias(__pthread_getschedparam,pthread_getschedparam)162 strong_alias (__pthread_getschedparam, pthread_getschedparam)
163 
164 int pthread_setaffinity_np(pthread_t __th, size_t __cpusetsize,
165                            const cpu_set_t *__cpuset)
166 {
167   pthread_handle handle = thread_handle(__th);
168 
169   if (__cpusetsize < sizeof(__cpuset->__bits[0]))
170     return EINVAL;
171 
172   __pthread_lock(handle_to_lock(handle), NULL);
173 
174   pthread_descr th = handle_to_descr(handle);
175 
176   L4::Cap<L4::Thread> t(th->p_th_cap);
177   l4_sched_param_t sp = l4_sched_param(th->p_priority, 0);
178   static_assert(sizeof(__cpuset->__bits[0]) == sizeof(l4_umword_t),
179                 "Size mismatch");
180   th->p_affinity_mask[0] = __cpuset->__bits[0];
181   sp.affinity = l4_sched_cpu_set(0, 0, __cpuset->__bits[0]);
182   int e = l4_error(L4Re::Env::env()->scheduler()->run_thread(t, sp));
183 
184   __pthread_unlock(handle_to_lock(handle));
185 
186   return -e;
187 }
188 
189 int
pthread_getaffinity_np(pthread_t th,size_t cpusetsize,cpu_set_t * cpuset)190 pthread_getaffinity_np(pthread_t th, size_t cpusetsize, cpu_set_t *cpuset)
191 {
192   pthread_handle handle = thread_handle(th);
193 
194   if (cpusetsize < sizeof(cpuset->__bits[0]))
195     return EINVAL;
196 
197   __pthread_lock(handle_to_lock(handle), NULL);
198   cpuset->__bits[0] = handle_to_descr(handle)->p_affinity_mask[0];
199   __pthread_unlock(handle_to_lock(handle));
200 
201   return 0;
202 }
203 
pthread_l4_start(pthread_t th,void * (* func)(void *),void * arg)204 int pthread_l4_start(pthread_t th, void *(*func)(void *), void *arg)
205 {
206   pthread_handle handle = thread_handle(th);
207   __pthread_lock(handle_to_lock(handle), NULL);
208 
209   if (nonexisting_handle(handle, th))
210     {
211       __pthread_unlock(handle_to_lock(handle));
212       return -EINVAL;
213     }
214 
215   pthread_descr thread = handle_to_descr(handle);
216   if (thread->p_start_args.start_routine)
217     {
218       __pthread_unlock(handle_to_lock(handle));
219       return -EBUSY;
220     }
221 
222   thread->p_start_args.start_routine = func;
223   thread->p_start_args.arg = arg;
224 
225   int prio;
226   if (thread->p_sched_policy < 0)
227     /* Default scheduling required, but thread manager runs in realtime
228        scheduling: switch new thread to SCHED_OTHER policy */
229     prio = __pthread_l4_getprio(SCHED_OTHER, 0);
230   else
231     prio = __pthread_l4_getprio(thread->p_sched_policy,
232                                 thread->p_priority);
233 
234   l4_sched_param_t sp = l4_sched_param(prio >= 0 ? prio : 2);
235   int res = l4_error(L4Re::Env::env()->scheduler()
236     ->run_thread(L4::Cap<L4::Thread>(thread->p_th_cap), sp));
237 
238   __pthread_unlock(handle_to_lock(handle));
239   return res;
240 }
241 
242