Subversion Repositories Kolibri OS

Rev

Rev 864 | Rev 888 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | Download | RSS feed

  1.  
  2. #include <types.h>
  3. #include <core.h>
  4. #include <spinlock.h>
  5. #include <link.h>
  6. #include <mm.h>
  7. #include <slab.h>
  8.  
  9. typedef struct
  10. {
  11.    link_t link;
  12.    link_t adj;
  13.    addr_t base;
  14.    size_t size;
  15.    void*  parent;
  16.    u32_t  state;
  17. }md_t;
  18.  
  19. #define   MD_FREE    1
  20. #define   MD_USED    2
  21.  
  22. typedef struct {
  23.     SPINLOCK_DECLARE(lock);   /**< this lock protects everything below */
  24.  
  25.     u32_t  availmask;
  26.     link_t free[32];
  27.  
  28.     link_t used;
  29. }heap_t;
  30.  
  31.  
  32. slab_cache_t *md_slab;
  33. slab_cache_t *phm_slab;
  34.  
  35.  
  36. heap_t        lheap;
  37. heap_t        sheap;
  38.  
  39.  
  40.  
  41. static inline void _set_lmask(count_t idx)
  42. { asm volatile ("bts %0, _lheap"::"r"(idx):"cc"); }
  43.  
  44. static inline void _reset_lmask(count_t idx)
  45. { asm volatile ("btr %0, _lheap"::"r"(idx):"cc"); }
  46.  
  47. static inline void _set_smask(count_t idx)
  48. { asm volatile ("bts %0, _sheap"::"r"(idx):"cc"); }
  49.  
  50. static inline void _reset_smask(count_t idx)
  51. { asm volatile ("btr %0, _sheap"::"r"(idx):"cc"); }
  52.  
  53.  
  54. int __fastcall init_heap(addr_t base, size_t size)
  55. {
  56.    md_t *md;
  57.    u32_t i;
  58.  
  59.    ASSERT(base != 0);
  60.    ASSERT(size != 0)
  61.    ASSERT((base & 0x3FFFFF) == 0);
  62.    ASSERT((size & 0x3FFFFF) == 0);
  63.  
  64.    for (i = 0; i < 32; i++)
  65.    {
  66.      list_initialize(&lheap.free[i]);
  67.      list_initialize(&sheap.free[i]);
  68.    };
  69.  
  70.    list_initialize(&lheap.used);
  71.    list_initialize(&sheap.used);
  72.  
  73.  
  74.    md_slab = slab_cache_create(sizeof(md_t), 32,NULL,NULL,SLAB_CACHE_MAGDEFERRED);
  75.  
  76.    md = (md_t*)slab_alloc(md_slab,0);
  77.  
  78.    list_initialize(&md->adj);
  79.    md->base = base;
  80.    md->size = size;
  81.    md->parent = NULL;
  82.    md->state = MD_FREE;
  83.  
  84.    list_prepend(&md->link, &lheap.free[31]);
  85.    lheap.availmask = 0x80000000;
  86.    sheap.availmask = 0x00000000;
  87.  
  88.   // phm_slab = slab_cache_create(sizeof(phismem_t), 32,NULL,NULL,SLAB_CACHE_MAGDEFERRED);
  89.  
  90.    return 1;
  91. };
  92.  
  93. md_t* __fastcall find_large_md(size_t size)
  94. {
  95.    md_t *md = NULL;
  96.  
  97.    count_t idx0;
  98.    u32_t mask;
  99.  
  100.    ASSERT((size & 0x3FFFFF) == 0);
  101.  
  102.    idx0 = (size>>22) - 1 < 32 ? (size>>22) - 1 : 31;
  103.    mask = lheap.availmask & ( -1<<idx0 );
  104.  
  105.    if(mask)
  106.    {
  107.      if(idx0 == 31)
  108.      {
  109.         md_t *tmp = (md_t*)lheap.free[31].next;
  110.         while((link_t*)tmp != &lheap.free[31])
  111.         {
  112.           if(tmp->size >= size)
  113.           {
  114.             DBG("remove large tmp %x\n", tmp);
  115.  
  116.             md = tmp;
  117.             break;
  118.           };
  119.         };
  120.         tmp = (md_t*)tmp->link.next;
  121.      }
  122.      else
  123.      {
  124.        idx0 = _bsf(mask);
  125.  
  126.        ASSERT( !list_empty(&lheap.free[idx0]))
  127.  
  128.        md = (md_t*)lheap.free[idx0].next;
  129.      };
  130.    }
  131.    else
  132.      return NULL;
  133.  
  134.    ASSERT(md->state == MD_FREE);
  135.  
  136.    list_remove((link_t*)md);
  137.    if(list_empty(&lheap.free[idx0]))
  138.      _reset_lmask(idx0);
  139.  
  140.    if(md->size > size)
  141.    {
  142.      count_t idx1;
  143.      md_t *new_md = (md_t*)slab_alloc(md_slab,0);         /* FIXME check */
  144.  
  145.      link_initialize(&new_md->link);
  146.      list_insert(&new_md->adj, &md->adj);
  147.  
  148.      new_md->base = md->base;
  149.      new_md->size = size;
  150.      new_md->state = MD_USED;
  151.  
  152.      md->base+= size;
  153.      md->size-= size;
  154.  
  155.      idx1 = (md->size>>22) - 1 < 32 ? (md->size>>22) - 1 : 31;
  156.  
  157.      list_prepend(&md->link, &lheap.free[idx1]);
  158.      _set_lmask(idx1);
  159.  
  160.      return new_md;
  161.    };
  162.    md->state = MD_USED;
  163.  
  164.    return md;
  165. }
  166.  
  167. md_t* __fastcall find_small_md(size_t size)
  168. {
  169.     eflags_t efl;
  170.  
  171.     md_t *md = NULL;
  172.  
  173.     count_t idx0;
  174.     u32_t mask;
  175.  
  176.     ASSERT((size & 0xFFF) == 0);
  177.  
  178.     efl = safe_cli();
  179.  
  180.     idx0 = (size>>12) - 1 < 32 ? (size>>12) - 1 : 31;
  181.     mask = sheap.availmask & ( -1<<idx0 );
  182.  
  183.     DBG("smask %x size %x idx0 %x mask %x\n",sheap.availmask, size, idx0, mask);
  184.  
  185.     if(mask)
  186.     {
  187.         if(idx0 == 31)
  188.         {
  189.             ASSERT( !list_empty(&sheap.free[31]));
  190.  
  191.             md_t *tmp = (md_t*)sheap.free[31].next;
  192.             while((link_t*)tmp != &sheap.free[31])
  193.             {
  194.                 if(tmp->size >= size)
  195.                 {
  196.                     md = tmp;
  197.                     break;
  198.                 };
  199.                 tmp = (md_t*)tmp->link.next;
  200.             };
  201.         }
  202.         else
  203.         {
  204.             idx0 = _bsf(mask);
  205.  
  206.             ASSERT( !list_empty(&sheap.free[idx0]));
  207.  
  208.             md = (md_t*)sheap.free[idx0].next;
  209.         }
  210.     };
  211.  
  212.     if(md)
  213.     {
  214.         DBG("remove md %x\n", md);
  215.  
  216.         ASSERT(md->state==MD_FREE);
  217.  
  218.         list_remove((link_t*)md);
  219.         if(list_empty(&sheap.free[idx0]))
  220.             _reset_smask(idx0);
  221.     }
  222.     else
  223.     {
  224.         md_t *lmd;
  225.         lmd = find_large_md((size+0x3FFFFF)&~0x3FFFFF);
  226.  
  227.         DBG("get large md %x\n", lmd);
  228.  
  229.         if( !lmd)
  230.         {
  231.             safe_sti(efl);
  232.             return NULL;
  233.         };
  234.  
  235.         md = (md_t*)slab_alloc(md_slab,0);    /* FIXME check */
  236.  
  237.         link_initialize(&md->link);
  238.         list_initialize(&md->adj);
  239.         md->base = lmd->base;
  240.         md->size = lmd->size;
  241.         md->parent  = lmd;
  242.         md->state = MD_USED;
  243.     };
  244.  
  245.     if(md->size > size)
  246.     {
  247.         count_t idx1;
  248.         md_t *new_md = (md_t*)slab_alloc(md_slab,0);    /* FIXME check */
  249.  
  250.         link_initialize(&new_md->link);
  251.         list_insert(&new_md->adj, &md->adj);
  252.  
  253.         new_md->base = md->base;
  254.         new_md->size = size;
  255.         new_md->parent = md->parent;
  256.         new_md->state = MD_USED;
  257.  
  258.         md->base+= size;
  259.         md->size-= size;
  260.         md->state = MD_FREE;
  261.  
  262.         idx1 = (md->size>>12) - 1 < 32 ? (md->size>>12) - 1 : 31;
  263.  
  264.         DBG("insert md %x, base %x size %x idx %x\n", md,md->base, md->size,idx1);
  265.  
  266.         if( idx1 < 31)
  267.           list_prepend(&md->link, &sheap.free[idx1]);
  268.         else
  269.         {
  270.             if( list_empty(&sheap.free[31]))
  271.                 list_prepend(&md->link, &sheap.free[31]);
  272.             else
  273.             {
  274.                 md_t *tmp = (md_t*)sheap.free[31].next;
  275.  
  276.                 while((link_t*)tmp != &sheap.free[31])
  277.                 {
  278.                     if(md->base < tmp->base)
  279.                         break;
  280.                     tmp = (md_t*)tmp->link.next;
  281.                 }
  282.                 list_insert(&md->link, &tmp->link);
  283.             };
  284.         };
  285.  
  286.         _set_smask(idx1);
  287.  
  288.         safe_sti(efl);
  289.  
  290.         return new_md;
  291.     };
  292.  
  293.     md->state = MD_USED;
  294.  
  295.     safe_sti(efl);
  296.  
  297.     return md;
  298. }
  299.  
  300. void __fastcall free_small_md(md_t *md)
  301. {
  302.     eflags_t  efl ;
  303.     md_t     *fd;
  304.     md_t     *bk;
  305.     count_t   idx;
  306.  
  307.     efl = safe_cli();
  308.     spinlock_lock(&sheap.lock);
  309.  
  310.     if( !list_empty(&md->adj))
  311.     {
  312.         bk = (md_t*)md->adj.prev;
  313.         fd = (md_t*)md->adj.next;
  314.  
  315.         if(fd->state == MD_FREE)
  316.         {
  317.             idx = (fd->size>>12) - 1 < 32 ? (fd->size>>12) - 1 : 31;
  318.  
  319.             list_remove((link_t*)fd);
  320.             if(list_empty(&sheap.free[idx]))
  321.                 _reset_smask(idx);
  322.  
  323.             md->size+= fd->size;
  324.             md->adj.next = fd->adj.next;
  325.             md->adj.next->prev = (link_t*)md;
  326.             slab_free(md_slab, fd);
  327.         };
  328.         if(bk->state == MD_FREE)
  329.         {
  330.             idx = (bk->size>>12) - 1 < 32 ? (bk->size>>12) - 1 : 31;
  331.  
  332.             list_remove((link_t*)bk);
  333.             if(list_empty(&sheap.free[idx]))
  334.                 _reset_smask(idx);
  335.  
  336.             bk->size+= md->size;
  337.             bk->adj.next = md->adj.next;
  338.             bk->adj.next->prev = (link_t*)bk;
  339.             slab_free(md_slab, md);
  340.             md = fd;
  341.         };
  342.     };
  343.  
  344.     md->state = MD_FREE;
  345.  
  346.     idx = (md->size>>12) - 1 < 32 ? (md->size>>12) - 1 : 31;
  347.  
  348.     _set_smask(idx);
  349.  
  350.     if( idx < 31)
  351.         list_prepend(&md->link, &sheap.free[idx]);
  352.     else
  353.     {
  354.         if( list_empty(&sheap.free[31]))
  355.             list_prepend(&md->link, &sheap.free[31]);
  356.         else
  357.         {
  358.             md_t *tmp = (md_t*)sheap.free[31].next;
  359.  
  360.             while((link_t*)tmp != &sheap.free[31])
  361.             {
  362.                 if(md->base < tmp->base)
  363.                     break;
  364.                 tmp = (md_t*)tmp->link.next;
  365.             }
  366.             list_insert(&md->link, &tmp->link);
  367.         };
  368.     };
  369.     spinlock_unlock(&sheap.lock);
  370.     safe_sti(efl);
  371.  
  372. };
  373.  
  374.  
  375. #define page_tabs 0xDF800000
  376.  
  377. /*
  378. phismem_t* __fastcall phis_alloc(count_t count)
  379. {
  380.    phismem_t *phm;
  381.    count_t tmp;
  382.    phm = (phismem_t*)slab_alloc(phm_slab, 0);
  383.  
  384.    phm->count = count;
  385.    tmp = count;
  386.    while(tmp)
  387.    {
  388.       u32_t order;
  389.  
  390.       asm volatile ("bsr %0, %1":"=&r"(order):"r"(tmp):"cc");
  391.       asm volatile ("btr %0, %1" :"=r"(tmp):"r"(order):"cc");
  392.  
  393.       phm->frames[order] = core_alloc(order);
  394.  
  395.    };
  396.  
  397.    return phm;
  398. }
  399.  
  400. void map_phm(addr_t base, phismem_t *phm, u32_t mapflags)
  401. {
  402.    count_t count;
  403.    addr_t  *pte;
  404.  
  405.    count = phm->count;
  406.    pte = &((addr_t*)page_tabs)[base>>12];
  407.  
  408.    while(count)
  409.    {
  410.      u32_t order;
  411.      addr_t frame;
  412.      count_t size;
  413.  
  414.      asm volatile ("bsr %0, %1":"=&r"(order):"r"(count):"cc");
  415.      asm volatile ("btr %0, %1" :"=r"(count):"r"(order):"cc");
  416.  
  417.      frame = phm->frames[order] | mapflags;
  418.      size = (1 << order);
  419.      while(size--)
  420.      {
  421.        *pte++ = frame;
  422.        frame+= 4096;
  423.      }
  424.    }
  425. };
  426. */
  427.  
  428. void * __fastcall mem_alloc(size_t size, u32_t flags)
  429. {
  430.     eflags_t efl;
  431.  
  432.     md_t *md;
  433.  
  434.     DBG("\nmem_alloc: %x bytes\n", size);
  435.  
  436.     ASSERT(size != 0);
  437.  
  438.     size = (size+4095)&~4095;
  439.  
  440.     md = find_small_md(size);
  441.  
  442.     if( md )
  443.     {
  444.         ASSERT(md->state == MD_USED);
  445.  
  446.         if( flags & PG_MAP )
  447.         {
  448.             count_t tmp = size >> 12;
  449.             addr_t  *pte = &((addr_t*)page_tabs)[md->base>>12];
  450.  
  451.             while(tmp)
  452.             {
  453.                 u32_t  order;
  454.                 addr_t frame;
  455.                 size_t size;
  456.  
  457.                 asm volatile ("bsr %1, %0":"=&r"(order):"r"(tmp):"cc");
  458.                 asm volatile ("btr %1, %0" :"=r"(tmp):"r"(order):"cc");
  459.  
  460.                 frame = core_alloc(order) | flags;         /* FIXME check */
  461.  
  462.                 size = (1 << order);
  463.                 while(size--)
  464.                 {
  465.                     *pte++ = frame;
  466.                     frame+= 4096;
  467.                 };
  468.             };
  469.         };
  470.  
  471.         efl = safe_cli();
  472.         spinlock_lock(&sheap.lock);
  473.  
  474.         if( list_empty(&sheap.used) )
  475.             list_prepend(&md->link, &sheap.used);
  476.         else
  477.         {
  478.             md_t *tmp = (md_t*)sheap.used.next;
  479.  
  480.             while((link_t*)tmp != &sheap.used)
  481.             {
  482.                 if(md->base < tmp->base)
  483.                     break;
  484.                 tmp = (md_t*)tmp->link.next;
  485.             }
  486.             list_insert(&md->link, &tmp->link);
  487.         };
  488.  
  489.         spinlock_unlock(&sheap.lock);
  490.         safe_sti(efl);
  491.  
  492.         DBG("allocate: %x size %x\n\n",md->base, size);
  493.         return (void*)md->base;
  494.     };
  495.     return NULL;
  496. };
  497.  
  498. void __fastcall mem_free(void *mem)
  499. {
  500.     eflags_t efl;
  501.  
  502.     md_t *tmp;
  503.     md_t *md = NULL;
  504.  
  505.     DBG("mem_free: %x\n",mem);
  506.  
  507.     ASSERT( mem != 0 );
  508.     ASSERT( ((addr_t)mem & 0xFFF) == 0 );
  509.     ASSERT( ! list_empty(&sheap.used));
  510.  
  511.     efl = safe_cli();
  512.  
  513.     tmp = (md_t*)sheap.used.next;
  514.  
  515.     while((link_t*)tmp != &sheap.used)
  516.     {
  517.         if( tmp->base == (addr_t)mem )
  518.         {
  519.             md = tmp;
  520.             break;
  521.         };
  522.         tmp = (md_t*)tmp->link.next;
  523.     }
  524.  
  525.     if( md )
  526.     {
  527.         DBG("\tmd: %x base: %x size: %x\n",md, md->base, md->size);
  528.  
  529.         ASSERT(md->state == MD_USED);
  530.  
  531.         count_t tmp  = md->size >> 12;
  532.         addr_t  *pte = &((addr_t*)page_tabs)[md->base>>12];
  533.  
  534.         while(tmp--)
  535.         {
  536.             *pte++ = 0;
  537.             asm volatile (
  538.                 "invlpg (%0)"
  539.                 :
  540.                 :"r" (mem) );
  541.             mem+= 4096;
  542.         };
  543.         list_remove((link_t*)md);
  544.         free_small_md( md );
  545.     }
  546.     else
  547.     {
  548.         DBG("\tERROR: invalid base address: %x\n", mem);
  549.     };
  550.  
  551.     safe_sti(efl);
  552. };
  553.