Usermode/ld-acess - Clean up loadlib.c, improved debug
[tpg/acess2.git] / KernelLand / Kernel / drvutil_disk.c
1 /*
2  * Acess2 Kernel
3  * - By John Hodge
4  *
5  * drvutil_disk.c
6  * - Storage Driver Helper Functions
7  */
8 #define DEBUG   0
9 #include <acess.h>
10 #include <api_drv_disk.h>
11
12 // --- Disk Driver Helpers ---
13 size_t DrvUtil_ReadBlock(Uint64 Start, size_t Length, void *Buffer,
14         tDrvUtil_Read_Callback ReadBlocks, size_t BlockSize, void *Argument)
15 {
16         Uint8   tmp[BlockSize]; // C99
17         Uint64  block = Start / BlockSize;
18          int    offset = Start - block * BlockSize;
19         size_t  leading = BlockSize - offset;
20         Uint64  num;
21          int    tailings;
22         size_t  ret;
23         
24         ENTER("XStart XLength pBuffer pReadBlocks XBlockSize pArgument",
25                 Start, Length, Buffer, ReadBlocks, BlockSize, Argument);
26         
27         // Non aligned start, let's fix that!
28         if(offset != 0)
29         {
30                 if(leading > Length)    leading = Length;
31                 LOG("Reading %i bytes from Block1+%i", leading, offset);
32                 ret = ReadBlocks(block, 1, tmp, Argument);
33                 if(ret != 1) {
34                         LEAVE('i', 0);
35                         return 0;
36                 }
37                 memcpy( Buffer, &tmp[offset], leading );
38                 
39                 if(leading == Length) {
40                         LEAVE('i', leading);
41                         return leading;
42                 }
43                 
44                 Buffer = (Uint8*)Buffer + leading;
45                 block ++;
46                 num = ( Length - leading ) / BlockSize;
47                 tailings = Length - num * BlockSize - leading;
48         }
49         else {
50                 num = Length / BlockSize;
51                 tailings = Length % BlockSize;
52                 leading = 0;
53         }
54         
55         // Read central blocks
56         if(num)
57         {
58                 LOG("Reading %i blocks", num);
59                 ret = ReadBlocks(block, num, Buffer, Argument);
60                 if(ret != num ) {
61                         LOG("Incomplete read (%i != %i)", ret, num);
62                         LEAVE('X', leading + ret * BlockSize);
63                         return leading + ret * BlockSize;
64                 }
65         }
66         
67         // Read last tailing block
68         if(tailings != 0)
69         {
70                 LOG("Reading %i bytes from last block", tailings);
71                 block += num;
72                 Buffer = (Uint8*)Buffer + num * BlockSize;
73                 ret = ReadBlocks(block, 1, tmp, Argument);
74                 if(ret != 1) {
75                         LEAVE('X', leading + num * BlockSize);
76                         return leading + num * BlockSize;
77                 }
78                 memcpy( Buffer, tmp, tailings );
79         }
80         
81         LEAVE('X', Length);
82         return Length;
83 }
84
85 size_t DrvUtil_WriteBlock(Uint64 Start, size_t Length, const void *Buffer,
86         tDrvUtil_Read_Callback ReadBlocks, tDrvUtil_Write_Callback WriteBlocks,
87         size_t BlockSize, void *Argument)
88 {
89         Uint8   tmp[BlockSize]; // C99
90         Uint64  block = Start / BlockSize;
91         size_t  offset = Start - block * BlockSize;
92         size_t  leading = BlockSize - offset;
93         Uint64  num;
94          int    tailings;
95         size_t  ret;
96         
97         ENTER("XStart XLength pBuffer pReadBlocks pWriteBlocks XBlockSize pArgument",
98                 Start, Length, Buffer, ReadBlocks, WriteBlocks, BlockSize, Argument);
99         
100         // Non aligned start, let's fix that!
101         if(offset != 0)
102         {
103                 if(leading > Length)    leading = Length;
104                 LOG("Writing %i bytes to Block1+%i", leading, offset);
105                 // Read a copy of the block
106                 ret = ReadBlocks(block, 1, tmp, Argument);
107                 if(ret != 1) {
108                         LEAVE('i', 0);
109                         return 0;
110                 }
111                 // Modify
112                 memcpy( &tmp[offset], Buffer, leading );
113                 // Write Back
114                 ret = WriteBlocks(block, 1, tmp, Argument);
115                 if(ret != 1) {
116                         LEAVE('i', 0);
117                         return 0;
118                 }
119                 
120                 if(leading == Length) {
121                         LEAVE('i', leading);
122                         return leading;
123                 }
124                 
125                 Buffer = (Uint8*)Buffer + leading;
126                 block ++;
127                 num = ( Length - leading ) / BlockSize;
128                 tailings = Length - num * BlockSize - leading;
129         }
130         else {
131                 num = Length / BlockSize;
132                 tailings = Length % BlockSize;
133         }
134         
135         // Read central blocks
136         if(num)
137         {
138                 LOG("Writing %i blocks", num);
139                 ret = WriteBlocks(block, num, Buffer, Argument);
140                 if(ret != num ) {
141                         LEAVE('X', leading + ret * BlockSize);
142                         return leading + ret * BlockSize;
143                 }
144         }
145         
146         // Read last tailing block
147         if(tailings != 0)
148         {
149                 LOG("Writing %i bytes to last block", tailings);
150                 block += num;
151                 Buffer = (Uint8*)Buffer + num * BlockSize;
152                 // Read
153                 ret = ReadBlocks(block, 1, tmp, Argument);
154                 if(ret != 1) {
155                         LEAVE('X', leading + num * BlockSize);
156                         return leading + num * BlockSize;
157                 }
158                 // Modify
159                 memcpy( tmp, Buffer, tailings );
160                 // Write
161                 ret = WriteBlocks(block, 1, tmp, Argument);
162                 if(ret != 1) {
163                         LEAVE('X', leading + num * BlockSize);
164                         return leading + num * BlockSize;
165                 }
166                 
167         }
168         
169         LEAVE('X', Length);
170         return Length;
171 }

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