static char* test_spi_stream_read_rx_packet(void) {
CircularBuffer* mybuf = cbuffer_new();
uint32_t my_pkt_expected[10] = {0xBEEF, 5, 1, 2, 3, 4, 5, 0xDEADBEEF, 0xDEADBEEF, -1};
uint32_t my_pkt_expected_2[10] = {0xBEEF, 7, 1, 2, 3, 4, 5, 1, 2, -1};
add_checksum(my_pkt_expected, 10);
add_checksum(my_pkt_expected_2, 10);
uint32_t my_data[7] = {1, 2, 3, 4, 5, 1, 2};
// read with non-full buffer
int ret = spi_stream_read_rx_packet(my_pkt_expected, mybuf);
mu_assert_eq("okay1", ret, 1);
mu_assert_eq("data1", memcmp(my_data, mybuf->data, 5 * sizeof(uint32_t)), 0);
mybuf->tail = 0;
// read with full buffer
ret = spi_stream_read_rx_packet(my_pkt_expected_2, mybuf);
mu_assert_eq("okay2", ret, 1);
mu_assert_eq("data2", memcmp(my_data, mybuf->data, 7 * sizeof(uint32_t)), 0);
// overflowing local buffer
mybuf->tail = IO_BUFFER_SIZE - 5;
mu_assert_eq("its too small", cbuffer_freespace(mybuf), 4);
ret = spi_stream_read_rx_packet(my_pkt_expected_2, mybuf);
mu_assert_eq("overflow err", ret, 0);
mu_assert_eq("unmodified", cbuffer_freespace(mybuf), 4);
return 0;
}
static char* test_spi_stream_construct_empty_tx_packet(void) {
CircularBuffer* mybuf = cbuffer_new();
uint32_t my_pkt_expected[10] = {0xBEEF, 0,
0xDEADBEEF,
0xDEADBEEF,
0xDEADBEEF,
0xDEADBEEF,
0xDEADBEEF,
0xDEADBEEF,
0xDEADBEEF,
-1};
add_checksum(my_pkt_expected, 10);
int checksum_err = -1;
spi_stream_verify_packet(my_pkt_expected, 10, &checksum_err);
mu_assert_eq("no cksum err", checksum_err, 0);
uint32_t my_pkt[10];
spi_stream_construct_tx_packet(0xBEEF, my_pkt, 10, mybuf);
// for (int i = 0; i < 10; ++i) {
// printf("%i %lx %lx\n", i, my_pkt_expected[i], my_pkt[i]);
// }
mu_assert_eq("packet", memcmp(my_pkt_expected, my_pkt, 10 * sizeof(uint32_t)), 0);
spi_stream_verify_packet(my_pkt, 10, &checksum_err);
mu_assert_eq("no cksum actual", checksum_err, 0);
return 0;
}
//===========================================
void set_motors_pid(uchar p, uchar i, uchar d)
{
buff_command[0]=3;
buff_command[1]=p;
buff_command[2]=i;
buff_command[3]=d;
buff_command[4]=add_checksum(buff_command, COMMAND_LEN);
sendto(socket_command, buff_command, COMMAND_LEN, 0, (struct sockaddr *) &myaddr_command, sizeof(myaddr_command));
}
int rudp_recv(int sock, char *receive_buf, struct sockaddr_in *self_addr,\
struct sockaddr *src_addr, unsigned *src_addr_len, size_t *recv_size)
{
size_t size;
char *p=receive_buf;
char buffer[PACKET_SIZE];
header_t head;
enum recPkt sw_return;
do {
size = recvfrom(sock, (void *)buffer, PACKET_SIZE, 0, src_addr, src_addr_len);
assert (size==PACKET_SIZE);
assert (add_checksum(size, (u_char *)&(((struct sockaddr_in *)src_addr)->sin_addr.s_addr),
(u_char *)&(self_addr->sin_addr.s_addr), size%2, (u_short *)buffer) == 0);
printf("checksum correct\n");
read_header(&head,(packet_t *)buffer);
assert (head.offset==PAYLOAD_SIZE || head.flag==FIN);
printf("-----------recevie packet %d\n",head.seq);
if (packet_lost(drop_p) && head.flag!=FIN) {
// next_byte_expected--;
//p+=head.offset;
printf("packet %u dropped\n",head.seq);
continue;
}
sw_return = receiver_receive_packet(head.seq);
if (sw_return == dropPkt) {
if(head.seq>next_byte_expected)
{
send_ack(next_byte_expected-1);
printf("seq wrong. drop packet %d. send ack for %d\n",head.seq,next_byte_expected);
}
continue;
}
//printf("Seq %d Ack %d Offset %d, Flag %d \n",head.seq,head.ack,head.offset,head.flag);
*recv_size += head.offset;
read_packet((u_char *)p,(packet_t*)buffer,(u_short)head.offset);
p+=head.offset;
send_ack(head.seq);
if (head.flag==FIN) {
if (delay_p != 0) {
while (DELAY_HEAD->next!=NULL) {
traverse_dh();
}
}
return (int)head.flag;
}
} while (*recv_size < BUFFERSIZE);
return (int)head.flag;
}
//===================================================================
void set_motors_speed_dir(uchar lspeed, uchar rspeed, uchar direction)
{
#ifdef debug_printf
printf("SET: speed %d %d dir %d\n",lspeed, rspeed, direction);
#endif
buff_command[0]=2;
buff_command[1]=lspeed;
buff_command[2]=rspeed;
buff_command[3]=direction;
buff_command[4]=add_checksum(buff_command, COMMAND_LEN);
sendto(socket_command, buff_command, COMMAND_LEN, 0, (struct sockaddr *) &myaddr_command, sizeof(myaddr_command));
}
static char* test_spi_stream_construct_tx_packet(void) {
uint32_t my_data[5] = {1, 2, 3, 4, 5};
CircularBuffer* mybuf = cbuffer_new();
cbuffer_append(mybuf, my_data, 5);
uint32_t my_pkt_expected[10] = {0xBEEF, 5, 1, 2, 3, 4, 5, 0xDEADBEEF, 0xDEADBEEF, -1};
add_checksum(my_pkt_expected, 10);
int checksum_err = -1;
spi_stream_verify_packet(my_pkt_expected, 10, &checksum_err);
mu_assert_eq("no cksum err", checksum_err, 0);
uint32_t my_pkt[10];
spi_stream_construct_tx_packet(0xBEEF, my_pkt, 10, mybuf);
// for (int i = 0; i < 10; ++i) {
// printf("%i %lx %lx\n", i, my_pkt_expected[i], my_pkt[i]);
// }
mu_assert_eq("packet", memcmp(my_pkt_expected, my_pkt, 10 * sizeof(uint32_t)), 0);
mu_assert_eq("consumed", cbuffer_size(mybuf), 0);
// put more in the buffer than we can consume at once
cbuffer_append(mybuf, my_data, 5);
cbuffer_append(mybuf, my_data, 5);
cbuffer_append(mybuf, my_data, 5);
uint32_t my_pkt_expected_2[10] = {0xBEEF, 7, 1, 2, 3, 4, 5, 1, 2, -1};
add_checksum(my_pkt_expected_2, 10);
spi_stream_verify_packet(my_pkt_expected_2, 10, &checksum_err);
mu_assert_eq("no cksum err 2", checksum_err, 0);
spi_stream_construct_tx_packet(0xBEEF, my_pkt, 10, mybuf);
mu_assert_eq("packet", memcmp(my_pkt_expected_2, my_pkt, 10 * sizeof(uint32_t)), 0);
mu_assert_eq("consumed", cbuffer_size(mybuf), 8);
spi_stream_verify_packet(my_pkt, 10, &checksum_err);
mu_assert_eq("no cksum actual", checksum_err, 0);
return 0;
}
void
UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
{
struct ubx_header header;
uint8_t ck_a = 0, ck_b = 0;
header.sync1 = UBX_SYNC1;
header.sync2 = UBX_SYNC2;
header.msg_class = msg_class;
header.msg_id = msg_id;
header.length = size;
add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b);
add_checksum((uint8_t *)msg, size, ck_a, ck_b);
/* configure ACK check */
_message_class_needed = msg_class;
_message_id_needed = msg_id;
write(_fd, (const char *)&header, sizeof(header));
write(_fd, (const char *)msg, size);
write(_fd, (const char *)&ck_a, 1);
write(_fd, (const char *)&ck_b, 1);
}
static char* test_checksumming(void) {
uint32_t my_data[10] = {1, 2, 3, 4, 5, 6, 7, 8, 9, -1};
add_checksum(my_data, 10);
mu_assert("end modified", my_data[9] != -1);
mu_assert("closure", verify_checksum(my_data, 10));
my_data[2] = 1000;
mu_assert("detect error", !verify_checksum(my_data, 10));
return 0;
}
static char* test_verify_packet(void) {
uint32_t my_data[10] = {0xBEEF, 2, 3, 4, 5, 6, 7, 8, 9, 10};
add_checksum(my_data, 10);
mu_assert_eq("not clobbered", my_data[8], 9);
mu_assert("chksum", my_data[9] != 10);
int cksum_error = -1;
uint32_t packet_id = spi_stream_verify_packet(my_data, 10, &cksum_error);
mu_assert_eq("pkt_id", packet_id, 0xBEEF);
mu_assert_eq("error", cksum_error, 0);
// simulate error
my_data[5] = 1;
packet_id = spi_stream_verify_packet(my_data, 10, &cksum_error);
mu_assert_eq("pkt_id", packet_id, 0xBEEF);
mu_assert_eq("error", cksum_error, 1);
return 0;
}
DLLEXPORT uint16_t *trace_checksum_transport(libtrace_packet_t *packet,
uint16_t *csum) {
void *header = NULL;
uint16_t ethertype;
uint32_t remaining;
uint32_t sum = 0;
uint8_t proto = 0;
uint16_t *csum_ptr = NULL;
int plen = 0;
uint8_t safety[65536];
uint8_t *ptr = safety;
header = trace_get_layer3(packet, ðertype, &remaining);
if (header == NULL)
return NULL;
if (ethertype == TRACE_ETHERTYPE_IP) {
libtrace_ip_t *ip = (libtrace_ip_t *)header;
if (remaining < sizeof(libtrace_ip_t))
return NULL;
sum = ipv4_pseudo_checksum(ip);
} else if (ethertype == TRACE_ETHERTYPE_IPV6) {
libtrace_ip6_t *ip = (libtrace_ip6_t *)header;
if (remaining < sizeof(libtrace_ip6_t))
return 0;
sum = ipv6_pseudo_checksum(ip);
}
header = trace_get_transport(packet, &proto, &remaining);
if (proto == TRACE_IPPROTO_TCP) {
libtrace_tcp_t *tcp = (libtrace_tcp_t *)header;
header = trace_get_payload_from_tcp(tcp, &remaining);
csum_ptr = &tcp->check;
memcpy(ptr, tcp, tcp->doff * 4);
tcp = (libtrace_tcp_t *)ptr;
tcp->check = 0;
ptr += (tcp->doff * 4);
}
else if (proto == TRACE_IPPROTO_UDP) {
libtrace_udp_t *udp = (libtrace_udp_t *)header;
header = trace_get_payload_from_udp(udp, &remaining);
csum_ptr = &udp->check;
memcpy(ptr, udp, sizeof(libtrace_udp_t));
udp = (libtrace_udp_t *)ptr;
udp->check = 0;
ptr += sizeof(libtrace_udp_t);
}
else if (proto == TRACE_IPPROTO_ICMP) {
/* ICMP doesn't use the pseudo header */
sum = 0;
libtrace_icmp_t *icmp = (libtrace_icmp_t *)header;
header = trace_get_payload_from_icmp(icmp, &remaining);
csum_ptr = &icmp->checksum;
memcpy(ptr, icmp, sizeof(libtrace_icmp_t));
icmp = (libtrace_icmp_t *)ptr;
icmp->checksum = 0;
ptr += sizeof(libtrace_icmp_t);
}
else {
return NULL;
}
sum += add_checksum(safety, (uint16_t)(ptr - safety));
plen = trace_get_payload_length(packet);
if (plen < 0)
return NULL;
if (remaining < (uint32_t)plen)
return NULL;
if (header == NULL)
return NULL;
sum += add_checksum(header, (uint16_t)plen);
*csum = ntohs(finish_checksum(sum));
//assert(0);
return csum_ptr;
}
int main()
{
char unlock_code_str [50];
char input_c;
char box_id_str [50];
char filename_str [50];
uint32_t unlock_code = 0;
uint8_t i;
uint32_t blah;
FILE* unlock_code_file;
//Clear screen and print header
#ifdef __WIN32__
system("cls");
#else
//system("clear");
#endif
printf("Generate and tests the unlock codes for the stand alone battery box \n\r");
printf("____________\r\n");
//Read in the Box ID
do {
box_id_str[0] = '\0';
while( strlen(box_id_str) <= 1 ) //Checks for nothing entered (endline counts as 1 char)
{
printf("Please enter the box ID ( 0 -> 16383 ): \n\r");
fgets( box_id_str, 256, stdin);
}
} while ( set_box_id( atoi(box_id_str) ) );
printf("What do you want to do? Press 'g' to generate codes, 'p' for php testing, 't' to test codes\n\r");
input_c = getchar();
if (input_c == 'g') {
//Generate the filename for the output file and open it for writing
box_id_str[strcspn ( box_id_str, "\n" )] = '\0'; //Removes the newline
strcpy(filename_str, box_id_str); //Copies the box_id into the filename
strcat(filename_str, "_unlock_codes.txt"); //Adds the second part and file extension
printf("Writing to file: %s\n\r", filename_str); //Prints out update
unlock_code_file = fopen(filename_str, "w"); //Opens the file for writing
//Generate codes for given box_id, each unlocks the box for 2 Weeks
SET_UNLOCK_DAYS(unlock_code, TWO_WEEKS);
if (unlock_code_file) {
//Generate 51 unlock codes
for (i = 0; i < 51; i++){
SET_UNLOCK_COUNT(unlock_code, i);
SET_PAD( unlock_code, rand() )
add_checksum(&unlock_code);
fprintf(unlock_code_file, "Code %03i: %010u \n\r", i, unlock_code);
}
//Generate a final full unlock code
SET_UNLOCK_DAYS(unlock_code, FULL_UNLOCK);
add_checksum(&unlock_code);
fprintf(unlock_code_file, "Full unlock code: %010u \n\r", unlock_code);
fclose(unlock_code_file);
printf("%i unlock codes have been written to the output file \n\r", (i+1) );
}
else {
printf("The file could not be opened \n\r");
}
}
else if (input_c == 'p') {
blah = 0x0000D77C;
get_checksum(blah);
//(uint8_t*)&
//Fletcher16((uint8_t*)&blah, 6);
}
else {
while (1)
{
//Read in the unlock code
unlock_code_str[0] = '\0';
//Checks for nothing entered (endline counts as 1 char)
while( strlen(unlock_code_str) <= 1 )
{
printf("\r\nPlease enter the unlock code (ten digits):\r\nInput: ");
fgets( unlock_code_str, 50, stdin);
}
//Use to strtoul to return an unsigned value
//atoi returns signed int
unlock_code = (uint32_t)strtoul(unlock_code_str, NULL, 10);
//printf("%s \n\r", unlock_code_str);
printf("\r\nUnlock code hex: 0x%x \r\n", unlock_code);
printf("Unlock code: %010u \r\n", unlock_code);
printf("Box ID: %u \r\n\r\n", get_box_id());
if (check_checksum(unlock_code)) {
printf("Code checksum correct \r\n\r\n");
printf("The unlock count is %u \r\n", GET_UNLOCK_COUNT(unlock_code) );
switch (GET_UNLOCK_DAYS(unlock_code)) {
case FULL_UNLOCK:
printf("Full unlock \r\n");
break;
case TWO_DAYS:
printf("Two day unlock \r\n");
break;
case FIVE_DAYS:
printf("Five day unlock \r\n");
break;
case SEVEN_DAYS:
printf("Seven day unlock \r\n");
break;
case TWO_WEEKS:
printf("Two week unlock \r\n");
break;
case THREE_WEEKS:
printf("Three week unlock \r\n");
break;
case ONE_MONTH:
printf("One month unlock \r\n");
break;
case TWO_MONTHS:
printf("Two month unlock \r\n");
break;
default:
printf("Invalid code \r\n");
break;
}
}
else {
printf("Code checksum incorrect");
}
printf("\r\n_____________________\r\n");
}
}
//Keeps the window open until enter is pressed
getchar();
return 0;
}
int main(int argc, char**argv)
{
int port = 69;
char ch;
/* Read command line options */
while ((ch = getopt(argc, argv, "p:")) != -1) {
switch (ch) {
case 'p':
port = atoi(optarg);
break;
case '?':
printf("Usage: ./server [-p port]\n");
exit(1);
}
}
char PORT[10];
sprintf(PORT,"%d",port);
int sockfd, new_fd; // listen on sock_fd, new connection on new_fd
struct addrinfo hints, *servinfo, *p;
struct sockaddr_storage their_addr; // connector's address information
socklen_t sin_size;
struct sigaction sa;
int yes=1;
char s[INET6_ADDRSTRLEN];
int rv;
memset(&hints, 0, sizeof hints);
hints.ai_family = AF_UNSPEC;
hints.ai_socktype = SOCK_STREAM;
hints.ai_flags = AI_PASSIVE; // use my IP
if ((rv = getaddrinfo(NULL, PORT, &hints, &servinfo)) != 0) {
fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(rv));
return 1;
}
// loop through all the results and bind to the first we can
for(p = servinfo; p != NULL; p = p->ai_next) {
if ((sockfd = socket(p->ai_family, p->ai_socktype,
p->ai_protocol)) == -1) {
perror("server: socket");
continue;
}
if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &yes,
sizeof(int)) == -1) {
perror("setsockopt");
exit(1);
}
if (bind(sockfd, p->ai_addr, p->ai_addrlen) == -1) {
close(sockfd);
perror("server: bind");
continue;
}
break;
}
if (p == NULL) {
fprintf(stderr, "server: failed to bind\n");
return 2;
}
freeaddrinfo(servinfo); // all done with this structure
if (listen(sockfd, BACKLOG) == -1) {
perror("listen");
exit(1);
}
sa.sa_handler = sigchld_handler; // reap all dead processes
sigemptyset(&sa.sa_mask);
sa.sa_flags = SA_RESTART;
if (sigaction(SIGCHLD, &sa, NULL) == -1) {
perror("sigaction");
exit(1);
}
printf("server: waiting for connections...\n");
while(1) { // main accept() loop
sin_size = sizeof their_addr;
new_fd = accept(sockfd, (struct sockaddr *)&their_addr, &sin_size);
if (new_fd == -1) {
perror("accept");
continue;
}
inet_ntop(their_addr.ss_family,
get_in_addr((struct sockaddr *)&their_addr),
s, sizeof s);
printf("server: got connection from %s\n", s);
if (!fork()) { // this is the child process
close(sockfd); // child doesn't need the listener
/* Set Socket to non blocking */
//fcntl(new_fd, F_SETFL, O_NONBLOCK);
char buf[PACKET_SIZE];
char databuf[PAYLOAD_SIZE];
int numbytes, ack_bytes;
header_t head;
packet_t packet;
packet_t ackr;
//char recved[PACKET_SIZE];
int getfilename = 1;
char *filename = (char*) "recvfile.txt";
FILE * filefd;
/* Zero the arrays */
bzero(buf, PACKET_SIZE);
bzero(databuf, PAYLOAD_SIZE);
while(getfilename) {
if ((numbytes = recv(new_fd, buf, PACKET_SIZE, 0)) == -1) {
perror("recv");
exit(1);
}
/* We have a packet with filename! */
if(numbytes > 0)
{
read_header(&head, (packet_t *)buf);
read_packet((u_char*) databuf, (packet_t *)buf, (u_short) (numbytes - HEADER_SIZE));
printf("Packet '%d' received\n", (int)(head.seq));
if(add_checksum(numbytes, numbytes%2, (u_short *)buf) == 0) {
printf("Checksum OK\n");
/* Create ACK to send back with sequence number */
fill_header((int)(head.seq), 0, HEADER_SIZE, ACK, &ackr);
printf("Filename: '%s' bytes '%d'\n", databuf, numbytes);
if((ack_bytes = send(new_fd, &ackr, HEADER_SIZE, 0)) == -1) {
perror("send: ack");
exit(1);
} else {
printf("ACK '%d' sent\n", (int)(head.seq));
getfilename = 0;
}
} else {
printf("Checksum failed, discarding packet...\n");
}
}
} /* END first while for filename */
//filefd = open(argv[3], O_WRONLY | O_CREAT | O_APPEND);
if ( (filefd = fopen(filename, "w")) == NULL)
{
fprintf(stderr, "Could not open destination file, using stdout.\n");
} else {
printf("Preparing to start writing file '%s'\n", filename );
}
int recvFIN = 0;
/* re-zero array */
bzero(buf, PACKET_SIZE);
bzero(databuf, PAYLOAD_SIZE);
/* read the file from the socket as long as there is data */
do {
if ((numbytes = recv(new_fd, buf, PACKET_SIZE, 0)) == -1) {
perror("recv");
fclose(filefd);
close(new_fd);
exit(1);
}
read_header(&head, (packet_t *)buf);
if((int)head.flag == ACK)
{
read_packet((u_char*) databuf, (packet_t *)buf, (u_short) (numbytes - HEADER_SIZE));
printf("Packet '%d' received with '%d' bytes with an offset of '%d'\n", (int)(head.seq), numbytes, (int)(head.offset));
if(add_checksum(numbytes, numbytes%2, (u_short *)buf) == 0) {
printf("Checksum OK\n");
/* Create ACK to send back with sequence number */
fill_header((int)(head.seq), 0, HEADER_SIZE, ACK, &ackr);
if((ack_bytes = send(new_fd, &ackr, HEADER_SIZE, 0)) == -1) {
perror("send: ack");
exit(1);
} else {
printf("ACK '%d' sent\n", (int)(head.seq));
/* write to file */
fwrite(databuf,1 , (int)(head.offset) ,filefd );
}
} else {
printf("Checksum failed, discarding packet...\n");
}
} else if ((int)head.flag == FIN) {
printf("FIN received...\n");
/* Create FIN to send back with sequence number */
fill_header((int)(head.seq), 0, HEADER_SIZE, FIN, &ackr);
if((ack_bytes = send(new_fd, &ackr, HEADER_SIZE, 0)) == -1) {
perror("send: ack");
exit(1);
} else {
printf("FIN '%d' sent\n", (int)(head.seq));
recvFIN = 1;
}
}
//else if ((int)head.flag == FIN) {
/* re-zero */
bzero(buf, PACKET_SIZE);
bzero(databuf, PAYLOAD_SIZE);
} while (!recvFIN );
printf("Done...\n");
fclose(filefd);
close(new_fd);
exit(0);
}
close(new_fd); // parent doesn't need this
}
return 0;
}
