Separate this functionality out of the net.c behemoth Signed-off-by: Joe Hershberger <joe.hershberger@ni.com>master
Joe Hershberger
13 years ago
parent
eafc8db0e3
commit
f575ae1f7d
@ -0,0 +1,375 @@ |
||||
/*
|
||||
* Copied from Linux Monitor (LiMon) - Networking. |
||||
* |
||||
* Copyright 1994 - 2000 Neil Russell. |
||||
* (See License) |
||||
* Copyright 2000 Roland Borde |
||||
* Copyright 2000 Paolo Scaffardi |
||||
* Copyright 2000-2002 Wolfgang Denk, wd@denx.de |
||||
*/ |
||||
|
||||
#include <common.h> |
||||
#include <net.h> |
||||
#if defined(CONFIG_CDP_VERSION) |
||||
#include <timestamp.h> |
||||
#endif |
||||
|
||||
#include "cdp.h" |
||||
|
||||
/* Ethernet bcast address */ |
||||
const uchar NetCDPAddr[6] = { 0x01, 0x00, 0x0c, 0xcc, 0xcc, 0xcc }; |
||||
|
||||
#define CDP_DEVICE_ID_TLV 0x0001 |
||||
#define CDP_ADDRESS_TLV 0x0002 |
||||
#define CDP_PORT_ID_TLV 0x0003 |
||||
#define CDP_CAPABILITIES_TLV 0x0004 |
||||
#define CDP_VERSION_TLV 0x0005 |
||||
#define CDP_PLATFORM_TLV 0x0006 |
||||
#define CDP_NATIVE_VLAN_TLV 0x000a |
||||
#define CDP_APPLIANCE_VLAN_TLV 0x000e |
||||
#define CDP_TRIGGER_TLV 0x000f |
||||
#define CDP_POWER_CONSUMPTION_TLV 0x0010 |
||||
#define CDP_SYSNAME_TLV 0x0014 |
||||
#define CDP_SYSOBJECT_TLV 0x0015 |
||||
#define CDP_MANAGEMENT_ADDRESS_TLV 0x0016 |
||||
|
||||
#define CDP_TIMEOUT 250UL /* one packet every 250ms */ |
||||
|
||||
static int CDPSeq; |
||||
static int CDPOK; |
||||
|
||||
ushort CDPNativeVLAN; |
||||
ushort CDPApplianceVLAN; |
||||
|
||||
static const uchar CDP_SNAP_hdr[8] = { |
||||
0xAA, 0xAA, 0x03, 0x00, 0x00, 0x0C, 0x20, 0x00 }; |
||||
|
||||
static ushort |
||||
CDP_compute_csum(const uchar *buff, ushort len) |
||||
{ |
||||
ushort csum; |
||||
int odd; |
||||
ulong result = 0; |
||||
ushort leftover; |
||||
ushort *p; |
||||
|
||||
if (len > 0) { |
||||
odd = 1 & (ulong)buff; |
||||
if (odd) { |
||||
result = *buff << 8; |
||||
len--; |
||||
buff++; |
||||
} |
||||
while (len > 1) { |
||||
p = (ushort *)buff; |
||||
result += *p++; |
||||
buff = (uchar *)p; |
||||
if (result & 0x80000000) |
||||
result = (result & 0xFFFF) + (result >> 16); |
||||
len -= 2; |
||||
} |
||||
if (len) { |
||||
leftover = (signed short)(*(const signed char *)buff); |
||||
/*
|
||||
* CISCO SUCKS big time! (and blows too): |
||||
* CDP uses the IP checksum algorithm with a twist; |
||||
* for the last byte it *sign* extends and sums. |
||||
*/ |
||||
result = (result & 0xffff0000) | |
||||
((result + leftover) & 0x0000ffff); |
||||
} |
||||
while (result >> 16) |
||||
result = (result & 0xFFFF) + (result >> 16); |
||||
|
||||
if (odd) |
||||
result = ((result >> 8) & 0xff) | |
||||
((result & 0xff) << 8); |
||||
} |
||||
|
||||
/* add up 16-bit and 17-bit words for 17+c bits */ |
||||
result = (result & 0xffff) + (result >> 16); |
||||
/* add up 16-bit and 2-bit for 16+c bit */ |
||||
result = (result & 0xffff) + (result >> 16); |
||||
/* add up carry.. */ |
||||
result = (result & 0xffff) + (result >> 16); |
||||
|
||||
/* negate */ |
||||
csum = ~(ushort)result; |
||||
|
||||
/* run time endian detection */ |
||||
if (csum != htons(csum)) /* little endian */ |
||||
csum = htons(csum); |
||||
|
||||
return csum; |
||||
} |
||||
|
||||
static int |
||||
CDPSendTrigger(void) |
||||
{ |
||||
uchar *pkt; |
||||
ushort *s; |
||||
ushort *cp; |
||||
Ethernet_t *et; |
||||
int len; |
||||
ushort chksum; |
||||
#if defined(CONFIG_CDP_DEVICE_ID) || defined(CONFIG_CDP_PORT_ID) || \ |
||||
defined(CONFIG_CDP_VERSION) || defined(CONFIG_CDP_PLATFORM) |
||||
char buf[32]; |
||||
#endif |
||||
|
||||
pkt = NetTxPacket; |
||||
et = (Ethernet_t *)pkt; |
||||
|
||||
/* NOTE: trigger sent not on any VLAN */ |
||||
|
||||
/* form ethernet header */ |
||||
memcpy(et->et_dest, NetCDPAddr, 6); |
||||
memcpy(et->et_src, NetOurEther, 6); |
||||
|
||||
pkt += ETHER_HDR_SIZE; |
||||
|
||||
/* SNAP header */ |
||||
memcpy((uchar *)pkt, CDP_SNAP_hdr, sizeof(CDP_SNAP_hdr)); |
||||
pkt += sizeof(CDP_SNAP_hdr); |
||||
|
||||
/* CDP header */ |
||||
*pkt++ = 0x02; /* CDP version 2 */ |
||||
*pkt++ = 180; /* TTL */ |
||||
s = (ushort *)pkt; |
||||
cp = s; |
||||
/* checksum (0 for later calculation) */ |
||||
*s++ = htons(0); |
||||
|
||||
/* CDP fields */ |
||||
#ifdef CONFIG_CDP_DEVICE_ID |
||||
*s++ = htons(CDP_DEVICE_ID_TLV); |
||||
*s++ = htons(CONFIG_CDP_DEVICE_ID); |
||||
sprintf(buf, CONFIG_CDP_DEVICE_ID_PREFIX "%pm", NetOurEther); |
||||
memcpy((uchar *)s, buf, 16); |
||||
s += 16 / 2; |
||||
#endif |
||||
|
||||
#ifdef CONFIG_CDP_PORT_ID |
||||
*s++ = htons(CDP_PORT_ID_TLV); |
||||
memset(buf, 0, sizeof(buf)); |
||||
sprintf(buf, CONFIG_CDP_PORT_ID, eth_get_dev_index()); |
||||
len = strlen(buf); |
||||
if (len & 1) /* make it even */ |
||||
len++; |
||||
*s++ = htons(len + 4); |
||||
memcpy((uchar *)s, buf, len); |
||||
s += len / 2; |
||||
#endif |
||||
|
||||
#ifdef CONFIG_CDP_CAPABILITIES |
||||
*s++ = htons(CDP_CAPABILITIES_TLV); |
||||
*s++ = htons(8); |
||||
*(ulong *)s = htonl(CONFIG_CDP_CAPABILITIES); |
||||
s += 2; |
||||
#endif |
||||
|
||||
#ifdef CONFIG_CDP_VERSION |
||||
*s++ = htons(CDP_VERSION_TLV); |
||||
memset(buf, 0, sizeof(buf)); |
||||
strcpy(buf, CONFIG_CDP_VERSION); |
||||
len = strlen(buf); |
||||
if (len & 1) /* make it even */ |
||||
len++; |
||||
*s++ = htons(len + 4); |
||||
memcpy((uchar *)s, buf, len); |
||||
s += len / 2; |
||||
#endif |
||||
|
||||
#ifdef CONFIG_CDP_PLATFORM |
||||
*s++ = htons(CDP_PLATFORM_TLV); |
||||
memset(buf, 0, sizeof(buf)); |
||||
strcpy(buf, CONFIG_CDP_PLATFORM); |
||||
len = strlen(buf); |
||||
if (len & 1) /* make it even */ |
||||
len++; |
||||
*s++ = htons(len + 4); |
||||
memcpy((uchar *)s, buf, len); |
||||
s += len / 2; |
||||
#endif |
||||
|
||||
#ifdef CONFIG_CDP_TRIGGER |
||||
*s++ = htons(CDP_TRIGGER_TLV); |
||||
*s++ = htons(8); |
||||
*(ulong *)s = htonl(CONFIG_CDP_TRIGGER); |
||||
s += 2; |
||||
#endif |
||||
|
||||
#ifdef CONFIG_CDP_POWER_CONSUMPTION |
||||
*s++ = htons(CDP_POWER_CONSUMPTION_TLV); |
||||
*s++ = htons(6); |
||||
*s++ = htons(CONFIG_CDP_POWER_CONSUMPTION); |
||||
#endif |
||||
|
||||
/* length of ethernet packet */ |
||||
len = (uchar *)s - ((uchar *)NetTxPacket + ETHER_HDR_SIZE); |
||||
et->et_protlen = htons(len); |
||||
|
||||
len = ETHER_HDR_SIZE + sizeof(CDP_SNAP_hdr); |
||||
chksum = CDP_compute_csum((uchar *)NetTxPacket + len, |
||||
(uchar *)s - (NetTxPacket + len)); |
||||
if (chksum == 0) |
||||
chksum = 0xFFFF; |
||||
*cp = htons(chksum); |
||||
|
||||
(void) eth_send(NetTxPacket, (uchar *)s - NetTxPacket); |
||||
return 0; |
||||
} |
||||
|
||||
static void |
||||
CDPTimeout(void) |
||||
{ |
||||
CDPSeq++; |
||||
|
||||
if (CDPSeq < 3) { |
||||
NetSetTimeout(CDP_TIMEOUT, CDPTimeout); |
||||
CDPSendTrigger(); |
||||
return; |
||||
} |
||||
|
||||
/* if not OK try again */ |
||||
if (!CDPOK) |
||||
NetStartAgain(); |
||||
else |
||||
NetState = NETLOOP_SUCCESS; |
||||
} |
||||
|
||||
static void |
||||
CDPDummyHandler(uchar *pkt, unsigned dest, IPaddr_t sip, unsigned src, |
||||
unsigned len) |
||||
{ |
||||
/* nothing */ |
||||
} |
||||
|
||||
void |
||||
CDPHandler(const uchar *pkt, unsigned len) |
||||
{ |
||||
const uchar *t; |
||||
const ushort *ss; |
||||
ushort type, tlen; |
||||
ushort vlan, nvlan; |
||||
|
||||
/* minimum size? */ |
||||
if (len < sizeof(CDP_SNAP_hdr) + 4) |
||||
goto pkt_short; |
||||
|
||||
/* check for valid CDP SNAP header */ |
||||
if (memcmp(pkt, CDP_SNAP_hdr, sizeof(CDP_SNAP_hdr)) != 0) |
||||
return; |
||||
|
||||
pkt += sizeof(CDP_SNAP_hdr); |
||||
len -= sizeof(CDP_SNAP_hdr); |
||||
|
||||
/* Version of CDP protocol must be >= 2 and TTL != 0 */ |
||||
if (pkt[0] < 0x02 || pkt[1] == 0) |
||||
return; |
||||
|
||||
/*
|
||||
* if version is greater than 0x02 maybe we'll have a problem; |
||||
* output a warning |
||||
*/ |
||||
if (pkt[0] != 0x02) |
||||
printf("**WARNING: CDP packet received with a protocol version " |
||||
"%d > 2\n", pkt[0] & 0xff); |
||||
|
||||
if (CDP_compute_csum(pkt, len) != 0) |
||||
return; |
||||
|
||||
pkt += 4; |
||||
len -= 4; |
||||
|
||||
vlan = htons(-1); |
||||
nvlan = htons(-1); |
||||
while (len > 0) { |
||||
if (len < 4) |
||||
goto pkt_short; |
||||
|
||||
ss = (const ushort *)pkt; |
||||
type = ntohs(ss[0]); |
||||
tlen = ntohs(ss[1]); |
||||
if (tlen > len) |
||||
goto pkt_short; |
||||
|
||||
pkt += tlen; |
||||
len -= tlen; |
||||
|
||||
ss += 2; /* point ss to the data of the TLV */ |
||||
tlen -= 4; |
||||
|
||||
switch (type) { |
||||
case CDP_DEVICE_ID_TLV: |
||||
break; |
||||
case CDP_ADDRESS_TLV: |
||||
break; |
||||
case CDP_PORT_ID_TLV: |
||||
break; |
||||
case CDP_CAPABILITIES_TLV: |
||||
break; |
||||
case CDP_VERSION_TLV: |
||||
break; |
||||
case CDP_PLATFORM_TLV: |
||||
break; |
||||
case CDP_NATIVE_VLAN_TLV: |
||||
nvlan = *ss; |
||||
break; |
||||
case CDP_APPLIANCE_VLAN_TLV: |
||||
t = (const uchar *)ss; |
||||
while (tlen > 0) { |
||||
if (tlen < 3) |
||||
goto pkt_short; |
||||
|
||||
ss = (const ushort *)(t + 1); |
||||
|
||||
#ifdef CONFIG_CDP_APPLIANCE_VLAN_TYPE |
||||
if (t[0] == CONFIG_CDP_APPLIANCE_VLAN_TYPE) |
||||
vlan = *ss; |
||||
#else |
||||
/* XXX will this work; dunno */ |
||||
vlan = ntohs(*ss); |
||||
#endif |
||||
t += 3; tlen -= 3; |
||||
} |
||||
break; |
||||
case CDP_TRIGGER_TLV: |
||||
break; |
||||
case CDP_POWER_CONSUMPTION_TLV: |
||||
break; |
||||
case CDP_SYSNAME_TLV: |
||||
break; |
||||
case CDP_SYSOBJECT_TLV: |
||||
break; |
||||
case CDP_MANAGEMENT_ADDRESS_TLV: |
||||
break; |
||||
} |
||||
} |
||||
|
||||
CDPApplianceVLAN = vlan; |
||||
CDPNativeVLAN = nvlan; |
||||
|
||||
CDPOK = 1; |
||||
return; |
||||
|
||||
pkt_short: |
||||
printf("** CDP packet is too short\n"); |
||||
return; |
||||
} |
||||
|
||||
void |
||||
CDPStart(void) |
||||
{ |
||||
printf("Using %s device\n", eth_get_name()); |
||||
CDPSeq = 0; |
||||
CDPOK = 0; |
||||
|
||||
CDPNativeVLAN = htons(-1); |
||||
CDPApplianceVLAN = htons(-1); |
||||
|
||||
NetSetTimeout(CDP_TIMEOUT, CDPTimeout); |
||||
NetSetHandler(CDPDummyHandler); |
||||
|
||||
CDPSendTrigger(); |
||||
} |
||||
@ -0,0 +1,20 @@ |
||||
/*
|
||||
* Copied from Linux Monitor (LiMon) - Networking. |
||||
* |
||||
* Copyright 1994 - 2000 Neil Russell. |
||||
* (See License) |
||||
* Copyright 2000 Roland Borde |
||||
* Copyright 2000 Paolo Scaffardi |
||||
* Copyright 2000-2002 Wolfgang Denk, wd@denx.de |
||||
*/ |
||||
|
||||
#if defined(CONFIG_CMD_CDP) |
||||
|
||||
#ifndef __CDP_H__ |
||||
#define __CDP_H__ |
||||
|
||||
void CDPStart(void); |
||||
void CDPHandler(const uchar *pkt, unsigned len); |
||||
|
||||
#endif /* __CDP_H__ */ |
||||
#endif |
||||
Loading…
Reference in new issue