mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_SITL: add simulated SPI devices
This commit is contained in:
Peter Barker
Peter Barker
parent
c524b6e5b5
commit
58a7f022fd
|
|
@ -22,6 +22,7 @@
|
|||
#include "Util.h"
|
||||
#include "DSP.h"
|
||||
#include "CANSocketIface.h"
|
||||
#include "SPIDevice.h"
|
||||
|
||||
#include <AP_BoardConfig/AP_BoardConfig.h>
|
||||
#include <AP_HAL_Empty/AP_HAL_Empty.h>
|
||||
|
|
@ -50,7 +51,6 @@ static DSP dspDriver;
|
|||
|
||||
|
||||
// use the Empty HAL for hardware we don't emulate
|
||||
static Empty::SPIDeviceManager emptySPI;
|
||||
static Empty::OpticalFlow emptyOpticalFlow;
|
||||
static Empty::Flash emptyFlash;
|
||||
|
||||
|
|
@ -66,12 +66,13 @@ static UARTDriver sitlUart8Driver(8, &sitlState);
|
|||
|
||||
#if defined(HAL_BUILD_AP_PERIPH)
|
||||
static Empty::I2CDeviceManager i2c_mgr_instance;
|
||||
static Empty::SPIDeviceManager spi_mgr_instance;
|
||||
#else
|
||||
static I2CDeviceManager i2c_mgr_instance;
|
||||
static SPIDeviceManager spi_mgr_instance;
|
||||
#endif
|
||||
static Util utilInstance(&sitlState);
|
||||
|
||||
|
||||
#if HAL_NUM_CAN_IFACES
|
||||
static HALSITL::CANIface* canDrivers[HAL_NUM_CAN_IFACES];
|
||||
#endif
|
||||
|
|
@ -90,7 +91,7 @@ HAL_SITL::HAL_SITL() :
|
|||
&sitlUart7Driver, /* uartH */
|
||||
&sitlUart8Driver, /* uartI */
|
||||
&i2c_mgr_instance,
|
||||
&emptySPI, /* spi */
|
||||
&spi_mgr_instance, /* spi */
|
||||
&qspi_mgr_instance,
|
||||
&sitlAnalogIn, /* analogin */
|
||||
&sitlStorage, /* storage */
|
||||
|
|
|
|||
|
|
@ -0,0 +1,263 @@
|
|||
/*
|
||||
* Copyright (C) 2021 Peter Barker. 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "SPIDevice.h"
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
|
||||
|
||||
#include <SITL/SITL.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
using namespace HALSITL;
|
||||
|
||||
/*
|
||||
* SPIBus
|
||||
*/
|
||||
class SPIBus {
|
||||
friend class SPIDeviceManager;
|
||||
public:
|
||||
|
||||
SPIBus(uint8_t _bus) :
|
||||
bus{_bus}
|
||||
{ }
|
||||
|
||||
uint8_t bus;
|
||||
Semaphore sem;
|
||||
int ioctl(uint8_t cs_pin, uint8_t ioctl_number, void *data) {
|
||||
return _ioctl(cs_pin, ioctl_number, data);
|
||||
}
|
||||
// void _timer_tick(); // in lieu of a thread-per-bus
|
||||
AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb);
|
||||
|
||||
class SPIBus *next;
|
||||
|
||||
private:
|
||||
int _ioctl(uint8_t cs_pin, uint8_t ioctl_number, void *data);
|
||||
|
||||
struct callback_info {
|
||||
struct callback_info *next;
|
||||
AP_HAL::Device::PeriodicCb cb;
|
||||
uint32_t period_usec;
|
||||
uint64_t next_usec;
|
||||
} *callbacks;
|
||||
|
||||
static uint8_t spi_buscount;
|
||||
|
||||
};
|
||||
|
||||
uint8_t SPIBus::spi_buscount;
|
||||
|
||||
// FIXME: do the whole subdev dance here and get an actual FD to do an ioctl on?
|
||||
int SPIBus::_ioctl(uint8_t cs_pin, uint8_t ioctl_number, void *data)
|
||||
{
|
||||
SITL::SIM *sitl = AP::sitl();
|
||||
return sitl->spi_ioctl(bus, cs_pin, ioctl_number, data);
|
||||
}
|
||||
|
||||
AP_HAL::Device::PeriodicHandle SPIBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
|
||||
{
|
||||
// mostly swiped from ChibiOS:
|
||||
SPIBus::callback_info *callback = new SPIBus::callback_info;
|
||||
if (callback == nullptr) {
|
||||
return nullptr;
|
||||
}
|
||||
callback->cb = cb;
|
||||
callback->period_usec = period_usec;
|
||||
callback->next_usec = AP_HAL::micros64() + period_usec;
|
||||
|
||||
// add to linked list of callbacks on thread
|
||||
callback->next = callbacks;
|
||||
callbacks = callback;
|
||||
return callback;
|
||||
}
|
||||
|
||||
// void SPIBus::_timer_tick()
|
||||
// {
|
||||
// const uint64_t now = AP_HAL::micros64();
|
||||
// for (struct callback_info *ci = callbacks; ci != nullptr; ci = ci->next) {
|
||||
// if (ci->next_usec < now) {
|
||||
// WITH_SEMAPHORE(sem);
|
||||
// ci->cb();
|
||||
// ci->next_usec += ci->period_usec;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
/*
|
||||
* SPIDeviceManager
|
||||
*/
|
||||
SPIBus *SPIDeviceManager::buses;
|
||||
|
||||
SPIDeviceManager::SPIDeviceManager()
|
||||
{
|
||||
for (uint8_t i=0; i<ARRAY_SIZE(buses); i++) {
|
||||
buses[i].bus = i;
|
||||
}
|
||||
}
|
||||
|
||||
static const struct SPIDriverInfo {
|
||||
// SPIDriver *driver;
|
||||
uint8_t busid; // used for device IDs in parameters
|
||||
// uint8_t dma_channel_rx;
|
||||
// uint8_t dma_channel_tx;
|
||||
} spi_devices[] = {
|
||||
{ 0, },
|
||||
};
|
||||
|
||||
// name, bus, cs_pin
|
||||
SPIDesc SPIDeviceManager::device_table[] = {
|
||||
{ "ramtron", 0, 0 },
|
||||
};
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice>
|
||||
SPIDeviceManager::get_device(const char *name)
|
||||
{
|
||||
// this was swiped from AP_HAL_ChibiOS
|
||||
|
||||
/* Find the bus description in the table */
|
||||
uint8_t i;
|
||||
for (i = 0; i<ARRAY_SIZE(device_table); i++) {
|
||||
if (strcmp(device_table[i].name, name) == 0) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (i == ARRAY_SIZE(device_table)) {
|
||||
return AP_HAL::OwnPtr<AP_HAL::SPIDevice>(nullptr);
|
||||
}
|
||||
|
||||
SPIDesc &desc = device_table[i];
|
||||
|
||||
// find the bus
|
||||
SPIBus *busp;
|
||||
for (busp = buses; busp; busp = (SPIBus *)busp->next) {
|
||||
if (busp->bus == desc.bus) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (busp == nullptr) {
|
||||
// create a new one
|
||||
busp = new SPIBus(desc.bus);
|
||||
if (busp == nullptr) {
|
||||
return nullptr;
|
||||
}
|
||||
busp->next = buses;
|
||||
busp->bus = desc.bus;
|
||||
|
||||
buses = busp;
|
||||
}
|
||||
|
||||
return AP_HAL::OwnPtr<AP_HAL::SPIDevice>(new SPIDevice(*busp, desc));
|
||||
}
|
||||
|
||||
// void SPIDeviceManager::_timer_tick()
|
||||
// {
|
||||
// for (auto &bus : buses) {
|
||||
// bus._timer_tick();
|
||||
// }
|
||||
// }
|
||||
|
||||
/*
|
||||
* SPIDevice
|
||||
*/
|
||||
|
||||
SPIDevice::SPIDevice(SPIBus &_bus, SPIDesc &_device_desc)
|
||||
: bus(_bus)
|
||||
, device_desc(_device_desc)
|
||||
{
|
||||
set_device_bus(spi_devices[_bus.bus].busid);
|
||||
set_device_address(_device_desc.cs_pin);
|
||||
}
|
||||
|
||||
AP_HAL::Semaphore *SPIDevice::get_semaphore()
|
||||
{
|
||||
return &bus.sem;
|
||||
}
|
||||
|
||||
/*
|
||||
low level transfer function - swiped from the Linux HAL
|
||||
*/
|
||||
// this is copied into the SITL SPI layer
|
||||
struct spi_ioc_transfer {
|
||||
uint64_t tx_buf;
|
||||
uint64_t rx_buf;
|
||||
|
||||
uint32_t len;
|
||||
// uint32_t speed_hz;
|
||||
|
||||
// uint16_t delay_usecs;
|
||||
// uint8_t bits_per_word;
|
||||
// uint8_t cs_change;
|
||||
};
|
||||
|
||||
#define SPI_TRANSACTION_1LONG 17
|
||||
#define SPI_TRANSACTION_2LONG 18
|
||||
|
||||
bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len,
|
||||
uint8_t *recv, uint32_t recv_len)
|
||||
{
|
||||
bus.sem.check_owner();
|
||||
|
||||
struct spi_ioc_transfer msgs[2] = { };
|
||||
unsigned nmsgs = 0;
|
||||
|
||||
if (send && send_len != 0) {
|
||||
msgs[nmsgs].tx_buf = (uint64_t) send;
|
||||
msgs[nmsgs].rx_buf = 0;
|
||||
msgs[nmsgs].len = send_len;
|
||||
nmsgs++;
|
||||
}
|
||||
|
||||
if (recv && recv_len != 0) {
|
||||
msgs[nmsgs].tx_buf = 0;
|
||||
msgs[nmsgs].rx_buf = (uint64_t) recv;
|
||||
msgs[nmsgs].len = recv_len;
|
||||
nmsgs++;
|
||||
}
|
||||
|
||||
if (!nmsgs) {
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t ioctl_number;
|
||||
switch (nmsgs) {
|
||||
case 1:
|
||||
ioctl_number = SPI_TRANSACTION_1LONG;
|
||||
break;
|
||||
case 2:
|
||||
ioctl_number = SPI_TRANSACTION_2LONG;
|
||||
break;
|
||||
default:
|
||||
abort();
|
||||
}
|
||||
const int r = bus.ioctl(device_desc.cs_pin, ioctl_number, &msgs);
|
||||
|
||||
if (r == -1) {
|
||||
hal.console->printf("SPIDevice: error transferring data\n");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
AP_HAL::Device::PeriodicHandle SPIDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
|
||||
{
|
||||
return bus.register_periodic_callback(period_usec, cb);
|
||||
}
|
||||
|
||||
#endif // HAL_BOARD_SITL
|
||||
|
|
@ -0,0 +1,81 @@
|
|||
/*
|
||||
* Copyright (C) 2021 Peter Barker. 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#include <AP_HAL/HAL.h>
|
||||
#include <AP_HAL/SPIDevice.h>
|
||||
|
||||
#include "Semaphores.h"
|
||||
|
||||
class SPIBus;
|
||||
|
||||
namespace HALSITL {
|
||||
|
||||
struct SPIDesc {
|
||||
SPIDesc(const char *_name, uint8_t _bus, uint8_t _cs_pin)
|
||||
: name(_name), bus(_bus), cs_pin(_cs_pin)
|
||||
{ }
|
||||
|
||||
const char *name;
|
||||
uint8_t bus;
|
||||
uint8_t cs_pin; // cs
|
||||
};
|
||||
|
||||
class SPIDevice : public AP_HAL::SPIDevice {
|
||||
public:
|
||||
SPIDevice(SPIBus &_bus, SPIDesc &_device_desc);
|
||||
|
||||
// no concept of speed in SITL yet
|
||||
bool set_speed(AP_HAL::Device::Speed speed) override { return true; }
|
||||
|
||||
bool transfer(const uint8_t *send, uint32_t send_len,
|
||||
uint8_t *recv, uint32_t recv_len) override;
|
||||
|
||||
bool transfer_fullduplex(const uint8_t *send, uint8_t *recv,
|
||||
uint32_t len) override {
|
||||
abort();
|
||||
}
|
||||
|
||||
|
||||
AP_HAL::Semaphore *get_semaphore() override;
|
||||
|
||||
AP_HAL::Device::PeriodicHandle register_periodic_callback(
|
||||
uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
|
||||
|
||||
private:
|
||||
SPIBus &bus;
|
||||
SPIDesc &device_desc;
|
||||
|
||||
Semaphore _semaphore;
|
||||
|
||||
bool do_transfer(const uint8_t *send, uint8_t *recv, uint32_t len);
|
||||
};
|
||||
|
||||
class SPIDeviceManager : public AP_HAL::SPIDeviceManager {
|
||||
public:
|
||||
|
||||
SPIDeviceManager();
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> get_device(const char *name) override;
|
||||
|
||||
static SPIDesc device_table[];
|
||||
static SPIBus *buses;
|
||||
};
|
||||
|
||||
}
|
||||
Loading…
Reference in New Issue