From 2731b2eadeaa141e6f305fa8086106608112bbaa Mon Sep 17 00:00:00 2001 From: Roshni Shah Date: Mon, 14 Mar 2011 06:49:42 -0400 Subject: Add support for the i.MX53 QSB This patch seems to have originated from the 11.01.00 release from Freescale, which is no longer available except through the gitweb interface from Freescale. http://opensource.freescale.com/git?p=imx/linux-2.6-imx.git;a=commit;h=27fdf7bae11978d21e8aba09bb635f49b07edd4a --- drivers/net/imx_ptp.c | 1748 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 1748 insertions(+) create mode 100644 drivers/net/imx_ptp.c (limited to 'drivers/net/imx_ptp.c') diff --git a/drivers/net/imx_ptp.c b/drivers/net/imx_ptp.c new file mode 100644 index 000000000000..8580b1c14a66 --- /dev/null +++ b/drivers/net/imx_ptp.c @@ -0,0 +1,1748 @@ +/* + * drivers/net/imx_ptp.c + * + * Copyright (C) 2010 Freescale Semiconductor, Inc. All rights reserved. + * + * Description: IEEE 1588 driver supporting imx5 Fast Ethernet Controller. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation; either version 2 of the License, or (at your + * option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "fec.h" +#include "fec_1588.h" +#include "imx_ptp.h" + +#ifdef PTP_DEBUG +#define VDBG(fmt, args...) printk(KERN_DEBUG "[%s] " fmt "\n", \ + __func__, ## args) +#else +#define VDBG(fmt, args...) do {} while (0) +#endif + +static DECLARE_WAIT_QUEUE_HEAD(ptp_rx_ts_wait); +static DECLARE_WAIT_QUEUE_HEAD(ptp_tx_ts_wait); +#define PTP_GET_RX_TIMEOUT (HZ/10) +#define PTP_GET_TX_TIMEOUT (HZ/100) + +static struct fec_ptp_private *ptp_private; +static void ptp_rtc_get_current_time(struct ptp *p_ptp, + struct ptp_time *p_time); +static struct ptp *ptp_dev; + +/* The ring resource create and manage */ +static int fec_ptp_init_circ(struct circ_buf *ptp_buf, int size) +{ + ptp_buf->buf = vmalloc(size * sizeof(struct fec_ptp_data_t)); + + if (!ptp_buf->buf) + return 1; + ptp_buf->head = 0; + ptp_buf->tail = 0; + + return 0; +} + +static inline int fec_ptp_calc_index(int size, int curr_index, int offset) +{ + return (curr_index + offset) % size; +} + +static int fec_ptp_is_empty(struct circ_buf *buf) +{ + return (buf->head == buf->tail); +} + +static int fec_ptp_nelems(struct circ_buf *buf, int size) +{ + const int front = buf->head; + const int end = buf->tail; + int n_items; + + if (end > front) + n_items = end - front; + else if (end < front) + n_items = size - (front - end); + else + n_items = 0; + + return n_items; +} + +static int fec_ptp_is_full(struct circ_buf *buf, int size) +{ + if (fec_ptp_nelems(buf, size) == (size - 1)) + return 1; + else + return 0; +} + +static int fec_ptp_insert(struct circ_buf *ptp_buf, + struct fec_ptp_data_t *data, + struct fec_ptp_private *priv, + int size) +{ + struct fec_ptp_data_t *tmp; + + if (fec_ptp_is_full(ptp_buf, size)) + return 1; + + spin_lock(&priv->ptp_lock); + tmp = (struct fec_ptp_data_t *)(ptp_buf->buf) + ptp_buf->tail; + + tmp->key = data->key; + memcpy(tmp->spid, data->spid, 10); + tmp->ts_time.sec = data->ts_time.sec; + tmp->ts_time.nsec = data->ts_time.nsec; + + ptp_buf->tail = fec_ptp_calc_index(size, ptp_buf->tail, 1); + spin_unlock(&priv->ptp_lock); + + return 0; +} + +static int fec_ptp_find_and_remove(struct circ_buf *ptp_buf, + struct fec_ptp_data_t *data, + struct fec_ptp_private *priv, + int size) +{ + int i; + int end = ptp_buf->tail; + unsigned long flags; + struct fec_ptp_data_t *tmp; + + if (fec_ptp_is_empty(ptp_buf)) + return 1; + + i = ptp_buf->head; + tmp = (struct fec_ptp_data_t *)(ptp_buf->buf) + i; + while (i != end) { + tmp = (struct fec_ptp_data_t *)(ptp_buf->buf) + i; + if (tmp->key == data->key && + !memcmp(tmp->spid, data->spid, 10)) + break; + i = fec_ptp_calc_index(size, i, 1); + } + + spin_lock_irqsave(&priv->ptp_lock, flags); + if (i == end) { + ptp_buf->head = end; + spin_unlock_irqrestore(&priv->ptp_lock, flags); + return 1; + } + + data->ts_time.sec = tmp->ts_time.sec; + data->ts_time.nsec = tmp->ts_time.nsec; + + ptp_buf->head = fec_ptp_calc_index(size, i, 1); + spin_unlock_irqrestore(&priv->ptp_lock, flags); + + return 0; +} + +/* ptp and rtc param configuration */ +static void rtc_default_param(struct ptp_rtc *rtc) +{ + struct ptp_rtc_driver_param *drv_param = rtc->driver_param; + int i; + + rtc->bypass_compensation = DEFAULT_BYPASS_COMPENSATION; + rtc->output_clock_divisor = DEFAULT_OUTPUT_CLOCK_DIVISOR; + + drv_param->src_clock = DEFAULT_SRC_CLOCK; + drv_param->src_clock_freq_hz = clk_get_rate(rtc->clk); + + drv_param->invert_input_clk_phase = DEFAULT_INVERT_INPUT_CLK_PHASE; + drv_param->invert_output_clk_phase = DEFAULT_INVERT_OUTPUT_CLK_PHASE; + drv_param->pulse_start_mode = DEFAULT_PULSE_START_MODE; + drv_param->events_mask = DEFAULT_EVENTS_RTC_MASK; + + for (i = 0; i < PTP_RTC_NUM_OF_ALARMS; i++) + drv_param->alarm_polarity[i] = DEFAULT_ALARM_POLARITY ; + + for (i = 0; i < PTP_RTC_NUM_OF_TRIGGERS; i++) + drv_param->trigger_polarity[i] = DEFAULT_TRIGGER_POLARITY; +} + +static int ptp_rtc_config(struct ptp_rtc *rtc) +{ + /*allocate memory for RTC driver parameter*/ + rtc->driver_param = kzalloc(sizeof(struct ptp_rtc_driver_param), + GFP_KERNEL); + if (!rtc->driver_param) { + printk(KERN_ERR "allocate memory failed\n"); + return -ENOMEM; + } + + /* expected RTC input clk frequency */ + rtc->driver_param->rtc_freq_hz = PTP_RTC_FREQ * MHZ; + + /*set default RTC configuration parameters*/ + rtc_default_param(rtc); + + return 0; +} + +static void ptp_param_config(struct ptp *p_ptp) +{ + struct ptp_driver_param *drv_param; + + p_ptp->driver_param = kzalloc(sizeof(struct ptp_driver_param), + GFP_KERNEL); + if (!p_ptp->driver_param) { + printk(KERN_ERR "allocate memory failed for " + "PTP driver parameters\n"); + return; + } + + drv_param = p_ptp->driver_param; + /*set the default configuration parameters*/ + drv_param->eth_type_value = ETH_TYPE_VALUE; + drv_param->vlan_type_value = VLAN_TYPE_VALUE; + drv_param->udp_general_port = UDP_GENERAL_PORT; + drv_param->udp_event_port = UDP_EVENT_PORT; + drv_param->ip_type_value = IP_TYPE_VALUE; + drv_param->eth_type_offset = ETH_TYPE_OFFSET; + drv_param->ip_type_offset = IP_TYPE_OFFSET; + drv_param->udp_dest_port_offset = UDP_DEST_PORT_OFFSET; + drv_param->ptp_type_offset = PTP_TYPE_OFFSET; + + drv_param->ptp_msg_codes[e_PTP_MSG_SYNC] = DEFAULT_MSG_SYNC; + drv_param->ptp_msg_codes[e_PTP_MSG_DELAY_REQ] = DEFAULT_MSG_DELAY_REQ; + drv_param->ptp_msg_codes[e_PTP_MSG_FOLLOW_UP] = DEFAULT_MSG_FOLLOW_UP; + drv_param->ptp_msg_codes[e_PTP_MSG_DELAY_RESP] = DEFAULT_MSG_DELAY_RESP; + drv_param->ptp_msg_codes[e_PTP_MSG_MANAGEMENT] = DEFAULT_MSG_MANAGEMENT; +} + +/* 64 bits operation */ +static u32 div64_oper(u64 dividend, u32 divisor, u32 *quotient) +{ + u32 time_h, time_l; + u32 result; + u64 tmp_dividend; + int i; + + time_h = (u32)(dividend >> 32); + time_l = (u32)dividend; + time_h = time_h % divisor; + for (i = 1; i <= 32; i++) { + tmp_dividend = (((u64)time_h << 32) | (u64)time_l); + tmp_dividend = (tmp_dividend << 1); + time_h = (u32)(tmp_dividend >> 32); + time_l = (u32)tmp_dividend; + result = time_h / divisor; + time_h = time_h % divisor; + *quotient += (result << (32 - i)); + } + + return time_h; +} + +/*64 bites add and return the result*/ +static u64 add64_oper(u64 addend, u64 augend) +{ + u64 result = 0; + u32 addendh, addendl, augendl, augendh; + + addendh = (u32)(addend >> 32); + addendl = (u32)addend; + + augendh = (u32)(augend>>32); + augendl = (u32)augend; + + __asm__( + "adds %0,%2,%3\n" + "adc %1,%4,%5" + : "=r" (addendl), "=r" (addendh) + : "r" (addendl), "r" (augendl), "r" (addendh), "r" (augendh) + ); + + udelay(1); + result = (((u64)addendh << 32) | (u64)addendl); + + return result; +} + +/*64 bits multiplication and return the result*/ +static u64 multi64_oper(u32 multiplier, u32 multiplicand) +{ + u64 result = 0; + u64 tmp_ret = 0; + u32 tmp_multi = multiplicand; + int i; + + for (i = 0; i < 32; i++) { + if (tmp_multi & 0x1) { + tmp_ret = ((u64)multiplier << i); + result = add64_oper(result, tmp_ret); + } + tmp_multi = (tmp_multi >> 1); + } + + VDBG("multi 64 low result is 0x%x\n", result); + VDBG("multi 64 high result is 0x%x\n", (u32)(result>>32)); + + return result; +} + +/*convert the 64 bites time stamp to second and nanosecond*/ +static void convert_rtc_time(u64 *rtc_time, struct ptp_time *p_time) +{ + u32 time_h; + u32 time_sec = 0; + + time_h = div64_oper(*rtc_time, NANOSEC_IN_SEC, &time_sec); + + p_time->sec = time_sec; + p_time->nsec = time_h; +} + +/* convert rtc time to 64 bites timestamp */ +static u64 convert_unsigned_time(struct ptp_time *ptime) +{ + return add64_oper(multi64_oper(ptime->sec, NANOSEC_IN_SEC), + (u64)ptime->nsec); +} + +/*RTC interrupt handler*/ +static irqreturn_t ptp_rtc_interrupt(int irq, void *_ptp) +{ + struct ptp *p_ptp = (struct ptp *)_ptp; + struct ptp_rtc *rtc = p_ptp->rtc; + struct ptp_time time; + register u32 events; + + /*get valid events*/ + events = readl(rtc->mem_map + PTP_TMR_TEVENT); + + /*clear event bits*/ + writel(events, rtc->mem_map + PTP_TMR_TEVENT); + + /*get the current time as quickly as possible*/ + ptp_rtc_get_current_time(p_ptp, &time); + + if (events & RTC_TEVENT_ALARM_1) { + p_ptp->alarm_counters[0]++; + VDBG("PTP Alarm 1 event, time = %2d:%09d[sec:nsec]\n", + time.sec, time.nsec); + } + if (events & RTC_TEVENT_ALARM_2) { + p_ptp->alarm_counters[1]++; + VDBG("PTP Alarm 2 event, time = %2d:%09d[sec:nsec]\n", + time.sec, time.nsec); + } + if (events & RTC_TEVENT_PERIODIC_PULSE_1) { + p_ptp->pulse_counters[0]++; + VDBG("PTP Pulse 1 event, time = %2d:%09d[sec:nsec]\n", + time.sec, time.nsec); + } + if (events & RTC_TEVENT_PERIODIC_PULSE_2) { + p_ptp->pulse_counters[1]++; + VDBG("PTP Pulse 2 event, time = %2d:%09d[sec:nsec]\n", + time.sec, time.nsec); + } + if (events & RTC_TEVENT_PERIODIC_PULSE_3) { + p_ptp->pulse_counters[2]++; + VDBG("PTP Pulse 3 event, time = %2d:%09d[sec:nsec]\n", + time.sec, time.nsec); + } + + return IRQ_HANDLED; +} + +static int ptp_rtc_init(struct ptp *p_ptp) +{ + struct ptp_rtc *rtc = p_ptp->rtc; + struct ptp_rtc_driver_param *rtc_drv_param = rtc->driver_param; + void __iomem *rtc_mem = rtc->mem_map; + u32 freq_compensation = 0; + u32 tmr_ctrl = 0; + int ret = 0; + int i; + + rtc = p_ptp->rtc; + rtc_drv_param = rtc->driver_param; + rtc_mem = rtc->mem_map; + + if (!rtc->bypass_compensation) + rtc->clock_period_nansec = NANOSEC_PER_ONE_HZ_TICK / + rtc_drv_param->rtc_freq_hz; + else { + /*In bypass mode,the RTC clock equals the source clock*/ + rtc->clock_period_nansec = NANOSEC_PER_ONE_HZ_TICK / + rtc_drv_param->src_clock_freq_hz; + tmr_ctrl |= RTC_TMR_CTRL_BYP; + } + + tmr_ctrl |= ((rtc->clock_period_nansec << + RTC_TMR_CTRL_TCLK_PERIOD_SHIFT) & + RTC_TMR_CTRL_TCLK_PERIOD_MSK); + + if (rtc_drv_param->invert_input_clk_phase) + tmr_ctrl |= RTC_TMR_CTRL_CIPH; + if (rtc_drv_param->invert_output_clk_phase) + tmr_ctrl |= RTC_TMR_CTRL_COPH; + if (rtc_drv_param->pulse_start_mode == e_PTP_RTC_PULSE_START_ON_ALARM) { + tmr_ctrl |= RTC_TMR_CTRL_FS; + rtc->start_pulse_on_alarm = TRUE; + } + + for (i = 0; i < PTP_RTC_NUM_OF_ALARMS; i++) { + if (rtc_drv_param->alarm_polarity[i] == + e_PTP_RTC_ALARM_POLARITY_ACTIVE_LOW) + tmr_ctrl |= (RTC_TMR_CTRL_ALMP1 >> i); + + } + + /*clear TMR_ALARM registers*/ + writel(0xFFFFFFFF, rtc_mem + PTP_TMR_ALARM1_L); + writel(0xFFFFFFFF, rtc_mem + PTP_TMR_ALARM1_H); + writel(0xFFFFFFFF, rtc_mem + PTP_TMR_ALARM2_L); + writel(0xFFFFFFFF, rtc_mem + PTP_TMR_ALARM2_H); + + for (i = 0; i < PTP_RTC_NUM_OF_TRIGGERS; i++) { + if (rtc_drv_param->trigger_polarity[i] == + e_PTP_RTC_TRIGGER_ON_FALLING_EDGE) + tmr_ctrl |= (RTC_TMR_CTRL_ETEP1 << i); + } + + /*clear TMR_FIPER registers*/ + writel(0xFFFFFFFF, rtc_mem + PTP_TMR_FIPER1); + writel(0xFFFFFFFF, rtc_mem + PTP_TMR_FIPER2); + writel(0xFFFFFFFF, rtc_mem + PTP_TMR_FIPER3); + + /*set the source clock*/ + /*use a clock from the QE bank of clocks*/ + tmr_ctrl |= RTC_TMR_CTRL_CKSEL_EXT_CLK; + + /*write register and perform software reset*/ + writel((tmr_ctrl | RTC_TMR_CTRL_TMSR), rtc_mem + PTP_TMR_CTRL); + writel(tmr_ctrl, rtc_mem + PTP_TMR_CTRL); + + /*clear TMR_TEVEMT*/ + writel(RTC_EVENT_ALL, rtc_mem + PTP_TMR_TEVENT); + + /*initialize TMR_TEMASK*/ + writel(rtc_drv_param->events_mask, rtc_mem + PTP_TMR_TEMASK); + + /*initialize TMR_ADD with the initial frequency compensation value: + freq_compensation = (2^32 / frequency ratio)*/ + div64_oper(((u64)rtc_drv_param->rtc_freq_hz << 32), + rtc_drv_param->src_clock_freq_hz, &freq_compensation); + p_ptp->orig_freq_comp = freq_compensation; + writel(freq_compensation, rtc_mem + PTP_TMR_ADD); + + /*initialize TMR_PRSC*/ + writel(rtc->output_clock_divisor, rtc_mem + PTP_TMR_PRSC); + + /*initialize TMR_OFF*/ + writel(0, rtc_mem + PTP_TMR_OFF_L); + writel(0, rtc_mem + PTP_TMR_OFF_H); + + return ret; +} + +static void init_ptp_parser(struct ptp *p_ptp) +{ + void __iomem *mem_map = p_ptp->mem_map; + struct ptp_driver_param *drv_param = p_ptp->driver_param; + u32 reg32; + + /*initialzie PTP TSPDR1*/ + reg32 = ((drv_param->eth_type_value << PTP_TSPDR1_ETT_SHIFT) & + PTP_TSPDR1_ETT_MASK); + reg32 |= ((drv_param->ip_type_value << PTP_TSPDR1_IPT_SHIFT) & + PTP_TSPDR1_IPT_MASK); + writel(reg32, mem_map + PTP_TSPDR1); + + /*initialize PTP TSPDR2*/ + reg32 = ((drv_param->udp_general_port << PTP_TSPDR2_DPNGE_SHIFT) & + PTP_TSPDR2_DPNGE_MASK); + reg32 |= (drv_param->udp_event_port & PTP_TSPDR2_DPNEV_MASK); + writel(reg32, mem_map + PTP_TSPDR2); + + /*initialize PTP TSPDR3*/ + reg32 = ((drv_param->ptp_msg_codes[e_PTP_MSG_SYNC] << + PTP_TSPDR3_SYCTL_SHIFT) & PTP_TSPDR3_SYCTL_MASK); + reg32 |= ((drv_param->ptp_msg_codes[e_PTP_MSG_DELAY_REQ] << + PTP_TSPDR3_DRCTL_SHIFT) & PTP_TSPDR3_DRCTL_MASK); + reg32 |= ((drv_param->ptp_msg_codes[e_PTP_MSG_DELAY_RESP] << + PTP_TSPDR3_DRPCTL_SHIFT) & PTP_TSPDR3_DRPCTL_MASK); + reg32 |= (drv_param->ptp_msg_codes[e_PTP_MSG_FOLLOW_UP] & + PTP_TSPDR3_FUCTL_MASK); + writel(reg32, mem_map + PTP_TSPDR3); + + /*initialzie PTP TSPDR4*/ + reg32 = ((drv_param->ptp_msg_codes[e_PTP_MSG_MANAGEMENT] << + PTP_TSPDR4_MACTL_SHIFT) & PTP_TSPDR4_MACTL_MASK); + reg32 |= (drv_param->vlan_type_value & PTP_TSPDR4_VLAN_MASK); + writel(reg32, mem_map + PTP_TSPDR4); + + /*initialize PTP TSPOV*/ + reg32 = ((drv_param->eth_type_offset << PTP_TSPOV_ETTOF_SHIFT) & + PTP_TSPOV_ETTOF_MASK); + reg32 |= ((drv_param->ip_type_offset << PTP_TSPOV_IPTOF_SHIFT) & + PTP_TSPOV_IPTOF_MASK); + reg32 |= ((drv_param->udp_dest_port_offset << PTP_TSPOV_UDOF_SHIFT) & + PTP_TSPOV_UDOF_MASK); + reg32 |= (drv_param->ptp_type_offset & PTP_TSPOV_PTOF_MASK); + writel(reg32, mem_map + PTP_TSPOV); +} + +/* compatible with MXS 1588 */ +#ifdef CONFIG_IN_BAND +void fec_ptp_store_txstamp(struct fec_ptp_private *priv, + struct sk_buff *skb, + struct bufdesc *bdp) +{ + int msg_type, seq_id, control; + struct fec_ptp_data_t tmp_tx_time, tmp; + struct fec_ptp_private *fpp = priv; + struct ptp *p_ptp = ptp_dev; + int flag; + unsigned char *sp_id; + unsigned short portnum; + u64 timestamp; + + /* Check for PTP Event */ + if ((bdp->cbd_sc & BD_ENET_TX_PTP) == 0) + return; + + /* Get ts from tx ts queue */ + memset(&tmp, 0, sizeof(struct fec_ptp_data_t)); + tmp.key = SEQ_ID_OUT_OF_BAND; + flag = fec_ptp_find_and_remove(&(priv->txstamp), &tmp, + priv, DEFAULT_PTP_TX_BUF_SZ); + if (!flag) { + tmp_tx_time.ts_time.sec = tmp.ts_time.sec; + tmp_tx_time.ts_time.nsec = tmp.ts_time.nsec; + } else { + /*read timestamp from register*/ + timestamp = ((u64)readl(p_ptp->mem_map + PTP_TMR_TXTS_H) + << 32) | + (readl(p_ptp->mem_map + PTP_TMR_TXTS_L)); + convert_rtc_time(×tamp, &(tmp_tx_time.ts_time)); + } + + seq_id = *((u16 *)(skb->data + FEC_PTP_SEQ_ID_OFFS)); + control = *((u8 *)(skb->data + FEC_PTP_CTRL_OFFS)); + sp_id = skb->data + FEC_PTP_SPORT_ID_OFFS; + portnum = ntohs(*((unsigned short *)(sp_id + 8))); + + tmp_tx_time.key = ntohs(seq_id); + memcpy(tmp_tx_time.spid, sp_id, 8); + memcpy(tmp_tx_time.spid + 8, (unsigned char *)&portnum, 2); + + switch (control) { + + case PTP_MSG_SYNC: + fec_ptp_insert(&(priv->tx_time_sync), &tmp_tx_time, priv, + DEFAULT_PTP_TX_BUF_SZ); + break; + + case PTP_MSG_DEL_REQ: + fec_ptp_insert(&(priv->tx_time_del_req), &tmp_tx_time, priv, + DEFAULT_PTP_TX_BUF_SZ); + break; + + /* clear transportSpecific field*/ + case PTP_MSG_ALL_OTHER: + msg_type = (*((u8 *)(skb->data + + FEC_PTP_MSG_TYPE_OFFS))) & 0x0F; + switch (msg_type) { + case PTP_MSG_P_DEL_REQ: + fec_ptp_insert(&(priv->tx_time_pdel_req), &tmp_tx_time, + priv, DEFAULT_PTP_TX_BUF_SZ); + break; + case PTP_MSG_P_DEL_RESP: + fec_ptp_insert(&(priv->tx_time_pdel_resp), &tmp_tx_time, + priv, DEFAULT_PTP_TX_BUF_SZ); + break; + default: + break; + } + break; + default: + break; + } + + wake_up_interruptible(&ptp_tx_ts_wait); +} +#else +void fec_ptp_store_txstamp(struct fec_ptp_private *priv, + struct sk_buff *skb, + struct bufdesc *bdp) +{ +} +#endif + +static void ptp_store_txstamp(struct fec_ptp_private *priv, + struct ptp *p_ptp, + struct ptp_time *pts, + u32 events) +{ + struct fec_ptp_data_t tmp_tx_time; + u16 seq_id; + + seq_id = SEQ_ID_OUT_OF_BAND; + + memset(&tmp_tx_time, 0, sizeof(struct fec_ptp_data_t)); + tmp_tx_time.key = ntohs(seq_id); + tmp_tx_time.ts_time.sec = pts->sec; + tmp_tx_time.ts_time.nsec = pts->nsec; + fec_ptp_insert(&(priv->txstamp), &tmp_tx_time, + priv, DEFAULT_PTP_TX_BUF_SZ); +} + +/* out-of-band rx ts store */ +static void ptp_store_rxstamp(struct fec_ptp_private *priv, + struct ptp *p_ptp, + struct ptp_time *pts, + u32 events) +{ + int control = PTP_MSG_ALL_OTHER; + u16 seq_id; + struct fec_ptp_data_t tmp_rx_time; + + /* out-of-band mode can't get seq_id */ + seq_id = SEQ_ID_OUT_OF_BAND; + + memset(&tmp_rx_time, 0, sizeof(struct fec_ptp_data_t)); + tmp_rx_time.key = ntohs(seq_id); + tmp_rx_time.ts_time.sec = pts->sec; + tmp_rx_time.ts_time.nsec = pts->nsec; + if (events & PTP_TS_RX_SYNC1) + control = PTP_MSG_SYNC; + else if (events & PTP_TS_RX_DELAY_REQ1) + control = PTP_MSG_DEL_REQ; + else if (events & PTP_TS_PDRQRE1) + control = PTP_MSG_P_DEL_REQ; + else if (events & PTP_TS_PDRSRE1) + control = PTP_MSG_DEL_RESP; + + switch (control) { + case PTP_MSG_SYNC: + fec_ptp_insert(&(priv->rx_time_sync), &tmp_rx_time, + priv, DEFAULT_PTP_RX_BUF_SZ); + break; + + case PTP_MSG_DEL_REQ: + fec_ptp_insert(&(priv->rx_time_del_req), &tmp_rx_time, + priv, DEFAULT_PTP_RX_BUF_SZ); + break; + + case PTP_MSG_P_DEL_REQ: + fec_ptp_insert(&(priv->rx_time_pdel_req), &tmp_rx_time, + priv, DEFAULT_PTP_RX_BUF_SZ); + break; + + case PTP_MSG_P_DEL_RESP: + fec_ptp_insert(&(priv->rx_time_pdel_resp), &tmp_rx_time, + priv, DEFAULT_PTP_RX_BUF_SZ); + break; + + default: + break; + } + + wake_up_interruptible(&ptp_rx_ts_wait); + +} + +/* in-band rx ts store */ +#ifdef CONFIG_IN_BAND +void fec_ptp_store_rxstamp(struct fec_ptp_private *priv, + struct sk_buff *skb, + struct bufdesc *bdp) +{ + int msg_type, seq_id, control; + struct fec_ptp_data_t tmp_rx_time; + struct fec_ptp_private *fpp = priv; + u64 timestamp; + unsigned char *sp_id; + unsigned short portnum; + + /* Check for PTP Event */ + if ((bdp->cbd_sc & BD_ENET_RX_PTP) == 0) { + skb_pull(skb, 8); + return; + } + + /* Get ts from skb data */ + timestamp = *((u64 *)(skb->data)); + convert_rtc_time(×tamp, &(tmp_rx_time.ts_time)); + skb_pull(skb, 8); + + seq_id = *((u16 *)(skb->data + FEC_PTP_SEQ_ID_OFFS)); + control = *((u8 *)(skb->data + FEC_PTP_CTRL_OFFS)); + sp_id = skb->data + FEC_PTP_SPORT_ID_OFFS; + portnum = ntohs(*((unsigned short *)(sp_id + 8))); + + tmp_rx_time.key = ntohs(seq_id); + memcpy(tmp_rx_time.spid, sp_id, 8); + memcpy(tmp_rx_time.spid + 8, (unsigned char *)&portnum, 2); + + switch (control) { + + case PTP_MSG_SYNC: + fec_ptp_insert(&(priv->rx_time_sync), &tmp_rx_time, priv, + DEFAULT_PTP_RX_BUF_SZ); + break; + + case PTP_MSG_DEL_REQ: + fec_ptp_insert(&(priv->rx_time_del_req), &tmp_rx_time, priv, + DEFAULT_PTP_RX_BUF_SZ); + break; + + /* clear transportSpecific field*/ + case PTP_MSG_ALL_OTHER: + msg_type = (*((u8 *)(skb->data + + FEC_PTP_MSG_TYPE_OFFS))) & 0x0F; + switch (msg_type) { + case PTP_MSG_P_DEL_REQ: + fec_ptp_insert(&(priv->rx_time_pdel_req), &tmp_rx_time, + priv, DEFAULT_PTP_RX_BUF_SZ); + break; + case PTP_MSG_P_DEL_RESP: + fec_ptp_insert(&(priv->rx_time_pdel_resp), &tmp_rx_time, + priv, DEFAULT_PTP_RX_BUF_SZ); + break; + default: + break; + } + break; + default: + break; + } + + wake_up_interruptible(&ptp_rx_ts_wait); +} +#else +void fec_ptp_store_rxstamp(struct fec_ptp_private *priv, + struct sk_buff *skb, + struct bufdesc *bdp) +{ +} +#endif + +/*PTP interrupt handler*/ +static irqreturn_t ptp_interrupt(int irq, void *dev_id) +{ + struct ptp *p_ptp = (struct ptp *)dev_id; + void __iomem *mem_map = p_ptp->mem_map; + struct ptp_time ps; + u64 timestamp; + u32 events, orig_events; + + /*get valid events*/ + events = readl(mem_map + PTP_TMR_PEVENT); + while (events) { + if (events & PTP_TS_TX_FRAME1) { + /*read timestamp from register*/ + timestamp = ((u64)readl(mem_map + PTP_TMR_TXTS_H) + << 32) | + (readl(mem_map + PTP_TMR_TXTS_L)); + + /*clear event ASAP,hoping to prevent overrun*/ + writel((u32)PTP_TS_TX_FRAME1, + mem_map + PTP_TMR_PEVENT); + /*check for overrun(which incalidates last timestamp)*/ + events = readl(mem_map + PTP_TMR_PEVENT); + + if (events & PTP_TS_TX_OVR1) { + /*lost synchronization with TX timestamps*/ + /*clear overrun event*/ + writel(PTP_TS_TX_OVR1, + mem_map + PTP_TMR_PEVENT); + + p_ptp->tx_time_stamps_overrun++; + } else { + /*insert the Tx timestamps into the queue*/ + convert_rtc_time(×tamp, &ps); + ptp_store_txstamp(ptp_private, p_ptp, + &ps, orig_events); + + /*this event is never really masked, + *but it should be reported only + *if includeed in usre events mask*/ + if (p_ptp->events_mask & PTP_TS_TX_FRAME1) + p_ptp->tx_time_stamps++; + VDBG("tx interrupt\n"); + } + } + + /*typically only one of these events is relevant, + *depending on whether the device is PTP master or slave*/ + if (events & PTP_TS_RX_ALL) { + /*out-of-band mode:read timestamp + *from registers*/ + timestamp = ((u64)readl(mem_map + PTP_TMR_RXTS_H) + << 32) | + (readl(mem_map + PTP_TMR_RXTS_L)); + + /*clear event ASAP,hoping to prevent overrun*/ + orig_events = events; + writel((u32)(PTP_TS_RX_ALL), + mem_map + PTP_TMR_PEVENT); + + /*check for overrun (which invalidates + *last tiemstamp)*/ + events = readl(mem_map + PTP_TMR_PEVENT); + + if (events & PTP_TS_RX_OVR1) { + /*lost synchronization with Rx timestamp*/ + /*clear overrun event. clear the + *timestamp event as well, because + *it may have arrived after it was + *cleared above,but still it is not + *synchronized with received frames*/ + writel((u32)(PTP_TS_RX_ALL | + PTP_TS_RX_OVR1), + mem_map + PTP_TMR_RXTS_H); + p_ptp->rx_time_stamps_overrun++; + } else { + /*insert Rx timestamp into the queue*/ + convert_rtc_time(×tamp, &ps); + ptp_store_rxstamp(ptp_private, p_ptp, + &ps, orig_events); + + /*the Rx TS event is never masked in + *out-of-ban mode,but it should be + *reported only if included in user's + *event's mask*/ + if (p_ptp->events_mask & + (PTP_TS_RX_SYNC1 | + PTP_TS_RX_DELAY_REQ1)) + p_ptp->rx_time_stamps++; + } + } + + writel(~PTP_TMR_PEVENT_VALID, mem_map + PTP_TMR_PEVENT); + events = readl(mem_map + PTP_TMR_PEVENT); + } + + return IRQ_HANDLED; +} + +static void init_ptp_tsu(struct ptp *p_ptp) +{ + struct ptp_driver_param *drv_param = p_ptp->driver_param; + void __iomem *mem_map; + u32 tsmr, pemask, events_mask; + + mem_map = p_ptp->mem_map; + + /*Tx timestamp events are required in all modes*/ + events_mask = PTP_TS_TX_FRAME1 | PTP_TS_TX_OVR1; + + /*read current values of TSU registers*/ + tsmr = readl(mem_map + PTP_TSMR); + pemask = readl(mem_map + PTP_TMR_PEMASK); + + if (drv_param->delivery_mode == e_PTP_TSU_DELIVERY_IN_BAND) { + tsmr |= PTP_TSMR_OPMODE1_IN_BAND; + events_mask &= ~(PTP_TS_TX_OVR1); + } else + /*rx timestamp events are required for out of band mode*/ + events_mask |= (PTP_TS_RX_SYNC1 | PTP_TS_RX_DELAY_REQ1 | + PTP_TS_RX_OVR1); + + pemask |= events_mask; + + /*update TSU register*/ + writel(tsmr, mem_map + PTP_TSMR); + writel(pemask, mem_map + PTP_TMR_PEMASK); +} + +/* ptp module init */ +static void ptp_tsu_init(struct ptp *p_ptp) +{ + void __iomem *mem_map = p_ptp->mem_map; + + /*initialization of registered PTP modules*/ + init_ptp_parser(p_ptp); + + /*reset timestamp*/ + writel(0, mem_map + PTP_TSMR); + writel(0, mem_map + PTP_TMR_PEMASK); + writel(PTP_TMR_PEVENT_ALL, mem_map + PTP_TMR_PEVENT); + +} + +/* TSU configure function */ +static u32 ptp_tsu_enable(struct ptp *p_ptp) +{ + void __iomem *mem_map; + u32 tsmr; + + /*enable the TSU*/ + mem_map = p_ptp->mem_map; + + /*set the TSU enable bit*/ + tsmr = readl(mem_map + PTP_TSMR); + tsmr |= PTP_TSMR_EN1; + + writel(tsmr, mem_map + PTP_TSMR); + + return 0; +} + +static u32 ptp_tsu_disable(struct ptp *p_ptp) +{ + void __iomem *mem_map; + u32 tsmr; + + mem_map = p_ptp->mem_map; + + tsmr = readl(mem_map + PTP_TSMR); + tsmr &= ~(PTP_TSMR_EN1); + writel(tsmr, mem_map + PTP_TSMR); + + return 0; +} + +static int ptp_tsu_config_events_mask(struct ptp *p_ptp, + u32 events_mask) +{ + + p_ptp->events_mask = events_mask; + + return 0; +} + +/* rtc configure function */ +static u32 rtc_enable(struct ptp_rtc *rtc, bool reset_clock) +{ + u32 tmr_ctrl; + + tmr_ctrl = readl(rtc->mem_map + PTP_TMR_CTRL); + if (reset_clock) { + writel((tmr_ctrl | RTC_TMR_CTRL_TMSR), + rtc->mem_map + PTP_TMR_CTRL); + + /*clear TMR_OFF*/ + writel(0, rtc->mem_map + PTP_TMR_OFF_L); + writel(0, rtc->mem_map + PTP_TMR_OFF_H); + } + + writel((tmr_ctrl | RTC_TMR_CTRL_TE), + rtc->mem_map + PTP_TMR_CTRL); + + return 0; +} + +static u32 rtc_disable(struct ptp_rtc *rtc) +{ + u32 tmr_ctrl; + + tmr_ctrl = readl(rtc->mem_map + PTP_TMR_CTRL); + writel((tmr_ctrl & ~RTC_TMR_CTRL_TE), + rtc->mem_map + PTP_TMR_CTRL); + + return 0; +} + +static u32 rtc_set_periodic_pulse( + struct ptp_rtc *rtc, + enum e_ptp_rtc_pulse_id pulse_ID, + u32 pulse_periodic) +{ + u32 factor; + + if (rtc->start_pulse_on_alarm) { + /*from the spec:the ratio between the prescale register value + *and the fiper value should be decisable by the clock period + *FIPER_VALUE = (prescale_value * tclk_per * N) - tclk_per*/ + factor = (u32)((pulse_periodic + rtc->clock_period_nansec) / + (rtc->clock_period_nansec * rtc->output_clock_divisor)); + + if ((factor * rtc->clock_period_nansec * + rtc->output_clock_divisor) < + (pulse_periodic + rtc->clock_period_nansec)) + pulse_periodic = ((factor * rtc->clock_period_nansec * + rtc->output_clock_divisor) - + rtc->clock_period_nansec); + } + + /* Decrease it to fix PPS question (frequecy error)*/ + pulse_periodic -= rtc->clock_period_nansec; + + writel((u32)pulse_periodic, rtc->mem_map + PTP_TMR_FIPER1 + + (pulse_ID * 4)); + return 0; +} + +static u32 ptp_rtc_set_periodic_pulse( + struct ptp *p_ptp, + enum e_ptp_rtc_pulse_id pulse_ID, + struct ptp_time *ptime) +{ + u32 ret; + u64 pulse_periodic; + + if (pulse_ID >= PTP_RTC_NUM_OF_PULSES) + return -1; + if (ptime->nsec < 0) + return -1; + + pulse_periodic = convert_unsigned_time(ptime); + if (pulse_periodic > 0xFFFFFFFF) + return -1; + + ret = rtc_set_periodic_pulse(p_ptp->rtc, pulse_ID, (u32)pulse_periodic); + + return ret; +} + +static u32 rtc_set_alarm( + struct ptp_rtc *rtc, + enum e_ptp_rtc_alarm_id alarm_ID, + u64 alarm_time) +{ + u32 fiper; + int i; + + if ((alarm_ID == e_PTP_RTC_ALARM_1) && rtc->start_pulse_on_alarm) + alarm_time -= (3 * rtc->clock_period_nansec); + + /*TMR_ALARM_L must be written first*/ + writel((u32)alarm_time, rtc->mem_map + PTP_TMR_ALARM1_L + + (alarm_ID * 4)); + writel((u32)(alarm_time >> 32), + rtc->mem_map + PTP_TMR_ALARM1_H + (alarm_ID * 4)); + + if ((alarm_ID == e_PTP_RTC_ALARM_1) && rtc->start_pulse_on_alarm) { + /*we must write the TMR_FIPER register again(hardware + *constraint),From the spec:in order to keep tracking + *the prescale output clock each tiem before enabling + *the fiper,the user must reset the fiper by writing + *a new value to the reigster*/ + for (i = 0; i < PTP_RTC_NUM_OF_PULSES; i++) { + fiper = readl(rtc->mem_map + PTP_TMR_FIPER1 + + (i * 4)); + writel(fiper, rtc->mem_map + PTP_TMR_FIPER1 + + (i * 4)); + } + } + + return 0; +} + +static u32 ptp_rtc_set_alarm( + struct ptp *p_ptp, + enum e_ptp_rtc_alarm_id alarm_ID, + struct ptp_time *ptime) +{ + u32 ret; + u64 alarm_time; + + if (alarm_ID >= PTP_RTC_NUM_OF_ALARMS) + return -1; + if (ptime->nsec < 0) + return -1; + + alarm_time = convert_unsigned_time(ptime); + + ret = rtc_set_alarm(p_ptp->rtc, alarm_ID, alarm_time); + + return ret; +} + +/* rtc ioctl function */ +/*get the current time from RTC time counter register*/ +static void ptp_rtc_get_current_time(struct ptp *p_ptp, + struct ptp_time *p_time) +{ + u64 times; + struct ptp_rtc *rtc = p_ptp->rtc; + + /*TMR_CNT_L must be read first to get an accurate value*/ + times = (u64)readl(rtc->mem_map + PTP_TMR_CNT_L); + times |= ((u64)readl(rtc->mem_map + PTP_TMR_CNT_H)) << 32; + + /*convert RTC time*/ + convert_rtc_time(×, p_time); +} + +static void ptp_rtc_reset_counter(struct ptp *p_ptp, struct ptp_time *p_time) +{ + u64 times; + struct ptp_rtc *rtc = p_ptp->rtc; + + times = convert_unsigned_time(p_time); + writel((u32)times, rtc->mem_map + PTP_TMR_CNT_L); + writel((u32)(times >> 32), rtc->mem_map + PTP_TMR_CNT_H); + +} + +static void rtc_modify_frequency_compensation( + struct ptp_rtc *rtc, + u32 freq_compensation) +{ + writel(freq_compensation, rtc->mem_map + PTP_TMR_ADD); +} + +/* Set the BD to ptp */ +int fec_ptp_do_txstamp(struct sk_buff *skb) +{ + struct iphdr *iph; + struct udphdr *udph; + + if (skb->len > 44) { + /* Check if port is 319 for PTP Event, and check for UDP */ + iph = ip_hdr(skb); + if (iph == NULL || iph->protocol != FEC_PACKET_TYPE_UDP) + return 0; + + udph = udp_hdr(skb); + if (udph != NULL && ntohs(udph->dest) == 319) + return 1; + } + + return 0; +} + +static int fec_get_tx_timestamp(struct fec_ptp_private *priv, + struct ptp_ts_data *pts, + struct ptp_time *tx_time) +{ + struct fec_ptp_data_t tmp; + int flag; + +#ifdef CONFIG_IN_BAND + u8 mode; + tmp.key = pts->seq_id; + memcpy(tmp.spid, pts->spid, 10); + mode = pts->message_type; + + switch (mode) { + case PTP_MSG_SYNC: + flag = fec_ptp_find_and_remove(&(priv->tx_time_sync), &tmp, + priv, DEFAULT_PTP_TX_BUF_SZ); + break; + case PTP_MSG_DEL_REQ: + flag = fec_ptp_find_and_remove(&(priv->tx_time_del_req), &tmp, + priv, DEFAULT_PTP_TX_BUF_SZ); + break; + + case PTP_MSG_P_DEL_REQ: + flag = fec_ptp_find_and_remove(&(priv->tx_time_pdel_req), &tmp, + priv, DEFAULT_PTP_TX_BUF_SZ); + break; + case PTP_MSG_P_DEL_RESP: + flag = fec_ptp_find_and_remove(&(priv->tx_time_pdel_resp), + &tmp, priv, DEFAULT_PTP_TX_BUF_SZ); + break; + + default: + flag = 1; + printk(KERN_ERR "ERROR\n"); + break; + } + + if (!flag) { + tx_time->sec = tmp.ts_time.sec; + tx_time->nsec = tmp.ts_time.nsec; + return 0; + } else { + wait_event_interruptible_timeout(ptp_tx_ts_wait, 0, + PTP_GET_TX_TIMEOUT); + + switch (mode) { + case PTP_MSG_SYNC: + flag = fec_ptp_find_and_remove(&(priv->tx_time_sync), + &tmp, priv, DEFAULT_PTP_TX_BUF_SZ); + break; + case PTP_MSG_DEL_REQ: + flag = fec_ptp_find_and_remove( + &(priv->tx_time_del_req), &tmp, + priv, DEFAULT_PTP_TX_BUF_SZ); + break; + case PTP_MSG_P_DEL_REQ: + flag = fec_ptp_find_and_remove( + &(priv->tx_time_pdel_req), &tmp, + priv, DEFAULT_PTP_TX_BUF_SZ); + break; + case PTP_MSG_P_DEL_RESP: + flag = fec_ptp_find_and_remove( + &(priv->tx_time_pdel_resp), &tmp, + priv, DEFAULT_PTP_TX_BUF_SZ); + break; + } + + if (flag == 0) { + tx_time->sec = tmp.ts_time.sec; + tx_time->nsec = tmp.ts_time.nsec; + return 0; + } + + return -1; + } + +#else + memset(tmp.spid, 0, 10); + tmp.key = SEQ_ID_OUT_OF_BAND; + + flag = fec_ptp_find_and_remove(&(priv->txstamp), &tmp, + priv, DEFAULT_PTP_TX_BUF_SZ); + tx_time->sec = tmp.ts_time.sec; + tx_time->nsec = tmp.ts_time.nsec; + return 0; +#endif +} + +static uint8_t fec_get_rx_timestamp(struct fec_ptp_private *priv, + struct ptp_ts_data *pts, + struct ptp_time *rx_time) +{ + struct fec_ptp_data_t tmp; + int flag; + u8 mode; + +#ifdef CONFIG_IN_BAND + tmp.key = pts->seq_id; + memcpy(tmp.spid, pts->spid, 10); +#else + memset(tmp.spid, 0, 10); + tmp.key = SEQ_ID_OUT_OF_BAND; +#endif + mode = pts->message_type; + switch (mode) { + case PTP_MSG_SYNC: + flag = fec_ptp_find_and_remove(&(priv->rx_time_sync), &tmp, + priv, DEFAULT_PTP_RX_BUF_SZ); + break; + case PTP_MSG_DEL_REQ: + flag = fec_ptp_find_and_remove(&(priv->rx_time_del_req), &tmp, + priv, DEFAULT_PTP_RX_BUF_SZ); + break; + + case PTP_MSG_P_DEL_REQ: + flag = fec_ptp_find_and_remove(&(priv->rx_time_pdel_req), &tmp, + priv, DEFAULT_PTP_RX_BUF_SZ); + break; + case PTP_MSG_P_DEL_RESP: + flag = fec_ptp_find_and_remove(&(priv->rx_time_pdel_resp), + &tmp, priv, DEFAULT_PTP_RX_BUF_SZ); + break; + + default: + flag = 1; + printk(KERN_ERR "ERROR\n"); + break; + } + + if (!flag) { + rx_time->sec = tmp.ts_time.sec; + rx_time->nsec = tmp.ts_time.nsec; + return 0; + } else { + wait_event_interruptible_timeout(ptp_rx_ts_wait, 0, + PTP_GET_RX_TIMEOUT); + + switch (mode) { + case PTP_MSG_SYNC: + flag = fec_ptp_find_and_remove(&(priv->rx_time_sync), + &tmp, priv, DEFAULT_PTP_RX_BUF_SZ); + break; + case PTP_MSG_DEL_REQ: + flag = fec_ptp_find_and_remove( + &(priv->rx_time_del_req), &tmp, + priv, DEFAULT_PTP_RX_BUF_SZ); + break; + case PTP_MSG_P_DEL_REQ: + flag = fec_ptp_find_and_remove( + &(priv->rx_time_pdel_req), &tmp, + priv, DEFAULT_PTP_RX_BUF_SZ); + break; + case PTP_MSG_P_DEL_RESP: + flag = fec_ptp_find_and_remove( + &(priv->rx_time_pdel_resp), &tmp, + priv, DEFAULT_PTP_RX_BUF_SZ); + break; + } + + if (flag == 0) { + rx_time->sec = tmp.ts_time.sec; + rx_time->nsec = tmp.ts_time.nsec; + return 0; + } + + return -1; + } +} + +/* 1588 Module start */ +int fec_ptp_start(struct fec_ptp_private *priv) +{ + struct ptp *p_ptp = ptp_dev; + + /* Enable TSU clk */ + clk_enable(p_ptp->clk); + + /*initialize the TSU using the register function*/ + init_ptp_tsu(p_ptp); + + /* start counter */ + p_ptp->fpp = ptp_private; + ptp_tsu_enable(p_ptp); + + return 0; +} + +/* Cleanup routine for 1588 module. + * When PTP is disabled this routing is called */ +void fec_ptp_stop(struct fec_ptp_private *priv) +{ + struct ptp *p_ptp = ptp_dev; + + /* stop counter */ + ptp_tsu_disable(p_ptp); + clk_disable(p_ptp->clk); + + return; +} + +static int ptp_open(struct inode *inode, struct file *file) +{ + return 0; +} + +static int ptp_release(struct inode *inode, struct file *file) +{ + return 0; +} + +/* ptp device ioctl function */ +static int ptp_ioctl( + struct inode *inode, + struct file *file, + unsigned int cmd, + unsigned long arg) +{ + struct ptp_rtc_time *cnt; + struct ptp_rtc_time curr_time; + struct ptp_time rx_time, tx_time; + struct ptp_ts_data *p_ts; + struct ptp_set_comp *p_comp; + struct fec_ptp_private *priv; + int retval = 0; + + priv = (struct fec_ptp_private *) ptp_private; + switch (cmd) { + case PTP_GET_RX_TIMESTAMP: + p_ts = (struct ptp_ts_data *)arg; + retval = fec_get_rx_timestamp(priv, p_ts, &rx_time); + if (retval == 0) + retval = copy_to_user((void __user *)(&(p_ts->ts)), + &rx_time, sizeof(rx_time)); + break; + case PTP_GET_TX_TIMESTAMP: + p_ts = (struct ptp_ts_data *)arg; + fec_get_tx_timestamp(priv, p_ts, &tx_time); + retval = copy_to_user((void __user *)(&(p_ts->ts)), + &tx_time, sizeof(tx_time)); + break; + case PTP_GET_CURRENT_TIME: + ptp_rtc_get_current_time(ptp_dev, &(curr_time.rtc_time)); + retval = copy_to_user((void __user *)arg, &curr_time, + sizeof(curr_time)); + break; + case PTP_SET_RTC_TIME: + cnt = (struct ptp_rtc_time *)arg; + ptp_rtc_reset_counter(ptp_dev, &(cnt->rtc_time)); + break; + case PTP_FLUSH_TIMESTAMP: + /* reset sync buffer */ + priv->rx_time_sync.head = 0; + priv->rx_time_sync.tail = 0; + /* reset delay_req buffer */ + priv->rx_time_del_req.head = 0; + priv->rx_time_del_req.tail = 0; + /* reset pdelay_req buffer */ + priv->rx_time_pdel_req.head = 0; + priv->rx_time_pdel_req.tail = 0; + /* reset pdelay_resp buffer */ + priv->rx_time_pdel_resp.head = 0; + priv->rx_time_pdel_resp.tail = 0; + /* reset sync buffer */ + priv->tx_time_sync.head = 0; + priv->tx_time_sync.tail = 0; + /* reset delay_req buffer */ + priv->tx_time_del_req.head = 0; + priv->tx_time_del_req.tail = 0; + /* reset pdelay_req buffer */ + priv->tx_time_pdel_req.head = 0; + priv->tx_time_pdel_req.tail = 0; + /* reset pdelay_resp buffer */ + priv->tx_time_pdel_resp.head = 0; + priv->tx_time_pdel_resp.tail = 0; + priv->txstamp.head = 0; + priv->txstamp.tail = 0; + break; + case PTP_SET_COMPENSATION: + p_comp = (struct ptp_set_comp *)arg; + rtc_modify_frequency_compensation(ptp_dev->rtc, + p_comp->freq_compensation); + break; + case PTP_GET_ORIG_COMP: + ((struct ptp_get_comp *)arg)->dw_origcomp = + ptp_dev->orig_freq_comp; + break; + default: + return -EINVAL; + } + return retval; +} + +static const struct file_operations ptp_fops = { + .owner = THIS_MODULE, + .llseek = NULL, + .read = NULL, + .write = NULL, + .ioctl = ptp_ioctl, + .open = ptp_open, + .release = ptp_release, +}; + +static int init_ptp_driver(struct ptp *p_ptp) +{ + struct ptp_time ptime; + int ret; + + /* configure RTC param */ + ret = ptp_rtc_config(p_ptp->rtc); + if (ret) + return -1; + + /* initialize RTC register */ + ptp_rtc_init(p_ptp); + + /* initialize PTP TSU param */ + ptp_param_config(p_ptp); + + /* set TSU configuration parameters */ +#ifdef CONFIG_IN_BAND + p_ptp->driver_param->delivery_mode = e_PTP_TSU_DELIVERY_IN_BAND; +#else + p_ptp->driver_param->delivery_mode = e_PTP_TSU_DELIVERY_OUT_OF_BAND; +#endif + + if (ptp_tsu_config_events_mask(p_ptp, DEFAULT_events_PTP_Mask)) + goto end; + + /* initialize PTP TSU register */ + ptp_tsu_init(p_ptp); + + /* set periodic pulses */ + ptime.sec = USE_CASE_PULSE_1_PERIOD / NANOSEC_IN_SEC; + ptime.nsec = USE_CASE_PULSE_1_PERIOD % NANOSEC_IN_SEC; + ret = ptp_rtc_set_periodic_pulse(p_ptp, e_PTP_RTC_PULSE_1, &ptime); + if (ret) + goto end; + + ptime.sec = USE_CASE_PULSE_2_PERIOD / NANOSEC_IN_SEC; + ptime.nsec = USE_CASE_PULSE_2_PERIOD % NANOSEC_IN_SEC; + ret = ptp_rtc_set_periodic_pulse(p_ptp, e_PTP_RTC_PULSE_2, &ptime); + if (ret) + goto end; + + ptime.sec = USE_CASE_PULSE_3_PERIOD / NANOSEC_IN_SEC; + ptime.nsec = USE_CASE_PULSE_3_PERIOD % NANOSEC_IN_SEC; + ret = ptp_rtc_set_periodic_pulse(p_ptp, e_PTP_RTC_PULSE_3, &ptime); + if (ret) + goto end; + + /* set alarm */ + ptime.sec = (USE_CASE_ALARM_1_TIME / NANOSEC_IN_SEC); + ptime.nsec = (USE_CASE_ALARM_1_TIME % NANOSEC_IN_SEC); + ret = ptp_rtc_set_alarm(p_ptp, e_PTP_RTC_ALARM_1, &ptime); + if (ret) + goto end; + + ptime.sec = (USE_CASE_ALARM_2_TIME / NANOSEC_IN_SEC); + ptime.nsec = (USE_CASE_ALARM_2_TIME % NANOSEC_IN_SEC); + ret = ptp_rtc_set_alarm(p_ptp, e_PTP_RTC_ALARM_2, &ptime); + if (ret) + goto end; + + /* enable the RTC */ + ret = rtc_enable(p_ptp->rtc, FALSE); + if (ret) + goto end; + + udelay(10); + ptp_rtc_get_current_time(p_ptp, &ptime); + if (ptime.nsec == 0) { + printk(KERN_ERR "PTP RTC is not running\n"); + goto end; + } + + if (register_chrdev(PTP_MAJOR, "ptp", &ptp_fops)) + printk(KERN_ERR "Unable to register PTP device as char\n"); + else + printk(KERN_INFO "Register PTP as char device\n"); + +end: + return ret; +} + +static void ptp_free(void) +{ + rtc_disable(ptp_dev->rtc); + /*unregister the PTP device*/ + unregister_chrdev(PTP_MAJOR, "ptp"); +} + +/* + * Resource required for accessing 1588 Timer Registers. + */ +int fec_ptp_init(struct fec_ptp_private *priv, int id) +{ + fec_ptp_init_circ(&(priv->rx_time_sync), DEFAULT_PTP_RX_BUF_SZ); + fec_ptp_init_circ(&(priv->rx_time_del_req), DEFAULT_PTP_RX_BUF_SZ); + fec_ptp_init_circ(&(priv->rx_time_pdel_req), DEFAULT_PTP_RX_BUF_SZ); + fec_ptp_init_circ(&(priv->rx_time_pdel_resp), DEFAULT_PTP_RX_BUF_SZ); + fec_ptp_init_circ(&(priv->tx_time_sync), DEFAULT_PTP_TX_BUF_SZ); + fec_ptp_init_circ(&(priv->tx_time_del_req), DEFAULT_PTP_TX_BUF_SZ); + fec_ptp_init_circ(&(priv->tx_time_pdel_req), DEFAULT_PTP_TX_BUF_SZ); + fec_ptp_init_circ(&(priv->tx_time_pdel_resp), DEFAULT_PTP_TX_BUF_SZ); + + fec_ptp_init_circ(&(priv->txstamp), DEFAULT_PTP_TX_BUF_SZ); + + spin_lock_init(&priv->ptp_lock); + spin_lock_init(&priv->cnt_lock); + ptp_private = priv; + + return 0; +} +EXPORT_SYMBOL(fec_ptp_init); + +void fec_ptp_cleanup(struct fec_ptp_private *priv) +{ + if (priv->rx_time_sync.buf) + vfree(priv->rx_time_sync.buf); + if (priv->rx_time_del_req.buf) + vfree(priv->rx_time_del_req.buf); + if (priv->rx_time_pdel_req.buf) + vfree(priv->rx_time_pdel_req.buf); + if (priv->rx_time_pdel_resp.buf) + vfree(priv->rx_time_pdel_resp.buf); + if (priv->tx_time_sync.buf) + vfree(priv->tx_time_sync.buf); + if (priv->tx_time_del_req.buf) + vfree(priv->tx_time_del_req.buf); + if (priv->tx_time_pdel_req.buf) + vfree(priv->tx_time_pdel_req.buf); + if (priv->tx_time_pdel_resp.buf) + vfree(priv->tx_time_pdel_resp.buf); + if (priv->txstamp.buf) + vfree(priv->txstamp.buf); + + ptp_free(); +} +EXPORT_SYMBOL(fec_ptp_cleanup); + +/* probe just register memory and irq */ +static int __devinit +ptp_probe(struct platform_device *pdev) +{ + int i, irq, ret = 0; + struct resource *r; + + /* setup board info structure */ + ptp_dev = kzalloc(sizeof(struct ptp), GFP_KERNEL); + if (!ptp_dev) { + ret = -ENOMEM; + goto err1; + } + ptp_dev->rtc = kzalloc(sizeof(struct ptp_rtc), + GFP_KERNEL); + if (!ptp_dev->rtc) { + ret = -ENOMEM; + goto err2; + } + + /* PTP register memory */ + r = platform_get_resource(pdev, IORESOURCE_MEM, 0); + if (!r) { + ret = -ENXIO; + goto err3; + } + + r = request_mem_region(r->start, resource_size(r), pdev->name); + if (!r) { + ret = -EBUSY; + goto err3; + } + + ptp_dev->mem_map = ioremap(r->start, resource_size(r)); + if (!ptp_dev->mem_map) { + ret = -ENOMEM; + goto failed_ioremap; + } + + /* RTC register memory */ + r = platform_get_resource(pdev, IORESOURCE_MEM, 1); + if (!r) { + ret = -ENXIO; + goto err4; + } + + r = request_mem_region(r->start, resource_size(r), "PTP_RTC"); + if (!r) { + ret = -EBUSY; + goto err4; + } + + ptp_dev->rtc->mem_map = ioremap(r->start, resource_size(r)); + + if (!ptp_dev->rtc->mem_map) { + ret = -ENOMEM; + goto failed_ioremap1; + } + + /* This device has up to two irqs on some platforms */ + for (i = 0; i < 2; i++) { + irq = platform_get_irq(pdev, i); + if (i && irq < 0) + break; + if (i == 0) + ret = request_irq(irq, ptp_interrupt, + IRQF_DISABLED, pdev->name, ptp_dev); + else + ret = request_irq(irq, ptp_rtc_interrupt, + IRQF_DISABLED, "ptp_rtc", ptp_dev); + if (ret) { + while (i >= 0) { + irq = platform_get_irq(pdev, i); + free_irq(irq, ptp_dev); + i--; + } + goto failed_irq; + } + } + + ptp_dev->rtc->clk = clk_get(NULL, "ieee_rtc_clk"); + if (IS_ERR(ptp_dev->rtc->clk)) { + ret = PTR_ERR(ptp_dev->rtc->clk); + goto failed_clk1; + } + + ptp_dev->clk = clk_get(&pdev->dev, "ieee_1588_clk"); + if (IS_ERR(ptp_dev->clk)) { + ret = PTR_ERR(ptp_dev->clk); + goto failed_clk2; + } + + clk_enable(ptp_dev->clk); + + init_ptp_driver(ptp_dev); + clk_disable(ptp_dev->clk); + + return 0; + +failed_clk2: + clk_put(ptp_dev->rtc->clk); +failed_clk1: + for (i = 0; i < 2; i++) { + irq = platform_get_irq(pdev, i); + if (irq > 0) + free_irq(irq, ptp_dev); + } +failed_irq: + iounmap((void __iomem *)ptp_dev->rtc->mem_map); +failed_ioremap1: +err4: + iounmap((void __iomem *)ptp_dev->mem_map); +failed_ioremap: +err3: + kfree(ptp_dev->rtc); +err2: + kfree(ptp_dev); +err1: + return ret; +} + +static int __devexit +ptp_drv_remove(struct platform_device *pdev) +{ + clk_disable(ptp_dev->clk); + clk_put(ptp_dev->clk); + clk_put(ptp_dev->rtc->clk); + iounmap((void __iomem *)ptp_dev->rtc->mem_map); + iounmap((void __iomem *)ptp_dev->mem_map); + kfree(ptp_dev->rtc->driver_param); + kfree(ptp_dev->rtc); + kfree(ptp_dev->driver_param); + kfree(ptp_dev); + return 0; +} + +static struct platform_driver ptp_driver = { + .driver = { + .name = "ptp", + .owner = THIS_MODULE, + }, + .probe = ptp_probe, + .remove = __devexit_p(ptp_drv_remove), +}; + +static int __init +ptp_module_init(void) +{ + printk(KERN_INFO "iMX PTP Driver\n"); + + return platform_driver_register(&ptp_driver); +} + +static void __exit +ptp_cleanup(void) +{ + platform_driver_unregister(&ptp_driver); +} + +module_exit(ptp_cleanup); +module_init(ptp_module_init); + +MODULE_LICENSE("GPL"); -- cgit v1.2.3