1 files changed, 106 insertions, 0 deletions
diff --git a/src/udp.cpp b/src/udp.cpp
new file mode 100644
index 0000000..f54e60a
--- /dev/null
+++ b/src/udp.cpp
@@ -0,0 +1,106 @@
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <string.h>
+
+#include "udp.h"
+
+UDP_LISTENER::UDP_LISTENER() {
+  fd = -1;
+}
+
+UDP_LISTENER::~UDP_LISTENER() {
+  if( fd >= 0 )
+    close( fd );
+}
+
+STAT udp_open( UDP_LISTENER* udp, I32 port ) {
+  udp->fd = socket( AF_INET, SOCK_DGRAM, 0 );
+  if( udp->fd < 0 )
+    return STAT_ERR;
+
+  I32 one = 1;
+  setsockopt( udp->fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one) );
+
+  struct sockaddr_in a;
+  memset( &a, 0, sizeof( a ) );
+  a.sin_family      = AF_INET;
+  a.sin_addr.s_addr = htonl( INADDR_ANY );
+  a.sin_port        = htons( (U16)port );
+
+  if( bind( udp->fd, (struct sockaddr*)&a, sizeof(a) ) < 0 ) {
+    close( udp->fd );
+    udp->fd = -1;
+    return STAT_ERR;
+  }
+
+  fcntl( udp->fd, F_SETFL, O_NONBLOCK );
+  return STAT_OK;
+}
+
+void udp_parse( UDP_LISTENER* udp, TELEMETRY* t ) {
+  if( udp->fd < 0 )
+    return;
+
+  U8 buf[2048];
+  for( ;; ) {
+    ssize_t n = recv( udp->fd, buf, sizeof( buf ), 0 );
+    if( n < 0 )
+      break;     // EAGAIN
+    if( n < 20 )
+      continue;  // not a forza packet
+
+    I32 race;
+    F32 fmax, fidle, fcur;
+    memcpy( &race,  buf +  0, 4 );
+    memcpy( &fmax,  buf +  8, 4 );
+    memcpy( &fidle, buf + 12, 4 );
+    memcpy( &fcur,  buf + 16, 4 );
+
+    if( !( fmax >= 0.0f && fmax < 60000.0f ) )
+      continue;
+    if( !( fcur >= 0.0f && fcur < 60000.0f ) )
+      continue;
+
+    memcpy( &t->vel_x, buf + 28, 4 );
+    memcpy( &t->vel_y, buf + 32, 4 );
+    memcpy( &t->vel_z, buf + 40, 4 );
+
+    t->race_on  = race;
+    t->rpm_max  = fmax;
+    t->rpm_idle = fidle;
+    t->rpm      = fcur;
+    t->extended = n >= 324;
+
+    if( t->extended ) {
+      I32 dt;
+      memcpy( &t->pos_x,     buf + 244, 4 );
+      memcpy( &t->pos_y,     buf + 248, 4 );
+      memcpy( &t->pos_z,     buf + 252, 4 );
+      memcpy( &t->power,     buf + 260, 4 );
+      memcpy( &t->speed_ms,  buf + 256, 4 );
+      memcpy( &t->boost_psi, buf + 284, 4 );
+      memcpy( &dt,           buf + 224, 4 );
+      memcpy( &t->laptime,   buf + 304, 4 );
+      memcpy( &t->racetime,  buf + 308, 4 );
+      t->lapnum     = *(U16*)( &buf[312] );
+      t->racepos    = buf[314];
+      t->drivetrain = dt;
+      t->accel      = buf[315];
+      t->brake      = buf[316];
+      t->clutch     = buf[317];
+      t->handbrake  = buf[318];
+      t->gear       = buf[319];
+      t->norm_driving_line = buf[321];
+      t->norm_ai_brake_diff = buf[322];
+      memcpy( &t->car_ordinal, buf + 212, 4 );
+      for( I32 i = 0; i < 4; i++ ) {
+        memcpy( &t->slip_ratio[i],    buf +  84 + 4*  i, 4 );
+        memcpy( &t->combined_slip[i], buf + 180 + 4*  i, 4 );
+        memcpy( &t->tire_temp[i],      buf + 268 + 4*  i, 4 );
+      }
+    }
+    t->stamp = t_nows();
+  }
+}