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         if( !data )
  70                 memset(newsect->Data, 0xFF, data_len);
  71         else
  72                 memcpy(newsect->Data, data, data_len);
  73
  74         newsect->Next = *prevptr;
  75         *prevptr = newsect;
  76         LOG("@0x%x : %p <= %p+%i", relofs, newsect->Data, data, data_len);
  77
  78         return newsect;
  79 }
  80
  81 udi_buf_t *_udi_buf_allocate(const void *data, udi_size_t length, udi_buf_path_t path_handle)
  82 {
  83         tUDI_BufInt *buf = NEW(tUDI_BufInt,);
  84         udi_buf_t *ret = &buf->buf;
  85
  86         if( data ) {
  87                 UDI_int_BufAddSect(length, 0, &buf->Sections, data, path_handle);
  88         }
  89         ret->buf_size = length;
  90
  91         return ret;
  92 }
  93
  94 /**
  95  * \brief Write to a buffer
  96  * \param callback      Function to call once the write has completed
  97  * \param gcb   Control Block
  98  * \param src_mem       Source Data
  99  * \param src_len       Length of source data
 100  * \param dst_buf       Destination buffer
 101  * \param dst_off       Destination offset in the buffer
 102  * \param dst_len       Length of area to be replaced
 103  * \param path_handle   ???
 104  */
 105 void udi_buf_write(
 106         udi_buf_write_call_t *callback,
 107         udi_cb_t        *gcb,
 108         const void      *src_mem,
 109         udi_size_t      src_len,
 110         udi_buf_t       *dst_buf,
 111         udi_size_t      dst_off,
 112         udi_size_t      dst_len,
 113         udi_buf_path_t path_handle
 114         )
 115 {
 116         tUDI_BufInt     *dst = (void*)dst_buf;
 117         if( !dst ) {
 118                 dst = NEW(tUDI_BufInt,);
 119                 dst_buf = &dst->buf;
 120         }
 121
 122         // Find dst_off segment
 123         tUDI_BufSect    **prevptr = &dst->Sections;
 124         tUDI_BufSect    *sect = dst->Sections;
 125         for( ; sect; prevptr = &sect->Next, sect = sect->Next )
 126         {
 127                 if(sect->RelOfs >= dst_off)
 128                         break;
 129                 if(sect->RelOfs + sect->Length > dst_off)
 130                         break ;
 131                 dst_off -= sect->RelOfs + sect->Length;
 132         }
 133
 134         // Overwrite MIN(src_len,dst_len) bytes
 135         // then delete/append remainder
 136         size_t  len = MIN(src_len,dst_len);
 137         src_len -= len;
 138         dst_len -= len;
 139         while( len > 0 )
 140         {
 141                 // Create new section
 142                 if( !sect || sect->RelOfs > dst_off ) {
 143                         size_t  newsize = MIN(sect->RelOfs - dst_off, len);
 144                         sect = UDI_int_BufAddSect(len, dst_off, prevptr, src_mem, path_handle);
 145                         len -= newsize;
 146                         src_mem += newsize;
 147                         prevptr = &sect->Next;
 148                         sect = sect->Next;
 149                         dst_off = 0;
 150                 }
 151                 if( len == 0 )
 152                         break;
 153
 154                 // Update existing section
 155                 size_t  bytes = MIN(len, sect->Length - dst_off);
 156                 memcpy(sect->Data + dst_off, src_mem, bytes);
 157                 len -= bytes;
 158                 src_mem += bytes;
 159
 160                 prevptr = &sect->Next;
 161                 sect = sect->Next;
 162         }
 163
 164         if( dst_len > 0 )
 165         {
 166                 ASSERT(src_len == 0);
 167                 // Delete
 168                 while( dst_len > 0 )
 169                 {
 170                         if( !sect ) {
 171                                 dst_buf->buf_size = dst_off;
 172                                 dst_len = 0;
 173                         }
 174                         else if( sect->RelOfs > dst_off ) {
 175                                 size_t  bytes = MIN(dst_len, sect->RelOfs - dst_off);
 176                                 dst_len -= bytes;
 177                                 dst_buf->buf_size -= bytes;
 178                                 sect->RelOfs -= bytes;
 179                         }
 180                         else if( dst_off == 0 && sect->Length <= dst_len ) {
 181                                 // Remove entire section
 182                                 dst_len -= sect->Length;
 183                                 dst_buf->buf_size -= sect->Length;
 184                                 *prevptr = sect->Next;
 185                                 free(sect);
 186
 187                                 // Next block
 188                                 sect = *prevptr;
 189                         }
 190                         else if( dst_off + dst_len >= sect->Length ) {
 191                                 // block truncate
 192                                 size_t  bytes = MIN(dst_len, sect->Length - dst_off);
 193                                 dst_len -= bytes;
 194                                 dst_buf->buf_size -= bytes;
 195                                 sect->Length -= bytes;
 196
 197                                 // Next block
 198                                 prevptr = &sect->Next;
 199                                 sect = sect->Next;
 200                         }
 201                         else {
 202                                 // in-block removal (must be last)
 203                                 ASSERTC(dst_off + dst_len, <, sect->Length);
 204                                 size_t  tail = sect->Length - (dst_off + dst_len);
 205                                 memmove(sect->Data+dst_off, sect->Data+dst_off+dst_len, tail);
 206                                 dst_buf->buf_size -= dst_len;
 207                                 sect->Length -= dst_len;
 208                                 dst_len = 0;
 209                         }
 210                 }
 211         }
 212         else if( src_len > 0 )
 213         {
 214                 ASSERT(dst_len == 0);
 215                 // Insert
 216                 if( !sect || sect->RelOfs > dst_off ) {
 217                         // Simple: Just add a new section
 218                         UDI_int_BufAddSect(src_len, dst_off, prevptr, src_mem, path_handle);
 219                         dst_buf->buf_size += src_len;
 220                 }
 221                 else if( sect->RelOfs + sect->Length == dst_off ) {
 222                         // End of block inserts
 223                         size_t  avail = sect->Space - sect->Length;
 224                         if( avail ) {
 225                                 size_t  bytes = MIN(avail, src_len);
 226                                 memcpy(sect->Data + sect->Length, src_mem, bytes);
 227                                 src_mem += bytes;
 228                                 src_len -= bytes;
 229                         }
 230                         if( src_len ) {
 231                                 // New block(s)
 232                                 UDI_int_BufAddSect(src_len, 0, prevptr, src_mem, path_handle);
 233                         }
 234                         dst_buf->buf_size += src_len;
 235                 }
 236                 else {
 237                         // Complex: Need to handle mid-section inserts
 238                         Log_Warning("UDI", "TODO: udi_buf_write - mid-section inserts");
 239                 }
 240         }
 241         else
 242         {
 243                 // No-op
 244         }
 245
 246         // HACK: udi_pio_trans calls this with a NULL cb, so handle that
 247         if( callback ) {
 248                 callback(gcb, &dst->buf);
 249         }
 250 }
 251
 252 void udi_buf_read(
 253         udi_buf_t       *src_buf,
 254         udi_size_t      src_off,
 255         udi_size_t      src_len,
 256         void    *dst_mem )
 257 {
 258         ENTER("psrc_buf isrc_off isrc_len pdst_mem",
 259                 src_buf, src_off, src_len, dst_mem);
 260         tUDI_BufInt     *src = (void*)src_buf;
 261
 262         tUDI_BufSect    *sect = src->Sections;
 263         while( src_len )
 264         {
 265                 for( ; sect; sect = sect->Next )
 266                 {
 267                         LOG("%x <= %x <= +%x", sect->RelOfs, src_off, sect->Length);
 268                         if(sect->RelOfs >= src_off)
 269                                 break;
 270                         if(sect->RelOfs + sect->Length > src_off)
 271                                 break;
 272                         src_off -= sect->RelOfs + sect->Length;
 273                 }
 274                 if( !sect ) {
 275                         LOG("no section");
 276                         break;
 277                 }
 278                 if( src_off < sect->RelOfs ) {
 279                         size_t  undef_len = MIN(src_len, sect->RelOfs - src_off);
 280                         LOG("%i undef", undef_len);
 281                         memset(dst_mem, 0xFF, undef_len);
 282                         dst_mem += undef_len;
 283                         src_len -= undef_len;
 284                         src_off += undef_len;
 285                 }
 286                 if( src_len == 0 )
 287                         break;
 288                 ASSERTC(src_off, >=, sect->RelOfs);
 289                 size_t  ofs = src_off - sect->RelOfs;
 290                 size_t  len = MIN(src_len, sect->Length - ofs);
 291                 LOG("%i data from %p + %i", len, sect->Data, ofs);
 292                 memcpy(dst_mem, sect->Data+ofs, len);
 293                 dst_mem += len;
 294                 src_len -= len;
 295
 296                 src_off -= sect->RelOfs + sect->Length;
 297                 sect = sect->Next;
 298         }
 299         LEAVE('-');
 300 }
 301
 302 void udi_buf_free(udi_buf_t *buf)
 303 {
 304         UNIMPLEMENTED();
 305 }