From 5795bc1d3212dfebd8223b971c5f07c6a0b5f710 Mon Sep 17 00:00:00 2001 From: /jdi/ Date: Thu, 24 Sep 2015 20:01:26 +0200 Subject: [PATCH] parse header --- src/Program.cpp | 25 +++++--------- src/smrtlink.cpp | 8 ----- src/transfer/Packet.cpp | 76 ++++++++++++++++++++++++++++++++++------- src/transfer/Packet.h | 12 +++++-- src/transfer/Socket.cpp | 65 ++++++++++++++++++----------------- src/transfer/Socket.h | 16 +++++---- 6 files changed, 123 insertions(+), 79 deletions(-) diff --git a/src/Program.cpp b/src/Program.cpp index 6052fbb..7b49b0b 100644 --- a/src/Program.cpp +++ b/src/Program.cpp @@ -6,11 +6,12 @@ */ #include +#include "Utils.h" #include "Program.h" -#include "Device.h" -#include "Host.h" -#include "Socket.h" -#include "Packet.h" +#include "device/Device.h" +#include "device/Host.h" +#include "transfer/Socket.h" +#include "transfer/Packet.h" Program::Program() { // TODO Auto-generated constructor stub @@ -22,28 +23,20 @@ Program::~Program() { int Program::run() { Device d = Device(); - printf(" %d", d.getName()); - - bytes b = { 255, 255, 0, 0}; + printf(" %d\n", d.getName()); + bytes b = { 255, 255, 0, 0 }; Host h = Host(); Packet p = Packet(Packet::DISCOVERY); p.setBody(b); p.setHostMac(h.getMac()); bytes a = p.getBytes(); - printf("\na ="); - for (unsigned i = 0; i < sizeof(a); i++) - printf(" %d", a[i]); p.encode(a); - printf("\nb ="); - for (unsigned i = 0; i < sizeof(a); i++) - printf(" %d", a[i]); - printf("\n"); try { asio::io_service io_service; - Socket s(io_service, dst_port, src_port); - //s.listen(); + Socket s(io_service); + s.init(dst_port, src_port); s.send(a); io_service.run(); } catch (std::exception& e) { diff --git a/src/smrtlink.cpp b/src/smrtlink.cpp index bc77e8e..c0e5b2c 100644 --- a/src/smrtlink.cpp +++ b/src/smrtlink.cpp @@ -84,14 +84,6 @@ int main(int argc, char *argv[]) { optind++; } - if (p.src_port != 0) { - printf("src_port = %d\n", p.src_port); - } - - if (p.dst_port != 0) { - printf("dst_port = %d\n", p.dst_port); - } - if (p.run()) exit(EXIT_SUCCESS); else diff --git a/src/transfer/Packet.cpp b/src/transfer/Packet.cpp index 69fe3a9..fc1ec6f 100644 --- a/src/transfer/Packet.cpp +++ b/src/transfer/Packet.cpp @@ -5,11 +5,13 @@ * Author: jdi */ -#include +#include #include -#include +#include +#include #include "Packet.h" #include "../Types.h" +#include "../Utils.h" Packet::Packet(OpCode c) { srand(time(NULL)); @@ -17,13 +19,10 @@ Packet::Packet(OpCode c) { opCode = c; } -Packet::~Packet() { - // TODO Auto-generated destructor stub -} - bytes Packet::getBytes() { int i = 0; - push(body, i, payload); + for (unsigned j = 0; j < payload.size(); j++) + push(body, i, payload[j]); push(body, i, (int) PACKET_END); i = 0; push(head, i, version); @@ -31,16 +30,44 @@ bytes Packet::getBytes() { push(head, i, switchMac); push(head, i, hostMac); push(head, i, sequenceId); - push(head, i, err); + push(head, i, errorCode); push(head, i, this->getLength()); push(head, i, fragmentOffset); push(head, i, flag); push(head, i, tokenId); push(head, i, checkSum); bytes data = head + body; + utils::printBytes("Send Head:\t",head); return data; } +void Packet::parse(bytes data) { + memcpy(&head[0], &data[0], HEADER_LEN); + body.resize(data.size() - HEADER_LEN); + memcpy(&body[0], &data[HEADER_LEN], data.size() - HEADER_LEN); + utils::printBytes("Receive Head:\t",head); + int i = 0; + short checkLen = 0x0; + pull(head, i, version); + pull(head, i, opCode); + pull(head, i, switchMac, switchMac.size()); + pull(head, i, hostMac, hostMac.size()); + pull(head, i, sequenceId); + pull(head, i, errorCode); + pull(head, i, checkLen); + pull(head, i, fragmentOffset); + pull(head, i, flag); + pull(head, i, tokenId); + pull(head, i, checkSum); + utils::printBytes("MAC: ", switchMac); + utils::printBytes("MAC: ", hostMac); + if(this->getLength()!= checkLen){ + printf("Packet Length doesn't match: %hd != %hd\n", this->getLength(), checkLen); + } + dataset d; + payload = {}; +} + void Packet::setBody(bytes data) { this->body = data; } @@ -118,8 +145,7 @@ void Packet::encode(bytes &data) { } void Packet::push(bytes &arr, int &index, byte data) { - arr[index] = data; - index++; + arr[index++] = data; } void Packet::push(bytes &arr, int &index, bytes data) { @@ -151,7 +177,31 @@ void Packet::push(bytes &arr, int &index, dataset data) { push(arr, index, data.value); } -void Packet::push(bytes &arr, int &index, datasets data) { - for (unsigned j = 0; j < data.size(); j++) - push(arr, index, data[j]); +void Packet::pull(bytes &arr, int &index, byte &ret) { + ret = arr[index++]; +} + +void Packet::pull(bytes &arr, int &index, bytes &ret, unsigned len) { + ret.resize(len); + memcpy(&ret[0], &arr[index], len); + index+=len; +} + +void Packet::pull(bytes &arr, int &index, short &ret) { + ret = (arr[index++] << 8); + ret |= arr[index++] & 0xFF; + ret &=0xFFFF; +} + +void Packet::pull(bytes &arr, int &index, int &ret) { + ret = arr[index++] << 24; + ret |= (arr[index++] & 0xFF) << 16; + ret |= (arr[index++] & 0xFF) << 8; + ret |= arr[index++] & 0xFF; +} + +void Packet::pull(bytes &arr, int &index, dataset &ret) { + pull(arr, index, ret.type); + pull(arr, index, ret.len); + pull(arr, index, ret.value, (unsigned) ret.len); } diff --git a/src/transfer/Packet.h b/src/transfer/Packet.h index 1c31fb1..3eb6d5a 100644 --- a/src/transfer/Packet.h +++ b/src/transfer/Packet.h @@ -19,9 +19,10 @@ public: DISCOVERY, GET, SET }; Packet(OpCode); - virtual ~Packet(); + virtual ~Packet(){}; void encode(bytes&); bytes getBytes(); + void parse(bytes); short getLength() const ; int getCheckSum() const; short getSequenceId() const; @@ -47,7 +48,7 @@ private: short sequenceId; short tokenId = 0; short fragmentOffset = 0; - int err = 0; + int errorCode = 0; int checkSum = 0; short flag = 0; @@ -59,7 +60,12 @@ private: void push(bytes&, int&, byte); void push(bytes&, int&, bytes); void push(bytes&, int&, dataset); - void push(bytes&, int&, datasets); + + void pull(bytes&, int&, short&); + void pull(bytes&, int&, int&); + void pull(bytes&, int&, byte&); + void pull(bytes&, int&, bytes&, unsigned); + void pull(bytes&, int&, dataset&); }; #endif /* PACKET_H_ */ diff --git a/src/transfer/Socket.cpp b/src/transfer/Socket.cpp index 5747129..d79a285 100644 --- a/src/transfer/Socket.cpp +++ b/src/transfer/Socket.cpp @@ -6,75 +6,76 @@ */ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include #include +#include #include #include "Socket.h" #include "../Types.h" #include "../device/Host.h" #include "Packet.h" -Socket::Socket(asio::io_service& io_service, short dst_port, short src_port) : - socket_(io_service) { +Socket::Socket(asio::io_service& io_service) : + send_socket_(io_service), receive_socket_(io_service) { +} +void Socket::init(short dst_port, short src_port) { + + wildcard_endpoint_ = asio::ip::udp::endpoint( + asio::ip::address::from_string("0.0.0.0"), src_port); local_endpoint_ = asio::ip::udp::endpoint( - asio::ip::address::from_string("192.168.0.3"), src_port); //a - remote_endpoint_ = asio::ip::udp::endpoint( - asio::ip::address_v4::from_string("255.255.255.255"), dst_port); + asio::ip::address::from_string("192.168.0.3"), src_port); broadcast_endpoint_ = asio::ip::udp::endpoint( asio::ip::address_v4::from_string("255.255.255.255"), dst_port); - asio::ip::address_v4 local_addr = asio::ip::address_v4::from_string( - "192.168.0.3"); - socket_.open(asio::ip::udp::v4()); - socket_.bind(local_endpoint_); //TODO reuse Address + send_socket_.open(asio::ip::udp::v4()); + send_socket_.set_option(asio::socket_base::broadcast(true)); + send_socket_.set_option(asio::socket_base::reuse_address(true)); + send_socket_.bind(local_endpoint_); //TODO reuse Address - //socket_.set_option(asio::ip::multicast::outbound_interface(local_addr)); // TODO broadcast outbound interface - socket_.set_option(asio::socket_base::broadcast(true)); - socket_.set_option(asio::socket_base::reuse_address(true)); + receive_socket_.open(asio::ip::udp::v4()); + receive_socket_.set_option(asio::socket_base::broadcast(true)); + receive_socket_.set_option(asio::socket_base::reuse_address(true)); + receive_socket_.bind(wildcard_endpoint_); //TODO reuse Address } -Socket::~Socket() { -// TODO Auto-generated destructor stub -} void Socket::send(bytes data) { - unsigned char * a = &data[0]; - - socket_.async_send_to(asio::buffer(a, data.size()), broadcast_endpoint_, + send_socket_.async_send_to(asio::buffer(a, data.size()), + broadcast_endpoint_, [this](asio::error_code ec, std::size_t bytes_sent) { listen(); }); - - printf("Send\n"); - } void Socket::listen() { - socket_.async_receive_from(asio::buffer(data_, max_length), + receive_socket_.async_receive_from(asio::buffer(data_, MAX_LENGTH), remote_endpoint_, [this](asio::error_code ec, std::size_t bytes_recvd) { - printf("Echo"); if (ec || bytes_recvd == 0) { listen(); } else { - printf("Receive %s\n", data_); - bytes b = { 255, 255, 0, 0}; - Host h = Host(); + data_.resize(bytes_recvd); Packet p = Packet(Packet::DISCOVERY); + p.encode(data_); + p.parse(data_); + /* + sleep(1); + bytes b = {255, 255, 0, 0}; + Host h = Host(); + p = Packet(Packet::DISCOVERY); p.setBody(b); p.setHostMac(h.getMac()); bytes a = p.getBytes(); p.encode(a); send(a); + */ } }); diff --git a/src/transfer/Socket.h b/src/transfer/Socket.h index 06840cf..d0332dd 100644 --- a/src/transfer/Socket.h +++ b/src/transfer/Socket.h @@ -11,21 +11,23 @@ #include #include "../Types.h" +#define MAX_LENGTH 1024 + class Socket { public: - Socket(asio::io_service&, short, short); - virtual ~Socket(); + Socket(asio::io_service&); + virtual ~Socket(){}; + void init(short, short); void send(bytes); void listen(); private: - asio::ip::udp::socket socket_; + asio::ip::udp::socket send_socket_; + asio::ip::udp::socket receive_socket_; asio::ip::udp::endpoint broadcast_endpoint_; asio::ip::udp::endpoint remote_endpoint_; + asio::ip::udp::endpoint wildcard_endpoint_; asio::ip::udp::endpoint local_endpoint_; - enum { - max_length = 1024 - }; - char data_[max_length]; + bytes data_ = bytes(MAX_LENGTH); };