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         void    *stream = Buffer;
  16          int    rem = Length;
  17          int    op;
  18         while( rem )
  19         {
  20                 rem --;
  21                 op = *(Uint8*)stream;
  22                 stream = (void*)((tVAddr)stream + 1);
  23
  24                 if(op > NUM_VIDEO_2DOPS) {
  25                         Log_Warning("DrvUtil", "DrvUtil_Video_2DStream: Unknown"
  26                                 " operation %i", op);
  27                 }
  28
  29                 if(op*sizeof(void*) > SizeofHandlers) {
  30                         Log_Warning("DrvUtil", "DrvUtil_Video_2DStream: Driver does"
  31                                 " not support op %i", op);
  32                         return Length-rem;
  33                 }
  34
  35                 switch(op)
  36                 {
  37                 case VIDEO_2DOP_NOP:    break;
  38
  39                 case VIDEO_2DOP_FILL:
  40                         if(rem < 12)    return Length-rem;
  41
  42                         if(!Handlers->Fill) {
  43                                 Log_Warning("DrvUtil", "DrvUtil_Video_2DStream: Driver"
  44                                         " does not support VIDEO_2DOP_FILL");
  45                                 return Length-rem;
  46                         }
  47
  48                         Handlers->Fill(
  49                                 Ent,
  50                                 ((Uint16*)stream)[0], ((Uint16*)stream)[1],
  51                                 ((Uint16*)stream)[2], ((Uint16*)stream)[3],
  52                                 ((Uint32*)stream)[2]
  53                                 );
  54
  55                         rem -= 12;
  56                         stream = (void*)((tVAddr)stream + 12);
  57                         break;
  58
  59                 case VIDEO_2DOP_BLIT:
  60                         if(rem < 12)    return Length-rem;
  61
  62                         if(!Handlers->Blit) {
  63                                 Log_Warning("DrvUtil", "DrvUtil_Video_2DStream: Driver"
  64                                         " does not support VIDEO_2DOP_BLIT");
  65                                 return Length-rem;
  66                         }
  67
  68                         Handlers->Blit(
  69                                 Ent,
  70                                 ((Uint16*)stream)[0], ((Uint16*)stream)[1],
  71                                 ((Uint16*)stream)[2], ((Uint16*)stream)[3],
  72                                 ((Uint16*)stream)[4], ((Uint16*)stream)[5]
  73                                 );
  74
  75                         rem -= 12;
  76                         stream = (void*)((tVAddr)stream + 12);
  77                         break;
  78
  79                 }
  80         }
  81         return 0;
  82 }
  83
  84 // --- Disk Driver Helpers ---
  85 Uint64 DrvUtil_ReadBlock(Uint64 Start, Uint64 Length, void *Buffer,
  86         tDrvUtil_Callback ReadBlocks, Uint64 BlockSize, Uint Argument)
  87 {
  88         Uint8   tmp[BlockSize]; // C99
  89         Uint64  block = Start / BlockSize;
  90          int    offset = Start - block * BlockSize;
  91          int    leading = BlockSize - offset;
  92         Uint64  num;
  93          int    tailings;
  94         Uint64  ret;
  95
  96         ENTER("XStart XLength pBuffer pReadBlocks XBlockSize xArgument",
  97                 Start, Length, Buffer, ReadBlocks, BlockSize, Argument);
  98
  99         // Non aligned start, let's fix that!
 100         if(offset != 0)
 101         {
 102                 if(leading > Length)    leading = Length;
 103                 LOG("Reading %i bytes from Block1+%i", leading, offset);
 104                 ret = ReadBlocks(block, 1, tmp, Argument);
 105                 if(ret != 1) {
 106                         LEAVE('i', 0);
 107                         return 0;
 108                 }
 109                 memcpy( Buffer, &tmp[offset], leading );
 110
 111                 if(leading == Length) {
 112                         LEAVE('i', leading);
 113                         return leading;
 114                 }
 115
 116                 Buffer = (Uint8*)Buffer + leading;
 117                 block ++;
 118                 num = ( Length - leading ) / BlockSize;
 119                 tailings = Length - num * BlockSize - leading;
 120         }
 121         else {
 122                 num = Length / BlockSize;
 123                 tailings = Length % BlockSize;
 124         }
 125
 126         // Read central blocks
 127         if(num)
 128         {
 129                 LOG("Reading %i blocks", num);
 130                 ret = ReadBlocks(block, num, Buffer, Argument);
 131                 if(ret != num ) {
 132                         LEAVE('X', leading + ret * BlockSize);
 133                         return leading + ret * BlockSize;
 134                 }
 135         }
 136
 137         // Read last tailing block
 138         if(tailings != 0)
 139         {
 140                 LOG("Reading %i bytes from last block", tailings);
 141                 block += num;
 142                 Buffer = (Uint8*)Buffer + num * BlockSize;
 143                 ret = ReadBlocks(block, 1, tmp, Argument);
 144                 if(ret != 1) {
 145                         LEAVE('X', leading + num * BlockSize);
 146                         return leading + num * BlockSize;
 147                 }
 148                 memcpy( Buffer, tmp, tailings );
 149         }
 150
 151         LEAVE('X', Length);
 152         return Length;
 153 }
 154
 155 Uint64 DrvUtil_WriteBlock(Uint64 Start, Uint64 Length, void *Buffer,
 156         tDrvUtil_Callback ReadBlocks, tDrvUtil_Callback WriteBlocks,
 157         Uint64 BlockSize, Uint Argument)
 158 {
 159         Uint8   tmp[BlockSize]; // C99
 160         Uint64  block = Start / BlockSize;
 161          int    offset = Start - block * BlockSize;
 162          int    leading = BlockSize - offset;
 163         Uint64  num;
 164          int    tailings;
 165         Uint64  ret;
 166
 167         ENTER("XStart XLength pBuffer pReadBlocks pWriteBlocks XBlockSize xArgument",
 168                 Start, Length, Buffer, ReadBlocks, WriteBlocks, BlockSize, Argument);
 169
 170         // Non aligned start, let's fix that!
 171         if(offset != 0)
 172         {
 173                 if(leading > Length)    leading = Length;
 174                 LOG("Writing %i bytes to Block1+%i", leading, offset);
 175                 // Read a copy of the block
 176                 ret = ReadBlocks(block, 1, tmp, Argument);
 177                 if(ret != 1) {
 178                         LEAVE('i', 0);
 179                         return 0;
 180                 }
 181                 // Modify
 182                 memcpy( &tmp[offset], Buffer, leading );
 183                 // Write Back
 184                 ret = WriteBlocks(block, 1, tmp, Argument);
 185                 if(ret != 1) {
 186                         LEAVE('i', 0);
 187                         return 0;
 188                 }
 189
 190                 if(leading == Length) {
 191                         LEAVE('i', leading);
 192                         return leading;
 193                 }
 194
 195                 Buffer = (Uint8*)Buffer + leading;
 196                 block ++;
 197                 num = ( Length - leading ) / BlockSize;
 198                 tailings = Length - num * BlockSize - leading;
 199         }
 200         else {
 201                 num = Length / BlockSize;
 202                 tailings = Length % BlockSize;
 203         }
 204
 205         // Read central blocks
 206         if(num)
 207         {
 208                 LOG("Writing %i blocks", num);
 209                 ret = WriteBlocks(block, num, Buffer, Argument);
 210                 if(ret != num ) {
 211                         LEAVE('X', leading + ret * BlockSize);
 212                         return leading + ret * BlockSize;
 213                 }
 214         }
 215
 216         // Read last tailing block
 217         if(tailings != 0)
 218         {
 219                 LOG("Writing %i bytes to last block", tailings);
 220                 block += num;
 221                 Buffer = (Uint8*)Buffer + num * BlockSize;
 222                 // Read
 223                 ret = ReadBlocks(block, 1, tmp, Argument);
 224                 if(ret != 1) {
 225                         LEAVE('X', leading + num * BlockSize);
 226                         return leading + num * BlockSize;
 227                 }
 228                 // Modify
 229                 memcpy( tmp, Buffer, tailings );
 230                 // Write
 231                 ret = WriteBlocks(block, 1, tmp, Argument);
 232                 if(ret != 1) {
 233                         LEAVE('X', leading + num * BlockSize);
 234                         return leading + num * BlockSize;
 235                 }
 236
 237         }
 238
 239         LEAVE('X', Length);
 240         return Length;
 241 }