#include <linux/device.h>
#include <linux/fs.h>
#include <linux/vmalloc.h>
-#include <linux/spinlock.h>
#include <linux/ip.h>
#include <linux/udp.h>
#include "fec.h"
#include "fec_1588.h"
-static DECLARE_WAIT_QUEUE_HEAD(ptp_rx_ts_wait);
-static DECLARE_WAIT_QUEUE_HEAD(ptp_tx_ts_wait);
-
-#define PTP_GET_RX_TIMEOUT (HZ)
-#define PTP_GET_TX_TIMEOUT (HZ)
-
#if defined(CONFIG_ARCH_MX28)
static struct fec_ptp_private *ptp_private[2];
#elif defined(CONFIG_ARCH_MX6)
#endif
/* Alloc the ring resource */
-static int fec_ptp_init_circ(struct circ_buf *ptp_buf, int size)
+static int fec_ptp_init_circ(struct fec_ptp_circular *buf, int size)
{
- ptp_buf->buf = vmalloc(size * sizeof(struct fec_ptp_data_t));
+ buf->data_buf = (struct fec_ptp_ts_data *)
+ vmalloc(size * sizeof(struct fec_ptp_ts_data));
- if (!ptp_buf->buf)
+ if (!buf->data_buf)
return 1;
- ptp_buf->head = 0;
- ptp_buf->tail = 0;
-
+ buf->front = 0;
+ buf->end = 0;
+ buf->size = size;
return 0;
}
return (curr_index + offset) % size;
}
-static int fec_ptp_is_empty(struct circ_buf *buf)
+static int fec_ptp_is_empty(struct fec_ptp_circular *buf)
{
- return (buf->head == buf->tail);
+ return (buf->front == buf->end);
}
-static int fec_ptp_nelems(struct circ_buf *buf, int size)
+static int fec_ptp_nelems(struct fec_ptp_circular *buf)
{
- const int front = buf->head;
- const int end = buf->tail;
+ const int front = buf->front;
+ const int end = buf->end;
+ const int size = buf->size;
int n_items;
if (end > front)
return n_items;
}
-static int fec_ptp_is_full(struct circ_buf *buf, int size)
+static int fec_ptp_is_full(struct fec_ptp_circular *buf)
{
- if (fec_ptp_nelems(buf, size) ==
- (size - 1))
+ if (fec_ptp_nelems(buf) == (buf->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;
+static int fec_ptp_insert(struct fec_ptp_circular *ptp_buf,
+ struct fec_ptp_ts_data *data)
- spin_lock(&priv->ptp_lock);
- tmp = (struct fec_ptp_data_t *)(ptp_buf->buf) + ptp_buf->tail;
+{
+ struct fec_ptp_ts_data *tmp;
- 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;
+ if (fec_ptp_is_full(ptp_buf))
+ ptp_buf->end = fec_ptp_calc_index(ptp_buf->size,
+ ptp_buf->end, 1);
- ptp_buf->tail = fec_ptp_calc_index(size, ptp_buf->tail, 1);
- spin_unlock(&priv->ptp_lock);
+ tmp = (ptp_buf->data_buf + ptp_buf->end);
+ memcpy(tmp, data, sizeof(struct fec_ptp_ts_data));
+ ptp_buf->end = fec_ptp_calc_index(ptp_buf->size, ptp_buf->end, 1);
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)
+static int fec_ptp_find_and_remove(struct fec_ptp_circular *ptp_buf,
+ struct fec_ptp_ident *ident, struct ptp_time *ts)
{
int i;
- int end = ptp_buf->tail;
- unsigned long flags;
- struct fec_ptp_data_t *tmp;
+ int size = ptp_buf->size, end = ptp_buf->end;
+ struct fec_ptp_ident *tmp_ident;
if (fec_ptp_is_empty(ptp_buf))
return 1;
- i = ptp_buf->head;
+ i = ptp_buf->front;
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;
+ tmp_ident = &(ptp_buf->data_buf + i)->ident;
+ if (tmp_ident->version == ident->version) {
+ if (tmp_ident->message_type == ident->message_type) {
+ if ((tmp_ident->netw_prot == ident->netw_prot)
+ || (ident->netw_prot ==
+ FEC_PTP_PROT_DONTCARE)) {
+ if (tmp_ident->seq_id ==
+ ident->seq_id) {
+ int ret =
+ memcmp(tmp_ident->spid,
+ ident->spid,
+ PTP_SOURCE_PORT_LENGTH);
+ if (0 == ret)
+ break;
+ }
+ }
+ }
+ }
+ /* get next */
i = fec_ptp_calc_index(size, i, 1);
}
- spin_lock_irqsave(&priv->ptp_lock, flags);
+ /* not found ? */
if (i == end) {
- ptp_buf->head = end;
- spin_unlock_irqrestore(&priv->ptp_lock, flags);
+ /* buffer full ? */
+ if (fec_ptp_is_full(ptp_buf))
+ /* drop one in front */
+ ptp_buf->front =
+ fec_ptp_calc_index(size, ptp_buf->front, 1);
+
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);
+ *ts = (ptp_buf->data_buf + i)->ts;
return 0;
}
writel(FEC_T_CTRL_RESTART, fpp->hwp + FEC_ATIME_CTRL);
priv->ptp_active = 0;
priv->ptp_slave = 0;
-
}
static void fec_get_curr_cnt(struct fec_ptp_private *priv,
spin_unlock_irqrestore(&priv->cnt_lock, flags);
}
+/**
+ * Parse packets if they are PTP.
+ * The PTP header can be found in an IPv4, IPv6 or in an IEEE802.3
+ * ethernet frame. The function returns the position of the PTP packet
+ * or NULL, if no PTP found
+ */
+u8 *fec_ptp_parse_packet(struct sk_buff *skb, u16 *eth_type)
+{
+ u8 *position = skb->data + ETH_ALEN + ETH_ALEN;
+ u8 *ptp_loc = NULL;
+
+ *eth_type = *((u16 *)position);
+ /* Check if outer vlan tag is here */
+ if (*eth_type == ETH_P_8021Q) {
+ position += FEC_VLAN_TAG_LEN;
+ *eth_type = *((u16 *)position);
+ }
+
+ /* set position after ethertype */
+ position += FEC_ETHTYPE_LEN;
+ if (ETH_P_1588 == *eth_type) {
+ ptp_loc = position;
+ /* IEEE1588 event message which needs timestamping */
+ if ((ptp_loc[0] & 0xF) <= 3) {
+ if (skb->len >=
+ ((ptp_loc - skb->data) + PTP_HEADER_SZE))
+ return ptp_loc;
+ }
+ } else if (ETH_P_IP == ntohs(*eth_type)) {
+ u8 *ip_header, *prot, *udp_header;
+ u8 ip_version, ip_hlen;
+ ip_header = position;
+ ip_version = ip_header[0] >> 4; /* correct IP version? */
+ if (0x04 == ip_version) { /* IPv4 */
+ prot = ip_header + 9; /* protocol */
+ if (FEC_PACKET_TYPE_UDP == *prot) {
+ u16 udp_dstPort;
+ /* retrieve the size of the ip-header
+ * with the first byte of the ip-header:
+ * version ( 4 bits) + Internet header
+ * length (4 bits)
+ */
+ ip_hlen = (*ip_header & 0xf) * 4;
+ udp_header = ip_header + ip_hlen;
+ udp_dstPort = *((u16 *)(udp_header + 2));
+ /* check the destination port address
+ * ( 319 (0x013F) = PTP event port )
+ */
+ if (ntohs(udp_dstPort) == PTP_EVENT_PORT) {
+ ptp_loc = udp_header + 8;
+ /* long enough ? */
+ if (skb->len >= ((ptp_loc - skb->data)
+ + PTP_HEADER_SZE))
+ return ptp_loc;
+ }
+ }
+ }
+ } else if (ETH_P_IPV6 == ntohs(*eth_type)) {
+ u8 *ip_header, *udp_header, *prot;
+ u8 ip_version;
+ ip_header = position;
+ ip_version = ip_header[0] >> 4;
+ if (0x06 == ip_version) {
+ prot = ip_header + 6;
+ if (FEC_PACKET_TYPE_UDP == *prot) {
+ u16 udp_dstPort;
+ udp_header = ip_header + 40;
+ udp_dstPort = *((u16 *)(udp_header + 2));
+ /* check the destination port address
+ * ( 319 (0x013F) = PTP event port )
+ */
+ if (ntohs(udp_dstPort) == PTP_EVENT_PORT) {
+ ptp_loc = udp_header + 8;
+ /* long enough ? */
+ if (skb->len >= ((ptp_loc - skb->data)
+ + PTP_HEADER_SZE))
+ return ptp_loc;
+ }
+ }
+ }
+ }
+
+ return NULL; /* no PTP frame */
+}
+
/* 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;
- }
+ u8 *ptp_loc;
+ u16 eth_type;
+
+ ptp_loc = fec_ptp_parse_packet(skb, ð_type);
+ if (ptp_loc != NULL)
+ return 1;
return 0;
}
struct sk_buff *skb,
struct bufdesc *bdp)
{
- int msg_type, seq_id, control;
- struct fec_ptp_data_t tmp_tx_time;
+ struct fec_ptp_ts_data tmp_tx_time;
struct fec_ptp_private *fpp;
- unsigned char *sp_id;
- unsigned short portnum;
+ u8 *ptp_loc;
+ u16 eth_type;
if (!priv->ptp_slave)
fpp = priv;
else
fpp = ptp_private[0];
- 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);
- tmp_tx_time.ts_time.sec = fpp->prtc;
- tmp_tx_time.ts_time.nsec = bdp->ts;
-
- 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);
+ ptp_loc = fec_ptp_parse_packet(skb, ð_type);
+ if (ptp_loc != NULL) {
+ /* store identification data */
+ switch (ntohs(eth_type)) {
+ case ETH_P_IP:
+ tmp_tx_time.ident.netw_prot = FEC_PTP_PROT_IPV4;
break;
- case PTP_MSG_P_DEL_RESP:
- fec_ptp_insert(&(priv->tx_time_pdel_resp), &tmp_tx_time,
- priv, DEFAULT_PTP_TX_BUF_SZ);
+ case ETH_P_IPV6:
+ tmp_tx_time.ident.netw_prot = FEC_PTP_PROT_IPV6;
break;
- default:
+ case ETH_P_1588:
+ tmp_tx_time.ident.netw_prot = FEC_PTP_PROT_802_3;
break;
+ default:
+ return;
}
- break;
- default:
- break;
+ tmp_tx_time.ident.version = (*(ptp_loc + 1)) & 0X0F;
+ tmp_tx_time.ident.message_type = (*(ptp_loc)) & 0x0F;
+ tmp_tx_time.ident.seq_id =
+ ntohs(*((u16 *)(ptp_loc + PTP_HEADER_SEQ_OFFS)));
+ memcpy(tmp_tx_time.ident.spid, &ptp_loc[PTP_SPID_OFFS],
+ PTP_SOURCE_PORT_LENGTH);
+ /* store tx timestamp */
+ tmp_tx_time.ts.sec = fpp->prtc;
+ tmp_tx_time.ts.nsec = bdp->ts;
+ /* insert timestamp in circular buffer */
+ fec_ptp_insert(&(priv->tx_timestamps), &tmp_tx_time);
}
-
- wake_up_interruptible(&ptp_tx_ts_wait);
}
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_ts_data tmp_rx_time;
struct fec_ptp_private *fpp;
- struct iphdr *iph;
- struct udphdr *udph;
- unsigned char *sp_id;
- unsigned short portnum;
+ u8 *ptp_loc;
+ u16 eth_type;
if (!priv->ptp_slave)
fpp = priv;
else
fpp = ptp_private[0];
- /* Check for UDP, and Check if port is 319 for PTP Event */
- iph = (struct iphdr *)(skb->data + FEC_PTP_IP_OFFS);
- if (iph->protocol != FEC_PACKET_TYPE_UDP)
- return;
-
- udph = (struct udphdr *)(skb->data + FEC_PTP_UDP_OFFS);
- if (ntohs(udph->dest) != 319)
- return;
-
- 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);
- tmp_rx_time.ts_time.sec = fpp->prtc;
- tmp_rx_time.ts_time.nsec = bdp->ts;
-
- 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);
+ ptp_loc = fec_ptp_parse_packet(skb, ð_type);
+ if (ptp_loc != NULL) {
+ /* store identification data */
+ tmp_rx_time.ident.version = (*(ptp_loc + 1)) & 0X0F;
+ tmp_rx_time.ident.message_type = (*(ptp_loc)) & 0x0F;
+ switch (ntohs(eth_type)) {
+ case ETH_P_IP:
+ tmp_rx_time.ident.netw_prot = FEC_PTP_PROT_IPV4;
break;
- case PTP_MSG_P_DEL_RESP:
- fec_ptp_insert(&(priv->rx_time_pdel_resp), &tmp_rx_time,
- priv, DEFAULT_PTP_RX_BUF_SZ);
+ case ETH_P_IPV6:
+ tmp_rx_time.ident.netw_prot = FEC_PTP_PROT_IPV6;
break;
- default:
+ case ETH_P_1588:
+ tmp_rx_time.ident.netw_prot = FEC_PTP_PROT_802_3;
break;
+ default:
+ return;
}
- break;
- default:
- break;
+ tmp_rx_time.ident.seq_id =
+ ntohs(*((u16 *)(ptp_loc + PTP_HEADER_SEQ_OFFS)));
+ memcpy(tmp_rx_time.ident.spid, &ptp_loc[PTP_SPID_OFFS],
+ PTP_SOURCE_PORT_LENGTH);
+ /* store rx timestamp */
+ tmp_rx_time.ts.sec = fpp->prtc;
+ tmp_rx_time.ts.nsec = bdp->ts;
+
+ /* insert timestamp in circular buffer */
+ fec_ptp_insert(&(fpp->rx_timestamps), &tmp_rx_time);
}
-
- wake_up_interruptible(&ptp_rx_ts_wait);
}
static uint8_t fec_get_tx_timestamp(struct fec_ptp_private *priv,
- struct ptp_ts_data *pts,
+ struct fec_ptp_ts_data *pts,
struct ptp_time *tx_time)
{
- struct fec_ptp_data_t tmp;
- int flag;
- 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;
+ int ret = 0;
- 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;
- }
+ ret = fec_ptp_find_and_remove(&(priv->tx_timestamps),
+ &pts->ident, tx_time);
- return -1;
- }
+ return ret;
}
static uint8_t fec_get_rx_timestamp(struct fec_ptp_private *priv,
- struct ptp_ts_data *pts,
+ struct fec_ptp_ts_data *pts,
struct ptp_time *rx_time)
{
- struct fec_ptp_data_t tmp;
- int flag;
- 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->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;
- }
+ int ret = 0;
- 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);
+ ret = fec_ptp_find_and_remove(&(priv->rx_timestamps),
+ &pts->ident, rx_time);
- 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;
- }
+ return ret;
}
static void fec_handle_ptpdrift(struct ptp_set_comp *comp,
adj_inc = 1;
if (ndrift > (FEC_ATIME_CLK / FEC_T_INC_CLK)) {
- adj_inc = FEC_T_INC_CLK / 2;
- } else if (ndrift > (FEC_ATIME_CLK / (FEC_T_INC_CLK * 4))) {
- adj_inc = FEC_T_INC_CLK / 4;
- adj_period = 2;
+ adj_inc = FEC_T_INC_CLK / FEC_PTP_SPINNER_2;
+ } else if (ndrift > (FEC_ATIME_CLK /
+ (FEC_T_INC_CLK * FEC_PTP_SPINNER_4))) {
+ adj_inc = FEC_T_INC_CLK / FEC_PTP_SPINNER_4;
+ adj_period = FEC_PTP_SPINNER_2;
} else {
- adj_inc = 4;
- adj_period = 4;
+ adj_inc = FEC_PTP_SPINNER_4;
+ adj_period = FEC_PTP_SPINNER_4;
}
for (i = 1; i < adj_inc; i++) {
writel(tc.corr_period, fpp->hwp + FEC_ATIME_CORR);
}
-static int ptp_open(struct inode *inode, struct file *file)
-{
- return 0;
-}
-
-static int ptp_release(struct inode *inode, struct file *file)
-{
- return 0;
-}
-
-static long ptp_ioctl(
- struct file *file,
- unsigned int cmd,
- unsigned long arg)
+int fec_ptp_ioctl(struct fec_ptp_private *priv, struct ifreq *ifr, int cmd)
{
- 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;
- unsigned int minor = MINOR(file->f_path.dentry->d_inode->i_rdev);
+ struct fec_ptp_ts_data p_ts;
+ struct fec_ptp_ts_data *p_ts_user;
+ struct ptp_set_comp p_comp;
+ u32 freq_compensation;
int retval = 0;
- priv = (struct fec_ptp_private *) ptp_private[minor];
-
switch (cmd) {
+ case PTP_ENBL_TXTS_IOCTL:
+ case PTP_DSBL_TXTS_IOCTL:
+ case PTP_ENBL_RXTS_IOCTL:
+ case PTP_DSBL_RXTS_IOCTL:
+ break;
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 &&
- copy_to_user((void __user *)(&(p_ts->ts)), &rx_time,
- sizeof(rx_time)))
- retval = -EFAULT;
+ p_ts_user = (struct fec_ptp_ts_data *)ifr->ifr_data;
+ if (0 != copy_from_user(&p_ts.ident,
+ &p_ts_user->ident, sizeof(p_ts.ident)))
+ return -EINVAL;
+ if (fec_get_rx_timestamp(priv, &p_ts, &rx_time) != 0)
+ return -EAGAIN;
+ if (copy_to_user((void __user *)(&p_ts_user->ts),
+ &rx_time, sizeof(rx_time)))
+ return -EFAULT;
break;
case PTP_GET_TX_TIMESTAMP:
- p_ts = (struct ptp_ts_data *)arg;
- fec_get_tx_timestamp(priv, p_ts, &tx_time);
- if (copy_to_user((void __user *)(&(p_ts->ts)), &tx_time,
- sizeof(tx_time)))
+ p_ts_user = (struct fec_ptp_ts_data *)ifr->ifr_data;
+ if (0 != copy_from_user(&p_ts.ident,
+ &p_ts_user->ident, sizeof(p_ts.ident)))
+ return -EINVAL;
+ retval = fec_get_tx_timestamp(priv, &p_ts, &tx_time);
+ if (retval == 0 &&
+ copy_to_user((void __user *)(&p_ts_user->ts),
+ &tx_time, sizeof(tx_time)))
retval = -EFAULT;
break;
case PTP_GET_CURRENT_TIME:
fec_get_curr_cnt(priv, &curr_time);
- if (copy_to_user((void __user *)arg, &curr_time,
- sizeof(curr_time)))
- retval = -EFAULT;;
+ if (0 != copy_to_user(ifr->ifr_data,
+ &(curr_time.rtc_time),
+ sizeof(struct ptp_time)))
+ return -EFAULT;
break;
case PTP_SET_RTC_TIME:
- cnt = (struct ptp_rtc_time *)arg;
- fec_set_1588cnt(priv, cnt);
+ if (0 != copy_from_user(&(curr_time.rtc_time),
+ ifr->ifr_data,
+ sizeof(struct ptp_time)))
+ return -EINVAL;
+ fec_set_1588cnt(priv, &curr_time);
break;
case PTP_FLUSH_TIMESTAMP:
- /* reset sync buffer */
- priv->rx_time_sync.head = 0;
- priv->rx_time_sync.tail = 0;
- priv->tx_time_sync.head = 0;
- priv->tx_time_sync.tail = 0;
- /* reset delay_req buffer */
- priv->rx_time_del_req.head = 0;
- priv->rx_time_del_req.tail = 0;
- priv->tx_time_del_req.head = 0;
- priv->tx_time_del_req.tail = 0;
- /* reset pdelay_req buffer */
- priv->rx_time_pdel_req.head = 0;
- priv->rx_time_pdel_req.tail = 0;
- priv->tx_time_pdel_req.head = 0;
- priv->tx_time_pdel_req.tail = 0;
- /* reset pdelay_resp buffer */
- priv->rx_time_pdel_resp.head = 0;
- priv->rx_time_pdel_resp.tail = 0;
- priv->tx_time_pdel_resp.head = 0;
- priv->tx_time_pdel_resp.tail = 0;
+ /* reset tx-timestamping buffer */
+ priv->tx_timestamps.front = 0;
+ priv->tx_timestamps.end = 0;
+ priv->tx_timestamps.size = (DEFAULT_PTP_TX_BUF_SZ + 1);
+ /* reset rx-timestamping buffer */
+ priv->rx_timestamps.front = 0;
+ priv->rx_timestamps.end = 0;
+ priv->rx_timestamps.size = (DEFAULT_PTP_RX_BUF_SZ + 1);
break;
case PTP_SET_COMPENSATION:
- p_comp = (struct ptp_set_comp *)arg;
- fec_set_drift(priv, p_comp);
+ if (0 != copy_from_user(&p_comp, ifr->ifr_data,
+ sizeof(struct ptp_set_comp)))
+ return -EINVAL;
+ fec_set_drift(priv, &p_comp);
break;
case PTP_GET_ORIG_COMP:
- ((struct ptp_get_comp *)arg)->dw_origcomp = FEC_PTP_ORIG_COMP;
+ freq_compensation = FEC_PTP_ORIG_COMP;
+ if (copy_to_user(ifr->ifr_data, &freq_compensation,
+ sizeof(freq_compensation)) > 0)
+ return -EFAULT;
break;
default:
return -EINVAL;
return retval;
}
-static const struct file_operations ptp_fops = {
- .owner = THIS_MODULE,
- .llseek = NULL,
- .read = NULL,
- .write = NULL,
- .unlocked_ioctl = ptp_ioctl,
- .open = ptp_open,
- .release = ptp_release,
-};
-
-static int init_ptp(void)
-{
- 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 device as char /dev/ptp\n");
-
- return 0;
-}
-
-static void ptp_free(void)
-{
- /*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);
-
- spin_lock_init(&priv->ptp_lock);
+ /* initialize circular buffer for tx timestamps */
+ if (fec_ptp_init_circ(&(priv->tx_timestamps),
+ (DEFAULT_PTP_TX_BUF_SZ+1)))
+ return 1;
+ /* initialize circular buffer for rx timestamps */
+ if (fec_ptp_init_circ(&(priv->rx_timestamps),
+ (DEFAULT_PTP_RX_BUF_SZ+1)))
+ return 1;
+
spin_lock_init(&priv->cnt_lock);
ptp_private[id] = priv;
priv->dev_id = id;
- if (id == 0)
- init_ptp();
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);
-
- ptp_free();
+ if (priv->tx_timestamps.data_buf)
+ vfree(priv->tx_timestamps.data_buf);
+ if (priv->rx_timestamps.data_buf)
+ vfree(priv->rx_timestamps.data_buf);
}
EXPORT_SYMBOL(fec_ptp_cleanup);
projects / karo-tx-linux.git / commitdiff