Fixed MTP to work with TWRP

Documentation/connector/Makefile

@@ -0,0 +1,16 @@
+ifneq ($(CONFIG_CONNECTOR),)
+obj-m += cn_test.o
+endif
+
+# List of programs to build
+hostprogs-y := ucon
+
+# Tell kbuild to always build the programs
+always := $(hostprogs-y)
+
+HOSTCFLAGS_ucon.o += -I$(objtree)/usr/include
+
+all: modules
+
+modules clean:
+	$(MAKE) -C ../.. SUBDIRS=$(PWD) $@

Documentation/connector/cn_test.c

@@ -0,0 +1,201 @@
+/*
+ * 	cn_test.c
+ * 
+ * 2004+ Copyright (c) Evgeniy Polyakov <zbr@ioremap.net>
+ * All rights reserved.
+ * 
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#define pr_fmt(fmt) "cn_test: " fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/skbuff.h>
+#include <linux/slab.h>
+#include <linux/timer.h>
+
+#include <linux/connector.h>
+
+static struct cb_id cn_test_id = { CN_NETLINK_USERS + 3, 0x456 };
+static char cn_test_name[] = "cn_test";
+static struct sock *nls;
+static struct timer_list cn_test_timer;
+
+static void cn_test_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
+{
+	pr_info("%s: %lu: idx=%x, val=%x, seq=%u, ack=%u, len=%d: %s.\n",
+	        __func__, jiffies, msg->id.idx, msg->id.val,
+	        msg->seq, msg->ack, msg->len,
+	        msg->len ? (char *)msg->data : "");
+}
+
+/*
+ * Do not remove this function even if no one is using it as
+ * this is an example of how to get notifications about new
+ * connector user registration
+ */
+#if 0
+static int cn_test_want_notify(void)
+{
+	struct cn_ctl_msg *ctl;
+	struct cn_notify_req *req;
+	struct cn_msg *msg = NULL;
+	int size, size0;
+	struct sk_buff *skb;
+	struct nlmsghdr *nlh;
+	u32 group = 1;
+
+	size0 = sizeof(*msg) + sizeof(*ctl) + 3 * sizeof(*req);
+
+	size = NLMSG_SPACE(size0);
+
+	skb = alloc_skb(size, GFP_ATOMIC);
+	if (!skb) {
+		pr_err("failed to allocate new skb with size=%u\n", size);
+		return -ENOMEM;
+	}
+
+	nlh = nlmsg_put(skb, 0, 0x123, NLMSG_DONE, size - sizeof(*nlh), 0);
+	if (!nlh) {
+		kfree_skb(skb);
+		return -EMSGSIZE;
+	}
+
+	msg = nlmsg_data(nlh);
+
+	memset(msg, 0, size0);
+
+	msg->id.idx = -1;
+	msg->id.val = -1;
+	msg->seq = 0x123;
+	msg->ack = 0x345;
+	msg->len = size0 - sizeof(*msg);
+
+	ctl = (struct cn_ctl_msg *)(msg + 1);
+
+	ctl->idx_notify_num = 1;
+	ctl->val_notify_num = 2;
+	ctl->group = group;
+	ctl->len = msg->len - sizeof(*ctl);
+
+	req = (struct cn_notify_req *)(ctl + 1);
+
+	/*
+	 * Idx.
+	 */
+	req->first = cn_test_id.idx;
+	req->range = 10;
+
+	/*
+	 * Val 0.
+	 */
+	req++;
+	req->first = cn_test_id.val;
+	req->range = 10;
+
+	/*
+	 * Val 1.
+	 */
+	req++;
+	req->first = cn_test_id.val + 20;
+	req->range = 10;
+
+	NETLINK_CB(skb).dst_group = ctl->group;
+	//netlink_broadcast(nls, skb, 0, ctl->group, GFP_ATOMIC);
+	netlink_unicast(nls, skb, 0, 0);
+
+	pr_info("request was sent: group=0x%x\n", ctl->group);
+
+	return 0;
+}
+#endif
+
+static u32 cn_test_timer_counter;
+static void cn_test_timer_func(unsigned long __data)
+{
+	struct cn_msg *m;
+	char data[32];
+
+	pr_debug("%s: timer fired with data %lu\n", __func__, __data);
+
+	m = kzalloc(sizeof(*m) + sizeof(data), GFP_ATOMIC);
+	if (m) {
+
+		memcpy(&m->id, &cn_test_id, sizeof(m->id));
+		m->seq = cn_test_timer_counter;
+		m->len = sizeof(data);
+
+		m->len =
+		    scnprintf(data, sizeof(data), "counter = %u",
+			      cn_test_timer_counter) + 1;
+
+		memcpy(m + 1, data, m->len);
+
+		cn_netlink_send(m, 0, 0, GFP_ATOMIC);
+		kfree(m);
+	}
+
+	cn_test_timer_counter++;
+
+	mod_timer(&cn_test_timer, jiffies + msecs_to_jiffies(1000));
+}
+
+static int cn_test_init(void)
+{
+	int err;
+
+	err = cn_add_callback(&cn_test_id, cn_test_name, cn_test_callback);
+	if (err)
+		goto err_out;
+	cn_test_id.val++;
+	err = cn_add_callback(&cn_test_id, cn_test_name, cn_test_callback);
+	if (err) {
+		cn_del_callback(&cn_test_id);
+		goto err_out;
+	}
+
+	setup_timer(&cn_test_timer, cn_test_timer_func, 0);
+	mod_timer(&cn_test_timer, jiffies + msecs_to_jiffies(1000));
+
+	pr_info("initialized with id={%u.%u}\n",
+		cn_test_id.idx, cn_test_id.val);
+
+	return 0;
+
+      err_out:
+	if (nls && nls->sk_socket)
+		sock_release(nls->sk_socket);
+
+	return err;
+}
+
+static void cn_test_fini(void)
+{
+	del_timer_sync(&cn_test_timer);
+	cn_del_callback(&cn_test_id);
+	cn_test_id.val--;
+	cn_del_callback(&cn_test_id);
+	if (nls && nls->sk_socket)
+		sock_release(nls->sk_socket);
+}
+
+module_init(cn_test_init);
+module_exit(cn_test_fini);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Evgeniy Polyakov <zbr@ioremap.net>");
+MODULE_DESCRIPTION("Connector's test module");

Documentation/connector/connector.txt

@@ -0,0 +1,188 @@
+/*****************************************/
+Kernel Connector.
+/*****************************************/
+
+Kernel connector - new netlink based userspace <-> kernel space easy
+to use communication module.
+
+The Connector driver makes it easy to connect various agents using a
+netlink based network.  One must register a callback and an identifier.
+When the driver receives a special netlink message with the appropriate
+identifier, the appropriate callback will be called.
+
+From the userspace point of view it's quite straightforward:
+
+	socket();
+	bind();
+	send();
+	recv();
+
+But if kernelspace wants to use the full power of such connections, the
+driver writer must create special sockets, must know about struct sk_buff
+handling, etc...  The Connector driver allows any kernelspace agents to use
+netlink based networking for inter-process communication in a significantly
+easier way:
+
+int cn_add_callback(struct cb_id *id, char *name, void (*callback) (struct cn_msg *, struct netlink_skb_parms *));
+void cn_netlink_send_multi(struct cn_msg *msg, u16 len, u32 portid, u32 __group, int gfp_mask);
+void cn_netlink_send(struct cn_msg *msg, u32 portid, u32 __group, int gfp_mask);
+
+struct cb_id
+{
+	__u32			idx;
+	__u32			val;
+};
+
+idx and val are unique identifiers which must be registered in the
+connector.h header for in-kernel usage.  void (*callback) (void *) is a
+callback function which will be called when a message with above idx.val
+is received by the connector core.  The argument for that function must
+be dereferenced to struct cn_msg *.
+
+struct cn_msg
+{
+	struct cb_id		id;
+
+	__u32			seq;
+	__u32			ack;
+
+	__u32			len;		/* Length of the following data */
+	__u8			data[0];
+};
+
+/*****************************************/
+Connector interfaces.
+/*****************************************/
+
+int cn_add_callback(struct cb_id *id, char *name, void (*callback) (struct cn_msg *, struct netlink_skb_parms *));
+
+ Registers new callback with connector core.
+
+ struct cb_id *id		- unique connector's user identifier.
+				  It must be registered in connector.h for legal in-kernel users.
+ char *name			- connector's callback symbolic name.
+ void (*callback) (struct cn..)	- connector's callback.
+				  cn_msg and the sender's credentials
+
+
+void cn_del_callback(struct cb_id *id);
+
+ Unregisters new callback with connector core.
+
+ struct cb_id *id		- unique connector's user identifier.
+
+
+int cn_netlink_send_multi(struct cn_msg *msg, u16 len, u32 portid, u32 __groups, int gfp_mask);
+int cn_netlink_send(struct cn_msg *msg, u32 portid, u32 __groups, int gfp_mask);
+
+ Sends message to the specified groups.  It can be safely called from
+ softirq context, but may silently fail under strong memory pressure.
+ If there are no listeners for given group -ESRCH can be returned.
+
+ struct cn_msg *		- message header(with attached data).
+ u16 len			- for *_multi multiple cn_msg messages can be sent
+ u32 port			- destination port.
+ 				  If non-zero the message will be sent to the
+				  given port, which should be set to the
+				  original sender.
+ u32 __group			- destination group.
+				  If port and __group is zero, then appropriate group will
+				  be searched through all registered connector users,
+				  and message will be delivered to the group which was
+				  created for user with the same ID as in msg.
+				  If __group is not zero, then message will be delivered
+				  to the specified group.
+ int gfp_mask			- GFP mask.
+
+ Note: When registering new callback user, connector core assigns
+ netlink group to the user which is equal to its id.idx.
+
+/*****************************************/
+Protocol description.
+/*****************************************/
+
+The current framework offers a transport layer with fixed headers.  The
+recommended protocol which uses such a header is as following:
+
+msg->seq and msg->ack are used to determine message genealogy.  When
+someone sends a message, they use a locally unique sequence and random
+acknowledge number.  The sequence number may be copied into
+nlmsghdr->nlmsg_seq too.
+
+The sequence number is incremented with each message sent.
+
+If you expect a reply to the message, then the sequence number in the
+received message MUST be the same as in the original message, and the
+acknowledge number MUST be the same + 1.
+
+If we receive a message and its sequence number is not equal to one we
+are expecting, then it is a new message.  If we receive a message and
+its sequence number is the same as one we are expecting, but its
+acknowledge is not equal to the sequence number in the original
+message + 1, then it is a new message.
+
+Obviously, the protocol header contains the above id.
+
+The connector allows event notification in the following form: kernel
+driver or userspace process can ask connector to notify it when
+selected ids will be turned on or off (registered or unregistered its
+callback).  It is done by sending a special command to the connector
+driver (it also registers itself with id={-1, -1}).
+
+As example of this usage can be found in the cn_test.c module which
+uses the connector to request notification and to send messages.
+
+/*****************************************/
+Reliability.
+/*****************************************/
+
+Netlink itself is not a reliable protocol.  That means that messages can
+be lost due to memory pressure or process' receiving queue overflowed,
+so caller is warned that it must be prepared.  That is why the struct
+cn_msg [main connector's message header] contains u32 seq and u32 ack
+fields.
+
+/*****************************************/
+Userspace usage.
+/*****************************************/
+
+2.6.14 has a new netlink socket implementation, which by default does not
+allow people to send data to netlink groups other than 1.
+So, if you wish to use a netlink socket (for example using connector)
+with a different group number, the userspace application must subscribe to
+that group first.  It can be achieved by the following pseudocode:
+
+s = socket(PF_NETLINK, SOCK_DGRAM, NETLINK_CONNECTOR);
+
+l_local.nl_family = AF_NETLINK;
+l_local.nl_groups = 12345;
+l_local.nl_pid = 0;
+
+if (bind(s, (struct sockaddr *)&l_local, sizeof(struct sockaddr_nl)) == -1) {
+	perror("bind");
+	close(s);
+	return -1;
+}
+
+{
+	int on = l_local.nl_groups;
+	setsockopt(s, 270, 1, &on, sizeof(on));
+}
+
+Where 270 above is SOL_NETLINK, and 1 is a NETLINK_ADD_MEMBERSHIP socket
+option.  To drop a multicast subscription, one should call the above socket
+option with the NETLINK_DROP_MEMBERSHIP parameter which is defined as 0.
+
+2.6.14 netlink code only allows to select a group which is less or equal to
+the maximum group number, which is used at netlink_kernel_create() time.
+In case of connector it is CN_NETLINK_USERS + 0xf, so if you want to use
+group number 12345, you must increment CN_NETLINK_USERS to that number.
+Additional 0xf numbers are allocated to be used by non-in-kernel users.
+
+Due to this limitation, group 0xffffffff does not work now, so one can
+not use add/remove connector's group notifications, but as far as I know, 
+only cn_test.c test module used it.
+
+Some work in netlink area is still being done, so things can be changed in
+2.6.15 timeframe, if it will happen, documentation will be updated for that
+kernel.

Documentation/connector/ucon.c

@@ -0,0 +1,250 @@
+/*
+ * 	ucon.c
+ *
+ * Copyright (c) 2004+ Evgeniy Polyakov <zbr@ioremap.net>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include <asm/types.h>
+
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/poll.h>
+
+#include <linux/netlink.h>
+#include <linux/rtnetlink.h>
+
+#include <arpa/inet.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <errno.h>
+#include <time.h>
+#include <getopt.h>
+
+#include <linux/connector.h>
+
+#define DEBUG
+#define NETLINK_CONNECTOR 	11
+
+/* Hopefully your userspace connector.h matches this kernel */
+#define CN_TEST_IDX		CN_NETLINK_USERS + 3
+#define CN_TEST_VAL		0x456
+
+#ifdef DEBUG
+#define ulog(f, a...) fprintf(stdout, f, ##a)
+#else
+#define ulog(f, a...) do {} while (0)
+#endif
+
+static int need_exit;
+static __u32 seq;
+
+static int netlink_send(int s, struct cn_msg *msg)
+{
+	struct nlmsghdr *nlh;
+	unsigned int size;
+	int err;
+	char buf[128];
+	struct cn_msg *m;
+
+	size = NLMSG_SPACE(sizeof(struct cn_msg) + msg->len);
+
+	nlh = (struct nlmsghdr *)buf;
+	nlh->nlmsg_seq = seq++;
+	nlh->nlmsg_pid = getpid();
+	nlh->nlmsg_type = NLMSG_DONE;
+	nlh->nlmsg_len = size;
+	nlh->nlmsg_flags = 0;
+
+	m = NLMSG_DATA(nlh);
+#if 0
+	ulog("%s: [%08x.%08x] len=%u, seq=%u, ack=%u.\n",
+	       __func__, msg->id.idx, msg->id.val, msg->len, msg->seq, msg->ack);
+#endif
+	memcpy(m, msg, sizeof(*m) + msg->len);
+
+	err = send(s, nlh, size, 0);
+	if (err == -1)
+		ulog("Failed to send: %s [%d].\n",
+			strerror(errno), errno);
+
+	return err;
+}
+
+static void usage(void)
+{
+	printf(
+		"Usage: ucon [options] [output file]\n"
+		"\n"
+		"\t-h\tthis help screen\n"
+		"\t-s\tsend buffers to the test module\n"
+		"\n"
+		"The default behavior of ucon is to subscribe to the test module\n"
+		"and wait for state messages.  Any ones received are dumped to the\n"
+		"specified output file (or stdout).  The test module is assumed to\n"
+		"have an id of {%u.%u}\n"
+		"\n"
+		"If you get no output, then verify the cn_test module id matches\n"
+		"the expected id above.\n"
+		, CN_TEST_IDX, CN_TEST_VAL
+	);
+}
+
+int main(int argc, char *argv[])
+{
+	int s;
+	char buf[1024];
+	int len;
+	struct nlmsghdr *reply;
+	struct sockaddr_nl l_local;
+	struct cn_msg *data;
+	FILE *out;
+	time_t tm;
+	struct pollfd pfd;
+	bool send_msgs = false;
+
+	while ((s = getopt(argc, argv, "hs")) != -1) {
+		switch (s) {
+		case 's':
+			send_msgs = true;
+			break;
+
+		case 'h':
+			usage();
+			return 0;
+
+		default:
+			/* getopt() outputs an error for us */
+			usage();
+			return 1;
+		}
+	}
+
+	if (argc != optind) {
+		out = fopen(argv[optind], "a+");
+		if (!out) {
+			ulog("Unable to open %s for writing: %s\n",
+				argv[1], strerror(errno));
+			out = stdout;
+		}
+	} else
+		out = stdout;
+
+	memset(buf, 0, sizeof(buf));
+
+	s = socket(PF_NETLINK, SOCK_DGRAM, NETLINK_CONNECTOR);
+	if (s == -1) {
+		perror("socket");
+		return -1;
+	}
+
+	l_local.nl_family = AF_NETLINK;
+	l_local.nl_groups = -1; /* bitmask of requested groups */
+	l_local.nl_pid = 0;
+
+	ulog("subscribing to %u.%u\n", CN_TEST_IDX, CN_TEST_VAL);
+
+	if (bind(s, (struct sockaddr *)&l_local, sizeof(struct sockaddr_nl)) == -1) {
+		perror("bind");
+		close(s);
+		return -1;
+	}
+
+#if 0
+	{
+		int on = 0x57; /* Additional group number */
+		setsockopt(s, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP, &on, sizeof(on));
+	}
+#endif
+	if (send_msgs) {
+		int i, j;
+
+		memset(buf, 0, sizeof(buf));
+
+		data = (struct cn_msg *)buf;
+
+		data->id.idx = CN_TEST_IDX;
+		data->id.val = CN_TEST_VAL;
+		data->seq = seq++;
+		data->ack = 0;
+		data->len = 0;
+
+		for (j=0; j<10; ++j) {
+			for (i=0; i<1000; ++i) {
+				len = netlink_send(s, data);
+			}
+
+			ulog("%d messages have been sent to %08x.%08x.\n", i, data->id.idx, data->id.val);
+		}
+
+		return 0;
+	}
+
+
+	pfd.fd = s;
+
+	while (!need_exit) {
+		pfd.events = POLLIN;
+		pfd.revents = 0;
+		switch (poll(&pfd, 1, -1)) {
+			case 0:
+				need_exit = 1;
+				break;
+			case -1:
+				if (errno != EINTR) {
+					need_exit = 1;
+					break;
+				}
+				continue;
+		}
+		if (need_exit)
+			break;
+
+		memset(buf, 0, sizeof(buf));
+		len = recv(s, buf, sizeof(buf), 0);
+		if (len == -1) {
+			perror("recv buf");
+			close(s);
+			return -1;
+		}
+		reply = (struct nlmsghdr *)buf;
+
+		switch (reply->nlmsg_type) {
+		case NLMSG_ERROR:
+			fprintf(out, "Error message received.\n");
+			fflush(out);
+			break;
+		case NLMSG_DONE:
+			data = (struct cn_msg *)NLMSG_DATA(reply);
+
+			time(&tm);
+			fprintf(out, "%.24s : [%x.%x] [%08u.%08u].\n",
+				ctime(&tm), data->id.idx, data->id.val, data->seq, data->ack);
+			fflush(out);
+			break;
+		default:
+			break;
+		}
+	}
+
+	close(s);
+	return 0;
+}