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

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