From fbeb94544c21d8ca4ac3f4630089ee956f510280 Mon Sep 17 00:00:00 2001
From: Andreas Steffen <andreas.steffen@strongswan.org>
Date: Thu, 27 Oct 2011 00:37:24 +0200
Subject: [PATCH] extended bio_reader and bio_writer to handle u_int64_t

---
 src/libstrongswan/bio/bio_reader.c | 15 ++++++++++++++
 src/libstrongswan/bio/bio_reader.h |  8 ++++++++
 src/libstrongswan/bio/bio_writer.c | 12 +++++++++++
 src/libstrongswan/bio/bio_writer.h |  7 +++++++
 src/libstrongswan/utils.h          | 42 ++++++++++++++++++++++++++++++++++++++
 5 files changed, 84 insertions(+)

diff --git a/src/libstrongswan/bio/bio_reader.c b/src/libstrongswan/bio/bio_reader.c
index d047887..fce0d1a 100644
--- a/src/libstrongswan/bio/bio_reader.c
+++ b/src/libstrongswan/bio/bio_reader.c
@@ -103,6 +103,20 @@ METHOD(bio_reader_t, read_uint32, bool,
 	return TRUE;
 }
 
+METHOD(bio_reader_t, read_uint64, bool,
+	private_bio_reader_t *this, u_int64_t *res)
+{
+	if (this->buf.len < 8)
+	{
+		DBG1(DBG_LIB, "%d bytes insufficient to parse u_int64 data",
+			 this->buf.len);
+		return FALSE;
+	}
+	*res = untoh64(this->buf.ptr);
+	this->buf = chunk_skip(this->buf, 8);
+	return TRUE;
+}
+
 METHOD(bio_reader_t, read_data, bool,
 	private_bio_reader_t *this, u_int32_t len, chunk_t *res)
 {
@@ -186,6 +200,7 @@ bio_reader_t *bio_reader_create(chunk_t data)
 			.read_uint16 = _read_uint16,
 			.read_uint24 = _read_uint24,
 			.read_uint32 = _read_uint32,
+			.read_uint64 = _read_uint64,
 			.read_data = _read_data,
 			.read_data8 = _read_data8,
 			.read_data16 = _read_data16,
diff --git a/src/libstrongswan/bio/bio_reader.h b/src/libstrongswan/bio/bio_reader.h
index c6eba36..85434a7 100644
--- a/src/libstrongswan/bio/bio_reader.h
+++ b/src/libstrongswan/bio/bio_reader.h
@@ -77,6 +77,14 @@ struct bio_reader_t {
 	bool (*read_uint32)(bio_reader_t *this, u_int32_t *res);
 
 	/**
+	 * Read a 64-bit integer from the buffer, advance.
+	 *
+	 * @param res		pointer to result
+	 * @return			TRUE if integer read successfully
+	 */
+	bool (*read_uint64)(bio_reader_t *this, u_int64_t *res);
+
+	/**
 	 * Read a chunk of len bytes, advance.
 	 *
 	 * @param len		number of bytes to read
diff --git a/src/libstrongswan/bio/bio_writer.c b/src/libstrongswan/bio/bio_writer.c
index a5df5ba..bf373d6 100644
--- a/src/libstrongswan/bio/bio_writer.c
+++ b/src/libstrongswan/bio/bio_writer.c
@@ -97,6 +97,17 @@ METHOD(bio_writer_t, write_uint32, void,
 	this->used += 4;
 }
 
+METHOD(bio_writer_t, write_uint64, void,
+	private_bio_writer_t *this, u_int64_t value)
+{
+	if (this->used + 8 > this->buf.len)
+	{
+		increase(this);
+	}
+	htoun64(this->buf.ptr + this->used, value);
+	this->used += 8;
+}
+
 METHOD(bio_writer_t, write_data, void,
 	private_bio_writer_t *this, chunk_t value)
 {
@@ -214,6 +225,7 @@ bio_writer_t *bio_writer_create(u_int32_t bufsize)
 			.write_uint16 = _write_uint16,
 			.write_uint24 = _write_uint24,
 			.write_uint32 = _write_uint32,
+			.write_uint64 = _write_uint64,
 			.write_data = _write_data,
 			.write_data8 = _write_data8,
 			.write_data16 = _write_data16,
diff --git a/src/libstrongswan/bio/bio_writer.h b/src/libstrongswan/bio/bio_writer.h
index 4789aad..0b50f78 100644
--- a/src/libstrongswan/bio/bio_writer.h
+++ b/src/libstrongswan/bio/bio_writer.h
@@ -59,6 +59,13 @@ struct bio_writer_t {
 	void (*write_uint32)(bio_writer_t *this, u_int32_t value);
 
 	/**
+	 * Append a 64-bit integer to the buffer.
+	 *
+	 * @param value		value to append
+	 */
+	void (*write_uint64)(bio_writer_t *this, u_int64_t value);
+
+	/**
 	 * Append a chunk of data without a length header.
 	 *
 	 * @param value		value to append
diff --git a/src/libstrongswan/utils.h b/src/libstrongswan/utils.h
index 0f06fec..5154a0f 100644
--- a/src/libstrongswan/utils.h
+++ b/src/libstrongswan/utils.h
@@ -483,6 +483,27 @@ static inline void htoun32(void *network, u_int32_t host)
 }
 
 /**
+ * Write a 64-bit host order value in network order to an unaligned address.
+ *
+ * @param host		host order 32-bit value
+ * @param network	unaligned address to write network order value to
+ */
+static inline void htoun64(void *network, u_int64_t host)
+{
+	char *unaligned = (char*)network;
+	u_int32_t high_part, low_part;
+
+	high_part = host >> 32;
+	high_part = htonl(high_part);
+	low_part  = host & 0xFFFFFFFFLL;
+	low_part  = htonl(low_part);
+
+	memcpy(unaligned, &high_part, sizeof(high_part));
+	unaligned += sizeof(high_part);
+	memcpy(unaligned, &low_part, sizeof(low_part));
+}
+
+/**
  * Read a 16-bit value in network order from an unaligned address to host order.
  *
  * @param network	unaligned address to read network order value from
@@ -513,6 +534,27 @@ static inline u_int32_t untoh32(void *network)
 }
 
 /**
+ * Read a 64-bit value in network order from an unaligned address to host order.
+ *
+ * @param network	unaligned address to read network order value from
+ * @return			host order value
+ */
+static inline u_int64_t untoh64(void *network)
+{
+	char *unaligned = (char*)network;
+	u_int32_t high_part, low_part;
+
+	memcpy(&high_part, unaligned, sizeof(high_part));
+	unaligned += sizeof(high_part);
+	memcpy(&low_part, unaligned, sizeof(low_part));
+
+	high_part = ntohl(high_part);
+	low_part  = ntohl(low_part);
+
+	return (((u_int64_t)high_part) << 32) + low_part;
+}
+
+/**
  * Special type to count references
  */
 typedef volatile u_int refcount_t;
-- 
2.7.4