Debug log with dlog_if_slow, MACRAW ping working, wfi disabled

firmware/lib/dispatch.cpp

@@ -31,11 +31,10 @@ void dispatch_schedule_ms(uint32_t ms, std::function<void()> fn) {
     static static_vector<uint8_t, 256> usb_rx_buf;
 
     while (true) {
-        tud_task();
-
-        usb.drain();
-        timers.run();
-        //net_poll();
+        dlog_if_slow("tud_task", 1000, [&]{ tud_task(); });
+        dlog_if_slow("drain", 1000, [&]{ usb.drain(); });
+        dlog_if_slow("timers", 1000, [&]{ timers.run(); });
+        dlog_if_slow("net_poll", 1000, [&]{ net_poll(); });
 
         while (tud_cdc_available()) {
             uint8_t byte;
@@ -54,7 +53,13 @@ void dispatch_schedule_ms(uint32_t ms, std::function<void()> fn) {
             auto it = handler_map.find(msg->type_id);
             if (it != handler_map.end()) {
                 for (auto& response : it->second(msg->message_id, msg->payload)) {
-                    usb.send(response);
+                    if (response.size() > usb.tx.free()) {
+                        auto err = encode_response(msg->message_id,
+                            DeviceError{2, "response too large: " + std::to_string(response.size())});
+                        usb.send(err);
+                    } else {
+                        usb.send(response);
+                    }
                 }
                 if (msg->type_id == RequestPICOBOOT::ext_id) {
                     sleep_ms(100);
@@ -63,6 +68,6 @@ void dispatch_schedule_ms(uint32_t ms, std::function<void()> fn) {
             }
         }
 
-        __wfi();
+        // __wfi();
     }
 }

firmware/lib/net.cpp

@@ -1,7 +1,9 @@
 #include "net.h"
 #include <cstring>
 #include "pico/unique_id.h"
+#include "pico/time.h"
 #include "w6300.h"
+#include "debug_log.h"
 
 static net_state state;
 static w6300::socket_id raw_socket{0};
@@ -43,8 +45,10 @@ static bool ip_match(const uint8_t* dst) {
 }
 
 static void send_raw(const uint8_t* data, size_t len) {
-    w6300::ip_address dummy = {};
-    w6300::sendto(raw_socket, std::span<const uint8_t>{data, len}, dummy, w6300::port_num{0});
+    dlog_if_slow("send_raw", 1000, [&]{
+        w6300::ip_address dummy = {};
+        w6300::sendto(raw_socket, std::span<const uint8_t>{data, len}, dummy, w6300::port_num{0});
+    });
 }
 
 static void handle_arp(const uint8_t* frame, size_t len) {
@@ -54,6 +58,7 @@ static void handle_arp(const uint8_t* frame, size_t len) {
     if (read_u16(arp) != 1) return;
     if (read_u16(arp + 2) != ETHERTYPE_IPV4) return;
     if (arp[4] != 6 || arp[5] != 4) return;
+
     if (read_u16(arp + 6) != ARP_OP_REQUEST) return;
     if (!ip_match(arp + 24)) return;
 
@@ -88,6 +93,7 @@ static void handle_icmp(const uint8_t* frame, size_t len) {
     const uint8_t* icmp = ip + ip_hdr_len;
     size_t icmp_len = ip_total_len - ip_hdr_len;
     if (icmp_len < 8) return;
+
     if (icmp[0] != ICMP_ECHO_REQUEST) return;
 
     uint8_t reply[1514];
@@ -127,9 +133,11 @@ static void handle_ipv4(const uint8_t* frame, size_t len) {
 
 static void process_frame(const uint8_t* frame, size_t len) {
     if (len < 14) return;
+
     if (!mac_match(frame)) return;
 
     uint16_t ethertype = read_u16(frame + 12);
+
     switch (ethertype) {
     case ETHERTYPE_ARP:
         handle_arp(frame, len);