From 052ef6dc5a4ed2444aeda90af7b97c0a92e98e54 Mon Sep 17 00:00:00 2001
From: Michael Brown <mcb30@etherboot.org>
Date: Mon, 4 Dec 2006 22:06:54 +0000
Subject: [PATCH] Added nvs_write().  This shares a lot of code in common with
 nvs_read(); this can probably be optimised down quite a lot.

---
 src/drivers/nvs/nvs.c | 43 +++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 43 insertions(+)

diff --git a/src/drivers/nvs/nvs.c b/src/drivers/nvs/nvs.c
index 3f22a0ee..a566cdfd 100644
--- a/src/drivers/nvs/nvs.c
+++ b/src/drivers/nvs/nvs.c
@@ -68,3 +68,46 @@ int nvs_read ( struct nvs_device *nvs, unsigned int address,
 
 	return 0;
 }
+
+/**
+ * Write to non-volatile storage device
+ *
+ * @v nvs		NVS device
+ * @v address		Address to which to write
+ * @v data		Data buffer
+ * @v len		Length of data buffer
+ * @ret rc		Return status code
+ */
+int nvs_write ( struct nvs_device *nvs, unsigned int address,
+		const void *data, size_t len ) {
+	size_t frag_len;
+	int rc;
+
+	/* We don't even attempt to handle buffer lengths that aren't
+	 * an integral number of words.
+	 */
+	assert ( ( len & ( ( 1 << nvs->word_len_log2 ) - 1 ) ) == 0 );
+
+	while ( len ) {
+
+		/* Calculate space remaining up to next block boundary */
+		frag_len = ( ( nvs->block_size -
+			       ( address & ( nvs->block_size - 1 ) ) )
+			     << nvs->word_len_log2 );
+
+		/* Limit to space remaining in buffer */
+		if ( frag_len > len )
+			frag_len = len;
+
+		/* Read this portion of the buffer from the device */
+		if ( ( rc = nvs->write ( nvs, address, data, frag_len ) ) != 0)
+			return rc;
+
+		/* Update parameters */
+		data += frag_len;
+		address += ( frag_len >> nvs->word_len_log2 );
+		len -= frag_len;
+	}
+
+	return 0;
+}