add decode functions for info and data packets

This commit is contained in:
2024-12-18 22:48:08 -06:00
parent 90c734a023
commit 0a74b6a956
18 changed files with 724 additions and 0 deletions
+31
View File
@@ -0,0 +1,31 @@
package main
import (
"fmt"
"log/slog"
"github.com/jwetzell/psn-go/internal/decoders"
)
func main() {
infoDecoded, error := decoders.DecodePacketChunk([]byte{0x56, 0x67, 0x34, 0x80, 0x00, 0x00, 0x0c, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x01,
0x01, 0x01, 0x00, 0x0b, 0x00, 0x53, 0x65, 0x72, 0x76, 0x65, 0x72, 0x20, 0x4e, 0x61, 0x6d, 0x65, 0x02, 0x00, 0x11,
0x80, 0x01, 0x00, 0x0d, 0x80, 0x00, 0x00, 0x09, 0x00, 0x54, 0x72, 0x61, 0x63, 0x6b, 0x65, 0x72, 0x20, 0x31})
if error != nil {
slog.Error("problem decoding packet", "error", error)
} else {
fmt.Printf("decoded: %+v\n", infoDecoded)
}
dataDecoded, error := decoders.DecodePacketChunk([]byte{0x55, 0x67, 0x28, 0x80, 0x00, 0x00, 0x0c, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x01,
0x01, 0x01, 0x00, 0x14, 0x80, 0x01, 0x00, 0x10, 0x80, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x00, 0x80, 0x3f, 0x00, 0x00,
0x80, 0x3f, 0x00, 0x00, 0x80, 0x3f})
if error != nil {
slog.Error("problem decoding packet", "error", error)
} else {
fmt.Printf("decoded: %+v\n", dataDecoded)
}
}
+3
View File
@@ -0,0 +1,3 @@
module github.com/jwetzell/psn-go
go 1.23.1
+43
View File
@@ -0,0 +1,43 @@
package decoders
import (
"encoding/binary"
)
type ChunkHeader struct {
Id uint16
DataLen uint16
HasSubchunks bool
}
type Chunk struct {
Header ChunkHeader
ChunkData []byte
}
func DecodeChunk(bytes []byte) Chunk {
id := binary.LittleEndian.Uint16(bytes[0:2])
lengthAndFlag := binary.LittleEndian.Uint16(bytes[2:4])
data_len := lengthAndFlag
has_subchunks := lengthAndFlag > 32768
if has_subchunks {
data_len = data_len - 32768
}
header := ChunkHeader{
Id: id,
DataLen: data_len,
HasSubchunks: has_subchunks,
}
chunk_data := bytes[4 : 4+header.DataLen]
return Chunk{
Header: header,
ChunkData: chunk_data,
}
}
+60
View File
@@ -0,0 +1,60 @@
package decoders
import (
"fmt"
"reflect"
"testing"
)
func TestChunkDecoding(t *testing.T) {
testCases := []struct {
description string
bytes []byte
expected Chunk
}{
{
description: "info packet",
bytes: []byte{0x56, 0x67, 0x34, 0x80, 0x00, 0x00, 0x0c, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x01,
0x01, 0x01, 0x00, 0x0b, 0x00, 0x53, 0x65, 0x72, 0x76, 0x65, 0x72, 0x20, 0x4e, 0x61, 0x6d, 0x65, 0x02, 0x00, 0x11,
0x80, 0x01, 0x00, 0x0d, 0x80, 0x00, 0x00, 0x09, 0x00, 0x54, 0x72, 0x61, 0x63, 0x6b, 0x65, 0x72, 0x20, 0x31},
expected: Chunk{
Header: ChunkHeader{
Id: uint16(26454),
DataLen: uint16(52),
HasSubchunks: true,
},
ChunkData: []byte{0x00, 0x00, 0x0c, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x01,
0x01, 0x01, 0x00, 0x0b, 0x00, 0x53, 0x65, 0x72, 0x76, 0x65, 0x72, 0x20, 0x4e, 0x61, 0x6d, 0x65, 0x02, 0x00, 0x11,
0x80, 0x01, 0x00, 0x0d, 0x80, 0x00, 0x00, 0x09, 0x00, 0x54, 0x72, 0x61, 0x63, 0x6b, 0x65, 0x72, 0x20, 0x31},
},
},
{
description: "data packet",
bytes: []byte{0x55, 0x67, 0x28, 0x80, 0x00, 0x00, 0x0c, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x01,
0x01, 0x01, 0x00, 0x14, 0x80, 0x01, 0x00, 0x10, 0x80, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x00, 0x80, 0x3f, 0x00, 0x00,
0x80, 0x3f, 0x00, 0x00, 0x80, 0x3f},
expected: Chunk{
Header: ChunkHeader{
Id: uint16(26453),
DataLen: uint16(40),
HasSubchunks: true,
},
ChunkData: []byte{0x00, 0x00, 0x0c, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x01,
0x01, 0x01, 0x00, 0x14, 0x80, 0x01, 0x00, 0x10, 0x80, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x00, 0x80, 0x3f, 0x00, 0x00,
0x80, 0x3f, 0x00, 0x00, 0x80, 0x3f},
},
},
}
for _, testCase := range testCases {
actual := DecodeChunk(testCase.bytes)
if !reflect.DeepEqual(actual, testCase.expected) {
t.Errorf("Test '%s' failed to decode chunk properly", testCase.description)
fmt.Printf("expected: %v\n", testCase.expected)
fmt.Printf("actual: %v\n", actual)
}
}
}
+5
View File
@@ -0,0 +1,5 @@
package decoders
var PACKET_HEADER_SIZE = 16
var MAX_UDP_PACKET_SIZE = 1500
var CHUNK_HEADER_SIZE = 4
+58
View File
@@ -0,0 +1,58 @@
package decoders
import (
"encoding/binary"
"log/slog"
)
type DataPacketChunkData struct {
PacketHeader *PacketHeaderChunk
TrackerList *DataTrackerListChunk
}
func decodeDataPacketChunkData(dataPacketChunk Chunk) DataPacketChunkData {
data := DataPacketChunkData{}
if dataPacketChunk.Header.HasSubchunks && dataPacketChunk.ChunkData != nil && dataPacketChunk.Header.DataLen > 0 {
offset := 0
for offset < int(dataPacketChunk.Header.DataLen) {
switch id := binary.LittleEndian.Uint16(dataPacketChunk.ChunkData[offset : offset+2]); id {
case 0:
packet_header := DecodePacketHeaderChunk(dataPacketChunk.ChunkData[offset:])
data.PacketHeader = &packet_header
offset = offset + CHUNK_HEADER_SIZE
if packet_header.Chunk.Header.DataLen > 0 {
offset = offset + int(packet_header.Chunk.Header.DataLen)
}
case 1:
tracker_list := DecodeDataTrackerListChunk(dataPacketChunk.ChunkData[offset:])
data.TrackerList = &tracker_list
offset = offset + CHUNK_HEADER_SIZE
if tracker_list.Chunk.Header.DataLen > 0 {
offset = offset + int(tracker_list.Chunk.Header.DataLen)
}
default:
slog.Error("unhandled info packet id", "id", id)
}
}
}
return data
}
type DataPacketChunk struct {
Chunk Chunk
Data DataPacketChunkData `json:"data"`
}
func DecodeDataPacketChunk(bytes []byte) DataPacketChunk {
chunk := DecodeChunk(bytes)
data := decodeDataPacketChunkData(chunk)
return DataPacketChunk{
Chunk: chunk,
Data: data,
}
}
+99
View File
@@ -0,0 +1,99 @@
package decoders
import (
"encoding/binary"
"log/slog"
)
type DataTrackerChunkData struct {
Pos *DataTrackerXYZChunk
Speed *DataTrackerXYZChunk
Ori *DataTrackerXYZChunk
Status *DataTrackerStatusChunk
Accel *DataTrackerXYZChunk
TrgtPos *DataTrackerXYZChunk
Timestamp *DataTrackerTimestampChunk
}
func decodeDataTrackerChunkData(dataTrackerChunk Chunk) DataTrackerChunkData {
data := DataTrackerChunkData{}
if dataTrackerChunk.Header.HasSubchunks && dataTrackerChunk.ChunkData != nil && dataTrackerChunk.Header.DataLen > 0 {
offset := 0
for offset < int(dataTrackerChunk.Header.DataLen) {
switch id := binary.LittleEndian.Uint16(dataTrackerChunk.ChunkData[offset : offset+2]); id {
case 0:
pos := DecodeDataTrackerXYZChunk(dataTrackerChunk.ChunkData[offset:])
data.Pos = &pos
offset = offset + CHUNK_HEADER_SIZE
if data.Pos.Chunk.Header.DataLen > 0 {
offset = offset + int(data.Pos.Chunk.Header.DataLen)
}
case 1:
speed := DecodeDataTrackerXYZChunk(dataTrackerChunk.ChunkData[offset:])
data.Speed = &speed
offset = offset + CHUNK_HEADER_SIZE
if data.Speed.Chunk.Header.DataLen > 0 {
offset = offset + int(data.Speed.Chunk.Header.DataLen)
}
case 2:
ori := DecodeDataTrackerXYZChunk(dataTrackerChunk.ChunkData[offset:])
data.Ori = &ori
offset = offset + CHUNK_HEADER_SIZE
if data.Ori.Chunk.Header.DataLen > 0 {
offset = offset + int(data.Ori.Chunk.Header.DataLen)
}
case 3:
status := DecodeDataTrackerStatusChunk(dataTrackerChunk.ChunkData[offset:])
data.Status = &status
offset = offset + CHUNK_HEADER_SIZE
if data.Status.Chunk.Header.DataLen > 0 {
offset = offset + int(data.Status.Chunk.Header.DataLen)
}
case 4:
accel := DecodeDataTrackerXYZChunk(dataTrackerChunk.ChunkData[offset:])
data.Accel = &accel
offset = offset + CHUNK_HEADER_SIZE
if data.Accel.Chunk.Header.DataLen > 0 {
offset = offset + int(data.Accel.Chunk.Header.DataLen)
}
case 5:
trgtpos := DecodeDataTrackerXYZChunk(dataTrackerChunk.ChunkData[offset:])
data.TrgtPos = &trgtpos
offset = offset + CHUNK_HEADER_SIZE
if data.TrgtPos.Chunk.Header.DataLen > 0 {
offset = offset + int(data.TrgtPos.Chunk.Header.DataLen)
}
case 6:
timestamp := DecodeDataTrackerTimestampChunk(dataTrackerChunk.ChunkData[offset:])
data.Timestamp = &timestamp
offset = offset + CHUNK_HEADER_SIZE
if data.Timestamp.Chunk.Header.DataLen > 0 {
offset = offset + int(data.Timestamp.Chunk.Header.DataLen)
}
default:
offset = int(dataTrackerChunk.Header.DataLen)
slog.Error("unhandled data tracker packet chunk id", "id", id)
}
}
}
return data
}
type DataTrackerChunk struct {
Chunk Chunk
Data DataTrackerChunkData
}
func DecodeDataTrackerChunk(bytes []byte) DataTrackerChunk {
chunk := DecodeChunk(bytes)
data := decodeDataTrackerChunkData(chunk)
return DataTrackerChunk{
Chunk: chunk,
Data: data,
}
}
@@ -0,0 +1,40 @@
package decoders
type DataTrackerListChunkData struct {
Trackers []DataTrackerChunk
}
func decodeDataTrackerListChunkData(dataTrackerListChunk Chunk) DataTrackerListChunkData {
var trackers []DataTrackerChunk
if dataTrackerListChunk.Header.HasSubchunks && dataTrackerListChunk.Header.DataLen > 0 {
offset := 0
for offset < int(dataTrackerListChunk.Header.DataLen) {
trackerChunk := DecodeDataTrackerChunk(dataTrackerListChunk.ChunkData[offset:])
offset += CHUNK_HEADER_SIZE
if trackerChunk.Chunk.Header.DataLen > 0 {
offset += int(trackerChunk.Chunk.Header.DataLen)
}
trackers = append(trackers, trackerChunk)
}
}
return DataTrackerListChunkData{
Trackers: trackers,
}
}
type DataTrackerListChunk struct {
Chunk Chunk
Data DataTrackerListChunkData
}
func DecodeDataTrackerListChunk(bytes []byte) DataTrackerListChunk {
chunk := DecodeChunk(bytes)
data := decodeDataTrackerListChunkData(chunk)
return DataTrackerListChunk{
Chunk: chunk,
Data: data,
}
}
@@ -0,0 +1,34 @@
package decoders
import (
"encoding/binary"
"math"
)
type DataTrackerStatusChunkData struct {
Validity float32
}
func decodeDataTrackerStatusChunk(dataTrackerStatusChunk Chunk) DataTrackerStatusChunkData {
statusBits := binary.LittleEndian.Uint32(dataTrackerStatusChunk.ChunkData[0:4])
return DataTrackerStatusChunkData{
Validity: math.Float32frombits(statusBits),
}
}
type DataTrackerStatusChunk struct {
Chunk Chunk
Data DataTrackerStatusChunkData
}
func DecodeDataTrackerStatusChunk(bytes []byte) DataTrackerStatusChunk {
chunk := DecodeChunk(bytes)
data := decodeDataTrackerStatusChunk(chunk)
return DataTrackerStatusChunk{
Chunk: chunk,
Data: data,
}
}
@@ -0,0 +1,33 @@
package decoders
import (
"encoding/binary"
)
type DataTrackerTimestampChunkData struct {
Timestamp uint64
}
func decodeDataTrackerTimestampChunk(dataTrackerTimestampChunk Chunk) DataTrackerTimestampChunkData {
timestamp := binary.LittleEndian.Uint64(dataTrackerTimestampChunk.ChunkData[0:8])
return DataTrackerTimestampChunkData{
Timestamp: timestamp,
}
}
type DataTrackerTimestampChunk struct {
Chunk Chunk
Data DataTrackerTimestampChunkData
}
func DecodeDataTrackerTimestampChunk(bytes []byte) DataTrackerTimestampChunk {
chunk := DecodeChunk(bytes)
data := decodeDataTrackerTimestampChunk(chunk)
return DataTrackerTimestampChunk{
Chunk: chunk,
Data: data,
}
}
@@ -0,0 +1,40 @@
package decoders
import (
"encoding/binary"
"math"
)
type DataTrackerXYZChunkData struct {
X float32
Y float32
Z float32
}
func decodeDataTrackerXYZChunk(dataTrackerXYZChunk Chunk) DataTrackerXYZChunkData {
xBits := binary.LittleEndian.Uint32(dataTrackerXYZChunk.ChunkData[0:4])
yBits := binary.LittleEndian.Uint32(dataTrackerXYZChunk.ChunkData[4:8])
zBits := binary.LittleEndian.Uint32(dataTrackerXYZChunk.ChunkData[8:12])
return DataTrackerXYZChunkData{
X: math.Float32frombits(xBits),
Y: math.Float32frombits(yBits),
Z: math.Float32frombits(zBits),
}
}
type DataTrackerXYZChunk struct {
Chunk Chunk
Data DataTrackerXYZChunkData
}
func DecodeDataTrackerXYZChunk(bytes []byte) DataTrackerXYZChunk {
chunk := DecodeChunk(bytes)
data := decodeDataTrackerXYZChunk(chunk)
return DataTrackerXYZChunk{
Chunk: chunk,
Data: data,
}
}
+68
View File
@@ -0,0 +1,68 @@
package decoders
import (
"encoding/binary"
"log/slog"
)
type InfoPacketChunkData struct {
PacketHeader *PacketHeaderChunk
SystemName *InfoSystemNameChunk
TrackerList *InfoTrackerListChunk
}
type EmptyData struct{}
func decodeInfoPacketChunkData(infoPacketChunk Chunk) InfoPacketChunkData {
data := InfoPacketChunkData{}
if infoPacketChunk.Header.HasSubchunks && infoPacketChunk.ChunkData != nil && infoPacketChunk.Header.DataLen > 0 {
offset := 0
for offset < int(infoPacketChunk.Header.DataLen) {
switch id := binary.LittleEndian.Uint16(infoPacketChunk.ChunkData[offset : offset+2]); id {
case 0:
packet_header := DecodePacketHeaderChunk(infoPacketChunk.ChunkData[offset:])
data.PacketHeader = &packet_header
offset = offset + CHUNK_HEADER_SIZE
if packet_header.Chunk.Header.DataLen > 0 {
offset = offset + int(packet_header.Chunk.Header.DataLen)
}
case 1:
system_name := DecodeInfoSystemNameChunk(infoPacketChunk.ChunkData[offset:])
data.SystemName = &system_name
offset = offset + CHUNK_HEADER_SIZE
if system_name.Chunk.Header.DataLen > 0 {
offset = offset + int(system_name.Chunk.Header.DataLen)
}
case 2:
tracker_list := DecodeInfoTrackerListChunk(infoPacketChunk.ChunkData[offset:])
data.TrackerList = &tracker_list
offset = offset + CHUNK_HEADER_SIZE
if tracker_list.Chunk.Header.DataLen > 0 {
offset = offset + int(tracker_list.Chunk.Header.DataLen)
}
default:
slog.Error("unhandled info packet id", "id", id)
}
}
}
return data
}
type InfoPacketChunk struct {
Chunk Chunk
Data InfoPacketChunkData
}
func DecodeInfoPacketChunk(bytes []byte) InfoPacketChunk {
chunk := DecodeChunk(bytes)
data := decodeInfoPacketChunkData(chunk)
return InfoPacketChunk{
Chunk: chunk,
Data: data,
}
}
@@ -0,0 +1,29 @@
package decoders
type InfoSystemNameChunkData struct {
SystemName string
}
func decodeInfoSystemNameChunkData(infoSystemNameChunk Chunk) InfoSystemNameChunkData {
system_name := string(infoSystemNameChunk.ChunkData[0:infoSystemNameChunk.Header.DataLen])
return InfoSystemNameChunkData{
SystemName: system_name,
}
}
type InfoSystemNameChunk struct {
Chunk Chunk
Data InfoSystemNameChunkData
}
func DecodeInfoSystemNameChunk(bytes []byte) InfoSystemNameChunk {
chunk := DecodeChunk(bytes)
data := decodeInfoSystemNameChunkData(chunk)
return InfoSystemNameChunk{
Chunk: chunk,
Data: data,
}
}
+49
View File
@@ -0,0 +1,49 @@
package decoders
import (
"encoding/binary"
"log/slog"
)
type InfoTrackerChunkData struct {
TrackerName *InfoTrackerNameChunk
}
func decodeInfoTrackerChunkData(infoTrackerChunk Chunk) InfoTrackerChunkData {
data := InfoTrackerChunkData{}
if infoTrackerChunk.Header.HasSubchunks && infoTrackerChunk.ChunkData != nil && infoTrackerChunk.Header.DataLen > 0 {
offset := 0
for offset < int(infoTrackerChunk.Header.DataLen) {
switch id := binary.LittleEndian.Uint16(infoTrackerChunk.ChunkData[offset : offset+2]); id {
case 0:
tracker_name := DecodeInfoTrackerNameChunk(infoTrackerChunk.ChunkData[offset:])
data.TrackerName = &tracker_name
offset = offset + CHUNK_HEADER_SIZE
if tracker_name.Chunk.Header.DataLen > 0 {
offset = offset + int(tracker_name.Chunk.Header.DataLen)
}
default:
slog.Error("unhandled info tracker chunk id", "id", id)
}
}
}
return data
}
type InfoTrackerChunk struct {
Chunk Chunk
Data InfoTrackerChunkData
}
func DecodeInfoTrackerChunk(bytes []byte) InfoTrackerChunk {
chunk := DecodeChunk(bytes)
data := decodeInfoTrackerChunkData(chunk)
return InfoTrackerChunk{
Chunk: chunk,
Data: data,
}
}
@@ -0,0 +1,40 @@
package decoders
type InfoTrackerListChunkData struct {
Trackers []InfoTrackerChunk
}
func decodeInfoTrackerListChunkData(infoTrackerListChunk Chunk) InfoTrackerListChunkData {
var trackers []InfoTrackerChunk
if infoTrackerListChunk.Header.HasSubchunks && infoTrackerListChunk.Header.DataLen > 0 {
offset := 0
for offset < int(infoTrackerListChunk.Header.DataLen) {
trackerChunk := DecodeInfoTrackerChunk(infoTrackerListChunk.ChunkData[offset:])
offset += CHUNK_HEADER_SIZE
if trackerChunk.Chunk.Header.DataLen > 0 {
offset += int(trackerChunk.Chunk.Header.DataLen)
}
trackers = append(trackers, trackerChunk)
}
}
return InfoTrackerListChunkData{
Trackers: trackers,
}
}
type InfoTrackerListChunk struct {
Chunk Chunk
Data InfoTrackerListChunkData
}
func DecodeInfoTrackerListChunk(bytes []byte) InfoTrackerListChunk {
chunk := DecodeChunk(bytes)
data := decodeInfoTrackerListChunkData(chunk)
return InfoTrackerListChunk{
Chunk: chunk,
Data: data,
}
}
@@ -0,0 +1,29 @@
package decoders
type InfoTrackerNameChunkData struct {
TrackerName string
}
func decodeInfoTrackerNameChunkData(infoTrackerNameChunk Chunk) InfoTrackerNameChunkData {
tracker_name := string(infoTrackerNameChunk.ChunkData[0:infoTrackerNameChunk.Header.DataLen])
return InfoTrackerNameChunkData{
TrackerName: tracker_name,
}
}
type InfoTrackerNameChunk struct {
Chunk Chunk
Data InfoTrackerNameChunkData
}
func DecodeInfoTrackerNameChunk(bytes []byte) InfoTrackerNameChunk {
chunk := DecodeChunk(bytes)
data := decodeInfoTrackerNameChunkData(chunk)
return InfoTrackerNameChunk{
Chunk: chunk,
Data: data,
}
}
+20
View File
@@ -0,0 +1,20 @@
package decoders
import (
"encoding/binary"
"errors"
"log/slog"
)
func DecodePacketChunk(bytes []byte) (interface{}, error) {
switch id := binary.LittleEndian.Uint16(bytes[0:2]); id {
case 0x6756:
return DecodeInfoPacketChunk(bytes), nil
case 0x6755:
return DecodeDataPacketChunk(bytes), nil
default:
slog.Error("unhandled packet id", "id", id)
return Chunk{}, errors.New("unhandled packet id")
}
}
+43
View File
@@ -0,0 +1,43 @@
package decoders
import "encoding/binary"
type PacketHeaderChunkData struct {
PacketTimestamp uint64
VersionHigh uint8
VersionLow uint8
FrameId uint8
FramePacketCount uint8
}
func decodePacketHeaderChunkData(packetHeaderChunk Chunk) PacketHeaderChunkData {
packet_timestamp := binary.LittleEndian.Uint64(packetHeaderChunk.ChunkData[0:8])
version_high := packetHeaderChunk.ChunkData[8]
version_low := packetHeaderChunk.ChunkData[9]
frame_id := packetHeaderChunk.ChunkData[10]
frame_packet_count := packetHeaderChunk.ChunkData[11]
return PacketHeaderChunkData{
PacketTimestamp: packet_timestamp,
VersionHigh: version_high,
VersionLow: version_low,
FrameId: frame_id,
FramePacketCount: frame_packet_count,
}
}
type PacketHeaderChunk struct {
Chunk Chunk
Data PacketHeaderChunkData
}
func DecodePacketHeaderChunk(bytes []byte) PacketHeaderChunk {
chunk := DecodeChunk(bytes)
data := decodePacketHeaderChunkData(chunk)
return PacketHeaderChunk{
Chunk: chunk,
Data: data,
}
}