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         if( !src_len ) {
  56                 // why?
  57         }
  58         // Quick and evil option - allocate temp buffer, udi_buf_read + udi_buf_write
  59         void    *tmp = malloc(src_len);
  60         udi_buf_read(src_buf, src_off, src_len, tmp);
  61
  62         void tmp_callback(udi_cb_t *gcb, udi_buf_t *new_buf) {
  63                 dst_buf = new_buf;
  64         }
  65
  66         udi_buf_write(tmp_callback, NULL, tmp, src_len, dst_buf, dst_off, dst_len, path_handle);
  67         free(tmp);
  68
  69         if( callback ) {
  70                 callback(gcb, dst_buf);
  71         }
  72 }
  73
  74 tUDI_BufSect *UDI_int_BufAddSect(size_t data_len, size_t relofs, tUDI_BufSect **prevptr, const void *data,
  75         udi_buf_path_t path_handle
  76         )
  77 {
  78         const int       space_atom = 1<<7;
  79         size_t  space = (data_len + space_atom-1) & ~(space_atom-1);
  80         tUDI_BufSect    *newsect = NEW(tUDI_BufSect, + space);
  81         newsect->RelOfs = relofs;
  82         newsect->Length = data_len;
  83         newsect->Space = space_atom;
  84         newsect->Data = newsect+1;
  85         if( !data )
  86                 memset(newsect->Data, 0xFF, data_len);
  87         else
  88                 memcpy(newsect->Data, data, data_len);
  89
  90         newsect->Next = *prevptr;
  91         *prevptr = newsect;
  92         LOG("@0x%x : %p <= %p+%i", relofs, newsect->Data, data, data_len);
  93
  94         return newsect;
  95 }
  96
  97 udi_buf_t *_udi_buf_allocate(const void *data, udi_size_t length, udi_buf_path_t path_handle)
  98 {
  99         tUDI_BufInt *buf = NEW(tUDI_BufInt,);
 100         udi_buf_t *ret = &buf->buf;
 101
 102         if( data ) {
 103                 UDI_int_BufAddSect(length, 0, &buf->Sections, data, path_handle);
 104         }
 105         ret->buf_size = length;
 106
 107         return ret;
 108 }
 109
 110 /**
 111  * \brief Write to a buffer
 112  * \param callback      Function to call once the write has completed
 113  * \param gcb   Control Block
 114  * \param src_mem       Source Data
 115  * \param src_len       Length of source data
 116  * \param dst_buf       Destination buffer
 117  * \param dst_off       Destination offset in the buffer
 118  * \param dst_len       Length of area to be replaced
 119  * \param path_handle   ???
 120  */
 121 void udi_buf_write(
 122         udi_buf_write_call_t *callback,
 123         udi_cb_t        *gcb,
 124         const void      *src_mem,
 125         udi_size_t      src_len,
 126         udi_buf_t       *dst_buf,
 127         udi_size_t      dst_off,
 128         udi_size_t      dst_len,
 129         udi_buf_path_t path_handle
 130         )
 131 {
 132         ENTER("psrc_mem isrc_len pdst_buf idst_off idst_len",
 133                 src_mem, src_len, dst_buf, dst_off, dst_len);
 134
 135         tUDI_BufInt     *dst = (void*)dst_buf;
 136         if( !dst ) {
 137                 dst = NEW(tUDI_BufInt,);
 138                 dst_buf = &dst->buf;
 139         }
 140
 141         // Find dst_off segment
 142         tUDI_BufSect    **prevptr = &dst->Sections;
 143         tUDI_BufSect    *sect = dst->Sections;
 144         for( ; sect; prevptr = &sect->Next, sect = sect->Next )
 145         {
 146                 if(sect->RelOfs >= dst_off)
 147                         break;
 148                 if(sect->RelOfs + sect->Length > dst_off)
 149                         break ;
 150                 dst_off -= sect->RelOfs + sect->Length;
 151         }
 152
 153         LOG("sect = %p", sect);
 154
 155         // Overwrite MIN(src_len,dst_len) bytes
 156         // then delete/append remainder
 157         size_t  len = MIN(src_len,dst_len);
 158         src_len -= len;
 159         dst_len -= len;
 160         while( len > 0 )
 161         {
 162                 LOG("Overwriting %i bytes", len);
 163                 // Create new section
 164                 if( !sect || sect->RelOfs > dst_off ) {
 165                         size_t  newsize = (sect && sect->RelOfs - dst_off < len) ? sect->RelOfs - dst_off : len;
 166                         sect = UDI_int_BufAddSect(len, dst_off, prevptr, src_mem, path_handle);
 167                         len -= newsize;
 168                         src_mem += newsize;
 169                         prevptr = &sect->Next;
 170                         sect = sect->Next;
 171                         dst_off = 0;
 172                 }
 173                 if( len == 0 )
 174                         break;
 175                 LOG("- dst_off = %i, data=%p", dst_off, sect->Data);
 176
 177                 // Update existing section
 178                 size_t  bytes = MIN(len, sect->Length - dst_off);
 179                 memcpy(sect->Data + dst_off, src_mem, bytes);
 180                 len -= bytes;
 181                 src_mem += bytes;
 182
 183                 dst_off += bytes;
 184                 if( dst_off == sect->Length )
 185                 {
 186                         prevptr = &sect->Next;
 187                         sect = sect->Next;
 188                         dst_off = 0;
 189                 }
 190         }
 191
 192         if( dst_len > 0 )
 193         {
 194                 LOG("Deleting %i bytes at %i", dst_len, dst_off);
 195                 ASSERT(src_len == 0);
 196                 // Delete
 197                 while( dst_len > 0 )
 198                 {
 199                         if( !sect ) {
 200                                 dst_buf->buf_size = dst_off;
 201                                 dst_len = 0;
 202                         }
 203                         else if( sect->RelOfs > dst_off ) {
 204                                 size_t  bytes = MIN(dst_len, sect->RelOfs - dst_off);
 205                                 dst_len -= bytes;
 206                                 dst_buf->buf_size -= bytes;
 207                                 sect->RelOfs -= bytes;
 208                         }
 209                         else if( dst_off == 0 && sect->Length <= dst_len ) {
 210                                 // Remove entire section
 211                                 dst_len -= sect->Length;
 212                                 dst_buf->buf_size -= sect->Length;
 213                                 *prevptr = sect->Next;
 214                                 free(sect);
 215
 216                                 // Next block
 217                                 sect = *prevptr;
 218                         }
 219                         else if( dst_off + dst_len >= sect->Length ) {
 220                                 // block truncate
 221                                 size_t  bytes = MIN(dst_len, sect->Length - dst_off);
 222                                 dst_len -= bytes;
 223                                 dst_buf->buf_size -= bytes;
 224                                 sect->Length -= bytes;
 225
 226                                 // Next block
 227                                 prevptr = &sect->Next;
 228                                 sect = sect->Next;
 229                         }
 230                         else {
 231                                 // in-block removal (must be last)
 232                                 ASSERTC(dst_off + dst_len, <, sect->Length);
 233                                 size_t  tail = sect->Length - (dst_off + dst_len);
 234                                 memmove(sect->Data+dst_off, sect->Data+dst_off+dst_len, tail);
 235                                 dst_buf->buf_size -= dst_len;
 236                                 sect->Length -= dst_len;
 237                                 dst_len = 0;
 238                         }
 239                 }
 240         }
 241         else if( src_len > 0 )
 242         {
 243                 LOG("Inserting %i bytes", src_len);
 244                 ASSERT(dst_len == 0);
 245                 // Insert
 246                 if( !sect || sect->RelOfs > dst_off ) {
 247                         // Simple: Just add a new section
 248                         UDI_int_BufAddSect(src_len, dst_off, prevptr, src_mem, path_handle);
 249                         dst_buf->buf_size += src_len;
 250                 }
 251                 else if( sect->RelOfs + sect->Length == dst_off ) {
 252                         // End of block inserts
 253                         size_t  avail = sect->Space - sect->Length;
 254                         if( avail ) {
 255                                 size_t  bytes = MIN(avail, src_len);
 256                                 ASSERT(src_mem);
 257                                 memcpy(sect->Data + sect->Length, src_mem, bytes);
 258                                 src_mem += bytes;
 259                                 src_len -= bytes;
 260                         }
 261                         if( src_len ) {
 262                                 // New block(s)
 263                                 UDI_int_BufAddSect(src_len, 0, prevptr, src_mem, path_handle);
 264                         }
 265                         dst_buf->buf_size += src_len;
 266                 }
 267                 else {
 268                         // Complex: Need to handle mid-section inserts
 269                         Log_Warning("UDI", "TODO: udi_buf_write - mid-section inserts");
 270                 }
 271         }
 272         else
 273         {
 274                 LOG("No insert/delete, ovr only");
 275                 // No-op
 276         }
 277
 278         LOG("dst_buf->size = %i", dst->buf.buf_size);
 279         LEAVE('p', &dst->buf);
 280         // HACK: udi_pio_trans calls this with a NULL cb, so handle that
 281         if( callback ) {
 282                 callback(gcb, &dst->buf);
 283         }
 284 }
 285
 286 void udi_buf_read(
 287         udi_buf_t       *src_buf,
 288         udi_size_t      src_off,
 289         udi_size_t      src_len,
 290         void    *dst_mem )
 291 {
 292         ENTER("psrc_buf isrc_off isrc_len pdst_mem",
 293                 src_buf, src_off, src_len, dst_mem);
 294         tUDI_BufInt     *src = (void*)src_buf;
 295
 296         tUDI_BufSect    *sect = src->Sections;
 297         while( src_len )
 298         {
 299                 for( ; sect; sect = sect->Next )
 300                 {
 301                         LOG("%x <= %x <= +%x", sect->RelOfs, src_off, sect->Length);
 302                         if(sect->RelOfs >= src_off)
 303                                 break;
 304                         if(sect->RelOfs + sect->Length > src_off)
 305                                 break;
 306                         src_off -= sect->RelOfs + sect->Length;
 307                 }
 308                 if( !sect ) {
 309                         LOG("no section");
 310                         break;
 311                 }
 312                 if( src_off < sect->RelOfs ) {
 313                         size_t  undef_len = MIN(src_len, sect->RelOfs - src_off);
 314                         LOG("%i undef", undef_len);
 315                         memset(dst_mem, 0xFF, undef_len);
 316                         dst_mem += undef_len;
 317                         src_len -= undef_len;
 318                         src_off += undef_len;
 319                 }
 320                 if( src_len == 0 )
 321                         break;
 322                 ASSERTC(src_off, >=, sect->RelOfs);
 323                 size_t  ofs = src_off - sect->RelOfs;
 324                 size_t  len = MIN(src_len, sect->Length - ofs);
 325                 LOG("%i data from %p + %i", len, sect->Data, ofs);
 326                 memcpy(dst_mem, sect->Data+ofs, len);
 327                 dst_mem += len;
 328                 src_len -= len;
 329
 330                 src_off -= sect->RelOfs + sect->Length;
 331                 sect = sect->Next;
 332         }
 333         LEAVE('-');
 334 }
 335
 336 void udi_buf_free(udi_buf_t *buf_ptr)
 337 {
 338         if( buf_ptr )
 339         {
 340                 tUDI_BufInt     *buf = (void*)buf_ptr;
 341
 342                 while( buf->Tags )
 343                 {
 344                         tUDI_BufTag *tag = buf->Tags;
 345                         buf->Tags = tag->Next;
 346
 347                         free(tag);
 348                 }
 349
 350                 while( buf->Sections )
 351                 {
 352                         tUDI_BufSect *sect = buf->Sections;
 353                         buf->Sections = sect->Next;
 354
 355                         free(sect);
 356                 }
 357
 358                 free(buf);
 359         }
 360 }
 361