N-sim Emulation and simulation of Wireless Sensor Networks    Home    Project Page    Download    CVS    Installation    Configuration    Plug-ins  Hosted by

/home/brennan/n-sim/Vaike/linux/system-addons/networking/mesh_neighbor.c Go to the documentation of this file. /* * Copyright 2007. Los Alamos National Security, LLC. This material * was produced under U.S. Government contract DE-AC52-06NA25396 for * Los Alamos National Laboratory (LANL), which is operated by Los * Alamos National Security, LLC, for the Department of Energy. The * U.S. Government has rights to use, reproduce, and distribute this * software. NEITHER THE GOVERNMENT NOR LOS ALAMOS NATIONAL SECURITY, * LLC, MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY LEGAL * LIABILITY FOR THE USE OF THIS SOFTWARE. If software is modified to * produce derivative works, such modified software should be clearly * marked, so as not to confuse it with the version available from LANL. * * Additionally, this program is free software; you can redistribute * it and/or modify it under the terms of the GNU General Public * License as published by the Free Software Foundation; version 2 of * the License. Accordingly, this program is distributed in the hope * it will be useful, but WITHOUT ANY WARRANTY; without even the * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR * PURPOSE. See the GNU General Public License for more details. */ #include <stdio.h> #include <stdlib.h> #include <string.h> #include <unistd.h> #include <sys/socket.h> #include <netinet/in.h> #include <arpa/inet.h> #include <net/route.h> #include <errno.h> #include <syslog.h> #include <netdb.h> #include <math.h> #include "mesh_neighbor.h" #include "sensor_self.h" #define MESH_N_DISCOVERY_PORT 652 #define HELLO_LEN ((sizeof(double)*3) + 32) #define HELLO_INTERVAL 180 // 3 minutes unsigned int mac_to_id(char *addr) { return addr[4] << 12 | addr[5] << 8 | addr[6] << 4 | addr[7]; } void init_n_entry(self_t *self, neighbor_entry_t *entry) { entry->node_id = mac_to_id(self->mac_addr); entry->lat = self->latitude; entry->lon = self->longitude; entry->alt = self->altitude; } neighbor_entry_t *new_n_entry(self_t *self) { neighbor_entry_t *neighbor = malloc(sizeof(neighbor_entry_t)); init_n_entry(self, neighbor); return neighbor; } int add_n_entry(self_t *self, unsigned int id, neighbor_entry_t *entry) { if (self == NULL || entry == NULL) { errno = EINVAL; return -1; } entry->node_id = id; return avl_insert(entry, id, &self->neighbors); } int remove_n_entry(self_t *self, unsigned int id, neighbor_entry_t *entry) { if (self == NULL) { errno = EINVAL; return -1; } return avl_delete(entry, id, &self->neighbors); } neighbor_entry_t *retrieve_n_entry(self_t *self, unsigned int id) { return avl_retrieve(id, &self->neighbors); } neighbor_entry_t *iterate_n_entries(self_t *self) { if (self->n_iter == NULL) self->n_iter = avl_find_min(&self->neighbors); else self->n_iter = self->n_iter->next; if (self->n_iter == NULL) return NULL; return (neighbor_entry_t*) self->n_iter->element; } unsigned int num_n_entries(self_t *self) { return avl_size(&self->neighbors); } void empty_n_entries(self_t *self) { neighbor_entry_t *cur = NULL; self->n_iter = NULL; while ((cur = iterate_n_entries(self)) != NULL) free(cur); destroy_avltree(self->neighbors.root); self->neighbors.root = NULL; } neighbor_entry_t *neighbor_by_position(self_t *self, double bearing, double distance) { neighbor_entry_t *cur = NULL; self->n_iter = NULL; while ((cur = iterate_n_entries(self)) != NULL) { if (fabs(bearing - cur->bearing) < 1 && fabs(distance - cur->distance) < 1) return cur; } return NULL; } neighbor_entry_t *neighbor_by_ip(self_t *self, struct sockaddr_in *ip) { neighbor_entry_t *cur = NULL; self->n_iter = NULL; while ((cur = iterate_n_entries(self)) != NULL) { if (memcmp(&ip->sin_addr, &cur->ip.sin_addr, sizeof(struct sockaddr_in)) == 0) return cur; } return NULL; } int neighbor_address(self_t *self, char *addr, double bearing, double distance) { neighbor_entry_t *cur = neighbor_by_position(self, bearing, distance); if (cur == NULL) return -1; memcpy(addr, cur->address, MAC_ADDR_LEN); return 0; } int neighbor_position(self_t *self, double *bearing, double *distance, char *addr) { neighbor_entry_t *entry = retrieve_n_entry(self, mac_to_id(addr)); if (entry == NULL || memcmp(entry->address, addr, MAC_ADDR_LEN) != 0) { errno = EINVAL; return -1; } *bearing = entry->bearing; *distance = entry->distance; return 0; } int find_neighbors(self_t *self) { int sd; char hello[HELLO_LEN]; struct sockaddr_in to; struct hostent *he; if ((sd = mesh_socket(self->interface, MESH_N_DISCOVERY_PORT)) < 0) return sd; if ((he = gethostbyname(self->broadcast)) == NULL) { syslog(LOG_ERR, "gethostbyname(%s): %s", self->broadcast, hstrerror(h_errno)); return -1; } to.sin_family = AF_INET; to.sin_port = htons(MESH_N_DISCOVERY_PORT); to.sin_addr = *((struct in_addr*)he->h_addr); memset(&(to.sin_zero), 0, sizeof(to.sin_zero)); sprintf(hello, "HELLO::%hhu:%hhu:%hhu:%hhu:%hhu:%hhu:%hhu:%hhu::%lf:%lf:%lf", self->mac_addr[0], self->mac_addr[1], self->mac_addr[2], self->mac_addr[3], self->mac_addr[4], self->mac_addr[5], self->mac_addr[6], self->mac_addr[7], self->latitude, self->longitude, self->altitude); return mesh_sendmsg(self->interface, self->mac_addr, sd, hello, strlen(hello), &to); } // TODO: make three dimensional /* using a spherical approximation (haversine) */ static double latlng_to_dist(double lat1, double lng1, double alt1, double lat2, double lng2, double alt2) { double unit_conversion = 1852.0; /* meters */ double to_radians = M_PI / 180.0; double to_degrees = 180.0 / M_PI; double rad_lat1 = to_radians * lat1; double rad_lat2 = to_radians * lat2; double dlng = (to_radians * lng2) - (to_radians * lng1); double a = pow(sin(dlng / 2), 2) + cos(rad_lat1) * cos(rad_lat2) * pow(sin(dlng / 2), 2); double dist = 2 * atan2(sqrt(a), sqrt(1.0 - a)); return (dist * to_degrees) * 60.0 * unit_conversion; } // TODO: make three dimensional static double latlng_to_bearing(double lat1, double lng1, double alt1, double lat2, double lng2, double alt2) { double to_radians = M_PI / 180.0; double to_degrees = 180.0 / M_PI; double rad_lat1 = to_radians * lat1; double rad_lat2 = to_radians * lat2; double dlng = (to_radians * lng2) - (to_radians * lng1); double y = sin(dlng) * cos(rad_lat2); double x = cos(rad_lat1) * sin(rad_lat2) - sin(rad_lat1) * cos(rad_lat2) * cos(dlng); double theta = atan2(y, x) * to_degrees; return (theta < 0.0) ? (theta + 360.0) : theta; } /* thread */ int neighbor_discovery(self_t *self) { int sd; if (self == NULL) { errno = EINVAL; return -1; } if ((sd = mesh_socket(self->interface, MESH_N_DISCOVERY_PORT)) < 0) return sd; while (1) { ssize_t bytes = 0; int error = 0; char hello[HELLO_LEN+1]; struct sockaddr_in from; neighbor_entry_t *neighbor; if ((bytes = timed_recv(sd, hello, HELLO_LEN, &from, HELLO_INTERVAL)) < 0) { if (bytes == -2) find_neighbors(self); else syslog(LOG_ERR, "timed_recv: %m"); continue; } hello[bytes] = '\0'; neighbor = new_n_entry(self); if ((error = sscanf(hello, "HELLO::%hhu:%hhu:%hhu:%hhu:%hhu:%hhu:%hhu:%hhu::%lf:%lf:%lf", &neighbor->address[0], &neighbor->address[1], &neighbor->address[2], &neighbor->address[3], &neighbor->address[4], &neighbor->address[5], &neighbor->address[6], &neighbor->address[7], &neighbor->lat, &neighbor->lon, &neighbor->alt)) < 11) { if (error < 0) syslog(LOG_ERR, "sscanf: %m"); free(neighbor); } memcpy(&neighbor->ip, &from, sizeof(struct sockaddr_in)); neighbor->ip.sin_port = htons(MESH_N_COMM_PORT); neighbor->node_id = mac_to_id(neighbor->address); neighbor->bearing = latlng_to_bearing(self->latitude, self->longitude, self->altitude, neighbor->lat, neighbor->lon, neighbor->alt); neighbor->distance = latlng_to_dist(self->latitude, self->longitude, self->altitude, neighbor->lat, neighbor->lon, neighbor->alt); neighbor->next = NULL; add_n_entry(self, neighbor->node_id, neighbor); } close(sd); return 0; }

© 2007, Los Alamos National Security, LLC.