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