Message ID | 1327020811-1538-6-git-send-email-joe.hershberger@ni.com |
---|---|
State | Superseded |
Delegated to: | Joe Hershberger |
Headers | show |
Hi Joe, On Thu, Jan 19, 2012 at 4:53 PM, Joe Hershberger <joe.hershberger@ni.com> wrote: > Signed-off-by: Joe Hershberger <joe.hershberger@ni.com> > Cc: Joe Hershberger <joe.hershberger@gmail.com> > Cc: Wolfgang Denk <wd@denx.de> > --- > arch/powerpc/cpu/mpc8xx/fec.c | 2 +- > include/net.h | 9 +- > net/Makefile | 1 + > net/cdp.c | 380 +++++++++++++++++++++++++++++++++++++++++ > net/cdp.h | 18 ++ > net/net.c | 368 +--------------------------------------- > 6 files changed, 406 insertions(+), 372 deletions(-) > create mode 100644 net/cdp.c > create mode 100644 net/cdp.h > > diff --git a/arch/powerpc/cpu/mpc8xx/fec.c b/arch/powerpc/cpu/mpc8xx/fec.c > index f2a2c3a..0bc1e5d 100644 > --- a/arch/powerpc/cpu/mpc8xx/fec.c > +++ b/arch/powerpc/cpu/mpc8xx/fec.c > @@ -274,7 +274,7 @@ static int fec_recv (struct eth_device *dev) > #if defined(CONFIG_CMD_CDP) > if ((rx[0] & 1) != 0 > && memcmp ((uchar *) rx, NetBcastAddr, 6) != 0 > - && memcmp ((uchar *) rx, NetCDPAddr, 6) != 0) > + && !is_cdp_packet((uchar *)rx)) Do you think the is_cdp_packet() change should be in a separate commit? Regards, Simon > rx = NULL; > #endif > /* > diff --git a/include/net.h b/include/net.h > index 09d1b51..0396b69 100644 > --- a/include/net.h > +++ b/include/net.h > @@ -355,10 +355,6 @@ extern uchar NetEtherNullAddr[6]; > extern ushort NetOurVLAN; /* Our VLAN */ > extern ushort NetOurNativeVLAN; /* Our Native VLAN */ > > -extern uchar NetCDPAddr[6]; /* Ethernet CDP address */ > -extern ushort CDPNativeVLAN; /* CDP returned native VLAN */ > -extern ushort CDPApplianceVLAN; /* CDP returned appliance VLAN */ > - > extern int NetState; /* Network loop state */ > #define NETLOOP_CONTINUE 1 > #define NETLOOP_RESTART 2 > @@ -386,8 +382,9 @@ extern IPaddr_t NetPingIP; /* the ip address to ping */ > > #if defined(CONFIG_CMD_CDP) > /* when CDP completes these hold the return values */ > -extern ushort CDPNativeVLAN; > -extern ushort CDPApplianceVLAN; > +extern ushort CDPNativeVLAN; /* CDP returned native VLAN */ > +extern ushort CDPApplianceVLAN; /* CDP returned appliance VLAN */ > +int is_cdp_packet(const uchar *et_addr); > #endif > > #if defined(CONFIG_CMD_SNTP) > diff --git a/net/Makefile b/net/Makefile > index 5901046..b350bfc 100644 > --- a/net/Makefile > +++ b/net/Makefile > @@ -28,6 +28,7 @@ include $(TOPDIR)/config.mk > LIB = $(obj)libnet.o > > COBJS-$(CONFIG_CMD_NET) += bootp.o > +COBJS-$(CONFIG_CMD_CDP) += cdp.o > COBJS-$(CONFIG_CMD_DNS) += dns.o > COBJS-$(CONFIG_CMD_NET) += eth.o > COBJS-$(CONFIG_CMD_NET) += net.o > diff --git a/net/cdp.c b/net/cdp.c > new file mode 100644 > index 0000000..c5e54b2 > --- /dev/null > +++ b/net/cdp.c > @@ -0,0 +1,380 @@ > +/* > + * 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 */ > +static 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(); > +} > + > +int > +is_cdp_packet(const uchar *et_addr) > +{ > + return memcmp(et_addr, NetCDPAddr, 6) == 0; > +} > diff --git a/net/cdp.h b/net/cdp.h > new file mode 100644 > index 0000000..fef744e > --- /dev/null > +++ b/net/cdp.h > @@ -0,0 +1,18 @@ > +/* > + * 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 > + */ > + > +#ifndef __CDP_H__ > +#define __CDP_H__ > + > +void CDPStart(void); > +void CDPHandler(const uchar *pkt, unsigned len); > + > +#endif /* __CDP_H__ */ > + > diff --git a/net/net.c b/net/net.c > index 1d6252f..48d3ca8 100644 > --- a/net/net.c > +++ b/net/net.c > @@ -91,8 +91,8 @@ > #if defined(CONFIG_CMD_SNTP) > #include "sntp.h" > #endif > -#if defined(CONFIG_CDP_VERSION) > -#include <timestamp.h> > +#if defined(CONFIG_CMD_CDP) > +#include "cdp.h" > #endif > #if defined(CONFIG_CMD_DNS) > #include "dns.h" > @@ -163,10 +163,6 @@ uchar NetEtherNullAddr[6]; > #ifdef CONFIG_API > void (*push_packet)(volatile void *, int len) = 0; > #endif > -#if defined(CONFIG_CMD_CDP) > -/* Ethernet bcast address */ > -uchar NetCDPAddr[6] = { 0x01, 0x00, 0x0c, 0xcc, 0xcc, 0xcc }; > -#endif > /* Network loop state */ > int NetState; > /* Tried all network devices */ > @@ -192,10 +188,6 @@ IPaddr_t NetPingIP; > static void PingStart(void); > #endif > > -#if defined(CONFIG_CMD_CDP) > -static void CDPStart(void); > -#endif > - > #if defined(CONFIG_CMD_SNTP) > /* NTP server IP address */ > IPaddr_t NetNtpServerIP; > @@ -841,360 +833,6 @@ static void PingStart(void) > } > #endif > > -#if defined(CONFIG_CMD_CDP) > - > -#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; > -} > - > -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 */ > -} > - > -static 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; > -} > - > -static 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(); > -} > -#endif > - > #ifdef CONFIG_IP_DEFRAG > /* > * This function collects fragments in a single packet, according > @@ -1463,7 +1101,7 @@ NetReceive(volatile uchar *inpkt, int len) > > #if defined(CONFIG_CMD_CDP) > /* keep track if packet is CDP */ > - iscdp = memcmp(et->et_dest, NetCDPAddr, 6) == 0; > + iscdp = is_cdp_packet(et->et_dest); > #endif > > myvlanid = ntohs(NetOurVLAN); > -- > 1.6.0.2 > > _______________________________________________ > U-Boot mailing list > U-Boot@lists.denx.de > http://lists.denx.de/mailman/listinfo/u-boot
On Thursday 19 January 2012 19:53:08 Joe Hershberger wrote: > --- a/include/net.h > +++ b/include/net.h > > +int is_cdp_packet(const uchar *et_addr); seems like this would be better as a static inline since it's just a memcmp > --- /dev/null > +++ b/net/cdp.h > @@ -0,0 +1,18 @@ > +/* > + * 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 > + */ > + > +#ifndef __CDP_H__ > +#define __CDP_H__ > + > +void CDPStart(void); > +void CDPHandler(const uchar *pkt, unsigned len); > + > +#endif /* __CDP_H__ */ > + no blank newlines at end of files > --- a/net/net.c > +++ b/net/net.c > > +#if defined(CONFIG_CMD_CDP) > +#include "cdp.h" > #endif let's push this ifdef down into cdp.h -mike
diff --git a/arch/powerpc/cpu/mpc8xx/fec.c b/arch/powerpc/cpu/mpc8xx/fec.c index f2a2c3a..0bc1e5d 100644 --- a/arch/powerpc/cpu/mpc8xx/fec.c +++ b/arch/powerpc/cpu/mpc8xx/fec.c @@ -274,7 +274,7 @@ static int fec_recv (struct eth_device *dev) #if defined(CONFIG_CMD_CDP) if ((rx[0] & 1) != 0 && memcmp ((uchar *) rx, NetBcastAddr, 6) != 0 - && memcmp ((uchar *) rx, NetCDPAddr, 6) != 0) + && !is_cdp_packet((uchar *)rx)) rx = NULL; #endif /* diff --git a/include/net.h b/include/net.h index 09d1b51..0396b69 100644 --- a/include/net.h +++ b/include/net.h @@ -355,10 +355,6 @@ extern uchar NetEtherNullAddr[6]; extern ushort NetOurVLAN; /* Our VLAN */ extern ushort NetOurNativeVLAN; /* Our Native VLAN */ -extern uchar NetCDPAddr[6]; /* Ethernet CDP address */ -extern ushort CDPNativeVLAN; /* CDP returned native VLAN */ -extern ushort CDPApplianceVLAN; /* CDP returned appliance VLAN */ - extern int NetState; /* Network loop state */ #define NETLOOP_CONTINUE 1 #define NETLOOP_RESTART 2 @@ -386,8 +382,9 @@ extern IPaddr_t NetPingIP; /* the ip address to ping */ #if defined(CONFIG_CMD_CDP) /* when CDP completes these hold the return values */ -extern ushort CDPNativeVLAN; -extern ushort CDPApplianceVLAN; +extern ushort CDPNativeVLAN; /* CDP returned native VLAN */ +extern ushort CDPApplianceVLAN; /* CDP returned appliance VLAN */ +int is_cdp_packet(const uchar *et_addr); #endif #if defined(CONFIG_CMD_SNTP) diff --git a/net/Makefile b/net/Makefile index 5901046..b350bfc 100644 --- a/net/Makefile +++ b/net/Makefile @@ -28,6 +28,7 @@ include $(TOPDIR)/config.mk LIB = $(obj)libnet.o COBJS-$(CONFIG_CMD_NET) += bootp.o +COBJS-$(CONFIG_CMD_CDP) += cdp.o COBJS-$(CONFIG_CMD_DNS) += dns.o COBJS-$(CONFIG_CMD_NET) += eth.o COBJS-$(CONFIG_CMD_NET) += net.o diff --git a/net/cdp.c b/net/cdp.c new file mode 100644 index 0000000..c5e54b2 --- /dev/null +++ b/net/cdp.c @@ -0,0 +1,380 @@ +/* + * 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 */ +static 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(); +} + +int +is_cdp_packet(const uchar *et_addr) +{ + return memcmp(et_addr, NetCDPAddr, 6) == 0; +} diff --git a/net/cdp.h b/net/cdp.h new file mode 100644 index 0000000..fef744e --- /dev/null +++ b/net/cdp.h @@ -0,0 +1,18 @@ +/* + * 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 + */ + +#ifndef __CDP_H__ +#define __CDP_H__ + +void CDPStart(void); +void CDPHandler(const uchar *pkt, unsigned len); + +#endif /* __CDP_H__ */ + diff --git a/net/net.c b/net/net.c index 1d6252f..48d3ca8 100644 --- a/net/net.c +++ b/net/net.c @@ -91,8 +91,8 @@ #if defined(CONFIG_CMD_SNTP) #include "sntp.h" #endif -#if defined(CONFIG_CDP_VERSION) -#include <timestamp.h> +#if defined(CONFIG_CMD_CDP) +#include "cdp.h" #endif #if defined(CONFIG_CMD_DNS) #include "dns.h" @@ -163,10 +163,6 @@ uchar NetEtherNullAddr[6]; #ifdef CONFIG_API void (*push_packet)(volatile void *, int len) = 0; #endif -#if defined(CONFIG_CMD_CDP) -/* Ethernet bcast address */ -uchar NetCDPAddr[6] = { 0x01, 0x00, 0x0c, 0xcc, 0xcc, 0xcc }; -#endif /* Network loop state */ int NetState; /* Tried all network devices */ @@ -192,10 +188,6 @@ IPaddr_t NetPingIP; static void PingStart(void); #endif -#if defined(CONFIG_CMD_CDP) -static void CDPStart(void); -#endif - #if defined(CONFIG_CMD_SNTP) /* NTP server IP address */ IPaddr_t NetNtpServerIP; @@ -841,360 +833,6 @@ static void PingStart(void) } #endif -#if defined(CONFIG_CMD_CDP) - -#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; -} - -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 */ -} - -static 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; -} - -static 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(); -} -#endif - #ifdef CONFIG_IP_DEFRAG /* * This function collects fragments in a single packet, according @@ -1463,7 +1101,7 @@ NetReceive(volatile uchar *inpkt, int len) #if defined(CONFIG_CMD_CDP) /* keep track if packet is CDP */ - iscdp = memcmp(et->et_dest, NetCDPAddr, 6) == 0; + iscdp = is_cdp_packet(et->et_dest); #endif myvlanid = ntohs(NetOurVLAN);
Signed-off-by: Joe Hershberger <joe.hershberger@ni.com> Cc: Joe Hershberger <joe.hershberger@gmail.com> Cc: Wolfgang Denk <wd@denx.de> --- arch/powerpc/cpu/mpc8xx/fec.c | 2 +- include/net.h | 9 +- net/Makefile | 1 + net/cdp.c | 380 +++++++++++++++++++++++++++++++++++++++++ net/cdp.h | 18 ++ net/net.c | 368 +--------------------------------------- 6 files changed, 406 insertions(+), 372 deletions(-) create mode 100644 net/cdp.c create mode 100644 net/cdp.h