src/naonet.c

   1 /*
   2  * nao-ulib
   3  * Copyright 2010 - 2011 Daniel Borkmann <dborkma@tik.ee.ethz.ch>
   4  * Subject to the GPL.
   5  * Nao-Team HTWK,
   6  * Faculty of Computer Science, Mathematics and Natural Sciences,
   7  * Leipzig University of Applied Sciences (HTWK Leipzig)
   8  */
   9
  10 /* Needs here & there some cleanups and rewrites */
  11
  12 #include <stdint.h>
  13 #include <stdio.h>
  14 #include <unistd.h>
  15 #include <sys/types.h>
  16 #include <sys/stat.h>
  17 #include <fcntl.h>
  18 #include <stdlib.h>
  19 #include <errno.h>
  20 #include <string.h>
  21 #include <pthread.h>
  22 #include <sys/types.h>
  23 #include <netinet/in.h>
  24
  25 #include "naonet.h"
  26 #include "comp_x86.h"
  27
  28 #define NET_DEV_FILE         "/dev/nao"
  29 #define NET_DEV_MAX_SEG_SIZE 1500
  30
  31 pthread_mutex_t        list_head_lock;
  32 static naonet_block_t *list_head = NULL;
  33
  34 static int __register_rx_hook(naonet_block_t **head, naonet_block_t *block)
  35 {
  36         if(!block || !head)
  37                 return -EINVAL;
  38         if(!block->rx_hook)
  39                 return -EINVAL;
  40
  41         pthread_mutex_lock(&list_head_lock);
  42         while((*head) != NULL) {
  43                 if(block->priority > (*head)->priority)
  44                         break;
  45                 head = &((*head)->next);
  46         }
  47         block->next = (*head);
  48         (*head) = block;
  49         pthread_mutex_unlock(&list_head_lock);
  50
  51         return 0;
  52 }
  53
  54 static int __register_rx_hook_once(naonet_block_t **head, naonet_block_t *block)
  55 {
  56         if(!block || !head)
  57                 return -EINVAL;
  58         if(!block->rx_hook)
  59                 return -EINVAL;
  60
  61         pthread_mutex_lock(&list_head_lock);
  62         while((*head) != NULL) {
  63                 if(unlikely(block == (*head)))
  64                         return (-EEXIST);
  65                 if(block->priority > (*head)->priority)
  66                         break;
  67                 head = &((*head)->next);
  68         }
  69         block->next = (*head);
  70         (*head) = block;
  71         pthread_mutex_unlock(&list_head_lock);
  72
  73         return 0;
  74 }
  75
  76 static int __unregister_rx_hook(naonet_block_t **head, naonet_block_t *block)
  77 {
  78         if(!block || !head)
  79                 return -EINVAL;
  80
  81         pthread_mutex_lock(&list_head_lock);
  82         while((*head) != NULL) {
  83                 if(unlikely(block == (*head))) {
  84                         (*head) = block->next;
  85                         break;
  86                 }
  87                 head = &((*head)->next);
  88         }
  89         pthread_mutex_unlock(&list_head_lock);
  90
  91         return 0;
  92 }
  93
  94 static int __call_rx_hook_chain(naonet_block_t **head, uint8_t type,
  95                                 uint32_t src, char *message, size_t len,
  96                                 int *called)
  97 {
  98         int rc = BLOCK_SUCC_DONE;
  99         naonet_block_t *block = (*head), *next_block;
 100
 101         if(!head || !message || !len)
 102                 return -EINVAL;
 103         if(called)
 104                 (*called) = 0;
 105
 106         pthread_mutex_lock(&list_head_lock);
 107         while(block) {
 108                 next_block = block->next;
 109                 if(block->type != type)
 110                         goto next;
 111                 rc = block->rx_hook(block, src, message, len);
 112                 if((rc & BLOCK_STOP_CHAIN) == BLOCK_STOP_CHAIN)
 113                         break;
 114                 if(called)
 115                         (*called)++;
 116 next:
 117                 block = next_block;
 118         }
 119         pthread_mutex_unlock(&list_head_lock);
 120         return rc;
 121 }
 122
 123 int register_rx_hook(naonet_block_t *block)
 124 {
 125         return __register_rx_hook(&list_head, block);
 126 }
 127
 128 int register_rx_hook_once(naonet_block_t *block)
 129 {
 130         return __register_rx_hook_once(&list_head, block);
 131 }
 132
 133 int unregister_rx_hook(naonet_block_t *block)
 134 {
 135         return __unregister_rx_hook(&list_head, block);
 136 }
 137
 138 static int call_rx_hook_chain(uint8_t type, uint32_t src, char *message,
 139                               size_t len, int *called)
 140 {
 141         return  __call_rx_hook_chain(&list_head, type, src, message, len,
 142                                      called);
 143 }
 144
 145 int send_message(char *message, size_t len, uint8_t type)
 146 {
 147         int fd, rc = 0;
 148         size_t ret;
 149         char buf[NET_DEV_MAX_SEG_SIZE];
 150
 151         if(!message || !len || len >= NET_DEV_MAX_SEG_SIZE)
 152                 return -EINVAL;
 153
 154         fd = open(NET_DEV_FILE, O_WRONLY);
 155         if(fd < 0) {
 156                 perror("open");
 157                 exit(EXIT_FAILURE);
 158         }
 159
 160         /* No memset in fastpath */
 161         memcpy(&buf[1], message, len);
 162         buf[0] = type;
 163
 164         ret = write(fd, buf, len + 1);
 165         if(ret != (len + 1)) {
 166                 perror("write");
 167                 rc = -EIO;
 168         }
 169
 170         close(fd);
 171         return rc;
 172 }
 173
 174 static void *rx_thread(void *null)
 175 {
 176         int fd;
 177         size_t len;
 178         char buf[NET_DEV_MAX_SEG_SIZE];
 179         naonet_header_t *nh;
 180
 181         fd = open(NET_DEV_FILE, O_RDONLY);
 182         if(fd < 0) {
 183                 perror("open");
 184                 pthread_exit(0);
 185         }
 186
 187         while((len = read(fd, buf, sizeof(buf))) > 0) {
 188                 nh = (naonet_header_t *) ((void *) buf);
 189                 call_rx_hook_chain((uint8_t) (*(buf + sizeof(*nh))), nh->src_id,
 190                                    (buf + sizeof(*nh) + 1), ntohs(nh->len) - 1,
 191                                    NULL);
 192                 memset(buf, 0, sizeof(buf));
 193         }
 194
 195         close(fd);
 196         pthread_exit(0);
 197 }
 198
 199 uint32_t naonet_fetch_id(void)
 200 {
 201         int fd;
 202         uint32_t id = -1;
 203         char buff[200] = {0};
 204
 205         fd = open("/proc/naonet/id", O_RDONLY);
 206         if(fd < 0) {
 207                 perror("Cannot open /proc/naonet/id");
 208                 return EXIT_FAILURE;
 209         }
 210
 211         if(read(fd, buff, sizeof(buff)) > 0) {
 212                 sscanf(buff, "%u", &id);
 213         }
 214         close(fd);
 215
 216         if(id == -1)
 217                 printf("Cannot parse naonet ID from /var/log/messages!\n");
 218         return id;
 219 }
 220
 221 int naonet_init(pthread_t *rx_thread_p)
 222 {
 223         int rc;
 224
 225         if(!rx_thread_p)
 226                 return -EINVAL;
 227
 228         rc = pthread_create(rx_thread_p, NULL, rx_thread, NULL);
 229         if(rc) {
 230                 perror("pthread_create");
 231                 exit(EXIT_FAILURE);
 232         }
 233
 234         pthread_detach((*rx_thread_p));
 235         return 0;
 236 }