qrtr: Introduce initial qrtr support

In recent platforms the previous SMD based transport is replaced by QRTR
sockets, introduce the initial support for this.

This has been tested by booting the modem and sending loopback messages
and retrieving the associated responses. More work is needed to handle
dying remotes etc.

Signed-off-by: Bjorn Andersson <bjorn.andersson@linaro.org>

Makefile

@@ -1,4 +1,5 @@
-HAVE_LIBUDEV=1
+HAVE_LIBUDEV=0
+HAVE_LIBQRTR=0
 
 .PHONY: all
 
@@ -12,6 +13,10 @@ ifeq ($(HAVE_LIBUDEV),1)
 CFLAGS += -DHAS_LIBUDEV=1
 LDFLAGS += -ludev
 endif
+ifeq ($(HAVE_LIBQRTR),1)
+CFLAGS += -DHAS_LIBQRTR=1
+LDFLAGS += -lqrtr
+endif
 
 SRCS := app_cmds.c \
 	circ_buf.c \
@@ -35,6 +40,10 @@ ifeq ($(HAVE_LIBUDEV),1)
 SRCS += peripheral-rpmsg.c
 endif
 
+ifeq ($(HAVE_LIBQRTR),1)
+SRCS += peripheral-qrtr.c
+endif
+
 OBJS := $(SRCS:.c=.o)
 
 $(DIAG): $(OBJS)

diag.h

@@ -75,8 +75,11 @@ struct peripheral {
 	struct list_head dataq;
 
 	int cntl_fd;
+	bool cntl_open;
 	int data_fd;
+	bool data_open;
 	int cmd_fd;
+	int dci_cmd_fd;
 
 	int diag_id;
 

peripheral-qrtr.c

@@ -0,0 +1,308 @@
+/*
+ * Copyright (c) 2016-2018, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2016-2018, Linaro Ltd.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * 3. Neither the name of the copyright holder nor the names of its contributors
+ * may be used to endorse or promote products derived from this software without
+ * specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <err.h>
+#include <errno.h>
+#include <libqrtr.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "diag.h"
+#include "diag_cntl.h"
+#include "dm.h"
+#include "peripheral-qrtr.h"
+#include "watch.h"
+#include "util.h"
+
+#define DIAG_SERVICE_ID		4097
+
+#define DIAG_INSTANCE_BASE_MODEM	0
+#define DIAG_INSTANCE_BASE_LPASS	64
+#define DIAG_INSTANCE_BASE_WCNSS	128
+#define DIAG_INSTANCE_BASE_SENSORS	192
+#define DIAG_INSTANCE_BASE_CDSP		256
+#define DIAG_INSTANCE_BASE_WDSP		320
+
+enum {
+	DIAG_INSTANCE_CNTL,
+	DIAG_INSTANCE_CMD,
+	DIAG_INSTANCE_DATA,
+	DIAG_INSTANCE_DCI_CMD,
+	DIAG_INSTANCE_DCI,
+};
+
+static int qrtr_cntl_recv(int fd, void *data)
+{
+	struct peripheral *perif = data;
+	struct sockaddr_qrtr sq;
+	struct qrtr_packet pkt;
+        socklen_t sl;
+	uint8_t buf[4096];
+	ssize_t n;
+	int ret;
+
+	sl = sizeof(sq);
+	n = recvfrom(fd, buf, sizeof(buf), 0, (void *)&sq, &sl);
+	if (n < 0) {
+		ret = -errno;
+		if (ret != -ENETRESET)
+			fprintf(stderr, "[DIAG-QRTR] recvfrom failed: %d\n", ret);
+		return ret;
+	}
+
+	ret = qrtr_decode(&pkt, buf, n, &sq);
+	if (ret < 0) {
+		fprintf(stderr, "[PD-MAPPER] unable to decode qrtr packet\n");
+		return ret;
+	}
+
+	switch (pkt.type) {
+	case QRTR_TYPE_DEL_CLIENT:
+		break;
+	case QRTR_TYPE_DATA:
+		if (!perif->cntl_open) {
+			connect(perif->cntl_fd, (struct sockaddr *)&sq, sizeof(sq));
+			perif->cntl_open = true;
+
+			/* Send current message mask to the newly found peripheral */
+			diag_cntl_send_masks(perif);
+			diag_cntl_set_diag_mode(perif, true);
+			diag_cntl_set_buffering_mode(perif, 0);
+			watch_add_writeq(perif->cntl_fd, &perif->cntlq);
+		}
+
+		return diag_cntl_recv(perif, pkt.data, pkt.data_len);
+	default:
+		fprintf(stderr, "Unhandled DIAG CNTL message from %d:%d (%d)\n",
+			pkt.node, pkt.port, pkt.type);
+		break;
+	}
+
+	return 0;
+}
+
+struct non_hdlc_pkt {
+	uint8_t start;
+	uint8_t version;
+	uint16_t length;
+	char payload[];
+};
+
+static int qrtr_cmd_recv(int fd, void *data)
+{
+	struct peripheral *perif = data;
+	struct non_hdlc_pkt *frame;
+	struct sockaddr_qrtr cmdsq;
+	struct sockaddr_qrtr sq;
+	struct qrtr_packet pkt;
+	struct circ_buf *buf = &perif->recv_buf;
+        socklen_t sl;
+	ssize_t n;
+	int ret;
+
+	sl = sizeof(sq);
+	n = recvfrom(fd, buf->buf, sizeof(buf->buf), 0, (void *)&sq, &sl);
+	if (n < 0) {
+		ret = -errno;
+		if (ret != -ENETRESET)
+			fprintf(stderr, "[DIAG-QRTR] recvfrom failed: %d\n", ret);
+		return ret;
+	}
+
+	ret = qrtr_decode(&pkt, buf->buf, n, &sq);
+	if (ret < 0) {
+		fprintf(stderr, "[PD-MAPPER] unable to decode qrtr packet\n");
+		return ret;
+	}
+
+	switch (pkt.type) {
+	case QRTR_TYPE_DEL_CLIENT:
+		break;
+	case QRTR_TYPE_DATA:
+		frame = pkt.data;
+		if (frame->start != 0x7e || frame->version != 1) {
+			fprintf(stderr, "invalid non-HDLC frame\n");
+			break;
+		}
+
+		if (sizeof(*frame) + frame->length + 1 > pkt.data_len) {
+			fprintf(stderr, "truncated non-HDLC frame\n");
+			break;
+		}
+
+		if (frame->payload[frame->length] != 0x7e) {
+			fprintf(stderr, "non-HDLC frame is not truncated\n");
+			break;
+		}
+
+		dm_broadcast(frame->payload, frame->length);
+		break;
+	case QRTR_TYPE_NEW_SERVER:
+		if (pkt.node == 0 && pkt.port == 0)
+			break;
+
+		printf("Connecting CMD socket to %d:%d\n", pkt.node, pkt.port);
+		cmdsq.sq_family = AF_QIPCRTR;
+		cmdsq.sq_node = pkt.node;
+		cmdsq.sq_port = pkt.port;
+
+		ret = connect(perif->cmd_fd, (struct sockaddr *)&cmdsq, sizeof(cmdsq));
+		if (ret < 0)
+			err(1, "failed to connect to %d:%d", cmdsq.sq_node, cmdsq.sq_port);
+		watch_add_writeq(perif->cmd_fd, &perif->cmdq);
+		break;
+	default:
+		fprintf(stderr, "Unhandled DIAG CMD message from %d:%d (%d)\n",
+			pkt.node, pkt.port, pkt.type);
+		break;
+	}
+
+	return 0;
+}
+
+static int qrtr_data_recv(int fd, void *data)
+{
+	struct peripheral *perif = data;
+	struct sockaddr_qrtr sq;
+	struct qrtr_packet pkt;
+        socklen_t sl;
+	uint8_t buf[4096];
+	ssize_t n;
+	int ret;
+
+	sl = sizeof(sq);
+	n = recvfrom(fd, buf, sizeof(buf), 0, (void *)&sq, &sl);
+	if (n < 0) {
+		ret = -errno;
+		if (ret != -ENETRESET)
+			fprintf(stderr, "[DIAG-QRTR] recvfrom failed: %d\n", ret);
+		return ret;
+	}
+
+	ret = qrtr_decode(&pkt, buf, n, &sq);
+	if (ret < 0) {
+		fprintf(stderr, "[PD-MAPPER] unable to decode qrtr packet\n");
+		return ret;
+	}
+
+	switch (pkt.type) {
+	case QRTR_TYPE_DEL_CLIENT:
+		break;
+	case QRTR_TYPE_DATA:
+		if (!perif->data_open) {
+			connect(perif->data_fd, (struct sockaddr *)&sq, sizeof(sq));
+			perif->data_open = true;
+
+			watch_add_writeq(perif->data_fd, &perif->dataq);
+		}
+
+		dm_broadcast(buf, n);
+		break;
+	default:
+		fprintf(stderr, "Unhandled DIAG DATA message from %d:%d (%d)\n",
+			pkt.node, pkt.port, pkt.type);
+		break;
+	}
+
+	return 0;
+}
+
+int qrtr_perif_send(struct peripheral *perif, const void *ptr, size_t len)
+{
+	if (perif->features & DIAG_FEATURE_APPS_HDLC_ENCODE)
+		queue_push(&perif->cmdq, ptr, len);
+	else
+		hdlc_enqueue(&perif->cmdq, ptr, len);
+
+	return 0;
+}
+
+void qrtr_perif_close(struct peripheral *perif)
+{
+}
+
+static int qrtr_perif_init_subsystem(const char *name, int instance_base)
+{
+	struct peripheral *perif;
+
+	perif = calloc(1, sizeof(*perif));
+
+	perif->name = strdup(name);
+	perif->send = qrtr_perif_send;
+	perif->close = qrtr_perif_close;
+	perif->sockets = true;
+
+	list_init(&perif->cmdq);
+	list_init(&perif->cntlq);
+	list_init(&perif->dataq);
+
+	perif->cntl_fd = qrtr_open(0);
+	if (perif->cntl_fd < 0)
+		err(1, "failed to create control socket");
+
+	perif->data_fd = qrtr_open(0);
+	if (perif->data_fd < 0)
+		err(1, "failed to create data socket");
+
+	perif->cmd_fd = qrtr_open(0);
+	if (perif->cmd_fd < 0)
+		err(1, "failed to create command socket");
+
+	perif->dci_cmd_fd = qrtr_open(0);
+	if (perif->dci_cmd_fd < 0)
+		err(1, "failed to create dci command socket");
+
+	qrtr_publish(perif->cntl_fd, DIAG_SERVICE_ID, instance_base + DIAG_INSTANCE_CNTL, 0);
+	qrtr_new_lookup(perif->cmd_fd, DIAG_SERVICE_ID, instance_base + DIAG_INSTANCE_CMD, 0);
+	qrtr_publish(perif->data_fd, DIAG_SERVICE_ID, instance_base + DIAG_INSTANCE_DATA, 0);
+	qrtr_publish(perif->dci_cmd_fd, DIAG_SERVICE_ID, instance_base + DIAG_INSTANCE_DCI_CMD, 0);
+
+	watch_add_readfd(perif->cntl_fd, qrtr_cntl_recv, perif);
+	watch_add_readfd(perif->cmd_fd, qrtr_cmd_recv, perif);
+	watch_add_readfd(perif->data_fd, qrtr_data_recv, perif);
+
+	return 0;
+}
+
+int peripheral_qrtr_init(void)
+{
+	qrtr_perif_init_subsystem("modem", DIAG_INSTANCE_BASE_MODEM);
+	qrtr_perif_init_subsystem("lpass", DIAG_INSTANCE_BASE_LPASS);
+	qrtr_perif_init_subsystem("wcnss", DIAG_INSTANCE_BASE_WCNSS);
+	qrtr_perif_init_subsystem("sensors", DIAG_INSTANCE_BASE_SENSORS);
+	qrtr_perif_init_subsystem("cdsp", DIAG_INSTANCE_BASE_CDSP);
+	qrtr_perif_init_subsystem("wdsp", DIAG_INSTANCE_BASE_WDSP);
+
+	return 0;
+}

peripheral-qrtr.h

@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2018, Linaro Ltd.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * 3. Neither the name of the copyright holder nor the names of its contributors
+ * may be used to endorse or promote products derived from this software without
+ * specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef __PERIPHERAL_QRTR_H__
+#define __PERIPHERAL_QRTR_H__
+
+#if HAS_LIBQRTR
+int peripheral_qrtr_init(void);
+#else
+static inline int peripheral_qrtr_init(void)
+{
+	return 0;
+}
+#endif
+
+#endif

peripheral.c

@@ -44,6 +44,7 @@
 #include "hdlc.h"
 #include "list.h"
 #include "peripheral.h"
+#include "peripheral-qrtr.h"
 #include "peripheral-rpmsg.h"
 #include "util.h"
 #include "watch.h"
@@ -63,6 +64,7 @@ void peripheral_close(struct peripheral *peripheral)
 int peripheral_init(void)
 {
 	peripheral_rpmsg_init();
+	peripheral_qrtr_init();
 
 	return 0;
 }