can-queue.h

Go to the documentation of this file.

/* SPDX-License-Identifier: BSD-2-Clause OR Apache-2.0 OR GPL-2.0-or-later */
 
/*
 * Copyright (C) 2023-2024 Michal Lenc <michallenc@seznam.cz>
 * Implementation is based on original LinCAN - Linux CAN bus driver
 * Part of OrtCAN project https://ortcan.sourceforge.net/
 * Copyright (C) 2002-2009 DCE FEE CTU Prague <http://control.fel.cvut.cz>
 * Copyright (C) 2002-2024 Pavel Pisa <pisa@cmp.felk.cvut.cz>
 *
 * 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 <stdatomic.h>
#include <inttypes.h>
#include <sys/queue.h>
#include <rtems.h>
#include <rtems/timespec.h>
#include <rtems/status-checks.h>
#include <rtems/thread.h>
 
#include <dev/can/can-frame.h>
#include <dev/can/can-filter.h>
 
struct canque_slot_t {
  struct canque_slot_t *next;
  atomic_uint slot_flags;
  struct can_frame frame;
};
 
#define RTEMS_CAN_SLOTF_CMD  ( 0x00ff )
#define RTEMS_CAN_QUEUE_PRIO_NR  ( 3 )
 
struct canque_fifo_t {
  atomic_uint fifo_flags;
  unsigned int out_taken;
  unsigned long error_code;
  struct canque_slot_t *head;
  struct canque_slot_t **tail;
  struct canque_slot_t *flist;
  struct canque_slot_t *entry;
  int maxdlen;
  int slotsnr;
  rtems_mutex fifo_lock;
};
 
#define CAN_FIFOF_DESTROY      ( 1 << 15 )
#define CAN_FIFOF_ERROR        ( 1 << 14 )
#define CAN_FIFOF_ERR2BLOCK    ( 1 << 13 )
#define CAN_FIFOF_BLOCK        ( 1 << 12 )
#define CAN_FIFOF_OVERRUN      ( 1 << 11 )
#define CAN_FIFOF_FULL         ( 1 << 10 )
#define CAN_FIFOF_EMPTY        ( 1 << 9 )
#define CAN_FIFOF_DEAD         ( 1 << 8 )
#define CAN_FIFOF_INACTIVE     ( 1 << 7 )
#define CAN_FIFOF_FREEONEMPTY  ( 1 << 6 )
#define CAN_FIFOF_READY        ( 1 << 5 )
#define CAN_FIFOF_NOTIFYPEND   ( 1 << 4 )
#define CAN_FIFOF_RTL_MEM      ( 1 << 3 )
 
static inline int canque_fifo_test_fl( struct canque_fifo_t *fifo, int flag )
{
  return ( atomic_load( &fifo->fifo_flags ) &flag ) ? 1 : 0;
}
 
static inline void canque_fifo_set_fl( struct canque_fifo_t *fifo, int flag )
{
  atomic_fetch_or( &fifo->fifo_flags, flag );
}
 
static inline void canque_fifo_clear_fl( struct canque_fifo_t *fifo, int flag )
{
  atomic_fetch_and( &fifo->fifo_flags, ~flag );
}
 
static inline int canque_fifo_test_and_set_fl(
  struct canque_fifo_t *fifo,
  int flag
)
{
  return ( atomic_fetch_or( &fifo->fifo_flags, flag ) & flag ) ? 1 : 0;
}
 
static inline int canque_fifo_test_and_clear_fl(
  struct canque_fifo_t *fifo,
  int flag
)
{
  return ( atomic_fetch_and( &fifo->fifo_flags, ~flag ) & flag ) ? 1 : 0;
}
 
static inline int canque_fifo_out_is_ready_unprotected(
  struct canque_fifo_t *fifo
)
{
  return ( ( fifo->head ) != NULL ) ? 1 : 0;
}
 
static inline bool canque_filter_match(
  struct can_filter *filter,
  uint32_t id,
  uint32_t flags )
{
  return ( ( filter->id^id ) & filter->id_mask ) ||
         ( ( filter->flags^flags ) & filter->flags_mask ) ? false : true;
}
 
static inline int canque_fifo_get_inslot(
  struct canque_fifo_t *fifo,
  struct canque_slot_t **slotp,
  int cmd
)
{
  struct canque_slot_t *slot;
  rtems_mutex_lock( &fifo->fifo_lock );
 
  /* Get the first free slot slot from flist */
  if ( ( slot = fifo->flist ) == NULL ) {
    canque_fifo_set_fl( fifo, CAN_FIFOF_OVERRUN );
    canque_fifo_set_fl( fifo, CAN_FIFOF_FULL );
    rtems_mutex_unlock( &fifo->fifo_lock );
    *slotp = NULL;
    return -1;
  }
 
  /* Adjust free slot list */
  if ( ( fifo->flist = slot->next ) == NULL ) {
    canque_fifo_set_fl( fifo, CAN_FIFOF_FULL );
  }
 
  rtems_mutex_unlock( &fifo->fifo_lock );
  *slotp = slot;
  slot->slot_flags = cmd & RTEMS_CAN_SLOTF_CMD;
 
  return 1;
}
 
static inline int canque_fifo_put_inslot(
  struct canque_fifo_t *fifo,
  struct canque_slot_t *slot
)
{
  int ret = 0;
 
  slot->next = NULL;
  rtems_mutex_lock( &fifo->fifo_lock );
  *fifo->tail = slot;
  fifo->tail = &slot->next;
 
  if ( canque_fifo_test_and_clear_fl( fifo, CAN_FIFOF_EMPTY ) ) {
    /* Fifo has been empty before put */
    ret = CAN_FIFOF_EMPTY;
  }
 
  if ( canque_fifo_test_and_clear_fl( fifo, CAN_FIFOF_INACTIVE ) ) {
    /* Fifo has been empty before put */
    ret |= CAN_FIFOF_INACTIVE;
  }
 
  if ( canque_fifo_test_and_clear_fl( fifo, CAN_FIFOF_OVERRUN ) ) {
    /* RX overflow occured, report it with frame flag */
    slot->frame.header.flags |= CAN_FRAME_FIFO_OVERFLOW;
  }
 
  rtems_mutex_unlock( &fifo->fifo_lock );
 
  return ret;
}
 
static inline int canque_fifo_abort_inslot(
  struct canque_fifo_t *fifo,
  struct canque_slot_t *slot
)
{
  int ret = 0;
 
  rtems_mutex_lock( &fifo->fifo_lock );
  slot->next = fifo->flist;
  fifo->flist = slot;
  if ( canque_fifo_test_and_clear_fl( fifo, CAN_FIFOF_FULL ) ) {
    ret = CAN_FIFOF_FULL;
  }
 
  rtems_mutex_unlock( &fifo->fifo_lock );
 
  return ret;
}
 
static inline int canque_fifo_test_outslot(
  struct canque_fifo_t *fifo,
  struct canque_slot_t **slotp
)
{
  int cmd;
  struct canque_slot_t *slot;
 
  rtems_mutex_lock( &fifo->fifo_lock );
  if ( ( slot = fifo->head ) == NULL ) {
    rtems_mutex_unlock( &fifo->fifo_lock );
    *slotp = NULL;
    return -1;
  }
 
  if ( ( fifo->head=slot->next ) == NULL ) {
    fifo->tail = &fifo->head;
  }
 
  fifo->out_taken += 1;
 
  rtems_mutex_unlock( &fifo->fifo_lock );
 
  *slotp = slot;
  cmd = slot->slot_flags;
 
  return ( cmd & RTEMS_CAN_SLOTF_CMD );
}
 
static inline int canque_fifo_free_outslot(
  struct canque_fifo_t *fifo,
  struct canque_slot_t *slot
)
{
  int ret = 0;
 
  rtems_mutex_lock( &fifo->fifo_lock );
  slot->next = fifo->flist;
  fifo->flist = slot;
  if ( canque_fifo_test_and_clear_fl( fifo, CAN_FIFOF_FULL ) ) {
    ret = CAN_FIFOF_FULL;
  }
 
  fifo->out_taken -= 1;
 
  if ( ( fifo->head ) == NULL ) {
    canque_fifo_set_fl( fifo, CAN_FIFOF_INACTIVE );
    ret |= CAN_FIFOF_INACTIVE;
    if ( fifo->out_taken == 0 ) {
      canque_fifo_set_fl( fifo, CAN_FIFOF_EMPTY );
      ret |= CAN_FIFOF_EMPTY;
    }
  }
 
  rtems_mutex_unlock( &fifo->fifo_lock );
 
  return ret;
}
 
static inline int canque_fifo_again_outslot(
  struct canque_fifo_t *fifo,
  struct canque_slot_t *slot
)
{
  int ret = 0;
 
  rtems_mutex_lock( &fifo->fifo_lock );
  if ( ( slot->next = fifo->head ) == NULL ) {
    fifo->tail = &slot->next;
    ret = CAN_FIFOF_INACTIVE;
  }
 
  fifo->out_taken -= 1;
 
  fifo->head = slot;
  rtems_mutex_unlock( &fifo->fifo_lock );
 
  return ret;
}
 
static inline int canque_fifo_slot_size( int maxdlen )
{
  return offsetof( struct canque_slot_t, frame.data ) + maxdlen +
    ( ( RTEMS_ALIGNOF(struct canque_slot_t) - 1 ) &
    ~( RTEMS_ALIGNOF(struct canque_slot_t) - 1 ) );
}
 
int canque_fifo_flush_slots( struct canque_fifo_t *fifo );
 
int canque_fifo_init_slots( struct canque_fifo_t *fifo );
 
struct canque_edge_t;
TAILQ_HEAD( canqueue_edges_list_t, canque_edge_t );
 
struct canque_edge_t {
  struct canque_fifo_t fifo;
  struct can_filter filter;
  TAILQ_ENTRY( canque_edge_t ) inpeers;
  TAILQ_ENTRY( canque_edge_t ) outpeers;
  TAILQ_ENTRY( canque_edge_t ) activepeers;
  struct canqueue_edges_list_t *peershead;
  struct canque_ends_t *inends;
  struct canque_ends_t *outends;
  atomic_uint edge_used;
  int edge_prio;
  int edge_num;
};
 
struct canque_ends_t;
TAILQ_HEAD( canqueue_ends_list_t, canque_ends_t );
 
struct canque_ends_t {
  struct canqueue_edges_list_t active[RTEMS_CAN_QUEUE_PRIO_NR];
  struct canqueue_edges_list_t idle;
  struct canqueue_edges_list_t inlist;
  struct canqueue_edges_list_t outlist;
  rtems_mutex ends_lock;
  void (*notify)( struct canque_ends_t *qends, struct canque_edge_t *qedge,
                  int what );
  TAILQ_ENTRY( canque_ends_t ) dead_peers;
};
 
#define CANQUEUE_NOTIFY_EMPTY  ( 1 )
#define CANQUEUE_NOTIFY_SPACE  ( 2 )
#define CANQUEUE_NOTIFY_PROC   ( 3 )
#define CANQUEUE_NOTIFY_NOUSR  ( 4 )
#define CANQUEUE_NOTIFY_DEAD   ( 5 )
#define CANQUEUE_NOTIFY_DEAD_WANTED ( 6 )
#define CANQUEUE_NOTIFY_ATTACH ( 7 )
 
static inline void canque_notify_inends( struct canque_edge_t *qedge, int what )
{
  if ( qedge->inends ) {
    if ( qedge->inends->notify ) {
      qedge->inends->notify( qedge->inends, qedge,what );
    }
  }
}
 
static inline void canque_notify_outends(
  struct canque_edge_t *qedge,
  int what
)
{
  if ( qedge->outends ) {
    if ( qedge->outends->notify ) {
      qedge->outends->notify( qedge->outends, qedge, what );
    }
  }
}
 
static inline void canque_notify_bothends(
  struct canque_edge_t *qedge,
  int what
)
{
  canque_notify_inends( qedge, what );
  canque_notify_outends( qedge, what );
}
 
static inline void canque_activate_edge(
  struct canque_ends_t *inends,
  struct canque_edge_t *qedge
)
{
  struct canque_ends_t *outends;
 
  if ( qedge->edge_prio >= RTEMS_CAN_QUEUE_PRIO_NR ) {
    qedge->edge_prio = RTEMS_CAN_QUEUE_PRIO_NR - 1;
  }
 
  if ( ( outends = qedge->outends ) != NULL ) {
    rtems_mutex_lock( &outends->ends_lock );
    rtems_mutex_lock( &qedge->fifo.fifo_lock );
    if ( qedge->peershead != &outends->active[qedge->edge_prio] ) {
      if ( qedge->peershead != NULL ) {
        TAILQ_REMOVE( qedge->peershead, qedge, activepeers );
      }
 
      TAILQ_INSERT_HEAD( &outends->active[qedge->edge_prio], qedge, activepeers );
      qedge->peershead = &outends->active[qedge->edge_prio];
    }
    rtems_mutex_unlock( &qedge->fifo.fifo_lock );
    rtems_mutex_unlock( &outends->ends_lock );
  }
}
 
static inline void canque_edge_to_idle_unprotected(
  struct canque_ends_t *outends,
  struct canque_edge_t *qedge
)
{
  if ( qedge->peershead != NULL ) {
    TAILQ_REMOVE( qedge->peershead, qedge, activepeers );
  }
  TAILQ_INSERT_TAIL( &outends->idle, qedge, activepeers );
  qedge->peershead = &outends->idle;
}
 
int canque_get_inslot(
  struct canque_ends_t *qends,
  struct canque_edge_t **qedgep,
  struct canque_slot_t **slotp,
  int cmd
);
 
int canque_get_inslot4prio(
  struct canque_ends_t *qends,
  struct canque_edge_t **qedgep,
  struct canque_slot_t **slotp,
  const struct can_frame_header *header,
  int cmd,
  int prio
);
 
int canque_test_inslot( struct canque_ends_t *qends );
 
int canque_put_inslot(
  struct canque_ends_t *qends,
  struct canque_edge_t *qedge,
  struct canque_slot_t *slot
);
 
int canque_abort_inslot(
  struct canque_ends_t *qends,
  struct canque_edge_t *qedge,
  struct canque_slot_t *slot
);
 
int canque_filter_frame2edges(
  struct canque_ends_t *qends,
  struct canque_edge_t *src_edge,
  struct can_frame *frame,
  unsigned int flags2add
);
 
int canque_test_outslot(
  struct canque_ends_t *qends,
  struct canque_edge_t **qedgep,
  struct canque_slot_t **slotp
);
 
int canque_pending_outslot_prio(
  struct canque_ends_t *qends,
  int prio_min
);
 
int canque_free_outslot(
  struct canque_ends_t *qends,
  struct canque_edge_t *qedge,
  struct canque_slot_t *slot
);
 
int canque_again_outslot(
  struct canque_ends_t *qends,
  struct canque_edge_t *qedge,
  struct canque_slot_t *slot
);
 
int canque_flush( struct canque_edge_t *qedge );
 
int canqueue_disconnect_edge( struct canque_edge_t *qedge );
 
int canqueue_connect_edge(
  struct canque_edge_t *qedge,
  struct canque_ends_t *inends,
  struct canque_ends_t *outends
);
 
int canqueue_ends_init_gen( struct canque_ends_t *qends );
 
void canqueue_block_inlist( struct canque_ends_t *qends );
 
void canqueue_block_outlist( struct canque_ends_t *qends );
 
int canqueue_ends_kill_inlist( struct canque_ends_t *qends, int send_rest );
 
int canqueue_ends_kill_outlist( struct canque_ends_t *qends );
 
int canqueue_ends_flush_inlist( struct canque_ends_t *qends );
 
int canqueue_ends_flush_outlist( struct canque_ends_t *qends );
 
/* edge reference and traversal functions */
 
void canque_edge_do_dead( struct canque_edge_t *edge );
 
static inline void canque_edge_incref( struct canque_edge_t *edge )
{
  atomic_fetch_add( &edge->edge_used, 1 );
}
 
/* Non-inlined version of edge reference decrement */
void __canque_edge_decref( struct canque_edge_t *edge );
 
static inline void __canque_edge_decref_body( struct canque_edge_t *edge )
{
  int dead_fl = 0;
  struct canque_ends_t *inends=edge->inends;
  struct canque_ends_t *outends=edge->outends;
 
  if ( inends < outends ) {
    rtems_mutex_lock( &inends->ends_lock );
    rtems_mutex_lock( &outends->ends_lock );
    if ( atomic_fetch_sub( &edge->edge_used, 1 ) == 1 ) {
      dead_fl = !canque_fifo_test_and_set_fl( &edge->fifo, CAN_FIFOF_DEAD );
    }
    rtems_mutex_unlock( &outends->ends_lock );
    rtems_mutex_unlock( &inends->ends_lock );
  } else {
    rtems_mutex_lock( &outends->ends_lock );
    if ( outends != inends ) {
      rtems_mutex_lock( &inends->ends_lock );
    }
    if ( atomic_fetch_sub( &edge->edge_used, 1 ) == 1 ) {
      dead_fl = !canque_fifo_test_and_set_fl( &edge->fifo, CAN_FIFOF_DEAD );
    }
    if ( outends != inends ) {
      rtems_mutex_unlock( &inends->ends_lock );
    }
    rtems_mutex_unlock( &outends->ends_lock );
  }
 
  if ( dead_fl ) {
    canque_edge_do_dead( edge );
  }
}
 
static inline void canque_edge_decref( struct canque_edge_t *edge )
{
  unsigned int x;
 
  x = atomic_load_explicit( &edge->edge_used, memory_order_relaxed );
 
  do {
    if ( x <= 1 ) {
      return __canque_edge_decref( edge );
    }
  } while( !atomic_compare_exchange_strong(&edge->edge_used, &x, x-1 ) );
}
 
static inline struct canque_edge_t *canque_first_inedge(
  struct canque_ends_t *qends
)
{
  struct canque_edge_t *edge;
 
  rtems_mutex_lock( &qends->ends_lock );
  edge = TAILQ_FIRST( &qends->inlist );
 
skip_dead:
  if ( edge != NULL ) {
    if ( canque_fifo_test_fl( &edge->fifo, CAN_FIFOF_DEAD ) ) {
      edge = TAILQ_NEXT( edge, inpeers );
      goto skip_dead;
    }
    canque_edge_incref( edge );
  } else {
    edge = NULL;
  }
  rtems_mutex_unlock( &qends->ends_lock );
  return edge;
}
 
 
static inline struct canque_edge_t *canque_next_inedge(
  struct canque_ends_t *qends,
  struct canque_edge_t *edge
)
{
  struct canque_edge_t *next;
 
  rtems_mutex_lock( &qends->ends_lock );
  next = TAILQ_NEXT( edge, inpeers );
 
skip_dead:
  if ( next != NULL ) {
    if ( canque_fifo_test_fl( &edge->fifo,CAN_FIFOF_DEAD ) ) {
      next = TAILQ_NEXT( next, inpeers );
      goto skip_dead;
    }
    canque_edge_incref( next );
  } else {
    next = NULL;
  }
  rtems_mutex_unlock( &qends->ends_lock );
  canque_edge_decref( edge );
  return next;
}
 
#define canque_for_each_inedge( qends, edge ) \
  for ( edge = canque_first_inedge( qends ); edge; edge = canque_next_inedge( qends, edge ) )
 
static inline
struct canque_edge_t *canque_first_outedge( struct canque_ends_t *qends )
{
  struct canque_edge_t *edge;
 
  rtems_mutex_lock( &qends->ends_lock );
  edge = TAILQ_FIRST( &qends->outlist );
 
skip_dead:
  if ( edge != NULL ) {
    if ( canque_fifo_test_fl( &edge->fifo, CAN_FIFOF_DEAD ) ) {
      edge = TAILQ_NEXT( edge, outpeers );
      goto skip_dead;
    }
    canque_edge_incref( edge );
  } else {
    edge = NULL;
  }
  rtems_mutex_unlock( &qends->ends_lock );
  return edge;
}
 
 
static inline
struct canque_edge_t *canque_next_outedge(
  struct canque_ends_t *qends,
  struct canque_edge_t *edge
)
{
  struct canque_edge_t *next;
 
  rtems_mutex_lock( &qends->ends_lock );
  next = TAILQ_NEXT( edge, outpeers );
 
skip_dead:
  if ( next != NULL ) {
    if ( canque_fifo_test_fl( &edge->fifo,CAN_FIFOF_DEAD ) ) {
      next = TAILQ_NEXT( next, outpeers );
      goto skip_dead;
    }
    canque_edge_incref( next );
  } else {
    next = NULL;
  }
  rtems_mutex_unlock( &qends->ends_lock );
  canque_edge_decref( edge );
  return next;
}
 
#define canque_for_each_outedge( qends, edge ) \
  for ( edge = canque_first_outedge( qends ); edge; edge = canque_next_outedge( qends, edge ) )
 
/* Linux kernel specific functions */
 
int canque_fifo_init_kern(
  struct canque_fifo_t *fifo,
  int slotsnr,
  int maxdlen
);
 
int canque_fifo_done_kern( struct canque_fifo_t *fifo );
 
struct canque_edge_t *canque_new_edge_kern( int slotsnrm, int maxdlen );
 
int canqueue_ends_sync_all_kern(
  struct canque_ends_t *qends,
  struct timespec *ts
);
 
int canque_sync_wait_kern(
  struct canque_ends_t *qends,
  struct canque_edge_t *qedge,
  bool nowait,
  rtems_interval timeout
);
 
int canqueue_ends_dispose_kern( struct canque_ends_t *qends, bool nonblock );
int canqueue_kern_initialize( void );
 
#endif /*_CAN_QUEUE_H*/