rsvpte_packet.c

Linux平台下

#include "rsvpte_struct.h"

inline void htonl_nba(u_char * buf, u_int * extra) {
    u_int temp;
    memcpy(&temp, buf, sizeof(u_int));
    temp = htonl(temp);
    memcpy(buf, &temp, sizeof(u_int));
    if (extra) {
	(*extra) = temp;
    }
}

inline void ntohl_nba(u_char * buf, u_int * extra) {
    u_int temp;
    memcpy(&temp, buf, sizeof(u_int));
    temp = ntohl(temp);
    memcpy(buf, &temp, sizeof(u_int));
    if (extra) {
	(*extra) = temp;
    }
}

inline void htons_nba(u_char * buf, u_short * extra) {
    u_short temp;
    memcpy(&temp, buf, sizeof(u_short));
    temp = htons(temp);
    memcpy(buf, &temp, sizeof(u_short));
    if (extra) {
	(*extra) = temp;
    }
}

inline void ntohs_nba(u_char * buf, u_short * extra) {
    u_int temp;
    memcpy(&temp, buf, sizeof(u_short));
    temp = ntohs(temp);
    memcpy(buf, &temp, sizeof(u_short));
    if (extra) {
	(*extra) = temp;
    }
}

/* Message Header */

int rsvpteEncodeMsgHeader(rsvpHeader_t * header, u_char * buf, int bufSize) {
    rsvpHeader_t headerCopy;

    if (RSVP_HDRSIZE > bufSize) {
	return MPLS_ENC_BUFFTOOSMALL;
    }

    headerCopy = *header;
    headerCopy.checksum = htons(header->checksum);
    headerCopy.length = htons(header->length);

    memcpy(buf, &headerCopy, RSVP_HDRSIZE);
    return RSVP_HDRSIZE;
}

int rsvpteDecodeMsgHeader(rsvpHeader_t * header, u_char * buf, int bufSize) {
    if (RSVP_HDRSIZE > bufSize) {
	return MPLS_DEC_BUFFTOOSMALL;
    }

    memcpy((u_char*)header, buf, RSVP_HDRSIZE);
    header->checksum = ntohs(header->checksum);
    header->length = ntohs(header->length);

    if (header->length > RSVP_PDUMAXLEN) {
	return MPLS_PDU_LENGTH_ERROR;
    }
    return RSVP_HDRSIZE;
}

/* Object Header */

int rsvpteEncodeObjectHeader(rsvpObjectHeader_t * header, u_char * buf, int bufSize) {
    rsvpObjectHeader_t headerCopy;

    if (RSVP_OBJECT_HDRSIZE > bufSize) {
	return MPLS_ENC_BUFFTOOSMALL;
    }

    headerCopy = *header;
    headerCopy.length = htons(header->length);

    memcpy(buf, &headerCopy, RSVP_OBJECT_HDRSIZE);
    return RSVP_OBJECT_HDRSIZE;
}

int rsvpteDecodeObjectHeader(rsvpObjectHeader_t * header, u_char * buf, int bufSize) {
    if (RSVP_OBJECT_HDRSIZE > bufSize) {
	return MPLS_DEC_BUFFTOOSMALL;
    }

    memcpy(header, buf, RSVP_OBJECT_HDRSIZE);
    header->length = ntohs(header->length);
    return RSVP_OBJECT_HDRSIZE;
}

/* Session Object */

int rsvpteEncodeSession4(rsvpSession4_t * session, u_char * buf, int bufSize) {
    rsvpSession4_t sessionCopy;

    if (RSVP_SESSION4_SIZE > bufSize) {
	return MPLS_ENC_BUFFTOOSMALL;
    }
    sessionCopy = *session;

    sessionCopy.destAddr = htonl(session->destAddr);
    sessionCopy.destPort = htons(session->destPort);

    memcpy(buf, &sessionCopy, RSVP_SESSION4_SIZE);
    return RSVP_SESSION4_SIZE;
}

int rsvpteDecodeSession4(rsvpSession4_t * session, u_char * buf, int bufSize) {
    if (RSVP_SESSION4_SIZE > bufSize) {
	return MPLS_DEC_BUFFTOOSMALL;
    }

    memcpy(session, buf, RSVP_SESSION4_SIZE);
    session->destAddr = nthol(session->destAddr);
    session->destPort = nthos(session->destPort);

    return RSVP_SESSION4_SIZE;
}

int rsvpteEncodeSession6(rsvpSession6_t * session, u_char * buf, int bufSize) {
    rsvpSession6_t sessionCopy;
    if (RSVP_SESSION6_SIZE > bufSize) {
	return MPLS_ENC_BUFFTOOSMALL;
    }
    sessionCopy = *session;

    sessionCopy.destPort = htons(session->destPort);

    memcpy(buf, &sessionCopy, RSVP_SESSION4_SIZE);
    return RSVP_SESSION4_SIZE;
}

int rsvpteDecodeSession6(rsvpSession6_t * session, u_char * buf, int bufSize) {
    if (RSVP_SESSION6_SIZE > bufSize) {
	return MPLS_DEC_BUFFTOOSMALL;
    }

    memcpy(session, buf, RSVP_SESSION4_SIZE);
    session->destPort = nthos(session->destPort);

    return RSVP_SESSION4_SIZE;
}

int rsvpteEncodeSessionTunnel4(rsvpSessionTunnel4_t * session, u_char * buf, int bufSize) {
    rsvpSessionTunnel4_t sessionCopy;
    if (RSVP_SESSION_TUNNEL4_SIZE > bufSize) {
	return MPLS_ENC_BUFFTOOSMALL;
    }
    sessionCopy = *session;

    sessionCopy.endPointAddress = htonl(session->endPointAddress);
    sessionCopy.reserved = 0;
    sessionCopy.tunnelId = htons(session->tunnelId);
    sessionCopy.extentedTunnelId = htonl(session->destPort);

    memcpy(buf, &sessionCopy, RSVP_SESSION_TUNNEL4_SIZE);
    return RSVP_SESSION_TUNNEL4_SIZE;
}

int rsvpteDecodeSessionTunnel4(rsvpSessionTunnel4_t * session, u_char * buf, int bufSize) {
    if (RSVP_SESSION_TUNNEL4_SIZE > bufSize) {
	return MPLS_DEC_BUFFTOOSMALL;
    }

    memcpy(session, buf, RSVP_SESSION_TUNNEL4_SIZE);
    session->endPointAddress = nthol(session->endPointAddress);
    session->reserved = nthos(session->reserved);
    session->tunnelId = nthos(session->tunnelId);
    session->extentedTunnelId = nthol(session->extentedTunnelId);

    return RSVP_SESSION_TUNNEL4_SIZE;
}

int rsvpteEncodeSessionTunnel6(rsvpSessionTunnel6_t * session, u_char * buf, int bufSize) {
    rsvpSessionTunnel6_t sessionCopy;
    if (RSVP_SESSION_TUNNEL6_SIZE > bufSize) {
	return MPLS_ENC_BUFFTOOSMALL;
    }
    sessionCopy = *session;

    sessionCopy.reserved = 0;
    sessionCopy.tunnelId = htons(session->tunnelId);

    memcpy(buf, &sessionCopy, RSVP_SESSION_TUNNEL6_SIZE);
    return RSVP_SESSION_TUNNEL6_SIZE;
}

int rsvpteDecodeSessionTunnel6(rsvpSessionTunnel6_t * session, u_char * buf, int bufSize) {
    if (RSVP_SESSION_TUNNEL6_SIZE > bufSize) {
	return MPLS_DEC_BUFFTOOSMALL;
    }

    memcpy(session, buf, RSVP_SESSION_TUNNEL6_SIZE);
    session->reserved = nthos(session->reserved);
    session->tunnelId = nthos(session->tunnelId);

    return RSVP_SESSION_TUNNEL6_SIZE;
}

int rsvpteEncodeSessionObject(rsvpSessionObject_t * session, u_char * buf, int bufSize) {
    int encodedSize = 0;
    char *bufHdr = buf;
    int size;

    if (RSVP_OBJECT_HDRSIZE > bufSize) {
	return MPLS_ENC_BUFFTOOSMALL;
    }

    size = RSVP_OBJECT_HDRSIZE;
    encodeSize += size;
    bufSize -= size;
    buf += size;

    switch(session->hdr.cType) {
	case RSVP_SESSION_CTYPE_IPV4:
	    size = rsvpteEncodeSession4(&session->u.ipv4, buf, bufSize);
	    break;
	case RSVP_SESSION_CTYPE_IPV6:
	    size = rsvpteEncodeSession6(&session->u.ipv6, buf, bufSize);
	    break;
	case RSVP_SESSION_CTYPE_TUNNEL4:
	    size = rsvpteEncodeSessionTunnel4(&session->u.tunnel4, buf, bufSize);
	    break;
	case RSVP_SESSION_CTYPE_TUNNEL6:
	    size = rsvpteEncodeSessionTunnel6(&session->u.tunnel6, buf, bufSize);
	    break;
  	default:
	    return RSVP_ENC_OBJECTERROR;
    }

    if (size < 0) {
	return size;
    }

    encodeSize += size;

    session->hdr.length = encodeSize;
    session->hdr.class = RSVP_SESSION_CLASS;
    size = rsvpteEncodeObjectHeader(&session->hdr, bufHdr, RSVP_OBJECT_HDRSIZE);
    if (size < 0) {
	return size;
    }

    return encodeSize;
}

int rsvpteDecodeSessionObject(rsvpSessionObject_t * session, u_char * buf, int bufSize) {
    int decodedSize = 0;

    size = rsvpteDecodeObjectHeader(&session->hdr, buf, bufSize);
    if (size < 0) {
	return size;
    }
    decodeSize += size;
    bufSize -= size;
    buf += size;

    if (session->hdr.length > bufSize) {
	return MPLS_DEC_BUFFTOOSMALL;
    }

    if (session->hdr.class != RSVP_SESSION_CLASS) {
	return RSVP_DEC_OBJECTERROR;
    }

    switch(session->hdr.cType) {
	case RSVP_SESSION_CTYPE_IPV4:
	    size = rsvpteDecodeSession4(&session->u.ipv4, buf, bufSize);
	    break;
	case RSVP_SESSION_CTYPE_IPV6:
	    size = rsvpteDecodeSession6(&session->u.ipv6, buf, bufSize);
	    break;
	case RSVP_SESSION_CTYPE_TUNNEL4:
	    size = rsvpteDecodeSessionTunnel4(&session->u.tunnel4, buf, bufSize);
	    break;
	case RSVP_SESSION_CTYPE_TUNNEL6:
	    size = rsvpteDecodeSessionTunnel6(&session->u.tunnel6, buf, bufSize);
	    break;
  	default:
	    return RSVP_DEC_OBJECTERROR;
    }

    if (size < 0) {
	return size;
    }

    return decodeSize + size;
}

int rsvpteEncodeHop4(rsvpHop4_t * hop, u_char * buf, int bufSize) {
    rsvpHop4_t hopCopy;
    if (RSVP_HOP4_SIZE > bufSize) {
	return MPLS_ENC_BUFFTOOSMALL;
    }
    hopCopy = *hop;

    hopCopy.hopAddr = htonl(hop->hopAddr);
    hopCopy.logicalIfHandle = htonl(hop->logicalIfHandle);
    memcpy(buf, &hopCopy, RSVP_HOP4_SIZE);

    return RSVP_HOP4_SIZE;
}

int rsvpteDecodeHop4(rsvpHop4_t * hop, u_char * buf, int bufSize) {
    if (RSVP_HOP4_SIZE > bufSize) {
	return MPLS_DEC_BUFFTOOSMALL;
    }

    memcpy(hop, buf, RSVP_HOP4_SIZE);
    hop->hopAddr = nthol(hop->hopAddr);
    hop->logicalIfHandle = nthol(hop->logicalIfHandle);

    return RSVP_HOP4_SIZE;
}

int rsvpteEncodeHop6(rsvpHop6_t * hop, u_char * buf, int bufSize) {
    rsvpHop6_t hopCopy;
    if (RSVP_HOP6_SIZE > bufSize) {
	return MPLS_ENC_BUFFTOOSMALL;
    }
    hopCopy = *hop;

    hopCopy.logicalIfHandle = htonl(hop->logicalIfHandle);
    memcpy(buf, &hopCopy, RSVP_HOP6_SIZE);

    return RSVP_HOP6_SIZE;
}

int rsvpteDecodeHop6(rsvpHop6_t * hop, u_char * buf, int bufSize) {
    if (RSVP_HOP6_SIZE > bufSize) {
	return MPLS_DEC_BUFFTOOSMALL;
    }

    memcpy(hop, buf, RSVP_HOP6_SIZE);
    hop->logicalIfHandle = nthol(hop->logicalIfHandle);

    return RSVP_HOP6_SIZE;
}

int rsvpteEncodeHopObject(rsvpHopObject_t * hop, u_char * buf, int bufSize) {
    int encodedSize = 0;
    char *bufHdr = buf;
    int size;

    if (RSVP_OBJECT_HDRSIZE > bufSize) {
        return MPLS_ENC_BUFFTOOSMALL;
    }

    size = RSVP_OBJECT_HDRSIZE;
    encodeSize += size;
    bufSize -= size;
    buf += size;

    switch(hop->hdr.cType) {
	case RSVP_HOP_CTYPE_IPV4:
	    size = rsvpteEncodeHop4(&hop->u.ipv4, buf, bufSize);
	    break;
	case RSVP_HOP_CTYPE_IPV6:
	    size = rsvpteEncodeHop6(&hop->u.ipv6, buf, bufSize);
	    break;
  	default:
	    return RSVP_ENC_OBJECTERROR;
    }

    if (size < 0) {
        return size;
    }

    encodeSize += size;

    session->hdr.length = encodeSize;
    session->hdr.class = RSVP_HOP_CLASS;
    size = rsvpteEncodeObjectHeader(&session->hdr, bufHdr, RSVP_OBJECT_HDRSIZE);    if (size < 0) {
        return size;
    }

    return encodeSize;
}

int rsvpteDecodeHopObject(rsvpHopObject_t * hop, u_char * buf, int bufSize) {
    int decodedSize = 0;
    int size = rsvpteDecodeObjectHeader(&session->hdr, buf, bufSize);

    if (size < 0) {
        return size;
    }
    decodeSize += size;
    bufSize -= size;
    buf += size;

    if (session->hdr.length > bufSize) {
        return MPLS_DEC_BUFFTOOSMALL;
    }

    if (session->hdr.class != RSVP_HOP_CLASS) {
        return RSVP_DEC_OBJECTERROR;
    }

    switch(session->hdr.cType) {
	case RSVP_HOP_CTYPE_IPV4:
	    size = rsvpteDecodeHop4(&hop->u.ipv4, buf, bufSize);
	    break;
	case RSVP_HOP_CTYPE_IPV6:
	    size = rsvpteDecodeHop4(&hop->u.ipv6, buf, bufSize);
	    break;
  	default:
	    return RSVP_DEC_OBJECTERROR;
    }

    if (size < 0) {
        return size;
    }

    return decodeSize + size;
}

/* Time Object */

int rsvpteEncodeTimeObject(rsvpTimeObject_t * time, u_char * buf, int bufSize) {
    int encodedSize = 0;
    char *bufHdr = buf;
    int size;

    if (RSVP_OBJECT_HDRSIZE > bufSize) {
        return MPLS_ENC_BUFFTOOSMALL;
    }

    size = RSVP_OBJECT_HDRSIZE;
    encodeSize += size;
    bufSize -= size;
    buf += size;

    {
	u_int refresh = htonl(time->refresh);
	if (RSVP_TIME_SIZE > bufSize) {
	    return MPLS_DEC_BUFFTOOSMALL;
	}
	memcpy(buf, &refresh, RSVP_TIME_SIZE);
	size = RSVP_TIME_SIZE;
    }
    encodeSize += size;

    session->hdr.length = encodeSize;
    session->hdr.class = RSVP_TIME_CLASS;
    session->hdr.cType = RSVP_TIME_CTYPE;

    size = rsvpteEncodeObjectHeader(&session->hdr, bufHdr, RSVP_OBJECT_HDRSIZE);    if (size < 0) {
        return size;
    }

    return encodeSize;
}

int rsvpteDecodeTimeObject(rsvpTimeObject_t * time, u_char * buf, int bufSize) {
    int decodedSize = 0;
    int size = rsvpteDecodeObjectHeader(&session->hdr, buf, bufSize);

    if (size < 0) {
        return size;
    }
    decodeSize += size;
    bufSize -= size;
    buf += size;

    if (session->hdr.length > bufSize) {
        return MPLS_DEC_BUFFTOOSMALL;
    }

    if (session->hdr.class != RSVP_TIME_CLASS) {
        return RSVP_DEC_OBJECTERROR;
    }

    switch(time->hdr.cType) {
	case RSVP_TIME_CTYPE:
	    if (RSVP_TIME_SIZE > bufSize) {
		return MPLS_DEC_BUFFTOOSMALL;
	    }
	    memcpy(&time->refresh, buf, RSVP_TIME_SIZE);
	    time->refresh = ntohl(time->refresh);
	    size = RSVP_TIME_SIZE;
	    break;
  	default:
	    return RSVP_DEC_OBJECTERROR;
    }

    return decodeSize + size;
}

/* Explicit Route Object */

int rsvpteEncodeExplicitRoute4(rsvpExplicitRoute4_t * er, u_char * buf, int bufSize) {
    rsvpExplicitRoute4_t erCopy;

    if (RSVP_ER4_SIZE > bufSize) {
	return MPLS_ENC_BUFFTOOSMALL;
    }
    erCopy = *er;

    erCopy.flags.mark = htons(er->flags.mark);
    htonl_nba(erCopy.address, &erCopy.uIntAddress);

    memcpy(buf, &erCopy, RSVP_ER4_SIZE);

    return RSVP_ER4_SIZE;
}

int rsvpteDecodeExplicitRoute4(rsvpExplicitRoute4_t * er, u_char * buf, int bufSize) {
    if (RSVP_ER4_SIZE > bufSize) {
	return MPLS_DEC_BUFFTOOSMALL;
    }

    memcpy(er, buf, RSVP_ER4_SIZE);
    er->flags.mark = ntohs(er->flags.mark);
    ntohl_nba(er->address, &er->uIntAddress);

    return RSVP_ER4_SIZE;
}