int main(int argc, char **argv) {
(void) argc; (void)argv;
enum ach_status r;
/* create channels */
r = ach_unlink("control"); /* delete first */
if( ! ach_status_match(r, ACH_MASK_OK|ACH_MASK_ENOENT) ) abort();
r = ach_unlink("feedback"); /* delete first */
if( ! ach_status_match(r, ACH_MASK_OK|ACH_MASK_ENOENT) ) abort();
r = ach_create("control", 10ul, 256ul, NULL );
if(ACH_OK != r) abort();
r = ach_create("feedback", 10ul, 256ul, NULL );
if(ACH_OK != r) abort();
/* fork processes */
int pid_ctrl = fork();
if(pid_ctrl < 0) abort();
if(!pid_ctrl) controller();
int pid_bot = fork();
if(pid_bot < 0) abort();
if(!pid_bot) robot();
int pid_periodic = fork();
if(pid_periodic < 0) abort();
if(!pid_periodic) periodic_logger();
int pid_full = fork();
if(pid_full < 0) abort();
if(!pid_full) full_logger();
/* wait for a signal */
pause();
return 0;
}
static PyObject *
open_channel( PyObject *self, PyObject *args ) {
(void)self;
const char *name = NULL;
long frame_count = 0, frame_size = 0;
if( !PyArg_ParseTuple(args, "sll", &name, &frame_count, &frame_size ) ) {
return NULL;
}
/* Alloc struct */
ach_channel_t *c = (ach_channel_t*)malloc(sizeof(ach_channel_t));
/* Open it */
ach_status_t r = ach_open(c, name, NULL);
/* Create channel if necessary */
if( ACH_ENOENT == r ) {
r = ach_create( name, (size_t)frame_count, (size_t)frame_size, NULL );
if( ach_status_match(r, ACH_MASK_OK | ACH_MASK_EEXIST) ) {
r = ach_open(c, name, NULL);
}
}
/* Check result */
if( ACH_OK != r ) {
return raise_error(r);
}
return PyLong_FromVoidPtr(c);
}
/**********************************************************************************
* ach control device driver
**********************************************************************************/
static int
ctrl_create (struct ach_ctrl_create_ch *arg )
{
int ret;
struct ach_ch_device *dev;
ret = ach_ch_device_alloc(arg->name);
if( unlikely(ret < 0) )
return ret;
// newest is first in queue
dev = ctrl_data.in_use;
dev->ach_data = ach_create(arg->name, arg->frame_cnt, arg->frame_size, arg->clock);
if (unlikely(NULL == dev->ach_data)) {
printk(KERN_ERR "ach: Unable to allocate buffer memory\n");
return -ENOBUFS;
}
ret = ach_ch_device_setup(dev, arg->name);
if( unlikely(ret < 0) ) {
printk(KERN_ERR "ach: Failed creating char device\n");
kfree(dev->ach_data);
return ret;
}
printk( KERN_INFO "ach: created device %s\n", ach_ch_device_name(dev) );
return ret;
}
int cmd_create(void) {
if( opt_verbosity > 0 ) {
fprintf(stderr, "Creating Channel %s\n", opt_chan_name);
}
if( opt_msg_cnt < 1 ) {
fprintf(stderr, "Message count must be greater than zero, not %"PRIuPTR".\n", opt_msg_cnt);
return -1;
}
if( opt_msg_size < 1 ) {
fprintf(stderr, "Message size must be greater than zero, not %"PRIuPTR".\n", opt_msg_size);
return -1;
}
ach_status_t i;
{
ach_create_attr_t attr;
ach_create_attr_init(&attr);
if( opt_truncate ) attr.truncate = 1;
i = ach_create( opt_chan_name, opt_msg_cnt, opt_msg_size, &attr );
}
if( ! (opt_1 && i == ACH_EEXIST) ) {
check_status( i, "Error creating channel '%s'", opt_chan_name );
} else i = ACH_OK;
if( opt_mode > 0 ) {
i = cmd_chmod();
}
return i;
}
static int testfault(void)
{
if( ACH_OK == ach_create( NULL, opt_msg_cnt, opt_msg_size, NULL ) ) {
fprintf(stderr, "Error on ach_create with null channel\n");
exit(EXIT_FAILURE);
}
return 0;
}
int main( int argc, char **argv ) {
(void)argc;
(void) argv;
ach_channel_t channel;
ach_status_t r;
/* unlink */
r = ach_unlink(OPT_CHAN);
test( ach_status_match(r, ACH_MASK_OK | ACH_MASK_ENOENT),
r, "ach_unlink");
/* create */
r = ach_create(OPT_CHAN, 32ul, 64ul, NULL );
test(ACH_OK == r, r, "ach_create");
/* open */
r = ach_open(&channel, OPT_CHAN, NULL);
test(ACH_OK == r, r, "ach_open");
/* first test */
r = ach_get( &channel, NULL, 0, NULL, NULL, ACH_O_LAST );
test( ACH_STALE_FRAMES == r, r, "get stale");
/* read test */
make_locked();
r = ach_get( &channel, NULL, 0, NULL, NULL, ACH_O_LAST );
test( ACH_STALE_FRAMES == r, r, "get stale");
/* corrupt test */
make_locked();
channel.shm->sync.dirty = 1;
r = ach_get( &channel, NULL, 0, NULL, NULL, ACH_O_LAST );
test( ACH_CORRUPT == r, r, "get corrupt");
/* and again */
r = ach_get( &channel, NULL, 0, NULL, NULL, ACH_O_LAST );
test( ACH_CORRUPT == r, r, "get corrupt");
/* another read test */
channel.shm->sync.dirty = 0;
r = ach_get( &channel, NULL, 0, NULL, NULL, ACH_O_LAST );
r = ach_get( &channel, NULL, 0, NULL, NULL, ACH_O_LAST );
test( ACH_STALE_FRAMES == r, r, "get stale");
return 0;
}
void setup_ach(void) {
/* create channel */
enum ach_status r = ach_unlink("bench"); /* delete first */
if( ! ach_status_match(r, ACH_MASK_OK | ACH_MASK_ENOENT) ) abort();
ach_create_attr_t cattr;
if (KERNDEV) {
ach_create_attr_init(&cattr);
cattr.map = ACH_MAP_KERNEL;
}
r = ach_create("bench", 10, 256, KERNDEV ? &cattr : NULL );
if(ACH_OK != r) abort();
/* open channel */
r = ach_open(&chan, "bench", NULL);
if(ACH_OK != r) abort();
}
void init_time_chan(void) {
/* create channel */
ach_create_attr_t cattr;
enum ach_status r = ach_unlink("time"); /* delete first */
if( ! ach_status_match(r, ACH_MASK_OK | ACH_MASK_ENOENT) ) abort();
if (KERNDEV) {
ach_create_attr_init(&cattr);
cattr.map = ACH_MAP_KERNEL;
}
r = ach_create("time", (size_t)FREQUENCY*(size_t)SECS*RECV_RT,
sizeof(float), KERNDEV ? &cattr : NULL );
if(ACH_OK != r) abort();
/* open channel */
r = ach_open(&time_chan, "time", NULL);
if(ACH_OK != r) abort();
}
int ach_move_init() {
// Channels
int r = 0;
printf("ACH: Creating Channel\n");
ach_create_attr_t attr;
ach_create_attr_init(&attr);
r = ach_create((char*)craftyo_chan_name, 100, IBOARDSTATE_SIZE+IMOVESTRING_SIZE, &attr);
printf("ACH: %s\n", ach_result_to_string((ach_status_t)r));
printf("ACH: Opening Channel\n");
craftyo_chan = (ach_channel_t*) malloc (sizeof(ach_channel_t));
if(craftyo_chan == NULL) return 0;
r = ach_open(craftyo_chan, craftyo_chan_name, NULL);
if(r != ACH_OK) {
printf("ACH: %s\n", ach_result_to_string((ach_status_t)r));
return 0;
}
ach_chmod(craftyo_chan, SOMATIC_CHANNEL_MODE );
r = ach_flush(craftyo_chan);
if(r != ACH_OK) {
printf("ACH: %s\n", ach_result_to_string((ach_status_t)r));
return 0;
}
// Data
craftyo_message = (Somatic__Crafty*)malloc(sizeof(Somatic__Crafty));
if(craftyo_message == NULL) return 0;
somatic__crafty__init(craftyo_message);
//craftyo_message->boardstate.data = (uint8_t*) malloc(128);
//craftyo_message->has_boardstate = 1;
//craftyo_message->boardstate.len = 128;
craftyo_message->move = (char*)malloc(10);
printf("ACH: Info\n"
"ACH: Name: %s\n"
"ACH: Size: %d\n", craftyo_chan_name, somatic__crafty__get_packed_size(craftyo_message));
return 1;
}
int test_multi() {
ach_status_t r = ach_unlink(opt_channel_name);
if( ! ach_status_match(r, ACH_MASK_OK | ACH_MASK_ENOENT) ) {
fprintf(stderr, "ach_unlink failed\n: %s",
ach_result_to_string(r));
return -1;
}
r = ach_create(opt_channel_name, 32ul, 64ul, NULL );
/* pid_t sub_pid[opt_n_sub]; */
/* pid_t pub_pid[opt_n_pub]; */
int i;
/* create subscribers */
for( i = 0; i < opt_n_sub; i++ ) {
pid_t p = fork();
if( p < 0 ) exit(-1);
else if( 0 == p ) return subscriber(i);
/* else sub_pid[i] = p; */
}
/* create publishers */
for( i = 0; i < opt_n_pub; i++ ) {
pid_t p = fork();
if( p < 0 ) exit(-1);
else if( 0 == p ) return publisher(i);
/* else pub_pid[i] = p; */
}
/* wait */
for( i = 0; i < opt_n_sub+opt_n_pub; i++ ) {
int s;
pid_t pid = wait(&s);
(void)pid;
if( 0 != s ) return -1;
}
return 0;
}
static int testsig(void)
{
enum ach_status r;
/* Fork 0 */
pid_t pid_p = fork();
check_errno( "Fork 0", pid_p );
/* GP: wait */
if( pid_p ) {
return testsig_gp(pid_p);
}
/* Parent */
/* Create Kernel Channel */
{
ach_create_attr_t attr;
ach_create_attr_init(&attr);
attr.map = ACH_MAP_KERNEL;
r = ach_unlink(OPT_CHAN);
if( ! ach_status_match(r, ACH_MASK_OK | ACH_MASK_ENOENT) ) {
fail_ach( "unlink before create", r );
}
r = ach_unlink(OPT_CHAN);
if( ACH_ENOENT != r ) fail_ach( "unlink noent", r );
check_ach( "ach_create",
ach_create( OPT_CHAN, opt_msg_cnt, opt_msg_size, &attr ) );
}
/* Open Kernel Channel */
struct ach_channel chan;
{
for(;;) {
usleep(1000); /* Race against udev */
r = ach_open( &chan, OPT_CHAN, NULL );
if( ACH_EACCES == r ) continue;
else if (ACH_OK == r) break;
else fail_ach("ach_open", r);
}
}
/* Install Parent sighandler */
sighandler_install();
/* fork 1 */
pid_t pid_c = fork();
check_errno( "fork 1", pid_c );
if( pid_c ) {
/* Parent: */
for(;;) {
usleep(10000); /* Racy... */
check_errno( "kill child", kill( pid_c, SIGUSR1) );
usleep(10000);
int i = 42;
check_ach( "put to child", ach_put( &chan, &i, sizeof(i) ) );
int status;
pid_t wp = waitpid( pid_c, &status, WNOHANG );
if( wp ) {
if( wp != pid_c ) {
fail_errno("Wait 1");
} else if( WIFEXITED(status) && (0 == WEXITSTATUS(status)) ) {
exit(EXIT_SUCCESS);
} else {
fprintf(stderr, "Child 1 failed\n");
exit(EXIT_FAILURE);
}
}
}
} else {
/* child */
sig_atomic_t s0, s1;
int i;
do {
size_t frame_size;
s0 = count_sigusr1;
r = ach_get(&chan, &i, sizeof(i), &frame_size, NULL, ACH_O_WAIT );
s1 = count_sigusr1;
check_ach("child sig get", r);
} while( s1 == s0 || s1 < 10 ); /* This is racy... */
printf("done: %s, %d,%d,%d\n", ach_result_to_string(r), s0, s1, i);
exit(EXIT_SUCCESS);
}
return 0;
}
int test_basic() {
/* unlink */
ach_status_t r = ach_unlink(opt_channel_name);
if( ! ach_status_match(r, ACH_MASK_OK | ACH_MASK_ENOENT) ) {
fprintf(stderr, "ach_unlink failed: %s\n",
ach_result_to_string(r));
return -1;
}
/* create */
r = ach_create(opt_channel_name, 32ul, 64ul, NULL );
test(r, "ach_create");
ach_channel_t chan;
int s, p;
size_t frame_size;
struct timespec ts;
/* open */
r = ach_open(&chan, opt_channel_name, NULL);
/* empty channel means stale */
r = ach_get( &chan, &s, sizeof(s), &frame_size, NULL,
ACH_O_LAST);
if( ACH_STALE_FRAMES != r ) {
printf("get stale failed: %s\n", ach_result_to_string(r));
exit(-1);
}
r = ach_get( &chan, &s, sizeof(s), &frame_size, NULL,
0);
if( ACH_STALE_FRAMES != r ) {
printf("get stale failed: %s\n", ach_result_to_string(r));
exit(-1);
}
/* put */
p = 42;
r = ach_put( &chan, &p, sizeof(p) );
test(r, "ach_put");
/* get */
r = ach_get( &chan, &s, sizeof(s), &frame_size, NULL,
0);
test(r, "first ach_get");
if(frame_size!= sizeof(s) || s != 42 ) exit(-1);
/* put 2 */
p = 43;
r = ach_put( &chan, &p, sizeof(p) );
test(r, "ach_put");
p = 44;
r = ach_put( &chan, &p, sizeof(p) );
test(r, "ach_put");
/* get last */
r = ach_get( &chan, &s, sizeof(s), &frame_size, NULL,
ACH_O_LAST);
if( ACH_MISSED_FRAME != r ) {
printf("get last failed: %s\n", ach_result_to_string(r));
exit(-1);
}
if(frame_size != sizeof(s) || s != 44 ) exit(-1);
/* wait last */
p = 45;
r = ach_put( &chan, &p, sizeof(p) );
test(r, "ach_put");
clock_gettime(ACH_DEFAULT_CLOCK, &ts);
ts.tv_sec += 30; /* don't yield too long now */
r = ach_get( &chan, &s, sizeof(s), &frame_size, &ts,
ACH_O_LAST | ACH_O_WAIT );
if( ACH_OK != r ) {
printf("get wait failed: %s\n", ach_result_to_string(r));
exit(-1);
}
if(frame_size != sizeof(s) || s != 45 ) exit(-1);
/* get last stale */
r = ach_get( &chan, &s, sizeof(s), &frame_size, NULL,
ACH_O_LAST);
if( ACH_STALE_FRAMES != r ) {
printf("get stale failed: %s\n", ach_result_to_string(r));
exit(-1);
}
/* timeout */
clock_gettime(ACH_DEFAULT_CLOCK, &ts);
ts.tv_sec -= 10;
r = ach_get( &chan, &s, sizeof(s), &frame_size, &ts,
ACH_O_LAST | ACH_O_WAIT );
if( ACH_TIMEOUT != r ) {
printf("get timeout failed: %s\n", ach_result_to_string(r));
exit(-1);
}
/* copy last */
printf("> copy start\n");
r = ach_get( &chan, &s, sizeof(s), &frame_size, NULL,
ACH_O_LAST | ACH_O_COPY);
printf("> copy done\n");
if( ACH_OK != r ) {
printf("copy_last failed: %s\n", ach_result_to_string(r));
exit(-1);
}
if( p != s ) {
printf("wrong copy last : %d\n", s);
exit(-1);
}
/* get copy */
/*ach_dump(chan.shm);*/
/*printf("chan seq_num: %"PRIu64"\n", chan.seq_num);*/
r = ach_get( &chan, &s, sizeof(s), &frame_size, NULL,
ACH_O_COPY);
if( ACH_OK != r ) {
printf("copy_last failed: %s\n", ach_result_to_string(r));
exit(-1);
}
/* missed frames */
size_t i;
for( i = 0; i < 100; i ++ ) {
r = ach_put( &chan, &p, sizeof(p) );
test(r, "ach_put");
}
r = ach_get( &chan, &s, sizeof(s), &frame_size, NULL,
ACH_O_LAST);
if( ACH_MISSED_FRAME != r ) {
printf("get missed failed: %s\n", ach_result_to_string(r));
exit(-1);
}
/* close */
r = ach_close(&chan);
test(r, "ach_close");
/* unlink */
r = ach_unlink(opt_channel_name);
test(r, "ach_unlink");
fprintf(stderr, "basic ok\n");
return 0;
}
