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