void process_data (PacketProtoDecoder *enc) { int was_error = 0; do { uint8_t *data = enc->buf + enc->buf_start; int left = enc->buf_used; // check if header was received if (left < sizeof(struct packetproto_header)) { break; } struct packetproto_header header; memcpy(&header, data, sizeof(header)); data += sizeof(struct packetproto_header); left -= sizeof(struct packetproto_header); int data_len = ltoh16(header.len); // check data length if (data_len > enc->output_mtu) { BLog(BLOG_NOTICE, "error: packet too large"); was_error = 1; break; } // check if whole packet was received if (left < data_len) { break; } // update buffer enc->buf_start += sizeof(struct packetproto_header) + data_len; enc->buf_used -= sizeof(struct packetproto_header) + data_len; // submit packet PacketPassInterface_Sender_Send(enc->output, data, data_len); return; } while (0); if (was_error) { // reset buffer enc->buf_start = 0; enc->buf_used = 0; } else { // if we reached the end of the buffer, wrap around to allow more data to be received if (enc->buf_start + enc->buf_used == enc->buf_size) { memmove(enc->buf, enc->buf + enc->buf_start, enc->buf_used); enc->buf_start = 0; } } // receive data StreamRecvInterface_Receiver_Recv(enc->input, enc->buf + (enc->buf_start + enc->buf_used), enc->buf_size - (enc->buf_start + enc->buf_used)); // if we had error, report it if (was_error) { enc->handler_error(enc->user); return; } }
static void send_request (BArpProbe *o) { if (o->send_sending) { BLog(BLOG_ERROR, "cannot send packet while another packet is being sent!"); return; } // build packet struct arp_packet *arp = &o->send_packet; arp->hardware_type = hton16(ARP_HARDWARE_TYPE_ETHERNET); arp->protocol_type = hton16(ETHERTYPE_IPV4); arp->hardware_size = hton8(6); arp->protocol_size = hton8(4); arp->opcode = hton16(ARP_OPCODE_REQUEST); memcpy(arp->sender_mac, o->if_mac, 6); arp->sender_ip = hton32(0); memset(arp->target_mac, 0, sizeof(arp->target_mac)); arp->target_ip = o->addr; // send packet PacketPassInterface_Sender_Send(o->send_if, (uint8_t *)&o->send_packet, sizeof(o->send_packet)); // set sending o->send_sending = 1; }
void auth_finished (BSocksClient *o) { // allocate request buffer bsize_t size = bsize_fromsize(sizeof(struct socks_request_header)); switch (o->dest_addr.type) { case BADDR_TYPE_IPV4: size = bsize_add(size, bsize_fromsize(sizeof(struct socks_addr_ipv4))); break; case BADDR_TYPE_IPV6: size = bsize_add(size, bsize_fromsize(sizeof(struct socks_addr_ipv6))); break; } if (!reserve_buffer(o, size)) { report_error(o, BSOCKSCLIENT_EVENT_ERROR); return; } // write request struct socks_request_header header; header.ver = hton8(SOCKS_VERSION); header.cmd = hton8(SOCKS_CMD_CONNECT); header.rsv = hton8(0); switch (o->dest_addr.type) { case BADDR_TYPE_IPV4: { header.atyp = hton8(SOCKS_ATYP_IPV4); struct socks_addr_ipv4 addr; addr.addr = o->dest_addr.ipv4.ip; addr.port = o->dest_addr.ipv4.port; memcpy(o->buffer + sizeof(header), &addr, sizeof(addr)); } break; case BADDR_TYPE_IPV6: { header.atyp = hton8(SOCKS_ATYP_IPV6); struct socks_addr_ipv6 addr; memcpy(addr.addr, o->dest_addr.ipv6.ip, sizeof(o->dest_addr.ipv6.ip)); addr.port = o->dest_addr.ipv6.port; memcpy(o->buffer + sizeof(header), &addr, sizeof(addr)); } break; default: ASSERT(0); } memcpy(o->buffer, &header, sizeof(header)); // send request PacketPassInterface_Sender_Send(o->control.send_if, (uint8_t *)o->buffer, size.value); // set state o->state = STATE_SENDING_REQUEST; }
static void schedule (PacketPassFifoQueue *o) { ASSERT(!o->freeing) ASSERT(!o->sending_flow) ASSERT(!LinkedList1_IsEmpty(&o->waiting_flows_list)) ASSERT(!BPending_IsSet(&o->schedule_job)) // get first waiting flow PacketPassFifoQueueFlow *flow = UPPER_OBJECT(LinkedList1_GetFirst(&o->waiting_flows_list), PacketPassFifoQueueFlow, waiting_flows_list_node); ASSERT(flow->queue == o) ASSERT(flow->is_waiting) // remove it from queue LinkedList1_Remove(&o->waiting_flows_list, &flow->waiting_flows_list_node); flow->is_waiting = 0; // send PacketPassInterface_Sender_Send(o->output, flow->waiting_data, flow->waiting_len); o->sending_flow = flow; }
void output_handler_done (PacketBuffer *buf) { DebugObject_Access(&buf->d_obj); // remember if buffer is full int was_full = (buf->buf.input_avail < buf->input_mtu); // remove packet from buffer ChunkBuffer2_ConsumePacket(&buf->buf); // if buffer was full and there is space, schedule receive if (was_full && buf->buf.input_avail >= buf->input_mtu) { PacketRecvInterface_Receiver_Recv(buf->input, buf->buf.input_dest); } // if there is more data, schedule send if (buf->buf.output_avail >= 0) { PacketPassInterface_Sender_Send(buf->output, buf->buf.output_dest, buf->buf.output_avail); } }
void input_handler_done (PacketBuffer *buf, int in_len) { ASSERT(in_len >= 0) ASSERT(in_len <= buf->input_mtu) DebugObject_Access(&buf->d_obj); // remember if buffer is empty int was_empty = (buf->buf.output_avail < 0); // submit packet to buffer ChunkBuffer2_SubmitPacket(&buf->buf, in_len); // if there is space, schedule receive if (buf->buf.input_avail >= buf->input_mtu) { PacketRecvInterface_Receiver_Recv(buf->input, buf->buf.input_dest); } // if buffer was empty, schedule send if (was_empty) { PacketPassInterface_Sender_Send(buf->output, buf->buf.output_dest, buf->buf.output_avail); } }
void recv_handler_done (BSocksClient *o, int data_len) { ASSERT(data_len >= 0) ASSERT(data_len <= o->control.recv_total - o->control.recv_len) DebugObject_Access(&o->d_obj); o->control.recv_len += data_len; if (o->control.recv_len < o->control.recv_total) { do_receive(o); return; } switch (o->state) { case STATE_SENT_HELLO: { BLog(BLOG_DEBUG, "received hello"); struct socks_server_hello imsg; memcpy(&imsg, o->buffer, sizeof(imsg)); if (ntoh8(imsg.ver) != SOCKS_VERSION) { BLog(BLOG_NOTICE, "wrong version"); goto fail; } size_t auth_index; for (auth_index = 0; auth_index < o->num_auth_info; auth_index++) { if (o->auth_info[auth_index].auth_type == ntoh8(imsg.method)) { break; } } if (auth_index == o->num_auth_info) { BLog(BLOG_NOTICE, "server didn't accept any authentication method"); goto fail; } const struct BSocksClient_auth_info *ai = &o->auth_info[auth_index]; switch (ai->auth_type) { case SOCKS_METHOD_NO_AUTHENTICATION_REQUIRED: { BLog(BLOG_DEBUG, "no authentication"); auth_finished(o); } break; case SOCKS_METHOD_USERNAME_PASSWORD: { BLog(BLOG_DEBUG, "password authentication"); if (ai->password.username_len == 0 || ai->password.username_len > 255 || ai->password.password_len == 0 || ai->password.password_len > 255 ) { BLog(BLOG_NOTICE, "invalid username/password length"); goto fail; } // allocate password packet bsize_t size = bsize_fromsize(1 + 1 + ai->password.username_len + 1 + ai->password.password_len); if (!reserve_buffer(o, size)) { goto fail; } // write password packet char *ptr = o->buffer; *ptr++ = 1; *ptr++ = ai->password.username_len; memcpy(ptr, ai->password.username, ai->password.username_len); ptr += ai->password.username_len; *ptr++ = ai->password.password_len; memcpy(ptr, ai->password.password, ai->password.password_len); ptr += ai->password.password_len; // start sending PacketPassInterface_Sender_Send(o->control.send_if, (uint8_t *)o->buffer, size.value); // set state o->state = STATE_SENDING_PASSWORD; } break; default: ASSERT(0); } } break; case STATE_SENT_REQUEST: { BLog(BLOG_DEBUG, "received reply header"); struct socks_reply_header imsg; memcpy(&imsg, o->buffer, sizeof(imsg)); if (ntoh8(imsg.ver) != SOCKS_VERSION) { BLog(BLOG_NOTICE, "wrong version"); goto fail; } if (ntoh8(imsg.rep) != SOCKS_REP_SUCCEEDED) { BLog(BLOG_NOTICE, "reply not successful"); goto fail; } int addr_len; switch (ntoh8(imsg.atyp)) { case SOCKS_ATYP_IPV4: addr_len = sizeof(struct socks_addr_ipv4); break; case SOCKS_ATYP_IPV6: addr_len = sizeof(struct socks_addr_ipv6); break; default: BLog(BLOG_NOTICE, "reply has unknown address type"); goto fail; } // receive the rest of the reply start_receive(o, (uint8_t *)o->buffer + sizeof(imsg), addr_len); // set state o->state = STATE_RECEIVED_REPLY_HEADER; } break; case STATE_SENT_PASSWORD: { BLog(BLOG_DEBUG, "received password reply"); if (o->buffer[0] != 1) { BLog(BLOG_NOTICE, "password reply has unknown version"); goto fail; } if (o->buffer[1] != 0) { BLog(BLOG_NOTICE, "password reply is negative"); goto fail; } auth_finished(o); } break; case STATE_RECEIVED_REPLY_HEADER: { BLog(BLOG_DEBUG, "received reply rest"); // free buffer BFree(o->buffer); o->buffer = NULL; // free control I/O free_control_io(o); // init up I/O init_up_io(o); // set state o->state = STATE_UP; // call handler o->handler(o->user, BSOCKSCLIENT_EVENT_UP); return; } break; default: ASSERT(0); } return; fail: report_error(o, BSOCKSCLIENT_EVENT_ERROR); }
void connector_handler (BSocksClient* o, int is_error) { DebugObject_Access(&o->d_obj); ASSERT(o->state == STATE_CONNECTING) // check connection result if (is_error) { // PSIPHON // BLog(BLOG_ERROR, "connection failed"); BLog(BLOG_WARNING, "connection failed"); goto fail0; } // init connection if (!BConnection_Init(&o->con, BConnection_source_connector(&o->connector), o->reactor, o, (BConnection_handler)connection_handler)) { BLog(BLOG_ERROR, "BConnection_Init failed"); goto fail0; } BLog(BLOG_DEBUG, "connected"); // init control I/O init_control_io(o); // check number of methods if (o->num_auth_info == 0 || o->num_auth_info > 255) { BLog(BLOG_ERROR, "invalid number of authentication methods"); goto fail1; } // allocate buffer for sending hello bsize_t size = bsize_add( bsize_fromsize(sizeof(struct socks_client_hello_header)), bsize_mul( bsize_fromsize(o->num_auth_info), bsize_fromsize(sizeof(struct socks_client_hello_method)) ) ); if (!reserve_buffer(o, size)) { goto fail1; } // write hello header struct socks_client_hello_header header; header.ver = hton8(SOCKS_VERSION); header.nmethods = hton8(o->num_auth_info); memcpy(o->buffer, &header, sizeof(header)); // write hello methods for (size_t i = 0; i < o->num_auth_info; i++) { struct socks_client_hello_method method; method.method = hton8(o->auth_info[i].auth_type); memcpy(o->buffer + sizeof(header) + i * sizeof(method), &method, sizeof(method)); } // send PacketPassInterface_Sender_Send(o->control.send_if, (uint8_t *)o->buffer, size.value); // set state o->state = STATE_SENDING_HELLO; return; fail1: free_control_io(o); BConnection_Free(&o->con); fail0: report_error(o, BSOCKSCLIENT_EVENT_ERROR); return; }
static void input_handler_done (SinglePacketBuffer *o, int in_len) { DebugObject_Access(&o->d_obj); PacketPassInterface_Sender_Send(o->output, o->buf, in_len); }