basic udp echo server

ipv6.ino

@@ -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 :-) */

udp.h

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

udp.ino

@@ -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);
+}
+
+}