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 }