lightspi/lightspi.c

#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, &params))
	{
		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);
}