fixed pthread_barrier_wait bug
[akaros.git] / user / parlib / src / hart.c
1 #include <hart.h>
2 #include <string.h>
3 #include <stdlib.h>
4 #include <unistd.h>
5 #include <parlib.h>
6
7 // TODO: HART_ALLOCATE_STACKS should be disposed of by means of a better ABI.
8
9 static size_t _hart_current_harts = 1;
10 static hart_lock_t _hart_lock = HART_LOCK_INIT;
11
12 static void hart_abort(const char* str)
13 {
14         write(2,str,strlen(str));
15         exit(-1);
16 }
17
18 #pragma weak hart_entry
19 void hart_entry()
20 {
21         hart_abort("You should write your own damn hart_entry()!\n");
22 }
23
24 static void _hart_init()
25 {
26         static int initialized = 0;
27         if(initialized)
28                 return;
29
30         initialized = 1;
31
32         #ifdef HART_ALLOCATE_STACKS
33         extern void** stack_ptr_array;
34         stack_ptr_array = (void**)malloc(hart_max_harts()*sizeof(void*));
35         memset(stack_ptr_array,0,hart_max_harts()*sizeof(void*));
36         if(stack_ptr_array == NULL)
37                 hart_abort("Harts initialization ran out of memory!\n");
38         #endif
39 }
40
41 error_t hart_request(size_t k)
42 {
43         size_t i,j;
44         const int user_stack_size = 1024*1024;
45
46         #ifdef HART_ALLOCATE_STACKS
47         extern void** stack_ptr_array;
48         #endif
49
50         _hart_init();
51
52         hart_lock_lock(&_hart_lock);
53
54         if(k < 0 || _hart_current_harts+k > hart_max_harts())
55                 return -1;
56
57         #ifdef HART_ALLOCATE_STACKS
58         for(i = _hart_current_harts; i < _hart_current_harts+k; i++)
59         {
60                 char* stack = (char*)malloc(user_stack_size);
61                 if(stack == NULL)
62                 {
63                         for(j = _hart_current_harts; j < i; j++)
64                         {
65                                 free(stack_ptr_array[j]);
66                                 stack_ptr_array[j] = 0;
67                         }
68                         hart_lock_unlock(&_hart_lock);
69                         return -ENOMEM;
70                 }
71                 stack_ptr_array[i] = stack + user_stack_size;
72         }
73         #endif
74
75         error_t ret;
76         if((ret = sys_resource_req(0,_hart_current_harts+k,0)) == 0)
77         {
78                 _hart_current_harts += k;
79                 hart_lock_unlock(&_hart_lock);
80                 return 0;
81         }
82
83         #ifdef HART_ALLOCATE_STACKS
84         for(i = _hart_current_harts; i < _hart_current_harts+k; i++)
85         {
86                 free(stack_ptr_array[i]);
87                 stack_ptr_array[i] = 0;
88         }
89         #endif
90
91         hart_lock_unlock(&_hart_lock);
92         return ret;
93 }
94
95 void hart_yield()
96 {
97         hart_lock_lock(&_hart_lock);
98         _hart_current_harts--;
99         hart_lock_unlock(&_hart_lock);
100         syscall(SYS_yield,0,0,0,0,0);
101 }
102
103 size_t hart_max_harts()
104 {
105         return procinfo.max_harts < HART_MAX_MAX_HARTS ? procinfo.max_harts : HART_MAX_MAX_HARTS;
106 }
107
108 size_t hart_current_harts()
109 {
110         return _hart_current_harts;
111 }
112
113 // MCS locks!!
114 void hart_lock_init(hart_lock_t* lock)
115 {
116         memset(lock,0,sizeof(hart_lock_t));
117 }
118
119 static inline hart_lock_qnode_t* hart_qnode_swap(hart_lock_qnode_t** addr, hart_lock_qnode_t* val)
120 {
121         return (hart_lock_qnode_t*)hart_swap((size_t*)addr,(size_t)val);
122 }
123
124 void hart_lock_lock(hart_lock_t* lock)
125 {
126         hart_lock_qnode_t* qnode = &lock->qnode[hart_self()];
127         qnode->next = 0;
128         hart_lock_qnode_t* predecessor = hart_qnode_swap(&lock->lock,qnode);
129         if(predecessor)
130         {
131                 qnode->locked = 1;
132                 predecessor->next = qnode;
133                 while(qnode->locked);
134         }
135 }
136
137 void hart_lock_unlock(hart_lock_t* lock)
138 {
139         hart_lock_qnode_t* qnode = &lock->qnode[hart_self()];
140         if(qnode->next == 0)
141         {
142                 hart_lock_qnode_t* old_tail = hart_qnode_swap(&lock->lock,0);
143                 if(old_tail == qnode)
144                         return;
145
146                 hart_lock_qnode_t* usurper = hart_qnode_swap(&lock->lock,old_tail);
147                 while(qnode->next == 0);
148                 if(usurper)
149                         usurper->next = qnode->next;
150                 else
151                         qnode->next->locked = 0;
152         }
153         else
154                 qnode->next->locked = 0;
155 }
156
157 // MCS dissemination barrier!
158 error_t hart_barrier_init(hart_barrier_t* b, size_t np)
159 {
160         if(np > hart_max_harts())
161                 return -1;
162         b->allnodes = (hart_dissem_flags_t*)malloc(np*sizeof(hart_dissem_flags_t));
163         memset(b->allnodes,0,np*sizeof(hart_dissem_flags_t));
164         b->nprocs = np;
165
166         b->logp = (np & (np-1)) != 0;
167         while(np >>= 1)
168                 b->logp++;
169
170         size_t i,k;
171         for(i = 0; i < b->nprocs; i++)
172         {
173                 b->allnodes[i].parity = 0;
174                 b->allnodes[i].sense = 1;
175
176                 for(k = 0; k < b->logp; k++)
177                 {
178                         size_t j = (i+(1<<k)) % b->nprocs;
179                         b->allnodes[i].partnerflags[0][k] = &b->allnodes[j].myflags[0][k];
180                         b->allnodes[i].partnerflags[1][k] = &b->allnodes[j].myflags[1][k];
181                 } 
182         }
183
184         return 0;
185 }
186
187 void hart_barrier_wait(hart_barrier_t* b, size_t pid)
188 {
189         hart_dissem_flags_t* localflags = &b->allnodes[pid];
190         size_t i;
191         for(i = 0; i < b->logp; i++)
192         {
193                 *localflags->partnerflags[localflags->parity][i] = localflags->sense;
194                 while(localflags->myflags[localflags->parity][i] != localflags->sense);
195         }
196         if(localflags->parity)
197                 localflags->sense = 1-localflags->sense;
198         localflags->parity = 1-localflags->parity;
199 }
200
201 size_t
202 hart_self()
203 {
204         // defined in ros/arch/hart.h
205         return __hart_self();
206 }