int32_t CMessage::Encode(char* out_str, int32_t& out_len) const
{
char* str = out_str;
int32_t head_len = out_len;
if (m_iMessageHead == NULL)
{
TRACE_WARN("Head is NULL when encode message.");
return fail;
}
int32_t result = m_iMessageHead->Encode(str, head_len);
if (result == fail)
{
TRACE_WARN("encode messgae head failed.");
return fail;
}
int32_t body_len = out_len - head_len;
if (m_iMessageBody == NULL)
{
TRACE_WARN("Body is NULL when encode message.");
return fail;
}
result = m_iMessageBody->Encode(&str[head_len], body_len);
if (result == fail)
{
TRACE_WARN("encode messgae body failed.");
return fail;
}
out_len = head_len + body_len;
return success;
}
/**
Remove a video frame from the queue
*/
FrameInfo* CMediaQueue::get()
{
FrameInfo* frame = NULL;
if (count < 1)
{
TRACE_WARN("No frames in queue, waiting");
WaitForSingleObject(hRecvEvent, 500);
}
ResetEvent(hRecvEvent);
WaitForSingleObject(hFrameListLock,INFINITE);
if(count > 0)
{
frame = readpos->frame;
readpos->frame = NULL;
readpos = readpos->next;
count--;
}else{
TRACE_ERROR("No frames in queue");
}
ReleaseMutex(hFrameListLock);
return(frame);
}
void CMessage::SetMessageBody(CMessageBody* body)
{
m_iMessageBody = body;
::google::protobuf::Message* pb = m_iMessageBody->GetPB();
if (pb == NULL)
{
TRACE_WARN("pb is NULL when set Message Body");
return;
}
if (m_iMessageHead == NULL)
{
TRACE_WARN("msg head is NULL when set Message Body");
return;
}
m_iMessageHead->SetLen(m_iMessageHead->Size() + pb->ByteSize());
srand(time(NULL));
m_iMessageHead->SetMsq((int32_t)(time(NULL) << 16) | (1 + rand()));
}
int32_t CMessageBody::Decode(const char* in_str, int32_t in_len)
{
if (!message->ParseFromArray(in_str, in_len))
{
TRACE_WARN("parse message from array failed.");
return fail;
}
return success;
}
int32_t CFrameRegister::RegistFrame(int32_t msgID, CFrameBase* frame)
{
if (msgID_to_Frame.find(msgID) != msgID_to_Frame.end())
{
TRACE_WARN("regist the same msgID(%d) with different frame", msgID);
return fail;
}
msgID_to_Frame[msgID] = frame;
return success;
}
int CJudgeGPP::Compile()
{
//g++ Main.cpp -o Main -fno-asm -Wall -lm --static -std=c++0x -DONLINE_JUDGE
const char * CP[] = { "g++", "Main.cpp", "-o", "Main", "-fno-asm",
"-lm", "--static", "-std=c++0x", "-DONLINE_JUDGE", NULL };
if (!CJudgeBase::Compile(CP))
{
TRACE_WARN("an error when compile.");
return -1;
}
return result;
}
bool CJudgeGPP::Initialize(const Record& record)
{
ClearFiles();
// initial required fields
if (!InitializeFields(record, false))
{
TRACE_WARN("initial fields failed!\n");
return false;
}
// initial data path
if (!InitializeDataPath(record.problem_id))
{
TRACE_WARN("initial data path failed!\n");
return false;
}
// clear all file in judge path
if (!GenerateMain("Main.cpp"))
{
TRACE_WARN("Generate Main.cpp failed!\n");
return false;
}
return true;
}
THD_FUNCTION(moduleSAT, arg) {
// Print infos
module_params_t* parm = (module_params_t*)arg;
TRACE_INFO("SAT > Startup module SATELLITE");
TRACE_MODULE_INFO(parm, "SAT", "SATELLITE");
systime_t time = chVTGetSystemTimeX();
while(true)
{
TRACE_INFO("SAT > Do module SATELLITE cycle");
TRACE_WARN("SAT > Module SATELLITE not implemented"); // FIXME
time = chThdSleepUntilWindowed(time, time + S2ST(parm->cycle)); // Wait until time + cycletime
}
}
int32_t CMessage::Decode(const char* in_str, int32_t in_len)
{
if (m_iMessageHead == NULL)
{
m_iMessageHead = new CMessageHead();
}
int32_t head_len = m_iMessageHead->Size();
if (fail == m_iMessageHead->Decode(in_str, head_len))
{
TRACE_WARN("decode message head fail.");
delete m_iMessageHead;
return fail;
}
if (m_iMessageBody == NULL)
{
m_iMessageBody = NewRequestBodyWithMsgID(m_iMessageHead->GetMid());
if (m_iMessageBody == NULL)
{
TRACE_WARN("can not construct MessageBody with msgID(%d), " \
"please make sure your PB has been registed on this id", m_iMessageHead->GetMid());
delete m_iMessageHead;
delete m_iMessageBody;
return fail;
}
}
int32_t body_len = in_len - head_len;
if (fail == m_iMessageBody->Decode(&in_str[head_len], body_len))
{
TRACE_WARN("decode message body fail.");
delete m_iMessageHead;
delete m_iMessageBody;
return fail;
}
return success;
}
int32_t CMessageHead::Decode(const char* in_str, int32_t in_len)
{
if (in_len < Size())
{
TRACE_WARN("char array length is too short when decode message head");
return fail;
}
int32_t len = 0;
const char* str = in_str;
m_iMessageLen = DecodeInt32(str);
m_iUin = DecodeInt32(str);
m_iMessageID = DecodeInt32(str);
m_iMessageSequece = DecodeInt32(str);
return success;
}
NS_LS_BEGIN
int32_t CMessageHead::Encode(char* out_str, int32_t& out_len) const
{
if (out_len < Size())
{
TRACE_WARN("char array length too short when Encode message head.");
return fail;
}
char* str = out_str;
int32_t len = 0;
len += EncodeInt32(str, m_iMessageLen);
len += EncodeInt32(str, m_iUin);
len += EncodeInt32(str, m_iMessageID);
len += EncodeInt32(str, m_iMessageSequece);
out_len = len;
return success;
}
/**
* @brief Receives data from socket
*
* @param sock_fd Socket Descriptor on which to receive data
* @param sock_type Type of socket to determine method of receiving
* @param *msg_buffer Message Buffer into which data must be written
* @param length Length of buffer
* @param **src_addr A @c SOCKADDR type of structure where incoming connection
* parameters will be written if connection is Datagram based.
*
* @return Total bytes read from the socket
*/
int32_t hm_tprt_recv_on_socket(uint32_t sock_fd , uint32_t sock_type,
BYTE * msg_buffer, uint32_t length, SOCKADDR **src_addr)
{
/***************************************************************************/
/* Local variables */
/***************************************************************************/
int32_t total_bytes_rcvd=0;
uint32_t bytes_rcvd=0;
int32_t os_error;
int32_t op_complete = FALSE;
BYTE *buf = msg_buffer;
SOCKADDR_IN * ip_addr = NULL;
extern fd_set hm_tprt_conn_set;
socklen_t len = sizeof(SOCKADDR);
TRACE_ENTRY();
TRACE_ASSERT(msg_buffer != NULL);
/***************************************************************************/
/* Now try to receive data */
/***************************************************************************/
if(src_addr != NULL)
{
*src_addr = NULL;
}
do
{
TRACE_DETAIL(("Try to receive %d bytes on Socket %d", (length - bytes_rcvd), sock_fd));
if(sock_type==HM_TRANSPORT_SOCK_TYPE_TCP)
{
TRACE_DETAIL(("TCP Socket"));
bytes_rcvd = recv(sock_fd,
buf,
(length - bytes_rcvd),
0);
}
else if(sock_type==HM_TRANSPORT_SOCK_TYPE_UDP)
{
TRACE_DETAIL(("UDP Socket"));
ip_addr = (SOCKADDR_IN*)malloc(sizeof(SOCKADDR_IN));
if(ip_addr == NULL)
{
TRACE_ERROR(("Error allocating memory for incoming Address"));
op_complete = TRUE;
bytes_rcvd = 0;
break;
}
bytes_rcvd = recvfrom(sock_fd,
buf,
(length - bytes_rcvd),
0,
(struct sockaddr *)ip_addr ,
&len);
/***************************************************************************/
/* Do we need to fetch the sender IP information too, from the socket? */
/* or will they tell it themselves? */
/* In some upper layer structure? */
/***************************************************************************/
//FIXME: So far, they're not telling, so we need to get it here.
{
char net_addr[128];
int32_t length = 128;
inet_ntop(AF_INET, &ip_addr->sin_addr, net_addr, length);
TRACE_DETAIL(("%s:%d", net_addr, ntohs(ip_addr->sin_port)));
}
}
if(bytes_rcvd == length)
{
TRACE_DETAIL(("Message received in full"));
#if 0
#ifdef I_WANT_TO_DEBUG
{
uint32_t debug_length;
TRACE_INFO(("Buffer:"));
for(debug_length=0; debug_length<length; debug_length++)
{
TRACE_INFO(("%02X", *((unsigned char *)buf+debug_length)));
}
}
#endif
#endif
buf = NULL;
op_complete = TRUE;
total_bytes_rcvd +=bytes_rcvd;
break;
}
else if (bytes_rcvd == -1)
{
/***********************************************************************/
/* An error was returned - check for retryable socket error values. */
/***********************************************************************/
TRACE_PERROR(("Recv failed on socket"));
os_error = errno;
if ((os_error == EWOULDBLOCK))
{
/*********************************************************************/
/* This seems to be a flow control condition - clear the bytes sent */
/* value to show that no data was written. */
/*********************************************************************/
TRACE_WARN(("Resource shortage - try again"));
bytes_rcvd = 0;
}
else if (os_error == EINTR)
{
TRACE_WARN(("Receive interrupted - loop round again"));
bytes_rcvd = 0;
}
else
{
/*********************************************************************/
/* Socket failed so work source needs to be unregistered. */
/*********************************************************************/
TRACE_ERROR(("Socket failed"));
FD_CLR(sock_fd, &hm_tprt_conn_set);
op_complete = TRUE;
bytes_rcvd = 0;
break;
}
}
else if(bytes_rcvd == 0)
{
TRACE_WARN(("The peer has disconnected"));
op_complete = TRUE;
FD_CLR(sock_fd, &hm_tprt_conn_set);
total_bytes_rcvd = HM_ERR;
break;
}
else
{
TRACE_INFO(("%d bytes received. Returning.", bytes_rcvd));
op_complete = TRUE;
break;
}
/***************************************************************************/
/* Advance current pointer to remaining part of data */
/***************************************************************************/
buf += bytes_rcvd;
total_bytes_rcvd +=bytes_rcvd;
} while(total_bytes_rcvd< length);
if(op_complete == TRUE)
{
if(total_bytes_rcvd == length)
{
TRACE_DETAIL(("Message received in full"));
buf = NULL;
}
else
{
TRACE_WARN(("Some error happened"));
buf = NULL;
total_bytes_rcvd = bytes_rcvd;
}
}
#ifdef I_WANT_TO_DEBUG
if(ip_addr != NULL)
{
if(src_addr== NULL)
{
TRACE_WARN((
"UDP/Multicast UDP port was specified but address to fill sender addr was not given"
));
}
TRACE_ASSERT(src_addr!=NULL);
}
#endif
if(src_addr != NULL)
{
*src_addr = (SOCKADDR *)ip_addr;
}
TRACE_EXIT();
return (total_bytes_rcvd);
} /* hm_tprt_recv_on_socket */
/**
* @brief Send Message on Socket
*
* @param *ip a @t< struct sockaddr @t type of structure to which data must be sent.
* @param sock_fd Socket Descriptor on which to send data
* @param sock_type Type of socket to determine sending mechanism
* @param *msg_buffer A byte buffer to be sent
* @param length Length of data to be sent from @p msg_buffer
*
* @return Total number of bytes sent
*/
int32_t hm_tprt_send_on_socket(struct sockaddr* ip,int32_t sock_fd,
uint32_t sock_type,BYTE *msg_buffer, uint32_t length )
{
/***************************************************************************/
/* Local variables */
/***************************************************************************/
int32_t total_bytes_sent=0;
int32_t bytes_sent=0;
int32_t success = FALSE;
BYTE *buf = NULL;
int32_t os_error;
#ifdef I_WANT_TO_DEBUG
char temp[129];
#endif
TRACE_ENTRY();
TRACE_DETAIL(("Socket type %d", sock_type));
TRACE_DETAIL(("Attempt to send %d bytes of data on socket %d",
length, sock_fd));
buf = msg_buffer;
do
{
if((sock_type== HM_TRANSPORT_TCP_IN) || (sock_type==HM_TRANSPORT_TCP_OUT))
{
TRACE_DETAIL(("data to be sent on TCP connection "));
bytes_sent = send(sock_fd,
buf,
(length - bytes_sent),
0);
}
else if(sock_type==HM_TRANSPORT_UDP)
{
TRACE_DETAIL(("Data to be sent on UDP Ucast connection "));
bytes_sent = sendto(sock_fd,
buf,
(length - bytes_sent),
0,
ip,
sizeof(struct sockaddr));
}
else if(sock_type==HM_TRANSPORT_MCAST)
{
TRACE_DETAIL(("Data to be sent on UDP Mcast connection "));
#ifdef I_WANT_TO_DEBUG
{
char address_value[128];
HM_SOCKADDR_UNION addr;
socklen_t addrlen = sizeof(addr);
getsockname(sock_fd, &addr.sock_addr, &addrlen);
inet_ntop(addr.in_addr.sin_family,
&addr.in_addr.sin_addr,
address_value, sizeof(address_value));
TRACE_INFO(("Address value: %s:%d",address_value,
ntohs(addr.in_addr.sin_port)));
TRACE_INFO(("Family: %d", addr.in_addr.sin_family));
}
TRACE_DETAIL(("Send to %s:%d",
inet_ntop(((SOCKADDR_IN *)ip)->sin_family,
&((SOCKADDR_IN *)ip)->sin_addr,
temp, sizeof(temp)),
ntohs(((SOCKADDR_IN *)ip)->sin_port)));
TRACE_DETAIL(("Family: %d",((SOCKADDR_IN *)ip)->sin_family));
#endif
bytes_sent = sendto(sock_fd,
buf,
(length - bytes_sent),
0,
ip,
sizeof(struct sockaddr));
}
if(bytes_sent == length)
{
TRACE_DETAIL(("Message sent in full"));
buf = NULL;
success = TRUE;
total_bytes_sent +=bytes_sent;
break;
}
else if (bytes_sent == -1)
{
/***********************************************************************/
/* An error was returned - check for retryable socket error values. */
/***********************************************************************/
os_error = errno;
TRACE_PERROR(("Send failed on socket %d.",sock_fd));
if ((os_error == EWOULDBLOCK) ||
(os_error == ENOMEM) ||
(os_error == ENOSR))
{
/*********************************************************************/
/* This seems to be a flow control condition - clear the bytes sent */
/* value to show that no data was written. */
/*********************************************************************/
TRACE_WARN(("Resource shortage - try again"));
bytes_sent = 0;
}
else if (os_error == EINTR)
{
TRACE_WARN(("Send interrupted - loop round again"));
bytes_sent = 0;
}
else
{
/*********************************************************************/
/* Socket failed so work source needs to be unregistered. */
/*********************************************************************/
TRACE_ERROR(("Socket failed"));
success = HM_ERR;
break;
}
}
/***************************************************************************/
/* Advance current pointer to remaining part of data */
/***************************************************************************/
buf += bytes_sent;
total_bytes_sent +=bytes_sent;
} while(total_bytes_sent< length);
if(total_bytes_sent == length)
{
TRACE_DETAIL(("Message sent in full"));
success = TRUE;
}
TRACE_EXIT();
return (success);
} /* hm_tprt_send_on_socket */
/**
* @brief Creates a non-blocking socket and returns its descriptor.
*
* @param conn_type Type of connection to be opened
* @param *params Structure containing paramters of request for destination.
*
* @return Socket CB (#HM_SOCKET_CB) of the connection opened, else @c NULL
*/
HM_SOCKET_CB * hm_tprt_open_connection(uint32_t conn_type, void * params )
{
/***************************************************************************/
/* Descriptor of the socket created. Default value denotes error condition */
/***************************************************************************/
int32_t sock_fd = -1;
int32_t val;
int32_t option_val = 1;
socklen_t length;
HM_SOCKET_CB *sock_cb = NULL;
HM_INET_ADDRESS *address = params;
struct ip_mreq mreq;
char mcast_addr[129];
int32_t mcast_group = HM_MCAST_BASE_ADDRESS; /* Base value */
char target[128], service[128];
int32_t ret_val = HM_OK;
struct addrinfo hints, *res, *ressave;
HM_SOCKADDR_UNION *mcast_cast = NULL, *addr = NULL;
TRACE_ENTRY();
TRACE_ASSERT(params != NULL);
/***************************************************************************/
/* Create a socket control block */
/***************************************************************************/
sock_cb = hm_alloc_sock_cb();
if(sock_cb == NULL)
{
TRACE_ERROR(("Resource allocation failed for socket control block"));
ret_val = HM_ERR;
goto EXIT_LABEL;
}
/***************************************************************************/
/* Depending on the connection type, determine the sockaddr strcuture */
switch(conn_type)
{
case HM_TRANSPORT_TCP_LISTEN:
TRACE_INFO(("IPv4 Listen Socket"));
hints.ai_family = AF_INET;
hints.ai_flags = AI_PASSIVE;
hints.ai_socktype = SOCK_STREAM;
hints.ai_protocol = IPPROTO_TCP;
inet_ntop(hints.ai_family, &((SOCKADDR_IN *)params)->sin_addr, target, sizeof(target));
snprintf(service, sizeof(service), "%d", ntohs(((SOCKADDR_IN *)params)->sin_port));
break;
case HM_TRANSPORT_TCP_OUT:
TRACE_INFO(("IPv4 Outgoing Socket"));
hints.ai_family = AF_INET;
hints.ai_socktype = SOCK_STREAM;
hints.ai_protocol = IPPROTO_TCP;
hints.ai_flags = AI_NUMERICHOST|AI_NUMERICSERV;
inet_ntop(hints.ai_family, &((SOCKADDR_IN *)params)->sin_addr, target, sizeof(target));
snprintf(service, sizeof(service), "%d", ntohs(((SOCKADDR_IN *)params)->sin_port));
break;
case HM_TRANSPORT_TCP_IN:
TRACE_INFO(("IPv4 Incoming Socket"));
hints.ai_family = AF_INET;
hints.ai_socktype = SOCK_STREAM;
hints.ai_protocol = IPPROTO_TCP;
break;
case HM_TRANSPORT_UDP:
TRACE_INFO(("IPv4 UDP Socket"));
hints.ai_family = AF_INET;
hints.ai_socktype = SOCK_DGRAM;
hints.ai_protocol = IPPROTO_UDP;
inet_ntop(hints.ai_family, &((SOCKADDR_IN *)params)->sin_addr, target, sizeof(target));
snprintf(service, sizeof(service), "%d", ntohs(((SOCKADDR_IN *)params)->sin_port));
break;
case HM_TRANSPORT_MCAST:
TRACE_INFO(("IPv4 Multicast Socket"));
hints.ai_family = AF_INET;
hints.ai_socktype = SOCK_DGRAM;
hints.ai_protocol = IPPROTO_UDP;
hints.ai_flags = AI_PASSIVE | AI_NUMERICSERV;
inet_ntop(hints.ai_family, &((SOCKADDR_IN *)params)->sin_addr, target, sizeof(target));
snprintf(service, sizeof(service), "%d", ntohs(((SOCKADDR_IN *)params)->sin_port));
break;
case HM_TRANSPORT_SCTP:
TRACE_INFO(("IPv4 SCTP Socket"));
hints.ai_family = AF_INET;
hints.ai_socktype = SOCK_STREAM;
hints.ai_protocol = IPPROTO_SCTP;
inet_ntop(hints.ai_family, &((SOCKADDR_IN *)params)->sin_addr, target, sizeof(target));
snprintf(service, sizeof(service), "%d", ntohs(((SOCKADDR_IN *)params)->sin_port));
break;
case HM_TRANSPORT_TCP_IPv6_LISTEN:
TRACE_INFO(("IPv6 Listen Socket"));
hints.ai_family = AF_INET6;
hints.ai_flags = AI_PASSIVE;
hints.ai_socktype = SOCK_STREAM;
inet_ntop(hints.ai_family, &((SOCKADDR_IN6 *)params)->sin6_addr, target, sizeof(target));
snprintf(service, sizeof(service), "%d", ntohs(((SOCKADDR_IN6 *)params)->sin6_port));
break;
case HM_TRANSPORT_TCP_IPv6_OUT:
TRACE_INFO(("IPv6 I/O Socket"));
hints.ai_family = AF_INET6;
hints.ai_socktype = SOCK_STREAM;
inet_ntop(hints.ai_family, &((SOCKADDR_IN6 *)params)->sin6_addr, target, sizeof(target));
snprintf(service, sizeof(service), "%d", ntohs(((SOCKADDR_IN6 *)params)->sin6_port));
break;
case HM_TRANSPORT_TCP_IPv6_IN:
TRACE_INFO(("IPv6 I/O Socket"));
hints.ai_family = AF_INET6;
hints.ai_socktype = SOCK_STREAM;
break;
case HM_TRANSPORT_UDP_IPv6:
TRACE_INFO(("IPv6 UDP Socket"));
hints.ai_family = AF_INET6;
hints.ai_socktype = SOCK_DGRAM;
inet_ntop(hints.ai_family, &((SOCKADDR_IN6 *)params)->sin6_addr, target, sizeof(target));
snprintf(service, sizeof(service), "%d", ntohs(((SOCKADDR_IN6 *)params)->sin6_port));
break;
case HM_TRANSPORT_MCAST_IPv6:
TRACE_INFO(("IPv6 Multicast Socket"));
hints.ai_family = AF_INET6;
hints.ai_socktype = SOCK_DGRAM;
hints.ai_flags = AI_PASSIVE | AI_NUMERICSERV;
inet_ntop(hints.ai_family, &((SOCKADDR_IN6 *)params)->sin6_addr, target, sizeof(target));
snprintf(service, sizeof(service), "%d", ntohs(((SOCKADDR_IN6 *)params)->sin6_port));
break;
case HM_TRANSPORT_SCTP_IPv6:
TRACE_INFO(("IPv6 SCTP Socket"));
hints.ai_family = AF_INET6;
hints.ai_socktype = SOCK_STREAM;
inet_ntop(hints.ai_family, &((SOCKADDR_IN6 *)params)->sin6_addr, target, sizeof(target));
snprintf(service, sizeof(service), "%d", ntohs(((SOCKADDR_IN6 *)params)->sin6_port));
break;
default:
TRACE_ERROR(("Unknown connection Type %d", conn_type));
TRACE_ASSERT(0==1);
goto EXIT_LABEL;
}
switch(conn_type)
{
case HM_TRANSPORT_TCP_IN:
case HM_TRANSPORT_TCP_IPv6_IN:
TRACE_INFO(("Accept incoming connection"));
goto EXIT_LABEL;
default:
TRACE_INFO(("One of Outgoing Connections Type %d", conn_type));
}
/***************************************************************************/
/* If unsatisfactory, this code chunk may be replaced by a more elaborate */
/* memset(0) and filling of sin_addr structures. */
/***************************************************************************/
TRACE_DETAIL(("AI_FAMILY: %d", hints.ai_family));
TRACE_DETAIL(("Target: %s:%s", target, service));
if((conn_type != HM_TRANSPORT_MCAST)&&(conn_type != HM_TRANSPORT_MCAST_IPv6))
{
TRACE_DETAIL(("Unicast Address"));
if((ret_val = getaddrinfo(target, service, &hints, &res)) !=0)
{
TRACE_GAI_ERROR(("Error getting information on target %s:%s", target, service),ret_val);
ret_val = HM_ERR;
goto EXIT_LABEL;
}
ressave = res;
/***************************************************************************/
/* Right now, we are only expecting a single address in response. */
/* Set appropriate socket options depending on the type of socket. */
/***************************************************************************/
TRACE_DETAIL(("Opening Socket"));
sock_fd = hm_open_socket(res, (SOCKADDR **)&addr, &length);
}
else
{
TRACE_DETAIL(("Multicast Address"));
if((ret_val = getaddrinfo(NULL, service, &hints, &res)) !=0)
{
TRACE_GAI_ERROR(("Error getting information on target %s:%s", target, service),ret_val);
ret_val = HM_ERR;
goto EXIT_LABEL;
}
ressave = res;
TRACE_DETAIL(("Opening Socket"));
sock_fd = hm_open_socket(res, (SOCKADDR **)&addr, &length);
}
if(sock_fd == -1)
{
TRACE_ERROR(("Error opening socket"));
ret_val = HM_ERR;
goto EXIT_LABEL;
}
TRACE_DETAIL(("Socket opened: %d", sock_fd));
TRACE_ASSERT(addr != NULL);
/***************************************************************************/
/* Other specific options and processing. */
/***************************************************************************/
switch (conn_type)
{
case HM_TRANSPORT_TCP_LISTEN:
/***************************************************************************/
/* Set SOCKOPTS to TCP_NODELAY to disable Nagle's algorithm */
/***************************************************************************/
val = setsockopt(sock_fd,
IPPROTO_TCP,
TCP_NODELAY,
&option_val,
sizeof(option_val));
if(val == -1)
{
TRACE_PERROR(("Error Setting TCP_NODELAY"));
close(sock_fd);
sock_fd = -1;
goto EXIT_LABEL;
}
/***************************************************************************/
/* Bind to address */
/***************************************************************************/
TRACE_DETAIL(("Binding to address"));
if(bind(sock_fd, &addr->sock_addr, length) != 0)
{
TRACE_PERROR(("Error binding to port"));
close(sock_fd);
sock_fd = -1;
goto EXIT_LABEL;
}
TRACE_DETAIL(("Start Listen on Port"));
if(listen(sock_fd, HM_MAX_PENDING_CONNECT_REQ) != 0)
{
TRACE_PERROR(("Listen on socket %d failed.", sock_fd));
close(sock_fd);
sock_fd = -1;
goto EXIT_LABEL;
}
sock_cb->sock_type = HM_TRANSPORT_SOCK_TYPE_TCP;
break;
case HM_TRANSPORT_TCP_OUT:
TRACE_INFO(("Trying to connect"));
if((val = connect(sock_fd, &addr->sock_addr, length))!=0)
{
if(errno == EINPROGRESS)
{
TRACE_DETAIL(("Connect will complete asynchronously."));
FD_SET(sock_fd, &hm_tprt_write_set);
sock_cb->conn_state = HM_TPRT_CONN_INIT;
}
else
{
TRACE_PERROR(("Connect failed on socket %d", sock_fd));
FD_CLR(sock_cb->sock_fd, &hm_tprt_write_set);
close(sock_fd);
sock_fd = -1;
goto EXIT_LABEL;
}
}
else
{
TRACE_DETAIL(("Connect succeeded!"));
/* Directly move connection to active state */
sock_cb->conn_state = HM_TPRT_CONN_ACTIVE;
}
//TODO: Add socket to write_set and poll on write_set too.
sock_cb->sock_type = HM_TRANSPORT_SOCK_TYPE_TCP;
FD_SET(sock_fd, &hm_tprt_write_set);
break;
case HM_TRANSPORT_UDP:
/***************************************************************************/
/* Bind to address */
/***************************************************************************/
TRACE_DETAIL(("Binding to address"));
if(bind(sock_fd, &addr->sock_addr, length) != 0)
{
TRACE_PERROR(("Error binding to port"));
close(sock_fd);
sock_fd = -1;
goto EXIT_LABEL;
}
sock_cb->sock_type = HM_TRANSPORT_SOCK_TYPE_UDP;
break;
case HM_TRANSPORT_MCAST:
/***************************************************************************/
/* Bind to address */
/***************************************************************************/
if(bind(sock_fd, &addr->sock_addr, length) != 0)
{
TRACE_PERROR(("Error binding to port"));
close(sock_fd);
sock_fd = -1;
goto EXIT_LABEL;
}
sock_cb->sock_type = HM_TRANSPORT_SOCK_TYPE_UDP;
#ifdef I_WANT_TO_DEBUG
{
char address_value[128];
HM_SOCKADDR_UNION addr;
socklen_t addrlen = sizeof(addr);
getsockname(sock_fd, &addr.sock_addr, &addrlen);
inet_ntop(addr.in_addr.sin_family,
&addr.in_addr.sin_addr,
address_value, sizeof(address_value));
TRACE_INFO(("Address value: %s:%d",address_value,
ntohs(addr.in_addr.sin_port)));
TRACE_INFO(("Family: %d", addr.in_addr.sin_family));
}
#endif
snprintf(mcast_addr, sizeof(mcast_addr), "224.0.0.%d", mcast_group+address->mcast_group);
TRACE_DETAIL(("Multicast Group Address: %s", mcast_addr));
/***************************************************************************/
/* Convert to network representation. */
/***************************************************************************/
inet_pton(res->ai_family, mcast_addr, &mreq.imr_multiaddr.s_addr);
mreq.imr_interface.s_addr = htonl(INADDR_ANY);
if(setsockopt(sock_fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) == -1)
{
TRACE_PERROR(("Error joining Multicast Group"));
close(sock_fd);
sock_fd = -1;
goto EXIT_LABEL;
}
TRACE_INFO(("Joined Multicast Group %d.", address->mcast_group));
TRACE_INFO(("Update Global Multicast Address Destination"));
/***************************************************************************/
/* Also update the Multicast sending address in LOCAL */
/* This is a quickfix. */
/* FIXME */
/***************************************************************************/
TRACE_ASSERT(LOCAL.mcast_addr != NULL);
LOCAL.mcast_addr->address.mcast_group = address->mcast_group;
//IPv4 Specific
mcast_cast = (HM_SOCKADDR_UNION *)&LOCAL.mcast_addr->address.address;
inet_pton(res->ai_family, mcast_addr,
((SOCKADDR_IN *)&mcast_cast->in_addr.sin_addr));
mcast_cast->in_addr.sin_port = ((SOCKADDR_IN*)res->ai_addr)->sin_port;
mcast_cast->in_addr.sin_family = res->ai_family;
#ifdef I_WANT_TO_DEBUG
{
char address_value[128];
inet_ntop(res->ai_family,
((SOCKADDR_IN *)&mcast_cast->in_addr.sin_addr),
address_value, sizeof(address_value));
TRACE_INFO(("Multicast Address value: %s:%d",address_value,
ntohs(mcast_cast->in_addr.sin_port)));
TRACE_INFO(("Family: %d", res->ai_family));
}
#endif
/***************************************************************************/
/* Do not loopback the packets */
/***************************************************************************/
{
TRACE_DETAIL(("Setting Loopback to off."));
u_char flag;
flag = 0;
if(setsockopt(sock_fd, IPPROTO_IP, IP_MULTICAST_LOOP, &flag, sizeof(flag)) == -1)
{
TRACE_PERROR(("Error setting loopback option on Multicast socket"));
}
}
LOCAL.mcast_addr->sock_cb = sock_cb;
LOCAL.mcast_addr->location_cb = &LOCAL.local_location_cb;
break;
default:
TRACE_WARN(("Unknown Connection type"));
break;
}
/***************************************************************************/
/* All went well. Set the sock_fd as that of sock_cb */
/***************************************************************************/
sock_cb->sock_fd = sock_fd;
memcpy(&sock_cb->addr, &address->address, sizeof(SOCKADDR));
/***************************************************************************/
/* Insert in Connection List */
/***************************************************************************/
HM_INSERT_BEFORE(LOCAL.conn_list, sock_cb->node);
EXIT_LABEL:
if (ret_val == HM_OK)
{
/***************************************************************************/
/* Free the address structures that were allocated in getaddrinfo in kernel*/
/* NOTE: We are not saving the struct sockaddr, we will fetch it later */
/* using getaddrinfo() */
/***************************************************************************/
freeaddrinfo(ressave);
/***************************************************************************/
/* Add the socket descriptor to the global FD set. */
/***************************************************************************/
//FIXME
}
else if(ret_val == HM_ERR)
{
hm_free_sock_cb(sock_cb);
sock_cb = NULL;
}
TRACE_EXIT();
return (sock_cb);
} /* hm_tprt_open_connection */
/**
* @brief Accepts the connection into a SOCKET_CB and returns the CB
*
* @param sock_fd Socket Descriptor
* @return Socket Control Block (#HM_SOCKET_CB) if successful, @c NULL otherwise
*/
HM_SOCKET_CB * hm_tprt_accept_connection(int32_t sock_fd)
{
/***************************************************************************/
/* Variable Declarations */
/***************************************************************************/
HM_SOCKET_CB *sock_cb =NULL;
uint32_t client_len = sizeof(SOCKADDR);
#ifdef I_WANT_TO_DEBUG
char address[128];
HM_SOCKADDR_UNION *addr = NULL;
#endif
/***************************************************************************/
/* Sanity Checks */
/***************************************************************************/
TRACE_ENTRY();
TRACE_ASSERT(sock_fd > 0);
/***************************************************************************/
/* Main Routine */
/***************************************************************************/
sock_cb = hm_alloc_sock_cb();
if(sock_cb == NULL)
{
TRACE_ERROR(("Error allocating resources for incoming connection request"));
goto EXIT_LABEL;
}
TRACE_DETAIL(("Socket: %d", sock_fd));
sock_cb->sock_fd = accept(sock_fd,
(SOCKADDR *)&(sock_cb->addr), (socklen_t *)&client_len);
if (sock_cb->sock_fd < 0)
{
if (errno != EWOULDBLOCK)
{
TRACE_PERROR(("Failed to accept local connection."));
hm_free_sock_cb(sock_cb);
sock_cb = NULL;
goto EXIT_LABEL;
}
TRACE_WARN(("No new connection requests"));
}
#ifdef I_WANT_TO_DEBUG
/***************************************************************************/
/* Accepted Connection. Its parameters are enumerated. */
/***************************************************************************/
addr = (HM_SOCKADDR_UNION *)&sock_cb->addr;
TRACE_INFO(("New Connection from %s:%d",
inet_ntop(AF_INET, &addr->in_addr.sin_addr, address, client_len),
ntohs(addr->in_addr.sin_port)));
#endif
/***************************************************************************/
/* Add the descriptor to global descriptor set */
/***************************************************************************/
TRACE_DETAIL(("Add FD to set"));
FD_SET(sock_cb->sock_fd, &hm_tprt_conn_set);
if(max_fd < sock_cb->sock_fd)
{
TRACE_DETAIL(("Update maximum socket descriptor value to %d", sock_cb->sock_fd));
max_fd = sock_cb->sock_fd;
}
/***************************************************************************/
/* We're going to INIT state. Connect has been received, but nothing else */
/* has happened. Chances are, it has not been mapped on to a Transport CB */
/***************************************************************************/
sock_cb->conn_state = HM_TPRT_CONN_INIT;
sock_cb->sock_type = HM_TRANSPORT_SOCK_TYPE_TCP;
TRACE_DETAIL(("Connection Accepted on Socket %d. Wait for Messages.",
sock_cb->sock_fd));
HM_INSERT_BEFORE(LOCAL.conn_list, sock_cb->node);
EXIT_LABEL:
/***************************************************************************/
/* Exit Level Checks */
/***************************************************************************/
TRACE_EXIT();
return (sock_cb);
}/* hm_tprt_accept_connection */
/**
* Tracking Module (Thread)
*/
THD_FUNCTION(moduleTRACKING, arg) {
// Print infos
module_params_t* parm = (module_params_t*)arg;
TRACE_INFO("TRAC > Startup module TRACKING MANAGER");
TRACE_INFO("TRAC > Module TRACKING MANAGER info\r\n"
"%s Cycle: %d sec",
TRACE_TAB, parm->cycle
);
uint32_t id = 1;
lastTrackPoint = &trackPoints[0];
systime_t time = chVTGetSystemTimeX();
while(true)
{
parm->lastCycle = chVTGetSystemTimeX(); // Watchdog timer
TRACE_INFO("TRAC > Do module TRACKING MANAGER cycle");
trackPoint_t* tp = &trackPoints[id % (sizeof(trackPoints) / sizeof(trackPoint_t))]; // Current track point
trackPoint_t* ltp = &trackPoints[(id-1) % (sizeof(trackPoints) / sizeof(trackPoint_t))]; // Last track point
// Search for GPS satellites
gpsFix_t gpsFix;
GPS_Init();
do {
gps_get_fix(&gpsFix);
} while(!isGPSLocked(&gpsFix) && chVTGetSystemTimeX() <= time + S2ST(parm->cycle-3)); // Do as long no GPS lock and within timeout, timeout=cycle-1sec (-1sec in order to keep synchronization)
if(isGPSLocked(&gpsFix)) { // GPS locked
// Switch off GPS
GPS_Deinit();
// Debug
TRACE_INFO("TRAC > GPS sampling finished GPS LOCK");
TRACE_GPSFIX(&gpsFix);
// Calibrate RTC
setTime(gpsFix.time);
// Take time from GPS
tp->time.year = gpsFix.time.year;
tp->time.month = gpsFix.time.month;
tp->time.day = gpsFix.time.day;
tp->time.hour = gpsFix.time.hour;
tp->time.minute = gpsFix.time.minute;
tp->time.second = gpsFix.time.second;
// Set new GPS fix
tp->gps_lat = gpsFix.lat;
tp->gps_lon = gpsFix.lon;
tp->gps_alt = gpsFix.alt;
tp->gps_lock = isGPSLocked(&gpsFix);
tp->gps_sats = gpsFix.num_svs;
} else { // GPS lost (keep GPS switched on)
// Debug
TRACE_WARN("TRAC > GPS sampling finished GPS LOSS");
// Take time from internal RTC
ptime_t time;
getTime(&time);
tp->time.year = time.year;
tp->time.month = time.month;
tp->time.day = time.day;
tp->time.hour = time.hour;
tp->time.minute = time.minute;
tp->time.second = time.second;
// Take GPS fix from old lock
tp->gps_lat = ltp->gps_lat;
tp->gps_lon = ltp->gps_lon;
tp->gps_alt = ltp->gps_alt;
// Mark gpsloss
tp->gps_lock = false;
tp->gps_sats = 0;
}
tp->id = id; // Serial ID
tp->gps_ttff = ST2S(chVTGetSystemTimeX() - time); // Time to first fix
// Power management
tp->adc_solar = getSolarVoltageMV();
tp->adc_battery = getBatteryVoltageMV();
tp->adc_charge = pac1720_getAverageChargePower();
tp->adc_discharge = pac1720_getAverageDischargePower();
bme280_t bmeInt;
bme280_t bmeExt;
// Atmosphere condition
if(BME280_isAvailable(BME280_ADDRESS_INT)) {
BME280_Init(&bmeInt, BME280_ADDRESS_INT);
tp->air_press = BME280_getPressure(&bmeInt, 256);
tp->air_hum = BME280_getHumidity(&bmeInt);
tp->air_temp = BME280_getTemperature(&bmeInt);
} else { // No internal BME280 found
TRACE_ERROR("TRAC > Internal BME280 not available");
tp->air_press = 0;
tp->air_hum = 0;
tp->air_temp = 0;
}
// Balloon condition
if(BME280_isAvailable(BME280_ADDRESS_EXT)) {
BME280_Init(&bmeExt, BME280_ADDRESS_EXT);
tp->bal_press = BME280_getPressure(&bmeExt, 256);
tp->bal_hum = BME280_getHumidity(&bmeExt);
tp->bal_temp = BME280_getTemperature(&bmeExt);
} else { // No external BME280 found
TRACE_WARN("TRAC > External BME280 not available");
tp->bal_press = 0;
tp->bal_hum = 0;
tp->bal_temp = 0;
}
// Trace data
TRACE_INFO( "TRAC > New tracking point available (ID=%d)\r\n"
"%s Time %04d-%02d-%02d %02d:%02d:%02d\r\n"
"%s Pos %d.%07d %d.%07d Alt %dm\r\n"
"%s Sats %d TTFF %dsec\r\n"
"%s ADC Vbat=%d.%03dV Vsol=%d.%03dV Pin=%dmW Pout=%dmW\r\n"
"%s Air p=%6d.%01dPa T=%2d.%02ddegC phi=%2d.%01d%%\r\n"
"%s Ball p=%6d.%01dPa T=%2d.%02ddegC phi=%2d.%01d%%",
tp->id,
TRACE_TAB, tp->time.year, tp->time.month, tp->time.day, tp->time.hour, tp->time.minute, tp->time.day,
TRACE_TAB, tp->gps_lat/10000000, tp->gps_lat%10000000, tp->gps_lon/10000000, tp->gps_lon%10000000, tp->gps_alt,
TRACE_TAB, tp->gps_sats, tp->gps_ttff,
TRACE_TAB, tp->adc_battery/1000, (tp->adc_battery%1000), tp->adc_solar/1000, (tp->adc_solar%1000), tp->adc_charge, tp->adc_discharge,
TRACE_TAB, tp->air_press/10, tp->air_press%10, tp->air_temp/100, tp->air_temp%100, tp->air_hum/10, tp->air_hum%10,
TRACE_TAB, tp->bal_press/10, tp->bal_press%10, tp->bal_temp/100, tp->bal_temp%100, tp->bal_hum/10, tp->bal_hum%10
);
// Switch last recent track point
lastTrackPoint = tp;
id++;
time = chThdSleepUntilWindowed(time, time + S2ST(parm->cycle)); // Wait until time + cycletime
}
}
int32_t main(int32_t argc, char **argv)
{
int32_t ret_val = HM_OK;
SOCKADDR_IN addr;
int32_t cmd_opt;
/* Randomly select a group for subscription */
int32_t subs_group = random_num(0, 4);
int32_t node[2] = {random_num(1,4), random_num(1,4)};
int32_t pct_type = 0x75010001;
int32_t pid = 0x00000034;
extern char *optarg;
while((cmd_opt = getopt(argc, argv, "l:")) != -1)
{
switch(cmd_opt)
{
case 'l':
TRACE_INFO(("Location Index: %s", optarg));
location_index = atoi(optarg);
break;
default:
printf("\nUsage: %s -l <location_number>", argv[0]);
break;
}
}
if(location_index == 0)
{
TRACE_ERROR(("Did not find Location Index"));
goto EXIT_LABEL;
}
pid = pid | (location_index << 24);
TRACE_INFO(("PID Assigned: 0x%x", pid));
/***************************************************************************/
/* Setup address */
/***************************************************************************/
memset(&addr, 0, sizeof(SOCKADDR));
addr.sin_family = AF_INET;
addr.sin_port = htons(4999);
inet_pton(AF_INET, "127.0.0.1", &addr.sin_addr.s_addr);
/***************************************************************************/
/* Open Socket */
/***************************************************************************/
sock_fd = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
if(sock_fd < 0)
{
TRACE_PERROR(("Error opening socket"));
goto EXIT_LABEL;
}
/***************************************************************************/
/* Send connect request */
/***************************************************************************/
ret_val = connect(sock_fd, (SOCKADDR *)&addr, (socklen_t)sizeof(SOCKADDR));
if(ret_val < 0)
{
TRACE_PERROR(("Error connecting to HM."));
goto EXIT_LABEL;
}
/***************************************************************************/
/* Connected! Go ahead, send an init */
/***************************************************************************/
init_msg.hdr.msg_id = 1;
init_msg.hdr.msg_len = sizeof(init_msg);
init_msg.hdr.msg_type = HM_MSG_TYPE_INIT;
init_msg.hdr.request = TRUE;
init_msg.hdr.response_ok = FALSE;
init_msg.hardware_num = 0;
init_msg.index = location_index;
init_msg.keepalive_period = 1000; /* ms */
init_msg.location_status = TRUE;
init_msg.service_group_index = location_index;
TRACE_INFO(("Sending INIT message!"));
ret_val = send(sock_fd, (char *)&init_msg, sizeof(init_msg), 0);
if(ret_val != sizeof(init_msg))
{
TRACE_PERROR(("Error sending complete message on socket!"));
goto EXIT_LABEL;
}
TRACE_INFO(("INIT Message sent."));
ret_val = recv(sock_fd, (char *)&init_msg, sizeof(init_msg), 0);
if(ret_val != sizeof(init_msg))
{
TRACE_WARN(("Partial Message Received!"));
goto EXIT_LABEL;
}
TRACE_INFO(("Message response received"));
if(init_msg.hdr.response_ok == TRUE)
{
TRACE_INFO(("Hardware Index is %d", init_msg.hardware_num));
}
//TRACE_INFO(("Send Keepalive"));
//TODO: LATER
//Send Process UP Notification
proc_msg.hdr.msg_id = 1;
proc_msg.hdr.msg_len = sizeof(proc_msg);
proc_msg.hdr.msg_type = HM_MSG_TYPE_PROCESS_CREATE;
proc_msg.hdr.request = TRUE;
proc_msg.hdr.response_ok = FALSE;
proc_msg.if_offset = 0;
snprintf(proc_msg.name, sizeof(proc_msg.name), "TEST");
proc_msg.pid = pid;
proc_msg.proc_type = pct_type;
TRACE_INFO(("Sending PROCESS_CREATED message!"));
ret_val = send(sock_fd, (char *)&proc_msg, sizeof(proc_msg), 0);
if(ret_val != sizeof(proc_msg))
{
TRACE_PERROR(("Error sending complete message on socket!"));
}
TRACE_INFO(("PROCESS_CREATED Message sent."));
ret_val = recv(sock_fd, (char *)&proc_msg, sizeof(proc_msg), 0);
if(ret_val != sizeof(proc_msg))
{
TRACE_WARN(("Partial Message Received!"));
}
TRACE_INFO(("Message response received"));
if(proc_msg.hdr.response_ok == TRUE)
{
TRACE_INFO(("Process Create Notification OK"));
}
//Send REGISTER for Group
// TRACE_INFO(("Sending Register for Group %d", subs_group));
reg_msg = (HM_REGISTER_MSG *)malloc(sizeof(HM_REGISTER_MSG) + 1* sizeof(HM_REGISTER_TLV_CB));
reg_msg->hdr.msg_id = 1;
reg_msg->hdr.msg_len = sizeof(HM_REGISTER_MSG) + 2* sizeof(HM_REGISTER_TLV_CB);
reg_msg->hdr.msg_type = HM_MSG_TYPE_REGISTER;
reg_msg->hdr.request = TRUE;
reg_msg->hdr.response_ok = FALSE;
reg_msg->num_register = 2;
reg_msg->subscriber_pid = pid;
reg_msg->type = HM_REG_SUBS_TYPE_PROC;
tlv = (HM_REGISTER_TLV_CB *)((char *)reg_msg + sizeof(HM_REGISTER_MSG));
tlv->id = pct_type;
TRACE_INFO(("Sending PROCESS_REGISTER message!"));
ret_val = send(sock_fd, (char *)reg_msg, reg_msg->hdr.msg_len, 0);
if(ret_val != reg_msg->hdr.msg_len)
{
TRACE_PERROR(("Error sending complete message on socket!"));
}
TRACE_INFO(("Sent Register"));
//Receive REGISTER Response
memset((void *)reg_msg, 0, sizeof(reg_msg->hdr.msg_len));
ret_val = recv(sock_fd, (char *)reg_msg,
(sizeof(HM_REGISTER_MSG) + 1* sizeof(HM_REGISTER_TLV_CB)), 0);
if(ret_val != (sizeof(HM_REGISTER_MSG) + 1* sizeof(HM_REGISTER_TLV_CB)))
{
TRACE_WARN(("Partial Message Received!"));
}
TRACE_INFO(("Register response received"));
if(reg_msg->hdr.response_ok == TRUE)
{
TRACE_INFO(("Register OK"));
}
//Send Unregister
//Receive Unregister Response
//Send REGISTER for Nodes
// TRACE_INFO(("Sending Register for Nodes %d, %d", node[0], node[1]));
//Receive REGISTER Response
//Send Unregister
//Receive Unregister Response
while(1)
{
continue;
}
EXIT_LABEL:
if (sock_fd != -1)
{
close(sock_fd);
sock_fd = -1;
}
return ret_val;
}
