score/src/semaphore.c

0001 /* SPDX-License-Identifier: BSD-2-Clause */
0002
0003 /**
0004  * @file
0005  *
0006  * @ingroup RTEMSScore
0007  *
0008  * @brief This source file contains the implementation of
0009  *   _Semaphore_Wait(), _Semaphore_Wait_timed_ticks(), _Semaphore_Try_wait(),
0010  *   _Semaphore_Post(), and _Semaphore_Post_binary().
0011  */
0012
0013 /*
0014  * Copyright (C) 2015, 2017 embedded brains GmbH & Co. KG
0015  *
0016  * Redistribution and use in source and binary forms, with or without
0017  * modification, are permitted provided that the following conditions
0018  * are met:
0019  * 1. Redistributions of source code must retain the above copyright
0020  *    notice, this list of conditions and the following disclaimer.
0021  * 2. Redistributions in binary form must reproduce the above copyright
0022  *    notice, this list of conditions and the following disclaimer in the
0023  *    documentation and/or other materials provided with the distribution.
0024  *
0025  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
0026  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
0027  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
0028  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
0029  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
0030  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
0031  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
0032  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
0033  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
0034  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
0035  * POSSIBILITY OF SUCH DAMAGE.
0036  */
0037
0038 #ifdef HAVE_CONFIG_H
0039 #include "config.h"
0040 #endif
0041
0042 #include <rtems/score/semaphoreimpl.h>
0043 #include <rtems/score/statesimpl.h>
0044 #include <rtems/score/threadimpl.h>
0045
0046 #include <errno.h>
0047
0048 RTEMS_STATIC_ASSERT(
0049   offsetof( Sem_Control, Queue )
0050     == offsetof( struct _Semaphore_Control, _Queue ),
0051   SEMAPHORE_CONTROL_QUEUE
0052 );
0053
0054 RTEMS_STATIC_ASSERT(
0055   offsetof( Sem_Control, count )
0056     == offsetof( struct _Semaphore_Control, _count ),
0057   SEMAPHORE_CONTROL_COUNT
0058 );
0059
0060 RTEMS_STATIC_ASSERT(
0061   sizeof( Sem_Control ) == sizeof( struct _Semaphore_Control ),
0062   SEMAPHORE_CONTROL_SIZE
0063 );
0064
0065 void _Semaphore_Wait( struct _Semaphore_Control *_sem )
0066 {
0067   Sem_Control          *sem;
0068   ISR_Level             level;
0069   Thread_queue_Context  queue_context;
0070   Thread_Control       *executing;
0071   unsigned int          count;
0072
0073   sem = _Sem_Get( _sem );
0074   _Thread_queue_Context_initialize( &queue_context );
0075   _Thread_queue_Context_ISR_disable( &queue_context, level );
0076   executing = _Sem_Queue_acquire_critical( sem, &queue_context );
0077
0078   count = sem->count;
0079   if ( RTEMS_PREDICT_TRUE( count > 0 ) ) {
0080     sem->count = count - 1;
0081     _Sem_Queue_release( sem, level, &queue_context );
0082   } else {
0083     _Thread_queue_Context_set_thread_state(
0084       &queue_context,
0085       STATES_WAITING_FOR_SEMAPHORE
0086     );
0087     _Thread_queue_Context_set_enqueue_do_nothing_extra( &queue_context );
0088     _Thread_queue_Context_set_ISR_level( &queue_context, level );
0089     _Thread_queue_Enqueue(
0090       &sem->Queue.Queue,
0091       SEMAPHORE_TQ_OPERATIONS,
0092       executing,
0093       &queue_context
0094     );
0095   }
0096 }
0097
0098 int _Semaphore_Wait_timed_ticks( struct _Semaphore_Control *_sem, uint32_t ticks )
0099 {
0100   Sem_Control          *sem;
0101   ISR_Level             level;
0102   Thread_queue_Context  queue_context;
0103   Thread_Control       *executing;
0104   unsigned int          count;
0105
0106   sem = _Sem_Get( _sem );
0107   _Thread_queue_Context_initialize( &queue_context );
0108   _Thread_queue_Context_ISR_disable( &queue_context, level );
0109   executing = _Sem_Queue_acquire_critical( sem, &queue_context );
0110
0111   count = sem->count;
0112   if ( RTEMS_PREDICT_TRUE( count > 0 ) ) {
0113     sem->count = count - 1;
0114     _Sem_Queue_release( sem, level, &queue_context );
0115     return 0;
0116   } else {
0117     _Thread_queue_Context_set_thread_state(
0118       &queue_context,
0119       STATES_WAITING_FOR_SEMAPHORE
0120     );
0121     _Thread_queue_Context_set_enqueue_timeout_ticks( &queue_context, ticks );
0122     _Thread_queue_Context_set_ISR_level( &queue_context, level );
0123     _Thread_queue_Enqueue(
0124       &sem->Queue.Queue,
0125       SEMAPHORE_TQ_OPERATIONS,
0126       executing,
0127       &queue_context
0128     );
0129     return STATUS_GET_POSIX( _Thread_Wait_get_status( executing ) );
0130   }
0131 }
0132
0133 int _Semaphore_Try_wait( struct _Semaphore_Control *_sem )
0134 {
0135   Sem_Control          *sem;
0136   ISR_Level             level;
0137   Thread_queue_Context  queue_context;
0138   unsigned int          count;
0139   int                   eno;
0140
0141   sem = _Sem_Get( _sem );
0142   _Thread_queue_Context_initialize( &queue_context );
0143   _Thread_queue_Context_ISR_disable( &queue_context, level );
0144   _Sem_Queue_acquire_critical( sem, &queue_context );
0145
0146   count = sem->count;
0147   if ( RTEMS_PREDICT_TRUE( count > 0 ) ) {
0148     sem->count = count - 1;
0149     eno = 0;
0150   } else {
0151     eno = EAGAIN;
0152   }
0153
0154   _Sem_Queue_release( sem, level, &queue_context );
0155   return eno;
0156 }
0157
0158 void _Semaphore_Post( struct _Semaphore_Control *_sem )
0159 {
0160   Sem_Control          *sem;
0161   ISR_Level             level;
0162   Thread_queue_Context  queue_context;
0163   Thread_queue_Heads   *heads;
0164
0165   sem = _Sem_Get( _sem );
0166   _Thread_queue_Context_initialize( &queue_context );
0167   _Thread_queue_Context_ISR_disable( &queue_context, level );
0168   _Sem_Queue_acquire_critical( sem, &queue_context );
0169
0170   heads = sem->Queue.Queue.heads;
0171   if ( RTEMS_PREDICT_TRUE( heads == NULL ) ) {
0172     ++sem->count;
0173     _Sem_Queue_release( sem, level, &queue_context );
0174   } else {
0175     _Thread_queue_Context_set_ISR_level( &queue_context, level );
0176     _Thread_queue_Surrender_no_priority(
0177       &sem->Queue.Queue,
0178       heads,
0179       &queue_context,
0180       SEMAPHORE_TQ_OPERATIONS
0181     );
0182   }
0183 }
0184
0185 void _Semaphore_Post_binary( struct _Semaphore_Control *_sem )
0186 {
0187   Sem_Control          *sem;
0188   ISR_Level             level;
0189   Thread_queue_Context  queue_context;
0190   Thread_queue_Heads   *heads;
0191
0192   sem = _Sem_Get( _sem );
0193   _Thread_queue_Context_initialize( &queue_context );
0194   _Thread_queue_Context_ISR_disable( &queue_context, level );
0195   _Sem_Queue_acquire_critical( sem, &queue_context );
0196
0197   heads = sem->Queue.Queue.heads;
0198   if ( RTEMS_PREDICT_TRUE( heads == NULL ) ) {
0199     sem->count = 1;
0200     _Sem_Queue_release( sem, level, &queue_context );
0201   } else {
0202     _Thread_queue_Context_set_ISR_level( &queue_context, level );
0203     _Thread_queue_Surrender_no_priority(
0204       &sem->Queue.Queue,
0205       heads,
0206       &queue_context,
0207       SEMAPHORE_TQ_OPERATIONS
0208     );
0209   }
0210 }