static int server_receive(int socket, struct server_data *data, int (*data_handler)(const char*,size_t,void*), void* data_handler_param)
{
size_t size;
size = server_receive_size(socket, data);
//VIR_DEBUG("expecting %lu bytes\n", size);
//VIR_DEBUG("i_begin: %lu , available: %lu", data->i_begin, data->available);
//Data size is bigger than what is available -> do multiple retrieval iterations
while (data->available >= 0 && size > data->available) {
if (data_handler(data->buf+data->i_begin, data->available, data_handler_param) < 0) {
VIR_DEBUG("data handler error!\n");
return -1;
}
size -= data->available; //all available data has been consumed
//Receive new data and reset i_begin
if ((data->available = recv(socket, data->buf, SD_BUFSIZE, 0)) < 0) {
VIR_DEBUG("error while recv'ing\n");
return -1;
}
data->i_begin = 0;
}
//available data is bigger than data size -> retrieve and update i_begin and available
if (data_handler(data->buf+data->i_begin, size, data_handler_param) < 0) {
VIR_DEBUG("data handler error!\n");
return -1;
}
data->i_begin += size;
data->available -= size;
return 0;
}
/**
* oseaclient_request_call_user_function:
* @type: * @type: Type of oseaclient user space data: AFDAL_REQUEST_DATA,
* AFDAL_REQUEST_SIMPLE_DATA, AFDAL_REQUEST_NUL_DATA
* @handler: user space function to be executed
* @user_data: user space data to be passed to @handler.
* @oseaclient_data: afdal level data to be passed to @handler.
*
* Convenience function to make life easy to those who are writing
* liboseaclient* function. It's more dificult to explain what does this
* function do, so please see
* @oseaclient_request_process_data/simple_data/nul_data function.
**/
void oseaclient_request_call_user_function (AfDalRequestReturnData type, gpointer handler,
gpointer user_data, gpointer oseaclient_data)
{
AfDalDataFunc data_handler;
AfDalSimpleFunc simple_handler;
AfDalNulFunc nul_handler;
AfDalMultiFunc multi_handler;
gboolean result = FALSE;
g_log (LOG_DOMAIN, G_LOG_LEVEL_DEBUG, "Executing user handler");
switch (type) {
case AFDAL_REQUEST_DATA:
data_handler = (AfDalDataFunc) handler;
result = data_handler (oseaclient_data, user_data);
break;
case AFDAL_REQUEST_SIMPLE_DATA:
simple_handler = (AfDalSimpleFunc) handler;
result = simple_handler (oseaclient_data, user_data);
break;
case AFDAL_REQUEST_NUL_DATA:
nul_handler = (AfDalNulFunc) handler;
result = nul_handler (oseaclient_data, user_data);
break;
case AFDAL_REQUEST_MULTI_DATA:
multi_handler = (AfDalMultiFunc) handler;
result = multi_handler (oseaclient_data, user_data);
break;
}
g_log (LOG_DOMAIN, G_LOG_LEVEL_DEBUG, "User handler executed");
return;
}
/**
* Start Handler for the Expat parser.
*/
void start_handler(void* data, const XML_Char* name, const XML_Char** attributes)
{
struct state_t* state = (struct state_t*)data;
struct conversion_t* c;
unsigned i;
++state->depth;
if (state->depth < DEPTH_MAX) {
state->level[state->depth].tag = name;
state->level[state->depth].data = 0;
state->level[state->depth].len = 0;
if (state->error == 0) {
c = identify(state->depth, state->level);
if (c) {
state->level[state->depth].process = c->process;
state->level[state->depth].process(state, token_open, 0, 0, attributes);
} else {
state->level[state->depth].process = 0;
}
for(i=0;attributes[i];i+=2) {
const char* null_atts[1] = { 0 };
start_handler(data, attributes[i], null_atts);
data_handler(data, attributes[i+1], strlen(attributes[i+1]));
end_handler(data, attributes[i]);
}
} else {
state->level[state->depth].process = 0;
}
}
}
static void select(ClickRecognizerRef ref, void* context) {
if(playing) return;
playing = TRUE;
layer_set_hidden(text_layer_get_layer(s_title_layer),TRUE);
layer_set_hidden(text_layer_get_layer(s_bplay_layer),TRUE);
layer_set_hidden(text_layer_get_layer(s_diff_layer),TRUE);
data_handler(NULL);
}
//used to control the leds
void LED_CONTROL(int LED_NUMBER, int LED_COMMAND)
{
data_handler(LED_NUMBER,LED_COMMAND);
// Slave address of TCA6507 is 0x45 (binary 1001 101)
// false=write / true=read
I2CMasterSlaveAddrSet(I2C0_MASTER_BASE, 0x45, false);
// the for loop is there to make the code more compact and removes some redundant commands.
int COUNTER;
for (COUNTER=0;COUNTER!=5;COUNTER++)
{
if(COUNTER == 2)
{
//puts the command-byte in the dataput getting ready to sending it.
I2CMasterDataPut(I2C0_MASTER_BASE, COMMAND_BYTE_INCREMENT);
//starts sending the data.
I2CMasterControl(I2C0_MASTER_BASE,I2C_MASTER_CMD_BURST_SEND_START);
}
else if(COUNTER == 3)
{
//gets the first led_current_setting containing the byte for select0 ready to transmitting.
I2CMasterDataPut(I2C0_MASTER_BASE,led_current_setting[0] );
//keeps sending data.
I2CMasterControl(I2C0_MASTER_BASE,I2C_MASTER_CMD_BURST_SEND_CONT);
}
else if(COUNTER == 4)
{
//gets the second led_current_setting containing the byte for select1 ready to transmitting.
I2CMasterDataPut(I2C0_MASTER_BASE,led_current_setting[1] );
//keeps sending data.
I2CMasterControl(I2C0_MASTER_BASE,I2C_MASTER_CMD_BURST_SEND_CONT);
}
else if(COUNTER == 5)
{
//gets the third led_current_setting containing the byte for select2 ready to transmitting.
I2CMasterDataPut(I2C0_MASTER_BASE,led_current_setting[2] );
//transmitting the final byte and a stop command.
I2CMasterControl(I2C0_MASTER_BASE, I2C_MASTER_CMD_BURST_SEND_FINISH );
}
// Wait for I2C to finish.
while(I2CMasterBusy(I2C0_MASTER_BASE));
//a short delay.
SysCtlDelay(80000);
}
}
void send_rawip(void)
{
char *packet;
packet = malloc(data_size);
if (packet == NULL)
{
perror("[send_rawip] malloc()");
return;
}
memset(packet, 0, data_size);
data_handler(packet, data_size);
send_ip_handler(packet, data_size);
free(packet);
}
static void dtls_recv_handler(struct mbuf *mb, void *arg)
{
struct allocation *alloc = arg;
struct sa src;
int err;
/* forward packet to TURN-client */
err = turnc_recv(alloc->turnc, &src, mb);
if (err) {
alloc->alloch(err, 0, NULL, NULL, NULL, alloc->arg);
return;
}
/* available application data? */
if (mbuf_get_left(mb)) {
data_handler(alloc, &src, mb);
}
}
void send_icmp_echo(void)
{
char *packet, *data;
struct myicmphdr *icmp;
packet = malloc(ICMPHDR_SIZE + data_size);
if (packet == NULL) {
perror("[send_icmp] malloc");
return;
}
memset(packet, 0, ICMPHDR_SIZE + data_size);
icmp = (struct myicmphdr*) packet;
data = packet + ICMPHDR_SIZE;
/* fill icmp hdr */
icmp->type = opt_icmptype; /* echo replay or echo request */
icmp->code = opt_icmpcode; /* should be indifferent */
icmp->checksum = 0;
icmp->un.echo.id = getpid() & 0xffff;
icmp->un.echo.sequence = _icmp_seq;
/* data */
data_handler(data, data_size);
/* icmp checksum */
if (icmp_cksum == -1)
icmp->checksum = cksum((u_short*)packet, ICMPHDR_SIZE + data_size);
else
icmp->checksum = icmp_cksum;
/* adds this pkt in delaytable */
if (opt_icmptype == ICMP_ECHO)
delaytable_add(_icmp_seq, 0, time(NULL), get_usec(), S_SENT);
/* send packet */
send_ip_handler(packet, ICMPHDR_SIZE + data_size);
free (packet);
_icmp_seq++;
}
static void data_handler2(void *opaque, int irq, int level)
{
data_handler(opaque, irq, level, 2);
}
void send_icmp_other(void)
{
char *packet, *data, *ph_buf;
struct myicmphdr *icmp;
struct myiphdr icmp_ip;
struct myudphdr *icmp_udp;
int udp_data_len = 0;
struct pseudohdr *pseudoheader;
int left_space = IPHDR_SIZE + UDPHDR_SIZE + data_size;
packet = malloc(ICMPHDR_SIZE + IPHDR_SIZE + UDPHDR_SIZE + data_size);
ph_buf = malloc(PSEUDOHDR_SIZE + UDPHDR_SIZE + udp_data_len);
if (packet == NULL || ph_buf == NULL) {
perror("[send_icmp] malloc");
return;
}
memset(packet, 0, ICMPHDR_SIZE + IPHDR_SIZE + UDPHDR_SIZE + data_size);
memset(ph_buf, 0, PSEUDOHDR_SIZE + UDPHDR_SIZE + udp_data_len);
icmp = (struct myicmphdr*) packet;
data = packet + ICMPHDR_SIZE;
pseudoheader = (struct pseudohdr *) ph_buf;
icmp_udp = (struct myudphdr *) (ph_buf + PSEUDOHDR_SIZE);
/* fill icmp hdr */
icmp->type = opt_icmptype; /* ICMP_TIME_EXCEEDED */
icmp->code = opt_icmpcode; /* should be 0 (TTL) or 1 (FRAGTIME) */
icmp->checksum = 0;
if (opt_icmptype == ICMP_REDIRECT)
memcpy(&icmp->un.gateway, &icmp_gw.sin_addr.s_addr, 4);
else
icmp->un.gateway = 0; /* not used, MUST be 0 */
/* concerned packet headers */
/* IP header */
icmp_ip.version = icmp_ip_version; /* 4 */
icmp_ip.ihl = icmp_ip_ihl; /* IPHDR_SIZE >> 2 */
icmp_ip.tos = icmp_ip_tos; /* 0 */
icmp_ip.tot_len = htons((icmp_ip_tot_len ? icmp_ip_tot_len : (icmp_ip_ihl<<2) + UDPHDR_SIZE + udp_data_len));
icmp_ip.id = htons(getpid() & 0xffff);
icmp_ip.frag_off = 0; /* 0 */
icmp_ip.ttl = 64; /* 64 */
icmp_ip.protocol = icmp_ip_protocol; /* 6 (TCP) */
icmp_ip.check = 0;
memcpy(&icmp_ip.saddr, &icmp_ip_src.sin_addr.s_addr, 4);
memcpy(&icmp_ip.daddr, &icmp_ip_dst.sin_addr.s_addr, 4);
icmp_ip.check = cksum((__u16 *) &icmp_ip, IPHDR_SIZE);
/* UDP header */
memcpy(&pseudoheader->saddr, &icmp_ip_src.sin_addr.s_addr, 4);
memcpy(&pseudoheader->daddr, &icmp_ip_dst.sin_addr.s_addr, 4);
pseudoheader->protocol = icmp_ip.protocol;
pseudoheader->lenght = icmp_ip.tot_len;
icmp_udp->uh_sport = htons(icmp_ip_srcport);
icmp_udp->uh_dport = htons(icmp_ip_dstport);
icmp_udp->uh_ulen = htons(UDPHDR_SIZE + udp_data_len);
icmp_udp->uh_sum = cksum((__u16 *) ph_buf, PSEUDOHDR_SIZE + UDPHDR_SIZE + udp_data_len);
/* filling icmp body with concerned packet header */
/* fill IP */
if (left_space == 0) goto no_space_left;
memcpy(packet+ICMPHDR_SIZE, &icmp_ip, left_space);
left_space -= IPHDR_SIZE;
data += IPHDR_SIZE;
if (left_space <= 0) goto no_space_left;
/* fill UDP */
memcpy(packet+ICMPHDR_SIZE+IPHDR_SIZE, icmp_udp, left_space);
left_space -= UDPHDR_SIZE;
data += UDPHDR_SIZE;
if (left_space <= 0) goto no_space_left;
/* fill DATA */
data_handler(data, left_space);
no_space_left:
/* icmp checksum */
if (icmp_cksum == -1)
icmp->checksum = cksum((u_short*)packet, ICMPHDR_SIZE + IPHDR_SIZE + UDPHDR_SIZE + data_size);
else
icmp->checksum = icmp_cksum;
/* send packet */
send_ip_handler(packet, ICMPHDR_SIZE + IPHDR_SIZE + UDPHDR_SIZE + data_size);
free (packet);
free (ph_buf);
}
int start_server(void (*data_handler)(const char *buf, const int len, const int socket_fd), int port_number)
{
int num_of_fd = 0;
struct pollfd fds[MAX_CONN];
int len, socket_fd, tmp = 0;
struct sockaddr_in sock;
memset(fds, 0, sizeof(fds));
memset(&sock, 0, sizeof(sock));
if((socket_fd = socket(AF_INET, SOCK_STREAM, 0)) < 0)
{
log_msg("socket() call failed");
exit(1);
}
port_number = (0 == port_number) ? DEFAULT_PORT : port_number;
sock.sin_family = AF_INET;
sock.sin_port = htons(port_number);
len = INADDR_ANY;
memset(&sock.sin_addr, len, sizeof(struct in_addr));
if(bind(socket_fd, (struct sockaddr *) &sock, sizeof(struct sockaddr_in)) < 0)
{
printf("bind() call failed\n");
close(socket_fd);
exit(1);
}
if(setsockopt(socket_fd, SOL_SOCKET, SO_REUSEADDR, &tmp, sizeof(int)) < 0)
{
printf("setsockopt call failed\n");
}
if(listen(socket_fd, 10) < 0)
{
printf("listen() call failed\n");
close(socket_fd);
exit(1);
}
log_error("listening on port %d", port_number);
fds[0].fd = socket_fd;
fds[0].events = POLLIN;
num_of_fd++;
while(1)
{
int num_processed = 0;
int ret = poll(fds, num_of_fd, TIMEOUT);
if(ret == POLL_EXPIRE)
{
}
else if(ret == POLL_ERR)
{
printf("Error in POLL aborting %s\n", strerror(errno));
break;
}
else
{
if(fds[0].revents & POLLIN)
{
socklen_t len = sizeof(struct sockaddr_in);
int new_socket = accept(socket_fd, (struct sockaddr *)&sock, &len);
if(num_of_fd < MAX_CONN)
{
fds[num_of_fd].fd = new_socket;
fds[num_of_fd].events = POLLIN;
fds[num_of_fd].revents = 0;
num_of_fd++;
log_msg("new incoming connection");
}
else
{
log_msg("ignoring new connections as only 1024 simulatenous connections are supported");
}
num_processed++;
}
for(int i = 1; (i < num_of_fd) && (num_processed < ret); i++)
{
num_processed++;
if(fds[i].revents & POLLIN)
{
char buf[1024];
int len;
len = read(fds[i].fd, buf, 1024);
if(len == 0)
{
close(fds[i].fd);
log_msg("fd %d closed I guess %d", fds[i].fd, num_of_fd);
if(i != (num_of_fd-1))
{
memcpy(&fds[i], &fds[num_of_fd-1], sizeof(struct pollfd));
i--;
}
num_of_fd--;
}
else
{
data_handler(buf, len, fds[i].fd);
}
}
else if(fds[i].revents & POLLERR || (fds[i].revents & POLLNVAL))
{
log_msg("fd = %d POLLERR|POLLNVAL", fds[i].fd);
close(fds[i].fd);
if(i < (num_of_fd-1))
{
memcpy(&fds[i], &fds[num_of_fd-1], sizeof(struct pollfd));
i--;
}
num_of_fd--;
}
else if(fds[i].revents != 0)
{
log_msg("fd = %d UNKNOWN REVENT : %#x", fds[i].fd, fds[i].revents);
}
else
num_processed--;
}
}
}
close(socket_fd);
return 0;
}
/*!
******************************************************************************
* @brief Decode byte per byte for each binary frame, being sensor data themselves
* Incoming format is always sensorid timestamp fields
* Output format will always be sensorname, timestamp, fields formated
* @param[in] data pointer to next incoming token to be parsed
* @param[in] tokenId token ID in the binary frame, start from 0
* @param[inout] bytesEaten the length of the frame analyzed, incremented based on fields length decoded
* @return 0 keeps on processing next token / -1 end of frame
******************************************************************************
*/
static int decode_one_binary_token(struct inv_icm30xxx * s, char * data, unsigned char tokenId ,unsigned short *bytesEaten)
{
unsigned char lIndx = 0;
// this is sensorid field
if ( tokenId == 0)
{
// parse decode_Frame array to check sensor_id is known
do
{
if ( *data == decode_Frame[lIndx].sensor_id )
{
// sensor found, print it on console
s->response.sensorid = lIndx;
// number of bytes analyzed for this is 1 char
*bytesEaten = 1;
// keep on processing current frame
return 0;
}
lIndx++;
} while (decode_Frame[lIndx].sensor_id != 0xFF );
// we reached end of decode_Frame array, this is an error, sensor id is unknown
// stop processing current frame this has no sense
return -1;
}
// this is timestamp field
else if ( tokenId == 1)
{
// print timestamp on console
unsigned long long int timestamp_64b = *( unsigned long long int*) data;
s->response.payload.sensor_data.timestamp = (uint32_t) timestamp_64b;
// number of bytes analyzed for this is 1 long long
*bytesEaten += 8;
}
// this is payload data fields
else
{
switch ( decode_Frame[s->response.sensorid].decodeFormat[tokenId-2] )
{
case 'f' :
// print float on console
memcpy(&s->response.payload.sensor_data.data[4*(tokenId-2)], data, 4);
// number of bytes analyzed for this is 1 float
*bytesEaten += 4;
break;
case 'i' :
// print integer on console
memcpy(&s->response.payload.sensor_data.data[4*(tokenId-2)], data, 4);
// number of bytes analyzed for this is 1 int
*bytesEaten += 4;
break;
case 'F' :
// print float on console together with carriage return
memcpy(&s->response.payload.sensor_data.data[4*(tokenId-2)], data, 4);
// number of bytes analyzed for this is 1 int
*bytesEaten += 4;
s->response.payload.sensor_data.len = *bytesEaten;
/* signal the response */
s->response.event = true;
// and end processing frame
data_handler(s, decode_Frame[s->response.sensorid].sensor_id,
s->response.payload.sensor_data.timestamp,
(const uint8_t *)s->response.payload.sensor_data.data,
*bytesEaten);
return -1;
case 'I' :
// print integer on console together with carriage return
memcpy(&s->response.payload.sensor_data.data[4*(tokenId-2)], data, 4);
// number of bytes analyzed for this is 1 int
*bytesEaten += 4;
s->response.payload.sensor_data.len = *bytesEaten;
/* signal the response */
s->response.event = true;
// and end processing frame
data_handler(s, decode_Frame[s->response.sensorid].sensor_id,
s->response.payload.sensor_data.timestamp,
(const uint8_t *)s->response.payload.sensor_data.data,
*bytesEaten);
return -1;
default:
// this is a 0 or unknown field, so end processing frame
return -1;
break;
}
}
return 0;
}
static void tcp_recv_handler(struct mbuf *mb_pkt, void *arg)
{
struct allocation *alloc = arg;
int err = 0;
/* re-assembly of fragments */
if (alloc->mb) {
size_t pos;
pos = alloc->mb->pos;
alloc->mb->pos = alloc->mb->end;
err = mbuf_write_mem(alloc->mb,
mbuf_buf(mb_pkt), mbuf_get_left(mb_pkt));
if (err)
goto out;
alloc->mb->pos = pos;
}
else {
alloc->mb = mem_ref(mb_pkt);
}
for (;;) {
size_t len, pos, end;
struct sa src;
uint16_t typ;
if (mbuf_get_left(alloc->mb) < 4)
break;
typ = ntohs(mbuf_read_u16(alloc->mb));
len = ntohs(mbuf_read_u16(alloc->mb));
if (typ < 0x4000)
len += STUN_HEADER_SIZE;
else if (typ < 0x8000)
len += 4;
else {
err = EBADMSG;
goto out;
}
alloc->mb->pos -= 4;
if (mbuf_get_left(alloc->mb) < len)
break;
pos = alloc->mb->pos;
end = alloc->mb->end;
alloc->mb->end = pos + len;
/* forward packet to TURN client */
err = turnc_recv(alloc->turnc, &src, alloc->mb);
if (err)
goto out;
if (mbuf_get_left(alloc->mb)) {
data_handler(alloc, &src, alloc->mb);
}
/* 4 byte alignment */
while (len & 0x03)
++len;
alloc->mb->pos = pos + len;
alloc->mb->end = end;
if (alloc->mb->pos >= alloc->mb->end) {
alloc->mb = mem_deref(alloc->mb);
break;
}
}
out:
if (err) {
alloc->alloch(err, 0, NULL, NULL, NULL, alloc->arg);
}
}
static void udp_recv(const struct sa *src, struct mbuf *mb, void *arg)
{
struct allocation *alloc = arg;
data_handler(alloc, src, mb);
}
static int parse_fifo(struct inv_icm30xxx * s, const uint8_t * buffer, uint32_t len)
{
int rc = 0;
struct FifoProtocolPacket packet;
uint32_t index = 0;
int32_t remaining = (int32_t)len;
inv_fifo_protocol_parse_packet_reset(&packet);
packet.type = FIFOPROTOCOL_PACKET_TYPE_DATA;
while(remaining >= FIFOPROTOCOL_PACKET_SIZE) {
/* check header / footer */
if(buffer[index] != FIFOPROTOCOL_PROTECTED_HEADER
|| buffer[index+FIFOPROTOCOL_PACKET_SIZE-1] != FIFOPROTOCOL_PROTECTED_FOOTER) {
INV_MSG(INV_MSG_LEVEL_VERBOSE, "Bad header/footer in fifo packet, dropping one byte...");
index += 1;
remaining -= 1;
continue;
}
/* header/footer ok - parse packet */
rc = inv_fifo_protocol_parse_packet_r(&packet, &buffer[index],
FIFOPROTOCOL_PACKET_SIZE, s->fifop_exp_pkt_cnt);
/* retry if parsing error with last packet */
if(rc == FIFOPROTOCOL_ERROR_PARSE) {
INV_MSG(INV_MSG_LEVEL_VERBOSE, "Error parsing fifo packet. One packet lost."
" Retrying with current packet...");
rc = inv_fifo_protocol_parse_packet_r(&packet, &buffer[index],
FIFOPROTOCOL_PACKET_SIZE, s->fifop_exp_pkt_cnt);
}
/* invalid packet, skip it */
if(rc == FIFOPROTOCOL_ERROR_SIZE || rc == FIFOPROTOCOL_ERROR_PARSE) {
INV_MSG(INV_MSG_LEVEL_VERBOSE, "Error parsing fifo packet. Dropping full packet...");
}
/* valid packet, call handlers to notify upper layer */
else if(rc == FIFOPROTOCOL_ERROR_NO) {
if(packet.type == FIFOPROTOCOL_PACKET_TYPE_ANSWER) {
response_handler(s, packet.sensorid,
(FifoProtocolCmd)packet.u.command.cmd,
packet.arg, packet.size);
} else {
data_handler(s, packet.sensorid,
packet.u.data.timestamp, packet.u.data.status,
packet.u.data.accuracy, packet.arg, packet.size);
}
}
else {
/* incomplete packet */
}
/* parse next packet */
index += FIFOPROTOCOL_PACKET_SIZE;
remaining -= FIFOPROTOCOL_PACKET_SIZE;
}
/* If we did not receive all packets needed for current sensor id to be fully parsed, then make
sure the already analyzed packet(s) will be part of next call to this function */
if (rc == FIFOPROTOCOL_ERROR_INCOMPLETE) {
// INV_MSG(INV_MSG_LEVEL_DEBUG, "Incomplete fifo packet, rewind fifo index.");
remaining += packet.states.packet_cnt * FIFOPROTOCOL_PACKET_SIZE;
}
/* return number of remaining bytes from input buffer */
return remaining;
}
