summaryrefslogtreecommitdiff
path: root/cleopatre/devkit/plcdrv/src/frame.c
blob: a22b90ba6b244be21b9ca01dd88c7b553260484b (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
/* Cleopatre project {{{
 *
 * Copyright (C) 2012 Spidcom
 *
 * <<<Licence>>>
 *
 * }}} */
/**
 * \file    src/frame.c
 * \brief   Interfaces to send/receive/alloc/free frames for firmware.
 * \ingroup plcdrv
 */
#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/errno.h>
#include <linux/netdevice.h>
#include <linux/etherdevice.h>
#include <linux/wait.h>
#include <linux/if_ether.h>
#include <linux/dma-mapping.h>
#include <linux/netlink.h>
#include <linux/jiffies.h>
#include <linux/bug.h>
#include <net/seq_check.h>

#include "plcdrv.h"
#include "debug_dump.h"

#include "frame.h"

#include "common/ipmbox/msg.h"
#include "common/ipmbox/protocol.h"

/** Define HPAV MME format */
#define ETH_MME_VERSION_OFFSET (sizeof (struct ethhdr))
#define ETH_MME_VERSION_SIZE (1)
#define ETH_MME_TYPE_OFFSET (ETH_MME_VERSION_OFFSET + ETH_MME_VERSION_SIZE)

#define HPAV_MME_P_DRV_BASE 0xB000

/** Frame info magic word. */
#define FRAME_INFO_MAGIC 0xc742a74e

/**
 * Frame information stored before data (in headroom of the skb).
 * This way, we are able to get the corresponding skb from the data buffer.
 */
typedef struct frame_info_t
{
    /** Magic word. */
    uint32_t magic;
    /** Corresponding skb. */
    struct sk_buff *skb;
    /** \warning must be smaller than NET_SKB_PAD. */
} frame_info_t;

/** Compute an aligned pointer address before data in skb headroom. */
#define FRAME_INFO_PTR_ALIGN(data) \
    ((frame_info_t *) \
    (((uint32_t) (data) - sizeof (frame_info_t)) & ~(sizeof (uint32_t) - 1)))

/**
 * Find the Ethernet MME type.
 * \param  eth_frame  Ethernet frame pointer
 * \return  HPAV MME type
 */
static inline uint16_t
frame_get_eth_mme_type (uint8_t *eth_frame)
{
    /* Warning: mme type is stored in little endian. */
    return *(uint16_t *) (eth_frame + ETH_MME_TYPE_OFFSET);
}

/**
 * Prepare a sk buff for sending to firmware.
 * \param  priv  PLC device private context
 * \param  skb  sk buff to send to firmware
 * \param  data_dir  DMA_TO_DEVICE if skb is filled with data to send,
 * DMA_FROM_DEVICE if skb is empty and to be filled by firmware
 * \return  physical address to send to firmware
 */
static uint32_t
frame_skb_to_fw (plcdrv_t *priv, struct sk_buff *skb,
                 enum dma_data_direction data_dir)
{
    frame_info_t *frame_info;
    size_t len;
    int delta = 0;

    /* Reserve some space to store the skb pointer. */
    if (skb_headroom (skb) < NET_SKB_PAD)
        delta = NET_SKB_PAD;
    if (delta || skb_header_cloned (skb))
    {
        atomic_inc (&priv->plcdrv_stats.skb_to_fw_no_headroom);
        BUG_ON (pskb_expand_head (skb, delta, 0, GFP_ATOMIC));
    }
    /* Store frame information. */
    frame_info = FRAME_INFO_PTR_ALIGN (skb->data);
    frame_info->magic = FRAME_INFO_MAGIC;
    frame_info->skb = skb;

    /* Get length. */
    if (data_dir == DMA_TO_DEVICE)
        len = skb->len;
    else
        len = PKT_BUF_SZ;

    /* Map in DMA zone, dma_map_single is always happy on our architecture. */
    return dma_map_single (NULL, skb->data, len, data_dir);
}

/**
 * Retrieve sk buff from a buffer received from firmware.
 * \param  buffer  physical buffer address received from firmware
 * \param  data_dir  DMA_TO_DEVICE if buffer is empty, DMA_FROM_DEVICE if a
 * filled buffer is received with data
 * \return  associated sk buff or NULL on invalid magic word
 */
static struct sk_buff *
frame_skb_from_fw (uint32_t buffer, enum dma_data_direction data_dir)
{
    struct sk_buff *skb;
    frame_info_t *frame_info;
    size_t len;

    /* Retrieve frame information. */
    frame_info = FRAME_INFO_PTR_ALIGN (dma_to_virt (NULL, buffer));
    if (frame_info->magic != FRAME_INFO_MAGIC)
        return NULL;
    skb = frame_info->skb;
    /* Clear magic. */
    frame_info->magic = 0;

    /* Get length. */
    if (data_dir == DMA_TO_DEVICE)
        len = skb->len;
    else
        len = PKT_BUF_SZ;

    /* Unmap from DMA zone. */
    dma_unmap_single (NULL, buffer, len, data_dir);

    return skb;
}

bool
frame_buffer_alloc (plcdrv_t *priv)
{
    struct sk_buff *skb;
    uint32_t skb_data_addr[PLCDRV_RX_POOL];
    unsigned int i;
    unsigned int new_skb_nb = PLCDRV_RX_POOL - skb_queue_len (&priv->rx_pool);

    for (i = 0; i < new_skb_nb; i++)
    {
        /* Allocate an sk_buff. */
        skb = dev_alloc_skb (PKT_BUF_SZ);
        if (!skb)
            break;

        /* Add it to the RX pool. */
        __skb_queue_head (&priv->rx_pool, skb);

        /* Store sk buff physical data address to send it to firmware. */
        skb_data_addr[i] = frame_skb_to_fw (priv, skb, DMA_FROM_DEVICE);
    }

    /* Send it to firmware. */
    if (i)
        ipmbox_send_empty_buf (&priv->ipmbox, skb_data_addr, i);

    if (skb_queue_len (&priv->rx_pool) != PLCDRV_RX_POOL)
        return false;
    return true;
}

void
frame_buffer_free (plcdrv_t *priv, uint32_t buffer)
{
    struct sk_buff *skb;
    struct sk_buff_head *pool;

    /* Get skb. */
    skb = frame_skb_from_fw (buffer, DMA_TO_DEVICE);

    if (!skb)
    {
        printk (KERN_CRIT "bad buffer in frame_buffer_free\n");
        return;
    }

    /* Remove it from the right TX pool. */
    pool = *(struct sk_buff_head **) skb->cb;
    __skb_unlink (skb, pool);

    /* Free it. */
    kfree_skb (skb);
}

void
frame_rx_data (plcdrv_t *priv, uint32_t data_addr,
               uint32_t data_length)
{
    struct sk_buff *skb;

    /* Retrieve skb. */
    skb = frame_skb_from_fw (data_addr, DMA_FROM_DEVICE);

    if (!skb)
    {
        printk (KERN_CRIT "bad buffer in frame_rx_data\n");
        return;
    }

    /* Remove it from the RX pool, this is not our buffer anymore. */
    __skb_unlink (skb, &priv->rx_pool);

    /* Prepare skb for linux receive level. */
    skb->dev = priv->dev;
    skb->ip_summed = CHECKSUM_UNNECESSARY;
    skb_put (skb, data_length);

    /* Apply QOS mark. */
    qos_frame_set_mark (&priv->qos, skb);

    skb->protocol = eth_type_trans (skb, priv->dev);

    /* Check sequence number on receive. */
    seq_check_rx (&priv->seq_check_ctx, skb);

    /* Pass data to the Linux internal receive level. */
    netif_receive_skb (skb);
}

void
frame_rx_mme_priv (plcdrv_t *priv, uint32_t data_addr,
                   uint32_t data_length)
{
    struct sk_buff *skb, *nlskb;
    struct nlmsghdr *nlh;
    netlink_t *nl;

    /* Retrieve skb. */
    skb = frame_skb_from_fw (data_addr, DMA_FROM_DEVICE);

    if (!skb)
    {
        printk (KERN_CRIT "bad buffer in frame_rx_mme_priv\n");
        return;
    }

    /* Remove it from the RX pool, this is not our buffer anymore. */
    __skb_unlink (skb, &priv->rx_pool);

    /* Prepare sk buff for linux receive level. */
    skb->dev = priv->dev;
    skb->ip_summed = CHECKSUM_UNNECESSARY;
    skb_put (skb, data_length);

    /* Allocate a new sk_buff to add netlink header. */
    nlskb = alloc_skb (NLMSG_LENGTH (skb->len), GFP_ATOMIC);
    if (!nlskb)
        goto msg_failure;

    /* Fill netlink header. */
    nlh = NLMSG_PUT (nlskb, 0, 0, NLMSG_DONE,
                     NLMSG_LENGTH (skb->len) - sizeof(*nlh));
    NETLINK_CB (nlskb).pid = 0; /* From kernel. */
    NETLINK_CB (nlskb).dst_group = 0; /* Unicast. */

    /* Fill this new sk_buff with the old one after netlink header. */
    memcpy (NLMSG_DATA (nlh), skb->data, skb->len);

    /* Suppress old sk_buff. */
    kfree_skb (skb);

    /* Get destination netlink. */
    BUG_ON (data_length <= ETH_MME_TYPE_OFFSET);
    if (frame_get_eth_mme_type (skb->data) >= HPAV_MME_P_DRV_BASE)
        nl = &priv->nl_plcd;
    else
        nl = &priv->nl_managerd;

    /* Send to netlink. */
    if (netlink_unicast (nl->sock, nlskb, nl->pid, MSG_DONTWAIT) < 0)
        goto nlmsg_failure;

    return;

    /* nlmsg_failure is used by NLMSG_PUT (yeark!). */
nlmsg_failure:
    kfree_skb (nlskb);
msg_failure:
    kfree_skb (skb);
}

void
frame_rx_debug_dump (plcdrv_t *priv, uint32_t data_addr,
                     uint32_t data_length)
{
    /* Check parameter. */
    BUG_ON (!priv);
    BUG_ON (data_length > DEBUG_DUMP_BUFFER_LENGTH);

    if (priv->debug_dump.started)
    {
        /* Sanity check. */
        BUG_ON (priv->debug_dump.waiting_for_buffer == false);

        /* We do not use data_length here because we have allocated a fixed size,
         * without regards of the actual size of the data. */
        dma_unmap_single (NULL, data_addr, DEBUG_DUMP_BUFFER_LENGTH,
                          DMA_FROM_DEVICE);

        /* Copy received length. */
        priv->debug_dump.buffer_received_length = data_length;

        /* Wake up our process. */
        wake_up_interruptible (&priv->debug_dump.wait_queue);
    }
}

int
frame_tx_data (struct sk_buff *skb, struct net_device *dev)
{
    unsigned short vlan_prio;
    uint32_t phy_addr;
    plcdrv_t *priv = netdev_priv (dev);

    /* Check there is an Ethernet header. */
    if (skb->len < sizeof (struct ethhdr))
    {
        /* Packet is too small to be transmitted. Free skb because we have
         * handled it (ret is ok). */
        kfree_skb (skb);
        priv->stats.tx_errors++;
        priv->stats.tx_fifo_errors++;
        return NETDEV_TX_OK;
    }

    /* Sequence check. */
    seq_check_tx (&priv->seq_check_ctx, skb);

    /* Get VLAN priority. */
    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. */
    BUG_ON (skb_queue_len (&priv->tx_pool_data) == PLCDRV_TX_POOL_DATA);

    /* Update TX pool. */
    __skb_queue_head (&priv->tx_pool_data, skb);
    *(struct sk_buff_head **) skb->cb = &priv->tx_pool_data;

    /* Map it to DMA. */
    phy_addr = frame_skb_to_fw (priv, skb, DMA_TO_DEVICE);

    /* Send it to firmware. */
    ipmbox_send_data (&priv->ipmbox, phy_addr,
                      ipmbox_msg_create_header_data (skb->len, vlan_prio));

    /* Update trans start to jiffies. */
    dev->trans_start = jiffies;

    /* Queue is now full? */
    if (skb_queue_len (&priv->tx_pool_data) == PLCDRV_TX_POOL_DATA)
    {
        /* Stop queue. */
        netif_stop_queue (dev);
    }
    return NETDEV_TX_OK;
}

void
frame_tx_mbx_mme_priv (plcdrv_t *priv, struct sk_buff *skb)
{
    uint32_t phy_addr, header;

    /* Check parameters. */
    BUG_ON (!priv);
    BUG_ON (!skb);

    /* Update TX pool, with lock. */
    skb_queue_head (&priv->tx_pool_mme, skb);
    *(struct sk_buff_head **) skb->cb = &priv->tx_pool_mme;

    /* Get physical address. */
    phy_addr = frame_skb_to_fw (priv, skb, DMA_TO_DEVICE);

    /* Build message header. */
    header = ipmbox_msg_create_header_mme_priv (skb->len);

    /* Send to firmware. */
    ipmbox_send_mbx (&priv->ipmbox, phy_addr, header);
}

void
frame_tx_mbx_debug_dump (plcdrv_t *priv, uint32_t *buffer,
                         unsigned int length)
{
    uint32_t phy_addr, header;

    /* Check parameters. */
    BUG_ON (!priv);
    BUG_ON (!buffer);
    BUG_ON (!length);

    /* Get physical address. */
    phy_addr = dma_map_single (NULL, buffer, length, DMA_FROM_DEVICE);

    /* Build message header. */
    header = ipmbox_msg_create_header_debug_dump (length);

    /* Send to firmware. */
    ipmbox_send_mbx (&priv->ipmbox, phy_addr, header);
}