working ethernet test

examples/simple_java/Encoder.java

@@ -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();

examples/simple_java/simple.lc

@@ -7,3 +7,8 @@ sample struct {
   int x;
   string s;
 } IntString;
+
+sample struct {
+  double x;
+  double y;
+} TwoDoubles;

examples/simple_java/thr_compile.sh

+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

examples/simple_java/thr_decoder.c

+#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);
+  }
+}

examples/simple_java/thr_encoder.c

+#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);
+ }
+}

lib/c/ThrottleDrv/display.h

+#define PC_MODE
+#ifdef PC_MODE
+#include <stdio.h>
+#define DISPLAY_ERR(s) 	perror(s);
+#else
+#define DISPLAY_ERR(s)  ;
+#endif

lib/c/ThrottleDrv/ethernet_drv.c

+#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);
+}
+
+
+

lib/c/ThrottleDrv/ethernet_drv.h

+#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

lib/c/ThrottleDrv/throttle_drv.c

+#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);