Newer
Older
#include <algorithm>
#include <cctype>
#include <arpa/inet.h>
#include "commsignia_layer_factory.hh"
#include "loggers.hh"
#include "converter.hh"
unsigned char commsignia_layer::_fixed_header[10] = { 0x11, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAA, 0x0D };
commsignia_layer::commsignia_layer(const std::string & p_type, const std::string & param) : layer(p_type), _params(), _mac_src(), _eth_type() {
loggers::get_instance().log(">>> commsignia_layer::commsignia_layer: %s, %s", to_string().c_str(), param.c_str());
// Setup parameters
params::convert(_params, param);
params::const_iterator it = _params.find(params::mac_src);
if (it == _params.cend()) {
_params.insert(std::pair<std::string, std::string>(std::string("mac_src"), "000000000000"));
}
_mac_src = converter::get_instance().hexa_to_bytes(_params[params::mac_src]);
it = _params.find(params::mac_bc);
if (it == _params.cend()) {
_params.insert(std::pair<std::string, std::string>(std::string("mac_bc"), "FFFFFFFFFFFF"));
}
it = _params.find(params::eth_type);
if (it == _params.cend()) {
_params.insert(std::pair<std::string, std::string>(std::string("eth_type"), "8947"));
}
_eth_type = converter::get_instance().hexa_to_bytes(_params[params::eth_type]);
it = _params.find(params::interface_id);
if (it == _params.cend()) {
_params.insert(std::pair<std::string, std::string>(std::string("interface_id"), "1")); // The interfce id for filtering & to send on
it = _params.find(std::string("power_tx"));
if (it == _params.cend()) {
_params.insert(std::pair<std::string, std::string>(std::string("power_tx"), "20")); // 20db
}
it = _params.find(std::string("data_rate"));
if (it == _params.cend()) {
_params.insert(std::pair<std::string, std::string>(std::string("data_rate"), "12")); // 12 * 500Kbps = 6Mbps
}
it = _params.find(std::string("use_vpn"));
if (it == _params.cend()) {
_params.insert(std::pair<std::string, std::string>(std::string("use_vpn"), "0"));
}
//_params.log();
}
void commsignia_layer::send_data(OCTETSTRING& data, params& params) {
loggers::get_instance().log_msg(">>> commsignia_layer::send_data: ", data);
OCTETSTRING buffer(10, commsignia_layer::_fixed_header);
if (_params[std::string("use_vpn")].compare("1") == 0) {
buffer += int2oct(1, 1); // Injection to software
} else {
buffer += int2oct(0, 1); // Injection to radio
}
buffer += int2oct(0, 1); // Fix
loggers::get_instance().log_msg("commsignia_layer::send_data: buffer: Injection=", buffer);
buffer += int2oct(std::stoi(_params[params::interface_id]), 4);
loggers::get_instance().log_msg("commsignia_layer::send_data: buffer: Interface=", buffer);
buffer += int2oct(std::stoi(_params[std::string("data_rate")]), 2);
loggers::get_instance().log_msg("commsignia_layer::send_data: buffer: data_rate=", buffer);
buffer += int2oct(0x80000000 & std::stoi(_params[std::string("power_tx")]), 4); // Negative number
loggers::get_instance().log_msg("commsignia_layer::send_data: buffer: PowerTx=", buffer);
// Destination MAC address
params::const_iterator it = params.find(params::mac_dst); // Find in provided parameters, params
if (it != params.cend()) {
buffer += str2oct(CHARSTRING(it->second.c_str()));
} else {
it = _params.find(params::mac_dst);
if (it != _params.cend()) {
buffer += str2oct(CHARSTRING(it->second.c_str()));
buffer += str2oct(CHARSTRING(_params[params::mac_bc].c_str()));
loggers::get_instance().log_msg("commsignia_layer::send_data: buffer: mac_dst=", buffer);
// Source MAC address
it = params.find(params::mac_src); // Find in provided parameters, params
if (it != params.cend()) {
buffer += str2oct(CHARSTRING(it->second.c_str()));
} else {
it = _params.find(params::mac_src);
if (it != _params.cend()) {
buffer += str2oct(CHARSTRING(it->second.c_str()));
} else {
buffer += str2oct(CHARSTRING(_params[params::mac_src].c_str()));
}
loggers::get_instance().log_msg("commsignia_layer::send_data: buffer: mac_src=", buffer);
loggers::get_instance().log_msg("commsignia_layer::send_data: buffer: Fixed=", buffer);
buffer += int2oct(data.lengthof(), 2); // Data length
loggers::get_instance().log_msg("commsignia_layer::send_data: buffer: Data length=", buffer);
buffer += data; // Payload
loggers::get_instance().log_msg("commsignia_layer::send_data: Final buffer=", buffer);
}
void commsignia_layer::receive_data(OCTETSTRING& data, params& params) {
loggers::get_instance().log_msg(">>> commsignia_layer::receive_data: ", data);
const unsigned char* p = static_cast<const unsigned char *>(data);
const unsigned char c2p_ver = (unsigned char)*p;
if ((c2p_ver != 0x11) && (c2p_ver != 0x17)) { // Rx G5 or LTE-V2X
// Discard it, only use G5 or LTE-V2X RX version
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
if (c2p_ver == 0x11) { // G5
commsignia_layer::c2p_s_v1_rx_t r;
r.s_header.u8_ver_type = (unsigned char)*p++;
loggers::get_instance().log("commsignia_layer::receive_data: version=%02x", r.s_header.u8_ver_type);
memcpy((void*)&r.s_header.u32_tst_sec, (unsigned int*)p, sizeof(unsigned int)); p += sizeof(unsigned int);
loggers::get_instance().log("commsignia_layer::receive_data: time (in s)=%08x", /*ntohl(*/r.s_header.u32_tst_sec)/*)*/;
memcpy((void*)&r.s_header.u32_tst_msec, (unsigned int*)p, sizeof(unsigned int)); p += sizeof(unsigned int);
loggers::get_instance().log("commsignia_layer::receive_data: time (in ms)=%08x", /*ntohl(*/r.s_header.u32_tst_msec)/*)*/;
r.u8_primary_channel = (unsigned char)*p++;
loggers::get_instance().log("commsignia_layer::receive_data: primary_channel=%02x", r.u8_primary_channel);
r.u8_secondary_channel = (unsigned char)*p++;
loggers::get_instance().log("commsignia_layer::receive_data: secondary_channel=%02x", r.u8_secondary_channel);
r.u8_used_interface = (unsigned char)*p++;
loggers::get_instance().log("commsignia_layer::receive_data: used_interface=%02x", r.u8_used_interface);
r.u8_data_rate = (unsigned char)*p++;
loggers::get_instance().log("commsignia_layer::receive_data: data_rate=%02x", r.u8_data_rate);
r.u8_antenna = (unsigned char)*p++;
loggers::get_instance().log("commsignia_layer::receive_data: antenna=%02x", r.u8_antenna);
memcpy((void*)&r.s32_latitude, (int*)p, sizeof(int)); p += sizeof(int);
loggers::get_instance().log("commsignia_layer::receive_data: latitude=%08x", r.s32_latitude);
memcpy((void*)&r.s32_longitude, (int*)p, sizeof(int)); p += sizeof(int);
loggers::get_instance().log("commsignia_layer::receive_data: longitude=%08x", r.s32_longitude);
memcpy((void*)&r.u16_speed, (unsigned short*)p, sizeof(unsigned short)); p += sizeof(unsigned short);
loggers::get_instance().log("commsignia_layer::receive_data: speed=%02x", r.u16_speed); // ntohs
memcpy((void*)&r.u16_heading, (unsigned short*)p, sizeof(unsigned short)); p += sizeof(unsigned short);
loggers::get_instance().log("commsignia_layer::receive_data: heading=%02x", r.u16_heading);
r.s8_rssi_ant_1 = (unsigned char)*p++;
loggers::get_instance().log("commsignia_layer::receive_data: s8_rssi_ant_1=%02x", r.s8_rssi_ant_1);
r.s8_rssi_ant_2 = (unsigned char)*p++;
loggers::get_instance().log("commsignia_layer::receive_data: s8_rssi_ant_2=%02x", r.s8_rssi_ant_2);
r.s8_noise_ant_1 = (unsigned char)*p++;
loggers::get_instance().log("commsignia_layer::receive_data: s8_noise_ant_1=%02x", r.s8_noise_ant_1);
r.s8_noise_ant_2 = (unsigned char)*p++;
loggers::get_instance().log("commsignia_layer::receive_data: s8_noise_ant_2=%02x", r.s8_noise_ant_2);
memcpy((void*)&r.u16_cbr_ant_1, (unsigned short*)p, sizeof(unsigned short)); p += sizeof(unsigned short);
loggers::get_instance().log("commsignia_layer::receive_data: cbr_ant_1=%04x", r.u16_cbr_ant_1);
memcpy((void*)&r.u16_cbr_ant_2, (unsigned short*)p, sizeof(unsigned short)); p += sizeof(unsigned short);
loggers::get_instance().log("commsignia_layer::receive_data: cbr_ant_2=%04x", r.u16_cbr_ant_2);
// Filtering on antenna index
loggers::get_instance().log("commsignia_layer::receive_data: compare %02x with %02x", r.u8_antenna, static_cast<unsigned char>(std::stoi(_params[params::interface_id])));
if (r.u8_antenna != std::stoi(_params[params::interface_id])) {
// Discard packet
loggers::get_instance().warning("commsignia_layer::receive_data: Discard packet due to wrong antenna id");
// TODO return;
} // else, continue
} else { // LTE-CV2X
commsignia_layer::c2p_s_v1_rx_cv2x_t r;
r.s_header.u8_ver_type = (unsigned char)*p++;
loggers::get_instance().log("commsignia_layer::receive_data: version=%02x", r.s_header.u8_ver_type);
memcpy((void*)&r.s_header.u32_tst_sec, (unsigned int*)p, sizeof(unsigned int)); p += sizeof(unsigned int);
loggers::get_instance().log("commsignia_layer::receive_data: time (in s)=%08x", /*ntohl(*/r.s_header.u32_tst_sec)/*)*/;
memcpy((void*)&r.s_header.u32_tst_msec, (unsigned int*)p, sizeof(unsigned int)); p += sizeof(unsigned int);
loggers::get_instance().log("commsignia_layer::receive_data: time (in ms)=%08x", /*ntohl(*/r.s_header.u32_tst_msec)/*)*/;
r.u8_socket_index = (unsigned char)*p++;
loggers::get_instance().log("commsignia_layer::receive_data: u8_socket_index=%02x", r.u8_socket_index);
memcpy((void*)&r.u16_ethertype, (unsigned short*)p, sizeof(unsigned short)); p += sizeof(unsigned short);
loggers::get_instance().log("commsignia_layer::receive_data: u16_ethertype=%04x", r.u16_ethertype);
r.s8_rssi = (signed char)*p++;
loggers::get_instance().log("commsignia_layer::receive_data: s8_rssi=%02x", r.s8_rssi);
r.u8_datarate_500kbps = (unsigned char)*p++;
loggers::get_instance().log("commsignia_layer::receive_data: u8_datarate_500kbps=%02x", r.u8_datarate_500kbps);
}
commsignia_layer::c2p_802_11p_hdr h;
memcpy((void*)&h.frame_ctrl, (unsigned short*)p, sizeof(unsigned short)); p += sizeof(unsigned short);
loggers::get_instance().log("commsignia_layer::receive_data: frame_ctrl=%04x", h.frame_ctrl);
memcpy((void*)&h.duration, (unsigned short*)p, sizeof(unsigned short)); p += sizeof(unsigned short);
loggers::get_instance().log("commsignia_layer::receive_data: duration=%04x", h.duration);
memcpy((void*)&h.dst_addr, (unsigned short*)p, 6 * sizeof(unsigned char)); p += 6 * sizeof(unsigned char);
memcpy((void*)&h.src_addr, (unsigned short*)p, 6 * sizeof(unsigned char)); p += 6 * sizeof(unsigned char);
memcpy((void*)&h.bss_id, (unsigned short*)p, 6 * sizeof(unsigned char)); p += 6 * sizeof(unsigned char);
memcpy((void*)&h.fragment_seq_num, (unsigned short*)p, sizeof(unsigned short)); p += sizeof(unsigned short);
loggers::get_instance().log("commsignia_layer::receive_data: fragment_seq_num=%04x", h.fragment_seq_num);
OCTETSTRING dst = OCTETSTRING(6, (const unsigned char*)&h.dst_addr);
loggers::get_instance().log_msg("commsignia_layer::receive_data: dst: ", dst);
OCTETSTRING src = OCTETSTRING(6, (const unsigned char*)&h.src_addr);
loggers::get_instance().log_msg("commsignia_layer::receive_data: src: ", src);
// Filtering on source MAC address of the packet
if (!std::equal(_mac_src.cbegin(), _mac_src.cend(), static_cast<const unsigned char*>(src))) {
// Discard packet
loggers::get_instance().warning("commsignia_layer::receive_data: Discard packet due to wrong comparison");
} // else, continue
const commsignia_layer::c2p_llc_hdr* l;
int length;
if (c2p_ver == 0x11) {
if ((ntohs(h.frame_ctrl) & 0xf000) == 0x8000) {
l = (const commsignia_layer::c2p_llc_hdr*)(static_cast<const unsigned char *>(data) + sizeof(commsignia_layer::c2p_s_v1_rx_t) + sizeof(commsignia_layer::c2p_802_11p_hdr) + sizeof(commsignia_layer::c2p_qos_ctrl));
length = sizeof(commsignia_layer::c2p_s_v1_rx_t) + sizeof(commsignia_layer::c2p_802_11p_hdr) + sizeof(commsignia_layer::c2p_qos_ctrl) + sizeof(commsignia_layer::c2p_llc_hdr);
} else {
l = (const commsignia_layer::c2p_llc_hdr*)(p + sizeof(commsignia_layer::c2p_s_v1_rx_t) + sizeof(commsignia_layer::c2p_802_11p_hdr));
length = sizeof(commsignia_layer::c2p_s_v1_rx_t) + sizeof(commsignia_layer::c2p_802_11p_hdr) + sizeof(commsignia_layer::c2p_llc_hdr);
}
loggers::get_instance().log("commsignia_layer::receive_data: dsap=%02x", l->dsap);
loggers::get_instance().log("commsignia_layer::receive_data: ssap=%02x", l->ssap);
loggers::get_instance().log("commsignia_layer::receive_data: ssap=%02x", l->ctrl);
loggers::get_instance().log("commsignia_layer::receive_data: type=%04x", l->type);
//if ((_eth_type[1] == (unsigned char)((l->type & 0xff00) >> 8)) && (_eth_type[0] == (unsigned char)(l->type & 0xff))) { // Warning: Network ordered bytes
// Extract payload
data = OCTETSTRING(data.lengthof() - length, length + static_cast<const unsigned char *>(data));
//loggers::get_instance().log_msg("commsignia_layer::receive_data: payload for upper layer:", data);
// Update params
CHARSTRING s = oct2str(dst);
params[params::mac_dst] = std::string(static_cast<const char *>(s));
s = oct2str(src);
params[params::mac_src] = std::string(static_cast<const char *>(s));
receive_to_all_layers(data, params);
/*} else {
// Discard packet
loggers::get_instance().warning("commsignia_layer::receive_data: Discard packet due to wrong ethernet type");
//TODO return;
}*/
}
commsignia_layer_factory commsignia_layer_factory::_f;