Created
October 13, 2020 06:56
-
-
Save Grissess/303510055d2167e90aee90284aa78bc5 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment