Kernel - Added snprintf function
[tpg/acess2.git] / KernelLand / Kernel / drvutil.c
1 /*
2  * Acess2 Kernel
3  * - By John Hodge
4  *
5  * drvutil.c
6  * - Common Driver/Filesystem Helper Functions
7  */
8 #define DEBUG   0
9 #include <acess.h>
10 #include <api_drv_disk.h>
11 #include <api_drv_video.h>
12
13 // === TYPES ===
14
15 // === PROTOTYPES ===
16 //int   DrvUtil_Video_2DStream(void *Ent, void *Buffer, int Length, tDrvUtil_Video_2DHandlers *Handlers, int SizeofHandlers);
17 //size_t        DrvUtil_Video_WriteLFB(int Mode, tDrvUtil_Video_BufInfo *FBInfo, size_t Offset, size_t Length, void *Src);
18 //void  DrvUtil_Video_SetCursor(tDrvUtil_Video_BufInfo *Buf, tVideo_IOCtl_Bitmap *Bitmap);
19 //void  DrvUtil_Video_DrawCursor(tDrvUtil_Video_BufInfo *Buf, int X, int Y);
20 void    DrvUtil_Video_RenderCursor(tDrvUtil_Video_BufInfo *Buf);
21 //void  DrvUtil_Video_RemoveCursor(tDrvUtil_Video_BufInfo *Buf);
22 void    DrvUtil_Video_2D_Fill(void *Ent, Uint16 X, Uint16 Y, Uint16 W, Uint16 H, Uint32 Colour);
23 void    DrvUtil_Video_2D_Blit(void *Ent, Uint16 DstX, Uint16 DstY, Uint16 SrcX, Uint16 SrcY, Uint16 W, Uint16 H);
24
25 // === GLOBALS ===
26 tDrvUtil_Video_2DHandlers       gDrvUtil_Stub_2DFunctions = {
27         NULL,
28         DrvUtil_Video_2D_Fill,
29         DrvUtil_Video_2D_Blit
30 };
31 tVideo_IOCtl_Bitmap     gDrvUtil_TextModeCursor = {
32         8, 16,
33         0, 0,
34         {
35                 0, 0, 0         , 0, 0, 0, 0, 0,
36                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
37                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
38                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
39                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
40                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
41                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
42                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
43                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
44                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
45                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
46                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
47                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
48                 0,-1, 0xFF000000, 0, 0, 0, 0, 0,
49                 0, 0, 0         , 0, 0, 0, 0, 0,
50                 0, 0, 0         , 0, 0, 0, 0, 0
51         }
52 };
53
54 // === CODE ===
55 // --- Video Driver Helpers ---
56 int DrvUtil_Video_2DStream(void *Ent, const void *Buffer, int Length,
57         tDrvUtil_Video_2DHandlers *Handlers, int SizeofHandlers)
58 {
59         const Uint8     *stream = Buffer;
60          int    rem = Length;
61          int    op;
62         while( rem )
63         {
64                 rem --;
65                 op = *stream;
66                 stream ++;
67                 
68                 if(op > NUM_VIDEO_2DOPS) {
69                         Log_Warning("DrvUtil",
70                                 "DrvUtil_Video_2DStream: Unknown operation %i",
71                                 op);
72                         return Length-rem;
73                 }
74                 
75                 if(op*sizeof(void*) > SizeofHandlers) {
76                         Log_Warning("DrvUtil",
77                                 "DrvUtil_Video_2DStream: Driver does not support op %i",
78                                 op);
79                         return Length-rem;
80                 }
81                 
82                 switch(op)
83                 {
84                 case VIDEO_2DOP_NOP:    break;
85                 
86                 case VIDEO_2DOP_FILL:
87                         if(rem < 10)    return Length-rem;
88                         
89                         if(!Handlers->Fill) {
90                                 Log_Warning("DrvUtil", "DrvUtil_Video_2DStream: Driver"
91                                         " does not support VIDEO_2DOP_FILL");
92                                 return Length-rem;
93                         }
94                         
95                         Handlers->Fill(
96                                 Ent,
97                                 ((const Uint16*)stream)[0], ((const Uint16*)stream)[1],
98                                 ((const Uint16*)stream)[2], ((const Uint16*)stream)[3],
99                                 ((const Uint32*)stream)[4]
100                                 );
101                         
102                         rem -= 10;
103                         stream += 10;
104                         break;
105                 
106                 case VIDEO_2DOP_BLIT:
107                         if(rem < 12)    return Length-rem;
108                         
109                         if(!Handlers->Blit) {
110                                 Log_Warning("DrvUtil", "DrvUtil_Video_2DStream: Driver"
111                                         " does not support VIDEO_2DOP_BLIT");
112                                 return Length-rem;
113                         }
114                         
115                         Handlers->Blit(
116                                 Ent,
117                                 ((const Uint16*)stream)[0], ((const Uint16*)stream)[1],
118                                 ((const Uint16*)stream)[2], ((const Uint16*)stream)[3],
119                                 ((const Uint16*)stream)[4], ((const Uint16*)stream)[5]
120                                 );
121                         
122                         rem -= 12;
123                         stream += 12;
124                         break;
125                 
126                 }
127         }
128         return 0;
129 }
130
131 int DrvUtil_Video_WriteLFB(tDrvUtil_Video_BufInfo *FBInfo, size_t Offset, size_t Length, const void *Buffer)
132 {
133         Uint8   *dest;
134         const Uint32    *src = Buffer;
135          int    csr_x, csr_y;
136          int    x, y;
137          int    bytes_per_px = (FBInfo->Depth + 7) / 8;
138         ENTER("pFBInfo xOffset xLength pBuffer",
139                 Mode, FBInfo, Offset, Length, Buffer);
140
141         csr_x = FBInfo->CursorX;
142         csr_y = FBInfo->CursorY;
143
144         DrvUtil_Video_RemoveCursor(FBInfo);
145
146         switch( FBInfo->BufferFormat )
147         {
148         case VIDEO_BUFFMT_TEXT:
149                 {
150                 const tVT_Char  *chars = Buffer;
151                  int    widthInChars = FBInfo->Width/giVT_CharWidth;
152                  int    heightInChars = FBInfo->Height/giVT_CharHeight;
153                  int    i;
154         
155                 LOG("bytes_per_px = %i", bytes_per_px);
156                 LOG("widthInChars = %i, heightInChars = %i", widthInChars, heightInChars);
157         
158                 Length /= sizeof(tVT_Char);     Offset /= sizeof(tVT_Char);
159                 
160                 x = Offset % widthInChars;      y = Offset / widthInChars;
161                 LOG("x = %i, y = %i", x, y);    
162         
163                 // Sanity Check
164                 if(Offset > heightInChars * widthInChars)       LEAVE_RET('i', 0);
165                 if(y >= heightInChars)  LEAVE_RET('i', 0);
166                 
167                 if( Offset + Length > heightInChars*widthInChars )
168                 {
169                         Length = heightInChars*widthInChars - Offset;
170                 }
171                 
172                 dest = FBInfo->Framebuffer;
173                 LOG("dest = %p", dest);
174                 dest += y * giVT_CharHeight * FBInfo->Pitch;
175                 LOG("dest = %p", dest);
176                 
177                 for( i = 0; i < Length; i++ )
178                 {
179                         if( y >= heightInChars )
180                         {
181                                 Log_Notice("DrvUtil", "Stopped at %i", i);
182                                 break;
183                         }
184
185                         VT_Font_Render(
186                                 chars->Ch,
187                                 dest + x*giVT_CharWidth*bytes_per_px, FBInfo->Depth, FBInfo->Pitch,
188                                 VT_Colour12toN(chars->BGCol, FBInfo->Depth),
189                                 VT_Colour12toN(chars->FGCol, FBInfo->Depth)
190                                 );
191                         
192                         chars ++;
193                         x ++;
194                         if( x >= widthInChars )
195                         {
196                                 x = 0;
197                                 y ++;
198                                 dest += FBInfo->Pitch*giVT_CharHeight;
199                                 LOG("dest = %p", dest);
200                         }
201                 }
202                 Length = i * sizeof(tVT_Char);
203                 }
204                 break;
205         
206         case VIDEO_BUFFMT_FRAMEBUFFER:
207                 if(FBInfo->Width*FBInfo->Height*4 < Offset+Length)
208                 {
209                         Log_Warning("DrvUtil", "DrvUtil_Video_WriteLFB - Framebuffer Overflow");
210                         return 0;
211                 }
212                 
213                 switch(FBInfo->Depth)
214                 {
215                 case 15:
216                 case 16:
217                         Log_Warning("DrvUtil", "TODO: Support 15/16 bpp modes in LFB write");
218                         break;
219                 case 24:
220                         x = Offset % FBInfo->Width;
221                         y = Offset / FBInfo->Width;
222                         dest = (Uint8*)FBInfo->Framebuffer + y*FBInfo->Pitch;
223                         for( ; Length >= 4; Length -= 4 )
224                         {
225                                 dest[x*3+0] = *src & 0xFF;
226                                 dest[x*3+1] = (*src >> 8) & 0xFF;
227                                 dest[x*3+2] = (*src >> 16) & 0xFF;
228                                 x ++;
229                                 if(x == FBInfo->Width) {
230                                         dest += FBInfo->Pitch;
231                                         x = 0;
232                                 }
233                         }
234                         break;
235                 case 32:
236                         // Copy to Frambuffer
237                         if( FBInfo->Pitch != FBInfo->Width*4 )
238                         {
239                                 Uint32  *px;
240                                 // Pitch isn't 4*Width
241                                 x = Offset % FBInfo->Width;
242                                 y = Offset / FBInfo->Height;
243                                 
244                                 px = (Uint32*)FBInfo->Framebuffer + y*FBInfo->Pitch/4;
245
246                                 for( ; Length >= 4; Length -= 4, x )
247                                 {
248                                         px[x++] = *src ++;
249                                         if( x == FBInfo->Width ) {
250                                                 x = 0;
251                                                 px += FBInfo->Pitch;
252                                         }
253                                 }
254                         }
255                         else
256                         {
257                                 dest = (Uint8 *)FBInfo->Framebuffer + Offset;
258                                 memcpy(dest, Buffer, Length);
259                         }
260                         break;
261                 default:
262                         Log_Warning("DrvUtil", "DrvUtil_Video_WriteLFB - Unknown bit depthn %i", FBInfo->Depth);
263                         break;
264                 }
265                 break;
266         
267         case VIDEO_BUFFMT_2DSTREAM:
268                 Length = DrvUtil_Video_2DStream(
269                         FBInfo, Buffer, Length,
270                         &gDrvUtil_Stub_2DFunctions, sizeof(gDrvUtil_Stub_2DFunctions)
271                         );
272                 break;
273         
274         default:
275                 LEAVE('i', -1);
276                 return -1;
277         }
278
279         DrvUtil_Video_DrawCursor(FBInfo, csr_x, csr_y);
280
281         LEAVE('x', Length);
282         return Length;
283 }
284
285 int DrvUtil_Video_SetCursor(tDrvUtil_Video_BufInfo *Buf, tVideo_IOCtl_Bitmap *Bitmap)
286 {
287          int    csrX = Buf->CursorX, csrY = Buf->CursorY;
288         size_t  size;
289
290         // Clear old bitmap
291         if( Buf->CursorBitmap )
292         {
293                 DrvUtil_Video_RemoveCursor(Buf);
294                 if( !Bitmap || Bitmap->W != Buf->CursorBitmap->W || Bitmap->H != Buf->CursorBitmap->H )
295                 {
296                         free( Buf->CursorSaveBuf );
297                         Buf->CursorSaveBuf = NULL;
298                 }
299                 if( Buf->CursorBitmap != &gDrvUtil_TextModeCursor)
300                         free(Buf->CursorBitmap);
301                 Buf->CursorBitmap = NULL;
302         }
303         
304         // If the new bitmap is null, disable drawing
305         if( !Bitmap )
306         {
307                 Buf->CursorX = -1;
308                 Buf->CursorY = -1;
309                 return 0;
310         }
311
312         // Sanity check the bitmap
313         if( !CheckMem(Bitmap, sizeof(*Bitmap)) || !CheckMem(Bitmap->Data, Bitmap->W*Bitmap->H*sizeof(Uint32)) )
314         {
315                 Log_Warning("DrvUtil", "DrvUtil_Video_SetCursor: Bitmap (%p) is in invalid memory", Bitmap);
316                 errno = -EINVAL;
317                 return -1;
318         }
319
320         // Don't take a copy of the DrvUtil provided cursor
321         if( Bitmap == &gDrvUtil_TextModeCursor )
322         {
323                 Buf->CursorBitmap = Bitmap;
324         }
325         else
326         {
327                 size = sizeof(tVideo_IOCtl_Bitmap) + Bitmap->W*Bitmap->H*4;
328                 
329                 // Take a copy
330                 Buf->CursorBitmap = malloc( size );
331                 memcpy(Buf->CursorBitmap, Bitmap, size);
332         }
333         
334         // Restore cursor position
335         DrvUtil_Video_DrawCursor(Buf, csrX, csrY);
336         return 0;
337 }
338
339 void DrvUtil_Video_DrawCursor(tDrvUtil_Video_BufInfo *Buf, int X, int Y)
340 {
341          int    render_ox=0, render_oy=0, render_w, render_h;
342
343         DrvUtil_Video_RemoveCursor(Buf);
344
345         // X < 0 disables the cursor
346         if( X < 0 ) {
347                 Buf->CursorX = -1;
348                 return ;
349         }
350
351         // Sanity checking
352         if( X < 0 || Y < 0 )    return;
353         if( X >= Buf->Width || Y >= Buf->Height )       return;
354
355         // Ensure the cursor is enabled
356         if( !Buf->CursorBitmap )        return ;
357         
358         // Save cursor position (for changing the bitmap)
359         Buf->CursorX = X;       Buf->CursorY = Y;
360
361         // Apply cursor's center offset
362         X -= Buf->CursorBitmap->XOfs;
363         Y -= Buf->CursorBitmap->YOfs;
364         
365         // Get the width of the cursor on screen (clipping to right/bottom edges)
366         render_w = X > Buf->Width  - Buf->CursorBitmap->W ? Buf->Width  - X : Buf->CursorBitmap->W;
367         render_h = Y > Buf->Height - Buf->CursorBitmap->H ? Buf->Height - Y : Buf->CursorBitmap->H;
368
369         // Clipp to left/top edges
370         if(X < 0) {     render_ox = -X; X = 0;  }
371         if(Y < 0) {     render_oy = -Y; Y = 0;  }
372
373         // Save values
374         Buf->CursorRenderW = render_w;  Buf->CursorRenderH = render_h;
375         Buf->CursorDestX   = X;         Buf->CursorDestY = Y;
376         Buf->CursorReadX   = render_ox; Buf->CursorReadY = render_oy;
377
378         // Call render routine
379         DrvUtil_Video_RenderCursor(Buf);
380 }
381
382 void DrvUtil_Video_RenderCursor(tDrvUtil_Video_BufInfo *Buf)
383 {
384          int    src_x = Buf->CursorReadX, src_y = Buf->CursorReadY;
385          int    render_w = Buf->CursorRenderW, render_h = Buf->CursorRenderH;
386          int    dest_x = Buf->CursorDestX, dest_y = Buf->CursorDestY;
387          int    bytes_per_px = (Buf->Depth + 7) / 8;
388          int    save_pitch = Buf->CursorBitmap->W * bytes_per_px;
389         void    *dest;
390         Uint32  *src;
391          int    x, y;
392
393         dest = (Uint8*)Buf->Framebuffer + dest_y*Buf->Pitch + dest_x*bytes_per_px;
394         src = Buf->CursorBitmap->Data + src_y * Buf->CursorBitmap->W + src_x;
395         
396         // Allocate save buffer if not already
397         if( !Buf->CursorSaveBuf )
398                 Buf->CursorSaveBuf = malloc( Buf->CursorBitmap->W*Buf->CursorBitmap->H*bytes_per_px );
399
400         // Save behind the cursor
401         for( y = 0; y < render_h; y ++ )
402                 memcpy(
403                         (Uint8*)Buf->CursorSaveBuf + y*save_pitch,
404                         (Uint8*)dest + y*Buf->Pitch,
405                         render_w*bytes_per_px
406                         );
407
408         // Draw the cursor
409         switch(Buf->Depth)
410         {
411         case 15:
412         case 16:
413                 Log_Warning("DrvUtil", "TODO: Support 15/16 bpp modes in cursor draw");
414                 break;
415         case 24:
416                 for( y = 0; y < render_h; y ++ )
417                 {
418                         Uint8   *px;
419                         px = dest;
420                         for(x = 0; x < render_w; x ++, px += 3)
421                         {
422                                 Uint32  value = src[x];
423                                 // TODO: Should I implement alpha blending?
424                                 if(value & 0xFF000000)
425                                 {
426                                         px[0] = value & 0xFF;
427                                         px[1] = (value >> 8) & 0xFF;
428                                         px[2] = (value >> 16) & 0xFF;
429                                 }
430                                 else
431                                         ;
432                         }
433                         src += Buf->CursorBitmap->W;
434                         dest = (Uint8*)dest + Buf->Pitch;
435                 }
436                 break;
437         case 32:
438                 for( y = 0; y < render_h; y ++ )
439                 {
440                         Uint32  *px;
441                         px = dest;
442                         for(x = 0; x < render_w; x ++, px ++)
443                         {
444                                 Uint32  value = src[x];
445                                 // TODO: Should I implement alpha blending?
446                                 if(value & 0xFF000000)
447                                         *px = value;
448                                 else
449                                         ;       // NOP, completely transparent
450                         }
451                         src += Buf->CursorBitmap->W;
452                         dest = (Uint8*)dest + Buf->Pitch;
453                 }
454                 break;
455         default:
456                 Log_Error("DrvUtil", "RenderCursor - Unknown bit depth %i", Buf->Depth);
457                 Buf->CursorX = -1;
458                 break;
459         }
460 }
461
462 void DrvUtil_Video_RemoveCursor(tDrvUtil_Video_BufInfo *Buf)
463 {
464          int    bytes_per_px = (Buf->Depth + 7) / 8;
465          int    y, save_pitch;
466         Uint8   *dest, *src;
467
468         // Just a little sanity
469         if( !Buf->CursorBitmap || Buf->CursorX == -1 )  return ;
470         if( !Buf->CursorSaveBuf )       return ;
471
472 //      Debug("DrvUtil_Video_RemoveCursor: (Buf=%p) dest_x=%i, dest_y=%i", Buf, Buf->CursorDestX, Buf->CursorDestY);
473
474         // Set up
475         save_pitch = Buf->CursorBitmap->W * bytes_per_px;
476         dest = (Uint8*)Buf->Framebuffer + Buf->CursorDestY * Buf->Pitch + Buf->CursorDestX*bytes_per_px;
477         src = Buf->CursorSaveBuf;
478         
479         // Copy each line back
480         for( y = 0; y < Buf->CursorRenderH; y ++ )
481         {
482                 memcpy( dest, src, Buf->CursorRenderW * bytes_per_px );
483                 src += save_pitch;
484                 dest += Buf->Pitch;
485         }
486         
487         // Set the cursor as removed
488         Buf->CursorX = -1;
489 }
490
491 void DrvUtil_Video_2D_Fill(void *Ent, Uint16 X, Uint16 Y, Uint16 W, Uint16 H, Uint32 Colour)
492 {
493         tDrvUtil_Video_BufInfo  *FBInfo = Ent;
494
495         // TODO: Handle non-32bit modes
496         if( FBInfo->Depth != 32 )       return;
497
498         // TODO: Be less hacky
499          int    pitch = FBInfo->Pitch/4;
500         Uint32  *buf = (Uint32*)FBInfo->Framebuffer + Y*pitch + X;
501         while( H -- ) {
502                 Uint32 *tmp;
503                  int    i;
504                 tmp = buf;
505                 for(i=W;i--;tmp++)      *tmp = Colour;
506                 buf += pitch;
507         }
508 }
509
510 void DrvUtil_Video_2D_Blit(void *Ent, Uint16 DstX, Uint16 DstY, Uint16 SrcX, Uint16 SrcY, Uint16 W, Uint16 H)
511 {
512         tDrvUtil_Video_BufInfo  *FBInfo = Ent;
513          int    scrnpitch = FBInfo->Pitch;
514          int    bytes_per_px = (FBInfo->Depth + 7) / 8;
515          int    dst = DstY*scrnpitch + DstX;
516          int    src = SrcY*scrnpitch + SrcX;
517          int    tmp;
518         
519         //Log("Vesa_2D_Blit: (Ent=%p, DstX=%i, DstY=%i, SrcX=%i, SrcY=%i, W=%i, H=%i)",
520         //      Ent, DstX, DstY, SrcX, SrcY, W, H);
521         
522         if(SrcX + W > FBInfo->Width)    W = FBInfo->Width - SrcX;
523         if(DstX + W > FBInfo->Width)    W = FBInfo->Width - DstX;
524         if(SrcY + H > FBInfo->Height)   H = FBInfo->Height - SrcY;
525         if(DstY + H > FBInfo->Height)   H = FBInfo->Height - DstY;
526         
527         //Debug("W = %i, H = %i", W, H);
528         
529         if( dst > src ) {
530                 // Reverse copy
531                 dst += H*scrnpitch;
532                 src += H*scrnpitch;
533                 while( H -- ) {
534                         dst -= scrnpitch;
535                         src -= scrnpitch;
536                         tmp = W*bytes_per_px;
537                         for( tmp = W; tmp --; ) {
538                                 *((Uint8*)FBInfo->Framebuffer + dst + tmp) = *((Uint8*)FBInfo->Framebuffer + src + tmp);
539                         }
540                 }
541         }
542         else if(W == FBInfo->Width && FBInfo->Pitch == FBInfo->Width*bytes_per_px) {
543                 memmove((Uint8*)FBInfo->Framebuffer + dst, (Uint8*)FBInfo->Framebuffer + src, H*FBInfo->Pitch);
544         }
545         else {
546                 // Normal copy is OK
547                 while( H -- ) {
548                         memcpy((Uint8*)FBInfo->Framebuffer + dst, (Uint8*)FBInfo->Framebuffer + src, W*bytes_per_px);
549                         dst += scrnpitch;
550                         src += scrnpitch;
551                 }
552         }
553         //Log("Vesa_2D_Blit: RETURN");
554 }
555         
556
557 // --- Disk Driver Helpers ---
558 Uint64 DrvUtil_ReadBlock(Uint64 Start, Uint64 Length, void *Buffer,
559         tDrvUtil_Read_Callback ReadBlocks, Uint64 BlockSize, Uint Argument)
560 {
561         Uint8   tmp[BlockSize]; // C99
562         Uint64  block = Start / BlockSize;
563          int    offset = Start - block * BlockSize;
564          int    leading = BlockSize - offset;
565         Uint64  num;
566          int    tailings;
567         Uint64  ret;
568         
569         ENTER("XStart XLength pBuffer pReadBlocks XBlockSize xArgument",
570                 Start, Length, Buffer, ReadBlocks, BlockSize, Argument);
571         
572         // Non aligned start, let's fix that!
573         if(offset != 0)
574         {
575                 if(leading > Length)    leading = Length;
576                 LOG("Reading %i bytes from Block1+%i", leading, offset);
577                 ret = ReadBlocks(block, 1, tmp, Argument);
578                 if(ret != 1) {
579                         LEAVE('i', 0);
580                         return 0;
581                 }
582                 memcpy( Buffer, &tmp[offset], leading );
583                 
584                 if(leading == Length) {
585                         LEAVE('i', leading);
586                         return leading;
587                 }
588                 
589                 Buffer = (Uint8*)Buffer + leading;
590                 block ++;
591                 num = ( Length - leading ) / BlockSize;
592                 tailings = Length - num * BlockSize - leading;
593         }
594         else {
595                 num = Length / BlockSize;
596                 tailings = Length % BlockSize;
597         }
598         
599         // Read central blocks
600         if(num)
601         {
602                 LOG("Reading %i blocks", num);
603                 ret = ReadBlocks(block, num, Buffer, Argument);
604                 if(ret != num ) {
605                         LEAVE('X', leading + ret * BlockSize);
606                         return leading + ret * BlockSize;
607                 }
608         }
609         
610         // Read last tailing block
611         if(tailings != 0)
612         {
613                 LOG("Reading %i bytes from last block", tailings);
614                 block += num;
615                 Buffer = (Uint8*)Buffer + num * BlockSize;
616                 ret = ReadBlocks(block, 1, tmp, Argument);
617                 if(ret != 1) {
618                         LEAVE('X', leading + num * BlockSize);
619                         return leading + num * BlockSize;
620                 }
621                 memcpy( Buffer, tmp, tailings );
622         }
623         
624         LEAVE('X', Length);
625         return Length;
626 }
627
628 Uint64 DrvUtil_WriteBlock(Uint64 Start, Uint64 Length, const void *Buffer,
629         tDrvUtil_Read_Callback ReadBlocks, tDrvUtil_Write_Callback WriteBlocks,
630         Uint64 BlockSize, Uint Argument)
631 {
632         Uint8   tmp[BlockSize]; // C99
633         Uint64  block = Start / BlockSize;
634          int    offset = Start - block * BlockSize;
635          int    leading = BlockSize - offset;
636         Uint64  num;
637          int    tailings;
638         Uint64  ret;
639         
640         ENTER("XStart XLength pBuffer pReadBlocks pWriteBlocks XBlockSize xArgument",
641                 Start, Length, Buffer, ReadBlocks, WriteBlocks, BlockSize, Argument);
642         
643         // Non aligned start, let's fix that!
644         if(offset != 0)
645         {
646                 if(leading > Length)    leading = Length;
647                 LOG("Writing %i bytes to Block1+%i", leading, offset);
648                 // Read a copy of the block
649                 ret = ReadBlocks(block, 1, tmp, Argument);
650                 if(ret != 1) {
651                         LEAVE('i', 0);
652                         return 0;
653                 }
654                 // Modify
655                 memcpy( &tmp[offset], Buffer, leading );
656                 // Write Back
657                 ret = WriteBlocks(block, 1, tmp, Argument);
658                 if(ret != 1) {
659                         LEAVE('i', 0);
660                         return 0;
661                 }
662                 
663                 if(leading == Length) {
664                         LEAVE('i', leading);
665                         return leading;
666                 }
667                 
668                 Buffer = (Uint8*)Buffer + leading;
669                 block ++;
670                 num = ( Length - leading ) / BlockSize;
671                 tailings = Length - num * BlockSize - leading;
672         }
673         else {
674                 num = Length / BlockSize;
675                 tailings = Length % BlockSize;
676         }
677         
678         // Read central blocks
679         if(num)
680         {
681                 LOG("Writing %i blocks", num);
682                 ret = WriteBlocks(block, num, Buffer, Argument);
683                 if(ret != num ) {
684                         LEAVE('X', leading + ret * BlockSize);
685                         return leading + ret * BlockSize;
686                 }
687         }
688         
689         // Read last tailing block
690         if(tailings != 0)
691         {
692                 LOG("Writing %i bytes to last block", tailings);
693                 block += num;
694                 Buffer = (Uint8*)Buffer + num * BlockSize;
695                 // Read
696                 ret = ReadBlocks(block, 1, tmp, Argument);
697                 if(ret != 1) {
698                         LEAVE('X', leading + num * BlockSize);
699                         return leading + num * BlockSize;
700                 }
701                 // Modify
702                 memcpy( tmp, Buffer, tailings );
703                 // Write
704                 ret = WriteBlocks(block, 1, tmp, Argument);
705                 if(ret != 1) {
706                         LEAVE('X', leading + num * BlockSize);
707                         return leading + num * BlockSize;
708                 }
709                 
710         }
711         
712         LEAVE('X', Length);
713         return Length;
714 }

UCC git Repository :: git.ucc.asn.au