1 files changed, 77 insertions, 40 deletions

diff --git a/fw/fe310/eos/net/sock.c b/fw/fe310/eos/net/sock.c
index c9934e8..c55b8e8 100644
--- a/fw/fe310/eos/net/sock.c
+++ b/fw/fe310/eos/net/sock.c
@@ -10,26 +10,40 @@
 
 static eos_evt_handler_t evt_handler[EOS_SOCK_MAX_SOCK];
 
-static void sock_handle_msg(unsigned char type, unsigned char *buffer, uint16_t len) {
+static void sock_handle_msg(unsigned char type, unsigned char *buffer, uint16_t buf_len) {
     unsigned char sock;
+    int i;
 
-    if ((buffer == NULL) || (len < 2)) {
-        eos_net_bad_handler(type, buffer, len);
-        return;
-    }
-
-    sock = buffer[1];
-    if ((sock == 0) || (sock > EOS_SOCK_MAX_SOCK) || (evt_handler[sock - 1] == NULL)) {
-        eos_net_bad_handler(type, buffer, len);
+    if ((buffer == NULL) || (buf_len < 2)) {
+        eos_net_bad_handler(type, buffer, buf_len);
         return;
     }
 
     switch(buffer[0]) {
-        case EOS_SOCK_MTYPE_PKT:
-            evt_handler[sock - 1](type, buffer, len);
+        case EOS_SOCK_MTYPE_PKT: {
+            sock = buffer[1];
+            if ((sock == 0) || (sock > EOS_SOCK_MAX_SOCK) || (evt_handler[sock - 1] == NULL)) {
+                eos_net_bad_handler(type, buffer, buf_len);
+                return;
+            }
+            evt_handler[sock - 1](type, buffer, buf_len);
+            break;
+        }
+
+        case EOS_SOCK_MTYPE_CLOSE: {
+            for (i=1; i<buf_len; i++) {
+                sock = buffer[i];
+                if ((sock == 0) || (sock > EOS_SOCK_MAX_SOCK) || (evt_handler[sock - 1] == NULL)) {
+                    eos_net_bad_handler(type, buffer, buf_len);
+                    return;
+                }
+                evt_handler[sock - 1](type, buffer, buf_len);
+            }
             break;
+        }
+
         default:
-            eos_net_bad_handler(type, buffer, len);
+            eos_net_bad_handler(type, buffer, buf_len);
             break;
     }
 }
@@ -54,7 +68,7 @@ eos_evt_handler_t eos_sock_get_handler(unsigned char sock) {
 
 int eos_sock_open_udp(eos_evt_handler_t handler, unsigned char *buffer) {
     unsigned char type;
-    uint16_t len;
+    uint16_t buf_len;
     int do_release;
     int rv, sock;
 
@@ -65,23 +79,23 @@ int eos_sock_open_udp(eos_evt_handler_t handler, unsigned char *buffer) {
     }
 
     type = EOS_NET_MTYPE_SOCK;
-    len = 1;
+    buf_len = 1;
     buffer[0] = EOS_SOCK_MTYPE_OPEN_DGRAM;
 
-    rv = eos_net_xchg(&type, buffer, &len);
+    rv = eos_net_xchg(&type, buffer, &buf_len);
     if (rv) goto sock_open_udp_fin;
 
     if (type != EOS_NET_MTYPE_SOCK) {
         rv = EOS_ERR_NET;
         goto sock_open_udp_fin;
     }
-    if (len < 2) {
+    if (buf_len < 2) {
         rv = EOS_ERR_SIZE;
         goto sock_open_udp_fin;
     }
 
     sock = buffer[1];
-    if (sock == 0) {
+    if ((sock == 0) || (sock > EOS_SOCK_MAX_SOCK)) {
         rv = EOS_ERR_NET;
         goto sock_open_udp_fin;
     }
@@ -108,7 +122,29 @@ void eos_sock_close(unsigned char sock, unsigned char *buffer) {
     eos_sock_set_handler(sock, NULL);
 }
 
-static int sock_send(unsigned char sock, unsigned char *msg, uint16_t msg_len, EOSNetAddr *addr, unsigned char *buffer) {
+int eos_sock_pkt_alloc(unsigned char **buffer, unsigned char *pkt, size_t pkt_len) {
+    *buffer = NULL;
+
+    if (pkt && (pkt_len + EOS_SOCK_SIZE_UDP_HDR > EOS_NET_SIZE_BUF)) return EOS_ERR_SIZE;
+
+    *buffer = eos_net_alloc();
+    *buffer += EOS_SOCK_SIZE_UDP_HDR;
+    if (pkt) memcpy(*buffer, pkt, pkt_len);
+
+    return EOS_OK;
+}
+
+unsigned char *eos_sock_buf2pkt(unsigned char *buf, uint16_t buf_len) {
+    if (buf_len < EOS_SOCK_SIZE_UDP_HDR) return NULL;
+
+    return buf + EOS_SOCK_SIZE_UDP_HDR;
+}
+
+unsigned char *eos_sock_pkt2buf(unsigned char *pkt) {
+    return pkt - EOS_SOCK_SIZE_UDP_HDR;
+}
+
+static void sock_sendto(unsigned char sock, EOSNetAddr *addr, unsigned char *buffer) {
     buffer[0] = EOS_SOCK_MTYPE_PKT;
     buffer[1] = sock;
     buffer += 2;
@@ -117,46 +153,47 @@ static int sock_send(unsigned char sock, unsigned char *msg, uint16_t msg_len, E
     buffer[0] = addr->port >> 8;
     buffer[1] = addr->port;
     buffer += sizeof(addr->port);
-    if (msg) {
-        if (msg_len + EOS_SOCK_SIZE_UDP_HDR > EOS_NET_SIZE_BUF) return EOS_ERR_SIZE;
-        memcpy(buffer, msg, msg_len);
-    }
-
-    return EOS_OK;
 }
 
-int eos_sock_sendto(unsigned char sock, unsigned char *msg, size_t msg_len, EOSNetAddr *addr, unsigned char *buffer) {
+int eos_sock_sendto(unsigned char sock, EOSNetAddr *addr, unsigned char *buffer, uint16_t buf_len, unsigned char more) {
     int rv;
 
-    rv = sock_send(sock, msg, msg_len, addr, buffer);
-    if (rv) return rv;
+    buffer -= EOS_SOCK_SIZE_UDP_HDR;
+    sock_sendto(sock, addr, buffer);
 
-    return eos_net_send(EOS_NET_MTYPE_SOCK, buffer, msg_len + EOS_SOCK_SIZE_UDP_HDR);
+    rv = eos_net_send(EOS_NET_MTYPE_SOCK, buffer, buf_len + EOS_SOCK_SIZE_UDP_HDR, more);
+    return rv;
 }
 
-int eos_sock_sendto_async(unsigned char sock, unsigned char *msg, size_t msg_len, EOSNetAddr *addr, unsigned char *buffer, unsigned char more) {
+int eos_sock_sendto_sync(unsigned char sock, EOSNetAddr *addr, unsigned char *buffer, uint16_t buf_len) {
     int rv;
 
-    rv = sock_send(sock, msg, msg_len, addr, buffer);
-    if (rv) return rv;
+    buffer -= EOS_SOCK_SIZE_UDP_HDR;
+    sock_sendto(sock, addr, buffer);
 
-    return eos_net_send_async(EOS_NET_MTYPE_SOCK, buffer, msg_len + EOS_SOCK_SIZE_UDP_HDR, more);
+    rv = eos_net_send_sync(EOS_NET_MTYPE_SOCK, buffer, buf_len + EOS_SOCK_SIZE_UDP_HDR);
+    return rv;
 }
 
-int eos_sock_recvfrom(unsigned char *buffer, uint16_t len, unsigned char *msg, size_t msg_size, EOSNetAddr *addr) {
-    if (len < EOS_SOCK_SIZE_UDP_HDR) return EOS_ERR_SIZE;
+int eos_sock_recvfrom(unsigned char *buffer, uint16_t buf_len, EOSNetAddr *addr, unsigned char *pkt, size_t pkt_size) {
+    if (buf_len < EOS_SOCK_SIZE_UDP_HDR) return EOS_ERR_SIZE;
 
+    if (buffer[0] == EOS_SOCK_MTYPE_CLOSE) return EOS_SOCK_ERR_CLOSED;
     if (buffer[0] != EOS_SOCK_MTYPE_PKT) return EOS_ERR_NET;
 
     buffer += 2;
-    memcpy(addr->host, buffer, sizeof(addr->host));
+    if (addr) {
+        memcpy(addr->host, buffer, sizeof(addr->host));
+    }
     buffer += sizeof(addr->host);
-    addr->port  = (uint16_t)buffer[0] << 8;
-    addr->port |= (uint16_t)buffer[1];
+    if (addr) {
+        addr->port  = (uint16_t)buffer[0] << 8;
+        addr->port |= (uint16_t)buffer[1];
+    }
     buffer += sizeof(addr->port);
-    if (msg) {
-        if (msg_size < len - EOS_SOCK_SIZE_UDP_HDR) return EOS_ERR_SIZE;
-        memcpy(msg, buffer, len - EOS_SOCK_SIZE_UDP_HDR);
+    if (pkt) {
+        if (pkt_size < buf_len - EOS_SOCK_SIZE_UDP_HDR) return EOS_ERR_SIZE;
+        memcpy(pkt, buffer, buf_len - EOS_SOCK_SIZE_UDP_HDR);
     }
 
     return EOS_OK;