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                         Handlers->Blit(
  68                                 Ent,
  69                                 *(Uint16*)(&stream[0]), *(Uint16*)(&stream[2]),
  70                                 *(Uint16*)(&stream[4]), *(Uint16*)(&stream[6]),
  71                                 *(Uint16*)(&stream[8]), *(Uint16*)(&stream[10])
  72                                 );
  73
  74                         rem -= 12;
  75                         stream += 12;
  76                         break;
  77
  78                 }
  79         }
  80         return 0;
  81 }
  82
  83 // --- Disk Driver Helpers ---
  84 Uint64 DrvUtil_ReadBlock(Uint64 Start, Uint64 Length, void *Buffer,
  85         tDrvUtil_Callback ReadBlocks, Uint64 BlockSize, Uint 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 XBlockSize xArgument",
  96                 Start, Length, Buffer, ReadBlocks, BlockSize, Argument);
  97
  98         // Non aligned start, let's fix that!
  99         if(offset != 0)
 100         {
 101                 if(leading > Length)    leading = Length;
 102                 LOG("Reading %i bytes from Block1+%i", leading, offset);
 103                 ret = ReadBlocks(block, 1, tmp, Argument);
 104                 if(ret != 1) {
 105                         LEAVE('i', 0);
 106                         return 0;
 107                 }
 108                 memcpy( Buffer, &tmp[offset], leading );
 109
 110                 if(leading == Length) {
 111                         LEAVE('i', leading);
 112                         return leading;
 113                 }
 114
 115                 Buffer += leading;
 116                 block ++;
 117                 num = ( Length - leading ) / BlockSize;
 118                 tailings = Length - num * BlockSize - leading;
 119         }
 120         else {
 121                 num = Length / BlockSize;
 122                 tailings = Length % BlockSize;
 123         }
 124
 125         // Read central blocks
 126         if(num)
 127         {
 128                 LOG("Reading %i blocks", num);
 129                 ret = ReadBlocks(block, num, Buffer, Argument);
 130                 if(ret != num ) {
 131                         LEAVE('X', leading + ret * BlockSize);
 132                         return leading + ret * BlockSize;
 133                 }
 134         }
 135
 136         // Read last tailing block
 137         if(tailings != 0)
 138         {
 139                 LOG("Reading %i bytes from last block", tailings);
 140                 block += num;
 141                 Buffer += num * BlockSize;
 142                 ret = ReadBlocks(block, 1, tmp, Argument);
 143                 if(ret != 1) {
 144                         LEAVE('X', leading + num * BlockSize);
 145                         return leading + num * BlockSize;
 146                 }
 147                 memcpy( Buffer, tmp, tailings );
 148         }
 149
 150         LEAVE('X', Length);
 151         return Length;
 152 }
 153
 154 Uint64 DrvUtil_WriteBlock(Uint64 Start, Uint64 Length, void *Buffer,
 155         tDrvUtil_Callback ReadBlocks, tDrvUtil_Callback WriteBlocks,
 156         Uint64 BlockSize, Uint Argument)
 157 {
 158         Uint8   tmp[BlockSize]; // C99
 159         Uint64  block = Start / BlockSize;
 160          int    offset = Start - block * BlockSize;
 161          int    leading = BlockSize - offset;
 162         Uint64  num;
 163          int    tailings;
 164         Uint64  ret;
 165
 166         ENTER("XStart XLength pBuffer pReadBlocks pWriteBlocks XBlockSize xArgument",
 167                 Start, Length, Buffer, ReadBlocks, WriteBlocks, BlockSize, Argument);
 168
 169         // Non aligned start, let's fix that!
 170         if(offset != 0)
 171         {
 172                 if(leading > Length)    leading = Length;
 173                 LOG("Writing %i bytes to Block1+%i", leading, offset);
 174                 // Read a copy of the block
 175                 ret = ReadBlocks(block, 1, tmp, Argument);
 176                 if(ret != 1) {
 177                         LEAVE('i', 0);
 178                         return 0;
 179                 }
 180                 // Modify
 181                 memcpy( &tmp[offset], Buffer, leading );
 182                 // Write Back
 183                 ret = WriteBlocks(block, 1, tmp, Argument);
 184                 if(ret != 1) {
 185                         LEAVE('i', 0);
 186                         return 0;
 187                 }
 188
 189                 if(leading == Length) {
 190                         LEAVE('i', leading);
 191                         return leading;
 192                 }
 193
 194                 Buffer += leading;
 195                 block ++;
 196                 num = ( Length - leading ) / BlockSize;
 197                 tailings = Length - num * BlockSize - leading;
 198         }
 199         else {
 200                 num = Length / BlockSize;
 201                 tailings = Length % BlockSize;
 202         }
 203
 204         // Read central blocks
 205         if(num)
 206         {
 207                 LOG("Writing %i blocks", num);
 208                 ret = WriteBlocks(block, num, Buffer, Argument);
 209                 if(ret != num ) {
 210                         LEAVE('X', leading + ret * BlockSize);
 211                         return leading + ret * BlockSize;
 212                 }
 213         }
 214
 215         // Read last tailing block
 216         if(tailings != 0)
 217         {
 218                 LOG("Writing %i bytes to last block", tailings);
 219                 block += num;
 220                 Buffer += num * BlockSize;
 221                 // Read
 222                 ret = ReadBlocks(block, 1, tmp, Argument);
 223                 if(ret != 1) {
 224                         LEAVE('X', leading + num * BlockSize);
 225                         return leading + num * BlockSize;
 226                 }
 227                 // Modify
 228                 memcpy( tmp, Buffer, tailings );
 229                 // Write
 230                 ret = WriteBlocks(block, 1, tmp, Argument);
 231                 if(ret != 1) {
 232                         LEAVE('X', leading + num * BlockSize);
 233                         return leading + num * BlockSize;
 234                 }
 235
 236         }
 237
 238         LEAVE('X', Length);
 239         return Length;
 240 }