ES->PES->PS打包程序

#ifndef TPACKPS_H_
#define TPACKPS_H_

#include <string>
#include <stdint.h>

class TPackPs
{
public:
    enum
    {
        PSMUX_ST_RESERVED                = 0x00,
        PSMUX_ST_VIDEO_MPEG1            = 0x01,
        PSMUX_ST_VIDEO_MPEG2            = 0x02,
        PSMUX_ST_AUDIO_MPEG1            = 0x03,
        PSMUX_ST_AUDIO_MPEG2            = 0x04,
        PSMUX_ST_PRIVATE_SECTIONS        = 0x05,
        PSMUX_ST_PRIVATE_DATA            = 0x06,
        PSMUX_ST_MHEG                    = 0x07,
        PSMUX_ST_DSMCC                    = 0x08,
        PSMUX_ST_H222_1                    = 0x09,
        PSMUX_ST_AUDIO_AAC                = 0x0f,
        PSMUX_ST_VIDEO_MPEG4            = 0x10,
        PSMUX_ST_VIDEO_H264                = 0x1b,
        PSMUX_ST_VIDEO_H265                = 0x24,
        PSMUX_ST_PS_VIDEO_SVAC            = 0x80,
        PSMUX_ST_PS_AUDIO_AC3            = 0x81,
        PSMUX_ST_PS_AUDIO_DTS            = 0x8a,
        PSMUX_ST_PS_AUDIO_LPCM            = 0x8b,
        PSMUX_ST_PS_AUDIO_G711A            = 0x90,
        PSMUX_ST_PS_AUDIO_G711U            = 0x91,
        PSMUX_ST_PS_AUDIO_G722_1        = 0x92,
        PSMUX_ST_PS_AUDIO_G723_1        = 0x93,
        PSMUX_ST_PS_AUDIO_G729            = 0x99,
        PSMUX_ST_PS_AUDIO_SVAC            = 0x9b,
        PSMUX_ST_PS_DVD_SUBPICTURE        = 0xff,
    };
public:
    TPackPs();
    virtual ~TPackPs();
    void setDecoderId(int decoderId, bool isVideo);
    int packFrameToPs(char * PSFrameBuffer, int PSFrameBufferLen, char *pFrame, int frameLen, int nStreamType, bool isKeyFrame, int nTimeStamp);


private:
    int z_videoDecoderId;
    int z_autioDecoderId;
    //std::string z_psbuf;

    int getPsDecoderType(bool isVideo);
    void SetHeaderTimeStamp(uint8_t *dest, uint64_t pts);
    void SetPESTimeStamp(uint8_t *buff, uint64_t ts);
    int GetSinglePESHeader(uint8_t *header, uint64_t mtime, uint16_t farmLen);
    int GetPSHeader(uint8_t *header, uint64_t mtime, uint16_t farmLen, int streamType, bool isKeyframe);

    static const unsigned char PS_HEAD[];
    static const unsigned char SYS_MAP_HEAD[];
    static const unsigned char PES_HEAD[];
};

#endif  //TPACKPS_H_

#include "TPackPs.h"
#include "ll_log.h"

const unsigned char TPackPs::PS_HEAD[] = {
    /*PS头*/
    0x00, 0x00, 0x01, 0xba,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,/*时间戳*/
    0x01, 0x47, 0xb3,
    0xf8
};
const unsigned char TPackPs::SYS_MAP_HEAD[] = {
    /*PS_SYS头*/
    0x00, 0x00, 0x01, 0xbb,
    0x00, 0x0c,/*sys头长度,不含自己，6+3*流的数目*/
    0x80, 0xa3, 0xd9,/*速率*/
    0x04, 0xe1,/*音频流数，视频流数加3个1标识*/
    0xff,/**/
    0xb9, 0xe0, 0x00, 0xb8, 0xc0, 0x40,/*流信息，b9视频，b8音频*/
    /*PS_MAP头*/
    0x00, 0x00, 0x01, 0xbc,
    0x00, 0x12,/*psm长度*/
    0x04, 0xff,/**/
    0x00, 0x00, 0x00, 0x08,/*固定2路流*/
    0x1b, 0xe0, 0x00, 0x00,/*视频，第一个字节(0x1b), 更具不同的视频编码改变即可封装不同的流，见开头宏定义*/
    0x90, 0xc0, 0x00, 0x00,/*音频，同视频*/
    0x00, 0x00, 0x00, 0x00/*4b CRC，暂时没设置*/
};
const unsigned char TPackPs::PES_HEAD[] = {
    /*PS_PES头*/
    0x00, 0x00, 0x01, 0xe0,
    0x00, 0x00,/*pes长度*/
    0x80, 0xc0,/*附加信息*/
    0x0a,/*附加信息长度*/
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 /*pts和pds*/
};

TPackPs::TPackPs()
    :z_videoDecoderId(0x201)
    ,z_autioDecoderId(0x211)
{

}

TPackPs::~TPackPs()
{
}

void TPackPs::setDecoderId(int decoderId, bool isVideo)
{
    if(decoderId < 0x200 || decoderId > 0x204)
    {
        //LLOGW("decoder Id error[0x%x]", decoderId);
        return;
    }

    if(isVideo && z_videoDecoderId != decoderId)
    {
        LLOGI("z_videoDecoderId changer [%@]->[%@]", z_videoDecoderId, decoderId);
        z_videoDecoderId = decoderId;
    }
    if(!isVideo && z_autioDecoderId != decoderId)
    {
        LLOGI("z_autioDecoderId changer [%@]->[%@]", z_autioDecoderId, decoderId);
        z_autioDecoderId = decoderId;
    }
}

int TPackPs::getPsDecoderType(bool isVideo)
{
    if(isVideo)
    {
        int psDecoderType = PSMUX_ST_VIDEO_H264;
        switch(z_videoDecoderId)//tb575 decoderid->ps video type
        {
        case 0x200://ＭＰＥＧ�４
            psDecoderType = PSMUX_ST_VIDEO_MPEG4;
            break;
        case 0x201://Ｈ� ２６４
            psDecoderType = PSMUX_ST_VIDEO_H264;
            break;
        case 0x202://Ｈ� ２６５
            psDecoderType = PSMUX_ST_VIDEO_H265;
            break;
        case 0x203://ＳＶＡＣ
            psDecoderType = PSMUX_ST_PS_VIDEO_SVAC;
            break;
        case 0x204://ＡＶＳ
            //psDecoderType= ;
            //break;    
        default:
            break;
        }    
        return psDecoderType;
    }
    else
    {
        int psDecoderType = PSMUX_ST_PS_AUDIO_G711A;
        switch(z_autioDecoderId)
        {
        case 0x210://Ｇ� ７１１�ｕ
            psDecoderType = PSMUX_ST_PS_AUDIO_G711U;
            break;
        case 0x211://Ｇ� ７１１�ａ
            psDecoderType = PSMUX_ST_PS_AUDIO_G711A;
            break;
        case 0x212://Ｇ� ７２２� １
            psDecoderType = PSMUX_ST_PS_AUDIO_G722_1;
            break;
        case 0x213://Ｇ� ７２３
            psDecoderType = PSMUX_ST_PS_AUDIO_G723_1;
            break;
        case 0x214://Ｇ� ７２９
            psDecoderType = PSMUX_ST_PS_AUDIO_G729;
            break;
        case 0x215://ＡＡＣ
            psDecoderType = PSMUX_ST_AUDIO_AAC;
            break;
        default:
            //printf("unknow decder Id[0x%x]
", decoderId);
            break;
        }    
        return psDecoderType;
    }
}

void TPackPs::SetHeaderTimeStamp(uint8_t *dest, uint64_t pts)
{
    uint8_t *scr_buf = dest + 4;
    scr_buf[0] = 0x40 | (((uint8_t)(pts >> 30) & 0x07) << 3) | 0x04 | ((uint8_t)(pts >> 28) & 0x03);
    scr_buf[1] = (uint8_t)((pts >> 20) & 0xff);
    scr_buf[2] = (((uint8_t)(pts >> 15) & 0x1f) << 3) | 0x04 | ((uint8_t)(pts >> 13) & 0x03);
    scr_buf[3] = (uint8_t)((pts >> 5) & 0xff);
    scr_buf[4] = (((uint8_t)pts & 0x1f) << 3) | 0x04;
    scr_buf[5] = 1;
}

// 设置PES头中的PTS和DTS字段
void TPackPs::SetPESTimeStamp(uint8_t *buff, uint64_t ts)
{
    buff += 9;
    // PTS
    buff[0] = (uint8_t)(((ts >> 30) & 0x07) << 1) | 0x30 | 0x01;
    buff[1] = (uint8_t)((ts >> 22) & 0xff);
    buff[2] = (uint8_t)(((ts >> 15) & 0xff) << 1) | 0x01;
    buff[3] = (uint8_t)((ts >> 7) & 0xff);
    buff[4] = (uint8_t)((ts & 0xff) << 1) | 0x01;
    // DTS
    buff[5] = (uint8_t)(((ts >> 30) & 0x07) << 1) | 0x10 | 0x01;
    buff[6] = (uint8_t)((ts >> 22) & 0xff);
    buff[7] = (uint8_t)(((ts >> 15) & 0xff) << 1) | 0x01;
    buff[8] = (uint8_t)((ts >> 7) & 0xff);
    buff[9] = (uint8_t)((ts & 0xff) << 1) | 0x01;
}

int TPackPs::GetSinglePESHeader(uint8_t *header, uint64_t mtime, uint16_t farmLen)
{
    farmLen += 13;
    memcpy(header, PES_HEAD, sizeof(PES_HEAD));
    *(header+4) = (uint8_t)(farmLen>>8);
    *(header+5) = (uint8_t)farmLen;

    SetPESTimeStamp(header, mtime);
    return sizeof(PES_HEAD);
}

//streamType 0 audio, 1 video
int TPackPs::GetPSHeader(uint8_t *header, uint64_t mtime, uint16_t farmLen, int streamType, bool isKeyframe)
{
    if(streamType == 0)//语音包
    {
        GetSinglePESHeader(header, mtime, farmLen);
        *(header+3) = 0xc0;
        return sizeof(PES_HEAD);
    }

    if(isKeyframe) //I帧
    {
        memcpy(header, PS_HEAD, sizeof(PS_HEAD));
        SetHeaderTimeStamp(header, mtime);
        header += sizeof(PS_HEAD);

        memcpy(header, SYS_MAP_HEAD, sizeof(SYS_MAP_HEAD));
        header[30] = getPsDecoderType(true);//set video type
        header += sizeof(SYS_MAP_HEAD);

        GetSinglePESHeader(header, mtime, farmLen);
        return sizeof(PS_HEAD) + sizeof(SYS_MAP_HEAD) + sizeof(PES_HEAD);
    }
    else
    {
        memcpy(header, PS_HEAD, sizeof(PS_HEAD));
        SetHeaderTimeStamp(header, mtime);
        header += sizeof(PS_HEAD);

        GetSinglePESHeader(header, mtime, farmLen);
        return sizeof(PS_HEAD) + sizeof(PES_HEAD);
    }
}

int TPackPs::packFrameToPs(char * PSFrameBuffer, int PSFrameBufferLen, char *pFrame, int frameLen, int nStreamType, bool isKeyFrame, int nTimeStamp)
{
    if(!pFrame || !frameLen)
    {
        printf("Input error
");
        return 0;
    }

    int nFrameLength = frameLen;
    // 每个pes最多65400数据
    int PesLenth = nFrameLength > 65400 ? 65400 : nFrameLength;
    // 第一个pes需要有ps头，其它不需要，音频直接打包pes（00 00 01 c0)
    int psHeadLen = GetPSHeader((unsigned char*)PSFrameBuffer, nTimeStamp, PesLenth, nStreamType, isKeyFrame);
    if(psHeadLen + PesLenth >  PSFrameBufferLen)
    {
        printf("PSFrameBufferLen too small
");
        return -1;        
    }
    memcpy(PSFrameBuffer + psHeadLen, pFrame, PesLenth);
    int psSize = psHeadLen + PesLenth;
    int pod = PesLenth;

    nFrameLength -= PesLenth;
    while (nFrameLength > 0)
    {
        PesLenth = nFrameLength > 65400 ? 65400 : nFrameLength;
        psHeadLen = GetSinglePESHeader((unsigned char*)PSFrameBuffer + psSize, nTimeStamp, PesLenth);
        if( psSize + psHeadLen + PesLenth > PSFrameBufferLen)
        {
            printf("PSFrameBufferLen too small2
");
            return -1;        
        }
        memcpy(PSFrameBuffer + psSize + psHeadLen, pFrame + pod, PesLenth);
        psSize += (PesLenth + psHeadLen);
        pod += PesLenth;
        nFrameLength -= PesLenth;
    }

    return psSize;
}

        bool isKeyFrame = true;//(pHeader->marker == 1);//attention:no marker in 3.0
        int decoderId = ::ntohl(pHeader->decoderId) & 0xffff;
        psPackage.setDecoderId(decoderId, true);
        int psPackageLen = psPackage.packFrameToPs(psPtr, z_psbuf.size(), dataPtr, dataLen, 1, isKeyFrame, pHeader->timestamp);