KernelLand/Modules/IPStack/buffer.c

   1 /*
   2  * Acess2 Networking Stack
   3  * - By John Hodge (thePowersGang)
   4  *
   5  * buffer.c
   6  * - Scatter-gather handling
   7  */
   8 #define DEBUG   0
   9 #include "ipstack.h"
  10 #include "include/buffer.h"
  11
  12 // === STRUCTURES ===
  13 struct sIPStackBuffer
  14 {
  15          int    MaxSubBufffers;
  16          int    nSubBuffers;
  17         size_t  TotalLength;
  18         tMutex  lBufferLock;
  19
  20         struct _subbuffer
  21         {
  22                 const void      *Data;
  23                 size_t  PreLength;
  24                 size_t  PostLength;
  25                 tIPStackBufferCb        Cb;
  26                 void    *CbArg;
  27         } SubBuffers[];
  28 };
  29
  30 // === CODE ===
  31 tIPStackBuffer *IPStack_Buffer_CreateBuffer(int MaxBuffers)
  32 {
  33         tIPStackBuffer *ret;
  34
  35         ret = malloc( sizeof(*ret) + MaxBuffers * sizeof(ret->SubBuffers[0]) );
  36         ASSERTR(ret, NULL);
  37         ret->MaxSubBufffers = MaxBuffers;
  38         ret->nSubBuffers = 0;
  39         ret->TotalLength = 0;
  40         memset(&ret->lBufferLock, 0, sizeof(ret->lBufferLock));
  41         memset(ret->SubBuffers, 0, MaxBuffers * sizeof(ret->SubBuffers[0]));
  42         return ret;
  43 }
  44
  45 void IPStack_Buffer_ClearBuffer(tIPStackBuffer *Buffer)
  46 {
  47         ASSERT(Buffer);
  48         IPStack_Buffer_LockBuffer(Buffer);
  49         for( int i = 0; i < Buffer->nSubBuffers; i ++ )
  50         {
  51                 if( Buffer->SubBuffers[i].Cb == NULL )
  52                         continue ;
  53                 Buffer->SubBuffers[i].Cb(
  54                         Buffer->SubBuffers[i].CbArg,
  55                         Buffer->SubBuffers[i].PreLength,
  56                         Buffer->SubBuffers[i].PostLength,
  57                         Buffer->SubBuffers[i].Data
  58                         );
  59         }
  60         Buffer->nSubBuffers = 0;
  61         IPStack_Buffer_UnlockBuffer(Buffer);
  62 }
  63
  64 void IPStack_Buffer_DestroyBuffer(tIPStackBuffer *Buffer)
  65 {
  66         LOG("Called with %p by %p", Buffer, __builtin_return_address(0));
  67         ASSERT(Buffer);
  68         IPStack_Buffer_ClearBuffer(Buffer);
  69         Buffer->MaxSubBufffers = 0;
  70         free(Buffer);
  71 }
  72
  73 void IPStack_Buffer_LockBuffer(tIPStackBuffer *Buffer)
  74 {
  75         ASSERT(Buffer);
  76         Mutex_Acquire(&Buffer->lBufferLock);
  77 }
  78 void IPStack_Buffer_UnlockBuffer(tIPStackBuffer *Buffer)
  79 {
  80         ASSERT(Buffer);
  81         Mutex_Release(&Buffer->lBufferLock);
  82 }
  83
  84 void IPStack_Buffer_AppendSubBuffer(tIPStackBuffer *Buffer,
  85         size_t HeaderLen, size_t FooterLen, const void *Data,
  86         tIPStackBufferCb Cb, void *Arg
  87         )
  88 {
  89         ASSERT(Buffer);
  90         if( Buffer->nSubBuffers == Buffer->MaxSubBufffers ) {
  91                 // Ah, oops?
  92                 Log_Error("IPStack", "Buffer %p only had %i sub-buffers allocated, which was not enough",
  93                         Buffer, Buffer->MaxSubBufffers);
  94                 return ;
  95         }
  96
  97         int index = Buffer->nSubBuffers++;
  98         Buffer->TotalLength += HeaderLen + FooterLen;
  99
 100         Buffer->SubBuffers[index].Data = Data;
 101         Buffer->SubBuffers[index].PreLength = HeaderLen;
 102         Buffer->SubBuffers[index].PostLength = FooterLen;
 103         Buffer->SubBuffers[index].Cb = Cb;
 104         Buffer->SubBuffers[index].CbArg = Arg;
 105 }
 106
 107 size_t IPStack_Buffer_GetLength(tIPStackBuffer *Buffer)
 108 {
 109         ASSERT(Buffer);
 110         return Buffer->TotalLength;
 111 }
 112
 113 size_t IPStack_Buffer_GetData(tIPStackBuffer *Buffer, void *Dest, size_t MaxBytes)
 114 {
 115         ASSERT(Buffer);
 116         Uint8   *dest = Dest;
 117         size_t  rem_space = MaxBytes;
 118         size_t  len;
 119
 120         for( int i = Buffer->nSubBuffers; i -- && rem_space != 0; )
 121         {
 122                 len = MIN(Buffer->SubBuffers[i].PreLength, rem_space);
 123                 #if !DISABLE_ASSERTS
 124                 if( !CheckMem(Buffer->SubBuffers[i].Data, len) ) {
 125                         Log_Error("IPStack", "Buffer pre %i invalid (%p+0x%x)",
 126                                 i, Buffer->SubBuffers[i].Data, len);
 127                         return 0;
 128                 }
 129                 #endif
 130                 memcpy(dest,
 131                         Buffer->SubBuffers[i].Data,
 132                         len
 133                         );
 134                 dest += len;
 135                 rem_space -= len;
 136         }
 137         for( int i = 0; i < Buffer->nSubBuffers && rem_space; i ++ )
 138         {
 139                 if( Buffer->SubBuffers[i].PostLength == 0  )
 140                         continue ;
 141
 142                 len = MIN(Buffer->SubBuffers[i].PostLength, rem_space);
 143                 void *ptr = (Uint8*)Buffer->SubBuffers[i].Data + Buffer->SubBuffers[i].PreLength;
 144                 ASSERT( CheckMem(ptr, len) );
 145                 memcpy(dest, ptr, len);
 146                 dest += len;
 147                 rem_space -= len;
 148         }
 149
 150         return MaxBytes - rem_space;
 151 }
 152
 153 void *IPStack_Buffer_CompactBuffer(tIPStackBuffer *Buffer, size_t *Length)
 154 {
 155         ASSERT(Buffer);
 156         void    *ret = malloc(Buffer->TotalLength);
 157         if(!ret) {
 158                 *Length = 0;
 159                 return NULL;
 160         }
 161
 162         *Length = Buffer->TotalLength;
 163
 164         IPStack_Buffer_GetData(Buffer, ret, Buffer->TotalLength);
 165
 166         return ret;
 167 }
 168
 169 int IPStack_Buffer_GetBuffer(tIPStackBuffer *Buffer, int Index, size_t *Length, const void **DataPtr)
 170 {
 171         ASSERT(Buffer);
 172         if( Index == -1 )       Index = 0;
 173
 174         if( Index >= Buffer->nSubBuffers*2 ) {
 175                 return -1;
 176         }
 177
 178         if( Index >= Buffer->nSubBuffers )
 179         {
 180                 // Appended buffers
 181                 Index -= Buffer->nSubBuffers;
 182
 183                 // Bit of a hack to avoid multiple calls which return a zero length
 184                 while( !Buffer->SubBuffers[Index].PostLength )
 185                 {
 186                         if( Index++ == Buffer->nSubBuffers )
 187                                 return -1;
 188                 }
 189
 190                 if( DataPtr )
 191                         *DataPtr = (Uint8*)Buffer->SubBuffers[Index].Data + Buffer->SubBuffers[Index].PreLength;
 192                 if( Length )
 193                         *Length = Buffer->SubBuffers[Index].PostLength;
 194
 195                 return (Index + 1) + Buffer->nSubBuffers;
 196         }
 197         else
 198         {
 199                  int    rv = Index + 1;
 200                 Index = Buffer->nSubBuffers - Index - 1;
 201                 // Prepended buffers
 202                 if( DataPtr )
 203                         *DataPtr = Buffer->SubBuffers[Index].Data;
 204                 if( Length )
 205                         *Length = Buffer->SubBuffers[Index].PreLength;
 206                 return rv;
 207         }
 208 }
 209