libmad/mp3player.c

   1 /***************************************************************************
   2  *   Low Level MAD Library Implementation for the GameCube                 *
   3  *   By Daniel "nForce" Bloemendal                                         *
   4  *   nForce@Sympatico.ca                                                   *
   5  ***************************************************************************/
   6 // Modified by Hermes to include SNDLIB / ASNDLIB support
   7
   8 #include <asm.h>
   9 #include <processor.h>
  10 #include <stdlib.h>
  11 #include <stdio.h>
  12 #include <string.h>
  13 #include <math.h>
  14 #include <lwp.h>
  15 #include <pad.h>
  16 #include <limits.h>
  17 #include <system.h>
  18 #include <ogcsys.h>
  19 #include <malloc.h>
  20
  21 #include "asndlib.h"
  22 #include "mp3player.h"
  23
  24 static s32 have_samples = 0;
  25 static u32 mp3_volume = 255;
  26
  27 #ifndef __SNDLIB_H__
  28         #define ADMA_BUFFERSIZE                 (4992)
  29 #else
  30         #define ADMA_BUFFERSIZE                 (8192)
  31 #endif
  32 #define STACKSIZE                               (32768)
  33
  34 #define DATABUFFER_SIZE                 (32768)
  35
  36 typedef struct _eqstate_s
  37 {
  38         f32 lf;
  39         f32 f1p0;
  40         f32 f1p1;
  41         f32 f1p2;
  42         f32 f1p3;
  43
  44         f32 hf;
  45         f32 f2p0;
  46         f32 f2p1;
  47         f32 f2p2;
  48         f32 f2p3;
  49
  50         f32 sdm1;
  51         f32 sdm2;
  52         f32 sdm3;
  53
  54         f32 lg;
  55         f32 mg;
  56         f32 hg;
  57 } EQState;
  58
  59 struct _outbuffer_s
  60 {
  61         void *bs;
  62         u32 *put,*get;
  63         s32 buf_filled;
  64         u8 buffer[DATABUFFER_SIZE];
  65 };
  66
  67 static u8 InputBuffer[DATABUFFER_SIZE+MAD_BUFFER_GUARD];
  68 static u8 OutputBuffer[2][ADMA_BUFFERSIZE] ATTRIBUTE_ALIGN(32);
  69 static struct _outbuffer_s OutputRingBuffer;
  70
  71 static u32 init_done = 0;
  72 static u32 CurrentBuffer = 0;
  73 static f32 VSA = (1.0/4294967295.0);
  74 static BOOL thr_running = FALSE;
  75 static BOOL MP3Playing = FALSE;
  76 static void *mp3cb_data = NULL;
  77
  78 static void* StreamPlay(void *);
  79 static u8 StreamPlay_Stack[STACKSIZE];
  80 static lwp_t hStreamPlay;
  81 static lwpq_t thQueue;
  82
  83 static s32 (*mp3read)(void*,void *,s32);
  84 static void (*mp3filterfunc)(struct mad_stream *,struct mad_frame *);
  85
  86 static void DataTransferCallback();
  87 static void Init3BandState(EQState *es,s32 lowfreq,s32 highfreq,s32 mixfreq);
  88 static s16 Do3Band(EQState *es,s16 sample);
  89 static void Resample(struct mad_pcm *Pcm,EQState eqs[2],u32 stereo,u32 src_samplerate);
  90
  91 struct _rambuffer
  92 {
  93         const void *buf_addr;
  94         s32 len,pos;
  95 } rambuffer;
  96
  97 static __inline__ s16 FixedToShort(mad_fixed_t Fixed)
  98 {
  99         /* Clipping */
 100         if(Fixed>=MAD_F_ONE)
 101                 return(SHRT_MAX);
 102         if(Fixed<=-MAD_F_ONE)
 103                 return(-SHRT_MAX);
 104
 105         Fixed=Fixed>>(MAD_F_FRACBITS-15);
 106         return((s16)Fixed);
 107 }
 108
 109 static __inline__ void buf_init(struct _outbuffer_s *buf)
 110 {
 111         buf->buf_filled = 0;
 112         buf->bs = buf->buffer;
 113         buf->put = buf->get = buf->bs;
 114 }
 115
 116 static __inline__ s32 buf_used(struct _outbuffer_s *buf)
 117 {
 118         return ((DATABUFFER_SIZE + ((u32)buf->put - (u32)buf->get)) % DATABUFFER_SIZE);
 119 }
 120
 121 static __inline__ s32 buf_space(struct _outbuffer_s *buf)
 122 {
 123         return ((DATABUFFER_SIZE - ((u32)buf->put - (u32)buf->get) - 1) % DATABUFFER_SIZE);
 124 }
 125
 126 static __inline__ s32 buf_get(struct _outbuffer_s *buf,void *data,s32 len)
 127 {
 128         u32 *p;
 129         s32 cnt,i;
 130
 131         if(len>buf_used(buf))
 132                 len = buf_used(buf);
 133
 134         if(len==0) {
 135                 LWP_ThreadSignal(thQueue);
 136                 return 0;
 137         }
 138
 139         p = data;
 140         cnt = ((u32)buf->bs + DATABUFFER_SIZE - (u32)buf->get);
 141         if(len>cnt) {
 142                 for(i=0;i<(cnt>>2);i++)
 143                         *p++ = *buf->get++;
 144                 buf->get = buf->bs;
 145                 for(i=0;i<((len-cnt)>>2);i++)
 146                         *p++ = *buf->get++;
 147         } else {
 148                 for(i=0;i<(len>>2);i++)
 149                         *p++ = *buf->get++;
 150         }
 151
 152         DCFlushRangeNoSync(data,len);
 153         LWP_ThreadSignal(thQueue);
 154         _sync();
 155
 156         return len;
 157 }
 158
 159 static __inline__ s32 buf_put(struct _outbuffer_s *buf,void *data,s32 len)
 160 {
 161         u32 *p;
 162         s32 cnt,i;
 163
 164         while(len>buf_space(buf))
 165                 LWP_ThreadSleep(thQueue);
 166
 167         p = data;
 168         cnt = ((u32)buf->bs + DATABUFFER_SIZE - (u32)buf->put);
 169         if(len>cnt) {
 170                 for(i=0;i<(cnt>>2);i++)
 171                         *buf->put++ = *p++;
 172                 buf->put = buf->bs;
 173                 for(i=0;i<((len-cnt)>>2);i++)
 174                         *buf->put++ = *p++;
 175         } else {
 176                 for(i=0;i<(len>>2);i++)
 177                         *buf->put++ = *p++;
 178         }
 179
 180         if(buf->buf_filled==0 && buf_used(buf)>=(DATABUFFER_SIZE>>1)) {
 181                 buf->buf_filled = 1;
 182                 memset(OutputBuffer[CurrentBuffer],0,ADMA_BUFFERSIZE);
 183
 184 #ifndef __SNDLIB_H__
 185                 DCFlushRange(OutputBuffer[CurrentBuffer],ADMA_BUFFERSIZE);
 186                 AUDIO_InitDMA((u32)OutputBuffer[CurrentBuffer],ADMA_BUFFERSIZE);
 187                 AUDIO_StartDMA();
 188 #else
 189                 have_samples = 0;
 190                 SND_SetVoice(0,VOICE_STEREO_16BIT,48000,0,(void*)OutputBuffer[CurrentBuffer],ADMA_BUFFERSIZE,mp3_volume,mp3_volume,DataTransferCallback);
 191 #endif
 192
 193                 CurrentBuffer ^= 1;
 194         }
 195
 196         return len;
 197 }
 198
 199 static s32 _mp3ramcopy(void *usr_data,void *buffer,s32 len)
 200 {
 201         struct _rambuffer *ram = (struct _rambuffer*)usr_data;
 202         const void *ptr = ((u8*)ram->buf_addr+ram->pos);
 203
 204         if((ram->pos+len)>ram->len) {
 205                 len = (ram->len - ram->pos);
 206         }
 207         if(len<=0) return 0;
 208
 209         memcpy(buffer,ptr,len);
 210         ram->pos += len;
 211
 212         return len;
 213 }
 214
 215 void MP3Player_Init()
 216 {
 217         if(!init_done) {
 218                 init_done = 1;
 219 #ifndef __SNDLIB_H__
 220                 AUDIO_Init(NULL);
 221                 AUDIO_SetDSPSampleRate(AI_SAMPLERATE_48KHZ);
 222 #else
 223                 SND_Pause(0);
 224                 SND_StopVoice(0);
 225 #endif
 226         }
 227 }
 228
 229 s32 MP3Player_PlayBuffer(const void *buffer,s32 len,void (*filterfunc)(struct mad_stream *,struct mad_frame *))
 230 {
 231         if(thr_running==TRUE) return -1;
 232
 233         rambuffer.buf_addr = buffer;
 234         rambuffer.len = len;
 235         rambuffer.pos = 0;
 236
 237         mp3cb_data = &rambuffer;
 238         mp3read = _mp3ramcopy;
 239         mp3filterfunc = filterfunc;
 240         if(LWP_CreateThread(&hStreamPlay,StreamPlay,NULL,StreamPlay_Stack,STACKSIZE,80)<0) {
 241                 return -1;
 242         }
 243         return 0;
 244 }
 245
 246 s32 MP3Player_PlayFile(void *cb_data,s32 (*reader)(void *,void *,s32),void (*filterfunc)(struct mad_stream *,struct mad_frame *))
 247 {
 248         if(thr_running==TRUE) return -1;
 249
 250         mp3cb_data = cb_data;
 251         mp3read = reader;
 252         mp3filterfunc = filterfunc;
 253         if(LWP_CreateThread(&hStreamPlay,StreamPlay,NULL,StreamPlay_Stack,STACKSIZE,80)<0) {
 254                 return -1;
 255         }
 256         return 0;
 257 }
 258
 259 void MP3Player_Stop()
 260 {
 261         if(thr_running==FALSE) return;
 262
 263         thr_running = FALSE;
 264         LWP_JoinThread(hStreamPlay,NULL);
 265 }
 266
 267 BOOL MP3Player_IsPlaying()
 268 {
 269         return thr_running;
 270 }
 271
 272 static void *StreamPlay(void *arg)
 273 {
 274         BOOL atend;
 275         u8 *GuardPtr = NULL;
 276         struct mad_stream Stream;
 277         struct mad_frame Frame;
 278         struct mad_synth Synth;
 279         mad_timer_t Timer;
 280         EQState eqs[2];
 281
 282         thr_running = TRUE;
 283
 284         CurrentBuffer = 0;
 285         memset(OutputBuffer[0],0,ADMA_BUFFERSIZE);
 286         memset(OutputBuffer[1],0,ADMA_BUFFERSIZE);
 287
 288         buf_init(&OutputRingBuffer);
 289         LWP_InitQueue(&thQueue);
 290         Init3BandState(&eqs[0],880,5000,48000);
 291         Init3BandState(&eqs[1],880,5000,48000);
 292
 293 #ifndef __SNDLIB_H__
 294         AUDIO_RegisterDMACallback(DataTransferCallback);
 295 #endif
 296
 297         mad_stream_init(&Stream);
 298         mad_frame_init(&Frame);
 299         mad_synth_init(&Synth);
 300         mad_timer_reset(&Timer);
 301
 302         atend = FALSE;
 303         MP3Playing = FALSE;
 304         while(atend==FALSE && thr_running==TRUE) {
 305                 if(Stream.buffer==NULL || Stream.error==MAD_ERROR_BUFLEN) {
 306                         u8 *ReadStart;
 307                         s32 ReadSize, Remaining;
 308
 309                         if(Stream.next_frame!=NULL) {
 310                                 Remaining = Stream.bufend - Stream.next_frame;
 311                                 memmove(InputBuffer,Stream.next_frame,Remaining);
 312                                 ReadStart = InputBuffer + Remaining;
 313                                 ReadSize = DATABUFFER_SIZE - Remaining;
 314                         } else {
 315                                 ReadSize = DATABUFFER_SIZE;
 316                                 ReadStart = InputBuffer;
 317                                 Remaining = 0;
 318                         }
 319
 320
 321                         ReadSize = mp3read(mp3cb_data,ReadStart,ReadSize);
 322                         if(ReadSize<=0) {
 323                                 GuardPtr = ReadStart;
 324                                 memset(GuardPtr,0,MAD_BUFFER_GUARD);
 325                                 ReadSize = MAD_BUFFER_GUARD;
 326                                 atend = TRUE;
 327                         }
 328
 329                         mad_stream_buffer(&Stream,InputBuffer,(ReadSize + Remaining));
 330                         //Stream.error = 0;
 331                 }
 332
 333                 if(mad_frame_decode(&Frame,&Stream)) {
 334                         if(MAD_RECOVERABLE(Stream.error)) {
 335                           if(Stream.error!=MAD_ERROR_LOSTSYNC
 336                                 || Stream.this_frame!=GuardPtr) continue;
 337                         } else {
 338                                 if(Stream.error!=MAD_ERROR_BUFLEN) break;
 339                         }
 340                 }
 341
 342                 if(mp3filterfunc)
 343                         mp3filterfunc(&Stream,&Frame);
 344
 345                 mad_timer_add(&Timer,Frame.header.duration);
 346                 mad_synth_frame(&Synth,&Frame);
 347
 348                 Resample(&Synth.pcm,eqs,(MAD_NCHANNELS(&Frame.header)==2),Frame.header.samplerate);
 349         }
 350
 351         mad_synth_finish(&Synth);
 352         mad_frame_finish(&Frame);
 353         mad_stream_finish(&Stream);
 354
 355         while(MP3Playing==TRUE)
 356                 LWP_ThreadSleep(thQueue);
 357
 358 #ifndef __SNDLIB_H__
 359         AUDIO_StopDMA();
 360         AUDIO_RegisterDMACallback(NULL);
 361 #else
 362         SND_StopVoice(0);
 363 #endif
 364
 365         LWP_CloseQueue(thQueue);
 366
 367         thr_running = FALSE;
 368
 369         return 0;
 370 }
 371
 372 typedef union {
 373         struct {
 374                 u16 hi;
 375                 u16 lo;
 376         } aword;
 377         u32 adword;
 378 } dword;
 379
 380 static void Resample(struct mad_pcm *Pcm,EQState eqs[2],u32 stereo,u32 src_samplerate)
 381 {
 382         u16 val16;
 383         u32 val32;
 384         dword pos;
 385         s32 incr;
 386
 387         pos.adword = 0;
 388         incr = (u32)(((f32)src_samplerate/48000.0F)*65536.0F);
 389         while(pos.aword.hi<Pcm->length) {
 390                 val16 = Do3Band(&eqs[0],FixedToShort(Pcm->samples[0][pos.aword.hi]));
 391                 val32 = (val16<<16);
 392
 393                 if(stereo) val16 = Do3Band(&eqs[1],FixedToShort(Pcm->samples[1][pos.aword.hi]));
 394                 val32 |= val16;
 395
 396                 buf_put(&OutputRingBuffer,&val32,sizeof(u32));
 397                 pos.adword += incr;
 398         }
 399 }
 400
 401 static void Init3BandState(EQState *es,s32 lowfreq,s32 highfreq,s32 mixfreq)
 402 {
 403         memset(es,0,sizeof(EQState));
 404
 405         es->lg = 1.0;
 406         es->mg = 1.0;
 407         es->hg = 1.0;
 408
 409         es->lf = 2.0F*sinf(M_PI*((f32)lowfreq/(f32)mixfreq));
 410         es->hf = 2.0F*sinf(M_PI*((f32)highfreq/(f32)mixfreq));
 411 }
 412
 413 static s16 Do3Band(EQState *es,s16 sample)
 414 {
 415         f32 l,m,h;
 416
 417         es->f1p0 += (es->lf*((f32)sample - es->f1p0))+VSA;
 418         es->f1p1 += (es->lf*(es->f1p0 - es->f1p1));
 419         es->f1p2 += (es->lf*(es->f1p1 - es->f1p2));
 420         es->f1p3 += (es->lf*(es->f1p2 - es->f1p3));
 421         l = es->f1p3;
 422
 423         es->f2p0 += (es->hf*((f32)sample - es->f2p0))+VSA;
 424         es->f2p1 += (es->hf*(es->f2p0 - es->f2p1));
 425         es->f2p2 += (es->hf*(es->f2p1 - es->f2p2));
 426         es->f2p3 += (es->hf*(es->f2p2 - es->f2p3));
 427         h = es->sdm3 - es->f2p3;
 428
 429         m = es->sdm3 - (h+l);
 430
 431         l *= es->lg;
 432         m *= es->mg;
 433         h *= es->hg;
 434
 435         es->sdm3 = es->sdm2;
 436         es->sdm2 = es->sdm1;
 437         es->sdm1 = (f32)sample;
 438
 439         return (s16)(l+m+h);
 440 }
 441
 442 static void DataTransferCallback()
 443 {
 444 #ifndef __SNDLIB_H__
 445         AUDIO_StopDMA();
 446         AUDIO_InitDMA((u32)OutputBuffer[CurrentBuffer],ADMA_BUFFERSIZE);
 447         AUDIO_StartDMA();
 448
 449         CurrentBuffer ^= 1;
 450         MP3Playing = (buf_get(&OutputRingBuffer,OutputBuffer[CurrentBuffer],ADMA_BUFFERSIZE)>0);
 451 #else
 452         if(thr_running!=TRUE) {
 453                 MP3Playing = (buf_get(&OutputRingBuffer,OutputBuffer[CurrentBuffer],ADMA_BUFFERSIZE)>0);
 454                 return;
 455         }
 456         if(have_samples==1) {
 457                 if(SND_AddVoice(0,(void*)OutputBuffer[CurrentBuffer],ADMA_BUFFERSIZE)==SND_OK) {
 458                         have_samples = 0;
 459                         CurrentBuffer ^= 1;
 460                 }
 461         }
 462         if(!(SND_TestPointer(0,(void*)OutputBuffer[CurrentBuffer]) && SND_StatusVoice(0)!=SND_UNUSED)) {
 463                 if(have_samples==0) {
 464                         MP3Playing = (buf_get(&OutputRingBuffer,OutputBuffer[CurrentBuffer],ADMA_BUFFERSIZE)>0);
 465                         have_samples = 1;
 466                 }
 467         }
 468 #endif
 469 }
 470
 471 void MP3Player_Volume(u32 volume)
 472 {
 473         if(volume>255) volume = 255;
 474
 475         mp3_volume = volume;
 476 #ifdef __SNDLIB_H__
 477         SND_ChangeVolumeVoice(0,volume,volume);
 478 #endif
 479 }