From: Fugang Duan Date: Mon, 29 Oct 2012 10:00:09 +0000 (+0800) Subject: ENGR00232528 - IEEE1588:imx6:Upgrade driver to support IXXAT stack V1.05.03 X-Git-Tag: v3.0.35-fsl~270 X-Git-Url: https://git.karo-electronics.de/?a=commitdiff_plain;h=1734bc28134bc480d95d6bba35bcb094fb004ff2;p=karo-tx-linux.git ENGR00232528 - IEEE1588:imx6:Upgrade driver to support IXXAT stack V1.05.03 Upgrade drivers to support IXXAT IEEE1588 stack V1.05.03. Ptp message protocol support IPv4, IPv6, and IEEE802.3 L2. Signed-off-by: Fugang Duan --- diff --git a/drivers/net/fec.c b/drivers/net/fec.c index 86e4ea3404b3..db48dfda84e2 100755 --- a/drivers/net/fec.c +++ b/drivers/net/fec.c @@ -1197,7 +1197,9 @@ static struct ethtool_ops fec_enet_ethtool_ops = { static int fec_enet_ioctl(struct net_device *ndev, struct ifreq *rq, int cmd) { struct fec_enet_private *fep = netdev_priv(ndev); + struct fec_ptp_private *priv = fep->ptp_priv; struct phy_device *phydev = fep->phy_dev; + int retVal = 0; if (!netif_running(ndev)) return -EINVAL; @@ -1205,7 +1207,16 @@ static int fec_enet_ioctl(struct net_device *ndev, struct ifreq *rq, int cmd) if (!phydev) return -ENODEV; - return phy_mii_ioctl(phydev, rq, cmd); + if ((cmd >= PTP_ENBL_TXTS_IOCTL) && + (cmd <= PTP_FLUSH_TIMESTAMP)) { + if (fep->ptimer_present) + retVal = fec_ptp_ioctl(priv, rq, cmd); + else + retVal = -ENODEV; + } else + retVal = phy_mii_ioctl(phydev, rq, cmd); + + return retVal; } static void fec_enet_free_buffers(struct net_device *ndev) diff --git a/drivers/net/fec_1588.c b/drivers/net/fec_1588.c index 43b60941cc99..74fa168f3f7d 100644 --- a/drivers/net/fec_1588.c +++ b/drivers/net/fec_1588.c @@ -26,18 +26,11 @@ #include #include #include -#include #include #include #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) @@ -45,15 +38,16 @@ static struct fec_ptp_private *ptp_private[1]; #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; } @@ -62,15 +56,16 @@ 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) +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) @@ -83,73 +78,76 @@ static int fec_ptp_nelems(struct circ_buf *buf, int size) 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; } @@ -211,7 +209,6 @@ void fec_ptp_stop(struct fec_ptp_private *priv) 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, @@ -263,22 +260,100 @@ static void fec_set_1588cnt(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; } @@ -287,288 +362,113 @@ 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; + 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, @@ -593,13 +493,14 @@ 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++) { @@ -647,86 +548,79 @@ static void fec_set_drift(struct fec_ptp_private *priv, 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; @@ -734,76 +628,32 @@ static long ptp_ioctl( 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); diff --git a/drivers/net/fec_1588.h b/drivers/net/fec_1588.h index 8e21519ec45d..bd3565ac5194 100644 --- a/drivers/net/fec_1588.h +++ b/drivers/net/fec_1588.h @@ -41,7 +41,6 @@ #define FEC_T_INC_CORR_MASK 0x00007f00 #define FEC_T_INC_CORR_OFFSET 8 - #define FEC_T_INC_50MHZ 20 #define FEC_ATIME_50MHZ 50000000 #define FEC_T_INC_CLK FEC_T_INC_50MHZ @@ -51,41 +50,45 @@ /* IEEE 1588 definition */ #define FEC_ECNTRL_TS_EN 0x10 -#define PTP_MAJOR 232 /*the temporary major number - *used by PTP driver, the major - *number 232~239 is unassigned*/ - -#define DEFAULT_PTP_RX_BUF_SZ 2048 -#define DEFAULT_PTP_TX_BUF_SZ 16 -#define PTP_MSG_SYNC 0x0 -#define PTP_MSG_DEL_REQ 0x1 -#define PTP_MSG_P_DEL_REQ 0x2 -#define PTP_MSG_P_DEL_RESP 0x3 -#define PTP_MSG_DEL_RESP 0x4 -#define PTP_MSG_ALL_OTHER 0x5 - -#define PTP_GET_TX_TIMESTAMP 0x1 -#define PTP_GET_RX_TIMESTAMP 0x9 -#define PTP_SET_RTC_TIME 0x3 -#define PTP_SET_COMPENSATION 0x4 -#define PTP_GET_CURRENT_TIME 0x5 -#define PTP_FLUSH_TIMESTAMP 0x6 -#define PTP_ADJ_ADDEND 0x7 -#define PTP_GET_ORIG_COMP 0x8 -#define PTP_GET_ADDEND 0xB -#define PTP_GET_RX_TIMESTAMP_PDELAY_REQ 0xC -#define PTP_GET_RX_TIMESTAMP_PDELAY_RESP 0xD - -#define FEC_PTP_DOMAIN_DLFT 0xe0000181 -#define FEC_PTP_IP_OFFS 0xE -#define FEC_PTP_UDP_OFFS 0x22 -#define FEC_PTP_MSG_TYPE_OFFS 0x2A -#define FEC_PTP_SPORT_ID_OFFS 0x3E -#define FEC_PTP_SEQ_ID_OFFS 0x48 -#define FEC_PTP_CTRL_OFFS 0x4A + +#define DEFAULT_PTP_RX_BUF_SZ 64 +#define DEFAULT_PTP_TX_BUF_SZ 64 + +/* 1588stack API defines */ +#define PTP_ENBL_TXTS_IOCTL SIOCDEVPRIVATE +#define PTP_DSBL_TXTS_IOCTL (SIOCDEVPRIVATE + 1) +#define PTP_ENBL_RXTS_IOCTL (SIOCDEVPRIVATE + 2) +#define PTP_DSBL_RXTS_IOCTL (SIOCDEVPRIVATE + 3) +#define PTP_GET_TX_TIMESTAMP (SIOCDEVPRIVATE + 4) +#define PTP_GET_RX_TIMESTAMP (SIOCDEVPRIVATE + 5) +#define PTP_SET_RTC_TIME (SIOCDEVPRIVATE + 6) +#define PTP_GET_CURRENT_TIME (SIOCDEVPRIVATE + 7) +#define PTP_SET_COMPENSATION (SIOCDEVPRIVATE + 9) +#define PTP_GET_ORIG_COMP (SIOCDEVPRIVATE + 10) +#define PTP_FLUSH_TIMESTAMP (SIOCDEVPRIVATE + 11) + +/* IEEE1588 ptp head format */ +#define PTP_CTRL_OFFS 0x52 +#define PTP_SOURCE_PORT_LENGTH 10 +#define PTP_HEADER_SEQ_OFFS 30 +#define PTP_HEADER_CTL_OFFS 32 +#define PTP_SPID_OFFS 20 +#define PTP_HEADER_SZE 34 +#define PTP_EVENT_PORT 0x013F + +#define FEC_VLAN_TAG_LEN 0x04 +#define FEC_ETHTYPE_LEN 0x02 + +/* 1588-2008 network protocol enumeration values */ +#define FEC_PTP_PROT_IPV4 1 +#define FEC_PTP_PROT_IPV6 2 +#define FEC_PTP_PROT_802_3 3 +#define FEC_PTP_PROT_DONTCARE 0xFFFF #define FEC_PACKET_TYPE_UDP 0x11 #define FEC_PTP_ORIG_COMP 0x15555555 +#define FEC_PTP_SPINNER_2 2 +#define FEC_PTP_SPINNER_4 4 /* PTP standard time representation structure */ struct ptp_time{ @@ -93,27 +96,30 @@ struct ptp_time{ u32 nsec; /* nanoseconds */ }; -/* Structure for PTP Time Stamp */ -struct fec_ptp_data_t { - u8 spid[10]; - int key; - struct ptp_time ts_time; +/* struct needed to identify a timestamp */ +struct fec_ptp_ident { + u8 version; + u8 message_type; + u16 netw_prot; + u16 seq_id; + u8 spid[10]; }; /* interface for PTP driver command GET_TX_TIME */ -struct ptp_ts_data { - /* PTP version */ - u8 version; - /* PTP source port ID */ - u8 spid[10]; - /* PTP sequence ID */ - u16 seq_id; - /* PTP message type */ - u8 message_type; +struct fec_ptp_ts_data { + struct fec_ptp_ident ident; /* PTP timestamp */ struct ptp_time ts; }; +/* circular buffer for ptp timestamps over ioctl */ +struct fec_ptp_circular { + int front; + int end; + int size; + struct fec_ptp_ts_data *data_buf; +}; + /* interface for PTP driver command SET_RTC_TIME/GET_CURRENT_TIME */ struct ptp_rtc_time { struct ptp_time rtc_time; @@ -156,15 +162,8 @@ struct fec_ptp_private { void __iomem *hwp; int dev_id; - struct circ_buf rx_time_sync; - struct circ_buf rx_time_del_req; - struct circ_buf rx_time_pdel_req; - struct circ_buf rx_time_pdel_resp; - struct circ_buf tx_time_sync; - struct circ_buf tx_time_del_req; - struct circ_buf tx_time_pdel_req; - struct circ_buf tx_time_pdel_resp; - spinlock_t ptp_lock; + struct fec_ptp_circular tx_timestamps; + struct fec_ptp_circular rx_timestamps; spinlock_t cnt_lock; u64 prtc; @@ -190,6 +189,8 @@ extern void fec_ptp_store_txstamp(struct fec_ptp_private *priv, extern void fec_ptp_store_rxstamp(struct fec_ptp_private *priv, struct sk_buff *skb, struct bufdesc *bdp); +extern int fec_ptp_ioctl(struct fec_ptp_private *priv, + struct ifreq *ifr, int cmd); #else static inline int fec_ptp_malloc_priv(struct fec_ptp_private **priv) { @@ -215,6 +216,9 @@ static inline void fec_ptp_store_txstamp(struct fec_ptp_private *priv, static inline void fec_ptp_store_rxstamp(struct fec_ptp_private *priv, struct sk_buff *skb, struct bufdesc *bdp) {} +static inline int fec_ptp_ioctl(struct fec_ptp_private *priv, + struct ifreq *ifr, int cmd) {} + #endif /* 1588 */ #endif