rtems_aio_request_chain *
rtems_aio_search_fd (rtems_chain_control *chain, int fildes, int create)
{
rtems_aio_request_chain *r_chain;
rtems_chain_node *node;
node = rtems_chain_first (chain);
r_chain = (rtems_aio_request_chain *) node;
while (r_chain->fildes < fildes && !rtems_chain_is_tail (chain, node)) {
node = rtems_chain_next (node);
r_chain = (rtems_aio_request_chain *) node;
}
if (r_chain->fildes == fildes)
r_chain->new_fd = 0;
else {
if (create == 0)
r_chain = NULL;
else {
r_chain = malloc (sizeof (rtems_aio_request_chain));
rtems_chain_initialize_empty (&r_chain->perfd);
if (rtems_chain_is_empty (chain))
rtems_chain_prepend (chain, &r_chain->next_fd);
else
rtems_chain_insert (rtems_chain_previous (node), &r_chain->next_fd);
r_chain->new_fd = 1;
r_chain->fildes = fildes;
}
}
return r_chain;
}
static void
rtems_aio_insert_prio (rtems_chain_control *chain, rtems_aio_request *req)
{
rtems_chain_node *node;
AIO_printf ("FD exists \n");
node = rtems_chain_first (chain);
if (rtems_chain_is_empty (chain)) {
AIO_printf ("First in chain \n");
rtems_chain_prepend (chain, &req->next_prio);
} else {
AIO_printf ("Add by priority \n");
int prio = ((rtems_aio_request *) node)->aiocbp->aio_reqprio;
while (req->aiocbp->aio_reqprio > prio &&
!rtems_chain_is_tail (chain, node)) {
node = rtems_chain_next (node);
prio = ((rtems_aio_request *) node)->aiocbp->aio_reqprio;
}
rtems_chain_insert (node->previous, &req->next_prio);
}
}
rtems_status_code mpc55xx_edma_obtain_channel(
edma_channel_context *ctx,
unsigned irq_priority
)
{
rtems_status_code sc = mpc55xx_edma_obtain_channel_by_tcd(ctx->edma_tcd);
if (sc == RTEMS_SUCCESSFUL) {
unsigned channel_index = edma_channel_index_of_tcd(ctx->edma_tcd);
sc = mpc55xx_interrupt_handler_install(
MPC55XX_IRQ_EDMA(channel_index),
"eDMA Channel",
RTEMS_INTERRUPT_SHARED,
irq_priority,
edma_interrupt_handler,
ctx
);
if (sc == RTEMS_SUCCESSFUL) {
rtems_chain_prepend(&edma_channel_chain, &ctx->node);
mpc55xx_edma_enable_error_interrupts(ctx->edma_tcd);
} else {
mpc55xx_edma_release_channel_by_tcd(ctx->edma_tcd);
sc = RTEMS_IO_ERROR;
}
}
return sc;
}
int
rtems_aio_enqueue (rtems_aio_request *req)
{
rtems_aio_request_chain *r_chain;
rtems_chain_control *chain;
pthread_t thid;
int result, policy;
struct sched_param param;
/* The queue should be initialized */
AIO_assert (aio_request_queue.initialized == AIO_QUEUE_INITIALIZED);
result = pthread_mutex_lock (&aio_request_queue.mutex);
if (result != 0) {
free (req);
return result;
}
/* _POSIX_PRIORITIZED_IO and _POSIX_PRIORITY_SCHEDULING are defined,
we can use aio_reqprio to lower the priority of the request */
pthread_getschedparam (pthread_self(), &policy, ¶m);
req->caller_thread = pthread_self ();
req->priority = param.sched_priority - req->aiocbp->aio_reqprio;
req->policy = policy;
req->aiocbp->error_code = EINPROGRESS;
req->aiocbp->return_value = 0;
if ((aio_request_queue.idle_threads == 0) &&
aio_request_queue.active_threads < AIO_MAX_THREADS)
/* we still have empty places on the active_threads chain */
{
chain = &aio_request_queue.work_req;
r_chain = rtems_aio_search_fd (chain, req->aiocbp->aio_fildes, 1);
if (r_chain->new_fd == 1) {
rtems_chain_prepend (&r_chain->perfd, &req->next_prio);
r_chain->new_fd = 0;
pthread_mutex_init (&r_chain->mutex, NULL);
pthread_cond_init (&r_chain->cond, NULL);
AIO_printf ("New thread \n");
result = pthread_create (&thid, &aio_request_queue.attr,
rtems_aio_handle, (void *) r_chain);
if (result != 0) {
pthread_mutex_unlock (&aio_request_queue.mutex);
return result;
}
++aio_request_queue.active_threads;
}
else {
/* put request in the fd chain it belongs to */
pthread_mutex_lock (&r_chain->mutex);
rtems_aio_insert_prio (&r_chain->perfd, req);
pthread_cond_signal (&r_chain->cond);
pthread_mutex_unlock (&r_chain->mutex);
}
}
else
{
/* the maximum number of threads has been already created
even though some of them might be idle.
The request belongs to one of the active fd chain */
r_chain = rtems_aio_search_fd (&aio_request_queue.work_req,
req->aiocbp->aio_fildes, 0);
if (r_chain != NULL)
{
pthread_mutex_lock (&r_chain->mutex);
rtems_aio_insert_prio (&r_chain->perfd, req);
pthread_cond_signal (&r_chain->cond);
pthread_mutex_unlock (&r_chain->mutex);
} else {
/* or to the idle chain */
chain = &aio_request_queue.idle_req;
r_chain = rtems_aio_search_fd (chain, req->aiocbp->aio_fildes, 1);
if (r_chain->new_fd == 1) {
/* If this is a new fd chain we signal the idle threads that
might be waiting for requests */
AIO_printf (" New chain on waiting queue \n ");
rtems_chain_prepend (&r_chain->perfd, &req->next_prio);
r_chain->new_fd = 0;
pthread_mutex_init (&r_chain->mutex, NULL);
pthread_cond_init (&r_chain->cond, NULL);
} else
/* just insert the request in the existing fd chain */
rtems_aio_insert_prio (&r_chain->perfd, req);
if (aio_request_queue.idle_threads > 0)
pthread_cond_signal (&aio_request_queue.new_req);
}
}
pthread_mutex_unlock (&aio_request_queue.mutex);
return 0;
}
static void test_chain_with_notification(void)
{
rtems_status_code sc = RTEMS_SUCCESSFUL;
rtems_chain_control chain;
rtems_chain_node a;
rtems_chain_node b;
rtems_chain_node *p = (rtems_chain_node *) 1;
rtems_event_set out = 0;
puts( "INIT - Verify rtems_chain_append_with_notification" );
rtems_chain_initialize_empty( &chain );
sc = rtems_chain_append_with_notification(
&chain,
&a,
rtems_task_self(),
EVENT
);
rtems_test_assert( sc == RTEMS_SUCCESSFUL );
sc = rtems_chain_get_with_wait( &chain, EVENT, TIMEOUT, &p );
rtems_test_assert( sc == RTEMS_SUCCESSFUL );
rtems_test_assert( p == &a );
rtems_chain_initialize_empty( &chain );
rtems_chain_append( &chain, &b );
sc = rtems_chain_append_with_notification(
&chain,
&a,
rtems_task_self(),
EVENT
);
rtems_test_assert( sc == RTEMS_SUCCESSFUL );
rtems_test_assert( p == &a );
puts( "INIT - Verify rtems_chain_prepend_with_notification" );
rtems_chain_initialize_empty( &chain );
sc = rtems_chain_prepend_with_notification(
&chain,
&a,
rtems_task_self(),
EVENT
);
rtems_test_assert( sc == RTEMS_SUCCESSFUL );
sc = rtems_chain_get_with_wait( &chain, EVENT, TIMEOUT, &p );
rtems_test_assert( sc == RTEMS_SUCCESSFUL );
rtems_test_assert( p == &a );
rtems_chain_prepend( &chain, &b );
sc = rtems_chain_prepend_with_notification(
&chain,
&a,
rtems_task_self(),
EVENT
);
rtems_test_assert( sc == RTEMS_SUCCESSFUL );
rtems_test_assert( p == &a );
puts( "INIT - Verify rtems_chain_prepend_with_notification" );
puts( "INIT - Verify rtems_chain_get_with_notification" );
rtems_chain_initialize_empty( &chain );
rtems_chain_append( &chain, &b );
rtems_chain_append( &chain, &a );
sc = rtems_chain_get_with_notification(&chain, rtems_task_self(), EVENT, &p);
rtems_test_assert( sc == RTEMS_SUCCESSFUL );
rtems_test_assert( p == &b );
sc = rtems_chain_get_with_notification(&chain, rtems_task_self(), EVENT, &p);
rtems_test_assert( sc == RTEMS_SUCCESSFUL );
rtems_test_assert( p == &a );
sc = rtems_event_receive(
EVENT,
RTEMS_EVENT_ALL | RTEMS_WAIT,
TIMEOUT,
&out
);
rtems_test_assert( sc == RTEMS_SUCCESSFUL );
rtems_test_assert( out == EVENT );
}
int
rtems_bsd_rc_conf_service_add(const char* name,
const char* control,
rtems_bsd_rc_conf_service entry)
{
service* srv;
char* ctl = NULL;
char* s;
char* c;
srv = malloc(sizeof(*srv));
if (srv == NULL) {
errno = ENOMEM;
return -1;
}
memset(srv, 0, sizeof(*srv));
srv->name = strdup(name);
if (control != NULL) {
ctl = strdup(control);
srv->control = ctl;
}
srv->entry = entry;
if (srv->name == NULL || (control != NULL && ctl == NULL)) {
fprintf(stderr, "error: rc.conf: add service: no memory\n");
free((void*) srv->control);
free((void*) srv->name);
free(srv);
errno = ENOMEM;
return -1;
}
if (control != NULL) {
s = c = ctl;
while (*c != '\0') {
if (*c == ';') {
*c = '\0';
if (strncasecmp("before:", s, sizeof("before:") - 1) == 0) {
if (srv->before == NULL) {
srv->before = s + sizeof("before:") - 1;
s = NULL;
}
else {
fprintf(stderr, "error: rc.conf: add service: repeated 'before'\n");
c = NULL;
}
}
else if (strncasecmp("after:", s, sizeof("after:") - 1) == 0) {
if (srv->after == NULL) {
srv->after = s + sizeof("after:") - 1;
s = NULL;
}
else {
fprintf(stderr, "error: rc.conf: add service: repeated 'after'\n");
c = NULL;
}
}
else if (strncasecmp("require:", s, sizeof("require:") - 1) == 0) {
if (srv->require == NULL) {
srv->require = s + sizeof("require:") - 1;
s = NULL;
}
else {
fprintf(stderr, "error: rc.conf: add service: repeated 'require'\n");
c = NULL;
}
}
else {
fprintf(stderr, "error: rc.conf: add service: unknown keyword: %s\n", s);
c = NULL;
}
if (c == NULL) {
free((void*) srv->control);
free((void*) srv->name);
free(srv);
errno = EINVAL;
return -1;
}
}
else if (s == NULL) {
s = c;
}
++c;
}
if (s != NULL) {
fprintf(stderr, "error: rc.conf: add service: no ';' found\n");
free((void*) srv->control);
free((void*) srv->name);
free(srv);
errno = EINVAL;
return -1;
}
/*
* Place on the services list. The node is removed before being inserted. If
* there are competing positions the last position is used. As a result
* handle 'after' before 'before'.
*/
rtems_chain_prepend(&services, &srv->node);
}
else {
/*
* No control string, add the end.
*/
rtems_chain_append(&services, &srv->node);
}
/*
* After.
*/
if (srv->after != NULL) {
const char* cc = srv->after;
while (*cc != '\0') {
const char* cs = cc;
size_t l;
while (*cc != ',' && *cc != '\0')
++cc;
l = cc - cs;
if (strncasecmp(cs, "last", l) == 0) {
fprintf(stderr,
"error: rc.conf: add service: 'last' in 'after': %s\n",
control);
rtems_chain_extract(&srv->node);
free((void*) srv->control);
free((void*) srv->name);
free(srv);
errno = EINVAL;
return -1;
}
else if (strncasecmp(cs, "first", l) == 0) {
/* already prepended */
}
else {
rtems_chain_node* node = rtems_chain_first(&services);
while (!rtems_chain_is_tail(&services, node)) {
service* ss = (service*) node;
if (ss != srv &&
strlen(ss->name) == l && strncasecmp(ss->name, cs, l) == 0) {
rtems_chain_extract(&srv->node);
rtems_chain_insert(&ss->node, &srv->node);
break;
}
node = rtems_chain_next(node);
}
}
}
}
/*
* Before.
*/
if (srv->before != NULL) {
const char* cc = srv->before;
while (*cc != '\0') {
const char* cs = cc;
size_t l;
while (*cc != ',' && *cc != '\0')
++cc;
l = cc - cs;
if (strncasecmp(cs, "first", l) == 0) {
fprintf(stderr, "error: rc.conf: add service: 'first' in 'before'\n");
rtems_chain_extract(&srv->node);
free((void*) srv->control);
free((void*) srv->name);
free(srv);
errno = EINVAL;
return -1;
}
else if (strncasecmp(cs, "last", l) == 0) {
rtems_chain_extract(&srv->node);
rtems_chain_append(&services, &srv->node);
}
else {
rtems_chain_node* node = rtems_chain_first(&services);
while (!rtems_chain_is_tail(&services, node)) {
service* ss = (service*) node;
if (strlen(ss->name) == l && strncasecmp(ss->name, cs, l) == 0) {
rtems_chain_extract(&srv->node);
if (rtems_chain_is_first(node))
rtems_chain_prepend(&services, &srv->node);
else {
service* sp = (service*) rtems_chain_previous(node);
rtems_chain_insert(&sp->node, &srv->node);
}
break;
}
node = rtems_chain_next(node);
}
}
}
}
return 0;
}
