// list of packet->next from key:[{},{}]
packet_t packet_get_packets(packet_t p, char *key)
{
int i;
packet_t parr, pent, plast, pret = NULL;
if(!p || !key) return NULL;
parr = packet_get_packet(p, key);
if(!parr || *parr->json != '[')
{
packet_free(parr);
return NULL;
}
// parse each object in the array, link together
for(i=0;parr->js[i];i+=2)
{
pent = packet_new();
packet_json(pent, parr->json+parr->js[i], parr->js[i+1]);
if(!pret) pret = pent;
else plast->next = pent;
plast = pent;
}
packet_free(parr);
return pret;
}
// create a new open packet
packet_t crypt_openize_1a(crypt_t self, crypt_t c, packet_t inner)
{
unsigned char secret[uECC_BYTES], iv[16], hash[32];
packet_t open;
int inner_len;
crypt_1a_t cs = (crypt_1a_t)c->cs, scs = (crypt_1a_t)self->cs;
open = packet_chain(inner);
packet_json(open,&(self->csid),1);
inner_len = packet_len(inner);
if(!packet_body(open,NULL,4+40+inner_len)) return NULL;
// copy in the line public key
memcpy(open->body+4, cs->line_public, 40);
// get the shared secret to create the iv+key for the open aes
if(!uECC_shared_secret(cs->id_public, cs->line_private, secret)) return packet_free(open);
crypt_hash(secret,uECC_BYTES,hash);
fold1(hash,hash);
memset(iv,0,16);
iv[15] = 1;
// encrypt the inner
aes_128_ctr(hash,inner_len,iv,packet_raw(inner),open->body+4+40);
// generate secret for hmac
if(!uECC_shared_secret(cs->id_public, scs->id_private, secret)) return packet_free(open);
hmac_256(secret,uECC_BYTES,open->body+4,40+inner_len,hash);
fold3(hash,open->body);
return open;
}
packet_t packet_parse(unsigned char *raw, unsigned short len)
{
packet_t p;
uint16_t nlen, jlen;
// make sure is at least size valid
if(!raw || len < 2) return NULL;
memcpy(&nlen,raw,2);
jlen = platform_short(nlen);
if(jlen > len-2) return NULL;
// copy in and update pointers
p = packet_new();
if(!(p->raw = realloc(p->raw,len))) return packet_free(p);
memcpy(p->raw,raw,len);
p->json_len = jlen;
p->json = p->raw+2;
p->body_len = len-(2+p->json_len);
p->body = p->raw+(2+p->json_len);
// parse json (if any) and validate
if(jlen >= 2 && js0n(p->json,p->json_len,p->js,JSONDENSITY)) return packet_free(p);
return p;
}
/* Free a flow_t object */
int
flow_free(flow_t *f)
{
packet_t *cp;
http_pair_t *h;
while(f->pkt_dst_f != NULL){
cp = f->pkt_dst_f;
f->pkt_dst_f = f->pkt_dst_f->next;
packet_free(cp);
}
while(f->pkt_src_f != NULL){
cp = f->pkt_src_f;
f->pkt_src_f = f->pkt_src_f->next;
packet_free(cp);
}
if(f->order != NULL){
order_free(f->order);
}
if(f->http_f != NULL){
h = f->http_f;
f->http_f = f->http_f->next;
http_free(h);
}
free(f);
return 0;
}
// client handlers
void vc_handler_createchannel(ascnhalf_socket *s, packet *p)
{
uint8 type;
uint32 request_id;
packet reply;
voice_channel * chn;
uint8 error;
uint16 reply_id;
type = packet_readu8(p);
request_id = packet_readu32(p);
// attempt to create the channel
chn = voice_channel_create((int)type, (void*)s);
if( chn == NULL )
{
// channel creation failed
error = 1;
packet_init(VOICECHAT_SMSG_CHANNEL_CREATED, 5, &reply);
packet_writeu32(&reply, request_id);
packet_writeu8(&reply, error);
// send reply
socket_send(s, &reply);
// free buffer
packet_free(&reply);
}
else
{
// channel creation successful
error = 0;
reply_id = chn->channel_id;
packet_init(VOICECHAT_SMSG_CHANNEL_CREATED, 7, &reply);
packet_writeu32(&reply, request_id);
packet_writeu8(&reply, error);
packet_writeu16(&reply, reply_id);
// send reply
socket_send(s, &reply);
// free buffer
packet_free(&reply);
// increment this
++s->channelcount;
++g_channelCount;
}
}
int process_get_public_key(CLIENT *client, const VCRYPT_PACKET *packet)
{
MYSQL_RES *res = db_select("select public_key from users where username=%s",
packet->username);
VCRYPT_PACKET *ret_packet;
if (res) {
MYSQL_ROW row = mysql_fetch_row(res);
unsigned long *lengths = mysql_fetch_lengths(res);
if (row) {
if (row[0]) {
ret_packet = packet_new(DEST_SERVER, NULL, RESP_OK, lengths[0]);
memcpy(ret_packet->payload, row[0], lengths[0]);
} else {
ret_packet = packet_new(DEST_SERVER, NULL, RESP_PUBLIC_KEY_NONE,
0);
}
} else {
ret_packet = packet_new(DEST_SERVER, NULL, RESP_ERR_NOSUCHUSER, 0);
}
} else {
ret_packet = packet_new(DEST_SERVER, NULL, RESP_ERR_TEMPORARY, 0);
}
ret_packet->queue_id = packet->queue_id;
int retval = packet_send_client(client, ret_packet);
packet_free(ret_packet);
return retval;
}
int main( int argc, char **argv ) {
if (argc != 2)
usage();
int fd = open(argv[1], O_RDONLY);
if (!fd) {
printf("Cannot open %s\n", argv[1]);
return 1;
}
t_packet *p = read_next_packet(fd);
while(p) {
int i;
for (i = 0; i < p->nb_info; i++) {
char addr[18];
ba2str(&p->infos[0]->bdaddr, addr);
printf("%s\n", addr);
}
packet_free(p);
p = read_next_packet(fd);
}
close(fd);
return 0;
}
/**
* @brief 按照消息结构发送数据
*
* @param cli 客户端数据结构
*
* @return
*/
int client_flush(ChatClient *cli)
{
int ret = 0;
ChatPacket *pkt = cli->pktsnd;
if(pkt == NULL)
{
return 0;
}
if((ret=socket_send_head(cli->sktfd, HEAD_START, NULL))<0) goto FAILED;
if((ret=socket_send_head(cli->sktfd, HEAD_FROM, pkt->from))<0) goto FAILED;
if((ret=socket_send_head(cli->sktfd, HEAD_TO, pkt->to))<0) goto FAILED;
if((ret=socket_send_head(cli->sktfd, HEAD_TIME, pkt->time))<0) goto FAILED;
if((ret=socket_send_head(cli->sktfd, HEAD_FDBK, pkt->fdbk))<0) goto FAILED;
if((ret=socket_send_head(cli->sktfd, HEAD_TYPE, pkt->type))<0) goto FAILED;
int i;
for(i=0;i<(cli->pktsnd->nmsg);i++)
if((ret=socket_send_head(cli->sktfd, HEAD_MSG, pkt->msg[i]))<0)
goto FAILED;
if ((ret=socket_send_head(cli->sktfd, HEAD_END, NULL))<0)
goto FAILED;
FAILED:
if(ret<0)
{
printf("Failed to flush client\n");
}
packet_free(pkt);
cli->pktsnd = NULL;
return ret;
}
void
pkthdr_free(pkthdr_t *ph)
{
packet_free(ph->data);
free(ph);
}
static void pkt_received(packet_t *packet, uint16_t src)
{
// unused
(void) src;
if (packet->length == 1)
{
log_printf("Packet received from 0x%04x : %u\n", src, *(packet->data));
/* make the lend blink */
if (*(packet->data) % 2)
{
leds_on(LED_0);
}
else
{
leds_off(LED_0);
}
}
else
{
log_printf("Unknown Packet received from 0x%04x\n", src);
}
/* free the packet */
packet_free(packet);
}
int32_t window_free(
window_t* window)
{
debug_print("window_free()\n");
if (window) {
pthread_mutex_lock(&window->lock);
if (window->buffer) {
queue_node_t* node;
packet_t* packet;
while (queue_size(window->buffer)) {
node = queue_dequeue(window->buffer, false);
packet = (packet_t*) node->data;
packet_free(packet);
free(node);
}
queue_free(window->buffer, true);
window->buffer = 0;
}
sem_post(&window->available);
sem_destroy(&window->available);
pthread_mutex_unlock(&window->lock);
pthread_mutex_destroy(&window->lock);
free(window);
}
}
probe_t * probe_create()
{
probe_t * probe;
// We calloc probe to set *_time and caller members to 0
if (!(probe = calloc(1, sizeof(probe_t)))) goto ERR_PROBE;
if (!(probe->packet = packet_create())) {
fprintf(stderr, "Cannot create packet\n");
goto ERR_PACKET;
}
if (!(probe->layers = dynarray_create())) goto ERR_LAYERS;
// if (!(probe->bitfield = bitfield_create(0))) goto ERR_BITFIELD;
probe_set_left_to_send(probe, 1);
return probe;
/*
ERR_BITFIELD:
probe_layers_free(probe);
*/
ERR_LAYERS:
packet_free(probe->packet);
ERR_PACKET:
free(probe);
ERR_PROBE:
return NULL;
}
void draw_init_env()
{
framebuffer_t frame;
zbuffer_t z;
packet_t *packet = packet_init(16,PACKET_NORMAL);
qword_t *q = packet->data;
frame.width = 640;
frame.height = 448;
frame.psm = GS_PSM_32;
frame.mask = 0;
frame.address = graph_vram_allocate(frame.width,frame.height,frame.psm,GRAPH_ALIGN_PAGE);
z.enable = 0;
z.method = ZTEST_METHOD_GREATER;
z.address = 0;
z.mask = 1;
z.zsm = 0;
graph_initialize(frame.address,640,448,GS_PSM_32,0,0);
q = draw_setup_environment(q,0,&frame,&z);
q = draw_finish(q);
dma_channel_send_normal(DMA_CHANNEL_GIF,packet->data, q - packet->data, 0,0);
dma_wait_fast();
packet_free(packet);
}
probe_t * probe_dup(const probe_t * probe)
{
probe_t * ret;
packet_t * packet;
if (!(packet = packet_dup(probe->packet))) goto ERR_PACKET_DUP;
if (!(ret = probe_wrap_packet(packet))) goto ERR_PROBE_WRAP_PACKET;
// if (!(ret->bitfield = bitfield_dup(probe->bitfield))) goto ERR_BITFIELD_DUP;
ret->sending_time = probe->sending_time;
ret->queueing_time = probe->queueing_time;
ret->recv_time = probe->recv_time;
ret->caller = probe->caller;
#ifdef USE_SCHEDULING
ret->delay = probe->delay ? field_dup(probe->delay): NULL;
#endif
return ret;
/*
ERR_BITFIELD_DUP:
probe_free(ret);
packet = NULL;
*/
ERR_PROBE_WRAP_PACKET:
if (packet) packet_free(packet);
ERR_PACKET_DUP:
return NULL;
}
void window_slide(
window_t* window)
{
debug_print("window_slide()\n");
queue_node_t* node;
packet_t* packet;
window_lock(window);
while (window->buffer->size > 0) {
node = queue_head(window->buffer);
packet = node->data;
if (!packet->needs_ack && packet->transmission_time) {
node = queue_dequeue(window->buffer, false);
packet_free(node->data);
free(node);
if (window->size) {
window->size--;
}
} else {
break;
}
}
window_unlock(window);
window_out_commit(window);
success_print("window_slide() succeed\n");
}
/* Reset a flow_t object to reuse */
static void
flow_reset(flow_t *f)
{
packet_t *cp;
while(f->pkt_dst_f != NULL){
cp = f->pkt_dst_f;
f->pkt_dst_f = f->pkt_dst_f->next;
packet_free(cp);
}
while(f->pkt_src_f != NULL){
cp = f->pkt_src_f;
f->pkt_src_f = f->pkt_src_f->next;
packet_free(cp);
}
if(f->order != NULL){
order_free(f->order);
f->order = order_new();
}
/*
* Note: maintain the f->socket.
*/
f->pkt_src_e = NULL;
f->pkt_dst_e = NULL;
f->rtt = 0;
f->syn_sec = 0;
f->syn_usec = 0;
f->ack2_sec = 0;
f->ack2_usec = 0;
f->fb_sec = 0;
f->fb_usec = 0;
f->lb_sec = 0;
f->lb_usec = 0;
f->last_action_sec = 0;
f->last_action_usec = 0;
f->payload_src = 0;
f->payload_dst = 0;
f->pkt_src_n = 0;
f->pkt_dst_n = 0;
f->pkts_src = 0;
f->pkts_dst = 0;
f->http = 0;
f->close = 0;
}
static void sniff_send_to_serial(handler_arg_t arg)
{
packet_t *pkt = arg;
if (!iotlab_serial_send_frame(RADIO_NOTIF_SNIFFED, pkt))
{
log_error("Failed to send captured frame");
packet_free(pkt);
}
}
int packet_new(struct packet_pool* pool, struct pcap_pkthdr* header, const unsigned char* data)
{
struct packet* ret = NULL;
pthread_mutex_lock(&pool->free_lock);
struct list_element_t* e = list_pop_front(pool->free_list);
pthread_mutex_unlock(&pool->free_lock);
pool->packets_seen++;
if (!(pool->packets_seen % 1000000)) {
msg(MSG_STATS, "Seen: %llu, Used: %llu, Free: %llu", pool->packets_seen, pool->used_list->size, pool->free_list->size);
}
if (!e) {
pool->packets_lost++;
return -1;
}
ret = e->data;
memcpy(&ret->header, header, sizeof(*header));
memcpy(ret->data, data, header->caplen);
uint16_t et = ntohs(ETHERNET(data)->ether_type);
if (!(et == ETHERTYPE_IP || et == ETHERTYPE_IPV6 || et == ETHERTYPE_VLAN)) {
ret->is_ip = ret->is_ip6 = 0;
ret->ip = NULL;
ret->ip6 = NULL;
}
uint8_t offset = et == ETHERTYPE_VLAN?4:0; // ethernetheader is shifted by four bytes if vlan is available
// we don't know whether we received ip or ipv6. So lets try:
if ((IP(data + offset))->ip_v == 4 || et == ETHERTYPE_IP) {
ret->is_ip6 = 0;
ret->is_ip = 1;
ret->ip = IP(ret->data);
ret->ip6 = NULL;
//msg(MSG_ERROR, "Found IPv4 packet");
} else if ((IP(data + offset))->ip_v == 6 || et == ETHERTYPE_IPV6) {
ret->is_ip6 = 1;
ret->is_ip = 0;
ret->ip = NULL;
ret->ip6 = IP6(ret->data);
} else {
//msg(MSG_ERROR, "Well. Something is weird here!: Ethertype: %d, IP vesrsion: %d", et, (IP(data + offset))->ip_v);
}
// only handle packets if its connection is still active
ret->connection = connection_get(ret);
// TODO: we should discard the packet earlier, at best before copying the packet content
if (ret->connection && ret->connection->active) {
pthread_mutex_lock(&pool->used_lock);
list_push_back(pool->used_list, e);
pthread_mutex_unlock(&pool->used_lock);
} else {
packet_free(pool, ret);
}
return 0;
}
packet_t packet_link(packet_t parent, packet_t child)
{
if(!parent) parent = packet_new();
if(!parent) return NULL;
if(parent->chain) packet_free(parent->chain);
parent->chain = child;
if(child && child->chain == parent) child->chain = NULL;
return parent;
}
static void pkt_sent(void *arg, enum tdma_result res)
{
// unused
(void) res;
log_printf("Packet sending result %d\n", res);
/* free packet */
packet_free((packet_t *) arg);
}
packet_t packet_free(packet_t p)
{
if(!p) return NULL;
// DEBUG_PRINTF("packet --- %d",p);
if(p->chain) packet_free(p->chain);
if(p->jsoncp) free(p->jsoncp);
if(p->raw) free(p->raw);
free(p);
return NULL;
}
packet_t packet_new()
{
packet_t p;
if(!(p = malloc(sizeof (struct packet_struct)))) return NULL;
memset(p,0,sizeof (struct packet_struct));
if(!(p->raw = malloc(2))) return packet_free(p);
memset(p->raw,0,2);
// DEBUG_PRINTF("packet +++ %d",p);
return p;
}
packet_t crypt_delineize_1a(crypt_t c, packet_t p)
{
packet_t line;
unsigned char iv[16], hmac[32];
crypt_1a_t cs = (crypt_1a_t)c->cs;
memset(iv,0,16);
memcpy(iv+12,p->body+16+4,4);
hmac_256(cs->keyIn,16,p->body+16+4,p->body_len-(16+4),hmac);
fold3(hmac,hmac);
if(memcmp(hmac,p->body+16,4) != 0) return packet_free(p);
aes_128_ctr(cs->keyIn,p->body_len-(16+4+4),iv,p->body+16+4+4,p->body+16+4+4);
line = packet_parse(p->body+16+4+4, p->body_len-(16+4+4));
packet_free(p);
return line;
}
int net_send_version(struct peer *peer, int start_height)
{
struct packet *packet;
int ret;
packet = packet_version(peer, start_height);
ret = packet_send(peer, packet);
packet_free(packet);
return ret;
}
void probe_free(probe_t * probe)
{
if (probe) {
// bitfield_free(probe->bitfield);
probe_layers_free(probe);
if (probe->packet) {
packet_free(probe->packet);
}
free(probe);
}
}
void socket_send_packet(struct packet_struct *packet)
{
HARD_ASSERT(packet != NULL);
if (csocket.sc == NULL) {
packet_free(packet);
return;
}
packet_struct *packet_meta = packet_new(0, 4, 0);
if (socket_is_secure(csocket.sc)) {
bool checksum_only = !socket_crypto_client_should_encrypt(packet->type);
packet = socket_crypto_encrypt(csocket.sc,
packet,
packet_meta,
checksum_only);
if (packet == NULL) {
/* Logging already done. */
cpl.state = ST_START;
return;
}
} else {
packet_append_uint16(packet_meta, packet->len + 1);
packet_append_uint8(packet_meta, packet->type);
}
command_buffer *buf1 = command_buffer_new(packet_meta->len,
packet_meta->data);
packet_free(packet_meta);
command_buffer *buf2 = command_buffer_new(packet->len,
packet->data);
packet_free(packet);
SDL_LockMutex(output_buffer_mutex);
command_buffer_enqueue(buf1, &output_queue_start, &output_queue_end);
command_buffer_enqueue(buf2, &output_queue_start, &output_queue_end);
SDL_CondSignal(output_buffer_cond);
SDL_UnlockMutex(output_buffer_mutex);
}
void vc_handler_ping(ascnhalf_socket *s, packet *p)
{
packet bp;
uint32 pi;
printf("ping!\n");
pi = packet_readu32(p);
packet_init(VOICECHAT_SMSG_PONG, 4, &bp);
packet_writeu32(&bp, pi);
socket_send(s, &bp);
packet_free(&bp);
}
void ndp_spoof(const u_char *bytes, size_t len)
{
struct ether *ether = NULL;
struct ip6 *ip = NULL;
struct icmp6 *icmp = NULL;
struct icmp6_nd_ns *ns = NULL;
char s1[INET6_ADDRSTRLEN], s2[INET6_ADDRSTRLEN];
struct packet *p = NULL;
u_char *mac = NULL;
ether = (struct ether *)bytes;
ip = (struct ip6 *)(bytes + sizeof(struct ether));
if (ip->nxt == IP6_NXT_ICMP6) {
icmp = (struct icmp6 *)(bytes + sizeof(struct ether) + sizeof(struct ip6));
} else {
/* TODO */
fatal("no ICMPv6 packet!!! Skipping...");
return;
}
if (icmp->type == ICMP6_T_ND_NS) {
mac = get_mac(iface);
if (mac == NULL)
return;
if (memcmp(mac, ether->src, ETH_ADDR_LEN) == 0) {
free(mac);
return;
}
ns = (struct icmp6_nd_ns *)(bytes + sizeof(struct ether) + sizeof(struct ip6) + sizeof(struct icmp6));
bzero(s1, INET6_ADDRSTRLEN);
inet_ntop(AF_INET6, ns->target, s1, INET6_ADDRSTRLEN);
bzero(s2, INET6_ADDRSTRLEN);
inet_ntop(AF_INET6, ip->src, s2, INET6_ADDRSTRLEN);
printf("Spoofing %s with target %s\n", s2, s1);
p = packet_init();
packet_add_ether(p, mac, ether->src, ETH_TYPE_IP6);
packet_add_ip6(p, sizeof(struct icmp6) + sizeof(struct icmp6_nd_na), IP6_NXT_ICMP6, IP6_HLIM, ns->target, ip->src);
packet_add_icmp6_nd_na(p, ICMP6_ND_NA_F_SOLICIT | ICMP6_ND_NA_F_OVERRIDE, ns->target);
packet_send(iface, p);
packet_free(p);
free(mac);
}
}
int main(void)
{
switch_t s;
chan_t c;
packet_t p;
path_t in;
int sock;
crypt_init();
s = switch_new(0);
if(util_loadjson(s) != 0 || (sock = util_server(0,1000)) <= 0)
{
printf("failed to startup %s or %s\n", strerror(errno), crypt_err());
return -1;
}
printf("loaded hashname %s\n",s->id->hexname);
// create/send a ping packet
c = chan_new(s, bucket_get(s->seeds, 0), "link", 0);
p = chan_packet(c);
chan_send(c, p);
util_sendall(s,sock);
in = path_new("ipv4");
while(util_readone(s, sock, in) == 0)
{
switch_loop(s);
while((c = switch_pop(s)))
{
printf("channel active %d %s %s\n",c->state,c->hexid,c->to->hexname);
if(util_cmp(c->type,"connect") == 0) ext_connect(c);
if(util_cmp(c->type,"link") == 0) ext_link(c);
if(util_cmp(c->type,"path") == 0) ext_path(c);
while((p = chan_pop(c)))
{
printf("unhandled channel packet %.*s\n", p->json_len, p->json);
packet_free(p);
}
if(c->state == ENDED) chan_free(c);
}
util_sendall(s,sock);
}
perror("exiting");
return 0;
}
static void proper_stop()
{
// Stop timer
soft_timer_stop(&radio.period_tim);
// Set PHY idle
phy_idle(platform_phy);
// Free polling packet
if (radio.poll.serial_pkt)
{
packet_free(radio.poll.serial_pkt);
radio.poll.serial_pkt = NULL;
}
}
