1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24#include <linux/time.h>
25#include <sound/core.h>
26#include <sound/pcm.h>
27#include "pcm_plugin.h"
28
29#define SIGN_BIT (0x80)
30#define QUANT_MASK (0xf)
31#define NSEGS (8)
32#define SEG_SHIFT (4)
33#define SEG_MASK (0x70)
34
35static inline int val_seg(int val)
36{
37 int r = 0;
38 val >>= 7;
39 if (val & 0xf0) {
40 val >>= 4;
41 r += 4;
42 }
43 if (val & 0x0c) {
44 val >>= 2;
45 r += 2;
46 }
47 if (val & 0x02)
48 r += 1;
49 return r;
50}
51
52#define BIAS (0x84)
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
83static unsigned char linear2ulaw(int pcm_val)
84{
85 int mask;
86 int seg;
87 unsigned char uval;
88
89
90 if (pcm_val < 0) {
91 pcm_val = BIAS - pcm_val;
92 mask = 0x7F;
93 } else {
94 pcm_val += BIAS;
95 mask = 0xFF;
96 }
97 if (pcm_val > 0x7FFF)
98 pcm_val = 0x7FFF;
99
100
101 seg = val_seg(pcm_val);
102
103
104
105
106
107 uval = (seg << 4) | ((pcm_val >> (seg + 3)) & 0xF);
108 return uval ^ mask;
109}
110
111
112
113
114
115
116
117
118
119
120static int ulaw2linear(unsigned char u_val)
121{
122 int t;
123
124
125 u_val = ~u_val;
126
127
128
129
130
131 t = ((u_val & QUANT_MASK) << 3) + BIAS;
132 t <<= ((unsigned)u_val & SEG_MASK) >> SEG_SHIFT;
133
134 return ((u_val & SIGN_BIT) ? (BIAS - t) : (t - BIAS));
135}
136
137
138
139
140
141typedef void (*mulaw_f)(struct snd_pcm_plugin *plugin,
142 const struct snd_pcm_plugin_channel *src_channels,
143 struct snd_pcm_plugin_channel *dst_channels,
144 snd_pcm_uframes_t frames);
145
146struct mulaw_priv {
147 mulaw_f func;
148 int cvt_endian;
149 unsigned int native_ofs;
150 unsigned int copy_ofs;
151 unsigned int native_bytes;
152 unsigned int copy_bytes;
153 u16 flip;
154};
155
156static inline void cvt_s16_to_native(struct mulaw_priv *data,
157 unsigned char *dst, u16 sample)
158{
159 sample ^= data->flip;
160 if (data->cvt_endian)
161 sample = swab16(sample);
162 if (data->native_bytes > data->copy_bytes)
163 memset(dst, 0, data->native_bytes);
164 memcpy(dst + data->native_ofs, (char *)&sample + data->copy_ofs,
165 data->copy_bytes);
166}
167
168static void mulaw_decode(struct snd_pcm_plugin *plugin,
169 const struct snd_pcm_plugin_channel *src_channels,
170 struct snd_pcm_plugin_channel *dst_channels,
171 snd_pcm_uframes_t frames)
172{
173 struct mulaw_priv *data = (struct mulaw_priv *)plugin->extra_data;
174 int channel;
175 int nchannels = plugin->src_format.channels;
176 for (channel = 0; channel < nchannels; ++channel) {
177 char *src;
178 char *dst;
179 int src_step, dst_step;
180 snd_pcm_uframes_t frames1;
181 if (!src_channels[channel].enabled) {
182 if (dst_channels[channel].wanted)
183 snd_pcm_area_silence(&dst_channels[channel].area, 0, frames, plugin->dst_format.format);
184 dst_channels[channel].enabled = 0;
185 continue;
186 }
187 dst_channels[channel].enabled = 1;
188 src = src_channels[channel].area.addr + src_channels[channel].area.first / 8;
189 dst = dst_channels[channel].area.addr + dst_channels[channel].area.first / 8;
190 src_step = src_channels[channel].area.step / 8;
191 dst_step = dst_channels[channel].area.step / 8;
192 frames1 = frames;
193 while (frames1-- > 0) {
194 signed short sample = ulaw2linear(*src);
195 cvt_s16_to_native(data, dst, sample);
196 src += src_step;
197 dst += dst_step;
198 }
199 }
200}
201
202static inline signed short cvt_native_to_s16(struct mulaw_priv *data,
203 unsigned char *src)
204{
205 u16 sample = 0;
206 memcpy((char *)&sample + data->copy_ofs, src + data->native_ofs,
207 data->copy_bytes);
208 if (data->cvt_endian)
209 sample = swab16(sample);
210 sample ^= data->flip;
211 return (signed short)sample;
212}
213
214static void mulaw_encode(struct snd_pcm_plugin *plugin,
215 const struct snd_pcm_plugin_channel *src_channels,
216 struct snd_pcm_plugin_channel *dst_channels,
217 snd_pcm_uframes_t frames)
218{
219 struct mulaw_priv *data = (struct mulaw_priv *)plugin->extra_data;
220 int channel;
221 int nchannels = plugin->src_format.channels;
222 for (channel = 0; channel < nchannels; ++channel) {
223 char *src;
224 char *dst;
225 int src_step, dst_step;
226 snd_pcm_uframes_t frames1;
227 if (!src_channels[channel].enabled) {
228 if (dst_channels[channel].wanted)
229 snd_pcm_area_silence(&dst_channels[channel].area, 0, frames, plugin->dst_format.format);
230 dst_channels[channel].enabled = 0;
231 continue;
232 }
233 dst_channels[channel].enabled = 1;
234 src = src_channels[channel].area.addr + src_channels[channel].area.first / 8;
235 dst = dst_channels[channel].area.addr + dst_channels[channel].area.first / 8;
236 src_step = src_channels[channel].area.step / 8;
237 dst_step = dst_channels[channel].area.step / 8;
238 frames1 = frames;
239 while (frames1-- > 0) {
240 signed short sample = cvt_native_to_s16(data, src);
241 *dst = linear2ulaw(sample);
242 src += src_step;
243 dst += dst_step;
244 }
245 }
246}
247
248static snd_pcm_sframes_t mulaw_transfer(struct snd_pcm_plugin *plugin,
249 const struct snd_pcm_plugin_channel *src_channels,
250 struct snd_pcm_plugin_channel *dst_channels,
251 snd_pcm_uframes_t frames)
252{
253 struct mulaw_priv *data;
254
255 if (snd_BUG_ON(!plugin || !src_channels || !dst_channels))
256 return -ENXIO;
257 if (frames == 0)
258 return 0;
259#ifdef CONFIG_SND_DEBUG
260 {
261 unsigned int channel;
262 for (channel = 0; channel < plugin->src_format.channels; channel++) {
263 if (snd_BUG_ON(src_channels[channel].area.first % 8 ||
264 src_channels[channel].area.step % 8))
265 return -ENXIO;
266 if (snd_BUG_ON(dst_channels[channel].area.first % 8 ||
267 dst_channels[channel].area.step % 8))
268 return -ENXIO;
269 }
270 }
271#endif
272 data = (struct mulaw_priv *)plugin->extra_data;
273 data->func(plugin, src_channels, dst_channels, frames);
274 return frames;
275}
276
277static void init_data(struct mulaw_priv *data, snd_pcm_format_t format)
278{
279#ifdef SNDRV_LITTLE_ENDIAN
280 data->cvt_endian = snd_pcm_format_big_endian(format) > 0;
281#else
282 data->cvt_endian = snd_pcm_format_little_endian(format) > 0;
283#endif
284 if (!snd_pcm_format_signed(format))
285 data->flip = 0x8000;
286 data->native_bytes = snd_pcm_format_physical_width(format) / 8;
287 data->copy_bytes = data->native_bytes < 2 ? 1 : 2;
288 if (snd_pcm_format_little_endian(format)) {
289 data->native_ofs = data->native_bytes - data->copy_bytes;
290 data->copy_ofs = 2 - data->copy_bytes;
291 } else {
292
293 data->native_ofs = data->native_bytes -
294 snd_pcm_format_width(format) / 8;
295 }
296}
297
298int snd_pcm_plugin_build_mulaw(struct snd_pcm_substream *plug,
299 struct snd_pcm_plugin_format *src_format,
300 struct snd_pcm_plugin_format *dst_format,
301 struct snd_pcm_plugin **r_plugin)
302{
303 int err;
304 struct mulaw_priv *data;
305 struct snd_pcm_plugin *plugin;
306 struct snd_pcm_plugin_format *format;
307 mulaw_f func;
308
309 if (snd_BUG_ON(!r_plugin))
310 return -ENXIO;
311 *r_plugin = NULL;
312
313 if (snd_BUG_ON(src_format->rate != dst_format->rate))
314 return -ENXIO;
315 if (snd_BUG_ON(src_format->channels != dst_format->channels))
316 return -ENXIO;
317
318 if (dst_format->format == SNDRV_PCM_FORMAT_MU_LAW) {
319 format = src_format;
320 func = mulaw_encode;
321 }
322 else if (src_format->format == SNDRV_PCM_FORMAT_MU_LAW) {
323 format = dst_format;
324 func = mulaw_decode;
325 }
326 else {
327 snd_BUG();
328 return -EINVAL;
329 }
330 if (snd_BUG_ON(!snd_pcm_format_linear(format->format)))
331 return -ENXIO;
332
333 err = snd_pcm_plugin_build(plug, "Mu-Law<->linear conversion",
334 src_format, dst_format,
335 sizeof(struct mulaw_priv), &plugin);
336 if (err < 0)
337 return err;
338 data = (struct mulaw_priv *)plugin->extra_data;
339 data->func = func;
340 init_data(data, format->format);
341 plugin->transfer = mulaw_transfer;
342 *r_plugin = plugin;
343 return 0;
344}
345