N58_HPOC/app/poc_code.c

#include "poc_code.h"
#include "netio.h"
#include <stddef.h>
#include <stdio.h>
#include <string.h>

#define MOD_ADLER 45521
#define PTT_VOICE_DATA_FORMAT_2  //?????UDP????,rtp_hdr?????

#ifdef PTT_VOICE_DATA_FORMAT_2
  #define UDP_VOICE_FORMAT_V     2
  #define COOKIE_RESERVE_BYTES  3   // 3???cookie
#endif

T_UINT32 pro_adler32(T_UINT8 *data, size_t len) /* data: Pointer to the data to be summed; len is in bytes */
{
    T_UINT32 a = 1, b = 0;
    size_t index;

    if (NULL == data)
    {
        return 0;
    }

    /* Loop over each byte of data, in order */
    for (index = 0; index < len; ++index)
    {
        a = (a + data[index]) % MOD_ADLER;
        b = (b + a) % MOD_ADLER;
    }

    b = (b & 0xFF);
    a = (a & 0xFF);

    return (b << 8) | a;
}


T_UINT32 packet_encode(
		T_UINT8* buf,T_UINT32 buf_size,
		T_UINT8  name_cmd,
		const T_UINT8* protobuf,T_UINT32 protosize) {


	struct Protocol_Descriptor descriptor;

	if( buf == NULL)
		return 0;

	descriptor.packet_ptr = buf;
	descriptor.name_ptr = (char*)buf;   //?????????
	descriptor.msg_size = protosize;
	descriptor.msg_ptr = buf + sizeof(T_UINT16) + sizeof(T_UINT8);   // 1Byte cmd, 2 Bytes len
	descriptor.packet_size = sizeof(T_UINT8) + protosize + sizeof(T_UINT16);   //  2????,1?????,?????protosize


	if( descriptor.packet_size + sizeof(T_UINT32) > buf_size ) {
        wlog_error( "packet_encode== buf_size:%d,pkt_size:%d", buf_size, descriptor.packet_size);

		return 0;
	}

	write_uint8(descriptor.packet_ptr,  name_cmd);
	write_uint16(descriptor.packet_ptr+sizeof(T_UINT8),descriptor.packet_size);

	if( descriptor.msg_ptr && descriptor.msg_size > 0 ) {
		memcpy(descriptor.msg_ptr,protobuf,descriptor.msg_size);
	}
	descriptor.check_sum = pro_adler32(descriptor.packet_ptr,descriptor.packet_size);
	write_uint16(descriptor.packet_ptr + descriptor.packet_size,descriptor.check_sum);

	return descriptor.packet_size + sizeof(T_UINT16);   //  2 ??CRC
}

T_UINT32 packet_decode(
		T_UINT8* buf,T_UINT32 buf_size,
		struct Protocol_Descriptor* descriptor) {

	T_UINT32 expected_checksum;


	descriptor->packet_ptr = buf;

	descriptor->name_ptr = (char*) descriptor->packet_ptr;

	read_uint16(descriptor->packet_ptr + sizeof(T_UINT8),  &(descriptor->packet_size));


	if( descriptor->packet_size + sizeof(T_UINT16) > buf_size ) {

		return PROTOCOL_ERROR;
	}

	read_uint16(descriptor->packet_ptr + descriptor->packet_size,&(descriptor->check_sum));

	if( descriptor->check_sum != 0 ) {
		expected_checksum = pro_adler32(descriptor->packet_ptr,descriptor->packet_size);
		if( descriptor->check_sum != expected_checksum ) {
			return PROTOCOL_ERROR;
		}
	}

	descriptor->msg_ptr = descriptor->packet_ptr + sizeof(T_UINT16) + sizeof(T_UINT8);
	descriptor->msg_size = descriptor->packet_size - sizeof(T_UINT8) - sizeof(T_UINT16);

	return descriptor->packet_size + sizeof(T_UINT16);   //??2??CRC
}

T_UINT32 hdr_rtp_encode(
		T_UINT8* buf,T_UINT32 buf_size,
		rtp_hdr_t* rtp_hdr) {
	// 1000 0000 0[payload] [seq] [ts] [ssrc]
	// 0x80

	T_UINT8 i = 0;
	T_UINT8 value8 = 0;
	T_UINT32 offset = 0;

	value8 = ( (rtp_hdr->version << 6) & 0xC0) | ((rtp_hdr->p << 5) & 0x20) | ((rtp_hdr->x << 4) & 0x10) | (rtp_hdr->cc & 0x0F) ;
	offset += write_uint8(buf,value8);

	value8 = ((rtp_hdr->pt & 0x7F) | ( rtp_hdr->m << 7) );
	offset += write_uint8(buf+offset,value8);

	offset += write_uint16(buf+offset,rtp_hdr->seq);
	offset += write_uint32(buf+offset,rtp_hdr->ts);
	offset += write_uint32(buf+offset,rtp_hdr->ssrc);

	for(i = 0; i < rtp_hdr->cc; ++i ) {
		if( offset > buf_size - sizeof(T_UINT32) ) {
			offset = 0;
			break;
		}
		offset += write_uint32(buf+offset,rtp_hdr->csrc[i]);
	}
	return offset;
}

T_UINT32 hdr_rtp_decode(const T_UINT8* buf,T_UINT32 buf_size,rtp_hdr_t* rtp_hdr) {

	T_UINT32 offset = 0;
	T_UINT8 i = 0;
	T_UINT8 value8 = 0;

	if( buf == 0 || buf_size < 12 )
		return 0;

	memset(rtp_hdr,0,sizeof(rtp_hdr_t));

	offset += read_uint8(buf+offset,&value8);
	rtp_hdr->version = ((value8 & 0xC0) >> 6);
	rtp_hdr->p = ((value8 & 0x20) >> 5);
	rtp_hdr->x = ((value8 & 0x10) >> 4);
	rtp_hdr->cc = (value8 & 0xF);

	if( buf_size < (sizeof(T_UINT32) * rtp_hdr->cc + 12) )
		return 0;

	offset += read_uint8(buf+offset,&value8);
	rtp_hdr->m = (( value8 & 0x80 ) >> 7);
	rtp_hdr->pt = (value8 & 0x7F);

	offset += read_uint16(buf+offset,&(rtp_hdr->seq));
	offset += read_uint32(buf+offset,&(rtp_hdr->ts));
	offset += read_uint32(buf+offset,&(rtp_hdr->ssrc));
	for(i = 0; i < rtp_hdr->cc; ++i) {
		offset += read_uint32(buf+offset,&(rtp_hdr->csrc[i]));
	}
	return offset;
}


void rtp_hdr_init(rtp_hdr_t* rtp_hdr) {
	memset(rtp_hdr,0,sizeof(rtp_hdr_t));

	rtp_hdr->version = UDP_VOICE_FORMAT_V;
	rtp_hdr->ssrc = get_uid();
}


T_UINT32 pa_rtp_heartbeat(T_BYTE* buffer,int len,int uid)
{
	T_UINT32 l;
	rtp_hdr_t rtp;

	rtp_hdr_init(&rtp);
	rtp.ssrc = uid;
	rtp.pt = CODEC_PROTOBUF;

	l = hdr_rtp_encode(buffer,len,&rtp);
//	l += cp_heartbeat(ptl,buffer + l,len - l);

	return l;
}

T_UINT32 packet_rtp_encode(
			T_UINT8 * buf, T_UINT32 buf_size,
			rtp_hdr_t* rtp_hdr,
			const T_UINT8 * data, T_UINT32 size)
{
	T_UINT32 offset = hdr_rtp_encode(buf,buf_size,rtp_hdr);
	if(offset>0 && offset + size <= buf_size)
	{
		memcpy(buf+offset,data,size);
		offset += size;
	}
	return offset;
}