adpcm.c

betaplayer的源码 tcpmp的老版本

/*****************************************************************************
 *
 * 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
 *
 * $Id: adpcm.c 11 2004-07-15 13:46:20Z picard $
 *
 * BetaPlayer Core
 * Copyright (c) 2004 Gabor Kovacs
 *
 ****************************************************************************/

#include "..\common.h"
#include "adpcm.h"
#include "g726\g72x.h"

#define ADPCM_INPUT		0x100
#define ADPCM_OUTPUT	0x101
 
typedef struct status 
{
    int Predictor;
    int StepIndex;
    int Step;
    int Sample1;
    int Sample2;
    int CoEff1;
    int CoEff2;
    int IDelta;

} status;

typedef struct adpcm
{
	node VMT;

	int Id;
	int Format;

	audio InputFormat;
	audio OutputFormat;
	pin Output;
	pin Comments;
	merge Merge;

	// output data
	packet Packet;
	tick_t RefTime;
	bool_t Pending;
	int Total;

	int Channel; //IMA_QT
	int16_t* Buffer;
	status Status[2];
	g726_state G726[2];

} adpcm;

const datadef ADPCMParams[] = 
{
	{ ADPCM_ID,		TYPE_CLASS },
	{ ADPCM_INPUT,	TYPE_PACKET, DF_INPUT, PACKET_AUDIO },
	{ ADPCM_OUTPUT,	TYPE_PIN, DF_OUTPUT, PACKET_AUDIO },

	DATADEF_END
};

_CONST int IndexTable[16] = 
{
    -1, -1, -1, -1, 2, 4, 6, 8,
    -1, -1, -1, -1, 2, 4, 6, 8,
};

_CONST int StepTable[89] = 
{
    7, 8, 9, 10, 11, 12, 13, 14, 16, 17,
    19, 21, 23, 25, 28, 31, 34, 37, 41, 45,
    50, 55, 60, 66, 73, 80, 88, 97, 107, 118,
    130, 143, 157, 173, 190, 209, 230, 253, 279, 307,
    337, 371, 408, 449, 494, 544, 598, 658, 724, 796,
    876, 963, 1060, 1166, 1282, 1411, 1552, 1707, 1878, 2066,
    2272, 2499, 2749, 3024, 3327, 3660, 4026, 4428, 4871, 5358,
    5894, 6484, 7132, 7845, 8630, 9493, 10442, 11487, 12635, 13899,
    15289, 16818, 18500, 20350, 22385, 24623, 27086, 29794, 32767
};

// AdaptationTable[], AdaptCoeff1[], and AdaptCoeff2[] are from libsndfile
_CONST int AdaptationTable[] = 
{
	230, 230, 230, 230, 307, 409, 512, 614,
	768, 614, 512, 409, 307, 230, 230, 230
};
_CONST int AdaptCoeff1[] = 
{
	256, 512, 0, 192, 240, 460, 392
};
_CONST int AdaptCoeff2[] = 
{
	0, -256, 0, 64, 0, -208, -232
};

static int SetFormat( adpcm* p, const audio* Format )
{
	memset(&p->InputFormat,0,sizeof(p->InputFormat));
	MergeDone(&p->Merge);
	Free(p->Buffer);
	p->Buffer = NULL;

	if (Format)
	{
		if (Format->Format != p->Format)
			return ERR_INVALID_DATA;

		p->InputFormat = *Format;

		p->OutputFormat.Flags = 0;
		p->OutputFormat.Format = AUDIOFMT_PCM;
		p->OutputFormat.Bits = 16;
		p->OutputFormat.FracBits = 15;
		p->OutputFormat.SampleRate = p->InputFormat.SampleRate;
		p->OutputFormat.Channels = p->InputFormat.Channels;

		p->Pending = 0;
		p->RefTime = 0;
		p->Total = 0;
		MergeInit(&p->Merge);

		if (!p->InputFormat.BlockAlign)
			p->InputFormat.BlockAlign = 1024;

		if (p->Id == ADPCM_IMA_QT_ID)
			p->InputFormat.BlockAlign = (32+2)*p->InputFormat.Channels;

		if (p->Id == ADPCM_G726_ID)
		{
			p->InputFormat.BlockAlign = 120;
			g726_init_state(&p->G726[0]);
			g726_init_state(&p->G726[1]);
		}

		p->Buffer = (int16_t*) Alloc( sizeof(int16_t)*4*p->InputFormat.BlockAlign );
	}

	return ERR_NONE;
}

static _INLINE int IMA_Calc(status* s, int v)
{
    int StepIndex;
    int Predictor;
    int Diff,Step;

    Step = StepTable[s->StepIndex];
    StepIndex = s->StepIndex + IndexTable[v];

    if (StepIndex < 0) 
		StepIndex = 0;
    else if (StepIndex > 88) 
		StepIndex = 88;

    Diff = ((2 * (v & 7) + 1) * Step) >> 3;

    Predictor = s->Predictor;
    if (v & 8) 
		Predictor -= Diff;
    else 
		Predictor += Diff;

	if (Predictor > 32767) 
		Predictor = 32767;
	else if (Predictor < -32768) 
		Predictor = -32768;

    s->Predictor = Predictor;
    s->StepIndex = StepIndex;

    return Predictor;
}

static _INLINE int MS_Calc(status* s, int v)
{
    int Predictor;

    Predictor = ((s->Sample1 * s->CoEff1) + (s->Sample2 * s->CoEff2)) >> 8;
    Predictor += ((v & 0x08) ? v-0x10:v) * s->IDelta;

	if (Predictor > 32767) 
		Predictor = 32767;
	else if (Predictor < -32768) 
		Predictor = -32768;

    s->Sample2 = s->Sample1;
    s->Sample1 = Predictor;
    s->IDelta = (AdaptationTable[v] * s->IDelta) >> 8;
    if (s->IDelta < 16) 
		s->IDelta = 16;

    return Predictor;
}

static bool_t DecodeFrame( adpcm* p )
{
	int i;
	int Predictor;
	const uint8_t* In;
	const uint8_t* InEnd;
	int16_t* Out = p->Buffer;

	if (!MergeRead(&p->Merge,&In,p->InputFormat.BlockAlign))
		return 0;

	InEnd = In + p->InputFormat.BlockAlign;

	switch (p->Format)
	{
	case AUDIOFMT_ADPCM_G726:
	{
		g726_state *g1,*g2;
		g1 = g2 = &p->G726[0];
		if (p->InputFormat.Channels==2)
			++g2;

		switch (p->InputFormat.Bits)
		{
		case 2:
			for (;In<InEnd;++In,Out+=4)
			{
				Out[0] = (int16_t)g726_16_decoder(In[0] >> 6,g1);
				Out[1] = (int16_t)g726_16_decoder(In[0] >> 4,g2);
				Out[2] = (int16_t)g726_16_decoder(In[0] >> 2,g1);
				Out[3] = (int16_t)g726_16_decoder(In[0],g2);
			}
			break;
		case 3:
			InEnd -= 2;
			for (;In<InEnd;In+=3,Out+=8)
			{
				Out[0] = (int16_t)g726_24_decoder(In[0] >> 5,g1);
				Out[1] = (int16_t)g726_24_decoder(In[0] >> 2,g2);
				Out[2] = (int16_t)g726_24_decoder((In[0] << 1) | (In[1] >> 7),g1);
				Out[3] = (int16_t)g726_24_decoder(In[1] >> 4,g2);
				Out[4] = (int16_t)g726_24_decoder(In[1] >> 1,g1);
				Out[5] = (int16_t)g726_24_decoder((In[1] << 2) | (In[2] >> 6),g2);
				Out[6] = (int16_t)g726_24_decoder(In[2] >> 3,g1);
				Out[7] = (int16_t)g726_24_decoder(In[2] >> 0,g2);
			}
			break;
		case 4:
			for (;In<InEnd;++In,Out+=2)
			{
				Out[0] = (int16_t)g726_32_decoder(In[0] >> 4,g1);
				Out[1] = (int16_t)g726_32_decoder(In[0],g2);
			}
			break;
		case 5:
			InEnd -= 4;
			for (;In<InEnd;In+=5,Out+=8)
			{
				Out[0] = (int16_t)g726_40_decoder(In[0] >> 3,g1);
				Out[1] = (int16_t)g726_40_decoder((In[0] << 2) | (In[1] >> 6),g2);
				Out[2] = (int16_t)g726_40_decoder(In[1] >> 1,g1);
				Out[3] = (int16_t)g726_40_decoder((In[1] << 4) | (In[2] >> 4),g2);
				Out[4] = (int16_t)g726_40_decoder((In[2] << 1) | (In[3] >> 7),g1);
				Out[5] = (int16_t)g726_40_decoder(In[3] >> 2,g2);
				Out[6] = (int16_t)g726_40_decoder((In[3] << 3) | (In[4] >> 5),g1);
				Out[7] = (int16_t)g726_40_decoder(In[4] >> 0,g2);
			}
			break;
		}
		break;
	}
	case AUDIOFMT_ADPCM_IMA_QT:
	{
		int No,Ch;
		Ch = p->InputFormat.Channels;
		for (No=0;No<Ch;++No)
		{
			status *s;
			s = &p->Status[Ch];
			s->Predictor = (int16_t)((In[1] & 0x80) | (In[0] << 8));

			s->StepIndex = In[1] & 0x7F;
			if (s->StepIndex > 88) 
				s->StepIndex = 88;

			In+=2;
			InEnd=In+32;
			Out = p->Buffer+No;

			for (;In<InEnd;++In)
			{
				*Out = (int16_t)IMA_Calc(s, In[0] & 0x0F);
				Out+=Ch;
				*Out = (int16_t)IMA_Calc(s, In[0] >> 4);
				Out+=Ch;
			}
		}
		break;
	}

	case AUDIOFMT_ADPCM_IMA:
	{
		status *s1,*s2;
		s1 = &p->Status[0];
		s1->Predictor = (int16_t)(In[0] | (In[1] << 8));
		In+=2;

		s1->StepIndex = *In++;
		if (s1->StepIndex > 88) 
			s1->StepIndex = 88;
		++In;

		if (p->InputFormat.Channels == 2)
		{
			s2 = &p->Status[1];
			s2->Predictor = (int16_t)(In[0] | (In[1] << 8));
			In+=2;

			s2->StepIndex = *In++;
			if (s2->StepIndex > 88) 
				s2->StepIndex = 88;
			++In;

			for (i=4;In<InEnd;++In,Out+=4)
			{
				Out[0] = (int16_t)IMA_Calc(s1, In[0] & 0x0F);
				Out[1] = (int16_t)IMA_Calc(s2, In[4] & 0x0F);
				Out[2] = (int16_t)IMA_Calc(s1, In[0] >> 4);
				Out[3] = (int16_t)IMA_Calc(s2, In[4] >> 4);

				if (--i==0)
				{
					i=4;
					In+=4;
				}
			}
		}
		else
		{
			for (;In<InEnd;++In,Out+=2)
			{
				Out[0] = (int16_t)IMA_Calc(s1, In[0] & 0x0F);
				Out[1] = (int16_t)IMA_Calc(s1, In[0] >> 4);
			}
		}
		break;
	}
	case AUDIOFMT_ADPCM_MS:
	{
		status *s1,*s2;
		s1 = &p->Status[0];
		s2 = p->InputFormat.Channels==2 ? &p->Status[1] : s1;

		Predictor = *In++;
		if (Predictor > 7)
			Predictor = 7;
		s1->CoEff1 = AdaptCoeff1[Predictor];
		s1->CoEff2 = AdaptCoeff2[Predictor];

		if (s2 != s1)
		{
			Predictor = *In++;
			if (Predictor > 7)
				Predictor = 7;
			s2->CoEff1 = AdaptCoeff1[Predictor];
			s2->CoEff2 = AdaptCoeff2[Predictor];
		}

		s1->IDelta = (int16_t)(In[0] | (In[1] << 8));
		In+=2;

		if (s2 != s1)
		{
			s2->IDelta = (int16_t)(In[0] | (In[1] << 8));
			In+=2;
		}

		s1->Sample1 = (int16_t)(In[0] | (In[1] << 8));
		In+=2;
		if (s2 != s1)
		{
			s2->Sample1 = (int16_t)(In[0] | (In[1] << 8));
			In+=2;
		}

		s1->Sample2 = (int16_t)(In[0] | (In[1] << 8));
		In+=2;
		if (s2 != s1)
		{
			s2->Sample2 = (int16_t)(In[0] | (In[1] << 8));
			In+=2;
		}

		*Out++ = (int16_t)s1->Sample1;
		if (s2 != s1) *Out++ = (int16_t)s2->Sample1;

		*Out++ = (int16_t)s1->Sample2;
		if (s2 != s1) *Out++ = (int16_t)s2->Sample2;

		for (;In<InEnd;++In,Out+=2)
		{
			Out[0] = (int16_t)MS_Calc(s1, In[0] >> 4);
			Out[1] = (int16_t)MS_Calc(s2, In[0] & 0x0F);
		}
		break;
	}
	}

	// build output packet
	p->Packet.RefTime = p->RefTime;
	p->Packet.Length = (uint8_t*)Out - (uint8_t*)p->Buffer;
	p->Packet.Data[0] = p->Buffer;
	p->RefTime = -1;

	if (p->InputFormat.Channels==2)
		p->Total += p->Packet.Length >> 2; //2x16bit per sample
	else
		p->Total += p->Packet.Length >> 1; //16bit per sample
	return 1;
}

static int Decode( adpcm* p, const packet* Packet )
{
	int Result = ERR_NONE;

	p->Packet.CurrTime = Packet->CurrTime;
	p->Packet.DropLevel = Packet->DropLevel;

	if (p->Pending)
	{
		do
		{
			if (p->Output.Node->Set(p->Output.Node,p->Output.No,
					&p->Packet,sizeof(packet)) == ERR_BUFFER_FULL)
				return ERR_BUFFER_FULL;
		}
		while (DecodeFrame(p));

		p->Pending = 0;
	}

	MergePack(&p->Merge,0);
	MergeWrite(&p->Merge,Packet->Data[0],Packet->Length);
	p->RefTime = Packet->RefTime;

	if (DecodeFrame(p))
	{
		Result = ERR_NONE;
		do
		{
			int i = p->Output.Node->Set(p->Output.Node,p->Output.No,
						 				 &p->Packet,sizeof(packet));
			if (i == ERR_BUFFER_FULL)
			{
				p->Pending = 1;
				//return last result
				break;
			}

			Result = i;
		}
		while (DecodeFrame(p));
	}
	else
		Result = ERR_NEED_MORE_DATA; // if no frame decoded: need more packet

	return Result;
}

static bool_t IsEmpty( adpcm* p )
{
	packet Zero;

	if (!p->InputFormat.Format) 
		return 1;

	Zero.CurrTime = -1;
	Zero.DropLevel = p->Packet.DropLevel;
	Zero.Length = 0;
	Zero.Data[0] = NULL;
	Zero.RefTime = -1;

	return Decode(p,&Zero) == ERR_NEED_MORE_DATA;
}

static int Enum( adpcm* p, int No, datadef* Param )
{
	return NodeEnumType(&No,Param,NodeParams,FlowParams,ADPCMParams,NULL);
}

static int Get( adpcm* p, int No, void* Data, int Size )
{
	int Result = ERR_INVALID_PARAM;
	switch (No)
	{
	case NODE_ID: GETVALUE(p->Id,int); break;
	case ADPCM_INPUT | PACKET_FORMAT: GETVALUE(p->InputFormat,audio); break;
	case ADPCM_OUTPUT | PACKET_FORMAT: GETVALUE(p->OutputFormat,audio); break;
	case ADPCM_OUTPUT: GETVALUE(p->Output,pin); break;
	case FLOW_TOTAL:GETVALUE(p->Total,int); break;
	case FLOW_EMPTY:GETVALUE(IsEmpty(p),bool_t); break;
	}
	return Result;
}

static int Set( adpcm* p, int No, const void* Data, int Size )
{
	int Result = ERR_INVALID_PARAM;
	switch (No)
	{
	case ADPCM_INPUT | PACKET_FORMAT:
		if (!Size || Size == sizeof(audio))
		{
			if (!Size) Data = NULL;
			Result = SetFormat(p,(const audio*)Data);
		}
		break;

	case ADPCM_INPUT:
		if (Size == sizeof(packet))
			Result = Decode(p,(const packet*)Data);
		break;

	case ADPCM_OUTPUT:
		if (Size == sizeof(pin))
			Result = NodeSetPin(&p->Output,(pin*)Data,&p->OutputFormat,sizeof(audio));
		break;

	case FLOW_TOTAL: SETVALUE(p->Total,int,ERR_NONE); break;
	case FLOW_EMPTY:

		if (p->Id == ADPCM_G726_ID)
		{
			g726_init_state(&p->G726[0]);
			g726_init_state(&p->G726[1]);
		}
		MergeClear(&p->Merge);
		p->Pending = 0;
		p->Channel = 0;
		break;
	}
	return Result;
}

static adpcm ADPCM_MS;
static adpcm ADPCM_IMA;
static adpcm ADPCM_IMA_QT;
static adpcm ADPCM_G726;

void ADPCM_Init()
{
	NodeFill(&ADPCM_MS.VMT,sizeof(adpcm),(nodeenum)Enum,(nodeget)Get,(nodeset)Set);
	ADPCM_MS.Id = ADPCM_MS_ID;
	ADPCM_MS.Format = AUDIOFMT_ADPCM_MS;
	NodeRegister(&ADPCM_MS.VMT,PRI_DEFAULT);

	NodeFill(&ADPCM_IMA.VMT,sizeof(adpcm),(nodeenum)Enum,(nodeget)Get,(nodeset)Set);
	ADPCM_IMA.Id = ADPCM_IMA_ID;
	ADPCM_IMA.Format = AUDIOFMT_ADPCM_IMA;
	NodeRegister(&ADPCM_IMA.VMT,PRI_DEFAULT);

	NodeFill(&ADPCM_IMA_QT.VMT,sizeof(adpcm),(nodeenum)Enum,(nodeget)Get,(nodeset)Set);
	ADPCM_IMA_QT.Id = ADPCM_IMA_QT_ID;
	ADPCM_IMA_QT.Format = AUDIOFMT_ADPCM_IMA_QT;
	NodeRegister(&ADPCM_IMA_QT.VMT,PRI_DEFAULT);

	NodeFill(&ADPCM_G726.VMT,sizeof(adpcm),(nodeenum)Enum,(nodeget)Get,(nodeset)Set);
	ADPCM_G726.Id = ADPCM_G726_ID;
	ADPCM_G726.Format = AUDIOFMT_ADPCM_G726;
	NodeRegister(&ADPCM_G726.VMT,PRI_DEFAULT);
}

void ADPCM_Done()
{
	NodeUnRegister(&ADPCM_MS.VMT);
	NodeUnRegister(&ADPCM_IMA.VMT);
	NodeUnRegister(&ADPCM_IMA_QT.VMT);
	NodeUnRegister(&ADPCM_G726.VMT);
}