cuseqmi: functional but needing cleanup

Signed-off-by: Bjørn Mork <bjorn@mork.no>

2 files changed, 597 insertions, 22 deletions

diff --git a/src/Makefile b/src/Makefile
index 6b8f3ab..664e992 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -24,4 +24,4 @@ qcqmifs: qcqmifs.c
 	$(CC) $(CFLAGS) $(CFLAGS_FUSE) $(LDFLAGS) -o $@ $^ $(LDLIBS) $(LDLIBS_FUSE)
 
 cuseqmi: cuseqmi.c
-	$(CC) $(CFLAGS) $(CFLAGS_FUSE) $(LDFLAGS) -o $@ $^ $(LDLIBS) $(LDLIBS_FUSE)
+	$(CC) $(CFLAGS) $(CFLAGS_FUSE) $(LDFLAGS) -lpthread -o $@ $^ $(LDLIBS) $(LDLIBS_FUSE)
diff --git a/src/cuseqmi.c b/src/cuseqmi.c
index d6814c7..0895a36 100644
--- a/src/cuseqmi.c
+++ b/src/cuseqmi.c
@@ -22,7 +22,7 @@
  * See the file COPYING.
  *
  * Building it:
- *   gcc -Wall `pkg-config fuse --cflags --libs` cuseqmi.c -o cuseqmi
+ *   gcc -Wall `pkg-config fuse --cflags --libs` -lpthread cuseqmi.c -o cuseqmi
  *
  *
  
@@ -46,9 +46,309 @@ TODO:
 #include <string.h>
 #include <unistd.h>
 #include <errno.h>
+#include <pthread.h>
+#include <linux/types.h>
+
+
+/* -- from qcqmi.c --- */
+
+#define IOCTL_QMI_GET_SERVICE_FILE      (0x8BE0 + 1)
+#define IOCTL_QMI_GET_DEVICE_VIDPID     (0x8BE0 + 2)
+#define IOCTL_QMI_GET_DEVICE_MEID       (0x8BE0 + 3)
+#define IOCTL_QMI_CLOSE                 (0x8BE0 + 4)
+
+#define DBG(fmt, arg...)						\
+do {									\
+	fprintf(stderr, "%s: " fmt "\n", __func__, ##arg);		\
+} while (0)
+
+struct qmux {
+	__u8 tf;	/* always 1 */
+	__u16 len;
+	__u8 ctrl;
+	__u8 service;
+	__u8 qmicid;
+} __attribute__((__packed__));
+
+const size_t qmux_size = sizeof(struct qmux);
+
+struct qmictl {
+	struct qmux h;
+	__u8 req;
+	__u8 tid;
+	__u16 msgid;
+	__u16 tlvsize;
+} __attribute__((__packed__));
+
+/* -- eof from qcqmi.c --- */
+
+
+
+/* global data */
+
+/* /dev/cdc-wdmX: */
+static int fd;                               /* handle */
+static int bufsz = 4096;                     /* message size */
+static char filename[] = "/dev/cdc-wdm0";    /* filename */
+static pthread_mutex_t wr_mutex = PTHREAD_MUTEX_INITIALIZER; /* write lock */
+static __u16 vid = 0x1199;
+static __u16 pid = 0x68a2;  /* USB vid:pid */
+#define MEIDLEN 14
+static char meid[MEIDLEN] = "0123456789abcd"; /* meid */
+
+/* defining a QMI reply or indication message */
+struct qmimsg {
+	struct qmimsg *next; /* next message */
+	size_t len;     /* length of msg */
+	struct qmux h;  /* header, which will be stripped when sending to client */
+	char msg[];
+};
+
+/* defining a client */
+struct qclient {
+	__u16 cid;
+	struct qmimsg *rq;
+	pthread_mutex_t rqlock;
+	pthread_cond_t ready;  /* data available for reading */
+	struct qclient *next;
+};
+
+/* sorted (by cid) list of open clients */
+static struct qclient *clients = NULL;
+static pthread_mutex_t cl_mutex = PTHREAD_MUTEX_INITIALIZER; /* client list lock */
+
+
+struct qclient *new_client(int cid)
+{
+	struct qclient *client = malloc(sizeof(struct qclient));
+	
+	if (!client)
+		return NULL;
+
+	client->cid = cid;
+	client->rq = NULL;
+	pthread_mutex_init(&client->rqlock, NULL);
+	pthread_cond_init(&client->ready, NULL);
+	/* can always insert at head */
+	pthread_mutex_lock(&cl_mutex);
+	client->next = clients;
+	clients = client;
+	pthread_mutex_unlock(&cl_mutex);
+	return client;
+}
+
+void destroy_client(struct qclient *client)
+{
+	struct qclient *p;
+	struct qmimsg *m, *tmp;
+
+	/* locate client in list */
+	pthread_mutex_lock(&cl_mutex);
+	if (clients == client)
+		clients = client->next;
+	else {
+		for (p = clients; p && p->next != client; p = p->next);
+		/* unlink client */
+		if (p)
+			p->next = client->next;
+	}
+	pthread_mutex_unlock(&cl_mutex);
+
+	/* unlink all unread messages */
+	pthread_mutex_lock(&client->rqlock);
+	m = client->rq;
+	while  (m) {
+		tmp = m;
+		m = m->next;
+		free(tmp);
+	}
+	pthread_mutex_unlock(&client->rqlock);
+	pthread_mutex_destroy(&client->rqlock);
+	pthread_cond_destroy(&client->ready);
+	free(client);
+}
+
+
+
+/* format and send qmi */
+
+/* predefined QMI_CTL get versions message */
+static char get_ver_msg[] = { 0x01, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x21, 0x00, 0x04, 0x00, 0x01, 0x01, 0x00, 0xff };
+
+/* predefined QMI_CTL alloc CID message */
+static char alloc_cid_msg[] = { 0x01, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x22, 0x00, 0x04, 0x00, 0x01, 0x01, 0x00, 0x00 };
+
+/* predefined QMI_CTL release CID message */
+static char release_cid_msg[] = {  0x01, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x23, 0x00, 0x05, 0x00, 0x01, 0x02, 0x00, 0x00, 0x00 };
+
+/* check if msg is a reply to a "msgid" QMI_CTL message */
+static int is_match(struct qmimsg *msg, __u16 msgid)
+{
+	struct qmictl *ctl = (struct qmictl *)&msg->h;
+	return (ctl->h.ctrl == 0x80 &&
+		ctl->h.service == 0 && 
+		ctl->h.qmicid == 0 &&
+		ctl->msgid == msgid);
+}
+
+/* send a QMI_CTL message and wait until timeout for the reply */
+static int do_ctl(char *buf, size_t buflen, int timeout)
+{
+	int rc = 0;
+	int retry = 5;
+	struct qclient *client = new_client(0); /* QMI_CTL */
+	struct qmictl *ctl;
+	__u16 msgid;
+	struct qmimsg *msg;
+
+	DBG("");
+	if (!client)
+		return -ENOMEM;
+
+	/* set up matching key */
+	ctl = (struct qmictl *)buf;
+	msgid = ctl->msgid;
+
+	/* take the write lock - no one are allowed to write anything while we run this! */
+	pthread_mutex_lock(&wr_mutex);
+	rc = write(fd, buf, buf[1] + 1); /* assuming that we always construct valid QMUX... */
+	pthread_mutex_unlock(&wr_mutex);
+
+	pthread_mutex_lock(&client->rqlock);
+retry:
+	if (!client->rq)
+		pthread_cond_wait(&client->ready, &client->rqlock);
+
+	/* check the new message(s) and retry if not matching */
+	do {
+		msg = client->rq;
+		if (msg)
+			client->rq = msg->next;
+	} while (msg && !is_match(msg, msgid));
+	if (!msg && retry--)
+		goto retry;
+
+	pthread_mutex_unlock(&client->rqlock);
+
+	/* destroy temporary client */
+	destroy_client(client);
+
+	/* may have timed out */
+	if (!msg)
+		return -ETIMEDOUT;
+
+	if (msg->h.len < buflen)
+		memcpy(buf, &msg->h, msg->h.len + 1);
+	else
+		rc = -EINVAL;
+
+	free(msg);
+	return rc;
+}
+
+static int get_ver(void)
+{
+	int rc;
+	char *buf = malloc(bufsz);
+
+	if (!buf)
+		return -ENOMEM;
+
+	/* initialize buf with default message */
+	memcpy(buf, get_ver_msg, sizeof(get_ver_msg));
+	rc = do_ctl(buf, bufsz, 5000);
+
+	/* the reply will have two TLVs: Status + result
+
+    # decode the list of supported systems in TLV 0x01
+    my $data = $ret->{'tlvs'}{0x01};
+    my $n = unpack("C", $data);
+    $data = substr($data, 1);
+    print "supports $n QMI subsystems:\n";
+    for (my $i = 0; $i < $n; $i++) {
+	my ($sys, $maj, $min) = unpack("Cvv", $data);
+	my $system = $sysname{$sys} || sprintf("%#04x", $sys);
+	print "  $system ($maj.$min)\n";
+	$data = substr($data, 5);
+    }
+	*/
+
+/*	n = buf[];
+	for (i = 0; i < n; i++) {
+		sys = buf[ + i * 5];
+		maj = le16_to_cpu(*
+		DBG("%02x: %u.%u", sys, maj, min);
+	}
+*/
+
+	free(buf);
+	return rc;
+}
+
+static int alloc_cid(struct qclient *client, __u8 system)
+{
+	int rc;
+	char *buf = malloc(bufsz);
+
+	if (!buf)
+		return -ENOMEM;
+
+	/* initialize buf with default message */
+	memcpy(buf, alloc_cid_msg, sizeof(alloc_cid_msg));
+
+	/* the last byte is the requested system */
+	buf[sizeof(alloc_cid_msg) - 1] = system;
+	
+	/* send it */
+	rc = do_ctl(buf, bufsz, 5000);
+
+	/* the reply will have two TLVs: Status + result
+	 *  01 17 00 80 00 00 01 01 22 00 0c 00 02 04 00 00 00 00 00 01 02 00 02 01
+	 * we'll just blindly assume that the last byte is the wanted one
+ 	 */
+	pthread_mutex_lock(&cl_mutex);
+	if (rc < 0)
+		client->cid = (__u16)-1;
+	else
+		client->cid = system << 8 | buf[0x17];
+	pthread_mutex_unlock(&cl_mutex);
+
+	free(buf);
+	return rc;
+}
+
+static int release_cid(struct qclient *client)
+{
+	int rc;
+	__u8 system, cid;
+	char *buf;
+
+	DBG("client=%p, cid=%04x", client, client->cid);
+	system = client->cid >> 8 & 0xff;
+	cid = client->cid & 0xff;
+
+	/* invalidate now */
+	pthread_mutex_lock(&cl_mutex);
+	client->cid = (__u16)-1;
+	pthread_mutex_unlock(&cl_mutex);
+
+	buf = malloc(bufsz);
+	if (!buf)
+		return -ENOMEM;
+
+	/* initialize buf with default message */
+	memcpy(buf, release_cid_msg, sizeof(release_cid_msg));
+	buf[sizeof(release_cid_msg) - 2] = system;
+	buf[sizeof(release_cid_msg) - 1] = cid;
+
+	/* send it */
+	rc = do_ctl(buf, bufsz, 5000);
+	free(buf);
+	return rc;
+}
+
+/* -- eof from qcqmi.c --- */
 
-static void *cuseqmi_buf;
-static size_t cuseqmi_size;
 
 static const char *usage =
 "usage: cuseqmi [options]\n"
@@ -62,49 +362,189 @@ static const char *usage =
 
 static void cuseqmi_open(fuse_req_t req, struct fuse_file_info *fi)
 {
+	struct qclient *client = new_client(-1); /* invalid CID */
+
+	fprintf(stderr, "%s\n", __func__);
+	if (!client) {
+		fuse_reply_err(req, ENOMEM);
+		return;
+	}
+	fi->nonseekable = 1;
+	fi->fh = (uint64_t)client;
 	fuse_reply_open(req, fi);
 }
 
-static void cuseqmi_read(fuse_req_t req, size_t size, off_t off,
-			 struct fuse_file_info *fi)
+static void cuseqmi_release(fuse_req_t req, struct fuse_file_info *fi)
+{
+	struct qclient *client = (void *)fi->fh;
+
+	fprintf(stderr, "%s\n", __func__);
+	fi->fh = (uint64_t)NULL;
+	destroy_client(client);
+	fuse_reply_err(req, 0);
+}
+
+/* reply with the next queued message only */
+static void cuseqmi_read(fuse_req_t req, size_t size, off_t off, struct fuse_file_info *fi)
 {
-	(void)fi;
+	struct qmimsg *msg;
+	struct qclient *client = (void *)fi->fh;
 
-	if (off >= cuseqmi_size)
-		off = cuseqmi_size;
-	if (size > cuseqmi_size - off)
-		size = cuseqmi_size - off;
+	/* fixme: don't wait if non-blocking */
+	pthread_mutex_lock(&client->rqlock);
+	if (!client->rq)
+		pthread_cond_wait(&client->ready, &client->rqlock);
+	msg = client->rq;
+	if (msg)
+		client->rq = msg->next;
+	pthread_mutex_unlock(&client->rqlock);
 
-	fuse_reply_buf(req, cuseqmi_buf + off, size);
+	/* fixme:  verify that msg->len <= size */
+	if (msg) {
+		fuse_reply_buf(req, msg->msg, msg->len);
+		free(msg);
+	} else {
+		fuse_reply_err(req, EAGAIN);
+	}
 }
 
-static void cuseqmi_write(fuse_req_t req, const char *buf, size_t size,
-			  off_t off, struct fuse_file_info *fi)
+static void qmuxify(char *buf, int cid, int len)
 {
-	(void)fi;
+	struct qmux *q = (void *)buf;
 
-	fuse_reply_write(req, size);
+	q->tf = 1;
+	q->len = len + qmux_size - 1;
+	q->ctrl = 0;
+	q->service = cid >> 8 & 0xff;
+	q->qmicid = cid & 0xff;
 }
 
+static void cuseqmi_write(fuse_req_t req, const char *buf, size_t size, off_t off, struct fuse_file_info *fi)
+{
+	char *wbuf;
+	int status = 0;
+	struct qclient *client = (void *)fi->fh;
+
+	fprintf(stderr, "%s\n", __func__);
+	if (!client) {
+		DBG("Bad file data\n");
+		status = -EBADF;
+		goto err;
+	}
+	if (client->cid == (__u16)-1) {
+		DBG("Client ID must be set before writing 0x%04X", client->cid);
+		status = -EBADR;
+		goto err;
+	}
+	wbuf = malloc(size + qmux_size);
+	if (!wbuf) {
+		status = -ENOMEM;
+		goto err;
+	}
+	memcpy(wbuf + qmux_size, buf, size);
+	qmuxify(wbuf, client->cid, size);
+
+	/* lock for write */
+	pthread_mutex_lock(&wr_mutex);
+	status = write(fd, wbuf, size + qmux_size);
+	pthread_mutex_unlock(&wr_mutex);
+
+	if (status > qmux_size)
+		status -= qmux_size;
+	else
+		status = -EIO;
+	free(wbuf);
+
+	if (status >= 0)
+		fuse_reply_write(req, status);
+	else
+err:
+		fuse_reply_err(req, -status);
+}
 
 static void cuseqmi_ioctl(fuse_req_t req, int cmd, void *arg,
 			  struct fuse_file_info *fi, unsigned flags,
 			  const void *in_buf, size_t in_bufsz, size_t out_bufsz)
 {
-	(void)fi;
+	struct qclient *client = (void *)fi->fh;
+	int ret = 0;
+	unsigned int vidpid = vid << 16 | pid;
 
-	fprintf(stderr, "%s: here\n", __func__);
+	fprintf(stderr, "%s: cmd=%#010x, arg=%p\n", __func__, cmd, arg);
 
-	if (flags & FUSE_IOCTL_COMPAT) {
+/*	if (flags & FUSE_IOCTL_COMPAT) {
 		fuse_reply_err(req, ENOSYS);
 		return;
 	}
-
+*/
 	switch (cmd) {
 
+	case IOCTL_QMI_GET_SERVICE_FILE:
+		if (client->cid != (__u16)-1) {
+			DBG("Close the current connection before opening a new one\n");
+			fuse_reply_err(req, EBADR);
+                } else {
+			__u8 cid = (long)arg;
+			DBG("Setting up QMI for service %u", cid);
+			ret = alloc_cid(client, cid);
+			fuse_reply_ioctl(req, ret, NULL, 0);
+		}
+		break;
+
+	/* Okay, all aboard the nasty hack express. If we don't have this
+	 * ioctl() (and we just rely on userspace to close() the file
+	 * descriptors), if userspace has any refs left to this fd (like, say, a
+	 * pending read()), then the read might hang around forever. Userspace
+	 * needs a way to cause us to kick people off those waitqueues before
+	 * closing the fd for good.
+	 *
+	 * If this driver used workqueues, the correct approach here would
+	 * instead be to make the file descriptor select()able, and then just
+	 * use select() instead of aio in userspace (thus allowing us to get
+	 * away with one thread total and avoiding the recounting mess
+	 * altogether).
+	 */
+	case IOCTL_QMI_CLOSE:
+		DBG("Tearing down QMI for service %lu", (long)arg);
+		if (client->cid == (__u16)-1) {
+			DBG("no qmi cid");
+			ret = -EBADR;
+			goto err;
+		}
+
+		ret = release_cid(client);
+		fuse_reply_ioctl(req, ret, NULL, 0);
+		break;
+
+	case IOCTL_QMI_GET_DEVICE_VIDPID:
+		DBG("IOCTL_QMI_GET_DEVICE_VIDPID, out_bufsz=%zu", out_bufsz);
+                if (!out_bufsz) {
+                        struct iovec iov = { arg, sizeof(__u32) };
+                        fuse_reply_ioctl_retry(req, NULL, 0, &iov, 1);
+                } else {
+			DBG("copying vid:pid to userspace\n");
+                        fuse_reply_ioctl(req, 0,  &vidpid, sizeof(__u32));
+		}
+		break;
+
+	case IOCTL_QMI_GET_DEVICE_MEID:
+		DBG("IOCTL_QMI_GET_DEVICE_MEID, out_bufsz=%zu", out_bufsz);
+                if (!out_bufsz) {
+                        struct iovec iov = { arg, MEIDLEN };
+                        fuse_reply_ioctl_retry(req, NULL, 0, &iov, 1);
+                } else {
+			DBG("copying MEID to userspace\n");
+                        fuse_reply_ioctl(req, 0, &meid, MEIDLEN);
+		}
+		break;
+
 	default:
+		DBG("unsupported ioctl");
 		fuse_reply_err(req, EINVAL);
 	}
+	return;
+err:
+	fuse_reply_err(req, -ret);
 }
 
 struct cuseqmi_param {
@@ -146,8 +586,100 @@ static int cuseqmi_process_arg(void *data, const char *arg, int key,
 	}
 }
 
+
+
+/* add a copy of the complete QMUX in buf to client's read queue */
+static void add_msg_to_client(struct qclient *client, char *buf, int len)
+{
+	struct qmimsg *new, *p;
+
+	DBG("client=%p", client);
+
+	/* allocate a new message entry */
+	new = malloc(sizeof(struct qmimsg) + len - qmux_size);
+	if (!new)
+		return; /* FIMXE: warn about this */
+
+	new->next = NULL; /* always at the end */
+	new->len = len - qmux_size;
+	memcpy(&new->h, buf, len);
+
+	/* get the client lock */
+	pthread_mutex_lock(&client->rqlock);
+	if (!client->rq)
+		client->rq = new;
+	else {
+		for (p = client->rq; p->next; p = p->next);
+		p->next = new;
+	}
+	pthread_mutex_unlock(&client->rqlock);
+	pthread_cond_signal(&client->ready);
+}
+
+/* allocate a copy of the QMUX in buf for every client that should receive it */
+static void copy_msg_to_clients(char *buf, int len)
+{
+	struct qclient *p;
+	struct qmux *q;
+	int mask = 0, val = 0;
+	__u8 flags;
+
+	DBG("");
+	
+	/* analyze the QMI message first */
+	if (len < sizeof(struct qmux) + 1)
+		return;
+	q = (struct qmux *)buf;
+	flags = buf[qmux_size]; /* the first byte after the QMUX */
+
+	if (q->service == 0) { /* QMI_CTL */
+		if (flags == 0x01) /* QMI_CTL response */
+			mask = 0xffff;
+	} else { /* clients with this service */
+		val = q->service << 8;
+		mask = 0xff << 8;
+		if (q->qmicid != 0xff) { /* only address clients with this cid */
+			val |= q->qmicid;
+			mask |= 0xff;
+		}
+	}
+
+
+	/* cannot accept than anyone modifies the list while we're scanning it */
+	pthread_mutex_lock(&cl_mutex);
+	for (p = clients; p; p= p->next)
+		if ((p->cid & mask) == val)
+			add_msg_to_client(p, buf, len);
+	pthread_mutex_unlock(&cl_mutex);
+}
+
+
+/* ==== reader thread ===== */
+void *readcdcwdm(void *tmp)
+{
+   int n;
+   char *buf = malloc(bufsz);
+
+   printf("Hello World! It's me\n");
+   do {
+	   n = read(fd, buf, bufsz);
+	   printf("%s: read %d bytes\n", __func__, n);
+
+	   /* find matching client(s) and link a copy into the rq */
+	   if (n > 0)
+		   copy_msg_to_clients(buf, n);
+
+   } while (n >= 0);
+   free(buf);
+   perror("reader exiting:");
+   pthread_exit(NULL);
+}
+
+
+
 static const struct cuse_lowlevel_ops cuseqmi_clop = {
 	.open		= cuseqmi_open,
+	.release        = cuseqmi_release,
 	.read		= cuseqmi_read,
 	.write		= cuseqmi_write,
 	.ioctl		= cuseqmi_ioctl,
@@ -160,6 +692,10 @@ int main(int argc, char **argv)
 	char dev_name[128] = "DEVNAME=";
 	const char *dev_info_argv[] = { dev_name };
 	struct cuse_info ci;
+	pthread_t readthread;
+	pthread_attr_t attr;
+	void *status;
+	int rc;
 
 	if (fuse_opt_parse(&args, &param, cuseqmi_opts, cuseqmi_process_arg)) {
 		printf("failed to parse option\n");
@@ -174,6 +710,33 @@ int main(int argc, char **argv)
 		strncat(dev_name, param.dev_name, sizeof(dev_name) - 9);
 	}
 
+	/* open QMI device */
+	fd = open(filename, O_RDWR);
+	if (fd < 0) {
+		perror("Error in open");
+		return -1;
+	}
+
+	/* use the new ioctl to get the message size, falling back to
+	 * static default if it fails
+	 */
+
+
+	/* create reader thread */
+	pthread_attr_init(&attr);
+	pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
+	printf("In main: creating reader thread\n");
+	rc = pthread_create(&readthread, &attr, readcdcwdm, NULL);
+	if (rc) {
+		printf("ERROR; return code from pthread_create() is %d\n", rc);
+		return -1;
+	}
+	pthread_attr_destroy(&attr);
+
+	/* run QMI_CTL get version, serial numbers etc */
+
+
+	/* create qcqmi device */
 	memset(&ci, 0, sizeof(ci));
 	ci.dev_major = param.major;
 	ci.dev_minor = param.minor;
@@ -181,6 +744,18 @@ int main(int argc, char **argv)
 	ci.dev_info_argv = dev_info_argv;
 	ci.flags = CUSE_UNRESTRICTED_IOCTL;
 
-	return cuse_lowlevel_main(args.argc, args.argv, &ci, &cuseqmi_clop,
-				  NULL);
+	rc = cuse_lowlevel_main(args.argc, args.argv, &ci, &cuseqmi_clop,  NULL);
+
+	printf("cuse_lowlevel_main returned %d\n", rc);
+	
+	/* close file and wait for reader to exit */
+	close(fd);
+
+	pthread_cancel(readthread);
+	if (pthread_join(readthread, &status) < 0)
+		perror("pthread_join:");
+	else
+		printf("status=%ld\n", (long)status);
+
+	return rc;
 }