add functions for allocating and releasing bandwidth and channels

git-svn-id: svn://svn.linux1394.org/libraw1394/trunk@154 53a565d1-3bb7-0310-b661-cf11e63c67ab

3 files changed, 164 insertions, 1 deletions

diff --git a/src/main.c b/src/main.c
index 235a3a8..73e55e6 100644
--- a/src/main.c
+++ b/src/main.c
@@ -27,8 +27,10 @@
 #include <string.h>
 #include <fcntl.h>
 #include <unistd.h>
+#include <netinet/in.h>
 
 #include "raw1394.h"
+#include "csr.h"
 #include "kernel-raw1394.h"
 #include "raw1394_private.h"
 
@@ -367,3 +369,111 @@ int raw1394_get_config_rom(raw1394handle_t handle, quadlet_t *buffer,
 
         return status;
 }
+
+int raw1394_bandwidth_modify (raw1394handle_t handle, unsigned int bandwidth,
+	enum raw1394_modify_mode mode)
+{
+        quadlet_t buffer, compare, swap, new;
+        int retry = 3;
+        int result;
+        
+        if (bandwidth == 0)
+                return 0;
+        
+        /* Reading current bandwidth usage from IRM. */
+        result = raw1394_read (handle, raw1394_get_irm_id (handle),
+                CSR_REGISTER_BASE + CSR_BANDWIDTH_AVAILABLE,
+                sizeof (quadlet_t), &buffer);
+        if (result < 0)
+                return -1;
+  
+        buffer = ntohl (buffer);
+        compare = buffer;
+  
+        while (retry > 0) {
+                if (mode == RAW1394_MODIFY_ALLOC ) {
+                        if (compare < bandwidth) {
+                                return -1;
+                        }
+  
+                        swap = compare - bandwidth;
+                }
+                else {
+                        swap = compare + bandwidth;
+  
+                        if( swap > MAXIMUM_BANDWIDTH ) {
+                                swap = MAXIMUM_BANDWIDTH;
+                        }
+                }
+                
+                result = raw1394_lock (handle, raw1394_get_irm_id (handle),
+                                        CSR_REGISTER_BASE + CSR_BANDWIDTH_AVAILABLE, 
+                                        RAW1394_EXTCODE_COMPARE_SWAP, ntohl(swap), ntohl(compare),
+                                        &new);
+                if (result < 0)
+                        return -1;
+                
+                new = ntohl (new);
+                
+                if (new != compare) {
+                        compare = new;
+                        retry--;
+                        if ( retry == 0 )
+                                return -1;
+                }
+                else {
+                        /* Success. */
+                        retry = 0;
+                        return 0;
+                }
+        }
+  
+        return 0;
+}
+
+int raw1394_channel_modify (raw1394handle_t handle, unsigned int channel,
+	enum raw1394_modify_mode mode)
+{
+        quadlet_t buffer;
+        int result;
+        nodeaddr_t addr = CSR_REGISTER_BASE;
+        unsigned int c = channel;
+        quadlet_t compare, swap = 0, new;
+        
+        if (c > 31 && c < 64) {
+                addr += CSR_CHANNELS_AVAILABLE_LO;
+                c -= 32;
+        } else if (c < 64)
+                addr += CSR_CHANNELS_AVAILABLE_HI;
+        else
+                return -1;
+        c = 31 - c;
+  
+        result = raw1394_read (handle, raw1394_get_irm_id (handle), addr, 
+                sizeof (quadlet_t), &buffer);
+        if (result < 0)
+                return -1;
+        
+        buffer = ntohl (buffer);
+  
+        if ( mode == RAW1394_MODIFY_ALLOC ) {
+                if( (buffer & (1 << c)) == 0 )
+                        return -1;
+                swap = htonl (buffer & ~(1 << c));
+        }
+        else if ( mode == RAW1394_MODIFY_FREE ) {
+                if ( (buffer & (1 << c)) != 0 )
+                        return -1;
+                swap = htonl (buffer | (1 << c));
+        }
+  
+        compare = htonl (buffer);
+  
+        result = raw1394_lock (handle, raw1394_get_irm_id (handle), addr,
+                                RAW1394_EXTCODE_COMPARE_SWAP, swap, compare, &new);
+        if ( (result < 0) || (new != compare) )
+                return -1;
+  
+        return 0;
+}
+
diff --git a/src/raw1394.h b/src/raw1394.h
index ee6ad74..5f19e20 100644
--- a/src/raw1394.h
+++ b/src/raw1394.h
@@ -12,6 +12,8 @@
 #ifndef _LIBRAW1394_RAW1394_H
 #define _LIBRAW1394_RAW1394_H
 
+#include <sys/types.h>
+
 #define RAW1394_ARM_READ  1
 #define RAW1394_ARM_WRITE 2
 #define RAW1394_ARM_LOCK  4
@@ -23,7 +25,21 @@
 #define RAW1394_NOTIFY_OFF 0
 #define RAW1394_NOTIFY_ON  1
 
-#include <sys/types.h>
+/* extended transaction codes (lock-request-response) */
+#define RAW1394_EXTCODE_MASK_SWAP        0x1
+#define RAW1394_EXTCODE_COMPARE_SWAP     0x2
+#define RAW1394_EXTCODE_FETCH_ADD        0x3
+#define RAW1394_EXTCODE_LITTLE_ADD       0x4
+#define RAW1394_EXTCODE_BOUNDED_ADD      0x5
+#define RAW1394_EXTCODE_WRAP_ADD         0x6
+
+/* response codes */
+#define RAW1394_RCODE_COMPLETE           0x0
+#define RAW1394_RCODE_CONFLICT_ERROR     0x4
+#define RAW1394_RCODE_DATA_ERROR         0x5
+#define RAW1394_RCODE_TYPE_ERROR         0x6
+#define RAW1394_RCODE_ADDRESS_ERROR      0x7
+
 typedef u_int8_t  byte_t;
 typedef u_int32_t quadlet_t;
 typedef u_int64_t octlet_t;
@@ -92,6 +108,12 @@ enum raw1394_iso_disposition {
 	RAW1394_ISO_STOP_NOSYNC = 4,
 };
 
+enum raw1394_modify_mode {
+	RAW1394_MODIFY_ALLOC,
+	RAW1394_MODIFY_FREE
+};
+
+
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -1124,6 +1146,36 @@ int raw1394_update_config_rom(raw1394handle_t handle, const quadlet_t
 int raw1394_get_config_rom(raw1394handle_t handle, quadlet_t *buffer,
         size_t buffersize, size_t *rom_size, unsigned char *rom_version);
 
+/**
+ * raw1394_bandwidth_modify - allocate or release bandwidth
+ * @handle: a libraw1394 handle
+ * @bandwidth: IEEE 1394 Bandwidth Alloction Units
+ * @mode: whether to allocate or free
+ *
+ * Communicates with the isochronous resource manager.
+ *
+ * Return:
+ * -1 for failure, 0 for success
+ **/
+int 
+raw1394_bandwidth_modify (raw1394handle_t handle, unsigned int bandwidth, 
+	enum raw1394_modify_mode mode);
+
+/**
+ * raw1394_bandwidth_modify - allocate or release isochronous channel
+ * @handle: a libraw1394 handle
+ * @channel: isochronous channel
+ * @mode: whether to allocate or free
+ *
+ * Communicates with the isochronous resource manager.
+ *
+ * Return:
+ * -1 for failure, 0 for success
+ **/
+int 
+raw1394_channel_modify (raw1394handle_t handle, unsigned int channel, 
+	enum raw1394_modify_mode mode);
+
 
 /**
  * iso_handler_t - DEPRECATED
diff --git a/src/raw1394_private.h b/src/raw1394_private.h
index 2a72dda..27683f8 100644
--- a/src/raw1394_private.h
+++ b/src/raw1394_private.h
@@ -4,6 +4,7 @@
 
 #define HBUF_SIZE      8192
 #define ARM_REC_LENGTH 4096 
+#define MAXIMUM_BANDWIDTH 4915
 
 struct raw1394_handle {
         int fd;