KernelLand/Modules/Interfaces/UDI/udi_lib/core/buf.c

   1 /**
   2  * \file buf.c
   3  * \author John Hodge (thePowersGang)
   4  *
   5  * Buffer Manipulation
   6  */
   7 #define DEBUG   0
   8 #include <acess.h>
   9 #include <udi.h>
  10 #include <udi_internal.h>
  11
  12 typedef struct sUDI_BufTag
  13 {
  14         struct sUDI_BufTag      *Next;
  15 //      udi_buf_tag_t   tag;
  16         struct sUDI_BufSect     *Sect;
  17 } tUDI_BufTag;
  18
  19 typedef struct sUDI_BufSect
  20 {
  21         struct sUDI_BufSect     *Next;
  22         size_t  RelOfs;
  23         size_t  Length;
  24         size_t  Space;
  25         void    *Data;
  26         // data
  27 } tUDI_BufSect;
  28
  29 typedef struct
  30 {
  31         udi_buf_t       buf;
  32         tUDI_BufTag     *Tags;
  33         tUDI_BufSect    *Sections;
  34 } tUDI_BufInt;
  35
  36 // === EXPORTS ===
  37 EXPORT(udi_buf_copy);
  38 EXPORT(udi_buf_write);
  39 EXPORT(udi_buf_read);
  40 EXPORT(udi_buf_free);
  41
  42 // === CODE ===
  43 void udi_buf_copy(
  44         udi_buf_copy_call_t *callback,
  45         udi_cb_t        *gcb,
  46         udi_buf_t       *src_buf,
  47         udi_size_t      src_off,
  48         udi_size_t      src_len,
  49         udi_buf_t       *dst_buf,
  50         udi_size_t      dst_off,
  51         udi_size_t      dst_len,
  52         udi_buf_path_t  path_handle
  53         )
  54 {
  55         UNIMPLEMENTED();
  56 }
  57
  58 tUDI_BufSect *UDI_int_BufAddSect(size_t data_len, size_t relofs, tUDI_BufSect **prevptr, const void *data,
  59         udi_buf_path_t path_handle
  60         )
  61 {
  62         const int       space_atom = 1<<7;
  63         size_t  space = (data_len + space_atom-1) & ~(space_atom-1);
  64         tUDI_BufSect    *newsect = NEW(tUDI_BufSect, + space);
  65         newsect->RelOfs = relofs;
  66         newsect->Length = data_len;
  67         newsect->Space = space_atom;
  68         newsect->Data = newsect+1;
  69         memcpy(newsect->Data, data, data_len);
  70
  71         newsect->Next = *prevptr;
  72         *prevptr = newsect;
  73         LOG("@0x%x : %p <= %p+%i", relofs, newsect->Data, data, data_len);
  74
  75         return newsect;
  76 }
  77
  78 udi_buf_t *_udi_buf_allocate(const void *data, udi_size_t length, udi_buf_path_t path_handle)
  79 {
  80         tUDI_BufInt *buf = NEW(tUDI_BufInt,);
  81         udi_buf_t *ret = &buf->buf;
  82
  83         if( data ) {
  84                 UDI_int_BufAddSect(length, 0, &buf->Sections, data, path_handle);
  85         }
  86         ret->buf_size = length;
  87
  88         return ret;
  89 }
  90
  91 /**
  92  * \brief Write to a buffer
  93  * \param callback      Function to call once the write has completed
  94  * \param gcb   Control Block
  95  * \param src_mem       Source Data
  96  * \param src_len       Length of source data
  97  * \param dst_buf       Destination buffer
  98  * \param dst_off       Destination offset in the buffer
  99  * \param dst_len       Length of area to be replaced
 100  * \param path_handle   ???
 101  */
 102 void udi_buf_write(
 103         udi_buf_write_call_t *callback,
 104         udi_cb_t        *gcb,
 105         const void      *src_mem,
 106         udi_size_t      src_len,
 107         udi_buf_t       *dst_buf,
 108         udi_size_t      dst_off,
 109         udi_size_t      dst_len,
 110         udi_buf_path_t path_handle
 111         )
 112 {
 113         tUDI_BufInt     *dst = (void*)dst_buf;
 114         if( !dst ) {
 115                 dst = NEW(tUDI_BufInt,);
 116                 dst_buf = &dst->buf;
 117         }
 118
 119         tUDI_BufSect    **prevptr = &dst->Sections;
 120         tUDI_BufSect    *sect = dst->Sections;
 121         for( ; sect; prevptr = &sect->Next, sect = sect->Next )
 122         {
 123                 if(sect->RelOfs >= dst_off)
 124                         break;
 125                 if(sect->RelOfs + sect->Length > dst_off)
 126                         break ;
 127                 dst_off -= sect->RelOfs + sect->Length;
 128         }
 129
 130         // Find dst_off segment (or create one if src_len>0)
 131         // Copy in src data
 132         // or (if src_len==0) delete dst_len bytes
 133         if( dst_len == 0 )
 134         {
 135                 // Insert / Initialise
 136                 if( src_len > 0 )
 137                 {
 138                         if( !sect || sect->RelOfs > dst_off ) {
 139                                 // Simple: Just add a new section
 140                                 UDI_int_BufAddSect(src_len, dst_off, prevptr, src_mem, path_handle);
 141                                 dst_buf->buf_size += src_len;
 142                         }
 143                         else if( sect->RelOfs + sect->Length == dst_off ) {
 144                                 // End of block inserts
 145                                 size_t  avail = sect->Space - sect->Length;
 146                                 if( avail ) {
 147                                         size_t  bytes = MIN(avail, src_len);
 148                                         memcpy(sect->Data + sect->Length, src_mem, bytes);
 149                                         src_mem += bytes;
 150                                         src_len -= bytes;
 151                                 }
 152                                 if( src_len ) {
 153                                         // New block(s)
 154                                         UDI_int_BufAddSect(src_len, 0, prevptr, src_mem, path_handle);
 155                                 }
 156                                 dst_buf->buf_size += src_len;
 157                         }
 158                         else {
 159                                 // Complex: Need to handle mid-section inserts
 160                                 Log_Warning("UDI", "TODO: udi_buf_write - mid-section inserts");
 161                         }
 162                 }
 163                 // no-op
 164                 else
 165                 {
 166                 }
 167         }
 168         else
 169         {
 170                 // Delete
 171                 if( src_len == 0 )
 172                 {
 173                         Log_Warning("UDI", "TODO: udi_buf_write - delete");
 174                 }
 175                 // Overwrite
 176                 else
 177                 {
 178                         Log_Warning("UDI", "TODO: udi_buf_write - overwrite");
 179                 }
 180         }
 181
 182         callback(gcb, &dst->buf);
 183 }
 184
 185 void udi_buf_read(
 186         udi_buf_t       *src_buf,
 187         udi_size_t      src_off,
 188         udi_size_t      src_len,
 189         void    *dst_mem )
 190 {
 191         ENTER("psrc_buf isrc_off isrc_len pdst_mem",
 192                 src_buf, src_off, src_len, dst_mem);
 193         tUDI_BufInt     *src = (void*)src_buf;
 194
 195         tUDI_BufSect    *sect = src->Sections;
 196         while( src_len )
 197         {
 198                 for( ; sect; sect = sect->Next )
 199                 {
 200                         LOG("%x <= %x <= +%x", sect->RelOfs, src_off, sect->Length);
 201                         if(sect->RelOfs >= src_off)
 202                                 break;
 203                         if(sect->RelOfs + sect->Length > src_off)
 204                                 break;
 205                         src_off -= sect->RelOfs + sect->Length;
 206                 }
 207                 if( !sect ) {
 208                         LOG("no section");
 209                         break;
 210                 }
 211                 if( src_off < sect->RelOfs ) {
 212                         size_t  undef_len = MIN(src_len, sect->RelOfs - src_off);
 213                         LOG("%i undef", undef_len);
 214                         memset(dst_mem, 0xFF, undef_len);
 215                         dst_mem += undef_len;
 216                         src_len -= undef_len;
 217                         src_off += undef_len;
 218                 }
 219                 if( src_len == 0 )
 220                         break;
 221                 ASSERTC(src_off, >=, sect->RelOfs);
 222                 size_t  ofs = src_off - sect->RelOfs;
 223                 size_t  len = MIN(src_len, sect->Length - ofs);
 224                 LOG("%i data from %p + %i", len, sect->Data, ofs);
 225                 memcpy(dst_mem, sect->Data+ofs, len);
 226                 dst_mem += len;
 227                 src_len -= len;
 228
 229                 src_off -= sect->RelOfs + sect->Length;
 230                 sect = sect->Next;
 231         }
 232         LEAVE('-');
 233 }
 234
 235 void udi_buf_free(udi_buf_t *buf)
 236 {
 237         UNIMPLEMENTED();
 238 }