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);