summaryrefslogtreecommitdiff
path: root/cleopatre/devkit/plcdrv
diff options
context:
space:
mode:
Diffstat (limited to 'cleopatre/devkit/plcdrv')
-rw-r--r--cleopatre/devkit/plcdrv/inc/plcdrv.h3
-rw-r--r--cleopatre/devkit/plcdrv/inc/qos.h102
-rw-r--r--cleopatre/devkit/plcdrv/src/frame.c35
-rw-r--r--cleopatre/devkit/plcdrv/src/plcdrv.c33
4 files changed, 140 insertions, 33 deletions
diff --git a/cleopatre/devkit/plcdrv/inc/plcdrv.h b/cleopatre/devkit/plcdrv/inc/plcdrv.h
index a72fe455d2..255081faf3 100644
--- a/cleopatre/devkit/plcdrv/inc/plcdrv.h
+++ b/cleopatre/devkit/plcdrv/inc/plcdrv.h
@@ -28,6 +28,7 @@
#include "debug_dump.h"
#include "ipmbox.h"
+#include "qos.h"
#include "common/ipmbox/protocol.h"
#include "common/ipmbox/msg.h"
@@ -117,6 +118,8 @@ struct plcdrv_t
debug_dump_t debug_dump;
/** Version of PLC firmware. */
uint8_t version[ROM_VERSION_SIZE];
+ /** QOS. */
+ qos_t qos;
#ifdef CONFIG_SEQ_CHECK
/** Sequence check context. */
struct seq_check_ctx seq_check_ctx;
diff --git a/cleopatre/devkit/plcdrv/inc/qos.h b/cleopatre/devkit/plcdrv/inc/qos.h
new file mode 100644
index 0000000000..025a3e959d
--- /dev/null
+++ b/cleopatre/devkit/plcdrv/inc/qos.h
@@ -0,0 +1,102 @@
+#ifndef inc_qos_h
+#define inc_qos_h
+/* Cleopatre project {{{
+ *
+ * Copyright (C) 2012 Spidcom
+ *
+ * <<<Licence>>>
+ *
+ * }}} */
+/**
+ * \file inc/qos.h
+ * \brief EoC QOS rules.
+ * \ingroup plcdrv
+ */
+#include <linux/types.h>
+#include <linux/if_vlan.h>
+#include <asm/arch/ioctl.h>
+
+/** Default VLAN prio for unmatched frames. */
+#define QOS_FRAME_PRIO_DEFAULT 0
+
+/** Default VLAN prio for MME. */
+#define QOS_FRAME_PRIO_MME 6
+
+/** Define VLAN TCI format. */
+#define VLAN_PRIO_MASK 0xe000
+#define VLAN_PRIO_SHIFT 13
+#define VLAN_PRIO(tci) (((tci) & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT)
+
+/** EoC service type. */
+typedef enum
+{
+ QOS_SERVICE_TYPE_NONE = 0,
+ QOS_SERVICE_TYPE_VID = 1,
+ QOS_SERVICE_TYPE_PRIO = 2,
+ QOS_SERVICE_TYPE_TOS = 3,
+ QOS_SERVICE_TYPE_NB
+} qos_service_type_t;
+
+/** QOS context. */
+typedef struct
+{
+ /** Current service type for TX frames. */
+ qos_service_type_t service_type;
+ /** Matching rules used depending of service type. */
+ plcdrv_qos_rules_t rules;
+} qos_t;
+
+/**
+ * Return VLAN prio to send to firmware according to current service type
+ * policy.
+ * \param qos QOS private context
+ * \param skb frame to test
+ * \return VLAN prio
+ */
+static inline unsigned int
+qos_frame_prio_get (qos_t *qos, struct sk_buff *skb)
+{
+ switch (qos->service_type)
+ {
+ case QOS_SERVICE_TYPE_NONE:
+ {
+ /* Default service type, use frame VLAN prio and raise MME
+ * priority. */
+ const struct ethhdr *eth = (const struct ethhdr *) skb->data;
+ if (eth->h_proto == htons (ETH_P_8021Q))
+ {
+ const struct vlan_ethhdr *veth
+ = (const struct vlan_ethhdr *) eth;
+ return VLAN_PRIO (ntohs (veth->h_vlan_TCI));
+ }
+ else if (eth->h_proto == htons (ETH_P_HPAV))
+ return QOS_FRAME_PRIO_MME;
+ return QOS_FRAME_PRIO_DEFAULT;
+ }
+ case QOS_SERVICE_TYPE_VID:
+ case QOS_SERVICE_TYPE_PRIO:
+ {
+ /* Rules based service types. */
+ plcdrv_qos_rules_t *rules = &qos->rules;
+ unsigned short vlan_tci, matching_value;
+ int i;
+ if (__vlan_get_tag (skb, &vlan_tci) == 0)
+ {
+ if (qos->service_type == QOS_SERVICE_TYPE_VID)
+ matching_value = vlan_tci & VLAN_VID_MASK;
+ else
+ matching_value = VLAN_PRIO (vlan_tci);
+ for (i = 0; i < rules->rules_nb; i++)
+ {
+ if (rules->rules[i].matching_value == matching_value)
+ return rules->rules[i].packet_prio;
+ }
+ }
+ return QOS_FRAME_PRIO_DEFAULT;
+ }
+ default:
+ return QOS_FRAME_PRIO_DEFAULT;
+ }
+}
+
+#endif /* inc_qos_h */
diff --git a/cleopatre/devkit/plcdrv/src/frame.c b/cleopatre/devkit/plcdrv/src/frame.c
index 5f19eaaaa9..3673f1c9c2 100644
--- a/cleopatre/devkit/plcdrv/src/frame.c
+++ b/cleopatre/devkit/plcdrv/src/frame.c
@@ -16,7 +16,6 @@
#include <linux/netdevice.h>
#include <linux/etherdevice.h>
#include <linux/wait.h>
-#include <linux/if_vlan.h>
#include <linux/if_ether.h>
#include <linux/dma-mapping.h>
#include <linux/netlink.h>
@@ -39,17 +38,6 @@
#define HPAV_MME_P_DRV_BASE 0xB000
-/** Define VLAN format */
-#define VLAN_CFI_MASK 0x1000
-#define VLAN_PRIO_MASK 0xe000
-#define VLAN_VID_SHIFT 0
-#define VLAN_CFI_SHIFT 12
-#define VLAN_PRIO_SHIFT 13
-
-#define VLAN_PRIO(tci) ((tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT)
-#define VLAN_MAX_PRIO 7
-#define VLAN_MME_PRIO 6 /** VLAN priorities 6 and 7 correspond to CAP 3. */
-
/** Frame info magic word. */
#define FRAME_INFO_MAGIC 0xc742a74e
@@ -84,18 +72,6 @@ frame_get_eth_mme_type (uint8_t *eth_frame)
}
/**
- * Find the Ethernet type.
- * \param eth_frame Ethernet frame pointer.
- * \return Ethernet type.
- */
-static inline uint16_t
-frame_get_eth_type (uint8_t *eth_frame)
-{
- struct ethhdr *eth = (struct ethhdr *) eth_frame;
- return ntohs (eth->h_proto);
-}
-
-/**
* Prepare a sk buff for sending to firmware.
* \param priv PLC device private context
* \param skb sk buff to send to firmware
@@ -340,8 +316,7 @@ frame_rx_debug_dump (plcdrv_t *priv, uint32_t data_addr,
int
frame_tx_data (struct sk_buff *skb, struct net_device *dev)
{
- uint16_t eth_type;
- unsigned int vlan_prio = 0;
+ unsigned short vlan_prio;
uint32_t phy_addr;
plcdrv_t *priv = netdev_priv (dev);
@@ -360,13 +335,7 @@ frame_tx_data (struct sk_buff *skb, struct net_device *dev)
seq_check_tx (&priv->seq_check_ctx, skb);
/* Get VLAN priority. */
- eth_type = frame_get_eth_type ((uint8_t *) skb->data);
- if (eth_type == ETH_P_8021Q)
- vlan_prio
- = (unsigned int) VLAN_PRIO (ntohs (((struct vlan_ethhdr *)
- skb->data)->h_vlan_TCI));
- else if (eth_type == ETH_P_HPAV)
- vlan_prio = VLAN_MME_PRIO;
+ vlan_prio = qos_frame_prio_get (&priv->qos, skb);
/* TX pool full? This can not happen because netif queue would have been
* stopped before data queue is full. */
diff --git a/cleopatre/devkit/plcdrv/src/plcdrv.c b/cleopatre/devkit/plcdrv/src/plcdrv.c
index a78386ea46..375ba770a5 100644
--- a/cleopatre/devkit/plcdrv/src/plcdrv.c
+++ b/cleopatre/devkit/plcdrv/src/plcdrv.c
@@ -539,12 +539,45 @@ plcdrv_set_mac_address (struct net_device *dev, void *p)
static int
plcdrv_ioctl (struct net_device *dev, struct ifreq *ifr, int cmd)
{
+ int i;
plcdrv_t *priv = netdev_priv (dev);
struct plcdrv_setpid user_data;
+ plcdrv_qos_rules_t qos_rules;
+ qos_service_type_t qos_service_type;
/* Find the command. */
switch (cmd)
{
+ case PLCDRV_IOCTL_SETQOS_RULES:
+ /* Get user data */
+ if (copy_from_user (&qos_rules, ifr->ifr_data,
+ sizeof (plcdrv_qos_rules_t)))
+ return -EFAULT;
+ /* Check user provided rules. */
+ if (qos_rules.rules_nb > PLCDRV_SERVICES_MAX_NB)
+ return -EINVAL;
+ for (i = 0; i < qos_rules.rules_nb; i++)
+ {
+ if (qos_rules.rules[i].packet_prio
+ > (VLAN_PRIO_MASK >> VLAN_PRIO_SHIFT))
+ return -EINVAL;
+ }
+ /* Copy to context. */
+ priv->qos.rules = qos_rules;
+ break;
+
+ case PLCDRV_IOCTL_SETQOS_TYPE:
+ /* Get user data */
+ if (copy_from_user (&qos_service_type, ifr->ifr_data,
+ sizeof (qos_service_type_t)))
+ return -EFAULT;
+ /* Check user provided data. */
+ if (qos_service_type >= QOS_SERVICE_TYPE_NB)
+ return -EINVAL;
+ /* Copy to context. */
+ priv->qos.service_type = qos_service_type;
+ break;
+
case PLCDRV_IOCTL_SETPID:
/* Get user data */
if (copy_from_user (&user_data, ifr->ifr_data, sizeof (user_data)))