public RPCResult <bool> PacketReceived(byte[] packet, int dev_no, int payload_offset,
int payload_len, p_addr devsrc)
{
HWAddr dest = ReadHWAddr(packet, payload_offset);
HWAddr src = ReadHWAddr(packet, payload_offset + 6);
/* Whole packet is 6 bytes dest MAC, 6 bytes source MAC,
* optional 4 bytes VLAN packet, 2 bytes ethertype,
* payload, 4 bytes frame check sequence (FCS) */
payload_len -= 18;
payload_offset += 14;
if (packet[14] == 0x81 && packet[15] == 0x00)
{
/* This is a VLAN tagged packet
* We don't do anything special (yet) */
payload_len -= 4;
payload_offset += 4;
}
ushort ethertype = (ushort)(((uint)packet[payload_offset - 2] << 8) |
packet[payload_offset - 1]);
System.Diagnostics.Debugger.Log(0, null, "Received packet of size " +
packet.Length.ToString() + ", ethertype: " + ethertype.ToString("X4") +
" from device " + dev_no.ToString());
if (packet_handlers.ContainsKey(ethertype))
{
packet_handlers[ethertype].PacketReceived(packet, dev_no, payload_offset, payload_len, src);
}
return(true);
}
public RPCResult <bool> PacketReceived(byte[] packet, int dev_no, int payload_offset,
int payload_len, p_addr src)
{
/* Parse the provided packet */
byte type = packet[payload_offset];
byte code = packet[payload_offset + 1];
uint csum = ipv4.calc_checksum(packet, payload_offset, payload_len);
if (csum != 0)
{
System.Diagnostics.Debugger.Log(0, null, "Received ICMP message with " +
"invalid checksum " + csum.ToString("X4"));
return(false);
}
ushort ident = net.ReadWord(packet, payload_offset + 4);
ushort seq = net.ReadWord(packet, payload_offset + 6);
System.Diagnostics.Debugger.Log(0, null, "ICMP message received from " +
src.ToString() + ": type: " + type.ToString() + ", code: " +
code.ToString() + ", ident: " + ident.ToString() + ", seq: " +
seq.ToString());
return(true);
}
List <RPCMessage> GetPendingRequestList(int dev_no, ushort etype,
p_addr addr)
{
/* Get the address->hwaddr dictionary, creating the various layers
* as we go */
Dictionary <ushort, Dictionary <p_addr, List <RPCMessage> > > nd_dict;
if (pending_reqs.TryGetValue(dev_no, out nd_dict) == false)
{
nd_dict = new Dictionary <ushort, Dictionary <p_addr, List <RPCMessage> > >(
new tysos.Program.MyGenericEqualityComparer <ushort>());
pending_reqs[dev_no] = nd_dict;
}
Dictionary <p_addr, List <RPCMessage> > prot_dict;
if (nd_dict.TryGetValue(etype, out prot_dict) == false)
{
prot_dict = new Dictionary <p_addr, List <RPCMessage> >(
new tysos.Program.MyGenericEqualityComparer <p_addr>());
nd_dict[etype] = prot_dict;
}
List <RPCMessage> ret;
if (prot_dict.TryGetValue(addr, out ret) == false)
{
ret = new List <RPCMessage>();
prot_dict[addr] = ret;
}
return(ret);
}
public override bool Equals(p_addr other)
{
IPv4Address o = other as IPv4Address;
if (o == null)
{
return(false);
}
return(addr == o.addr);
}
public override bool Equals(p_addr other)
{
HWAddr o = other as HWAddr;
if (o == null)
{
return(false);
}
for (int i = 0; i < 6; i++)
{
if (mac[i] != o.mac[i])
{
return(false);
}
}
return(true);
}
public void TransmitEthernetPacket(byte[] packet, int dev_no, int payload_offset,
int payload_len, p_addr dest, ushort etype)
{
/* Build an ethernet packet from the higher level packet passed to us.
*
* First, decide if we have enough space in the original buffer to
* build the packet around. We need 14 bytes before the packet and
* optionally 4 bytes after if the device doesn't add FCS.
*
* TODO: add support also for padding here */
var nd = devs[dev_no];
bool need_new_buf = false;
int header_len = 14;
if (payload_offset < header_len)
{
need_new_buf = true;
}
int crc_len = 0;
if (nd.dev_appends_crc_on_tx == false)
{
crc_len = 4;
}
if (packet.Length < payload_offset + payload_len + crc_len)
{
need_new_buf = true;
}
if (need_new_buf)
{
byte[] new_buf = new byte[header_len + crc_len + payload_len];
for (int i = 0; i < payload_len; i++)
{
new_buf[header_len + i] = packet[payload_offset + i];
}
payload_offset = header_len;
packet = new_buf;
}
// Add in the header
payload_offset -= header_len;
payload_len += header_len;
// dest
var d = dest as HWAddr;
if (d == null)
{
System.Diagnostics.Debugger.Log(0, null, "TransmitEthernetPacket: "
+ "dest is not a hardware address: " + dest.ToString());
return;
}
for (int i = 0; i < 6; i++)
{
packet[payload_offset + i] = d.MAC[i];
}
// src
for (int i = 0; i < 6; i++)
{
packet[payload_offset + 6 + i] = nd.HWAddr.MAC[i];
}
// ethertype
packet[payload_offset + 12] = (byte)((etype >> 8) & 0xff);
packet[payload_offset + 13] = (byte)(etype & 0xff);
// TODO: pad if required
if (nd.dev_pads_on_tx == false && payload_len + crc_len < 64)
{
System.Diagnostics.Debugger.Log(0, null, "TransmitEthernetPacket: "
+ "packet too short and device does not automatically pad");
return;
}
// TODO: add FCS if required
if (nd.dev_appends_crc_on_tx == false)
{
System.Diagnostics.Debugger.Log(0, null, "TransmitEthernetPacket: "
+ "device does not automatically add FCS");
return;
}
// Send packet
nd.s.TransmitPacket(packet, dev_no, payload_offset, payload_len, dest);
}
public RPCResult <bool> PacketReceived(byte[] packet, int dev_no, int payload_offset,
int payload_len, p_addr src)
{
ushort htype = net.ReadWord(packet, payload_offset);
ushort ptype = net.ReadWord(packet, payload_offset + 2);
byte hlen = packet[payload_offset + 4];
byte plen = packet[payload_offset + 5];
ushort oper = net.ReadWord(packet, payload_offset + 6);
if (htype != 1)
{
System.Diagnostics.Debugger.Log(0, null, "Packet with unknown HTYPE: " +
htype.ToString("X4"));
return(false);
}
if (hlen != 6)
{
System.Diagnostics.Debugger.Log(0, null, "Invalid HLEN: " +
hlen.ToString());
return(false);
}
HWAddr sha = net.ReadHWAddr(packet, payload_offset + 8);
HWAddr tha = net.ReadHWAddr(packet, payload_offset + 8 + hlen + plen);
var nd = net.devs[dev_no];
// Reject those packets sent by ourselves (don't reply to our own
// announce packets)
if (sha.Equals(nd.HWAddr))
{
return(false);
}
switch (ptype)
{
case 0x0800:
// IPv4
{
if (plen != 4)
{
System.Diagnostics.Debugger.Log(0, null, "Invalid HLEN: " +
hlen.ToString());
return(false);
}
IPv4Address spa = net.ReadIPv4Addr(packet, payload_offset + 14);
IPv4Address tpa = net.ReadIPv4Addr(packet, payload_offset + 24);
System.Diagnostics.Debugger.Log(0, null, "Received IPv4 packet: oper: " +
oper.ToString() + ", SHA: " + sha.ToString() +
", SPA: " + spa.ToString() +
", THA: " + tha.ToString() +
", TPA: " + tpa.ToString());
if (oper == 1)
{
/* Is it a request for our own IP address? */
//var addr = net.devs[dev_no].a as IPv4Address
if (net.devs[dev_no].a.Equals(tpa))
{
System.Diagnostics.Debugger.Log(0, null, "Received ARP request for our IP");
// build a response packet. We are usually directly above
// the link layer in the stack, so only need to reserve space
// for ethernet headers (if we are not big enough, the
// ethernet layer will do this for us anyway, but it involves
// a memcpy and so is less efficient)
byte[] ret = new byte[100];
int packet_offset = 32;
int packet_len = 0;
ret[packet_offset + packet_len++] = 0x00; // HTYPE
ret[packet_offset + packet_len++] = 0x01;
ret[packet_offset + packet_len++] = 0x08; // PTYPE
ret[packet_offset + packet_len++] = 0x00;
ret[packet_offset + packet_len++] = 0x06; // HLEN
ret[packet_offset + packet_len++] = 0x04; // PLEN
ret[packet_offset + packet_len++] = 0x00; // OPER
ret[packet_offset + packet_len++] = 0x02;
// SHA
for (int i = 0; i < 6; i++)
{
ret[packet_offset + packet_len++] = net.devs[dev_no].HWAddr.MAC[i];
}
// SPA
for (int i = 0; i < 4; i++)
{
ret[packet_offset + packet_len++] = tpa.Octets[i];
}
// THA
for (int i = 0; i < 6; i++)
{
ret[packet_offset + packet_len++] = sha.MAC[i];
}
// TPA
for (int i = 0; i < 4; i++)
{
ret[packet_offset + packet_len++] = spa.Octets[i];
}
/*net.InvokeAsync("TransmitPacket",
* new object[] { ret, dev_no, packet_offset, packet_len, sha },
* net.sig_packet);*/
net.TransmitEthernetPacket(ret, dev_no, packet_offset,
packet_len, sha, 0x0806);
}
else if (tpa.Equals(spa) && tha.Equals(HWAddr.Zero))
{
// This is a gratuitous request (announce) packet
// Interpret it as a response
oper = 2;
}
}
if (oper == 2)
{
/* This is a reply to a previous request, or an announcement */
// Cache the current response
System.Diagnostics.Debugger.Log(0, null, "Cacheing " +
spa.ToString() + " to " + sha.ToString());
GetProtocolDictionary(dev_no, 0x0800)[spa] = sha;
// Fulfil pending requests
var pending_list = GetPendingRequestList(dev_no, 0x0800,
spa);
while (pending_list.Count > 0)
{
var e = pending_list[pending_list.Count - 1];
System.Diagnostics.Debugger.Log(0, null, "Handling pending request");
((RPCResult <HWAddr>)e.result).Result = sha;
e.result.Set();
pending_list.RemoveAt(pending_list.Count - 1);
}
// Remove the pointer to the pending list so it is collected
pending_reqs[dev_no][0x0800].Remove(spa);
}
}
break;
default:
System.Diagnostics.Debugger.Log(0, null, "Packet with unknown PTYPE: " +
htype.ToString("X4"));
return(false);
}
return(true);
}
public RPCResult <HWAddr> ResolveAddress(int dev_no, p_addr addr)
{
/* Get the appropriate dictionary */
var prot_dict = GetProtocolDictionary(dev_no, addr.EtherType);
/* If the cache contains the value we need, then we can use it
* directly, otherwise we need to send a request for it */
HWAddr ret;
if (prot_dict.TryGetValue(addr, out ret) == true)
{
System.Diagnostics.Debugger.Log(0, null, "Resolving " +
addr.ToString() + " from cache to " + ret.ToString());
return(ret);
}
/* We need to send a ARP request out to the network, then respond to
* the reply. Unfortunately, messages sent to the arp subsystem are
* processed sequentially, therefore the reply will never be processed
* until the current function exits. To get around this, we tell the
* Invoke mechanism not to return from the synchronous call until
* we have that response */
var e = CurrentMessage;
e.EventSetsOnReturn = false;
/* Send the request */
switch (addr.EtherType)
{
case 0x0800:
/* IPv4 */
HWAddr sha = net.devs[dev_no].HWAddr;
IPv4Address spa = net.devs[dev_no].addresses[0x0800] as IPv4Address;
IPv4Address tpa = addr as IPv4Address;
byte[] pkt = new byte[100];
int packet_offset = 32;
int packet_len = 0;
pkt[packet_offset + packet_len++] = 0x00; // HTYPE
pkt[packet_offset + packet_len++] = 0x01;
pkt[packet_offset + packet_len++] = 0x08; // PTYPE
pkt[packet_offset + packet_len++] = 0x00;
pkt[packet_offset + packet_len++] = 0x06; // HLEN
pkt[packet_offset + packet_len++] = 0x04; // PLEN
pkt[packet_offset + packet_len++] = 0x00; // OPER
pkt[packet_offset + packet_len++] = 0x01;
// SHA
for (int i = 0; i < 6; i++)
{
pkt[packet_offset + packet_len++] = sha.MAC[i];
}
// SPA
for (int i = 0; i < 4; i++)
{
pkt[packet_offset + packet_len++] = spa.Octets[i];
}
// THA
for (int i = 0; i < 6; i++)
{
pkt[packet_offset + packet_len++] = 0;
}
// TPA
for (int i = 0; i < 4; i++)
{
pkt[packet_offset + packet_len++] = tpa.Octets[i];
}
net.TransmitEthernetPacket(pkt, dev_no, packet_offset,
packet_len, HWAddr.Multicast, 0x0806);
break;
}
// Store this request
GetPendingRequestList(dev_no, addr.EtherType, addr).Add(e);
// Return null for now until we actually get a result
return(null);
}
public RPCResult <bool> PacketReceived(byte[] packet, int dev_no, int payload_offset,
int payload_len, p_addr src)
{
/* Parse the provided packet */
// check version
uint ver = (uint)(packet[payload_offset] >> 4) & 0xfU;
if (ver != 4)
{
System.Diagnostics.Debugger.Log(0, null, "Packet dropped as version incorrect: " +
ver.ToString());
return(false);
}
// get header length (in bytes)
int hlen = (int)((packet[payload_offset] & 0xfU) * 4);
// checksum
uint csum = calc_checksum(packet, payload_offset, hlen);
if (csum != 0)
{
StringBuilder sb = new StringBuilder();
sb.Append("Packet dropped as checksum incorrect: ");
sb.Append(csum.ToString());
sb.Append(". Packet header length: ");
sb.Append(hlen.ToString());
sb.Append(", words: ");
for (int i = 0; i < hlen; i += 2)
{
if (i != 0)
{
sb.Append(" ");
}
sb.Append(net.ReadWord(packet, payload_offset + i).ToString("X4"));
}
System.Diagnostics.Debugger.Log(0, null, sb.ToString());
return(false);
}
// get packet length (excluding header)
int plen = net.ReadWord(packet, payload_offset + 2) - hlen;
// get fragment offset and flags field
ushort frag_off_flags = net.ReadWord(packet, payload_offset + 6);
uint flags = (uint)(frag_off_flags >> 13) & 0x3U;
// don't handle fragmented packets yet
if (flags != 0)
{
System.Diagnostics.Debugger.Log(0, null, "Packet dropped as fragmented");
return(false);
}
// get protocol
byte prot = packet[payload_offset + 9];
// get source and dest IPs
IPv4Address spa = net.ReadIPv4Addr(packet, payload_offset + 12);
IPv4Address dpa = net.ReadIPv4Addr(packet, payload_offset + 16);
System.Diagnostics.Debugger.Log(0, null, "Received packet from " +
spa.ToString() + " to " + dpa.ToString() + ", protocol 0x" +
prot.ToString("X2"));
// Do we accept the destination address?
bool found = false;
foreach (var addr in net.addrs)
{
// check octet by octet so we also catch broadcast addresses
IPv4Address tpa = addr.Key as IPv4Address;
if (tpa != null)
{
bool pass = true;
for (int i = 0; i < 4; i++)
{
uint mask = 0xffU << ((3 - i) * 8);
uint dpa_masked = dpa.addr & mask;
if (dpa_masked != mask && dpa_masked != (tpa.addr & mask))
{
pass = false;
break;
}
}
if (pass)
{
found = true;
break;
}
}
}
if (found)
{
IPacketHandler prot_handler;
if (packet_handlers.TryGetValue(prot, out prot_handler))
{
prot_handler.PacketReceived(packet, dev_no, payload_offset + hlen, plen, spa);
}
}
return(true);
}
