From dd6d52038557d9c570d8dbd42a067ae58978cf33 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Mon, 25 Aug 2014 21:48:48 +0530
Subject: [PATCH] HAL_Linux:Create subclass to handle FRAM Storage under main
 Storage Class

---
 .../AP_HAL_Linux/AP_HAL_Linux_Namespace.h     |   2 +
 libraries/AP_HAL_Linux/HAL_Linux_Class.cpp    |   8 +
 libraries/AP_HAL_Linux/Storage.h              |  41 ++-
 libraries/AP_HAL_Linux/Storage_FRAM.cpp       | 289 ++++++------------
 libraries/AP_HAL_Linux/Storage_FRAM.h         |  30 +-
 libraries/AP_HAL_Linux/Storage_FS.cpp         |   4 -
 libraries/AP_HAL_Linux/Storage_FS.h           |  45 ---
 7 files changed, 140 insertions(+), 279 deletions(-)
 delete mode 100644 libraries/AP_HAL_Linux/Storage_FS.h

diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h
index 93fd3f5f70..748682156c 100644
--- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h
+++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h
@@ -17,6 +17,8 @@ namespace Linux {
     class LinuxStorage;
     class LinuxGPIO_BBB;
     class LinuxGPIO_RPI;
+    class LinuxStorage;
+    class LinuxStorage_FRAM;
     class LinuxDigitalSource;
     class LinuxRCInput;
     class LinuxRCInput_PRU;
diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
index b7315a6e1f..826070ee92 100644
--- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
+++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
@@ -23,7 +23,15 @@ static LinuxSemaphore  i2cSemaphore;
 static LinuxI2CDriver  i2cDriver(&i2cSemaphore, "/dev/i2c-1");
 static LinuxSPIDeviceManager spiDeviceManager;
 static LinuxAnalogIn analogIn;
+
+/*
+  select between FRAM and FS
+ */
+#if LINUX_STORAGE_USE_FRAM == 1
+static LinuxStorage_FRAM storageDriver;
+#else
 static LinuxStorage storageDriver;
+#endif
 
 /*
   use the BBB gpio driver on ERLE and PXF
diff --git a/libraries/AP_HAL_Linux/Storage.h b/libraries/AP_HAL_Linux/Storage.h
index 8ac7dbf802..f89c34815a 100644
--- a/libraries/AP_HAL_Linux/Storage.h
+++ b/libraries/AP_HAL_Linux/Storage.h
@@ -7,12 +7,45 @@
 #define LINUX_STORAGE_USE_FRAM 0
 #endif
 
+#include <AP_HAL.h>
+#include "AP_HAL_Linux_Namespace.h"
+
+#define LINUX_STORAGE_SIZE 4096
+#define LINUX_STORAGE_MAX_WRITE 512
+#define LINUX_STORAGE_LINE_SHIFT 9
+#define LINUX_STORAGE_LINE_SIZE (1<<LINUX_STORAGE_LINE_SHIFT)
+#define LINUX_STORAGE_NUM_LINES (LINUX_STORAGE_SIZE/LINUX_STORAGE_LINE_SIZE)
+
+class Linux::LinuxStorage : public AP_HAL::Storage 
+{
+public:
+    LinuxStorage() :
+	_fd(-1),
+	_dirty_mask(0)
+	{}
+    void init(void* machtnichts) {}
+    uint8_t  read_byte(uint16_t loc);
+    uint16_t read_word(uint16_t loc);
+    uint32_t read_dword(uint16_t loc);
+    void     read_block(void *dst, uint16_t src, size_t n);
+
+    void write_byte(uint16_t loc, uint8_t value);
+    void write_word(uint16_t loc, uint16_t value);
+    void write_dword(uint16_t loc, uint32_t value);
+    void write_block(uint16_t dst, const void* src, size_t n);
+
+    virtual void _timer_tick(void);
+protected:
+    void _mark_dirty(uint16_t loc, uint16_t length);
+    virtual void _storage_create(void);
+    virtual void _storage_open(void);
+    int _fd;
+    volatile bool _initialised;
+    uint8_t _buffer[LINUX_STORAGE_SIZE];
+    volatile uint32_t _dirty_mask;
+};
 
-#if LINUX_STORAGE_USE_FRAM
 #include "Storage_FRAM.h"
-#else
-#include "Storage_FS.h"
-#endif
 
 #endif // __AP_HAL_LINUX_STORAGE_H__
 
diff --git a/libraries/AP_HAL_Linux/Storage_FRAM.cpp b/libraries/AP_HAL_Linux/Storage_FRAM.cpp
index 90e6d4b6f7..9f1bbdc32c 100644
--- a/libraries/AP_HAL_Linux/Storage_FRAM.cpp
+++ b/libraries/AP_HAL_Linux/Storage_FRAM.cpp
@@ -1,7 +1,6 @@
 #include <AP_HAL.h>
-#include "Storage.h"
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && LINUX_STORAGE_USE_FRAM
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
 #include <assert.h>
 #include <sys/types.h>
 #include <sys/stat.h>
@@ -9,6 +8,7 @@
 #include <unistd.h>
 #include <errno.h>
 #include <stdio.h>
+#include "Storage.h"
 
 using namespace Linux;
 
@@ -21,16 +21,12 @@ using namespace Linux;
 // card for ArduCopter and ArduPlane
 
 extern const AP_HAL::HAL& hal;
-LinuxStorage::LinuxStorage():
-_fd(-1),
-_dirty_mask(0),
+LinuxStorage_FRAM::LinuxStorage_FRAM():
 _spi(NULL),
-_spi_sem(NULL),
-_initialised(false)
+_spi_sem(NULL)
 {}
 
-
-void LinuxStorage::_storage_create(void)
+void LinuxStorage_FRAM::_storage_create(void)
 {
 
     int fd = open();
@@ -51,7 +47,7 @@ void LinuxStorage::_storage_create(void)
     close(fd);
 }
 
-void LinuxStorage::_storage_open(void)
+void LinuxStorage_FRAM::_storage_open(void)
 {
     if (_initialised) {
         return;
@@ -80,112 +76,7 @@ void LinuxStorage::_storage_open(void)
     _initialised = true;
 }
 
-/*
-  mark some lines as dirty. Note that there is no attempt to avoid
-  the race condition between this code and the _timer_tick() code
-  below, which both update _dirty_mask. If we lose the race then the
-  result is that a line is written more than once, but it won't result
-  in a line not being written.
- */
-void LinuxStorage::_mark_dirty(uint16_t loc, uint16_t length)
-{
-    uint16_t end = loc + length;
-    for (uint8_t line=loc>>LINUX_STORAGE_LINE_SHIFT;
-         line <= end>>LINUX_STORAGE_LINE_SHIFT;
-         line++) {
-        _dirty_mask |= 1U << line;
-    }
-}
-
-uint8_t LinuxStorage::read_byte(uint16_t loc) 
-{
-    if (loc >= sizeof(_buffer)) {
-        return 0;
-    }
-    _storage_open();
-    return _buffer[loc];
-}
-
-uint16_t LinuxStorage::read_word(uint16_t loc) 
-{
-    uint16_t value;
-    if (loc >= sizeof(_buffer)-(sizeof(value)-1)) {
-        return 0;
-    }
-    _storage_open();
-    memcpy(&value, &_buffer[loc], sizeof(value));
-    return value;
-}
-
-uint32_t LinuxStorage::read_dword(uint16_t loc) 
-{
-    uint32_t value;
-    if (loc >= sizeof(_buffer)-(sizeof(value)-1)) {
-        return 0;
-    }
-    _storage_open();
-    memcpy(&value, &_buffer[loc], sizeof(value));
-    return value;
-}
-
-void LinuxStorage::read_block(void *dst, uint16_t loc, size_t n) 
-{
-    if (loc >= sizeof(_buffer)-(n-1)) {
-        return;
-    }
-    _storage_open();
-    memcpy(dst, &_buffer[loc], n);
-}
-
-void LinuxStorage::write_byte(uint16_t loc, uint8_t value) 
-{
-    if (loc >= sizeof(_buffer)) {
-        return;
-    }
-    if (_buffer[loc] != value) {
-        _storage_open();
-        _buffer[loc] = value;
-        _mark_dirty(loc, sizeof(value));
-    }
-}
-
-void LinuxStorage::write_word(uint16_t loc, uint16_t value) 
-{
-    if (loc >= sizeof(_buffer)-(sizeof(value)-1)) {
-        return;
-    }
-    if (memcmp(&value, &_buffer[loc], sizeof(value)) != 0) {
-        _storage_open();
-        memcpy(&_buffer[loc], &value, sizeof(value));
-        _mark_dirty(loc, sizeof(value));
-    }
-}
-
-void LinuxStorage::write_dword(uint16_t loc, uint32_t value) 
-{
-    if (loc >= sizeof(_buffer)-(sizeof(value)-1)) {
-        return;
-    }
-    if (memcmp(&value, &_buffer[loc], sizeof(value)) != 0) {
-        _storage_open();
-        memcpy(&_buffer[loc], &value, sizeof(value));
-        _mark_dirty(loc, sizeof(value));
-    }
-}
-
-void LinuxStorage::write_block(uint16_t loc, const void *src, size_t n) 
-{
-    if (loc >= sizeof(_buffer)-(n-1)) {
-        return;
-    }
-    if (memcmp(src, &_buffer[loc], n) != 0) {
-        _storage_open();
-        memcpy(&_buffer[loc], src, n);
-        _mark_dirty(loc, n);
-    }
-}
-
-void LinuxStorage::_timer_tick(void)
+void LinuxStorage_FRAM::_timer_tick(void)
 {
     if (!_initialised || _dirty_mask == 0) {
         return;
@@ -195,51 +86,51 @@ void LinuxStorage::_timer_tick(void)
         _fd = open();
         if (_fd == -1) {
             return;
-		}
-	}
+        }
+    }
 
-	// write out the first dirty set of lines. We don't write more
-	// than one to keep the latency of this call to a minimum
-	uint8_t i, n;
-	for (i=0; i<LINUX_STORAGE_NUM_LINES; i++) {
-		if (_dirty_mask & (1<<i)) {
-			break;
-		}
-	}
-	if (i == LINUX_STORAGE_NUM_LINES) {
-		// this shouldn't be possible
-		return;
-	}
-	uint32_t write_mask = (1U<<i);
-	// see how many lines to write
-	for (n=1; (i+n) < LINUX_STORAGE_NUM_LINES && 
-		     n < (LINUX_STORAGE_MAX_WRITE>>LINUX_STORAGE_LINE_SHIFT); n++) {
-		if (!(_dirty_mask & (1<<(n+i)))) {
-			break;
-		}		
-		// mark that line clean
-		write_mask |= (1<<(n+i));
-	}
+    // write out the first dirty set of lines. We don't write more
+    // than one to keep the latency of this call to a minimum
+    uint8_t i, n;
+    for (i=0; i<LINUX_STORAGE_NUM_LINES; i++) {
+        if (_dirty_mask & (1<<i)) {
+            break;
+        }
+    }
+    if (i == LINUX_STORAGE_NUM_LINES) {
+        // this shouldn't be possible
+        return;
+    }
+    uint32_t write_mask = (1U<<i);
+    // see how many lines to write
+    for (n=1; (i+n) < LINUX_STORAGE_NUM_LINES && 
+             n < (LINUX_STORAGE_MAX_WRITE>>LINUX_STORAGE_LINE_SHIFT); n++) {
+        if (!(_dirty_mask & (1<<(n+i)))) {
+            break;
+        }        
+        // mark that line clean
+        write_mask |= (1<<(n+i));
+    }
 
-	/*
-	  write the lines. This also updates _dirty_mask. Note that
-	  because this is a SCHED_FIFO thread it will not be preempted
-	  by the main task except during blocking calls. This means we
-	  don't need a semaphore around the _dirty_mask updates.
-	 */
-	if (lseek(_fd, i<<LINUX_STORAGE_LINE_SHIFT, SEEK_SET) == (i<<LINUX_STORAGE_LINE_SHIFT)) {
-		_dirty_mask &= ~write_mask;
-		if (write(_fd, &_buffer[i<<LINUX_STORAGE_LINE_SHIFT], n<<LINUX_STORAGE_LINE_SHIFT) != n<<LINUX_STORAGE_LINE_SHIFT) {
-			// write error - likely EINTR
-			_dirty_mask |= write_mask;
-			_fd = -1;
-		}
-	}
+    /*
+      write the lines. This also updates _dirty_mask. Note that
+      because this is a SCHED_FIFO thread it will not be preempted
+      by the main task except during blocking calls. This means we
+      don't need a semaphore around the _dirty_mask updates.
+     */
+    if (lseek(_fd, i<<LINUX_STORAGE_LINE_SHIFT, SEEK_SET) == (uint32_t)(i<<LINUX_STORAGE_LINE_SHIFT)) {
+        _dirty_mask &= ~write_mask;
+        if (write(_fd, &_buffer[i<<LINUX_STORAGE_LINE_SHIFT], n<<LINUX_STORAGE_LINE_SHIFT) != n<<LINUX_STORAGE_LINE_SHIFT) {
+            // write error - likely EINTR
+            _dirty_mask |= write_mask;
+            _fd = -1;
+        }
+    }
 }
 
 //File control function overloads
 
-int8_t LinuxStorage::open()
+int8_t LinuxStorage_FRAM::open()
 {
     if(_initialised){
         return 0;
@@ -279,92 +170,92 @@ int8_t LinuxStorage::open()
     return 0;
 }
 
-int32_t LinuxStorage::write(uint16_t fd,uint8_t *Buff, uint16_t NumBytes){
-	if( _register_write(Buff,fptr,NumBytes) == -1){
-		return -1;
-	}
-	return NumBytes;
+int32_t LinuxStorage_FRAM::write(uint16_t fd,uint8_t *Buff, uint16_t NumBytes){
+    if( _register_write(Buff,fptr,NumBytes) == -1){
+        return -1;
+    }
+    return NumBytes;
 }
-int32_t LinuxStorage::read(uint16_t fd, uint8_t *Buff, uint16_t NumBytes){
+int32_t LinuxStorage_FRAM::read(uint16_t fd, uint8_t *Buff, uint16_t NumBytes){
     for(uint16_t i=fptr;i<(fptr+NumBytes);i++){
         Buff[i-fptr]= _register_read(i,OPCODE_READ);
 
-		if(Buff[i-fptr]==-1){
-			return -1;
-		}
+        if(Buff[i-fptr]==-1){
+            return -1;
+        }
     }
     fptr+=NumBytes; 
     return NumBytes; 
 }
-uint32_t LinuxStorage::lseek(uint16_t fd,uint32_t offset,uint16_t whence){
+uint32_t LinuxStorage_FRAM::lseek(uint16_t fd,uint32_t offset,uint16_t whence){
     fptr = offset;
     return offset; 
 }
 
 //FRAM I/O functions
 
-int8_t LinuxStorage::_register_write( uint8_t* src, uint16_t addr, uint16_t len ){
+int8_t LinuxStorage_FRAM::_register_write( uint8_t* src, uint16_t addr, uint16_t len ){
 
     uint8_t *tx;
     uint8_t *rx;
     uint16_t i;
-	tx = new uint8_t[len+3];
-	rx = new uint8_t[len+3];
-	_write_enable(true);
+    tx = new uint8_t[len+3];
+    rx = new uint8_t[len+3];
+    _write_enable(true);
     
     tx[0] = OPCODE_WRITE;
     tx[1] = addr>>8;
     tx[2] = addr;
-	
+    
     for(i=0;i<len;i++){
-		tx[i+3] = src[i];
-	}
-	if(transaction(tx, rx, len+3) == -1){
-		return -1;
-	}
-	if(_write_enable(false) == -1){
-		return -1;
-	}
-	return len;
+        tx[i+3] = src[i];
+    }
+    if(transaction(tx, rx, len+3) == -1){
+        return -1;
+    }
+    if(_write_enable(false) == -1){
+        return -1;
+    }
+    return len;
 }
 
-int8_t LinuxStorage::_write_enable(bool enable)
+int8_t LinuxStorage_FRAM::_write_enable(bool enable)
 {
     uint8_t tx[2];
     uint8_t rx[2];
-	if(enable){
-	    tx[0] = OPCODE_WREN;
-	    tx[1] = 0;
-	    return transaction(tx, rx, 2);
-	}
-	else{
-	    tx[0] = OPCODE_WRDI;
-	    tx[1] = 0;
-	    return transaction(tx, rx, 2);
-	}
-	
+    if(enable){
+        tx[0] = OPCODE_WREN;
+        tx[1] = 0;
+        return transaction(tx, rx, 2);
+    }
+    else{
+        tx[0] = OPCODE_WRDI;
+        tx[1] = 0;
+        return transaction(tx, rx, 2);
+    }
+    
 }
 
-int8_t LinuxStorage::_register_read( uint16_t addr, uint8_t opcode )
+int8_t LinuxStorage_FRAM::_register_read( uint16_t addr, uint8_t opcode )
 {
-    uint8_t tx[4] = {opcode, addr>>8, addr, 0};
+    uint8_t tx[4] = {opcode, addr >> 8U, addr, 0};
     uint8_t rx[4];
     
     if(transaction(tx, rx, 4) == -1){
-    	return -1;
+        return -1;
     }
     return rx[3];
 }
 
-int8_t LinuxStorage::transaction(uint8_t* tx, uint8_t* rx, uint16_t len){
-	_spi_sem = _spi->get_semaphore();
+int8_t LinuxStorage_FRAM::transaction(uint8_t* tx, uint8_t* rx, uint16_t len){
+    _spi_sem = _spi->get_semaphore();
     if (!_spi_sem->take(100)) {
        // FRAM: Unable to get semaphore
-		return -1;
+        return -1;
     }
     _spi->transaction(tx, rx, len);
-	_spi_sem->give();
-	return 0;
+    _spi_sem->give();
+    return 0;
 }
 
 #endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_HAL_Linux/Storage_FRAM.h b/libraries/AP_HAL_Linux/Storage_FRAM.h
index b9b3e64ec0..225417b5fd 100644
--- a/libraries/AP_HAL_Linux/Storage_FRAM.h
+++ b/libraries/AP_HAL_Linux/Storage_FRAM.h
@@ -12,34 +12,15 @@
 #define OPCODE_WRITE  0b0010        /* Write Memory */
 #define OPCODE_RDID   0b10011111    /* Read Device ID */
 
-#define LINUX_STORAGE_SIZE 4096
-#define LINUX_STORAGE_MAX_WRITE 512
-#define LINUX_STORAGE_LINE_SHIFT 9
-#define LINUX_STORAGE_LINE_SIZE (1<<LINUX_STORAGE_LINE_SHIFT)
-#define LINUX_STORAGE_NUM_LINES (LINUX_STORAGE_SIZE/LINUX_STORAGE_LINE_SIZE)
-
-class Linux::LinuxStorage : public AP_HAL::Storage 
+class Linux::LinuxStorage_FRAM : public Linux::LinuxStorage 
 {
 public:
-    LinuxStorage();
-    void init(void* machtnichts) {}
-    uint8_t  read_byte(uint16_t loc);
-    uint16_t read_word(uint16_t loc);
-    uint32_t read_dword(uint16_t loc);
-    void     read_block(void *dst, uint16_t src, size_t n);
-
-    void write_byte(uint16_t loc, uint8_t value);
-    void write_word(uint16_t loc, uint16_t value);
-    void write_dword(uint16_t loc, uint32_t value);
-    void write_block(uint16_t dst, const void* src, size_t n);
-
+    LinuxStorage_FRAM();
     void _timer_tick(void);
 
 private:
     uint32_t fptr;
-    int _fd;
-    volatile bool _initialised;
-
+   
     int32_t write(uint16_t fd, uint8_t *Buff, uint16_t NumBytes);
     int32_t read(uint16_t fd, uint8_t *Buff, uint16_t NumBytes);
     uint32_t lseek(uint16_t fd,uint32_t offset,uint16_t whence);
@@ -52,13 +33,8 @@ private:
 
     void _storage_create(void);
     void _storage_open(void);
-    void _mark_dirty(uint16_t loc, uint16_t length);
-    
 
     AP_HAL::SPIDeviceDriver *_spi;
     AP_HAL::Semaphore *_spi_sem;
-
-    uint8_t _buffer[LINUX_STORAGE_SIZE];
-    volatile uint32_t _dirty_mask;
 };
 #endif // __AP_HAL_LINUX_STORAGE_H__
diff --git a/libraries/AP_HAL_Linux/Storage_FS.cpp b/libraries/AP_HAL_Linux/Storage_FS.cpp
index 97dd0a218a..b5e2a58d85 100644
--- a/libraries/AP_HAL_Linux/Storage_FS.cpp
+++ b/libraries/AP_HAL_Linux/Storage_FS.cpp
@@ -1,8 +1,6 @@
 #include <AP_HAL.h>
 #include "Storage.h"
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && !LINUX_STORAGE_USE_FRAM
-
 #include <assert.h>
 #include <sys/types.h>
 #include <sys/stat.h>
@@ -169,5 +167,3 @@ void LinuxStorage::_timer_tick(void)
 		}
 	}
 }
-
-#endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_HAL_Linux/Storage_FS.h b/libraries/AP_HAL_Linux/Storage_FS.h
deleted file mode 100644
index 5f300e65c1..0000000000
--- a/libraries/AP_HAL_Linux/Storage_FS.h
+++ /dev/null
@@ -1,45 +0,0 @@
-
-
-#ifndef __AP_HAL_LINUX_STORAGE_FS_H__
-#define __AP_HAL_LINUX_STORAGE_FS_H__
-
-#include <AP_HAL.h>
-#include "AP_HAL_Linux_Namespace.h"
-
-#define LINUX_STORAGE_SIZE 4096
-#define LINUX_STORAGE_MAX_WRITE 512
-#define LINUX_STORAGE_LINE_SHIFT 9
-#define LINUX_STORAGE_LINE_SIZE (1<<LINUX_STORAGE_LINE_SHIFT)
-#define LINUX_STORAGE_NUM_LINES (LINUX_STORAGE_SIZE/LINUX_STORAGE_LINE_SIZE)
-
-class Linux::LinuxStorage : public AP_HAL::Storage 
-{
-public:
-    LinuxStorage() :
-	_fd(-1),
-	_dirty_mask(0)
-	{}
-    void init(void* machtnichts) {}
-    uint8_t  read_byte(uint16_t loc);
-    uint16_t read_word(uint16_t loc);
-    uint32_t read_dword(uint16_t loc);
-    void     read_block(void *dst, uint16_t src, size_t n);
-
-    void write_byte(uint16_t loc, uint8_t value);
-    void write_word(uint16_t loc, uint16_t value);
-    void write_dword(uint16_t loc, uint32_t value);
-    void write_block(uint16_t dst, const void* src, size_t n);
-
-    void _timer_tick(void);
-
-private:
-    int _fd;
-    volatile bool _initialised;
-    void _storage_create(void);
-    void _storage_open(void);
-    void _mark_dirty(uint16_t loc, uint16_t length);
-    uint8_t _buffer[LINUX_STORAGE_SIZE];
-    volatile uint32_t _dirty_mask;
-};
-
-#endif // __AP_HAL_LINUX_STORAGE_H__
\ No newline at end of file