mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_Linux: create one thread per spi bus rather than device
Do not create one thread per chardev (i.e. bus + kernel's chip select). Since the shared resources are actually the bus controller and the bus lines, it makes sense to have 1 thread per bus, otherwise it will just get locked again on the mutex in the kernel side.
This commit is contained in:
parent
cf4fb09881
commit
2c6dd64c67
|
@ -149,12 +149,15 @@ SPIDesc SPIDeviceManager::_device[] = {
|
||||||
#define LINUX_SPI_DEVICE_NUM_DEVICES ARRAY_SIZE(SPIDeviceManager::_device)
|
#define LINUX_SPI_DEVICE_NUM_DEVICES ARRAY_SIZE(SPIDeviceManager::_device)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#define MAX_SUBDEVS 6
|
||||||
|
|
||||||
const uint8_t SPIDeviceManager::_n_device_desc = LINUX_SPI_DEVICE_NUM_DEVICES;
|
const uint8_t SPIDeviceManager::_n_device_desc = LINUX_SPI_DEVICE_NUM_DEVICES;
|
||||||
|
|
||||||
|
|
||||||
/* Private struct to maintain for each bus */
|
/* Private struct to maintain for each bus */
|
||||||
class SPIBus : public TimerPollable::WrapperCb {
|
class SPIBus : public TimerPollable::WrapperCb {
|
||||||
public:
|
public:
|
||||||
|
SPIBus(uint16_t bus_);
|
||||||
~SPIBus();
|
~SPIBus();
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -164,21 +167,26 @@ public:
|
||||||
void start_cb() override;
|
void start_cb() override;
|
||||||
void end_cb() override;
|
void end_cb() override;
|
||||||
|
|
||||||
bool open(uint16_t bus, uint16_t subdev);
|
void open(uint16_t subdev);
|
||||||
|
|
||||||
PollerThread thread;
|
PollerThread thread;
|
||||||
Semaphore sem;
|
Semaphore sem;
|
||||||
int fd = -1;
|
int fd[MAX_SUBDEVS];
|
||||||
uint16_t bus;
|
uint16_t bus;
|
||||||
uint16_t subdev;
|
|
||||||
int16_t last_mode = -1;
|
int16_t last_mode = -1;
|
||||||
uint8_t ref;
|
uint8_t ref;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
SPIBus::SPIBus(uint16_t bus_)
|
||||||
|
: bus(bus_)
|
||||||
|
{
|
||||||
|
memset(fd, -1, sizeof(fd));
|
||||||
|
}
|
||||||
|
|
||||||
SPIBus::~SPIBus()
|
SPIBus::~SPIBus()
|
||||||
{
|
{
|
||||||
if (fd >= 0) {
|
for (unsigned i = 0; i < MAX_SUBDEVS; i++) {
|
||||||
::close(fd);
|
::close(fd[i]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -193,25 +201,21 @@ void SPIBus::end_cb()
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
int SPIBus::open(uint16_t bus_, uint16_t subdev_)
|
void SPIBus::open(uint16_t subdev)
|
||||||
{
|
{
|
||||||
char path[sizeof("/dev/spidevXXXXX.XXXXX")];
|
char path[sizeof("/dev/spidevXXXXX.XXXXX")];
|
||||||
|
|
||||||
if (fd > 0) {
|
/* Already open by another device */
|
||||||
return -EBUSY;
|
if (fd[subdev] >= 0) {
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
snprintf(path, sizeof(path), "/dev/spidev%u.%u", bus_, subdev_);
|
snprintf(path, sizeof(path), "/dev/spidev%u.%u", bus, subdev);
|
||||||
fd = ::open(path, O_RDWR | O_CLOEXEC);
|
fd[subdev] = ::open(path, O_RDWR | O_CLOEXEC);
|
||||||
if (fd < 0) {
|
if (fd[subdev] < 0) {
|
||||||
AP_HAL::panic("SPI: unable to open SPI bus %s: %s",
|
AP_HAL::panic("SPI: unable to open SPI bus %s: %s",
|
||||||
path, strerror(errno));
|
path, strerror(errno));
|
||||||
}
|
}
|
||||||
|
|
||||||
bus = bus_;
|
|
||||||
subdev = subdev_;
|
|
||||||
|
|
||||||
return fd;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -261,8 +265,9 @@ bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len,
|
||||||
{
|
{
|
||||||
struct spi_ioc_transfer msgs[2] = { };
|
struct spi_ioc_transfer msgs[2] = { };
|
||||||
unsigned nmsgs = 0;
|
unsigned nmsgs = 0;
|
||||||
|
int fd = _bus.fd[_desc.subdev];
|
||||||
|
|
||||||
assert(_bus.fd >= 0);
|
assert(fd >= 0);
|
||||||
|
|
||||||
if (send && send_len != 0) {
|
if (send && send_len != 0) {
|
||||||
msgs[nmsgs].tx_buf = (uint64_t) send;
|
msgs[nmsgs].tx_buf = (uint64_t) send;
|
||||||
|
@ -301,33 +306,33 @@ bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len,
|
||||||
an extra syscall per transfer.
|
an extra syscall per transfer.
|
||||||
*/
|
*/
|
||||||
uint8_t current_mode;
|
uint8_t current_mode;
|
||||||
if (ioctl(_bus.fd, SPI_IOC_RD_MODE, ¤t_mode) < 0) {
|
if (ioctl(fd, SPI_IOC_RD_MODE, ¤t_mode) < 0) {
|
||||||
hal.console->printf("SPIDevice: error on getting mode fd=%d (%s)\n",
|
hal.console->printf("SPIDevice: error on getting mode fd=%d (%s)\n",
|
||||||
_bus.fd, strerror(errno));
|
fd, strerror(errno));
|
||||||
_bus.last_mode = -1;
|
_bus.last_mode = -1;
|
||||||
} else if (current_mode != _bus.last_mode) {
|
} else if (current_mode != _bus.last_mode) {
|
||||||
hal.console->printf("SPIDevice: bus mode conflict fd=%d mode=%u/%u\n",
|
hal.console->printf("SPIDevice: bus mode conflict fd=%d mode=%u/%u\n",
|
||||||
_bus.fd, (unsigned)_bus.last_mode, (unsigned)current_mode);
|
fd, (unsigned)_bus.last_mode, (unsigned)current_mode);
|
||||||
_bus.last_mode = -1;
|
_bus.last_mode = -1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (_desc.mode != _bus.last_mode) {
|
if (_desc.mode != _bus.last_mode) {
|
||||||
r = ioctl(_bus.fd, SPI_IOC_WR_MODE, &_desc.mode);
|
r = ioctl(fd, SPI_IOC_WR_MODE, &_desc.mode);
|
||||||
if (r < 0) {
|
if (r < 0) {
|
||||||
hal.console->printf("SPIDevice: error on setting mode fd=%d (%s)\n",
|
hal.console->printf("SPIDevice: error on setting mode fd=%d (%s)\n",
|
||||||
_bus.fd, strerror(errno));
|
fd, strerror(errno));
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
_bus.last_mode = _desc.mode;
|
_bus.last_mode = _desc.mode;
|
||||||
}
|
}
|
||||||
|
|
||||||
_cs_assert();
|
_cs_assert();
|
||||||
r = ioctl(_bus.fd, SPI_IOC_MESSAGE(nmsgs), &msgs);
|
r = ioctl(fd, SPI_IOC_MESSAGE(nmsgs), &msgs);
|
||||||
_cs_release();
|
_cs_release();
|
||||||
|
|
||||||
if (r == -1) {
|
if (r == -1) {
|
||||||
hal.console->printf("SPIDevice: error transferring data fd=%d (%s)\n",
|
hal.console->printf("SPIDevice: error transferring data fd=%d (%s)\n",
|
||||||
_bus.fd, strerror(errno));
|
fd, strerror(errno));
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -338,8 +343,9 @@ bool SPIDevice::transfer_fullduplex(const uint8_t *send, uint8_t *recv,
|
||||||
uint32_t len)
|
uint32_t len)
|
||||||
{
|
{
|
||||||
struct spi_ioc_transfer msgs[1] = { };
|
struct spi_ioc_transfer msgs[1] = { };
|
||||||
|
int fd = _bus.fd[_desc.subdev];
|
||||||
|
|
||||||
assert(_bus.fd >= 0);
|
assert(fd >= 0);
|
||||||
|
|
||||||
if (!send || !recv || len == 0) {
|
if (!send || !recv || len == 0) {
|
||||||
return false;
|
return false;
|
||||||
|
@ -353,20 +359,20 @@ bool SPIDevice::transfer_fullduplex(const uint8_t *send, uint8_t *recv,
|
||||||
msgs[0].bits_per_word = _desc.bits_per_word;
|
msgs[0].bits_per_word = _desc.bits_per_word;
|
||||||
msgs[0].cs_change = 0;
|
msgs[0].cs_change = 0;
|
||||||
|
|
||||||
int r = ioctl(_bus.fd, SPI_IOC_WR_MODE, &_desc.mode);
|
int r = ioctl(fd, SPI_IOC_WR_MODE, &_desc.mode);
|
||||||
if (r < 0) {
|
if (r < 0) {
|
||||||
hal.console->printf("SPIDevice: error on setting mode fd=%d (%s)\n",
|
hal.console->printf("SPIDevice: error on setting mode fd=%d (%s)\n",
|
||||||
_bus.fd, strerror(errno));
|
fd, strerror(errno));
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
_cs_assert();
|
_cs_assert();
|
||||||
r = ioctl(_bus.fd, SPI_IOC_MESSAGE(1), &msgs);
|
r = ioctl(fd, SPI_IOC_MESSAGE(1), &msgs);
|
||||||
_cs_release();
|
_cs_release();
|
||||||
|
|
||||||
if (r == -1) {
|
if (r == -1) {
|
||||||
hal.console->printf("SPIDevice: error transferring data fd=%d (%s)\n",
|
hal.console->printf("SPIDevice: error transferring data fd=%d (%s)\n",
|
||||||
_bus.fd, strerror(errno));
|
fd, strerror(errno));
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -442,17 +448,16 @@ SPIDeviceManager::get_device(const char *name)
|
||||||
return AP_HAL::OwnPtr<AP_HAL::SPIDevice>(nullptr);
|
return AP_HAL::OwnPtr<AP_HAL::SPIDevice>(nullptr);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Find if bus is already open */
|
/* Find if bus already exists */
|
||||||
for (uint8_t i = 0, n = _buses.size(); i < n; i++) {
|
for (uint8_t i = 0, n = _buses.size(); i < n; i++) {
|
||||||
if (_buses[i]->bus == desc->bus &&
|
if (_buses[i]->bus == desc->bus) {
|
||||||
_buses[i]->subdev == desc->subdev) {
|
|
||||||
return _create_device(*_buses[i], *desc);
|
return _create_device(*_buses[i], *desc);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Bus not found for this device, create a new one */
|
/* Bus not found for this device, create a new one */
|
||||||
AP_HAL::OwnPtr<SPIBus> b{new SPIBus()};
|
AP_HAL::OwnPtr<SPIBus> b{new SPIBus(desc->bus)};
|
||||||
if (!b || b->open(desc->bus, desc->subdev) < 0) {
|
if (!b) {
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -480,6 +485,9 @@ const char* SPIDeviceManager::get_device_name(uint8_t idx)
|
||||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice>
|
AP_HAL::OwnPtr<AP_HAL::SPIDevice>
|
||||||
SPIDeviceManager::_create_device(SPIBus &b, SPIDesc &desc) const
|
SPIDeviceManager::_create_device(SPIBus &b, SPIDesc &desc) const
|
||||||
{
|
{
|
||||||
|
// Ensure bus is open
|
||||||
|
b.open(desc.subdev);
|
||||||
|
|
||||||
auto dev = AP_HAL::OwnPtr<AP_HAL::SPIDevice>(new SPIDevice(b, desc));
|
auto dev = AP_HAL::OwnPtr<AP_HAL::SPIDevice>(new SPIDevice(b, desc));
|
||||||
if (!dev) {
|
if (!dev) {
|
||||||
return nullptr;
|
return nullptr;
|
||||||
|
@ -497,7 +505,7 @@ void SPIDeviceManager::_unregister(SPIBus &b)
|
||||||
}
|
}
|
||||||
|
|
||||||
for (auto it = _buses.begin(); it != _buses.end(); it++) {
|
for (auto it = _buses.begin(); it != _buses.end(); it++) {
|
||||||
if ((*it)->bus == b.bus && (*it)->subdev == b.subdev) {
|
if ((*it)->bus == b.bus) {
|
||||||
_buses.erase(it);
|
_buses.erase(it);
|
||||||
delete &b;
|
delete &b;
|
||||||
break;
|
break;
|
||||||
|
|
Loading…
Reference in New Issue