SP encoder and decoder added (not used yet)

Signed-off-by: Martin Sustrik <sustrik@250bpm.com>

include/net/sp_decoder.h

@@ -0,0 +1,37 @@
+/*
+ * SP: An implementation of SP sockets.
+ *
+ * Copyright 2010 VMware, Inc.
+ */
+
+#ifndef __LINUX_NET_SP_DECODER_H
+#define __LINUX_NET_SP_DECODER_H
+
+#include <linux/types.h>
+
+struct sp_decoder
+{
+	/* Function to get more data to decode */
+	int (*read)(struct sp_decoder *dcdr, void *data, int size);
+
+	/* State of the finite state machine */
+	void (*next)(struct sp_decoder*);
+	void *read_pos;
+	int read_size;
+
+	/* The message being read */
+	void *msg_data;
+	int msg_size;
+	int msg_ready;
+
+	/* Temporary buffer */
+	u8 buff[8];
+};
+
+void sp_decoder_init(struct sp_decoder *dcdr,
+	int (*read)(struct sp_decoder*, void*, int));
+void sp_decoder_destroy(struct sp_decoder *dcdr);
+int sp_decoder_get_message(struct sp_decoder *dcdr, int maxsize,
+	void **data, int *size);
+
+#endif

include/net/sp_encoder.h

@@ -0,0 +1,39 @@
+/*
+ * SP: An implementation of SP sockets.
+ *
+ * Copyright 2010 VMware, Inc.
+ */
+
+#ifndef __LINUX_NET_SP_ENCODER_H
+#define __LINUX_NET_SP_ENCODER_H
+
+#include <linux/types.h>
+#include <linux/socket.h>
+
+struct sp_encoder
+{
+	/* Function to send the encoded data */
+	int (*write)(struct sp_encoder *ecdr, void *data, int size);
+
+	/* State of the finite state machine */
+	void (*next)(struct sp_encoder *ecdr);
+	void *write_pos;
+	int write_size;
+
+	/* The message being sent */
+	void *msg_data;
+	int msg_size;
+	int msg_sent;
+
+	/* Temporary buffer */
+	u8 buff[10];
+};
+
+void sp_encoder_init(struct sp_encoder *ecdr,
+	int (*write)(struct sp_encoder*, void*, int));
+void sp_encoder_destroy(struct sp_encoder *ecdr);
+int sp_encoder_put_message(struct sp_encoder *ecdr, struct msghdr *hdr,
+	int len);
+void sp_encoder_flush(struct sp_encoder *ecdr);
+
+#endif

net/sp/Makefile

@@ -4,4 +4,4 @@
 
 obj-$(CONFIG_SP)	+= sp.o
 
-sp-y			:= af_sp.o
+sp-y			:= af_sp.o sp_encoder.o sp_decoder.o

net/sp/sp_decoder.c

@@ -0,0 +1,149 @@
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <net/sp_decoder.h>
+
+static inline u64 sp_read_u64 (u8 *buff);
+static void sp_decoder_alloc_message(struct sp_decoder *dcdr, int size);
+
+/* State machine actions */
+static void sp_decoder_idle(struct sp_decoder *dcdr);
+static void sp_decoder_one_byte_size_ready(struct sp_decoder *dcdr);
+static void sp_decoder_eight_byte_size_ready(struct sp_decoder *dcdr);
+static void sp_decoder_flags_ready(struct sp_decoder *dcdr);
+static void sp_decoder_data_ready(struct sp_decoder *dcdr);
+
+/*
+ * Reads 64-bit unsigned integer from buffer in network byte order
+ */
+static inline u64 sp_read_u64 (u8 *buff)
+{
+	return
+		(((u64) buff[0]) << 56) |
+		(((u64) buff[1]) << 48) |
+		(((u64) buff[2]) << 40) |
+		(((u64) buff[3]) << 32) |
+		(((u64) buff[4]) << 24) |
+		(((u64) buff[5]) << 16) |
+		(((u64) buff[6]) << 8) |
+		((u64) buff[7]);
+}
+
+/*
+ * Initialise the SP decoder
+ */
+void sp_decoder_init(struct sp_decoder *dcdr,
+	int (*read)(struct sp_decoder*, void*, int))
+{
+	dcdr->read = read;
+	dcdr->next = sp_decoder_idle;
+	dcdr->read_pos = NULL;
+	dcdr->read_size = 0;
+	dcdr->msg_data = NULL;
+	dcdr->msg_size = 0;
+	dcdr->msg_ready = 0;
+}
+
+/*
+ * Uninitialise the SP decoder
+ */
+void sp_decoder_destroy(struct sp_decoder *dcdr)
+{
+	if (dcdr->msg_data)
+		kfree(dcdr->msg_data);
+}
+
+int sp_decoder_get_message(struct sp_decoder *dcdr, int maxsize,
+	void **data, int *size)
+{
+	int n;
+
+	while(1) {
+
+		/* If there is a message available return it */
+		if(dcdr->msg_ready) {
+
+			if(dcdr->msg_size > maxsize)
+				return -EMSGSIZE;
+
+			*data = dcdr->msg_data;
+			*size = dcdr->msg_size;
+			dcdr->msg_data = NULL;
+			dcdr->msg_size = 0;
+			dcdr->msg_ready = 0;
+			return 0;
+		}
+
+		/* If there is no more data to read invoke the state machine */
+		if(dcdr->read_size == 0)
+			dcdr->next(dcdr);
+
+		/* Try to read as much data as required by the state machine */
+		n = dcdr->read(dcdr, dcdr->read_pos, dcdr->read_size);
+		dcdr->read_pos += n;
+		dcdr->read_size -= n;
+
+		/* If not all data requested was read we have to wait */
+		if (dcdr->read_size)
+			return -EAGAIN;
+	}
+}
+
+/*
+ * Allocates a new message of a specified size
+ */
+static void sp_decoder_alloc_message(struct sp_decoder *dcdr, int size)
+{
+	dcdr->msg_data = kmalloc(size, GFP_KERNEL);
+	BUG_ON(!dcdr->msg_data);
+	dcdr->msg_size = size;
+
+	dcdr->read_pos = dcdr->buff;
+	dcdr->read_size = 1;
+	dcdr->next = sp_decoder_flags_ready;
+}
+
+static void sp_decoder_idle(struct sp_decoder *dcdr)
+{
+	dcdr->read_pos = dcdr->buff;
+	dcdr->read_size = 1;
+	dcdr->next = sp_decoder_one_byte_size_ready;
+}
+
+static void sp_decoder_one_byte_size_ready(struct sp_decoder *dcdr)
+{
+	u8 size = dcdr->buff[0];
+
+	if(size == 0xff) {
+		dcdr->read_pos = dcdr->buff;
+		dcdr->read_size = 8;
+		dcdr->next = sp_decoder_eight_byte_size_ready;
+		return;
+	}
+
+	sp_decoder_alloc_message(dcdr, size);
+}
+
+static void sp_decoder_eight_byte_size_ready(struct sp_decoder *dcdr)
+{
+	int size = (int) sp_read_u64(dcdr->buff);
+	sp_decoder_alloc_message(dcdr, size);
+}
+
+static void sp_decoder_flags_ready(struct sp_decoder *dcdr)
+{
+	/* Ignore the flags for now and continue on */
+	dcdr->read_pos = dcdr->msg_data;
+	dcdr->read_size = dcdr->msg_size;
+	dcdr->next = sp_decoder_data_ready;
+}
+
+static void sp_decoder_data_ready(struct sp_decoder *dcdr)
+{
+	dcdr->msg_ready = 1;
+
+	dcdr->read_pos = NULL;
+	dcdr->read_size = 0;
+	dcdr->next = sp_decoder_idle;
+}

net/sp/sp_encoder.c

@@ -0,0 +1,143 @@
+/*
+ * SP: An implementation of SP sockets.
+ *
+ * Copyright 2010 VMware, Inc.
+ */
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <net/sp_encoder.h>
+
+/* State machine actions */
+static void sp_encoder_idle(struct sp_encoder *ecdr);
+static void sp_encoder_size_and_flags_ready(struct sp_encoder *ecdr);
+static void sp_encoder_data_ready(struct sp_encoder *ecdr);
+
+/*
+ * Writes 64-bit unsigned integer to buffer in network byte order
+ */
+static inline void sp_write_u64 (u8 *buff, u64 value)
+{
+	buff[0] = (u8) (((value) >> 56) & 0xff);
+	buff[1] = (u8) (((value) >> 48) & 0xff);
+	buff[2] = (u8) (((value) >> 40) & 0xff);
+	buff[3] = (u8) (((value) >> 32) & 0xff);
+	buff[4] = (u8) (((value) >> 24) & 0xff);
+	buff[5] = (u8) (((value) >> 16) & 0xff);
+	buff[6] = (u8) (((value) >> 8) & 0xff);
+	buff[7] = (u8) (value & 0xff);
+}
+
+void sp_encoder_init(struct sp_encoder *ecdr,
+	int (*write)(struct sp_encoder*, void*, int))
+{
+	ecdr->write = write;
+	ecdr->next = sp_encoder_idle;
+	ecdr->write_pos = NULL;
+	ecdr->write_size = 0;
+	ecdr->msg_data = NULL;
+	ecdr->msg_size = 0;
+	ecdr->msg_sent = 1;
+}
+
+void sp_encoder_destroy(struct sp_encoder *ecdr)
+{
+	if(ecdr->msg_data)
+		kfree(ecdr->msg_data);
+}
+
+int sp_encoder_put_message(struct sp_encoder *ecdr, struct msghdr *msg, int len)
+{
+	int rc = 0;
+
+	/* If there's still message being sent return error */
+	if(!ecdr->msg_sent)
+		return -EAGAIN;
+
+	/* Create a buffer from the message */
+	ecdr->msg_data = kmalloc(len, GFP_KERNEL);
+	if (!ecdr->msg_data) {
+		rc = -ENOMEM;
+		goto out;
+	}
+
+	/* Copy the message to the buffer */
+	rc = memcpy_fromiovec(ecdr->msg_data, msg->msg_iov, len);
+	if (rc < 0) 
+		goto out_dealloc;
+	ecdr->msg_size = len;
+	ecdr->msg_sent = 0;
+
+	/* Try to flush the message to the network */
+	ecdr->next = sp_encoder_idle;
+	sp_encoder_flush(ecdr);
+
+	goto out;
+
+out_dealloc:
+	kfree(ecdr->msg_data);
+	ecdr->msg_data = NULL;
+out:
+	return rc;
+}
+
+void sp_encoder_flush(struct sp_encoder *ecdr)
+{
+	int n;
+
+	for(;;) {
+
+		/* If there's no data available exit */
+		if (!ecdr->write_size)
+			break;
+
+		/* Try to send the remaining data */
+		n = ecdr->write(ecdr, ecdr->write_pos, ecdr->write_size);
+		ecdr->write_pos += n;
+		ecdr->write_size -= n;
+
+		/* If more data cannot be sent exit */
+		if(ecdr->write_size)
+			break;
+
+		/* If there's no more data available run the state machine */
+		ecdr->next(ecdr);
+	}
+}
+
+static void sp_encoder_idle(struct sp_encoder *ecdr)
+{
+	if (ecdr->msg_size < 0xff) {
+		ecdr->buff[0] = (u8)ecdr->msg_size;
+		ecdr->buff[1] = 0;
+		ecdr->write_pos = ecdr->buff;
+		ecdr->write_size = 2;
+		ecdr->next = sp_encoder_size_and_flags_ready;
+		return;
+	}
+
+	ecdr->buff[0] = 0xff;
+	sp_write_u64(ecdr->buff + 1, ecdr->msg_size);
+	ecdr->buff[9] = 0;
+	ecdr->write_pos = ecdr->buff;
+	ecdr->write_size = 10;
+	ecdr->next = sp_encoder_size_and_flags_ready;
+}
+
+static void sp_encoder_size_and_flags_ready(struct sp_encoder *ecdr)
+{
+	ecdr->write_pos = ecdr->msg_data;
+	ecdr->write_size = ecdr->msg_size;
+	ecdr->next = sp_encoder_data_ready;
+}
+
+static void sp_encoder_data_ready(struct sp_encoder *ecdr)
+{
+	if (ecdr->msg_data)
+		kfree(ecdr->msg_data);
+
+	ecdr->write_pos = NULL;
+	ecdr->write_size = 0;
+	ecdr->next = sp_encoder_idle;
+}