implement non-blocking read with timeout

much better than before :)

src/command.rs

@@ -7,15 +7,7 @@ pub struct Command {
 }
 
 impl Command {
-    pub fn initgprs() -> Command {
+    pub fn modem_info() -> Command {
-        Command {
-            text: "AT+SAPBR=3,1,\"Contype\",\"GPRS\"".to_string(),
-            timeout: Duration::from_millis(3000),
-            contains: Some("OK".to_string()),
-        }
-    }
-
-    pub fn modeminfo() -> Command {
         Command {
             text: "ATI".to_string(),
             timeout: Duration::from_millis(6000),
@@ -23,7 +15,7 @@ impl Command {
         }
     }
 
-    pub fn fwrevision() -> Command {
+    pub fn fw_revision() -> Command {
         Command {
             text: "AT+CGMR".to_string(),
             timeout: Duration::from_millis(3000),
@@ -63,7 +55,7 @@ impl Command {
         }
     }
 
-    pub fn checkreg() -> Command {
+    pub fn check_reg() -> Command {
         Command {
             text: "AT+CREG?".to_string(),
             timeout: Duration::from_millis(3000),
@@ -71,7 +63,15 @@ impl Command {
         }
     }
 
-    pub fn opengprs() -> Command {
+    pub fn gprs_init() -> Command {
+        Command {
+            text: "AT+SAPBR=3,1,\"Contype\",\"GPRS\"".to_string(),
+            timeout: Duration::from_millis(3000),
+            contains: Some("OK".to_string()),
+        }
+    }
+
+    pub fn gprs_open() -> Command {
         Command {
             text: "AT+SAPBR=1,1".to_string(),
             timeout: Duration::from_millis(3000),
@@ -79,6 +79,30 @@ impl Command {
         }
     }
 
+    pub fn gprs_set_apn(apn: &str) -> Command {
+        Command {
+            text: format!("AT+SAPBR=3,1,\"APN\",\"{}\"", apn),
+            timeout: Duration::from_millis(3000),
+            contains: Some("OK".to_string()),
+        }
+    }
+
+    pub fn gprs_set_user(user: &str) -> Command {
+        Command {
+            text: format!("AT+SAPBR=3,1,\"USER\",\"{}\"", user),
+            timeout: Duration::from_millis(3000),
+            contains: Some("OK".to_string()),
+        }
+    }
+
+    pub fn gprs_set_pwd(password: &str) -> Command {
+        Command {
+            text: format!("AT+SAPBR=3,1,\"PWD\",\"{}\"", password),
+            timeout: Duration::from_millis(3000),
+            contains: Some("OK".to_string()),
+        }
+    }
+
     pub fn getbear() -> Command {
         Command {
             text: "AT+SAPBR=2,1".to_string(),
@@ -87,7 +111,23 @@ impl Command {
         }
     }
 
-    pub fn inithttp() -> Command {
+    pub fn get_local_ip_addr() -> Command {
+        Command {
+            text: "AT+CIFSR".to_string(),
+            timeout: Duration::from_millis(3000),
+            contains: Some("OK".to_string()),
+        }
+    }
+
+    pub fn ping(domain: &str) -> Command {
+        Command {
+            text: format!("AT+CIPPING=\"{}\"", domain),
+            timeout: Duration::from_millis(3000),
+            contains: Some("OK".to_string()),
+        }
+    }
+
+    pub fn http_init() -> Command {
         Command {
             text: "AT+HTTPINIT".to_string(),
             timeout: Duration::from_millis(3000),
@@ -95,7 +135,7 @@ impl Command {
         }
     }
 
-    pub fn sethttp() -> Command {
+    pub fn http_set() -> Command {
         Command {
             text: "AT+HTTPPARA=\"CID\",1".to_string(),
             timeout: Duration::from_millis(3000),
@@ -103,15 +143,7 @@ impl Command {
         }
     }
 
-    pub fn checkssl() -> Command {
+    pub fn http_enable_ssl() -> Command {
-        Command {
-            text: "AT+CIPSSL=?".to_string(),
-            timeout: Duration::from_millis(3000),
-            contains: Some("OK".to_string()),
-        }
-    }
-
-    pub fn enablessl() -> Command {
         Command {
             text: "AT+HTTPSSL=1".to_string(),
             timeout: Duration::from_millis(3000),
@@ -119,7 +151,7 @@ impl Command {
         }
     }
 
-    pub fn disablessl() -> Command {
+    pub fn http_disable_ssl() -> Command {
         Command {
             text: "AT+HTTPSSL=0".to_string(),
             timeout: Duration::from_millis(3000),
@@ -127,7 +159,7 @@ impl Command {
         }
     }
 
-    pub fn initurl() -> Command {
+    pub fn http_init_url() -> Command {
         Command {
             text: "AT+HTTPPARA=\"URL\",\"{}\"".to_string(),
             timeout: Duration::from_millis(3000),
@@ -135,7 +167,7 @@ impl Command {
         }
     }
 
-    pub fn doget() -> Command {
+    pub fn http_get() -> Command {
         Command {
             text: "AT+HTTPACTION=0".to_string(),
             timeout: Duration::from_millis(3000),
@@ -143,7 +175,7 @@ impl Command {
         }
     }
 
-    pub fn setcontent() -> Command {
+    pub fn http_set_content() -> Command {
         Command {
             text: "AT+HTTPPARA=\"CONTENT\",\"{}\"".to_string(),
             timeout: Duration::from_millis(3000),
@@ -151,7 +183,7 @@ impl Command {
         }
     }
 
-    pub fn postlen() -> Command {
+    pub fn http_post_len() -> Command {
         Command {
             text: "AT+HTTPDATA={}5000".to_string(),
             timeout: Duration::from_millis(3000),
@@ -159,7 +191,7 @@ impl Command {
         }
     }
 
-    pub fn dopost() -> Command {
+    pub fn http_post() -> Command {
         Command {
             text: "AT+HTTPACTION=1".to_string(),
             timeout: Duration::from_millis(3000),
@@ -167,7 +199,7 @@ impl Command {
         }
     }
 
-    pub fn getdata() -> Command {
+    pub fn http_get_data() -> Command {
         Command {
             text: "AT+HTTPREAD".to_string(),
             timeout: Duration::from_millis(3000),
@@ -191,30 +223,6 @@ impl Command {
         }
     }
 
-    pub fn setapn(apn: &str) -> Command {
-        Command {
-            text: format!("AT+SAPBR=3,1,\"APN\",\"{}\"", apn),
-            timeout: Duration::from_millis(3000),
-            contains: Some("OK".to_string()),
-        }
-    }
-
-    pub fn setuser(user: &str) -> Command {
-        Command {
-            text: format!("AT+SAPBR=3,1,\"USER\",\"{}\"", user),
-            timeout: Duration::from_millis(3000),
-            contains: Some("OK".to_string()),
-        }
-    }
-
-    pub fn setpwd(password: &str) -> Command {
-        Command {
-            text: format!("AT+SAPBR=3,1,\"PWD\",\"{}\"", password),
-            timeout: Duration::from_millis(3000),
-            contains: Some("OK".to_string()),
-        }
-    }
-
     pub fn probe() -> Command {
         Command {
             text: "AT".to_string(),
@@ -230,4 +238,60 @@ impl Command {
             contains: Some("OK".to_string()),
         }
     }
+
+    pub fn tcp_ssl_disable() -> Command {
+        Command {
+            text: "AT+CIPSSL=0".to_string(),
+            timeout: Duration::from_millis(3000),
+            contains: Some("OK".to_string()),
+        }
+    }
+
+    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(),
+            timeout: Duration::from_millis(3000),
+            contains: Some("OK".to_string()),
+        }
+    }
+
+    pub fn tcp_connect(addr: &str, port: u16) -> Command {
+        Command {
+            text: format!("AT+CIPSTART=\"TCP\",\"{}\",\"{}\"", addr, port),
+            timeout: Duration::from_millis(5000),
+            contains: Some("CONNECT OK".to_string()),
+        }
+    }
+
+    pub fn tcp_send(size: usize) -> Command {
+        Command {
+            text: format!("AT+CIPSEND={}", size),
+            timeout: Duration::from_millis(3000),
+            contains: Some("OK".to_string()),
+        }
+    }
+
+    pub fn tcp_write(payload: &str) -> Command {
+        Command {
+            text: payload.to_string(),
+            timeout: Duration::from_millis(2000),
+            contains: Some("> ".to_string()),
+        }
+    }
+
+    pub fn tcp_set_quick_mode(quick_mode: bool) -> Command {
+        Command {
+            text: format!("AT+CIPQSEND={}", quick_mode as u8),
+            timeout: Duration::from_millis(3000),
+            contains: Some("OK".to_string()),
+        }
+    }
 }

src/config.rs

@@ -9,4 +9,3 @@ pub const A1_GPRS_AP: GprsAp = GprsAp {
     username: "internet",
     password: "internet",
 };
-

src/main.rs

@@ -7,10 +7,6 @@ use anyhow;
 use esp_idf_hal::prelude::*;
 use esp_idf_hal::peripherals::Peripherals;
 use esp_idf_hal::serial;
-use std::time::Duration;
-use std::thread;
-
-use std::sync::mpsc::{channel, Receiver, Sender};
 
 fn main() -> anyhow::Result<()> {
     esp_idf_sys::link_patches();
@@ -36,23 +32,36 @@ fn main() -> anyhow::Result<()> {
         serial::config::Config::default().baudrate(Hertz(115200)),
     ).expect("Failed to create serial ... :(");
 
+    let (tx, rx) = serial.split();
+    let mut mdm = modem::Modem::new(tx, rx);
+
     // Unwrap all of these, without them we can't do anything anyways.
     let modem_pwrkey = dp.pins.gpio4.into_output().unwrap();
     let modem_rst = dp.pins.gpio5.into_output().unwrap();
     let modem_power = dp.pins.gpio23.into_output().unwrap();
 
-    modem::init(modem_pwrkey, modem_rst, modem_power)?;
+    mdm.init(modem_pwrkey, modem_rst, modem_power)?;
-    let (tx, rx) = serial.split();
-    let mut mdm = modem::Modem::new(tx, rx);
 
-    let _ = mdm.probe()?;
     let _ = mdm.connect_to_gprs_ap(
         config::A1_GPRS_AP.apn,
         config::A1_GPRS_AP.username,
         config::A1_GPRS_AP.password,
     )?;
-    if mdm.is_gprs_attached()? {
+
-        println!("GPRS ATTACHEEEDDDDDDDDD!!!!!!!!!!!!!!!1");
+    loop {
+        if mdm.is_gprs_attached()? {
+            if mdm.tcp_is_ssl_enabled()? {
+                mdm.tcp_ssl_disable()?;
+            }
+            let _ = mdm.get_ip_addr()?;
+
+            println!("connecting to server!");
+            let _ = mdm.tcp_set_quick_mode(true);
+            let _ = mdm.tcp_connect("51.158.66.64", 9988)?;
+            let _ = mdm.tcp_send("aaaaa")?;
+            break;
+        }
+        println!("!!!!!!!!!!!!!!!! GPRS NOT ATTACHED !!!!!!!!!!!!!!!!");
     }
 
     Ok(())

src/modem.rs

@@ -1,19 +1,17 @@
 use crate::command::Command;
 
 use std::thread;
-use std::io::{Error as IoError, ErrorKind, BufRead};
 use std::error::Error;
 use std::time::{Duration, Instant};
 
 use embedded_hal::serial::{Read, Write};
 use embedded_hal::digital::v2::OutputPin;
-use esp_idf_hal::serial::{self, Rx, Tx, SerialError};
+use esp_idf_hal::serial::{self, Rx, Tx};
 
 pub type Result<T> = std::result::Result<T, ModemError>;
 
 pub struct Modem<UART: serial::Uart> {
-    is_connected: bool,
+    rx: Rx<UART>,
-    rx: MyRx<UART>,
     tx: Tx<UART>,
 }
 
@@ -21,6 +19,7 @@ pub struct Modem<UART: serial::Uart> {
 pub enum ModemError {
     CommandError(String),
     SetupError(String),
+    ReadError,
     TimeoutError,
 }
 
@@ -32,161 +31,191 @@ impl std::fmt::Display for ModemError {
     }
 }
 
-struct MyRx<UART: serial::Uart>(Rx<UART>);
-
-impl<UART: serial::Uart> MyRx<UART> {
-    fn read(&mut self) -> nb::Result<u8, SerialError> {
-        self.0.read()
-    }
-
-    fn read_blocking(&mut self) -> std::result::Result<u8, SerialError> {
-        nb::block!(self.read())
-    }
-}
-
-impl<UART: serial::Uart> std::io::Read for MyRx<UART> {
-    fn read(&mut self, buf: &mut [u8]) -> std::io::Result<usize> {
-        let mut read_bytes: usize = 0;
-        loop {
-            let b = self.read_blocking()
-                .map_err(|err| IoError::new(ErrorKind::Other, format!("Serial error {:?}", err)))?;
-            buf[read_bytes] = b;
-            read_bytes += 1;
-            if b == '\0' as u8 || b == '\n' as u8 {
-                break;
-            }
-        };
-        Ok(read_bytes)
-    }
-}
-
 impl<UART: serial::Uart> Modem<UART> {
     pub fn new(tx: Tx<UART>, rx: Rx<UART>) -> Self {
         Self {
-            is_connected: false,
+            rx,
-            rx: MyRx(rx),
             tx,
         }
     }
 
-    /// Reads the serial RX until it has bytes, or until a timeout is reached. The timeout is
+    /// Initialize the modem (sim800l in this case). The initialization process sets all pins in the
-    /// provided on input via the `timeout` argument. The first argument `expected` is the expected
+    /// required state so that the modem is turned on, then resets it a couple of times (beats me) and
-    /// end of the buffer. If it's `None`, the whole response is returned as is. If it's
+    /// sleeps for 3 seconds, which is enough for the modem to come online.
-    /// `Some(expected_end)`, then the end of the response is matched against `expected_end`. If
-    /// they match, great! The response is returned from the function, but if not, then a
-    /// [ModemError::CommandError](crate::modem::ModemError::CommandError) is returned.
     ///
-    /// It's ok to use this function like this for now because the write/read cycle is blocking,
+    /// Below is an example for sim800l pins on a LilyGo TTGO T-Call.
-    /// hence the case where multiple writes happen asynchronously isn't handled. See
+    ///
-    /// [send_command](crate::modem::Modem::send_command) for more info on the blocking part.
+    /// # Examples
-    fn read_response(&mut self, contains: Option<String>, timeout: Duration) -> Result<String> {
+    ///
-        let mut response = String::new();
+    /// ```
-        let now = Instant::now();
+    /// let modem_pwrkey = dp.pins.gpio4.into_output().unwrap();
-        let match_text: String = contains.unwrap_or("".to_string());
+    /// let modem_rst = dp.pins.gpio5.into_output().unwrap();
+    /// let modem_power = dp.pins.gpio23.into_output().unwrap();
+    ///
+    /// 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<()> {
+        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()))?;
+        // 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()))?;
+        thread::sleep(Duration::from_millis(1000));
+        pwrkey.set_high().map_err(|_| ModemError::SetupError("Error setting PWRKEY to high.".to_string()))?;
+        println!("Waiting 3s for sim module to come online ...");
+        thread::sleep(Duration::from_millis(3000));
+        Ok(())
+    }
 
-        let mut reader = std::io::BufReader::new(&mut self.rx);
+    /// Reads a whole line (that ends with \\n) within the given `timeout` passed on input.
+    /// `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 read_line(&mut self, timeout: Duration) -> Result<String> {
+        let mut buf = String::new();
+        let start = Instant::now();
 
         loop {
-            let mut line = String::new();
+            match self.rx.read() {
-            reader.read_line(&mut line)
+                Ok(b) => {
-                .map_err(|err| ModemError::CommandError(format!("read line failed: {}", err)))?;
+                    print!("{}, ", b);
-            println!("Read {} from serial ...", line.trim());
+                    buf.push(b as char);
+                    if b == '\n' as u8 {
+                        return Ok(buf);
+                    }
+                },
+                _ => {
+                    if Instant::now() > start + timeout {
+                        return Err(ModemError::ReadError);
+                    }
+                    else {
+                        thread::sleep(Duration::from_millis(200));
+                    }
+                },
+            }
+        }
+    }
 
-            // check if empty first because it's much simpler and more frequent
+    /// Reads the serial RX until a \\n char is encoutered, or a timeout is reached. The timeout is
-            if line == "" {
+    /// provided on input via the `timeout` argument. The first argument `contains` is checked
-                if now + timeout > Instant::now() {
+    /// against a line in the response, if it's there the reading stops.
-                    return Err(ModemError::TimeoutError);
+    ///
-                }
+    /// If `contains` is `None`, the first line only is returned in the response. If it's
-                thread::sleep(Duration::from_millis(1000));
+    /// `Some(match_txt)`, then the end of the response is matched against `match_txt`.
-                continue;
+    fn read_response(&mut self, contains: Option<String>, timeout: Duration) -> Result<String> {
-            } else {
+        let mut response = String::new();
+        let start = Instant::now();
+        let match_text: String = contains.unwrap_or("".to_string());
+
+        loop {
+            if let Ok(line) = self.read_line(start + timeout - Instant::now()) {
                 response.push_str(&line);
-                if line.contains(&match_text) {
+                if line.contains("ERROR") || line.contains(&match_text) {
                     println!("Found match {} for line {} ... exiting response reader now!", match_text, line);
                     println!("-----------------------------------------------------------");
                     break;
                 }
+            } else {
+                if Instant::now() > start + timeout {
+                    return Err(ModemError::TimeoutError);
+                }
+                println!("got into retry loop in read_response");
+                thread::sleep(Duration::from_millis(200));
             }
         }
         Ok(response.to_string())
     }
 
-    fn send_command(&mut self, cmd: Command) -> Result<String> {
+    fn send(&mut self, b: u8) -> Result<()> {
-        for b in cmd.text.as_bytes().iter() {
+        nb::block!(self.tx.write(b))
-            nb::block!(self.tx.write(*b)).map_err(|_| ModemError::CommandError("error writing to serial".to_string()))?;
+            .map_err(|_| ModemError::CommandError(format!("error writing {} to serial", b)))?;
+        Ok(())
+    }
+
+    fn send_bytes(&mut self, payload: &[u8]) -> Result<()> {
+        for b in payload.iter() {
+            self.send(*b)?;
         }
-        nb::block!(self.tx.write('\r' as u8)).map_err(|_| ModemError::CommandError("error writing <CR> to serial".to_string()))?;
+        self.send('\r' as u8)?;
+        Ok(())
+    }
+
+    fn send_command(&mut self, cmd: Command) -> Result<String> {
+        println!("-----------------------------------------------------------");
+        println!("Sending {} ...", cmd.text);
+        let _ = self.send_bytes(cmd.text.as_bytes())?;
         self.read_response(cmd.contains.clone(), cmd.timeout.clone())
     }
 
     pub fn get_ip_addr(&mut self) -> Result<String> {
-        println!("getting ip addres ...");
+        self.send_command(Command::getbear())
-        let res = self.send_command(Command::getbear())?;
+    }
-        println!("{}", res);
+
-        Ok(res)
+    pub fn get_local_ip_addr(&mut self) -> Result<()> {
+        let _ = self.send_command(Command::get_local_ip_addr())?;
+        Ok(())
     }
 
     pub fn connect_to_gprs_ap(&mut self, apn: &str, username: &str, password: &str)-> Result<()> {
         println!("init gprs ...");
-        let _ = self.send_command(Command::initgprs())?;
+        let _ = self.send_command(Command::gprs_init())?;
 
-        println!("connecting to {} with {}:{}", apn, username, password);
+        println!("setting up gprs credentials for apn {}, {}:{})", apn, username, password);
 
-        let _ = self.send_command(Command::setapn(apn))?;
+        let _ = self.send_command(Command::gprs_set_apn(apn))?;
-        let _ = self.send_command(Command::setuser(username))?;
+        let _ = self.send_command(Command::gprs_set_user(username))?;
-        let _ = self.send_command(Command::setpwd(password))?;
+        let _ = self.send_command(Command::gprs_set_pwd(password))?;
 
         println!("open gprs ...");
-        let _ = self.send_command(Command::initgprs())?;
+        let _ = self.send_command(Command::gprs_open())?;
 
-        self.is_connected = true;
         Ok(())
     }
 
     pub fn info(&mut self)-> Result<String> {
-        println!("getting modem info with API command");
+        self.send_command(Command::modem_info())
-        self.send_command(Command::modeminfo())
     }
 
     pub fn probe(&mut self)-> Result<String> {
-        println!("probing modem with AP command");
         self.send_command(Command::probe())
     }
 
     pub fn is_gprs_attached(&mut self)-> Result<bool> {
-        println!("testing gprs connection ...");
         let res = self.send_command(Command::is_gprs_attached())?;
-
         Ok(res.contains("+CGATT: 1"))
     }
-}
 
-/// Initialize the modem (sim800l in this case). The initialization process sets all pins in the
+    pub fn ping(&mut self, domain: &str)-> Result<()> {
-/// required state so that the modem is turned on, then resets it a couple of times (beats me) and
+        self.send_command(Command::ping(domain))?;
-/// sleeps for 3 seconds, which is enough for the modem to come online.
+        Ok(())
-///
+    }
-/// Below is an example for sim800l pins on a LilyGo TTGO T-Call.
+
-///
+    pub fn tcp_is_ssl_enabled(&mut self) -> Result<bool> {
-/// # Examples
+        let res = self.send_command(Command::tcp_ssl_check())?;
-///
+        Ok(res.contains("+CIPSSL: (1)"))
-/// ```
+    }
-/// let modem_pwrkey = dp.pins.gpio4.into_output().unwrap();
+
-/// let modem_rst = dp.pins.gpio5.into_output().unwrap();
+    pub fn tcp_ssl_disable(&mut self) -> Result<()> {
-/// let modem_power = dp.pins.gpio23.into_output().unwrap();
+        let _ = self.send_command(Command::tcp_ssl_disable())?;
-///
+        Ok(())
-/// modem::init(modem_pwrkey, modem_rst, modem_power);
+    }
-/// ```
+
-pub fn init(mut pwrkey: impl OutputPin, mut rst: impl OutputPin, mut power: impl OutputPin) -> Result<()> {
+    pub fn tcp_connect(&mut self, addr: &str, port: u16) -> Result<()> {
-    println!("Turning SIM800L on ...");
+        self.send_command(Command::tcp_connect(addr, port))?;
-    power.set_high().map_err(|_| ModemError::SetupError("Error setting POWER to high.".to_string()))?;
+        Ok(())
-    rst.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()))?;
+    pub fn tcp_set_quick_mode(&mut self, mode: bool) -> Result<()> {
-    thread::sleep(Duration::from_millis(100));
+        self.send_command(Command::tcp_set_quick_mode(mode))?;
-    pwrkey.set_low().map_err(|_| ModemError::SetupError("Error setting PWRKEY to low.".to_string()))?;
+        Ok(())
-    thread::sleep(Duration::from_millis(1000));
+    }
-    pwrkey.set_high().map_err(|_| ModemError::SetupError("Error setting PWRKEY to high.".to_string()))?;
+
-    println!("Waiting 3s for sim module to come online ...");
+    pub fn tcp_send(&mut self, payload: &str) -> Result<()> {
-    thread::sleep(Duration::from_millis(3000));
+        self.send_command(Command::tcp_send(payload.len()))?;
-    Ok(())
+        self.send_command(Command::tcp_write(payload))?;
+        Ok(())
+    }
 }