diff --git a/Cargo.toml b/Cargo.toml
index 8afb676..f582a29 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -16,7 +16,10 @@ opt-level = "z"
 pio = ["esp-idf-sys/pio"]
 
 [dependencies]
+embedded-hal = "0.2.7"
+esp-idf-hal = "0.37.4"
 esp-idf-sys = { version = "0.31.5", features = ["binstart"] }
+nb = "1.0.0"
 
 
 [build-dependencies]
diff --git a/src/main.rs b/src/main.rs
index bcb3da9..6f9055a 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,11 +1,10 @@
 mod modem;
 
-use std;
-use std::io::{BufReader, BufWriter, Cursor};
-use std::io::{Read, Write};
-use esp_idf_sys as _; // If using the `binstart` feature of `esp-idf-sys`, always keep this module imported
+use esp_idf_hal::prelude::*;
+use esp_idf_hal::peripherals::Peripherals;
+use esp_idf_hal::serial;
 
-fn start_loop<R: Read, W: Write>(_m: &mut modem::Modem<R, W>) -> () {
+fn start_loop<UART: serial::Uart>(_m: &mut modem::Modem<UART>) -> () {
     println!("Starting rx/tx loop (not implemented yet) ...");
 }
 
@@ -22,14 +21,39 @@ const A1: Ap = Ap {
 };
 
 fn main() {
-    println!("Starting GPRS ...");
+    esp_idf_sys::link_patches();
 
-    let rx = Cursor::new(Vec::new());
-    let tx = Cursor::new(Vec::new());
-    let mut sim800l = modem::Modem::new(BufReader::new(rx), BufWriter::new(tx));
+    let dp = Peripherals::take().unwrap();
 
-    match sim800l.connect_to_gprs_ap(A1.apn, A1.username, A1.password) {
-        Ok(_) => start_loop(&mut sim800l),
-        Err(e) => println!("{}", e),
-    }
+    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).unwrap();
+
+    let rx_pin = dp.pins.gpio26;
+    let tx_pin = dp.pins.gpio27;
+
+    let serial_pins = serial::Pins {
+        tx: tx_pin,
+        rx: rx_pin,
+        cts: None,
+        rts: None,
+    };
+
+    println!("Setting up serial ...");
+    let serial: serial::Serial<serial::UART1, _, _> = serial::Serial::new(
+        dp.uart1,
+        serial_pins,
+        serial::config::Config::default().baudrate(Hertz(9600)),
+    ).expect("Failed to create serial ... bailing");
+    println!("Serial done ...");
+
+    println!("Writing to serial TX ...");
+
+    let (tx, rx) = serial.split();
+
+    let mut modem = modem::Modem::new(tx, rx);
+    let _ = modem.connect_to_gprs_ap(A1.apn, A1.username, A1.password).unwrap();
+    let _ = modem.test_modem().unwrap();
 }
diff --git a/src/modem.rs b/src/modem.rs
index e2664b8..aaadd35 100644
--- a/src/modem.rs
+++ b/src/modem.rs
@@ -1,32 +1,50 @@
-use std;
-use std::io::{Read, Write};
+use std::fmt::Write;
+use std::thread;
+use std::time::Duration;
 
-pub struct Modem<R: Read, W: Write> {
-    is_connected: bool,
-    rx: R,
-    tx: W,
+use embedded_hal::digital::v2::OutputPin;
+use embedded_hal::serial::Read;
+use esp_idf_hal::serial::{self, Rx, Tx};
+
+pub fn init(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_owned()))?;
+    rst.set_high().map_err(|_| ModemError::SetupError("Error setting RST to high.".to_owned()))?;
+    // 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_owned()))?;
+    thread::sleep(Duration::from_millis(100));
+    pwrkey.set_low().map_err(|_| ModemError::SetupError("Error setting PWRKEY to low.".to_owned()))?;
+    thread::sleep(Duration::from_millis(1000));
+    pwrkey.set_high().map_err(|_| ModemError::SetupError("Error setting PWRKEY to high.".to_owned()))?;
+    println!("Waiting 5s for sim module to come online ...");
+    thread::sleep(Duration::from_millis(5000));
+    Ok(())
 }
 
+pub struct Modem<UART: serial::Uart> {
+    is_connected: bool,
+    rx: Rx<UART>,
+    tx: Tx<UART>,
+}
+
+#[derive(Debug)]
 pub enum ModemError {
     GprsAPConnectionError(String),
     ATCommandError(String),
     CommandError(String),
+    SetupError(String),
 }
 
 impl std::fmt::Display for ModemError {
     fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
-        match self {
-            Self::GprsAPConnectionError(msg) => write!(f, "gprs connection error {}", msg),
-            Self::ATCommandError(msg) => write!(f, "AT command error {}", msg),
-            Self::CommandError(msg) => write!(f, "Modem command error {}", msg),
-        }
+        write!(f, "{:?}", self)
     }
 }
 
 pub type Result<T> = std::result::Result<T, ModemError>;
 
-impl<R: Read, W: Write> Modem<R, W> {
-    pub fn new(rx: R, tx: W) -> Self {
+impl<UART: serial::Uart> Modem<UART> {
+    pub fn new(tx: Tx<UART>, rx: Rx<UART>) -> Self {
         Self {
             is_connected: false,
             rx,
@@ -37,27 +55,55 @@ impl<R: Read, W: Write> Modem<R, W> {
     fn at_command(&mut self, cmd: &str) -> Result<String> {
         let mut msg = "AT+".to_owned();
         msg.push_str(cmd);
-        self.send_command(msg.to_owned())
+        self.send_command(&msg)
             .map_err(|err| ModemError::ATCommandError(format!("{}", err)))
     }
 
-    fn send_command(&mut self, cmd: String) -> Result<String> {
-        self.tx.write(cmd.as_bytes())
-            .map_err(|err| ModemError::CommandError(format!("{}", err)))?;
-        let mut response = String::new();
-        self.rx.read_to_string(&mut response)
-            .map_err(|err| ModemError::CommandError(format!("{}", err)))?;
+    fn read_response(&mut self) -> Result<String> {
+        let len = self.rx.count()
+            .map_err(|_| ModemError::CommandError("Error getting RX fifo length".to_owned()))?;
+
         if cfg!(debug_assertions) {
-            println!("{} = {}", cmd, response);
+            println!("Reading {} bytes from serial RX ...", len);
+        }
+
+        let mut response = String::new();
+        for _ in 0..len {
+            nb::block!(self.rx.read())
+                .map(|b| response.push(b as char))
+                .map_err(|_| ModemError::CommandError("Error reading from RX".to_owned()))?;
+        }
+        response = response.trim().to_owned();
+
+        if cfg!(debug_assertions) {
+            println!("Received: {}", response);
         }
         Ok(response)
     }
 
-    pub fn connect_to_gprs_ap(&mut self, apn: &str, username: &str, password: &str)-> Result<()> {
+    fn send_command(&mut self, cmd: &str) -> Result<String> {
+        writeln!(self.tx, "{}", cmd)
+            .map_err(|_| ModemError::CommandError("error writing to serial".to_owned()))?;
+        self.read_response()
+    }
+
+    pub fn connect_to_gprs_ap(&mut self, apn: &str, username: &str, password: &str)-> Result<String> {
+        println!("connecting to {} with {}:{}", apn, username, password);
         self.at_command("CGATTCGATT=1")
-            .map(|_| {
-                println!("connecting to {} with {}:{}", apn, username, password);
+            .map(|response| {
+                println!("connected to {} with response: {}", apn, response);
                 self.is_connected = true;
+                response
+            })
+            .map_err(|err| ModemError::GprsAPConnectionError(format!("{}", err)))
+    }
+
+    pub fn test_modem(&mut self)-> Result<String> {
+        println!("testing modem with AP command");
+        self.at_command("AT")
+            .map(|response| {
+                println!("modem responded with: {}", response);
+                response
             })
             .map_err(|err| ModemError::GprsAPConnectionError(format!("{}", err)))
     }