From 3811729f82ef05f3ae43341022e7b65a92d333a2 Mon Sep 17 00:00:00 2001
From: Alasdair Allan <alasdair@babilim.co.uk>
Date: Sat, 3 Aug 2013 21:01:15 +0100
Subject: [PATCH] Added Multicast UDP support

---
 libraries/Ethernet/EthernetUdp.cpp | 38 ++++++++++++++++++++++++++++++
 libraries/Ethernet/EthernetUdp.h   |  1 +
 2 files changed, 39 insertions(+)

diff --git a/libraries/Ethernet/EthernetUdp.cpp b/libraries/Ethernet/EthernetUdp.cpp
index 37600529f8b..561aa5cbc92 100644
--- a/libraries/Ethernet/EthernetUdp.cpp
+++ b/libraries/Ethernet/EthernetUdp.cpp
@@ -216,3 +216,41 @@ void EthernetUDP::flush()
   }
 }
 
+
+/* Start EthernetUDP socket, listening at local port PORT */
+uint8_t EthernetUDP::beginMulti(IPAddress ip, uint16_t port) {
+    Serial.println("beginMulti()");
+    if (_sock != MAX_SOCK_NUM)
+        return 0;
+    
+    for (int i = 0; i < MAX_SOCK_NUM; i++) {
+        uint8_t s = W5100.readSnSR(i);
+        if (s == SnSR::CLOSED || s == SnSR::FIN_WAIT) {
+            _sock = i;
+            break;
+        }
+    }
+    
+    if (_sock == MAX_SOCK_NUM)
+        return 0;
+    
+    
+    // Calculate MAC address from Multicast IP Address
+    byte mac[] = {  0x01, 0x00, 0x5E, 0x00, 0x00, 0x00 };
+    
+    mac[3] = ip[1] & 0x7F;
+    mac[4] = ip[2];
+    mac[5] = ip[3];
+    
+    W5100.writeSnDIPR(_sock, rawIPAddress(ip));   //239.255.0.1
+    W5100.writeSnDPORT(_sock, port);
+    W5100.writeSnDHAR(_sock,mac);
+    
+    _remaining = 0;
+    
+    socket(_sock, SnMR::UDP, port, SnMR::MULTI);
+    
+    return 1;
+}
+
+
diff --git a/libraries/Ethernet/EthernetUdp.h b/libraries/Ethernet/EthernetUdp.h
index 8a6b7ab5a8b..c772cb93452 100644
--- a/libraries/Ethernet/EthernetUdp.h
+++ b/libraries/Ethernet/EthernetUdp.h
@@ -53,6 +53,7 @@ class EthernetUDP : public UDP {
 public:
   EthernetUDP();  // Constructor
   virtual uint8_t begin(uint16_t);	// initialize, start listening on specified port. Returns 1 if successful, 0 if there are no sockets available to use
+  virtual uint8_t beginMulti(IPAddress, uint16_t);	// initialize, start listening on specified port. Returns 1 if successful, 0 if there are no sockets available to use
   virtual void stop();  // Finish with the UDP socket
 
   // Sending UDP packets