2018-01-05 02:19:51 -04:00
|
|
|
/*
|
|
|
|
* 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/>.
|
|
|
|
*/
|
2022-02-20 23:44:56 -04:00
|
|
|
|
|
|
|
#include <hal.h>
|
2018-01-05 02:19:51 -04:00
|
|
|
#include "Device.h"
|
|
|
|
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#include <AP_HAL/utility/OwnPtr.h>
|
|
|
|
#include <stdio.h>
|
2018-08-07 05:47:10 -03:00
|
|
|
|
2021-05-29 16:47:40 -03:00
|
|
|
#if HAL_USE_I2C == TRUE || HAL_USE_SPI == TRUE || HAL_USE_WSPI == TRUE
|
2018-08-07 05:47:10 -03:00
|
|
|
|
2018-01-05 02:19:51 -04:00
|
|
|
#include "Scheduler.h"
|
|
|
|
#include "Semaphores.h"
|
2018-01-09 17:18:28 -04:00
|
|
|
#include "Util.h"
|
2018-06-02 00:27:02 -03:00
|
|
|
#include "hwdef/common/stm32_util.h"
|
2018-01-05 02:19:51 -04:00
|
|
|
|
2019-05-26 22:45:30 -03:00
|
|
|
#ifndef HAL_DEVICE_THREAD_STACK
|
|
|
|
#define HAL_DEVICE_THREAD_STACK 1024
|
|
|
|
#endif
|
|
|
|
|
2018-01-09 17:18:28 -04:00
|
|
|
using namespace ChibiOS;
|
2018-01-05 02:19:51 -04:00
|
|
|
|
2020-02-29 03:57:50 -04:00
|
|
|
extern const AP_HAL::HAL& hal;
|
2018-01-05 02:19:51 -04:00
|
|
|
|
2018-06-02 00:27:02 -03:00
|
|
|
DeviceBus::DeviceBus(uint8_t _thread_priority) :
|
|
|
|
thread_priority(_thread_priority)
|
|
|
|
{
|
2019-03-06 20:21:51 -04:00
|
|
|
bouncebuffer_init(&bounce_buffer_tx, 10, false);
|
|
|
|
bouncebuffer_init(&bounce_buffer_rx, 10, false);
|
2018-06-02 00:27:02 -03:00
|
|
|
}
|
|
|
|
|
2021-05-29 16:47:40 -03:00
|
|
|
DeviceBus::DeviceBus(uint8_t _thread_priority, bool axi_sram) :
|
|
|
|
thread_priority(_thread_priority)
|
|
|
|
{
|
|
|
|
bouncebuffer_init(&bounce_buffer_tx, 10, axi_sram);
|
|
|
|
bouncebuffer_init(&bounce_buffer_rx, 10, axi_sram);
|
|
|
|
}
|
|
|
|
|
2018-01-05 02:19:51 -04:00
|
|
|
/*
|
|
|
|
per-bus callback thread
|
|
|
|
*/
|
|
|
|
void DeviceBus::bus_thread(void *arg)
|
|
|
|
{
|
|
|
|
struct DeviceBus *binfo = (struct DeviceBus *)arg;
|
|
|
|
|
|
|
|
while (true) {
|
|
|
|
uint64_t now = AP_HAL::micros64();
|
|
|
|
DeviceBus::callback_info *callback;
|
|
|
|
|
|
|
|
// find a callback to run
|
|
|
|
for (callback = binfo->callbacks; callback; callback = callback->next) {
|
|
|
|
if (now >= callback->next_usec) {
|
|
|
|
while (now >= callback->next_usec) {
|
|
|
|
callback->next_usec += callback->period_usec;
|
|
|
|
}
|
|
|
|
// call it with semaphore held
|
2020-01-18 17:42:33 -04:00
|
|
|
WITH_SEMAPHORE(binfo->semaphore);
|
|
|
|
callback->cb();
|
2018-01-05 02:19:51 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// work out when next loop is needed
|
|
|
|
uint64_t next_needed = 0;
|
|
|
|
now = AP_HAL::micros64();
|
|
|
|
|
|
|
|
for (callback = binfo->callbacks; callback; callback = callback->next) {
|
|
|
|
if (next_needed == 0 ||
|
|
|
|
callback->next_usec < next_needed) {
|
|
|
|
next_needed = callback->next_usec;
|
|
|
|
if (next_needed < now) {
|
|
|
|
next_needed = now;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// delay for at most 50ms, to handle newly added callbacks
|
|
|
|
uint32_t delay = 50000;
|
|
|
|
if (next_needed >= now && next_needed - now < delay) {
|
|
|
|
delay = next_needed - now;
|
|
|
|
}
|
2018-08-30 18:08:14 -03:00
|
|
|
// don't delay for less than 100usec, so one thread doesn't
|
2018-01-05 02:19:51 -04:00
|
|
|
// completely dominate the CPU
|
2018-01-17 04:36:12 -04:00
|
|
|
if (delay < 100) {
|
|
|
|
delay = 100;
|
2018-01-05 02:19:51 -04:00
|
|
|
}
|
|
|
|
hal.scheduler->delay_microseconds(delay);
|
|
|
|
}
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2018-03-28 23:07:50 -03:00
|
|
|
#if CH_CFG_USE_HEAP == TRUE
|
2018-01-05 02:19:51 -04:00
|
|
|
AP_HAL::Device::PeriodicHandle DeviceBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb, AP_HAL::Device *_hal_device)
|
|
|
|
{
|
|
|
|
if (!thread_started) {
|
|
|
|
thread_started = true;
|
|
|
|
|
|
|
|
hal_device = _hal_device;
|
|
|
|
// setup a name for the thread
|
2018-02-14 18:33:12 -04:00
|
|
|
const uint8_t name_len = 7;
|
|
|
|
char *name = (char *)malloc(name_len);
|
2024-09-17 00:18:37 -03:00
|
|
|
if (name == nullptr){
|
|
|
|
return nullptr;
|
|
|
|
}
|
2018-01-05 02:19:51 -04:00
|
|
|
switch (hal_device->bus_type()) {
|
|
|
|
case AP_HAL::Device::BUS_TYPE_I2C:
|
2020-04-28 03:32:29 -03:00
|
|
|
snprintf(name, name_len, "I2C%u",
|
2018-01-05 02:19:51 -04:00
|
|
|
hal_device->bus_num());
|
|
|
|
break;
|
|
|
|
|
|
|
|
case AP_HAL::Device::BUS_TYPE_SPI:
|
2020-04-28 03:32:29 -03:00
|
|
|
snprintf(name, name_len, "SPI%u",
|
2018-01-05 02:19:51 -04:00
|
|
|
hal_device->bus_num());
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2019-05-26 22:45:30 -03:00
|
|
|
thread_ctx = thread_create_alloc(THD_WORKING_AREA_SIZE(HAL_DEVICE_THREAD_STACK),
|
2019-02-26 20:14:58 -04:00
|
|
|
name,
|
|
|
|
thread_priority, /* Initial priority. */
|
|
|
|
DeviceBus::bus_thread, /* Thread function. */
|
|
|
|
this); /* Thread parameter. */
|
|
|
|
if (thread_ctx == nullptr) {
|
|
|
|
AP_HAL::panic("Failed to create bus thread %s", name);
|
|
|
|
}
|
2018-01-05 02:19:51 -04:00
|
|
|
}
|
2024-05-26 22:24:11 -03:00
|
|
|
DeviceBus::callback_info *callback = NEW_NOTHROW DeviceBus::callback_info;
|
2018-01-05 02:19:51 -04:00
|
|
|
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;
|
|
|
|
}
|
2018-03-28 23:07:50 -03:00
|
|
|
#endif // CH_CFG_USE_HEAP
|
2018-01-05 02:19:51 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
* Adjust the timer for the next call: it needs to be called from the bus
|
|
|
|
* thread, otherwise it will race with it
|
|
|
|
*/
|
|
|
|
bool DeviceBus::adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
|
|
|
|
{
|
|
|
|
if (chThdGetSelfX() != thread_ctx) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
DeviceBus::callback_info *callback = static_cast<DeviceBus::callback_info *>(h);
|
|
|
|
|
|
|
|
callback->period_usec = period_usec;
|
|
|
|
callback->next_usec = AP_HAL::micros64() + period_usec;
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2018-01-09 17:18:28 -04:00
|
|
|
/*
|
|
|
|
setup to use DMA-safe bouncebuffers for device transfers
|
|
|
|
*/
|
2020-01-17 00:22:44 -04:00
|
|
|
bool DeviceBus::bouncebuffer_setup(const uint8_t *&buf_tx, uint16_t tx_len,
|
2018-01-09 17:18:28 -04:00
|
|
|
uint8_t *&buf_rx, uint16_t rx_len)
|
2018-04-28 21:09:09 -03:00
|
|
|
{
|
2019-03-10 00:01:37 -04:00
|
|
|
if (buf_rx) {
|
2020-01-17 00:22:44 -04:00
|
|
|
if (!bouncebuffer_setup_read(bounce_buffer_rx, &buf_rx, rx_len)) {
|
|
|
|
return false;
|
|
|
|
}
|
2018-01-09 17:18:28 -04:00
|
|
|
}
|
2019-03-10 00:01:37 -04:00
|
|
|
if (buf_tx) {
|
2020-01-17 00:22:44 -04:00
|
|
|
if (!bouncebuffer_setup_write(bounce_buffer_tx, &buf_tx, tx_len)) {
|
|
|
|
if (buf_rx) {
|
|
|
|
bouncebuffer_abort(bounce_buffer_rx);
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
2018-01-09 17:18:28 -04:00
|
|
|
}
|
2020-01-17 00:22:44 -04:00
|
|
|
return true;
|
2018-01-09 17:18:28 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
complete a transfer using DMA bounce buffer
|
|
|
|
*/
|
2018-06-02 00:27:02 -03:00
|
|
|
void DeviceBus::bouncebuffer_finish(const uint8_t *buf_tx, uint8_t *buf_rx, uint16_t rx_len)
|
2018-01-09 17:18:28 -04:00
|
|
|
{
|
2018-06-02 00:27:02 -03:00
|
|
|
if (buf_rx) {
|
|
|
|
bouncebuffer_finish_read(bounce_buffer_rx, buf_rx, rx_len);
|
|
|
|
}
|
|
|
|
if (buf_tx) {
|
|
|
|
bouncebuffer_finish_write(bounce_buffer_tx, buf_tx);
|
|
|
|
}
|
2018-01-05 02:19:51 -04:00
|
|
|
}
|
2018-03-06 18:41:03 -04:00
|
|
|
|
|
|
|
#endif // HAL_USE_I2C || HAL_USE_SPI
|