URL
https://opencores.org/ocsvn/fpga-cf/fpga-cf/trunk
Subversion Repositories fpga-cf
[/] [fpga-cf/] [trunk/] [cpp/] [fcpprotocol.cpp] - Rev 11
Go to most recent revision | Compare with Previous | Blame | View Log
// fcpprotocol.cpp #include "fcpprotocol.hpp" #include "rawethernet.hpp" #include <iostream> #include <cstring> #include <arpa/inet.h> using namespace std; FCPProtocol::FCPProtocol() { this->snd_cur = 0; this->last_ack = 0; this->enet = new RawEthernet(); } FCPProtocol::~FCPProtocol() { delete this->enet; } bool FCPProtocol::connect(byte mac[], byte ip[]) { if (!this->enet->connect(mac)) return false; memcpy((void*)this->ip, (void*)ip, 4); // send connection request memcpy((void*)this->buffer, (void*)this->packet_con, 34); memcpy((void*)(this->buffer+16), (void*)ip, 4); if (!this->enet->sendData(this->buffer, 34)) return false; // receive conack if (!this->receiveFcpUdpIp(1500, this->buffer)) return false; if (this->buffer[28] != (byte)0x1) return false; memcpy((void*)(this->sendbuf+16), (void*)this->ip, 4); this->snd_cur = 1; this->last_ack = 0; return true; } void FCPProtocol::disconnect() { this->enet->disconnect(); this->snd_cur = 0; this->last_ack = 0; } bool FCPProtocol::sendData(int channel, byte data[], int len) { // wrap in packet this->wrapFcpUdpIp(FCP_DATA_SND, channel, data, len); // send packet if (!this->enet->sendData(this->buffer, len+34)) return false; // receive ack if (!this->receiveFcpUdpIp(1500, this->buffer)) return false; int seq = (int) ntohs(*((short*)&this->buffer[30])); if ((seq != this->snd_cur) || (this->buffer[28] != 1)) return false; this->last_ack = seq; this->snd_cur = seq+1; //this->snd_cur++; return true; } bool FCPProtocol::requestData(int channel, int len, byte *data) { // send data request wrapFcpUdpIp(FCP_DATA_REQ, channel, NULL, len); if (!this->enet->sendData(this->buffer, 34)) return false; // receive data response if (!this->receiveFcpUdpIp(1500, this->buffer)) return false; int seq = (int) ntohs(*((short*)&this->buffer[30])); if ((seq != this->snd_cur) || (this->buffer[28] != 5)) return false; this->last_ack = seq; this->snd_cur = seq+1; memcpy((void*)data, (void*)(&this->buffer[34]), len); return true; } bool FCPProtocol::connected() { return (this->snd_cur > 0); } void FCPProtocol::wrapFcpUdpIp(byte command, int channel, byte *data, int len) { memcpy((void*)(this->buffer), (void*)this->ip_header, 20); memcpy((void*)(this->buffer+16), (void*)ip, 4); *((short*)&buffer[2]) = htons(34+len); //*** insert IP header checksum memcpy((void*)(this->buffer+20), (void*)this->udp_header, 8); *((short*)&buffer[24]) = htons(len+14); buffer[26] = 0; buffer[27] = 0; buffer[28] = command; buffer[29] = (byte)channel; *((short*)&(this->buffer[30])) = htons(this->snd_cur); *((short*)&(this->buffer[32])) = htons(len); if (len > 0 && data != NULL) memcpy((void*)(this->buffer+34), (void*)data, len); this->insertIPChecksum(); } void FCPProtocol::insertIPChecksum() { int sum = 0; short* buf = (short*)this->buffer; for (int i=0; i<20; i++) { sum += (int) ntohs(buf[i]); if (sum > 0xffff) { sum = (sum & 0xffff) + 1; } } buf[5] = ~htons((short)sum&0xffff); } bool FCPProtocol::receiveFcpUdpIp(int len, byte *data) { for (int i=0; i<20; i++) { if (!this->enet->requestData(len, data)) return false; if (data[9] != 0x11) continue; if (ntohs(*((short*)(data+20))) != 0x3001) continue; if (ntohs(*((short*)(data+22))) != 0x3000) continue; return true; } return false; } const byte FCPProtocol::packet_con[] = {//0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x45, 0, 0, 34/*len*/, 0, 0, 0, 0, 0x80, 0x11, 0, 0, 0xc0, 0xa8, 1, 0x66, 0, 0, 0, 0, 0x30, 0, 0x30, 1, 0, 0x0e, 0, 0, 0x02, 0, 0, 0, 0, 0};// len=34 const byte FCPProtocol::ip_header[] = {0x45, 0, 0, 34, 0, 0, 0, 0, 0x80, 0x11, 0, 0, 0xc0, 0xa8, 1, 0x66, 0, 0, 0, 0};// len=20 const byte FCPProtocol::udp_header[] = {0x30, 0, 0x30, 1, 0, 0x06, 0, 0};// len=8
Go to most recent revision | Compare with Previous | Blame | View Log