Kernel/x86 - Disabled task tracing
[tpg/acess2.git] / Kernel / drvutil.c
1 /*
2  * Acess2
3  * Common Driver/Filesystem Helper Functions
4  */
5 #define DEBUG   0
6 #include <acess.h>
7 #include <api_drv_disk.h>
8 #include <api_drv_video.h>
9
10 // === CODE ===
11 // --- Video Driver Helpers ---
12 Uint64 DrvUtil_Video_2DStream(void *Ent, void *Buffer, int Length,
13         tDrvUtil_Video_2DHandlers *Handlers, int SizeofHandlers)
14 {
15         void    *stream = Buffer;
16          int    rem = Length;
17          int    op;
18         while( rem )
19         {
20                 rem --;
21                 op = *(Uint8*)stream;
22                 stream = (void*)((tVAddr)stream + 1);
23                 
24                 if(op > NUM_VIDEO_2DOPS) {
25                         Log_Warning("DrvUtil", "DrvUtil_Video_2DStream: Unknown"
26                                 " operation %i", op);
27                 }
28                 
29                 if(op*sizeof(void*) > SizeofHandlers) {
30                         Log_Warning("DrvUtil", "DrvUtil_Video_2DStream: Driver does"
31                                 " not support op %i", op);
32                         return Length-rem;
33                 }
34                 
35                 switch(op)
36                 {
37                 case VIDEO_2DOP_NOP:    break;
38                 
39                 case VIDEO_2DOP_FILL:
40                         if(rem < 12)    return Length-rem;
41                         
42                         if(!Handlers->Fill) {
43                                 Log_Warning("DrvUtil", "DrvUtil_Video_2DStream: Driver"
44                                         " does not support VIDEO_2DOP_FILL");
45                                 return Length-rem;
46                         }
47                         
48                         Handlers->Fill(
49                                 Ent,
50                                 ((Uint16*)stream)[0], ((Uint16*)stream)[1],
51                                 ((Uint16*)stream)[2], ((Uint16*)stream)[3],
52                                 ((Uint32*)stream)[2]
53                                 );
54                         
55                         rem -= 12;
56                         stream = (void*)((tVAddr)stream + 12);
57                         break;
58                 
59                 case VIDEO_2DOP_BLIT:
60                         if(rem < 12)    return Length-rem;
61                         
62                         if(!Handlers->Blit) {
63                                 Log_Warning("DrvUtil", "DrvUtil_Video_2DStream: Driver"
64                                         " does not support VIDEO_2DOP_BLIT");
65                                 return Length-rem;
66                         }
67                         
68                         Handlers->Blit(
69                                 Ent,
70                                 ((Uint16*)stream)[0], ((Uint16*)stream)[1],
71                                 ((Uint16*)stream)[2], ((Uint16*)stream)[3],
72                                 ((Uint16*)stream)[4], ((Uint16*)stream)[5]
73                                 );
74                         
75                         rem -= 12;
76                         stream = (void*)((tVAddr)stream + 12);
77                         break;
78                 
79                 }
80         }
81         return 0;
82 }
83
84 // --- Disk Driver Helpers ---
85 Uint64 DrvUtil_ReadBlock(Uint64 Start, Uint64 Length, void *Buffer,
86         tDrvUtil_Callback ReadBlocks, Uint64 BlockSize, Uint Argument)
87 {
88         Uint8   tmp[BlockSize]; // C99
89         Uint64  block = Start / BlockSize;
90          int    offset = Start - block * BlockSize;
91          int    leading = BlockSize - offset;
92         Uint64  num;
93          int    tailings;
94         Uint64  ret;
95         
96         ENTER("XStart XLength pBuffer pReadBlocks XBlockSize xArgument",
97                 Start, Length, Buffer, ReadBlocks, BlockSize, Argument);
98         
99         // Non aligned start, let's fix that!
100         if(offset != 0)
101         {
102                 if(leading > Length)    leading = Length;
103                 LOG("Reading %i bytes from Block1+%i", leading, offset);
104                 ret = ReadBlocks(block, 1, tmp, Argument);
105                 if(ret != 1) {
106                         LEAVE('i', 0);
107                         return 0;
108                 }
109                 memcpy( Buffer, &tmp[offset], leading );
110                 
111                 if(leading == Length) {
112                         LEAVE('i', leading);
113                         return leading;
114                 }
115                 
116                 Buffer = (Uint8*)Buffer + leading;
117                 block ++;
118                 num = ( Length - leading ) / BlockSize;
119                 tailings = Length - num * BlockSize - leading;
120         }
121         else {
122                 num = Length / BlockSize;
123                 tailings = Length % BlockSize;
124         }
125         
126         // Read central blocks
127         if(num)
128         {
129                 LOG("Reading %i blocks", num);
130                 ret = ReadBlocks(block, num, Buffer, Argument);
131                 if(ret != num ) {
132                         LEAVE('X', leading + ret * BlockSize);
133                         return leading + ret * BlockSize;
134                 }
135         }
136         
137         // Read last tailing block
138         if(tailings != 0)
139         {
140                 LOG("Reading %i bytes from last block", tailings);
141                 block += num;
142                 Buffer = (Uint8*)Buffer + num * BlockSize;
143                 ret = ReadBlocks(block, 1, tmp, Argument);
144                 if(ret != 1) {
145                         LEAVE('X', leading + num * BlockSize);
146                         return leading + num * BlockSize;
147                 }
148                 memcpy( Buffer, tmp, tailings );
149         }
150         
151         LEAVE('X', Length);
152         return Length;
153 }
154
155 Uint64 DrvUtil_WriteBlock(Uint64 Start, Uint64 Length, void *Buffer,
156         tDrvUtil_Callback ReadBlocks, tDrvUtil_Callback WriteBlocks,
157         Uint64 BlockSize, Uint Argument)
158 {
159         Uint8   tmp[BlockSize]; // C99
160         Uint64  block = Start / BlockSize;
161          int    offset = Start - block * BlockSize;
162          int    leading = BlockSize - offset;
163         Uint64  num;
164          int    tailings;
165         Uint64  ret;
166         
167         ENTER("XStart XLength pBuffer pReadBlocks pWriteBlocks XBlockSize xArgument",
168                 Start, Length, Buffer, ReadBlocks, WriteBlocks, BlockSize, Argument);
169         
170         // Non aligned start, let's fix that!
171         if(offset != 0)
172         {
173                 if(leading > Length)    leading = Length;
174                 LOG("Writing %i bytes to Block1+%i", leading, offset);
175                 // Read a copy of the block
176                 ret = ReadBlocks(block, 1, tmp, Argument);
177                 if(ret != 1) {
178                         LEAVE('i', 0);
179                         return 0;
180                 }
181                 // Modify
182                 memcpy( &tmp[offset], Buffer, leading );
183                 // Write Back
184                 ret = WriteBlocks(block, 1, tmp, Argument);
185                 if(ret != 1) {
186                         LEAVE('i', 0);
187                         return 0;
188                 }
189                 
190                 if(leading == Length) {
191                         LEAVE('i', leading);
192                         return leading;
193                 }
194                 
195                 Buffer = (Uint8*)Buffer + leading;
196                 block ++;
197                 num = ( Length - leading ) / BlockSize;
198                 tailings = Length - num * BlockSize - leading;
199         }
200         else {
201                 num = Length / BlockSize;
202                 tailings = Length % BlockSize;
203         }
204         
205         // Read central blocks
206         if(num)
207         {
208                 LOG("Writing %i blocks", num);
209                 ret = WriteBlocks(block, num, Buffer, Argument);
210                 if(ret != num ) {
211                         LEAVE('X', leading + ret * BlockSize);
212                         return leading + ret * BlockSize;
213                 }
214         }
215         
216         // Read last tailing block
217         if(tailings != 0)
218         {
219                 LOG("Writing %i bytes to last block", tailings);
220                 block += num;
221                 Buffer = (Uint8*)Buffer + num * BlockSize;
222                 // Read
223                 ret = ReadBlocks(block, 1, tmp, Argument);
224                 if(ret != 1) {
225                         LEAVE('X', leading + num * BlockSize);
226                         return leading + num * BlockSize;
227                 }
228                 // Modify
229                 memcpy( tmp, Buffer, tailings );
230                 // Write
231                 ret = WriteBlocks(block, 1, tmp, Argument);
232                 if(ret != 1) {
233                         LEAVE('X', leading + num * BlockSize);
234                         return leading + num * BlockSize;
235                 }
236                 
237         }
238         
239         LEAVE('X', Length);
240         return Length;
241 }

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