package/lqtapi/src/mps/mps-fifo.c

   1 #include <linux/io.h>
   2
   3 #include "mps.h"
   4
   5 void mps_fifo_init(struct mps_fifo *fifo, void __iomem *base,
   6         void __iomem *head_addr, void __iomem *tail_addr, uint32_t size)
   7 {
   8         fifo->base = base;
   9         fifo->head_addr = head_addr;
  10         fifo->tail_addr = tail_addr;
  11         fifo->size = size;
  12         mps_fifo_reset(fifo);
  13 }
  14
  15 void mps_fifo_in(struct mps_fifo *fifo, const uint32_t *from, size_t len)
  16 {
  17         uint32_t head = __raw_readl(fifo->head_addr);
  18         void __iomem *base = fifo->base + head;
  19         size_t i = 0;
  20         size_t byte_len = len * 4;
  21
  22         if (head < byte_len) {
  23                 for(; i <= head / 4; ++i) {
  24                         __raw_writel(from[i], base);
  25                         base -= 4;
  26                 }
  27
  28                 base += fifo->size;
  29                 head += fifo->size;
  30         }
  31
  32         for(; i < len; ++i) {
  33                 __raw_writel(from[i], base);
  34                 base -= 4;
  35         }
  36
  37         head -= byte_len;
  38         __raw_writel(head, fifo->head_addr);
  39
  40 }
  41 EXPORT_SYMBOL_GPL(mps_fifo_in);
  42
  43 void mps_fifo_out(struct mps_fifo *fifo, uint32_t *to, size_t len)
  44 {
  45         uint32_t tail = __raw_readl(fifo->tail_addr);
  46         void __iomem *base = fifo->base + tail;
  47         size_t i = 0;
  48         size_t byte_len = len * 4;
  49
  50         if (tail < byte_len) {
  51                 for(; i <= tail / 4; ++i) {
  52                         to[i] = __raw_readl(base);
  53                         base -= 4;
  54                 }
  55
  56                 base += fifo->size;
  57                 tail += fifo->size;
  58         }
  59
  60         for(; i < len; ++i) {
  61                 to[i] = __raw_readl(base);
  62                 base -= 4;
  63         }
  64
  65         tail -= byte_len;
  66         __raw_writel(tail, fifo->tail_addr);
  67 }
  68 EXPORT_SYMBOL_GPL(mps_fifo_out);
  69
  70 uint32_t mps_fifo_peek(struct mps_fifo *fifo)
  71 {
  72         uint32_t tail = __raw_readl(fifo->tail_addr);
  73         void __iomem *base = fifo->base + tail;
  74         return __raw_readl(base);
  75 }
  76
  77 void mps_fifo_reset(struct mps_fifo *fifo)
  78 {
  79         void __iomem *base = fifo->base + fifo->size - 4;
  80         size_t i;
  81
  82         __raw_writel(fifo->size - 4, fifo->head_addr);
  83         __raw_writel(fifo->size - 4, fifo->tail_addr);
  84
  85         for(i = 0; i < 16; ++i) {
  86                 __raw_writel(0x0, base);
  87                 base -= 4;
  88         }
  89 }
  90
  91 size_t mps_fifo_len(struct mps_fifo *fifo)
  92 {
  93         uint32_t head = __raw_readl(fifo->head_addr);
  94         uint32_t tail = __raw_readl(fifo->tail_addr);
  95
  96         if (tail < head)
  97                 return head - tail;
  98         else
  99                 return fifo->size - (tail - head);
 100 }
 101 EXPORT_SYMBOL_GPL(mps_fifo_len);
 102