minidol: initial commit

[jump.git] / minidol / lib / ports.c

diff --git a/minidol/lib/ports.c b/minidol/lib/ports.c
new file mode 100644 (file)
index 0000000..3ae1ece
--- /dev/null
+++ b/minidol/lib/ports.c
@@ -0,0 +1,80 @@
+/* ports implementation */
+#include <string.h>
+
+#include "../shared.h"
+
+extern shm_t shm;
+#define DEBUG0(X) { shm.states[14] = (uint32_t)(X); }
+#define DEBUG1(X) { shm.states[15] = (uint32_t)(X); }
+
+void delay()
+{
+       for(volatile long a = 0; a < 25; a++)
+               for(volatile long b = 0; b < 2000000; b++)
+                       ;
+}
+
+int size_shm(void *hwbuf)
+{
+       hwbuf_t *b = (hwbuf_t*)hwbuf;
+       return(b->size);
+}
+
+int level_shm(void *hwbuf)
+{
+       hwbuf_t *b = (hwbuf_t*)hwbuf;
+
+       int level = (b->wp - b->rp) % b->size;
+       if(level < 0) level += b->size;
+
+       return(level);
+}
+
+int read_shm (void *hwbuf, void *buf, int len)
+{
+       hwbuf_t *b = (hwbuf_t*)hwbuf;
+
+       /* block if necessary */
+       while(level_shm(hwbuf) < len);
+
+       /* copy data */
+       if(b->rp + len > b->size) {
+               /* wrap-around -> split access */
+               uint32_t amount = b->size - b->rp;
+               memcpy(buf, &b->buf[b->rp], amount);
+               memcpy(buf + amount, &b->buf[0], len - amount);
+       } else {
+               memcpy(buf, &b->buf[b->rp], len);
+       }
+
+       /* update read pointer */
+       b->rp = (b->rp + len) % b->size;
+
+       return(len);
+}
+
+int write_shm(void *hwbuf, void *buf, int len)
+{
+       hwbuf_t *b = (hwbuf_t*)hwbuf;
+
+       /* block if necessary */
+       while(b->size - level_shm(hwbuf) < len);
+
+       /* copy data */
+       if(b->wp + len > b->size) {
+               /* wrap-around -> split access */
+               uint32_t amount = b->size - b->wp;
+               memcpy(&b->buf[b->wp], buf, amount);
+               memcpy(&b->buf[0], buf + amount, len - amount);
+       } else {
+               memcpy(&b->buf[b->wp], buf, len);
+       }
+
+       /* update write pointer */
+       b->wp = (b->wp + len) % b->size;
+
+DEBUG1(level_shm(hwbuf)); delay();
+
+       return(len);
+}
+