packages/kernel/v2_0/include/mqueue.inl

   1 #ifndef CYGONCE_KERNEL_MQUEUE_INL
   2 #define CYGONCE_KERNEL_MQUEUE_INL
   3 /*========================================================================
   4 //
   5 //      mqueue.inl
   6 //
   7 //      Message queues implementation
   8 //
   9 //========================================================================
  10 //####ECOSGPLCOPYRIGHTBEGIN####
  11 // -------------------------------------------
  12 // This file is part of eCos, the Embedded Configurable Operating System.
  13 // Copyright (C) 1998, 1999, 2000, 2001, 2002 Red Hat, Inc.
  14 //
  15 // eCos is free software; you can redistribute it and/or modify it under
  16 // the terms of the GNU General Public License as published by the Free
  17 // Software Foundation; either version 2 or (at your option) any later version.
  18 //
  19 // eCos is distributed in the hope that it will be useful, but WITHOUT ANY
  20 // WARRANTY; without even the implied warranty of MERCHANTABILITY or
  21 // FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
  22 // for more details.
  23 //
  24 // You should have received a copy of the GNU General Public License along
  25 // with eCos; if not, write to the Free Software Foundation, Inc.,
  26 // 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  27 //
  28 // As a special exception, if other files instantiate templates or use macros
  29 // or inline functions from this file, or you compile this file and link it
  30 // with other works to produce a work based on this file, this file does not
  31 // by itself cause the resulting work to be covered by the GNU General Public
  32 // License. However the source code for this file must still be made available
  33 // in accordance with section (3) of the GNU General Public License.
  34 //
  35 // This exception does not invalidate any other reasons why a work based on
  36 // this file might be covered by the GNU General Public License.
  37 //
  38 // Alternative licenses for eCos may be arranged by contacting Red Hat, Inc.
  39 // at http://sources.redhat.com/ecos/ecos-license/
  40 // -------------------------------------------
  41 //####ECOSGPLCOPYRIGHTEND####
  42 //========================================================================
  43 //#####DESCRIPTIONBEGIN####
  44 //
  45 // Author(s):     jlarmour
  46 // Contributors:
  47 // Date:          2000-05-09
  48 // Purpose:       This file provides the implementation for eCos message
  49 //                queues
  50 // Description:   This differs from the message boxes also supported
  51 //                by eCos primarily because the requirements of message
  52 //                queues are driven by POSIX semantics. POSIX semantics are
  53 //                more dynamic and therefore heavyweight than Mboxes,
  54 //                including prioritization, and variable sized queues and
  55 //                message lengths
  56 // Usage:         Do not include this file directly - instead
  57 //                #include <cyg/kernel/mqueue.hxx>
  58 //
  59 //####DESCRIPTIONEND####
  60 //
  61 //======================================================================
  62 */
  63
  64 /* CONFIGURATION */
  65
  66 #include <pkgconf/system.h>
  67 #include <pkgconf/kernel.h>          // Configuration header
  68
  69 /* INCLUDES */
  70
  71 #include <stddef.h>                  // size_t, NULL
  72 #include <cyg/infra/cyg_type.h>      // Types
  73 #include <cyg/kernel/mqueue.hxx>     // Header for this file, just in case
  74 #include <cyg/infra/cyg_ass.h>       // Assertion support
  75 #include <cyg/infra/cyg_trac.h>      // Tracing support
  76 #include <cyg/kernel/sched.hxx>      // scheduler
  77 #include <cyg/kernel/sched.inl>      // scheduler inlines
  78 #include <cyg/kernel/sema.hxx>       // Cyg_Counting_Semaphore
  79
  80 #ifdef CYGPKG_ISOINFRA
  81 # include <string.h>                 // memcpy
  82 #else
  83 externC void * memcpy( void *, const void *, size_t );
  84 #endif
  85
  86 // NOTE:
  87 // An alternative implementation based on mutexes and condition variables
  88 // rather than semaphores/scheduler locking was considered. But it was
  89 // not thought quite as good because it isn't driver safe. You would
  90 // also have to manage explicitly what counting semaphores do for you
  91 // intrinsically. Also with the mutex approach, the message queue would
  92 // be locked the whole time a new entry was being filled in, or copied out
  93 //
  94 // It also makes the non-blocking case properly non-blocking rather than
  95 // still being able to block while waiting for a mutex protecting
  96 // the message queue internal structures
  97
  98 /* INLINE FUNCTIONS */
  99
 100 #ifndef CYGPRI_KERNEL_SYNCH_MQUEUE_INLINE
 101 # define CYGPRI_KERNEL_SYNCH_MQUEUE_INLINE inline
 102 #endif
 103
 104 //------------------------------------------------------------------------
 105
 106 CYGPRI_KERNEL_SYNCH_MQUEUE_INLINE cyg_bool
 107 Cyg_Mqueue::check_this( cyg_assert_class_zeal zeal ) const
 108 {
 109     if (zeal != cyg_none) {
 110         CYG_CHECK_DATA_PTRC(this);  // extreme paranoia
 111
 112 #ifdef CYGDBG_USE_ASSERTS
 113         if ( qlen <= 0 || msgsize <= 0 )
 114             return false;
 115 #endif
 116
 117         if ( queuespacesize < sizeof(struct qentry)+1 )
 118             return false;
 119
 120         CYG_CHECK_DATA_PTRC(queuespace);
 121         CYG_CHECK_FUNC_PTRC(free_fn);
 122
 123         // prevent pre-emption through this. Not so bad since
 124         // this is only a diagnostic function
 125         Cyg_Scheduler::lock();
 126
 127         if (NULL != q)
 128             CYG_CHECK_DATA_PTRC(q);
 129         if (NULL != freelist)
 130             CYG_CHECK_DATA_PTRC(freelist);
 131         if (NULL != callback)
 132             CYG_CHECK_FUNC_PTRC(callback);
 133
 134         // check each queue entry
 135         long msgs=0, busymsgs=0;
 136         unsigned int oldprio=0;
 137         struct qentry *qtmp;
 138
 139         if ( NULL != q )
 140             oldprio = q->priority;
 141         for ( qtmp=q; NULL != qtmp; qtmp=qtmp->next ) {
 142             if ( NULL != qtmp->next )
 143                 CYG_CHECK_DATA_PTRC( qtmp->next );
 144
 145             // queue should be priority ordered
 146             if ( qtmp->priority > oldprio )
 147                 goto fail;
 148             oldprio = qtmp->priority;
 149
 150 #ifdef CYGDBG_USE_ASSERTS
 151             // valid length
 152             if ( !qtmp->busy )
 153                 if ( qtmp->buflen > msgsize )
 154                     goto fail;
 155 #endif
 156             if ( qtmp->busy )
 157                 busymsgs++;
 158             else
 159                 msgs++;
 160         } // for
 161
 162         long freemsgs=0;
 163
 164         // check that number of used and unused messages == q length
 165         for ( qtmp=freelist; NULL != qtmp; qtmp=qtmp->next ) {
 166             if ( NULL != qtmp->next )
 167                 CYG_CHECK_DATA_PTRC( qtmp->next );
 168             if ( qtmp->busy )
 169                 busymsgs++;
 170             else
 171                 freemsgs++;
 172         }
 173
 174 #ifdef CYGDBG_USE_ASSERTS
 175         // and sum of all messages should be the total q length
 176         if ( qlen != (msgs+freemsgs+busymsgs) )
 177             goto fail;
 178 #endif
 179
 180         Cyg_Scheduler::unlock();
 181
 182     }
 183     return true; // object OK
 184  fail:
 185     Cyg_Scheduler::unlock();
 186     return false; // object fubar'd
 187 }
 188
 189 //------------------------------------------------------------------------
 190
 191 CYGPRI_KERNEL_SYNCH_MQUEUE_INLINE
 192 Cyg_Mqueue::Cyg_Mqueue( long maxmsgs, long maxmsgsize,
 193                         qalloc_fn_t qalloc, qfree_fn_t qfree, qerr_t *err )
 194     : putsem(maxmsgs), getsem(0)
 195 {
 196     CYG_REPORT_FUNCTION();
 197     CYG_REPORT_FUNCARG5( "maxmsgs=%ld, maxmsgsize=%ld, qalloc=%08x, "
 198                          "qfree=%08x, &err=%08x", maxmsgs, maxmsgsize,
 199                          qalloc, qfree, err);
 200     CYG_PRECONDITIONC( (maxmsgs > 0) && (maxmsgsize > 0) );
 201     CYG_CHECK_DATA_PTRC( err );
 202     CYG_CHECK_FUNC_PTRC( qalloc );
 203     CYG_CHECK_FUNC_PTRC( qfree );
 204
 205     // mem to allocate for entire queue size. Also wants to be rounded
 206     // up so that the structs are aligned.
 207     const long addralign = sizeof(void *) - 1;
 208     long entrysize = (sizeof(struct qentry) + maxmsgsize + addralign)
 209        & ~addralign;
 210
 211     queuespacesize = entrysize * maxmsgs;
 212     queuespace = qalloc( queuespacesize );
 213
 214     if (NULL == queuespace) {
 215         *err=NOMEM;
 216         CYG_REPORT_RETURN();
 217         return;
 218     }
 219
 220     // link up freelist
 221     long i;
 222     struct qentry *qtmp;
 223     for ( i=0, qtmp=(struct qentry *)queuespace;
 224           i<maxmsgs-1;
 225           i++, qtmp=qtmp->next ) {
 226         qtmp->busy = false;
 227         qtmp->next = (struct qentry *)((char *)qtmp + entrysize);
 228     } // for
 229
 230     freelist   = (struct qentry *)queuespace;
 231
 232     // set the last entry in the chain to the start to make the list circular
 233     qtmp->next = NULL;
 234     qtmp->busy = false;
 235     callback   = NULL;
 236     q          = NULL;
 237     free_fn    = qfree;
 238 #ifdef CYGDBG_USE_ASSERTS
 239     qlen       = maxmsgs;
 240     msgsize    = maxmsgsize;
 241 #endif
 242
 243     *err = OK;
 244
 245     // object should be valid now
 246     CYG_ASSERT_THISC();
 247
 248     CYG_REPORT_RETURN();
 249 }
 250
 251 //------------------------------------------------------------------------
 252
 253 CYGPRI_KERNEL_SYNCH_MQUEUE_INLINE
 254 Cyg_Mqueue::~Cyg_Mqueue()
 255 {
 256     CYG_REPORT_FUNCTION();
 257
 258     if ( NULL != queuespace ) {
 259         // object should be valid if queuespace was successfully allocated
 260         CYG_ASSERT_THISC();
 261         free_fn( queuespace, queuespacesize );
 262     }
 263
 264 #ifdef CYGDBG_USE_ASSERTS
 265     qlen = msgsize = 0; // deliberately make it fail check_this() if used
 266 #endif
 267
 268     CYG_REPORT_RETURN();
 269 }
 270
 271 //------------------------------------------------------------------------
 272
 273 // put() copies len bytes of *buf into the queue at priority prio
 274 CYGPRI_KERNEL_SYNCH_MQUEUE_INLINE Cyg_Mqueue::qerr_t
 275 Cyg_Mqueue::put( const char *buf, size_t len, unsigned int prio, bool block
 276 #ifdef CYGFUN_KERNEL_THREADS_TIMER
 277                  , cyg_tick_count timeout
 278 #endif
 279                )
 280 {
 281     CYG_REPORT_FUNCTYPE( "err=%d");
 282     CYG_REPORT_FUNCARG4( "buf=%08x, len=%ld, prio=%ud, block=%d",
 283                          buf, len, prio, block==true );
 284     CYG_CHECK_DATA_PTRC( buf );
 285     CYG_ASSERT_THISC();
 286     CYG_PRECONDITIONC( len <= (size_t)msgsize );
 287
 288     qerr_t err;
 289     struct qentry *qtmp, *qent;
 290
 291     // wait till a freelist entry is available
 292     if ( true == block ) {
 293 #ifdef CYGFUN_KERNEL_THREADS_TIMER
 294         if ( timeout != 0) {
 295             if ( false == putsem.wait(timeout) ) {
 296                 err = TIMEOUT;
 297                 goto exit;
 298             }
 299         }
 300         else
 301 #endif
 302         if ( false == putsem.wait() ) {
 303             err = INTR;
 304             goto exit;
 305         }
 306     } else {
 307         if ( false == putsem.trywait() ) {
 308             err = WOULDBLOCK;
 309             goto exit;
 310         }
 311     }
 312
 313     // prevent preemption when fiddling with important members
 314     Cyg_Scheduler::lock();
 315
 316     CYG_ASSERT_THISC();
 317
 318     // get a queue entry from the freelist
 319     // don't need to check the freelist - the semaphore tells us there's
 320     // definitely a usable non-busy one there. It's just a question of
 321     // locating it.
 322
 323     if (!freelist->busy) { // fast-track common case
 324         qent     = freelist;
 325         freelist = freelist->next;
 326     } else {
 327         for ( qtmp=freelist; qtmp->next->busy; qtmp=qtmp->next )
 328             CYG_EMPTY_STATEMENT; // skip through
 329         qent       = qtmp->next;
 330         qtmp->next = qent->next;
 331     }
 332
 333     // now put it in place in q
 334
 335     if ( NULL == q ) {
 336         q = qent;
 337         q->next = NULL;
 338     } else {
 339         struct qentry **qentp;
 340
 341         // insert into queue according to prio
 342         for ( qentp=&q; NULL != *qentp; qentp = &((*qentp)->next) ) {
 343             if ((*qentp)->priority < prio)
 344                 break;
 345         } // for
 346
 347         qent->next = *qentp;
 348         *qentp = qent;
 349     } // else
 350
 351     qent->priority = prio; // have to set this now so when the sched is
 352                            // unlocked, other qent's can be added in the
 353                            // right place
 354     qent->busy = true; // let things know this entry should be ignored until
 355                        // it's finished having its data copied
 356
 357     // unlock the scheduler, and potentially switch threads, but
 358     // that's okay now. We don't want it locked for the expensive memcpy
 359     Cyg_Scheduler::unlock();
 360
 361     qent->buflen   = len;
 362     memcpy( qent->buf(), buf, len );
 363
 364     // make available now - setting non-atomically is alright if you think
 365     // about it - the only thing that matters is that it's completed before
 366     // the post()
 367     qent->busy = false;
 368
 369     // if we have to notify someone, we only do it if no-one's already
 370     // sitting waiting for a message to appear, AND if it's a transition
 371     // from empty to non-empty
 372
 373     if ( callback != NULL && !getsem.waiting() && (0 == getsem.peek()) ) {
 374         getsem.post();
 375         callback( *this, callback_data );
 376     } else
 377         getsem.post();
 378
 379     err = OK;
 380
 381  exit:
 382     CYG_ASSERT_THISC();
 383     CYG_REPORT_RETVAL(err);
 384     return err;
 385 } // Cyg_Mqueue::put()
 386
 387 //------------------------------------------------------------------------
 388
 389
 390 // get() returns the oldest highest priority message in the queue in *buf
 391 // and sets *prio to the priority (if prio is non-NULL) and *len to the
 392 // actual message size
 393
 394 CYGPRI_KERNEL_SYNCH_MQUEUE_INLINE Cyg_Mqueue::qerr_t
 395 Cyg_Mqueue::get( char *buf, size_t *len, unsigned int *prio, bool block
 396 #ifdef CYGFUN_KERNEL_THREADS_TIMER
 397                  , cyg_tick_count timeout
 398 #endif
 399                )
 400 {
 401     CYG_REPORT_FUNCTYPE( "err=%d");
 402     CYG_REPORT_FUNCARG4( "buf=%08x, len=%08x, prio=%08x, block=%d",
 403                          buf, len, prio, block==true );
 404     CYG_CHECK_DATA_PTRC( buf );
 405     CYG_CHECK_DATA_PTRC( len );
 406     if ( NULL != prio )
 407         CYG_CHECK_DATA_PTRC( prio );
 408     CYG_ASSERT_THISC();
 409
 410     qerr_t err;
 411     struct qentry *qent;
 412
 413     // wait till a q entry is available
 414     if ( true == block ) {
 415 #ifdef CYGFUN_KERNEL_THREADS_TIMER
 416         if ( timeout != 0) {
 417             if ( false == getsem.wait(timeout) ) {
 418                 err = TIMEOUT;
 419                 goto exit;
 420             }
 421         }
 422         else
 423 #endif
 424         if ( false == getsem.wait() ) {
 425             err = INTR;
 426             goto exit;
 427         }
 428     } else {
 429         if ( false == getsem.trywait() ) {
 430             err = WOULDBLOCK;
 431             goto exit;
 432         }
 433     }
 434
 435     // prevent preemption when fiddling with important members
 436
 437     Cyg_Scheduler::lock();
 438
 439     // don't need to check the q - the semaphore tells us there's
 440     // definitely a usable non-busy one there. It's just a question of
 441     // locating it.
 442
 443     if ( !q->busy ) {   // fast-track the common case
 444         qent       = q;
 445         q          = qent->next;
 446     } else {
 447         struct qentry *qtmp;
 448
 449         for ( qtmp=q; qtmp->next->busy; qtmp=qtmp->next )
 450             CYG_EMPTY_STATEMENT; // skip through
 451
 452         qent = qtmp->next;
 453         qtmp->next = qent->next;
 454     } // else
 455
 456     // now stick at front of freelist, but marked busy
 457     qent->next = freelist;
 458     freelist   = qent;
 459
 460     qent->busy = true; // don't let it truly be part of the freelist just yet
 461                        // till the data is copied out
 462
 463     // unlock the scheduler, and potentially switch threads, but
 464     // that's okay now. We don't want it locked for the expensive memcpy
 465     Cyg_Scheduler::unlock();
 466
 467     *len  = qent->buflen;
 468     if ( NULL != prio )
 469         *prio = qent->priority;
 470     memcpy( buf, qent->buf(), *len );
 471
 472     // make available now - setting non-atomically is alright if you think
 473     // about it - the only thing that matters is that it's completed before
 474     // the post()
 475     qent->busy = false;
 476
 477     putsem.post();
 478
 479     err = OK;
 480
 481  exit:
 482     CYG_ASSERT_THISC();
 483     CYG_REPORT_RETVAL(err);
 484     return err;
 485
 486 } // Cyg_Mqueue::get()
 487
 488 //------------------------------------------------------------------------
 489
 490 // count() returns the number of messages in the queue
 491 inline long
 492 Cyg_Mqueue::count()
 493 {
 494     CYG_REPORT_FUNCTYPE("curmsgs=%d");
 495
 496     long curmsgs = (long)getsem.peek();
 497
 498     CYG_REPORT_RETVAL(curmsgs);
 499     return curmsgs;
 500 } // Cyg_Mqueue::count()
 501
 502 //------------------------------------------------------------------------
 503
 504
 505 // Supply a callback function to call (with the supplied data argument)
 506 // when the queue goes from empty to non-empty (unless someone's already
 507 // doing a get()). This returns the old callback_fn, and if olddata is
 508 // non-NULL sets it to the old data (yes, really!)
 509 CYGPRI_KERNEL_SYNCH_MQUEUE_INLINE Cyg_Mqueue::callback_fn_t
 510 Cyg_Mqueue::setnotify( callback_fn_t callback_fn, CYG_ADDRWORD data,
 511                        CYG_ADDRWORD *olddata)
 512 {
 513     CYG_REPORT_FUNCTYPE("old callback=%08x");
 514     CYG_REPORT_FUNCARG3XV( callback_fn, data, olddata );
 515     if ( NULL != callback_fn )
 516         CYG_CHECK_FUNC_PTRC( callback_fn );
 517     if (NULL != olddata)
 518         CYG_CHECK_DATA_PTRC( olddata );
 519
 520     callback_fn_t oldfn;
 521
 522     // Need to prevent preemption for accessing common structures
 523     // Just locking the scheduler has the least overhead
 524     Cyg_Scheduler::lock();
 525
 526     oldfn = callback;
 527     if (NULL != olddata)
 528         *olddata = callback_data;
 529
 530     callback_data = data;
 531     callback      = callback_fn;
 532
 533     Cyg_Scheduler::unlock();
 534
 535     CYG_REPORT_RETVAL(oldfn);
 536     return oldfn;
 537 }
 538
 539 //------------------------------------------------------------------------
 540
 541 #endif /* CYGONCE_KERNEL_MQUEUE_INL multiple inclusion protection */
 542
 543 /* EOF mqueue.inl */