From 25149569297f5d9e2b4eee51e93f696c54756d08 Mon Sep 17 00:00:00 2001 From: darkcoven Date: Fri, 11 Oct 2013 22:15:27 +0200 Subject: [PATCH] Start reorganization of low level library --- lib/src/network.c | 39 +- lib/src/network.h | 4 +- lib/src/session.c | 4 +- raw/include/Makefile.am | 2 +- raw/include/attr.h | 149 +++----- raw/include/encoding.h | 50 +++ raw/include/misc.h | 19 + raw/include/protocol.h | 207 ++++++----- raw/src/Makefile.am | 2 +- raw/src/attr.c | 555 ++--------------------------- raw/src/{protocol.c => encoding.c} | 128 +------ raw/src/encoding_attr.c | 548 ++++++++++++++++++++++++++++ raw/src/encoding_attr.h | 24 ++ raw/src/misc.c | 42 +++ spy/src/spy.c | 12 +- 15 files changed, 914 insertions(+), 871 deletions(-) create mode 100644 raw/include/encoding.h create mode 100644 raw/include/misc.h rename raw/src/{protocol.c => encoding.c} (56%) create mode 100644 raw/src/encoding_attr.c create mode 100644 raw/src/encoding_attr.h create mode 100644 raw/src/misc.c diff --git a/lib/src/network.c b/lib/src/network.c index 1e0211b..ee40862 100644 --- a/lib/src/network.c +++ b/lib/src/network.c @@ -10,7 +10,8 @@ #include #include -#include +#include +#include #include "network.h" @@ -159,10 +160,10 @@ int updateTimeout (struct ngadmin *nga) } -int sendNgPacket (struct ngadmin *nga, char code, const List *attr) +int sendNsdpPacket (struct ngadmin *nga, char code, const List *attr) { - char buffer[1500]; - struct ng_packet np; + unsigned char buffer[1500]; + struct nsdp_packet np; struct sockaddr_in remote; const struct swi_attr *sa = nga->current; int ret; @@ -170,8 +171,8 @@ int sendNgPacket (struct ngadmin *nga, char code, const List *attr) np.buffer = buffer; np.maxlen = sizeof(buffer); - initNgPacket(&np); - initNgHeader(np.nh, code, &nga->localmac, sa == NULL ? NULL : &sa->mac, ++nga->seq); + initNsdpPacket(&np); + initNsdpHeader(np.nh, code, &nga->localmac, sa == NULL ? NULL : &sa->mac, ++nga->seq); ret = addPacketAttributes(&np, attr, sa == NULL ? 0 : sa->ports); if (ret < 0) @@ -198,10 +199,10 @@ int sendNgPacket (struct ngadmin *nga, char code, const List *attr) } -int recvNgPacket (struct ngadmin *nga, char code, unsigned char *error, unsigned short *attr_error, List *attr) +int recvNsdpPacket (struct ngadmin *nga, char code, unsigned char *error, unsigned short *attr_error, List *attr) { - char buffer[1500]; - struct ng_packet np; + unsigned char buffer[1500]; + struct nsdp_packet np; struct sockaddr_in remote; socklen_t slen = sizeof(struct sockaddr_in); const struct swi_attr *sa = nga->current; @@ -218,22 +219,20 @@ int recvNgPacket (struct ngadmin *nga, char code, unsigned char *error, unsigned rem = nga->timeout; while (1) { - FD_ZERO(&fs); FD_SET(nga->sock, &fs); - select(nga->sock+1, &fs, NULL, NULL, &rem); /* FIXME: non portable */ + select(nga->sock + 1, &fs, NULL, NULL, &rem); /* FIXME: non portable */ len = recvfrom(nga->sock, buffer, sizeof(buffer), MSG_DONTWAIT, (struct sockaddr*)&remote, &slen); - if (len < 0) break; np.maxlen = len; - initNgPacket(&np); + initNsdpPacket(&np); if (ntohs(remote.sin_port) != SWITCH_PORT || - len < (int)sizeof(struct ng_header) || - !validateNgHeader(np.nh, code, &nga->localmac, sa == NULL ? NULL : &sa->mac, nga->seq) || + len < (int)sizeof(struct nsdp_header) || + !validateNsdpHeader(np.nh, code, &nga->localmac, sa == NULL ? NULL : &sa->mac, nga->seq) || extractPacketAttributes(&np, attr, sa == NULL ? 0 : sa->ports) < 0) continue; @@ -279,13 +278,13 @@ int readRequest (struct ngadmin *nga, List *attr) /* add end attribute to end */ pushBackList(attr, newEmptyAttr(ATTR_END)); - i = sendNgPacket(nga, CODE_READ_REQ, attr); + i = sendNsdpPacket(nga, CODE_READ_REQ, attr); /* the list will be filled again by recvNgPacket */ clearList(attr, (void(*)(void*))freeAttr); if (i >= 0) - i = recvNgPacket(nga, CODE_READ_REP, &err, &attr_error, attr); + i = recvNsdpPacket(nga, CODE_READ_REP, &err, &attr_error, attr); if (i == -EINVAL) { ret = ERR_INVARG; @@ -334,20 +333,20 @@ int writeRequest (struct ngadmin *nga, List *attr) /* add end attribute to end */ pushBackList(attr, newEmptyAttr(ATTR_END)); - i = sendNgPacket(nga, CODE_WRITE_REQ, attr); + i = sendNsdpPacket(nga, CODE_WRITE_REQ, attr); /* the list will be filled again by recvNgPacket but normally it will be still empty */ clearList(attr, (void(*)(void*))freeAttr); if (i >= 0) - i = recvNgPacket(nga, CODE_WRITE_REP, &err, &attr_error, attr); + i = recvNsdpPacket(nga, CODE_WRITE_REP, &err, &attr_error, attr); if (i == -EINVAL) { ret = ERR_INVARG; goto end; } else if (i < 0) { - ret = ( errno==EAGAIN || errno==EWOULDBLOCK ) ? ERR_TIMEOUT : ERR_NET ; + ret = (errno == EAGAIN || errno == EWOULDBLOCK) ? ERR_TIMEOUT : ERR_NET ; goto end; } diff --git a/lib/src/network.h b/lib/src/network.h index 5137c98..5295a52 100644 --- a/lib/src/network.h +++ b/lib/src/network.h @@ -22,10 +22,10 @@ int forceInterface (struct ngadmin *nga); int updateTimeout (struct ngadmin *nga); -int sendNgPacket (struct ngadmin *nga, char code, const List *attr); +int sendNsdpPacket (struct ngadmin *nga, char code, const List *attr); -int recvNgPacket (struct ngadmin *nga, char code, unsigned char *error, unsigned short *attr_error, List *attr); +int recvNsdpPacket (struct ngadmin *nga, char code, unsigned char *error, unsigned short *attr_error, List *attr); int readRequest (struct ngadmin *nga, List *attr); diff --git a/lib/src/session.c b/lib/src/session.c index 672bbe7..c36c772 100644 --- a/lib/src/session.c +++ b/lib/src/session.c @@ -64,7 +64,7 @@ int ngadmin_scan (struct ngadmin *nga) } /* send request to all potential switches */ - i = sendNgPacket(nga, CODE_READ_REQ, attr); + i = sendNsdpPacket(nga, CODE_READ_REQ, attr); clearList(attr, (void(*)(void*))freeAttr); if (i == -EINVAL) return ERR_INVARG; @@ -74,7 +74,7 @@ int ngadmin_scan (struct ngadmin *nga) /* try to receive any packets until timeout */ swiList = createEmptyList(); /* FIXME: end after timeout whatever received packet is good or not */ - while (recvNgPacket(nga, CODE_READ_REP, NULL, NULL, attr) >= 0) { + while (recvNsdpPacket(nga, CODE_READ_REP, NULL, NULL, attr) >= 0) { sa = malloc(sizeof(struct swi_attr)); if (sa == NULL) return ERR_MEM; diff --git a/raw/include/Makefile.am b/raw/include/Makefile.am index 2183ae6..307afdf 100644 --- a/raw/include/Makefile.am +++ b/raw/include/Makefile.am @@ -1,3 +1,3 @@ -noinst_HEADERS = attr.h list.h protocol.h +noinst_HEADERS = attr.h encoding.h list.h misc.h protocol.h diff --git a/raw/include/attr.h b/raw/include/attr.h index dfbdce0..24fa023 100644 --- a/raw/include/attr.h +++ b/raw/include/attr.h @@ -3,130 +3,75 @@ #define DEF_ATTR -#include - -#include "protocol.h" - - -#define ATTR_PRODUCT 0x0001 -#define ATTR_UNK_0002 0x0002 -#define ATTR_NAME 0x0003 -#define ATTR_MAC 0x0004 -#define ATTR_UNK_0005 0x0005 -#define ATTR_IP 0x0006 -#define ATTR_NETMASK 0x0007 -#define ATTR_GATEWAY 0x0008 -#define ATTR_NEW_PASSWORD 0x0009 -#define ATTR_PASSWORD 0x000A -#define ATTR_DHCP 0x000B -#define ATTR_UNK_000C 0x000C -#define ATTR_FIRM_VER 0x000D -#define ATTR_UNK_000E 0x000E -#define ATTR_UNK_000F 0x000F -#define ATTR_FIRM_UPGRADE 0x0010 -#define ATTR_RESTART 0x0013 -#define ATTR_ENCPASS 0x0014 -#define ATTR_DEFAULTS 0x0400 -#define ATTR_PORT_STATUS 0x0C00 -#define ATTR_PORT_STATISTICS 0x1000 -#define ATTR_STATS_RESET 0x1400 -#define ATTR_CABLETEST_DO 0x1800 -#define ATTR_CABLETEST_RESULT 0x1C00 -#define ATTR_VLAN_TYPE 0x2000 -#define ATTR_VLAN_PORT_CONF 0x2400 -#define ATTR_VLAN_DOT_CONF 0x2800 -#define ATTR_VLAN_DESTROY 0x2C00 -#define ATTR_VLAN_PVID 0x3000 -#define ATTR_QOS_TYPE 0x3400 -#define ATTR_QOS_CONFIG 0x3800 -#define ATTR_BITRATE_INPUT 0x4C00 -#define ATTR_BITRATE_OUTPUT 0x5000 -#define ATTR_STORM_ENABLE 0x5400 -#define ATTR_STORM_BITRATE 0x5800 -#define ATTR_MIRROR 0x5C00 -#define ATTR_PORTS_COUNT 0x6000 -#define ATTR_MAX_VLAN 0x6400 -#define ATTR_IGMP_ENABLE_VLAN 0x6800 -#define ATTR_IGMP_BLOCK_UNK 0x6C00 -#define ATTR_IGMP_VALID_V3 0x7000 -#define ATTR_TLV_BITMAP 0x7400 -#define ATTR_END 0xFFFF - -#define UNUSED __attribute__((unused)) - - -struct attr_handler { - unsigned short attr; /* attribute code */ - unsigned int size; /* expected data size */ - bool (*encode)(struct attr *at, unsigned char ports); /* encode function */ - bool (*decode)(struct attr *at, unsigned char ports); /* decode function */ -}; +#include +#include -const struct attr_handler* getAttrHandler (unsigned short attrcode); +#include +struct attr { + unsigned short attr; /* attribute code */ + unsigned short size; /* attribute size */ + void *data; /* attribute data */ +}; -struct attr_port_status { - unsigned char port; - unsigned char status; - unsigned char unk; -} __attribute__((packed)); -struct attr_port_stat { - unsigned char port; - unsigned long long recv; - unsigned long long sent; - unsigned long long unk1; - unsigned long long unk2; - unsigned long long unk3; - unsigned long long crc; -} __attribute__((packed)); +struct attr* newAttr (unsigned short attr, unsigned short size, void *data); -struct attr_bitrate { - unsigned char port; - int bitrate; -} __attribute__((packed)); +static inline struct attr* newEmptyAttr (unsigned short attr) +{ + return newAttr(attr, 0, NULL); +} -struct attr_qos { - unsigned char port; - unsigned char prio; -} __attribute__((packed)); +static inline struct attr* newByteAttr (unsigned short attr, unsigned char value) +{ + char *v = malloc(sizeof(char)); + + *v = value; + + return newAttr(attr, sizeof(char), v); +} -struct attr_pvid { - unsigned char port; - unsigned short vlan; -} __attribute__((packed)); +static inline struct attr* newShortAttr (unsigned short attr, short value) +{ + short *v = malloc(sizeof(short)); + + *v = value; + + return newAttr(attr, sizeof(short), v); +} -struct attr_igmp_vlan { - unsigned short enable; - unsigned short vlan; -} __attribute__((packed)); +static inline struct attr* newIntAttr (unsigned short attr, int value) +{ + int *v = malloc(sizeof(int)); + + *v = value; + + return newAttr(attr, sizeof(int), v); +} -struct attr_cabletest_do { - unsigned char port; - unsigned char action; -} __attribute__((packed)); +static inline struct attr* newAddrAttr (unsigned short attr, struct in_addr value) +{ + struct in_addr *v = malloc(sizeof(struct in_addr)); + + *v = value; + + return newAttr(attr, sizeof(struct in_addr), v); +} -struct attr_cabletest_result { - unsigned char port; - unsigned int v1; - unsigned int v2; -} __attribute__((packed)); +void freeAttr (struct attr *at); -struct attr_vlan_conf { - unsigned short vlan; - unsigned char ports[0]; -}; +void filterAttributes (List *attr, ...); #endif diff --git a/raw/include/encoding.h b/raw/include/encoding.h new file mode 100644 index 0000000..1c21939 --- /dev/null +++ b/raw/include/encoding.h @@ -0,0 +1,50 @@ + +#ifndef DEF_ENCODING +#define DEF_ENCODING + + +#include + +#include + +#include +#include + + +struct nsdp_packet { + union { + unsigned char *buffer; + struct nsdp_header *nh; + }; + int maxlen; + struct attr_header *ah; +}; + + + +void initNsdpHeader (struct nsdp_header *nh, char code, const struct ether_addr *client_mac, const struct ether_addr *switch_mac, unsigned int seqnum); + + +bool validateNsdpHeader (const struct nsdp_header *nh, char code, const struct ether_addr *client_mac, const struct ether_addr *switch_mac, unsigned int seqnum); + + +static inline void initNsdpPacket (struct nsdp_packet *np) +{ + np->ah = (struct attr_header*)np->nh->data; +} + + +static inline int getPacketTotalSize (const struct nsdp_packet *np) +{ + return ((unsigned char*)np->ah) - np->buffer; +} + + +int addPacketAttributes (struct nsdp_packet *np, const List* attr, unsigned char ports); + + +int extractPacketAttributes (struct nsdp_packet *np, List *attr, unsigned char ports); + + +#endif + diff --git a/raw/include/misc.h b/raw/include/misc.h new file mode 100644 index 0000000..6aaa412 --- /dev/null +++ b/raw/include/misc.h @@ -0,0 +1,19 @@ + +#ifndef DEF_MISC +#define DEF_MISC + + +void passwordEndecode (char *buf, unsigned int len); + + +int trim (char *txt, int start); + + +static inline int min (int a, int b) +{ + return a < b ? a : b; +} + + +#endif + diff --git a/raw/include/protocol.h b/raw/include/protocol.h index b6d9da6..297a663 100644 --- a/raw/include/protocol.h +++ b/raw/include/protocol.h @@ -3,17 +3,15 @@ #define DEF_PROTOCOL -#include -#include - #include -#include "list.h" - #define CLIENT_PORT 63321 #define SWITCH_PORT 63322 +#define NSDP_VERSION 1 +#define NSDP_PROTOID "NSDP" + #define CODE_READ_REQ 1 #define CODE_READ_REP 2 #define CODE_WRITE_REQ 3 @@ -24,7 +22,53 @@ #define ERROR_DENIED 7 -struct ng_header { +#define ATTR_PRODUCT 0x0001 +#define ATTR_UNK_0002 0x0002 +#define ATTR_NAME 0x0003 +#define ATTR_MAC 0x0004 +#define ATTR_UNK_0005 0x0005 +#define ATTR_IP 0x0006 +#define ATTR_NETMASK 0x0007 +#define ATTR_GATEWAY 0x0008 +#define ATTR_NEW_PASSWORD 0x0009 +#define ATTR_PASSWORD 0x000A +#define ATTR_DHCP 0x000B +#define ATTR_UNK_000C 0x000C +#define ATTR_FIRM_VER 0x000D +#define ATTR_UNK_000E 0x000E +#define ATTR_UNK_000F 0x000F +#define ATTR_FIRM_UPGRADE 0x0010 +#define ATTR_RESTART 0x0013 +#define ATTR_ENCPASS 0x0014 +#define ATTR_DEFAULTS 0x0400 +#define ATTR_PORT_STATUS 0x0C00 +#define ATTR_PORT_STATISTICS 0x1000 +#define ATTR_STATS_RESET 0x1400 +#define ATTR_CABLETEST_DO 0x1800 +#define ATTR_CABLETEST_RESULT 0x1C00 +#define ATTR_VLAN_TYPE 0x2000 +#define ATTR_VLAN_PORT_CONF 0x2400 +#define ATTR_VLAN_DOT_CONF 0x2800 +#define ATTR_VLAN_DESTROY 0x2C00 +#define ATTR_VLAN_PVID 0x3000 +#define ATTR_QOS_TYPE 0x3400 +#define ATTR_QOS_CONFIG 0x3800 +#define ATTR_BITRATE_INPUT 0x4C00 +#define ATTR_BITRATE_OUTPUT 0x5000 +#define ATTR_STORM_ENABLE 0x5400 +#define ATTR_STORM_BITRATE 0x5800 +#define ATTR_MIRROR 0x5C00 +#define ATTR_PORTS_COUNT 0x6000 +#define ATTR_MAX_VLAN 0x6400 +#define ATTR_IGMP_ENABLE_VLAN 0x6800 +#define ATTR_IGMP_BLOCK_UNK 0x6C00 +#define ATTR_IGMP_VALID_V3 0x7000 +#define ATTR_TLV_BITMAP 0x7400 +#define ATTR_END 0xFFFF + + + +struct nsdp_header { char version; /* always 1, maybe version */ char code; /* request code: read request, read reply, write request, write reply */ unsigned char error; /* error code, 0 when no error */ @@ -36,129 +80,82 @@ struct ng_header { unsigned int seqnum; /* sequence number */ char proto_id[4]; /* always "NSDP", maybe short for "Netgear Switch Description Protocol" */ char unk3[4]; /* always 0, unknown */ - char data[0]; + char data[0]; /* attributes data */ } __attribute__((packed)); struct attr_header { - unsigned short attr; - unsigned short size; - char data[0]; + unsigned short attr; /* attribute code */ + unsigned short size; /* attribute data size */ + char data[0]; /* attribute data */ } __attribute__((packed)); -struct ng_packet { - union { - char *buffer; - struct ng_header *nh; - }; - int maxlen; - struct attr_header *ah; -}; - - -struct attr { - unsigned short attr; - unsigned short size; - void *data; -}; - - -extern const char passwordKey[]; - - -void passwordEndecode (char *buf, unsigned int len); - - -int trim (char *txt, int start); - - -static inline int min (int a, int b) -{ - return a < b ? a : b; -} - - -void initNgHeader (struct ng_header *nh, char code, const struct ether_addr *client_mac, const struct ether_addr *switch_mac, unsigned int seqnum); - - -bool validateNgHeader (const struct ng_header *nh, char code, const struct ether_addr *client_mac, const struct ether_addr *switch_mac, unsigned int seqnum); - -static inline void initNgPacket (struct ng_packet *np) -{ - np->ah = (struct attr_header*)np->nh->data; -} - - -void addPacketAttr (struct ng_packet *np, struct attr *at); - - -static inline int getPacketTotalSize (const struct ng_packet *np) -{ - return ((char*)np->ah) - np->buffer; -} - - -struct attr* newAttr (unsigned short attr, unsigned short size, void *data); - - -static inline struct attr* newEmptyAttr (unsigned short attr) -{ - return newAttr(attr, 0, NULL); -} +struct attr_port_status { + unsigned char port; /* port number */ + unsigned char status; /* port status (speed index) */ + unsigned char unk; /* unknown */ +} __attribute__((packed)); -static inline struct attr* newByteAttr (unsigned short attr, unsigned char value) -{ - char *v = malloc(sizeof(char)); - - *v = value; - - return newAttr(attr, sizeof(char), v); -} +struct attr_port_stat { + unsigned char port; /* port number */ + unsigned long long recv; /* received bytes */ + unsigned long long sent; /* sent bytes */ + unsigned long long unk1; /* unknown */ + unsigned long long unk2; /* unknown */ + unsigned long long unk3; /* unknown */ + unsigned long long crc; /* CRC errors */ +} __attribute__((packed)); -static inline struct attr* newShortAttr (unsigned short attr, short value) -{ - short *v = malloc(sizeof(short)); - - *v = value; - - return newAttr(attr, sizeof(short), v); -} +struct attr_bitrate { + unsigned char port; /* port number */ + int bitrate; /* bitrate index */ +} __attribute__((packed)); -static inline struct attr* newIntAttr (unsigned short attr, int value) -{ - int *v = malloc(sizeof(int)); - - *v = value; - - return newAttr(attr, sizeof(int), v); -} +struct attr_qos { + unsigned char port; /* port number */ + unsigned char prio; /* prio index */ +} __attribute__((packed)); -static inline struct attr* newAddrAttr (unsigned short attr, struct in_addr value) -{ - struct in_addr *v = malloc(sizeof(struct in_addr)); - - *v = value; - - return newAttr(attr, sizeof(struct in_addr), v); -} +struct attr_pvid { + unsigned char port; /* port number */ + unsigned short vlan; /* VLAN */ +} __attribute__((packed)); -void freeAttr (struct attr *at); +struct attr_igmp_vlan { + unsigned short enable; /* IGMP filtering enabled */ + unsigned short vlan; /* VLAN where IGMP packets are filtered */ +} __attribute__((packed)); -int addPacketAttributes (struct ng_packet *np, const List* attr, unsigned char ports); +struct attr_cabletest_do { + unsigned char port; /* port number */ + unsigned char action; /* action index */ +} __attribute__((packed)); -int extractPacketAttributes (struct ng_packet *np, List *attr, unsigned char ports); +struct attr_cabletest_result { + unsigned char port; /* port number */ + unsigned int v1; /* raw value 1 (values unknown yet) */ + unsigned int v2; /* raw value 2 (values unknown yet) */ +} __attribute__((packed)); -void filterAttributes (List *attr, ...); +/** + * Note: this structure is not sent "as-is" on the wire. + * A translation is done between the wire format (which uses somewhat not + * trivial bitmap) and this simpler format. See encoding.c for more details. + */ +struct attr_vlan_conf { + unsigned short vlan; /* port number */ + unsigned char ports[0]; /* array, maps each port to a VLAN */ +}; #endif diff --git a/raw/src/Makefile.am b/raw/src/Makefile.am index bf354b0..df23edf 100644 --- a/raw/src/Makefile.am +++ b/raw/src/Makefile.am @@ -1,7 +1,7 @@ noinst_LTLIBRARIES = librawnsdp.la -librawnsdp_la_SOURCES = attr.c list.c protocol.c +librawnsdp_la_SOURCES = attr.c encoding.c encoding_attr.c list.c misc.c librawnsdp_la_CPPFLAGS = -I$(top_srcdir)/raw/include/ -I$(top_srcdir)/lib/include/ librawnsdp_la_CFLAGS = -fno-strict-aliasing diff --git a/raw/src/attr.c b/raw/src/attr.c index 4547713..b83598a 100644 --- a/raw/src/attr.c +++ b/raw/src/attr.c @@ -1,550 +1,69 @@ -#include +#include -#include "attr.h" -#include "protocol.h" +#include +#include -#define ATTR_HANDLER_ENTRY(at, sz, enc, dec) {.attr = at, .size = sz, .encode = enc, .decode = dec} - - -static bool bool_endecode (struct attr *at, unsigned char ports UNUSED) -{ - *(char*)at->data = (*(char*)at->data != 0); - - return true; -} - - -static bool ports_status_decode (struct attr *at, unsigned char ports) -{ - struct attr_port_status *ps = at->data; - - if (ps->port < 1 || ps->port > ports) - return false; - - switch (ps->status) { - case SPEED_DOWN: - case SPEED_10: - case SPEED_100: - case SPEED_1000: - break; - default: - return false; - } - - return true; -} - - -static bool port_stat_decode (struct attr *at, unsigned char ports) -{ - struct attr_port_stat *ps = at->data; - unsigned long long *v; - - if (ps->port < 1 || ps->port > ports) - return false; - - for (v = &ps->recv; ((char*)v) - ((char*)ps) < (int)sizeof(struct attr_port_stat); v++) - *v = be64toh(*v); - - - return true; -} - - -static bool bitrate_decode (struct attr *at, unsigned char ports) -{ - struct attr_bitrate *sb = at->data; - - if (sb->port < 1 || sb->port > ports) - return false; - - sb->bitrate = ntohl(sb->bitrate); - if (sb->bitrate < BITRATE_UNSPEC || sb->bitrate > BITRATE_512M) - return false; - - return true; -} - - -static bool bitrate_encode (struct attr *at, unsigned char ports) -{ - struct attr_bitrate *sb = at->data; - - if (sb->port < 1 || sb->port > ports) - return false; - - if (sb->bitrate < BITRATE_UNSPEC || sb->bitrate > BITRATE_512M) - return false; - sb->bitrate = htonl(sb->bitrate); - - return true; -} - - -static bool qos_mode_endecode (struct attr *at, unsigned char ports UNUSED) -{ - unsigned char v = *(char*)at->data; - - return (v == QOS_PORT || v == QOS_DOT); -} - - -static bool qos_endecode (struct attr *at, unsigned char ports) -{ - struct attr_qos *aq = at->data; - - if (aq->port < 1 || aq->port > ports) - return false; - - return (aq->prio >= PRIO_HIGH && aq->prio <= PRIO_LOW); -} - - -static bool pvid_decode (struct attr *at, unsigned char ports) -{ - struct attr_pvid *ap= at->data; - - if (ap->port < 1 || ap->port > ports) - return false; - - ap->vlan = ntohs(ap->vlan); - - return (ap->vlan >= VLAN_MIN && ap->vlan <= VLAN_DOT_MAX); -} - - -static bool pvid_encode (struct attr *at, unsigned char ports) -{ - struct attr_pvid *ap= at->data; - - if (ap->port < 1 || ap->port > ports) - return false; - - if (ap->vlan < VLAN_MIN || ap->vlan > VLAN_DOT_MAX) - return false; - - ap->vlan = htons(ap->vlan); - - return true; -} - - -static bool vlan_destroy_encode (struct attr *at, unsigned char ports UNUSED) -{ - unsigned short v = *(unsigned short*)at->data; - - if (v < VLAN_MIN || v > VLAN_DOT_MAX) - return false; - - *(unsigned short*)at->data = htons(v); - - return true; -} - - -static bool mirror_decode (struct attr *at, unsigned char ports) -{ - unsigned char *r = at->data, *p; - int port; - - - if (at->size != 3 + ((ports - 1) >> 3)) - return false; - - /* r[0] == 0 is allowed and means mirroring is disabled */ - if (r[0] > ports) - return false; - - p = malloc(1 + ports); - if (p == NULL) - return false; - - memset(p, 0, 1 + ports); - - if (r[0] == 0) - goto end; - - p[0] = r[0]; - - for (port = 1; port <= ports; port++) - p[port] = (r[2] >> (8 - port)) & 1; /* FIXME: if ports > 8 */ - -end: - free(at->data); - at->data = p; - at->size = 1 + ports; - - - return true; -} - - -static bool mirror_encode (struct attr *at, unsigned char ports) +struct attr* newAttr (unsigned short attr, unsigned short size, void *data) { - unsigned char *p = at->data, *r; - int port; - - - if (at->size != 1 + ports) - return false; - - /* p[0] == 0 is allowed and means mirroring is disabled */ - if (p[0] > ports) - return false; + struct attr *at; - r = malloc(3 + ((ports - 1) >> 3)); - if (r == NULL) - return false; - - memset(r, 0, 3 + ((ports - 1) >> 3)); - - if (p[0] == 0) - goto end; - - r[0] = p[0]; - - for (port = 1; port <= ports; port++) { - if (p[0] != port) - r[2] |= (p[port] & 1) << (8 - port); /* FIXME: if ports > 8 */ - } - -end: - free(at->data); - at->data = r; - at->size = 3 + ((ports - 1) >> 3); - - - return true; -} - - -static bool igmp_vlan_decode (struct attr *at, unsigned char ports UNUSED) -{ - struct attr_igmp_vlan *aiv = at->data; - aiv->enable = ntohs(aiv->enable); - if (aiv->enable != 0 && aiv->enable != 1) - return false; + at = malloc(sizeof(struct attr)); + if (at == NULL) + return NULL; - aiv->vlan = ntohs(aiv->vlan); - if (aiv->vlan < VLAN_MIN || aiv->vlan > VLAN_DOT_MAX) - return false; - - return true; -} - - -static bool igmp_vlan_encode (struct attr *at, unsigned char ports UNUSED) -{ - struct attr_igmp_vlan *aiv = at->data; - - if (aiv->enable != 0 && aiv->enable != 1) - return false; - aiv->enable = htons(aiv->enable); - - if (aiv->vlan < VLAN_MIN || aiv->vlan > VLAN_DOT_MAX) - return false; - aiv->vlan = htons(aiv->vlan); - - return true; -} - - -static bool cabletest_do_encode (struct attr *at, unsigned char ports) -{ - struct attr_cabletest_do *acd = at->data; - - if (acd->port < 1 || acd->port > ports) - return false; - - return (acd->action == 1); -} - - -static bool cabletest_result_encode (struct attr *at, unsigned char ports) -{ - unsigned char v = *(unsigned char*)at->data; - - return (v >= 1 && v <= ports); -} - - -static bool cabletest_result_decode (struct attr *at, unsigned char ports) -{ - struct attr_cabletest_result *acr = at->data; - - if (acr->port < 1 || acr->port > ports) - return false; - - acr->v1 = ntohl(acr->v1); - acr->v2 = ntohl(acr->v2); - - return true; -} - - -static bool cabletest_result_endecode (struct attr *at, unsigned char ports) -{ - switch (at->size) { - - case 1: - return cabletest_result_encode(at, ports); - - case sizeof(struct attr_cabletest_result): - return cabletest_result_decode(at, ports); - - default: - return false; - } -} - - -static bool vlan_type_endecode (struct attr *at, unsigned char ports UNUSED) -{ - char v = *(char*)at->data; - - return (v >= VLAN_DISABLED && v <= VLAN_DOT_ADV); -} - - -static bool vlan_port_decode (struct attr *at, unsigned char ports) -{ - char *r = at->data; - struct attr_vlan_conf *avc; - int port; - - - if (at->size != (2 + 1 + ((ports - 1) >> 3))) - return false; - - avc = malloc(sizeof(struct attr_vlan_conf) + ports); - if (avc == NULL) - return false; - - avc->vlan = ntohs(*(unsigned short*)r); - r += 2; - - for (port = 0; port < ports; port++) { - /* FIXME: if ports > 8 */ - if ((r[0] >> (7 - port)) & 1) - avc->ports[port] = VLAN_UNTAGGED; - else - avc->ports[port] = VLAN_NO; - } - - free(at->data); - at->data = avc; - at->size = sizeof(struct attr_vlan_conf) + ports; - - - return true; -} - - -static bool vlan_port_encode (struct attr *at, unsigned char ports) -{ - struct attr_vlan_conf *avc = at->data; - char *r; - unsigned int size, port; - - - if (avc->vlan < VLAN_MIN || avc->vlan > VLAN_PORT_MAX) - return false; - - /* just a header is valid */ - if (at->size == sizeof(struct attr_vlan_conf)) - size = 2; - else if (at->size == sizeof(struct attr_vlan_conf) + ports) - size = (2 + 1 + ((ports - 1) >> 3)); - else - return false; - - r = malloc(size); - if (r == NULL) - return false; - - memset(r, 0, size); - *(unsigned short*)r = htons(avc->vlan); - - if (size == 2) - goto end; - - r += 2; - - for (port = 0; port < ports; port++) { - /* FIXME: if ports > 8 */ - if (avc->ports[port] == VLAN_UNTAGGED) - r[0] |= (1 << (7 - port)); - } - - r -= 2; - -end: - free(at->data); - at->data = r; + at->attr = attr; at->size = size; + at->data = data; - return true; + return at; } -static bool vlan_dot_decode (struct attr *at, unsigned char ports) +void freeAttr (struct attr *at) { - char *r = at->data; - struct attr_vlan_conf *avc; - int port; - - - if (at->size != (2 + 2 * (1 + ((ports - 1) >> 3)))) - return false; - - avc = malloc(sizeof(struct attr_vlan_conf) + ports); - if (avc == NULL) - return false; - - avc->vlan = ntohs(*(unsigned short*)r); - r += 2; - - for (port = 0; port < ports; port++) { - /* FIXME: if ports > 8 */ - if ((r[1] >> (7 - port)) & 1) - avc->ports[port] = VLAN_TAGGED; - else if ((r[0] >> (7 - port)) & 1) - avc->ports[port] = VLAN_UNTAGGED; - else - avc->ports[port] = VLAN_NO; + if (at != NULL) { + free(at->data); + free(at); } - - free(at->data); - at->data = avc; - at->size = sizeof(struct attr_vlan_conf) + ports; - - - return true; } -static bool vlan_dot_encode (struct attr *at, unsigned char ports) +void filterAttributes (List *attr, ...) { - struct attr_vlan_conf *avc = at->data; - char *r, fl; - unsigned int size, port; + va_list ap; + ListNode *ln, *pr; + struct attr *at; + unsigned short attrcode; + bool keep; - if (avc->vlan < VLAN_MIN || avc->vlan > VLAN_DOT_MAX) - return false; - - /* just a header is valid */ - if (at->size == sizeof(struct attr_vlan_conf)) - size = 2; - else if (at->size == sizeof(struct attr_vlan_conf) + ports) - size = (2 + 2 * (1 + ((ports - 1) >> 3))); - else - return false; - - r = malloc(size); - if (r == NULL) - return false; - - memset(r, 0, size); - *(unsigned short*)r = htons(avc->vlan); - - if (size == 2) - goto end; - - r += 2; - - for (port = 0; port < ports; port++) { - /* FIXME: if ports > 8 */ - fl = (1 << (7 - port)); - switch (avc->ports[port]) { - case VLAN_TAGGED: - r[1] |= fl; - /* a tagged VLAN is also marked as untagged - * so do not put a "break" here */ - case VLAN_UNTAGGED: - r[0] |= fl; + ln = attr->first; + while (ln != NULL) { + at = ln->data; + + va_start(ap, attr); + keep = false; + attrcode = 0; + while (!keep && attrcode != ATTR_END) { + attrcode = (unsigned short)va_arg(ap, unsigned int); + keep = keep || (at->attr == attrcode); } - } - - r -= 2; - -end: - free(at->data); - at->data = r; - at->size = size; - - - return true; -} - - - -/* WARNING: attributes codes MUST be in ascending order */ -static const struct attr_handler attrtab[] = { - ATTR_HANDLER_ENTRY(ATTR_MAC, 6, NULL, NULL), - ATTR_HANDLER_ENTRY(ATTR_IP, 4, NULL, NULL), - ATTR_HANDLER_ENTRY(ATTR_NETMASK, 4, NULL, NULL), - ATTR_HANDLER_ENTRY(ATTR_GATEWAY, 4, NULL, NULL), - ATTR_HANDLER_ENTRY(ATTR_DHCP, 2, NULL, NULL), - ATTR_HANDLER_ENTRY(ATTR_RESTART, 1, NULL, NULL), - ATTR_HANDLER_ENTRY(ATTR_ENCPASS, 4, NULL, NULL), - ATTR_HANDLER_ENTRY(ATTR_DEFAULTS, 1, NULL, NULL), - ATTR_HANDLER_ENTRY(ATTR_PORT_STATUS, sizeof(struct attr_port_status), NULL, ports_status_decode), - ATTR_HANDLER_ENTRY(ATTR_PORT_STATISTICS, sizeof(struct attr_port_stat), NULL, port_stat_decode), - ATTR_HANDLER_ENTRY(ATTR_STATS_RESET, 1, bool_endecode, bool_endecode), - ATTR_HANDLER_ENTRY(ATTR_CABLETEST_DO, sizeof(struct attr_cabletest_do), cabletest_do_encode, NULL), - ATTR_HANDLER_ENTRY(ATTR_CABLETEST_RESULT, 0, cabletest_result_endecode, cabletest_result_endecode), - ATTR_HANDLER_ENTRY(ATTR_VLAN_TYPE, 1, vlan_type_endecode, vlan_type_endecode), - ATTR_HANDLER_ENTRY(ATTR_VLAN_PORT_CONF, 0, vlan_port_encode, vlan_port_decode), - ATTR_HANDLER_ENTRY(ATTR_VLAN_DOT_CONF, 0, vlan_dot_encode, vlan_dot_decode), - ATTR_HANDLER_ENTRY(ATTR_VLAN_DESTROY, 2, vlan_destroy_encode, NULL), - ATTR_HANDLER_ENTRY(ATTR_VLAN_PVID, sizeof(struct attr_pvid), pvid_encode, pvid_decode), - ATTR_HANDLER_ENTRY(ATTR_QOS_TYPE, 1, qos_mode_endecode, qos_mode_endecode), - ATTR_HANDLER_ENTRY(ATTR_QOS_CONFIG, sizeof(struct attr_qos), qos_endecode, qos_endecode), - ATTR_HANDLER_ENTRY(ATTR_BITRATE_INPUT, sizeof(struct attr_bitrate), bitrate_encode, bitrate_decode), - ATTR_HANDLER_ENTRY(ATTR_BITRATE_OUTPUT, sizeof(struct attr_bitrate), bitrate_encode, bitrate_decode), - ATTR_HANDLER_ENTRY(ATTR_STORM_ENABLE, 1, bool_endecode, bool_endecode), - ATTR_HANDLER_ENTRY(ATTR_STORM_BITRATE, sizeof(struct attr_bitrate), bitrate_encode, bitrate_decode), - ATTR_HANDLER_ENTRY(ATTR_MIRROR, 0, mirror_encode, mirror_decode), - ATTR_HANDLER_ENTRY(ATTR_PORTS_COUNT, 1, NULL, NULL), - ATTR_HANDLER_ENTRY(ATTR_IGMP_ENABLE_VLAN, sizeof(struct attr_igmp_vlan), igmp_vlan_encode, igmp_vlan_decode), - ATTR_HANDLER_ENTRY(ATTR_IGMP_BLOCK_UNK, 1, bool_endecode, bool_endecode), - ATTR_HANDLER_ENTRY(ATTR_IGMP_VALID_V3, 1, bool_endecode, bool_endecode) -}; - - - -const struct attr_handler* getAttrHandler (unsigned short attrcode) -{ - const struct attr_handler *ah; - const unsigned int tab_size = sizeof(attrtab) / sizeof(struct attr_handler); - unsigned int inf, sup, index; - - - inf = 0; - sup = tab_size; - index = tab_size >> 1; - while (index < sup) { - ah = &attrtab[index]; + va_end(ap); - if (ah->attr > attrcode) { - sup = index; - index -= ((index - inf) >> 1) + ((index - inf) & 1); - } else if (ah->attr < attrcode) { - inf = index; - index += ((sup - index) >> 1) + ((sup - index) & 1); + if (keep) { + ln = ln->next; } else { - return ah; + pr = ln; + ln = ln->next; + destroyElement(attr, pr, (void(*)(void*))freeAttr); } } - - return NULL; } diff --git a/raw/src/protocol.c b/raw/src/encoding.c similarity index 56% rename from raw/src/protocol.c rename to raw/src/encoding.c index afb5aa2..9fe4b70 100644 --- a/raw/src/protocol.c +++ b/raw/src/encoding.c @@ -1,55 +1,17 @@ -#include -#include -#include #include #include -#include "attr.h" -#include "protocol.h" +#include +#include +#include "encoding_attr.h" -const char passwordKey[] = "NtgrSmartSwitchRock"; - - -void passwordEndecode (char *buf, unsigned int len) +void initNsdpHeader (struct nsdp_header *nh, char code, const struct ether_addr *client_mac, const struct ether_addr *switch_mac, unsigned int seqnum) { - const char *k = passwordKey; - unsigned int i; - - if (buf == NULL || len <= 0) - return; - - for (i = 0; i < len; i++) { - if (*k == '\0') - k = passwordKey; - buf[i] ^= *k++; - } -} - - -int trim (char *txt, int start) -{ - char *p; - - if (txt == NULL) - return 0; - - p = txt + start; - while (p >= txt && (*p == ' ' || *p == '\n')) { - *p = '\0'; - p--; - } - - return p - txt + 1; -} - - -void initNgHeader (struct ng_header *nh, char code, const struct ether_addr *client_mac, const struct ether_addr *switch_mac, unsigned int seqnum) -{ - memset(nh, 0, sizeof(struct ng_header)); - nh->version = 1; + memset(nh, 0, sizeof(struct nsdp_header)); + nh->version = NSDP_VERSION; nh->code = code; memcpy(nh->client_mac, client_mac, ETH_ALEN); @@ -58,13 +20,13 @@ void initNgHeader (struct ng_header *nh, char code, const struct ether_addr *cli memcpy(nh->switch_mac, switch_mac, ETH_ALEN); nh->seqnum = htonl(seqnum); - memcpy(nh->proto_id, "NSDP", 4); + memcpy(nh->proto_id, NSDP_PROTOID, 4); } -bool validateNgHeader (const struct ng_header *nh, char code, const struct ether_addr *client_mac, const struct ether_addr *switch_mac, unsigned int seqnum) +bool validateNsdpHeader (const struct nsdp_header *nh, char code, const struct ether_addr *client_mac, const struct ether_addr *switch_mac, unsigned int seqnum) { - if (nh->version != 1) + if (nh->version != NSDP_VERSION) return false; if (code > 0 && nh->code != code) @@ -85,7 +47,7 @@ bool validateNgHeader (const struct ng_header *nh, char code, const struct ether if (seqnum > 0 && ntohl(nh->seqnum) != seqnum) return false; - if (memcmp(nh->proto_id, "NSDP", 4) != 0) + if (memcmp(nh->proto_id, NSDP_PROTOID, 4) != 0) return false; if (*(unsigned int*)nh->unk3 != 0) @@ -95,7 +57,7 @@ bool validateNgHeader (const struct ng_header *nh, char code, const struct ether } -void addPacketAttr (struct ng_packet *np, struct attr *at) +static void addPacketAttr (struct nsdp_packet *np, struct attr *at) { struct attr_header *ah = np->ah; @@ -113,34 +75,7 @@ void addPacketAttr (struct ng_packet *np, struct attr *at) } -struct attr* newAttr (unsigned short attr, unsigned short size, void *data) -{ - struct attr *at; - - - at = malloc(sizeof(struct attr)); - if (at == NULL) - return NULL; - - at->attr = attr; - at->size = size; - at->data = data; - - - return at; -} - - -void freeAttr (struct attr *at) -{ - if (at != NULL) { - free(at->data); - free(at); - } -} - - -int addPacketAttributes (struct ng_packet *np, const List* attr, unsigned char ports) +int addPacketAttributes (struct nsdp_packet *np, const List* attr, unsigned char ports) { ListNode *ln; struct attr *at; @@ -168,7 +103,7 @@ int addPacketAttributes (struct ng_packet *np, const List* attr, unsigned char p } -int extractPacketAttributes (struct ng_packet *np, List *attr, unsigned char ports) +int extractPacketAttributes (struct nsdp_packet *np, List *attr, unsigned char ports) { struct attr *at; const struct attr_handler *ah; @@ -178,7 +113,6 @@ int extractPacketAttributes (struct ng_packet *np, List *attr, unsigned char por while (getPacketTotalSize(np) < np->maxlen) { - /* no room for an attribute header: error */ if (getPacketTotalSize(np) + (int)sizeof(struct attr_header) > np->maxlen) { ret = -1; @@ -204,7 +138,7 @@ int extractPacketAttributes (struct ng_packet *np, List *attr, unsigned char por if (size == 0) { at->data = NULL; } else { - at->data = malloc(size * sizeof(char)); + at->data = malloc(size * sizeof(unsigned char)); memcpy(at->data, np->ah->data, size); } @@ -244,37 +178,3 @@ next: } -void filterAttributes (List *attr, ...) -{ - va_list ap; - ListNode *ln, *pr; - struct attr *at; - unsigned short attrcode; - bool keep; - - - ln = attr->first; - while (ln != NULL) { - at = ln->data; - - va_start(ap, attr); - keep = false; - attrcode = 0; - while (!keep && attrcode != ATTR_END) { - attrcode = (unsigned short)va_arg(ap, unsigned int); - keep = keep || (at->attr == attrcode); - } - va_end(ap); - - if (keep) { - ln = ln->next; - } else { - pr = ln; - ln = ln->next; - destroyElement(attr, pr, (void(*)(void*))freeAttr); - } - } - -} - - diff --git a/raw/src/encoding_attr.c b/raw/src/encoding_attr.c new file mode 100644 index 0000000..19f7208 --- /dev/null +++ b/raw/src/encoding_attr.c @@ -0,0 +1,548 @@ + +#include /* FIXME */ +#include +#include "encoding_attr.h" + + +#define UNUSED __attribute__((unused)) +#define ATTR_HANDLER_ENTRY(at, sz, enc, dec) {.attr = at, .size = sz, .encode = enc, .decode = dec} + + + +static bool bool_endecode (struct attr *at, unsigned char ports UNUSED) +{ + *(char*)at->data = (*(char*)at->data != 0); + + return true; +} + + +static bool ports_status_decode (struct attr *at, unsigned char ports) +{ + struct attr_port_status *ps = at->data; + + if (ps->port < 1 || ps->port > ports) + return false; + + switch (ps->status) { + case SPEED_DOWN: + case SPEED_10: + case SPEED_100: + case SPEED_1000: + break; + default: + return false; + } + + return true; +} + + +static bool port_stat_decode (struct attr *at, unsigned char ports) +{ + struct attr_port_stat *ps = at->data; + unsigned long long *v; + + if (ps->port < 1 || ps->port > ports) + return false; + + for (v = &ps->recv; ((char*)v) - ((char*)ps) < (int)sizeof(struct attr_port_stat); v++) + *v = be64toh(*v); + + + return true; +} + + +static bool bitrate_decode (struct attr *at, unsigned char ports) +{ + struct attr_bitrate *sb = at->data; + + if (sb->port < 1 || sb->port > ports) + return false; + + sb->bitrate = ntohl(sb->bitrate); + if (sb->bitrate < BITRATE_UNSPEC || sb->bitrate > BITRATE_512M) + return false; + + return true; +} + + +static bool bitrate_encode (struct attr *at, unsigned char ports) +{ + struct attr_bitrate *sb = at->data; + + if (sb->port < 1 || sb->port > ports) + return false; + + if (sb->bitrate < BITRATE_UNSPEC || sb->bitrate > BITRATE_512M) + return false; + sb->bitrate = htonl(sb->bitrate); + + return true; +} + + +static bool qos_mode_endecode (struct attr *at, unsigned char ports UNUSED) +{ + unsigned char v = *(char*)at->data; + + return (v == QOS_PORT || v == QOS_DOT); +} + + +static bool qos_endecode (struct attr *at, unsigned char ports) +{ + struct attr_qos *aq = at->data; + + if (aq->port < 1 || aq->port > ports) + return false; + + return (aq->prio >= PRIO_HIGH && aq->prio <= PRIO_LOW); +} + + +static bool pvid_decode (struct attr *at, unsigned char ports) +{ + struct attr_pvid *ap= at->data; + + if (ap->port < 1 || ap->port > ports) + return false; + + ap->vlan = ntohs(ap->vlan); + + return (ap->vlan >= VLAN_MIN && ap->vlan <= VLAN_DOT_MAX); +} + + +static bool pvid_encode (struct attr *at, unsigned char ports) +{ + struct attr_pvid *ap= at->data; + + if (ap->port < 1 || ap->port > ports) + return false; + + if (ap->vlan < VLAN_MIN || ap->vlan > VLAN_DOT_MAX) + return false; + + ap->vlan = htons(ap->vlan); + + return true; +} + + +static bool vlan_destroy_encode (struct attr *at, unsigned char ports UNUSED) +{ + unsigned short v = *(unsigned short*)at->data; + + if (v < VLAN_MIN || v > VLAN_DOT_MAX) + return false; + + *(unsigned short*)at->data = htons(v); + + return true; +} + + +static bool mirror_decode (struct attr *at, unsigned char ports) +{ + unsigned char *r = at->data, *p; + int port; + + + if (at->size != 3 + ((ports - 1) >> 3)) + return false; + + /* r[0] == 0 is allowed and means mirroring is disabled */ + if (r[0] > ports) + return false; + + p = malloc(1 + ports); + if (p == NULL) + return false; + + memset(p, 0, 1 + ports); + + if (r[0] == 0) + goto end; + + p[0] = r[0]; + + for (port = 1; port <= ports; port++) + p[port] = (r[2] >> (8 - port)) & 1; /* FIXME: if ports > 8 */ + +end: + free(at->data); + at->data = p; + at->size = 1 + ports; + + + return true; +} + + +static bool mirror_encode (struct attr *at, unsigned char ports) +{ + unsigned char *p = at->data, *r; + int port; + + + if (at->size != 1 + ports) + return false; + + /* p[0] == 0 is allowed and means mirroring is disabled */ + if (p[0] > ports) + return false; + + r = malloc(3 + ((ports - 1) >> 3)); + if (r == NULL) + return false; + + memset(r, 0, 3 + ((ports - 1) >> 3)); + + if (p[0] == 0) + goto end; + + r[0] = p[0]; + + for (port = 1; port <= ports; port++) { + if (p[0] != port) + r[2] |= (p[port] & 1) << (8 - port); /* FIXME: if ports > 8 */ + } + +end: + free(at->data); + at->data = r; + at->size = 3 + ((ports - 1) >> 3); + + + return true; +} + + +static bool igmp_vlan_decode (struct attr *at, unsigned char ports UNUSED) +{ + struct attr_igmp_vlan *aiv = at->data; + + aiv->enable = ntohs(aiv->enable); + if (aiv->enable != 0 && aiv->enable != 1) + return false; + + aiv->vlan = ntohs(aiv->vlan); + if (aiv->vlan < VLAN_MIN || aiv->vlan > VLAN_DOT_MAX) + return false; + + return true; +} + + +static bool igmp_vlan_encode (struct attr *at, unsigned char ports UNUSED) +{ + struct attr_igmp_vlan *aiv = at->data; + + if (aiv->enable != 0 && aiv->enable != 1) + return false; + aiv->enable = htons(aiv->enable); + + if (aiv->vlan < VLAN_MIN || aiv->vlan > VLAN_DOT_MAX) + return false; + aiv->vlan = htons(aiv->vlan); + + return true; +} + + +static bool cabletest_do_encode (struct attr *at, unsigned char ports) +{ + struct attr_cabletest_do *acd = at->data; + + if (acd->port < 1 || acd->port > ports) + return false; + + return (acd->action == 1); +} + + +static bool cabletest_result_encode (struct attr *at, unsigned char ports) +{ + unsigned char v = *(unsigned char*)at->data; + + return (v >= 1 && v <= ports); +} + + +static bool cabletest_result_decode (struct attr *at, unsigned char ports) +{ + struct attr_cabletest_result *acr = at->data; + + if (acr->port < 1 || acr->port > ports) + return false; + + acr->v1 = ntohl(acr->v1); + acr->v2 = ntohl(acr->v2); + + return true; +} + + +static bool cabletest_result_endecode (struct attr *at, unsigned char ports) +{ + switch (at->size) { + + case 1: + return cabletest_result_encode(at, ports); + + case sizeof(struct attr_cabletest_result): + return cabletest_result_decode(at, ports); + + default: + return false; + } +} + + +static bool vlan_type_endecode (struct attr *at, unsigned char ports UNUSED) +{ + char v = *(char*)at->data; + + return (v >= VLAN_DISABLED && v <= VLAN_DOT_ADV); +} + + +static bool vlan_port_decode (struct attr *at, unsigned char ports) +{ + char *r = at->data; + struct attr_vlan_conf *avc; + int port; + + + if (at->size != (2 + 1 + ((ports - 1) >> 3))) + return false; + + avc = malloc(sizeof(struct attr_vlan_conf) + ports); + if (avc == NULL) + return false; + + avc->vlan = ntohs(*(unsigned short*)r); + r += 2; + + for (port = 0; port < ports; port++) { + /* FIXME: if ports > 8 */ + if ((r[0] >> (7 - port)) & 1) + avc->ports[port] = VLAN_UNTAGGED; + else + avc->ports[port] = VLAN_NO; + } + + free(at->data); + at->data = avc; + at->size = sizeof(struct attr_vlan_conf) + ports; + + + return true; +} + + +static bool vlan_port_encode (struct attr *at, unsigned char ports) +{ + struct attr_vlan_conf *avc = at->data; + char *r; + unsigned int size, port; + + + if (avc->vlan < VLAN_MIN || avc->vlan > VLAN_PORT_MAX) + return false; + + /* just a header is valid */ + if (at->size == sizeof(struct attr_vlan_conf)) + size = 2; + else if (at->size == sizeof(struct attr_vlan_conf) + ports) + size = (2 + 1 + ((ports - 1) >> 3)); + else + return false; + + r = malloc(size); + if (r == NULL) + return false; + + memset(r, 0, size); + *(unsigned short*)r = htons(avc->vlan); + + if (size == 2) + goto end; + + r += 2; + + for (port = 0; port < ports; port++) { + /* FIXME: if ports > 8 */ + if (avc->ports[port] == VLAN_UNTAGGED) + r[0] |= (1 << (7 - port)); + } + + r -= 2; + +end: + free(at->data); + at->data = r; + at->size = size; + + + return true; +} + + +static bool vlan_dot_decode (struct attr *at, unsigned char ports) +{ + char *r = at->data; + struct attr_vlan_conf *avc; + int port; + + + if (at->size != (2 + 2 * (1 + ((ports - 1) >> 3)))) + return false; + + avc = malloc(sizeof(struct attr_vlan_conf) + ports); + if (avc == NULL) + return false; + + avc->vlan = ntohs(*(unsigned short*)r); + r += 2; + + for (port = 0; port < ports; port++) { + /* FIXME: if ports > 8 */ + if ((r[1] >> (7 - port)) & 1) + avc->ports[port] = VLAN_TAGGED; + else if ((r[0] >> (7 - port)) & 1) + avc->ports[port] = VLAN_UNTAGGED; + else + avc->ports[port] = VLAN_NO; + } + + free(at->data); + at->data = avc; + at->size = sizeof(struct attr_vlan_conf) + ports; + + + return true; +} + + +static bool vlan_dot_encode (struct attr *at, unsigned char ports) +{ + struct attr_vlan_conf *avc = at->data; + char *r, fl; + unsigned int size, port; + + + if (avc->vlan < VLAN_MIN || avc->vlan > VLAN_DOT_MAX) + return false; + + /* just a header is valid */ + if (at->size == sizeof(struct attr_vlan_conf)) + size = 2; + else if (at->size == sizeof(struct attr_vlan_conf) + ports) + size = (2 + 2 * (1 + ((ports - 1) >> 3))); + else + return false; + + r = malloc(size); + if (r == NULL) + return false; + + memset(r, 0, size); + *(unsigned short*)r = htons(avc->vlan); + + if (size == 2) + goto end; + + r += 2; + + for (port = 0; port < ports; port++) { + /* FIXME: if ports > 8 */ + fl = (1 << (7 - port)); + switch (avc->ports[port]) { + case VLAN_TAGGED: + r[1] |= fl; + /* a tagged VLAN is also marked as untagged + * so do not put a "break" here */ + case VLAN_UNTAGGED: + r[0] |= fl; + } + } + + r -= 2; + +end: + free(at->data); + at->data = r; + at->size = size; + + + return true; +} + + +/* WARNING: attributes codes MUST be in ascending order */ +static const struct attr_handler attrtab[] = { + ATTR_HANDLER_ENTRY(ATTR_MAC, 6, NULL, NULL), + ATTR_HANDLER_ENTRY(ATTR_IP, 4, NULL, NULL), + ATTR_HANDLER_ENTRY(ATTR_NETMASK, 4, NULL, NULL), + ATTR_HANDLER_ENTRY(ATTR_GATEWAY, 4, NULL, NULL), + ATTR_HANDLER_ENTRY(ATTR_DHCP, 2, NULL, NULL), + ATTR_HANDLER_ENTRY(ATTR_RESTART, 1, NULL, NULL), + ATTR_HANDLER_ENTRY(ATTR_ENCPASS, 4, NULL, NULL), + ATTR_HANDLER_ENTRY(ATTR_DEFAULTS, 1, NULL, NULL), + ATTR_HANDLER_ENTRY(ATTR_PORT_STATUS, sizeof(struct attr_port_status), NULL, ports_status_decode), + ATTR_HANDLER_ENTRY(ATTR_PORT_STATISTICS, sizeof(struct attr_port_stat), NULL, port_stat_decode), + ATTR_HANDLER_ENTRY(ATTR_STATS_RESET, 1, bool_endecode, bool_endecode), + ATTR_HANDLER_ENTRY(ATTR_CABLETEST_DO, sizeof(struct attr_cabletest_do), cabletest_do_encode, NULL), + ATTR_HANDLER_ENTRY(ATTR_CABLETEST_RESULT, 0, cabletest_result_endecode, cabletest_result_endecode), + ATTR_HANDLER_ENTRY(ATTR_VLAN_TYPE, 1, vlan_type_endecode, vlan_type_endecode), + ATTR_HANDLER_ENTRY(ATTR_VLAN_PORT_CONF, 0, vlan_port_encode, vlan_port_decode), + ATTR_HANDLER_ENTRY(ATTR_VLAN_DOT_CONF, 0, vlan_dot_encode, vlan_dot_decode), + ATTR_HANDLER_ENTRY(ATTR_VLAN_DESTROY, 2, vlan_destroy_encode, NULL), + ATTR_HANDLER_ENTRY(ATTR_VLAN_PVID, sizeof(struct attr_pvid), pvid_encode, pvid_decode), + ATTR_HANDLER_ENTRY(ATTR_QOS_TYPE, 1, qos_mode_endecode, qos_mode_endecode), + ATTR_HANDLER_ENTRY(ATTR_QOS_CONFIG, sizeof(struct attr_qos), qos_endecode, qos_endecode), + ATTR_HANDLER_ENTRY(ATTR_BITRATE_INPUT, sizeof(struct attr_bitrate), bitrate_encode, bitrate_decode), + ATTR_HANDLER_ENTRY(ATTR_BITRATE_OUTPUT, sizeof(struct attr_bitrate), bitrate_encode, bitrate_decode), + ATTR_HANDLER_ENTRY(ATTR_STORM_ENABLE, 1, bool_endecode, bool_endecode), + ATTR_HANDLER_ENTRY(ATTR_STORM_BITRATE, sizeof(struct attr_bitrate), bitrate_encode, bitrate_decode), + ATTR_HANDLER_ENTRY(ATTR_MIRROR, 0, mirror_encode, mirror_decode), + ATTR_HANDLER_ENTRY(ATTR_PORTS_COUNT, 1, NULL, NULL), + ATTR_HANDLER_ENTRY(ATTR_IGMP_ENABLE_VLAN, sizeof(struct attr_igmp_vlan), igmp_vlan_encode, igmp_vlan_decode), + ATTR_HANDLER_ENTRY(ATTR_IGMP_BLOCK_UNK, 1, bool_endecode, bool_endecode), + ATTR_HANDLER_ENTRY(ATTR_IGMP_VALID_V3, 1, bool_endecode, bool_endecode) +}; + + +const struct attr_handler* getAttrHandler (unsigned short attrcode) +{ + const struct attr_handler *ah; + const unsigned int tab_size = sizeof(attrtab) / sizeof(struct attr_handler); + unsigned int inf, sup, index; + + + inf = 0; + sup = tab_size; + index = tab_size >> 1; + while (index < sup) { + ah = &attrtab[index]; + + if (ah->attr > attrcode) { + sup = index; + index -= ((index - inf) >> 1) + ((index - inf) & 1); + } else if (ah->attr < attrcode) { + inf = index; + index += ((sup - index) >> 1) + ((sup - index) & 1); + } else { + return ah; + } + } + + + return NULL; +} + + diff --git a/raw/src/encoding_attr.h b/raw/src/encoding_attr.h new file mode 100644 index 0000000..a3cb18b --- /dev/null +++ b/raw/src/encoding_attr.h @@ -0,0 +1,24 @@ + +#ifndef DEF_ENCODING_ATTR +#define DEF_ENCODING_ATTR + + +#include + +#include + + + +struct attr_handler { + unsigned short attr; /* attribute code */ + unsigned int size; /* expected data size */ + bool (*encode)(struct attr *at, unsigned char ports); /* encode function */ + bool (*decode)(struct attr *at, unsigned char ports); /* decode function */ +}; + + +const struct attr_handler* getAttrHandler (unsigned short attrcode); + + +#endif + diff --git a/raw/src/misc.c b/raw/src/misc.c new file mode 100644 index 0000000..58db4aa --- /dev/null +++ b/raw/src/misc.c @@ -0,0 +1,42 @@ + +#include + +#include + + +static const char passwordKey[] = "NtgrSmartSwitchRock"; + + +void passwordEndecode (char *buf, unsigned int len) +{ + const char *k = passwordKey; + unsigned int i; + + if (buf == NULL || len <= 0) + return; + + for (i = 0; i < len; i++) { + if (*k == '\0') + k = passwordKey; + buf[i] ^= *k++; + } +} + + +int trim (char *txt, int start) +{ + char *p; + + if (txt == NULL) + return 0; + + p = txt + start; + while (p >= txt && (*p == ' ' || *p == '\n')) { + *p = '\0'; + p--; + } + + return p - txt + 1; +} + + diff --git a/spy/src/spy.c b/spy/src/spy.c index df374ee..074f919 100644 --- a/spy/src/spy.c +++ b/spy/src/spy.c @@ -3,14 +3,14 @@ #include #include -#include #include +#include int main (void) { - char buffer[1500]; - struct ng_packet np; + unsigned char buffer[1500]; + struct nsdp_packet np; int err = 0, s, len; struct sockaddr_in local, remote; socklen_t slen = sizeof(struct sockaddr_in); @@ -53,13 +53,13 @@ int main (void) np.buffer = buffer; np.maxlen = len; - initNgPacket(&np); + initNsdpPacket(&np); attr = createEmptyList(); if (ntohs(remote.sin_port) != CLIENT_PORT || - len < (int)sizeof(struct ng_header) || - !validateNgHeader(np.nh, 0, NULL, NULL, 0) || + len < (int)sizeof(struct nsdp_header) || + !validateNsdpHeader(np.nh, 0, NULL, NULL, 0) || extractPacketAttributes(&np, attr, 0) < 0) { printf("wrong packet\n"); goto end; -- 2.39.2