Use the generic device::SPI locking strategy.

This commit is contained in:
px4dev 2013-09-12 23:56:53 -07:00
parent 3796235967
commit 19fdaf2009
5 changed files with 41 additions and 28 deletions

View File

@ -67,6 +67,7 @@ SPI::SPI(const char *name,
CDev(name, devname, irq),
// public
// protected
locking_mode(LOCK_PREEMPTION),
// private
_bus(bus),
_device(device),
@ -132,13 +133,25 @@ SPI::probe()
int
SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
{
irqstate_t state;
if ((send == nullptr) && (recv == nullptr))
return -EINVAL;
/* do common setup */
if (!up_interrupt_context())
/* lock the bus as required */
if (!up_interrupt_context()) {
switch (locking_mode) {
default:
case LOCK_PREEMPTION:
state = irqsave();
break;
case LOCK_THREADS:
SPI_LOCK(_dev, true);
break;
case LOCK_NONE:
break;
}
}
SPI_SETFREQUENCY(_dev, _frequency);
SPI_SETMODE(_dev, _mode);
@ -151,8 +164,19 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
/* and clean up */
SPI_SELECT(_dev, _device, false);
if (!up_interrupt_context())
if (!up_interrupt_context()) {
switch (locking_mode) {
default:
case LOCK_PREEMPTION:
irqrestore(state);
break;
case LOCK_THREADS:
SPI_LOCK(_dev, false);
break;
case LOCK_NONE:
break;
}
}
return OK;
}

View File

@ -101,6 +101,17 @@ protected:
*/
int transfer(uint8_t *send, uint8_t *recv, unsigned len);
/**
* Locking modes supported by the driver.
*/
enum LockMode {
LOCK_PREEMPTION, /**< the default; lock against all forms of preemption. */
LOCK_THREADS, /**< lock only against other threads, using SPI_LOCK */
LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */
};
LockMode locking_mode; /**< selected locking mode */
private:
int _bus;
enum spi_dev_e _device;

View File

@ -367,7 +367,6 @@ out:
int
L3GD20::probe()
{
irqstate_t flags = irqsave();
/* read dummy value to void to clear SPI statemachine on sensor */
(void)read_reg(ADDR_WHO_AM_I);
@ -393,8 +392,6 @@ L3GD20::probe()
success = true;
}
irqrestore(flags);
if (success)
return OK;

View File

@ -525,7 +525,6 @@ out:
void
LSM303D::reset()
{
irqstate_t flags = irqsave();
/* enable accel*/
write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
@ -540,7 +539,6 @@ LSM303D::reset()
mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
irqrestore(flags);
_accel_read = 0;
_mag_read = 0;
@ -549,15 +547,12 @@ LSM303D::reset()
int
LSM303D::probe()
{
irqstate_t flags = irqsave();
/* read dummy value to void to clear SPI statemachine on sensor */
(void)read_reg(ADDR_WHO_AM_I);
/* verify that the device is attached and functioning */
bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM);
irqrestore(flags);
if (success)
return OK;

View File

@ -142,23 +142,15 @@ MS5611_SPI::init()
goto out;
}
/* disable interrupts, make this section atomic */
flags = irqsave();
/* send reset command */
ret = _reset();
/* re-enable interrupts */
irqrestore(flags);
if (ret != OK) {
debug("reset failed");
goto out;
}
/* disable interrupts, make this section atomic */
flags = irqsave();
/* read PROM */
ret = _read_prom();
/* re-enable interrupts */
irqrestore(flags);
if (ret != OK) {
debug("prom readout failed");
goto out;
@ -270,13 +262,7 @@ MS5611_SPI::_reg16(unsigned reg)
int
MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len)
{
irqstate_t flags = irqsave();
int ret = transfer(send, recv, len);
irqrestore(flags);
return ret;
return transfer(send, recv, len);
}
#endif /* PX4_SPIDEV_BARO */