/*---------------------------------------------------------------------------*/
static void
handle_data(struct rudolph1_conn *c, struct rudolph1_datapacket *p)
{
if(LT(c->version, p->h.version)) {
PRINTF("%d.%d: rudolph1 new version %d, chunk %d\n",
rimeaddr_node_addr.u8[0], rimeaddr_node_addr.u8[1],
p->h.version, p->h.chunk);
c->version = p->h.version;
c->highest_chunk_heard = c->chunk = 0;
if(p->h.chunk != 0) {
send_nack(c);
} else {
write_data(c, 0, p->data, p->datalen);
c->chunk = 1; /* Next chunk is 1. */
}
/* }*/
} else if(p->h.version == c->version) {
PRINTF("%d.%d: got chunk %d (%d) highest heard %d\n",
rimeaddr_node_addr.u8[0], rimeaddr_node_addr.u8[1],
p->h.chunk, c->chunk, c->highest_chunk_heard);
if(p->h.chunk == c->chunk) {
PRINTF("%d.%d: received chunk %d\n",
rimeaddr_node_addr.u8[0], rimeaddr_node_addr.u8[1],
p->h.chunk);
write_data(c, p->h.chunk, p->data, p->datalen);
if(c->highest_chunk_heard < c->chunk) {
c->highest_chunk_heard = c->chunk;
}
c->chunk++;
} else if(p->h.chunk > c->chunk) {
PRINTF("%d.%d: received chunk %d > %d, sending NACK\n",
rimeaddr_node_addr.u8[0], rimeaddr_node_addr.u8[1],
p->h.chunk, c->chunk);
send_nack(c);
c->highest_chunk_heard = p->h.chunk;
} else if(p->h.chunk < c->chunk) {
/* Ignore packets with a lower chunk number */
}
/* If we have heard a higher chunk number, we send a NACK so that
we get a repair for the next packet. */
if(c->highest_chunk_heard > p->h.chunk) {
send_nack(c);
}
} else { /* p->h.version < c->current.h.version */
/* Ignore packets with old version */
}
}
static bool ast_write_data(struct drm_device *dev, u8 data)
{
struct ast_private *ast = dev->dev_private;
if (wait_nack(ast)) {
send_nack(ast);
ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x9a, 0x00, data);
send_ack(ast);
if (wait_ack(ast)) {
send_nack(ast);
return true;
}
}
send_nack(ast);
return false;
}
/* called from layer 3, when a packet arrives for layer 4 */
void A_input(struct pkt packet)
{
printf("CCH> A_input> Got packet\n");
// isChecksumValid
if(pkt_checksum_valid(&packet)) {
printf("CCH> A_input> Valid checksum\n");
// isACK
if(strncmp(packet.payload, ACK, strlen(ACK)) == 0) {
if(packet.acknum == last_pkt->seqnum) {
printf("CCH> A_input> Received valid ACK\n");
last_ack = &packet;
stoptimer(A);
} else {
// We received an ACK we don't care about
printf("CCH> A_input> Received invalid ACK (ignoring)\n");
}
// isNACK
} else if (strncmp(packet.payload, NACK, strlen(ACK)) == 0) {
printf("CCH> A_input> Received NACK\n");
send_pkt(A, last_pkt);
} else {
// Message
stoptimer(A);
tolayer5(A, packet.payload);
}
} else {
printf("CCH> A_input> Invalid checksum\n");
send_nack(A, &packet);
stoptimer(A);
starttimer(A, TIMEOUT);
return;
}
}
static bool ast_read_data(struct drm_device *dev, u8 *data)
{
struct ast_private *ast = dev->dev_private;
u8 tmp;
*data = 0;
if (wait_ack(ast) == false)
return false;
tmp = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xd3, 0xff);
*data = tmp;
if (wait_nack(ast) == false) {
send_nack(ast);
return false;
}
send_nack(ast);
return true;
}
static bool ast_write_cmd(struct drm_device *dev, u8 data)
{
struct ast_private *ast = dev->dev_private;
int retry = 0;
if (wait_nack(ast)) {
send_nack(ast);
ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x9a, 0x00, data);
send_ack(ast);
set_cmd_trigger(ast);
do {
if (wait_ack(ast)) {
clear_cmd_trigger(ast);
send_nack(ast);
return true;
}
} while (retry++ < 100);
}
clear_cmd_trigger(ast);
send_nack(ast);
return false;
}
/*---------------------------------------------------------------------------*/
static void
recv(struct stbroadcast_conn *stbroadcast)
{
struct rudolph0_conn *c = (struct rudolph0_conn *)stbroadcast;
struct rudolph0_datapacket *p = packetbuf_dataptr();
if(p->h.type == TYPE_DATA) {
if(c->current.h.version != p->h.version) {
PRINTF("rudolph0 new version %d\n", p->h.version);
c->current.h.version = p->h.version;
c->current.h.chunk = 0;
c->cb->write_chunk(c, 0, RUDOLPH0_FLAG_NEWFILE, p->data, 0);
if(p->h.chunk != 0) {
send_nack(c);
} else {
c->cb->write_chunk(c, 0, RUDOLPH0_FLAG_NONE, p->data, p->datalen);
c->current.h.chunk++;
}
} else if(p->h.version == c->current.h.version) {
if(p->h.chunk == c->current.h.chunk) {
PRINTF("received chunk %d\n", p->h.chunk);
if(p->datalen < RUDOLPH0_DATASIZE) {
c->cb->write_chunk(c, c->current.h.chunk * RUDOLPH0_DATASIZE,
RUDOLPH0_FLAG_LASTCHUNK, p->data, p->datalen);
} else {
c->cb->write_chunk(c, c->current.h.chunk * RUDOLPH0_DATASIZE,
RUDOLPH0_FLAG_NONE, p->data, p->datalen);
}
c->current.h.chunk++;
} else if(p->h.chunk > c->current.h.chunk) {
PRINTF("received chunk %d > %d, sending NACK\n", p->h.chunk, c->current.h.chunk);
send_nack(c);
}
} else { /* p->h.version < c->current.h.version */
/* Ignore packets with old version */
}
}
}
void UdpMulticastChannel::notify_received(char* data, size_t sz,boost::asio::ip::udp::endpoint sender_endpoint)
{
std::vector<unsigned>* lost;
NetworkPacket* packet = new NetworkPacket;
if (packet->ParseFromArray(data,sz))
{
lost = incoming->add_packet(packet);
if (lost->size() > 0 )
{
send_nack(packet,sender_endpoint,lost);
}
}
//std::cout << "Received: " << sz << " bytes " << std::endl;
receive_from_network();
}
void* rms_manager(void* arg) {
//init
int nfound;
int ret;
int err;
int itr;
int ipc_read;
int count;
int gottime;
char *data;
float t1;
float t2;
fd_set readset;
struct timeval timeout;
struct timeval ratechk;
unsigned long *keys;
unsigned long *seq;
rms_meta_msg *msg;
rms_meta_peer *peer;
int addr_len;
rms_msg_hbeat *hbeat;
rmscb *rmsb = (rmscb*)arg;
gottime = 0;
while(rmsb->active) {
FD_ZERO(&readset);
FD_SET(rmsb->ipc_sock, &readset);
FD_SET(rmsb->mcast_sock, &readset);
timeout.tv_sec = 1;
timeout.tv_usec = 0;
nfound= select(rmsb->maxsock, &readset, (fd_set*)0, (fd_set*)0, &timeout);
if(nfound > 0) {
//available to read on mcast port
if((nfound = FD_ISSET(rmsb->mcast_sock, &readset)) > 0) {
msg = alloc_meta_msg();
ret = recvfrom(rmsb->mcast_sock,
&msg->buf,
msg->size,
0,
(struct sockaddr*)&rmsb->recv_addr,
&addr_len);
printf("\n\tRMC: recvd %d bytes from %ld",
ret,
rmsb->recv_addr.sin_addr.s_addr);
if(ret < sizeof(rms_msg_hdr)) {
//not enough data
continue;
}
msg->hdr->length = ntohs(msg->hdr->length);
msg->hdr->sequence = ntohl(msg->hdr->sequence);
msg->hdr->id = ntohl(msg->hdr->id);
if(ret < msg->hdr->length + sizeof(rms_msg_hdr)) {
//not enough data
continue;
}
switch(msg->hdr->type) {
case RMS_DATA:
if(!htbl_get(rmsb->ht_peers, msg->hdr->id, (void*)&peer)) {
//peer not registered, drop packet
release_meta_msg(msg);
}
else {
peer->sequence_sent = msg->hdr->sequence;
if(peer->sequence_rcvd + 1 != msg->hdr->sequence) {
//gap in sequence num
if(peer->sequence_rcvd < msg->hdr->sequence) {
//cache msg
htbl_put(peer->ht_unseq_msgs, msg->hdr->sequence, msg);
//send NACK
msg = alloc_meta_msg();
send_nack(rmsb, peer, msg);
}
//else retransmitted data, drop packet
release_meta_msg(msg);
}
else {
//in sequence
//syncqueue_signal_data(rmsb->inmsgq, msg);
nfound = msg->hdr->length;
ret = send(rmsb->ipc_sock, &nfound, sizeof(int),0);
ret = 0;
while(ret < nfound) {
ret+=send(rmsb->ipc_sock, msg->data+ret, nfound-ret, 0);
}
peer->sequence_rcvd++;
//release out of sequence data if any
while(htbl_remove(peer->ht_unseq_msgs, peer->sequence_sent, (void*)&msg)) {
//syncqueue_signal_data(rmsb->inmsgq, msg);
nfound = msg->hdr->length;
ret = send(rmsb->ipc_sock, &nfound, sizeof(int),0);
ret = 0;
while(ret < nfound) {
ret+=send(rmsb->ipc_sock, msg->data+ret, nfound-ret, 0);
}
peer->sequence_rcvd++;
}
}}
break;
case RMS_HEARTBEAT:
hbeat = (rms_msg_hbeat*) msg->data;
data = msg->data + sizeof(rms_msg_hbeat);
if(!htbl_get(rmsb->ht_peers, msg->hdr->id, (void*)&peer)) {
//peer not registered, register it
if(hbeat->namelen) {
rms_meta_peer_init(&peer, msg->data, msg->hdr->id);
}
else {
rms_meta_peer_init(&peer, "noname", msg->hdr->id);
}
htbl_put(rmsb->ht_peers, peer->id, peer);
mqueue_add(rmsb->mq_peers, peer);
}
peer->lost = ntohs(hbeat->lost);
//check for namelen
if(hbeat->namelen) {
hbeat->namelen = htonl(hbeat->namelen);
data+=hbeat->namelen;
}
//check for acks
if(hbeat->acks) {
seq = (unsigned long*)data;
hbeat->acks = ntohs(hbeat->acks);
for(itr=0;itr < hbeat->acks; itr++) {
if(ntohl(seq[itr*2])== rmsb->self->id) {
peer->sequence_sync = ntohl(seq[itr*2 + 1]);
break;
}
}
data+=hbeat->acks*2*sizeof(long);
}
//check for piggybacked data
if(hbeat->pgback) {
hbeat->pgback = ntohs(hbeat->pgback);
}
gettimeofday(&timeout, NULL);
gottime=1;
peer->last_hbeat.tv_sec = timeout.tv_sec;
peer->last_hbeat.tv_usec = timeout.tv_usec;
if(peer->sequence_sent + 1 != msg->hdr->sequence) {
//gap in sequence num
if(peer->sequence_sent < msg->hdr->sequence) {
//send NACK
send_nack(rmsb, peer, msg);
}
}
release_meta_msg(msg);
break;
case RMS_NACK:
seq = (unsigned long*)msg->data;
count = seq[0];
rmsb->ucast_addr.sin_addr.s_addr = htonl(msg->hdr->id);
for(itr=1; itr <= count; itr++) {
seq[itr]=ntohl(seq[itr]);
if(htbl_get(rmsb->outmsgbuf, seq[itr], (void*)&msg)) {
ret = sendto(rmsb->mcast_sock,
msg->buf,
msg->length + sizeof(rms_msg_hdr),
0,
(struct sockaddr*) &rmsb->ucast_addr,
sizeof(rmsb->ucast_addr));
}
}
break;
case RMS_LEAVE:
if(htbl_remove(rmsb->ht_peers, msg->hdr->id, (void*)&peer)) {
//clean up unseq msgs buffer
count = htbl_count(peer->ht_unseq_msgs);
keys = (unsigned long*)malloc(count*sizeof(unsigned long));
memset(keys,0,count*sizeof(unsigned long));
count = htbl_enum_keys(peer->ht_unseq_msgs, (void*)keys, count);
for(itr = 0; itr < count; itr++) {
if(htbl_remove(peer->ht_unseq_msgs, keys[itr], (void*)&msg)) {
release_meta_msg(msg);
}
}
free(keys);
mqueue_remove_item(rmsb->mq_peers, peer);
rms_meta_peer_free(peer);
}
break;
case RMS_ACK:
case RMS_JOIN:
case RMS_CERT:
default:
//unimplemented options, drop packet
release_meta_msg(msg);
break;
}
}
//available to read on ipc port
if((nfound = FD_ISSET(rmsb->ipc_sock, &readset)) > 0) {
err = 1;
ret = recv(rmsb->ipc_sock,
&ipc_read,
sizeof(int),
0);
if(ret > 0) {
//alloc buffer
msg = alloc_meta_msg();
if(ipc_read > rmsb->bufsize) {
rmsb->bufsize = ipc_read * 1.5;
msg->size = rmsb->bufsize;
msg->buf = realloc(msg->buf,msg->size);
msg->hdr = (rms_msg_hdr*)msg->buf;
msg->data = msg->buf + sizeof(rms_msg_hdr);
}
ret = recv(rmsb->ipc_sock,
&msg->data,
ipc_read,
0);
if(ret == ipc_read) {
msg->length = ipc_read;
msg->hdr->type = RMS_DATA;
msg->hdr->length = htons(msg->length);
msg->hdr->sequence = htonl(rmsb->self->sequence_sent);
msg->hdr->id = htonl(rmsb->self->id);
ret = sendto(rmsb->mcast_sock,
msg->buf,
ret + sizeof(rms_msg_hdr),
0,
(struct sockaddr*) &rmsb->mcast_addr,
sizeof(rmsb->mcast_addr));
if (ret > 0) {
htbl_put(rmsb->outmsgbuf, rmsb->self->sequence_sent, msg);
rmsb->self->sequence_sent++;
rmsb->msg_this_sec++;
err = 0;
}
}
}
if(err) {
release_meta_msg(msg);
}
}
}
//do periodic things
if(!gottime)
gettimeofday(&timeout, NULL);
gottime=0;
//heartbeat
if(timeout.tv_sec - rmsb->self->last_hbeat.tv_sec >= rmsb->hbeat_rate) {
//time for hbeat
send_hbeat(rmsb, NULL,0);
//check msg send rate
t1 = (float)timeout.tv_sec + ((float)timeout.tv_usec)/1000000;
t2 = (float)rmsb->self->last_hbeat.tv_sec
+ ((float)rmsb->self->last_hbeat.tv_usec)/1000000;
rmsb->msg_per_sec = rmsb->msg_this_sec/(t1-t2);
//calculate heartbeat rate
rmsb->hbeat_rate = RMS_RATE_FACTOR / rmsb->msg_per_sec;
//clamp
if(rmsb->hbeat_rate > 5)
rmsb->hbeat_rate = 5;
else if(rmsb->hbeat_rate < 1)
rmsb->hbeat_rate = 1;
rmsb->msg_this_sec = 0;
rmsb->self->last_hbeat.tv_sec = timeout.tv_sec;
rmsb->self->last_hbeat.tv_usec = timeout.tv_usec;
}
//if acks are used
if(rmsb->hb_ack) {
//clear buffers
count = mqueue_size(rmsb->mq_peers);
for(itr=0; itr < count; itr++) {
peer = (rms_meta_peer*) mqueue_peek_at_index(rmsb->mq_peers, itr);
if(peer->sequence_sync < rmsb->self->sequence_sync);
rmsb->self->sequence_sync = peer->sequence_sync;
}
while(rmsb->oldest_seq < rmsb->self->sequence_sync){
if(htbl_remove(rmsb->outmsgbuf, rmsb->oldest_seq, (void*)&msg)) {
release_meta_msg(msg);
}
rmsb->oldest_seq++;
}
}
}
//send leave message
msg = alloc_meta_msg();
msg->hdr->type = RMS_LEAVE;
msg->hdr->length = 0;
msg->hdr->sequence = htonl(rmsb->self->sequence_sent);
msg->hdr->id = htonl(rmsb->self->id);
ret = sendto(rmsb->mcast_sock,
msg->buf,
sizeof(rms_msg_hdr),
0,
(struct sockaddr*) &rmsb->mcast_addr,
sizeof(rmsb->mcast_addr));
release_meta_msg(msg);
return 0;
}
/*---------------------------------------------------------------------------*/
void
rudolph0_force_restart(struct rudolph0_conn *c)
{
c->current.h.chunk = 0;
send_nack(c);
}
/*---------------------------------------------------------------------*/
static
PT_THREAD(recv_udpthread(struct pt *pt))
{
int len;
struct codeprop_udphdr *uh = (struct codeprop_udphdr *)uip_appdata;
/* if(uip_newdata()) {
PRINTF(("recv_udpthread: id %d uh->id %d\n", s.id, htons(uh->id)));
}*/
PT_BEGIN(pt);
while(1) {
do {
PT_WAIT_UNTIL(pt, uip_newdata() &&
uh->type == HTONS(TYPE_DATA) &&
htons(uh->id) > s.id);
if(htons(uh->addr) != 0) {
s.addr = 0;
send_nack(uh, 0);
}
} while(htons(uh->addr) != 0);
/* leds_on(LEDS_YELLOW);
beep_down(10000);*/
s.addr = 0;
s.id = htons(uh->id);
s.len = htons(uh->len);
timer_set(&s.timer, CONNECTION_TIMEOUT);
/* process_post(PROCESS_BROADCAST, codeprop_event_quit, (process_data_t)NULL); */
while(s.addr < s.len) {
if(htons(uh->addr) == s.addr) {
/* leds_blink();*/
len = uip_datalen() - UDPHEADERSIZE;
if(len > 0) {
/* eeprom_write(EEPROMFS_ADDR_CODEPROP + s.addr,
&uh->data[0], len);*/
cfs_seek(fd, s.addr, CFS_SEEK_SET);
cfs_write(fd, &uh->data[0], len);
/* beep();*/
PRINTF(("Saved %d bytes at address %d, %d bytes left\n",
uip_datalen() - UDPHEADERSIZE, s.addr,
s.len - s.addr));
s.addr += len;
}
} else if(htons(uh->addr) > s.addr) {
PRINTF(("sending nack since 0x%x != 0x%x\n", htons(uh->addr), s.addr));
send_nack(uh, s.addr);
}
if(s.addr < s.len) {
/* timer_set(&s.nacktimer, NACK_TIMEOUT);*/
do {
timer_set(&s.nacktimer, HIT_NACK_TIMEOUT);
PT_YIELD_UNTIL(pt, timer_expired(&s.nacktimer) ||
(uip_newdata() &&
uh->type == HTONS(TYPE_DATA) &&
htons(uh->id) == s.id));
if(timer_expired(&s.nacktimer)) {
send_nack(uh, s.addr);
}
} while(timer_expired(&s.nacktimer));
}
}
/* leds_off(LEDS_YELLOW);
beep_quick(2);*/
/* printf("Received entire bunary over udr\n");*/
codeprop_start_program();
PT_EXIT(pt);
}
PT_END(pt);
}
static void clear_cmd(struct ast_private *ast)
{
send_nack(ast);
ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0x9a, 0x00, 0x00);
}
error_code_t ls_remote(receiver_param *rp, const char *command)
{
struct frame_t frame_info;
struct frame_t recv_frame_info;
char *recv_frame;
char *frame;
char buffer[32];
int done;
error_code_t e = OK;
int timeout_count = 0;
int sequence = 0;
if (command[2] != ' ' && command[2] != '\0')
return INV_CMD;
frame_info.size = strlen(&command[3]);
frame_info.sequence = 0;
frame_info.type = 'L';
frame_info.data = malloc(frame_info.size);
strcpy(frame_info.data, &command[3]);
frame = create_frame(&frame_info);
done = 0;
while (!done)
{
if (timeout_count == 0)
{
if (send(rp->source_sd, frame, FRAME_MAX_SIZE, 0) <= 0)
perror("WARNING");
}
pthread_mutex_lock(&receive_queue_mutex);
recv_frame = list_dequeue(rp->queue);
pthread_mutex_unlock(&receive_queue_mutex);
if (recv_frame)
{
if (decode_frame(recv_frame, &recv_frame_info))
{
if (sequence == recv_frame_info.sequence)
{
if (recv_frame_info.type == 'X' || recv_frame_info.type == 'Z')
{
send_ack(rp->source_sd, sequence);
sequence = (sequence+1) % (FRAME_SEQ_MAX+1);
memcpy(buffer, recv_frame_info.data, recv_frame_info.size);
buffer[recv_frame_info.size] = '\0';
printf("%s", buffer);
if (recv_frame_info.type == 'Z') done = 1;
}
else if (recv_frame_info.type == 'N')
timeout_count = 0;
}
else if (abs((frame_info.sequence-sequence) % (FRAME_SEQ_MAX+1)) < WINDOW_SIZE)
{
send_nack(rp->source_sd, frame_info.sequence);
}
else
{
send_ack(rp->source_sd, frame_info.sequence);
}
free(recv_frame_info.data);
}
else
{
send_nack(rp->source_sd, sequence);
}
free(recv_frame);
}
else
{
usleep(2);
timeout_count = (timeout_count + 1) % (TIMEOUT_LIMIT + 1);
}
}
free(frame);
free(frame_info.data);
return e;
}
/*---------------------------------------------------------------------------*/
static void
recv(struct polite_conn *polite)
{
struct rudolph2_conn *c = (struct rudolph2_conn *)polite;
struct rudolph2_hdr *hdr = packetbuf_dataptr();
/* Only accept NACKs from nodes that are farther away from the base
than us. */
if(hdr->type == TYPE_NACK && hdr->hops_from_base > c->hops_from_base) {
c->nacks++;
PRINTF("%d.%d: Got NACK for %d:%d (%d:%d)\n",
rimeaddr_node_addr.u8[RIMEADDR_SIZE-2], rimeaddr_node_addr.u8[RIMEADDR_SIZE-1],
hdr->version, hdr->chunk,
c->version, c->rcv_nxt);
if(hdr->version == c->version) {
if(hdr->chunk < c->rcv_nxt) {
c->snd_nxt = hdr->chunk;
send_data(c, SEND_INTERVAL);
}
} else if(LT(hdr->version, c->version)) {
c->snd_nxt = 0;
send_data(c, SEND_INTERVAL);
}
} else if(hdr->type == TYPE_DATA) {
if(hdr->hops_from_base < c->hops_from_base) {
/* Only accept data from nodes that are closer to the base than
us. */
c->hops_from_base = hdr->hops_from_base + 1;
if(LT(c->version, hdr->version)) {
PRINTF("%d.%d: rudolph2 new version %d, chunk %d\n",
rimeaddr_node_addr.u8[RIMEADDR_SIZE-2], rimeaddr_node_addr.u8[RIMEADDR_SIZE-1],
hdr->version, hdr->chunk);
c->version = hdr->version;
c->snd_nxt = c->rcv_nxt = 0;
c->flags &= ~FLAG_LAST_RECEIVED;
c->flags &= ~FLAG_LAST_SENT;
if(hdr->chunk != 0) {
send_nack(c);
} else {
packetbuf_hdrreduce(sizeof(struct rudolph2_hdr));
write_data(c, 0, packetbuf_dataptr(), packetbuf_totlen());
}
} else if(hdr->version == c->version) {
PRINTF("%d.%d: got chunk %d snd_nxt %d rcv_nxt %d\n",
rimeaddr_node_addr.u8[RIMEADDR_SIZE-2], rimeaddr_node_addr.u8[RIMEADDR_SIZE-1],
hdr->chunk, c->snd_nxt, c->rcv_nxt);
if(hdr->chunk == c->rcv_nxt) {
int len;
packetbuf_hdrreduce(sizeof(struct rudolph2_hdr));
PRINTF("%d.%d: received chunk %d len %d\n",
rimeaddr_node_addr.u8[RIMEADDR_SIZE-2], rimeaddr_node_addr.u8[RIMEADDR_SIZE-1],
hdr->chunk, packetbuf_totlen());
len = packetbuf_totlen();
write_data(c, hdr->chunk, packetbuf_dataptr(), packetbuf_totlen());
c->rcv_nxt++;
if(len < RUDOLPH2_DATASIZE) {
c->flags |= FLAG_LAST_RECEIVED;
send_data(c, RESEND_INTERVAL);
ctimer_set(&c->t, RESEND_INTERVAL, timed_send, c);
}
} else if(hdr->chunk > c->rcv_nxt) {
PRINTF("%d.%d: received chunk %d > %d, sending NACK\n",
rimeaddr_node_addr.u8[RIMEADDR_SIZE-2], rimeaddr_node_addr.u8[RIMEADDR_SIZE-1],
hdr->chunk, c->rcv_nxt);
send_nack(c);
} else if(hdr->chunk < c->rcv_nxt) {
/* Ignore packets with a lower chunk number */
}
}
}
}
}
void process_cmd(struct cmd_frame_t *cmd)
{
int res;
reply.status = ACK; // change to NACK as necessary
reply.cmd = cmd->cmd;
switch (cmd->cmd) {
case CMD_ECHO:
reply.data[0] = cmd->echo.length;
memcpy(&reply.data[1], cmd->echo.data, cmd->echo.length);
send_reply(&reply, cmd->echo.length+3);
break;
case CMD_RESET:
if (cmd->reset_magic == RESET_MAGIC) {
usb_peripheral_reset();
CREG_M4MEMMAP = 0x10400000;
scb_reset_system();
} else {
send_nack();
}
break;
case CMD_GET_EVENT_COUNTERS:
{
struct event_counters counters = get_last_event_counters();
memcpy(reply.data, &counters, sizeof(struct event_counters));
send_reply(&reply, 2+sizeof(struct event_counters));
break;
}
case CMD_GET_STAGE_GAINS:
memcpy(reply.data, stage_fb_gains, sizeof(stage_fb_gains));
send_reply(&reply, 2+sizeof(stage_fb_gains));
break;
case CMD_SET_STAGE_GAINS:
memcpy(stage_fb_gains, cmd->set_stage_gains, sizeof(stage_fb_gains));
send_ack();
break;
case CMD_GET_STAGE_SETPOINT:
memcpy(reply.data, stage_fb_setpoint, sizeof(stage_fb_setpoint));
send_reply(&reply, 2+sizeof(stage_fb_setpoint));
break;
case CMD_SET_STAGE_SETPOINT:
memcpy(stage_fb_setpoint, cmd->set_stage_setpoint, sizeof(stage_fb_setpoint));
send_ack();
break;
case CMD_GET_PSD_GAINS:
memcpy(reply.data, psd_fb_gains, sizeof(psd_fb_gains));
send_reply(&reply, 2+sizeof(psd_fb_gains));
break;
case CMD_SET_PSD_GAINS:
memcpy(psd_fb_gains, cmd->set_psd_gains, sizeof(psd_fb_gains));
send_ack();
break;
case CMD_GET_PSD_SETPOINT:
memcpy(reply.data, psd_fb_setpoint, sizeof(psd_fb_setpoint));
send_reply(&reply, 2+sizeof(psd_fb_setpoint));
break;
case CMD_SET_PSD_SETPOINT:
memcpy(psd_fb_setpoint, cmd->set_psd_setpoint, sizeof(psd_fb_setpoint));
send_ack();
break;
case CMD_GET_MAX_ERROR:
memcpy(reply.data, &max_error, sizeof(max_error));
send_reply(&reply, 2+sizeof(max_error));
break;
case CMD_SET_MAX_ERROR:
max_error = cmd->set_max_error;
send_ack();
break;
case CMD_GET_OUTPUT_GAINS:
{
fixed16_t* tmp = (fixed16_t*) reply.data;
for (unsigned int i=0; i<STAGE_OUTPUTS; i++) {
*tmp = stage_outputs[i].p_gain; tmp++;
*tmp = stage_outputs[i].i_gain; tmp++;
}
send_reply(&reply, 2+2*STAGE_OUTPUTS*sizeof(fixed16_t));
break;
}
case CMD_SET_OUTPUT_GAINS:
{
for (unsigned int i=0; i<STAGE_OUTPUTS; i++) {
stage_outputs[i].p_gain = cmd->set_output_gains[i][0];
stage_outputs[i].i_gain = cmd->set_output_gains[i][1];
}
send_ack();
break;
}
case CMD_GET_OUTPUT_TAUS:
{
for (unsigned int i=0; i<STAGE_OUTPUTS; i++) {
reply.data[i] = pi_get_tau(&stage_outputs[i]);
}
send_reply(&reply, 2+3);
break;
}
case CMD_SET_OUTPUT_TAUS:
{
for (unsigned int i=0; i<STAGE_OUTPUTS; i++) {
pi_set_tau(&stage_outputs[i], cmd->set_output_taus[i]);
}
send_ack();
break;
}
case CMD_GET_ADC_FREQ:
reply.data32[0] = 0; //TODO adc_get_trigger_freq();
send_reply(&reply, 2+4);
break;
case CMD_SET_ADC_FREQ:
adc_set_trigger_freq(cmd->set_adc_freq);
send_ack();
break;
case CMD_GET_ADC_TRIGGER_MODE:
reply.data[0] = adc_get_trigger_mode();
send_reply(&reply, 2+1);
break;
case CMD_SET_ADC_TRIGGER_MODE:
adc_set_trigger_mode(cmd->set_adc_trigger_mode);
send_ack();
break;
case CMD_START_ADC_STREAM:
adc_streaming = true;
send_ack();
break;
case CMD_STOP_ADC_STREAM:
adc_streaming = false;
adc_flush();
send_ack();
break;
case CMD_FLUSH_ADC_STREAM:
if (adc_flush() == 0) {
send_ack();
} else {
send_nack();
}
break;
case CMD_GET_ADC_DECIMATION:
{
reply.data32[0] = adc_get_decimation();
send_reply(&reply, 2+4);
break;
}
case CMD_SET_ADC_DECIMATION:
if (adc_set_decimation(cmd->set_adc_decimation) == 0) {
send_ack();
} else {
send_nack();
}
break;
case CMD_GET_FEEDBACK_FREQ:
{
unsigned int freq = feedback_get_loop_freq();
reply.data32[0] = freq;
send_reply(&reply, 2+4);
break;
}
case CMD_SET_FEEDBACK_FREQ:
feedback_set_loop_freq(cmd->set_feedback_freq);
send_ack();
break;
case CMD_GET_FEEDBACK_MODE:
reply.data[0] = feedback_get_mode();
send_reply(&reply, 3);
break;
case CMD_SET_FEEDBACK_MODE:
feedback_set_mode(cmd->set_feedback_mode);
send_ack();
break;
case CMD_SET_RAW_POS:
if (feedback_set_position(cmd->set_raw_pos) == 0) {
send_ack();
} else {
send_nack();
}
break;
case CMD_CLEAR_PATH:
clear_path();
send_ack();
break;
case CMD_ENQUEUE_POINTS:
res = 0;
if (cmd->enqueue_points.npts > 0)
res = enqueue_points((uint16_t*) &cmd->enqueue_points.points, cmd->enqueue_points.npts);
if (res == -1 || res == 0) {
reply.data[0] = res != -1; // were the points added?
reply.data[1] = is_path_running();
send_reply(&reply, 4);
} else {
send_nack();
}
break;
case CMD_START_PATH:
if (start_path(cmd->start_path.freq, cmd->start_path.synchronous_adc) == 0) {
send_ack();
} else {
send_nack();
}
break;
case CMD_SET_EXCITATION:
{
struct set_excitation* const se = &cmd->set_excitation;
struct excitation_buffer* const exc = &excitations[se->channel];
if ((se->length + se->offset > MAX_EXCITATION_LENGTH)
|| (se->channel >= 3))
{
send_nack();
break;
}
exc->length = 0;
exc->offset = 0;
if (se->length != 0) {
memcpy(&exc->samples[se->offset],
&se->samples,
sizeof(uint16_t) * se->length);
}
exc->length = se->total_length;
send_ack();
break;
}
case CMD_GET_SEARCH_STEP:
memcpy(reply.data, search_fb_step,
sizeof(search_fb_step));
send_reply(&reply, 2+sizeof(search_fb_step));
break;
case CMD_SET_SEARCH_STEP:
memcpy(search_fb_step, cmd->set_search_step,
sizeof(search_fb_step));
send_ack();
break;
case CMD_GET_SEARCH_OBJ_GAINS:
memcpy(reply.data, search_obj_gains,
sizeof(search_obj_gains));
send_reply(&reply, 2+sizeof(search_obj_gains));
break;
case CMD_SET_SEARCH_OBJ_GAINS:
memcpy(search_obj_gains, cmd->set_search_obj_gains,
sizeof(search_obj_gains));
send_ack();
break;
case CMD_GET_SEARCH_OBJ_THRESH:
memcpy(reply.data, &search_obj_thresh,
sizeof(search_obj_thresh));
send_reply(&reply, 2+sizeof(search_obj_thresh));
break;
case CMD_SET_SEARCH_OBJ_THRESH:
search_obj_thresh = cmd->set_search_obj_thresh,
send_ack();
break;
case CMD_GET_COARSE_FB_PARAMS:
memcpy(reply.data, &coarse_fb_channels,
sizeof(coarse_fb_channels));
send_reply(&reply, 2+sizeof(coarse_fb_channels));
break;
case CMD_SET_COARSE_FB_PARAMS:
memcpy(coarse_fb_channels, cmd->set_coarse_fb_params,
sizeof(coarse_fb_channels));
send_ack();
break;
default:
send_nack();
return;
}
}
/*---------------------------------------------------------------------------*/
static void
handle_data(struct rudolph1mh_conn *c, struct rudolph1mh_datapacket *p)
{
//Only called if partner is correct of NULL
PRINTF("Handle P: %d:%d - %d\n",
c->partner.u8[0], c->partner.u8[1], p->h.chunk);
if(p->h.chunk == 0) { //New stream
PRINTF("%d.%d: rudolph1mh new v_id %d, chunk %d\n",
rimeaddr_node_addr.u8[0], rimeaddr_node_addr.u8[1],
p->h.s_id, p->h.chunk);
c->s_id = p->h.s_id;
c->highest_chunk = c->chunk = 0;
if(p->h.chunk != 0) {
send_nack(c);
}
else {
if(!c->cb->write_chunk){
printf("No write-callback\n");
return;
}
c->cb->write_chunk(c, 0, RUDOLPH1MH_FLAG_NEWFILE, p->data, 0);
write_data(c, 0, p->data, p->datalen);
c->chunk = 1; /* Next chunk is 1. */
//ctimer_set(&c->t, send_interval * 30, send_next_packet, c);
}
/* }*/
}
else if(p->h.s_id == c->s_id) {
PRINTF("%d.%d: got chunk %d (%d) highest heard %d\n",
rimeaddr_node_addr.u8[0], rimeaddr_node_addr.u8[1],
p->h.chunk, c->chunk, c->highest_chunk);
if(p->h.chunk == c->chunk) {
PRINTF("%d.%d: received chunk %d\n",
rimeaddr_node_addr.u8[0], rimeaddr_node_addr.u8[1],
p->h.chunk);
write_data(c, p->h.chunk, p->data, p->datalen);
if(c->highest_chunk < c->chunk) {
c->highest_chunk = c->chunk;
}
c->chunk++;
}
else if(p->h.chunk > c->chunk) {
PRINTF("%d.%d: received chunk %d > %d, sending NACK\n",
rimeaddr_node_addr.u8[0], rimeaddr_node_addr.u8[1],
p->h.chunk, c->chunk);
send_nack(c);
c->highest_chunk = p->h.chunk;
}
else if(p->h.chunk < c->chunk) {
/* Ignore packets with a lower chunk number */
}
/* If we have heard a higher chunk number, we send a NACK so that
we get a repair for the next packet. */
if(c->highest_chunk > p->h.chunk) {
send_nack(c);
}
}
else { /* p->h.s_id < c->current.h.s_id */
PRINTF("%d.%d: Recieved s_id %i expected %i\n",
rimeaddr_node_addr.u8[0], rimeaddr_node_addr.u8[1],
p->h.s_id, c->s_id);
/* Ignore packets with old s_id */
}
}
