/** Copies frame pointed to by index entry at index_offset.
\pre hold read lock on the channel
\pre on success, buf holds the frame seq_num and next_index fields
are incremented. The variable pointed to by size_written holds the
number of bytes written to buf (0 on failure).
*/
static enum ach_status
ach_get_from_offset( ach_channel_t *chan, size_t index_offset,
char *buf, size_t size, size_t *frame_size ) {
ach_header_t *shm = chan->shm;
assert( index_offset < shm->index_cnt );
ach_index_t *idx = ACH_SHM_INDEX(shm) + index_offset;
/* assert( idx->size ); */
assert( idx->seq_num );
assert( idx->offset < shm->data_size );
/* check idx */
if( chan->seq_num > idx->seq_num ) {
fprintf(stderr,
"ach bug: chan->seq_num (%"PRIu64") > idx->seq_num (%"PRIu64")\n"
"ach bug: index offset: %"PRIuPTR"\n",
chan->seq_num, idx->seq_num,
index_offset );
return ACH_BUG;
}
if( idx->size > size ) {
/* buffer overflow */
*frame_size = idx->size;
return ACH_OVERFLOW;
} else {
/* good to copy */
uint8_t *data_buf = ACH_SHM_DATA(shm);
if( idx->offset + idx->size < shm->data_size ) {
/* simple memcpy */
memcpy( (uint8_t*)buf, data_buf + idx->offset, idx->size );
}else {
/* wraparound memcpy */
size_t end_cnt = shm->data_size - idx->offset;
memcpy( (uint8_t*)buf, data_buf + idx->offset, end_cnt );
memcpy( (uint8_t*)buf + end_cnt, data_buf, idx->size - end_cnt );
}
*frame_size = idx->size;
chan->seq_num = idx->seq_num;
chan->next_index = (index_offset + 1) % shm->index_cnt;
return ACH_OK;
}
}
static long ach_ch_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
{
/* TODO: Validate argument */
int ret = 0;
struct ach_ch_file *ch_file = (struct ach_ch_file *)file->private_data;
KDEBUG("ach: In ach_ch_ioctl\n");
switch (cmd) {
case ACH_CH_SET_MODE: {
struct achk_opt opt;
if (copy_from_user(&opt, (void*)arg, sizeof(opt)) ) {
ret = -EFAULT;
} else {
/* This is not threadsafe */
ch_file->mode = opt;
/* if (ch_file->mode.reltime.tv_sec != 0 */
/* || ch_file->mode.reltime.tv_nsec != 0) */
/* KDEBUG("ach: Setting wait time to %ld.%09ld\n", */
/* ch_file->mode.reltime.tv_sec, */
/* ch_file->mode.reltime.tv_nsec); */
/* KDEBUG("ach: Got cmd ACH_CH_SET_MODE: \n"); */
/* KDEBUG1(" ACH_O_WAIT=%d\n", */
/* ch_file->mode.mode & ACH_O_WAIT); */
/* KDEBUG1(" ACH_O_LAST=%d\n", */
/* ch_file->mode.mode & ACH_O_LAST); */
/* KDEBUG1(" ACH_O_COPY=%d\n", */
/* ch_file->mode.mode & ACH_O_COPY); */
ret = 0;
break;
}
}
case ACH_CH_GET_MODE: {
KDEBUG("ach: Got cmd ACH_CH_GET_MODE: %ld\n", arg);
if( copy_to_user((void*)arg, &ch_file->mode, sizeof(ch_file->mode)) )
ret = -EFAULT;
else
ret = 0;
break;
}
case ACH_CH_GET_STATUS: {
KDEBUG("ach: Got cmd ACH_CH_GET_STATUS\n");
if (rt_mutex_lock_interruptible(&ch_file->shm->sync.mutex)) {
ret = -ERESTARTSYS;
break;
}
{
struct ach_ch_status stat;
struct ach_header *shm = ch_file->shm;
ach_index_t *index_ar = ACH_SHM_INDEX(shm);
size_t oldest_index = oldest_index_i(shm);
uint64_t oldest_seq =
index_ar[oldest_index].seq_num;
stat.mode = ch_file->mode.options;
stat.size = shm->len;
stat.count = shm->index_cnt - shm->index_free;
if (oldest_seq > ch_file->seq_num) {
stat.new_msgs = shm->last_seq - oldest_seq;
} else {
stat.new_msgs =
shm->last_seq - ch_file->seq_num;
}
stat.last_seq = shm->last_seq;
stat.last_seq_read = ch_file->seq_num;
printk(KERN_INFO "ach: Status:\n");
printk(KERN_INFO "ach: mode : %02x\n",
stat.mode);
printk(KERN_INFO "ach: size : %zu\n",
stat.size);
printk(KERN_INFO "ach: count: %zu\n",
stat.count);
printk(KERN_INFO "ach: new : %zu\n",
stat.new_msgs);
printk(KERN_INFO "ach: last_seq : %lu\n",
stat.last_seq);
printk(KERN_INFO "ach: last_seq_read : %lu\n",
stat.last_seq_read);
if (copy_to_user((void *)arg, &stat, sizeof(stat))) {
ret = -EFAULT;
}
}
rt_mutex_unlock(&ch_file->shm->sync.mutex);
}
case ACH_CH_FLUSH:
KDEBUG("ach: Got cmd ACH_CH_FLUSH\n");
ret = -get_errno( ach_flush(ch_file) );
break;
case ACH_CH_CANCEL: {
unsigned int unsafe = (unsigned int)arg;
KDEBUG("ach: Got cmd ACH_CH_CANCEL\n");
ret = -get_errno(ach_cancel(ch_file, unsafe));
break;
}
case ACH_CH_GET_OPTIONS: {
struct ach_ch_options retval;
retval.mode = ch_file->mode;
retval.clock = ch_file->shm->clock;
if( copy_to_user( (void*)arg, &retval, sizeof(retval) ) )
ret = -EFAULT;
else
ret = 0;
break;
}
default:
printk(KERN_ERR "ach: Unknown ioctl option: %d\n", cmd);
ret = -ENOSYS;
break;
}
return ret;
}
memcpy( (uint8_t*)buf + end_cnt, data_buf, idx->size - end_cnt );
}
*frame_size = idx->size;
chan->seq_num = idx->seq_num;
chan->next_index = (index_offset + 1) % shm->index_cnt;
return ACH_OK;
}
}
enum ach_status
ach_get( ach_channel_t *chan, void *buf, size_t size,
size_t *frame_size,
const struct timespec *ACH_RESTRICT abstime,
int options ) {
ach_header_t *shm = chan->shm;
ach_index_t *index_ar = ACH_SHM_INDEX(shm);
/* Check guard bytes */
{
enum ach_status r = check_guards(shm);
if( ACH_OK != r ) return r;
}
const bool o_wait = options & ACH_O_WAIT;
const bool o_last = options & ACH_O_LAST;
const bool o_copy = options & ACH_O_COPY;
/* take read lock */
if( o_wait ) {
enum ach_status r;
if( ACH_OK != (r = rdlock_wait( shm, chan, abstime ) ) ) {
