c6e6948ffdc28ee36cbe26e7c836386efb803a60
[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         syscall(SYS_yield,0,0,0,0,0);
98 }
99
100 size_t hart_max_harts()
101 {
102         return procinfo.max_harts < HART_MAX_MAX_HARTS ? procinfo.max_harts : HART_MAX_MAX_HARTS;
103 }
104
105 // MCS locks!!
106 void hart_lock_init(hart_lock_t* lock)
107 {
108         memset(lock,0,sizeof(hart_lock_t));
109 }
110
111 static inline hart_lock_qnode_t* hart_qnode_swap(hart_lock_qnode_t** addr, hart_lock_qnode_t* val)
112 {
113         return (hart_lock_qnode_t*)hart_swap((size_t*)addr,(size_t)val);
114 }
115
116 void hart_lock_lock(hart_lock_t* lock)
117 {
118         hart_lock_qnode_t* qnode = &lock->qnode[hart_self()];
119         qnode->next = 0;
120         hart_lock_qnode_t* predecessor = hart_qnode_swap(&lock->lock,qnode);
121         if(predecessor)
122         {
123                 qnode->locked = 1;
124                 predecessor->next = qnode;
125                 while(qnode->locked);
126         }
127 }
128
129 void hart_lock_unlock(hart_lock_t* lock)
130 {
131         hart_lock_qnode_t* qnode = &lock->qnode[hart_self()];
132         if(qnode->next == 0)
133         {
134                 hart_lock_qnode_t* old_tail = hart_qnode_swap(&lock->lock,0);
135                 if(old_tail == qnode)
136                         return;
137
138                 hart_lock_qnode_t* usurper = hart_qnode_swap(&lock->lock,old_tail);
139                 while(qnode->next == 0);
140                 if(usurper)
141                         usurper->next = qnode->next;
142                 else
143                         qnode->next->locked = 0;
144         }
145         else
146                 qnode->next->locked = 0;
147 }
148
149 // MCS dissemination barrier!
150 error_t hart_barrier_init(hart_barrier_t* b, size_t np)
151 {
152         if(np > hart_max_harts())
153                 return -1;
154         b->allnodes = (hart_dissem_flags_t*)malloc(np*sizeof(hart_dissem_flags_t));
155         memset(b->allnodes,0,np*sizeof(hart_dissem_flags_t));
156         b->nprocs = np;
157
158         b->logp = (np & (np-1)) != 0;
159         while(np >>= 1)
160                 b->logp++;
161
162         size_t i,k;
163         for(i = 0; i < b->nprocs; i++)
164         {
165                 b->allnodes[i].parity = 0;
166                 b->allnodes[i].sense = 1;
167
168                 for(k = 0; k < b->logp; k++)
169                 {
170                         size_t j = (i+(1<<k)) % b->nprocs;
171                         b->allnodes[i].partnerflags[0][k] = &b->allnodes[j].myflags[0][k];
172                         b->allnodes[i].partnerflags[1][k] = &b->allnodes[j].myflags[1][k];
173                 } 
174         }
175
176         return 0;
177 }
178
179 void hart_barrier_wait(hart_barrier_t* b, size_t pid)
180 {
181         hart_dissem_flags_t* localflags = &b->allnodes[pid];
182         size_t i;
183         for(i = 0; i < b->logp; i++)
184         {
185                 *localflags->partnerflags[localflags->parity][i] = localflags->sense;
186                 while(localflags->myflags[localflags->parity][i] != localflags->sense);
187         }
188         if(localflags->parity)
189                 localflags->sense = 1-localflags->sense;
190         localflags->parity = 1-localflags->parity;
191 }
192
193 size_t
194 hart_self()
195 {
196         // defined in ros/arch/hart.h
197         return __hart_self();
198 }