Improved IMEI filtering

Makefile

@@ -4,6 +4,7 @@ LDFLAGS=
 LIBS=
 TARGET=wstationd
 SRC= \
+	src/imei.c \
 	src/wstationd.c
 OBJ=$(SRC:.c=.o)
 

src/imei.c

@@ -0,0 +1,79 @@
+#include <stdlib.h>
+#include <inttypes.h>
+#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;
+}

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

src/wstationd.c

@@ -18,7 +18,10 @@
 #include <poll.h>
 #include <syslog.h>
 #include <signal.h>
+#include <inttypes.h>
+
 #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 <limit>    Set concurrent connection limit (default: 128)\n");
 	fprintf(stdout, "  -p, --port <port>      Port to listen on (default: 10800)\n");
 	fprintf(stdout, "  -b, --bind <address>   Address to bind to (default: 0.0.0.0)\n");
-	fprintf(stdout, "  -i, --imei <imei list> Comma separated list of IMEIs to allow (default: all)\n");
+	fprintf(stdout, "  -i, --imei <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,10 +346,16 @@ 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){
+						// 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)
@@ -345,8 +365,6 @@ int main(int argc, char* argv[])
 									exec_name, 15, (conns[i-1]->buff + 10)
 								);
 							}
-								ok = 0;
-							}
 						}
 
 						if(ok) write_connection(conns[i - 1], strip);

src/wstationd.h

@@ -1 +1 @@
-#define VERSION "1.1"
+#define VERSION "1.5"