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 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(thr_msg)       (struct ether_addr*)(&thr_msg[THR_DST_ADR_POS])
#define THR_MSG_SRC_ADR(thr_msg)       (struct ether_addr*)(&thr_msg[THR_SRC_ADR_POS])
#define THR_MSG_ETH_TYP(thr_msg)       *(unsigned short*)(&thr_msg[THR_ETH_TYP_POS])
#define THR_MSG_CHN_ID(thr_msg)        *(unsigned char*)(&thr_msg[THR_CHN_ID_POS])
#define THR_MSG_FRAG_NUM(thr_msg)      *(unsigned short*)(&thr_msg[THR_FRAG_NUM_POS])
#define THR_MSG_FRAG_TOT_NUM(thr_msg)  *(unsigned short*)(&thr_msg[THR_FRAG_TOT_NUM_POS])
#define THR_MSG_FRAG_LEN(thr_msg)      *(unsigned short*)(&thr_msg[THR_FRAG_LEN_POS])
#define THR_MSG_PAYLOAD(thr_msg)       (unsigned char*)(&thr_msg[THR_PAYLOAD_POS])


/* local type for the Throttle Channel structure */
struct thr_chn_t
{
	struct ether_addr dst_adr;         /* 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 char* p_msg;
	struct ether_addr last_sender_adr;/* src MAC address of last message */
};

/* Type for the Throttle message */
typedef struct
{
   struct ether_addr dst_adr;  /* destination MAC address  */
   struct ether_addr src_adr;  /* 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 struct ether_addr* 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;
      }
#ifdef DEBUG
      printf("thr_open_chn: callback = %x\n", tmp_chn->funct);
#endif

   }

   return(tmp_chn);
}

void thr_close_chn(struct thr_chn_t* c){
   free(c);
}

/**
* @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_msg), &thr_chn->dst_adr, sizeof(thr_chn->dst_adr)); /* Destiantion MAC Address  */
            eth_getMACadr(eth_int, THR_MSG_SRC_ADR(thr_msg));                        /* Source MAC Address       */
            THR_MSG_ETH_TYP(thr_msg) = htons(THROTTLENET_PROTO);                     /* Ethernet Packet Type     */
            THR_MSG_CHN_ID(thr_msg) = thr_chn->id;                                   /* Channel identification   */
            THR_MSG_FRAG_TOT_NUM(thr_msg) = ((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(thr_msg); i++)
            {
               THR_MSG_FRAG_NUM(thr_msg) = i; /* fragment number */
               /* update the fragment length */
               if (length >= thr_chn->frag_size)
               {
                  THR_MSG_FRAG_LEN(thr_msg) = thr_chn->frag_size;
                  length -= thr_chn->frag_size;
               }
               else
               {
                  THR_MSG_FRAG_LEN(thr_msg) = length;
               }
               memcpy ((void*)THR_MSG_PAYLOAD(thr_msg), (void*)data, THR_MSG_FRAG_LEN(thr_msg));                /* update the payload */
               ret = eth_send(eth_int, &thr_msg[0], (THR_MSG_HEADER_LEN + THR_MSG_FRAG_LEN(thr_msg))); /* 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(thr_msg);
                  nanosleep(&thr_time, NULL);
               }
            }
            /*deallocate the memory */
            free(thr_msg);
            thr_msg = NULL;
         }
      }
   }
   return(ret);
}

int thr_receive(struct thr_chn_t* thr_chn, unsigned char* data, void* param)
{
   unsigned char* thr_msg = NULL;
   unsigned char* p_data = NULL;
   unsigned short frag_index = 1;
   short byte_rec = 0;
   unsigned char msg_received = 0;
   int ret;

   if ((NULL == thr_chn) || (NULL == data) || (NULL == thr_chn->funct) || (NULL == param))
   {
      ret = -1;
      errno = EINVAL;
      DISPLAY_ERR("Input parameter(s) NULL");
   }
   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
      {
         p_data = data;
         do
         {
            byte_rec = eth_receive(eth_int, thr_msg, (THR_MSG_HEADER_LEN + thr_chn->frag_size));  /* receive the Ethernet Raw packet */
            if (-1 == byte_rec)
            {
               /* discard the message */
               DISPLAY_ERR("Error during Throttle msg reception: Fragment discarded.");
            }
            else
            {
               /* check if the Ethernet Raw message is correct (Ethernet protocol, Channel ID, length) */
               ret = thr_msg_check(thr_msg, thr_chn->id, byte_rec);
               if (ret < 0)
               {
                  //printf("Throttle msg mismatch: Fragment discarded.\n");
               }
               else
               {
#ifdef DEBUG
                  printf("thr_receive: Message Index %d on %d. Actual Index %d\n", THR_MSG_FRAG_NUM(thr_msg), THR_MSG_FRAG_TOT_NUM(thr_msg), frag_index);
#endif
                  if (frag_index == THR_MSG_FRAG_NUM(thr_msg)) /* The fragment is the one expected ? */
                  {
                     /* Rebuild the original data linking the payloads of each fragment */
                     memcpy((void*)p_data, (void*)THR_MSG_PAYLOAD(thr_msg), THR_MSG_FRAG_LEN(thr_msg));
                     p_data += THR_MSG_FRAG_LEN(thr_msg);  /* update the pointer to the buffer   */
                     frag_index++;                /* update the fragment index          */
                     ret = p_data - data;         /* update the number of received byte */
                     if (frag_index > THR_MSG_FRAG_TOT_NUM(thr_msg))
                     {
                        msg_received = 1;
                     }
                  }
                  else
                  {
                     printf("thr_receive: Fragment mismatch: Fragment discarded.\n");
                     frag_index = 1;
                     p_data = data;
                  }
               }
            }
         }while (msg_received != 1);

         thr_chn->p_msg = (unsigned char*)malloc(ret);
         if (NULL == thr_chn->p_msg)
         {
            ret = -1;
            DISPLAY_ERR("Error allocating memory!");
         }
         else
         {
#ifdef DEBUG
            printf("thr_receive: Number of byte receive %d\n",ret);
#endif
            thr_chn->msg_length = ret;
            memcpy(thr_chn->p_msg, data, ret); /* copy the msg into the thr structure */
            memcpy(&thr_chn->last_sender_adr, THR_MSG_SRC_ADR(thr_msg), 6); /* ... and the address of the sender */
#ifdef DEBUG
            printf("thr_receive: calling %x\n", thr_chn->funct);
#endif
            (thr_chn->funct)(param);
            free(thr_chn->p_msg);
            thr_chn->p_msg = NULL;
         }

         /* deallocate memory for the Throttle Message */
         free(thr_msg);
         thr_msg = NULL;
         p_data = NULL;
      }
   }
   return(ret);
}


static int thr_msg_check(const unsigned char* thr_msg, unsigned short chn_id, unsigned short length)
{
   int ret = 0;

   if (length > THR_MSG_HEADER_LEN)                    /* Ethernet Raw Packet contains a valid Payload ? */
   {
      if (THROTTLENET_PROTO == ntohs(THR_MSG_ETH_TYP(thr_msg))) /* Is Ethernet Type THROTTLENET Protocol ?        */
      {
         if (THR_MSG_CHN_ID(thr_msg) == chn_id)                 /* Is Channel identification correct ?            */
         {
            /* throttle message is correct */
         }
         else
         {
            ret = -1;
         }
      }
      else
      {
         /* discard the message */
         ret = -2;
      }
   }
   else
   {
      /* discard the message */
      ret = -3;
   }
   return(ret);
}


int thr_read(struct thr_chn_t* thr_chn, unsigned char* data, int length)
{
   int ret = 0;

   if ((NULL == thr_chn) || (NULL == data))
   {
      ret = -1;
      errno = EINVAL;
      DISPLAY_ERR("Input parameter(s) NULL");
   }
   else
   {
      if (length > thr_chn->msg_length)
      {
         length = thr_chn->msg_length;
#ifdef DEBUG
         printf("thr_read: truncating length to %d\n", length);
#endif
      }
#ifdef DEBUG
      printf("thr_read: calling memcpy(%x, %x, %d\n", data, thr_chn->p_msg, length);
#endif
      memcpy(data, thr_chn->p_msg, length); /* copy the msg into the thr structure */
      ret = length;
      thr_chn->msg_length = 0;
   }
   return(ret);
}

struct ether_addr* get_sender_addr(struct thr_chn_t* ch) {
	return &ch->last_sender_adr;
}

unsigned char get_channel(struct thr_chn_t* ch) {
	return ch->id;
}