add some more things

README.md

@@ -2,4 +2,5 @@
 
 _A proxy to encrypt the Traccar Freematics protocol._
 
+Inspired by previous work:
 https://github.com/rfhigler/Freematics/commit/25cf781ca9fecc3e3082348ce9d28e4d69ff7764

server/encryption/decrypt.go

@@ -17,8 +17,9 @@ func Decrypt(key, ciphertextMsg []byte) ([]byte, error) {
 		return nil, errors.New("ciphertext too short")
 	}
 
-	// Split nonce and ciphertext.
-	nonce, ciphertext, tag := ciphertextMsg[:nonceSize], ciphertextMsg[nonceSize:len(ciphertextMsg)-tagSize], ciphertextMsg[len(ciphertextMsg)-tagSize:]
+	// Split the message apart.
+	// The order is nonce, ciphertext, and tag. The last two aren't used.
+	nonce, _, _ := ciphertextMsg[:nonceSize], ciphertextMsg[nonceSize:len(ciphertextMsg)-tagSize], ciphertextMsg[len(ciphertextMsg)-tagSize:]
 
-	return aead.Open(nil, nonce, append(ciphertext, tag...), nil)
+	return aead.Open(nil, nonce, ciphertextMsg[nonceSize:], nil)
 }

server/encryption/encrypt.go

@@ -0,0 +1,24 @@
+package encryption
+
+import (
+	"crypto/rand"
+	"golang.org/x/crypto/chacha20poly1305"
+	"io"
+)
+
+func Encrypt(key, plaintextMsg []byte) ([]byte, error) {
+	aead, err := chacha20poly1305.New(key)
+	if err != nil {
+		return nil, err
+	}
+
+	// Generate a new nonce for this encryption.
+	nonce := make([]byte, aead.NonceSize())
+	if _, err := io.ReadFull(rand.Reader, nonce); err != nil {
+		return nil, err
+	}
+
+	// Encrypt the message and append the nonce and the ciphertext.
+	ciphertext := aead.Seal(nonce, nonce, plaintextMsg, nil)
+	return ciphertext, nil
+}

server/server.go

@@ -73,13 +73,53 @@ func main() {
 			}
 			defer conn.Close()
 
-			// Address to forward the decrypted messages
+			logger.Infof("Listening on 0.0.0.0:%s\n", port)
+
+			for {
+				buf := make([]byte, 1500) // 1500 is the standard internet MTU.
+				n, addr, err := conn.ReadFromUDP(buf)
+				if err != nil {
+					logger.Fatalf(formatLogMsg(addr.IP.String(), dest.Address, dest.Port, fmt.Sprintf("Error reading from UDP: %s", err)))
+				}
+
+				// Handle the message.
+				go func(addr *net.UDPAddr, buf []byte, n int) {
+					// Do the decryption.
+					var plaintext []byte
+					if len(buf[:n]) > 0 {
+						plaintext, err = encryption.Decrypt(key, buf[:n]) // Use only the part of the buffer that has data.
+						if err != nil {
+							rawHex := hex.EncodeToString(buf[:n])
+							logger.Warnf(formatLogMsg(addr.IP.String(), dest.Address, dest.Port, fmt.Sprintf(`Error decrypting message: %s. Length: %d, Raw: "%s"`, err, len(rawHex), rawHex)))
+							// Forward the raw message to the backend without bothering with decryption.
+							plaintext = buf[:n]
+						}
+					} else {
+						plaintext = buf[:n]
+					}
+
 					forwardAddr, err := net.ResolveUDPAddr("udp", fmt.Sprintf("%s:%d", dest.Address, dest.Port))
 					if err != nil {
 						logger.Fatalln("Error resolving forward address:", err)
 						return
 					}
 
+					// Create a new UDP address for listening to the backend server's response.
+					listenAddr, err := net.ResolveUDPAddr("udp", ":0") // Let the OS pick a free port.
+					if err != nil {
+						logger.Fatalln("Error resolving listen address:", err)
+						return
+					}
+
+					// Create a new UDP listener for the backend server's response.
+					listenConn, err := net.ListenUDP("udp", listenAddr)
+					if err != nil {
+						logger.Fatalln("Error listening for backend response:", err)
+						return
+					}
+					defer listenConn.Close()
+
+					// Dial the backend server without binding a local address.
 					forwardConn, err := net.DialUDP("udp", nil, forwardAddr)
 					if err != nil {
 						logger.Fatalln("Error dialing to forward address:", err)
@@ -87,29 +127,37 @@ func main() {
 					}
 					defer forwardConn.Close()
 
-			logger.Infof("Listening on 0.0.0.0:%s\n", port)
-
-			for {
-				buf := make([]byte, 1500) // 1500 is the standard internet MTU
-				n, addr, err := conn.ReadFromUDP(buf)
-				if err != nil {
-					logger.Fatalf(formatLogMsg(addr.IP.String(), dest.Address, dest.Port, fmt.Sprintf("Error reading from UDP: %s", err)))
-					continue
-				}
-
-				// Forward the decrypted message
-				go func(addr *net.UDPAddr, buf []byte, n int) {
-					plaintext, err := encryption.Decrypt(key, buf[:n]) // Use only the part of the buffer that has data
-					if err != nil {
-						logger.Errorf(formatLogMsg(addr.IP.String(), dest.Address, dest.Port, fmt.Sprintf("Error decrypting message: %s", err)))
-						return
-					}
-
+					// Forward the plaintext to the backend.
 					_, err = forwardConn.Write(plaintext)
 					if err != nil {
 						logger.Errorf(formatLogMsg(addr.IP.String(), dest.Address, dest.Port, fmt.Sprintf("Error forwarding message: %s", err)))
 						return
 					}
+
+					// Read the response from the backend.
+					backendResponse := make([]byte, 1500)
+					n, err = forwardConn.Read(backendResponse)
+					if err != nil {
+						logger.Errorf(formatLogMsg(addr.IP.String(), dest.Address, dest.Port, fmt.Sprintf("Error reading response from backend server: %s", err)))
+						return
+					}
+
+					fmt.Println(string(backendResponse[:]))
+
+					// Encrypt the backend's response.
+					encryptedBackendResponse, err := encryption.Encrypt(key, backendResponse[:n])
+					if err != nil {
+						logger.Errorf(formatLogMsg(addr.IP.String(), dest.Address, dest.Port, fmt.Sprintf("Error encrypting response: %s", err)))
+						return
+					}
+
+					// Forward the encrypted backend response to the client.
+					_, err = conn.WriteToUDP(encryptedBackendResponse, addr)
+					if err != nil {
+						logger.Errorf(formatLogMsg(addr.IP.String(), dest.Address, dest.Port, fmt.Sprintf("Error forwarding response to client: %s", err)))
+						return
+					}
+
 					logger.Infof(formatLogMsg(addr.IP.String(), dest.Address, dest.Port, string(plaintext)))
 				}(addr, buf, n)
 			}

test/test.py

@@ -13,7 +13,7 @@ sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
 message = ('Hello, Server! ' + str(time.time())).encode()
 
 # The key and nonce
-key = bytes.fromhex('example123')
+key = bytes.fromhex('d38a3b96a26d0b1139bd30c174884f5dbc8eaaf492493725633ecebfa4ab19e9')
 
 # Encrypt the message
 cipher = ChaCha20_Poly1305.new(key=key)

test/test_raw.py

@@ -0,0 +1,36 @@
+import socket
+import binascii
+
+from Crypto.Cipher import ChaCha20_Poly1305
+
+# Send an initalization message and decrypt the response.
+# Use this key: d38a3b96a26d0b1139bd30c174884f5dbc8eaaf492493725633ecebfa4ab19e9
+
+# The server's address and port
+server_address = ('localhost', 5171)
+
+sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+# Convert hex string to bytes
+ciphertext = binascii.unhexlify('40a7a1ef284e36e65cdb87abdb9aaea7ba4df5ae527b7311ba79a7d7f73729268d5b136c0c701fe366d775315f33e9ef893214fbf26a6ec281c8eadf46663b9d90')
+
+# Send the encrypted message to the server
+sock.sendto(ciphertext, server_address)
+
+# Receive the response from the server
+response, server = sock.recvfrom(4096)
+
+# Decrypt that response
+key = bytes.fromhex('d38a3b96a26d0b1139bd30c174884f5dbc8eaaf492493725633ecebfa4ab19e9')
+# ChaCha20_Poly1305 nonce size is 12 bytes and tag size is 16 bytes
+nonce = response[:12]
+ciphertext_and_tag = response[12:]
+ciphertext = ciphertext_and_tag[:-16]
+tag = ciphertext_and_tag[-16:]
+
+cipher = ChaCha20_Poly1305.new(key=key, nonce=nonce)
+plaintext = cipher.decrypt_and_verify(ciphertext, tag)
+
+print("Decrypted message: ", plaintext)
+
+sock.close()