//IEEE 1588 PTP Protocol slave implementation
static void sync_clock(int times, int *sock, struct sockaddr_in *master) {
char useless_buffer[FIXED_BUFFER];
int i; //to prevent C99 error in for loop
printf("Running IEEE1588 PTP...");
fflush(stdout);
send_packet(sock, "ready", 5, NULL, master);
//run protocol num of times determined by master
for(i = 0; i < times; i++) {
sync_packet(sock, master);
delay_packet(sock, master);
receive_packet(sock, useless_buffer, sizeof(useless_buffer), NULL, NULL);
}
printf("Done!\n");
}
uint16_t net_send(uint8_t packet[], uint16_t length, uint16_t dst_addr) {
uint8_t* route;
if (state == STATE_TX) {
printf("net_send state error\n");
return 0;
}
if (length > MAX_DATA_LEN) {
printf("net_send length error\n");
return 0;
}
state = STATE_TX;
tx_pkt.dst_addr[0] = dst_addr>>8;
tx_pkt.dst_addr[1] = dst_addr&0xFF;
tx_pkt.src_addr[0] = node_addr>>8;
tx_pkt.src_addr[1] = node_addr&0xFF;
tx_pkt.id = my_packet_id++;
tx_pkt.data_len = length;
tx_pkt.route_len = 1;
memcpy(tx_pkt.data, packet, length);
route = tx_pkt.data + length;
route[0] = node_addr>>8;
route[1] = node_addr&0xFF;
tx_length = HEADER_LENGTH + length + 2;
// put packet to known packets
known_packets[known_packet_id].src_addr[0] = tx_pkt.src_addr[0];
known_packets[known_packet_id].src_addr[1] = tx_pkt.src_addr[1];
known_packets[known_packet_id].id = tx_pkt.id;
known_packet_id += 1;
known_packet_id %= MAX_KNOWN_PACKETS;
delay_packet();
return 1;
}
/* ----PRIVATE FUNCTIONS---- */
static uint16_t frame_received(uint8_t packet[], uint16_t length, uint16_t src_addr, int16_t rssi) {
packet_t *rx_pkt;
uint16_t dst;
uint16_t ret_val = 0;
// check min length
if (length < HEADER_LENGTH) {
return 0;
}
// cast the received packet
rx_pkt = (packet_t*) packet;
// ckeck the length
if ( (HEADER_LENGTH + (rx_pkt->data_len) + (rx_pkt->route_len*2)) != length ) {
return 0;
}
// if packet known, abort
if (is_packet_known(rx_pkt)) {
return 0;
}
// put packet to known packets
known_packets[known_packet_id].src_addr[0] = rx_pkt->src_addr[0];
known_packets[known_packet_id].src_addr[1] = rx_pkt->src_addr[1];
known_packets[known_packet_id].id = rx_pkt->id;
known_packet_id += 1;
known_packet_id %= MAX_KNOWN_PACKETS;
// check the destination, if for me call the callback
dst = (rx_pkt->dst_addr[0]<<8) + (rx_pkt->dst_addr[1]);
if ( (dst == node_addr || dst == MAC_BROADCAST) && rx_cb) {
uint16_t src;
src = (rx_pkt->src_addr[0]<<8) + (rx_pkt->src_addr[1]);
ret_val = rx_cb(rx_pkt->data, rx_pkt->data_len, src);
int i;
uint8_t *route;
uint16_t hop;
route = rx_pkt->data + rx_pkt->data_len;
printf("route = ");
for (i=0; i<rx_pkt->route_len; i++) {
hop = (route[2*i]<<8) + (route[2*i+1]);
printf("%.4x-", hop);
}
printf("%.4x\n", node_addr);
}
// check if it needs to be forwarded
if ( dst == MAC_BROADCAST || dst != node_addr ) {
uint8_t *route;
// check if state is correct
if (state == STATE_TX) {
// already a packet being sent, abort
return ret_val;
}
// check if there is room for one more hop
if (rx_pkt->route_len >= MAX_ROUTE_LEN) {
// too big, drop
printf("Too many hops!\n");
return ret_val;
}
// insert my address
route = rx_pkt->data + rx_pkt->data_len;
route[rx_pkt->route_len*2] = node_addr>>8;
route[rx_pkt->route_len*2+1] = node_addr & 0xFF;
rx_pkt->route_len+=1;
length +=2;
// copy packet to send in a local buffer
memcpy(&tx_pkt, rx_pkt, length);
tx_length = length;
// start delay to send
delay_packet();
}
