stun_msg.c

一个开源的sip源代码

PJ_DEF(pj_status_t) pj_stun_msg_create_response(pj_pool_t *pool,
						const pj_stun_msg *req_msg,
						unsigned err_code,
						const pj_str_t *err_msg,
						pj_stun_msg **p_response)
{
    unsigned msg_type = req_msg->hdr.type;
    pj_stun_msg *response = NULL;
    pj_status_t status;

    PJ_ASSERT_RETURN(pool && p_response, PJ_EINVAL);

    PJ_ASSERT_RETURN(PJ_STUN_IS_REQUEST(msg_type), 
		     PJNATH_EINSTUNMSGTYPE);

    /* Create response or error response */
    if (err_code)
	msg_type |= PJ_STUN_ERROR_RESPONSE_BIT;
    else
	msg_type |= PJ_STUN_RESPONSE_BIT;

    status = pj_stun_msg_create(pool, msg_type, req_msg->hdr.magic, 
				req_msg->hdr.tsx_id, &response);
    if (status != PJ_SUCCESS) {
	return status;
    }

    /* Add error code attribute */
    if (err_code) {
	status = pj_stun_msg_add_errcode_attr(pool, response, 
					      err_code, err_msg);
	if (status != PJ_SUCCESS) {
	    return status;
	}
    }

    *p_response = response;
    return PJ_SUCCESS;
}


/*
 * Parse incoming packet into STUN message.
 */
PJ_DEF(pj_status_t) pj_stun_msg_decode(pj_pool_t *pool,
				       const pj_uint8_t *pdu,
				       unsigned pdu_len,
				       unsigned options,
				       pj_stun_msg **p_msg,
				       unsigned *p_parsed_len,
				       pj_stun_msg **p_response)
{
    
    pj_stun_msg *msg;
    unsigned uattr_cnt;
    const pj_uint8_t *start_pdu = pdu;
    pj_bool_t has_msg_int = PJ_FALSE;
    pj_bool_t has_fingerprint = PJ_FALSE;
    pj_status_t status;

    PJ_UNUSED_ARG(options);

    PJ_ASSERT_RETURN(pool && pdu && pdu_len && p_msg, PJ_EINVAL);
    PJ_ASSERT_RETURN(sizeof(pj_stun_msg_hdr) == 20, PJ_EBUG);

    if (p_parsed_len)
	*p_parsed_len = 0;
    if (p_response)
	*p_response = NULL;

    /* Check if this is a STUN message, if necessary */
    if (options & PJ_STUN_CHECK_PACKET) {
	status = pj_stun_msg_check(pdu, pdu_len, options);
	if (status != PJ_SUCCESS)
	    return status;
    }

    /* Create the message, copy the header, and convert to host byte order */
    msg = PJ_POOL_ZALLOC_T(pool, pj_stun_msg);
    pj_memcpy(&msg->hdr, pdu, sizeof(pj_stun_msg_hdr));
    msg->hdr.type = pj_ntohs(msg->hdr.type);
    msg->hdr.length = pj_ntohs(msg->hdr.length);
    msg->hdr.magic = pj_ntohl(msg->hdr.magic);

    pdu += sizeof(pj_stun_msg_hdr);
    /* pdu_len -= sizeof(pj_stun_msg_hdr); */
    pdu_len = msg->hdr.length;

    /* No need to create response if this is not a request */
    if (!PJ_STUN_IS_REQUEST(msg->hdr.type))
	p_response = NULL;

    /* Parse attributes */
    uattr_cnt = 0;
    while (pdu_len >= 4) {
	unsigned attr_type, attr_val_len;
	const struct attr_desc *adesc;

	/* Get attribute type and length. If length is not aligned
	 * to 4 bytes boundary, add padding.
	 */
	attr_type = GETVAL16H(pdu, 0);
	attr_val_len = GETVAL16H(pdu, 2);
	attr_val_len = (attr_val_len + 3) & (~3);

	/* Check length */
	if (pdu_len < attr_val_len) {
	    pj_str_t err_msg;
	    char err_msg_buf[80];

	    err_msg.ptr = err_msg_buf;
	    err_msg.slen = pj_ansi_snprintf(err_msg_buf, sizeof(err_msg_buf),
					    "Attribute %s has invalid length",
					    pj_stun_get_attr_name(attr_type));

	    PJ_LOG(4,(THIS_FILE, "Error decoding message: %.*s",
		      (int)err_msg.slen, err_msg.ptr));

	    if (p_response) {
		pj_stun_msg_create_response(pool, msg, 
					    PJ_STUN_SC_BAD_REQUEST, 
					    &err_msg, p_response);
	    }
	    return PJNATH_ESTUNINATTRLEN;
	}

	/* Get the attribute descriptor */
	adesc = find_attr_desc(attr_type);

	if (adesc == NULL) {
	    /* Unrecognized attribute */

	    PJ_LOG(4,(THIS_FILE, "Unrecognized attribute type %d", 
		      attr_type));

	    /* Is this a fatal condition? */
	    if (attr_type <= 0x7FFF) {
		/* This is a mandatory attribute, we must return error
		 * if we don't understand the attribute.
		 */
		if (p_response) {
		    unsigned err_code = PJ_STUN_SC_UNKNOWN_ATTRIBUTE;

		    status = pj_stun_msg_create_response(pool, msg,
							 err_code, NULL, 
							 p_response);
		    if (status==PJ_SUCCESS) {
			pj_uint16_t d = (pj_uint16_t)attr_type;
			pj_stun_msg_add_unknown_attr(pool, *p_response, 1, &d);
		    }
		}

		return PJ_STATUS_FROM_STUN_CODE(PJ_STUN_SC_UNKNOWN_ATTRIBUTE);
	    }

	} else {
	    void *attr;
	    char err_msg1[PJ_ERR_MSG_SIZE],
		 err_msg2[PJ_ERR_MSG_SIZE];

	    /* Parse the attribute */
	    status = (adesc->decode_attr)(pool, pdu, &attr);

	    if (status != PJ_SUCCESS) {
		pj_strerror(status, err_msg1, sizeof(err_msg1));

		if (p_response) {
		    pj_str_t e;

		    e.ptr = err_msg2;
		    e.slen= pj_ansi_snprintf(err_msg2, sizeof(err_msg2),
					     "%s in %s",
					     err_msg1,
					     pj_stun_get_attr_name(attr_type));

		    pj_stun_msg_create_response(pool, msg,
						PJ_STUN_SC_BAD_REQUEST,
						&e, p_response);
		}

		PJ_LOG(4,(THIS_FILE, 
			  "Error parsing STUN attribute %s: %s",
			  pj_stun_get_attr_name(attr_type), 
			  err_msg1));

		return status;
	    }

	    if (attr_type == PJ_STUN_ATTR_MESSAGE_INTEGRITY && 
		!has_fingerprint) 
	    {
		if (has_msg_int) {
		    /* Already has MESSAGE-INTEGRITY */
		    if (p_response) {
			pj_stun_msg_create_response(pool, msg,
						    PJ_STUN_SC_BAD_REQUEST,
						    NULL, p_response);
		    }
		    return PJNATH_ESTUNDUPATTR;
		}
		has_msg_int = PJ_TRUE;

	    } else if (attr_type == PJ_STUN_ATTR_FINGERPRINT) {
		if (has_fingerprint) {
		    /* Already has FINGERPRINT */
		    if (p_response) {
			pj_stun_msg_create_response(pool, msg,
						    PJ_STUN_SC_BAD_REQUEST,
						    NULL, p_response);
		    }
		    return PJNATH_ESTUNDUPATTR;
		}
		has_fingerprint = PJ_TRUE;
	    } else {
		if (has_msg_int || has_fingerprint) {
		    /* Another attribute is found which is not FINGERPRINT
		     * after FINGERPRINT or MESSAGE-INTEGRITY */
		    if (p_response) {
			pj_stun_msg_create_response(pool, msg,
						    PJ_STUN_SC_BAD_REQUEST,
						    NULL, p_response);
		    }
		    return has_fingerprint ? PJNATH_ESTUNFINGERPOS :
					     PJNATH_ESTUNMSGINTPOS;
		}
	    }

	    /* Make sure we have rooms for the new attribute */
	    if (msg->attr_count >= PJ_STUN_MAX_ATTR) {
		if (p_response) {
		    pj_stun_msg_create_response(pool, msg,
						PJ_STUN_SC_BAD_REQUEST,
						NULL, p_response);
		}
		return PJNATH_ESTUNTOOMANYATTR;
	    }

	    /* Add the attribute */
	    msg->attr[msg->attr_count++] = (pj_stun_attr_hdr*)attr;
	}

	if (attr_val_len + 4 >= pdu_len) {
	    pdu += pdu_len;
	    pdu_len = 0;
	} else {
	    pdu += (attr_val_len + 4);
	    pdu_len -= (attr_val_len + 4);
	}
    }

    if (pdu_len > 0) {
	/* Stray trailing bytes */
	PJ_LOG(4,(THIS_FILE, 
		  "Error decoding STUN message: unparsed trailing %d bytes",
		  pdu_len));
	return PJNATH_EINSTUNMSGLEN;
    }

    *p_msg = msg;

    if (p_parsed_len)
	*p_parsed_len = (pdu - start_pdu);

    return PJ_SUCCESS;
}

/* Calculate HMAC-SHA1 key for long term credential, by getting
 * MD5 digest of username, realm, and password. 
 */
static void calc_md5_key(pj_uint8_t digest[16],
			 const pj_str_t *realm,
			 const pj_str_t *username,
			 const pj_str_t *passwd)
{
    /* The 16-byte key for MESSAGE-INTEGRITY HMAC is formed by taking
     * the MD5 hash of the result of concatenating the following five
     * fields: (1) The username, with any quotes and trailing nulls
     * removed, (2) A single colon, (3) The realm, with any quotes and
     * trailing nulls removed, (4) A single colon, and (5) The 
     * password, with any trailing nulls removed.
     */
    pj_md5_context ctx;
    pj_str_t s;

    pj_md5_init(&ctx);

#define REMOVE_QUOTE(s)	if (s.slen && *s.ptr=='"') \
			    s.ptr++, s.slen--; \
			if (s.slen && s.ptr[s.slen-1]=='"') \
			    s.slen--;

    /* Add username */
    s = *username;
    REMOVE_QUOTE(s);
    pj_md5_update(&ctx, (pj_uint8_t*)s.ptr, s.slen);

    /* Add single colon */
    pj_md5_update(&ctx, (pj_uint8_t*)":", 1);

    /* Add realm */
    s = *realm;
    REMOVE_QUOTE(s);
    pj_md5_update(&ctx, (pj_uint8_t*)s.ptr, s.slen);

#undef REMOVE_QUOTE

    /* Another colon */
    pj_md5_update(&ctx, (pj_uint8_t*)":", 1);

    /* Add password */
    pj_md5_update(&ctx, (pj_uint8_t*)passwd->ptr, passwd->slen);

    /* Done */
    pj_md5_final(&ctx, digest);
}


/*
 * Create authentication key to be used for encoding the message with
 * MESSAGE-INTEGRITY. 
 */
PJ_DEF(void) pj_stun_create_key(pj_pool_t *pool,
				pj_str_t *key,
				const pj_str_t *realm,
				const pj_str_t *username,
				const pj_str_t *passwd)
{
    PJ_ASSERT_ON_FAIL(pool && key && username && passwd, return);

    if (realm && realm->slen) {
	key->ptr = (char*) pj_pool_alloc(pool, 16);
	calc_md5_key((pj_uint8_t*)key->ptr, realm, username, passwd);
	key->slen = 16;
    } else {
	pj_strdup(pool, key, passwd);
    }
}


/*
static char *print_binary(const pj_uint8_t *data, unsigned data_len)
{
    static char static_buffer[1024];
    char *buffer = static_buffer;
    unsigned length=sizeof(static_buffer), i;

    if (length < data_len * 2 + 8)
	return "";

    pj_ansi_sprintf(buffer, ", data=");
    buffer += 7;

    for (i=0; i<data_len; ++i) {
	pj_ansi_sprintf(buffer, "%02x", (*data) & 0xFF);
	buffer += 2;
	data++;
    }

    pj_ansi_sprintf(buffer, "\n");
    buffer++;

    return static_buffer;
}
*/

/*
 * Print the message structure to a buffer.
 */
PJ_DEF(pj_status_t) pj_stun_msg_encode(pj_stun_msg *msg,
				       pj_uint8_t *buf, unsigned buf_size,
				       unsigned options,
				       const pj_str_t *key,
				       unsigned *p_msg_len)
{
    pj_uint8_t *start = buf;
    pj_stun_msgint_attr *amsgint = NULL;
    pj_stun_fingerprint_attr *afingerprint = NULL;
    unsigned printed = 0, body_len;
    pj_status_t status;
    unsigned i;


    PJ_ASSERT_RETURN(msg && buf && buf_size, PJ_EINVAL);

    PJ_UNUSED_ARG(options);
    PJ_ASSERT_RETURN(options == 0, PJ_EINVAL);

    /* Copy the message header part and convert the header fields to
     * network byte order
     */
    if (buf_size < sizeof(pj_stun_msg_hdr))
	return PJ_ETOOSMALL;
    
    PUTVAL16H(buf, 0, msg->hdr.type);
    PUTVAL16H(buf, 2, 0);   /* length will be calculated later */
    PUTVAL32H(buf, 4, msg->hdr.magic);
    pj_memcpy(buf+8, msg->hdr.tsx_id, sizeof(msg->hdr.tsx_id));

    buf += sizeof(pj_stun_msg_hdr);
    buf_size -= sizeof(pj_stun_msg_hdr);

    /* Encode each attribute to the message */
    for (i=0; i<msg->attr_count; ++i) {
	const struct attr_desc *adesc;
	const pj_stun_attr_hdr *attr_hdr = msg->attr[i];

	if (attr_hdr->type == PJ_STUN_ATTR_MESSAGE_INTEGRITY) {
	    pj_assert(amsgint == NULL);
	    amsgint = (pj_stun_msgint_attr*) attr_hdr;

	    /* Stop when encountering MESSAGE-INTEGRITY */
	    break;

	} else if (attr_hdr->type == PJ_STUN_ATTR_FINGERPRINT) {
	    afingerprint = (pj_stun_fingerprint_attr*) attr_hdr;
	    break;
	}

	adesc = find_attr_desc(attr_hdr->type);
	PJ_ASSERT_RETURN(adesc != NULL, PJ_EBUG);

	status = adesc->encode_attr(attr_hdr, buf, buf_size, &printed);
	if (status != PJ_SUCCESS)
	    return status;

	buf += printed;
	buf_size -= printed;
    }

    /* We may have stopped printing attribute because we found
     * MESSAGE-INTEGRITY or FINGERPRINT. Scan the rest of the
     * attributes.
     */
    for ( ++i; i<msg->attr_count; ++i) {
	const pj_stun_attr_hdr *attr_hdr = msg->attr[i];

	/* There mustn't any attribute after FINGERPRINT */
	PJ_ASSERT_RETURN(afingerprint == NULL, PJNATH_ESTUNFINGERPOS);

	if (attr_hdr->type == PJ_STUN_ATTR_MESSAGE_INTEGRITY) {
	    /* There mustn't be MESSAGE-INTEGRITY before */
	    PJ_ASSERT_RETURN(amsgint == NULL, 
			     PJNATH_ESTUNMSGINTPOS);
	    amsgint = (pj_stun_msgint_attr*) attr_hdr;

	} else if (attr_hdr->type == PJ_STUN_ATTR_FINGERPRINT) {
	    afingerprint = (pj_stun_fingerprint_attr*) attr_hdr;
	}
    }

    /* We MUST update the message length in the header NOW before
     * calculating MESSAGE-INTEGRITY and FINGERPRINT. 
     * Note that length is not including the 20 bytes header.
     */
    if (amsgint && afingerprint) {
	body_len = (pj_uint16_t)((buf - start) - 20 + 24 + 8);
    } else if (amsgint) {
	body_len = (pj_uint16_t)((buf - start) - 20 + 24);
    } else if (afingerprint) {
	body_len = (pj_uint16_t)((buf - start) - 20 + 8);
    } else {
	body_len = (pj_uint16_t)((buf - start) - 20);
    }

    /* hdr->length = pj_htons(length); */
    PUTVAL16H(start, 2, (pj_uint16_t)body_len);

    /* Calculate message integrity, if present */
    if (amsgint != NULL) {
	pj_hmac_sha1_context ctx;

	/* Key MUST be specified */
	PJ_ASSERT_RETURN(key, PJ_EINVALIDOP);

	/* MESSAGE-INTEGRITY must be the last attribute in the message, or
	 * the last attribute before FINGERPRINT.
	 */
	if (i < msg->attr_count-2) {
	    /* Should not happen for message generated by us */
	    pj_assert(PJ_FALSE);
	    return PJNATH_ESTUNMSGINTPOS;

	} else if (i == msg->attr_count-2)  {
	    if (msg->attr[i+1]->type != PJ_STUN_ATTR_FINGERPRINT) {
		/* Should not happen for message generated by us */
		pj_assert(PJ_FALSE);
		return PJNATH_ESTUNMSGINTPOS;
	    } else {
		afingerprint = (pj_stun_fingerprint_attr*) msg->attr[i+1];
	    }
	}

	/* Calculate HMAC-SHA1 digest, add zero padding to input
	 * if necessary to make the input 64 bytes aligned.
	 */
	pj_hmac_sha1_init(&ctx, (pj_uint8_t*)key->ptr, key->slen);
	pj_hmac_sha1_update(&ctx, (pj_uint8_t*)start, buf-start);
	if ((buf-start) & 0x3F) {
	    pj_uint8_t zeroes[64];
	    pj_bzero(zeroes, sizeof(zeroes));
	    pj_hmac_sha1_update(&ctx, zeroes, 64-((buf-start) & 0x3F));
	}
	pj_hmac_sha1_final(&ctx, amsgint->hmac);

	/* Put this attribute in the message */
	status = encode_msgint_attr(amsgint, buf, buf_size, 
			            &printed);
	if (status != PJ_SUCCESS)
	    return status;

	buf += printed;
	buf_size -= printed;
    }

    /* Calculate FINGERPRINT if present */
    if (afingerprint != NULL) {
	afingerprint->value = pj_crc32_calc(start, buf-start);
	afingerprint->value ^= STUN_XOR_FINGERPRINT;

	/* Put this attribute in the message */
	status = encode_uint_attr(afingerprint, buf, buf_size, 
				          &printed);
	if (status != PJ_SUCCESS)
	    return status;

	buf += printed;
	buf_size -= printed;
    }

    /* Update message length. */
    msg->hdr.length = (pj_uint16_t) ((buf - start) - 20);

    /* Return the length */
    if (p_msg_len)
	*p_msg_len = (buf - start);

    return PJ_SUCCESS;
}


/*
 * Find STUN attribute in the STUN message, starting from the specified
 * index.
 */
PJ_DEF(pj_stun_attr_hdr*) pj_stun_msg_find_attr( const pj_stun_msg *msg,
						 int attr_type,
						 unsigned index)
{
    PJ_ASSERT_RETURN(msg, NULL);

    for (; index < msg->attr_count; ++index) {
	if (msg->attr[index]->type == attr_type)
	    return (pj_stun_attr_hdr*) msg->attr[index];
    }

    return NULL;
}