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)
|
||||
#endif
|
||||
|
||||
#define MAX_SUBDEVS 6
|
||||
|
||||
const uint8_t SPIDeviceManager::_n_device_desc = LINUX_SPI_DEVICE_NUM_DEVICES;
|
||||
|
||||
|
||||
/* Private struct to maintain for each bus */
|
||||
class SPIBus : public TimerPollable::WrapperCb {
|
||||
public:
|
||||
SPIBus(uint16_t bus_);
|
||||
~SPIBus();
|
||||
|
||||
/*
|
||||
|
@ -164,21 +167,26 @@ public:
|
|||
void start_cb() override;
|
||||
void end_cb() override;
|
||||
|
||||
bool open(uint16_t bus, uint16_t subdev);
|
||||
void open(uint16_t subdev);
|
||||
|
||||
PollerThread thread;
|
||||
Semaphore sem;
|
||||
int fd = -1;
|
||||
int fd[MAX_SUBDEVS];
|
||||
uint16_t bus;
|
||||
uint16_t subdev;
|
||||
int16_t last_mode = -1;
|
||||
uint8_t ref;
|
||||
};
|
||||
|
||||
SPIBus::SPIBus(uint16_t bus_)
|
||||
: bus(bus_)
|
||||
{
|
||||
memset(fd, -1, sizeof(fd));
|
||||
}
|
||||
|
||||
SPIBus::~SPIBus()
|
||||
{
|
||||
if (fd >= 0) {
|
||||
::close(fd);
|
||||
for (unsigned i = 0; i < MAX_SUBDEVS; i++) {
|
||||
::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")];
|
||||
|
||||
if (fd > 0) {
|
||||
return -EBUSY;
|
||||
/* Already open by another device */
|
||||
if (fd[subdev] >= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
snprintf(path, sizeof(path), "/dev/spidev%u.%u", bus_, subdev_);
|
||||
fd = ::open(path, O_RDWR | O_CLOEXEC);
|
||||
if (fd < 0) {
|
||||
snprintf(path, sizeof(path), "/dev/spidev%u.%u", bus, subdev);
|
||||
fd[subdev] = ::open(path, O_RDWR | O_CLOEXEC);
|
||||
if (fd[subdev] < 0) {
|
||||
AP_HAL::panic("SPI: unable to open SPI bus %s: %s",
|
||||
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] = { };
|
||||
unsigned nmsgs = 0;
|
||||
int fd = _bus.fd[_desc.subdev];
|
||||
|
||||
assert(_bus.fd >= 0);
|
||||
assert(fd >= 0);
|
||||
|
||||
if (send && send_len != 0) {
|
||||
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.
|
||||
*/
|
||||
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",
|
||||
_bus.fd, strerror(errno));
|
||||
fd, strerror(errno));
|
||||
_bus.last_mode = -1;
|
||||
} else if (current_mode != _bus.last_mode) {
|
||||
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;
|
||||
}
|
||||
}
|
||||
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) {
|
||||
hal.console->printf("SPIDevice: error on setting mode fd=%d (%s)\n",
|
||||
_bus.fd, strerror(errno));
|
||||
fd, strerror(errno));
|
||||
return false;
|
||||
}
|
||||
_bus.last_mode = _desc.mode;
|
||||
}
|
||||
|
||||
_cs_assert();
|
||||
r = ioctl(_bus.fd, SPI_IOC_MESSAGE(nmsgs), &msgs);
|
||||
r = ioctl(fd, SPI_IOC_MESSAGE(nmsgs), &msgs);
|
||||
_cs_release();
|
||||
|
||||
if (r == -1) {
|
||||
hal.console->printf("SPIDevice: error transferring data fd=%d (%s)\n",
|
||||
_bus.fd, strerror(errno));
|
||||
fd, strerror(errno));
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -338,8 +343,9 @@ bool SPIDevice::transfer_fullduplex(const uint8_t *send, uint8_t *recv,
|
|||
uint32_t len)
|
||||
{
|
||||
struct spi_ioc_transfer msgs[1] = { };
|
||||
int fd = _bus.fd[_desc.subdev];
|
||||
|
||||
assert(_bus.fd >= 0);
|
||||
assert(fd >= 0);
|
||||
|
||||
if (!send || !recv || len == 0) {
|
||||
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].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) {
|
||||
hal.console->printf("SPIDevice: error on setting mode fd=%d (%s)\n",
|
||||
_bus.fd, strerror(errno));
|
||||
fd, strerror(errno));
|
||||
return false;
|
||||
}
|
||||
|
||||
_cs_assert();
|
||||
r = ioctl(_bus.fd, SPI_IOC_MESSAGE(1), &msgs);
|
||||
r = ioctl(fd, SPI_IOC_MESSAGE(1), &msgs);
|
||||
_cs_release();
|
||||
|
||||
if (r == -1) {
|
||||
hal.console->printf("SPIDevice: error transferring data fd=%d (%s)\n",
|
||||
_bus.fd, strerror(errno));
|
||||
fd, strerror(errno));
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -442,17 +448,16 @@ SPIDeviceManager::get_device(const char *name)
|
|||
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++) {
|
||||
if (_buses[i]->bus == desc->bus &&
|
||||
_buses[i]->subdev == desc->subdev) {
|
||||
if (_buses[i]->bus == desc->bus) {
|
||||
return _create_device(*_buses[i], *desc);
|
||||
}
|
||||
}
|
||||
|
||||
/* Bus not found for this device, create a new one */
|
||||
AP_HAL::OwnPtr<SPIBus> b{new SPIBus()};
|
||||
if (!b || b->open(desc->bus, desc->subdev) < 0) {
|
||||
AP_HAL::OwnPtr<SPIBus> b{new SPIBus(desc->bus)};
|
||||
if (!b) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
|
@ -480,6 +485,9 @@ const char* SPIDeviceManager::get_device_name(uint8_t idx)
|
|||
AP_HAL::OwnPtr<AP_HAL::SPIDevice>
|
||||
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));
|
||||
if (!dev) {
|
||||
return nullptr;
|
||||
|
@ -497,7 +505,7 @@ void SPIDeviceManager::_unregister(SPIBus &b)
|
|||
}
|
||||
|
||||
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);
|
||||
delete &b;
|
||||
break;
|
||||
|
|
Loading…
Reference in New Issue