Commit 35f90c3d authored by Riccardo Gaiati's avatar Riccardo Gaiati
Browse files

working ethernet test

parent d00f2951
......@@ -4,6 +4,9 @@ import java.io.OutputStream;
import se.lth.control.labcomm.LabCommEncoderChannel;
/**
* Simple encoder
*/
public class Encoder
{
......@@ -35,7 +38,7 @@ public class Encoder
public static void main(String[] arg) throws Exception {
FileOutputStream fos = new FileOutputStream(new File(arg[0]));
FileOutputStream fos = new FileOutputStream(arg[0]);
Encoder example = new Encoder(fos);
example.doEncode();
fos.close();
......
......@@ -7,3 +7,8 @@ sample struct {
int x;
string s;
} IntString;
sample struct {
double x;
double y;
} TwoDoubles;
gcc -o thr_encoder -L ../../lib/c -I . -I ../../lib/c thr_encoder.c gen/simple.c ../../lib/c/labcomm_thr_reader_writer.c ../../lib/c/ThrottleDrv/*.c -llabcomm
gcc -o thr_decoder -L ../../lib/c -I . -I ../../lib/c thr_decoder.c gen/simple.c ../../lib/c/labcomm_thr_reader_writer.c ../../lib/c/ThrottleDrv/*.c -llabcomm
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <labcomm_thr_reader_writer.h>
#include "gen/simple.h"
#include "ThrottleDrv/throttle_drv.h"
static void handle_simple_TwoInts(simple_TwoInts *v,void *context) {
printf("Got TwoInts. a=%d, b=%d\n", v->a, v->b);
}
static void handle_simple_IntString(simple_IntString *v,void *context) {
printf("Got IntString. x=%d, s=%s\n", v->x, v->s);
}
int main(int argc, char *argv[]) {
struct thr_chn_t *p_thr_chn = NULL;
struct labcomm_decoder *decoder;
void *context = NULL;
int ret = 0;
unsigned char dest_mac[ETH_ADR_SIZE] = {0x06, 0x05, 0x04, 0x03, 0x02, 0x01}; /* other host MAC address, hardcoded...... :-( */
unsigned char chn_id = 0x01;
unsigned short frag_size = 60;
unsigned short freq = 1000; /* milliseconds */
unsigned char data[200];
char *filename = argv[1];
if (-1 == thr_init("eth2"))
{
printf("Throttle Init failure.");
}
else
{
p_thr_chn = thr_open_chn(dest_mac, chn_id, frag_size, freq, labcomm_decoder_decode_one);
decoder = labcomm_decoder_new(labcomm_thr_reader, p_thr_chn);
if (!decoder)
{
printf("Failed to allocate decoder %s:%d\n", __FUNCTION__, __LINE__);
return 1;
}
labcomm_decoder_register_simple_TwoInts(decoder, handle_simple_TwoInts, context);
labcomm_decoder_register_simple_IntString(decoder, handle_simple_IntString, context);
printf("Decoding:\n");
do {
ret = thr_receive(p_thr_chn, data, decoder);
} while(-1 != ret);
{
printf("Throttle Send receive failure.");
}
printf("--- End Of File ---:\n");
labcomm_decoder_free(decoder);
//server_exit(fd);
}
}
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <labcomm_thr_reader_writer.h>
#include "gen/simple.h"
#include "ThrottleDrv/throttle_drv.h"
int main(int argc, char *argv[]) {
struct thr_chn_t *p_thr_chn = NULL;
struct labcomm_encoder *encoder;
int i, j;
unsigned char dest_mac[ETH_ADR_SIZE] = {0x00, 0x09, 0x6b, 0x10, 0xf3, 0x80}; /* other host MAC address, hardcoded...... :-( */
unsigned char chn_id = 0x01;
unsigned short frag_size = 60;
unsigned short freq = 1000; /* milliseconds */
char *filename = argv[1];
if (-1 == thr_init("eth0"))
{
printf("Throttle Init failure.");
}
else
{
p_thr_chn = thr_open_chn(dest_mac, chn_id, frag_size, freq, NULL);
encoder = labcomm_encoder_new(labcomm_thr_writer, p_thr_chn);
labcomm_encoder_register_simple_TwoInts(encoder);
labcomm_encoder_register_simple_IntString(encoder);
simple_IntString is;
is.x = 24;
is.s = "Hello, LabComm!";
printf("Encoding IntString, x=%d, s=%s\n", is.x, is.s);
labcomm_encode_simple_IntString(encoder, &is);
simple_TwoInts ti;
ti.a = 13;
ti.b = 37;
printf("Encoding TwoInts, a=%d, b=%d\n", ti.a, ti.b);
labcomm_encode_simple_TwoInts(encoder, &ti);
//client_exit(fd);
}
}
#define PC_MODE
#ifdef PC_MODE
#include <stdio.h>
#define DISPLAY_ERR(s) perror(s);
#else
#define DISPLAY_ERR(s) ;
#endif
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <stdio.h>
#include <errno.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <linux/if_ether.h>
#include <linux/if_packet.h>
#include <linux/if_arp.h>
#include <netinet/in.h>
#include "ethernet_drv.h"
#include "display.h"
/** LOCAL FUNCTIONS **/
/* local type for the ethernet interface */
struct eth_int_t
{
unsigned char mac_adr[ETH_ALEN]; /* MAC address */
int socket_id; /* socket file descriptor */
int index; /* index of the eth interface */
unsigned char validity;
};
/***
Open an Ethernet Raw packet connection
***/
struct eth_int_t* eth_open(const char* eth_int_name)
{
struct eth_int_t* tmp_eth = NULL; /* pointer to ethernet interface struct */
struct ifreq ifr; /* struct used by ioctl function */
/* TO-DO: check if a ethernet socket has already been created for eth_int_name interface */
/* Ethernet interface name pointer valid ? */
if (NULL == eth_int_name)
{
errno = EINVAL;
DISPLAY_ERR("Input parameter NULL");
}
else
{
/* allocate memory for the Ethernet Interface */
tmp_eth = (struct eth_int_t*)malloc(sizeof(struct eth_int_t));
if (NULL == tmp_eth)
{
DISPLAY_ERR("Error allocating memory!");
}
else
{
/* creates a Packet Socket, raw delivery, accept all protocol */
tmp_eth->socket_id = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
if (-1 == tmp_eth->socket_id) /* error occurred during socket creation ? */
{
free(tmp_eth); /* deallocate the memory */
tmp_eth = NULL;
DISPLAY_ERR("Error in creating socket");
}
else
{
/* retrieve the Ethernet card MAC address */
strcpy(ifr.ifr_name, eth_int_name);
if (ioctl(tmp_eth->socket_id, SIOCGIFHWADDR, &ifr) == -1) /* error during the retrieve of MAC address ? */
{
close(tmp_eth->socket_id); /* close the socket */
free(tmp_eth); /* deallocate the memory */
tmp_eth = NULL;
DISPLAY_ERR("ioctl error SIOCGIFHWADDR");
}
else
{
/* copy the MAC address into the eth interface struct */
memcpy(tmp_eth->mac_adr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
/*retrieve the Ethernet interface index*/
if (ioctl(tmp_eth->socket_id, SIOCGIFINDEX, &ifr) == -1)/* error during the retrieve of index */
{
close(tmp_eth->socket_id); /* close the socket */
free(tmp_eth); /* deallocate the memory */
tmp_eth = NULL;
DISPLAY_ERR("ioctl error SIOCGIFINDEX");
}
else
{
/* copy the interface index into the eth interface struct */
tmp_eth->index = ifr.ifr_ifindex;
printf("Successfully got our MAC address: %02X:%02X:%02X:%02X:%02X:%02X\n",
tmp_eth->mac_adr[0],tmp_eth->mac_adr[1],tmp_eth->mac_adr[2],tmp_eth->mac_adr[3],tmp_eth->mac_adr[4],tmp_eth->mac_adr[5]);
printf("Successfully got interface index for %s: %i\n",eth_int_name, tmp_eth->index);
}
}
}
}
}
return(tmp_eth);
}
/***
Close the Ethernet Raw packet connection
***/
int eth_close(struct eth_int_t* eth_int)
{
int ret = 0;
/* Ethernet interface pointer valid ? */
if (NULL == eth_int)
{
ret = -1;
errno = EINVAL;
DISPLAY_ERR("Input parameter NULL");
}
else
{
if (-1 == close(eth_int->socket_id)) /* error during the closure of the socket ? */
{
ret = -1;
DISPLAY_ERR("Socket closure error");
}
else
{
free(eth_int); /* deallocate the memory */
eth_int = NULL;
}
}
return(ret);
}
/***
Returns the MAC address of the Ethernet Interface
***/
int eth_getMACadr(const struct eth_int_t* eth_int, unsigned char* mac_adr)
{
int ret = 0;
if ((NULL == eth_int) || (NULL == mac_adr)) /* Input parameters valid ? */
{
ret = -1;
errno = EINVAL;
DISPLAY_ERR("Input parameter(s) NULL");
}
else
{
/* TO-DO: introduce a validity flag to be check before accessing to eth_int (better a crc) */
memcpy(mac_adr, eth_int->mac_adr, ETH_ALEN);
}
return(ret);
}
/***
send a complete Ethernet Raw Packet
***/
int eth_send(const struct eth_int_t* eth_int, const unsigned char* eth_frame, unsigned short length)
{
struct ethhdr *eth_header = NULL; /* Pointer to Ethernet frame header (Dest,Source,Type) */
struct sockaddr_ll socket_address;
int ret;
if ((NULL == eth_int) || (NULL == eth_frame))
{
ret = -1;
errno = EINVAL;
DISPLAY_ERR("Input parameter(s) Null!");
}
else
{
eth_header = (struct ethhdr*)eth_frame;
/*prepare sockaddr_ll (address structure for PACKET_SOCKET) */
socket_address.sll_family = AF_PACKET;
socket_address.sll_protocol = htons(ETH_P_IP); /* Physical layer protocol */
socket_address.sll_ifindex = eth_int->index; /* Ethernet Interface index */
socket_address.sll_hatype = ARPHRD_ETHER; /* ARP hardware identifier: Ethernet */
socket_address.sll_pkttype = PACKET_OTHERHOST; /* Packet type: Another host */
socket_address.sll_halen = ETH_ALEN; /* Length of the MAC address */
socket_address.sll_addr[0] = eth_header->h_dest[0];
socket_address.sll_addr[1] = eth_header->h_dest[1];
socket_address.sll_addr[2] = eth_header->h_dest[2];
socket_address.sll_addr[3] = eth_header->h_dest[3];
socket_address.sll_addr[4] = eth_header->h_dest[4];
socket_address.sll_addr[5] = eth_header->h_dest[5];
socket_address.sll_addr[6] = 0x00; /* not used */
socket_address.sll_addr[7] = 0x00; /* not used */
/*send the Ethernet frame */
ret = sendto(eth_int->socket_id, eth_frame, length, 0, (struct sockaddr*)&socket_address, sizeof(socket_address));
if (ret == -1) /* error during the trasmission of the Ethernet frame ? */
{
DISPLAY_ERR("sendto error!");
}
}
return(ret);
}
/***
receive a complete Ethernet Raw Packet
***/
int eth_receive (const struct eth_int_t* eth_int, unsigned char* eth_frame, unsigned short length)
{
int ret;
if ((NULL == eth_int) || (NULL == eth_frame))
{
ret = -1;
errno = EINVAL;
DISPLAY_ERR("Input parameter(s) Null!");
}
else
{
/*Wait for incoming packet...*/
ret = recvfrom(eth_int->socket_id, (void*)eth_frame, length, 0, NULL, NULL);
if (ret == -1) /* error during the reception of the Ethernet frame ? */
{
DISPLAY_ERR("recvfrom error!");
}
}
return(ret);
}
#ifndef _ETHERNET_DRV_H_
#define _ETHERNET_DRV_H_
struct eth_int_t;
struct eth_int_t* eth_open(const char* eth_int_name);
int eth_close(struct eth_int_t* eth_int);
int eth_getMACadr(const struct eth_int_t* eth_int, unsigned char* mac_adr);
int eth_send(const struct eth_int_t* eth_int, const unsigned char* eth_frame, unsigned short length);
int eth_receive (const struct eth_int_t* eth_int, unsigned char* eth_frame, unsigned short length);
#endif
#include "throttle_drv.h"
#include "ethernet_drv.h"
#include <errno.h>
#include "display.h"
#include "stdlib.h"
#include <string.h>
#include <time.h>
#include <arpa/inet.h>
#define THROTTLENET_PROTO 0x544e
#define ETH_ADR_LEN 6
#define THR_DST_ADR_POS 0
#define THR_SRC_ADR_POS (THR_DST_ADR_POS + sizeof(((thr_header_t*)0)->dst_adr)) //6
#define THR_ETH_TYP_POS (THR_SRC_ADR_POS + sizeof(((thr_header_t*)0)->src_adr)) //12
#define THR_CHN_ID_POS (THR_ETH_TYP_POS + sizeof(((thr_header_t*)0)->eth_type)) //14
#define THR_FRAG_NUM_POS (THR_CHN_ID_POS + sizeof(((thr_header_t*)0)->chn_id)) //15
#define THR_FRAG_TOT_NUM_POS (THR_FRAG_NUM_POS + sizeof(((thr_header_t*)0)->frag_num)) //17
#define THR_FRAG_LEN_POS (THR_FRAG_TOT_NUM_POS + sizeof(((thr_header_t*)0)->frag_num_tot)) //19
#define THR_PAYLOAD_POS (THR_FRAG_LEN_POS + sizeof(((thr_header_t*)0)->frag_len)) //21
#define THR_MSG_HEADER_LEN THR_PAYLOAD_POS
#define THR_MSG_DST_ADR (unsigned char*)(&thr_msg[THR_DST_ADR_POS])
#define THR_MSG_SRC_ADR (unsigned char*)(&thr_msg[THR_SRC_ADR_POS])
#define THR_MSG_ETH_TYP *(unsigned short*)(&thr_msg[THR_ETH_TYP_POS])
#define THR_MSG_CHN_ID *(unsigned char*)(&thr_msg[THR_CHN_ID_POS])
#define THR_MSG_FRAG_NUM *(unsigned short*)(&thr_msg[THR_FRAG_NUM_POS])
#define THR_MSG_FRAG_TOT_NUM *(unsigned short*)(&thr_msg[THR_FRAG_TOT_NUM_POS])
#define THR_MSG_FRAG_LEN *(unsigned short*)(&thr_msg[THR_FRAG_LEN_POS])
#define THR_MSG_PAYLOAD (unsigned char*)(&thr_msg[THR_PAYLOAD_POS])
/* local type for the Throttle Channel structure */
struct thr_chn_t
{
unsigned char dst_adr[ETH_ADR_LEN];/* destination MAC address */
unsigned char id; /* Channel id */
unsigned short frag_size; /* Fragment size */
unsigned short freq; /* Message transmission frequency */
thr_msg_handler_t funct; /* Callback function invoked at the reception */
unsigned short msg_length;
unsigned short* p_msg;
};
/* Type for the Throttle message */
typedef struct
{
unsigned char dst_adr[ETH_ADR_LEN]; /* destination MAC address */
unsigned char src_adr[ETH_ADR_LEN]; /* source MAC address */
unsigned short eth_type; /* ethernet packet type */
unsigned char chn_id; /* channel identification */
unsigned short frag_num; /* fragment number */
unsigned short frag_num_tot; /* total fragment number */
unsigned short frag_len; /* fragment length */
}thr_header_t;
/* Local functions */
static int thr_msg_check(const unsigned char* thr_msg, unsigned short chn_id, unsigned short length);
static struct eth_int_t* eth_int = NULL;
/**
* @fn int thr_init(const char* eth_int_name)
*
* @brief Initializes the Throttle Net Driver, enabling the Ethernet Raw communication for the eth_int_name interface.
*
* @param [in] eth_int_name: ethernet interface name (e.g. "eth0","eth1")
*
* @retval int: 0 on success; -1 on error and errno is set appropriately
*
*******************************************************************************/
int thr_init(const char* eth_int_name)
{
int ret = 0;
if (NULL == eth_int_name)
{
ret = -1;
errno = EINVAL;
DISPLAY_ERR("Input parameter NULL");
}
else
{
eth_int = eth_open(eth_int_name); /* open the Ethernet socket */
if (NULL == eth_int)
{
ret = -1;
}
}
return(ret);
}
/**
* @fn struct thr_chn_t* thr_open_chn(const char* dst_adr, unsigned char chn_id, unsigned char frag_size, unsigned short freq, const thr_msg_handler_t funct);
*
* @brief Open a logic channel
*
* @param [in] dst_adr: destination MAC address;
* @param [in] chn_id: identification of the channel;
* @param [in] frag_size: fragment size;
* @param [in] freq: frequency of trasmission for each fragment;
* @param [in] msg_handler: callback invoked when a message is completely received;
*
* @retval struct thr_chn_t*: pointer to the Channel structure. NULL on error and errno is set appropriately.
*
*******************************************************************************/
struct thr_chn_t* thr_open_chn(const unsigned char* dst_adr, unsigned char chn_id, unsigned char frag_size, unsigned short freq, const thr_msg_handler_t funct)
{
struct thr_chn_t* tmp_chn = NULL; /* pointer to Channel structure */
/* TO-DO: check if a similar channel has already been created */
/* Input parameters not valid ? */
if ((NULL == dst_adr) || (0 == frag_size))
{
errno = EINVAL;
DISPLAY_ERR("Input parameter(s) NULL");
}
else
{
/* allocate memory for the Channel structure */
tmp_chn = (struct thr_chn_t*)malloc(sizeof(struct thr_chn_t));
if (NULL == tmp_chn)
{
DISPLAY_ERR("Error allocating memory!");
}
else
{
/* Update the Channel structure */
memcpy(tmp_chn->dst_adr, dst_adr, sizeof(tmp_chn->dst_adr));
tmp_chn->id = chn_id;
tmp_chn->frag_size = frag_size;
tmp_chn->freq = freq;
tmp_chn->funct = funct;
}
printf("thr_open_chn: callback = %x\n", tmp_chn->funct);
}
return(tmp_chn);
}
/**
* @fn int thr_send(const struct thr_chn_t* thr_chn, const char* data, unsigned int length)
*
* @brief Sends a Throttle message. A channel must be opened first.
*
* @param [in] thr_chn: pointer to the Throttle channel descriptor;
* @param [in] data: data buffer to be sent;
* @param [in] length: length of the data buffer;
*
* @retval int: number of byte sent, on success. -1 on error and errno is set appropriately.
*
*******************************************************************************/
int thr_send(const struct thr_chn_t* thr_chn, const char* data, unsigned int length)
{
unsigned char* thr_msg;
unsigned char i;
int ret = 0;
/* Check the input parameters */
if ((NULL == thr_chn) || (NULL == data) || (0 == length))
{
ret = -1;
errno = EINVAL;
DISPLAY_ERR("Input parameter(s) NULL");
}
else
{
if (0 == thr_chn->frag_size)
{
ret = -1;
errno = ERANGE;
DISPLAY_ERR("Division by zero");
}
else
{
/* allocate memory for the Throttle Message */
thr_msg = (unsigned char*)malloc(THR_MSG_HEADER_LEN + thr_chn->frag_size);
if (NULL == thr_msg)
{
ret = -1;
DISPLAY_ERR("Error allocating memory!");
}
else
{
/* Compose the Ethernet Frame to be sent */
memcpy(THR_MSG_DST_ADR, &thr_chn->dst_adr[0], ETH_ADR_LEN); /* Destiantion MAC Address */
eth_getMACadr(eth_int, THR_MSG_SRC_ADR); /* Source MAC Address */
THR_MSG_ETH_TYP = htons(THROTTLENET_PROTO); /* Ethernet Packet Type */
THR_MSG_CHN_ID = thr_chn->id; /* Channel identification */
THR_MSG_FRAG_TOT_NUM = ((length - 1) / thr_chn->frag_size) + 1; /* Total number of fragment */
struct timespec thr_time;
thr_time.tv_sec = thr_chn->freq / 1000;
thr_time.tv_nsec = (thr_chn->freq % 1000) * 1000000;
/* Message is splitted into fragments and they are sent */
for (i = 1; i <= THR_MSG_FRAG_TOT_NUM; i++)
{
THR_MSG_FRAG_NUM = i; /* fragment number */
/* update the fragment length */
if (length >= thr_chn->frag_size)
{
THR_MSG_FRAG_LEN = thr_chn->frag_size;
length -= thr_chn->frag_size;
}
else
{
THR_MSG_FRAG_LEN = length;
}
memcpy ((void*)THR_MSG_PAYLOAD, (void*)data, THR_MSG_FRAG_LEN); /* update the payload */
ret = eth_send(eth_int, &thr_msg[0], (THR_MSG_HEADER_LEN + THR_MSG_FRAG_LEN)); /* send the message */
if (-1 == ret) /*Error during the Ethernet trasmission ? */
{
DISPLAY_ERR("Error during Throttle msg trasmission!");
break;
}
else
{
data += THR_MSG_FRAG_LEN;
nanosleep(&thr_time, NULL);
}
}
/*deallocate the memory */
free(thr_msg);
thr_msg = NULL;
}
}
}
return(ret);