rawsock_lamp.c

// Rawsock library, licensed under GPLv2
// Version 0.3.4
#include "rawsock.h"
#include "rawsock_lamp.h"
#include "minirighi_udp_checksum.h"
#include <sys/time.h>
#include <string.h>

void lampHeadPopulate(struct lamphdr *lampHeader, unsigned char ctrl, unsigned short id, unsigned short seq) {
    lampHeader->reserved=PROTO_LAMP;
    lampHeader->ctrl=(uint8_t) ctrl;
    lampHeader->id=htons(id);
    lampHeader->seq=htons(seq);
    lampHeader->len=0;

    lampHeader->sec=0;
    lampHeader->usec=0;
}

void lampHeadSetConnType(struct lamphdr *initLampHeader, uint16_t mode_index) {
    // If packet type is not INIT, ignore any operation on the LaMP header
    if(initLampHeader->ctrl==CTRL_CONN_INIT && IS_INIT_INDEX_VALID(mode_index)) {
        initLampHeader->len=htons(mode_index);
    }
}

void lampHeadSetFollowupCtrlType(struct lamphdr *followupLampHeader, uint16_t followup_type) {
    // If packet type is not FOLLOWUP_CTRL, ignore any operation on the LaMP header
    if(followupLampHeader->ctrl==CTRL_FOLLOWUP_CTRL && IS_FOLLOWUP_CTRL_TYPE_VALID(followup_type)) {
        followupLampHeader->len=htons(followup_type);
    }
}

void lampHeadSetTimestamp(struct lamphdr *lampHeader, struct timeval *tStampPtr) {
    struct timeval currtime;

    if(lampHeader->ctrl!=CTRL_PINGLIKE_REQ_TLESS && lampHeader->ctrl!=CTRL_PINGLIKE_REPLY_TLESS && lampHeader->ctrl!=CTRL_PINGLIKE_ENDREQ_TLESS && lampHeader->ctrl!=CTRL_PINGLIKE_ENDREPLY_TLESS) {
        if(tStampPtr==NULL) {
            gettimeofday(&currtime,NULL); // Set timestamp as very last operation, only if it is not a TLESS packet

            lampHeader->sec=hton64((uint64_t) currtime.tv_sec);
            lampHeader->usec=hton64((uint64_t) currtime.tv_usec);
        } else {
            lampHeader->sec=hton64((uint64_t) tStampPtr->tv_sec);
            lampHeader->usec=hton64((uint64_t) tStampPtr->tv_usec);
        }
    }
}

void lampHeadIncreaseSeq(struct lamphdr *inpacket_headerptr) {
    uint16_t seq=htons(inpacket_headerptr->seq); // Get current sequence number in the proper bit order
    // Take into account ciclicity in the sequence numbers
    if(inpacket_headerptr->seq==UINT16_MAX) {
        seq=0;
    } else {
        seq++;
    }
    inpacket_headerptr->seq=ntohs(seq);
}

void lampSetUnidirStop(struct lamphdr *lampHeader) {
    lampHeader->ctrl=CTRL_UNIDIR_STOP;
}

void lampSetPinglikeEndreq(struct lamphdr *lampHeader) {
    lampHeader->ctrl=CTRL_PINGLIKE_ENDREQ;
}

void lampSetPinglikeEndreqTless(struct lamphdr *lampHeader) {
    lampHeader->ctrl=CTRL_PINGLIKE_ENDREQ_TLESS;
}

void lampSetPinglikeEndreqAll(struct lamphdr *lampHeader) {
    if(lampHeader->ctrl==CTRL_PINGLIKE_REQ) {
        lampHeader->ctrl=CTRL_PINGLIKE_ENDREQ;
    } else if(lampHeader->ctrl==CTRL_PINGLIKE_REQ_TLESS) {
        lampHeader->ctrl=CTRL_PINGLIKE_ENDREQ_TLESS;
    }
}

void lampEncapsulate(byte_t *packet, struct lamphdr *lampHeader, byte_t *data, size_t payloadsize) {
    lampHeader->len=htons(payloadsize);

    memcpy(packet,lampHeader,sizeof(struct lamphdr));
    memcpy(packet+sizeof(struct lamphdr),data,payloadsize);
}

int rawLampSend(int descriptor, struct sockaddr_ll addrll, struct lamphdr *inpacket_headerptr, byte_t *ethernetpacket, size_t finalpacketsize, endflag_t end_flag, protocol_t llprot) {
    struct timeval currtime;
    struct udphdr *inpacket_headerptr_udp;
    struct iphdr *inpacket_headerptr_ipv4;
    size_t packetsize;

    if(IS_UNIDIR(inpacket_headerptr->ctrl) && end_flag==FLG_STOP) {
        inpacket_headerptr->ctrl=CTRL_UNIDIR_STOP;
    } else if(IS_PINGLIKE(inpacket_headerptr->ctrl) && end_flag==FLG_STOP) {
        if(inpacket_headerptr->ctrl==CTRL_PINGLIKE_REQ) {
            inpacket_headerptr->ctrl=CTRL_PINGLIKE_ENDREQ;
        } else if(inpacket_headerptr->ctrl==CTRL_PINGLIKE_REQ_TLESS) {
            inpacket_headerptr->ctrl=CTRL_PINGLIKE_ENDREQ_TLESS;
        }
    }

    if(IS_UNIDIR(inpacket_headerptr->ctrl) || inpacket_headerptr->ctrl==CTRL_PINGLIKE_REQ || inpacket_headerptr->ctrl==CTRL_PINGLIKE_ENDREQ) {
        gettimeofday(&currtime,NULL); // Set timestamp as very last operation, only if it is not a ping-like reply

        inpacket_headerptr->sec=hton64((uint64_t) currtime.tv_sec);
        inpacket_headerptr->usec=hton64((uint64_t) currtime.tv_usec);
    }

    // Compute again the checksum depending on the lower layer protocol (UDP is supported as of now)
    switch(llprot) {
        case UDP:
            // Try to obtain the UDP header pointer by subtracting a certain offset to the LaMP inpacket_headerptr
            inpacket_headerptr_udp=(struct udphdr *) ((byte_t *)inpacket_headerptr-sizeof(struct udphdr));
            // Try to obtain the IPv4 header pointer by subtracting a certain offset to the LaMP inpacket_headerptr
            inpacket_headerptr_ipv4=(struct iphdr *) ((byte_t *)inpacket_headerptr_udp-sizeof(struct iphdr));

            inpacket_headerptr_udp->check=0;
            if(IS_INIT(inpacket_headerptr->ctrl) || IS_FOLLOWUP_CTRL(inpacket_headerptr->ctrl)) {
                packetsize=sizeof(struct udphdr)+LAMP_HDR_SIZE();
            } else {
                packetsize=sizeof(struct udphdr)+LAMP_HDR_PAYLOAD_SIZE(ntohs(inpacket_headerptr->len));
            }

            inpacket_headerptr_udp->check=minirighi_udp_checksum(inpacket_headerptr_udp,packetsize,inpacket_headerptr_ipv4->saddr,inpacket_headerptr_ipv4->daddr);
        break;
        default: // case UNSET_P -> do nothing
        break;
    }

    return (sendto(descriptor,ethernetpacket,finalpacketsize,0,(struct sockaddr *)&addrll,sizeof(struct sockaddr_ll))!=finalpacketsize);
}

void lampHeadGetData(byte_t *lampPacket, lamptype_t *type, unsigned short *id, unsigned short *seq, unsigned short *len, struct timeval *timestamp, byte_t *payload) {
    struct lamphdr *lampHeader=(struct lamphdr *) lampPacket;
    byte_t *payloadptr=(lampPacket+LAMP_HDR_SIZE());

    if(type) *type=(lampHeader->ctrl) & 0x0F;
    if(id) *id=ntohs(lampHeader->id);
    if(seq) *seq=ntohs(lampHeader->seq);
    if(len) *len=ntohs(lampHeader->len);
    if(timestamp) {
        timestamp->tv_sec=(time_t) ntoh64(lampHeader->sec);
        timestamp->tv_usec=(suseconds_t) ntoh64(lampHeader->usec);
    }

    if(payload && lampHeader->len!=0x00) {
        memcpy(payload,payloadptr,lampHeader->len);
    }
}

byte_t *lampGetPacketPointers(byte_t *pktbuf,struct lamphdr **lampHeader) {
    byte_t *payload;

    *lampHeader=(struct lamphdr*) pktbuf;
    payload=pktbuf+sizeof(struct lamphdr);

    return payload;
}