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[3][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 #ifndef __SNDLIB_H__
 153         DCFlushRangeNoSync(data,len);
 154 #endif
 155         LWP_ThreadSignal(thQueue);
 156 #ifndef __SNDLIB_H__
 157         _sync();
 158 #endif
 159         return len;
 160 }
 161
 162 static __inline__ s32 buf_put(struct _outbuffer_s *buf,void *data,s32 len)
 163 {
 164         u32 *p;
 165         s32 cnt,i;
 166
 167         while(len>buf_space(buf))
 168                 LWP_ThreadSleep(thQueue);
 169
 170         p = data;
 171         cnt = ((u32)buf->bs + DATABUFFER_SIZE - (u32)buf->put);
 172         if(len>cnt) {
 173                 for(i=0;i<(cnt>>2);i++)
 174                         *buf->put++ = *p++;
 175                 buf->put = buf->bs;
 176                 for(i=0;i<((len-cnt)>>2);i++)
 177                         *buf->put++ = *p++;
 178         } else {
 179                 for(i=0;i<(len>>2);i++)
 180                         *buf->put++ = *p++;
 181         }
 182
 183         if(buf->buf_filled==0 && buf_used(buf)>=(DATABUFFER_SIZE>>1)) {
 184                 buf->buf_filled = 1;
 185                 memset(OutputBuffer[CurrentBuffer],0,ADMA_BUFFERSIZE);
 186
 187 #ifndef __SNDLIB_H__
 188                 DCFlushRange(OutputBuffer[CurrentBuffer],ADMA_BUFFERSIZE);
 189                 AUDIO_InitDMA((u32)OutputBuffer[CurrentBuffer],ADMA_BUFFERSIZE);
 190                 AUDIO_StartDMA();
 191 #else
 192                 have_samples = 0;
 193                 SND_SetVoice(0,VOICE_STEREO_16BIT,48000,0,(void*)OutputBuffer[CurrentBuffer],ADMA_BUFFERSIZE,mp3_volume,mp3_volume,DataTransferCallback);
 194 #endif
 195
 196                 CurrentBuffer = (CurrentBuffer+1)%3;
 197         }
 198
 199         return len;
 200 }
 201
 202 static s32 _mp3ramcopy(void *usr_data,void *buffer,s32 len)
 203 {
 204         struct _rambuffer *ram = (struct _rambuffer*)usr_data;
 205         const void *ptr = ((u8*)ram->buf_addr+ram->pos);
 206
 207         if((ram->pos+len)>ram->len) {
 208                 len = (ram->len - ram->pos);
 209         }
 210         if(len<=0) return 0;
 211
 212         memcpy(buffer,ptr,len);
 213         ram->pos += len;
 214
 215         return len;
 216 }
 217
 218 void MP3Player_Init()
 219 {
 220         if(!init_done) {
 221                 init_done = 1;
 222 #ifndef __SNDLIB_H__
 223                 AUDIO_Init(NULL);
 224                 AUDIO_SetDSPSampleRate(AI_SAMPLERATE_48KHZ);
 225 #else
 226                 SND_Pause(0);
 227                 SND_StopVoice(0);
 228 #endif
 229         }
 230 }
 231
 232 s32 MP3Player_PlayBuffer(const void *buffer,s32 len,void (*filterfunc)(struct mad_stream *,struct mad_frame *))
 233 {
 234         if(thr_running==TRUE) return -1;
 235
 236         rambuffer.buf_addr = buffer;
 237         rambuffer.len = len;
 238         rambuffer.pos = 0;
 239
 240         mp3cb_data = &rambuffer;
 241         mp3read = _mp3ramcopy;
 242         mp3filterfunc = filterfunc;
 243         if(LWP_CreateThread(&hStreamPlay,StreamPlay,NULL,StreamPlay_Stack,STACKSIZE,80)<0) {
 244                 return -1;
 245         }
 246         return 0;
 247 }
 248
 249 s32 MP3Player_PlayFile(void *cb_data,s32 (*reader)(void *,void *,s32),void (*filterfunc)(struct mad_stream *,struct mad_frame *))
 250 {
 251         if(thr_running==TRUE) return -1;
 252
 253         mp3cb_data = cb_data;
 254         mp3read = reader;
 255         mp3filterfunc = filterfunc;
 256         if(LWP_CreateThread(&hStreamPlay,StreamPlay,NULL,StreamPlay_Stack,STACKSIZE,80)<0) {
 257                 return -1;
 258         }
 259         return 0;
 260 }
 261
 262 void MP3Player_Stop()
 263 {
 264         if(thr_running==FALSE) return;
 265
 266         thr_running = FALSE;
 267         LWP_JoinThread(hStreamPlay,NULL);
 268 }
 269
 270 BOOL MP3Player_IsPlaying()
 271 {
 272         return thr_running;
 273 }
 274
 275 static void *StreamPlay(void *arg)
 276 {
 277         BOOL atend;
 278         u8 *GuardPtr = NULL;
 279         struct mad_stream Stream;
 280         struct mad_frame Frame;
 281         struct mad_synth Synth;
 282         mad_timer_t Timer;
 283         EQState eqs[2];
 284
 285         thr_running = TRUE;
 286
 287         CurrentBuffer = 0;
 288         memset(OutputBuffer[0],0,ADMA_BUFFERSIZE);
 289         memset(OutputBuffer[1],0,ADMA_BUFFERSIZE);
 290         memset(OutputBuffer[2],0,ADMA_BUFFERSIZE);
 291
 292         buf_init(&OutputRingBuffer);
 293         LWP_InitQueue(&thQueue);
 294         Init3BandState(&eqs[0],880,5000,48000);
 295         Init3BandState(&eqs[1],880,5000,48000);
 296
 297 #ifndef __SNDLIB_H__
 298         AUDIO_RegisterDMACallback(DataTransferCallback);
 299 #endif
 300
 301         mad_stream_init(&Stream);
 302         mad_frame_init(&Frame);
 303         mad_synth_init(&Synth);
 304         mad_timer_reset(&Timer);
 305
 306         atend = FALSE;
 307         MP3Playing = FALSE;
 308         while(atend==FALSE && thr_running==TRUE) {
 309                 if(Stream.buffer==NULL || Stream.error==MAD_ERROR_BUFLEN) {
 310                         u8 *ReadStart;
 311                         s32 ReadSize, Remaining;
 312
 313                         if(Stream.next_frame!=NULL) {
 314                                 Remaining = Stream.bufend - Stream.next_frame;
 315                                 memmove(InputBuffer,Stream.next_frame,Remaining);
 316                                 ReadStart = InputBuffer + Remaining;
 317                                 ReadSize = DATABUFFER_SIZE - Remaining;
 318                         } else {
 319                                 ReadSize = DATABUFFER_SIZE;
 320                                 ReadStart = InputBuffer;
 321                                 Remaining = 0;
 322                         }
 323
 324
 325                         ReadSize = mp3read(mp3cb_data,ReadStart,ReadSize);
 326                         if(ReadSize<=0) {
 327                                 GuardPtr = ReadStart;
 328                                 memset(GuardPtr,0,MAD_BUFFER_GUARD);
 329                                 ReadSize = MAD_BUFFER_GUARD;
 330                                 atend = TRUE;
 331                         }
 332
 333                         mad_stream_buffer(&Stream,InputBuffer,(ReadSize + Remaining));
 334                         //Stream.error = 0;
 335                 }
 336
 337                 if(mad_frame_decode(&Frame,&Stream)) {
 338                         if(MAD_RECOVERABLE(Stream.error)) {
 339                           if(Stream.error!=MAD_ERROR_LOSTSYNC
 340                                 || Stream.this_frame!=GuardPtr) continue;
 341                         } else {
 342                                 if(Stream.error!=MAD_ERROR_BUFLEN) break;
 343                         }
 344                 }
 345
 346                 if(mp3filterfunc)
 347                         mp3filterfunc(&Stream,&Frame);
 348
 349                 mad_timer_add(&Timer,Frame.header.duration);
 350                 mad_synth_frame(&Synth,&Frame);
 351
 352                 Resample(&Synth.pcm,eqs,(MAD_NCHANNELS(&Frame.header)==2),Frame.header.samplerate);
 353         }
 354
 355         mad_synth_finish(&Synth);
 356         mad_frame_finish(&Frame);
 357         mad_stream_finish(&Stream);
 358
 359         while(MP3Playing==TRUE)
 360                 LWP_ThreadSleep(thQueue);
 361
 362 #ifndef __SNDLIB_H__
 363         AUDIO_StopDMA();
 364         AUDIO_RegisterDMACallback(NULL);
 365 #else
 366         SND_StopVoice(0);
 367 #endif
 368
 369         LWP_CloseQueue(thQueue);
 370
 371         thr_running = FALSE;
 372
 373         return 0;
 374 }
 375
 376 typedef union {
 377         struct {
 378                 u16 hi;
 379                 u16 lo;
 380         } aword;
 381         u32 adword;
 382 } dword;
 383
 384 static void Resample(struct mad_pcm *Pcm,EQState eqs[2],u32 stereo,u32 src_samplerate)
 385 {
 386         u16 val16;
 387         u32 val32;
 388         dword pos;
 389         s32 incr;
 390
 391         pos.adword = 0;
 392         incr = (u32)(((f32)src_samplerate/48000.0F)*65536.0F);
 393         while(pos.aword.hi<Pcm->length) {
 394                 val16 = Do3Band(&eqs[0],FixedToShort(Pcm->samples[0][pos.aword.hi]));
 395                 val32 = (val16<<16);
 396
 397                 if(stereo) val16 = Do3Band(&eqs[1],FixedToShort(Pcm->samples[1][pos.aword.hi]));
 398                 val32 |= val16;
 399
 400                 buf_put(&OutputRingBuffer,&val32,sizeof(u32));
 401                 pos.adword += incr;
 402         }
 403 }
 404
 405 static void Init3BandState(EQState *es,s32 lowfreq,s32 highfreq,s32 mixfreq)
 406 {
 407         memset(es,0,sizeof(EQState));
 408
 409         es->lg = 1.0;
 410         es->mg = 1.0;
 411         es->hg = 1.0;
 412
 413         es->lf = 2.0F*sinf(M_PI*((f32)lowfreq/(f32)mixfreq));
 414         es->hf = 2.0F*sinf(M_PI*((f32)highfreq/(f32)mixfreq));
 415 }
 416
 417 static s16 Do3Band(EQState *es,s16 sample)
 418 {
 419         f32 l,m,h;
 420
 421         es->f1p0 += (es->lf*((f32)sample - es->f1p0))+VSA;
 422         es->f1p1 += (es->lf*(es->f1p0 - es->f1p1));
 423         es->f1p2 += (es->lf*(es->f1p1 - es->f1p2));
 424         es->f1p3 += (es->lf*(es->f1p2 - es->f1p3));
 425         l = es->f1p3;
 426
 427         es->f2p0 += (es->hf*((f32)sample - es->f2p0))+VSA;
 428         es->f2p1 += (es->hf*(es->f2p0 - es->f2p1));
 429         es->f2p2 += (es->hf*(es->f2p1 - es->f2p2));
 430         es->f2p3 += (es->hf*(es->f2p2 - es->f2p3));
 431         h = es->sdm3 - es->f2p3;
 432
 433         m = es->sdm3 - (h+l);
 434
 435         l *= es->lg;
 436         m *= es->mg;
 437         h *= es->hg;
 438
 439         es->sdm3 = es->sdm2;
 440         es->sdm2 = es->sdm1;
 441         es->sdm1 = (f32)sample;
 442
 443         return (s16)(l+m+h);
 444 }
 445
 446 static void DataTransferCallback()
 447 {
 448 #ifndef __SNDLIB_H__
 449         AUDIO_InitDMA((u32)OutputBuffer[CurrentBuffer],ADMA_BUFFERSIZE);
 450
 451         CurrentBuffer = (CurrentBuffer+1)%3;
 452         MP3Playing = (buf_get(&OutputRingBuffer,OutputBuffer[CurrentBuffer],ADMA_BUFFERSIZE)>0);
 453 #else
 454         if(thr_running!=TRUE) {
 455                 MP3Playing = (buf_get(&OutputRingBuffer,OutputBuffer[CurrentBuffer],ADMA_BUFFERSIZE)>0);
 456                 return;
 457         }
 458         if(have_samples==1) {
 459                 if(SND_AddVoice(0,(void*)OutputBuffer[CurrentBuffer],ADMA_BUFFERSIZE)==SND_OK) {
 460                         have_samples = 0;
 461                         CurrentBuffer = (CurrentBuffer+1)%3;
 462                 }
 463         }
 464         if(!(SND_TestPointer(0,(void*)OutputBuffer[CurrentBuffer]) && SND_StatusVoice(0)!=SND_UNUSED)) {
 465                 if(have_samples==0) {
 466                         MP3Playing = (buf_get(&OutputRingBuffer,OutputBuffer[CurrentBuffer],ADMA_BUFFERSIZE)>0);
 467                         have_samples = 1;
 468                 }
 469         }
 470 #endif
 471 }
 472
 473 void MP3Player_Volume(u32 volume)
 474 {
 475         if(volume>255) volume = 255;
 476
 477         mp3_volume = volume;
 478 #ifdef __SNDLIB_H__
 479         SND_ChangeVolumeVoice(0,volume,volume);
 480 #endif
 481 }