Norimasa Okamoto / Mbed 2 deprecated emuISP

Dependencies:   mbed

BaseEmuISP.cpp

Committer:
va009039
Date:
2016-03-23
Revision:
4:05f33cc747fd
Parent:
3:ccc673a10485
Child:
5:e2c275b33bbf

File content as of revision 4:05f33cc747fd:

// BaseEmuISP.cpp 2016/3/23
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include "BaseEmuISP.h"

void BaseEmuISP::Reset() {
    mode = M_RESET;
}

void BaseEmuISP::Poll() {
    Mode_t prev_mode = mode;
    int result;
    char buf[32];
    switch(mode) {
        case M_RESET:
            echoFlag = true;
            lockFlag = true;
            version.Major = BootCodeVersion() >>8;
            version.Minor = BootCodeVersion() & 0xff;
            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 (cmd_w_data()) {
                mode = M_CMD;
            }
            break;
        case M_CMD_R_DATA:
            if (cmd_r_data()) {
                mode = M_CMD;
            }
            break;
        case M_CMD_J:
            snprintf(buf, sizeof(buf), "%d", PartID());
            putln(buf);
            mode = M_CMD;
            break;
        case M_CMD_K:
            snprintf(buf, sizeof(buf), "%d", version.Major);
            putln(buf);
            snprintf(buf, sizeof(buf), "%d", version.Minor);
            putln(buf);
            mode = M_CMD;
            break;
        case M_CMD_N:
            mode = M_CMD;
            break;
        case M_CMD_S:
            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(myvector<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();
}

ReturnCode_t BaseEmuISP::cmd(const char *str) {
    char buf[strlen(str) + 1];
    strcpy(buf, str);
    myvector<char*> p;
    if (split(p, buf) == 0) {
        return INVALID_COMMAND;
    }
    vint_t param;
    param.push_back(p[0][0]);
    for(size_t i = 1; i < p.size(); i++) {
        param.push_back(atoi(p[i]));
    }
    switch(param[0]) {
        case 'U': return cmd_u(param);
        case 'A': return cmd_a(param);
        case 'W': return cmd_w(param);
        case 'R': return cmd_r(param);
        case 'P': return cmd_p(param);
        case 'C': return cmd_c(param);
        case 'E': return cmd_e(param);
        case 'I': return cmd_i(param);
        case 'J': return cmd_j(param);
        case 'K': return cmd_k(param);
        case 'N': return cmd_n(param);
        case 'S': return cmd_s(param);
        default: return INVALID_COMMAND;
    }
}

ReturnCode_t BaseEmuISP::cmd_a(vint_t &param) {
    if (param.size() > 1) {
        if (param[1] == 0 || param[1] == 1) {
            echoFlag = (param[1] == 1) ? true : false;
            return CMD_SUCCESS;
        }
    }
    return PARAM_ERROR;
}

ReturnCode_t BaseEmuISP::cmd_u(vint_t &param) {
    if (param.size() > 1) {
        if (param[1] == 23130) {
            lockFlag = false;
            return CMD_SUCCESS;
        }
    }
    return PARAM_ERROR;
}

ReturnCode_t BaseEmuISP::cmd_w(vint_t &param) {
    if (param.size() > 2) {
        if (param[1] % 4 != 0) {
            return ADDR_ERROR;
        }
        if (param[2] % 4 != 0) {
            return COUNT_ERROR;
        }
        addr = param[1];
        dataCount = param[2];
        mode = M_CMD_W_DATA;
        return CMD_SUCCESS;
    }
    return PARAM_ERROR;
}

ReturnCode_t BaseEmuISP::cmd_r(vint_t &param) {
    if (param.size() > 2) {
        if (param[1] % 4 != 0) {
            return ADDR_ERROR;
        }
        if (param[2] % 4 != 0) {
            return COUNT_ERROR;
        }
        addr = param[1];
        dataCount = param[2];
        mode = M_CMD_R_DATA;
        return CMD_SUCCESS;
    }
    return PARAM_ERROR;
}

ReturnCode_t BaseEmuISP::cmd_p(vint_t &param) {
    if (param.size() > 2) {
        for(int sector = param[1]; sector <= param[2]; sector++) {
            if (!Prepare(sector)) {
                return INVALID_SECTOR;
            }
        }
        return CMD_SUCCESS;
    }
    return PARAM_ERROR;
}

ReturnCode_t BaseEmuISP::cmd_c(vint_t &param) {
    if (lockFlag) {
        return CMD_LOCKED;
    }
    if (param.size() > 3) {
        if (param[1] % 4 != 0) {
            return DST_ADDR_ERROR;
        }
        if (param[2] % 4 != 0) {
            return SRC_ADDR_ERROR;
        }
        if (param[3] != 64 && param[3] != 128 && param[3] != 256 &&
            param[3] != 512 && param[3] != 1024) {
            return COUNT_ERROR;
        }
        CopyData(param[1], param[2], param[3]);
        return CMD_SUCCESS;
    }
    return PARAM_ERROR;
}

ReturnCode_t BaseEmuISP::cmd_e(vint_t &param) {
    if (lockFlag) {
        return CMD_LOCKED;
    }
    if (param.size() > 2) {
        for(int sector = param[1]; sector <= param[2]; sector++) {
            if (!Erase(sector)) {
                return INVALID_SECTOR;
            }
        }
        return CMD_SUCCESS;
    }
    return PARAM_ERROR;
}

ReturnCode_t BaseEmuISP::cmd_i(vint_t &param) {
    if (param.size() > 2) {
        for(int sector = param[1]; sector <= param[2]; sector++) {
            if (!Blank(sector)) {
                return SECTOR_NOT_BLANK;
            }
        }
        return CMD_SUCCESS;
    }
    return PARAM_ERROR;
}

ReturnCode_t BaseEmuISP::cmd_j(vint_t &param) {
    mode = M_CMD_J;
    return CMD_SUCCESS;
}

ReturnCode_t BaseEmuISP::cmd_k(vint_t &param) {
    mode = M_CMD_K;
    return CMD_SUCCESS;
}

ReturnCode_t BaseEmuISP::cmd_m(vint_t &param) {
    if (param.size() > 3) {
        if (param[1] % 4 != 0 || param[2] % 4 != 0) {
            return ADDR_ERROR;
        }
        if (param[3] % 4 != 0) {
            return COUNT_ERROR;
        }
        if (!Compare(param[1], param[2], param[3])) {
            return COMPARE_ERROR;
        }
        return CMD_SUCCESS;
    }
    return PARAM_ERROR;
}

ReturnCode_t BaseEmuISP::cmd_n(vint_t &param) {
    mode = M_CMD_N;
    return CMD_SUCCESS;
}

ReturnCode_t BaseEmuISP::cmd_s(vint_t &param) {
    mode = M_CMD_S;
    return CMD_SUCCESS;
}

void uudecode(vbyte_t &dst, const char* src) {
    dst.clear();
    int len = strlen(src);
    if (len > 0) {
        size_t size = (src[0]^0x20)&0x3f;
        uint8_t b[4];
        int k = 0;
        for(int i = 1; i < len && dst.size() < size; i++) {
            b[k] = (src[i]^0x20)&0x3f;
            if (k == 0) {
                k++;
            } else if (k == 1) {
                dst.push_back(b[0]<<2|b[1]>>4);
                k++;
            } else if (k == 2) {
                dst.push_back(b[1]<<4|b[2]>>2);
                k++;
            } else {
                dst.push_back(b[2]<<6|b[3]);
                k = 0;
            }
        }
    }
}

bool BaseEmuISP::cmd_w_data() {
    int c;
    switch(seq) {
        case 0:
            dataCurrent = 0;
            if (UuencodeMode()) {
                seq = 2;
            } else {
                seq = 1;
            }
            break;
        case 1: // binary mode
            if (dataCurrent >= dataCount) {
                return true;
            }
            c = Getch();
            if (c != (-1)) {
                if (echoFlag) {
                    Putch(c);
                }
                WriteData(addr++, c);
                dataCurrent++;
            }
            break;
        case 2: // uuencode mode
            dataLine = 0;
            dataCksum = 0;
            line.Clear();
            seq++;
            break;
        case 3: // uu data start
            if (line_proc()) {
                vbyte_t data;
                uudecode(data, line.C_str());
                for(size_t i = 0; i < data.size(); i++) {
                    int c = data[i];
                    WriteData(addr++, c);
                    dataCksum += c;
                    dataCurrent++;
                }
                dataLine++;
                if (dataLine >= 20 || dataCurrent>= dataCount) {
                    seq++;
                }
                line.Clear();
            }
            break;
        case 4: // check sum
            if (line_proc()) {
                if (line == dataCksum) {
                    putln("OK");
                    if (dataCurrent >= dataCount) {
                        mode = M_CMD;
                    } else {
                        seq = 2;
                    }
                } else {
                    putln("RESEND");
                    seq = 2;
                }
            }
            break;
    }
    return false;
}

bool BaseEmuISP::cmd_r_data() {
    int c;
    switch(seq) {
        case 0:
            dataCurrent = 0;
            if (UuencodeMode()) {
                dataLine = 0;
                dataCksum = 0;
                line.Clear();
                seq = 2;
            } else {
                seq = 1;
            }
            break;
        case 1: // binary mode
            if (dataCurrent >= dataCount) {
                return true;
            }
            c = ReadData(addr++);
            Putch(c);
            dataCurrent++;
            break;
        case 2:
            break;
    }
    return false;
}


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) {
            if (version.Major >= 4 || c != '\n') {
                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);
}