Kernel/PTYs - NL->CR translation (disabled)

[tpg/acess2.git] / KernelLand / Modules / IPStack / buffer.c

diff --git a/KernelLand/Modules/IPStack/buffer.c b/KernelLand/Modules/IPStack/buffer.c
index 9151c1c..3fe8b89 100644 (file)
--- a/KernelLand/Modules/IPStack/buffer.c
+++ b/KernelLand/Modules/IPStack/buffer.c
@@ -5,6 +5,7 @@
  * buffer.c
  * - Scatter-gather handling
  */
+#define DEBUG  0
 #include "ipstack.h"
 #include "include/buffer.h"
 
@@ -14,12 +15,15 @@ struct sIPStackBuffer
         int    MaxSubBufffers;
         int    nSubBuffers;
        size_t  TotalLength;
+       tMutex  lBufferLock;
+
        struct _subbuffer
        {
                const void      *Data;
                size_t  PreLength;
                size_t  PostLength;
-               // TODO: Callbacks?
+               tIPStackBufferCb        Cb;
+               void    *CbArg;
        } SubBuffers[];
 };
 
@@ -29,26 +33,60 @@ tIPStackBuffer *IPStack_Buffer_CreateBuffer(int MaxBuffers)
        tIPStackBuffer *ret;
        
        ret = malloc( sizeof(*ret) + MaxBuffers * sizeof(ret->SubBuffers[0]) );
+       ASSERTR(ret, NULL);
        ret->MaxSubBufffers = MaxBuffers;
        ret->nSubBuffers = 0;
        ret->TotalLength = 0;
+       memset(&ret->lBufferLock, 0, sizeof(ret->lBufferLock));
        memset(ret->SubBuffers, 0, MaxBuffers * sizeof(ret->SubBuffers[0]));
        return ret;
 }
 
+void IPStack_Buffer_ClearBuffer(tIPStackBuffer *Buffer)
+{
+       ASSERT(Buffer);
+       IPStack_Buffer_LockBuffer(Buffer);
+       for( int i = 0; i < Buffer->nSubBuffers; i ++ )
+       {
+               if( Buffer->SubBuffers[i].Cb == NULL )
+                       continue ;
+               Buffer->SubBuffers[i].Cb(
+                       Buffer->SubBuffers[i].CbArg,
+                       Buffer->SubBuffers[i].PreLength,
+                       Buffer->SubBuffers[i].PostLength,
+                       Buffer->SubBuffers[i].Data
+                       );
+       }
+       Buffer->nSubBuffers = 0;
+       IPStack_Buffer_UnlockBuffer(Buffer);
+}
+
 void IPStack_Buffer_DestroyBuffer(tIPStackBuffer *Buffer)
 {
-       // TODO: Fire callbacks?
+       LOG("Called with %p by %p", Buffer, __builtin_return_address(0));
+       ASSERT(Buffer);
+       IPStack_Buffer_ClearBuffer(Buffer);
        Buffer->MaxSubBufffers = 0;
-       Buffer->nSubBuffers = 0;
        free(Buffer);
 }
 
+void IPStack_Buffer_LockBuffer(tIPStackBuffer *Buffer)
+{
+       ASSERT(Buffer);
+       Mutex_Acquire(&Buffer->lBufferLock);
+}
+void IPStack_Buffer_UnlockBuffer(tIPStackBuffer *Buffer)
+{
+       ASSERT(Buffer);
+       Mutex_Release(&Buffer->lBufferLock);
+}
+
 void IPStack_Buffer_AppendSubBuffer(tIPStackBuffer *Buffer,
        size_t HeaderLen, size_t FooterLen, const void *Data,
        tIPStackBufferCb Cb, void *Arg
        )
 {
+       ASSERT(Buffer);
        if( Buffer->nSubBuffers == Buffer->MaxSubBufffers ) {
                // Ah, oops?
                Log_Error("IPStack", "Buffer %p only had %i sub-buffers allocated, which was not enough",
@@ -62,56 +100,82 @@ void IPStack_Buffer_AppendSubBuffer(tIPStackBuffer *Buffer,
        Buffer->SubBuffers[index].Data = Data;
        Buffer->SubBuffers[index].PreLength = HeaderLen;
        Buffer->SubBuffers[index].PostLength = FooterLen;
+       Buffer->SubBuffers[index].Cb = Cb;
+       Buffer->SubBuffers[index].CbArg = Arg;
 }
 
 size_t IPStack_Buffer_GetLength(tIPStackBuffer *Buffer)
 {
+       ASSERT(Buffer);
        return Buffer->TotalLength;
 }
 
-void *IPStack_Buffer_CompactBuffer(tIPStackBuffer *Buffer, size_t *Length)
+size_t IPStack_Buffer_GetData(tIPStackBuffer *Buffer, void *Dest, size_t MaxBytes)
 {
-       void    *ret;
-       ret = malloc(Buffer->TotalLength);
-       if(!ret) {
-               *Length = 0;
-               return NULL;
-       }
+       ASSERT(Buffer);
+       Uint8   *dest = Dest;
+       size_t  rem_space = MaxBytes;
+       size_t  len;
        
-       *Length = Buffer->TotalLength;
-       
-       Uint8   *dest = ret;
-       for( int i = Buffer->nSubBuffers; i --; )
+       for( int i = Buffer->nSubBuffers; i -- && rem_space != 0; )
        {
+               len = MIN(Buffer->SubBuffers[i].PreLength, rem_space);
+               #if !DISABLE_ASSERTS
+               if( !CheckMem(Buffer->SubBuffers[i].Data, len) ) {
+                       Log_Error("IPStack", "Buffer pre %i invalid (%p+0x%x)",
+                               i, Buffer->SubBuffers[i].Data, len);
+                       return 0;
+               }
+               #endif
                memcpy(dest,
                        Buffer->SubBuffers[i].Data,
-                       Buffer->SubBuffers[i].PreLength
+                       len
                        );
-               dest += Buffer->SubBuffers[i].PreLength;
+               dest += len;
+               rem_space -= len;
        }
-       for( int i = 0; i < Buffer->nSubBuffers; i ++ )
+       for( int i = 0; i < Buffer->nSubBuffers && rem_space; i ++ )
        {
-               if( Buffer->SubBuffers[i].PostLength )
-               {
-                       memcpy(dest,
-                               (Uint8*)Buffer->SubBuffers[i].Data + Buffer->SubBuffers[i].PreLength,
-                               Buffer->SubBuffers[i].PostLength
-                               );
-                       dest += Buffer->SubBuffers[i].PreLength;
-               }
+               if( Buffer->SubBuffers[i].PostLength == 0  )
+                       continue ;
+               
+               len = MIN(Buffer->SubBuffers[i].PostLength, rem_space);
+               void *ptr = (Uint8*)Buffer->SubBuffers[i].Data + Buffer->SubBuffers[i].PreLength;
+               ASSERT( CheckMem(ptr, len) );
+               memcpy(dest, ptr, len);
+               dest += len;
+               rem_space -= len;
        }
+       
+       return MaxBytes - rem_space;
+}
+
+void *IPStack_Buffer_CompactBuffer(tIPStackBuffer *Buffer, size_t *Length)
+{
+       ASSERT(Buffer);
+       void    *ret = malloc(Buffer->TotalLength);
+       if(!ret) {
+               *Length = 0;
+               return NULL;
+       }
+       
+       *Length = Buffer->TotalLength;
+
+       IPStack_Buffer_GetData(Buffer, ret, Buffer->TotalLength);
+
        return ret;
 }
 
 int IPStack_Buffer_GetBuffer(tIPStackBuffer *Buffer, int Index, size_t *Length, const void **DataPtr)
 {
+       ASSERT(Buffer);
        if( Index == -1 )       Index = 0;
 
        if( Index >= Buffer->nSubBuffers*2 ) {
                return -1;
        }
 
-       if( Index > Buffer->nSubBuffers )
+       if( Index >= Buffer->nSubBuffers )
        {
                // Appended buffers
                Index -= Buffer->nSubBuffers;
@@ -123,18 +187,23 @@ int IPStack_Buffer_GetBuffer(tIPStackBuffer *Buffer, int Index, size_t *Length,
                                return -1;
                }
 
-               *DataPtr = (Uint8*)Buffer->SubBuffers[Index].Data + Buffer->SubBuffers[Index].PreLength;
-               *Length = Buffer->SubBuffers[Index].PostLength;
+               if( DataPtr )
+                       *DataPtr = (Uint8*)Buffer->SubBuffers[Index].Data + Buffer->SubBuffers[Index].PreLength;
+               if( Length )
+                       *Length = Buffer->SubBuffers[Index].PostLength;
 
                return (Index + 1) + Buffer->nSubBuffers;
        }
        else
        {
-               Index = Buffer->nSubBuffers - Index;
+                int    rv = Index + 1;
+               Index = Buffer->nSubBuffers - Index - 1;
                // Prepended buffers
-               *DataPtr = Buffer->SubBuffers[Index].Data;
-               *Length = Buffer->SubBuffers[Index].PreLength;
-               return Buffer->nSubBuffers - (Index - 1);
+               if( DataPtr )
+                       *DataPtr = Buffer->SubBuffers[Index].Data;
+               if( Length )
+                       *Length = Buffer->SubBuffers[Index].PreLength;
+               return rv;
        }
 }