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;
}
/*
* 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);
}
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",
stype >> IEEE80211_FC0_SUBTYPE_SHIFT);
return;
}
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;
}
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);
}
int handle_M(libgdbr_t* g) {
return send_ack (g);
}
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);
}
/**
* 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);
}
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);
}
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);
}
int handle_setbp(libgdbr_t* g) {
return send_ack (g);
}
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;
}
/* 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;
}
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;
}
