1 files changed, 51 insertions, 34 deletions

diff --git a/fw/esp32/components/eos/sock.c b/fw/esp32/components/eos/sock.c
index 355d7c7..ec198f0 100644
--- a/fw/esp32/components/eos/sock.c
+++ b/fw/esp32/components/eos/sock.c
@@ -35,7 +35,7 @@ static int t_open_dgram(void) {
     memset((char *)&_myaddr, 0, sizeof(_myaddr));
     _myaddr.sin_family = AF_INET;
     _myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
-    _myaddr.sin_port = htons(3000);
+    _myaddr.sin_port = htons(0);
 
     int rv = bind(sock, (struct sockaddr *)&_myaddr, sizeof(_myaddr));
     if (rv < 0) {
@@ -54,7 +54,7 @@ static ssize_t t_sendto(int sock, void *msg, size_t msg_size, EOSNetAddr *addr)
 
     memset((void *)&servaddr, 0, sizeof(servaddr));
     servaddr.sin_family = AF_INET;
-    servaddr.sin_port = addr->port;
+    servaddr.sin_port = htons(addr->port);
     memcpy((void *)&servaddr.sin_addr, addr->host, sizeof(addr->host));
     return sendto(sock, msg, msg_size, 0, (struct sockaddr *)&servaddr, sizeof(servaddr));
 }
@@ -68,85 +68,102 @@ static ssize_t t_recvfrom(int sock, void *msg, size_t msg_size, EOSNetAddr *addr
     if (recvlen < 0) return recvlen;
 
     if (addr) {
-        addr->port = servaddr.sin_port;
+        addr->port = ntohs(servaddr.sin_port);
         memcpy(addr->host, (void *)&servaddr.sin_addr, sizeof(addr->host));
     }
     return recvlen;
 }
 
 static void udp_rcvr_task(void *pvParameters) {
-    EOSNetAddr addr;
-    uint8_t esock = (uint8_t)pvParameters;
-    int sock = _socks[esock-1];
-    unsigned char *buf;
+    uint8_t sock_i = (uint8_t)pvParameters;
+    int sock = _socks[sock_i-1];
 
     do {
+        EOSNetAddr addr;
+        unsigned char *buf, *_buf;
         ssize_t rv;
 
         buf = eos_net_alloc();
         rv = t_recvfrom(sock, buf + EOS_SOCK_SIZE_UDP_HDR, EOS_NET_MTU - EOS_SOCK_SIZE_UDP_HDR, &addr);
         if (rv < 0) {
-            sock = 0;
             eos_net_free(buf);
             ESP_LOGE(TAG, "UDP RECV ERR:%d", rv);
-            continue;
+            break;
         }
-        buf[0] = EOS_SOCK_MTYPE_PKT;
-        buf[1] = esock;
-        memcpy(buf+2, addr.host, sizeof(addr.host));
-        memcpy(buf+2+sizeof(addr.host), &addr.port, sizeof(addr.port));
+        _buf = buf;
+        _buf[0] = EOS_SOCK_MTYPE_PKT;
+        _buf[1] = sock_i;
+        _buf += 2;
+        memcpy(_buf, addr.host, sizeof(addr.host));
+        _buf += sizeof(addr.host);
+        _buf[0] = addr.port >> 8;
+        _buf[1] = addr.port;
+        _buf += sizeof(addr.port);
         eos_net_send(EOS_NET_MTYPE_SOCK, buf, rv + EOS_SOCK_SIZE_UDP_HDR);
-    } while(sock);
+    } while (1);
+
     xSemaphoreTake(mutex, portMAX_DELAY);
-    _socks[esock-1] = 0;
+    _socks[sock_i-1] = 0;
     xSemaphoreGive(mutex);
     vTaskDelete(NULL);
 }
 
-static void sock_handler(unsigned char type, unsigned char *buffer, uint16_t size) {
+static void sock_handler(unsigned char type, unsigned char *buffer, uint16_t buf_len) {
     EOSNetAddr addr;
-    uint8_t esock;
+    uint8_t sock_i;
     int sock, i;
-    unsigned char *rbuf;
 
-    if (size < 1) return;
+    if (buf_len < 1) return;
 
     switch(buffer[0]) {
         case EOS_SOCK_MTYPE_PKT:
-            if (size < EOS_SOCK_SIZE_UDP_HDR) return;
-            sock = _socks[buffer[1] - 1];
-            memcpy(addr.host, buffer + 2, sizeof(addr.host));
-            memcpy(&addr.port, buffer + 2 + sizeof(addr.host), sizeof(addr.port));
-            t_sendto(sock, buffer + EOS_SOCK_SIZE_UDP_HDR, size - EOS_SOCK_SIZE_UDP_HDR, &addr);
+            if (buf_len < EOS_SOCK_SIZE_UDP_HDR) return;
+
+            sock_i = buffer[1]-1;
+            if (sock_i >= EOS_SOCK_MAX_SOCK) return;
+
+            sock = _socks[sock_i];
+            buffer += 2;
+            memcpy(addr.host, buffer, sizeof(addr.host));
+            buffer += sizeof(addr.host);
+            addr.port  = (uint16_t)buffer[0] << 8;
+            addr.port |= (uint16_t)buffer[1];
+            buffer += sizeof(addr.port);
+            t_sendto(sock, buffer, buf_len - EOS_SOCK_SIZE_UDP_HDR, &addr);
             break;
 
         case EOS_SOCK_MTYPE_OPEN_DGRAM:
             sock = t_open_dgram();
-            esock = 0;
+            sock_i = 0;
             if (sock > 0) {
                 xSemaphoreTake(mutex, portMAX_DELAY);
                 for (i=0; i<EOS_SOCK_MAX_SOCK; i++) {
                     if (_socks[i] == 0) {
-                        esock = i+1;
+                        sock_i = i+1;
                         _socks[i] = sock;
+                        break;
                     }
                 }
                 xSemaphoreGive(mutex);
             }
-            xTaskCreate(&udp_rcvr_task, "udp_rcvr", EOS_TASK_SSIZE_UDP_RCVR, (void *)esock, EOS_TASK_PRIORITY_UDP_RCVR, NULL);
-            rbuf = eos_net_alloc();
-            rbuf[0] = EOS_SOCK_MTYPE_OPEN_DGRAM;
-            rbuf[1] = esock;
-            eos_net_send(EOS_NET_MTYPE_SOCK, rbuf, 2);
+            if (sock_i) xTaskCreate(&udp_rcvr_task, "udp_rcvr", EOS_TASK_SSIZE_UDP_RCVR, (void *)sock_i, EOS_TASK_PRIORITY_UDP_RCVR, NULL);
+            buffer[0] = EOS_SOCK_MTYPE_OPEN_DGRAM;
+            buffer[1] = sock_i;
+            eos_net_reply(EOS_NET_MTYPE_SOCK, buffer, 2);
             break;
 
         case EOS_SOCK_MTYPE_CLOSE:
-            if (size < 2) return;
-            sock = _socks[buffer[1]-1];
+            if (buf_len < 2) return;
+
+            sock_i = buffer[1]-1;
+            if (sock_i >= EOS_SOCK_MAX_SOCK) return;
+
+            sock = _socks[sock_i];
             t_close(sock);
             break;
 
         default:
+            ESP_LOGE(TAG, "BAD TYPE:%d", buffer[0]);
             break;
     }
 }
@@ -156,4 +173,4 @@ void eos_sock_init(void) {
     xSemaphoreGive(mutex);
     eos_net_set_handler(EOS_NET_MTYPE_SOCK, sock_handler);
     ESP_LOGI(TAG, "INIT");
-}
\ No newline at end of file
+}