diff --git a/src/main.rs b/src/main.rs
index da32e01..edc6190 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -10,10 +10,7 @@ use cyw43_pio::PioSpi;
 use defmt::{debug, error, info, warn, Display2Format, Format};
 use embassy_boot::{AlignedBuffer, BlockingFirmwareUpdater, FirmwareUpdaterConfig};
 use embassy_executor::Spawner;
-use embassy_futures::{
-    select::{select3, select4, Either3},
-    yield_now,
-};
+use embassy_futures::select::{select3, select4, Either3};
 use embassy_net::{dns::DnsQueryType, tcp::TcpSocket, Config, Stack, StackResources};
 use embassy_rp::{
     bind_interrupts,
@@ -243,14 +240,17 @@ unsafe fn get_firmware() -> (&'static [u8], &'static [u8]) {
 async fn wait_for_config(
     stack: &'static Stack<cyw43::NetDriver<'static>>,
 ) -> embassy_net::StaticConfigV4 {
-    loop {
+    for _ in 0..120 {
         // We are essentially busy looping here since there is no Async API for this
         if let Some(config) = stack.config_v4() {
             return config;
         }
 
-        yield_now().await;
+        Timer::after_secs(1).await;
     }
+
+    info!("Restarting...");
+    cortex_m::peripheral::SCB::sys_reset();
 }
 
 #[embassy_executor::task]
@@ -390,7 +390,9 @@ async fn main(spawner: Spawner) {
     );
 
     info!("Connecting to MQTT...");
-    client.connect_to_broker().await.unwrap();
+    if client.connect_to_broker().await.is_err() {
+        cortex_m::peripheral::SCB::sys_reset();
+    };
     info!("MQTT Connected!");
 
     // We wait with marking as booted until everything is connected
@@ -460,7 +462,10 @@ async fn main(spawner: Spawner) {
                 Ok(_) => None,
                 Err(err) => {
                     error!("{}", err);
-                    None
+                    info!("Restarting in 5s...");
+                    Timer::after(Duration::from_secs(5)).await;
+                    info!("Restarting...");
+                    cortex_m::peripheral::SCB::sys_reset();
                 }
             },
             Either3::Third(state) => Some(StateMessage::new(state)),