解析RTP流for mpeg4

#include <stdio.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <libgen.h>
#include <fcntl.h>
#include <stdlib.h>

#define Printval(x) \
	printf("[%s][%s:%d]:%s=%d\n", __FILE__, __FUNCTION__, __LINE__, #x, (int)x)

typedef char					INT8;
typedef unsigned char			UINT8;
typedef short					INT16;
typedef unsigned short			UINT16;
typedef int						INT32;
typedef unsigned int			UINT32;
typedef long long int			INT64;
typedef unsigned long long int	UINT64;
#pragma pack(push)
#pragma (1)

typedef struct tagRTP_HEAD_S
{
    UINT8 cc:4;			/* CSRC count */
    UINT8 x:1;			/* header extension flag */
    UINT8 p:1;			/* padding flag */
	UINT8 version:2;	/* protocol version */
    UINT8 pt:7;			/* payload type */
    UINT8 m:1;			/* marker bit */
    INT16  seq;			/* sequence number */
	INT32  ts;			/* timestamp */
    UINT32 ssrc;		/* synchronization source */
}RTP_HEAD_S;

#pragma pack(pop)

INT64 llStampTmp = 1024LL*90000/44100;

int WriteFile(const char *pcName, const UINT8 *pucBuf, int len)
{
	int fd = open(pcName, O_WRONLY | O_CREAT | O_APPEND, 0644);
	write(fd, pucBuf, len);
	close(fd);

	return 0;
}

#define AUDIO_FILE_NAME "./audio.ts"

inline int WriteAudioFile(const UINT8 *pucBuf, int len)
{
	WriteFile(AUDIO_FILE_NAME, pucBuf, len);

	return 0;
}

#define VIDEO_FILE_NAME "./video.ts"
inline int WriteVideoFile(const UINT8 *pucBuf, int len)
{
	WriteFile(VIDEO_FILE_NAME, pucBuf, len);

	return 0;
}


#define PES_START_CODE	 0x000001  /*3字节*/
#define PES_AUD_STREAM_ID  0xc0
#define PES_VID_STREAM_ID  0xe0  /*1字节*/
#define PES_PACKET_LENGTH	  0X0  /*2字节*/
#define PES_CONTROL1		 (0x89)   /*1字节*/
#define PES_CONTROL2		 (0x2<<(8-2))  /*PTS_DTS_FLAG*/   /*1字节*/
#define PES_HEADER_DATA_LENGTH  5 /*1字节*/
#define ADTS_BUFFER_FULLNESS_VBR    0x7FF

#if 0
#define ADTS_BUFFER_FULLNESS_HBR    0x000
#else
#define ADTS_BUFFER_FULLNESS_HBR    0x7CF
#endif

int MakePesHead(int iType, UINT8 *pucBuf, int iLen, INT64 pts)
{
	UINT16 usLen = 0;
	pucBuf[0] = (PES_START_CODE >> 16) & 0xff;  
	pucBuf[1] = (PES_START_CODE >> 8) & 0xff;  
	pucBuf[2] = (PES_START_CODE) & 0xff;  

    if (iType == 0)
    {
    	pucBuf[3] = PES_AUD_STREAM_ID;
	}
	else
	{
    	pucBuf[3] = PES_VID_STREAM_ID;
	}

	usLen = iLen + 8;
	pucBuf[4] = (usLen >> 8)&0xff;  
	pucBuf[5] = (usLen)&0xff;
	
	pucBuf[6] = PES_CONTROL1;  
	pucBuf[7] = PES_CONTROL2;

	pucBuf[8] = PES_HEADER_DATA_LENGTH;

	pucBuf[9] = 0x21 | ((pts >> 29) & 0x0E);
	pucBuf[10] =  (pts >>22 & 0xFF);
	pucBuf[11] = 0x01 | ((pts >> 14 ) & 0xFE);
	pucBuf[12] =  (pts >> 7 & 0xFF);
	pucBuf[13] = 0x01 | ((pts << 1 ) & 0xFE);

	return 0;
}

int MakeAdtsHead(UINT8 *pucBuf, int iLen)
{
	UINT8 channel = 0x02;
	pucBuf[0] = 0xFF;
	pucBuf[1] = 0xF1;

	pucBuf[2] = 0xFE & ((0x01<<6) | (0x04<<2));
	pucBuf[2] |= ((channel>>2)&0x01);

	pucBuf[3] = (channel<<6) & 0xC0;

	pucBuf[3] |= 0x03 & (iLen>>11); 
	pucBuf[4] = (iLen>>3)&0xFF;
	pucBuf[5] = (iLen<<5)&0xE0;

	pucBuf[5] |= (ADTS_BUFFER_FULLNESS_HBR>>6) & 0x1F;
	pucBuf[6] = (ADTS_BUFFER_FULLNESS_HBR<<2) & 0xFE;

	return 0;
}

UINT8 ChangeChar(char cChar)
{
    switch(cChar)
    {
    case '0':
        return 0x00;
    case '1':
        return 0x01;
    case '2':
        return 0x02;
    case '3':
        return 0x03;
    case '4':
        return 0x04;
    case '5':
        return 0x05;
    case '6':
        return 0x06;
    case '7':
        return 0x07;
    case '8':
        return 0x08;
    case '9':
        return 0x09;
    case 'A':
    case 'a':
        return 0x0a;
    case 'B':
    case 'b':
        return 0x0b;
    case 'C':
    case 'c':
        return 0x0c;
    case 'D':
    case 'd':
        return 0x0d;
    case 'E':
    case 'e':
        return 0x0e;
    case 'F':
    case 'f':
        return 0x0f;
    default:
printf("##########[%s][%s:%d]Default Err\n", __FILE__, __FUNCTION__, __LINE__);
    }
}

int GetVideoHead(UINT8 *pucBuf)
{
    char *pcBuf = "000001B0F4000001B50900000100000001200086C400670C5A1120518F";
    int i = 0;
    int iNum = 0;

    while(*pcBuf != '\0')
    {
        *(pucBuf+iNum) = (ChangeChar(*pcBuf++))<<4;
        *(pucBuf+iNum) |= ChangeChar(*pcBuf++);
        iNum++;
    }

    printf("config:");
    for (i=0; i<iNum;i++)
    {
        printf("%02X ", *(pucBuf+i));
    }
    printf("\n");
    
    return iNum;
}

#define PES_HEADER_LEN 14

int DealVideo(UINT8 *pucRtp)
{
	RTP_HEAD_S *pstRtpHead = (RTP_HEAD_S *)pucRtp;
	UINT64 pts = ntohl(pstRtpHead->ts);
	int iLen = ntohl(pstRtpHead->ssrc);
	//static int iDataLen = 0;
	static int iFirst = 0;
	static int iBufPtr = 0;
	static UINT8 aucBufTmp[100*1024]; 
	static UINT8 aucVideoHead[64];

	Printval(iLen-12);
printf("##########[%s][%s:%d]m=%d\n", __FILE__, __FUNCTION__, __LINE__, pstRtpHead->m);
	if (0 == iBufPtr)
	{
    	memset(aucBufTmp,    0, sizeof(aucBufTmp));
    	memset(aucVideoHead, 0, sizeof(aucVideoHead));
	}

    if (iFirst == 0)
    {
        int iConfigLen = 0;
        iFirst = 1;
        iConfigLen = GetVideoHead(aucVideoHead);
    	Printval(iConfigLen);
        memcpy(aucBufTmp+iBufPtr+PES_HEADER_LEN, aucVideoHead, iConfigLen);
        iBufPtr += iConfigLen;
    }
    memcpy(aucBufTmp+iBufPtr+PES_HEADER_LEN, pucRtp+12, iLen - 12);
    iBufPtr += iLen - 12;
	
	if (1 == pstRtpHead->m)
	{
    	Printval(iBufPtr);
    	Printval(pts);
        MakePesHead(1, aucBufTmp, iBufPtr, pts);
    	WriteVideoFile(aucBufTmp, PES_HEADER_LEN+iBufPtr);
	    iBufPtr = 0;
	}
    
	
    return 0;
}

int DealAudio(UINT8 *pucRtp)
{
	RTP_HEAD_S *pstRtpHead = (RTP_HEAD_S *)pucRtp;
	UINT16 usAuHeader[50] = {0};
	int i = 0;
	static UINT8 aucBufTmp[100*1024]; 
	UINT16 usAuHeadersLen = htons(*(UINT16 *)(pucRtp + 12));
	usAuHeadersLen = (usAuHeadersLen+7) / 8 / 2;
	//Printval(usAuHeadersLen);

//printf("##########[%s][%s:%d]\n", __FILE__, __FUNCTION__, __LINE__);
	memset(aucBufTmp, 0, sizeof(aucBufTmp));

	pucRtp += 12 + 2;
	for (i=0; i<usAuHeadersLen; i++)
	{
		usAuHeader[i] = htons(*(UINT16 *)pucRtp);
		usAuHeader[i] = (usAuHeader[i] >> 3);
		//Printval(usAuHeader[i]);
		pucRtp += 2;
	}

	UINT64 pts = ntohl(pstRtpHead->ts);
	//Printval(pts);
	pts = pts * 90000LL / 44100LL;
	//Printval(pts);

	for(i=0; i<usAuHeadersLen; i++)
	{
		MakePesHead(0, aucBufTmp, usAuHeader[i]+7, pts+i*llStampTmp);
		MakeAdtsHead(aucBufTmp+14,usAuHeader[i]+7);
		memcpy(aucBufTmp+21, pucRtp, usAuHeader[i]);
		pucRtp += usAuHeader[i];

		WriteAudioFile(aucBufTmp, 21+usAuHeader[i]);
	}

	return 0;
}


int DealRtpMsg(UINT8 *pucRtp)
{
	RTP_HEAD_S *pstRtpHead = (RTP_HEAD_S *)pucRtp;
	int pt = pstRtpHead->pt;
//printf("##########[%s][%s:%d]pt=%d\n", __FILE__, __FUNCTION__, __LINE__, pt);
	if (pt == 97)
	{
		DealAudio(pucRtp);
	}
	else
	{
		DealVideo(pucRtp);
	}

	return 0;
}

int main(int argc, char **argv)
{
	int fd = 0;
	int iFileLen = 0;
	UINT8 *pucFileBuf = NULL;
	int iLenTmp = 0;

	system("rm -rf "AUDIO_FILE_NAME);
	system("rm -rf "VIDEO_FILE_NAME);

	fd = open(argv[1], O_RDONLY);
	iFileLen = lseek(fd, 0, SEEK_END);
	lseek(fd, 0, SEEK_SET);
	Printval(iFileLen);

	pucFileBuf = malloc(iFileLen+1);
	memset(pucFileBuf, 0, iFileLen);

Printval(llStampTmp);

	while (iLenTmp < iFileLen)
	{
		iLenTmp += read(fd, pucFileBuf+iLenTmp, iFileLen - iLenTmp);
	}
Printval(iLenTmp);

	printf("##########[%s][%s:%d]\n", __FILE__, __FUNCTION__, __LINE__);
	
	iLenTmp = 0;
	while (iLenTmp < iFileLen)
	{
		int i = 0;
		RTP_HEAD_S *pstRtpHead = (RTP_HEAD_S *)(pucFileBuf+iLenTmp);
#if 0
		printf("RTP_HEAD:\n");
		for(i=0; i<12; i++)
		{
			printf("%02x ", *(pucFileBuf+iLenTmp+i));
		}
		printf("\n");
#endif
		DealRtpMsg(pucFileBuf+iLenTmp);

		iLenTmp += ntohl(pstRtpHead->ssrc);
//Printval(iLenTmp);
	}

	close(fd);

    return 0;
}