git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@1073 35acf78f-673a-0410-8e92-d51de3d6d3f4

1 files changed, 304 insertions, 0 deletions

diff --git a/os/kernel/src/chqueues.c b/os/kernel/src/chqueues.c
new file mode 100644
index 000000000..a72b83696
--- /dev/null
+++ b/os/kernel/src/chqueues.c
@@ -0,0 +1,304 @@
+/*

+    ChibiOS/RT - Copyright (C) 2006-2007 Giovanni Di Sirio.

+

+    This file is part of ChibiOS/RT.

+

+    ChibiOS/RT is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS/RT is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+*/

+

+/**

+ * @file chqueues.c

+ * @brief I/O Queues code.

+ * @addtogroup IOQueues

+ * @{

+ */

+

+#include <ch.h>

+

+#if CH_USE_QUEUES

+

+/**

+ * @brief Initializes an input queue.

+ * @details A Semaphore is internally initialized and works as a counter of

+ *          the bytes contained in the queue.

+ *

+ * @param[out] iqp pointer to an @p InputQueue structure

+ * @param[in] buffer pointer to a memory area allocated as queue buffer

+ * @param[in] size size of the queue buffer

+ * @param[in] inotify pointer to a callback function that is invoked when

+ *                    some data is read from the queue. The value can be

+ *                    @p NULL.

+ *

+ * @note The callback is invoked from within the S-Locked system state,

+ *       see @ref system_states.

+ */

+void chIQInit(InputQueue *iqp, uint8_t *buffer,

+              size_t size, qnotify_t inotify) {

+

+  iqp->q_buffer = iqp->q_rdptr = iqp->q_wrptr = buffer;

+  iqp->q_top = buffer + size;

+  chSemInit(&iqp->q_sem, 0);

+  iqp->q_notify = inotify;

+}

+

+/**

+ * @brief Resets an input queue.

+ * @details All the data in the input queue is erased and lost, any waiting

+ *          thread is resumed with status @p Q_RESET.

+ *

+ * @param[in] iqp pointer to an @p InputQueue structure

+ *

+ * @note A reset operation can be used by a low level driver in order to obtain

+ *       immediate attention from the high level layers.

+ */

+void chIQResetI(InputQueue *iqp) {

+

+  iqp->q_rdptr = iqp->q_wrptr = iqp->q_buffer;

+  chSemResetI(&iqp->q_sem, 0);

+}

+

+/**

+ * @brief Input queue write.

+ * @details A byte value is written into the low end of an input queue.

+ *

+ * @param[in] iqp pointer to an @p InputQueue structure

+ * @param[in] b the byte value to be written in the queue

+ * @return The operation status, it can be one of:

+ * @retval Q_OK if the operation has been completed with success.

+ * @retval Q_FULL if the queue is full and the operation cannot be completed.

+ */

+msg_t chIQPutI(InputQueue *iqp, uint8_t b) {

+

+  if (chIQIsFull(iqp))

+    return Q_FULL;

+

+  *iqp->q_wrptr++ = b;

+  if (iqp->q_wrptr >= iqp->q_top)

+    iqp->q_wrptr = iqp->q_buffer;

+  chSemSignalI(&iqp->q_sem);

+  return Q_OK;

+}

+

+/**

+ * @brief Input queue read with timeout.

+ * @details This function reads a byte value from an input queue. If the queue

+ *          is empty then the calling thread is suspended until a byte arrives

+ *          in the queue or a timeout occurs.

+ *

+ * @param[in] iqp pointer to an @p InputQueue structure

+ * @param[in] timeout the number of ticks before the operation timeouts,

+ *             the following special values are allowed:

+ *             - @a TIME_IMMEDIATE immediate timeout.

+ *             - @a TIME_INFINITE no timeout.

+ *             .

+ * @return A byte value from the queue or:

+ * @retval Q_TIMEOUT if the specified time expired.

+ * @retval Q_RESET if the queue was reset.

+ */

+msg_t chIQGetTimeout(InputQueue *iqp, systime_t timeout) {

+  uint8_t b;

+  msg_t msg;

+

+  chSysLock();

+  if ((msg = chSemWaitTimeoutS(&iqp->q_sem, timeout)) < RDY_OK) {

+    chSysUnlock();

+    return msg;

+  }

+  b = *iqp->q_rdptr++;

+  if (iqp->q_rdptr >= iqp->q_top)

+    iqp->q_rdptr = iqp->q_buffer;

+

+  if (iqp->q_notify)

+    iqp->q_notify();

+

+  chSysUnlock();

+  return b;

+}

+

+/**

+ * @brief Non-blocking read.

+ * @details The function reads data from an input queue into a buffer. The

+ *          transfer is non-blocking and can return zero if the queue is

+ *          empty.

+ *

+ * @param[in] iqp pointer to an @p InputQueue structure

+ * @param[out] buffer pointer to the buffer where the input data is copied

+ * @param[in] n the maximum amount of data to be transferred

+ * @return The number of bytes transferred.

+ *

+ * @note The function is not atomic, if you need atomicity it is suggested

+ *       to use a semaphore or a mutex for mutual exclusion.

+ */

+size_t chIQRead(InputQueue *iqp, uint8_t *buffer, size_t n) {

+  size_t r = 0;

+

+  while (n--) {

+    chSysLock();

+    if (chIQIsEmpty(iqp)) {

+      chSysUnlock();

+      break;

+    }

+    chSemFastWaitI(&iqp->q_sem);

+    *buffer++ = *iqp->q_rdptr++;

+    if (iqp->q_rdptr >= iqp->q_top)

+      iqp->q_rdptr = iqp->q_buffer;

+    chSysUnlock();

+    r++;

+  }

+  if (r && iqp->q_notify) {

+    chSysLock();

+    iqp->q_notify();

+    chSysUnlock();

+  }

+  return r;

+}

+

+/**

+ * @brief Initializes an output queue.

+ * @details A Semaphore is internally initialized and works as a counter of

+ *          the free bytes in the queue.

+ *

+ * @param[out] oqp pointer to an @p OutputQueue structure

+ * @param[in] buffer pointer to a memory area allocated as queue buffer

+ * @param[in] size size of the queue buffer

+ * @param[in] onotify pointer to a callback function that is invoked when

+ *                    some data is written to the queue. The value can be

+ *                    @p NULL.

+ *

+ * @note The callback is invoked from within the S-Locked system state,

+ *       see @ref system_states.

+ */

+void chOQInit(OutputQueue *oqp, uint8_t *buffer,

+              size_t size, qnotify_t onotify) {

+

+  oqp->q_buffer = oqp->q_rdptr = oqp->q_wrptr = buffer;

+  oqp->q_top = buffer + size;

+  chSemInit(&oqp->q_sem, size);

+  oqp->q_notify = onotify;

+}

+

+/**

+ * @brief Resets an output queue.

+ * @details All the data in the output queue is erased and lost, any waiting

+ *          thread is resumed with status @p Q_RESET.

+ *

+ * @param[in] oqp pointer to an @p OutputQueue structure

+ *

+ * @note A reset operation can be used by a low level driver in order to obtain

+ *       immediate attention from the high level layers.

+ */

+void chOQResetI(OutputQueue *oqp) {

+

+  oqp->q_rdptr = oqp->q_wrptr = oqp->q_buffer;

+  chSemResetI(&oqp->q_sem, (cnt_t)(oqp->q_top - oqp->q_buffer));

+}

+

+/**

+ * @brief Output queue write with timeout.

+ * @details This function writes a byte value to an output queue. If the queue

+ *          is full then the calling thread is suspended until there is space

+ *          in the queue or a timeout occurs.

+ *

+ * @param[in] oqp pointer to an @p OutputQueue structure

+ * @param[in] b the byte value to be written in the queue

+ * @param[in] timeout the number of ticks before the operation timeouts,

+ *             the following special values are allowed:

+ *             - @a TIME_IMMEDIATE immediate timeout.

+ *             - @a TIME_INFINITE no timeout.

+ *             .

+ * @return The operation status:

+ * @retval Q_OK if the operation succeeded.

+ * @retval Q_TIMEOUT if the specified time expired.

+ * @retval Q_RESET if the queue was reset.

+ */

+msg_t chOQPutTimeout(OutputQueue *oqp, uint8_t b, systime_t timeout) {

+  msg_t msg;

+

+  chSysLock();

+  if ((msg = chSemWaitTimeoutS(&oqp->q_sem, timeout)) < RDY_OK) {

+    chSysUnlock();

+    return msg;

+  }

+  *oqp->q_wrptr++ = b;

+  if (oqp->q_wrptr >= oqp->q_top)

+    oqp->q_wrptr = oqp->q_buffer;

+

+  if (oqp->q_notify)

+    oqp->q_notify();

+

+  chSysUnlock();

+  return Q_OK;

+}

+

+/**

+ * @brief Output queue read.

+ * @details A byte value is read from the low end of an output queue.

+ *

+ * @param[in] oqp pointer to an @p OutputQueue structure

+ * @return The byte value from the queue or:

+ * @retval Q_EMPTY if the queue is empty.

+ */

+msg_t chOQGetI(OutputQueue *oqp) {

+  uint8_t b;

+

+  if (chOQIsEmpty(oqp))

+    return Q_EMPTY;

+

+  b = *oqp->q_rdptr++;

+  if (oqp->q_rdptr >= oqp->q_top)

+    oqp->q_rdptr = oqp->q_buffer;

+  chSemSignalI(&oqp->q_sem);

+  return b;

+}

+

+/**

+ * @brief Non-blocking write.

+ * @details The function writes data from a buffer to an output queue. The

+ *          transfer is non-blocking and can return zero if the queue is

+ *          already full.

+ *

+ * @param[in] oqp pointer to an @p OutputQueue structure

+ * @param[out] buffer pointer to the buffer where the output data is stored

+ * @param[in] n the maximum amount of data to be transferred

+ * @return The number of bytes transferred.

+ *

+ * @note The function is not atomic, if you need atomicity it is suggested

+ *       to use a semaphore or a mutex for mutual exclusion.

+ */

+size_t chOQWrite(OutputQueue *oqp, uint8_t *buffer, size_t n) {

+

+  size_t w = 0;

+  while (n--) {

+    chSysLock();

+    if (chOQIsFull(oqp)) {

+      chSysUnlock();

+      break;

+    }

+    chSemFastWaitI(&oqp->q_sem);

+    *oqp->q_wrptr++ = *buffer++;

+    if (oqp->q_wrptr >= oqp->q_top)

+      oqp->q_wrptr = oqp->q_buffer;

+    chSysUnlock();

+    w++;

+  }

+  if (w && oqp->q_notify) {

+    chSysLock();

+    oqp->q_notify();

+    chSysUnlock();

+  }

+  return w;

+}

+#endif  /* CH_USE_QUEUES */

+

+/** @} */