C++ NTOHS函数代码示例

void udp_packet_handler(void)
{
    msg_t m_recv_ip, m_send_ip, m_recv_udp, m_send_udp;
    ipv6_hdr_t *ipv6_header;
    udp_hdr_t *udp_header;
    socket_internal_t *udp_socket = NULL;
    uint16_t chksum;

    while (1) {
        msg_receive(&m_recv_ip);
        ipv6_header = ((ipv6_hdr_t *)m_recv_ip.content.ptr);
        udp_header = ((udp_hdr_t *)(m_recv_ip.content.ptr + IPV6_HDR_LEN));

        chksum = ipv6_csum(ipv6_header, (uint8_t*) udp_header, NTOHS(udp_header->length), IPPROTO_UDP);

        if (chksum == 0xffff) {
            udp_socket = get_udp_socket(udp_header);

            if (udp_socket != NULL) {
                m_send_udp.content.ptr = (char *)ipv6_header;
                msg_send_receive(&m_send_udp, &m_recv_udp, udp_socket->recv_pid);
            }
            else {
                printf("Dropped UDP Message because no thread ID was found for delivery!\n");
            }
        }
        else {
            printf("Wrong checksum (%x)!\n", chksum);
        }

        msg_reply(&m_recv_ip, &m_send_ip);
    }
}

/**@brief Function for notifying application of the ICMPv6 received packet.
 *
 * @param[in]   p_interface    Pointer to external interface from which packet come.
 * @param[in]   p_pbuffer      Pointer to packet buffer of ICMP6_PACKET_TYPE.
 * @param[in]   process_result Result of internal processing packet.
 *
 * @return      NRF_SUCCESS after successful processing, error otherwise. 
 */
static uint32_t app_notify_icmp_data(iot_interface_t  * p_interface, 
                                 iot_pbuffer_t    * p_pbuffer,
                                 uint32_t           process_result)
{
    uint32_t err_code = NRF_SUCCESS;

    if(m_event_handler != NULL)
    {

       ipv6_header_t  * p_ip_header   = (ipv6_header_t *)
                               (p_pbuffer->p_payload - ICMP6_HEADER_SIZE - IPV6_IP_HEADER_SIZE);
       icmp6_header_t * p_icmp_header = (icmp6_header_t *)
                               (p_pbuffer->p_payload - ICMP6_HEADER_SIZE);

        ICMP6_MUTEX_UNLOCK();
        
        // Change byte order of ICMP header given to application.
        p_icmp_header->checksum = NTOHS(p_icmp_header->checksum);

        err_code = m_event_handler(p_interface,
                                   p_ip_header,
                                   p_icmp_header,
                                   process_result,
                                   p_pbuffer);

        ICMP6_MUTEX_LOCK();
    }

    return err_code;
}

int32_t destiny_socket_sendto(int s, const void *buf, uint32_t len, int flags,
                              sockaddr6_t *to, uint32_t tolen)
{
    if (isUDPSocket(s) &&
        (get_socket(s)->socket_values.foreign_address.sin6_port == 0)) {
        uint8_t send_buffer[BUFFER_SIZE];

        ipv6_hdr_t *temp_ipv6_header = ((ipv6_hdr_t *)(&send_buffer));
        udp_hdr_t *current_udp_packet = ((udp_hdr_t *)(&send_buffer[IPV6_HDR_LEN]));
        uint8_t *payload = &send_buffer[IPV6_HDR_LEN + UDP_HDR_LEN];

        memcpy(&(temp_ipv6_header->destaddr), &to->sin6_addr, 16);
        ipv6_iface_get_best_src_addr(&(temp_ipv6_header->srcaddr), &(temp_ipv6_header->destaddr));

        current_udp_packet->src_port = get_free_source_port(IPPROTO_UDP);
        current_udp_packet->dst_port = to->sin6_port;
        current_udp_packet->checksum = 0;

        memcpy(payload, buf, len);
        current_udp_packet->length = HTONS(UDP_HDR_LEN + len);
        temp_ipv6_header->length = UDP_HDR_LEN + len;

        current_udp_packet->checksum = ~udp_csum(temp_ipv6_header,
                                       current_udp_packet);

        ipv6_sendto(&to->sin6_addr, IPPROTO_UDP,
                    (uint8_t *)(current_udp_packet),
                    NTOHS(current_udp_packet->length));
        return current_udp_packet->length;
    }
    else {
        return -1;
    }
}

/**
 * Parser + hash function for VLAN packet
 */
static inline uint32_t
decode_vlan_n_hash(struct ethhdr *ethh, uint8_t hash_split, uint8_t seed)
{
	TRACE_PKTHASH_FUNC_START();
	uint32_t rc = 0;
	struct vlanhdr *vhdr = (struct vlanhdr *)(ethh + 1);
	
	switch (NTOHS(vhdr->proto)) {
	case ETH_P_IP:
		rc = decode_ip_n_hash((struct iphdr *)(vhdr + 1),
				      hash_split, seed);
		break;
	case ETH_P_IPV6:
		rc = decode_ipv6_n_hash((struct ipv6hdr *)(vhdr + 1),
					hash_split, seed);
		break;
	case ETH_P_PPP_DISC:
	case ETH_P_PPP_SES:
	case ETH_P_IPX:
	case ETH_P_LOOP:
	case ETH_P_MPLS_MC:
	case ETH_P_MPLS_UC:
	case ETH_P_ARP:
	case ETH_P_RARP:
	default:
		/* others */
		rc = decode_others_n_hash(ethh, seed);
		break;
	}
	TRACE_PKTHASH_FUNC_END();
	return rc;
}

/**
  Function to handle received ACK packet. 
  
  If the ACK number matches the expected block number, and there are more 
  data pending, send the next block. Otherwise tell the caller that we are done.

  @param  Instance              The MTFTP upload session
  @param  Packet                The MTFTP packet received
  @param  Len                   The packet length
  @param  Completed             Return whether the upload has finished.

  @retval EFI_SUCCESS           The ACK is successfully processed.
  @retval EFI_TFTP_ERROR        The block number loops back.
  @retval Others                Failed to transmit the next data packet.

**/
EFI_STATUS
Mtftp4WrqHandleAck (
  IN     MTFTP4_PROTOCOL       *Instance,
  IN     EFI_MTFTP4_PACKET     *Packet,
  IN     UINT32                Len,
     OUT BOOLEAN               *Completed
  )
{
  UINT16                    AckNum;
  INTN                      Expected;
  UINT64                    TotalBlock;
 
  *Completed  = FALSE;
  AckNum      = NTOHS (Packet->Ack.Block[0]);
  Expected    = Mtftp4GetNextBlockNum (&Instance->Blocks);

  ASSERT (Expected >= 0);

  //
  // Get an unwanted ACK, return EFI_SUCCESS to let Mtftp4WrqInput
  // restart receive.
  //
  if (Expected != AckNum) {
    return EFI_SUCCESS;
  }

  //
  // Remove the acked block number, if this is the last block number,
  // tell the Mtftp4WrqInput to finish the transfer. This is the last
  // block number if the block range are empty..
  //
  Mtftp4RemoveBlockNum (&Instance->Blocks, AckNum, *Completed,&TotalBlock);

  Expected = Mtftp4GetNextBlockNum (&Instance->Blocks);

  if (Expected < 0) {
  
    //
    // The block range is empty. It may either because the the last
    // block has been ACKed, or the sequence number just looped back,
    // that is, there is more than 0xffff blocks.
    //
    if (Instance->LastBlock == AckNum) {
      ASSERT (Instance->LastBlock >= 1);
      *Completed = TRUE;
      return EFI_SUCCESS;

    } else {
      Mtftp4SendError (
        Instance,
        EFI_MTFTP4_ERRORCODE_REQUEST_DENIED,
        (UINT8 *) "Block number rolls back, not supported, try blksize option"
        );

      return EFI_TFTP_ERROR;
    }
  }

  return Mtftp4WrqSendBlock (Instance, (UINT16) Expected);
}

/**@brief Function for multiplexing transport protocol to different modules.
 *
 * @param[in]   p_interface  Pointer to external interface from which packet come.
 * @param[in]   p_pbuffer    Pointer to packet buffer.
 *
 * @return      NRF_SUCCESS if success, otherwise an error code.
 */
static uint32_t ipv6_input(iot_interface_t * p_interface, iot_pbuffer_t * p_pbuffer)
{
    uint32_t        err_code = NRF_SUCCESS;
    ipv6_header_t * p_iphdr = (ipv6_header_t *)(p_pbuffer->p_payload - IPV6_IP_HEADER_SIZE);

    // Change byte order of IP header given to application.
    p_iphdr->length    = NTOHS(p_iphdr->length);
    p_iphdr->flowlabel = NTOHS(p_iphdr->flowlabel);

    switch (p_iphdr->next_header)
    {
        case IPV6_NEXT_HEADER_ICMP6:
            IPV6_TRC("Got ICMPv6 packet.");

            IPV6_MUTEX_UNLOCK();
            err_code = icmp6_input(p_interface, p_iphdr, p_pbuffer);
            IPV6_MUTEX_LOCK();

            break;

        case IPV6_NEXT_HEADER_UDP:
            IPV6_TRC("Got UDP packet.");

            IPV6_MUTEX_UNLOCK();
            err_code = udp_input(p_interface, p_iphdr, p_pbuffer);
            IPV6_MUTEX_LOCK();

           break;

        default:
            IPV6_ERR("Got unsupported protocol packet. Protocol ID = 0x%x!",
                     p_iphdr->next_header);

#if (IPV6_ENABLE_USNUPORTED_PROTOCOLS_TO_APPLICATION == 1)
            app_notify_rx_data(p_interface, p_pbuffer);
#endif
            break;
    }

    // Free packet buffer unless marked explicitly as pending
    if (err_code != IOT_IPV6_ERR_PENDING)
    {
        UNUSED_VARIABLE(iot_pbuffer_free(p_pbuffer, true));
    }

    return err_code;
}

/*
 * Convert a port name to its port and protocol numbers.
 * We assume only TCP or UDP.
 * Return 0 upon failure.
 */
int
pcap_nametoport(const char *name, int *port, int *proto)
{
	struct servent *sp;
	char *other;

	sp = getservbyname(name, (char *)0);
	if (sp != NULL) {
		NTOHS(sp->s_port);
		*port = sp->s_port;
		*proto = pcap_nametoproto(sp->s_proto);
		/*
		 * We need to check /etc/services for ambiguous entries.
		 * If we find the ambiguous entry, and it has the
		 * same port number, change the proto to PROTO_UNDEF
		 * so both TCP and UDP will be checked.
		 */
		if (*proto == IPPROTO_TCP)
			other = "udp";
		else
			other = "tcp";

		sp = getservbyname(name, other);
		if (sp != 0) {
			NTOHS(sp->s_port);
#ifdef notdef
			if (*port != sp->s_port)
				/* Can't handle ambiguous names that refer
				   to different port numbers. */
				warning("ambiguous port %s in /etc/services",
					name);
#endif
			*proto = PROTO_UNDEF;
		}
		return 1;
	}
#if defined(ultrix) || defined(__osf__)
	/* Special hack in case NFS isn't in /etc/services */
	if (strcmp(name, "nfs") == 0) {
		*port = 2049;
		*proto = PROTO_UNDEF;
		return 1;
	}
#endif
	return 0;
}

void print_socket(socket_t *current_socket)
{
    char addr_str[IPV6_MAX_ADDR_STR_LEN];
    printf("Domain: %i, Type: %i, Protocol: %i \n",
           current_socket->domain,
           current_socket->type,
           current_socket->protocol);
    printf("Local address: %s\n",
           ipv6_addr_to_str(addr_str,
                            &current_socket->local_address.sin6_addr));
    printf("Foreign address: %s\n",
           ipv6_addr_to_str(addr_str,
                            &current_socket->foreign_address.sin6_addr));
    printf("Local Port: %u, Foreign Port: %u\n",
           NTOHS(current_socket->local_address.sin6_port),
           NTOHS(current_socket->foreign_address.sin6_port));
}

/**
  Get a UINT16 value from buffer.

  @param[in] Buf              Pointer to input buffer.

  @return                     The UINT16 value obtained from the buffer.

**/
UINT16
TcpGetUint16 (
  IN UINT8 *Buf
  )
{
  UINT16  Value;
  CopyMem (&Value, Buf, sizeof (UINT16));
  return NTOHS (Value);
}

void	swab_dbrec (struct or_dbrec *rec,  SWABDIR direction)
{
#ifndef BYTE_SWAP
    return;
#else
    if (direction == NTOH) {
	NTOHL (rec->or_dbflags);
	NTOHL (rec->or_dbuflags);
	NTOHL (rec->or_reccount);
	NTOHL (rec->or_maxdba);
	NTOHL (rec->or_availd99);
	NTOHL (rec->or_unavaild99);
	NTOHL (rec->or_hufid);
	NTOHL (rec->or_dbsecmask);

	NTOHS (rec->or_dbotype);
	NTOHS (rec->or_compflags);
	NTOHS (rec->or_dbaccess);
	NTOHS (rec->or_minwordsz);
	NTOHS (rec->or_maxwordsz);
	NTOHS (rec->or_recslots);
	NTOHS (rec->or_fzkeysz);
	NTOHS (rec->or_abstrsz);
	NTOHS (rec->or_language);
    }

    else {	/* going the other direction */
	HTONL (rec->or_dbflags);
	HTONL (rec->or_dbuflags);
	HTONL (rec->or_reccount);
	HTONL (rec->or_maxdba);
	HTONL (rec->or_availd99);
	HTONL (rec->or_unavaild99);
	HTONL (rec->or_hufid);
	HTONL (rec->or_dbsecmask);

	HTONS (rec->or_dbotype);
	HTONS (rec->or_compflags);
	HTONS (rec->or_dbaccess);
	HTONS (rec->or_minwordsz);
	HTONS (rec->or_maxwordsz);
	HTONS (rec->or_recslots);
	HTONS (rec->or_fzkeysz);
	HTONS (rec->or_abstrsz);
	HTONS (rec->or_language);
    }
    return;
#endif /* BYTE_SWAP */
}  /* swab_dbrec() */

VOID
INITIALIZE_NDFS_REQUEST_HEADER (
	PNDFS_REQUEST_HEADER	NdfsRequestHeader,
	UINT8					Command,
	PSECONDARY				Secondary,
	UINT8					IrpMajorFunction,
	UINT32					DataSize
	)
{
	ExAcquireFastMutex( &Secondary->FastMutex );		

	RtlCopyMemory(NdfsRequestHeader->Protocol, NDFS_PROTOCOL, sizeof(NdfsRequestHeader->Protocol));
	NdfsRequestHeader->Command	= Command;				
	NdfsRequestHeader->Flags	= Secondary->Thread.SessionContext.Flags;							    
	NdfsRequestHeader->Uid2		= NTOHS(Secondary->Thread.SessionContext.Uid);									
	NdfsRequestHeader->Tid2		= NTOHS(Secondary->Thread.SessionContext.Tid);									
	NdfsRequestHeader->Mid2		= 0;																	    

	NdfsRequestHeader->MessageSize4																			
		= sizeof(NDFS_REQUEST_HEADER)																			
			+ (																									
				(Secondary->Thread.SessionContext.MessageSecurity == 0)										
	 			 ? sizeof(NDFS_WINXP_REQUEST_HEADER) + DataSize											
				 : (																							
					 ((IrpMajorFunction == IRP_MJ_WRITE														
						&& Secondary->Thread.SessionContext.RwDataSecurity == 0								
						&& DataSize <= Secondary->Thread.SessionContext.PrimaryMaxDataSize)				
					 ||																							
					 IrpMajorFunction == IRP_MJ_READ																
						&& Secondary->Thread.SessionContext.RwDataSecurity == 0								
						&& DataSize <= Secondary->Thread.SessionContext.SecondaryMaxDataSize)

					? ADD_ALIGN8(sizeof(NDFS_WINXP_REQUEST_HEADER) + DataSize)							
					: ADD_ALIGN8(sizeof(NDFS_WINXP_REQUEST_HEADER) + DataSize)				
				   )																							
				);																								
	
	NdfsRequestHeader->MessageSize4 = NTOHL(NdfsRequestHeader->MessageSize4);

	ExReleaseFastMutex( &Secondary->FastMutex );

	return;
}																										

uint16_t udp_csum(ipv6_hdr_t *ipv6_header, udp_hdr_t *udp_header)
{
    uint16_t sum;
    uint16_t len = NTOHS(udp_header->length);

    sum = len + IPPROTO_UDP;
    sum = csum(sum, (uint8_t *)&ipv6_header->srcaddr, 2 * sizeof(ipv6_addr_t));
    sum = csum(sum, (uint8_t *)udp_header, len);
    return (sum == 0) ? 0xffff : HTONS(sum);
}

static
int decode_ip_hdr(struct iphdr *iphd)
{
    cprintf("IP Header\n");
    cprintf("SRC IP:     \t%ld.%ld.%ld.%ld\n",
            IP_ADDR_FORMAT((long)NTOHL(iphd->ip_source)));
    cprintf("DST IP:     \t%ld.%ld.%ld.%ld\n",
            IP_ADDR_FORMAT((long)NTOHL(iphd->ip_dest)));
    cprintf("version:    \t%d\n", iphd->ip_v);
    cprintf("protocol:   \t%d\n", iphd->ip_p);
    cprintf("frag offset:\t%d\n", NTOHS(iphd->ip_off)&0x1fff);
    cprintf("more frags: \t%d\n", (NTOHS(iphd->ip_off)&0x2000) >> 13);
    cprintf("ip hdr len: \t%d\n", iphd->ip_hl*4);
    cprintf("total len:  \t%d\n", NTOHS(iphd->ip_len));
    cprintf("id:         \t%d\n", NTOHS(iphd->ip_id));
    cprintf("\n");

    return 0;
}

int conn_udp_getlocaladdr(conn_udp_t *conn, void *addr, uint16_t *port)
{
    if (conn->sock.input_callback != NULL) {
        mutex_lock(&conn->mutex);
        memset(addr, 0, sizeof(ipv6_addr_t));
        *port = NTOHS(conn->sock.udp_conn->lport);
        mutex_unlock(&conn->mutex);
        return sizeof(ipv6_addr_t);
    }
    return -EBADF;
}

static
int decode_tcp_hdr(struct tcphdr *tcphd)
{
    cprintf("TCP Header\n");
    cprintf("SRC Port:\t%d\n", NTOHS(tcphd->tcp_source));
    cprintf("DST Port:\t%d\n", NTOHS(tcphd->tcp_dest));
    cprintf("seq #:   \t%lu\n", (long)NTOHL(tcphd->tcp_seq));
    cprintf("ack #:   \t%lu\n", (long)NTOHL(tcphd->tcp_ack_seq));
    cprintf("hdr len: \t%d\n", tcphd->tcp_doff * 4);
    cprintf("rst:     \t%d\n", tcphd->tcp_rst);
    cprintf("syn:     \t%d\n", tcphd->tcp_syn);
    cprintf("fin:     \t%d\n", tcphd->tcp_fin);
    cprintf("window:  \t%d\n", NTOHS(tcphd->tcp_window));
    cprintf("cksum:   \t%d\n", NTOHS(tcphd->tcp_sum));
    cprintf("urg_ptr: \t%d\n", NTOHS(tcphd->tcp_urg_ptr));
    cprintf("P: %d, A: %d, U: %d, R1: %d, R2: %d\n",
            tcphd->tcp_psh, tcphd->tcp_ack, tcphd->tcp_urg, tcphd->tcp_res1,
            tcphd->tcp_res2);

    return 0;
}