public List <RtspFrame> ProcessData(byte[] buf, int length, RtpSession session)
{
List <RtspFrame> frames = new List <RtspFrame>();
int offset = 0;
int pos = 0;
while (true)
{
if (offset >= length)
{
logger.Debug("!!!!!!!!!!!!!!!!!!! " + offset + " >= " + length);
break;
//return;
}
if (tcpBufferOffset > 0)
{// если остались не обработанные данные
int len = length - offset;
int newSize = tcpBufferOffset + len;
byte[] newBuf = new byte[newSize];
Array.Copy(tcpBuffer, 0, newBuf, 0, tcpBufferOffset);
Array.Copy(buf, offset, newBuf, tcpBufferOffset, len);
tcpBufferOffset = 0;
// Debug.WriteLine("ProcessTcpBuffer newSize " + newSize);
buf = newBuf;
length = newSize;
}
if (streamState == RtspStreamState.WaitDollar ||
streamState == RtspStreamState.ReadHeader ||
streamState == RtspStreamState.ReadContent)
{
for (int i = offset; i < length; i++)
{
char ch = (char)buf[i];
if (ch == '$')
{
//Console.WriteLine("Find dollar at pos: " + i);
streamState = RtspStreamState.ReadId;
pos = i + 1; //header
rtspFrame = new RtspFrame();
break;
}
}
}
if (streamState == RtspStreamState.ReadId)
{// идентификатор 1 байт
int len = length - pos;
if (len > 0)
{
rtspFrame.Id = buf[pos];
//Console.WriteLine("ID: " + rtspFrame.Id + " pos: " + pos);
pos++;
streamState = RtspStreamState.ReadLen;
}
else
{ // буфер закончился выходим
break;
}
}
if (streamState == RtspStreamState.ReadLen)
{// длина пакета с данными 2 байта
int len = length - pos;
if (len > 1)
{
//int frameLength = BitConverter.ToInt16(buf, pos);
int frameLength = BigEndian.ReadInt16(buf, pos);
if (frameLength <= 0)
{
logger.Warn("frameLength " + frameLength);
//TODO:
//...
}
rtspFrame.Init(frameLength);
//Console.WriteLine("frameLength: " + frameLength + " pos: " + pos);
pos += 2;
streamState = RtspStreamState.ReadPkt;
}
else
{// сохраняем в буффер то что есть и выходим
// Debug.WriteLine(len + " < " + 2);
Array.Copy(buf, pos, tcpBuffer, tcpBufferOffset, len);
tcpBufferOffset += len;
//...
break;
}
}
if (streamState == RtspStreamState.ReadPkt)
{
if (rtspFrame != null && rtspFrame.Length > 0)
{
// сколькоданных в буфере
int bufToRead = length - pos;
int bytesAdded = rtspFrame.AddData(buf, pos, bufToRead);
pos += bytesAdded;
if (rtspFrame.BytesToRead == 0)
{// все данные прочитаны
// client.ProcessRtspFrame(rtspFrame);
//Console.WriteLine(rtspFrame.ToString());
var rtpData = rtspFrame.Data;
RtpPacket rtpPacket = RtpPacket.Create(rtpData, rtpData.Length, session);
// OnRtpPacketReceived(rtpPacket);
frames.Add(rtspFrame);
// переходим в режим ожидания доллара
streamState = RtspStreamState.WaitDollar;
// читаем следующий rtsp frame
rtspFrame = null;
}
if (pos < length)
{
//ProcessTcpBuffer(buf, pos, length);
offset = pos;
continue;
}
break;
// return;
}
}
{ // понять что это за данные пока не удалось копируем в буфер
int bufToRead = length - offset;
Array.Copy(buf, offset, tcpBuffer, tcpBufferOffset, bufToRead);
tcpBufferOffset += bufToRead;
logger.Debug("Array.Copy tcpBufferOffset " + tcpBufferOffset);
break;
}
}
return(frames);
}
//public static RtpPacket Create(byte[] data)
//{
// return Create(data, data.Length);
//}
public static RtpPacket Create(byte[] data, int len, RtpSession session)
{
if (len < RtpConst.MinRtpLength)
{
throw new InvalidOperationException("Invalid RTP packet. Packet size < " + RtpConst.MinRtpLength);
}
RtpPacket packet = new RtpPacket();
packet.packetBytes = data;
int dataLength = len;//data.Length;
int offset = 0;
byte firstByte = data[offset++];
packet.Version = (byte)(firstByte >> 6);
if (packet.Version != RtpConst.Version)
{
throw new InvalidOperationException("Invalid RTP packet. Unsupported version " + packet.Version);
}
/*
* If the padding bit is set, the packet contains one or more
* additional padding octets at the end which are not part of the
* payload. The last octet of the padding contains a count of how
* many padding octets should be ignored, including itself. Padding
* may be needed by some encryption algorithms with fixed block sizes
* or for carrying several RTP packets in a lower-layer protocol data
* unit.
*/
packet.Padding = ((firstByte & 0x20) == 0x20);
if (packet.Padding)
{// последний байт в пакете длинна области заполнения
byte padding = data.Last();
dataLength -= padding;
}
packet.Extension = (firstByte & 0x10) == 0x10;
packet.CSRCCount = (byte)(firstByte & 0x0F);
byte secondByte = data[offset++];
packet.Marker = (secondByte & 0x80) == 0x80;
packet.PayloadType = (secondByte & 0x7F);
packet.Sequence = BigEndian.ReadUInt16(data, offset);
offset += 2;
packet.Timestamp = BigEndian.ReadUInt32(data, offset);
offset += 4;
packet.SSRC = BigEndian.ReadUInt32(data, offset);
offset += 4;
if (packet.SSRC != session.SSRC)
{// Invalid session id
return(null);
}
offset += packet.CSRCCount * 4;
if (packet.Extension)
{ /*
* An extension mechanism is provided to allow individual
* implementations to experiment with new payload-format-independent
* functions that require additional information to be carried in the
* RTP data packet header. This mechanism is designed so that the
* header extension may be ignored by other interoperating
* implementations that have not been extended.
*/
packet.HeaderExtensionProfile = BigEndian.ReadUInt16(data, offset);
offset += 2;
var headerExtensionLength = BigEndian.ReadUInt16(data, offset);
offset += 2;
packet.HeaderExtension = new ArraySegment <byte>(data, offset, headerExtensionLength);
offset += headerExtensionLength;
}
else
{
packet.HeaderExtensionProfile = 0;
}
int payloadLength = (dataLength - offset);
packet.Payload = new ArraySegment <byte>(data, offset, payloadLength);
return(packet);
}
private void SendPackets(Socket _socket)
{
if (!running)
{
return;
}
int bytesSend = 0;
while (packetQueue.Count > 0)
{
if (!running)
{
break;
}
RtpPacket pkt = null;
lock (locker)
{
pkt = packetQueue.Dequeue();
}
if (pkt != null)
{
try
{
// RFC 2326 10.12 Embedded (Interleaved) Binary Data
/*
* S->C: $\000{2 byte length}{"length" bytes data, w/RTP header}
* S->C: $\000{2 byte length}{"length" bytes data, w/RTP header}
* S->C: $\001{2 byte length}{"length" bytes RTCP packet}
*/
const byte magicSymbol = (byte)'$';
byte channelId = 0x42;
var data = pkt.GetBytes();
ushort dataLength = (ushort)data.Length;
int frameSize = data.Length + 4; // data + header
byte[] frameBytes = new byte[frameSize];
int offset = 0;
frameBytes[offset] = magicSymbol;
offset++;
frameBytes[offset] = channelId;
offset++;
BigEndian.WriteUInt16(frameBytes, offset, dataLength);
offset += 2;
Array.Copy(data, 0, frameBytes, offset, data.Length);
offset += data.Length;
_socket?.Send(frameBytes, 0, frameBytes.Length, SocketFlags.None);
bytesSend += frameBytes.Length;
// Statistic.RtpStats.Update(MediaTimer.GetRelativeTime(), rtp.Length);
}
catch (ObjectDisposedException) { }
}
}
}
unsafe private NALUnit ParseH264Payload(byte[] payload, long timestamp)
{
if (payload == null)
{
return(null);
}
int payloadLength = payload.Length;
if (payloadLength == 0)
{
logger.Warn("payloadLength == 0");
return(null);
}
// RFC6184 Section 6 Packetization Mode:
// 0 or not present: Single NAL mode (Only nals from 1-23 are allowed) - All receivers MUST support this mode.
// 1: Non-interleaved Mode: 1-23, 24 (STAP-A), 28 (FU-A) are allowed. - This mode SHOULD be supported
// 2: Interleaved Mode: 25 (STAP-B), 26 (MTAP16), 27 (MTAP24), 28 (FU-A), and 29 (FU-B) are allowed. - Some receivers MAY support this mode
/*
* 7.1. Single NAL Unit and Non-Interleaved Mode
*
* The receiver includes a receiver buffer to compensate for
* transmission delay jitter. The receiver stores incoming packets in
* reception order into the receiver buffer. Packets are de-packetized
* in RTP sequence number order. If a de-packetized packet is a single
* NAL unit packet, the NAL unit contained in the packet is passed
* directly to the decoder. If a de-packetized packet is an STAP-A, the
* NAL units contained in the packet are passed to the decoder in the
* order in which they are encapsulated in the packet. For all the FU-A
* packets containing fragments of a single NAL unit, the de-packetized
* fragments are concatenated in their sending order to recover the NAL
* unit, which is then passed to the decoder.
*/
/*
* Type Name
* 0[unspecified]
* 1 Coded slice
* 2 Data Partition A
* 3 Data Partition B
* 4 Data Partition C
* 5 IDR(Instantaneous Decoding Refresh) Picture
* 6 SEI(Supplemental Enhancement Information)
* 7 SPS(Sequence Parameter Set)
* 8 PPS(Picture Parameter Set)
* 9 Access Unit Delimiter
* 10 EoS(End of Sequence)
* 11 EoS(End of Stream)
* 12 Filter Data
* 13 - 23[extended]
* 24 - 31[unspecified]
*/
byte firstByte = payload[0];
byte unitType = (byte)(firstByte & 0x1f);
//logger.Verb("unitType " + unitType + " "+ timestamp);
if (unitType >= 1 && unitType <= 23) // в одном пакете - один NAL unit ()
{ // Single NAL Unit Packet
/*
* 0 1 2 3
* 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|F|NRI| Type | |
+-+-+-+-+-+-+-+-+ |
| |
| Bytes 2..n of a single NAL unit |
| |
| +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| :...OPTIONAL RTP padding |
*/
//logger.Debug("unitType = " + unitType);
int size = startSequence.Length + payloadLength;
byte[] nalData = new byte[size];
int offset = 0;
Array.Copy(startSequence, 0, nalData, offset, startSequence.Length);
offset += startSequence.Length;
Array.Copy(payload, 0, nalData, offset, payloadLength);
offset += payloadLength;
//if (unitType == 1)
//{// Coded slice of a non-IDR picture
// //logger.Verb("--------------------- " + arrival);
// // return nalData;
//}
//else if (unitType == 5)
//{// IDR(Instantaneous Decoding Refresh) Picture
// ////logger.Verb(" ========================== isIFrame =============================== " + arrival);
// //return nalUnit;
//}
//else if (unitType == 6)
//{//Supplemental enhancement information (SEI)
//}
//else if (unitType == 7)
//{// Sequence parameter set
//}
//else if (unitType == 8)
//{// Picture parameter set
//}
//else if (unitType == 9)
//{ //Access unit delimiter
// //return nalData;
//}
iFrameFlag = (unitType == 5);
return(new NALUnit
{
data = nalData,
timestamp = timestamp,
type = unitType,
iFrame = iFrameFlag,
});
}
else if (unitType == 28) // фрагментированный пакет (FU-A)
{
/* RFC6184 5.8.Fragmentation Units(FUs)
* 0 1 2 3
* 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| FU indicator | FU header | |
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ |
| |
| FU payload |
| |
| +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| :...OPTIONAL RTP padding |
*/
//logger.Debug("unitType = " + unitType);
if (payloadLength > 1)
{
byte fuHeader = payload[1];
byte startBit = (byte)(fuHeader >> 7);
byte endBit = (byte)((fuHeader & 0x40) >> 6);
//byte forbiddenBit = (byte)((fuHeader & 0x40) >> 7);
byte nalUnitType = (byte)(fuHeader & 0x1f); // идентификатор пакета
int payloadDataOffset = 2; // смещение 2 байта
payloadLength -= payloadDataOffset;
int flags = (nalUnitType == 5) ? 1 : 0;
int fuSize = (startBit == 1) ? (payloadLength + startSequence.Length + 1) : payloadLength;
int offset = 0;
byte[] fuBytes = new byte[fuSize];
if (startBit == 1) //начало фрагмента
{
iFrameFlag = (nalUnitType == 5 && ((fuHeader & 0x80) == 128));
//if (iFrameFlag)
//{
// logger.Verb(" ========================== isIFrame =============================== ");
//}
Array.Copy(startSequence, fuBytes, startSequence.Length);
offset += startSequence.Length;
fuBytes[offset] = (byte)((firstByte & 0xe0) | nalUnitType);
offset++;
}
Array.Copy(payload, payloadDataOffset, fuBytes, offset, payloadLength);
offset += payloadLength;
payloadBuffer.Add(fuBytes);
if (endBit == 1)
{
var totalLength = payloadBuffer.Sum(n => n.Length);
var nalData = new byte[totalLength];
int bufferOffset = 0;
foreach (var buf in payloadBuffer)
{// TODO: убрать копирование в промежуточный буфер
Array.Copy(buf, 0, nalData, bufferOffset, buf.Length);
bufferOffset += buf.Length;
}
payloadBuffer.Clear();
//logger.Verb("--------------------- " + arrival);
return(new NALUnit
{
data = nalData,
timestamp = timestamp,
type = nalUnitType,
iFrame = iFrameFlag
});
}
}
}
else if (unitType == 24) // этот режим может понадобится для приема данных от сторонних RTSP серверов
{ // STAP-A (one packet, multiple nals)
/*
* 0 1 2 3
* 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| RTP Header |
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|STAP-A NAL HDR | NALU 1 Size | NALU 1 HDR |
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| NALU 1 Data |
| : :
+ +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| | NALU 2 Size | NALU 2 HDR |
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| NALU 2 Data |
| : :
| +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| :...OPTIONAL RTP padding |
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
*/
List <byte[]> nals = new List <byte[]>(16);
int naluPointer = 1; //STAP-A NAL HDR
while (naluPointer < payload.Length - 1)
{
int naluSize = BigEndian.ReadInt16(payload, naluPointer); //NALU 1 Size
naluPointer += 2;
if (naluSize > (payload.Length - naluPointer))
{
logger.Error("naluSize > (payload.Length - naluPointer) ");
}
byte[] nal = new byte[naluSize + startSequence.Length];
Array.Copy(startSequence, nal, startSequence.Length);
Array.Copy(payload, naluPointer, nal, startSequence.Length, naluSize); //NALU 1 HDR + NALU 1 Data
naluPointer += naluSize;
nals.Add(nal);
}
var totalLength = nals.Sum(n => n.Length);
var nalData = new byte[totalLength];
{
int offset = 0;
foreach (var buf in nals)
{
Array.Copy(buf, 0, nalData, offset, buf.Length);
offset += buf.Length;
}
}
return(new NALUnit
{
data = nalData,
timestamp = timestamp,
type = unitType,
});
}
if (unitType == 25 || unitType == 26 || unitType == 27 || unitType == 29) // Interleaved Mode
{ // Не нужно
throw new InvalidOperationException("Not supported unit type " + unitType);
}
if (unitType == 0 || unitType == 30 || unitType == 31) // reserved
{ // эти пакеты приходить не должны!
throw new InvalidOperationException("Invalid packet. Reserved unit type " + unitType);
}
return(null);
}
