C实现SSDP协议的设备发现及设备搜索

/*
* ===========================================================================
*
*       Filename:  ssdpServer.c
*    Description:  设备发现服务（自实现ssdp协议，获取USERNAME进行绑定）
*        Version:  1.0
*        Created:  08/29/2017 07:20:10 PM
*       Revision:  none
*       Compiler:  gcc
*         Author:   (syc),
*        Company:  xxxx
*
* ===========================================================================
*/

#include <sys/types.h>
#include <sys/select.h>
#include <sys/time.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <net/if.h>
#include <unistd.h>
#include <pthread.h>
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "utils/queue.h"
#include "utils/log.h"
#include "Discovery/ssdpServer.h"

#define SSDP_MCAST_ADDR ("239.255.255.250")
#define SSDP_PORT (1900)
#define M1_PORT (8200)

#define OS_VERSION "3.4.72-rt89"
#define SERVER_NAME "MiniDLNA"
#define MINIDLNA_VERSION "1.1.0"
#define MINIDLNA_SERVER_STRING  OS_VERSION " DLNADOC/1.50 UPnP/1.0 " SERVER_NAME "/" MINIDLNA_VERSION
#define ROOTDESC_PATH "/rootDesc.xml"

#define MAC_ADDR_LEN (16)
#define IP_ADDR_LEN (16)
#define MAXSIZE (1024)

queue_t *handle_queue = NULL;
pthread_t handle_ThreadID;
pthread_t handle_ThreadID1;

typedef struct _packet_{
char data[MAXSIZE];
int len;
int type;
}msg_packet_t;

eq_discovery_cb_t callback = {0};
//全局变量
struct sockaddr_in   addrin     ;
struct timeval       rtime      ;
int                ssdp_sock    ;
int                peer_listen  ;
int                peer_sock    ;
socklen_t          addrlen      ;
fd_set               fds        ;
int                  maxfdp     ;

char              buf[1024]     ;

//这两个参数根据业务需要而定
char              UUID[32] = {0};//D的唯一标识（MAC地址）
char              M1_IP[32] = {0}; //D的eth0.2的IP地址

static const char * const known_service_types[] =
{
"uuid:00000000-0000-0000-0000-000000000000",
"upnp:rootdevice",
"urn:schemas-upnp-org:device:MediaServer:",
"urn:schemas-upnp-org:service:ContentDirectory:",
"urn:schemas-upnp-org:service:ConnectionManager:",
"urn:microsoft.com:service:X_MS_MediaReceiverRegistrar:",
0
};
//获取IP地址
int get_lan_ip(unsigned char *ipaddr)
{
struct sockaddr_in *addr;
struct ifreq ifr;
int sockfd;

//char *name = "br-lan";
char *name = "eth0.2";//eth0.2的IP地址
if( strlen(name) >= IFNAMSIZ)
return -1;

strcpy( ifr.ifr_name, name);
sockfd = socket(AF_INET,SOCK_DGRAM,0);

//get ipaddr
if(ioctl(sockfd, SIOCGIFADDR, &ifr) == -1)
{
close(sockfd);
return -1;
}

addr = (struct sockaddr_in *)&(ifr.ifr_addr);

//memcpy(ipaddr, &addr->sin_addr, IP_ADDR_LEN);
strcpy(ipaddr, inet_ntoa(addr->sin_addr));
//printf("get_lan_ip: %d.%d.%d.%d\n",ipaddr[0], ipaddr[1], ipaddr[2], ipaddr[3]);
printf("get_lan_ip: %s\n",ipaddr);

close(sockfd);
return 0;
}
//获取MAC地址
int get_lan_mac(char *macaddr)
{
struct sockaddr_in *addr;
struct ifreq ifr;
int sockfd;

char *name = "br-lan";
if( strlen(name) >= IFNAMSIZ)
return -1;

strcpy( ifr.ifr_name, name);
sockfd = socket(AF_INET,SOCK_DGRAM,0);

//get HWaddr
if(ioctl(sockfd, SIOCGIFHWADDR, &ifr) == -1)
{
close(sockfd);
return -1;
}

memcpy(macaddr, ifr.ifr_hwaddr.sa_data, MAC_ADDR_LEN);
printf("get_lan_mac: %02X:%02X:%02X:%02X:%02X:%02X\n",macaddr[0], macaddr[1], macaddr[2], macaddr[3], macaddr[4], macaddr[5]);

close(sockfd);
return 0;
}
//字符串转化
static char HEX_DIGITS[] = "0123456789ABCDEF";
char* bintohex(const void *s, int slen, char *out/*=NULL*/, int olen)
{
int i=0, j=0;
int outlen = (slen<<1)+1;
const unsigned char *in = s;

if(out)
{
while(slen>0 && olen<outlen)
{
outlen = ((--slen)<<1)+1;
}
}
else
{
out = (char*)malloc(outlen);
}

for (i = 0, j = 0; i < slen; i++)
{
out[j++] = HEX_DIGITS[(in[i] >> 4) & 0x0F];
out[j++] = HEX_DIGITS[ in[i] & 0x0F];
}
out[outlen-1] = 0;

return out;
}

long GetCurrentSecond()
{
struct timeval timeofday;
struct timezone tz;

gettimeofday(&timeofday , &tz);

return timeofday.tv_sec;
}

//TCP通道获取USERNAME
int GetPeerListenSocket(unsigned short port)
{
int s;
int i = 1;
struct sockaddr_in listenname;

s = socket(PF_INET, SOCK_STREAM, 0);
if (s < 0)
{
return -1;
}

if (setsockopt(s, SOL_SOCKET, SO_REUSEADDR, &i, sizeof(i)) < 0)

memset(&listenname, 0, sizeof(struct sockaddr_in));
listenname.sin_family = AF_INET;
listenname.sin_port = htons(port);
listenname.sin_addr.s_addr = htonl(INADDR_ANY);

if (bind(s, (struct sockaddr *)&listenname, sizeof(struct sockaddr_in)) < 0)
{
close(s);
return -1;
}

if (listen(s, 6) < 0)
{
close(s);
return -1;
}

return s;
}

//alive心跳包，目前一次单纯的设备发现流程，不需要
int handle_alive(int udp_sock,struct sockaddr_in client,socklen_t addrlen)
{
int count = 0;
int i = 0;
char buf[512] = {0};

while(count < 1)
{
snprintf(buf, sizeof(buf),
"NOTIFY * HTTP/1.1\r\n"
"HOST:%s:%d\r\n"
"CACHE-CONTROL:max-age=%u\r\n"
"LOCATION:http://%s:%d" ROOTDESC_PATH"\r\n"
"SERVER: " MINIDLNA_SERVER_STRING "\r\n"
"NT:upnp:rootdevice\r\n"
"USN:uuid:%s::upnp:rootdevice\r\n"
"NTS:ssdp:alive\r\n"
"\r\n",
SSDP_MCAST_ADDR, SSDP_PORT,
(895<<1)+10,
M1_IP, M1_PORT,
UUID);
{
INFO("ssdp:alive send ==:%s\n",buf);
sendto(udp_sock,buf,strlen(buf),0,(struct sockaddr*)&client,addrlen);
}
count ++;
}

return 0;
}
//回复客户端的ssdp:discovery信息
int  handle_discover(int udp_sock,struct sockaddr_in client,socklen_t addrlen)
{

int   i  = 0;
int   count  = 0;
char buf[512] , tmstr[30];
time_t tm = time(NULL);
strftime(tmstr, sizeof(tmstr), "%a, %d %b %Y %H:%M:%S GMT", gmtime(&tm));
//Get IP Address

while(count < 1)
{
snprintf(buf, sizeof(buf), "HTTP/1.1 200 OK\r\n"
"CACHE-CONTROL: max-age=%u\r\n"
"DATE: %s\r\n"
"ST:uuid:%s\r\n"
"USN:uuid:%s\r\n"
"EXT:\r\n"
"SERVER: " MINIDLNA_SERVER_STRING "\r\n"
"LOCATION: http://%s:%u" ROOTDESC_PATH "\r\n"
"Content-Length: 0\r\n"
"\r\n",
(895 << 1) + 10,
tmstr,
UUID,// Mac Addres
UUID,// Mac Addres
M1_IP,M1_PORT);
{
//INFO("handle discover send ==:%s\n",buf);
sendto(udp_sock,buf,strlen(buf),0,(struct sockaddr*)&client,addrlen);

}
count ++;
}
return 0;

}

//获取USERNAME
int handle_peer(int tcp_sock,char*buf) {

//互斥锁
char name[32] = {0} ;
char *s = strstr(buf,"USERNAME:");
char *e = strstr(buf,":END");
memcpy(name,s+9,e-s-9);

callback.on_get_username(name);

close(tcp_sock);
INFO("Get username done %s",name);

}

//ssdp主线程：select轮询udp和tcp文件句柄
void *ssdp_discovery(void *data)
{
int ret     = -1               ;
struct timeval timeout  ;
addrlen    = sizeof(addrin)    ;
pthread_detach(pthread_self()) ;

//----------------------------------------------------------------------------//
bzero(&addrin, sizeof(addrin));
addrin.sin_family = AF_INET;
//addrin.sin_addr.s_addr = inet_addr("0.0.0.0"); //htonl(INADDR_ANY)
addrin.sin_addr.s_addr = inet_addr("239.255.255.250"); //htonl(INADDR_ANY)
addrin.sin_port = htons(1900);

ssdp_sock=socket(AF_INET,SOCK_DGRAM,0);
if( ssdp_sock < 0) {perror("1"); return NULL;}

struct ip_mreq mreq;
bzero(&mreq,sizeof(mreq));
mreq.imr_multiaddr.s_addr =inet_addr("239.255.255.250");
mreq.imr_interface.s_addr =htonl(INADDR_ANY);
setsockopt(ssdp_sock, IPPROTO_IP,IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) ;

ret=bind(ssdp_sock, (struct sockaddr *)&addrin, sizeof(addrin));
if( ret < 0 )   {perror("2"); return NULL;}

//----------------------------------------------------------------------------//

peer_listen = GetPeerListenSocket(M1_PORT);//接收USERNAME的TCP句柄

//----------------------------------------------------------------------------//
long time_record = GetCurrentSecond();
msg_packet_t msg;

while(1)
{
//alive心跳包暂不需要
//if((GetCurrentSecond() - time_record) > 20) {
//  handle_alive(ssdp_sock,addrin,addrlen);
//  time_record = GetCurrentSecond();
//}

FD_ZERO(&fds);
FD_SET(ssdp_sock,&fds);

FD_SET(peer_listen,&fds);

timeout.tv_sec = 3;//如果要做超时，每次都要重新设置

timeout.tv_usec = 0;
maxfdp = ssdp_sock > peer_listen ? ssdp_sock + 1 : peer_listen + 1;

ret = select(maxfdp,&fds,NULL,NULL,&timeout);
//INFO("ret == %d\n",ret);
switch (ret) {
case -1:
{
INFO("Select Error\n");
abort();
break;
}
case 0:
{
//INFO("Time out\n");
break;
}
default:
{

memset(&msg,0,sizeof(msg_packet_t));
if(FD_ISSET(ssdp_sock,&fds) > 0) {
//接收UDP组播消息：ssdp消息
int num = recvfrom(ssdp_sock,&msg.data,MAXSIZE,0,(struct sockaddr *)&addrin,&addrlen);
if(0 < num) {
msg.len = num;
msg.type = 0; //ssdp messages
msg.data[num] = '\0';
//INFO("Get ssdp message %s\n",msg.data);
queue_write(handle_queue,&msg,sizeof(msg_packet_t));
}
} else if(FD_ISSET(peer_listen,&fds) > 0) {
//接收TCP通道数据：USERNAME
peer_sock = accept(peer_listen,(struct sockaddr *)&addrin,&addrlen);
int num = recv(peer_sock,&msg.data,MAXSIZE,0);
if(0 < num) {
msg.len = num;
msg.type = 1; //peer messages
msg.data[num] = '\0';
//INFO("Get peer message %s\n",msg.data);
queue_write(handle_queue,&msg,sizeof(msg_packet_t));
}

}

break;
}
}

}

close(ssdp_sock);
close(peer_listen);
close(peer_sock);

return NULL;
}

//消息处理线程：分别处理UDP和TCP
void *handle_MSG(void*data) {

pthread_detach(pthread_self());
int ret = -1;
msg_packet_t MSG;

while(1) {

memset(&MSG, 0, sizeof(msg_packet_t));
ret = queue_read(handle_queue,&MSG,sizeof(msg_packet_t));
if(0 != ret) {
break;
}

if(NULL != strstr(MSG.data,"M-SEARCH") && 0 == MSG.type && NULL != strstr(MSG.data,UUID)) {
INFO("message of ssdp %s\n",MSG.data);
handle_discover(ssdp_sock,addrin,addrlen);

} else if (NULL != strstr(MSG.data,"NOTIFY") && 0 == MSG.type) {
//INFO("message of ssdp %s\n",MSG.data);

}else if(NULL != strstr(MSG.data,"M-SEARCH") && 0 == MSG.type && NULL != strstr(MSG.data,"0000000000000000")){

handle_alive(ssdp_sock,addrin,addrlen,uuid,ip);//消息0000000000000000为客户端请求MAC地址，用于客户端设备搜索接口

}else if (1 == MSG.type && NULL != strstr(MSG.data,"USERNAME:")){
INFO("message of peer %s\n",MSG.data);
handle_peer(peer_sock,MSG.data);

} else {

}
usleep(30*1000);

}

return NULL;
}

//初始化
int Init_DeviceDiscovery (eq_discovery_cb_t *cb) {
if(NULL == cb) {
return -1;
}

int ret = -1;
char mac[16] = {0};
memcpy(&callback,cb,sizeof(eq_discovery_cb_t));

//获取设备eth0.2的IP和UUID
if(0 != get_lan_ip(M1_IP)){
INFO("get ip failed\n");
return -1;
}
if(0 != get_lan_mac(mac)){
INFO("get mac failed\n");
return -1;
}
//UUID由MAC地址补“0000”得到
bintohex(mac,6,UUID,sizeof(UUID));
strcpy(&UUID[12],"0000");

INFO("Device ip :%s uuid: %s\n",M1_IP,UUID);

ret = queue_create(&handle_queue,sizeof(msg_packet_t),100);
if(0 != ret) {
INFO("queue create failed\n");
return -1;
}

//消息处理线程：分别处理UDP和TCP
ret = pthread_create(&handle_ThreadID,NULL,handle_MSG,NULL);
if(0 != ret) {
INFO("handle Msg phread create failed\n");
return -1;
}

//ssdp主线程：select轮询udp和tcp文件句柄
ret = pthread_create(&handle_ThreadID1,NULL,ssdp_discovery,NULL);
if(0 != ret) {
INFO("ssdp_discovery phread create failed\n");
return -1;
}
INFO("Init DeviceDiscovery done\n");
return 0;

}