Subversion Repositories fade_ether_protocol

/*

 * fpga_l3_fade - driver for L3 communication protocol with FPGA based system

 * Copyright (C) 2012 by Wojciech M. Zabolotny

 * Institute of Electronic Systems, Warsaw University of Technology

 *

 *  This program is free software; you can redistribute it and/or modify

 *  it under the terms of the GNU General Public License as published by

 *  the Free Software Foundation; either version 2 of the License, or

 *  (at your option) any later version.

 *

 *  This program is distributed in the hope that it will be useful,

 *  but WITHOUT ANY WARRANTY; without even the implied warranty of

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

 *  GNU General Public License for more details.

 *

 *  You should have received a copy of the GNU General Public License

 *  along with this program; if not, write to the Free Software

 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

 */

#include <linux/module.h>

#include <linux/kernel.h>

#include <asm/uaccess.h>

MODULE_LICENSE("GPL v2");

//#define FADE_DEBUG 1

#include <linux/device.h>

#include <linux/netdevice.h>

#include <linux/fs.h>

#include <linux/cdev.h>

#include <linux/poll.h>

#include <linux/mm.h>

#include <asm/io.h>

#include <linux/wait.h>

#include <linux/sched.h>

#include <asm/uaccess.h>  /* for put_user */

#include <linux/mutex.h>

#include "fpga_l3_fade.h"

#define SUCCESS 0

#define DEVICE_NAME "fpga_l3_fade"

/* Length of the user header in the packet

 * The following fields are BEFORE the user header

 * SRC+TGT - 12 bytes : source & destination

 * 0xFADE - 2 bytes : protocol ID

 *

 * Fields which belong to the user header:

 * 0x0100 - 2 bytes : protocol version - offset 0

 * For DATA - to PC

 * 0xA5A5 - 2 bytes : data ID 0 - offset 2

 * retry number - 2 bytes - offset 4

 * packet number - 4 bytes - offset 6

 * transm_delay - 4 bytes - offset 10

 * cmd_response - 12 bytes - offset 14

 *

 * Total: 26 bytes!

 * If the packet contains "flushed" buffer,

 * then data ID is 0xA5A6, and the last word

 * contains number of transmitted words.

 * For command response

 * 0xA55A : cmd_resp ID 2 bytes - offset 2

 * 0x0000 : filler - 2 bytes - offset 4

 * cmd_response - 12 bytes offset 6

 */

#define USER_HDR_LEN 26

/* Number of bytes of user data in a packet */

#define LOG2_USER_LEN 13

#define USER_LEN (1<<LOG2_USER_LEN) 

#define PAYL_LEN ( USER_HDR_LEN + USER_LEN )

/* Number of packets in window - this number depends on amount of RAM

 * in the FPGA - Packets in a widnow must fit in the FPGA RAM

 * Should be power of two! */

#define PKTS_IN_WINDOW (1<<4)

#define PKTS_IN_WINDOW_MASK (PKTS_IN_WINDOW-1)

/* Capacity of kernel buffer (mmapped into user space) measured in

 * number of windows - should be equal to power of two, to simplify

 * the modulo operation (replacing it by binary AND) */

#define WINDOWS_IN_BUFFER (1<<8)

#define MY_BUF_LEN (WINDOWS_IN_BUFFER * PKTS_IN_WINDOW * USER_LEN)

#define MY_BUF_LEN_MASK (MY_BUF_LEN-1)

/* Length of the acknowledment packet and command packets */

#define MY_ACK_LEN 64

/* Number of bytes to be copied from data packet to ack packet

 * It covers: retry_number, packet_number and transm_delay - 10 bytes

 */

#define MY_ACK_COPIED 10

/* The Ethernet type of our packet. This is NOT OFFICIALLY REGISTERED type,

 * however our protocol is:

 * 1. experimental,

 * 2. supposed to be used only in private networks

 */

#define MY_PROTO_ID 0xfade

#define MY_PROTO_VER 0x0100

static int max_slaves = 4;

module_param(max_slaves,int,0);

MODULE_PARM_DESC(max_slaves,"Maximum number of slave FPGA devices serviced by the system.");

static int proto_registered = 0; //Was the protocol registred? Should be deregistered at exit?

DEFINE_RWLOCK(slave_table_lock); //Used to protect table of slaves

/* Structure used to store offset of two currently serviced sets in the data buffer */

struct pkt_map {

  int num;

  int offset;

};

typedef struct

{

  // fields related to the circular buffer

  volatile int head;

  volatile int tail;

  rwlock_t ptrs_lock; //Used to protect the head and tail pointers

  unsigned char * buffer;

  struct mutex usercmd_lock;

  uint16_t cmd_code;

  uint16_t cmd_seq;

  uint8_t cmd_ack;

  uint8_t cmd_resp[12];

  rwlock_t pkts_rwlock; //Protects the pkts table and last_pkt

  uint32_t last_nack_pkt; /* Number of the last not acknowledged packet. This is also the number

                             of the first packet in the current transmission window */

  uint32_t pkts[PKTS_IN_WINDOW]; /* This array stores numbers of the last received packets in the

                                  * transmission window. It is used to avoid unnecessary copying of duplicated

                                  * packets into the receiver buffer */

  rwlock_t flags_lock; //Protects other fields of the slave_data struct

  uint32_t last_pkt_num; /* Number of the last "flushed" packet */

  uint32_t last_pkt_len; /* Number of words in the last "flushed" packet */

  char err_flag;

  char stopped_flag; /* Flag informing, that transmission has been already terminated */

  char eof_flag; /* Flag informing, that all packets are delivered after transmission is terminated */

  char active;

  char is_open;

  int rx_wakeup_thr;

  unsigned char mac[ETH_ALEN];

  struct net_device * dev;

} slave_data;

/* Auxiliary inline functions */

static inline uint16_t get_be_u16(char * buf)

{

  return be16_to_cpu(*(uint16_t *)buf);

}

static inline uint32_t get_be_u32(char * buf)

{

  return be32_to_cpu(*(uint32_t *)buf);

}

static inline uint64_t get_be_u64(char * buf)

{

  return be64_to_cpu(*(uint64_t *)buf);

}

static inline void put_skb_u16(struct sk_buff * skb, uint16_t val)

{

  void * data = skb_put(skb,sizeof(val));

  * (uint16_t *) data = cpu_to_be16(val);

}

static inline void put_skb_u32(struct sk_buff * skb, uint32_t val)

{

  void * data = skb_put(skb,sizeof(val));

  * (uint32_t *) data = cpu_to_be32(val);

}

static inline void put_skb_u64(struct sk_buff * skb, uint64_t val)

{

  void * data = skb_put(skb,sizeof(val));

  * (uint64_t *) data = cpu_to_be64(val);

}

static slave_data * slave_table = NULL;

static int my_proto_rcv(struct sk_buff * skb, struct net_device * dev, struct packet_type * pt,

                        struct net_device * orig_dev);

static struct packet_type my_proto_pt __read_mostly = {

  .type = cpu_to_be16(MY_PROTO_ID),

  .dev = NULL,

  .func = my_proto_rcv,

};

// Prototypes of functions defined in module

void cleanup_my_proto1( void );

int init_my_proto1( void );

static int my_proto1_open(struct inode *inode, struct file *file);

static int my_proto1_release(struct inode *inode, struct file *file);

static long my_proto1_ioctl (struct file *filp,  unsigned int cmd, unsigned long arg);

int my_proto1_mmap(struct file *filp, struct vm_area_struct *vma);

unsigned int my_proto1_poll(struct file *filp,poll_table *wait);

//Wait queue for user application

DECLARE_WAIT_QUEUE_HEAD (read_queue);

//Wait queue for user commands

DECLARE_WAIT_QUEUE_HEAD (usercmd_queue);

dev_t my_dev=0;

struct cdev * my_cdev = NULL;

static struct class *class_my_proto = NULL;

struct file_operations Fops = {

  .owner = THIS_MODULE,

  .open=my_proto1_open,

  .release=my_proto1_release,  /* a.k.a. close */

  .poll = my_proto1_poll,

  .unlocked_ioctl=my_proto1_ioctl,

  .mmap=my_proto1_mmap

};

/* Function used to send the user command and wait for confirmation */

static inline

long send_cmd(slave_data * sd, uint16_t cmd, uint32_t arg, void * resp, int nof_retries, int timeout)

{

  long result = -ETIMEDOUT;

  //First check, if the Ethernet device is claimed, otherwise return an error

  if(sd->dev==NULL) return -ENODEV;

  //Each slave may perform only one user command, so first check, if no other thread

  //attempts to send the command

  if ( mutex_trylock(&sd->usercmd_lock)==0) return -EBUSY;

  //Mutex acquired, we can proceed

  //First allocate the sequence number for the command

  sd->cmd_seq += 1;

  sd->cmd_code = cmd;

  sd->cmd_ack = 1; //Mark, that we are waiting for response

  //Now in the loop we send the packet, requesting execution of the command

  //and then we wait for response with timeout

  while(nof_retries--) {

    //Send the packet 

    struct sk_buff *newskb = NULL;

    uint8_t * my_data = NULL;

    newskb = alloc_skb(LL_RESERVED_SPACE(sd->dev)+MY_ACK_LEN, GFP_KERNEL);

    skb_reserve(newskb,LL_RESERVED_SPACE(sd->dev));

    skb_reset_network_header(newskb);

    newskb->dev = sd->dev;

    newskb->protocol = htons(MY_PROTO_ID);

    //Build the MAC header for the new packet

    // Tu http://lxr.linux.no/linux+*/net/ipv4/arp.c#L586 jest pokazane jak zbudować pakiet!

    if (dev_hard_header(newskb,sd->dev,MY_PROTO_ID,&sd->mac,sd->dev->dev_addr,MY_ACK_LEN+ETH_HLEN) < 0)

      {

        kfree_skb(newskb);

        return -EINVAL;

      }

    //Put the protocol version id to the packet

    put_skb_u16(newskb,MY_PROTO_VER);

    //Put the command code

    put_skb_u16(newskb,cmd);

    //Put the sequence number

    put_skb_u16(newskb,sd->cmd_seq);

    //Put the argument

    put_skb_u32(newskb,arg);

    //Fill the packet

    my_data = skb_put(newskb,MY_ACK_LEN-10);

    memset(my_data,0xa5,MY_ACK_LEN-10);

    dev_queue_xmit(newskb);

    //Sleep with short timeout, waiting for response

    if(wait_event_timeout(usercmd_queue,sd->cmd_ack==2,timeout)) {

      //Response received

      //If target buffer provided, copy data to the userspace buffer

      if(resp) memcpy(resp,sd->cmd_resp,12);

      result = SUCCESS;

      break; //exit loop

    }

  }

  //We don't wait for response any more

  sd->cmd_ack = 0;

  mutex_unlock(&sd->usercmd_lock);

  return result;

}

/* Function free_mac may be safely called even if the MAC was not taken

   it checks sd->active to detect such situation

*/

static inline

long free_mac(slave_data *sd) {

  write_lock_bh(&slave_table_lock);

  if(sd->active) {

    /* Clear the MAC address */

    sd->active = 0;

    memset(&sd->mac,0,ETH_ALEN);

    write_unlock_bh(&slave_table_lock);

    /* Now send the "stop transmission" packet to the slave */

    /* Find the net device */

    if (!sd->dev) return -ENODEV;

    dev_put(sd->dev);

    sd->dev=NULL;

  } else {

    write_unlock_bh(&slave_table_lock);

  }

  return SUCCESS;

}

static long my_proto1_ioctl (struct file *filp,

                             unsigned int cmd, unsigned long arg)

{

  slave_data * sd = filp->private_data;

  if (_IOC_TYPE(cmd) != L3_V1_IOC_MAGIC) {

    return -EINVAL;

  }

  switch (cmd) {

  case L3_V1_IOC_SETWAKEUP:

    if (arg > MY_BUF_LEN/2)

      return -EINVAL; //Don't allow to set too high read threshold!

    write_lock_bh(&sd->flags_lock);

    sd->rx_wakeup_thr = arg;

    write_unlock_bh(&sd->flags_lock);

    return 0;

  case L3_V1_IOC_GETBUFLEN:

    /* Inform the user application about the length of the buffer */

    return MY_BUF_LEN;

  case L3_V1_IOC_READPTRS:

    {

      void * res = (void *) arg;

      long res2;

      struct l3_v1_buf_pointers bp;

      if (!access_ok(VERIFY_WRITE,res,sizeof(bp))) {

        return -EFAULT;

      } else {

        read_lock_bh(&sd->ptrs_lock);

        bp.head=sd->head;

        bp.tail=sd->tail;

        bp.eof=sd->eof_flag;

        read_unlock_bh(&sd->ptrs_lock);

        res2 = __copy_to_user(res,&bp,sizeof(bp));

        if(res2)

          return -EFAULT;

        if (sd->err_flag)

          return -EIO; /* In this case user must him/herself

                          calculate the number of available bytes */

        else

          return (bp.head-bp.tail) & MY_BUF_LEN_MASK;

        /* Return the number of available bytes */

      }

    }

  case L3_V1_IOC_WRITEPTRS:

    /* Update the read pointer

     * The argument contains information about the number of bytes

     * consumed by the application

     */

    {

      int rptr;

      int wptr;

      int available_data;

      //We need to check if the amount of consumed data is correct

      write_lock_bh(&sd->ptrs_lock);

      wptr = sd->head;

      rptr = sd->tail;

      available_data = (wptr - rptr) & MY_BUF_LEN_MASK;

      if (arg>available_data)

        {

          write_unlock_bh(&sd->ptrs_lock);

          return -EINVAL;

        }

      //If the number of consumed bytes is correct, update the number of bytes

      sd->tail = (rptr + arg) & MY_BUF_LEN_MASK;

      write_unlock_bh(&sd->ptrs_lock);

      return SUCCESS;

    }

  case L3_V1_IOC_STARTMAC: //Open the slave

    {

      sd->stopped_flag = 0;

      sd->eof_flag = 0;

      //We just send a request to start transmission and wait for confirmation

      return send_cmd(sd,FCMD_START,0,NULL,100,2);

    }

  case L3_V1_IOC_STOPMAC: //Close the slave and reset it to stop transmission immediately

    {

      return send_cmd(sd,FCMD_STOP,0,NULL,100,2);

    }

  case L3_V1_IOC_RESETMAC: //Reset MAC so, that it stops transmission immediately

    {

      return send_cmd(sd,FCMD_RESET,0,NULL,100,2);

    }

  case L3_V1_IOC_GETMAC: //Open the slave

    {

      void * source = (void *) arg;

      struct l3_v1_slave sl;

      struct net_device *dev = NULL;

      long res2;

      if (!access_ok(VERIFY_READ,source,sizeof(sl))) {

        return -EFAULT;

      }

      /* First deactivate the slave to avoid situation where data are modified

       * while slave is active */

      if (sd->active) sd->active = 0;

      //Set the numbers of stored packets to MAX

      write_lock_bh(&sd->pkts_rwlock);

      memset(&sd->pkts,0xff,sizeof(sd->pkts));

      sd->last_nack_pkt=0;

      write_unlock_bh(&sd->pkts_rwlock);

      //Copy arguments from the user space

      res2 = __copy_from_user(&sl,source,sizeof(sl));

      if(res2) {

        return -EFAULT;

      }

      write_lock_bh(&slave_table_lock);

      /* Copy the MAC address */

      memcpy(&sd->mac,sl.mac,ETH_ALEN);

      sd->active = 1;

      write_unlock_bh(&slave_table_lock);

      /* Find the net device */

      sl.devname[IFNAMSIZ-1]=0; // Protect against incorrect device name

      if (sd->dev) {

        //Maybe there was no STOPMAC call after previous STARTMAC?

        dev_put(sd->dev);

        sd->dev=NULL;

      }

      dev = dev_get_by_name(&init_net,sl.devname);

      if (!dev) return -ENODEV;

      sd->dev = dev;

      return SUCCESS;

    }

  case L3_V1_IOC_FREEMAC: //Close the slave and reset it to stop transmission immediately

    {

      free_mac(sd);

      return SUCCESS;

    }

  case L3_V1_IOC_USERCMD: //Perform the user command

    {

      void * source = (void *) arg;

      long result = -EINVAL;

      struct l3_v1_usercmd ucmd;

      //First copy command data

      result = __copy_from_user(&ucmd,source,sizeof(ucmd));

      if(result) {

        return -EFAULT;

      }

      //Now we check if the command is valid user command

      if(ucmd.cmd < 0x0100) return -EINVAL;

      result = send_cmd(sd,ucmd.cmd, ucmd.arg, ucmd.resp, ucmd.nr_of_retries,ucmd.timeout);

      if(result<0) return result;

      result = __copy_to_user(source,&ucmd,sizeof(ucmd));

      return result;

    }

  }

  return -EINVAL;

}

/*

  Implementation of the poll method

*/

unsigned int my_proto1_poll(struct file *filp,poll_table *wait)

{

  unsigned int mask =0;

  slave_data * sd = filp->private_data;

  unsigned int data_available;

  poll_wait(filp,&read_queue,wait);

  read_lock_bh(&sd->ptrs_lock);

  data_available = (sd->head - sd->tail) & MY_BUF_LEN_MASK;

  if (data_available >= sd->rx_wakeup_thr) mask |= POLLIN |POLLRDNORM;

  if (sd->eof_flag) {

    if(data_available) mask |= POLLIN | POLLRDNORM;

    else mask |= POLLHUP;

  }

#ifdef FADE_DEBUG

  printk(KERN_INFO "poll head: %d tail: %d data: %d prog: %d.\n",sd->head,sd->tail,data_available,sd->rx_wakeup_thr);

#endif

  //Check if the error occured

  if (sd->err_flag) mask |= POLLERR;

  read_unlock_bh(&sd->ptrs_lock);

  return mask;

}

/* Module initialization  */

int init_my_proto1( void )

{

  int res;

  int i;

  /* Create the device class for udev */

  class_my_proto = class_create(THIS_MODULE, "my_proto");

  if (IS_ERR(class_my_proto)) {

    printk(KERN_ERR "Error creating my_proto class.\n");

    res=PTR_ERR(class_my_proto);

    goto err1;

  }

  /* Allocate the device number */

  res=alloc_chrdev_region(&my_dev, 0, max_slaves, DEVICE_NAME);

  if (res) {

    printk (KERN_ERR "Alocation of the device number for %s failed\n",

            DEVICE_NAME);

    goto err1;

  };

  /* Allocate the character device structure */

  my_cdev = cdev_alloc( );

  if (my_cdev == NULL) {

    printk (KERN_ERR "Allocation of cdev for %s failed\n",

            DEVICE_NAME);

    goto err1;

  }

  my_cdev->ops = &Fops;

  my_cdev->owner = THIS_MODULE;

  /* Add the character device to the system */

  res=cdev_add(my_cdev, my_dev, max_slaves);

  if (res) {

    printk (KERN_ERR "Registration of the device number for %s failed\n",

            DEVICE_NAME);

    goto err1;

  };

  /* Create our devices in the system */

  for (i=0;i<max_slaves;i++) {

    device_create(class_my_proto,NULL,MKDEV(MAJOR(my_dev),MINOR(my_dev)+i),NULL,"l3_fpga%d",i);

  }

  printk (KERN_ERR "%s The major device number is %d.\n",

          "Registration is a success.",

          MAJOR(my_dev));

  //Prepare the table of slaves

  slave_table = kzalloc(sizeof(slave_data)*max_slaves, GFP_KERNEL);

  if (!slave_table) return -ENOMEM;

  for (i=0;i<max_slaves;i++) {

    /* Perform initialization, which should be done only once, when the module

     * is loaded. Other actions may be needed, when the transmission from

     * particular slave is started. This will be done in IOCTL STARTMAC

     */

    slave_data * sd = &slave_table[i];

    sd->active=0; //Entry not used

    sd->dev=NULL;

    rwlock_init(&sd->pkts_rwlock);

    rwlock_init(&sd->ptrs_lock);

    rwlock_init(&sd->flags_lock);

    mutex_init(&sd->usercmd_lock);

  }

  //Install our protocol sniffer

  dev_add_pack(&my_proto_pt);

  proto_registered = 1;

  return SUCCESS;

 err1:

  /* In case of error free all allocated resources */

  cleanup_my_proto1();

  return res;

}

module_init(init_my_proto1);

/* Clean-up when removing the module */

void cleanup_my_proto1( void )

{

  /* Unregister the protocol sniffer */

  if (proto_registered) dev_remove_pack(&my_proto_pt);

  /* Free the slave table */

  if (slave_table) {

    int i;

    for (i=0;i<max_slaves;i++) {

      if (slave_table[i].buffer) {

        vfree(slave_table[i].buffer);

        slave_table[i].buffer = NULL;

      }

      if (slave_table[i].dev) {

        dev_put(slave_table[i].dev);

        slave_table[i].dev=NULL;

      }

      if (slave_table[i].active) {

        slave_table[i].active = 0;

      }

    }

    kfree(slave_table);

    slave_table=NULL;

  }

  /* Remove device from the class */

  if (my_dev && class_my_proto) {

    int i;

    for (i=0;i<max_slaves;i++) {

      device_destroy(class_my_proto,MKDEV(MAJOR(my_dev),MINOR(my_dev)+i));

    }

  }

  /* Deregister device */

  if (my_cdev) cdev_del(my_cdev);

  my_cdev=NULL;

  /* Free the device number */

  unregister_chrdev_region(my_dev, max_slaves);

  /* Deregister class */

  if (class_my_proto) {

    class_destroy(class_my_proto);

    class_my_proto=NULL;

  }

}

module_exit(cleanup_my_proto1);

/*

  Function, which receives my packet, copies the data and acknowledges the packet

  as soon as possible...

  I've tried to allow this function to handle multiple packets in parallel

  in the SMP system, however I've used rwlocks for that.

  Probably it should be improved, according to the last tendency to avoid

  rwlocks in the kernel...

*/

static int my_proto_rcv(struct sk_buff * skb, struct net_device * dev, struct packet_type * pt,

                        struct net_device * orig_dev)

{

  struct sk_buff *newskb = NULL;

  struct ethhdr * rcv_hdr = NULL;

  //unsigned int head;

  //unsigned int tail;

  int is_duplicate = 0;

  int res;

  uint32_t packet_number;

  int ns; //Number of slave

  slave_data * sd = NULL;

  int32_t pkt_dist;

  char * my_data = NULL;

  unsigned char tmp_buf[USER_HDR_LEN];

  char ack_packet = 0; //Should we acknowledge the packet?

  uint32_t pkt_pos, needed_space, buf_free;

  //Extract the MAC header from the received packet

  rcv_hdr=eth_hdr(skb);

  //First we try to identify the sender so we search the table of active slaves

  //The table is protected during the search, so it should not be changed

#ifdef FADE_DEBUG

  printk("snd: %2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x\n",(int)rcv_hdr->h_source[0],

         (int)rcv_hdr->h_source[1],(int)rcv_hdr->h_source[2],(int)rcv_hdr->h_source[3],

         (int)rcv_hdr->h_source[4],(int)rcv_hdr->h_source[5]);

#endif

  read_lock_bh(&slave_table_lock);

  for (ns=0;ns<max_slaves;ns++) {

#ifdef FADE_DEBUG

    printk("slv: %2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x  act: %d\n",

           (int)slave_table[ns].mac[0],(int)slave_table[ns].mac[1],(int)slave_table[ns].mac[2],

           (int)slave_table[ns].mac[3],(int)slave_table[ns].mac[4],(int)slave_table[ns].mac[5],

           (int)slave_table[ns].active);

#endif

    if (

        slave_table[ns].active!=0 &&

        memcmp(slave_table[ns].mac,rcv_hdr->h_source, sizeof(slave_table[0].mac))==0

        ) break;

  }

  read_unlock_bh(&slave_table_lock);

  //Now we know which slave sent us the packet (ns<max_slaves) or that

  //the packet came from an unknown slave (ns==max_slaves)

  if (unlikely(ns==max_slaves)) {

    printk(KERN_WARNING " Received packet from incorrect slave!\n");

    //Sender is not opened, so ignore the packet, and send

    //to the sender request to stop the transmission immediately

    newskb = alloc_skb(LL_RESERVED_SPACE(dev)+MY_ACK_LEN, GFP_ATOMIC);

    skb_reserve(newskb,LL_RESERVED_SPACE(dev));

    skb_reset_network_header(newskb);

    newskb->dev = dev;

    newskb->protocol = htons(MY_PROTO_ID);