1 files changed, 70 insertions, 127 deletions

diff --git a/bsp/meta-qcom/dynamic-layers/openembedded-layer/recipes-navigation/gpsd/gpsd/0001-Introduce-Qualcomm-PDS-service-support.patch b/bsp/meta-qcom/dynamic-layers/openembedded-layer/recipes-navigation/gpsd/gpsd/0001-Introduce-Qualcomm-PDS-service-support.patch
index aae427e5..15c5c469 100644
--- a/bsp/meta-qcom/dynamic-layers/openembedded-layer/recipes-navigation/gpsd/gpsd/0001-Introduce-Qualcomm-PDS-service-support.patch
+++ b/bsp/meta-qcom/dynamic-layers/openembedded-layer/recipes-navigation/gpsd/gpsd/0001-Introduce-Qualcomm-PDS-service-support.patch
@@ -1,7 +1,10 @@
-From 4854806fdbd53b4b25d18f966f273c8f32d57c35 Mon Sep 17 00:00:00 2001
+From fd6199f456c7c5dcb4cb782e4af63111d22079bf Mon Sep 17 00:00:00 2001
 From: Bjorn Andersson <bjorn.andersson@linaro.org>
 Date: Wed, 4 Apr 2018 04:29:09 +0000
 Subject: [PATCH] Introduce Qualcomm PDS service support
+MIME-Version: 1.0
+Content-Type: text/plain; charset=UTF-8
+Content-Transfer-Encoding: 8bit
 
 The Qualcomm PDS service provides location data on a wide range of
 Qualcomm platforms. It used QMI encoded messages sent over a shared
@@ -28,32 +31,33 @@ a numerical identifier of the node in the AF_QIPCRTR network or the
 string "any".
 
 Signed-off-by: Bjorn Andersson <bjorn.andersson@linaro.org>
+Signed-off-by: Aníbal Limón <anibal.limon@linaro.org>
 ---
  SConstruct     |  10 ++
- driver_pds.c   | 361 +++++++++++++++++++++++++++++++++++++++++++++++++
+ driver_pds.c   | 325 +++++++++++++++++++++++++++++++++++++++++++++++++
  driver_pds.h   |  20 +++
  drivers.c      |   5 +
- gpsd.h         |  20 ++-
- libgpsd_core.c |  15 +-
- 6 files changed, 424 insertions(+), 7 deletions(-)
+ gpsd.h         |   2 +
+ libgpsd_core.c |  15 ++-
+ 6 files changed, 376 insertions(+), 1 deletion(-)
  create mode 100644 driver_pds.c
  create mode 100644 driver_pds.h
 
 diff --git a/SConstruct b/SConstruct
-index d9d75653..6f53fef3 100644
+index 5160481a7..31545b07f 100644
 --- a/SConstruct
 +++ b/SConstruct
-@@ -155,6 +155,7 @@ boolopts = (
+@@ -191,6 +191,7 @@ boolopts = (
+     ("tripmate",      True,  "DeLorme TripMate support"),
+     ("tsip",          True,  "Trimble TSIP support"),
      ("ublox",         True,  "u-blox Protocol support"),
-     ("fury",          True,  "Jackson Labs Fury and Firefly support"),
-     ("nmea2000",      True,  "NMEA2000/CAN support"),
 +    ("pds",           True,  "Qualcomm PDS support"),
      # Non-GPS protocols
      ("aivdm",         True,  "AIVDM support"),
      ("gpsclock",      True,  "GPSClock support"),
-@@ -778,6 +779,14 @@ else:
+@@ -923,6 +924,14 @@ else:
          announce("You do not have kernel CANbus available.")
-         env["nmea2000"] = False
+         config.env["nmea2000"] = False
  
 +    if config.CheckHeader(["bits/sockaddr.h", "linux/qrtr.h"]):
 +        confdefs.append("#define HAVE_LINUX_QRTR_H 1\n")
@@ -65,21 +69,21 @@ index d9d75653..6f53fef3 100644
 +
      # check for C11 or better, and __STDC__NO_ATOMICS__ is not defined
      # before looking for stdatomic.h
-     if ((config.CheckC11()
-@@ -1121,6 +1130,7 @@ libgpsd_sources = [
+     if ((config.CheckC11() and
+@@ -1295,6 +1304,7 @@ libgpsd_sources = [
      "driver_nmea0183.c",
      "driver_nmea2000.c",
      "driver_oncore.c",
 +    "driver_pds.c",
      "driver_rtcm2.c",
      "driver_rtcm3.c",
-     "driver_sirf.c",
+     "drivers.c",
 diff --git a/driver_pds.c b/driver_pds.c
 new file mode 100644
-index 00000000..62464323
+index 000000000..7fe9e5339
 --- /dev/null
 +++ b/driver_pds.c
-@@ -0,0 +1,361 @@
+@@ -0,0 +1,325 @@
 +#include <sys/socket.h>
 +#include <errno.h>
 +#include <fcntl.h>
@@ -94,43 +98,7 @@ index 00000000..62464323
 +#include <linux/qrtr.h>
 +
 +#define QMI_PDS_SERVICE_ID	0x10
-+#define QMI_PDS_INSTANCE_ID	0x2
-+
-+#ifndef QRTR_PORT_CTRL
-+#define QRTR_PORT_CTRL	0xfffffffeu
-+
-+enum qrtr_pkt_type {
-+	QRTR_TYPE_DATA		= 1,
-+	QRTR_TYPE_HELLO		= 2,
-+	QRTR_TYPE_BYE		= 3,
-+	QRTR_TYPE_NEW_SERVER	= 4,
-+	QRTR_TYPE_DEL_SERVER	= 5,
-+	QRTR_TYPE_DEL_CLIENT	= 6,
-+	QRTR_TYPE_RESUME_TX	= 7,
-+	QRTR_TYPE_EXIT          = 8,
-+	QRTR_TYPE_PING          = 9,
-+	QRTR_TYPE_NEW_LOOKUP	= 10,
-+	QRTR_TYPE_DEL_LOOKUP	= 11,
-+};
-+
-+struct qrtr_ctrl_pkt {
-+	__le32 cmd;
-+
-+	union {
-+		struct {
-+			__le32 service;
-+			__le32 instance;
-+			__le32 node;
-+			__le32 port;
-+		} server;
-+
-+		struct {
-+			__le32 node;
-+			__le32 port;
-+		} client;
-+	};
-+} __packed;
-+#endif /* of ifndef QRTR_PORT_CTRL */
++#define QMI_PDS_VERSION		0x2
 +
 +struct qmi_header {
 +	uint8_t type;
@@ -181,13 +149,9 @@ index 00000000..62464323
 +		return -1;
 +	}
 +
-+	session->lexer.type = QMI_PDS_PACKET;
++	/* TODO: Validate sq to be our peer */
 +
-+	if (sq.sq_node != session->driver.pds.node ||
-+	    sq.sq_port != session->driver.pds.port) {
-+		session->lexer.outbuflen = 0;
-+		return ret;
-+	}
++	session->lexer.type = QMI_PDS_PACKET;
 +
 +	hdr = buf;
 +	if (hdr->type != QMI_INDICATION ||
@@ -223,36 +187,31 @@ index 00000000..62464323
 +
 +static void qmi_pds_event_hook(struct gps_device_t *session, event_t event)
 +{
-+	struct sockaddr_qrtr sq;
 +	struct qmi_header *hdr;
 +	struct qmi_tlv *tlv;
 +	static int txn_id;
 +	char buf[128];
++	char *ptr;
 +	int sock = session->gpsdata.gps_fd;
 +	int ret;
 +
-+	session->driver.pds.node = 0;
-+	session->driver.pds.port = 14;
-+
-+	sq.sq_family = AF_QIPCRTR;
-+	sq.sq_node = session->driver.pds.node;
-+	sq.sq_port = session->driver.pds.port;
-+
 +	switch (event) {
 +	case event_deactivate:
-+		hdr = (struct qmi_header *)buf;
++		ptr = buf;
++		hdr = (struct qmi_header *)ptr;
 +		hdr->type = QMI_REQUEST;
 +		hdr->txn = txn_id++;
 +		hdr->msg = QMI_LOC_STOP;
 +		hdr->len = sizeof(*tlv) + sizeof(uint8_t);
++		ptr += sizeof(*hdr);
 +
-+		tlv = (struct qmi_tlv *)(buf + sizeof(*hdr));
++		tlv = (struct qmi_tlv *)ptr;
 +		tlv->key = QMI_TLV_SESSION_ID;
 +		tlv->len = sizeof(uint8_t);
 +		*(uint8_t*)tlv->value = 1;
++		ptr += sizeof(*tlv) + sizeof(uint8_t);
 +
-+		ret = sendto(sock, buf, sizeof(*hdr) + hdr->len, 0,
-+			     (struct sockaddr *)&sq, sizeof(sq));
++		ret = send(sock, buf, ptr - buf, 0);
 +		if (ret < 0) {
 +			gpsd_log(&session->context->errout, LOG_ERROR,
 +				 "QRTR event_hook: failed to send STOP request.\n");
@@ -260,38 +219,42 @@ index 00000000..62464323
 +		}
 +		break;
 +	case event_reactivate:
-+		hdr = (struct qmi_header *)buf;
++		ptr = buf;
++		hdr = (struct qmi_header *)ptr;
 +		hdr->type = QMI_REQUEST;
 +		hdr->txn = txn_id++;
 +		hdr->msg = QMI_LOC_REG_EVENTS;
 +		hdr->len = sizeof(*tlv) + sizeof(uint64_t);
++		ptr += sizeof(*hdr);
 +
-+		tlv = (struct qmi_tlv *)(buf + sizeof(*hdr));
++		tlv = (struct qmi_tlv *)ptr;
 +		tlv->key = QMI_TLV_EVENT_MASK;
 +		tlv->len = sizeof(uint64_t);
 +		*(uint64_t*)tlv->value = QMI_EVENT_MASK_NMEA;
++		ptr += sizeof(*tlv) + sizeof(uint64_t);
 +
-+		ret = sendto(sock, buf, sizeof(*hdr) + hdr->len, 0,
-+			     (struct sockaddr *)&sq, sizeof(sq));
++		ret = send(sock, buf, ptr - buf, 0);
 +		if (ret < 0) {
 +			gpsd_log(&session->context->errout, LOG_ERROR,
 +				 "QRTR event_hook: failed to send REG_EVENTS request.\n");
 +			return;
 +		}
 +
-+		hdr = (struct qmi_header *)buf;
++		ptr = buf;
++		hdr = (struct qmi_header *)ptr;
 +		hdr->type = QMI_REQUEST;
 +		hdr->txn = txn_id++;
 +		hdr->msg = QMI_LOC_START;
 +		hdr->len = sizeof(*tlv) + sizeof(uint8_t);
++		ptr += sizeof(*hdr);
 +
 +		tlv = (struct qmi_tlv *)(buf + sizeof(*hdr));
 +		tlv->key = QMI_TLV_SESSION_ID;
 +		tlv->len = sizeof(uint8_t);
 +		*(uint8_t*)tlv->value = 1;
++		ptr += sizeof(*tlv) + sizeof(uint8_t);
 +
-+		ret = sendto(sock, buf, sizeof(*hdr) + hdr->len, 0,
-+			     (struct sockaddr *)&sq, sizeof(sq));
++		ret = send(sock, buf, ptr - buf, 0);
 +		if (ret < 0) {
 +			gpsd_log(&session->context->errout, LOG_ERROR,
 +				 "QRTR event_hook: failed to send START request.\n");
@@ -349,7 +312,7 @@ index 00000000..62464323
 +	memset(&pkt, 0, sizeof(pkt));
 +	pkt.cmd = QRTR_TYPE_NEW_LOOKUP;
 +	pkt.server.service = QMI_PDS_SERVICE_ID;
-+	pkt.server.instance = QMI_PDS_INSTANCE_ID;
++	pkt.server.instance = QMI_PDS_VERSION;
 +
 +	sq_ctrl.sq_port = QRTR_PORT_CTRL;
 +	ret = sendto(sock, &pkt, sizeof(pkt), 0, (struct sockaddr *)&sq_ctrl, sizeof(sq_ctrl));
@@ -387,7 +350,7 @@ index 00000000..62464323
 +			break;
 +
 +		/* Filter results based on specified node */
-+		if (hostid != -1 && hostid != pkt.server.node)
++		if (hostid != -1 && hostid != (int)pkt.server.node)
 +			continue;
 +
 +		pds_node = pkt.server.node;
@@ -408,14 +371,22 @@ index 00000000..62464323
 +	gpsd_log(&session->context->errout, LOG_INF,
 +		 "QRTR open: Found PDS at %d %d.\n", pds_node, pds_port);
 +
++	sq.sq_family = AF_QIPCRTR;
++	sq.sq_node = pds_node;
++	sq.sq_port = pds_port;
++	ret = connect(sock, (struct sockaddr *)&sq, sizeof(sq));
++	if (ret < 0) {
++		gpsd_log(&session->context->errout, LOG_ERROR,
++			 "QRTR open: Failed to connect socket.\n");
++		close(sock);
++		return -1;
++	}
++
 +	gpsd_switch_driver(session, "Qualcomm PDS");
 +	session->gpsdata.gps_fd = sock;
 +	session->sourcetype = source_qrtr;
 +	session->servicetype = service_sensor;
 +
-+	session->driver.pds.node = pds_node;
-+	session->driver.pds.port = pds_port;
-+
 +	return session->gpsdata.gps_fd;
 +}
 +
@@ -425,9 +396,6 @@ index 00000000..62464323
 +		close(session->gpsdata.gps_fd);
 +		INVALIDATE_SOCKET(session->gpsdata.gps_fd);
 +	}
-+
-+	session->driver.pds.node = 0;
-+	session->driver.pds.port = 0;
 +}
 +
 +const struct gps_type_t driver_pds = {
@@ -443,7 +411,7 @@ index 00000000..62464323
 +#endif /* of defined(PDS_ENABLE) */
 diff --git a/driver_pds.h b/driver_pds.h
 new file mode 100644
-index 00000000..3b373743
+index 000000000..3b373743d
 --- /dev/null
 +++ b/driver_pds.h
 @@ -0,0 +1,20 @@
@@ -468,10 +436,10 @@ index 00000000..3b373743
 +#endif /* of defined(PDS_ENABLE) */
 +#endif /* of ifndef _DRIVER_PDS_H_ */
 diff --git a/drivers.c b/drivers.c
-index eda1fd61..92d7eba8 100644
+index f49ba0a93..a2be4d9ad 100644
 --- a/drivers.c
 +++ b/drivers.c
-@@ -1744,6 +1744,7 @@ extern const struct gps_type_t driver_geostar;
+@@ -1766,6 +1766,7 @@ extern const struct gps_type_t driver_greis;
  extern const struct gps_type_t driver_italk;
  extern const struct gps_type_t driver_navcom;
  extern const struct gps_type_t driver_nmea2000;
@@ -479,7 +447,7 @@ index eda1fd61..92d7eba8 100644
  extern const struct gps_type_t driver_oncore;
  extern const struct gps_type_t driver_sirf;
  extern const struct gps_type_t driver_skytraq;
-@@ -1838,6 +1839,10 @@ static const struct gps_type_t *gpsd_driver_array[] = {
+@@ -1863,6 +1864,10 @@ static const struct gps_type_t *gpsd_driver_array[] = {
      &driver_nmea2000,
  #endif /* NMEA2000_ENABLE */
  
@@ -491,30 +459,18 @@ index eda1fd61..92d7eba8 100644
      &driver_rtcm104v2,
  #endif /* RTCM104V2_ENABLE */
 diff --git a/gpsd.h b/gpsd.h
-index 2bd5f4c0..b24c6e65 100644
+index e78afd4c3..fdbbd8f4c 100644
 --- a/gpsd.h
 +++ b/gpsd.h
-@@ -163,12 +163,13 @@ struct gps_lexer_t {
- #define ONCORE_PACKET   	13
+@@ -191,6 +191,7 @@ struct gps_lexer_t {
  #define GEOSTAR_PACKET   	14
  #define NMEA2000_PACKET 	15
--#define MAX_GPSPACKET_TYPE	15	/* increment this as necessary */
--#define RTCM2_PACKET    	16
--#define RTCM3_PACKET    	17
--#define JSON_PACKET    	    	18
--#define PACKET_TYPES		19	/* increment this as necessary */
--#define SKY_PACKET     		20
+ #define GREIS_PACKET		16
 +#define QMI_PDS_PACKET		16
-+#define MAX_GPSPACKET_TYPE	16	/* increment this as necessary */
-+#define RTCM2_PACKET    	17
-+#define RTCM3_PACKET    	18
-+#define JSON_PACKET    	    	19
-+#define PACKET_TYPES		20	/* increment this as necessary */
-+#define SKY_PACKET     		21
- #define TEXTUAL_PACKET_TYPE(n)	((((n)>=NMEA_PACKET) && ((n)<=MAX_TEXTUAL_TYPE)) || (n)==JSON_PACKET)
- #define GPS_PACKET_TYPE(n)	(((n)>=NMEA_PACKET) && ((n)<=MAX_GPSPACKET_TYPE))
- #define LOSSLESS_PACKET_TYPE(n)	(((n)>=RTCM2_PACKET) && ((n)<=RTCM3_PACKET))
-@@ -411,6 +412,7 @@ typedef enum {source_unknown,
+ #define MAX_GPSPACKET_TYPE	16	/* increment this as necessary */
+ #define RTCM2_PACKET    	17
+ #define RTCM3_PACKET    	18
+@@ -439,6 +440,7 @@ typedef enum {source_unknown,
  	      source_usb,	/* potential GPS source, discoverable */
  	      source_bluetooth,	/* potential GPS source, discoverable */
  	      source_can,	/* potential GPS source, fixed CAN format */
@@ -522,24 +478,11 @@ index 2bd5f4c0..b24c6e65 100644
  	      source_pty,	/* PTY: we don't require exclusive access */
  	      source_tcp,	/* TCP/IP stream: case detected but not used */
  	      source_udp,	/* UDP stream: case detected but not used */
-@@ -699,6 +701,12 @@ struct gps_device_t {
- 	    unsigned char sid[8];
- 	} nmea2000;
- #endif /* NMEA2000_ENABLE */
-+#ifdef PDS_ENABLE
-+	struct {
-+		unsigned int node;
-+		unsigned int port;
-+	} pds;
-+#endif /* PDS_ENABLE */
- 	/*
- 	 * This is not conditionalized on RTCM104_ENABLE because we need to
- 	 * be able to build gpsdecode even when RTCM support is not
 diff --git a/libgpsd_core.c b/libgpsd_core.c
-index 85b8d86a..4f6a11ed 100644
+index 3bf420c3e..848da6aeb 100644
 --- a/libgpsd_core.c
 +++ b/libgpsd_core.c
-@@ -48,6 +48,9 @@
+@@ -39,6 +39,9 @@
  #if defined(NMEA2000_ENABLE)
  #include "driver_nmea2000.h"
  #endif /* defined(NMEA2000_ENABLE) */
@@ -561,7 +504,7 @@ index 85b8d86a..4f6a11ed 100644
          (void)gpsd_close(session);
      if (session->mode == O_OPTIMIZE)
  	gpsd_run_device_hook(&session->context->errout,
-@@ -547,6 +555,11 @@ int gpsd_open(struct gps_device_t *session)
+@@ -549,6 +557,11 @@ int gpsd_open(struct gps_device_t *session)
          return nmea2000_open(session);
      }
  #endif /* defined(NMEA2000_ENABLE) */
@@ -573,7 +516,7 @@ index 85b8d86a..4f6a11ed 100644
      /* fall through to plain serial open */
      /* could be a naked /dev/ppsX */
      return gpsd_serial_open(session);
-@@ -575,7 +588,7 @@ int gpsd_activate(struct gps_device_t *session, const int mode)
+@@ -577,7 +590,7 @@ int gpsd_activate(struct gps_device_t *session, const int mode)
  #ifdef NON_NMEA0183_ENABLE
      /* if it's a sensor, it must be probed */
      if ((session->servicetype == service_sensor) &&
@@ -583,5 +526,5 @@ index 85b8d86a..4f6a11ed 100644
  
  	for (dp = gpsd_drivers; *dp; dp++) {
 -- 
-2.17.0
+2.23.0