355 lines
7.1 KiB
C
355 lines
7.1 KiB
C
/*
|
|
* Copyright (c) 1997-2010, 2013-2014 Wind River Systems, Inc.
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
/**
|
|
* @file
|
|
* @brief FIFO kernel services
|
|
*
|
|
* This file contains all the services needed for the implementation of a FIFO
|
|
* for the microkernel.
|
|
*
|
|
*
|
|
*/
|
|
|
|
|
|
#include <micro_private.h>
|
|
#include <string.h>
|
|
#include <toolchain.h>
|
|
#include <sections.h>
|
|
|
|
/**
|
|
*
|
|
* @brief Finish performing an incomplete FIFO enqueue request
|
|
*
|
|
* @return N/A
|
|
*/
|
|
void _k_fifo_enque_reply(struct k_args *A)
|
|
{
|
|
#ifdef CONFIG_SYS_CLOCK_EXISTS
|
|
if (A->Time.timer)
|
|
FREETIMER(A->Time.timer);
|
|
if (unlikely(A->Comm == _K_SVC_FIFO_ENQUE_REPLY_TIMEOUT)) {
|
|
REMOVE_ELM(A);
|
|
A->Time.rcode = RC_TIME;
|
|
} else {
|
|
A->Time.rcode = RC_OK;
|
|
}
|
|
#else
|
|
A->Time.rcode = RC_OK;
|
|
#endif
|
|
|
|
_k_state_bit_reset(A->Ctxt.task, TF_ENQU);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Finish performing an incomplete FIFO enqueue request with timeout.
|
|
*
|
|
* @param A Pointer to a k_args structure
|
|
*
|
|
* @return N/A
|
|
*
|
|
* @sa _k_fifo_enque_reply
|
|
*/
|
|
void _k_fifo_enque_reply_timeout(struct k_args *A)
|
|
{
|
|
_k_fifo_enque_reply(A);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Perform a FIFO enqueue request
|
|
*
|
|
* @return N/A
|
|
*/
|
|
void _k_fifo_enque_request(struct k_args *A)
|
|
{
|
|
struct k_args *W;
|
|
struct _k_fifo_struct *Q;
|
|
int Qid, n, w;
|
|
char *p, *q; /* Ski char->uint32_t ??? */
|
|
|
|
Qid = A->args.q1.queue;
|
|
Q = (struct _k_fifo_struct *)Qid;
|
|
w = OCTET_TO_SIZEOFUNIT(Q->element_size);
|
|
q = A->args.q1.data;
|
|
n = Q->num_used;
|
|
if (n < Q->Nelms) {
|
|
W = Q->waiters;
|
|
if (W) {
|
|
Q->waiters = W->next;
|
|
p = W->args.q1.data;
|
|
memcpy(p, q, w);
|
|
|
|
#ifdef CONFIG_SYS_CLOCK_EXISTS
|
|
if (W->Time.timer) {
|
|
_k_timeout_cancel(W);
|
|
W->Comm = _K_SVC_FIFO_DEQUE_REPLY;
|
|
} else {
|
|
#endif
|
|
W->Time.rcode = RC_OK;
|
|
_k_state_bit_reset(W->Ctxt.task, TF_DEQU);
|
|
}
|
|
#ifdef CONFIG_SYS_CLOCK_EXISTS
|
|
}
|
|
#endif
|
|
else {
|
|
p = Q->enqueue_point;
|
|
memcpy(p, q, w);
|
|
p = (char *)((int)p + w);
|
|
if (p == Q->end_point)
|
|
Q->enqueue_point = Q->base;
|
|
else
|
|
Q->enqueue_point = p;
|
|
Q->num_used = ++n;
|
|
#ifdef CONFIG_OBJECT_MONITOR
|
|
if (Q->high_watermark < n)
|
|
Q->high_watermark = n;
|
|
#endif
|
|
}
|
|
|
|
A->Time.rcode = RC_OK;
|
|
#ifdef CONFIG_OBJECT_MONITOR
|
|
Q->count++;
|
|
#endif
|
|
} else {
|
|
if (likely(A->Time.ticks != TICKS_NONE)) {
|
|
A->Ctxt.task = _k_current_task;
|
|
A->priority = _k_current_task->priority;
|
|
_k_state_bit_set(_k_current_task, TF_ENQU);
|
|
INSERT_ELM(Q->waiters, A);
|
|
#ifdef CONFIG_SYS_CLOCK_EXISTS
|
|
if (A->Time.ticks == TICKS_UNLIMITED)
|
|
A->Time.timer = NULL;
|
|
else {
|
|
A->Comm = _K_SVC_FIFO_ENQUE_REPLY_TIMEOUT;
|
|
_k_timeout_alloc(A);
|
|
}
|
|
#endif
|
|
} else {
|
|
A->Time.rcode = RC_FAIL;
|
|
}
|
|
}
|
|
}
|
|
|
|
int task_fifo_put(kfifo_t queue, void *data, int32_t timeout)
|
|
{
|
|
struct k_args A;
|
|
|
|
A.Comm = _K_SVC_FIFO_ENQUE_REQUEST;
|
|
A.Time.ticks = timeout;
|
|
A.args.q1.data = (char *)data;
|
|
A.args.q1.queue = queue;
|
|
|
|
KERNEL_ENTRY(&A);
|
|
|
|
return A.Time.rcode;
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Finish performing an incomplete FIFO dequeue request
|
|
*
|
|
* @return N/A
|
|
*/
|
|
void _k_fifo_deque_reply(struct k_args *A)
|
|
{
|
|
#ifdef CONFIG_SYS_CLOCK_EXISTS
|
|
if (A->Time.timer)
|
|
FREETIMER(A->Time.timer);
|
|
if (unlikely(A->Comm == _K_SVC_FIFO_DEQUE_REPLY_TIMEOUT)) {
|
|
REMOVE_ELM(A);
|
|
A->Time.rcode = RC_TIME;
|
|
} else {
|
|
A->Time.rcode = RC_OK;
|
|
}
|
|
#else
|
|
A->Time.rcode = RC_OK;
|
|
#endif
|
|
|
|
_k_state_bit_reset(A->Ctxt.task, TF_DEQU);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Finish performing an incomplete FIFO dequeue request with timeout.
|
|
*
|
|
* @param A Pointer to a k_args structure.
|
|
*
|
|
* @return N/A
|
|
*
|
|
* @sa _k_fifo_deque_reply
|
|
*/
|
|
void _k_fifo_deque_reply_timeout(struct k_args *A)
|
|
{
|
|
_k_fifo_deque_reply(A);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Perform FIFO dequeue request
|
|
*
|
|
* @return N/A
|
|
*/
|
|
void _k_fifo_deque_request(struct k_args *A)
|
|
{
|
|
struct k_args *W;
|
|
struct _k_fifo_struct *Q;
|
|
int Qid, n, w;
|
|
char *p, *q; /* idem */
|
|
|
|
Qid = A->args.q1.queue;
|
|
Q = (struct _k_fifo_struct *)Qid;
|
|
w = OCTET_TO_SIZEOFUNIT(Q->element_size);
|
|
p = A->args.q1.data;
|
|
n = Q->num_used;
|
|
if (n) {
|
|
q = Q->dequeue_point;
|
|
memcpy(p, q, w);
|
|
q = (char *)((int)q + w);
|
|
if (q == Q->end_point)
|
|
Q->dequeue_point = Q->base;
|
|
else
|
|
Q->dequeue_point = q;
|
|
|
|
A->Time.rcode = RC_OK;
|
|
W = Q->waiters;
|
|
if (W) {
|
|
Q->waiters = W->next;
|
|
p = Q->enqueue_point;
|
|
q = W->args.q1.data;
|
|
w = OCTET_TO_SIZEOFUNIT(Q->element_size);
|
|
memcpy(p, q, w);
|
|
p = (char *)((int)p + w);
|
|
if (p == Q->end_point)
|
|
Q->enqueue_point = Q->base;
|
|
else
|
|
Q->enqueue_point = p;
|
|
|
|
#ifdef CONFIG_SYS_CLOCK_EXISTS
|
|
if (W->Time.timer) {
|
|
_k_timeout_cancel(W);
|
|
W->Comm = _K_SVC_FIFO_ENQUE_REPLY;
|
|
} else {
|
|
#endif
|
|
W->Time.rcode = RC_OK;
|
|
_k_state_bit_reset(W->Ctxt.task, TF_ENQU);
|
|
#ifdef CONFIG_SYS_CLOCK_EXISTS
|
|
}
|
|
#endif
|
|
#ifdef CONFIG_OBJECT_MONITOR
|
|
Q->count++;
|
|
#endif
|
|
} else
|
|
Q->num_used = --n;
|
|
} else {
|
|
if (likely(A->Time.ticks != TICKS_NONE)) {
|
|
A->Ctxt.task = _k_current_task;
|
|
A->priority = _k_current_task->priority;
|
|
_k_state_bit_set(_k_current_task, TF_DEQU);
|
|
|
|
INSERT_ELM(Q->waiters, A);
|
|
#ifdef CONFIG_SYS_CLOCK_EXISTS
|
|
if (A->Time.ticks == TICKS_UNLIMITED)
|
|
A->Time.timer = NULL;
|
|
else {
|
|
A->Comm = _K_SVC_FIFO_DEQUE_REPLY_TIMEOUT;
|
|
_k_timeout_alloc(A);
|
|
}
|
|
#endif
|
|
} else {
|
|
A->Time.rcode = RC_FAIL;
|
|
}
|
|
}
|
|
}
|
|
|
|
int task_fifo_get(kfifo_t queue, void *data, int32_t timeout)
|
|
{
|
|
struct k_args A;
|
|
|
|
A.Comm = _K_SVC_FIFO_DEQUE_REQUEST;
|
|
A.Time.ticks = timeout;
|
|
A.args.q1.data = (char *)data;
|
|
A.args.q1.queue = queue;
|
|
|
|
KERNEL_ENTRY(&A);
|
|
|
|
return A.Time.rcode;
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Perform miscellaneous FIFO request
|
|
* @param A Kernel Argument
|
|
*
|
|
* @return N/A
|
|
*/
|
|
void _k_fifo_ioctl(struct k_args *A)
|
|
{
|
|
struct _k_fifo_struct *Q;
|
|
int Qid;
|
|
|
|
Qid = A->args.q1.queue;
|
|
Q = (struct _k_fifo_struct *)Qid;
|
|
if (A->args.q1.size) {
|
|
if (Q->num_used) {
|
|
struct k_args *X;
|
|
|
|
while ((X = Q->waiters)) {
|
|
Q->waiters = X->next;
|
|
#ifdef CONFIG_SYS_CLOCK_EXISTS
|
|
if (likely(X->Time.timer)) {
|
|
_k_timeout_cancel(X);
|
|
X->Comm = _K_SVC_FIFO_ENQUE_REPLY;
|
|
} else {
|
|
#endif
|
|
X->Time.rcode = RC_FAIL;
|
|
_k_state_bit_reset(X->Ctxt.task, TF_ENQU);
|
|
#ifdef CONFIG_SYS_CLOCK_EXISTS
|
|
}
|
|
#endif
|
|
}
|
|
}
|
|
Q->num_used = 0;
|
|
Q->enqueue_point = Q->dequeue_point = Q->base;
|
|
A->Time.rcode = RC_OK;
|
|
} else
|
|
A->Time.rcode = Q->num_used;
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @brief Miscellaneous FIFO request
|
|
*
|
|
* Depending upon the chosen operation, this routine will ...
|
|
* 1. <op> = 0 : query the number of FIFO entries
|
|
* 2. <op> = 1 : purge the FIFO of its entries
|
|
*
|
|
* @param queue FIFO queue
|
|
* @param op 0 for status query and 1 for purge
|
|
* @return # of FIFO entries on query; RC_OK on purge
|
|
*/
|
|
int _task_fifo_ioctl(kfifo_t queue, int op)
|
|
{
|
|
struct k_args A;
|
|
|
|
A.Comm = _K_SVC_FIFO_IOCTL;
|
|
A.args.q1.queue = queue;
|
|
A.args.q1.size = op;
|
|
KERNEL_ENTRY(&A);
|
|
return A.Time.rcode;
|
|
}
|