mdio-tool.c

#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <fcntl.h>
#include <getopt.h>
#include <time.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/sockios.h>

#ifndef __GLIBC__
#include <linux/if_arp.h>
#include <linux/if_ether.h>
#endif
#include "mii.h"

#define MDIO_TOOL_VERSION	"v1.0"

#define PHY_SLECT_PAGE	31
#define PHY_BASE_PAGE	0

//you can find kernel/driver/net/phy/phy.c : phy_mii_ioctl
#define MII_ADDR_C45	(1 << 30)
#define PHY_IS_C45	0x8000
#define PHY_C45_MASK	0x03e0


static struct ifreq ifr;
static int skfd = -1;

static int mdio_read(int location)
{
	struct mii_data *mii = (struct mii_data *)&ifr.ifr_data;
	mii->reg_num = location;

	if (ioctl(skfd, SIOCGMIIREG, &ifr) < 0) {
		fprintf(stderr, "SIOCGMIIREG on %s failed: %s\n", ifr.ifr_name,
			strerror(errno));
		return -1;
	}
	return mii->val_out;
}

static int mdio_write(int location, int value)
{
	struct mii_data *mii = (struct mii_data *)&ifr.ifr_data;
	mii->reg_num = location;
	mii->val_in = value;

	if (ioctl(skfd, SIOCSMIIREG, &ifr) < 0) {
		fprintf(stderr, "SIOCSMIIREG on %s failed: %s\n", ifr.ifr_name,
		strerror(errno));
		return -1;
	}

	return 0;
}

static int mdio_mmd_read(int devad, int reg)
{
	mdio_write(13, (devad | 0x0000));
	mdio_write(14, reg);
	mdio_write(13, (devad | 0x4000));
	return mdio_read(14);
}

static int mdio_mmd_write(int devad, int reg, int val)
{
	mdio_write(13, (devad | 0x0000));
	mdio_write(14, reg);
	mdio_write(13, (devad | 0x4000));
	return mdio_write(14, val);
}

static int mdio_c45_read(int devad, int reg)
{
	struct mii_data *mii = (struct mii_data *)&ifr.ifr_data;

//	mii->reg_num = (MII_ADDR_C45 | (devad << 16) | (reg & 0xffff));
	mii->reg_num = reg;
	if (ioctl(skfd, SIOCGMIIREG, &ifr) < 0) {
		fprintf(stderr, "SIOCGMIIREG on %s failed: %s\n", ifr.ifr_name,
			strerror(errno));
		return -1;
	}
	return mii->val_out;
}

static int mdio_c45_write(int devad, int reg, int val)
{
	struct mii_data *mii = (struct mii_data *)&ifr.ifr_data;

//	mii->reg_num = (MII_ADDR_C45 | (devad << 16) | (reg & 0xffff));
	mii->reg_num = reg;
	mii->val_in = val;

	if (ioctl(skfd, SIOCSMIIREG, &ifr) < 0) {
		fprintf(stderr, "SIOCGMIIREG on %s failed: %s\n", ifr.ifr_name,
			strerror(errno));
		return -1;
	}

	return 0;
}

static void help(void *op)
{
	if (((char *)op)[0] == 'w') {
		printf("Usage: mdio-tool [w] [dev] [reg] [val]\n");
		printf("or\n");
		printf("Usage: mdio-tool [w] [dev] [page] [reg] [val]\n");
		printf("or\n");
		printf("Usage: mdio-tool [w] [dev] [phyaddr] [page] [reg] [val]\n");

	} else if (((char*)op)[0] == 'r') {
		printf("Usage: mdio-tool [r] [dev] [reg]\n");
		printf("or\n");
		printf("Usage: mdio-tool [r] [dev] [page] [reg]\n");
		printf("or\n");
		printf("Usage: mdio-tool [r] [dev] [phyaddr] [page] [reg]\n");
	}else if (((char*)op)[0] == 'm'){
		if (((char*)op)[1] == 'w') {
			printf("Usage: mdio-tool [mw] [dev] [devad] [reg] [val]\n");
			printf("or\n");
			printf("Usage: mdio-tool [mw] [dev] [phyaddr] [devad] [reg] [val]\n");
		} else if (((char*)op)[1] == 'r'){
			printf("Usage: mdio-tool [mr] [dev] [devad] [reg]\n");
			printf("or\n");
			printf("Usage: mdio-tool [mr] [dev] [phyaddr] [devad] [reg]\n");
		}
	}else if (((char*)op)[0] == 'd'){
		if (((char*)op)[1] == 'w') {
			printf("Usage: mdio-tool [dw] [dev] [devad] [reg] [val]\n");
			printf("or\n");
			printf("Usage: mdio-tool [dw] [dev] [phyaddr] [devad] [reg] [val]\n");
		} else if (((char*)op)[1] == 'r'){
			printf("Usage: mdio-tool [dr] [dev] [devad] [reg]\n");
			printf("or\n");
			printf("Usage: mdio-tool [dr] [dev] [phyaddr] [devad] [reg]\n");
		}
	} else {
		printf("===========================================================\n");
		printf("mdio-tool version %s\n", MDIO_TOOL_VERSION);
		printf("[dev]: Ethernet device name: eth0/eth1...\n");
		printf("============================c22============================\n");
		printf("Usage: mdio-tool [r/w] [dev] [reg] [val]\n");
		printf("or\n");
		printf("Usage: mdio-tool [r/w] [dev] [page] [reg] [val]\n");
		printf("or\n");
		printf("Usage: mdio-tool [r/w] [dev] [phyaddr] [page] [reg] [val]\n");
		printf("============================mmd(c45)=======================\n");
		printf("Usage: mdio-tool [mr/mw] [dev] [devad] [reg] [val]\n");
		printf("or\n");
		printf("Usage: mdio-tool [mr/mw] [dev] [phyaddr] [devad] [reg] [val]\n");
		printf("============================dct(c45)=======================\n");
		printf("Usage: mdio-tool [dr/mw] [dev] [devad] [reg] [val]\n");
		printf("or\n");
		printf("Usage: mdio-tool [dr/mw] [dev] [phyaddr] [devad] [reg] [val]\n");
	}
}

int phy_write_page(int page)
{
	return mdio_write(PHY_SLECT_PAGE, page);
}

int phy_read_page(int page)
{
	return mdio_read(PHY_SLECT_PAGE);
}

static inline int __phy_save_page(int page)
{
	return phy_read_page(page);
}

static inline int __phy_select_page(int page)
{
	int ret, oldpage;

	oldpage = ret = __phy_save_page(page);
	if (ret < 0)
		return ret;

	if (oldpage != page) {
		ret = phy_write_page(page);
		if (ret < 0)
			return ret;
	}

	return oldpage;
}

static inline int __phy_restore_page(int oldpage, int ret)
{
	int r;

	if (oldpage >= 0) {
		r = phy_write_page(oldpage);
		if (ret >= 0 && r < 0)
			ret = r;
	} else {
		ret = oldpage;
	}

	return ret;
}

static int phy_write_paged(int page, int reg, int val)
{
	int oldpage, ret;

	oldpage = __phy_select_page(page);
	if (oldpage >= 0)
		ret = mdio_write(reg, val);

	return  __phy_restore_page(oldpage, ret);
}

static int phydrv_register_addr_config(char **argv)
{
	if (ioctl(skfd, SIOCGMIIPHY, &ifr) < 0) {
		fprintf(stderr, "SIOCGMIIPHY on '%s' failed: %s\n",
			argv[2], strerror(errno));
		return -1;
	}

	return 0;
}

static void phy_write(int argc, char **argv)
{
	struct mii_data *mii = (struct mii_data *)&ifr.ifr_data;
	int addr, page, reg, val;

	if (argc == 7) {
		addr = strtol(argv[3], NULL, 0);
		page = strtol(argv[4], NULL, 0);
		reg = strtol(argv[5], NULL, 0);
		val = strtol(argv[6], NULL, 0);
		if (addr > 31 || addr < 0) {
			printf("0<= phyaddr <=31\n");
			return;
		} else {
			mii->phy_id = addr;
		}
		phy_write_paged(page, reg, val);
	} else if (argc == 6) {
		page = strtol(argv[3], NULL, 0);
		reg = strtol(argv[4], NULL, 0);
		val = strtol(argv[5], NULL, 0);
		phy_write_paged(page, reg, val);
	} else if (argc == 5) {
		reg = strtol(argv[3], NULL, 0);
		val = strtol(argv[4], NULL, 0);
		mdio_write(reg, val);
	} else {
		help((void *)&argv[1][0]);
	}
}

static int phy_read_paged(int page, int reg)
{
	int ret = 0, oldpage;

	oldpage = __phy_select_page(page);

	if (oldpage >= 0)
		ret = mdio_read(reg);

	return __phy_restore_page(oldpage, ret);
}

static int phy_read(int argc, char **argv)
{
	struct mii_data *mii = (struct mii_data *)&ifr.ifr_data;
	int retval, addr, page, reg, err;

	if (argc == 6) {
		addr = strtol(argv[3], NULL, 0);
		page = strtol(argv[4], NULL, 0);
		reg = strtol(argv[5], NULL, 0);
		if (addr > 31 || addr < 0) {
			printf("0<= phyaddr <=31\n");
			return 0;
		} else {
			mii->phy_id = addr;
		}
		retval = phy_read_paged(page, reg);
		printf("phyaddr: %d page: %d reg: %d val: 0x%.4x\n",
				mii->phy_id, page, reg, retval);
	} else if (argc == 5) {
		err = phydrv_register_addr_config(argv);
		if (err < 0)
			return err;
		page = strtol(argv[3], NULL, 0);
		reg = strtol(argv[4], NULL, 0);
		retval = phy_read_paged(page, reg);
		printf("phyaddr: %d page: %d reg: %d val: 0x%.4x\n",
				mii->phy_id, page, reg, retval);
	} else if (argc == 4) {
		err = phydrv_register_addr_config(argv);
		if (err < 0)
			return err;
		reg = strtol(argv[3], NULL, 0);
		retval = mdio_read(reg);
		printf("phyaddr: %d reg: %d val: 0x%.4x\n",
				mii->phy_id, reg, retval);
	} else {
		help((void *)&argv[1][0]);
		return -1;
	}

	return retval;
}

static void phy_mmd_write(int argc, char **argv)
{
	struct mii_data *mii = (struct mii_data *)&ifr.ifr_data;
	int addr, devad, reg, val;

	if (argc == 7) {
		addr = strtol(argv[3], NULL, 0);
		devad = strtol(argv[4], NULL, 0);
		reg = strtol(argv[5], NULL, 0);
		val = strtol(argv[6], NULL, 0);
		if (addr > 31 || addr < 0) {
			printf("0<= phyaddr <=31\n");
			return;
		} else {
			mii->phy_id = addr;
		}
	} else if (argc == 6) {
		devad = strtol(argv[3], NULL, 0);
		reg = strtol(argv[4], NULL, 0);
		val = strtol(argv[5], NULL, 0);
	} else {
		help((void *)&argv[1][0]);
		return;
	}
	mdio_mmd_write(devad, reg, val);
}

static int phy_mmd_read(int argc, char **argv)
{
	struct mii_data *mii = (struct mii_data *)&ifr.ifr_data;
	int retval, addr, devad, reg, err;

	if (argc == 6) {
		addr = strtol(argv[3], NULL, 0);
		devad = strtol(argv[4], NULL, 0);
		reg = strtol(argv[5], NULL, 0);
		if (addr > 31 || addr < 0) {
			printf("0<= phyaddr <=31\n");
			return 0;
		} else {
			mii->phy_id = addr;
		}
	} else if (argc == 5) {
		err = phydrv_register_addr_config(argv);
		if (err < 0)
			return err;
		devad = strtol(argv[3], NULL, 0);
		reg = strtol(argv[4], NULL, 0);
	} else {
		help((void *)&argv[1][0]);
		return -1;
	}
	retval = mdio_mmd_read(devad, reg);
	printf("phyaddr: %d devad: %d reg: %d val: 0x%.4x\n",
			mii->phy_id, devad, reg, retval);

	return retval;
}

static int phy_c45_write(int argc, char **argv)
{
	struct mii_data *mii = (struct mii_data *)&ifr.ifr_data;
	int addr, devad, reg, val, err;

	if (argc == 7) {
		addr = strtol(argv[3], NULL, 0);
		devad = strtol(argv[4], NULL, 0);
		reg = strtol(argv[5], NULL, 0);
		val = strtol(argv[6], NULL, 0);
		if (addr > 31 || addr < 0) {
			printf("0<= phyaddr <=31\n");
			return -1;
		}
	} else if (argc == 6) {
		err = phydrv_register_addr_config(argv);
		if (err < 0)
			return err;
		addr = mii->phy_id;
		devad = strtol(argv[3], NULL, 0);
		reg = strtol(argv[4], NULL, 0);
		val = strtol(argv[5], NULL, 0);
	} else {
		help((void *)&argv[1][0]);
		return -1;
	}

	mii->phy_id = PHY_IS_C45 | (PHY_C45_MASK & (addr << 5)) | (devad & 0x1f);
	mdio_c45_write(devad, reg, val);
	printf("phyaddr: %d devad: %d reg: %d val: 0x%.4x\n",
			((mii->phy_id & PHY_C45_MASK) >> 5), devad, reg, val);
	return 0;
}

static int phy_c45_read(int argc, char **argv)
{
	struct mii_data *mii = (struct mii_data *)&ifr.ifr_data;
	int retval, addr, devad, reg, err;

	if (argc == 6) {
		addr = strtol(argv[3], NULL, 0);
		devad = strtol(argv[4], NULL, 0);
		reg = strtol(argv[5], NULL, 0);
		if (addr > 31 || addr < 0) {
			printf("0<= phyaddr <=31\n");
			return -1;
		}
	} else if (argc == 5) {
		err = phydrv_register_addr_config(argv);
		if (err < 0)
			return err;
		addr = mii->phy_id;
		devad = strtol(argv[3], NULL, 0);
		reg = strtol(argv[4], NULL, 0);
	} else {
		help((void *)&argv[1][0]);
		return -1;
	}

	mii->phy_id = PHY_IS_C45 | (PHY_C45_MASK & (addr << 5)) | (devad & 0x1f);
	retval = mdio_c45_read(devad, reg);
	printf("phyaddr: %d devad: %d reg: %d val: 0x%.4x\n",
			((mii->phy_id & PHY_C45_MASK) >> 5), devad, reg, retval);
	return retval;
}

int main(int argc, char **argv)
{
	int retval;

	if(7 < argc || argc < 2 || !argv[2]) {
		help((void *)argv);
		return 0;
	}

	/* Open a basic socket. */
	if ((skfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
		perror("socket");
		return -1;
	}

	/* Get the vitals from the interface. */
	strncpy(ifr.ifr_name, argv[2], IFNAMSIZ);

	if(argv[1][0] == 'r') {
		retval = phy_read(argc, argv);
		if (retval < 0) {
			printf("Unexcept error for phy read!\n");
			return -1;
		}
	} else if(argv[1][0] == 'w') {
		phy_write(argc, argv);
	} else if (argv[1][0] == 'm') {
		if (argv[1][1] == 'w') {
			phy_mmd_write(argc, argv);
		} else if (argv[1][1] == 'r') {
			phy_mmd_read(argc, argv);
		} else {
			help((void *)&argv[1][0]);
		}
	} else if (argv[1][0] == 'd') {
		if (argv[1][1] == 'w') {
			phy_c45_write(argc, argv);
		} else if (argv[1][1] == 'r') {
			phy_c45_read(argc, argv);
		} else {
			help((void *)&argv[1][0]);
		}

	} else {
		help((void *)&argv[1][0]);
	}

	close(skfd);
}

mii.h 

#ifndef _NETTOOL_MII_H
#define _NETTOOLS_MII_H

#include <linux/sockios.h>

/* network interface ioctl's for MII commands */
#ifndef SIOCGMIIPHY
#warning "SIOCGMIIPHY is not defined by your kernel source"
#define SIOCGMIIPHY (SIOCDEVPRIVATE)	/* Read from current PHY */
#define SIOCGMIIREG (SIOCDEVPRIVATE+1) 	/* Read any PHY register */
#define SIOCSMIIREG (SIOCDEVPRIVATE+2) 	/* Write any PHY register */
#define SIOCGPARAMS (SIOCDEVPRIVATE+3) 	/* Read operational parameters */
#define SIOCSPARAMS (SIOCDEVPRIVATE+4) 	/* Set operational parameters */
#endif

#include <linux/types.h>

/* This data structure is used for all the MII ioctl's */
struct mii_data {
    __u16	phy_id;
    __u16	reg_num;
    __u16	val_in;
    __u16	val_out;
};

/* Basic Mode Control Register */
#define MII_BMCR		0x00
#define MII_BMCR_RESET		0x8000
#define MII_BMCR_LOOPBACK	0x4000
#define MII_BMCR_100MBIT	0x2000
#define MII_BMCR_AN_ENA		0x1000
#define MII_BMCR_ISOLATE	0x0400
#define MII_BMCR_RESTART	0x0200
#define MII_BMCR_DUPLEX		0x0100
#define MII_BMCR_COLTEST	0x0080
#define MII_BMCR_SPEED1000	0x0040

/* Basic Mode Status Register */
#define MII_BMSR		0x01
#define MII_BMSR_CAP_MASK	0xf800
#define MII_BMSR_100BASET4	0x8000
#define MII_BMSR_100BASETX_FD	0x4000
#define MII_BMSR_100BASETX_HD	0x2000
#define MII_BMSR_10BASET_FD	0x1000
#define MII_BMSR_10BASET_HD	0x0800
#define MII_BMSR_NO_PREAMBLE	0x0040
#define MII_BMSR_AN_COMPLETE	0x0020
#define MII_BMSR_REMOTE_FAULT	0x0010
#define MII_BMSR_AN_ABLE	0x0008
#define MII_BMSR_LINK_VALID	0x0004
#define MII_BMSR_JABBER		0x0002
#define MII_BMSR_EXT_CAP	0x0001

#define MII_PHY_ID1		0x02
#define MII_PHY_ID2		0x03

/* Auto-Negotiation Advertisement Register */
#define MII_ANAR		0x04
/* Auto-Negotiation Link Partner Ability Register */
#define MII_ANLPAR		0x05
#define MII_AN_NEXT_PAGE	0x8000
#define MII_AN_ACK		0x4000
#define MII_AN_REMOTE_FAULT	0x2000
#define MII_AN_ABILITY_MASK	0x07e0
#define MII_AN_FLOW_CONTROL	0x0400
#define MII_AN_100BASET4	0x0200
#define MII_AN_100BASETX_FD	0x0100
#define MII_AN_100BASETX_HD	0x0080
#define MII_AN_10BASET_FD	0x0040
#define MII_AN_10BASET_HD	0x0020
#define MII_AN_PROT_MASK	0x001f
#define MII_AN_PROT_802_3	0x0001

/* Auto-Negotiation Expansion Register */
#define MII_ANER		0x06
#define MII_ANER_MULT_FAULT	0x0010
#define MII_ANER_LP_NP_ABLE	0x0008
#define MII_ANER_NP_ABLE	0x0004
#define MII_ANER_PAGE_RX	0x0002
#define MII_ANER_LP_AN_ABLE	0x0001

#define MII_CTRL1000		0x09
#define MII_BMCR2_1000FULL	0x0200
#define MII_BMCR2_1000HALF	0x0100

#define MII_STAT1000		0x0a
#define MII_LPA2_1000LOCALOK	0x2000
#define MII_LPA2_1000REMRXOK	0x1000
#define MII_LPA2_1000FULL	0x0800
#define MII_LPA2_1000HALF	0x0400

/* Last register we need for show_basic_mii() */
#define MII_BASIC_MAX          (MII_STAT1000+1)

#endif /* _NETTOOLS_MII_H */