1 files changed, 58 insertions, 11 deletions

diff --git a/arch/arm/mach-ixp4xx/include/mach/qmgr.h b/arch/arm/mach-ixp4xx/include/mach/qmgr.h
index 0cbe6ceb67c5..9e7cad2d54cb 100644
--- a/arch/arm/mach-ixp4xx/include/mach/qmgr.h
+++ b/arch/arm/mach-ixp4xx/include/mach/qmgr.h
@@ -15,7 +15,7 @@
 #define DEBUG_QMGR      0
 
 #define HALF_QUEUES     32
-#define QUEUES          64      /* only 32 lower queues currently supported */
+#define QUEUES          64
 #define MAX_QUEUE_LENGTH 4      /* in dwords */
 
 #define QUEUE_STAT1_EMPTY               1 /* queue status bits */
@@ -110,48 +110,95 @@ static inline u32 qmgr_get_entry(unsigned int queue)
         return val;
 }
 
-static inline int qmgr_get_stat1(unsigned int queue)
+static inline int __qmgr_get_stat1(unsigned int queue)
 {
         extern struct qmgr_regs __iomem *qmgr_regs;
         return (__raw_readl(&qmgr_regs->stat1[queue >> 3])
                 >> ((queue & 7) << 2)) & 0xF;
 }
 
-static inline int qmgr_get_stat2(unsigned int queue)
+static inline int __qmgr_get_stat2(unsigned int queue)
 {
         extern struct qmgr_regs __iomem *qmgr_regs;
+        BUG_ON(queue >= HALF_QUEUES);
         return (__raw_readl(&qmgr_regs->stat2[queue >> 4])
                 >> ((queue & 0xF) << 1)) & 0x3;
 }
 
+/**
+ * qmgr_stat_empty() - checks if a hardware queue is empty
+ * @queue:      queue number
+ *
+ * Returns non-zero value if the queue is empty.
+ */
 static inline int qmgr_stat_empty(unsigned int queue)
 {
-        return !!(qmgr_get_stat1(queue) & QUEUE_STAT1_EMPTY);
+        BUG_ON(queue >= HALF_QUEUES);
+        return __qmgr_get_stat1(queue) & QUEUE_STAT1_EMPTY;
 }
 
-static inline int qmgr_stat_nearly_empty(unsigned int queue)
+/**
+ * qmgr_stat_below_low_watermark() - checks if a queue is below low watermark
+ * @queue:      queue number
+ *
+ * Returns non-zero value if the queue is below low watermark.
+ */
+static inline int qmgr_stat_below_low_watermark(unsigned int queue)
 {
-        return !!(qmgr_get_stat1(queue) & QUEUE_STAT1_NEARLY_EMPTY);
+        extern struct qmgr_regs __iomem *qmgr_regs;
+        if (queue >= HALF_QUEUES)
+                return (__raw_readl(&qmgr_regs->statne_h) >>
+                        (queue - HALF_QUEUES)) & 0x01;
+        return __qmgr_get_stat1(queue) & QUEUE_STAT1_NEARLY_EMPTY;
 }
 
-static inline int qmgr_stat_nearly_full(unsigned int queue)
+/**
+ * qmgr_stat_above_high_watermark() - checks if a queue is above high watermark
+ * @queue:      queue number
+ *
+ * Returns non-zero value if the queue is above high watermark
+ */
+static inline int qmgr_stat_above_high_watermark(unsigned int queue)
 {
-        return !!(qmgr_get_stat1(queue) & QUEUE_STAT1_NEARLY_FULL);
+        BUG_ON(queue >= HALF_QUEUES);
+        return __qmgr_get_stat1(queue) & QUEUE_STAT1_NEARLY_FULL;
 }
 
+/**
+ * qmgr_stat_full() - checks if a hardware queue is full
+ * @queue:      queue number
+ *
+ * Returns non-zero value if the queue is full.
+ */
 static inline int qmgr_stat_full(unsigned int queue)
 {
-        return !!(qmgr_get_stat1(queue) & QUEUE_STAT1_FULL);
+        extern struct qmgr_regs __iomem *qmgr_regs;
+        if (queue >= HALF_QUEUES)
+                return (__raw_readl(&qmgr_regs->statf_h) >>
+                        (queue - HALF_QUEUES)) & 0x01;
+        return __qmgr_get_stat1(queue) & QUEUE_STAT1_FULL;
 }
 
+/**
+ * qmgr_stat_underflow() - checks if a hardware queue experienced underflow
+ * @queue:      queue number
+ *
+ * Returns non-zero value if the queue experienced underflow.
+ */
 static inline int qmgr_stat_underflow(unsigned int queue)
 {
-        return !!(qmgr_get_stat2(queue) & QUEUE_STAT2_UNDERFLOW);
+        return __qmgr_get_stat2(queue) & QUEUE_STAT2_UNDERFLOW;
 }
 
+/**
+ * qmgr_stat_overflow() - checks if a hardware queue experienced overflow
+ * @queue:      queue number
+ *
+ * Returns non-zero value if the queue experienced overflow.
+ */
 static inline int qmgr_stat_overflow(unsigned int queue)
 {
-        return !!(qmgr_get_stat2(queue) & QUEUE_STAT2_OVERFLOW);
+        return __qmgr_get_stat2(queue) & QUEUE_STAT2_OVERFLOW;
 }
 
 #endif