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