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

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