Removed references to intptr_t
[akaros.git] / lib / parlib / hart.c
1 #include <stdbool.h>
2 #include <errno.h>
3 #include <hart.h>
4 #include <parlib.h>
5 #include <unistd.h>
6 #include <stdlib.h>
7
8 static size_t _current_harts = 1;
9 static hart_lock_t _hart_lock = HART_LOCK_INIT;
10
11 extern void** hart_thread_control_blocks;
12
13 static void hart_free_tls(int id)
14 {
15         extern void _dl_deallocate_tls (void *tcb, bool dealloc_tcb);
16         if(hart_thread_control_blocks[id])
17         {
18                 _dl_deallocate_tls(hart_thread_control_blocks[id],true);
19                 hart_thread_control_blocks[id] = 0;
20         }
21 }
22
23 static int hart_allocate_tls(int id)
24 {
25         extern void *_dl_allocate_tls (void *mem);
26         // instead of freeing any old tls that may be present, and then
27         // reallocating it, we could instead just reinitialize it.
28         hart_free_tls(id);
29         if((hart_thread_control_blocks[id] = _dl_allocate_tls(NULL)) == NULL)
30         {
31                 errno = ENOMEM;
32                 return -1;
33         }
34         return 0;
35 }
36
37 #define HART_STACK_SIZE (32*1024)
38
39 static void hart_free_stack(int id)
40 {
41         // don't actually free stacks
42 }
43
44 static int hart_allocate_stack(int id)
45 {
46         if(__procdata.stack_pointers[id])
47                 return 0; // reuse old stack
48
49         if(!(__procdata.stack_pointers[id] = (uintptr_t)malloc(HART_STACK_SIZE)))
50         {
51                 errno = ENOMEM;
52                 return -1;
53         }
54         return 0;
55 }
56
57 static int hart_init()
58 {
59         static int initialized = 0;
60         if(initialized)
61                 return 0;
62
63         hart_thread_control_blocks = (void**)calloc(hart_max_harts(),sizeof(void*));
64
65         if(!hart_thread_control_blocks)
66         {
67                 free(hart_thread_control_blocks);
68                 errno = ENOMEM;
69                 return -1;
70         }
71
72         initialized = 1;
73         return 0;
74 }
75
76 int hart_request(size_t k)
77 {
78         int ret = -1;
79         size_t i,j;
80
81         if(hart_init() < 0)
82                 return -1;
83
84         hart_lock_lock(&_hart_lock);
85
86         if(k < 0 || _current_harts+k > hart_max_harts())
87         {
88                 errno = EAGAIN;
89                 goto out;
90         }
91
92         for(i = _current_harts, j = 0; i < _current_harts+k; i++, j++)
93         {
94                 if(hart_allocate_stack(i) || hart_allocate_tls(i))
95                         goto fail;
96         }
97
98         if((ret = sys_resource_req(0,_current_harts+k,0)) == 0)
99         {
100                 _current_harts += k;
101                 goto out;
102         }
103
104 fail:
105         for(i = _current_harts; i < _current_harts+k; i++)
106         {
107                 hart_free_tls(i);
108                 hart_free_stack(i);
109         }
110
111 out:
112         hart_lock_unlock(&_hart_lock);
113         return ret;
114 }
115
116 void hart_yield()
117 {
118         int id = hart_self();
119
120         hart_lock_lock(&_hart_lock);
121         _current_harts--;
122         if(_current_harts == 0)
123                 exit(0);
124         hart_lock_unlock(&_hart_lock);
125
126         sys_yield();
127 }
128
129 size_t hart_max_harts()
130 {
131         return __procinfo.max_harts < HART_MAX_MAX_HARTS ? __procinfo.max_harts : HART_MAX_MAX_HARTS;
132 }
133
134 size_t hart_current_harts()
135 {
136         return _current_harts;
137 }
138
139 // MCS locks!!
140 void hart_lock_init(hart_lock_t* lock)
141 {
142         memset(lock,0,sizeof(hart_lock_t));
143 }
144
145 static inline hart_lock_qnode_t* hart_qnode_swap(hart_lock_qnode_t** addr, hart_lock_qnode_t* val)
146 {
147         return (hart_lock_qnode_t*)hart_swap((int*)addr,(int)val);
148 }
149
150 void hart_lock_lock(hart_lock_t* lock)
151 {
152         hart_lock_qnode_t* qnode = &lock->qnode[hart_self()];
153         qnode->next = 0;
154         hart_lock_qnode_t* predecessor = hart_qnode_swap(&lock->lock,qnode);
155         if(predecessor)
156         {
157                 qnode->locked = 1;
158                 predecessor->next = qnode;
159                 while(qnode->locked);
160         }
161 }
162
163 void hart_lock_unlock(hart_lock_t* lock)
164 {
165         hart_lock_qnode_t* qnode = &lock->qnode[hart_self()];
166         if(qnode->next == 0)
167         {
168                 hart_lock_qnode_t* old_tail = hart_qnode_swap(&lock->lock,0);
169                 if(old_tail == qnode)
170                         return;
171
172                 hart_lock_qnode_t* usurper = hart_qnode_swap(&lock->lock,old_tail);
173                 while(qnode->next == 0);
174                 if(usurper)
175                         usurper->next = qnode->next;
176                 else
177                         qnode->next->locked = 0;
178         }
179         else
180                 qnode->next->locked = 0;
181 }
182
183 // MCS dissemination barrier!
184 int hart_barrier_init(hart_barrier_t* b, size_t np)
185 {
186         if(np > hart_max_harts())
187                 return -1;
188         b->allnodes = (hart_dissem_flags_t*)malloc(np*sizeof(hart_dissem_flags_t));
189         memset(b->allnodes,0,np*sizeof(hart_dissem_flags_t));
190         b->nprocs = np;
191
192         b->logp = (np & (np-1)) != 0;
193         while(np >>= 1)
194                 b->logp++;
195
196         size_t i,k;
197         for(i = 0; i < b->nprocs; i++)
198         {
199                 b->allnodes[i].parity = 0;
200                 b->allnodes[i].sense = 1;
201
202                 for(k = 0; k < b->logp; k++)
203                 {
204                         size_t j = (i+(1<<k)) % b->nprocs;
205                         b->allnodes[i].partnerflags[0][k] = &b->allnodes[j].myflags[0][k];
206                         b->allnodes[i].partnerflags[1][k] = &b->allnodes[j].myflags[1][k];
207                 } 
208         }
209
210         return 0;
211 }
212
213 void hart_barrier_wait(hart_barrier_t* b, size_t pid)
214 {
215         hart_dissem_flags_t* localflags = &b->allnodes[pid];
216         size_t i;
217         for(i = 0; i < b->logp; i++)
218         {
219                 *localflags->partnerflags[localflags->parity][i] = localflags->sense;
220                 while(localflags->myflags[localflags->parity][i] != localflags->sense);
221         }
222         if(localflags->parity)
223                 localflags->sense = 1-localflags->sense;
224         localflags->parity = 1-localflags->parity;
225 }
226
227 int
228 hart_self()
229 {
230         // defined in ros/arch/hart.h
231         return __hart_self();
232 }
233
234 int
235 hart_swap(int* addr, int val)
236 {
237         return __hart_swap(addr,val);
238 }
239
240 void
241 hart_relax()
242 {
243         __hart_relax();
244 }
245