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