void SendQueryTimePacked(int sockfd, struct sockaddr_in *addr_src)
{
NTPPACKED SynNtpPacked;
int ticks;
int timer;
memset(&SynNtpPacked, 0, sizeof(SynNtpPacked));
SynNtpPacked.header.ntptd.ntptime.local_precision = -6;
SynNtpPacked.header.ntptd.ntptime.Poll = 4;
SynNtpPacked.header.ntptd.ntptime.stratum = 0;
SynNtpPacked.header.ntptd.ntptime.Mode = 3;
SynNtpPacked.header.ntptd.ntptime.VN = 3;
SynNtpPacked.header.ntptd.ntptime.LI = 0;
SynNtpPacked.root_delay = 1<<16; /* Root Delay (seconds) */
SynNtpPacked.root_dispersion = 1<<16; /* Root Dispersion (seconds) */
SynNtpPacked.header.ntptd.headData = htonl(SynNtpPacked.header.ntptd.headData);
SynNtpPacked.root_delay = htonl(SynNtpPacked.root_dispersion);
SynNtpPacked.root_dispersion = htonl(SynNtpPacked.root_dispersion);
ticks = OSTimeGet();
timer = ticks / OS_TICKS_PER_SEC ;
SynNtpPacked.trantime.coarse = htonl((long)timer + JAN_1970); /* Transmit Timestamp coarse */
SynNtpPacked.trantime.fine = htonl(NTPFRAC(timer)); /* Transmit Timestamp fine */
sendto(sockfd, &SynNtpPacked, sizeof(SynNtpPacked), 0, addr_src, sizeof(struct sockaddr));
}
static void send_packet(int sd)
{
uint32_t data[12];
struct timeval now;
memset(data, 0, sizeof(data));
data[0] = htonl (
( LI << 30 ) | ( VN << 27 ) | ( MODE << 24 ) |
( STRATUM << 16) | ( POLL << 8 ) | ( PREC & 0xff ) );
data[1] = htonl(1<<16); /* Root Delay (seconds) */
data[2] = htonl(1<<16); /* Root Dispersion (seconds) */
gettimeofday(&now,NULL);
data[10] = htonl(now.tv_sec + JAN_1970); /* Transmit Timestamp coarse */
data[11] = htonl(NTPFRAC(now.tv_usec)); /* Transmit Timestamp fine */
send(sd,data,48,0);
}
void send_packet(int usd)
{
__u32 data[12];
struct timeval now;
#define LI 0
#define VN 3
#define MODE 3
#define STRATUM 0
#define POLL 4
#define PREC -6
bzero((char *) data,sizeof(data));
data[0] = htonl (
( LI << 30 ) | ( VN << 27 ) | ( MODE << 24 ) |
( STRATUM << 16) | ( POLL << 8 ) | ( PREC & 0xff ) );
data[1] = htonl(1<<16); /* Root Delay (seconds) */
data[2] = htonl(1<<16); /* Root Dispersion (seconds) */
gettimeofday(&now,NULL);
data[10] = htonl(now.tv_sec + JAN_1970); /* Transmit Timestamp coarse */
data[11] = htonl(NTPFRAC(now.tv_usec)); /* Transmit Timestamp fine */
send(usd,data,48,0);
}
int
u_ntp_req(int usd)
{
__u32 data[12];
struct timeval now;
#define LI 0
#define VN 3
#define MODE 3
#define STRATUM 0
#define POLL 4
#define PREC -6
bzero((char *) data,sizeof(data));
data[0] = htonl (
( LI << 30 ) | ( VN << 27 ) | ( MODE << 24 ) |
( STRATUM << 16) | ( POLL << 8 ) | ( PREC & 0xff ) );
data[1] = htonl(1<<16);
data[2] = htonl(1<<16);
gettimeofday(&now,NULL);
data[10] = htonl(now.tv_sec + JAN_1970);
data[11] = htonl(NTPFRAC(now.tv_usec));
return send(usd, data, 48, 0);
}
static int _get_packet(unsigned char *packet, int *len)
{
uint32_t data[12];
struct timeval_t now = {0, 0};
if (*len < 48) {
log_err("packet buf too short!\n");
return -1;
}
memset(packet, 0, *len);
data[0] = _htonl((LI << 30) | (VN << 27) | (MODE << 24) |
(STRATUM << 16) | (POLL << 8) | (PREC & 0xff));
data[1] = _htonl(1 << 16); /* Root Delay (seconds) */
data[2] = _htonl(1 << 16); /* Root Dispersion (seconds) */
data[10] = _htonl(now.tv_sec + JAN_1970); /* Transmit Timestamp coarse */
data[11] = _htonl(NTPFRAC(now.tv_usec)); /* Transmit Timestamp fine */
memcpy(packet, data, 48);
*len = 48;
return 0;
}
s32 mpls_echo_input(struct sk_buff * skb)
{
s32 err = 0;
s32 temp = 0;
u32 saddr = 0;
struct timeval tv;
mpls_echo_hdr * echo;
struct ethhdr * eth ;
struct fib_mg_res res ;
struct iphdr *iph = skb->nh.iph;
struct udphdr * udph = skb->h.uh;
echo = (mpls_echo_hdr *)(skb->data + sizeof(struct udphdr));
skb_push(skb, iph->ihl * 4);
err = fib_mg_lookup(if_dev_vrf(skb->dev)->vrf_id, iph->saddr, 32, &res);
if (err)
{
printk("mpls_echo_input -fib_mg_lookup drop.\n");
goto drop;
}
temp = iph->daddr;
iph->daddr = iph->saddr;
if (res.dev)
{
iph->saddr = inet_select_addr(res.dev, res.nexthop, RT_SCOPE_LINK);
}
else
{
iph->saddr = temp;
}
iph->ttl = 255;
iph->check = 0;
iph->check = ip_fast_csum(iph, iph->ihl);
echo->type = MPLS_ECHO_REPLY;
do_gettimeofday(&tv);
echo->t_rcvd[0] = htonl(tv.tv_sec+JAN_1970);
echo->t_rcvd[1] = NTPFRAC(tv.tv_usec);
temp = udph->source;
udph->source = udph->dest;
udph->dest = temp;
if (MPLS_MODE_DO_NOT_REPLY == echo->mode)
{
printk("mpls_echo_input -not reply mode drop.\n");
goto drop;
}
if (MPLS_MODE_IPX_UDP_ALERT == echo->mode)
{
//Push router alert to ip option.
}
skb->protocol = htons(ETH_P_IP);
if (FIB_MG_TYPE_FTN_BASIC == res.type)
{
if (MPLS_IMPLICIT_NULL != MPLS_LABEL(res.glabel))
{
skb->protocol = htons(ETH_P_MPLS_UC) ;
skb_push_label(skb, 128, res.glabel, 1);
}
if (2 == res.count)
{
if (MPLS_IMPLICIT_NULL != MPLS_LABEL(res.glabel2))
{
skb->protocol = htons(ETH_P_MPLS_UC) ;
skb_push_label(skb, 128, res.glabel2, 0);
}
}
}
else
{
skb->protocol = htons(ETH_P_MPLS_UC) ;
skb_push_label(skb, 128, res.label, 1);
if (MPLS_IMPLICIT_NULL != MPLS_LABEL(res.glabel))
{
skb_push_label(skb, 128, res.glabel, 0);
}
if (3 == res.count)
{
if (MPLS_IMPLICIT_NULL != MPLS_LABEL(res.glabel2))
{
skb->protocol = htons(ETH_P_MPLS_UC) ;
skb_push_label(skb, 128, res.glabel2, 0);
}
}
}
if (MPLS_MODE_CTRL_CHANNEL == echo->mode)
{
skb_push_label(skb, 128, 1, 0);
}
skb->nh.raw = skb->data;
skb->dev = res.dev;
if (!res.neigh)
{
res.neigh = __neigh_arp_lookup(&arp_tbl, &res.nexthop, NULL, res.dev, NEIGHBOUR_CREAT);
}
read_lock_bh(&res.neigh->lock);
if (!(res.neigh->nud_state&(NUD_CONNECTED|NUD_DELAY|NUD_PROBE)))
{
saddr = inet_select_addr(res.dev, res.nexthop, RT_SCOPE_LINK);
arp_send(ARPOP_REQUEST, ETH_P_ARP, res.nexthop, res.dev, saddr,
res.neigh->ha, res.dev->dev_addr, NULL);
read_unlock_bh(&res.neigh->lock);
printk("mpls_echo_input -neigh invalid drop.\n");
goto drop;
}
eth = (struct ethhdr *)skb_push(skb, ETH_HLEN);
eth->h_proto = (ETH_P_802_3 != skb->protocol) ? htons(skb->protocol) : htons(skb->len);
memcpy(eth->h_source, res.dev->dev_addr, res.dev->addr_len);
memcpy(eth->h_dest, res.neigh->ha, res.dev->addr_len);
skb->mac.raw = skb->data ;
skb->mac_len = ETH_HLEN;
read_unlock_bh(&res.neigh->lock);
neigh_release(res.neigh);
dev_queue_xmit(skb);
return 0;
drop:
kfree_skb(skb);
return -1;
}
/* Send a EAPOL-Key packet */
void
eapol_key(nas_t *nas, nas_sta_t *sta,
unsigned char *send_key, int send_key_len,
unsigned char *recv_key, int recv_key_len,
unsigned char *key, int key_len, int index, int unicast)
{
struct iovec packet;
eapol_header_t *eapol;
eapol_key_header_t *body;
struct timeval tv;
struct timezone tz;
unsigned short length;
unsigned int replay[2];
unsigned char rc4_seed[48] = { 0 };
rc4_ks_t rc4_key;
/* Allocate packet */
packet.iov_len = EAPOL_HEADER_LEN + EAPOL_KEY_HEADER_LEN;
if (key)
packet.iov_len += key_len;
if (!(packet.iov_base = (caddr_t) malloc(packet.iov_len))) {
perror("malloc");
return;
}
/* Fill EAPOL header */
eapol = (eapol_header_t *) packet.iov_base;
memcpy(&eapol->eth.ether_dhost, &sta->ea, ETHER_ADDR_LEN);
memcpy(&eapol->eth.ether_shost, &nas->ea, ETHER_ADDR_LEN);
if (sta->flags & STA_FLAG_PRE_AUTH)
eapol->eth.ether_type = htons(ETHER_TYPE_802_1X_PREAUTH);
else
eapol->eth.ether_type = htons(ETHER_TYPE_802_1X);
eapol->version = sta->eapol_version;
eapol->type = EAPOL_KEY;
eapol->length = htons(packet.iov_len - EAPOL_HEADER_LEN);
/* Fill EAPOL-Key header */
body = (eapol_key_header_t *) eapol->body;
body->type = EAPOL_RC4_KEY;
/* Length field is unaligned */
length = htons(key_len);
memcpy(&body->length, &length, sizeof(body->length));
/* Replay Counter field is unaligned */
gettimeofday(&tv, &tz);
/*
* keep track timestamp locally in case gettimeofday() does not return
* correct usec value, for example, on vx. It is ok to not adjust
* tv.tv_sec since tv.tv_usec is not going to overflow anyway.
*/
if (tv.tv_usec == sta->rc4keyusec && tv.tv_sec == sta->rc4keysec) {
tv.tv_usec += sta->rc4keycntr;
sta->rc4keycntr ++;
}
else {
sta->rc4keysec = tv.tv_sec;
sta->rc4keyusec = tv.tv_usec;
sta->rc4keycntr = 1;
}
replay[0] = htonl(tv.tv_sec + JAN_1970);
replay[1] = htonl(NTPFRAC(tv.tv_usec));
memcpy(body->replay, replay, sizeof(body->replay));
/* Fill Key IV */
nas_rand128(body->iv);
/* Fill Key Index */
body->index = index;
if (unicast)
body->index |= EAPOL_KEY_UNICAST;
/* Encrypt Key */
if (key) {
memcpy(rc4_seed, body->iv, 16);
memcpy(&rc4_seed[16], recv_key, recv_key_len);
prepare_key(rc4_seed, 16 + recv_key_len, &rc4_key);
memcpy(body->key, key, key_len);
rc4(body->key, key_len, &rc4_key);
}
/* Calculate HMAC-MD5 checksum with null signature */
if (send_key) {
memset(body->signature, 0, 16);
hmac_md5(&eapol->version, packet.iov_len - OFFSETOF(eapol_header_t, version),
send_key, send_key_len, body->signature);
}
if (sta->flags & STA_FLAG_PRE_AUTH)
nas_preauth_send_packet(nas, &packet, 1);
else
nas_eapol_send_packet(nas, &packet, 1);
free(packet.iov_base);
}
