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

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

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

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

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

/**
 * [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;
}

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

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

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

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

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

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

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

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",
//.........这里部分代码省略.........

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;
	}
//.........这里部分代码省略.........

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

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

int handle_removebp(libgdbr_t* g) {
	return send_ack (g);
}

int handle_cont(libgdbr_t* g) {
	// Possible answers here 'S,T,W,X,O,F'
	return send_ack (g);
}

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