forked from Imagelibrary/rtems
Revert "cpukit/dev/can: Added CAN support"
This reverts commit cd91b37dce.
Closes #4803.
This commit is contained in:
@@ -1,505 +0,0 @@
|
||||
/* SPDX-License-Identifier: BSD-2-Clause */
|
||||
|
||||
/**
|
||||
* @file
|
||||
*
|
||||
* @ingroup CANBus
|
||||
*
|
||||
* @brief Controller Area Network (CAN) Bus Implementation
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (C) 2022 Prashanth S (fishesprashanth@gmail.com)
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <rtems/imfs.h>
|
||||
#include <rtems/thread.h>
|
||||
|
||||
#include <dev/can/canqueueimpl.h>
|
||||
#include <dev/can/can.h>
|
||||
|
||||
#define can_interrupt_lock_acquire(bus) \
|
||||
do { \
|
||||
CAN_DEBUG_LOCK("acquiring lock\n"); \
|
||||
real_can_interrupt_lock_acquire(bus); \
|
||||
} while (0);
|
||||
|
||||
#define can_interrupt_lock_release(bus) \
|
||||
do { \
|
||||
CAN_DEBUG_LOCK("releasing lock\n"); \
|
||||
real_can_interrupt_lock_release(bus); \
|
||||
} while (0);
|
||||
|
||||
static ssize_t
|
||||
can_bus_open(rtems_libio_t *iop, const char *path, int oflag, mode_t mode);
|
||||
static ssize_t
|
||||
can_bus_read(rtems_libio_t *iop, void *buffer, size_t count);
|
||||
static ssize_t
|
||||
can_bus_write(rtems_libio_t *iop, const void *buffer, size_t count);
|
||||
static ssize_t
|
||||
can_bus_ioctl(rtems_libio_t *iop, ioctl_command_t request, void *buffer);
|
||||
|
||||
static int can_xmit(struct can_bus *bus);
|
||||
|
||||
static int can_bus_create_sem(struct can_bus *);
|
||||
static int try_sem(struct can_bus *);
|
||||
static int take_sem(struct can_bus *);
|
||||
static int give_sem(struct can_bus *);
|
||||
|
||||
|
||||
static void can_bus_obtain(can_bus *bus)
|
||||
{
|
||||
CAN_DEBUG("can_bus_obtain Entry\n");
|
||||
rtems_mutex_lock(&bus->mutex);
|
||||
CAN_DEBUG("can_bus_obtain Exit\n");
|
||||
}
|
||||
|
||||
static void can_bus_release(can_bus *bus)
|
||||
{
|
||||
CAN_DEBUG("can_bus_release Entry\n");
|
||||
rtems_mutex_unlock(&bus->mutex);
|
||||
CAN_DEBUG("can_bus_release Exit\n");
|
||||
}
|
||||
|
||||
static void can_bus_destroy_mutex(struct can_bus *bus)
|
||||
{
|
||||
rtems_mutex_destroy(&bus->mutex);
|
||||
}
|
||||
|
||||
static int can_bus_create_sem(struct can_bus *bus)
|
||||
{
|
||||
int ret = 0;
|
||||
|
||||
ret = rtems_semaphore_create(rtems_build_name('c', 'a', 'n', bus->index),
|
||||
CAN_TX_BUF_COUNT, RTEMS_FIFO | RTEMS_COUNTING_SEMAPHORE | RTEMS_LOCAL,
|
||||
0, &bus->tx_fifo_sem_id);
|
||||
|
||||
if (ret != 0) {
|
||||
CAN_ERR("can_create_sem: rtems_semaphore_create failed %d\n", ret);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static void can_bus_free_tx_semaphore(struct can_bus *bus)
|
||||
{
|
||||
rtems_semaphore_delete(bus->tx_fifo_sem_id);
|
||||
}
|
||||
|
||||
static void real_can_interrupt_lock_acquire(struct can_bus *bus)
|
||||
{
|
||||
bus->can_dev_ops->dev_int(bus->priv, false);
|
||||
can_bus_obtain(bus);
|
||||
}
|
||||
|
||||
static void real_can_interrupt_lock_release(struct can_bus *bus)
|
||||
{
|
||||
can_bus_release(bus);
|
||||
bus->can_dev_ops->dev_int(bus->priv, true);
|
||||
}
|
||||
|
||||
static int take_sem(struct can_bus *bus)
|
||||
{
|
||||
int ret = rtems_semaphore_obtain(bus->tx_fifo_sem_id, RTEMS_WAIT,
|
||||
RTEMS_NO_TIMEOUT);
|
||||
#ifdef CAN_DEBUG_LOCK
|
||||
if (ret == RTEMS_SUCCESSFUL) {
|
||||
bus->sem_count++;
|
||||
CAN_DEBUG_LOCK("take_sem: Semaphore count = %d\n", bus->sem_count);
|
||||
if (bus->sem_count > CAN_TX_BUF_COUNT) {
|
||||
CAN_ERR("take_sem error: sem_count is misleading\n");
|
||||
return RTEMS_INTERNAL_ERROR;
|
||||
}
|
||||
}
|
||||
#endif /* CAN_DEBUG_LOCK */
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static int give_sem(struct can_bus *bus)
|
||||
{
|
||||
int ret = rtems_semaphore_release(bus->tx_fifo_sem_id);
|
||||
|
||||
#ifdef CAN_DEBUG_LOCK
|
||||
if (ret == RTEMS_SUCCESSFUL) {
|
||||
bus->sem_count--;
|
||||
CAN_DEBUG_LOCK("give_sem: Semaphore count = %d\n", bus->sem_count);
|
||||
if (bus->sem_count < 0) {
|
||||
CAN_ERR("give_sem error: sem_count is misleading\n");
|
||||
return RTEMS_INTERNAL_ERROR;
|
||||
}
|
||||
}
|
||||
#endif /* CAN_DEBUG_LOCK */
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static int try_sem(struct can_bus *bus)
|
||||
{
|
||||
int ret = rtems_semaphore_obtain(bus->tx_fifo_sem_id, RTEMS_NO_WAIT,
|
||||
RTEMS_NO_TIMEOUT);
|
||||
#ifdef CAN_DEBUG_LOCK
|
||||
if (ret == RTEMS_SUCCESSFUL) {
|
||||
bus->sem_count++;
|
||||
CAN_DEBUG_LOCK("try_sem: Semaphore count = %d\n", bus->sem_count);
|
||||
if (bus->sem_count > CAN_TX_BUF_COUNT) {
|
||||
CAN_ERR("take_sem error: sem_count is misleading\n");
|
||||
return RTEMS_INTERNAL_ERROR;
|
||||
}
|
||||
}
|
||||
#endif /* CAN_DEBUG_LOCK */
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static ssize_t
|
||||
can_bus_open(rtems_libio_t *iop, const char *path, int oflag, mode_t mode)
|
||||
{
|
||||
CAN_DEBUG("can_bus_open\n");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* Should be called only with CAN interrupts disabled */
|
||||
int can_receive(struct can_bus *bus, struct can_msg *msg)
|
||||
{
|
||||
memcpy(&bus->can_rx_msg, msg, CAN_MSG_LEN(msg));
|
||||
|
||||
return CAN_MSG_LEN(msg);
|
||||
}
|
||||
|
||||
/* FIXME: Should be modified to read count number of bytes, Now sending one can_msg */
|
||||
static ssize_t can_bus_read(rtems_libio_t *iop, void *buffer, size_t count)
|
||||
{
|
||||
int len;
|
||||
|
||||
can_bus *bus = IMFS_generic_get_context_by_iop(iop);
|
||||
if (bus == NULL) {
|
||||
return -RTEMS_NOT_DEFINED;
|
||||
}
|
||||
|
||||
struct can_msg *msg = (struct can_msg *)buffer;
|
||||
|
||||
len = CAN_MSG_LEN(&bus->can_rx_msg);
|
||||
|
||||
if (count < len) {
|
||||
CAN_DEBUG("can_bus_read: buffer size is small min sizeof(struct can_msg) = %u\n",
|
||||
sizeof(struct can_msg));
|
||||
return -RTEMS_INVALID_SIZE;
|
||||
}
|
||||
|
||||
can_interrupt_lock_acquire(bus);
|
||||
memcpy(msg, &bus->can_rx_msg, len);
|
||||
can_interrupt_lock_release(bus);
|
||||
|
||||
return len;
|
||||
}
|
||||
|
||||
/* This function is a critical section and should be called
|
||||
* with CAN interrupts disabled and mutually exclusive
|
||||
*/
|
||||
static int can_xmit(struct can_bus *bus)
|
||||
{
|
||||
int ret = RTEMS_SUCCESSFUL;
|
||||
|
||||
struct can_msg *msg = NULL;
|
||||
|
||||
CAN_DEBUG(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> can_xmit Entry\n");
|
||||
|
||||
while (1) {
|
||||
CAN_DEBUG("can_dev_ops->dev_tx_ready\n");
|
||||
if (bus->can_dev_ops->dev_tx_ready(bus->priv) != true) {
|
||||
break;
|
||||
}
|
||||
|
||||
CAN_DEBUG("can_tx_get_data_buf\n");
|
||||
msg = can_bus_tx_get_data_buf(bus);
|
||||
if (msg == NULL) {
|
||||
break;
|
||||
}
|
||||
|
||||
CAN_DEBUG("can_dev_ops->dev_tx\n");
|
||||
ret = bus->can_dev_ops->dev_tx(bus->priv, msg);
|
||||
if (ret != RTEMS_SUCCESSFUL) {
|
||||
CAN_ERR("can_xmit: dev_send failed\n");
|
||||
break;
|
||||
}
|
||||
|
||||
ret = give_sem(bus);
|
||||
if (ret != RTEMS_SUCCESSFUL) {
|
||||
CAN_ERR("can_xmit: rtems_semaphore_release failed = %d\n", ret);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
CAN_DEBUG(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> can_xmit Exit\n");
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void can_print_msg(struct can_msg const *msg)
|
||||
{
|
||||
#ifdef CAN_DEBUG
|
||||
CAN_DEBUG("\n----------------------------------------------------------------\n");
|
||||
CAN_DEBUG("id = %d len = %d flags = 0x%08X\n", msg->id, msg->len, msg->flags);
|
||||
|
||||
CAN_DEBUG("msg->data[0] = 0x%08x\n", ((uint32_t *)msg->data)[0]);
|
||||
CAN_DEBUG("msg->data[1] = 0x%08x\n", ((uint32_t *)msg->data)[1]);
|
||||
|
||||
for (int i = 0; i < msg->len; i++) {
|
||||
CAN_DEBUG("%02x\n", ((char *)msg->data)[i]);
|
||||
}
|
||||
|
||||
CAN_DEBUG("\n-----------------------------------------------------------------\n");
|
||||
#endif /* CAN_DEBUG */
|
||||
}
|
||||
|
||||
/* can_tx_done should be called only with CAN interrupts disabled */
|
||||
int can_tx_done(struct can_bus *bus)
|
||||
{
|
||||
CAN_DEBUG("------------ can_tx_done Entry\n");
|
||||
int ret = RTEMS_SUCCESSFUL;
|
||||
|
||||
if (bus->can_dev_ops->dev_tx_ready(bus->priv) == true) {
|
||||
ret = can_xmit(bus);
|
||||
}
|
||||
CAN_DEBUG("------------ can_tx_done Exit\n");
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* FIXME: Add support to send multiple CAN msgs for can_bus_write */
|
||||
static ssize_t
|
||||
can_bus_write(rtems_libio_t *iop, const void *buffer, size_t count)
|
||||
{
|
||||
can_bus *bus = IMFS_generic_get_context_by_iop(iop);
|
||||
|
||||
if (bus == NULL || bus->can_dev_ops->dev_tx == NULL) {
|
||||
return -RTEMS_NOT_DEFINED;
|
||||
}
|
||||
|
||||
int ret = RTEMS_SUCCESSFUL;
|
||||
|
||||
struct can_msg const *msg = buffer;
|
||||
struct can_msg *fifo_buf = NULL;
|
||||
|
||||
uint32_t msg_size = CAN_MSG_LEN(msg);
|
||||
|
||||
CAN_DEBUG_TX("can_bus_write: can_msg_size = %u\n", msg_size);
|
||||
if (msg_size > sizeof(struct can_msg)) {
|
||||
CAN_ERR("can_bus_write:"
|
||||
"can message len error msg_size = %u struct can_msg = %u\n",
|
||||
msg_size, sizeof(struct can_msg));
|
||||
return -RTEMS_INVALID_SIZE;
|
||||
}
|
||||
|
||||
if ((iop->flags & O_NONBLOCK) != 0) {
|
||||
ret = try_sem(bus);
|
||||
if (ret != RTEMS_SUCCESSFUL) {
|
||||
return -ret;
|
||||
}
|
||||
} else {
|
||||
ret = take_sem(bus);
|
||||
if (ret != RTEMS_SUCCESSFUL) {
|
||||
CAN_ERR("can_bus_write: cannot take semaphore\n");
|
||||
return -ret;
|
||||
}
|
||||
}
|
||||
|
||||
can_interrupt_lock_acquire(bus);
|
||||
|
||||
fifo_buf = can_bus_tx_get_empty_buf(bus);
|
||||
if (fifo_buf == NULL) {
|
||||
/* This error will not happen until buffer counts are not synchronized */
|
||||
CAN_ERR("can_bus_write: Buffer counts are not synchronized with semaphore count\n");
|
||||
ret = -RTEMS_INTERNAL_ERROR;
|
||||
goto release_lock_and_return;
|
||||
}
|
||||
|
||||
CAN_DEBUG_TX("can_bus_write: empty_count = %u\n", bus->tx_fifo.empty_count);
|
||||
|
||||
CAN_DEBUG_TX("can_bus_write: copying msg from application to driver buffer\n");
|
||||
memcpy(fifo_buf, msg, msg_size);
|
||||
|
||||
if (bus->can_dev_ops->dev_tx_ready(bus->priv) == true) {
|
||||
ret = can_xmit(bus);
|
||||
if (ret != RTEMS_SUCCESSFUL) {
|
||||
ret = -ret;
|
||||
goto release_lock_and_return;
|
||||
}
|
||||
}
|
||||
|
||||
ret = msg_size;
|
||||
|
||||
release_lock_and_return:
|
||||
can_interrupt_lock_release(bus);
|
||||
return ret;
|
||||
}
|
||||
|
||||
static ssize_t
|
||||
can_bus_ioctl(rtems_libio_t *iop, ioctl_command_t request, void *buffer)
|
||||
{
|
||||
can_bus *bus = IMFS_generic_get_context_by_iop(iop);
|
||||
|
||||
if (bus == NULL || bus->can_dev_ops->dev_ioctl == NULL) {
|
||||
return -RTEMS_NOT_DEFINED;
|
||||
}
|
||||
|
||||
can_bus_obtain(bus);
|
||||
|
||||
bus->can_dev_ops->dev_ioctl(bus->priv, NULL, 0);
|
||||
|
||||
can_bus_release(bus);
|
||||
|
||||
return RTEMS_SUCCESSFUL;
|
||||
}
|
||||
|
||||
static const rtems_filesystem_file_handlers_r can_bus_handler = {
|
||||
.open_h = can_bus_open,
|
||||
.close_h = rtems_filesystem_default_close,
|
||||
.read_h = can_bus_read,
|
||||
.write_h = can_bus_write,
|
||||
.ioctl_h = can_bus_ioctl,
|
||||
.lseek_h = rtems_filesystem_default_lseek,
|
||||
.fstat_h = IMFS_stat,
|
||||
.ftruncate_h = rtems_filesystem_default_ftruncate,
|
||||
.fsync_h = rtems_filesystem_default_fsync_or_fdatasync,
|
||||
.fdatasync_h = rtems_filesystem_default_fsync_or_fdatasync,
|
||||
.fcntl_h = rtems_filesystem_default_fcntl,
|
||||
.kqfilter_h = rtems_filesystem_default_kqfilter,
|
||||
.mmap_h = rtems_filesystem_default_mmap,
|
||||
.poll_h = rtems_filesystem_default_poll,
|
||||
.readv_h = rtems_filesystem_default_readv,
|
||||
.writev_h = rtems_filesystem_default_writev
|
||||
};
|
||||
|
||||
static void can_bus_node_destroy(IMFS_jnode_t *node)
|
||||
{
|
||||
can_bus *bus;
|
||||
|
||||
bus = IMFS_generic_get_context_by_node(node);
|
||||
(*bus->destroy)(bus);
|
||||
|
||||
IMFS_node_destroy_default(node);
|
||||
}
|
||||
|
||||
static const
|
||||
IMFS_node_control can_bus_node_control = IMFS_GENERIC_INITIALIZER(&can_bus_handler,
|
||||
IMFS_node_initialize_generic, can_bus_node_destroy);
|
||||
|
||||
rtems_status_code can_bus_register(can_bus *bus, const char *bus_path)
|
||||
{
|
||||
int ret = RTEMS_SUCCESSFUL;
|
||||
|
||||
static uint8_t index = 0;
|
||||
|
||||
if (index == CAN_BUS_REG_MAX) {
|
||||
CAN_ERR("can_bus_register: can bus registration limit reached\n");
|
||||
return RTEMS_TOO_MANY;
|
||||
}
|
||||
|
||||
bus->index = index++;
|
||||
CAN_DEBUG("Registering CAN bus index = %u\n", bus->index);
|
||||
|
||||
ret = IMFS_make_generic_node(bus_path, S_IFCHR | S_IRWXU | S_IRWXG | S_IRWXO,
|
||||
&can_bus_node_control, bus);
|
||||
if (ret != RTEMS_SUCCESSFUL) {
|
||||
CAN_ERR("can_bus_register: Creating node failed = %d\n", ret);
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if ((ret = can_bus_create_sem(bus)) != RTEMS_SUCCESSFUL) {
|
||||
CAN_ERR("can_bus_register: can_create_sem failed = %d\n", ret);
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if ((ret = can_bus_create_tx_buffers(bus)) != RTEMS_SUCCESSFUL) {
|
||||
CAN_ERR("can_bus_register: can_create_tx_buffers failed = %d\n", ret);
|
||||
goto free_tx_semaphore;
|
||||
}
|
||||
|
||||
return ret;
|
||||
|
||||
free_tx_semaphore:
|
||||
rtems_semaphore_delete(bus->tx_fifo_sem_id);
|
||||
|
||||
fail:
|
||||
can_bus_destroy_mutex(bus);
|
||||
return ret;
|
||||
|
||||
}
|
||||
|
||||
static void can_bus_destroy(can_bus *bus)
|
||||
{
|
||||
can_bus_free_tx_buffers(bus);
|
||||
can_bus_free_tx_semaphore(bus);
|
||||
can_bus_destroy_mutex(bus);
|
||||
}
|
||||
|
||||
static int can_bus_do_init(can_bus *bus, void (*destroy)(can_bus *bus))
|
||||
{
|
||||
rtems_mutex_init(&bus->mutex, "CAN Bus");
|
||||
bus->destroy = can_bus_destroy;
|
||||
|
||||
return RTEMS_SUCCESSFUL;
|
||||
}
|
||||
|
||||
static void can_bus_destroy_and_free(can_bus *bus)
|
||||
{
|
||||
can_bus_destroy(bus);
|
||||
free(bus);
|
||||
}
|
||||
|
||||
int can_bus_init(can_bus *bus)
|
||||
{
|
||||
memset(bus, 0, sizeof(*bus));
|
||||
|
||||
return can_bus_do_init(bus, can_bus_destroy);
|
||||
}
|
||||
|
||||
can_bus *can_bus_alloc_and_init(size_t size)
|
||||
{
|
||||
can_bus *bus = NULL;
|
||||
|
||||
if (size >= sizeof(*bus)) {
|
||||
bus = calloc(1, size);
|
||||
if (bus == NULL) {
|
||||
CAN_ERR("can_bus_alloc_and_init: calloc failed\b");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
int rv = can_bus_do_init(bus, can_bus_destroy_and_free);
|
||||
if (rv != 0) {
|
||||
CAN_ERR("can_bus_alloc_and_init: can_bus_do_init failed\n");
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
return bus;
|
||||
}
|
||||
@@ -1,105 +0,0 @@
|
||||
/* SPDX-License-Identifier: BSD-2-Clause */
|
||||
|
||||
/**
|
||||
* @file
|
||||
*
|
||||
* @ingroup CANBus
|
||||
*
|
||||
* @brief Controller Area Network (CAN) Bus Implementation
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (C) 2022 Prashanth S <fishesprashanth@gmail.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef _DEV_CAN_CAN_MSG_H
|
||||
#define _DEV_CAN_CAN_MSG_H
|
||||
|
||||
/**
|
||||
* @defgroup Controller Area Network (CAN) Driver
|
||||
*
|
||||
* @ingroup RTEMSDeviceDrivers
|
||||
*
|
||||
* @brief Controller Area Network (CAN) bus and device driver support.
|
||||
*
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @defgroup CANBus CAN Bus Driver
|
||||
*
|
||||
* @ingroup CAN
|
||||
*
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief CAN message size
|
||||
*/
|
||||
#define CAN_MSG_MAX_SIZE (8u)
|
||||
|
||||
/**
|
||||
* @brief Number of CAN tx buffers
|
||||
*/
|
||||
#define CAN_TX_BUF_COUNT (10u)
|
||||
|
||||
/**
|
||||
* @brief Number of CAN rx buffers
|
||||
*/
|
||||
#define CAN_RX_BUF_COUNT (2u)
|
||||
|
||||
#define CAN_MSGLEN(msg) (sizeof(struct can_msg) - (CAN_MSG_MAX_SIZE - msg->len))
|
||||
|
||||
/**
|
||||
* @brief CAN message structure.
|
||||
*/
|
||||
struct can_msg {
|
||||
/**
|
||||
* @brief CAN message id.
|
||||
*/
|
||||
uint32_t id;
|
||||
/**
|
||||
* @brief CAN message timestamp.
|
||||
*/
|
||||
uint32_t timestamp;
|
||||
/**
|
||||
* @brief CAN message flags RTR | BRS | EXT.
|
||||
*/
|
||||
uint16_t flags;
|
||||
/**
|
||||
* @brief CAN message length.
|
||||
*/
|
||||
uint16_t len;
|
||||
/**
|
||||
* @brief CAN message.
|
||||
*/
|
||||
uint8_t data[CAN_MSG_MAX_SIZE];
|
||||
};
|
||||
|
||||
/** @} */ /* end of CAN message */
|
||||
|
||||
/** @} */
|
||||
|
||||
#endif /* _DEV_CAN_CAN_MSG_H */
|
||||
@@ -1,284 +0,0 @@
|
||||
/* SPDX-License-Identifier: BSD-2-Clause */
|
||||
|
||||
/**
|
||||
* @file
|
||||
*
|
||||
* @ingroup CANBus
|
||||
*
|
||||
* @brief Controller Area Network (CAN) Bus Implementation
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (C) 2022 Prashanth S (fishesprashanth@gmail.com)
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef _DEV_CAN_CAN_H
|
||||
#define _DEV_CAN_CAN_H
|
||||
|
||||
#include <rtems/imfs.h>
|
||||
#include <rtems/thread.h>
|
||||
#include <semaphore.h>
|
||||
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <dev/can/can-msg.h>
|
||||
|
||||
#define DEBUG(str, ...) \
|
||||
do { \
|
||||
printf("CAN: %s:%d ID: %08X ", __FILE__, __LINE__, rtems_task_self()); \
|
||||
printf(str, ##__VA_ARGS__); \
|
||||
} while (false);
|
||||
|
||||
#define CAN_DEBUG(str, ...) DEBUG(str, ##__VA_ARGS__)
|
||||
#define CAN_DEBUG_BUF(str, ...) CAN_DEBUG(str, ##__VA_ARGS__)
|
||||
#define CAN_DEBUG_ISR(str, ...) CAN_DEBUG(str, ##__VA_ARGS__)
|
||||
#define CAN_DEBUG_LOCK(str, ...) CAN_DEBUG(str, ##__VA_ARGS__)
|
||||
#define CAN_DEBUG_RX(str, ...) CAN_DEBUG(str, ##__VA_ARGS__)
|
||||
#define CAN_DEBUG_TX(str, ...) CAN_DEBUG(str, ##__VA_ARGS__)
|
||||
#define CAN_DEBUG_REG(str, ...) //CAN_DEBUG(str, ##__VA_ARGS__)
|
||||
#define CAN_ERR(str, ...) DEBUG(str, ##__VA_ARGS__)
|
||||
|
||||
#define CAN_MSG_LEN(msg) ((char *)(&((struct can_msg *)msg)->data[(uint16_t)((struct can_msg *)msg)->len]) - (char *)(msg))
|
||||
|
||||
/* Maximum Bus Reg (255) */
|
||||
#define CAN_BUS_REG_MAX (255)
|
||||
|
||||
/**
|
||||
* @defgroup Controller Area Network (CAN) Driver
|
||||
*
|
||||
* @ingroup RTEMSDeviceDrivers
|
||||
*
|
||||
* @brief Controller Area Network (CAN) bus and device driver support.
|
||||
*
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @defgroup CANBus CAN Bus Driver
|
||||
*
|
||||
* @ingroup CAN
|
||||
*
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief CAN tx fifo data structure.
|
||||
*/
|
||||
struct ring_buf {
|
||||
/**
|
||||
* @brief Pointer to array of can_msg structure.
|
||||
*/
|
||||
struct can_msg *pbuf;
|
||||
/**
|
||||
* @brief Index of the next free buffer.
|
||||
*/
|
||||
uint32_t head;
|
||||
/**
|
||||
* @brief Index of the produced buffer.
|
||||
*/
|
||||
uint32_t tail;
|
||||
/**
|
||||
* @brief Number of empty buffers.
|
||||
*/
|
||||
uint32_t empty_count;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief CAN Controller device specific operations.
|
||||
* These function pointers are initialized by the CAN device driver while
|
||||
* registering (can_bus_register).
|
||||
*/
|
||||
typedef struct can_dev_ops {
|
||||
/**
|
||||
* @brief Transfers CAN messages to device fifo.
|
||||
*
|
||||
* @param[in] priv device control structure.
|
||||
* @param[in] msg can_msg message structure.
|
||||
*
|
||||
* @retval 0 Successful operation.
|
||||
* @retval >0 error number in case of an error.
|
||||
*/
|
||||
int32_t (*dev_tx)(void *priv, struct can_msg *msg);
|
||||
/**
|
||||
* @brief Check device is ready to transfer a CAN message
|
||||
*
|
||||
* @param[in] priv device control structure.
|
||||
*
|
||||
* @retval true device ready.
|
||||
* @retval false device not ready.
|
||||
*/
|
||||
bool (*dev_tx_ready)(void *priv);
|
||||
/**
|
||||
* @brief Enable/Disable CAN interrupts.
|
||||
*
|
||||
* @param[in] priv device control structure.
|
||||
* @param[in] flag true/false to Enable/Disable CAN interrupts.
|
||||
*
|
||||
*/
|
||||
void (*dev_int)(void *priv, bool flag);
|
||||
/**
|
||||
* @brief CAN device specific I/O controls.
|
||||
*
|
||||
* @param[in] priv device control structure.
|
||||
* @param[in] buffer This depends on the cmd.
|
||||
* @param[in] cmd Device specific I/O commands.
|
||||
*
|
||||
* @retval 0 Depends on the cmd.
|
||||
*/
|
||||
int32_t (*dev_ioctl)(void *priv, void *buffer, size_t cmd);
|
||||
} can_dev_ops;
|
||||
|
||||
/**
|
||||
* @name CAN bus control
|
||||
*
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief Obtains the bus.
|
||||
*
|
||||
* This command has no argument.
|
||||
*/
|
||||
typedef struct can_bus {
|
||||
/**
|
||||
* @brief Device specific control.
|
||||
*/
|
||||
void *priv;
|
||||
/**
|
||||
* @brief Device controller index.
|
||||
*/
|
||||
uint8_t index;
|
||||
/**
|
||||
* @brief Device specific operations.
|
||||
*/
|
||||
struct can_dev_ops *can_dev_ops;
|
||||
/**
|
||||
* @brief tx fifo.
|
||||
*/
|
||||
struct ring_buf tx_fifo;
|
||||
/**
|
||||
* @brief Counting semaphore id (for fifo sync).
|
||||
*/
|
||||
rtems_id tx_fifo_sem_id;
|
||||
|
||||
/* FIXME: Using only one CAN msg buffer, Should create a ring buffer */
|
||||
/**
|
||||
* @brief rx fifo.
|
||||
*/
|
||||
struct can_msg can_rx_msg;
|
||||
/**
|
||||
* @brief Mutex to handle bus concurrency.
|
||||
*/
|
||||
rtems_mutex mutex;
|
||||
/**
|
||||
* @brief Destroys the bus.
|
||||
*
|
||||
* @param[in] bus control structure.
|
||||
*/
|
||||
void (*destroy)(struct can_bus *bus);
|
||||
#ifdef CAN_DEBUG_LOCK
|
||||
|
||||
/**
|
||||
* @brief For debugging semaphore obtain/release.
|
||||
*/
|
||||
int sem_count;
|
||||
|
||||
#endif /* CAN_DEBUG_LOCK */
|
||||
|
||||
} can_bus;
|
||||
|
||||
/** @} */
|
||||
|
||||
/**
|
||||
* @brief Register a CAN node with the CAN bus driver.
|
||||
*
|
||||
* @param[in] bus bus control structure.
|
||||
* @param[in] bus_path path of device node.
|
||||
*
|
||||
* @retval >=0 rtems status.
|
||||
*/
|
||||
rtems_status_code can_bus_register(can_bus *bus, const char *bus_path);
|
||||
|
||||
/**
|
||||
* @brief Allocate and initilaize bus control structure.
|
||||
*
|
||||
* @param[in] size Size of the bus control structure.
|
||||
*
|
||||
* @retval NULL No memory available.
|
||||
* @retval Address Pointer to the allocated bus control structure.
|
||||
*/
|
||||
can_bus *can_bus_alloc_and_init(size_t size);
|
||||
|
||||
/**
|
||||
* @brief initilaize bus control structure.
|
||||
*
|
||||
* @param[in] bus bus control structure.
|
||||
*
|
||||
* @retval 0 success.
|
||||
* @retval >0 error number.
|
||||
*/
|
||||
int can_bus_init(can_bus *bus);
|
||||
|
||||
/**
|
||||
* @brief Initiates CAN message transfer.
|
||||
*
|
||||
* Should be called with CAN interrupt disabled.
|
||||
*
|
||||
* @param[in] bus Bus control structure.
|
||||
*
|
||||
* @retval 0 success.
|
||||
* @retval >0 error number.
|
||||
*/
|
||||
int can_tx_done(struct can_bus *bus);
|
||||
|
||||
/**
|
||||
* @brief Sends the received CAN message to the application.
|
||||
*
|
||||
* Should be called by the device when CAN message should be sent to applicaiton.
|
||||
* Should be called only with CAN interrupts disabled.
|
||||
*
|
||||
* @param[in] bus bus control structure.
|
||||
* @param[in] msg can_msg structure.
|
||||
*
|
||||
* @retval 0 success.
|
||||
* @retval >0 error number.
|
||||
*/
|
||||
int can_receive(struct can_bus *bus, struct can_msg *msg);
|
||||
|
||||
/**
|
||||
* @brief Prints the can_msg values pointed by msg.
|
||||
*
|
||||
* @param[in] msg can_msg structure.
|
||||
*
|
||||
*/
|
||||
void can_print_msg(struct can_msg const *msg);
|
||||
|
||||
/** @} */ /* end of CAN device driver */
|
||||
|
||||
/** @} */
|
||||
|
||||
#endif /* _DEV_CAN_CAN_H */
|
||||
@@ -1,231 +0,0 @@
|
||||
/* SPDX-License-Identifier: BSD-2-Clause */
|
||||
|
||||
/**
|
||||
* @file
|
||||
*
|
||||
* @ingroup CANBus
|
||||
*
|
||||
* @brief Controller Area Network (CAN) Bus Implementation
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (C) 2022 Prashanth S <fishesprashanth@gmail.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef _DEV_CAN_CAN_QUEUE_H
|
||||
#define _DEV_CAN_CAN_QUEUE_H
|
||||
|
||||
#include <rtems/imfs.h>
|
||||
#include <rtems/thread.h>
|
||||
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <dev/can/can-msg.h>
|
||||
#include <dev/can/can.h>
|
||||
|
||||
/**
|
||||
* @defgroup Controller Area Network (CAN) Driver
|
||||
*
|
||||
* @ingroup RTEMSDeviceDrivers
|
||||
*
|
||||
* @brief Controller Area Network (CAN) bus and device driver support.
|
||||
*
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @defgroup CANBus CAN Bus Driver
|
||||
*
|
||||
* @ingroup CAN
|
||||
*
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief Create CAN tx buffers.
|
||||
*
|
||||
* @param[in] bus Bus control structure.
|
||||
*
|
||||
* @retval 0 Successful operation.
|
||||
* @retval >0 error number in case of an error.
|
||||
*/
|
||||
static rtems_status_code can_bus_create_tx_buffers(struct can_bus *bus);
|
||||
|
||||
/**
|
||||
* @brief Free CAN tx buffers.
|
||||
*
|
||||
* @param[in] bus Bus control structure.
|
||||
*
|
||||
*/
|
||||
static void can_bus_free_tx_buffers(struct can_bus *bus);
|
||||
|
||||
/**
|
||||
* @brief Check for atleast one empty CAN tx buffer.
|
||||
*
|
||||
* @param[in] bus Bus control structure.
|
||||
*
|
||||
* @retval true If atleast one CAN buffer is empty.
|
||||
* @retval false If no CAN buffer is empty.
|
||||
*/
|
||||
static bool can_bus_tx_buf_is_empty(struct can_bus *bus);
|
||||
|
||||
/**
|
||||
* @brief Get a produced tx buffer to transmit from the tx fifo.
|
||||
*
|
||||
* @param[in] bus Bus control structure.
|
||||
*
|
||||
* @retval can_msg Pointer to can_msg structure buffer.
|
||||
* @retval NULL If no can_msg buffer.
|
||||
*/
|
||||
static struct can_msg *can_bus_tx_get_data_buf(struct can_bus *bus);
|
||||
|
||||
/**
|
||||
* @brief Get a empty tx buffer.
|
||||
*
|
||||
* @param[in] bus Bus control structure.
|
||||
*
|
||||
* @retval can_msg Pointer to can_msg structure buffer.
|
||||
* @retval NULL If no empty can_msg buffer.
|
||||
*/
|
||||
static struct can_msg *can_bus_tx_get_empty_buf(struct can_bus *bus);
|
||||
|
||||
/**
|
||||
* @brief Creates tx buffers for the CAN bus driver.
|
||||
*
|
||||
* @param[in] bus Bus control structure.
|
||||
*
|
||||
* @retval rtems_status_code
|
||||
*/
|
||||
static rtems_status_code can_bus_create_tx_buffers(struct can_bus *bus)
|
||||
{
|
||||
bus->tx_fifo.pbuf = (struct can_msg *)malloc(CAN_TX_BUF_COUNT *
|
||||
sizeof(struct can_msg));
|
||||
if (bus->tx_fifo.pbuf == NULL) {
|
||||
CAN_ERR("can_create_tx_buffers: malloc failed\n");
|
||||
return RTEMS_NO_MEMORY;
|
||||
}
|
||||
|
||||
bus->tx_fifo.empty_count = CAN_TX_BUF_COUNT;
|
||||
|
||||
return RTEMS_SUCCESSFUL;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Free tx buffers for the CAN bus driver.
|
||||
*
|
||||
* @param[in] bus Bus control structure.
|
||||
*
|
||||
*/
|
||||
static void can_bus_free_tx_buffers(struct can_bus *bus)
|
||||
{
|
||||
free(bus->tx_fifo.pbuf);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Check if there is atleast one tx buffer in the CAN
|
||||
* bus driver.
|
||||
*
|
||||
* @param[in] bus Bus control structure.
|
||||
*
|
||||
* @retval true - If there is at least one free tx buffer.
|
||||
* false - If there is no free tx buffer.
|
||||
*/
|
||||
static bool can_bus_tx_buf_is_empty(struct can_bus *bus)
|
||||
{
|
||||
if (bus->tx_fifo.empty_count == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief To get a can_msg tx buf which contains valid data to send in
|
||||
* in the CAN bus.
|
||||
*
|
||||
* Note: freeing the returned data buf is done in the same function,
|
||||
* So the returned buffer should be sent before releasing the
|
||||
* lock acquired while calling this function.
|
||||
*
|
||||
* @param[in] bus Bus control structure.
|
||||
*
|
||||
* @retval *can_msg - If there is atleast one tx buffer to send in the CAN bus.
|
||||
* NULL - If there is no valid tx buffer.
|
||||
*/
|
||||
static struct can_msg *can_bus_tx_get_data_buf(struct can_bus *bus)
|
||||
{
|
||||
struct can_msg *msg = NULL;
|
||||
|
||||
if (bus->tx_fifo.empty_count == CAN_TX_BUF_COUNT ||
|
||||
bus->tx_fifo.tail >= CAN_TX_BUF_COUNT) {
|
||||
CAN_DEBUG_BUF("can_bus_tx_get_data_buf: All buffers are empty\n");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
msg = &bus->tx_fifo.pbuf[bus->tx_fifo.tail];
|
||||
bus->tx_fifo.empty_count++;
|
||||
bus->tx_fifo.tail = (bus->tx_fifo.tail + 1) % CAN_TX_BUF_COUNT;
|
||||
|
||||
return msg;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief To get a can_msg tx buf which is empty (contains no valid data).
|
||||
*
|
||||
* Note: marking the returned buf valid is done in the same function
|
||||
* So a valid CAN message should be copied to the returned buffer before
|
||||
* releasing the lock acquired while calling this function.
|
||||
*
|
||||
* @param[in] bus Bus control structure.
|
||||
*
|
||||
* @retval *can_msg - If there is atleast one empty tx buffer.
|
||||
* NULL - If there is no empty tx buffer.
|
||||
*/
|
||||
static struct can_msg *can_bus_tx_get_empty_buf(struct can_bus *bus)
|
||||
{
|
||||
struct can_msg *msg = NULL;
|
||||
|
||||
/* Check whether there is a empty CAN msg buffer */
|
||||
if (can_bus_tx_buf_is_empty(bus) == false) {
|
||||
CAN_DEBUG_BUF("can_bus_tx_get_empty_buf: No empty buffer\n");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
bus->tx_fifo.empty_count--;
|
||||
|
||||
/* tx_fifo.head always points to a empty buffer if there is atleast one */
|
||||
msg = &bus->tx_fifo.pbuf[bus->tx_fifo.head];
|
||||
bus->tx_fifo.head = (bus->tx_fifo.head + 1) % CAN_TX_BUF_COUNT;
|
||||
|
||||
return msg;
|
||||
}
|
||||
|
||||
/** @} */ /* end of CAN device driver */
|
||||
|
||||
/** @} */
|
||||
|
||||
#endif /*_DEV_CAN_CAN_QUEUE_H */
|
||||
@@ -55,11 +55,6 @@ install:
|
||||
- destination: ${BSP_INCLUDEDIR}/dev/flash
|
||||
source:
|
||||
- cpukit/include/dev/flash/flashdev.h
|
||||
- destination: ${BSP_INCLUDEDIR}/dev/can
|
||||
source:
|
||||
- cpukit/include/dev/can/can.h
|
||||
- cpukit/include/dev/can/can-msg.h
|
||||
- cpukit/include/dev/can/canqueueimpl.h
|
||||
- destination: ${BSP_INCLUDEDIR}/linux
|
||||
source:
|
||||
- cpukit/include/linux/i2c-dev.h
|
||||
@@ -528,7 +523,6 @@ source:
|
||||
- cpukit/compression/xz/xz_crc32.c
|
||||
- cpukit/compression/xz/xz_dec_lzma2.c
|
||||
- cpukit/compression/xz/xz_dec_stream.c
|
||||
- cpukit/dev/can/can.c
|
||||
- cpukit/dev/flash/flashdev.c
|
||||
- cpukit/dev/i2c/eeprom.c
|
||||
- cpukit/dev/i2c/fpga-i2c-slave.c
|
||||
|
||||
@@ -1,20 +0,0 @@
|
||||
SPDX-License-Identifier: CC-BY-SA-4.0 OR BSD-2-Clause
|
||||
build-type: test-program
|
||||
cflags: []
|
||||
copyrights:
|
||||
- Copyright (c) 2022 Prashanth S (fishesprashanth@gmail.com).
|
||||
cppflags: []
|
||||
cxxflags: []
|
||||
enabled-by: true
|
||||
features: c cprogram
|
||||
includes: []
|
||||
ldflags: []
|
||||
links: []
|
||||
source:
|
||||
- testsuites/libtests/can01/init.c
|
||||
- testsuites/libtests/can01/can-loopback.c
|
||||
stlib: []
|
||||
target: testsuites/libtests/can01.exe
|
||||
type: build
|
||||
use-after: []
|
||||
use-before: []
|
||||
@@ -76,8 +76,6 @@ links:
|
||||
uid: cpuuse
|
||||
- role: build-dependency
|
||||
uid: crypt01
|
||||
- role: build-dependency
|
||||
uid: can01
|
||||
- role: build-dependency
|
||||
uid: debugger01
|
||||
- role: build-dependency
|
||||
|
||||
@@ -1,110 +0,0 @@
|
||||
/**
|
||||
* @file
|
||||
*
|
||||
* @ingroup CANBus
|
||||
*
|
||||
* @brief Controller Area Network (CAN) loopback device Implementation
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (C) 2022 Prashanth S (fishesprashanth@gmail.com)
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <dev/can/can.h>
|
||||
|
||||
struct can_loopback_priv {
|
||||
struct can_bus *bus;
|
||||
};
|
||||
|
||||
static bool can_loopback_tx_ready(void *data);
|
||||
static void can_loopback_int(void *data, bool flag);
|
||||
static int can_loopback_tx(void *data, struct can_msg *msg);
|
||||
int can_loopback_init(const char *can_dev_file);
|
||||
|
||||
static struct can_dev_ops dev_ops = {
|
||||
.dev_tx = can_loopback_tx,
|
||||
.dev_tx_ready = can_loopback_tx_ready,
|
||||
.dev_int = can_loopback_int,
|
||||
};
|
||||
|
||||
static bool can_loopback_tx_ready(void *data)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
static void can_loopback_int(void *data, bool flag)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
static int can_loopback_tx(void *data, struct can_msg *msg)
|
||||
{
|
||||
struct can_loopback_priv *priv = data;
|
||||
|
||||
can_receive(priv->bus, msg);
|
||||
|
||||
return RTEMS_SUCCESSFUL;
|
||||
}
|
||||
|
||||
int can_loopback_init(const char *can_dev_file)
|
||||
{
|
||||
int ret;
|
||||
struct can_loopback_priv *priv = NULL;
|
||||
|
||||
struct can_bus *bus = can_bus_alloc_and_init(sizeof(struct can_bus));
|
||||
if (bus == NULL) {
|
||||
CAN_ERR("can_loopback_init: can_bus_alloc_and_init failed\n");
|
||||
return RTEMS_NO_MEMORY;
|
||||
}
|
||||
|
||||
priv = (struct can_loopback_priv *)calloc(1, sizeof(struct can_loopback_priv));
|
||||
if (priv == NULL) {
|
||||
CAN_ERR("can_loopback_init: calloc failed\n");
|
||||
ret = RTEMS_NO_MEMORY;
|
||||
goto free_bus_return;
|
||||
}
|
||||
|
||||
priv->bus = bus;
|
||||
bus->priv = priv;
|
||||
|
||||
priv->bus->can_dev_ops = &dev_ops;
|
||||
|
||||
if ((ret = can_bus_register(bus, can_dev_file)) != RTEMS_SUCCESSFUL) {
|
||||
CAN_ERR("can_loopback_init: bus register failed\n");
|
||||
goto free_priv_return;
|
||||
}
|
||||
|
||||
CAN_DEBUG("can_loopback_init: can_loopback driver registered\n");
|
||||
|
||||
return ret;
|
||||
|
||||
free_priv_return:
|
||||
free(priv);
|
||||
|
||||
free_bus_return:
|
||||
free(bus);
|
||||
|
||||
return ret;
|
||||
}
|
||||
@@ -1,249 +0,0 @@
|
||||
/* SPDX-License-Identifier: BSD-2-Clause */
|
||||
|
||||
/*
|
||||
* Copyright (c) 2022 Prashanth S (fishesprashanth@gmail.com) All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifdef HAVE_CONFIG_H
|
||||
#include "config.h"
|
||||
#endif
|
||||
|
||||
#include <rtems.h>
|
||||
#include <rtems/error.h>
|
||||
#include <sched.h>
|
||||
#include <tmacros.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <dev/can/can.h>
|
||||
|
||||
#define TASKS (12)
|
||||
|
||||
#define CAN_DEV_FILE "/dev/can-loopback"
|
||||
#define NUM_TEST_MSGS (0xf)
|
||||
|
||||
#define NEXT_TASK_NAME(c1, c2, c3, c4) \
|
||||
if (c4 == '9') { \
|
||||
if (c3 == '9') { \
|
||||
if (c2 == 'z') { \
|
||||
if (c1 == 'z') { \
|
||||
printf("not enough task letters for names !!!\n"); \
|
||||
exit( 1 ); \
|
||||
} else \
|
||||
c1++; \
|
||||
c2 = 'a'; \
|
||||
} else \
|
||||
c2++; \
|
||||
c3 = '0'; \
|
||||
} else \
|
||||
c3++; \
|
||||
c4 = '0'; \
|
||||
} \
|
||||
else \
|
||||
c4++ \
|
||||
|
||||
static void test_task(rtems_task_argument);
|
||||
int can_loopback_init(const char *);
|
||||
int create_task(int);
|
||||
|
||||
static rtems_id task_id[TASKS];
|
||||
static rtems_id task_test_status[TASKS] = {[0 ... (TASKS - 1)] = false};
|
||||
|
||||
const char rtems_test_name[] = "CAN test TX, RX with CAN loopback driver";
|
||||
|
||||
/*FIXME: Should Implement one more test application for the
|
||||
* RTR support
|
||||
*
|
||||
* For testing, the number of successful read and write
|
||||
* count is verified.
|
||||
*/
|
||||
static void test_task(rtems_task_argument data)
|
||||
{
|
||||
//sleep so that other tasks will be created.
|
||||
sleep(1);
|
||||
|
||||
int fd, task_num = (uint32_t)data;
|
||||
uint32_t count = 0, msg_size;
|
||||
|
||||
struct can_msg msg;
|
||||
|
||||
printf("CAN tx and rx for %s\n", CAN_DEV_FILE);
|
||||
|
||||
fd = open(CAN_DEV_FILE, O_RDWR);
|
||||
if (fd < 0) {
|
||||
printf("open error: task = %u %s: %s\n", task_num, CAN_DEV_FILE, strerror(errno));
|
||||
}
|
||||
|
||||
rtems_test_assert(fd >= 0);
|
||||
|
||||
for (int i = 0; i < NUM_TEST_MSGS; i++) {
|
||||
printf("test_task %u\n", task_num);
|
||||
|
||||
msg.id = task_num;
|
||||
//FIXME: Implement Test cases for other flags also.
|
||||
msg.flags = 0;
|
||||
msg.len = (i + 1) % 9;
|
||||
|
||||
for (int j = 0; j < msg.len; j++) {
|
||||
msg.data[j] = 'a' + j;
|
||||
}
|
||||
|
||||
msg_size = ((char *)&msg.data[msg.len] - (char *)&msg);
|
||||
|
||||
printf("calling write task = %u\n", task_num);
|
||||
|
||||
count = write(fd, &msg, sizeof(msg));
|
||||
rtems_test_assert(count == msg_size);
|
||||
printf("task = %u write count = %u\n", task_num, count);
|
||||
|
||||
printf("calling read task = %u\n", task_num);
|
||||
count = read(fd, &msg, sizeof(msg));
|
||||
rtems_test_assert(count > 0);
|
||||
printf("task = %u read count = %u\n", task_num, count);
|
||||
|
||||
printf("received message\n");
|
||||
can_print_msg(&msg);
|
||||
|
||||
sleep(1);
|
||||
}
|
||||
close(fd);
|
||||
|
||||
task_test_status[task_num] = true;
|
||||
|
||||
printf("task exited = %u\n", task_num);
|
||||
rtems_task_exit();
|
||||
}
|
||||
|
||||
int create_task(int i)
|
||||
{
|
||||
printf("Creating task %d\n", i);
|
||||
|
||||
rtems_status_code result;
|
||||
rtems_name name;
|
||||
|
||||
char c1 = 'a';
|
||||
char c2 = 'a';
|
||||
char c3 = '1';
|
||||
char c4 = '1';
|
||||
|
||||
name = rtems_build_name(c1, c2, c3, c4);
|
||||
|
||||
result = rtems_task_create(name,
|
||||
1,
|
||||
RTEMS_MINIMUM_STACK_SIZE,
|
||||
RTEMS_PREEMPT | RTEMS_TIMESLICE,
|
||||
RTEMS_FIFO | RTEMS_FLOATING_POINT,
|
||||
&task_id[i]);
|
||||
if (result != RTEMS_SUCCESSFUL) {
|
||||
printf("rtems_task_create error: %s\n", rtems_status_text(result));
|
||||
rtems_test_assert(result == RTEMS_SUCCESSFUL);
|
||||
}
|
||||
|
||||
printf("number = %3" PRIi32 ", id = %08" PRIxrtems_id ", starting, ", i, task_id[i]);
|
||||
|
||||
fflush(stdout);
|
||||
|
||||
printf("starting task\n");
|
||||
result = rtems_task_start(task_id[i],
|
||||
test_task,
|
||||
(rtems_task_argument)i);
|
||||
|
||||
if (result != RTEMS_SUCCESSFUL) {
|
||||
printf("rtems_task_start failed %s\n", rtems_status_text(result));
|
||||
rtems_test_assert(result == RTEMS_SUCCESSFUL);
|
||||
}
|
||||
|
||||
NEXT_TASK_NAME(c1, c2, c3, c4);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
static rtems_task Init(
|
||||
rtems_task_argument ignored
|
||||
)
|
||||
{
|
||||
printf("Init\n");
|
||||
|
||||
int ret;
|
||||
|
||||
rtems_print_printer_fprintf_putc(&rtems_test_printer);
|
||||
|
||||
TEST_BEGIN();
|
||||
|
||||
rtems_task_priority old_priority;
|
||||
rtems_mode old_mode;
|
||||
|
||||
rtems_task_set_priority(RTEMS_SELF, RTEMS_MAXIMUM_PRIORITY - 1, &old_priority);
|
||||
rtems_task_mode(RTEMS_PREEMPT, RTEMS_PREEMPT_MASK, &old_mode);
|
||||
|
||||
ret = can_loopback_init(CAN_DEV_FILE);
|
||||
if (ret != RTEMS_SUCCESSFUL) {
|
||||
printf("%s failed\n", rtems_test_name);
|
||||
rtems_test_assert(ret == RTEMS_SUCCESSFUL);
|
||||
}
|
||||
|
||||
for (int i = 0; i < TASKS; i++) {
|
||||
create_task(i);
|
||||
}
|
||||
|
||||
/* Do not exit untill all the tasks are exited */
|
||||
while (1) {
|
||||
int flag = 0;
|
||||
for (int i = 0; i < TASKS; i++) {
|
||||
if (task_test_status[i] == false) {
|
||||
printf("task not exited = %d\n", i);
|
||||
sleep(1);
|
||||
flag = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (flag == 0) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
TEST_END();
|
||||
|
||||
rtems_test_exit( 0 );
|
||||
}
|
||||
|
||||
|
||||
#define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
|
||||
|
||||
#define CONFIGURE_APPLICATION_NEEDS_SIMPLE_CONSOLE_DRIVER
|
||||
#define CONFIGURE_RTEMS_INIT_TASKS_TABLE
|
||||
|
||||
#define CONFIGURE_INIT_TASK_ATTRIBUTES RTEMS_FLOATING_POINT
|
||||
|
||||
#define CONFIGURE_MAXIMUM_TASKS (TASKS + TASKS)
|
||||
|
||||
#define CONFIGURE_MAXIMUM_SEMAPHORES 10
|
||||
|
||||
#define CONFIGURE_MAXIMUM_FILE_DESCRIPTORS (TASKS * 2)
|
||||
|
||||
#define CONFIGURE_TICKS_PER_TIMESLICE 100
|
||||
|
||||
#define CONFIGURE_INIT
|
||||
|
||||
#include <rtems/confdefs.h>
|
||||
Reference in New Issue
Block a user