From 838ab97ce3bda9db0b6d5beb98f8c5bc16582be9 Mon Sep 17 00:00:00 2001
From: Michael Brown
Date: Mon, 16 Mar 2015 15:37:39 +0000
Subject: [usb] Add functions for manual device address assignment

Signed-off-by: Michael Brown <mcb30@ipxe.org>
---
 src/drivers/bus/usb.c | 44 ++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 44 insertions(+)

(limited to 'src/drivers')

diff --git a/src/drivers/bus/usb.c b/src/drivers/bus/usb.c
index 57a253320..548aa7b08 100644
--- a/src/drivers/bus/usb.c
+++ b/src/drivers/bus/usb.c
@@ -26,6 +26,7 @@ FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
 #include <stdlib.h>
 #include <stdio.h>
 #include <string.h>
+#include <strings.h>
 #include <unistd.h>
 #include <errno.h>
 #include <assert.h>
@@ -1849,6 +1850,49 @@ void free_usb_bus ( struct usb_bus *bus ) {
 	free ( bus );
 }
 
+/******************************************************************************
+ *
+ * USB address assignment
+ *
+ ******************************************************************************
+ */
+
+/**
+ * Allocate device address
+ *
+ * @v bus		USB bus
+ * @ret address		Device address, or negative error
+ */
+int usb_alloc_address ( struct usb_bus *bus ) {
+	unsigned int address;
+
+	/* Find first free device address */
+	address = ffsll ( ~bus->addresses );
+	if ( ! address )
+		return -ENOENT;
+
+	/* Mark address as used */
+	bus->addresses |= ( 1ULL << ( address - 1 ) );
+
+	return address;
+}
+
+/**
+ * Free device address
+ *
+ * @v bus		USB bus
+ * @v address		Device address
+ */
+void usb_free_address ( struct usb_bus *bus, unsigned int address ) {
+
+	/* Sanity check */
+	assert ( address > 0 );
+	assert ( bus->addresses & ( 1ULL << ( address - 1 ) ) );
+
+	/* Mark address as free */
+	bus->addresses &= ~( 1ULL << ( address - 1 ) );
+}
+
 /******************************************************************************
  *
  * USB bus topology
-- 
cgit v1.2.3-55-g7522