From feaf167e854944c7b03f0aca39afc3e7e8dcab6a Mon Sep 17 00:00:00 2001
From: Thomas Klaehn <thomas.klaehn@perinet.io>
Date: Tue, 25 Feb 2025 00:47:44 +0100
Subject: [PATCH] Add mqtt publish

Signed-off-by: Thomas Klaehn <thomas.klaehn@perinet.io>
---
 Cargo.lock          | 10 ++++++
 hp-core/Cargo.toml  |  3 +-
 hp-core/src/main.rs | 83 ++++++++++++++++++++++++++++++++++++++++++++-
 3 files changed, 94 insertions(+), 2 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index 0a52129..2147266 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -1229,6 +1229,7 @@ dependencies = [
  "ieee80211",
  "ieee802154",
  "log",
+ "mqttrust",
  "nb 1.1.0",
  "portable-atomic",
  "sha2",
@@ -1411,6 +1412,15 @@ dependencies = [
  "serde",
 ]
 
+[[package]]
+name = "mqttrust"
+version = "0.6.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ae32f7b780d20b0fc574bba0ca71b33838bb264d7a48b098cd18e3327c30edde"
+dependencies = [
+ "heapless 0.7.17",
+]
+
 [[package]]
 name = "nb"
 version = "0.1.3"
diff --git a/hp-core/Cargo.toml b/hp-core/Cargo.toml
index ac5fc4c..538e02a 100644
--- a/hp-core/Cargo.toml
+++ b/hp-core/Cargo.toml
@@ -23,6 +23,7 @@ embedded-hal-async  = "1.0.0"
 embedded-io         = { version = "0.6.1", default-features = false }
 embedded-io-async   = "0.6.1"
 embedded-storage    = "0.3.1"
+embedded-time       = "=0.12.1"
 esp-alloc           = "0.6.0"
 esp-backtrace       = { version = "0.15.0", features = ["esp32c6", "exception-handler", "panic-handler", "println"] }
 esp-hal             = { version = "0.23.1", features = ["esp32c6", "log"] }
@@ -36,11 +37,11 @@ hmac                = { version = "0.12.1", default-features = false }
 ieee80211           = { version = "0.4.0", default-features = false }
 ieee802154          = "0.6.1"
 log                 = "0.4.22"
+mqttrust            = "0.6.0"
 nb                  = "1.1.0"
 portable-atomic     = { version = "1.9.0",  default-features = false }
 sha2                = { version = "0.10.8", default-features = false }
 smoltcp             = { version = "0.12.0", default-features = false, features = [ "medium-ethernet", "socket-raw"] }
-embedded-time       = "=0.12.1"
 static_cell         = { version = "2.1.0", features = ["nightly"] }
 usb-device          = "0.3.2"
 usbd-serial         = "0.2.2"
diff --git a/hp-core/src/main.rs b/hp-core/src/main.rs
index fee5766..282065c 100644
--- a/hp-core/src/main.rs
+++ b/hp-core/src/main.rs
@@ -3,6 +3,10 @@
 
 extern crate alloc;
 
+use embedded_io::{
+    Read,
+    Write
+};
 use blocking_network_stack::Stack;
 use esp_backtrace as _;
 use esp_hal::{
@@ -45,12 +49,25 @@ use esp_wifi::{
         WifiStaDevice,
     }
 };
+use mqttrust::encoding::v4::{
+    encode_slice,
+    Connect,
+    Packet,
+    Protocol,
+    Publish,
+    QoS
+};
+
 use smoltcp::{
     iface::{
         SocketSet,
         SocketStorage
     },
-    wire::DhcpOption
+    wire::{
+        DhcpOption,
+        IpAddress,
+        Ipv4Address
+    }
 };
 
 const SSID: &str = env!("SSID");
@@ -135,6 +152,63 @@ fn main() -> ! {
         }
     }
 
+    let mut rx_buffer = [0u8; 1536];
+    let mut tx_buffer = [0u8; 1536];
+    let mut socket = stack.get_socket(&mut rx_buffer, &mut tx_buffer);
+    socket
+        .open(IpAddress::Ipv4(Ipv4Address::new(192, 168, 178, 143)), 1883) // p5
+        .unwrap();
+
+
+    sleep_millis(1_000);
+    println!("Trying to connect");
+
+    let connect = Packet::Connect(Connect {
+        protocol: Protocol::MQTT311,
+        keep_alive: 10,
+        client_id: "tmp",
+        clean_session: true,
+        last_will: None,
+        username: None,
+        password: None,
+    });
+    let mut buf = [0u8; 1024];
+    let len = encode_slice(&connect, &mut buf).unwrap();
+
+    let _ = match socket.write(&buf[..len]) {
+        Ok(value) => value,
+        Err(err) => panic!("Error connection mqtt: {:?}",err),
+    };
+
+    println!("Connected to MQTT broker");
+
+    let packet = Publish {
+        dup: false,
+        qos: QoS::AtMostOnce,
+        pid: None,
+        retain: false,
+        topic_name: "topic",
+        payload: b"hello",
+    }.into();
+    let len = encode_slice(&packet, &mut buf).unwrap();
+    let _ = match socket.write(&buf[..len]) {
+        Ok(value) => value,
+        Err(err) => panic!("Error publishing mqtt: {:?}",err),
+    };
+    // read back result (?!?)
+    let len = match socket.read(&mut buf) {
+        Ok(value) => value,
+        Err(err) => panic!("Error reading socket: {:?}", err),
+    };
+    if len > 0 {
+        println!("got {} bytes: {:02x?}", len, &buf[..len]);
+    }
+
+    println!("Disconnecting");
+    socket.disconnect();
+    println!("Disconnected");
+
+
     let lp_pin = LowPowerOutput::new(peripherals.GPIO1);
     let uart_cfg = Config::default();
     let tx = LowPowerOutput::new(peripherals.GPIO5);
@@ -170,3 +244,10 @@ fn main() -> ! {
         // rtc.sleep_light(&[&reason]);
     }
 }
+
+pub fn sleep_millis(delay: u32) {
+    let sleep_end = as_millis() + delay as u64;
+    while as_millis() < sleep_end {
+        // wait
+    }
+}
\ No newline at end of file