usb.c

#include "main-header.h"
#include "loader-api.h"
#include "addressing.h"
#include "espressif/esp8266_flash.h"
#include "powersave.h"
#include "proto-esp-usart.h"
#include "multiram.h"
#include "platform-header.h"
#include "wireless_state_machine.h"
#include "streams.h"
#include "usb.h"
#include "flashapp.h"
#include "app_update.h"
#include "metrics/metrics.h"
#include "hijack.h"
struct mculib_acm acm;
 
static int esp_bridge_mode = 0;
uint32_t acm_byte_received;
extern struct ringbuffer console_ringbuffer;
 
// we add bytes received from usb into the serial port buffer
// we share a buffer, so if we DO receive a byte from USB, we temporarily
// stop storing any bytes from serial port
void usb_byte_received(byte b) {
  uint8_t buf[1];
 
  buf[0] = b;
  if (hijack_receive_on_usb_acm((uint8_t *)&buf, 1)) {
    return;
  }
 
  IncMetric_METRIC_IRQ_USB;
  acm_byte_received = 500;
  console_add_byte(b);
}
 
void usb_restore_irq() {
  mculib_usb_set_received_callback(MAIN_MCULIBHANDLE, usb_byte_received);
}
 
void usb_start() {
  memset(&acm, 0, sizeof(acm));
  uint64_t modid = config_get_moduleid();
 
  if (modid == 0) {
    acm.serial[0] = 'n';
    acm.serial[1] = '-';
    node_to_str(get_my_node_id(), acm.serial + 2);
  } else {
    acm.serial[0] = 'm';
    acm.serial[1] = '-';
    snprintf(acm.serial + 2, 11, "%i", (int)modid);
  }
  //strncpy(acm.serial,"000E000002",12);
  //strncpy(acm.serial,"00000E000002",12);
  strncpy(acm.product, "module v3", 16);
  acm.usb_callback = NULL;
  acm.usb_received_callback = &usb_byte_received;
  mculib_enable_usb_acm(MAIN_MCULIBHANDLE, &acm);
}
 
/***************************** app update stuff ***********************************/
/*
 * the packet is simple:
 *
 * sender (host to singingcat)
 * start upload:
 * [uint8 ] 0xFF control
 * [uint8 ] 0x06 start_upload
 * [uint16] packet number (must be consecutive!)
 * [uint32] baseaddress
 *
 * data packet:
 * [uint8 ] 0xFF control
 * [uint8 ] 0x01: start data
 * [uint16] packet number (must be consecutive!)
 * [uint8 ] header length (how many bytes to skip to arrive at first data byte)
 * [uint16] data length
 * [uint32] crc32 checksum (over data only)
 *
 * just sync: (no replies expected) (sent by module if idle)
 * [uint8 ] 0xFF control
 * [uint8 ] 0x02: start data
 *
 * done:
 * [uint8 ] 0xFF control
 * [uint8 ] 0x05 done
 * [uint16] packet number (must be consecutive!)
 * [uint32] length in bytes
 * [uint32] total checksum
 * [uint8]  flags (none defined atm)
 *
 * replies (singingcat to host):
 * ACK
 * [uint8 ] 0xFF control
 * [uint8 ] 0x03 ACK
 * [uint16] packet number
 * [uint32] extra data (depends on what is being acked)
 * NACK
 * [uint8 ] 0xFF control
 * [uint8 ] 0x04 NACK
 * [uint16] packet number
 * [uint8 ] error code
 * [uint32] extra
 *
 * various actions (sent host->module)
 * [uint8 ] 0xFF control
 * [uint8] 0x07 perform an action
 * [uint32] # of action to perform
 * [uint32] # data to pass to action
 *
 * various actions with data (sent host->module)
 * [uint8 ] 0xFF control
 * [uint8] 0x08 perform an action with data
 * [uint32] # of data-action to perform
 * [uint32] number of bytes to follow
 * bytes # data to pass to action
 *
 * actions:
 * 1 reset
 * 2 running (returns current baseaddress in extra)
 * 3 setlatest (sets actiondata baseaddress to be latest in loader, returns new seq in extra)
 * 4 abort usb flash
 * 5 restart usb flash
 * 6 get nodeid
 * 7 get version
 * 8 set wifi chip to flash mode and bridge its serial port to usb
 *
 * actions with data
 * 1 set wifi ssid (data == ssid-nul-password)
 * 2 set cloudtoken
 */
#ifdef usb_app_debug
#define SENDTOUSB simulate_send_to_usb
#else
#define SENDTOUSB send_usb
#endif
 
const static char *ready = "[USB_APPFLASH]\r\n";
static byte pleasestop;
static byte *usbbuf;
static void *baseaddr;          // baseaddr advertised on start packet
static uint32_t flash_flags;    // flags set on start packet
static uint32_t total_size;     // total size
static int curpos = 0;
static byte state;
static byte packet_ready = 0;
struct usb_app_packet curpacket;
static uint16_t lastpack = 0;
static uint32_t received = 0;
static long timer_ts = 0;
static byte ackbuf[10];
static byte started = 0;
static int pctr; // count every 2 seconds roughly
// for howlong do we allow idle (== no packets received) before aborting?
#define MAX_SECS_USBFLASH 20
 
#ifdef usb_app_debug
uint8_t usbdebugmode = 1;
void usbdebugf(char *format, ...) {
  va_list args;
 
  if (!usbdebugmode) {
    return;
  }
 
  va_start(args, format);
  vprintf(format, args);
  va_end(args);
}
#endif
static void preerase(void *adr, uint32_t size);
void print_usb_app_packet(struct usb_app_packet *pack) {
#ifdef usb_app_debug
  if (pack->type == 0x06) {
    usbdebugf("[usbapp] Packet #0x%x START_UPLOAD: base=0x%X\r\n", pack->number, pack->start_upload.base);
  } else if (pack->type == 0x03) {
    usbdebugf("[usbapp] Packet #0x%x ACK\r\n", pack->number);
  } else if (pack->type == 0x04) {
    usbdebugf("[usbapp] Packet #0x%x NACK (code=%i)\r\n", pack->number, pack->nack.code);
  } else if (pack->type == 0x01) {
    usbdebugf("[usbapp] Packet #0x%x DATA: len=0x%X, checksum=0x%0X\r\n", pack->number, pack->data.data_length, pack->data.checksum);
  } else if (pack->type == 0x05) {
    usbdebugf("[usbapp] Packet #0x%x DONE, checksum=0x%X, len=%i,flags=%i\r\n", pack->number, pack->done.checksum, pack->done.len, pack->done.flags);
  } else if (pack->type == 0x07) {
    usbdebugf("[usbapp] Packet #0x%x ACTION, action=0x%X\r\n", pack->number, pack->action.action);
  } else if (pack->type == 0x08) {
    usbdebugf("[usbapp] Packet #0x%x ACTION_WITH_DATA, action=0x%X\r\n", pack->number, pack->action.action);
  } else {
    usbdebugf("[usbapp] Packet type=0x%x\r\n", pack->type);
  }
#endif
}
 
uint32_t parse32(byte *tmpbuf, int offset) {
  uint32_t res;
 
  res = (tmpbuf[offset++] & 0xFF);
  res = (res << 8) | (tmpbuf[offset++] & 0xFF);
  res = (res << 8) | (tmpbuf[offset++] & 0xFF);
  res = (res << 8) | (tmpbuf[offset++] & 0xFF);
  return res;
}
uint16_t parse16(byte *tmpbuf, int offset) {
  uint16_t res;
 
  res = (tmpbuf[offset++] & 0xFF);
  res = (res << 8) | (tmpbuf[offset++] & 0xFF);
  return res;
}
// expects headersize & type to be available in target
int usb_app_parse(byte *tmpbuf, int size, struct usb_app_packet *target) {
#ifdef usb_app_debug
  usbdebugf("[usbapp] Parsing struct from %i bytes, type %i\n", size, target->type);
#endif
  if (target->type == 0x06) { // start
    target->number = parse16(tmpbuf, 0);
    target->start_upload.base = parse32(tmpbuf, 2);
    target->start_upload.flags = parse32(tmpbuf, 6);
    target->start_upload.size = parse32(tmpbuf, 10);
    return 0;
  }
  if (target->type == 0x05) { // done
    target->number = parse16(tmpbuf, 0);
    target->done.checksum = parse32(tmpbuf, 2);
    target->done.len = parse32(tmpbuf, 6);
    target->done.flags = tmpbuf[10];
    return 0;
  }
  if (target->type == 0x07) { // action
    target->number = parse16(tmpbuf, 0);
    target->action.action = parse32(tmpbuf, 2);
    target->action.actiondata = parse32(tmpbuf, 6);
    return 0;
  }
  if (target->type == 0x08) { // action with data
    target->number = parse16(tmpbuf, 0);
    target->action.action = parse32(tmpbuf, 2);
    target->action.datalen = parse32(tmpbuf, 6);
    return 0;
  }
  if (target->type == 0x01) { // data
    target->number = parse16(tmpbuf, 0);
    target->data.header_length = tmpbuf[2];
    target->data.data_length = parse16(tmpbuf, 3);
    target->data.checksum = parse32(tmpbuf, 5);
    return 0;
  }
  if (target->type == 0x04) {
    target->number = parse16(tmpbuf, 0);
    target->nack.code = tmpbuf[2];
    target->nack.extra = parse32(tmpbuf, 3);
    return 0;
  }
  if (target->type == 0x03) {
    target->number = parse16(tmpbuf, 0);
    target->ack.extra = parse32(tmpbuf, 2);
    return 0;
  }
 
#ifdef usb_app_debug
  usbdebugf("[usbapp] Parsing struct failed from %i bytes, inv type: %i\n", size, target->type);
#endif
  return 5;
}
 
void usb_appupdate_byte_received(byte b) {
#ifdef usb_app_debug
  if (curpacket.type != 0x01) {
    usbdebugf("[usbapp] State: %i, Pos: %i, Headersize=%i, Received 0x%x\n", state, curpos, curpacket.headersize, b);
  }
#endif
  // if we're in state 0, only accept new packets...
  if (state == 0) {
    if (b != 0xFF) {
      return;
    }
    state = 1;
    curpos = 0;
    packet_ready = 0;
#ifdef usb_app_debug
    usbdebugf("[usbapp] Command start\n");
#endif
    return;
  } else if (state == 1) {
    curpacket.type = b;
#ifdef usb_app_debug
    usbdebugf("[usbapp] Command type %i\n", curpacket.type);
#endif
    state = 2;
    curpacket.datasize = 0;
    // headersize (from type onwards)
    if (curpacket.type == 0x06) {               // start
      curpacket.headersize = 14;
    } else if (curpacket.type == 0x07) {        // action
      curpacket.headersize = 10;
    } else if (curpacket.type == 0x08) {        // action with data
      curpacket.headersize = 10;
    } else if (curpacket.type == 0x01) {        // data
      curpacket.headersize = 9;
    } else if (curpacket.type == 0x05) {        //done
      curpacket.headersize = 11;
    } else if (curpacket.type == 0x04) {        // nack
      curpacket.headersize = 7;
    } else {
      printf("Invpackettype %i\r\n", curpacket.type);
      state = 0;
    }
    return;
  } else if (state == 2) {
    usbbuf[curpos] = b;
    curpos++;
    if (curpos >= curpacket.headersize) {
#ifdef usb_app_debug
      usbdebugf("[usbapp] command(header) completed\n");
#endif
      int i = usb_app_parse(usbbuf, curpos, &curpacket);
      if (i != 0) {
#ifdef usb_app_debug
        usbdebugf("[usbapp] parse failed: %i\n", i);
#endif
        state = 0;
        return;
      }
      print_usb_app_packet(&curpacket);
      state = 3;
      if ((curpacket.type == 0x01) || (curpacket.type == 0x08)) {   // var data in these packets
        curpos = 0;
      } else {
        packet_ready = 1;
      }
    }
  } else if (state == 3) {
    if (curpacket.type == 0x01) { // var-len data packet?
      usbbuf[curpos] = b;
      curpos++;
      if (curpos < curpacket.data.data_length) {
        return;
      }
      packet_ready = 1;
      state = 5;
      return;
    } else if (curpacket.type == 0x08) {   // var-len actiondata packet?
      usbbuf[curpos] = b;
      curpos++;
      if (curpos < curpacket.action.datalen) {
        return;
      }
      packet_ready = 1;
      state = 5;
      return;
    }
 
    printf("packet too early.\r\n");
    state = 0;
  } else {
    printf("Inv state %i\r\n", state);
    state = 0;
  }
}
 
static void send_usb(int len, byte *b) {
  int i;
 
  i = mculib_usb_send(MAIN_MCULIBHANDLE, len, b);
  if (i != len) {
    printf("USB send fail %i!=%i\r\n", b, i);
  }
}
 
static void sendsync() {
  byte b[3];
 
  SENDTOUSB(strlen(ready), (byte *)ready);
  b[0] = 0xFF;
  b[1] = 0x02;
  b[2] = '\n';
  SENDTOUSB(3, b);
  printf("idle sync\r\n");
}
static void add32(byte *buf, int offset, uint32_t value) {
  buf[offset++] = (value >> 24) & 0xFF;
  buf[offset++] = (value >> 16) & 0xFF;
  buf[offset++] = (value >> 8) & 0xFF;
  buf[offset++] = (value) & 0xFF;
}
 
void usb_app_update_ack(struct usb_app_packet *pack, uint32_t extra) {
#ifdef usb_app_debug
  usbdebugf("[usbapp] [usbapp] ack type %i, seq=%i,extra=%i\r\n", pack->type, pack->number, extra);
#endif
  ackbuf[0] = 0xFF;
  ackbuf[1] = 0x03;
  ackbuf[2] = (pack->number >> 8) & 0xFF;
  ackbuf[3] = (pack->number) & 0xFF;
  add32(ackbuf, 4, extra);
  ackbuf[8] = '\r';
  ackbuf[9] = '\n';
  SENDTOUSB(10, ackbuf);
}
void usb_app_update_nack(struct usb_app_packet *pack, uint8_t code, uint32_t extra) {
#ifdef usb_app_debug
  usbdebugf("[usbapp] nack type %i, seq=%i: %i\r\n", pack->type, pack->number, code);
#endif
  ackbuf[0] = 0xFF;
  ackbuf[1] = 0x04;
  ackbuf[2] = (pack->number >> 8) & 0xFF;
  ackbuf[3] = (pack->number) & 0xFF;
  ackbuf[4] = code;
  add32(ackbuf, 5, extra);
  ackbuf[9] = '\n';
  SENDTOUSB(10, ackbuf);
}
static void reinit() {
  pctr = 0;
  started = 0;
  curpos = 0;
  state = 0;
  lastpack = 0;
  received = 0;
  timer_ts = 0;
}
/**************************************************
* \\brief process action packets arriving on usb
* returns 0 if ok, otherwise errorcode. extra will be set to some extra value
**************************************************/
static int runaction_data(uint32_t action, uint32_t datalen, uint32_t *extra) {
  // data in 'usbbuf'
  usbbuf[datalen] = 0;
  //  printf("usbbuf (%i): \"%s\"\r\n",datalen,usbbuf);
  char *line = (char *)usbbuf;
 
  if (action == 1) { // set ssid
    int i;
    for (i = 0; i < datalen; i++) {
      if (line[i] == 0) {
        printf("setting ssid to \"%s\" with password \"%s\"\r\n", line, line + i + 1);
        esp8266_add_ap(line, line + i + 1);
        break;
      }
    }
    esp8266_reset();
    *extra = 0;
  } else if (action == 2) {   // set cloudtoken
    config_set_cloud_token(line);
    config_commit();
    *extra = 0;
  }
  return 0;
}
/**************************************************
* \\brief process action packets arriving on usb
* returns 0 if ok, otherwise errorcode. extra will be set to some extra value
**************************************************/
static int runaction(uint32_t action, uint32_t data, uint32_t *extra) {
#ifdef usb_app_debug
  usbdebugf("[usbapp] action %i, data %i\r\n", action, data);
#endif
  *extra = 0;
  if (action == 1) {
    reboot_hard(); // won't send an answer...
  }
  if (action == 2) {
    *extra = (uint32_t)get_app_header();
#ifdef usb_app_debug
    usbdebugf("[usbapp] action request for base address: 0x%p\n", *extra);
#endif
  } else if (action == 3) {
    int r = set_latest_app((void *)data);
    if (r != 0) {
      return r;
    }
    struct meta_header *m = get_other_meta_header((void *)data);
    if (m == NULL) {
      return 44;
    }
    *extra = m->meta_seq;
  } else if (action == 5) {
    reinit();
  } else if (action == 4) {
    pleasestop = 1;
    // 5 is special and not here
  } else if (action == 6) {
    *extra = get_my_node_id();
  } else if (action == 7) {
    *extra = CNW_BUILD_VERSION;
  } else if (action == 8) {
    // wifi update
    int wf = esp8266_get_firmware_version();
    esp_bridge_mode = 1;
    usb_app_update_ack(&curpacket, wf); // must ack here, because bridge mode prevents us from ack'ing
    esp8266_wififlash_usb_bridge();     // this isn't quite right. we should only 'ack' once the bridge is ready
  } else {
    return 111;
  }
  return 0;
}
 
/**************************************************
* \\brief process packets arriving on usb
**************************************************/
void usb_process_packet_appupdate() {
  int i;
 
  if (!packet_ready) {
    return;
  }
  pctr = 0;
  // this particular action may be out of order
  if ((curpacket.type == 0x07) && (curpacket.action.action == 5)) {
    usb_app_update_ack(&curpacket, 0);
    reinit();
  }
 
  if (curpacket.number == 0) {
    printf("[usbapp] Ignored pack 0\r\n");
    packet_ready = 0;   // clear it again
    state = 0;
    curpos = 0;
  }
 
  // check for out-of-order packets
  if (curpacket.number != (lastpack + 1)) {
#ifdef usb_app_debug
    usbdebugf("[usbapp] Out of sequence. got %i, wanted %i\r\n", curpacket.number, (lastpack + 1));
#endif
    usb_app_update_nack(&curpacket, USBAPP_ERROR_PACKET_ORDER, ((curpacket.number << 16) | (lastpack + 1)));
    packet_ready = 0;   // clear it again
    state = 0;
    curpos = 0;
    return;
  }
  lastpack = curpacket.number;
#ifdef usb_app_debug
  usbdebugf("[usbapp] Processing type %i\r\n", curpacket.type);
#endif
  /********************************* START UPLOAD *******************/
  if (curpacket.type == 0x06) { // start_upload...
    if (((uint32_t)get_app_header()) == curpacket.start_upload.base) {
      started = 0;
      usb_app_update_nack(&curpacket, USBAPP_ERROR_CURRENT_BASE, (uint32_t)get_app_header());
      goto out;
    }
    printf("USB Flash. base: %p, length: %i\r\n", (void *)curpacket.start_upload.base, curpacket.start_upload.size);
    baseaddr = (void *)curpacket.start_upload.base;
    flash_flags = curpacket.start_upload.flags;
    flash_app_init(baseaddr, flash_flags);
    started = 1;
    usb_app_update_ack(&curpacket, 0);
    if (flash_flags & (1 << FLASH_FLAGS_PRE_ERASE)) {
      preerase((void *)curpacket.start_upload.base, curpacket.start_upload.size);
    }
    /********************************* ACTION *******************/
  } else if (curpacket.type == 0x07) {  //action
    uint32_t res;
    int r = runaction(curpacket.action.action, curpacket.action.actiondata, &res);
    if (r != 0) {
      usb_app_update_nack(&curpacket, USBAPP_ERROR_ACTION, r);
      goto out;
    } else {
#ifdef usb_app_debug
      usbdebugf("[usbapp] Action result: %i\r\n", res);
#endif
      usb_app_update_ack(&curpacket, res);
    }
    /********************************* ACTION WITH DATA *******************/
  } else if (curpacket.type == 0x08) {  //action with data
    uint32_t res;
    int r = runaction_data(curpacket.action.action, curpacket.action.datalen, &res);
    if (r != 0) {
      usb_app_update_nack(&curpacket, USBAPP_ERROR_ACTION, r);
      goto out;
    } else {
#ifdef usb_app_debug
      usbdebugf("[usbapp] Action result: %i\r\n", res);
#endif
      usb_app_update_ack(&curpacket, res);
    }
    /********************************* FINISH *******************/
  } else if (curpacket.type == 0x05) {  // finish
    total_size = curpacket.done.len;
    // received enough data?
    if (received != total_size) {
#ifdef usb_app_debug
      usbdebugf("[usbapp] ERROR: Received %i bytes, but advertised was %i\n", received, total_size);
#endif
      usb_app_update_nack(&curpacket, USBAPP_ERROR_TOTAL_SIZE_MISMATCH, received);
      goto out;
    }
    // checksum?
    uint32_t c = crc32(baseaddr, received);
    if (c != curpacket.done.checksum) {
#ifdef usb_app_debug
      usbdebugf("[usbapp] ERROR: checksum calculated: 0x%X, reported: 0x%X\n", c, curpacket.done.checksum);
#endif
 
      usb_app_update_nack(&curpacket, USBAPP_ERROR_TOTAL_CHECKSUM, c);
      goto out;
    }
    // all went well, ...
    mculib_flash_lock(MAIN_MCULIBHANDLE);
    usb_app_update_ack(&curpacket, 0);
    /********************************* DATA *******************/
  } else if (curpacket.type == 0x01) {  // data
    if (!started) {
      usb_app_update_nack(&curpacket, USBAPP_ERROR_DATA_BEFORE_START, 0);
      goto out;
    }
    if (curpacket.data.data_length != 512) {
      usb_app_update_nack(&curpacket, USBAPP_ERROR_DATA_SIZE_INVALID, 512);
      goto out;
    }
 
    // verify checksum
    uint32_t c = crc32(usbbuf, curpacket.data.data_length);
    if (c != curpacket.data.checksum) {
#ifdef usb_app_debug
      usbdebugf("[usbapp] checksum calculated: 0x%X, reported: 0x%X\n", c, curpacket.data.checksum);
#endif
 
      usb_app_update_nack(&curpacket, USBAPP_ERROR_DATA_CHECKSUM, c);
      goto out;
    }
    i = flash_app_consumer(0, usbbuf, curpacket.data.data_length);
    if (i != 0) {
#ifdef usb_app_debug
      usbdebugf("[usbapp] flash failed: %i\n", i);
#endif
      usb_app_update_nack(&curpacket, USBAPP_ERROR_FLASH_FAILURE, i);
      goto out;
    }
    /*
     * void *app = findAppAtBlock(baseaddr + received); // code intented to prevent accidental overwrite of app. not working
     * usb_app_update_ack(&curpacket, (uint32_t)app);
     */
    usb_app_update_ack(&curpacket, 0);
    received = received + curpacket.data.data_length;
    /********************************* UNKNOWN PACKET *******************/
  } else {
#ifdef usb_app_debug
    usbdebugf("[usbapp] Failed to process packet of type %i\n", curpacket.type);
#endif
    usb_app_update_nack(&curpacket, USBAPP_ERROR_INV_PACKET, curpacket.type);
  }
out:
  packet_ready = 0;   // clear it again
  state = 0;
  curpos = 0;
}
 
int usb_start_app_update() {
  printf("[usbapp] Starting usb app update\r\n");
  pctr = 0;
  curpos = 0;
  state = 0;
  lastpack = 0;
  received = 0;
  timer_ts = 0;
  usbbuf = mpm_reserve(1024);
  if (usbbuf == NULL) {
    printf("no usbbuf\r\n");
    return 10;
  }
  mculib_usb_set_received_callback(MAIN_MCULIBHANDLE, usb_appupdate_byte_received);
  print_to_usb(0);
  printf(ready);
  SENDTOUSB(strlen(ready), (byte *)ready);
  return 0;
}
void usb_finish_app_update() {
  usb_restore_irq();
  print_to_usb(1);
  mpm_free(usbbuf);
  ringbuffer_init(&console_ringbuffer); // remove left-over stuff from usb buffer(s)
  printf("[usbapp] finished usb app update\r\n");
}
 
/* called by main - return 0 on success, otherwise failure number */
int usb_appupdate() {
  unhijack_usb_acm();
  power_set_speed(10);
  int i = usb_start_app_update();
 
  if (i != 0) {
    return i;
  }
  int syncctr = 0;
 
  pleasestop = 0;
  for (;;) {
    if (pleasestop) {
      break;
    }
    if (mculib_has_time_passed(2, &timer_ts)) {
      pctr++;
      syncctr++;
      if ((pctr >= 3) && (syncctr >= 3)) {
        syncctr = 0;
        sendsync();
      }
      if (pctr >= (MAX_SECS_USBFLASH / 2)) {
        break;
      }
    }
    avoid_watchdog();
    usb_process_packet_appupdate();
  }
  flash_app_close(0, 0);
  usb_finish_app_update();
  return 0;
}
 
 
static void preerase(void *adr, uint32_t size) {
  int r;
 
  printf("Erasing %i bytes @ %p\r\n", size, adr);
  mculib_flash_lock(MAIN_MCULIBHANDLE);
  if ((r = mculib_flash_unlock(MAIN_MCULIBHANDLE))) {
    printf("Flash unlock failed (%i)\r\n", r);
    mculib_flash_lock(MAIN_MCULIBHANDLE);
    return;
  }
  if ((r = mculib_flash_erase(MAIN_MCULIBHANDLE, adr, size))) {
    printf("Flash erase failed (%i)\r\n", r);
  }
  mculib_flash_lock(MAIN_MCULIBHANDLE);
  printf("Erase complete.\r\n");
}