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); }