A collection of files from lightspi that weren't under VCS.
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

  1. #include <stdlib.h>
  2. #include <stdio.h>
  3. #include <stdint.h>
  4. #include <unistd.h>
  5. #include <errno.h>
  6. #include <string.h>
  7. #include <sys/types.h>
  8. #include <sys/time.h>
  9. #include <sys/stat.h>
  10. #include <sys/mman.h>
  11. #include <sys/socket.h>
  12. #include <sys/un.h>
  13. #include <netdb.h>
  14. #include <pthread.h>
  15. #include <signal.h>
  16. #include <fcntl.h>
  17. #include <time.h>
  18. int quit = 0;
  19. uint32_t *gpio;
  20. int enable = 0;
  21. uint32_t interval = 20000000;
  22. uint32_t width = 1500000;
  23. struct timeval last_update = {0, 0};
  24. struct timespec max_idle_time = {10, 0};
  25. double timespec_to_double(struct timespec t)
  26. {
  27. return t.tv_sec + t.tv_nsec * 1e-9;
  28. }
  29. double timeval_to_double(struct timeval t)
  30. {
  31. return t.tv_sec + t.tv_usec * 1e-6;
  32. }
  33. void do_pwm()
  34. {
  35. struct timespec on = {0, width}, off = {0, interval - width};
  36. while (!quit)
  37. {
  38. if (enable) { gpio[7] = 1 << 18; }
  39. nanosleep(&on, NULL);
  40. gpio[10] = 1 << 18;
  41. nanosleep(&off, NULL);
  42. on.tv_nsec = width;
  43. off.tv_nsec = interval - width;
  44. }
  45. return;
  46. }
  47. void do_quit()
  48. {
  49. quit = 1;
  50. }
  51. struct sock_params
  52. {
  53. int sock;
  54. struct sockaddr *addr;
  55. socklen_t addrlen;
  56. struct addrinfo *freeme;
  57. };
  58. int do_resolve(char *address, char *portstr, struct sock_params *params)
  59. {
  60. if (strspn(address, "unix:") == 5)
  61. {
  62. address += 5;
  63. int sock = socket(AF_UNIX, SOCK_STREAM, 0);
  64. if (sock == -1)
  65. return errno;
  66. struct sockaddr_un *addr = malloc(sizeof(struct sockaddr_un));;
  67. addr->sun_family = AF_UNIX; // Polymorphism in C is pretty cool.
  68. strncpy(addr->sun_path, address, sizeof(addr->sun_path) - 1);
  69. addr->sun_path[sizeof(addr->sun_path) - 1] = 0;
  70. params->sock = sock;
  71. params->addr = (struct sockaddr*) addr;
  72. params->addrlen = sizeof(struct sockaddr_un);
  73. params->freeme = NULL;
  74. } else
  75. {
  76. struct addrinfo hints =
  77. {
  78. .ai_flags = 0,
  79. .ai_family = AF_UNSPEC,
  80. .ai_socktype = SOCK_STREAM,
  81. .ai_protocol = 0,
  82. };
  83. struct addrinfo *ai;
  84. unsigned short port = 7140;
  85. int err;
  86. if (portstr == NULL)
  87. {
  88. if ((err = getaddrinfo(address, NULL, &hints, &ai)))
  89. {
  90. return err;
  91. }
  92. ((struct sockaddr_in*) ai->ai_addr)->sin_port = htons(port);
  93. } else if (1 == sscanf(portstr, "%hu", &port))
  94. {
  95. if ((err = getaddrinfo(address, NULL, &hints, &ai)))
  96. {
  97. return err;
  98. }
  99. ((struct sockaddr_in*) ai->ai_addr)->sin_port = htons(port);
  100. } else
  101. {
  102. if ((err = getaddrinfo(address, portstr, &hints, &ai)))
  103. {
  104. return err;
  105. }
  106. }
  107. int sock = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol);
  108. if (sock == -1)
  109. return errno;
  110. params->sock = sock;
  111. params->addr = ai->ai_addr;
  112. params->addrlen = ai->ai_addrlen;
  113. params->freeme = ai;
  114. }
  115. return 0;
  116. }
  117. void do_unresolve(struct sock_params params)
  118. {
  119. close(params.sock);
  120. if (params.freeme == NULL)
  121. free(params.addr);
  122. freeaddrinfo(params.freeme);
  123. }
  124. char *address, *portstr;
  125. struct llnode
  126. {
  127. struct llnode *next;
  128. struct llnode *prev;
  129. struct llnode *next_done;
  130. int done;
  131. pthread_t *join;
  132. };
  133. struct llist
  134. {
  135. struct llnode *head;
  136. pthread_mutex_t lock;
  137. };
  138. struct conn_info
  139. {
  140. int sockfd;
  141. struct llnode *this;
  142. struct llist *list;
  143. };
  144. void do_conn(struct conn_info *ci)
  145. {
  146. int sockfd = ci->sockfd;
  147. printf("accepted conn\n");
  148. FILE *sockstream = fdopen(sockfd, "rw");
  149. int pwm;
  150. if (1 == fscanf(sockstream, "%i", &pwm))
  151. {
  152. if (pwm == 0)
  153. {
  154. printf("disabling pwm\n");
  155. enable = 0;
  156. shutdown(sockfd, SHUT_RDWR);
  157. } else if (pwm > 0 && pwm < 20000)
  158. {
  159. gettimeofday(&last_update, NULL);
  160. printf("pwm at: %i\n", pwm);
  161. enable = 1;
  162. width = pwm * 1000;
  163. shutdown(sockfd, SHUT_RDWR);
  164. struct timespec delay = max_idle_time, rem = {0, 0};
  165. while (delay.tv_nsec != 0 || delay.tv_sec != 0)
  166. {
  167. nanosleep(&delay, &rem);
  168. delay = rem;
  169. }
  170. struct timeval tod;
  171. gettimeofday(&tod, NULL);
  172. if (timeval_to_double(tod) - timeval_to_double(last_update) + 0.1 >= timespec_to_double(max_idle_time))
  173. {
  174. enable = 0;
  175. printf("disabling pwm due to idle timeout\n");
  176. } else
  177. {
  178. printf("[debug] no timeout, not disabling\n");
  179. }
  180. } else
  181. {
  182. printf("pwm out of range\n");
  183. shutdown(sockfd, SHUT_RDWR);
  184. }
  185. } else
  186. {
  187. printf("pwm not an integer\n");
  188. shutdown(sockfd, SHUT_RDWR);
  189. }
  190. fclose(sockstream);
  191. struct llist *list = ci->list;
  192. pthread_mutex_lock(&list->lock);
  193. ci->this->next_done = list->head->next_done;
  194. list->head->next_done = ci->this;
  195. ci->this->done = 1;
  196. pthread_mutex_unlock(&list->lock);
  197. free(ci);
  198. }
  199. void node_unlink(struct llnode *node)
  200. {
  201. node->next->prev = node->prev;
  202. node->prev->next = node->next;
  203. }
  204. void walk_and_destroy_done(struct llist *list)
  205. {
  206. pthread_mutex_lock(&list->lock);
  207. struct llnode *active = list->head->next_done;
  208. list->head->next_done = NULL;
  209. while (active != NULL)
  210. {
  211. node_unlink(active);
  212. pthread_join(*active->join, NULL);
  213. struct llnode *next = active->next_done;
  214. free(active->join);
  215. free(active);
  216. active = next;
  217. }
  218. pthread_mutex_unlock(&list->lock);
  219. }
  220. void listener()
  221. {
  222. struct sock_params params;
  223. if (do_resolve(address, portstr, &params))
  224. {
  225. perror("could not resolve bind address");
  226. goto exit;
  227. }
  228. signal(SIGPIPE, SIG_IGN);
  229. struct llist list;
  230. pthread_mutex_init(&list.lock, NULL);
  231. struct llnode head = { &head, &head, NULL, 0, NULL };
  232. list.head = &head;
  233. int ec, one = 1;
  234. setsockopt(params.sock, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(int));
  235. ec = bind(params.sock, params.addr, params.addrlen);
  236. if (ec)
  237. {
  238. perror("could not bind");
  239. goto exit;
  240. }
  241. ec = listen(params.sock, 1);
  242. if (ec)
  243. {
  244. perror("could not open listener");
  245. goto exit;
  246. }
  247. struct linger linger = {0, 0};
  248. setsockopt(params.sock, SOL_SOCKET, SO_LINGER, &linger, sizeof(struct linger));
  249. while (!quit)
  250. {
  251. walk_and_destroy_done(&list);
  252. int sockfd = accept(params.sock, NULL, NULL);
  253. if (ec == -1)
  254. {
  255. close(params.sock);
  256. perror("could not open listener");
  257. goto exit;
  258. }
  259. pthread_t *handler = malloc(sizeof(pthread_t));
  260. struct llnode *spnode = malloc(sizeof(struct llnode));
  261. struct conn_info *ci = malloc(sizeof(struct conn_info));
  262. spnode->next_done = NULL;
  263. spnode->done = 0;
  264. spnode->join = handler;
  265. pthread_mutex_lock(&list.lock);
  266. spnode->next = head.next;
  267. spnode->prev = &head;
  268. head.next->prev = spnode;
  269. head.next = spnode;
  270. pthread_mutex_unlock(&list.lock);
  271. ci->sockfd = sockfd;
  272. ci->list = &list;
  273. ci->this = spnode;
  274. pthread_create(handler, NULL, (void *(*)(void*)) do_conn, ci);
  275. }
  276. exit:
  277. pthread_mutex_destroy(&list.lock);
  278. do_unresolve(params);
  279. quit = 1;
  280. return;
  281. }
  282. int main(int argc, char **argv)
  283. {
  284. if (argc != 3)
  285. {
  286. fprintf(stderr, "incorrect usage\n");
  287. return 1;
  288. }
  289. address = argv[1];
  290. portstr = argv[2];
  291. int gpiofd = open("/dev/gpiomem", O_RDWR);
  292. if (gpiofd == -1)
  293. {
  294. perror("could not open memory file");
  295. return 1;
  296. }
  297. gpio = mmap(NULL, 12 * 4096, PROT_READ | PROT_WRITE, MAP_SHARED, gpiofd, 0);
  298. if (gpio == MAP_FAILED)
  299. {
  300. fprintf(stderr, "could not map gpio\n");
  301. return 1;
  302. }
  303. gpio[1] = 1 << 24;
  304. signal(SIGINT, do_quit);
  305. signal(SIGTERM, do_quit);
  306. pthread_t pwmthread;
  307. pthread_create(&pwmthread, NULL, (void *(*)(void*)) do_pwm, NULL);
  308. pthread_t listenthread;
  309. pthread_create(&listenthread, NULL, (void *(*)(void*)) listener, NULL);
  310. while (!quit) { pause(); }
  311. exit:
  312. quit = 1;
  313. pthread_join(pwmthread, NULL);
  314. munmap(gpio, 4096);
  315. close(gpiofd);
  316. }