giomba/coppino
1
0
Fork 0
Code Issues 4 Pull Requests Projects Releases Wiki Activity

basic udp echo server

Browse Source
This commit is contained in:
giomba 2019-12-24 14:47:32 +01:00
parent c05386db26
commit b21625cd27
3 changed files with 95 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ipv6.ino
View File

@ -1,4 +1,5 @@
#include "ipv6.h"
#include "udp.h"
namespace ipv6 {
/******** costants ********/
@ -97,7 +98,7 @@ void IPv6Packet::doAction() {
handleICMP(packet + 8, packet + 24, packet + 1, packet + 40, len);
break;
case NH_UDP:
// handleUDP(packet + 40, len);
udp::handleUDP(packet + 8, packet + 24, packet + 1, packet + 40, len);
break;
default:
/* no other protocols are known here, sorry :-) */

26
udp.h Normal file
View File

@ -0,0 +1,26 @@
#ifndef COPPINO_UDP_H
#define COPPINO_UDP_H
namespace udp {
const uint8_t LEN = 128;
class UDPPacket {
private:
char packet[LEN];
public:
UDPPacket();
UDPPacket(const char* packet, int len);
void setSrcPort(uint16_t port);
uint16_t getSrcPort();
void setDstPort(uint16_t port);
uint16_t getDstPort();
void setPayload(const char* payload, int len);
void computeChecksum(const char* src_addr, const char* dst_addr, int len);
};
void handleUDP(const char* src_addr, const char* dst_addr, const char* flow, char* packet, int len);
}
#endif

67
udp.ino Normal file
View File

@ -0,0 +1,67 @@
#include "udp.h"
namespace udp {
UDPPacket::UDPPacket() {
memset(packet, 0, LEN);
}
UDPPacket::UDPPacket(const char* buffer, int len) {
memcpy(packet, buffer, len);
}
void UDPPacket::setSrcPort(uint16_t port) {
*(packet + 0) = port >> 8;
*(packet + 1) = port & 0xff;
}
void UDPPacket::setDstPort(uint16_t port) {
*(packet + 2) = port >> 8;
*(packet + 3) = port & 0xff;
}
uint16_t UDPPacket::getSrcPort() {
return (*(packet + 0) << 8) + (uint8_t)*(packet + 1);
}
uint16_t UDPPacket::getDstPort() {
return (*(packet + 2) << 8) + (uint8_t)*(packet + 3);
}
void UDPPacket::setPayload(const char* payload, int len) {
memcpy(packet + 8, payload, len);
*(packet + 4) = (len + 8) >> 8;
*(packet + 5) = (len + 8) & 0xff;
}
void UDPPacket::computeChecksum(const char* src_addr, const char* dst_addr, int len) {
memset(packet + 6, 0, 2);
uint16_t checksum = ipv6::compute_checksum(src_addr, dst_addr, ipv6::NH_UDP, len, packet, len);
memcpy(packet + 6, &checksum, 2);
}
void handleUDP(const char* src_addr, const char* dst_addr, const char* flow, char* packet, int len) {
UDPPacket udp_packet(packet, len);
/* swap ports */
uint16_t tmp;
tmp = udp_packet.getSrcPort();
udp_packet.setSrcPort(udp_packet.getDstPort());
udp_packet.setDstPort(tmp);
udp_packet.computeChecksum(src_addr, dst_addr, len);
/* prepare IPv6 packet */
ipv6::IPv6Packet reply_packet;
reply_packet.setPayload((char*)&udp_packet, len);
reply_packet.setFlow(flow);
ipv6::IPv6Addr tmp_addr;
memcpy(&tmp_addr, dst_addr, 16);
reply_packet.setSrcAddress(tmp_addr);
memcpy(&tmp_addr, src_addr, 16);
reply_packet.setDstAddress(tmp_addr);
reply_packet.setNextHeader(ipv6::NH_UDP);
slip::send((void*)&reply_packet, 40 + len);
}
}