dspoutrtp.cpp

KphoneSI (kpsi) is a SIP (Session Initiation Protocol) user agent for Linux, with which you can in

#include <sys/types.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <unistd.h>
#include <stdio.h>
#include <g711.h>
#include <qsettings.h>
#include <qtimer.h>
#include "../config.h"
#ifdef SPEEX
#include "../Speex/speex.h"
#include "../Speex/speex_define.h"
#endif
#include "../dissipate2/sipuri.h"
#include "../dissipate2/sipclient.h"
#include "audiobuffer.h"
#include "rtpdataheader.h"
#define SRTPWRAPPER_HX
#include "dspoutrtp.h"
#include "kstatics.h"

short ILBCencode_20( iLBC_Enc_Inst_t *iLBCenc_inst,short *encoded_data, short *data);
short ILBCdecode_20( iLBC_Dec_Inst_t *iLBCdec_inst,short *decoded_data, short *encoded_data, short mode);
short ILBCencode_30( iLBC_Enc_Inst_t *iLBCenc_inst,short *encoded_data, short *data);
short ILBCdecode_30( iLBC_Dec_Inst_t *iLBCdec_inst,short *decoded_data, short *encoded_data, short mode);

DspOutRtp::DspOutRtp( const codecType newCodec, int newCodecNum,  int minP, int maxP,const QString &hostName, UDPMessageSocket *s ){ 
	
	
	portnum=0;
	curseq=0;
	qlen=0;
	numunsend=0;
	fixedrtplen=160;
	deb_frag=0L;
	deb_rtp=0L;
	ssrc=10;
	ref_sec=0;
	ref_usec=0;
	dsize=4096;
	useStun=false;
	destroySocket=false;
	QSettings settings;
	sdebug =  KStatics::debugLevel;
        useSRTP=false;
	wrapper=0;
	if(settings.readEntry(KStatics::dBase+ "SRTP/Mode", "") != "disabled"){

#ifdef  SRTP
		useSRTP = true;
		wrapper = SRTPWrapper::getInstance();
		
		if(wrapper==0)	{
		    useSRTP=false;
		    if(sdebug >= 2) printf( "=====DspOutRtp Do not use SRTP");
		} else {
		    if(sdebug >= 2) printf( "=====DspOutRtp Use SRTP" );
		}
#endif		
	} else{
		if(sdebug >= 2) printf( "=====DspOutRtp Do not use SRTP" );
	}
	codec = newCodec;
	codecNum = newCodecNum;
	if(sdebug >= 2) printf( " codecNum=%d\n",codecNum );
	if (s) {
		destroySocket = false;
		socket = s;
	} else {
		socket = new UDPMessageSocket();
		destroySocket = true;
		if (socket == 0) {
			if(sdebug >= 2) printf( QObject::tr("=====DspOutRtp:: Can't create socket") + "\n" );
			abort();
		}
	}
	if( hostName != QString::null && (socket->setHostname( hostName.latin1() ) == 0) ) {
		if(sdebug >= 2) printf( QObject::tr("=====DspOutRtp::Hostname lookup failed") + "\n" );
	}
	dsize = 4096;  // initial max packet size
	packetbuf = new unsigned char[ sizeof( rtp_hdr_t ) + dsize ];
	dtmfbuf = new unsigned char[ 20];
	bufunsend = new unsigned char[ dsize ];
	if( !( gsmInstEnc = gsm_create() ) ) {
		if(sdebug >= 2) printf( "=====DspOutRtp::GSM_CREATE -error !\n" );
	}
	if( !( gsmInstDec = gsm_create() ) ) {
		if(sdebug >= 2) printf( "=====DspOutRtp::GSM_CREATE -error !\n" );
	}
	outbuf = new unsigned char[ dsize ];
	tmpbuf = new unsigned char[ dsize ];
	quebuf = new unsigned char[ dsize ];
	initEncode(&ilbcEncInst_20, 20 );
	initDecode(&ilbcDecInst_20, 20, 1);
	initEncode(&ilbcEncInst_30, 30 );
	initDecode(&ilbcDecInst_30, 30, 1);
#ifdef SPEEX
	initspeexEncode (&speexEncInst);
	initspeexDecode (&speexDecInst);
#endif
	audio_buf.resize( dsize * sizeof( short )  );
	setDeviceName("rtp");
	min=minP;
	max=maxP;
}

DspOutRtp::~DspOutRtp( void )
{
	if( gsmInstEnc ) gsm_destroy( gsmInstEnc );
	
	if( gsmInstDec ) gsm_destroy( gsmInstDec );
	
	delete[] static_cast<unsigned char *>( packetbuf );	
	delete[] static_cast<unsigned char *>( bufunsend );
	delete[] static_cast<unsigned char *>( outbuf );
	delete[] static_cast<unsigned char *>( tmpbuf );
	delete[] static_cast<unsigned char *>( quebuf ); 
	delete[] static_cast<unsigned char *>( dtmfbuf );
#ifdef SPEEX
	termspeexEncode (&speexEncInst);
	termspeexDecode (&speexDecInst);
#endif

	if (destroySocket) {
			delete socket;
			if(sdebug >= 2) printf(" ermordet");
			}
	if(sdebug >= 2) printf("\n");
#ifdef  SRTP
	if(useSRTP){
		if(!wrapper == 0){
			wrapper->dispose();
			wrapper->destroy();
		}
	}
#endif
}
bool DspOutRtp::openDevice( DeviceMode mode )
{
	devmode = mode;
	if( devmode == ReadOnly ) {
		socket->listenOnEvenPort(min, max);
		portnum = socket->getPortNumber();
		if(sdebug >= 2) printf("=====DspOutRtp::openDevice readonly minport=%d maxport=%d usedport=%d\n",min, max, portnum);
		SipUri stun;
		if( useStun ) {
		    stun = SipUri( stunSrv );
		    if( socket->setHostname( stun.getHostname() ) == 0 ) {
			return false;
		    }    
		    if ( socket->sendStunRequest(stun.getHostname(),stun.getPortNumber())==0 ) {
				portnum = socket->receiveStunResponse();
				if( portnum > 0 ) {
					socket->forcePortNumber( portnum );
					if(sdebug >= 2) printf("=====DspOutRtp::openDevice readonly %d\n",portnum);
					
				}
		    }
		}
	} else {
	socket->connect( portnum );
	socket->SetTOS();
	if(sdebug >= 2) printf("=====DspOutRtp::openDevice notreadonly %d\n",portnum);
	}
	devstate = DeviceOpened;
	return true;
}

#define PCMUCODEC 0
#define GSMCODEC 3
#define PCMACODEC 8
#define SPEEXCODEC 98
#define ILBCCODEC 97

typedef struct cb {
unsigned char (*func)( int frombuf );
} cb_t;

bool DspOutRtp::writeBuffer( void )
{
	short *s;
	short *frombuf;
	unsigned char *databuf;
	rtp_hdr_t *h = (rtp_hdr_t *) packetbuf;
	size_t fromsize;
	unsigned char *wlbuf;
	size_t bytesnew;
	size_t bytesgot;
	int tmpsize;
	unsigned int i;
	bytesnew = audio_buf.getSize() / sizeof( short );
	fromsize = audio_buf.getSize() / sizeof( short );
	inbuf = (unsigned char*)audio_buf.getData();
	frombuf = (short *)inbuf;
	if( codec == codecGSM ) {
		short *frombuf_test;
		frombuf_test = frombuf;
		s = (short *)inbuf;
		for( i=0; i<fromsize; ++i) {
			s[i] = (short)(*frombuf_test);
			++frombuf_test;
		}
		tmpsize = writeGSMBuffer( gsmInstEnc, inbuf, outbuf, tmpbuf, quebuf, &qlen, fromsize );
		h->version = 2;
		h->p = 0;
		h->x = 0;
		h->cc = 0;
		h->m = 0;
		h->pt = GSMCODEC;
		while( tmpsize > 0 ) {
			h->seq = htons( ++curseq );
			h->ts = htonl( ts );
			ts += GSM_DEC_SAMPLES;
			h->ssrc = ssrc;
			databuf = packetbuf + sizeof( rtp_hdr_t );
			for( int i = 0; i < tmpsize; ++i ) {
				databuf[ i ] = outbuf[ i ];
			}
			int length = sizeof( rtp_hdr_t ) + tmpsize;
#ifdef  SRTP

			if(useSRTP){
				wrapper->protect(packetbuf, &length);
			}
#endif
			if( socket->send( (char *) packetbuf, length) < 0 ) {
			
				if(sdebug >= 3) printf("=====DspOutRtp::writeBuffer: %s\n", strerror(errno));
				return false;
			}
			tmpsize = writeGSMBuffer( gsmInstEnc, inbuf, outbuf, tmpbuf, quebuf, &qlen, 0 );
		}
	}
	else if( codec == codecILBC_20 || codec == codecILBC_30 ) {
		short *frombuf_test;
		frombuf_test = frombuf;
		s = (short *)inbuf;
		for( i=0; i<fromsize; ++i) {
			s[i] = (short)(*frombuf_test);
			++frombuf_test;
		}
		if( codec == codecILBC_20 ) {
			tmpsize = writeILBCBuffer_20( &ilbcEncInst_20, inbuf, outbuf, tmpbuf, quebuf, &qlen, fromsize );
		} else {
			tmpsize = writeILBCBuffer_30( &ilbcEncInst_30, inbuf, outbuf, tmpbuf, quebuf, &qlen, fromsize );
		}
		h->version = 2;
		h->p = 0;
		h->x = 0;
		h->cc = 0;
		h->m = 0;
		h->pt = ILBCCODEC;
		while( tmpsize > 0 ) {
			h->seq = htons( ++curseq );
			h->ts = htonl( ts );
			if( codec == codecILBC_20 ) {
				ts += BLOCKL_20MS;
			} else {
				ts += BLOCKL_30MS;
			}
			h->ssrc = ssrc;
			databuf = packetbuf + sizeof( rtp_hdr_t );
			for( int i = 0; i < tmpsize; ++i ) {
				databuf[ i ] = outbuf[ i ];
			}
			int length = sizeof( rtp_hdr_t ) + tmpsize;
#ifdef  SRTP

			if(useSRTP){
				wrapper->protect(packetbuf, &length);
			}
#endif
			if( socket->send( (char *) packetbuf, length) < 0 ) {
				if(sdebug >= 3) printf("=====DspOutRtp::writeBuffer: %s\n", strerror(errno));
				return false;
			}
			if( codec == codecILBC_20 ) {
				tmpsize = writeILBCBuffer_20( &ilbcEncInst_20, inbuf, outbuf, tmpbuf, quebuf, &qlen, 0 );
			} else {
				tmpsize = writeILBCBuffer_30( &ilbcEncInst_30, inbuf, outbuf, tmpbuf, quebuf, &qlen, 0 );
			}
		}
#ifdef SPEEX
	} else if( codec == codecSpeex ) {
		short *frombuf_test;
		frombuf_test = frombuf;
		int available_payload_size;
		tmpsize = writespeexBuffer( &speexEncInst, inbuf, outbuf, tmpbuf, quebuf, &qlen, fromsize );
		while( tmpsize > 0 ) {
			available_payload_size = fixedrtplen;
			h->version = 2;
			h->p = 0;
			h->x = 0;
			h->cc = 0;
			h->m = 0;
			h->pt = SPEEXCODEC;
			h->seq = htons( curseq++ );
			h->ts = htonl( ts );
			h->ssrc = ssrc;
			databuf = packetbuf + sizeof( rtp_hdr_t );

			int j = 0;
			while (tmpsize > 0) {

				for( int i = 0; i < tmpsize; i++ ) {
					databuf[ j ] = outbuf[ i ];
					j++;
				}
				ts += FRAME_SIZE;
				available_payload_size -= FRAME_SIZE;
				if (available_payload_size >= FRAME_SIZE)
					tmpsize = writespeexBuffer( &speexEncInst, inbuf, outbuf, tmpbuf, quebuf, &qlen, 0 );
				else
					tmpsize = 0;
			}


			int length = sizeof( rtp_hdr_t ) + j;// tmpsize;
#ifdef  SRTP

			if(useSRTP){
				wrapper->protect(packetbuf, &length);
			}
#endif
			if( socket->send( (char *) packetbuf, length) < 0 ) {
				
				if(sdebug >= 3) printf( "=====DspOutRtp::writeBuffer: %s\n", strerror(errno));
				return false;
			}
			tmpsize = writespeexBuffer( &speexEncInst, inbuf, outbuf, tmpbuf, quebuf, &qlen, 0 );
		}
#endif	
	} else if( codec == codecALAW || codec == codecULAW ) {
		cb_t callb;
		wlbuf = bufunsend;
		h->version = 2;
		h->p = 0;
		h->x = 0;
		h->cc = 0;
		h->m = 0;
		switch ( codec ) {
          case codecULAW:
			  h->pt = PCMUCODEC;
			  callb.func = &linear2ulaw;
			  break;
	  case codecALAW:
			  h->pt = PCMACODEC;
			  callb.func = &linear2pcma;
	  default:
			  break;
		}
		while(bytesnew+numunsend >= fixedrtplen){
			++deb_rtp;
			h->seq = htons( ++curseq );
			h->ts = htonl( ts );
			ts += fixedrtplen;
			h->ssrc = ssrc;
			databuf = packetbuf + sizeof( rtp_hdr_t );
			bytesgot = 0;
			while( numunsend > 0 ) {
				*databuf = *wlbuf;
				++databuf;
				++wlbuf;
				--numunsend;
				++bytesgot;
			}
			wlbuf = bufunsend;
			while( bytesgot < fixedrtplen ) {
				*databuf = callb.func( *frombuf );
				++databuf;
				++frombuf;
				--bytesnew;
				++bytesgot;
			}
			int length = sizeof( rtp_hdr_t ) + fixedrtplen;
#ifdef  SRTP

			if(useSRTP){
				wrapper->protect(packetbuf, &length);
			}
#endif

			if( socket->send( (char *) packetbuf, length) < 0 ) {
				
				if(sdebug >= 3) printf("=====DspOutRtp::writeBuffer: %s\n", strerror(errno));
				return false;
			}
		}
		if( bytesnew > 0 ) {
			wlbuf = bufunsend + numunsend;
			numunsend += bytesnew;
			while( bytesnew > 0 ){
				*wlbuf = callb.func( *frombuf );
				++wlbuf;
				++frombuf;
				--bytesnew;
			}
		}
		++deb_frag;
	}
	else {
       if(sdebug >= 3) printf("=====DspOutRtp::writeBuffer: unknown Codec %i\n",codec);
	   return false;
	}

	return true;
}

bool DspOutRtp::setPortNum( int newport )
{
	portnum = newport;
	return true;
}

unsigned int DspOutRtp::readableBytes( void )
{
	struct timeval timeout;
	fd_set read_fds;
	int highest_fd;
	timeout.tv_sec = 0;
	timeout.tv_usec = 0;
	FD_ZERO( &read_fds );
	FD_SET( socket->getFileDescriptor(), &read_fds );
	highest_fd = socket->getFileDescriptor() + 1;
retry:
	if ( select( highest_fd, &read_fds, NULL, NULL, &timeout ) == -1 ) {
		if ( errno == EINTR ) goto retry;
		perror( "=====DspOutRtp::doSelect(): select() punted" );
		exit( 1 );
	}
	if ( FD_ISSET( socket->getFileDescriptor(), &read_fds ) ) {
		return 1;
	}
	return 0;
}

bool DspOutRtp::readBuffer( int bytes )
{
	unsigned char *inbuf;
	short *outbuf;
	int recvsize;
	int i;
	int size;
	recvsize = socket->receive( (char *) packetbuf, sizeof( rtp_hdr_t ) + dsize );
	rtp_hdr_t *h = (rtp_hdr_t *) packetbuf;

#ifdef  SRTP
	if(useSRTP){
		int length = recvsize;
		
		wrapper->unprotect(packetbuf, &length);
		recvsize = length;
	}
#endif
	if( h->pt == PCMUCODEC ) {
		audio_buf.resize( ( recvsize - (int) sizeof( rtp_hdr_t ) ) * sizeof( short ) );
		outbuf = (short *) audio_buf.getData();
		inbuf = &packetbuf[ sizeof( rtp_hdr_t ) ];
		for( i = 0; i < recvsize - (int) sizeof( rtp_hdr_t ); ++i ) {
			*outbuf = (short) ulaw2linear( inbuf[ i ] );
			++outbuf;
		}
	} else if( h->pt == GSMCODEC ) {
		audio_buf.resize( GSM_DEC_SAMPLES * sizeof( short ) );
		outbuf = (short *) audio_buf.getData();
		inbuf = &packetbuf[ sizeof( rtp_hdr_t ) ];
		readGSMBuffer( gsmInstDec, inbuf, tmpbuf, false );
		short *s = (short *)tmpbuf;
		for( i = 0; i < GSM_DEC_SAMPLES; ++i) {
			*outbuf = (short)(s[i]);
			++outbuf;
		}
	} else if( h->pt == PCMACODEC ) {
		audio_buf.resize( ( recvsize - (int) sizeof( rtp_hdr_t ) ) * sizeof( short ) );
		outbuf = (short *) audio_buf.getData();
		inbuf = &packetbuf[ sizeof( rtp_hdr_t ) ];
		for( i = 0; i < recvsize - (int) sizeof( rtp_hdr_t ); ++i ) {
			*outbuf = (short) pcma2linear( inbuf[ i ] );
			++outbuf;
		}
	} else if( codec == codecILBC_20 && (int)h->pt == ILBCCODEC ) {
		audio_buf.resize( BLOCKL_20MS * sizeof( short ) );
		outbuf = (short *) audio_buf.getData();
		inbuf = &packetbuf[ sizeof( rtp_hdr_t ) ];
		size = readILBCBuffer_20( &ilbcDecInst_20, inbuf, tmpbuf, 1, false );
		short *s = (short *)tmpbuf;
		for( i = 0; i < size; ++i) {
			*outbuf = (short)(s[i]);
			++outbuf;
		}
	} else if( codec == codecILBC_30 && (int)h->pt == ILBCCODEC ) {
		audio_buf.resize( BLOCKL_30MS * sizeof( short ) );
		outbuf = (short *) audio_buf.getData();
		inbuf = &packetbuf[ sizeof( rtp_hdr_t ) ];
		size = readILBCBuffer_30( &ilbcDecInst_30, inbuf, tmpbuf, 1, false );
		short *s = (short *)tmpbuf;
		for( i = 0; i < size; i++) {
			*outbuf++ = (short)(s[i]);
		}
#ifdef SPEEX
	} else if((int)h->pt == SPEEXCODEC ) {
			int lg = recvsize - (int) sizeof( rtp_hdr_t ) ;

			audio_buf.resize( FRAME_SIZE * sizeof (short));
			outbuf = (short *) audio_buf.getData();
			inbuf = &packetbuf[ sizeof( rtp_hdr_t ) ];

				size = readspeexBuffer( &speexDecInst, inbuf, tmpbuf, 1, lg );

				short *s = (short *)tmpbuf;
				for( i = 0; i < size; i++) {
					*outbuf++ = (short)(s[i]);

				}
				lg -= 10;
				inbuf += 10;
#endif
	} else {
			/* A  good place to process incoming DTMF */
			audio_buf.resize (0);
	}

	return true;
}


void DspOutRtp::setCodec( const codecType newCodec, int newCodecNum )
{
	codec = newCodec;