简易someip服务发现SD报文演示

news2024/11/23 18:28:43

环境

$ cat /etc/os-release
PRETTY_NAME="Ubuntu 22.04.1 LTS"
NAME="Ubuntu"
VERSION_ID="22.04"
VERSION="22.04.1 LTS (Jammy Jellyfish)"
VERSION_CODENAME=jammy
ID=ubuntu
ID_LIKE=debian
HOME_URL="https://www.ubuntu.com/"
SUPPORT_URL="https://help.ubuntu.com/"
BUG_REPORT_URL="https://bugs.launchpad.net/ubuntu/"
PRIVACY_POLICY_URL="https://www.ubuntu.com/legal/terms-and-policies/privacy-policy"
UBUNTU_CODENAME=jammy

wireshark效果

sendto.c

$ cat sendto.c
#include <stdio.h>
#include <unistd.h>
#include <sys/types.h> /* See NOTES */
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

int gsiSockFd = -1;
struct sockaddr_in gstSDTargetAddr = {0};
struct sockaddr_in gstTargetAddr = {0};
struct sockaddr_in gstMyAddr = {0};
unsigned char gucOfferService0001Instance0001[] = {
0xff, 0xff, 0x81, 0x00, 0x00, 0x00, 0x00, 0x30,
0x00, 0x00, 0x02, 0x35, 0x01, 0x01, 0x02, 0x00,
0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10,
0x01, 0x00, 0x00, 0x10, 0x00, 0x01, 0x00, 0x01,
0x01, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x0c, 0x00, 0x09, 0x04, 0x00,
0xc0, 0xa8, 0x3c, 0x01, 0x00, 0x11, 0x77, 0x25
};

unsigned char gucSdSubAckService0001Instance0001[] = {
0xff, 0xff, 0x81, 0x00, 0x00, 0x00, 0x00, 0x30,
0x00, 0x00, 0x00, 0x9f, 0x01, 0x01, 0x02, 0x00,
0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10,
//0x07, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01,
0x07, 0x00, 0x00, 0x10, 0x00, 0x01, 0x00, 0x01,
0x01, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01,
0x00, 0x00, 0x00, 0x0c, 0x00, 0x09, 0x14, 0x00,
0xed, 0x3c, 0x00, 0x01, 0x00, 0x11, 0x77, 0x1a
};
unsigned char gucService0001Method8005[] = {0x00, 0x01, 0x80, 0x05, 0x00, 0x00, 0x00, 0x09, 0x00, 0x00, 0x00, 0x39, 0x01, 0x01, 0x02, 0x00, 0x01};

struct SomeipHead
{
   /* byte 0-3 */
   uint16_t ServiceID;
   uint16_t MethodID;
   /* byte 4-7 */
   uint32_t Length;
   /* byte 8-11 */
   uint16_t ClientID;
   uint16_t SessionID;
   /* byte 12-15 */
   uint8_t SomeipVersion;
   uint8_t InterfaceVersion;
   uint8_t MessageType;
   uint8_t ReturnCode;
   /* byte 16*/
   uint8_t payload[0];
};

void SOMEIP_ShowInfo(const struct SomeipHead *lHead)
{
   printf("\n");
   printf("ServiceID=%04X\n", ntohs(lHead->ServiceID));
   printf("MethodID=%04X\n", ntohs(lHead->MethodID));
   printf("Length=%08X\n", ntohl(lHead->Length));
   printf("ClientID=%04X\n", ntohs(lHead->ClientID));
   printf("SessionID=%04X\n", ntohs(lHead->SessionID));
   printf("SomeipVersion=%04X\n", lHead->SomeipVersion);
   printf("InterfaceVersion=%04X\n", lHead->InterfaceVersion);
   printf("MessageType=%04X\n", lHead->MessageType);
   printf("ReturnCode=%04X\n", lHead->ReturnCode);
   printf("\n");
}

int SOMEIP_SocketInit(void)
{
   int res = -1;

   gstTargetAddr.sin_family = AF_INET;
   gstTargetAddr.sin_port = htons(30491);
   gstTargetAddr.sin_addr.s_addr = inet_addr("127.0.0.1");

   gstSDTargetAddr.sin_family = AF_INET;
   gstSDTargetAddr.sin_port = htons(30490);
   gstSDTargetAddr.sin_addr.s_addr = inet_addr("237.60.0.1");

   gstMyAddr.sin_family = AF_INET;
   gstMyAddr.sin_port = htons(30490);
   gstMyAddr.sin_addr.s_addr = inet_addr("127.0.0.1");

   if((gsiSockFd = socket(AF_INET, SOCK_DGRAM, 0)) == -1)
   {
       perror("socket");
   }
   else if(bind(gsiSockFd,(struct sockaddr*)&gstMyAddr,sizeof(struct sockaddr)) == -1)
   {
       perror("bind");
   }
   else
   {
       res = 0;
   }

   in_addr_t lstMuticastAddr = inet_addr("127.0.0.1");

   if(setsockopt(gsiSockFd, IPPROTO_IP, IP_MULTICAST_IF,(char *)&lstMuticastAddr, sizeof(lstMuticastAddr)) == -1)
   {
       perror("setsockopt");
   }
   else
   {
       /* keep */
   }

   return res;
}

int main(int argc, char *argv[])
{
   if(SOMEIP_SocketInit())
   {
       /* keep */
   }
   else
   {
       /* offer service */
       sendto(gsiSockFd, &gucOfferService0001Instance0001[0], sizeof(gucOfferService0001Instance0001), 0, (struct sockaddr *)&gstSDTargetAddr, (socklen_t)sizeof(gstSDTargetAddr));
       sleep(1);
       /* subscribe ack */
       sendto(gsiSockFd, &gucSdSubAckService0001Instance0001[0], sizeof(gucSdSubAckService0001Instance0001), 0, (struct sockaddr *)&gstSDTargetAddr, (socklen_t)sizeof(gstSDTargetAddr));
       sleep(1);
       /* connected */
       while(1)
       {
           SOMEIP_ShowInfo(&gucService0001Method8005[0]);
           /* send notification */
           sendto(gsiSockFd, &gucService0001Method8005[0], sizeof(gucService0001Method8005), 0, (struct sockaddr *)&gstTargetAddr, (socklen_t)sizeof(gstTargetAddr));
           sleep(1);
       }
   }
   return 0;
}