[tools] Update flint to support ConnectX driver.
authortzachid <tzachid@ad392aa1-c5ef-ae45-8dd8-e69d62a5ef86>
Wed, 23 Apr 2008 13:27:47 +0000 (13:27 +0000)
committertzachid <tzachid@ad392aa1-c5ef-ae45-8dd8-e69d62a5ef86>
Wed, 23 Apr 2008 13:27:47 +0000 (13:27 +0000)
git-svn-id: svn://openib.tc.cornell.edu/gen1/trunk@1067 ad392aa1-c5ef-ae45-8dd8-e69d62a5ef86

tools/flint/user/SOURCES
tools/flint/user/flint.cpp
tools/flint/user/mflash.c [new file with mode: 0644]
tools/flint/user/mflash.h [new file with mode: 0644]
tools/mtcr/user/SOURCES
tools/spark/user/spark.cpp

index ab59308..507c899 100644 (file)
@@ -3,58 +3,58 @@ TARGETTYPE=PROGRAM
 UMTYPE=console\r
 USE_MSVCRT=1\r
 \r
-\r
-!if !defined(WINIBHOME)\r
-WINIBHOME=..\..\..\r
+TRUNK=..\..\..\r
+!if !defined(EXTERNAL_TOOLS)\r
+EXTERNAL_TOOLS=$(TRUNK)\ext_libs\r
 !endif\r
 \r
-TARGETPATH=$(WINIBHOME)\bin\user\obj$(BUILD_ALT_DIR)\r
-\r
-\r
-\r
-ZLIB=$(WINIBHOME)\tools\ext_libs\user\zlib-1.1.4\r
-\r
+ZLIB=$(EXTERNAL_TOOLS)\zlib-1.1.4\r
 \r
-SOURCES=flint.rc  \\r
-        flint.cpp\r
+TARGETPATH=$(TRUNK)\bin\user\obj$(BUILD_ALT_DIR)\r
 \r
+386_STDCALL=0\r
+amd64_STDCALL=0\r
 \r
-INCLUDES= $(WINIBHOME)\inc;                    \\r
-          $(WINIBHOME)\inc\user;               \\r
-         $(WINIBHOME)\inc\iba;                \\r
-         $(WINIBHOME)\tools\mtcr\user;        \\r
-          $(ZLIB)\include;\r
-\r
-TARGETLIBS= \\r
+# TODO: Define free and checked.\r
+TARGETLIBS=\\r
 !if $(FREEBUILD)\r
-                       $(SDK_LIB_PATH)\Ws2_32.lib\\r
-                       $(TARGETPATH)\*\mtcr.lib\r
+       $(CRT_LIB_PATH)\msvcprt.lib \\r
 !else\r
-                       $(SDK_LIB_PATH)\Ws2_32.lib\\r
-                       $(TARGETPATH)\*\mtcr.lib\r
+       $(CRT_LIB_PATH)\msvcprt.lib \\r
+!endif \r
+    $(TARGETPATH)\*\mtcr.lib \\r
+    $(SDK_LIB_PATH)\Ws2_32.lib\r
+\r
+\r
+#!if defined(NO_ZLIB)\r
+#C_DEFINES=$(C_DEFINES) -DNO_ZLIB\r
+#!else\r
+#TARGETLIBS=$(TARGETLIBS) $(ZLIB)\lib\zlib.lib\r
+#C_DEFINES=$(C_DEFINES) -DZEXPORT=__cdecl\r
+#!endif\r
+\r
+               \r
+SOURCES=..\mflash.c ..\flint.cpp\r
+\r
+INCLUDES=  \\r
+        ..; \\r
+       $(TRUNK)\tools\WinMst\user\mtcr;  \\r
+       $(TRUNK)\tools\common; \\r
+       $(ZLIB)\include;\\r
+       ..\..\mtcr\user\r
+       \r
+# Could be any special flag needed for this project\r
+USER_C_FLAGS=$(USER_C_FLAGS) /MD /EHsc\r
+\r
+C_DEFINES=$(C_DEFINES) -DWIN32 -D__WIN__ -D__i386__ -DNO_ZLIB\r
+!if !$(FREEBUILD)\r
+C_DEFINES=$(C_DEFINES) -D_DEBUG -DDEBUG -DDBG\r
 !endif\r
 \r
-USER_C_FLAGS=$(USER_C_FLAGS) /Ze /EHsc\r
-\r
-# TODO:Should I define the __WIN__ manually\r
-C_DEFINES=$(C_DEFINES) -D__WIN__ -DZEXPORT=__cdecl\r
-\r
-C_DEFINES=$(C_DEFINES) -DNO_ZLIB\r
-\r
-\r
-!if $(FREEBUILD)\r
-\r
-!else\r
-C_DEFINES=$(C_DEFINES) -DDEBUG\r
-!endif\r
+# LINKER_FLAGS= $(LINKER_FLAGS)\r
 \r
 # Version:\r
-!if !defined(MFT_BLD_VER)\r
-MFT_BLD_VER=Devel\r
+!if defined(VERSION_ID)\r
+C_DEFINES=$(C_DEFINES) "-DVERSION_ID=$(VERSION_ID)"\r
 !endif\r
-C_DEFINES=$(C_DEFINES) "-DVERSION_ID=$(MFT_BLD_VER)"\r
-\r
-386_STDCALL=0\r
-\r
-MSC_WARNING_LEVEL= /W3\r
 \r
index e932663..a598834 100644 (file)
@@ -2,13 +2,10 @@
  *
  * flint.cpp - FLash INTerface
  *
- * Copyright (c) 2005 Mellanox Technologies Ltd.  All rights reserved.
+ * Copyright (c) 2005-2008 Mellanox Technologies Ltd.  All rights reserved.
  *
- * This software is available to you under a choice of one of two
- * licenses.  You may choose to be licensed under the terms of the GNU
- * General Public License (GPL) Version 2, available from the file
- * COPYING in the main directory of this source tree, or the
- * OpenIB.org BSD license below:
+ * This software is available to you under the OpenIB.org BSD license
+ * below:
  *
  *     Redistribution and use in source and binary forms, with or
  *     without modification, are permitted provided that the following
  *
  */
 
-//MTCR needs to be first since it needs to define all kind of
-//macros which affect standard headers.
-
-#include "mtcr.h"
 
 #include <ctype.h>
 #include <stdarg.h>
 #ifndef __WIN__ 
 
 //
-// Linux
+// GCC Compiler
+//
+
+
+#if defined __DJGPP__
+//
+// DJGPP - GCC PORT TO MS DOS 
 //
 
+#include <mtcr.h> // This contains the u_* types definitions
+
+#include <netinet/in.h>
+#include <unistd.h>
+
+#define bswap_32(x) ntohl(x)
+
+// djgpp stdio does not define vsnprintf. I simply call vsprintf (and pray ...)
+#define vsnprintf(buf, len, format, args) (vsprintf(buf, format, args))
+
+#else // Linux GCC 
+
 #include <byteswap.h>
 #include <endian.h>
 #include <alloca.h>
 #include <netinet/in.h>
 #include <unistd.h>
 
+#endif // __DJGPP__
+
 #else // __WIN__
 
 //
@@ -79,7 +92,7 @@
 
 #include <io.h>
 #include <Winsock2.h>
-
+#include <mtcr.h>
 // Sleep adaptor
 #define        usleep(x) Sleep((x)/1000)
 #define sleep(x)  Sleep((x)*1000)
 #include <memory>
 #include <vector>
 
+#include <mflash.h>
+
+
 #ifndef DEV_MST_EXAMPLE1
     #define DEV_MST_EXAMPLE1 "/dev/mst/mt23108_pci_cr0"
 #endif
@@ -133,7 +149,7 @@ char* _versionID = _VFSTR( VERSION_ID ) ;
 char* _versionID = "VERSION_ID_HERE";
 #endif
 
-char* _svnID     = "$Revision$";
+char* _svnID     = "$Revision: 4337 $";
 
 #ifndef __be32_to_cpu
     #define __be32_to_cpu(x) ntohl(x)
@@ -168,29 +184,6 @@ char* _svnID     = "$Revision$";
     #endif
 #endif
 
-#if __BYTE_ORDER == __LITTLE_ENDIAN
-    #ifndef __cpu_to_le16
-        #define  __cpu_to_le16(x) (x)
-    #endif
-    #ifndef __le16_to_cpu
-        #define  __le16_to_cpu(x) (x)
-    #endif
-#elif __BYTE_ORDER == __BIG_ENDIAN
-    #ifndef __cpu_to_le16
-        #define  __cpu_to_le16(x) bswap_16(x)
-    #endif
-    #ifndef __le16_to_cpu
-        #define  __le16_to_cpu(x) bswap_16(x)
-    #endif
-#else
-    #ifndef __cpu_to_le16
-        #define  __cpu_to_le16(x) bswap_16(__cpu_to_be16(x))
-    #endif
-    #ifndef __le16_to_cpu
-        #define  __le16_to_cpu(x) __be16_to_cpu(bswap_16(x))
-    #endif
-#endif
-
 
 ////////////////////////////////////////////////////////////////////////
 //                                                                    //
@@ -215,6 +208,7 @@ static inline void cpu_to_be_guid(guid_t* to, guid_t* from) {
 }
 
 #define GUID_FORMAT "%8.8x%8.8x"
+#define MAC_FORMAT  "%4.4x%8.8x"
 #define TOCPU1(s) s = __be32_to_cpu(s)
 #define TOCPU(s) do {                                              \
     u_int32_t *p = (u_int32_t *)(s);                               \
@@ -268,30 +262,12 @@ static inline void cpu_to_be_guid(guid_t* to, guid_t* from) {
     } while(0)
 #define CHECKB2(f,b,o,n,p) do { if (!checkBoot2(f,b,o,n,p)) return false; } while (0)
 #define CHECKGN(f,b,o,n,p) do { if (!checkGen(f,b,o,n,p)) return false; } while (0)
-#define CHECKLS(f,o,p) do { if (!checkList(f,o,p)) return false; } while(0)
+#define CHECKLS(f,o,s,p) do { if (!checkList(f,o,s,p)) return false; } while(0)
 #define READ4(f,o,d,p) do { if (!f.read(o,d)) {  \
     return errmsg("%s - read error (%s)\n", p, f.err()); }} while (0)
 #define READBUF(f,o,d,l,p) do { if (!f.read(o,d,l)) { \
     return errmsg("%s - read error (%s)\n", p, f.err()); }} while (0)
 
-#if  1
-
-    #define MWRITE4(offs,val) do { \
-    if (mwrite4(_mf, offs, val) != 4) return false; } while (0)
-    #define MREAD4(offs,val) do { \
-   if (mread4(_mf, offs, val) != 4) return false; } while (0)
-
-#else
-
-    #define MWRITE4(offs,val) do { \
-    printf("%08x <- %08x\n", (u_int32_t)offs, (u_int32_t)val); \
-    if (mwrite4(_mf, offs, val) != 4) return false; } while (0)
-    #define MREAD4(offs,val) do { \
-    if (mread4(_mf, offs, val) != 4) return false; \
-     printf("%08x -> %08x\n", (u_int32_t)offs, (u_int32_t)(*(val))); \
-    } while (0)
-
-#endif
 
 class ErrMsg
 {
@@ -350,7 +326,8 @@ bool ErrMsg::errmsg(const char *format, ...) {
 
 
 enum {
-    SIGNATURE=0x5a445a44
+    SIGNATURE          = 0x5a445a44,
+    MELLANOX_VENDOR_ID = 0x15b3
 };
 struct PS {
     u_int32_t fi_addr;
@@ -362,6 +339,7 @@ struct PS {
     u_int32_t branch_to;
     u_int32_t crc016;
 };
+
 enum {
     H_FIRST     =  1, 
     H_DDR       =  1, 
@@ -374,9 +352,26 @@ enum {
     H_USER_DATA =  8,
     H_FW_CONF   =  9,
     H_IMG_INFO  = 10,
-    H_LAST      = 10 
+    H_DDRZ      = 11,
+    H_LAST
+};
+
+const char* g_sectNames[] = {
+    "UNKNOWN (0 - Reserved)",
+    "DDR"             , 
+    "Configuration"   ,
+    "Jump addresses"  ,
+    "EMT Service"     ,
+    "ROM"             ,
+    "GUID"            ,
+    "BOARD ID"        , 
+    "User Data"       ,
+    "FW Configuration",
+    "Image Info"      ,
+    "DDRZ"
 };
 
+
 struct GPH {
     u_int32_t type;
     u_int32_t size;
@@ -384,6 +379,8 @@ struct GPH {
     u_int32_t next;
 };
 
+#define MAX_SECTION_SIZE 0x400000
+
 const u_int32_t BOARD_ID_BSN_LEN=64;
 const u_int32_t BOARD_ID_BID_LEN=32;
 const u_int32_t BOARD_ID_PID=7;
@@ -395,6 +392,7 @@ struct BOARD_ID {
 
 int  const VSD_LEN  = 208;
 int  const PSID_LEN = 16;
+int  const PRODUCT_VER_LEN = 16;
 
 //
 // TODO: Remove the below globals to class members.
@@ -402,7 +400,7 @@ int  const PSID_LEN = 16;
 bool _print_crc = false;
 bool _silent = false;
 bool _assume_yes = false;
-bool _image_is_full;
+bool _assume_no = false;
 bool _no_erase = false;
 bool _no_burn = false;
 
@@ -446,16 +444,6 @@ void report_erase(const char *format, ...)
        printf("\b");
 } // report_erase
 
-static u_int32_t log2up (u_int32_t in) {
-    u_int32_t i;
-    for (i = 0; i < 32; i++) {
-        if (in <= (u_int32_t)(1 << i))
-            break;
-    }
-
-    return i;
-}
-
 
 ////////////////////////////////////////////////////////////////////////
 //                                                                    //
@@ -570,6 +558,66 @@ private:
     u_int8_t   _eptr;
 };
 
+//////////////////////////////////////////////////////////////////////
+//
+//  class Aligner:
+//  A utillity class for accessing an addr/size region 
+//  in a specific alignment.
+//  If a 0 alignment is given, infinity (no alignment requirements) 
+//  is assumed - This is to support single flow for the caller.
+//
+//////////////////////////////////////////////////////////////////////
+
+class Aligner {
+public:
+    Aligner(u_int32_t log2_alignment_size) :
+        _log2_alignment_size(log2_alignment_size),
+        _alignment_size(1 << log2_alignment_size),
+        _alignment_mask(_alignment_size - 1)
+    {
+        if (_log2_alignment_size == 0) {
+            _log2_alignment_size = 31;
+            _alignment_size      = 1 << _log2_alignment_size;
+            _alignment_mask      = _alignment_size - 1;
+        }
+    }
+    
+    void Init        (u_int32_t  addr, u_int32_t  size) {
+        _curr_addr = addr;
+        _curr_size = size;
+    }
+
+    bool GetNextChunk(u_int32_t& chunk_addr, u_int32_t& chunk_size) {
+        if (_curr_size == 0) {
+            return false;
+        }
+
+        chunk_addr = _curr_addr;
+
+        if ( (_curr_addr               >> _log2_alignment_size) != 
+            ((_curr_addr + _curr_size) >> _log2_alignment_size)) {
+            // Next chunk crosses alignment boundary
+            chunk_size = _alignment_size - (_curr_addr & _alignment_mask);
+        } else {
+            chunk_size = _curr_size;
+        }
+
+        _curr_addr += chunk_size;
+        _curr_size -= chunk_size;
+        
+        return true;
+    }
+
+private:
+    u_int32_t _curr_addr;
+    u_int32_t _curr_size;
+    u_int32_t _log2_alignment_size;
+    u_int32_t _alignment_size;
+    u_int32_t _alignment_mask;
+};
+
+
+
 
 ////////////////////////////////////////////////////////////////////////
 //                                                                    //
@@ -580,9 +628,13 @@ private:
 ////////////////////////////////////////////////////////////////////////
 
 // Common base class for Flash and for FImage
+
+
 class FBase : public ErrMsg{
 public:
-    FBase()           {}
+    FBase(bool is_flash) :
+        _log2_chunk_size(0), 
+        _is_flash(is_flash) {}
     virtual ~FBase()  {}
 
     virtual bool open(const char *, bool)                  {return false;}
@@ -594,15 +646,65 @@ public:
     virtual u_int32_t get_sector_size()                    = 0;
     virtual u_int32_t get_size()                           = 0;
 
+    virtual u_int32_t get_dev_id()                         = 0;
+
+    bool              is_flash() {return _is_flash;};
+
+    virtual void set_address_convertor(u_int32_t log2_chunk_size, bool is_image_in_odd_chunks) {
+        _log2_chunk_size = log2_chunk_size;
+        _is_image_in_odd_chunks = is_image_in_odd_chunks;
+    }
+
     enum {
         MAX_FLASH = 4*1048576
     };
+
+protected:
+
+    // Address translation functions for ConnectX.
+    // Translate between contiguous "logical" addresses 
+
+    // If Failsafe zebra mapping is enabled:
+
+    // Result is concatenation of:
+    // The lower log2_chunk_size bits of the cons_addr
+    // The is_image_in_odd_chunk bit
+    // The remaining upper bits of the cons_addr
+
+    u_int32_t cont2phys(u_int32_t cont_addr) {
+        u_int32_t result;
+        if (_log2_chunk_size) {
+            result  = (cont_addr       & (0xffffffff >> (32 - _log2_chunk_size))) |
+                      (_is_image_in_odd_chunks       << _log2_chunk_size)         |
+                     ((cont_addr << 1) & (0xffffffff << (_log2_chunk_size + 1)));
+        } else {
+            result = cont_addr;
+        }
+        return result;
+    }
+    
+    u_int32_t phys2cont(u_int32_t phys_addr) {
+        u_int32_t result;
+        if (_log2_chunk_size) {
+            result =  (phys_addr       & (0xffffffff >> (32 - _log2_chunk_size)))  |
+                     ((phys_addr >> 1) & (0xffffffff << (     _log2_chunk_size)));
+        } else {
+            result = phys_addr;
+        }
+        return result;
+    }
+
+    bool       _is_image_in_odd_chunks;
+    u_int32_t  _log2_chunk_size;
+
+    const bool _is_flash;
+
 };
 
 // Flash image (RO)
 class FImage : public FBase {
 public:
-    FImage() : _buf(0) {}
+    FImage() :  FBase(false), _buf(0) {}
     virtual ~FImage() { close();}
 
     u_int32_t    *getBuf()      { return _buf;}
@@ -614,76 +716,24 @@ public:
 
     virtual u_int32_t get_sector_size();
     virtual u_int32_t get_size()     { return  getBufLength();}
+    virtual u_int32_t get_dev_id()   { return  0;}
 
 private:
     u_int32_t *_buf;
     u_int32_t _len;
 };
 
-// Flash access (R/W)
-
-//
-// Flash classes heirarchy:
-//
-//
-//    Flash { CmdSet (abstract) }
-//      |
-//      | 
-//      +----> ParallelFlash { CmdSetAmd, CmdSetIntel }
-//      |
-//      |
-//      |----> SerialFlash
-//                  |
-//               +--------> SpiFlash { CmdSetStSpi }
-//               |
-//               +- - - - > LpcFlash (currently not uset - not implemented)
-//
-//
-//    Flash Interface:
-//      - open
-//      - close
-//      - read
-//      - detect type (and allocate CmdSet accordingly)  
-//
-//
-//    Flash Class HAS A command set.
-//
-//    CmdSet Interface:
-//      - write 
-//      - erase_sector
-//
-//    Q: Why is diferentiation needed for both flash type and CmdSet ?
-//    A: Read operation is done in the same way for all flash devices of
-//       the same type (serial or parallel). This is a basic requirement 
-//       from the flash, since the HCA HW reads from flash at boot, and 
-//       the way it reads can not be changed. 
-//       However, Write and Erase operations protocol varies between flash 
-//       vendors.
-//       The term 'command set' is taken from the Common Flash Interface (CFI)
-//       specification.
-//
-//
-//    Flash Allocation flow:
-//
-//    1. Main checks device type, and allocates Flash sub class accordingly.
-//    2. In Flash.open() , get_cmd_set() is called. this function checks flash
-//       type, gets flash attributes (size, sector size) and allocates CmdSet.
-//
-//       
-
-
-
 // 
-//
+// Flash access (R/W)
 //
 class Flash : public FBase {
 public:
-    Flash(u_int32_t log2_bank_size) :  
-    _mf(0), 
-    _cmd_set(NULL), 
-    _curr_bank(0xffffffff),
-    _log2_bank_size(log2_bank_size) 
-    {}
+    Flash() :
+        FBase(true),
+        _mfl(0),
+        _curr_sector(0xffffffff),
+        _port_num(0)
+     {}
 
     virtual ~Flash()  { close();};
 
@@ -696,2298 +746,381 @@ public:
     virtual void close         ();
 
     virtual bool read          (u_int32_t addr, 
-                                u_int32_t *data)            = 0;
+                                u_int32_t *data);
 
     virtual bool read          (u_int32_t addr, 
                                 void*     data, 
                                 int       len, 
                                 bool      verbose = false);
+
+    bool         cr_write      (u_int32_t addr,
+                                u_int32_t data) {return mf_cr_write(_mfl, addr, data) == MFE_OK;}
+
     //
     // Flash Interface
     //
 
-    virtual u_int32_t 
-     get_sector_size        () { return _get_sector_size(); }
+    u_int32_t get_sector_size        ()  {return _attr.sector_size; } 
+    u_int32_t get_size               ()  {return _attr.size;}
     
-    virtual u_int32_t 
-       get_size               ()                           {return _cfi_data.device_size ? _cfi_data.device_size : (u_int32_t)MAX_FLASH;}
-
-
-    virtual bool wait_ready    (const char* msg = NULL)     = 0;
+    u_int32_t get_dev_id             ()  {return _attr.hw_dev_id;}
 
+    u_int32_t get_port_num           ()  {return _port_num;}
 
     // Write and Erase functions are performed by the Command Set
 
-    virtual bool erase_sector  (u_int32_t addr)             {if (!set_bank(addr)) return false;return _cmd_set->erase_sector(addr);}
+    virtual bool erase_sector  (u_int32_t addr);
 
     virtual bool write         (u_int32_t addr, 
                                 void*     data, 
                                 int       cnt,
-                                bool      noerase = false, 
-                                bool      noverify = false);
+                                bool      noerase = false);
 
     virtual bool write         (u_int32_t addr, 
                                 u_int32_t data);
 
-    bool         print_cfi_info();
-
+    bool         print_attr();
+    bool         print_attr_old_format();
 
     enum {
         TRANS = 4096
     };
 
+    static bool _byte_mode;
+
 #ifndef _MSC_VER
 protected:
 #endif
 
-    // FLASH constants
-    enum FlashConstant {
-        FLASH_CMD_CNT  = 5000,    // Number of reads till flash cmd is zeroed
-        ERASE_DELAY    = 200000,  // Delay between reads when wating for sector erase
-        ERASE_CNT      = 80,      // Maximal number of reads when wating for sector erase
-        READ_CNT_FAST  = 5000,    // Number of fast reads after write byte
-        READ_CNT_SLOW  = 50,      // Number of slow reads after write byte
-        READ_DELAY     = 100000,  // Delay between slow reads after write byte
-        WR_REPORT_FAST = 256,     // Report frequency when write (fast interfaces)
-        WR_REPORT_SLOW = 4,       // Report frequency when write (slow interfaces)
-        RD_REPORT_FAST = 4096,    // Report frequency when read (fast interfaces)
-        RD_REPORT_SLOW = 64,      // Report frequency when read (slow interfaces)
-        GPIO_SEM_TRIES = 10240,   // Number of tries to obtain a GPIO sem.
-        MAX_WRITE_BUFFER_SIZE = 32// Max buffer size for buffer write devices
-    };
+    mflash*    _mfl;
+    flash_attr _attr;
 
+    u_int32_t  _curr_sector;
+    u_int32_t  _port_num;
+};
 
-    enum CrAddres {
-        GPIO_DIR_L     = 0xf008c, 
-        GPIO_POL_L     = 0xf0094,
-        GPIO_MOD_L     = 0xf009c,
-        GPIO_DAT_L     = 0xf0084,
-        GPIO_DATACLEAR_L = 0xf00d4, 
-        GPIO_DATASET_L   = 0xf00dc,
 
-        SEMAP63        = 0xf03fc
-    };
+////////////////////////////////////////////////////////////////////////
+//
+// FImage Class Implementation
+//
+////////////////////////////////////////////////////////////////////////
+
+bool FImage::open(const char *fname, bool read_only)
+{
+    int                fsize;
+    int                r_cnt;
+    FILE              *fh;
 
-    virtual bool lock           (bool retry=true);
-    virtual bool unlock         ();
+    read_only = true;  // FImage can be opened only for read
 
-    virtual bool init_gpios    ()                           = 0;
+    fh = fopen(fname, "rb");
 
-    virtual bool get_cmd_set   ()                           = 0;
+    if (!fh) {
+        return errmsg("Can not open file \"%s\" - %s\n", fname, strerror(errno));
+    }
 
-    bool set_bank      (u_int32_t addr);
+    // Get the file size:
+    if (fseek(fh, 0, SEEK_END) < 0) {
+        return errmsg("Can not get file size for \"%s\" - %s\n", fname, strerror(errno));
+    }
 
-    virtual bool write_internal(u_int32_t addr, 
-                                u_int8_t  data)             = 0;
+    fsize = ftell(fh);
+    if (fsize < 0) {
+        return errmsg("Can not get file size for \"%s\" - %s\n", fname, strerror(errno));
+    }
+    rewind(fh);
 
-    bool         write_internal(u_int32_t addr, 
-                                u_int8_t* data, 
-                                u_int32_t cnt);
+    //printf("-D- %s size is %d\n", fname, fsize);
+    if (fsize & 0x3) {
+        return errmsg("Image size should be 4-bytes aligned. Make sure file %s is in the right format (binary image)",
+                     fname);
+    }
 
-    class CmdSet {
-    public:
-        CmdSet                     () : _curr_sector(0xffffffff) {}
-        virtual ~CmdSet            ()                 {}
+    _buf = new u_int32_t[fsize/4];
+    if ((r_cnt = fread(_buf, 1, fsize, fh)) != fsize) {
+        if (r_cnt < 0)
+            return errmsg("Read error on file \"%s\" - %s\n",fname, strerror(errno));
+        else
+            return errmsg("Read error on file \"%s\" - read only %d bytes (from %ld)\n",
+                         fname, r_cnt, (unsigned long)fsize);
+    }
 
-        virtual bool write         (u_int32_t addr, 
-                                    void*     data, 
-                                    int       cnt,
-                                    bool      noerase  = false, 
-                                    bool      noverify = false)  = 0;
+    _len = fsize;
+    fclose(fh);
 
-        virtual bool erase_sector  (u_int32_t addr)              = 0;
+    return true;
+} // FImage::open
 
-        virtual bool reset         ()                            = 0;
+////////////////////////////////////////////////////////////////////////
+void FImage::close()
+{
+    delete [] _buf;
+    _buf = 0;
+} // FImage::close
 
-    protected:
+////////////////////////////////////////////////////////////////////////
+bool FImage::read(u_int32_t addr, u_int32_t *data)
+{
+    return read(addr, data, 4);
+} // FImage::read
 
-        u_int32_t    _curr_sector;
-    };
+////////////////////////////////////////////////////////////////////////
+bool FImage::read(u_int32_t addr, void *data, int len, bool)
+{
 
+    if (addr & 0x3) {
+        return errmsg("Address should be 4-bytes aligned.");
+    }
+    if (len & 0x3) {
+        return errmsg("Length should be 4-bytes aligned.");
+    }
+    if (!_buf) {
+       return errmsg("read() when not opened");
+    }
 
-    //         
-    // This structure holds all CFI query information as defined
-    // in the JEDEC standard. All information up to 
-    // primary_extended_query is standard among all amnufactures
-    // with CFI enabled devices.
-    //
+    if (cont2phys(addr + len) > _len) {
+        return errmsg("Reading 0x%x bytes from %saddress 0x%x is out of image limits (0x%x bytes)", 
+                     len,
+                      _log2_chunk_size ? "physical " : "",
+                      addr, 
+                      _len);
+    }
 
-    struct cfi_query {
-       cfi_query() {
-           memset(this, 0, sizeof(*this));
-       }
-        u_int8_t  manuf_id;
-        u_int8_t  device_id;
-
-        char      query_string[4];      // Should be 'QRY' 
-        u_int16_t oem_command_set;              // Command set 
-        u_int16_t primary_table_address;        // Addy of entended table 
-        u_int16_t alt_command_set;              // Alt table 
-        u_int16_t alt_table_address;            // Alt table addy 
-        float     vcc_min;                      // Vcc minimum 
-        float     vcc_max;          // Vcc maximum 
-        float     vpp_min;          // Vpp minimum, if supported 
-        float     vpp_max;          // Vpp maximum, if supported 
-        int       timeout_single_write;         // Time of single write 
-        int       timeout_buffer_write;         // Time of buffer write 
-        int       timeout_block_erase;          // Time of sector erase 
-        int       timeout_chip_erase;       // Time of chip erase 
-        int       max_timeout_single_write;     // Max time of single write 
-        int       max_timeout_buffer_write;     // Max time of buffer write 
-        int       max_timeout_block_erase;  // Max time of sector erase 
-        int       max_timeout_chip_erase;   // Max time of chip erase 
-        long      device_size;              // Device size in bytes 
-        u_int16_t interface_description;    // Interface description 
-        int       max_multi_byte_write;         // Time of multi-byte write 
-        int       num_erase_blocks;     // Number of sector defs. 
-        struct {
-            unsigned long sector_size;          // Byte size of sector 
-            int           num_sectors;      // Num sectors of this size 
-            u_int32_t     sector_mask;          // Sector mask 
-        } erase_block[8];               // Max of 256, but 8 is good 
-
-        // AMD SPECIFIC
-        char primary_extended_query[4];         // Vendor specific info here 
-        u_int16_t major_version;        // Major code version 
-        u_int16_t minor_version;        // Minor code version 
-        u_int8_t  sensitive_unlock;     // Is byte sensitive unlock? 
-        u_int8_t  erase_suspend;        // Capable of erase suspend? 
-        u_int8_t  sector_protect;       // Can Sector protect? 
-        u_int8_t  sector_temp_unprotect;    // Can we temporarily unprotect? 
-        u_int8_t  protect_scheme;       // Scheme of unprotection 
-        u_int8_t  is_simultaneous;      // Is a smulataneous part? 
-        u_int8_t  is_burst;         // Is a burst mode part? 
-        u_int8_t  is_page;          // Is a page mode part? 
-    };
+    u_int32_t chunk_addr;
+    u_int32_t chunk_size;    
+    Aligner align(_log2_chunk_size);
+    align.Init (addr, len);
+    while (align.GetNextChunk(chunk_addr, chunk_size)) {
+        u_int32_t phys_addr = cont2phys(chunk_addr);
 
-    bool print_cfi_info ( const cfi_query *q );
+        memcpy((u_int8_t*)data + (chunk_addr - addr), 
+               (u_int8_t*)_buf +  phys_addr, 
+                chunk_size);
+    }
+
+    return true;
+} // FImage::read
 
+////////////////////////////////////////////////////////////////////////
+u_int32_t FImage::get_sector_size() 
+{   
+    u_int32_t log2_sector_sz_ptr;
+    u_int32_t log2_sector_sz;
+    u_int32_t signature;
 
-    virtual bool set_bank_int  (u_int32_t bank)              = 0;
-    u_int32_t    bank_mask     ()                 {return((1 << _log2_bank_size) -1 );}
+    read(0x24, &signature);
+    TOCPU1(signature);
+    if (signature == SIGNATURE) {
+        // full image:
+        read(0x14, &log2_sector_sz_ptr);
+        TOCPU1(log2_sector_sz_ptr);
+        log2_sector_sz_ptr &= 0xffff;
 
-    mfile        *_mf;
+        read(0x30 + log2_sector_sz_ptr, &log2_sector_sz);
+        TOCPU1(log2_sector_sz);
+        log2_sector_sz &= 0xffff;
 
-    cfi_query    _cfi_data;
-    CmdSet*      _cmd_set;
+        return(1 << log2_sector_sz);
 
-    u_int32_t    _curr_bank;
-    u_int32_t    _log2_bank_size;
-    
-    bool         _locked;
-
-
-    /* Work around for MX flashes reporting weird erase sector size. */
-    /* It reports two sector sizes, actually works as 1. */
-    bool _mx_flash_workaround() {
-           return (_cfi_data.num_erase_blocks == 2 &&
-               //_cfi_data.manuf_id == 0xff && _cfi_data.device_id == 0xff &&
-               _cfi_data.erase_block[0].sector_size == 0x2000 &&
-               _cfi_data.erase_block[0].sector_mask == 0xffffe000 &&
-               _cfi_data.erase_block[0].num_sectors == 8 &&
-               _cfi_data.erase_block[1].sector_size == 0x10000 &&
-               _cfi_data.erase_block[1].sector_mask == 0xffff0000 &&
-               _cfi_data.erase_block[1].num_sectors == 63);
-    }
-    u_int32_t
-    _get_sector_mask        ()
-    {
-           return _mx_flash_workaround()?
-                   _cfi_data.erase_block[1].sector_mask :
-                   _cfi_data.erase_block[0].sector_mask;
-    }
-    u_int32_t
-    _get_sector_size        ()
-    {
-           return _mx_flash_workaround()?
-                   _cfi_data.erase_block[1].sector_size :
-                   _cfi_data.erase_block[0].sector_size;
+    } else {
+        return 0;
     }
-};
+}
 
 
-class ParallelFlash : public Flash {
-public:
-    ParallelFlash();
-    ~ParallelFlash() {close();}
 
-    enum {
-        CS_INTEL = 0x01,
-        CS_AMD   = 0x02,
-    };
+////////////////////////////////////////////////////////////////////////
+//
+// Flash Class Implementation
+//
+////////////////////////////////////////////////////////////////////////
 
-    // FBase Interface
 
-//    virtual bool open          (const char *device,
-//                             bool read_only = false);
+bool Flash::_byte_mode = false;
 
-    virtual void close         ();
+////////////////////////////////////////////////////////////////////////
+bool Flash::open(const char *device, bool force_lock, bool read_only)
+{
+    // Open device
+    int rc;
+    read_only = false;
 
+    rc = mf_open(&_mfl, device);
 
-    virtual bool read          (u_int32_t addr, 
-                                u_int32_t *data);
+    if ((rc == MFE_SEM_LOCKED) && force_lock) {
+        report("Warning: Taking flash lock even though semaphore is set.\n");
+        rc = mf_open_ignore_lock(_mfl);
+    }
 
-    virtual bool read          (u_int32_t addr, 
-                                void*     data, 
-                                int       len, 
-                                bool      verbose=false) {return Flash::read(addr, data, len, verbose);}
+    if (rc == MFE_SEM_LOCKED) {
+        return errmsg("Can not obtain Flash semaphore (63). You can run \"flint -clear_semaphore -d <device>\" to force semaphore unlock. See help for details.");
+    } else 
+        if (rc != MFE_OK) {
+        return errmsg("%s %s", errno == 0 ? "" : strerror(errno), mf_err2str(rc));
+    }
 
-    virtual bool wait_ready    (const char* msg = NULL);
+    rc = mf_get_attr(_mfl, &_attr);
+    if (rc != MFE_OK) {
+        return errmsg("Failed getting flash attributes for device %s: %s", device,  mf_err2str(rc));
+    }  
 
-    static  void set_byte_mode (bool mode) {CmdSetAmd::set_byte_mode(mode);}
+    if (_attr.hw_dev_id == 25204 || _attr.hw_dev_id == 24204) {
+        _port_num = 1;
+    } else {
+        _port_num = 2;
+    }
 
+    if (_byte_mode) {
+        rc = mf_set_opt(_mfl, MFO_AMD_BYTE_MODE, 1); 
+        if (rc != MFE_OK) {
+            return errmsg("Failed setting byte mode fore device %s: %s", device,  mf_err2str(rc));
+        }
+    }
 
-#ifndef _MSC_VER
-protected:
-#endif
+    return true;
+} // Flash::open
 
-    virtual bool init_gpios    ();
+////////////////////////////////////////////////////////////////////////
+void Flash::close()
+{
+    if (!_mfl)
+        return;
 
-    virtual bool get_cmd_set   ();
+    mf_close(_mfl); 
+    _mfl = 0;
+} // Flash::close
 
-    virtual bool set_bank_int  (u_int32_t bank);
 
-    virtual bool write_internal(u_int32_t addr, 
-                                u_int8_t  data);
+bool Flash::read(u_int32_t addr, 
+                 u_int32_t *data) {
+    int rc;
 
-    enum FlashCmds {
-        IDLE           = 0, 
-        READ4          = (1<<29), 
-        WRITE1         = (2<<29) 
-    };
-
-    enum {
-        BANK_SHIFT     = 19,     
-        BANK_MASK      = 0xfff80000 
-    };                 
-
-    enum {
-        FLASH          =    0xf01a4, 
-        ADDR_MSK       =    0x7ffffUL, 
-        CMD_MASK       = 0xe0000000UL 
-    };
-
-    enum {
-        LEN_MSK        = 0x3ff, 
-        LEN_SHIFT      = 19
-    };
-
-    enum {
-        CPUMODE_MSK    = 0xc0000000UL, 
-        CPUMODE_SHIFT  = 30 
-    };
-
-    enum CrAddres {
-        CPUMODE        = 0xf0150
-    };
-
-
-    //
-    // AMD's Am29LV033C command set
-    //
-    class CmdSetAmd : public Flash::CmdSet {
-    public:
-        CmdSetAmd                  (ParallelFlash& f  ) : _f(f) {}
-
-        virtual bool write         (u_int32_t addr, 
-                                    void*     data, 
-                                    int       cnt,
-                                    bool      noerase  = false, 
-                                    bool      noverify = false);
-
-        virtual bool erase_sector  (u_int32_t addr);
-        virtual bool reset         ();
-
-        static  void set_byte_mode (bool mode) {_byte_mode = mode;}
-
-    protected:
-        ParallelFlash& _f;
-
-        static bool  _byte_mode;       
-
-        bool         unlock_bypass (bool      unlock);
-    };
-
-
-    //
-    // Intel's 28F320J3 x8 command set, using buffer writes
-    //
-    class CmdSetIntel : public Flash::CmdSet {
-    public:
-        enum FlashCommand {
-            FC_ReadID      = 0x90,
-            FC_Read        = 0xFF,
-            FC_Erase       = 0x20,
-            FC_Confirm     = 0xD0,
-            FC_Clear       = 0x50,
-            FC_Write       = 0x40,
-            FC_LoadPB      = 0xE0,
-            FC_PBWrite     = 0x0C,
-            FC_Status      = 0x70,
-            FC_Suspend     = 0xB0,
-            FC_Resume      = 0xD0,
-            FC_ReadESR     = 0x71,
-            FC_QueryCFI    = 0x98,
-            FC_SCSErase    = 0x28,
-            FC_SCSWrite    = 0xE8
-        };
-
-        enum FlashStatus {
-            FS_Ready       = 0x80,
-            FS_Suspended   = 0x40,
-            FS_Error       = 0x3E,
-            FS_BlockError  = 0x3F
-        };
-
-        CmdSetIntel                (ParallelFlash& f  ) : _f(f) {}
-
-        virtual bool write         (u_int32_t addr, 
-                                    void*     data, 
-                                    int       cnt,
-                                    bool      noerase  = false, 
-                                    bool      noverify = false);
-
-        virtual bool erase_sector  (u_int32_t addr);
-        virtual bool reset         ();
-
-    protected:
-        ParallelFlash& _f;
-    };
-
-    //
-    // Intel's 28F320J3 x8 command set, using byte write (For debug only).
-    //
-    class CmdSetIntelWriteByte : public CmdSetIntel {
-    public:
-        CmdSetIntelWriteByte       (ParallelFlash& f) : CmdSetIntel(f) {}
-
-        virtual bool write         (u_int32_t addr, 
-                                    void*     data, 
-                                    int       cnt,
-                                    bool      noerase  = false, 
-                                    bool      noverify = false);
-
-    };
-
-    bool get_cfi                (cfi_query *query);
-
-    u_int16_t extract_word   ( const u_int8_t* pb, int data_width)  {
-        assert (data_width == 1 || data_width == 2);
-        u_int16_t ret = *pb | ((*(pb + data_width)) << 8);
-        return ret;
-    }
-
-    u_int32_t     USE_SCR;
-    bool         _use_scr;
-
-    // Place holders to keep GPIO data for restoring after closing flash.
-    u_int32_t    _dir;
-    u_int32_t    _pol;
-    u_int32_t    _mod;
-    u_int32_t    _data;
-
-};
-
-
-class SerialFlash : public Flash {
-public:
-    SerialFlash() : Flash(20) {}
-
-    enum CrAddres {
-        FLASH_GW     = 0xf0400,
-        FLASH_ADDR   = 0xf0404,
-        FLASH_DATA   = 0xf0408,
-        FLASH_CS     = 0xf0418,
-
-        GPIO_LOCK    = 0xf00ec
-    };
-
-    enum BitOffset {
-        READ_OP      = 0,
-        ADDR_INCR    = 1,
-
-        LPC_STOP     = 3,
-        SPI_NO_DATA  = 4,
-        SPI_NO_ADDR  = 5,
-        SPI_SPECIAL  = 6,
-
-        MSIZE_S      = 8,
-        MSIZE_E      = 10,
-
-        STATUS_S     = 26,
-        STATUS_E     = 29,
-
-        BUSY         = 30,
-
-        SPI_ADDR_S   = 0,
-        SPI_ADDR_E   = 23,
-
-        SPI_CMD_S    = 24,
-        SPI_CMD_E    = 31,
-
-        SPI_GPIO_S   = 5,
-        SPI_GPIO_E   = 7
-    };
-
-
-protected:
-
-    virtual bool write_internal(u_int32_t addr, 
-                                u_int8_t  data);
-
-};
-
-bool SerialFlash::write_internal (u_int32_t addr, 
-                                  u_int8_t  data)  {addr = 0; data = 0; return true;}
-
-
-class SpiFlash : public SerialFlash {
-public:
-    SpiFlash() {}
-    ~SpiFlash() {close();}
-
-
-    // FBase Interface
-
-    virtual void close         ();
-
-    virtual bool read          (u_int32_t addr, 
-                                u_int32_t *data);
-
-    virtual bool read          (u_int32_t addr, 
-                                void*     data, 
-                                int       len, 
-                                bool      verbose=false) {return Flash::read(addr, data, len, verbose);}
-
-    virtual bool wait_ready    (const char* msg);
-
-protected:
-
-    virtual bool init_gpios    ();
-
-    virtual bool get_cmd_set   ();
-
-    virtual bool set_bank_int  (u_int32_t bank);
-
-    
-    virtual bool read_id       (u_int8_t *data, u_int8_t cmd);
-
-
-    //
-    // ST's M25P80 command set
-    //
-    class CmdSetStSpi : public Flash::CmdSet {
-    public:
-        CmdSetStSpi                (SpiFlash& f  ) : _f(f) , _mf(f._mf){}
-
-        virtual bool write         (u_int32_t addr, 
-                                    void*     data, 
-                                    int       cnt,
-                                    bool      noerase  = false, 
-                                    bool      noverify = false);
-
-        virtual bool erase_sector  (u_int32_t addr);
-
-        virtual bool reset         ()                {return true;}
-
-        enum FlashCommand {
-            FC_SE    = 0xD8,
-            FC_PP    = 0x02,
-            FC_RDSR  = 0x05,
-            FC_WREN  = 0x06,
-            FC_READ  = 0x03,
-            FC_RDID  = 0xF9,
-            FC_RES   = 0xAB
-        }; 
-
-
-    protected:
-        bool         write_block   (u_int32_t block_addr, 
-                                    void*     block_data, 
-                                    u_int32_t block_size);
-
-        bool         wait_wip      (u_int32_t delay, 
-                                    u_int32_t retrys, 
-                                    u_int32_t fast_retry = 0);
-
-        bool         write_enable  ();
-
-
-        SpiFlash& _f;
-        mfile*    _mf;
-
-    };
-
-    bool fill_cfi                  (cfi_query *query);
-
-};
-
-
-////////////////////////////////////////////////////////////////////////
-//
-// FImage Class Implementation
-//
-////////////////////////////////////////////////////////////////////////
-
-bool FImage::open(const char *fname, bool read_only)
-{
-    int                fsize;
-    int                r_cnt;
-    FILE              *fh;
-
-    read_only = true;  // FImage can be opened only for read
-
-    fh = fopen(fname, "rb");
-
-    if (!fh) {
-        return errmsg("Can't open file \"%s\" - %s\n", fname, strerror(errno));
-    }
-
-    // Get the file size:
-    if (fseek(fh, 0, SEEK_END) < 0) {
-        return errmsg("Can't get file size for \"%s\" - %s\n", fname, strerror(errno));
-    }
-
-    fsize = ftell(fh);
-    if (fsize < 0) {
-        return errmsg("Can't get file size for \"%s\" - %s\n", fname, strerror(errno));
-    }
-    rewind(fh);
-
-    //printf("-D- %s size is %d\n", fname, fsize);
-    if (fsize & 0x3) {
-        return errmsg("Image size should be 4-bytes aligned. Make sure file %s is in the right format (binary image)",
-                     fname);
-    }
-
-    _buf = new u_int32_t[fsize/4];
-    if ((r_cnt = fread(_buf, 1, fsize, fh)) != fsize) {
-        if (r_cnt < 0)
-            return errmsg("Read error on file \"%s\" - %s\n",fname, strerror(errno));
-        else
-            return errmsg("Read error on file \"%s\" - read only %d bytes (from %ld)\n",
-                         fname, r_cnt, (unsigned long)fsize);
-    }
-
-    _len = fsize;
-    fclose(fh);
-
-    return true;
-} // FImage::open
-
-////////////////////////////////////////////////////////////////////////
-void FImage::close()
-{
-    delete [] _buf;
-    _buf = 0;
-} // FImage::close
-
-////////////////////////////////////////////////////////////////////////
-bool FImage::read(u_int32_t addr, u_int32_t *data)
-{
-    return read(addr, data, 4);
-} // FImage::read
-
-////////////////////////////////////////////////////////////////////////
-bool FImage::read(u_int32_t addr, void *data, int len, bool)
-{
-    if (addr & 0x3) {
-        return errmsg("Address should be 4-bytes aligned.");
-    }
-    if (len & 0x3) {
-        return errmsg("Length should be 4-bytes aligned.");
-    }
-    if (!_buf) {
-       return errmsg("read() when not opened");
-    }
-
-    if (addr + len > _len) {
-        return errmsg("Reading 0x%x bytes from address 0x%x is out of image limits (0x%x bytes)", 
-                     len, addr, _len);
-    }
-
-    u_int32_t *p = (u_int32_t *)data;
-    for (int i=0; i<len/4; i++) {
-       p[i] = _buf[addr/4];
-        addr += 4;
-    }
-    return true;
-} // FImage::read
-
-////////////////////////////////////////////////////////////////////////
-u_int32_t FImage::get_sector_size() 
-{   
-    u_int32_t log2_sector_sz_ptr;
-    u_int32_t log2_sector_sz;
-    u_int32_t signature;
-
-    read(0x24, &signature);
-    TOCPU1(signature);
-    if (signature == SIGNATURE) {
-        // full image:
-        read(0x14, &log2_sector_sz_ptr);
-        TOCPU1(log2_sector_sz_ptr);
-        log2_sector_sz_ptr &= 0xffff;
-
-        read(0x30 + log2_sector_sz_ptr, &log2_sector_sz);
-        TOCPU1(log2_sector_sz);
-        log2_sector_sz &= 0xffff;
-
-        return(1 << log2_sector_sz);
-
-    } else {
-        return 0;
-    }
-}
-
-
-
-////////////////////////////////////////////////////////////////////////
-//
-// Flash Class Implementation
-//
-////////////////////////////////////////////////////////////////////////
-
-
-////////////////////////////////////////////////////////////////////////
-bool Flash::open(const char *device, bool force_lock, bool read_only)
-{
-
-    // Open device
-    _mf = mopen(device);
-    if (!_mf) {
-        return errmsg("Can't open device %s: %s", device, strerror(errno));
-    }
-
-    _locked = lock();
-
-    if (!_locked) {
-       if (force_lock) {
-           report("Warning: Taking flash lock even though semaphore is set.\n");
-           _locked = true;
-       } else { 
-           return false;
-       }
-    }
-
-    if (!init_gpios())
-        return false;
-
-    if (!read_only) {
-        if (!get_cmd_set())
-            return false;
-
-        // Reset flash
-        return _cmd_set->reset(); 
-    }
-
-    return true;
-} // Flash::open
-
-////////////////////////////////////////////////////////////////////////
-void Flash::close()
-{
-    if (!_mf)
-        return;
-
-    delete _cmd_set;
-
-    // ??? Check if unlock should be before delete _cmd_set
-    if (_locked) {
-       unlock();
-    }
-
-    mclose(_mf); 
-    _mf = 0;
-
-} // Flash::close
-
-
-bool Flash::lock(bool retry) {
-
-    retry = false; // compiler - REMOVE ???
-
-    // Obtain GPIO Semaphore
-    u_int32_t cnt=0;
-    u_int32_t word;
-    do {
-        if (++cnt > GPIO_SEM_TRIES) {
-            return errmsg("Can not obtain Flash semaphore (63). You can run \"flint -clear_semaphore -d <device>\" to force semaphore unlock. See help for details.");
-        }
-        MREAD4(SEMAP63, &word);
-    } // while (word);
-      // HACK! : On win 64, the semaphore reg, when read through pci (mmap), does not appear
-      //         as 0x00000000 when cleared, but as 0xffffff00 , or 0xffff00ff . Could not find the 
-      //         reason for that. 
-      //         The hack is to treat anything which is NOT 0xffffffff or 0x1 as a free semaphore.
-      //         Though ugly, this is safe.
-      while (word == 0xffffffff || word == 0x00000001);
-
-    return true;
-}
-
-
-bool Flash::unlock() {
-
-    // Free GPIO Semaphore
-    mwrite4(_mf, SEMAP63, 0);
-    return true;
-}
-
-
-////////////////////////////////////////////////////////////////////////
-bool Flash::read(u_int32_t addr, void *data, int len, bool verbose)
-{
-    u_int32_t  perc = 0xffffffff;
-
-    if (addr & 0x3) {
-        return errmsg("Address should be 4-bytes aligned.");
-    }
-    if (len & 0x3) {
-        return errmsg("Length should be 4-bytes aligned.");
-    }
-
-    // Report
-    if (verbose) {
-        printf("000%%");
-        fflush(stdout);
-    }
-
-    u_int32_t *p = (u_int32_t *)data;
-    for (int i=0; i<len/4; i++) {
-        if (!read(addr, p++))
-            return false;
-        addr += 4;
-
-        // Report
-        if (verbose) {
-            u_int32_t new_perc = (i * 100) / len;
-            if (new_perc != perc) {
-                printf("\b\b\b\b%03d%%", new_perc);
-                fflush(stdout);
-                perc = new_perc;
-            }
-        }
-    }
-
-    // Report
-    if (verbose) {
-        printf("\b\b\b\b100%%");
-        fflush(stdout);
-    }
-
-    return true;
-} // Flash::read
-
-
-////////////////////////////////////////////////////////////////////////
-bool Flash::write         (u_int32_t addr, 
-                   void*     data, 
-                   int       cnt,
-                   bool      noerase,
-                   bool      noverify)
-{
-    if (addr + cnt > get_size()) {
-       return errmsg( 
-           "Trying to write %d bytes to address 0x%x, which exceeds flash size (0x%x).",
-           cnt, 
-           addr,
-           get_size());  
-    }
-    
-    return _cmd_set->write(addr, data, cnt, noerase, noverify);
-}
-
-
-////////////////////////////////////////////////////////////////////////
-bool Flash::write(u_int32_t addr, u_int32_t data)
-{
-    if (!_mf) {
-       return errmsg("Not opened");
-    }
-    if (addr & 0x3) {
-        return errmsg("Address should be 4-bytes aligned.");
-    }
-
-    // Here, we use non-virtual variants for efficiency
-    // TODO: Rewrite using get_sector_size() only
-    u_int32_t word;
-    u_int32_t sector_mask = _get_sector_mask();
-    u_int32_t sector_size = _get_sector_size();
-
-    u_int32_t sector = addr & sector_mask;
-    u_int32_t word_in_sector = (addr & ~sector_mask)/sizeof(u_int32_t);
-
-    if (!read(addr, &word))
-        return false;
-    if (word == data)
-        return true;   // already there
-
-    vector<u_int32_t> buff(sector_size/sizeof(u_int32_t));
-    if (!read(sector, &buff[0] , sector_size))
-        return false;
-    buff[word_in_sector] = data;
-    return write(sector, &buff[0], sector_size);
-} // Flash::write
-
-////////////////////////////////////////////////////////////////////////
-
-inline
-bool Flash::set_bank      (u_int32_t addr) {
-    u_int32_t bank = (addr >> _log2_bank_size);
-
-    if (bank != _curr_bank ) {
-        if (!set_bank_int(bank))
-            return false;
-
-        _curr_bank = bank;
-    }
-
-    return true;
-}
-
-
-// A smaple function which loops through and prints out the data
-// contained in the CFI query structure.  Should ONLY be called
-// after init_flash 
-
-bool Flash::print_cfi_info()
-{
-
-    const cfi_query *q = &_cfi_data;
-
-    int i=0;
-    printf("\n");
-
-    char* head_fmt = "%-50s ";
-
-    printf(head_fmt, "CFI Query String Read:");
-    printf("[%s]\n", (char *) q->query_string);
-
-    printf(head_fmt, "Primary table address at offset:");
-    printf("[0x%2x] hex.\n", q->primary_table_address);
-
-    printf(head_fmt, "Manufacturer ID, Device ID:");
-    printf("[0x%02x,0x%02x] hex.\n", q->manuf_id, q->device_id );
-
-    printf(head_fmt, "Command set:");
-    printf("[0x%04x] hex.\n", q->oem_command_set);
-
-    printf(head_fmt, "Write buffer:");
-    printf("[%d] bytes\n", q->max_multi_byte_write );
-
-    printf("\n----Voltage and Signal Timing Parameters-------------------\n");
-    printf(head_fmt, "Vcc operating voltage:");
-    printf("[%2.3f] to [%2.3f] Volts\n", q->vcc_min, q->vcc_max);
-
-
-    printf(head_fmt, "Vpp operating voltage:");
-    if (q->vpp_min == 0.0)
-        printf("Device does not support Vpp voltage.\n");
-    else {
-        printf("[%2.3f] to [%2.3f] Volts\n", q->vpp_min, q->vpp_max);
-    }
-
-    printf(head_fmt, "Typical timeout for single write (micro-sec):");
-    printf("[%8i]us\n", q->timeout_single_write);
-
-
-
-    printf(head_fmt,"Typical timeout for single write (micro-sec):");
-    if (q->timeout_buffer_write == 0x00)
-        printf("Buffer writes not supported in this device.\n");
-    else {
-        printf("[%8i]us\n" ,q->timeout_buffer_write);
-    }
-
-    printf(head_fmt, "Typical timeout for block erase (milli-sec):", q->timeout_block_erase);
-    printf("[%8i]ms\n", q->timeout_block_erase);
-
-    printf(head_fmt, "Typical timeout for chip erase (milli-sec):");
-    if (q->timeout_chip_erase == 0x00)
-        printf("Not supported in this device.\n");
-    else {
-        printf("[%8i]ms\n", q->timeout_single_write);
-    }
-
-    printf(head_fmt, "Maximum timeout for single write (micro-sec) :");
-    printf("[%8i]us\n", q->max_timeout_single_write);
-
-    printf(head_fmt, "Maximum timeout for buffer write (micro-sec) :");
-    if (q->max_timeout_buffer_write == 0x00)
-        printf("Not supported in this device.\n");
-    else {
-        printf("[%8i]us\n", q->max_timeout_buffer_write);
-    }
-
-    printf(head_fmt, "Maximum timeout for block erase (milli-sec) :");
-    printf("[%8i]ms\n", q->max_timeout_block_erase);
-
-    printf(head_fmt, "Maximum timeout for chip erase (milli-sec) :");
-    if (q->max_timeout_chip_erase == 0x00)
-        printf("Not supported in this device.\n");
-    else {
-        printf("[%8i]ms\n", q->max_timeout_chip_erase);
-    }
-
-
-
-
-    printf("\n----Sector Organization Parameters-------------------\n\n");
-
-    printf(head_fmt, "Device size:");
-    printf("[%8li] bytes, or [%2i] Mbit\n",
-           q->device_size, 
-           (int) (q->device_size/((long)0x20000)));
-
-    printf(head_fmt, "Number of erase block regions:");
-    printf("%d\n",q->num_erase_blocks);
-
-    for (i=0; i<q->num_erase_blocks; i++) {
-        printf("  Size:[%8lx] bytes, Mask [%08x], [Number:[%4i]\n",
-               q->erase_block[i].sector_size,
-               q->erase_block[i].sector_mask,
-               q->erase_block[i].num_sectors);
-    }
-
-    printf("\n----Primary Vendor-Specific Extended Parameters----\n\n");
-
-    printf(head_fmt, "CFI Extended String Read:");
-    printf("[%s]\n", (char *) q->primary_extended_query);
-
-    printf(head_fmt, "Major version:", q->major_version);
-    printf("[%3x]\n", q->major_version);
-
-    printf(head_fmt, "Minor version:", q->minor_version);
-    printf("[%3x]\n", q->minor_version);
-
-    printf(head_fmt, "Sensitive Unlock:", q->sensitive_unlock);
-    printf("[%3x]\n", q->sensitive_unlock);
-
-    printf(head_fmt, "Erase Suspend:", q->erase_suspend);
-    printf("[%3x]\n", q->erase_suspend);
-
-    printf(head_fmt, "Sector Protect:", q->sector_protect);
-    printf("[%3x]\n", q->sector_protect);
-
-    printf(head_fmt, "Temporary Sector Unprotect:", q->sector_temp_unprotect);
-    printf("[%3x]\n", q->sector_temp_unprotect);
-
-    printf(head_fmt, "Protection Scheme:", q->protect_scheme);
-    printf("[%3x]\n", q->protect_scheme);
-
-    printf(head_fmt, "Is simultaneous? :", q->is_simultaneous);
-    printf("[%3x]\n", q->is_simultaneous);
-
-    printf(head_fmt, "Is Burst capable? :", q->is_burst);
-    printf("[%3x]\n", q->is_burst);
-
-    printf(head_fmt, "Is Page capable? :", q->is_page);
-    printf("[%3x]\n", q->is_page);
-
-    printf("Done.\n\n");
-
-    return true;
-} // Flash::print_cfi_info()
-
-
-
-////////////////////////////////////////////////////////////////////////
-//
-// ParallelFlash Class Implementation
-//
-////////////////////////////////////////////////////////////////////////
-
-ParallelFlash::ParallelFlash() : Flash(19)
-{
-    char *use_scr_p = getenv("FLINT_USE_SCRATCHPAD");
-
-    if (use_scr_p) {
-        char *endp;
-
-        USE_SCR = strtoul(use_scr_p, &endp, 0);
-        if (*endp) {
-            printf("Invalid FLINT_USE_SCRATCHPAD syntax (%s). Must be integer.",
-                   use_scr_p);
-            _use_scr = false;
-        } else {
-            printf("Burning via SCRATCHPAD interface by addr 0x%x\n", USE_SCR);
-            _use_scr = true;
-        }
-    } else
-        _use_scr = false;
-} // Flash::Flash
-
-
-
-////////////////////////////////////////////////////////////////////////
-bool ParallelFlash::init_gpios() {
-
-    // Save old values
-    MREAD4(GPIO_DIR_L, &_dir);
-    MREAD4(GPIO_POL_L, &_pol);
-    MREAD4(GPIO_MOD_L, &_mod);
-    MREAD4(GPIO_DAT_L, &_data);
-
-    // Set Direction=1, Polarity=0, Mode=0 for 3 GPIO lower bits
-    u_int32_t dir = _dir | 0x70;
-    u_int32_t pol = _pol & ~0x70;
-    u_int32_t mod = _mod & ~0x70;
-    MWRITE4(GPIO_DIR_L, dir);
-    MWRITE4(GPIO_POL_L, pol);
-    MWRITE4(GPIO_MOD_L, mod);
-
-    // Set CPUMODE
-    u_int32_t word;
-    MREAD4(CPUMODE, &word);
-    word &= ~CPUMODE_MSK;
-    word |= 1 << CPUMODE_SHIFT;
-    MWRITE4(CPUMODE, word);
-
-    return true;
-}
-
-void ParallelFlash::close() {
-
-    // Restore origin values
-    mwrite4(_mf, GPIO_DIR_L, _dir);
-    mwrite4(_mf, GPIO_POL_L, _pol);
-    mwrite4(_mf, GPIO_MOD_L, _mod);
-    mwrite4(_mf, GPIO_DAT_L, _data);
-
-    _curr_bank = 0xffffffff;
-
-    Flash::close();
-}
-
-
-bool ParallelFlash::get_cmd_set() {
-
-    //
-    // CFI Query
-    //
-    if (!get_cfi(&_cfi_data))
-        return false;
-
-    //
-    // Some sanity checks:
-    //
-
-    if (_cfi_data.max_multi_byte_write > MAX_WRITE_BUFFER_SIZE) {
-        return errmsg("Device write buffer(%d) is larger than the supported size(%d).", 
-                     _cfi_data.max_multi_byte_write, MAX_WRITE_BUFFER_SIZE);
-    }
-
-    if (!_mx_flash_workaround() && _cfi_data.num_erase_blocks > 1) {
-       return errmsg("Device has more than one sector size - not supported by this tool");
-    }
-
-    //
-    // Sellect CmdSet
-    //
-
-    switch (_cfi_data.oem_command_set) {
-    case CS_INTEL:
-        if (_byte_write || _cfi_data.max_multi_byte_write == 0)
-            _cmd_set = new CmdSetIntelWriteByte(*this);
-        else
-            _cmd_set = new CmdSetIntel(*this);
-        break;
-    case CS_AMD: 
-        _cmd_set =     new CmdSetAmd(*this);
-        break;
-
-    default:
-        return errmsg("Unknown CFI command set (%d)",_cfi_data.oem_command_set) ;
-    }
-
-    return true;
-}
-
-
-
-////////////////////////////////////////////////////////////////////////
-bool ParallelFlash::set_bank_int(u_int32_t bank)
-{
-    if (!_mf) {
-        return errmsg("Not opened");
-    }
-
-    //printf("\n*** Flash::set_bank(0x%lx) : 0x%lx\n", bank, (bank >> 19) & 0x07);
-    MWRITE4(GPIO_DATACLEAR_L, 0x70);
-    MWRITE4(GPIO_DATASET_L, (bank << 4) & 0x70);
-
-    return true;
-} // Flash::ParallelFlashGw::set_bank_int
-
-
-////////////////////////////////////////////////////////////////////////
-bool ParallelFlash::wait_ready(const char* msg) {
-    u_int32_t cnt = 0;
-    u_int32_t cmd;
-    do {
-        // Timeout checks
-        if (++cnt > FLASH_CMD_CNT) {
-            return errmsg("Flash gateway timeout: %s", msg);
-        }
-
-        MREAD4(FLASH, &cmd);
-
-    } while (cmd & CMD_MASK);
-
-    return true;
-}
-
-////////////////////////////////////////////////////////////////////////
-bool ParallelFlash::read(u_int32_t addr, u_int32_t *data)
-{
-    if (!_mf) {
-        return errmsg("Not opened");
-    }
-
-    u_int32_t cmd;
-    if (addr & 0x3) {
-        return errmsg("Address should be 4-bytes aligned.");
-    }
-
-    if (!set_bank(addr))
-        return false;
-
-
-    MWRITE4(FLASH, READ4 | (addr & ADDR_MSK));
-
-    if (!wait_ready("Read"))
-        return false;
-
-    MREAD4(FLASH+4, &cmd);
-    cmd = __cpu_to_be32(cmd);
-    memcpy(data, &cmd, sizeof(u_int32_t));
-
-    return true;
-} // Flash::read
-
-////////////////////////////////////////////////////////////////////////
-bool ParallelFlash::write_internal(u_int32_t addr, u_int8_t data)
-{
-    MWRITE4(FLASH+4, data << 24);
-    MWRITE4(FLASH, WRITE1 | (addr & ADDR_MSK));
-
-    if (!wait_ready("Write"))
-        return false;
-
-    return true;
-} // Flash::write_internal
-
-
-////////////////////////////////////////////////////////////////////////
-bool ParallelFlash::CmdSetAmd::unlock_bypass(bool unlock) {
-
-    if (unlock) {
-        // unlock bypass
-
-        if (!_f.write_internal(0x555, 0xaa))
-            return false;
-        if (!_f.write_internal(0x2aa, 0x55))
-            return false;
-        if (!_f.write_internal(0x555, 0x20))
-            return false;
-    } else {
-        // unlock reset
-        if (!_f.write_internal(0x555, 0x90))
-            return false;
-        if (!_f.write_internal(0x2aa, 0x00))
-            return false;
-    }
-    return true;
-}
-
-
-////////////////////////////////////////////////////////////////////////
-
-bool ParallelFlash::CmdSetAmd::_byte_mode = false;
-
-bool ParallelFlash::CmdSetAmd::write(u_int32_t addr, void *data, int cnt,
-                                     bool noerase, bool noverify)
-{
-    if (!_f._mf) {
-        return _f.errmsg("Not opened");
-    }
-    if (addr & 0x3) {
-        return _f.errmsg("Address should be 4-bytes aligned.");
-    }
-
-    char         *p = (char *)data;
-
-    if (_unlock_bypass) {
-       if (!unlock_bypass(true)) {
-           return _f.errmsg("Failed unlock bypass"); 
-       }
-    }
-
-    for (int i=0; i<cnt; i++,addr++) {
-        u_int32_t  word;
-        u_int8_t   act, exp;
-        int        cnt1 = 0;
-
-        if (!_f.set_bank(addr))
-            return false;
-
-        if (!noerase) {
-            u_int32_t sector = (addr / _f.get_sector_size()) * _f.get_sector_size();
-            if (sector != _curr_sector) {
-                _curr_sector = sector;
-
-               // If we're in unlock mode, we must re-lock before erase sector
-               if (_unlock_bypass) {
-                   if (!unlock_bypass(false)) {
-                       return _f.errmsg("Failed re-lock bypass");
-                   }
-               }
-
-
-                if (!erase_sector(_curr_sector))
-                    return false;
-
-
-               if (_unlock_bypass) {
-                   if (!unlock_bypass(true)) {
-                       return _f.errmsg("Failed unlock bypass");
-                   }
-               }
-            }
-        }
-
-        if (_no_burn)
-            continue;
-
-        if (_f._use_scr) {
-
-            //
-            // Special burning interface via Scratchpad
-            // ----------------------------------------
-            //
-            u_int32_t cmd;
-            u_int32_t data = (*p++ & 0xff) << 24;
-            data |= (*p++ & 0xff) << 16;
-            data |= (*p++ & 0xff) << 8;
-            data |= (*p++ & 0xff);
-            if (mwrite4(_f._mf, _f.USE_SCR+4 , data) != 4) return false;
-            if (mwrite4(_f._mf, _f.USE_SCR ,   WRITE1 | (3 << LEN_SHIFT) | (addr & ADDR_MSK)) != 4) return false;
-            do {
-                int  loop_cnt = 0;
-                // Timeout checks
-                if (++loop_cnt > FLASH_CMD_CNT) {
-                    return _f.errmsg("Use scratchpad: CMD doesn't become zero");
-                }
-                if (mread4(_f._mf, _f.USE_SCR , &cmd) != 4) return false;
-
-            } while (cmd & CMD_MASK);
-            i += 3;
-            addr += 3;
-        } else if ((u_int8_t)(*p) != 0xff) {
-
-            if (_byte_mode) {
-            
-                if (!_f.write_internal(0xaaa, 0xaa))
-                    return false;
-                if (!_f.write_internal(0x555, 0x55))
-                    return false;
-                if (!_f.write_internal(0xaaa, 0xa0))
-                    return false;
-            } else {
-                if (!_unlock_bypass) {
-                    if (!_f.write_internal(0x555, 0xaa))
-                        return false;
-                    if (!_f.write_internal(0x2aa, 0x55))
-                        return false;
-                }
-
-                if (!_f.write_internal(0x555, 0xa0))
-                    return false;
-            }
-
-            if (!_f.write_internal(addr, *p++))
-                return false;
-
-            do {
-                // Timeout checks
-                if (++cnt1 > READ_CNT_FAST)
-                    usleep(READ_DELAY);
-                if (cnt1 > READ_CNT_FAST + READ_CNT_SLOW) {
-                    return _f.errmsg("Flash write error - read value didn't stabilize.");
-                    return false;
-                }
-
-                if (!_f.read(addr & ~3, &word))
-                    return false;
-
-                word = __be32_to_cpu(word);
-                act = (u_int8_t) ((word >> ((3 - (addr & 3)) * 8)) & 0xff);
-                exp = *(p-1) & 0xff;
-                //if (act != exp)
-                //    printf("write: %08x - exp:%02x act:%02x /%08x/\n",
-                //           addr, exp & 0xff, act & 0xff, word);
-            } while (!noverify && act != exp);      
-
-        } else {
-            p++;
-        }
-    } 
-
-    if (_unlock_bypass) {
-        if (!unlock_bypass(false)) {
-            return _f.errmsg("Failed re-lock bypass");
-        }
-    }
-
-    return true;
-} // flash_write
-
-
-
-bool ParallelFlash::CmdSetIntelWriteByte::write(u_int32_t addr, void *data, int cnt,
-                                                bool noerase, bool noverify)
-{
-    if (!_f._mf) {
-        return _f.errmsg("Not opened");
-    }
-    if (addr & 0x3) {
-        return _f.errmsg("Address should be 4-bytes aligned.");
-    }
-
-    char         *p = (char *)data;
-
-    for (int i=0; i<cnt; i++,addr++) {
-        u_int32_t  status;
-        u_int8_t   act, exp;
-        int        cnt1 = 0;
-
-        if (!_f.set_bank(addr))
-            return false;
-
-        if (!noerase) {
-            u_int32_t sector = (addr / _f.get_sector_size()) * _f.get_sector_size();
-            if (sector != _curr_sector) {
-                _curr_sector = sector;
-                if (!erase_sector(_curr_sector))
-                    return false;
-            }
-        }
-
-        if (_no_burn)
-            continue;
-
-        if ((u_int8_t)(*p) != 0xff) {
-            // Write byte
-            if (!_f.write_internal(addr, FC_Write))
-                return false;
-
-            if (!_f.write_internal(addr, *p++))
-                return false;
-
-            do {
-                // Timeout checks
-                if (++cnt1 > READ_CNT_FAST)
-                    usleep(READ_DELAY);
-                if (cnt1 > READ_CNT_FAST + READ_CNT_SLOW) {
-                    return _f.errmsg("Flash write error - timeout waiting for ready after write.");
-                }
-
-                // TODO - Move to read single for Arbel
-                if (!_f.read(addr & ~3, &status))
-                    return false;
-
-                //if (act != exp)
-                //    printf("write: %08x - exp:%02x act:%02x /%08x/\n",
-                //           addr, exp & 0xff, act & 0xff, word);
-            } while ((status & FS_Ready) == 0);
-
-            if (status & FS_Error) {
-                return _f.errmsg("Flash write error - error staus detected.");
-            }
-
-            if (!noverify) {
-                u_int32_t word;
-                if (!reset())
-                    return false;
-                // TODO - Move to read single for Arbel
-                if (!_f.read(addr & ~3, &word))
-                    return false;
-
-                word = __be32_to_cpu(word);
-                act = (u_int8_t) ((word >> ((3 - (addr & 3)) * 8)) & 0xff);
-                exp = *(p-1) & 0xff;
-
-                if (act != exp) {
-                    printf("write: %08x - exp:%02x act:%02x /%08x/\n",
-                           addr, exp & 0xff, act & 0xff, word);
-
-                    return _f.errmsg("Write verification failed");
-                }
-            }
-
-        } else {
-            p++;
-        }
-    }
-
-    if (!reset())
-        return false;
-
-    return true;
-} // flash_write
-
-///////////////////////////////////////////////////////////////////////////
-
-//
-// Use the buffer write capability.
-//
-bool ParallelFlash::CmdSetIntel::write(u_int32_t addr, void *data, int cnt,
-                                       bool noerase, bool noverify)
-{
-    if (!_f._mf) {
-        return _f.errmsg("Not opened");
-    }
-    if (addr & 0x3) {
-        return _f.errmsg("Address should be 4-bytes aligned.");
-    }
-
-    u_int8_t         *p = (u_int8_t *)data;
-
-    u_int32_t block_size = _f._cfi_data.max_multi_byte_write;
-    u_int32_t block_mask = ~(block_size - 1 );
-
-    // TODO - Check MAX_WRITE_BUFFER_SIZE against block_size in open (or here)
-    u_int8_t  tmp_buff[MAX_WRITE_BUFFER_SIZE];
-
-    while (cnt) {
-
-        u_int32_t prefix_pad_size = 0;
-        u_int32_t suffix_pad_size = 0;
-
-        u_int32_t block_addr = addr & block_mask;
-        u_int32_t data_size  = block_size;
-
-        u_int8_t* write_data = p;
-
-
-        //
-        // First and last cycles (can be the same one) may not be block aligned.
-        // Check the status, and copy data to a padded temp bufer if not alligned.
-        // (there's an option to write partial buffer, but Intel reference code always
-        // writes full buffer, with pads if needed. I do the dame ...)
-        //
-
-        prefix_pad_size = addr - block_addr;
-
-        if ((addr & block_mask) == ((addr + cnt) & block_mask)) {
-            suffix_pad_size = block_size - ((addr + cnt) % block_size);
-        }
-
-        if (suffix_pad_size || prefix_pad_size) {
-            memset(tmp_buff, 0xff, block_size);
-
-            data_size -= prefix_pad_size;
-            data_size -= suffix_pad_size;
-
-            memcpy(tmp_buff + prefix_pad_size, p , data_size);
-
-            write_data = tmp_buff;
-        }
-
-        int        cnt1 = 0;
-
-        //
-        // Bank setup.
-        //
-        if (!_f.set_bank(addr))
-            return false;
-
-        if (!noerase) {
-            u_int32_t sector = (addr / _f.get_sector_size()) * _f.get_sector_size();
-            if (sector != _curr_sector) {
-                _curr_sector = sector;
-                if (!erase_sector(_curr_sector))
-                    return false;
-            }
-        }
-
-        if (_no_burn)
-            continue;
-
-        //
-        // Check to see if there's something to do
-        //
-        bool all_ffs = true;
-        for (u_int32_t i = 0; i < block_size ; i++) {
-            if (write_data[i] != 0xff) {
-                all_ffs = false;
-                break;
-            }
-        }
-
-        if (!all_ffs) {
-
-            u_int32_t status;
-            cnt1 = 0;
-            do {
-                // Get Write buffer
-                if (!_f.write_internal(block_addr, FC_SCSWrite))
-                    return false;
-
-                if (cnt1 > ((READ_CNT_FAST + READ_CNT_SLOW) * 4)) {
-                    //printf("-D- status = %08x\n", status);
-                    reset();
-                    return _f.errmsg("Flash write error - Write buffer not ready.");  
-                }
-
-                cnt1++;
-
-                if (!_f.read(block_addr, &status))
-                    return false;
-
-            } while (!(status & FS_Ready));
-
-            if (status & FS_Error) {
-                return _f.errmsg("Flash write error - Error getting write buffer");
-            }
-
-            // word count (allways full buffer, coded as cull buffer size -1)
-            if (!_f.write_internal(block_addr, block_size - 1))
-                return false;
-
-            // Write data to buffer
-            for (u_int32_t i = 0; i < block_size ; i++ ) {
-                if (!_f.write_internal(block_addr + i, write_data[i]))
-                    return false;
-            }
-
-            // write confirm
-            if (!_f.write_internal(block_addr, FC_Confirm))
-                return false;
-
-            cnt1 = 0;
-            do {
-                // Timeout checks
-                if (++cnt1 > READ_CNT_FAST)
-                    usleep(READ_DELAY);
-                if (cnt1 > READ_CNT_FAST + READ_CNT_SLOW) {
-                    reset();
-                    return _f.errmsg("Flash write error - Write buffer status timeout");
-                }
-
-                // TODO - Move to read single for Arbel
-                if (!_f.read(block_addr, &status))
-                    return false;
-
-                //if (act != exp)
-                //    printf("write: %08x - exp:%02x act:%02x /%08x/\n",
-                //           addr, exp & 0xff, act & 0xff, word);
-            } while ((status & 0x80) == 0);
-
-            //
-            // TODO: Status checks.
-            //
-
-            if (!noverify) {
-                u_int8_t verify_buffer[MAX_WRITE_BUFFER_SIZE];
-                if (!reset())
-                    return false;
-
-                if (!_f.read(addr, verify_buffer, data_size))
-                    return false;
-
-                for (u_int32_t i = 0 ; i < data_size ; i++) {
-                    if (verify_buffer[i] != write_data[i + prefix_pad_size]) {
-                       return _f.errmsg(
-                           "Write verification failed. Addr: %08x - exp:%02x act:%02x",
-                           addr + i, 
-                           write_data[i + prefix_pad_size] , 
-                           verify_buffer[i]);   
-                    }
-                }  
-            }
-        }
-
-
-        //
-        // loop advance
-        //
-
-        addr += data_size;
-        p    += data_size;
-        cnt  -= data_size;
-    }
-
-    if (!reset())
-        return false;
-
-    return true;
-} // flash_write
-
-////////////////////////////////////////////////////////////////////////
-bool ParallelFlash::CmdSetAmd::reset() {
-    if (!_f.write_internal(0x555, 0xf0)) {
-       return _f.errmsg("Device reset failed: %s", _f.err());
-    }
-    return true;
-}
-
-////////////////////////////////////////////////////////////////////////
-bool ParallelFlash::CmdSetAmd::erase_sector(u_int32_t addr)
-{
-    u_int32_t word = 0;
-    int       cnt  = 0;
-
-    if (_no_erase)
-        return true;
-
-    // Just to insure zeroes because erase completion waits for ones
-    if (!write(addr, &word, sizeof(word), true))
-        return false;
-
-    // erase sector sequence
-    if (_byte_mode ) {
-    
-        if (!_f.write_internal(0xaaa, 0xaa))
-            return false;
-        if (!_f.write_internal(0x555, 0x55))
-            return false;
-        if (!_f.write_internal(0xaaa, 0x80))
-            return false;
-        if (!_f.write_internal(0xaaa, 0xaa))
-            return false;
-        if (!_f.write_internal(0x555, 0x55))
-            return false;
-        if (!_f.write_internal(addr, 0x30))
-            return false;
-    } else {
-    
-        if (!_f.write_internal(0x555, 0xaa))
-            return false;
-        if (!_f.write_internal(0x2aa, 0x55))
-            return false;
-        if (!_f.write_internal(0x555, 0x80))
-            return false;
-        if (!_f.write_internal(0x555, 0xaa))
-            return false;
-        if (!_f.write_internal(0x2aa, 0x55))
-            return false;
-        if (!_f.write_internal(addr, 0x30))
-            return false;
-
-    }
-
-    // Wait while erase completes
-    do {
-        // Timeout checks
-        if (++cnt > ERASE_CNT) {
-            return _f.errmsg("Flash erase sector timeout");
-        }
-        if (!_f.read(addr, &word))
-            return false;
-
-        //printf("erase_sector: addr:%08lx, %08x\n", addr, word);
-        usleep(ERASE_DELAY);
-    } while (word != 0xffffffff);
-
-
-    return true;
-} // Flash::erase_sector
-
-////////////////////////////////////////////////////////////////////////
-
-bool ParallelFlash::CmdSetIntel::reset() {
-    if (!_f.write_internal(0x555, FC_Read)) {
-        return _f.errmsg("Device reset failed");
-    }
-    return true;
-}
-
-////////////////////////////////////////////////////////////////////////
-bool ParallelFlash::CmdSetIntel::erase_sector(u_int32_t addr)
-{
-    u_int32_t    status = 0;
-    int          cnt    = 0;
-
-    if (_no_erase)
-        return true;
-
-    // Just to insure zeroes because erase completion waits for ones
-    //if (!write(addr, &word, sizeof(word), true))
-    //    return false;
-
-    // Erase command
-    if (!_f.write_internal(addr, FC_Erase))
-        return false;
-
-    // Erase confirm
-    if (!_f.write_internal(addr, FC_Confirm))
-        return false;
-
-    usleep(ERASE_DELAY);
-
-    // Wait while erase completes
-    do {
-        // Timeout checks
-        if (++cnt > ERASE_CNT) {
-            return _f.errmsg("Flash erase sector timeout");
-        }
-        if (!_f.read(addr, &status))
-            return false;
-
-        //printf("CmdSetIntel::erase_sector: addr:%08lx, %08x\n", addr, word);
-        usleep(ERASE_DELAY);
-    } while ((status & FS_Ready) == 0);
-
-    if (status & FS_Error) {
-        return _f.errmsg("Status register detected erase error (0x%x)", status & FS_Error);
-    }
-
-    // Reset
-    if (!reset())
-        return false;
-
-    return true;
-} // ParallelFlash::CmdSetIntel::erase_sector
-
-//*******************************************************************
-// flash_get_cfi() is the main CFI workhorse function.  Due to it's  
-// complexity and size it need only be called once upon              
-// initializing the flash system.  Once it is called, all operations 
-// are performed by looking at the cfi_query structure.                
-// All possible care was made to make this algorithm as efficient as 
-// possible.  90% of all operations are memory reads, and all        
-// calculations are done using bit-shifts when possible              
-//*******************************************************************
-
-bool ParallelFlash::get_cfi(struct cfi_query *query)
-{
-
-    enum {
-        TOTAL_QUERY_SIZE    = 1024,
-        EXTENDED_QUERY_SIZE =   12 
-    };
-
-    u_int8_t fwp[TOTAL_QUERY_SIZE]; // flash window 
-
-    int volts=0, milli=0, temp=0, i=0;
-    int offset=0;
-
-    u_int32_t query_base = 0x10;
-
-    // Initial house-cleaning 
-    memset(fwp, 0xff, TOTAL_QUERY_SIZE);
-
-    for (i=0; i < 8; i++) {
-        query->erase_block[i].sector_size = 0;
-        query->erase_block[i].num_sectors = 0;
-    }   query->erase_block[i].sector_mask = 0;
-
-    // reset
-    if (!write_internal(0x55, 0xff))
-        return false;
-
-    // CFI QUERY 
-    if (!write_internal(0x55, 0x98))
-        return false;
-
-    char query_str_x8[4];
-    char query_str_x16asx8[8];
-
-    if (!read(0x10, query_str_x8, 0x4))
-        return false;
-
-    if (!read(0x20, query_str_x16asx8, 0x8))
-        return false;
-
-    query_str_x8[3]       = '\0';
-    query_str_x16asx8[7]  = '\0';
-
-    if ( strncmp( query_str_x8 ,      "QRY" ,3 ) == 0) {
-        // x8 CFI flash (AMD)
-        query_base = 1;
-    } else if ( query_str_x16asx8[0] == 'Q' && 
-                query_str_x16asx8[2] == 'R' &&
-                query_str_x16asx8[4] == 'Y') {
-        // x16 CFI flash worqing in x8 mode
-        query_base = 2;
-    } else {
-
-        printf(" Received CFI query from addr 0x10: [%s]\n", query_str_x8 );
-        printf(" Received CFI query from addr 0x20: [%s]\n", query_str_x16asx8);
-
-        return errmsg("Failed CFI query");
-    } 
-
-    if (!read(0x0, fwp, 0x4)) // Dev ID
-        return false;
-
-    if (!read(query_base * 0x10, fwp + query_base * 0x10, query_base * 0x20))
-        return false;
-
-    query->manuf_id              = fwp[query_base * 0];
-    query->device_id             = fwp[query_base * 1];
-
-    query->query_string[0]       = fwp[query_base * 0x10];
-    query->query_string[1]       = fwp[query_base * 0x11];
-    query->query_string[2]       = fwp[query_base * 0x12];
-    query->query_string[3]       = '\0';
-
-    query->oem_command_set       = extract_word(fwp + query_base * 0x13, query_base);
-    query->primary_table_address = extract_word(fwp + query_base * 0x15, query_base); // Important one! 
-    query->alt_command_set       = extract_word(fwp + query_base * 0x17, query_base);
-    query->alt_table_address     = extract_word(fwp + query_base * 0x19, query_base);
-
-    // We will do some bit translation to give the following values
-    // numerical meaning in terms of C 'float' numbers 
-
-    volts = ((fwp[query_base * 0x1B] & 0xF0) >> 4);
-    milli = ( fwp[query_base * 0x1B] & 0x0F);
-    query->vcc_min = (float) (volts + ((float)milli/10));
-
-    volts = ((fwp[query_base * 0x1C] & 0xF0) >> 4);
-    milli = ( fwp[query_base * 0x1C] & 0x0F);
-    query->vcc_max = (float) (volts + ((float)milli/10));
-
-    volts = ((fwp[query_base * 0x1D] & 0xF0) >> 4);
-    milli = ( fwp[query_base * 0x1D] & 0x0F);
-    query->vpp_min = (float) (volts + ((float)milli/10));
-
-    volts = ((fwp[query_base * 0x1E] & 0xF0) >> 4);
-    milli = ( fwp[query_base * 0x1E] & 0x0F);
-    query->vpp_max = (float) (volts + ((float)milli/10));
-
-    // Let's not drag in the libm library to calculate powers
-    // for something as simple as 2^(power)
-    // Use a bit shift instead - it's faster 
-
-    temp = fwp[query_base * 0x1F];
-    query->timeout_single_write = (1 << temp);
-
-    temp = fwp[query_base * 0x20];
-    if (temp != 0x00)
-        query->timeout_buffer_write = (1 << temp);
-    else
-        query->timeout_buffer_write = 0x00;
-
-    temp = 0;
-    temp = fwp[query_base * 0x21];
-    query->timeout_block_erase = (1 << temp);
-
-    temp = fwp[query_base * 0x22];
-    if (temp != 0x00)
-        query->timeout_chip_erase = (1 << temp);
-    else
-        query->timeout_chip_erase = 0x00;
-
-    temp = fwp[query_base * 0x23];
-    query->max_timeout_single_write = (1 << temp) *
-                                      query->timeout_single_write;
-
-    temp = fwp[query_base * 0x24];
-    if (temp != 0x00)
-        query->max_timeout_buffer_write = (1 << temp) *
-                                          query->timeout_buffer_write;
-    else
-        query->max_timeout_buffer_write = 0x00;
-
-    temp = fwp[query_base * 0x25];
-    query->max_timeout_block_erase = (1 << temp) *
-                                     query->timeout_block_erase;
-
-    temp = fwp[query_base * 0x26];
-    if (temp != 0x00)
-        query->max_timeout_chip_erase = (1 << temp) *
-                                        query->timeout_chip_erase;
-    else
-        query->max_timeout_chip_erase = 0x00;
-
-    temp = fwp[query_base * 0x27];
-    query->device_size = (long) (((long)1) << temp);
-
-    query->interface_description = extract_word(fwp + query_base * 0x28, query_base);
-
-    temp = fwp[query_base * 0x2A];
-    if (temp != 0x00)
-        query->max_multi_byte_write = (1 << temp);
-    else
-        query->max_multi_byte_write = 0;
-
-    query->num_erase_blocks = fwp[query_base * 0x2C];
-
-    if (!read(query_base * 0x2C, fwp + query_base * 0x2C ,query_base * 4 * (query->num_erase_blocks + 1)))
-        return false;
-
-    for (i=0; i < query->num_erase_blocks; i++) {
-        query->erase_block[i].num_sectors = extract_word(fwp + query_base * (0x2D+(4*i)), query_base);
-        query->erase_block[i].num_sectors++;
-
-        query->erase_block[i].sector_size = (long) 256 *
-                                            ( (long)256 * fwp[(query_base * (0x30+(4*i)))] +
-                                              fwp[(query_base * (0x2F+(4*i)))] );
-
-        query->erase_block[i].sector_mask = ~(query->erase_block[i].sector_size - 1);
-    }
-
-    // Store primary table offset in variable for clarity 
-    offset = query->primary_table_address;
-
-    if ((offset + EXTENDED_QUERY_SIZE) * query_base > TOTAL_QUERY_SIZE) {
-        return errmsg("Primary extended query larger than TOTAL_QUERY_SIZE (%d)",TOTAL_QUERY_SIZE) ;
-    }
-
-
-    // DEBUG: 
-    //printf("Raw Cfi query:\n");
-    //printf("  0123456789abcdef_123456789abcdef_123456789abcdef\n  ");
-    //for (u_int32_t i = 0x10 * query_base ; i <= (0x30 * query_base); i+= query_base) {
-    //    printf("%02x",  fwp[i]);
-    //}
-    //printf("\n");
-
-    u_int32_t dw_aligned_offs = (((offset * query_base) >> 2 ) << 2);
-
-    if (!read(dw_aligned_offs , fwp + dw_aligned_offs  , EXTENDED_QUERY_SIZE * query_base))
-        return false;
-
-    query->primary_extended_query[0] = fwp[query_base * (offset)];
-    query->primary_extended_query[1] = fwp[query_base * (offset + 1)];
-    query->primary_extended_query[2] = fwp[query_base * (offset + 2)];
-    query->primary_extended_query[3] = '\0';
-
-    if ( query->primary_extended_query[0] != 'P' &&
-         query->primary_extended_query[1] != 'R' &&
-         query->primary_extended_query[2] != 'I') {
-        return errmsg("Bad primary table address in CFI query");
-    }
-
-    query->major_version = fwp[query_base * (offset + 3)];
-    query->minor_version = fwp[query_base * (offset + 4)];
-
-    query->sensitive_unlock      = (u_int8_t) (fwp[query_base * (offset+5)] & 0x0F);
-    query->erase_suspend         = (u_int8_t) (fwp[query_base * (offset+6)] & 0x0F);
-    query->sector_protect        = (u_int8_t) (fwp[query_base * (offset+7)] & 0x0F);
-    query->sector_temp_unprotect = (u_int8_t) (fwp[query_base * (offset+8)] & 0x0F);
-    query->protect_scheme        = (u_int8_t) (fwp[query_base * (offset+9)] & 0x0F);
-    query->is_simultaneous       = (u_int8_t) (fwp[query_base * (offset+10)] & 0x0F);
-    query->is_burst              = (u_int8_t) (fwp[query_base * (offset+11)] & 0x0F);
-    query->is_page               = (u_int8_t) (fwp[query_base * (offset+12)] & 0x0F);
-
-    return true;
-} 
-
-
-
-////////////////////////////////////////////////////////////////////////
-//
-// SpiFlash Class Implementation
-//
-////////////////////////////////////////////////////////////////////////
-
-bool SpiFlash::init_gpios() {
-
-    //
-    // Set Multi SPI CS to output and 0.
-    // Assuming 4 flashes. If there are less than 4 flashes and there's
-    // a write attempt, it will fail.
-    //
-
-    u_int32_t       num_of_spis = 4;
-    u_int32_t       spi_en      = (1 << (num_of_spis - 1 ) ) -1;
-
-    u_int32_ba      dir;
-    u_int32_ba      mod; 
-    u_int32_ba      pol;
-
-    // No need to set the data - SPI GW CS does that in HW
-    //MREAD4(GPIO_DAT_L, &data);
-
-    MREAD4(GPIO_DIR_L, &dir);
-    MREAD4(GPIO_POL_L, &pol);
-    MREAD4(GPIO_MOD_L, &mod);
-
-    dir.range (SPI_GPIO_E, SPI_GPIO_S) = spi_en;
-    pol.range (SPI_GPIO_E, SPI_GPIO_S) = ~spi_en;
-    mod.range(SPI_GPIO_E, SPI_GPIO_S) = ~spi_en;
-
-    // unlock gpio
-    MWRITE4(GPIO_LOCK , 0xaaaa);
-
-    MWRITE4(GPIO_DIR_L, dir); 
-    MWRITE4(GPIO_POL_L, pol); 
-    MWRITE4(GPIO_MOD_L, mod); 
+    u_int32_t phys_addr = cont2phys(addr);
+    rc = mf_read(_mfl, phys_addr, 4, (u_int8_t*)data);
+    if (rc != MFE_OK) {
+        return errmsg("Flash read failed at address %s%x : %s",
+                      _log2_chunk_size ? "physical " : "",
+                      addr,
+                      mf_err2str(rc));
+    }
 
     return true;
 }
 
-void SpiFlash::close() {
-    // Chip reset does not reset the chip sellect - Make sure after reset
-    // boot loads FW from SPI 0.
-    set_bank(0);
-
-    // unlock gpio
-    mwrite4(_mf, GPIO_LOCK , 0xaaaa);
-}
-
-bool SpiFlash::get_cmd_set   () {
-
-    //
-    // Read device ID and allocate command set accordingly.
-    //
-
-    //
-    // Initiate some CFI fields to mimic cfi query procedure of parallel flash:
-    //
-
-    _cfi_data.max_multi_byte_write       = 16;   // In SPI context, this is the transaction size. Max is 16.
-    _cfi_data.num_erase_blocks           = 1;
-    _cfi_data.erase_block[0].sector_size = 64 * 1024;
-    _cfi_data.erase_block[0].sector_mask = ~(_cfi_data.erase_block[0].sector_size - 1);
-
-    u_int32_t spi_size = 0;
-    u_int32_t num_spis = 0;
-
-    for (u_int32_t spi_sel = 0 ; spi_sel < 4 ; spi_sel++) {
-        if (!set_bank_int(spi_sel)) return false;
+////////////////////////////////////////////////////////////////////////
+bool Flash::read(u_int32_t addr, void *data, int len, bool verbose)
+{
+    int rc;
+    u_int32_t  perc = 0xffffffff;
 
-        unsigned char es; // electronic signature
-        u_int32_t cur_spi_size = 0;
+    if (addr & 0x3) {
+        return errmsg("Address should be 4-bytes aligned.");
+    }
+    if (len & 0x3) {
+        return errmsg("Length should be 4-bytes aligned.");
+    }
 
-        if (!read_id(&es, CmdSetStSpi::FC_RES)) return false;
+    // Report
+    if (verbose) {
+        printf("000%%");
+        fflush(stdout);
+    }
 
-        if (es >= 0x10 && es < 0x16) {
-            // Range OK:
+    // Much better perf for read in a single chunk. need to work on progress report though.
+    bool read_in_single_chunk = true;
 
-            // NOTE: This mapping between electronic signature and device size is device specific!
-            //       This mapping works for ST M25Pxx and Saifun SA25Fxxx families.
-            cur_spi_size = 1 << (es + 1);
+    if (read_in_single_chunk) {
+        u_int32_t chunk_addr;
+        u_int32_t chunk_size;
 
-            num_spis++;
+        Aligner align(_log2_chunk_size);
+        align.Init (addr, len);
+        while (align.GetNextChunk(chunk_addr, chunk_size)) {
+            u_int32_t phys_addr = cont2phys(chunk_addr);
 
-            if (spi_sel == 0) {
-                spi_size = cur_spi_size;
-            } else if (cur_spi_size != spi_size){
-                return errmsg("SPI flash #%d of size 0x%x bytes differs in size from SPI flash #%d of size 0x%x bytes. "
-                              "All flash devices must be of the same size.",
-                              spi_sel,
-                              cur_spi_size,
-                              spi_sel - 1,
-                              spi_size);
+            rc = mf_read(_mfl, phys_addr, chunk_size, ((u_int8_t*)data) + chunk_addr - addr);
+            if (rc != MFE_OK) {
+                return errmsg("Flash read failed at address %s%x : %s",
+                              _log2_chunk_size ? "physical " : "",
+                              chunk_addr,
+                              mf_err2str(rc));
             }
-            
-
-        } else if (es == 0xff) {
-            // No spi device on this chip_select
-            break;
-        } else {
-            return errmsg("Unexpected SPI electronic signature value (0x%2x) when detecting flash size. "
-                          "Flash #%d my be defected.",
-                          es,
-                          spi_sel);
         }
-
-        // printf("-D- %3d %08x\n", spi_sel, cur_spi_size);
-    }
-
-    _cfi_data.device_size = spi_size * num_spis;
-    _log2_bank_size       = log2up(spi_size);
-
     
-    _cmd_set = new CmdSetStSpi(*this);
-
-    return true;
-}
-
-
-bool SpiFlash::set_bank_int(u_int32_t bank)
-{
-    if (!_mf) {
-        return errmsg("Not opened");
+    } else {
+        u_int32_t *p = (u_int32_t *)data;
+        for (int i=0; i<len/4; i++) {
+            if (!read(addr, p++))
+                return false;
+            
+            addr += 4;
+
+            // Report
+            if (verbose) {
+                u_int32_t new_perc = (i * 100) / len;
+                if (new_perc != perc) {
+                    printf("\b\b\b\b%03d%%", new_perc);
+                    fflush(stdout);
+                    perc = new_perc;
+                }
+            }
+        }
     }
-
-    // TODO: Check number of banks in open!
-    if (bank > 3) {
-        return errmsg("Tried to set bank to %d but %d is the is the largest bank number", bank, 3);
+    // Report
+    if (verbose) {
+        printf("\b\b\b\b100%%");
+        fflush(stdout);
     }
 
-    //printf("\n*** Flash::set_bank(0x%lx) : 0x%lx\n", bank, (bank >> 19) & 0x07);
-    u_int32_ba flash_cs;
-    flash_cs.range(31,30) = bank;
-    MWRITE4(FLASH_CS, flash_cs);  
-
     return true;
-} // Flash::SpiFlash::set_bank
+} // Flash::read
 
 
 ////////////////////////////////////////////////////////////////////////
-bool SpiFlash::wait_ready(const char* msg)
+bool Flash::write  (u_int32_t addr, 
+                   void*     data, 
+                   int       cnt,
+                   bool      noerase)
 {
-    u_int32_ba gw_cmd = 0;
-    u_int32_t cnt = 0;
-    do {
-        // Timeout checks
-        if (++cnt > FLASH_CMD_CNT) {
-            return errmsg("Flash gateway timeout: %s.", msg);
-        }
-
-        MREAD4(FLASH_GW, &gw_cmd);
-
-    } while (gw_cmd[BUSY]);
-
-    return true;
-}
-
+    
+    // FIX:
+    noerase = _no_erase || noerase;    
 
-////////////////////////////////////////////////////////////////////////
-bool SpiFlash::read(u_int32_t addr, u_int32_t *data)
-{
-    if (!_mf) {
+    if (!_mfl) {
         return errmsg("Not opened");
     }
-
     if (addr & 0x3) {
         return errmsg("Address should be 4-bytes aligned.");
     }
 
-    if (!set_bank(addr))
-        return false;
-
-
-    //
-    // Prepare command word
-    //
-
-    u_int32_ba gw_cmd;
-    u_int32_ba gw_addr;
-
-    gw_cmd[BUSY]                   = 1;
-    gw_cmd[READ_OP]                = 1;
-    //gw_cmd[ADDR_INCR]              = 0;
-
-    gw_cmd.range(MSIZE_E, MSIZE_S) = 2;
-
-    gw_addr.range(SPI_ADDR_E, SPI_ADDR_S) = addr & bank_mask();
-
-    MWRITE4(FLASH_ADDR, gw_addr);
-    MWRITE4(FLASH_GW,   gw_cmd);
-
-    if (!wait_ready("Read"))
-        return false;
-
-    MREAD4(FLASH_DATA, data);
-
-    *data = __cpu_to_be32(*data);
-
-    return true;
-} // SpiFlash::read
-
-
-////////////////////////////////////////////////////////////////////////
-bool SpiFlash::read_id(u_int8_t *data, u_int8_t cmd)
-{
-
-    //
-    // Prepare command word
-    //
-
-    u_int32_ba gw_cmd;
-    u_int32_ba gw_addr;
-    u_int32_t flash_data;
-
-    gw_cmd[BUSY]                   = 1;
-    gw_cmd[READ_OP]                = 1;
-    gw_cmd[SPI_SPECIAL]            = 1;
-    gw_cmd[SPI_NO_ADDR]            = 1;
-
-    gw_cmd.range(MSIZE_E, MSIZE_S) = 2;
-
-    gw_addr.range(SPI_CMD_E, SPI_CMD_S) = cmd;
-
-    MWRITE4(FLASH_ADDR, gw_addr);
-    MWRITE4(FLASH_GW,   gw_cmd);
-
-    if (!wait_ready("Read id"))
-        return false;
-
-    MREAD4(FLASH_DATA, &flash_data);
-
-    /* ID is at offset 3 in word */
-    *data = (u_int8_t)(flash_data & 0xff);
-
-    //printf("-D- ES is %02x\n", *data);
-
-    return true;
-} // SpiFlash::read
-
-//
-// TODO: Unify all the block handling code with the CmdSet001 write.
-//
-
-bool SpiFlash::CmdSetStSpi::write (u_int32_t addr, 
-                                   void*     data, 
-                                   int       cnt,
-                                   bool      noerase, 
-                                   bool      noverify) {
 
-    if (!_f._mf) {
-        return _f.errmsg("Not opened");
-    }
-    if (addr & 0x3) {
-        return _f.errmsg("Address should be 4-bytes aligned.");
+    if (cont2phys(addr + cnt) > get_size()) {
+       return errmsg( 
+           "Trying to write %d bytes to address 0x%x, which exceeds max image size (0x%x - half of total flash size).",
+           cnt, 
+           addr,
+           get_size() / 2);
     }
-
+    
     u_int8_t         *p = (u_int8_t *)data;
+    u_int32_t sect_size = get_sector_size();
 
-    u_int32_t block_size = _f._cfi_data.max_multi_byte_write;
-    u_int32_t block_mask = ~(block_size - 1 );
-
-    // TODO - Check MAX_WRITE_BUFFER_SIZE against block_size in open (or here)
-    u_int8_t  tmp_buff[MAX_WRITE_BUFFER_SIZE];
-
-    while (cnt) {
-
-        u_int32_t prefix_pad_size = 0;
-        u_int32_t suffix_pad_size = 0;
-
-        u_int32_t block_addr = addr & block_mask;
-        u_int32_t data_size  = block_size;
-
-        u_int8_t* block_data = p;
-
-
-        //
-        // First and last cycles (can be the same one) may not be block aligned.
-        // Check the status, and copy data to a padded temp bufer if not alligned.
-        // (there's an option to write partial buffer, but Intel reference code always
-        // writes full buffer, with pads if needed. I do the dame ...)
-        //
-
-        prefix_pad_size = addr - block_addr;
-
-        if ((addr & block_mask) == ((addr + cnt) & block_mask)) {
-            suffix_pad_size = block_size - ((addr + cnt) % block_size);
-        }
-
-        if (suffix_pad_size || prefix_pad_size) {
-            memset(tmp_buff, 0xff, block_size);
-
-            data_size -= prefix_pad_size;
-            data_size -= suffix_pad_size;
-
-            memcpy(tmp_buff + prefix_pad_size, p , data_size);
-
-            block_data = tmp_buff;
-        }
+    u_int32_t chunk_addr;
+    u_int32_t chunk_size;
 
-        //
-        // Bank setup.
-        //
-        if (!_f.set_bank(addr))
-            return false;
+    u_int32_t first_set;
+    for (first_set = 0; ((sect_size >> first_set) & 1) == 0; first_set++ )
+        ;
 
+    Aligner align(first_set);
+    align.Init (addr, cnt);
+    while (align.GetNextChunk(chunk_addr, chunk_size)) {
+        // Write / Erase in sector_size alligned chunks
+        int rc;
+        
         if (!noerase) {
-            u_int32_t sector = (addr / _f.get_sector_size()) * _f.get_sector_size();
+            u_int32_t sector = (chunk_addr / sect_size) * sect_size;
             if (sector != _curr_sector) {
                 _curr_sector = sector;
                 if (!erase_sector(_curr_sector))
@@ -2998,222 +1131,108 @@ bool SpiFlash::CmdSetStSpi::write (u_int32_t addr,
         if (_no_burn)
             continue;
 
-        //
-        // Check to see if there's something to do
-        //
-        bool all_ffs = true;
-        for (u_int32_t i = 0; i < block_size ; i++) {
-            if (block_data[i] != 0xff) {
-                all_ffs = false;
-                break;
-            }
-        }
-
-        if (!all_ffs) {
-
-            write_block(block_addr, block_data, block_size);
-
-            if (!noverify) {
-                u_int8_t verify_buffer[MAX_WRITE_BUFFER_SIZE];
-                if (!reset())
-                    return false;
-
-                if (!_f.read(addr, verify_buffer, data_size))
-                    return false;
-
-                for (u_int32_t i = 0 ; i < data_size ; i++) {
-                    if (verify_buffer[i] != block_data[i + prefix_pad_size]) {
-                        return _f.errmsg("Write verification failed. Addr %08x - exp:%02x act:%02x\n",
-                               addr + i, 
-                               block_data[i + prefix_pad_size] , 
-                               verify_buffer[i]);
-                    }
-                }  
-            }
+        // Actual write:
+        u_int32_t phys_addr = cont2phys(chunk_addr);
+        rc = mf_write(_mfl, phys_addr, chunk_size, p);
+        if (rc != MFE_OK) {
+            return errmsg("Flash write of %d bytes to address %s0x%x failed: %s",
+                          chunk_size,
+                          _log2_chunk_size ? "physical " : "",
+                          chunk_addr,
+                          mf_err2str(rc));
         }
 
-
-        //
-        // loop advance
-        //
-
-        addr += data_size;
-        p    += data_size;
-        cnt  -= data_size;
+        // Loop advance
+        p    += chunk_size;
     }
 
-    if (!reset())
-        return false;
-
-    return true;
-} 
-
-bool SpiFlash::CmdSetStSpi::erase_sector(u_int32_t addr)
-{
-
-    if (_no_erase)
-        return true;
-
-    u_int32_ba gw_cmd;
-    u_int32_ba gw_addr;
-
-
-    if (!write_enable())
-        return false;
-
-    //
-    // Erase sector command:
-    //
-
-    gw_cmd[BUSY]                   = 1;
-    gw_cmd[SPI_SPECIAL]            = 1;
-    gw_cmd[SPI_NO_DATA]            = 1;
-
-    gw_addr.range(SPI_CMD_E,  SPI_CMD_S)  = FC_SE;
-    gw_addr.range(SPI_ADDR_E, SPI_ADDR_S) = addr & _f.bank_mask();
-
-    MWRITE4(FLASH_ADDR, gw_addr);
-    MWRITE4(FLASH_GW,   gw_cmd);
-
-    if (!_f.wait_ready("ES"))
-        return false;
-
-    //
-    // Wait for erase completion
-    //
-
-    if (!wait_wip(ERASE_DELAY, ERASE_CNT))
-        return false;
-
     return true;
-} // Flash::erase_sector
-
-
-
-bool SpiFlash::CmdSetStSpi::write_block(u_int32_t block_addr, 
-                                        void*     block_data, 
-                                        u_int32_t block_size) {
+}
 
-    u_int32_ba gw_cmd;
-    u_int32_ba gw_addr;
 
-    // sanity check ??? remove ???
-    if (block_size != (u_int32_t)_f._cfi_data.max_multi_byte_write) {
-        return _f.errmsg("Block write of wrong block size. %d instead of %d", 
-                        block_size, (u_int32_t)_f._cfi_data.max_multi_byte_write);
+////////////////////////////////////////////////////////////////////////
+bool Flash::write(u_int32_t addr, u_int32_t data)
+{
+    if (!_mfl) {
+       return errmsg("Not opened");
     }
-
-    if (!write_enable())
-        return false;
-
-    //
-    // Write the data block
-    //
-
-
-    gw_cmd[BUSY]                   = 1;
-    gw_cmd[SPI_SPECIAL]            = 1;
-
-    gw_cmd.range(MSIZE_E, MSIZE_S) = log2up(block_size);
-
-    gw_addr.range(SPI_CMD_E,  SPI_CMD_S)  = FC_PP;
-    gw_addr.range(SPI_ADDR_E, SPI_ADDR_S) = block_addr & _f.bank_mask();
-
-    MWRITE4(FLASH_ADDR, gw_addr);
-
-    // Data:
-    for (u_int32_t offs = 0 ; offs < block_size ; offs += 4) {
-        // NOTE: !!! To much swapping around the data. !!!
-        // Flash GW in sinai eats full DWords with byte0 as high data.
-        // TODO: Swap on writes in Parallel flash. Save double swapping for serial flash. 
-        u_int32_t word = *((u_int32_t*)((u_int8_t*)block_data + offs));
-        word = __be32_to_cpu(word);
-        MWRITE4(FLASH_DATA + offs, word );
+    if (addr & 0x3) {
+        return errmsg("Address should be 4-bytes aligned.");
     }
 
-    MWRITE4(FLASH_GW,   gw_cmd);
+    u_int32_t word;
 
-    if (!_f.wait_ready("PP command"))
-        return false;
+    u_int32_t sector_size = _attr.sector_size;
+    u_int32_t sector_mask = ~(sector_size - 1);
 
-    //
-    // Wait for end of write in flash (WriteInProgress = 0):
-    //
+    u_int32_t sector = addr & sector_mask;
+    u_int32_t word_in_sector = (addr & ~sector_mask)/sizeof(u_int32_t);
 
-    if (!wait_wip(READ_DELAY, READ_CNT_SLOW + READ_CNT_FAST, READ_CNT_FAST))
+    if (!read(addr, &word))
         return false;
+    if (word == data)
+        return true;   // already there
 
-    return true;
-}
-
-
-bool SpiFlash::CmdSetStSpi::write_enable() {
-
-    u_int32_ba gw_cmd;
-    u_int32_ba gw_addr;
-
-    //
-    // Write enable:
-    //
-
-    gw_cmd[BUSY]                   = 1;
-    gw_cmd[SPI_NO_ADDR]            = 1;
-    gw_cmd[SPI_NO_DATA]            = 1;
-    gw_cmd[SPI_SPECIAL]            = 1;
-
-    gw_addr.range(SPI_CMD_E,  SPI_CMD_S)  = FC_WREN;
-
-    MWRITE4(FLASH_ADDR, gw_addr);
-    MWRITE4(FLASH_GW,   gw_cmd);
-
-    if (!_f.wait_ready("WREN command"))
+    vector<u_int32_t> buff(sector_size/sizeof(u_int32_t));
+    if (!read(sector, &buff[0] , sector_size))
         return false;
+    buff[word_in_sector] = data;
+    return write(sector, &buff[0], sector_size);
+} // Flash::write
 
-    return true;
-}
-
-bool SpiFlash::CmdSetStSpi::wait_wip(u_int32_t delay, u_int32_t retrys, u_int32_t fast_retrys  ) {
-
-
-    u_int32_ba gw_cmd;
-    u_int32_ba gw_data;
-    u_int32_ba gw_addr;
-
-    u_int32_t cnt = 0;
 
-    //
-    // Read SR:
-    //
+bool Flash::erase_sector  (u_int32_t addr) {
+    int rc;
 
-    gw_cmd[BUSY]                   = 1;
-    gw_cmd[READ_OP]                = 1;
-    gw_cmd[SPI_NO_ADDR]            = 1;
-    gw_cmd[SPI_SPECIAL]            = 1;
+    u_int32_t phys_addr = cont2phys(addr);
+    rc = mf_erase_sector(_mfl, phys_addr);
+    if (rc != MFE_OK) {
+        return errmsg("Flash erase of address 0x%x failed: %s", 
+                      phys_addr,
+                      mf_err2str(rc));
+    }
 
-    gw_addr.range(SPI_CMD_E,  SPI_CMD_S)  = FC_RDSR;
+    return true;
+}
 
-    do {
+bool Flash::print_attr() {
+    printf("Flash attributes:\n");
+    printf("  HwDevId          %d\n",     _attr.hw_dev_id);
+    printf("  TotalSize        0x%x\n",   _attr.size);
+    printf("  Banks            0x%x\n",   _attr.size/_attr.bank_size );
+    printf("  SectorSize       0x%x\n",   _attr.sector_size );
+    printf("  WriteBlockSize   0x%x\n",   _attr.block_write);
+    printf("  CmdSet           0x%x\n",   _attr.command_set);
 
-        if (++cnt > fast_retrys)
-            usleep(delay);
-        if (cnt > retrys) {
-            reset();
-            return _f.errmsg("Flash write error - Write In Progress bit didn't clear.");
-        }
+    return true;
+}
 
-        MWRITE4(FLASH_ADDR, gw_addr);
-        MWRITE4(FLASH_GW,   gw_cmd);
+bool Flash::print_attr_old_format() {
+    // Needed for some old tools which parce the size section of the CFI query in oder flint versions:
 
-        if (!_f.wait_ready("RDSR"))
-            return false;
+    int i;
+    printf("\n----Sector Organization Parameters-------------------\n\n");
 
-        MREAD4(FLASH_DATA, &gw_data);
+    printf("%-50s ", "Device size:");
+    printf("[%8li] bytes, or [%2i] Mbit\n",
+           (long int)_attr.size, 
+           (int) (_attr.size/((long)0x20000)));
 
-    } while (gw_data[24]); // WIP bit in status reg - Note byte 0 is in bits 31-24 of data word.
+    printf("%-50s ", "Number of erase block regions:");
+    printf("%d\n", _attr.num_erase_blocks);
 
+    for (i = 0; i < _attr.num_erase_blocks; i++) {
+        printf("  Size:[%8lx] bytes, Mask [%08x], [Number:[%4i]\n",
+               _attr.erase_block[i].sector_size,
+               _attr.erase_block[i].sector_mask,
+               _attr.erase_block[i].num_sectors);
+    }
     return true;
 }
 
+////////////////////////////////////////////////////////////////////////
+
+
 
 ////////////////////////////////////////////////////////////////////////
 //
@@ -3223,11 +1242,18 @@ bool SpiFlash::CmdSetStSpi::wait_wip(u_int32_t delay, u_int32_t retrys, u_int32_
 
 class Operations : public ErrMsg {
 public:
-
-    Operations() : _last_image_addr(0), _num_ports(2), _allow_skip_is(false) {}
+    Operations() : 
+        _last_image_addr(0), 
+        _num_ports(2), 
+        _allow_skip_is(false), 
+        _is_cntx(false), 
+        _cntx_striped_image(false),
+        _burn_blank_guids(false) {}
 
     enum {
-       GUIDS = 4
+       GUIDS = 4,
+        MACS  = 2,
+        MAX_GUIDS = 6
     };
 
     enum ImageInfoTags {
@@ -3238,46 +1264,107 @@ public:
         II_PSID               = 4,
         II_VSD                = 5,
         II_SuppurtedPsids     = 6,
-        II_Last               = 7,  // Mark the end of used tag ids
+        II_ProductVer         = 7,
+        II_VsdVendorId        = 8,
+        II_IsGa               = 9,
+        II_Last               = 10,  // Mark the end of used tag ids
         II_End                = 0xff
     };
     
     struct ImageInfo;
 
+    // Burn operations:
     bool write_image     (Flash& f, u_int32_t addr, void *data, int cnt, bool need_report);
     bool WriteSignature  (Flash& f, u_int32_t image_idx, u_int32_t sig);
     bool repair          (Flash& f, const int from, const int to, bool need_report);
-    bool FailSafe_burn   (Flash& f, void *data, int size, bool single_image_burn, bool need_report);
+    bool FailSafeBurn    (Flash& f, FImage& fim, bool need_report, bool single_image_burn);
+    bool CntxFailSafeBurn(Flash&     f, 
+                          FImage&    fim, 
+                          bool       need_report, 
+                          ImageInfo* flash_info, 
+                          ImageInfo* image_info,
+                          bool       allow_nofs = false);
+    // Image operations:
+    bool Verify          (FBase& f, ImageInfo* info = NULL, bool both_images = false);
+    bool VerifyCntx      (FBase& f, ImageInfo* info = NULL, bool both_images = false, bool only_get_start = false);
+
+    bool LoadAsExpRom    (FBase& f);
 
-    bool Verify          (FBase& f);
     bool DumpConf        (const char* conf_file = NULL);
+    bool GetExpRomVersion(ImageInfo* info);
 
-    
-    bool DisplayImageInfo(ImageInfo* info);
+    bool DisplayExpRomInfo(ImageInfo* info);
+    bool DisplayImageInfo (ImageInfo* info);
 
-    bool QueryAll        (FBase& f, ImageInfo* info) {return QueryIs(f, info) && 
-                                                             (!info->isFailsafe || QueryPs(f, info)) && 
+    bool QueryAll        (FBase& f, ImageInfo* info) {return (IsCntx() ||
+                                                              (QueryIs(f, info) && 
+                                                              (!info->isFailsafe || QueryPs(f, info)))) && 
                                                              QueryImage(f, info);}
 
     bool getBSN          (char *s, guid_t *guid);
     bool getGUID         (const char *s, guid_t *guid);
 
     bool patchVSD        (FImage& f, 
+                          Operations::ImageInfo* info,
                           const char *user_vsd, 
                           const char *user_psid,
                           const char *curr_vsd,
                           const char *curr_psid,
                           const char *image_psid);
-    
-    bool patchGUIDs      (FImage& f, guid_t guids[GUIDS], guid_t old_guids[GUIDS], bool interactive);
+
+    // Misc operations
+    void SetDevFlags(u_int16_t devType, bool& ib_dev, bool& eth_dev);
+    bool CheckGuidsFlags(u_int16_t devType,
+                         bool ib_dev,
+                         bool guids_specified,
+                         bool eth_dev,
+                         bool macs_specified);
+
+    bool patchGUIDs      (FImage&    f,
+                          ImageInfo* info,
+                          bool       patch_guids, 
+                          bool       patch_macs, 
+                          bool       user_guids,
+                          bool       user_macs,
+                          guid_t     new_guids[MAX_GUIDS], 
+                          guid_t     old_guids[MAX_GUIDS],
+                          u_int32_t num_of_old_guids,
+                          bool       interactive);
+
+    bool printGUIDs(const char* msg, guid_t guids[MAX_GUIDS], bool print_guids, bool print_macs, bool old_guid_fmt);
 
     void SetNumPorts     (u_int32_t num_ports) {_num_ports = num_ports;}
     void SetAllowSkipIs  (bool asis)           {_allow_skip_is = asis;}
-    
-    bool ask_user        (const char* msg);
 
-    u_int32_t              _last_image_addr;
+    void SetBurnBlankGuids(bool b) {_burn_blank_guids = b;}
+    bool GetBurnBlankGuids() {return _burn_blank_guids;}
+
+    // ConnectX methods
+    void SetCntxMode     (bool cntx_mode)      {_is_cntx = cntx_mode;}
+    void SetCntxStripedImage(bool si)          {_cntx_striped_image = si;}
+    bool IsCntx          ()                    {return _is_cntx;}
+    bool CheckMac        (u_int64_t mac);
+
+    bool CheckIsCntx     (FBase& f);
+
+    bool CntxIsIb        (u_int32_t devid)     {return (devid == 25408) || // IB SDR
+                                                       (devid == 25418) || // IB DDR
+                                                       (devid == 25428) || // IB QDR
+                                                       (devid == 26418) || // IB DDR
+                                                       (devid == 26428);   // IB QDR
+    }
+
+    bool CntxIsEth       (u_int32_t devid)     {return (devid == 25448) || // ETH
+                                                       (devid == 26448) || // ETH
+                                                        CntxIsIb(devid);   // From FW 2.5.0, CntX ib devices also support ETH
+    }
+
+    bool CntxIsMp        (u_int32_t devid)     {return CntxIsIb(devid) && CntxIsEth(devid);}
+
+    bool ask_user        ();
 
+    // _last_image_addr is set by the Verify() op
+    u_int32_t            _last_image_addr;
 
     // 
     // ImageInfo struct: Everything you wanted to know about the FW image (and was afraid to ask).
@@ -3288,15 +1375,20 @@ public:
         ImageInfo() :
             invSectOk(false),
             psOk(false),
-            imageOk(false) 
+            imageOk(false)
         {
+            memset(allImgStart,    0, sizeof(allImgStart));
+            memset(guids      , 0xff, sizeof(guids));
+
             psid[0] = '\0';
             vsd[0]  = '\0';
             for (int i=0; i < II_Last; i++ ) 
-                infoFound[i] = false;
+                infoOffs[i] = 0;
+
+            expRomFound = false;
         }
         
-        // *Ok : The exit status ofthe specific query.
+        // *Ok : The exit status of the specific query.
         // Note - invSectOk = true doesnt mean that invariant sector exists, it
         //        only means that the query was OK (and isFailsafe may be false).
 
@@ -3304,30 +1396,72 @@ public:
         bool         psOk;
         bool         imageOk;
 
+        u_int32_t    allImgStart[2];
+
         bool         isFailsafe;
 
+        // ConnectX:
+        // For an image file where the image is marked as FW but actually contiguous, this bit would be cleared. 
+        bool         actuallyFailsafe;
+        u_int32_t    cntxLog2ChunkSize;
 
         bool         validImage[2];
         u_int32_t    psStart;
         u_int32_t    imgStart;
 
-        guid_t       guids[4];
-        char         vsd[209];
-        char         psid[17];
+        // For ConnectX, guids 4:5 are the cards MAC addresses (if applicable)
+        guid_t       guids[MAX_GUIDS];
+        u_int32_t    guidPtr;
+        u_int32_t    guidNum;
+        bool         blankGuids;
+
+        u_int32_t    infoSectPtr;
+
+        u_int16_t    vsdVendorId;
+        char         vsd[VSD_LEN+1];
+        char         psid[PSID_LEN+1];
+        char         productVer[17];
+
+        bool         isGa;
 
         u_int8_t     isVer;
-        u_int16_t    fwVer[3];   // = {major_ver, minor_ver , sum_minor_ver}
+        u_int16_t    fwVer[3];        // = {major_ver, minor_ver , sum_minor_ver}
         u_int16_t    fwTime[6];  // = {year, month, day, hour, minute, second}
 
         u_int16_t    devType;
         u_int8_t     devRev;
 
-        bool         infoFound[II_Last];
+        u_int32_t    infoOffs[II_Last];  // Offset of the tag data inside the info section data. 
+                                         // Can not be 0 (because of tag header) - 0 means not found.
+        bool         expRomFound;
+        bool         expRomValidVersion;
+        u_int16_t    expRomProductId;    // 0 - invalid.
+        u_int16_t    expRomVer[3];
+        u_int16_t    expRomDevId;
+        u_int8_t     expRomPort;
 
+        bool         magicPatternFound;
     };
-    
 
+    bool FwVerLessThan(u_int16_t r1[3], u_int16_t r2[3]) {
+        int i;
+        for (i = 0; i < 3 ; i++)
+            if      (r1[i] < r2[i]) 
+                return true;
+            else if (r1[i] > r2[i])
+                return false;
+            
+        return false; // equal versions
+    }
+
+    enum {CNTX_START_POS_SIZE = 6};
+
+    static const u_int32_t _cntx_image_start_pos[CNTX_START_POS_SIZE];
+
+    static const u_int32_t _cntx_magic_pattern[4];
 
+    void patchGUIDsSection      (u_int32_t *buf, u_int32_t ind,
+                                guid_t guids[GUIDS], int nguids);
 private:
 
     bool FailSafe_burn_image   (Flash&       f, 
@@ -3350,35 +1484,61 @@ private:
 
     bool checkPS                (FBase& f, u_int32_t offs, u_int32_t& next, const char *pref);
 
-    bool checkList              (FBase& f, u_int32_t offs, const char *pref);
+    bool checkList              (FBase& f, u_int32_t offs, u_int32_t fw_start, const char *pref);
 
     bool extractGUIDptr         (u_int32_t sign, u_int32_t *buf, int buf_len,
                                 char *pref, u_int32_t *ind, int *nguids);
 
-    void patchGUIDsSection      (u_int32_t *buf, u_int32_t ind,
-                                guid_t guids[GUIDS], int nguids);
-    
-    u_int32_t BSN_subfield      (const char *s, int beg, int len);
 
-    void _patchVSD              (FImage& f, int ind, char *vsd);
+    void recalcSectionCrc       (u_int8_t *buf, u_int32_t data_size);
+
+    u_int32_t BSN_subfield      (const char *s, int beg, int len);
 
     void PatchPs                (u_int8_t*      rawPs, 
                                  const char     vsd[VSD_LEN],
                                  const char     psid[PSID_LEN]   = NULL,
                                  u_int32_t      imageAddr        = 0);
 
+    void PatchInfoSect          (u_int8_t*      rawSect,
+                                 u_int32_t      vsdOffs,
+                                 const char*    vsd);
 
     bool QueryIs                (FBase& f,  ImageInfo* info);
     bool QueryPs                (FBase& f,  ImageInfo* info);
     bool QueryImage             (FBase& f,  ImageInfo* info);
 
-    
+    bool CntxFindMagicPattern   (FBase& f,  u_int32_t addr);
+    bool CntxFindAllImageStart  (FBase& f,  u_int32_t start_locations[CNTX_START_POS_SIZE], u_int32_t* found_images);
+    bool CntxGetFsData          (FBase& f,  u_int32_t img_addr, bool& fs_en, u_int32_t& log2chunk_size);
+
     bool ParseInfoSect          (u_int8_t* buff, u_int32_t byteSize, ImageInfo *info);
 
     u_int32_t _num_ports;
     bool      _allow_skip_is;
+    bool      _is_cntx;
+    bool      _cntx_striped_image;
+
+    bool      _burn_blank_guids;
 
     std::vector<u_int8_t>  _fw_conf_sect;
+    std::vector<u_int8_t>  _rom_sect;
+};
+
+
+const u_int32_t Operations::_cntx_magic_pattern[4] = {
+    0x4D544657,   // Ascii of "MTFW" 
+    0x8CDFD000,   // Random data     
+    0xDEAD9270,                      
+    0x4154BEEF
+};
+
+const u_int32_t Operations::_cntx_image_start_pos[Operations::CNTX_START_POS_SIZE] = {
+           0,
+     0x10000,
+     0x20000,
+     0x40000,
+     0x80000,
+    0x100000
 };
 
 
@@ -3387,16 +1547,20 @@ private:
 // Returns true if user chose Y, false if user chose N.
 //
 
-bool Operations::ask_user(const char* msg) {
-    printf(msg);
+bool Operations::ask_user() {
+    printf("\n Do you want to continue ? (y/n) [n] : ");
     if (_assume_yes)
         printf("y\n");
     else {
         char ansbuff[32];
         ansbuff[0] = '\0';
+        if (_assume_no) {
+            printf("n\n");
+            return errmsg("-no flag is set");
+        }
 
         if (!isatty(0)) {
-            return errmsg("Not on tty - Can't interact. assuming \"no\" for question \"%s\"", msg);
+            return errmsg("Not on tty - Can not interact. assuming \"no\"");
         }
         fflush(stdout);
         fgets(ansbuff, 30, stdin);
@@ -3424,6 +1588,7 @@ bool Operations::write_image(Flash& f, u_int32_t addr, void *data, int cnt, bool
         fflush(stdout);
     }
 
+
     while (towrite) {
         // Write
         int trans = (towrite > (int)Flash::TRANS) ? (int)Flash::TRANS : towrite;
@@ -3457,7 +1622,7 @@ bool Operations::write_image(Flash& f, u_int32_t addr, void *data, int cnt, bool
 bool Operations::WriteSignature(Flash& f, u_int32_t image_idx, u_int32_t sig) {
     u_int32_t sect_size = f.get_sector_size();
 
-    if (!f.write( sect_size * (image_idx + 1)  + 8, &sig, 4, true, false))
+    if (!f.write( sect_size * (image_idx + 1)  + 8, &sig, 4, true))
         return false;
 
     return true;
@@ -3483,8 +1648,6 @@ bool Operations::repair(Flash& f, const int from, const int to, bool need_report
     }
     report_erase(" READ %s 100%", from ? "SPS" : "PPS");
 
-
-    
     u_int32_t im_ptr = sect[0];
     u_int32_t sig    = sect[2];
 
@@ -3493,7 +1656,7 @@ bool Operations::repair(Flash& f, const int from, const int to, bool need_report
 
     // Make sure ps ik ok:
     if (sig != SIGNATURE) {
-        return errmsg("Can't copy image. Pointer sector %d signature is bad (%08x).", from, sig);
+        return errmsg("Can not copy image. Pointer sector %d signature is bad (%08x).", from, sig);
     }
 
     // Valid image size in bytes
@@ -3587,12 +1750,12 @@ bool Operations::FailSafe_burn_image(Flash&       f,
     u_int8_t* data8 = (u_int8_t*) data;
     u_int32_t sect_size = f.get_sector_size();
 
-    report("Burning %-9s FW image without signatures  - ", image_name);
+    report("Burning %-6s FW image without signatures  - ", image_name);
     fflush(stdout);
 
     // Invalidate signature
     u_int32_t zeros = 0;
-    if (!f.write(ps_addr + 8, &zeros, 4, true, false)) {
+    if (!f.write(ps_addr + 8, &zeros, 4, true)) {
         report("FAILED (Invalidating signature)\n\n");
         return false;
     }
@@ -3606,7 +1769,7 @@ bool Operations::FailSafe_burn_image(Flash&       f,
         return false;
     }
     report("\b\b\b\bOK  \n");
-    report("Restoring %-9s signature                  - ", image_name); 
+    report("Restoring %-6s signature                  - ", image_name); 
     fflush(stdout);
 
     // Burn PS
@@ -3618,7 +1781,7 @@ bool Operations::FailSafe_burn_image(Flash&       f,
     // Validate signature
     u_int32_t sig = SIGNATURE;
     TOCPU1(sig);
-    if (!f.write(ps_addr + 8, &sig, 4, true, false)) {
+    if (!f.write(ps_addr + 8, &sig, 4, true)) {
         report("FAILED\n\n");
         return false;
     }
@@ -3649,7 +1812,7 @@ bool Operations::FailSafe_burn_internal(Flash& f, void *data, int cnt, bool need
         return errmsg("Invalid image: too small.");
     }
     if (prim_len != scnd_len) {
-        return errmsg("Invalid image: two FW images should be in a same size.");
+        return errmsg("Invalid image: two FW images should have the same size.");
     }
 
     // Image size from flash
@@ -3697,7 +1860,7 @@ bool Operations::FailSafe_burn_internal(Flash& f, void *data, int cnt, bool need
 bool Operations::CheckInvariantSector(Flash& f, u_int32_t *data32, int sect_size) {
     int i;
 
-    report("\nRead and verify Invariant Sector               - ");
+    report("\nRead and verify Invariant Sector            - ");
     fflush(stdout);
 
     // Once more check signature - the Inv.Sector signature should be OK
@@ -3733,23 +1896,22 @@ bool Operations::CheckInvariantSector(Flash& f, u_int32_t *data32, int sect_size
     if (first_diff != -1) {
             report("DIFF DETECTED\n\n");
             printf(" Invariant sector mismatch. Address 0x%x "
-                   " in image: 0x%08x, while in flash: 0x%08x\n\n",
+                   " in image: 0x%08x, while on flash: 0x%08x\n\n",
                    first_diff*4 , data32[first_diff], buf1[first_diff]);
 
-            printf(" The invariant sector can not be burnt in a failsafe manner.\n"
-                   " To force burn of the invariant sector, rerun with -nofs flag.\n");
+            printf(" The invariant sector can not be burnt in a failsafe manner.\n");
 
         if (_allow_skip_is) {
-            printf(" You can also continue to update the FW without updating the invariant sector.\n"
-                   " See the firmware release notes for more details.\n\n");
+            printf(" You can continue the FW update without burning the invariant sector.\n"
+                   " See FW release notes for details on invariant sector updates.\n\n");
 
-            return ask_user(" Do you want to continue  ? ");
+            return ask_user();
 
         } else {
             // Continue with burn
-            printf(" You can also update the FW without updating the invariant sector by\n"
-                   " specifying the -skip_is flag.\n" 
-                   " See the firmware release notes for more details.\n\n");
+            printf(" You can perform the FW update without burning the invariant sector by\n"
+                   " by specifying the -skip_is flag.\n" 
+                   " See FW release notes for details on invariant sector updates.\n\n");
 
             return errmsg("Invariant sector mismatch");
         }
@@ -3761,14 +1923,25 @@ bool Operations::CheckInvariantSector(Flash& f, u_int32_t *data32, int sect_size
 }
 
 ////////////////////////////////////////////////////////////////////////
-bool Operations::FailSafe_burn(Flash& f, void *data, int size, bool single_image_burn, bool need_report)
+bool Operations::FailSafeBurn(Flash& f, FImage& fim, bool need_report, bool single_image_burn)
 {
-    u_int32_t *data32 = (u_int32_t *)data;
-    u_int8_t  *data8 =  (u_int8_t *)data;
+    u_int32_t *data32 = fim.getBuf();
+    u_int8_t  *data8  = (u_int8_t *) data32;
+    void*      data   = data8;
+    int        size   = fim.getBufLength();
 
     u_int32_t i;
         
-    u_int32_t sect_size = f.get_sector_size();
+    u_int32_t sect_size     = f.get_sector_size();
+    u_int32_t img_sect_size = fim.get_sector_size();
+
+    // Check that the flash sector size is well defined in the image
+    if (img_sect_size && (img_sect_size != sect_size)) {
+        return errmsg("Flash sector size(0x%x) differs from sector size defined in the image (0x%x).\n"
+                      "This means that the given FW file is not configured to work with the burnt HCA board type.\n",
+                      sect_size,
+                      img_sect_size);
+    }
 
     if (size < (int)sect_size * 3) {
         report("FAILED\n\n");
@@ -3805,7 +1978,7 @@ bool Operations::FailSafe_burn(Flash& f, void *data, int size, bool single_image
     u_int32_t  cur_image_size[2];
 
     // Check signatures on flash
-    report("Read and verify PPS/SPS in flash               - ");
+    report("Read and verify PPS/SPS on flash            - ");
     for (i = 0 ; i < 2 ; i++) {
         if (!f.read(sect_size * (i+1) + 8, &signature_for_compare)) {
 
@@ -3827,12 +2000,12 @@ bool Operations::FailSafe_burn(Flash& f, void *data, int size, bool single_image
 
     if (!cur_image_ok[0] && !cur_image_ok[1]) {
         //
-        // Both images are invalid in flash
+        // Both images are invalid on flash
         // --------------------------------
         //
-        printf("\nBoth images (primary and secondary) are invalid in flash.\n");
-        printf("The burning can't be failsafe, but it is harmless for host.\n");
-        if(!ask_user("\n    Do you want to continue ? (y/n) [n] : ")) {
+        printf("\nBoth images (primary and secondary) are invalid on flash.\n");
+        printf("The burning can not be failsafe, but it is harmless for host.\n");
+        if(!ask_user()) {
             return false;
         }
 
@@ -3884,7 +2057,7 @@ bool Operations::FailSafe_burn(Flash& f, void *data, int size, bool single_image
         }
 
         //
-        // Both images are valid in flash
+        // Both images are valid on flash
         //
         return FailSafe_burn_internal(f, data, size, need_report);
 
@@ -3908,7 +2081,7 @@ bool Operations::FailSafe_burn(Flash& f, void *data, int size, bool single_image
             //   first image, and new image would be written as second.
 
             if (frst_new_image_addr + frst_new_image_size > cur_image_addr[1]) {
-                // New image is too large - can't get in between first image start 
+                // New image is too large - can not get in between first image start 
                 // and current (second) image - move current image to be first.
                 if (!repair(f, 1, 0, need_report))
                     return false;
@@ -3927,8 +2100,7 @@ bool Operations::FailSafe_burn(Flash& f, void *data, int size, bool single_image
         }
 
         if (cur_image_ok[0] && cur_image_ok[1]) {
-
-            // Invalidate second image 
+            // Invalidate second image
             if (!WriteSignature(f, 1, 0)) {
                 report("FAILED\n");
                 return false;
@@ -3966,7 +2138,6 @@ bool Operations::FailSafe_burn(Flash& f, void *data, int size, bool single_image
 
                 return true;
 
-
             } else {
                 if (!FailSafe_burn_image(f, data, sect_size * 2, "second", 
                                      sect_size * (3 + new_image_size_sect) , frst_new_image_size, need_report))
@@ -3979,8 +2150,6 @@ bool Operations::FailSafe_burn(Flash& f, void *data, int size, bool single_image
                 return true;
 
             }
-
-
         } else {
             report("Bad flash state: Valid images = (%d,%d).\n", cur_image_ok[0], cur_image_ok[1] );
             return false;
@@ -3991,6 +2160,120 @@ bool Operations::FailSafe_burn(Flash& f, void *data, int size, bool single_image
     return true;
 }
 
+bool Operations::CntxFailSafeBurn(Flash&    f, 
+                                  FImage&   fim, 
+                                  bool      need_report, 
+                                  Operations::ImageInfo* flash_info, 
+                                  Operations::ImageInfo* image_info,
+                                  bool      allow_nofs) {
+
+    // TODO: See getBuf effect on zebra image.
+    u_int8_t  *data8      = (u_int8_t *) fim.getBuf();
+    int        image_size = fim.getBufLength();
+    u_int32_t  zeroes     = 0;
+
+    bool is_curr_image_in_odd_chunks;
+
+    if (!allow_nofs) {
+        if (!image_info->isFailsafe) {
+            return errmsg("The given image is not a failsae image");
+        }
+
+        if (flash_info->cntxLog2ChunkSize != image_info->cntxLog2ChunkSize) {
+            return errmsg("Failsafe chunk sizes in flash (0x%x) and in image (0x%x) are not the same.",
+                          1 << flash_info->cntxLog2ChunkSize,
+                          1 << image_info->cntxLog2ChunkSize);
+        }
+    }
+
+    u_int32_t new_image_start;
+
+    if (flash_info->imgStart != 0) {
+        is_curr_image_in_odd_chunks = 1;
+        new_image_start = 0;
+    } else {
+        is_curr_image_in_odd_chunks = 0;
+        new_image_start = (1 << image_info->cntxLog2ChunkSize);
+    }
+
+    //printf("-I- Chunk=%x . Cur image start=%x burning from %x, flash_log2_chunk_size=%d\n", 
+    //       1 << flash_info->cntxLog2ChunkSize, 
+    //       flash_info->imgStart, 
+    //       new_image_start,
+    //       flash_info->cntxLog2ChunkSize);
+
+    if (image_info->isFailsafe) {
+        f.set_address_convertor(image_info->cntxLog2ChunkSize, !is_curr_image_in_odd_chunks);
+    } else {
+        f.set_address_convertor(0,0);
+        new_image_start = 0;
+    }
+
+    // Go ahead and burn!
+    const char* image_name = new_image_start == 0 ? "first" : "second";
+    report("\nBurning %-6s FW image without signatures  - ", image_name);
+
+    fflush(stdout);
+
+    if (!write_image(f, 16 , data8 + 16, image_size - 16, need_report)) {
+        report("FAILED\n\n");
+        return false;
+    }
+    report("\b\b\b\bOK  \n");
+
+    report("Restoring %-6s signature                  -     ", image_name); 
+
+    fflush(stdout);
+
+    // Write new signature
+    if (!f.write(0, data8, 16, true)) {
+        report("FAILED\n\n");
+        return false;
+    }
+
+    // Write new image start address to crspace (for SW reset)
+    f.cr_write(0xf0000, (new_image_start << 8) | 0x06);
+
+    if (image_info->isFailsafe) {
+        if (allow_nofs) {
+            // When burning in nofs, remnant of older image with different chunk size
+            // may reside on the flash - 
+            // Invalidate all images marking on flash except the one we've just burnt
+
+            u_int32_t cntx_image_start[CNTX_START_POS_SIZE];
+            u_int32_t cntx_image_num;
+            u_int32_t i;
+
+            CntxFindAllImageStart(f, cntx_image_start, &cntx_image_num);
+            // Address convertor is disabled now - use phys addresses
+            for (i = 0; i < cntx_image_num; i++) {
+                if (cntx_image_start[i] != new_image_start) {
+                    if (!f.write(cntx_image_start[i], &zeroes, sizeof(zeroes), true)) {
+                        report("FAILED\n\n");
+                        return false;
+                    } 
+                }
+            }
+        } else {
+            // invalidate previous signature
+            f.set_address_convertor(image_info->cntxLog2ChunkSize, is_curr_image_in_odd_chunks);
+            if (!f.write(0, &zeroes, sizeof(zeroes), true)) {
+                report("FAILED\n\n");
+                return false;
+            }
+        }
+    }
+    report("\b\b\b\bOK  \n");
+
+    if (allow_nofs && image_info->isFailsafe) {
+
+    }
+
+    return true;
+}
+
+
+
 
 ////////////////////////////////////////////////////////////////////////
 //                                                                    //
@@ -3999,6 +2282,70 @@ bool Operations::FailSafe_burn(Flash& f, void *data, int size, bool single_image
 // ****************************************************************** //
 //                                                                    //
 ////////////////////////////////////////////////////////////////////////
+
+
+//
+// Cntx image verification flow:
+// 
+// The "zebra"failsafe scheme presents few problems:
+// - Need to search for a valid image in the flash.
+// - handle no image / more than a single valid image cases
+// - check image actual start location(s) vs. fs_zebra_size field in the image.
+// - Image file is not striped though its fs_zebra_size sais it is - need to allow that
+//   when checking a file.
+//
+// Verification flow:
+// 
+// 1. Find image start addresses.
+//    if num of start_addr not in [1,2] - Error - corrupted flash
+// 2. for each start_addr:
+//      Check that its fs_zebra_size is identical to other images (if exists)
+//      For Flash: 
+//          Set address translation according to the fs_zebra_size.
+//      For File:
+//          *GUESS* if image is contiguous or striped. This can be according
+//          to the file size vs. image size field in the image.
+//          Set address translation according to the fs_zebra_size or to 0.
+//      verify - same as previous projects nofs image, with the aditional start
+//
+
+
+bool Operations::CntxFindMagicPattern       (FBase& f,  u_int32_t addr) {
+    int i;
+    if (addr + 16 > f.get_size()) {
+        return false;
+    }
+    for (i = 0; i < 4 ; i++) {
+        u_int32_t w;
+        READ4(f, addr + i * 4, &w, "Magic Pattern" );
+        TOCPU1(w);
+        if (w != _cntx_magic_pattern[i]) {
+            //printf("-D- Looking for magic pattern %d addr %06x: Exp=%08x Act=%08x\n", i, addr + i * 4, _cntx_magic_pattern[i], w);
+            return false;
+        }
+    }
+
+    return true;
+}
+
+// FindAllImageStart
+// OUT: start_locations: set to the start addresses of the found image markers (in accending order)
+// OUT: found_images:    Number of found images (and number of valid entries in the start_locations array).
+bool Operations::CntxFindAllImageStart       (FBase& f,  u_int32_t start_locations[CNTX_START_POS_SIZE], u_int32_t* found_images) {
+    int i;
+    f.set_address_convertor(0,0);
+
+    *found_images = 0;
+    for (i = 0; i < CNTX_START_POS_SIZE; i++) {
+        if (CntxFindMagicPattern(f, _cntx_image_start_pos[i])) {
+            start_locations[*found_images] = _cntx_image_start_pos[i];
+            (*found_images)++;
+        }
+    }
+
+    return true;
+}
+
 bool Operations::checkBoot2(FBase& f, u_int32_t beg, u_int32_t offs,
                 u_int32_t& next, const char *pref)
 {
@@ -4030,7 +2377,7 @@ bool Operations::checkBoot2(FBase& f, u_int32_t beg, u_int32_t offs,
     if (crc.get() != crc_act) {
         report("%s /0x%08x/ - wrong CRC (exp:0x%x, act:0x%x)\n",
                pr, offs+beg, crc.get(), crc_act);
-        return false;
+        return errmsg("Bad CRC");
     }
 
     if (_print_crc)
@@ -4049,6 +2396,9 @@ bool Operations::checkGen(FBase& f, u_int32_t beg,
 {
     char         *pr = (char *)alloca(strlen(pref) + 100);
 
+    char         unknown_sect_name[128];
+    const char*  sect_name;
+
     u_int32_t    size=0;
     GPH          gph;
 
@@ -4058,11 +2408,10 @@ bool Operations::checkGen(FBase& f, u_int32_t beg,
     TOCPUBY(gph);
 
     // Body
-
     part_cnt++;
 
     // May be BOOT3?
-    if (gph.type < H_FIRST  ||  gph.type > H_LAST) {
+    if (gph.type < H_FIRST  ||  gph.type >= H_LAST) {
         if (part_cnt > 2) {
             //report("%s /0x%x/ - Invalid partition type (%d)\n",
             //       pref, offs+beg, gph.type);
@@ -4073,88 +2422,40 @@ bool Operations::checkGen(FBase& f, u_int32_t beg,
 
     // All partitions here
     offs += beg;
-    switch (gph.type) {
-    case H_DDR:
-        size = gph.size * sizeof(u_int32_t);
-        sprintf(pr, "%s /0x%08x-0x%08x (0x%06x)/ (DDR)",
-                pref, offs, offs+size+(u_int32_t)sizeof(gph)+3,
-                size+(u_int32_t)sizeof(gph)+4);
-        break;
-    case H_CNF:
-        size = gph.size * sizeof(u_int32_t);
-        sprintf(pr, "%s /0x%08x-0x%08x (0x%06x)/ (Configuration)",
-                pref, offs, offs+size+(u_int32_t)sizeof(gph)+3,
-                size+(u_int32_t)sizeof(gph)+4);
-        break;
-    case H_JMP:
-        size = gph.size * sizeof(u_int32_t);
-        sprintf(pr, "%s /0x%08x-0x%08x (0x%06x)/ (Jump addresses)",
-                pref, offs, offs+size+(u_int32_t)sizeof(gph)+3,
-                size+(u_int32_t)sizeof(gph)+4);
-        break;
-    case H_EMT:
-        size = gph.size;
-        size = (size + 3) / 4 * 4;
-        sprintf(pr, "%s /0x%08x-0x%08x (0x%06x)/ (EMT Service)",
-                pref, offs, offs+size+(u_int32_t)sizeof(gph)+3,
-                size+(u_int32_t)sizeof(gph)+4);
-        break;
-    case H_FW_CONF:
-        size = gph.size * sizeof(u_int32_t);
-        sprintf(pr, "%s /0x%08x-0x%08x (0x%06x)/ (FW Configuration)",
-                pref, offs, offs+size+(u_int32_t)sizeof(gph)+3,
-                size+(u_int32_t)sizeof(gph)+4);
-        break;
-    case H_ROM:
-        size = gph.size * sizeof(u_int32_t);
-        sprintf(pr, "%s /0x%08x-0x%08x (0x%06x)/ (ROM)",
-                pref, offs, offs+size+(u_int32_t)sizeof(gph)+3,
-                size+(u_int32_t)sizeof(gph)+4);
-        break;
-    case H_USER_DATA:
-        size = gph.size * sizeof(u_int32_t);
-        sprintf(pr, "%s /0x%08x-0x%08x (0x%06x)/ (User Data)",
-                pref, offs, offs+size+(u_int32_t)sizeof(gph)+3,
-                size+(u_int32_t)sizeof(gph)+4);
-        break;
-    case H_IMG_INFO:
-        size = gph.size * sizeof(u_int32_t);
-        sprintf(pr, "%s /0x%08x-0x%08x (0x%06x)/ (Image Info)",
-                pref, offs, offs+size+(u_int32_t)sizeof(gph)+3,
-                size+(u_int32_t)sizeof(gph)+4);
-        break;
-    case H_BOARD_ID:
-        size = gph.size * sizeof(u_int32_t);
-        sprintf(pr, "%s /0x%08x-0x%08x (0x%06x)/ (Board ID)",
-                pref, offs, offs+size+(u_int32_t)sizeof(gph)+3,
-                size+(u_int32_t)sizeof(gph)+4);
-        break;
-    case H_GUID:
-        size = gph.size * sizeof(u_int32_t);
-        sprintf(pr, "%s /0x%08x-0x%08x (0x%06x)/ (GUID)",
-                pref, offs, offs+size+(u_int32_t)sizeof(gph)+3,
-                size+(u_int32_t)sizeof(gph)+4);
-        break;
-    default:
+
+    if (gph.type < H_FIRST  ||  gph.type >= H_LAST) {
         // For forward compatibility, try analyzing even if section type is uncknown
         // Assuming the size is in DW, like all other sections (except emt service).
         // If this assumption is wrong, CRC calc would fail - no harm done.
-        size = gph.size * sizeof(u_int32_t);
-        sprintf(pr, "%s /0x%08x-0x%08x (0x%06x)/ (UNKNOWN SECTION TYPE (%d))",
-                pref, offs, offs+size+(u_int32_t)sizeof(gph)+3,
-                size+(u_int32_t)sizeof(gph)+4, gph.type);
+        sprintf(unknown_sect_name, "UNKNOWN (%d)" , gph.type);
+        sect_name = unknown_sect_name;
+        size = gph.size * 4;
+    } else {
+        if (gph.type == H_EMT) {
+            size = (gph.size + 3) / 4 * 4;
+        } else {
+            size = gph.size * 4;
+        }
+        sect_name = g_sectNames[gph.type];
+    }
 
+    sprintf(pr, "%s /0x%08x-0x%08x (0x%06x)/ (%s)",
+                pref, offs, offs+size+(u_int32_t)sizeof(gph)+3,
+                size+(u_int32_t)sizeof(gph)+4, sect_name);
+    
+    if (size > MAX_SECTION_SIZE) {
+        report("%s - size too big (0x%x)\n",
+               pr, size);
+        return false;
     }
 
     // CRC
     Crc16        crc;
-
-    // Fix for win32: alloca fails on large allocations.
-    // TODO: Mem leak possible - fix.
-    //u_int32_t *buff = (u_int32_t*)alloca(size);
-    u_int32_t *buff = new u_int32_t[size/4];  
+    std::vector<u_int8_t> buffv(size);
+    u_int32_t *buff = (u_int32_t*)(&(buffv[0]));
 
     READBUF(f, offs+sizeof(gph), buff, size, pr);
+
     TOCPUn(buff,size/4);
     CRCBY(crc, gph);
     CRCn(crc, buff, size/4);
@@ -4162,16 +2463,23 @@ bool Operations::checkGen(FBase& f, u_int32_t beg,
     u_int32_t crc_act;
     READ4(f, offs+sizeof(gph)+size, &crc_act, pr);
     TOCPU1(crc_act);
-    if (crc.get() != crc_act) {
+    bool blank_crc = false;
+    if (gph.type == H_GUID && crc_act == 0xffff) {
+        blank_crc = true;
+    } else if (crc.get() != crc_act) {
         report("%s /0x%08x/ - wrong CRC (exp:0x%x, act:0x%x)\n",
                pr, offs, crc.get(), crc_act);
-        return false;
+        return errmsg("Bad CRC");
     }
 
     if (_print_crc)
         report("%s - OK (CRC:0x%04x)\n", pr, crc_act&0xffff);
     else
-        report("%s - OK\n", pr);
+        if (blank_crc) {
+            report("%s - BLANK CRC (0xffff)\n", pr);
+        } else {
+            report("%s - OK\n", pr);
+        }
     next = gph.next;
 
     if (gph.type == H_FW_CONF) {
@@ -4182,11 +2490,16 @@ bool Operations::checkGen(FBase& f, u_int32_t beg,
 
     }
 
+    if (gph.type == H_ROM && _rom_sect.empty()) {
+        _rom_sect.clear();
+        _rom_sect.insert(_rom_sect.end(), 
+                         vector<u_int8_t>::iterator((u_int8_t*)buff), 
+                         vector<u_int8_t>::iterator((u_int8_t*)buff + size));
+    }
+
     // mark last read addr
     _last_image_addr = offs + size +sizeof(gph) + 4;  // the 4 is for the trailing crc
-
-    delete buff;
-
+    
     return true;
 } // checkGen
 
@@ -4205,69 +2518,375 @@ bool Operations::checkPS(FBase& f, u_int32_t offs, u_int32_t& next, const char *
         return false;
     }
 
-    // CRC
-    CRC1BY(crc, ps);
-    crc.finish();
-    if (crc.get() != ps.crc016) {
-        report("%s Pointer Sector /0x%08x/ - wrong CRC (exp:0x%x, act:0x%x)\n",
-               pref, offs, ps.crc016, crc.get());
-        return false;
+    // CRC
+    CRC1BY(crc, ps);
+    crc.finish();
+    if (crc.get() != ps.crc016) {
+        report("%s Pointer Sector /0x%08x/ - wrong CRC (exp:0x%x, act:0x%x)\n",
+               pref, offs, ps.crc016, crc.get());
+        return errmsg("Bad CRC");
+    }
+
+    next = ps.fi_addr;
+    if (_print_crc)
+        report("%s Image /0x%08x-0x%08x (0x%06x)/ (Pointer Sector)- OK (CRC:0x%04x)\n", pref, offs,
+               offs+(u_int32_t)sizeof(ps)-1, (u_int32_t)sizeof(ps), ps.crc016&0xffff);
+    else
+        report("%s Image /0x%08x-0x%08x (0x%06x)/ (Pointer Sector)- OK\n", pref, offs,
+               offs+(u_int32_t)sizeof(ps)-1, (u_int32_t)sizeof(ps));
+    return true;
+} // checkPS
+
+////////////////////////////////////////////////////////////////////////
+bool Operations::checkList(FBase& f, u_int32_t offs, u_int32_t fw_start, const char *pref)
+{
+    u_int32_t next_ptr;
+
+    CHECKB2(f, offs, fw_start, next_ptr, pref);
+    part_cnt = 1;
+    while (next_ptr && next_ptr != 0xff000000)
+        CHECKGN(f, offs, next_ptr, next_ptr, pref);
+
+    return true;
+} // checkList
+
+////////////////////////////////////////////////////////////////////////
+
+bool Operations::CheckIsCntx(FBase& f) {
+    if (f.is_flash()) {
+        return (((Flash*)&f)->get_dev_id() == 400);
+    } else {
+        u_int32_t found_images;
+        u_int32_t image_start[CNTX_START_POS_SIZE];
+
+        // Image - check if magic pattern is somewhere in the file:
+        CntxFindAllImageStart(f, image_start, &found_images);
+        return found_images > 0;
+    }
+}
+
+bool Operations::CheckMac(u_int64_t mac) {
+    if ((mac >> 40) & 0x1) {
+        return errmsg("Multicast bit (bit 40) is set");
+    }
+
+    if (mac >> 48) {
+        return errmsg("More than 48 bits are used");
+    }
+
+    return true;
+}
+
+
+bool Operations::CntxGetFsData(FBase& f, u_int32_t img_addr, bool& fs_en, u_int32_t& log2chunk_size) {
+    u_int32_t fs_info_word;
+    u_int8_t  checksum;
+
+    f.read(img_addr + 0x28, &fs_info_word);
+    TOCPU1(fs_info_word);
+
+    // printf("-D- fs_info_word=%08x\n", fs_info_word);
+
+    checksum = ((fs_info_word      ) & 0xff) +
+               ((fs_info_word >>  8) & 0xff) +
+               ((fs_info_word >> 16) & 0xff) +
+               ((fs_info_word >> 24) & 0xff);
+
+    if (checksum != 0) {
+        return errmsg("Corrupted chunk size checksum");
+    }
+
+    fs_en = (fs_info_word & 0x8) != 0;
+
+    if (fs_en) {
+        log2chunk_size = (fs_info_word & 0x7) + 16;
+    } else {
+        log2chunk_size = 0;
+    }
+
+    return true;
+}
+
+bool Operations::VerifyCntx(FBase& f, Operations::ImageInfo* info, bool both_images, bool only_get_start) {
+    u_int32_t cntx_image_start[CNTX_START_POS_SIZE];
+    u_int32_t cntx_image_num;
+    u_int32_t i;
+    bool      ret = true;
+
+    // Look for image in "physical addresses
+
+    CntxFindAllImageStart(f, cntx_image_start, &cntx_image_num);
+
+    if (cntx_image_num == 0) {
+        return errmsg("No valid image found");
+    } else if (cntx_image_num > 2) {
+                // This check may be redundant - Maybe ignore if more than 2 images found
+                return errmsg("More than 2 image start locations found at addresses %x, %x and %x. Image may be corrupted.",
+                              cntx_image_start[0],
+                              cntx_image_start[1],
+                              cntx_image_start[2]);
+    }
+
+    if (!both_images) {
+        // Check only the first image. This is enough to ensure that the device is bootable.
+        cntx_image_num = 1;
     }
 
-    next = ps.fi_addr;
-    if (_print_crc)
-        report("%s Image /0x%08x-0x%08x (0x%06x)/ (Pointer Sector)- OK (CRC:0x%04x)\n", pref, offs,
-               offs+(u_int32_t)sizeof(ps)-1, (u_int32_t)sizeof(ps), ps.crc016&0xffff);
-    else
-        report("%s Image /0x%08x-0x%08x (0x%06x)/ (Pointer Sector)- OK\n", pref, offs,
-               offs+(u_int32_t)sizeof(ps)-1, (u_int32_t)sizeof(ps));
-    return true;
-} // checkPS
+    bool info_set = false;
 
-////////////////////////////////////////////////////////////////////////
-bool Operations::checkList(FBase& f, u_int32_t offs, const char *pref)
-{
-    u_int32_t next_ptr;
+    // Verify the images:
+    for (i = 0; i < cntx_image_num; i++ ) {
+        bool      fs_en;
+        u_int32_t log2chunk_size;
 
-    CHECKB2(f, offs, 0x28, next_ptr, pref);
-    part_cnt = 1;
-    while (next_ptr && next_ptr != 0xff000000)
-        CHECKGN(f, offs, next_ptr, next_ptr, pref);
+        f.set_address_convertor(0,0);
+        if (!CntxGetFsData(f, cntx_image_start[i], fs_en, log2chunk_size)) {
+            report("\n    Can't read failsafe info word: %s\n", err());
 
-    return true;
-} // checkList
+            return (i > 0);
+        }
 
-////////////////////////////////////////////////////////////////////////
-bool Operations::Verify(FBase& f)
+        // If fw not enabled, image must start at addr 0
+        if (!fs_en && cntx_image_start[i] != 0) {
+            return errmsg("ConnectX Non Failsafe image must start at address 0. Found non-fs image at address %x",
+                          cntx_image_start[i]);
+        }
+
+        if (fs_en) {
+            report("\n     ConnectX failsafe image. Start address: %x. Chunk size %x:\n\n", cntx_image_start[i], 1 << log2chunk_size);
+            report("     NOTE: The addresses below are contiguous logical addresses. Physical addresses on\n"
+                   "           flash may be different, based on the image start address and chunk size\n\n");
+        } else {
+            report("\n     ConnectX non failsafe image:\n\n");
+        }
+
+        if (fs_en && cntx_image_start[i] != 0 && cntx_image_start[i] != (u_int32_t)(1 << log2chunk_size)) {
+            return errmsg("ConnectX Failsafe image must start at address 0 or at chunk size. Found a failsafe image at address %x",
+                          cntx_image_start[i]);
+        }
+
+        if (info && !info_set) {
+            info->imgStart          = cntx_image_start[i];
+            info->isFailsafe        = fs_en;
+            info->actuallyFailsafe  = true;
+            info->cntxLog2ChunkSize = log2chunk_size;
+        }
+
+        if (f.is_flash()) {
+            // In flash, image layout must match the FS Data
+            if (info && !info_set) {
+                info->actuallyFailsafe    = true;
+                info_set = true;
+            }
+
+            if (fs_en) {
+                f.set_address_convertor(log2chunk_size, cntx_image_start[i] != 0);
+            }
+        } else {
+            // In an image file there are 2 cases:
+            // 1. Image generated by mlxburn 
+            //    The image in the file is contiguous (though it is marked as FS) - no need to set address convertion.
+            // 2. The image was raw read from flash. In this case it would be in "zebra" format.
+            //
+            // So - I try both cases, and see which verify() succeeds.
+            //
+            // Heuristics that may come in handy:
+            // If the image does not start at addr 0, it's not an mlxburn image.
+            // If there is more than a single valid image, it's not an mlxburn image.
+            // If the file size matches the image size, it is an mlxburn image.
+            //
+            // For now, get the "striped" indication from user.
+
+            if (_cntx_striped_image) {
+                f.set_address_convertor(log2chunk_size, cntx_image_start[i] != 0);
+            } else {
+                f.set_address_convertor(0,0); //disable conversion
+            }
+            if (info && !info_set) {
+                info->actuallyFailsafe    = _cntx_striped_image;
+                info_set = true;
+            }   
+        }
+
+        bool imgStat = true;
+
+        if (!only_get_start) {
+            imgStat = checkList(f, 0, 0x38, "    ");
+        }
+        if (i == 0) {
+            ret = ret && imgStat;
+        }
+    }
+    return ret;
+}
+
+bool Operations::Verify(FBase& f, Operations::ImageInfo* info, bool both_images)
 {
     u_int32_t prim_ptr, scnd_ptr;
     u_int32_t signature;
 
-    bool ret = true;
+    bool      ret = true;
+
+    if (IsCntx()) {
+        return VerifyCntx(f, info, both_images);
+    }
 
     READ4(f, 0x24, &signature, "Signature");
     TOCPU1(signature);
     if (signature == SIGNATURE) {
         // Full image
-        _image_is_full = true;
+
+        bool psStat[2];
+
         report("\nFailsafe image:\n\n");
         CHECKB2(f, 0, 0x28, prim_ptr, "Invariant      ");
         report("\n");
-        if (checkPS(f, f.get_sector_size(), prim_ptr, "Primary  "))
-            ret &= checkList(f, prim_ptr, "               ");
+        psStat[0] = checkPS(f, f.get_sector_size(), prim_ptr, "Primary  ");
+        if (psStat[0]) {
+            ret &= checkList(f, prim_ptr, 0x28, "               ");
+        }
         report("\n");
-        if (checkPS(f, f.get_sector_size() * 2, scnd_ptr, "Secondary"))
-            CHECKLS(f, scnd_ptr, "               ");
+
+        if (psStat[0] && !both_images) {
+            return ret;
+        }
+
+        psStat[1] = checkPS(f, f.get_sector_size() * 2, scnd_ptr, "Secondary");
+        bool scndStat;
+        if (psStat[1]) {
+            scndStat = checkList(f, scnd_ptr, 0x28, "               ");
+            if (!psStat[0]) {
+                // If the first image is valid, the HCA would boot OK even if the secondary image is messed up - 
+                // consider this status only if the first image is not valid.
+                ret &= scndStat;
+            }
+        }
+
+        if (psStat[0] == false && psStat[1] == false) {
+            ret = false;
+        }
     } else {
-        // Short image
-        _image_is_full = false;
+
         report("\nShort image:\n");
-        CHECKLS(f, 0, "    ");
+        CHECKLS(f, 0, 0x28, "    ");
     }
 
     return ret;
 } // Verify
 
+bool Operations::GetExpRomVersion(ImageInfo* info) {
+    const char* magic_string = "mlxsign:";
+    u_int32_t   magic_len    = strlen(magic_string);
+    u_int32_t   i;
+    bool        magic_found  = false;
+    u_int32_t   ver_offset;
+    u_int8_t    rom_checksum = 0;
+    u_int32_t   rom_checksum_range;
+
+    info->expRomValidVersion = false;
+    if (_rom_sect.empty()) {
+        return errmsg("Expansion Rom section not found.");
+    }
+
+    // When checking the version of the expansion rom, only the first image has
+    // to be checked. This is because the second image  the uefi image does not 
+    // have to comply with checksumming to 0. To do this you have to read  byte 
+    // 2 (third) of the image  and multiply by 512 to get the size of the x86 
+    // image.
+
+    // Checksum:
+    if (_rom_sect.size() < 4) {
+        return errmsg("ROM size (0x%x) is too small", (u_int32_t)_rom_sect.size());
+    }
+
+    // restore endianess.
+    TOCPUn(&(_rom_sect[0]), _rom_sect.size()/4);
+
+    rom_checksum_range = _rom_sect[2] * 512 ;
+    if (rom_checksum_range > _rom_sect.size()) {
+       return errmsg("ROM size field (0x%2x) is larger than actual ROM size (0x%x)", 
+                     rom_checksum_range , 
+                     (u_int32_t)_rom_sect.size());
+    } else if (rom_checksum_range == 0) {
+       return errmsg("ROM size field is 0. Unknown ROM format or corrupted ROM.");
+    }
+
+    for (i = 0; i < rom_checksum_range; i++) {
+        rom_checksum += _rom_sect[i];
+    }
+
+    if (rom_checksum != 0) {
+        return errmsg("Bad ROM Checksum (0x%02x)", rom_checksum);
+    }
+
+    for (i = 0 ; i < rom_checksum_range; i++) {
+        for (u_int32_t j = 0; j < magic_len; j++) {
+            if (_rom_sect[i+j] != magic_string[j]) {
+                break;
+            } else if (j == magic_len - 1) {
+                magic_found = true;
+            }
+        }
+
+        if (magic_found) {
+            break;
+        }
+    }
+
+    if (magic_found) {
+        ver_offset = i + magic_len;
+    } else {
+        return errmsg("Mellanox version string (%s) not found in ROM section.", magic_string);
+    }
+
+    // Following mlxsign:
+    // 31:24   0        Compatible with UEFI
+    // 23:16   ProductID       Product ID:
+    //                          1 - CLP implementation for Sinai (MT25408)
+    //                          2 - CLP implementation for Hermon DDR (MT25418)
+    //                          0X10 - PXE
+    // 15:0    Major version   If ProductID < 0x10 this field is subversion 
+    //                          number, otherwise It's product major version. 
+    // 
+    // 31:16   Minor version   Product minor version*. Not valid if 
+    //                          roductID < 0x10.
+    // 15:0    SubMinor version        Product sub minor version*. Not valid if 
+    //                                  ProductID < 0x10.
+    // 
+    // 31:16   Device ID       The PCI Device ID (ex. 0x634A for Hermon 
+    //                          DDR). Not valid if ProductID < 0x10.
+    // 15:12   Port Number     Port number: 0 - Port independent, 1 - Port 1, 2 - Port 2
+    // 11:0    Reserved        
+
+    u_int32_t tmp;
+
+    tmp = __le32_to_cpu(*((u_int32_t*) &_rom_sect[ver_offset]));
+    info->expRomProductId = tmp >> 16;
+    info->expRomVer[0]    = tmp & 0xffff;
+
+    if (info->expRomProductId >= 0x10) {
+        tmp = __le32_to_cpu(*((u_int32_t*) &_rom_sect[ver_offset + 4]));
+        info->expRomVer[1] = tmp >> 16;
+        info->expRomVer[2] = tmp  & 0xffff;
+
+        tmp = __le32_to_cpu(*((u_int32_t*) &_rom_sect[ver_offset + 8]));
+        info->expRomDevId  = tmp >> 16;
+        info->expRomPort   = (tmp >> 12) & 0xf;
+    }
+    info->expRomValidVersion = true;
+    return true;
+}
+
+bool Operations::LoadAsExpRom (FBase& f) {
+    _rom_sect.clear();
+    _rom_sect.resize(f.get_size());
+    if (!f.read(0, &_rom_sect[0], f.get_size()))
+        return errmsg(f.err());
+
+    TOCPUn(&_rom_sect[0], _rom_sect.size()/4);
+
+    return true;
+}
 
 bool Operations::DumpConf        (const char* conf_file) {
 #ifndef NO_ZLIB
@@ -4279,12 +2898,12 @@ bool Operations::DumpConf        (const char* conf_file) {
         out = fopen(conf_file, "w");
 
         if (out == NULL) {
-            return errmsg("Can't open file %s for write: %s.", conf_file, strerror(errno));
+            return errmsg("Can not open file %s for write: %s.", conf_file, strerror(errno));
         }
     }
 
     if (_fw_conf_sect.empty()) {
-        return errmsg("Fw configuration section not found in the given image.");
+        return errmsg("FW configuration section not found in the given image.");
     }
 
     // restore endianess.
@@ -4300,7 +2919,7 @@ bool Operations::DumpConf        (const char* conf_file) {
 
     if (rc != Z_OK)
     {
-        return errmsg("Failed uncompressing FW Info section. uncompress returnes %d", rc);
+        return errmsg("Failed uncompressing FW configuration section. uncompress returnes %d", rc);
     }
 
     dest.resize(destLen);
@@ -4410,29 +3029,29 @@ bool Operations::getBSN(char *s, guid_t *guid)
     if (dd > 31)
         BSN_RET1("Day (dd) should not exceed 31");
     if (!dd)
-        BSN_RET1("Day (dd) can't be zero");
+        BSN_RET1("Day (dd) can not be zero");
     u_int32_t mm = BSN_subfield(s, 10+date_offs, 2);
     if (mm > 12)
         BSN_RET1("Months (mm) should not exceed 12");
     if (!mm)
-        BSN_RET1("Months (mm) can't be zero");
+        BSN_RET1("Months (mm) can not be zero");
     u_int32_t yy = BSN_subfield(s, 12+date_offs, 2);
     if (yy > 99)
         BSN_RET1("Year (yy) should not exceed 99");
     if (!yy)
-        BSN_RET1("Year (yy) can't be zero");
+        BSN_RET1("Year (yy) can not be zero");
     u_int32_t num = BSN_subfield(s, 15+date_offs, 3);
     if (num > 999)
         BSN_RET1("Number (num) should not exceed 999");
     if (!num)
-        BSN_RET1("Number (num) can't be zero");
+        BSN_RET1("Number (num) can not be zero");
     int cc = 1;
     if (cc_present) {
         cc = BSN_subfield(s, 19+date_offs, 2);
         if (cc > 14)
             BSN_RET1("Chip number (cc) should not exceed 14");
         if (!cc)
-            BSN_RET1("Chip number (cc) can't be zero");
+            BSN_RET1("Chip number (cc) can not be zero");
     }
     u_int64_t id = ((((yy*12+mm-1)*31+ dd-1) * 1000) + num-1) * 112;
     id += (cc-1)*8;
@@ -4466,6 +3085,10 @@ bool Operations::extractGUIDptr(u_int32_t sign, u_int32_t *buf, int buf_len,
 {
     u_int32_t offs = 0;
 
+    if (IsCntx()) {
+        offs = 16;
+    }
+
     // Check signature
     if (sign) {
         u_int32_t signature = buf[(sign + 8)/4];
@@ -4481,9 +3104,11 @@ bool Operations::extractGUIDptr(u_int32_t sign, u_int32_t *buf, int buf_len,
     // Get GUID ptr
     *ind = buf[(offs+0x24)/4];
     TOCPU1(*ind);
-    *ind += offs;
+    if (!IsCntx()) {
+        *ind += offs;
+    }
     if (*ind >= (u_int32_t)buf_len) {
-        printf("%s image - insane GUID pointer (%08x)\n", pref, *ind);
+        printf("%s image - illegal GUID pointer (%08x)\n", pref, *ind);
         return false;
     }
     *nguids = buf[*ind/4 - 3];
@@ -4491,21 +3116,39 @@ bool Operations::extractGUIDptr(u_int32_t sign, u_int32_t *buf, int buf_len,
     *nguids /= 2;
 
     // More sanity check
-    if (*nguids > GUIDS) {
-        printf("%s image - insane number of GUIDs (%d)\n", pref, *nguids);
+    if (IsCntx() ? (*nguids > MAX_GUIDS) : (*nguids > GUIDS)) {
+        printf("%s image - illegal number of GUIDs (0x%x)\n", pref, *nguids);
         return false;
     }
 
     return true;
 } // extractGUIDptr
 
+
+
+////////////////////////////////////////////////////////////////////////
+
+//
+// This function calculates CRC over the geven buf/size, and stores 
+// the crc in the dwors after the data.
+// Caller should make sure CRC word memory is really there.
+void Operations::recalcSectionCrc(u_int8_t *buf, u_int32_t data_size) {
+    Crc16              crc;
+    u_int32_t          i;
+
+    for (i = 0; i < data_size; i += 4) {
+        crc << __be32_to_cpu(*(u_int32_t*)(buf + i));
+    }
+    crc.finish();
+    *(u_int32_t*)(buf + data_size) = __cpu_to_be32(crc.get());
+}
+
 ////////////////////////////////////////////////////////////////////////
 void Operations::patchGUIDsSection(u_int32_t *buf, u_int32_t ind,
-                       guid_t guids[GUIDS], int nguids)
+                       guid_t guids[MAX_GUIDS], int nguids)
 {
-    u_int32_t       i, word;
-    u_int32_t       new_buf[GUIDS*2];
-    Crc16              crc;
+    u_int32_t       i;
+    u_int32_t       new_buf[MAX_GUIDS*2];
 
     // Form new GUID section
     for (i=0; i<(u_int32_t)nguids; i++) {
@@ -4513,46 +3156,55 @@ void Operations::patchGUIDsSection(u_int32_t *buf, u_int32_t ind,
         new_buf[i*2+1] = guids[i].l;
     }
 
-    // Calculate new CRC16
-    for (i=ind/4 - 4; i<ind/4; i++) {
-        word = __be32_to_cpu(buf[i]);
-        crc << word;
-    }
-    for (i=0; i<(u_int32_t)nguids*2; i++)
-        crc << new_buf[i];
-
     // Patch GUIDs
     for (i=0; i<sizeof(new_buf)/sizeof(u_int32_t); ++i) {
-        new_buf[i] = __cpu_to_be32(new_buf[i]);
+        new_buf[i] = _burn_blank_guids ? 0xffffffff : __cpu_to_be32(new_buf[i]);
     }
     memcpy(&buf[ind/4], &new_buf[0], nguids * 2 * sizeof(u_int32_t));
 
     // Insert new CRC
-    crc.finish();
-    word = crc.get();
-    buf[ind/4 + nguids*2] = __cpu_to_be32(word);
+    if (_burn_blank_guids) {
+        buf[ind/4 + nguids*2] =  __cpu_to_be32(0xffff);
+    } else {
+        recalcSectionCrc((u_int8_t*)buf + ind - sizeof(GPH), sizeof(GPH) + nguids * 8);
+    }
 } // patchGUIDsSection
 
 
-////////////////////////////////////////////////////////////////////////
-void Operations::_patchVSD(FImage& f, int ind, char *vsd)
-{
-    u_int32_t  *buf = f.getBuf();
-    Crc16      crc;
-    PS         *ps = (PS*)&buf[ind/4];
 
-    memcpy(&ps->vsd[0], vsd, VSD_LEN + PSID_LEN);
 
-    u_int32_t *qp = (u_int32_t *)ps;
-    for (unsigned int i=0; i<sizeof(PS)/sizeof(u_int32_t) - 1; i++) {
-        u_int32_t qq = *qp++;
-        TOCPU1(qq);
-        crc << qq;
+// 
+// PatchInfoSect() :
+// This func assumes it gets a pointer (rawSect) to a valid info sect.
+// It patches the it with the given data, recalculated CRC ,
+// and copies it back to the geven data
+//
+
+void Operations::PatchInfoSect(u_int8_t*      rawSect,
+                               u_int32_t      vsdOffs,
+                               const char*    vsd) {
+
+    u_int32_t  vsdSize  = __be32_to_cpu(*((u_int32_t*)(rawSect + sizeof(GPH) + vsdOffs - 4))) & 0xffffff;
+    u_int32_t  infoSize = __be32_to_cpu(*((u_int32_t*)(rawSect + 4)));
+
+    // byte size;
+    infoSize *= 4;
+
+    //printf("-D- vsdOffs=%x, vsdSize=%x, infoSize=%x\n", vsdOffs,vsdSize, infoSize );
+    if (vsd) {
+        u_int32_t len = strlen(vsd);
+
+        if (len > vsdSize) {
+            report("Warning: The given VSD length is too large (%d chars). Truncating to %d chars.\n", len, vsdSize);
+            len = vsdSize;
+        }
+       
+        memset(rawSect + sizeof(GPH) + vsdOffs,  0,   vsdSize );
+        memcpy(rawSect + sizeof(GPH) + vsdOffs,  vsd, len);
     }
-    crc.finish();
-    u_int32_t crc016 = crc.get();
-    ps->crc016 = __cpu_to_be32(crc016);
-} // _patchVSD
+
+    recalcSectionCrc(rawSect, sizeof(GPH) + infoSize);
+}
 
 
 // 
@@ -4567,7 +3219,6 @@ void Operations::PatchPs(u_int8_t*      rawPs,
                          const char*    psid,
                          u_int32_t      imageAddr) {
 
-    Crc16      crc;
     PS         *ps = (PS*)rawPs;
 
     u_int32_t fix_start = 0;
@@ -4591,7 +3242,7 @@ void Operations::PatchPs(u_int8_t*      rawPs,
         fix_end += PSID_LEN;
     }
 
-    //vsd is kept in flash byte-swapped.
+    //VSD is kept on flash byte-swapped.
     //recode it back before patching
     u_int32_t *qp;
 
@@ -4605,22 +3256,14 @@ void Operations::PatchPs(u_int8_t*      rawPs,
         ps->fi_addr = __cpu_to_be32(imageAddr);
     }
 
-    qp = (u_int32_t *)ps;
-    for (unsigned int i=0; i<sizeof(PS)/sizeof(u_int32_t) - 1; i++) {
-        u_int32_t qq = *qp++;
-        TOCPU1(qq);
-        crc << qq;
-    }
-    crc.finish();
-    u_int32_t crc016 = crc.get();
-    ps->crc016 = __cpu_to_be32(crc016);
-
+    recalcSectionCrc((u_int8_t *)ps, sizeof(PS) - 4);
 }
         
 
 ////////////////////////////////////////////////////////////////////////
 //Note that vsd1 is a string of bytes.
-bool Operations::patchVSD(FImage& f, 
+bool Operations::patchVSD(FImage& f,
+                          Operations::ImageInfo* info,
                           const char *user_vsd, 
                           const char *user_psid,
                           const char *curr_vsd,
@@ -4630,14 +3273,17 @@ bool Operations::patchVSD(FImage& f,
     const char* vsd_to_use  = curr_vsd ? curr_vsd : "";
     const char* psid_to_use = image_psid;
 
+    // TODO: Should not give the user_psid param. Do not allow for on-the-fly PSID changes.
+
+    curr_psid = NULL;
     // Form new VSD
 
     if (user_psid) {
         // New psid is explicitly given - take it from user
         printf("\n    You are about to replace current PSID in the image file - \"%s\" with a different PSID - \"%s\".\n"
-               "    Note: It is highly recommended NOT to change the image PSID.\n", user_psid, image_psid);
+               "    Note: It is highly recommended not to change the image PSID.\n", user_psid, image_psid);
 
-        if (! ask_user("\n    Is it OK ? (y/n) [n] : "))
+        if (! ask_user())
             return false;
 
         psid_to_use = user_psid;
@@ -4647,102 +3293,200 @@ bool Operations::patchVSD(FImage& f,
         vsd_to_use = user_vsd;
     }
 
+    if (IsCntx() && info->infoOffs[II_VSD]) {
+        PatchInfoSect((u_int8_t*)f.getBuf() + info->infoSectPtr - sizeof(GPH), 
+                      info->infoOffs[II_VSD],
+                      vsd_to_use);
+    } else {
+        PatchPs((u_int8_t*)f.getBuf() + f.get_sector_size(),     vsd_to_use, psid_to_use);
+        PatchPs((u_int8_t*)f.getBuf() + f.get_sector_size() * 2, vsd_to_use, psid_to_use);
+    }
 
-    if (curr_psid && strncmp( psid_to_use, (char*) curr_psid, PSID_LEN)) {
-        printf("\n    You are about to replace current PSID in flash - \"%s\" with a different PSID - \"%s\".\n", curr_psid, psid_to_use);
+    return true;
+} // pathVSD
 
-        if (! ask_user("\n    Is it OK ? (y/n) [n] : "))
-            return false;
+
+bool Operations::printGUIDs(const char* msg, guid_t guids[MAX_GUIDS], bool print_guids, bool print_macs, bool old_guid_fmt) {
+    const char* g_m;
+
+    if        ( print_guids &&  print_macs) {
+        g_m = "GUIDs/MACs";
+    } else if (!print_guids &&  print_macs) {
+        g_m = "MACs";
+    } else if ( print_guids && !print_macs) {
+        g_m = "GUIDs";
+    } else {
+        return errmsg("Internal error: Operations::PrintGuids() with both guid and mac turned off");
     }
 
-    PatchPs((u_int8_t*)f.getBuf() + f.get_sector_size(),     vsd_to_use, psid_to_use);
-    PatchPs((u_int8_t*)f.getBuf() + f.get_sector_size() * 2, vsd_to_use, psid_to_use);
+    printf("%s %s:\n", msg, g_m);
+    if (print_guids) {
+        printf("        Node  GUID:     " GUID_FORMAT "\n", guids[0].h, guids[0].l);
+        printf("        Port1 GUID:     " GUID_FORMAT "\n", guids[1].h, guids[1].l);
+        if (_num_ports > 1) 
+            printf("        Port2 GUID:     " GUID_FORMAT "\n", guids[2].h, guids[2].l);
+        if (!old_guid_fmt)
+            printf("        Sys.Image GUID: " GUID_FORMAT "\n", guids[3].h, guids[3].l);
+    }
 
+    if (print_macs) {
+        printf("        Port1 MAC:      " MAC_FORMAT "\n", guids[4].h, guids[4].l);
+        printf("        Port2 MAC:      " MAC_FORMAT "\n", guids[5].h, guids[5].l);
+    }
     return true;
-} // pathVSD
+}   
+
+void Operations::SetDevFlags(u_int16_t devType, bool& ib_dev, bool& eth_dev) {
+    ib_dev  = !IsCntx() || CntxIsIb(devType);
+    eth_dev = IsCntx()  && CntxIsEth(devType);
 
+    if (!ib_dev && !eth_dev) {
+        // Unknown device id - for forward compat - assume that ConnectX is MP and
+        // prev HCAs are IB only (these flags are for printing only - no real harm can be done).
+        ib_dev = true;
+        if (IsCntx()) {
+            eth_dev = true;
+        }
+    }
+}
+
+bool Operations::CheckGuidsFlags (u_int16_t devType,
+                                  bool ib_dev,
+                                  bool guids_specified,
+                                  bool eth_dev,
+                                  bool macs_specified) {
+    // Patch GUIDS
+    if  (guids_specified || macs_specified) {
+        if (!IsCntx() && macs_specified) {
+            return errmsg("-mac flag is not applicable for IB MT%d device.\n",
+                          devType);
+        } else if ((ib_dev && eth_dev) && !(guids_specified && macs_specified)) {
+            return errmsg("Use both -mac and -guid flags for device id %d.\n",
+                          devType);
+        } else if (ib_dev  && !guids_specified) {
+            return errmsg("Use -guid(s) flag for IB MT%d device.\n",
+                          devType);
+        } else if (eth_dev && !macs_specified) {
+            return errmsg("*** ERROR *** Use -mac(s) flag for ETH MT%d device.\n",
+                   devType);
+        }  
+    } 
+    return true;
+}
 
 ////////////////////////////////////////////////////////////////////////
-bool Operations::patchGUIDs(FImage& f, guid_t new_guids[GUIDS], guid_t old_guids[GUIDS], bool interactive)
+bool Operations::patchGUIDs (FImage&   f, 
+                             ImageInfo* info,
+                             bool      patch_guids, 
+                             bool      patch_macs,
+                             bool      user_guids,
+                             bool      user_macs,
+                             guid_t    new_guids[MAX_GUIDS], 
+                             guid_t    old_guids[MAX_GUIDS],
+                             u_int32_t num_of_old_guids,
+                             bool      interactive)
 {
-    guid_t          image_file_guids[GUIDS];
     guid_t*         used_guids = old_guids ? old_guids : new_guids;
     u_int32_t       *buf = f.getBuf();
-    int             buf_len = f.getBufLength();
-    u_int32_t       signature = buf[0x24/4];
-    u_int32_t       ind1=0,ind2=0;
-    int             nguid1, nguid2;
-
-    TOCPU1(signature);
-    if (signature == SIGNATURE) {
-        // Full image
-        if (interactive)
-            printf("\nFull image:\n\n");
-        if (!extractGUIDptr(f.get_sector_size()   , buf, buf_len, "Primary"  , &ind1, &nguid1) &&
-            !extractGUIDptr(f.get_sector_size() *2, buf, buf_len, "Secondary", &ind2, &nguid2))
-            return false;
+    int i;
 
-    } else {
-        // Short image
-        if (interactive)
-            printf("\nShort image:\n\n");
-        if (!extractGUIDptr(0, buf, buf_len, "Primary", &ind1, &nguid1))
-            return false;
-    }
+    bool old_guids_fmt = info->guidNum < GUIDS;
 
     // Print old GUIDs and get confirmation
-    if (interactive && new_guids) {
-        bool image_file_old_guids_fmt = nguid1 < GUIDS;
-        for (int i=0; i<GUIDS; i++) {
-            u_int32_t h = buf[ind1/4 + i*2];
-            TOCPU1(h);
-            u_int32_t l = buf[ind1/4 + i*2 + 1];
-            TOCPU1(l);
-            image_file_guids[i].h = h;
-            image_file_guids[i].l = l;
+    if (new_guids) {
+        if (old_guids_fmt) 
+            printf("    Old image!!!! Only %d GUIDs may be set.\n", info->guidNum);
+            // if only guids or only macs are specified by user, keep the other
+            // as currently set of flash. This is in order to simplify transitions between
+            // burning IB and ETH FW.
+
+        if (old_guids && !user_guids) {
+            for (i = 0; i < GUIDS; i++) {
+                new_guids[i] = old_guids[i];
+            }
         }
-        if (image_file_old_guids_fmt)
-            printf("    Old image!!!! Only %d GUIDs may be set.\n", nguid1);
-        if (old_guids) {
-            printf("    Current GUIDs are:\n");
-            printf("        Node:      " GUID_FORMAT "\n", old_guids[0].h,old_guids[0].l);
-            printf("        Port1:     " GUID_FORMAT "\n", old_guids[1].h,old_guids[1].l);
-            if (_num_ports > 1) 
-                printf("        Port2:     " GUID_FORMAT "\n", old_guids[2].h,old_guids[2].l);
-            if (!image_file_old_guids_fmt)
-                printf("        Sys.Image: " GUID_FORMAT "\n", old_guids[3].h,old_guids[3].l);
+        
+        if (old_guids && !user_macs) {
+            for (i = GUIDS; i < MAX_GUIDS; i++) {
+                new_guids[i] = old_guids[i];
+            }
         }
+    
+        used_guids = new_guids;
+    }   
 
-        printf("\n    You are about to burn the image with the following GUIDs:\n");
-        printf("        Node:      " GUID_FORMAT "\n", new_guids[0].h,new_guids[0].l);
-        printf("        Port1:     " GUID_FORMAT "\n", new_guids[1].h,new_guids[1].l);
-       if (_num_ports > 1) 
-           printf("        Port2:     " GUID_FORMAT "\n", new_guids[2].h,new_guids[2].l);
-        if (!image_file_old_guids_fmt)
-            printf("        Sys.Image: " GUID_FORMAT "\n", new_guids[3].h,new_guids[3].l);
+    if (patch_macs) {
 
-        if (!ask_user("\n    Is it OK ? (y/n) [n] : "))
-            return false;
+        // To ease upgrade from 4 GUIDS format to 4+2 format, or to move from IB to ETH,
+        // if macs are not
+        // explicitly set in flash, they are derived from the GUIDs according to 
+        // Mellanox methodology - 48 bit MAC == 64 bit GUID without the middle 16 bits.
+        
+        if (old_guids && ((num_of_old_guids == 4) || 
+                          (num_of_old_guids == 6 && 
+                            (old_guids[GUIDS  ].h & 0xffff)     == 0xffff     &&
+                            (old_guids[GUIDS  ].l & 0xffffffff) == 0xffffffff &&
+                            (old_guids[GUIDS+1].h & 0xffff)     == 0xffff     &&
+                            (old_guids[GUIDS+1].l & 0xffffffff) == 0xffffffff))) {
+            for (i = 0 ; i < MACS; i++) {
+                u_int64_t mac  =  old_guids[i+1].h >> 8;
+                mac <<= 24;
+                mac |= (old_guids[i+1].l & 0xffffff);
+
+                old_guids[GUIDS+i].h = mac >> 32;
+                old_guids[GUIDS+i].l = mac  & 0xffffffff;
+
+                // printf("-D- Guid " GUID_FORMAT " to MAC "MAC_FORMAT"\n", old_guids[i+1].h, old_guids[i+1].l, old_guids[i+GUIDS].h,old_guids[i+GUIDS].l  );
+            }
+        }
 
-        used_guids = new_guids;
+        guid_t* macs = &used_guids[4];
+        int i;
+                
+        for (i = 0 ; i < Operations::MACS ; i++) {
+            u_int64_t mac = (((u_int64_t)macs[i].h) << 32) | macs[i].l;
+            if (!_burn_blank_guids && !CheckMac(mac)) {
+                printf("*** ERROR *** Bad mac (" MAC_FORMAT ") %s: %s. Please re-burn with a valid -mac flag value.\n", 
+                       macs[i].h,
+                       macs[i].l,
+                       user_macs ? "given" : "found on flash",
+                       err());
+                return false;
+            }
+        }
+    }
+
+    if (new_guids && old_guids) {
+        printGUIDs("    Current settings of",
+                   old_guids,
+                   patch_guids,
+                   patch_macs,
+                   old_guids_fmt);
+        printf("\n");
+    }
+
+    if (interactive) {
+        printGUIDs("    You are about to burn the image with the following",
+                   used_guids,
+                   patch_guids,
+                   patch_macs,
+                   old_guids_fmt);
+
+        if (!ask_user())
+            return false;
     }
 
     // Path GUIDs section
-    if (ind1) 
-        patchGUIDsSection(buf, ind1, used_guids, nguid1);
-    if (ind2)
-        patchGUIDsSection(buf, ind2, used_guids, nguid2);
-
-    if (!interactive) {
-        bool old_guids_fmt = nguid1 < GUIDS;
-        printf("\n    Burn image with the following GUIDs:\n");
-        printf("        Node:      " GUID_FORMAT "\n", used_guids[0].h,used_guids[0].l);
-        printf("        Port1:     " GUID_FORMAT "\n", used_guids[1].h,used_guids[1].l);
-       if (_num_ports > 1) 
-           printf("        Port2:     " GUID_FORMAT "\n", used_guids[2].h,used_guids[2].l);
-        if (!old_guids_fmt)
-            printf("        Sys.Image: " GUID_FORMAT "\n", used_guids[3].h,used_guids[3].l);
+    if (info->guidPtr) {
+        patchGUIDsSection(buf, info->imgStart + info->guidPtr, used_guids, info->guidNum);
+
+        if (info->allImgStart[1]) {
+            // For no ConnectX HCAs, patch also the secondary image (if found). This is applicable 
+            // only for nofs burn, where both images are burnt as is.
+            patchGUIDsSection(buf, info->allImgStart[1] + info->guidPtr, used_guids, info->guidNum);
+        }
     }
+
     return true;
 } // patchGUIDs
 
@@ -4797,12 +3541,19 @@ bool Operations::QueryPs (FBase& f,
     _silent = true;
 
     if (checkPS(f, sectSize, prim_ptr, "Primary  ")) {
+        info->allImgStart[0] = prim_ptr;
         info->imgStart = prim_ptr;
         info->psStart  = sectSize;
-    } else if (checkPS(f, sectSize * 2, scnd_ptr, "Secondary")) {
-        info->imgStart = scnd_ptr;
-        info->psStart  = sectSize * 2;
-    } else {
+    } 
+    if (checkPS(f, sectSize * 2, scnd_ptr, "Secondary")) {
+        info->allImgStart[1] = scnd_ptr;
+        if (info->allImgStart[0] == 0) {
+            info->imgStart = scnd_ptr;
+            info->psStart  = sectSize * 2;
+        }
+    } 
+
+    if (info->allImgStart[0] == 0 && info->allImgStart[1] == 0)  {
         return errmsg("No valid image found.");
     }
 
@@ -4810,7 +3561,10 @@ bool Operations::QueryPs (FBase& f,
 
     memset(vsd, 0, sizeof(vsd));
     READBUF(f, info->psStart + 0x20, vsd, VSD_LEN+PSID_LEN , "Vendor Specific Data (Board ID)");
-    TOCPUBY(vsd);
+    u_int32_t* vsd_dwp = (u_int32_t*)vsd;
+
+    for (u_int32_t i=0; i < sizeof(vsd)/sizeof(u_int32_t); i++)
+         vsd_dwp[i] = bswap_32(vsd_dwp[i]);
 
     memcpy(info->vsd,  vsd,           VSD_LEN);
     memcpy(info->psid, vsd + VSD_LEN, PSID_LEN);
@@ -4830,34 +3584,78 @@ bool Operations::QueryImage (FBase& f,
                              Operations::ImageInfo* info) {
 
     u_int32_t guid_ptr, nguids;
-    guid_t guids[GUIDS];
+    guid_t guids[MAX_GUIDS];
 
     // FW ID
     u_int32_t fw_id;
+    u_int32_t fw_id_offs;
     u_int32_t im_start = info->imgStart;
 
+    if (IsCntx()) {
+        if (info->isFailsafe && info->actuallyFailsafe) {
+            f.set_address_convertor(info->cntxLog2ChunkSize, im_start != 0);
+        } else {
+            f.set_address_convertor(0,0);
+        }
+
+        im_start = 0; // offset is done by address convertor
+        info->magicPatternFound = true;
+        fw_id_offs = 0x20;
+    } else {
+        info->magicPatternFound = false;
+        fw_id_offs = 0x10;
+    }
 
-    READ4(f, im_start + 0x10, &fw_id, "FW ID");
+    READ4(f, im_start + fw_id_offs, &fw_id, "FW ID");
     TOCPU1(fw_id);
 
     info->devRev  = fw_id >> 24;    
     // Read GUIDs
-    READ4(f, im_start + 0x24, &guid_ptr, "GUID PTR");
+    READ4(f, im_start + fw_id_offs + 0x14 , &guid_ptr, "GUID PTR");
     TOCPU1(guid_ptr);
+    info->guidPtr = guid_ptr;
+
     guid_ptr += im_start;
     if (guid_ptr >= f.get_size()) {
-        return errmsg("Failed to read GUIDs - Insane GUID pointer (%08x). Probably image is corrupted", guid_ptr);
+        return errmsg("Failed to read GUIDs - Illegal GUID pointer (%08x). Probably image is corrupted", guid_ptr);
     }
     READ4(f, guid_ptr - 3*sizeof(u_int32_t), &nguids, "Number of GUIDs");
     TOCPU1(nguids);
-    if (nguids > GUIDS*2) {
-        report("Failed to read GUIDs - Insane Number of GUIDs (%d)\n", nguids);
+    nguids /= 2;
+    if (nguids > MAX_GUIDS) {
+        report("Failed to read GUIDs - Illegal Number of GUIDs (%d)\n", nguids);
         return false;
     }
-    READBUF(f, guid_ptr, guids, nguids / 2 * sizeof(u_int64_t), "GUIDS");
+    READBUF(f, guid_ptr, guids, nguids * sizeof(u_int64_t), "GUIDS");
     TOCPUBY64(guids);
-    for (u_int32_t i = 0 ; i < nguids/2 ; i++) {
+
+    u_int32_t guids_crc;
+    READ4(f, guid_ptr + nguids * sizeof(u_int64_t), &guids_crc, "GUIDS CRC");
+    guids_crc = __be32_to_cpu(guids_crc); 
+
+    info->blankGuids = true;
+
+    if ((guids_crc & 0xffff) != 0xffff ) {
+       info->blankGuids = false; 
+    }
+
+    info->guidNum = nguids;
+    for (u_int32_t i = 0 ; i < nguids ; i++) {
         info->guids[i] = guids[i];
+        if (guids[i].h != 0xffffffff || guids[i].l != 0xffffffff) {
+            info->blankGuids = false;
+        }
+    }
+
+    // Expansion Rom version:
+    if (_rom_sect.empty()) {
+        info->expRomFound = false;
+    } else {
+        info->expRomFound = true;
+        if (!GetExpRomVersion(info)) {
+            report("\nWarning: Failed to get ROM Version: %s\n\n", err()); 
+            info->expRomValidVersion = false;
+        }
     }
 
     // Read Info:
@@ -4865,7 +3663,7 @@ bool Operations::QueryImage (FBase& f,
     u_int32_t  info_ptr;
     u_int32_t  info_size;
     u_int8_t   info_ptr_cs = 0;
-    READ4(f, im_start + 0x1C, &info_ptr, "INFO PTR");
+    READ4(f, im_start + fw_id_offs + 0xC, &info_ptr, "INFO PTR");
     TOCPU1(info_ptr);
 
     // Verify info_ptr checksum (should be 0)
@@ -4875,15 +3673,16 @@ bool Operations::QueryImage (FBase& f,
     }
 
     if (info_ptr_cs) {
-        return errmsg("Failed to read Info Section - Bad checksum for Info section pointer (%08x). Probably image is corrupted", info_ptr);        
+        return errmsg("Failed to read Info Section - Bad checksum for Info section pointer (%08x). Probably the image is corrupted.", info_ptr);        
     }
 
     info_ptr = info_ptr_ba.range(23,0);
     if (info_ptr_cs == 0 && info_ptr != 0) {
-        
+        info->infoSectPtr = info_ptr;
+
         info_ptr += im_start;
         if (info_ptr >= f.get_size()) {
-            return errmsg("Failed to read Info Section - Info section pointer (%08x) too large. Probably image is corrupted", info_ptr);
+            return errmsg("Failed to read Info Section - Info section pointer (%08x) too large. Probably the image is corrupted.", info_ptr);
         }
         READ4(f, info_ptr - 3*sizeof(u_int32_t), &info_size, "Info section size");
         TOCPU1(info_size);
@@ -4894,7 +3693,7 @@ bool Operations::QueryImage (FBase& f,
         u_int8_t* info_buff = (u_int8_t*)alloca(info_size);
         READBUF(f, info_ptr, info_buff, info_size, "Info Section");
 
-        if (ParseInfoSect(info_buff, info_size,  info)) {
+        if (!ParseInfoSect(info_buff, info_size,  info)) {
             return false;
         }
     }
@@ -4908,14 +3707,20 @@ bool Operations::ParseInfoSect(u_int8_t* buff, u_int32_t byteSize, Operations::I
 
     u_int32_t *p = (u_int32_t*)buff;
     u_int32_t offs = 0;
-
+    u_int32_t tagNum = 0;
     bool endFound = false;
 
-    while ((__be32_to_cpu(*p) >> 24) != II_End && offs < byteSize) {
+    while (!endFound && offs < byteSize) {
         u_int32_t tagSize = __be32_to_cpu(*p) & 0xffffff;
         u_int32_t tagId   = __be32_to_cpu(*p) >> 24;
 
+        if (offs + tagSize > byteSize) {
+            return errmsg("Info section corrupted: Tag %d (TagId %d, size %d) exceeds Info section size (%d bytes) ",
+                          tagNum, tagId, tagSize, byteSize);
+        }
+
         u_int32_t   tmp;
+        const char* str;
 
         switch (tagId) {
         case II_FwVersion:
@@ -4923,30 +3728,59 @@ bool Operations::ParseInfoSect(u_int8_t* buff, u_int32_t byteSize, Operations::I
             tmp = __be32_to_cpu(*(p+2));
             info->fwVer[1] = tmp >> 16;
             info->fwVer[2] = tmp & 0xffff;
-
-            info->infoFound[tagId] = true;
             break;
 
         case II_DeviceType:
             tmp = __be32_to_cpu(*(p+1));
             info->devType = tmp & 0xffff;
             //info->devRev  = (tmp >> 16) & 0xff;
-            info->infoFound[tagId] = true;
+            break;
+
+        case II_VsdVendorId:
+            tmp = __be32_to_cpu(*(p+1));
+            info->vsdVendorId = tmp & 0xffff;
+            break;
+
+        case II_IsGa:
+            tmp = __be32_to_cpu(*(p+1));
+            info->isGa = tmp ? true : false;;
+            break;
 
         case II_PSID: 
             // set psid only if not previosly found in PS
             if (!info->psOk) {
-                const char* str = (const char*)p;
+                str = (const char*)p;
                 str += 4;
 
                 for (int i = 0 ; i < PSID_LEN ; i++) {
                     info->psid[i] = str[i];
                 }
                 info->psid[PSID_LEN] = '\0';
-                
-                info->infoFound[tagId] = true;
+            }
+            break;
+
+        case II_VSD: 
+            // set psid only if not previosly found in PS
+            if (!info->psOk) {
+                str = (const char*)p;
+                str += 4;
+
+                for (int i = 0 ; i < VSD_LEN ; i++) {
+                    info->vsd[i] = str[i];
+                }
+                info->vsd[VSD_LEN] = '\0';
+            }
+            break;
+
+        case II_ProductVer: 
+        
+            str = (const char*)p;
+            str += 4;
 
+            for (int i = 0 ; i < PRODUCT_VER_LEN ; i++) {
+                info->productVer[i] = str[i];
             }
+            info->productVer[PRODUCT_VER_LEN] = '\0';
             break;
 
         case II_End:
@@ -4957,12 +3791,15 @@ bool Operations::ParseInfoSect(u_int8_t* buff, u_int32_t byteSize, Operations::I
             //printf("-D- Found tag ID %d of size %d - ignoring.\n", tagId, tagSize);
         }
 
+        if (tagId < II_Last) {
+            info->infoOffs[tagId] = offs + 4;
+        }
+
         p    += tagSize/4 + 1;
-        offs += tagSize;
+        offs += tagSize + 4;
+        tagNum++;
     }
 
-    offs += 4;
-
     if (offs != byteSize) {
         if (endFound) {
             return errmsg("Info section corrupted: Section data size is %x bytes, "
@@ -4971,151 +3808,149 @@ bool Operations::ParseInfoSect(u_int8_t* buff, u_int32_t byteSize, Operations::I
             return errmsg("Info section corrupted: Section data size is %x bytes, "
                           "but end tag not found before section end.", byteSize);
         }
-    }
+    }
+
+    return true;
+}
+
+bool Operations::DisplayExpRomInfo(Operations::ImageInfo* info) {
+    report("Rom Info:        ");
+    if (info->expRomValidVersion) {
+        report("type=");
+        switch (info->expRomProductId) {
+        case 1   : report("CLP1 "); break;
+        case 2   : report("CLP2 "); break;
+        case 0x10: report("GPXE "); break;
+        default:   report("0x%x ", info->expRomProductId);
+        }
+
+        report("version=%d", info->expRomVer[0]);
 
+        if (info->expRomProductId >= 0x10) {
+            report(".%d.%d devid=%d", 
+                   info->expRomVer[1],
+                   info->expRomVer[2],
+                   info->expRomDevId);
+
+            if (info->expRomPort) {
+                // Do not display if 0 - port independant
+                report(" port=%d", info->expRomPort);
+            }
+        }
+        report("\n");
+    } else {
+        report("N/A\n");
+    }
     return true;
 }
 
 bool Operations::DisplayImageInfo(Operations::ImageInfo* info) {
+    report("Image type:      %s\n", info->magicPatternFound ? "ConnectX"   : 
+                                    info->isFailsafe        ? "Failsafe" : 
+                                                              "Short");
 
-    report("Image type:      %s\n", info->isFailsafe ? "Failsafe" : "Short");
-
-    if (info->infoFound[II_FwVersion]) {
+    if (info->infoOffs[II_FwVersion]) {
         report("FW Version:      %d.%d.%d\n", info->fwVer[0], info->fwVer[1], info->fwVer[2]);
     }
 
-    if (info->isFailsafe) {
+    if (info->infoOffs[II_ProductVer] && strlen(info->productVer)) {
+        report("Product Version: %s\n", info->productVer);
+    }
+
+    if (info->expRomFound) {
+        DisplayExpRomInfo(info);
+    }
+
+    if (info->isFailsafe && !IsCntx()) {
         report("I.S. Version:    %d\n", info->isVer );
     }
 
-    if (info->infoFound[II_DeviceType]) {
+    if (info->infoOffs[II_DeviceType]) {
         report("Device ID:       %d\n", info->devType);
+        if (info->devType == 25204 || info->devType == 24204) {
+            _num_ports = 1;
+        }
     }
 
     report("Chip Revision:   %X\n", info->devRev);
 
     // GUIDS:
-    report("GUID Des:        Node             Port1            ");
+    // TODO: Handle case where devtype not found.
+    bool ib_dev; 
+    bool eth_dev;
+    SetDevFlags(info->devType, ib_dev, eth_dev);
+
+    char* mac_indent = "";
+    if (ib_dev) {
+        //report("GUID Des:        Node             Port1            ");        
+        report("Description:     Node             Port1            ");
+
+        if (_num_ports > 1)
+            report("Port2            ");
+        report( "Sys image\n");
+        
+        report("GUIDs:           ");
+        for (u_int32_t i=0; i < GUIDS; i++) {
+            if (i != 2 || _num_ports > 1 ) 
+                report(GUID_FORMAT " ", info->guids[i].h, info->guids[i].l);
+        }
+        mac_indent = "                 ";
+    }
 
-    if (_num_ports > 1)
-        report("Port2            ");
-    report( "Sys image\n");
+    // MACS:
+    if (eth_dev) {
+        if (info->guidNum == MAX_GUIDS) {
+            if (!ib_dev) 
+                report("Description:%s     Port1            Port2\n", mac_indent);
+            else 
+                printf("\n");
+            
+            report("MACs:    %s        ", mac_indent);
+            for (u_int32_t i=GUIDS; i < MAX_GUIDS; i++) {
+                report(MAC_FORMAT "     ", info->guids[i].h, info->guids[i].l);
+            }
 
-    report("GUIDs:           ");
-    for (u_int32_t i=0; i < GUIDS; i++) {
-        if (i != 2 || _num_ports > 1 ) 
-            report(GUID_FORMAT " ", info->guids[i].h, info->guids[i].l);
+            for (u_int32_t i=GUIDS; i < MAX_GUIDS; i++) {
+                u_int64_t mac = (((u_int64_t)info->guids[i].h) << 32) | info->guids[GUIDS + i].l;
+                if (!info->blankGuids && !CheckMac(mac)) {
+                    if (i==GUIDS) printf("\n\n");
+                    printf("Warning: Bad mac address (" MAC_FORMAT "): %s\n", info->guids[i].h, info->guids[i].l, err());
+                }
+            }
+        } else {
+            printf("\nWarning: Can not get MAC addrerss: Expecting %d entries in guid section, got %d. Probably an old FW image. Please update.\n",
+                   MAX_GUIDS,
+                   info->guidNum);
+        }
     }
 
-
     // VSD, PSID
+    if (!info->infoOffs[II_VsdVendorId] || info->vsdVendorId == MELLANOX_VENDOR_ID) {
+        report("\nBoard ID:        %s", info->vsd);
+        if (info->psid[0])
+            report(" (%s)\n", info->psid);
+        else
+            report("\n");
 
-    report("\nBoard ID:        %s", info->vsd);
-    if (info->psid[0])
-        report(" (%s)\n", info->psid);
-    else
-        report("\n");
-
-    report("VSD:             %s\n", info->vsd);
-    report("PSID:            %s\n", info->psid);
-    
-    return true;
-}
-
-////////////////////////////////////////////////////////////////////////
-//                                                                    //
-// ****************************************************************** //
-//       Detect Device type and return matching Flash interface       //
-// ****************************************************************** //
-//                                                                    //
-////////////////////////////////////////////////////////////////////////
-
-Flash* get_serial_flash(mfile* mf) {
-
-    enum {
-        CR_FLASH_TYPE   = 0xf0810,
-        BO_FLASH_TYPE_S = 10,
-        BO_FLASH_TYPE_E = 11,
-    };
-
-    enum FlashType {
-        FT_LPC    = 0,
-        FT_SPI    = 1,
-        FT_XBUS   = 2,
-        FT_EEPROM = 3
-    };
-
-    char* flash_type_str[] = {"LPC", "SPI", "XBUS", "EEPROM"};
-
-    u_int32_ba strap_option;
-    u_int32_t  flash_type;
-
-    if (mread4(mf, CR_FLASH_TYPE, &strap_option) != 4) return false;
-
-    flash_type = strap_option.range(BO_FLASH_TYPE_E, BO_FLASH_TYPE_S);
-
-
-    switch (flash_type) {
-    case FT_SPI:
-        return new SpiFlash;
-    case FT_LPC:
-    case FT_XBUS:
-    case FT_EEPROM:
-        printf("*** ERROR *** flash of type %s not supported.\n",
-               flash_type_str[flash_type]);
+        report("VSD:             %s\n", info->vsd);
+        report("PSID:            %s\n", info->psid);
+    } else {
+        report("\n\nWarning: Not a Mellanox FW image (vendor_id = 0x%04x). VSD and PSID are not displayed.\n\n", info->vsdVendorId);
     }
 
-    return NULL;
-
-}
-
-
-Flash* get_flash(const char* device, u_int32_t& num_ports) {
-    Flash* f = NULL;
-
-    //
-    // Check device ID. Allocate flash accordingly
-    //
-
-    u_int32_t dev_id;
-
-    mfile* mf = mopen(device);
-    if (!mf) {
-        printf("*** ERROR *** Can't open %s: %s\n", device,  strerror(errno));
-        return NULL;
+    if (info->infoOffs[II_IsGa]) {
+        if (!info->isGa) {
+            report("BOARD GA:        no\n");
+        }
     }
 
-    if (mread4(mf, 0xf0014, &dev_id) != 4) return false;
-
-    dev_id &= 0xffff;
-
-    //printf("-D- read dev id: %d\n", dev_id);
-
-    switch (dev_id) {
-    case 23108:
-    case 25208:
-        num_ports = 2;
-        f = new ParallelFlash;
-        break;
-
-    case 24204:
-    case 25204:
-        num_ports = 1;
-        f = get_serial_flash(mf);
-        break;
-
-    case 0xffff:
-        printf("*** ERROR *** Read a corrupted device id (0x%x). Probably HW/PCI access problem\n", dev_id);
-    default:
-        printf("*** ERROR *** Device type %d not supported.\n", dev_id);
+    if (info->blankGuids) {
+        report("\nWarning: GUIDs%s values and their CRC are not set.\n",
+               IsCntx() ? "/MACs" : "");
     }
-
-    mclose(mf);
-
-    return f;
+    return true;
 }
 
-
 ////////////////////////////////////////////////////////////////////////
 //                                                                    //
 // ****************************************************************** //
@@ -5130,7 +3965,8 @@ void usage(const char *sname, bool full = false)
     "\n"
     "               FLINT - FLash INTerface\n"
     "\n"
-    "InfiniHost flash memory operations.\n"
+    "FW (firmware) burning and flash memory operations tool for\n"
+    "Mellanox Infiniband HCAs and Ethernet NIC cards.\n"
     "\n"
     "Usage:\n"
     "------\n"
@@ -5140,18 +3976,19 @@ void usage(const char *sname, bool full = false)
     "\n"
     "Switches summary:\n"
     "-----------------\n"
-    "    -bsn <BSN>         - Mellanox Board Serial Number (BSN).\n"
-    "                         Valid BSN format is:\n"
-    "                                 MTxxxxx[-]R[xx]ddmmyy-nnn[-cc]\n"
-    "                         Commands affected: burn\n"
-    "\n"
-    "    -crc               - Print CRC after each section when verify.\n"
+//    "    -bsn <BSN>         - Mellanox Board Serial Number (BSN).\n"
+//    "                         Valid BSN format is:\n"
+//    "                                 MTxxxxx[-]R[xx]ddmmyy-nnn[-cc]\n"
+//    "                         Commands affected: burn\n"
+//    "\n"
+//    "    -crc               - Print out each section CRC.\n"
+//    "                         Commands affected: verify\n"
     "\n"
     "    -d[evice] <device> - Device flash is connected to.\n"
     "                         Commands affected: all\n"
     "\n"
-    "    -guid <GUID>       - Base value for up to 4 GUIDs, which\n"
-    "                         are automatically assigned the\n"
+    "    -guid <GUID>       - GUID base value. 4 GUIDs\n"
+    "                         are automatically assigned to the\n"
     "                         following values:\n"
     "\n"
     "                         guid   -> node GUID\n"
@@ -5159,27 +3996,51 @@ void usage(const char *sname, bool full = false)
     "                         guid+2 -> port2\n"
     "                         guid+3 -> system image GUID.\n"
     "\n"
-    "                         Note: For a single port HCA, port2 guid is assigned\n"
-    "                               with the 'guid + 2' value, although it is ignored.\n"
+    "                         Note: port2 guid will be assigned even for a"
+    "                         single port HCA - The HCA ignores this value.\n"
     "\n"
-    "                         Commands affected: burn\n"
+    "                         Commands affected: burn, sg\n"
     "\n"
     "    -guids <GUIDs...>  - 4 GUIDs must be specified here.\n"
     "                         The specified GUIDs are assigned\n"
     "                         the following values, repectively:\n"
     "                         node, port1, port2 and system image GUID.\n"
     "\n"
-    "                         Note: For a single port HCA, port2 guid must be\n"
-    "                               specified (can be set to 0x0), although it is ignored.\n"
+    "                         Note: port2 guid must be specified even for a\n"
+    "                         single port HCA - The HCA ignores this value.\n"
+    "                         It can be set to 0x0.\n"
+    "\n"
+    "                         Commands affected: burn, sg\n"
+    "\n"
+    "    -mac <MAC>         - MAC address base value. 2 MACs\n"
+    "                         are automatically assigned to the\n"
+    "                         following values:\n"
+    "\n"
+    "                         mac    -> port1\n"
+    "                         mac+1  -> port2\n"
+    "\n"
+    "                         Commands affected: burn, sg\n"
+    "\n"
+    "    -macs <MACs...>    - 2 MACs must be specified here.\n"
+    "                         The specified MACs are assigned\n"
+    "                         to port1, port2, repectively.\n"
+    "\n"
+    "                         Commands affected: burn, sg\n"
+    "                         Note: -mac/-macs flags are applicable only for Mellanox\n"
+    "                               Technologies ethernet products.\n"
+    "\n"
+    "    -blank_guids       - Burn the image with blank GUIDs and MACs (where\n"
+    "                         applicable). These values can be set later using\n"
+    "                         the \"sg\" command (see details below).\n"
     "\n"
     "                         Commands affected: burn\n"
     "\n"
-    "    -clear_semaphore   - Force clear of the flash semaphore on the device.\n"
-    "                         This flag should come BEFORE the -d[evice] flag in the command line.\n"
+    "    -clear_semaphore   - Force clear the flash semaphore on the device.\n"
     "                         No command is allowed when this flag is used.\n"
-    "                         NOTE: Using this flag may result in an unstable behavior and flash image\n"
-    "                               corruption if the device or another flash application is currently\n"
-    "                               using the flash. Handle with care.\n"
+    "                         NOTE: May result in system instability or flash\n"
+    "                               corruption if the device or another\n"
+    "                               application is currently using the flash.\n"
+    "                               Exercise caution.\n"
     "\n"
     "    -h[elp]            - Prints this message and exits\n"
     "    -hh                - Prints extended command help\n"
@@ -5187,10 +4048,10 @@ void usage(const char *sname, bool full = false)
     "    -i[mage] <image>   - Binary image file.\n"
     "                         Commands affected: burn, verify\n"
     "\n"
-    "    -nofs              - Burn image not in failsafe manner.\n"
+    "    -nofs              - Burn image in a non failsafe manner.\n"
     "\n"
     "    -skip_is           - Allow burning the FW image without updating the invariant sector,\n"
-    "                         to insures failsafe burning even when invariant sector difference is detected.\n"
+    "                         to ensure failsafe burning even when an invariant sector difference is detected.\n"
     "                         See the specific FW release notes for more details.\n"
     "\n"
     "    -byte_mode         - Shift address when accessing flash internal registers. May\n"
@@ -5209,11 +4070,13 @@ void usage(const char *sname, bool full = false)
     "                         \"yes\" to all questions.\n"
     "                         Commands affected: all\n"
     "\n"
-    "    -vsd  <string>     - Write this string, of up to 208 characters, to VSD when burn.\n"
+    "    -no                - Non interactive mode - assume answer\n"
+    "                         \"no\" to all questions.\n"
+    "                         Commands affected: all\n"
     "\n"
-    "    -psid <PSID>       - Write the Parameter Set ID (PSID) string to PS-ID field (last 16 bytes of VSD) when burn.\n"
+    "    -vsd  <string>     - Write this string, of up to 208 characters, to VSD when burn.\n"
     "\n"
-    "    -use_image_ps      - Burn vsd as appears in the given image - don't keep existing vsd on flash.\n"
+    "    -use_image_ps      - Burn vsd as appears in the given image - do not keep existing VSD on flash.\n"
     "                         Commands affected: burn\n"
     "\n"
     "    -dual_image        - Make the burn process burn two images on flash (previously default algorithm). Current\n" 
@@ -5224,18 +4087,29 @@ void usage(const char *sname, bool full = false)
     "\n"
     "Commands summary (use -hh flag for full commands description):\n"
     "-----------------\n"
-    "    b[urn]   - Burn flash\n"
-    "    e[rase]  - Erase sector\n"
-    "    q[uery]  - Query misc. flash/FW characteristics\n"
-    "    rw       - Read one dword from flash\n"
-    "    v[erify] - Verify entire flash\n"
-    "    ww       - Write one dword to flash\n"
-    "    bb       - Burn Block - Burns the given image as is. No checks are done.\n"
-    "    wwne     - Write one dword to flash without sector erase\n"
-    "    wbne     - Write a data block to flash without sector erase\n"
-    "    rb       - Read  a data block from flash\n"
-    "    ri       - Read the fw image on the flash.\n"
-    "    dc       - Dump Configuration: print fw configuration file for the given image.\n"
+    "  b[urn]              - Burn flash\n"
+    "  q[uery]             - Query misc. flash/firmware characteristics\n"
+    "  v[erify]            - Verify entire flash\n"
+    "  bb                  - Burn Block - Burns the given image as is. \n"
+    "                        No checks are done.\n"
+    "  sg                  - Set Guids\n"
+    "  ri       <out-file> - Read the fw image on the flash.\n"
+    "  dc       [out-file] - Dump Configuration: print fw configuration file\n"
+    "                        for the given image.\n"
+    "  e[rase]  <addr>     - Erase sector\n"
+    "  rw       <addr>     - Read one dword from flash\n"
+    "  ww       <addr> < data> \n"
+    "                      - Write one dword to flash\n"
+    "  wwne     <addr>     - Write one dword to flash without sector erase\n"
+    "  wbne     <addr> <size> <data ...> \n"
+    "                      - Write a data block to flash without sector erase\n"
+    "  rb       <addr> <size> [out-file]\n"
+    "                      - Read  a data block from flash\n"
+    "\n"
+    "  Return values:\n"
+    "  0 - Successful completion\n"
+    "  1 - An error has occurred\n"
+    "  7 - For burn command - FW already updated - burn was aborted.\n"
     "\n";
 
     const char* full_descr =
@@ -5267,6 +4141,25 @@ void usage(const char *sname, bool full = false)
     "        " FLINT_NAME " -d " DEV_MST_EXAMPLE1 " -i image1.bin bb\n"
     "\n"
     "\n"
+    "* sg\n"
+    "  Set GUIDs/MACs in the given device.\n"
+    "  Use -guid(s) and -mac(s) flags to set the desired values.\n"
+    "  This command is applicable only for images with blank (0xff)\n"
+    "  GUIDs/MACs values and crc, I.E., that were burnt or generated\n"
+    "  using -blank_guids flag.\n"
+    "  The sg command  is used in production to apply GUIDs/MACs values\n"
+    "  to cards that were pre-burnt with blank guids. It is not meant for\n"
+    "  use in field\n"
+    "\n"
+    "    Command:\n"
+    "        sg\n"
+    "    Parameters:\n"
+    "        None\n"
+    "    Examples:\n"
+    "        " FLINT_NAME " -d " DEV_MST_EXAMPLE1 " -guid 0x0002c9000100d050 sg\n"
+    "\n"
+    "\n"
     "* Erase sector.\n"
     "  Erases a sector that contains specified address.\n"
     "\n"
@@ -5379,7 +4272,7 @@ void usage(const char *sname, bool full = false)
     "    Example:\n"
     "        " FLINT_NAME " -d " DEV_MST_EXAMPLE1 " wbne 0x10000 12 0x30000 0x76800 0x5a445a44\n"
     "\n"
-    "* Print (to screen or to a file) the firmware configuration text file used by the image generation process.\n"
+    "* Print (to screen or to a file) the FW configuration text file used by the image generation process.\n"
     "  This command would fail if the image does not contain a FW configuration section. Existence of this\n"
     "  section depends on the version of the image generation tool.\n"
     "\n"
@@ -5450,7 +4343,7 @@ void TerminationHandler (int signum)
         report("\n Received signal %d. Cleaning up ...", signum);
         fflush(stdout);
         sleep(1); // let erase sector end
-        g_flash->wait_ready("Process termination");
+        //g_flash->wait_ready("Process termination");
 
         g_flash->close();
         report(" Done.\n");
@@ -5472,8 +4365,11 @@ enum CommandInput {
 enum CommandType {
     CMD_UNKNOWN,
     CMD_BURN,
+    CMD_SET_GUIDS,
     CMD_BURN_BLOCK,
     CMD_QUERY,
+    CMD_QUERY_ROM,
+    CMD_QUERY_FORCE,
     CMD_VERIFY,
     CMD_READ_WORD,
     CMD_READ_BLOCK,
@@ -5501,7 +4397,10 @@ struct CommandInfo {
 CommandInfo const g_commands[] = {
     { CMD_BURN           , "burn"  ,false , 0 , CI_IMG_AND_DEV , ""},
     { CMD_BURN_BLOCK     , "bb"    ,true  , 0 , CI_IMG_AND_DEV , ""},
+    { CMD_SET_GUIDS      , "sg"    ,true  , 0 , CI_DEV_ONLY    , ""},
+    { CMD_QUERY_FORCE    , "qf"    ,true  , 0 , CI_IMG_OR_DEV  , ""},
     { CMD_QUERY          , "query" ,false , 0 , CI_IMG_OR_DEV  , ""},
+    { CMD_QUERY_ROM      , "qrom"  ,true  , 0 , CI_IMG_ONLY    , ""},
     { CMD_VERIFY         , "verify",false , 0 , CI_IMG_OR_DEV  , ""},            
     { CMD_READ_WORD      , "rw"    ,true  , 1 , CI_DEV_ONLY    , ""},      
     { CMD_READ_BLOCK     , "rb"    ,true  , 3 , CI_IMG_OR_DEV  , ""},
@@ -5620,6 +4519,11 @@ bool CheckMaxCmdArguments(CommandType cmd, int numArgs) {
 }
 
 ////////////////////////////////////////////////////////////////////////
+// 
+
+// Return values:
+#define RC_FW_ALREADY_UPDATED 7
+
 #define NEXTS(s) do {                                        \
     if (++i >= ac)                                           \
     {                                                        \
@@ -5642,7 +4546,8 @@ int main(int ac, char *av[])
     char         *image_fname=0, *device=0;
     bool         clear_semaphore  = false;
     bool         silent           = false; 
-    bool         guids_specified  = false; 
+    bool         guids_specified  = false;
+    bool         macs_specified   = false;
     bool         burn_failsafe    = true;
     bool         use_image_ps     = false;
     bool         single_image_burn = true;
@@ -5651,7 +4556,7 @@ int main(int ac, char *av[])
 
     char         *user_vsd=0;
     char         *user_psid=0;
-    guid_t       user_guids[Operations::GUIDS];
+    guid_t       user_guids[Operations::MAX_GUIDS];
     int                 rc = 0;
 
     CommandType cmd = CMD_UNKNOWN;
@@ -5661,6 +4566,12 @@ int main(int ac, char *av[])
 
     Operations            ops;
 
+    FBase*      fbase     = NULL;
+    char*       cmdTarget = NULL;
+    char*       cmdAccess = NULL;
+
+    bool        cntx_image  = false;
+    bool        cntx_device = false;
     //
     // Map termination signal handlers
     //
@@ -5674,6 +4585,9 @@ int main(int ac, char *av[])
         rc =  1; goto done; 
     }
 
+    // Init with FFs - for ConnectX if only MAC or GUID is specified
+    memset(user_guids, 0xff, sizeof(user_guids));
+
     // Go thru command line options
     for (i=1; i < ac; i++) {
         //
@@ -5694,8 +4608,6 @@ int main(int ac, char *av[])
                 NEXTS("-device");
                 device = av[i];
 
-                //f.reset( get_flash(device) );
-
             } else if (!strcmp(av[i], "-v") || !strcmp(av[i], "-vv")) {
                 printf("%s: %s .",
                        av[0],
@@ -5764,16 +4676,39 @@ int main(int ac, char *av[])
                 }
                 guids_specified = true;
             } else if (!strcmp(av[i], "-guids")) {
-                NEXTS("-guids");
+                if (i + 4 >= ac) {
+                    printf("Exactly four GUIDs must be specified.\n");
+                    rc =  1; goto done;                    
+                }
+                i++;
                 for (int j=0; j<Operations::GUIDS; j++) {
-                    GETGUID(av[i], &user_guids[j]);
-                    if (++i >= ac) {
-                        printf("Exactly four GUIDs must be specified.\n");
-                        rc =  1; goto done; 
-                    }
+                    GETGUID(av[i+j], &user_guids[j]);
                 }
-                i--;
+                i += 3;
                 guids_specified = true;
+
+            } else if (!strcmp(av[i], "-mac")) {
+                NEXTS("-mac");
+                GETGUID(av[i], &user_guids[Operations::GUIDS]);
+                for (int i=1; i < Operations::MACS ; i++) {
+                    u_int64_t g=user_guids[Operations::GUIDS].h;
+                    g=(g<<32) | user_guids[Operations::GUIDS].l;
+                    g += i;
+                    user_guids[Operations::GUIDS + i].h = (u_int32_t)(g>>32);
+                    user_guids[Operations::GUIDS + i].l = (u_int32_t)g;
+                }
+                macs_specified = true;
+            } else if (!strcmp(av[i], "-macs")) {
+                if (i + 2 >= ac) {
+                    printf("Exactly two MACs must be specified.\n");
+                    rc =  1; goto done;                    
+                }
+                i++;
+                for (int j=0; j<Operations::MACS; j++) {
+                    GETGUID(av[i+j], &user_guids[Operations::GUIDS+j]);
+                }
+                i += 1;
+                macs_specified = true;
             } else if (!strncmp(av[i], "-silent", switchLen))
                 silent = true;
             else if (!strncmp(av[i], "-use_image_ps", 2))
@@ -5782,17 +4717,23 @@ int main(int ac, char *av[])
                 burn_failsafe = false;
             else if (!strcmp(av[i], "-skip_is"))
                 ops.SetAllowSkipIs(true);
+            else if (!strcmp(av[i], "-striped_image"))
+                ops.SetCntxStripedImage(true);
+            else if (!strcmp(av[i], "-blank_guids"))
+                ops.SetBurnBlankGuids(true);
             else if (!strncmp(av[i], "-yes", switchLen))
                 _assume_yes = true;
+            else if (!strcmp(av[i], "-no"))
+                _assume_no = true;
             else if (!strcmp(av[i], "-byte_mode"))
-                ParallelFlash::set_byte_mode(true);
+                Flash::_byte_mode = true;
 
             else if (!strncmp(av[i], "-hh", 3) ||  !strncmp(av[i], "--hh", 4)) {
                 usage(av[0], true);
-                rc =  1; goto done; 
+                rc =  0; goto done; 
             } else if (!strncmp(av[i], "-help", switchLen) ||  !strncmp(av[i], "--h", 3)) {
                 usage(av[0]);
-                rc =  1; goto done; 
+                rc =  0; goto done; 
             } else {
                 printf("*** ERROR *** Invalid switch \"%s\" is specified.\n", av[i]);
                 rc =  1; goto done; 
@@ -5804,6 +4745,21 @@ int main(int ac, char *av[])
         }
     }
 
+    if (_assume_yes && _assume_no) {
+        printf("*** ERROR *** -yes and -no options can not be specified together.\n");
+        rc =  1; goto done; 
+    }
+
+    if (ops.GetBurnBlankGuids() && (guids_specified || macs_specified)) {
+        const char* flag = "-guid(s)";
+
+        if (macs_specified && !guids_specified) {
+            flag = "-mac(s)";
+        }
+
+        printf("*** ERROR *** -blank_guids and %s options can not be specified together.\n", flag);
+        rc =  1; goto done; 
+    }
 
     //
     // Commands
@@ -5846,34 +4802,30 @@ int main(int ac, char *av[])
         rc = 1; goto done;
     }
 
-
-    FBase*      fbase;
-    char*       cmdTarget;
-    char*       cmdAccess;
-
     if (device) {
         // Open the device
-
-        u_int32_t num_ports;
-        auto_ptr<Flash>       tmp( get_flash(device, num_ports));
+        auto_ptr<Flash>       tmp(new Flash);
         f = tmp;
         
         if (f.get() == NULL) {
-            printf("*** ERROR *** Can't get flash type using device %s\n", device);
+            printf("*** ERROR *** Memory allocation failed\n");
             rc =  1; goto done; 
         }
-        
-        ops.SetNumPorts(num_ports);
-        
+               
         g_flash = f.get();
         if (!f->open(device, clear_semaphore)) {
-            printf("*** ERROR *** Can't open %s: %s\n", device, f->err());
+            printf("*** ERROR *** Can not open %s: %s\n", device, f->err());
             rc =  1; goto done; 
         }
 
+        ops.SetNumPorts(f->get_port_num());
+
         cmdTarget = "Flash";
         cmdAccess = device;
         fbase     = f.get();
+
+        // Connectx Mode:
+        cntx_device = ops.CheckIsCntx(*fbase);
     }
 
     if (image_fname) {
@@ -5885,11 +4837,12 @@ int main(int ac, char *av[])
         cmdTarget = "Image file";
         cmdAccess = image_fname;
         fbase     = &fim;
-    }
-
-
 
+        // Connectx Mode:
+        cntx_image = ops.CheckIsCntx(fim);
+    }
 
+    ops.SetCntxMode(cntx_image || cntx_device);
 
     switch (cmd) {
     case CMD_BURN:
@@ -5899,31 +4852,27 @@ int main(int ac, char *av[])
         //
         // BURN
         //
-
+        Operations::ImageInfo fileInfo;
+        Operations::ImageInfo flashInfo;
         bool burn_block = (cmd == CMD_BURN_BLOCK);
                 
         if (!burn_block) {
-            // Make checks and replace vsd/guids.
-            
-            Operations::ImageInfo fileInfo;
-            Operations::ImageInfo flashInfo;
-            
+
+            if (cntx_image != cntx_device) {
+                printf("*** ERROR *** The given device is %sa ConnectX HCA, but the given image file %s a ConnectX FW image\n", 
+                       cntx_device ? ""         : "not ",
+                       cntx_image  ? "contains" : "does not contain");
+                rc =  1; goto done; 
+            }
+
+            // Make checks and replace vsd/guids.    
             bool old_silent = _silent;
             _silent = true;
-            if (!ops.Verify(fim) || !ops.QueryAll(fim, &fileInfo)) {
+            if (!ops.Verify(fim, &fileInfo) || !ops.QueryAll(fim, &fileInfo)) {
                 printf("*** ERROR *** %s: Not a valid image file (%s)\n", image_fname, ops.err());
                 rc =  1; goto done; 
             }
-            
-            // Check that the flash sector size is well defined in the image
-            if (fim.get_sector_size() && (fim.get_sector_size() != f->get_sector_size())) {
-                printf("*** ERROR *** Flash sector size(0x%x) differs from sector size defined in the image (0x%x).\n"
-                       "              This means that the given FW file is not configured to work with the burnt HCA board type.\n",
-                       f->get_sector_size(),
-                       fim.get_sector_size());
-                rc =  1; goto done;
-            }
-            
+                        
             // Get GUID and VSD info from flash
             
             bool read_guids = true;
@@ -5932,89 +4881,199 @@ int main(int ac, char *av[])
             // Flash query (unlike image file query) does not have to
             // pass. E.G. blank flash and the user supplies the needed data (guids, vsd).
             
-            bool flash_query_res = ops.QueryAll(*f, &flashInfo);
+            bool flash_query_res = ops.Verify(*f, &flashInfo) && ops.QueryAll(*f, &flashInfo);
+
+            bool ib_dev;
+            bool eth_dev;
+
+            ops.SetDevFlags(fileInfo.devType, ib_dev,eth_dev);
 
-            if (guids_specified)
-                read_guids = false;
-            
             if ((user_vsd && user_psid) || use_image_ps)
                 read_ps = false;
 
+            if (ops.GetBurnBlankGuids() ||
+                (guids_specified && ib_dev) || 
+                (macs_specified && eth_dev))
+                read_guids = false;
+
             if (read_guids && !flash_query_res) {
-                if (read_guids && !flashInfo.imageOk) {
+                char* missing_info;
+                char* missing_flags;
+
+                if (ib_dev && eth_dev) {
+                    missing_info  = "GUIDs / MACs";
+                    missing_flags = "-guid(s) / -mac(s)";
+                } else if (ib_dev) {
+                    missing_info  = "GUIDs";
+                    missing_flags = "-guid(s)";
+                } else {
+                    missing_info  = "MACs";
+                    missing_flags = "-mac(s)";
+                }
 
+                if (read_guids && !flashInfo.imageOk) {
                     printf("\n");
-                    printf("*** ERROR *** Can't extract GUIDS info from flash. "
-                           "Please specify GUIDs (using command line flags -guid(s) ). \n");
+                    printf("*** ERROR *** Can not extract %s info from flash. "
+                           "Please specify %s (using command line flags %s ). \n", missing_info, missing_info, missing_flags);
                 }
-                
+
                 if (burn_failsafe) {
-                    printf("              Can't burn in a failsafe mode. Please use \"-nofs\" flag to burn in a none failsafe mode.\n");
+                    printf("              Can not burn in a failsafe mode.\n");
+                   printf("              If you want to burn in non failsafe mode, use the \"-nofs\" switch.\n");
                 }
                 rc =  1; goto done;
             }
 
-            if (read_ps && !flashInfo.psOk) {
+            // Patch GUIDS
+            if  (guids_specified || macs_specified) {
+                if (!ops.CheckGuidsFlags(fileInfo.devType, ib_dev, guids_specified, eth_dev, macs_specified)) {
+                    printf("*** ERROR *** %s\n", ops.err());
+                    rc =  1; goto done;
+                }
+
+                if (!ops.patchGUIDs(fim,
+                                    &fileInfo,
+                                    ib_dev,
+                                    eth_dev,
+                                    guids_specified,
+                                    macs_specified,
+                                    user_guids,
+                                    flashInfo.imageOk ? flashInfo.guids : NULL,
+                                    flashInfo.guidNum,
+                                    true)) {
+                    rc =  1; goto done;
+                }
+            } else {
+                if (!ops.patchGUIDs(fim,
+                                    &fileInfo,
+                                    ib_dev,
+                                    eth_dev,
+                                    false,
+                                    false,
+                                    NULL,  
+                                    flashInfo.guids,
+                                    flashInfo.guidNum,
+                                    false)) {
+                    rc =  1; goto done;
+                }
+            }
+
+            if (burn_failsafe && (!fileInfo.isFailsafe || !flashInfo.isFailsafe)) {
+                printf("*** ERROR *** Failsafe burn failed: FW image in the %s is non failsafe.\n",         fileInfo.isFailsafe ? "flash" : "given file");
+                printf("              It is impossible to burn %sa non failsafe image in a failsafe mode.\n", fileInfo.isFailsafe ? "over " : "");
+                printf("              If you want to burn in non failsafe mode, use the \"-nofs\" switch.\n");
+                rc =  1; goto done; 
+            }
+
+            if (!user_vsd && !(flashInfo.psOk || (flashInfo.infoOffs[Operations::II_PSID] && 
+                                                flashInfo.infoOffs[Operations::II_VSD]  ))) {
                 printf("\n");
                 if (burn_failsafe) {
 
-                    printf("*** ERROR *** Can't extract VSD/PSID info from flash.\n"
-                           "              Can't burn in a failsafe mode. Please use \"-nofs\" flag to burn in a none failsafe mode.\n");
+                    printf("*** ERROR *** Can not extract VSD/PSID info from flash.\n"
+                           "              Can not burn in a failsafe mode. Please use \"-nofs\" flag to burn in a non failsafe mode.\n");
                     rc =  1; goto done;
                 }  else {
-                    printf("*** WARNING *** Can't extract VSD/PSID info from flash.\n\n"
+                    printf("*** WARNING *** Can not extract VSD/PSID info from flash.\n\n"
                            "    To use a specific VSD, abort and re-burn specifying the\n"
                            "    needed info (using command line flags -vsd / -use_image_ps).\n"
                            "    You can also continue burn using blank VSD.\n");
                 
-                    if (!ops.ask_user("\n    Continue burn using a blank VSD ? (y/n) ")) {
+                    if (!ops.ask_user()) {
                         rc =  1; goto done;
                     }
                 }
             }
 
             // Print FW versions:
+            printf("\n");
             printf("    Current FW version on flash:  ");
-            if (flashInfo.infoFound[Operations::II_FwVersion]) {
+            if (flashInfo.infoOffs[Operations::II_FwVersion]) {
                 printf("%d.%d.%d\n", flashInfo.fwVer[0], flashInfo.fwVer[1], flashInfo.fwVer[2]);  
             } else {
                 printf("N/A\n");
             }
             
             printf("    New FW version:               ");
-            if (fileInfo.infoFound[Operations::II_FwVersion]) {
+            if (fileInfo.infoOffs[Operations::II_FwVersion]) {
                 printf("%d.%d.%d\n", fileInfo.fwVer[0], fileInfo.fwVer[1], fileInfo.fwVer[2]);  
             } else {
                 printf("N/A\n");
             }
 
+            bool updateRequired = true;
 
-            // Patch GUIDS
-            if (guids_specified) {
-                if (!ops.patchGUIDs(fim, 
-                                    user_guids, 
-                                    flashInfo.imageOk ? flashInfo.guids : NULL, 
-                                    isatty(0) != 0)) {
-                    rc =  1; goto done;
+            if (fileInfo.infoOffs[Operations::II_FwVersion]  && 
+                flashInfo.infoOffs[Operations::II_FwVersion]) {
+
+                updateRequired = ops.FwVerLessThan(flashInfo.fwVer,fileInfo.fwVer);
+            }
+
+            if (!updateRequired) {
+                printf("\n    Note: The new FW version is not newer than the current FW version on flash.\n");
+                if (! ops.ask_user()) {
+                    rc =  RC_FW_ALREADY_UPDATED; goto done;
                 }
-            } else {
-                if (!ops.patchGUIDs(fim, NULL,       flashInfo.guids, false)) {
+            }
+
+            if (fileInfo.infoOffs[Operations::II_IsGa] && !fileInfo.isGa) {
+                printf("\n    Note: You are attempting to burn a pre-production FW image.\n");
+                if (! ops.ask_user()){
                     rc =  1; goto done;
                 }
             }
 
-            if (_image_is_full && !use_image_ps)
-                if (!ops.patchVSD(fim, 
-                                  user_vsd, 
-                                  user_psid,
-                                  flashInfo.psOk ? flashInfo.vsd  : NULL,
-                                  flashInfo.psOk ? flashInfo.psid : NULL,
-                                  fileInfo.psid )) {
+            if (!use_image_ps) {
+                if (fileInfo.psOk || (ops.IsCntx() && fileInfo.infoOffs[Operations::II_VSD])) {
+                    if (!ops.patchVSD(fim, 
+                                      &fileInfo,
+                                      user_vsd, 
+                                      user_psid,
+                                      flashInfo.vsd,
+                                      NULL,
+                                      fileInfo.psid )) {
+                        rc =  1; goto done;
+                    }
+                }
+            }
+
+            // Check PSID and ib -> eth change.
+
+            if (fileInfo.infoOffs[Operations::II_PSID]  && 
+                flashInfo.infoOffs[Operations::II_PSID] && 
+                strncmp( fileInfo.psid, flashInfo.psid, PSID_LEN)) {
+                if (ops.IsCntx() && 
+                    (!ib_dev && eth_dev) && 
+                    flashInfo.infoOffs[Operations::II_DeviceType] &&
+                     ops.CntxIsIb(flashInfo.devType) &&
+                    !ops.CntxIsEth(flashInfo.devType)) {
+
+                    printf("\n    You are about to replace FW image type from IB to ETH image.\n");
+                } else {
+                    printf("\n    You are about to replace current PSID on flash - \"%s\" with a different PSID - \"%s\".\n"
+                           "    Note: It is highly recommended not to change the PSID.\n",
+                           flashInfo.psid, 
+                           fileInfo.psid);
+                }
+
+                if (! ops.ask_user()){
+                    rc =  1; goto done;
+                }
+            }           
+
+            // Check exp rom:
+            if (!fileInfo.expRomFound && flashInfo.expRomFound) {
+                printf("\n    Expansion-ROM mismatch: \n"
+                       "    Current FW on flash contains an expansion-ROM.\n"
+                       "    The new FW image does not contain an expansion-ROM\n");
+
+                if (! ops.ask_user()){
                     rc =  1; goto done;
                 }
+            }
+
+            _silent = old_silent;
 
-                _silent = old_silent;
         } else {
             // BURN BLOCK:
             burn_failsafe = false;
@@ -6022,27 +5081,30 @@ int main(int ac, char *av[])
 
         // Burn it
         if (burn_failsafe) {
-            // Failsafe burn
-            if (!_image_is_full) {
-                printf("*** ERROR *** Failsafe burn failed: FW Image on flash is short.\n");
-                printf("It is impossible to burn a short image in a failsafe mode.\n");
-                printf("If you want to burn in non failsafe mode, use the \"-nofs\" switch.\n");
-                rc =  1; goto done; 
+            // FS burn
+            bool ret;
+            if (ops.IsCntx()) {
+                ret = ops.CntxFailSafeBurn(*f,
+                                           fim,
+                                           !silent,
+                                           &flashInfo,
+                                           &fileInfo,
+                                           false);
+                                           
+            } else {
+                ret = ops.FailSafeBurn(*f, 
+                                       fim,
+                                       !silent,
+                                       single_image_burn);
             }
 
-            // FS burn
-            if (!ops.FailSafe_burn(*f, 
-                               fim.getBuf(), 
-                               fim.getBufLength(),
-                               single_image_burn,
-                               !silent)) {
+            if (!ret) {
                if (f->err()) {
                    // The error is in flash access:
                    printf("*** ERROR *** Flash access failed during burn: %s\n", f->err());
                } else {
                    // operation/ algorithm error:
-                   printf("*** ERROR *** Failsafe burn failed: %s\n", ops.err());  
-                   printf("If you want to burn in non failsafe mode, use the \"-nofs\" switch.\n");
+                   printf("*** ERROR *** Failsafe burn error: %s\n", ops.err());  
                }
                 rc =  1; goto done; 
             }
@@ -6057,17 +5119,30 @@ int main(int ac, char *av[])
                 printf("Block burn: The given image will be burnt as is. No fields (such\n");
                 printf("as GUIDS,VSD) are taken from current image on flash.\n");
             }
-            printf("Burn process will not be failsafe. No checks are performed.\n");
-            printf("ALL flash, including Invariant Sector will be overwritten.\n");
-            printf("If this process fails computer may remain in unoperatable state.\n");
+            printf("Burn process will not be failsafe. No checks will be performed.\n");
 
-            if (!ops.ask_user("\nAre you sure ? (y/n) [n] : ")) {
+            printf("ALL flash, including the Invariant Sector will be overwritten.\n");
+            printf("If this process fails, computer may remain in an inoperable state.\n");
+
+            if (!ops.ask_user()) {
                 rc =  1; goto done;
            }
 
             // Non FS burn
-            if (!ops.write_image(*f, 0, fim.getBuf(), fim.getBufLength(),
-                             !silent)) {
+            bool ret;
+            if (ops.IsCntx() && !burn_block) {
+                ret = ops.CntxFailSafeBurn(*f,
+                                           fim,
+                                           !silent,
+                                           &flashInfo,
+                                           &fileInfo,
+                                           true); // Allow nofs
+            } else {
+                ret = ops.write_image(*f, 0, fim.getBuf(), fim.getBufLength(),!silent);
+            }
+
+        
+            if (!ret) {
                 report("\n");
                 printf("*** ERROR *** Non failsafe burn failed: %s\n", ops.err());
                 rc =  1; goto done; 
@@ -6076,6 +5151,90 @@ int main(int ac, char *av[])
         }
     }
     break;
+    case CMD_SET_GUIDS:
+    {
+        Operations::ImageInfo info;
+        u_int32_t guid_sect_addr[2] = {0};
+        u_int32_t i;
+
+        if (ops.IsCntx()) {
+            _silent       = true;
+            if (!ops.VerifyCntx(*fbase, &info, false, true)) {
+                printf("\n*** ERROR *** Can not set GUIDs: %s. \n", ops.err());
+                rc =  1; goto done;
+            }
+            _silent = false;
+        }
+
+        if (!ops.QueryAll(*fbase, &info)) {
+            printf("*** ERROR *** Can not set GUIDs: %s query (%s) failed: %s\n", cmdTarget , cmdAccess, ops.err());
+            rc =  1; goto done;
+        }
+
+        if (!info.blankGuids) {
+            printf("\n*** ERROR *** Can not set GUIDs: Guids are already set.\n");
+            rc =  1; goto done;
+        }
+
+        bool ib_dev;
+        bool eth_dev;
+
+        ops.SetDevFlags(info.devType, ib_dev, eth_dev);
+
+        if (macs_specified || guids_specified) {
+            if (!ops.CheckGuidsFlags(info.devType, ib_dev, guids_specified, eth_dev, macs_specified)) {
+                printf("*** ERROR *** %s\n", ops.err());
+                rc =  1; goto done;
+            }
+        } else {
+            char* missing_info;
+            char* missing_flags;
+
+            if (ib_dev && eth_dev) {
+                missing_info  = "GUIDs / MACs";
+                missing_flags = "-guid(s) / -mac(s)";
+            } else if (ib_dev) {
+                missing_info  = "GUIDs";
+                missing_flags = "-guid(s)";
+            } else {
+                missing_info  = "MACs";
+                missing_flags = "-mac(s)";
+            }
+
+            printf("\n");
+            printf("*** ERROR *** For set_guids command, "
+                   "Please specify %s (using command line flags %s ). \n", missing_info, missing_flags);
+
+            rc = 1; goto done;
+        }
+
+        if (ops.IsCntx() || !info.isFailsafe) {
+            guid_sect_addr[0] = info.guidPtr;
+        } else {
+            int addr_idx = 0;
+            for (i = 0; i < 2; i++) {
+                if (info.allImgStart[i]) {
+                    guid_sect_addr[addr_idx] = info.allImgStart[i] + info.guidPtr;
+                    addr_idx++;
+                }
+            }
+        }
+
+        for (i = 0; i < 2 && guid_sect_addr[i]; i++ ) {
+            u_int32_t guid_sect[Operations::MAX_GUIDS*2 + 5]; // Save room for header + crc
+            
+            if (!f->read(guid_sect_addr[i] - 16 , guid_sect, 16)) {
+                printf("*** ERROR *** Failed to read guids section - flash read error (%s)\n", fbase->err());
+            }
+            
+            ops.patchGUIDsSection (guid_sect, 16, user_guids, info.guidNum);
+    
+            if (!f->write(guid_sect_addr[i], guid_sect + 4 , info.guidNum * 8 + 4, true)) {
+                printf("*** ERROR *** Guids set failed - flash write error (%s)\n", fbase->err());
+            }
+        }
+    }
+    break;
 
     case CMD_ERASE_SECT:
     {    
@@ -6102,10 +5261,31 @@ int main(int ac, char *av[])
     }
     break;
 
+    case CMD_QUERY_FORCE:
     case CMD_QUERY:
     {
         // QUERY
         Operations::ImageInfo info;
+        bool imageOk;
+        bool checkValidImage = false;
+
+        _silent       = true;
+        if (cmd == CMD_QUERY_FORCE) {
+            printf("\n*** WARNING *** Running query without verifying the image first. Results may be undefined.\n"); 
+            if (ops.IsCntx()) {
+                imageOk = ops.VerifyCntx(*fbase, &info, false, true);
+                checkValidImage = true;
+            }
+        } else {
+            imageOk = ops.Verify(*fbase, &info);
+            checkValidImage = true;
+        }
+        _silent = false;
+
+        if (checkValidImage && !imageOk) {
+            printf("\n*** ERROR *** %s query (%s) failed. Not a valid image.\n", cmdTarget , cmdAccess);
+            rc =  1; goto done; 
+        }
 
         if (!ops.QueryAll(*fbase, &info)) {
                 printf("*** ERROR *** %s query (%s) failed: %s\n", cmdTarget , cmdAccess, ops.err());
@@ -6116,6 +5296,17 @@ int main(int ac, char *av[])
     }
     break;
 
+    case CMD_QUERY_ROM:
+    {
+        Operations::ImageInfo info;
+        if (!ops.LoadAsExpRom(*fbase)    || 
+            !ops.GetExpRomVersion(&info) || 
+            !ops.DisplayExpRomInfo(&info)) {
+            printf("*** ERROR *** %s rom query (%s) failed: %s\n", cmdTarget , cmdAccess, ops.err());
+            rc =  1; goto done;
+        }
+    }
+    break;
     case CMD_READ_BLOCK:
     {
         // READ BLOCK
@@ -6144,7 +5335,7 @@ int main(int ac, char *av[])
         data = new u_int8_t[length];
 
         // Output file
-        FILE*  fh;
+        FILE*  fh = NULL;
 
         if (i + 2 == ac)
             to_file = true;
@@ -6152,7 +5343,7 @@ int main(int ac, char *av[])
         if (to_file) {
             NEXTC("<OUT_FILENAME>", "rb");
             if ((fh = fopen(av[i], "wb")) == NULL) {
-                fprintf(stderr, "Can't open ");
+                fprintf(stderr, "Can not open ");
                 perror(av[i]);
                 rc =  1; goto done; 
             }
@@ -6212,11 +5403,15 @@ int main(int ac, char *av[])
     case CMD_VERIFY:
     {
         // VERIFY
-        if (!ops.Verify(*fbase)) {
-            printf("\n*** ERROR *** FW Image verification failed. AN HCA DEVICE CAN NOT BOOT FROM THIS IMAGE.\n");
+        if (!ops.Verify(*fbase, NULL, true)) {
+            printf("\n*** ERROR *** FW image verification failed");
+            if (ops.err()) {
+                printf(": %s", ops.err());
+            }
+            printf(". AN HCA DEVICE CAN NOT BOOT FROM THIS IMAGE.\n");
             rc =  1; goto done; 
         } else {
-            printf("\nFW Image verification succeeded. Image is OK.\n\n");
+            printf("\nFW image verification succeeded. Image is bootable.\n\n");
         }
     }
     break;
@@ -6250,7 +5445,9 @@ int main(int ac, char *av[])
         NEXTC("<OUT_FILENAME>", "ri");
         img_file = av[i];
 
-        ops.Verify(*f);
+        // For ConnectX, read only a single image. For other HCAs, try to read both images, since
+        // the distributed image binary file also contains both images.
+        ops.Verify(*f, NULL, !ops.IsCntx());
         
         //printf("Last addr: 0x%08x\n", ops._last_image_addr);
 
@@ -6260,7 +5457,7 @@ int main(int ac, char *av[])
         FILE* fh;
 
         if ((fh = fopen(av[i], "wb")) == NULL) {
-            fprintf(stderr, "Can't open ");
+            fprintf(stderr, "Can not open ");
             perror(av[i]);
             rc =  1; goto done; 
         }
@@ -6289,20 +5486,12 @@ int main(int ac, char *av[])
         u_int32_t    addr;
         char         *endp;
 
-        char*        fname;
-        
-        // Device
-        if (!device) {
-            printf("For wb command \"-device\" switch must be specified.\n");
-            rc =  1; goto done; 
-        }
-
         // Input file
         FImage fim;
 
         NEXTC("<IN_FILENAME>", "wb");
 
-        fname = av[i];
+        image_fname = av[i];
 
         // Address
         NEXTC("<ADDR>", "wb");
@@ -6386,7 +5575,7 @@ int main(int ac, char *av[])
             //printf("-D- writing: %08x : %08x\n", addr + w*4 , data_vec[w]);
         }
 
-        if (!f->write(addr, &data_vec[0], size, true, false)) {
+        if (!f->write(addr, &data_vec[0], size, true)) {
             printf("*** ERROR *** Flash write failed: %s\n", f->err());
             rc =  1; goto done; 
         }
@@ -6414,7 +5603,7 @@ int main(int ac, char *av[])
             rc =  1; goto done; 
         }
 
-        if (!f->write(addr, &data, 4, true, false)) {
+        if (!f->write(addr, &data, 4, true)) {
             printf("*** ERROR *** Flash write failed: %s\n", f->err());
             rc =  1; goto done; 
         }
@@ -6423,7 +5612,7 @@ int main(int ac, char *av[])
 
     case CMD_CFI:
     {
-        if (!f->print_cfi_info()) {
+        if (!f->print_attr() || !f->print_attr_old_format()) {
             printf("*** ERROR *** Cfi query failed: %s\n", f->err());
             rc =  1; goto done; 
         }
diff --git a/tools/flint/user/mflash.c b/tools/flint/user/mflash.c
new file mode 100644 (file)
index 0000000..bf74a50
--- /dev/null
@@ -0,0 +1,2308 @@
+/*
+ * Copyright (c) 2006 Mellanox Technologies.  All rights reserved.
+ *
+ * This software is available to you under the OpenIB.org BSD license
+ * below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifdef IRISC
+#include <tavor_mac.h>
+#define __cpu_to_be32(val) (val)
+#define NULL 0
+#else
+
+#include <mtcr.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <errno.h>
+#include <string.h>
+
+
+// Bit Slicing macros
+#define ONES32(size)                    ((size)?(0xffffffff>>(32-(size))):0)
+#define MASK32(offset,size)             (ONES32(size)<<(offset))
+
+#define EXTRACT_C(source,offset,size)   ((((unsigned)(source))>>(offset)) & ONES32(size))
+#define EXTRACT(src,start,len)          (((len)==32)?(src):EXTRACT_C(src,start,len))
+
+#define MERGE_C(rsrc1,rsrc2,start,len)  ((((rsrc2)<<(start)) & (MASK32((start),(len)))) | ((rsrc1) & (~MASK32((start),(len)))))
+#define MERGE(rsrc1,rsrc2,start,len)    (((len)==32)?(rsrc2):MERGE_C(rsrc1,rsrc2,start,len))
+
+
+#ifndef __WIN__
+
+#if defined __DJGPP__
+//
+// DJGPP - GCC PORT TO MS DOS
+//
+
+#include <netinet/in.h>
+#include <unistd.h>
+
+#define bswap_32(x) ntohl(x)
+
+
+#else // Linux GCC
+
+#include <byteswap.h>
+#include <endian.h>
+
+#endif // __DJGPP__
+
+#define SWAPL(l) bswap_32(l)
+
+#ifndef __cpu_to_be32
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+#define __cpu_to_be32(val) SWAPL(val)
+#else
+#define __cpu_to_be32(val) (val)
+#endif
+#endif
+
+#else // __WIN__
+
+//
+// Windows (Under DDK)
+//
+
+#include <io.h>
+#include <Winsock2.h>
+#define SWAPL(l) ntohl(l)
+#define inline __inline
+#define __cpu_to_be32(val) SWAPL(val) // Win is only run on LE CPUS
+
+#endif // __WIN__
+#endif
+
+#include "mflash.h"
+
+#ifndef __be32_to_cpu
+#define __be32_to_cpu(val)  __cpu_to_be32(val)
+#endif
+
+#ifndef zero
+#define zero 0
+#endif
+
+//
+// Mellanox Technologies LTD Devices naming/abbreviation convention in this file:
+//
+// InfiniHost       ihst
+// InfiniHostIII    ihst3
+// InfiniHostIIILx  ih3lx
+// ConnectX         cntx
+//
+
+
+/*
+*  Low level operations
+*/
+
+/* Flash Functions: */
+//typedef struct mflash *mflash;
+
+typedef int (*f_mf_lock)      (mflash* mfl);
+typedef int (*f_mf_unlock)    (mflash* mfl);
+
+typedef int (*f_mf_set_bank)  (mflash* mfl, u_int32_t bank);
+typedef int (*f_mf_read)      (mflash* mfl, u_int32_t addr, u_int32_t len, u_int8_t* data);
+typedef int (*f_mf_write)     (mflash* mfl, u_int32_t addr, u_int32_t len, u_int8_t* data);
+typedef int (*f_mf_erase_sect)(mflash* mfl, u_int32_t addr);
+typedef int (*f_mf_reset)     (mflash* mfl);
+
+typedef int (*f_st_spi_status)(mflash* mfl, u_int8_t op_type, u_int8_t* status);
+
+// This is an interface function when running in IRISC
+int mf_open_fw(mflash* mfl);
+
+//
+// mflash struct
+//
+#ifndef IRISC
+#define MFLASH_ERR_STR_SIZE 1024
+#else
+#define MFLASH_ERR_STR_SIZE    4
+#endif
+
+struct mflash {
+#ifndef IRISC
+    mfile*      mf;
+#endif
+
+    // Functions:
+    f_mf_lock       f_lock;
+    f_mf_unlock     f_unlock;
+    f_mf_set_bank   f_set_bank;
+
+    f_mf_read       f_read;
+    f_mf_write      f_write;
+    f_mf_write      f_write_blk;   // write and write_block have the same signateure, but theyr'e not the same func !
+    f_mf_read       f_read_blk;    // read  and read_block have the same signateure, but theyr'e not the same func !
+    f_mf_erase_sect f_erase_sect;
+    f_mf_reset      f_reset;
+
+    // Relevant for SPI flash (InfiniHostIIILx, ConnectX) only.
+    f_st_spi_status f_spi_status;
+
+    int             curr_bank;
+    int             is_locked;
+
+    flash_attr      attr;
+
+    int             opts[MFO_LAST];
+    char            last_err_str[MFLASH_ERR_STR_SIZE];
+};
+
+
+// NOTE: This macro returns ... not nice.
+#define CHECK_RC(rc) do {if (rc) return rc;} while(0)
+
+#define ARRSIZE(arr) (sizeof(arr)/sizeof(arr[0]))
+
+#ifdef ESS
+#define MREAD4(offs, val)  do {*val=READ_WORD(offs);} while (0)
+#define MWRITE4(offs, val) do {WRITE_WORD(offs, val);} while (0)
+#else
+#define MREAD4(offs, val)  do { if (mread4 (mfl->mf, offs, val) != 4) { \
+                                  fprintf(stderr, "-E- Cr read (0x%08x) failed: %s(%d)\n", (u_int32_t)(offs), strerror(errno), (u_int32_t)errno); \
+                                 exit(2); } \
+                                  } while (0)
+                                  
+#define MWRITE4(offs, val) do { if (mwrite4(mfl->mf, offs, val) != 4) { \
+                                  fprintf(stderr, "-E- Cr write (0x%08x, 0x%08x) failed: %s(%d)\n", (u_int32_t)(offs), (u_int32_t)(val), strerror(errno), (u_int32_t)errno); \
+                                 mclose(mfl->mf); exit(2); } \
+                                  } while (0)
+#endif
+
+// Constants:
+
+// General:
+#define GPIO_DIR_L       0xf008c
+#define GPIO_POL_L       0xf0094
+#define GPIO_MOD_L       0xf009c
+#define GPIO_DAT_L       0xf0084
+#define GPIO_DATACLEAR_L 0xf00d4
+#define GPIO_DATASET_L   0xf00dc
+
+#define SEMAP63          0xf03fc
+
+// InfiniHost specific
+#define IDLE           0
+#define READ4          (1<<29)
+#define WRITE1         (2<<29)
+
+#define CR_FLASH       0xf01a4
+#define ADDR_MSK       0x7ffffUL
+#define CMD_MASK       0xe0000000UL
+
+#define CPUMODE_MSK    0xc0000000UL
+#define CPUMODE_SHIFT  30
+#define CPUMODE        0xf0150
+
+enum FlashConstant {
+    FLASH_CMD_CNT  = 5000,    // Number of reads till flash cmd is zeroed
+    ERASE_DELAY    = 200000,  // Delay between reads when wating for sector erase
+    ERASE_CNT      = 1000000,      // Maximal number of reads when wating for sector erase
+    READ_CNT_FAST  = 50000,        // Number of fast reads after write byte
+    READ_CNT_SLOW  = 50,      // Number of slow reads after write byte
+    READ_DELAY     = 100000,  // Delay between slow reads after write byte
+    WR_REPORT_FAST = 256,     // Report frequency when write (fast interfaces)
+    WR_REPORT_SLOW = 4,       // Report frequency when write (slow interfaces)
+    RD_REPORT_FAST = 4096,    // Report frequency when read (fast interfaces)
+    RD_REPORT_SLOW = 64,      // Report frequency when read (slow interfaces)
+    GPIO_SEM_TRIES = 1024 ,   // Number of tries to obtain a GPIO sem.
+    MAX_WRITE_BUFFER_SIZE = 32// Max buffer size for buffer write devices
+};
+
+
+enum IntelFlashCommand {
+    FC_ReadID      = 0x90,
+    FC_Read        = 0xFF,
+    FC_Erase       = 0x20,
+    FC_Confirm     = 0xD0,
+    FC_Clear       = 0x50,
+    FC_Write       = 0x40,
+    FC_LoadPB      = 0xE0,
+    FC_PBWrite     = 0x0C,
+    FC_Status      = 0x70,
+    FC_Suspend     = 0xB0,
+    FC_Resume      = 0xD0,
+    FC_ReadESR     = 0x71,
+    FC_QueryCFI    = 0x98,
+    FC_SCSErase    = 0x28,
+    FC_SCSWrite    = 0xE8
+};
+
+enum IntelFlashStatus {
+    FS_Ready       = 0x80,
+    FS_Suspended   = 0x40,
+    FS_Error       = 0x3E,
+    FS_BlockError  = 0x3F
+};
+
+// InfiniHost funcs:
+int ihst_flash_init      (mflash* mfl);
+
+int fill_attr_cfi         (mflash* mfl);
+
+int ihst_write_internal  (mflash* mfl, u_int32_t addr, u_int8_t data);
+int ihst_fill_attr       (mflash* mfl);
+
+int ihst_flash_read      (mflash* mfl, u_int32_t addr, u_int32_t len, u_int8_t* data);
+int ihst_flash_lock      (mflash* mfl);
+
+//static inline
+//int ihst_set_bank        (mflash* mfl, u_int32_t addr);
+static u_int32_t log2up (u_int32_t in) {
+    u_int32_t i;
+    for (i = 0; i < 32; i++) {
+        if (in <= (u_int32_t)(1 << i))
+            break;
+    }
+
+    return i;
+}
+
+static inline
+int set_bank(mflash* mfl, u_int32_t addr) {
+    int bank = addr >> mfl->attr.log2_bank_size;
+
+    if (mfl->curr_bank != bank) {
+        mfl->curr_bank = bank;
+        return mfl->f_set_bank(mfl, bank);
+    }
+
+    return MFE_OK;
+}
+
+// amd parallel flash funcs:
+int amd_flash_reset       (mflash* mfl);
+int amd_flash_write       (mflash* mfl, u_int32_t addr, u_int32_t len, u_int8_t* data);
+int amd_flash_erase_sect  (mflash* mfl, u_int32_t addr);
+
+// intel parallel flash funcs:
+int intel_flash_reset      (mflash* mfl);
+//int intel_flash_byte_write (mflash* mfl, u_int32_t addr, u_int32_t len, u_int8_t* data);
+int intel_flash_block_write(mflash* mfl, u_int32_t addr, u_int32_t len, u_int8_t* data);
+int intel_flash_erase_sect (mflash* mfl, u_int32_t addr);
+
+
+// InfiniHostIIILx SPI interface:
+int ih3lx_flash_init      (mflash* mfl);
+
+int ih3lx_fill_attr       (mflash* mfl);
+
+int ih3lx_flash_read      (mflash* mfl, u_int32_t addr, u_int32_t len, u_int8_t* data);
+
+int ih3lx_st_spi_reset          (mflash* mfl);
+int ih3lx_st_spi_erase_sect     (mflash* mfl, u_int32_t addr);
+
+
+// ConnectX SPI interface:
+int cntx_flash_init      (mflash* mfl);
+
+int cntx_fill_attr       (mflash* mfl);
+
+int cntx_flash_read      (mflash* mfl, u_int32_t addr, u_int32_t len, u_int8_t* data);
+int cntx_flash_lock      (mflash* mfl);
+
+int cntx_st_spi_reset          (mflash* mfl);
+int cntx_st_spi_erase_sect     (mflash* mfl, u_int32_t addr);
+
+int cntx_st_spi_block_write_ex (mflash*   mfl, 
+                                u_int32_t blk_addr, 
+                                u_int32_t blk_size, 
+                                u_int8_t* data, 
+                                u_int8_t  is_first, 
+                                u_int8_t  is_last);
+
+int cntx_st_spi_block_read_ex  (mflash*   mfl, 
+                                u_int32_t blk_addr, 
+                                u_int32_t blk_size, 
+                                u_int8_t* data, 
+                                u_int8_t  is_first, 
+                                u_int8_t  is_last);
+
+int my_memset(void* dst, u_int8_t data, u_int32_t len) {
+    u_int32_t i;
+    u_int8_t* bytes = (u_int8_t*)dst;
+
+    for (i = 0; i < len;  i++) {
+        bytes[i] = data;
+    }
+
+    return 0;
+}
+
+int my_memcpy(void* dst, void* src, u_int32_t len) {
+    u_int32_t i;
+    u_int8_t* dbytes = (u_int8_t*)dst;
+    u_int8_t* sbytes = (u_int8_t*)src;
+
+    for (i = 0; i < len;  i++) {
+        dbytes[i] = sbytes[i];
+    }
+
+    return 0;
+}
+
+#if 0 // TODO: Currently the polling is in max throttle - no sleep is done
+int usleep(u_int32_t usecs) {
+    u_int32_t i;
+    u_int32_t stub = 0;
+    // TODO: Some more intelegent /accurate way to do it, or remove T.Os completely.
+    u_int32_t loops = (usecs << 9);
+    for (i = 0; i < loops; i++) {
+        stub++;
+    }
+
+    return stub;
+}
+#endif
+
+int write_chunks   (mflash* mfl, u_int32_t addr, u_int32_t len, u_int8_t* data) {
+    int       rc;
+    u_int8_t  *p         = (u_int8_t *)data;
+
+    int       all_ffs;
+
+    // TODO - Check MAX_WRITE_BUFFER_SIZE against block_size in open (or here)
+    u_int8_t  tmp_buff[MAX_WRITE_BUFFER_SIZE];
+
+    if (!mfl) {
+        return MFE_BAD_PARAMS;
+    }
+
+    //printf("-D- write_chunks(addr=%x, size=%x)\n", addr, len); 
+
+    while (len) {
+        u_int32_t i;
+        u_int32_t prefix_pad_size = 0;
+        u_int32_t suffix_pad_size = 0;
+
+        u_int32_t block_size = mfl->attr.block_write;
+        u_int32_t block_mask = ~(block_size - 1 );
+
+        u_int32_t block_addr = addr & block_mask;
+        u_int32_t data_size  = block_size;
+
+        u_int8_t* block_data = p;
+
+        //
+        // To keep things simple, we always write full blocks.
+        // (there's an option to write partial buffer, but Intel reference code always
+        // writes full buffer, with pads if needed. I do the same ...)
+        //
+        // The First and last cycles (can be the same one) may not be block aligned.
+        // If this is the case, copy the block data to a 0xFF padded temp buffer (writing 0xFF 
+        // does not effect the flash)
+        //
+
+        prefix_pad_size = addr - block_addr;
+
+        if ((addr & block_mask) == ((addr + len) & block_mask)) {
+            suffix_pad_size = block_size - ((addr + len) % block_size);
+        }
+
+        if (suffix_pad_size || prefix_pad_size) {
+            my_memset(tmp_buff, 0xff, block_size);
+
+            data_size -= prefix_pad_size;
+            data_size -= suffix_pad_size;
+
+            my_memcpy(tmp_buff + prefix_pad_size, p , data_size);
+
+            block_data = tmp_buff;
+        } else if (mfl->attr.page_write) {
+            // current write is aligned to block size, 
+            // write data to next page, or to last full block (if we're in the last block in a page)
+            u_int32_t page_mask             = ~(mfl->attr.page_write - 1);
+            u_int32_t next_page_addr        = (addr + mfl->attr.page_write + 1) & page_mask;
+            u_int32_t size_to_page_boundary = next_page_addr - addr;
+            if (len > size_to_page_boundary) {
+                block_size = size_to_page_boundary;
+            } else {
+                block_size = len & block_mask;
+            }
+            data_size = block_size;
+        }
+
+        //
+        // Check to see if there's something to do
+        //
+        all_ffs = 1;
+        for (i = 0; i < block_size ; i++) {
+            if (block_data[i] != 0xff) {
+                all_ffs = 0;
+                break;
+            }
+        }
+
+        if (!all_ffs) {
+            rc = mfl->f_write_blk(mfl, block_addr, block_size, block_data); CHECK_RC(rc);
+
+            if (mfl->opts[MFO_NO_VERIFY]) {
+                u_int8_t verify_buffer[MAX_WRITE_BUFFER_SIZE];
+                rc = mfl->f_reset(mfl);                                CHECK_RC(rc);
+                rc = mfl->f_read(mfl, addr, data_size, verify_buffer); CHECK_RC(rc);
+
+                // Verify data
+                for (i = 0 ; i < data_size ; i++) {
+                    if (verify_buffer[i] != block_data[i + prefix_pad_size]) {
+                        //return _f.errmsg("Write verification failed. Addr %08x - exp:%02x act:%02x\n",
+                        //       addr + i,
+                        //       block_data[i + prefix_pad_size] ,
+                        //       verify_buffer[i]);
+                        return MFE_VERIFY_ERROR;
+                    }
+                }
+            }
+        }
+
+        //
+        // loop advance
+        //
+
+        addr += data_size;
+        p    += data_size;
+        len  -= data_size;
+    }
+
+    rc = mfl->f_reset(mfl); CHECK_RC(rc);
+
+    return MFE_OK;
+}
+
+static int apply_mx_flash_workaround(flash_attr* attr) {
+    if (attr->num_erase_blocks == 1) {
+        attr->sector_size = attr->erase_block[0].sector_size;
+    } else if (attr->num_erase_blocks == 2 &&
+               attr->erase_block[0].sector_size == 0x2000 &&
+               attr->erase_block[0].sector_mask == 0xffffe000 &&
+               attr->erase_block[0].num_sectors == 8 &&
+               attr->erase_block[1].sector_size == 0x10000 &&
+               attr->erase_block[1].sector_mask == 0xffff0000 &&
+               attr->erase_block[1].num_sectors == 63) {
+
+        attr->sector_size = attr->erase_block[1].sector_size;
+    } else {
+        return MFE_UNSUPPORTED_FLASH_TOPOLOGY;
+    }
+
+    return MFE_OK;
+}
+
+
+int ihst_init_gpios(mflash* mfl) {
+    u_int32_t dir;
+    u_int32_t pol;
+    u_int32_t mod;
+    u_int32_t word;
+    u_int32_t data;
+
+    // Save old values
+    MREAD4(GPIO_DIR_L, &dir);
+    MREAD4(GPIO_POL_L, &pol);
+    MREAD4(GPIO_MOD_L, &mod);
+    MREAD4(GPIO_DAT_L, &data);
+
+    // Set Direction=1, Polarity=0, Mode=0 for 3 GPIO lower bits
+    dir = dir | 0x70;
+    pol = pol & ~0x70;
+    mod = mod & ~0x70;
+
+    MWRITE4(GPIO_DIR_L, dir);
+    MWRITE4(GPIO_POL_L, pol);
+    MWRITE4(GPIO_MOD_L, mod);
+
+    // Set CPUMODE
+    MREAD4(CPUMODE, &word);
+    word &= ~CPUMODE_MSK;
+    word |= 1 << CPUMODE_SHIFT;
+    MWRITE4(CPUMODE, word);
+
+    return MFE_OK;
+}
+
+int ihst_set_bank(mflash* mfl, u_int32_t bank) {
+    MWRITE4(GPIO_DATACLEAR_L, 0x70);
+    MWRITE4(GPIO_DATASET_L, (bank << 4) & 0x70);
+    return MFE_OK;
+}
+
+int ihst_flash_init(mflash* mfl) {
+    int rc;
+
+    mfl->f_read           = ihst_flash_read;
+    mfl->f_lock           = ihst_flash_lock;
+    mfl->f_set_bank       = ihst_set_bank;
+
+    rc = mfl->f_lock(mfl);
+    if (!mfl->opts[MFO_IGNORE_SEM_LOCK]) {
+        CHECK_RC(rc);
+    } else {
+        mfl->is_locked = 1;
+    }
+
+    rc = ihst_init_gpios(mfl);  CHECK_RC(rc);
+
+    mfl->attr.log2_bank_size = 19;
+    mfl->attr.bank_size = (1 << mfl->attr.log2_bank_size);
+
+    rc = fill_attr_cfi(mfl); CHECK_RC(rc);
+    rc = apply_mx_flash_workaround(&mfl->attr);  CHECK_RC(rc);
+
+    if        (mfl->attr.command_set == CS_AMD) {
+        mfl->f_reset      = amd_flash_reset;
+        mfl->f_write      = amd_flash_write;
+        mfl->f_erase_sect = amd_flash_erase_sect;
+    } else if (mfl->attr.command_set == CS_INTEL) {
+        mfl->f_reset      = intel_flash_reset;
+        //mfl->f_write      = intel_flash_blk_write;
+        mfl->f_write_blk  = intel_flash_block_write;
+        mfl->f_write      = write_chunks;
+        mfl->f_erase_sect = intel_flash_erase_sect;
+    } else {
+        return MFE_UNSUPPORTED_FLASH_TYPE;
+    }
+
+    rc = mfl->f_reset(mfl); CHECK_RC(rc);
+
+    return MFE_OK;
+}
+
+
+static u_int16_t extract_word   ( const u_int8_t* pb)  {
+    u_int16_t ret = *pb | ((*(pb + 1)) << 8);
+    return ret;
+}
+
+int fill_attr_cfi(mflash* mfl) {
+    int rc;
+
+    #define TOTAL_QUERY_SIZE 0x100
+
+    u_int8_t fwp[TOTAL_QUERY_SIZE]; // flash window
+
+    int temp=0, i=0;
+
+    // Initial house-cleaning
+    my_memset(fwp, 0xff, TOTAL_QUERY_SIZE);
+
+    for (i=0; i < (int)ARRSIZE(mfl->attr.erase_block); i++) {
+        mfl->attr.erase_block[i].sector_size = 0;
+        mfl->attr.erase_block[i].num_sectors = 0;
+        mfl->attr.erase_block[i].sector_mask = 0;
+    }
+
+    // reset
+    rc = ihst_write_internal(mfl, 0x55, 0xff);          CHECK_RC(rc);
+
+    // CFI QUERY
+    rc = ihst_write_internal(mfl, 0x55, 0x98);          CHECK_RC(rc);
+
+    
+    rc = mfl->f_read(mfl, 0, TOTAL_QUERY_SIZE, fwp);     CHECK_RC(rc);
+
+    if (        fwp[0x10]      == 'Q' &&
+                fwp[0x11]      == 'R' &&
+                fwp[0x12]      == 'Y') {
+
+        // x8 CFI flash (AMD)
+    } else if ( fwp[0x20] == 'Q' &&
+                fwp[0x22] == 'R' &&
+                fwp[0x24] == 'Y') {
+        // x16 CFI flash worqing in x8 mode
+        
+        for (i = 0; i < TOTAL_QUERY_SIZE ; i+=2) {
+            fwp[i/2] = fwp[i];
+        }
+
+    } else {
+
+        printf(" Received CFI query from addr 0x10: [%c%c%c]\n", fwp[0x10], fwp[0x11], fwp[0x12] );
+        printf(" Received CFI query from addr 0x20: [%c%c%c]\n", fwp[0x20], fwp[0x22], fwp[0x24]);
+
+        return MFE_CFI_FAILED;
+    }
+
+    // mfl->attr.manuf_id              = fwp[query_base * 0];
+    // mfl->attr.device_id             = fwp[query_base * 1];
+
+
+    mfl->attr.command_set =          extract_word(fwp + 0x13);
+
+    temp = fwp[0x27];
+
+    mfl->attr.size = (1 << temp);
+
+    temp = fwp[0x2A];
+    if (temp != 0x00)
+        mfl->attr.block_write = (1 << temp);
+    else
+        mfl->attr.block_write = 0;
+
+    mfl->attr.num_erase_blocks = fwp[0x2C];
+
+    if (mfl->attr.num_erase_blocks > 8) {
+        printf("-E- More than 8 erase blocks (%d found)- not supported\n",
+               mfl->attr.num_erase_blocks);
+        return MFE_CFI_FAILED;
+    }
+
+//    rc = mfl->f_read(mfl,
+//                     0x2C,
+//                     4 * (mfl->attr.num_erase_blocks + 1),
+//                     fwp + 0x2C );
+//    CHECK_RC(rc);
+
+    for (i=0; i < mfl->attr.num_erase_blocks; i++) {
+        mfl->attr.erase_block[i].num_sectors = extract_word(fwp + 0x2D+(4*i)) + 1;
+
+        mfl->attr.erase_block[i].sector_size = extract_word(fwp + 0x2F+(4*i)) * 256;
+
+        mfl->attr.erase_block[i].sector_mask =
+            ~( mfl->attr.erase_block[i].sector_size - 1);
+    }
+
+    return MFE_OK;
+}
+
+
+int ihst_wait_ready(mflash* mfl, char* msg) {
+    //int rc;
+    u_int32_t cnt = 0;
+    u_int32_t cmd;
+    do {
+        // Timeout checks
+        if (++cnt > FLASH_CMD_CNT) {
+            printf("-E- Flash gateway timeout: %s", msg);
+            return MFE_TIMEOUT;
+        }
+
+        MREAD4(CR_FLASH, &cmd);
+    } while (cmd & CMD_MASK);
+
+    return MFE_OK;
+}
+
+int ihst_write_internal  (mflash* mfl, u_int32_t addr, u_int8_t data) {
+    int rc;
+
+    MWRITE4(CR_FLASH+4, data << 24);
+    MWRITE4(CR_FLASH, WRITE1 | (addr & ADDR_MSK));
+
+    rc = ihst_wait_ready(mfl, "Write"); CHECK_RC(rc);
+
+    return MFE_OK;
+}
+
+
+int ihst_flash_read4(mflash* mfl, u_int32_t addr, u_int32_t* data) {
+
+    int rc;
+    u_int32_t cmd;
+    if (addr & 0x3) {
+        return MFE_BAD_ALIGN;
+    }
+
+    rc = set_bank(mfl, addr); CHECK_RC(rc);
+
+    MWRITE4(CR_FLASH, READ4 | (addr & ADDR_MSK));
+
+    rc = ihst_wait_ready(mfl, "Read"); CHECK_RC(rc);
+
+    MREAD4(CR_FLASH+4, &cmd);
+
+    *data =  __cpu_to_be32(cmd);
+
+    return MFE_OK;
+
+}
+
+int ihst_flash_read(mflash* mfl, u_int32_t addr, u_int32_t len, u_int8_t* data) {
+    u_int32_t i;
+    int rc;
+
+    if ((len & 3) || (addr & 3)) {
+        return MFE_BAD_ALIGN;
+    }
+
+    for (i = 0; i < len; i += 4) {
+        rc = ihst_flash_read4(mfl, addr + i, (u_int32_t*) (data + i)); CHECK_RC(rc);
+    }
+
+    return MFE_OK;
+}
+
+int ihst_flash_lock(mflash* mfl) {
+
+    // Obtain GPIO Semaphore
+    u_int32_t cnt=0;
+    u_int32_t word;
+    do {
+        if (++cnt > GPIO_SEM_TRIES) {
+            //printf("-E- Can not obtain Flash semaphore");
+            return MFE_SEM_LOCKED;
+        }
+        MREAD4(SEMAP63, &word);
+    } while (word);
+
+    mfl->is_locked = 1;
+    return MFE_OK;
+}
+
+
+int ihst_flash_unlock(mflash* mfl) {
+    if (mfl->is_locked) {
+        MWRITE4(SEMAP63, 0);
+        mfl->is_locked = 0;
+    }
+
+    return MFE_OK;
+}
+
+
+
+// amd parallel flash funcs:
+int amd_flash_reset       (mflash* mfl) {
+    int rc;
+    rc = ihst_write_internal(mfl, 0x555, 0xf0); CHECK_RC(rc);
+    return MFE_OK;
+}
+
+int amd_flash_write       (mflash* mfl, u_int32_t addr, u_int32_t len, u_int8_t* data) {
+    int rc;
+    u_int32_t i;
+
+    char         *p = (char *)data;
+
+    for (i = 0; i < len; i++ , addr++) {
+        u_int32_t  word;
+        u_int8_t   act, exp;
+        int        cnt1 = 0;
+
+        rc = set_bank(mfl, addr); CHECK_RC(rc);
+
+        if ((u_int8_t)(*p) != 0xff) {
+
+            if (mfl->opts[MFO_AMD_BYTE_MODE]) {
+
+                rc = ihst_write_internal(mfl, 0xaaa, 0xaa); CHECK_RC(rc);
+                rc = ihst_write_internal(mfl, 0x555, 0x55); CHECK_RC(rc);
+                rc = ihst_write_internal(mfl, 0xaaa, 0xa0); CHECK_RC(rc);
+            } else {
+                if (!mfl->opts[MFO_AMD_UNLOCK_BYPASS]) {
+                    rc = ihst_write_internal(mfl, 0x555, 0xaa); CHECK_RC(rc);
+                    rc = ihst_write_internal(mfl, 0x2aa, 0x55); CHECK_RC(rc);
+                }
+
+                rc = ihst_write_internal(mfl, 0x555, 0xa0); CHECK_RC(rc);
+            }
+
+            rc = ihst_write_internal(mfl, addr, *p++); CHECK_RC(rc);
+
+            do {
+                // Timeout checks
+                //if (++cnt1 > READ_CNT_FAST)
+                //    usleep(READ_DELAY);
+                if (cnt1 > READ_CNT_FAST + READ_CNT_SLOW) {
+                    return MFE_WRITE_TIMEOUT;
+                }
+
+                rc = ihst_flash_read4(mfl, addr & ~3, &word); CHECK_RC(rc);
+
+                word = __be32_to_cpu(word);
+                act = (u_int8_t) ((word >> ((3 - (addr & 3)) * 8)) & 0xff);
+                exp = *(p-1) & 0xff;
+                //if (act != exp)
+                //    printf("write: %08x - exp:%02x act:%02x /%08x/\n",
+                //           addr, exp & 0xff, act & 0xff, word);
+            } while (act != exp);
+
+        } else {
+            p++;
+        }
+    }
+
+    return MFE_OK;
+}
+
+int amd_flash_erase_sect  (mflash* mfl, u_int32_t addr) {
+    u_int32_t word = 0;
+    int       cnt  = 0;
+    int       rc;
+
+    rc = set_bank(mfl, addr); CHECK_RC(rc);
+
+    // Just to insure zeroes because erase completion waits for ones
+    rc = amd_flash_write(mfl, addr,  sizeof(word), (u_int8_t*) &word); CHECK_RC(rc);
+
+    // erase sector sequence
+    if (mfl->opts[MFO_AMD_BYTE_MODE]) {
+        rc = ihst_write_internal(mfl, 0xaaa, 0xaa); CHECK_RC(rc);
+        rc = ihst_write_internal(mfl, 0x555, 0x55); CHECK_RC(rc);
+        rc = ihst_write_internal(mfl, 0xaaa, 0x80); CHECK_RC(rc);
+        rc = ihst_write_internal(mfl, 0xaaa, 0xaa); CHECK_RC(rc);
+        rc = ihst_write_internal(mfl, 0x555, 0x55); CHECK_RC(rc);
+        rc = ihst_write_internal(mfl, addr , 0x30); CHECK_RC(rc);
+    } else {
+        rc = ihst_write_internal(mfl, 0x555, 0xaa); CHECK_RC(rc);
+        rc = ihst_write_internal(mfl, 0x2aa, 0x55); CHECK_RC(rc);
+        rc = ihst_write_internal(mfl, 0x555, 0x80); CHECK_RC(rc);
+        rc = ihst_write_internal(mfl, 0x555, 0xaa); CHECK_RC(rc);
+        rc = ihst_write_internal(mfl, 0x2aa, 0x55); CHECK_RC(rc);
+        rc = ihst_write_internal(mfl, addr , 0x30); CHECK_RC(rc);
+    }
+
+    // Wait while erase completes
+    do {
+        // Timeout checks
+        if (++cnt > ERASE_CNT) {
+            return MFE_ERASE_TIMEOUT;
+        }
+        rc = ihst_flash_read4(mfl, addr, &word); CHECK_RC(rc);
+
+        //printf("erase_sector: addr:%08lx, %08x\n", addr, word);
+        //usleep(ERASE_DELAY);
+    } while (word != 0xffffffff);
+
+    return MFE_OK;
+}
+
+// intel parallel flash funcs:
+int intel_flash_reset     (mflash* mfl) {
+    int rc;
+    rc = ihst_write_internal(mfl, 0x555, FC_Read); CHECK_RC(rc);
+    return MFE_OK;
+}
+
+
+int intel_flash_byte_write  (mflash* mfl, u_int32_t addr, u_int32_t len, u_int8_t* data) {
+    int rc;
+    u_int32_t i;
+    char         *p = (char *)data;
+
+    for (i=0; i<len; i++,addr++) {
+        u_int32_t  status;
+        int        cnt1 = 0;
+
+        rc = set_bank(mfl, addr); CHECK_RC(rc);
+
+        if ((u_int8_t)(*p) != 0xff) {
+            // Write byte
+            rc = ihst_write_internal(mfl, addr, FC_Write); CHECK_RC(rc);
+            rc = ihst_write_internal(mfl, addr, *p++);     CHECK_RC(rc);
+
+            do {
+                // Timeout checks
+                //if (++cnt1 > READ_CNT_FAST)
+                //    usleep(READ_DELAY);
+                if (cnt1 > READ_CNT_FAST + READ_CNT_SLOW) {
+                    return MFE_WRITE_TIMEOUT;
+                }
+
+                rc = ihst_flash_read(mfl, addr & ~3, 4, (u_int8_t*)&status); CHECK_RC(rc);
+
+            } while ((status & FS_Ready) == 0);
+
+            if (status & FS_Error) {
+                return MFE_WRITE_ERROR;
+            }
+
+            // TODO: Check verify
+
+            /* For ESS: verify is done by the test.
+            if (!noverify) {
+                u_int32_t word;
+                if (!reset())
+                    return false;
+                if (!_f.read(addr & ~3, &word))
+                    return false;
+
+                word = __be32_to_cpu(word);
+                act = (u_int8_t) ((word >> ((3 - (addr & 3)) * 8)) & 0xff);
+                exp = *(p-1) & 0xff;
+
+                if (act != exp) {
+                    printf("write: %08x - exp:%02x act:%02x /%08x/\n",
+                           addr, exp & 0xff, act & 0xff, word);
+
+                    return _f.errmsg("Write verification failed");
+                }
+            }
+            */
+        } else {
+            p++;
+        }
+    }
+
+    rc = intel_flash_reset(mfl); CHECK_RC(rc);
+
+    return MFE_OK;
+}
+
+
+int intel_flash_block_write(mflash* mfl, u_int32_t blk_addr, u_int32_t blk_size, u_int8_t* data) {
+    int rc;
+    u_int32_t status;
+    int cnt1 = 0;
+    u_int32_t i;
+
+    rc = set_bank(mfl, blk_addr); CHECK_RC(rc);
+
+    do {
+        // Get Write buffer
+        rc = ihst_write_internal(mfl, blk_addr, FC_SCSWrite); CHECK_RC(rc);
+
+        if (cnt1 > ((READ_CNT_FAST + READ_CNT_SLOW) * 4)) {
+            //printf("-D- status = %08x\n", status);
+            intel_flash_reset(mfl);
+            // TODO: specific err code.
+            return MFE_WRITE_ERROR;
+        }
+
+        cnt1++;
+
+        rc = mfl->f_read(mfl, blk_addr, 4, (u_int8_t*) &status); CHECK_RC(rc);
+
+    } while (!(status & FS_Ready));
+
+    if (status & FS_Error) {
+        return MFE_WRITE_ERROR;
+    }
+
+    // word count (allways full buffer, coded as cull buffer size -1)
+    rc = ihst_write_internal(mfl, blk_addr, blk_size - 1); CHECK_RC(rc);
+
+    // Write data to buffer
+    for (i = 0; i < blk_size ; i++ ) {
+        rc = ihst_write_internal(mfl, blk_addr + i, data[i]); CHECK_RC(rc);
+    }
+
+    // write confirm
+    rc = ihst_write_internal(mfl, blk_addr, FC_Confirm); CHECK_RC(rc);
+    cnt1 = 0;
+    do {
+        // Timeout checks
+        //if (++cnt1 > READ_CNT_FAST)
+        //    usleep(READ_DELAY);
+        if (cnt1 > READ_CNT_FAST + READ_CNT_SLOW) {
+            intel_flash_reset(mfl);
+            // TODO: specific err code.
+            return MFE_WRITE_TIMEOUT;
+        }
+
+        rc = mfl->f_read(mfl, blk_addr, 4, (u_int8_t*) &status); CHECK_RC(rc);
+    } while ((status & 0x80) == 0);
+
+    return MFE_OK;
+}
+
+int intel_flash_erase_sect(mflash* mfl, u_int32_t addr) {
+    int rc;
+
+    u_int32_t    status = 0;
+    int          cnt    = 0;
+
+    rc = set_bank(mfl, addr); CHECK_RC(rc);
+
+    // Just to insure zeroes because erase completion waits for ones
+    //if (!write(addr, &word, sizeof(word), true))
+    //    return false;
+
+    // Erase command
+    rc = ihst_write_internal(mfl, addr, FC_Erase);   CHECK_RC(rc);
+
+    // Erase confirm
+    rc = ihst_write_internal(mfl, addr, FC_Confirm); CHECK_RC(rc);
+
+    //usleep(ERASE_DELAY);
+
+    // Wait while erase completes
+    do {
+        // Timeout checks
+        if (++cnt > ERASE_CNT) {
+            return MFE_ERASE_TIMEOUT;
+        }
+        rc = ihst_flash_read(mfl, addr, 4, (u_int8_t*) &status); CHECK_RC(rc);
+
+        //printf("CmdSetIntel::erase_sector: addr:%08lx, %08x\n", addr, word);
+        //usleep(ERASE_DELAY);
+    } while ((status & FS_Ready) == 0);
+
+    if (status & FS_Error) {
+        // printf("-D- read %d times. status=%x\n",cnt,  status);
+        return MFE_ERASE_ERROR;
+    }
+
+    rc = intel_flash_reset(mfl); CHECK_RC(rc);
+
+    return MFE_OK;
+}
+
+
+////////////////////////////////////////
+//
+// ST SPI functions - common for InfiniHostIIILx and ConnectX
+//
+////////////////////////////////////////
+
+enum StFlashCommand {
+    SFC_SE    = 0xD8,
+    SFC_PP    = 0x02,
+    SFC_RDSR  = 0x05,
+    SFC_WREN  = 0x06,
+    SFC_READ  = 0x03,
+    SFC_RDID  = 0xF9,
+    SFC_RES   = 0xAB
+};
+
+int st_spi_fill_attr(mflash* mfl) {
+    int rc;
+
+    u_int32_t spi_size = 0;
+    u_int32_t log2spi_size = 0;
+    u_int32_t num_spis = 0;
+    u_int32_t spi_sel;
+
+    mfl->attr.block_write                = 16; // In SPI context, this is the transaction size. Max is 16.
+
+    mfl->attr.num_erase_blocks           = 1;
+
+    mfl->attr.erase_block[0].sector_size = 64 * 1024;
+    mfl->attr.erase_block[0].sector_mask = ~(mfl->attr.erase_block[0].sector_size - 1);
+
+    mfl->attr.sector_size = mfl->attr.erase_block[0].sector_size;
+
+    for (spi_sel = 0 ; spi_sel < 4 ; spi_sel++) {
+        unsigned char es; // electronic signature
+        u_int32_t cur_spi_size = 0;
+
+        rc = set_bank(mfl, spi_sel);                    CHECK_RC(rc);
+
+        rc = mfl->f_spi_status(mfl, SFC_RES, &es);      CHECK_RC(rc);
+
+        if (es >= 0x10 && es < 0x17) {
+            // Range OK:
+
+            // NOTE: This mapping between electronic signature and device size is device specific!
+            //       This mapping works for ST M25Pxx and Saifun SA25Fxxx families.
+            log2spi_size = (es + 1);
+            cur_spi_size = 1 << log2spi_size;
+
+            num_spis++;
+
+            if (spi_sel == 0) {
+                spi_size = cur_spi_size;
+            } else if (cur_spi_size != spi_size){
+                printf ("-E- SPI flash #%d of size 0x%x bytes differs in size from SPI flash #%d of size 0x%x bytes. "
+                              "All flash devices must be of the same size.",
+                              spi_sel,
+                              cur_spi_size,
+                              spi_sel - 1,
+                              spi_size);
+                return MFE_UNSUPPORTED_FLASH_TOPOLOGY;
+            }
+        } else if (es == 0xff 
+                   || es == 0
+                   ) {
+            // No spi device on this chip_select
+            break;
+        } else {
+            printf("-E- Unexpected SPI electronic signature value (0x%2x) when detecting flash size. "
+                          "Flash #%d my be defected.",
+                          es,
+                          spi_sel);
+            return MFE_UNSUPPORTED_FLASH_TOPOLOGY;
+        }
+
+        // printf("-D- %3d %08x\n", spi_sel, cur_spi_size);
+    }
+
+    if (num_spis == 0) {
+        return MFE_UNSUPPORTED_FLASH_TYPE;
+    }
+
+    mfl->attr.bank_size      = spi_size;
+    mfl->attr.size           = spi_size * num_spis;
+    mfl->attr.log2_bank_size = log2spi_size;
+
+    mfl->attr.command_set    = MCS_STSPI;
+
+    return MFE_OK;
+}
+
+int st_spi_wait_wip(mflash* mfl, u_int32_t delay, u_int32_t retrys, u_int32_t fast_retrys) {
+
+    int       rc;
+    u_int8_t  status;
+    u_int32_t cnt = 0;
+
+    delay = 0; // UNUSED FOR NOW
+
+    do {
+        if (++cnt > fast_retrys) {
+            //usleep(delay);
+        }
+        if (cnt > retrys) {
+            return MFE_WRITE_TIMEOUT;
+        }
+
+        rc = mfl->f_spi_status(mfl, SFC_RDSR, &status);
+    } while (status & 0x01);
+
+    return MFE_OK;
+}
+
+int read_chunks   (mflash* mfl, u_int32_t addr, u_int32_t len, u_int8_t* data) {
+
+    int       rc;
+    u_int8_t  *p         = (u_int8_t *)data;
+
+    // Note:
+    // Assuming read block is the same as write block size.
+    // This is true for current Mellanox devices SPI flash access implementation.
+    // Check for future devices.
+    u_int32_t block_size = mfl->attr.block_write;
+    u_int32_t block_mask;
+
+
+    // TODO - Check MAX_WRITE_BUFFER_SIZE against block_size in open (or here)
+    u_int8_t  tmp_buff[MAX_WRITE_BUFFER_SIZE];
+
+    if (!mfl) {
+        return MFE_BAD_PARAMS;
+    }
+
+    if (len < block_size) {
+        // If we're reading a small chunk, use the smallest block_size to avoid the extra reads and padding overhead
+        block_size = 4;
+    }
+
+    block_mask = ~(block_size - 1);
+
+    while (len) {
+
+        u_int32_t i;
+        u_int32_t prefix_pad_size = 0;
+        u_int32_t suffix_pad_size = 0;
+
+        u_int32_t block_addr = addr & block_mask;
+        u_int32_t data_size  = block_size;
+
+        u_int8_t* block_data = p;
+
+        //
+        // First and last cycles (can be the same one) may not be block aligned.
+        // Check the status, and copy data to a padded temp bufer if not alligned.
+        // (there's an option to write partial buffer, but Intel reference code always
+        // writes full buffer, with pads if needed. I do the dame ...)
+        //
+
+        prefix_pad_size = addr - block_addr;
+
+        if ((addr & block_mask) == ((addr + len) & block_mask)) {
+            suffix_pad_size = block_size - ((addr + len) % block_size);
+        }
+
+        if (suffix_pad_size || prefix_pad_size) {
+            // block exceeds given buffer - read to a temp bufer and 
+            // copy the required data to user's bufer.
+            data_size -= suffix_pad_size;
+            data_size -= prefix_pad_size;
+            block_data = tmp_buff;
+        }
+        
+        rc = mfl->f_read_blk(mfl, block_addr, block_size, block_data); CHECK_RC(rc);
+
+        if (suffix_pad_size || prefix_pad_size) {
+            for (i = 0; i < data_size; i++) {
+                p[i] = tmp_buff[prefix_pad_size + i];
+            }
+        }
+
+        //
+        // loop advance
+        //
+
+        addr += data_size;
+        p    += data_size;
+        len  -= data_size;
+    }
+
+    return MFE_OK;
+}
+
+
+
+
+////////////////////////////////////////
+//
+// InfiniHostIIILx spi  access functions
+//
+////////////////////////////////////////
+
+
+//
+// Relevant CR addresses, Bit offset and bit size
+//
+
+enum Ih3lxCrConstans {
+    CR_FLASH_GW     = 0xf0400,
+    CR_FLASH_ADDR   = 0xf0404,
+    CR_FLASH_DATA   = 0xf0408,
+    CR_FLASH_CS     = 0xf0418,
+
+    CR_GPIO_LOCK    = 0xf00ec,
+
+    BO_READ_OP      = 0,
+    BO_ADDR_INCR    = 1,
+
+    BO_LPC_STOP     = 3,
+    BO_SPI_NO_DATA  = 4,
+    BO_SPI_NO_ADDR  = 5,
+    BO_SPI_SPECIAL  = 6,
+
+    BO_MSIZE        = 8,
+    BS_MSIZE        = 3,
+
+    BO_STATUS       = 26,
+    BS_STATUS       = 4,
+
+    BO_BUSY         = 30,
+
+    BO_SPI_ADDR     = 0,
+    BS_SPI_ADDR     = 24,
+
+    BO_SPI_CMD      = 24,
+    BS_SPI_CMD      = 8,
+
+    BO_SPI_GPIO     = 25,
+    BS_SPI_GPIO     = 4
+};
+
+int ih3lx_wait_ready(mflash* mfl, char* msg) {
+    u_int32_t gw_cmd;
+    u_int32_t cnt = 0;
+    msg = 0; // NOT USED FOR NOW
+    do {
+        // Timeout checks
+        if (++cnt > FLASH_CMD_CNT) {
+            //return errmsg("Flash gateway timeout: %s.", msg);
+            return MFE_TIMEOUT;
+        }
+
+        MREAD4(CR_FLASH_GW, &gw_cmd);
+
+    } while (EXTRACT(gw_cmd, BO_BUSY, 1));
+
+    return MFE_OK;
+}
+
+//__inline
+int ih3lx_exec_cmd(mflash* mfl, u_int32_t gw_cmd, u_int32_t gw_addr, char* msg) {
+    gw_cmd = MERGE(gw_cmd, 1 , BO_BUSY, 1); 
+
+    MWRITE4(CR_FLASH_ADDR, gw_addr);
+    MWRITE4(CR_FLASH_GW,   gw_cmd);
+
+    return ih3lx_wait_ready(mfl, msg);
+}
+
+int ih3lx_set_bank(mflash* mfl, u_int32_t bank) {
+    u_int32_t flash_cs = 0;
+    // TODO: Check number of banks in open
+    if (bank > 3) {
+        //return errmsg("Tried to set bank to %d but %d is the is the largest bank number", bank, 3);
+        return MFE_BAD_PARAMS;
+    }
+
+    //printf("\n*** Flash::set_bank(0x%lx) : 0x%lx\n", bank, (bank >> 19) & 0x07);
+
+    flash_cs = MERGE(flash_cs, bank,30, 2);
+    MWRITE4(CR_FLASH_CS, flash_cs);
+
+    return MFE_OK;
+}
+
+int ih3lx_st_spi_get_status(mflash* mfl, u_int8_t op_type, u_int8_t* status) {
+    int rc;
+
+    u_int32_t gw_cmd  = 0;
+    u_int32_t gw_addr = 0;
+    u_int32_t flash_data;
+
+    gw_cmd = MERGE(gw_cmd, 1, BO_READ_OP,     1);
+    gw_cmd = MERGE(gw_cmd, 1, BO_SPI_SPECIAL, 1);
+    gw_cmd = MERGE(gw_cmd, 1, BO_SPI_NO_ADDR, 1);
+
+    gw_cmd = MERGE(gw_cmd, 2, BO_MSIZE  , BS_MSIZE);
+
+    gw_addr = MERGE(gw_addr, op_type, BO_SPI_CMD, BS_SPI_CMD);
+
+    rc = ih3lx_exec_cmd(mfl, gw_cmd, gw_addr, "Read id"); CHECK_RC(rc);
+
+    MREAD4(CR_FLASH_DATA, &flash_data);
+
+    // Return status reg byte is at offset 3 in word
+    *status = (u_int8_t)(flash_data & 0xff);
+
+    return MFE_OK;
+}
+
+int ih3lx_init_gpios(mflash* mfl) {
+    //
+    // Set Multi SPI CS to output and 0.
+    // Assuming 4 flashes. If there are less than 4 flashes and there's
+    // a write attempt, it will fail.
+    //
+
+    u_int32_t       num_of_spis = 4;
+    u_int32_t       spi_en      = (1 << (num_of_spis - 1 ) ) - 1;
+
+    u_int32_t       dir;
+    u_int32_t       mod;
+    u_int32_t       pol;
+
+    // No need to set the data - SPI GW CS does that in HW
+    //MREAD4(GPIO_DAT_L, &data);
+
+    MREAD4(GPIO_DIR_L, &dir);
+    MREAD4(GPIO_POL_L, &pol);
+    MREAD4(GPIO_MOD_L, &mod);
+
+    dir = MERGE(dir,  spi_en, BO_SPI_GPIO, BS_SPI_GPIO);
+    pol = MERGE(pol, ~spi_en, BO_SPI_GPIO, BS_SPI_GPIO);
+    mod = MERGE(mod, ~spi_en, BO_SPI_GPIO, BS_SPI_GPIO);
+
+    // unlock gpio
+    MWRITE4(CR_GPIO_LOCK , 0xaaaa);
+
+    MWRITE4(GPIO_DIR_L, dir);
+    MWRITE4(GPIO_POL_L, pol);
+    MWRITE4(GPIO_MOD_L, mod);
+
+    return MFE_OK;
+}
+
+
+int ih3lx_st_spi_write_enable(mflash* mfl) {
+    u_int32_t gw_cmd  = 0;
+    u_int32_t gw_addr = 0;
+
+    // Write enable:
+    gw_cmd = MERGE(gw_cmd, 1, BO_SPI_NO_ADDR ,  1);
+    gw_cmd = MERGE(gw_cmd, 1, BO_SPI_NO_DATA ,  1);
+    gw_cmd = MERGE(gw_cmd, 1, BO_SPI_SPECIAL ,  1);
+
+    gw_addr = MERGE(gw_addr, SFC_WREN, BO_SPI_CMD, BS_SPI_CMD);
+
+    return ih3lx_exec_cmd(mfl, gw_cmd, gw_addr, "WREN command");
+}
+
+int ih3lx_st_spi_block_read    (mflash* mfl, u_int32_t blk_addr, u_int32_t blk_size, u_int8_t* data) {
+    int rc;
+    u_int32_t offs;
+    u_int32_t gw_cmd   = 0;
+    u_int32_t gw_addr  = 0;
+
+    if (blk_addr & (blk_size - 1 )) {
+        //return _f.errmsg("Address should be 4-bytes aligned.");
+        return MFE_BAD_ALIGN;
+    }
+
+    if (blk_size > (u_int32_t)mfl->attr.block_write  || blk_size < 4) {
+        //return _f.errmsg("Block write of wrong block size. %d instead of %d",
+       //               block_size, (u_int32_t)_f._cfi_data.max_multi_byte_write);
+        return MFE_BAD_PARAMS;
+    }
+
+    rc = set_bank(mfl, blk_addr);        CHECK_RC(rc);
+
+    // Write the data block
+    gw_cmd = MERGE(gw_cmd, 1               , BO_READ_OP,     1);
+    gw_cmd = MERGE(gw_cmd, log2up(blk_size), BO_MSIZE,       BS_MSIZE);
+
+    gw_addr = blk_addr & ONES32(mfl->attr.log2_bank_size);
+
+    rc = ih3lx_exec_cmd(mfl, gw_cmd, gw_addr, "Read"); CHECK_RC(rc);
+
+    // Data:
+    for (offs = 0 ; offs < blk_size ; offs += 4) {
+        u_int32_t word;
+        MREAD4(CR_FLASH_DATA + offs, &word);
+        *((u_int32_t*)(data + offs))= __cpu_to_be32(word);
+    }
+
+    return MFE_OK;
+}
+
+
+int ih3lx_st_spi_reset          (mflash* mfl) {
+    mfl = NULL;
+    return MFE_OK;
+}
+
+int ih3lx_st_spi_block_write    (mflash* mfl, u_int32_t blk_addr, u_int32_t blk_size, u_int8_t* data) {
+    int rc;
+    u_int32_t offs;
+    u_int32_t gw_cmd   = 0;
+    u_int32_t gw_addr  = 0;
+
+    if (blk_addr & (blk_size - 1 )) {
+        //return _f.errmsg("Address should be 4-bytes aligned.");
+        return MFE_BAD_ALIGN;
+    }
+
+    // sanity check ??? remove ???
+    if (blk_size != (u_int32_t)mfl->attr.block_write ) {
+        //return _f.errmsg("Block write of wrong block size. %d instead of %d",
+       //               block_size, (u_int32_t)_f._cfi_data.max_multi_byte_write);
+        return MFE_BAD_PARAMS;
+    }
+
+    rc = set_bank(mfl, blk_addr);        CHECK_RC(rc);
+
+    rc = ih3lx_st_spi_write_enable(mfl); CHECK_RC(rc);
+
+    // Write the data block
+    gw_cmd = MERGE(gw_cmd, 1               , BO_SPI_SPECIAL,        1);
+    gw_cmd = MERGE(gw_cmd, log2up(blk_size), BO_MSIZE,       BS_MSIZE);
+
+    gw_addr = blk_addr & ONES32(mfl->attr.log2_bank_size);
+
+    gw_addr = MERGE(gw_addr, SFC_PP, BO_SPI_CMD, BS_SPI_CMD);
+
+    // Data:
+    for (offs = 0 ; offs < blk_size ; offs += 4) {
+        u_int32_t word = zero;
+        word = MERGE(word, data[offs + 0] , 24 , 8);
+        word = MERGE(word, data[offs + 1] , 16 , 8);
+        word = MERGE(word, data[offs + 2] ,  8 , 8);
+        word = MERGE(word, data[offs + 3] ,  0 , 8);
+        MWRITE4(CR_FLASH_DATA + offs, word );
+    }
+
+    rc = ih3lx_exec_cmd(mfl, gw_cmd, gw_addr, "PP command"); CHECK_RC(rc);
+
+    //
+    // Wait for end of write in flash (WriteInProgress = 0):
+    //
+
+    rc = st_spi_wait_wip(mfl, READ_DELAY, READ_CNT_SLOW + READ_CNT_FAST, READ_CNT_FAST); CHECK_RC(rc);
+
+    return MFE_OK;
+}
+
+int ih3lx_st_spi_erase_sect     (mflash* mfl, u_int32_t addr) {
+    int rc;
+
+    u_int32_t gw_cmd  = 0;
+    u_int32_t gw_addr = 0;
+
+    rc = set_bank(mfl, addr); CHECK_RC(rc);
+
+    rc = ih3lx_st_spi_write_enable(mfl); CHECK_RC(rc);
+
+    // Erase sector command:
+    gw_cmd = MERGE(gw_cmd, 1, BO_SPI_NO_DATA ,  1);
+    gw_cmd = MERGE(gw_cmd, 1, BO_SPI_SPECIAL ,  1);
+
+    gw_addr = addr & ONES32(mfl->attr.log2_bank_size);
+    gw_addr = MERGE(gw_addr, SFC_SE, BO_SPI_CMD, BS_SPI_CMD);
+
+    rc = ih3lx_exec_cmd(mfl, gw_cmd, gw_addr, "ES"); CHECK_RC(rc);
+
+    // Wait for erase completion
+    rc = st_spi_wait_wip(mfl, ERASE_DELAY, ERASE_CNT, 0); CHECK_RC(rc);
+
+    return MFE_OK;
+}
+
+int ih3lx_flash_init(mflash* mfl) {
+    int rc;
+
+    enum {
+        CR_FLASH_TYPE   = 0xf0810,
+        BO_FLASH_TYPE_S = 10,
+        BO_FLASH_TYPE_E = 11,
+    };
+
+    enum FlashType {
+        FT_LPC    = 0,
+        FT_SPI    = 1,
+        FT_XBUS   = 2,
+        FT_EEPROM = 3
+    };
+
+    char* flash_type_str[] = {"LPC", "SPI", "XBUS", "EEPROM"};
+
+    u_int32_t  strap_option;
+    u_int32_t  flash_type;
+
+    MREAD4(CR_FLASH_TYPE, &strap_option);
+
+    flash_type = EXTRACT_C(strap_option, BO_FLASH_TYPE_S, 2);
+
+    if (flash_type == FT_LPC   ||
+        flash_type == FT_XBUS  ||
+        flash_type == FT_EEPROM) {
+        printf("-E- flash of type %s not supported.\n", flash_type_str[flash_type]);
+        return MFE_UNSUPPORTED_FLASH_TYPE;
+    }
+
+    //mfl->f_read           = ih3lx_flash_read;
+    mfl->f_read           = read_chunks;
+    mfl->f_read_blk       = ih3lx_st_spi_block_read;
+    mfl->f_lock           = ihst_flash_lock; // Flash lock has same address and functionality as in InfiniHost.
+    mfl->f_set_bank       = ih3lx_set_bank;
+
+    rc = mfl->f_lock(mfl);
+    if (!mfl->opts[MFO_IGNORE_SEM_LOCK]) {
+        CHECK_RC(rc);
+    } else {
+        mfl->is_locked = 1;
+    }
+
+    rc = ih3lx_init_gpios(mfl);  CHECK_RC(rc);
+
+    mfl->f_spi_status = ih3lx_st_spi_get_status;
+
+    rc = st_spi_fill_attr(mfl);   CHECK_RC(rc);
+
+    if        (mfl->attr.command_set == MCS_STSPI) {
+        mfl->f_reset      = ih3lx_st_spi_reset;
+        mfl->f_write_blk  = ih3lx_st_spi_block_write;
+        mfl->f_write      = write_chunks;
+        mfl->f_erase_sect = ih3lx_st_spi_erase_sect;
+    } else {
+        return MFE_UNSUPPORTED_FLASH_TYPE;
+    }
+
+    rc = mfl->f_reset(mfl);
+
+    return MFE_OK;
+}
+
+//////////////////////////////////////////
+//
+// ConnectX functions implementation
+//
+//////////////////////////////////////////
+enum CntxCrConstants{
+    HCR_FLASH_CMD     = 0xf0400,
+    HCR_FLASH_ADDR    = 0xf0404,
+
+    HCR_FLASH_DATA    = 0xf0410,
+
+    HBO_READ_OP       = 0, 
+    HBO_ADDR_AUTO_INC = 1,  
+    HBO_CMD_PHASE     = 2,  
+    HBO_ADDR_PHASE    = 3,  
+    HBO_DATA_PHASE    = 4,  
+    HBO_CS_HOLD       = 5, 
+
+    HBO_MSIZE         = 8,
+    HBS_MSIZE         = 3,
+
+    HBO_CHIP_SELECT   = 11,
+    HBS_CHIP_SELECT   =  2,
+
+    HBO_FLASH_ENABLE  = 13,
+
+    HBO_CMD           = 16,
+    HBS_CMD           =  8,
+
+    HBO_BUSY          = 30,
+
+    HBO_ADDR          =  0,
+    HBS_ADDR          = 24,
+
+
+    // GPIOs
+    HCR_GPIO_LOCK     = 0xf0048,
+    HCR_GPIO_LOCK_VAL = 0xd42f,
+
+    HCR_GPIO_DATA_OUT = 0xf0040,
+    HCR_GPIO_MODE0    = 0xf0050,
+    HCR_GPIO_MODE1    = 0xf004c,
+
+    HBO_GPIO_CS       = 25,
+    HBS_GPIO_CS       =  4
+};
+
+int cntx_set_bank(mflash* mfl, u_int32_t bank) {
+    // NULL function - No actual work here - in ConnectX the curr_bank is written in the command word.
+    mfl = NULL; // Compiler warning
+    bank = 0;   // Compiler warning
+    return MFE_OK;
+}
+
+int cntx_exec_cmd(mflash* mfl, u_int32_t gw_cmd, char* msg) {
+    gw_cmd = MERGE(gw_cmd,              1, HBO_BUSY,                       1); 
+    gw_cmd = MERGE(gw_cmd,              1, HBO_FLASH_ENABLE,               1);
+    gw_cmd = MERGE(gw_cmd, (u_int32_t)mfl->curr_bank, 
+                                           HBO_CHIP_SELECT,  HBS_CHIP_SELECT);
+
+    MWRITE4(CR_FLASH_GW,   gw_cmd);
+
+    return ih3lx_wait_ready(mfl, msg);
+}
+
+int cntx_st_spi_get_status(mflash* mfl, u_int8_t op_type, u_int8_t* status) {
+    int rc;
+
+    u_int32_t gw_cmd  = 0;
+    u_int32_t flash_data;
+
+    gw_cmd = MERGE(gw_cmd,       1, HBO_READ_OP,    1);
+    gw_cmd = MERGE(gw_cmd,       1, HBO_CMD_PHASE,  1);
+    gw_cmd = MERGE(gw_cmd,       1, HBO_DATA_PHASE, 1);
+    gw_cmd = MERGE(gw_cmd,       2, HBO_MSIZE,      HBS_MSIZE);
+
+    gw_cmd = MERGE(gw_cmd, op_type, HBO_CMD,        HBS_CMD);
+
+    rc = cntx_exec_cmd(mfl, gw_cmd, "Read id");  CHECK_RC(rc);
+
+    MREAD4(HCR_FLASH_DATA, &flash_data);
+
+    // Return status reg byte is at offset 3 in word
+    *status = (u_int8_t)(flash_data & 0xff);
+
+    return MFE_OK;
+}
+
+int cntx_init_gpios(mflash* mfl) {
+    //
+    // Set Multi SPI CS to output and with value of 1 (inactive)
+    // Assuming 4 flashes. If there are less than 4 flashes and there's
+    // an access to a void flash, the access will fail.
+    //
+
+    u_int32_t       spi_en      = 0xf;
+
+    u_int32_t       mode0;
+    u_int32_t       mode1;
+    u_int32_t       data;
+
+    // Set 4 Chip selects to 1
+    MREAD4(HCR_GPIO_DATA_OUT, &data);
+    data = MERGE(data, spi_en, BO_SPI_GPIO, BS_SPI_GPIO);
+    MWRITE4(HCR_GPIO_DATA_OUT, data);
+
+    MREAD4(HCR_GPIO_MODE0, &mode0);
+    MREAD4(HCR_GPIO_MODE1, &mode1);
+
+    // Set 4 Chip selects to outputs
+    if (EXTRACT(mode0, HBO_GPIO_CS, HBS_GPIO_CS) != spi_en ||
+        EXTRACT(mode1, HBO_GPIO_CS, HBS_GPIO_CS) != 0          ) {
+        mode0 = MERGE(mode0,  spi_en, BO_SPI_GPIO, BS_SPI_GPIO);
+        mode1 = MERGE(mode1, ~spi_en, BO_SPI_GPIO, BS_SPI_GPIO);
+
+        // unlock gpio
+        MWRITE4(HCR_GPIO_LOCK , HCR_GPIO_LOCK_VAL);
+
+        MWRITE4(HCR_GPIO_MODE0, mode0);
+        MWRITE4(HCR_GPIO_MODE1, mode1);
+
+        // re-lock
+        MWRITE4(HCR_GPIO_LOCK , 0);
+    }
+
+    return MFE_OK;
+}
+
+int cntx_st_spi_write_enable(mflash* mfl) {
+    u_int32_