]> jspc29.x-matter.uni-frankfurt.de Git - trbnettools.git/commitdiff
moved from trbrich to here
authorhadaq <hadaq>
Fri, 23 Jul 2010 21:56:19 +0000 (21:56 +0000)
committerhadaq <hadaq>
Fri, 23 Jul 2010 21:56:19 +0000 (21:56 +0000)
libtrbnet/trbflash.c [new file with mode: 0644]

diff --git a/libtrbnet/trbflash.c b/libtrbnet/trbflash.c
new file mode 100644 (file)
index 0000000..e54bb60
--- /dev/null
@@ -0,0 +1,1568 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <signal.h>
+#include <limits.h>
+#include <libgen.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <errno.h>
+#include <time.h>
+#include <ctype.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#include <trbnet.h>
+
+#include <trberror.h>
+
+static const uint16_t HardwareId = 0x0042;
+static const uint16_t CtrlReg = 0xd000; 
+static const uint16_t SetupReg = 0xd001;
+static const uint16_t BlockRam = 0xd100;
+static const uint16_t MDCFlashRomSelect = 0xd200;
+
+static uint32_t* trbBuffer = NULL;
+static uint8_t** pageBuffer = NULL;
+static uint16_t* pageBufferAddress = NULL;
+
+static uint8_t* imageBuffer = NULL;
+
+/* Maximum number of endpoints */
+#define NUM_ENDPOINTS 1024
+
+/* 32-Bit Words: */
+#define BLOCKRAM_SIZE 64
+#define TRB_BUFFER_SIZE (BLOCKRAM_SIZE * NUM_ENDPOINTS + NUM_ENDPOINTS) 
+
+/* Bytes: */
+#define PAGE_SIZE 256
+static unsigned int NUM_PAGES = 0;
+
+#define BLOCK_SIZE (64 * 1024)
+static unsigned int NUM_BLOCKS = 0;
+
+#define PAGE_BUFFER_SIZE (PAGE_SIZE * NUM_ENDPOINTS) 
+
+static FILE *logFile = NULL;
+static const char logFileName[256] = "trbflash.log";
+
+static const char busy[4][5] = {
+  "|\b",
+  "/\b",
+  "-\b",
+  "\\\b"
+};
+
+static const unsigned int timeout = 10000;
+
+typedef enum {
+  FLASH_INVALID = 0,
+  
+  FLASH_RICH_ADCM_V1 = 1,
+  FLASH_RICH_ADCM_V2 = 2,
+  FLASH_RICH_ADCM_V3 = 3,
+  
+  FLASH_MDC_HUB_V2_FPGA1234 = 4,
+  FLASH_MDC_HUB_V2_FPGA5 = 5,
+  
+  FLASH_MDC_OEP_V2 = 6,
+  FLASH_MDC_OEP_V3 = 7,
+  
+  FLASH_SHOWER_ADDON_V2_FPGA1 = 8,
+  FLASH_SHOWER_ADDON_V2_FPGA2 = 9,
+  FLASH_SHOWER_ADDON_V2_FPGA3 = 10,
+  
+  FLASH_CTS_FPGA1 = 11,
+  FLASH_CTS_FPGA2 = 12
+} FlashType;
+
+static const char FlashTypeStr[16][32] =
+  {
+    "INVALID",
+    "RICH_ADCM_V1",
+    "RICH_ADCM_V2",
+    "RICH_ADCM_V3", 
+    "MDC_HUB_V2_FPGA1234",
+    "MDC_HUB_V2_FPGA5",
+    "MDC_OEP_V2",
+    "MDC_OEP_V3",
+    "SHOWER_ADDON_V2_FPGA1",
+    "SHOWER_ADDON_V2_FPGA2",
+    "SHOWER_ADDON_V2_FPGA3",
+    "CTS_FPGA1",
+    "CTS_FPGA2"
+  };
+
+static FlashType flashType = FLASH_INVALID; 
+static uint32_t manId = 0;
+static const char trbflash_version[] = "$Revision: 2.21 $"; 
+
+static uint32_t mdcFlashSelect = 1;
+
+static int yesToAll = 0;
+
+static int skipFirmwareIdCheck = 0;
+
+static int skipHwIdCheck = 0;
+
+/* ------ Local Functions ----------------------------------------------- */
+
+static void atexit0()
+{
+  unsigned int i;
+  
+  /* Close files */
+  if ((logFile != NULL) && (logFile != stderr)) {
+    fclose(logFile);
+  }
+
+  /* Free memory */
+  free(pageBufferAddress);  
+  for (i = 0; i < NUM_ENDPOINTS; i++) {
+    free(pageBuffer[i]);
+  }
+  free(pageBuffer);
+  free(trbBuffer);
+  free(imageBuffer);
+}
+
+static int writeSetupRegister(uint16_t trb_address, uint8_t value)
+{
+  unsigned int ctr = 0;
+  int status = -1;
+  
+  /* Wait until NoMoreData is withdrawn */
+  do {
+    if ((status = trb_register_write(trb_address, SetupReg,
+                                     (uint32_t)(value << 24))) == -1) {
+      fprintf(logFile, "Error > writeSetupRegister: TRBNet %s\n",
+              trb_strerrorf());
+      return -1;
+    }
+    
+    /* Check timeout */
+    if (ctr >=  timeout) {
+      fprintf(logFile, "Error > writeSetupRegister: timeout\n");
+      return -1;
+    }
+    ctr++;
+  } while (trb_term.status_channel != 0);
+  
+  if (trb_term.status_channel != 0) {
+    fprintf(logFile,
+            "Error > writeSetupRegister: invalid Status returned %s\n",
+            trb_strerrorf());
+    return -1;
+  }
+
+  return 0; 
+}
+
+static int readCtrlRegister(uint16_t trb_address,
+                            uint32_t value[NUM_ENDPOINTS])
+{
+  unsigned int ctr = 0;
+  int status = -1;
+  int c;
+  int i;
+  
+  /* Wait until NoMoreData is withdrawn */
+  do {
+    if ((status = trb_register_read(trb_address, CtrlReg,
+                                    trbBuffer, TRB_BUFFER_SIZE)) == -1) {
+      fprintf(logFile, "Error > readCtrlRegister: TRBNet %s\n",
+              trb_strerrorf());
+      return -1;
+    }
+    
+    /* Check timeout */
+    if (ctr >=  timeout) {
+      fprintf(logFile, "Error > readCtrlRegister: timeout\n");
+      return -1;
+    }
+    ctr++;
+  } while (trb_term.status_channel != 0);
+  
+  if (status <= 0) {
+    fprintf(logFile, "Error > readCtrlRegister: invalid length returned\n");
+    return -1;
+  }
+  
+  c = 0;
+  for (i = 1; i < status; i += 2) {
+    if (value != NULL) value[c] = trbBuffer[i];
+    c++;
+  }
+  
+  return c;
+}
+
+static int writeCtrlRegister(uint16_t trb_address, uint32_t value)
+{
+  unsigned int ctr = 0;
+  int status = -1;
+  
+  /* Wait until NoMoreData is withdrawn */
+  do {
+    if ((status = trb_register_write(trb_address, CtrlReg,
+                                     value)) == -1) {
+      fprintf(logFile, "Error > writeCtrlRegister: TRBNet %s\n",
+              trb_strerrorf());
+      return -1;
+    }
+    
+    /* Check timeout */
+    if (ctr >=  timeout) {
+      fprintf(logFile, "Error writeCtrlRegister, timeout\n");
+      return -1;
+    }
+    ctr++;
+  } while (trb_term.status_channel != 0);
+    
+  return 0; 
+}
+
+static int sendCommand(uint16_t trb_address, uint32_t cmd, uint8_t max)
+{
+  if (writeSetupRegister(trb_address, max) == -1) {
+    return -1;
+  }
+  if (writeCtrlRegister(trb_address, cmd) == -1) {
+    return -1;
+  }
+  if (readCtrlRegister(trb_address, NULL) == -1) {
+    return -1;
+  }
+  
+  return 0;
+}
+
+static int checkStatus(uint16_t trb_address)
+{
+  uint32_t trbcmd;
+  int status;
+  int ret;
+  int i;
+  
+  /* Read Status Register */
+  trbcmd = 0x05 << 24;
+  if (sendCommand(trb_address, trbcmd, 0) == -1) {
+    return -1;
+  }
+  
+  if ((status = 
+       trb_register_read(trb_address, BlockRam, trbBuffer, 
+                         TRB_BUFFER_SIZE)) == -1) {
+    fprintf(logFile, "Error > checkStatus: TRBNet %s\n",
+            trb_strerrorf());
+    return -1;
+  }
+  
+  if (status <= 0) {
+    return -1;
+  }
+
+  ret = 0;
+  for (i = 0; i < status; i += 2) {
+    if ((manId == 0x01461f) || 
+        (manId == 0x00471f)) {
+      /* Check EPE Bit (ADCM and SHOWER) */
+      if (((trbBuffer[i + 1] >> 5) & 0x01) == 1) {
+        fprintf(logFile, "Error > checkStatus: Erase or program error on "
+                "EndPoint 0x%04x\n", trbBuffer[i] & 0xffff);
+        return -1; /* Fatal Error */
+      }
+    }
+    if ((trbBuffer[i + 1] & 0x01) == 1) {
+      ret = -2;  /* One is busy */
+    }
+  }
+  
+  return ret;
+}
+
+static int writeStatusRegister(uint16_t trb_address, uint8_t value)
+{
+  int status;
+  
+  /* Enable writing */
+  if (sendCommand(trb_address, 0x06 << 24, 0) == -1) {
+    fprintf(logFile, "Error > writeStatusRegister: write enable\n");
+    return -1;
+  }
+    
+  if (trb_register_write(trb_address, BlockRam, (uint32_t)value) == -1) {
+    fprintf(logFile, "Error > writeStatusRegister: TRBNet %s\n",
+            trb_strerrorf());
+    return -1;
+  }
+  if (sendCommand(trb_address, 0x01 << 24, 0) == -1) {
+    fprintf(logFile, "Error > writeStatusRegister: sendCommand\n");
+    return -1;
+  } 
+  
+  /* Wait for not busy and check status */
+  while ((status = checkStatus(trb_address) == -2)) {}
+  
+  if (status != 0) {
+    fprintf(logFile, "Error > writeStatusRegister: invalid status\n");
+    return -1;         
+  }
+  
+  return 0;
+}
+
+static int initTransfer(uint16_t trb_address)
+{
+  /* Find Endpoint(s) ManId and allocate needed memory */
+  uint32_t trbcmd;
+  int status;
+  unsigned int counter = 0;
+  int i;
+
+  /* Read HardwareIds from all Boards and validate (all must be the same) */
+  if ((status = 
+       trb_register_read(trb_address, HardwareId, trbBuffer, 
+                         TRB_BUFFER_SIZE)) == -1) {
+    fprintf(logFile, "Error > initTransfer, read HardwareIds: TRBNet %s\n",
+            trb_strerrorf());
+    return -1;
+  }
+  
+  flashType = FLASH_INVALID;
+  for (i = 0; i < status; i += 2) {
+    FlashType fType = FLASH_INVALID;
+
+    uint16_t hardwareId = (uint16_t)((trbBuffer[i + 1] >> 16) & 0xffff);
+    switch (hardwareId) {
+      
+      /* Rich */
+    case 0x3100:
+      fType = FLASH_RICH_ADCM_V1;
+      break;
+    case 0x3200:
+      fType = FLASH_RICH_ADCM_V2;
+      break;
+    case 0x3300:
+    case 0x0002:
+      fType = FLASH_RICH_ADCM_V3;
+      break;
+      
+      /* MDC HUB */
+    case 0x1210:
+      fType = FLASH_MDC_HUB_V2_FPGA1234;
+      break;
+    case 0x1250:
+      fType = FLASH_MDC_HUB_V2_FPGA5;
+      break;
+      
+      /* MDC OEP */
+    case 0x2200:
+      fType = FLASH_MDC_OEP_V2;
+      break;
+    case 0x2300:
+    case 0x1234:
+      fType = FLASH_MDC_OEP_V3;
+      break;
+      
+      /* SHOWER */
+    case 0x4210:
+      fType = FLASH_SHOWER_ADDON_V2_FPGA1;
+      break;
+    case 0x4220:
+      fType = FLASH_SHOWER_ADDON_V2_FPGA2;
+      break;
+    case 0x4230:
+      fType = FLASH_SHOWER_ADDON_V2_FPGA3;
+      break;
+      
+      /* CTS */
+    case 5100:
+      fType = FLASH_CTS_FPGA1;
+      break;
+    case 5200:
+      fType = FLASH_CTS_FPGA1;
+      break;
+
+    default:
+      /* Error: Unknown FlashType */
+      fprintf(logFile, "Error > initTransfer: "
+              "Unsupported HardwareId 0x%04x on EndPoint 0x%04x\n",
+              hardwareId, trbBuffer[i] & 0xffff);
+      return -1;
+    }
+        
+    if (flashType == FLASH_INVALID) {
+      flashType = fType;
+      continue;
+    }
+    if (fType != flashType) {
+      fprintf(logFile, "Error > initTransfer: "
+              "HardwareId 0x%04x differs on EndPoint 0x%04x\n",
+              hardwareId, trbBuffer[i] & 0xffff);
+      return -1;
+    }
+  }
+    
+  /* Read ManIds from all Boards and validate (all must be the same as well) */
+  trbcmd = 0x9f << 24;
+  if (sendCommand(trb_address, trbcmd, 2) == -1) {
+    return -1;
+  }
+  
+  if ((status = 
+       trb_register_read(trb_address, BlockRam, trbBuffer, 
+                         TRB_BUFFER_SIZE)) == -1) {
+    fprintf(logFile, "Error > initTransfer, read ManIds: TRBNet %s\n",
+            trb_strerrorf());
+    return -1;
+  }
+  
+  if (status <= 0) {
+    return -1;
+  }
+  
+  manId = 0;
+  counter = 0;
+  for (i = 0; i < status; i += 2,counter++) {
+    uint32_t tmpManId = (trbBuffer[i + 1] & 0x00ffffff);
+    
+    if (!((tmpManId == 0x01461f) || 
+          (tmpManId == 0x00471f) ||
+          (tmpManId == 0x1520c2))) {
+      fprintf(logFile, "Error > initTransfer: "
+              "Unsupported ManId(s) 0x%04x on EndPoint 0x%04x\n",
+              tmpManId, trbBuffer[i] & 0xffff);
+      return -1;
+    }
+    if (manId == 0) {
+      manId = tmpManId;
+      continue;
+    }
+    if (tmpManId != manId) {
+      fprintf(logFile, "Error > initTransfer: ManId 0x%04x differs"
+              "on EndPoint 0x%04x\n",
+              manId, trbBuffer[i] & 0xffff);
+      return -1;
+    }
+  }
+  
+  /* Set NUM_PAGES */
+  switch (manId) {
+  
+  case 0x01461f:
+    NUM_PAGES = 8192;
+    break;
+  
+  case 0x00471f:
+    NUM_PAGES = 16384;
+    break;
+    
+  case 0x1520c2:
+    NUM_PAGES = 8192;
+    break;
+  
+  default:
+    abort();
+  }
+      
+  NUM_BLOCKS = (NUM_PAGES * PAGE_SIZE) / BLOCK_SIZE;
+  
+  /* Buffer holding the entire rom-image */
+  imageBuffer =
+    (uint8_t*)malloc(sizeof(uint8_t) * (PAGE_SIZE * (NUM_PAGES + 2)));
+  if (imageBuffer == NULL) {
+    abort();
+  }
+
+  fprintf(stderr, "Found %d Endpoint(s) of type %s\n",
+          counter, FlashTypeStr[flashType]);
+
+  return 0;
+}
+
+static int readPage(uint16_t trb_address, uint32_t pageNumber, 
+                    uint32_t numBytes)
+{
+  uint32_t* temp = NULL;
+  uint32_t* end = NULL;
+  unsigned int endPoint;
+  uint16_t size = 0;
+  uint32_t trbcmd;
+  int status = -1;
+    
+  if (pageNumber >= NUM_PAGES) return -1;
+  if ((numBytes > PAGE_SIZE) || (numBytes == 0)) return -1;
+  
+  trbcmd = (0x03 << 24) | ((pageNumber * PAGE_SIZE) & 0xffffff);
+  if (sendCommand(trb_address, trbcmd, numBytes - 1) == -1) {
+    return -1;
+  }
+
+  size = (numBytes / 4) + (numBytes % 4 != 0 ? 1 : 0);
+  if ((status = trb_register_read_mem(trb_address, BlockRam, 0, size,
+                                      trbBuffer, TRB_BUFFER_SIZE)) 
+      == -1) {
+    fprintf(logFile, "Error > readPage: TRBNet %s\n",
+            trb_strerrorf());
+    return -1;
+  }
+  
+  if (status <= 0) {
+    return -1;
+  }
+  
+  /* Copy trbBuffer to pageBuffer and check content */
+  endPoint = 0;
+  temp = trbBuffer;
+  end = trbBuffer + status;
+  while (temp < trbBuffer + status) {
+    unsigned int len = (*temp >> 16) & 0xffff;
+    unsigned int address = *temp & 0xffff;
+    unsigned int c;
+    
+    if (len != size) {
+      fprintf(logFile,
+              "Error > readPage: Invalid len %d returned by endpoint 0x%04x\n",
+              len, address);
+    } 
+    
+    /* read one page */
+    pageBufferAddress[endPoint] = address;
+    for (c = 0; c < (len * 4); c++) {
+      if ((c % 4) == 0) {
+        temp++;
+      }
+      pageBuffer[endPoint][c] = (*temp >> ((c % 4) * 8)) & 0xff;
+    }
+    temp++;
+    endPoint++;
+  }
+    
+  return endPoint;
+}
+
+
+static int writePage(uint16_t trb_address, uint32_t pageNumber, 
+                     const uint8_t* pageBuffer, uint32_t numBytes)
+{
+  uint32_t* temp = NULL;
+  uint16_t size = 0;
+  uint32_t trbcmd;
+  unsigned int c;
+  int status = -1;
+  
+  if (pageNumber >= NUM_PAGES) return -1;
+  if ((numBytes > PAGE_SIZE) || (numBytes == 0)) return -1;
+  
+  /* Copy pageBuffer to trbBuffer */
+  temp = trbBuffer - 1;
+  for (c = 0; c < numBytes; c++) {
+    if ((c % 4) == 0) {
+      temp++;
+      *temp = 0;
+    }
+    *temp |= (pageBuffer[c] << ((c % 4) * 8));
+  }
+  
+  /* Transfer trbBuffer */
+  size = temp - trbBuffer + 1;
+  if ((status = trb_register_write_mem(trb_address, BlockRam, 0,
+                                       trbBuffer, size)) 
+      == -1) {
+    fprintf(logFile, "Error > writePage: TRBNet %s\n",
+            trb_strerrorf());
+    return -1;
+  }
+  /* Enable writing */
+  if (sendCommand(trb_address, 0x06 << 24, 0) == -1) {
+    fprintf(logFile, "Error > writePage: write enable\n");
+    return -1;
+  }
+  
+  /* Write page */
+  trbcmd = (0x02 << 24) | ((pageNumber * PAGE_SIZE) & 0xffffff);
+  if (sendCommand(trb_address, trbcmd, numBytes - 1) == -1) {
+    fprintf(logFile, "Error > writePage: write page\n");
+    return -1;
+  }
+  
+  /* Wait for not busy and check status */
+  while ((status = checkStatus(trb_address) == -2)) {
+    /* fprintf(logFile, "Wait pageProgram ...\n"); */
+  }
+  
+  if (status != 0) {
+    fprintf(logFile, "Error > pageWrite: invalid status\n");
+    return -1;         
+  }
+  
+  return 0;
+}
+
+typedef enum {
+  PMODE_PROGRAM,
+  PMODE_PROGRAM_FULL,
+  PMODE_VERIFY
+} PMode;
+
+static int programImageBuffer(uint16_t trb_address,
+                              unsigned int size, 
+                              PMode mode, 
+                              int infoPage)
+{
+  int bla = 0;
+  unsigned int block;
+  int status;
+  int errorCtr = 0;
+  unsigned int i;
+  unsigned int page;
+  int bytesWritten = size;
+  int tmp;
+  
+  if ((mode != PMODE_VERIFY) && (yesToAll == 0)) {
+    /* Be nice and ask before start flashing the roms */
+    char c;
+    if ((flashType == FLASH_MDC_OEP_V2) || 
+        (flashType == FLASH_MDC_OEP_V3)) {
+      if (mdcFlashSelect == 0) {
+        fprintf(stdout,
+                "You decided to reprogram the FlashRom(s) #0 of "
+                "MDC_OEP, the so called 'GoldenImage'. "
+                "Do you really know what you're about to do [N,y]: ");
+      } 
+    } else {
+      fprintf(stdout,
+              "You decided to reprogram the FlashRom(s) of "
+              "%s, are you sure [N,y]: ", FlashTypeStr[flashType]);
+    }
+    
+    c = getc(stdin);
+    if (!((c == 'Y') || (c == 'y'))) {
+      fprintf(stderr, "\nAborting on user request\n");
+      return -1;
+    }
+  }
+  
+  if (mode != PMODE_VERIFY) { 
+    fprintf(stderr,
+            "Programming Endpoint(s) @ Address 0x%04x\n",
+            trb_address);
+    fprintf(stderr,
+            "Symbols:\n"
+            "  E: Erasing\n"
+            "  P: Programming\n"
+            "  @: Success\n"
+            "  .: Skipped\n\n");
+  } else {
+    fprintf(stderr,
+            "Verifying Endpoint(s) @ Address 0x%04x\n",
+            trb_address);
+    fprintf(stderr,
+            "Symbols:\n"
+            "  V: Verifying\n"
+            "  X: Failed (see logfile 'trbflash.log' for details)\n"
+            "  @: Success\n"
+            "  .: Skipped\n\n");
+  }
+  fprintf(stderr, "Block: 0 1 2 3 4 5 6 7 8 9 A B C D E F");
+  
+  errorCtr = 0;
+  
+  if ((mode != PMODE_VERIFY) && (manId == 0x1520c2)) {
+    /* Unprotect all Sectors */
+    if (writeStatusRegister(trb_address, 0) == -1) {
+      fprintf(stderr,
+              "\nError > program: unprotect all sectors, aborting\n");
+      return -1;
+    }
+  } 
+    
+  for (block = 0; (block < NUM_BLOCKS); block++) {
+    int error = 0;
+    int writeInfoPage = 
+      (block == NUM_BLOCKS - 1) && (infoPage == 1) ? 1 : 0;
+    if (block % 16 == 0) {
+      fprintf(stderr, "\n%x      ", block / 16); 
+    }
+    fprintf(stderr, ".\b");
+    
+    if (!(bytesWritten > 0) && (writeInfoPage == 0)) {
+      fprintf(stderr, ". ");
+      continue;
+    }
+    
+    if ((mode != PMODE_VERIFY) && 
+        ((bytesWritten > 0) || (writeInfoPage == 1))) {
+      if ((manId == 0x01461f) || 
+          (manId == 0x00471f)) {
+        /* Enable writing */
+        if (sendCommand(trb_address, 0x06 << 24, 0) == -1) {
+          fprintf(stderr, "\nError > program: write enable, aborting\n");
+          return -1;
+        }
+        
+        /* Unprotect sector */
+        if (sendCommand(trb_address, 0x39 << 24 | (BLOCK_SIZE * block), 3) 
+            == -1) {
+          fprintf(stderr,
+                  "\nError > program: unprotect sector #%d, aborting\n",
+                  block);
+          return -1;
+        }
+      }
+      
+      /* Enable writing */
+      if (sendCommand(trb_address, 0x06 << 24, 0) == -1) {
+        fprintf(stderr, "\nError > program: write enable, aborting\n");
+        return -1;
+      }
+    
+      /* Erase block */
+      fprintf(stderr, "E\b");
+      if (sendCommand(trb_address, 0xd8 << 24 | (BLOCK_SIZE * block), 3) 
+          == -1) {
+        fprintf(stderr, "\nError > program: erase block #%d, aborting\n",
+                block);
+        return -1;
+      }
+      
+      /* Wait for not busy and check status */
+      while ((status = checkStatus(trb_address) == -2)) {}
+      
+      if (status != 0) {
+        fprintf(stderr, "\nError > program: invalid status, aborting\n");
+        return -1;         
+      }
+      
+      /* Now write pages */
+      fprintf(stderr, "P\b");
+      tmp = bytesWritten;
+      for (i = 0, page = (block * BLOCK_SIZE) / PAGE_SIZE;
+           (i < (BLOCK_SIZE / PAGE_SIZE)) && (tmp > 0); 
+           i++, page++) {
+        int bytes = tmp < PAGE_SIZE ? tmp : PAGE_SIZE;        
+        tmp -= bytes;
+        bytesWritten = tmp;
+        
+        status = writePage(trb_address, page, 
+                           imageBuffer + page * PAGE_SIZE, bytes);
+        if (status == -1) {
+          fprintf(stderr,
+                  "\nError > program: pageProgram page #%d, aborting\n",
+                  page);
+          return -1;
+        }
+      }
+
+      if (writeInfoPage == 1) {
+        /* Write Info-Page */
+        status = writePage(trb_address, (NUM_PAGES - 1), 
+                           imageBuffer + (NUM_PAGES - 1) * 
+                           PAGE_SIZE, PAGE_SIZE);
+        if (status == -1) {
+          fprintf(stderr,
+                  "\nError > program: pageProgram page #%d, aborting\n",
+                  page);
+          return -1;
+        }
+      }
+      
+      if ((manId == 0x01461f) || 
+          (manId == 0x00471f)) {
+        /* Enable writing */
+        if (sendCommand(trb_address, 0x06 << 24, 0x00) == -1) {
+          fprintf(stderr, "\nError > program: write enable, aborting\n");
+          return -1;
+        }
+        
+        /* Protect sector */
+        if (sendCommand(trb_address, 0x36 << 24 | (BLOCK_SIZE * block), 3) 
+            == -1) {
+          fprintf(stderr, "\nError > program: protect sector #%d, aborting\n",
+                  block);
+          return -1;
+        }
+      }
+    } else {
+      /* Verify pages */
+      fprintf(stderr, "V\b");
+      tmp = bytesWritten;
+      bla++;
+      for (i = 0, page = (block * BLOCK_SIZE) / PAGE_SIZE;
+           (i < (BLOCK_SIZE / PAGE_SIZE)) && (tmp > 0);
+           i++, page++) {
+        unsigned int endPoint;
+        unsigned int c;
+        int bytes = tmp < PAGE_SIZE ? tmp : PAGE_SIZE;        
+        tmp -= bytes;
+        bytesWritten = tmp;
+        
+        if ((status = readPage(trb_address, page, bytes)) == -1) {
+          fprintf(stderr, "\nError > program: reading Page #%d, aborting\n",
+                  page);
+          return -1;
+        }
+        
+        for (endPoint = 0; endPoint < status; endPoint++) {
+          for (c = 0; c < bytes; c++) {
+            if (pageBuffer[endPoint][c] != imageBuffer[page * PAGE_SIZE + c]) {
+              error = -1;
+              errorCtr++;
+              fprintf(logFile, 
+                      "verify failed page #%d, byte #%d "
+                      "(exp: 0x%02x rec: 0x%02x), EndPoint: 0x%04x\n",
+                      page, c, 
+                      imageBuffer[page * PAGE_SIZE + c], 
+                      pageBuffer[endPoint][c],
+                      pageBufferAddress[endPoint]);
+            }
+          }
+        }
+      }
+      
+      /* Verify Info-Page */
+      if (writeInfoPage == 1) {
+        unsigned int endPoint;
+        unsigned int c;
+        int bytes = tmp < PAGE_SIZE ? tmp : PAGE_SIZE;        
+        tmp -= bytes;
+        
+        if ((status = readPage(trb_address, NUM_PAGES - 1, PAGE_SIZE)) == -1) {
+          fprintf(stderr, "\nError > program: reading InfoPage, aborting\n");
+          return -1;
+        }
+        
+        for (endPoint = 0; endPoint < status; endPoint++) {
+          for (c = 0; c < PAGE_SIZE; c++) {
+            if (pageBuffer[endPoint][c] != 
+                imageBuffer[(NUM_PAGES - 1) * PAGE_SIZE + c]) {
+              error = -1;
+              errorCtr++;
+              fprintf(logFile, 
+                      "verify failed InfoPage, byte #%d "
+                      "(exp: 0x%02x rec: 0x%02x), EndPoint: 0x%04x\n",
+                      c, 
+                      imageBuffer[(NUM_PAGES - 1) * PAGE_SIZE + c], 
+                      pageBuffer[endPoint][c],
+                      pageBufferAddress[endPoint]);
+            }
+          }
+        }
+      }
+    }
+    
+    if (error == 0) {
+      fprintf(stderr, "@ ");
+    } else {
+      fprintf(stderr, "X ");
+    }
+  }
+  
+  if ((mode != PMODE_VERIFY) && (manId == 0x1520c2)) {
+    /* Protect all Sectors  i.e. write 0x3c to statusRegister */
+    if (writeStatusRegister(trb_address, 0x00) == -1) {
+      fprintf(stderr, "\nError > program: protect all sectors, aborting\n");
+      return -1;
+    }
+  } 
+  
+  if (errorCtr == 0) {
+    fprintf(stderr, "\n\nSuccess\n\n");
+  } else {
+    fprintf(stderr,
+            "\n\n%d Errors have occured, see logFile %s for details\n\n",
+            errorCtr, logFileName);
+  }
+
+  return errorCtr == 0 ? 0 : -1;
+}
+
+
+static int readImageFile(const char *imageFileName)
+{ 
+  FILE *imageFile = NULL;
+  int imageSize;
+  unsigned int i;
+  
+  /* Cleanup Buffer */
+  for (i = 0; i < (PAGE_SIZE * (NUM_PAGES + 2)); i++) imageBuffer[i] = 0;
+  
+  /* Read in image */
+  imageFile = fopen(imageFileName, "r");    
+  if (imageFile == NULL) {
+    fprintf(logFile,
+            "Error > readImageFile: Could not open ImageFile %s: %s\n", 
+            imageFileName, strerror(errno));
+    return -1;
+  }
+  
+  imageSize = 0;
+  do {
+    imageSize += fread((void*)(imageBuffer + imageSize), sizeof(uint8_t),
+                       PAGE_SIZE, imageFile);
+  } while ((feof(imageFile) == 0) && (imageSize <= PAGE_SIZE * NUM_PAGES));
+  
+  fclose(imageFile);
+  
+  if (imageSize > (PAGE_SIZE * NUM_PAGES)) {
+    fprintf(logFile,
+            "Error > readImageFile: Imagefile '%s' is too large (%d bytes)\n", 
+            imageFileName, imageSize); 
+    return -1;
+  }
+  
+  return imageSize;
+}
+
+static int prepareImageBuffer()
+{
+  char* strId = "INVALID";
+  int found;
+  unsigned int end;
+  unsigned int i;
+
+  /* Verify imageFile Id */
+  switch (flashType) {
+    
+  case FLASH_RICH_ADCM_V1:
+    strId = "adcmv1";
+    break;
+    
+  case FLASH_RICH_ADCM_V2:
+    strId = "adcmv2";
+    break;
+    
+  case FLASH_RICH_ADCM_V3:
+    strId = "adcmv3";
+    break;
+    
+  case FLASH_MDC_HUB_V2_FPGA1234:
+    strId = "mdchub_fpga1234";
+    break;
+  case FLASH_MDC_HUB_V2_FPGA5:
+    strId = "mdchub_fpga5";
+    break;
+
+  case FLASH_MDC_OEP_V2:
+  case FLASH_MDC_OEP_V3:
+    strId = "mdc_oepb";
+    break;
+
+  case FLASH_SHOWER_ADDON_V2_FPGA1:
+    strId = "shower_fpga1";
+    break;
+  case FLASH_SHOWER_ADDON_V2_FPGA2:
+    strId = "shower_fpga2";
+    break;
+  case FLASH_SHOWER_ADDON_V2_FPGA3:
+    strId = "shower_fpga3";
+    break;
+  
+  case FLASH_CTS_FPGA1:
+    strId = "cts_fpga1";
+  case FLASH_CTS_FPGA2:
+    strId = "cts_fpga2";
+    break;
+    
+  default:
+    abort();
+    break;
+  }
+  
+  /* Verify imageFile Id */
+  found = 0;
+  for (i = 0; i < 2 * PAGE_SIZE; i++) {
+    if (memcmp(imageBuffer + i, strId, strlen(strId)) == 0) {
+      found = 1;
+      break;
+    }
+  }
+  
+  if ((skipFirmwareIdCheck == 0) && (found == 0)) {
+    fprintf(logFile, "Error > prepareImageBuffer: "
+            "invalid Firmware-Id of Image-File, should be: %s\n", strId);
+    return -1;
+  }
+    
+  /* Overwrite Header with 0xff */
+  end = 0;
+  for (i = 0; i < 2 * PAGE_SIZE; i++) {
+    if (strncmp((char*)(imageBuffer + i), "Bitstream CRC: 0x", 17) == 0) {
+      end = i + 22;
+      break;
+    }
+  }
+  if (end == 0) {
+    fprintf(logFile, "Error > prepareImageBuffer: invalid Firmware, "
+            "CRC not found\n"); 
+  }
+  for (i = 0; i < end; i++) {
+    imageBuffer[i] = 0xff;
+  }
+
+  return 0;
+}
+
+static int createInfoPage(const char* fileName, const char* userString)
+{
+  char* buffer = (char*)(imageBuffer + (NUM_PAGES - 1) * PAGE_SIZE);
+  struct stat statBuf;
+
+  unsigned int i;
+  for (i = 0; i < PAGE_SIZE; i++) {
+    buffer[i] = '\0';
+  }
+  if (stat(fileName, &statBuf) == -1) {
+    fprintf(logFile, "Error > prepareInfoPage: statCall failed: %s\n",
+            strerror(errno));  
+    return -1;
+  }
+  snprintf(buffer + 0, 63, "NAME: %s", basename((char*)fileName)); 
+  snprintf(buffer + 64, 31, "DATE: %s", ctime(&statBuf.st_mtime));
+  snprintf(buffer + 96, 159, "USER: %s", userString);
+  fprintf(stderr, "%s\n", buffer); 
+  fprintf(stderr, "%s\n", buffer + 64); 
+  fprintf(stderr, "%s\n", buffer + 96);
+  
+  return 0;
+}
+
+static int openLog() {
+  time_t datet;
+  char date[128];
+    
+  /* Open LogFile */
+  logFile = fopen(logFileName, "w+");
+  if (logFile == NULL) {
+    fprintf(stderr, "Could not open logFile %s: %s\n", 
+            logFileName, strerror(errno));
+    return -1;
+  }
+  
+  datet = time(NULL);
+  ctime_r(&datet, date);
+  date[strlen(date) - 1] = '\0';
+  fprintf(logFile, 
+          "\n\n------------ %s ----------------------------------\n\n", date);
+
+  return 0;
+}
+
+static int selectMdcFlashRom(uint16_t trb_address, uint8_t number) {
+  /* Select MDC-FlashRom */
+  if (trb_register_write(trb_address, MDCFlashRomSelect, 
+                         (uint32_t)number) == -1) {
+    fprintf(logFile, "\nError > selcetMdcFlashRom: TRBNet %s\n",
+            trb_strerrorf());
+    return -1;
+  }
+  
+  return 0;
+}
+
+/* ------ MAIN ---------------------------------------------------------- */
+
+void usage(const char *progName)
+{
+  printf("Usage: %s [-s str] [-g] [-y] [-f] [-h] [-V] <COMMAND>\n",
+         progName);
+  printf("Options:\n");
+  printf("  -s    set user-string of info page (default: empty string)\n");
+  printf("  -g    select 'FlashRom #0 (the 'Golden Image') "
+         "(MDC only, default = no)\n");
+  printf("  -y    assume yes to all questions which might will be asked\n");
+  printf("  -f    do not validate FirmwareId of ImageFile (DON'T USE, for "
+         "EXPERTS ONLY)\n");
+  /*  printf("  -F    do not crosscheck HardwareIDs of Endpoints "
+      "(DON'T USE, for EXPERTS ONLY)\n"); */
+  printf("  -h    give this help\n");
+  printf("  -V    Version number\n");
+  printf("\nCommands:\n");
+  printf("   program <trbaddress> <bit-file>  -> "
+         "program bit-file to flash-memory\n");
+  printf("   verify <trbaddress> <bit-file>   -> "
+         "compare bit-file with flash-memory\n");
+  printf("   backup <trbaddress> <raw-file>   -> "
+         "write entire flash-memory to raw-file\n");
+  printf("   restore <trbaddress> <raw-file>  -> "
+         "write backuped raw-file to flash-memory\n");
+  printf("   info <trbaddress>                -> "
+         "dump content of info-page to stdout\n");
+  printf("   dumppage <trbaddress> pagenumber -> "
+         "dump one flash-memory page to stdout\n");
+}
+
+int main(int argc, char ** argv)
+{
+  unsigned int i;
+  char userInfoStr[256] = "";
+  
+  trb_debug = 0;
+  logFile = stderr;
+  mdcFlashSelect = 1;
+  yesToAll = 0;
+  skipFirmwareIdCheck = 0;
+  skipHwIdCheck = 0;
+  
+  /* Parse Arguments */
+  while ((i = getopt(argc, argv, "+hd:s:gyVf")) != -1) {
+    switch (i) {
+    case '?':
+      usage(basename(argv[0]));
+      exit(EXIT_FAILURE);
+    case 'h':
+      usage(basename(argv[0]));
+      exit(EXIT_SUCCESS);
+    case 's':
+      strncpy(userInfoStr, optarg, 256);
+      break;
+    case 'g':
+      mdcFlashSelect = 0;
+      break;  
+    case 'd':
+      trb_debug = strtoul(optarg, NULL, 0);
+      break;
+    case 'y':
+      yesToAll = 1;
+      break;
+    case 'f':
+      skipFirmwareIdCheck = 1;
+      break;
+    case 'F':
+      skipHwIdCheck = 2;
+      break;
+    case 'V':
+      printf("%s %s, using libtrbnet %s\n",
+             basename(argv[0]), trbflash_version, trbnet_version);
+      exit(EXIT_SUCCESS);
+      break;
+    default:
+      usage(basename(argv[0]));
+      exit(EXIT_FAILURE);
+      break;
+    }
+  }
+
+  if (argc - optind < 1) {
+    usage(basename(argv[0]));
+    exit(EXIT_FAILURE);
+  }
+  if (init_ports() == -1) {
+    trb_error("Init_Ports");
+    exit(EXIT_FAILURE);
+  }
+  
+  atexit(atexit0);
+
+  /* Allocate memory for buffers */
+  
+  /* Buffer holding the TRBNet packages, e.g. use by trb_read_register */
+  trbBuffer = (uint32_t*)malloc(sizeof(uint32_t) * TRB_BUFFER_SIZE);
+  if (trbBuffer == NULL) {
+    abort();
+  }
+  
+  /* Buffer holding the pages of the endpoints */
+  pageBuffer = (uint8_t**)malloc(sizeof(uint8_t*) * NUM_ENDPOINTS);
+  if (pageBuffer == NULL) {
+    abort();
+  }
+  for (i = 0; i < NUM_ENDPOINTS; i++) {
+    pageBuffer[i] = (uint8_t*)malloc(sizeof(uint8_t) * PAGE_SIZE);
+    if (pageBuffer[i] == NULL) abort();
+  }
+  
+  /* Buffer holding the corresponding TRBAddresses */
+  pageBufferAddress = (uint16_t*)malloc(sizeof(uint16_t) * NUM_ENDPOINTS);
+  if (pageBufferAddress == NULL) {
+    abort();
+  }
+  
+  /* Execute command */
+  if (strcmp(argv[optind], "program") == 0) {
+    
+    /*********************************************************/
+    /* Program Image                                         */
+    /*********************************************************/
+    
+    uint16_t trb_address;
+    char* imageFileName = NULL;
+    int size;
+    
+    if (argc - optind != 3) {
+      usage(basename(argv[0]));
+      exit(EXIT_FAILURE);
+    }
+    
+    trb_address = strtoul(argv[optind + 1], NULL, 0);
+    imageFileName = argv[optind + 2];
+    
+    /* Check for correct ManId */
+    if (initTransfer(trb_address) == -1) {
+      fprintf(stderr, "InitTransfer failed, aborting\n");
+      exit(EXIT_FAILURE);
+    }
+    
+    /* Read imageFile to imageBuffer */
+    if ((size = readImageFile(imageFileName)) == -1) {
+      exit(EXIT_FAILURE);
+    }
+    if (size > (PAGE_SIZE * (NUM_PAGES - 1))) {
+      fprintf(stderr, "ImageFile to large (%d bytes, maximum is %d bytes)\n",
+              size, PAGE_SIZE * (NUM_PAGES - 1));
+      exit(EXIT_FAILURE);
+    }
+    
+    /* Validate Id and prepare imageBuffer */
+    if (prepareImageBuffer() == -1) {
+      exit(EXIT_FAILURE); 
+    }
+    
+    /* Create InfoPage */
+    createInfoPage(imageFileName, userInfoStr);
+    
+    /* In case of MDC: select FlashRom */
+    if ((flashType == FLASH_MDC_OEP_V2) || 
+        (flashType == FLASH_MDC_OEP_V3)) {
+      if (selectMdcFlashRom(trb_address, mdcFlashSelect) == -1) {
+        exit(EXIT_FAILURE);
+      }
+    }
+    
+    /* Open LogFile */
+    if (openLog() == -1) {
+      exit(EXIT_FAILURE);
+    }
+    
+    fprintf(stderr, "Start programming ImageFile '%s'\n",
+            imageFileName);
+    
+    if (programImageBuffer(trb_address, size, PMODE_PROGRAM, 1) == -1) {
+      exit(EXIT_FAILURE);
+    }
+
+    if (programImageBuffer(trb_address, size, PMODE_VERIFY, 1) == -1) {
+      exit(EXIT_FAILURE);
+    }
+    
+  } else if (strcmp(argv[optind], "verify") == 0) {
+    
+    /*********************************************************/
+    /* Verify Image                                          */
+    /*********************************************************/
+    
+    uint16_t trb_address;
+    char* imageFileName = NULL;
+    int size;
+    
+    if (argc - optind != 3) {
+      usage(basename(argv[0]));
+      exit(EXIT_FAILURE);
+    }
+    
+    trb_address = strtoul(argv[optind + 1], NULL, 0);
+    imageFileName = argv[optind + 2];
+    
+    /* Check for correct ManId */
+    if (initTransfer(trb_address) == -1) {
+      fprintf(stderr, "InitTransfer failed, aborting\n");
+      exit(EXIT_FAILURE);
+    }
+    
+    /* Read imageFile to imageBuffer */
+    if ((size = readImageFile(imageFileName)) == -1) {
+      exit(EXIT_FAILURE);
+    }
+    if (size > (PAGE_SIZE * (NUM_PAGES - 1))) {
+      fprintf(stderr, "ImageFile to large (%d bytes, maximum is %d bytes)\n",
+              size, PAGE_SIZE * (NUM_PAGES - 1));
+      exit(EXIT_FAILURE);
+    }
+
+    /* Validate Id and prepare imageBuffer */
+    if (prepareImageBuffer() == -1) {
+      exit(EXIT_FAILURE); 
+    }
+    
+    /* In case of MDC: select FlashRom */
+    if ((flashType == FLASH_MDC_OEP_V2) || 
+        (flashType == FLASH_MDC_OEP_V3)) {
+      if (selectMdcFlashRom(trb_address, mdcFlashSelect) == -1) {
+        exit(EXIT_FAILURE);
+      }
+    }
+
+    /* Open LogFile */
+    if (openLog() == -1) {
+      exit(EXIT_FAILURE);
+    }
+    
+    fprintf(stderr, "Start verifying ImageFile '%s'\n",
+            imageFileName);
+    
+    if (programImageBuffer(trb_address, size, PMODE_VERIFY, 0) == -1) {
+      exit(EXIT_FAILURE);
+    }
+    
+  } else if (strcmp(argv[optind], "backup") == 0) {
+    
+    /*********************************************************/
+    /* Backup Image                                          */
+    /*********************************************************/
+    
+    uint16_t trb_address;
+    char* imageFileName;
+    FILE* imageFile = NULL;
+    unsigned int page;
+    
+    if (argc - optind != 3) {
+      usage(basename(argv[0]));
+      exit(EXIT_FAILURE);
+    }
+    trb_address = strtoul(argv[optind + 1], NULL, 0);
+    imageFileName = argv[optind + 2];
+    
+    if (trb_address >= 0xff00) {
+      fprintf(stderr,
+              "Broadcast addresses are not supported by this command\n");
+      exit(EXIT_FAILURE);
+    }
+    
+    /* In case of MDC: select FlashRom */
+    if ((flashType == FLASH_MDC_OEP_V2) || 
+        (flashType == FLASH_MDC_OEP_V3)) {
+      if (selectMdcFlashRom(trb_address, mdcFlashSelect) == -1) {
+        exit(EXIT_FAILURE);
+      }
+    }
+    
+    if (initTransfer(trb_address) == -1) {
+      fprintf(stderr, "InitTransfer failed, aborting\n");
+      exit(EXIT_FAILURE);
+    }
+
+    if (checkStatus(trb_address) < 0) {
+      exit(EXIT_FAILURE);
+    }
+    
+    /* Write Pages to file */
+    fprintf(stderr,
+            "Dumping entire flashrom of EndPoint 0x%04x to file '%s' ",
+            trb_address, imageFileName);
+    
+    imageFile = fopen(imageFileName, "w+");    
+    if (imageFile == NULL) {
+      fprintf(stderr, "Could not open Imagefile %s: %s\n", 
+              imageFileName, strerror(errno));
+      exit(EXIT_FAILURE);
+    }
+
+    for (page = 0; page < NUM_PAGES; page++) {
+      if (readPage(trb_address, page, PAGE_SIZE) == -1) {
+        fprintf(stderr, "\nError reading Page #%d, aborting..\n", page);
+        fclose(imageFile);
+        exit(EXIT_FAILURE);
+      }
+      if (fwrite((void*)pageBuffer[0], PAGE_SIZE, 1, imageFile) != 1) {
+        fprintf(stderr, 
+                "\nError writing Page #%d to file, aborting..\n", page);
+        fclose(imageFile);
+        exit(EXIT_FAILURE);
+      }
+      
+      fprintf(stderr, "%s", busy[page % 4]);
+    }
+    fprintf(stderr, " \nDone\n");
+    
+    fclose(imageFile);
+    
+  } else if (strcmp(argv[optind], "restore") == 0) {
+    
+    /*********************************************************/
+    /* Restore Image                                         */
+    /*********************************************************/
+        
+    uint16_t trb_address;
+    char* imageFileName;
+    int size;
+            
+    if (argc - optind != 3) {
+      usage(basename(argv[0]));
+      exit(EXIT_FAILURE);
+    }
+    trb_address = strtoul(argv[optind + 1], NULL, 0);
+    imageFileName = argv[optind + 2];
+    
+    /* Check for correct ManId */
+    if (initTransfer(trb_address) == -1) {
+      fprintf(stderr, "InitTransfer failed, aborting\n");
+      exit(EXIT_FAILURE);
+    }
+    
+    /* Read imageFile to imageBuffer */
+    if ((size = readImageFile(imageFileName)) == -1) {
+      exit(EXIT_FAILURE);
+    }
+    
+    if (size != PAGE_SIZE * NUM_PAGES) {
+      fprintf(stderr, "Invalid ImageFile %s, aborting\n", imageFileName);
+      exit(EXIT_FAILURE);
+    }
+    
+    /* Check ImageBuffer ??? */
+
+    /* In case of MDC: select FlashRom */
+    if ((flashType == FLASH_MDC_OEP_V2) || 
+        (flashType == FLASH_MDC_OEP_V3)) {
+      if (selectMdcFlashRom(trb_address, mdcFlashSelect) == -1) {
+        exit(EXIT_FAILURE);
+      }
+    }
+
+    /* Open LogFile */
+    if (openLog() == -1) {
+      exit(EXIT_FAILURE);
+    }
+    
+    fprintf(stderr, "Start restoring ImageFile '%s'\n",
+            imageFileName);
+    
+    if (programImageBuffer(trb_address, size, PMODE_PROGRAM_FULL, 0) == -1) {
+      exit(EXIT_FAILURE);
+    }
+  
+    if (programImageBuffer(trb_address, size, PMODE_VERIFY, 0) == -1) {
+      exit(EXIT_FAILURE);
+    }
+    
+  } else if (strcmp(argv[optind], "info") == 0) {
+    
+    /*********************************************************/
+    /* Dump Info-Page                                        */
+    /*********************************************************/
+    
+    uint16_t trb_address;
+    unsigned int i;
+    int status;
+    
+    if (argc - optind != 2) {
+      usage(basename(argv[0]));
+      exit(EXIT_FAILURE);
+    }
+    trb_address = strtoul(argv[optind + 1], NULL, 0);
+    
+    /* Check for correct ManId */
+    if (initTransfer(trb_address) == -1) {
+      fprintf(stderr, "InitTransfer failed, aborting\n");
+      exit(EXIT_FAILURE);
+    }
+
+    /* In case of MDC: select FlashRom */
+    if ((flashType == FLASH_MDC_OEP_V2) || 
+        (flashType == FLASH_MDC_OEP_V3)) {
+      if (selectMdcFlashRom(trb_address, mdcFlashSelect) == -1) {
+        exit(EXIT_FAILURE);
+      }
+    }
+
+    if ((status = readPage(trb_address, NUM_PAGES - 1, PAGE_SIZE)) == -1) {
+      fprintf(stderr, "Error reading InfoPage, aborting\n");
+      exit(EXIT_FAILURE);
+    }
+
+    for (i = 0; i < status; i++) {
+      pageBuffer[i][63] = '\0';
+      pageBuffer[i][95] = '\0';
+      pageBuffer[i][255] = '\0';
+      fprintf(stdout, "\nEndPoint: 0x%04x   InfoPage\n",
+              pageBufferAddress[i]);
+      if (
+          (strncmp((char*)(pageBuffer[i] + 0), "NAME:", 5) != 0) ||
+          (strncmp((char*)(pageBuffer[i] + 64), "DATE:", 5) != 0) ||
+          (strncmp((char*)(pageBuffer[i] + 96), "USER:", 5) != 0)
+          ) {
+        fprintf(stdout, "INVALID CONTENT\n");
+        continue;
+      }
+      
+      fprintf(stdout, "%s\n", pageBuffer[i]); 
+      fprintf(stdout, "%s\n", pageBuffer[i] + 64); 
+      fprintf(stdout, "%s\n", pageBuffer[i] + 96);
+    }
+    fprintf(stdout, "\n");
+          
+  } else if (strcmp(argv[optind], "dumppage") == 0) {
+    
+    /*********************************************************/
+    /* Read and dump a page                                  */
+    /*********************************************************/
+    
+    uint16_t trb_address;
+    uint32_t pageNumber;
+    unsigned int i;
+    int status;
+    
+    if (argc - optind != 3) {
+      usage(basename(argv[0]));
+      exit(EXIT_FAILURE);
+    }
+    trb_address = strtoul(argv[optind + 1], NULL, 0);
+    pageNumber = strtoul(argv[optind + 2], NULL, 0);
+    
+    /* Check for correct ManId */
+    if (initTransfer(trb_address) == -1) {
+      fprintf(stderr, "InitTransfer failed, aborting\n");
+      exit(EXIT_FAILURE);
+    }
+
+    /* In case of MDC: select FlashRom */
+    if ((flashType == FLASH_MDC_OEP_V2) || 
+        (flashType == FLASH_MDC_OEP_V3)) {
+      if (selectMdcFlashRom(trb_address, mdcFlashSelect) == -1) {
+        exit(EXIT_FAILURE);
+      }
+    }
+
+    if ((status = readPage(trb_address, pageNumber, PAGE_SIZE)) == -1) {
+      fprintf(stderr, "Error reading Page, aborting\n");
+      exit(EXIT_FAILURE);
+    }
+
+    for (i = 0; i < status; i++) {
+      char text[32] = "";
+      char cc[2] = " \0";
+      int c;
+      fprintf(stdout, "\nEndPoint: 0x%04x   Page #%d\n",
+              pageBufferAddress[i], pageNumber); 
+      for (c = 0; c < PAGE_SIZE; c++) {
+        if ((c % 16) == 0) {
+          text[0] = '\0';
+          fprintf(stdout, "0x%02x  ", c);
+        }
+        fprintf(stdout, "%02x ", pageBuffer[i][c]);
+        cc[0] = isprint((char)pageBuffer[i][c]) == 0 
+          ? '.' : (char)pageBuffer[i][c];
+        strncat(text, cc, 32);
+        if ((c % 16) == 15) {
+          fprintf(stdout, "  %s\n", text);
+        }
+      }
+    }
+    fprintf(stdout, "\n");
+  
+  } else {
+    
+    /* Invalid command */
+    usage(basename(argv[0]));
+    exit(EXIT_FAILURE);
+  }
+  
+  exit(EXIT_SUCCESS);
+}