From a7a052d4ce77f536964d60492b419e38c83dc94e Mon Sep 17 00:00:00 2001
From: Dreaded_X <tim@huizinga.dev>
Date: Thu, 7 Sep 2023 00:30:56 +0200
Subject: [PATCH] Implemented first version of OTA

---
 .cargo/config.toml    |   2 +-
 Cargo.lock            | 413 +++++++++++++++++++++++++++++++++++++++---
 Cargo.toml            |   8 +-
 bootloader/Cargo.toml |   1 -
 src/main.rs           | 137 ++++++++++++--
 5 files changed, 515 insertions(+), 46 deletions(-)

diff --git a/.cargo/config.toml b/.cargo/config.toml
index 9876bd2..431a642 100644
--- a/.cargo/config.toml
+++ b/.cargo/config.toml
@@ -9,4 +9,4 @@ runner = "./wrapper.sh"
 target = "thumbv6m-none-eabi" # Cortex-M0 and Cortex-M0+
 
 [env]
-DEFMT_LOG = "warn,rp=trace,bootloader=trace"
+DEFMT_LOG = "warn,rp=info"
diff --git a/Cargo.lock b/Cargo.lock
index 9c580aa..314112d 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -2,6 +2,41 @@
 # It is not intended for manual editing.
 version = 3
 
+[[package]]
+name = "aead"
+version = "0.5.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d122413f284cf2d62fb1b7db97e02edb8cda96d769b16e443a4f6195e35662b0"
+dependencies = [
+ "crypto-common",
+ "generic-array 0.14.7",
+]
+
+[[package]]
+name = "aes"
+version = "0.8.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ac1f845298e95f983ff1944b728ae08b8cebab80d684f0a832ed0fc74dfa27e2"
+dependencies = [
+ "cfg-if",
+ "cipher",
+ "cpufeatures",
+]
+
+[[package]]
+name = "aes-gcm"
+version = "0.10.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "209b47e8954a928e1d72e86eca7000ebb6655fe1436d33eefc2201cad027e237"
+dependencies = [
+ "aead",
+ "aes",
+ "cipher",
+ "ctr",
+ "ghash",
+ "subtle",
+]
+
 [[package]]
 name = "aho-corasick"
 version = "1.0.5"
@@ -98,6 +133,18 @@ dependencies = [
  "rustc_version 0.2.3",
 ]
 
+[[package]]
+name = "base16ct"
+version = "0.2.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "4c7f02d4ea65f2c1853089ffd8d2787bdbc63de2f0d29dedbcf8ccdfa0ccd4cf"
+
+[[package]]
+name = "base64"
+version = "0.21.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "414dcefbc63d77c526a76b3afcf6fbb9b5e2791c19c3aa2297733208750c6e53"
+
 [[package]]
 name = "bit-set"
 version = "0.5.3"
@@ -153,7 +200,16 @@ dependencies = [
  "embassy-sync",
  "embassy-time",
  "embedded-storage",
- "embedded-storage-async",
+]
+
+[[package]]
+name = "buffered-io"
+version = "0.2.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9de70b60ec8429fe379d65fbc49bdd6d53d68486ff1e3482f07ab47814912782"
+dependencies = [
+ "embedded-io",
+ "embedded-io-async",
 ]
 
 [[package]]
@@ -183,6 +239,16 @@ version = "1.0.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd"
 
+[[package]]
+name = "cipher"
+version = "0.4.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "773f3b9af64447d2ce9850330c473515014aa235e6a783b02db81ff39e4a3dad"
+dependencies = [
+ "crypto-common",
+ "inout",
+]
+
 [[package]]
 name = "codespan-reporting"
 version = "0.11.1"
@@ -193,6 +259,12 @@ dependencies = [
  "unicode-width",
 ]
 
+[[package]]
+name = "const-oid"
+version = "0.9.5"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "28c122c3980598d243d63d9a704629a2d748d101f278052ff068be5a4423ab6f"
+
 [[package]]
 name = "cortex-m"
 version = "0.7.7"
@@ -226,6 +298,15 @@ dependencies = [
  "syn 1.0.109",
 ]
 
+[[package]]
+name = "cpufeatures"
+version = "0.2.9"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a17b76ff3a4162b0b27f354a0c87015ddad39d35f9c0c36607a3bdd175dde1f1"
+dependencies = [
+ "libc",
+]
+
 [[package]]
 name = "crc-any"
 version = "2.4.3"
@@ -253,6 +334,18 @@ version = "0.2.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "7a81dae078cea95a014a339291cec439d2f232ebe854a9d672b796c6afafa9b7"
 
+[[package]]
+name = "crypto-bigint"
+version = "0.5.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "740fe28e594155f10cfc383984cbefd529d7396050557148f79cb0f621204124"
+dependencies = [
+ "generic-array 0.14.7",
+ "rand_core",
+ "subtle",
+ "zeroize",
+]
+
 [[package]]
 name = "crypto-common"
 version = "0.1.6"
@@ -263,10 +356,19 @@ dependencies = [
  "typenum",
 ]
 
+[[package]]
+name = "ctr"
+version = "0.9.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "0369ee1ad671834580515889b80f2ea915f23b8be8d0daa4bbaf2ac5c7590835"
+dependencies = [
+ "cipher",
+]
+
 [[package]]
 name = "cyw43"
 version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy#9d8c527308522698bfb6596bdb67bec826e0fb5a"
+source = "git+https://github.com/embassy-rs/embassy#af7c93abba768057f1d3299c7b4f4aa4501b3e56"
 dependencies = [
  "atomic-polyfill 0.1.11",
  "cortex-m",
@@ -284,7 +386,7 @@ dependencies = [
 [[package]]
 name = "cyw43-pio"
 version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy#9d8c527308522698bfb6596bdb67bec826e0fb5a"
+source = "git+https://github.com/embassy-rs/embassy#af7c93abba768057f1d3299c7b4f4aa4501b3e56"
 dependencies = [
  "cyw43",
  "defmt",
@@ -377,6 +479,16 @@ dependencies = [
  "defmt",
 ]
 
+[[package]]
+name = "der"
+version = "0.7.8"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "fffa369a668c8af7dbf8b5e56c9f744fbd399949ed171606040001947de40b1c"
+dependencies = [
+ "const-oid",
+ "zeroize",
+]
+
 [[package]]
 name = "diff"
 version = "0.1.13"
@@ -391,6 +503,7 @@ checksum = "9ed9a281f7bc9b7576e61468ba615a66a5c8cfdff42420a70aa82701a3b1e292"
 dependencies = [
  "block-buffer",
  "crypto-common",
+ "subtle",
 ]
 
 [[package]]
@@ -436,23 +549,43 @@ version = "1.9.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "a26ae43d7bcc3b814de94796a5e736d4029efb0ee900c12e2d54c993ad1a1e07"
 
+[[package]]
+name = "elliptic-curve"
+version = "0.13.5"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "968405c8fdc9b3bf4df0a6638858cc0b52462836ab6b1c87377785dd09cf1c0b"
+dependencies = [
+ "base16ct",
+ "crypto-bigint",
+ "digest",
+ "ff",
+ "generic-array 0.14.7",
+ "group",
+ "hkdf",
+ "rand_core",
+ "sec1",
+ "subtle",
+ "zeroize",
+]
+
 [[package]]
 name = "embassy-boot"
 version = "0.1.1"
-source = "git+https://github.com/embassy-rs/embassy#9d8c527308522698bfb6596bdb67bec826e0fb5a"
+source = "git+https://github.com/embassy-rs/embassy#af7c93abba768057f1d3299c7b4f4aa4501b3e56"
 dependencies = [
  "defmt",
  "digest",
  "embassy-embedded-hal",
  "embassy-sync",
  "embedded-storage",
+ "embedded-storage-async",
  "signature",
 ]
 
 [[package]]
 name = "embassy-boot-rp"
 version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy#9d8c527308522698bfb6596bdb67bec826e0fb5a"
+source = "git+https://github.com/embassy-rs/embassy#af7c93abba768057f1d3299c7b4f4aa4501b3e56"
 dependencies = [
  "cfg-if",
  "cortex-m",
@@ -463,12 +596,13 @@ dependencies = [
  "embassy-sync",
  "embassy-time",
  "embedded-storage",
+ "embedded-storage-async",
 ]
 
 [[package]]
 name = "embassy-embedded-hal"
 version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy#9d8c527308522698bfb6596bdb67bec826e0fb5a"
+source = "git+https://github.com/embassy-rs/embassy#af7c93abba768057f1d3299c7b4f4aa4501b3e56"
 dependencies = [
  "embassy-futures",
  "embassy-sync",
@@ -484,7 +618,7 @@ dependencies = [
 [[package]]
 name = "embassy-executor"
 version = "0.3.0"
-source = "git+https://github.com/embassy-rs/embassy#9d8c527308522698bfb6596bdb67bec826e0fb5a"
+source = "git+https://github.com/embassy-rs/embassy#af7c93abba768057f1d3299c7b4f4aa4501b3e56"
 dependencies = [
  "atomic-polyfill 1.0.3",
  "cortex-m",
@@ -499,7 +633,7 @@ dependencies = [
 [[package]]
 name = "embassy-futures"
 version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy#9d8c527308522698bfb6596bdb67bec826e0fb5a"
+source = "git+https://github.com/embassy-rs/embassy#af7c93abba768057f1d3299c7b4f4aa4501b3e56"
 dependencies = [
  "defmt",
 ]
@@ -507,7 +641,7 @@ dependencies = [
 [[package]]
 name = "embassy-hal-internal"
 version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy#9d8c527308522698bfb6596bdb67bec826e0fb5a"
+source = "git+https://github.com/embassy-rs/embassy#af7c93abba768057f1d3299c7b4f4aa4501b3e56"
 dependencies = [
  "cortex-m",
  "critical-section",
@@ -518,7 +652,7 @@ dependencies = [
 [[package]]
 name = "embassy-macros"
 version = "0.2.1"
-source = "git+https://github.com/embassy-rs/embassy#9d8c527308522698bfb6596bdb67bec826e0fb5a"
+source = "git+https://github.com/embassy-rs/embassy#af7c93abba768057f1d3299c7b4f4aa4501b3e56"
 dependencies = [
  "darling",
  "proc-macro2",
@@ -529,7 +663,7 @@ dependencies = [
 [[package]]
 name = "embassy-net"
 version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy#9d8c527308522698bfb6596bdb67bec826e0fb5a"
+source = "git+https://github.com/embassy-rs/embassy#af7c93abba768057f1d3299c7b4f4aa4501b3e56"
 dependencies = [
  "as-slice 0.2.1",
  "atomic-polyfill 1.0.3",
@@ -542,7 +676,7 @@ dependencies = [
  "embedded-nal-async",
  "futures",
  "generic-array 0.14.7",
- "heapless",
+ "heapless 0.7.16",
  "managed",
  "smoltcp",
  "stable_deref_trait",
@@ -551,7 +685,7 @@ dependencies = [
 [[package]]
 name = "embassy-net-driver"
 version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy#9d8c527308522698bfb6596bdb67bec826e0fb5a"
+source = "git+https://github.com/embassy-rs/embassy#af7c93abba768057f1d3299c7b4f4aa4501b3e56"
 dependencies = [
  "defmt",
 ]
@@ -559,7 +693,7 @@ dependencies = [
 [[package]]
 name = "embassy-net-driver-channel"
 version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy#9d8c527308522698bfb6596bdb67bec826e0fb5a"
+source = "git+https://github.com/embassy-rs/embassy#af7c93abba768057f1d3299c7b4f4aa4501b3e56"
 dependencies = [
  "embassy-futures",
  "embassy-net-driver",
@@ -569,7 +703,7 @@ dependencies = [
 [[package]]
 name = "embassy-rp"
 version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy#9d8c527308522698bfb6596bdb67bec826e0fb5a"
+source = "git+https://github.com/embassy-rs/embassy#af7c93abba768057f1d3299c7b4f4aa4501b3e56"
 dependencies = [
  "atomic-polyfill 1.0.3",
  "cfg-if",
@@ -605,19 +739,19 @@ dependencies = [
 [[package]]
 name = "embassy-sync"
 version = "0.2.0"
-source = "git+https://github.com/embassy-rs/embassy#9d8c527308522698bfb6596bdb67bec826e0fb5a"
+source = "git+https://github.com/embassy-rs/embassy#af7c93abba768057f1d3299c7b4f4aa4501b3e56"
 dependencies = [
  "cfg-if",
  "critical-section",
  "defmt",
  "futures-util",
- "heapless",
+ "heapless 0.7.16",
 ]
 
 [[package]]
 name = "embassy-time"
 version = "0.1.3"
-source = "git+https://github.com/embassy-rs/embassy#9d8c527308522698bfb6596bdb67bec826e0fb5a"
+source = "git+https://github.com/embassy-rs/embassy#af7c93abba768057f1d3299c7b4f4aa4501b3e56"
 dependencies = [
  "atomic-polyfill 1.0.3",
  "cfg-if",
@@ -627,20 +761,20 @@ dependencies = [
  "embedded-hal 1.0.0-rc.1",
  "embedded-hal-async",
  "futures-util",
- "heapless",
+ "heapless 0.7.16",
 ]
 
 [[package]]
 name = "embassy-usb"
 version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy#9d8c527308522698bfb6596bdb67bec826e0fb5a"
+source = "git+https://github.com/embassy-rs/embassy#af7c93abba768057f1d3299c7b4f4aa4501b3e56"
 dependencies = [
  "defmt",
  "embassy-futures",
  "embassy-net-driver-channel",
  "embassy-sync",
  "embassy-usb-driver",
- "heapless",
+ "heapless 0.7.16",
  "ssmarshal",
  "usbd-hid",
 ]
@@ -648,7 +782,7 @@ dependencies = [
 [[package]]
 name = "embassy-usb-driver"
 version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy#9d8c527308522698bfb6596bdb67bec826e0fb5a"
+source = "git+https://github.com/embassy-rs/embassy#af7c93abba768057f1d3299c7b4f4aa4501b3e56"
 dependencies = [
  "defmt",
 ]
@@ -713,7 +847,7 @@ version = "0.7.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "447416d161ba378782c13e82b11b267d6d2104b4913679a7c5640e7e94f96ea7"
 dependencies = [
- "heapless",
+ "heapless 0.7.16",
  "nb 1.1.0",
  "no-std-net",
 ]
@@ -726,7 +860,7 @@ checksum = "e706b5288eceaa582155b24d54e23385429402e4e99ae984f4a0a6a65b635f15"
 dependencies = [
  "embedded-io-async",
  "embedded-nal",
- "heapless",
+ "heapless 0.7.16",
  "no-std-net",
 ]
 
@@ -745,6 +879,29 @@ dependencies = [
  "embedded-storage",
 ]
 
+[[package]]
+name = "embedded-tls"
+version = "0.15.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e0427cd46c51f67e68abe046a63f2c1d3820d9f1a99b55792869a407d981afd5"
+dependencies = [
+ "aes-gcm",
+ "atomic-polyfill 1.0.3",
+ "defmt",
+ "digest",
+ "embedded-io",
+ "embedded-io-async",
+ "generic-array 0.14.7",
+ "heapless 0.6.1",
+ "heapless 0.7.16",
+ "hkdf",
+ "hmac",
+ "p256",
+ "rand_core",
+ "sha2",
+ "typenum",
+]
+
 [[package]]
 name = "ena"
 version = "0.14.2"
@@ -787,6 +944,16 @@ dependencies = [
  "libc",
 ]
 
+[[package]]
+name = "ff"
+version = "0.13.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ded41244b729663b1e574f1b4fb731469f69f79c17667b5d776b16cda0479449"
+dependencies = [
+ "rand_core",
+ "subtle",
+]
+
 [[package]]
 name = "fixed"
 version = "1.23.1"
@@ -910,6 +1077,7 @@ checksum = "85649ca51fd72272d7821adaf274ad91c288277713d9c18820d8499a7ff69e9a"
 dependencies = [
  "typenum",
  "version_check",
+ "zeroize",
 ]
 
 [[package]]
@@ -923,6 +1091,27 @@ dependencies = [
  "wasi",
 ]
 
+[[package]]
+name = "ghash"
+version = "0.5.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d930750de5717d2dd0b8c0d42c076c0e884c81a73e6cab859bbd2339c71e3e40"
+dependencies = [
+ "opaque-debug",
+ "polyval",
+]
+
+[[package]]
+name = "group"
+version = "0.13.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f0f9ef7462f7c099f518d754361858f86d8a07af53ba9af0fe635bbccb151a63"
+dependencies = [
+ "ff",
+ "rand_core",
+ "subtle",
+]
+
 [[package]]
 name = "half"
 version = "2.3.1"
@@ -933,6 +1122,15 @@ dependencies = [
  "crunchy",
 ]
 
+[[package]]
+name = "hash32"
+version = "0.1.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d4041af86e63ac4298ce40e5cca669066e75b6f1aa3390fe2561ffa5e1d9f4cc"
+dependencies = [
+ "byteorder",
+]
+
 [[package]]
 name = "hash32"
 version = "0.2.1"
@@ -948,6 +1146,18 @@ version = "0.14.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "2c6201b9ff9fd90a5a3bac2e56a830d0caa509576f0e503818ee82c181b3437a"
 
+[[package]]
+name = "heapless"
+version = "0.6.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "634bd4d29cbf24424d0a4bfcbf80c6960129dc24424752a7d1d1390607023422"
+dependencies = [
+ "as-slice 0.1.5",
+ "generic-array 0.14.7",
+ "hash32 0.1.1",
+ "stable_deref_trait",
+]
+
 [[package]]
 name = "heapless"
 version = "0.7.16"
@@ -956,7 +1166,7 @@ checksum = "db04bc24a18b9ea980628ecf00e6c0264f3c1426dac36c00cb49b6fbad8b0743"
 dependencies = [
  "atomic-polyfill 0.1.11",
  "defmt",
- "hash32",
+ "hash32 0.2.1",
  "rustc_version 0.4.0",
  "serde",
  "spin",
@@ -969,6 +1179,36 @@ version = "0.3.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "443144c8cdadd93ebf52ddb4056d257f5b52c04d3c804e657d19eb73fc33668b"
 
+[[package]]
+name = "hex"
+version = "0.4.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7f24254aa9a54b5c858eaee2f5bccdb46aaf0e486a595ed5fd8f86ba55232a70"
+
+[[package]]
+name = "hkdf"
+version = "0.12.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "791a029f6b9fc27657f6f188ec6e5e43f6911f6f878e0dc5501396e09809d437"
+dependencies = [
+ "hmac",
+]
+
+[[package]]
+name = "hmac"
+version = "0.12.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "6c49c37c09c17a53d937dfbb742eb3a961d65a994e6bcdcf37e7399d0cc8ab5e"
+dependencies = [
+ "digest",
+]
+
+[[package]]
+name = "httparse"
+version = "1.8.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d897f394bad6a705d5f4104762e116a75639e470d80901eed05a860a95cb1904"
+
 [[package]]
 name = "ident_case"
 version = "1.0.1"
@@ -985,6 +1225,15 @@ dependencies = [
  "hashbrown",
 ]
 
+[[package]]
+name = "inout"
+version = "0.1.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a0c10553d664a4d0bcff9f4215d0aac67a639cc68ef660840afe309b807bc9f5"
+dependencies = [
+ "generic-array 0.14.7",
+]
+
 [[package]]
 name = "is-terminal"
 version = "0.4.9"
@@ -1146,6 +1395,22 @@ version = "1.18.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "dd8b5dd2ae5ed71462c540258bedcb51965123ad7e7ccf4b9a8cafaa4a63576d"
 
+[[package]]
+name = "opaque-debug"
+version = "0.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "624a8340c38c1b80fd549087862da4ba43e08858af025b236e509b6649fc13d5"
+
+[[package]]
+name = "p256"
+version = "0.13.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "c9863ad85fa8f4460f9c48cb909d38a0d689dba1f6f6988a5e3e0d31071bcd4b"
+dependencies = [
+ "elliptic-curve",
+ "primeorder",
+]
+
 [[package]]
 name = "panic-probe"
 version = "0.3.1"
@@ -1256,6 +1521,18 @@ dependencies = [
  "syn 1.0.109",
 ]
 
+[[package]]
+name = "polyval"
+version = "0.6.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d52cff9d1d4dee5fe6d03729099f4a310a41179e0a10dbf542039873f2e826fb"
+dependencies = [
+ "cfg-if",
+ "cpufeatures",
+ "opaque-debug",
+ "universal-hash",
+]
+
 [[package]]
 name = "ppv-lite86"
 version = "0.2.17"
@@ -1268,6 +1545,15 @@ version = "0.1.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "925383efa346730478fb4838dbe9137d2a47675ad789c546d150a6e1dd4ab31c"
 
+[[package]]
+name = "primeorder"
+version = "0.13.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3c2fcef82c0ec6eefcc179b978446c399b3cdf73c392c35604e399eee6df1ee3"
+dependencies = [
+ "elliptic-curve",
+]
+
 [[package]]
 name = "proc-macro-error"
 version = "1.0.4"
@@ -1400,6 +1686,25 @@ version = "0.7.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "dbb5fb1acd8a1a18b3dd5be62d25485eb770e05afb408a9627d14d451bae12da"
 
+[[package]]
+name = "reqwless"
+version = "0.5.0"
+dependencies = [
+ "base64",
+ "buffered-io",
+ "defmt",
+ "embedded-io",
+ "embedded-io-async",
+ "embedded-nal-async",
+ "embedded-tls",
+ "heapless 0.7.16",
+ "hex",
+ "httparse",
+ "nourl",
+ "rand_chacha",
+ "rand_core",
+]
+
 [[package]]
 name = "rp"
 version = "0.1.0"
@@ -1414,6 +1719,8 @@ dependencies = [
  "defmt-rtt",
  "dotenvy",
  "dsmr5",
+ "embassy-boot",
+ "embassy-boot-rp",
  "embassy-executor",
  "embassy-futures",
  "embassy-net",
@@ -1422,11 +1729,13 @@ dependencies = [
  "embassy-time",
  "embassy-usb",
  "embedded-io-async",
- "heapless",
+ "embedded-storage",
+ "heapless 0.7.16",
  "log",
  "nourl",
  "panic-probe",
  "rand",
+ "reqwless",
  "rust-mqtt",
  "serde",
  "serde-json-core",
@@ -1460,7 +1769,7 @@ source = "git+https://git.huizinga.dev/Dreaded_X/rust-mqtt#a040fef484227697a7b3b
 dependencies = [
  "defmt",
  "embedded-io-async",
- "heapless",
+ "heapless 0.7.16",
  "rand_core",
 ]
 
@@ -1513,6 +1822,19 @@ version = "1.2.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49"
 
+[[package]]
+name = "sec1"
+version = "0.7.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d3e97a565f76233a6003f9f5c54be1d9c5bdfa3eccfb189469f11ec4901c47dc"
+dependencies = [
+ "base16ct",
+ "der",
+ "generic-array 0.14.7",
+ "subtle",
+ "zeroize",
+]
+
 [[package]]
 name = "semver"
 version = "0.9.0"
@@ -1549,7 +1871,7 @@ version = "0.5.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "3c9e1ab533c0bc414c34920ec7e5f097101d126ed5eac1a1aac711222e0bbb33"
 dependencies = [
- "heapless",
+ "heapless 0.7.16",
  "ryu",
  "serde",
 ]
@@ -1565,6 +1887,17 @@ dependencies = [
  "syn 2.0.29",
 ]
 
+[[package]]
+name = "sha2"
+version = "0.10.7"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "479fb9d862239e610720565ca91403019f2f00410f1864c5aa7479b950a76ed8"
+dependencies = [
+ "cfg-if",
+ "cpufeatures",
+ "digest",
+]
+
 [[package]]
 name = "signature"
 version = "1.6.4"
@@ -1593,7 +1926,7 @@ dependencies = [
  "byteorder",
  "cfg-if",
  "defmt",
- "heapless",
+ "heapless 0.7.16",
  "managed",
 ]
 
@@ -1650,6 +1983,12 @@ version = "0.10.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623"
 
+[[package]]
+name = "subtle"
+version = "2.5.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "81cdd64d312baedb58e21336b31bc043b77e01cc99033ce76ef539f78e965ebc"
+
 [[package]]
 name = "syn"
 version = "1.0.109"
@@ -1745,6 +2084,16 @@ version = "0.2.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "f962df74c8c05a667b5ee8bcf162993134c104e96440b663c8daa176dc772d8c"
 
+[[package]]
+name = "universal-hash"
+version = "0.5.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "fc1de2c688dc15305988b563c3854064043356019f97a4b46276fe734c4f07ea"
+dependencies = [
+ "crypto-common",
+ "subtle",
+]
+
 [[package]]
 name = "usb-device"
 version = "0.2.9"
@@ -1915,3 +2264,9 @@ name = "windows_x86_64_msvc"
 version = "0.48.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "ed94fce61571a4006852b7389a063ab983c02eb1bb37b47f8272ce92d06d9538"
+
+[[package]]
+name = "zeroize"
+version = "1.6.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2a0956f1ba7c7909bfb66c2e9e4124ab6f6482560f6628b5aaeba39207c9aad9"
diff --git a/Cargo.toml b/Cargo.toml
index a1d257c..290ff9c 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -27,6 +27,8 @@ embassy-rp = { version = "0.1", features = [
 	"time-driver",
 	"critical-section-impl",
 ] }
+embassy-boot-rp = { version = "0.1", features = ["nightly", "defmt"] }
+embassy-boot = { version = "0.1", features = ["nightly", "defmt"] }
 embassy-time = { version = "0.1", features = [
 	"defmt",
 	"unstable-traits",
@@ -78,6 +80,8 @@ smoltcp = { version = "0.10.0", default-features = false, features = [
 	"dns-max-server-count-4",
 ] }
 nourl = { version = "0.1.1", features = ["defmt"] }
+reqwless = { version = "0.5.0", features = ["defmt"] }
+embedded-storage = "0.3.0"
 
 [patch.crates-io]
 embassy-executor = { git = "https://github.com/embassy-rs/embassy" }
@@ -87,12 +91,14 @@ embassy-usb = { git = "https://github.com/embassy-rs/embassy" }
 embassy-net = { git = "https://github.com/embassy-rs/embassy" }
 embassy-sync = { git = "https://github.com/embassy-rs/embassy" }
 embassy-futures = { git = "https://github.com/embassy-rs/embassy" }
-
 embassy-boot-rp = { git = "https://github.com/embassy-rs/embassy" }
+embassy-boot = { git = "https://github.com/embassy-rs/embassy" }
 
 rust-mqtt = { git = "https://git.huizinga.dev/Dreaded_X/rust-mqtt" }
 nourl = { git = "https://git.huizinga.dev/Dreaded_X/nourl" }
 
+reqwless = { path = "../reqwless" }
+
 [build-dependencies]
 dotenvy = "0.15"
 
diff --git a/bootloader/Cargo.toml b/bootloader/Cargo.toml
index 80dce99..3332d96 100644
--- a/bootloader/Cargo.toml
+++ b/bootloader/Cargo.toml
@@ -18,7 +18,6 @@ cortex-m = { version = "0.7", features = [
 ] }
 cortex-m-rt = { version = "0.7" }
 embedded-storage = "0.3.0"
-embedded-storage-async = "0.4.0"
 
 [features]
 defmt = ["dep:defmt", "embassy-boot-rp/defmt", "embassy-rp/defmt"]
diff --git a/src/main.rs b/src/main.rs
index 3e6369d..6c73730 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -2,7 +2,11 @@
 #![no_main]
 #![feature(type_alias_impl_trait)]
 
-use heapless::Vec;
+use core::cell::RefCell;
+
+use embassy_boot_rp::{AlignedBuffer, BlockingFirmwareUpdater, FirmwareUpdaterConfig};
+use embedded_storage::nor_flash::NorFlash;
+use heapless::{String, Vec};
 use rand::{
     rngs::{SmallRng, StdRng},
     RngCore, SeedableRng,
@@ -19,13 +23,14 @@ use embassy_net::{dns::DnsQueryType, tcp::TcpSocket, Config, Stack, StackResourc
 use embassy_rp::{
     bind_interrupts,
     clocks::RoscRng,
+    flash::{Flash, WRITE_SIZE},
     gpio::{Level, Output},
     peripherals::{DMA_CH0, PIN_23, PIN_25, PIO0, UART0},
     pio::{self, Pio},
     uart::{self, BufferedUartRx, Parity},
 };
 use embassy_sync::{
-    blocking_mutex::raw::NoopRawMutex,
+    blocking_mutex::{Mutex, raw::NoopRawMutex},
     channel::{Channel, Sender},
 };
 use embassy_time::{Duration, Ticker, Timer};
@@ -33,6 +38,10 @@ use embedded_io_async::Read;
 
 use dsmr5::Readout;
 use nourl::Url;
+use reqwless::{
+    request::{Method, Request, RequestBuilder},
+    response::Response,
+};
 use rust_mqtt::{
     client::{
         client::MqttClient,
@@ -43,7 +52,7 @@ use rust_mqtt::{
 use serde::Deserialize;
 use static_cell::make_static;
 
-use defmt::{debug, error, info, trace, warn, Format};
+use defmt::{debug, error, info, warn, Debug2Format, Format};
 
 use {defmt_rtt as _, panic_probe as _};
 
@@ -57,6 +66,12 @@ struct UpdateMessage<'a> {
     url: &'a str,
 }
 
+impl UpdateMessage<'_> {
+    fn get_url(&self) -> String<1024> {
+        self.url.into()
+    }
+}
+
 #[embassy_executor::task]
 async fn wifi_task(
     runner: cyw43::Runner<
@@ -239,7 +254,7 @@ async fn main(spawner: Spawner) {
     let cfg = wait_for_config(stack).await;
     info!("IP Address: {}", cfg.address.address());
 
-    let mut rx_buffer = [0; 4096];
+    let mut rx_buffer = [0; 1024];
     let mut tx_buffer = [0; 4096];
 
     let mut socket = TcpSocket::new(stack, &mut rx_buffer, &mut tx_buffer);
@@ -270,7 +285,7 @@ async fn main(spawner: Spawner) {
     // Leads to InsufficientBufferSize error
     config.add_will("pico/test", b"disconnected", true);
 
-    let mut recv_buffer = [0; 4096];
+    let mut recv_buffer = [0; 1024];
     let mut write_buffer = [0; 4096];
 
     let mut client =
@@ -280,8 +295,20 @@ async fn main(spawner: Spawner) {
     client.connect_to_broker().await.unwrap();
     info!("MQTT Connected!");
 
+    // TODO: Ideally we use async flash
+    // This has issues with alignment right now
+    let flash = Flash::<_, _, FLASH_SIZE>::new_blocking(p.FLASH);
+    let flash = Mutex::new(RefCell::new(flash));
+
+    let config = FirmwareUpdaterConfig::from_linkerfile_blocking(&flash);
+    let mut aligned = AlignedBuffer([0; WRITE_SIZE]);
+    let mut updater = BlockingFirmwareUpdater::new(config, &mut aligned.0);
+
+    // We wait with marking as booted until everything is connected
+    updater.mark_booted().unwrap();
+
     client
-        .send_message("pico/test", b"connected", QualityOfService::QoS0, true)
+        .send_message("pico/test", b"connected", QualityOfService::QoS1, true)
         .await
         .unwrap();
 
@@ -292,7 +319,6 @@ async fn main(spawner: Spawner) {
 
     let mut keep_alive = Ticker::every(Duration::from_secs(30));
     let receiver = channel.receiver();
-
     loop {
         match select3(
             keep_alive.next(),
@@ -311,7 +337,7 @@ async fn main(spawner: Spawner) {
                     .expect("The buffer should be large enough to contain all the data");
 
                 client
-                    .send_message("pico/test", &msg, QualityOfService::QoS0, false)
+                    .send_message("pico/test", &msg, QualityOfService::QoS1, false)
                     .await
                     .unwrap();
             }
@@ -332,12 +358,9 @@ async fn main(spawner: Spawner) {
                     }
                 };
 
-                trace!("UpdateMessage: {}", message);
-
-                let url = Url::parse(message.url).unwrap();
-                let ip = stack.dns_query(url.host(), DnsQueryType::A).await;
-
-                debug!("Update IP: {}", ip);
+                let url = message.get_url();
+                let url = Url::parse(url.as_str()).unwrap();
+                attempt_update(stack, &mut updater, &mut client, url).await;
             }
         }
     }
@@ -355,3 +378,89 @@ async fn wait_for_config(
         yield_now().await;
     }
 }
+
+const FLASH_SIZE: usize = 2 * 1024 * 1024;
+
+async fn attempt_update<T, const MAX_PROPERTIES: usize, R, F>(
+    stack: &'static Stack<cyw43::NetDriver<'static>>,
+    updater: &mut BlockingFirmwareUpdater<'_, F, F>,
+    client: &mut MqttClient<'_, T, MAX_PROPERTIES, R>,
+    url: Url<'_>,
+)
+where
+    T: embedded_io_async::Write + embedded_io_async::Read,
+    R: rand::RngCore,
+    F: NorFlash,
+{
+    info!("Installing OTA...");
+    let ip = stack.dns_query(url.host(), DnsQueryType::A).await.unwrap()[0];
+
+    let mut rx_buffer = [0; 4096 * 2];
+    let mut tx_buffer = [0; 1024];
+
+    let mut socket = TcpSocket::new(stack, &mut rx_buffer, &mut tx_buffer);
+    let addr = (ip, url.port_or_default());
+    debug!("Addr: {}", addr);
+    socket.connect(addr).await.unwrap();
+
+    debug!("Path: {}", url.path());
+    Request::get(url.path())
+        .build()
+        .write(&mut socket)
+        .await
+        .unwrap();
+
+    let mut headers = [0; 4096];
+    let resp = Response::read(&mut socket, Method::GET, &mut headers)
+        .await
+        .unwrap();
+
+    let mut body = resp.body().reader();
+
+    debug!("Preparing updater...");
+    client
+        .send_message("pico/test", b"preparing", QualityOfService::QoS1, false)
+        .await
+        .unwrap();
+
+    let writer = updater
+        .prepare_update()
+        .map_err(|e| warn!("E: {:?}", Debug2Format(&e)))
+        .unwrap();
+
+    debug!("Updater prepared!");
+    client
+        .send_message("pico/test", b"prepared", QualityOfService::QoS1, false)
+        .await
+        .unwrap();
+
+    let mut buffer = AlignedBuffer([0; 4096]);
+    let mut offset = 0;
+    while let Ok(read) = body.read(&mut buffer.0).await {
+        if read == 0 {
+            break;
+        }
+        debug!("Chunk size: {}", read);
+        writer.write(offset, &buffer.0[..read]).unwrap();
+        offset += read as u32;
+    }
+    client
+        .send_message("pico/test", b"written", QualityOfService::QoS1, false)
+        .await
+        .unwrap();
+    debug!("Total size: {}", offset);
+
+    updater.mark_updated().unwrap();
+
+    client
+        .send_message("pico/test", b"restarting", QualityOfService::QoS1, false)
+        .await
+        .unwrap();
+
+    client.disconnect().await.unwrap();
+
+    info!("Restarting in 5 seconds...");
+    Timer::after(Duration::from_secs(5)).await;
+
+    cortex_m::peripheral::SCB::sys_reset();
+}