c499c7a8ed06be6f664f8b39663bc355c1b7fbaa
[tpg/acess2.git] / Kernel / arch / x86 / lib.c
1 /*
2  * AcessOS Microkernel Version
3  * lib.c
4  */
5 #include <acess.h>
6
7 // === CODE ===
8 void Spinlock(int *lock)
9 {
10          int    v = 1;
11         while(v)        __asm__ __volatile__ ("lock xchgl %%eax, (%%edi)":"=a"(v):"a"(1),"D"(lock));
12 }
13
14 void Release(int *lock)
15 {
16         __asm__ __volatile__ ("lock andl $0, (%0)"::"r"(lock));
17 }
18
19 // === IO Commands ===
20 void outb(Uint16 Port, Uint8 Data)
21 {
22         __asm__ __volatile__ ("outb %%al, %%dx"::"d"(Port),"a"(Data));
23 }
24 void outw(Uint16 Port, Uint16 Data)
25 {
26         __asm__ __volatile__ ("outw %%ax, %%dx"::"d"(Port),"a"(Data));
27 }
28 void outd(Uint16 Port, Uint32 Data)
29 {
30         __asm__ __volatile__ ("outl %%eax, %%dx"::"d"(Port),"a"(Data));
31 }
32 Uint8 inb(Uint16 Port)
33 {
34         Uint8   ret;
35         __asm__ __volatile__ ("inb %%dx, %%al":"=a"(ret):"d"(Port));
36         return ret;
37 }
38 Uint16 inw(Uint16 Port)
39 {
40         Uint16  ret;
41         __asm__ __volatile__ ("inw %%dx, %%ax":"=a"(ret):"d"(Port));
42         return ret;
43 }
44 Uint32 ind(Uint16 Port)
45 {
46         Uint32  ret;
47         __asm__ __volatile__ ("inl %%dx, %%eax":"=a"(ret):"d"(Port));
48         return ret;
49 }
50
51 /**
52  * \fn void *memset(void *Dest, int Val, size_t Num)
53  * \brief Do a byte granuality set of Dest
54  */
55 void *memset(void *Dest, int Val, size_t Num)
56 {
57         Uint32  val = Val&0xFF;
58         val |= val << 8;
59         val |= val << 16;
60         __asm__ __volatile__ (
61                 "rep stosl;\n\t"
62                 "mov %3, %%ecx;\n\t"
63                 "rep stosb"
64                 :: "D" (Dest), "a" (val), "c" (Num/4), "r" (Num&3));
65         return Dest;
66 }
67 /**
68  * \brief Set double words
69  */
70 void *memsetd(void *Dest, Uint32 Val, size_t Num)
71 {
72         __asm__ __volatile__ ("rep stosl" :: "D" (Dest), "a" (Val), "c" (Num));
73         return Dest;
74 }
75
76 /**
77  * \fn int memcmp(const void *m1, const void *m2, size_t Num)
78  * \brief Compare two pieces of memory
79  */
80 int memcmp(const void *m1, const void *m2, size_t Num)
81 {
82         while(Num--)
83         {
84                 if(*(Uint8*)m1 != *(Uint8*)m2)  break;
85                 m1 ++;
86                 m2 ++;
87         }
88         return *(Uint8*)m1 - *(Uint8*)m2;
89 }
90
91 /**
92  * \fn void *memcpy(void *Dest, const void *Src, size_t Num)
93  * \brief Copy \a Num bytes from \a Src to \a Dest
94  */
95 void *memcpy(void *Dest, const void *Src, size_t Num)
96 {
97         if( ((Uint)Dest & 3) || ((Uint)Src & 3) )
98                 __asm__ __volatile__ ("rep movsb" :: "D" (Dest), "S" (Src), "c" (Num));
99         else {
100                 __asm__ __volatile__ (
101                         "rep movsl;\n\t"
102                         "mov %3, %%ecx;\n\t"
103                         "rep movsb"
104                         :: "D" (Dest), "S" (Src), "c" (Num/4), "r" (Num&3));
105         }
106         return Dest;
107 }
108 /**
109  * \fn void *memcpyd(void *Dest, const void *Src, size_t Num)
110  * \brief Copy \a Num DWORDs from \a Src to \a Dest
111  */
112 void *memcpyd(void *Dest, const void *Src, size_t Num)
113 {
114         __asm__ __volatile__ ("rep movsl" :: "D" (Dest), "S" (Src), "c" (Num));
115         return Dest;
116 }
117
118 /**
119  * \fn Uint64 __udivdi3(Uint64 Num, Uint64 Den)
120  * \brief Divide two 64-bit integers
121  */
122 Uint64 __udivdi3(Uint64 Num, Uint64 Den)
123 {
124         Uint64  P[2];
125         Uint64  q = 0;
126          int    i;
127         
128         if(Den == 0)    __asm__ __volatile__ ("int $0x0");
129         // Common speedups
130         if(Num <= 0xFFFFFFFF && Den <= 0xFFFFFFFF)
131                 return (Uint32)Num / (Uint32)Den;
132         if(Den == 1)    return Num;
133         if(Den == 2)    return Num >> 1;        // Speed Hacks
134         if(Den == 4)    return Num >> 2;        // Speed Hacks
135         if(Den == 8)    return Num >> 3;        // Speed Hacks
136         if(Den == 16)   return Num >> 4;        // Speed Hacks
137         if(Den == 32)   return Num >> 5;        // Speed Hacks
138         if(Den == 1024) return Num >> 10;       // Speed Hacks
139         if(Den == 2048) return Num >> 11;       // Speed Hacks
140         if(Den == 4096) return Num >> 12;
141         if(Num < Den)   return 0;
142         if(Num < Den*2) return 1;
143         if(Num == Den*2)        return 2;
144         
145         // Restoring division, from wikipedia
146         // http://en.wikipedia.org/wiki/Division_(digital)
147         P[0] = Num;     P[1] = 0;
148         for( i = 64; i--; )
149         {
150                 // P <<= 1;
151                 P[1] = (P[1] << 1) | (P[0] >> 63);
152                 P[0] = P[0] << 1;
153                 
154                 // P -= Den << 64
155                 P[1] -= Den;
156                 
157                 // P >= 0
158                 if( !(P[1] & (1ULL<<63)) ) {
159                         q |= (Uint64)1 << (63-i);
160                 }
161                 else {
162                         //q |= 0 << (63-i);
163                         P[1] += Den;
164                 }
165         }
166         
167         return q;
168 }
169
170 /**
171  * \fn Uint64 __umoddi3(Uint64 Num, Uint64 Den)
172  * \brief Get the modulus of two 64-bit integers
173  */
174 Uint64 __umoddi3(Uint64 Num, Uint64 Den)
175 {
176         if(Den == 0)    __asm__ __volatile__ ("int $0x0");      // Call Div by Zero Error
177         if(Den == 1)    return 0;       // Speed Hacks
178         if(Den == 2)    return Num & 1; // Speed Hacks
179         if(Den == 4)    return Num & 3; // Speed Hacks
180         if(Den == 8)    return Num & 7; // Speed Hacks
181         if(Den == 16)   return Num & 15;        // Speed Hacks
182         if(Den == 32)   return Num & 31;        // Speed Hacks
183         if(Den == 1024) return Num & 1023;      // Speed Hacks
184         if(Den == 2048) return Num & 2047;      // Speed Hacks
185         if(Den == 4096) return Num & 4095;      // Speed Hacks
186         
187         if(Num >> 32 == 0 && Den >> 32 == 0)
188                 return (Uint32)Num % (Uint32)Den;
189         
190         return Num - __udivdi3(Num, Den) * Den;
191 }
192
193 Uint16 LittleEndian16(Uint16 Val)
194 {
195         return Val;
196 }
197 Uint16 BigEndian16(Uint16 Val)
198 {
199         return ((Val&0xFF)<<8) | ((Val>>8)&0xFF);
200 }
201 Uint32 LittleEndian32(Uint32 Val)
202 {
203         return Val;
204 }
205 Uint32 BigEndian32(Uint32 Val)
206 {
207         return ((Val&0xFF)<<24) | ((Val&0xFF00)<<8) | ((Val>>8)&0xFF00) | ((Val>>24)&0xFF);
208 }
209
210 // --- EXPORTS ---
211 EXPORT(memcpy); EXPORT(memset);
212 EXPORT(memcmp);
213 //EXPORT(memcpyw);      EXPORT(memsetw);
214 EXPORT(memcpyd);        EXPORT(memsetd);
215 EXPORT(inb);    EXPORT(inw);    EXPORT(ind);
216 EXPORT(outb);   EXPORT(outw);   EXPORT(outd);
217 EXPORT(__udivdi3);      EXPORT(__umoddi3);
218
219 EXPORT(LittleEndian16); EXPORT(BigEndian16);
220 EXPORT(LittleEndian32); EXPORT(BigEndian32);

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