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);
}
