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 Uint64 DrvUtil_ReadBlock(Uint64 Start, Uint64 Length, void *Buffer,
  14         tDrvUtil_Read_Callback ReadBlocks, Uint64 BlockSize, void *Argument)
  15 {
  16         Uint8   tmp[BlockSize]; // C99
  17         Uint64  block = Start / BlockSize;
  18          int    offset = Start - block * BlockSize;
  19          int    leading = BlockSize - offset;
  20         Uint64  num;
  21          int    tailings;
  22         Uint64  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         }
  53
  54         // Read central blocks
  55         if(num)
  56         {
  57                 LOG("Reading %i blocks", num);
  58                 ret = ReadBlocks(block, num, Buffer, Argument);
  59                 if(ret != num ) {
  60                         LEAVE('X', leading + ret * BlockSize);
  61                         return leading + ret * BlockSize;
  62                 }
  63         }
  64
  65         // Read last tailing block
  66         if(tailings != 0)
  67         {
  68                 LOG("Reading %i bytes from last block", tailings);
  69                 block += num;
  70                 Buffer = (Uint8*)Buffer + num * BlockSize;
  71                 ret = ReadBlocks(block, 1, tmp, Argument);
  72                 if(ret != 1) {
  73                         LEAVE('X', leading + num * BlockSize);
  74                         return leading + num * BlockSize;
  75                 }
  76                 memcpy( Buffer, tmp, tailings );
  77         }
  78
  79         LEAVE('X', Length);
  80         return Length;
  81 }
  82
  83 Uint64 DrvUtil_WriteBlock(Uint64 Start, Uint64 Length, const void *Buffer,
  84         tDrvUtil_Read_Callback ReadBlocks, tDrvUtil_Write_Callback WriteBlocks,
  85         Uint64 BlockSize, void *Argument)
  86 {
  87         Uint8   tmp[BlockSize]; // C99
  88         Uint64  block = Start / BlockSize;
  89          int    offset = Start - block * BlockSize;
  90          int    leading = BlockSize - offset;
  91         Uint64  num;
  92          int    tailings;
  93         Uint64  ret;
  94
  95         ENTER("XStart XLength pBuffer pReadBlocks pWriteBlocks XBlockSize pArgument",
  96                 Start, Length, Buffer, ReadBlocks, WriteBlocks, BlockSize, Argument);
  97
  98         // Non aligned start, let's fix that!
  99         if(offset != 0)
 100         {
 101                 if(leading > Length)    leading = Length;
 102                 LOG("Writing %i bytes to Block1+%i", leading, offset);
 103                 // Read a copy of the block
 104                 ret = ReadBlocks(block, 1, tmp, Argument);
 105                 if(ret != 1) {
 106                         LEAVE('i', 0);
 107                         return 0;
 108                 }
 109                 // Modify
 110                 memcpy( &tmp[offset], Buffer, leading );
 111                 // Write Back
 112                 ret = WriteBlocks(block, 1, tmp, Argument);
 113                 if(ret != 1) {
 114                         LEAVE('i', 0);
 115                         return 0;
 116                 }
 117
 118                 if(leading == Length) {
 119                         LEAVE('i', leading);
 120                         return leading;
 121                 }
 122
 123                 Buffer = (Uint8*)Buffer + leading;
 124                 block ++;
 125                 num = ( Length - leading ) / BlockSize;
 126                 tailings = Length - num * BlockSize - leading;
 127         }
 128         else {
 129                 num = Length / BlockSize;
 130                 tailings = Length % BlockSize;
 131         }
 132
 133         // Read central blocks
 134         if(num)
 135         {
 136                 LOG("Writing %i blocks", num);
 137                 ret = WriteBlocks(block, num, Buffer, Argument);
 138                 if(ret != num ) {
 139                         LEAVE('X', leading + ret * BlockSize);
 140                         return leading + ret * BlockSize;
 141                 }
 142         }
 143
 144         // Read last tailing block
 145         if(tailings != 0)
 146         {
 147                 LOG("Writing %i bytes to last block", tailings);
 148                 block += num;
 149                 Buffer = (Uint8*)Buffer + num * BlockSize;
 150                 // Read
 151                 ret = ReadBlocks(block, 1, tmp, Argument);
 152                 if(ret != 1) {
 153                         LEAVE('X', leading + num * BlockSize);
 154                         return leading + num * BlockSize;
 155                 }
 156                 // Modify
 157                 memcpy( tmp, Buffer, tailings );
 158                 // Write
 159                 ret = WriteBlocks(block, 1, tmp, Argument);
 160                 if(ret != 1) {
 161                         LEAVE('X', leading + num * BlockSize);
 162                         return leading + num * BlockSize;
 163                 }
 164
 165         }
 166
 167         LEAVE('X', Length);
 168         return Length;
 169 }