A board support package for the LPC4088 Display Module.

Dependencies:   DM_HttpServer DM_USBHost

Dependents:   lpc4088_displaymodule_emwin lpc4088_displaymodule_demo_sphere sampleGUI sampleEmptyGUI ... more

Fork of DMSupport by EmbeddedArtists AB

Files at this revision

API Documentation at this revision

Comitter:
embeddedartists
Date:
Fri Jan 16 11:13:39 2015 +0100
Parent:
21:1e6bc4aca5b7
Child:
23:6afd6a716e80
Commit message:
- Updated SPIFI code to allow BIOS to id chips not yet supported by DMSupport
- Updated QSPIFileSystem to be chip independant. Erase block info from SPIFI.cpp
- Split BiosDisplayAndTouch into BiosDisplay and BiosTouch
- Added BiosLoader with common functionallity
- Removed BIOS code from DMBoard
- Added first version of a touch listener

Changed in this revision

Bios/BiosLoader.cpp Show annotated file Show diff for this revision Revisions of this file
Bios/BiosLoader.h Show annotated file Show diff for this revision Revisions of this file
Bios/bios.h Show annotated file Show diff for this revision Revisions of this file
DMBoard.cpp Show annotated file Show diff for this revision Revisions of this file
DMBoard.h Show annotated file Show diff for this revision Revisions of this file
Display/BiosDisplay.cpp Show annotated file Show diff for this revision Revisions of this file
Display/BiosDisplay.h Show annotated file Show diff for this revision Revisions of this file
Display/BiosDisplayAndTouch.cpp Show diff for this revision Revisions of this file
Display/BiosDisplayAndTouch.h Show diff for this revision Revisions of this file
Display/BiosTouch.cpp Show annotated file Show diff for this revision Revisions of this file
Display/BiosTouch.h Show annotated file Show diff for this revision Revisions of this file
Display/Display.h Show annotated file Show diff for this revision Revisions of this file
Display/TouchPanel.h Show annotated file Show diff for this revision Revisions of this file
Display/bios.h Show diff for this revision Revisions of this file
FileSystems/QSPIFileSystem.cpp Show annotated file Show diff for this revision Revisions of this file
Memory/SPIFI.cpp Show annotated file Show diff for this revision Revisions of this file
Memory/SPIFI.h Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Bios/BiosLoader.cpp	Fri Jan 16 11:13:39 2015 +0100
@@ -0,0 +1,263 @@
+/*
+ *  Copyright 2014 Embedded Artists AB
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *    http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#include "mbed.h"
+#include "BiosLoader.h"
+#include "DMBoard.h"
+#include "BiosEEPROM.h"
+#include "crc.h"
+#include "bios.h"
+
+#if defined(DM_BOARD_BIOS_DEVELOPMENT)
+  #ifdef __cplusplus
+    extern "C" {
+  #endif
+      void bios_debug_aid(bios_header_t* header);
+  #ifdef __cplusplus
+    }
+  #endif
+#endif
+
+/******************************************************************************
+ * Defines and typedefs
+ *****************************************************************************/
+ 
+#define MOVE_POINTER(__x, __off) (   ( (uint32_t*)(__x) ) = (uint32_t*)( (uint32_t)(__x) + (__off) ) )
+
+/*
+ * Make sure that we reserve at least this amount of RAM for future
+ * expansion of the BIOS. This prevents the user from squeezing out
+ * the last drop of available RAM in his application.
+ */
+#define BIOS_RESERVED_CHUNK  0x1000
+#define BIOS_MAX_SIZE        0x100000
+#ifndef MAX
+  #define MAX(__a, __b) (((__a)>(__b))?(__a):(__b))
+#endif
+
+
+/******************************************************************************
+ * Local variables
+ *****************************************************************************/
+
+
+/******************************************************************************
+ * Private Functions
+ *****************************************************************************/
+
+// Function called from the BIOS
+static uint32_t readTimeMs()
+{
+  return us_ticker_read()/1000;
+}
+
+
+BiosLoader::BiosLoader() : 
+    _initialized(false),
+    _biosData(NULL),
+    _conf(NULL),
+    _confSize(0)
+{
+}
+
+BiosLoader::~BiosLoader()
+{
+  if (_biosData != NULL) {
+    free(_biosData);
+    _biosData = NULL;
+  }
+  if (_conf != NULL) {
+    free(_conf);
+    _conf = NULL;
+    _confSize = 0;
+  }
+}
+
+DMBoard::BoardError BiosLoader::readBIOS(uint8_t** data, uint32_t* size)
+{
+  DMBoard::BoardError err = DMBoard::Ok;
+  BiosEEPROM eeprom;
+  file_header_t fh;
+
+  if (_conf != NULL) {
+    *data = _conf;
+    *size = _confSize;
+    return DMBoard::Ok;
+  }
+    
+  do {
+    if (!eeprom.read(0, (char*)&fh, sizeof(file_header_t))) {
+      resetI2C();
+      if (!eeprom.read(0, (char*)&fh, sizeof(file_header_t))) {
+        err = DMBoard::BiosStorageError;
+        break;
+      }
+    }
+    
+    if (fh.magic != BIOS_MAGIC) {
+      err = DMBoard::BiosInvalidError;
+      break;
+    }
+    
+    if (fh.version != BIOS_VER) {
+      err = DMBoard::BiosVersionError;
+      break;
+    }
+    
+    if ((fh.headerSize + fh.size) > BIOS_MAX_SIZE) {
+      err = DMBoard::BiosInvalidError;
+      break;
+    }
+    
+    _confSize = fh.headerSize + fh.size;
+    _conf = (uint8_t*)malloc(MAX(_confSize,BIOS_RESERVED_CHUNK));
+    if (_conf == NULL) {
+      _confSize = 0;
+      err = DMBoard::MemoryError;
+      break;
+    }
+    
+    if (!eeprom.read(0, (char*)_conf, _confSize)) {
+      err = DMBoard::BiosStorageError;
+      break;
+    }
+    
+    uint32_t crc = crc_Buffer((uint32_t*)(&_conf[fh.headerSize]), fh.size/4);
+    if (crc != fh.crc) {
+      err = DMBoard::BiosInvalidError;
+      break;
+    }
+    
+    // Bios header has been verified and seems ok
+    *data = _conf;
+    *size = _confSize;
+    err = DMBoard::Ok;
+  } while (false);
+  
+  if (err != DMBoard::Ok) {
+    if (_conf != NULL) {
+      free(_conf);
+      _conf = NULL;
+      _confSize = 0;
+    }
+  }
+
+  return err;
+}
+
+DMBoard::BoardError BiosLoader::params(bios_header_t** header, void** instanceData)
+{
+  if (!_initialized) {
+    DMBoard::BoardError err = init();
+    if (err != DMBoard::Ok) {
+      return err;
+    }    
+  }
+  if (_initialized) {
+    *header = &_bios;
+    *instanceData = _biosData;
+    return DMBoard::Ok;
+  } else {
+    return DMBoard::BiosInvalidError;
+  }
+}
+
+DMBoard::BoardError BiosLoader::init()
+{
+  DMBoard::BoardError err = DMBoard::Ok;
+  if (!_initialized) {
+    do {
+      
+      // Get the display bios from the DMBoard. DMBoard will have verified it
+      // and will keep it in RAM so there is no need to copy it.
+      uint8_t* p = NULL;
+      uint32_t size = 0;
+      err = readBIOS(&p, &size);
+      if (err != BiosError_Ok) {
+        break;
+      }
+      
+      // Extract the function pointers so that they can be modified to match the
+      // actual location of the code
+      file_header_t* file_header = (file_header_t*)p;      
+      memcpy(&_bios, &file_header->header, sizeof(bios_header_t));
+      
+      // Allocate memory for the BIOS instance data
+      _biosData = malloc(file_header->paramSize);
+      if (_biosData == NULL) {
+        err = DMBoard::MemoryError;
+        break;
+      }
+      
+      // All offsets must be moved by two factors:
+      // 1) The position of the code in RAM (location of "p")
+      // 2) The header size (the code/data comes after it)
+      uint32_t offset = ((uint32_t)p) + file_header->headerSize;
+      uint32_t* functions = (uint32_t*)&_bios;
+      for (int i = 0; i < (sizeof(bios_header_t)/sizeof(uint32_t)); i++) {
+        functions[i] += offset;
+      }
+
+#if defined(DM_BOARD_BIOS_DEVELOPMENT)
+      // This requires that the project contains the source code for the BIOS
+      bios_debug_aid(&_bios);
+#endif
+      
+      // Prepare the BIOS instance data before calling the first function
+      BiosError_t e = _bios.initParams(_biosData, SystemCoreClock, PeripheralClock, wait_us, readTimeMs);
+      if (e != BiosError_Ok) {
+        err = DMBoard::BiosInvalidError;
+        break;
+      }
+      
+      _initialized = true;
+    } while(0);
+  }
+  return err;
+}
+
+void BiosLoader::resetI2C()
+{
+    DMBoard::instance().logger()->printf("BiosLoader::resetI2C()\n");
+    DigitalOut reset(P0_23);
+    reset = 0;
+    wait_ms(1);
+    reset = 1;
+    wait_ms(10);
+}
+
+
+/******************************************************************************
+ * Public Functions
+ *****************************************************************************/
+
+bool BiosLoader::isKnownSPIFIMemory(uint8_t mfgr, uint8_t devType, uint8_t devID, uint32_t memSize, uint32_t* eraseBlockSize)
+{
+  if (!_initialized) {
+    DMBoard::BoardError err = init();
+    if (err != DMBoard::Ok) {
+      return false;
+    }    
+  }
+  if (_initialized) {
+    bool known = false;
+    BiosError_t err = _bios.spifiIsSupported(_biosData, mfgr,devType,devID,memSize,&known,eraseBlockSize);
+    if (err == BiosError_Ok) {
+      return known;
+    }
+  }
+  return false;
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Bios/BiosLoader.h	Fri Jan 16 11:13:39 2015 +0100
@@ -0,0 +1,78 @@
+/*
+ *  Copyright 2014 Embedded Artists AB
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *    http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#ifndef BIOSLOADER_H
+#define BIOSLOADER_H
+
+#include "mbed.h"
+#include "DMBoard.h"
+#include "BiosDisplay.h"
+#include "BiosTouch.h"
+#include "bios.h"
+
+/**
+ * Unpacks and prepares the BIOS code.
+ */
+class BiosLoader {
+public:
+    
+    /** Get the only instance of the BiosLoader
+     *
+     *  @returns The BIOS
+     */
+    static BiosLoader& instance()
+    {
+        static BiosLoader singleton;
+        return singleton;
+    }
+  
+
+    bool isKnownSPIFIMemory(uint8_t mfgr, uint8_t devType, uint8_t devID, uint32_t memSize, uint32_t* eraseBlockSize);
+    
+    friend class BiosDisplay;
+    friend class BiosTouch;
+    friend class DMBoard;
+
+private:
+
+    bool _initialized;
+
+    bios_header_t _bios;
+    void* _biosData;
+    uint8_t* _conf;
+    uint32_t _confSize;
+
+    explicit BiosLoader();
+    // hide copy constructor
+    BiosLoader(const BiosLoader&);
+    // hide assign operator
+    BiosLoader& operator=(const BiosLoader&);
+    ~BiosLoader();
+
+    /** Loads, verifies and prepares the BIOS
+     *
+     *  @returns
+     *       Ok on success
+     *       An error code on failure
+     */
+    DMBoard::BoardError init();
+    DMBoard::BoardError readBIOS(uint8_t** data, uint32_t* size);
+    DMBoard::BoardError params(bios_header_t** header, void** instanceData);
+    
+    void resetI2C();
+};
+
+#endif /* BIOSLOADER_H */
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Bios/bios.h	Fri Jan 16 11:13:39 2015 +0100
@@ -0,0 +1,94 @@
+#ifndef BIOS_H
+#define BIOS_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#define BIOS_MAGIC  0xEA0123EA
+#define BIOS_VER      1
+
+typedef enum {
+    BiosError_Ok            =  0,
+    BiosError_ConfigError   =  1,
+    BiosError_WrongBPP      =  2,
+    BiosError_InvalidParam  =  3,
+    BiosError_NoInit        =  4,
+    BiosError_Calibration   =  5,
+    BiosError_Timeout       =  6,
+    BiosError_CommError     =  7,
+    BiosError_NotSupported  =  8,
+} BiosError_t;
+
+typedef enum {
+    Res_16bit_rgb565 = 1<<0,
+    Res_18bit_rgb666 = 1<<1,
+    Res_24bit_rgb888 = 1<<2,
+} Resolution_t;
+
+typedef struct {
+    uint16_t x;
+    uint16_t y;
+    uint16_t z;
+} touch_coordinate_t;
+
+typedef void (*delayUsFunc)(int us);
+typedef uint32_t (*readTimeMsFunc)(void);
+
+typedef BiosError_t (*initParamFunc)(void* data, uint32_t SystemCoreClock, uint32_t PeripheralClock, delayUsFunc delay, readTimeMsFunc readMs);
+typedef BiosError_t (*simpleFunc)(void* data);
+
+typedef BiosError_t (*powerUpFunc)(void* data, void* framebuffer, Resolution_t wanted);
+typedef BiosError_t (*backlightFunc)(void* data, int percent);
+typedef BiosError_t (*infoFuncD)(void* data,
+                                 uint16_t* width,
+                                 uint16_t* height,
+                                 uint16_t* bytesPerPixel,
+                                 bool* landscape,
+                                 uint16_t* supportedResolutions,
+                                 Resolution_t* currentResolution);
+typedef BiosError_t (*infoFuncT)(void* data,
+                                 bool* supportsTouch,
+                                 bool* supportsCalibration,
+                                 bool* resistive,
+                                 uint8_t* numPoints);
+
+typedef void (*touchIrqFunc)(uint32_t arg, bool enable, bool rising);
+typedef BiosError_t (*touchInitFunc)(void* data, touchIrqFunc irqEnabler, uint32_t enablerArg);
+typedef BiosError_t (*readFunc)(void* data, touch_coordinate_t* coords, int num);
+typedef BiosError_t (*nextFunc)(void* data, uint16_t* x, uint16_t* y, bool* last);
+typedef BiosError_t (*waitCalibFunc)(void* data, bool* morePoints, uint32_t timeoutMs);
+typedef BiosError_t (*spifiFunc)(void* data, uint8_t mfgr, uint8_t devType, uint8_t devID, uint32_t memSize, bool* known, uint32_t* eraseBlockSize);
+
+typedef struct {
+  initParamFunc initParams;
+
+  simpleFunc    displayInit;
+  powerUpFunc   displayPowerUp;
+  simpleFunc    displayPowerDown;
+  backlightFunc displayBacklight;
+  infoFuncD     displayInformation;
+
+  touchInitFunc touchInit;
+  simpleFunc    touchPowerUp;
+  simpleFunc    touchPowerDown;
+  readFunc      touchRead;
+  simpleFunc    touchCalibrateStart;
+  nextFunc      touchGetNextCalibPoint;
+  waitCalibFunc touchWaitForCalibratePoint;
+  simpleFunc    touchIrqHandler;
+  infoFuncT     touchInformation;
+    
+  spifiFunc     spifiIsSupported;
+} bios_header_t;
+
+typedef struct {
+  uint32_t magic;
+  uint32_t size;
+  uint32_t crc;
+  uint32_t version;
+  uint32_t paramSize;
+  uint32_t headerSize;
+  bios_header_t header;
+} file_header_t;
+
+#endif /* BIOS_H */
--- a/DMBoard.cpp	Mon Jan 12 10:37:57 2015 +0100
+++ b/DMBoard.cpp	Fri Jan 16 11:13:39 2015 +0100
@@ -16,12 +16,12 @@
 
 #include "mbed.h"
 #include "DMBoard.h"
-#include "BiosEEPROM.h"
-#include "bios.h"
-#include "crc.h"
 
 #if defined(DM_BOARD_USE_DISPLAY)
-  #include "BiosDisplayAndTouch.h"
+  #include "BiosDisplay.h"
+#endif
+#if defined(DM_BOARD_USE_TOUCH)
+  #include "BiosTouch.h"
 #endif
 
 #if defined(DM_BOARD_ENABLE_MEASSURING_PINS)
@@ -58,16 +58,6 @@
 static DevNull null("null");
 #endif
 
-/*
- * Make sure that we reserve at least this amount of RAM for future
- * expansion of the BIOS. This prevents the user from squeezing out
- * the last drop of available RAM in his application.
- */
-#define BIOS_RESERVED_CHUNK  0x1000
-#define BIOS_MAX_SIZE        0x100000
-#ifndef MAX
-  #define MAX(__a, __b) (((__a)>(__b))?(__a):(__b))
-#endif
 
 /******************************************************************************
  * Local variables
@@ -79,7 +69,6 @@
 
 DMBoard::DMBoard() : 
     _initialized(false),
-    _conf(NULL), _confSize(0),
 #if defined(DM_BOARD_USE_MCI_FS)
     _mcifs("mci", P4_16),
 #endif
@@ -97,83 +86,8 @@
 
 DMBoard::~DMBoard()
 {
-  if (_conf != NULL) {
-    free(_conf);
-    _conf = NULL;
-    _confSize = 0;
-  }
 }
 
-#if defined(DM_BOARD_USE_DISPLAY)
-DMBoard::BoardError DMBoard::readDisplayConfiguration(uint8_t** data, uint32_t* size)
-{
-  BoardError err = Ok;
-  BiosEEPROM eeprom;
-  file_header_t fh;
-
-  if (_conf != NULL) {
-    *data = _conf;
-    *size = _confSize;
-    return Ok;
-  }      
-  do {
-    if (!eeprom.read(0, (char*)&fh, sizeof(file_header_t))) {
-      err = BiosStorageError;
-      break;
-    }
-    
-    if (fh.magic != BIOS_MAGIC) {
-      err = BiosInvalidError;
-      break;
-    }
-    
-    if (fh.version != BIOS_VER) {
-      err = BiosVersionError;
-      break;
-    }
-    
-    if ((fh.headerSize + fh.size) > BIOS_MAX_SIZE) {
-      err = BiosInvalidError;
-      break;
-    }
-    
-    _confSize = fh.headerSize + fh.size;
-    _conf = (uint8_t*)malloc(MAX(_confSize,BIOS_RESERVED_CHUNK));
-    if (_conf == NULL) {
-      _confSize = 0;
-      err = MemoryError;
-      break;
-    }
-    
-    if (!eeprom.read(0, (char*)_conf, _confSize)) {
-      err = BiosStorageError;
-      break;
-    }
-    
-    uint32_t crc = crc_Buffer((uint32_t*)(&_conf[fh.headerSize]), fh.size/4);
-    if (crc != fh.crc) {
-      err = BiosInvalidError;
-      break;
-    }
-    
-    // Bios header has been verified and seems ok
-    *data = _conf;
-    *size = _confSize;
-    err = Ok;
-  } while (false);
-  
-  if (err != Ok) {
-    if (_conf != NULL) {
-      free(_conf);
-      _conf = NULL;
-      _confSize = 0;
-    }
-  }
-
-  return err;
-}
-#endif
-
 /******************************************************************************
  * Public Functions
  *****************************************************************************/
@@ -219,14 +133,14 @@
 #endif
       
 #if defined(DM_BOARD_USE_DISPLAY)
-      if (BiosDisplayAndTouch::instance().initDisplay() != Display::DisplayError_Ok) {
+      if (BiosDisplay::instance().init() != Display::DisplayError_Ok) {
         err = DisplayError;
         break;
       }
 #endif
       
 #if defined(DM_BOARD_USE_TOUCH)
-      if (BiosDisplayAndTouch::instance().initTouchController() != TouchPanel::TouchError_Ok) {
+      if (BiosTouch::instance().init() != TouchPanel::TouchError_Ok) {
         err = TouchError;
         break;
       }
@@ -281,11 +195,11 @@
   return _button.read() == 0;
 }
 
-#if defined(DM_BOARD_USE_DISPLAY)
+#if defined(DM_BOARD_USE_TOUCH)
   TouchPanel* DMBoard::touchPanel()
   {
-    if (BiosDisplayAndTouch::instance().isTouchSupported()) {
-      return &BiosDisplayAndTouch::instance();
+    if (BiosTouch::instance().isTouchSupported()) {
+      return &BiosTouch::instance();
     }
     return NULL;
   }
@@ -294,6 +208,6 @@
 #if defined(DM_BOARD_USE_DISPLAY)
   Display* DMBoard::display()
   {
-    return &BiosDisplayAndTouch::instance();
+    return &BiosDisplay::instance();
   }
 #endif
--- a/DMBoard.h	Mon Jan 12 10:37:57 2015 +0100
+++ b/DMBoard.h	Fri Jan 16 11:13:39 2015 +0100
@@ -136,7 +136,6 @@
      */
     Display* display();
 
-    friend class BiosDisplayAndTouch;
 #endif
     /** Returns the logger interface
      *
@@ -157,8 +156,6 @@
 private:
 
     bool _initialized;
-    uint8_t* _conf;
-    uint32_t _confSize;
 
 #if defined(DM_BOARD_USE_MCI_FS)
     MCIFileSystem _mcifs;
@@ -182,10 +179,6 @@
     // hide assign operator
     DMBoard& operator=(const DMBoard&);
     ~DMBoard();
-    
-#if defined(DM_BOARD_USE_DISPLAY)
-    BoardError readDisplayConfiguration(uint8_t** data, uint32_t* size);
-#endif
 };
 
 #endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Display/BiosDisplay.cpp	Fri Jan 16 11:13:39 2015 +0100
@@ -0,0 +1,188 @@
+/*
+ *  Copyright 2014 Embedded Artists AB
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *    http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#include "mbed.h"
+#include "BiosDisplay.h"
+#include "BiosLoader.h"
+#include "DMBoard.h"
+#include "bios.h"
+
+
+/******************************************************************************
+ * Defines and typedefs
+ *****************************************************************************/
+ 
+
+/******************************************************************************
+ * Local variables
+ *****************************************************************************/
+
+
+/******************************************************************************
+ * Private Functions
+ *****************************************************************************/
+
+BiosDisplay::BiosDisplay() : 
+    _initialized(false),
+    _poweredOn(false),
+    _bios(NULL),
+    _biosData(NULL),
+    _width(0),
+    _height(0),
+    _bpp(0),
+    _supportedRes(0),
+    _activeRes(Res_16bit_rgb565),
+    _landscape(false)
+{
+}
+
+BiosDisplay::~BiosDisplay()
+{
+  // _bios and _biosData are deallocated by BiosLoader
+}
+
+
+/******************************************************************************
+ * Public Functions
+ *****************************************************************************/
+
+BiosDisplay::DisplayError BiosDisplay::init()
+{
+  DisplayError err = DisplayError_Ok;
+  if (!_initialized) {
+    do {
+      if (BiosLoader::instance().params(&_bios, &_biosData) != DMBoard::Ok) {
+        err = DisplayError_ConfigError;
+        break;
+      }
+      
+      err = (DisplayError)_bios->displayInit(_biosData);
+      if (err != DisplayError_Ok) {
+        break;
+      }
+
+      err = (DisplayError)_bios->displayInformation(_biosData, &_width, &_height, &_bpp, &_landscape, &_supportedRes, &_activeRes);
+      if (err != DisplayError_Ok) {
+        break;
+      }
+      
+      _initialized = true;
+    } while(0);
+  }
+  return err;
+}
+
+BiosDisplay::DisplayError BiosDisplay::powerUp(void* framebuffer, Display::Resolution wanted)
+{
+  DisplayError err = DisplayError_Ok;
+  if (!_poweredOn) {
+    err = init();
+    if (err == DisplayError_Ok) {
+      do {
+        err = (DisplayError)_bios->displayPowerUp(_biosData, framebuffer, (Resolution_t)wanted);
+          
+        if (err != DisplayError_Ok) {
+          break;
+        }
+
+        err = (DisplayError)_bios->displayInformation(_biosData, &_width, &_height, &_bpp, &_landscape, &_supportedRes, &_activeRes);     
+        if (err != DisplayError_Ok) {
+          break;
+        }
+        
+        _poweredOn = true;
+      } while(0);
+    }
+  }
+  return err;
+}
+
+BiosDisplay::DisplayError BiosDisplay::powerDown()
+{
+  DisplayError err = DisplayError_Ok;
+  if (_poweredOn) {
+    err = (DisplayError)_bios->displayPowerDown(_biosData);
+    _poweredOn = false;
+  }
+  return err;
+}
+
+BiosDisplay::DisplayError BiosDisplay::backlight(int percent)
+{
+  DisplayError err = DisplayError_Ok;
+  if (!_initialized) {
+      err = DisplayError_NoInit;
+  } else {
+    err = (DisplayError)_bios->displayBacklight(_biosData, percent);
+  }      
+  return err;
+}
+
+uint16_t BiosDisplay::width()
+{
+  return _width;
+}
+
+uint16_t BiosDisplay::height()
+{
+  return _height;
+}
+
+uint16_t BiosDisplay::bytesPerPixel()
+{
+  return _bpp;
+}
+
+uint32_t BiosDisplay::fbSize()
+{
+  return _width * _height * _bpp;
+}
+
+bool BiosDisplay::landscape()
+{
+  return _landscape;
+}
+
+void* BiosDisplay::allocateFramebuffer(Display::Resolution res)
+{
+  if (_initialized) {
+    return malloc(fbSize());
+  }
+  return NULL;
+}
+
+void BiosDisplay::setFramebuffer(void* buff)
+{
+  LPC_LCD->UPBASE = (uint32_t)buff;
+}
+
+void* BiosDisplay::swapFramebuffer(void* buff)
+{
+  uint32_t old = LPC_LCD->UPBASE;
+  LPC_LCD->UPBASE = (uint32_t)buff;
+  return (void*)old;
+}
+
+bool BiosDisplay::isSupported(Display::Resolution res)
+{
+  return (_supportedRes & res);
+}
+
+BiosDisplay::Resolution BiosDisplay::currentResolution()
+{
+  return (Resolution)_activeRes;
+}
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Display/BiosDisplay.h	Fri Jan 16 11:13:39 2015 +0100
@@ -0,0 +1,95 @@
+/*
+ *  Copyright 2014 Embedded Artists AB
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *    http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#ifndef BIOSDISPLAY_H
+#define BIOSDISPLAY_H
+
+#include "mbed.h"
+#include "TouchPanel.h"
+#include "Display.h"
+#include "bios.h"
+
+/**
+ * Glue between the BIOS and the Display interface.
+ */
+class BiosDisplay : public Display {
+public:
+    
+    /** Get the only instance of the BiosDisplay
+     *
+     *  @returns The display
+     */
+    static BiosDisplay& instance()
+    {
+        static BiosDisplay singleton;
+        return singleton;
+    }
+  
+
+    /** Initializes the display but does not turn it on
+     *
+     *  @returns
+     *       Ok on success
+     *       An error code on failure
+     */
+    DisplayError init();
+  
+    // From the Display interface
+    virtual DisplayError powerUp(void* framebuffer, Resolution wanted = Resolution_16bit_rgb565);
+    virtual DisplayError powerDown();
+    virtual DisplayError backlight(int percent);
+    virtual uint16_t width();
+    virtual uint16_t height();
+    virtual uint16_t bytesPerPixel();
+    virtual uint32_t fbSize();
+    virtual bool landscape();
+    virtual bool isSupported(Resolution res);
+    virtual Resolution currentResolution();
+    virtual void setFramebuffer(void* buff);
+    virtual void* swapFramebuffer(void* buff);
+    virtual void* allocateFramebuffer(Resolution res=Resolution_16bit_rgb565);
+
+private:
+
+    bool _initialized;
+    bool _poweredOn;
+    //bool _initializedTouch;
+    //InterruptIn _touchIRQ;
+    //FunctionPointer _touchFP;
+
+    bios_header_t* _bios;
+    void* _biosData;
+
+    uint16_t _width;
+    uint16_t _height;
+    uint16_t _bpp;
+    uint16_t _supportedRes;
+    Resolution_t _activeRes;
+    bool _landscape;
+    //bool _supportsTouch;
+    //bool _supportsTouchCalibration;
+    //uint8_t _touchNumFingers;
+    //bool _touchIsResistive;
+
+    explicit BiosDisplay();
+    // hide copy constructor
+    BiosDisplay(const BiosDisplay&);
+    // hide assign operator
+    BiosDisplay& operator=(const BiosDisplay&);
+    ~BiosDisplay();
+};
+
+#endif /* BIOSDISPLAY_H */
--- a/Display/BiosDisplayAndTouch.cpp	Mon Jan 12 10:37:57 2015 +0100
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,383 +0,0 @@
-/*
- *  Copyright 2014 Embedded Artists AB
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *    http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-#include "mbed.h"
-#include "BiosDisplayAndTouch.h"
-#include "DMBoard.h"
-#include "bios.h"
-
-#if defined(DM_BOARD_BIOS_DEVELOPMENT)
-  #ifdef __cplusplus
-    extern "C" {
-  #endif
-      void bios_debug_aid(bios_header_t* header);
-  #ifdef __cplusplus
-    }
-  #endif
-#endif
-
-#if defined(DM_BOARD_USE_DISPLAY)
-
-/******************************************************************************
- * Defines and typedefs
- *****************************************************************************/
- 
-#define MOVE_POINTER(__x, __off) (   ( (uint32_t*)(__x) ) = (uint32_t*)( (uint32_t)(__x) + (__off) ) )
-
-
-/******************************************************************************
- * Local variables
- *****************************************************************************/
-
-
-/******************************************************************************
- * Private Functions
- *****************************************************************************/
-
-BiosDisplayAndTouch::BiosDisplayAndTouch() : 
-    _initializedDisplay(false),
-    _poweredOn(false),
-    _initializedTouch(false),
-    _touchIRQ(P2_25),
-    _biosData(NULL),
-    _width(0),
-    _height(0),
-    _bpp(0),
-    _supportedRes(0),
-    _activeRes(Res_16bit_rgb565),
-    _landscape(false),
-    _supportsTouch(false)
-{
-}
-
-BiosDisplayAndTouch::~BiosDisplayAndTouch()
-{
-  if (_biosData != NULL) {
-    free(_biosData);
-    _biosData = NULL;
-  }
-}
-
-// Function called from the BIOS
-static uint32_t readTimeMs()
-{
-  return us_ticker_read()/1000;
-}
-
-// Function called from the BIOS
-static void touchIrqEnabler(uint32_t arg, bool enable, bool rising)
-{
-  ((BiosDisplayAndTouch*)arg)->changeTouchInterrupt(enable, rising);
-}
-
-/******************************************************************************
- * Public Functions
- *****************************************************************************/
-
-BiosDisplayAndTouch::DisplayError BiosDisplayAndTouch::initDisplay()
-{
-  DisplayError err = DisplayError_Ok;
-  if (!_initializedDisplay) {
-    do {
-      
-      // Get the display bios from the DMBoard. DMBoard will have verified it
-      // and will keep it in RAM so there is no need to copy it.
-      uint8_t* p = NULL;
-      uint32_t size = 0;
-      if (DMBoard::instance().readDisplayConfiguration(&p, &size) != DMBoard::Ok) {
-        err = DisplayError_ConfigError;
-        break;
-      }
-      
-      // Extract the function pointers so that they can be modified to match the
-      // actual location of the code
-      file_header_t* file_header = (file_header_t*)p;      
-      memcpy(&_bios, &file_header->header, sizeof(bios_header_t));
-      
-      // Allocate memory for the BIOS instance data
-      _biosData = malloc(file_header->paramSize);
-      if (_biosData == NULL) {
-        err = DisplayError_MemoryError;
-        break;
-      }
-      
-      // All offsets must be moved by two factors:
-      // 1) The position of the code in RAM (location of "p")
-      // 2) The header size (the code/data comes after it)
-      uint32_t offset = ((uint32_t)p) + file_header->headerSize;
-      uint32_t* functions = (uint32_t*)&_bios;
-      for (int i = 0; i < (sizeof(bios_header_t)/sizeof(uint32_t)); i++) {
-        functions[i] += offset;
-      }
-
-#if defined(DM_BOARD_BIOS_DEVELOPMENT)
-      // This requires that the project contains the source code for the BIOS
-      bios_debug_aid(&_bios);
-#endif
-      
-      // Prepare the BIOS instance data before calling the first function
-      err = (DisplayError)_bios.initParams(_biosData, SystemCoreClock, PeripheralClock, wait_us, readTimeMs);
-      if (err != DisplayError_Ok) {
-        break;
-      }
-
-      err = (DisplayError)_bios.displayInit(_biosData);
-      if (err != DisplayError_Ok) {
-        break;
-      }
-
-      err = (DisplayError)_bios.displayInformation(_biosData, &_width, &_height, &_bpp, &_landscape, &_supportsTouch, &_supportedRes, &_activeRes);
-      if (err != DisplayError_Ok) {
-        break;
-      }
-      
-      _initializedDisplay = true;
-    } while(0);
-  }
-  return err;
-}
-
-BiosDisplayAndTouch::DisplayError BiosDisplayAndTouch::powerUp(void* framebuffer, Display::Resolution wanted)
-{
-  DisplayError err = DisplayError_Ok;
-  if (!_poweredOn) {
-    err = initDisplay();
-    if (err == DisplayError_Ok) {
-      do {
-        err = (DisplayError)_bios.displayPowerUp(_biosData, framebuffer, (Resolution_t)wanted);
-          
-        if (err != DisplayError_Ok) {
-          break;
-        }
-
-        err = (DisplayError)_bios.displayInformation(_biosData, &_width, &_height, &_bpp, &_landscape, &_supportsTouch, &_supportedRes, &_activeRes);     
-        if (err != DisplayError_Ok) {
-          break;
-        }
-        
-        _poweredOn = true;
-      } while(0);
-    }
-  }
-  return err;
-}
-
-BiosDisplayAndTouch::DisplayError BiosDisplayAndTouch::powerDown()
-{
-  DisplayError err = DisplayError_Ok;
-  if (_poweredOn) {
-    err = (DisplayError)_bios.displayPowerDown(_biosData);
-    _poweredOn = false;
-  }
-  return err;
-}
-
-BiosDisplayAndTouch::DisplayError BiosDisplayAndTouch::backlight(int percent)
-{
-  DisplayError err = DisplayError_Ok;
-  if (!_initializedDisplay) {
-      err = DisplayError_NoInit;
-  } else {
-    err = (DisplayError)_bios.displayBacklight(_biosData, percent);
-  }      
-  return err;
-}
-
-uint16_t BiosDisplayAndTouch::width()
-{
-//  uint16_t res = 0;
-//  if (_initializedDisplay) {
-//    res = _bios.displayWidth(_biosData);
-//  }
-//  return res;
-  return _width;
-}
-
-uint16_t BiosDisplayAndTouch::height()
-{
-//  uint16_t res = 0;
-//  if (_initializedDisplay) {
-//    res = _bios.displayHeight(_biosData);
-//  }
-//  return res;
-  return _height;
-}
-
-uint16_t BiosDisplayAndTouch::bytesPerPixel()
-{
-//  uint16_t res = 0;
-//  if (_initializedDisplay) {
-//    res = _bios.displayBytesPerPixel(_biosData);
-//  }
-//  return res;
-  return _bpp;
-}
-
-uint32_t BiosDisplayAndTouch::fbSize()
-{
-//  if (_initializedDisplay) {
-//    return width() * height() * bytesPerPixel();
-//  }
-//  return 0;
-  return _width * _height * _bpp;
-}
-
-bool BiosDisplayAndTouch::landscape()
-{
-//  bool res = false;
-//  if (_initializedDisplay) {
-//    res = _bios.displayLandscape(_biosData);
-//  }
-//  return res;
-  return _landscape;
-}
-
-void* BiosDisplayAndTouch::allocateFramebuffer(Display::Resolution res)
-{
-  if (_initializedDisplay) {
-    return malloc(fbSize());
-  }
-  return NULL;
-}
-
-void BiosDisplayAndTouch::setFramebuffer(void* buff)
-{
-  LPC_LCD->UPBASE = (uint32_t)buff;
-}
-
-void* BiosDisplayAndTouch::swapFramebuffer(void* buff)
-{
-  uint32_t old = LPC_LCD->UPBASE;
-  LPC_LCD->UPBASE = (uint32_t)buff;
-  return (void*)old;
-}
-
-bool BiosDisplayAndTouch::isSupported(Display::Resolution res)
-{
-  //bool result = false;
-  //if (_initializedDisplay) {
-  //  result = (_bios.displayResolutions(_biosData) & res);
-  //}
-  //return result;
-  return (_supportedRes & res);
-}
-
-bool BiosDisplayAndTouch::isTouchSupported()
-{
-  return _supportsTouch;
-}
-
-BiosDisplayAndTouch::Resolution BiosDisplayAndTouch::currentResolution()
-{
-  return (Resolution)_activeRes;
-}
-
-BiosDisplayAndTouch::TouchError BiosDisplayAndTouch::initTouchController()
-{
-  TouchError result = TouchError_Ok;
-  if (!_initializedTouch) {
-    do {
-      // must initialize display first
-      if (!_initializedDisplay) {
-        if (initDisplay() != DisplayError_Ok) {
-          result = TouchError_NoInit;
-          break;
-        }
-      }
-      
-      // is it supported at all?
-      if (!_supportsTouch) {
-        result = TouchError_TouchNotSupported;
-        break;
-      }
-          
-      result = (TouchError)_bios.touchInit(_biosData, touchIrqEnabler, (uint32_t)this);
-      if (result != TouchError_Ok) {
-        break;
-      }
-      _initializedTouch = true;
-    } while(0);
-  }
-  return result;
-}
-
-BiosDisplayAndTouch::TouchError BiosDisplayAndTouch::read(touchCoordinate_t &coord)
-{
-  TouchError err = TouchError_Ok;
-  if (!_initializedTouch) {
-      err = TouchError_NoInit;
-  } else {
-    err = (TouchError)_bios.touchRead(_biosData, &coord.x, &coord.y, &coord.z);
-  }      
-  return err;
-}
-
-BiosDisplayAndTouch::TouchError BiosDisplayAndTouch::calibrateStart()
-{
-  TouchError err = TouchError_Ok;
-  if (!_initializedTouch) {
-      err = TouchError_NoInit;
-  } else {
-    err = (TouchError)_bios.touchCalibrateStart(_biosData);
-  }      
-  return err;
-}
-
-BiosDisplayAndTouch::TouchError BiosDisplayAndTouch::getNextCalibratePoint(uint16_t* x, uint16_t* y, bool* last)
-{
-  TouchError err = TouchError_Ok;
-  if (!_initializedTouch) {
-      err = TouchError_NoInit;
-  } else {
-    err = (TouchError)_bios.touchGetNextCalibPoint(_biosData, x, y, last);
-  }      
-  return err;
-}
-
-BiosDisplayAndTouch::TouchError BiosDisplayAndTouch::waitForCalibratePoint(bool* morePoints, uint32_t timeout)
-{
-  TouchError err = TouchError_Ok;
-  if (!_initializedTouch) {
-      err = TouchError_NoInit;
-  } else {
-    err = (TouchError)_bios.touchWaitForCalibratePoint(_biosData, morePoints, timeout);
-  }      
-  return err;
-}
-
-void BiosDisplayAndTouch::handleTouchInterrupt()
-{
-  _bios.touchIrqHandler(_biosData);
-}
-
-void BiosDisplayAndTouch::changeTouchInterrupt(bool enable, bool rising)
-{
-  if (enable) {
-    if (rising) {
-      _touchIRQ.rise(this, &BiosDisplayAndTouch::handleTouchInterrupt);
-    } else {
-      _touchIRQ.fall(this, &BiosDisplayAndTouch::handleTouchInterrupt);
-    }
-  } else {
-    if (rising) {
-      _touchIRQ.rise(NULL);
-    } else {
-      _touchIRQ.fall(NULL);
-    }
-  }
-}
-
-#endif //defined(DM_BOARD_USE_DISPLAY)
--- a/Display/BiosDisplayAndTouch.h	Mon Jan 12 10:37:57 2015 +0100
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,121 +0,0 @@
-/*
- *  Copyright 2014 Embedded Artists AB
- *
- *  Licensed under the Apache License, Version 2.0 (the "License");
- *  you may not use this file except in compliance with the License.
- *  You may obtain a copy of the License at
- *
- *    http://www.apache.org/licenses/LICENSE-2.0
- *
- *  Unless required by applicable law or agreed to in writing, software
- *  distributed under the License is distributed on an "AS IS" BASIS,
- *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- *  See the License for the specific language governing permissions and
- *  limitations under the License.
- */
-
-#ifndef BIOSDISPLAYANDTOUCH_H
-#define BIOSDISPLAYANDTOUCH_H
-
-#include "mbed.h"
-#include "TouchPanel.h"
-#include "Display.h"
-#include "bios.h"
-
-/**
- * Glue between the BIOS and the Display/TouchPanel interfaces.
- */
-class BiosDisplayAndTouch : public TouchPanel, public Display {
-public:
-    
-    /** Get the only instance of the BiosDisplayAndTouch
-     *
-     *  @returns The display
-     */
-    static BiosDisplayAndTouch& instance()
-    {
-        static BiosDisplayAndTouch singleton;
-        return singleton;
-    }
-  
-
-    /** Initializes the display but does not turn it on
-     *
-     *  @returns
-     *       Ok on success
-     *       An error code on failure
-     */
-    DisplayError initDisplay();
-  
-    /**
-     * Initialize the touch controller. This method must be called before
-     * calibrating or reading data from the controller
-     *
-     *  @returns
-     *       Ok on success
-     *       An error code on failure
-     */
-    TouchError initTouchController();
-
-    /**
-     * Tests if a touch controller is available or not.
-     *
-     * Note that this function only returns a valid value
-     * after the display has been intitialized.
-     *
-     * @return true if there is a touch controller
-     */
-    bool isTouchSupported();
-    
-    void handleTouchInterrupt();
-    void changeTouchInterrupt(bool enable, bool rising);
-  
-    // From the Display interface
-    virtual DisplayError powerUp(void* framebuffer, Resolution wanted = Resolution_16bit_rgb565);
-    virtual DisplayError powerDown();
-    virtual DisplayError backlight(int percent);
-    virtual uint16_t width();
-    virtual uint16_t height();
-    virtual uint16_t bytesPerPixel();
-    virtual uint32_t fbSize();
-    virtual bool landscape();
-    virtual bool isSupported(Resolution res);
-    virtual Resolution currentResolution();
-    virtual void setFramebuffer(void* buff);
-    virtual void* swapFramebuffer(void* buff);
-    virtual void* allocateFramebuffer(Resolution res=Resolution_16bit_rgb565);
-
-    // From the TouchPanel interface
-    virtual TouchError read(touchCoordinate_t &coord);
-    virtual TouchError calibrateStart();
-    virtual TouchError getNextCalibratePoint(uint16_t* x, uint16_t* y, bool* last=NULL);
-    virtual TouchError waitForCalibratePoint(bool* morePoints, uint32_t timeout);
-
-private:
-
-    bool _initializedDisplay;
-    bool _poweredOn;
-    bool _initializedTouch;
-    InterruptIn _touchIRQ;
-    //FunctionPointer _touchFP;
-
-    bios_header_t _bios;
-    void* _biosData;
-
-    uint16_t _width;
-    uint16_t _height;
-    uint16_t _bpp;
-    uint16_t _supportedRes;
-    Resolution_t _activeRes;
-    bool _landscape;
-    bool _supportsTouch;
-
-    explicit BiosDisplayAndTouch();
-    // hide copy constructor
-    BiosDisplayAndTouch(const BiosDisplayAndTouch&);
-    // hide assign operator
-    BiosDisplayAndTouch& operator=(const BiosDisplayAndTouch&);
-    ~BiosDisplayAndTouch();
-};
-
-#endif /* BIOSDISPLAYANDTOUCH_H */
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Display/BiosTouch.cpp	Fri Jan 16 11:13:39 2015 +0100
@@ -0,0 +1,337 @@
+/*
+ *  Copyright 2014 Embedded Artists AB
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *    http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#include "mbed.h"
+#include "BiosTouch.h"
+#include "BiosLoader.h"
+#include "DMBoard.h"
+#include "bios.h"
+#include "meas.h"
+
+/******************************************************************************
+ * Defines and typedefs
+ *****************************************************************************/
+
+#define SIG_NEW_DATA 0x1
+class TouchHandler {
+    public:
+        TouchHandler(bios_header_t* bios, void* biosData, int num) :
+           _latest(NULL), _touchIRQ(P2_25), _bios(bios), 
+           _biosData(biosData), _haveData(false), _points(num),
+           _thread(NULL), _listener(NULL) {}
+        void handleTouchInterrupt();
+        void changeTouchInterrupt(bool enable, bool rising);
+        TouchPanel::TouchError read(touch_coordinate_t* coord, int num);
+        void run();
+        void setThread(Thread* t) { _thread = t; }
+        FunctionPointer* setListener(FunctionPointer* listener);
+    private:
+        Mutex _mutex;
+        touch_coordinate_t* _latest;
+        InterruptIn _touchIRQ;
+        bios_header_t* _bios;
+        void* _biosData;
+        bool _haveData; //TODO: improve
+        int _points;
+        Thread* _thread;
+        FunctionPointer* _listener;
+};
+
+/******************************************************************************
+ * Local variables
+ *****************************************************************************/
+
+/******************************************************************************
+ * Private Functions
+ *****************************************************************************/
+
+BiosTouch::BiosTouch() : 
+    _initialized(false),
+    _haveInfo(false),
+    _poweredOn(false),
+    //_touchIRQ(P2_25),
+    _bios(NULL),
+    _biosData(NULL),
+    _handlerThread(NULL),
+    _handler(NULL),
+    _supportsTouch(false)
+{
+}
+
+BiosTouch::~BiosTouch()
+{
+  // _bios and _biosData are deallocated by BiosLoader
+    
+  if (_handlerThread != NULL) {
+    delete _handlerThread;
+    _handlerThread = NULL;
+  }
+  if (_handler != NULL) {
+    delete _handler;
+    _handler = NULL;
+  }
+}
+
+// Function called from the BIOS
+static void touchIrqEnabler(uint32_t arg, bool enable, bool rising)
+{
+  ((TouchHandler*)arg)->changeTouchInterrupt(enable, rising);
+#if defined(DM_BOARD_ENABLE_MEASSURING_PINS)
+  if (enable) {
+    SET_MEAS_PIN_1();
+  } else {
+    CLR_MEAS_PIN_1();
+  }
+#endif
+}
+
+static void touchTask(void const* args)
+{
+  ((TouchHandler*)args)->run();
+}
+
+
+void TouchHandler::run()
+{
+  RtosLog* log = DMBoard::instance().logger();
+  BiosError_t err;
+  
+  _latest = (touch_coordinate_t*)malloc(_points*sizeof(touch_coordinate_t));
+  if (_latest == NULL) {
+    log->printf("Failed to allocate memory for touch events\n");
+    mbed_die();
+  }
+  memset(_latest, 0, _points*sizeof(touch_coordinate_t));
+  while(true) {
+      Thread::signal_wait(SIG_NEW_DATA);
+//    if (_haveData) {
+//      _haveData = false;
+      SET_MEAS_PIN_3();
+      _bios->touchIrqHandler(_biosData);
+      CLR_MEAS_PIN_3();
+      //read
+      _mutex.lock();
+      err = _bios->touchRead(_biosData, _latest, _points);
+      FunctionPointer* fp = _listener;
+      _mutex.unlock();
+      if (err == BiosError_Ok) {
+        //notify registered callbacks
+        if (fp != NULL) {
+      SET_MEAS_PIN_4();
+          fp->call();
+      SET_MEAS_PIN_4();
+        }
+      } else {
+        log->printf("Failed to read touch event, err = %d\n", err);
+      }
+//    }
+  }
+  //if (_latest != NULL) {
+  //  free(_latest);
+  //  _latest = NULL;
+  //}
+}
+
+TouchPanel::TouchError TouchHandler::read(touch_coordinate_t* coord, int num)
+{
+  if (num > _points || num < 1) {
+    return TouchPanel::TouchError_InvalidParam;
+  }
+  _mutex.lock();
+  memcpy(coord, _latest, num*sizeof(touch_coordinate_t));
+  _mutex.unlock();
+  
+  return TouchPanel::TouchError_Ok;
+}
+
+void TouchHandler::handleTouchInterrupt()
+{
+  SET_MEAS_PIN_2();
+  //_haveData = true;
+  if (_thread != NULL) {
+    _thread->signal_set(SIG_NEW_DATA);
+  }
+  CLR_MEAS_PIN_2();
+}
+
+void TouchHandler::changeTouchInterrupt(bool enable, bool rising)
+{
+  if (enable) {
+    if (rising) {
+      _touchIRQ.rise(this, &TouchHandler::handleTouchInterrupt);
+    } else {
+      _touchIRQ.fall(this, &TouchHandler::handleTouchInterrupt);
+    }
+  } else {
+    if (rising) {
+      _touchIRQ.rise(NULL);
+    } else {
+      _touchIRQ.fall(NULL);
+    }
+  }
+}
+
+FunctionPointer* TouchHandler::setListener(FunctionPointer* listener)
+{
+  _mutex.lock();
+  FunctionPointer* old = _listener;
+  _listener = listener;
+  _mutex.unlock();
+  return old;
+}
+
+
+/******************************************************************************
+ * Public Functions
+ *****************************************************************************/
+
+BiosTouch::TouchError BiosTouch::init()
+{
+  TouchError result = TouchError_Ok;
+  if (!_initialized) {
+    do {
+      if (BiosLoader::instance().params(&_bios, &_biosData) != DMBoard::Ok) {
+        result = TouchError_ConfigError;
+        break;
+      }
+      
+      result = (TouchError)_bios->touchInformation(_biosData, &_supportsTouch, &_supportsTouchCalibration, &_touchIsResistive, &_touchNumFingers);
+      if (result != TouchError_Ok) {
+        break;
+      }
+      _haveInfo = true;
+      
+      // is it supported at all?
+      if (!_supportsTouch) {
+        result = TouchError_TouchNotSupported;
+        break;
+      }
+      
+      _handler = new TouchHandler(_bios, _biosData, _touchNumFingers);
+          
+      result = (TouchError)_bios->touchInit(_biosData, touchIrqEnabler, (uint32_t)_handler);
+      if (result != TouchError_Ok) {
+        break;
+      }
+      
+      result = (TouchError)_bios->touchPowerUp(_biosData);
+      if (result != TouchError_Ok) {
+        break;
+      }
+
+      _handlerThread = new Thread(touchTask, _handler);
+      _handler->setThread(_handlerThread);
+
+      _initialized = true;
+    } while(0);
+    
+    if (!_initialized) {
+      if (_handler != NULL) {
+        delete _handler;
+        _handler = NULL;
+      }
+    }
+  }
+  return result;
+}
+
+BiosTouch::TouchError BiosTouch::read(touch_coordinate_t &coord)
+{
+  TouchError err = TouchError_Ok;
+  if (!_initialized) {
+      err = TouchError_NoInit;
+  } else {
+    //err = (TouchError)_bios->touchRead(_biosData, &coord, 1);
+    err = _handler->read(&coord, 1);
+  }      
+  return err;
+}
+
+BiosTouch::TouchError BiosTouch::read(touch_coordinate_t* coord, int num)
+{
+  TouchError err = TouchError_Ok;
+  if (!_initialized) {
+      err = TouchError_NoInit;
+  } else {
+    //err = (TouchError)_bios->touchRead(_biosData, coord, num);
+    err = _handler->read(coord, num);
+  }      
+  return err;
+}
+
+BiosTouch::TouchError BiosTouch::info(bool* resistive, int* maxPoints, bool* calibrated)
+{
+  TouchError err = TouchError_Ok;
+  if (!_haveInfo) {
+      err = TouchError_NoInit;
+  } else {
+    *resistive = _touchIsResistive;
+    *maxPoints = _touchNumFingers;
+    *calibrated = _supportsTouchCalibration;
+  }      
+  return err;
+}
+
+bool BiosTouch::isTouchSupported()
+{
+#if defined(DM_BOARD_USE_TOUCH)
+  if (_haveInfo) {
+    return _supportsTouch;
+  }
+#endif
+  return false;
+}
+
+BiosTouch::TouchError BiosTouch::calibrateStart()
+{
+  TouchError err = TouchError_Ok;
+  if (!_initialized) {
+      err = TouchError_NoInit;
+  } else {
+    err = (TouchError)_bios->touchCalibrateStart(_biosData);
+  }      
+  return err;
+}
+
+BiosTouch::TouchError BiosTouch::getNextCalibratePoint(uint16_t* x, uint16_t* y, bool* last)
+{
+  TouchError err = TouchError_Ok;
+  if (!_initialized) {
+      err = TouchError_NoInit;
+  } else {
+    err = (TouchError)_bios->touchGetNextCalibPoint(_biosData, x, y, last);
+  }      
+  return err;
+}
+
+BiosTouch::TouchError BiosTouch::waitForCalibratePoint(bool* morePoints, uint32_t timeout)
+{
+  TouchError err = TouchError_Ok;
+  if (!_initialized) {
+      err = TouchError_NoInit;
+  } else {
+    err = (TouchError)_bios->touchWaitForCalibratePoint(_biosData, morePoints, timeout);
+  }      
+  return err;
+}
+
+FunctionPointer* BiosTouch::setListener(FunctionPointer* listener)
+{
+  if (_initialized) {
+    return _handler->setListener(listener);
+  }
+  return NULL;
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Display/BiosTouch.h	Fri Jan 16 11:13:39 2015 +0100
@@ -0,0 +1,110 @@
+/*
+ *  Copyright 2014 Embedded Artists AB
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License");
+ *  you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *    http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+#ifndef BIOSTOUCH_H
+#define BIOSTOUCH_H
+
+#include "mbed.h"
+#include "TouchPanel.h"
+#include "Display.h"
+#include "bios.h"
+#include "rtos.h"
+
+class TouchHandler;
+
+/**
+ * Glue between the BIOS and the TouchPanel interface.
+ */
+class BiosTouch : public TouchPanel {
+public:
+    
+    /** Get the only instance of the BiosTouch
+     *
+     *  @returns The display
+     */
+    static BiosTouch& instance()
+    {
+        static BiosTouch singleton;
+        return singleton;
+    }
+  
+    /**
+     * Initialize the touch controller. This method must be called before
+     * calibrating or reading data from the controller
+     *
+     *  @returns
+     *       Ok on success
+     *       An error code on failure
+     */
+    TouchError init();
+
+    /**
+     * Tests if a touch controller is available or not.
+     *
+     * Note that this function only returns a valid value
+     * after the display has been intitialized.
+     *
+     * @return true if there is a touch controller
+     */
+    bool isTouchSupported();
+    
+    //void handleTouchInterrupt();
+    //void changeTouchInterrupt(bool enable, bool rising);
+  
+    // From the TouchPanel interface
+    virtual TouchError read(touch_coordinate_t &coord);
+    virtual TouchError read(touch_coordinate_t* coord, int num);
+    virtual TouchError info(bool* resistive, int* maxPoints, bool* calibrated);
+    virtual TouchError calibrateStart();
+    virtual TouchError getNextCalibratePoint(uint16_t* x, uint16_t* y, bool* last=NULL);
+    virtual TouchError waitForCalibratePoint(bool* morePoints, uint32_t timeout);
+    virtual FunctionPointer* setListener(FunctionPointer* listener);
+
+private:
+
+    //bool _initializedDisplay;
+    bool _initialized;
+    bool _haveInfo;
+    bool _poweredOn;
+    //InterruptIn _touchIRQ;
+    //FunctionPointer _touchFP;
+
+    bios_header_t* _bios;
+    void* _biosData;
+
+    Thread* _handlerThread;
+    TouchHandler* _handler;
+
+    //uint16_t _width;
+    //uint16_t _height;
+    //uint16_t _bpp;
+    //uint16_t _supportedRes;
+    //Resolution_t _activeRes;
+    //bool _landscape;
+    bool _supportsTouch;
+    bool _supportsTouchCalibration;
+    uint8_t _touchNumFingers;
+    bool _touchIsResistive;
+
+    explicit BiosTouch();
+    // hide copy constructor
+    BiosTouch(const BiosTouch&);
+    // hide assign operator
+    BiosTouch& operator=(const BiosTouch&);
+    ~BiosTouch();
+};
+
+#endif /* BIOSTOUCH_H */
--- a/Display/Display.h	Mon Jan 12 10:37:57 2015 +0100
+++ b/Display/Display.h	Fri Jan 16 11:13:39 2015 +0100
@@ -62,6 +62,8 @@
         DisplayError_InvalidParam      =   BiosError_InvalidParam,
         DisplayError_NoInit            =   BiosError_NoInit,
         DisplayError_CalibrationError  =   BiosError_Calibration,
+        DisplayError_Timeout           =   BiosError_Timeout,
+        DisplayError_TouchNotSupported =   BiosError_NotSupported,
         DisplayError_MemoryError,
     };
     
--- a/Display/TouchPanel.h	Mon Jan 12 10:37:57 2015 +0100
+++ b/Display/TouchPanel.h	Fri Jan 16 11:13:39 2015 +0100
@@ -25,27 +25,24 @@
 class TouchPanel {
 public:
 
-    typedef struct {
-        uint16_t x;
-        uint16_t y;
-        uint16_t z;
-    } touchCoordinate_t;
-
     enum TouchError {
         TouchError_Ok                =   BiosError_Ok,
         TouchError_ConfigError       =   BiosError_ConfigError,
         TouchError_WrongBPP          =   BiosError_WrongBPP,
         TouchError_InvalidParam      =   BiosError_InvalidParam,
         TouchError_NoInit            =   BiosError_NoInit,
-        TouchError_CalibrationError  =   BiosError_Calibration,        
+        TouchError_CalibrationError  =   BiosError_Calibration,
+        TouchError_Timeout           =   BiosError_Timeout,
+        TouchError_TouchNotSupported =   BiosError_NotSupported,
         TouchError_MemoryError,
-        TouchError_TouchNotSupported,
-        TouchError_Timeout,
     };
 
     /**
      * Read coordinates from the touch panel.
      *
+     * In case of multitouch (capacitive touch screen) only the first touch
+     * will be returned.
+     *
      * @param coord pointer to coordinate object. The read coordinates will be
      * written to this object.
      *
@@ -53,7 +50,32 @@
      *       Ok on success
      *       An error code on failure
      */
-    virtual TouchError read(touchCoordinate_t &coord) = 0;
+    virtual TouchError read(touch_coordinate_t &coord) = 0;
+
+    /**
+     * Read up to num coordinates from the touch panel.
+     *
+     * @param coords a list of at least num coordinates
+     * @param num the number of coordinates to read
+     *
+     *  @returns
+     *       Ok on success
+     *       An error code on failure
+     */
+    virtual TouchError read(touch_coordinate_t* coord, int num) = 0;
+    
+    /**
+     * Returns information about the touch panel
+     *
+     * @param resistive true for Resistive, false for Capacitive
+     * @param maxPoints the maximum number of simultaneous touches
+     * @param calibration true if the controller can be calibrated
+     *
+     *  @returns
+     *       Ok on success
+     *       An error code on failure
+     */
+    virtual TouchError info(bool* resistive, int* maxPoints, bool* calibrated) = 0;
 
     /**
      * Start to calibrate the display
@@ -94,6 +116,8 @@
      *       An error code on failure
      */
     virtual TouchError waitForCalibratePoint(bool* morePoints, uint32_t timeout) = 0;
+    
+    virtual FunctionPointer* setListener(FunctionPointer* listener) = 0;
 };
 
 #endif
--- a/Display/bios.h	Mon Jan 12 10:37:57 2015 +0100
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,75 +0,0 @@
-#ifndef BIOS_H
-#define BIOS_H
-
-#include <stdint.h>
-#include <stdbool.h>
-
-#define BIOS_MAGIC  0xEA0123EA
-#define BIOS_VER      1
-
-typedef enum {
-    BiosError_Ok            =  0,
-    BiosError_ConfigError   =  1,
-    BiosError_WrongBPP      =  2,
-    BiosError_InvalidParam  =  3,
-    BiosError_NoInit        =  4,
-    BiosError_Calibration   =  5,
-} BiosError_t;
-    
-typedef enum {
-    Res_16bit_rgb565 = 1<<0,
-    Res_18bit_rgb666 = 1<<1,
-    Res_24bit_rgb888 = 1<<2,
-} Resolution_t;
-
-typedef void (*delayUsFunc)(int us);
-typedef uint32_t (*readTimeMsFunc)(void);
-
-typedef BiosError_t (*initParamFunc)(void* data, uint32_t SystemCoreClock, uint32_t PeripheralClock, delayUsFunc delay, readTimeMsFunc readMs);
-typedef BiosError_t (*simpleFunc)(void* data); 
-
-typedef BiosError_t (*powerUpFunc)(void* data, void* framebuffer, Resolution_t wanted);
-typedef BiosError_t (*backlightFunc)(void* data, int percent); 
-typedef BiosError_t (*infoFunc)(void* data, 
-                                uint16_t* width, 
-                                uint16_t* height, 
-                                uint16_t* bytesPerPixel,
-                                bool* landscape,
-                                bool* supportsTouch,
-                                uint16_t* supportedResolutions,
-                                Resolution_t* currentResolution); 
-
-typedef void (*touchIrqFunc)(uint32_t arg, bool enable, bool rising);
-typedef BiosError_t (*touchInitFunc)(void* data, touchIrqFunc irqEnabler, uint32_t enablerArg); 
-typedef BiosError_t (*readFunc)(void* data, uint16_t* x, uint16_t* y, uint16_t* z);
-typedef BiosError_t (*nextFunc)(void* data, uint16_t* x, uint16_t* y, bool* last); 
-typedef BiosError_t (*waitCalibFunc)(void* data, bool* morePoints, uint32_t timeoutMs);
-
-typedef struct {
-  initParamFunc initParams;
-    
-  simpleFunc    displayInit;          
-  powerUpFunc   displayPowerUp;    
-  simpleFunc    displayPowerDown;
-  backlightFunc displayBacklight;
-  infoFunc      displayInformation;
-    
-  touchInitFunc touchInit;          
-  readFunc      touchRead;    
-  simpleFunc    touchCalibrateStart;
-  nextFunc      touchGetNextCalibPoint;
-  waitCalibFunc touchWaitForCalibratePoint;  
-  simpleFunc    touchIrqHandler;    
-} bios_header_t;
-
-typedef struct {
-  uint32_t magic;
-  uint32_t size;
-  uint32_t crc;
-  uint32_t version;
-  uint32_t paramSize;    
-  uint32_t headerSize;
-  bios_header_t header;
-} file_header_t;
-
-#endif /* BIOS_H */
--- a/FileSystems/QSPIFileSystem.cpp	Mon Jan 12 10:37:57 2015 +0100
+++ b/FileSystems/QSPIFileSystem.cpp	Fri Jan 16 11:13:39 2015 +0100
@@ -190,71 +190,30 @@
     /* Make sure it is a tested flash module */
     switch (SPIFI::instance().device()) {
       case SPIFI::Spansion_S25FL032:
-        /* For the Spansion memory the TOC occupies 256bytes and the TOC block will
-           hold 256 TOCs. */
         strcpy(memInfo.memName, "Spansion S25FL032");
-        memInfo.memSize        = obj->memSize;
-        memInfo.eraseBlockSize = 64*1024;
-        memInfo.numEraseBlocks = memInfo.memSize / memInfo.eraseBlockSize;
-        memInfo.tocSizeInBytes = sizeof(toc_entry_t) * memInfo.numEraseBlocks;
-        memInfo.numTocs        = memInfo.eraseBlockSize / memInfo.tocSizeInBytes;
-        memInfo.tocBlockAddr   = SPIFI_MEM_BASE + (NUM_BLOCKS * ERASE_SIZE) - (memInfo.numTocs * memInfo.tocSizeInBytes);
         break;
       
       case SPIFI::Winbond_W25Q64FV:
-        /* For the Winbond memory the TOC occupies 8192 bytes and that is bigger than 
-           one erase block (which is 4096 bytes). It is possible to either keep only
-           one TOC or to create a couple to reduce wear on the memory. In this case 
-           the multiple TOCs option is used. */
         strcpy(memInfo.memName, "Winbond W25Q64FV");
-        memInfo.memSize        = obj->memSize;
-        memInfo.eraseBlockSize = 4*1024;
-        memInfo.numEraseBlocks = memInfo.memSize / memInfo.eraseBlockSize;
-        memInfo.tocSizeInBytes = sizeof(toc_entry_t) * memInfo.numEraseBlocks;
-        memInfo.numTocs        = 8;
-        memInfo.tocBlockAddr   = SPIFI_MEM_BASE + (NUM_BLOCKS * ERASE_SIZE) - (memInfo.numTocs * memInfo.tocSizeInBytes);      
         break;
       
       case SPIFI::Macronix_MX25L6435E:
-        /* For the Macronix memory the TOC occupies 8192 bytes and that is bigger than 
-           one erase block (which is 4096 bytes). It is possible to either keep only
-           one TOC or to create a couple to reduce wear on the memory. In this case 
-           the multiple TOCs option is used. */
         strcpy(memInfo.memName, "Macronix_MX25L6435E");
-        memInfo.memSize        = obj->memSize;
-        memInfo.eraseBlockSize = 4*1024;
-        memInfo.numEraseBlocks = memInfo.memSize / memInfo.eraseBlockSize;
-        memInfo.tocSizeInBytes = sizeof(toc_entry_t) * memInfo.numEraseBlocks;
-        memInfo.numTocs        = 8;
-        memInfo.tocBlockAddr   = SPIFI_MEM_BASE + (NUM_BLOCKS * ERASE_SIZE) - (memInfo.numTocs * memInfo.tocSizeInBytes);      
         break;
       
       case SPIFI::Macronix_MX25L12835F:
-        /* For the Macronix memory the TOC occupies 16384 bytes and that is bigger than 
-           one erase block (which is 4096 bytes). It is possible to either keep only
-           one TOC or to create a couple to reduce wear on the memory. In this case 
-           the multiple TOCs option is used. */
         strcpy(memInfo.memName, "Macronix_MX25L12835F");
-        memInfo.memSize        = obj->memSize;
-        memInfo.eraseBlockSize = 4*1024;
-        memInfo.numEraseBlocks = memInfo.memSize / memInfo.eraseBlockSize;
-        memInfo.tocSizeInBytes = sizeof(toc_entry_t) * memInfo.numEraseBlocks;
-        memInfo.numTocs        = 4;
-        memInfo.tocBlockAddr   = SPIFI_MEM_BASE + (NUM_BLOCKS * ERASE_SIZE) - (memInfo.numTocs * memInfo.tocSizeInBytes);      
+        break;
+      
+      case SPIFI::SpecifiedInBios:
+        strcpy(memInfo.memName, "Specified in BIOS");
         break;
       
       case SPIFI::UnknownDevice:
       default:
         debug("INIT: Memory is unknown and may not work as expected\n");
         
-        // Asuming it has 64Kb erase blocks (i.e. same setup as the Spansion S25FL032
         strcpy(memInfo.memName, "Unknown - check ID");
-        memInfo.memSize        = obj->memSize;
-        memInfo.eraseBlockSize = 64*1024;
-        memInfo.numEraseBlocks = memInfo.memSize / memInfo.eraseBlockSize;      
-        memInfo.tocSizeInBytes = sizeof(toc_entry_t) * memInfo.numEraseBlocks;
-        memInfo.numTocs        = memInfo.eraseBlockSize / memInfo.tocSizeInBytes;
-        memInfo.tocBlockAddr   = SPIFI_MEM_BASE + (NUM_BLOCKS * ERASE_SIZE) - (memInfo.numTocs * memInfo.tocSizeInBytes);      
 
         /*
          * If this happens, check the manufacturer and device information
@@ -264,6 +223,35 @@
          */
         break;
     }
+    
+    /*
+     * The size of the erase blocks gives the number of erase blocks on the
+     * flash which in turn dictates the size of each TOC. For a flash with
+     * a large erase block size (Spansion_S25FL032 has 64Kb blocks) the
+     * the number of blocks is low, resulting in a small TOC with many fitting
+     * inside a single erase block.
+     * For a large flash with small erase block size (Macronix_MX25L12835F is
+     * 16Mb with 4Kb erase blocks) the number of blocks is high, resulting in
+     * a large TOC which doesn't even fit in one erase block.
+     *
+     *  4Mb, 64Kb erase block => TOC size   256b => 256 TOC/erase block
+     *  8Mb,  4Kb erase block => TOC size  8192b => 0.5 TOC/erase block
+     * 16Mb,  4Kb erase block => TOC size 16384b => 0.25 TOC/erase block
+     *
+     * In all cases we select a number of TOCs so that we use 64Kb of the
+     * memory for them. This will reduce wear on the flash as the TOC can
+     * be moved around.
+     */
+    memInfo.memSize        = obj->memSize;
+    memInfo.eraseBlockSize = SPIFI::instance().eraseBlockSize();
+    memInfo.numEraseBlocks = memInfo.memSize / memInfo.eraseBlockSize;      
+    memInfo.tocSizeInBytes = sizeof(toc_entry_t) * memInfo.numEraseBlocks;
+    memInfo.numTocs        = (64*1024) / memInfo.tocSizeInBytes;
+    if (memInfo.numTocs < 2) {
+      // just in case a unknown size combination appears
+      memInfo.numTocs = 2;
+    }
+    memInfo.tocBlockAddr   = SPIFI_MEM_BASE + (NUM_BLOCKS * ERASE_SIZE) - (memInfo.numTocs * memInfo.tocSizeInBytes);      
       
     debug_if(QSPI_DBG, "INIT: Found %dMB %s\n", memInfo.memSize/0x100000, memInfo.memName);
     
--- a/Memory/SPIFI.cpp	Mon Jan 12 10:37:57 2015 +0100
+++ b/Memory/SPIFI.cpp	Fri Jan 16 11:13:39 2015 +0100
@@ -16,6 +16,7 @@
 
 #include "SPIFI.h"
 #include "mbed_debug.h"
+#include "BiosLoader.h"
 
 
 /******************************************************************************
@@ -172,7 +173,15 @@
       _device = Macronix_MX25L12835F;
       _memorySize = _romData->memSize;
       _eraseBlockSize = 4*1024;
-    } 
+    }
+    else if (BiosLoader::instance().isKnownSPIFIMemory(_romData->mfger, _romData->devType, _romData->devID, _romData->memSize, &_eraseBlockSize))
+    {
+      /* The BIOS was able to identify the FLASH and we will use the 
+       * eraseBlockSize specified in the BIOS.
+       */
+      _device = SpecifiedInBios;
+      _memorySize = _romData->memSize;
+    }
     else 
     {
       debug("SPIFI::init(): Memory is unknown and may not work as expected\n");
--- a/Memory/SPIFI.h	Mon Jan 12 10:37:57 2015 +0100
+++ b/Memory/SPIFI.h	Fri Jan 16 11:13:39 2015 +0100
@@ -87,6 +87,7 @@
         Winbond_W25Q64FV,       /* Manufacturer: 0xEF, devType: 0x40, devID: 0x17 */
         Macronix_MX25L6435E,  /* Manufacturer: 0xC2, devType: 0x20, devID: 0x17 */
         Macronix_MX25L12835F, /* Manufacturer: 0xC2, devType: 0x20, devID: 0x18 */
+        SpecifiedInBios,      /* The device is known by the BIOS */
         UnknownDevice
     };