|
|
@@ -132,12 +132,15 @@ func PrintPacket(p *Packet) {
|
|
|
|
|
|
func printPacket(p *Packet, indent int, printBytes bool) {
|
|
|
indent_str := ""
|
|
|
+
|
|
|
for len(indent_str) != indent {
|
|
|
indent_str += " "
|
|
|
}
|
|
|
|
|
|
class_str := ClassMap[p.ClassType]
|
|
|
+
|
|
|
tagtype_str := TypeMap[p.TagType]
|
|
|
+
|
|
|
tag_str := fmt.Sprintf("0x%02X", p.Tag)
|
|
|
|
|
|
if p.ClassType == ClassUniversal {
|
|
|
@@ -146,6 +149,7 @@ func printPacket(p *Packet, indent int, printBytes bool) {
|
|
|
|
|
|
value := fmt.Sprint(p.Value)
|
|
|
description := ""
|
|
|
+
|
|
|
if p.Description != "" {
|
|
|
description = p.Description + ": "
|
|
|
}
|
|
|
@@ -163,13 +167,16 @@ func printPacket(p *Packet, indent int, printBytes bool) {
|
|
|
|
|
|
func resizeBuffer(in []byte, new_size uint64) (out []byte) {
|
|
|
out = make([]byte, new_size)
|
|
|
+
|
|
|
copy(out, in)
|
|
|
+
|
|
|
return
|
|
|
}
|
|
|
|
|
|
func readBytes(reader io.Reader, buf []byte) error {
|
|
|
idx := 0
|
|
|
buflen := len(buf)
|
|
|
+
|
|
|
for idx < buflen {
|
|
|
n, err := reader.Read(buf[idx:])
|
|
|
if err != nil {
|
|
|
@@ -177,56 +184,74 @@ func readBytes(reader io.Reader, buf []byte) error {
|
|
|
}
|
|
|
idx += n
|
|
|
}
|
|
|
+
|
|
|
return nil
|
|
|
}
|
|
|
|
|
|
func ReadPacket(reader io.Reader) (*Packet, error) {
|
|
|
buf := make([]byte, 2)
|
|
|
+
|
|
|
err := readBytes(reader, buf)
|
|
|
+
|
|
|
if err != nil {
|
|
|
return nil, err
|
|
|
}
|
|
|
+
|
|
|
idx := uint64(2)
|
|
|
datalen := uint64(buf[1])
|
|
|
+
|
|
|
if Debug {
|
|
|
fmt.Printf("Read: datalen = %d len(buf) = %d ", datalen, len(buf))
|
|
|
+
|
|
|
for _, b := range buf {
|
|
|
fmt.Printf("%02X ", b)
|
|
|
}
|
|
|
+
|
|
|
fmt.Printf("\n")
|
|
|
}
|
|
|
+
|
|
|
if datalen&128 != 0 {
|
|
|
a := datalen - 128
|
|
|
+
|
|
|
idx += a
|
|
|
buf = resizeBuffer(buf, 2+a)
|
|
|
+
|
|
|
err := readBytes(reader, buf[2:])
|
|
|
+
|
|
|
if err != nil {
|
|
|
return nil, err
|
|
|
}
|
|
|
+
|
|
|
datalen = DecodeInteger(buf[2 : 2+a])
|
|
|
+
|
|
|
if Debug {
|
|
|
fmt.Printf("Read: a = %d idx = %d datalen = %d len(buf) = %d", a, idx, datalen, len(buf))
|
|
|
+
|
|
|
for _, b := range buf {
|
|
|
fmt.Printf("%02X ", b)
|
|
|
}
|
|
|
+
|
|
|
fmt.Printf("\n")
|
|
|
}
|
|
|
}
|
|
|
|
|
|
buf = resizeBuffer(buf, idx+datalen)
|
|
|
err = readBytes(reader, buf[idx:])
|
|
|
+
|
|
|
if err != nil {
|
|
|
return nil, err
|
|
|
}
|
|
|
|
|
|
if Debug {
|
|
|
fmt.Printf("Read: len( buf ) = %d idx=%d datalen=%d idx+datalen=%d\n", len(buf), idx, datalen, idx+datalen)
|
|
|
+
|
|
|
for _, b := range buf {
|
|
|
fmt.Printf("%02X ", b)
|
|
|
}
|
|
|
}
|
|
|
|
|
|
p := DecodePacket(buf)
|
|
|
+
|
|
|
return p, nil
|
|
|
}
|
|
|
|
|
|
@@ -234,6 +259,7 @@ func DecodeString(data []byte) (ret string) {
|
|
|
for _, c := range data {
|
|
|
ret += fmt.Sprintf("%c", c)
|
|
|
}
|
|
|
+
|
|
|
return
|
|
|
}
|
|
|
|
|
|
@@ -242,29 +268,38 @@ func DecodeInteger(data []byte) (ret uint64) {
|
|
|
ret = ret * 256
|
|
|
ret = ret + uint64(i)
|
|
|
}
|
|
|
+
|
|
|
return
|
|
|
}
|
|
|
|
|
|
func EncodeInteger(val uint64) []byte {
|
|
|
var out bytes.Buffer
|
|
|
+
|
|
|
found := false
|
|
|
+
|
|
|
shift := uint(56)
|
|
|
+
|
|
|
mask := uint64(0xFF00000000000000)
|
|
|
+
|
|
|
for mask > 0 {
|
|
|
if !found && (val&mask != 0) {
|
|
|
found = true
|
|
|
}
|
|
|
+
|
|
|
if found || (shift == 0) {
|
|
|
out.Write([]byte{byte((val & mask) >> shift)})
|
|
|
}
|
|
|
+
|
|
|
shift -= 8
|
|
|
mask = mask >> 8
|
|
|
}
|
|
|
+
|
|
|
return out.Bytes()
|
|
|
}
|
|
|
|
|
|
func DecodePacket(data []byte) *Packet {
|
|
|
p, _ := decodePacket(data)
|
|
|
+
|
|
|
return p
|
|
|
}
|
|
|
|
|
|
@@ -272,13 +307,16 @@ func decodePacket(data []byte) (*Packet, []byte) {
|
|
|
if Debug {
|
|
|
fmt.Printf("decodePacket: enter %d\n", len(data))
|
|
|
}
|
|
|
+
|
|
|
p := new(Packet)
|
|
|
+
|
|
|
p.ClassType = data[0] & ClassBitmask
|
|
|
p.TagType = data[0] & TypeBitmask
|
|
|
p.Tag = data[0] & TagBitmask
|
|
|
|
|
|
datalen := DecodeInteger(data[1:2])
|
|
|
datapos := uint64(2)
|
|
|
+
|
|
|
if datalen&128 != 0 {
|
|
|
datalen -= 128
|
|
|
datapos += datalen
|
|
|
@@ -286,7 +324,9 @@ func decodePacket(data []byte) (*Packet, []byte) {
|
|
|
}
|
|
|
|
|
|
p.Data = new(bytes.Buffer)
|
|
|
+
|
|
|
p.Children = make([]*Packet, 0, 2)
|
|
|
+
|
|
|
p.Value = nil
|
|
|
|
|
|
value_data := data[datapos : datapos+datalen]
|
|
|
@@ -294,6 +334,7 @@ func decodePacket(data []byte) (*Packet, []byte) {
|
|
|
if p.TagType == TypeConstructed {
|
|
|
for len(value_data) != 0 {
|
|
|
var child *Packet
|
|
|
+
|
|
|
child, value_data = decodePacket(value_data)
|
|
|
p.AppendChild(child)
|
|
|
}
|
|
|
@@ -305,6 +346,7 @@ func decodePacket(data []byte) (*Packet, []byte) {
|
|
|
case TagEOC:
|
|
|
case TagBoolean:
|
|
|
val := DecodeInteger(value_data)
|
|
|
+
|
|
|
p.Value = val != 0
|
|
|
case TagInteger:
|
|
|
p.Value = DecodeInteger(value_data)
|
|
|
@@ -351,36 +393,46 @@ func (p *Packet) DataLength() uint64 {
|
|
|
|
|
|
func (p *Packet) Bytes() []byte {
|
|
|
var out bytes.Buffer
|
|
|
+
|
|
|
out.Write([]byte{p.ClassType | p.TagType | p.Tag})
|
|
|
packet_length := EncodeInteger(p.DataLength())
|
|
|
+
|
|
|
if p.DataLength() > 127 || len(packet_length) > 1 {
|
|
|
out.Write([]byte{byte(len(packet_length) | 128)})
|
|
|
out.Write(packet_length)
|
|
|
} else {
|
|
|
out.Write(packet_length)
|
|
|
}
|
|
|
+
|
|
|
out.Write(p.Data.Bytes())
|
|
|
+
|
|
|
return out.Bytes()
|
|
|
}
|
|
|
|
|
|
func (p *Packet) AppendChild(child *Packet) {
|
|
|
p.Data.Write(child.Bytes())
|
|
|
+
|
|
|
if len(p.Children) == cap(p.Children) {
|
|
|
newChildren := make([]*Packet, cap(p.Children)*2)
|
|
|
+
|
|
|
copy(newChildren, p.Children)
|
|
|
p.Children = newChildren[0:len(p.Children)]
|
|
|
}
|
|
|
+
|
|
|
p.Children = p.Children[0 : len(p.Children)+1]
|
|
|
p.Children[len(p.Children)-1] = child
|
|
|
}
|
|
|
|
|
|
func Encode(ClassType, TagType, Tag uint8, Value interface{}, Description string) *Packet {
|
|
|
p := new(Packet)
|
|
|
+
|
|
|
p.ClassType = ClassType
|
|
|
p.TagType = TagType
|
|
|
p.Tag = Tag
|
|
|
p.Data = new(bytes.Buffer)
|
|
|
+
|
|
|
p.Children = make([]*Packet, 0, 2)
|
|
|
+
|
|
|
p.Value = Value
|
|
|
p.Description = Description
|
|
|
|
|
|
@@ -391,6 +443,7 @@ func Encode(ClassType, TagType, Tag uint8, Value interface{}, Description string
|
|
|
switch Tag {
|
|
|
case TagOctetString:
|
|
|
sv, ok := v.Interface().(string)
|
|
|
+
|
|
|
if ok {
|
|
|
p.Data.Write([]byte(sv))
|
|
|
}
|
|
|
@@ -407,26 +460,33 @@ func NewSequence(Description string) *Packet {
|
|
|
|
|
|
func NewBoolean(ClassType, TagType, Tag uint8, Value bool, Description string) *Packet {
|
|
|
intValue := 0
|
|
|
+
|
|
|
if Value {
|
|
|
intValue = 1
|
|
|
}
|
|
|
|
|
|
p := Encode(ClassType, TagType, Tag, nil, Description)
|
|
|
+
|
|
|
p.Value = Value
|
|
|
p.Data.Write(EncodeInteger(uint64(intValue)))
|
|
|
+
|
|
|
return p
|
|
|
}
|
|
|
|
|
|
func NewInteger(ClassType, TagType, Tag uint8, Value uint64, Description string) *Packet {
|
|
|
p := Encode(ClassType, TagType, Tag, nil, Description)
|
|
|
+
|
|
|
p.Value = Value
|
|
|
p.Data.Write(EncodeInteger(Value))
|
|
|
+
|
|
|
return p
|
|
|
}
|
|
|
|
|
|
func NewString(ClassType, TagType, Tag uint8, Value, Description string) *Packet {
|
|
|
p := Encode(ClassType, TagType, Tag, nil, Description)
|
|
|
+
|
|
|
p.Value = Value
|
|
|
p.Data.Write([]byte(Value))
|
|
|
+
|
|
|
return p
|
|
|
}
|