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;
}
VOID
LfsTable_DeleteNetDiskPartitionInfoUser(
IN PLFS_TABLE LfsTable,
IN PNETDISK_PARTITION_INFO NetDiskPartitionInfo,
IN BOOLEAN Primary
) {
PLFSTAB_ENTRY entry ;
BOOLEAN Found ;
Found = LfsTable_LookupEx(LfsTable, NetDiskPartitionInfo, Primary, &entry) ;
if(Found) {
LfsTable_DereferenceEntry (
entry
) ;
LfsTable_DereferenceEntry (
entry
) ;
} else {
SPY_LOG_PRINT( LFS_DEBUG_TABLE_INFO,
("LFS: LfsTable_DeleteNetDiskPartitionInfoUser: Could not find NetDisk:%02x:%02x:%02x:%02x:%02x:%02x/%d\n",
NetDiskPartitionInfo->NetDiskAddress.Node[0],
NetDiskPartitionInfo->NetDiskAddress.Node[1],
NetDiskPartitionInfo->NetDiskAddress.Node[2],
NetDiskPartitionInfo->NetDiskAddress.Node[3],
NetDiskPartitionInfo->NetDiskAddress.Node[4],
NetDiskPartitionInfo->NetDiskAddress.Node[5],
NTOHS(NetDiskPartitionInfo->NetDiskAddress.Port)
));
}
}
/**
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,
¤t_socket->local_address.sin6_addr));
printf("Foreign address: %s\n",
ipv6_addr_to_str(addr_str,
¤t_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() */
static
BOOLEAN
LfsTable_Lookup(
IN PLFS_TABLE LfsTable,
IN PNETDISK_PARTITION_INFO NetDiskPartitionInfo,
OUT PLFSTAB_ENTRY *LfsTabEntry
) {
PLIST_ENTRY listEntry ;
KIRQL oldIrql ;
PLFSTAB_ENTRY entry ;
BOOLEAN Found ;
Found = FALSE ;
ExAcquireSpinLock(&LfsTable->SpinLock, &oldIrql) ;
listEntry = LfsTable->LfsTabPartitionList.Flink ;
while(listEntry != &LfsTable->LfsTabPartitionList) {
entry = CONTAINING_RECORD(listEntry, LFSTAB_ENTRY, LfsTabPartitionEntry) ;
if( RtlCompareMemory( // NetDisk Address
&entry->LocalNetDiskPartitionInfo.NetDiskPartitionInfo.NetDiskAddress,
&NetDiskPartitionInfo->NetDiskAddress,
sizeof(LPX_ADDRESS)) == sizeof(LPX_ADDRESS) &&
entry->LocalNetDiskPartitionInfo.NetDiskPartitionInfo.UnitDiskNo == NetDiskPartitionInfo->UnitDiskNo // UnitDisk No
&& entry->LocalNetDiskPartitionInfo.NetDiskPartitionInfo.StartingOffset.QuadPart == NetDiskPartitionInfo->StartingOffset.QuadPart // Partition Starting Byte Offset
) {
LfsTable_ReferenceEntry (
entry
) ;
*LfsTabEntry = entry ;
Found = TRUE ;
break ;
}
listEntry = listEntry->Flink ;
}
ExReleaseSpinLock(&LfsTable->SpinLock, oldIrql) ;
#if DBG
if(!Found) {
SPY_LOG_PRINT( LFS_DEBUG_TABLE_INFO,
("LFS: LfsTable_Lookup: updated the primary address of NetDisk:%02x:%02x:%02x:%02x:%02x:%02x/%d UnitDisk:%d\n",
NetDiskPartitionInfo->NetDiskAddress.Node[0],
NetDiskPartitionInfo->NetDiskAddress.Node[1],
NetDiskPartitionInfo->NetDiskAddress.Node[2],
NetDiskPartitionInfo->NetDiskAddress.Node[3],
NetDiskPartitionInfo->NetDiskAddress.Node[4],
NetDiskPartitionInfo->NetDiskAddress.Node[5],
NTOHS(NetDiskPartitionInfo->NetDiskAddress.Port),
(int)NetDiskPartitionInfo->UnitDiskNo
));
}
#endif
return Found ;
}
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;
}
/**
* Parser + hash function for the IPv4 packet
*/
static uint32_t
decode_ip_n_hash(struct iphdr *iph, uint8_t hash_split, uint8_t seed)
{
TRACE_PKTHASH_FUNC_START();
uint32_t rc = 0;
if (hash_split == 2) {
rc = sym_hash_fn(NTOHL(iph->saddr),
NTOHL(iph->daddr),
NTOHS(0xFFFD) + seed,
NTOHS(0xFFFE) + seed);
} else {
struct tcphdr *tcph = NULL;
struct udphdr *udph = NULL;
switch (iph->protocol) {
case IPPROTO_TCP:
tcph = (struct tcphdr *)((uint8_t *)iph + (iph->ihl<<2));
rc = sym_hash_fn(NTOHL(iph->saddr),
NTOHL(iph->daddr),
NTOHS(tcph->source) + seed,
NTOHS(tcph->dest) + seed);
break;
case IPPROTO_UDP:
udph = (struct udphdr *)((uint8_t *)iph + (iph->ihl<<2));
rc = sym_hash_fn(NTOHL(iph->saddr),
NTOHL(iph->daddr),
NTOHS(udph->source) + seed,
NTOHS(udph->dest) + seed);
break;
case IPPROTO_IPIP:
/* tunneling */
rc = decode_ip_n_hash((struct iphdr *)((uint8_t *)iph + (iph->ihl<<2)),
hash_split, seed);
break;
default:
/*
* the hash strength (although weaker but) should still hold
* even with 2 fields
*/
rc = sym_hash_fn(NTOHL(iph->saddr),
NTOHL(iph->daddr),
NTOHS(0xFFFD) + seed,
NTOHS(0xFFFE) + seed);
break;
}
}
TRACE_PKTHASH_FUNC_END();
return rc;
}
VOID
LfsTable_UpdatePrimaryInfo(
IN PLFS_TABLE LfsTable,
IN PLPX_ADDRESS NetDiskAddress,
IN UCHAR UnitDiskNo,
IN PLPX_ADDRESS PrimaryAddress
) {
PLIST_ENTRY listEntry ;
KIRQL oldIrql ;
KIRQL oldIrql2 ;
PLFSTAB_ENTRY entry ;
ExAcquireSpinLock(&LfsTable->SpinLock, &oldIrql) ;
listEntry = LfsTable->LfsTabPartitionList.Flink ;
while(listEntry != &LfsTable->LfsTabPartitionList) {
entry = CONTAINING_RECORD(listEntry, LFSTAB_ENTRY, LfsTabPartitionEntry) ;
if( RtlCompareMemory( // NetDisk Address
&entry->LocalNetDiskPartitionInfo.NetDiskPartitionInfo.NetDiskAddress.Node,
NetDiskAddress->Node,
LPXADDR_NODE_LENGTH) == LPXADDR_NODE_LENGTH &&
entry->LocalNetDiskPartitionInfo.NetDiskPartitionInfo.NetDiskAddress.Port == NetDiskAddress->Port &&
entry->LocalNetDiskPartitionInfo.NetDiskPartitionInfo.UnitDiskNo == UnitDiskNo // UnitDisk No
) {
ExAcquireSpinLock(&entry->SpinLock, &oldIrql2) ;
RtlCopyMemory(&entry->PrimaryAddress, PrimaryAddress, sizeof(LPX_ADDRESS)) ;
entry->Flags |= LFSTABENTRY_FLAG_VAILDPRIMADDRESS;
ExReleaseSpinLock(&entry->SpinLock, oldIrql2) ;
SPY_LOG_PRINT( LFS_DEBUG_TABLE_INFO,
("LFS: LfsTable_UpdatePrimaryInfo: updated the primary address of NetDisk:%02x:%02x:%02x:%02x:%02x:%02x/%d UnitDisk:%d\n",
entry->LocalNetDiskPartitionInfo.NetDiskPartitionInfo.NetDiskAddress.Node[0],
entry->LocalNetDiskPartitionInfo.NetDiskPartitionInfo.NetDiskAddress.Node[1],
entry->LocalNetDiskPartitionInfo.NetDiskPartitionInfo.NetDiskAddress.Node[2],
entry->LocalNetDiskPartitionInfo.NetDiskPartitionInfo.NetDiskAddress.Node[3],
entry->LocalNetDiskPartitionInfo.NetDiskPartitionInfo.NetDiskAddress.Node[4],
entry->LocalNetDiskPartitionInfo.NetDiskPartitionInfo.NetDiskAddress.Node[5],
NTOHS(entry->LocalNetDiskPartitionInfo.NetDiskPartitionInfo.NetDiskAddress.Port),
(int)entry->LocalNetDiskPartitionInfo.NetDiskPartitionInfo.UnitDiskNo
));
}
listEntry = listEntry->Flink ;
}
ExReleaseSpinLock(&LfsTable->SpinLock, oldIrql) ;
}
VOID
CNdasIXServer::OnIXPrimaryUpdate(
CLpxDatagramSocket& sock,
CONST SOCKADDR_LPX* pRemoteAddr,
CONST LSINFOX_PRIMARY_UPDATE* pData)
{
NDAS_UNITDEVICE_ID unitDeviceId = {
{
pData->NetDiskNode[0], pData->NetDiskNode[1],
pData->NetDiskNode[2], pData->NetDiskNode[3],
pData->NetDiskNode[4], pData->NetDiskNode[5]
},
pData->UnitDiskNo
};
CNdasUnitDevicePtr pUnitDevice = pGetNdasUnitDevice(unitDeviceId);
if (0 == pUnitDevice.get()) {
//
// Discard non-discovered unit device
//
return;
}
NDAS_UNITDEVICE_PRIMARY_HOST_INFO hostinfo;
::CopyMemory(
hostinfo.Host.Node,
pData->PrimaryNode,
sizeof(hostinfo.Host.Node));
hostinfo.Host.Port = NTOHS(pData->PrimaryPort);
hostinfo.SWMajorVersion = pData->SWMajorVersion;
hostinfo.SWMinorVersion = pData->SWMinorVersion;
hostinfo.SWBuildNumber = pData->SWBuildNumber;
hostinfo.NDFSCompatVersion = pData->NDFSCompatVersion;
hostinfo.NDFSVersion = pData->NDFSVersion;
XTLTRACE2(NDASSVC_IX, TRACE_LEVEL_RESERVED6,
"LSINFOX_PRIMATE_UPDATE_MESSAGE: %02X:%02X:%02X:%02X:%02X:%02X@%d\n",
pData->NetDiskNode[0],
pData->NetDiskNode[1],
pData->NetDiskNode[2],
pData->NetDiskNode[3],
pData->NetDiskNode[4],
pData->NetDiskNode[5],
pData->UnitDiskNo);
pUnitDevice->UpdatePrimaryHostInfo(hostinfo);
return;
}
/* Start a fake net connection (used for demos on CLIENT) */
netconnection_t *net_fake_connection(char *demoname, demo_header *header)
{
int fd;
ssize_t size;
netconnection_t *nc;
if( !(fd = open(demoname, O_RDONLY)) ) {
perror("open");
return NULL;
}
size = read(fd, header, sizeof(demo_header));
if( size != sizeof(demo_header) ) {
printf("Couldn't Read header information from \"%s\"\n", demoname);
if( size < 0 )
perror("read");
else
printf("Read only %d bytes of %d.\n", size, sizeof(demo_header));
close(fd);
return NULL;
}
NTOHS( header->x_tiles );
NTOHS( header->y_tiles );
NTOHS( header->fps );
if( (nc = nc_alloc()) ) {
net_connection_reset(nc);
nc->fd = fd;
nc->record_fd = 0;
nc->fake = 1;
nc->remote_address = fake_address;
}
return nc;
}
void rpl_remove_srh_header(ipv6_hdr_t *ipv6_header, const void *buf, uint8_t nextheader)
{
ipv6_hdr_t *temp_ipv6_header = ((ipv6_hdr_t *)(&srh_send_buffer));
uint8_t *payload = &srh_send_buffer[IPV6_HDR_LEN];
memcpy(temp_ipv6_header, ipv6_header, sizeof(ipv6_hdr_t));
int msg_length = NTOHS(ipv6_header->length) - sizeof(ipv6_srh_t);
temp_ipv6_header->length = HTONS(msg_length);
temp_ipv6_header->nextheader = nextheader;
memcpy(payload, buf, msg_length);
DEBUGF("Source routing header extraction finished.\n");
DEBUGF("Dest is now: %s\n", ipv6_addr_to_str(addr_str, IPV6_MAX_ADDR_STR_LEN, &temp_ipv6_header->destaddr));
srh_m_send.content.ptr = (char *) srh_send_buffer;
DEBUGF("Return from relay: %d\n", msg_send_receive(&srh_m_send, &srh_m_recv, ip_process_pid));
}
/**
* A temp solution while hash for other protocols are filled...
* (See decode_vlan_n_hash & pkt_hdr_hash functions).
*/
static uint32_t
decode_others_n_hash(struct ethhdr *ethh, uint8_t seed)
{
TRACE_PKTHASH_FUNC_START();
uint32_t saddr, daddr, rc;
saddr = ethh->h_source[5] |
(ethh->h_source[4] << 8) |
(ethh->h_source[3] << 16) |
(ethh->h_source[2] << 24);
daddr = ethh->h_dest[5] |
(ethh->h_dest[4] << 8) |
(ethh->h_dest[3] << 16) |
(ethh->h_dest[2] << 24);
rc = sym_hash_fn(NTOHL(saddr),
NTOHL(daddr),
NTOHS(0xFFFD) + seed,
NTOHS(0xFFFE) + seed);
TRACE_PKTHASH_FUNC_END();
return rc;
}
BOOL
FindPortCallback(
PSOCKET_INFORMATION Socket,
DWORD ActualAddress,
LPVOID Context
)
{
SOCKADDR_IN addr;
ULONG result;
DWORD port;
if( Socket->AddressFamily != AF_INET ) {
return TRUE;
}
if( !ReadMemory(
(DWORD)Socket->LocalAddress,
&addr,
sizeof(addr),
&result
) ) {
dprintf(
"port: cannot read SOCKADDR @ %08lx\n",
Socket->LocalAddress
);
return TRUE;
}
port = (DWORD)NTOHS( addr.sin_port );
if( addr.sin_family == AF_INET &&
port == (DWORD)Context ) {
DumpSocket(
Socket,
ActualAddress
);
}
return TRUE;
} // FindPortCallback
int16_t adt7310_read_raw(adt7310_t *dev)
{
int status;
int16_t raw;
/* Read the temperature value register */
status = adt7310_read_reg(dev, ADT7310_REG_VALUE, ADT7310_REG_SIZE_VALUE, (uint8_t*)&raw);
if (status < 0) {
/* communication error */
return INT16_MIN;
}
/* The temperature value is sent big endian (network byte order) */
raw = (int16_t)NTOHS((uint16_t)raw);
return raw;
}
LPCTSTR
CSockLpxAddr::ToString(DWORD radix)
{
HRESULT hr = ::StringCchPrintf(m_buffer, 60,
(radix == 10) ? _T("%02X:%02X:%02X:%02X:%02X:%02X(%05d)") :
_T("%02X:%02X:%02X:%02X:%02X:%02X(%04X)"),
m_sockLpxAddr.LpxAddress.Node[0],
m_sockLpxAddr.LpxAddress.Node[1],
m_sockLpxAddr.LpxAddress.Node[2],
m_sockLpxAddr.LpxAddress.Node[3],
m_sockLpxAddr.LpxAddress.Node[4],
m_sockLpxAddr.LpxAddress.Node[5],
NTOHS(m_sockLpxAddr.LpxAddress.Port));
_ASSERTE(SUCCEEDED(hr));
return m_buffer;
}
void rpl_process(void)
{
msg_t m_recv;
msg_init_queue(msg_queue, RPL_PKT_RECV_BUF_SIZE);
while (1) {
msg_receive(&m_recv);
uint8_t *code;
code = ((uint8_t *)m_recv.content.ptr);
/* differentiate packet types */
ipv6_buf = ipv6_get_buf();
memcpy(&rpl_buffer, ipv6_buf, NTOHS(ipv6_buf->length) + IPV6_HDR_LEN);
DEBUG("%s, %d: Reveived RPL information of type %04X\n", __FILE__, __LINE__, *code);
switch (*code) {
case (ICMP_CODE_DIS): {
recv_rpl_dis();
mutex_unlock(&rpl_recv_mutex);
break;
}
case (ICMP_CODE_DIO): {
recv_rpl_dio();
mutex_unlock(&rpl_recv_mutex);
break;
}
case (ICMP_CODE_DAO): {
recv_rpl_dao();
mutex_unlock(&rpl_recv_mutex);
break;
}
case (ICMP_CODE_DAO_ACK): {
recv_rpl_dao_ack();
mutex_unlock(&rpl_recv_mutex);
break;
}
default:
mutex_unlock(&rpl_recv_mutex);
break;
}
}
}
int net_if_send_packet_long(int if_id, net_if_eui64_t *target,
const void *payload, size_t payload_len)
{
DEBUG("net_if_send_packet: if_id = %d, target = %016" PRIx64 ", "
"payload = %p, payload_len = %d\n", if_id, NTOHLL(target->uint64), payload,
payload_len);
uint32_t response;
if (if_id < 0 || if_id >= NET_IF_MAX || !interfaces[if_id].initialized) {
DEBUG("Send packet: No interface initialized with ID %d.\n", if_id);
return -1;
}
if (interfaces[if_id].transceivers & (TRANSCEIVER_CC2420 |
TRANSCEIVER_AT86RF231 |
TRANSCEIVER_MC1322X)) {
ieee802154_packet_t p;
memset(&p, 0, sizeof(ieee802154_packet_t));
p.frame.payload = (uint8_t *)payload;
p.frame.payload_len = (uint8_t)payload_len;
p.frame.fcf.src_addr_m = (uint8_t)interfaces[if_id].trans_src_addr_m;
p.frame.fcf.dest_addr_m = IEEE_802154_LONG_ADDR_M;
p.frame.fcf.ack_req = 0;
p.frame.fcf.sec_enb = 0;
p.frame.fcf.frame_type = IEEE_802154_DATA_FRAME;
p.frame.fcf.frame_pend = 0;
p.frame.dest_pan_id = net_if_get_pan_id(if_id);
uint64_t target_h = NTOHLL(target->uint64);
memcpy(p.frame.dest_addr, &target_h, 8);
response = net_if_transceiver_get_set_handler(if_id, SND_PKT, (void *)&p);
}
else {
radio_packet_t p;
memset(&p, 0, sizeof(radio_packet_t));
p.data = (uint8_t *) payload;
p.length = payload_len;
p.dst = NTOHS(target->uint16[3]);
response = net_if_transceiver_get_set_handler(if_id, SND_PKT, (void *)&p);
}
return (response > payload_len) ? (int)payload_len : (int)response;
}
LPCSTR
CSockLpxAddr::ToStringA(DWORD radix)
{
LPSTR lpBuffer = reinterpret_cast<LPSTR>(m_buffer);
HRESULT hr = ::StringCchPrintfA(
lpBuffer, RTL_NUMBER_OF(m_buffer),
(radix == 10) ?
"%02X:%02X:%02X:%02X:%02X:%02X(%05d)" :
"%02X:%02X:%02X:%02X:%02X:%02X(%04X)",
m_sockLpxAddr.LpxAddress.Node[0],
m_sockLpxAddr.LpxAddress.Node[1],
m_sockLpxAddr.LpxAddress.Node[2],
m_sockLpxAddr.LpxAddress.Node[3],
m_sockLpxAddr.LpxAddress.Node[4],
m_sockLpxAddr.LpxAddress.Node[5],
NTOHS(m_sockLpxAddr.LpxAddress.Port));
_ASSERTE(SUCCEEDED(hr));
return lpBuffer;
}
uint8_t isTCP(unsigned char *buf)
{
TRACE_PKTHASH_FUNC_START();
uint8_t rc = 0;
struct ethhdr *ethh = (struct ethhdr *)buf;
switch (NTOHS(ethh->h_proto)) {
case ETH_P_IP:
rc = decode_ip_n_hash((struct iphdr *)(ethh + 1));
break;
case ETH_P_IPV6:
rc = decode_ipv6_n_hash((struct ipv6hdr *)(ethh + 1));
break;
case ETH_P_8021Q:
rc = decode_vlan_n_hash(ethh);
break;
default:
break;
}
TRACE_PKTHASH_FUNC_END();
return rc;
}
