/***************************************************************************** delfinampeg.device - mpeg.device for Delfina DSP Copyright (C) 2000 Michael Henke This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA *****************************************************************************/ /*#define DEBUG*/ #include #include #include #include #include #include #ifdef DEBUG extern void KPutStr(STRPTR); extern LONG KPutChar(LONG); #endif extern struct DelfObj DSP56K_PCM; extern struct DelfObj DSP56K_MP2; extern struct DelfObj DSP56K_MP3; #include #include "PCM.h" #include "MP2.h" #include "MP3.h" /** static LONG pow43tab[8206]; **/ #include "MP3_pow43tab.h" /** static LONG quantab[512]; **/ #include "MP3_quantab.h" extern char DevName; /* "delfinampeg.device" */ extern struct Library *DelfinaBase; /* initialized by device Init routine */ struct devunit { struct Unit unit; ULONG unitnum; struct List ioreqlist; struct IOMPEGReq *currioreq; UBYTE *currpt; ULONG currlen; ULONG volumeleft, volumeright; ULONG layer, freqidx, mono, firstheader; ULONG initdelf_ok, pause; struct DelfPrg *prg_pcm, *prg_mp2, *prg_mp3; struct DelfModule *mod_pcm; DELFPTR mem_il_mp2, mem_ip_mp2; DELFPTR mem_il_mp3, mem_ip_mp3, mem_x_mp3_ro, mem_y_mp3_quantab; struct Interrupt delfint; ULONG intkey; UBYTE framebuf[4096], bitresbuf[4096]; UWORD bitresoffset, bitresok, framebufstate; UWORD framebufoffset, framebufleft, III_main_data_size; UWORD delfcopysize, II_translate, II_jsbound, modext; UBYTE *delfcopypt; }; /* possible values for 'framebufstate' */ #define FBS_GETHEADER 0 #define FBS_GETFRAMEDATA 1 #define FBS_FILLED 2 #define MPG_MD_STEREO 0 #define MPG_MD_JOINT_STEREO 1 #define MPG_MD_DUAL_CHANNEL 2 #define MPG_MD_MONO 3 #define HDR_MPEG1 0xfff80000 #define HDR_CONSTANT 0xfffe0c00 /* layer, sampling frequency */ void* C_initunit(ULONG unitnum); ULONG C_expungeunit(struct devunit *unit); void C_setpause(struct devunit *unit, struct IOMPEGReq *iomr); void C_setvolume(struct devunit *unit, struct IOMPEGReq *iomr); void C_flush(struct devunit *unit); ULONG C_write(struct devunit *unit, struct IOMPEGReq *iomr); static ULONG initDelfina(struct devunit *unit); static void cleanupDelfina(struct devunit *unit); static void __asm IntServer(register __a1 struct devunit *unit); static UWORD GetBits(UWORD num); static ULONG volumeleft=0x400000, volumeright=0x400000; /* default: 100% */ static UWORD *gb_pt=NULL, gb_buf=0, gb_num=0; static const ULONG mpgfreq[4]={44100,48000,32000,0}; static const UWORD mpgbitrate[3][16]= { {0,32,64,96,128,160,192,224,256,288,320,352,384,416,448,0}, /* I */ {0,32,48,56,64,80,96,112,128,160,192,224,256,320,384,0}, /* II */ {0,32,40,48,56,64,80,96,112,128,160,192,224,256,320,0} }; /* III */ static const UWORD mp2translate[3][2][16] = { { { 0,2,2,2,2,2,2,0,0,0,1,1,1,1,1,0 } , /* 44100 stereo */ { 0,2,2,0,0,0,1,1,1,1,1,1,1,1,1,0 } } , /* 44100 mono */ { { 0,2,2,2,2,2,2,0,0,0,0,0,0,0,0,0 } , /* 48000 stereo */ { 0,2,2,0,0,0,0,0,0,0,0,0,0,0,0,0 } } , /* 48000 mono */ { { 0,3,3,3,3,3,3,0,0,0,1,1,1,1,1,0 } , /* 32000 stereo */ { 0,3,3,0,0,0,1,1,1,1,1,1,1,1,1,0 } } }; /* 32000 mono */ static const UWORD mp2sblimit[4]={27,30,8,12}; /** called by OpenDevice() **/ void* C_initunit(ULONG unitnum) { struct devunit *unit; #ifdef DEBUG KPutStr("C_initunit\n"); #endif unit=(struct devunit*)AllocVec(sizeof(struct devunit),MEMF_PUBLIC|MEMF_CLEAR); if(!unit) return(unit); unit->unit.unit_MsgPort.mp_Node.ln_Type=NT_MSGPORT; unit->unit.unit_MsgPort.mp_Node.ln_Name= &DevName; unit->unit.unit_MsgPort.mp_Flags=PA_IGNORE; NewList(&unit->unit.unit_MsgPort.mp_MsgList); unit->unitnum=unitnum; unit->volumeleft=volumeleft; unit->volumeright=volumeright; NewList(&unit->ioreqlist); return(unit); } /** called by CloseDevice() **/ ULONG C_expungeunit(struct devunit *unit) { ULONG unitnum=unit->unitnum; #ifdef DEBUG KPutStr("C_expungeunit\n"); #endif C_flush(unit); FreeVec(unit); return(unitnum); } /** called by MPEGCMD_PAUSE **/ void C_setpause(struct devunit *unit, struct IOMPEGReq *iomr) { #ifdef DEBUG KPutStr("C_setpause\n"); #endif unit->pause=iomr->iomr_Arg1; } /** called by MPEGCMD_SETAUDIOPARAMS **/ void C_setvolume(struct devunit *unit, struct IOMPEGReq *iomr) { struct MPEGAudioParams *ap; #ifdef DEBUG KPutStr("C_setvolume\n"); #endif ap=(struct MPEGAudioParams*)iomr->iomr_Req.io_Data; if(ap) { unit->volumeleft =volumeleft =(0x400000*((ap->map_VolumeLeft >>8)&0xff))/0xff; unit->volumeright=volumeright=(0x400000*((ap->map_VolumeRight>>8)&0xff))/0xff; if(unit->prg_pcm) { Delf_Run(unit->prg_pcm->prog+PROG_PCM_INIT, 0, 0, unit->mono, /*_forcemono_ is not implemented yet*/ unit->volumeleft, mpgfreq[unit->freqidx], unit->volumeright ); } } } /** called by CMD_FLUSH **/ void C_flush(struct devunit *unit) { struct IOMPEGReq *iomr; #ifdef DEBUG KPutStr("C_flush\n"); #endif cleanupDelfina(unit); unit->currlen=0; unit->currpt=NULL; unit->layer=unit->freqidx=unit->mono=unit->firstheader=0; unit->bitresoffset=unit->bitresok=0; unit->framebufstate=FBS_GETHEADER; if(unit->currioreq) { unit->currioreq->iomr_Req.io_Error=IOERR_ABORTED; ReplyMsg(&unit->currioreq->iomr_Req.io_Message); unit->currioreq=NULL; } for(;;) { if(!(iomr=(struct IOMPEGReq*)RemHead(&unit->ioreqlist))) break; iomr->iomr_Req.io_Error=IOERR_ABORTED; ReplyMsg(&iomr->iomr_Req.io_Message); } } /** called by CMD_WRITE **/ ULONG C_write(struct devunit *unit, struct IOMPEGReq *iomr) { #ifdef DEBUG /*KPutStr("C_write\n");*/ #endif if( (iomr->iomr_Req.io_Length==0) || (iomr->iomr_Req.io_Data==NULL) || (iomr->iomr_StreamType!=MPEGSTREAM_AUDIO) ) return(1); /*ERROR*/ Disable(); AddTail(&unit->ioreqlist, (struct Node*)iomr); Enable(); #ifdef DEBUG KPutStr("C_write...AddTail\n"); #endif /*even if initDelfina() returns an error we must not return it */ /*here because the request is already queued in our ioreqlist. */ /*(the requests in ioreqlist will be replied by C_flush() later)*/ if(!unit->initdelf_ok) initDelfina(unit); return(0); } /** called by C_write() **/ static ULONG initDelfina(struct devunit *unit) { while(!unit->firstheader) { ULONG header, i; UBYTE *pt, *ptmax; #ifdef DEBUG KPutStr("initDelfina...firstheader\n"); #endif /** find frame header and extract some info: layer, freqidx, mono **/ if(!unit->currioreq) { if(!(unit->currioreq=(struct IOMPEGReq*)RemHead(&unit->ioreqlist))) return(1); /*ERROR*/ unit->currpt=unit->currioreq->iomr_Req.io_Data; unit->currlen=unit->currioreq->iomr_Req.io_Length; } pt=unit->currpt; ptmax=pt+unit->currlen; header=0; retry_firstheader: i=0; while(pt>17)&3)!=0) && /*layer!=4*/ (((header>>17)&3)!=3) && /*layer!=1*/ (((header>>12)&15)!=0) && /*bitrate!=0*/ (((header>>12)&15)!=15) && /*bitrate!=15*/ (((header>>10)&3)!=3) /*freqidx!=3*/ ) {i=1; break;} /*pattern match*/ } if(i) /*found something!*/ { /**check next header (for safer recognition!)**/ i=((ULONG)mpgbitrate[3-((header>>17)&3)][(header>>12)&15]*144000)/mpgfreq[(header>>10)&3]+((header>>9)&1)-4; if((pt+i)>=(ptmax-4)) goto retry_firstheader; /*beyond this buffer!*/ i=((ULONG)(*(pt+i))<<24)|((ULONG)(*(pt+i+1))<<16)|((ULONG)(*(pt+i+2))<<8)|(ULONG)(*(pt+i+3)); if( ((i&HDR_CONSTANT)!=(header&HDR_CONSTANT)) || (((i>>12)&15)==0) || (((i>>12)&15)==15) ) goto retry_firstheader; /*header mismatch!*/ /**now we are quite sure that it really is an MPEG frame header**/ unit->currpt=pt-4; unit->currlen=ptmax-pt+4; unit->firstheader=header; unit->layer=4-((header>>17)&3); unit->freqidx=(header>>10)&3; unit->mono=( (((header>>6)&3)==MPG_MD_MONO) ? 1 : 0 ); } else /*not found*/ { ReplyMsg(&unit->currioreq->iomr_Req.io_Message); unit->currioreq=NULL; unit->currpt=NULL; unit->currlen=0; } } if(!unit->prg_pcm) { #ifdef DEBUG KPutStr("initDelfina...prg_pcm\n"); #endif if(!(unit->prg_pcm=Delf_AddPrg(&DSP56K_PCM))) return(1); /*ERROR*/ Delf_Run(unit->prg_pcm->prog+PROG_PCM_INIT, 0, 0, unit->mono, /*_forcemono_ is not implemented yet*/ unit->volumeleft, mpgfreq[unit->freqidx], unit->volumeright ); } if(unit->layer==2) { if(!unit->mem_il_mp2) unit->mem_il_mp2=Delf_AllocMem(INTL_MP2_DATL, DMEMF_LDATA|DMEMF_INTERNAL|DMEMF_ALIGN_64); if(!unit->mem_ip_mp2) unit->mem_ip_mp2=Delf_AllocMem(INTP_MP2_PROG, DMEMF_PROG|DMEMF_INTERNAL); if(!unit->prg_mp2) { #ifdef DEBUG KPutStr("initDelfina...prg_mp2\n"); #endif if(!(unit->prg_mp2=Delf_AddPrg(&DSP56K_MP2))) return(1); /*ERROR*/ Delf_Run(unit->prg_mp2->prog+PROG_MP2_INIT, 0, 0, unit->mem_il_mp2, unit->mem_ip_mp2, 0, 0); } } else if(unit->layer==3) { if(!unit->mem_il_mp3) unit->mem_il_mp3=Delf_AllocMem(INTL_MP3_DATL, DMEMF_LDATA|DMEMF_INTERNAL|DMEMF_ALIGN_64); if(!unit->mem_ip_mp3) unit->mem_ip_mp3=Delf_AllocMem(INTP_MP3_PROG, DMEMF_PROG|DMEMF_INTERNAL); if(!unit->mem_x_mp3_ro) { if(!(unit->mem_x_mp3_ro=Delf_AllocMem(EXTX_MP3_RO,DMEMF_XDATA))) return(1); /*ERROR*/ } if(!unit->mem_y_mp3_quantab) { if(!(unit->mem_y_mp3_quantab=Delf_AllocMem(EXTY_MP3_QUANTAB,DMEMF_YDATA))) return(1); /*ERROR*/ Delf_CopyMem(quantab, (void*)unit->mem_y_mp3_quantab, 512*4, DCPF_FROM_AMY|DCPF_YDATA|DCPF_32BIT); } if(!unit->prg_mp3) { #ifdef DEBUG KPutStr("initDelfina...prg_mp3\n"); #endif if(!(unit->prg_mp3=Delf_AddPrg(&DSP56K_MP3))) return(1); /*ERROR*/ Delf_Run(unit->prg_mp3->prog+PROG_MP3_INIT, 0, 0, unit->mem_il_mp3, unit->mem_ip_mp3, unit->freqidx, unit->mem_x_mp3_ro); Delf_CopyMem(pow43tab, (void*)(unit->prg_mp3->ydata+DATY_MP3_POW43TAB), 8206*4, DCPF_FROM_AMY|DCPF_YDATA|DCPF_32BIT); Delf_Poke(unit->prg_mp3->xdata+DATX_MP3_QUANTAB_P, DMEMF_XDATA, unit->mem_y_mp3_quantab); } } else return(1); /*ERROR: unsupported layer*/ if(!unit->intkey) { #ifdef DEBUG KPutStr("initDelfina...AddIntServer\n"); #endif unit->delfint.is_Code=(void(*)(void))IntServer; unit->delfint.is_Data=unit; if(!(unit->intkey=Delf_AddIntServer(unit->prg_pcm->prog+PROG_PCM_INTKEY,&unit->delfint))) return(1); /*ERROR*/ } if(!unit->mod_pcm) { #ifdef DEBUG KPutStr("initDelfina...AddModule\n"); #endif if(!(unit->mod_pcm=Delf_AddModule(DM_Inputs, 0, DM_Outputs, 1, DM_Code, unit->prg_pcm->prog+PROG_PCM_MODULE, DM_Freq, mpgfreq[unit->freqidx], /*_dacrate_ is not implemented yet*/ DM_Name, &DevName, 0 ))) return(1); /*ERROR*/ } unit->initdelf_ok=1; #ifdef DEBUG KPutStr("initDelfina...ok\n"); #endif return(0); } /** called by C_flush() **/ static void cleanupDelfina(struct devunit *unit) { #ifdef DEBUG KPutStr("cleanupDelfina\n"); #endif if(unit->mod_pcm) {Delf_RemModule(unit->mod_pcm); unit->mod_pcm=NULL;} if(unit->intkey) {Delf_RemIntServer(unit->intkey); unit->intkey=0;} if(unit->prg_mp2) {Delf_RemPrg(unit->prg_mp2); unit->prg_mp2=NULL;} if(unit->prg_mp3) {Delf_RemPrg(unit->prg_mp3); unit->prg_mp3=NULL;} if(unit->prg_pcm) {Delf_RemPrg(unit->prg_pcm); unit->prg_pcm=NULL;} if(unit->mem_il_mp2) {Delf_FreeMem(unit->mem_il_mp2,DMEMF_LDATA|DMEMF_INTERNAL); unit->mem_il_mp2=NULL;} if(unit->mem_ip_mp2) {Delf_FreeMem(unit->mem_ip_mp2,DMEMF_PROG |DMEMF_INTERNAL); unit->mem_ip_mp2=NULL;} if(unit->mem_il_mp3) {Delf_FreeMem(unit->mem_il_mp3,DMEMF_LDATA|DMEMF_INTERNAL); unit->mem_il_mp3=NULL;} if(unit->mem_ip_mp3) {Delf_FreeMem(unit->mem_ip_mp3,DMEMF_PROG |DMEMF_INTERNAL); unit->mem_ip_mp3=NULL;} if(unit->mem_x_mp3_ro) {Delf_FreeMem(unit->mem_x_mp3_ro,DMEMF_XDATA); unit->mem_x_mp3_ro=NULL;} if(unit->mem_y_mp3_quantab) {Delf_FreeMem(unit->mem_y_mp3_quantab,DMEMF_YDATA); unit->mem_y_mp3_quantab=NULL;} unit->initdelf_ok=0; } static void __asm IntServer(register __a1 struct devunit *unit) { if(unit->pause || (unit->framebufstate!=FBS_FILLED)) { Delf_Run(unit->prg_pcm->prog+PROG_PCM_MUTE,0,DRUNF_ASYNCH,0,0,0,0); } else { if(unit->layer==2) { if(!Delf_Peek(unit->prg_mp2->ydata+DATY_MP2_BUSY,DMEMF_YDATA)) { Delf_CopyMem( unit->delfcopypt, (void*)(unit->prg_mp2->xdata+DATX_MP2_INBUF), (ULONG)unit->delfcopysize, DCPF_FROM_AMY|DCPF_XDATA|DCPF_24BIT ); Delf_Run( unit->prg_mp2->prog+PROG_MP2_DECODE, 0, DRUNF_ASYNCH, unit->mono, Delf_Peek(unit->prg_pcm->ydata+DATY_PCM_BUFPTR,DMEMF_YDATA), (ULONG)unit->II_translate, (ULONG)unit->II_jsbound ); unit->framebufstate=FBS_GETHEADER; } } else if(unit->layer==3) { if(!Delf_Peek(unit->prg_mp3->ydata+DATY_MP3_BUSY,DMEMF_YDATA)) { if(unit->bitresok) { Delf_CopyMem( &unit->bitresbuf[0], (void*)(unit->prg_mp3->xdata+DATX_MP3_INBUF), (ULONG)(unit->III_main_data_size+33), DCPF_FROM_AMY|DCPF_XDATA|DCPF_24BIT ); Delf_Run( unit->prg_mp3->prog+PROG_MP3_DECODE, 0, DRUNF_ASYNCH, unit->mono, Delf_Peek(unit->prg_pcm->ydata+DATY_PCM_BUFPTR,DMEMF_YDATA), (ULONG)unit->modext, 0 ); } unit->framebufstate=FBS_GETHEADER; } } } if(unit->framebufstate==FBS_GETHEADER) { /*find next valid frame header*/ ULONG fh=unit->firstheader&HDR_CONSTANT, ch=0; while( ((ch&HDR_CONSTANT)!=fh) || /*sync, layer, freqidx*/ (((ch>>12)&15)==0) || /*bitrate!=0*/ (((ch>>12)&15)==15) ) /*bitrate!=15*/ { if(unit->currioreq) { if(unit->currlen) { ch=(ch<<8)|(ULONG)(*unit->currpt); unit->currpt++; unit->currlen--; } else { ReplyMsg(&unit->currioreq->iomr_Req.io_Message); unit->currioreq=NULL; unit->currpt=NULL; /*unit->currlen=0;*/ } } else { if(!(unit->currioreq=(struct IOMPEGReq*)RemHead(&unit->ioreqlist))) { ch=0; break; /*ERROR*/ } unit->currpt=unit->currioreq->iomr_Req.io_Data; unit->currlen=unit->currioreq->iomr_Req.io_Length; } } if(ch) /*found it!*/ { unit->framebufoffset=0; unit->framebufleft=((ULONG)mpgbitrate[unit->layer-1][(ch>>12)&15]*144000)/mpgfreq[(ch>>10)&3]+((ch>>9)&1)-4; unit->II_translate=mp2translate[unit->freqidx][unit->mono][(ch>>12)&15]; if(((ch>>6)&3)==MPG_MD_JOINT_STEREO) { unit->modext=(ch>>4)&3; unit->II_jsbound=(unit->modext<<2)+4; } else { unit->modext=0; unit->II_jsbound=mp2sblimit[unit->II_translate]; } unit->delfcopysize=unit->framebufleft; unit->delfcopypt= &unit->framebuf[0]; if(!((ch>>16)&1)) { unit->delfcopysize-=2; unit->delfcopypt+=2; } unit->framebufstate=FBS_GETFRAMEDATA; } } if(unit->framebufstate==FBS_GETFRAMEDATA) { /*fetch frame data*/ #ifdef DEBUG KPutChar('i'); #endif while(unit->framebufleft) { if(unit->currioreq) { if(unit->currlen) { ULONG i; if(unit->framebufleftcurrlen) i=unit->framebufleft; else i=unit->currlen; CopyMem(unit->currpt, &unit->framebuf[unit->framebufoffset], i); unit->currpt+=i; unit->currlen-=i; unit->framebufoffset+=i; unit->framebufleft-=i; if(unit->framebufleft==0) unit->framebufstate=FBS_FILLED; } else { ReplyMsg(&unit->currioreq->iomr_Req.io_Message); unit->currioreq=NULL; unit->currpt=NULL; /*unit->currlen=0;*/ } } else { if(!(unit->currioreq=(struct IOMPEGReq*)RemHead(&unit->ioreqlist))) { break; /*ERROR*/ } unit->currpt=unit->currioreq->iomr_Req.io_Data; unit->currlen=unit->currioreq->iomr_Req.io_Length; } } if((unit->framebufstate==FBS_FILLED) && (unit->layer==3)) { UWORD md; /**extract main data size**/ gb_pt=(UWORD*)(unit->delfcopypt+2); gb_num=0; if(unit->mono) { GetBits(9+5+4-16); md=GetBits(12); } else { GetBits(9+3+8-16); md=GetBits(12); GetBits(59-12); md+=GetBits(12); GetBits(59-12); md+=GetBits(12); } GetBits(59-12); md+=GetBits(12); unit->III_main_data_size=(md+7)>>3; /**bit reservoir handling**/ { UWORD main_data_begin, i; UBYTE *p0, *p1; p1=unit->delfcopypt; main_data_begin= (UWORD)(*p1)<<1 | (UWORD)(*(p1+1))>>7; /* copy side information */ p0= &unit->bitresbuf[0]; for(i=32; i>0; i--) *p0++= *p1++; /* copy previous main_data */ p0++; /* &unit->bitresbuf[33] */ if(unit->bitresoffset>=main_data_begin) { unit->bitresok=1; p1=p0+unit->bitresoffset-main_data_begin; for(i=main_data_begin; i>0; i--) *p0++= *p1++; unit->bitresoffset=main_data_begin; } else { unit->bitresok=0; /* not enough data in reservoir */ p0+=unit->bitresoffset; } /* copy current main_data */ i=(unit->mono) ? 17 : 32; /* side info length */ p1=unit->delfcopypt+i; /* begin of main_data area */ i=unit->delfcopysize-i; /* mainslots */ unit->bitresoffset+=i; for(; i>0; i--) *p0++= *p1++; } } } } static UWORD GetBits(UWORD num) { UWORD val=0; for(; num>0; num--) { if(gb_num==0) { gb_buf= *gb_pt++; gb_num= 16; } gb_num--; val+= val; val|= (gb_buf>>gb_num)&1; } return(val); }