diff --git a/libraries/AP_HAL_Linux/SPIDevice.cpp b/libraries/AP_HAL_Linux/SPIDevice.cpp new file mode 100644 index 0000000000..eddccad41f --- /dev/null +++ b/libraries/AP_HAL_Linux/SPIDevice.cpp @@ -0,0 +1,266 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + * Copyright (C) 2015 Intel Corporation. All rights reserved. + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#include "SPIDevice.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "Util.h" + +namespace Linux { + +static const AP_HAL::HAL &hal = AP_HAL::get_HAL(); + +/* Private struct to maintain for each bus */ +class SPIBus { +public: + ~SPIBus() + { + if (fd >= 0) { + ::close(fd); + } + } + + int open(uint16_t bus_, uint16_t kernel_cs_) + { + char path[sizeof("/dev/spidevXXXXX.XXXXX")]; + + if (fd > 0) { + return -EBUSY; + } + + snprintf(path, sizeof(path), "/dev/spidev%u.%u", bus_, kernel_cs_); + fd = ::open(path, O_RDWR | O_CLOEXEC); + if (fd < 0) { + AP_HAL::panic("SPI: unable to open SPI bus %s: %s", + path, strerror(errno)); + } + + bus = bus_; + kernel_cs = kernel_cs_; + + return fd; + } + + Semaphore sem; + int fd = -1; + uint16_t bus; + uint16_t kernel_cs; + uint8_t ref; +}; + +SPIDevice::SPIDevice(SPIBus &bus, SPIDeviceDriver &device_desc) + : _bus(bus) + , _desc(device_desc) +{ + _desc.init(); + _cs_release(); +} + +SPIDevice::~SPIDevice() +{ + // Unregister itself from the SPIDeviceManager + SPIDeviceManager::from(hal.spi)->_unregister(_bus); +} + +bool SPIDevice::set_speed(AP_HAL::Device::Speed speed) +{ + switch (speed) { + case AP_HAL::Device::SPEED_HIGH: + _desc._speed = _desc._highspeed; + break; + case AP_HAL::Device::SPEED_LOW: + _desc._speed = _desc._lowspeed; + break; + } + + return true; +} + +bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) +{ + struct spi_ioc_transfer msgs[2] = { }; + unsigned nmsgs = 0; + + assert(_bus.fd >= 0); + + if (send && send_len != 0) { + msgs[nmsgs].tx_buf = (uint64_t) send; + msgs[nmsgs].rx_buf = 0; + msgs[nmsgs].len = send_len; + msgs[nmsgs].speed_hz = _desc._speed; + msgs[nmsgs].delay_usecs = 0; + msgs[nmsgs].bits_per_word = _desc._bitsPerWord; + msgs[nmsgs].cs_change = 0; + nmsgs++; + } + + if (recv && recv_len != 0) { + msgs[nmsgs].tx_buf = 0; + msgs[nmsgs].rx_buf = (uint64_t) recv; + msgs[nmsgs].len = recv_len; + msgs[nmsgs].speed_hz = _desc._speed; + msgs[nmsgs].delay_usecs = 0; + msgs[nmsgs].bits_per_word = _desc._bitsPerWord; + msgs[nmsgs].cs_change = 0; + nmsgs++; + } + + if (!nmsgs) { + return false; + } + + int r = ioctl(_bus.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)); + return false; + } + + _cs_assert(); + r = ioctl(_bus.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)); + return false; + } + + return true; +} + +void SPIDevice::_cs_assert() +{ + if (_desc._cs_pin == SPI_CS_KERNEL) { + return; + } + + _desc._cs->write(0); +} + +void SPIDevice::_cs_release() +{ + if (_desc._cs_pin == SPI_CS_KERNEL) { + return; + } + + _desc._cs->write(1); +} + +AP_HAL::Semaphore *SPIDevice::get_semaphore() +{ + return &_bus.sem; +} + +int SPIDevice::get_fd() +{ + return _bus.fd; +} + +AP_HAL::OwnPtr +SPIDeviceManager::get_device(const char *name) +{ + SPIDeviceDriver *desc = nullptr; + + /* Find the bus description in the table */ + for (uint8_t i = 0; i < _n_device_desc; i++) { + if (!strcmp(_device[i]._name, name)) { + desc = &_device[i]; + break; + } + } + + if (!desc) { + AP_HAL::panic("SPI: invalid device name"); + } + + return get_device(*desc); +} + +AP_HAL::OwnPtr +SPIDeviceManager::get_device(SPIDeviceDriver &desc) +{ + /* Find if bus is already open */ + for (uint8_t i = 0, n = _buses.size(); i < n; i++) { + if (_buses[i]->bus == desc._bus && + _buses[i]->kernel_cs == desc._subdev) { + return _create_device(*_buses[i], desc); + } + } + + /* Bus not found for this device, create a new one */ + AP_HAL::OwnPtr b{new SPIBus()}; + if (!b) { + return nullptr; + } + + if (b->open(desc._bus, desc._subdev) < 0) { + return nullptr; + } + + auto dev = _create_device(*b, desc); + if (!dev) { + return nullptr; + } + + _buses.push_back(b.leak()); + + return dev; +} + +/* Create a new device increasing the bus reference */ +AP_HAL::OwnPtr +SPIDeviceManager::_create_device(SPIBus &b, SPIDeviceDriver &desc) const +{ + auto dev = AP_HAL::OwnPtr(new SPIDevice(b, desc)); + if (!dev) { + return nullptr; + } + b.ref++; + return dev; +} + +void SPIDeviceManager::_unregister(SPIBus &b) +{ + if (b.ref == 0 || --b.ref > 0) { + return; + } + + for (auto it = _buses.begin(); it != _buses.end(); it++) { + if ((*it)->bus == b.bus && (*it)->kernel_cs == b.kernel_cs) { + _buses.erase(it); + delete &b; + break; + } + } +} + +} diff --git a/libraries/AP_HAL_Linux/SPIDevice.h b/libraries/AP_HAL_Linux/SPIDevice.h new file mode 100644 index 0000000000..030e3a0850 --- /dev/null +++ b/libraries/AP_HAL_Linux/SPIDevice.h @@ -0,0 +1,74 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + * Copyright (C) 2015 Intel Corporation. All rights reserved. + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#pragma once + +#include + +#include +#include + +#include "SPIDriver.h" + +namespace Linux { + +class SPIBus; + +class SPIDevice : public AP_HAL::SPIDevice { +public: + SPIDevice(SPIBus &bus, SPIDeviceDriver &device_desc); + + virtual ~SPIDevice(); + + /* AP_HAL::SPIDevice implementation */ + + /* See AP_HAL::Device::set_speed() */ + bool set_speed(AP_HAL::Device::Speed speed) override; + + /* See AP_HAL::Device::transfer() */ + bool transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) override; + + /* See AP_HAL::Device::get_semaphore() */ + AP_HAL::Semaphore *get_semaphore() override; + + /* See AP_HAL::Device::register_periodic_callback() */ + AP_HAL::Device::PeriodicHandle *register_periodic_callback( + uint32_t period_usec, AP_HAL::MemberProc) override + { + return nullptr; + } + + /* See AP_HAL::Device::get_fd() */ + int get_fd() override; + +protected: + SPIBus &_bus; + SPIDeviceDriver &_desc; + + /* + * Select device if using userspace CS + */ + void _cs_assert(); + + /* + * Deselect device if using userspace CS + */ + void _cs_release(); +}; + +} diff --git a/libraries/AP_HAL_Linux/SPIDriver.h b/libraries/AP_HAL_Linux/SPIDriver.h index 47f0ccec70..8fc68403aa 100644 --- a/libraries/AP_HAL_Linux/SPIDriver.h +++ b/libraries/AP_HAL_Linux/SPIDriver.h @@ -1,6 +1,9 @@ +#pragma once -#ifndef __AP_HAL_EMPTY_SPIDRIVER_H__ -#define __AP_HAL_EMPTY_SPIDRIVER_H__ +#include + +#include +#include #include "AP_HAL_Linux.h" #include "Semaphores.h" @@ -10,9 +13,14 @@ // Fake CS pin to indicate in-kernel handling #define SPI_CS_KERNEL -1 -class Linux::SPIDeviceDriver : public AP_HAL::SPIDeviceDriver { +namespace Linux { + +class SPIBus; + +class SPIDeviceDriver : public AP_HAL::SPIDeviceDriver { public: - friend class Linux::SPIDeviceManager; + friend class SPIDeviceManager; + friend class SPIDevice; SPIDeviceDriver(const char *name, uint16_t bus, uint16_t subdev, enum AP_HAL::SPIDeviceType type, uint8_t mode, uint8_t bitsPerWord, int16_t cs_pin, uint32_t lowspeed, uint32_t highspeed); @@ -41,8 +49,26 @@ private: int _fd; // Per-device FD. }; -class Linux::SPIDeviceManager : public AP_HAL::SPIDeviceManager { +class SPIDeviceManager : public AP_HAL::SPIDeviceManager { public: + friend class SPIDevice; + + static SPIDeviceManager *from(AP_HAL::SPIDeviceManager *spi_mgr) + { + return static_cast(spi_mgr); + } + + SPIDeviceManager() + { + /* Reserve space up-front for 3 buses */ + _buses.reserve(3); + } + + AP_HAL::OwnPtr get_device(const char *name); + + // Temporary function to interoperate with SPIDeviceDriver interface + AP_HAL::OwnPtr get_device(SPIDeviceDriver &desc); + void init(); AP_HAL::SPIDeviceDriver* device(enum AP_HAL::SPIDeviceType, uint8_t index = 0); @@ -53,9 +79,14 @@ public: static bool transaction(SPIDeviceDriver &driver, const uint8_t *tx, uint8_t *rx, uint16_t len); protected: + void _unregister(SPIBus &b); + AP_HAL::OwnPtr _create_device(SPIBus &b, SPIDeviceDriver &device_desc) const; + + std::vector _buses; + static const uint8_t _n_device_desc; static SPIDeviceDriver _device[]; static Semaphore _semaphore[LINUX_SPI_MAX_BUSES]; }; -#endif // __AP_HAL_LINUX_SPIDRIVER_H__ +}