diff options
| -rw-r--r-- | os/hal/include/hal_queues.h | 83 | ||||
| -rw-r--r-- | os/hal/src/hal_queues.c | 118 |
2 files changed, 167 insertions, 34 deletions
diff --git a/os/hal/include/hal_queues.h b/os/hal/include/hal_queues.h index ffafc8805..300ccb18d 100644 --- a/os/hal/include/hal_queues.h +++ b/os/hal/include/hal_queues.h @@ -25,6 +25,10 @@ #ifndef HAL_QUEUES_H
#define HAL_QUEUES_H
+/*===========================================================================*/
+/* Driver constants. */
+/*===========================================================================*/
+
/**
* @name Queue functions returned status value
* @{
@@ -36,6 +40,18 @@ #define Q_FULL MSG_TIMEOUT /**< @brief Queue full, */
/** @} */
+/*===========================================================================*/
+/* Driver pre-compile time settings. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver data structures and types. */
+/*===========================================================================*/
+
/**
* @brief Type of a generic I/O queue structure.
*/
@@ -69,6 +85,34 @@ struct io_queue { };
/**
+ * @extends io_queue_t
+ *
+ * @brief Type of an input queue structure.
+ * @details This structure represents a generic asymmetrical input queue.
+ * Writing to the queue is non-blocking and can be performed from
+ * interrupt handlers or from within a kernel lock zone.
+ * Reading the queue can be a blocking operation and is supposed to
+ * be performed by a system thread.
+ */
+typedef io_queue_t input_queue_t;
+
+/**
+ * @extends io_queue_t
+ *
+ * @brief Type of an output queue structure.
+ * @details This structure represents a generic asymmetrical output queue.
+ * Reading from the queue is non-blocking and can be performed from
+ * interrupt handlers or from within a kernel lock zone.
+ * Writing the queue can be a blocking operation and is supposed to
+ * be performed by a system thread.
+ */
+typedef io_queue_t output_queue_t;
+
+/*===========================================================================*/
+/* Driver macros. */
+/*===========================================================================*/
+
+/**
* @name Macro Functions
* @{
*/
@@ -107,25 +151,8 @@ struct io_queue { * @special
*/
#define qGetLink(qp) ((qp)->q_link)
-/** @} */
/**
- * @extends io_queue_t
- *
- * @brief Type of an input queue structure.
- * @details This structure represents a generic asymmetrical input queue.
- * Writing to the queue is non-blocking and can be performed from
- * interrupt handlers or from within a kernel lock zone.
- * Reading the queue can be a blocking operation and is supposed to
- * be performed by a system thread.
- */
-typedef io_queue_t input_queue_t;
-
-/**
- * @name Macro Functions
- * @{
- */
-/**
* @brief Returns the filled space into an input queue.
*
* @param[in] iqp pointer to an @p input_queue_t structure
@@ -187,25 +214,8 @@ typedef io_queue_t input_queue_t; * @api
*/
#define iqGet(iqp) iqGetTimeout(iqp, TIME_INFINITE)
-/** @} */
-
-/**
- * @extends io_queue_t
- *
- * @brief Type of an output queue structure.
- * @details This structure represents a generic asymmetrical output queue.
- * Reading from the queue is non-blocking and can be performed from
- * interrupt handlers or from within a kernel lock zone.
- * Writing the queue can be a blocking operation and is supposed to
- * be performed by a system thread.
- */
-typedef io_queue_t output_queue_t;
/**
- * @name Macro Functions
- * @{
- */
-/**
* @brief Returns the filled space into an output queue.
*
* @param[in] oqp pointer to an @p output_queue_t structure
@@ -269,6 +279,11 @@ typedef io_queue_t output_queue_t; * @api
*/
#define oqPut(oqp, b) oqPutTimeout(oqp, b, TIME_INFINITE)
+/** @} */
+
+/*===========================================================================*/
+/* External declarations. */
+/*===========================================================================*/
/** @} */
#ifdef __cplusplus
diff --git a/os/hal/src/hal_queues.c b/os/hal/src/hal_queues.c index 66e8a89cd..a4f363541 100644 --- a/os/hal/src/hal_queues.c +++ b/os/hal/src/hal_queues.c @@ -35,8 +35,126 @@ * @{
*/
+#include <string.h>
+
#include "hal.h"
+/*===========================================================================*/
+/* Driver local definitions. */
+/*===========================================================================*/
+
+/**
+ * @brief Non-blocking input queue read.
+ * @details The function reads data from an input queue into a buffer. The
+ * operation completes when the specified amount of data has been
+ * transferred or when the input queue has been emptied.
+ *
+ * @param[in] iqp pointer to an @p input_queue_t structure
+ * @param[out] bp pointer to the data buffer
+ * @param[in] n the maximum amount of data to be transferred, the
+ * value 0 is reserved
+ * @return The number of bytes effectively transferred.
+ *
+ * @notapi
+ */
+static size_t iq_read(input_queue_t *iqp, uint8_t *bp, size_t n) {
+ size_t s1, s2;
+
+ osalDbgCheck(n > 0U);
+
+ /* Number of bytes that can be read in a single atomic operation.*/
+ if (n > iqGetFullI(iqp)) {
+ n = iqGetFullI(iqp);
+ }
+
+ /* Number of bytes before buffer limit.*/
+ s1 = iqp->q_top - iqp->q_rdptr;
+ if (n < s1) {
+ memcpy((void *)bp, (void *)iqp->q_rdptr, n);
+ iqp->q_rdptr += n;
+ }
+ else if (n > s1) {
+ memcpy((void *)bp, (void *)iqp->q_rdptr, s1);
+ s2 = n - s1;
+ bp += s1;
+ memcpy((void *)bp, (void *)iqp->q_buffer, s2);
+ iqp->q_rdptr = iqp->q_buffer + s2;
+ }
+ else { /* n == s1 */
+ memcpy((void *)bp, (void *)iqp->q_rdptr, n);
+ iqp->q_rdptr = iqp->q_buffer;
+ }
+
+ iqp->q_counter -= n;
+ return n;
+}
+
+/**
+ * @brief Non-blocking output queue write.
+ * @details The function writes data from a buffer to an output queue. The
+ * operation completes when the specified amount of data has been
+ * transferred or when the output queue has been filled.
+ *
+ * @param[in] oqp pointer to an @p output_queue_t structure
+ * @param[in] bp pointer to the data buffer
+ * @param[in] n the maximum amount of data to be transferred, the
+ * value 0 is reserved
+ * @return The number of bytes effectively transferred.
+ *
+ * @notapi
+ */
+static size_t oq_write(output_queue_t *oqp, const uint8_t *bp, size_t n) {
+ size_t s1, s2;
+
+ osalDbgCheck(n > 0U);
+
+ /* Number of bytes that can be written in a single atomic operation.*/
+ if (n > oqGetEmptyI(oqp)) {
+ n = oqGetEmptyI(oqp);
+ }
+
+ /* Number of bytes before buffer limit.*/
+ s1 = oqp->q_top - oqp->q_wrptr;
+ if (n < s1) {
+ memcpy((void *)oqp->q_wrptr, (void *)bp, n);
+ oqp->q_wrptr += n;
+ }
+ else if (n > s1) {
+ memcpy((void *)oqp->q_wrptr, (void *)bp, s1);
+ s2 = n - s1;
+ bp += s1;
+ memcpy((void *)oqp->q_buffer, (void *)bp, s2);
+ oqp->q_wrptr = oqp->q_buffer + s2;
+ }
+ else { /* n == s1 */
+ memcpy((void *)oqp->q_wrptr, (void *)bp, n);
+ oqp->q_wrptr = oqp->q_buffer;
+ }
+
+ oqp->q_counter -= n;
+ return n;
+}
+
+/*===========================================================================*/
+/* Driver exported variables. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local variables and types. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local functions. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver interrupt handlers. */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported functions. */
+/*===========================================================================*/
+
/**
* @brief Initializes an input queue.
* @details A Semaphore is internally initialized and works as a counter of
|