summaryrefslogtreecommitdiffstats
path: root/package/librtk-inband/src/ioh.c
diff options
context:
space:
mode:
Diffstat (limited to 'package/librtk-inband/src/ioh.c')
-rw-r--r--package/librtk-inband/src/ioh.c315
1 files changed, 315 insertions, 0 deletions
diff --git a/package/librtk-inband/src/ioh.c b/package/librtk-inband/src/ioh.c
new file mode 100644
index 000000000..71989dc1b
--- /dev/null
+++ b/package/librtk-inband/src/ioh.c
@@ -0,0 +1,315 @@
+/*
+ * IOH helper functions
+ * Copyright (C)2010, Realtek Semiconductor Corp. All rights reserved
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <signal.h>
+#include <unistd.h>
+#include <time.h>
+#include <sys/ioctl.h>
+
+//#include <linux/if_packet.h>
+#include "ioh.h"
+
+#include <linux/if_arp.h>
+
+int bin2hex(const unsigned char *bin, char *hex, const int len)
+{
+ int i, idx;
+ char hex_char[] = "0123456789ABCDEF";
+
+ for (i=0, idx=0; i<len; i++)
+ {
+ hex[idx++] = hex_char[(bin[i] & 0xf0) >> 4];
+ hex[idx++] = hex_char[bin[i] & 0x0f];
+ }
+
+ hex[idx] = 0;
+ return 0;
+}
+
+int hex2bin(const char *hex, unsigned char *bin, const int len)
+{
+ int i, idx;
+ unsigned char bytes[2];
+
+ for (i=0, idx=0; hex[i]; i++)
+ {
+ if (hex[i & 0x01] == 0)
+ return -1; // hex length != even
+
+ if (hex[i] >= '0' && hex[i] <= '9')
+ bytes[i & 0x01] = hex[i] - '0';
+ else if (hex[i] >= 'A' && hex[i] <= 'F')
+ bytes[i & 0x01] = hex[i] - 'A' + 10;
+ else if (hex[i] >= 'a' && hex[i] <= 'f')
+ bytes[i & 0x01] = hex[i] - 'a' + 10;
+ else
+ return -1; // not hex
+
+ if (i & 0x01)
+ {
+ if (idx >= len)
+ return -1; // out of size
+
+ bin[idx++] = (bytes[0] << 4) | bytes[1];
+ }
+ }
+
+ return 0;
+}
+
+void hex_dump(void *data, int size)
+{
+ /* dumps size bytes of *data to stdout. Looks like:
+ * [0000] 75 6E 6B 6E 6F 77 6E 20
+ * 30 FF 00 00 00 00 39 00 unknown 0.....9.
+ * (in a single line of course)
+ */
+
+ unsigned char *p = data;
+ unsigned char c;
+ int n;
+ char bytestr[4] = {0};
+ char addrstr[10] = {0};
+ char hexstr[ 16*3 + 5] = {0};
+ char charstr[16*1 + 5] = {0};
+ for(n=1;n<=size;n++) {
+ if (n%16 == 1) {
+ /* store address for this line */
+ snprintf(addrstr, sizeof(addrstr), "%.4x",
+ ((unsigned int)p-(unsigned int)data) );
+ }
+
+ c = *p;
+ if (isalnum(c) == 0) {
+ c = '.';
+ }
+
+ /* store hex str (for left side) */
+ snprintf(bytestr, sizeof(bytestr), "%02X ", *p);
+ strncat(hexstr, bytestr, sizeof(hexstr)-strlen(hexstr)-1);
+
+ /* store char str (for right side) */
+ snprintf(bytestr, sizeof(bytestr), "%c", c);
+ strncat(charstr, bytestr, sizeof(charstr)-strlen(charstr)-1);
+
+ if(n%16 == 0) {
+ /* line completed */
+ printf("[%4.4s] %-50.50s %s\n", addrstr, hexstr, charstr);
+ hexstr[0] = 0;
+ charstr[0] = 0;
+ } else if(n%8 == 0) {
+ /* half line: add whitespaces */
+ strncat(hexstr, " ", sizeof(hexstr)-strlen(hexstr)-1);
+ strncat(charstr, " ", sizeof(charstr)-strlen(charstr)-1);
+ }
+ p++; /* next byte */
+ }
+
+ if (strlen(hexstr) > 0) {
+ /* print rest of buffer if not empty */
+ printf("[%4.4s] %-50.50s %s\n", addrstr, hexstr, charstr);
+ }
+}
+
+
+int ioh_open(struct ioh_class *obj, char *dev, char *da,unsigned short eth_type, int debug)
+{
+ struct ifreq ifr;
+ int ifindex;
+
+ strcpy(obj->dev, dev);
+
+ if (da == NULL)
+ ; // da == NULL if iohd
+ else
+ hex2bin(da, obj->dest_mac, ETH_MAC_LEN);
+
+ obj->debug = debug;
+
+ obj->tx_header = (void *) obj->tx_buffer;
+ obj->rx_header = (void *) obj->rx_buffer;
+ obj->tx_data = (unsigned char *) obj->tx_buffer + sizeof(*obj->tx_header);
+ obj->rx_data = (unsigned char *) obj->rx_buffer + sizeof(*obj->rx_header);
+ obj->eth_type = eth_type;
+ // create raw socket
+ obj->sockfd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
+ if (obj->sockfd == -1)
+ {
+ perror("socket():");
+ return -1;
+ }
+
+ if (obj->debug)
+ printf("Successfully opened socket: %i\n", obj->sockfd);
+
+ // retrieve ethernet interface index
+ bzero(&ifr, sizeof(ifr));
+ strncpy(ifr.ifr_name, obj->dev, IFNAMSIZ);
+ if (ioctl(obj->sockfd, SIOCGIFINDEX, &ifr) == -1)
+ {
+ perror("SIOCGIFINDEX");
+ return -1;
+ }
+ ifindex = ifr.ifr_ifindex;
+ if (obj->debug)
+ printf("Successfully got interface index: %i\n", ifindex);
+
+ // retrieve corresponding MAC
+ if (ioctl(obj->sockfd, SIOCGIFHWADDR, &ifr) == -1)
+ {
+ perror("SIOCGIFHWADDR");
+ return -1;
+ }
+
+ memcpy(obj->src_mac, ifr.ifr_hwaddr.sa_data, sizeof(obj->src_mac));
+ if (obj->debug)
+ {
+ printf("Successfully got our MAC address: %02X:%02X:%02X:%02X:%02X:%02X\n",
+ obj->src_mac[0], obj->src_mac[1], obj->src_mac[2],
+ obj->src_mac[3], obj->src_mac[4], obj->src_mac[5]);
+ }
+
+ // Bind our raw socket to this interface
+ bzero(&obj->socket_address, sizeof(obj->socket_address));
+ obj->socket_address.sll_family = PF_PACKET;
+ obj->socket_address.sll_protocol = htons(obj->eth_type);
+ obj->socket_address.sll_ifindex = ifindex;
+ if((bind(obj->sockfd, (struct sockaddr *) &obj->socket_address,
+ sizeof(obj->socket_address)))== -1)
+ {
+ perror("Error binding socket to interface\n");
+ return -1;
+ }
+
+ return 0;
+}
+
+int ioh_close(struct ioh_class *obj)
+{
+ close(obj->sockfd);
+ return 0;
+}
+
+int ioh_send(struct ioh_class *obj , unsigned int send_len)
+{
+ int sent; // length of sent packet
+
+ //fill pkt header
+ memcpy(obj->tx_header->da, obj->dest_mac, ETH_MAC_LEN);
+ memcpy(obj->tx_header->sa, obj->src_mac, ETH_MAC_LEN);
+ obj->tx_header->eth_type = htons(obj->eth_type);
+
+ obj->socket_address.sll_hatype = ARPHRD_ETHER;
+ obj->socket_address.sll_pkttype = PACKET_OTHERHOST;
+ obj->socket_address.sll_halen = ETH_ALEN;
+ obj->socket_address.sll_addr[0] = obj->dest_mac[0];
+ obj->socket_address.sll_addr[1] = obj->dest_mac[1];
+ obj->socket_address.sll_addr[2] = obj->dest_mac[2];
+ obj->socket_address.sll_addr[3] = obj->dest_mac[3];
+ obj->socket_address.sll_addr[4] = obj->dest_mac[4];
+ obj->socket_address.sll_addr[5] = obj->dest_mac[5];
+ obj->socket_address.sll_addr[6] = 0x00;
+ obj->socket_address.sll_addr[7] = 0x00;
+
+ if (obj->debug)
+ {
+ printf("%s: tx len = %d\n", __FUNCTION__, send_len);
+ hex_dump(obj->tx_buffer, send_len);
+ }
+
+ sent = sendto(obj->sockfd, obj->tx_buffer,
+ send_len, 0,
+ (struct sockaddr*) &obj->socket_address, sizeof(obj->socket_address));
+
+ if (sent < 0)
+ {
+ perror("sendto():");
+ return -1;
+ }
+
+ return sent;
+}
+
+static int check_rcv_header(struct ioh_class *obj,int rx_len)
+{
+ if (rx_len < 0)
+ {
+ perror("check_rcv_header:");
+ return -1;
+ }
+
+ /* obj->rx_header->rrcp_type != RRCP_P_IOH) //mark_inband
+ return -1;*/
+
+ if (obj->rx_header->eth_type != ntohs(obj->eth_type) )
+ return -1;
+#if 0 //mark_inband
+ if (rx_len != ntohs(obj->rx_header->ioh_data_len) + sizeof(*obj->rx_header))
+ {
+ if (ntohs(obj->rx_header->ioh_data_len) + sizeof(*obj->rx_header) <
+ ETH_MIN_FRAME_LEN && rx_len == ETH_MIN_FRAME_LEN)
+ {
+ // its ok for min ethernet packet padding
+ }
+ else
+ {
+ printf("%s: rx len (%d) != %d\n", __FUNCTION__,
+ rx_len, ntohs(obj->rx_header->ioh_data_len) + sizeof(*obj->rx_header));
+ return -1;
+ }
+ }
+#endif
+
+ return rx_len;
+
+}
+
+int ioh_recv(struct ioh_class *obj, int timeout_ms)
+{
+ fd_set rfds;
+ struct timeval timeout;
+ int retval;
+ int rx_len;
+
+ FD_ZERO(&rfds);
+ FD_SET(obj->sockfd, &rfds);
+
+ if (timeout_ms < 0)
+ {
+ retval = select(obj->sockfd + 1, &rfds, NULL, NULL, NULL);
+ }
+ else
+ {
+ timeout.tv_sec = 0;
+ timeout.tv_usec = timeout_ms * 1000;
+ retval = select(obj->sockfd + 1, &rfds, NULL, NULL, &timeout);
+ }
+
+ if (retval && FD_ISSET(obj->sockfd, &rfds))
+ {
+ rx_len = recvfrom(obj->sockfd, obj->rx_buffer,
+ sizeof(obj->rx_buffer), 0, NULL, NULL);
+
+ return check_rcv_header(obj,rx_len);
+ }
+ else if (retval == 0)
+ {
+ if (obj->debug)
+ printf("Timeout!!!\n");
+
+ return -1;
+ }
+ else
+ {
+ perror("select():");
+ return -1;
+ }
+
+ return -2;
+}
+
generated by cgit v1.2.3 (git 2.25.1) at 2024-10-31 21:43:05 +0000