00001
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include <stdio.h>
00036 #include <unistd.h>
00037 #include <stdarg.h>
00038 #include <string.h>
00039 #include <sys/types.h>
00040 #include <sys/socket.h>
00041 #include <sys/time.h>
00042 #include <sys/select.h>
00043 #include <sys/ipc.h>
00044 #include <sys/shm.h>
00045 #include <netinet/in.h>
00046 #include <arpa/inet.h>
00047 #include <net/if.h>
00048 #include <limits.h>
00049 #include <errno.h>
00050 #include <netdb.h>
00051
00052 #include "mesh_ipc.h"
00053 #include "sensor_self.h"
00054 #include "mesh_discovery.h"
00055 #include "mesh_protocol.h"
00056 #include "mesh_metrics.h"
00057 #include "mesh_net.h"
00058 #include "mesh_neighbor.h"
00059
00060
00061 static char *keywords[20] = { "rediscover", "getRecvBuf", "setRecvBuf",
00062 "getSendBuf", "setSendBuf",
00063 "getMTU", "setMTU",
00064 "getResendTimeout", "setResendTimeout",
00065 "getRouteMetric", "getLinkMetric",
00066 "getNeighborAddress",
00067 "getNeighborPosition",
00068 "getSocket", "sendSeqData", "recvSeqData",
00069 "sendRawData", "recvRawData",
00070 "sendNeighborData", "recvNeighborData", };
00071
00072 #define REQUEST_LEN 256 // TODO: this is arbitrary
00073
00074
00075
00076 #ifdef MESH_IPC_SERVER
00077
00078 int mesh_ioserv(self_t *self)
00079 {
00080 int sd, error;
00081 struct sockaddr_in my_addr, their_addr;
00082 socklen_t addr_len = sizeof(their_addr);
00083 struct hostent *he;
00084
00085 if ((sd = socket(PF_INET, SOCK_DGRAM, 0)) < 0) {
00086 p_error("mesh_ioserv", "socket");
00087 return sd;
00088 }
00089
00090 if ((he = gethostbyname("127.0.0.1")) == NULL) {
00091 h_error("mesh_ioserv", "gethostbyname (localhost)");
00092 return -1;
00093 }
00094
00095 my_addr.sin_family = AF_INET;
00096 my_addr.sin_port = htons(MESH_CONTROL_PORT);
00097 my_addr.sin_addr.s_addr = INADDR_ANY;
00098 memset(&(my_addr.sin_zero), 0, sizeof(my_addr.sin_zero));
00099
00100 if ((error = bind(sd, (struct sockaddr*)&my_addr, sizeof(my_addr))) < 0) {
00101 p_error("mesh_ioserv", "bind");
00102 return error;
00103 }
00104
00105 while (1) {
00106 int nbytes, length = 128;
00107 char buf[length], reply[length], *args = NULL;
00108 int request, error = 0;
00109 char *keyword;
00110
00111 if ((nbytes = recvfrom(sd, buf, length, 0,
00112 (struct sockaddr*)&their_addr, &addr_len)) < 0) {
00113 p_error("mesh_ioserv", "recvfrom");
00114 continue;
00115 }
00116 if (sscanf(buf, "%d %s", &request, args) < 1) {
00117 p_error("mesh_ioserv", "sscanf");
00118 continue;
00119 }
00120 keyword = keywords[request - MESH_IOREDISCVR];
00121
00122 switch (request) {
00123 case MESH_IOREDISCVR:
00124 propagate_routes(protocol_by_string(self, args), MESH_REDISCOVER);
00125 break;
00126 case MESH_IOGSNDQ:
00127 sprintf(reply, "%d", protocol_by_string(self, args)->sendqueue_size);
00128 break;
00129 case MESH_IOSSNDQ: {
00130 int data, error = 0;
00131 if ((error = sscanf(args, "%d %s", &data, args)) < 1) {
00132 if (error < 0)
00133 p_error(keyword, "sscanf");
00134 } else
00135 protocol_by_string(self, args)->sendqueue_size = data;
00136 sprintf(reply, "%d", error);
00137 break; }
00138 case MESH_IOGMTU:
00139 sprintf(reply, "%d", protocol_by_string(self, args)->mtu);
00140 break;
00141 case MESH_IOSMTU:
00142 {
00143 int data, error = 0;
00144 if ((error = sscanf(args, "%d %s", &data, args)) < 1) {
00145 if (error < 0)
00146 p_error(keyword, "sscanf");
00147 } else
00148 protocol_by_string(self, args)->mtu = data;
00149 sprintf(reply, "%d", error);
00150 break;
00151 }
00152 case MESH_IOGSNDTMO:
00153 sprintf(reply, "%lu:%lu",
00154 (unsigned long) (protocol_by_string(self, args)->queue_timeout
00155 / 1000),
00156 (unsigned long) ((protocol_by_string(self, args)->queue_timeout
00157 % 1000) * 1000));
00158 break;
00159 case MESH_IOSSNDTMO:
00160 {
00161 struct timeval tv;
00162 if ((error = sscanf(args, "%lu:%lu %s", &tv.tv_sec, &tv.tv_usec,
00163 args)) < 2) {
00164 if (error < 0)
00165 p_error(keyword, "sscanf");
00166 } else {
00167 protocol_by_string(self, args)->queue_timeout =
00168 ((u_int64_t)tv.tv_sec) * 1000 + ((u_int64_t)tv.tv_usec) / 1000;
00169 }
00170 sprintf(reply, "%d", error);
00171 break;
00172 }
00173 case MESH_IOGRTMTR:
00174 case MESH_IOGLNKMTR:
00175 {
00176 double derror = -1.0;
00177 char protocol_name[80];
00178 char metric_name[80];
00179 char destination[(NET_ADDR_LEN > MAC_ADDR_LEN) ?
00180 NET_ADDR_LEN : MAC_ADDR_LEN];
00181 if ((error = sscanf(args, "%s %s %s",
00182 metric_name, destination, protocol_name)) < 3) {
00183 if (error < 0)
00184 p_error(keyword, "sscanf");
00185 derror = (double)error;
00186 } else {
00187 if (request == MESH_IOGRTMTR) {
00188 protocol_t *proto = protocol_by_string(self, protocol_name);
00189 int metric = metric_by_name(metric_name);
00190
00191 if (proto != NULL && metric >= 0)
00192 derror = compile_mesh_metrics(proto, metric, destination);
00193 } else if (request == MESH_IOGLNKMTR)
00194 derror = 0.0 - ENOSYS;
00195 }
00196 sprintf(reply, "%lf", derror);
00197 break;
00198 }
00199 case MESH_IOGNGHADDR:
00200 {
00201 char mac_addr[MAC_ADDR_LEN];
00202 double angle, distance;
00203
00204 if ((error = sscanf(args, "%lf:%lf", &angle, &distance)) < 2) {
00205 if (error < 0)
00206 p_error(keyword, "sscanf");
00207 } else {
00208 if ((error = neighbor_address(self, mac_addr, angle, distance)) < 0)
00209 p_error(keyword, "neighbor_address");
00210 }
00211 sprintf(reply, "%hhu:%hhu:%hhu:%hhu:%hhu:%hhu:%hhu:%hhu",
00212 mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3],
00213 mac_addr[4], mac_addr[5], mac_addr[6], mac_addr[7]);
00214 break;
00215 }
00216 case MESH_IOGNGHPOS: {
00217 double angle, distance;
00218 char mac_addr[MAC_ADDR_LEN];
00219
00220 if ((error = sscanf(args, "%hhu:%hhu:%hhu:%hhu:%hhu:%hhu:%hhu:%hhu",
00221 &mac_addr[0], &mac_addr[1], &mac_addr[2],
00222 &mac_addr[3], &mac_addr[4], &mac_addr[5],
00223 &mac_addr[6], &mac_addr[7])) < 8)
00224
00225 if ((error = neighbor_position(self, &angle, &distance, mac_addr)) < 0)
00226 p_error(keyword, "neighbor_position");
00227
00228 sprintf(reply, "%lf:%lf", angle, distance);
00229 break; }
00230 case MESH_IOSOCKET: {
00231 int data, error = 0;
00232 if ((error = sscanf(args, "%d", &data)) < 1) {
00233 if (error < 0)
00234 p_error(keyword, "sscanf");
00235 } else
00236 error = mesh_socket(self->interface, data);
00237 sprintf(reply, "%d", error);
00238 break; }
00239 case MESH_IOSENDSEQ:
00240 case MESH_IOSENDRAW:
00241 case MESH_IOSENDNEAR:
00242 {
00243 int fd, shmid, len;
00244 char peer[40], proto_name[PROTO_NAMELEN];
00245 void *shmptr;
00246 protocol_t *proto;
00247
00248 if ((error = sscanf(args, "%s %d %d %d %s",
00249 proto_name, &fd, &shmid, &len, peer)) < 5) {
00250 if (error < 0)
00251 p_error(keyword, "sscanf");
00252 sprintf(reply, "%d", error);
00253 break;
00254 }
00255 if ((proto = protocol_by_string(self, proto_name)) == NULL) {
00256 p_error(keyword, "invalid protocol");
00257 sprintf(reply, "%d", -1);
00258 break;
00259 }
00260 if ((shmptr = shmat(shmid, NULL, 0)) == NULL) {
00261 p_error(keyword, "shmat");
00262 sprintf(reply, "%d", -1);
00263 break;
00264 }
00265 if (request == MESH_IOSENDSEQ) {
00266 struct sockaddr_in to;
00267 inet_aton(peer, &to.sin_addr);
00268 error = mesh_send(proto, fd, shmptr, len, &to);
00269 } else if (request == MESH_IOSENDRAW) {
00270 struct sockaddr_in to;
00271 inet_aton(peer, &to.sin_addr);
00272 error = mesh_raw_send(proto, fd, shmptr, len, &to);
00273 } else if (request == MESH_IOSENDNEAR) {
00274 double angle, dist;
00275 neighbor_position(self, &angle, &dist, peer);
00276 error = mesh_send_nextdoor(self, fd, shmptr, len, angle, dist);
00277 }
00278 shmdt(shmptr);
00279 sprintf(reply, "%d", error);
00280 break;
00281 }
00282 case MESH_IORECVSEQ:
00283 case MESH_IORECVRAW:
00284 case MESH_IORECVNEAR:
00285 {
00286 int fd, shmid, len;
00287 char peer[40], proto_name[PROTO_NAMELEN];
00288 void *shmptr;
00289 protocol_t *proto;
00290
00291 if ((error = sscanf(args, "%s %d %d %d %s",
00292 proto_name, &fd, &shmid, &len, peer)) < 4) {
00293 if (error < 0)
00294 p_error(keyword, "sscanf");
00295 sprintf(reply, "%d", error);
00296 break;
00297 }
00298 proto = protocol_by_string(self, proto_name);
00299 if ((shmptr = shmat(shmid, NULL, 0)) == NULL) {
00300 p_error(keyword, "shmat");
00301 sprintf(reply, "%d", -1);
00302 break;
00303 }
00304 if (request == MESH_IORECVNEAR) {
00305 double angle, dist;
00306 error = mesh_recv_nextdoor(self, fd, shmptr, len, &angle, &dist);
00307 neighbor_address(self, peer, angle, dist);
00308 } else {
00309 struct sockaddr_in from;
00310 error = mesh_recv(proto->sensor, fd, shmptr, len, &from);
00311 strcpy(peer, inet_ntoa(from.sin_addr));
00312 }
00313 shmdt(shmptr);
00314 sprintf(reply, "%d %s", error, peer);
00315 break;
00316 }
00317 }
00318
00319 if ((error = sendto(sd, reply, strlen(reply), 0,
00320 (struct sockaddr*)&their_addr, addr_len)) < 0) {
00321 p_error("mesh_ioserv", "sendto");
00322 continue;
00323 }
00324
00325 }
00326 return 0;
00327 }
00328
00329
00330 #else // not MESH_IPC_SERVER
00331
00332
00333 static int sd;
00334 static struct sockaddr_in their_addr;
00335
00336 int mesh_ipc_init(void)
00337 {
00338 struct hostent *he;
00339
00340 if ((sd = socket(PF_INET, SOCK_DGRAM, 0)) < 0) {
00341 p_error("mesh_ipc_init", "socket");
00342 return sd;
00343 }
00344
00345 if ((he = gethostbyname("127.0.0.1")) == NULL) {
00346 h_error("mesh_ipc_init", "gethostbyname (localhost)");
00347 return -1;
00348 }
00349 their_addr.sin_family = AF_INET;
00350 their_addr.sin_port = htons(MESH_CONTROL_PORT);
00351 their_addr.sin_addr = *((struct in_addr*)he->h_addr);
00352 memset(&(their_addr.sin_zero), 0, sizeof(their_addr.sin_zero));
00353
00354 return 0;
00355 }
00356
00357
00358 static int ipc_timed_recv(int sd, char *buf, int len, int timeout)
00359 {
00360 int n;
00361 fd_set fds;
00362 struct timeval tv;
00363
00364 FD_ZERO(&fds);
00365 FD_SET(sd, &fds);
00366 tv.tv_sec = timeout;
00367 tv.tv_usec = 0;
00368
00369 n = select(sd+1, &fds, NULL, NULL, &tv);
00370 if (n == 0) {
00371 errno = ETIMEDOUT;
00372 return -2;
00373 }
00374 if (n < 0)
00375 return -1;
00376 return recv(sd, buf, len, 0);
00377 }
00378
00379
00380 static int mesh_network_metadata(char *answer, size_t len,
00381 const char *format, ...)
00382 {
00383 int error = 0, reply_size = 32;
00384 char *keyword, request[REQUEST_LEN];
00385 char reply[reply_size];
00386 va_list args, extra;
00387
00388 va_start(args, format);
00389 va_copy(extra, args);
00390 vsprintf(request, format, args);
00391 keyword = keywords[va_arg(extra, int) - MESH_IOREDISCVR];
00392
00393 if ((error = sendto(sd, request, strlen(request), 0,
00394 (struct sockaddr*)&their_addr,
00395 sizeof(struct sockaddr))) < 0) {
00396 p_error(keyword, "sendto");
00397 return error;
00398 }
00399
00400 if ((error = connect(sd, (struct sockaddr*)&their_addr,
00401 sizeof(their_addr))) < 0) {
00402 p_error(keyword, "connect");
00403 return error;
00404 }
00405
00406 if ((error = ipc_timed_recv(sd, reply, reply_size, 1)) < 0) {
00407 p_error(keyword, "recv");
00408 return error;
00409 }
00410
00411 va_end(args);
00412 return error;
00413 }
00414
00415
00416 int mesh_ioctl(int request, ...)
00417 {
00418 va_list args;
00419 int error = 0;
00420 char *keyword = keywords[request - MESH_IOREDISCVR];
00421 int errval, reply_size = 32;
00422 char reply[reply_size];
00423
00424 va_start(args, request);
00425
00426 switch (request) {
00427 case MESH_IOREDISCVR:
00428 {
00429 char *protocol = va_arg(args, char*);
00430
00431 if ((errval = mesh_network_metadata(reply, reply_size, "%d %s",
00432 request, protocol)) < 0)
00433 return errval;
00434
00435 if ((errval = sscanf(reply, "%u", &error)) < 1) {
00436 if (errval < 0)
00437 p_error(keyword, "sscanf");
00438 return errval;
00439 }
00440 break;
00441 }
00442 case MESH_IOGSNDQ:
00443 case MESH_IOGMTU:
00444 {
00445 int *size = va_arg(args, int*);
00446 if ((errval = mesh_network_metadata(reply, reply_size, "%d",
00447 request)) < 0)
00448 return errval;
00449
00450 if ((errval = sscanf(reply, "%u", size)) < 1) {
00451 if (errval < 0)
00452 p_error(keyword, "sscanf");
00453 return errval;
00454 }
00455 break;
00456 }
00457 case MESH_IOSSNDQ:
00458 case MESH_IOSMTU:
00459 {
00460 int size = va_arg(args, int);
00461 if ((errval = mesh_network_metadata(reply, reply_size, "%d %d",
00462 request, size)) < 0)
00463 return errval;
00464
00465 if ((errval = sscanf(reply, "%d", &error)) < 1) {
00466 if (errval< 0)
00467 p_error(keyword, "sscanf");
00468 return errval;
00469 }
00470 break;
00471 }
00472 case MESH_IOGRTMTR:
00473 case MESH_IOGLNKMTR:
00474 {
00475 double *data = va_arg(args, double*);
00476 char *metric_name = va_arg(args, char*);
00477 char * destination = va_arg(args, char*);
00478
00479 if ((errval = mesh_network_metadata(reply, reply_size, "%d %s %s",
00480 request, metric_name, destination)) < 0)
00481 return errval;
00482
00483 if ((errval = sscanf(reply, "%lf", data)) < 1) {
00484 if (errval < 0)
00485 p_error(keyword, "sscanf");
00486 return errval;
00487 }
00488 break;
00489 }
00490 case MESH_IOGSNDTMO:
00491 {
00492 long *secs = va_arg(args, long*);
00493 long *usecs = va_arg(args, long*);
00494
00495 if ((errval = mesh_network_metadata(reply, reply_size, "%s",
00496 request)) < 0)
00497 return errval;
00498
00499 if ((errval = sscanf(reply, "%lu:%lu", secs, usecs)) < 2) {
00500 if (errval < 0)
00501 p_error(keyword, "sscanf");
00502 return errval;
00503 }
00504 break;
00505 }
00506 case MESH_IOSSNDTMO:
00507 {
00508 long secs = va_arg(args, long);
00509 long usecs = va_arg(args, long);
00510
00511 if ((errval = mesh_network_metadata(reply, reply_size, "%d %lu:%lu",
00512 request, secs, usecs)) < 0)
00513 return errval;
00514
00515 if ((errval = sscanf(reply, "%d", &error)) < 1) {
00516 if (errval< 0)
00517 p_error(keyword, "sscanf");
00518 return errval;
00519 }
00520 break;
00521 }
00522 case MESH_IOGNGHADDR:
00523 {
00524 char *mac_addr = va_arg(args, char*);
00525 double angle = va_arg(args, double);
00526 double distance = va_arg(args, double);
00527
00528 if ((errval = mesh_network_metadata(reply, reply_size, "%s %lf:%lf",
00529 request, angle, distance)) < 0)
00530 return errval;
00531
00532 if ((errval = sscanf(reply, "%hhu:%hhu:%hhu:%hhu:%hhu:%hhu:%hhu:%hhu",
00533 &mac_addr[0], &mac_addr[1], &mac_addr[2],
00534 &mac_addr[3], &mac_addr[4], &mac_addr[5],
00535 &mac_addr[6], &mac_addr[7])) < 8) {
00536 if (errval < 0)
00537 p_error(keyword, "sscanf");
00538 return errval;
00539 }
00540 break;
00541 }
00542 case MESH_IOGNGHPOS:
00543 {
00544 double *angle = va_arg(args, double*);
00545 double *distance = va_arg(args, double*);
00546 char * mac_addr = va_arg(args, char*);
00547
00548 if ((errval =
00549 mesh_network_metadata(reply, reply_size,
00550 "%d %hhu:%hhu:%hhu:%hhu:%hhu:%hhu:%hhu:%hhu",
00551 request, mac_addr[0], mac_addr[1],
00552 mac_addr[2], mac_addr[3], mac_addr[4],
00553 mac_addr[5], mac_addr[6], mac_addr[7])) < 0)
00554 return errval;
00555
00556 if ((errval = sscanf(reply, "%lf:%lf", angle, distance)) < 2) {
00557 if (errval< 0)
00558 p_error(keyword, "sscanf");
00559 return errval;
00560 }
00561 break;
00562 }
00563 case MESH_IOSOCKET:
00564 {
00565 int *sd = va_arg(args, int*);
00566 int port = va_arg(args, int);
00567
00568 if ((errval = mesh_network_metadata(reply, reply_size, "%d %d",
00569 request, port)) < 0)
00570 return errval;
00571
00572 if ((errval = sscanf(reply, "%u", sd)) < 1) {
00573 if (errval < 0)
00574 p_error(keyword, "sscanf");
00575 return errval;
00576 }
00577 break;
00578 }
00579 case MESH_IOSENDSEQ:
00580 case MESH_IOSENDRAW:
00581 case MESH_IOSENDNEAR:
00582 {
00583 int shmid;
00584 void *shmptr, *buf;
00585 int sd = va_arg(args, int);
00586 char *buffer = va_arg(args, char*);
00587 int len = va_arg(args, int);
00588 char *to = va_arg(args, char*);
00589
00590 if ((shmid = shmget(IPC_PRIVATE, MESH_HEADER_SPACE + len,
00591 (SHM_R | SHM_W))) < 0) {
00592 p_error(keyword, "shmget");
00593 return shmid;
00594 }
00595 if ((shmptr = shmat(shmid, NULL, 0)) == NULL) {
00596 p_error(keyword, "shmat");
00597 return -1;
00598 }
00599 memset(shmptr, 0, MESH_HEADER_SPACE + len);
00600 buf = shmptr + MESH_HEADER_SPACE;
00601 memcpy(buf, buffer, len);
00602 shmdt(shmptr);
00603
00604 if ((errval =
00605 mesh_network_metadata(reply, reply_size, "%d %d %d %d %s",
00606 request, sd, shmid, len, to)) < 0)
00607 return errval;
00608
00609 if ((errval = sscanf(reply, "%d", &error)) < 1) {
00610 if (errval< 0)
00611 p_error(keyword, "sscanf");
00612 return errval;
00613 }
00614 break;
00615 }
00616 case MESH_IORECVSEQ:
00617 case MESH_IORECVRAW:
00618 case MESH_IORECVNEAR:
00619 {
00620 int shmid;
00621 void *shmptr, *buf;
00622 int sd = va_arg(args, int);
00623 char *buffer = va_arg(args, char*);
00624 int len = va_arg(args, int);
00625 char *from = va_arg(args, char*);
00626
00627 if ((shmid = shmget(IPC_PRIVATE, MESH_HEADER_SPACE + len,
00628 (SHM_R | SHM_W))) < 0) {
00629 p_error(keyword, "shmget");
00630 return shmid;
00631 }
00632 if ((shmptr = shmat(shmid, NULL, 0)) == NULL) {
00633 p_error(keyword, "shmat");
00634 return -1;
00635 }
00636 memset(shmptr, 0, MESH_HEADER_SPACE + len);
00637 shmdt(shmptr);
00638
00639 if ((errval =
00640 mesh_network_metadata(reply, reply_size, "%d %d %d %d %s",
00641 request, sd, shmid, len)) < 0)
00642 return errval;
00643
00644 if ((errval = sscanf(reply, "%d %s", &error, from)) < 1) {
00645 if (errval< 0)
00646 p_error(keyword, "sscanf");
00647 return errval;
00648 }
00649
00650 if ((shmptr = shmat(shmid, NULL, 0)) == NULL) {
00651 p_error(keyword, "shmat");
00652 return -1;
00653 }
00654 buf = shmptr + MESH_HEADER_SPACE;
00655 memcpy(buffer, buf, len);
00656 shmdt(shmptr);
00657 shmctl(shmid, IPC_RMID, 0);
00658 break;
00659 }
00660 }
00661 va_end(args);
00662 return error;
00663 }
00664
00665 #endif