cae36a52 |
1 | #include <u.h> |
2 | #include <libc.h> |
3 | #include <ureg.h> |
4 | #include "dat.h" |
5 | #include "fns.h" |
6 | #include "linux.h" |
7 | |
8 | enum { |
9 | FREQUENCY = 44100, |
10 | CHANNELS = 2, |
11 | DELAY = 100, |
12 | FRAGSIZE = 4096, |
13 | }; |
14 | |
15 | typedef struct Chan Chan; |
16 | typedef struct DSP DSP; |
17 | |
18 | struct Chan |
19 | { |
20 | ulong phase; |
21 | int last; |
22 | }; |
23 | |
24 | struct DSP |
25 | { |
26 | Ufile; |
27 | |
28 | int channels; /* number of channels (2 for stereo) */ |
29 | int freq; /* frequency of sound stream */ |
30 | |
31 | int rfreq; /* frequency of /dev/audio */ |
32 | |
33 | uchar *buf; /* resampling */ |
34 | ulong nbuf; |
35 | Chan chan[CHANNELS]; |
36 | |
37 | vlong time; /* time point of the last sample in device buffer */ |
38 | |
39 | ulong written; /* number of bytes written to dsp */ |
40 | ulong written2; /* same as written, will be reset on every GETOPTR ioctl */ |
41 | }; |
42 | |
43 | static int |
44 | closedsp(Ufile *file) |
45 | { |
46 | DSP *dsp = (DSP*)file; |
47 | |
48 | trace("dsp: closedsp"); |
49 | free(dsp->buf); |
50 | close(dsp->fd); |
51 | |
52 | return 0; |
53 | } |
54 | |
55 | static int |
56 | polldsp(Ufile *, void *) |
57 | { |
58 | return POLLOUT; |
59 | } |
60 | |
61 | static int |
62 | readdsp(Ufile *, void *, int, vlong) |
63 | { |
64 | return 0; /* not implemented */ |
65 | } |
66 | |
67 | static int |
68 | resample(Chan *c, uchar *src, uchar *dst, int sstep, int dstep, ulong delta, ulong count) |
69 | { |
70 | int last, val, out; |
71 | ulong phase, pos; |
72 | uchar *dp, *sp; |
73 | |
74 | dp = dst; |
75 | last = val = c->last; |
76 | phase = c->phase; |
77 | pos = phase >> 16; |
78 | while(pos < count){ |
79 | sp = src + sstep*pos; |
80 | val = sp[0] | (sp[1] << 8); |
81 | val = (val & 0x7FFF) - (val & 0x8000); |
82 | if(pos){ |
83 | sp -= sstep; |
84 | last = sp[0] | (sp[1] << 8); |
85 | last = (last & 0x7FFF) - (last & 0x8000); |
86 | } |
87 | out = last + (((val - last) * (phase & 0xFFFF)) >> 16); |
88 | dp[0] = out; |
89 | dp[1] = out >> 8; |
90 | dp += dstep; |
91 | phase += delta; |
92 | pos = phase >> 16; |
93 | } |
94 | c->last = val; |
95 | if(delta < 0x10000){ |
96 | c->phase = phase & 0xFFFF; |
97 | } else { |
98 | c->phase = phase - (count << 16); |
99 | } |
100 | return (dp - dst) / dstep; |
101 | } |
102 | |
103 | static int |
104 | convertout(DSP *dsp, uchar *buf, int len, uchar **out) |
105 | { |
106 | int ret, ch; |
107 | ulong count, delta; |
108 | |
109 | /* no conversion required? */ |
110 | if(dsp->freq == dsp->rfreq && dsp->channels == CHANNELS){ |
111 | *out = buf; |
112 | return len; |
113 | } |
114 | |
115 | /* |
116 | * delta is the number of input samples to |
117 | * produce one output sample. scaled by 16 bit to |
118 | * get fractional part. |
119 | */ |
120 | delta = ((ulong)dsp->freq << 16) / dsp->rfreq; |
121 | count = len / (2 * dsp->channels); |
122 | |
123 | /* |
124 | * get maximum required size of output bufer. this is not exact! |
125 | * number of output samples depends on phase! |
126 | */ |
127 | ret = (((count << 16) + delta-1) / delta) * 2*CHANNELS; |
128 | if(ret > dsp->nbuf){ |
129 | free(dsp->buf); |
130 | dsp->buf = kmalloc(ret); |
131 | dsp->nbuf = ret; |
132 | } |
133 | for(ch=0; ch < CHANNELS; ch++) |
134 | ret = resample(dsp->chan + ch, |
135 | buf + 2*(ch % dsp->channels), |
136 | dsp->buf + 2*ch, |
137 | 2*dsp->channels, |
138 | 2*CHANNELS, |
139 | delta, |
140 | count); |
141 | |
142 | *out = dsp->buf; |
143 | return ret * 2*CHANNELS; |
144 | } |
145 | |
146 | static int |
147 | writedsp(Ufile *file, void *buf, int len, vlong) |
148 | { |
149 | DSP *dsp = (DSP*)file; |
150 | vlong now; |
151 | int ret, diff; |
152 | uchar *out; |
153 | |
154 | if((ret = convertout(dsp, buf, len, &out)) <= 0) |
155 | return ret; |
156 | |
157 | if((ret = write(dsp->fd, out, ret)) < 0) |
158 | return mkerror(); |
159 | |
160 | now = nsec(); |
161 | if(dsp->time < now){ |
162 | dsp->time = now; |
163 | dsp->written = 0; |
164 | dsp->written2 = 0; |
165 | } else { |
166 | diff = (dsp->time - now) / 1000000; |
167 | if(diff > DELAY) |
168 | sleep(diff - DELAY); |
169 | } |
170 | dsp->time += ((1000000000LL) * ret / (dsp->rfreq * 2*CHANNELS)); |
171 | dsp->written += len; |
172 | dsp->written2 += len; |
173 | |
174 | return len; |
175 | } |
176 | |
177 | enum |
178 | { |
179 | AFMT_S16_LE = 0x10, |
180 | }; |
181 | |
182 | static int |
183 | ioctldsp(Ufile *file, int cmd, void *arg) |
184 | { |
185 | DSP *dsp = (DSP*)file; |
186 | int ret, i; |
187 | vlong now; |
188 | static int counter; |
189 | |
190 | ret = 0; |
191 | switch(cmd){ |
192 | default: |
193 | trace("dsp: unknown ioctl %lux %p", (ulong)cmd, arg); |
194 | ret = -ENOTTY; |
195 | break; |
196 | |
197 | case 0xC004500A: |
198 | trace("dsp: SNDCTL_DSP_SETFRAGMENT(%lux)", *(ulong*)arg); |
199 | break; |
200 | |
201 | case 0xC0045004: |
202 | trace("dsp: SNDCTL_DSP_GETBLKSIZE"); |
203 | *((int*)arg) = FRAGSIZE; |
204 | break; |
205 | |
206 | case 0x800c5011: |
207 | trace("dsp: SNDCTL_DSP_GETIPTR"); |
208 | ret = -EPERM; |
209 | break; |
210 | |
211 | case 0x800c5012: |
212 | trace("dsp: SNDCTL_DSP_GETOPTR"); |
213 | ((int*)arg)[0] = dsp->written; // Total # of bytes processed |
214 | ((int*)arg)[1] = dsp->written2 / FRAGSIZE; // # of fragment transitions since last time |
215 | dsp->written2 = 0; |
216 | ((int*)arg)[2] = 0; // Current DMA pointer value |
217 | break; |
218 | |
219 | case 0x8010500D: |
220 | trace("dsp: SNDCTL_DSG_GETISPACE"); |
221 | ret = -EPERM; |
222 | break; |
223 | case 0x8010500C: |
224 | trace("dsp: SNDCTL_DSP_GETOSPACE"); |
225 | i = (2 * dsp->channels) * ((dsp->freq*DELAY)/1000); |
226 | ((int*)arg)[1] = i / FRAGSIZE; // fragstot |
227 | ((int*)arg)[2] = FRAGSIZE; // fragsize |
228 | now = nsec(); |
229 | if(now < dsp->time){ |
230 | i -= ((2 * dsp->channels) * (((dsp->time - now) * (vlong)dsp->freq) / 1000000000)); |
231 | if(i < 0) |
232 | i = 0; |
233 | } |
234 | ((int*)arg)[0] = i / FRAGSIZE; // available fragment count |
235 | ((int*)arg)[3] = i; // available space in bytes |
236 | break; |
237 | |
238 | case 0x8004500B: |
239 | trace("dsp: SNDCTL_DSP_GETFMTS(%d)", *(int*)arg); |
240 | *(int*)arg = AFMT_S16_LE; |
241 | break; |
242 | |
243 | case 0x8004500F: |
244 | trace("dsp: SNDCTL_DSP_GETCAPS"); |
245 | *(int*)arg = 0x400; |
246 | break; |
247 | |
248 | case 0xC0045005: |
249 | trace("dsp: SNDCTL_DSP_SETFMT(%d)", *(int*)arg); |
250 | *(int*)arg = AFMT_S16_LE; |
251 | break; |
252 | |
253 | case 0xC0045006: |
254 | trace("dsp: SOUND_PCM_WRITE_CHANNELS(%d)", *(int*)arg); |
255 | dsp->channels = *(int*)arg; |
256 | break; |
257 | |
258 | case 0xC0045003: |
259 | trace("dsp: SNDCTL_DSP_STEREO(%d)", *(int*)arg); |
260 | dsp->channels = 2; |
261 | *(int*)arg = 1; |
262 | break; |
263 | |
264 | case 0xC0045002: |
265 | trace("dsp: SNDCTL_DSP_SPEED(%d)", *(int*)arg); |
266 | dsp->freq = *(int*)arg; |
267 | for(i=0; i<CHANNELS; i++){ |
268 | dsp->chan[i].phase = 0; |
269 | dsp->chan[i].last = 0; |
270 | } |
271 | break; |
272 | |
273 | case 0x00005000: |
274 | trace("dsp: SNDCTL_DSP_RESET"); |
275 | break; |
276 | |
277 | case 0x00005001: |
278 | trace("dsp: SNDCTL_DSP_SYNC"); |
279 | break; |
280 | } |
281 | |
282 | return ret; |
283 | } |
284 | |
285 | static int |
286 | getaudiofreq(void) |
287 | { |
288 | int ret, n, fd; |
289 | char buf[1024]; |
290 | |
291 | ret = FREQUENCY; |
292 | if((fd = open("/dev/volume", OREAD)) < 0) |
293 | return ret; |
294 | if((n = read(fd, buf, sizeof(buf)-1)) > 0){ |
295 | char *p; |
296 | |
297 | buf[n] = 0; |
298 | if(p = strstr(buf, "speed out ")) |
299 | ret = atoi(p + 10); |
300 | } |
301 | close(fd); |
302 | return ret; |
303 | } |
304 | |
305 | int opendsp(char *path, int mode, int, Ufile **pf) |
306 | { |
307 | DSP *dsp; |
308 | int freq; |
309 | int fd; |
310 | |
311 | if(strcmp(path, "/dev/dsp")==0 || strcmp(path, "/dev/dsp0")==0){ |
312 | if((fd = open("/dev/audio", OWRITE)) < 0) |
313 | return mkerror(); |
314 | |
315 | freq = getaudiofreq(); |
316 | dsp = mallocz(sizeof(DSP), 1); |
317 | dsp->ref = 1; |
318 | dsp->mode = mode; |
319 | dsp->dev = DSPDEV; |
320 | dsp->fd = fd; |
321 | dsp->path = kstrdup(path); |
322 | dsp->rfreq = freq; |
323 | dsp->freq = freq; |
324 | dsp->channels = CHANNELS; |
325 | |
326 | *pf = dsp; |
327 | return 0; |
328 | } |
329 | |
330 | return -ENOENT; |
331 | } |
332 | |
333 | static int |
334 | fstatdsp(Ufile *f, Ustat *s) |
335 | { |
336 | s->mode = 0666 | S_IFCHR; |
337 | s->uid = current->uid; |
338 | s->gid = current->gid; |
339 | s->ino = hashpath(f->path); |
340 | s->size = 0; |
341 | return 0; |
342 | }; |
343 | |
344 | static int |
345 | statdsp(char *path, int , Ustat *s) |
346 | { |
347 | if(strcmp(path, "/dev/dsp")==0 || strcmp(path, "/dev/dsp0")==0){ |
348 | s->mode = 0666 | S_IFCHR; |
349 | s->uid = current->uid; |
350 | s->gid = current->gid; |
351 | s->ino = hashpath(path); |
352 | s->size = 0; |
353 | return 0; |
354 | } |
355 | |
356 | return -ENOENT; |
357 | } |
358 | |
359 | static Udev dspdev = |
360 | { |
361 | .open = opendsp, |
362 | .read = readdsp, |
363 | .write = writedsp, |
364 | .poll = polldsp, |
365 | .close = closedsp, |
366 | .ioctl = ioctldsp, |
367 | .stat = statdsp, |
368 | .fstat = fstatdsp, |
369 | }; |
370 | |
371 | void dspdevinit(void) |
372 | { |
373 | devtab[DSPDEV] = &dspdev; |
374 | |
375 | fsmount(&dspdev, "/dev/dsp"); |
376 | fsmount(&dspdev, "/dev/dsp0"); |
377 | } |