update esp toolchain to [rust] 1.69

closes #4
closes #12

.gitignore

@@ -2,3 +2,4 @@
 /.embuild
 /target
 /Cargo.lock
+/secret

Cargo.toml

@@ -17,9 +17,11 @@ anyhow = "1.0.57"
 embedded-hal = "0.2.7"
 esp-idf-hal = "0.37.4"
 esp-idf-sys = { version = "0.31.5", features = ["binstart", "native"] }
-minimq = "0.5.3"
+mqtt-protocol = "0.11.2"
 nb = "1.0.0"
-std-embedded-time = "0.1.0"
+nmea0183 = "0.3.0"
+serde-json-core = "0.5.0"
+serde = "*"
 
 [build-dependencies]
 embuild = "0.29"

rust-toolchain.toml

@@ -1,2 +1,2 @@
 [toolchain]
-channel = "esp"
+channel = "esp-1.69.0.0"

sdkconfig.defaults

@@ -1,10 +1,13 @@
 # Rust often needs a bit of an extra main task stack size compared to C (the default is 3K)
-CONFIG_ESP_MAIN_TASK_STACK_SIZE=17000
+CONFIG_ESP_MAIN_TASK_STACK_SIZE=128000
 
 # Use this to set FreeRTOS kernel tick frequency to 1000 Hz (100 Hz by default).
 # This allows to use 1 ms granuality for thread sleeps (10 ms by default).
 #CONFIG_FREERTOS_HZ=1000
 
+# Explicitly specify UART0 for console debugging.
+CONFIG_CONSOLE_UART_NUM=0
+
 # Workaround for https://github.com/espressif/esp-idf/issues/7631
 CONFIG_MBEDTLS_CERTIFICATE_BUNDLE=n
 CONFIG_MBEDTLS_CERTIFICATE_BUNDLE_DEFAULT_FULL=n

src/accel.rs

@@ -0,0 +1,18 @@
+use anyhow;
+
+use std::sync::mpsc::SyncSender;
+use std::thread;
+use std::time::Duration;
+
+use crate::types::*;
+
+pub fn main(sender: SyncSender<Msg>) -> anyhow::Result<()> {
+    let mut c = 1_usize;
+    println!("entering ACCELERATOR sender loop ...");
+    loop {
+        println!("sending ACCELERATOR message No. {}", c);
+        let _ = sender.send(Msg::Accelerometer("{\"velocity\": 21.43, \"altitude\": 367}".to_string()))?;
+        thread::sleep(Duration::from_secs(5));
+        c += 1;
+    }
+}

src/command.rs

@@ -1,5 +1,6 @@
 use std::time::Duration;
 
+#[derive(Debug)]
 pub struct Command {
     pub text: String,
     pub timeout: Duration,
@@ -11,7 +12,7 @@ impl Command {
         Command {
             text: "ATI".to_string(),
             timeout: Duration::from_millis(6000),
-            contains: Some("+CIEV".to_string()),
+            contains: Some("OK".to_string()),
         }
     }
 
@@ -71,14 +72,6 @@ impl Command {
         }
     }
 
-    pub fn gprs_open() -> Command {
-        Command {
-            text: "AT+SAPBR=1,1".to_string(),
-            timeout: Duration::from_millis(3000),
-            contains: Some("OK".to_string()),
-        }
-    }
-
     pub fn gprs_set_apn(apn: &str) -> Command {
         Command {
             text: format!("AT+SAPBR=3,1,\"APN\",\"{}\"", apn),
@@ -103,7 +96,7 @@ impl Command {
         }
     }
 
-    pub fn getbear() -> Command {
+    pub fn gprs_bearer_status() -> Command {
         Command {
             text: "AT+SAPBR=2,1".to_string(),
             timeout: Duration::from_millis(3000),
@@ -111,6 +104,22 @@ impl Command {
         }
     }
 
+    pub fn gprs_bearer_open() -> Command {
+        Command {
+            text: "AT+SAPBR=1,1".to_string(),
+            timeout: Duration::from_millis(3000),
+            contains: Some("OK".to_string()),
+        }
+    }
+
+    pub fn gprs_bearer_close() -> Command {
+        Command {
+            text: "AT+SAPBR=0,1".to_string(),
+            timeout: Duration::from_millis(3000),
+            contains: Some("OK".to_string()),
+        }
+    }
+
     pub fn get_local_ip_addr() -> Command {
         Command {
             text: "AT+CIFSR".to_string(),
@@ -135,7 +144,7 @@ impl Command {
         }
     }
 
-    pub fn http_set() -> Command {
+    pub fn http_set_cid() -> Command {
         Command {
             text: "AT+HTTPPARA=\"CID\",1".to_string(),
             timeout: Duration::from_millis(3000),
@@ -143,25 +152,25 @@ impl Command {
         }
     }
 
-    pub fn http_enable_ssl() -> Command {
+    pub fn http_set_url(url: &str) -> Command {
         Command {
-            text: "AT+HTTPSSL=1".to_string(),
+            text: format!("AT+HTTPPARA=\"URL\",\"{}\"", url),
             timeout: Duration::from_millis(3000),
             contains: Some("OK".to_string()),
         }
     }
 
-    pub fn http_disable_ssl() -> Command {
+    pub fn http_set_ssl(enabled: bool) -> Command {
         Command {
-            text: "AT+HTTPSSL=0".to_string(),
-            timeout: Duration::from_millis(3000),
+            text: format!("AT+HTTPSSL={}", enabled as u8),
+            timeout: Duration::from_millis(1000),
             contains: Some("OK".to_string()),
         }
     }
 
-    pub fn http_init_url() -> Command {
+    pub fn http_set_header(header: &str, value: &str) -> Command {
         Command {
-            text: "AT+HTTPPARA=\"URL\",\"{}\"".to_string(),
+            text: format!("AT+HTTPPARA=\"USERDATA\",\"{}: {}\"", header, value),
             timeout: Duration::from_millis(3000),
             contains: Some("OK".to_string()),
         }
@@ -175,31 +184,39 @@ impl Command {
         }
     }
 
-    pub fn http_set_content() -> Command {
+    pub fn http_post() -> Command {
         Command {
-            text: "AT+HTTPPARA=\"CONTENT\",\"{}\"".to_string(),
+            text: "AT+HTTPACTION=1".to_string(),
+            timeout: Duration::from_millis(10000),
+            contains: Some("HTTPACTION".to_string()),
+        }
+    }
+
+    pub fn http_set_content(content: &str) -> Command {
+        Command {
+            text: format!("AT+HTTPPARA=\"CONTENT\",\"{}\"", content),
             timeout: Duration::from_millis(3000),
             contains: Some("OK".to_string()),
         }
     }
 
-    pub fn http_post_len() -> Command {
+    pub fn http_set_redirect(redirect: bool) -> Command {
         Command {
-            text: "AT+HTTPDATA={}5000".to_string(),
+            text: format!("AT+HTTPPARA=\"REDIR\",\"{}\"", redirect as u8),
             timeout: Duration::from_millis(3000),
-            contains: Some("DOWNLOAD".to_string()),
+            contains: Some("OK".to_string()),
         }
     }
 
-    pub fn http_post() -> Command {
+    pub fn http_post_len(size: usize, time: usize) -> Command {
         Command {
-            text: "AT+HTTPACTION=1".to_string(),
-            timeout: Duration::from_millis(3000),
-            contains: Some("+HTTPACTION".to_string()),
+            text: format!("AT+HTTPDATA={},{}", size, time),
+            timeout: Duration::from_millis(5000),
+            contains: Some("OK".to_string()),
         }
     }
 
-    pub fn http_get_data() -> Command {
+    pub fn http_read_response() -> Command {
         Command {
             text: "AT+HTTPREAD".to_string(),
             timeout: Duration::from_millis(3000),
@@ -207,7 +224,7 @@ impl Command {
         }
     }
 
-    pub fn closehttp() -> Command {
+    pub fn http_close() -> Command {
         Command {
             text: "AT+HTTPTERM".to_string(),
             timeout: Duration::from_millis(3000),
@@ -215,19 +232,11 @@ impl Command {
         }
     }
 
-    pub fn closebear() -> Command {
-        Command {
-            text: "AT+SAPBR=0,1".to_string(),
-            timeout: Duration::from_millis(3000),
-            contains: Some("OK".to_string()),
-        }
-    }
-
     pub fn probe() -> Command {
         Command {
             text: "AT".to_string(),
             timeout: Duration::from_millis(3000),
-            contains: Some("+CIEV".to_string()),
+            contains: Some("OK".to_string()),
         }
     }
 
@@ -247,14 +256,6 @@ impl Command {
         }
     }
 
-    pub fn tcp_ssl_enable() -> Command {
-        Command {
-            text: "AT+CIPSSL=1".to_string(),
-            timeout: Duration::from_millis(3000),
-            contains: Some("OK".to_string()),
-        }
-    }
-
     pub fn tcp_ssl_check() -> Command {
         Command {
             text: "AT+CIPSSL=?".to_string(),
@@ -267,7 +268,7 @@ impl Command {
         Command {
             text: format!("AT+CIPSTART=\"TCP\",\"{}\",\"{}\"", addr, port),
             timeout: Duration::from_millis(5000),
-            contains: Some("CONNECT OK".to_string()),
+            contains: Some("OK".to_string()),
         }
     }
 
@@ -343,11 +344,91 @@ impl Command {
         }
     }
 
-    pub fn tcp_connection_state() -> Command {
+    pub fn manufacturer_id() -> Command {
         Command {
-            text: "AT+CIPSTATUS".to_string(),
+            text: "AT+GMI".to_string(),
+            timeout: Duration::from_millis(3000),
+            contains: Some("OK".to_string()),
+        }
+    }
+
+    pub fn model_id() -> Command {
+        Command {
+            text: "AT+GMM".to_string(),
+            timeout: Duration::from_millis(3000),
+            contains: Some("OK".to_string()),
+        }
+    }
+
+    pub fn release_id() -> Command {
+        Command {
+            text: "AT+GMR".to_string(),
+            timeout: Duration::from_millis(3000),
+            contains: Some("OK".to_string()),
+        }
+    }
+
+    pub fn get_location() -> Command {
+        Command {
+            text: "AT+CLBS=1,1".to_string(),
+            timeout: Duration::from_millis(10000),
+            contains: Some("OK".to_string()),
+        }
+    }
+
+    pub fn ssl_opt() -> Command {
+        Command {
+            text: "AT+SSLOPT=1,1".to_string(),
+            timeout: Duration::from_millis(3000),
+            contains: Some("OK".to_string()),
+        }
+    }
+
+    pub fn ssl_set_client_cert(path: &str, password: &str) -> Command {
+        Command {
+            text: format!("AT+SSLSETCERT={},{}", path, password),
             timeout: Duration::from_millis(2000),
-            contains: Some("STATE".to_string()),
+            contains: Some("SSLSETCERT".to_string()),
+        }
+    }
+
+    pub fn ssl_set_root_cert(path: &str, size: usize) -> Command {
+        Command {
+            text: format!("AT+SSLSETROOT={},{}", path, size),
+            timeout: Duration::from_millis(2000),
+            contains: Some("SSLSETCERT".to_string()),
+        }
+    }
+
+    pub fn fs_file_create(path: &str) -> Command {
+        Command {
+            text: format!("AT+FSCREATE={}", path),
+            timeout: Duration::from_millis(2000),
+            contains: Some("OK".to_string()),
+        }
+    }
+
+    pub fn fs_file_write(path: &str, append: bool, size: usize, input_time_sec: usize) -> Command {
+        Command {
+            text: format!("AT+FSWRITE={},{},{},{}", path, append as u8, size, input_time_sec),
+            timeout: Duration::from_millis(20000),
+            contains: Some("OK".to_string()),
+        }
+    }
+
+    pub fn fs_list(path: &str) -> Command {
+        Command {
+            text: format!("AT+FSLS={}", path),
+            timeout: Duration::from_millis(2000),
+            contains: Some("OK".to_string()),
+        }
+    }
+
+    pub fn fs_free_size() -> Command {
+        Command {
+            text: "AT+FSMEM".to_string(),
+            timeout: Duration::from_millis(2000),
+            contains: Some("OK".to_string()),
         }
     }
 }

src/config.rs

@@ -1,11 +1,19 @@
+#![allow(dead_code)]
+
 pub struct GprsAp<'a> {
     pub apn: &'a str,
     pub username: &'a str,
     pub password: &'a str,
 }
 
-pub const A1_GPRS_AP: GprsAp = GprsAp {
+pub const A1: GprsAp = GprsAp {
     apn: "internet",
     username: "internet",
     password: "internet",
 };
+
+pub const MTS: GprsAp = GprsAp {
+    apn: "gprswap",
+    username: "mts",
+    password: "064",
+};

src/gps.rs

@@ -0,0 +1,90 @@
+use anyhow;
+
+use std::{
+    sync::mpsc::SyncSender,
+    thread,
+    time::Duration,
+    io::Read,
+};
+
+use esp_idf_hal::prelude::*;
+use esp_idf_hal::serial::{self, Rx, Tx};
+
+use nmea0183::{Parser, ParseResult, Sentence, Source};
+
+use crate::types::Msg;
+use crate::serial::SerialIO;
+
+struct GpsModule<UART: serial::Uart> {
+    port: SerialIO<UART>,
+}
+
+impl<UART: serial::Uart> GpsModule<UART> {
+    pub fn new(tx: Tx<UART>, rx: Rx<UART>) -> Self {
+        GpsModule {
+            port: SerialIO::new(tx, rx),
+        }
+    }
+}
+
+pub fn main<PRx,PTx>
+(
+    tx: PTx,
+    rx: PRx,
+    uart: serial::UART2,
+    sender: SyncSender<Msg>,
+) -> std::result::Result<(), anyhow::Error>
+where
+    PRx: esp_idf_hal::gpio::Pin + esp_idf_hal::gpio::InputPin + esp_idf_hal::gpio::OutputPin,
+    PTx: esp_idf_hal::gpio::Pin + esp_idf_hal::gpio::InputPin + esp_idf_hal::gpio::OutputPin,
+{
+    let serial_pins = serial::Pins {
+        tx,
+        rx,
+        cts: None,
+        rts: None,
+    };
+
+    let serial: serial::Serial<serial::UART2, _, _> = serial::Serial::new(
+        uart,
+        serial_pins,
+        serial::config::Config::default().baudrate(Hertz(9600)),
+    )?;
+
+    let (tx, rx) = serial.split();
+    let mut device = GpsModule::new(tx, rx);
+
+    let mut parser = Parser::new()
+        .source_only(Source::GPS)
+        .sentence_filter(Sentence::GLL | Sentence::GGA);
+
+    let mut c = 0;
+    let mut nmea = [0_u8; 1024];
+    loop {
+        if let Ok(_) = device.port.read(nmea.as_mut_slice()) {
+            println!("\r\n\r\n\r\n\r\n");
+            for result in parser.parse_from_bytes(&nmea[..]) {
+                match result {
+                    Ok(ParseResult::GLL(Some(gll))) => {
+                        sender.send(Msg::Gps(gll.into()))?;
+                    },
+                    Ok(ParseResult::GGA(Some(gga))) => {
+                        sender.send(Msg::Gps(gga.into()))?;
+                    }
+                    _ => { }
+                }
+            }
+            c = 0;
+        } else {
+            println!("nothing to read after {} tries ...", c);
+            if c > 100 {
+                println!("reached {} retries ... bailing!", c);
+                break;
+            }
+        }
+        thread::sleep(Duration::from_millis(5000));
+        c += 1;
+    }
+
+    Ok(())
+}

src/main.rs

@@ -1,91 +1,89 @@
+mod accel;
 mod config;
-mod modem;
 #[allow(dead_code)]
 mod command;
+mod modem;
+mod serial;
+mod types;
+mod gps;
 
 use anyhow;
-use esp_idf_hal::prelude::*;
+use std::{thread::{self, JoinHandle}, time::Duration};
 use esp_idf_hal::peripherals::Peripherals;
-use esp_idf_hal::serial;
+use esp_idf_hal::prelude::*;
+use esp_idf_hal::serial::{Pins, config::Config, Serial, UART1, Uart};
+use embedded_hal::digital::v2::OutputPin;
 
-use minimq::{Minimq, QoS, Retain};
+use types::*;
 
 fn main() -> anyhow::Result<()> {
     esp_idf_sys::link_patches();
 
     let dp = Peripherals::take().expect("error taking peripherals");
 
-    // LilyGo TTGO T-Call sim800l board serial pins.
-    let serial_rx = dp.pins.gpio26;
-    let serial_tx = dp.pins.gpio27;
+    println!("Rust main thread: {:?}", thread::current());
 
-    let serial_pins = serial::Pins {
-        tx: serial_tx,
-        rx: serial_rx,
+    let mut threads: Vec<JoinHandle<anyhow::Result<_>>> = vec![];
+
+    // // Rx/Tx pins for the GPS modem
+    let gps_rx = dp.pins.gpio32;
+    let gps_tx = dp.pins.gpio33;
+    // // UART interface for the GPS modem
+    let gps_uart = dp.uart2;
+
+
+    let (gps_sender, receiver) = std::sync::mpsc::sync_channel::<Msg>(3);
+    //let accel_sender = gps_sender.clone();
+
+    //let _ = gps::main(gps_tx, gps_rx, gps_uart, gps_sender)?;
+    //threads.push(thread::spawn(move || accel::main(accel_sender)));
+
+    // ==================================
+    // MODEM INITIALIZATION AND MAIN LOOP
+    // ==================================
+
+    // LilyGo TTGO T-Call sim800l board serial pins.
+    let modem_rx = dp.pins.gpio26;
+    let modem_tx = dp.pins.gpio27;
+    // LilyGo TTGO T-Call sim800l board power / reset pins.
+    let modem_pwrkey = dp.pins.gpio4.into_output()?;
+    let modem_rst = dp.pins.gpio5.into_output()?;
+    let modem_power = dp.pins.gpio23.into_output()?;
+    // UART interface for the GSM modem
+    let modem_uart = dp.uart1;
+
+    let serial_pins = Pins {
+        tx: modem_tx,
+        rx: modem_rx,
         cts: None,
         rts: None,
     };
 
-    // Create the serial and panic with a message ... if we can't create the serial port, then we
-    // can't communicate with the sim800l module, hence we don't run anymore.
-    let serial: serial::Serial<serial::UART1, _, _> = serial::Serial::new(
-        dp.uart1,
+    let serial: Serial<UART1, _, _> = Serial::new(
+        modem_uart,
         serial_pins,
-        serial::config::Config::default().baudrate(Hertz(115200)),
+        Config::default().baudrate(Hertz(115200)),
     )?;
 
     let (tx, rx) = serial.split();
-    let mut mdm = modem::Modem::new(tx, rx);
+    type PwrkeyOutput = esp_idf_hal::gpio::Gpio4<esp_idf_hal::gpio::Output>;
+    type ResetOutput = esp_idf_hal::gpio::Gpio5<esp_idf_hal::gpio::Output>;
+    type PowerOutput = esp_idf_hal::gpio::Gpio23<esp_idf_hal::gpio::Output>;
 
-    let modem_pwrkey = dp.pins.gpio4.into_output()?;
-    let modem_rst = dp.pins.gpio5.into_output()?;
-    let modem_power = dp.pins.gpio23.into_output()?;
+    let mut mdm: modem::Modem<UART1, PwrkeyOutput, ResetOutput, PowerOutput> = modem::Modem::new(tx, rx, modem_pwrkey, modem_rst, modem_power, receiver);
 
-    mdm.init(modem_pwrkey, modem_rst, modem_power)?;
+    let mqtt_username = include_str!("../secret/username").trim();
+    let mqtt_password = include_str!("../secret/password").trim();
 
-    if !mdm.is_gprs_attached()? {
-        let _ = mdm.connect_to_gprs_ap(
-            config::A1_GPRS_AP.apn,
-            config::A1_GPRS_AP.username,
-            config::A1_GPRS_AP.password,
-        )?;
-    }
+    threads.push(thread::spawn(move || gps::main(gps_tx, gps_rx, gps_uart, gps_sender.clone())));
 
-    if mdm.is_gprs_attached()? {
-        let _ = mdm.get_ip_addr()?;
-
-        //println!("connecting to server!");
-        //if !mdm.tcp_is_ssl_enabled()? {
-        //    let _ = mdm.tcp_ssl_enable()?;
-        //}
-        if mdm.tcp_is_ssl_enabled()? {
-            let _ = mdm.tcp_ssl_disable()?;
-        }
-        let _ = mdm.tcp_set_quick_mode(false);
-        let _ = mdm.tcp_set_manual_receive(true)?;
-
-        let mut mqtt: Minimq<_, _, 256, 16> = Minimq::new(
-            "51.158.66.64".parse().unwrap(),
-            "e-bike-tracker",
-            mdm,
-            std_embedded_time::StandardClock::default()).unwrap();
-
-        mqtt.client
-            .set_will(
-                "exit",
-                "Test complete".as_bytes(),
-                QoS::AtMostOnce,
-                Retain::NotRetained,
-                &[],
-            )
-            .unwrap();
-
-        println!("created mqtt client ... ");
-        let message = "{\"lat\": 20, \"long\": 44}";
-        println!("message = {}", message);
-        mqtt.client.publish("devices/location", message.as_bytes(), QoS::AtMostOnce, Retain::NotRetained, &[]).unwrap();
-        println!("published message ... ");
-    }
-
-    Ok(())
+    println!("======================= MAIN =======================");
+    mdm.init().unwrap_or(());
+    let _ = mdm.echo(false).unwrap_or(());
+    println!("resetting modem ... ");
+    println!("======================= MODEM =======================");
+    let _ = mdm.mqtt_send_position_loop("51.158.66.64", 7887, mqtt_username, mqtt_password).unwrap_or(());
+    let _ = mdm.tcp_close_connection().unwrap_or(());
+    thread::sleep(Duration::from_millis(1500));
+    panic!("rebooting");
 }

src/modem.rs

@@ -1,29 +1,43 @@
+#![allow(dead_code)]
+
 use crate::command::Command;
+use crate::serial::SerialIO;
+use crate::types::*;
 
-use std::thread;
-use std::error::Error;
-use std::time::{Duration, Instant};
+use anyhow;
+use std::{
+    error::Error,
+    io::{Read, Write},
+    thread,
+    time::Duration,
+    sync::mpsc::Receiver,
+};
 
-use embedded_hal::serial::{Read, Write};
-use embedded_hal::digital::v2::OutputPin;
 use esp_idf_hal::serial::{self, Rx, Tx};
-use minimq::embedded_nal::{TcpClientStack, SocketAddr};
 
-const MAX_TCP_MANUAL_REPLY_SIZE: usize = 300;
+use embedded_hal::digital::v2::OutputPin;
+
+use mqtt::{
+    Encodable,
+    Decodable,
+    TopicName,
+    packet::{
+        ConnectPacket,
+        PublishPacket,
+        QoSWithPacketIdentifier,
+        VariablePacket,
+    },
+};
+use serde_json_core;
 
 pub type Result<T> = std::result::Result<T, ModemError>;
 
-pub struct Modem<UART: serial::Uart> {
-    rx: RxIter<UART>,
-    tx: Tx<UART>,
-}
-
 #[derive(Debug)]
 pub enum ModemError {
     CommandError(String),
     SetupError(String),
-    SendDataError,
-    ReadError,
+    SendDataError(String),
+    ReadError(String),
     TimeoutError,
 }
 
@@ -35,74 +49,22 @@ impl std::fmt::Display for ModemError {
     }
 }
 
-pub struct RxIter<UART: serial::Uart> {
-    inner: Rx<UART>,
-    timeout: Duration,
+pub struct Modem<UART: serial::Uart, PWK: OutputPin, RST: OutputPin, PW: OutputPin> {
+    serial: SerialIO<UART>,
+    reset: RST,
+    power: PW,
+    power_key: PWK,
+    receiver: Receiver<Msg>,
 }
 
-impl<UART: serial::Uart> RxIter<UART> {
-    fn reset(&mut self, timeout: Duration) -> &mut Self {
-        self.timeout = timeout;
-        self
-    }
-
-    fn clear(&mut self) -> () {
-        println!("clearing serial rx");
-        self.reset(Duration::from_millis(500)).for_each(drop);
-    }
-
-    /// Reads a whole line (that ends with \\n) within the given `timeout` passed on input.
-    fn read_line(&mut self, timeout: Duration) -> Result<String> {
-        let mut line: String = self.reset(timeout)
-            .map(|b| char::from(b))
-            .take_while(|c| *c != '\n')
-            .collect();
-
-        // \r must come right before \n on read; take_while excludes the matched element.
-        if line.ends_with('\r') {
-            line.push('\n');
-            Ok(line)
-        }
-        else if self.timeout.as_millis() == 0 {
-            Err(ModemError::TimeoutError)
-        }
-        else {
-            Err(ModemError::ReadError)
-        }
-    }
-}
-
-impl<UART: serial::Uart> Iterator for RxIter<UART> {
-    type Item = u8;
-
-    /// `nb` returns Ok(byte), or one of Err(WouldBlock) and Err(Other) which isn't of anyone's
-    /// interest, so the retry mechanism is triggered on _any_ error every 200ms until a byte is
-    /// received, or the timeout is reached.
-    fn next(&mut self) -> Option<Self::Item> {
-        let start = Instant::now();
-        loop {
-            match self.inner.read() {
-                Ok(b) => {
-                    self.timeout = self.timeout.saturating_sub(start.elapsed());
-                    break Some(b)
-                },
-                Err(_) => {
-                    if start.elapsed() > self.timeout {
-                        self.timeout = Duration::ZERO;
-                        break None
-                    }
-                    thread::sleep(Duration::from_millis(200));
-                }
-            }
-        }
-    }
-}
-
-impl<UART: serial::Uart> Modem<UART> {
-    pub fn new(tx: Tx<UART>, rx: Rx<UART>) -> Self {
+impl<UART: serial::Uart, PWK: OutputPin, RST: OutputPin, PW: OutputPin> Modem<UART, PWK, RST, PW> {
+    pub fn new(tx: Tx<UART>, rx: Rx<UART>, mut pwrkey: PWK, mut rst: RST, mut power: PW, receiver: Receiver<Msg>) -> Self {
         Self {
-            rx: RxIter { inner: rx, timeout: Duration::from_millis(0) },
-            tx,
+            serial: SerialIO::new(tx, rx),
+            reset: rst,
+            power,
+            power_key: pwrkey,
+            receiver,
         }
     }
 
@@ -121,141 +83,241 @@ impl<UART: serial::Uart> Modem<UART> {
     ///
     /// modem::init(modem_pwrkey, modem_rst, modem_power);
     /// ```
-    pub fn init(&mut self, mut pwrkey: impl OutputPin, mut rst: impl OutputPin, mut power: impl OutputPin) -> Result<()> {
+    pub fn init(&mut self) -> Result<()> {
         println!("Turning SIM800L on ...");
-        power.set_high().map_err(|_| ModemError::SetupError("Error setting POWER to high.".to_string()))?;
-        rst.set_high().map_err(|_| ModemError::SetupError("Error setting RST to high.".to_string()))?;
+        self.power.set_high().map_err(|_| ModemError::SetupError("Error setting POWER to high.".to_string()))?;
+        self.reset.set_high().map_err(|_| ModemError::SetupError("Error setting RST to high.".to_string()))?;
         // Pull down PWRKEY for more than 1 second according to manual requirements
-        pwrkey.set_high().map_err(|_| ModemError::SetupError("Error setting PWRKEY to high.".to_string()))?;
-        thread::sleep(Duration::from_millis(100));
-        pwrkey.set_low().map_err(|_| ModemError::SetupError("Error setting PWRKEY to low.".to_string()))?;
+        self.power_key.set_high().map_err(|_| ModemError::SetupError("Error setting PWRKEY to high.".to_string()))?;
+        thread::sleep(Duration::from_millis(1500));
+        self.power_key.set_low().map_err(|_| ModemError::SetupError("Error setting PWRKEY to low.".to_string()))?;
         thread::sleep(Duration::from_millis(1000));
-        pwrkey.set_high().map_err(|_| ModemError::SetupError("Error setting PWRKEY to high.".to_string()))?;
+        self.power_key.set_high().map_err(|_| ModemError::SetupError("Error setting PWRKEY to high.".to_string()))?;
         println!("Waiting for sim module to come online ...");
-        loop {
-            match self.send_command(Command::probe()) {
-                Ok(_) => break,
-                _ => continue,
-            }
+        thread::sleep(Duration::from_millis(3000));
+        for _ in 0..10 {
+            let _ = self.send_command(Command::probe()).unwrap_or("".to_string());
+            thread::sleep(Duration::from_millis(1000));
         }
+        self.serial.clear();
         Ok(())
     }
 
-    /// Reads the serial RX until a \\n char is encoutered, or a timeout is reached. The timeout is
-    /// provided on input via the `timeout` argument. The first argument `contains` is checked
-    /// against a line in the response, if it's there the reading stops.
-    ///
-    /// If `contains` is `None`, the first line only is returned in the response. If it's
-    /// `Some(match_txt)`, then the end of the response is matched against `match_txt`.
-    fn read_response(&mut self, contains: Option<String>, timeout: Duration) -> Result<String> {
+    pub fn echo(&mut self, enabled: bool) -> Result<()> {
+        let cmd = format!("ATE{}", if enabled { 1 } else { 0 });
+        self.send(&cmd, "OK").map(|_| ())
+    }
+
+    /// Reads the serial RX until the `contains` string is encoutered if `contains` is Some(s), if
+    /// None, then the first line is returned. If a timeout is reached. The timeout is provided on
+    /// input via the `timeout` argument. The first argument `contains` is checked against every
+    /// line in the response.
+    fn command_read_response(&mut self, contains: Option<String>) -> Result<String> {
         let mut response = String::new();
-        let start = Instant::now();
-        let match_text: String = contains.unwrap_or("\n".to_string());
 
         loop {
-            let timeout = timeout.saturating_sub(start.elapsed());
-            let line = self.rx.read_line(timeout)?;
-            print!("Read {} bytes from serial: {}", line.len(), line);
-            response.push_str(&line);
-            if line.contains("ERROR") || line.contains(&match_text) {
-                println!("Found match {} for line {} ; exiting response reader now ...", match_text, line);
-                println!("-----------------------------------------------------------");
-                break Ok(response.to_string())
+            let mut buf = vec![0; 1024];
+            let num_bytes = self.serial
+                .read(buf.as_mut_slice())
+                .map_err(|err| ModemError::ReadError(format!("Error in serial.read(buf) ({:?})", err)))?;
+
+            response.push_str(std::str::from_utf8(&buf[0..num_bytes])
+                .map_err(|err| ModemError::ReadError(format!("Error in str::from_utf8 ({:?})", err)))?);
+
+            if num_bytes < buf.len() {
+                break
             }
         }
-    }
 
-    #[inline(always)]
-    fn send_bytes(&mut self, payload: &[u8], eos: char) -> Result<()> {
-        for b in payload.iter() {
-            nb::block!(self.tx.write(*b))
-                .map_err(|_| ModemError::CommandError(format!("error writing {} to serial", b)))?;
+        print!("Read {} bytes from serial: {}", response.len(), response);
+        if let Some(c) = contains {
+            if response.contains(&c) {
+                Ok(response)
+            } else {
+                Err(ModemError::CommandError(format!("Didn't get expected ({}) from modem. Got: {}", c, response)))
+            }
+        } else {
+            Ok(response)
         }
-        nb::block!(self.tx.write(eos as u8))
-            .map_err(|_| ModemError::CommandError(format!("error writing {} to serial", eos)))?;
-        Ok(())
     }
 
     fn send_command(&mut self, cmd: Command) -> Result<String> {
-        println!("-----------------------------------------------------------");
-        println!("Sending {} ...", cmd.text);
-        let _ = self.send_bytes(cmd.text.as_bytes(), '\r')?;
-        self.read_response(cmd.contains, cmd.timeout)
-    }
-
-    fn send_data(&mut self, buf: &[u8]) -> Result<usize> {
-        self.rx.clear();
-        let _ = self.send_bytes("AT+CIPSEND".as_bytes(), '\r')?;
-        let send_request: String = self.rx.reset(Duration::from_millis(3000))
-            .map(char::from)
-            .take_while(|c| *c != '>').collect();
-
-        if send_request == "" {
-            return Err(ModemError::SendDataError);
+        if let Some(contains) = cmd.contains {
+            self.send(&cmd.text, &contains)
+        } else {
+            self.send(&cmd.text, "")
         }
-
-        self.send_bytes(buf, 26 as char)?; // 26_u8 = Ctrl+z - to end sending data
-        let _ = self.read_response(Some("SEND OK".to_string()), Duration::from_millis(1000))?;
-        Ok(buf.len())
     }
 
-    pub fn get_ip_addr(&mut self) -> Result<String> {
-        self.send_command(Command::getbear())
+    fn send(&mut self, at_command: &str, contains: &str) -> Result<String> {
+        self.serial.clear();
+        println!("-----------------------------------------------------------");
+        println!("Sending {} ...", at_command);
+
+        let _ = nb::block!(self.serial
+            .write_bytes(&[at_command.as_bytes(), &['\r' as u8]].concat()))
+            .map_err(|_| ModemError::SendDataError(format!("Error in send_command({})", at_command)))?;
+
+        let contains_opt = if contains == "" { None } else { Some(contains.to_string()) };
+
+        self.command_read_response(contains_opt)
     }
 
-    pub fn connect_to_gprs_ap(&mut self, apn: &str, username: &str, password: &str)-> Result<()> {
+    fn handle_prompt(&mut self) -> Result<()> {
+        let mut prompt_buf = vec![0; 256];
+        let prompt_len = self.serial.read(&mut prompt_buf)
+            .map_err(|err| ModemError::ReadError(format!("Error in handle_prompt() ({:?})", err)))?;
+
+        let prompt = String::from_utf8(prompt_buf[0..prompt_len].to_vec())
+            .unwrap_or("".to_string())
+            .trim()
+            .to_string();
+
+        println!("Prompt is: ({})", prompt);
+
+        if prompt != ">" {
+            let msg = format!("Prompt error, expected (>), got ({})", prompt);
+            Err(ModemError::SendDataError(msg))
+        } else {
+            Ok(())
+        }
+    }
+
+    fn tcp_manual_send_data(&mut self, buf: &[u8]) -> Result<String> {
+        println!("Sending AT+CIPSEND to serial TX!");
+        let _ = self.serial
+            .write("AT+CIPSEND\r".as_bytes())
+            .map_err(|_| ModemError::SendDataError("Error in tcp_manual_send_data ... AT_CIPSEND\\r".to_string()))?;
+
+        let _ = self.handle_prompt()?;
+        println!("Handled prompt OK!!");
+
+        println!("Writing bytes in serial TX! ({:?})", buf.into_iter().map(|b| char::from(*b)).collect::<String>());
+        self.serial
+            .write_bytes(buf)
+            .map_err(|err| ModemError::SendDataError(format!("{:?}", err)))?;
+        self.serial
+            .write(&[26_u8])  // 26_u8 = Ctrl+z - to end sending data
+            .map_err(|err| ModemError::SendDataError(format!("{:?}", err)))?;
+        println!("DONE Writing bytes in serial TX!");
+
+        thread::sleep(Duration::from_millis(500));
+
+        println!("Reading bytes in serial RX!");
+        for _ in 0..3 {
+            let res = self.command_read_response(Some("SEND OK".into()));
+            if res.is_ok() {
+                return res;
+            }
+            thread::sleep(Duration::from_millis(1000))
+        }
+        Err(ModemError::ReadError(format!("ReadError: cannot read serial RX!")))
+    }
+
+    pub fn gprs_status(&mut self) -> Result<String> {
+        self.send_command(Command::gprs_bearer_status())
+    }
+
+    pub fn gprs_attach_ap(&mut self, config: crate::config::GprsAp)-> Result<()> {
         println!("init gprs ...");
         let _ = self.send_command(Command::gprs_init())?;
 
-        println!("setting up gprs credentials for apn {}, {}:{})", apn, username, password);
+        println!("setting up gprs credentials for apn {}, {}:{})", config.apn, config.username, config.password);
 
-        let _ = self.send_command(Command::gprs_set_apn(apn))?;
-        let _ = self.send_command(Command::gprs_set_user(username))?;
-        let _ = self.send_command(Command::gprs_set_pwd(password))?;
-
-        println!("open gprs ...");
-        let _ = self.send_command(Command::gprs_open())?;
+        let _ = self.send_command(Command::gprs_set_apn(config.apn))?;
+        let _ = self.send_command(Command::gprs_set_user(config.username))?;
+        let _ = self.send_command(Command::gprs_set_pwd(config.password))?;
 
         Ok(())
     }
 
+    pub fn gprs_connect(&mut self)-> Result<()> {
+        println!("open gprs ...");
+        self.send_command(Command::gprs_bearer_open())
+            .map(|_| ())
+    }
+
     pub fn is_gprs_attached(&mut self)-> Result<bool> {
         let res = self.send_command(Command::is_gprs_attached())?;
         Ok(res.contains("+CGATT: 1"))
     }
 
+    fn try_connect_gprs(&mut self) -> Result<()> {
+        let mut retries = 0;
+        println!("TRYING TO CONNECT TO GPRS");
+        loop {
+            if self.is_gprs_attached()? {
+                let _ = self.gprs_connect()?;
+                thread::sleep(Duration::from_millis(1000));
+                let ip_addr = self.gprs_status()?;
+                if ip_addr.contains("0.0.0.0") && retries < 5 {
+                    thread::sleep(Duration::from_millis(1000));
+                    retries += 1;
+                    continue
+                } else if retries < 5 {
+                    break Ok(())
+                } else {
+                    break Err(ModemError::SetupError(format!("Cannot connect to GPRS after {} retries ... bailing!", retries)));
+                }
+            }
+        }
+    }
+
     pub fn tcp_is_ssl_enabled(&mut self) -> Result<bool> {
         let res = self.send_command(Command::tcp_ssl_check())?;
         Ok(res.contains("+CIPSSL: (1)"))
     }
 
     pub fn tcp_ssl_disable(&mut self) -> Result<()> {
-        let _ = self.send_command(Command::tcp_ssl_set(false))?;
-        Ok(())
+        self.send_command(Command::tcp_ssl_set(false))
+            .map(|_| ())
     }
 
     pub fn tcp_ssl_enable(&mut self) -> Result<()> {
-        let _ = self.send_command(Command::tcp_ssl_set(true))?;
-        Ok(())
+        self.send_command(Command::tcp_ssl_set(true))
+            .map(|_| ())
     }
 
     pub fn tcp_connect(&mut self, addr: &str, port: u16) -> Result<()> {
-        self.send_command(Command::tcp_connect(addr, port))?;
+        let at_command = format!("AT+CIPSTART=\"TCP\",\"{}\",\"{}\"", addr, port);
+        let mut reply_result = self.send(&at_command, "CONNECT OK");
+        for _ in 0..3 {
+            if let Ok(reply) = reply_result {
+                println!("TCP connect replied with {}", reply);
+                break
+            } else {
+                reply_result = self.command_read_response(Some("CONNECT OK".to_string()));
+            }
+            thread::sleep(Duration::from_millis(1000));
+        }
+        Ok(())
+    }
+
+    pub fn tls_connect(&mut self, addr: &str, port: u16) -> Result<()> {
+        let _ = self.tcp_connect(addr, port)?;
+
+        // ------------------------
+        // TLS handshake goes here.
+        // ------------------------
+
         Ok(())
     }
 
     pub fn tcp_set_quick_mode(&mut self, mode: bool) -> Result<()> {
-        self.send_command(Command::tcp_set_quick_mode(mode))?;
-        Ok(())
+        self.send_command(Command::tcp_set_quick_mode(mode))
+            .map(|_| ())
     }
 
     pub fn tcp_set_manual_receive(&mut self, is_manual: bool) -> Result<()> {
-        self.send_command(Command::tcp_set_manual_receive(is_manual))?;
-        Ok(())
+        self.send_command(Command::tcp_set_manual_receive(is_manual))
+            .map(|_| ())
     }
 
-    pub fn tcp_send(&mut self, buf: &[u8]) -> Result<usize> {
-        self.send_data(buf)
+    pub fn tcp_manual_send(&mut self, buf: &[u8]) -> Result<()> {
+        thread::sleep(Duration::from_millis(200));
+        // self.serial.clear();
+        self.tcp_manual_send_data(buf)
+            .map(|_| ())
     }
 
     fn tcp_parse_response_size(&mut self, reply_line: &str) -> Result<usize> {
@@ -269,117 +331,249 @@ impl<UART: serial::Uart> Modem<UART> {
 
     pub fn tcp_receive_reply_len(&mut self) -> Result<usize> {
         let reply = self.send_command(Command::tcp_receive_reply_len())?;
-        reply.lines()
+        println!("Receiving TCP reply length!");
+        let res = reply.lines()
             .filter(|line| line.contains("+CIPRXGET: 4"))
             .next()
-            .ok_or(ModemError::CommandError("reply not found :/".to_string()))
-            .map(|line| self.tcp_parse_response_size(line))
-            .unwrap_or(Err(ModemError::CommandError(format!("received 0 elements from parsing"))))
+            .ok_or(ModemError::CommandError("reply body missing :/".to_string()))
+            .and_then(|line| self.tcp_parse_response_size(line))
+            .map_err(|_| ModemError::CommandError(format!("received 0 elements from parsing")));
+        println!("Received ({:?})", res);
+        res
     }
 
     pub fn tcp_receive(&mut self, buf: &mut [u8]) -> Result<usize> {
         let mut size = 0;
         loop {
-            let reply_len: usize = self.send_command(Command::tcp_receive(MAX_TCP_MANUAL_REPLY_SIZE))
-                .map(|reply: String| {
-                    reply.lines()
-                        .fold(0, |acc, line| {
-                            if !line.starts_with("\r") && !line.contains("+CIPRXGET: 2,") && !line.contains("OK") {
-                                acc += line.chars().enumerate().map(|(idx, c)| { buf[size + idx] = c as u8; }).count()
-                            }
-                            else {
-                                0
-                            }
-                        })
+            let reply = self.send_command(Command::tcp_receive(buf.len()))
+                .map(|reply| {
+                    // TODO: parse the response properly
+                    // 1. the first line is \r\n
+                    // 2. next is the +CIPRXGET: 2,X,Y where X is the number of bytes read and Y is
+                    //    the number of bytes left to be read
+                    // 3. immediately after this the payload is returned (with size X)
+                    // 4. OK
+                    reply
+                        .split("\r\n")
+                        .filter(|line| line.len() > 2 && !line.contains("+CIPRXGET: 2,"))
+                        .next()
+                        .map(|line| line.chars().enumerate().map(|(idx, c)| buf[size + idx] = c as u8).count())
                 })?;
-            if reply_len == 0 {
-                break Ok(size)
-            }
-            else {
-                size += reply_len;
-                continue
+            match reply {
+                Some(0) | None => {
+                    break Ok(size)
+                },
+                Some(x) => {
+                    size += x;
+                    continue
+                },
             }
         }
     }
 
     pub fn tcp_close_connection(&mut self) -> Result<()> {
-        self.send_command(Command::tcp_close())?;
+        self.send_command(Command::tcp_close()).map(|_| ())
+    }
+
+    pub fn http_post(&mut self, url: &str, token: &str, content: &[u8]) -> Result<String> {
+        let _ = self.send_command(Command::http_init());
+        let _ = self.send_command(Command::http_set_cid());
+        let _ = self.send_command(Command::http_set_url(url));
+        let _ = self.send_command(Command::http_set_header("X-Secret", token));
+        let _ = self.send_command(Command::http_set_header("X-Topic", "device-dev"));
+        let _ = self.send_command(Command::http_set_content("application/json"));
+        let _ = self.send_command(Command::http_set_ssl(true));
+        let _ = self.send_command(Command::http_post_len(content.len(), 100000));
+        let _ = self.serial.write_bytes(content);
+        let _ = self.serial.write(&[26_u8]);
+        let _ = self.send_command(Command::http_post());
+        self.send_command(Command::http_read_response())
+    }
+
+    pub fn http_get(&mut self, url: &str) -> Result<String> {
+        let _ = self.send_command(Command::http_init());
+        let _ = self.send_command(Command::http_set_cid());
+        let _ = self.send_command(Command::http_set_url(url));
+        let _ = self.send_command(Command::http_set_redirect(true));
+        let _ = self.send_command(Command::http_set_ssl(true));
+        let _ = self.send_command(Command::http_get());
+        self.send_command(Command::http_read_response())
+    }
+
+    pub fn http_close(&mut self) -> Result<()> {
+        self.send_command(Command::http_close())
+            .map(|_| ())
+    }
+
+    pub fn chip_info(&mut self) -> Result<()> {
+        let _ = self.send_command(Command::manufacturer_id())?;
+        thread::sleep(Duration::from_millis(1000));
+        let _ = self.send_command(Command::model_id())?;
+        thread::sleep(Duration::from_millis(1000));
+        let _ = self.send_command(Command::release_id())?;
         Ok(())
     }
 
-    pub fn tcp_is_connected(&mut self) -> Result<bool> {
-        let response = self.send_command(Command::tcp_connection_state())?;
-        let state = response.lines().last().and_then(|line| line.split(",").last());
-        Ok(state.unwrap_or("CLOSED") == "CONNECTED")
-    }
-}
-
-pub struct ModemTcpStack;
-
-pub struct ModemSocket<T> {
-    state: SocketState<T>,
-}
-
-pub enum SocketState<UART: serial::Uart> {
-    Building,
-    Connected(Modem<UART>),
-}
-
-impl<T: serial::Uart> SocketState<T> {
-    fn new() -> Self {
-        Self::Building
+    pub fn location(&mut self) -> Result<()> {
+        self.send_command(Command::get_location())
+            .map(|_| ())
     }
 
-    fn get_running(&mut self) -> std::io::Result<&mut T> {
-        match self {
-            SocketState::Connected(ref mut s) => Ok(s),
-            _ => OutOfOrder.into(),
+    pub fn ssl_opt(&mut self) -> Result<()> {
+        self.send_command(Command::ssl_opt())
+            .map(|_| ())
+    }
+
+    fn file_write(&mut self, buf: &[u8], path: &str, append: bool, input_time_sec: usize) -> Result<()> {
+        let cmd = Command::fs_file_write(path, append, buf.len(), input_time_sec);
+        let _ = self.serial
+            .write(cmd.text.as_bytes())
+            .map_err(|err| ModemError::SendDataError(format!("File write error ({:?})", err)))?;
+
+        let _ = self.handle_prompt()?;
+
+        self.serial
+            .write(buf)
+            .map_err(|err| ModemError::SendDataError(format!("Error sending bytes via serial ({:?})", err)))?;
+        let _ = self.command_read_response(None);
+
+        Ok(())
+    }
+
+    pub fn upload_cert(&mut self, path: &str, cert: &[u8]) -> Result<()> {
+        let _ = self.send_command(Command::fs_file_create(path))?;
+        let _ = self.file_write(cert, path, false, 20000)?;
+        Ok(())
+    }
+
+    pub fn fs_list(&mut self, path: &str) -> Result<()> {
+        self.send_command(Command::fs_list(path))
+            .map(|_| ())
+    }
+
+    pub fn fs_free_space(&mut self) -> Result<()> {
+        self.send_command(Command::fs_free_size())
+            .map(|_| ())
+    }
+
+    pub fn ssl_set_client_cert(&mut self, path: &str, password: &str) -> Result<()> {
+        self.send_command(Command::ssl_set_client_cert(path, password))
+            .map(|_| ())
+    }
+
+    pub fn ssl_set_root_cert(&mut self, path: &str, filesize: usize) -> Result<()> {
+        self.send_command(Command::ssl_set_root_cert(path, filesize))
+            .map(|_| ())
+    }
+
+    fn mqtt_receive_reply(&mut self)  -> Result<VariablePacket> {
+        for _ in 0..3 {
+            let size = self.tcp_receive_reply_len()?;
+            println!("received reply len({}) ...", size);
+            if size == 0 {
+                println!("retrying ...");
+                continue
+            } else {
+                let mut reply = vec![0 as u8; size];
+                println!("receiving mqtt reply ...");
+                let _ = self.tcp_receive(&mut reply);
+                let reply = std::str::from_utf8(&reply).unwrap_or("");
+                println!("received mqtt reply ({})", reply);
+                return VariablePacket::decode(&mut reply.as_bytes())
+                    .map_err(|err| ModemError::CommandError(format!("Undecodable MQTT message. ({:?})", err)));
+            }
         }
+        Err(ModemError::ReadError("TCP server didn't respond!".into()))
     }
-}
 
-impl<T: serial::Uart> ModemSocket<T> {
-    fn new() -> Self {
-        Self {
-            state: SocketState::new(),
+    fn mqtt_connect(&mut self, device_id: &str, username: &str, password: &str) -> anyhow::Result<()> {
+        let mut buf = Vec::new();
+        let mut conn = ConnectPacket::new(device_id);
+        conn.set_clean_session(true);
+        conn.set_keep_alive(100);
+        conn.set_user_name(Some(username.to_string()));
+        conn.set_password(Some(password.to_string()));
+        let _ = conn.encode(&mut buf)?;
+        let _ = self.tcp_manual_send(&mut buf)?;
+
+        let reply = self.mqtt_receive_reply()?;
+        println!("mqtt decoded packet: ({:?})", reply);
+
+        match reply {
+            VariablePacket::ConnackPacket(_) => Ok(()),
+            _ => Err(anyhow::Error::msg("Invalid MQTT reply ... expected CONNACK!"))
         }
     }
 
-    fn get_running(s: Modem<T>) -> Self {
-        Self {
-            state: SocketState::Connected(s)
+    fn mqtt_publish(&mut self, _device_id: &str, message: &str) -> anyhow::Result<()> {
+        println!("entered mqtt publish ...");
+        let mut buf = Vec::new();
+        let packet = PublishPacket::new(
+            TopicName::new(format!("bajsevi/location")).unwrap(),
+            QoSWithPacketIdentifier::Level0,
+            message.as_bytes(),
+        );
+        let _ = packet.encode(&mut buf)?;
+        self.tcp_manual_send(&mut buf)?;
+        Ok(())
+    }
+
+    pub fn mqtt_send_position_loop(&mut self, host: &str, port: u16, username: &str, password: &str) -> anyhow::Result<()> {
+        if !self.is_gprs_attached()? {
+            let _ = self.gprs_attach_ap(crate::config::MTS)?;
+            let _ = self.try_connect_gprs()?;
         }
+        // When command AT+CIPQSEND=0, it is in normal sending mode. In this mode, after user
+        // sends data by AT+CIPSEND, if the server receives TCP data, it will give ACK message
+        // to module, and the module will respond SEND OK.
+        let _ = self.send("AT+CIPQSEND=0", "OK");
+        // Enables getting data from network manually.
+        let _ = self.send("AT+CIPRXGET=1", "OK");
+
+        for _ in 0..5 {
+            if let Ok(_) = self.tcp_connect(host, port) {
+                break
+            }
+        }
+
+        let device_id = "c36a72df-5bd6-4f9b-995d-059433bc3267";
+        println!("connecting to MQTT with ({}:{})", username, password);
+        let _ = self.mqtt_connect(device_id, username, password)?;
+
+        println!("entering queue receive loop ...");
+        let mut err_count = 0;
+        let _ = loop {
+            match self.receiver.recv() {
+                Ok(Msg::Gps(solution)) => {
+                    println!("received GPS solution {:?} | sending to mqtt ...", solution);
+                    serde_json_core::ser::to_string::<Solution, 512>(&solution)
+                        .map_err(|e| anyhow::Error::new(e))
+                        .and_then(|sol| self.mqtt_publish(device_id, &sol))?;
+                    err_count = 0;
+                },
+                Ok(Msg::Accelerometer(acc)) => {
+                    println!("received accel {} | sending to mqtt ...", acc);
+                    let _ = self.mqtt_publish(device_id, &format!("{:?}", acc))?;
+                    err_count = 0;
+                }
+                Err(e) => {
+                    if err_count < 5 {
+                        err_count += 1;
+                        println!("received error {} | NOT sending to mqtt ...", e);
+                    }
+                    else {
+                        break
+                    }
+                }
+            }
+        };
+
+        Ok(())
     }
 }
 
-impl<UART: serial::Uart> TcpClientStack for ModemTcpStack {
-    type Error = ModemError;
-    type TcpSocket = ModemSocket<UART>;
-
-    fn socket(&mut self) -> Result<Self::TcpSocket> {
-        Ok(self.modem)
-    }
-
-    fn connect(&mut self, socket: &mut Self::TcpSocket, remote: SocketAddr) ->  std::result::Result<(), nb::Error<Self::Error>> {
-        self.tcp_connect(&format!("{}", remote.ip()), remote.port())
-            .map_err(|err| nb::Error::Other(err))
-    }
-
-    fn is_connected(&mut self, _socket: &Self::TcpSocket) -> Result<bool> {
-        self.tcp_is_connected()
-    }
-
-    fn send(&mut self, _socket: &mut Self::TcpSocket, buffer: &[u8]) -> std::result::Result<usize, nb::Error<Self::Error>> {
-        self.tcp_send(buffer)
-            .map_err(|err| nb::Error::Other(err))
-    }
-
-    fn receive( &mut self, _socket: &mut Self::TcpSocket, buffer: &mut [u8]) -> std::result::Result<usize, nb::Error<Self::Error>> {
-        self.tcp_receive(buffer)
-            .map_err(|err| nb::Error::Other(err))
-    }
-
-    fn close(&mut self, _socket: Self::TcpSocket) -> Result<()> {
-        self.tcp_close_connection()
+impl<UART: serial::Uart, PWK: OutputPin, RST: OutputPin, PW: OutputPin> std::io::Read for Modem<UART, PWK, RST, PW> {
+    fn read(&mut self, buf: &mut [u8]) -> std::io::Result<usize> {
+        self.tcp_receive(buf).map_err(|_| std::io::Error::from(std::io::ErrorKind::ConnectionAborted))
     }
 }

src/serial.rs

@@ -0,0 +1,129 @@
+use std::error::Error;
+use std::io;
+use std::thread;
+use std::time::Duration;
+use embedded_hal::serial::{Read, Write};
+use esp_idf_hal::serial::{self, Rx, Tx};
+
+const READ_MAX_RETRIES: usize = 5;
+const READ_WAIT_TIME: u64 = 10;
+
+#[derive(Debug)]
+pub enum SerialError {
+    ReadError(String),
+    WriteError(String),
+}
+
+impl Error for SerialError {}
+
+impl std::fmt::Display for SerialError {
+    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
+        write!(f, "{:?}", self)
+    }
+}
+
+pub type Result<T> = nb::Result<T, SerialError>;
+
+pub struct SerialIO<UART: serial::Uart> {
+    pub rx: Rx<UART>,
+    pub tx: Tx<UART>,
+}
+
+impl<UART: serial::Uart> SerialIO<UART> {
+    pub fn new(tx: Tx<UART>, rx: Rx<UART>) -> Self {
+        Self { rx, tx }
+    }
+
+    pub fn write_bytes(&mut self, payload: &[u8]) -> Result<usize> {
+        let mut num_bytes = 0;
+        for b in payload.iter() {
+            self.tx.write(*b)
+                .map_err(|err| SerialError::WriteError(
+                        format!("Error writing in serial port ({:?})", err)))?;
+            num_bytes += 1;
+        }
+        if num_bytes == payload.len() {
+            Ok(num_bytes)
+        }
+        else {
+            Err(nb::Error::Other(
+                SerialError::WriteError(
+                    "Written bytes shorter than payload length (write_bytes)".to_string()
+                )
+            ))
+        }
+    }
+
+    pub fn read_bytes(&mut self, buf: &mut [u8]) -> Result<usize> {
+        let mut started_reading = false;
+        let mut count = 0;
+        let mut retries = 0;
+
+        loop {
+            match self.rx.read() {
+                Ok(b) => {
+                    started_reading = true;
+                    if count < buf.len() {
+                        buf[count] = b;
+                        count += 1;
+                    }
+                    else { break }
+                },
+                Err(nb::Error::WouldBlock) => {
+                    if started_reading || retries > READ_MAX_RETRIES { break }
+                    else {
+                        thread::sleep(Duration::from_millis(READ_WAIT_TIME));
+                        retries += 1;
+                    }
+                },
+                Err(nb::Error::Other(err)) => println!("Serial read error :: {:?}", err),
+            }
+        };
+        if count > 0 {
+            Ok(count)
+        } else {
+            Err(nb::Error::Other(SerialError::ReadError("Rx buffer empty.".to_string())))
+        }
+    }
+
+    pub fn clear(&mut self) {
+        let mut started_reading = false;
+        let mut retries = 0;
+
+        loop {
+            match self.rx.read() {
+                Ok(_) => {
+                    started_reading = true;
+                },
+                Err(nb::Error::WouldBlock) => {
+                    if started_reading || retries > READ_MAX_RETRIES { break; }
+                    else {
+                        thread::sleep(Duration::from_millis(READ_WAIT_TIME));
+                        retries += 1;
+                    }
+                },
+                Err(nb::Error::Other(err)) => println!("Serial read error :: {:?}", err),
+            }
+        }
+    }
+}
+
+impl<UART: serial::Uart> io::Read for SerialIO<UART> {
+    fn read(&mut self, buf: &mut [u8]) -> io::Result<usize> {
+        let count = nb::block!(self.read_bytes(buf))
+            .map_err(|err| io::Error::new(io::ErrorKind::Other, err))?;
+        Ok(count)
+    }
+}
+
+impl<UART: serial::Uart> io::Write for SerialIO<UART> {
+    fn write(&mut self, buf: &[u8]) -> io::Result<usize> {
+        nb::block!(self.write_bytes(buf))
+            .map_err(|err| io::Error::new(io::ErrorKind::Other, err))
+    }
+
+    fn flush(&mut self) -> io::Result<()> {
+        self.tx.flush()
+            .map_err(|err| io::Error::new(io::ErrorKind::Other, SerialError::ReadError(format!("Flush error ({:?})", err))))
+    }
+}

src/types.rs

@@ -0,0 +1,106 @@
+#![allow(dead_code)]
+use nmea0183::{
+    GGA,
+    GLL,
+    coords::{
+        Latitude as NMEALatitude,
+        Longitude as NMEALongitude,
+        Hemisphere as NMEAHemisphere,
+    },
+};
+use serde::Serialize;
+
+#[derive(Debug, Serialize)]
+pub enum Hemisphere {
+    North,
+    South,
+    East,
+    West,
+}
+
+#[derive(Debug, Serialize)]
+pub struct Latitude {
+    degrees: u8,
+    minutes: u8,
+    seconds: f32,
+    hemisphere: Hemisphere,
+}
+
+#[derive(Debug, Serialize)]
+pub struct Longitude {
+    degrees: u8,
+    minutes: u8,
+    seconds: f32,
+    hemisphere: Hemisphere,
+}
+
+impl From<NMEALatitude> for Latitude {
+    fn from(lat: NMEALatitude) -> Self {
+        Self {
+            degrees: lat.degrees,
+            minutes: lat.minutes,
+            seconds: lat.seconds,
+            hemisphere: lat.hemisphere.into(),
+        }
+    }
+}
+
+impl From<NMEALongitude> for Longitude {
+    fn from(lon: NMEALongitude) -> Self {
+        Self {
+            degrees: lon.degrees,
+            minutes: lon.minutes,
+            seconds: lon.seconds,
+            hemisphere: lon.hemisphere.into(),
+        }
+    }
+}
+
+impl From<NMEAHemisphere> for Hemisphere {
+    fn from(hem: NMEAHemisphere) -> Self {
+        match hem {
+            NMEAHemisphere::North => Self::North,
+            NMEAHemisphere::South => Self::South,
+            NMEAHemisphere::East => Self::East,
+            NMEAHemisphere::West => Self::West,
+        }
+    }
+}
+
+#[derive(Debug, Serialize)]
+pub struct Solution {
+    pub latitude: Latitude,
+    pub longitude: Longitude,
+    pub altitude: Option<f32>,
+    pub speed: Option<f32>,
+    pub direction: Option<f32>,
+}
+
+impl From<GGA> for Solution {
+    fn from(gga: GGA) -> Self {
+        Self {
+            latitude: gga.latitude.into(),
+            longitude: gga.longitude.into(),
+            altitude: Some(gga.altitude.meters),
+            speed: None,
+            direction: None,
+        }
+    }
+}
+
+impl From<GLL> for Solution {
+    fn from(gll: GLL) -> Self {
+        Self {
+            latitude: gll.latitude.into(),
+            longitude: gll.longitude.into(),
+            altitude: None,
+            speed: None,
+            direction: None,
+        }
+    }
+}
+
+pub enum Msg {
+    Gps(Solution),
+    Accelerometer(String),
+}