static EVENT_HANDLER(wait)
{
int i;
printf("Timeout. Resending\n");
for(i = (last_ack_recv + 1) % (MAXSEQ + 1); i != next_frame; i = (i + 1) % (MAXSEQ + 1))
frame_send(&(t_window[i].message), t_window[i].length, i);
frame_send(&(t_window[next_frame].message), t_window[next_frame].length, i);
}
static EVENT_HANDLER(ready_app)
{
CnetAddr dest;
int num_unack = next_frame - last_ack_recv;
if(num_unack==(MAXSEQ+1)/2){
timer_wait = CNET_start_timer(EV_TIMER1, TIMEOUT, 0);
CNET_disable_application(ALLNODES);
return;
}
if(num_unack<0){
num_unack=MAXSEQ;
}
// if(trans_frame_state[next_frame%((MAXSEQ+1)/2)] == ACK_WAIT)
// {
// timer_wait = CNET_start_timer(EV_TIMER1, TIMEOUT, 0);
// CNET_disable_application(ALLNODES);
// return;
// }
trans_frame_state[next_frame] = ACK_WAIT;
size_t length = MAX_MESSAGE_SIZE;
CHECK(CNET_read_application(&dest, (char*) last_msg, &length));
frame_send(last_msg, length, next_frame);
next_frame = (next_frame + 1) % (MAXSEQ + 1);
}
HJ_CASE(B, PID_REQ) {
struct hj_pkt_pid_k k = HJ_PKT_PID_K_INITIALIZER;
pid_k_pack(&k, 0);
pid_k_pack(&k, 1);
frame_send(&k, HJ_PL_PID_K);
break;
}
void *router(void *v) {
wrk_info_t wrk_info = *(wrk_info_t*)v;
int result, rcvid;
iov_t *piov, header;
frame_t frame;
size_t i;
SETIOV(&header, &frame, sizeof(frame_t));
while(true) {
rcvid = MsgReceivev(wrk_info.chid, &header, 1, NULL);
printf("rcvid %i\n", rcvid);
__ERROR_CONT(rcvid, -1, MSG_ERR_MSGREC);
if(0 == rcvid) {
switch(frame.header.code) {
case _PULSE_CODE_DISCONNECT:
printf("Client has gone\n");
break;
default:
break;
}
} else {
if (frame.header.type == _IO_CONNECT) {
printf("Send OK\n");
frame_reply(rcvid, NULL);
continue;
}
if(frame.protocol == STANDART) {
__ERROR_CHECK(MsgError(rcvid, ENOTSUP),-1,MSG_ERR_MSGERR);
continue;
}
printf("Thread %i, client %i\n", wrk_info.id, frame.cid);
if(frame.protocol == NOREPLY) {
for(i=0; i<wrk_info.pslave->amount; ++i) {
if(FULL != wrk_info.pslave->pslave[i].status) {
if(0 == sem_trywait(&wrk_info.pslave->pslave[i].sem)) {
wrk_info.pslave->pslave[i].clientnow ++;
if(wrk_info.pslave->pslave[i].clientnow ==
wrk_info.pslave->pslave[i].clientmax) {
wrk_info.pslave->pslave[i].status = FULL;
}
sem_post(&wrk_info.pslave->pslave[i].sem);
break;
}
}
}
if(i == wrk_info.pslave->amount) {
__ERROR_CHECK(MsgError(rcvid, EAGAIN),-1,MSG_ERR_MSGERR);
continue;
}
frame_datareceive(rcvid, &frame);
frame_reply(rcvid, NULL);
__ERROR_CHECK(frame_send(wrk_info.pslave->pslave[i].coid, &frame, NULL), -1, "sendv");
}
}
}
}
/* return true = failure */
static bool hj_parse(uint8_t *buf, uint8_t len)
{
if (len < HJ_PL_MIN) {
hj_send_error(1);
return true;
}
struct hj_pkt_header *head = (typeof(head)) buf;
switch(head->type) {
HJ_CASE(B, SET_SPEED) {
struct hjb_pkt_set_speed *pkt = (typeof(pkt)) buf;
update_vel(pkt);
break;
}
HJ_CASE(B, REQ_INFO) {
uint16_t vals[ADC_CHANNEL_CT];
adc_val_cpy(vals);
/* send info */
struct hja_pkt_info info = HJA_PKT_INFO_INITIALIZER;
motor_info_get(&info.m[0], vals[0], 0);
motor_info_get(&info.m[1], vals[1], 1);
frame_send(&info, HJA_PL_INFO);
break;
}
#ifdef MCTRL_PID
HJ_CASE( , PID_K) {
struct hj_pkt_pid_k *k = (typeof(k)) buf;
pid_k_update(k);
break;
}
HJ_CASE(B, PID_SAVE) {
pid_k_store_all();
break;
}
static EVENT_HANDLER(ready_physical)
{
if(nodetype != RECEIVER)
{
ACK_FRAME f;
int link;
size_t length;
length = sizeof(ACK_FRAME);
CHECK(CNET_read_physical(&link, (char*) &f, &length));
uint16_t check = f.check;
f.check = 0;
if(CNET_ccitt((unsigned char*) &f, sizeof(ACK_FRAME)) != check)
return;
if(f.sequence <= 0)
{
// SREJ FRAME
printf("\t\t\t\tSREJ received, sequence=%d\n", -f.sequence);
f.sequence = -f.sequence;
CNET_stop_timer(timer);
trans_frame_state[f.sequence] = ACK_WAIT;
frame_send(&(t_window[f.sequence].message), t_window[f.sequence].length, f.sequence);
}
else
{
// RECVREADY FRAME
printf("\t\t\t\tACK received, sequence=%d\n", f.sequence);
int i;
for(i = (last_ack_recv + 1)%(MAXSEQ + 1); i != f.sequence - 1; i = (i + 1) % (MAXSEQ + 1))
trans_frame_state[i] = ACK_RECEIVED;
last_ack_recv = f.sequence - 1;
trans_frame_state[f.sequence-1] = ACK_RECEIVED;
CNET_stop_timer(timer_wait);
if(packets_sent >= 10000)
{
if(time_end == 0)
time_end = nodeinfo.time_of_day.sec;
}
else
{
packets_sent++;
CNET_enable_application(ALLNODES);
}
}
}
else
{
FRAME_DATA f;
size_t length;
int link;
int checksum;
length = sizeof(FRAME_DATA);
CHECK(CNET_read_physical(&link, (char*) &f, &length));
checksum = f.checksum;
f.checksum = 0;
if(CNET_ccitt((unsigned char*) &f, CHECK_BYTES) != checksum)
{
// bad checksum, ignore frame
printf("\t\t\t\tBAD checksum - frame ignored\n");
ack_send(last_ack_sent + 1, SREJ);
printf("Requesting to send frame %d\n",last_ack_sent+1);
return;
}
recv_frame_status[f.sequence] = RECEIVED;
printf("\t\t\t\tDATA received, sequence=%d\n", f.sequence);
int i;
for(i = (last_ack_sent + 1) % (MAXSEQ + 1); i != f.sequence; i = (i + 1) % (MAXSEQ + 1))
{
if(recv_frame_status[i] != RECEIVED)
break;
}
if(i == f.sequence)
ack_send(f.sequence + 1, RECVREADY);
}
}
/**
* \brief Slave driver main state machine
* For UML graph, please refer to SDK documentation
*/
static void spi_slave_event_handle(spi_slave_evt_t event)
{
static uint32_t err_code = NRF_SUCCESS;
static uint16_t packetLength;
switch (m_trans_state)
{
case SPI_RAW_STATE_SETUP_HEADER:
m_trans_state = SPI_RAW_STATE_RX_HEADER;
err_code = header_get();
break;
case SPI_RAW_STATE_RX_HEADER:
if (event.evt_type == SPI_SLAVE_BUFFERS_SET_DONE)
{
DEBUG_EVT_SPI_SLAVE_RAW_BUFFERS_SET(0);
set_ready_line();
}
if (event.evt_type == SPI_SLAVE_XFER_DONE)
{
DEBUG_EVT_SPI_SLAVE_RAW_RX_XFER_DONE(event.rx_amount);
spi_slave_raw_assert(event.rx_amount == SER_PHY_HEADER_SIZE);
packetLength = uint16_decode(m_header_rx_buffer);
if (packetLength != 0 )
{
m_trans_state = SPI_RAW_STATE_MEM_REQUESTED;
m_buffer_reqested_flag = true;
m_rx_packet_length = packetLength;
callback_memory_request(packetLength);
}
else
{
if (m_p_tx_buffer)
{
clear_request_line();
m_trans_state = SPI_RAW_STATE_TX_HEADER;
err_code = header_send(m_tx_packet_length);
}
else
{
//there is nothing to send - zero response facilitates pooling - but perhaps, it should be assert
err_code = header_send(0);
}
}
}
break;
case SPI_RAW_STATE_MEM_REQUESTED:
if (event.evt_type == SPI_SLAVE_EVT_TYPE_MAX) //This is API dummy event
{
m_buffer_reqested_flag = false;
m_trans_state = SPI_RAW_STATE_RX_PAYLOAD;
m_accumulated_rx_packet_length = 0;
err_code = frame_get();
}
break;
case SPI_RAW_STATE_RX_PAYLOAD:
if (event.evt_type == SPI_SLAVE_BUFFERS_SET_DONE)
{
DEBUG_EVT_SPI_SLAVE_RAW_BUFFERS_SET(0);
set_ready_line();
}
if (event.evt_type == SPI_SLAVE_XFER_DONE)
{
DEBUG_EVT_SPI_SLAVE_RAW_RX_XFER_DONE(event.rx_amount);
spi_slave_raw_assert(event.rx_amount == m_current_rx_frame_length);
m_accumulated_rx_packet_length += m_current_rx_frame_length;
if (m_accumulated_rx_packet_length < m_rx_packet_length )
{
err_code = frame_get();
}
else
{
spi_slave_raw_assert(m_accumulated_rx_packet_length == m_rx_packet_length);
m_trans_state = SPI_RAW_STATE_RX_HEADER;
err_code = header_get();
if (!m_trash_payload_flag)
{
callback_packet_received(m_p_rx_buffer, m_accumulated_rx_packet_length);
}
else
{
callback_packet_dropped();
}
}
}
break;
case SPI_RAW_STATE_TX_HEADER:
if (event.evt_type == SPI_SLAVE_BUFFERS_SET_DONE)
{
DEBUG_EVT_SPI_SLAVE_RAW_BUFFERS_SET(0);
set_ready_line();
}
if (event.evt_type == SPI_SLAVE_XFER_DONE)
{
DEBUG_EVT_SPI_SLAVE_RAW_TX_XFER_DONE(event.tx_amount);
spi_slave_raw_assert(event.tx_amount == SER_PHY_HEADER_SIZE + 1);
m_trans_state = SPI_RAW_STATE_TX_PAYLOAD;
m_accumulated_tx_packet_length = 0;
err_code = frame_send();
}
break;
case SPI_RAW_STATE_TX_PAYLOAD:
if (event.evt_type == SPI_SLAVE_BUFFERS_SET_DONE)
{
DEBUG_EVT_SPI_SLAVE_RAW_BUFFERS_SET(0);
set_ready_line();
}
if (event.evt_type == SPI_SLAVE_XFER_DONE)
{
DEBUG_EVT_SPI_SLAVE_RAW_TX_XFER_DONE(event.tx_amount);
spi_slave_raw_assert(event.tx_amount == m_current_tx_frame_length + 1);
m_accumulated_tx_packet_length += m_current_tx_frame_length;
if ( m_accumulated_tx_packet_length < m_tx_packet_length )
{
err_code = frame_send();
}
else
{
spi_slave_raw_assert(m_accumulated_tx_packet_length == m_tx_packet_length);
//clear pointer before callback
m_p_tx_buffer = NULL;
callback_packet_transmitted();
//spi slave TX transfer is possible only when RX is ready, so return to waiting for a header
m_trans_state = SPI_RAW_STATE_RX_HEADER;
err_code = header_get();
}
}
break;
default:
err_code = NRF_ERROR_INVALID_STATE;
break;
}
APP_ERROR_CHECK(err_code);
}
void send_packet(char *packet) {
// packet must be at least PACKET_LEN bytes (or must it?)
frame_init();
frame_send((int8_t*)packet);
frame_end();
}
/**
* \brief Master driver main state machine
* Executed only in the context of PendSV_Handler()
* For UML graph, please refer to SDK documentation
*/
static void ser_phy_switch_state(ser_phy_event_source_t evt_src)
{
uint32_t err_code = NRF_SUCCESS;
static bool m_wait_for_ready_flag = false; //local scheduling flag to defer RDY events
switch (m_spi_master_state)
{
case SER_PHY_STATE_IDLE:
if (evt_src == SER_PHY_EVT_GPIO_REQ)
{
m_wait_for_ready_flag = false;
if (m_slave_ready_flag)
{
m_spi_master_state = SER_PHY_STATE_TX_ZERO_HEADER;
err_code = header_send(0);
}
else
{
m_spi_master_state = SER_PHY_STATE_RX_WAIT_FOR_RDY;
}
}
else if (evt_src == SER_PHY_EVT_TX_API_CALL)
{
spi_master_raw_assert(mp_tx_buffer != NULL); //api event with tx_buffer == NULL has no sense
m_wait_for_ready_flag = false;
if (m_slave_ready_flag)
{
m_spi_master_state = SER_PHY_STATE_TX_HEADER;
err_code = header_send(m_tx_buf_len);
}
else
{
m_spi_master_state = SER_PHY_STATE_TX_WAIT_FOR_RDY;
}
}
break;
case SER_PHY_STATE_TX_WAIT_FOR_RDY:
if (evt_src == SER_PHY_EVT_GPIO_RDY)
{
m_spi_master_state = SER_PHY_STATE_TX_HEADER;
err_code = header_send(m_tx_buf_len);
}
break;
case SER_PHY_STATE_RX_WAIT_FOR_RDY:
if (evt_src == SER_PHY_EVT_GPIO_RDY)
{
m_spi_master_state = SER_PHY_STATE_TX_ZERO_HEADER;
err_code = header_send(0);
}
break;
case SER_PHY_STATE_TX_HEADER:
if (evt_src == SER_PHY_EVT_SPI_TRANSFER_DONE)
{
m_tx_packet_length = m_tx_buf_len;
m_accumulated_tx_packet_length = 0;
if (m_slave_ready_flag)
{
m_spi_master_state = SER_PHY_STATE_TX_PAYLOAD;
err_code = frame_send();
}
else
{
m_wait_for_ready_flag = true;
}
}
else if ((evt_src == SER_PHY_EVT_GPIO_RDY) && m_wait_for_ready_flag)
{
m_wait_for_ready_flag = false;
m_spi_master_state = SER_PHY_STATE_TX_PAYLOAD;
err_code = frame_send();
}
break;
case SER_PHY_STATE_TX_PAYLOAD:
if (evt_src == SER_PHY_EVT_SPI_TRANSFER_DONE)
{
if (m_accumulated_tx_packet_length < m_tx_packet_length)
{
if (m_slave_ready_flag)
{
err_code = frame_send();
}
else
{
m_wait_for_ready_flag = true;
}
}
else
{
spi_master_raw_assert(m_accumulated_tx_packet_length == m_tx_packet_length);
buffer_release(&mp_tx_buffer, &m_tx_buf_len);
callback_packet_sent();
if ( m_slave_request_flag)
{
if (m_slave_ready_flag)
{
m_spi_master_state = SER_PHY_STATE_TX_ZERO_HEADER;
err_code = header_send(0);
}
else
{
m_spi_master_state = SER_PHY_STATE_RX_WAIT_FOR_RDY;
}
}
else
{
m_spi_master_state = SER_PHY_STATE_IDLE; //m_Tx_buffer is NULL - have to wait for API event
}
}
}
else if ((evt_src == SER_PHY_EVT_GPIO_RDY) && m_wait_for_ready_flag )
{
m_wait_for_ready_flag = false;
err_code = frame_send();
}
break;
case SER_PHY_STATE_TX_ZERO_HEADER:
if (evt_src == SER_PHY_EVT_SPI_TRANSFER_DONE)
{
if (m_slave_ready_flag)
{
m_spi_master_state = SER_PHY_STATE_RX_HEADER;
err_code = header_get();
}
else
{
m_wait_for_ready_flag = true;
}
}
else if ( (evt_src == SER_PHY_EVT_GPIO_RDY) && m_wait_for_ready_flag)
{
m_wait_for_ready_flag = false;
m_spi_master_state = SER_PHY_STATE_RX_HEADER;
err_code = header_get();
}
break;
case SER_PHY_STATE_RX_HEADER:
if (evt_src == SER_PHY_EVT_SPI_TRANSFER_DONE)
{
m_spi_master_state = SER_PHY_STATE_MEMORY_REQUEST;
m_rx_buf_len = uint16_decode(m_header_buffer);
m_rx_packet_length = m_rx_buf_len;
callback_mem_request();
}
break;
case SER_PHY_STATE_MEMORY_REQUEST:
if (evt_src == SER_PHY_EVT_RX_API_CALL)
{
m_accumulated_rx_packet_length = 0;
if (m_slave_ready_flag)
{
m_spi_master_state = SER_PHY_STATE_RX_PAYLOAD;
err_code = frame_get();
}
else
{
m_wait_for_ready_flag = true;
}
}
else if ((evt_src == SER_PHY_EVT_GPIO_RDY) && m_wait_for_ready_flag)
{
m_wait_for_ready_flag = false;
m_spi_master_state = SER_PHY_STATE_RX_PAYLOAD;
err_code = frame_get();
}
break;
case SER_PHY_STATE_RX_PAYLOAD:
if (evt_src == SER_PHY_EVT_SPI_TRANSFER_DONE)
{
m_accumulated_rx_packet_length += m_current_rx_packet_length;
if (m_accumulated_rx_packet_length < m_rx_packet_length)
{
if (m_slave_ready_flag)
{
err_code = frame_get();
}
else
{
m_wait_for_ready_flag = true;
}
}
else
{
spi_master_raw_assert(m_accumulated_rx_packet_length == m_rx_packet_length);
if (mp_rx_buffer == NULL)
{
callback_packet_dropped();
}
else
{
callback_packet_received();
}
buffer_release(&mp_rx_buffer, &m_rx_buf_len);
if (mp_tx_buffer != NULL) //mp_tx_buffer !=NULL, this means that API_EVT was scheduled
{
if (m_slave_ready_flag )
{
err_code = header_send(m_tx_buf_len);
m_spi_master_state = SER_PHY_STATE_TX_HEADER;
}
else
{
m_spi_master_state = SER_PHY_STATE_TX_WAIT_FOR_RDY;
}
}
else if (m_slave_request_flag)
{
if (m_slave_ready_flag)
{
m_spi_master_state = SER_PHY_STATE_TX_ZERO_HEADER;
err_code = header_send(0);
}
else
{
m_spi_master_state = SER_PHY_STATE_RX_WAIT_FOR_RDY;
}
}
else
{
m_spi_master_state = SER_PHY_STATE_IDLE;
}
}
}
else if ( evt_src == SER_PHY_EVT_GPIO_RDY && m_wait_for_ready_flag)
{
m_wait_for_ready_flag = false;
err_code = frame_get();
}
break;
default:
break;
}
if (err_code != NRF_SUCCESS)
{
(void)err_code;
}
}
int main(int argc, char **argv) {
if(argc != 2) {
printf("Set client id\n");
return EXIT_SUCCESS;
}
uint32_t cid = atoi(argv[1]);//!!
struct timespec timeout, stime, etime;
frame_t frame, rframe;
char buf[BUFFER_SIZE], cmd[BUFFER_SIZE], param[BUFFER_SIZE];
int itr = -1, n, k;
FILE *pfile = 0;
uint32_t itr_max = ITR;
double_t *pfcn, *psig, init;
uint64_t timeoutsend = 200000000;
//*****************************************************************************
timeout.tv_nsec = 0;
timeout.tv_sec = 1;
strcpy(buf, CFGFILENAME);
strcat(buf, argv[1]);
pfile = fopen(buf, "r");
if(NULL != pfile) {
while(0 == feof(pfile)) {
fscanf(pfile, "%s %s", cmd, param);
if(0 == strcmp(cmd, "itr_max")) {
itr_max = atoi(param);
}
else if(0 == strcmp(cmd, "timeoutsend")) {
timeoutsend = atol(param);
}
else if(0 == strcmp(cmd, "timeout_sec")) {
timeout.tv_sec = atol(param);
}
else if(0 == strcmp(cmd, "timeout_nsec")) {
timeout.tv_nsec = atol(param);
}
}
fclose(pfile);
} else {
perror(MSG_ERR_CFGFILE);
}
strcpy(buf, OUTFILENAME);
strcat(buf, argv[1]);
pfile = fopen(buf, "w");
fprintf(pfile, "itr_max %i\ntimeoutsend %llu\ntimeout %lu %lu\n",
itr_max, timeoutsend,
timeout.tv_sec, timeout.tv_nsec);
pfcn = malloc(sizeof(double_t)*(SIZE*2));
psig = malloc(sizeof(double_t)*((SIZE-1)*2));
srand(time(NULL));
for(size_t i = 0; i<SIZE; i++) {
pfcn[i] = i*STEP;
pfcn[i+SIZE] = sin(pfcn[i])+2;
}
for(size_t i = 0; i<SIZE-1; i++) {
psig[i] = SHIFT + (rand()%SIZE)*STEP;
psig[i+SIZE-1] = sin(psig[i])+2;
}
frame.cid = cid;
frame.fid = 0;
frame.size = 2;
//*****************************************************************************
int srv_coid = name_open(SRV_NAME, NAME_FLAG_ATTACH_GLOBAL);
__ERROR_EXIT(srv_coid, -1, SRV_NAME);
frame.ptask = malloc(sizeof(task_t)*2);
frame.ptask[0].cmd = SPLINE_INIT;
frame.ptask[0].n = SIZE*2*sizeof(double_t);
frame.ptask[0].px = (void*)pfcn;
frame.timeout = timeout;
frame.ptask[1].cmd = SPLINE_GETVAL;
frame.ptask[1].n = 2*sizeof(double_t);
frame.ptask[1].px = (void*)psig;
frame_repinit(&frame, &rframe);
__ERROR_CHECK(TimerTimeout(CLOCK_REALTIME, _NTO_TIMEOUT_SEND | _NTO_TIMEOUT_REPLY,
NULL, &timeoutsend, NULL), -1, "TimerTimeout");
// send init data about spline
__ERROR_CHECK(frame_send(srv_coid, &frame, &rframe), -1, "sendv");
frame_destroy(&rframe);
free(frame.ptask);
frame.ptask = malloc(sizeof(task_t));
frame.ptask[0].cmd = SPLINE_GETVAL;
frame.ptask[0].n = sizeof(double_t);
frame.size = 1;
frame_repinit(&frame, &rframe);
k=0;
for(uint32_t i=0; i<itr_max; ++i) {
frame.fid = i;
n = rand()%(SIZE-1);
frame.ptask[0].px = (void*)&psig[n];
trace_msg_start();
// send tasks to server and wait for results
__ERROR_CONT(frame_send(srv_coid, &frame, &rframe), -1, "sendv");
trace_msg_stop();
clock_gettime(CLOCK_MONOTONIC, &etime);
fprintf(pfile, "%f %f %u %u\n", psig[n], *(double_t*)rframe.ptask->px,
etime.tv_sec-stime.tv_sec, etime.tv_nsec-stime.tv_nsec);
printf("I receive data\n");
}
frame.ptask[0].cmd = SPLINE_DESTROY;
frame.ptask[0].n = 0;
frame.ptask[0].px = 0;
frame.fid++;
// say server to destroy our spline
__ERROR_CHECK(frame_send(srv_coid, &frame, &rframe), -1, "sendv");
fclose(pfile);
frame_destroy(&rframe);
free(pfcn);
free(psig);
__ERROR_CHECK(name_close(srv_coid), -1, "name_close");
return EXIT_SUCCESS;
}
