This is an automated email from the ASF dual-hosted git repository.
jerpelea pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/nuttx.git
commit f49fa466b6b78dd8e8daa89b144041871a0b169a
Author: SPRESENSE <41312067+sprese...@users.noreply.github.com>
AuthorDate: Sat Oct 28 19:20:40 2023 +0900
boards: cxd56xx: Add cxd5610 gnss driver
Add cxd5610 gnss driver as board-specific sensor driver.
---
boards/arm/cxd56xx/drivers/sensors/Kconfig | 51 +
boards/arm/cxd56xx/drivers/sensors/Make.defs | 4 +
boards/arm/cxd56xx/drivers/sensors/cxd5610_gnss.c | 2266 +++++++++++++++++++++
include/nuttx/sensors/cxd5610_gnss.h | 98 +
4 files changed, 2419 insertions(+)
diff --git a/boards/arm/cxd56xx/drivers/sensors/Kconfig
b/boards/arm/cxd56xx/drivers/sensors/Kconfig
index 17909755cf..1523a62a8d 100644
--- a/boards/arm/cxd56xx/drivers/sensors/Kconfig
+++ b/boards/arm/cxd56xx/drivers/sensors/Kconfig
@@ -203,3 +203,54 @@ config RPR0521RS_PROXIMITY_INTERRUPT
endif # SENSORS_RPR0521RS_SCU
endif # SCU_SENSORS
+
+config SENSORS_CXD5610_GNSS
+ bool "Sony CXD5610 GNSS"
+ default n
+ ---help---
+ Enable driver for CXD5610 GNSS device.
+
+if SENSORS_CXD5610_GNSS
+
+config SENSORS_CXD5610_GNSS_SNDBUF_SIZE
+ int "CXD5610 GNSS send buffer size"
+ default 64
+
+config SENSORS_CXD5610_GNSS_RCVBUF_SIZE
+ int "CXD5610 GNSS receive buffer size"
+ default 64
+
+config SENSORS_CXD5610_GNSS_NOTIFYBUF_SIZE
+ int "CXD5610 GNSS notify buffer size"
+ default 1536
+
+config SENSORS_CXD5610_GNSS_NPOLLWAITERS
+ int "CXD5610 GNSS max poll waiters"
+ default 4
+
+config SENSORS_CXD5610_GNSS_NSIGNALRECEIVERS
+ int "CXD5610 GNSS max signal receivers"
+ default 4
+
+config SENSORS_CXD5610_GNSS_RX_THREAD_PRIORITY
+ int "CXD5610 GNSS receive thread priority"
+ default 120
+
+config SENSORS_CXD5610_GNSS_RX_THREAD_STACKSIZE
+ int "CXD5610 GNSS receive thread stack size"
+ default 1024
+
+config SENSORS_CXD5610_GNSS_UNALIGNED_ACCESS
+ bool "Unaligned access"
+ default y
+ depends on !ENDIAN_BIG
+ ---help---
+ Support unaligned word and half-word access on little endian.
+
+config SENSORS_CXD5610_GNSS_READ_COMPAT
+ bool "Compatible with CXD5602 GNSS"
+ default y
+ ---help---
+ Support compatible with CXD5602 GNSS
+
+endif # SENSORS_CXD5610_GNSS
diff --git a/boards/arm/cxd56xx/drivers/sensors/Make.defs
b/boards/arm/cxd56xx/drivers/sensors/Make.defs
index cad7d3cf97..c39dd385ea 100644
--- a/boards/arm/cxd56xx/drivers/sensors/Make.defs
+++ b/boards/arm/cxd56xx/drivers/sensors/Make.defs
@@ -62,6 +62,10 @@ ifeq ($(CONFIG_SENSORS_RPR0521RS_SCU),y)
CSRCS += rpr0521rs_scu.c
endif
+ifeq ($(CONFIG_SENSORS_CXD5610_GNSS),y)
+ CSRCS += cxd5610_gnss.c
+endif
+
DEPPATH += --dep-path platform$(DELIM)sensors
VPATH += :platform$(DELIM)sensors
CFLAGS +=
${INCDIR_PREFIX}$(TOPDIR)$(DELIM)drivers$(DELIM)platform$(DELIM)sensors
diff --git a/boards/arm/cxd56xx/drivers/sensors/cxd5610_gnss.c
b/boards/arm/cxd56xx/drivers/sensors/cxd5610_gnss.c
new file mode 100644
index 0000000000..9f3d901c6c
--- /dev/null
+++ b/boards/arm/cxd56xx/drivers/sensors/cxd5610_gnss.c
@@ -0,0 +1,2266 @@
+/****************************************************************************
+ * boards/arm/cxd56xx/drivers/sensors/cxd5610_gnss.c
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership. The
+ * ASF licenses this file to you 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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <assert.h>
+#include <debug.h>
+#include <poll.h>
+#include <spawn.h>
+#include <nuttx/kmalloc.h>
+#include <nuttx/signal.h>
+#include <nuttx/mutex.h>
+#include <nuttx/sensors/cxd5610_gnss.h>
+#include <arch/chip/gnss.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#ifndef MIN
+# define MIN(a,b) (((a) < (b)) ? (a) : (b))
+#endif /* MIN */
+
+#ifndef MAX
+# define MAX(a,b) (((a) > (b)) ? (a) : (b))
+#endif /* MAX */
+
+/* Configurations */
+
+#ifndef CONFIG_SENSORS_CXD5610_GNSS_NPOLLWAITERS
+# define CONFIG_SENSORS_CXD5610_GNSS_NPOLLWAITERS 4
+#endif
+
+#ifndef CONFIG_SENSORS_CXD5610_GNSS_NSIGNALRECEIVERS
+# define CONFIG_SENSORS_CXD5610_GNSS_NSIGNALRECEIVERS 4
+#endif
+
+/* Communication buffer size */
+
+#ifndef CONFIG_SENSORS_CXD5610_GNSS_SNDBUF_SIZE
+# define CONFIG_SENSORS_CXD5610_GNSS_SNDBUF_SIZE 64
+#endif
+
+#ifndef CONFIG_SENSORS_CXD5610_GNSS_RCVBUF_SIZE
+# define CONFIG_SENSORS_CXD5610_GNSS_RCVBUF_SIZE 64
+#endif
+
+#ifndef CONFIG_SENSORS_CXD5610_GNSS_NOTIFYBUF_SIZE
+# define CONFIG_SENSORS_CXD5610_GNSS_NOTIFYBUF_SIZE 1536
+#endif
+
+/* Receive thread */
+
+#ifndef CONFIG_SENSORS_CXD5610_GNSS_RX_THREAD_PRIORITY
+# define CONFIG_SENSORS_CXD5610_GNSS_RX_THREAD_PRIORITY 120
+#endif
+
+#ifndef CONFIG_SENSORS_CXD5610_GNSS_RX_THREAD_STACKSIZE
+# define CONFIG_SENSORS_CXD5610_GNSS_RX_THREAD_STACKSIZE 1024
+#endif
+
+/* Read type */
+
+#define CXD56_READ_DATA_TYPE_GNSS 0
+#define CXD56_READ_DATA_TYPE_DCREPORT 15
+
+/* OPC definitions */
+
+#define OPC_SYS_STATE_CHANGE_INSTRUCTION (0x00)
+#define OPC_FWVER_REQ (0x06)
+#define OPC_BACKUP_MANUAL (0x10)
+#define OPC_PPS_OUTPUT (0x15)
+#define OPC_GNSS_START (0x30)
+#define OPC_GNSS_STOP (0x31)
+#define OPC_BINARY_OUTPUT_SET (0x34)
+#define OPC_RECEIVER_POS_SET (0x35)
+#define OPC_UTC_TIME_SET (0x36)
+#define OPC_OP_MODE_SET (0x3d)
+#define OPC_OP_MODE_GET (0x3e)
+#define OPC_TIME_NOTIFY (0x80)
+#define OPC_RECEIVER_POS_NOTIFY (0x81)
+#define OPC_RECEIVER_VEL_NOTIFY (0x82)
+#define OPC_SAT_INFO_NOTIFY (0x83)
+#define OPC_ACCURACY_IDX_NOTIFY (0x89)
+#define OPC_DISASTER_CRISIS_NOTIFY (0x8b)
+
+/* Command packet definitions */
+
+#define PACKET_SYNC (0x7f)
+#define PACKET_MAXLEN (512)
+#define PACKET_MASK (PACKET_MAXLEN - 1)
+#define PACKET_NR(n) (((n) + PACKET_MASK) / PACKET_MAXLEN)
+#define PACKET_HEADERLEN (5)
+#define PACKET_CHECKSUMLEN (1)
+#define PACKET_LEN(n) ((n) + PACKET_HEADERLEN + PACKET_CHECKSUMLEN)
+
+#define IS_NOTIFY_INVALID(v) (((v) & 0x80) == 0)
+#define GET_PACKET_VERSION(v) ((v) & 0x7f)
+
+/* System state definitions */
+
+#define STATE_RESET 0x01
+#define STATE_WAKEUP 0x02
+#define STATE_DEEPSLEEP 0x03
+#define STATE_SLEEP 0x04
+
+/* Command wait timeout in seconds */
+
+#define COMMAND_WAIT_TIMEOUT 5
+
+/* Boot wait timeout in seconds */
+
+#define BOOT_WAIT_TIMEOUT 2
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/* Structure for POSIX signal */
+
+struct gnss_sig_s
+{
+ uint8_t enable;
+ pid_t pid;
+ struct cxd56_gnss_signal_info_s info;
+};
+
+/* Structure for cxd5610 driver */
+
+struct cxd5610_gnss_dev_s
+{
+ struct cxd5610_gnss_lowerhalf_s *lower;
+
+ pid_t pid;
+ uint8_t cref;
+ mutex_t dev_lock;
+ mutex_t buf_lock;
+ sem_t cmd_sync;
+ sem_t boot_sync;
+#if CONFIG_SENSORS_CXD5610_GNSS_NPOLLWAITERS != 0
+ struct pollfd *fds[CONFIG_SENSORS_CXD5610_GNSS_NPOLLWAITERS];
+ bool has_event;
+#endif
+#if CONFIG_SENSORS_CXD5610_GNSS_NSIGNALRECEIVERS != 0
+ struct gnss_sig_s sigs[CONFIG_SENSORS_CXD5610_GNSS_NSIGNALRECEIVERS];
+#endif
+ bool wait_reset;
+ bool wait_wakeup;
+ bool sleeping;
+ uint8_t *sndbuf;
+ uint8_t *rcvbuf;
+ uint8_t *notifybuf;
+ struct cxd56_gnss_positiondata2_s *posdat2;
+ struct cxd56_gnss_dcreport_data_s *dcrdat;
+};
+
+/* Command packets */
+
+begin_packed_struct struct cmd_op_mode_s
+{
+ uint8_t mode8;
+ uint32_t cycle32;
+ uint32_t sleep32;
+} end_packed_struct;
+
+begin_packed_struct struct cmd_ellipsoidal_position_s
+{
+ int32_t lat32;
+ int32_t lon32;
+ int32_t alt32;
+} end_packed_struct;
+
+begin_packed_struct struct cmd_datetime_s
+{
+ uint8_t type;
+ uint8_t yearl;
+ uint8_t yearu_mon;
+ uint8_t day;
+ uint8_t hour;
+ uint8_t min;
+ uint8_t sec;
+ uint8_t sec100;
+} end_packed_struct;
+
+begin_packed_struct struct cmd_notify_time_s
+{
+ uint8_t ver8;
+ uint8_t type;
+ uint8_t yearl;
+ uint8_t yearu_mon;
+ uint8_t day;
+ uint8_t hour;
+ uint8_t min;
+ uint8_t sec;
+ uint8_t sec100;
+} end_packed_struct;
+
+begin_packed_struct struct cmd_notify_pos_s
+{
+ uint8_t ver8;
+ uint8_t mode;
+ int32_t lat32;
+ int32_t lon32;
+ int32_t alt32;
+} end_packed_struct;
+
+begin_packed_struct struct cmd_notify_vel_s
+{
+ uint8_t ver8;
+ uint8_t mode;
+ uint16_t course16;
+ uint16_t mag_course16;
+ int16_t vel16;
+ int16_t up_vel16;
+} end_packed_struct;
+
+begin_packed_struct struct cmd_notify_sat_s
+{
+ uint8_t ver8;
+ uint8_t mode;
+ uint16_t numsv;
+} end_packed_struct;
+
+begin_packed_struct struct cmd_notify_satinfo_s
+{
+ uint8_t signal;
+ uint8_t svid;
+ uint8_t cn;
+ uint8_t elevation;
+ uint16_t azimuth;
+} end_packed_struct;
+
+begin_packed_struct struct cmd_notify_acc_s
+{
+ uint8_t ver8;
+ uint16_t h_uc16;
+ uint16_t v_uc16;
+ uint16_t h_speed_uc16;
+ uint16_t v_speed_uc16;
+ uint8_t pdop8;
+ uint8_t hdop8;
+ uint8_t vdop8;
+ uint16_t semimajor16;
+ uint16_t semiminor16;
+ uint8_t orientation;
+} end_packed_struct;
+
+begin_packed_struct struct cmd_notify_dcreport_s
+{
+ uint8_t ver8;
+ uint8_t nr;
+ struct mt43_data_s
+ {
+ uint8_t svid;
+ uint8_t data[32];
+ }
+ msg[3];
+} end_packed_struct;
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/* Character driver methods */
+
+static int cxd5610_gnss_open(struct file *filep);
+static int cxd5610_gnss_close(struct file *filep);
+static ssize_t cxd5610_gnss_read(struct file *filep,
+ char *buffer,
+ size_t buflen);
+static ssize_t cxd5610_gnss_write(struct file *filep,
+ const char *buffer,
+ size_t buflen);
+static int cxd5610_gnss_ioctl(struct file *filep,
+ int cmd,
+ unsigned long arg);
+static int cxd5610_gnss_poll(struct file *filep, struct pollfd *fds,
+ bool setup);
+
+/* Semaphore controls */
+
+static int cxd5610_gnss_device_init(struct cxd5610_gnss_dev_s *priv);
+static int cxd5610_gnss_device_lock(struct cxd5610_gnss_dev_s *priv);
+static int cxd5610_gnss_device_unlock(struct cxd5610_gnss_dev_s *priv);
+static int cxd5610_gnss_buffer_init(struct cxd5610_gnss_dev_s *priv);
+static int cxd5610_gnss_buffer_lock(struct cxd5610_gnss_dev_s *priv);
+static int cxd5610_gnss_buffer_unlock(struct cxd5610_gnss_dev_s *priv);
+static int cxd5610_gnss_init_boot(struct cxd5610_gnss_dev_s *priv);
+static int cxd5610_gnss_wait_boot(struct cxd5610_gnss_dev_s *priv, int sec);
+static int cxd5610_gnss_post_boot(struct cxd5610_gnss_dev_s *priv);
+static int cxd5610_gnss_init_command(struct cxd5610_gnss_dev_s *priv);
+static int cxd5610_gnss_wait_command(struct cxd5610_gnss_dev_s *priv,
+ int sec);
+static int cxd5610_gnss_post_command(struct cxd5610_gnss_dev_s *priv);
+static int cxd5610_gnss_init_interrupt(void);
+static int cxd5610_gnss_wait_interrupt(void);
+static int cxd5610_gnss_post_interrupt(void);
+
+#if CONFIG_SENSORS_CXD5610_GNSS_NSIGNALRECEIVERS != 0
+static void cxd5610_gnss_signalhandler(struct cxd5610_gnss_dev_s *priv,
+ uint8_t sigtype);
+#endif
+
+#if CONFIG_SENSORS_CXD5610_GNSS_NPOLLWAITERS != 0
+static void cxd5610_gnss_pollnotify(struct cxd5610_gnss_dev_s *dev);
+#endif
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+static const struct file_operations g_cxd5610fops =
+{
+ cxd5610_gnss_open, /* open */
+ cxd5610_gnss_close, /* close */
+ cxd5610_gnss_read, /* read */
+ cxd5610_gnss_write, /* write */
+ NULL, /* seek */
+ cxd5610_gnss_ioctl, /* ioctl */
+ NULL, /* mmap */
+ NULL, /* truncate */
+ cxd5610_gnss_poll /* poll */
+};
+
+/* Semaphore for interrupt */
+
+static sem_t g_int_sync;
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: cxd5610_checksum
+ ****************************************************************************/
+
+static uint8_t cxd5610_checksum(const uint8_t *data, uint16_t datalen)
+{
+ int i;
+ uint8_t checksum = 0x00;
+
+ for (i = 0; i < datalen; i++)
+ {
+ checksum += data[i];
+ }
+
+ return checksum;
+}
+
+/****************************************************************************
+ * Name: cxd5610_store16
+ ****************************************************************************/
+
+static inline void cxd5610_store16(void *dst, void *src)
+{
+#if defined(CONFIG_SENSORS_CXD5610_GNSS_UNALIGNED_ACCESS)
+ *(uint16_t *)dst = *(uint16_t *)src;
+#elif !defined(CONFIG_ENDIAN_BIG)
+ ((uint8_t *)dst)[0] = ((uint8_t *)src)[0];
+ ((uint8_t *)dst)[1] = ((uint8_t *)src)[1];
+#else
+ ((uint8_t *)dst)[0] = ((uint8_t *)src)[1];
+ ((uint8_t *)dst)[1] = ((uint8_t *)src)[0];
+#endif
+}
+
+/****************************************************************************
+ * Name: cxd5610_store32
+ ****************************************************************************/
+
+static void cxd5610_store32(void *dst, void *src)
+{
+#if defined(CONFIG_SENSORS_CXD5610_GNSS_UNALIGNED_ACCESS)
+ *(uint32_t *)dst = *(uint32_t *)src;
+#elif !defined(CONFIG_ENDIAN_BIG)
+ ((uint8_t *)dst)[0] = ((uint8_t *)src)[0];
+ ((uint8_t *)dst)[1] = ((uint8_t *)src)[1];
+ ((uint8_t *)dst)[2] = ((uint8_t *)src)[2];
+ ((uint8_t *)dst)[3] = ((uint8_t *)src)[3];
+#else
+ ((uint8_t *)dst)[0] = ((uint8_t *)src)[3];
+ ((uint8_t *)dst)[1] = ((uint8_t *)src)[2];
+ ((uint8_t *)dst)[2] = ((uint8_t *)src)[1];
+ ((uint8_t *)dst)[3] = ((uint8_t *)src)[0];
+#endif
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_recv
+ ****************************************************************************/
+
+static int cxd5610_gnss_recv(struct cxd5610_gnss_dev_s *priv,
+ uint8_t *buffer, int buflen)
+{
+ int ret = OK;
+
+ if (priv->lower && priv->lower->ops->recv)
+ {
+ ret = priv->lower->ops->recv(priv->lower, buffer, buflen);
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_notify_time
+ ****************************************************************************/
+
+static int cxd5610_gnss_notify_time(struct cxd5610_gnss_dev_s *priv, int len)
+{
+ struct cmd_notify_time_s *param =
+ (struct cmd_notify_time_s *)priv->notifybuf;
+ struct cxd56_gnss_receiver2_s *receiver = &priv->posdat2->receiver;
+ struct timespec ts;
+
+ /* If the packet is invalid, do not update received data */
+
+ if (IS_NOTIFY_INVALID(param->ver8))
+ {
+ return OK;
+ }
+
+ /* Get exclusive control for buffer access */
+
+ cxd5610_gnss_buffer_lock(priv);
+
+ /* Record the current timestamp in usec */
+
+ clock_systime_timespec(&ts);
+ priv->posdat2->timestamp = 1000000ull * ts.tv_sec + ts.tv_nsec / 1000;
+
+ /* Receive UTC time information */
+
+ receiver->date.year = ((param->yearu_mon >> 4) << 8) | param->yearl;
+ receiver->date.month = param->yearu_mon & 0xf;
+ receiver->date.day = param->day;
+ receiver->time.hour = param->hour;
+ receiver->time.minute = param->min;
+ receiver->time.sec = param->sec;
+ receiver->time.usec = param->sec100 * 10000;
+
+ sninfo("%04d/%02d/%02d %02d:%02d:%02d.%ld\n",
+ receiver->date.year,
+ receiver->date.month,
+ receiver->date.day,
+ receiver->time.hour,
+ receiver->time.minute,
+ receiver->time.sec,
+ receiver->time.usec);
+
+ /* Release exclusive control for buffer access */
+
+ cxd5610_gnss_buffer_unlock(priv);
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_notify_pos
+ ****************************************************************************/
+
+static int cxd5610_gnss_notify_pos(struct cxd5610_gnss_dev_s *priv, int len)
+{
+ struct cmd_notify_pos_s *param =
+ (struct cmd_notify_pos_s *)priv->notifybuf;
+ struct cxd56_gnss_receiver2_s *receiver = &priv->posdat2->receiver;
+ int32_t lat32;
+ int32_t lon32;
+ int32_t alt32;
+
+ /* If the packet is invalid, do not update received data */
+
+ if (IS_NOTIFY_INVALID(param->ver8))
+ {
+ return OK;
+ }
+
+ /* Get exclusive control for buffer access */
+
+ cxd5610_gnss_buffer_lock(priv);
+
+ /* Receive position information */
+
+ cxd5610_store32(&lat32, ¶m->lat32);
+ cxd5610_store32(&lon32, ¶m->lon32);
+ cxd5610_store32(&alt32, ¶m->alt32);
+ receiver->latitude = (double)lat32 / 10000000.0;
+ receiver->longitude = (double)lon32 / 10000000.0;
+ receiver->altitude = (double)alt32 / 100.0;
+ receiver->geoid = 0.0;
+ receiver->svtype = (param->mode >> 4);
+ receiver->fix_indicator = (param->mode & 0xf);
+
+ if (receiver->fix_indicator > 0)
+ {
+ receiver->pos_dataexist = 1;
+ }
+
+ sninfo("lat=%.7lf[deg] lon=%.7lf[deg] alt=%.2lf[m] svtype=%d fix=%d\n",
+ receiver->latitude,
+ receiver->longitude,
+ receiver->altitude,
+ receiver->svtype,
+ receiver->fix_indicator);
+
+ /* Release exclusive control for buffer access */
+
+ cxd5610_gnss_buffer_unlock(priv);
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_notify_vel
+ ****************************************************************************/
+
+static int cxd5610_gnss_notify_vel(struct cxd5610_gnss_dev_s *priv, int len)
+{
+ struct cmd_notify_vel_s *param =
+ (struct cmd_notify_vel_s *)priv->notifybuf;
+ struct cxd56_gnss_receiver2_s *receiver = &priv->posdat2->receiver;
+ uint16_t course16;
+ uint16_t mag_course16;
+ int16_t vel16;
+ int16_t up_vel16;
+
+ /* If the packet is invalid, do not update received data */
+
+ if (IS_NOTIFY_INVALID(param->ver8))
+ {
+ return OK;
+ }
+
+ /* Get exclusive control for buffer access */
+
+ cxd5610_gnss_buffer_lock(priv);
+
+ /* Receive velocity information */
+
+ cxd5610_store16(&course16, ¶m->course16);
+ cxd5610_store16(&mag_course16, ¶m->mag_course16);
+ cxd5610_store16(&vel16, ¶m->vel16);
+ cxd5610_store16(&up_vel16, ¶m->up_vel16);
+ receiver->velocity = (float)vel16 / 10.0f;
+ receiver->direction = (float)course16 / 10.0f;
+ receiver->mag_course = (float)mag_course16 / 10.0f;
+ receiver->up_velocity = (float)up_vel16 / 10.0f;
+ receiver->vel_fixmode = (param->mode & 0x3);
+
+ sninfo("vel=%.1lf[km/s] course=%.1lf[deg] upvel=%.1lf[km/s] mode=%d\n",
+ receiver->velocity,
+ receiver->direction,
+ receiver->up_velocity,
+ receiver->vel_fixmode);
+
+ /* Release exclusive control for buffer access */
+
+ cxd5610_gnss_buffer_unlock(priv);
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_notify_sat
+ ****************************************************************************/
+
+static int cxd5610_gnss_notify_sat(struct cxd5610_gnss_dev_s *priv, int len)
+{
+ struct cmd_notify_sat_s *param =
+ (struct cmd_notify_sat_s *)priv->notifybuf;
+ struct cmd_notify_satinfo_s *info =
+ (struct cmd_notify_satinfo_s *)&priv->notifybuf[4];
+ struct cxd56_gnss_receiver2_s *receiver = &priv->posdat2->receiver;
+ struct cxd56_gnss_sv2_s *sv = priv->posdat2->sv;
+ uint16_t numsv;
+ uint16_t azimuth;
+ int i;
+ int j;
+ int n;
+ uint8_t stat;
+ uint8_t type;
+ uint8_t svid;
+ bool isdup;
+
+ /* If the packet is invalid, do not update received data */
+
+ if (IS_NOTIFY_INVALID(param->ver8))
+ {
+ return OK;
+ }
+
+ /* Get exclusive control for buffer access */
+
+ cxd5610_gnss_buffer_lock(priv);
+
+ /* Receive satellite information */
+
+ cxd5610_store16(&numsv, ¶m->numsv);
+ numsv = MIN(numsv, CXD56_GNSS_MAX_SV2_NUM);
+ receiver->pos_fixmode = param->mode & 0x3;
+
+ for (i = 0, n = 0; i < numsv; i++)
+ {
+ isdup = false;
+ stat = info[i].signal;
+ type = info[i].signal & 0xf;
+ svid = info[i].svid;
+
+ /* Workaround an issue that there are duplicated satellites */
+
+ for (j = 0; j < n; j++)
+ {
+ if ((sv[j].type == type) && (sv[j].svid == svid))
+ {
+ sninfo("Duplicated signal=%d svid=%d\n", type, svid);
+ isdup = true;
+ break;
+ }
+ }
+
+ if (isdup)
+ {
+ continue;
+ }
+
+ sv[n].type = type;
+ sv[n].svid = svid;
+ sv[n].stat = CXD56_GNSS_SV_STAT_VISIBLE;
+ sv[n].stat |= (stat & 0x20) ? CXD56_GNSS_SV_STAT_TRACKING : 0;
+ sv[n].stat |= (stat & 0x80) ? CXD56_GNSS_SV_STAT_POSITIONING : 0;
+ sv[n].stat |= (stat & 0x40) ? CXD56_GNSS_SV_STAT_CALC_VELOCITY : 0;
+ cxd5610_store16(&azimuth, &info[i].azimuth);
+ sv[n].azimuth = (azimuth < 360) ? azimuth : 0;
+ sv[n].elevation = (info[i].elevation <= 90) ? info[i].elevation : 0;
+ sv[n].siglevel = info[i].cn;
+ n++;
+ }
+
+ priv->posdat2->svcount = receiver->numsv = n;
+
+ sninfo("pos_fixmode=%d numsv=%d\n",
+ receiver->pos_fixmode, receiver->numsv);
+
+ for (i = 0; i < receiver->numsv; i++)
+ {
+ sninfo("%c%c%c signal=%2d svid=%3d cn=%2d elv=%2d azi=%3d\n",
+ (sv[i].stat & 0x2) ? 'P' : '-',
+ (sv[i].stat & 0x4) ? 'V' : '-',
+ (sv[i].stat & 0x1) ? 'T' : '-',
+ sv[i].type,
+ sv[i].svid, sv[i].siglevel, sv[i].elevation, sv[i].azimuth);
+ }
+
+ /* Release exclusive control for buffer access */
+
+ cxd5610_gnss_buffer_unlock(priv);
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_notify_acc
+ ****************************************************************************/
+
+static int cxd5610_gnss_notify_acc(struct cxd5610_gnss_dev_s *priv, int len)
+{
+ struct cmd_notify_acc_s *param =
+ (struct cmd_notify_acc_s *)priv->notifybuf;
+ struct cxd56_gnss_receiver2_s *receiver = &priv->posdat2->receiver;
+ uint16_t h_uc16;
+ uint16_t v_uc16;
+ uint16_t h_speed_uc16;
+ uint16_t v_speed_uc16;
+ uint16_t semimajor16;
+ uint16_t semiminor16;
+
+ /* If the packet is invalid, do not update received data */
+
+ if (IS_NOTIFY_INVALID(param->ver8))
+ {
+ return OK;
+ }
+
+ /* Get exclusive control for buffer access */
+
+ cxd5610_gnss_buffer_lock(priv);
+
+ /* Receive accuracy information */
+
+ receiver->pdop = (float)param->pdop8 / 10.0f;
+ receiver->hdop = (float)param->hdop8 / 10.0f;
+ receiver->vdop = (float)param->vdop8 / 10.0f;
+ cxd5610_store16(&h_uc16, ¶m->h_uc16);
+ cxd5610_store16(&v_uc16, ¶m->v_uc16);
+ cxd5610_store16(&h_speed_uc16, ¶m->h_speed_uc16);
+ cxd5610_store16(&v_speed_uc16, ¶m->v_speed_uc16);
+ cxd5610_store16(&semimajor16, ¶m->semimajor16);
+ cxd5610_store16(&semiminor16, ¶m->semiminor16);
+ receiver->majdop = (float)semimajor16;
+ receiver->mindop = (float)semiminor16;
+ receiver->oridop = (float)param->orientation;
+ receiver->hvar = (float)h_uc16;
+ receiver->vvar = (float)v_uc16;
+ receiver->hvar_speed = (float)h_speed_uc16 / 10.0f;
+ receiver->vvar_speed = (float)v_speed_uc16 / 10.0f;
+
+ sninfo("h=%.1f[m] v=%.1f[m] hs=%.1f[km/s] vs=%.1f[km/s] "
+ "PDOP,HDOP,VDOP=%.1f,%.1f,%.1f "
+ "maj,min,ori=%.1f[m],%.1f[m],%.1f[deg]\n",
+ receiver->hvar, receiver->vvar,
+ receiver->hvar_speed, receiver->vvar_speed,
+ receiver->pdop, receiver->hdop, receiver->vdop,
+ receiver->majdop, receiver->mindop, receiver->oridop);
+
+ /* Release exclusive control for buffer access */
+
+ cxd5610_gnss_buffer_unlock(priv);
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_notify_dcreport
+ ****************************************************************************/
+
+static int
+cxd5610_gnss_notify_dcreport(struct cxd5610_gnss_dev_s *priv, int len)
+{
+ struct cmd_notify_dcreport_s *param =
+ (struct cmd_notify_dcreport_s *)priv->notifybuf;
+ struct cxd56_gnss_dcreport_data_s *dcrdat = priv->dcrdat;
+ int i;
+
+ /* If the packet is invalid, do not update received data */
+
+ if (IS_NOTIFY_INVALID(param->ver8))
+ {
+ return OK;
+ }
+
+ if (param->nr == 0)
+ {
+ return -ENOENT;
+ }
+
+ /* Only one message is received due to duplicate messages */
+
+ param->nr = 1;
+
+ /* Get exclusive control for buffer access */
+
+ cxd5610_gnss_buffer_lock(priv);
+
+ /* Receive disaster and crisis report information */
+
+ for (i = 0; i < param->nr; i++)
+ {
+ sninfo("svid=%d\n", param->msg[i].svid);
+ dcrdat->svid = param->msg[i].svid;
+ memcpy(dcrdat->sf, param->msg[i].data, sizeof(dcrdat->sf));
+ }
+
+ /* Release exclusive control for buffer access */
+
+ cxd5610_gnss_buffer_unlock(priv);
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_notify
+ ****************************************************************************/
+
+static int
+cxd5610_gnss_notify(struct cxd5610_gnss_dev_s *priv, uint8_t opc, int len)
+{
+ int ret = OK;
+
+ switch (opc)
+ {
+ case OPC_TIME_NOTIFY:
+ ret = cxd5610_gnss_notify_time(priv, len);
+ break;
+ case OPC_RECEIVER_POS_NOTIFY:
+ ret = cxd5610_gnss_notify_pos(priv, len);
+ break;
+ case OPC_RECEIVER_VEL_NOTIFY:
+ ret = cxd5610_gnss_notify_vel(priv, len);
+ break;
+ case OPC_SAT_INFO_NOTIFY:
+ ret = cxd5610_gnss_notify_sat(priv, len);
+ break;
+ case OPC_DISASTER_CRISIS_NOTIFY:
+ ret = cxd5610_gnss_notify_dcreport(priv, len);
+#if CONFIG_SENSORS_CXD5610_GNSS_NSIGNALRECEIVERS != 0
+ if (ret >= 0)
+ {
+ cxd5610_gnss_signalhandler(priv, CXD56_GNSS_SIG_DCREPORT);
+ }
+#endif
+ break;
+ case OPC_ACCURACY_IDX_NOTIFY:
+ ret = cxd5610_gnss_notify_acc(priv, len);
+
+ /* Notify after the last message received */
+
+#if CONFIG_SENSORS_CXD5610_GNSS_NPOLLWAITERS != 0
+ cxd5610_gnss_pollnotify(priv);
+#endif
+#if CONFIG_SENSORS_CXD5610_GNSS_NSIGNALRECEIVERS != 0
+ cxd5610_gnss_signalhandler(priv, CXD56_GNSS_SIG_GNSS);
+#endif
+ break;
+ default:
+ break;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_send
+ ****************************************************************************/
+
+static int cxd5610_gnss_send(struct cxd5610_gnss_dev_s *priv,
+ uint8_t *buffer, int buflen)
+{
+ int ret = OK;
+
+ if (priv->lower && priv->lower->ops->send)
+ {
+ ret = priv->lower->ops->send(priv->lower, buffer, buflen);
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_sendcmd
+ ****************************************************************************/
+
+static int cxd5610_gnss_sendcmd(struct cxd5610_gnss_dev_s *priv,
+ uint8_t opc, const void *opr, int oprlen)
+{
+ int ret;
+ int buflen = 5;
+ uint8_t *pkt = priv->sndbuf;
+ int8_t errcode;
+
+ /* Check parameter */
+
+ if ((oprlen < 0) ||
+ (CONFIG_SENSORS_CXD5610_GNSS_SNDBUF_SIZE < PACKET_LEN(oprlen)))
+ {
+ return -EINVAL;
+ }
+
+ /* Set command header */
+
+ pkt[0] = PACKET_SYNC;
+ pkt[1] = (uint8_t)(oprlen & 0xff);
+ pkt[2] = (uint8_t)(oprlen >> 8);
+ pkt[3] = opc;
+ pkt[4] = cxd5610_checksum(pkt, 4);
+
+ /* Set command payload */
+
+ if (opr && (oprlen > 0))
+ {
+ memcpy(&pkt[5], opr, oprlen);
+ pkt[5 + oprlen] = cxd5610_checksum(&pkt[5], oprlen);
+ buflen += (oprlen + 1);
+ }
+
+ /* Send a command */
+
+ ret = cxd5610_gnss_send(priv, priv->sndbuf, buflen);
+ if (ret >= 0)
+ {
+ /* Wait for command response */
+
+ ret = cxd5610_gnss_wait_command(priv, COMMAND_WAIT_TIMEOUT);
+ if (ret >= 0)
+ {
+ errcode = (int8_t)priv->rcvbuf[0];
+ ret = (int)errcode;
+ }
+ }
+ else if ((opc == OPC_SYS_STATE_CHANGE_INSTRUCTION) &&
+ (((uint8_t *)opr)[0] == STATE_WAKEUP))
+ {
+ /* Wait for wakeup command response */
+
+ ret = cxd5610_gnss_wait_command(priv, COMMAND_WAIT_TIMEOUT);
+ if (ret >= 0)
+ {
+ errcode = (int8_t)priv->rcvbuf[0];
+ ret = (int)errcode;
+ }
+ }
+ else
+ {
+ snerr("ERROR: send command opc=0x%02x ret=%d\n", opc, ret);
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_handler
+ ****************************************************************************/
+
+static void cxd5610_gnss_handler(void)
+{
+ /* Issue event from interrupt */
+
+ cxd5610_gnss_post_interrupt();
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_thread_loop
+ ****************************************************************************/
+
+static int cxd5610_gnss_thread_loop(struct cxd5610_gnss_dev_s *priv)
+{
+ uint16_t oprlen;
+ uint8_t header[5];
+ uint8_t opc;
+ uint8_t *pkt;
+ int totallen;
+ int num;
+ int rcvlen;
+ int maxlen;
+
+ /* Wait for interrupt from device */
+
+ cxd5610_gnss_wait_interrupt();
+
+ /* Read header */
+
+ memset(header, 0, sizeof(header));
+ cxd5610_gnss_recv(priv, header, sizeof(header));
+ if (header[0] != PACKET_SYNC)
+ {
+ snerr("ERROR: Failed to receive sync packet\n");
+ return OK;
+ }
+
+ cxd5610_store16(&oprlen, &header[1]);
+ opc = header[3];
+
+ /* Verify header checksum */
+
+ if (header[4] != cxd5610_checksum(header, 4))
+ {
+ snerr("ERROR: header checksum (opc=0x%02x oprlen=%d)\n",
+ opc, oprlen);
+ return OK;
+ }
+
+ sninfo("opc=0x%02x oprlen=%d\n", opc, oprlen);
+
+ if (oprlen == 0)
+ {
+ /* If no payload, wait for next received data */
+
+ return OK;
+ }
+
+ /* Check received buffer size */
+
+ maxlen = (opc < OPC_TIME_NOTIFY) ?
+ CONFIG_SENSORS_CXD5610_GNSS_RCVBUF_SIZE :
+ CONFIG_SENSORS_CXD5610_GNSS_NOTIFYBUF_SIZE;
+ if (maxlen <= oprlen)
+ {
+ snerr("ERROR: insufficient buffer size (opc=0x%02x oprlen=%d)\n",
+ opc, oprlen);
+ return ERROR;
+ }
+
+ /* Wait for interrupt from device */
+
+ cxd5610_gnss_wait_interrupt();
+
+ /* Read payload */
+
+ pkt = (opc < OPC_TIME_NOTIFY) ? priv->rcvbuf : priv->notifybuf;
+
+ totallen = oprlen + 1;
+ num = PACKET_NR(totallen);
+ rcvlen = MIN(PACKET_MAXLEN, totallen);
+ cxd5610_gnss_recv(priv, pkt, rcvlen);
+ while (--num)
+ {
+ /* Wait for interrupt from device */
+
+ cxd5610_gnss_wait_interrupt();
+
+ totallen -= rcvlen;
+ rcvlen = MIN(PACKET_MAXLEN, totallen);
+ pkt += PACKET_MAXLEN;
+ cxd5610_gnss_recv(priv, pkt, rcvlen);
+ }
+
+ /* Verify payload checksum */
+
+ pkt = (opc < OPC_TIME_NOTIFY) ? priv->rcvbuf : priv->notifybuf;
+ if (pkt[oprlen] != cxd5610_checksum(pkt, oprlen))
+ {
+ snerr("ERROR: payload checksum (opc=0x%02x oprlen=%d)\n",
+ opc, oprlen);
+ return OK;
+ }
+
+ /* Synchronous command or asynchronous notify */
+
+ if (opc < OPC_TIME_NOTIFY)
+ {
+ /* Release waiting for command response */
+
+ if (opc == OPC_SYS_STATE_CHANGE_INSTRUCTION)
+ {
+ if (pkt[0] == STATE_RESET)
+ {
+ sninfo("Boot notification\n");
+ cxd5610_gnss_post_boot(priv);
+ if (priv->wait_reset)
+ {
+ cxd5610_gnss_post_command(priv);
+ priv->wait_reset = false;
+ }
+
+ return OK;
+ }
+ else if (pkt[0] == STATE_WAKEUP)
+ {
+ sninfo("Wakeup notification\n");
+ priv->sleeping = false;
+ if (priv->wait_wakeup)
+ {
+ cxd5610_gnss_post_command(priv);
+ priv->wait_wakeup = false;
+ }
+
+ return OK;
+ }
+ }
+
+ cxd5610_gnss_post_command(priv);
+ }
+ else
+ {
+ cxd5610_gnss_notify(priv, opc, oprlen);
+ }
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_start
+ ****************************************************************************/
+
+static int cxd5610_gnss_start(struct cxd5610_gnss_dev_s *priv,
+ unsigned long arg)
+{
+ int ret;
+ uint8_t param = 3;
+
+ ret = cxd5610_gnss_sendcmd(priv, OPC_GNSS_START, ¶m, sizeof(param));
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_stop
+ ****************************************************************************/
+
+static int cxd5610_gnss_stop(struct cxd5610_gnss_dev_s *priv,
+ unsigned long arg)
+{
+ int ret;
+
+ ret = cxd5610_gnss_sendcmd(priv, OPC_GNSS_STOP, NULL, 0);
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_select_satellite_system
+ ****************************************************************************/
+
+static int
+cxd5610_gnss_select_satellite_system(struct cxd5610_gnss_dev_s *priv,
+ unsigned long arg)
+{
+ return OK;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_get_satellite_system
+ ****************************************************************************/
+
+static int cxd5610_gnss_get_satellite_system(struct cxd5610_gnss_dev_s *priv,
+ unsigned long arg)
+{
+ *(uint32_t *)arg = 0xffff;
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_set_receiver_position_ellipsoidal
+ ****************************************************************************/
+
+static int
+cxd5610_gnss_set_receiver_position_ellipsoidal(
+ struct cxd5610_gnss_dev_s *priv,
+ unsigned long arg)
+{
+ int ret;
+ struct cxd56_gnss_ellipsoidal_position_s *pos;
+ struct cmd_ellipsoidal_position_s param;
+
+ pos = (struct cxd56_gnss_ellipsoidal_position_s *)arg;
+
+ param.lat32 = (int32_t)(pos->latitude * 1000000);
+ param.lon32 = (int32_t)(pos->longitude * 1000000);
+ param.alt32 = (int32_t)pos->altitude;
+
+ ret = cxd5610_gnss_sendcmd(priv, OPC_RECEIVER_POS_SET,
+ ¶m, sizeof(param));
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_set_ope_mode
+ ****************************************************************************/
+
+static int cxd5610_gnss_set_ope_mode(struct cxd5610_gnss_dev_s *priv,
+ unsigned long arg)
+{
+ int ret;
+ struct cxd56_gnss_ope_mode_param_s *ope_mode;
+ struct cmd_op_mode_s param;
+
+ ope_mode = (struct cxd56_gnss_ope_mode_param_s *)arg;
+
+ param.mode8 = (uint8_t)ope_mode->mode;
+ param.sleep32 = 0;
+ cxd5610_store32(¶m.cycle32, &ope_mode->cycle);
+
+ ret = cxd5610_gnss_sendcmd(priv, OPC_OP_MODE_SET, ¶m, sizeof(param));
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_get_ope_mode
+ ****************************************************************************/
+
+static int cxd5610_gnss_get_ope_mode(struct cxd5610_gnss_dev_s *priv,
+ unsigned long arg)
+{
+ int ret;
+ struct cxd56_gnss_ope_mode_param_s *ope_mode;
+
+ ope_mode = (struct cxd56_gnss_ope_mode_param_s *)arg;
+
+ ret = cxd5610_gnss_sendcmd(priv, OPC_OP_MODE_GET, NULL, 0);
+
+ if (ret == OK)
+ {
+ uint8_t *res = priv->rcvbuf;
+ ope_mode->mode = 1;
+ cxd5610_store32(&ope_mode->cycle, &res[1]);
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_set_time
+ ****************************************************************************/
+
+static int cxd5610_gnss_set_time(struct cxd5610_gnss_dev_s *priv,
+ unsigned long arg)
+{
+ int ret;
+ struct cxd56_gnss_datetime_s *date_time;
+ struct cmd_datetime_s param;
+
+ date_time = (struct cxd56_gnss_datetime_s *)arg;
+
+ param.type = 0;
+ param.yearl = (uint8_t)(date_time->date.year & 0xff);
+ param.yearu_mon = (uint8_t)((date_time->date.year >> 8) << 4) |
+ (date_time->date.month & 0x0f);
+ param.day = date_time->date.day;
+ param.hour = date_time->time.hour;
+ param.min = date_time->time.minute;
+ param.sec = date_time->time.sec;
+ param.sec100 = (uint8_t)(date_time->time.usec / 10000);
+
+ ret = cxd5610_gnss_sendcmd(priv, OPC_UTC_TIME_SET, ¶m, sizeof(param));
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_save_backup_data
+ ****************************************************************************/
+
+static int cxd5610_gnss_save_backup_data(struct cxd5610_gnss_dev_s *priv,
+ unsigned long arg)
+{
+ int ret;
+ uint8_t param = 1;
+
+ ret = cxd5610_gnss_sendcmd(priv, OPC_BACKUP_MANUAL, ¶m, sizeof(param));
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_set_1pps_output
+ ****************************************************************************/
+
+static int cxd5610_gnss_set_1pps_output(struct cxd5610_gnss_dev_s *priv,
+ unsigned long arg)
+{
+ int ret;
+ uint8_t param = (uint8_t)arg;
+
+ ret = cxd5610_gnss_sendcmd(priv, OPC_PPS_OUTPUT, ¶m, sizeof(param));
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_get_version
+ ****************************************************************************/
+
+static int cxd5610_gnss_get_version(struct cxd5610_gnss_dev_s *priv,
+ unsigned long arg)
+{
+ int ret;
+ char *version;
+
+ version = (char *)arg;
+
+ memset(version, 0, CXD56_GNSS_VERSION_MAXLEN);
+
+ ret = cxd5610_gnss_sendcmd(priv, OPC_FWVER_REQ, NULL, 0);
+
+ if (ret == OK)
+ {
+ int i;
+ char *ch = (char *)&priv->rcvbuf[1];
+
+ /* Get version information */
+
+ for (i = 0; i < CXD56_GNSS_VERSION_MAXLEN - 1; i++)
+ {
+ if ((*ch == '\r') || (*ch == '\n'))
+ {
+ version[i] = '\0';
+ break;
+ }
+
+ version[i] = *ch++;
+ }
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_sleep
+ ****************************************************************************/
+
+static int cxd5610_gnss_sleep(struct cxd5610_gnss_dev_s *priv,
+ unsigned long arg)
+{
+ int ret;
+ uint8_t param = (arg == 0) ? STATE_SLEEP : STATE_DEEPSLEEP;
+
+ /* If already in sleeping mode, do nothing. */
+
+ if (priv->sleeping)
+ {
+ return OK;
+ }
+
+ ret = cxd5610_gnss_sendcmd(priv, OPC_SYS_STATE_CHANGE_INSTRUCTION,
+ ¶m, sizeof(param));
+
+ /* If the command succeeds, enter sleeping mode. */
+
+ if (ret == (int)param)
+ {
+ priv->sleeping = true;
+ return OK;
+ }
+ else
+ {
+ return -EPERM;
+ }
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_wakeup
+ ****************************************************************************/
+
+static int cxd5610_gnss_wakeup(struct cxd5610_gnss_dev_s *priv,
+ unsigned long arg)
+{
+ int ret;
+ uint8_t param = STATE_WAKEUP;
+
+ /* If it's not in sleeping mode, do nothing. */
+
+ if (!priv->sleeping)
+ {
+ return OK;
+ }
+
+ priv->wait_wakeup = true;
+ ret = cxd5610_gnss_sendcmd(priv, OPC_SYS_STATE_CHANGE_INSTRUCTION,
+ ¶m, sizeof(param));
+
+ return (ret == (int)param) ? OK : -EPERM;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_reset
+ ****************************************************************************/
+
+static int cxd5610_gnss_reset(struct cxd5610_gnss_dev_s *priv,
+ unsigned long arg)
+{
+ int ret;
+ uint8_t param = STATE_RESET;
+
+ priv->wait_reset = true;
+ ret = cxd5610_gnss_sendcmd(priv, OPC_SYS_STATE_CHANGE_INSTRUCTION,
+ ¶m, sizeof(param));
+
+ return (ret == (int)param) ? OK : -EPERM;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_set_notify
+ ****************************************************************************/
+
+static int cxd5610_gnss_set_notify(struct cxd5610_gnss_dev_s *priv)
+{
+ int ret;
+ uint8_t param[] = {
+ OPC_TIME_NOTIFY,
+ OPC_RECEIVER_POS_NOTIFY,
+ OPC_RECEIVER_VEL_NOTIFY,
+ OPC_SAT_INFO_NOTIFY,
+ OPC_ACCURACY_IDX_NOTIFY,
+ OPC_DISASTER_CRISIS_NOTIFY
+ };
+
+ ret = cxd5610_gnss_sendcmd(priv, OPC_BINARY_OUTPUT_SET,
+ ¶m, sizeof(param));
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_core_initialize
+ ****************************************************************************/
+
+static int cxd5610_gnss_core_initialize(struct cxd5610_gnss_dev_s *priv)
+{
+ int ret = OK;
+
+ /* Enable interrupt from CXD5610 device */
+
+ if (priv->lower && priv->lower->ops->enableint)
+ {
+ ret = priv->lower->ops->enableint(priv->lower, cxd5610_gnss_handler);
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_core_finalize
+ ****************************************************************************/
+
+static int cxd5610_gnss_core_finalize(struct cxd5610_gnss_dev_s *priv)
+{
+ int ret = OK;
+
+ /* Disable interrupt */
+
+ if (priv->lower && priv->lower->ops->disableint)
+ {
+ ret = priv->lower->ops->disableint(priv->lower);
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_thread
+ ****************************************************************************/
+
+static int cxd5610_gnss_thread(int argc, char** argv)
+{
+ int ret;
+ struct cxd5610_gnss_dev_s *priv = (struct cxd5610_gnss_dev_s *)
+ ((uintptr_t)strtoul(argv[1], NULL, 0));
+
+ do
+ {
+ ret = cxd5610_gnss_thread_loop(priv);
+ }
+ while (ret == OK);
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_set_signal
+ ****************************************************************************/
+
+static int cxd5610_gnss_set_signal(struct cxd5610_gnss_dev_s *priv,
+ unsigned long arg)
+{
+ int ret = 0;
+
+#if CONFIG_SENSORS_CXD5610_GNSS_NSIGNALRECEIVERS != 0
+ struct cxd56_gnss_signal_setting_s *setting;
+ struct gnss_sig_s *sig;
+ struct gnss_sig_s *checksig;
+ pid_t pid;
+ int i;
+
+ DEBUGASSERT(arg != 0);
+
+ setting = (struct cxd56_gnss_signal_setting_s *)arg;
+ if ((setting->gnsssig != CXD56_GNSS_SIG_GNSS) &&
+ (setting->gnsssig != CXD56_GNSS_SIG_DCREPORT))
+ {
+ return -EPROTOTYPE;
+ }
+
+ sig = NULL;
+ pid = getpid();
+ for (i = 0; i < CONFIG_SENSORS_CXD5610_GNSS_NSIGNALRECEIVERS; i++)
+ {
+ checksig = &priv->sigs[i];
+ if (setting->enable)
+ {
+ if (sig == NULL && !checksig->enable)
+ {
+ sig = checksig;
+ }
+ else if (checksig->info.gnsssig == setting->gnsssig &&
+ checksig->pid == pid)
+ {
+ sig = checksig;
+ break;
+ }
+ }
+ else if (checksig->info.gnsssig == setting->gnsssig &&
+ checksig->pid == pid)
+ {
+ checksig->enable = 0;
+ goto errout;
+ }
+ }
+
+ if (sig == NULL)
+ {
+ ret = -ENOENT;
+ goto errout;
+ }
+
+ sig->enable = 1;
+ sig->pid = pid;
+ sig->info.fd = setting->fd;
+ sig->info.gnsssig = setting->gnsssig;
+ sig->info.signo = setting->signo;
+ sig->info.data = setting->data;
+
+errout:
+#endif /* CONFIG_SENSORS_CXD5610_GNSS_NSIGNALRECEIVERS != 0 */
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_initialize
+ ****************************************************************************/
+
+static int cxd5610_gnss_initialize(struct cxd5610_gnss_dev_s *priv)
+{
+ int ret = OK;
+ char *argv[2];
+ char arg1[32];
+ posix_spawnattr_t attr;
+
+ /* Create thread for receiving from CXD5610 device */
+
+ snprintf(arg1, 16, "0x%" PRIxPTR, (uintptr_t)priv);
+ argv[0] = arg1;
+ argv[1] = NULL;
+
+ posix_spawnattr_init(&attr);
+ attr.priority = CONFIG_SENSORS_CXD5610_GNSS_RX_THREAD_PRIORITY;
+ attr.stacksize = CONFIG_SENSORS_CXD5610_GNSS_RX_THREAD_STACKSIZE;
+
+ priv->pid = task_spawn("cxd5610_gnss_thread",
+ cxd5610_gnss_thread,
+ NULL, &attr, argv, NULL);
+
+ posix_spawnattr_destroy(&attr);
+
+ /* Allocate various buffers */
+
+ priv->sndbuf = kmm_malloc(CONFIG_SENSORS_CXD5610_GNSS_SNDBUF_SIZE);
+ if (priv->sndbuf == NULL)
+ {
+ snerr("ERROR: Failed to allocate sndbuf\n");
+ ret = -ENOMEM;
+ goto errout1;
+ }
+
+ priv->rcvbuf = kmm_malloc(CONFIG_SENSORS_CXD5610_GNSS_RCVBUF_SIZE);
+ if (priv->rcvbuf == NULL)
+ {
+ snerr("ERROR: Failed to allocate rcvbuf\n");
+ ret = -ENOMEM;
+ goto errout2;
+ }
+
+ priv->notifybuf = kmm_malloc(CONFIG_SENSORS_CXD5610_GNSS_NOTIFYBUF_SIZE);
+ if (priv->notifybuf == NULL)
+ {
+ snerr("ERROR: Failed to allocate notifybuf\n");
+ ret = -ENOMEM;
+ goto errout3;
+ }
+
+ priv->posdat2 = kmm_zalloc(sizeof(struct cxd56_gnss_positiondata2_s));
+ if (priv->posdat2 == NULL)
+ {
+ snerr("ERROR: Failed to allocate position data\n");
+ ret = -ENOMEM;
+ goto errout4;
+ }
+
+ priv->dcrdat = kmm_zalloc(sizeof(struct cxd56_gnss_dcreport_data_s));
+ if (priv->dcrdat == NULL)
+ {
+ snerr("ERROR: Failed to allocate dcreport data\n");
+ ret = -ENOMEM;
+ goto errout5;
+ }
+
+ /* Initialize CXD5610 device */
+
+ cxd5610_gnss_core_initialize(priv);
+
+ return OK;
+
+errout5:
+ kmm_free(priv->posdat2);
+errout4:
+ kmm_free(priv->notifybuf);
+errout3:
+ kmm_free(priv->rcvbuf);
+errout2:
+ kmm_free(priv->sndbuf);
+errout1:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_finalize
+ ****************************************************************************/
+
+static int cxd5610_gnss_finalize(struct cxd5610_gnss_dev_s *priv)
+{
+ int ret = 0;
+
+ /* Finalize CXD5610 device */
+
+ cxd5610_gnss_core_finalize(priv);
+ nxsig_sleep(1);
+
+ /* Terminate thread */
+
+ ret = nxtask_delete(priv->pid);
+
+ /* Free buffers */
+
+ kmm_free(priv->sndbuf);
+ kmm_free(priv->rcvbuf);
+ kmm_free(priv->notifybuf);
+ kmm_free(priv->posdat2);
+ kmm_free(priv->dcrdat);
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_open
+ ****************************************************************************/
+
+static int cxd5610_gnss_open(struct file *filep)
+{
+ struct inode *inode = filep->f_inode;
+ struct cxd5610_gnss_dev_s *priv = inode->i_private;
+ int ret = OK;
+
+ cxd5610_gnss_device_lock(priv);
+
+ priv->cref++;
+
+ if (priv->cref == 1)
+ {
+ ret = cxd5610_gnss_initialize(priv);
+
+ if (ret == OK)
+ {
+ cxd5610_gnss_wait_boot(priv, BOOT_WAIT_TIMEOUT);
+ cxd5610_gnss_set_notify(priv);
+ }
+ }
+
+ cxd5610_gnss_device_unlock(priv);
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_close
+ ****************************************************************************/
+
+static int cxd5610_gnss_close(struct file *filep)
+{
+ struct inode *inode = filep->f_inode;
+ struct cxd5610_gnss_dev_s *priv = inode->i_private;
+ int ret = OK;
+
+ cxd5610_gnss_device_lock(priv);
+
+ priv->cref--;
+
+ if (priv->cref == 0)
+ {
+ ret = cxd5610_gnss_finalize(priv);
+ }
+
+ cxd5610_gnss_device_unlock(priv);
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_select_readtype
+ ****************************************************************************/
+
+static int8_t cxd5610_gnss_select_readtype(off_t fpos, uint32_t *offset)
+{
+ int8_t type;
+
+ if ((fpos >= CXD56_GNSS_READ_OFFSET_LAST_GNSS) &&
+ (fpos < CXD56_GNSS_READ_OFFSET_AGPS))
+ {
+ type = CXD56_READ_DATA_TYPE_GNSS;
+ *offset = 0;
+ }
+ else if (fpos == CXD56_GNSS_READ_OFFSET_DCREPORT)
+ {
+ type = CXD56_READ_DATA_TYPE_DCREPORT;
+ *offset = 0;
+ }
+ else
+ {
+ type = -1;
+ }
+
+ return type;
+}
+
+#ifdef CONFIG_SENSORS_CXD5610_GNSS_READ_COMPAT
+/****************************************************************************
+ * Name: cxd5610_gnss_signal2type
+ ****************************************************************************/
+
+static uint8_t cxd5610_gnss_signal2type(uint8_t signal)
+{
+ return (signal <= CXD56_GNSS_SIGNAL_GPS_L5) ? CXD56_GNSS_SAT_GPS :
+ (signal <= CXD56_GNSS_SIGNAL_GLN_L1OF) ? CXD56_GNSS_SAT_GLONASS :
+ (signal == CXD56_GNSS_SIGNAL_QZS_L1S) ? CXD56_GNSS_SAT_QZ_L1S :
+ (signal <= CXD56_GNSS_SIGNAL_QZS_L5) ? CXD56_GNSS_SAT_QZ_L1CA :
+ (signal <= CXD56_GNSS_SIGNAL_BDS_B2A) ? CXD56_GNSS_SAT_BEIDOU :
+ (signal <= CXD56_GNSS_SIGNAL_GAL_E5A) ? CXD56_GNSS_SAT_GALILEO : 0;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_read_compat
+ ****************************************************************************/
+
+static ssize_t cxd5610_gnss_read_compat(struct cxd5610_gnss_dev_s *priv,
+ char *buffer, size_t len)
+{
+ uint8_t numsv_tracking = 0;
+ uint8_t numsv_calcpos = 0;
+ uint8_t numsv_calcvel = 0;
+ uint16_t svtype = 0;
+ uint16_t pos_svtype = 0;
+ uint16_t vel_svtype = 0;
+ uint32_t svnum;
+ int i;
+ struct cxd56_gnss_positiondata_s *posdat =
+ (struct cxd56_gnss_positiondata_s *)buffer;
+
+ /* Copy the position data into the old structure for compatibility */
+
+ posdat->data_timestamp = priv->posdat2->timestamp;
+ posdat->status = 0;
+ svnum = MIN(priv->posdat2->svcount, CXD56_GNSS_MAX_SV_NUM);
+ posdat->svcount = svnum;
+ posdat->receiver.type = (priv->posdat2->receiver.fix_indicator > 0) ?
+ CXD56_GNSS_PVT_TYPE_GNSS : 0;
+ posdat->receiver.dgps = (priv->posdat2->receiver.fix_indicator == 2) ?
+ 1 : 0;
+ posdat->receiver.pos_fixmode = priv->posdat2->receiver.pos_fixmode;
+ posdat->receiver.vel_fixmode = CXD56_GNSS_PVT_VELFIX_3D;
+ posdat->receiver.numsv = priv->posdat2->receiver.numsv;
+ posdat->receiver.assist = CXD56_GNSS_PVT_RECEIVER_ASSIST_NONE;
+ posdat->receiver.pos_dataexist = priv->posdat2->receiver.pos_dataexist;
+ posdat->receiver.possource = CXD56_GNSS_PVT_TYPE_GNSS;
+ posdat->receiver.tcxo_offset = 0;
+ posdat->receiver.pos_dop.pdop = priv->posdat2->receiver.pdop;
+ posdat->receiver.pos_dop.hdop = priv->posdat2->receiver.hdop;
+ posdat->receiver.pos_dop.vdop = priv->posdat2->receiver.vdop;
+ posdat->receiver.pos_dop.tdop = 0;
+ posdat->receiver.pos_dop.ewdop = 0;
+ posdat->receiver.pos_dop.nsdop = 0;
+ posdat->receiver.pos_dop.majdop = priv->posdat2->receiver.majdop;
+ posdat->receiver.pos_dop.mindop = priv->posdat2->receiver.mindop;
+ posdat->receiver.pos_dop.oridop = priv->posdat2->receiver.oridop;
+ posdat->receiver.pos_accuracy.hvar = priv->posdat2->receiver.hvar;
+ posdat->receiver.pos_accuracy.vvar = priv->posdat2->receiver.vvar;
+ posdat->receiver.latitude = priv->posdat2->receiver.latitude;
+ posdat->receiver.longitude = priv->posdat2->receiver.longitude;
+ posdat->receiver.altitude = priv->posdat2->receiver.altitude;
+ posdat->receiver.geoid = priv->posdat2->receiver.geoid;
+ posdat->receiver.velocity = priv->posdat2->receiver.velocity;
+ posdat->receiver.direction = priv->posdat2->receiver.direction;
+ posdat->receiver.date = priv->posdat2->receiver.date;
+ posdat->receiver.time = priv->posdat2->receiver.time;
+ for (i = 0; i < priv->posdat2->svcount; i++)
+ {
+ uint8_t stat = priv->posdat2->sv[i].stat;
+ uint8_t type = priv->posdat2->sv[i].type;
+
+ if (stat & 0x1)
+ {
+ svtype |= cxd5610_gnss_signal2type(type);
+ numsv_tracking++;
+ }
+
+ if (stat & 0x2)
+ {
+ pos_svtype |= cxd5610_gnss_signal2type(type);
+ numsv_calcpos++;
+ }
+
+ if (stat & 0x4)
+ {
+ vel_svtype |= cxd5610_gnss_signal2type(type);
+ numsv_calcvel++;
+ }
+ }
+
+ posdat->receiver.numsv_tracking = numsv_tracking;
+ posdat->receiver.numsv_calcpos = numsv_calcpos;
+ posdat->receiver.numsv_calcvel = numsv_calcvel;
+ posdat->receiver.svtype = svtype;
+ posdat->receiver.pos_svtype = pos_svtype;
+ posdat->receiver.vel_svtype = vel_svtype;
+
+ for (i = 0; i < svnum; i++)
+ {
+ uint8_t type = priv->posdat2->sv[i].type;
+ posdat->sv[i].type = cxd5610_gnss_signal2type(type);
+ posdat->sv[i].svid = priv->posdat2->sv[i].svid;
+ posdat->sv[i].stat = priv->posdat2->sv[i].stat;
+ posdat->sv[i].azimuth = priv->posdat2->sv[i].azimuth;
+ posdat->sv[i].elevation = priv->posdat2->sv[i].elevation;
+ posdat->sv[i].siglevel = priv->posdat2->sv[i].siglevel;
+ }
+
+ return len;
+}
+#endif /* CONFIG_SENSORS_CXD5610_GNSS_READ_COMPAT */
+
+/****************************************************************************
+ * Name: cxd5610_gnss_read
+ ****************************************************************************/
+
+static ssize_t cxd5610_gnss_read(struct file *filep, char *buffer,
+ size_t len)
+{
+ struct inode *inode = filep->f_inode;
+ struct cxd5610_gnss_dev_s *priv = inode->i_private;
+ uint32_t offset = 0;
+ int8_t type;
+
+ if (!buffer)
+ {
+ return -EINVAL;
+ }
+
+ if (len == 0)
+ {
+ return OK;
+ }
+
+ cxd5610_gnss_device_lock(priv);
+
+ /* Setect data type */
+
+ type = cxd5610_gnss_select_readtype(filep->f_pos, &offset);
+ if (type < 0)
+ {
+ cxd5610_gnss_device_unlock(priv);
+ return -ESPIPE;
+ }
+
+ /* Get exclusive control for buffer access */
+
+ cxd5610_gnss_buffer_lock(priv);
+
+ if (type == CXD56_READ_DATA_TYPE_GNSS)
+ {
+#ifdef CONFIG_SENSORS_CXD5610_GNSS_READ_COMPAT
+ if (len == sizeof(struct cxd56_gnss_positiondata_s))
+ {
+ cxd5610_gnss_read_compat(priv, buffer, len);
+ }
+ else
+#endif
+ {
+ len = MIN(sizeof(struct cxd56_gnss_positiondata2_s), len);
+ memcpy(buffer, priv->posdat2, len);
+ }
+ }
+ else if (type == CXD56_READ_DATA_TYPE_DCREPORT)
+ {
+ len = MIN(sizeof(struct cxd56_gnss_dcreport_data_s), len);
+ memcpy(buffer, priv->dcrdat, len);
+ }
+
+ /* Release exclusive control for buffer access */
+
+ cxd5610_gnss_buffer_unlock(priv);
+
+ cxd5610_gnss_device_unlock(priv);
+ return len;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_write
+ ****************************************************************************/
+
+static ssize_t cxd5610_gnss_write(struct file *filep, const char *buffer,
+ size_t buflen)
+{
+ return -ENOSYS;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_ioctl
+ ****************************************************************************/
+
+static int cxd5610_gnss_ioctl(struct file *filep, int cmd, unsigned long arg)
+{
+ struct inode *inode = filep->f_inode;
+ struct cxd5610_gnss_dev_s *priv = inode->i_private;
+ int ret = OK;
+
+ sninfo("cmd=%d arg=0x%08lx\n", cmd, arg);
+
+ cxd5610_gnss_device_lock(priv);
+
+ switch (cmd)
+ {
+ case CXD56_GNSS_IOCTL_START:
+ ret = cxd5610_gnss_start(priv, arg);
+ break;
+ case CXD56_GNSS_IOCTL_STOP:
+ ret = cxd5610_gnss_stop(priv, arg);
+ break;
+ case CXD56_GNSS_IOCTL_SELECT_SATELLITE_SYSTEM:
+ ret = cxd5610_gnss_select_satellite_system(priv, arg);
+ break;
+ case CXD56_GNSS_IOCTL_GET_SATELLITE_SYSTEM:
+ DEBUGASSERT(arg != 0);
+ ret = cxd5610_gnss_get_satellite_system(priv, arg);
+ break;
+ case CXD56_GNSS_IOCTL_SET_RECEIVER_POSITION_ELLIPSOIDAL:
+ DEBUGASSERT(arg != 0);
+ ret = cxd5610_gnss_set_receiver_position_ellipsoidal(priv, arg);
+ break;
+ case CXD56_GNSS_IOCTL_SET_OPE_MODE:
+ DEBUGASSERT(arg != 0);
+ ret = cxd5610_gnss_set_ope_mode(priv, arg);
+ break;
+ case CXD56_GNSS_IOCTL_GET_OPE_MODE:
+ DEBUGASSERT(arg != 0);
+ ret = cxd5610_gnss_get_ope_mode(priv, arg);
+ break;
+ case CXD56_GNSS_IOCTL_SET_TIME:
+ DEBUGASSERT(arg != 0);
+ ret = cxd5610_gnss_set_time(priv, arg);
+ break;
+ case CXD56_GNSS_IOCTL_SAVE_BACKUP_DATA:
+ ret = cxd5610_gnss_save_backup_data(priv, arg);
+ break;
+ case CXD56_GNSS_IOCTL_SIGNAL_SET:
+ DEBUGASSERT(arg != 0);
+ ret = cxd5610_gnss_set_signal(priv, arg);
+ break;
+ case CXD56_GNSS_IOCTL_SET_1PPS_OUTPUT:
+ ret = cxd5610_gnss_set_1pps_output(priv, arg);
+ break;
+ case CXD56_GNSS_IOCTL_GET_VERSION:
+ DEBUGASSERT(arg != 0);
+ ret = cxd5610_gnss_get_version(priv, arg);
+ break;
+ case CXD56_GNSS_IOCTL_SLEEP:
+ ret = cxd5610_gnss_sleep(priv, arg);
+ break;
+ case CXD56_GNSS_IOCTL_WAKEUP:
+ ret = cxd5610_gnss_wakeup(priv, arg);
+ break;
+ case CXD56_GNSS_IOCTL_RESET:
+ ret = cxd5610_gnss_reset(priv, arg);
+ break;
+ default:
+ break;
+ }
+
+ cxd5610_gnss_device_unlock(priv);
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_poll
+ ****************************************************************************/
+
+static int cxd5610_gnss_poll(struct file *filep, struct pollfd *fds,
+ bool setup)
+{
+ int ret = OK;
+#if CONFIG_SENSORS_CXD5610_GNSS_NPOLLWAITERS != 0
+ struct inode *inode = filep->f_inode;
+ struct cxd5610_gnss_dev_s *priv = inode->i_private;
+ int i;
+
+ cxd5610_gnss_device_lock(priv);
+
+ if (setup)
+ {
+ if ((fds->events & POLLIN) == 0)
+ {
+ ret = -EDEADLK;
+ goto errout;
+ }
+
+ for (i = 0; i < CONFIG_SENSORS_CXD5610_GNSS_NPOLLWAITERS; i++)
+ {
+ /* Find an unused slot */
+
+ if (priv->fds[i] == NULL)
+ {
+ /* Bind the poll structure and this slot */
+
+ priv->fds[i] = fds;
+ fds->priv = &priv->fds[i];
+ break;
+ }
+ }
+
+ /* No space in priv fds array for poll handling */
+
+ if (i >= CONFIG_SENSORS_CXD5610_GNSS_NPOLLWAITERS)
+ {
+ fds->priv = NULL;
+ ret = -EBUSY;
+ goto errout;
+ }
+
+ /* Should we immediately notify on any of the requested events? */
+
+ if (priv->has_event)
+ {
+ cxd5610_gnss_pollnotify(priv);
+ }
+ }
+ else if (fds->priv)
+ {
+ /* This is a request to tear down the poll. */
+
+ struct pollfd **slot = (struct pollfd **)fds->priv;
+
+ /* Remove all memory of the poll setup */
+
+ *slot = NULL;
+ fds->priv = NULL;
+ priv->has_event = false;
+ }
+
+errout:
+ cxd5610_gnss_device_unlock(priv);
+#endif
+ return ret;
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_signalhandler
+ ****************************************************************************/
+
+#if CONFIG_SENSORS_CXD5610_GNSS_NSIGNALRECEIVERS != 0
+static void cxd5610_gnss_signalhandler(struct cxd5610_gnss_dev_s *priv,
+ uint8_t sigtype)
+{
+ int i;
+
+ for (i = 0; i < CONFIG_SENSORS_CXD5610_GNSS_NSIGNALRECEIVERS; i++)
+ {
+ struct gnss_sig_s *sig = &priv->sigs[i];
+ if (sig->enable && sig->info.gnsssig == sigtype)
+ {
+ union sigval value;
+
+ value.sival_ptr = &sig->info;
+ nxsig_queue(sig->pid, sig->info.signo, value);
+ }
+ }
+}
+#endif
+
+/****************************************************************************
+ * Name: cxd5610_gnss_pollnotify
+ ****************************************************************************/
+
+#if CONFIG_SENSORS_CXD5610_GNSS_NPOLLWAITERS != 0
+static void cxd5610_gnss_pollnotify(struct cxd5610_gnss_dev_s *dev)
+{
+ poll_notify(dev->fds, CONFIG_SENSORS_CXD5610_GNSS_NPOLLWAITERS, POLLIN);
+ dev->has_event = true;
+}
+#endif
+
+/****************************************************************************
+ * Name: cxd5610_gnss_device_init/lock/unlock
+ ****************************************************************************/
+
+static int cxd5610_gnss_device_init(struct cxd5610_gnss_dev_s *priv)
+{
+ return nxmutex_init(&priv->dev_lock);
+}
+
+static int cxd5610_gnss_device_lock(struct cxd5610_gnss_dev_s *priv)
+{
+ return nxmutex_lock(&priv->dev_lock);
+}
+
+static int cxd5610_gnss_device_unlock(struct cxd5610_gnss_dev_s *priv)
+{
+ return nxmutex_unlock(&priv->dev_lock);
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_buffer_init/lock/unlock
+ ****************************************************************************/
+
+static int cxd5610_gnss_buffer_init(struct cxd5610_gnss_dev_s *priv)
+{
+ return nxmutex_init(&priv->buf_lock);
+}
+
+static int cxd5610_gnss_buffer_lock(struct cxd5610_gnss_dev_s *priv)
+{
+ return nxmutex_lock(&priv->buf_lock);
+}
+
+static int cxd5610_gnss_buffer_unlock(struct cxd5610_gnss_dev_s *priv)
+{
+ return nxmutex_unlock(&priv->buf_lock);
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_init/wait/post_boot
+ ****************************************************************************/
+
+static int cxd5610_gnss_init_boot(struct cxd5610_gnss_dev_s *priv)
+{
+ return nxsem_init(&priv->boot_sync, 0, 0);
+}
+
+static int cxd5610_gnss_wait_boot(struct cxd5610_gnss_dev_s *priv,
+ int sec)
+{
+ if (sec <= 0)
+ {
+ return nxsem_wait_uninterruptible(&priv->boot_sync);
+ }
+ else
+ {
+ return nxsem_tickwait_uninterruptible(&priv->boot_sync, SEC2TICK(sec));
+ }
+}
+
+static int cxd5610_gnss_post_boot(struct cxd5610_gnss_dev_s *priv)
+{
+ return nxsem_post(&priv->boot_sync);
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_init/wait/post_command
+ ****************************************************************************/
+
+static int cxd5610_gnss_init_command(struct cxd5610_gnss_dev_s *priv)
+{
+ return nxsem_init(&priv->cmd_sync, 0, 0);
+}
+
+static int cxd5610_gnss_wait_command(struct cxd5610_gnss_dev_s *priv,
+ int sec)
+{
+ if (sec <= 0)
+ {
+ return nxsem_wait_uninterruptible(&priv->cmd_sync);
+ }
+ else
+ {
+ return nxsem_tickwait_uninterruptible(&priv->cmd_sync, SEC2TICK(sec));
+ }
+}
+
+static int cxd5610_gnss_post_command(struct cxd5610_gnss_dev_s *priv)
+{
+ return nxsem_post(&priv->cmd_sync);
+}
+
+/****************************************************************************
+ * Name: cxd5610_gnss_init/wait/post_interrupt
+ ****************************************************************************/
+
+static int cxd5610_gnss_init_interrupt(void)
+{
+ return nxsem_init(&g_int_sync, 0, 0);
+}
+
+static int cxd5610_gnss_wait_interrupt(void)
+{
+ return nxsem_wait_uninterruptible(&g_int_sync);
+}
+
+static int cxd5610_gnss_post_interrupt(void)
+{
+ return nxsem_post(&g_int_sync);
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: cxd5610_gnss_register
+ *
+ * Description:
+ * Register the CXD5610 GNSS character device as 'devpath'
+ *
+ * Input Parameters:
+ * devpath - The full path to the driver to register. E.g., "/dev/gps2"
+ * lower - An instance of the lower half interface
+ *
+ * Returned Value:
+ * Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+int cxd5610_gnss_register(const char *devpath,
+ struct cxd5610_gnss_lowerhalf_s *lower)
+{
+ struct cxd5610_gnss_dev_s *priv;
+ int ret;
+
+ /* Initialize the CXD5610 GNSS device structure */
+
+ priv = (struct cxd5610_gnss_dev_s *)
+ kmm_zalloc(sizeof(struct cxd5610_gnss_dev_s));
+ if (!priv)
+ {
+ snerr("ERROR: Failed to allocate instance\n");
+ return -ENOMEM;
+ }
+
+ /* Register lower-half driver */
+
+ priv->lower = lower;
+
+ cxd5610_gnss_device_init(priv);
+ cxd5610_gnss_buffer_init(priv);
+ cxd5610_gnss_init_boot(priv);
+ cxd5610_gnss_init_command(priv);
+ cxd5610_gnss_init_interrupt();
+
+ /* Register the character driver */
+
+ ret = register_driver(devpath, &g_cxd5610fops, 0666, priv);
+ if (ret < 0)
+ {
+ snerr("ERROR: Failed to register driver: %d\n", ret);
+ kmm_free(priv);
+ }
+
+ sninfo("CXD5610 GNSS driver loaded successfully!\n");
+
+ return ret;
+}
diff --git a/include/nuttx/sensors/cxd5610_gnss.h
b/include/nuttx/sensors/cxd5610_gnss.h
new file mode 100644
index 0000000000..1b72199fb0
--- /dev/null
+++ b/include/nuttx/sensors/cxd5610_gnss.h
@@ -0,0 +1,98 @@
+/****************************************************************************
+ * include/nuttx/sensors/cxd5610_gnss.h
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership. The
+ * ASF licenses this file to you 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.
+ *
+ ****************************************************************************/
+
+#ifndef __INCLUDE_NUTTX_SENSORS_CXD5610_GNSS_H
+#define __INCLUDE_NUTTX_SENSORS_CXD5610_GNSS_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <stdint.h>
+
+/****************************************************************************
+ * Public Types
+ ****************************************************************************/
+
+/* This structure provides the "lower-half" driver operations available to
+ * the "upper-half" driver.
+ */
+
+struct cxd5610_gnss_lowerhalf_s;
+
+/* Structure for cxd5610 lower-half operations */
+
+struct cxd5610_gnss_lowerops_s
+{
+ /* Configure the cxd5610 gnss sensor device */
+
+ CODE int (*send)(FAR struct cxd5610_gnss_lowerhalf_s *lower,
+ FAR uint8_t *buffer, int buflen);
+ CODE int (*recv)(FAR struct cxd5610_gnss_lowerhalf_s *lower,
+ FAR uint8_t *buffer, int buflen);
+ CODE int (*enableint)(FAR struct cxd5610_gnss_lowerhalf_s *lower,
+ CODE void (*handler)(void));
+ CODE int (*disableint)(FAR struct cxd5610_gnss_lowerhalf_s *lower);
+};
+
+/* Structure for cxd5610 lower-half driver */
+
+struct cxd5610_gnss_lowerhalf_s
+{
+ FAR const struct cxd5610_gnss_lowerops_s *ops;
+};
+
+#ifdef __cplusplus
+#define EXTERN extern "C"
+extern "C"
+{
+#else
+#define EXTERN extern
+#endif
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: cxd5610_gnss_register
+ *
+ * Description:
+ * Register the CXD5610 GNSS character device as 'devpath'
+ *
+ * Input Parameters:
+ * devpath - The full path to the driver to register.
+ * lower - An instance of the lower half interface
+ *
+ * Returned Value:
+ * Zero (OK) on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+int cxd5610_gnss_register(FAR const char *devpath,
+ FAR struct cxd5610_gnss_lowerhalf_s *lower);
+
+#undef EXTERN
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __INCLUDE_NUTTX_SENSORS_CXD5610_GNSS_H */
