Example #1
0
void handle_servo_ctrl()
{
	if (state != StateConnected)
	{
		send_error(ErrorDisconnect, message.sequence);
		return;
	}

	if (message.data_size != 2)
	{
		send_error(ErrorParser, message.sequence);
		return;
	}

	uint8_t id = message.data[0];
	if (id == DOCK_SERVO_ZAXIS_ID)
	{
		dock_set_docktype(ManualDocking);
	}

	servo_set_position(id, message.data[1]);
	send_ack(message.sequence);
}
Example #2
0
static bool process_receive_response(FILE *file, int sd, char *data,
        ssize_t bytes_received, struct sockaddr_in *client,
        struct sockaddr_in *current_client, int blksize, int *block_nr)
{
    uint16_t opcode;
    uint16_t block;

    if (client->sin_port != current_client->sin_port)
    {
        send_error(current_client, TFTP_ERR_UNKNOWN_ID,
                (char *)"Invalid TID.", sd);
        return false;
    }
    opcode = (uint16_t)(((unsigned char)(*data++)) << 8);
    opcode |= (uint16_t)((unsigned char)(*data++));
    if (opcode == TFTP_DATA)
    {
        block = (uint16_t)(((unsigned char)(*data++)) << 8);
        block |= ((unsigned char)(*data++));
        if (*block_nr != block)
            return false;
        (*block_nr)++;
        fwrite(data, 1, (size_t)(bytes_received - 4), file);
        send_ack(sd, current_client, block);
        if (bytes_received < blksize + 4)
            return true;
    }
    else if (opcode == TFTP_ERR)
    {
        printf("An error occured. Transfer aborted.\n");
        return true;
    }
    else
        send_error(current_client, TFTP_ERR_UNDEFINED,
                (char *)"Expected data packet.", sd);
    return false;
}
Example #3
0
File: core.c Project: P4N74/radare2
static int _server_handle_vCont(libgdbr_t *g, gdbr_server_cmd_cb cmd_cb, void *core_ptr) {
	char *action = NULL;
	if (send_ack (g) < 0) {
		return -1;
	}
	g->data[g->data_len] = '\0';
	if (g->data[5] == '?') {
		// Query about everything we support
		return send_msg (g, "vCont;c;s");
	}
	if (!(action = strtok (g->data, ";"))) {
		return send_msg (g, "E01");
	}
	while ((action = strtok (NULL, ";"))) {
		eprintf ("action: %s\n", action);
		switch (action[0]) {
		case 's':
			// TODO handle thread selections
			if (cmd_cb (g, core_ptr, "ds", NULL, 0) < 0) {
				send_msg (g, "E01");
				return -1;
			}
			return send_msg (g, "OK");
		case 'c':
			// TODO handle thread selections
			if (cmd_cb (g, core_ptr, "dc", NULL, 0) < 0) {
				send_msg (g, "E01");
				return -1;
			}
			return send_msg (g, "OK");
		default:
			// TODO support others
			return send_msg (g, "E01");
		}
	}
	return -1;
}
Example #4
0
/*
 * WRQ - receive a file from the client
 */
void
tftp_wrq(int peer, char *recvbuffer, ssize_t size)
{
	char *cp;
	int has_options = 0, ecode;
	char *filename, *mode;
	char fnbuf[PATH_MAX];

	cp = parse_header(peer, recvbuffer, size, &filename, &mode);
	size -= (cp - recvbuffer) + 1;

	strcpy(fnbuf, filename);
	reduce_path(fnbuf);
	filename = fnbuf;

	if (size > 0) {
		if (options_rfc_enabled)
			has_options = !parse_options(peer, cp, size);
		else
			tftp_log(LOG_INFO, "Options found but not enabled");
	}

	ecode = validate_access(peer, &filename, WRQ);
	if (ecode == 0) {
		if (has_options)
			send_oack(peer);
		else
			send_ack(peer, 0);
	}
	if (logging) {
		tftp_log(LOG_INFO, "%s: write request for %s: %s", peername,
			    filename, errtomsg(ecode));
	}

	tftp_recvfile(peer, mode);
	exit(0);
}
Example #5
0
void
TcpSink::receivePacket(Packet& pkt) {
    TcpPacket *p = (TcpPacket*)(&pkt);
    TcpPacket::seq_t seqno = p->seqno();
    int size = p->size(); // TODO: the following code assumes all packets are the same size
    pkt.flow().logTraffic(pkt,*this,TrafficLogger::PKT_RCVDESTROY);
    p->free();

    if (seqno == _cumulative_ack+1) { // it's the next expected seq no
        _cumulative_ack = seqno + size - 1;
        /* are there any additional received packets we can now ack? */
        while (!_received.empty() && (_received.front() == _cumulative_ack+1) ) {
            _received.pop_front();
            _cumulative_ack+= size;
        }
    }
    else if (seqno < _cumulative_ack+1) {} //must have been a bad retransmit
    else { // it's not the next expected sequence number
        if (_received.empty()) {
            _received.push_front(seqno);
        }
        else if (seqno > _received.back()) { // likely case
            _received.push_back(seqno);
        }
        else { // uncommon case - it fills a hole
            list<uint32_t>::iterator i;
            for (i = _received.begin(); i != _received.end(); i++) {
                if (seqno == *i) break; // it's a bad retransmit
                if (seqno < (*i)) {
                    _received.insert(i, seqno);
                    break;
                }
            }
        }
    }
    send_ack();
}
Example #6
0
File: core.c Project: Xxmmy/radare2
int gdbr_detach_pid(libgdbr_t *g, int pid) {
	char *cmd;
	int ret;
	size_t buffer_size;

	if (!g || !g->sock || !g->stub_features.multiprocess) {
		return -1;
	}
	reg_cache.valid = false;

	buffer_size = strlen (CMD_DETACH_MP) + (sizeof (pid) * 2) + 1;
	cmd = calloc (buffer_size, sizeof (char));
	if (!cmd) {
		return -1;
	}

	if ((snprintf (cmd, buffer_size, "%s%x", CMD_DETACH_MP, g->pid)) < 0) {
		free(cmd);
		return -1;
	}

	ret = send_msg (g, cmd);
	free(cmd);
	if (ret < 0) {
		return ret;
	}

	read_packet (g);
	if ((ret = send_ack (g)) < 0) {
		return ret;
	}

	if (strncmp (g->data, "OK", 2)) {
		return -1;
	}
	return 0;
}
Example #7
0
/**
 * [Client-only]
 * TCP handshake with server. This includes the SYN, SYN-ACK, and ACK segments.
 *
 * returns: A connection object if able to connect, NULL otherwise. This
 *          object must be freed.
 */
conn_t *tcp_handshake(void) { ASSERT_CLIENT_ONLY;
  char buf[MAX_PACKET_SIZE];

  /* Send a SYN segment to the server. */
  if (send_syn(config->sconn))
    exit(EXIT_FAILURE);

  /* Wait to receive SYN-ACK. */
  int r = recv_filter(config->socket, buf, MAX_PACKET_SIZE, 0, NULL);
  if (r <= 0)
    return NULL;

  tcphdr_t *synack = (tcphdr_t *) (buf + IP_HDR_SIZE);

  /* Set window size for the other host. */
  ctcp_cfg->send_window = ntohs(synack->window);

  /* If an ACK is received instead of a SYN-ACK, continue previous
     connection. Get sequence numbers from previous connection. */
  if ((synack->th_flags & TH_SYN) == 0) {
    config->sconn->init_seqno = ntohl(synack->th_ack) - 1;
    config->sconn->their_init_seqno = ntohl(synack->th_seq) - 1;

    config->sconn->next_seqno = config->sconn->init_seqno + 1;
    config->sconn->ackno = ntohl(synack->th_seq);
  }

  /* Otherwise, set new acknowledgement number and send ACK response */
  else {
    config->sconn->next_seqno++;
    config->sconn->their_init_seqno = ntohl(synack->th_seq);
    config->sconn->ackno = ntohl(synack->th_seq) + 1;
    send_ack(config->sconn);
  }

  return config->sconn;
}
Example #8
0
bool gdbr_kill_pid(libgdbr_t *g, int pid) {
	char *cmd;
	int ret;
	size_t buffer_size;

	if (!g || !g->sock || !g->stub_features.multiprocess) {
		return false;
	}
	reg_cache.valid = false;
	g->stop_reason.is_valid = false;

	buffer_size = strlen (CMD_KILL_MP) + (sizeof (pid) * 2) + 1;
	cmd = calloc (buffer_size, sizeof (char));
	if (!cmd) {
		return false;
	}

	if ((snprintf (cmd, buffer_size, "%s%x", CMD_KILL_MP, g->pid)) < 0) {
		free(cmd);
		return false;
	}
	ret = send_msg (g, cmd);
	free(cmd);
	if (ret < 0) {
		return false;
	}

	read_packet (g, false);
	if ((ret = send_ack (g)) < 0) {
		return false;
	}
	if (strncmp (g->data, "OK", 2)) {
		return false;
	}
	return true;
}
Example #9
0
/* ***************************************************
 * Function: handle_network_data
 * ***************************************************
 *  A packet has arrived.  If its an ACK packet, call a helper 
 * function that deals with acks.  If the packet has data or it's
 * a FIN packet, then add the packet to the receiver buffer only
 * if it fits in the receiving window.  If it doesn't fit,
 * drop the packet.  If it fits, send an acknowledgement.
 */
static void handle_network_data(mysocket_t sd, context_t *ctx)
{
	   char *pkt = (char *)calloc(1, (TH_MAX_OFFSET*sizeof(uint32_t)) + STCP_MSS);
	   size_t total_length = network_recv(sd, pkt);
	   struct tcphdr* hdr = (struct tcphdr *)pkt;
	   ctx->recv_win = MIN(hdr->th_win, CWIN);
	   if (hdr->th_flags & TH_ACK) {
			handle_ack(hdr, sd, ctx);
	   }
	   size_t data_length = total_length - TCP_DATA_START(hdr);
	   if (data_length>0 || (hdr->th_flags & TH_FIN)) {
	   		our_dprintf("Received %d bytes of data with seq: %d, last ack sent: %d\n", 
	   					data_length, hdr->th_seq, ctx->last_ack_sent);
	   		/* Does the packet fit in the receiving window?*/
	   		if ((hdr->th_seq + data_length)<=(ctx->last_ack_sent + RWIN)) {
	   			add_to_recv_buffer(ctx, pkt, total_length);
	   			if (hdr->th_seq == ctx->last_ack_sent){
	   				/* The packet arrived in order. We can remove all in-order
	   				 * packets from the receive buffer now */
	   				our_dprintf("Packet In Order. Handling buffer\n");
	   				handle_recv_buffer(sd, ctx);
	   			}
	   			else {
	   				if (hdr->th_seq > ctx->last_ack_sent) {
	   					our_dprintf("This packet is out of order. Adding to recv buffer\n");
	   				}
	   			}
	   			send_ack(sd, ctx);
	   		}
	   		else {
	   			our_dprintf("Packet outside receiver buffer. Dropping!!!\n");	
	   	
	   		}		
	   }
	   free(pkt);			
}
Example #10
0
File: core.c Project: P4N74/radare2
// Set thread for all operations other than "step" and "continue"
static int _server_handle_Hg(libgdbr_t *g, gdbr_server_cmd_cb cmd_cb, void *core_ptr) {
	// We don't yet support multiprocess. Client is not supposed to send Hgp. If we receive it anyway,
	// send error
	char cmd[32];
	int tid;
	if (send_ack (g) < 0) {
		return -1;
	}
	if (g->data_len <= 2 || isalpha (g->data[2])) {
		return send_msg (g, "E01");
	}
	// Hg-1 = "all threads", Hg0 = "pick any thread"
	if (g->data[2] == '0' || !strncmp (g->data + 2, "-1", 2)) {
		return send_msg (g, "OK");
	}
	sscanf (g->data + 2, "%x", &tid);
	snprintf (cmd, sizeof (cmd) - 1, "dpt=%d", tid);
	// Set thread for future operations
	if (cmd_cb (g, core_ptr, cmd, NULL, 0) < 0) {
		send_msg (g, "E01");
		return -1;
	}
	return send_msg (g, "OK");
}
Example #11
0
void
rel_output (rel_t *r)
{ 
  size_t left = conn_bufspace(r->c);
  int index = (r->LSR + 1) % r->window_size;
  packet_t * cur = &r->recvWindow[index];
  int data_len = ntohs(cur->len) - 12;

  while ((uint16_t)left >= data_len) { 
    conn_output (r->c, cur->data, data_len);
    r->recvState[index] = 2;
    r->LSR ++ ;
    r->LAS = r->LSR + r->RWS;
    index = (r->LSR + 1) % r->window_size;
    if (r->recvState[index] == 1) {
      cur = &r->recvWindow[index];
    } else {
      break;
    }
    data_len = ntohs(cur->len) - 12;
    left = conn_bufspace(r->c);
  }
  send_ack(r);
}
Example #12
0
/*-----------------------------------------------------------------------
 * if ack == I2C_ACK, ACK the byte so can continue reading, else
 * send I2C_NOACK to end the read.
 */
static uchar read_byte(int ack)
{
	int  data;
	int  j;

	/*
	 * Read 8 bits, MSB first.
	 */
	I2C_TRISTATE;
	I2C_SDA(1);
	data = 0;
	for(j = 0; j < 8; j++) {
		I2C_SCL(0);
		I2C_DELAY;
		I2C_SCL(1);
		I2C_DELAY;
		data <<= 1;
		data |= I2C_READ;
		I2C_DELAY;
	}
	send_ack(ack);

	return(data);
}
Example #13
0
static msg_t PlannerThread(void* arg){
  chRegSetThreadName("Planner");
  (void)arg;

  while(GlobalFlags.messaging_ready == 0)
    chThdSleepMilliseconds(50);

  struct EventListener el_mission_request_list;
  struct EventListener el_mission_count;
  struct EventListener el_mission_clear_all;
  struct EventListener el_mission_item;

  chEvtRegisterMask(&event_mavlink_in_mission_request_list, &el_mission_request_list, EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST);
  chEvtRegisterMask(&event_mavlink_in_mission_count,        &el_mission_count,        EVMSK_MAVLINK_IN_MISSION_COUNT);
  chEvtRegisterMask(&event_mavlink_in_mission_clear_all,    &el_mission_clear_all,    EVMSK_MAVLINK_IN_MISSION_CLEAR_ALL);
  chEvtRegisterMask(&event_mavlink_in_mission_item,         &el_mission_item,         EVMSK_MAVLINK_IN_MISSION_ITEM);

  eventmask_t evt = 0;
  while (!chThdShouldTerminate()) {
    evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST |
                              EVMSK_MAVLINK_IN_MISSION_COUNT |
                              EVMSK_MAVLINK_IN_MISSION_CLEAR_ALL |
                              EVMSK_MAVLINK_IN_MISSION_ITEM,
                              MS2ST(100));
    switch (evt){

    /* ground want to know how many items we have */
    case EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST:
      chEvtUnregister(&event_mavlink_in_mission_request_list, &el_mission_request_list);
      mav2gcs();
      chEvtRegisterMask(&event_mavlink_in_mission_request_list, &el_mission_request_list, EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST);
      break;

    /* ground says how many items it wants to send here */
    case EVMSK_MAVLINK_IN_MISSION_COUNT:
      /* this event now will be handled inside write loop */
      chEvtUnregister(&event_mavlink_in_mission_item, &el_mission_item);
      gcs2mav(mavlink_in_mission_count_struct.count);
      /* register event back to main cycle */
      chEvtRegisterMask(&event_mavlink_in_mission_item, &el_mission_item, EVMSK_MAVLINK_IN_MISSION_ITEM);
      break;

    /* ground wants erase all wps */
    case EVMSK_MAVLINK_IN_MISSION_CLEAR_ALL:
      mission_clear_all();
      break;

    case EVMSK_MAVLINK_IN_MISSION_ITEM:
      /* If a waypoint planner component receives WAYPOINT messages outside
       * of transactions it answers with a WAYPOINT_ACK message. */
      send_ack(MAV_MISSION_DENIED);
      break;

    default:
      //chDbgPanic("unimplemented");
      break;
    }
  }

  chEvtUnregister(&event_mavlink_in_mission_request_list, &el_mission_request_list);
  chEvtUnregister(&event_mavlink_in_mission_count,        &el_mission_count);
  chEvtUnregister(&event_mavlink_in_mission_clear_all,    &el_mission_clear_all);
  chEvtUnregister(&event_mavlink_in_mission_item,         &el_mission_item);

  chThdExit(0);
  return 0;
}
Example #14
0
void EMServer::handle_receive(const boost::system::error_code &ec, size_t bytes_received)
{
	if (ec || bytes_received == 0) {
		warn() << "server error in udp\n";
	} else {
		std::string message(input_buffer.begin(), input_buffer.begin() + bytes_received);
		EM::Messages::Type type = EM::Messages::get_type(message);
		log() << "message from: " << get_address_from_endpoint(udp_endpoint) << "\n";
		switch (type) {
			case EM::Messages::Type::Client: {
				uint cid = 0;
				if (EM::Messages::read_client(message, cid)) {
					log() << "READ " << message << " from "
						<< get_address_from_endpoint(udp_endpoint) << ".\n";;
					clients[cid]->set_udp_endpoint(udp_endpoint);
					info() << "Added client: " << clients[cid]->get_name() << "\n";
				} else {
					info() << "READ invalid CLIENT datagram from "
						<< get_address_from_endpoint(udp_endpoint) << ".\n";
				}
				break;
			}
			case EM::Messages::Type::Upload: {
				uint nr = 0;
				uint cid =
					get_cid_from_address(
						get_address_from_endpoint(udp_endpoint));
				if (EM::Messages::read_upload(message, nr) && cid != 0) {
					size_t index =
						message.find('\n');

					if (index == message.size()) {
						info() << "READ empty UPLOAD datagram from "
							<< clients[cid]->get_name() << "\n";
						break;
					}

					log() << "READ UPLOAD " << nr << " from "
						<< clients[cid]->get_name()
						<< " (" << bytes_received - index - 1 << ")\n";

					ClientQueue &queue = clients[cid]->get_queue();
					if (queue.insert(message.substr(index + 1), nr))
						send_ack(udp_endpoint,
							queue.get_expected_nr(),
							queue.get_available_space_size());
					else
						log() << "READ invalid UPLOAD datagram from "
							<< clients[cid]->get_name() << "\n";
				} else {
					info() << "READ invalid UPLOAD datagram from "
						<< clients[cid]->get_name() << ".\n";
				}
				break;
			}
			case EM::Messages::Type::Retransmit: {
				uint nr;
				uint cid =
					get_cid_from_address(
						get_address_from_endpoint(udp_endpoint));
				if (EM::Messages::read_retransmit(message, nr) && cid != 0) {
					log() << "READ " << message;
					if (current_nr - nr <= get_buffer_length()) {
						for (uint i = nr; i < current_nr; ++i) {
							ClientQueue q = clients[cid]->get_queue();
							send_data(udp_endpoint, cid, i,
								q.get_expected_nr(),
								q.get_available_space_size(),
								messages[i]);
						}
					}
				} else {
					info() << "READ invalid RETRANSMIT datagram.\n";
				}
				break;
			}
			case EM::Messages::Type::KeepAlive: {
				break;
			}
			default: {
				info() << "READ Unrecognized datagram: " << message << " ("
					<< bytes_received << ")\n";
			}
		}
	}
	udp_receive_routine();
}
Example #15
0
void generic_process(struct ieee80211_frame *wh, struct params *p, int len)
{
	int type, stype;
	int dup = 0;

#if 0
	ack(p, wh);
#endif

#if 0
	if (!for_me(wh, p->mac))
		return;
#endif
	/* ignore my own shit */
	if (memcmp(wh->i_addr2, p->mac, 6) == 0) {
		return;
	}

	type = wh->i_fc[0] & IEEE80211_FC0_TYPE_MASK;
	stype = wh->i_fc[0] & IEEE80211_FC0_SUBTYPE_MASK;

	if (for_me(wh, p->mac) && type == IEEE80211_FC0_TYPE_DATA) {
		/* sequence number & dups */
		if (p->seq_rx == -1)
			p->seq_rx = seqno(wh);
		else {
			int s = seqno(wh);
			
			if (s > p->seq_rx) {
				/* normal case */
				if (p->seq_rx + 1 == s) {
#if 0				
					printf("S=%d\n", s);
#endif					
					p->seq_rx = s;
				}	
				else { /* future */
#if 0				
					printf("Got seq %d, prev %d\n",
					       s, p->seq_rx);
#endif					       
					p->seq_rx = s;
				}
			} else { /* we got pas stuff... */
				if (p->seq_rx - s > 1000) {
#if 0				
					printf("Seqno wrap seq %d, last %d\n",
					       s, p->seq_rx);
#endif					       
					/* seqno wrapping ? */
					p->seq_rx = 0;
				}
				else { /* dup */
					dup = 1;
#if 0
					printf("Got dup seq %d, last %d\n",
					       s, p->seq_rx);
#endif					       
				}
			}
		}
	}
#if 0
	if (wh->i_fc[1] & IEEE80211_FC1_RETRY) {
		printf("Got retry\n");
	}
#endif	
#if 0
	if ((wh->i_fc[0] & IEEE80211_FC0_TYPE_MASK) != IEEE80211_FC0_TYPE_CTL) {
		int rc = send_ack(p->tx, wh->i_addr2);
		if (rc == -1)
			err(1, "send_ack()");
		if (rc != 10) {
			printf("Wrote ACK %d/%d\n", rc, 10);
			exit(1);
		}
	}
#endif

	/* data frames */
	if (type == IEEE80211_FC0_TYPE_DATA && !dup) {
		char *ptr;
		char src[6], dst[6];
		int rc;

		if (wh->i_fc[1] & IEEE80211_FC1_DIR_FROMDS) {
			if (memcmp(wh->i_addr2, p->ap, 6) != 0)
				return;
		} else {
			if (memcmp(wh->i_addr1, p->ap, 6) != 0)
				return;
		}
		

		if (p->state < S_ASSOCIATED) {
			printf("Got data when not associated!\n");
			return;
		}
		if (stype != IEEE80211_FC0_SUBTYPE_DATA) {
			printf("Got weird data frame stype=%d\n",
			       stype >> IEEE80211_FC0_SUBTYPE_SHIFT);
			return;
		}
Example #16
0
File: core.c Project: Xxmmy/radare2
int gdbr_connect(libgdbr_t *g, const char *host, int port) {
	const char *message = "qSupported:multiprocess+;qRelocInsn+;xmlRegisters=i386";
	RStrBuf tmp;
	r_strbuf_init (&tmp);
	int ret;
	if (!g || !host) {
		return -1;
	}
	// Initial max_packet_size for remote target (minimum so far for AVR = 64)
	g->stub_features.pkt_sz = 64;
	char *env_pktsz_str;
	ut32 env_pktsz;
	if ((env_pktsz_str = getenv ("R2_GDB_PKTSZ"))) {
		if ((env_pktsz = (ut32) strtoul (env_pktsz_str, NULL, 10))) {
			g->stub_features.pkt_sz = R_MAX (env_pktsz, 64);
		}
	}
	ret = snprintf (tmp.buf, sizeof (tmp.buf) - 1, "%d", port);
	if (!ret) {
		return -1;
	}
	if (*host == '/') {
		ret = r_socket_connect_serial (g->sock, host, port, 1);
	} else {
		ret = r_socket_connect_tcp (g->sock, host, tmp.buf, 200);
	}
	if (!ret) {
		return -1;
	}
	if (send_ack (g) < 0) {
		return -1;
	}
	read_packet (g);
	g->connected = 1;
	// TODO add config possibility here
	ret = send_msg (g, message);
	if (ret < 0) {
		return ret;
	}
	read_packet (g);
	ret = handle_qSupported (g);
	if (ret < 0) {
		return ret;
	}
	if (env_pktsz > 0) {
		g->stub_features.pkt_sz = R_MAX (R_MIN (env_pktsz, g->stub_features.pkt_sz), 64);
	}
	// If no-ack supported, enable no-ack mode (should speed up things)
	if (g->stub_features.QStartNoAckMode) {
		if (send_msg (g, "QStartNoAckMode") < 0) {
			return -1;
		}
		read_packet (g);
		if (!strncmp (g->data, "OK", 2)) {
			// Just in case, send ack
			send_ack (g);
			g->no_ack = true;
		}
	}
	if (g->remote_type == GDB_REMOTE_TYPE_LLDB) {
		return gdbr_connect_lldb (g);
	}
	// Query the thread / process id
	g->stub_features.qC = true;
	g->pid = g->tid = 0;
	ret = send_msg (g, "qC");
	if (ret < 0) {
		return ret;
	}
	read_packet (g);
	ret = handle_qC (g);
	if (ret < 0) {
		g->stub_features.qC = false;
	}
	// Check if vCont is supported
	gdbr_check_vcont (g);
	// Set pid/thread for operations other than "step" and "continue"
	if (g->stub_features.multiprocess) {
		snprintf (tmp.buf, sizeof (tmp.buf) - 1, "Hgp%x.0", (ut32) g->pid);
#if 0
		if (g->tid < 0) {
			snprintf (tmp.buf, sizeof (tmp.buf) - 1, "Hgp%x.-1", (ut32) g->pid);
		} else {
			snprintf (tmp.buf, sizeof (tmp.buf) - 1, "Hgp%x.%x", (ut32) g->pid, (ut32) g->tid);
		}
#endif
	} else {
		snprintf (tmp.buf, sizeof (tmp.buf) - 1, "Hg0");
#if 0
		if (g->tid < 0) {
			snprintf (tmp.buf, sizeof (tmp.buf) - 1, "Hg-1");
		} else {
			snprintf (tmp.buf, sizeof (tmp.buf) - 1, "Hg%x", (ut32) g->tid);
		}
#endif
	}
	ret = send_msg (g, tmp.buf);
	if (ret < 0) {
		return ret;
	}
	read_packet (g);
	ret = send_ack (g);
	if (strncmp (g->data, "OK", 2)) {
		// return -1;
	}
	// Set thread for "step" and "continue" operations
	snprintf (tmp.buf, sizeof (tmp.buf) - 1, "Hc-1");
	ret = send_msg (g, tmp.buf);
	if (ret < 0) {
		return ret;
	}
	read_packet (g);
	ret = send_ack (g);
	if (strncmp (g->data, "OK", 2)) {
		// return -1;
	}
	if (g->stub_features.qXfer_features_read) {
		gdbr_read_target_xml (g);
	}
	reg_cache_init (g);
	return ret;
}
Example #17
0
static bool gcs2mav(uint16_t N){
  uint32_t seq = 0;
  uint32_t retry_cnt = PLANNER_RETRY_CNT;
  eventmask_t evt = 0;
  mavlink_mission_item_t mi;        /* local copy */
  uint8_t status = MAV_MISSION_ERROR;

  /* check available space */
  if (N > wpdb.getCapacity()){
    send_ack(MAV_MISSION_NO_SPACE);
    return CH_FAILED;
  }

  /* prepare to harvest waypoints */
  struct EventListener el_mission_item;
  chEvtRegisterMask(&event_mavlink_in_mission_item, &el_mission_item, EVMSK_MAVLINK_IN_MISSION_ITEM);
  chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_ITEM, 1);/* fake wait to clear "queue" */
  if (CH_FAILED == wpdb.clear())
    chDbgPanic("");

  for (seq=0; seq<N; seq++){
    /* prepare request to ground */
    mavlink_out_mission_request_struct.target_component = MAV_COMP_ID_MISSIONPLANNER;
    mavlink_out_mission_request_struct.target_system = GROUND_STATION_ID;
    mavlink_out_mission_request_struct.seq = seq;
    retry_cnt = PLANNER_RETRY_CNT;
    chThdSleep(PLANNER_ADDITIONAL_TMO);

    do{
      /* drop message */
      chEvtBroadcastFlags(&event_mavlink_out_mission_request, EVMSK_MAVLINK_OUT_MISSION_REQUEST);

      /* wait answer */
      evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_ITEM, PLANNER_RETRY_TMO);
      if (EVMSK_MAVLINK_IN_MISSION_ITEM == evt){
        chSysLock();
        mi = mavlink_in_mission_item_struct;
        chSysUnlock();

        /* check waypoint cosherness and write it if cosher */
        status = check_wp(&mi, seq);
        if (status != MAV_MISSION_ACCEPTED)
          goto EXIT;
        else{
          if (CH_FAILED == wpdb.save(&mi, seq))
            chDbgPanic("");
          break; /* do-while */
        }
      }

      retry_cnt--;
      if(0 == retry_cnt)
        goto EXIT;

    }while(retry_cnt);
  }
  /* save waypoint count in eeprom only in the very end of transaction */
  if (CH_FAILED == wpdb.finalize())
    chDbgPanic("");

  /* final stuff */
EXIT:
  chEvtUnregister(&event_mavlink_in_mission_item, &el_mission_item);
  send_ack(status);
  if (0 == retry_cnt)
    return CH_FAILED;
  else
    return CH_SUCCESS;
}
Example #18
0
File: core.c Project: Xxmmy/radare2
ut64 gdbr_get_baddr(libgdbr_t *g) {
	if (!g || send_msg (g, "qOffsets") < 0 || read_packet (g) < 0
	    || send_ack (g) < 0 || g->data_len == 0) {
		return UINT64_MAX;
	}
	ut64 off, min = UINT64_MAX;
	char *ptr;
	if (r_str_startswith (g->data, "TextSeg=")) {
		ptr = g->data + strlen ("TextSeg=");
		if (!isxdigit (*ptr)) {
			return min;
		}
		off = strtoull (ptr, NULL, 16);
		if (off < min) {
			min = off;
		}
		if (!(ptr = strchr (ptr, ';'))) {
			return min;
		}
		ptr++;
		if (*ptr && r_str_startswith (ptr, "DataSeg=")) {
			ptr += strlen ("DataSeg=");
			if (!isxdigit (*ptr)) {
				return min;
			}
			off = strtoull (ptr, NULL, 16);
			if (off < min) {
				min = off;
			}
		}
		return min;
	}
	if (!r_str_startswith (g->data, "Text=")) {
		return min;
	}
	ptr = g->data + strlen ("Text=");
	if (!isxdigit (*ptr)) {
		return min;
	}
	off = strtoull (ptr, NULL, 16);
	if (off < min) {
		min = off;
	}
	if (!(ptr = strchr (ptr, ';')) || !r_str_startswith (ptr + 1, "Data=")) {
		return UINT64_MAX;
	}
	ptr += strlen (";Data=");
	if (!isxdigit (*ptr)) {
		return UINT64_MAX;
	}
	off = strtoull (ptr, NULL, 16);
	if (off < min) {
		min = off;
	}
	if (!(ptr = strchr (ptr, ';'))) {
		return min;
	}
	ptr++;
	if (r_str_startswith (ptr, "Bss=")) {
		ptr += strlen ("Bss=");
		if (!isxdigit (*ptr)) {
			return min;
		}
		off = strtoull (ptr, NULL, 16);
		if (off < min) {
			min = off;
		}
	}
	return min;
}
Example #19
0
int handle_removebp(libgdbr_t* g) {
	return send_ack (g);
}
Example #20
0
int handle_cont(libgdbr_t* g) {
	// Possible answers here 'S,T,W,X,O,F'
	return send_ack (g);
}
Example #21
0
int handle_cmd(libgdbr_t* g) {
	unpack_hex (g->data, strlen (g->data), g->data);
	g->data_len = strlen (g->data) / 2;
	return send_ack (g);
}
Example #22
0
int handle_M(libgdbr_t* g) {
	return send_ack (g);
}
Example #23
0
int handle_m(libgdbr_t* g) {
	int len = strlen (g->data);
	g->data_len = strlen (g->data) / 2;
	unpack_hex (g->data, len, g->data);
	return send_ack (g);
}
Example #24
0
/**
 * Lazy clear all routine.
 */
static void mission_clear_all(void){
  if (CH_SUCCESS == wpdb.clear())
    send_ack(MAV_MISSION_ACCEPTED);
  else
    send_ack(MAV_MISSION_ERROR);
}
Example #25
0
int handle_connect(libgdbr_t* g) {
	// TODO handle the message correct and set all infos like packetsize, thread stuff and features
	return send_ack (g);
}
Example #26
0
static void
handle_sol_port_payload(lanserv_data_t *lan, ipmi_sol_t *sol, msg_t *msg)
{
    soldata_t *sd = sol->soldata;
    unsigned char seq, ack, count;
    char isnack, isbreak, ctspause, deassert_dcd, flush_in, flush_out;
    unsigned char *data;
    unsigned int len;
    int need_send_ack = 0;
    struct timeval tv;

    if (!sol->active || msg->len < 4)
	return;

    seq = msg->data[0] & 0xf;
    ack = msg->data[1] & 0xf;
    count = msg->data[2];
    isnack = msg->data[3] & (1 << 6);
    /* Ring Indicator is ignored for now */
    isbreak = msg->data[3] & (1 << 4);
    ctspause = msg->data[3] & (1 << 3);
    deassert_dcd = msg->data[3] & (1 << 2);
    flush_in = msg->data[3] & (1 << 1);
    flush_out = msg->data[3] & (1 << 0);

    data = msg->data + 4;
    len = msg->len - 4;

    if (seq != 0) {
	if (seq == sd->last_acked_packet) {
	    need_send_ack = 1;
	} else if (sd->fd == -1) {
	    /* Ignore the data. */
	    if  (len) {
		sd->last_acked_packet = seq;
		need_send_ack = 1;
		sd->last_acked_packet_len = len;
	    }
	} else if (len) {
	    sd->last_acked_packet = seq;
	    if (sol->do_telnet) {
		unsigned int i;

		for (i = 0; i < len; i++) {
		    int left = sizeof(sd->inbuf) - sd->inlen;
		    if (left < 1)
			break;
		    if (data[i] == SOL_TELNET_IAC) {
			if (left < 2)
			    break;
			sd->inbuf[sd->inlen++] = SOL_TELNET_IAC;
		    }
		    sd->inbuf[sd->inlen++] = data[i];
		}
	    } else {
		if (len > (sizeof(sd->inbuf) - sd->inlen))
		    len = sizeof(sd->inbuf) - sd->inlen;
		memcpy(sd->inbuf + sd->inlen, data, len);
		sd->inlen += len;
	    }

	    sd->last_acked_packet_len = len;
	    need_send_ack = 1;
	    set_write_enable(sol->soldata);
	}
    }

    if (ack == sd->curr_packet_seq) {
	next_seq(sd);
	sd->sys->stop_timer(sd->timer);
	if (isnack) {
	    sd->in_nack = 1;
	    set_read_enable(sd);
	} else {
	    sd->in_nack = 0;
	    if (count < sd->outlen) {
		unsigned int i;
		len = sd->outlen - count;
		for (i = 0; i < len; i++)
		    sd->outbuf[i] = sd->outbuf[i + count];
		sd->outlen = len;
		
		send_data(sol, need_send_ack);
		need_send_ack = 0;
		tv.tv_sec = 1;
		tv.tv_usec = 0;
		sd->sys->start_timer(sd->timer, &tv);
	    } else {
		sd->waiting_ack = 0;
		sd->outlen = 0;
	    }
	    set_read_enable(sd);
	}
    }

    if (need_send_ack)
	send_ack(sol);

    if (flush_out) {
	sd->waiting_ack = 0;
	next_seq(sd);
	sd->outlen = 0;
    }

    if (flush_in)
	sd->inlen = 0;

    if (isbreak)
	sd->send_break(sol);

    sd->update_modemstate(sol, ctspause, deassert_dcd);
}
Example #27
0
int handle_setbp(libgdbr_t* g) {
	return send_ack (g);
}
Example #28
0
File: xml.c Project: agatti/radare2
static char *gdbr_read_feature(libgdbr_t *g, const char *file, ut64 *tot_len) {
	ut64 retlen = 0, retmax = 0, off = 0, len = g->stub_features.pkt_sz - 2,
	     blksz = g->data_max, subret_space = 0, subret_len = 0;
	char *tmp, *tmp2, *tmp3, *ret = NULL, *subret = NULL, msg[128] = { 0 },
	     status, tmpchar;
	while (1) {
		snprintf (msg, sizeof (msg), "qXfer:features:read:%s:%"PFMT64x
			",%"PFMT64x, file, off, len);
		if (send_msg (g, msg) < 0
		    || read_packet (g, false) < 0 || send_ack (g) < 0) {
			goto exit_err;
		}
		if (g->data_len == 0) {
			goto exit_err;
		}
		if (g->data_len == 1 && g->data[0] == 'l') {
			break;
		}
		status = g->data[0];
		if (retmax - retlen < g->data_len) {
			if (!(tmp = realloc (ret, retmax + blksz))) {
				goto exit_err;
			}
			retmax += blksz;
			ret = tmp;
		}
		strcpy (ret + retlen, g->data + 1);
		retlen += g->data_len - 1;
		off = retlen;
		if (status == 'l') {
			break;
		}
		if (status != 'm') {
			goto exit_err;
		}
	}
	if (!ret) {
		*tot_len = 0;
		return NULL;
	}
	tmp = strstr (ret, "<xi:include");
	while (tmp) {
		// inclusion
		if (!(tmp2 = strstr (tmp, "/>"))) {
			goto exit_err;
		}
		subret_space = tmp2 + 2 - tmp;
		if (!(tmp2 = strstr (tmp, "href="))) {
			goto exit_err;
		}
		tmp2 += 6;
		if (!(tmp3 = strchr (tmp2, '"'))) {
			goto exit_err;
		}
		tmpchar = *tmp3;
		*tmp3 = '\0';
		subret = gdbr_read_feature (g, tmp2, &subret_len);
		*tmp3 = tmpchar;
		if (subret) {
			if (subret_len <= subret_space) {
				memcpy (tmp, subret, subret_len);
				memcpy (tmp + subret_len, tmp + subret_space,
					retlen - (tmp + subret_space - ret));
				retlen -= subret_space - subret_len;
				ret[retlen] = '\0';
				tmp = strstr (tmp3, "<xi:include");
				continue;
			}
			if (subret_len > retmax - retlen - 1) {
				tmp3 = NULL;
				if (!(tmp3 = realloc (ret, retmax + subret_len))) {
					free (subret);
					goto exit_err;
				}
				tmp = tmp3 + (tmp - ret);
				ret = tmp3;
				retmax += subret_len + 1;
			}
			memmove (tmp + subret_len, tmp + subret_space,
				retlen - (tmp + subret_space - ret));
			memcpy (tmp, subret, subret_len);
			retlen += subret_len - subret_space;
			ret[retlen] = '\0';
			free (subret);
		}
		tmp = strstr (tmp3, "<xi:include");
	}
	*tot_len = retlen;
	return ret;
exit_err:
	free (ret);
	*tot_len = 0;
	return NULL;
}
Example #29
0
/*  This function is called whenever a reply for our module is received; 
  * we need to register  this function on module initialization;
  *  Returns :   0 - core router stops
  *              1 - core router relay statelessly
  */
int reply_received( struct sip_msg  *p_msg )
{
	int msg_status;
	int last_uac_status;
	int branch;
	int reply_status;
	utime_t timer;
	/* has the transaction completed now and we need to clean-up? */
	branch_bm_t cancel_bitmap;
	struct ua_client *uac;
	struct cell *t;
	struct usr_avp **backup_list;
	unsigned int has_reply_route;

	set_t(T_UNDEFINED);

	/* make sure we know the associated transaction ... */
	if (t_check(p_msg, &branch ) == -1) goto not_found;

	/*... if there is none, tell the core router to fwd statelessly */
	t = get_t();
	if ((t == 0) || (t == T_UNDEFINED)) goto not_found;

	cancel_bitmap=0;
	msg_status=p_msg->REPLY_STATUS;

	uac=&t->uac[branch];
	LM_DBG("org. status uas=%d, uac[%d]=%d local=%d is_invite=%d)\n",
		t->uas.status, branch, uac->last_received, 
		is_local(t), is_invite(t));
	last_uac_status=uac->last_received;
	if_update_stat( tm_enable_stats, tm_rcv_rpls , 1);

	/* it's a cancel which is not e2e ? */
	if ( get_cseq(p_msg)->method_id==METHOD_CANCEL && is_invite(t) ) {
		/* ... then just stop timers */
		reset_timer( &uac->local_cancel.retr_timer);
		if ( msg_status >= 200 ) {
				reset_timer( &uac->local_cancel.fr_timer);
		}
		LM_DBG("reply to local CANCEL processed\n");
		goto done;
	}

	/* *** stop timers *** */
	/* stop retransmission */
	reset_timer(&uac->request.retr_timer);

	/* stop final response timer only if I got a final response */
	if ( msg_status >= 200 ) {
		reset_timer( &uac->request.fr_timer);
	}

	/* acknowledge negative INVITE replies (do it before detailed
	 * on_reply processing, which may take very long, like if it
	 * is attempted to establish a TCP connection to a fail-over dst */
	if (is_invite(t) && ((msg_status >= 300) ||
	(is_local(t) && !no_autoack(t) && msg_status >= 200) )) {
		if (send_ack(p_msg, t, branch)!=0)
			LM_ERR("failed to send ACK (local=%s)\n", is_local(t)?"yes":"no");
	}

	_tm_branch_index = branch;

	/* processing of on_reply block */
	has_reply_route = (t->on_reply) || (t->uac[branch].on_reply);
	if (has_reply_route) {
		if (onreply_avp_mode) {
			/* lock the reply*/
			LOCK_REPLIES( t );
			/* set the as avp_list the one from transaction */
			backup_list = set_avp_list(&t->user_avps);
		} else {
			backup_list = 0;
		}
		/* transfer transaction flag to branch context */
		p_msg->flags = t->uas.request->flags;
		setb0flags(t->uac[branch].br_flags);
		/* run block - first per branch and then global one */
		if ( t->uac[branch].on_reply &&
		(run_top_route(onreply_rlist[t->uac[branch].on_reply].a,p_msg)
		&ACT_FL_DROP) && (msg_status<200) ) {
			if (onreply_avp_mode) {
				UNLOCK_REPLIES( t );
				set_avp_list( backup_list );
			}
			LM_DBG("dropping provisional reply %d\n", msg_status);
			goto done;
		}
		if ( t->on_reply && (run_top_route(onreply_rlist[t->on_reply].a,p_msg)
		&ACT_FL_DROP) && (msg_status<200) ) {
			if (onreply_avp_mode) {
				UNLOCK_REPLIES( t );
				set_avp_list( backup_list );
			}
			LM_DBG("dropping provisional reply %d\n", msg_status);
			goto done;
		}
		/* transfer current message context back to t */
		t->uac[branch].br_flags = getb0flags();
		t->uas.request->flags = p_msg->flags;
		if (onreply_avp_mode)
			/* restore original avp list */
			set_avp_list( backup_list );
	}

	if (!onreply_avp_mode || !has_reply_route)
		/* lock the reply*/
		LOCK_REPLIES( t );

	/* mark that the UAC received replies */
	uac->flags |= T_UAC_HAS_RECV_REPLY;

	/* we fire a cancel on spot if (a) branch is marked "to be canceled" or (b)
	 * the whole transaction was canceled (received cancel) and no cancel sent
	 * yet on this branch; and of course, only if a provisional reply :) */
	if (t->uac[branch].flags&T_UAC_TO_CANCEL_FLAG ||
	((t->flags&T_WAS_CANCELLED_FLAG) && !t->uac[branch].local_cancel.buffer.s)) {
		if ( msg_status < 200 )
			/* reply for an UAC with a pending cancel -> do cancel now */
			cancel_branch(t, branch);
		/* reset flag */
		t->uac[branch].flags &= ~(T_UAC_TO_CANCEL_FLAG);
	}

	if (is_local(t)) {
		reply_status = local_reply(t,p_msg, branch,msg_status,&cancel_bitmap);
		if (reply_status == RPS_COMPLETED) {
			cleanup_uac_timers(t);
			if (is_invite(t)) cancel_uacs(t, cancel_bitmap);
			/* There is no need to call set_final_timer because we know
			 * that the transaction is local */
			put_on_wait(t);
		}
	} else {
		reply_status = relay_reply(t,p_msg,branch,msg_status,&cancel_bitmap);
		/* clean-up the transaction when transaction completed */
		if (reply_status == RPS_COMPLETED) {
			/* no more UAC FR/RETR (if I received a 2xx, there may
			 * be still pending branches ...
			 */
			cleanup_uac_timers(t);
			if (is_invite(t)) cancel_uacs(t, cancel_bitmap);
			/* FR for negative INVITES, WAIT anything else */
			/* set_final_timer(t); */
		}
	}
	
	if (reply_status!=RPS_PROVISIONAL)
		goto done;
	
	/* update FR/RETR timers on provisional replies */
	if (msg_status < 200 && (restart_fr_on_each_reply ||
	((last_uac_status<msg_status) &&
	((msg_status >= 180) || (last_uac_status == 0)))
	) ) { /* provisional now */
		if (is_invite(t)) {
			/* invite: change FR to longer FR_INV, do not
			 * attempt to restart retransmission any more
			 */
			backup_list = set_avp_list(&t->user_avps);
			if (!fr_inv_avp2timer(&timer)) {
				LM_DBG("FR_INV_TIMER = %lld\n", timer);
				set_timer(&uac->request.fr_timer,
					FR_INV_TIMER_LIST, &timer);
			} else {
				set_timer(& uac->request.fr_timer, FR_INV_TIMER_LIST, 0);
			}
			set_avp_list(backup_list);
		} else {
			/* non-invite: restart retransmissions (slow now) */
			uac->request.retr_list = RT_T2;
			set_timer(&uac->request.retr_timer, RT_T2, 0);
		}
	} /* provisional replies */
	
done:
	/* we are done with the transaction, so unref it - the reference
	 * was incremented by t_check() function -bogdan*/
	t_unref(p_msg);
	/* don't try to relay statelessly neither on success
	 * (we forwarded statefully) nor on error; on troubles, 
	 * simply do nothing; that will make the other party to 
	 * retransmit; hopefuly, we'll then be better off 
	 */
	_tm_branch_index = 0;
	return 0;
not_found:
	set_t(T_UNDEFINED);
	return 1;
}
Example #30
0
File: core.c Project: Xxmmy/radare2
RList* gdbr_threads_list(libgdbr_t *g, int pid) {
	if (!g) {
		return NULL;
	}
	RList *list;
	int tpid = -1, ttid = -1;
	char *ptr, *ptr2, *exec_file;
	RDebugPid *dpid;
	if (!g->stub_features.qXfer_exec_file_read
	    || !(exec_file = gdbr_exec_file_read (g, pid))) {
		exec_file = "";
	}
	if (g->stub_features.qXfer_threads_read) {
		// XML thread description is supported
		// TODO: Handle this case
	}
	if (send_msg (g, "qfThreadInfo") < 0 || read_packet (g) < 0 || send_ack (g) < 0
	    || g->data_len == 0 || g->data[0] != 'm') {
		return NULL;
	}
	if (!(list = r_list_new())) {
		return NULL;
	}
	while (1) {
		g->data[g->data_len] = '\0';
		ptr = g->data + 1;
		while (ptr) {
			if ((ptr2 = strchr (ptr, ','))) {
				*ptr2 = '\0';
				ptr2++;
			}
			if (read_thread_id (ptr, &tpid, &ttid,
					    g->stub_features.multiprocess) < 0) {
				ptr = ptr2;
				continue;
			}
			if (g->stub_features.multiprocess && tpid != pid) {
				ptr = ptr2;
				continue;
			}
			if (!(dpid = R_NEW0 (RDebugPid)) || !(dpid->path = strdup (exec_file))) {
				r_list_free (list);
				free (dpid);
				return NULL;
			}
			dpid->uid = dpid->gid = -1; // TODO
			dpid->pid = ttid;
			dpid->runnable = true;
			// This is what linux native does as fallback, but
			// probably not correct.
			// TODO: Implement getting correct thread status from GDB
			dpid->status = R_DBG_PROC_STOP;
			r_list_append (list, dpid);
			ptr = ptr2;
		}
		if (send_msg (g, "qsThreadInfo") < 0 || read_packet (g) < 0
		    || send_ack (g) < 0 || g->data_len == 0
		    || (g->data[0] != 'm' && g->data[0] != 'l')) {
			r_list_free (list);
			return NULL;
		}
		if (g->data[0] == 'l') {
			break;
		}
	}
	RListIter *iter;
	// This is the all I've been able to extract from gdb so far
	r_list_foreach (list, iter, dpid) {
		if (gdbr_is_thread_dead (g, pid, dpid->pid)) {
			dpid->status = R_DBG_PROC_DEAD;
		}
	}
	return list;
}