modules: afbr: Add basic functionality
- Add AFBR module as a HAL. - Platform layer to support running AFBR API using Zephyr. - Ability to instantiate on device-tree. - Samples in the module proving foundations works. - Zephyr Sensor API support, by introducing: - Read/Decode for SENSOR_CHAN_DISTANCE (1-D results). - Streaming mode for DATA_READY (1-D results). Signed-off-by: Luis Ubieda <luisf@croxel.com>
This commit is contained in:
parent
85b6ff0b0b
commit
f9d9e5bb6d
22 changed files with 1504 additions and 0 deletions
|
@ -4950,6 +4950,18 @@ West:
|
|||
labels:
|
||||
- "platform: ADI"
|
||||
|
||||
"West project: hal_afbr":
|
||||
status: maintained
|
||||
maintainers:
|
||||
- ubieda
|
||||
- bperseghetti
|
||||
collaborators:
|
||||
- PetervdPerk-NXP
|
||||
- jgoppert
|
||||
files: []
|
||||
labels:
|
||||
- "platform: Broadcom"
|
||||
|
||||
"West project: hal_ambiq":
|
||||
status: odd fixes
|
||||
collaborators:
|
||||
|
|
|
@ -6,6 +6,7 @@ add_subdirectory(ams)
|
|||
add_subdirectory(aosong)
|
||||
add_subdirectory(asahi_kasei)
|
||||
add_subdirectory(bosch)
|
||||
add_subdirectory(broadcom)
|
||||
add_subdirectory(espressif)
|
||||
add_subdirectory(everlight)
|
||||
add_subdirectory(honeywell)
|
||||
|
|
|
@ -92,6 +92,7 @@ source "drivers/sensor/ams/Kconfig"
|
|||
source "drivers/sensor/aosong/Kconfig"
|
||||
source "drivers/sensor/asahi_kasei/Kconfig"
|
||||
source "drivers/sensor/bosch/Kconfig"
|
||||
source "drivers/sensor/broadcom/Kconfig"
|
||||
source "drivers/sensor/espressif/Kconfig"
|
||||
source "drivers/sensor/everlight/Kconfig"
|
||||
source "drivers/sensor/honeywell/Kconfig"
|
||||
|
|
5
drivers/sensor/broadcom/CMakeLists.txt
Normal file
5
drivers/sensor/broadcom/CMakeLists.txt
Normal file
|
@ -0,0 +1,5 @@
|
|||
# Copyright (c) 2025 Croxel Inc.
|
||||
# Copyright (c) 2025 CogniPilot Foundation
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
add_subdirectory_ifdef(CONFIG_AFBR_S50 afbr_s50)
|
5
drivers/sensor/broadcom/Kconfig
Normal file
5
drivers/sensor/broadcom/Kconfig
Normal file
|
@ -0,0 +1,5 @@
|
|||
# Copyright (c) 2025 Croxel Inc.
|
||||
# Copyright (c) 2025 CogniPilot Foundation
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
source "drivers/sensor/broadcom/afbr_s50/Kconfig"
|
9
drivers/sensor/broadcom/afbr_s50/CMakeLists.txt
Normal file
9
drivers/sensor/broadcom/afbr_s50/CMakeLists.txt
Normal file
|
@ -0,0 +1,9 @@
|
|||
# Copyright (c) 2025 Croxel Inc.
|
||||
# Copyright (c) 2025 CogniPilot Foundation
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
zephyr_library()
|
||||
zephyr_library_sources(
|
||||
afbr_s50.c
|
||||
afbr_s50_decoder.c
|
||||
)
|
17
drivers/sensor/broadcom/afbr_s50/Kconfig
Normal file
17
drivers/sensor/broadcom/afbr_s50/Kconfig
Normal file
|
@ -0,0 +1,17 @@
|
|||
# Copyright (c) 2025 Croxel Inc.
|
||||
# Copyright (c) 2025 CogniPilot Foundation
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
config AFBR_S50
|
||||
bool "AFBR-S50 Time-of-Flight Sensor"
|
||||
default y
|
||||
depends on DT_HAS_BRCM_AFBR_S50_ENABLED
|
||||
select SENSOR_ASYNC_API
|
||||
select SPI
|
||||
select SPI_RTIO
|
||||
select RTIO_WORKQ
|
||||
select AFBR_LIB
|
||||
select PINCTRL
|
||||
select PINCTRL_NON_STATIC
|
||||
help
|
||||
Enable driver for the AFBR-S50 Time-of-Flight sensor.
|
389
drivers/sensor/broadcom/afbr_s50/afbr_s50.c
Normal file
389
drivers/sensor/broadcom/afbr_s50/afbr_s50.c
Normal file
|
@ -0,0 +1,389 @@
|
|||
/*
|
||||
* Copyright (c) 2025 Croxel Inc.
|
||||
* Copyright (c) 2025 CogniPilot Foundation
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#define DT_DRV_COMPAT brcm_afbr_s50
|
||||
|
||||
#include <zephyr/drivers/sensor.h>
|
||||
#include <zephyr/drivers/sensor_clock.h>
|
||||
#include <zephyr/drivers/spi.h>
|
||||
#include <zephyr/drivers/gpio.h>
|
||||
#include <zephyr/sys/check.h>
|
||||
#include <zephyr/rtio/rtio.h>
|
||||
#include <zephyr/rtio/work.h>
|
||||
|
||||
#include <api/argus_api.h>
|
||||
#include <platform/argus_irq.h>
|
||||
|
||||
#include "afbr_s50_decoder.h"
|
||||
#include "platform.h"
|
||||
|
||||
#include <zephyr/logging/log.h>
|
||||
LOG_MODULE_REGISTER(AFBR_S50, CONFIG_SENSOR_LOG_LEVEL);
|
||||
|
||||
struct afbr_s50_data {
|
||||
/** RTIO section was included in the device data struct since the Argus
|
||||
* API does not support passing a parameter to get through the async
|
||||
* handler. Therefore, we're getting it through object composition:
|
||||
* (handler -> platform -> RTIO context). Not used for decoding,
|
||||
* which should be kept stateless.
|
||||
*/
|
||||
struct {
|
||||
struct rtio_iodev_sqe *iodev_sqe;
|
||||
} rtio;
|
||||
/** Not relevant for the driver (other than Argus section). Useful
|
||||
* for platform abstractions present under modules sub-directory.
|
||||
*/
|
||||
struct afbr_s50_platform_data platform;
|
||||
};
|
||||
|
||||
/* Only used to get DTS bindings, otherwise passed onto platform struct */
|
||||
struct afbr_s50_config {
|
||||
struct gpio_dt_spec cs;
|
||||
struct gpio_dt_spec clk;
|
||||
struct gpio_dt_spec mosi;
|
||||
struct gpio_dt_spec miso;
|
||||
struct gpio_dt_spec irq;
|
||||
};
|
||||
|
||||
static void data_ready_work_handler(struct rtio_iodev_sqe *iodev_sqe)
|
||||
{
|
||||
const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data;
|
||||
const struct device *dev = cfg->sensor;
|
||||
struct afbr_s50_data *data = dev->data;
|
||||
size_t edata_len = 0;
|
||||
status_t status;
|
||||
int err;
|
||||
|
||||
struct afbr_s50_edata *edata;
|
||||
|
||||
err = rtio_sqe_rx_buf(iodev_sqe,
|
||||
sizeof(struct afbr_s50_edata),
|
||||
sizeof(struct afbr_s50_edata),
|
||||
(uint8_t **)&edata,
|
||||
&edata_len);
|
||||
|
||||
CHECKIF(err || !edata || edata_len < sizeof(struct afbr_s50_edata)) {
|
||||
LOG_ERR("Failed to get buffer for edata");
|
||||
|
||||
data->rtio.iodev_sqe = NULL;
|
||||
rtio_iodev_sqe_err(iodev_sqe, -ENOMEM);
|
||||
return;
|
||||
}
|
||||
|
||||
uint64_t cycles;
|
||||
|
||||
err = sensor_clock_get_cycles(&cycles);
|
||||
CHECKIF(err != 0) {
|
||||
LOG_ERR("Failed to get sensor clock cycles");
|
||||
return;
|
||||
}
|
||||
|
||||
edata->header.timestamp = sensor_clock_cycles_to_ns(cycles);
|
||||
edata->header.channels = afbr_s50_encode_channel(SENSOR_CHAN_DISTANCE);
|
||||
edata->header.events = cfg->is_streaming ? afbr_s50_encode_event(SENSOR_TRIG_DATA_READY) :
|
||||
0;
|
||||
|
||||
status = Argus_EvaluateData(data->platform.argus.handle, &edata->payload);
|
||||
if (status != STATUS_OK || edata->payload.Status != STATUS_OK) {
|
||||
LOG_ERR("Data not valid: %d, %d", status, edata->payload.Status);
|
||||
|
||||
data->rtio.iodev_sqe = NULL;
|
||||
rtio_iodev_sqe_err(iodev_sqe, -EIO);
|
||||
} else {
|
||||
data->rtio.iodev_sqe = NULL;
|
||||
rtio_iodev_sqe_ok(iodev_sqe, 0);
|
||||
}
|
||||
|
||||
/** After freeing the buffer with EvaluateData, decide whether to
|
||||
* cancel future submissions.
|
||||
*/
|
||||
if (FIELD_GET(RTIO_SQE_CANCELED, iodev_sqe->sqe.flags) &&
|
||||
Argus_IsTimerMeasurementActive(data->platform.argus.handle)) {
|
||||
LOG_WRN("OP cancelled. Stopping stream");
|
||||
|
||||
(void)Argus_StopMeasurementTimer(data->platform.argus.handle);
|
||||
}
|
||||
}
|
||||
|
||||
static status_t data_ready_callback(status_t status, argus_hnd_t *hnd)
|
||||
{
|
||||
struct afbr_s50_platform_data *platform;
|
||||
int err;
|
||||
|
||||
/** Both this and the CONTAINER_OF calls are a workaround to obtain the
|
||||
* associated RTIO context and its buffer, given this callback does not
|
||||
* support passing an userparam where we'd typically send the
|
||||
* iodev_sqe.
|
||||
*/
|
||||
err = afbr_s50_platform_get_by_hnd(hnd, &platform);
|
||||
CHECKIF(err) {
|
||||
__ASSERT(false, "Failed to get platform data SQE response can't be sent");
|
||||
return ERROR_FAIL;
|
||||
}
|
||||
|
||||
struct afbr_s50_data *data = CONTAINER_OF(platform, struct afbr_s50_data, platform);
|
||||
struct rtio_iodev_sqe *iodev_sqe = data->rtio.iodev_sqe;
|
||||
|
||||
if (status != STATUS_OK) {
|
||||
LOG_ERR("Measurement failed: %d", status);
|
||||
|
||||
data->rtio.iodev_sqe = NULL;
|
||||
rtio_iodev_sqe_err(iodev_sqe, -EIO);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
/** RTIO workqueue used since the description of Argus_EvaluateResult()
|
||||
* discourages its use in the callback context as it may be blocking
|
||||
* while in ISR context.
|
||||
*/
|
||||
struct rtio_work_req *req = rtio_work_req_alloc();
|
||||
|
||||
CHECKIF(!req) {
|
||||
LOG_ERR("RTIO work item allocation failed. Consider to increase "
|
||||
"CONFIG_RTIO_WORKQ_POOL_ITEMS");
|
||||
|
||||
data->rtio.iodev_sqe = NULL;
|
||||
rtio_iodev_sqe_err(iodev_sqe, -ENOMEM);
|
||||
return ERROR_FAIL;
|
||||
}
|
||||
|
||||
rtio_work_req_submit(req, iodev_sqe, data_ready_work_handler);
|
||||
|
||||
return STATUS_OK;
|
||||
}
|
||||
|
||||
static void afbr_s50_submit_single_shot(const struct device *dev,
|
||||
struct rtio_iodev_sqe *iodev_sqe)
|
||||
{
|
||||
struct afbr_s50_data *data = dev->data;
|
||||
|
||||
/** If there's an op in process, reject ignore requests */
|
||||
if (data->rtio.iodev_sqe != NULL) {
|
||||
LOG_WRN("Operation in progress. Rejecting request");
|
||||
|
||||
rtio_iodev_sqe_err(iodev_sqe, -EBUSY);
|
||||
return;
|
||||
}
|
||||
data->rtio.iodev_sqe = iodev_sqe;
|
||||
|
||||
status_t status = Argus_TriggerMeasurement(data->platform.argus.handle,
|
||||
data_ready_callback);
|
||||
if (status != STATUS_OK) {
|
||||
LOG_ERR("Argus_TriggerMeasurement failed: %d", status);
|
||||
|
||||
data->rtio.iodev_sqe = NULL;
|
||||
rtio_iodev_sqe_err(iodev_sqe, -EIO);
|
||||
}
|
||||
}
|
||||
|
||||
static void afbr_s50_submit_streaming(const struct device *dev,
|
||||
struct rtio_iodev_sqe *iodev_sqe)
|
||||
{
|
||||
struct afbr_s50_data *data = dev->data;
|
||||
const struct sensor_read_config *read_cfg = iodev_sqe->sqe.iodev->data;
|
||||
|
||||
/** If there's an op in process, reject ignore requests */
|
||||
if (data->rtio.iodev_sqe != NULL) {
|
||||
LOG_WRN("Operation in progress");
|
||||
|
||||
rtio_iodev_sqe_err(iodev_sqe, -EBUSY);
|
||||
return;
|
||||
}
|
||||
data->rtio.iodev_sqe = iodev_sqe;
|
||||
|
||||
CHECKIF(read_cfg->triggers->trigger != SENSOR_TRIG_DATA_READY ||
|
||||
read_cfg->count != 1 ||
|
||||
read_cfg->triggers->opt != SENSOR_STREAM_DATA_INCLUDE) {
|
||||
LOG_ERR("Invalid trigger for streaming mode");
|
||||
|
||||
data->rtio.iodev_sqe = NULL;
|
||||
rtio_iodev_sqe_err(iodev_sqe, -EINVAL);
|
||||
return;
|
||||
}
|
||||
|
||||
/** Given the streaming mode involves multi-shot submissions, we only
|
||||
* need to kick-off the timer once until explicitly stopped.
|
||||
*/
|
||||
if (!Argus_IsTimerMeasurementActive(data->platform.argus.handle)) {
|
||||
status_t status = Argus_StartMeasurementTimer(data->platform.argus.handle,
|
||||
data_ready_callback);
|
||||
|
||||
if (status != STATUS_OK) {
|
||||
LOG_ERR("Argus_TriggerMeasurement failed: %d", status);
|
||||
|
||||
data->rtio.iodev_sqe = NULL;
|
||||
rtio_iodev_sqe_err(iodev_sqe, -EIO);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void afbr_s50_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
|
||||
{
|
||||
const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data;
|
||||
|
||||
if (!cfg->is_streaming) {
|
||||
afbr_s50_submit_single_shot(dev, iodev_sqe);
|
||||
} else {
|
||||
afbr_s50_submit_streaming(dev, iodev_sqe);
|
||||
}
|
||||
}
|
||||
|
||||
static DEVICE_API(sensor, afbr_s50_driver_api) = {
|
||||
.submit = afbr_s50_submit,
|
||||
.get_decoder = afbr_s50_get_decoder,
|
||||
};
|
||||
|
||||
static sys_slist_t afbr_s50_init_list = SYS_SLIST_STATIC_INIT(afbr_s50_init_list);
|
||||
|
||||
void afbr_s50_platform_init_hooks_add(struct afbr_s50_platform_init_node *node)
|
||||
{
|
||||
sys_slist_append(&afbr_s50_init_list, &node->node);
|
||||
}
|
||||
|
||||
int afbr_s50_platform_init(struct afbr_s50_platform_data *platform_data)
|
||||
{
|
||||
struct afbr_s50_platform_init_node *node;
|
||||
|
||||
SYS_SLIST_FOR_EACH_CONTAINER(&afbr_s50_init_list, node, node) {
|
||||
int err = node->init_fn(platform_data);
|
||||
|
||||
if (err) {
|
||||
return err;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int afbr_s50_init(const struct device *dev)
|
||||
{
|
||||
struct afbr_s50_data *data = dev->data;
|
||||
status_t status;
|
||||
int err;
|
||||
|
||||
err = afbr_s50_platform_init(&data->platform);
|
||||
if (err) {
|
||||
LOG_ERR("Failed to initialize platform hooks: %d", err);
|
||||
return err;
|
||||
}
|
||||
|
||||
data->platform.argus.handle = Argus_CreateHandle();
|
||||
if (data->platform.argus.handle == NULL) {
|
||||
LOG_ERR("Failed to create handle");
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
status = Argus_Init(data->platform.argus.handle, data->platform.argus.id);
|
||||
if (status != STATUS_OK) {
|
||||
LOG_ERR("Failed to initialize device");
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
status = Argus_SetConfigurationFrameTime(data->platform.argus.handle,
|
||||
100000);
|
||||
if (status != STATUS_OK) {
|
||||
LOG_ERR("Failed to set frame time");
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/** Macrobatics to get a list of compatible sensors in order to map them back
|
||||
* and forth at run-time.
|
||||
*/
|
||||
#define AFBR_S50_LIST(inst) DEVICE_DT_GET(DT_DRV_INST(inst)),
|
||||
|
||||
const static struct device *afbr_s50_list[] = {
|
||||
DT_INST_FOREACH_STATUS_OKAY(AFBR_S50_LIST)
|
||||
};
|
||||
|
||||
int afbr_s50_platform_get_by_id(s2pi_slave_t slave,
|
||||
struct afbr_s50_platform_data **data)
|
||||
{
|
||||
for (size_t i = 0 ; i < ARRAY_SIZE(afbr_s50_list) ; i++) {
|
||||
struct afbr_s50_data *drv_data = afbr_s50_list[i]->data;
|
||||
|
||||
if (drv_data->platform.argus.id == slave) {
|
||||
*data = &drv_data->platform;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
int afbr_s50_platform_get_by_hnd(argus_hnd_t *hnd,
|
||||
struct afbr_s50_platform_data **data)
|
||||
{
|
||||
for (size_t i = 0 ; i < ARRAY_SIZE(afbr_s50_list) ; i++) {
|
||||
struct afbr_s50_data *drv_data = afbr_s50_list[i]->data;
|
||||
|
||||
if (drv_data->platform.argus.handle == hnd) {
|
||||
*data = &drv_data->platform;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
BUILD_ASSERT(CONFIG_MAIN_STACK_SIZE >= 4096,
|
||||
"AFBR S50 driver requires a stack size of at least 4096 bytes to properly initialize");
|
||||
|
||||
#define AFBR_S50_INIT(inst) \
|
||||
\
|
||||
RTIO_DEFINE(afbr_s50_rtio_ctx_##inst, 8, 8); \
|
||||
SPI_DT_IODEV_DEFINE(afbr_s50_bus_##inst, \
|
||||
DT_DRV_INST(inst), \
|
||||
SPI_OP_MODE_MASTER | SPI_WORD_SET(8) | SPI_TRANSFER_MSB | \
|
||||
SPI_MODE_CPOL | SPI_MODE_CPHA, \
|
||||
0U); \
|
||||
\
|
||||
static const struct afbr_s50_config afbr_s50_cfg_##inst = { \
|
||||
.irq = GPIO_DT_SPEC_INST_GET_OR(inst, int_gpios, {0}), \
|
||||
.clk = GPIO_DT_SPEC_INST_GET_OR(inst, spi_sck_gpios, {0}), \
|
||||
.miso = GPIO_DT_SPEC_INST_GET_OR(inst, spi_miso_gpios, {0}), \
|
||||
.mosi = GPIO_DT_SPEC_INST_GET_OR(inst, spi_mosi_gpios, {0}), \
|
||||
}; \
|
||||
\
|
||||
PINCTRL_DT_DEV_CONFIG_DECLARE(DT_INST_PARENT(inst)); \
|
||||
\
|
||||
static struct afbr_s50_data afbr_s50_data_##inst = { \
|
||||
.platform = { \
|
||||
.argus.id = inst + 1, \
|
||||
.s2pi = { \
|
||||
.pincfg = PINCTRL_DT_DEV_CONFIG_GET(DT_INST_PARENT(inst)), \
|
||||
.rtio = { \
|
||||
.iodev = &afbr_s50_bus_##inst, \
|
||||
.ctx = &afbr_s50_rtio_ctx_##inst, \
|
||||
}, \
|
||||
.gpio = { \
|
||||
.spi = { \
|
||||
.cs = \
|
||||
&_spi_dt_spec_##afbr_s50_bus_##inst.config.cs.gpio,\
|
||||
.clk = &afbr_s50_cfg_##inst.clk, \
|
||||
.mosi = &afbr_s50_cfg_##inst.mosi, \
|
||||
.miso = &afbr_s50_cfg_##inst.miso, \
|
||||
}, \
|
||||
.irq = &afbr_s50_cfg_##inst.irq, \
|
||||
}, \
|
||||
}, \
|
||||
}, \
|
||||
}; \
|
||||
\
|
||||
SENSOR_DEVICE_DT_INST_DEFINE(inst, \
|
||||
afbr_s50_init, \
|
||||
NULL, \
|
||||
&afbr_s50_data_##inst, \
|
||||
&afbr_s50_cfg_##inst, \
|
||||
POST_KERNEL, \
|
||||
CONFIG_SENSOR_INIT_PRIORITY, \
|
||||
&afbr_s50_driver_api);
|
||||
|
||||
DT_INST_FOREACH_STATUS_OKAY(AFBR_S50_INIT)
|
141
drivers/sensor/broadcom/afbr_s50/afbr_s50_decoder.c
Normal file
141
drivers/sensor/broadcom/afbr_s50/afbr_s50_decoder.c
Normal file
|
@ -0,0 +1,141 @@
|
|||
/*
|
||||
* Copyright (c) 2025 Croxel Inc.
|
||||
* Copyright (c) 2025 CogniPilot Foundation
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#define DT_DRV_COMPAT brcm_afbr_s50
|
||||
|
||||
#include <zephyr/drivers/sensor_clock.h>
|
||||
#include <api/argus_res.h>
|
||||
|
||||
#include "afbr_s50_decoder.h"
|
||||
|
||||
#include <zephyr/logging/log.h>
|
||||
LOG_MODULE_REGISTER(AFBR_S50_DECODER, CONFIG_SENSOR_LOG_LEVEL);
|
||||
|
||||
uint8_t afbr_s50_encode_channel(uint16_t chan)
|
||||
{
|
||||
switch (chan) {
|
||||
case SENSOR_CHAN_DISTANCE:
|
||||
return BIT(0);
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t afbr_s50_encode_event(enum sensor_trigger_type trigger)
|
||||
{
|
||||
if (trigger == SENSOR_TRIG_DATA_READY) {
|
||||
return BIT(0);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int afbr_s50_decoder_get_frame_count(const uint8_t *buffer,
|
||||
struct sensor_chan_spec chan_spec,
|
||||
uint16_t *frame_count)
|
||||
{
|
||||
const struct afbr_s50_edata *edata = (const struct afbr_s50_edata *)buffer;
|
||||
|
||||
if (chan_spec.chan_idx != 0) {
|
||||
return -ENOTSUP;
|
||||
}
|
||||
|
||||
switch (chan_spec.chan_type) {
|
||||
case SENSOR_CHAN_DISTANCE:
|
||||
if (edata->header.channels & afbr_s50_encode_channel(SENSOR_CHAN_DISTANCE)) {
|
||||
*frame_count = 1;
|
||||
return 0;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
*frame_count = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int afbr_s50_decoder_get_size_info(struct sensor_chan_spec chan_spec,
|
||||
size_t *base_size,
|
||||
size_t *frame_size)
|
||||
{
|
||||
switch (chan_spec.chan_type) {
|
||||
case SENSOR_CHAN_DISTANCE:
|
||||
*base_size = sizeof(struct sensor_q31_data);
|
||||
*frame_size = sizeof(struct sensor_q31_sample_data);
|
||||
return 0;
|
||||
default:
|
||||
return -ENOTSUP;
|
||||
}
|
||||
}
|
||||
|
||||
static int afbr_s50_decoder_decode(const uint8_t *buffer,
|
||||
struct sensor_chan_spec chan_spec,
|
||||
uint32_t *fit,
|
||||
uint16_t max_count,
|
||||
void *data_out)
|
||||
{
|
||||
const struct afbr_s50_edata *edata = (const struct afbr_s50_edata *)buffer;
|
||||
|
||||
if (*fit != 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (max_count == 0 || chan_spec.chan_idx != 0) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
switch (chan_spec.chan_type) {
|
||||
case SENSOR_CHAN_DISTANCE: {
|
||||
struct sensor_q31_data *out = data_out;
|
||||
|
||||
if ((edata->header.channels & afbr_s50_encode_channel(SENSOR_CHAN_DISTANCE)) == 0) {
|
||||
return -ENODATA;
|
||||
}
|
||||
|
||||
out->header.base_timestamp_ns = edata->header.timestamp;
|
||||
out->header.reading_count = 1;
|
||||
|
||||
/* Result comes encoded in Q9.22 format */
|
||||
out->shift = 9;
|
||||
out->readings[0].value = edata->payload.Bin.Range;
|
||||
|
||||
*fit = 1;
|
||||
return 1;
|
||||
}
|
||||
default:
|
||||
return -EINVAL;
|
||||
}
|
||||
}
|
||||
|
||||
static bool afbr_s50_decoder_has_trigger(const uint8_t *buffer,
|
||||
enum sensor_trigger_type trigger)
|
||||
{
|
||||
const struct afbr_s50_edata *edata = (const struct afbr_s50_edata *)buffer;
|
||||
|
||||
if (trigger == SENSOR_TRIG_DATA_READY) {
|
||||
return edata->header.events & afbr_s50_encode_event(SENSOR_TRIG_DATA_READY);
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
SENSOR_DECODER_API_DT_DEFINE() = {
|
||||
.get_frame_count = afbr_s50_decoder_get_frame_count,
|
||||
.get_size_info = afbr_s50_decoder_get_size_info,
|
||||
.decode = afbr_s50_decoder_decode,
|
||||
.has_trigger = afbr_s50_decoder_has_trigger,
|
||||
};
|
||||
|
||||
int afbr_s50_get_decoder(const struct device *dev,
|
||||
const struct sensor_decoder_api **decoder)
|
||||
{
|
||||
ARG_UNUSED(dev);
|
||||
*decoder = &SENSOR_DECODER_NAME();
|
||||
|
||||
return 0;
|
||||
}
|
31
drivers/sensor/broadcom/afbr_s50/afbr_s50_decoder.h
Normal file
31
drivers/sensor/broadcom/afbr_s50/afbr_s50_decoder.h
Normal file
|
@ -0,0 +1,31 @@
|
|||
/*
|
||||
* Copyright (c) 2023 Google LLC
|
||||
* Copyright (c) 2025 Croxel Inc.
|
||||
* Copyright (c) 2025 CogniPilot Foundation
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#ifndef ZEPHYR_DRIVERS_SENSOR_AFBR_S50_DECODER_H_
|
||||
#define ZEPHYR_DRIVERS_SENSOR_AFBR_S50_DECODER_H_
|
||||
|
||||
#include <stdint.h>
|
||||
#include <zephyr/drivers/sensor.h>
|
||||
#include <api/argus_res.h>
|
||||
|
||||
struct afbr_s50_edata {
|
||||
struct {
|
||||
uint64_t timestamp;
|
||||
uint8_t channels : 1;
|
||||
uint8_t events : 1;
|
||||
} header;
|
||||
argus_results_t payload;
|
||||
};
|
||||
|
||||
uint8_t afbr_s50_encode_channel(uint16_t chan);
|
||||
uint8_t afbr_s50_encode_event(enum sensor_trigger_type trigger);
|
||||
|
||||
int afbr_s50_get_decoder(const struct device *dev,
|
||||
const struct sensor_decoder_api **decoder);
|
||||
|
||||
#endif /* ZEPHYR_DRIVERS_SENSOR_AFBR_S50_DECODER_H_ */
|
71
drivers/sensor/broadcom/afbr_s50/platform.h
Normal file
71
drivers/sensor/broadcom/afbr_s50/platform.h
Normal file
|
@ -0,0 +1,71 @@
|
|||
/*
|
||||
* Copyright (c) 2022 Intel Corporation
|
||||
* Copyright (c) 2025 Croxel Inc.
|
||||
* Copyright (c) 2025 CogniPilot Foundation
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#ifndef ZEPHYR_DRIVERS_SENSOR_BROADCOM_AFBR_S50_PLATFORM_H_
|
||||
#define ZEPHYR_DRIVERS_SENSOR_BROADCOM_AFBR_S50_PLATFORM_H_
|
||||
|
||||
#include <zephyr/kernel.h>
|
||||
#include <zephyr/drivers/gpio.h>
|
||||
#include <zephyr/drivers/pinctrl.h>
|
||||
#include <zephyr/rtio/rtio.h>
|
||||
#include <zephyr/sys/slist.h>
|
||||
|
||||
#include <platform/argus_s2pi.h>
|
||||
|
||||
struct afbr_s50_platform_data {
|
||||
struct {
|
||||
argus_hnd_t *handle;
|
||||
const uint8_t id;
|
||||
} argus;
|
||||
struct {
|
||||
struct k_timer timer;
|
||||
uint32_t interval_us;
|
||||
void *param;
|
||||
} timer;
|
||||
struct {
|
||||
atomic_t mode;
|
||||
const struct pinctrl_dev_config *pincfg;
|
||||
struct {
|
||||
struct rtio_iodev *iodev;
|
||||
struct rtio *ctx;
|
||||
atomic_t state;
|
||||
struct {
|
||||
s2pi_callback_t handler;
|
||||
void *data;
|
||||
} callback;
|
||||
} rtio;
|
||||
struct {
|
||||
struct gpio_callback cb;
|
||||
s2pi_irq_callback_t handler;
|
||||
void *data;
|
||||
} irq;
|
||||
struct {
|
||||
struct {
|
||||
const struct gpio_dt_spec *cs;
|
||||
const struct gpio_dt_spec *clk;
|
||||
const struct gpio_dt_spec *mosi;
|
||||
const struct gpio_dt_spec *miso;
|
||||
} spi;
|
||||
const struct gpio_dt_spec * const irq;
|
||||
} gpio;
|
||||
} s2pi;
|
||||
};
|
||||
|
||||
struct afbr_s50_platform_init_node {
|
||||
sys_snode_t node;
|
||||
int (*init_fn)(struct afbr_s50_platform_data *data);
|
||||
};
|
||||
|
||||
void afbr_s50_platform_init_hooks_add(struct afbr_s50_platform_init_node *node);
|
||||
|
||||
int afbr_s50_platform_get_by_id(s2pi_slave_t slave,
|
||||
struct afbr_s50_platform_data **data);
|
||||
int afbr_s50_platform_get_by_hnd(argus_hnd_t *hnd,
|
||||
struct afbr_s50_platform_data **data);
|
||||
|
||||
#endif /* ZEPHYR_DRIVERS_SENSOR_BROADCOM_AFBR_S50_PLATFORM_H_ */
|
76
dts/bindings/sensor/brcm,afbr-s50.yaml
Normal file
76
dts/bindings/sensor/brcm,afbr-s50.yaml
Normal file
|
@ -0,0 +1,76 @@
|
|||
# Copyright (c) 2025 Croxel Inc.
|
||||
# Copyright (c) 2025 CogniPilot Foundation
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
description: |
|
||||
AFBR-S50 3D ToF sensor
|
||||
|
||||
This sensor requires the SPI bus attached to it, to define the following
|
||||
pinctrl states:
|
||||
- pinctrl-0: default (SPI mode).
|
||||
- pinctrl-1: GPIO mode (used to access the internal EEPROM).
|
||||
|
||||
It also requires defining the GPIO pins used for the SPI bus in GPIO mode,
|
||||
under the AFBR node.
|
||||
|
||||
Example:
|
||||
|
||||
&spi_bus {
|
||||
status = "okay";
|
||||
pinctrl-0 = <&spi_bus_default>;
|
||||
pinctrl-1 = <&spi_bus_gpio>;
|
||||
pinctrl-names = "default", "priv_start";
|
||||
...
|
||||
|
||||
afbr_s50: afbr_s50@0 {
|
||||
compatible = "brcm,afbr-s50";
|
||||
reg = <0>;
|
||||
spi-max-frequency = <10000000>;
|
||||
int-gpios = <&gpio0 10 (GPIO_ACTIVE_LOW | GPIO_PULL_UP)>;
|
||||
spi-mosi-gpios = <&gpio0 24 GPIO_ACTIVE_HIGH>;
|
||||
spi-sck-gpios = <&gpio0 25 GPIO_ACTIVE_HIGH>;
|
||||
spi-miso-gpios = <&gpio0 26 GPIO_ACTIVE_HIGH>;
|
||||
};
|
||||
};
|
||||
|
||||
compatible: "brcm,afbr-s50"
|
||||
include: [sensor-device.yaml, spi-device.yaml]
|
||||
|
||||
properties:
|
||||
int-gpios:
|
||||
type: phandle-array
|
||||
required: true
|
||||
description: |
|
||||
The INT signal default configuration is active-low. The
|
||||
property value should ensure the flags properly describe the
|
||||
signal that is presented to the driver.
|
||||
|
||||
spi-mosi-gpios:
|
||||
type: phandle-array
|
||||
required: true
|
||||
description: |
|
||||
SPI MOSI GPIO pin. Used by the driver in order to use the bus in GPIO
|
||||
mode, which is required to interact with the internal EEPROM the
|
||||
AFBR contains (mandatory).
|
||||
This property is required since the SPI bus is configured and used
|
||||
through pinctrl, which is not directly mapped to a GPIO port/pin.
|
||||
|
||||
spi-miso-gpios:
|
||||
type: phandle-array
|
||||
required: true
|
||||
description: |
|
||||
SPI MISO GPIO pin. Used by the driver in order to use the bus in GPIO
|
||||
mode, which is required to interact with the internal EEPROM the
|
||||
AFBR contains (mandatory).
|
||||
This property is required since the SPI bus is configured and used
|
||||
through pinctrl, which is not directly mapped to a GPIO port/pin.
|
||||
|
||||
spi-sck-gpios:
|
||||
type: phandle-array
|
||||
required: true
|
||||
description: |
|
||||
SPI CLK GPIO pin. Used by the driver in order to use the bus in GPIO
|
||||
mode, which is required to interact with the internal EEPROM the
|
||||
AFBR contains (mandatory).
|
||||
This property is required since the SPI bus is configured and used
|
||||
through pinctrl, which is not directly mapped to a GPIO port/pin.
|
49
modules/hal_afbr/CMakeLists.txt
Normal file
49
modules/hal_afbr/CMakeLists.txt
Normal file
|
@ -0,0 +1,49 @@
|
|||
# Copyright (c) 2025 Croxel Inc.
|
||||
# Copyright (c) 2025 CogniPilot Foundation
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
if(CONFIG_AFBR_LIB)
|
||||
|
||||
zephyr_library()
|
||||
|
||||
set(HAL_AFBR_DIR ${ZEPHYR_CURRENT_MODULE_DIR})
|
||||
|
||||
zephyr_include_directories(
|
||||
${HAL_AFBR_DIR}/AFBR-S50/Include
|
||||
)
|
||||
|
||||
zephyr_library_sources(
|
||||
platform_irq.c
|
||||
platform_print.c
|
||||
platform_s2pi.c
|
||||
platform_timer.c
|
||||
platform_malloc.c
|
||||
platform_nvm.c
|
||||
platform_misc.c
|
||||
)
|
||||
|
||||
add_library(afbr_sdk_lib STATIC IMPORTED GLOBAL)
|
||||
|
||||
if(CONFIG_CPU_CORTEX_M0)
|
||||
set_target_properties(afbr_sdk_lib PROPERTIES IMPORTED_LOCATION
|
||||
${HAL_AFBR_DIR}/zephyr/blobs/AFBR-S50/Lib/libafbrs50_m0.a
|
||||
)
|
||||
elseif(CONFIG_CPU_CORTEX_M3)
|
||||
set_target_properties(afbr_sdk_lib PROPERTIES IMPORTED_LOCATION
|
||||
${HAL_AFBR_DIR}/zephyr/blobs/AFBR-S50/Lib/libafbrs50_m3.a
|
||||
)
|
||||
elseif(CONFIG_CPU_CORTEX_M33)
|
||||
set_target_properties(afbr_sdk_lib PROPERTIES IMPORTED_LOCATION
|
||||
${HAL_AFBR_DIR}/zephyr/blobs/AFBR-S50/Lib/libafbrs50_m33.a
|
||||
)
|
||||
elseif(CONFIG_CPU_CORTEX_M4)
|
||||
set_target_properties(afbr_sdk_lib PROPERTIES IMPORTED_LOCATION
|
||||
${HAL_AFBR_DIR}/zephyr/blobs/AFBR-S50/Lib/libafbrs50_m4.a
|
||||
)
|
||||
else()
|
||||
message(FATAL_ERROR "Unsupported CPU type")
|
||||
endif()
|
||||
|
||||
zephyr_link_libraries(afbr_sdk_lib)
|
||||
|
||||
endif() # CONFIG_AFBR_LIB
|
9
modules/hal_afbr/Kconfig
Normal file
9
modules/hal_afbr/Kconfig
Normal file
|
@ -0,0 +1,9 @@
|
|||
# Copyright (c) 2025 Croxel Inc.
|
||||
# Copyright (c) 2025 CogniPilot Foundation
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
|
||||
config ZEPHYR_HAL_AFBR_MODULE
|
||||
bool
|
||||
|
||||
config AFBR_LIB
|
||||
bool
|
28
modules/hal_afbr/platform_irq.c
Normal file
28
modules/hal_afbr/platform_irq.c
Normal file
|
@ -0,0 +1,28 @@
|
|||
/*
|
||||
* Copyright (c) 2025 Croxel Inc.
|
||||
* Copyright (c) 2025 CogniPilot Foundation
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include <zephyr/irq.h>
|
||||
|
||||
#include <platform/argus_irq.h>
|
||||
|
||||
/* We have an atomic lock count in order to only lock once and store the lock key. */
|
||||
static atomic_val_t lock_count;
|
||||
static uint32_t lock;
|
||||
|
||||
void IRQ_UNLOCK(void)
|
||||
{
|
||||
if (atomic_dec(&lock_count) == 1) {
|
||||
irq_unlock(lock);
|
||||
}
|
||||
}
|
||||
|
||||
void IRQ_LOCK(void)
|
||||
{
|
||||
if (atomic_inc(&lock_count) == 0) {
|
||||
lock = irq_lock();
|
||||
}
|
||||
}
|
40
modules/hal_afbr/platform_malloc.c
Normal file
40
modules/hal_afbr/platform_malloc.c
Normal file
|
@ -0,0 +1,40 @@
|
|||
/*
|
||||
* Copyright (c) 2025 Croxel Inc.
|
||||
* Copyright (c) 2025 CogniPilot Foundation
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include <stddef.h>
|
||||
#include <zephyr/kernel.h>
|
||||
#include <zephyr/sys/check.h>
|
||||
|
||||
#define DT_DRV_COMPAT brcm_afbr_s50
|
||||
#define NUM_AFBR_INST DT_NUM_INST_STATUS_OKAY(DT_DRV_COMPAT)
|
||||
|
||||
BUILD_ASSERT(NUM_AFBR_INST > 0, "Invalid number of AFBR-S50 instances");
|
||||
|
||||
/** Defined separate memslab to isolate library from the other components.
|
||||
* Through debugging, the library requests an initial allocation of ~4-KiB,
|
||||
* which is why the total pool is sized to 8-KiB per instance.
|
||||
*/
|
||||
K_MEM_SLAB_DEFINE(argus_memslab, 64, 128 * NUM_AFBR_INST, sizeof(void *));
|
||||
|
||||
void *Argus_Malloc(size_t size)
|
||||
{
|
||||
void *ptr = NULL;
|
||||
int err;
|
||||
|
||||
err = k_mem_slab_alloc(&argus_memslab, &ptr, K_NO_WAIT);
|
||||
|
||||
CHECKIF(err != 0 || ptr == NULL) {
|
||||
return NULL;
|
||||
}
|
||||
|
||||
return ptr;
|
||||
}
|
||||
|
||||
void Argus_Free(void *ptr)
|
||||
{
|
||||
k_mem_slab_free(&argus_memslab, ptr);
|
||||
}
|
24
modules/hal_afbr/platform_misc.c
Normal file
24
modules/hal_afbr/platform_misc.c
Normal file
|
@ -0,0 +1,24 @@
|
|||
/*
|
||||
* Copyright (c) 2025 Croxel Inc.
|
||||
* Copyright (c) 2025 CogniPilot Foundation
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include <api/argus_api.h>
|
||||
|
||||
/** Used to get instance data from slave index */
|
||||
#include <../drivers/sensor/broadcom/afbr_s50/platform.h>
|
||||
|
||||
argus_hnd_t *Argus_GetHandle(s2pi_slave_t spi_slave)
|
||||
{
|
||||
struct afbr_s50_platform_data *data;
|
||||
int err;
|
||||
|
||||
err = afbr_s50_platform_get_by_id(spi_slave, &data);
|
||||
if (err) {
|
||||
return NULL;
|
||||
}
|
||||
|
||||
return data->argus.handle;
|
||||
}
|
18
modules/hal_afbr/platform_nvm.c
Normal file
18
modules/hal_afbr/platform_nvm.c
Normal file
|
@ -0,0 +1,18 @@
|
|||
/*
|
||||
* Copyright (c) 2025 Croxel Inc.
|
||||
* Copyright (c) 2025 CogniPilot Foundation
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include <platform/argus_nvm.h>
|
||||
|
||||
status_t NVM_WriteBlock(uint32_t id, uint32_t block_size, uint8_t const *buf)
|
||||
{
|
||||
return ERROR_NOT_IMPLEMENTED;
|
||||
}
|
||||
|
||||
status_t NVM_ReadBlock(uint32_t id, uint32_t block_size, uint8_t *buf)
|
||||
{
|
||||
return ERROR_NOT_IMPLEMENTED;
|
||||
}
|
21
modules/hal_afbr/platform_print.c
Normal file
21
modules/hal_afbr/platform_print.c
Normal file
|
@ -0,0 +1,21 @@
|
|||
/*
|
||||
* Copyright (c) 2025 Croxel Inc.
|
||||
* Copyright (c) 2025 CogniPilot Foundation
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include <platform/argus_print.h>
|
||||
#include <api/argus_status.h>
|
||||
#include <zephyr/kernel.h>
|
||||
|
||||
status_t print(const char *fmt_s, ...)
|
||||
{
|
||||
va_list args;
|
||||
|
||||
va_start(args, fmt_s);
|
||||
vprintk(fmt_s, args);
|
||||
va_end(args);
|
||||
|
||||
return STATUS_OK;
|
||||
}
|
450
modules/hal_afbr/platform_s2pi.c
Normal file
450
modules/hal_afbr/platform_s2pi.c
Normal file
|
@ -0,0 +1,450 @@
|
|||
/*
|
||||
* Copyright (c) 2025 Croxel Inc.
|
||||
* Copyright (c) 2025 CogniPilot Foundation
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include <zephyr/sys/check.h>
|
||||
#include <zephyr/drivers/gpio.h>
|
||||
#include <zephyr/drivers/pinctrl.h>
|
||||
#include <zephyr/rtio/rtio.h>
|
||||
|
||||
#include <platform/argus_s2pi.h>
|
||||
#include <platform/argus_irq.h>
|
||||
#include <api/argus_status.h>
|
||||
|
||||
/** Used to get instance data from slave index */
|
||||
#include <../drivers/sensor/broadcom/afbr_s50/platform.h>
|
||||
|
||||
#include <zephyr/logging/log.h>
|
||||
LOG_MODULE_REGISTER(afbr_s2pi, CONFIG_SENSOR_LOG_LEVEL);
|
||||
|
||||
status_t S2PI_GetStatus(s2pi_slave_t slave)
|
||||
{
|
||||
struct afbr_s50_platform_data *data;
|
||||
int err;
|
||||
|
||||
err = afbr_s50_platform_get_by_id(slave, &data);
|
||||
CHECKIF(err) {
|
||||
return ERROR_FAIL;
|
||||
}
|
||||
|
||||
return atomic_get(&data->s2pi.rtio.state);
|
||||
}
|
||||
|
||||
/** This basic implementation assumes the AFBR is alone in the bus, hence no
|
||||
* need to get a mutex. If more AFBR's are lumped together in a bus, these
|
||||
* two APIs need an implementation.
|
||||
*/
|
||||
status_t S2PI_TryGetMutex(s2pi_slave_t slave)
|
||||
{
|
||||
return STATUS_OK;
|
||||
}
|
||||
|
||||
void S2PI_ReleaseMutex(s2pi_slave_t slave)
|
||||
{
|
||||
/* See comment above. */
|
||||
}
|
||||
|
||||
static void S2PI_complete_callback(struct rtio *ctx,
|
||||
const struct rtio_sqe *sqe,
|
||||
void *arg)
|
||||
{
|
||||
struct afbr_s50_platform_data *data = (struct afbr_s50_platform_data *)arg;
|
||||
struct rtio_cqe *cqe;
|
||||
int err = 0;
|
||||
status_t status = STATUS_OK;
|
||||
|
||||
do {
|
||||
cqe = rtio_cqe_consume(ctx);
|
||||
if (cqe != NULL) {
|
||||
err = err ? err : cqe->result;
|
||||
rtio_cqe_release(ctx, cqe);
|
||||
}
|
||||
} while (cqe != NULL);
|
||||
|
||||
/** If the transfer was aborted, skip notifying users */
|
||||
if (atomic_cas(&data->s2pi.rtio.state, STATUS_BUSY, STATUS_IDLE)) {
|
||||
status = STATUS_OK;
|
||||
} else if (atomic_cas(&data->s2pi.rtio.state, ERROR_ABORTED, STATUS_IDLE)) {
|
||||
status = ERROR_ABORTED;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
|
||||
if (err) {
|
||||
status = ERROR_FAIL;
|
||||
}
|
||||
|
||||
if (data->s2pi.rtio.callback.handler) {
|
||||
(void)data->s2pi.rtio.callback.handler(status, data->s2pi.rtio.callback.data);
|
||||
}
|
||||
}
|
||||
|
||||
status_t S2PI_TransferFrame(s2pi_slave_t slave,
|
||||
uint8_t const *txData,
|
||||
uint8_t *rxData,
|
||||
size_t frameSize,
|
||||
s2pi_callback_t callback,
|
||||
void *callbackData)
|
||||
{
|
||||
struct afbr_s50_platform_data *data;
|
||||
int err;
|
||||
|
||||
err = afbr_s50_platform_get_by_id(slave, &data);
|
||||
CHECKIF(err) {
|
||||
return ERROR_S2PI_INVALID_SLAVE;
|
||||
}
|
||||
|
||||
CHECKIF(atomic_get(&data->s2pi.mode) == STATUS_S2PI_GPIO_MODE) {
|
||||
LOG_ERR("S2PI is in GPIO MODE");
|
||||
return ERROR_S2PI_INVALID_STATE;
|
||||
}
|
||||
|
||||
/** Check if the SPI bus is busy */
|
||||
if (!atomic_cas(&data->s2pi.rtio.state, STATUS_IDLE, STATUS_BUSY)) {
|
||||
LOG_WRN("SPI bus is busy");
|
||||
return STATUS_BUSY;
|
||||
}
|
||||
|
||||
data->s2pi.rtio.callback.handler = callback;
|
||||
data->s2pi.rtio.callback.data = callbackData;
|
||||
|
||||
struct rtio *ctx = data->s2pi.rtio.ctx;
|
||||
struct rtio_iodev *iodev = data->s2pi.rtio.iodev;
|
||||
|
||||
struct rtio_sqe *xfer_sqe = rtio_sqe_acquire(ctx);
|
||||
struct rtio_sqe *cb_sqe = rtio_sqe_acquire(ctx);
|
||||
|
||||
CHECKIF(!xfer_sqe || !cb_sqe) {
|
||||
LOG_ERR("Failed to allocate SQE, please check RTIO queue "
|
||||
"configuration on afbr_s50.c");
|
||||
return ERROR_FAIL;
|
||||
}
|
||||
|
||||
if (rxData) {
|
||||
rtio_sqe_prep_transceive(xfer_sqe, iodev, RTIO_PRIO_HIGH,
|
||||
txData, rxData, frameSize, NULL);
|
||||
} else {
|
||||
rtio_sqe_prep_write(xfer_sqe, iodev, RTIO_PRIO_HIGH,
|
||||
txData, frameSize, NULL);
|
||||
}
|
||||
xfer_sqe->flags |= RTIO_SQE_CHAINED;
|
||||
|
||||
rtio_sqe_prep_callback_no_cqe(cb_sqe, S2PI_complete_callback, data, NULL);
|
||||
|
||||
rtio_submit(ctx, 0);
|
||||
|
||||
return STATUS_OK;
|
||||
}
|
||||
|
||||
status_t S2PI_Abort(s2pi_slave_t slave)
|
||||
{
|
||||
struct afbr_s50_platform_data *data;
|
||||
int err;
|
||||
|
||||
err = afbr_s50_platform_get_by_id(slave, &data);
|
||||
CHECKIF(err) {
|
||||
return ERROR_S2PI_INVALID_SLAVE;
|
||||
}
|
||||
|
||||
(void)atomic_set(&data->s2pi.rtio.state, ERROR_ABORTED);
|
||||
|
||||
S2PI_complete_callback(data->s2pi.rtio.ctx, NULL, data);
|
||||
|
||||
return STATUS_OK;
|
||||
}
|
||||
|
||||
status_t S2PI_SetIrqCallback(s2pi_slave_t slave,
|
||||
s2pi_irq_callback_t callback,
|
||||
void *callbackData)
|
||||
{
|
||||
struct afbr_s50_platform_data *data;
|
||||
int err;
|
||||
|
||||
err = afbr_s50_platform_get_by_id(slave, &data);
|
||||
CHECKIF(err) {
|
||||
return ERROR_S2PI_INVALID_SLAVE;
|
||||
}
|
||||
|
||||
data->s2pi.irq.handler = callback;
|
||||
data->s2pi.irq.data = callbackData;
|
||||
|
||||
if (data->s2pi.irq.handler) {
|
||||
err = gpio_pin_interrupt_configure_dt(data->s2pi.gpio.irq, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
CHECKIF(err) {
|
||||
LOG_ERR("Failed to enable IRQ GPIO: %d", err);
|
||||
return ERROR_FAIL;
|
||||
}
|
||||
} else {
|
||||
err = gpio_pin_interrupt_configure_dt(data->s2pi.gpio.irq, GPIO_INT_DISABLE);
|
||||
CHECKIF(err) {
|
||||
LOG_ERR("Failed to disable IRQ GPIO: %d", err);
|
||||
return ERROR_FAIL;
|
||||
}
|
||||
}
|
||||
|
||||
return STATUS_OK;
|
||||
}
|
||||
|
||||
uint32_t S2PI_ReadIrqPin(s2pi_slave_t slave)
|
||||
{
|
||||
struct afbr_s50_platform_data *data;
|
||||
int err;
|
||||
|
||||
err = afbr_s50_platform_get_by_id(slave, &data);
|
||||
CHECKIF(err) {
|
||||
LOG_ERR("Error getting platform data: %d", err);
|
||||
return 0;
|
||||
}
|
||||
|
||||
const struct gpio_dt_spec *irq = data->s2pi.gpio.irq;
|
||||
int value;
|
||||
|
||||
value = gpio_pin_get_dt(irq);
|
||||
CHECKIF(value < 0) {
|
||||
LOG_ERR("Error reading IRQ pin: %d", value);
|
||||
return 0;
|
||||
}
|
||||
|
||||
return !value;
|
||||
}
|
||||
|
||||
status_t S2PI_CycleCsPin(s2pi_slave_t slave)
|
||||
{
|
||||
struct afbr_s50_platform_data *data;
|
||||
int err;
|
||||
|
||||
err = afbr_s50_platform_get_by_id(slave, &data);
|
||||
CHECKIF(err) {
|
||||
LOG_ERR("Error getting platform data: %d", err);
|
||||
return ERROR_S2PI_INVALID_SLAVE;
|
||||
}
|
||||
|
||||
const struct gpio_dt_spec *cs = data->s2pi.gpio.spi.cs;
|
||||
|
||||
err = gpio_pin_set_dt(cs, 1);
|
||||
CHECKIF(err) {
|
||||
LOG_ERR("Error setting CS pin logical high: %d", err);
|
||||
return ERROR_FAIL;
|
||||
}
|
||||
|
||||
err = gpio_pin_set_dt(cs, 0);
|
||||
CHECKIF(err) {
|
||||
LOG_ERR("Error setting CS pin logical low: %d", err);
|
||||
return ERROR_FAIL;
|
||||
}
|
||||
|
||||
return STATUS_OK;
|
||||
}
|
||||
|
||||
status_t S2PI_CaptureGpioControl(s2pi_slave_t slave)
|
||||
{
|
||||
struct afbr_s50_platform_data *data;
|
||||
int err;
|
||||
|
||||
err = afbr_s50_platform_get_by_id(slave, &data);
|
||||
CHECKIF(err) {
|
||||
LOG_ERR("Error getting platform data: %d", err);
|
||||
return ERROR_S2PI_INVALID_SLAVE;
|
||||
}
|
||||
|
||||
err = pinctrl_apply_state(data->s2pi.pincfg, PINCTRL_STATE_PRIV_START);
|
||||
CHECKIF(err) {
|
||||
LOG_ERR("Error applying gpio pinctrl state: %d", err);
|
||||
return ERROR_FAIL;
|
||||
}
|
||||
|
||||
err = gpio_pin_configure_dt(data->s2pi.gpio.spi.miso, GPIO_INPUT | GPIO_PULL_UP);
|
||||
err |= gpio_pin_configure_dt(data->s2pi.gpio.spi.mosi, GPIO_OUTPUT);
|
||||
err |= gpio_pin_configure_dt(data->s2pi.gpio.spi.clk, GPIO_OUTPUT);
|
||||
CHECKIF(err) {
|
||||
LOG_ERR("Error configuring GPIO pins: %d", err);
|
||||
return ERROR_FAIL;
|
||||
}
|
||||
|
||||
(void)atomic_set(&data->s2pi.mode, STATUS_S2PI_GPIO_MODE);
|
||||
|
||||
return STATUS_OK;
|
||||
}
|
||||
|
||||
status_t S2PI_ReleaseGpioControl(s2pi_slave_t slave)
|
||||
{
|
||||
struct afbr_s50_platform_data *data;
|
||||
int err;
|
||||
|
||||
err = afbr_s50_platform_get_by_id(slave, &data);
|
||||
CHECKIF(err) {
|
||||
LOG_ERR("Error getting platform data: %d", err);
|
||||
return ERROR_S2PI_INVALID_SLAVE;
|
||||
}
|
||||
|
||||
(void)gpio_pin_configure_dt(data->s2pi.gpio.spi.miso, GPIO_DISCONNECTED);
|
||||
(void)gpio_pin_configure_dt(data->s2pi.gpio.spi.mosi, GPIO_DISCONNECTED);
|
||||
(void)gpio_pin_configure_dt(data->s2pi.gpio.spi.clk, GPIO_DISCONNECTED);
|
||||
|
||||
err = pinctrl_apply_state(data->s2pi.pincfg, PINCTRL_STATE_DEFAULT);
|
||||
CHECKIF(err) {
|
||||
LOG_ERR("Error applying default pinctrl state: %d", err);
|
||||
return ERROR_FAIL;
|
||||
}
|
||||
|
||||
(void)atomic_set(&data->s2pi.mode, STATUS_IDLE);
|
||||
|
||||
return STATUS_OK;
|
||||
}
|
||||
|
||||
status_t S2PI_WriteGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t value)
|
||||
{
|
||||
struct afbr_s50_platform_data *data;
|
||||
const struct gpio_dt_spec *gpio_pin;
|
||||
int err;
|
||||
|
||||
err = afbr_s50_platform_get_by_id(slave, &data);
|
||||
CHECKIF(err) {
|
||||
LOG_ERR("Error getting platform data: %d", err);
|
||||
return ERROR_S2PI_INVALID_SLAVE;
|
||||
}
|
||||
|
||||
CHECKIF(!(atomic_get(&data->s2pi.mode) == STATUS_S2PI_GPIO_MODE)) {
|
||||
LOG_ERR("S2PI is in SPI MODE");
|
||||
return ERROR_S2PI_INVALID_STATE;
|
||||
}
|
||||
|
||||
switch (pin) {
|
||||
case S2PI_CS:
|
||||
gpio_pin = data->s2pi.gpio.spi.cs;
|
||||
break;
|
||||
case S2PI_CLK:
|
||||
gpio_pin = data->s2pi.gpio.spi.clk;
|
||||
break;
|
||||
case S2PI_MOSI:
|
||||
gpio_pin = data->s2pi.gpio.spi.mosi;
|
||||
break;
|
||||
case S2PI_MISO:
|
||||
gpio_pin = data->s2pi.gpio.spi.miso;
|
||||
break;
|
||||
default:
|
||||
CHECKIF(true) {
|
||||
__ASSERT(false, "Not implemented for pin: %d", pin);
|
||||
return ERROR_FAIL;
|
||||
}
|
||||
}
|
||||
|
||||
err = gpio_pin_set_raw(gpio_pin->port, gpio_pin->pin, value);
|
||||
CHECKIF(err) {
|
||||
LOG_ERR("Error setting GPIO pin: %d", err);
|
||||
return ERROR_FAIL;
|
||||
}
|
||||
|
||||
/* Delay suggested by API documentation of S2PI_WriteGpioPin */
|
||||
k_sleep(K_USEC(10));
|
||||
|
||||
return STATUS_OK;
|
||||
}
|
||||
|
||||
status_t S2PI_ReadGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t *value)
|
||||
{
|
||||
struct afbr_s50_platform_data *data;
|
||||
const struct gpio_dt_spec *gpio_pin;
|
||||
int err;
|
||||
int pin_value;
|
||||
|
||||
err = afbr_s50_platform_get_by_id(slave, &data);
|
||||
CHECKIF(err) {
|
||||
LOG_ERR("Error getting platform data: %d", err);
|
||||
return ERROR_S2PI_INVALID_SLAVE;
|
||||
}
|
||||
|
||||
CHECKIF(!(atomic_get(&data->s2pi.mode) == STATUS_S2PI_GPIO_MODE)) {
|
||||
LOG_ERR("S2PI is in SPI MODE");
|
||||
return ERROR_S2PI_INVALID_STATE;
|
||||
}
|
||||
|
||||
switch (pin) {
|
||||
case S2PI_CS:
|
||||
gpio_pin = data->s2pi.gpio.spi.cs;
|
||||
break;
|
||||
case S2PI_CLK:
|
||||
gpio_pin = data->s2pi.gpio.spi.clk;
|
||||
break;
|
||||
case S2PI_MOSI:
|
||||
gpio_pin = data->s2pi.gpio.spi.mosi;
|
||||
break;
|
||||
case S2PI_MISO:
|
||||
gpio_pin = data->s2pi.gpio.spi.miso;
|
||||
break;
|
||||
default:
|
||||
CHECKIF(true) {
|
||||
__ASSERT(false, "Not implemented for pin: %d", pin);
|
||||
return ERROR_FAIL;
|
||||
}
|
||||
}
|
||||
|
||||
pin_value = gpio_pin_get_raw(gpio_pin->port, gpio_pin->pin);
|
||||
*value = (uint32_t)pin_value;
|
||||
|
||||
return STATUS_OK;
|
||||
}
|
||||
|
||||
static void s2pi_irq_gpio_callback(const struct device *dev,
|
||||
struct gpio_callback *cb,
|
||||
uint32_t pins)
|
||||
{
|
||||
struct afbr_s50_platform_data *data = CONTAINER_OF(cb,
|
||||
struct afbr_s50_platform_data,
|
||||
s2pi.irq.cb);
|
||||
|
||||
if (data->s2pi.irq.handler) {
|
||||
data->s2pi.irq.handler(data->s2pi.irq.data);
|
||||
}
|
||||
}
|
||||
|
||||
static int s2pi_init(struct afbr_s50_platform_data *data)
|
||||
{
|
||||
int err;
|
||||
|
||||
(void)atomic_set(&data->s2pi.rtio.state, STATUS_IDLE);
|
||||
(void)atomic_set(&data->s2pi.mode, STATUS_IDLE);
|
||||
|
||||
CHECKIF(!data->s2pi.gpio.irq || !data->s2pi.gpio.spi.cs) {
|
||||
LOG_ERR("GPIOs not supplied");
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
if (!gpio_is_ready_dt(data->s2pi.gpio.irq)) {
|
||||
LOG_ERR("IRQ GPIO not ready");
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
err = gpio_pin_configure_dt(data->s2pi.gpio.irq, GPIO_INPUT);
|
||||
CHECKIF(err) {
|
||||
LOG_ERR("Failed to configure IRQ GPIO");
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
gpio_init_callback(&data->s2pi.irq.cb, s2pi_irq_gpio_callback,
|
||||
BIT(data->s2pi.gpio.irq->pin));
|
||||
|
||||
err = gpio_add_callback(data->s2pi.gpio.irq->port, &data->s2pi.irq.cb);
|
||||
CHECKIF(err) {
|
||||
LOG_ERR("Failed to add IRQ callback");
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static struct afbr_s50_platform_init_node s2pi_init_node = {
|
||||
.init_fn = s2pi_init,
|
||||
};
|
||||
|
||||
static int s2pi_platform_init_hooks(void)
|
||||
{
|
||||
afbr_s50_platform_init_hooks_add(&s2pi_init_node);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
SYS_INIT(s2pi_platform_init_hooks, POST_KERNEL, 0);
|
102
modules/hal_afbr/platform_timer.c
Normal file
102
modules/hal_afbr/platform_timer.c
Normal file
|
@ -0,0 +1,102 @@
|
|||
/*
|
||||
* Copyright (c) 2025 Croxel Inc.
|
||||
* Copyright (c) 2025 CogniPilot Foundation
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include <platform/argus_timer.h>
|
||||
#include <api/argus_status.h>
|
||||
|
||||
#include <zephyr/kernel.h>
|
||||
|
||||
#include <../drivers/sensor/broadcom/afbr_s50/platform.h>
|
||||
|
||||
struct platform_argus_timer {
|
||||
struct k_timer *timer;
|
||||
uint32_t dt_microseconds;
|
||||
struct {
|
||||
timer_cb_t handler;
|
||||
void *param;
|
||||
} cb;
|
||||
};
|
||||
|
||||
static void argus_timer_handler(struct k_timer *timer);
|
||||
|
||||
K_TIMER_DEFINE(argus_timer, argus_timer_handler, NULL);
|
||||
|
||||
static struct platform_argus_timer platform_timer = {
|
||||
.timer = &argus_timer,
|
||||
};
|
||||
|
||||
static void argus_timer_handler(struct k_timer *timer)
|
||||
{
|
||||
struct platform_argus_timer *p_timer = &platform_timer;
|
||||
timer_cb_t handler = p_timer->cb.handler;
|
||||
|
||||
if (handler) {
|
||||
handler(p_timer->cb.param);
|
||||
}
|
||||
}
|
||||
|
||||
void Timer_GetCounterValue(uint32_t *hct, uint32_t *lct)
|
||||
{
|
||||
int64_t uptime_ticks = k_uptime_ticks();
|
||||
uint64_t uptime_us = k_ticks_to_us_floor64((uint64_t)uptime_ticks);
|
||||
|
||||
/* hct in seconds, lct in microseconds */
|
||||
*hct = uptime_us / USEC_PER_SEC;
|
||||
*lct = uptime_us % USEC_PER_SEC;
|
||||
}
|
||||
|
||||
status_t Timer_SetCallback(timer_cb_t f)
|
||||
{
|
||||
platform_timer.cb.handler = f;
|
||||
|
||||
return STATUS_OK;
|
||||
}
|
||||
|
||||
/** The API description talks about multiple timers going on at once,
|
||||
* distinguished by the parameter passed on, however there does not appear to
|
||||
* be a mention on what's the requirement, neither do the other platform
|
||||
* implementation in the upstream SDK follow a multi-timer pattern.
|
||||
* For starters, this implementation just covers single-timer use case.
|
||||
*/
|
||||
status_t Timer_SetInterval(uint32_t dt_microseconds, void *param)
|
||||
{
|
||||
if (dt_microseconds == 0) {
|
||||
k_timer_stop(platform_timer.timer);
|
||||
platform_timer.dt_microseconds = 0;
|
||||
platform_timer.cb.param = NULL;
|
||||
} else if (dt_microseconds != platform_timer.dt_microseconds) {
|
||||
platform_timer.dt_microseconds = dt_microseconds;
|
||||
platform_timer.cb.param = param;
|
||||
k_timer_start(platform_timer.timer,
|
||||
K_USEC(platform_timer.dt_microseconds),
|
||||
K_USEC(platform_timer.dt_microseconds));
|
||||
} else {
|
||||
platform_timer.cb.param = param;
|
||||
}
|
||||
|
||||
return STATUS_OK;
|
||||
}
|
||||
|
||||
static int timer_init(struct afbr_s50_platform_data *data)
|
||||
{
|
||||
ARG_UNUSED(data);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static struct afbr_s50_platform_init_node timer_init_node = {
|
||||
.init_fn = timer_init,
|
||||
};
|
||||
|
||||
static int timer_platform_init_hooks(void)
|
||||
{
|
||||
afbr_s50_platform_init_hooks_add(&timer_init_node);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
SYS_INIT(timer_platform_init_hooks, POST_KERNEL, 0);
|
5
west.yml
5
west.yml
|
@ -148,6 +148,11 @@ manifest:
|
|||
path: modules/hal/adi
|
||||
groups:
|
||||
- hal
|
||||
- name: hal_afbr
|
||||
revision: 4e1eea7ea283db9d9ce529b0e9f89c0b5c2660e3
|
||||
path: modules/hal/afbr
|
||||
groups:
|
||||
- hal
|
||||
- name: hal_ambiq
|
||||
revision: f46941f3427bbc05d893a601660e6e3cffe9e29d
|
||||
path: modules/hal/ambiq
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue