Skip to content

Commit

Permalink
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[UDP] replace UDP offsets by struct UdpHeader
Browse files Browse the repository at this point in the history
Arnaud authored and Arnaud committed Feb 10, 2019

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature.
1 parent 5ca0418 commit 7b2837f
Showing 7 changed files with 179 additions and 171 deletions.
25 changes: 13 additions & 12 deletions examples/SSDP/SSDP.ino
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
#include <EtherCard.h>// |Mac adress|
#include <EtherUtil.h>
const char SSDP_RESPONSE[] PROGMEM = "HTTP/1.1 200 OK\r\nCACHE-CONTROL: max-age=1200\r\nEXT:\r\nSERVER:Arduino\r\nST: upnp:rootdevice\r\nUSN: uuid:abcdefgh-7dec-11d0-a765-7499692d3040\r\nLOCATION: http://"; //dont forget our mac adress USN: uuid:abcdefgh-7dec-11d0-a765-Mac addr
const char SSDP_RESPONSE_XML[] PROGMEM = "/??\r\n\r\n"; // here is the adress of xml file /?? in this exemple but you could use another /upnp.xml\r\n\r\n
const char XML_DESCRIP[] PROGMEM = "HTTP/1.1 200 OK\r\nContent-Type: text/xml\r\n\r\n<?xml version='1.0'?>\r<root xmlns='urn:schemas-upnp-org:device-1-0'><device><deviceType>urn:schemas-upnp-org:device:BinaryLight:1</deviceType><presentationURL>/</presentationURL><friendlyName>Arduino</friendlyName><manufacturer>Fredycpu</manufacturer><manufacturerURL>http://fredycpu.pro</manufacturerURL><serialNumber>1</serialNumber><UDN>uuid:abcdefgh-7dec-11d0-a765-7499692d3040</UDN></device></root> ";
@@ -49,7 +50,7 @@ const char pageD[] PROGMEM =
;

void ssdpresp();
void addip(int udppos);
void addip(uint8_t *udppos);
void ssdpnotify();

void setup(){
@@ -100,46 +101,46 @@ void ssdpresp() { //response to m-search
for( int i=0; i<IP_LEN;i++) { //extract source IP of request
ip_dst[i]=Ethernet::buffer[i+26];
}
int udppos = UDP_DATA_P;
uint8_t *udppos = udp_payload();

EtherCard::udpPrepare(1900,ip_dst,port_dst);
memcpy_P(Ethernet::buffer + udppos, SSDP_RESPONSE, sizeof SSDP_RESPONSE);
memcpy_P(udppos, SSDP_RESPONSE, sizeof SSDP_RESPONSE);
udppos = udppos + sizeof SSDP_RESPONSE-1;
addip(udppos);
}

void ssdpnotify() { //Notification
int udppos = UDP_DATA_P;
int udppos = udp_payload();
EtherCard::udpPrepare(1900,ssdp,1900);
memcpy_P(Ethernet::buffer + udppos, SSDP_NOTIFY, sizeof SSDP_NOTIFY);
memcpy_P(udppos, SSDP_NOTIFY, sizeof SSDP_NOTIFY);
udppos = udppos + sizeof SSDP_NOTIFY-1;
addip(udppos);
}

void addip(int udppos) { // add current ip to the request and send it
void addip(uint8_t *udppos) { // add current ip to the request and send it
int adr;
for(int i=0;i<IP_LEN;i++) { // extract the current ip of arduino
adr = ether.myip[i]/100;
if (adr) {
Ethernet::buffer[udppos]=adr+48;
*udppos=adr+48;
udppos++;
}
adr=(ether.myip[i]%100)/10;
if (adr||(ether.myip[i]/100)) {
Ethernet::buffer[udppos]=adr+48;
*udppos=adr+48;
udppos++;
}
adr=ether.myip[i]%10;
Ethernet::buffer[udppos]=adr+48;
*udppos=adr+48;
udppos++;
Ethernet::buffer[udppos]=46;
*udppos=46;
udppos++; //"."
}
udppos--;//erase the last point
memcpy_P(Ethernet::buffer + udppos,SSDP_RESPONSE_XML,sizeof SSDP_RESPONSE_XML);
memcpy_P(udppos,SSDP_RESPONSE_XML,sizeof SSDP_RESPONSE_XML);
udppos = udppos + sizeof SSDP_RESPONSE_XML;
udppos--;
EtherCard::udpTransmit(udppos-UDP_DATA_P); // send all to the computer who make the request on her ip and port who make the request
EtherCard::udpTransmit(udppos-udp_payload()); // send all to the computer who make the request on her ip and port who make the request
}


53 changes: 53 additions & 0 deletions src/EtherUtil.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,53 @@
// Shortcuts to different protocols headers/payload
// Hence: GPL V2

#ifndef ETHERUTIL_H
#define ETHERUTIL_H

#include "enc28j60.h"
#include "net.h"

#define gPB ether.buffer

inline EthHeader &ethernet_header()
{
uint8_t *iter = gPB; // avoid strict aliasing warning
return *(EthHeader *)iter;
}

inline uint8_t *ethernet_payload()
{
return (uint8_t *)&ethernet_header() + sizeof(EthHeader);
}

inline ArpHeader &arp_header()
{
return *(ArpHeader *)ethernet_payload();
}

inline IpHeader &ip_header()
{
return *(IpHeader *)ethernet_payload();
}

inline uint8_t *ip_payload()
{
return (uint8_t *)&ip_header() + sizeof(IpHeader);
}

inline uint8_t *tcp_header()
{
return (uint8_t *)ip_payload();
}

inline UdpHeader &udp_header()
{
return *(UdpHeader *)ip_payload();
}

inline uint8_t *udp_payload()
{
return (uint8_t *)&udp_header() + sizeof(UdpHeader);
}

#endif /* ETHERUTIL_H */
35 changes: 17 additions & 18 deletions src/dhcp.cpp
Original file line number Diff line number Diff line change
@@ -15,10 +15,9 @@
//#define DHCPDEBUG

#include "EtherCard.h"
#include "EtherUtil.h"
#include "net.h"

#define gPB ether.buffer

#define DHCP_BOOTREQUEST 1
#define DHCP_BOOTRESPONSE 2

@@ -83,6 +82,7 @@ typedef struct {
uint16_t secs, flags;
byte ciaddr[IP_LEN], yiaddr[IP_LEN], siaddr[IP_LEN], giaddr[IP_LEN];
byte chaddr[16], sname[64], file[128];
uint32_t magicCookie;
// options
} DHCPdata;

@@ -162,8 +162,6 @@ static void addOption (byte opt, byte len, const byte* data) {

static void send_dhcp_message(uint8_t *requestip) {

memset(gPB, 0, UDP_DATA_P + sizeof( DHCPdata ));

EtherCard::udpPrepare(DHCP_CLIENT_PORT,
(dhcpState == DHCP_STATE_BOUND ? EtherCard::dhcpip : allOnes),
DHCP_SERVER_PORT);
@@ -174,7 +172,8 @@ static void send_dhcp_message(uint8_t *requestip) {
EtherCard::copyMac(gPB + ETH_DST_MAC, allOnes); //force broadcast mac

// Build DHCP Packet from buf[UDP_DATA_P]
DHCPdata *dhcpPtr = (DHCPdata*) (gPB + UDP_DATA_P);
DHCPdata *dhcpPtr = (DHCPdata*) (udp_payload());
memset(dhcpPtr, 0, sizeof(DHCPdata));
dhcpPtr->op = DHCP_BOOTREQUEST;
dhcpPtr->htype = 1;
dhcpPtr->hlen = 6;
@@ -184,12 +183,12 @@ static void send_dhcp_message(uint8_t *requestip) {
}
EtherCard::copyMac(dhcpPtr->chaddr, EtherCard::mymac);

// options defined as option, length, value
bufPtr = gPB + UDP_DATA_P + sizeof( DHCPdata );
// DHCP magic cookie
static const byte cookie[] PROGMEM = { 0x63,0x82,0x53,0x63 };
for (byte i = 0; i < sizeof(cookie); i++)
addToBuf(pgm_read_byte(&cookie[i]));
dhcpPtr->magicCookie = HTONL(0x63825363);

// options defined as option, length, value
bufPtr = (uint8_t *)dhcpPtr + sizeof( DHCPdata );

addToBuf(DHCP_OPT_MESSAGE_TYPE); // DHCP_STATE_SELECTING, DHCP_STATE_REQUESTING
addToBuf(1); // Length
addToBuf(dhcpState == DHCP_STATE_INIT ? DHCP_DISCOVER : DHCP_REQUEST);
@@ -229,18 +228,18 @@ static void send_dhcp_message(uint8_t *requestip) {
addToBuf(DHCP_OPT_END);

// packet size will be under 300 bytes
EtherCard::udpTransmit((bufPtr - gPB) - UDP_DATA_P);
EtherCard::udpTransmit(bufPtr - (uint8_t *)dhcpPtr);
}

static void process_dhcp_offer(uint16_t len, uint8_t *offeredip) {
// Map struct onto payload
DHCPdata *dhcpPtr = (DHCPdata*) (gPB + UDP_DATA_P);
DHCPdata *dhcpPtr = (DHCPdata*) (udp_payload());

// Offered IP address is in yiaddr
EtherCard::copyIp(offeredip, dhcpPtr->yiaddr);

// Search for the server IP
byte *ptr = (byte*) (dhcpPtr + 1) + 4;
byte *ptr = (byte*) (dhcpPtr + 1);
do {
byte option = *ptr++;
byte optionLen = *ptr++;
@@ -254,13 +253,13 @@ static void process_dhcp_offer(uint16_t len, uint8_t *offeredip) {

static void process_dhcp_ack(uint16_t len) {
// Map struct onto payload
DHCPdata *dhcpPtr = (DHCPdata*) (gPB + UDP_DATA_P);
DHCPdata *dhcpPtr = (DHCPdata*) (udp_payload());

// Allocated IP address is in yiaddr
EtherCard::copyIp(EtherCard::myip, dhcpPtr->yiaddr);

// Scan through variable length option list identifying options we want
byte *ptr = (byte*) (dhcpPtr + 1) + 4;
byte *ptr = (byte*) (dhcpPtr + 1);
bool done = false;
do {
byte option = *ptr++;
@@ -308,12 +307,12 @@ while (!done && ptr < gPB + len);

static bool dhcp_received_message_type (uint16_t len, byte msgType) {
// Map struct onto payload
DHCPdata *dhcpPtr = (DHCPdata*) (gPB + UDP_DATA_P);
DHCPdata *dhcpPtr = (DHCPdata*) (udp_payload());

if (len >= 70 && gPB[UDP_SRC_PORT_L_P] == DHCP_SERVER_PORT &&
if (len >= 70 && udp_header().sport == HTONS(DHCP_SERVER_PORT) &&
dhcpPtr->xid == currentXid ) {

byte *ptr = (byte*) (dhcpPtr + 1) + 4;
byte *ptr = (byte*) (dhcpPtr + 1);
do {
byte option = *ptr++;
byte optionLen = *ptr++;
27 changes: 14 additions & 13 deletions src/dns.cpp
Original file line number Diff line number Diff line change
@@ -5,10 +5,9 @@
// 2010-05-20 <[email protected]>

#include "EtherCard.h"
#include "EtherUtil.h"
#include "net.h"

#define gPB ether.buffer

static byte dnstid_l; // a counter for transaction ID
#define DNSCLIENT_SRC_PORT_H 0xE0

@@ -20,9 +19,11 @@ static void dnsRequest (const char *hostname, bool fromRam) {
if (ether.dnsip[0] == 0)
memset(ether.dnsip, 8, IP_LEN); // use 8.8.8.8 Google DNS as default
ether.udpPrepare((DNSCLIENT_SRC_PORT_H << 8) | dnstid_l, ether.dnsip, DNS_PORT);
memset(gPB + UDP_DATA_P, 0, 12);
uint8_t *udpp = udp_payload();
byte *p = udpp;
memset(p, 0, 12);

byte *p = gPB + UDP_DATA_P + 12;
p += 12;
char c;
do {
byte n = 0;
@@ -42,11 +43,11 @@ static void dnsRequest (const char *hostname, bool fromRam) {
*p++ = DNS_TYPE_A;
*p++ = 0;
*p++ = DNS_CLASS_IN;
byte i = p - gPB - UDP_DATA_P;
gPB[UDP_DATA_P] = i;
gPB[UDP_DATA_P+1] = dnstid_l;
gPB[UDP_DATA_P+2] = 1; // flags, standard recursive query
gPB[UDP_DATA_P+5] = 1; // 1 question
byte i = p - udpp;
udpp[0] = i;
udpp[1] = dnstid_l;
udpp[2] = 1; // flags, standard recursive query
udpp[5] = 1; // 1 question
ether.udpTransmit(i);
}

@@ -56,10 +57,10 @@ static void dnsRequest (const char *hostname, bool fromRam) {
@note hisip contains IP address of requested host or 0.0.0.0 on failure
*/
static bool checkForDnsAnswer (uint16_t plen) {
byte *p = gPB + UDP_DATA_P; //start of UDP payload
if (plen < 70 || gPB[UDP_SRC_PORT_L_P] != DNS_PORT || //from DNS source port
gPB[UDP_DST_PORT_H_P] != DNSCLIENT_SRC_PORT_H || //response to same port as we sent from (MSB)
gPB[UDP_DST_PORT_L_P] != dnstid_l || //response to same port as we sent from (LSB)
UdpHeader &udph = udp_header();
byte *p = udp_payload(); //start of UDP payload
if (plen < 70 || udph.sport != HTONS(DNS_PORT) || //from DNS source port
ntohs(udph.dport) != (uint16_t)(DNSCLIENT_SRC_PORT_H << 8 | dnstid_l) || //response to same port as we sent from
p[1] != dnstid_l) //message id same as we sent
return false; //not our DNS response
if((p[3] & 0x0F) != 0)
25 changes: 13 additions & 12 deletions src/net.h
Original file line number Diff line number Diff line change
@@ -50,6 +50,12 @@
# error __BYTE_ORDER__ not defined! PLease define it for your platform
#endif

inline void htons(uint16_t &d, const uint16_t v)
{
((uint8_t *)(&d))[0] = v >> 8;
((uint8_t *)(&d))[1] = v & 0xFF;
}

inline uint16_t htons(const uint16_t v)
{ return HTONS(v); }

@@ -193,18 +199,13 @@ struct ArpHeader
#define ICMP_DATA_P 0x2a

// ******* UDP *******
#define UDP_HEADER_LEN 8
//
#define UDP_SRC_PORT_H_P 0x22
#define UDP_SRC_PORT_L_P 0x23
#define UDP_DST_PORT_H_P 0x24
#define UDP_DST_PORT_L_P 0x25
//
#define UDP_LEN_H_P 0x26
#define UDP_LEN_L_P 0x27
#define UDP_CHECKSUM_H_P 0x28
#define UDP_CHECKSUM_L_P 0x29
#define UDP_DATA_P 0x2a
struct UdpHeader
{
uint16_t sport;
uint16_t dport;
uint16_t length;
uint16_t checksum;
};

// ******* TCP *******
#define TCP_SRC_PORT_H_P 0x22
140 changes: 43 additions & 97 deletions src/tcpip.cpp
Original file line number Diff line number Diff line change
@@ -13,10 +13,9 @@

#include "EtherCard.h"
#include "net.h"
#include "EtherUtil.h"
#undef word // arduino nonsense

#define gPB ether.buffer

#define PINGPATTERN 0x42

// Avoid spurious pgmspace warnings - http://forum.jeelabs.net/node/327
@@ -74,43 +73,6 @@ extern const uint8_t allOnes[] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }; // Used
// Serial.println();
// }

static inline EthHeader &ethernet_header()
{
uint8_t *iter = gPB; // avoid strict aliasing warning
return *(EthHeader *)iter;
}

static inline uint8_t *ethernet_payload()
{
return (uint8_t *)&ethernet_header() + sizeof(EthHeader);
}

static inline ArpHeader &arp_header()
{
return *(ArpHeader *)ethernet_payload();
}

static inline IpHeader &ip_header()
{
return *(IpHeader *)ethernet_payload();
}

static inline uint8_t *ip_payload()
{
return (uint8_t *)&ip_header() + sizeof(IpHeader);
}

static inline uint8_t *tcp_header()
{
return (uint8_t *)ip_payload();
}

static inline uint8_t *udp_header()
{
return (uint8_t *)ip_payload();
}


static void fill_checksum(uint16_t &checksum, const uint8_t *ptr, uint16_t len, uint8_t type) {
uint32_t sum = type==1 ? IP_PROTO_UDP_V+len-8 :
type==2 ? IP_PROTO_TCP_V+len-8 : 0;
@@ -232,7 +194,7 @@ static void make_eth_ip_reply(const uint16_t payloadlen = 0xFFFF) {

IpHeader &iph = ip_header();
if (payloadlen != 0xFFFF)
iph.totalLen = htons(ip_payload() - (uint8_t *)&iph + payloadlen);
htons(iph.totalLen, ip_payload() - (uint8_t *)&iph + payloadlen);

fill_ip_hdr_checksum(iph);
}
@@ -296,19 +258,16 @@ void EtherCard::makeUdpReply (const char *data,uint8_t datalen,uint16_t port) {
if (datalen>220)
datalen = 220;

make_eth_ip_reply(UDP_HEADER_LEN + datalen);
make_eth_ip_reply(sizeof(UdpHeader) + datalen);
IpHeader &iph = ip_header();
gPB[UDP_DST_PORT_H_P] = gPB[UDP_SRC_PORT_H_P];
gPB[UDP_DST_PORT_L_P] = gPB[UDP_SRC_PORT_L_P];
gPB[UDP_SRC_PORT_H_P] = port>>8;
gPB[UDP_SRC_PORT_L_P] = port;
gPB[UDP_LEN_H_P] = (UDP_HEADER_LEN+datalen) >> 8;
gPB[UDP_LEN_L_P] = UDP_HEADER_LEN+datalen;
gPB[UDP_CHECKSUM_H_P] = 0;
gPB[UDP_CHECKSUM_L_P] = 0;
memcpy(gPB + UDP_DATA_P, data, datalen);
fill_checksum(UDP_CHECKSUM_H_P, (uint8_t *)&iph.spaddr - gPB, 16 + datalen,1);
packetSend(udp_header() - gPB + UDP_HEADER_LEN + datalen);
UdpHeader &udph = udp_header();
udph.dport = udph.sport;
htons(udph.sport, port);
htons(udph.length, sizeof(UdpHeader)+datalen);
udph.checksum = 0;
memcpy(udp_payload(), data, datalen);
fill_checksum(udph.checksum, (const uint8_t *)&iph.spaddr, 16 + datalen,1);
packetSend(udp_payload() - gPB + datalen);
}

static void make_tcp_synack_from_syn() {
@@ -353,7 +312,7 @@ static void make_tcp_ack_from_any(int16_t datlentoack,uint8_t addflags) {

static void make_tcp_ack_with_data_noflags(uint16_t dlen) {
IpHeader &iph = ip_header();
iph.totalLen = htons(ip_payload() - (uint8_t *)&ip_header() + TCP_HEADER_LEN_PLAIN + dlen);
htons(iph.totalLen, ip_payload() - (uint8_t *)&ip_header() + TCP_HEADER_LEN_PLAIN + dlen);
fill_ip_hdr_checksum(iph);
gPB[TCP_CHECKSUM_H_P] = 0;
gPB[TCP_CHECKSUM_L_P] = 0;
@@ -428,23 +387,22 @@ void EtherCard::ntpRequest (uint8_t *ntpip, uint8_t srcport) {
IpHeader &iph = init_ip_frame(ntpip, IP_PROTO_UDP_V);
iph.totalLen = HTONS(0x4c);
fill_ip_hdr_checksum(iph);
gPB[UDP_DST_PORT_H_P] = 0;
gPB[UDP_DST_PORT_L_P] = NTP_PORT; // ntp = 123
gPB[UDP_SRC_PORT_H_P] = 10;
gPB[UDP_SRC_PORT_L_P] = srcport; // lower 8 bit of src port
gPB[UDP_LEN_H_P] = 0;
gPB[UDP_LEN_L_P] = 56; // fixed len
gPB[UDP_CHECKSUM_H_P] = 0;
gPB[UDP_CHECKSUM_L_P] = 0;
memset(gPB + UDP_DATA_P, 0, 48);
memcpy_P(gPB + UDP_DATA_P,ntpreqhdr,10);
fill_checksum(UDP_CHECKSUM_H_P, iph.spaddr - gPB, 16 + 48, 1);
UdpHeader &udph = udp_header();
uint8_t *udpp = udp_payload();
udph.dport = HTONS(NTP_PORT);
udph.sport = HTONS((10 << 8) | srcport);
udph.length = HTONS(56);
udph.checksum = 0;
memset(udpp, 0, 48);
memcpy_P(udpp,ntpreqhdr,10);
fill_checksum(udph.checksum, (const uint8_t *)&iph.spaddr, 16 + 48, 1);
packetSend(90);
}

uint8_t EtherCard::ntpProcessAnswer (uint32_t *time,uint8_t dstport_l) {
if ((dstport_l && gPB[UDP_DST_PORT_L_P]!=dstport_l) || gPB[UDP_LEN_H_P]!=0 ||
gPB[UDP_LEN_L_P]!=56 || gPB[UDP_SRC_PORT_L_P]!=0x7b)
UdpHeader &udph = udp_header();
if ((dstport_l && (ntohs(udph.dport) & 0xFF) != dstport_l) || udph.length != HTONS(56)
|| udph.sport != HTONS(NTP_PORT))
return 0;
((uint8_t*) time)[3] = gPB[0x52];
((uint8_t*) time)[2] = gPB[0x53];
@@ -455,54 +413,42 @@ uint8_t EtherCard::ntpProcessAnswer (uint32_t *time,uint8_t dstport_l) {

void EtherCard::udpPrepare (uint16_t sport, const uint8_t *dip, uint16_t dport) {
init_ip_frame(dip, IP_PROTO_UDP_V);
gPB[UDP_DST_PORT_H_P] = (dport>>8);
gPB[UDP_DST_PORT_L_P] = dport;
gPB[UDP_SRC_PORT_H_P] = (sport>>8);
gPB[UDP_SRC_PORT_L_P] = sport;
gPB[UDP_LEN_H_P] = 0;
gPB[UDP_CHECKSUM_H_P] = 0;
gPB[UDP_CHECKSUM_L_P] = 0;
UdpHeader &udph = udp_header();
htons(udph.dport, dport);
htons(udph.sport, sport);
udph.length = 0;
udph.checksum = 0;
}

void EtherCard::udpTransmit (uint16_t datalen) {
IpHeader &iph = ip_header();
iph.totalLen = htons(udp_header() - (uint8_t *)&ip_header() + UDP_HEADER_LEN + datalen);
htons(iph.totalLen, sizeof(IpHeader) + sizeof(UdpHeader) + datalen);
fill_ip_hdr_checksum(iph);
gPB[UDP_LEN_H_P] = (UDP_HEADER_LEN+datalen) >>8;
gPB[UDP_LEN_L_P] = UDP_HEADER_LEN+datalen;
fill_checksum(UDP_CHECKSUM_H_P, (uint8_t *)&iph.spaddr - gPB, 16 + datalen,1);
packetSend(udp_header() - gPB + UDP_HEADER_LEN + datalen);

UdpHeader &udph = udp_header();
htons(udph.length, sizeof(UdpHeader) + datalen);
fill_checksum(udph.checksum, (const uint8_t *)&iph.spaddr, 16 + datalen,1);
packetSend(udp_payload() - gPB + datalen);
}

void EtherCard::sendUdp (const char *data, uint8_t datalen, uint16_t sport,
const uint8_t *dip, uint16_t dport) {
udpPrepare(sport, dip, dport);
if (datalen>220)
datalen = 220;
memcpy(gPB + UDP_DATA_P, data, datalen);
memcpy(udp_payload(), data, datalen);
udpTransmit(datalen);
}

void EtherCard::sendWol (uint8_t *wolmac) {
IpHeader &iph = init_ip_frame(allOnes, IP_PROTO_UDP_V);
iph.totalLen = HTONS(0x82);
fill_ip_hdr_checksum(iph);
gPB[UDP_DST_PORT_H_P] = 0;
gPB[UDP_DST_PORT_L_P] = 0x9; // wol = normally 9
gPB[UDP_SRC_PORT_H_P] = 10;
gPB[UDP_SRC_PORT_L_P] = 0x42; // source port does not matter
gPB[UDP_LEN_H_P] = 0;
gPB[UDP_LEN_L_P] = 110; // fixed len
gPB[UDP_CHECKSUM_H_P] = 0;
gPB[UDP_CHECKSUM_L_P] = 0;
copyMac(gPB + UDP_DATA_P, allOnes);
uint8_t pos = UDP_DATA_P;
for (uint8_t m = 0; m < 16; ++m) {
pos += 6;
copyMac(gPB + pos, wolmac);
udpPrepare(0x1042, allOnes, 9);
uint8_t *pos = udp_payload();
copyMac(pos, allOnes);
pos += 6;
for (uint8_t m = 0; m < 16; ++m, pos += 6) {
copyMac(pos, wolmac);
}
fill_checksum(UDP_CHECKSUM_H_P, (uint8_t *)&iph.spaddr - gPB, 16 + 102,1);
packetSend(pos + 6);
udpTransmit(6 + 16*6);
}

// make a arp request
45 changes: 26 additions & 19 deletions src/udpserver.cpp
Original file line number Diff line number Diff line change
@@ -6,10 +6,9 @@
// See http://www.gnu.org/licenses/gpl.html

#include "EtherCard.h"
#include "EtherUtil.h"
#include "net.h"

#define gPB ether.buffer

#define UDPSERVER_MAXLISTENERS 8 //the maximum number of port listeners.

typedef struct {
@@ -31,22 +30,26 @@ void EtherCard::udpServerListenOnPort(UdpServerCallback callback, uint16_t port)
}
}

void EtherCard::udpServerPauseListenOnPort(uint16_t port) {
for(int i = 0; i < numListeners; i++)
static void udp_listen_on_port(const uint16_t port, const bool listen)
{
for (UdpServerListener *iter = listeners, *last = listeners + numListeners;
iter != last; ++iter)
{
if(gPB[UDP_DST_PORT_H_P] == (listeners[i].port >> 8) && gPB[UDP_DST_PORT_L_P] == ((byte) listeners[i].port)) {
listeners[i].listening = false;
UdpServerListener &l = *iter;
if(l.port == port)
{
l.listening = listen;
break;
}
}
}

void EtherCard::udpServerPauseListenOnPort(uint16_t port) {
udp_listen_on_port(port, false);
}

void EtherCard::udpServerResumeListenOnPort(uint16_t port) {
for(int i = 0; i < numListeners; i++)
{
if(gPB[UDP_DST_PORT_H_P] == (listeners[i].port >> 8) && gPB[UDP_DST_PORT_L_P] == ((byte) listeners[i].port)) {
listeners[i].listening = true;
}
}
udp_listen_on_port(port, true);
}

bool EtherCard::udpServerListening() {
@@ -55,16 +58,20 @@ bool EtherCard::udpServerListening() {

bool EtherCard::udpServerHasProcessedPacket(const IpHeader &iph, const uint8_t *iter, const uint8_t *last) {
bool packetProcessed = false;
for(int i = 0; i < numListeners; i++)
UdpHeader &udph = udp_header();
const uint16_t dport = ntohs(udph.dport);
for (UdpServerListener *iter = listeners, *last = listeners + numListeners;
iter != last; ++iter)
{
if(gPB[UDP_DST_PORT_H_P] == (listeners[i].port >> 8) && gPB[UDP_DST_PORT_L_P] == ((byte) listeners[i].port) && listeners[i].listening)
UdpServerListener &l = *iter;
if (l.listening && l.port == dport)
{
uint16_t datalen = (uint16_t) (gPB[UDP_LEN_H_P] << 8) + gPB[UDP_LEN_L_P] - UDP_HEADER_LEN;
listeners[i].callback(
listeners[i].port,
const uint16_t datalen = udph.length - sizeof(UdpHeader);
l.callback(
l.port,
(uint8_t *)iph.spaddr, // TODO: change definition of UdpServerCallback to const uint8_t *
(gPB[UDP_SRC_PORT_H_P] << 8) | gPB[UDP_SRC_PORT_L_P],
(const char *) (gPB + UDP_DATA_P),
ntohs(udph.sport),
(const char *)udp_payload(),
datalen);
packetProcessed = true;
}

0 comments on commit 7b2837f

Please sign in to comment.