RTP ES流解析

typedef struct tagNETDEVParseVideoData 
{
    BYTE   *pucData;             /* 视频数据指针  Pointer to video data */
    INT32  dwDataLen;            /* 视频数据长度  Video data length */
    INT32  dwVideoFrameType;     /* 视频帧类型,参考枚举#NETDEV_VIDEO_FRAME_TYPE_E  Frame type, see enumeration #NETDEV_VIDEO_FRAME_TYPE_E */
    INT32  dwVideoCodeFormat;    /* 视频编码格式,参考枚举#NETDEV_PLAYER_VIDEO_CODE_TYPE_E  Video encoding format, see enumeration #NETDEV_PLAYER_VIDEO_CODE_TYPE_E */
    INT32  dwHeight;             /* 视频图像高度  Video image height */
    INT32  dwWidth;              /* 视频图像宽度  Video image width */
    INT64  tTimeStamp;           /* 时间戳（毫秒） Time stamp (ms) */
    BYTE  byRes[8];              /* 保留字段  Reserved */
}NETDEV_PARSE_VIDEO_DATA_S, *LPNETDEV_PARSE_VIDEO_DATA_S;

#define  BYTE_OFFSET				(4)
#define  DataSize(b,e)				(e-b)

	unsigned char* m_pRtpBuf = new unsigned char[m_nRtpBuf];	
	// 给前四个字节赋值初始码0x00000001
	*(int*)(m_pRtpBuf) = 0x01000000;
	
// 进行RTP解析
		bool bMark;
		char* pOut;	// 裸数据
		int nRawSize; // 裸数据大小
		char payload; // 负载，可决定类型
		unsigned int ssrc; // ssrc
		unsigned int nRtpTimestamp; // 时间戳, 偏移
		bool bComplete = true;
		bool bRet = Parse((unsigned char*)pucBuffer, dwBufSize, payload, bMark, ssrc, nRtpTimestamp, pOut, nRawSize);
		if (!bRet)
		{
			return;
		}
		else
		{
			unsigned numBytesToSkip = 0;
			switch (payload)
			{
			case 105:
			{
				// 处理RTP载荷H264特殊头
				unsigned char fCurPacketNALUnitType = (pOut[0] & 0x1F);
				switch (fCurPacketNALUnitType) {
				case 24: { // STAP-A
					numBytesToSkip = 1; // discard the type byte
					break;
				}
				case 25: case 26: case 27: { // STAP-B, MTAP16, or MTAP24
					numBytesToSkip = 3; // discard the type byte, and the initial DON
					break;
				}
				case 28: case 29: { // // FU-A or FU-B
					// For these NALUs, the first two bytes are the FU indicator and the FU header.
					// If the start bit is set, we reconstruct the original NAL header into byte 1:
					unsigned char startBit = pOut[1] & 0x80;
					unsigned char endBit = pOut[1] & 0x40;
					if (startBit) {

						pOut[1] = (pOut[0] & 0xE0) | (pOut[1] & 0x1F);
						numBytesToSkip = 1;
					}
					else {
						// The start bit is not set, so we skip both the FU indicator and header:
						numBytesToSkip = 2;
					}
					bComplete = (endBit != 0);
					break;
				}
				default: {
					// This packet contains one complete NAL unit:
					numBytesToSkip = 0;
					bComplete = true;
					break;
				}
				}
				break;
			}
			case 108:
			{
				// 处理RTP载荷H265特殊头
				bool fExpectDONFields = false;
				unsigned short DONL = 0;
				unsigned char fCurPacketNALUnitType = (pOut[0] & 0x7E) >> 1;
				switch (fCurPacketNALUnitType) {
				case 48: { // Aggregation Packet (AP)
					// We skip over the 2-byte Payload Header, and the DONL header (if any).
					if (fExpectDONFields) {
						DONL = (pOut[2] << 8) | pOut[3];
						numBytesToSkip = 4;
					}
					else {
						numBytesToSkip = 2;
					}
					break;
				}
				case 49: { // Fragmentation Unit (FU)
					// This NALU begins with the 2-byte Payload Header, the 1-byte FU header, and (optionally)
					// the 2-byte DONL header.
					// If the start bit is set, we reconstruct the original NAL header at the end of these
					// 3 (or 5) bytes, and skip over the first 1 (or 3) bytes.
					unsigned char startBit = pOut[2] & 0x80; // from the FU header
					unsigned char endBit = pOut[2] & 0x40; // from the FU header
					if (startBit) {

						unsigned char nal_unit_type = pOut[2] & 0x3F; // the last 6 bits of the FU header
						unsigned char newNALHeader[2];
						newNALHeader[0] = ((pOut[0] & 0x81) | (nal_unit_type << 1));
						newNALHeader[1] = pOut[1];

						if (fExpectDONFields) {
							DONL = (pOut[3] << 8) | pOut[4];
							pOut[3] = newNALHeader[0];
							pOut[4] = newNALHeader[1];
							numBytesToSkip = 3;
						}
						else {
							pOut[1] = newNALHeader[0];
							pOut[2] = newNALHeader[1];
							numBytesToSkip = 1;
						}

						//headerStart[2] = headerStart[1];
						//headerStart[1] = (headerStart[0]&0x81)|(nal_unit_type<<1);
					}
					else {
						// The start bit is not set, so we skip over all headers:
						if (fExpectDONFields) {
							DONL = (pOut[3] << 8) | pOut[4];
							numBytesToSkip = 5;
						}
						else {
							numBytesToSkip = 3;
						}
					}
					bComplete = (endBit != 0);
					break;
				}
				default: {
					// This packet contains one complete NAL unit:
					bComplete = true;
					numBytesToSkip = 0;
					break;
				}
				}
			}
			default:
				break;
			}
			
			// 将数据拷贝至缓存
			if (m_nCurBuf + nRawSize > m_nRtpBuf)
			{
				// 缓存不够，需重新分配大小
				unsigned char* pBuf = new unsigned char[m_nCurBuf + nRawSize];
				memcpy(pBuf, m_pRtpBuf, m_nCurBuf);
				delete[] m_pRtpBuf;
				m_pRtpBuf = pBuf;
			}
			memcpy(m_pRtpBuf + m_nCurBuf, pOut + numBytesToSkip, nRawSize - numBytesToSkip);
			m_nCurBuf = m_nCurBuf + nRawSize - numBytesToSkip;
		}

		// 标记一帧结束
		if (bComplete)
		{
			NETDEV_PARSE_VIDEO_DATA_S framedata = { 0 };
			// 此时需将该帧数据处理后上传到流媒体
			switch (payload)
			{				
			case 0: // PCMU
				break;				
			case 8: // PCMA
				break;
			case 96: // H264
			case 105: // H264
			{
				if ((m_pRtpBuf[4] & 0x0F) == 5 ||
					(m_pRtpBuf[4] & 0x0F) == 7 ||
					(m_pRtpBuf[4] & 0x0F) == 8 ||
					(m_pRtpBuf[4] & 0x0F) == 6)
				{
					// I Frame
					framedata.dwVideoFrameType = NETDEV_VIDEO_FRAME_I;
				}
				else if ((m_pRtpBuf[4] & 0x0F) == 0x01)
				{
					// P Frame
					framedata.dwVideoFrameType = NETDEV_VIDEO_FRAME_P;
				}
				else
				{
					break;
				}

				framedata.dwDataLen = m_nCurBuf;
				framedata.dwHeight = m_stVideoStreamInfo.dwHeight;
				framedata.dwWidth = m_stVideoStreamInfo.dwWidth;
				framedata.dwVideoCodeFormat = NETDEV_PLAYER_VIDEO_CODE_H264;
				framedata.pucData = m_pRtpBuf;
				framedata.tTimeStamp = nRtpTimestamp;
				break;
			}
			case 108: // H265
			{
				unsigned short type = (m_pRtpBuf[4] >> 1) & 0x3F;

				if ((type >= 16 && type <= 21))
				{
					// I Frame
					framedata.dwVideoFrameType = NETDEV_VIDEO_FRAME_I;
				}
				else if (type >= 1 && type <= 9)
				{
					// P Frame
					framedata.dwVideoFrameType = NETDEV_VIDEO_FRAME_P;
				}
				else if (type >= 32 && type <= 34)
				{
					// VPS SPS PPS
					framedata.dwVideoFrameType = NETDEV_VIDEO_FRAME_I;
				}
				else if (type == 39)
				{
					// SEI
					framedata.dwVideoFrameType = NETDEV_VIDEO_FRAME_I;
				}
				else
				{
					break;
				}

				framedata.dwDataLen = m_nCurBuf;
				framedata.dwHeight = m_stVideoStreamInfo.dwHeight;
				framedata.dwWidth = m_stVideoStreamInfo.dwWidth;
				framedata.dwVideoCodeFormat = NETDEV_PLAYER_VIDEO_CODE_H265;
				framedata.pucData = m_pRtpBuf;
				framedata.tTimeStamp = nRtpTimestamp;
				break;
			}
			default:
				break;
			}

			ProcessRealData(&framedata);
			// 处理完，重置数据
			m_nCurBuf = BYTE_OFFSET;
		}
		else
		{
			// 继续接收数据
			return;
		}

bool Parse(unsigned char* IN pBuf, int IN nSize, char& OUT payload, bool& OUT bMark, unsigned int& OUT ssrc, unsigned& OUT nRtpTimestamp, char*& OUT pOut, int& OUT nRawSize)
{
	// rtp header must greater than 12 bytes
	if (nSize < 12) return false;
	// temp variable
	char* pData = (char*)pBuf;
	char* pEnd = pData + nSize;
	// v 2 p 1 x 1 cc 4 m 1 pt 7 seq 16 timestamp 32 SSRC 32 [CSRC ...]
	unsigned rtpHdr = ntohl(*(u_int32_t*)(pData)); Skip(pData, 4, pEnd);
	// mark bit
	bool rtpMarkerBit = (rtpHdr & 0x00800000) != 0;
	// seq number
	unsigned short rtpSeqNo = (unsigned short)(rtpHdr & 0xFFFF);
	// timestamp
	unsigned rtpTimestamp = ntohl(*(u_int32_t*)(pData)); Skip(pData, 4, pEnd);
	// SSRC
	unsigned rtpSSRC = ntohl(*(u_int32_t*)(pData)); Skip(pData, 4, pEnd);

	// Check the RTP version number (it should be 2):
	if ((rtpHdr & 0xC0000000) != 0x80000000) return false;
	// Skip over any CSRC identifiers in the header:
	unsigned cc = (rtpHdr >> 24) & 0xF;
	if (DataSize(pData, pEnd) < cc) return false; Skip(pData, 4 * cc, pEnd);
	// Check for (& ignore) any RTP header extension
	if (rtpHdr & 0x10000000) {
		if (DataSize(pData, pEnd) < 4) return false;
		unsigned extHdr = ntohl(*(u_int32_t*)(pData)); Skip(pData, 4, pEnd);
		unsigned remExtSize = 4 * (extHdr & 0xFFFF);
		if (DataSize(pData, pEnd) < remExtSize) return false;
		Skip(pData, remExtSize, pEnd);
	}
	// Discard any padding bytes:
	if (rtpHdr & 0x20000000) {
		if (DataSize(pData, pEnd) == 0) return false;
		unsigned numPaddingBytes = (unsigned)(pData)[DataSize(pData, pEnd) - 1];
		if (DataSize(pData, pEnd) < numPaddingBytes) return false;
		if (numPaddingBytes > DataSize(pData, pEnd)) numPaddingBytes = DataSize(pData, pEnd);
		pEnd -= numPaddingBytes;
	}
	// got the Payload Type.
	payload = (rtpHdr & 0x007F0000) >> 16;
	// The rest of the packet is the usable data.  return to user
	pOut = pData;
	nRawSize = (pEnd - pData);
	bMark = rtpMarkerBit;
	ssrc = rtpSSRC;
	nRtpTimestamp = rtpTimestamp;
	return true;
}

void Skip(char*& pBuf, int n, char* pEnd)
{
	pBuf += n;
	if (pBuf > pEnd)
	{
		pBuf = pEnd;
	}
}