1 files changed, 100 insertions, 65 deletions

diff --git a/cmd.c b/cmd.c
index 29bc8c9..74357bc 100644
--- a/cmd.c
+++ b/cmd.c
@@ -12,6 +12,7 @@
 #endif // USE_READLINE
 
 #include <sys/stat.h>
+#include <unistd.h>
 
 struct command;
 
@@ -153,93 +154,127 @@ cmd_read(int fd, const struct command *cmd, int argc, char **args)
 }
 
 static int
-cmd_write(int fd, const struct command *cmd, int argc, char **args)
+write_file(int fd, const char *pathname, uint8_t bank, uint16_t addr)
 {
-    int err;
-    char line[512];
+    int i, err;
+    uint16_t len = 0;
     uint8_t buf[MAX_PACKET_SIZE];
 
-    char *p;
+    FILE *fp;
+    off_t n = 0;
+    struct stat st;
 
-    uint8_t bank;
-    uint16_t address;
-    uint16_t length = 0;
-    const char *pathname = NULL;
+    if (!(fp = fopen(pathname, "r")) || (fstat(fileno(fp), &st) < 0))
+        goto file_error;
 
-    err = parse_args(cmd, argc, args, &bank, &address, &pathname);
-    if (err < 0)
-        return -1;
-
-    if (pathname) {
-        off_t n = 0;
-        FILE *fp;
-        struct stat st;
+    printf("Uploading %s :\n[  0%%]", pathname);
+    fflush(stdout);
 
-        if (!(fp = fopen(pathname, "r")) || (fstat(fileno(fp), &st) < 0))
-            goto file_error;
+    /*
+     * FIXME: fread MAX_PACKET_SIZE insted of 8
+     * FIXME: Add attempts to all variants
+     */
 
-        printf("Uploading %s :\n[  0%%]", pathname);
-        fflush(stdout);
+    // Split file into packets
+    while ((len = fread(buf, sizeof(uint8_t), 8, fp))) {
+        for (i = 0; i < 50 && (err = z_write(fd, bank, addr + n, len, buf)); i++)
+            usleep(1000000);
 
-        // FIXME: fread MAX_PACKET_SIZE insted of 8
-        // Split file into packets
-        while ((length = fread(buf, sizeof(uint8_t), 8, fp))) {
-            if ((err = z_write(fd, bank, address + n, length, buf)))
-                break;
+        if (err)
+            break;
 
-            n += length;
+        n += len;
 
-            if (param.show_progress) {
-                printf("\r[% 3d%%]", 100 * n / st.st_size);
-                fflush(stdout);
-            }
+        if (param.show_progress) {
+            printf("\r[% 3d%%]", 100 * n / st.st_size);
+            fflush(stdout);
         }
+    }
 
-        putchar('\n');
+    putchar('\n');
 
-        if (!err)
-            puts("Done!");
+    if (!err)
+        puts("Done!");
 
-        fclose(fp);
-    } else if (param.human_readable) {
-        do {
-            // TODO: Handle fgets error
-            fgets(line, LEN(line), stdin);
-            if (line && !strcmp("\n", line))
-                break;
+    fclose(fp);
 
-            p = strtok(line, " \n");
-            while (p && length < 10) {
-                errno = 0;
-                buf[length++] = strtoul(p, NULL, 16);
-                p = strtok(NULL, " \n");
-            }
+    return err;
 
-            if (p) {
-                fputs("WRITE: Input can't be longer than 256 bytes\n", stderr);
-                return -1;
-            }
-        } while (1);
+file_error:
+    perror("File could not be opened");
+    return -1;
+}
 
-        if ((err = z_write(fd, bank, address, length, buf))) {
-            print_error(err);
-            return err;
+static int
+write_repl(int fd, uint8_t bank, uint16_t addr)
+{
+    int err;
+    char line[512];
+    uint8_t buf[MAX_PACKET_SIZE];
+    uint16_t len = 0;
+
+    char *p;
+
+    do {
+        // TODO: Handle fgets error
+        fgets(line, LEN(line), stdin);
+        if (line && !strcmp("\n", line))
+            break;
+
+        p = strtok(line, " \n");
+        while (p && len < 10) {
+            errno = 0;
+            buf[len++] = strtoul(p, NULL, 16);
+            p = strtok(NULL, " \n");
         }
-    } else {
-        // FIXME: fread MAX_PACKET_SIZE insted of 8
-        // Split file into packets
-        while ((length = fread(buf, sizeof(uint8_t), 8, stdin))) {
-            if ((err = z_write(fd, bank, address, length, buf)))
-                break;
-            address += length;
+
+        if (p) {
+            fputs("WRITE: Input can't be longer than 256 bytes\n", stderr);
+            return -1;
         }
+    } while (1);
+
+    if ((err = z_write(fd, bank, addr, len, buf))) {
+        print_error(err);
+        return err;
     }
+}
 
-    return err;
+static int
+write_stdin(int fd, uint8_t bank, uint16_t addr)
+{
+    int err;
+    uint16_t len = 0;
+    uint8_t buf[MAX_PACKET_SIZE];
+    // FIXME: fread MAX_PACKET_SIZE insted of 8
+    // Split file into packets
+    while ((len = fread(buf, sizeof(uint8_t), 8, stdin))) {
+        if ((err = z_write(fd, bank, addr, len, buf)))
+            break;
+        addr += len;
+    }
+}
 
-file_error:
-    perror("File could not be opened");
-    return -1;
+static int
+cmd_write(int fd, const struct command *cmd, int argc, char **args)
+{
+    int err;
+    uint8_t bank;
+    uint16_t addr;
+    const char *pathname = NULL;
+
+    err = parse_args(cmd, argc, args, &bank, &addr, &pathname);
+    if (err < 0)
+        return -1;
+
+    if (pathname)
+        return write_file(fd, pathname, bank, addr);
+    else if (param.human_readable)
+        return write_repl(fd, bank, addr);
+    else
+        return write_stdin(fd, bank, addr);
+
+    return err;
 }
 
 static int