update for recent /gp2x changes
[picodrive.git] / platform / psp / mp3.c
1 // (c) Copyright 2007 notaz, All rights reserved.
2 // Free for non-commercial use.
3
4 // For commercial use, separate licencing terms must be obtained.
5
6 #include <stdio.h>
7 #include <string.h>
8
9 #include <pspkernel.h>
10 #include <pspsdk.h>
11 #include <pspaudiocodec.h>
12 #include <kubridge.h>
13
14 #include "../../pico/pico_int.h"
15 #include "../../pico/sound/mix.h"
16 #include "../common/lprintf.h"
17
18 int mp3_last_error = 0;
19
20 static int initialized = 0;
21 static SceUID thread_job_sem = -1;
22 static SceUID thread_busy_sem = -1;
23 static int thread_exit = 0;
24
25 // MPEG-1, layer 3
26 static int bitrates[] = { 0, 32, 40, 48, 56, 64, 80, 96, 112, 128, 160, 192, 224, 256, 320, 0 };
27 //static int samplerates[] = { 44100, 48000, 32000, 0 };
28
29 #define MIN_INFRAME_SIZE 96
30 #define IN_BUFFER_SIZE (2*1024)
31
32 static unsigned long mp3_codec_struct[65] __attribute__((aligned(64)));
33
34 static unsigned char mp3_src_buffer[2][IN_BUFFER_SIZE] __attribute__((aligned(64)));
35 static short mp3_mix_buffer[2][1152*2] __attribute__((aligned(64)));
36 static int working_buf = 0;
37
38 static const char *mp3_fname = NULL;
39 static SceUID mp3_handle = -1;
40 static int mp3_src_pos = 0, mp3_src_size = 0;
41
42 static int decode_thread(SceSize args, void *argp);
43
44
45 static void psp_sem_lock(SceUID sem)
46 {
47         int ret = sceKernelWaitSema(sem, 1, 0);
48         if (ret < 0) lprintf("sceKernelWaitSema(%08x) failed with %08x\n", sem, ret);
49 }
50
51 static void psp_sem_unlock(SceUID sem)
52 {
53         int ret = sceKernelSignalSema(sem, 1);
54         if (ret < 0) lprintf("sceKernelSignalSema(%08x) failed with %08x\n", sem, ret);
55 }
56
57 // only accepts MPEG-1, layer3
58 static int find_sync_word(unsigned char *data, int len)
59 {
60         int i;
61         for (i = 0; i < len-1; i++)
62         {
63                 if ( data[i+0] != 0xff) continue;
64                 if ((data[i+1] & 0xfe) == 0xfa) return i;
65                 i++;
66         }
67         return -1;
68 }
69
70 static int read_next_frame(int which_buffer)
71 {
72         int i, bytes_read, frame_offset;
73         int bitrate, padding, frame_size = 0;
74
75         for (i = 0; i < 32; i++)
76         {
77                 bytes_read = sceIoRead(mp3_handle, mp3_src_buffer[which_buffer], sizeof(mp3_src_buffer[which_buffer]));
78                 mp3_src_pos += bytes_read;
79                 if (bytes_read < MIN_INFRAME_SIZE) {
80                         mp3_src_pos = mp3_src_size;
81                         return 0; // EOF/IO failure
82                 }
83                 frame_offset = find_sync_word(mp3_src_buffer[which_buffer], bytes_read);
84                 if (frame_offset < 0) {
85                         lprintf("missing syncword, foffs=%i\n", mp3_src_pos - bytes_read);
86                         mp3_src_pos--;
87                         sceIoLseek32(mp3_handle, mp3_src_pos, PSP_SEEK_SET);
88                         continue;
89                 }
90                 if (bytes_read - frame_offset < 4) {
91                         lprintf("syncword @ EOB, foffs=%i\n", mp3_src_pos - bytes_read);
92                         mp3_src_pos--;
93                         sceIoLseek32(mp3_handle, mp3_src_pos, PSP_SEEK_SET);
94                         continue;
95                 }
96
97                 bitrate =  mp3_src_buffer[which_buffer][frame_offset+2] >> 4;
98                 padding = (mp3_src_buffer[which_buffer][frame_offset+2] & 2) >> 1;
99
100                 frame_size = 144000*bitrates[bitrate]/44100 + padding;
101                 if (frame_size <= 0) {
102                         lprintf("bad frame, foffs=%i\n", mp3_src_pos - bytes_read);
103                         continue; // bad frame
104                 }
105
106                 if (bytes_read - frame_offset < frame_size)
107                 {
108                         lprintf("unfit, foffs=%i\n", mp3_src_pos - bytes_read);
109                         mp3_src_pos -= bytes_read - frame_offset;
110                         if (mp3_src_size - mp3_src_pos < frame_size) {
111                                 mp3_src_pos = mp3_src_size;
112                                 return 0; // EOF
113                         }
114                         sceIoLseek32(mp3_handle, mp3_src_pos, PSP_SEEK_SET);
115                         continue; // didn't fit, re-read..
116                 }
117
118                 if (frame_offset) {
119                         //lprintf("unaligned, foffs=%i, offs=%i\n", mp3_src_pos - bytes_read, frame_offset);
120                         memmove(mp3_src_buffer[which_buffer], mp3_src_buffer[which_buffer] + frame_offset, frame_size);
121                 }
122
123                 // align for next frame read
124                 mp3_src_pos -= bytes_read - (frame_offset + frame_size);
125                 sceIoLseek32(mp3_handle, mp3_src_pos, PSP_SEEK_SET);
126
127                 break;
128         }
129
130         return frame_size > 0 ? frame_size : -1;
131 }
132
133
134 static SceUID load_start_module(const char *prxname)
135 {
136         SceUID mod, mod1;
137         int status, ret;
138
139         mod = pspSdkLoadStartModule(prxname, PSP_MEMORY_PARTITION_KERNEL);
140         if (mod < 0) {
141                 lprintf("failed to load %s (%08x), trying kuKernelLoadModule\n", prxname, mod);
142                 mod1 = kuKernelLoadModule(prxname, 0, NULL);
143                 if (mod1 < 0) lprintf("kuKernelLoadModule failed with %08x\n", mod1);
144                 else {
145                         ret = sceKernelStartModule(mod1, 0, NULL, &status, 0);
146                         if (ret < 0) lprintf("sceKernelStartModule failed with %08x\n", ret);
147                         else mod = mod1;
148                 }
149         }
150         return mod;
151 }
152
153
154 int mp3_init(void)
155 {
156         SceUID thid, mod;
157         int ret;
158
159         /* load modules */
160         /* <= 1.5 (and probably some other, not sure which) fw need this to for audiocodec to work,
161          * so if it fails, assume we are just on new enough firmware and continue.. */
162         load_start_module("flash0:/kd/me_for_vsh.prx");
163
164         if (sceKernelDevkitVersion() < 0x02070010)
165              mod = load_start_module("flash0:/kd/audiocodec.prx");
166         else mod = load_start_module("flash0:/kd/avcodec.prx");
167         if (mod < 0) {
168                 ret = mod;
169                 mod = load_start_module("flash0:/kd/audiocodec_260.prx"); // last chance..
170                 if (mod < 0) goto fail;
171         }
172
173         /* audiocodec init */
174         memset(mp3_codec_struct, 0, sizeof(mp3_codec_struct));
175         ret = sceAudiocodecCheckNeedMem(mp3_codec_struct, 0x1002);
176         if (ret < 0) {
177                 lprintf("sceAudiocodecCheckNeedMem failed with %08x\n", ret);
178                 goto fail;
179         }
180
181         ret = sceAudiocodecGetEDRAM(mp3_codec_struct, 0x1002);
182         if (ret < 0) {
183                 lprintf("sceAudiocodecGetEDRAM failed with %08x\n", ret);
184                 goto fail;
185         }
186
187         ret = sceAudiocodecInit(mp3_codec_struct, 0x1002);
188         if (ret < 0) {
189                 lprintf("sceAudiocodecInit failed with %08x\n", ret);
190                 goto fail1;
191         }
192
193         /* thread and stuff */
194         thread_job_sem = sceKernelCreateSema("p_mp3job_sem", 0, 0, 1, NULL);
195         if (thread_job_sem < 0) {
196                 lprintf("sceKernelCreateSema() failed: %08x\n", thread_job_sem);
197                 ret = thread_job_sem;
198                 goto fail1;
199         }
200
201         thread_busy_sem = sceKernelCreateSema("p_mp3busy_sem", 0, 1, 1, NULL);
202         if (thread_busy_sem < 0) {
203                 lprintf("sceKernelCreateSema() failed: %08x\n", thread_busy_sem);
204                 ret = thread_busy_sem;
205                 goto fail2;
206         }
207
208         /* use slightly higher prio then main */
209         thread_exit = 0;
210         thid = sceKernelCreateThread("mp3decode_thread", decode_thread, 30, 0x2000, 0, NULL);
211         if (thid < 0) {
212                 lprintf("failed to create decode thread: %08x\n", thid);
213                 ret = thid;
214                 goto fail3;
215         }
216         ret = sceKernelStartThread(thid, 0, 0);
217         if (ret < 0) {
218                 lprintf("failed to start decode thread: %08x\n", ret);
219                 goto fail3;
220         }
221
222         mp3_last_error = 0;
223         initialized = 1;
224         return 0;
225
226 fail3:
227         sceKernelDeleteSema(thread_busy_sem);
228         thread_busy_sem = -1;
229 fail2:
230         sceKernelDeleteSema(thread_job_sem);
231         thread_job_sem = -1;
232 fail1:
233         sceAudiocodecReleaseEDRAM(mp3_codec_struct);
234 fail:
235         mp3_last_error = ret;
236         initialized = 0;
237         return 1;
238 }
239
240 void mp3_deinit(void)
241 {
242         lprintf("mp3_deinit, initialized=%i\n", initialized);
243
244         if (!initialized) return;
245         thread_exit = 1;
246         psp_sem_lock(thread_busy_sem);
247         psp_sem_unlock(thread_busy_sem);
248
249         sceKernelSignalSema(thread_job_sem, 1);
250         sceKernelDelayThread(100*1000);
251
252         if (mp3_handle >= 0) sceIoClose(mp3_handle);
253         mp3_handle = -1;
254         mp3_fname = NULL;
255
256         sceKernelDeleteSema(thread_busy_sem);
257         thread_busy_sem = -1;
258         sceKernelDeleteSema(thread_job_sem);
259         thread_job_sem = -1;
260         sceAudiocodecReleaseEDRAM(mp3_codec_struct);
261         initialized = 0;
262 }
263
264 // may overflow stack?
265 static int decode_thread(SceSize args, void *argp)
266 {
267         int ret, frame_size;
268
269         lprintf("decode_thread started with id %08x, priority %i\n",
270                 sceKernelGetThreadId(), sceKernelGetThreadCurrentPriority());
271
272         while (!thread_exit)
273         {
274                 psp_sem_lock(thread_job_sem);
275                 if (thread_exit) break;
276
277                 psp_sem_lock(thread_busy_sem);
278                 //lprintf("{ job\n");
279
280                 frame_size = read_next_frame(working_buf);
281                 if (frame_size > 0)
282                 {
283                         mp3_codec_struct[6] = (unsigned long)mp3_src_buffer[working_buf];
284                         mp3_codec_struct[8] = (unsigned long)mp3_mix_buffer[working_buf];
285                         mp3_codec_struct[7] = mp3_codec_struct[10] = frame_size;
286                         mp3_codec_struct[9] = 1152 * 4;
287
288                         ret = sceAudiocodecDecode(mp3_codec_struct, 0x1002);
289                         if (ret < 0) lprintf("sceAudiocodecDecode failed with %08x\n", ret);
290                 }
291
292                 //lprintf("} job\n");
293                 psp_sem_unlock(thread_busy_sem);
294         }
295
296         lprintf("leaving decode thread\n");
297         sceKernelExitDeleteThread(0);
298         return 0;
299 }
300
301
302 // might be called before initialization
303 int mp3_get_bitrate(FILE *f, int size)
304 {
305         int ret, retval = -1, sample_rate, bitrate;
306         // filenames are stored instead handles in PSP, due to stupid max open file limit
307         char *fname = (char *)f;
308
309         /* make sure thread is not busy.. */
310         if (thread_busy_sem >= 0)
311                 psp_sem_lock(thread_busy_sem);
312
313         if (mp3_handle >= 0) sceIoClose(mp3_handle);
314         mp3_handle = sceIoOpen(fname, PSP_O_RDONLY, 0777);
315         if (mp3_handle < 0) {
316                 lprintf("sceIoOpen(%s) failed\n", fname);
317                 goto end;
318         }
319
320         mp3_src_pos = 0;
321         ret = read_next_frame(0);
322         if (ret <= 0) {
323                 lprintf("read_next_frame() failed (%s)\n", fname);
324                 goto end;
325         }
326         sample_rate = (mp3_src_buffer[0][2] & 0x0c) >> 2;
327         bitrate = mp3_src_buffer[0][2] >> 4;
328
329         if (sample_rate != 0) {
330                 lprintf("unsupported samplerate (%s)\n", fname);
331                 goto end; // only 44kHz supported..
332         }
333         bitrate = bitrates[bitrate];
334         if (bitrate == 0) {
335                 lprintf("unsupported bitrate (%s)\n", fname);
336                 goto end;
337         }
338
339         /* looking good.. */
340         retval = bitrate;
341 end:
342         if (mp3_handle >= 0) sceIoClose(mp3_handle);
343         mp3_handle = -1;
344         mp3_fname = NULL;
345         if (thread_busy_sem >= 0)
346                 psp_sem_unlock(thread_busy_sem);
347         if (retval < 0) mp3_last_error = -1; // remember we had a problem..
348         return retval;
349 }
350
351
352 static int mp3_job_started = 0, mp3_samples_ready = 0, mp3_buffer_offs = 0, mp3_play_bufsel = 0;
353
354 void mp3_start_play(FILE *f, int pos)
355 {
356         char *fname = (char *)f;
357
358         if (!initialized) return;
359
360         lprintf("mp3_start_play(%s) @ %i\n", fname, pos);
361         psp_sem_lock(thread_busy_sem);
362
363         if (mp3_fname != fname || mp3_handle < 0)
364         {
365                 if (mp3_handle >= 0) sceIoClose(mp3_handle);
366                 mp3_handle = sceIoOpen(fname, PSP_O_RDONLY, 0777);
367                 if (mp3_handle < 0) {
368                         lprintf("sceIoOpen(%s) failed\n", fname);
369                         psp_sem_unlock(thread_busy_sem);
370                         return;
371                 }
372                 mp3_src_size = sceIoLseek32(mp3_handle, 0, PSP_SEEK_END);
373                 mp3_fname = fname;
374         }
375
376         // clear decoder state
377         sceAudiocodecInit(mp3_codec_struct, 0x1002);
378
379         // seek..
380         mp3_src_pos = (int) (((float)pos / 1023.0f) * (float)mp3_src_size);
381         sceIoLseek32(mp3_handle, mp3_src_pos, PSP_SEEK_SET);
382         lprintf("seek %i: %i/%i\n", pos, mp3_src_pos, mp3_src_size);
383
384         mp3_job_started = 1;
385         mp3_samples_ready = mp3_buffer_offs = mp3_play_bufsel = 0;
386         working_buf = 0;
387
388         /* send a request to decode first frame */
389         psp_sem_unlock(thread_busy_sem);
390         psp_sem_unlock(thread_job_sem);
391         sceKernelDelayThread(1); // reschedule
392 }
393
394
395 void mp3_update(int *buffer, int length, int stereo)
396 {
397         int length_mp3;
398
399         // playback was started, track not ended
400         if (mp3_handle < 0 || mp3_src_pos >= mp3_src_size) return;
401
402         length_mp3 = length;
403         if (PsndRate == 22050) length_mp3 <<= 1;        // mp3s are locked to 44100Hz stereo
404         else if (PsndRate == 11025) length_mp3 <<= 2;   // so make length 44100ish
405
406         /* do we have to wait? */
407         if (mp3_job_started && mp3_samples_ready < length_mp3)
408         {
409                 psp_sem_lock(thread_busy_sem);
410                 psp_sem_unlock(thread_busy_sem);
411                 mp3_job_started = 0;
412                 mp3_samples_ready += 1152;
413         }
414
415         /* mix mp3 data, only stereo */
416         if (mp3_samples_ready >= length_mp3)
417         {
418                 int shr = 0;
419                 void (*mix_samples)(int *dest_buf, short *mp3_buf, int count) = mix_16h_to_32;
420                 if (PsndRate == 22050) { mix_samples = mix_16h_to_32_s1; shr = 1; }
421                 else if (PsndRate == 11025) { mix_samples = mix_16h_to_32_s2; shr = 2; }
422
423                 if (1152 - mp3_buffer_offs >= length_mp3) {
424                         mix_samples(buffer, mp3_mix_buffer[mp3_play_bufsel] + mp3_buffer_offs*2, length<<1);
425
426                         mp3_buffer_offs += length_mp3;
427                 } else {
428                         // collect samples from both buffers..
429                         int left = 1152 - mp3_buffer_offs;
430                         if (mp3_play_bufsel == 0)
431                         {
432                                 mix_samples(buffer, mp3_mix_buffer[0] + mp3_buffer_offs*2, length<<1);
433                                 mp3_buffer_offs = length_mp3 - left;
434                                 mp3_play_bufsel = 1;
435                         } else {
436                                 mix_samples(buffer, mp3_mix_buffer[1] + mp3_buffer_offs*2, (left>>shr)<<1);
437                                 mp3_buffer_offs = length_mp3 - left;
438                                 mix_samples(buffer + ((left>>shr)<<1),
439                                         mp3_mix_buffer[0], (mp3_buffer_offs>>shr)<<1);
440                                 mp3_play_bufsel = 0;
441                         }
442                 }
443                 mp3_samples_ready -= length_mp3;
444         }
445
446         // ask to decode more if we already can
447         if (!mp3_job_started)
448         {
449                 mp3_job_started = 1;
450                 working_buf ^= 1;
451
452                 /* next job.. */
453                 psp_sem_lock(thread_busy_sem);   // just in case
454                 psp_sem_unlock(thread_busy_sem);
455                 psp_sem_unlock(thread_job_sem);
456                 sceKernelDelayThread(1);
457         }
458 }
459
460
461 int mp3_get_offset(void) // 0-1023
462 {
463         unsigned int offs1024 = 0;
464         int cdda_on;
465
466         cdda_on = (PicoAHW & PAHW_MCD) && (PicoOpt&0x800) && !(Pico_mcd->s68k_regs[0x36] & 1) &&
467                         (Pico_mcd->scd.Status_CDC & 1) && mp3_handle >= 0;
468
469         if (cdda_on) {
470                 offs1024  = mp3_src_pos << 7;
471                 offs1024 /= mp3_src_size >> 3;
472         }
473         lprintf("offs1024=%u (%i/%i)\n", offs1024, mp3_src_pos, mp3_src_size);
474
475         return offs1024;
476 }
477
478
479 void mp3_reopen_file(void)
480 {
481         if (mp3_fname == NULL) return;
482         lprintf("mp3_reopen_file(%s)\n", mp3_fname);
483
484         // try closing, just in case
485         if (mp3_handle >= 0) sceIoClose(mp3_handle);
486
487         mp3_handle = sceIoOpen(mp3_fname, PSP_O_RDONLY, 0777);
488         if (mp3_handle >= 0)
489                 sceIoLseek32(mp3_handle, mp3_src_pos, PSP_SEEK_SET);
490         lprintf("mp3_reopen_file %s\n", mp3_handle >= 0 ? "ok" : "failed");
491 }
492