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; }