Diff: ESP8266/ESP8266.cpp

Revision:

44:16da10e7b3f7

Parent:

35:22d30e936e4c

Child:

45:3cfb7406d993

Child:

46:4abb80f0a920

--- a/ESP8266/ESP8266.cpp	Fri May 01 18:29:38 2015 +0000
+++ b/ESP8266/ESP8266.cpp	Wed Jun 03 21:44:20 2015 +0000
@@ -1,454 +1,395 @@
-/* Copyright (C) 2012 mbed.org, MIT License
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
- * and associated documentation files (the "Software"), to deal in the Software without restriction,
- * including without limitation the rights to use, copy, modify, merge, publish, distribute,
- * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in all copies or
- * substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
- * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
- * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
- */
-
-#include "mbed.h"
-#include "ESP8266.h"
-#include "Endpoint.h"
-#include <string>
-#include <algorithm>
-
-//Debug is disabled by default
-#ifdef DEBUG
-#define DBG(x, ...)  printf("[ESP8266 : DBG]"x"\r\n", ##__VA_ARGS__);
-#define WARN(x, ...) printf("[ESP8266 : WARN]"x"\r\n", ##__VA_ARGS__);
-#define ERR(x, ...)  printf("[ESP8266 : ERR]"x"\r\n", ##__VA_ARGS__);
-#else
-#define DBG(x, ...)
-#define WARN(x, ...)
-#define ERR(x, ...)
-#endif
-
-#ifdef DEBUG
-#define INFO(x, ...) printf("[ESP8266 : INFO]"x"\r\n", ##__VA_ARGS__);
-#else
-#define INFO(x, ...)
-#endif
-
-#define ESP_MAX_TRY_JOIN 3
-#define ESP_MAXID 4 // the largest possible ID Value (max num of sockets possible)
-
-extern Serial pc;
-
-ESP8266 * ESP8266::inst;
-char* ip = NULL;
-
-ESP8266::ESP8266(   PinName tx, PinName rx, PinName _reset, const char * ssid, const char * phrase, uint32_t baud):
-    wifi(tx, rx), reset_pin(_reset), buf_ESP8266(256)
-{
-    memset(&state, 0, sizeof(state));
-
-    // change all ' ' in '$' in the ssid and the passphrase
-    strcpy(this->ssid, ssid);
-    for (int i = 0; i < strlen(ssid); i++) {
-        if (this->ssid[i] == ' ')
-            this->ssid[i] = '$';
-    }
-    strcpy(this->phrase, phrase);
-    for (int i = 0; i < strlen(phrase); i++) {
-        if (this->phrase[i] == ' ')
-            this->phrase[i] = '$';
-    }
-
-
-    inst = this;
-    attach_rx(false);
-
-    wifi.baud(baud); // initial baud rate of the ESP8266
-
-    state.associated = false;
-    state.cmdMode = false;
-}
-
-bool ESP8266::join()
-{
-    sendCommand( "AT+CWMODE=1", "change", NULL, 1000);
-    string cmd="AT+CWJAP=\""+(string)this->ssid+"\",\""+(string)this->phrase+"\"";
-    if( sendCommand( cmd.c_str(), "OK", NULL, 10000) ) {
-        // successfully joined the network
-        state.associated = true;
-        INFO("\r\nssid: %s\r\nphrase: %s\r\nsecurity: %s\r\n\r\n", this->ssid, this->phrase);
-        return true;
-    }
-    return false;
-}
-
-bool ESP8266::connect()
-{
-    sendCommand("AT+CWDHCP=1,1","OK",NULL,1000); // DHCP Enabled in Station Mode
-    return ESP8266::join();
-}
-
-bool ESP8266::is_connected()
-{
-    return true;
-}
-
-bool ESP8266::start(bool type,char* ip, int port, int id)
-{
-    // Error Check
-    if(id > ESP_MAXID) {
-        ERR("startUDPMulti: max id is: %d, id given is %d",ESP_MAXID,id);
-        return false;
-    }
-    // Single Connection Mode
-    if(id < 0) {
-        DBG("Start Single Connection Mode");
-        char portstr[5];
-        char idstr[2];
-        bool check [3] = {0};
-        sprintf(idstr,"%d",id);
-        sprintf(portstr, "%d", port);
-        switch(type) {
-            case ESP_UDP_TYPE : //UDP
-                check[0] = sendCommand(( "AT+CIPSTART=\"UDP\",\"" + (string) ip + "\"," + (string) portstr ).c_str(), "OK", NULL, 10000);
-                break;
-            case ESP_TCP_TYPE : //TCP
-                check[0] = sendCommand(( "AT+CIPSTART=\"TCP\",\"" + (string) ip + "\"," + (string) portstr ).c_str(), "OK", NULL, 10000);
-                break;
-            default:
-                ERR("Default hit for starting connection, this shouldnt be possible!!");
-                break;
-        }
-        check[1] = sendCommand("AT+CIPMODE=1", "OK", NULL, 1000);// go into transparent mode
-        check[2] = sendCommand("AT+CIPSEND", ">", NULL, 1000);// go into transparent mode
-        // check that all commands were sucessful
-        if(check[0] and check[1] and check[2]) {
-            DBG("Data Mode\r\n");
-            state.cmdMode = false;
-            return true;
-        } else {
-            DBG("\r\nstartUDPTransparent Failed for ip:%s, port:%d\r\n",ip,port);
-            return false;
-        }
-    }
-    // Multi Connection Mode
-    else {
-        //TODO: impliment Multi Connection Mode
-        ERR("Not currently Supported!");
-        return false;
-        
-//        DBG("Start Multi Connection Mode");
-//        char portstr[5];
-//        char idstr[2];
-//        bool check [3] = {0};
-//        sprintf(idstr,"%d",id);
-//        sprintf(portstr, "%d", port);
-//        switch(type) {
-//            case ESP_UDP_TYPE : //UDP
-//                check[0] = sendCommand(( "AT+CIPSTART=" + (string) idstr + ",\"UDP\",\"" + (string) ip + "\"," + (string) portstr ).c_str(), "OK", NULL, 10000);
-//                break;
-//            case ESP_TCP_TYPE : //TCP
-//                check[0] = sendCommand(( "AT+CIPSTART=" + (string) idstr + ",\"TCP\",\"" + (string) ip + "\"," + (string) portstr ).c_str(), "OK", NULL, 10000);
-//                break;
-//            default:
-//                ERR("Default hit for starting connection, this shouldnt be possible!!");
-//                break;
-//        }
-    }
-}
-
-bool ESP8266::startUDP(char* ip, int port)
-{
-    char portstr[5];
-    sprintf(portstr, "%d", port);
-    sendCommand(( "AT+CIPSTART=\"UDP\",\"" + (string) ip + "\"," + (string) portstr ).c_str(), "OK", NULL, 10000);
-
-    sendCommand("AT+CIPMODE=1", "OK", NULL, 1000);// go into transparent mode
-    sendCommand("AT+CIPSEND", ">", NULL, 1000);// go into transparent mode
-    DBG("Data Mode\r\n");
-    state.cmdMode = false;
-
-    return true;
-}
-
-bool ESP8266::close()
-{
-    send("+++",3);
-    wait(1);
-    state.cmdMode = true;
-    sendCommand("AT+CIPCLOSE","OK", NULL, 10000);
-    return true;
-}
-
-bool ESP8266::disconnect()
-{
-    // if already disconnected, return
-    if (!state.associated)
-        return true;
-    // send command to quit AP
-    sendCommand("AT+CWQAP", "OK", NULL, 10000);
-    state.associated = false;
-    return true;
-}
-
-/*
-    Assuming Returned data looks like this:
-    +CIFSR:STAIP,"192.168.11.2"
-    +CIFSR:STAMAC,"18:fe:34:9f:3a:f5"
-    grabbing IP from first set of quotation marks
-*/
-char* ESP8266::getIPAddress()
-{
-    char result[30] = {0};
-    int check = 0;
-    check = sendCommand("AT+CIFSR", NULL, result, 1000);
-    //pc.printf("\r\nReceivedInfo for IP Command is: %s\r\n",result);
-    ip = ipString;
-    if(check) {
-        // Success
-        string resultString(result);
-        uint8_t pos1 = 0, pos2 = 0;
-        //uint8_t pos3 = 0, pos4 = 0;
-        pos1 = resultString.find("+CIFSR:STAIP");
-        pos1 = resultString.find('"',pos1);
-        pos2 = resultString.find('"',pos1+1);
-        //pos3 = resultString.find('"',pos2+1); //would find mac address
-        //pos4 = resultString.find('"',pos3+1);
-        strncpy(ipString,resultString.substr(pos1,pos2).c_str(),sizeof(ipString));
-        ipString[pos2 - pos1 +1] = 0; // null terminate string correctly.
-        DBG("IP: %s\r\n",ipString);
-        ip = ipString;
-
-    } else {
-        // Failure
-        DBG("getIPAddress() failed\r\n");
-        ip = NULL;
-    }
-    return ip;
-}
-
-bool ESP8266::gethostbyname(const char * host, char * ip)
-{
-    string h = host;
-    int nb_digits = 0;
-
-    // no dns needed
-    int pos = h.find(".");
-    if (pos != string::npos) {
-        string sub = h.substr(0, h.find("."));
-        nb_digits = atoi(sub.c_str());
-    }
-    //printf("substrL %s\r\n", sub.c_str());
-    if (count(h.begin(), h.end(), '.') == 3 && nb_digits > 0) {
-        strcpy(ip, host);
-        return true;
-    } else {
-        // dns needed, not currently available
-        ERR("gethostbyname(): DNS Not currently available, only use IP Addresses!");
-        return false;
-    }
-}
-
-void ESP8266::reset()
-{
-    reset_pin = 0;
-    wait(0.2);
-    reset_pin = 1;
-    wait(1);
-
-    //send("+++",3);
-    //wait(1);
-    state.cmdMode = true;
-    sendCommand("AT", "OK", NULL, 1000);
-    sendCommand("AT+RST", "ready", NULL, 10000);
-    state.associated = false;
-
-}
-
-bool ESP8266::reboot()
-{
-    reset();
-    return true;
-}
-
-void ESP8266::handler_rx(void)
-{
-    //read characters
-    char c;
-    while (wifi.readable()) {
-        c=wifi.getc();
-        buf_ESP8266.queue(c);
-        //if (state.cmdMode) pc.printf("%c",c); //debug echo, needs fast serial console to prevent UART overruns
-    }
-}
-
-void ESP8266::attach_rx(bool callback)
-{
-    if (!callback)
-        wifi.attach(NULL);
-    else
-        wifi.attach(this, &ESP8266::handler_rx);
-}
-
-int ESP8266::readable()
-{
-    return buf_ESP8266.available();
-}
-
-int ESP8266::writeable()
-{
-    return wifi.writeable();
-}
-
-char ESP8266::getc()
-{
-    char c=0;
-    while (!buf_ESP8266.available());
-    buf_ESP8266.dequeue(&c);
-    return c;
-}
-
-int ESP8266::putc(char c)
-{
-    while (!wifi.writeable() || wifi.readable()); //wait for echoed command characters to be read first
-    return wifi.putc(c);
-}
-
-void ESP8266::flush()
-{
-    buf_ESP8266.flush();
-}
-
-int ESP8266::send(const char * buf, int len)
-{
-    //TODO: need to add handler for data > 2048B, this is the max packet size of the ESP8266.
-    const char* bufptr=buf;
-    for(int i=0; i<len; i++) {
-        putc((int)*bufptr++);
-    }
-    return len;
-}
-
-bool ESP8266::sendCommand(const char * cmd, const char * ACK, char * res, int timeout)
-{
-    char read;
-    size_t found = string::npos;
-    string checking;
-    Timer tmr;
-    int result = 0;
-
-    DBG("will send: %s\r\n",cmd);
-
-    attach_rx(true);
-
-    //We flush the buffer
-    while (readable())
-        getc();
-
-    if (!ACK || !strcmp(ACK, "NO")) {
-        for (int i = 0; i < strlen(cmd); i++) {
-            result = (putc(cmd[i]) == cmd[i]) ? result + 1 : result;
-            wait(.005); // prevents stuck recieve ready (?) need to let echoed character get read first
-        }
-        putc(13); //CR
-        wait(.005); // wait for echo
-        putc(10); //LF
-
-    } else {
-        //We flush the buffer
-        while (readable())
-            getc();
-
-        tmr.start();
-        for (int i = 0; i < strlen(cmd); i++) {
-            result = (putc(cmd[i]) == cmd[i]) ? result + 1 : result;
-            wait(.005); // wait for echo
-        }
-        putc(13); //CR
-        wait(.005); // wait for echo
-        putc(10); //LF
-
-        while (1) {
-            if (tmr.read_ms() > timeout) {
-                //We flush the buffer
-                while (readable())
-                    getc();
-
-                DBG("check: %s\r\n", checking.c_str());
-
-                attach_rx(true);
-                return -1;
-            } else if (readable()) {
-                read = getc();
-                printf("%c",read); //debug echo
-                if ( read != '\r' && read != '\n') {
-                    checking += read;
-                    found = checking.find(ACK);
-                    if (found != string::npos) {
-                        wait(0.01);
-
-                        //We flush the buffer
-                        while (readable())
-                            read = getc();
-                        printf("%c",read); //debug echo
-                        break;
-                    }
-                }
-            }
-        }
-        DBG("check: %s\r\n", checking.c_str());
-
-        attach_rx(true);
-        return result;
-    }
-
-    //the user wants the result from the command (ACK == NULL, res != NULL)
-    if ( res != NULL) {
-        int i = 0;
-        Timer timeout;
-        timeout.start();
-        tmr.reset();
-        while (1) {
-            if (timeout.read() > 2) {
-                if (i == 0) {
-                    res = NULL;
-                    break;
-                }
-                res[i] = '\0';
-                DBG("user str 1: %s\r\n", res);
-
-                break;
-            } else {
-                if (tmr.read_ms() > 300) {
-                    res[i] = '\0';
-                    DBG("user str: %s\r\n", res);
-
-                    break;
-                }
-                if (readable()) {
-                    tmr.start();
-                    read = getc();
-
-                    // we drop \r and \n
-                    if ( read != '\r' && read != '\n') {
-                        res[i++] = read;
-                    }
-                }
-            }
-        }
-        DBG("user str: %s\r\n", res);
-    }
-
-    //We flush the buffer
-    while (readable())
-        getc();
-
-    attach_rx(true);
-    DBG("result: %d\r\n", result)
-    return result;
-}
+/* Copyright (C) 2012 mbed.org, MIT License
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+ * and associated documentation files (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge, publish, distribute,
+ * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or
+ * substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include "mbed.h"
+#include "ESP8266.h"
+#include "Endpoint.h"
+#include <string>
+#include <algorithm>
+
+//Debug is disabled by default
+#if 0
+#define DBG(x, ...)  printf("[ESP8266 : DBG]"x" \t[%s,%d]\r\n", ##__VA_ARGS__,__FILE__,__LINE__); 
+#define WARN(x, ...) printf("[ESP8266 : WARN]"x" \t[%s,%d]\r\n", ##__VA_ARGS__,__FILE__,__LINE__); 
+#define ERR(x, ...)  printf("[ESP8266 : ERR]"x" \t[%s,%d]\r\n", ##__VA_ARGS__,__FILE__,__LINE__); 
+#define INFO(x, ...) printf("[ESP8266 : INFO]"x" \t[%s,%d]\r\n", ##__VA_ARGS__,__FILE__,__LINE__); 
+#ifndef ESP8266_ECHO
+#define ESP8266_ECHO 1
+#endif
+#else
+#define DBG(x, ...)
+#define WARN(x, ...)
+#define ERR(x, ...)
+#define INFO(x, ...)
+#endif
+
+
+ESP8266 *ESP8266::_inst;
+
+ESP8266::ESP8266(PinName tx, PinName rx, PinName reset, int baud, int timeout) :
+        _serial(tx, rx), _reset_pin(reset) {
+    INFO("Initializing ESP8266 object");
+    
+    _baud = baud;
+    _timeout = timeout;
+    
+    _serial.baud(_baud);
+    
+    _inst = this;
+}
+
+bool ESP8266::reset() {
+    _reset_pin = 0;
+    wait_us(20);
+    _reset_pin = 1;
+    
+    // Send reboot command in case reset is not connected
+    return command("\x03\r\n" "node.restart()") && execute();
+}
+
+bool ESP8266::init() {
+    // Reset device to clear state
+    return reset();
+}
+
+bool ESP8266::connect(const char *ssid, const char *phrase) {
+    // Configure as station with passed ssid and passphrase
+    if (!(command("wifi.setmode(wifi.STATION);") &&
+          command("wifi.sta.config('") &&
+          command(ssid) &&
+          command("','") &&
+          command(phrase) &&
+          command("')") &&
+          execute()))
+        return false;
+        
+    // Wait for an IP address
+    // TODO WISHLIST make this a seperate check so it can run asynch?
+    Timer timer;
+    timer.start();
+    
+    while (true) {
+        int ip_len = 15;
+        
+        if (!(command("ip=wifi.sta.getip();") &&
+              command("print(ip)") &&
+              execute(_ip, &ip_len)))
+            return false;
+        
+        _ip[ip_len] = 0;
+        
+        if (strcmp(_ip, "nil") != 0)
+            return true;
+        
+        if (timer.read_ms() > _timeout)
+            return false;
+    }
+}
+
+bool ESP8266::disconnect() {
+    int ip_len = 15;
+    
+    if (!(command("wifi.sta.disconnect();") &&
+          command("ip=wifi.sta.getip();") &&
+          command("print(ip)") &&
+          execute(_ip, &ip_len)))
+        return false;
+        
+    _ip[ip_len] = 0;
+    
+    return (strcmp(_ip, "nil") == 0);
+}
+
+bool ESP8266::is_connected() {
+    return (strcmp(_ip, "nil") != 0);
+}
+
+bool ESP8266::open(bool type, char* ip, int port, int id) {
+    // Create the actual connection
+    if (!(command("c=net.createConnection(") &&
+          command(type ? "net.TCP" : "net.UDP") &&
+          command(")") &&
+          execute()))
+        return false;
+        
+    // Setup handlers for connecting and disconnecting
+    if (!(command("cc=nil;") &&
+          command("c:on('connection',function() cc=true end);") &&
+          command("c:on('disconnection',function() cc=false end)") &&
+          execute()))
+        return false;
+    
+    // Setup functions for sending and recieving characters on the esp side
+    if (!(command("cm='';") &&
+          command("function cs(n) c:send(n) end;") &&
+          command("function cr(n) print(cm:sub(1,n)); cm=cm:sub(n+1,-1) end;") && 
+          command("function ca() print(#cm) end;") &&
+          command("c:on('receive',function(c,n) cm=cm..n end)") && 
+          execute()))
+        return false;
+    
+    // Convert port to a string    
+    char port_buf[16];
+    sprintf(port_buf, "%d", port);
+        
+    // Connect to the ip address
+    if (!(command("c:connect(") &&
+          command(port_buf) &&
+          command(",'") &&
+          command(ip) &&
+          command("')") &&
+          execute()))
+        return false;
+        
+    // Wait for it to connect
+    // TODO WISHLIST make this a seperate check so it can run asynch?
+    Timer timer;
+    timer.start();
+    
+    while (true) {
+        char con_buf[5];
+        int con_len = 5;
+        
+        if (!(command("print(cc)") && execute(con_buf, &con_len)))
+            return false;
+        
+        if (memcmp(con_buf, "true", con_len) == 0)
+            return true;
+        else if (memcmp(con_buf, "false", con_len) == 0)
+            return false;
+        
+        if (timer.read_ms() > _timeout)
+            return false;
+    }
+}
+    
+bool ESP8266::close() {
+    return command("c:close();" "c=nil") && execute();
+}
+
+bool ESP8266::send(const char *buffer, int len) {
+    if (!(command("cs('") &&
+          payload(buffer, len) &&
+          command("')") &&
+          execute()))
+        return false;
+    
+    return true;
+}
+
+bool ESP8266::recv(char *buffer, int *len) {
+    char len_buf[16];
+    sprintf(len_buf, "%d", *len);
+    
+    if (!(command("cr(") &&
+          command(len_buf) &&
+          command(")") && 
+          execute(buffer, len)))
+        return false;
+    
+    return true;
+}
+
+int ESP8266::putc(char c) {
+    char buffer[1] = { c };
+    
+    return send(buffer, 1) ? 0 : -1;
+}
+
+int ESP8266::getc() {
+    char buffer[1];
+    
+    while (true) {
+        int len = 1;
+        
+        if (!recv(buffer, &len))
+            return -1;
+            
+        if (len > 0)
+            break;
+    }
+    
+    return buffer[0];
+}
+
+int ESP8266::writeable() {
+    // Always succeeds since message can be temporarily stored on the esp
+    return 1;
+}
+
+int ESP8266::readable() {
+    char count_buf[16];
+    int count_len = 15;
+    
+    if (!(command("ca()") && execute(count_buf, &count_len)))
+        return false;
+        
+    count_buf[count_len] = 0;
+    return atoi(count_buf);
+}   
+
+const char *ESP8266::getIPAddress() {
+    if (strcmp(_ip, "nil") != 0)
+        return _ip;
+    else
+        return 0;
+}
+
+bool ESP8266::getHostByName(const char *host, char *ip) {
+    if (!(command("dh='") && 
+          command(host) &&
+          command("';") &&
+          command("dn=nil;") &&
+          command("if dh:match('%d+.%d+.%d+.%d+') then ") &&
+          command("dn=dh ") &&
+          command("else ") &&
+          command("dc=net.createConnection(net.TCP);") &&
+          command("dc:dns(dh,function(dc,ip) dn=ip end);") &&
+          command("dc=nil ") &&
+          command("end") &&
+          execute()))
+        return false;
+
+    // Wait for a response
+    Timer timer;
+    timer.start();
+    
+    while (true) {
+        int ip_len = 15;
+        
+        if (!(command("print(dn)") && execute(ip, &ip_len)))
+            return false;
+            
+        ip[ip_len] = 0;
+            
+        if (strcmp(ip, "nil") != 0)
+            return true;
+        
+        if (timer.read_ms() > _timeout)
+            return false;
+    }
+}
+
+int ESP8266::serialgetc() {
+    Timer timer;
+    timer.start();
+    
+    while (true) {
+        if (_serial.readable()) {
+            char c = _serial.getc();
+#ifdef ESP8266_ECHO
+            printf("%c", c);
+#endif
+            return c;
+        }
+            
+        if (timer.read_ms() > _timeout)
+            return -1;
+    }
+}
+
+int ESP8266::serialputc(char c) {
+    Timer timer;
+    timer.start();
+    
+    while (true) {
+        if (_serial.writeable())
+            return _serial.putc(c);
+            
+        if (timer.read_ms() > _timeout)
+            return -1;
+    }
+}
+
+bool ESP8266::discardEcho() {
+    while (true) {
+        int c = serialgetc();
+        
+        if (c < 0)
+            return false;
+        else if (c == '>')
+            return true;
+    }
+}
+
+bool ESP8266::command(const char *cmd) {
+    DBG("command sent:\t %s", cmd);
+    
+    for (int i = 0; cmd[i]; i++) {
+        if (serialputc(cmd[i]) < 0)
+            return false;
+    }
+    
+    return true;
+}
+
+bool ESP8266::payload(const char *data, int len) {
+    for (int i = 0; i < len; i++) {
+        int a = data[i] / 100;
+        int b = (data[i] - a*100) / 10;
+        int c = (data[i] - a*100 - b*10);
+        
+        if (serialputc('\\') < 0 ||
+            serialputc(a + '0') < 0 ||
+            serialputc(b + '0') < 0 ||
+            serialputc(c + '0') < 0)
+            return false;
+    }
+    
+    return true;
+}   
+
+bool ESP8266::execute(char *resp_buf, int *resp_len) {
+    // Finish command with a newline
+    if (serialputc('\r') < 0 || serialputc('\n') < 0)
+        return false;
+        
+    // Drop echoed string
+    while (true) {
+        int c = serialgetc();
+        
+        if (c < 0)
+            return false;
+        else if (c == '\n')
+            break;
+    }
+        
+    // Read in response if any
+    if (resp_buf && resp_len) {
+        int i;
+        
+        for (i = 0; i < *resp_len; i++) {
+            int c = serialgetc();
+                
+            if (c < 0)
+                return false;
+            
+            if (c == '\r') {
+                *resp_len = i;
+                break;
+            }
+            
+            resp_buf[i] = c;
+        }
+        
+        DBG("command response:\t %.*s", *resp_len, resp_buf);
+    }
+    
+    // Flush to next prompt
+    return discardEcho();
+}