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