Kernel/drvutil.c

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