?login_element?

Subversion Repositories NedoOS

Rev

Rev 398 | Blame | Compare with Previous | Last modification | View Log | Download

  1. #include <stdint.h>
  2. #include <stdio.h>
  3. #include <stdlib.h>
  4. #include <string.h>
  5.  
  6. #include "ififo.h"
  7.  
  8. // "infinite" fifo for uint32_t values
  9. // NOT thread-safe! (i.e. can't put and get from different threads)
  10.  
  11. static volatile uint32_t head;
  12. static volatile uint32_t tail;
  13. static volatile uint32_t mask;
  14. static volatile uint32_t size;
  15.  
  16. static volatile uint32_t * volatile fifo = NULL;
  17.  
  18. #define IFIFO_INITIAL_SIZE (4096) // must be 2^n
  19.  
  20.  
  21. void ififo_init(void)
  22. {
  23.         head = tail = 0;
  24.  
  25.         mask = (size = IFIFO_INITIAL_SIZE) - 1;
  26.  
  27.         fifo = malloc(size*sizeof(uint32_t));
  28.         if( !fifo )
  29.         {
  30.                 fprintf(stderr,"%s: can't allocate initial memory chunk!\n",__PRETTY_FUNCTION__);
  31.                 exit(1);
  32.         }
  33. }
  34.  
  35. void ififo_free(void)
  36. {
  37.         if(fifo)
  38.         {
  39.                 free((void *)fifo);
  40.                 fifo=NULL;
  41.         }
  42. }
  43.  
  44.  
  45.  
  46. void ififo_put(uint32_t value)
  47. {
  48.         if( ((head+1)&mask)==(tail&mask) )
  49.         { // fifo is to be overflown, add more memory to it
  50. printf("ififo overflow: old head=%x, old tail=%x, old mask=%x, old size=%x, old ptr=%p\n",head,tail,mask,size,fifo);
  51.                 uint32_t old_size = size;
  52.                 uint32_t old_mask = mask;
  53.  
  54.                 size <<= 1;
  55.                 if( (size>>1)!=old_size )
  56.                 {
  57.                         fprintf(stderr,"%s: fifo have to become too large!\n",__PRETTY_FUNCTION__);
  58.                         exit(1);       
  59.                 }
  60.  
  61.                 mask = (size-1);
  62. printf("ififo overflow: new mask=%x, new size=%x\n",mask,size);
  63.                 fifo = realloc((void *)fifo,size*sizeof(uint32_t));
  64. printf("ififo overflow: new ptr=%p\n",fifo);
  65.                 if( !fifo )
  66.                 {
  67.                         fprintf(stderr,"%s: can't allocate more memory for fifo!\n",__PRETTY_FUNCTION__);
  68.                         exit(1);
  69.                 }
  70.  
  71.                 // realign and copy contents, if needed
  72.                 if( (head&old_mask)<(tail&old_mask) )
  73.                 {
  74.                         if( (head&old_mask)>0 )
  75.                         {
  76.                                 memcpy( (void *)(fifo+old_size), (void *)fifo, (head&old_mask)*sizeof(uint32_t) );
  77.                         }
  78.  
  79.                         head += old_size;
  80.                         head &= mask;
  81.                 }
  82. printf("ififo overflow: new head=%x\n",head);
  83.         }
  84.  
  85.         fifo[head&mask] = value;
  86.         head++;
  87.         head &= mask;
  88. }
  89.  
  90. uint32_t ififo_get(void)
  91. {
  92.         if( (head&mask)!=(tail&mask) )
  93.         {
  94.                 uint32_t value = fifo[tail&mask];
  95.                 tail++;
  96.                 tail &= mask;
  97.  
  98.                 return value;
  99.         }
  100.         else
  101.         {
  102.                 fprintf(stderr,"%s: attempted to get from empty ififo!\n",__PRETTY_FUNCTION__);
  103.                 exit(1);
  104.         }
  105. }
  106.  
  107. size_t ififo_used(void)
  108. { // return how many words are used in fifo (how many times it may supply 'get' without any 'put')
  109.  
  110.         return mask & (head-tail);
  111. }
  112.  
  113.