alesi2000

ASAP2DoIPExample

Aug 1st, 2021 (edited)
1,172
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. using jnsoft.Diagnose.Comm.UDS;
  2. using jnsoft.Helpers;
  3. using System;
  4. using System.Collections.Generic;
  5. using System.Net;
  6.  
  7. namespace jnsoft.Diagnose.Comm.DoIp.Examples
  8. {
  9.   /// <summary>
  10.   /// ASAP2 DoIP Example.
  11.   ///
  12.   /// This example collects DoIP Vehicle announcements on the specified local IPAddress.
  13.   /// - A DoIP Client is created to access a DoIP gateway
  14.   /// - Two UDS client are created to request TesterPresent commands
  15.   ///
  16.   /// Usage: ASAP2DoIPExample 192.168.0.2
  17.   /// </summary>
  18.   class Program
  19.   {
  20.     static readonly List<UDSClient> UDSClients = new List<UDSClient>();
  21.     static IPAddress LocalIp;
  22.     static IPAddress RemoteIp;
  23.  
  24.     static int Main(string[] args)
  25.     {
  26.       var orgColor = Console.ForegroundColor;
  27.       try
  28.       {
  29.         Console.ForegroundColor = ConsoleColor.White;
  30.         if (args.Length < 1)
  31.         { // no args -> present usage
  32.           var appName = Extensions.AppName;
  33.           Console.WriteLine($"{appName}({Extensions.AppVersion})");
  34.           Console.WriteLine("UDS over DoIP example...");
  35.           Console.WriteLine("Required input: the IP address of the local network to use");
  36.           Console.WriteLine($"Usage: {appName} Local_IPV4_or_IPV6_Address [Remote_IPV4_or_IPV6_Address]");
  37.           Console.WriteLine($"Example: {appName} 192.168.0.2");
  38.           return -1;
  39.         }
  40.  
  41.         LocalIp = IPAddress.Parse(args[0]);
  42.  
  43.         if (args.Length > 1)
  44.         { // try to connect to remote DoIP √§entity
  45.           RemoteIp = args.Length > 1 ? IPAddress.Parse(args[1]) : null;
  46.           var gateway = new DoIPClient(RemoteIp, LocalIp, Activation.Default, DoIPClient.DefaultTesterAdr, DoIPClient.DefaultEntityAdr);
  47.           Console.WriteLine($"DoIP client successfully connected to Server:{gateway.RemoteEp}");
  48.           CreateUDSClients(gateway);
  49.         }
  50.         else
  51.         { // wait for vehicle announcement
  52.           DoIPClient.EntityAnnounced += DoIPClient_EntityAnnounced;
  53.           DoIPClient.StartListenToEntityAnnouncement(LocalIp);
  54.           Console.WriteLine($"Listening on {LocalIp} for vehicle announcements");
  55.         }
  56.  
  57.         Console.WriteLine($"'T' to execute TesterPresent for created UDS Clients..., 'Q' to quit");
  58.  
  59.         MsgState udsState;
  60.         bool quitPressed = false;
  61.         while (!quitPressed)
  62.         {
  63.           var ci = Console.ReadKey(true);
  64.           switch (ci.Key)
  65.           {
  66.             case ConsoleKey.Q: quitPressed = true; break;
  67.  
  68.             case ConsoleKey.T:
  69.               lock (UDSClients)
  70.               {
  71.                 Console.ForegroundColor = ConsoleColor.White;
  72.                 Console.WriteLine($"Requesting TesterPresent on {UDSClients.Count} UDS clients...");
  73.                 foreach (var udsClient in UDSClients)
  74.                 {
  75.                   if (MsgState.Success == (udsState = udsClient.TesterPresent(out RespSFBase udsResponse)))
  76.                   {
  77.                     Console.ForegroundColor = ConsoleColor.White;
  78.                     Console.WriteLine($"{udsClient.DoIPClient.SrcAdr:X} successfully received a Tester present response from {udsClient.DoIPClient.DstAdr:X} ({udsClient.DoIPClient.RemoteEp})");
  79.                   }
  80.                   else
  81.                   {
  82.                     Console.ForegroundColor = ConsoleColor.Yellow;
  83.                     Console.WriteLine($"Failed to receive Tester present request ({udsState})");
  84.                   }
  85.                 }
  86.               }
  87.               break;
  88.           }
  89.         }
  90.         return 0;
  91.       }
  92.       catch (Exception ex)
  93.       {
  94.         Console.ForegroundColor = ConsoleColor.Red;
  95.         Console.WriteLine($"Something failed!\nDetails:\n{ex.Message}");
  96.         return -2;
  97.       }
  98.       finally
  99.       { // cleanup
  100.         DoIPClient.StopListenToEntityAnnouncement();
  101.  
  102.         foreach (var udsClient in UDSClients)
  103.           udsClient.Dispose();
  104.  
  105.         Console.ForegroundColor = orgColor;
  106.       }
  107.     }
  108.  
  109.     static void DoIPClient_EntityAnnounced(object sender, AnnoncementEventArgs e)
  110.     {
  111.       var vehicleId = e.Entity.VehicleId;
  112.       var status = e.Entity.EntityStatus;
  113.       var mode = e.Entity.PowerMode;
  114.       Console.WriteLine($"DoIP Vehicle announced from Server:{vehicleId.Frame.Sender} Protocolversion:{vehicleId.Frame.Version}");
  115.       Console.WriteLine($"Logical Address:{vehicleId.LogicalAdr:X} VIN:{vehicleId.VIN}, EID:{BitConverter.ToString(vehicleId.EID)}, Type:{status.NodeType}, PowerMode:{mode.PowerMode}");
  116.  
  117.       switch (status.NodeType)
  118.       {
  119.         case NodeType.Gateway:
  120.           var gateway = e.Entity.Client;
  121.           CreateUDSClients(gateway);
  122.           break;
  123.       }
  124.     }
  125.  
  126.     static void CreateUDSClients(DoIPClient gateway)
  127.     {
  128.       for (int i = 1; i <= 2; ++i)
  129.       {
  130.         Console.ForegroundColor = ConsoleColor.White;
  131.  
  132.         // Create DoIP Client
  133.         var srcAdr = (ushort)(gateway.SrcAdr + i);
  134.         var dstAdr = (ushort)(gateway.DstAdr + i);
  135.         var doIpClient = new DoIPClient(gateway.RemoteEp.Address, gateway.LocalIp, Activation.Default, srcAdr, dstAdr);
  136.  
  137.         ResponseState state;
  138.         if (ResponseState.Ok == (state = doIpClient.RoutingActivation(out RoutingActivationResponse activation)))
  139.         {
  140.           Console.WriteLine($"Routing activation result: {activation.ActivationResult}");
  141.           switch (activation.ActivationResult)
  142.           {
  143.             case ActivationCode.RoutingActivated:
  144.               lock (UDSClients)
  145.                 UDSClients.Add(new UDSClient(doIpClient, 200));
  146.               break;
  147.             default:
  148.               Console.ForegroundColor = ConsoleColor.Yellow;
  149.               Console.WriteLine($"Routing activation denied with ({activation.ActivationResult})");
  150.               break;
  151.           }
  152.         }
  153.         else
  154.         {
  155.           Console.ForegroundColor = ConsoleColor.Yellow;
  156.           Console.WriteLine($"Failed to request Routing activation ({state})");
  157.         }
  158.       }
  159.     }
  160.   }
  161. }
  162.  
RAW Paste Data