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#if !defined(_FIR_H_)
27#define _FIR_H_
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
61struct fir16_state_t {
62 int taps;
63 int curr_pos;
64 const int16_t *coeffs;
65 int16_t *history;
66};
67
68
69
70
71
72
73struct fir32_state_t {
74 int taps;
75 int curr_pos;
76 const int32_t *coeffs;
77 int16_t *history;
78};
79
80
81
82
83
84struct fir_float_state_t {
85 int taps;
86 int curr_pos;
87 const float *coeffs;
88 float *history;
89};
90
91static inline const int16_t *fir16_create(struct fir16_state_t *fir,
92 const int16_t *coeffs, int taps)
93{
94 fir->taps = taps;
95 fir->curr_pos = taps - 1;
96 fir->coeffs = coeffs;
97#if defined(__bfin__)
98 fir->history = kcalloc(2 * taps, sizeof(int16_t), GFP_KERNEL);
99#else
100 fir->history = kcalloc(taps, sizeof(int16_t), GFP_KERNEL);
101#endif
102 return fir->history;
103}
104
105static inline void fir16_flush(struct fir16_state_t *fir)
106{
107#if defined(__bfin__)
108 memset(fir->history, 0, 2 * fir->taps * sizeof(int16_t));
109#else
110 memset(fir->history, 0, fir->taps * sizeof(int16_t));
111#endif
112}
113
114static inline void fir16_free(struct fir16_state_t *fir)
115{
116 kfree(fir->history);
117}
118
119#ifdef __bfin__
120static inline int32_t dot_asm(short *x, short *y, int len)
121{
122 int dot;
123
124 len--;
125
126 __asm__("I0 = %1;\n\t"
127 "I1 = %2;\n\t"
128 "A0 = 0;\n\t"
129 "R0.L = W[I0++] || R1.L = W[I1++];\n\t"
130 "LOOP dot%= LC0 = %3;\n\t"
131 "LOOP_BEGIN dot%=;\n\t"
132 "A0 += R0.L * R1.L (IS) || R0.L = W[I0++] || R1.L = W[I1++];\n\t"
133 "LOOP_END dot%=;\n\t"
134 "A0 += R0.L*R1.L (IS);\n\t"
135 "R0 = A0;\n\t"
136 "%0 = R0;\n\t"
137 : "=&d"(dot)
138 : "a"(x), "a"(y), "a"(len)
139 : "I0", "I1", "A1", "A0", "R0", "R1"
140 );
141
142 return dot;
143}
144#endif
145
146static inline int16_t fir16(struct fir16_state_t *fir, int16_t sample)
147{
148 int32_t y;
149#if defined(__bfin__)
150 fir->history[fir->curr_pos] = sample;
151 fir->history[fir->curr_pos + fir->taps] = sample;
152 y = dot_asm((int16_t *) fir->coeffs, &fir->history[fir->curr_pos],
153 fir->taps);
154#else
155 int i;
156 int offset1;
157 int offset2;
158
159 fir->history[fir->curr_pos] = sample;
160
161 offset2 = fir->curr_pos;
162 offset1 = fir->taps - offset2;
163 y = 0;
164 for (i = fir->taps - 1; i >= offset1; i--)
165 y += fir->coeffs[i] * fir->history[i - offset1];
166 for (; i >= 0; i--)
167 y += fir->coeffs[i] * fir->history[i + offset2];
168#endif
169 if (fir->curr_pos <= 0)
170 fir->curr_pos = fir->taps;
171 fir->curr_pos--;
172 return (int16_t) (y >> 15);
173}
174
175static inline const int16_t *fir32_create(struct fir32_state_t *fir,
176 const int32_t *coeffs, int taps)
177{
178 fir->taps = taps;
179 fir->curr_pos = taps - 1;
180 fir->coeffs = coeffs;
181 fir->history = kcalloc(taps, sizeof(int16_t), GFP_KERNEL);
182 return fir->history;
183}
184
185static inline void fir32_flush(struct fir32_state_t *fir)
186{
187 memset(fir->history, 0, fir->taps * sizeof(int16_t));
188}
189
190static inline void fir32_free(struct fir32_state_t *fir)
191{
192 kfree(fir->history);
193}
194
195static inline int16_t fir32(struct fir32_state_t *fir, int16_t sample)
196{
197 int i;
198 int32_t y;
199 int offset1;
200 int offset2;
201
202 fir->history[fir->curr_pos] = sample;
203 offset2 = fir->curr_pos;
204 offset1 = fir->taps - offset2;
205 y = 0;
206 for (i = fir->taps - 1; i >= offset1; i--)
207 y += fir->coeffs[i] * fir->history[i - offset1];
208 for (; i >= 0; i--)
209 y += fir->coeffs[i] * fir->history[i + offset2];
210 if (fir->curr_pos <= 0)
211 fir->curr_pos = fir->taps;
212 fir->curr_pos--;
213 return (int16_t) (y >> 15);
214}
215
216#endif
217