From ba2f76f7e63f8384c79fa93087825e9e50ee68cf Mon Sep 17 00:00:00 2001
From: giomba <giomba@glgprograms.it>
Date: Tue, 24 Dec 2019 16:54:16 +0100
Subject: [PATCH] simple udp server handler

---
 coppino-main.ino |  8 ++++++++
 udp.h            |  5 +++++
 udp.ino          | 49 +++++++++++++++++++++++++++++-------------------
 3 files changed, 43 insertions(+), 19 deletions(-)

diff --git a/coppino-main.ino b/coppino-main.ino
index 94f2a81..fcfb44b 100644
--- a/coppino-main.ino
+++ b/coppino-main.ino
@@ -1,12 +1,20 @@
 #include "coppino.h"
 #include "ipv6.h"
 #include "slip.h"
+#include "udp.h"
+
+void udp_server(const char* buffer, int len) {
+  digitalWrite(13, !digitalRead(13));
+
+}
 
 void setup() {
   pinMode(13, OUTPUT);
   
   Serial.begin(19200);
   memcpy(&coppino::address, "\x20\x01\x04\x70\xc8\x44\x00\x31\x00\x00\x00\x00\x00\x00\xca\xfe", 16);
+
+  udp::set_handler(80, udp_server);
 }
 
 void loop() {
diff --git a/udp.h b/udp.h
index 5ecc41e..e7eb9ec 100644
--- a/udp.h
+++ b/udp.h
@@ -21,6 +21,11 @@ namespace udp {
   };
 
   void handleUDP(const ipv6::IPv6Addr* src_addr, const ipv6::IPv6Addr* dst_addr, const char* flow, char* packet, int len);
+
+  extern void(*server_handler) (const char* buffer, int size);
+  extern uint16_t server_port;
+
+  void set_handler(uint16_t port, void(* handler) (const char* buffer, int size));
 } 
 
 #endif
diff --git a/udp.ino b/udp.ino
index e20db65..f473b85 100644
--- a/udp.ino
+++ b/udp.ino
@@ -2,6 +2,9 @@
 
 namespace udp {
 
+void(*server_handler) (const char* buffer, int size) = NULL;
+uint16_t server_port = 0;
+
 UDPPacket::UDPPacket() {
   memset(packet, 0, LEN);
 }
@@ -39,29 +42,37 @@ void UDPPacket::computeChecksum(const ipv6::IPv6Addr* src_addr, const ipv6::IPv6
   memcpy(packet + 6, &checksum, 2);
 }
 
+void set_handler(uint16_t port, void(* server_handler) (const char* buffer, int size)) {
+  udp::server_port = port;
+  udp::server_handler = server_handler;
+}
+
 void handleUDP(const ipv6::IPv6Addr* src_addr, const ipv6::IPv6Addr* 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);
+  if (udp_packet.getDstPort() == server_port) { /* call user-defined server handler */
+    if (server_port != 0 && server_handler != NULL) {
+      server_handler(packet + 8, len - 8);
+    }
+  } else {  /* echo default UDP application handler */
+    /* swap ports */
+    uint16_t tmp;
+    tmp = udp_packet.getSrcPort();
+    udp_packet.setSrcPort(udp_packet.getDstPort());
+    udp_packet.setDstPort(tmp);
   
-  //ipv6::IPv6Addr tmp_addr;
-  //memcpy(&tmp_addr, dst_addr, 16);
-  reply_packet.setSrcAddress(dst_addr);
-  //memcpy(&tmp_addr, src_addr, 16);
-  reply_packet.setDstAddress(src_addr);
-  reply_packet.setNextHeader(ipv6::NH_UDP);
-  slip::send((void*)&reply_packet, 40 + len);
+    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);
+    
+    reply_packet.setSrcAddress(dst_addr);
+    reply_packet.setDstAddress(src_addr);
+    reply_packet.setNextHeader(ipv6::NH_UDP);
+    slip::send((void*)&reply_packet, 40 + len);
+  }
 }
 
 }