initial commit

Change-Id: I11c093ad112268a35317f44a5c2500c3978f38dd
Signed-off-by: José Bollo <jose.bollo@iot.bzh>

1 files changed, 467 insertions, 0 deletions

diff --git a/src/af-gps-binding.c b/src/af-gps-binding.c
new file mode 100644
index 0000000..a5b0c94
--- /dev/null
+++ b/src/af-gps-binding.c
@@ -0,0 +1,467 @@
+/*
+ * Copyright (C) 2016 "IoT.bzh"
+ * Author José Bollo <jose.bollo@iot.bzh>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#define _GNU_SOURCE
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netdb.h>
+#include <fcntl.h>
+
+#include <json-c/json.h>
+
+#include <systemd/sd-event.h>
+
+#include <afb/afb-binding.h>
+
+#define NAUTICAL_MILE_IN_METER     1852
+#define KNOT_TO_METER_PER_SECOND   0.5144444444444444
+
+/*
+ * references:
+ *
+ *       https://www.w3.org/TR/geolocation-API/
+ *       http://www.gpsinformation.org/dale/nmea.htm
+ */
+
+/* declare the connection routine */
+static int nmea_connect();
+
+/*
+ * the interface to afb-daemon
+ */
+const struct afb_binding_interface *afbitf;
+
+struct gps {
+	uint32_t time;
+	double latitude;
+	double longitude;
+} gps;
+
+static struct json_object *position()
+{
+	return NULL;
+}
+
+static int nmea_time(const char *text, uint32_t *result)
+{
+	uint32_t x;
+
+	if (text[0] < '0' || text[0] > '2'
+	 || text[1] < '0' || text[1] > (text[0] == '2' ? '3' : '9')
+	 || text[2] < '0' || text[2] > '5'
+	 || text[3] < '0' || text[3] > '9'
+	 || text[4] < '0' || text[4] > '5'
+	 || text[5] < '0' || text[5] > '9'
+	 || (text[6] != 0 && text[6] != '.'))
+		return 0;
+
+	x = (uint32_t)(text[0] - '0');
+	x = x * 10 + (uint32_t)(text[1]-'0');
+	x = x *  6 + (uint32_t)(text[2]-'0');
+	x = x * 10 + (uint32_t)(text[3]-'0');
+	x = x *  6 + (uint32_t)(text[4]-'0');
+	x = x * 10 + (uint32_t)(text[5]-'0');
+	x = x * 1000;
+	if (text[6] == '.') {
+		if (text[7] != 0) {
+			if (text[7] < '0' || text[7] > '9') return 0;
+			x += (uint32_t)(text[7]-'0') * 100;
+			if (text[8] != 0) {
+				if (text[8] < '0' || text[8] > '9') return 0;
+				x += (uint32_t)(text[8]-'0') * 10;
+				if (text[9] != 0) {
+					if (text[9] < '0' || text[9] > '9') return 0;
+					x += (uint32_t)(text[9]-'0');
+					if (text[10] != 0) {
+						if (text[10] < '0' || text[10] > '9') return 0;
+						x += text[10] > '5';
+					}
+				}
+			}
+		}
+	}
+
+	*result = x;
+	return 1;
+}
+
+static int nmea_angle(const char *text, double *result)
+{
+	uint32_t x = 0;
+	int dotidx = (int)(strchrnul(text, '.') - text);
+
+	switch(dotidx) {
+	case 5:
+		if (text[dotidx - 5] < '0' || text[dotidx - 5] > '9')
+			return 0;
+		x = x * 10 + (uint32_t)(text[dotidx - 5] - '0');
+	case 4:
+		if (text[dotidx - 4] < '0' || text[dotidx - 4] > '9')
+			return 0;
+		x = x * 10 + (uint32_t)(text[dotidx - 4] - '0');
+	case 3:
+		if (text[dotidx - 3] < '0' || text[dotidx - 3] > '9')
+			return 0;
+		x = x * 10 + (uint32_t)(text[dotidx - 3] - '0');
+	case 2:
+		if (text[dotidx - 2] < '0' || text[dotidx - 2] > '9')
+			return 0;
+		x = x * 6 + (uint32_t)(text[dotidx - 3] - '0');
+	case 1:
+		if (text[dotidx - 1] < '0' || text[dotidx - 1] > '9')
+			return 0;
+		x = x * 10 + (uint32_t)(text[dotidx - 3] - '0');
+	case 0:
+		break;
+	default:
+		return 0;
+	}
+
+	if (text[dotidx] == '.')
+		*result = atof(&text[dotidx]) + x;
+	else
+		*result = x;
+
+	return 1;
+}
+
+static int nmea_set(
+		const char *tim,
+		const char *lat, const char *latu,
+		const char *lon, const char *lonu,
+		const char *alt, const char *altu,
+		const char *spe,
+		const char *tra,
+		const char *dat
+)
+{
+	uint32_t time;
+	double latitude, longitude, altitude, speed, track;
+	struct gps *gps;
+
+	DEBUG(afbitf, "time=%s latitude=%s%s longitude=%s%s altitude=%s%s speed=%s track=%s date=%s",
+		tim, lat, latu, lon, lonu, alt, altu, spe, tra, dat);
+
+	/* get the time in milliseconds */
+	if (tim != NULL) {
+		if (!nmea_time(tim, &time))
+			return 0;
+	}
+
+	/* get the latitude */
+	if (lat != NULL && latu != NULL) {
+		if ((latu[0] != 'N' && latu[0] != 'S') || latu[1] != 0)
+			return 0;
+		if (!nmea_angle(lat, &latitude))
+			return 0;
+		if (latu[0] == 'S')
+			latitude = -latitude;
+	}
+
+	/* get the longitude */
+	if (lon != NULL && lonu != NULL) {
+		if ((lonu[0] != 'E' && lonu[0] != 'W') || lonu[1] != 0)
+			return 0;
+		if (!nmea_angle(lon, &longitude))
+			return 0;
+		if (lonu[0] == 'W')
+			longitude = 360.0 - longitude;
+	}
+
+	/* get the altitude */
+	if (alt != NULL && altu != NULL) {
+		if (altu[0] != 'M' || altu[1] != 0)
+			return 0;
+		altitude = atof(alt);
+	}
+
+	/* get the speed */
+	if (spe != NULL) {
+		speed = atof(spe) * KNOT_TO_METER_PER_SECOND;
+	}
+
+	/* get the track */
+	if (tra != NULL) {
+		track = atof(tra);
+	}
+
+	return 1;
+}
+
+static int nmea_split(char *s, char *fields[], int count)
+{
+	int index = 0;
+	while (*s && index < count) {
+		fields[index++] = s;
+		while (*s && *s != ',')
+			s++;
+		if (*s == ',')
+			*s++ = 0;
+	}
+	return !*s && index == count;
+}
+
+/*
+ * interprete one sentence GGA - Fix information
+ */
+static int nmea_gga(char *s)
+{
+	char *f[14];
+
+	return nmea_split(s, f, (int)(sizeof f / sizeof *f))
+		&& *f[5] != '0'
+		&&  nmea_set(f[0], f[1], f[2], f[3], f[4], f[6], f[7], NULL, NULL, NULL);
+}
+
+/*
+ * interprete one sentence RMC - Recommended Minimum
+ */
+static int nmea_rmc(char *s)
+{
+	char *f[12];
+
+	return nmea_split(s, f, (int)(sizeof f / sizeof *f))
+		&& *f[1] == 'A'
+		&&  nmea_set(f[0], f[2], f[3], f[4], f[5], NULL, NULL, f[6], f[7], f[8]);
+}
+
+
+/*
+ * interprete one NMEA sentence
+ */
+static int nmea_sentence(char *s)
+{
+	if (!s[0] || !s[1])
+		return 0;
+
+	if (s[2] == 'G' && s[3] == 'G' && s[4] == 'A' && s[5] == ',')
+		return nmea_gga(&s[6]);
+
+	if (s[2] == 'R' && s[3] == 'M' && s[4] == 'C' && s[5] == ',')
+		return nmea_rmc(&s[6]);
+
+	return 0;
+}
+
+/*
+ * reads the NMEA stream
+ */
+static int nmea_read(int fd)
+{
+	static char buffer[160];
+	static int pos = 0;
+	static int overflow = 0;
+
+	int rc;
+
+	for(;;) {
+		rc = (int)read(fd, &buffer[pos], sizeof buffer - (size_t)pos);
+		if (rc < 0) {
+			/* its an error if not interrupted */
+			if (errno != EINTR)
+				return rc;
+		} else if (rc == 0) {
+			/* nothing more to be read */
+			return 0;
+		} else {
+			/* scan the buffer */
+			while (pos != rc) {
+				if (buffer[pos] != '\n') {
+					pos++;
+					if (pos == rc) {
+						if (pos == (int)(sizeof buffer)) {
+							overflow = 1;
+							pos = 0;
+							rc = 0;
+						}
+					}
+				} else {
+					if (buffer[0] == '$' && pos > 0 && buffer[pos-1] == '\r' && !overflow) {
+						if (pos > 3 && buffer[pos-4] == '*') {
+							/* TODO: check the cheksum */
+							buffer[pos-4] = 0;
+						} else {
+							buffer[pos-1] = 0;
+						}
+						nmea_sentence(&buffer[1]);
+					}
+					pos++;
+					rc -= pos;
+					if (rc > 0)
+						memmove(buffer, buffer+pos, (size_t)rc);
+					pos = 0;
+					overflow = 0;
+				}
+			}
+		}
+	}
+}
+
+/*
+ * called on an event on the NMEA stream
+ */
+static int nmea_on_event(sd_event_source *s, int fd, uint32_t revents, void *userdata)
+{
+	/* read available data */
+	if ((revents & EPOLLIN) != 0)
+		nmea_read(fd);
+
+	/* check if error or hangup */
+	if ((revents & (EPOLLERR|EPOLLRDHUP|EPOLLHUP)) != 0) {
+		sd_event_source_unref(s);
+		close(fd);
+		nmea_connect(fd);
+	}
+	return 0;
+}
+
+/*
+ * opens a socket to a host and a service (or port)
+ */
+static int open_socket_to(const char *host, const char *service)
+{
+	int rc, fd;
+	struct addrinfo hint, *rai, *iai;
+	char xhost[32];
+
+	/* get addr */
+	memset(&hint, 0, sizeof hint);
+	hint.ai_family = AF_INET;
+	hint.ai_socktype = SOCK_STREAM;
+	rc = getaddrinfo(host, service, &hint, &rai);
+	if (rc != 0)
+		return -1;
+
+	/* get the socket */
+	iai = rai;
+	while (iai != NULL) {
+		struct sockaddr_in *a = (struct sockaddr_in*)(iai->ai_addr);
+		unsigned char *ipv4 = (unsigned char*)&(a->sin_addr.s_addr);
+		unsigned char *port = (unsigned char*)&(a->sin_port);
+		sprintf(xhost, "%d.%d.%d.%d:%d",
+			(int)ipv4[0], (int)ipv4[1], (int)ipv4[2], (int)ipv4[3],
+			(((int)port[0]) << 8)|(int)port[1]);
+		fd = socket(iai->ai_family, iai->ai_socktype, iai->ai_protocol);
+		if (fd >= 0) {
+			rc = connect(fd, iai->ai_addr, iai->ai_addrlen);
+			if (rc == 0) {
+				fcntl(fd, F_SETFL, O_NONBLOCK);
+				freeaddrinfo(rai);
+				return fd;
+			}
+			close(fd);
+		}
+		iai = iai->ai_next;
+	}
+	freeaddrinfo(rai);
+	return -1;
+}
+
+/*
+ * connection to nmea stream
+ */
+static int nmea_connect()
+{
+	sd_event_source *source;
+	int rc, fd;
+	const char *host;
+	const char *service;
+
+	/* TODO connect to somewhere else */
+	host = "sinagot.net";
+	service = "5001";
+	fd = open_socket_to(host, service);
+	if (fd < 0) {
+		ERROR(afbitf, "can't connect to host %s, service %s", host, service);
+		return fd;
+	}
+
+	/* adds to the event loop */
+	rc = sd_event_add_io(afb_daemon_get_event_loop(afbitf->daemon), &source, fd, EPOLLIN, nmea_on_event, NULL);
+	if (rc < 0) {
+		close(fd);
+		ERROR(afbitf, "can't coonect host %s, service %s to the event loop", host, service);
+	}
+	return rc;
+}
+
+/*
+ * Get the last known position
+ */
+static void get(struct afb_req req)
+{
+	afb_req_success(req, position(), NULL);
+}
+
+/*
+ * subscribe to notification of position
+ */
+static void subscribe(struct afb_req req)
+{
+	afb_req_success(req, position(), NULL);
+}
+
+/*
+ * unsubscribe a previous subscription
+ */
+static void unsubscribe(struct afb_req req)
+{
+	afb_req_success(req, position(), NULL);
+}
+
+/*
+ * array of the verbs exported to afb-daemon
+ */
+static const struct afb_verb_desc_v1 binding_verbs[] = {
+  /* VERB'S NAME            SESSION MANAGEMENT          FUNCTION TO CALL         SHORT DESCRIPTION */
+  { .name= "get",          .session= AFB_SESSION_NONE, .callback= get,          .info= "get the last known data" },
+  { .name= "subscribe",    .session= AFB_SESSION_NONE, .callback= subscribe,    .info= "subscribe to notification of position" },
+  { .name= "unsubscribe",  .session= AFB_SESSION_NONE, .callback= unsubscribe,  .info= "unsubscribe a previous subscription" },
+  { .name= NULL } /* marker for end of the array */
+};
+
+/*
+ * description of the binding for afb-daemon
+ */
+static const struct afb_binding binding_description =
+{
+  /* description conforms to VERSION 1 */
+  .type= AFB_BINDING_VERSION_1,
+  .v1= {			/* fills the v1 field of the union when AFB_BINDING_VERSION_1 */
+    .prefix= "gps",		/* the API name (or binding name or prefix) */
+    .info= "Access to the GPS data",	/* short description of of the binding */
+    .verbs = binding_verbs	/* the array describing the verbs of the API */
+  }
+};
+
+/*
+ * activation function for registering the binding called by afb-daemon
+ */
+const struct afb_binding *afbBindingV1Register(const struct afb_binding_interface *itf)
+{
+	afbitf = itf;			/* records the interface for accessing afb-daemon */
+
+	nmea_connect();
+
+	return &binding_description;	/* returns the description of the binding */
+}
+
+