static void *pthNotifyThread(void *x)
{
static int got = 0;
int bytes, r;
while (1)
{
bytes = read(gPigNotify, (char*)&gReport+got, sizeof(gReport)-got);
if (bytes > 0) got += bytes;
else break;
r = 0;
while (got >= sizeof(gpioReport_t))
{
dispatch_notification(&gReport[r]);
r++;
got -= sizeof(gpioReport_t);
}
/* copy any partial report to start of array */
if (got && r) gReport[0] = gReport[r];
}
return 0;
}
/*! @brief Handle messages received by the CPIO file server.
@param s The global file server state. (No ownership transfer)
@param msg The received message. (No ownership transfer)
@return DISPATCH_SUCCESS if message dispatched, DISPATCH_ERROR if unknown message.
*/
static int
fileserv_handle_message(struct fs_state *s, srv_msg_t *msg)
{
int result;
int label = seL4_GetMR(0);
void *userptr;
(void) result;
if (dispatch_notification(msg) == DISPATCH_SUCCESS) {
return DISPATCH_SUCCESS;
}
if (check_dispatch_serv(msg, &userptr) == DISPATCH_SUCCESS) {
result = rpc_sv_serv_dispatcher(userptr, label);
assert(result == DISPATCH_SUCCESS);
return DISPATCH_SUCCESS;
}
if (check_dispatch_data(msg, &userptr) == DISPATCH_SUCCESS) {
result = rpc_sv_data_dispatcher(userptr, label);
assert(result == DISPATCH_SUCCESS);
return DISPATCH_SUCCESS;
}
dprintf("Unknown message (badge = %d msgInfo = %d label = %d (0x%x)).\n",
msg->badge, seL4_MessageInfo_get_label(msg->message), label, label);
ROS_ERROR("File server unknown message.");
assert(!"File server unknown message.");
return DISPATCH_ERROR;
}
static void *pthNotifyThread(void *x)
{
static int got = 0;
int pi;
int bytes, r;
gpioReport_t report[PI_MAX_REPORTS_PER_READ];
pi = *((int*)x);
free(x); /* memory allocated in pigpio_start */
while (1)
{
bytes = read(gPigNotify[pi], (char*)&report+got, sizeof(report)-got);
if (bytes > 0) got += bytes;
else break;
r = 0;
while (got >= sizeof(gpioReport_t))
{
dispatch_notification(pi, &report[r]);
r++;
got -= sizeof(gpioReport_t);
}
/* copy any partial report to start of array */
if (got && r) report[0] = report[r];
}
fprintf(stderr, "notify thread for pi %d broke with read error %d\n",
pi, bytes);
while (1) sleep(1);
return NULL;
}
void dispatch(Router *r)
{
Message m = { { 0 } };
const static int CIRCULARITY_THRESHOLD=50;
static int circularity_count=0;
if(CIRCULARITY_THRESHOLD == ++circularity_count)
{
fprintf(stderr,"ERROR: CIRCULARITY_THRESHOLD reached. Message neglected.\n");
mqueue_pophead(&r->mqueue,&m);
circularity_count--;
return;
}
while (!mqueue_empty(&r->mqueue))
{
if (1 == mqueue_pophead(&r->mqueue,&m))
{
fprintf(stderr,"ERROR: message_router.c:dispatch\n");
return;
}
switch (m.type)
{
case NOTIFICATION:
dispatch_notification(r, &m);
break;
case REQUEST:
dispatch_request(r, &m);
break;
case REPLY:
dispatch_reply(r, &m);
break;
}
if (SYNC_MODE == m.mode)
*m.sync_message_status=PROCESSED;
(*m.mdfptr)(&m); // destroy the CONTENTS of the message.
}
circularity_count--;
}
