From 08566036791172f7a559d02f78eb06aa5a8b77a7 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Wed, 13 Sep 2023 14:21:43 +0000
Subject: [PATCH] [wiz5500] Added WizNet W5500 driver
---
CMakeLists.txt | 3 +
cmake/boardcore.cmake | 1 +
src/shared/drivers/WIZ5500/WIZ5500.cpp | 704 +++++++++++++++++++++++
src/shared/drivers/WIZ5500/WIZ5500.h | 324 +++++++++++
src/shared/drivers/WIZ5500/WIZ5500Defs.h | 175 ++++++
src/tests/drivers/test-wiz5500.cpp | 327 +++++++++++
6 files changed, 1534 insertions(+)
create mode 100644 src/shared/drivers/WIZ5500/WIZ5500.cpp
create mode 100644 src/shared/drivers/WIZ5500/WIZ5500.h
create mode 100644 src/shared/drivers/WIZ5500/WIZ5500Defs.h
create mode 100644 src/tests/drivers/test-wiz5500.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index bfc731dc5..aaba63944 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -281,6 +281,9 @@ sbs_target(test-i2c-driver-f7 stm32f767zi_nucleo)
add_executable(test-i2c-f7 src/tests/drivers/i2c/test-i2c.cpp)
sbs_target(test-i2c-f7 stm32f767zi_nucleo)
+add_executable(test-wiz5500 src/tests/drivers/test-wiz5500.cpp)
+sbs_target(test-wiz5500 stm32f767zi_gemini_gs)
+
#-----------------------------------------------------------------------------#
# Tests - Events #
#-----------------------------------------------------------------------------#
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index bea35c59b..49c749382 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -63,6 +63,7 @@ foreach(OPT_BOARD ${BOARDS})
${SBS_BASE}/src/shared/drivers/i2c/I2CDriver-f4.cpp
${SBS_BASE}/src/shared/drivers/i2c/I2CDriver-f7.cpp
${SBS_BASE}/src/shared/drivers/i2c/I2C.cpp
+ ${SBS_BASE}/src/shared/drivers/WIZ5500/WIZ5500.cpp
# Events
${SBS_BASE}/src/shared/events/EventBroker.cpp
diff --git a/src/shared/drivers/WIZ5500/WIZ5500.cpp b/src/shared/drivers/WIZ5500/WIZ5500.cpp
new file mode 100644
index 000000000..3cac50057
--- /dev/null
+++ b/src/shared/drivers/WIZ5500/WIZ5500.cpp
@@ -0,0 +1,704 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "WIZ5500.h"
+
+#include <drivers/interrupt/external_interrupts.h>
+#include <interfaces/endianness.h>
+#include <kernel/scheduler/scheduler.h>
+
+#include "WIZ5500Defs.h"
+
+using namespace Boardcore;
+using namespace miosix;
+
+// Keep a 500ms timeout even for the general INTn
+constexpr long long INTN_TIMEOUT = 500;
+
+SPIBusConfig getSpiBusConfig(SPI::ClockDivider clock_divider)
+{
+ SPIBusConfig bus_config = {};
+ bus_config.clockDivider = clock_divider;
+ bus_config.mode = SPI::Mode::MODE_0;
+ bus_config.bitOrder = SPI::Order::MSB_FIRST;
+ bus_config.byteOrder = SPI::Order::MSB_FIRST;
+
+ return bus_config;
+}
+
+Wiz5500::Wiz5500(SPIBus &bus, miosix::GpioPin cs, miosix::GpioPin intn,
+ SPI::ClockDivider clock_divider)
+ : intn(intn), slave(bus, cs, getSpiBusConfig(clock_divider))
+{
+ enableExternalInterrupt(intn, InterruptTrigger::FALLING_EDGE);
+
+ // Reset thread wait infos
+ for (int i = 0; i < NUM_THREAD_WAIT_INFOS; i++)
+ {
+ wait_infos[i].sock_n = -1;
+ wait_infos[i].irq_mask = 0;
+ wait_infos[i].irq = 0;
+ wait_infos[i].thread = nullptr;
+ }
+
+ // Reset socket infos
+ for (int i = 0; i < NUM_SOCKETS; i++)
+ {
+ socket_infos[i].mode = Wiz5500::SocketMode::CLOSED;
+ socket_infos[i].irq_mask = 0;
+ }
+}
+
+Wiz5500::~Wiz5500() { disableExternalInterrupt(intn); }
+
+void Wiz5500::setOnIpConflict(OnIpConflictCb cb)
+{
+ Lock<FastMutex> l(mutex);
+ on_ip_conflict = cb;
+}
+
+void Wiz5500::setOnDestUnreachable(OnDestUnreachableCb cb)
+{
+ Lock<FastMutex> l(mutex);
+ on_dest_unreachable = cb;
+}
+
+Wiz5500::PhyState Wiz5500::getPhyState()
+{
+ Lock<FastMutex> l(mutex);
+ uint8_t phycfg = spiRead8(0, Wiz::Common::REG_PHYCFGR);
+
+ bool full_duplex = (phycfg & (1 << 2)) != 0;
+ bool based_100mbps = (phycfg & (1 << 1)) != 0;
+ bool link_up = (phycfg & (1 << 0)) != 0;
+
+ return {full_duplex, based_100mbps, link_up};
+}
+
+bool Wiz5500::reset()
+{
+ Lock<FastMutex> l(mutex);
+
+ // First check that the device is actually present
+ if (spiRead8(0, Wiz::Common::REG_VERSIONR) != Wiz::VERSION)
+ {
+ return false;
+ }
+
+ // Perform a software reset
+ spiWrite8(0, Wiz::Common::REG_MR, 1 << 7);
+ // Enable all socket interrupts
+ spiWrite8(0, Wiz::Common::REG_SIMR, 0b11111111);
+ spiWrite8(0, Wiz::Common::REG_IMR, 0b11110000);
+
+ // Reset all socketsOSI
+ for (int i = 0; i < NUM_SOCKETS; i++)
+ {
+ spiWrite8(Wiz::getSocketRegBlock(i), Wiz::Socket::REG_MR, 0);
+ }
+
+ return true;
+}
+
+void Wiz5500::handleINTn()
+{
+ if (interrupt_service_thread)
+ {
+ interrupt_service_thread->IRQwakeup();
+ if (interrupt_service_thread->IRQgetPriority() >
+ miosix::Thread::IRQgetCurrentThread()->IRQgetPriority())
+ {
+ miosix::Scheduler::IRQfindNextThread();
+ }
+ }
+}
+
+void Wiz5500::setGatewayIp(WizIp ip)
+{
+ Lock<FastMutex> l(mutex);
+ spiWriteIp(0, Wiz::Common::REG_GAR, ip);
+}
+
+void Wiz5500::setSubnetMask(WizIp mask)
+{
+ Lock<FastMutex> l(mutex);
+ spiWriteIp(0, Wiz::Common::REG_SUBR, mask);
+}
+
+void Wiz5500::setSourceMac(WizMac mac)
+{
+ Lock<FastMutex> l(mutex);
+ spiWriteMac(0, Wiz::Common::REG_SHAR, mac);
+}
+
+void Wiz5500::setSourceIp(WizIp ip)
+{
+ Lock<FastMutex> l(mutex);
+ spiWriteIp(0, Wiz::Common::REG_SIPR, ip);
+}
+
+bool Wiz5500::connectTcp(int sock_n, uint16_t src_port, WizIp dst_ip,
+ uint16_t dst_port, int timeout)
+{
+ Lock<FastMutex> l(mutex);
+
+ // Check that we are closed
+ if (socket_infos[sock_n].mode != Wiz5500::SocketMode::CLOSED)
+ {
+ return false;
+ }
+
+ // Setup the socket
+ spiWrite8(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_MR,
+ Wiz::Socket::buildModeTcp(false));
+ spiWrite16(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_PORT, src_port);
+ spiWriteIp(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_DIPR, dst_ip);
+ spiWrite16(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_DPORT,
+ dst_port);
+
+ // Open the socket
+ spiWrite8(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_CR,
+ Wiz::Socket::CMD_OPEN);
+
+ // Ok now check that we actually went into that mode
+ int status = spiRead8(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_SR);
+ if (status != Wiz::Socket::STAT_INIT)
+ {
+ return false;
+ }
+
+ // Connect the socket
+ spiWrite8(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_CR,
+ Wiz::Socket::CMD_CONNECT);
+
+ // Ok now wait for either a connection, or a disconnection
+ int irq = waitForSocketIrq(
+ l, sock_n, Wiz::Socket::Irq::CON | Wiz::Socket::Irq::DISCON, timeout);
+
+ // Connection failed
+ if ((irq & Wiz::Socket::Irq::CON) == 0)
+ {
+ return false;
+ }
+
+ socket_infos[sock_n].mode = Wiz5500::SocketMode::TCP;
+ return true;
+}
+
+bool Wiz5500::listenTcp(int sock_n, uint16_t src_port, WizIp &dst_ip,
+ uint16_t &dst_port, int timeout)
+{
+ Lock<FastMutex> l(mutex);
+
+ // Check that we are closed
+ if (socket_infos[sock_n].mode != Wiz5500::SocketMode::CLOSED)
+ {
+ return false;
+ }
+
+ // Setup the socket
+ spiWrite8(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_MR,
+ Wiz::Socket::buildModeTcp(false));
+ spiWrite16(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_PORT, src_port);
+
+ // Open the socket
+ spiWrite8(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_CR,
+ Wiz::Socket::CMD_OPEN);
+
+ // Ok now check that we actually went into that mode
+ int status = spiRead8(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_SR);
+ if (status != Wiz::Socket::STAT_INIT)
+ {
+ return false;
+ }
+
+ // Connect the socket
+ spiWrite8(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_CR,
+ Wiz::Socket::CMD_LISTEN);
+
+ // Ok now wait for either a connection, or a disconnection
+ int irq =
+ waitForSocketIrq(l, sock_n,
+ Wiz::Socket::Irq::CON | Wiz::Socket::Irq::DISCON |
+ Wiz::Socket::Irq::TIMEOUT,
+ timeout);
+
+ // Connection failed
+ if ((irq & Wiz::Socket::Irq::CON) == 0)
+ {
+ return false;
+ }
+
+ // Read remote side infos
+ dst_ip = spiReadIp(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_DIPR);
+ dst_port =
+ spiRead16(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_DPORT);
+
+ socket_infos[sock_n].mode = Wiz5500::SocketMode::TCP;
+ return true;
+}
+
+bool Wiz5500::openUdp(int sock_n, uint16_t src_port, WizIp dst_ip,
+ uint16_t dst_port, int timeout)
+{
+ Lock<FastMutex> l(mutex);
+
+ // Check that we are closed
+ if (socket_infos[sock_n].mode != Wiz5500::SocketMode::CLOSED)
+ {
+ return false;
+ }
+
+ // Setup the socket
+ spiWrite8(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_MR,
+ Wiz::Socket::buildModeUdp(false, false, false, false));
+ spiWrite16(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_PORT, src_port);
+ spiWriteIp(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_DIPR, dst_ip);
+ spiWrite16(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_DPORT,
+ dst_port);
+
+ // Open the socket
+ spiWrite8(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_CR,
+ Wiz::Socket::CMD_OPEN);
+
+ // Ok now check that we actually went into that mode
+ int status = spiRead8(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_SR);
+ if (status != Wiz::Socket::STAT_UDP)
+ {
+ return false;
+ }
+
+ socket_infos[sock_n].mode = Wiz5500::SocketMode::UDP;
+ return true;
+}
+
+bool Wiz5500::send(int sock_n, const uint8_t *data, size_t len, int timeout)
+{
+ Lock<FastMutex> l(mutex);
+
+ // We cannot send through a closed socket
+ if (socket_infos[sock_n].mode == Wiz5500::SocketMode::CLOSED)
+ {
+ return false;
+ }
+
+ // First get the start write address
+ uint16_t start_addr =
+ spiRead16(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_TX_WR);
+
+ // Fill the TX buffer and update the counter
+ spiWrite(Wiz::getSocketTxBlock(sock_n), start_addr, data, len);
+ spiWrite16(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_TX_WR,
+ start_addr + len);
+
+ // Finally tell the device to send the data
+ spiWrite8(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_CR,
+ Wiz::Socket::CMD_SEND);
+
+ // Wait for the device to signal a correct send
+ int irq = waitForSocketIrq(l, sock_n, Wiz::Socket::Irq::SEND_OK, timeout);
+
+ // It didn't signal it as an error
+ if ((irq & Wiz::Socket::Irq::SEND_OK) == 0)
+ {
+ return false;
+ }
+
+ return true;
+}
+
+ssize_t Wiz5500::recv(int sock_n, uint8_t *data, size_t len, int timeout)
+{
+ Lock<FastMutex> l(mutex);
+
+ // This is only valid for TCP
+ if (socket_infos[sock_n].mode != Wiz5500::SocketMode::TCP)
+ {
+ return -1;
+ }
+
+ // Wait for the device to signal that we received something, or a
+ // disconnection
+ int irq = waitForSocketIrq(
+ l, sock_n, Wiz::Socket::Irq::RECV | Wiz::Socket::Irq::DISCON, timeout);
+ if ((irq & Wiz::Socket::Irq::RECV) == 0)
+ {
+ return -1;
+ }
+
+ // Ok we received something, get the received length
+ uint16_t recv_len =
+ spiRead16(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_RX_RSR);
+ uint16_t addr =
+ spiRead16(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_RX_RD);
+
+ // Check if we actually have space
+ if (recv_len < len)
+ {
+ spiRead(Wiz::getSocketRxBlock(sock_n), addr, data, recv_len);
+ }
+
+ addr += recv_len;
+ spiWrite16(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_RX_RD, addr);
+
+ // Finally tell the device that we correctly received and read the data
+ spiWrite8(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_CR,
+ Wiz::Socket::CMD_RECV);
+
+ return recv_len < len ? recv_len : -1;
+}
+
+ssize_t Wiz5500::recvfrom(int sock_n, uint8_t *data, size_t len, WizIp &dst_ip,
+ uint16_t &dst_port, int timeout)
+{
+ Lock<FastMutex> l(mutex);
+
+ // This is only valid for UDP
+ if (socket_infos[sock_n].mode != Wiz5500::SocketMode::UDP)
+ {
+ return -1;
+ }
+
+ // Wait for the device to signal that we received something, or a
+ // disconnection
+ int irq = waitForSocketIrq(
+ l, sock_n, Wiz::Socket::Irq::RECV | Wiz::Socket::Irq::DISCON, timeout);
+ if ((irq & Wiz::Socket::Irq::RECV) == 0)
+ {
+ return -1;
+ }
+
+ // Ok we received something, get the received length
+ uint16_t recv_len =
+ spiRead16(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_RX_RSR);
+ uint16_t addr =
+ spiRead16(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_RX_RD);
+
+ // First read the internal header
+ spiRead(Wiz::getSocketRxBlock(sock_n), addr,
+ reinterpret_cast<uint8_t *>(&dst_ip), sizeof(WizIp));
+ addr += sizeof(WizIp);
+ spiRead(Wiz::getSocketRxBlock(sock_n), addr,
+ reinterpret_cast<uint8_t *>(&dst_port), sizeof(uint16_t));
+ addr += sizeof(uint16_t);
+
+ // Now, what's this?
+ uint16_t what = 0;
+ spiRead(Wiz::getSocketRxBlock(sock_n), addr,
+ reinterpret_cast<uint8_t *>(&what), sizeof(uint16_t));
+ addr += sizeof(uint16_t);
+
+ // Remove what we have already read.
+ recv_len -= sizeof(WizIp) + sizeof(uint16_t) + sizeof(uint16_t);
+
+ // Check if we actually have space
+ if (recv_len < len)
+ {
+ spiRead(Wiz::getSocketRxBlock(sock_n), addr, data, recv_len);
+ }
+
+ addr += recv_len;
+ spiWrite16(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_RX_RD, addr);
+
+ // Finally tell the device that we correctly received and read the data
+ spiWrite8(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_CR,
+ Wiz::Socket::CMD_RECV);
+
+ return recv_len < len ? recv_len : -1;
+}
+
+void Wiz5500::close(int sock_n, int timeout)
+{
+ Lock<FastMutex> l(mutex);
+
+ // We cannot receive close a closed socket
+ if (socket_infos[sock_n].mode == Wiz5500::SocketMode::CLOSED)
+ {
+ return;
+ }
+
+ spiWrite8(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_CR,
+ Wiz::Socket::CMD_DISCON);
+
+ waitForSocketIrq(l, sock_n, Wiz::Socket::Irq::DISCON, timeout);
+ socket_infos[sock_n].mode = Wiz5500::SocketMode::CLOSED;
+}
+
+void Wiz5500::waitForINTn(Lock<FastMutex> &l)
+{
+ long long start = getTick();
+ TimedWaitResult result = TimedWaitResult::NoTimeout;
+
+ Unlock<FastMutex> ul(l);
+ FastInterruptDisableLock il;
+ while (intn.value() != 0 && result == TimedWaitResult::NoTimeout)
+ {
+ result = Thread::IRQenableIrqAndTimedWaitMs(il, start + INTN_TIMEOUT);
+ }
+}
+
+int Wiz5500::waitForSocketIrq(miosix::Lock<miosix::FastMutex> &l, int sock_n,
+ uint8_t irq_mask, int timeout)
+{
+ // Check that someone else isn't already waiting for one of ours interrupts
+ if ((socket_infos[sock_n].irq_mask & irq_mask) != 0)
+ {
+ return 0;
+ }
+
+ // Enable the interrupts requested, updating the IRQ mask
+ socket_infos[sock_n].irq_mask |= irq_mask;
+ spiWrite8(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_IMR,
+ socket_infos[sock_n].irq_mask);
+
+ Thread *this_thread = Thread::getCurrentThread();
+
+ // Find a free slot in the data structure
+ int i = 0;
+ while (i < NUM_THREAD_WAIT_INFOS)
+ {
+ if (wait_infos[i].sock_n == -1)
+ {
+ wait_infos[i].sock_n = sock_n;
+ wait_infos[i].irq_mask = irq_mask;
+ wait_infos[i].irq = 0;
+ wait_infos[i].thread = this_thread;
+ break;
+ }
+
+ i++;
+ }
+
+ // We didn't find any, return with failure
+ if (i == NUM_THREAD_WAIT_INFOS)
+ {
+ return 0;
+ }
+
+ long long start = getTick();
+ TimedWaitResult result = TimedWaitResult::NoTimeout;
+
+ if (interrupt_service_thread != nullptr)
+ {
+ // There is already someone managing interrupts for us, just wait
+
+ Unlock<FastMutex> ul(l);
+ FastInterruptDisableLock il;
+ while (wait_infos[i].irq == 0 && result == TimedWaitResult::NoTimeout &&
+ interrupt_service_thread != this_thread)
+ {
+ if (timeout != -1)
+ {
+ result =
+ Thread::IRQenableIrqAndTimedWaitMs(il, start + timeout);
+ }
+ else
+ {
+ Thread::IRQenableIrqAndWait(il);
+ }
+ }
+ }
+ else
+ {
+ // Nobody is managing interrupts, we are doing it ourself
+ FastInterruptDisableLock il;
+ interrupt_service_thread = this_thread;
+ }
+
+ while (interrupt_service_thread == this_thread)
+ {
+ // Run a single step of the ISR
+ runInterruptServiceRoutine(l);
+
+ // Check if we woke up ourself, then we need to elect a new interrupt
+ // service thread
+ if (wait_infos[i].irq != 0)
+ {
+ for (int j = 0; j < NUM_THREAD_WAIT_INFOS; j++)
+ {
+ if (wait_infos[j].irq == 0)
+ {
+ {
+ FastInterruptDisableLock il;
+ interrupt_service_thread = wait_infos[j].thread;
+ }
+
+ wait_infos[j].thread->wakeup();
+ break;
+ }
+ }
+ }
+ }
+
+ // The interrupt arrived, clear the slot
+ wait_infos[i].sock_n = -1;
+
+ // Disable the interrupts
+ socket_infos[sock_n].irq_mask &= ~irq_mask;
+ spiWrite8(Wiz::getSocketRegBlock(sock_n), Wiz::Socket::REG_IMR,
+ socket_infos[sock_n].irq_mask);
+
+ return wait_infos[i].irq;
+}
+
+void Wiz5500::runInterruptServiceRoutine(Lock<FastMutex> &l)
+{
+ // Other threads might wake us up in order to
+ waitForINTn(l);
+
+ // Ok something happened!
+
+ // First check for general interrupts
+ uint8_t ir = spiRead8(0, Wiz::Common::REG_IR);
+ spiWrite8(0, Wiz::Common::REG_IR, ir);
+
+ uint8_t sn_ir[NUM_SOCKETS] = {0};
+
+ // Then check for interrupt on all the sockets
+ uint8_t sir = spiRead8(0, Wiz::Common::REG_SIR);
+ for (int i = 0; i < NUM_SOCKETS; i++)
+ {
+ if (sir & (1 << i))
+ {
+ sn_ir[i] = spiRead8(Wiz::getSocketRegBlock(i), Wiz::Socket::REG_IR);
+ spiWrite8(Wiz::getSocketRegBlock(i), Wiz::Socket::REG_IR, sn_ir[i]);
+ }
+ }
+
+ // Ok now wake up all threads in sleep
+ for (int i = 0; i < NUM_THREAD_WAIT_INFOS; i++)
+ {
+ if (wait_infos[i].sock_n != -1)
+ {
+ int sock_n = wait_infos[i].sock_n;
+ int irq = sn_ir[sock_n] & wait_infos[i].irq_mask;
+
+ if (irq != 0)
+ {
+ wait_infos[i].irq = irq;
+ wait_infos[i].thread->wakeup();
+ }
+ }
+ }
+
+ // Dispatch generic interrupts
+ if (ir & Wiz::Common::Irq::CONFLICT)
+ {
+ auto cb = on_ip_conflict;
+ if (cb)
+ {
+ Unlock<FastMutex> ul(l);
+ cb();
+ }
+ }
+
+ if (ir & Wiz::Common::Irq::UNREACH)
+ {
+ auto cb = on_dest_unreachable;
+ if (cb)
+ {
+ WizIp ip = spiReadIp(0, Wiz::Common::REG_UIPR);
+ uint16_t port = spiRead16(0, Wiz::Common::REG_UPORTR);
+
+ Unlock<FastMutex> ul(l);
+ cb(ip, port);
+ }
+ }
+}
+
+void Wiz5500::spiRead(uint8_t block, uint16_t address, uint8_t *data,
+ size_t len)
+{
+ // Do a manual SPI transaction
+ slave.bus.configure(slave.config);
+
+ slave.bus.select(slave.cs);
+ slave.bus.write16(address);
+ slave.bus.write(Wiz::buildControlWord(block, false));
+ slave.bus.read(data, len);
+ slave.bus.deselect(slave.cs);
+}
+
+void Wiz5500::spiWrite(uint8_t block, uint16_t address, const uint8_t *data,
+ size_t len)
+{
+ // Do a manual SPI transaction
+ slave.bus.configure(slave.config);
+
+ slave.bus.select(slave.cs);
+ slave.bus.write16(address);
+ slave.bus.write(Wiz::buildControlWord(block, true));
+ slave.bus.write(data, len);
+ slave.bus.deselect(slave.cs);
+}
+
+uint8_t Wiz5500::spiRead8(uint8_t block, uint16_t address)
+{
+ uint8_t data;
+ spiRead(block, address, &data, 1);
+ return data;
+}
+
+uint16_t Wiz5500::spiRead16(uint8_t block, uint16_t address)
+{
+ uint16_t data;
+ spiRead(block, address, reinterpret_cast<uint8_t *>(&data),
+ sizeof(uint16_t));
+ return fromBigEndian16(data);
+}
+
+WizIp Wiz5500::spiReadIp(uint8_t block, uint16_t address)
+{
+ WizIp data;
+ spiRead(block, address, reinterpret_cast<uint8_t *>(&data), sizeof(WizIp));
+ return data;
+}
+
+/*WizMac Wiz5500::spiReadMac(uint8_t block, uint16_t address)
+{
+ WizMac data;
+ spiRead(block, address, reinterpret_cast<uint8_t *>(&data), sizeof(WizMac));
+ return data;
+}*/
+
+void Wiz5500::spiWrite8(uint8_t block, uint16_t address, uint8_t data)
+{
+ spiWrite(block, address, &data, 1);
+}
+
+void Wiz5500::spiWrite16(uint8_t block, uint16_t address, uint16_t data)
+{
+ data = toBigEndian16(data);
+ spiWrite(block, address, reinterpret_cast<uint8_t *>(&data),
+ sizeof(uint16_t));
+}
+
+void Wiz5500::spiWriteIp(uint8_t block, uint16_t address, WizIp data)
+{
+ spiWrite(block, address, reinterpret_cast<uint8_t *>(&data), sizeof(WizIp));
+}
+
+void Wiz5500::spiWriteMac(uint8_t block, uint16_t address, WizMac data)
+{
+ spiWrite(block, address, reinterpret_cast<uint8_t *>(&data),
+ sizeof(WizMac));
+}
\ No newline at end of file
diff --git a/src/shared/drivers/WIZ5500/WIZ5500.h b/src/shared/drivers/WIZ5500/WIZ5500.h
new file mode 100644
index 000000000..50053ca4b
--- /dev/null
+++ b/src/shared/drivers/WIZ5500/WIZ5500.h
@@ -0,0 +1,324 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <ActiveObject.h>
+#include <drivers/spi/SPIDriver.h>
+#include <miosix.h>
+
+#include <functional>
+
+namespace Boardcore
+{
+
+/**
+ * @brief Class representing an IPv4 ip.
+ */
+struct WizIp
+{
+ uint8_t a, b, c, d;
+};
+
+/**
+ * @brief Class representing an ethernet MAC address.
+ */
+struct WizMac
+{
+ uint8_t a, b, c, d, e, f;
+};
+
+/**
+ * @brief Driver for the WizNet W5500 ethernet.
+ */
+class Wiz5500
+{
+public:
+ struct PhyState
+ {
+ bool full_duplex; //< True if full duplex is enabled, false if link is
+ // only half duplex.
+ bool based_100mbps; //< True if 100Mbps, false if only 10Mpbs.
+ bool link_up; //< True if link is up, false if it is down.
+ };
+
+ using OnIpConflictCb = std::function<void()>;
+ using OnDestUnreachableCb = std::function<void(WizIp, uint16_t)>;
+
+ /**
+ * @brief Build an instance of the driver.
+ *
+ * @param bus The underlying SPI bus.
+ * @param cs The SPI cs pin.
+ * @param intn The INTn pin.
+ * @param clock_divider Selected SPI clock divider.
+ */
+ Wiz5500(SPIBus& bus, miosix::GpioPin cs, miosix::GpioPin intn,
+ SPI::ClockDivider clock_divider);
+ ~Wiz5500();
+
+ /**
+ * @brief Sets the callback to be invoked when the device detects an IP.
+ * conflict.
+ *
+ * WARNING: DO NOT BLOCK IN THIS FUNCTION! ESPECIALLY DO NOT CALL
+ * connectTcp, listenTcp, send, recv, recvfrom, close, AS THEY WILL
+ * DEADLOCK!
+ *
+ * This callback will run in the interrupt service routine, and blocking
+ * this will block interrupt handling. This is not an issue, but while the
+ * thread is blocked no interrupt will be dispatched, and the aforementioned
+ * functions will not return.
+ *
+ * @param cb Callback to be invoked.
+ */
+ void setOnIpConflict(OnIpConflictCb cb);
+
+ /**
+ * @brief Sets the callback to be invoked when the device detects an
+ * unreachable host.
+ *
+ * WARNING: DO NOT BLOCK IN THIS FUNCTION! ESPECIALLY DO NOT CALL
+ * connectTcp, listenTcp, send, recv, recvfrom, close, AS THEY WILL
+ * DEADLOCK! *DO NOT* CALL close!
+ *
+ * This callback will run in the interrupt service routine, and blocking
+ * this will block interrupt handling. This is not an issue, but while the
+ * thread is blocked no interrupt will be dispatched, and the aforementioned
+ * functions will not return.
+ *
+ * @param cb Callback to be invoked.
+ */
+ void setOnDestUnreachable(OnDestUnreachableCb cb);
+
+ /**
+ * @brief Get current PHY state, can be used to poll link status, and wait
+ * for link up.
+ *
+ * @returns The current PHY state.
+ */
+ PhyState getPhyState();
+
+ /**
+ * @brief Resets the device.
+ * Performs a software resets, resetting all registers and closing all
+ * sockets. Also checks for hardware presence.
+ *
+ * @returns False if the device is not connected properly (SPI comunication
+ * failure).
+ */
+ bool reset();
+
+ /**
+ * @brief Handle an interrupt from INTn.
+ */
+ void handleINTn();
+
+ /**
+ * @brief Set global gateway ip.
+ */
+ void setGatewayIp(WizIp ip);
+
+ /**
+ * @brief Set global subnet mask.
+ */
+ void setSubnetMask(WizIp mask);
+
+ /**
+ * @brief Set the device MAC address.
+ */
+ void setSourceMac(WizMac mac);
+
+ /**
+ * @brief Set the device IP address.
+ */
+ void setSourceIp(WizIp ip);
+
+ /**
+ * @brief Connect to a remote socket via TCP.
+ *
+ * @param sock_n Index of the socket, from 0 to 7.
+ * @param src_port Local port of the TCP socket.
+ * @param dst_ip Remote IP of the TCP socket.
+ * @param dst_port Remote port of the TCP socket.
+ * @param timeout Timeout for the operation in ms (or -1 if no timeout).
+ *
+ * @return True in case of success, false otherwise.
+ */
+ bool connectTcp(int sock_n, uint16_t src_port, WizIp dst_ip,
+ uint16_t dst_port, int timeout = -1);
+
+ /**
+ * @brief Listen for a single remote TCP connection.
+ *
+ * @param sock_n Index of the socket, from 0 to 7.
+ * @param src_port Local port of the TCP socket.
+ * @param dst_ip Remote IP of the TCP socket.
+ * @param dst_port Remote port of the TCP socket.
+ * @param timeout Timeout for the operation in ms (or -1 if no timeout).
+ *
+ * @return True in case of success, false otherwise.
+ */
+ bool listenTcp(int sock_n, uint16_t src_port, WizIp& dst_ip,
+ uint16_t& dst_port, int timeout = -1);
+
+ /**
+ * @brief Open a simple UDP socket.
+ *
+ * @param sock_n Index of the socket, from 0 to 7.
+ * @param src_port Local port of the UDP socket.
+ * @param dst_ip Remote IP of the UDP socket.
+ * @param dst_port Remote port of the UDP socket.
+ * @param timeout Timeout for the operation in ms (or -1 if no timeout).
+ *
+ * @return True in case of success, false otherwise.
+ */
+ bool openUdp(int sock_n, uint16_t src_port, WizIp dst_ip, uint16_t dst_port,
+ int timeout = -1);
+
+ /**
+ * @brief Send data through the socket (works both in TCP and UDP).
+ *
+ * @param sock_n Index of the socket, from 0 to 7.
+ * @param data Data to be transmitted.
+ * @param len Length of the data.
+ * @param timeout Timeout for the operation in ms (or -1 if no timeout).
+ *
+ * @return True in case of success, false otherwise.
+ */
+ bool send(int sock_n, const uint8_t* data, size_t len, int timeout = -1);
+
+ /**
+ * @brief Receive data from the socket (works only in TCP).
+ *
+ * @param sock_n Index of the socket, from 0 to 7.
+ * @param data Buffer to store the data.
+ * @param len Maximum length of the data.
+ * @param timeout Timeout for the operation in ms (or -1 if no timeout).
+ *
+ * @return The length of the received data in case of success, -1 otherwise.
+ */
+ ssize_t recv(int sock_n, uint8_t* data, size_t len, int timeout = -1);
+
+ /**
+ * @brief Receive data from the socket (works only in UDP).
+ *
+ * @param sock_n Index of the socket, from 0 to 7.
+ * @param data Buffer to store the data.
+ * @param len Maximum length of the data.
+ * @param dst_ip Remote IP of the UDP socket.
+ * @param dst_port Remote port of the UDP socket.
+ * @param timeout Timeout for the operation in ms (or -1 if no timeout).
+ *
+ * @return The length of the received data in case of success, -1 otherwise.
+ */
+ ssize_t recvfrom(int sock_n, uint8_t* data, size_t len, WizIp& dst_ip,
+ uint16_t& dst_port, int timeout = -1);
+
+ /**
+ * @brief Close a socket.
+ *
+ * @param sock_n Index of the socket, from 0 to 7.
+ * @param timeout Timeout for the operation in ms (or -1 if no timeout).
+ */
+ void close(int sock_n, int timeout = -1);
+
+private:
+ static constexpr int NUM_THREAD_WAIT_INFOS = 16;
+ static constexpr int NUM_SOCKETS = 8;
+
+ void waitForINTn(miosix::Lock<miosix::FastMutex>& l);
+ int waitForSocketIrq(miosix::Lock<miosix::FastMutex>& l, int sock_n,
+ uint8_t irq_mask, int timeout);
+
+ void runInterruptServiceRoutine(miosix::Lock<miosix::FastMutex>& l);
+
+ void spiRead(uint8_t block, uint16_t address, uint8_t* data, size_t len);
+ void spiWrite(uint8_t block, uint16_t address, const uint8_t* data,
+ size_t len);
+
+ uint8_t spiRead8(uint8_t block, uint16_t address);
+ uint16_t spiRead16(uint8_t block, uint16_t address);
+ WizIp spiReadIp(uint8_t block, uint16_t address);
+ // Avoid stupid linter error
+ // WizMac spiReadMac(uint8_t block, uint16_t address);
+
+ void spiWrite8(uint8_t block, uint16_t address, uint8_t data);
+ void spiWrite16(uint8_t block, uint16_t address, uint16_t data);
+ void spiWriteIp(uint8_t block, uint16_t address, WizIp data);
+ void spiWriteMac(uint8_t block, uint16_t address, WizMac data);
+
+ miosix::Thread* interrupt_service_thread = nullptr;
+
+ struct ThreadWaitInfo
+ {
+ int sock_n;
+ uint8_t irq_mask;
+ uint8_t irq;
+ miosix::Thread* thread;
+ };
+
+ enum class SocketMode
+ {
+ TCP,
+ UDP,
+ CLOSED
+ };
+
+ struct SocketInfo
+ {
+ SocketMode mode;
+ int irq_mask;
+ };
+
+ SocketInfo socket_infos[NUM_SOCKETS];
+ ThreadWaitInfo wait_infos[NUM_THREAD_WAIT_INFOS];
+
+ OnIpConflictCb on_ip_conflict;
+ OnDestUnreachableCb on_dest_unreachable;
+
+ miosix::GpioPin intn;
+ miosix::FastMutex mutex;
+ SPISlave slave;
+};
+
+} // namespace Boardcore
+
+namespace std
+{
+inline ostream& operator<<(ostream& os, const Boardcore::WizIp& ip)
+{
+ auto old_flags = os.flags(os.dec);
+ os << (int)ip.a << "." << (int)ip.b << "." << (int)ip.c << "." << (int)ip.d;
+ os.flags(old_flags);
+ return os;
+}
+
+inline ostream& operator<<(ostream& os, const Boardcore::WizMac& mac)
+{
+ auto old_flags = os.flags(os.hex);
+ os << (int)mac.a << ":" << (int)mac.b << ":" << (int)mac.c << ":"
+ << (int)mac.d << ":" << (int)mac.e << ":" << (int)mac.f;
+ os.flags(old_flags);
+ return os;
+}
+} // namespace std
\ No newline at end of file
diff --git a/src/shared/drivers/WIZ5500/WIZ5500Defs.h b/src/shared/drivers/WIZ5500/WIZ5500Defs.h
new file mode 100644
index 000000000..096ab3ff0
--- /dev/null
+++ b/src/shared/drivers/WIZ5500/WIZ5500Defs.h
@@ -0,0 +1,175 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <cstdint>
+
+namespace Boardcore
+{
+
+namespace Wiz
+{
+
+static constexpr uint8_t VERSION = 0x04;
+
+inline uint8_t buildControlWord(uint8_t block, bool write)
+{
+ return (block & 0b11111) << 3 | (write ? 1 << 2 : 0);
+}
+
+inline uint8_t getSocketRegBlock(int n) { return (n << 2) | 0b01; }
+
+inline uint8_t getSocketTxBlock(int n) { return (n << 2) | 0b10; }
+
+inline uint8_t getSocketRxBlock(int n) { return (n << 2) | 0b11; }
+
+namespace Common
+{
+
+namespace Irq
+{
+constexpr static uint8_t CONFLICT = 1 << 7;
+constexpr static uint8_t UNREACH = 1 << 6;
+constexpr static uint8_t PPPOE = 1 << 5;
+constexpr static uint8_t MP = 1 << 4;
+} // namespace Irq
+
+enum Registers
+{
+ REG_MR = 0x0000, //< Mode Register.
+ REG_GAR = 0x0001, //< Gateway IP Address Register.
+ REG_SUBR = 0x0005, //< Subnet Mask Register.
+ REG_SHAR = 0x0009, //< Source Hardware Address Register.
+ REG_SIPR = 0x000f, //< Source IP Address Register.
+ REG_INTLEVEL = 0x0013, //< Interrupt Low Level Timer Register.
+ REG_IR = 0x0015, //< Interrupt Register.
+ REG_IMR = 0x0016, //< Interrupt Mask Register.
+ REG_SIR = 0x0017, //< Socket Interrupt Register.
+ REG_SIMR = 0x0018, //< Socket Interrupt Mask Register.
+ REG_RTR = 0x0019, //< Retry Time-value Register.
+ REG_PTIMER = 0x001C, //< PPP Link Control Protocol Request Timer Register.
+ REG_PMAGIC = 0x001D, //< PPP Link Control Protocol Magic number Register.
+ REG_PHAR = 0x001E, //< Destination Hardware Register in PPPoE mode.
+ REG_PSID = 0x0024, //< Session ID Register in PPPoE mode.
+ REG_PMRU = 0x0026, //< Maximum Receive Unit in PPPoE mode.
+ REG_UIPR = 0x0028, //< Unreachable IP Address Register.
+ REG_UPORTR = 0x002C, //< Unreachable Port Register.
+ REG_PHYCFGR = 0x002E, //< W5500 PHY Configuration Register.
+ REG_VERSIONR = 0x0039, //< W5500 Chip Version Register.
+};
+
+} // namespace Common
+
+namespace Socket
+{
+
+namespace Irq
+{
+constexpr static uint8_t SEND_OK = 1 << 4;
+constexpr static uint8_t TIMEOUT = 1 << 3;
+constexpr static uint8_t RECV = 1 << 2;
+constexpr static uint8_t DISCON = 1 << 1;
+constexpr static uint8_t CON = 1 << 0;
+} // namespace Irq
+
+enum Command
+{
+ CMD_OPEN = 0x01,
+ CMD_LISTEN = 0x02,
+ CMD_CONNECT = 0x04,
+ CMD_DISCON = 0x08,
+ CMD_CLOSE = 0x10,
+ CMD_SEND = 0x20,
+ CMD_SEND_MAC = 0x21,
+ CMD_SEND_KEEP = 0x22,
+ CMD_RECV = 0x40
+};
+
+enum Status
+{
+ STAT_CLOSED = 0x00,
+ STAT_INIT = 0x13,
+ STAT_LISTEN = 0x14,
+ STAT_SYNSENT = 0x15,
+ STAT_SYNRECV = 0x16,
+ STAT_ESTABLISHED = 0x17,
+ STAT_FIN_WAIT = 0x18,
+ STAT_CLOSING = 0x1A,
+ STAT_TIME_WAIT = 0x1B,
+ STAT_CLOSE_WAIT = 0x1C,
+ STAT_LAST_ACK = 0x1D,
+ STAT_UDP = 0x22,
+ STAT_MACRAW = 0x42,
+};
+
+enum Registers
+{
+ REG_MR = 0x0000, //< Socket n Mode Register.
+ REG_CR = 0x0001, //< Socket n Command Register.
+ REG_IR = 0x0002, //< Socket n Interrupt Register.
+ REG_SR = 0x0003, //< Socket n Status Register.
+ REG_PORT = 0x0004, //< Socket n Source Port Register.
+ REG_DHAR = 0x0006, //< Socket n Destination Hardware Address Register.
+ REG_DIPR = 0x000C, //< Socket n Destination IP Address Register.
+ REG_DPORT = 0x0010, //< Socket n Destination Port Register.
+ REG_MSSR = 0x0012, //< Socket n Maximum Segment Size Register.
+ REG_TOS = 0x0015, //< Socket n IP Type of Server Register.
+ REG_TTL = 0x0016, //< Socket n TTL Register.
+ REG_RXBUF_SIZE = 0x001E, //< Socket n RX Buffer Size Register.
+ REG_TXBUF_SIZE = 0x001F, //< Socket n TX Buffer Size Register.
+ REG_TX_FSR = 0x0020, //< Socket n TX Free Size Register.
+ REG_TX_RD = 0x0022, //< Socket n TX Read Pointer Register.
+ REG_TX_WR = 0x0024, //< Socket n TX Write Pointer Register.
+ REG_RX_RSR = 0x0026, //< Socket n Received Size Register.
+ REG_RX_RD = 0x0028, //< Socket n RX Read Data Pointer Register.
+ REG_RX_WR = 0x002A, //< Socket n RX Write Pointer Register.
+ REG_IMR = 0x002C, //< Socket n Interrupt Mask Register.
+ REG_FRAG = 0x002D, //< Socket n Fragment Register.
+ REG_KPALVTR = 0x002F, //< Socket n Keep Alive Time Register.
+};
+
+inline uint8_t buildModeUdp(bool enable_multicast, bool block_broadcast,
+ bool multicast_use_igmp_v1, bool block_unicast)
+{
+ return (enable_multicast ? 1 << 7 : 0) | (block_broadcast ? 1 << 6 : 0) |
+ (multicast_use_igmp_v1 ? 1 << 5 : 0) | (block_unicast ? 1 << 4 : 0) |
+ 0b0010;
+}
+
+inline uint8_t buildModeTcp(bool enable_no_delayed_ack)
+{
+ return (enable_no_delayed_ack ? 1 << 5 : 0) | 0b0001;
+}
+
+inline uint8_t buildModeMacraw(bool enable_mac_filter, bool block_broadcast,
+ bool block_multicast, bool block_ipv6)
+{
+ return (enable_mac_filter ? 1 << 7 : 0) | (block_broadcast ? 1 << 6 : 0) |
+ (block_multicast ? 1 << 5 : 0) | (block_ipv6 ? 1 << 4 : 0) | 0b0100;
+}
+
+} // namespace Socket
+
+} // namespace Wiz
+
+}; // namespace Boardcore
\ No newline at end of file
diff --git a/src/tests/drivers/test-wiz5500.cpp b/src/tests/drivers/test-wiz5500.cpp
new file mode 100644
index 000000000..a5064910a
--- /dev/null
+++ b/src/tests/drivers/test-wiz5500.cpp
@@ -0,0 +1,327 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <arch/common/drivers/stm32_hardware_rng.h>
+#include <drivers/WIZ5500/WIZ5500.h>
+#include <drivers/WIZ5500/WIZ5500Defs.h>
+
+#include <cstring>
+#include <iostream>
+#include <thread>
+
+using namespace Boardcore;
+using namespace miosix;
+
+#if defined _BOARD_STM32F429ZI_STM32F4DISCOVERY
+
+using cs = Gpio<GPIOC_BASE, 13>;
+using sck = Gpio<GPIOE_BASE, 2>;
+using miso = Gpio<GPIOE_BASE, 5>;
+using mosi = Gpio<GPIOE_BASE, 6>;
+using intn = Gpio<GPIOF_BASE, 6>;
+
+SPIBus bus(SPI4);
+
+#define INTN_IRQ EXTI6_IRQHandlerImpl
+
+void setupBoard()
+{
+ sck::mode(Mode::ALTERNATE);
+ sck::alternateFunction(5);
+ miso::mode(Mode::ALTERNATE);
+ miso::alternateFunction(5);
+ mosi::mode(Mode::ALTERNATE);
+ mosi::alternateFunction(5);
+ cs::mode(Mode::OUTPUT);
+ cs::high();
+ intn::mode(Mode::INPUT);
+}
+
+#elif defined _BOARD_STM32F767ZI_GEMINI_GS
+#include "interfaces-impl/hwmapping.h"
+
+using cs = ethernet::cs;
+using sck = ethernet::spi::sck;
+using miso = ethernet::spi::miso;
+using mosi = ethernet::spi::mosi;
+using intn = ethernet::intr;
+
+SPIBus bus(MIOSIX_ETHERNET_SPI);
+
+#define INTN_IRQ MIOSIX_ETHERNET_IRQ
+
+void setupBoard() {}
+
+#else
+#error "Target not supported"
+#endif
+
+Wiz5500 *wiz = nullptr;
+
+#ifdef INTN_IRQ
+void __attribute__((used)) INTN_IRQ()
+{
+ if (wiz)
+ wiz->handleINTn();
+}
+#endif
+
+void socket0SendLoop()
+{
+ int i = 0;
+ while (true)
+ {
+ char msg[1024];
+ size_t len = sprintf(msg, "Suca palle (DIO0) (0 %d)\n", i);
+
+ printf("[wiz5500] Sending though socket 0...\n");
+ wiz->send(0, reinterpret_cast<uint8_t *>(msg), len);
+ Thread::sleep(2000);
+ i += 1;
+ }
+}
+
+void socket0RecvLoop()
+{
+ while (true)
+ {
+ char msg[1024];
+ ssize_t len =
+ wiz->recv(0, reinterpret_cast<uint8_t *>(msg), sizeof(msg));
+
+ if (len != -1)
+ {
+ msg[len] = '\0';
+ printf("[wiz5500] Received \"%s\" from socket 0\n", msg);
+ }
+ else
+ {
+ Thread::sleep(1000);
+ }
+ }
+}
+
+void socket0Start()
+{
+ WizIp dst_ip;
+ uint16_t dst_port;
+
+ printf("[wiz5500] Opening socket 0...\n");
+ bool opened = wiz->listenTcp(0, 8080, dst_ip, dst_port);
+ std::cout << dst_ip << " " << dst_port << std::endl;
+ if (opened)
+ {
+ std::thread t(socket0SendLoop);
+ socket0RecvLoop();
+ }
+ else
+ {
+ printf("[wiz5500] Failed to open socket 0!\n");
+ }
+}
+
+void socket1SendLoop()
+{
+ int i = 0;
+ while (true)
+ {
+ char msg[1024];
+ size_t len = sprintf(msg, "Suca palle (DIO0) (1 %d)\n", i);
+
+ printf("[wiz5500] Sending though socket 1...\n");
+ wiz->send(1, reinterpret_cast<uint8_t *>(msg), len);
+ Thread::sleep(1333);
+ i += 1;
+ }
+}
+
+void socket1RecvLoop()
+{
+ while (true)
+ {
+ char msg[1024];
+ ssize_t len =
+ wiz->recv(1, reinterpret_cast<uint8_t *>(msg), sizeof(msg));
+
+ if (len != -1)
+ {
+ msg[len] = '\0';
+ printf("[wiz5500] Received \"%s\" from socket 1\n", msg);
+ }
+ else
+ {
+ Thread::sleep(1000);
+ }
+ }
+}
+
+void socket1Start()
+{
+ printf("[wiz5500] Opening socket 1...\n");
+ bool opened = wiz->connectTcp(1, 8081, {192, 168, 1, 12}, 8080);
+ if (opened)
+ {
+ std::thread t(socket1SendLoop);
+ socket1RecvLoop();
+ }
+ else
+ {
+ printf("[wiz5500] Failed to open socket 1!\n");
+ }
+}
+
+void socket2SendLoop()
+{
+ int i = 0;
+ while (true)
+ {
+ char msg[1024];
+ size_t len = sprintf(msg, "Suca palle (DIO0) (2 %d)\n", i);
+
+ printf("[wiz5500] Sending though socket 2...\n");
+ wiz->send(2, reinterpret_cast<uint8_t *>(msg), len);
+ Thread::sleep(1666);
+ i += 1;
+ }
+}
+
+void socket2RecvLoop()
+{
+ while (true)
+ {
+ char msg[1024];
+ WizIp dst_ip;
+ uint16_t dst_port;
+ ssize_t len = wiz->recvfrom(2, reinterpret_cast<uint8_t *>(msg),
+ sizeof(msg), dst_ip, dst_port);
+
+ std::cout << dst_ip << " " << dst_port << std::endl;
+
+ if (len != -1)
+ {
+ msg[len] = '\0';
+ printf("[wiz5500] Received \"%s\" from socket 2\n", msg);
+ }
+ else
+ {
+ Thread::sleep(1000);
+ }
+ }
+}
+
+void socket2Start()
+{
+ printf("[wiz5500] Opening socket 2...\n");
+ bool opened = wiz->openUdp(2, 8081, {192, 168, 1, 12}, 8080);
+ if (opened)
+ {
+ std::thread t(socket2SendLoop);
+ socket2RecvLoop();
+ }
+ else
+ {
+ printf("[wiz5500] Failed to open socket 2!\n");
+ }
+}
+
+int genRandom(int min, int max) { return (rand() % (max - min)) + min; }
+
+WizIp genNewIp()
+{
+ // Generate IPs in the range 1-254
+ uint8_t d = genRandom(1, 254);
+ return {192, 168, 1, d};
+}
+
+WizMac genNewMac()
+{
+ uint8_t e = genRandom(1, 254);
+ uint8_t f = genRandom(1, 254);
+ return {0x69, 0x69, 0x69, 0x69, e, f};
+}
+
+int main()
+{
+ // Pick a random seed
+ srand(HardwareRng::instance().get());
+
+ setupBoard();
+
+ wiz = new Wiz5500(bus, cs::getPin(), intn::getPin(),
+ SPI::ClockDivider::DIV_64);
+
+ // Start the driver
+ if (!wiz->reset())
+ {
+ printf("[wiz5500] Wiz failed to start!\n");
+ while (1)
+ ;
+ }
+
+ wiz->setOnDestUnreachable(
+ [](WizIp ip, uint16_t port)
+ {
+ printf("[wiz5500] Destination unreachable\n");
+ std::cout << ip << ":" << port << std::endl;
+ });
+
+ WizIp ip = genNewIp();
+ WizMac mac = genNewMac();
+
+ std::cout << "New ip: " << ip << std::endl;
+ std::cout << "New mac: " << mac << std::endl;
+
+ // Change IP if conflict detected
+ wiz->setOnIpConflict(
+ [&ip]()
+ {
+ printf("[wiz5500] Ip conflict\n");
+ ip = genNewIp();
+ std::cout << "New ip: " << ip << std::endl;
+ wiz->setSourceIp(genNewIp());
+ });
+
+ wiz->setGatewayIp({192, 168, 1, 1});
+ wiz->setSubnetMask({255, 255, 225, 0});
+ wiz->setSourceIp(ip);
+ wiz->setSourceMac(mac);
+
+ std::thread t1(socket0Start);
+ std::thread t2(socket1Start);
+ std::thread t3(socket2Start);
+
+ while (1)
+ {
+ Wiz5500::PhyState state = wiz->getPhyState();
+
+ printf(
+ "Current PHY state:\n"
+ "- full_duplex: %d\n"
+ "- based_100mbps: %d\n"
+ "- link_up: %d\n",
+ state.full_duplex, state.based_100mbps, state.link_up);
+
+ Thread::sleep(5000);
+ }
+
+ return 0;
+}
\ No newline at end of file
--
GitLab