int rtdal_itfspscq_pop(r_itf_t obj, void **ptr, int *len, int tstamp) { cast(obj,itf); RTDAL_ASSERT_PARAM(ptr); RTDAL_ASSERT_PARAM(len); *ptr = NULL; *len = 0; if (spscq_is_empty(itf)) { qdebug("[empty] read=%d, tstamp=%d\n",itf->read,itf->packets[itf->read].tstamp); return 0; } if (itf->packets[itf->read].tstamp > tstamp) { qdebug("[delay] read=%d, tstamp=%d\n",itf->read,itf->packets[itf->read].tstamp); return 0; } if (itf->packets[itf->read].tstamp < tstamp && itf->parent.delay) { rtdal_itfspscq_release(obj); } qdebug("[ok] read=%d, tstamp=%d (now=%d)\n",itf->read,itf->packets[itf->read].tstamp,tstamp); *ptr = itf->packets[itf->read].data; *len = itf->packets[itf->read].len; return 1; }
int rtdal_itfspscq_release(r_itf_t obj) { cast(obj,itf); if (spscq_is_empty(itf)) { qdebug("[empty] read=%d, tstamp=%d\n",itf->read,itf->packets[itf->read].tstamp); return 0; } qdebug("read=%d, tstamp=%d\n",itf->read,itf->packets[itf->read].tstamp); itf->packets[itf->read].valid = 0; itf->read += (itf->read+1 >= itf->max_msg) ? (1-itf->max_msg) : 1; return 1; }
int rtdal_itfspscq_push(r_itf_t obj, void *ptr, int len, int tstamp) { cast(obj,itf); if (!len) { return 1; } /* if (spscq_is_full(itf)) { qdebug("[full] write=%d read=%d\n",itf->write,itf->read); RTDAL_SETERROR(RTDAL_ERROR_NOSPACE); return 0; } */ #ifdef USE_SYSTEM_TSTAMP tstamp=rtdal_time_slot(); #endif itf->packets[itf->write].tstamp = tstamp+itf->parent.delay; itf->packets[itf->write].len = len; itf->packets[itf->write].valid = 1; qdebug("write=%d/%d, len=%d, tstamp=%d, delay=%d\n",itf->write,itf->max_msg,len,tstamp,itf->parent.delay); itf->write += (itf->write+1 >= itf->max_msg) ? (1-itf->max_msg) : 1; if (itf->parent.delay < 0) { ring_buff_binary_sem_give(itf->sem_r); } return 1; }
int rtdal_itfspscq_request(r_itf_t obj, void **ptr) { cast(obj,itf); RTDAL_ASSERT_PARAM(ptr); *ptr = NULL; qdebug("write=%d, tstamp=%d\n",itf->write,itf->packets[itf->write].tstamp); if (spscq_is_full(itf)) { RTDAL_SETERROR(RTDAL_ERROR_NOSPACE); return 0; } qdebug("[ok] write=%d/%d, addr=0x%x\n",itf->write,itf->max_msg, itf->packets[itf->write].data); *ptr = itf->packets[itf->write].data; return 1; }
int rtdal_itfspscq_request(r_itf_t obj, void **ptr) { cast(obj,itf); RTDAL_ASSERT_PARAM(ptr); *ptr = NULL; qdebug("write=%d read=%d\n",itf->write,itf->read); if (spscq_is_full(itf)) { qdebug("[full] id=%d write=%d, valid=%d\n",itf->parent.id,itf->write,itf->packets[itf->write].valid); RTDAL_SETERROR(RTDAL_ERROR_NOSPACE); return 0; } qdebug("[ok] write=%d/%d\n",itf->write,itf->max_msg); *ptr = itf->packets[itf->write].data; return 1; }
inline static int spscq_is_full(rtdal_itfspscq_t *itf) { if (itf->parent.delay >= 0) { return (itf->packets[itf->write].valid != 0); } else { while(itf->packets[itf->write].valid != 0) { qdebug("wait write=%d read=%d\n",itf->write,itf->read); ring_buff_binary_sem_take(itf->sem_w); } return 0; } }
int rtdal_itfspscq_reset(r_itf_t obj) { cast(obj,itf); int i; itf->read = 0; itf->write = 0; qdebug("resetting %d msg \n",itf->max_msg); for (i=0;i<itf->max_msg;i++) { itf->packets[i].valid = 0; } return 0; }
inline static int spscq_is_empty(rtdal_itfspscq_t *itf, int tstamp) { if (itf->parent.delay >= 0 || itf->parent.delay == -2) { return (itf->packets[itf->read].valid == 0); } else { while (itf->packets[itf->read].valid == 0 && itf->packets[itf->read].tstamp > tstamp) { qdebug("wait write=%d read=%d\n",itf->write,itf->read); ring_buff_binary_sem_take(itf->sem_r); } return 0; } }
int rtdal_itfspscq_pop(r_itf_t obj, void **ptr, int *len, int tstamp) { cast(obj,itf); RTDAL_ASSERT_PARAM(ptr); RTDAL_ASSERT_PARAM(len); *ptr = NULL; *len = 0; if (spscq_is_empty(itf,tstamp)) { if (itf->parent.delay==-2) { usleep(1000); } qdebug("[empty] read=%d write=%d\n",itf->read,itf->write); return 0; } if (itf->parent.delay >= 0 ) { #ifdef USE_SYSTEM_TSTAMP tstamp=rtdal_time_slot(); #endif if (itf->packets[itf->read].tstamp > tstamp) { qdebug("[delay] read=%d, tstamp=%d now=%d\n",itf->read,itf->packets[itf->read].tstamp,tstamp); return 0; } /* if (itf->packets[itf->read].tstamp < tstamp-1) { qdebug("[old] read=%d, tstamp=%d now=%d\n",itf->read,itf->packets[itf->read].tstamp,tstamp); rtdal_itfspscq_release(obj,NULL,0); return 2; } */ } qdebug("[ok] read=%d, tstamp=%d (now=%d)\n",itf->read,itf->packets[itf->read].tstamp,tstamp); *ptr = itf->packets[itf->read].data; *len = itf->packets[itf->read].len; return 1; }
int rtdal_itfspscq_push(r_itf_t obj, int len, int tstamp) { cast(obj,itf); if (spscq_is_full(itf)) { RTDAL_SETERROR(RTDAL_ERROR_NOSPACE); return 0; } itf->packets[itf->write].tstamp = tstamp+itf->parent.delay; itf->packets[itf->write].len = len; itf->packets[itf->write].valid = 1; itf->write += (itf->write+1 >= itf->max_msg) ? (1-itf->max_msg) : 1; qdebug("write=%d/%d, len=%d\n",itf->write,itf->max_msg,len); return 1; }
int rtdal_itfspscq_release(r_itf_t obj, void *ptr, int len) { cast(obj,itf); /* if (spscq_is_empty(itf)) { qdebug("[empty] read=%d, write=%d\n",itf->read,itf->write); return 0; } */ itf->packets[itf->read].valid = 0; itf->read += (itf->read+1 >= itf->max_msg) ? (1-itf->max_msg) : 1; qdebug("read=%d, write=%d\n",itf->read,itf->write); if (itf->parent.delay < 0) { ring_buff_binary_sem_give(itf->sem_w); } return 1; }