Diff: cellular/Cellular.cpp

Revision:

35:f28acb1be52d

Parent:

33:e04aa7c013c9

Child:

37:61373f492403

Child:

39:6e94520a3217

Child:

41:81d035fb0b6a

--- a/cellular/Cellular.cpp	Wed Dec 18 23:05:31 2013 +0000
+++ b/cellular/Cellular.cpp	Thu Dec 19 15:56:05 2013 +0000
@@ -212,23 +212,39 @@
 }
 
 bool Cellular::close() {
-    if(!socketOpened) {
-        printf("[ERROR] Socket is not open\r\n");
+    if(io == NULL) {
+        printf("[ERROR] MTSBufferedIO not set\r\n");
         return false;
     }
-    
+
+    if(!socketOpened) {
+        printf("[WARNING] Socket close() called, but socket was not open\r\n");
+        return true;
+    }
+
     if(!socketCloseable) {
         printf("[ERROR] Socket is not closeable\r\n");
         return false;
     }
     
-    //Build Escape Message => ETX
-    char buffer[2] = { 0x03, 0x00 };
-    Code code = sendBasicCommand(buffer, 1000, NONE);
-    if(code != OK) {
-        printf("[ERROR] Radio did not accept close socket command\r\n");
+    Timer tmr;
+    int timeout = 1000;
+    int written = 0;
+    tmr.start();
+    do {
+        written = io->write(ETX);
+        if(written < 0) {
+            printf("[ERROR] Failed to write to MTSBufferedIO\r\n");
+            return false;   
+        }
+        wait(0.05);
+    } while(tmr.read_ms() <= timeout && written != 1);
+
+    if(written != 1) {
+        printf("[ERROR] Timed out attempting to close socket\r\n");
         return false;
     }
+    
     socketOpened = false;
     return true;
 }
@@ -248,7 +264,7 @@
     if(timeout >= 0) {
         Timer tmr;
         tmr.start();
-        while (tmr.read_ms() <= timeout && bytesRead < max) {
+        do {
             int available = io->readable();
             if (available > 0) {
                 int size = MIN(available, max - bytesRead);
@@ -256,7 +272,7 @@
             } else {
                 wait(0.05);   
             }
-        }
+        } while (tmr.read_ms() <= timeout && bytesRead < max);
     } else {
         bytesRead = io->read(data, max);
     }
@@ -318,23 +334,77 @@
         return -1;
     }
     
+    //In order to avoid allocating another buffer, capture indices of
+    //characters to escape during write
+    int specialWritten = 0;
+    std::vector<int> vSpecial;
+    if(socketCloseable) {
+        for(int i = 0; i < length; i++) {
+            if(data[i] == ETX || data[i] == DLE) {
+                //Push back index of special characters
+                vSpecial.push_back(i);
+            }                   
+        }
+    }
+    
     int bytesWritten = 0;
-    
     if(timeout >= 0) {
         Timer tmr;
         tmr.start();
-        while (tmr.read_ms() <= timeout && bytesWritten < length) {
-            if (io->writeable()) {
-                if(io->write(*data) == 1) {
-                    data++;
-                    bytesWritten++;
+        do {
+            int capacity = io->writeable();
+            if (capacity > 0) {
+                if(specialWritten < vSpecial.size()) {
+                    //Check if current index is at a special character
+                    if(bytesWritten == vSpecial[specialWritten]) {
+                        if(capacity < 2) {
+                            //Requires at least two bytes of space
+                            wait(0.05);
+                            continue;   
+                        }
+                        //Ready to write special character    
+                        if(io->write(DLE)) {
+                            specialWritten++;
+                            if(io->write(data[bytesWritten])) {
+                                bytesWritten++;
+                            }
+                        } else {
+                            //Unable to write escape character, try again next round
+                            wait(0.05);
+                        }
+                    } else {
+                        //We want to write all the way up to the next special character    
+                        int relativeIndex = vSpecial[specialWritten] - bytesWritten;
+                        int size = MIN(capacity, relativeIndex);
+                        bytesWritten += io->write(&data[bytesWritten], size);
+                    }                    
+                } else {                
+                    int size = MIN(capacity, length - bytesWritten);
+                    bytesWritten += io->write(&data[bytesWritten], size);
                 }
             } else {
-                wait(0.05);
+                wait(0.05);   
+            }
+        } while (tmr.read_ms() <= timeout && bytesWritten < length);
+    } else {
+        for(int i = 0; i < vSpecial.size(); i++) {
+            //Write up to the special character, then write the special character
+            int size = vSpecial[i] - bytesWritten;
+            int currentWritten = io->write(&data[bytesWritten], size);
+            bytesWritten += currentWritten;
+            if(currentWritten != size) {
+                //Failed to write up to the special character.
+                return bytesWritten;
+            }
+            if(io->write(DLE) && io->write(data[bytesWritten])) {
+                bytesWritten++;
+            } else {
+                //Failed to write the special character.
+                return bytesWritten;
             }
         }
-    } else {
-        bytesWritten = io->write(data, length);
+    
+        bytesWritten = io->write(&data[bytesWritten], length - bytesWritten);
     }
     
     return bytesWritten;