From 0f6b5c95d88dbf2f3a8cc170231f0e68f44a5968 Mon Sep 17 00:00:00 2001 From: Nonoo Date: Fri, 16 Oct 2020 23:37:12 +0200 Subject: [PATCH] Implement all required control packets --- main.go | 240 +++++++++++++++++++++++++++++++++++++------------------- 1 file changed, 160 insertions(+), 80 deletions(-) diff --git a/main.go b/main.go index 9af3cdf..4aa572a 100644 --- a/main.go +++ b/main.go @@ -18,8 +18,10 @@ var conn *net.UDPConn var localSID uint32 var remoteSID uint32 var sendSeq uint16 -var longPingSendSeq uint16 +var authSendSeq uint16 var authID [6]byte +var randIDByteForPktSeven [1]byte +var expectedPkt7ReplySeq uint16 func send(d []byte) { _, err := conn.Write(d) @@ -50,17 +52,118 @@ func setupCloseHandler() { go func() { <-c log.Print("disconnecting") - send([]byte{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, byte(sendSeq), byte(sendSeq >> 8), byte(remoteSID >> 24), byte(remoteSID >> 16), byte(remoteSID >> 8), byte(remoteSID), byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID), - 0x00, 0x00, 0x00, 0x30, 0x01, 0x01, 0x00, byte(longPingSendSeq), - byte(longPingSendSeq >> 8), 0x00, authID[0], authID[1], authID[2], authID[3], authID[4], authID[5], - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}) + sendDisconnect() os.Exit(0) }() } +func sendPkt7(replyID []byte) { + // Example request from PC: 0x15, 0x00, 0x00, 0x00, 0x07, 0x00, 0x09, 0x00, 0xbe, 0xd9, 0xf2, 0x63, 0xe4, 0x35, 0xdd, 0x72, 0x00, 0x78, 0x40, 0xf6, 0x02 + // Example reply from radio: 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x09, 0x00, 0xe4, 0x35, 0xdd, 0x72, 0xbe, 0xd9, 0xf2, 0x63, 0x01, 0x78, 0x40, 0xf6, 0x02 + var replyFlag byte + if replyID == nil { + replyID = make([]byte, 4) + var randID [2]byte + _, err := rand.Read(randID[:]) + if err != nil { + log.Fatal(err) + } + replyID[0] = randID[0] + replyID[1] = randID[1] + replyID[2] = randIDByteForPktSeven[0] + replyID[3] = 0x03 + } else { + replyFlag = 0x01 + } + + expectedPkt7ReplySeq = sendSeq + + send([]byte{0x15, 0x00, 0x00, 0x00, 0x07, 0x00, byte(sendSeq), byte(sendSeq >> 8), + byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID), + byte(remoteSID >> 24), byte(remoteSID >> 16), byte(remoteSID >> 8), byte(remoteSID), + replyFlag, replyID[0], replyID[1], replyID[2], replyID[3]}) +} + +func sendPkt3() { + send([]byte{0x10, 0x00, 0x00, 0x00, 0x03, 0x00, byte(sendSeq), byte(sendSeq >> 8), + byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID), + byte(remoteSID >> 24), byte(remoteSID >> 16), byte(remoteSID >> 8), byte(remoteSID)}) +} + +func sendPkt6() { + send([]byte{0x10, 0x00, 0x00, 0x00, 0x06, 0x00, 0x01, 0x00, + byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID), + byte(remoteSID >> 24), byte(remoteSID >> 16), byte(remoteSID >> 8), byte(remoteSID)}) +} + +func sendPktLogin() { + var randID [2]byte + _, err := rand.Read(randID[:]) + if err != nil { + log.Fatal(err) + } + send([]byte{0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, + byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID), + byte(remoteSID >> 24), byte(remoteSID >> 16), byte(remoteSID >> 8), byte(remoteSID), + 0x00, 0x00, 0x00, 0x70, 0x01, 0x00, 0x00, byte(authSendSeq), + byte(authSendSeq >> 8), 0x00, randID[0], randID[1], 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x2b, 0x3f, 0x55, 0x5c, 0x00, 0x00, 0x00, 0x00, // username: beer + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x2b, 0x3f, 0x55, 0x5c, 0x3f, 0x25, 0x77, 0x58, // pass: beerbeer + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x69, 0x63, 0x6f, 0x6d, 0x2d, 0x70, 0x63, 0x00, // icom-pc + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}) +} + +func sendPktReauth() { + // Example request from PC: 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0d, 0x00, + // 0xbb, 0x41, 0x3f, 0x2b, 0xe6, 0xb2, 0x7b, 0x7b, + // 0x00, 0x00, 0x00, 0x30, 0x01, 0x05, 0x00, 0x02, + // 0x00, 0x00, 0x5d, 0x37, 0x12, 0x82, 0x3b, 0xde, + // 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + // 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + // 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + // 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + // Example reply from radio: 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, + // 0xe6, 0xb2, 0x7b, 0x7b, 0xbb, 0x41, 0x3f, 0x2b, + // 0x00, 0x00, 0x00, 0x30, 0x02, 0x05, 0x00, 0x02, + // 0x00, 0x00, 0x5d, 0x37, 0x12, 0x82, 0x3b, 0xde, + // 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + // 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + // 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + // 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + send([]byte{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, byte(sendSeq), byte(sendSeq >> 8), + byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID), + byte(remoteSID >> 24), byte(remoteSID >> 16), byte(remoteSID >> 8), byte(remoteSID), + 0x00, 0x00, 0x00, 0x30, 0x01, 0x05, 0x00, byte(authSendSeq), + byte(authSendSeq >> 8), 0x00, authID[0], authID[1], authID[2], authID[3], authID[4], authID[5], + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}) +} + +func sendDisconnect() { + send([]byte{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, byte(sendSeq), byte(sendSeq >> 8), + byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID), + byte(remoteSID >> 24), byte(remoteSID >> 16), byte(remoteSID >> 8), byte(remoteSID), + 0x00, 0x00, 0x00, 0x30, 0x01, 0x01, 0x00, byte(authSendSeq), + byte(authSendSeq >> 8), 0x00, authID[0], authID[1], authID[2], authID[3], authID[4], authID[5], + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}) + send([]byte{0x10, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, + byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID), + byte(remoteSID >> 24), byte(remoteSID >> 16), byte(remoteSID >> 8), byte(remoteSID)}) +} + func main() { log.Init() parseArgs() @@ -82,13 +185,14 @@ func main() { localSID = binary.BigEndian.Uint32(laddr.IP[len(laddr.IP)-4:])<<16 | uint32(laddr.Port&0xffff) log.Debugf("using session id %.8x", localSID) - send([]byte{0x10, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID), 0x00, 0x00, 0x00, 0x00}) - // send([]byte{0x15, 0x00, 0x00, 0x00, 0x07, 0x00, 0x01, 0x00, byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID), 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x3e, 0x10, 0x00}) - // send([]byte{0x10, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID), 0x00, 0x00, 0x00, 0x00}) - // send([]byte{0x15, 0x00, 0x00, 0x00, 0x07, 0x00, 0x02, 0x00, byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID), 0x00, 0x00, 0x00, 0x00, 0x00, 0x70, 0x3e, 0x10, 0x00}) - // send([]byte{0x10, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID), 0x00, 0x00, 0x00, 0x00}) + sendPkt3() + sendSeq = 1 + sendPkt7(nil) + sendSeq = 0 + sendPkt3() for { + // Expecting a Pkt4 answer. r, _ := read() if bytes.Equal(r[:8], []byte{0x10, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00}) { remoteSID = binary.BigEndian.Uint32(r[8:12]) @@ -98,29 +202,22 @@ func main() { log.Debugf("got remote session id %.8x", remoteSID) - send([]byte{0x10, 0x00, 0x00, 0x00, 0x06, 0x00, 0x01, 0x00, byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID), byte(remoteSID >> 24), byte(remoteSID >> 16), byte(remoteSID >> 8), byte(remoteSID)}) - send([]byte{0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID), byte(remoteSID >> 24), byte(remoteSID >> 16), byte(remoteSID >> 8), byte(remoteSID), - 0x00, 0x00, 0x00, 0x70, 0x01, 0x00, 0x00, byte(longPingSendSeq), - byte(longPingSendSeq >> 8), 0x00, 0x09, 0x27, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x2b, 0x3f, 0x55, 0x5c, 0x00, 0x00, 0x00, 0x00, // username: beer - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x2b, 0x3f, 0x55, 0x5c, 0x3f, 0x25, 0x77, 0x58, // pass: beerbeer - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x69, 0x63, 0x6f, 0x6d, 0x2d, 0x70, 0x63, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}) + sendSeq = 1 + authSendSeq = 1 + sendPkt6() + sendPktLogin() + authSendSeq++ - var lastReceivedSeq uint16 - var receivedSeq bool var lastPingAt time.Time - var lastLongPingAt time.Time + var lastReauthAt time.Time + var lastStatusLog time.Time var errCount int - var randID [4]byte + + _, err = rand.Read(randIDByteForPktSeven[:]) + if err != nil { + log.Fatal(err) + } + for { r, err := read() if err != nil { @@ -153,66 +250,49 @@ func main() { } } if len(r) == 21 && bytes.Equal(r[1:6], []byte{0x00, 0x00, 0x00, 0x07, 0x00}) { - // Replying to the radio. - // Example request from radio: 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x1c, 0x0e, 0xe4, 0x35, 0xdd, 0x72, 0xbe, 0xd9, 0xf2, 0x63, 0x00, 0x57, 0x2b, 0x12, 0x00 - // Example answer from PC: 0x15, 0x00, 0x00, 0x00, 0x07, 0x00, 0x1c, 0x0e, 0xbe, 0xd9, 0xf2, 0x63, 0xe4, 0x35, 0xdd, 0x72, 0x01, 0x57, 0x2b, 0x12, 0x00 - gotSeq := binary.LittleEndian.Uint16(r[6:8]) - if receivedSeq && lastReceivedSeq+1 != gotSeq { - log.Error("packet loss detected") + if r[16] == 0x00 { // This is a pkt7 request from the radio. + // Replying to the radio. + // Example request from radio: 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x1c, 0x0e, 0xe4, 0x35, 0xdd, 0x72, 0xbe, 0xd9, 0xf2, 0x63, 0x00, 0x57, 0x2b, 0x12, 0x00 + // Example answer from PC: 0x15, 0x00, 0x00, 0x00, 0x07, 0x00, 0x1c, 0x0e, 0xbe, 0xd9, 0xf2, 0x63, 0xe4, 0x35, 0xdd, 0x72, 0x01, 0x57, 0x2b, 0x12, 0x00 + sendPkt7(r[17:21]) + } else { + gotSeq := binary.LittleEndian.Uint16(r[6:8]) + if expectedPkt7ReplySeq != gotSeq { + var missingPkts int + if gotSeq > expectedPkt7ReplySeq { + missingPkts = int(gotSeq) - int(expectedPkt7ReplySeq) + } else { + missingPkts = int(gotSeq) + 65536 - int(expectedPkt7ReplySeq) + } + if missingPkts < 1000 { + log.Error("lost ", missingPkts, " packets ", gotSeq, " ", expectedPkt7ReplySeq) + } + } } - lastReceivedSeq = gotSeq - receivedSeq = true - - send([]byte{0x15, 0x00, 0x00, 0x00, 0x07, 0x00, byte(gotSeq), byte(gotSeq >> 8), byte(remoteSID >> 24), byte(remoteSID >> 16), byte(remoteSID >> 8), byte(remoteSID), byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID), 0x01, - r[17], r[18], r[19], r[20]}) } if len(r) == 16 && bytes.Equal(r[:6], []byte{0x10, 0x00, 0x00, 0x00, 0x00, 0x00}) { // Replying to the radio. // Example request from radio: 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x13, 0x00, 0xe4, 0x35, 0xdd, 0x72, 0xbe, 0xd9, 0xf2, 0x63 // Example answer from PC: 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x13, 0x00, 0xbe, 0xd9, 0xf2, 0x63, 0xe4, 0x35, 0xdd, 0x72 gotSeq := binary.LittleEndian.Uint16(r[6:8]) - send([]byte{0x10, 0x00, 0x00, 0x00, 0x00, 0x00, byte(gotSeq), byte(gotSeq >> 8), byte(remoteSID >> 24), byte(remoteSID >> 16), byte(remoteSID >> 8), byte(remoteSID), byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID)}) + send([]byte{0x10, 0x00, 0x00, 0x00, 0x00, 0x00, byte(gotSeq), byte(gotSeq >> 8), byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID), byte(remoteSID >> 24), byte(remoteSID >> 16), byte(remoteSID >> 8), byte(remoteSID)}) } if time.Since(lastPingAt) >= 100*time.Millisecond { - // Example request from PC: 0x15, 0x00, 0x00, 0x00, 0x07, 0x00, 0x09, 0x00, 0xbe, 0xd9, 0xf2, 0x63, 0xe4, 0x35, 0xdd, 0x72, 0x00, 0x78, 0x40, 0xf6, 0x02 - // Example reply from radio: 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x09, 0x00, 0xe4, 0x35, 0xdd, 0x72, 0xbe, 0xd9, 0xf2, 0x63, 0x01, 0x78, 0x40, 0xf6, 0x02 - _, err = rand.Read(randID[:]) - if err != nil { - log.Fatal(err) - } - send([]byte{0x15, 0x00, 0x00, 0x00, 0x07, 0x00, byte(sendSeq), byte(sendSeq >> 8), byte(remoteSID >> 24), byte(remoteSID >> 16), byte(remoteSID >> 8), byte(remoteSID), byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID), 0x01, - randID[0], randID[1], randID[2], randID[3]}) - send([]byte{0x10, 0x00, 0x00, 0x00, 0x03, 0x00, byte(sendSeq), byte(sendSeq >> 8), byte(remoteSID >> 24), byte(remoteSID >> 16), byte(remoteSID >> 8), byte(remoteSID), byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID)}) + sendPkt7(nil) + sendPkt3() sendSeq++ lastPingAt = time.Now() - if time.Since(lastLongPingAt) >= 30*time.Second { - // Example request from PC: 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0d, 0x00, - // 0xbb, 0x41, 0x3f, 0x2b, 0xe6, 0xb2, 0x7b, 0x7b, - // 0x00, 0x00, 0x00, 0x30, 0x01, 0x05, 0x00, 0x02, - // 0x00, 0x00, 0x5d, 0x37, 0x12, 0x82, 0x3b, 0xde, - // 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - // 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - // 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - // 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 - // Example reply from radio: 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, - // 0xe6, 0xb2, 0x7b, 0x7b, 0xbb, 0x41, 0x3f, 0x2b, - // 0x00, 0x00, 0x00, 0x30, 0x02, 0x05, 0x00, 0x02, - // 0x00, 0x00, 0x5d, 0x37, 0x12, 0x82, 0x3b, 0xde, - // 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - // 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - // 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - // 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 - send([]byte{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, byte(sendSeq), byte(sendSeq >> 8), byte(remoteSID >> 24), byte(remoteSID >> 16), byte(remoteSID >> 8), byte(remoteSID), byte(localSID >> 24), byte(localSID >> 16), byte(localSID >> 8), byte(localSID), - 0x00, 0x00, 0x00, 0x30, 0x01, 0x05, 0x00, byte(longPingSendSeq), - byte(longPingSendSeq >> 8), 0x00, authID[0], authID[1], authID[2], authID[3], authID[4], authID[5], - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}) - sendSeq++ - lastLongPingAt = time.Now() + if time.Since(lastReauthAt) >= 60*time.Second { + sendPktReauth() + authSendSeq++ + lastReauthAt = time.Now() + } + + if time.Since(lastStatusLog) >= 10*time.Second { + log.Print("still connected") + lastStatusLog = time.Now() } } }