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
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.