minidol/lib/ports.c

   1 /* ports implementation */
   2 #include <string.h>
   3
   4 #include "../shared.h"
   5
   6 extern shm_t shm;
   7 #define DEBUG0(X) { shm.states[14] = (uint32_t)(X); }
   8 #define DEBUG1(X) { shm.states[15] = (uint32_t)(X); }
   9
  10 void delay()
  11 {
  12         for(volatile long a = 0; a < 25; a++)
  13                 for(volatile long b = 0; b < 2000000; b++)
  14                         ;
  15 }
  16
  17 int size_shm(void *hwbuf)
  18 {
  19         hwbuf_t *b = (hwbuf_t*)hwbuf;
  20         return(b->size);
  21 }
  22
  23 int level_shm(void *hwbuf)
  24 {
  25         hwbuf_t *b = (hwbuf_t*)hwbuf;
  26
  27         int level = (b->wp - b->rp) % b->size;
  28         if(level < 0) level += b->size;
  29
  30         return(level);
  31 }
  32
  33 int read_shm (void *hwbuf, void *buf, int len)
  34 {
  35         hwbuf_t *b = (hwbuf_t*)hwbuf;
  36
  37         /* block if necessary */
  38         while(level_shm(hwbuf) < len);
  39
  40         /* copy data */
  41         if(b->rp + len > b->size) {
  42                 /* wrap-around -> split access */
  43                 uint32_t amount = b->size - b->rp;
  44                 memcpy(buf, &b->buf[b->rp], amount);
  45                 memcpy(buf + amount, &b->buf[0], len - amount);
  46         } else {
  47                 memcpy(buf, &b->buf[b->rp], len);
  48         }
  49
  50         /* update read pointer */
  51         b->rp = (b->rp + len) % b->size;
  52
  53         return(len);
  54 }
  55
  56 int write_shm(void *hwbuf, void *buf, int len)
  57 {
  58         hwbuf_t *b = (hwbuf_t*)hwbuf;
  59
  60         /* block if necessary */
  61         while(b->size - level_shm(hwbuf) < len);
  62
  63         /* copy data */
  64         if(b->wp + len > b->size) {
  65                 /* wrap-around -> split access */
  66                 uint32_t amount = b->size - b->wp;
  67                 memcpy(&b->buf[b->wp], buf, amount);
  68                 memcpy(&b->buf[0], buf + amount, len - amount);
  69         } else {
  70                 memcpy(&b->buf[b->wp], buf, len);
  71         }
  72
  73         /* update write pointer */
  74         b->wp = (b->wp + len) % b->size;
  75
  76 DEBUG1(level_shm(hwbuf)); delay();
  77
  78         return(len);
  79 }
  80