Diff: BaseEmuISP.cpp

Revision:

1:4ff199bddbc1

Child:

2:e3c085ac77f1

diff -r 0d71b4e4d19b -r 4ff199bddbc1 BaseEmuISP.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BaseEmuISP.cpp	Tue Mar 08 12:03:48 2016 +0900
@@ -0,0 +1,252 @@
+// BaseEmuISP.cpp 2016/3/7
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdarg.h>
+#include "BaseEmuISP.h"
+
+void BaseEmuISP::Reset() {
+    mode = M_RESET;
+}
+
+void BaseEmuISP::Poll() {
+    int prev_mode = mode;
+    int result;
+    int c;
+    char buf[32];
+    switch(mode) {
+        case M_RESET:
+            echoFlag = true;
+            lockFlag = true;
+            mode = M_SYNC;
+            break;
+        case M_SYNC:
+            if (sync()) {
+                mode = M_CMD;
+            }
+            break;
+        case M_CMD:
+            if (line_proc()) {
+                result = cmd(line.c_str());
+                snprintf(buf, sizeof(buf), "%d", result);
+                putln(buf);
+                line.clear();
+            }
+            break;
+        case M_CMD_W_DATA:
+            if (count > 0) {
+                c = Getch();
+                if (c != (-1)) {
+                    if (echoFlag) {
+                        Putch(c);
+                    }
+                    WriteData(addr++, c);
+                    count--;
+                }
+            } else {
+                mode = M_CMD;
+            }
+            break;
+        case M_CMD_R_DATA:
+            if (count > 0) {
+                c = ReadData(addr++);
+                Putch(c);
+                count--;
+            } else {
+                mode = M_CMD;
+            }
+            break;
+        case M_CMD_J:
+            snprintf(buf, sizeof(buf), "%d", Part());
+            putln(buf);
+            mode = M_CMD;
+            break;
+    }
+    if (prev_mode != mode) {
+        seq = 0;
+        line.clear();
+    }
+}
+
+bool BaseEmuISP::sync() {
+    switch(seq) {
+        case 0:
+            if (Getch() == '?') {
+                putln("Synchronized");
+                line.clear();
+                echoFlag = true;
+                seq++;
+            }
+            break;
+        case 1:
+            if (line_proc()) {
+                if (line == "Synchronized") {
+                    putln("OK");
+                    line.clear();
+                    seq++;
+                } else {
+                    seq = 0;
+                }
+            }
+            break;
+        case 2:
+            if (line_proc()) {
+                freq = atoi(line.c_str());
+                putln("OK");
+                return true;
+            }
+            break;
+    }
+    return false;
+}
+
+static int split(std::vector<char*> &param, char *buf, const char* delimiters = " ") {
+    param.clear();
+    char* p = strtok(buf, delimiters);
+    while(p != NULL) {
+        param.push_back(p);
+        p = strtok(NULL, delimiters);
+    }
+    return param.size();
+}
+
+int BaseEmuISP::cmd(const char *str) {
+    char buf[strlen(str) + 1];
+    strcpy(buf, str);
+    std::vector<char*> p;
+    int arg = split(p, buf);
+    if (arg == 0) {
+        return INVALID_COMMAND;
+    }
+    std::vector<int> param;
+    param.push_back(p[0][0]);
+    for(int i = 1; i < p.size(); i++) {
+        param.push_back(atoi(p[i]));    
+    }
+    switch(param[0]) {
+        case 'U': return cmd_u(arg, param[1]);
+        case 'A': return cmd_a(arg, param[1]);
+        case 'W': return cmd_w(arg, param[1], param[2]);
+        case 'R': return cmd_r(arg, param[1], param[2]);
+        case 'P': return cmd_p(arg, param[1], param[2]);
+        case 'C': return cmd_c(arg, param[1], param[2], param[3]);
+        case 'E': return cmd_e(arg, param[1], param[2]);
+        case 'J': return cmd_j(arg);
+        default: return INVALID_COMMAND;
+    }
+}
+
+int BaseEmuISP::cmd_a(int arg, int p1) {
+    if (p1 == 0 || p1 == 1) {
+        echoFlag = (p1 == 1) ? true : false;
+        return CMD_SUCCESS;
+    }
+    return PARAM_ERROR;
+}
+
+int BaseEmuISP::cmd_u(int arg, int p1) {
+    if (p1 == 23130) {
+        lockFlag = false;
+        return CMD_SUCCESS;
+    }
+    return PARAM_ERROR;
+}
+
+int BaseEmuISP::cmd_w(int arg, int p1, int p2) {
+    if (p1 % 4 != 0) {
+        return ADDR_ERROR;
+    }
+    if (p2 % 4 != 0) {
+        return COUNT_ERROR;
+    }
+    addr = p1;
+    count = p2;
+    mode = M_CMD_W_DATA;
+    return CMD_SUCCESS;
+}
+
+int BaseEmuISP::cmd_r(int arg, int p1, int p2) {
+    if (p1 % 4 != 0) {
+        return ADDR_ERROR;
+    }
+    if (p2 % 4 != 0) {
+        return COUNT_ERROR;
+    }
+    addr = p1;
+    count = p2;
+    mode = M_CMD_R_DATA;
+    return CMD_SUCCESS;
+}
+
+int BaseEmuISP::cmd_p(int arg, int p1, int p2) {
+    return CMD_SUCCESS;
+}
+
+int BaseEmuISP::cmd_c(int arg, int p1, int p2, int p3) {
+    if (p1 % 4 != 0) {
+        return DST_ADDR_ERROR;
+    }
+    if (p2 % 4 != 0) {
+        return SRC_ADDR_ERROR;
+    }
+    if (p3 != 64 && p3 != 128 && p3 != 256 && p3 != 512 && p3 != 1024) {
+        return COUNT_ERROR;
+    }
+    CopyData(p1, p2, p3);
+    return CMD_SUCCESS;
+}
+
+int BaseEmuISP::cmd_e(int arg, int p1, int p2) {
+    if (SectorExist(p1) == false || SectorExist(p2) == false) {
+        return INVALID_SECTOR;
+    }
+    return CMD_SUCCESS;
+}
+
+int BaseEmuISP::cmd_j(int arg) {
+    mode = M_CMD_J;
+    return CMD_SUCCESS;
+}
+
+void BaseEmuISP::putln(const char *s) {
+    debugPrintf("send: %s<CR><LF>\n", s);
+    while(*s) {
+        Putch(*s++);
+    }
+    Putch('\r');
+    Putch('\n');
+}
+
+bool BaseEmuISP::line_proc() {
+    int c = Getch();
+    if (c != (-1)) {
+        if (echoFlag) {
+            Putch(c);
+        }
+        if (c == '\n') {
+            debugPrintf("<LF>\n");
+            return true;
+        } else if (c == '\r') {
+            debugPrintf("<CR>");
+        } else {
+            if (line.size() == 0) {
+                debugPrintf("recv: ");
+            }
+            debugPrintf("%c", c);
+            line += c;
+        }
+    }    
+    return false;
+}
+
+void BaseEmuISP::debugPrintf(const char *format, ...) {
+    char buf[256];
+    va_list args;
+    va_start(args, format);
+    vsprintf(buf, format, args);
+    for(const char *s = buf; *s; s++) {
+        DebugPutch(*s);
+    }
+    va_end(args);
+}
+