diff --git a/Makefile b/Makefile index 3a23c2c..d76dc45 100644 --- a/Makefile +++ b/Makefile @@ -4,6 +4,7 @@ LDFLAGS= LIBS= TARGET=wstationd SRC= \ + src/imei.c \ src/wstationd.c OBJ=$(SRC:.c=.o) diff --git a/src/imei.c b/src/imei.c new file mode 100644 index 0000000..43440d1 --- /dev/null +++ b/src/imei.c @@ -0,0 +1,79 @@ +#include +#include +#include "imei.h" + + +imei_set* imei_set_new(size_t capacity) +{ + imei_set* set = (imei_set*)malloc( + sizeof(imei_set) + (capacity * sizeof(uint64_t)) + ); + if(set == NULL) return NULL; + + set->sz = 0; + set->cp = capacity; + + return set; +} + + +// This is not efficient at all. It could be made vastly +// more efficient by having imei_set_add() insert in order +// and then implementing a binary search algorithm here +int imei_set_search(imei_set* set, uint64_t imei) +{ + for(size_t i = 0; i < set->sz; i++){ + if(set->imeis[i] == imei) return 1; + } + return 0; +} + + +void imei_set_free(imei_set *set){ + if(set != NULL) free(set); +} + + +int imei_set_add(imei_set** set, uint64_t imei) +{ + // Don't allow out-of-bounds imeis + if(imei == UINT64_MAX) return 1; + + // Don't allow duplicate entries + if(imei_set_search(*set, imei)) return 2; + + // If the set is full, expand it + if((*set)->sz == (*set)->cp){ + imei_set* nset = realloc( + *set, sizeof(imei_set) + (((*set)->cp + 1000) * sizeof(uint64_t)) + ); + if(nset == NULL) return 1; + nset->cp += 1000; + (*set) = nset; + } + + (*set)->imeis[(*set)->sz] = imei; + (*set)->sz++; + + return 0; +} + + +uint64_t imei_uint64(unsigned char *imei, size_t sz) +{ + // IMEI's are always 15 bytes in length + if(sz != 15) return UINT64_MAX; + + uint64_t r = 0; + for(int i = 0; i < 15; i++){ + // If any byte in the IMEI isn't a proper digit, + // return an error - UINT64_MAX is used because + // an imei will only use 60 bits, so the + // max for a 64-bit unsigned int is out of bounds. + if(imei[i] < '0' || imei[i] > '9') return UINT64_MAX; + + r = (r<<4) + (imei[i] - '0'); + } + + return r; +} diff --git a/src/imei.h b/src/imei.h new file mode 100644 index 0000000..e2c49c6 --- /dev/null +++ b/src/imei.h @@ -0,0 +1,30 @@ +#ifndef IMEI_SET_H +#define IMEI_SET_H + +typedef struct +{ + size_t sz; + size_t cp; + uint64_t imeis[]; +} imei_set; + + +// Convert a string representation of an IMEI +// to a uint64_t +uint64_t imei_uint64(unsigned char *, size_t); + +// Create a new IMEI set n size +imei_set* imei_set_new(size_t); + +// Free existing imei set +void imei_set_free(imei_set *); + +// Search an existing set for an IMEI, +// returns 1 if found, or 0 if not found +int imei_set_search(imei_set*, uint64_t); + +// Adds IMEI to existing set, +// returns 0 on success, non-zero on failure +int imei_set_add(imei_set**, uint64_t); + +#endif // IMEI_SET_H diff --git a/src/wstationd.c b/src/wstationd.c index 87cf96b..d9c16ca 100644 --- a/src/wstationd.c +++ b/src/wstationd.c @@ -18,7 +18,10 @@ #include #include #include +#include + #include "wstationd.h" +#include "imei.h" // As of the writing of this daemon, SBD maximum message length @@ -65,7 +68,8 @@ static void print_help() fprintf(stdout, " -l, --limit Set concurrent connection limit (default: 128)\n"); fprintf(stdout, " -p, --port Port to listen on (default: 10800)\n"); fprintf(stdout, " -b, --bind
Address to bind to (default: 0.0.0.0)\n"); - fprintf(stdout, " -i, --imei Comma separated list of IMEIs to allow (default: all)\n"); + fprintf(stdout, " -i, --imei IMEI to allow - specify multiple times for\n"); + fprintf(stdout, " additional IMEIs (default: allow all)\n"); fprintf(stdout, " -s, --strip Strip Iridium SBD header when writing files\n"); fprintf(stdout, " -h, --help Print this message and exit\n"); fprintf(stdout, " -v, --version Print version and exit\n"); @@ -94,8 +98,8 @@ int main(int argc, char* argv[]) struct connection** conns = NULL; struct pollfd* fds = NULL; - char *imei_list = NULL; - size_t imei_list_sz = 0; + imei_set* imeis = NULL; + uint64_t imei = 0; int strip = 0; @@ -122,15 +126,25 @@ int main(int argc, char* argv[]) break; case 'i': - imei_list = argv[optind-1]; - imei_list_sz = strlen(imei_list); - - // If the user provides the word "all" as the only imei entry, - // remove the filter. - if(imei_list_sz == 3 && memcmp(imei_list, "all", 3) == 0){ - imei_list_sz = 0; - imei_list = NULL; + imei = imei_uint64( + (unsigned char *)argv[optind-1], + strlen(argv[optind-1]) + ); + if(imei == UINT64_MAX){ + fprintf(stderr, "%s: invalid IMEI \"%s\"\n", exec_name, argv[optind-1]); + goto shutdown_error; } + + if(imeis == NULL){ + imeis = imei_set_new(255); + if(imeis == NULL){ + fprintf(stderr, "%s: cannot create IMEI set (out of memory?)\n", exec_name); + goto shutdown_error; + } + } + + // TODO: Probably should be some error handling here + imei_set_add(&imeis, imei); break; case 's': @@ -319,7 +333,7 @@ int main(int argc, char* argv[]) if(sz > 0) conns[i-1]->len += sz; else { - if(conns[i-1]->len > 3){ + if(conns[i-1]->len > 25){ uint16_t sbd_sz = *(uint16_t*)(&conns[i-1]->buff[1]); // Convert and add three bytes for the SBD header ssize_t expected = (ssize_t)htons(sbd_sz) + 3; @@ -332,20 +346,24 @@ int main(int argc, char* argv[]) // Is the protocol byte in place? if(ok && conns[i-1]->buff[0] != 1) ok = 0; - // Do we have an IMEI list? - if(imei_list_sz != 0 && conns[i-1]->len > 25){ - // If so, check IMEI against list - if(memmem(imei_list, imei_list_sz, (conns[i-1]->buff + 10), 15) == NULL){ - if(daemonize){ - syslog(LOG_NOTICE, "IMEI rejected: %.*s", - 15, (conns[i-1]->buff + 10) - ); - } else { - fprintf(stderr, "%s: IMEI rejected: %.*s\n", - exec_name, 15, (conns[i-1]->buff + 10) - ); - } - ok = 0; + // Parse connection's IMEI + uint64_t imei = imei_uint64((conns[i-1]->buff + 10), 15); + // Check for invalid IMEIs + if(ok && imei == UINT64_MAX) ok = 0; + + // If everything is ok so far, and + // we have a list of approved IMEIs, + // check it for this IMEI + if(ok && imeis != NULL && !imei_set_search(imeis, imei)){ + ok = 0; + if(daemonize){ + syslog(LOG_NOTICE, "IMEI rejected: %.*s", + 15, (conns[i-1]->buff + 10) + ); + } else { + fprintf(stderr, "%s: IMEI rejected: %.*s\n", + exec_name, 15, (conns[i-1]->buff + 10) + ); } } diff --git a/src/wstationd.h b/src/wstationd.h index 1fc901b..0b62be9 100644 --- a/src/wstationd.h +++ b/src/wstationd.h @@ -1 +1 @@ -#define VERSION "1.1" +#define VERSION "1.5"