qemu/roms/openbios/libopenbios/ofmem_common.c

   1 /*
   2  *      <ofmem_common.c>
   3  *
   4  *      OF Memory manager
   5  *
   6  *   Copyright (C) 1999-2004 Samuel Rydh (samuel@ibrium.se)
   7  *   Copyright (C) 2004 Stefan Reinauer
   8  *
   9  *   This program is free software; you can redistribute it and/or
  10  *   modify it under the terms of the GNU General Public License
  11  *   as published by the Free Software Foundation
  12  *
  13  */
  14
  15 #include "config.h"
  16 #include "libopenbios/bindings.h"
  17 #include "libopenbios/ofmem.h"
  18
  19 /* Default size of memory allocated for each of the MMU properties (in bytes) */
  20 #define OFMEM_DEFAULT_PROP_SIZE 2048
  21
  22 /*
  23  * define OFMEM_FILL_RANGE to claim any unclaimed virtual and
  24  * physical memory in the range for ofmem_map
  25  *
  26  * TODO: remove this macro and wrapped code if not needed by implementations
  27  */
  28 //#define OFMEM_FILL_RANGE
  29
  30
  31 static inline size_t align_size(size_t x, size_t a)
  32 {
  33     return (x + a - 1) & ~(a - 1);
  34 }
  35
  36 static inline phys_addr_t align_ptr(uintptr_t x, size_t a)
  37 {
  38     return (x + a - 1) & ~(a - 1);
  39 }
  40
  41 static ucell get_ram_size( void )
  42 {
  43         ofmem_t *ofmem = ofmem_arch_get_private();
  44         return ofmem->ramsize;
  45 }
  46
  47 /************************************************************************/
  48 /* debug                                                                */
  49 /************************************************************************/
  50
  51 #if 0
  52 static void
  53 print_range( range_t *r, const char *str )
  54 {
  55         printk("--- Range %s ---\n", str );
  56         for( ; r; r=r->next )
  57                 printk("%p : " FMT_plx " - " FMT_plx "\n", r, r->start, r->start + r->size - 1);
  58         printk("\n");
  59 }
  60
  61 static void
  62 print_phys_range(void)
  63 {
  64         ofmem_t *ofmem = ofmem_arch_get_private();
  65         print_range( ofmem->phys_range, "phys" );
  66 }
  67
  68 static void
  69 print_virt_range(void)
  70 {
  71         ofmem_t *ofmem = ofmem_arch_get_private();
  72         print_range( ofmem->virt_range, "virt" );
  73 }
  74
  75 static void
  76 print_trans( void )
  77 {
  78         ofmem_t *ofmem = ofmem_arch_get_private();
  79         translation_t *t = ofmem->trans;
  80
  81         printk("--- Translations ---\n");
  82         for( ; t; t=t->next )
  83                 printk("%p : " FMT_ucellx " -> " FMT_plx " [size " FMT_ucellx "]\n", t, t->virt, t->phys, t->size);
  84         printk("\n");
  85 }
  86 #endif
  87
  88 /************************************************************************/
  89 /* OF private allocations                                               */
  90 /************************************************************************/
  91
  92 int ofmem_posix_memalign( void **memptr, size_t alignment, size_t size )
  93 {
  94         ofmem_t *ofmem = ofmem_arch_get_private();
  95         alloc_desc_t *d, **pp;
  96         void *ret;
  97         ucell top;
  98         phys_addr_t pa;
  99
 100         if( !size )
 101                 return ENOMEM;
 102
 103         if( !ofmem->next_malloc )
 104                 ofmem->next_malloc = (char*)ofmem_arch_get_malloc_base();
 105
 106         size = align_size(size + sizeof(alloc_desc_t), alignment);
 107
 108         /* look in the freelist */
 109         for( pp=&ofmem->mfree; *pp && (**pp).size < size; pp = &(**pp).next ) {
 110         }
 111
 112         /* waste at most 4K by taking an entry from the freelist */
 113         if( *pp && (**pp).size > size + 0x1000 ) {
 114                 /* Alignment should be on physical not virtual address */
 115                 pa = va2pa((uintptr_t)*pp + sizeof(alloc_desc_t));
 116                 pa = align_ptr(pa, alignment);
 117                 ret = (void *)pa2va(pa);
 118
 119                 memset( ret, 0, (**pp).size - sizeof(alloc_desc_t) );
 120                 *pp = (**pp).next;
 121
 122                 *memptr = ret;
 123                 return 0;
 124         }
 125
 126         top = ofmem_arch_get_heap_top();
 127
 128         /* Alignment should be on physical not virtual address */
 129         pa = va2pa((uintptr_t)ofmem->next_malloc + sizeof(alloc_desc_t));
 130         pa = align_ptr(pa, alignment);
 131         ret = (void *)pa2va(pa);
 132
 133         if( pointer2cell(ret) + size > top ) {
 134                 printk("out of malloc memory (%x)!\n", size );
 135                 return ENOMEM;
 136         }
 137
 138         d = (alloc_desc_t*)((uintptr_t)ret - sizeof(alloc_desc_t));
 139         ofmem->next_malloc += size;
 140
 141         d->next = NULL;
 142         d->size = size;
 143
 144         memset( ret, 0, size - sizeof(alloc_desc_t) );
 145
 146         *memptr = ret;
 147         return 0;
 148 }
 149
 150 void* ofmem_malloc( size_t size )
 151 {
 152         void *memptr;
 153         int res;
 154
 155         res = ofmem_posix_memalign( &memptr, CONFIG_OFMEM_MALLOC_ALIGN, size );
 156         if (!res) {
 157                 /* Success */
 158                 return memptr;
 159         } else {
 160                 /* Failure */
 161                 return NULL;
 162         }
 163 }
 164
 165 void ofmem_free( void *ptr )
 166 {
 167         ofmem_t *ofmem = ofmem_arch_get_private();
 168         alloc_desc_t **pp, *d;
 169
 170         /* it is legal to free NULL pointers (size zero allocations) */
 171         if( !ptr )
 172                 return;
 173
 174         d = (alloc_desc_t*)((char *)ptr - sizeof(alloc_desc_t));
 175         d->next = ofmem->mfree;
 176
 177         /* insert in the (sorted) freelist */
 178         for( pp=&ofmem->mfree; *pp && (**pp).size < d->size ; pp = &(**pp).next ) {
 179         }
 180
 181         d->next = *pp;
 182         *pp = d;
 183 }
 184
 185 void* ofmem_realloc( void *ptr, size_t size )
 186 {
 187         alloc_desc_t *d = (alloc_desc_t*)((char *)ptr - sizeof(alloc_desc_t));
 188         char *p;
 189
 190         if( !ptr )
 191                 return malloc( size );
 192         if( !size ) {
 193                 free( ptr );
 194                 return NULL;
 195         }
 196         p = malloc( size );
 197         memcpy( p, ptr, MIN(d->size - sizeof(alloc_desc_t),size) );
 198         free( ptr );
 199         return p;
 200 }
 201
 202
 203 /************************************************************************/
 204 /* "translations" and "available" property tracking                     */
 205 /************************************************************************/
 206
 207 static int trans_prop_size = 0, phys_range_prop_size = 0, virt_range_prop_size = 0;
 208 static int trans_prop_used = 0, phys_range_prop_used = 0, virt_range_prop_used = 0;
 209 static ucell *trans_prop, *phys_range_prop, *virt_range_prop;
 210
 211 static void
 212 ofmem_set_property( phandle_t ph, const char *name, const char *buf, int len )
 213 {
 214         /* This is very similar to set_property() in libopenbios/bindings.c but allows
 215            us to set the property pointer directly, rather than having to copy it
 216            into the Forth dictonary every time we update the memory properties */
 217         if( !ph ) {
 218                 printk("ofmem_set_property: NULL phandle\n");
 219                 return;
 220         }
 221         PUSH(pointer2cell(buf));
 222         PUSH(len);
 223         push_str(name);
 224         PUSH_ph(ph);
 225         fword("encode-property");
 226 }
 227
 228 phandle_t s_phandle_memory = 0;
 229 phandle_t s_phandle_mmu = 0;
 230
 231 static void ofmem_update_mmu_translations( void )
 232 {
 233         ofmem_t *ofmem = ofmem_arch_get_private();
 234         translation_t *t;
 235         int ncells, prop_used, prop_size;
 236
 237         if (s_phandle_mmu == 0)
 238                 return;
 239
 240         for( t = ofmem->trans, ncells = 0; t ; t=t->next, ncells++ ) {
 241         }
 242
 243         /* Get the current number of bytes required for the MMU translation property */
 244         prop_used = ncells * sizeof(ucell) * ofmem_arch_get_translation_entry_size();
 245
 246         if (prop_used > trans_prop_size) {
 247
 248                 /* The property doesn't fit within the existing space, so keep doubling it
 249                    until it does */
 250                 prop_size = trans_prop_size;
 251                 while (prop_size < prop_used) {
 252                         prop_size *= 2;
 253                 }
 254
 255                 /* Allocate the new memory and copy all of the existing information across */
 256                 trans_prop = realloc(trans_prop, prop_size);
 257                 trans_prop_size = prop_size;
 258                 trans_prop_used = prop_used;
 259         }
 260
 261         if (trans_prop == NULL) {
 262                 /* out of memory! */
 263                 printk("Unable to allocate memory for translations property!\n");
 264                 return;
 265         }
 266
 267         /* Call architecture-specific routines to generate translation entries */
 268         for( t = ofmem->trans, ncells = 0 ; t ; t=t->next ) {
 269                 ofmem_arch_create_translation_entry(&trans_prop[ncells], t);
 270                 ncells += ofmem_arch_get_translation_entry_size();
 271         }
 272
 273         ofmem_set_property(s_phandle_mmu, "translations",
 274                         (char*)trans_prop, ncells * sizeof(trans_prop[0]));
 275
 276 }
 277
 278
 279 static void ofmem_update_memory_available( phandle_t ph, range_t *range,
 280                 ucell **mem_prop, int *mem_prop_size, int *mem_prop_used, u64 top_address )
 281 {
 282         range_t *r;
 283         int ncells, prop_used, prop_size;
 284         phys_addr_t start;
 285         ucell size, *prop;
 286
 287         if (s_phandle_memory == 0)
 288                 return;
 289
 290         /* count phys_range list entries */
 291         for( r = range, ncells = 0; r ; r=r->next, ncells++ ) {
 292         }
 293
 294         /* inverse of phys_range list could take 2 or more additional cells for the tail
 295            For /memory, physical addresses may be wider than one ucell. */
 296         prop_used = (ncells + 1) * sizeof(ucell) * ofmem_arch_get_available_entry_size(ph) + 1;
 297
 298         if (prop_used > *mem_prop_size) {
 299
 300                 /* The property doesn't fit within the existing space, so keep doubling it
 301                    until it does */
 302                 prop_size = *mem_prop_size;
 303                 while (prop_size < prop_used) {
 304                         prop_size *= 2;
 305                 }
 306
 307                 /* Allocate the new memory and copy all of the existing information across */
 308                 *mem_prop = realloc(*mem_prop, prop_size);
 309                 *mem_prop_size = prop_size;
 310                 *mem_prop_used = prop_used;
 311         }
 312
 313         if (*mem_prop == NULL) {
 314                 /* out of memory! */
 315                 printk("Unable to allocate memory for memory range property!\n");
 316                 return;
 317         }
 318
 319         start = 0;
 320         ncells = 0;
 321         prop = *mem_prop;
 322
 323         for (r = range; r; r=r->next) {
 324                 if (r->start >= top_address) {
 325                         break;
 326                 }
 327
 328                 size = r->start - start;
 329                 if (size) {
 330                         ofmem_arch_create_available_entry(ph, &prop[ncells], start, size);
 331                         ncells += ofmem_arch_get_available_entry_size(ph);
 332                 }
 333                 start = r->start + r->size;
 334         }
 335
 336         /* tail */
 337         if ((start - 1) < top_address) {
 338                 ofmem_arch_create_available_entry(ph, &prop[ncells], start, top_address - start + 1);
 339                 ncells += ofmem_arch_get_available_entry_size(ph);
 340         }
 341
 342         ofmem_set_property(ph, "available",
 343                         (char*)prop, ncells * sizeof(prop[0]));
 344 }
 345
 346 static void ofmem_update_translations( void )
 347 {
 348         ofmem_t *ofmem = ofmem_arch_get_private();
 349
 350         ofmem_update_memory_available(s_phandle_memory, ofmem->phys_range,
 351                         &phys_range_prop, &phys_range_prop_size, &phys_range_prop_used, get_ram_size() - 1);
 352         ofmem_update_memory_available(s_phandle_mmu, ofmem->virt_range,
 353                         &virt_range_prop, &virt_range_prop_size, &virt_range_prop_used, (ucell)-1);
 354         ofmem_update_mmu_translations();
 355 }
 356
 357
 358 /************************************************************************/
 359 /* client interface                                                     */
 360 /************************************************************************/
 361
 362 static int is_free( phys_addr_t ea, ucell size, range_t *r )
 363 {
 364         if( size == 0 )
 365                 return 1;
 366         for( ; r ; r=r->next ) {
 367                 if( r->start + r->size - 1 >= ea && r->start <= ea )
 368                         return 0;
 369                 if( r->start >= ea && r->start <= ea + size - 1 )
 370                         return 0;
 371         }
 372         return 1;
 373 }
 374
 375 static void add_entry_( phys_addr_t ea, ucell size, range_t **r )
 376 {
 377         range_t *nr;
 378
 379         for( ; *r && (**r).start < ea; r=&(**r).next ) {
 380         }
 381
 382         nr = (range_t*)malloc( sizeof(range_t) );
 383         nr->next = *r;
 384         nr->start = ea;
 385         nr->size = size;
 386         *r = nr;
 387 }
 388
 389 static int add_entry( phys_addr_t ea, ucell size, range_t **r )
 390 {
 391         if( !is_free( ea, size, *r ) ) {
 392                 OFMEM_TRACE("add_entry: range not free!\n");
 393                 return -1;
 394         }
 395         add_entry_( ea, size, r );
 396         return 0;
 397 }
 398
 399 #if defined(OFMEM_FILL_RANGE)
 400 static void join_ranges( range_t **rr )
 401 {
 402         range_t *n, *r = *rr;
 403         while( r ) {
 404                 if( !(n=r->next) )
 405                         break;
 406
 407                 if( r->start + r->size - 1 >= n->start -1 ) {
 408                         int s = n->size + (n->start - r->start - r->size);
 409                         if( s > 0 )
 410                                 r->size += s;
 411                         r->next = n->next;
 412                         free( n );
 413                         continue;
 414                 }
 415                 r=r->next;
 416         }
 417 }
 418
 419 static void fill_range( phys_addr_t ea, ucell size, range_t **rr )
 420 {
 421         add_entry_( ea, size, rr );
 422         join_ranges( rr );
 423 }
 424 #endif
 425
 426 static ucell find_area( ucell align, ucell size, range_t *r,
 427                 phys_addr_t min, phys_addr_t max, int reverse )
 428 {
 429         phys_addr_t base = min;
 430         range_t *r2;
 431         ucell old_align = align;
 432         int i;
 433
 434         if( (align < PAGE_SIZE) ) {
 435
 436                 /* Minimum alignment is page size */
 437                 align = PAGE_SIZE;
 438
 439                 OFMEM_TRACE("warning: bad alignment " FMT_ucellx " rounded up to " FMT_ucellx "\n", old_align, align);
 440         }
 441
 442         if( (align & (align-1)) ) {
 443
 444                 /* As per IEEE1275 specification, round up to the nearest power of 2 */
 445                 align--;
 446                 for (i = 1; i < sizeof(ucell) * 8; i<<=1) {
 447                         align |= align >> i;
 448                 }
 449                 align++;
 450
 451                 OFMEM_TRACE("warning: bad alignment " FMT_ucellx " rounded up to " FMT_ucellx "\n", old_align, align);
 452         }
 453
 454         base = reverse ? max - size : min;
 455         r2 = reverse ? NULL : r;
 456
 457         for( ;; ) {
 458                 if( !reverse ) {
 459                         base = (base + align - 1) & ~(align-1);
 460                         if( base < min )
 461                                 base = min;
 462                         if( base + size - 1 >= max -1 )
 463                                 break;
 464                 } else {
 465                         if( base > max - size )
 466                                 base = max - size;
 467                         base -= base & (align-1);
 468                 }
 469                 if( is_free( base, size, r ) )
 470                         return base;
 471
 472                 if( !reverse ) {
 473                         if( !r2 )
 474                                 break;
 475                         base = r2->start + r2->size;
 476                         r2 = r2->next;
 477                 } else {
 478                         range_t *rp;
 479
 480                         for( rp=r; rp && rp->next != r2 ; rp=rp->next ) {
 481                         }
 482
 483                         r2 = rp;
 484                         if( !r2 )
 485                                 break;
 486                         base = r2->start - size;
 487                 }
 488         }
 489         return -1;
 490 }
 491
 492 static phys_addr_t ofmem_claim_phys_( phys_addr_t phys, ucell size, ucell align,
 493                 phys_addr_t min, phys_addr_t max, int reverse )
 494 {
 495         ofmem_t *ofmem = ofmem_arch_get_private();
 496         if( !align ) {
 497                 if( !is_free( phys, size, ofmem->phys_range ) ) {
 498                         OFMEM_TRACE("Non-free physical memory claimed!\n");
 499                         return -1;
 500                 }
 501                 add_entry( phys, size, &ofmem->phys_range );
 502                 ofmem_update_translations();
 503                 return phys;
 504         }
 505         phys = find_area( align, size, ofmem->phys_range, min, max, reverse );
 506         if( phys == -1 ) {
 507                 printk("ofmem_claim_phys - out of space (failed request for " FMT_ucellx " bytes)\n", size);
 508                 return -1;
 509         }
 510         add_entry( phys, size, &ofmem->phys_range );
 511
 512         ofmem_update_translations();
 513
 514         return phys;
 515 }
 516
 517 /* if align != 0, phys is ignored. Returns -1 on error */
 518 phys_addr_t ofmem_claim_phys( phys_addr_t phys, ucell size, ucell align )
 519 {
 520     OFMEM_TRACE("ofmem_claim_phys phys=" FMT_plx " size=" FMT_ucellx
 521                 " align=" FMT_ucellx "\n",
 522                 phys, size, align);
 523
 524         return ofmem_claim_phys_( phys, size, align, 0, get_ram_size(), 1 );
 525 }
 526
 527 static ucell ofmem_claim_virt_( ucell virt, ucell size, ucell align,
 528                 ucell min, ucell max, int reverse )
 529 {
 530         ofmem_t *ofmem = ofmem_arch_get_private();
 531         if( !align ) {
 532                 if( !is_free( virt, size, ofmem->virt_range ) ) {
 533                         OFMEM_TRACE("Non-free virtual memory claimed!\n");
 534                         return -1;
 535                 }
 536                 add_entry( virt, size, &ofmem->virt_range );
 537                 ofmem_update_translations();
 538                 return virt;
 539         }
 540
 541         virt = find_area( align, size, ofmem->virt_range, min, max, reverse );
 542         if( virt == -1 ) {
 543                 printk("ofmem_claim_virt - out of space (failed request for " FMT_ucellx " bytes)\n", size);
 544                 return -1;
 545         }
 546         add_entry( virt, size, &ofmem->virt_range );
 547
 548         ofmem_update_translations();
 549
 550         return virt;
 551 }
 552
 553 ucell ofmem_claim_virt( ucell virt, ucell size, ucell align )
 554 {
 555     OFMEM_TRACE("ofmem_claim_virt virt=" FMT_ucellx " size=" FMT_ucellx
 556                 " align=" FMT_ucellx "\n",
 557                 virt, size, align);
 558
 559         /* printk("+ ofmem_claim virt %08lx %lx %ld\n", virt, size, align ); */
 560         return ofmem_claim_virt_( virt, size, align,
 561                         get_ram_size(), ofmem_arch_get_virt_top(), 1 );
 562 }
 563
 564 static ucell ofmem_claim_io_( ucell virt, ucell size, ucell align,
 565                 ucell min, ucell max, int reverse )
 566 {
 567         ofmem_t *ofmem = ofmem_arch_get_private();
 568         if( !align ) {
 569                 if( !is_free( virt, size, ofmem->io_range ) ) {
 570                         OFMEM_TRACE("Non-free I/O memory claimed!\n");
 571                         return -1;
 572                 }
 573                 add_entry( virt, size, &ofmem->io_range );
 574                 return virt;
 575         }
 576
 577         virt = find_area( align, size, ofmem->io_range, min, max, reverse );
 578         if( virt == -1 ) {
 579                 printk("ofmem_claim_io - out of space (failed request for " FMT_ucellx " bytes)\n", size);
 580                 return -1;
 581         }
 582         add_entry( virt, size, &ofmem->io_range );
 583         return virt;
 584 }
 585
 586 ucell ofmem_claim_io( ucell virt, ucell size, ucell align )
 587 {
 588         /* Claim a section of memory from the I/O range */
 589         return ofmem_claim_io_( virt, size, align,
 590                         ofmem_arch_get_iomem_base(), ofmem_arch_get_iomem_top(), 0 );
 591 }
 592
 593 /* if align != 0, phys is ignored. Returns -1 on error */
 594 phys_addr_t ofmem_retain( phys_addr_t phys, ucell size, ucell align )
 595 {
 596     retain_t *retained = ofmem_arch_get_retained();
 597     phys_addr_t retain_phys;
 598
 599     OFMEM_TRACE("ofmem_retain phys=" FMT_plx " size=" FMT_ucellx
 600                 " align=" FMT_ucellx "\n",
 601                 phys, size, align);
 602
 603         retain_phys = ofmem_claim_phys_( phys, size, align, 0, get_ram_size(), 1 /* reverse */ );
 604
 605         /* Add to the retain_phys_range list */
 606         retained->retain_phys_range[retained->numentries].next = NULL;
 607         retained->retain_phys_range[retained->numentries].start = retain_phys;
 608         retained->retain_phys_range[retained->numentries].size = size;
 609         retained->numentries++;
 610
 611         return retain_phys;
 612 }
 613
 614 /* allocate both physical and virtual space and add a translation */
 615 ucell ofmem_claim( ucell addr, ucell size, ucell align )
 616 {
 617         ofmem_t *ofmem = ofmem_arch_get_private();
 618         ucell virt;
 619         phys_addr_t phys;
 620         ucell offs = addr & (PAGE_SIZE - 1);
 621
 622         OFMEM_TRACE("ofmem_claim " FMT_ucellx " " FMT_ucellx " " FMT_ucellx "\n", addr, size, align );
 623         virt = phys = 0;
 624         if( !align ) {
 625                 if( is_free(addr, size, ofmem->virt_range) &&
 626                     is_free(addr, size, ofmem->phys_range) ) {
 627                         ofmem_claim_phys_( addr, size, 0, 0, 0, 0 );
 628                         ofmem_claim_virt_( addr, size, 0, 0, 0, 0 );
 629                         virt = phys = addr;
 630                 } else {
 631                         OFMEM_TRACE("**** ofmem_claim failure ***!\n");
 632                         return -1;
 633                 }
 634         } else {
 635                 if( align < PAGE_SIZE )
 636                         align = PAGE_SIZE;
 637                 phys = ofmem_claim_phys_( -1, size, align, 0, get_ram_size(), 1 /* reverse */ );
 638                 virt = ofmem_claim_virt_( phys, size, 0, 0, 0, 0 );
 639                 if( phys == -1 || virt == -1 ) {
 640                         OFMEM_TRACE("ofmem_claim failed\n");
 641                         return -1;
 642                 }
 643                 /* printk("...phys = %08lX, virt = %08lX, size = %08lX\n", phys, virt, size ); */
 644         }
 645
 646         /* align */
 647         if( phys & (PAGE_SIZE - 1) ) {
 648                 size += (phys & (PAGE_SIZE - 1));
 649                 virt -= (phys & (PAGE_SIZE - 1));
 650                 phys &= PAGE_MASK;
 651         }
 652         if( size & (PAGE_SIZE - 1) )
 653                 size = (size + (PAGE_SIZE - 1)) & PAGE_MASK;
 654
 655         /* printk("...free memory found... phys: %08lX, virt: %08lX, size %lX\n", phys, virt, size ); */
 656         ofmem_map( phys, virt, size, -1 );
 657         return virt + offs;
 658 }
 659
 660
 661 /************************************************************************/
 662 /* keep track of ea -> phys translations                                */
 663 /************************************************************************/
 664
 665 static void split_trans( ucell virt )
 666 {
 667         ofmem_t *ofmem = ofmem_arch_get_private();
 668         translation_t *t, *t2;
 669
 670         for( t=ofmem->trans; t; t=t->next ) {
 671                 if( virt > t->virt && virt < t->virt + t->size-1 ) {
 672                         t2 = (translation_t*)malloc( sizeof(translation_t) );
 673                         t2->virt = virt;
 674                         t2->size = t->size - (virt - t->virt);
 675                         t->size = virt - t->virt;
 676                         t2->phys = t->phys + t->size;
 677                         t2->mode = t->mode;
 678                         t2->next = t->next;
 679                         t->next = t2;
 680                 }
 681         }
 682 }
 683
 684 int ofmem_map_page_range( phys_addr_t phys, ucell virt, ucell size, ucell mode )
 685 {
 686         ofmem_t *ofmem = ofmem_arch_get_private();
 687         translation_t *t, **tt;
 688
 689         OFMEM_TRACE("ofmem_map_page_range " FMT_ucellx
 690                         " -> " FMT_plx " " FMT_ucellx " mode " FMT_ucellx "\n",
 691                         virt, phys, size, mode );
 692
 693         split_trans( virt );
 694         split_trans( virt + size );
 695
 696         /* detect remappings */
 697         for( t=ofmem->trans; t; ) {
 698                 if( virt == t->virt || (virt < t->virt && virt + size > t->virt )) {
 699                         if( t->phys + virt - t->virt != phys ) {
 700                                 OFMEM_TRACE("mapping altered virt=" FMT_ucellx ")\n", t->virt );
 701                         } else if( t->mode != mode ){
 702                                 OFMEM_TRACE("mapping mode altered virt=" FMT_ucellx
 703                                                 " old mode=" FMT_ucellx " new mode=" FMT_ucellx "\n",
 704                                                 t->virt, t->mode, mode);
 705                         }
 706
 707                         for( tt=&ofmem->trans; *tt != t ; tt=&(**tt).next ) {
 708                         }
 709
 710                         *tt = t->next;
 711
 712                         /* really unmap these pages */
 713                         ofmem_arch_unmap_pages(t->virt, t->size);
 714
 715                         free((char*)t);
 716
 717                         t=ofmem->trans;
 718                         continue;
 719                 }
 720                 t=t->next;
 721         }
 722
 723         /* add mapping */
 724         for( tt=&ofmem->trans; *tt && (**tt).virt < virt ; tt=&(**tt).next ) {
 725         }
 726
 727         t = (translation_t*)malloc( sizeof(translation_t) );
 728         t->virt = virt;
 729         t->phys = phys;
 730         t->size = size;
 731         t->mode = mode;
 732         t->next = *tt;
 733         *tt = t;
 734
 735         ofmem_update_translations();
 736
 737         return 0;
 738 }
 739
 740 static int unmap_page_range( ucell virt, ucell size )
 741 {
 742         ofmem_t *ofmem = ofmem_arch_get_private();
 743         translation_t **plink;
 744
 745         /* make sure there is exactly one matching translation entry */
 746
 747         split_trans( virt );
 748         split_trans( virt + size );
 749
 750         /* find and unlink entries in range */
 751         plink = &ofmem->trans;
 752
 753         while (*plink && (*plink)->virt < virt+size) {
 754                 translation_t **plinkentry = plink;
 755                 translation_t *t = *plink;
 756
 757                 /* move ahead */
 758                 plink = &t->next;
 759
 760                 if (t->virt >= virt && t->virt + t->size <= virt+size) {
 761
 762                         /* unlink entry */
 763                         *plinkentry = t->next;
 764
 765                         OFMEM_TRACE("unmap_page_range found "
 766                                         FMT_ucellx " -> " FMT_plx " " FMT_ucellx
 767                                         " mode " FMT_ucellx "\n",
 768                                         t->virt, t->phys, t->size, t->mode );
 769
 770                         // really map these pages
 771                         ofmem_arch_unmap_pages(t->virt, t->size);
 772
 773                         free((char*)t);
 774                 }
 775         }
 776
 777         ofmem_update_translations();
 778
 779         return 0;
 780 }
 781
 782 int ofmem_map( phys_addr_t phys, ucell virt, ucell size, ucell mode )
 783 {
 784         /* printk("+ofmem_map: %08lX --> %08lX (size %08lX, mode 0x%02X)\n",
 785            virt, phys, size, mode ); */
 786
 787         if( (phys & (PAGE_SIZE - 1)) || (virt & (PAGE_SIZE - 1)) || (size & (PAGE_SIZE - 1)) ) {
 788
 789                 OFMEM_TRACE("ofmem_map: Bad parameters ("
 790                                 FMT_plx " " FMT_ucellx " " FMT_ucellx ")\n",
 791                                 phys, virt, size );
 792
 793                 phys &= PAGE_MASK;
 794                 virt &= PAGE_MASK;
 795                 size = (size + (PAGE_SIZE - 1)) & PAGE_MASK;
 796         }
 797
 798 #if defined(OFMEM_FILL_RANGE)
 799         {
 800                 ofmem_t *ofmem = ofmem_arch_get_private();
 801                 /* claim any unclaimed virtual memory in the range */
 802                 fill_range( virt, size, &ofmem->virt_range );
 803                 /* hmm... we better claim the physical range too */
 804                 fill_range( phys, size, &ofmem->phys_range );
 805         }
 806 #endif
 807
 808         if (mode==-1) {
 809                 mode = ofmem_arch_default_translation_mode(phys);
 810         }
 811
 812         /* install translations */
 813         ofmem_map_page_range(phys, virt, size, mode);
 814
 815         /* allow arch to map the pages */
 816         ofmem_arch_map_pages(phys, virt, size, mode);
 817
 818         return 0;
 819 }
 820
 821 int ofmem_unmap( ucell virt, ucell size )
 822 {
 823         OFMEM_TRACE("ofmem_unmap " FMT_ucellx " " FMT_ucellx "\n",
 824                         virt, size );
 825
 826         if( (virt & (PAGE_SIZE - 1)) || (size & (PAGE_SIZE - 1)) ) {
 827                 /* printk("ofmem_unmap: Bad parameters (%08lX %08lX)\n",
 828                                 virt, size ); */
 829                 virt &= PAGE_MASK;
 830                 size = (size + (PAGE_SIZE - 1)) & PAGE_MASK;
 831         }
 832
 833         /* remove translations and unmap pages */
 834         unmap_page_range(virt, size);
 835
 836         return 0;
 837 }
 838
 839 ucell ofmem_map_io( phys_addr_t phys, ucell size )
 840 {
 841         /* Claim virtual memory from the I/O range and map the page-aligned
 842            physical address phys to it, returning the newly allocated
 843            virtual address */
 844         ucell virt, mode;
 845         phys_addr_t off;
 846         int npages;
 847
 848         off = phys & (PAGE_SIZE - 1);
 849         npages = (off + size - 1) / PAGE_SIZE + 1;
 850         phys &= ~(PAGE_SIZE - 1);
 851
 852         virt = ofmem_claim_io(-1, npages * PAGE_SIZE, PAGE_SIZE);
 853
 854         mode = ofmem_arch_io_translation_mode(off);
 855
 856         ofmem_map_page_range(phys, virt, npages * PAGE_SIZE, mode);
 857         ofmem_arch_map_pages(phys, virt, npages * PAGE_SIZE, mode);
 858
 859         return (virt + off);
 860 }
 861
 862 /* virtual -> physical. */
 863 phys_addr_t ofmem_translate( ucell virt, ucell *mode )
 864 {
 865         ofmem_t *ofmem = ofmem_arch_get_private();
 866         translation_t *t;
 867
 868         for( t=ofmem->trans; t && t->virt <= virt ; t=t->next ) {
 869                 ucell offs;
 870                 if( t->virt + t->size - 1 < virt )
 871                         continue;
 872                 offs = virt - t->virt;
 873                 *mode = t->mode;
 874                 return t->phys + offs;
 875         }
 876
 877         /*printk("ofmem_translate: no translation defined (%08lx)\n", virt);*/
 878         /*print_trans();*/
 879         return -1;
 880 }
 881
 882 static void remove_range_( phys_addr_t ea, ucell size, range_t **r )
 883 {
 884         range_t **t, *u;
 885
 886         /* If not an exact match then split the range */
 887         for (t = r; *t; t = &(**t).next) {
 888                 if (ea > (**t).start && ea < (**t).start + (**t).size - 1) {
 889                         u = (range_t*)malloc(sizeof(range_t));
 890                         u->start = ea;
 891                         u->size = size;
 892                         u->next = (**t).next;
 893
 894                         OFMEM_TRACE("remove_range_ splitting range with addr=" FMT_plx
 895                                         " size=" FMT_ucellx " -> addr=" FMT_plx " size=" FMT_ucellx ", "
 896                                         "addr=" FMT_plx " size=" FMT_ucellx "\n",
 897                                         (**t).start, (**t).size, (**t).start, (**t).size - size,
 898                                         u->start, u->size);
 899
 900                         (**t).size = (**t).size - size;
 901                         (**t).next = u;
 902                 }
 903         }
 904
 905         for (t = r; *t; t = &(**t).next) {
 906                 if (ea >= (**t).start && ea + size <= (**t).start + (**t).size) {
 907                         OFMEM_TRACE("remove_range_ freeing range with addr=" FMT_plx
 908                                         " size=" FMT_ucellx "\n", (**t).start, (**t).size);
 909                         u = *t;
 910                         *t = (**t).next;
 911                         free(u);
 912                         break;
 913                 }
 914         }
 915 }
 916
 917 static int remove_range( phys_addr_t ea, ucell size, range_t **r )
 918 {
 919         if( is_free( ea, size, *r ) ) {
 920                 OFMEM_TRACE("remove_range: range isn't occupied\n");
 921                 return -1;
 922         }
 923         remove_range_( ea, size, r );
 924         return 0;
 925 }
 926
 927 /* release memory allocated by ofmem_claim_phys */
 928 void ofmem_release_phys( phys_addr_t phys, ucell size )
 929 {
 930     OFMEM_TRACE("ofmem_release_phys addr=" FMT_plx " size=" FMT_ucellx "\n",
 931                 phys, size);
 932
 933     ofmem_t *ofmem = ofmem_arch_get_private();
 934     remove_range(phys, size, &ofmem->phys_range);
 935 }
 936
 937 /* release memory allocated by ofmem_claim_virt */
 938 void ofmem_release_virt( ucell virt, ucell size )
 939 {
 940     OFMEM_TRACE("ofmem_release_virt addr=" FMT_ucellx " size=" FMT_ucellx "\n",
 941                 virt, size);
 942
 943     ofmem_t *ofmem = ofmem_arch_get_private();
 944     remove_range(virt, size, &ofmem->virt_range);
 945 }
 946
 947 /* release memory allocated by ofmem_claim_io */
 948 void ofmem_release_io( ucell virt, ucell size )
 949 {
 950     OFMEM_TRACE("ofmem_release_io addr=" FMT_ucellx " size=" FMT_ucellx "\n",
 951                 virt, size);
 952
 953     ofmem_t *ofmem = ofmem_arch_get_private();
 954     remove_range(virt, size, &ofmem->io_range);
 955 }
 956
 957 /* release memory allocated by ofmem_claim - 6.3.2.4 */
 958 void ofmem_release( ucell virt, ucell size )
 959 {
 960     OFMEM_TRACE("%s addr=" FMT_ucellx " size=" FMT_ucellx "\n",
 961                 __func__, virt, size);
 962
 963     ucell mode;
 964     phys_addr_t phys = ofmem_translate(virt, &mode);
 965     if (phys == (phys_addr_t)-1) {
 966         OFMEM_TRACE("%s: no mapping\n", __func__);
 967         return;
 968     }
 969     ofmem_unmap(virt, size);
 970     ofmem_release_virt(virt, size);
 971     ofmem_release_phys(phys, size);
 972 }
 973
 974 /************************************************************************/
 975 /* init / cleanup                                                       */
 976 /************************************************************************/
 977
 978 void ofmem_register( phandle_t ph_memory, phandle_t ph_mmu )
 979 {
 980         s_phandle_memory = ph_memory;
 981         s_phandle_mmu = ph_mmu;
 982
 983         /* Initialise some default property sizes  */
 984         trans_prop_size = phys_range_prop_size = virt_range_prop_size = OFMEM_DEFAULT_PROP_SIZE;
 985         trans_prop = malloc(trans_prop_size);
 986         phys_range_prop = malloc(phys_range_prop_size);
 987         virt_range_prop = malloc(virt_range_prop_size);
 988
 989         ofmem_update_translations();
 990 }