handle stun binding message

net/server/internal/sturn/attribute.go

@@ -1,17 +1,174 @@
 package sturn
 
 import (
-//  "errors"
-//  "strings"
-//  "strconv"
-//  "fmt"
+  "errors"
+  "strings"
+  "strconv"
+  "fmt"
 )
 
-func readAttribute(buf []byte, pos int, length int) (error, *SturnAttribute, int) {
+func readAttribute(buf []byte, pos int) (error, *SturnAttribute, int) {
+
+  if len(buf) - pos < 4 {
+    return errors.New("invalid attribute length"), nil, 0
+  }
+  atrType := getAttributeType(buf[pos + 0], buf[pos + 1])
+  atrLength := int(buf[pos + 2]) * 256 + int(buf[pos + 3])
+  padLength := ((atrLength + 3) >> 2) << 2
+  if len(buf) - pos < 4 + padLength {
+    return errors.New("invalid attribute buffer"), nil, 0
+  }
+
+  var intValue int32
+  var strValue string
+  if atrType == ATRRequestedTransport {
+    if buf[pos + 5] != 0x00 || buf[pos + 6] != 0x00 || buf[pos + 7] != 0x00 {
+      return errors.New("invalid attribute"), nil, 0
+    }
+    intValue = int32(buf[pos + 4])
+  } else if atrType == ATRLifetime {
+    intValue = 256 * (256 * (256 * int32(buf[pos + 4]) + int32(buf[pos + 5])) + int32(buf[pos + 6])) + int32(buf[pos + 7]);
+  } else if atrType == ATRNonce {
+    strValue = string(buf[pos + 4:pos + 4+atrLength]);
+  } else if atrType == ATRUsername {
+    strValue = string(buf[pos + 4:pos + 4+atrLength]);
+  } else if atrType == ATRRealm {
+    strValue = string(buf[pos + 4:pos + 4+atrLength]);
+  } else if atrType == ATRMessageIntegrity {
+    fmt.Println("HANDLE: ATRMessageIntegrity");
+  } else if atrType == ATRMessageIntegritySha256 {
+    fmt.Println("HANDLE: ATRMessageIntegritySha256");
+  } else if atrType == ATRFingerprint {
+    fmt.Println("HANDLE: ATRFingerprint");
+  } else {
+    fmt.Println("UNKNOWN ATTRIBUTE");
+  }
+
+  return nil, &SturnAttribute{
+    atrType: atrType,
+    intValue: intValue,
+    strValue: strValue,
+  }, 4 + padLength;
+
   return nil, nil, 0
 }
 
-func writeAttribute(attribute *SturnAttribute, buf []byte, pos int, length int) (error, int) {
-  return nil, 0
-}
+func writeAttribute(attribute *SturnAttribute, buf []byte, pos int) (error, int) {
 
+  if len(buf) - pos < 4 {
+    return errors.New("invalid buffer size"), 0
+  }
+
+  if attribute.atrType == ATRXorMappedAddress {
+    if len(buf) - pos < 12 {
+      return errors.New("invalid buffer size"), 0
+    }
+    ip := 0
+    parts := strings.Split(attribute.strValue, ".");
+    for i := 0; i < 4; i++ {
+      val, _ := strconv.Atoi(parts[i]);
+      ip = (ip * 256) + val;
+    }
+    buf[pos + 1], buf[pos + 0] = setAttributeType(ATRXorMappedAddress);
+    buf[pos + 2] = 0x00
+    buf[pos + 3] = 0x08
+    buf[pos + 4] = 0x00
+    buf[pos + 5] = FAMIPv4
+    buf[pos + 6] = byte((attribute.intValue >> 8) % 256) ^ 0x21
+    buf[pos + 7] = byte((attribute.intValue) % 256) ^ 0x12
+    buf[pos + 8] = byte((ip >> 24) % 256) ^ 0x21
+    buf[pos + 9] = byte((ip >> 16) % 256) ^ 0x12
+    buf[pos + 10] = byte((ip >> 8) % 256) ^ 0xA4
+    buf[pos + 11] = byte(ip % 256) ^ 0x42
+    return nil, 12
+  } else if attribute.atrType == ATRXorRelayedAddress {
+    if len(buf) - pos < 12 {
+      return errors.New("invalid buffer size"), 0
+    }
+    ip := 0
+    parts := strings.Split(attribute.strValue, ".");
+    for i := 0; i < 4; i++ {
+      val, _ := strconv.Atoi(parts[i]);
+      ip = (ip * 256) + val;
+    }
+    buf[pos + 1], buf[pos + 0] = setAttributeType(ATRXorRelayedAddress);
+    buf[pos + 2] = 0x00
+    buf[pos + 3] = 0x08
+    buf[pos + 4] = 0x00
+    buf[pos + 5] = FAMIPv4
+    buf[pos + 6] = byte((attribute.intValue >> 8) % 256) ^ 0x21
+    buf[pos + 7] = byte(attribute.intValue % 256) ^ 0x12
+    buf[pos + 8] = byte((ip >> 24) % 256) ^ 0x21
+    buf[pos + 9] = byte((ip >> 16) % 256) ^ 0x12
+    buf[pos + 10] = byte((ip >> 8) % 256) ^ 0xA4
+    buf[pos + 11] = byte(ip % 256) ^ 0x42
+    return nil, 12
+  } else if attribute.atrType == ATRLifetime {
+    if len(buf) - pos < 8 {
+      return errors.New("invalid buffer size"), 0
+    }
+    buf[pos + 1], buf[pos + 0] = setAttributeType(ATRLifetime);
+    buf[pos + 2] = 0x00
+    buf[pos + 3] = 0x04
+    buf[pos + 4] = byte((attribute.intValue >> 24) % 256);
+    buf[pos + 5] = byte((attribute.intValue >> 16) % 256);
+    buf[pos + 6] = byte((attribute.intValue >> 8) % 256);
+    buf[pos + 7] = byte(attribute.intValue % 256);
+    return nil, 8
+  } else if attribute.atrType == ATRNonce {
+    raw := []byte(attribute.strValue)
+    rawLen := len(raw);
+    paddedLen := ((len(raw) + 3) >> 2) << 2
+    if paddedLen >= 256 {
+      return errors.New("invalid attribute size"), 0
+    }
+    if len(buf) - pos < 4 + paddedLen {
+      return errors.New("invalid buffer size"), 0
+    }
+    buf[pos + 1], buf[pos + 0] = setAttributeType(ATRNonce);
+    buf[pos + 2] = 0x00
+    buf[pos + 3] = byte(rawLen)
+    for i := 0; i < len(raw); i++ {
+      buf[pos + 4 + i] = raw[i];
+    }
+    for i := len(raw); i < paddedLen; i++ {
+      buf[pos + 4 + i] = 0x00;
+    }
+    return nil, 4 + paddedLen
+  } else if attribute.atrType == ATRRealm {
+    raw := []byte(attribute.strValue)
+    rawLen := len(raw);
+    paddedLen := ((len(raw) + 3) >> 2) << 2
+    if paddedLen >= 256 {
+      return errors.New("invalid attribute size"), 0
+    }
+    if len(buf) - pos < 4 + paddedLen {
+      return errors.New("invalid buffer size"), 0
+    }
+    buf[pos + 1], buf[pos + 0] = setAttributeType(ATRRealm);
+    buf[pos + 2] = 0x00
+    buf[pos + 3] = byte(rawLen);
+    for i := 0; i < len(raw); i++ {
+      buf[pos + 4 + i] = raw[i];
+    }
+    for i := len(raw); i < paddedLen; i++ {
+      buf[pos + 4 + i] = 0x00;
+    }
+    return nil, 4 + paddedLen
+  } else if attribute.atrType == ATRErrorCode {
+    if len(buf) - pos < 8 {
+      return errors.New("invalid buffer size"), 0
+    }
+    buf[pos + 1], buf[pos + 0] = setAttributeType(ATRErrorCode);
+    buf[pos + 2] = 0x00
+    buf[pos + 3] = 0x04
+    buf[pos + 4] = 0x00
+    buf[pos + 5] = 0x00
+    buf[pos + 6] = 0x04
+    buf[pos + 7] = 0x01
+    return nil, 8
+  } else {
+    fmt.Println("UNKNOWN!");
+  }
+  return nil, 8
+}

net/server/internal/sturn/message.go

@@ -3,15 +3,87 @@ package sturn
 import (
   "net"
   "fmt"
+  "errors"
+  "bytes"
+  "strings"
+  "strconv"
 )
 
 func readMessage(buf []byte) (error, *SturnMessage) {
-  return nil, nil
+  if len(buf) < 20 {
+    return errors.New("invalid header size"), nil
+  }
+  if buf[0] & 0xC0 != 0 {
+    return errors.New("invalid message prefix"), nil
+  }
+  magic := []byte{0x21, 0x12, 0xA4, 0x42 }
+  if !bytes.Equal(magic, buf[4:8]) {
+    return errors.New("invalid message cookie"), nil
+  }
+  atrLength := int(buf[2]) * 256 + int(buf[3])
+  if atrLength + 20 != len(buf) {
+    return errors.New("invalid message length"), nil
+  }
+
+  class, method := getMessageType(buf[0], buf[1]);
+  transaction := buf[8:20];
+
+  var attributes []SturnAttribute
+  var pos int = 0
+  for pos < atrLength {
+    err, attr, n := readAttribute(buf, pos + 20)
+    if err != nil {
+      return err, nil
+    }
+    pos += n
+    attributes = append(attributes, *attr);
+  }
+
+  return nil, &SturnMessage{
+    class: class,
+    method: method,
+    transaction: transaction,
+    attributes: attributes,
+  }
 }
 
-
 func writeMessage(msg *SturnMessage, buf []byte) (error, int) {
-  return nil, 0
+  if len(buf) < 20 {
+    return errors.New("invalid buffer length"), 0
+  }
+  // set prefix
+  buf[0], buf[1] = setMessageType(msg.class, msg.method)
+
+  // init size
+  buf[2] = 0x00
+  buf[3] = 0x00
+
+  // set cookie
+  buf[4] = 0x21
+  buf[5] = 0x12
+  buf[6] = 0xA4
+  buf[7] = 0x42
+
+  // set transaction
+  for i := 0; i < 12; i++ {
+    buf[8 + i] = msg.transaction[i];
+  }
+
+  // set each attribute
+  pos := 0
+  for _, attribute := range msg.attributes {
+    err, n := writeAttribute(&attribute, buf, 20 + pos);
+    if err != nil {
+      return err, 0
+    }
+    pos += n;
+
+    // set size
+    buf[2] = byte((pos >> 8) % 256);
+    buf[3] = byte(pos % 256);
+  }
+
+  return nil, pos + 20;
 }
 
 func (s *Sturn) handleMessage(buf []byte, addr net.Addr) {
@@ -38,15 +110,37 @@ func (s *Sturn) handleMessage(buf []byte, addr net.Addr) {
   } else {
     fmt.Println("unsupported message", buf);
   }
-
-  fmt.Println("STURN>", addr, buf);
 }
 
 func (s *Sturn) handleBindingRequest(msg *SturnMessage, addr net.Addr) (error) {
+
+  address := strings.Split(addr.String(), ":")
+  ip := address[0];
+  port, _ := strconv.Atoi(address[1]);
+  var attributes []SturnAttribute
+  attributes = append(attributes, SturnAttribute{
+    atrType: ATRXorMappedAddress,
+    byteValue: FAMIPv4,
+    intValue: int32(port),
+    strValue: ip,
+  });
+  response := &SturnMessage{
+    class: CLSResponse,
+    method: MEHBinding,
+    transaction: msg.transaction,
+    attributes: attributes,
+  };
+  err, n := writeMessage(response, s.buf);
+  if err != nil {
+    fmt.Printf("failed to write stun response");
+  } else {
+    (*s.conn).WriteTo(s.buf[:n], addr);
+  }
   return nil
 }
 
 func (s *Sturn) handleAllocateRequest(msg *SturnMessage, addr net.Addr) (error) {
+  fmt.Println("ALLOCATE REQUEST");
   return nil
 }
 

net/server/internal/sturn/sturn.go

@@ -10,6 +10,8 @@ import (
 
 var sturn *Sturn
 const SturnKeepAlive = 3600
+const SturnMaxSize = 1024
+const SturnMaxBindFail = 16
 
 type SturnSession struct {
 }
@@ -22,6 +24,7 @@ type Sturn struct {
   relayEnd uint
   conn *net.PacketConn
   closed chan bool
+  buf []byte
 }
 
 func Listen(port uint, relayStart uint, relayEnd uint) (error) {
@@ -45,6 +48,7 @@ func Listen(port uint, relayStart uint, relayEnd uint) (error) {
     relayStart: relayStart,
     relayEnd: relayEnd,
     conn: &conn,
+    buf: make([]byte, SturnMaxSize),
   }
 
   go sturn.serve(conn);
@@ -61,7 +65,7 @@ func Close() {
 
 func (s *Sturn) serve(conn net.PacketConn) {
   for {
-    buf := make([]byte, 1024)
+    buf := make([]byte, SturnMaxSize)
     n, addr, err := conn.ReadFrom(buf)
 		if err != nil {
       fmt.Println(err)