static int bfin_mac_hwtstamp_ioctl(struct net_device *netdev,
		struct ifreq *ifr, int cmd)
{
	struct hwtstamp_config config;
	struct bfin_mac_local *lp = netdev_priv(netdev);
	u16 ptpctl;
	u32 ptpfv1, ptpfv2, ptpfv3, ptpfoff;

	if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
		return -EFAULT;

	pr_debug("%s config flag:0x%x, tx_type:0x%x, rx_filter:0x%x\n",
			__func__, config.flags, config.tx_type, config.rx_filter);

	
	if (config.flags)
		return -EINVAL;

	if ((config.tx_type != HWTSTAMP_TX_OFF) &&
			(config.tx_type != HWTSTAMP_TX_ON))
		return -ERANGE;

	ptpctl = bfin_read_EMAC_PTP_CTL();

	switch (config.rx_filter) {
	case HWTSTAMP_FILTER_NONE:
		ptpfv3 = 0xFFFFFFFF;
		bfin_write_EMAC_PTP_FV3(ptpfv3);
		break;
	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
		ptpctl &= ~0x1F00;
		bfin_write_EMAC_PTP_CTL(ptpctl);
		ptpfoff = 0x4A24170C;
		bfin_write_EMAC_PTP_FOFF(ptpfoff);
		ptpfv1 = 0x11040800;
		bfin_write_EMAC_PTP_FV1(ptpfv1);
		ptpfv2 = 0x0140013F;
		bfin_write_EMAC_PTP_FV2(ptpfv2);
		ptpfv3 = 0xFFFFFFFC;
		bfin_write_EMAC_PTP_FV3(ptpfv3);

		config.rx_filter = HWTSTAMP_FILTER_PTP_V1_L4_EVENT;
		break;
	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
		ptpctl &= ~0x1F00;
		bfin_write_EMAC_PTP_CTL(ptpctl);
		ptpfoff = 0x2A24170C;
		bfin_write_EMAC_PTP_FOFF(ptpfoff);
		ptpfv1 = 0x11040800;
		bfin_write_EMAC_PTP_FV1(ptpfv1);
		ptpfv2 = 0x0140013F;
		bfin_write_EMAC_PTP_FV2(ptpfv2);
		ptpfv3 = 0xFFFFFFF0;
		bfin_write_EMAC_PTP_FV3(ptpfv3);

		config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
		break;
	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
		ptpctl &= ~0x1100;
		bfin_write_EMAC_PTP_CTL(ptpctl);
		ptpfoff = 0x0E24170C;
		bfin_write_EMAC_PTP_FOFF(ptpfoff);
		ptpfv1 = 0x110488F7;
		bfin_write_EMAC_PTP_FV1(ptpfv1);
		ptpfv2 = 0x0140013F;
		bfin_write_EMAC_PTP_FV2(ptpfv2);
		ptpfv3 = 0xFFFFFFF0;
		bfin_write_EMAC_PTP_FV3(ptpfv3);

		config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
		break;
	default:
		return -ERANGE;
	}

	if (config.tx_type == HWTSTAMP_TX_OFF &&
	    bfin_mac_hwtstamp_is_none(config.rx_filter)) {
		ptpctl &= ~PTP_EN;
		bfin_write_EMAC_PTP_CTL(ptpctl);

		SSYNC();
	} else {
		ptpctl |= PTP_EN;
		bfin_write_EMAC_PTP_CTL(ptpctl);

		bfin_read_EMAC_PTP_RXSNAPLO();
		bfin_read_EMAC_PTP_RXSNAPHI();

		bfin_read_EMAC_PTP_TXSNAPLO();
		bfin_read_EMAC_PTP_TXSNAPHI();

		bfin_write_EMAC_PTP_TIMELO(0x00000000);
		bfin_write_EMAC_PTP_TIMEHI(0xFF800000);

		SSYNC();

		lp->compare.last_update = 0;
		timecounter_init(&lp->clock,
				&lp->cycles,
				ktime_to_ns(ktime_get_real()));
		timecompare_update(&lp->compare, 0);
	}

	lp->stamp_cfg = config;
	return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
		-EFAULT : 0;
}
static int bfin_mac_hwtstamp_ioctl(struct net_device *netdev,
		struct ifreq *ifr, int cmd)
{
	struct hwtstamp_config config;
	struct bfin_mac_local *lp = netdev_priv(netdev);
	u16 ptpctl;
	u32 ptpfv1, ptpfv2, ptpfv3, ptpfoff;

	if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
		return -EFAULT;

	pr_debug("%s config flag:0x%x, tx_type:0x%x, rx_filter:0x%x\n",
			__func__, config.flags, config.tx_type, config.rx_filter);

	/* reserved for future extensions */
	if (config.flags)
		return -EINVAL;

	if ((config.tx_type != HWTSTAMP_TX_OFF) &&
			(config.tx_type != HWTSTAMP_TX_ON))
		return -ERANGE;

	ptpctl = bfin_read_EMAC_PTP_CTL();

	switch (config.rx_filter) {
	case HWTSTAMP_FILTER_NONE:
		/*
		 * Dont allow any timestamping
		 */
		ptpfv3 = 0xFFFFFFFF;
		bfin_write_EMAC_PTP_FV3(ptpfv3);
		break;
	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
		/*
		 * Clear the five comparison mask bits (bits[12:8]) in EMAC_PTP_CTL)
		 * to enable all the field matches.
		 */
		ptpctl &= ~0x1F00;
		bfin_write_EMAC_PTP_CTL(ptpctl);
		/*
		 * Keep the default values of the EMAC_PTP_FOFF register.
		 */
		ptpfoff = 0x4A24170C;
		bfin_write_EMAC_PTP_FOFF(ptpfoff);
		/*
		 * Keep the default values of the EMAC_PTP_FV1 and EMAC_PTP_FV2
		 * registers.
		 */
		ptpfv1 = 0x11040800;
		bfin_write_EMAC_PTP_FV1(ptpfv1);
		ptpfv2 = 0x0140013F;
		bfin_write_EMAC_PTP_FV2(ptpfv2);
		/*
		 * The default value (0xFFFC) allows the timestamping of both
		 * received Sync messages and Delay_Req messages.
		 */
		ptpfv3 = 0xFFFFFFFC;
		bfin_write_EMAC_PTP_FV3(ptpfv3);

		config.rx_filter = HWTSTAMP_FILTER_PTP_V1_L4_EVENT;
		break;
	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
		/* Clear all five comparison mask bits (bits[12:8]) in the
		 * EMAC_PTP_CTL register to enable all the field matches.
		 */
		ptpctl &= ~0x1F00;
		bfin_write_EMAC_PTP_CTL(ptpctl);
		/*
		 * Keep the default values of the EMAC_PTP_FOFF register, except set
		 * the PTPCOF field to 0x2A.
		 */
		ptpfoff = 0x2A24170C;
		bfin_write_EMAC_PTP_FOFF(ptpfoff);
		/*
		 * Keep the default values of the EMAC_PTP_FV1 and EMAC_PTP_FV2
		 * registers.
		 */
		ptpfv1 = 0x11040800;
		bfin_write_EMAC_PTP_FV1(ptpfv1);
		ptpfv2 = 0x0140013F;
		bfin_write_EMAC_PTP_FV2(ptpfv2);
		/*
		 * To allow the timestamping of Pdelay_Req and Pdelay_Resp, set
		 * the value to 0xFFF0.
		 */
		ptpfv3 = 0xFFFFFFF0;
		bfin_write_EMAC_PTP_FV3(ptpfv3);

		config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
		break;
	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
		/*
		 * Clear bits 8 and 12 of the EMAC_PTP_CTL register to enable only the
		 * EFTM and PTPCM field comparison.
		 */
		ptpctl &= ~0x1100;
		bfin_write_EMAC_PTP_CTL(ptpctl);
		/*
		 * Keep the default values of all the fields of the EMAC_PTP_FOFF
		 * register, except set the PTPCOF field to 0x0E.
		 */
		ptpfoff = 0x0E24170C;
		bfin_write_EMAC_PTP_FOFF(ptpfoff);
		/*
		 * Program bits [15:0] of the EMAC_PTP_FV1 register to 0x88F7, which
		 * corresponds to PTP messages on the MAC layer.
		 */
		ptpfv1 = 0x110488F7;
		bfin_write_EMAC_PTP_FV1(ptpfv1);
		ptpfv2 = 0x0140013F;
		bfin_write_EMAC_PTP_FV2(ptpfv2);
		/*
		 * To allow the timestamping of Pdelay_Req and Pdelay_Resp
		 * messages, set the value to 0xFFF0.
		 */
		ptpfv3 = 0xFFFFFFF0;
		bfin_write_EMAC_PTP_FV3(ptpfv3);

		config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
		break;
	default:
		return -ERANGE;
	}

	if (config.tx_type == HWTSTAMP_TX_OFF &&
	    bfin_mac_hwtstamp_is_none(config.rx_filter)) {
		ptpctl &= ~PTP_EN;
		bfin_write_EMAC_PTP_CTL(ptpctl);

		SSYNC();
	} else {
		ptpctl |= PTP_EN;
		bfin_write_EMAC_PTP_CTL(ptpctl);

		/*
		 * clear any existing timestamp
		 */
		bfin_read_EMAC_PTP_RXSNAPLO();
		bfin_read_EMAC_PTP_RXSNAPHI();

		bfin_read_EMAC_PTP_TXSNAPLO();
		bfin_read_EMAC_PTP_TXSNAPHI();

		/*
		 * Set registers so that rollover occurs soon to test this.
		 */
		bfin_write_EMAC_PTP_TIMELO(0x00000000);
		bfin_write_EMAC_PTP_TIMEHI(0xFF800000);

		SSYNC();

		lp->compare.last_update = 0;
		timecounter_init(&lp->clock,
				&lp->cycles,
				ktime_to_ns(ktime_get_real()));
		timecompare_update(&lp->compare, 0);
	}

	lp->stamp_cfg = config;
	return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
		-EFAULT : 0;
}