mbtk/mbtk_at/src/mbtk_rgmii_loopback.c

#if 1//MBTK_RGMII_TEST
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>// close()
#include <string.h>// strcpy, memset(), and memcpy()
#include <netdb.h> // struct addrinfo
#include <sys/types.h>  // needed for socket(), uint8_t, uint16_t, uint32_t
#include <sys/socket.h>  // needed for socket()
#include <netinet/in.h> // IPPROTO_ICMP, INET_ADDRSTRLEN
#include <netinet/ip.h>// struct ip and IP_MAXPACKET (which is 65535)
#include <netinet/ip_icmp.h> // struct icmp, ICMP_ECHO
#include <arpa/inet.h> // inet_pton() and inet_ntop()
#include <sys/ioctl.h> // macro ioctl is defined
//#include <bits/ioctls.h>  // defines values for argument "request" of ioctl.
#include <net/if.h> // struct ifreq
#include <linux/if_ether.h> // ETH_P_IP = 0x0800, ETH_P_IPV6 = 0x86DD
#include <linux/if_packet.h> // struct sockaddr_ll (see man 7 packet)
//#include <net/ethernet.h>
#include <errno.h> // errno, perror()

#include <linux/mii.h>
#include <linux/sockios.h>
#include <linux/types.h>

#include "mbtk_at.h"

#define ETH_P_DEAN 0x8874 //自定义的以太网协议type

//mdio eth0 1          	读取phy寄存器1的数值
//mdio eth0 0 0x1120      将0x1120写入 phy寄存器1
#define reteck(ret)     \
        if(ret < 0){    \
            LOG("[RGMIITEST]%s : line: %d", __func__, __LINE__);   \
            goto lab;   \
        }

#if 0
#define help() \
    printf("mdio:\n");                  \
    printf("read operation: mdio reg_addr\n");          \
    printf("write operation: mdio reg_addr value\n");    \
    printf("For example:\n");            \
    printf("mdio eth0 1\n");             \
    printf("mdio eth0 0 0x12\n\n");      \
    exit(0);
#endif

int sockfd;

//net_name:网卡名字，mode:get 1/set 0,reg_addr: 寄存器地址，value:值
int mbtk_mdio(char* net_name,int mode, uint16_t reg_addr, uint16_t value)
{
    if(net_name == NULL)
    {
        return -1;
    }

    struct mii_ioctl_data *mii = NULL;
    struct ifreq ifr;
    int ret;

    memset(&ifr, 0, sizeof(ifr));
    strncpy(ifr.ifr_name, net_name, IFNAMSIZ - 1);

    sockfd = socket(PF_LOCAL, SOCK_DGRAM, 0);
    reteck(sockfd);

    //get phy address in smi bus
    ret = ioctl(sockfd, SIOCGMIIPHY, &ifr);
    reteck(ret);

    mii = (struct mii_ioctl_data*)&ifr.ifr_data;

    if(mode == 1)
    {

        mii->reg_num = reg_addr;

        ret = ioctl(sockfd, SIOCGMIIREG, &ifr);
        reteck(ret);

        LOG("[RGMIITEST]read phy addr: 0x%x  reg: 0x%x   value : 0x%x\n", mii->phy_id, mii->reg_num, mii->val_out);

        if (mii->reg_num == 0x1)
        {
            LOG("[RGMIITEST]Link Status\n");

            if(mii->val_out& 0x0004)
            {
                LOG("[RGMIITEST]link is up\n");
            }
            else
            {
                LOG("[RGMIITEST]link is down\n");
            }
        }
    }
    else if(mode == 0)
    {

        mii->reg_num    = reg_addr;
        mii->val_in     = value;

        ret = ioctl(sockfd, SIOCSMIIREG, &ifr);
        reteck(ret);

        LOG("[RGMIITEST]write phy addr: 0x%x  reg: 0x%x  value : 0x%x\n", mii->phy_id, mii->reg_num, mii->val_in);
    }

    close(sockfd);
    return 0;
    lab:
    close(sockfd);
    return -1;
}
 
int mbtk_rgmii_loopback(void)
{
    int i, datalen, frame_length, sd, bytes;
    char *interface = "eth0";
    uint8_t data[100];
    uint8_t src_mac[6];
    uint8_t dst_mac[6];
    uint8_t ether_frame[100] = {0};
    struct sockaddr_ll device;
    struct ifreq ifr;
    int recvlen=0;
    uint8_t buffer[100] = {0};
    int loopback_state=0;
    int ret = -1;

    // Submit request for a socket descriptor to look up interface.
    if ((sd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL))) < 0)
    {
        LOG("[RGMIITEST]socket() failed to get socket descriptor for using ioctl()");
        return -1;
    }

    // Use ioctl() to look up interface name and get its MAC address.
    memset(&ifr, 0, sizeof(ifr));
    snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), "%s", interface);
    if (ioctl(sd, SIOCGIFHWADDR, &ifr) < 0)
    {
        close(sd);
        LOG("[RGMIITEST]ioctl() failed to get source MAC address ");
        return -1;
    }
    close(sd);
 
    // Copy source MAC address.
    memcpy(src_mac, ifr.ifr_hwaddr.sa_data, 6);
 
    // Report source MAC address to stdout.
    LOG("[RGMIITEST]MAC address for interface %s is %02x:%02x:%02x:%02x:%02x:%02x", interface, \
                            src_mac[0], src_mac[1], src_mac[2], src_mac[3], src_mac[4], src_mac[5]);
 
    // Find interface index from interface name and store index in
    // struct sockaddr_ll device, which will be used as an argument of sendto().
    memset(&device, 0, sizeof(device));
    if ((device.sll_ifindex = if_nametoindex(interface)) == 0)
    {
        LOG("[RGMIITEST]if_nametoindex() failed to obtain interface index ");
        return -1;
    }
    LOG("[RGMIITEST]Index for interface %s is %i", interface, device.sll_ifindex);
 
    // Set destination MAC address: you need to fill these out
    dst_mac[0] = 0x72; //设置目的网卡地址
    dst_mac[1] = 0x40;
    dst_mac[2] = 0xB8;
	dst_mac[3] = 0xF7;
    dst_mac[4] = 0x5A;
    dst_mac[5] = 0x64;
 
    // Fill out sockaddr_ll.
    device.sll_family = AF_PACKET;
    memcpy(device.sll_addr, src_mac, 6);
    device.sll_halen = htons(6);
 
    // 发送的data，长度可以任意，但是抓包时看到最小数据长度为46，这是以太网协议规定以太网帧数
    //据域部分最小为46字节，不足的自动补零处理
 
    datalen = 12;
    data[0] = 'h';
    data[1] = 'e';
    data[2] = 'l';
    data[3] = 'l';
    data[4] = 'o';
    data[5] = ' ';
    data[6] = 'w';
    data[7] = 'o';
    data[8] = 'r';
    data[9] = 'l';
    data[10] = 'd';
    data[11] = '!';
 
    // Fill out ethernet frame header.
    frame_length = 6 + 6 + 2 + datalen;
    // Destination and Source MAC addresses
    memcpy(ether_frame, dst_mac, 6);
    memcpy(ether_frame + 6, src_mac, 6);
 
    ether_frame[12] = ETH_P_DEAN / 256;
    ether_frame[13] = ETH_P_DEAN % 256;

    // data
    memcpy(ether_frame + 14, data, datalen);
    // Submit request for a raw socket descriptor.
    if ((sd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL))) < 0)
    { //创建正真发送的socket
        LOG("[RGMIITEST]socket() failed ");
		return -1;
    }

    //set recv timeout
    struct timeval tv;
    tv.tv_sec = 3;
    tv.tv_usec = 0;
    ret = setsockopt( sd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) );
    if(ret != 0)
    {
        LOG("[RGMIITEST]setsockopt is fail ret = [%d]", ret);
    }

    // Send ethernet frame to socket.
    if ((bytes = sendto(sd, ether_frame, frame_length, 0, (struct sockaddr *)&device,
        sizeof(device))) <= 0)
    {
        close(sd);
        LOG("[RGMIITEST]sendto() failed");
        return -1;
    }
	LOG("[RGMIITEST]IP_MAXPACKET = %d", IP_MAXPACKET, bytes);
    LOG("[RGMIITEST]sendlen = %d sendto data:", bytes);
    LOG("[RGMIITEST]%s", ether_frame);

    recvlen = recvfrom(sd,buffer,sizeof(buffer),0,NULL,NULL);
    if(recvlen < 0)
    {
        if(errno == EWOULDBLOCK)
        {
            LOG("[RGMIITEST] recv timeout");
        }
        close(sd);
        return -1;
    }
	LOG("[RGMIITEST]recvlen = %d recvfrom data:\n", recvlen);
    LOG("[RGMIITEST]%s", buffer);
 
    for(i=0; i<recvlen; i++) {
        if(bytes>recvlen)
        {
            loopback_state=-1;
            break;
        }
        else if(ether_frame[i]!=buffer[i])
        {
            loopback_state=-1;
			LOG("[RGMIITEST]ether_frame[%d]:%02x buffer[%d]:%02x", i, ether_frame[i], i, buffer[i]);
            break;
        }
    }
    
    // Close socket descriptor.
    close(sd);
    return loopback_state;
}

#endif