From cd91b37dce728b372f164355719a4e601e12e7b3 Mon Sep 17 00:00:00 2001 From: Prashanth S Date: Sun, 7 Aug 2022 19:23:13 +0530 Subject: cpukit/dev/can: Added CAN support --- cpukit/dev/can/can.c | 505 +++++++++++++++++++++++++++++++ cpukit/include/dev/can/can-msg.h | 105 +++++++ cpukit/include/dev/can/can.h | 284 +++++++++++++++++ cpukit/include/dev/can/canqueueimpl.h | 231 ++++++++++++++ spec/build/cpukit/librtemscpu.yml | 6 + spec/build/testsuites/libtests/can01.yml | 20 ++ spec/build/testsuites/libtests/grp.yml | 2 + testsuites/libtests/can01/can-loopback.c | 110 +++++++ testsuites/libtests/can01/init.c | 249 +++++++++++++++ 9 files changed, 1512 insertions(+) create mode 100644 cpukit/dev/can/can.c create mode 100644 cpukit/include/dev/can/can-msg.h create mode 100644 cpukit/include/dev/can/can.h create mode 100644 cpukit/include/dev/can/canqueueimpl.h create mode 100644 spec/build/testsuites/libtests/can01.yml create mode 100644 testsuites/libtests/can01/can-loopback.c create mode 100644 testsuites/libtests/can01/init.c diff --git a/cpukit/dev/can/can.c b/cpukit/dev/can/can.c new file mode 100644 index 0000000000..2e6d5df65b --- /dev/null +++ b/cpukit/dev/can/can.c @@ -0,0 +1,505 @@ +/* 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 +#include + +#include + +#include +#include + +#include +#include + +#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; +} diff --git a/cpukit/include/dev/can/can-msg.h b/cpukit/include/dev/can/can-msg.h new file mode 100644 index 0000000000..9863e14d84 --- /dev/null +++ b/cpukit/include/dev/can/can-msg.h @@ -0,0 +1,105 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ + +/** + * @file + * + * @ingroup CANBus + * + * @brief Controller Area Network (CAN) Bus Implementation + * + */ + +/* + * Copyright (C) 2022 Prashanth S + * + * 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 */ diff --git a/cpukit/include/dev/can/can.h b/cpukit/include/dev/can/can.h new file mode 100644 index 0000000000..9e55395039 --- /dev/null +++ b/cpukit/include/dev/can/can.h @@ -0,0 +1,284 @@ +/* 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 +#include +#include + +#include +#include +#include + +#include + +#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 */ diff --git a/cpukit/include/dev/can/canqueueimpl.h b/cpukit/include/dev/can/canqueueimpl.h new file mode 100644 index 0000000000..ef0d56fe31 --- /dev/null +++ b/cpukit/include/dev/can/canqueueimpl.h @@ -0,0 +1,231 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ + +/** + * @file + * + * @ingroup CANBus + * + * @brief Controller Area Network (CAN) Bus Implementation + * + */ + +/* + * Copyright (C) 2022 Prashanth S + * + * 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 +#include + +#include +#include +#include + +#include +#include + +/** + * @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 */ diff --git a/spec/build/cpukit/librtemscpu.yml b/spec/build/cpukit/librtemscpu.yml index a1be70c6da..0b0dc95912 100644 --- a/spec/build/cpukit/librtemscpu.yml +++ b/spec/build/cpukit/librtemscpu.yml @@ -52,6 +52,11 @@ install: - destination: ${BSP_INCLUDEDIR}/dev/spi source: - cpukit/include/dev/spi/spi.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 @@ -535,6 +540,7 @@ source: - cpukit/dev/serial/sc16is752-spi.c - cpukit/dev/serial/sc16is752.c - cpukit/dev/spi/spi-bus.c +- cpukit/dev/can/can.c - cpukit/dtc/libfdt/fdt.c - cpukit/dtc/libfdt/fdt_addresses.c - cpukit/dtc/libfdt/fdt_empty_tree.c diff --git a/spec/build/testsuites/libtests/can01.yml b/spec/build/testsuites/libtests/can01.yml new file mode 100644 index 0000000000..b5c1e1ae5c --- /dev/null +++ b/spec/build/testsuites/libtests/can01.yml @@ -0,0 +1,20 @@ +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: [] diff --git a/spec/build/testsuites/libtests/grp.yml b/spec/build/testsuites/libtests/grp.yml index 2aa7fa058e..9d91df75a0 100644 --- a/spec/build/testsuites/libtests/grp.yml +++ b/spec/build/testsuites/libtests/grp.yml @@ -76,6 +76,8 @@ links: uid: cpuuse - role: build-dependency uid: crypt01 +- role: build-dependency + uid: can01 - role: build-dependency uid: debugger01 - role: build-dependency diff --git a/testsuites/libtests/can01/can-loopback.c b/testsuites/libtests/can01/can-loopback.c new file mode 100644 index 0000000000..0aaf2f6a6d --- /dev/null +++ b/testsuites/libtests/can01/can-loopback.c @@ -0,0 +1,110 @@ +/** + * @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 + +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; +} diff --git a/testsuites/libtests/can01/init.c b/testsuites/libtests/can01/init.c new file mode 100644 index 0000000000..0675fe606f --- /dev/null +++ b/testsuites/libtests/can01/init.c @@ -0,0 +1,249 @@ +/* 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 +#include +#include +#include +#include +#include + +#include + +#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 -- cgit v1.2.3