You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
367 lines
7.4 KiB
367 lines
7.4 KiB
|
|
#include <stdlib.h>
|
|
#include <stdio.h>
|
|
#include <stdint.h>
|
|
#include <unistd.h>
|
|
#include <errno.h>
|
|
#include <string.h>
|
|
#include <sys/types.h>
|
|
#include <sys/time.h>
|
|
#include <sys/stat.h>
|
|
#include <sys/mman.h>
|
|
#include <sys/socket.h>
|
|
#include <sys/un.h>
|
|
#include <netdb.h>
|
|
#include <pthread.h>
|
|
#include <signal.h>
|
|
#include <fcntl.h>
|
|
#include <time.h>
|
|
|
|
int quit = 0;
|
|
|
|
uint32_t *gpio;
|
|
|
|
int enable = 0;
|
|
uint32_t interval = 20000000;
|
|
uint32_t width = 1500000;
|
|
struct timeval last_update = {0, 0};
|
|
struct timespec max_idle_time = {10, 0};
|
|
|
|
double timespec_to_double(struct timespec t)
|
|
{
|
|
return t.tv_sec + t.tv_nsec * 1e-9;
|
|
}
|
|
|
|
double timeval_to_double(struct timeval t)
|
|
{
|
|
return t.tv_sec + t.tv_usec * 1e-6;
|
|
}
|
|
|
|
void do_pwm()
|
|
{
|
|
struct timespec on = {0, width}, off = {0, interval - width};
|
|
|
|
while (!quit)
|
|
{
|
|
if (enable) { gpio[7] = 1 << 18; }
|
|
nanosleep(&on, NULL);
|
|
gpio[10] = 1 << 18;
|
|
nanosleep(&off, NULL);
|
|
on.tv_nsec = width;
|
|
off.tv_nsec = interval - width;
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
void do_quit()
|
|
{
|
|
quit = 1;
|
|
}
|
|
|
|
struct sock_params
|
|
{
|
|
int sock;
|
|
struct sockaddr *addr;
|
|
socklen_t addrlen;
|
|
struct addrinfo *freeme;
|
|
};
|
|
|
|
int do_resolve(char *address, char *portstr, struct sock_params *params)
|
|
{
|
|
if (strspn(address, "unix:") == 5)
|
|
{
|
|
address += 5;
|
|
int sock = socket(AF_UNIX, SOCK_STREAM, 0);
|
|
if (sock == -1)
|
|
return errno;
|
|
struct sockaddr_un *addr = malloc(sizeof(struct sockaddr_un));;
|
|
addr->sun_family = AF_UNIX; // Polymorphism in C is pretty cool.
|
|
strncpy(addr->sun_path, address, sizeof(addr->sun_path) - 1);
|
|
addr->sun_path[sizeof(addr->sun_path) - 1] = 0;
|
|
params->sock = sock;
|
|
params->addr = (struct sockaddr*) addr;
|
|
params->addrlen = sizeof(struct sockaddr_un);
|
|
params->freeme = NULL;
|
|
} else
|
|
{
|
|
struct addrinfo hints =
|
|
{
|
|
.ai_flags = 0,
|
|
.ai_family = AF_UNSPEC,
|
|
.ai_socktype = SOCK_STREAM,
|
|
.ai_protocol = 0,
|
|
};
|
|
struct addrinfo *ai;
|
|
unsigned short port = 7140;
|
|
int err;
|
|
if (portstr == NULL)
|
|
{
|
|
if ((err = getaddrinfo(address, NULL, &hints, &ai)))
|
|
{
|
|
return err;
|
|
}
|
|
((struct sockaddr_in*) ai->ai_addr)->sin_port = htons(port);
|
|
} else if (1 == sscanf(portstr, "%hu", &port))
|
|
{
|
|
if ((err = getaddrinfo(address, NULL, &hints, &ai)))
|
|
{
|
|
return err;
|
|
}
|
|
((struct sockaddr_in*) ai->ai_addr)->sin_port = htons(port);
|
|
} else
|
|
{
|
|
if ((err = getaddrinfo(address, portstr, &hints, &ai)))
|
|
{
|
|
return err;
|
|
}
|
|
}
|
|
int sock = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol);
|
|
if (sock == -1)
|
|
return errno;
|
|
params->sock = sock;
|
|
params->addr = ai->ai_addr;
|
|
params->addrlen = ai->ai_addrlen;
|
|
params->freeme = ai;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void do_unresolve(struct sock_params params)
|
|
{
|
|
close(params.sock);
|
|
if (params.freeme == NULL)
|
|
free(params.addr);
|
|
freeaddrinfo(params.freeme);
|
|
}
|
|
|
|
char *address, *portstr;
|
|
|
|
struct llnode
|
|
{
|
|
struct llnode *next;
|
|
struct llnode *prev;
|
|
struct llnode *next_done;
|
|
int done;
|
|
pthread_t *join;
|
|
};
|
|
|
|
struct llist
|
|
{
|
|
struct llnode *head;
|
|
pthread_mutex_t lock;
|
|
};
|
|
|
|
struct conn_info
|
|
{
|
|
int sockfd;
|
|
struct llnode *this;
|
|
struct llist *list;
|
|
};
|
|
|
|
void do_conn(struct conn_info *ci)
|
|
{
|
|
int sockfd = ci->sockfd;
|
|
|
|
printf("accepted conn\n");
|
|
|
|
FILE *sockstream = fdopen(sockfd, "rw");
|
|
|
|
int pwm;
|
|
if (1 == fscanf(sockstream, "%i", &pwm))
|
|
{
|
|
if (pwm == 0)
|
|
{
|
|
printf("disabling pwm\n");
|
|
enable = 0;
|
|
shutdown(sockfd, SHUT_RDWR);
|
|
} else if (pwm > 0 && pwm < 20000)
|
|
{
|
|
gettimeofday(&last_update, NULL);
|
|
printf("pwm at: %i\n", pwm);
|
|
enable = 1;
|
|
width = pwm * 1000;
|
|
shutdown(sockfd, SHUT_RDWR);
|
|
struct timespec delay = max_idle_time, rem = {0, 0};
|
|
while (delay.tv_nsec != 0 || delay.tv_sec != 0)
|
|
{
|
|
nanosleep(&delay, &rem);
|
|
delay = rem;
|
|
}
|
|
|
|
struct timeval tod;
|
|
gettimeofday(&tod, NULL);
|
|
if (timeval_to_double(tod) - timeval_to_double(last_update) + 0.1 >= timespec_to_double(max_idle_time))
|
|
{
|
|
enable = 0;
|
|
printf("disabling pwm due to idle timeout\n");
|
|
} else
|
|
{
|
|
printf("[debug] no timeout, not disabling\n");
|
|
}
|
|
} else
|
|
{
|
|
printf("pwm out of range\n");
|
|
shutdown(sockfd, SHUT_RDWR);
|
|
}
|
|
} else
|
|
{
|
|
printf("pwm not an integer\n");
|
|
shutdown(sockfd, SHUT_RDWR);
|
|
}
|
|
|
|
fclose(sockstream);
|
|
|
|
struct llist *list = ci->list;
|
|
pthread_mutex_lock(&list->lock);
|
|
ci->this->next_done = list->head->next_done;
|
|
list->head->next_done = ci->this;
|
|
ci->this->done = 1;
|
|
pthread_mutex_unlock(&list->lock);
|
|
|
|
free(ci);
|
|
}
|
|
|
|
void node_unlink(struct llnode *node)
|
|
{
|
|
node->next->prev = node->prev;
|
|
node->prev->next = node->next;
|
|
}
|
|
|
|
void walk_and_destroy_done(struct llist *list)
|
|
{
|
|
pthread_mutex_lock(&list->lock);
|
|
struct llnode *active = list->head->next_done;
|
|
list->head->next_done = NULL;
|
|
while (active != NULL)
|
|
{
|
|
node_unlink(active);
|
|
pthread_join(*active->join, NULL);
|
|
struct llnode *next = active->next_done;
|
|
free(active->join);
|
|
free(active);
|
|
active = next;
|
|
}
|
|
pthread_mutex_unlock(&list->lock);
|
|
}
|
|
|
|
void listener()
|
|
{
|
|
struct sock_params params;
|
|
if (do_resolve(address, portstr, ¶ms))
|
|
{
|
|
perror("could not resolve bind address");
|
|
goto exit;
|
|
}
|
|
|
|
signal(SIGPIPE, SIG_IGN);
|
|
|
|
struct llist list;
|
|
pthread_mutex_init(&list.lock, NULL);
|
|
struct llnode head = { &head, &head, NULL, 0, NULL };
|
|
list.head = &head;
|
|
|
|
int ec, one = 1;
|
|
setsockopt(params.sock, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(int));
|
|
|
|
ec = bind(params.sock, params.addr, params.addrlen);
|
|
if (ec)
|
|
{
|
|
perror("could not bind");
|
|
goto exit;
|
|
}
|
|
|
|
ec = listen(params.sock, 1);
|
|
if (ec)
|
|
{
|
|
perror("could not open listener");
|
|
goto exit;
|
|
}
|
|
|
|
struct linger linger = {0, 0};
|
|
setsockopt(params.sock, SOL_SOCKET, SO_LINGER, &linger, sizeof(struct linger));
|
|
|
|
while (!quit)
|
|
{
|
|
walk_and_destroy_done(&list);
|
|
|
|
int sockfd = accept(params.sock, NULL, NULL);
|
|
if (ec == -1)
|
|
{
|
|
close(params.sock);
|
|
perror("could not open listener");
|
|
goto exit;
|
|
}
|
|
|
|
pthread_t *handler = malloc(sizeof(pthread_t));
|
|
struct llnode *spnode = malloc(sizeof(struct llnode));
|
|
struct conn_info *ci = malloc(sizeof(struct conn_info));
|
|
spnode->next_done = NULL;
|
|
spnode->done = 0;
|
|
spnode->join = handler;
|
|
|
|
pthread_mutex_lock(&list.lock);
|
|
spnode->next = head.next;
|
|
spnode->prev = &head;
|
|
head.next->prev = spnode;
|
|
head.next = spnode;
|
|
pthread_mutex_unlock(&list.lock);
|
|
|
|
ci->sockfd = sockfd;
|
|
ci->list = &list;
|
|
ci->this = spnode;
|
|
|
|
pthread_create(handler, NULL, (void *(*)(void*)) do_conn, ci);
|
|
}
|
|
|
|
exit:
|
|
pthread_mutex_destroy(&list.lock);
|
|
do_unresolve(params);
|
|
quit = 1;
|
|
return;
|
|
}
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
if (argc != 3)
|
|
{
|
|
fprintf(stderr, "incorrect usage\n");
|
|
return 1;
|
|
}
|
|
|
|
address = argv[1];
|
|
portstr = argv[2];
|
|
|
|
int gpiofd = open("/dev/gpiomem", O_RDWR);
|
|
if (gpiofd == -1)
|
|
{
|
|
perror("could not open memory file");
|
|
return 1;
|
|
}
|
|
gpio = mmap(NULL, 12 * 4096, PROT_READ | PROT_WRITE, MAP_SHARED, gpiofd, 0);
|
|
if (gpio == MAP_FAILED)
|
|
{
|
|
fprintf(stderr, "could not map gpio\n");
|
|
return 1;
|
|
}
|
|
|
|
gpio[1] = 1 << 24;
|
|
|
|
signal(SIGINT, do_quit);
|
|
signal(SIGTERM, do_quit);
|
|
|
|
pthread_t pwmthread;
|
|
pthread_create(&pwmthread, NULL, (void *(*)(void*)) do_pwm, NULL);
|
|
|
|
pthread_t listenthread;
|
|
pthread_create(&listenthread, NULL, (void *(*)(void*)) listener, NULL);
|
|
|
|
while (!quit) { pause(); }
|
|
|
|
exit:
|
|
quit = 1;
|
|
pthread_join(pwmthread, NULL);
|
|
|
|
munmap(gpio, 4096);
|
|
close(gpiofd);
|
|
}
|