Kernel - So... should remember to initialise structures
[tpg/acess2.git] / Kernel / arch / x86 / lib.c
1 /*
2  * AcessOS Microkernel Version
3  * lib.c
4  */
5 #include <acess.h>
6 #include <threads.h>
7
8 #define TRACE_LOCKS     0
9
10 #if TRACE_LOCKS
11 struct sShortSpinlock   glDebug_Lock;
12 #endif
13
14 extern int      GetCPUNum(void);
15
16 // === CODE ===
17 /**
18  * \brief Determine if a short spinlock is locked
19  * \param Lock  Lock pointer
20  */
21 int IS_LOCKED(struct sShortSpinlock *Lock)
22 {
23         return !!Lock->Lock;
24 }
25
26 /**
27  * \brief Check if the current CPU has the lock
28  * \param Lock  Lock pointer
29  */
30 int CPU_HAS_LOCK(struct sShortSpinlock *Lock)
31 {
32         #if STACKED_LOCKS == 1
33         return Lock->Lock == GetCPUNum() + 1;
34         #elif STACKED_LOCKS == 2
35         return Lock->Lock == Proc_GetCurThread();
36         #else
37         return 0;
38         #endif
39 }
40
41 /**
42  * \brief Acquire a Short Spinlock
43  * \param Lock  Lock pointer
44  * 
45  * This type of mutex should only be used for very short sections of code,
46  * or in places where a Mutex_* would be overkill, such as appending
47  * an element to linked list (usually two assignement lines in C)
48  * 
49  * \note This type of lock halts interrupts, so ensure that no timing
50  * functions are called while it is held. As a matter of fact, spend as
51  * little time as possible with this lock held
52  * \note If \a STACKED_LOCKS is set, this type of spinlock can be nested
53  */
54 void SHORTLOCK(struct sShortSpinlock *Lock)
55 {
56          int    v = 1;
57         #if LOCK_DISABLE_INTS
58          int    IF;
59         #endif
60         #if STACKED_LOCKS == 1
61          int    cpu = GetCPUNum() + 1;
62         #elif STACKED_LOCKS == 2
63         void    *thread = Proc_GetCurThread();
64         #endif
65         
66         #if LOCK_DISABLE_INTS
67         // Save interrupt state
68         __ASM__ ("pushf;\n\tpop %0" : "=r"(IF));
69         IF &= 0x200;    // AND out all but the interrupt flag
70         #endif
71         
72         #if STACKED_LOCKS == 1
73         if( Lock->Lock == cpu ) {
74                 Lock->Depth ++;
75                 return ;
76         }
77         #elif STACKED_LOCKS == 2
78         if( Lock->Lock == thread ) {
79                 Lock->Depth ++;
80                 return ;
81         }
82         #endif
83         
84         // Wait for another CPU to release
85         while(v) {
86                 // CMPXCHG:
87                 //  If r/m32 == EAX, set ZF and set r/m32 = r32
88                 //  Else, clear ZF and set EAX = r/m32
89                 #if STACKED_LOCKS == 1
90                 __ASM__("lock cmpxchgl %2, (%3)"
91                         : "=a"(v)
92                         : "a"(0), "r"(cpu), "r"(&Lock->Lock)
93                         );
94                 #elif STACKED_LOCKS == 2
95                 __ASM__("lock cmpxchgl %2, (%3)"
96                         : "=a"(v)
97                         : "a"(0), "r"(thread), "r"(&Lock->Lock)
98                         );
99                 #else
100                 __ASM__("xchgl %%eax, (%%edi)":"=a"(v):"a"(1),"D"(&Lock->Lock));
101                 #endif
102                 
103                 #if LOCK_DISABLE_INTS
104                 if( v ) __ASM__("sti"); // Re-enable interrupts
105                 #endif
106         }
107         
108         #if LOCK_DISABLE_INTS
109         __ASM__("cli");
110         Lock->IF = IF;
111         #endif
112         
113         #if TRACE_LOCKS
114         if( Lock != &glDebug_Lock )
115         {
116                 //Log_Log("LOCK", "%p locked by %p", Lock, __builtin_return_address(0));
117                 LogF("Lock %p locked by %p\n", Lock, __builtin_return_address(0));
118         }
119         #endif
120 }
121 /**
122  * \brief Release a short lock
123  * \param Lock  Lock pointer
124  */
125 void SHORTREL(struct sShortSpinlock *Lock)
126 {       
127         #if STACKED_LOCKS
128         if( Lock->Depth ) {
129                 Lock->Depth --;
130                 return ;
131         }
132         #endif
133         
134         #if TRACE_LOCKS
135         if( Lock != &glDebug_Lock )
136         {
137                 //Log_Log("LOCK", "%p released by %p", Lock, __builtin_return_address(0));
138                 LogF("Lock %p released by %p\n", Lock, __builtin_return_address(0));
139         }
140         #endif
141         
142         #if LOCK_DISABLE_INTS
143         // Lock->IF can change anytime once Lock->Lock is zeroed
144         if(Lock->IF) {
145                 Lock->Lock = 0;
146                 __ASM__ ("sti");
147         }
148         else {
149                 Lock->Lock = 0;
150         }
151         #else
152         Lock->Lock = 0;
153         #endif
154 }
155
156 // === IO Commands ===
157 void outb(Uint16 Port, Uint8 Data)
158 {
159         __asm__ __volatile__ ("outb %%al, %%dx"::"d"(Port),"a"(Data));
160 }
161 void outw(Uint16 Port, Uint16 Data)
162 {
163         __asm__ __volatile__ ("outw %%ax, %%dx"::"d"(Port),"a"(Data));
164 }
165 void outd(Uint16 Port, Uint32 Data)
166 {
167         __asm__ __volatile__ ("outl %%eax, %%dx"::"d"(Port),"a"(Data));
168 }
169 Uint8 inb(Uint16 Port)
170 {
171         Uint8   ret;
172         __asm__ __volatile__ ("inb %%dx, %%al":"=a"(ret):"d"(Port));
173         return ret;
174 }
175 Uint16 inw(Uint16 Port)
176 {
177         Uint16  ret;
178         __asm__ __volatile__ ("inw %%dx, %%ax":"=a"(ret):"d"(Port));
179         return ret;
180 }
181 Uint32 ind(Uint16 Port)
182 {
183         Uint32  ret;
184         __asm__ __volatile__ ("inl %%dx, %%eax":"=a"(ret):"d"(Port));
185         return ret;
186 }
187
188 /**
189  * \fn void *memset(void *Dest, int Val, size_t Num)
190  * \brief Do a byte granuality set of Dest
191  */
192 void *memset(void *Dest, int Val, size_t Num)
193 {
194         Uint32  val = Val&0xFF;
195         val |= val << 8;
196         val |= val << 16;
197         __asm__ __volatile__ (
198                 "rep stosl;\n\t"
199                 "mov %3, %%ecx;\n\t"
200                 "rep stosb"
201                 :: "D" (Dest), "a" (val), "c" (Num/4), "r" (Num&3));
202         return Dest;
203 }
204 /**
205  * \brief Set double words
206  */
207 void *memsetd(void *Dest, Uint32 Val, size_t Num)
208 {
209         __asm__ __volatile__ ("rep stosl" :: "D" (Dest), "a" (Val), "c" (Num));
210         return Dest;
211 }
212
213 /**
214  * \fn int memcmp(const void *m1, const void *m2, size_t Num)
215  * \brief Compare two pieces of memory
216  */
217 int memcmp(const void *m1, const void *m2, size_t Num)
218 {
219         if( Num == 0 )  return 0;       // No bytes are always identical
220         
221         while(Num--)
222         {
223                 if(*(Uint8*)m1 != *(Uint8*)m2)
224                         return *(Uint8*)m1 - *(Uint8*)m2;
225                 m1 ++;
226                 m2 ++;
227         }
228         return 0;
229 }
230
231 /**
232  * \fn void *memcpy(void *Dest, const void *Src, size_t Num)
233  * \brief Copy \a Num bytes from \a Src to \a Dest
234  */
235 void *memcpy(void *Dest, const void *Src, size_t Num)
236 {
237         if( ((Uint)Dest & 3) || ((Uint)Src & 3) )
238                 __asm__ __volatile__ ("rep movsb" :: "D" (Dest), "S" (Src), "c" (Num));
239         else {
240                 __asm__ __volatile__ (
241                         "rep movsl;\n\t"
242                         "mov %3, %%ecx;\n\t"
243                         "rep movsb"
244                         :: "D" (Dest), "S" (Src), "c" (Num/4), "r" (Num&3));
245         }
246         return Dest;
247 }
248 /**
249  * \fn void *memcpyd(void *Dest, const void *Src, size_t Num)
250  * \brief Copy \a Num DWORDs from \a Src to \a Dest
251  */
252 void *memcpyd(void *Dest, const void *Src, size_t Num)
253 {
254         __asm__ __volatile__ ("rep movsl" :: "D" (Dest), "S" (Src), "c" (Num));
255         return Dest;
256 }
257
258 /**
259  * \fn Uint64 __udivdi3(Uint64 Num, Uint64 Den)
260  * \brief Divide two 64-bit integers
261  */
262 Uint64 __udivdi3(Uint64 Num, Uint64 Den)
263 {
264         Uint64  P[2];
265         Uint64  q = 0;
266          int    i;
267         
268         if(Den == 0)    __asm__ __volatile__ ("int $0x0");
269         // Common speedups
270         if(Num <= 0xFFFFFFFF && Den <= 0xFFFFFFFF)
271                 return (Uint32)Num / (Uint32)Den;
272         if(Den == 1)    return Num;
273         if(Den == 2)    return Num >> 1;        // Speed Hacks
274         if(Den == 4)    return Num >> 2;        // Speed Hacks
275         if(Den == 8)    return Num >> 3;        // Speed Hacks
276         if(Den == 16)   return Num >> 4;        // Speed Hacks
277         if(Den == 32)   return Num >> 5;        // Speed Hacks
278         if(Den == 1024) return Num >> 10;       // Speed Hacks
279         if(Den == 2048) return Num >> 11;       // Speed Hacks
280         if(Den == 4096) return Num >> 12;
281         if(Num < Den)   return 0;
282         if(Num < Den*2) return 1;
283         if(Num == Den*2)        return 2;
284         
285         // Restoring division, from wikipedia
286         // http://en.wikipedia.org/wiki/Division_(digital)
287         P[0] = Num;     P[1] = 0;
288         for( i = 64; i--; )
289         {
290                 // P <<= 1;
291                 P[1] = (P[1] << 1) | (P[0] >> 63);
292                 P[0] = P[0] << 1;
293                 
294                 // P -= Den << 64
295                 P[1] -= Den;
296                 
297                 // P >= 0
298                 if( !(P[1] & (1ULL<<63)) ) {
299                         q |= (Uint64)1 << (63-i);
300                 }
301                 else {
302                         //q |= 0 << (63-i);
303                         P[1] += Den;
304                 }
305         }
306         
307         return q;
308 }
309
310 /**
311  * \fn Uint64 __umoddi3(Uint64 Num, Uint64 Den)
312  * \brief Get the modulus of two 64-bit integers
313  */
314 Uint64 __umoddi3(Uint64 Num, Uint64 Den)
315 {
316         if(Den == 0)    __asm__ __volatile__ ("int $0x0");      // Call Div by Zero Error
317         if(Den == 1)    return 0;       // Speed Hacks
318         if(Den == 2)    return Num & 1; // Speed Hacks
319         if(Den == 4)    return Num & 3; // Speed Hacks
320         if(Den == 8)    return Num & 7; // Speed Hacks
321         if(Den == 16)   return Num & 15;        // Speed Hacks
322         if(Den == 32)   return Num & 31;        // Speed Hacks
323         if(Den == 1024) return Num & 1023;      // Speed Hacks
324         if(Den == 2048) return Num & 2047;      // Speed Hacks
325         if(Den == 4096) return Num & 4095;      // Speed Hacks
326         
327         if(Num >> 32 == 0 && Den >> 32 == 0)
328                 return (Uint32)Num % (Uint32)Den;
329         
330         return Num - __udivdi3(Num, Den) * Den;
331 }
332
333 Uint16 LittleEndian16(Uint16 Val)
334 {
335         return Val;
336 }
337 Uint16 BigEndian16(Uint16 Val)
338 {
339         return ((Val&0xFF)<<8) | ((Val>>8)&0xFF);
340 }
341 Uint32 LittleEndian32(Uint32 Val)
342 {
343         return Val;
344 }
345 Uint32 BigEndian32(Uint32 Val)
346 {
347         return ((Val&0xFF)<<24) | ((Val&0xFF00)<<8) | ((Val>>8)&0xFF00) | ((Val>>24)&0xFF);
348 }
349
350 // --- EXPORTS ---
351 EXPORT(memcpy); EXPORT(memset);
352 EXPORT(memcmp);
353 //EXPORT(memcpyw);      EXPORT(memsetw);
354 EXPORT(memcpyd);        EXPORT(memsetd);
355 EXPORT(inb);    EXPORT(inw);    EXPORT(ind);
356 EXPORT(outb);   EXPORT(outw);   EXPORT(outd);
357 EXPORT(__udivdi3);      EXPORT(__umoddi3);
358
359 EXPORT(LittleEndian16); EXPORT(BigEndian16);
360 EXPORT(LittleEndian32); EXPORT(BigEndian32);
361
362 EXPORT(SHORTLOCK);
363 EXPORT(SHORTREL);
364 EXPORT(IS_LOCKED);

UCC git Repository :: git.ucc.asn.au