From 35f90c3d350100d85afca7cd18f27d7b07ebe6a1 Mon Sep 17 00:00:00 2001
From: Riccardo Gaiati <riccardo@cs.lth.se>
Date: Tue, 13 Dec 2011 13:43:51 +0100
Subject: [PATCH] working ethernet test
---
examples/simple_java/Encoder.java | 5 +-
examples/simple_java/simple.lc | 5 +
examples/simple_java/thr_compile.sh | 3 +
examples/simple_java/thr_decoder.c | 56 ++++
examples/simple_java/thr_encoder.c | 43 ++++
lib/c/ThrottleDrv/display.h | 7 +
lib/c/ThrottleDrv/ethernet_drv.c | 223 ++++++++++++++++
lib/c/ThrottleDrv/ethernet_drv.h | 12 +
lib/c/ThrottleDrv/throttle_drv.c | 385 ++++++++++++++++++++++++++++
lib/c/ThrottleDrv/throttle_drv.h | 17 ++
lib/c/labcomm.c | 76 +++---
lib/c/labcomm_thr_reader_writer.c | 141 ++++++++++
lib/c/labcomm_thr_reader_writer.h | 5 +
13 files changed, 941 insertions(+), 37 deletions(-)
create mode 100644 examples/simple_java/thr_compile.sh
create mode 100644 examples/simple_java/thr_decoder.c
create mode 100644 examples/simple_java/thr_encoder.c
create mode 100644 lib/c/ThrottleDrv/display.h
create mode 100644 lib/c/ThrottleDrv/ethernet_drv.c
create mode 100644 lib/c/ThrottleDrv/ethernet_drv.h
create mode 100644 lib/c/ThrottleDrv/throttle_drv.c
create mode 100644 lib/c/ThrottleDrv/throttle_drv.h
create mode 100644 lib/c/labcomm_thr_reader_writer.c
create mode 100644 lib/c/labcomm_thr_reader_writer.h
diff --git a/examples/simple_java/Encoder.java b/examples/simple_java/Encoder.java
index 6a05792..3209f7a 100644
--- a/examples/simple_java/Encoder.java
+++ b/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();
diff --git a/examples/simple_java/simple.lc b/examples/simple_java/simple.lc
index b83c63a..2da6c48 100644
--- a/examples/simple_java/simple.lc
+++ b/examples/simple_java/simple.lc
@@ -7,3 +7,8 @@ sample struct {
int x;
string s;
} IntString;
+
+sample struct {
+ double x;
+ double y;
+} TwoDoubles;
diff --git a/examples/simple_java/thr_compile.sh b/examples/simple_java/thr_compile.sh
new file mode 100644
index 0000000..53b9865
--- /dev/null
+++ b/examples/simple_java/thr_compile.sh
@@ -0,0 +1,3 @@
+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
diff --git a/examples/simple_java/thr_decoder.c b/examples/simple_java/thr_decoder.c
new file mode 100644
index 0000000..de5907c
--- /dev/null
+++ b/examples/simple_java/thr_decoder.c
@@ -0,0 +1,56 @@
+#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);
+ }
+}
diff --git a/examples/simple_java/thr_encoder.c b/examples/simple_java/thr_encoder.c
new file mode 100644
index 0000000..6466175
--- /dev/null
+++ b/examples/simple_java/thr_encoder.c
@@ -0,0 +1,43 @@
+#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);
+ }
+}
diff --git a/lib/c/ThrottleDrv/display.h b/lib/c/ThrottleDrv/display.h
new file mode 100644
index 0000000..f8bed75
--- /dev/null
+++ b/lib/c/ThrottleDrv/display.h
@@ -0,0 +1,7 @@
+#define PC_MODE
+#ifdef PC_MODE
+#include <stdio.h>
+#define DISPLAY_ERR(s) perror(s);
+#else
+#define DISPLAY_ERR(s) ;
+#endif
diff --git a/lib/c/ThrottleDrv/ethernet_drv.c b/lib/c/ThrottleDrv/ethernet_drv.c
new file mode 100644
index 0000000..d0f5f02
--- /dev/null
+++ b/lib/c/ThrottleDrv/ethernet_drv.c
@@ -0,0 +1,223 @@
+#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);
+}
+
+
+
diff --git a/lib/c/ThrottleDrv/ethernet_drv.h b/lib/c/ThrottleDrv/ethernet_drv.h
new file mode 100644
index 0000000..1dc786e
--- /dev/null
+++ b/lib/c/ThrottleDrv/ethernet_drv.h
@@ -0,0 +1,12 @@
+#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
diff --git a/lib/c/ThrottleDrv/throttle_drv.c b/lib/c/ThrottleDrv/throttle_drv.c
new file mode 100644
index 0000000..7cece88
--- /dev/null
+++ b/lib/c/ThrottleDrv/throttle_drv.c
@@ -0,0 +1,385 @@
+#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);
+}
+
+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
+ {
+ printf("Message Index %d on %d. Actual Index %d\n", THR_MSG_FRAG_NUM, THR_MSG_FRAG_TOT_NUM, frag_index);
+ if (frag_index == THR_MSG_FRAG_NUM) /* 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_FRAG_LEN);
+ p_data += THR_MSG_FRAG_LEN; /* 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)
+ {
+ msg_received = 1;
+ }
+ }
+ else
+ {
+ printf("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
+ {
+ printf("Number of byte receive %d\n",ret);
+ thr_chn->msg_length = ret;
+ memcpy(thr_chn->p_msg, data, ret); /* copy the msg into the thr structure */
+ printf("calling %x\n", thr_chn->funct);
+ (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)) /* Is Ethernet Type THROTTLENET Protocol ? */
+ {
+ if (THR_MSG_CHN_ID == 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;
+ printf("thr_read: truncating length to %d\n", length);
+ }
+ printf("thr_read: calling memcpy(%x, %x, %d\n", data, thr_chn->p_msg, length);
+ memcpy(data, thr_chn->p_msg, length); /* copy the msg into the thr structure */
+ ret = length;
+ thr_chn->msg_length = 0;
+ }
+ return(ret);
+}
diff --git a/lib/c/ThrottleDrv/throttle_drv.h b/lib/c/ThrottleDrv/throttle_drv.h
new file mode 100644
index 0000000..567c36d
--- /dev/null
+++ b/lib/c/ThrottleDrv/throttle_drv.h
@@ -0,0 +1,17 @@
+#ifndef _THROTTLE_DRV_H_
+#define _THROTTLE_DRV_H_
+
+#define ETH_ADR_SIZE 6
+
+/* typedef of the callback used to manage the received Ethernet Frame (the User Data)*/
+typedef int (*thr_msg_handler_t)(void* data);
+struct thr_chn_t;
+
+
+int thr_init(const char* eth_int);
+struct thr_chn_t* thr_open_chn(const unsigned char* dst_adr, unsigned char chn_id, unsigned char frag_size, unsigned short freq, thr_msg_handler_t funct);
+int thr_receive (struct thr_chn_t* thr_chn, unsigned char* data, void* param);
+int thr_send(const struct thr_chn_t* thr_chn, const char* data, unsigned int length);
+int thr_read(struct thr_chn_t* thr_chn, unsigned char* data, int length);
+
+#endif
diff --git a/lib/c/labcomm.c b/lib/c/labcomm.c
index ee93aec..13aff96 100644
--- a/lib/c/labcomm.c
+++ b/lib/c/labcomm.c
@@ -41,7 +41,7 @@ static labcomm_sample_entry_t *get_sample_by_signature_address(
{
labcomm_sample_entry_t *p;
for (p = head ; p && p->signature != signature ; p = p->next) {
-
+
}
return p;
}
@@ -55,7 +55,7 @@ static labcomm_sample_entry_t *get_sample_by_signature_value(
if (p->signature->type == signature->type &&
p->signature->size == signature->size &&
strcmp(p->signature->name, signature->name) == 0 &&
- bcmp((void*)p->signature->signature, (void*)signature->signature,
+ bcmp((void*)p->signature->signature, (void*)signature->signature,
signature->size) == 0) {
break;
}
@@ -90,7 +90,7 @@ static int get_encoder_index(
return result;
}
-static void do_encoder_register(struct labcomm_encoder *e,
+static void do_encoder_register(struct labcomm_encoder *e,
labcomm_signature_t *signature,
labcomm_encode_typecast_t encode)
{
@@ -98,7 +98,7 @@ static void do_encoder_register(struct labcomm_encoder *e,
if (get_encoder_index(e, signature) == 0) {
int i;
labcomm_encoder_context_t *context = e->context;
- labcomm_sample_entry_t *sample =
+ labcomm_sample_entry_t *sample =
(labcomm_sample_entry_t*)malloc(sizeof(labcomm_sample_entry_t));
sample->next = context->sample;
sample->index = context->index;
@@ -112,7 +112,7 @@ static void do_encoder_register(struct labcomm_encoder *e,
labcomm_encode_type_index(e, signature);
labcomm_encode_string(e, signature->name);
for (i = 0 ; i < signature->size ; i++) {
- if (e->writer.pos >= e->writer.count) {
+ if (e->writer.pos >= e->writer.count) {
e->writer.write(&e->writer, labcomm_writer_continue);
}
e->writer.data[e->writer.pos] = signature->signature[i];
@@ -124,13 +124,13 @@ static void do_encoder_register(struct labcomm_encoder *e,
}
static void do_encode(
- labcomm_encoder_t *encoder,
- labcomm_signature_t *signature,
+ labcomm_encoder_t *encoder,
+ labcomm_signature_t *signature,
void *value)
{
labcomm_encoder_context_t *context = encoder->context;
labcomm_sample_entry_t *sample;
- sample = get_sample_by_signature_address(context->sample,
+ sample = get_sample_by_signature_address(context->sample,
signature);
if (sample && sample->encode) {
sample->encode(encoder, value);
@@ -165,7 +165,7 @@ labcomm_encoder_t *labcomm_encoder_new(
}
void labcomm_internal_encoder_register(
- labcomm_encoder_t *e,
+ labcomm_encoder_t *e,
labcomm_signature_t *signature,
labcomm_encode_typecast_t encode)
{
@@ -177,26 +177,26 @@ void labcomm_internal_encoder_register(
}
void labcomm_internal_encode(
- labcomm_encoder_t *e,
- labcomm_signature_t *signature,
+ labcomm_encoder_t *e,
+ labcomm_signature_t *signature,
void *value)
{
if (e && e->do_encode) {
e->do_encode(e, signature, value);
} else {
printf("Encoder is missing do_encode\n");
- }
+ }
}
-
+
void labcomm_internal_encoder_user_action(labcomm_encoder_t *e,
int action)
{
e->writer.write(&e->writer, action);
}
-void labcomm_encoder_free(labcomm_encoder_t* e)
+void labcomm_encoder_free(labcomm_encoder_t* e)
{
-
+
e->writer.write(&e->writer, labcomm_writer_free);
free(e);
}
@@ -208,7 +208,7 @@ void labcomm_encode_type_index(labcomm_encoder_t *e, labcomm_signature_t *s)
}
static int signature_writer(
- labcomm_writer_t *w,
+ labcomm_writer_t *w,
labcomm_writer_action_t action)
{
switch (action) {
@@ -219,13 +219,13 @@ static int signature_writer(
w->pos = 0;
} break;
case labcomm_writer_start: {
- w->data_size = 1000;
+ w->data_size = 1000;
w->count = w->data_size;
w->data = realloc(w->data, w->data_size);
w->pos = 0;
} break;
case labcomm_writer_continue: {
- w->data_size += 1000;
+ w->data_size += 1000;
w->count = w->data_size;
w->data = realloc(w->data, w->data_size);
} break;
@@ -257,7 +257,7 @@ static void collect_flat_signature(
switch (type) {
case LABCOMM_ARRAY: {
int dimensions, i;
-
+
dimensions = labcomm_decode_int(decoder);
labcomm_encode_int(signature_writer, dimensions);
for (i = 0 ; i < dimensions ; i++) {
@@ -294,16 +294,16 @@ static void collect_flat_signature(
}
static void do_decoder_register(
- labcomm_decoder_t *decoder,
- labcomm_signature_t *signature,
+ labcomm_decoder_t *decoder,
+ labcomm_signature_t *signature,
labcomm_decoder_typecast_t type_decoder,
labcomm_handler_typecast_t handler,
void *handler_context)
{
-
+
labcomm_decoder_context_t *context = decoder->context;
labcomm_sample_entry_t *sample;
- sample = get_sample_by_signature_address(context->sample,
+ sample = get_sample_by_signature_address(context->sample,
signature);
if (!sample) {
sample = (labcomm_sample_entry_t*)malloc(sizeof(labcomm_sample_entry_t));
@@ -311,7 +311,7 @@ static void do_decoder_register(
context->sample = sample;
sample->index = 0;
sample->signature = signature;
- }
+ }
sample->decoder = type_decoder;
sample->handler = handler;
sample->context = handler_context;
@@ -323,7 +323,7 @@ static int do_decode_one(labcomm_decoder_t *d)
do {
result = d->reader.read(&d->reader, labcomm_reader_start);
- if (result > 0) {
+ if (result > 0) {
labcomm_decoder_context_t *context = d->context;
result = labcomm_decode_int(d);
@@ -332,7 +332,7 @@ static int do_decode_one(labcomm_decoder_t *d)
labcomm_signature_t signature;
labcomm_sample_entry_t *entry;
int index;
-
+
e->writer.write(&e->writer, labcomm_writer_start);
signature.type = result;
index = labcomm_decode_int(d);
@@ -343,10 +343,10 @@ static int do_decode_one(labcomm_decoder_t *d)
entry = get_sample_by_signature_value(context->sample, &signature);
if (! entry) {
// Unknown datatype, bail out
- fprintf(stderr, "%s: unknown datatype '%s' (id=0x%x)\n",
+ fprintf(stderr, "%s: unknown datatype '%s' (id=0x%x)\n",
__FUNCTION__, signature.name, index);
} else if (entry->index && entry->index != index) {
- fprintf(stderr, "%s: index mismatch '%s' (id=0x%x != 0x%x)\n",
+ fprintf(stderr, "%s: index mismatch '%s' (id=0x%x != 0x%x)\n",
__FUNCTION__, signature.name, entry->index, index);
} else {
entry->index = index;
@@ -360,10 +360,10 @@ static int do_decode_one(labcomm_decoder_t *d)
labcomm_encoder_free(e);
} else {
labcomm_sample_entry_t *entry;
-
+
entry = get_sample_by_index(context->sample, result);
if (!entry) {
- fprintf(stderr, "%s: type not found (id=0x%x)\n",
+ fprintf(stderr, "%s: type not found (id=0x%x)\n",
__FUNCTION__, result);
result = -ENOENT;
} else {
@@ -372,7 +372,7 @@ static int do_decode_one(labcomm_decoder_t *d)
}
}
d->reader.read(&d->reader, labcomm_reader_end);
- } while (result > 0 && result < LABCOMM_USER);
+ } while (result > 0 && result < LABCOMM_USER);
return result;
}
@@ -400,8 +400,8 @@ labcomm_decoder_t *labcomm_decoder_new(
}
void labcomm_internal_decoder_register(
- labcomm_decoder_t *d,
- labcomm_signature_t *signature,
+ labcomm_decoder_t *d,
+ labcomm_signature_t *signature,
labcomm_decoder_typecast_t type_decoder,
labcomm_handler_typecast_t handler,
void *handler_context)
@@ -416,9 +416,13 @@ void labcomm_internal_decoder_register(
int labcomm_decoder_decode_one(labcomm_decoder_t *d)
{
int result = -1;
- if (d && d->do_decode_one) {
+ if (d && d->do_decode_one)
+ {
+ printf("labcomm decode 1\n");
result = d->do_decode_one(d);
- } else {
+ }
+ else
+ {
printf("Decoder is missing do_decode_one\n");
}
return result;
@@ -430,7 +434,7 @@ void labcomm_decoder_run(labcomm_decoder_t *d)
}
}
-void labcomm_decoder_free(labcomm_decoder_t* d)
+void labcomm_decoder_free(labcomm_decoder_t* d)
{
d->reader.read(&d->reader, labcomm_reader_free);
free(d);
diff --git a/lib/c/labcomm_thr_reader_writer.c b/lib/c/labcomm_thr_reader_writer.c
new file mode 100644
index 0000000..4ba0147
--- /dev/null
+++ b/lib/c/labcomm_thr_reader_writer.c
@@ -0,0 +1,141 @@
+#include <errno.h>
+#include <unistd.h>
+#include "labcomm.h"
+#include "ThrottleDrv/throttle_drv.h"
+
+#define BUFFER_SIZE 2048
+
+int labcomm_thr_reader(labcomm_reader_t *r, labcomm_reader_action_t action)
+{
+ int result = -EINVAL;
+ struct thr_chn_t *ctx = r->context;
+
+ switch (action)
+ {
+ case labcomm_reader_alloc:
+ {
+ r->data = malloc(BUFFER_SIZE);
+ if (r->data)
+ {
+ r->data_size = BUFFER_SIZE;
+ result = r->data_size;
+ }
+ else
+ {
+ r->data_size = 0;
+ result = -ENOMEM;
+ }
+ r->count = 0;
+ r->pos = 0;
+ }
+ break;
+
+ case labcomm_reader_start:
+ case labcomm_reader_continue:
+ {
+ printf("labcomm_reader_start or continue...\n");
+ if (r->pos < r->count)
+ {
+ result = r->count - r->pos;
+ }
+ else
+ {
+ int err;
+ r->pos = 0;
+ printf("labcomm_thr_reader_writer. calling thr_read: %x, %x, %d\n", ctx, r->data, r->data_size);
+ err = thr_read(ctx, r->data, r->data_size);
+ if (err <= 0)
+ {
+ r->count = 0;
+ result = -1;
+ } else
+ {
+ r->count = err;
+ result = r->count - r->pos;
+ }
+ }
+ }
+ break;
+
+ case labcomm_reader_end:
+ {
+ result = 0;
+ }
+ break;
+
+ case labcomm_reader_free:
+ {
+ free(r->data);
+ r->data = 0;
+ r->data_size = 0;
+ r->count = 0;
+ r->pos = 0;
+ result = 0;
+ }
+ break;
+ }
+ return result;
+}
+
+int labcomm_thr_writer(labcomm_writer_t *w, labcomm_writer_action_t action)
+{
+ int result = 0;
+ struct thr_chn_t *ctx = w->context;
+
+ switch (action)
+ {
+ case labcomm_writer_alloc:
+ {
+ w->data = malloc(BUFFER_SIZE);
+ if (! w->data)
+ {
+ result = -ENOMEM;
+ w->data_size = 0;
+ w->count = 0;
+ w->pos = 0;
+ }
+ else
+ {
+ w->data_size = BUFFER_SIZE;
+ w->count = BUFFER_SIZE;
+ w->pos = 0;
+ }
+ } break;
+ case labcomm_writer_free:
+ {
+ free(w->data);
+ w->data = 0;
+ w->data_size = 0;
+ w->count = 0;
+ w->pos = 0;
+ }
+ break;
+
+ case labcomm_writer_start:
+ {
+ w->pos = 0;
+ }
+ break;
+
+ case labcomm_writer_continue:
+ {
+ result = thr_send(ctx, w->data, w->pos);
+ w->pos = 0;
+ }
+ break;
+
+ case labcomm_writer_end:
+ {
+ result = thr_send(ctx, w->data, w->pos);
+ w->pos = 0;
+ }
+ break;
+
+ case labcomm_writer_available:
+ {
+ result = w->count - w->pos;
+ }
+ break;
+ }
+ return result;
+}
diff --git a/lib/c/labcomm_thr_reader_writer.h b/lib/c/labcomm_thr_reader_writer.h
new file mode 100644
index 0000000..0adf36e
--- /dev/null
+++ b/lib/c/labcomm_thr_reader_writer.h
@@ -0,0 +1,5 @@
+#include "labcomm.h"
+
+extern int labcomm_thr_reader(labcomm_reader_t *r, labcomm_reader_action_t action);
+
+extern int labcomm_thr_writer(labcomm_writer_t *r, labcomm_writer_action_t action);
--
GitLab