/*
* Late initialization enables I/O power.
*/
int bebopr_post_init( void)
{
int result = -1;
/*
* IO_PWR_ON = R9 / GPIO1[6] / gpio38 / gpmc_ad6
* !IO_PWR_ON = R8 / GPIO1[2] / gpio34 / gpmc_ad2
*/
gpio_write_int_value_to_file( "export", 38);
gpio_write_value_to_pin_file( 38, "direction", "out");
gpio_write_value_to_pin_file( 38, "value", "1");
gpio_write_int_value_to_file( "export", 34);
gpio_write_value_to_pin_file( 34, "direction", "out");
gpio_write_value_to_pin_file( 34, "value", "0");
fprintf( stderr, "Turned BEBOPR I/O power on\n");
result = 0;
return result;
}
static void limsw_exit( void)
{
// Gracefully handle exit
for (int i = 0 ; i < nr_limits ; ++i) {
if (gpio_fd[ i]) {
close( gpio_fd[ i]);
gpio_fd[ i] = -1;
}
gpio_write_value_to_pin_file( limit_gpios[ i], "edge", "none");
if (get_kernel_type() == e_kernel_3_2) {
gpio_write_int_value_to_file( "unexport", limit_gpios[ i]);
}
}
}
static void* limsw_watcher( void* arg)
{
int i;
unsigned int gpio[ nr_limits];
char buf[ 3];
int len;
if (debug_flags & DEBUG_LIMSW) {
printf( "Limit switches watcher thread: opening fds");
}
for (i = 0 ; i < nr_limits ; ++i) {
gpio[ i] = limit_gpios[ i];
if (get_kernel_type() == e_kernel_3_2) {
gpio_write_int_value_to_file( "export", gpio[ i]);
gpio_write_value_to_pin_file( gpio[ i], "direction", "in");
}
gpio_write_value_to_pin_file( gpio[ i], "edge", "both");
gpio_fd[ i] = gpio_open_file( gpio[ i], "value");
if (gpio_fd[ i] < 0) {
fprintf( stderr, ", open failed for gpio%d, bailing out\n", gpio[ i]);
pthread_exit( NULL);
}
}
atexit( limsw_exit);
for (i = 0 ; i < nr_limits ; ++i) {
(*fdset)[ i].fd = gpio_fd[ i];
(*fdset)[ i].events = POLLPRI;
}
if (debug_flags & DEBUG_LIMSW) {
printf( ", start polling on:");
for (i = 0 ; i < nr_limits ; ++i) {
printf( " gpio%d", gpio[ i]);
}
printf( "\n");
}
while (1) {
const int timeout = -1; /* no timeout */
int rc = poll( *fdset, nr_limits, timeout);
if (rc < 0) {
if (errno == EINTR) {
continue;
}
perror( "limsw_watcher: poll() failed, bailing out!");
break;
}
if (rc == 0) {
// poll timeout, shouldn't happen with 'timeout' == -1 !!!
fprintf( stderr, "\nlimsw_watcher: poll() unexpected timeout, bailing out!\n");
break;
}
for (i = 0 ; i < nr_limits ; ++i) {
if ((*fdset)[ i].revents & POLLPRI) {
int state = 0;
len = read( (*fdset)[ i].fd, buf, sizeof( buf));
if (len == 2 && (buf[ 0] == '0' || buf[ 0] == '1')) {
// Get initial state info
state = buf[ 0] - '0';
if (debug_flags & DEBUG_LIMSW) {
printf( "*** GPIO %2d event, state %d ***\n", gpio[ i], state);
}
} else {
fprintf( stderr, "\n*** GPIO %2d unexpected event, BUG??? ***\n", gpio[ i]);
}
switch (gpio[ i]) {
case XMIN_GPIO: x_min_state = state; break;
case XMAX_GPIO: x_max_state = state; break;
case YMIN_GPIO: y_min_state = state; break;
case YMAX_GPIO: y_max_state = state; break;
case ZMIN_GPIO: z_min_state = state; break;
case ZMAX_GPIO: z_max_state = state; break;
}
lseek( (*fdset)[ i].fd, 0, SEEK_SET);
}
}
}
pthread_exit( NULL);
}
