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;
}
