From 8015f0f626bbe5dfa201ab718b01c8d5eaf53caf Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 12 Jan 2013 22:28:53 +1100
Subject: [PATCH] DataFlash: update SITL implementaion for new API

---
 libraries/DataFlash/DataFlash_SITL.cpp | 24 ++++++++++++++++++++++++
 libraries/DataFlash/DataFlash_SITL.h   | 13 +++++++++++++
 2 files changed, 37 insertions(+)

diff --git a/libraries/DataFlash/DataFlash_SITL.cpp b/libraries/DataFlash/DataFlash_SITL.cpp
index 05b3909eff..1ca47962f7 100644
--- a/libraries/DataFlash/DataFlash_SITL.cpp
+++ b/libraries/DataFlash/DataFlash_SITL.cpp
@@ -97,11 +97,35 @@ void DataFlash_SITL::BufferWrite (unsigned char BufferNum, uint16_t IntPageAdr,
 	buffer[BufferNum][IntPageAdr] = (uint8_t)Data;
 }
 
+void DataFlash_SITL::BlockWrite(uint8_t BufferNum, uint16_t IntPageAdr, 
+                                const void *pHeader, uint8_t hdr_size,
+                                const void *pBuffer, uint16_t size)
+{
+    if (hdr_size) {
+        memcpy(&buffer[BufferNum][IntPageAdr],
+               pHeader,
+               hdr_size);
+    }
+    memcpy(&buffer[BufferNum][IntPageAdr+hdr_size],
+           pBuffer,
+           size);
+}
+
 unsigned char DataFlash_SITL::BufferRead (unsigned char BufferNum, uint16_t IntPageAdr)
 {
 	return (unsigned char)buffer[BufferNum][IntPageAdr];
 }
 
+// read size bytes of data to a page. The caller must ensure that
+// the data fits within the page, otherwise it will wrap to the
+// start of the page
+bool DataFlash_SITL::BlockRead(uint8_t BufferNum, uint16_t IntPageAdr, void *pBuffer, uint16_t size)
+{
+	memcpy(pBuffer, &buffer[BufferNum][IntPageAdr], size);
+    return true;
+}
+
+
 // *** END OF INTERNAL FUNCTIONS ***
 
 void DataFlash_SITL::PageErase (uint16_t PageAdr)
diff --git a/libraries/DataFlash/DataFlash_SITL.h b/libraries/DataFlash/DataFlash_SITL.h
index 03a143ce8d..dbf8214294 100644
--- a/libraries/DataFlash/DataFlash_SITL.h
+++ b/libraries/DataFlash/DataFlash_SITL.h
@@ -24,6 +24,19 @@ private:
     void                    PageErase (uint16_t PageAdr);
     void                    BlockErase (uint16_t BlockAdr);
     void                    ChipErase();
+
+    // write size bytes of data to a page. The caller must ensure that
+    // the data fits within the page, otherwise it will wrap to the
+    // start of the page
+    // If pHeader is not NULL then write the header bytes before the data
+    void		    BlockWrite(uint8_t BufferNum, uint16_t IntPageAdr, 
+				       const void *pHeader, uint8_t hdr_size,
+				       const void *pBuffer, uint16_t size);
+
+    // read size bytes of data to a page. The caller must ensure that
+    // the data fits within the page, otherwise it will wrap to the
+    // start of the page
+    bool 		    BlockRead(uint8_t BufferNum, uint16_t IntPageAdr, void *pBuffer, uint16_t size);
     
     AP_HAL::SPIDeviceDriver *_spi;
     AP_HAL::Semaphore *_spi_sem;