Commit 19172393 authored by David Johnson's avatar David Johnson
Browse files

Initial version. Compiles.

parent 5b51b884
This diff is collapsed.
#ifndef __EMCD_H__
#define __EMCD_H__
#endif
#include "robot_list.h"
struct robot_list *robot_list_create() {
struct robot_list *tmp = (struct robot_list *)malloc(sizeof(struct robot_list));
if (tmp != NULL) {
tmp->type = -1;
tmp->item_count = 0;
tmp->head = NULL;
tmp->tail = NULL;
}
return tmp;
}
void robot_list_destroy(struct robot_list *l) {
if (l->head != NULL) {
struct robot_list_item *n = l->head;
struct robot_list_item *nn = NULL;
while (n != NULL) {
nn = n->next;
free(n);
n = nn;
}
}
free(l);
}
int robot_list_append(struct robot_list *l,int id,void *data) {
if (l == NULL || data == NULL) {
return 0;
}
else {
struct robot_list_item *n = (struct robot_list_item *)malloc(sizeof(struct robot_list_item));
n->id = id;
n->data = data;
n->next = NULL;
if (n != NULL) {
if (l->head == NULL) {
l->head = n;
l->tail = n;
}
else {
l->tail->next = n;
l->tail = n;
}
++(l->item_count);
return 1;
}
else {
return 0;
}
}
}
int robot_list_insert(struct robot_list *l,int id,void *data) {
if (l == NULL || data == NULL) {
return 0;
}
else {
struct robot_list_item *n = (struct robot_list_item *)malloc(sizeof(struct robot_list_item));
n->id = id;
n->data = data;
n->next = NULL;
if (n != NULL) {
if (l->head == NULL) {
l->head = n;
l->tail = n;
}
else {
n->next = l->head;
l->head = n;
}
++(l->item_count);
return 1;
}
else {
return 0;
}
}
}
void *robot_list_search(struct robot_list *l,int id) {
if (l == NULL) {
return NULL;
}
else {
struct robot_list_item *h = NULL;
struct robot_list_item *i = l->head;
void *retval = NULL;
while (i != NULL) {
if (i->id == id) {
retval = i->data;
break;
}
h = i;
i = i->next;
}
return retval;
}
}
int robot_list_remove_by_data(struct robot_list *l,void *data) {
if (l == NULL || data == NULL) {
return -1;
}
else {
struct robot_list_item *h = NULL;
struct robot_list_item *i = l->head;
int retval = -1;
while (i != NULL) {
if (i->data == data) {
// if we're at head and there's only a single elm
if (i == l->head && l->head == l->tail) {
l->head = l->tail = NULL;
}
else {
if (i == l->head) {
l->head = i->next;
}
else if (i == l->tail) {
h->next = NULL;
l->tail = h;
}
else {
h->next = i->next;
}
}
retval = i->id;
free(i);
--(l->item_count);
break;
}
h = i;
i = i->next;
}
return retval;
}
}
void *robot_list_remove_by_id(struct robot_list *l,int id) {
if (l == NULL) {
return NULL;
}
else {
struct robot_list_item *h = NULL;
struct robot_list_item *i = l->head;
void *retval = NULL;
while (i != NULL) {
if (i->id == id) {
// if we're at head and there's only a single elm
if (i == l->head && l->head == l->tail) {
l->head = l->tail = NULL;
}
else {
if (i == l->head) {
l->head = i->next;
}
else if (i == l->tail) {
h->next = NULL;
l->tail = h;
}
else {
h->next = i->next;
}
}
retval = i->data;
free(i);
--(l->item_count);
break;
}
h = i;
i = i->next;
}
return retval;
}
}
struct robot_list_enum *robot_list_enum(struct robot_list *l) {
if (l == NULL || l->item_count == 0) {
return NULL;
}
else {
struct robot_list_enum *e = (struct robot_list_enum *)malloc(sizeof(struct robot_list_enum));
void **tmp = NULL;
if (e == NULL) {
return NULL;
}
else {
tmp = (void *)malloc(sizeof(void *)*(l->item_count));
if (tmp == NULL) {
free(e);
return NULL;
}
e->data = tmp;
e->current_index = 0;
e->size = l->item_count;
/* scroll through list forwards and copy data into e->data */
struct robot_list_item *a = l->head;
int i = 0;
while (a != NULL) {
e->data[i] = a->data;
a = a->next;
++i;
}
return e;
}
}
}
void *robot_list_enum_next_element(struct robot_list_enum *e) {
if (e != NULL) {
if (e->current_index == e->size) {
return NULL;
}
else {
void *retval = e->data[e->current_index];
++(e->current_index);
return retval;
}
}
else {
return NULL;
}
}
void robot_list_enum_destroy(struct robot_list_enum *e) {
if (e != NULL) {
free(e->data);
free(e);
}
}
#ifndef __ROBOT_LIST_H__
#define __ROBOT_LIST_H__
/* this is essentially an advanced list class -- data are of type void*;
* support for enumerations is provided for ease-of-use
*/
/* it essentially does for C what the STL does in c++ -- but I hate c++ --
* so the upshot is that it's not type-safe -- oh well.
*/
#ifndef NULL
#define NULL (void*)0
#endif
typedef struct robot_list {
int item_count;
int type;
struct robot_list_item *head;
struct robot_list_item *tail;
struct robot_list_item *iterator;
} robot_list_t;
typedef struct robot_list_item {
int id;
void *data;
struct robot_list_item *next;
} robot_list_item_t;
typedef struct robot_list_enum {
int size;
int current_index;
void **data;
} robot_list_enum_t;
/* creates a robot_list */
struct robot_list *robot_list_create();
/* frees all structs used by the list -- but leaves data intact */
void robot_list_destroy(struct robot_list *l);
/* return 0 on failure, 1 on success */
int robot_list_append(struct robot_list *l,int id,void *data);
int robot_list_insert(struct robot_list *l,int id,void *data);
/* return a pointer to the data on success; NULL otherwise */
void *robot_list_search(struct robot_list *l,int id);
/* returns the data for this id, if any */
void *robot_list_remove_by_id(struct robot_list *l,int id);
/* returns the id for this data; if no match on data ptr, returns -1 */
int robot_list_remove_by_data(struct robot_list *l,void *data);
/* enumeration crap -- very very similiar to java's java.util.Enumeration
* mumbo-jumbo.
*/
struct robot_list_enum *robot_list_enum(struct robot_list *l);
void *robot_list_enum_next_element(struct robot_list_enum *e);
void robot_list_enum_destroy(struct robot_list_enum *e);
#endif
#include "mtp.h"
#include <stdio.h>
#include <unistd.h>
#include <netinet/in.h>
int mtp_receive_packet(int fd,struct mtp_packet **packet) {
unsigned int length;
char clength[4];
char *buf = NULL;
int i;
int retval;
if (packet == NULL) {
return MTP_PP_ERROR_ARGS;
}
retval = read(fd,(void*)(&clength),4);
if (retval == -1) {
return MTP_PP_ERROR_READ;
}
else {
/* length is currently in bytes 0,1,2,and 3, in network byte order */
length = ntohl((uint32_t)(clength));
if (length > MTP_PACKET_MAXLEN) {
return MTP_PP_ERROR_LENGTH;
}
/* read the rest of the packet into buf */
int remaining_length = length - 4;
buf = (char*)malloc(sizeof(char)*(length+1));
if (buf == NULL) {
return MTP_PP_ERROR_MALLOC;
}
// copy the header bytes to the new buf
for (i = 0; i < 4; ++i) {
buf[i] = clength[i];
}
retval = read(fd,(void*)(buf+sizeof(char)*MTP_PACKET_HEADER_LEN),remaining_length);
if (retval == -1) {
free(buf);
return MTP_PP_ERROR_READ;
}
retval = mtp_decode_packet(buf,packet);
free(buf);
return retval;
}
}
int mtp_encode_packet(char **buf_ptr,struct mtp_packet *packet) {
char *buf;
int i,j,k;
int buf_size;
if (packet == NULL || buf == NULL) {
return MTP_PP_ERROR_ARGS;
}
// first we have to caculate how many bytes we need for this
// specific data -- we could've defined a static size for things,
// but we're encoding variable-length arrays and strings in addition
// to ints and floats -- but we didn't -- so we have to skim through.
if (packet->opcode == MTP_CONTROL_ERROR ||
packet->opcode == MTP_CONTROL_NOTIFY ||
packet->opcode == MTP_CONTROL_INIT ||
packet->opcode == MTP_CONTROL_CLOSE) {
struct mtp_control *data = packet->data.control;
buf_size = mtp_calc_size(packet->opcode,(void *)data);
buf = (char *)malloc(sizeof(char)*buf_size);
*buf_ptr = buf;
if (buf == NULL) {
return MTP_PP_ERROR_MALLOC;
}
*((int *)buf) = htonl(buf_size);
buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
buf[MTP_PACKET_HEADER_OFFSET_VERSION] = (char)(packet->version);
buf[MTP_PACKET_HEADER_OFFSET_ROLE] = (char)(packet->role);
i = MTP_PACKET_HEADER_LEN;
// write id field
*((int *)(buf+i)) = htonl(data->id);
i += 4;
// write code field
*((int *)(buf+i)) = htonl(data->code);
i += 4;
// write msg field, and null-term it
for (j = 0; j < strlen(data->msg); ++j) {
buf[i] = data->msg[j];
++i;
}
buf[i] = '\0';
}
else if (packet->opcode == MTP_CONFIG_RMC) {
struct mtp_config_rmc *data = packet->data.config_rmc;
buf_size = mtp_calc_size(packet->opcode,(void *)data);
buf = (char *)malloc(sizeof(char)*buf_size);
*buf_ptr = buf;
if (buf == NULL) {
return MTP_PP_ERROR_MALLOC;
}
*((int *)buf) = htonl(buf_size);
buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
buf[MTP_PACKET_HEADER_OFFSET_VERSION] = (char)(packet->version);
buf[MTP_PACKET_HEADER_OFFSET_ROLE] = (char)(packet->role);
// now write the specific data:
i = MTP_PACKET_HEADER_LEN;
*((int *)(buf+i)) = htonl(data->num_robots);
i += 4;
// now write the list
for (j = 0; j < data->num_robots; ++j) {
// write the id field
*((int *)(buf+i)) = htonl(data->robots[j].id);
i += 4;
// write the hostname field
int len = strlen(data->robots[j].hostname);
for (k = 0; k < len; ++k) {
buf[i] = data->robots[j].hostname[k];
++i;
}
buf[i] = '\0';
++i;
}
// now write the global_bound data
*((int *)(buf+i)) = htonl(data->box.horizontal);
i += 4;
*((int *)(buf+i)) = htonl(data->box.vertical);
}
else if (packet->opcode == MTP_CONFIG_VMC) {
struct mtp_config_rmc *data = packet->data.config_rmc;
buf_size = mtp_calc_size(packet->opcode,(void *)data);
buf = (char *)malloc(sizeof(char)*buf_size);
*buf_ptr = buf;
if (buf == NULL) {
return MTP_PP_ERROR_MALLOC;
}
*((int *)buf) = htonl(buf_size);
buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
buf[MTP_PACKET_HEADER_OFFSET_VERSION] = (char)(packet->version);
buf[MTP_PACKET_HEADER_OFFSET_ROLE] = (char)(packet->role);
// now write the specific data:
i = MTP_PACKET_HEADER_LEN;
*((int *)(buf+i)) = htonl(data->num_robots);
i += 4;
// now write the list
for (j = 0; j < data->num_robots; ++j) {
// write the id field
*((int *)(buf+i)) = htonl(data->robots[j].id);
i += 4;
// write the hostname field
int len = strlen(data->robots[j].hostname);
for (k = 0; k < len; ++k) {
buf[i] = data->robots[j].hostname[k];
++i;
}
buf[i] = '\0';
++i;
}
}
else if (packet->opcode == MTP_REQUEST_POSITION) {
struct mtp_request_position *data = packet->data.request_position;
buf_size = mtp_calc_size(packet->opcode,(void *)data);
buf = (char *)malloc(sizeof(char)*buf_size);
*buf_ptr = buf;
if (buf == NULL) {
return MTP_PP_ERROR_MALLOC;
}
*((int *)buf) = htonl(buf_size);
buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
buf[MTP_PACKET_HEADER_OFFSET_VERSION] = (char)(packet->version);
buf[MTP_PACKET_HEADER_OFFSET_ROLE] = (char)(packet->role);
// now write the specific data:
i = MTP_PACKET_HEADER_LEN;
*((int *)(buf+i)) = htonl(data->robot_id);
i += 4;
}
else if (packet->opcode == MTP_REQUEST_ID) {
struct mtp_request_id *data = packet->data.request_id;
buf_size = mtp_calc_size(packet->opcode,(void *)data);
buf = (char *)malloc(sizeof(char)*buf_size);
*buf_ptr = buf;
if (buf == NULL) {
return MTP_PP_ERROR_MALLOC;
}
*((int *)buf) = htonl(buf_size);
buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
buf[MTP_PACKET_HEADER_OFFSET_VERSION] = (char)(packet->version);
buf[MTP_PACKET_HEADER_OFFSET_ROLE] = (char)(packet->role);
// now write the specific data:
i = MTP_PACKET_HEADER_LEN;
*((int *)(buf+i)) = htonl(data->position.x);
i += 4;
*((int *)(buf+i)) = htonl(data->position.y);
i += 4;
*((int *)(buf+i)) = htonl(data->position.theta);
i += 4;
*((int *)(buf+i)) = htonl(data->timestamp);
i += 4;
}
else if (packet->opcode == MTP_UPDATE_POSITION) {
struct mtp_update_position *data = packet->data.update_position;
buf_size = mtp_calc_size(packet->opcode,(void *)data);
buf = (char *)malloc(sizeof(char)*buf_size);
*buf_ptr = buf;
if (buf == NULL) {
return MTP_PP_ERROR_MALLOC;
}
*((int *)buf) = htonl(buf_size);
buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
buf[MTP_PACKET_HEADER_OFFSET_VERSION] = (char)(packet->version);
buf[MTP_PACKET_HEADER_OFFSET_ROLE] = (char)(packet->role);
// now write the specific data:
i = MTP_PACKET_HEADER_LEN;
*((int *)(buf+i)) = htonl(data->robot_id);
i += 4;
*((int *)(buf+i)) = htonl(data->position.x);
i += 4;
*((int *)(buf+i)) = htonl(data->position.y);
i += 4;
*((int *)(buf+i)) = htonl(data->position.theta);
i += 4;
*((int *)(buf+i)) = htonl(data->status);
i += 4;
*((int *)(buf+i)) = htonl(data->timestamp);
i += 4;
}
else if (packet->opcode == MTP_UPDATE_ID) {
struct mtp_update_id *data = packet->data.update_id;
buf_size = mtp_calc_size(packet->opcode,(void *)data);
buf = (char *)malloc(sizeof(char)*buf_size);
*buf_ptr = buf;
if (buf == NULL) {
return MTP_PP_ERROR_MALLOC;
}
*((int *)buf) = htonl(buf_size);
buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
buf[MTP_PACKET_HEADER_OFFSET_VERSION] = (char)(packet->version);
buf[MTP_PACKET_HEADER_OFFSET_ROLE] = (char)(packet->role);
// now write the specific data:
i = MTP_PACKET_HEADER_LEN;
*((int *)(buf+i)) = htonl(data->robot_id);
i += 4;
}
else if (packet->opcode == MTP_COMMAND_GOTO) {
struct mtp_command_goto *data = packet->data.command_goto;
buf_size = mtp_calc_size(packet->opcode,(void *)data);
buf = (char *)malloc(sizeof(char)*buf_size);
*buf_ptr = buf;
if (buf == NULL) {
return MTP_PP_ERROR_MALLOC;
}
*((int *)buf) = htonl(buf_size);
buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
buf[MTP_PACKET_HEADER_OFFSET_VERSION] = (char)(packet->version);
buf[MTP_PACKET_HEADER_OFFSET_ROLE] = (char)(packet->role);
// now write the specific data:
i = MTP_PACKET_HEADER_LEN;
*((int *)(buf+i)) = htonl(data->command_id);
i += 4;