/*                                       -*- mode:C++; fill-column: 100 -*-
  AHAX:BOOT3 - A 'tertiary bootloader' for burning SFB sketches
  Copyright (C) 2009 The Regents of the University of New Mexico.  All rights reserved.

  This sketch is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
  USA
*/

#include "sfb.h"
#include "irq.h"                /* For disableInterrupts */
#include "timer.h"              /* For CCLK */

#define BOOT3_SOURCEID 0x00077619  /**< AHAX in B36 */
#define BOOT3_PROGRAMID 0x012b7e97 /**< BOOT3 in B36 */

#define CCLK_KHZ (CCLK/1000)

extern char __text_start, __text_end; /* From sfbloader.ld */ 

static void checkForSketches(bool resetKill) ;
static void displayBootMode(bool on) ;

void bootblock_bootcount_initialization() {
  /* Don't increment the bootcount during boot3 startup -- _we_ aren't the real boot */
  randomSeed(getBootBlockMergedChecksum());  /* But do init rnd#'s with something that changes often */
}

/**
 * The command numbers for performing In-Application-Programming
 * operations.
 */
enum IAPCommandCodes {
  PREPARE_SECTOR_FOR_WRITE=50,
  COPY_RAM_TO_FLASH=51,
  ERASE_SECTOR=52,
  BLANK_CHECK_SECTOR=53,
  READ_PART_ID=54,
  READ_BOOT_VER=55,
  COMPARE=56,
  REINVOKE_ISP=57
};

#define CMD_SUCCESS 0
#define IAP_ADDRESS 0x7FFFFFF1

typedef void (*IAPFunction)(u32 params[5],u32 results[5]);

static u32 IAPParams[5];
static u32 IAPResults[5];

void doIAP(u32 a, u32 b, u32 c, u32 d = 0, u32 e = 0) {
  IAPParams[0] = a;
  IAPParams[1] = b;
  IAPParams[2] = c;
  IAPParams[3] = d;
  IAPParams[4] = e;

  disableInterruptsFunction();
  IAPFunction iap = (IAPFunction) IAP_ADDRESS;
  iap(IAPParams,IAPResults);
  enableInterruptsFunction();

  API_ASSERT(IAPResults[0]==CMD_SUCCESS,E_B3_IAP_FAILURE);
}

/* writeCode256 erases the appropriate sector when the first address
   of the sector is written. */

#define MIN_WRITE_ADDRESS 0x10000

void writeCode256(u32 destAddr, u8 * data) {
  API_ASSERT(((destAddr)&255)==0, E_API_POINTER_ALIGNMENT); // Dest must be multiple of 256
  API_ASSERT_GREATER_EQUAL(destAddr, MIN_WRITE_ADDRESS);    // and a place we're willing to burn
  API_ASSERT((((u32)data)&3)==0, E_API_POINTER_ALIGNMENT);  // Src must be multiple of 4
  API_ASSERT(data >= (u8*) 0x40000000 && data+256 <= (u8*) 0x40008000, E_API_POINTER_ALIGNMENT); // data must all be in fastram.

  u8 sector = sectorOfAddress(destAddr); // sectorOfAddress will assert E_API_BAD_SECTOR if destAddr too large
  u32 start = sectorStart(sector);
  if (destAddr==start) {
    doIAP(PREPARE_SECTOR_FOR_WRITE,sector,sector,CCLK_KHZ);
    doIAP(ERASE_SECTOR,sector,sector,CCLK_KHZ);
  }
  doIAP(PREPARE_SECTOR_FOR_WRITE,sector,sector,CCLK_KHZ);
  doIAP(COPY_RAM_TO_FLASH,destAddr,(u32) data, 256, CCLK_KHZ);
}

#define MAX_SKETCHES 8  // Though so far we really only support one at a time

enum { NO_ANSWER, REJECTED, REPLIED, SELECTED };

/**
 * A structure holding state associated with a program we may burn or
 * are in the process of burning.
 */
struct Programmer {
  SFBChecksum cs;

  SFBProvenance *(sketchHeaders[MAX_SKETCHES]); // Code we already have
  u8 sketchHeadersFound;
  u8 newestSketchIndex;

  SFBProvenance bestProgram;    // Best code being offered to us
  u32 bootFlags;                // Boot flags out of eeprom
  u32 bestStartAddr;            // Startaddr of best code
  u16 snonce;                   // Snonce of best code
  u8 face;                      // Source face of best
  u8 haveBest;                  // Do we have a winning offer in the clubhouse?
  u8 currentSector;
  u8 lastSector;
  u32 nextAddress;              // Within currentSector
  SFBChecksum sectorChecksum;   // For checking if the sector burn was good
  u32 lastAddress;              // Within lastSector
  u32 alarmNumber;              // Initted in setup()
  u32 timeout;
  u16 rnonce[FACE_COUNT];
  u8 faceState[FACE_COUNT]; 
  u8 state;
  u8 bootMode;
  u8 nctr;
  u8 monitorFace;
  u8 programmingFlags;
} ps;

#include "statedefs.h"           // Suck in the state machine tables.

// Same as sectorStart but skip premagic section of bestprogram provenance block
static u32 adjustedSectorStart(int sector) {
  u32 startAddr = sectorStart(sector);
  u32 length = sectorSize(sector);

  // Adjust startAddr to skip the premagic portion of the provenance block, if we're in that sector
  if (startAddr <= ps.bestStartAddr && startAddr+length >= ps.bestStartAddr+sizeof(SFBProvenance)) {

    u32 adjustedStart = ps.bestStartAddr+STRUCT_MEMBER_OFFSET(SFBProvenance,magic);
    startAddr = adjustedStart;
  }
  return startAddr;
}

// Same as sectorSize but compensate for premagic skipping and incomplete last sector
static u32 adjustedSectorSize(int sector, u32 adjustedStartAddr) {

  u32 startAddr = sectorStart(sector);
  u32 length = sectorSize(sector);

  length -= adjustedStartAddr-startAddr;

  if (adjustedStartAddr+length > ps.lastAddress) // Usually won't need whole last sector
    length = ps.lastAddress-adjustedStartAddr; 

  return length;
}

void stepProgrammer(u32 when,u8 * packet) {
  if (packet) {
    if (zpacketPrefix(packet,"Pl")) {

      // Announce loader version
      facePrintf(Body.source(),"PL+%l%l%l%l\n",
                 BOOT3_SOURCEID,BOOT3_PROGRAMID,
                 SKETCH_BUILD_DATE,SKETCH_BUILD_TIME);
      return;
    }
    if (zpacketPrefix(packet,"Ph")) {
      /* Eat random code announcements -- we'll use PaA for that */
      return;
    }
  }
  while (stepFunctions[ps.state](when,packet)) ;
}

void programmerDispatcher(u8 * packet) {
  stepProgrammer(millis(), packet);
}

void programmerTimeout(u32 when) {
  stepProgrammer(when, 0);
}

void setState(u32 newState, u32 timeout, bool randomize = true) {
  ps.state = newState;
  if (randomize)
    ps.timeout = millis()+random(timeout) + (timeout>>1);
  else
    ps.timeout = millis()+timeout;

  Alarms.set(ps.alarmNumber,ps.timeout);
}

void initProgrammer(Programmer & ps) {
  ps.cs[0] = 0;
  ps.cs[1] = 0;

  for (u32 i = 0; i<=MAX_SKETCHES; ++i) {
    ps.sketchHeaders[i] = 0;
  }
  ps.sketchHeadersFound = 0;
  ps.newestSketchIndex = 0;  // Not valid unless sketchHeadersFound>0

  ps.bootFlags = 0;
  ps.bestStartAddr = 0;
  ps.snonce = 0;
  ps.face = 0;

  ps.haveBest = 0;
  ps.currentSector = 0;
  ps.lastSector = 0;

  ps.nextAddress = 0;
  ps.lastAddress = 0;

  //ps.alarmNumber is initted in setup()
  ps.timeout = 0;

  for (u32 i = NORTH; i<=WEST; ++i) {
    ps.rnonce[i] = 0;
    ps.faceState[i] = NO_ANSWER;

    pinMode(pinInFace(FACE_LED_PIN,i),REFLEX_MODE);  
    Faces[i].begin();           // Start packet processing on all faces
  }
  setState(PS_RESET,0);
  ps.bootMode = UNKNOWN_BOOT_MODE;
  displayBootMode(true);
  ps.nctr = 0;
  ps.programmingFlags = 0;
}

static void setWhite(bool on) {
  ledSet(BODY_RGB_RED_PIN,on);
  ledSet(BODY_RGB_GREEN_PIN,on);
  ledSet(BODY_RGB_BLUE_PIN,on);
}

u8 buttonDownOnSplash = 0;

void splashScreen(bool begin) {
  delay(100);
  for (u32 i = 0; i < (begin?3:1); ++i) {
    for (u32 face = NORTH; face <= WEST; ++face) {
      ledOn(pinInFace(FACE_LED_PIN,face));
    }
    if (i==1) {
      setWhite(true);
      delay(150);
      setWhite(false);
    } else
      delay(60);
    for (u32 face = NORTH; face <= WEST; ++face) {
      ledOff(pinInFace(FACE_LED_PIN,face));
    }
    delay(150);
  }
  delay(300);
  if (buttonDown())
    buttonDownOnSplash = 1;
}

static void monitorCommandReceiver(u8 *);
void setup() {

  bool watchdogFailure = watchdogGetTimeoutFlag() && !watchdogItWasSuicide();
  setBootBlockWatchdogResetFlag(!watchdogFailure);
  watchdogClearTimeoutFlag();

  SET_REFLEX_FLAGS(RF_DIE_ALOUD);
  splashScreen(true);
  for (u32 i = NORTH; i <= WEST; ++i) {
    Body.reflexReflex('P',programmerDispatcher,Body.faceToFlag(i));
    Body.empty(programmerDispatcher,Body.faceToFlag(i));
    Faces[i].begin(9600,false); // Start up in conscious mode, no brn
  }
  Body.otherwise(monitorCommandReceiver);

  ps.alarmNumber = Alarms.create(programmerTimeout);
  API_ASSERT(ps.alarmNumber!=0,E_API_NO_MORE_ALARMS);
  initProgrammer(ps);
}

////////////////////// 
/////////// PROGRAMMING STATE MACHINE STATES
static u32 monitorCommandsProcessed = 0;
static bool commandCompleted = false;
static u8 commandPending;
static u32 commandPendingArgument;
static u32 commandConfirmSteps = 0;
static u8 commandButtonState;

bool stepERROR_RESET(u32, u8 * packet) {

  buttonDownOnSplash = buttonDown(); // Reacquire button status now
  commandCompleted = false;
  commandPending = 0;
  commandPendingArgument = 0;
  commandConfirmSteps = 0;
  commandButtonState = 0;

  setState(PS_RESET,0);
  return true;
}

bool stepRESET(u32, u8 * packet) {

  if (packet)                   // Ignore packets now
    return false;

  initProgrammer(ps);

  ////////
  /// CHECK EEPROM FOR 'NOTES TO SELF'

  ps.bootFlags = getBootBlockFlags();
  ps.bootMode = getBootBlockBootMode();
  displayBootMode(true);

  if (buttonDownOnSplash==1) {
    setLogFace(ALL_FACES);
    setState(PS_MONITOR_WAIT,0);
    ps.nctr = 0;
  } else {
    setState(PS_BOOT_MODE,0);
  }
  return true;
}

static void setFaceLights(u8 mask) {
  for (u32 f = NORTH; f <= WEST; ++f) {
    ledSet(pinInFace(FACE_LED_PIN,f),mask&(1<<f));
  }
}
static void releaseFaceLights() {
  setFaceLights(0);

  for (u32 f = NORTH; f <= WEST; ++f) {
    pinMode(pinInFace(FACE_LED_PIN,f),REFLEX_MODE);
  }
}
#define MAX_MONITOR_WAIT_STEPS 120
bool stepMONITOR_WAIT(u32, u8 * packet) {

  if (packet && packetLength(packet)==0) { // Receiving empty packet enters the monitor
    ps.monitorFace = Body.source();
    setLogFace(ps.monitorFace);
    ps.nctr = 0;
    setState(PS_MONITOR_ANNOUNCE,0);
    return true;
  }
  if (packet) {                // Ignore other packets
    return false;
  }

  if (ps.nctr%20==0)
    logQuiet("Send empty packet (often: HIT ENTER) for boot monitor.  Waiting %d\n",
             (MAX_MONITOR_WAIT_STEPS-ps.nctr)/20);
  if (ps.nctr++ >= MAX_MONITOR_WAIT_STEPS) {
    setFaceLights(0);
    setState(PS_BOOT_MODE,0); // Give up.
  } else {
    u32 dir = GET_MAP4BY2(MAP4BY2(NORTH,WEST,SOUTH,EAST),ps.nctr&3);
    setFaceLights(~(1<<dir));
    setState(PS_MONITOR_WAIT,150);
  }
  return false;
}

static void badCommand(u8 * packet) {
  logNormal("Bad command '%s'\n",packet);
}
static const char * monhelp[] = {
  "--Commands--\n",
  "x       - exit and reboot\n",
  "r       - set RED BOOT *\n",
  "g       - set GREEN BOOT *\n",
  "b       - set BLUE BOOT *\n",
  "o=OWNER - set board OWNER *\n",
  "n=NAME  - set board NAME *\n",
  "k       - toggle kill flag on existing sketch *\n",
  "pFDS    - boot power: F:NSEW D:IO S:+- Eg pSO- South Output off\n",
  "i       - board info\n",
  "h       - this help\n",
  "  * - requires confirmation\n"
};
void printHelp() {
  for (u32 h = 0; h < sizeof(monhelp)/sizeof(monhelp[0]); ++h) {
    logNormal(monhelp[h]);
  }
}

static void reportPowerStatus() {
  logNormal(" Boot Power I/O:" "  N:%s/%s" "  S:%s/%s" "  E:%s/%s" "  W:%s/%s" "\n",
            getBootBlockPowerIn(NORTH)?"ON ":"OFF",
            getBootBlockPowerOut(NORTH)?"ON ":"OFF",
            getBootBlockPowerIn(SOUTH)?"ON ":"OFF",
            getBootBlockPowerOut(SOUTH)?"ON ":"OFF",
            getBootBlockPowerIn(EAST)?"ON ":"OFF",
            getBootBlockPowerOut(EAST)?"ON ":"OFF",
            getBootBlockPowerIn(WEST)?"ON ":"OFF",
            getBootBlockPowerOut(WEST)?"ON ":"OFF");
}

static void printBoardInfo() {
  logNormal(" Board:\n");
  logNormal("  Architecture code: %d\n",
            SKETCH_ARCH_CODE);
  logNormal("  Owner & Board id: %t/%t  BBVers=%d  Flags=%x  Boot Mode=%s\n",
            getBootBlockOwnerId(),
            getBootBlockBoardId(),
            getBootBlockVersion(),
            getBootBlockFlags(),
            bootModeColorName(getBootBlockBootMode()));
  logNormal("  Power on hours: %d   Boot count: %d   Sketches burned: %d\n",
            getBootBlockPowerOnHours(),
            getBootBlockBootCount(),
            getBootBlockBurnCount());
  logNormal("  Watchdog reset flags: %b\n",
            getBootBlockWatchdogResets());
  logNormal(" Bootloader:\n");
  logNormal("  Source id: %t  Program id: %t  Built: %x-%x  Size: %d\n",
            BOOT3_SOURCEID,BOOT3_PROGRAMID,
            SKETCH_BUILD_DATE,SKETCH_BUILD_TIME,
            getBootBlockBootLoaderLength()); 
  SFBChecksum cs;
  getBootBlockBootLoaderChecksum(cs);
  logNormal("  Bootloader burn count: %d   Checksum: %x-%x\n",
            getBootBlockBootLoaderBurnCount(),cs[0],cs[1]);
  reportPowerStatus();
}

void executePendingCommand() {
  switch (commandPending) {
  case 0:
    break;
  case 'o':
    setBootBlockFlags(getBootBlockFlags()|BOOTBLOCK_UNLOCKED);
    setBootBlockOwnerId(commandPendingArgument);
    logNormal("Board owner now '%t'\n",commandPendingArgument);
    break;
  case 'n':
    setBootBlockFlags(getBootBlockFlags()|BOOTBLOCK_UNLOCKED);
    setBootBlockBoardId(commandPendingArgument);
    logNormal("Board name now '%t'\n",commandPendingArgument);
    break;
  case 'k':
    {
      u32 flags = getBootBlockFlags()^BOOTBLOCK_KILL_SKETCH; // Toggle kill flag
      setBootBlockFlags(flags);
      logNormal("Kill sketch flag %s\n",flags&BOOTBLOCK_KILL_SKETCH?"SET":"CLEARED");
    }
    break;
  
  case 'p':
    {
      u32 face, dir, state;
      face = (commandPendingArgument>>16)&0xff;
      dir  = (commandPendingArgument>> 8)&0xff;
      state= (commandPendingArgument>> 0)&0xff;
      if (dir) 
        setBootBlockPowerIn(face,state);
      else
        setBootBlockPowerOut(face,state);
      reportPowerStatus();
    }
    break;

  case 'r': commandPendingArgument = RED_BOOT_MODE; goto setBootMode;
  case 'g': commandPendingArgument = GREEN_BOOT_MODE; goto setBootMode;
  case 'b': commandPendingArgument = BLUE_BOOT_MODE; goto setBootMode;

  setBootMode:
    setBootBlockBootMode(commandPendingArgument);
    ps.bootMode = getBootBlockBootMode();
    displayBootMode(true);
    logNormal("%s boot set\n",bootModeColorName(commandPendingArgument));
    break;

  default:
    logNormal("Command not recognized??\n");
    break;
  }
  commandPending = 0;
  commandPendingArgument = 0;
}

void monitorCommandReceiver(u8 * packet) {
  if (ps.state != PS_MONITOR_COMMAND_WAIT) // Ignore misc packets at wrong time
    return;
  if (Body.source() != ps.monitorFace) // Ignore misc packets not from where we expected
    return;
  
  u8 ch;
  if (packetScanf(packet,"%c",&ch)==0) // Empty packets shouldn't get here..
    return;

  commandCompleted = true;
  u32 arg = 0;
  switch (ch) {
  case 'h': 
    printHelp();
    return; 
  case 'S': facePrintf(ps.face,"S!\nSC\n"); return; // Defeat any inbound BRN attempts

  case 'x': reenterBootloader();//setState(PS_ERROR_RESET,0); return;
  case 'i': printBoardInfo(); return;
  case 'k': goto launchCommand;
  case 'r': goto launchCommand;
  case 'g': goto launchCommand;
  case 'b': goto launchCommand;
  case 'p': 
    {
      u8 face, dir, state;
      if (packetScanf(packet,"%F%c%c\n",&face,&dir,&state)!=4) {
        badCommand(packet);
        return;
      }
      if (dir=='i' || dir=='I') dir = 1;
      else if (dir=='o' || dir=='O') dir = 0;
      else { badCommand(packet); return; }
      
      if (state=='+') state = 1;
      else if (state=='-') state = 0;
      else { badCommand(packet); return; }
      
      arg = (((face<<8)|dir)<<8)|state;
    }    
    goto launchCommand;
  case 'n':
    /* FALL THROUGH */
  case 'o': 
    if (packetScanf(packet,"=%t\n",&arg)!=3) {
      badCommand(packet);
      return;
    }

  launchCommand:
    commandPending = ch;
    commandPendingArgument = arg;
    commandConfirmSteps = 0;
    commandButtonState = 0;
    ps.nctr = 0;
    setState(PS_MONITOR_CONFIRM_WAIT,0);
    break;
  
  default:
    badCommand(packet);
  }
}

bool stepMONITOR_ANNOUNCE(u32, u8 * packet) {
  logNormal("\n\nMONITOR %t:%t/%x-%x: %s BOOT on %t/%t \n",
            BOOT3_SOURCEID,BOOT3_PROGRAMID,
            SKETCH_BUILD_DATE,SKETCH_BUILD_TIME,
            bootModeColorName(ps.bootMode),
            getBootBlockOwnerId(),
            getBootBlockBoardId());
  checkForSketches(false);
  if (ps.sketchHeadersFound > 0) {
    logNormal("  %d sketch%s: %t:%t/%x-%x\n",
              ps.sketchHeadersFound,
              ps.sketchHeadersFound==1?"":"es, newest is",
              ps.sketchHeaders[ps.newestSketchIndex]->creatorId,
              ps.sketchHeaders[ps.newestSketchIndex]->programId,
              ps.sketchHeaders[ps.newestSketchIndex]->buildDate,
              ps.sketchHeaders[ps.newestSketchIndex]->buildTime);
  } else {
    logNormal("  No sketches found\n");
  }
  if (monitorCommandsProcessed == 0)
    printHelp();
  ps.nctr = 0;
  //  ps.nctr += testRemainingMemorySize[0];
  commandCompleted = false;
  setState(PS_MONITOR_COMMAND_WAIT,0);
  return false;
}

bool stepMONITOR_COMMAND_WAIT(u32, u8 * packet) {
  setFaceLights(0);
  if (commandCompleted) {
    ++monitorCommandsProcessed;
    setState(PS_MONITOR_ANNOUNCE,0);
    return true;
  }
  if (!packet) {
    setState(PS_MONITOR_COMMAND_WAIT,1000);
    setFaceLights(ps.nctr&1?1<<ps.monitorFace:0);

    if ((++ps.nctr % 10) == 0) {
      logNormal("Enter command ('h' for help).  Waiting: %d\n",
               (MAX_MONITOR_WAIT_STEPS-ps.nctr)/10);
    }
    if (ps.nctr >= MAX_MONITOR_WAIT_STEPS) {
      logNormal("Autoboot\n");
      setFaceLights(0);
      setState(PS_ERROR_RESET,0); // Give up.
    }
    return false;
  }
  return false;
}

#define COMMAND_CONFIRM_SPIN_STEPS 100000000
bool stepMONITOR_CONFIRM_WAIT(u32, u8 * packet) {
  setFaceLights((commandConfirmSteps>>13)&1?0xf:0x0);

  if (++commandConfirmSteps > COMMAND_CONFIRM_SPIN_STEPS || packet) { // If packet received, cancel confirmation
    logNormal("Canceled\n");
    setState(PS_MONITOR_ANNOUNCE,1000);
    return false;
  }
  switch (commandButtonState) {
  case 0:
    if (buttonDown()) {
      logNormal("Release button\n");
      commandButtonState = 1;
    } else
      commandButtonState = 2;
    break;

  case 1: 
    if (!buttonDown())          // Check for button up
      commandButtonState = 2;
    break;

  case 2:
    if (buttonDown()) 
      commandButtonState = 0;
    else {
      logNormal("Press button to confirm, or hit enter to cancel\n");
      commandButtonState = 3;
    } 
    break;

  case 3:
    if (buttonDown()) {
      logNormal("Confirmed\n");
      executePendingCommand();
      setState(PS_MONITOR_ANNOUNCE,1000);
      return false;
    } 
    if ((commandConfirmSteps&0xfffff)==0) 
      commandButtonState = 2;
    break;
      
  }

  setState(PS_MONITOR_CONFIRM_WAIT,1); // Spin fast here, watching the button
  return false;
}

bool newerSketch(SFBProvenance * p1,SFBProvenance * p2) {
  API_ASSERT(p1&&p2, E_API_NULL_POINTER);

  if (p1->buildDate > p2->buildDate) return true;
  if (p1->buildDate < p2->buildDate) return false;

  if (p1->buildTime > p2->buildTime) return true;
  return false;                 // Older or same
}

static void checkForSketches(bool resetKill) {
  ////////
  /// SEARCH FOR VALID SKETCHES, SELECT NEWEST

  ps.sketchHeadersFound = 0;
  ps.newestSketchIndex = 0;

  if (ps.bootFlags&BOOTBLOCK_KILL_SKETCH) {

    if (resetKill) {
      ps.bootFlags = getBootBlockFlags()&~BOOTBLOCK_KILL_SKETCH; // Reset kill flag
      setBootBlockFlags(ps.bootFlags);                           // Store
      logQuiet("Kill flagged - no sketch search!\n");
    }
  } else {
    u32 curAddr = (u32) &__text_end;  // Start where we end
    u32 endAddr = sectorStart(27+1);
    SFBProvenance * p;

    //    logVerbose("Sketch search %x-%x\n",curAddr,endAddr);

    while ((p = findProvenance(curAddr,endAddr,CHECKSUM_CHECK))) {
      ps.sketchHeaders[ps.sketchHeadersFound] = p;
      if (ps.sketchHeadersFound > 0 && 
          newerSketch(ps.sketchHeaders[ps.sketchHeadersFound],ps.sketchHeaders[ps.newestSketchIndex]))
        ps.newestSketchIndex = ps.sketchHeadersFound;
      if (++ps.sketchHeadersFound >= MAX_SKETCHES)
        break;
      curAddr = (u32) p+sizeof(u32);     // Bump us ahead a word
    }

    logVerbose("Sketches=%d\n",ps.sketchHeadersFound);
  }
}

bool stepBOOT_MODE(u32, u8 * packet) {

  if (packet)                   // Ignore packets now
    return false;

  releaseFaceLights();

  checkForSketches(true);

  if (ps.bootMode == RED_BOOT_MODE && ps.sketchHeadersFound==0) {
    ps.monitorFace = NORTH;
    setLogFace(ps.monitorFace);
    logQuiet("Red boot w/o sketch - entering monitor NORTH\n");
    ps.nctr = 0;
    setState(PS_MONITOR_ANNOUNCE,0);
  } else 
    setState(PS_SELF_CHECK,0);
  return true;
} 

bool stepSELF_CHECK(u32, u8 *) {

  ////////
  /// DECIDE ACTION

  if (buttonDownOnSplash==1 && ps.sketchHeadersFound > 0) {
    logQuiet("Button down - ignoring existing sketch!\n");
    ps.sketchHeadersFound = 0;
    buttonDownOnSplash = 3;
  } 

  if (ps.sketchHeadersFound > 0 && 
      (ps.bootMode==RED_BOOT_MODE || (ps.bootFlags&BOOTBLOCK_NO_UPDATES))) {
    u32 delayMs = 10;
    if (ps.haveBest) {          // If we accepted code
      facePrintf(ps.face,"Pg%h\n",ps.snonce); // Acknowledge it all worked
      delayMs = 200;            // and wait for final output to flush
    }
    setState(PS_ENTER_SELECTED_SKETCH,delayMs);
    return false;
  }

  for (u32 i = NORTH; i <= WEST; ++i) {
    ps.faceState[i] = NO_ANSWER;
    ps.rnonce[i] = (u16) randomBits(16);
  }
  ps.nctr = 0;

  setState(PS_NEIGHBORS,200);

  return false;
}

#define rejectSource(packet) (rejectSourceLine(__LINE__,packet))
static bool rejectSourceLine(int line,u8 * packet) {
  logVerbose("%s parse (%d)len=%d[%#p]\n",
             FACE_NAME(Body.source()),line,packetLength(packet),packet);
  ps.faceState[Body.source()] = REJECTED;    // Foo on them.
  return false;
}

bool stepNEIGHBORS(u32, u8 * packet) {
  const int WAIT_STEPS = 28;    // How long to look for any desirable code
  const int EAGER_WAIT_STEPS = 5; // How long to wait after we've seen something we want

  if (!packet) {                // Then here due to timeout
    if (++ps.nctr < WAIT_STEPS) {    // If it's just an interim timeout..
      displayBootMode(ps.nctr&1);

      for (u32 i = NORTH; i <= WEST; ++i) {
        if (ps.faceState[i]==NO_ANSWER) 
          facePrintf(i, "Pa%h%c\n",ps.rnonce[i],SKETCH_ARCH_CODE);
      }

      for (u32 i = NORTH; i <= WEST; ++i) {
        u32 state = 0;
        switch (ps.faceState[i]) {
        case NO_ANSWER: break;  
        case REJECTED:  state = !(ps.nctr&1); break;  
        case REPLIED:   state = 1; break;
        case SELECTED:  state = ps.nctr&1; break; 
        }
        ledSet(pinInFace(FACE_LED_PIN,i), state);
      }
      setState(PS_NEIGHBORS,300,false); // Back to self, with blink-time delay
      return false;
    }

    if (!ps.haveBest) {       // If no improvement was found, then..
      if (ps.sketchHeadersFound>0)  // if we already have something..
        setState(PS_ENTER_SELECTED_SKETCH,0);  // use it.. 
      else                    // But if we still don't have a sketch..
        setState(PS_ERROR_RESET,200); // Pick ourselves up, dust ourselves off..
      return true;
    }

    // Here is timeout + haveBest: Time to get some new code

    // Update the bootblock, announce
    //dumpBootBlocks(NORTH);
    //delay(100);
    incrementBootBlockBurnCount();
    //dumpBootBlocks(NORTH);
    //delay(100);
    logNormal("About to burn: %t/%t bootCount=%d; burnCount=%d (+%d); powerOnHours=%d; flags=%x\n",
              getBootBlockOwnerId(),
              getBootBlockBoardId(),
              getBootBlockBootCount(),
              getBootBlockBurnCount(),
              getBootBlockBootLoaderBurnCount(),
              getBootBlockPowerOnHours(),
              getBootBlockFlags());
    delay(100);

    for (u32 i = NORTH; i <= WEST; ++i) {
      if (i != ps.face) {
        Faces[i].reserve();      // Shut down non-selected faces
        ledOff(pinInFace(FACE_LED_PIN,i));
      } else {
        pinMode(pinInFace(FACE_LED_PIN,i),REFLEX_MODE);  
      }
    }

    // Figure out what sectors we'll need to check
    ps.currentSector = sectorOfAddress(ps.bestStartAddr);
    ps.lastAddress = ps.bestStartAddr+ps.bestProgram.programLength;
    ps.lastSector = sectorOfAddress(ps.lastAddress);
    ps.nctr = 0;
    setState(PS_PICK_SECTOR,0);
    return true;
  } // End of timeout code..

  // Here we have a packet.  Perform basic checks
  u16 rnonce;

  u32 matches = packetScanf(packet,"PA%h",&rnonce);

  if (matches == 1)             // Ignore out-of-sequence P packets 
    return false;

  if (matches != 3 || rnonce != ps.rnonce[Body.source()])
    return rejectSource(packet);  // Blown.

  if (packetLength(packet)==4)  // They're done sending.  But I don't think it matters; we just time out anyway
    return false;

  // Now, parse the packet
  SFBProvenance cs;
  u8 pflags;
  u8 archc;
  u16 snonce;
  u32 startAddr;

  if (packetScanf(packet,"%h%c%c",&snonce,&archc,&pflags) != 3)
    return rejectSource(packet);

  if (archc != SKETCH_ARCH_CODE)
    return rejectSource(packet);

  ps.programmingFlags |= pflags;   // NOTE WE ARE ONLY ADDING FLAGS!
  
  logVerbose("BootConf: pflags=%x; mode=%s; btn=%d\n",
             ps.programmingFlags,bootModeColorName(ps.bootMode),buttonDownOnSplash);

  if (ps.programmingFlags & PROGRAMMING_FLAG_VERBOSE)
    setLogLevel(LOGLEVEL_VERBOSE);
  else if (ps.programmingFlags & PROGRAMMING_FLAG_QUIET)
    setLogLevel(LOGLEVEL_QUIET);
  else 
    setLogLevel(LOGLEVEL_NORMAL);

  // Read rest of PA packet, through the checkbyte
  if (packetScanf(packet,"%l%l%l%l%l%l%l%l%l%l%\n",
                  &cs.sourceOwnerId, &cs.sourceBoardId,
                  &cs.creatorId, &cs.programId, &cs.buildDate, &cs.buildTime,
                  &startAddr, &cs.programLength,
                  &cs.checksum0, &cs.checksum1) != 11)
    return rejectSource(packet);

  if (startAddr < sectorStart(9) || ((startAddr&3) != 0)) return rejectSource(packet);

  cs.magic = SFB_PROVENANCE_MAGIC;
  cs.cigam = SFB_PROVENANCE_CIGAM;

  logVerbose("%s %t/%t offers: %t:%t/%x-%x sa %x pl %d ea %x\n",
             FACE_NAME(Body.source()),cs.sourceOwnerId,cs.sourceBoardId,
             cs.creatorId, cs.programId, cs.buildDate, cs.buildTime,
             startAddr, cs.programLength,sectorStart(27+1));

  if (!plausibleProvenance(&cs,startAddr,sectorStart(27+1))) return rejectSource(packet);

  // Well, okay, call the packet plausible.  Does it beat what we're already sitting on?

  ps.faceState[Body.source()] = REPLIED;

  bool wantIt = true;
  const char * reason = "newer or no prior sketch";
  const char * color = "red";
  if (buttonDownOnSplash != 0)
    reason = "button down or no sketch";

  if (ps.bootMode == BLUE_BOOT_MODE) {
    color = "blue";

    // Only want it if it's an update on what we have
    if (ps.sketchHeadersFound > 0) {
      // We do have something.  
      SFBProvenance * cur = ps.sketchHeaders[ps.newestSketchIndex];
      if (cur->creatorId==cs.creatorId && cur->programId==cs.programId) {
        // Wow, someone else has what we have!  How nice.  Is it newer?
        reason = "update of our sketch";
        if (!newerSketch(&cs,cur)) {
          wantIt = false;       // Nope..
          reason = "our sketch, not newer";
        }
      } else {
        wantIt = false;
        reason = "not our sketch";
      }
    } else {
      if (ps.haveBest) {        // Does this beat previous offers?
        if (!newerSketch(&cs,&ps.bestProgram)) {
          wantIt = false;       // Nope..
          reason = "have better offer";
        } else
          reason = "update of out sketch";
      } /* else default: wantit since no existing sketch */
    }
  } else if (ps.bootMode == GREEN_BOOT_MODE) {
    color = "green";
    if (ps.haveBest) {             // If we already have a candidate
      if (!newerSketch(&cs,&ps.bestProgram)) {
        wantIt = false;         // What we already have is newer
        reason = "already have newer";
      }
    } else if (ps.sketchHeadersFound > 0) {

      // We already have a sketch - offer better?
      SFBProvenance * cur = ps.sketchHeaders[ps.newestSketchIndex];

      if (!newerSketch(&cs,cur)) {
        wantIt = false;       // Nope..
        reason = "not newer than our sketch";
      }
    } else {
      reason = "no current";
    }
  }

  if (wantIt) {
    ps.haveBest = 1;
    ps.bestProgram = cs;
    ps.face = Body.source();
    setLogFace(ps.face);

    ps.faceState[ps.face] = SELECTED;
    ps.snonce = snonce;
    ps.bestStartAddr = startAddr;
    ps.nctr = WAIT_STEPS-EAGER_WAIT_STEPS; // Adjust counter based on our new expectations

    logNormal("Best offer so far: %t:%t/%x-%x (size %d) from %s %t/%t\n",
              cs.creatorId, cs.programId, cs.buildDate, cs.buildTime, cs.programLength,
              FACE_NAME(Body.source()), cs.sourceOwnerId, cs.sourceBoardId);
  }
  logVerbose("I %swant it (%s - %s)\n",wantIt?"":"don't ",reason, color);

  return false;
}

bool stepPICK_SECTOR(u32, u8 *) {

  logVerbose("PKSct%d\n",ps.currentSector);

  if (ps.currentSector > ps.lastSector) {

    char * begin = (char *) &(((SFBProvenance*)ps.bestStartAddr)->magic);
    SFBChecksum cs;
    SFBChecksumInit(cs);
    SFBChecksumAddBytes(cs, begin, (ps.lastAddress-(u32) begin));

    if (cs[0]==ps.bestProgram.checksum0 && cs[1]==ps.bestProgram.checksum1) {

      logVerbose("OKCS@%x\n",ps.bestStartAddr);

      ps.bootMode = RED_BOOT_MODE;     // We should have a valid sketch now.  Confirm that and go run it
      setState(PS_BOOT_MODE,0);
      return true;
    }

    ps.programmingFlags |= PROGRAMMING_FLAG_CHECKOFF; // We had a whole-program mismatch -- better be paranoid

    logNormal("Program checksum failure! Retrying with full download!\n");    
    logVerbose("Bad chksm: s=%x e=%x cs=%x-%x not %x-%x\n",
               ps.bestStartAddr,ps.lastAddress,cs[0],cs[1],ps.bestProgram.checksum0,ps.bestProgram.checksum1);

    setState(PS_ERROR_RESET,0);
    return true;
  }

  u32 startAddr = adjustedSectorStart(ps.currentSector);
  u32 length = adjustedSectorSize(ps.currentSector, startAddr);

  facePrintf(ps.face,"Pc%h%l%l\n",ps.snonce,startAddr,length); // Request the range we need checksummed

  setState(PS_CHECK_SECTOR,1000);
  return false;
}

#define sectorPacketBlown() sectorPacketBlownLine(__LINE__,packet)

static bool sectorPacketBlownLine(int line, u8 *packet) { // Just return false, for now

  logVerbose("%s bad parse (%d)l:%d[%#p]\n",
             FACE_NAME(Body.source()),line,packetLength(packet),packet);

  return false;                 // We'll count on the CHECK_SECTOR timeout/count to get us out 
}

bool stepCHECK_SECTOR(u32, u8 * packet) {

  if (!packet) {                // Foo!  Timeout :(
    logVerbose("T/O Sct%d\n",ps.currentSector);

    if (++ps.nctr > 3) {
      setState(PS_ERROR_RESET,0);     // Now we're in trouble.
      return true;
    }
    logVerbose("RETRY%d\n",ps.currentSector);
    setState(PS_PICK_SECTOR,0); // Well, probably dead here, but try again hoping it was a transient error
    return true;
  }

  //logVerbose("CHKSCTR%d\n",ps.currentSector);

  // Sanity check the packet we got.

  if (packetScanf(packet,"P") != 1)
    return false; // Some other kind of packet leaking through.  Ignore it

  // Parse rest of PC packet
  u16 rnonce;
  u32 startAddr, length;
  SFBChecksum cs;

  if (packetScanf(packet,"C%h%l%l%l%l%\n",
                  &rnonce,
                  &startAddr,&length,&cs[0],&cs[1]) != 7)
    return sectorPacketBlown();

  if (rnonce != ps.rnonce[Body.source()]) return sectorPacketBlown(); // You must know our rnonce

  u32 startAddrNeeded = adjustedSectorStart(ps.currentSector);
  u32 lengthNeeded = adjustedSectorSize(ps.currentSector,startAddrNeeded);

  // OK, it parsed.  Is it the info we wanted?
  if (startAddr != startAddrNeeded || length != lengthNeeded) {
    return sectorPacketBlown(); // WTH??
  }

  // Yes, it is.  We will trust it. 
  ps.nctr = 0;

  // Are we willing to trust a checksum match?
  if ((ps.programmingFlags & PROGRAMMING_FLAG_CHECKOFF)==0) {
    SFBChecksum cs2;
    SFBChecksumInit(cs2);
    SFBChecksumAddBytes(cs2,(const char *) startAddr, (int) length);

    if (SFBChecksumEqual(cs,cs2)) {
      // Holy moly they match!  We just saved a bunch of work!

      logVerbose("Sct%d OKcs! Gogo!\n",ps.currentSector);

      ++ps.currentSector;
      setState(PS_PICK_SECTOR,0);
      return true;
    }
  }

  // OK, sigh, we have to erase this whole sector and rewrite it
  ps.nextAddress = sectorStart(ps.currentSector); // So go for it all, from true (non-adjusted) sector start
 
  ps.sectorChecksum[0] = cs[0];       // Stash the cs (of the adjusted range) for checking later
  ps.sectorChecksum[1] = cs[1];       //  "
  setState(PS_PICK_BLOCK,0);
  return true;
}

bool stepPICK_BLOCK(u32, u8 *) {

  u32 endAddr = sectorEnd(ps.currentSector);
  if (ps.lastAddress < endAddr)
    endAddr = ps.lastAddress;

  if ((ps.nextAddress&255)==0 && ps.nextAddress >= endAddr) { // If at round 256 and past end..

    // Now the adjusted sector checksum better match..

    u32 adjustedStartAddr = adjustedSectorStart(ps.currentSector);
    u32 adjustedSectorLength = adjustedSectorSize(ps.currentSector,adjustedStartAddr);
    SFBChecksum scs;
    SFBChecksumInit(scs);
    SFBChecksumAddBytes(scs,(const char *) adjustedStartAddr, adjustedSectorLength);

    if (SFBChecksumEqual(ps.sectorChecksum,scs)) {
      ++ps.currentSector;       // Moving on!
    }

    setState(PS_PICK_SECTOR,0); // Either next sector or, :(, redoing this one..
    return true;
  }
  
  logVerbose("PKBlk0x%x\n",ps.nextAddress);

  facePrintf(ps.face,"Pb%h%l%c\n",ps.snonce,ps.nextAddress,127);
  setState(PS_CHECK_BLOCK,1000);
  return false;
}

#define blockPacketBlown() blockPacketBlownImpl(__LINE__)
static bool blockPacketBlownImpl(int lineno) { // Just return false, for now
  logVerbose(__FILE__ ":%d\n",lineno);
  return false;                 // We'll count on the CHECK_BLOCK timeout/count to get us out 
}

// Gah!  Puking IAP doesn't accept src data from USB/ENET RAM!  We HAVE to copy! :(
u8 pukingBuf[256] IN_FASTRAM_BSS __attribute__ ((aligned (4))); // So get a word-aligned place in fastram to burn from..

bool stepCHECK_BLOCK(u32, u8 * packet) {
  if (!packet) {                // Foo!  Timeout :(
    if (++ps.nctr > 3) {
      setState(PS_ERROR_RESET,0);     // Now we're in trouble.
      return true;
    }
    setState(PS_PICK_BLOCK,0); // Well, probably dead here, but try again hoping it was a transient error
    return true;
  }

  // Sanity check the packet we got.

  u16 rnonce;
  u32 startAddr;

  u32 matches = packetScanf(packet,"PB%h%l",&rnonce, &startAddr);

  if (matches==0)
    return false; // Some other kind of packet leaking through.  Ignore it

  if (matches==1) {
    logVerbose("Ugh:%#p\n",packet);  // Only PB valid here
    return false;
  }

  if (matches != 4)
    return blockPacketBlown(); 

  if (rnonce != ps.rnonce[Body.source()]) return blockPacketBlown(); // You must know our rnonce

  if (startAddr != ps.nextAddress) return blockPacketBlown(); // We must be expecting what you're offering

  if (packetReadLength(packet) != 128+1) return blockPacketBlown(); // Should be just the data and check byte left
  if (!packetCheckByteValid(packet)) return blockPacketBlown();

  u32 pos = packetCursor(packet);

  logVerbose("BlkOK %x\n",startAddr);
  ps.nctr = 0;

  if ((startAddr & 255)==0) {   // Then we're buffering a first half of a 256
    for (int i = 0; i < 128; ++i) // So just save it away
      pukingBuf[i] = packet[pos+i];
  } else {
    startAddr &= ~255;          // Else round back down to the start of the 256..

    for (int i = 0; i < 128; ++i) 
      pukingBuf[i+128] = packet[pos+i]; // ..buffer the second half of it..

    ///
    // Now we have one more disgusting hack to do: Check if we're
    // about to burn the provenance block, and update its source
    // provenance information, if so.  (You'd think this would destroy
    // the sector checksum we got, except that we carefully excluded
    // the premagic section of the provenance block from the sector
    // checksum range we requested in the "Pc" packet we sent in
    // PICK_SECTOR..)

    bool hackcs = false;

    if (startAddr <=  ps.bestStartAddr && startAddr+256 >= ps.bestStartAddr+sizeof(SFBProvenance)) {
      SFBProvenance * bp = (SFBProvenance*) &pukingBuf[ps.bestStartAddr-startAddr];

      // Let's check that we're looking at what we think we are..
      if (!plausibleProvenance(bp,ps.bestStartAddr,ps.lastAddress)) // WTH???  Who's nuts, us or them?  Blame them..
        return blockPacketBlown();

      // OK.  Customize our (non-checksummed) info about where this sucker came from.
      bp->sourceOwnerId = ps.bestProgram.sourceOwnerId;
      bp->sourceBoardId = ps.bestProgram.sourceBoardId;

      bp->checksum0 = ps.bestProgram.checksum0;
      bp->checksum1 = ps.bestProgram.checksum1;

      hackcs = true;
    }

    setWhite(true);
    writeCode256(startAddr,pukingBuf); // ..burn the whole schmear..
    setWhite(false);

    // ..then immediately check what we just wrote
    for (int i = 0; i < 256; ++i) {
      if (((u8*)startAddr)[i] != pukingBuf[i])
        API_BUG(E_B3_VERIFY_FAILURE);           // Oh, crap.
    }

    if (hackcs) {
      SFBProvenance * p = (SFBProvenance*) ps.bestStartAddr;
      logVerbose("Prv0x%x: %t/%t %x-%x\n", 
                 p,
                 p->sourceOwnerId, p->sourceBoardId,
                 p->checksum0, p->checksum1);
    }
  }

  ps.nextAddress += 128;
  setState(PS_PICK_BLOCK,0);
  return true;
}

typedef void (*voidFunc)(void);

bool stepENTER_SELECTED_SKETCH(u32, u8 *) {
  API_ASSERT(ps.sketchHeadersFound!=0,E_BUG_INCONSISTENT_STATE);
  u32 startAddr = (u32) &ps.sketchHeaders[ps.newestSketchIndex][1];
  voidFunc func = (voidFunc) startAddr;
  setWhite(false);              /* Clear the RGB's */ 
  splashScreen(false);          /* One flash to terminate */ 
  (*func)();                    /* Then go for it */
  return false; /* Actually: NOT REACHED */
}

static const u8 bootModePins[MAX_BOOT_MODES] = { 0, BODY_RGB_RED_PIN, BODY_RGB_GREEN_PIN, BODY_RGB_BLUE_PIN };
static void displayBootMode(bool on) {
  setWhite(false);
  if (on && ps.bootMode)
    ledOn(bootModePins[ps.bootMode]);
}

void loop()
{
  delay(100);                   // All the action is in the background!
}
