Merge pull request #90 from jjwbruijn/New_Flasher

New flasher

ESP32_AP-Flasher/OEPL-Flash.py

@@ -0,0 +1,318 @@
+import argparse
+import serial.tools.list_ports
+import serial
+import time
+from intelhex import IntelHex
+import os.path
+
+CMD_GET_VERSION = 1
+CMD_RESET_ESP = 2
+CMD_RESET = 11
+CMD_SET_POWER = 13
+
+CMD_ERASE_FLASH = 26
+CMD_ERASE_INFOPAGE = 27
+CMD_SAVE_MAC_FROM_FW = 40
+CMD_PASS_THROUGH = 50
+
+CMD_SELECT_ZBS243 = 60
+CMD_SELECT_NRF82511 = 61
+
+CMD_SELECT_PORT = 70
+
+CMD_READ_INFOPAGE = 80
+CMD_READ_FLASH = 81
+CMD_WRITE_INFOPAGE = 82
+CMD_WRITE_FLASH = 83
+CMD_AUTOFLASH = 87
+CMD_COMPLETE = 88
+
+
+def read_binary_file(file_path):
+    with open(file_path, 'rb') as file:
+        binary_data = file.read()
+    return binary_data
+
+
+def read_hex_file(file_path):
+    ih = IntelHex(file_path)
+    binary_data = ih.tobinarray()
+    return bytes(binary_data)
+
+
+def to_byte(input, number=4):
+    return input.to_bytes(number, byteorder='big')
+
+
+def send_cmd(cmd, data):
+    return_data = to_byte(cmd, 1) + to_byte(len(data), 4) + data
+    crc_val = 0xAB34
+    for x in return_data:
+        crc_val += x
+    return_data = b"AT" + return_data + to_byte(crc_val & 0xffff, 2)
+    ser.write(return_data)
+
+
+def wait_for_command():
+    start_time = time.time()
+    ser.timeout = 5  # Set the timeout to 1 second
+    while True:
+        if ser.in_waiting > 0:
+            command = ser.read(2)  # Read the "AT" prefix
+            if command == b"AT":
+                # Read the command byte
+                cmd = int.from_bytes(ser.read(1), byteorder='big')
+                data_length = int.from_bytes(
+                    ser.read(4), byteorder='big')  # Read the data length
+                data = ser.read(data_length)  # Read the data
+                # Read the CRC value
+                crc = int.from_bytes(ser.read(2), byteorder='big')
+                # Verify CRC
+                crc_val = 0xAB34
+                for x in to_byte(cmd, 1) + to_byte(data_length, 4) + data:
+                    crc_val += x
+                if crc_val & 0xffff == crc:
+                    return cmd, data
+                else:
+                    print("Invalid CRC. Discarding command. Got " +
+                          str(crc_val) + " but was expecting "+str(crc))
+                    print("data was:"+str(data))
+        if time.time() - start_time > ser.timeout:
+            print("timeout waiting for reply")
+            return None, None
+
+
+def list_available_com_ports():
+    ports = serial.tools.list_ports.comports()
+    available_ports = [port.device for port in ports]
+    print("Specify a serial port to use with -p <PORT>")
+    print("available COM ports:")
+    for port in available_ports:
+        print(port)
+
+
+def validate_arguments(args):
+    if not (args.nrf82511 or args.zbs243):
+        print("Either -nrf82511 or -zbs243 option is required.")
+        return False
+    if not (args.internalap or args.external or args.altradio):
+        print("One of -internalap, -external, or -altradio options is required.")
+        return False
+    if args.command in ["read", "write"] and not (args.flash or args.infopage):
+        print("One of --flash or --infopage arguments is required for read and write commands.")
+        return False
+    if ((args.command == "debug" or args.pt) and not (args.external)):
+        print("Debugging/Passthrough is only available on the external port!")
+        return False
+    if args.command == "write" and not os.path.isfile(args.filename):
+        print("Couldn't find the specified file!")
+        return False;
+    if args.command in ["read", "write"] and not args.filename:
+        print("Please specify a file to save read data")
+        return False;
+    if args.command == "read" and len(args.filename) < 2:
+        print("Please specify a file to save read data")
+        return False;
+    return True
+
+def read_from_serial(port, filename, flash):
+    if flash:
+        print(
+            f"Reading flash data and saving to file: {filename}.")
+    else:
+        print(
+            f"Reading info page and saving to file: {filename}.")
+
+    print("Now downloading...\n")
+    file = open(filename, "wb")
+    bytecount = 0
+    while True:
+
+        print(f'\r{bytecount} bytes', end='', flush=True)
+        if (flash):
+            send_cmd(CMD_READ_FLASH, bytearray([]))
+        else:
+            send_cmd(CMD_READ_INFOPAGE, bytearray([]))
+
+        cmd, answer = wait_for_command()
+        if ((cmd == CMD_READ_FLASH) or (cmd == CMD_READ_INFOPAGE)):
+            file.write(bytearray(answer))
+            bytecount += len(answer)
+        elif (cmd == CMD_COMPLETE):
+            file.close()
+            print("\r All done!         \nFile saved\n", end='', flush=True)
+            break
+        else:
+            print("Failed reading block, timeout?")
+
+def write_to_serial(port, filename, flash):
+
+    if flash:
+        print(f"\nErasing flash... ")
+        send_cmd(CMD_ERASE_FLASH, bytearray([]))
+    else:
+        print(f"\nErasing infopage... ")
+        send_cmd(CMD_ERASE_INFOPAGE, bytearray([]))
+
+    cmd, answer = wait_for_command()
+    if ((cmd == CMD_ERASE_FLASH) or (cmd == CMD_ERASE_INFOPAGE)):
+        print("DONE!\n")
+    else:
+        print("\nFailed to erase the flash?")
+        exit(0)
+
+    # TODO: Implement logic to write data from the file to the serial port with flash option
+    if flash:
+        print(f"Writing flash data from file: {filename}\n")
+    else:
+        print(f"Writing info page data from file: {filename}\n")
+
+    chunk_size = 1024
+
+    if filename.endswith('.bin'):
+        file_data = read_binary_file(filename)
+    elif filename.endswith('.hex'):
+        file_data = read_hex_file(filename)
+
+    for i in range(0, len(file_data), chunk_size):
+        chunk = file_data[i:i + chunk_size]
+        if (flash):
+            send_cmd(CMD_WRITE_FLASH, bytearray(chunk))
+        else:
+            send_cmd(CMD_WRITE_INFOPAGE, bytearray(chunk))
+        cmd, answer = wait_for_command()
+        if ((cmd == CMD_WRITE_FLASH) or (cmd == CMD_WRITE_INFOPAGE)):
+            print(f'\rSent {i} bytes', end='', flush=True)
+        elif (cmd == CMD_COMPLETE):
+            print(
+                '\Tried to write more bytes than we have room for!   \n', end='', flush=True)
+            return
+        else:
+            print("Some other error, dunno\n")
+            return
+    print('\rAll done writing! ', end='', flush=True)
+
+def short_passthough(period_time):
+    start_time = time.time()
+    while time.time() - start_time < period_time:
+        data = ser.read()
+        if data:
+            print(data.decode(), end='')
+            if chr(0x04) in data.decode():
+                break
+
+def main():
+    try:
+        parser = argparse.ArgumentParser(
+            description="OpenEPaperLink Flasher for AP/Flasher board")
+        parser.add_argument("-p", "--port", help="COM port to use")
+        parser.add_argument("command", choices=[
+                            "read", "write", "autoflash", "debug"], help="Command to execute")
+        parser.add_argument("filename", nargs="?",
+                            help="Filename for read/write commands")
+        parser.add_argument("-f", "--flash", action="store_true",
+                            help="Write to the flash")
+        parser.add_argument("-i", "--infopage", action="store_true",
+                            help="Write to the infopage/UICR")
+        parser.add_argument("-n", "--nrf82511", action="store_true",
+                            help="nRF82511 programming")
+        parser.add_argument("-z", "--zbs243", action="store_true",
+                            help="ZBS243 programming")
+        parser.add_argument("--internalap", action="store_true",
+                            help="Selects the internal accesspoint port")
+        parser.add_argument("-e", "--external", action="store_true",
+                            help="Selects the external(side) port")
+        parser.add_argument("--altradio", action="store_true",
+                            help="Selects the alternate radio port")
+        parser.add_argument("--pt","--passthrough", action="store_true",
+                            help="Enters serial passthrough for debug output after flashing")
+
+        args = parser.parse_args()
+
+        if not validate_arguments(args):
+            return
+
+        if not args.port:
+            list_available_com_ports()
+            return
+
+        global ser
+        ser = serial.Serial(args.port, baudrate=500000)
+        time.sleep(1)  # Flush serial data
+        while (ser.inWaiting() > 0):
+            data_str = ser.read(ser.inWaiting())
+
+        send_cmd(CMD_GET_VERSION, bytearray([]))
+        cmd, answer = wait_for_command()
+        if (cmd == CMD_GET_VERSION):
+            print("AP/Flasher version: " +
+                  str(answer[0] << 24 | answer[1] << 16 | answer[2] << 8 | answer[3]))
+        else:
+            print(
+                "Couldn't establish a connection with the flasher, did you select the correct serial port?")
+            exit(0)
+
+        if (args.command!="debug"):
+            if args.internalap:
+                send_cmd(CMD_SELECT_PORT, bytearray([0]))
+            if args.external:
+                send_cmd(CMD_SELECT_PORT, bytearray([1]))
+            if args.altradio:
+                send_cmd(CMD_SELECT_PORT, bytearray([2]))
+
+            if args.nrf82511:
+                send_cmd(CMD_SELECT_NRF82511, bytearray([]))
+            if args.zbs243:
+                send_cmd(CMD_SELECT_ZBS243, bytearray([]))
+
+            cmd, answer = wait_for_command()
+            if (answer[0] == 1):
+                print("Connection established to microcontroller")
+            else:
+                print("Failed to establish a connection to the microcontroller")
+                exit(0)
+
+            if args.command == "read":
+                read_from_serial(ser, args.filename, args.flash)
+            elif args.command == "write":
+                write_to_serial(ser, args.filename, args.flash)
+            elif args.command == "autoflash":
+                print("Starting automatic tag flash")
+                send_cmd(CMD_AUTOFLASH , bytearray([]))
+                short_passthough(30)
+            else:
+                print("Invalid command!")
+
+            send_cmd(CMD_RESET, bytearray([]))
+            cmd, answer = wait_for_command()
+            send_cmd(CMD_SET_POWER, bytearray([1]))
+            cmd, answer = wait_for_command()
+
+
+        if (args.pt or args.command=="debug"):
+            # enter passthrough mode
+            send_cmd(CMD_PASS_THROUGH , bytearray([]))
+            print("Now showing debug output from the tag - CTRL+C to quit");
+            print("---------------------------------------------------------------------------------");
+            while True:
+                try:
+                    data = ser.read()
+                    if data:
+                        print(data.decode('utf-8', errors='replace'), end='')
+                except UnicodeDecodeError:
+                    print(" ")
+                data = ser.read()
+                if data:
+                    print(data.decode(), end='')
+                    if chr(0x04) in data.decode():
+                        break
+
+
+    except KeyboardInterrupt:
+        print("\nBye!")
+        ser.close()
+        exit(1)
+
+
+if __name__ == "__main__":
+    main()

ESP32_AP-Flasher/include/flasher.h

@@ -1,4 +1,5 @@
 #include <Arduino.h>
+#include <LittleFS.h>
 
 uint16_t getAPUpdateVersion(uint8_t type);
 bool checkForcedAPFlash();
@@ -10,4 +11,54 @@ void flashCountDown(uint8_t c);
 #ifdef OPENEPAPERLINK_PCB
 bool extTagConnected();
 bool doTagFlash();
-#endif
+#endif
+
+class flasher {
+   public:
+    class ZBS_interface *zbs = nullptr;
+    uint8_t md5[16] = {0};
+    char md5char[34];
+    uint8_t tagtype;
+    uint8_t *infoblock = nullptr;
+
+    // Infoblock structure:
+    // 0x00-0x0F - Calibration data
+    // 0x10-0x17 - MAC
+    // 0x19      - OpenEPaperLink Type
+    // 0x30      - Original firmware MD5
+
+    uint8_t mac[8] = {0};
+    uint8_t mac_format = 0;
+    uint16_t mac_suffix = 0;
+    uint16_t mac_offset = 0;
+
+    flasher();
+    ~flasher();
+    bool connectTag(uint8_t port);
+    void getFirmwareMD5();
+    bool getFirmwareMac();
+    bool findTagByMD5();
+    bool findTagByType(uint8_t type);
+    bool getInfoBlockMD5();
+    bool getInfoBlockMac();
+    bool getInfoBlockType();
+    void getMacFromWiFi();
+    bool prepareInfoBlock();
+
+    bool backupFlash();
+
+    bool writeFlash(uint8_t *flashbuffer, uint16_t size);
+    bool writeFlashFromPack(String filename, uint8_t type);
+    bool writeFlashFromPackOffset(fs::File *file, uint16_t length);
+
+    bool readBlock(uint16_t offset, uint8_t* data, uint16_t len, bool infopage);
+    bool writeBlock(uint16_t offset, uint8_t* data, uint16_t len, bool infopage);
+
+    bool readInfoBlock();
+    bool writeInfoBlock();
+
+   protected:
+    bool writeBlock256(uint16_t offset, uint8_t *flashbuffer);
+    void get_mac_format1();
+    void get_mac_format2();
+};

ESP32_AP-Flasher/include/serialconsole.h

@@ -1,8 +0,0 @@
-#include <Arduino.h>
-extern QueueHandle_t consoleCmdQueue;
-extern TaskHandle_t consoleTaskHandle;
-
-
-void consoleStopTask();
-void consoleTask(void* parameter);
-void consoleUartHandler(uint8_t* data, uint8_t len);

ESP32_AP-Flasher/include/swd.h

@@ -0,0 +1,83 @@
+#include <Arduino.h>
+#include <stdint.h>
+
+/*
+   Copyright (c) 2021 Aaron Christophel ATCnetz.de
+   SPDX-License-Identifier: GPL-3.0-or-later
+*/
+#pragma once
+
+class swd {
+   public:
+    swd(uint8_t swdio, uint8_t swdclk);
+
+    uint32_t swd_Init();
+
+    bool AP_Write(unsigned addr, uint32_t data);
+    bool AP_Read(unsigned addr, uint32_t &data);
+    bool DP_Write(unsigned addr, uint32_t data);
+    bool DP_Read(unsigned addr, uint32_t &data);
+
+    uint32_t idCode;
+
+   protected:
+    void swd_Begin();
+    void swd_Direction(bool WorR);
+    bool swd_Transfer(unsigned port_address, bool APorDP, bool RorW, uint32_t &data);
+    bool calculate_Parity(uint32_t in_data);
+
+    void swd_Write(uint32_t in_data, uint8_t bits);
+    uint32_t swd_Read(uint8_t bits);
+
+    uint8_t swdio_pin;
+    uint8_t swdclk_pin;
+    bool cur_swd_direction = 0;
+};
+
+struct nrf_info_struct {
+    int flash_size;
+    uint32_t connected;
+    uint32_t codepage_size;
+    uint32_t codesize;
+    uint32_t config_id;
+    uint32_t device_id0;
+    uint32_t device_id1;
+    uint32_t info_part;
+    uint32_t info_variant;
+    uint32_t info_package;
+    uint16_t sd_info_area;
+    uint32_t ucir_lock;
+};
+
+class nrfswd : protected swd {
+   public:
+    nrfswd(uint8_t swdio, uint8_t swdclk);
+    bool init();
+
+    uint8_t nrf_read_bank(uint32_t address, uint32_t buffer[], int size);
+    uint8_t nrf_write_bank(uint32_t address, uint32_t buffer[], int size);
+    uint8_t erase_all_flash();
+    uint8_t erase_uicr();
+    uint8_t erase_page(uint32_t page);
+    void nrf_soft_reset();
+
+    bool isConnected = false;
+    bool isLocked = false;
+    bool showDebug = true;
+
+    struct nrf_info_struct nrf_info;
+
+   protected:
+    uint32_t nrf_read_port(bool APorDP, uint8_t address);
+    void nrf_write_port(bool APorDP, uint8_t address, uint32_t value);
+
+    void nrf_abort_all();
+    void nrf_halt();
+
+    void nrf_port_selection(bool new_port);
+    bool nrf_read_lock_state();
+    void nrf_read_ufcr();
+
+    void write_register(uint32_t address, uint32_t value);
+    uint32_t read_register(uint32_t address);
+};

ESP32_AP-Flasher/platformio.ini

@@ -65,7 +65,7 @@ build_flags =
   	-D FLASHER_LED=15
   	-D FLASHER_RGB_LED=33
 build_src_filter = 
-   +<*>-<usbflasher.cpp>-<serialconsole.cpp>
+   +<*>-<usbflasher.cpp>-<swd.cpp>
 board_build.psram_type=qspi_opi
 board_upload.maximum_size = 4194304
 board_upload.maximum_ram_size = 327680
@@ -100,7 +100,7 @@ build_flags =
   	-D FLASHER_LED=15
   	-D FLASHER_RGB_LED=-1
 build_src_filter = 
-   +<*>-<usbflasher.cpp>-<serialconsole.cpp>
+   +<*>-<usbflasher.cpp>-<swd.cpp>
 board_build.psram_type=qspi_opi
 board_upload.maximum_size = 4194304
 board_upload.maximum_ram_size = 327680
@@ -187,7 +187,7 @@ build_flags =
 	-D FLASHER_AP_RXD=16
 	-D FLASHER_LED=22
 build_src_filter = 
-   +<*>-<usbflasher.cpp>-<serialconsole.cpp>
+   +<*>-<usbflasher.cpp>-<swd.cpp>
 
 ; ----------------------------------------------------------------------------------------
 ; !!! this configuration expects an wemos_d1_mini32
@@ -213,7 +213,7 @@ build_flags =
 	-D FLASHER_AP_RXD=17
 	-D FLASHER_LED=22
 build_src_filter = 
-   +<*>-<usbflasher.cpp>-<serialconsole.cpp>
+   +<*>-<usbflasher.cpp>-<swd.cpp>
 
 ; ----------------------------------------------------------------------------------------
 ; !!! this configuration expects an m5stack esp32
@@ -250,4 +250,4 @@ build_flags =
 	-D ILI9341_DRIVER
 	-D SMOOTH_FONT
 build_src_filter = 
-   +<*>-<usbflasher.cpp>-<serialconsole.cpp>
+   +<*>-<usbflasher.cpp>-<swd.cpp>

ESP32_AP-Flasher/src/flasher.cpp

@@ -2,9 +2,10 @@
 
 #include <Arduino.h>
 #include <ArduinoJson.h>
-#include "storage.h"
-#include "LittleFS.h"
 #include <MD5Builder.h>
+
+#include "LittleFS.h"
+#include "storage.h"
 // #include <FS.h>
 
 #include "leds.h"
@@ -61,53 +62,6 @@ uint8_t pinsExt[] = {FLASHER_EXT_CLK, FLASHER_EXT_MISO, FLASHER_EXT_MOSI, FLASHE
 
 #endif
 
-class flasher {
-   public:
-    class ZBS_interface *zbs = nullptr;
-    uint8_t md5[16] = {0};
-    char md5char[34];
-    uint8_t tagtype;
-    uint8_t *infoblock = nullptr;
-
-    // Infoblock structure:
-    // 0x00-0x0F - Calibration data
-    // 0x10-0x17 - MAC
-    // 0x19      - OpenEPaperLink Type
-    // 0x30      - Original firmware MD5
-
-    uint8_t mac[8] = {0};
-    uint8_t mac_format = 0;
-    uint16_t mac_suffix = 0;
-    uint16_t mac_offset = 0;
-
-    flasher();
-    ~flasher();
-    bool connectTag(uint8_t port);
-    void getFirmwareMD5();
-    bool getFirmwareMac();
-    bool findTagByMD5();
-    bool findTagByType(uint8_t type);
-    bool getInfoBlockMD5();
-    bool getInfoBlockMac();
-    bool getInfoBlockType();
-    void getMacFromWiFi();
-    bool prepareInfoBlock();
-
-    bool backupFlash();
-
-    bool writeFlash(uint8_t *flashbuffer, uint16_t size);
-    bool writeFlashFromPack(String filename, uint8_t type);
-    bool writeFlashFromPackOffset(fs::File *file, uint16_t length);
-
-    bool readInfoBlock();
-    bool writeInfoBlock();
-
-   protected:
-    bool writeBlock256(uint16_t offset, uint8_t *flashbuffer);
-    void get_mac_format1();
-    void get_mac_format2();
-};
-
 flasher::flasher() {
     zbs = new ZBS_interface;
     Storage.end();
@@ -519,6 +473,34 @@ bool flasher::writeFlashFromPack(String filename, uint8_t type) {
     return false;
 }
 
+bool flasher::readBlock(uint16_t offset, uint8_t *data, uint16_t len, bool infopage) {
+    if (infopage) {
+        if (!zbs->select_flash(1)) return false;
+        if (offset > 1024) return false;
+    } else {
+        if (!zbs->select_flash(0)) return false;
+        if (offset > 65535) return false;
+    }
+    for (uint32_t c = 0; c < len; c++) {
+        data[c] = zbs->read_flash(offset + c);
+    }
+    return true;
+}
+
+bool flasher::writeBlock(uint16_t offset, uint8_t *data, uint16_t len, bool infopage) {
+    if (infopage) {
+        if (!zbs->select_flash(1)) return false;
+        if (offset > 1024) return false;
+    } else {
+        if (!zbs->select_flash(0)) return false;
+        if (offset > 65535) return false;
+    }
+    for (uint32_t c = 0; c < len; c++) {
+        zbs->write_flash(c + offset, data[c]);
+    }
+    return true;
+}
+
 uint16_t getAPUpdateVersion(uint8_t type) {
     StaticJsonDocument<512> doc;
     DynamicJsonDocument APconfig(512);

ESP32_AP-Flasher/src/serialconsole.cpp

@@ -1,118 +0,0 @@
-#include "serialconsole.h"
-
-#include <Arduino.h>
-
-#include "USB.h"
-
-QueueHandle_t consoleCmdQueue;
-TaskHandle_t consoleTaskHandle;
-extern USBCDC USBSerial;
-
-struct consoleCommand {
-    uint8_t command = 0;
-    uint8_t len = 0;
-    uint8_t* data = nullptr;
-};
-
-void consoleStopTask() {
-    if (consoleTaskHandle) vTaskDelete(consoleTaskHandle);
-    consoleTaskHandle = NULL;
-}
-/*
-.in>1B 5B 3C 30 3B 33 39 3B 39 4D
-,in>1B 5B 3C 30 3B 33 39 3B 39 6D
-,in>1B 5B 3C 32 3B 33 39 3B 39 4D
-,in>1B 5B 3C 32 3B 33 39 3B 39 6D
-
-in>1B 5B 3C 30 3B 32 38 3B 31 32 4D
-in>1B 5B 3C 30 3B 32 38 3B 31 32 6D
-
-in>1B 5B 3C 32 3B 32 38 3B 31 32 4D
-in>1B 5B 3C 32 3B 32 38 3B 31 32 6D
-
-in>1B 5B 3C 36 34 3B 32 39 3B 31 32 4D
-in>1B 5B 3C 36 35 3B 32 39 3B 31 32 4D
-*/
-
-bool escapeCommandComplete(struct consoleCommand* cmd) {
-    return true;
-}
-
-void consoleUartHandler(uint8_t* data, uint8_t len) {
-    static struct consoleCommand* cmd = nullptr;
-    static bool commandStarted = false;
-
-    while (len--) {
-        uint8_t usbbyte = *(data++);
-
-        if (cmd == nullptr) {
-            cmd = new struct consoleCommand;
-            cmd->data = (uint8_t*)calloc(65, 1);
-            cmd->len = 0;
-        }
-
-        // check if we've started a command in this byte
-        if ((!commandStarted) && (usbbyte == 0x1B)) {
-            commandStarted = true;
-            if (cmd->len != 0) {
-                BaseType_t queuestatus = xQueueSend(consoleCmdQueue, &cmd, 0);
-                if (queuestatus == pdFALSE) {
-                    if (cmd->data != nullptr) free(cmd->data);
-                    delete cmd;
-                }
-                cmd = nullptr;
-            }
-        }
-
-        if (cmd == nullptr) {
-            cmd = new struct consoleCommand;
-            cmd->data = (uint8_t*)calloc(65, 1);
-            cmd->len = 0;
-        }
-
-        cmd->data[cmd->len++] = usbbyte;
-        cmd->len %= 64;
-
-        if (commandStarted) {
-            if (escapeCommandComplete(cmd) || cmd->len == 0x00) {
-                BaseType_t queuestatus = xQueueSend(consoleCmdQueue, &cmd, 0);
-                if (queuestatus == pdFALSE) {
-                    if (cmd->data != nullptr) free(cmd->data);
-                    delete cmd;
-                }
-                commandStarted = false;
-                cmd = nullptr;
-            }
-        }
-    }
-
-    if (!commandStarted && cmd != nullptr) {
-        BaseType_t queuestatus = xQueueSend(consoleCmdQueue, &cmd, 0);
-        if (queuestatus == pdFALSE) {
-            if (cmd->data != nullptr) free(cmd->data);
-            delete cmd;
-        }
-        cmd = nullptr;
-    }
-}
-
-void consoleTask(void* parameter) {
-    struct consoleCommand* cmd;
-
-    USBSerial.print("\e[?1000;1006;1015h");  // works
-
-    while (true) {
-        BaseType_t queuereceive = xQueueReceive(consoleCmdQueue, &cmd, 1500 / portTICK_PERIOD_MS);
-        if (queuereceive == pdTRUE) {
-            uint8_t c = 0;
-            Serial.printf("queue>");
-            while (cmd->len--) {
-                Serial.printf(" %02X", cmd->data[c]);
-                c++;
-            }
-            if (cmd->data != nullptr) free(cmd->data);
-            delete cmd;
-            Serial.printf("\n");
-        }
-    }
-}

ESP32_AP-Flasher/src/swd.cpp

@@ -0,0 +1,393 @@
+/*
+   Copyright (c) 2021 Aaron Christophel ATCnetz.de
+   SPDX-License-Identifier: GPL-3.0-or-later
+*/
+#include "swd.h"
+
+#include "Arduino.h"
+
+// Many thanks to scanlime for the work on the ESP8266 SWD Library, parts of this code have inspiration and help from it
+// https://github.com/scanlime/esp8266-arm-swd
+
+#define DP_IDCODE 0x00
+
+swd::swd(uint8_t swdio, uint8_t swdclk) {
+    this->swdio_pin = swdio;
+    this->swdclk_pin = swdclk;
+    this->swd_Begin();
+}
+void swd::swd_Begin() {
+    pinMode(this->swdclk_pin, OUTPUT);
+    pinMode(this->swdio_pin, INPUT_PULLUP);
+}
+uint32_t swd::swd_Init() {  // Returns the ID
+    swd_Write(0xffffffff, 32);
+    swd_Write(0xffffffff, 32);
+    swd_Write(0xe79e, 16);
+    swd_Write(0xffffffff, 32);
+    swd_Write(0xffffffff, 32);
+    swd_Write(0, 32);
+    swd_Write(0, 32);
+
+    DP_Read(DP_IDCODE, this->idCode);
+    return this->idCode;
+}
+bool swd::AP_Write(unsigned addr, uint32_t data) {
+    uint8_t retry = 15;
+    while (retry--) {
+        bool state = swd_Transfer(addr, 1, 0, data);
+        if (state)
+            return true;
+    }
+    return false;
+}
+bool swd::AP_Read(unsigned addr, uint32_t &data) {
+    uint8_t retry = 15;
+    while (retry--) {
+        bool state = swd_Transfer(addr, 1, 1, data);
+        if (state)
+            return true;
+    }
+    return false;
+}
+bool swd::DP_Write(unsigned addr, uint32_t data) {
+    uint8_t retry = 15;
+    while (retry--) {
+        bool state = swd_Transfer(addr, 0, 0, data);
+        if (state)
+            return true;
+    }
+    return false;
+}
+bool swd::DP_Read(unsigned addr, uint32_t &data) {
+    uint8_t retry = 15;
+    while (retry--) {
+        bool state = swd_Transfer(addr, 0, 1, data);
+        if (state)
+            return true;
+    }
+    return false;
+}
+bool swd::swd_Transfer(unsigned port_address, bool APorDP, bool RorW, uint32_t &data) {
+    bool parity = APorDP ^ RorW ^ ((port_address >> 2) & 1) ^ ((port_address >> 3) & 1);
+    uint8_t filled_address = (1 << 0) | (APorDP << 1) | (RorW << 2) | ((port_address & 0xC) << 1) | (parity << 5) | (1 << 7);
+    swd_Write(filled_address, 8);
+    if (swd_Read(3) == 1) {
+        if (RorW) {  // Reading 32 bits from SWD
+            data = swd_Read(32);
+            if (swd_Read(1) == calculate_Parity(data)) {
+                swd_Write(0, 1);
+                return true;
+            }
+        } else {  // Writing 32bits to SWD
+            swd_Write(data, 32);
+            swd_Write(calculate_Parity(data), 1);
+            swd_Write(0, 1);
+            return true;
+        }
+    }
+    swd_Write(0, 32);
+    return false;
+}
+bool swd::calculate_Parity(uint32_t in_data) {
+    in_data = (in_data & 0xFFFF) ^ (in_data >> 16);
+    in_data = (in_data & 0xFF) ^ (in_data >> 8);
+    in_data = (in_data & 0xF) ^ (in_data >> 4);
+    in_data = (in_data & 0x3) ^ (in_data >> 2);
+    in_data = (in_data & 0x1) ^ (in_data >> 1);
+    return in_data;
+}
+void swd::swd_Write(uint32_t in_data, uint8_t bits) {
+    if (cur_swd_direction == 0)
+        swd_Direction(1);
+    while (bits--) {
+        digitalWrite(this->swdio_pin, in_data & 1);
+        digitalWrite(this->swdclk_pin, LOW);
+        delayMicroseconds(2);
+        in_data >>= 1;
+        digitalWrite(this->swdclk_pin, HIGH);
+        delayMicroseconds(2);
+    }
+}
+uint32_t swd::swd_Read(uint8_t bits) {
+    uint32_t out_data = 0;
+    uint32_t input_bit = 1;
+    if (cur_swd_direction == 1)
+        swd_Direction(0);
+    while (bits--) {
+        if (digitalRead(this->swdio_pin)) {
+            out_data |= input_bit;
+        }
+        digitalWrite(this->swdclk_pin, LOW);
+        delayMicroseconds(2);
+        input_bit <<= 1;
+        digitalWrite(this->swdclk_pin, HIGH);
+        delayMicroseconds(2);
+    }
+    return out_data;
+}
+void swd::swd_Direction(bool WorR) {  // 1 = Write 0 = Read
+    digitalWrite(this->swdio_pin, HIGH);
+    pinMode(this->swdio_pin, INPUT_PULLUP);
+    digitalWrite(this->swdclk_pin, LOW);
+    delayMicroseconds(2);
+    digitalWrite(this->swdclk_pin, HIGH);
+    delayMicroseconds(2);
+    if (WorR)
+        pinMode(this->swdio_pin, OUTPUT);
+    cur_swd_direction = WorR;
+}
+
+#define PORT_AP true
+#define PORT_DP false
+
+#define AP_NRF_RESET 0x00
+#define AP_NRF_ERASEALL 0x04
+#define AP_NRF_ERASEALLSTATUS 0x08
+#define AP_NRF_ERASEALL 0x04
+#define AP_NRF_APPROTECTSTATUS 0x0c
+#define AP_NRF_IDR 0xfc
+
+#define AP_CSW 0x00
+#define AP_TAR 0x04
+#define AP_DRW 0x0c
+#define AP_BD0 0x10
+#define AP_BD1 0x14
+#define AP_BD2 0x18
+#define AP_BD3 0x1c
+#define AP_DBGDRAR 0xf8
+#define AP_IDR 0xfc
+
+#define DP_ABORT 0x00
+#define DP_CTRLSTAT 0x04
+#define DP_SELECT 0x08
+#define DP_RDBUFF 0x0c
+
+nrfswd::nrfswd(uint8_t swdio, uint8_t swdclk) : swd(swdio, swdclk) {
+}
+bool nrfswd::init() {
+    uint32_t temp = swd_Init();
+    nrf_abort_all();
+    if (temp == 0x2ba01477) {  // if core id is readable the connection is working
+        if (showDebug) Serial.printf("Connected to nRF\n");
+        isConnected = true;
+        if (nrf_read_lock_state()) {  // nRF is unlocked so we can talk to the debugging interface
+            if (showDebug) Serial.printf("nRF is unlocked!\n");
+            isLocked = false;
+            nrf_halt();
+            nrf_read_ufcr();
+            return true;
+        } else {
+            if (showDebug) Serial.printf("nRF is locked ;_;\n");
+            isLocked = true;
+        }
+    } else {
+        isConnected = false;
+        isLocked = true;
+    }
+    return false;
+}
+uint32_t nrfswd::nrf_read_port(bool APorDP, uint8_t address) {
+    uint32_t temp = 0;
+    if (APorDP == PORT_AP)
+        AP_Read(address, temp);
+    else
+        DP_Read(address, temp);
+    DP_Read(DP_RDBUFF, temp);
+    DP_Read(DP_RDBUFF, temp);
+    Serial.printf("%s Read reg: 0x%02x : 0x%08x\r\n", APorDP ? "AP" : "DP", address, temp);
+    return temp;
+}
+void nrfswd::nrf_write_port(bool APorDP, uint8_t address, uint32_t value) {
+    uint32_t temp = 0;
+    if (APorDP == PORT_AP)
+        AP_Write(address, value);
+    else
+        DP_Write(address, value);
+    DP_Read(DP_RDBUFF, temp);
+    DP_Read(DP_RDBUFF, temp);
+    Serial.printf("%s Write reg: 0x%02x : 0x%08x\r\n", APorDP ? "AP" : "DP", address, value);
+}
+void nrfswd::nrf_abort_all() {
+    nrf_write_port(0, DP_ABORT, 0x1e);
+    nrf_write_port(0, DP_CTRLSTAT, 0x50000000);
+}
+void nrfswd::nrf_halt() {
+    AP_Write(AP_CSW, 0xa2000002);
+    AP_Write(AP_TAR, 0xe000edf0);
+    uint32_t retry = 500;
+    while (retry--) {
+        AP_Write(AP_DRW, 0xA05F0003);
+    }
+}
+void nrfswd::nrf_port_selection(bool new_port) {
+    DP_Write(DP_SELECT, new_port ? 0x01000000 : 0x00);  // Select AP
+}
+bool nrfswd::nrf_read_lock_state() {
+    uint32_t temp;
+    nrf_port_selection(1);
+    temp = nrf_read_port(1, AP_NRF_APPROTECTSTATUS);
+    nrf_port_selection(0);
+    return temp & 1;
+}
+void nrfswd::nrf_read_ufcr() {
+    nrf_info.codepage_size = read_register(0x10000010);
+    nrf_info.codesize = read_register(0x10000014);
+    nrf_info.flash_size = nrf_info.codepage_size * nrf_info.codesize;
+    nrf_info.config_id = read_register(0x1000005c);
+    nrf_info.device_id0 = read_register(0x10000060);
+    nrf_info.device_id1 = read_register(0x10000064);
+    nrf_info.info_part = read_register(0x10000100);
+    nrf_info.info_variant = read_register(0x10000104);
+    nrf_info.info_package = read_register(0x10000108);
+    nrf_info.sd_info_area = read_register(0x0000300C) & 0xffff;
+    nrf_info.ucir_lock = read_register(0x10001208);
+
+    if (showDebug) {
+        Serial.printf("Device: nRF%8X\n", nrf_info.info_part);
+        Serial.printf("Flash size: %i\r\n", nrf_info.flash_size);
+    }
+}
+uint32_t nrfswd::read_register(uint32_t address) {
+    uint32_t temp = 0;
+    bool state1 = AP_Write(AP_TAR, address);
+    bool state2 = AP_Read(AP_DRW, temp);
+    bool state3 = DP_Read(DP_RDBUFF, temp);
+    bool state4 = DP_Read(DP_RDBUFF, temp);
+    if (showDebug)
+        Serial.printf("%i%i%i%i Read Register: 0x%08x : 0x%08x\r\n", state1, state2, state3, state4, address, temp);
+    return temp;
+}
+void nrfswd::write_register(uint32_t address, uint32_t value) {
+    uint32_t temp = 0;
+    bool state1 = AP_Write(AP_TAR, address);
+    bool state2 = AP_Write(AP_DRW, value);
+    bool state3 = DP_Read(DP_RDBUFF, temp);
+    if (showDebug)
+        Serial.printf("%i%i%i Write Register: 0x%08x : 0x%08x\r\n", state1, state2, state3, address, value);
+}
+
+uint8_t nrfswd::erase_all_flash() {
+    write_register(0x4001e504, 2);
+    long timeout = millis();
+    while (read_register(0x4001e400) != 1) {
+        if (millis() - timeout > 100) return 1;
+    }
+    write_register(0x4001e50c, 1);
+    timeout = millis();
+    while (read_register(0x4001e400) != 1) {
+        if (millis() - timeout > 1000) return 1;
+    }
+    write_register(0x4001e504, 0);
+    timeout = millis();
+    while (read_register(0x4001e400) != 1) {
+        if (millis() - timeout > 100) return 1;
+    }
+    return 0;
+}
+
+uint8_t nrfswd::erase_uicr() {
+    write_register(0x4001e504, 2);
+    long timeout = millis();
+    while (read_register(0x4001e400) != 1) {
+        if (millis() - timeout > 100) return 1;
+    }
+    write_register(0x4001e514, 1);
+    timeout = millis();
+    while (read_register(0x4001e400) != 1) {
+        if (millis() - timeout > 1000) return 1;
+    }
+    write_register(0x4001e504, 0);
+    timeout = millis();
+    while (read_register(0x4001e400) != 1) {
+        if (millis() - timeout > 100) return 1;
+    }
+    return 0;
+}
+
+uint8_t nrfswd::erase_page(uint32_t page) {
+    write_register(0x4001e504, 2);
+    long timeout = millis();
+    while (read_register(0x4001e400) != 1) {
+        if (millis() - timeout > 100) return 1;
+    }
+    write_register(0x4001e508, page);
+    timeout = millis();
+    while (read_register(0x4001e400) != 1) {
+        if (millis() - timeout > 500) return 1;
+    }
+    write_register(0x4001e504, 0);
+    timeout = millis();
+    while (read_register(0x4001e400) != 1) {
+        if (millis() - timeout > 100) return 1;
+    }
+    return 0;
+}
+
+void nrfswd::nrf_soft_reset(){
+  nrf_port_selection(1);
+  nrf_write_port(1, AP_NRF_RESET, 1);
+  delay(100);
+  nrf_write_port(1, AP_NRF_RESET, 0);
+  nrf_port_selection(0);
+}
+
+uint8_t nrfswd::nrf_write_bank(uint32_t address, uint32_t buffer[], int size) {
+    if (!isConnected)
+        return 1;  // not connected to an nRF
+
+    if (size > 4096)
+        return 2;  // buffer bigger then a bank
+
+    uint32_t temp;
+
+    write_register(0x4001e504, 1);  // NVIC Enable writing
+    long timeout = millis();
+    while (read_register(0x4001e400) != 1) {
+        if (millis() - timeout > 100) return 3;
+    }
+
+    AP_Write(AP_CSW, 0xa2000012);
+    AP_Write(AP_TAR, address);
+
+    for (int posi = 0; posi < size; posi += 4) {
+        long end_micros = micros() + 400;  // wait till writing of nRF memory is done without asking for ready state
+        AP_Write(AP_DRW, buffer[posi / 4]);
+        while (micros() < end_micros) {
+        };
+    }
+
+    AP_Write(AP_CSW, 0xa2000002);
+    DP_Read(DP_RDBUFF, temp);
+    DP_Read(DP_RDBUFF, temp);
+
+    write_register(0x4001e504, 0);  // NVIC Diasble writing
+    timeout = millis();
+    while (read_register(0x4001e400) != 1) {
+        if (millis() - timeout > 100) return 3;
+    }
+
+    return 0;
+}
+uint8_t nrfswd::nrf_read_bank(uint32_t address, uint32_t buffer[], int size) {
+    if (!isConnected)
+        return 1;  // not connected to an nRF
+
+    uint32_t temp;
+
+    AP_Write(AP_CSW, 0xa2000012);
+    AP_Write(AP_TAR, address);
+    AP_Read(AP_DRW, temp);
+
+    uint32_t curr_word = 0;
+    for (int posi = 0; posi < size; posi += 4) {
+        AP_Read(AP_DRW, curr_word);
+        buffer[posi / 4] = curr_word;
+    }
+
+    AP_Write(AP_CSW, 0xa2000002);
+    DP_Read(DP_RDBUFF, temp);
+    DP_Read(DP_RDBUFF, temp);
+
+    return 0;
+}

ESP32_AP-Flasher/src/usbflasher.cpp

@@ -1,9 +1,10 @@
 #include <Arduino.h>
 
 #include "USB.h"
+#include "flasher.h"
 #include "powermgt.h"
-#include "serialconsole.h"
 #include "settings.h"
+#include "swd.h"
 #include "zbs_interface.h"
 
 USBCDC USBSerial;
@@ -11,6 +12,7 @@ USBCDC USBSerial;
 QueueHandle_t flasherCmdQueue;
 
 uint32_t usbConnectedStartTime = 0;
+bool serialPassthroughState = false;
 
 #define FLASHER_WAIT_A 0
 #define FLASHER_WAIT_T 1
@@ -22,55 +24,101 @@ uint32_t usbConnectedStartTime = 0;
 
 struct flasherCommand {
     uint8_t command = 0;
-    uint8_t len = 0;
+    uint32_t len = 0;
     uint8_t* data = nullptr;
 };
 
-#define FLASHER_MODE_UNKNOWN 0
-#define FLASHER_MODE_FLASHER 1
-#define FLASHER_MODE_CONSOLE 2
-volatile uint8_t usbFlasherMode = FLASHER_MODE_UNKNOWN;
-
-void enterConsoleMode() {
-    usbFlasherMode = FLASHER_MODE_CONSOLE;
-    xTaskCreate(consoleTask, "consoleTask", 10000, NULL, 2, &consoleTaskHandle);
-}
-
 int8_t powerPins[] = FLASHER_AP_POWER;
 #ifdef OPENEPAPERLINK_PCB
 int8_t powerPins2[] = FLASHER_EXT_POWER;
 int8_t powerPins3[] = FLASHER_ALT_POWER;
 #endif
 
-void sendFlasherAnswer(uint8_t answer_cmd, uint8_t* ans_buff, uint8_t len) {
-    uint8_t* answer_buffer = (uint8_t*)calloc(2 + 2 + len + 2, 1);
+bool autoFlash(flasher* f) {
+    f->getFirmwareMD5();
+
+    if (f->findTagByMD5()) {
+        // this tag currently contains original firmware, found its fingerprint
+        USBSerial.printf("Found original firmware tag, recognized its fingerprint (%s)\n", f->md5char);
+        f->readInfoBlock();
+        f->getFirmwareMac();
+        f->prepareInfoBlock();
+        f->writeInfoBlock();
+        USBSerial.printf("Attempting to perform a flash...\n");
+        if (f->writeFlashFromPack("/Tag_FW_Pack.bin", f->tagtype)) {
+            USBSerial.printf("Successfully flashed the tag!\n");
+            return true;
+        } else {
+            USBSerial.printf("Couldn't flash the tag, for some reason...\n");
+        }
+    } else if (f->getInfoBlockMD5()) {
+        // did find an infoblock MD5 that looks valid
+        if (f->findTagByMD5()) {
+            // did find the md5 in the database
+            USBSerial.printf("Found an already-flashed tag, recognized its fingerprint (%s)\n", f->md5char);
+            f->getInfoBlockMac();
+            f->getInfoBlockType();
+            f->readInfoBlock();
+            USBSerial.printf("Attempting to perform a flash...\n");
+            if (f->writeFlashFromPack("/Tag_FW_Pack.bin", f->tagtype)) {
+                USBSerial.printf("Successfully flashed the tag!\n");
+                return true;
+            } else {
+                USBSerial.printf("Couldn't flash the tag, for some reason...\n");
+            }
+        } else {
+            // couldn't find the md5 from the infoblock
+            USBSerial.printf("Found an already-flashed tag, but we couldn't find its fingerprint (%s) in the database\n", f->md5char);
+            return false;
+        }
+    } else {
+        // We couldn't recognize the tag from it's fingerprint...
+        USBSerial.printf("Found a tag but didn't recognize its fingerprint\n", f->md5char);
+        f->backupFlash();
+        USBSerial.printf("Saved this MD5 binary to filesystem\n");
+    }
+    return false;
+}
+
+void sendFlasherAnswer(uint8_t answer_cmd, uint8_t* ans_buff, uint32_t len) {
+    uint8_t* answer_buffer = (uint8_t*)calloc(3 + 2 + 2 + len + 2 + 13, 1);
     if (answer_buffer == nullptr) return;
     uint32_t CRC_value = 0xAB34;
     answer_buffer[0] = 'A';
     answer_buffer[1] = 'T';
     answer_buffer[2] = answer_cmd;
     CRC_value += answer_cmd;
-    answer_buffer[3] = len;
-    CRC_value += len;
+
+    for (uint8_t c = 0; c < 4; c++) {
+        answer_buffer[3 + c] = (uint8_t)(len >> (24 - (c * 8)));
+        CRC_value += answer_buffer[3 + c];
+    }
+
     for (int i = 0; i < len; i++) {
-        answer_buffer[4 + i] = ans_buff[i];
+        answer_buffer[7 + i] = ans_buff[i];
         CRC_value += ans_buff[i];
     }
-    answer_buffer[2 + 2 + len] = CRC_value >> 8;
-    answer_buffer[2 + 2 + len + 1] = CRC_value;
-    USBSerial.write(answer_buffer, 2 + 2 + len + 2);
+
+    answer_buffer[3 + 2 + 2 + len] = CRC_value >> 8;
+    answer_buffer[3 + 2 + 2 + len + 1] = CRC_value;
+    USBSerial.write(answer_buffer, 3 + 2 + 2 + len + 2);
+    // for(uint16_t c = 0; c< 3+2+2+len+2; c++){
+
+    //}
     free(answer_buffer);
 }
 
 void flasherUartHandler(uint8_t* data, uint8_t len) {
     static struct flasherCommand* cmd;
     static uint8_t flasherSerialState = FLASHER_WAIT_A;
-    static uint8_t flasherCmdDataIndex = 0;
+    static uint32_t flasherCmdDataIndex = 0;
     static uint16_t flasherCRC = 0xAB34;
     static uint32_t flasherLastCmd = 0;
+    static uint8_t curLenIndex = 0;
 
     if ((flasherSerialState != FLASHER_WAIT_A) && (millis() - flasherLastCmd >= 225)) {
         flasherSerialState = FLASHER_WAIT_A;
+        // we should probably do something with stale commands containing data (data leak!)
     }
 
     while (len--) {
@@ -81,7 +129,7 @@ void flasherUartHandler(uint8_t* data, uint8_t len) {
                     flasherSerialState = FLASHER_WAIT_T;
                     flasherLastCmd = millis();
                 } else {
-                    enterConsoleMode();
+                    // enterConsoleMode();
                 }
                 break;
             case FLASHER_WAIT_T:
@@ -98,15 +146,25 @@ void flasherUartHandler(uint8_t* data, uint8_t len) {
                 cmd->command = usbbyte;
                 flasherCRC += usbbyte;
                 flasherSerialState = FLASHER_WAIT_LEN;
+                curLenIndex = 0;
                 break;
             case FLASHER_WAIT_LEN:
                 flasherCRC += usbbyte;
-                if (usbbyte) {
-                    cmd->len = usbbyte;
-                    cmd->data = (uint8_t*)calloc(usbbyte, 1);
-                    flasherSerialState = FLASHER_WAIT_DATA;
-                } else {
-                    flasherSerialState = FLASHER_WAIT_CRCH;
+                cmd->len |= ((uint32_t)usbbyte) << (24 - (8 * curLenIndex));
+                curLenIndex++;
+                if (curLenIndex == sizeof(cmd->len)) {
+                    if (cmd->len) {
+                        // not 0
+                        cmd->data = (uint8_t*)calloc(cmd->len, 1);
+                        if (cmd->data == nullptr) {
+                            delete cmd;
+                            flasherSerialState = FLASHER_WAIT_A;
+                        }
+                        flasherSerialState = FLASHER_WAIT_DATA;
+                    } else {
+                        // 0 len, so skip to CRC immediately
+                        flasherSerialState = FLASHER_WAIT_CRCH;
+                    }
                 }
                 break;
             case FLASHER_WAIT_DATA:
@@ -123,10 +181,9 @@ void flasherUartHandler(uint8_t* data, uint8_t len) {
             case FLASHER_WAIT_CRCL:
                 flasherCRC -= ((uint16_t)usbbyte);
                 if (flasherCRC) {
-                    Serial.printf("CRC failed for flasher command :( %04X\n", flasherCRC);
                     cmd = nullptr;
+                    // we should probably delete the cmd and associated data here (data leak)
                 } else {
-                    if (usbFlasherMode == FLASHER_MODE_UNKNOWN) usbFlasherMode = FLASHER_MODE_FLASHER;
                     BaseType_t queuestatus = xQueueSend(flasherCmdQueue, &cmd, 0);
                     if (queuestatus == pdFALSE) {
                         if (cmd->data != nullptr) free(cmd->data);
@@ -141,12 +198,10 @@ void flasherUartHandler(uint8_t* data, uint8_t len) {
 }
 
 void resetFlasherState() {
-    if (usbFlasherMode != FLASHER_MODE_UNKNOWN) {
-        if (usbFlasherMode == FLASHER_MODE_CONSOLE) consoleStopTask();
-        Serial.print("Resetting flasher state");
-        usbFlasherMode = FLASHER_MODE_UNKNOWN;
-        usbConnectedStartTime = millis();
+    if (serialPassthroughState) {
+        Serial2.end();
     }
+    serialPassthroughState = false;
 }
 
 static void usbEventCallback(void* arg, esp_event_base_t event_base, int32_t event_id, void* event_data) {
@@ -196,11 +251,7 @@ static void usbEventCallback(void* arg, esp_event_base_t event_base, int32_t eve
                 {
                     uint8_t buf[data->rx.len];
                     size_t len = USBSerial.read(buf, data->rx.len);
-                    if (usbFlasherMode != FLASHER_MODE_CONSOLE) {
-                        flasherUartHandler(buf, len);
-                    } else {
-                        consoleUartHandler(buf, len);
-                    }
+                    flasherUartHandler(buf, len);
                 }
                 break;
             case ARDUINO_USB_CDC_RX_OVERFLOW_EVENT:
@@ -216,24 +267,36 @@ static void usbEventCallback(void* arg, esp_event_base_t event_base, int32_t eve
 typedef enum {
     CMD_GET_VERSION = 1,
     CMD_RESET_ESP = 2,
-    CMD_ZBS_BEGIN = 10,
-    CMD_RESET_ZBS = 11,
-    CMD_SELECT_PAGE = 12,
+    CMD_RESET = 11,
     CMD_SET_POWER = 13,
-    CMD_READ_RAM = 20,
-    CMD_WRITE_RAM = 21,
-    CMD_READ_FLASH = 22,
-    CMD_WRITE_FLASH = 23,
-    CMD_READ_SFR = 24,
-    CMD_WRITE_SFR = 25,
+
     CMD_ERASE_FLASH = 26,
-    CMD_ERASE_INFOBLOCK = 27,
+    CMD_ERASE_INFOPAGE = 27,
     CMD_SAVE_MAC_FROM_FW = 40,
     CMD_PASS_THROUGH = 50,
-} ZBS_UART_PROTO;
-uint32_t FLASHER_VERSION = 0x0000002F;
 
-static class ZBS_interface* zbs = nullptr;
+    CMD_SELECT_ZBS243 = 60,
+    CMD_SELECT_NRF82511 = 61,
+
+    CMD_SELECT_PORT = 70,
+
+    CMD_READ_INFOPAGE = 80,
+    CMD_READ_FLASH = 81,
+    CMD_WRITE_INFOPAGE = 82,
+    CMD_WRITE_FLASH = 83,
+    CMD_AUTOFLASH = 87,
+    CMD_COMPLETE = 88,
+
+} ZBS_UART_PROTO;
+uint32_t FLASHER_VERSION = 0x00000030;
+
+#define CONTROLLER_ZBS243 0
+#define CONTROLLER_NRF82511 1
+uint8_t selectedController = 0;
+uint8_t selectedFlasherPort;
+uint32_t currentFlasherOffset;
+flasher* zbsflasherp;
+nrfswd* nrfflasherp;
 
 void processFlasherCommand(struct flasherCommand* cmd) {
     uint8_t* tempbuffer;
@@ -255,128 +318,246 @@ void processFlasherCommand(struct flasherCommand* cmd) {
             delay(100);
             ESP.restart();
             break;
-        case CMD_ZBS_BEGIN:
-            if (zbs != nullptr) {
-                delete zbs;
-            }
-            zbs = new ZBS_interface;
-            if (cmd->data[0] & 1) {
-                spi_speed = 1000000;
-            } else {
-                spi_speed = 8000000;
-            }
-            curspeed = spi_speed;
-
-            if (cmd->data[0] & 2) {
-                powerPinCount = powerPins[0] != -1 ? sizeof(powerPins) : 0;
-                temp_buff[0] = zbs->begin(FLASHER_AP_SS, FLASHER_AP_CLK, FLASHER_AP_MOSI, FLASHER_AP_MISO, FLASHER_AP_RESET, (uint8_t*)powerPins, powerPinCount, spi_speed);
-            } else if (cmd->data[0] & 4) {
-#ifdef OPENEPAPERLINK_PCB
-                powerPinCount = powerPins3[0] != -1 ? sizeof(powerPins3) : 0;
-                temp_buff[0] = zbs->begin(FLASHER_ALT_SS, FLASHER_ALT_CLK, FLASHER_ALT_MOSI, FLASHER_ALT_MISO, FLASHER_ALT_RESET, (uint8_t*)powerPins3, powerPinCount, spi_speed);
-#endif
-            } else {
-#ifdef OPENEPAPERLINK_PCB
-                powerPinCount = powerPins2[0] != -1 ? sizeof(powerPins2) : 0;
-                temp_buff[0] = zbs->begin(FLASHER_EXT_SS, FLASHER_EXT_CLK, FLASHER_EXT_MOSI, FLASHER_EXT_MISO, FLASHER_EXT_RESET, (uint8_t*)powerPins2, powerPinCount, spi_speed);
-#endif
-            }
-            sendFlasherAnswer(cmd->command, temp_buff, 1);
-            break;
-        case CMD_RESET_ZBS:
-            zbs->reset();
-            temp_buff[0] = 1;
-            sendFlasherAnswer(cmd->command, temp_buff, 1);
-            break;
-        case CMD_SELECT_PAGE:
-            temp_buff[0] = zbs->select_flash(cmd->data[0] ? 1 : 0);
-            sendFlasherAnswer(cmd->command, temp_buff, 1);
-            break;
         case CMD_SET_POWER:
-            zbs->set_power(cmd->data[0] ? 1 : 0);
-            temp_buff[0] = 1;
-            sendFlasherAnswer(cmd->command, temp_buff, 1);
-            break;
-        case CMD_READ_RAM:
-            temp_buff[0] = zbs->read_ram(cmd->data[0]);
-            sendFlasherAnswer(cmd->command, temp_buff, 1);
-            break;
-        case CMD_WRITE_RAM:
-            zbs->write_ram(cmd->data[0], cmd->data[1]);
-            temp_buff[0] = 1;
-            sendFlasherAnswer(cmd->command, temp_buff, 1);
-            break;
-        case CMD_READ_FLASH:
-            tempbuffer = (uint8_t*)calloc(cmd->data[0], 1);
-            // cmd_buff[0] = len
-            // cmd_buff[1] << 8 | cmd_buff[2] = position
-            // Serial.printf("Loading %d bytes from %04X \n", cmd->data[0], (cmd->data[1] << 8 | cmd->data[2]));
-            for (int i = 0; i < cmd->data[0]; i++) {
-                tempbuffer[i] = zbs->read_flash((cmd->data[1] << 8 | cmd->data[2]) + i);
-            }
-            sendFlasherAnswer(cmd->command, tempbuffer, cmd->data[0]);
-            free(tempbuffer);
-            break;
-        case CMD_WRITE_FLASH:
-            // cmd_buff[0] = len
-            // cmd_buff[1] << 8 | cmd_buff[2] = position
-            // cmd_buff[3+i] = data
-            if (cmd->data[0] >= (0xff - 3)) {  // Len too high, only 0xFF - header len possible
-                temp_buff[0] = 0xEE;
-                sendFlasherAnswer(cmd->command, temp_buff, 1);
-                break;
-            }
-            // Serial.printf("Writing %d bytes to %04X \n", cmd->data[0], (cmd->data[1] << 8 | cmd->data[2]));
-            for (int i = 0; i < cmd->data[0]; i++) {
-                if (cmd->data[3 + i] != 0xff) {
-                    for (uint8_t attempts = 0; attempts < 10; attempts++) {
-                        zbs->write_flash((cmd->data[1] << 8 | cmd->data[2]) + i, cmd->data[3 + i]);
-                        if (zbs->read_flash((cmd->data[1] << 8 | cmd->data[2]) + i) == cmd->data[3 + i]) {
-                            goto flash_pass;
-                        }
-                        curspeed -= 100000;
-                        zbs->setSpeed(curspeed);
-                    }
-                flash_fail:
-                    temp_buff[0] = 0;
-                    sendFlasherAnswer(cmd->command, temp_buff, 1);
+            switch (selectedFlasherPort) {
+                case 0:
+                    powerControl(cmd->data[0], (uint8_t*)powerPins, 1);
+                    break;
+                case 1:
+                    powerControl(cmd->data[0], (uint8_t*)powerPins2, 1);
+                    break;
+                case 2:
+                    powerControl(cmd->data[0], (uint8_t*)powerPins3, 1);
                     break;
-                flash_pass:
-                    continue;
-                }
             }
-            temp_buff[0] = 1;
-            sendFlasherAnswer(cmd->command, temp_buff, 1);
+            sendFlasherAnswer(CMD_SET_POWER, NULL, 0);
             break;
-        case CMD_READ_SFR:
-            temp_buff[0] = zbs->read_sfr(cmd->data[0]);
-            sendFlasherAnswer(cmd->command, temp_buff, 1);
-            break;
-        case CMD_WRITE_SFR:
-            zbs->write_sfr(cmd->data[0], cmd->data[1]);
-            temp_buff[0] = 1;
-            sendFlasherAnswer(cmd->command, temp_buff, 1);
+        case CMD_RESET:
+            if (zbsflasherp != nullptr) {
+                zbsflasherp->zbs->reset();
+                delete zbsflasherp;
+                zbsflasherp = nullptr;
+            }
+            if (nrfflasherp != nullptr) {
+                nrfflasherp->nrf_soft_reset();
+                delete nrfflasherp;
+                nrfflasherp = nullptr;
+            }
+            sendFlasherAnswer(CMD_RESET, NULL, 0);
             break;
         case CMD_ERASE_FLASH:
-            zbs->erase_flash();
-            temp_buff[0] = 1;
-            sendFlasherAnswer(cmd->command, temp_buff, 1);
+            if (selectedController == CONTROLLER_NRF82511) {
+            } else if (selectedController == CONTROLLER_ZBS243) {
+                if (zbsflasherp == nullptr) return;
+                zbsflasherp->zbs->erase_flash();
+            }
+            sendFlasherAnswer(CMD_ERASE_FLASH, NULL, 0);
             break;
-        case CMD_ERASE_INFOBLOCK:
-            zbs->erase_infoblock();
-            temp_buff[0] = 1;
-            sendFlasherAnswer(cmd->command, temp_buff, 1);
+        case CMD_ERASE_INFOPAGE:
+            if (selectedController == CONTROLLER_NRF82511) {
+                nrfflasherp->erase_uicr();
+            } else if (selectedController == CONTROLLER_ZBS243) {
+                if (zbsflasherp == nullptr) return;
+                zbsflasherp->zbs->erase_infoblock();
+            }
+            sendFlasherAnswer(CMD_ERASE_INFOPAGE, NULL, 0);
+            break;
+        case CMD_SELECT_PORT:
+            selectedFlasherPort = cmd->data[0];
+            Serial.printf("Port selected = %d\n", cmd->data[0]);
+            break;
+        case CMD_SELECT_ZBS243:
+            zbsflasherp = new flasher;
+            temp_buff[0] = zbsflasherp->connectTag(selectedFlasherPort);
+            sendFlasherAnswer(CMD_SELECT_ZBS243, temp_buff, 1);
+            currentFlasherOffset = 0;
+            selectedController = CONTROLLER_ZBS243;
+            break;
+        case CMD_SELECT_NRF82511:
+            switch (selectedFlasherPort) {
+                case 0:
+                    powerControl(true, (uint8_t*)powerPins, 1);
+                    nrfflasherp = new nrfswd(FLASHER_AP_MISO, FLASHER_AP_CLK);
+                    break;
+                case 1:
+                    powerControl(true, (uint8_t*)powerPins2, 1);
+                    nrfflasherp = new nrfswd(FLASHER_EXT_MISO, FLASHER_EXT_CLK);
+                    break;
+                case 2:
+                    powerControl(true, (uint8_t*)powerPins3, 1);
+                    nrfflasherp = new nrfswd(FLASHER_ALT_MISO, FLASHER_ALT_CLK);
+                    break;
+            }
+            nrfflasherp->init();
+            temp_buff[0] = (nrfflasherp->isConnected && !nrfflasherp->isLocked);
+            sendFlasherAnswer(CMD_SELECT_NRF82511, temp_buff, 1);
+            currentFlasherOffset = 0;
+            selectedController = CONTROLLER_NRF82511;
+            break;
+        case CMD_READ_FLASH:
+            uint8_t* bufferp;
+            uint32_t cur_len;
+            if (selectedController == CONTROLLER_NRF82511) {
+                if (nrfflasherp == nullptr) return;
+                if (currentFlasherOffset >= nrfflasherp->nrf_info.flash_size) {
+                    sendFlasherAnswer(CMD_COMPLETE, temp_buff, 1);
+                } else {
+                    bufferp = (uint8_t*)malloc(1024);
+                    if (bufferp == nullptr) return;
+                    cur_len = (nrfflasherp->nrf_info.flash_size - currentFlasherOffset >= 1024) ? 1024 : nrfflasherp->nrf_info.flash_size - currentFlasherOffset;
+                    nrfflasherp->nrf_read_bank(currentFlasherOffset, (uint32_t*)bufferp, cur_len);
+                    currentFlasherOffset += cur_len;
+                    sendFlasherAnswer(CMD_READ_FLASH, bufferp, cur_len);
+                    if (bufferp != nullptr) free(bufferp);
+                }
+            } else if (selectedController == CONTROLLER_ZBS243) {
+                if (zbsflasherp == nullptr) return;
+                if (currentFlasherOffset >= 65536) {
+                    sendFlasherAnswer(CMD_COMPLETE, temp_buff, 1);
+                } else {
+                    bufferp = (uint8_t*)malloc(1024);
+                    if (bufferp == nullptr) return;
+                    cur_len = (65536 - currentFlasherOffset >= 1024) ? 1024 : 65536 - currentFlasherOffset;
+                    zbsflasherp->readBlock(currentFlasherOffset, bufferp, cur_len, false);
+                    currentFlasherOffset += cur_len;
+                    sendFlasherAnswer(CMD_READ_FLASH, bufferp, cur_len);
+                    if (bufferp != nullptr) free(bufferp);
+                }
+            }
+            break;
+        case CMD_READ_INFOPAGE:
+            uint8_t* ibufferp;
+            uint32_t icur_len;
+            if (selectedController == CONTROLLER_NRF82511) {
+                if (nrfflasherp == nullptr) return;
+                if (currentFlasherOffset >= 4096) {
+                    sendFlasherAnswer(CMD_COMPLETE, temp_buff, 1);
+                } else {
+                    ibufferp = (uint8_t*)malloc(1024);
+                    if (ibufferp == nullptr) return;
+                    icur_len = (4096 - currentFlasherOffset >= 256) ? 256 : 4096 - currentFlasherOffset;
+                    nrfflasherp->nrf_read_bank(0x10001000 + currentFlasherOffset, (uint32_t*)ibufferp, icur_len);
+                    currentFlasherOffset += icur_len;
+                    sendFlasherAnswer(CMD_READ_INFOPAGE, ibufferp, icur_len);
+                    if (ibufferp != nullptr) free(ibufferp);
+                }
+            } else if (selectedController == CONTROLLER_ZBS243) {
+                if (zbsflasherp == nullptr) return;
+                if (currentFlasherOffset >= 1024) {
+                    sendFlasherAnswer(CMD_COMPLETE, temp_buff, 1);
+                } else {
+                    ibufferp = (uint8_t*)malloc(1024);
+                    if (ibufferp == nullptr) return;
+                    icur_len = (1024 - currentFlasherOffset >= 256) ? 256 : 1024 - currentFlasherOffset;
+                    zbsflasherp->readBlock(currentFlasherOffset, ibufferp, icur_len, true);
+                    currentFlasherOffset += icur_len;
+                    sendFlasherAnswer(CMD_READ_INFOPAGE, ibufferp, icur_len);
+                    if (ibufferp != nullptr) free(ibufferp);
+                }
+            }
+            break;
+        case CMD_WRITE_FLASH:
+            if (selectedController == CONTROLLER_NRF82511) {
+                if (nrfflasherp == nullptr) return;
+                if (currentFlasherOffset >= nrfflasherp->nrf_info.flash_size) {
+                    sendFlasherAnswer(CMD_COMPLETE, temp_buff, 1);
+                } else {
+                    for (uint32_t c = currentFlasherOffset; c < (currentFlasherOffset + cmd->len);) {
+                        // very ugly and naive way to find out what page we're in, and erase all relevant pages before writing
+                        if (c % nrfflasherp->nrf_info.codepage_size == 0) {
+                            nrfflasherp->erase_page(c);
+                            Serial.printf("Erasing page %lu\n", c);
+                            c += nrfflasherp->nrf_info.codepage_size;
+                        } else {
+                            c++;
+                        }
+                    }
+                    nrfflasherp->nrf_write_bank(currentFlasherOffset, (uint32_t*)cmd->data, cmd->len);
+                    Serial.printf("wrote page to nrf\n");
+                    currentFlasherOffset += cmd->len;
+                    sendFlasherAnswer(CMD_WRITE_FLASH, NULL, 0);
+                }
+            } else if (selectedController == CONTROLLER_ZBS243) {
+                if (zbsflasherp == nullptr) return;
+                if (currentFlasherOffset >= 65536) {
+                    sendFlasherAnswer(CMD_COMPLETE, temp_buff, 1);
+                } else {
+                    zbsflasherp->writeBlock(currentFlasherOffset, cmd->data, cmd->len, false);
+                    currentFlasherOffset += cmd->len;
+                    sendFlasherAnswer(CMD_WRITE_FLASH, NULL, 0);
+                }
+            }
+            break;
+        case CMD_WRITE_INFOPAGE:
+            if (selectedController == CONTROLLER_NRF82511) {
+                if (nrfflasherp == nullptr) return;
+                if (currentFlasherOffset >= 4096) {
+                    sendFlasherAnswer(CMD_COMPLETE, temp_buff, 1);
+                } else {
+                    nrfflasherp->nrf_write_bank(0x10001000 + currentFlasherOffset, (uint32_t*)cmd->data, cmd->len);
+                    Serial.printf("wrote page to nrf\n");
+                    currentFlasherOffset += cmd->len;
+                    sendFlasherAnswer(CMD_WRITE_INFOPAGE, NULL, 0);
+                }
+            } else if (selectedController == CONTROLLER_ZBS243) {
+                if (zbsflasherp == nullptr) return;
+                if (currentFlasherOffset >= 1024) {
+                    sendFlasherAnswer(CMD_COMPLETE, temp_buff, 1);
+                } else {
+                    zbsflasherp->writeBlock(currentFlasherOffset, cmd->data, cmd->len, true);
+                    currentFlasherOffset += cmd->len;
+                    sendFlasherAnswer(CMD_WRITE_INFOPAGE, NULL, 0);
+                }
+            }
             break;
-        case CMD_SAVE_MAC_FROM_FW:
         case CMD_PASS_THROUGH:
+            Serial2.begin(115200, SERIAL_8N1, FLASHER_EXT_RXD, FLASHER_EXT_TXD);
+            USBSerial.println(">>>");
+            serialPassthroughState = true;
             break;
+        case CMD_AUTOFLASH:
+            if (selectedController == CONTROLLER_ZBS243) {
+                autoFlash(zbsflasherp);
+                zbsflasherp->zbs->reset();
+                delete zbsflasherp;
+                zbsflasherp=0;
+                USBSerial.write(0x04);
+            } else {
+                USBSerial.println("Not yet implemented!");
+            }
+            break;
+    }
+}
+
+uint32_t lastCmdTimeStamp = 0;
+#define USBFLASHER_CONNECTION_TIMEOUT 5000
+void flasherCommandTimeout() {
+    // this is triggered if no command has been received for a couple of seconds; this makes sure everything is cleaned up when the USB connection is lost
+    if (zbsflasherp != nullptr) {
+        delete zbsflasherp;
+        zbsflasherp = nullptr;
+    }
+    if (nrfflasherp != nullptr) {
+        delete nrfflasherp;
+        nrfflasherp = nullptr;
+    }
+    lastCmdTimeStamp = 0;
+}
+
+void tagDebugPassthrough() {
+    uint16_t len = Serial2.available();
+    if (len > 0) {
+        uint8_t* buf = (uint8_t*)malloc(len);
+        Serial2.read(buf, len);
+        Serial.write(buf, len);
+        USBSerial.write(buf, len);
+        free(buf);
     }
 }
 
 void usbFlasherTask(void* parameter) {
     flasherCmdQueue = xQueueCreate(10, sizeof(struct flasherCommand*));
-    consoleCmdQueue = xQueueCreate(10, sizeof(struct consoleCommand*));
-#if ARDUINO_USB_MODE
+#if ARDUINO_USB_MODEflash
 #warning Wrong USB mode is in use, check settings in platformio.ini
 #endif
     USB.onEvent(usbEventCallback);
@@ -387,13 +568,23 @@ void usbFlasherTask(void* parameter) {
 
     struct flasherCommand* cmd;
     while (true) {
-        BaseType_t queuereceive = xQueueReceive(flasherCmdQueue, &cmd, portMAX_DELAY);
+        while (serialPassthroughState) {
+            tagDebugPassthrough();
+            vTaskDelay(10 / portTICK_PERIOD_MS);
+        }
+        BaseType_t queuereceive = xQueueReceive(flasherCmdQueue, &cmd, 1000 / portTICK_PERIOD_MS);  // timeout every second to make sure the timeout gets triggered after a while
         if (queuereceive == pdTRUE) {
             processFlasherCommand(cmd);
+            lastCmdTimeStamp = millis();
             if (cmd->data != nullptr) {
                 free(cmd->data);
             }
             delete cmd;
+        } else {
+            if (lastCmdTimeStamp) {
+                if (millis() - lastCmdTimeStamp > USBFLASHER_CONNECTION_TIMEOUT)
+                    flasherCommandTimeout();
+            }
         }
     }
 }

ESP32_AP-Flasher/src/zbs_interface.cpp

@@ -8,7 +8,7 @@
 #include <stdio.h>
 
 #ifdef USE_SOFTSPI
-    #include <SoftSPI.h>
+#include <SoftSPI.h>
 #endif
 
 #include "powermgt.h"
@@ -40,7 +40,6 @@ uint8_t ZBS_interface::begin(uint8_t SS, uint8_t CLK, uint8_t MOSI, uint8_t MISO
     if (!spi) spi = new SPIClass(HSPI);
 #endif
 
-
     spiSettings = SPISettings(spi_speed, MSBFIRST, SPI_MODE0);
     spi_ready = 0;
 
@@ -58,7 +57,7 @@ void ZBS_interface::setSpeed(uint32_t speed) {
 }
 
 ZBS_interface::~ZBS_interface() {
-    if(spi)delete spi;
+    if (spi) delete spi;
 }
 void ZBS_interface::set_power(uint8_t state) {
     powerControl(state, _POWER_PIN, _POWER_PINS);
@@ -93,7 +92,11 @@ void ZBS_interface::enable_debug() {
 }
 
 void ZBS_interface::reset() {
-    spi->end();
+    if (spi) {
+        spi->end();
+        delete spi;
+        spi = nullptr;
+    }
     pinMode(_SS_PIN, INPUT);
     pinMode(_CLK_PIN, INPUT);
     pinMode(_MOSI_PIN, INPUT);