|
@@ -230,6 +230,27 @@ int hsr_prp_lredev_get_node_table(struct hsr_prp_priv *priv,
|
|
|
return ret;
|
|
return ret;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
+static int hsr_prp_set_sv_frame_vid(struct hsr_prp_priv *priv,
|
|
|
|
|
+ u16 vid)
|
|
|
|
|
+{
|
|
|
|
|
+ struct hsr_prp_port *port_a =
|
|
|
|
|
+ hsr_prp_get_port(priv, HSR_PRP_PT_SLAVE_A);
|
|
|
|
|
+ struct net_device *slave_a_dev;
|
|
|
|
|
+ int ret = -EINVAL;
|
|
|
|
|
+
|
|
|
|
|
+ if (!port_a)
|
|
|
|
|
+ return ret;
|
|
|
|
|
+
|
|
|
|
|
+ slave_a_dev = port_a->dev;
|
|
|
|
|
+
|
|
|
|
|
+ /* TODO can we use vlan_vid_add() here?? */
|
|
|
|
|
+ if (slave_a_dev && slave_a_dev->lredev_ops &&
|
|
|
|
|
+ slave_a_dev->lredev_ops->lredev_set_sv_vlan_id)
|
|
|
|
|
+ slave_a_dev->lredev_ops->lredev_set_sv_vlan_id(slave_a_dev,
|
|
|
|
|
+ vid);
|
|
|
|
|
+ return 0;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
int hsr_prp_lredev_get_lre_stats(struct hsr_prp_priv *priv,
|
|
int hsr_prp_lredev_get_lre_stats(struct hsr_prp_priv *priv,
|
|
|
struct lre_stats *stats)
|
|
struct lre_stats *stats)
|
|
|
{
|
|
{
|
|
@@ -384,42 +405,73 @@ static void send_supervision_frame(struct hsr_prp_port *master,
|
|
|
struct sk_buff *skb;
|
|
struct sk_buff *skb;
|
|
|
int hlen, tlen;
|
|
int hlen, tlen;
|
|
|
struct hsr_tag *hsr_tag = NULL;
|
|
struct hsr_tag *hsr_tag = NULL;
|
|
|
|
|
+ struct vlan_hdr *vhdr;
|
|
|
struct prp_rct *rct;
|
|
struct prp_rct *rct;
|
|
|
struct hsr_prp_sup_tag *hsr_stag;
|
|
struct hsr_prp_sup_tag *hsr_stag;
|
|
|
struct hsr_prp_sup_payload *hsr_sp;
|
|
struct hsr_prp_sup_payload *hsr_sp;
|
|
|
|
|
+ struct hsr_prp_priv *priv;
|
|
|
unsigned long irqflags;
|
|
unsigned long irqflags;
|
|
|
- u16 proto;
|
|
|
|
|
|
|
+ u16 proto, vlan_tci = 0;
|
|
|
u8 *tail;
|
|
u8 *tail;
|
|
|
|
|
+ int len;
|
|
|
|
|
+
|
|
|
|
|
+ priv = master->priv;
|
|
|
|
|
+
|
|
|
|
|
+ if (priv->disable_sv_frame)
|
|
|
|
|
+ return;
|
|
|
|
|
|
|
|
hlen = LL_RESERVED_SPACE(master->dev);
|
|
hlen = LL_RESERVED_SPACE(master->dev);
|
|
|
tlen = master->dev->needed_tailroom;
|
|
tlen = master->dev->needed_tailroom;
|
|
|
|
|
+ len = sizeof(struct hsr_tag) +
|
|
|
|
|
+ sizeof(struct hsr_prp_sup_tag) +
|
|
|
|
|
+ sizeof(struct hsr_prp_sup_payload) + hlen + tlen;
|
|
|
|
|
+
|
|
|
|
|
+ if (priv->use_vlan_for_sv)
|
|
|
|
|
+ len += VLAN_HLEN;
|
|
|
|
|
+
|
|
|
/* skb size is same for PRP/HSR frames, only difference
|
|
/* skb size is same for PRP/HSR frames, only difference
|
|
|
* being for PRP, it is a trailor and for HSR it is a
|
|
* being for PRP, it is a trailor and for HSR it is a
|
|
|
* header
|
|
* header
|
|
|
*/
|
|
*/
|
|
|
- skb = dev_alloc_skb(sizeof(struct hsr_tag) +
|
|
|
|
|
- sizeof(struct hsr_prp_sup_tag) +
|
|
|
|
|
- sizeof(struct hsr_prp_sup_payload) + hlen + tlen);
|
|
|
|
|
-
|
|
|
|
|
|
|
+ skb = dev_alloc_skb(len);
|
|
|
if (!skb)
|
|
if (!skb)
|
|
|
return;
|
|
return;
|
|
|
|
|
|
|
|
skb_reserve(skb, hlen);
|
|
skb_reserve(skb, hlen);
|
|
|
- if (!proto_ver)
|
|
|
|
|
- proto = ETH_P_PRP;
|
|
|
|
|
- else
|
|
|
|
|
- proto = (proto_ver == HSR_V1) ? ETH_P_HSR : ETH_P_PRP;
|
|
|
|
|
skb->dev = master->dev;
|
|
skb->dev = master->dev;
|
|
|
|
|
+ if (priv->use_vlan_for_sv) {
|
|
|
|
|
+ proto = ETH_P_8021Q;
|
|
|
|
|
+ skb->priority = priv->sv_frame_pcp;
|
|
|
|
|
+ } else {
|
|
|
|
|
+ if (!proto_ver)
|
|
|
|
|
+ proto = ETH_P_PRP;
|
|
|
|
|
+ else
|
|
|
|
|
+ proto = (proto_ver == HSR_V1) ? ETH_P_HSR : ETH_P_PRP;
|
|
|
|
|
+ skb->priority = TC_PRIO_CONTROL;
|
|
|
|
|
+ }
|
|
|
skb->protocol = htons(proto);
|
|
skb->protocol = htons(proto);
|
|
|
- skb->priority = TC_PRIO_CONTROL;
|
|
|
|
|
|
|
|
|
|
if (dev_hard_header(skb, skb->dev, proto,
|
|
if (dev_hard_header(skb, skb->dev, proto,
|
|
|
- master->priv->sup_multicast_addr,
|
|
|
|
|
|
|
+ priv->sup_multicast_addr,
|
|
|
skb->dev->dev_addr, skb->len) <= 0)
|
|
skb->dev->dev_addr, skb->len) <= 0)
|
|
|
goto out;
|
|
goto out;
|
|
|
skb_reset_mac_header(skb);
|
|
skb_reset_mac_header(skb);
|
|
|
skb_reset_network_header(skb);
|
|
skb_reset_network_header(skb);
|
|
|
|
|
|
|
|
|
|
+ if (priv->use_vlan_for_sv) {
|
|
|
|
|
+ vhdr = skb_put(skb, VLAN_HLEN);
|
|
|
|
|
+ vlan_tci = priv->sv_frame_vid;
|
|
|
|
|
+ vlan_tci |= (priv->sv_frame_pcp << VLAN_PRIO_SHIFT);
|
|
|
|
|
+ if (priv->sv_frame_dei)
|
|
|
|
|
+ vlan_tci |= VLAN_CFI_MASK;
|
|
|
|
|
+ if (!proto_ver)
|
|
|
|
|
+ proto = ETH_P_PRP;
|
|
|
|
|
+ else
|
|
|
|
|
+ proto = (proto_ver == HSR_V1) ? ETH_P_HSR : ETH_P_PRP;
|
|
|
|
|
+ vhdr->h_vlan_TCI = htons(vlan_tci);
|
|
|
|
|
+ vhdr->h_vlan_encapsulated_proto = htons(proto);
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
if (proto_ver == HSR_V1) {
|
|
if (proto_ver == HSR_V1) {
|
|
|
hsr_tag = skb_put(skb, sizeof(struct hsr_tag));
|
|
hsr_tag = skb_put(skb, sizeof(struct hsr_tag));
|
|
|
hsr_tag->encap_proto = htons(ETH_P_PRP);
|
|
hsr_tag->encap_proto = htons(ETH_P_PRP);
|
|
@@ -431,21 +483,21 @@ static void send_supervision_frame(struct hsr_prp_port *master,
|
|
|
set_hsr_stag_HSR_ver(hsr_stag, proto_ver ? 0x1 : 0x0);
|
|
set_hsr_stag_HSR_ver(hsr_stag, proto_ver ? 0x1 : 0x0);
|
|
|
|
|
|
|
|
/* From HSRv1 on we have separate supervision sequence numbers. */
|
|
/* From HSRv1 on we have separate supervision sequence numbers. */
|
|
|
- spin_lock_irqsave(&master->priv->seqnr_lock, irqflags);
|
|
|
|
|
|
|
+ spin_lock_irqsave(&priv->seqnr_lock, irqflags);
|
|
|
if (proto_ver > 0) {
|
|
if (proto_ver > 0) {
|
|
|
- hsr_stag->sequence_nr = htons(master->priv->sup_sequence_nr);
|
|
|
|
|
|
|
+ hsr_stag->sequence_nr = htons(priv->sup_sequence_nr);
|
|
|
if (hsr_tag)
|
|
if (hsr_tag)
|
|
|
- hsr_tag->sequence_nr = htons(master->priv->sequence_nr);
|
|
|
|
|
- master->priv->sup_sequence_nr++;
|
|
|
|
|
|
|
+ hsr_tag->sequence_nr = htons(priv->sequence_nr);
|
|
|
|
|
+ priv->sup_sequence_nr++;
|
|
|
if (proto_ver == HSR_V1) {
|
|
if (proto_ver == HSR_V1) {
|
|
|
- hsr_tag->sequence_nr = htons(master->priv->sequence_nr);
|
|
|
|
|
- master->priv->sequence_nr++;
|
|
|
|
|
|
|
+ hsr_tag->sequence_nr = htons(priv->sequence_nr);
|
|
|
|
|
+ priv->sequence_nr++;
|
|
|
}
|
|
}
|
|
|
} else {
|
|
} else {
|
|
|
- hsr_stag->sequence_nr = htons(master->priv->sequence_nr);
|
|
|
|
|
- master->priv->sequence_nr++;
|
|
|
|
|
|
|
+ hsr_stag->sequence_nr = htons(priv->sequence_nr);
|
|
|
|
|
+ priv->sequence_nr++;
|
|
|
}
|
|
}
|
|
|
- spin_unlock_irqrestore(&master->priv->seqnr_lock, irqflags);
|
|
|
|
|
|
|
+ spin_unlock_irqrestore(&priv->seqnr_lock, irqflags);
|
|
|
|
|
|
|
|
hsr_stag->HSR_TLV_type = type;
|
|
hsr_stag->HSR_TLV_type = type;
|
|
|
/* TODO: Why 12 in HSRv0? */
|
|
/* TODO: Why 12 in HSRv0? */
|
|
@@ -456,21 +508,26 @@ static void send_supervision_frame(struct hsr_prp_port *master,
|
|
|
hsr_sp = skb_put(skb, sizeof(struct hsr_prp_sup_payload));
|
|
hsr_sp = skb_put(skb, sizeof(struct hsr_prp_sup_payload));
|
|
|
ether_addr_copy(hsr_sp->macaddress_A, master->dev->dev_addr);
|
|
ether_addr_copy(hsr_sp->macaddress_A, master->dev->dev_addr);
|
|
|
|
|
|
|
|
- if (skb_put_padto(skb, ETH_ZLEN + HSR_PRP_HLEN))
|
|
|
|
|
- return;
|
|
|
|
|
|
|
+ if (!priv->use_vlan_for_sv) {
|
|
|
|
|
+ if (skb_put_padto(skb, ETH_ZLEN + HSR_PRP_HLEN))
|
|
|
|
|
+ return;
|
|
|
|
|
+ } else {
|
|
|
|
|
+ if (skb_put_padto(skb, ETH_ZLEN + HSR_PRP_HLEN + VLAN_HLEN))
|
|
|
|
|
+ return;
|
|
|
|
|
+ }
|
|
|
|
|
|
|
|
- spin_lock_irqsave(&master->priv->seqnr_lock, irqflags);
|
|
|
|
|
|
|
+ spin_lock_irqsave(&priv->seqnr_lock, irqflags);
|
|
|
if (proto_ver == PRP_V1) {
|
|
if (proto_ver == PRP_V1) {
|
|
|
tail = skb_tail_pointer(skb) - HSR_PRP_HLEN;
|
|
tail = skb_tail_pointer(skb) - HSR_PRP_HLEN;
|
|
|
rct = (struct prp_rct *)tail;
|
|
rct = (struct prp_rct *)tail;
|
|
|
rct->PRP_suffix = htons(ETH_P_PRP);
|
|
rct->PRP_suffix = htons(ETH_P_PRP);
|
|
|
set_prp_LSDU_size(rct, HSR_PRP_V1_SUP_LSDUSIZE);
|
|
set_prp_LSDU_size(rct, HSR_PRP_V1_SUP_LSDUSIZE);
|
|
|
- rct->sequence_nr = htons(master->priv->sequence_nr);
|
|
|
|
|
- master->priv->sequence_nr++;
|
|
|
|
|
|
|
+ rct->sequence_nr = htons(priv->sequence_nr);
|
|
|
|
|
+ priv->sequence_nr++;
|
|
|
}
|
|
}
|
|
|
- spin_unlock_irqrestore(&master->priv->seqnr_lock, irqflags);
|
|
|
|
|
|
|
+ spin_unlock_irqrestore(&priv->seqnr_lock, irqflags);
|
|
|
hsr_prp_forward_skb(skb, master);
|
|
hsr_prp_forward_skb(skb, master);
|
|
|
- INC_CNT_TX_SUP(master->priv);
|
|
|
|
|
|
|
+ INC_CNT_TX_SUP(priv);
|
|
|
return;
|
|
return;
|
|
|
|
|
|
|
|
out:
|
|
out:
|
|
@@ -730,7 +787,9 @@ static const unsigned char def_multicast_addr[ETH_ALEN] __aligned(2) = {
|
|
|
|
|
|
|
|
int hsr_prp_dev_finalize(struct net_device *hsr_prp_dev,
|
|
int hsr_prp_dev_finalize(struct net_device *hsr_prp_dev,
|
|
|
struct net_device *slave[2],
|
|
struct net_device *slave[2],
|
|
|
- unsigned char multicast_spec, u8 protocol_version)
|
|
|
|
|
|
|
+ unsigned char multicast_spec, u8 protocol_version,
|
|
|
|
|
+ bool sv_vlan_tag_needed, unsigned short vid,
|
|
|
|
|
+ unsigned char pcp, unsigned char dei)
|
|
|
{
|
|
{
|
|
|
netdev_features_t mask =
|
|
netdev_features_t mask =
|
|
|
NETIF_F_HW_PRP_RX_OFFLOAD | NETIF_F_HW_HSR_RX_OFFLOAD;
|
|
NETIF_F_HW_PRP_RX_OFFLOAD | NETIF_F_HW_HSR_RX_OFFLOAD;
|
|
@@ -775,6 +834,11 @@ int hsr_prp_dev_finalize(struct net_device *hsr_prp_dev,
|
|
|
|
|
|
|
|
ether_addr_copy(priv->sup_multicast_addr, def_multicast_addr);
|
|
ether_addr_copy(priv->sup_multicast_addr, def_multicast_addr);
|
|
|
priv->sup_multicast_addr[ETH_ALEN - 1] = multicast_spec;
|
|
priv->sup_multicast_addr[ETH_ALEN - 1] = multicast_spec;
|
|
|
|
|
+ /* update vlan tag infor for SV frames */
|
|
|
|
|
+ priv->use_vlan_for_sv = sv_vlan_tag_needed;
|
|
|
|
|
+ priv->sv_frame_vid = vid;
|
|
|
|
|
+ priv->sv_frame_dei = dei;
|
|
|
|
|
+ priv->sv_frame_pcp = pcp;
|
|
|
|
|
|
|
|
/* FIXME: should I modify the value of these?
|
|
/* FIXME: should I modify the value of these?
|
|
|
*
|
|
*
|
|
@@ -860,6 +924,12 @@ int hsr_prp_dev_finalize(struct net_device *hsr_prp_dev,
|
|
|
if (res)
|
|
if (res)
|
|
|
goto fail_procfs;
|
|
goto fail_procfs;
|
|
|
|
|
|
|
|
|
|
+ if (priv->use_vlan_for_sv)
|
|
|
|
|
+ res = hsr_prp_set_sv_frame_vid(priv, priv->sv_frame_vid);
|
|
|
|
|
+
|
|
|
|
|
+ if (res)
|
|
|
|
|
+ goto fail_procfs;
|
|
|
|
|
+
|
|
|
return 0;
|
|
return 0;
|
|
|
|
|
|
|
|
fail_procfs:
|
|
fail_procfs:
|