From 390e96311e0978686b993347b1af84ba3cef7cb9 Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Thu, 3 Jan 2013 10:34:25 -0800 Subject: [PATCH] DataFlash: change to new style Semaphore library --- libraries/DataFlash/DataFlash_APM1.cpp | 110 ++++++++++--------------- libraries/DataFlash/DataFlash_APM2.cpp | 88 +++++++------------- 2 files changed, 75 insertions(+), 123 deletions(-) diff --git a/libraries/DataFlash/DataFlash_APM1.cpp b/libraries/DataFlash/DataFlash_APM1.cpp index 8bee1341c5..336cad4734 100644 --- a/libraries/DataFlash/DataFlash_APM1.cpp +++ b/libraries/DataFlash/DataFlash_APM1.cpp @@ -86,11 +86,17 @@ void DataFlash_APM1::Init(void) _spi = hal.spi->device(AP_HAL::SPIDevice_Dataflash); if (_spi == NULL) { - hal.console->println_P( + hal.scheduler->panic( PSTR("PANIC: DataFlash SPIDeviceDriver not found")); - return; + return; /* never reached */ } + _spi_sem = _spi->get_semaphore(); + if (_spi_sem == NULL) { + hal.scheduler->panic( + PSTR("PANIC: DataFlash SPIDeviceDriver semaphore is null")); + return; /* never reached */ + } // get page size: 512 or 528 (by default: 528) df_PageSize = PageSize(); @@ -102,10 +108,8 @@ void DataFlash_APM1::Init(void) // This function is mainly to test the device void DataFlash_APM1::ReadManufacturerID() { - if (_spi_sem) { - bool got = _spi_sem->get(this); - if (!got) return; - } + if (!_spi_sem->take(5)) + return; // activate dataflash command decoder _spi->cs_assert(); @@ -120,9 +124,7 @@ void DataFlash_APM1::ReadManufacturerID() // release SPI bus for use by other sensors _spi->cs_release(); - if (_spi_sem) { - _spi_sem->release(this); - } + _spi_sem->give(); } // This function return 1 if Card is inserted on SD slot @@ -161,15 +163,12 @@ uint8_t DataFlash_APM1::ReadStatus() inline uint16_t DataFlash_APM1::PageSize() { - if (_spi_sem) { - bool got = _spi_sem->get(this); - if (!got) return 0; - } + if (!_spi_sem->take(5)) + return 0; + return(528-((ReadStatusReg()&0x01) << 4)); // if first bit 1 trhen 512 else 528 bytes - if (_spi_sem) { - _spi_sem->release(this); - } + _spi_sem->give(); } // Wait until DataFlash is in ready state... @@ -181,10 +180,9 @@ void DataFlash_APM1::WaitReady() void DataFlash_APM1::PageToBuffer(uint8_t BufferNum, uint16_t PageAdr) { - if (_spi_sem) { - bool got = _spi_sem->get(this); - if (!got) return; - } + if (!_spi_sem->take(1)) + return; + // activate dataflash command decoder _spi->cs_assert(); @@ -206,17 +204,14 @@ void DataFlash_APM1::PageToBuffer(uint8_t BufferNum, uint16_t PageAdr) _spi->cs_release(); while(!ReadStatus()) ; //monitor the status register, wait until busy-flag is high - if (_spi_sem) { - _spi_sem->release(this); - } + _spi_sem->give(); } void DataFlash_APM1::BufferToPage (uint8_t BufferNum, uint16_t PageAdr, uint8_t wait) { - if (_spi_sem) { - bool got = _spi_sem->get(this); - if (!got) return; - } + if (!_spi_sem->take(1)) + return; + // activate dataflash command decoder _spi->cs_assert(); @@ -240,18 +235,15 @@ void DataFlash_APM1::BufferToPage (uint8_t BufferNum, uint16_t PageAdr, uint8_t // Check if we need to wait to write the buffer to memory or we can continue... if (wait) while(!ReadStatus()) ; //monitor the status register, wait until busy-flag is high - if (_spi_sem) { - _spi_sem->release(this); - } + _spi_sem->give(); } void DataFlash_APM1::BufferWrite (uint8_t BufferNum, uint16_t IntPageAdr, uint8_t Data) { - if (_spi_sem) { - bool got = _spi_sem->get(this); - if (!got) return; - } + if (!_spi_sem->take(1)) + return; + // activate dataflash command decoder _spi->cs_assert(); @@ -267,19 +259,16 @@ void DataFlash_APM1::BufferWrite (uint8_t BufferNum, uint16_t IntPageAdr, uint8_ // release SPI bus for use by other sensors _spi->cs_release(); - if (_spi_sem) { - _spi_sem->release(this); - } + _spi_sem->give(); } uint8_t DataFlash_APM1::BufferRead (uint8_t BufferNum, uint16_t IntPageAdr) { uint8_t tmp; - if (_spi_sem) { - bool got = _spi_sem->get(this); - if (!got) return 0; - } + if (!_spi_sem->take(1)) + return 0; + // activate dataflash command decoder _spi->cs_assert(); @@ -297,19 +286,16 @@ uint8_t DataFlash_APM1::BufferRead (uint8_t BufferNum, uint16_t IntPageAdr) // release SPI bus for use by other sensors _spi->cs_release(); - if (_spi_sem) { - _spi_sem->release(this); - } + _spi_sem->give(); return (tmp); } // *** END OF INTERNAL FUNCTIONS *** void DataFlash_APM1::PageErase (uint16_t PageAdr) { - if (_spi_sem) { - bool got = _spi_sem->get(this); - if (!got) return; - } + if (!_spi_sem->take(1)) + return; + // activate dataflash command decoder _spi->cs_assert(); @@ -329,18 +315,15 @@ void DataFlash_APM1::PageErase (uint16_t PageAdr) //initiate flash page erase _spi->cs_release(); + _spi_sem->give(); while(!ReadStatus()) ; - if (_spi_sem) { - _spi_sem->release(this); - } } void DataFlash_APM1::BlockErase (uint16_t BlockAdr) { - if (_spi_sem) { - bool got = _spi_sem->get(this); - if (!got) return; - } + if (!_spi_sem->take(1)) + return; + // activate dataflash command decoder _spi->cs_assert(); @@ -370,19 +353,14 @@ void DataFlash_APM1::BlockErase (uint16_t BlockAdr) //initiate flash page erase _spi->cs_release(); while(!ReadStatus()) ; - if (_spi_sem) { - _spi_sem->release(this); - } + _spi_sem->give(); } void DataFlash_APM1::ChipErase() { - //serialDebug("Chip Erase\n"); - if (_spi_sem) { - bool got = _spi_sem->get(this); - if (!got) return; - } + if (!_spi_sem->take(5)) + return; // activate dataflash command decoder _spi->cs_assert(); @@ -400,8 +378,6 @@ void DataFlash_APM1::ChipErase() hal.scheduler->delay(6); } - if (_spi_sem) { - _spi_sem->release(this); - } - + _spi_sem->give(); + } diff --git a/libraries/DataFlash/DataFlash_APM2.cpp b/libraries/DataFlash/DataFlash_APM2.cpp index 80f95f3731..70aa0197fb 100644 --- a/libraries/DataFlash/DataFlash_APM2.cpp +++ b/libraries/DataFlash/DataFlash_APM2.cpp @@ -87,24 +87,26 @@ void DataFlash_APM2::Init(void) _spi = hal.spi->device(AP_HAL::SPIDevice_Dataflash); if (_spi == NULL) { - hal.console->println_P( + hal.scheduler->panic( PSTR("PANIC: DataFlash SPIDeviceDriver not found")); return; } _spi_sem = _spi->get_semaphore(); - if (_spi_sem) { - bool got = _spi_sem->get(this); - if (!got) return; + if (_spi_sem == NULL) { + hal.scheduler->panic( + PSTR("PANIC: DataFlash SPIDeviceDriver semaphore is null")); + return; /* never reached */ } + if (!_spi_sem->take(5)) + return; // get page size: 512 or 528 (by default: 528) df_PageSize=PageSize(); ReadManufacturerID(); + + _spi_sem->give(); - if (_spi_sem) { - _spi_sem->release(this); - } // see page 22 of the spec for the density code uint8_t density_code = (df_device >> 8) & 0x1F; @@ -186,10 +188,9 @@ void DataFlash_APM2::WaitReady() void DataFlash_APM2::PageToBuffer(uint8_t BufferNum, uint16_t PageAdr) { - if (_spi_sem) { - bool got = _spi_sem->get(this); - if (!got) return; - } + if (!_spi_sem->take(1)) + return; + // activate dataflash command decoder _spi->cs_assert(); @@ -215,17 +216,13 @@ void DataFlash_APM2::PageToBuffer(uint8_t BufferNum, uint16_t PageAdr) // release SPI bus for use by other sensors _spi->cs_release(); - if (_spi_sem) { - _spi_sem->release(this); - } + _spi_sem->give(); } void DataFlash_APM2::BufferToPage (uint8_t BufferNum, uint16_t PageAdr, uint8_t wait) { - if (_spi_sem) { - bool got = _spi_sem->get(this); - if (!got) return; - } + if (!_spi_sem->take(1)) + return; // activate dataflash command decoder _spi->cs_assert(); @@ -251,17 +248,13 @@ void DataFlash_APM2::BufferToPage (uint8_t BufferNum, uint16_t PageAdr, uint8_t while(!ReadStatus()) ; //monitor the status register, wait until busy-flag is high // release SPI bus for use by other sensors - if (_spi_sem) { - _spi_sem->release(this); - } + _spi_sem->give(); } void DataFlash_APM2::BufferWrite (uint8_t BufferNum, uint16_t IntPageAdr, uint8_t Data) { - if (_spi_sem) { - bool got = _spi_sem->get(this); - if (!got) return; - } + if (!_spi_sem->take(1)) + return; // activate dataflash command decoder _spi->cs_assert(); @@ -277,17 +270,14 @@ void DataFlash_APM2::BufferWrite (uint8_t BufferNum, uint16_t IntPageAdr, uint8_ // release SPI bus for use by other sensors _spi->cs_release(); - if (_spi_sem) { - _spi_sem->release(this); - } + _spi_sem->give(); } uint8_t DataFlash_APM2::BufferRead (uint8_t BufferNum, uint16_t IntPageAdr) { - if (_spi_sem) { - bool got = _spi_sem->get(this); - if (!got) return 0; - } + if (!_spi_sem->take(1)) + return 0; + uint8_t tmp; // activate dataflash command decoder @@ -307,19 +297,15 @@ uint8_t DataFlash_APM2::BufferRead (uint8_t BufferNum, uint16_t IntPageAdr) // release SPI bus for use by other sensors _spi->cs_release(); - if (_spi_sem) { - _spi_sem->release(this); - } + _spi_sem->give(); return (tmp); } // *** END OF INTERNAL FUNCTIONS *** void DataFlash_APM2::PageErase (uint16_t PageAdr) { - if (_spi_sem) { - bool got = _spi_sem->get(this); - if (!got) return; - } + if (!_spi_sem->take(1)) + return; // activate dataflash command decoder _spi->cs_assert(); @@ -341,18 +327,14 @@ void DataFlash_APM2::PageErase (uint16_t PageAdr) while(!ReadStatus()) ; // release SPI bus for use by other sensors - if (_spi_sem) { - _spi_sem->release(this); - } + _spi_sem->give(); } // erase a block of 8 pages. void DataFlash_APM2::BlockErase(uint16_t BlockAdr) { - if (_spi_sem) { - bool got = _spi_sem->get(this); - if (!got) return; - } + if (!_spi_sem->take(1)) + return; // activate dataflash command decoder _spi->cs_assert(); @@ -374,18 +356,14 @@ void DataFlash_APM2::BlockErase(uint16_t BlockAdr) while(!ReadStatus()) ; // release SPI bus for use by other sensors - if (_spi_sem) { - _spi_sem->release(this); - } + _spi_sem->give(); } void DataFlash_APM2::ChipErase() { - if (_spi_sem) { - bool got = _spi_sem->get(this); - if (!got) return; - } + if (!_spi_sem->take(1)) + return; //serialDebug("Chip Erase\n"); // activate dataflash command decoder @@ -405,7 +383,5 @@ void DataFlash_APM2::ChipErase() } // release SPI bus for use by other sensors - if (_spi_sem) { - _spi_sem->release(this); - } + _spi_sem->give(); }