diff --git a/camdriver.c b/camdriver.c
new file mode 100644
index 0000000..0a70b05
--- /dev/null
+++ b/camdriver.c
@@ -0,0 +1,223 @@
+/* 
+ * The MIT License (MIT)
+ * 
+ * Copyright (c) 2016 Johan Kanflo (github.com/kanflo)
+ * 
+ * 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 <arducam.h>
+#include <unistd.h>
+#include <string.h>
+#include <http_upload.h>
+#include "timeutils.h"
+#include "camdriver.h"
+
+
+#define MIN(a,b) (((a)<(b))?(a):(b))
+#define MAX(a,b) (((a)>(b))?(a):(b))
+
+#define BUF_SIZE (200)
+static char buffer[BUF_SIZE];
+
+bool arducam_setup(void)
+{
+    uint8_t vid, pid, temp;
+
+    arducam(smOV2640);
+    // Check if the ArduCAM SPI bus is OK
+    arducam_write_reg(ARDUCHIP_TEST1, 0x55);
+    temp = arducam_read_reg(ARDUCHIP_TEST1);
+    if(temp != 0x55) {
+        printf("SPI interface error (got 0x%02x)!\n", temp);
+        return false;
+    }
+
+    // Change MCU mode
+//    arducam_write_reg(ARDUCHIP_MODE, 0x00);
+
+    // Check if the camera module type is OV2640
+    arducam_i2c_read(OV2640_CHIPID_HIGH, &vid);
+    arducam_i2c_read(OV2640_CHIPID_LOW, &pid);
+    if((vid != 0x26) || (pid != 0x42)) {
+        printf("Error: cannot find OV2640 module (got 0x%02x, 0x%02x)\n", vid, pid);
+        return false;
+    } else {
+        printf("OV2640 detected\n");
+    }
+    printf("Setting JPEG\n");
+    arducam_set_format(fmtJPEG);
+    printf("Init\n");
+    arducam_init(); // Note to self. Must call set_format before init.
+    printf("Setting size\n");
+    arducam_set_jpeg_size(sz640x480);
+    // Allow for auto exposure loop to adapt to after resolution change
+    printf("Autoexposure working...\n");
+    delay_ms(1000);
+    printf("Done...\n");
+    return true;
+}
+
+bool arducam_capture(void)
+{
+    uint8_t temp;
+    uint32_t start_time = systime_ms();
+
+//    arducam_flush_fifo(); // TODO: These are the same
+    arducam_clear_fifo_flag(); // TODO: These are the same
+    printf("Start capture\n");
+    arducam_start_capture();
+    temp = arducam_read_reg(ARDUCHIP_TRIG);
+    if (!temp) {
+        printf("Failed to start capture!\n");
+        return false;
+    } else {
+        while (!(arducam_read_reg(ARDUCHIP_TRIG) & CAP_DONE_MASK)) {
+            delay_ms(1);
+        }
+        printf("Capture done after %ums\n", systime_ms()-start_time);
+    }
+    return true;
+}
+
+void arudcam_fifo_to_socket(int client_sock)
+{
+    uint8_t temp, temp_last = 0, buf_idx = 0;
+    uint32_t fifo_size, bytes_read, start_time = systime_ms();
+
+    fifo_size = (arducam_read_reg(0x44) << 16) | (arducam_read_reg(0x43) << 8) | (arducam_read_reg(0x42));
+    printf("%u bytes in fifo, according to camera\n", fifo_size); 
+
+    bytes_read = 1;
+    temp = arducam_read_fifo();
+//    printf("%02x ", temp);
+    buffer[buf_idx++] = temp;
+    // Read JPEG data from FIFO
+    while((temp != 0xd9) | (temp_last != 0xff)) {
+        temp_last = temp;
+        temp = arducam_read_fifo();
+        buffer[buf_idx++] = temp;
+        if (client_sock && buf_idx == BUF_SIZE) {
+            int res = write(client_sock, buffer, buf_idx);
+            if (res < 0) {
+                printf("\nERROR: write returned %d\n", res);
+                break;
+            }
+            buf_idx = 0;
+        }
+        bytes_read++;
+        if (bytes_read > fifo_size) {
+            printf("Fifo error\n");
+            break;
+        }
+    }
+    if (client_sock && buf_idx > 0) {
+        int res =  write(client_sock, buffer, buf_idx);
+        (void) res;
+    }
+    printf("Done, read %u bytes in %ums\n", bytes_read, systime_ms()-start_time);
+}
+
+void arudcam_fifo_to_devnull()
+{
+    uint8_t temp, temp_last = 0, buf_idx = 0;
+    uint32_t fifo_size, bytes_read, start_time = systime_ms();
+
+    fifo_size = (arducam_read_reg(0x44) << 16) | (arducam_read_reg(0x43) << 8) | (arducam_read_reg(0x42));
+    printf("%u bytes in fifo, according to camera\n", fifo_size); 
+
+    bytes_read = 1;
+    temp = arducam_read_fifo();
+//    printf("%02x ", temp);
+    buffer[buf_idx++] = temp;
+    // Read JPEG data from FIFO
+    while((temp != 0xd9) | (temp_last != 0xff)) {
+        temp_last = temp;
+        temp = arducam_read_fifo();
+        bytes_read++;
+        if (bytes_read > fifo_size) {
+            printf("Fifo error\n");
+            break;
+        }
+    }
+    printf("Done, read %u bytes in %ums\n", bytes_read, systime_ms()-start_time);
+}
+
+void arudcam_upload_fifo(char *host, uint16_t port)
+{
+    uint8_t temp, temp_last = 0, buf_idx = 0;
+    uint32_t http_res, fifo_size, bytes_read, start_time = systime_ms();
+
+    fifo_size = (arducam_read_reg(0x44) << 16) | (arducam_read_reg(0x43) << 8) | (arducam_read_reg(0x42));
+    printf("%u bytes in fifo, according to camera\n", fifo_size); 
+
+    do {
+        printf("Connecting to server...\n");
+        if (!upload_connect(host, port)) {
+            printf("Failed to connect to %s:%d\n", host, port);
+            break;
+        }
+
+        printf(" Connected\n");
+        if (!upload_begin("/", "image.jpg", fifo_size)) {
+            printf("Failed to upload\n");
+            break;
+        }
+        printf("Uploading...\n");
+
+        bytes_read = 1;
+        temp = arducam_read_fifo();
+        buffer[buf_idx++] = temp;
+        // Read JPEG data from FIFO
+        while((temp != 0xd9) | (temp_last != 0xff)) {
+            temp_last = temp;
+            temp = arducam_read_fifo();
+            buffer[buf_idx++] = temp;
+            if (buf_idx == BUF_SIZE) {
+                if (!upload_data(buffer, buf_idx)) {
+                    printf("Data upload failed\n");
+                    break;
+                }
+                buf_idx = 0;
+            }
+            bytes_read++;
+            if (bytes_read > fifo_size) {
+                printf("Fifo error\n");
+                break;
+            }
+        }
+        if (buf_idx > 0) {
+            (void) upload_data(buffer, buf_idx);
+        }
+        if (bytes_read < fifo_size) {
+            printf("Padding upload data with %d bytes\n", fifo_size - bytes_read);
+            memset(buffer, 0, BUF_SIZE);
+            while (bytes_read < fifo_size) {
+                uint32_t diff = fifo_size - bytes_read;
+                (void) upload_data(buffer, MIN(diff, BUF_SIZE));
+                bytes_read += MIN(diff, BUF_SIZE);
+            }
+        }
+        printf("Finishing upload\n");
+        http_res = upload_finish();
+        printf("Closing\n");
+        upload_close();
+        printf("Done, read %u bytes in %ums, server responded with %d\n", bytes_read, systime_ms()-start_time, http_res);
+    } while(0);
+}
diff --git a/camdriver.h b/camdriver.h
new file mode 100644
index 0000000..9dd351f
--- /dev/null
+++ b/camdriver.h
@@ -0,0 +1,47 @@
+/* 
+ * The MIT License (MIT)
+ * 
+ * Copyright (c) 2016 Johan Kanflo (github.com/kanflo)
+ * 
+ * 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.
+ */
+
+#ifndef _CAMDRIVER_H_
+#define _CAMDRIVER_H_
+
+#define OV2640_CHIPID_HIGH  0x0A
+#define OV2640_CHIPID_LOW   0x0B
+
+// Setup ArducamMini, return true if success
+bool arducam_setup(void);
+
+// Capture, return true if success
+bool arducam_capture(void);
+
+// Read FIFO and write to client socket
+void arudcam_fifo_to_socket(int client_sock);
+
+// Read FIFO and write to, well, nowhere
+void arudcam_fifo_to_devnull(void);
+
+// Read FIFO and hyyp upload to host:port
+void arudcam_upload_fifo(char *host, uint16_t port);
+
+#endif // _CAMDRIVER_H_
+
diff --git a/capture.c b/capture.c
index e5b8f16..880f764 100644
--- a/capture.c
+++ b/capture.c
@@ -38,6 +38,9 @@
 #include <espressif/sdk_private.h>
 #include <sys/types.h>
 #include <unistd.h>
+#include "camdriver.h"
+#include "cli.h"
+#include "timeutils.h"
 
 #include <ssid_config.h>
 
@@ -48,116 +51,10 @@
 
 #define ARDUCAM_PWR  (15) // Arducam Mini power enable
 
-
-#define OV2640_CHIPID_HIGH  0x0A
-#define OV2640_CHIPID_LOW   0x0B
-
-#define delay_ms(t) vTaskDelay((t) / portTICK_RATE_MS)
-#define systime_ms() (xTaskGetTickCount()*portTICK_RATE_MS)
-#define TIME_MARKER "[%8u] "
-
 #define BUF_SIZE (200)
 static char buffer[BUF_SIZE];
 
 
-static bool arducam_setup(void)
-{
-    uint8_t vid, pid, temp;
-
-    arducam(smOV2640);
-    printf("portTICK_RATE_MS is at %u\n", portTICK_RATE_MS);
-    // Check if the ArduCAM SPI bus is OK
-    arducam_write_reg(ARDUCHIP_TEST1, 0x55);
-    temp = arducam_read_reg(ARDUCHIP_TEST1);
-    if(temp != 0x55) {
-        printf(TIME_MARKER "SPI interface error (got 0x%02x)!\n", systime_ms(), temp);
-        return false;
-    }
-
-    // Change MCU mode
-//    arducam_write_reg(ARDUCHIP_MODE, 0x00);
-
-    // Check if the camera module type is OV2640
-    arducam_i2c_read(OV2640_CHIPID_HIGH, &vid);
-    arducam_i2c_read(OV2640_CHIPID_LOW, &pid);
-    if((vid != 0x26) || (pid != 0x42)) {
-        printf(TIME_MARKER "Error: cannot find OV2640 module (got 0x%02x, 0x%02x)\n", systime_ms(), vid, pid);
-        return false;
-    } else {
-        printf(TIME_MARKER "OV2640 detected\n", systime_ms());
-    }
-    printf(TIME_MARKER "Setting JPEG\n", systime_ms());
-    arducam_set_format(fmtJPEG);
-    printf(TIME_MARKER "Init\n", systime_ms());
-    arducam_init(); // Note to self. Must call set_format before init.
-    printf(TIME_MARKER "Setting size\n", systime_ms());
-    arducam_set_jpeg_size(sz640x480);
-    // Allow for auto exposure loop to adapt to after resolution change
-    printf(TIME_MARKER "Autoexposure working...\n", systime_ms());
-    delay_ms(1000);
-    printf(TIME_MARKER "Done...\n", systime_ms());
-    return true;
-}
-
-static bool arducam_capture(void)
-{
-    uint8_t temp;
-    uint32_t start_time = systime_ms();
-
-//    arducam_flush_fifo(); // TODO: These are the same
-    arducam_clear_fifo_flag(); // TODO: These are the same
-    printf(TIME_MARKER "Start capture\n", systime_ms());
-    arducam_start_capture();
-    temp = arducam_read_reg(ARDUCHIP_TRIG);
-    if (!temp) {
-        printf(TIME_MARKER "Failed to start capture!\n", systime_ms());
-        return false;
-    } else {
-        while (!(arducam_read_reg(ARDUCHIP_TRIG) & CAP_DONE_MASK)) {
-            delay_ms(1);
-        }
-        printf(TIME_MARKER "Capture done after %ums\n", systime_ms(), systime_ms()-start_time);
-    }
-    return true;
-}
-
-static void arudcam_fifo_read(int client_sock)
-{
-    uint8_t temp, temp_last = 0, buf_idx = 0;
-    uint32_t fifo_size, bytes_read, start_time = systime_ms();
-
-    fifo_size = (arducam_read_reg(0x44) << 16) | (arducam_read_reg(0x43) << 8) | (arducam_read_reg(0x42));
-    printf(TIME_MARKER "%u bytes in fifo, according to camera\n", systime_ms(), fifo_size); 
-
-    bytes_read = 1;
-    temp = arducam_read_fifo();
-//    printf("%02x ", temp);
-    buffer[buf_idx++] = temp;
-    // Read JPEG data from FIFO
-    while((temp != 0xd9) | (temp_last != 0xff)) {
-        temp_last = temp;
-        temp = arducam_read_fifo();
-        buffer[buf_idx++] = temp;
-        if (client_sock && buf_idx == BUF_SIZE) {
-            int res = write(client_sock, buffer, buf_idx);
-            if (res < 0) {
-                printf("\nERROR: write returned %d\n", res);
-                break;
-            }
-            buf_idx = 0;
-        }
-        bytes_read++;
-        if (bytes_read > fifo_size) {
-            printf("[%8u] Fifo error\n", systime_ms());
-            break;
-        }
-    }
-    if (client_sock && buf_idx > 0) {
-        int res =  write(client_sock, buffer, buf_idx);
-        (void) res;
-    }
-    printf("[%8u] Done, read %u bytes in %ums\n", systime_ms(), bytes_read, systime_ms()-start_time);
-}
 
 #ifndef CONFIG_NO_PIR
 extern uint16_t sdk_system_adc_read(void);
@@ -165,7 +62,7 @@ extern uint16_t sdk_system_adc_read(void);
 void pir_task(void *p)
 {
     delay_ms(1000);
-    printf(TIME_MARKER "PIR task\n", systime_ms());
+    printf("PIR task\n");
 
     uint16_t threshold = 500;
 //    uint8_t pir_pin = 16;
@@ -178,7 +75,7 @@ void pir_task(void *p)
         uint32_t adc = sdk_system_adc_read();
         bool new_state = adc > threshold;
         if (new_state != old_state) {
-            printf(TIME_MARKER "%s [%u]\n", systime_ms(), new_state ? "Motion" : "No motion", adc); 
+            printf("%s [%u]\n", new_state ? "Motion" : "No motion", adc); 
             old_state = new_state;
         }
         delay_ms(10);
@@ -186,7 +83,7 @@ void pir_task(void *p)
 #if 0
 //        gpio_write(pir_pin, old_state);
         __digitalWrite(pir_pin, old_state);
-        printf(TIME_MARKER "%s\n", systime_ms(), old_state ? "On" : "Off"); 
+        printf("%s\n", old_state ? "On" : "Off"); 
         old_state = !old_state;
         delay_ms(5000);
 #endif
@@ -200,7 +97,7 @@ void server_task(void *p)
 
     // Power cycle the the camera
     gpio_enable(ARDUCAM_PWR, GPIO_OUTPUT);
-    printf(TIME_MARKER "Camera power cycle\n", systime_ms());
+    printf("Camera power cycle\n");
     gpio_write(ARDUCAM_PWR, 1);
     delay_ms(250);
     gpio_write(ARDUCAM_PWR, 0);
@@ -208,7 +105,7 @@ void server_task(void *p)
 
     camera_ok = arducam_setup();
     if (!camera_ok) {
-        printf(TIME_MARKER "Camera init failed!\n", systime_ms()); 
+        printf("Camera init failed!\n"); 
     }
 
     while (1) {
@@ -224,19 +121,19 @@ void server_task(void *p)
 
         do {
             if (-1 == (server_sock = socket(AF_INET, SOCK_STREAM, 0))) {
-                printf(TIME_MARKER "Socket error\n", systime_ms());
+                printf("Socket error\n");
                 break;
             }
 
             if (-1 == bind(server_sock, (struct sockaddr *)(&server_addr), sizeof(struct sockaddr))) {
-                printf(TIME_MARKER "bind fail\n", systime_ms());
+                printf("bind fail\n");
                 break;
             }
 
-            printf(TIME_MARKER " bind port: %d\n", systime_ms(), ntohs(server_addr.sin_port));
+            printf(" bind port: %d\n", ntohs(server_addr.sin_port));
 
             if (-1 == listen(server_sock, 5)) {
-                printf(TIME_MARKER "listen fail\n", systime_ms());
+                printf("listen fail\n");
                 break;
             }
 
@@ -244,15 +141,15 @@ void server_task(void *p)
 
             for (;;) {
                 if ((client_sock = accept(server_sock, (struct sockaddr *) &client_addr, &sin_size)) < 0) {
-                    printf(TIME_MARKER "accept fail\n", systime_ms());
+                    printf("accept fail\n");
                     continue;
                 }
 
-                printf(TIME_MARKER "Client from %s:%d\n", systime_ms(), inet_ntoa(client_addr.sin_addr), htons(client_addr.sin_port));
+                printf("Client from %s:%d\n", inet_ntoa(client_addr.sin_addr), htons(client_addr.sin_port));
 
                 int opt = 50;
                 if (lwip_setsockopt(client_sock, SOL_SOCKET, SO_RCVTIMEO, &opt, sizeof(int)) < 0) {
-                    printf(TIME_MARKER "failed to set timeout on socket\n", systime_ms());
+                    printf("failed to set timeout on socket\n");
                 }
                 while ((recbytes = read(client_sock, buffer, BUF_SIZE)) > 0) {
                     buffer[recbytes] = 0;
@@ -269,14 +166,14 @@ void server_task(void *p)
                             if (success) {
                                 break;
                             } else {
-                                printf(TIME_MARKER "Capture retry\n", systime_ms()); 
+                                printf("Capture retry\n"); 
                                 delay_ms(10); // Arbitrary value
                             }
                         }
                         if (success) {
-                            printf(TIME_MARKER "Reading fifo\n", systime_ms()); 
-                            arudcam_fifo_read(client_sock);
-                            printf("[%8u] ---\n", systime_ms());
+                            printf("Reading fifo\n"); 
+                            arudcam_fifo_to_socket(client_sock);
+                            printf("---\n");
                         }
                     }
                 }
@@ -294,6 +191,7 @@ void user_init(void)
     sdk_uart_div_modify(0, UART_CLK_FREQ / 115200);
     printf("\n\n\n");
     printf("SDK version:%s\n", sdk_system_get_sdk_version());
+    cli_init();
 
 #ifndef CONFIG_NO_WIFI
     struct sdk_station_config config = {
diff --git a/cli.c b/cli.c
new file mode 100644
index 0000000..0f9f8bb
--- /dev/null
+++ b/cli.c
@@ -0,0 +1,157 @@
+/* 
+ * The MIT License (MIT)
+ * 
+ * Copyright (c) 2016 Johan Kanflo (github.com/kanflo)
+ * 
+ * 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 <stdint.h>
+#include <sys/types.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <esp8266.h>
+#include <esp/uart.h>
+#include <stdio.h>
+#include <FreeRTOS.h>
+#include <task.h>
+#include <arducam.h>
+#include <http_upload.h>
+#include "camdriver.h"
+
+
+#define MAX_ARGC (10)
+
+static void cmd_capture(uint32_t argc, char *argv[])
+{
+    if (arducam_capture()) {
+        arudcam_fifo_to_devnull();
+    } else {
+        printf("Capture failed\n");
+    }
+}
+
+static void cmd_capture_upload(uint32_t argc, char *argv[])
+{
+    if (argc == 2) {
+        if (arducam_capture()) {
+            arudcam_upload_fifo(argv[1], 8000);
+        } else {
+            printf("Capture failed\n");
+        }
+    } else {
+        printf("Error: missing host ip.\n");
+    }
+}
+
+static void cmd_on(uint32_t argc, char *argv[])
+{
+    if (argc >= 2) {
+        for(int i=1; i<argc; i++) {
+            uint8_t gpio_num = atoi(argv[i]);
+            gpio_enable(gpio_num, GPIO_OUTPUT);
+            gpio_write(gpio_num, true);
+            printf("On %d\n", gpio_num);
+        }
+    } else {
+        printf("Error: missing gpio number.\n");
+    }
+}
+
+static void cmd_off(uint32_t argc, char *argv[])
+{
+    if (argc >= 2) {
+        for(int i=1; i<argc; i++) {
+            uint8_t gpio_num = atoi(argv[i]);
+            gpio_enable(gpio_num, GPIO_OUTPUT);
+            gpio_write(gpio_num, false);
+            printf("Off %d\n", gpio_num);
+        }
+    } else {
+        printf("Error: missing gpio number.\n");
+    }
+}
+
+static void cmd_help(uint32_t argc, char *argv[])
+{
+    printf("upload:<hostname>                     Capture and upload image to host\n");
+    printf("capture                               Capture image to '/dev/null'\n");
+    printf("on:<gpio number>[:<gpio number>]+     Set gpio to 1\n");
+    printf("off:<gpio number>[:<gpio number>]+    Set gpio to 0\n");
+    printf("\nExample:\n");
+    printf("  upload:172.16.3.107<enter> captures and uploads image to host running server.py\n");
+    printf("  on:0<enter> switches on gpio 0\n");
+    printf("  on:0:2:4<enter> switches on gpios 0, 2 and 4\n");
+}
+
+static void handle_command(char *cmd)
+{
+    char *argv[MAX_ARGC];
+    int argc = 1;
+    char *temp, *rover;
+    memset((void*) argv, 0, sizeof(argv));
+    argv[0] = cmd;
+    rover = cmd;
+    // Split string "<command> <argument 1> <argument 2>  ...  <argument N>"
+    // into argv, argc style
+    while(argc < MAX_ARGC && (temp = strstr(rover, ":"))) {
+        rover = &(temp[1]);
+        argv[argc++] = rover;
+        *temp = 0;
+    }
+
+    if (strlen(argv[0]) > 0) {
+        if (strcmp(argv[0], "help") == 0) cmd_help(argc, argv);
+        else if (strcmp(argv[0], "capture") == 0) cmd_capture(argc, argv);
+        else if (strcmp(argv[0], "upload") == 0) cmd_capture_upload(argc, argv);
+        else if (strcmp(argv[0], "on") == 0) cmd_on(argc, argv);
+        else if (strcmp(argv[0], "off") == 0) cmd_off(argc, argv);
+        else printf("Unknown command %s, try 'help'\n", argv[0]);
+    }
+}
+
+static void cammon_task(void *p)
+{
+    char ch;
+    char cmd[81];
+    int i = 0;
+    printf("\n\n\nWelcome to camera mon. Type 'help<enter>' for, well, help\n");
+    printf("%% ");
+    while(1) {
+        if (read(0, (void*)&ch, 1)) { // 0 is stdin
+            printf("%c", ch);
+            if (ch == '\n' || ch == '\r') {
+                cmd[i] = 0;
+                i = 0;
+                printf("\n");
+                handle_command((char*) cmd);
+                printf("%% ");
+            } else {
+                if (i < sizeof(cmd)) cmd[i++] = ch;
+            }
+        }
+    }
+}
+
+void cli_init()
+{
+    xTaskCreate(&cammon_task, (signed char *) "cammon_task", 256, NULL, 2, NULL);
+}
+
diff --git a/cli.h b/cli.h
new file mode 100644
index 0000000..94fc65c
--- /dev/null
+++ b/cli.h
@@ -0,0 +1,30 @@
+/* 
+ * The MIT License (MIT)
+ * 
+ * Copyright (c) 2016 Johan Kanflo (github.com/kanflo)
+ * 
+ * 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.
+ */
+
+#ifndef _CLI_H_
+#define _CLI_H_
+
+void cli_init(void);
+
+#endif // _CLI_H_
diff --git a/server.py b/server.py
new file mode 100755
index 0000000..a6adcf6
--- /dev/null
+++ b/server.py
@@ -0,0 +1,304 @@
+#!/usr/bin/env python
+
+"""
+
+Run this script on a host and try the 'upload:<host>' command on the Esparducam
+
+Simple HTTP Server With Upload.
+
+This module builds on BaseHTTPServer by implementing the standard GET
+and HEAD requests in a fairly straightforward manner.
+
+
+https://gist.githubusercontent.com/UniIsland/3346170/raw/059aca1d510c615df3d9fedafabac4d538ebe352/SimpleHTTPServerWithUpload.py
+
+Directory listing removed, GET returns 501
+
+"""
+
+
+__version__ = "0.1"
+__all__ = ["SimpleHTTPRequestHandler"]
+__author__ = "bones7456"
+__home_page__ = "http://li2z.cn/"
+
+import os
+import posixpath
+import BaseHTTPServer
+import urllib
+import cgi
+import shutil
+import mimetypes
+import re
+try:
+    from cStringIO import StringIO
+except ImportError:
+    from StringIO import StringIO
+from subprocess import call
+
+class SimpleHTTPRequestHandler(BaseHTTPServer.BaseHTTPRequestHandler):
+
+    """Simple HTTP request handler with GET/HEAD/POST commands.
+
+    This serves files from the current directory and any of its
+    subdirectories.  The MIME type for files is determined by
+    calling the .guess_type() method. And can reveive file uploaded
+    by client.
+
+    The GET/HEAD/POST requests are identical except that the HEAD
+    request omits the actual contents of the file.
+
+    """
+
+    server_version = "SimpleHTTPWithUpload/" + __version__
+
+#    def do_GET(self):
+#        """Serve a GET request."""
+#        f = self.send_head()
+#        if f:
+#            self.copyfile(f, self.wfile)
+#            f.close()
+#
+#    def do_HEAD(self):
+#        """Serve a HEAD request."""
+#        f = self.send_head()
+#        if f:
+#            f.close()
+
+    def do_POST(self):
+        """Serve a POST request."""
+        r, info = self.deal_post_data()
+        print r, info, "by: ", self.client_address
+        f = StringIO()
+        f.write('<!DOCTYPE html PUBLIC "-//W3C//DTD HTML 3.2 Final//EN">')
+        f.write("<html>\n<title>Upload Result Page</title>\n")
+        f.write("<body>\n<h2>Upload Result Page</h2>\n")
+        f.write("<hr>\n")
+        if r:
+            f.write("<strong>Success:</strong>")
+        else:
+            f.write("<strong>Failed:</strong>")
+        f.write(info)
+        f.write("<br><a href=\"%s\">back</a>" % self.headers['referer'])
+        f.write("<hr><small>Powerd By: bones7456, check new version at ")
+        f.write("<a href=\"http://li2z.cn/?s=SimpleHTTPServerWithUpload\">")
+        f.write("here</a>.</small></body>\n</html>\n")
+        length = f.tell()
+        f.seek(0)
+        self.send_response(200)
+        self.send_header("Content-type", "text/html")
+        self.send_header("Content-Length", str(length))
+        self.end_headers()
+        if f:
+            self.copyfile(f, self.wfile)
+            f.close()
+        
+    def deal_post_data(self):
+        boundary = self.headers.plisttext.split("=")[1]
+        remainbytes = int(self.headers['content-length'])
+        line = self.rfile.readline()
+        remainbytes -= len(line)
+        if not boundary in line:
+            return (False, "Content NOT begin with boundary")
+        line = self.rfile.readline()
+        remainbytes -= len(line)
+        fn = re.findall(r'Content-Disposition.*name="file"; filename="(.*)"', line)
+        if not fn:
+            return (False, "Can't find out file name...")
+        path = self.translate_path(self.path)
+        fn = os.path.join(path, "uploads/%s" % fn[0])
+        line = self.rfile.readline()
+        remainbytes -= len(line)
+        line = self.rfile.readline()
+        remainbytes -= len(line)
+        try:
+            out = open(fn, 'wb')
+        except IOError:
+            return (False, "Can't create file to write, do you have permission to write?")
+                
+        preline = self.rfile.readline()
+        remainbytes -= len(preline)
+        while remainbytes > 0:
+            line = self.rfile.readline()
+            remainbytes -= len(line)
+            if boundary in line:
+                preline = preline[0:-1]
+                if preline.endswith('\r'):
+                    preline = preline[0:-1]
+                out.write(preline)
+                out.close()
+                call(["open", fn])
+                return (True, "File '%s' upload success!" % fn)
+            else:
+                out.write(preline)
+                preline = line
+        return (False, "Unexpect Ends of data.")
+
+    def send_head(self):
+        """Common code for GET and HEAD commands.
+
+        This sends the response code and MIME headers.
+
+        Return value is either a file object (which has to be copied
+        to the outputfile by the caller unless the command was HEAD,
+        and must be closed by the caller under all circumstances), or
+        None, in which case the caller has nothing further to do.
+
+        """
+        path = self.translate_path(self.path)
+        f = None
+        if os.path.isdir(path):
+            if not self.path.endswith('/'):
+                # redirect browser - doing basically what apache does
+                self.send_response(301)
+                self.send_header("Location", self.path + "/")
+                self.end_headers()
+                return None
+            for index in "index.html", "index.htm":
+                index = os.path.join(path, index)
+                if os.path.exists(index):
+                    path = index
+                    break
+#            else:
+#                return self.list_directory(path)
+        ctype = self.guess_type(path)
+        try:
+            # Always read in binary mode. Opening files in text mode may cause
+            # newline translations, making the actual size of the content
+            # transmitted *less* than the content-length!
+            f = open(path, 'rb')
+        except IOError:
+            self.send_error(404, "File not found")
+            return None
+        self.send_response(200)
+        self.send_header("Content-type", ctype)
+        fs = os.fstat(f.fileno())
+        self.send_header("Content-Length", str(fs[6]))
+        self.send_header("Last-Modified", self.date_time_string(fs.st_mtime))
+        self.end_headers()
+        return f
+
+    def list_directory(self, path):
+        """Helper to produce a directory listing (absent index.html).
+
+        Return value is either a file object, or None (indicating an
+        error).  In either case, the headers are sent, making the
+        interface the same as for send_head().
+
+        """
+        try:
+            list = os.listdir(path)
+        except os.error:
+            self.send_error(404, "No permission to list directory")
+            return None
+        list.sort(key=lambda a: a.lower())
+        f = StringIO()
+        displaypath = cgi.escape(urllib.unquote(self.path))
+        f.write('<!DOCTYPE html PUBLIC "-//W3C//DTD HTML 3.2 Final//EN">')
+        f.write("<html>\n<title>Directory listing for %s</title>\n" % displaypath)
+        f.write("<body>\n<h2>Directory listing for %s</h2>\n" % displaypath)
+        f.write("<hr>\n")
+        f.write("<form ENCTYPE=\"multipart/form-data\" method=\"post\">")
+        f.write("<input name=\"file\" type=\"file\"/>")
+        f.write("<input type=\"submit\" value=\"upload\"/></form>\n")
+        f.write("<hr>\n<ul>\n")
+        for name in list:
+            fullname = os.path.join(path, name)
+            displayname = linkname = name
+            # Append / for directories or @ for symbolic links
+            if os.path.isdir(fullname):
+                displayname = name + "/"
+                linkname = name + "/"
+            if os.path.islink(fullname):
+                displayname = name + "@"
+                # Note: a link to a directory displays with @ and links with /
+            f.write('<li><a href="%s">%s</a>\n'
+                    % (urllib.quote(linkname), cgi.escape(displayname)))
+        f.write("</ul>\n<hr>\n</body>\n</html>\n")
+        length = f.tell()
+        f.seek(0)
+        self.send_response(200)
+        self.send_header("Content-type", "text/html")
+        self.send_header("Content-Length", str(length))
+        self.end_headers()
+        return f
+
+    def translate_path(self, path):
+        """Translate a /-separated PATH to the local filename syntax.
+
+        Components that mean special things to the local file system
+        (e.g. drive or directory names) are ignored.  (XXX They should
+        probably be diagnosed.)
+
+        """
+        # abandon query parameters
+        path = path.split('?',1)[0]
+        path = path.split('#',1)[0]
+        path = posixpath.normpath(urllib.unquote(path))
+        words = path.split('/')
+        words = filter(None, words)
+        path = os.getcwd()
+        for word in words:
+            drive, word = os.path.splitdrive(word)
+            head, word = os.path.split(word)
+            if word in (os.curdir, os.pardir): continue
+            path = os.path.join(path, word)
+        return path
+
+    def copyfile(self, source, outputfile):
+        """Copy all data between two file objects.
+
+        The SOURCE argument is a file object open for reading
+        (or anything with a read() method) and the DESTINATION
+        argument is a file object open for writing (or
+        anything with a write() method).
+
+        The only reason for overriding this would be to change
+        the block size or perhaps to replace newlines by CRLF
+        -- note however that this the default server uses this
+        to copy binary data as well.
+
+        """
+        shutil.copyfileobj(source, outputfile)
+
+    def guess_type(self, path):
+        """Guess the type of a file.
+
+        Argument is a PATH (a filename).
+
+        Return value is a string of the form type/subtype,
+        usable for a MIME Content-type header.
+
+        The default implementation looks the file's extension
+        up in the table self.extensions_map, using application/octet-stream
+        as a default; however it would be permissible (if
+        slow) to look inside the data to make a better guess.
+
+        """
+
+        base, ext = posixpath.splitext(path)
+        if ext in self.extensions_map:
+            return self.extensions_map[ext]
+        ext = ext.lower()
+        if ext in self.extensions_map:
+            return self.extensions_map[ext]
+        else:
+            return self.extensions_map['']
+
+    if not mimetypes.inited:
+        mimetypes.init() # try to read system mime.types
+    extensions_map = mimetypes.types_map.copy()
+    extensions_map.update({
+        '': 'application/octet-stream', # Default
+        '.py': 'text/plain',
+        '.c': 'text/plain',
+        '.h': 'text/plain',
+        })
+
+
+def test(HandlerClass = SimpleHTTPRequestHandler,
+         ServerClass = BaseHTTPServer.HTTPServer):
+    BaseHTTPServer.test(HandlerClass, ServerClass)
+
+if __name__ == '__main__':
+    test()
diff --git a/timeutils.h b/timeutils.h
new file mode 100644
index 0000000..4cdaa6d
--- /dev/null
+++ b/timeutils.h
@@ -0,0 +1,34 @@
+/* 
+ * The MIT License (MIT)
+ * 
+ * Copyright (c) 2016 Johan Kanflo (github.com/kanflo)
+ * 
+ * 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.
+ */
+
+#ifndef _TIMEUTILS_H_
+#define _TIMEUTILS_H_
+
+#include <FreeRTOS.h>
+#include <task.h>
+
+#define delay_ms(t) vTaskDelay((t) / portTICK_RATE_MS)
+#define systime_ms() (xTaskGetTickCount()*portTICK_RATE_MS)
+
+#endif // _TIMEUTILS_H_
diff --git a/uploads/server.py.uploads.images.to.here b/uploads/server.py.uploads.images.to.here
new file mode 100644
index 0000000..e69de29