stun_msg.c

来自「一个开源的sip源代码」· C语言 代码 · 共 2,239 行 · 第 1/4 页

    if (stun_err_msg_map[first].err_code == err_code) {
	return pj_str((char*)stun_err_msg_map[first].err_msg);
    } else {
	return pj_str(NULL);
    }
#endif
}


//////////////////////////////////////////////////////////////////////////////


#define INIT_ATTR(a,t,l)    (a)->hdr.type=(pj_uint16_t)(t), \
			    (a)->hdr.length=(pj_uint16_t)(l)
#define ATTR_HDR_LEN	    4

static pj_uint16_t GETVAL16H(const pj_uint8_t *buf, unsigned pos)
{
    return (pj_uint16_t) ((buf[pos + 0] << 8) | \
			  (buf[pos + 1] << 0));
}

static pj_uint16_t GETVAL16N(const pj_uint8_t *buf, unsigned pos)
{
    return pj_htons(GETVAL16H(buf,pos));
}

static void PUTVAL16H(pj_uint8_t *buf, unsigned pos, pj_uint16_t hval)
{
    buf[pos+0] = (pj_uint8_t) ((hval & 0xFF00) >> 8);
    buf[pos+1] = (pj_uint8_t) ((hval & 0x00FF) >> 0);
}

static pj_uint32_t GETVAL32H(const pj_uint8_t *buf, unsigned pos)
{
    return (pj_uint32_t) ((buf[pos + 0] << 24UL) | \
	                  (buf[pos + 1] << 16UL) | \
	                  (buf[pos + 2] <<  8UL) | \
			  (buf[pos + 3] <<  0UL));
}

static pj_uint32_t GETVAL32N(const pj_uint8_t *buf, unsigned pos)
{
    return pj_htonl(GETVAL32H(buf,pos));
}

static void PUTVAL32H(pj_uint8_t *buf, unsigned pos, pj_uint32_t hval)
{
    buf[pos+0] = (pj_uint8_t) ((hval & 0xFF000000UL) >> 24);
    buf[pos+1] = (pj_uint8_t) ((hval & 0x00FF0000UL) >> 16);
    buf[pos+2] = (pj_uint8_t) ((hval & 0x0000FF00UL) >>  8);
    buf[pos+3] = (pj_uint8_t) ((hval & 0x000000FFUL) >>  0);
}

static void GETVAL64H(const pj_uint8_t *buf, unsigned pos, pj_timestamp *ts)
{
    ts->u32.hi = GETVAL32H(buf, pos);
    ts->u32.lo = GETVAL32H(buf, pos+4);
}

static void PUTVAL64H(pj_uint8_t *buf, unsigned pos, const pj_timestamp *ts)
{
    PUTVAL32H(buf, pos, ts->u32.hi);
    PUTVAL32H(buf, pos+4, ts->u32.lo);
}


static void GETATTRHDR(const pj_uint8_t *buf, pj_stun_attr_hdr *hdr)
{
    hdr->type = GETVAL16H(buf, 0);
    hdr->length = GETVAL16H(buf, 2);
}

//////////////////////////////////////////////////////////////////////////////
/*
 * STUN generic IP address container
 */
#define STUN_GENERIC_IP_ADDR_LEN	8

/*
 * Create a generic STUN IP address attribute for IPv4 address.
 */
PJ_DEF(pj_status_t) 
pj_stun_sockaddr_attr_create(pj_pool_t *pool,
			     int attr_type,
			     pj_bool_t xor_ed,
			     const pj_sockaddr_t *addr,
			     unsigned addr_len,
			     pj_stun_sockaddr_attr **p_attr)
{
    pj_stun_sockaddr_attr *attr;

    PJ_ASSERT_RETURN(pool && addr_len && addr && p_attr, PJ_EINVAL);
    PJ_ASSERT_RETURN(addr_len == sizeof(pj_sockaddr_in) ||
		     addr_len == sizeof(pj_sockaddr_in6), PJ_EINVAL);

    attr = PJ_POOL_ZALLOC_T(pool, pj_stun_sockaddr_attr);
    INIT_ATTR(attr, attr_type, STUN_GENERIC_IP_ADDR_LEN);

    pj_memcpy(&attr->sockaddr, addr, addr_len);
    attr->xor_ed = xor_ed;

    *p_attr = attr;

    return PJ_SUCCESS;
}


/*
 * Create and add generic STUN IP address attribute to a STUN message.
 */
PJ_DEF(pj_status_t) 
pj_stun_msg_add_sockaddr_attr(pj_pool_t *pool,
			      pj_stun_msg *msg,
			      int attr_type, 
			      pj_bool_t xor_ed,
			      const pj_sockaddr_t *addr,
			      unsigned addr_len)
{
    pj_stun_sockaddr_attr *attr;
    pj_status_t status;

    status = pj_stun_sockaddr_attr_create(pool, attr_type, xor_ed,
					         addr, addr_len, &attr);
    if (status != PJ_SUCCESS)
	return status;

    return pj_stun_msg_add_attr(msg, &attr->hdr);
}

static pj_status_t decode_sockaddr_attr(pj_pool_t *pool, 
				        const pj_uint8_t *buf, 
				        void **p_attr)
{
    pj_stun_sockaddr_attr *attr;
    pj_uint32_t val;

    /* Create the attribute */
    attr = PJ_POOL_ZALLOC_T(pool, pj_stun_sockaddr_attr);
    GETATTRHDR(buf, &attr->hdr);

    /* Check that the attribute length is valid */
    if (attr->hdr.length != STUN_GENERIC_IP_ADDR_LEN)
	return PJNATH_ESTUNINATTRLEN;

    /* Check address family */
    val = *(pj_uint8_t*)(buf + ATTR_HDR_LEN + 1);

    /* Check address family is valid (only supports ipv4 for now) */
    if (val != 1)
	return PJNATH_ESTUNIPV6NOTSUPP;

    /* Get port and address */
    pj_sockaddr_in_init(&attr->sockaddr.ipv4, NULL, 0);
    attr->sockaddr.ipv4.sin_port = GETVAL16N(buf, ATTR_HDR_LEN+2);
    attr->sockaddr.ipv4.sin_addr.s_addr = GETVAL32N(buf, ATTR_HDR_LEN+4);

    /* Done */
    *p_attr = (void*)attr;

    return PJ_SUCCESS;
}


static pj_status_t decode_xored_sockaddr_attr(pj_pool_t *pool, 
					      const pj_uint8_t *buf, 
					      void **p_attr)
{
    pj_stun_sockaddr_attr *attr;
    pj_status_t status;

    status = decode_sockaddr_attr(pool, buf, p_attr);
    if (status != PJ_SUCCESS)
	return status;

    attr = *(pj_stun_sockaddr_attr**)p_attr;

    attr->xor_ed = PJ_TRUE;
    attr->sockaddr.ipv4.sin_port ^= pj_htons(0x2112);
    attr->sockaddr.ipv4.sin_addr.s_addr ^= pj_htonl(0x2112A442);

    /* Done */
    *p_attr = attr;

    return PJ_SUCCESS;
}


static pj_status_t encode_sockaddr_attr(const void *a, pj_uint8_t *buf, 
				        unsigned len, unsigned *printed)
{
    enum {
	ATTR_LEN = ATTR_HDR_LEN + STUN_GENERIC_IP_ADDR_LEN
    };
    pj_uint8_t *start_buf = buf;
    const pj_stun_sockaddr_attr *ca = 
	(const pj_stun_sockaddr_attr *)a;

    if (len < ATTR_LEN) 
	return PJ_ETOOSMALL;

    /* Copy and convert headers to network byte order */
    PUTVAL16H(buf, 0, ca->hdr.type);
    PUTVAL16H(buf, 2, STUN_GENERIC_IP_ADDR_LEN);
    buf += ATTR_HDR_LEN;
    
    /* Ignored */
    *buf++ = '\0';

    /* Family (IPv4 only for now) */
    PJ_ASSERT_RETURN(ca->sockaddr.addr.sa_family == PJ_AF_INET, PJ_EINVAL);
    *buf++ = 1;

    if (ca->xor_ed) {
	pj_uint32_t addr;
	pj_uint16_t port;

	addr = ca->sockaddr.ipv4.sin_addr.s_addr;
	port = ca->sockaddr.ipv4.sin_port;

	port ^= pj_htons(0x2112);
	addr ^= pj_htonl(0x2112A442);

	/* Port */
	pj_memcpy(buf, &port, 2);
	buf += 2;

	/* Address */
	pj_memcpy(buf, &addr, 4);
	buf += 4;

    } else {
	/* Port */
	pj_memcpy(buf, &ca->sockaddr.ipv4.sin_port, 2);
	buf += 2;

	/* Address */
	pj_memcpy(buf, &ca->sockaddr.ipv4.sin_addr, 4);
	buf += 4;
    }

    pj_assert(buf - start_buf == ATTR_LEN);

    /* Done */
    *printed = buf - start_buf;

    return PJ_SUCCESS;
}


//////////////////////////////////////////////////////////////////////////////
/*
 * STUN generic string attribute
 */

/*
 * Create a STUN generic string attribute.
 */
PJ_DEF(pj_status_t) 
pj_stun_string_attr_create(pj_pool_t *pool,
			   int attr_type,
			   const pj_str_t *value,
			   pj_stun_string_attr **p_attr)
{
    pj_stun_string_attr *attr;

    PJ_ASSERT_RETURN(pool && value && p_attr, PJ_EINVAL);

    attr = PJ_POOL_ZALLOC_T(pool, pj_stun_string_attr);
    INIT_ATTR(attr, attr_type, value->slen);
    pj_strdup(pool, &attr->value, value);

    *p_attr = attr;

    return PJ_SUCCESS;
}


/*
 * Create and add STUN generic string attribute to the message.
 */
PJ_DEF(pj_status_t) 
pj_stun_msg_add_string_attr(pj_pool_t *pool,
			    pj_stun_msg *msg,
			    int attr_type,
			    const pj_str_t *value)
{
    pj_stun_string_attr *attr = NULL;
    pj_status_t status;

    status = pj_stun_string_attr_create(pool, attr_type, value, 
						&attr);
    if (status != PJ_SUCCESS)
	return status;

    return pj_stun_msg_add_attr(msg, &attr->hdr);
}


static pj_status_t decode_string_attr(pj_pool_t *pool, 
				      const pj_uint8_t *buf, 
				      void **p_attr)
{
    pj_stun_string_attr *attr;
    pj_str_t value;

    /* Create the attribute */
    attr = PJ_POOL_ZALLOC_T(pool, pj_stun_string_attr);
    GETATTRHDR(buf, &attr->hdr);

    /* Get pointer to the string in the message */
    value.ptr = ((char*)buf + ATTR_HDR_LEN);
    value.slen = attr->hdr.length;

    /* Copy the string to the attribute */
    pj_strdup(pool, &attr->value, &value);

    /* Done */
    *p_attr = attr;

    return PJ_SUCCESS;

}


static pj_status_t encode_string_attr(const void *a, pj_uint8_t *buf, 
				      unsigned len, unsigned *printed)
{
    const pj_stun_string_attr *ca = 
	(const pj_stun_string_attr*)a;

    /* Calculated total attr_len (add padding if necessary) */
    *printed = (ca->value.slen + ATTR_HDR_LEN + 3) & (~3);
    if (len < *printed) {
	*printed = 0;
	return PJ_ETOOSMALL;
    }

    PUTVAL16H(buf, 0, ca->hdr.type);
    PUTVAL16H(buf, 2, (pj_uint16_t)ca->value.slen);
    

    /* Copy the string */
    pj_memcpy(buf+ATTR_HDR_LEN, ca->value.ptr, ca->value.slen);

    /* Done */
    return PJ_SUCCESS;
}


//////////////////////////////////////////////////////////////////////////////
/*
 * STUN empty attribute (used by USE-CANDIDATE).
 */

/*
 * Create a STUN empty attribute.
 */
PJ_DEF(pj_status_t) 
pj_stun_empty_attr_create(pj_pool_t *pool,
			  int attr_type,
			  pj_stun_empty_attr **p_attr)
{
    pj_stun_empty_attr *attr;

    PJ_ASSERT_RETURN(pool && p_attr, PJ_EINVAL);

    attr = PJ_POOL_ZALLOC_T(pool, pj_stun_empty_attr);
    INIT_ATTR(attr, attr_type, 0);

    *p_attr = attr;

    return PJ_SUCCESS;
}


/*
 * Create STUN empty attribute and add the attribute to the message.
 */
PJ_DEF(pj_status_t) pj_stun_msg_add_empty_attr( pj_pool_t *pool,
						pj_stun_msg *msg,
						int attr_type)
{
    pj_stun_empty_attr *attr = NULL;
    pj_status_t status;

    status = pj_stun_empty_attr_create(pool, attr_type, &attr);
    if (status != PJ_SUCCESS)
	return status;

    return pj_stun_msg_add_attr(msg, &attr->hdr);
}

static pj_status_t decode_empty_attr(pj_pool_t *pool, 
				     const pj_uint8_t *buf, 
				     void **p_attr)
{
    pj_stun_empty_attr *attr;

    /* Check that the struct address is valid */
    pj_assert(sizeof(pj_stun_empty_attr) == ATTR_HDR_LEN);

    /* Create the attribute */
    attr = PJ_POOL_ZALLOC_T(pool, pj_stun_empty_attr);
    GETATTRHDR(buf, &attr->hdr);

    /* Check that the attribute length is valid */
    if (attr->hdr.length != 0)
	return PJNATH_ESTUNINATTRLEN;

    /* Done */
    *p_attr = attr;

    return PJ_SUCCESS;
}


static pj_status_t encode_empty_attr(const void *a, pj_uint8_t *buf, 
				     unsigned len, unsigned *printed)
{
    const pj_stun_empty_attr *ca = (pj_stun_empty_attr*)a;

    if (len < ATTR_HDR_LEN) 
	return PJ_ETOOSMALL;

    PUTVAL16H(buf, 0, ca->hdr.type);
    PUTVAL16H(buf, 2, 0);

    /* Done */
    *printed = ATTR_HDR_LEN;

    return PJ_SUCCESS;
}


//////////////////////////////////////////////////////////////////////////////
/*
 * STUN generic 32bit integer attribute.
 */

/*
 * Create a STUN generic 32bit value attribute.
 */
PJ_DEF(pj_status_t) 
pj_stun_uint_attr_create(pj_pool_t *pool,
			 int attr_type,
			 pj_uint32_t value,
			 pj_stun_uint_attr **p_attr)
{
    pj_stun_uint_attr *attr;

    PJ_ASSERT_RETURN(pool && p_attr, PJ_EINVAL);

    attr = PJ_POOL_ZALLOC_T(pool, pj_stun_uint_attr);
    INIT_ATTR(attr, attr_type, 4);
    attr->value = value;

    *p_attr = attr;

    return PJ_SUCCESS;
}

/* Create and add STUN generic 32bit value attribute to the message. */
PJ_DEF(pj_status_t) 
pj_stun_msg_add_uint_attr(pj_pool_t *pool,
			  pj_stun_msg *msg,
			  int attr_type,
			  pj_uint32_t value)
{
    pj_stun_uint_attr *attr = NULL;
    pj_status_t status;

    status = pj_stun_uint_attr_create(pool, attr_type, value, &attr);
    if (status != PJ_SUCCESS)
	return status;

    return pj_stun_msg_add_attr(msg, &attr->hdr);
}

static pj_status_t decode_uint_attr(pj_pool_t *pool, 
				    const pj_uint8_t *buf, 
				    void **p_attr)
{
    pj_stun_uint_attr *attr;

    /* Create the attribute */
    attr = PJ_POOL_ZALLOC_T(pool, pj_stun_uint_attr);
    GETATTRHDR(buf, &attr->hdr);

    attr->value = GETVAL32H(buf, 4);

    /* Check that the attribute length is valid */
    if (attr->hdr.length != 4)
	return PJNATH_ESTUNINATTRLEN;

    /* Done */
    *p_attr = attr;

    return PJ_SUCCESS;
}


static pj_status_t encode_uint_attr(const void *a, pj_uint8_t *buf, 
				    unsigned len, unsigned *printed)
{
    const pj_stun_uint_attr *ca = (const pj_stun_uint_attr*)a;

    if (len < 8) 
	return PJ_ETOOSMALL;

    PUTVAL16H(buf, 0, ca->hdr.type);
    PUTVAL16H(buf, 2, (pj_uint16_t)4);
    PUTVAL32H(buf, 4, ca->value);
    
    /* Done */
    *printed = 8;

    return PJ_SUCCESS;
}

//////////////////////////////////////////////////////////////////////////////

/*
 * Create a STUN generic 64bit value attribute.
 */
PJ_DEF(pj_status_t) 
pj_stun_uint64_attr_create(pj_pool_t *pool,
			   int attr_type,
			   const pj_timestamp *value,
			   pj_stun_uint64_attr **p_attr)
{
    pj_stun_uint64_attr *attr;

    PJ_ASSERT_RETURN(pool && p_attr, PJ_EINVAL);

    attr = PJ_POOL_ZALLOC_T(pool, pj_stun_uint64_attr);
    INIT_ATTR(attr, attr_type, 8);

    if (value) {
	attr->value.u32.hi = value->u32.hi;
	attr->value.u32.lo = value->u32.lo;
    }

    *p_attr = attr;

    return PJ_SUCCESS;
}

/* Create and add STUN generic 64bit value attribute to the message. */
PJ_DEF(pj_status_t)  pj_stun_msg_add_uint64_attr(pj_pool_t *pool,
					         pj_stun_msg *msg,
					         int attr_type,
					         const pj_timestamp *value)
{
    pj_stun_uint64_attr *attr = NULL;
    pj_status_t status;

    status = pj_stun_uint64_attr_create(pool, attr_type, value, &attr);
    if (status != PJ_SUCCESS)