1 files changed, 92 insertions, 15 deletions

diff --git a/satp/sequence-window.go b/satp/sequence-window.go
index 4c53ee5..c527f4e 100644
--- a/satp/sequence-window.go
+++ b/satp/sequence-window.go
@@ -32,9 +32,29 @@ package satp
 
 import (
 	"errors"
+	"fmt"
 	"sync"
 )
 
+// calculate distance of seq from top and whether it is less than top
+func seqDistance(top, seq uint32) (less bool, distance uint32) {
+	d := int32(seq - top)
+	if d < 0 {
+		return true, uint32(-d)
+	}
+	return false, uint32(d)
+}
+
+// index will only be valid if seq is inside the window
+// aka: top >= seq && (top-seq) <= window_size
+//        (with both conditions compensated for value wrap around)
+func bitSliceIndex(top, seq uint32) int {
+	if top > seq {
+		return int(top/32) - int(seq/32)
+	}
+	return int(top/32) + int(1<<(32-5)) - int(seq/32)
+}
+
 type SequenceWindow struct {
 	size int
 	head uint64
@@ -42,39 +62,96 @@ type SequenceWindow struct {
 	mtx  *sync.RWMutex
 }
 
+func octet2str(octet uint8) (str string) {
+	for i := 0; i < 8; i++ {
+		if (octet & 0x80) == 0 {
+			str += "."
+		} else {
+			str += "x"
+		}
+		octet <<= 1
+	}
+	return
+}
+
+func octet2str4(octets uint32) (str string) {
+	str += octet2str(uint8(octets>>24)) + " "
+	str += octet2str(uint8(octets>>16)) + " "
+	str += octet2str(uint8(octets>>8)) + " "
+	str += octet2str(uint8(octets))
+	return
+}
+
+func (w *SequenceWindow) String() string {
+	w.mtx.RLock()
+	defer w.mtx.RUnlock()
+
+	str := fmt.Sprintf("%d/[%d]{", w.top(), w.size)
+	str += octet2str4(w.bitSlice(0))
+	for i := 1; i <= len(w.body); i++ {
+		str += "  " + octet2str4(w.bitSlice(i))
+	}
+	return str + "}"
+}
+
 func (w *SequenceWindow) Size() int {
 	return w.size
 }
 
+func (w *SequenceWindow) bitSlice(idx int) uint32 {
+	if idx <= 0 {
+		// return uint32(atomic.LoadUint64(&w.head))
+		return uint32(w.head)
+	}
+	// return atomic.LoadUint32(&w.body[idx-1])
+	return w.body[idx-1]
+}
+
+func (w *SequenceWindow) top() uint32 {
+	// return uint32(atomic.LoadUint64(&w.head) >> 32)
+	return uint32(w.head >> 32)
+}
+
 func (w *SequenceWindow) Top() uint32 {
 	w.mtx.RLock()
 	defer w.mtx.RUnlock()
-	// return uint32(atomic.LoadUint64(&w.head) >> 32)
-	return uint32(w.head >> 32)
+	return w.top()
 }
 
-func (w *SequenceWindow) Check(sequenceNumber uint32) (bool, error) {
+func (w *SequenceWindow) Check(sequenceNumber uint32) bool {
 	if w.size <= 0 {
-		return false, nil
+		return true
 	}
 	w.mtx.RLock()
 	defer w.mtx.RUnlock()
-	// TODO: implement this
-	return false, nil
-}
 
-func (w *SequenceWindow) CheckAndSet(sequenceNumber uint32) (bool, error) {
-	if w.size <= 0 {
-		return false, nil
+	top := w.top()
+	lt, d := seqDistance(top, sequenceNumber)
+	if lt {
+		if d > uint32(w.size) {
+			return false
+		}
+		bits := w.bitSlice(bitSliceIndex(top, sequenceNumber))
+		return ((bits >> (sequenceNumber % 32)) & 1) != 1
 	}
-	w.mtx.Lock()
-	defer w.mtx.Unlock()
-	// TODO: implement this
-	return false, nil
+	return true
 }
 
+// func (w *SequenceWindow) advanceTop(sequenceNumber uint32) {
+// }
+
+// func (w *SequenceWindow) CheckAndSet(sequenceNumber uint32) bool {
+// 	if w.size <= 0 {
+// 		return true
+// 	}
+// 	w.mtx.Lock()
+// 	defer w.mtx.Unlock()
+// 	// TODO: implement this
+// 	return false
+// }
+
 func NewSequenceWindow(size int, top uint32) (w *SequenceWindow, err error) {
-	if size < 0 {
+	if size < 0 || size > int(^uint32(0)/4) {
 		return nil, errors.New("invalid sequence window size")
 	}
 	w = &SequenceWindow{size: size, head: uint64(top) << 32, mtx: &sync.RWMutex{}}