1 |
15 |
wzab |
/*
|
2 |
|
|
* fpga_l3_fade - driver for L3 communication protocol with FPGA based system
|
3 |
|
|
* Copyright (C) 2012 by Wojciech M. Zabolotny
|
4 |
|
|
* Institute of Electronic Systems, Warsaw University of Technology
|
5 |
|
|
*
|
6 |
|
|
* This program is free software; you can redistribute it and/or modify
|
7 |
|
|
* it under the terms of the GNU General Public License as published by
|
8 |
|
|
* the Free Software Foundation; either version 2 of the License, or
|
9 |
|
|
* (at your option) any later version.
|
10 |
|
|
*
|
11 |
|
|
* This program is distributed in the hope that it will be useful,
|
12 |
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
13 |
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
14 |
|
|
* GNU General Public License for more details.
|
15 |
|
|
*
|
16 |
|
|
* You should have received a copy of the GNU General Public License
|
17 |
|
|
* along with this program; if not, write to the Free Software
|
18 |
|
|
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
19 |
|
|
*/
|
20 |
|
|
|
21 |
|
|
#include <linux/module.h>
|
22 |
|
|
#include <linux/kernel.h>
|
23 |
|
|
#include <asm/uaccess.h>
|
24 |
|
|
|
25 |
|
|
MODULE_LICENSE("GPL v2");
|
26 |
|
|
//#define FADE_DEBUG 1
|
27 |
|
|
#include <linux/device.h>
|
28 |
|
|
#include <linux/netdevice.h>
|
29 |
|
|
#include <linux/fs.h>
|
30 |
|
|
#include <linux/cdev.h>
|
31 |
|
|
#include <linux/poll.h>
|
32 |
|
|
#include <linux/mm.h>
|
33 |
|
|
#include <asm/io.h>
|
34 |
|
|
#include <linux/wait.h>
|
35 |
|
|
#include <linux/sched.h>
|
36 |
|
|
#include <asm/uaccess.h> /* for put_user */
|
37 |
|
|
#include <linux/mutex.h>
|
38 |
|
|
|
39 |
|
|
#include "fpga_l3_fade.h"
|
40 |
|
|
|
41 |
|
|
#define SUCCESS 0
|
42 |
|
|
#define DEVICE_NAME "fpga_l3_fade"
|
43 |
|
|
|
44 |
|
|
|
45 |
|
|
/* Length of the user header in the packet
|
46 |
|
|
* The following fields are BEFORE the user header
|
47 |
|
|
* SRC+TGT - 12 bytes : source & destination
|
48 |
|
|
* 0xFADE - 2 bytes : protocol ID
|
49 |
|
|
*
|
50 |
|
|
* Fields which belong to the user header:
|
51 |
|
|
* 0x0100 - 2 bytes : protocol version - offset 0
|
52 |
|
|
* For DATA - to PC
|
53 |
|
|
* 0xA5A5 - 2 bytes : data ID 0 - offset 2
|
54 |
|
|
* retry number - 2 bytes - offset 4
|
55 |
|
|
* packet number - 4 bytes - offset 6
|
56 |
|
|
* transm_delay - 4 bytes - offset 10
|
57 |
|
|
* cmd_response - 12 bytes - offset 14
|
58 |
|
|
*
|
59 |
|
|
* Total: 26 bytes!
|
60 |
18 |
wzab |
* If the packet contains "flushed" buffer,
|
61 |
|
|
* then data ID is 0xA5A6, and the last word
|
62 |
|
|
* contains number of transmitted words.
|
63 |
15 |
wzab |
|
64 |
|
|
* For command response
|
65 |
|
|
* 0xA55A : cmd_resp ID 2 bytes - offset 2
|
66 |
|
|
* 0x0000 : filler - 2 bytes - offset 4
|
67 |
|
|
* cmd_response - 12 bytes offset 6
|
68 |
|
|
*/
|
69 |
|
|
#define USER_HDR_LEN 26
|
70 |
|
|
|
71 |
|
|
/* Number of bytes of user data in a packet */
|
72 |
|
|
#define LOG2_USER_LEN 13
|
73 |
|
|
#define USER_LEN (1<<LOG2_USER_LEN)
|
74 |
|
|
#define PAYL_LEN ( USER_HDR_LEN + USER_LEN )
|
75 |
|
|
|
76 |
|
|
/* Number of packets in window - this number depends on amount of RAM
|
77 |
|
|
* in the FPGA - Packets in a widnow must fit in the FPGA RAM
|
78 |
|
|
* Should be power of two! */
|
79 |
18 |
wzab |
#define PKTS_IN_WINDOW (1<<4)
|
80 |
15 |
wzab |
#define PKTS_IN_WINDOW_MASK (PKTS_IN_WINDOW-1)
|
81 |
|
|
|
82 |
|
|
/* Capacity of kernel buffer (mmapped into user space) measured in
|
83 |
|
|
* number of windows - should be equal to power of two, to simplify
|
84 |
|
|
* the modulo operation (replacing it by binary AND) */
|
85 |
|
|
#define WINDOWS_IN_BUFFER (1<<8)
|
86 |
|
|
|
87 |
|
|
#define MY_BUF_LEN (WINDOWS_IN_BUFFER * PKTS_IN_WINDOW * USER_LEN)
|
88 |
|
|
#define MY_BUF_LEN_MASK (MY_BUF_LEN-1)
|
89 |
|
|
|
90 |
|
|
|
91 |
|
|
/* Length of the acknowledment packet and command packets */
|
92 |
|
|
#define MY_ACK_LEN 64
|
93 |
|
|
|
94 |
|
|
/* Number of bytes to be copied from data packet to ack packet
|
95 |
|
|
* It covers: retry_number, packet_number and transm_delay - 10 bytes
|
96 |
|
|
*/
|
97 |
|
|
#define MY_ACK_COPIED 10
|
98 |
|
|
|
99 |
|
|
/* The Ethernet type of our packet. This is NOT OFFICIALLY REGISTERED type,
|
100 |
|
|
* however our protocol is:
|
101 |
|
|
* 1. experimental,
|
102 |
|
|
* 2. supposed to be used only in private networks
|
103 |
|
|
*/
|
104 |
|
|
#define MY_PROTO_ID 0xfade
|
105 |
|
|
#define MY_PROTO_VER 0x0100
|
106 |
|
|
|
107 |
|
|
static int max_slaves = 4;
|
108 |
|
|
module_param(max_slaves,int,0);
|
109 |
|
|
MODULE_PARM_DESC(max_slaves,"Maximum number of slave FPGA devices serviced by the system.");
|
110 |
|
|
|
111 |
|
|
static int proto_registered = 0; //Was the protocol registred? Should be deregistered at exit?
|
112 |
|
|
|
113 |
|
|
DEFINE_RWLOCK(slave_table_lock); //Used to protect table of slaves
|
114 |
|
|
|
115 |
|
|
/* Structure used to store offset of two currently serviced sets in the data buffer */
|
116 |
|
|
struct pkt_map {
|
117 |
|
|
int num;
|
118 |
|
|
int offset;
|
119 |
|
|
};
|
120 |
|
|
|
121 |
|
|
typedef struct
|
122 |
|
|
{
|
123 |
|
|
// fields related to the circular buffer
|
124 |
|
|
volatile int head;
|
125 |
|
|
volatile int tail;
|
126 |
|
|
rwlock_t ptrs_lock; //Used to protect the head and tail pointers
|
127 |
|
|
|
128 |
|
|
unsigned char * buffer;
|
129 |
|
|
struct mutex usercmd_lock;
|
130 |
|
|
uint16_t cmd_code;
|
131 |
|
|
uint16_t cmd_seq;
|
132 |
|
|
uint8_t cmd_ack;
|
133 |
|
|
uint8_t cmd_resp[12];
|
134 |
|
|
rwlock_t pkts_rwlock; //Protects the pkts table and last_pkt
|
135 |
|
|
uint32_t last_nack_pkt; /* Number of the last not acknowledged packet. This is also the number
|
136 |
|
|
of the first packet in the current transmission window */
|
137 |
|
|
uint32_t pkts[PKTS_IN_WINDOW]; /* This array stores numbers of the last received packets in the
|
138 |
|
|
* transmission window. It is used to avoid unnecessary copying of duplicated
|
139 |
|
|
* packets into the receiver buffer */
|
140 |
|
|
rwlock_t flags_lock; //Protects other fields of the slave_data struct
|
141 |
18 |
wzab |
uint32_t last_pkt_num; /* Number of the last "flushed" packet */
|
142 |
|
|
uint32_t last_pkt_len; /* Number of words in the last "flushed" packet */
|
143 |
15 |
wzab |
char err_flag;
|
144 |
18 |
wzab |
char stopped_flag; /* Flag informing, that transmission has been already terminated */
|
145 |
|
|
char eof_flag; /* Flag informing, that all packets are delivered after transmission is terminated */
|
146 |
15 |
wzab |
char active;
|
147 |
|
|
char is_open;
|
148 |
|
|
int rx_wakeup_thr;
|
149 |
|
|
unsigned char mac[ETH_ALEN];
|
150 |
|
|
struct net_device * dev;
|
151 |
|
|
} slave_data;
|
152 |
|
|
|
153 |
16 |
wzab |
/* Auxiliary inline functions */
|
154 |
|
|
static inline uint16_t get_be_u16(char * buf)
|
155 |
|
|
{
|
156 |
|
|
return be16_to_cpu(*(uint16_t *)buf);
|
157 |
|
|
}
|
158 |
|
|
|
159 |
|
|
static inline uint32_t get_be_u32(char * buf)
|
160 |
|
|
{
|
161 |
|
|
return be32_to_cpu(*(uint32_t *)buf);
|
162 |
|
|
}
|
163 |
|
|
|
164 |
|
|
static inline uint64_t get_be_u64(char * buf)
|
165 |
|
|
{
|
166 |
|
|
return be64_to_cpu(*(uint64_t *)buf);
|
167 |
|
|
}
|
168 |
|
|
|
169 |
|
|
static inline void put_skb_u16(struct sk_buff * skb, uint16_t val)
|
170 |
|
|
{
|
171 |
18 |
wzab |
void * data = skb_put(skb,sizeof(val));
|
172 |
16 |
wzab |
* (uint16_t *) data = cpu_to_be16(val);
|
173 |
|
|
}
|
174 |
|
|
|
175 |
|
|
static inline void put_skb_u32(struct sk_buff * skb, uint32_t val)
|
176 |
|
|
{
|
177 |
18 |
wzab |
void * data = skb_put(skb,sizeof(val));
|
178 |
16 |
wzab |
* (uint32_t *) data = cpu_to_be32(val);
|
179 |
|
|
}
|
180 |
|
|
|
181 |
|
|
static inline void put_skb_u64(struct sk_buff * skb, uint64_t val)
|
182 |
|
|
{
|
183 |
18 |
wzab |
void * data = skb_put(skb,sizeof(val));
|
184 |
16 |
wzab |
* (uint64_t *) data = cpu_to_be64(val);
|
185 |
|
|
}
|
186 |
|
|
|
187 |
15 |
wzab |
static slave_data * slave_table = NULL;
|
188 |
|
|
|
189 |
|
|
static int my_proto_rcv(struct sk_buff * skb, struct net_device * dev, struct packet_type * pt,
|
190 |
|
|
struct net_device * orig_dev);
|
191 |
|
|
|
192 |
|
|
static struct packet_type my_proto_pt __read_mostly = {
|
193 |
|
|
.type = cpu_to_be16(MY_PROTO_ID),
|
194 |
|
|
.dev = NULL,
|
195 |
|
|
.func = my_proto_rcv,
|
196 |
|
|
};
|
197 |
|
|
|
198 |
|
|
// Prototypes of functions defined in module
|
199 |
|
|
void cleanup_my_proto1( void );
|
200 |
|
|
int init_my_proto1( void );
|
201 |
|
|
static int my_proto1_open(struct inode *inode, struct file *file);
|
202 |
|
|
static int my_proto1_release(struct inode *inode, struct file *file);
|
203 |
|
|
static long my_proto1_ioctl (struct file *filp, unsigned int cmd, unsigned long arg);
|
204 |
|
|
int my_proto1_mmap(struct file *filp, struct vm_area_struct *vma);
|
205 |
|
|
unsigned int my_proto1_poll(struct file *filp,poll_table *wait);
|
206 |
|
|
|
207 |
|
|
//Wait queue for user application
|
208 |
|
|
DECLARE_WAIT_QUEUE_HEAD (read_queue);
|
209 |
|
|
//Wait queue for user commands
|
210 |
|
|
DECLARE_WAIT_QUEUE_HEAD (usercmd_queue);
|
211 |
|
|
dev_t my_dev=0;
|
212 |
|
|
struct cdev * my_cdev = NULL;
|
213 |
|
|
static struct class *class_my_proto = NULL;
|
214 |
|
|
|
215 |
|
|
struct file_operations Fops = {
|
216 |
|
|
.owner = THIS_MODULE,
|
217 |
|
|
.open=my_proto1_open,
|
218 |
|
|
.release=my_proto1_release, /* a.k.a. close */
|
219 |
|
|
.poll = my_proto1_poll,
|
220 |
|
|
.unlocked_ioctl=my_proto1_ioctl,
|
221 |
|
|
.mmap=my_proto1_mmap
|
222 |
|
|
};
|
223 |
|
|
|
224 |
18 |
wzab |
|
225 |
|
|
/* Function used to send the user command and wait for confirmation */
|
226 |
|
|
static inline
|
227 |
|
|
long send_cmd(slave_data * sd, uint16_t cmd, uint32_t arg, void * resp, int nof_retries, int timeout)
|
228 |
|
|
{
|
229 |
|
|
long result = -ETIMEDOUT;
|
230 |
19 |
wzab |
//First check, if the Ethernet device is claimed, otherwise return an error
|
231 |
|
|
if(sd->dev==NULL) return -ENODEV;
|
232 |
18 |
wzab |
//Each slave may perform only one user command, so first check, if no other thread
|
233 |
|
|
//attempts to send the command
|
234 |
|
|
if ( mutex_trylock(&sd->usercmd_lock)==0) return -EBUSY;
|
235 |
|
|
//Mutex acquired, we can proceed
|
236 |
|
|
//First allocate the sequence number for the command
|
237 |
|
|
sd->cmd_seq += 1;
|
238 |
|
|
sd->cmd_code = cmd;
|
239 |
|
|
sd->cmd_ack = 1; //Mark, that we are waiting for response
|
240 |
|
|
//Now in the loop we send the packet, requesting execution of the command
|
241 |
|
|
//and then we wait for response with timeout
|
242 |
|
|
while(nof_retries--) {
|
243 |
|
|
//Send the packet
|
244 |
|
|
struct sk_buff *newskb = NULL;
|
245 |
|
|
uint8_t * my_data = NULL;
|
246 |
|
|
newskb = alloc_skb(LL_RESERVED_SPACE(sd->dev)+MY_ACK_LEN, GFP_KERNEL);
|
247 |
|
|
skb_reserve(newskb,LL_RESERVED_SPACE(sd->dev));
|
248 |
|
|
skb_reset_network_header(newskb);
|
249 |
|
|
newskb->dev = sd->dev;
|
250 |
|
|
newskb->protocol = htons(MY_PROTO_ID);
|
251 |
|
|
//Build the MAC header for the new packet
|
252 |
|
|
// Tu http://lxr.linux.no/linux+*/net/ipv4/arp.c#L586 jest pokazane jak zbudować pakiet!
|
253 |
|
|
if (dev_hard_header(newskb,sd->dev,MY_PROTO_ID,&sd->mac,sd->dev->dev_addr,MY_ACK_LEN+ETH_HLEN) < 0)
|
254 |
|
|
{
|
255 |
|
|
kfree_skb(newskb);
|
256 |
|
|
return -EINVAL;
|
257 |
|
|
}
|
258 |
|
|
//Put the protocol version id to the packet
|
259 |
|
|
put_skb_u16(newskb,MY_PROTO_VER);
|
260 |
|
|
//Put the command code
|
261 |
|
|
put_skb_u16(newskb,cmd);
|
262 |
|
|
//Put the sequence number
|
263 |
|
|
put_skb_u16(newskb,sd->cmd_seq);
|
264 |
|
|
//Put the argument
|
265 |
|
|
put_skb_u32(newskb,arg);
|
266 |
|
|
//Fill the packet
|
267 |
|
|
my_data = skb_put(newskb,MY_ACK_LEN-10);
|
268 |
|
|
memset(my_data,0xa5,MY_ACK_LEN-10);
|
269 |
|
|
dev_queue_xmit(newskb);
|
270 |
|
|
//Sleep with short timeout, waiting for response
|
271 |
|
|
if(wait_event_timeout(usercmd_queue,sd->cmd_ack==2,timeout)) {
|
272 |
|
|
//Response received
|
273 |
|
|
//If target buffer provided, copy data to the userspace buffer
|
274 |
|
|
if(resp) memcpy(resp,sd->cmd_resp,12);
|
275 |
|
|
result = SUCCESS;
|
276 |
|
|
break; //exit loop
|
277 |
|
|
}
|
278 |
|
|
}
|
279 |
|
|
//We don't wait for response any more
|
280 |
|
|
sd->cmd_ack = 0;
|
281 |
|
|
mutex_unlock(&sd->usercmd_lock);
|
282 |
|
|
return result;
|
283 |
|
|
}
|
284 |
|
|
|
285 |
|
|
/* Function free_mac may be safely called even if the MAC was not taken
|
286 |
|
|
it checks sd->active to detect such situation
|
287 |
|
|
*/
|
288 |
|
|
static inline
|
289 |
|
|
long free_mac(slave_data *sd) {
|
290 |
|
|
write_lock_bh(&slave_table_lock);
|
291 |
|
|
if(sd->active) {
|
292 |
|
|
/* Clear the MAC address */
|
293 |
|
|
sd->active = 0;
|
294 |
|
|
memset(&sd->mac,0,ETH_ALEN);
|
295 |
|
|
write_unlock_bh(&slave_table_lock);
|
296 |
|
|
/* Now send the "stop transmission" packet to the slave */
|
297 |
|
|
/* Find the net device */
|
298 |
|
|
if (!sd->dev) return -ENODEV;
|
299 |
|
|
dev_put(sd->dev);
|
300 |
|
|
sd->dev=NULL;
|
301 |
|
|
} else {
|
302 |
|
|
write_unlock_bh(&slave_table_lock);
|
303 |
|
|
}
|
304 |
|
|
return SUCCESS;
|
305 |
|
|
}
|
306 |
|
|
|
307 |
15 |
wzab |
static long my_proto1_ioctl (struct file *filp,
|
308 |
|
|
unsigned int cmd, unsigned long arg)
|
309 |
|
|
{
|
310 |
|
|
slave_data * sd = filp->private_data;
|
311 |
|
|
if (_IOC_TYPE(cmd) != L3_V1_IOC_MAGIC) {
|
312 |
|
|
return -EINVAL;
|
313 |
|
|
}
|
314 |
|
|
switch (cmd) {
|
315 |
|
|
case L3_V1_IOC_SETWAKEUP:
|
316 |
|
|
if (arg > MY_BUF_LEN/2)
|
317 |
|
|
return -EINVAL; //Don't allow to set too high read threshold!
|
318 |
|
|
write_lock_bh(&sd->flags_lock);
|
319 |
|
|
sd->rx_wakeup_thr = arg;
|
320 |
|
|
write_unlock_bh(&sd->flags_lock);
|
321 |
|
|
return 0;
|
322 |
|
|
case L3_V1_IOC_GETBUFLEN:
|
323 |
|
|
/* Inform the user application about the length of the buffer */
|
324 |
|
|
return MY_BUF_LEN;
|
325 |
|
|
case L3_V1_IOC_READPTRS:
|
326 |
|
|
{
|
327 |
|
|
void * res = (void *) arg;
|
328 |
|
|
long res2;
|
329 |
|
|
struct l3_v1_buf_pointers bp;
|
330 |
|
|
if (!access_ok(VERIFY_WRITE,res,sizeof(bp))) {
|
331 |
|
|
return -EFAULT;
|
332 |
|
|
} else {
|
333 |
|
|
read_lock_bh(&sd->ptrs_lock);
|
334 |
|
|
bp.head=sd->head;
|
335 |
|
|
bp.tail=sd->tail;
|
336 |
18 |
wzab |
bp.eof=sd->eof_flag;
|
337 |
15 |
wzab |
read_unlock_bh(&sd->ptrs_lock);
|
338 |
|
|
res2 = __copy_to_user(res,&bp,sizeof(bp));
|
339 |
|
|
if(res2)
|
340 |
|
|
return -EFAULT;
|
341 |
|
|
if (sd->err_flag)
|
342 |
|
|
return -EIO; /* In this case user must him/herself
|
343 |
|
|
calculate the number of available bytes */
|
344 |
|
|
else
|
345 |
|
|
return (bp.head-bp.tail) & MY_BUF_LEN_MASK;
|
346 |
|
|
/* Return the number of available bytes */
|
347 |
|
|
}
|
348 |
|
|
}
|
349 |
|
|
case L3_V1_IOC_WRITEPTRS:
|
350 |
|
|
/* Update the read pointer
|
351 |
|
|
* The argument contains information about the number of bytes
|
352 |
|
|
* consumed by the application
|
353 |
|
|
*/
|
354 |
|
|
{
|
355 |
|
|
int rptr;
|
356 |
|
|
int wptr;
|
357 |
|
|
int available_data;
|
358 |
|
|
//We need to check if the amount of consumed data is correct
|
359 |
|
|
write_lock_bh(&sd->ptrs_lock);
|
360 |
|
|
wptr = sd->head;
|
361 |
|
|
rptr = sd->tail;
|
362 |
|
|
available_data = (wptr - rptr) & MY_BUF_LEN_MASK;
|
363 |
|
|
if (arg>available_data)
|
364 |
|
|
{
|
365 |
|
|
write_unlock_bh(&sd->ptrs_lock);
|
366 |
|
|
return -EINVAL;
|
367 |
|
|
}
|
368 |
|
|
//If the number of consumed bytes is correct, update the number of bytes
|
369 |
|
|
sd->tail = (rptr + arg) & MY_BUF_LEN_MASK;
|
370 |
|
|
write_unlock_bh(&sd->ptrs_lock);
|
371 |
|
|
return SUCCESS;
|
372 |
|
|
}
|
373 |
|
|
case L3_V1_IOC_STARTMAC: //Open the slave
|
374 |
|
|
{
|
375 |
18 |
wzab |
sd->stopped_flag = 0;
|
376 |
|
|
sd->eof_flag = 0;
|
377 |
|
|
//We just send a request to start transmission and wait for confirmation
|
378 |
|
|
return send_cmd(sd,FCMD_START,0,NULL,100,2);
|
379 |
15 |
wzab |
}
|
380 |
18 |
wzab |
case L3_V1_IOC_STOPMAC: //Close the slave and reset it to stop transmission immediately
|
381 |
|
|
{
|
382 |
|
|
return send_cmd(sd,FCMD_STOP,0,NULL,100,2);
|
383 |
|
|
}
|
384 |
|
|
case L3_V1_IOC_RESETMAC: //Reset MAC so, that it stops transmission immediately
|
385 |
|
|
{
|
386 |
|
|
return send_cmd(sd,FCMD_RESET,0,NULL,100,2);
|
387 |
|
|
}
|
388 |
15 |
wzab |
case L3_V1_IOC_GETMAC: //Open the slave
|
389 |
|
|
{
|
390 |
|
|
void * source = (void *) arg;
|
391 |
|
|
struct l3_v1_slave sl;
|
392 |
|
|
struct net_device *dev = NULL;
|
393 |
|
|
long res2;
|
394 |
|
|
if (!access_ok(VERIFY_READ,source,sizeof(sl))) {
|
395 |
|
|
return -EFAULT;
|
396 |
|
|
}
|
397 |
|
|
/* First deactivate the slave to avoid situation where data are modified
|
398 |
|
|
* while slave is active */
|
399 |
|
|
if (sd->active) sd->active = 0;
|
400 |
|
|
//Set the numbers of stored packets to MAX
|
401 |
|
|
write_lock_bh(&sd->pkts_rwlock);
|
402 |
|
|
memset(&sd->pkts,0xff,sizeof(sd->pkts));
|
403 |
|
|
sd->last_nack_pkt=0;
|
404 |
|
|
write_unlock_bh(&sd->pkts_rwlock);
|
405 |
|
|
//Copy arguments from the user space
|
406 |
|
|
res2 = __copy_from_user(&sl,source,sizeof(sl));
|
407 |
|
|
if(res2) {
|
408 |
|
|
return -EFAULT;
|
409 |
|
|
}
|
410 |
|
|
write_lock_bh(&slave_table_lock);
|
411 |
|
|
/* Copy the MAC address */
|
412 |
|
|
memcpy(&sd->mac,sl.mac,ETH_ALEN);
|
413 |
|
|
sd->active = 1;
|
414 |
|
|
write_unlock_bh(&slave_table_lock);
|
415 |
|
|
/* Find the net device */
|
416 |
|
|
sl.devname[IFNAMSIZ-1]=0; // Protect against incorrect device name
|
417 |
|
|
if (sd->dev) {
|
418 |
|
|
//Maybe there was no STOPMAC call after previous STARTMAC?
|
419 |
|
|
dev_put(sd->dev);
|
420 |
|
|
sd->dev=NULL;
|
421 |
|
|
}
|
422 |
|
|
dev = dev_get_by_name(&init_net,sl.devname);
|
423 |
|
|
if (!dev) return -ENODEV;
|
424 |
|
|
sd->dev = dev;
|
425 |
|
|
return SUCCESS;
|
426 |
|
|
}
|
427 |
|
|
case L3_V1_IOC_FREEMAC: //Close the slave and reset it to stop transmission immediately
|
428 |
|
|
{
|
429 |
18 |
wzab |
free_mac(sd);
|
430 |
15 |
wzab |
return SUCCESS;
|
431 |
|
|
}
|
432 |
|
|
case L3_V1_IOC_USERCMD: //Perform the user command
|
433 |
|
|
{
|
434 |
|
|
void * source = (void *) arg;
|
435 |
|
|
long result = -EINVAL;
|
436 |
|
|
struct l3_v1_usercmd ucmd;
|
437 |
|
|
//First copy command data
|
438 |
|
|
result = __copy_from_user(&ucmd,source,sizeof(ucmd));
|
439 |
|
|
if(result) {
|
440 |
|
|
return -EFAULT;
|
441 |
|
|
}
|
442 |
|
|
//Now we check if the command is valid user command
|
443 |
|
|
if(ucmd.cmd < 0x0100) return -EINVAL;
|
444 |
18 |
wzab |
result = send_cmd(sd,ucmd.cmd, ucmd.arg, ucmd.resp, ucmd.nr_of_retries,ucmd.timeout);
|
445 |
|
|
if(result<0) return result;
|
446 |
|
|
result = __copy_to_user(source,&ucmd,sizeof(ucmd));
|
447 |
15 |
wzab |
return result;
|
448 |
|
|
}
|
449 |
|
|
}
|
450 |
|
|
return -EINVAL;
|
451 |
|
|
}
|
452 |
|
|
/*
|
453 |
|
|
Implementation of the poll method
|
454 |
|
|
*/
|
455 |
|
|
unsigned int my_proto1_poll(struct file *filp,poll_table *wait)
|
456 |
|
|
{
|
457 |
|
|
unsigned int mask =0;
|
458 |
|
|
slave_data * sd = filp->private_data;
|
459 |
|
|
unsigned int data_available;
|
460 |
|
|
poll_wait(filp,&read_queue,wait);
|
461 |
|
|
read_lock_bh(&sd->ptrs_lock);
|
462 |
|
|
data_available = (sd->head - sd->tail) & MY_BUF_LEN_MASK;
|
463 |
18 |
wzab |
if (data_available >= sd->rx_wakeup_thr) mask |= POLLIN |POLLRDNORM;
|
464 |
|
|
if (sd->eof_flag) {
|
465 |
|
|
if(data_available) mask |= POLLIN | POLLRDNORM;
|
466 |
|
|
else mask |= POLLHUP;
|
467 |
|
|
}
|
468 |
15 |
wzab |
#ifdef FADE_DEBUG
|
469 |
|
|
printk(KERN_INFO "poll head: %d tail: %d data: %d prog: %d.\n",sd->head,sd->tail,data_available,sd->rx_wakeup_thr);
|
470 |
|
|
#endif
|
471 |
|
|
//Check if the error occured
|
472 |
|
|
if (sd->err_flag) mask |= POLLERR;
|
473 |
|
|
read_unlock_bh(&sd->ptrs_lock);
|
474 |
|
|
return mask;
|
475 |
|
|
}
|
476 |
|
|
|
477 |
|
|
/* Module initialization */
|
478 |
|
|
int init_my_proto1( void )
|
479 |
|
|
{
|
480 |
|
|
int res;
|
481 |
|
|
int i;
|
482 |
|
|
/* Create the device class for udev */
|
483 |
|
|
class_my_proto = class_create(THIS_MODULE, "my_proto");
|
484 |
|
|
if (IS_ERR(class_my_proto)) {
|
485 |
|
|
printk(KERN_ERR "Error creating my_proto class.\n");
|
486 |
|
|
res=PTR_ERR(class_my_proto);
|
487 |
|
|
goto err1;
|
488 |
|
|
}
|
489 |
|
|
/* Allocate the device number */
|
490 |
|
|
res=alloc_chrdev_region(&my_dev, 0, max_slaves, DEVICE_NAME);
|
491 |
|
|
if (res) {
|
492 |
|
|
printk (KERN_ERR "Alocation of the device number for %s failed\n",
|
493 |
|
|
DEVICE_NAME);
|
494 |
|
|
goto err1;
|
495 |
|
|
};
|
496 |
|
|
/* Allocate the character device structure */
|
497 |
|
|
my_cdev = cdev_alloc( );
|
498 |
|
|
if (my_cdev == NULL) {
|
499 |
|
|
printk (KERN_ERR "Allocation of cdev for %s failed\n",
|
500 |
|
|
DEVICE_NAME);
|
501 |
|
|
goto err1;
|
502 |
|
|
}
|
503 |
|
|
my_cdev->ops = &Fops;
|
504 |
|
|
my_cdev->owner = THIS_MODULE;
|
505 |
|
|
/* Add the character device to the system */
|
506 |
|
|
res=cdev_add(my_cdev, my_dev, max_slaves);
|
507 |
|
|
if (res) {
|
508 |
|
|
printk (KERN_ERR "Registration of the device number for %s failed\n",
|
509 |
|
|
DEVICE_NAME);
|
510 |
|
|
goto err1;
|
511 |
|
|
};
|
512 |
|
|
/* Create our devices in the system */
|
513 |
|
|
for (i=0;i<max_slaves;i++) {
|
514 |
|
|
device_create(class_my_proto,NULL,MKDEV(MAJOR(my_dev),MINOR(my_dev)+i),NULL,"l3_fpga%d",i);
|
515 |
|
|
}
|
516 |
|
|
printk (KERN_ERR "%s The major device number is %d.\n",
|
517 |
|
|
"Registration is a success.",
|
518 |
|
|
MAJOR(my_dev));
|
519 |
|
|
//Prepare the table of slaves
|
520 |
|
|
slave_table = kzalloc(sizeof(slave_data)*max_slaves, GFP_KERNEL);
|
521 |
|
|
if (!slave_table) return -ENOMEM;
|
522 |
|
|
for (i=0;i<max_slaves;i++) {
|
523 |
|
|
/* Perform initialization, which should be done only once, when the module
|
524 |
|
|
* is loaded. Other actions may be needed, when the transmission from
|
525 |
|
|
* particular slave is started. This will be done in IOCTL STARTMAC
|
526 |
|
|
*/
|
527 |
|
|
slave_data * sd = &slave_table[i];
|
528 |
|
|
sd->active=0; //Entry not used
|
529 |
|
|
sd->dev=NULL;
|
530 |
|
|
rwlock_init(&sd->pkts_rwlock);
|
531 |
|
|
rwlock_init(&sd->ptrs_lock);
|
532 |
|
|
rwlock_init(&sd->flags_lock);
|
533 |
|
|
mutex_init(&sd->usercmd_lock);
|
534 |
|
|
}
|
535 |
|
|
//Install our protocol sniffer
|
536 |
|
|
dev_add_pack(&my_proto_pt);
|
537 |
|
|
proto_registered = 1;
|
538 |
|
|
return SUCCESS;
|
539 |
|
|
err1:
|
540 |
|
|
/* In case of error free all allocated resources */
|
541 |
|
|
cleanup_my_proto1();
|
542 |
|
|
return res;
|
543 |
|
|
}
|
544 |
|
|
|
545 |
|
|
module_init(init_my_proto1);
|
546 |
|
|
|
547 |
|
|
/* Clean-up when removing the module */
|
548 |
|
|
void cleanup_my_proto1( void )
|
549 |
|
|
{
|
550 |
|
|
/* Unregister the protocol sniffer */
|
551 |
|
|
if (proto_registered) dev_remove_pack(&my_proto_pt);
|
552 |
|
|
/* Free the slave table */
|
553 |
|
|
if (slave_table) {
|
554 |
|
|
int i;
|
555 |
|
|
for (i=0;i<max_slaves;i++) {
|
556 |
|
|
if (slave_table[i].buffer) {
|
557 |
|
|
vfree(slave_table[i].buffer);
|
558 |
|
|
slave_table[i].buffer = NULL;
|
559 |
|
|
}
|
560 |
|
|
if (slave_table[i].dev) {
|
561 |
|
|
dev_put(slave_table[i].dev);
|
562 |
|
|
slave_table[i].dev=NULL;
|
563 |
|
|
}
|
564 |
|
|
if (slave_table[i].active) {
|
565 |
|
|
slave_table[i].active = 0;
|
566 |
|
|
}
|
567 |
|
|
}
|
568 |
|
|
kfree(slave_table);
|
569 |
|
|
slave_table=NULL;
|
570 |
|
|
}
|
571 |
|
|
/* Remove device from the class */
|
572 |
|
|
if (my_dev && class_my_proto) {
|
573 |
|
|
int i;
|
574 |
|
|
for (i=0;i<max_slaves;i++) {
|
575 |
|
|
device_destroy(class_my_proto,MKDEV(MAJOR(my_dev),MINOR(my_dev)+i));
|
576 |
|
|
}
|
577 |
|
|
}
|
578 |
|
|
/* Deregister device */
|
579 |
|
|
if (my_cdev) cdev_del(my_cdev);
|
580 |
|
|
my_cdev=NULL;
|
581 |
|
|
/* Free the device number */
|
582 |
|
|
unregister_chrdev_region(my_dev, max_slaves);
|
583 |
|
|
/* Deregister class */
|
584 |
|
|
if (class_my_proto) {
|
585 |
|
|
class_destroy(class_my_proto);
|
586 |
|
|
class_my_proto=NULL;
|
587 |
|
|
}
|
588 |
|
|
|
589 |
|
|
}
|
590 |
|
|
module_exit(cleanup_my_proto1);
|
591 |
|
|
/*
|
592 |
|
|
Function, which receives my packet, copies the data and acknowledges the packet
|
593 |
|
|
as soon as possible...
|
594 |
|
|
I've tried to allow this function to handle multiple packets in parallel
|
595 |
|
|
in the SMP system, however I've used rwlocks for that.
|
596 |
|
|
Probably it should be improved, according to the last tendency to avoid
|
597 |
|
|
rwlocks in the kernel...
|
598 |
|
|
*/
|
599 |
|
|
|
600 |
|
|
static int my_proto_rcv(struct sk_buff * skb, struct net_device * dev, struct packet_type * pt,
|
601 |
|
|
struct net_device * orig_dev)
|
602 |
|
|
{
|
603 |
|
|
struct sk_buff *newskb = NULL;
|
604 |
|
|
struct ethhdr * rcv_hdr = NULL;
|
605 |
|
|
//unsigned int head;
|
606 |
|
|
//unsigned int tail;
|
607 |
|
|
int is_duplicate = 0;
|
608 |
|
|
int res;
|
609 |
|
|
uint32_t packet_number;
|
610 |
|
|
int ns; //Number of slave
|
611 |
|
|
slave_data * sd = NULL;
|
612 |
|
|
int32_t pkt_dist;
|
613 |
|
|
char * my_data = NULL;
|
614 |
|
|
unsigned char tmp_buf[USER_HDR_LEN];
|
615 |
|
|
char ack_packet = 0; //Should we acknowledge the packet?
|
616 |
18 |
wzab |
uint32_t pkt_pos, needed_space, buf_free;
|
617 |
15 |
wzab |
//Extract the MAC header from the received packet
|
618 |
|
|
rcv_hdr=eth_hdr(skb);
|
619 |
|
|
//First we try to identify the sender so we search the table of active slaves
|
620 |
|
|
//The table is protected during the search, so it should not be changed
|
621 |
|
|
#ifdef FADE_DEBUG
|
622 |
|
|
printk("snd: %2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x\n",(int)rcv_hdr->h_source[0],
|
623 |
|
|
(int)rcv_hdr->h_source[1],(int)rcv_hdr->h_source[2],(int)rcv_hdr->h_source[3],
|
624 |
|
|
(int)rcv_hdr->h_source[4],(int)rcv_hdr->h_source[5]);
|
625 |
|
|
#endif
|
626 |
|
|
read_lock_bh(&slave_table_lock);
|
627 |
|
|
for (ns=0;ns<max_slaves;ns++) {
|
628 |
|
|
#ifdef FADE_DEBUG
|
629 |
|
|
printk("slv: %2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x act: %d\n",
|
630 |
|
|
(int)slave_table[ns].mac[0],(int)slave_table[ns].mac[1],(int)slave_table[ns].mac[2],
|
631 |
|
|
(int)slave_table[ns].mac[3],(int)slave_table[ns].mac[4],(int)slave_table[ns].mac[5],
|
632 |
|
|
(int)slave_table[ns].active);
|
633 |
|
|
#endif
|
634 |
|
|
if (
|
635 |
|
|
slave_table[ns].active!=0 &&
|
636 |
|
|
memcmp(slave_table[ns].mac,rcv_hdr->h_source, sizeof(slave_table[0].mac))==0
|
637 |
|
|
) break;
|
638 |
|
|
}
|
639 |
|
|
read_unlock_bh(&slave_table_lock);
|
640 |
|
|
//Now we know which slave sent us the packet (ns<max_slaves) or that
|
641 |
|
|
//the packet came from an unknown slave (ns==max_slaves)
|
642 |
|
|
if (unlikely(ns==max_slaves)) {
|
643 |
|
|
printk(KERN_WARNING " Received packet from incorrect slave!\n");
|
644 |
|
|
//Sender is not opened, so ignore the packet, and send
|
645 |
|
|
//to the sender request to stop the transmission immediately
|
646 |
|
|
newskb = alloc_skb(LL_RESERVED_SPACE(dev)+MY_ACK_LEN, GFP_ATOMIC);
|
647 |
|
|
skb_reserve(newskb,LL_RESERVED_SPACE(dev));
|
648 |
|
|
skb_reset_network_header(newskb);
|
649 |
|
|
newskb->dev = dev;
|
650 |
|
|
newskb->protocol = htons(MY_PROTO_ID);
|
651 |
|
|
//Build the MAC header for the new packet
|
652 |
|
|
// Here is shown how to build a packet: http://lxr.linux.no/linux+*/net/ipv4/arp.c#L586
|
653 |
|
|
if (dev_hard_header(newskb,dev,MY_PROTO_ID,&rcv_hdr->h_source,&rcv_hdr->h_dest,MY_ACK_LEN+ETH_HLEN) < 0)
|
654 |
|
|
goto error;
|
655 |
|
|
//Put the protocol version id to the packet
|
656 |
16 |
wzab |
put_skb_u16(newskb,MY_PROTO_VER);
|
657 |
18 |
wzab |
//Put the "restart" command to the packet, which should force it to stop transmission
|
658 |
|
|
//immediately
|
659 |
|
|
put_skb_u16(newskb,FCMD_RESET);
|
660 |
15 |
wzab |
my_data = skb_put(newskb,MY_ACK_LEN - 4);
|
661 |
|
|
memset(my_data,0xa5,MY_ACK_LEN - 4);
|
662 |
|
|
dev_queue_xmit(newskb);
|
663 |
|
|
kfree_skb(skb);
|
664 |
|
|
return NET_RX_DROP;
|
665 |
|
|
}
|
666 |
|
|
sd = &slave_table[ns]; //To speed up access to the data describing state of the slave
|
667 |
|
|
#ifdef FADE_DEBUG
|
668 |
|
|
printk(KERN_INFO " Received packet!\n");
|
669 |
|
|
#endif
|
670 |
|
|
//Now we should analyze the origin and meaning of the packet
|
671 |
|
|
//To avoid problems with scattered packets, we copy initial part of data to the buffer
|
672 |
|
|
//using the skb_copy_bits
|
673 |
|
|
skb_copy_bits(skb,0,tmp_buf,USER_HDR_LEN);
|
674 |
|
|
/* We extract the information from the user header
|
675 |
|
|
* First we check if this is correct version of the protocol */
|
676 |
18 |
wzab |
if (unlikely(get_be_u16(&tmp_buf[0]) != MY_PROTO_VER)) goto wrong_pkt_type_error;
|
677 |
16 |
wzab |
if (unlikely(get_be_u16(&tmp_buf[2]) == 0xa55a)) {
|
678 |
15 |
wzab |
//This is a command response packet
|
679 |
|
|
printk(KERN_INFO " received command response packet");
|
680 |
|
|
if(sd->cmd_ack==1) {
|
681 |
|
|
//We are waiting for response
|
682 |
|
|
printk(KERN_INFO "we were waiting for command response packet");
|
683 |
16 |
wzab |
if ((get_be_u16(&tmp_buf[6]) == sd->cmd_code) &&
|
684 |
18 |
wzab |
(get_be_u16(&tmp_buf[8]) == sd->cmd_seq)){
|
685 |
15 |
wzab |
//This is a response for the right command
|
686 |
|
|
//copy the response to the slave data
|
687 |
|
|
printk(KERN_INFO "It was response for the right command");
|
688 |
|
|
memcpy(&sd->cmd_resp,&tmp_buf[6],12);
|
689 |
|
|
sd->cmd_ack=2;
|
690 |
|
|
//Wake up the waiting process
|
691 |
|
|
wake_up_interruptible(&usercmd_queue);
|
692 |
|
|
}
|
693 |
|
|
}
|
694 |
|
|
kfree_skb(skb);
|
695 |
|
|
return NET_RX_SUCCESS;
|
696 |
|
|
}
|
697 |
18 |
wzab |
if (unlikely((get_be_u16(&tmp_buf[2]) != 0xa5a5) &&
|
698 |
|
|
(get_be_u16(&tmp_buf[2]) != 0xa5a6))
|
699 |
|
|
) {
|
700 |
15 |
wzab |
//This is not a data packet
|
701 |
|
|
goto wrong_pkt_type_error;
|
702 |
|
|
}
|
703 |
|
|
/* Now we handle the data packet
|
704 |
|
|
PLEASE NOTE, THAT THIS MUST TIGHTLY CORRESPOND
|
705 |
|
|
TO YOUR FPGA IMPLEMENTATION! */
|
706 |
|
|
//Check, if we need to read a command response
|
707 |
|
|
if(sd->cmd_ack==1) {
|
708 |
|
|
//We are waiting for response
|
709 |
16 |
wzab |
if ((get_be_u16(&tmp_buf[14]) == sd->cmd_code) &&
|
710 |
|
|
(get_be_u16(&tmp_buf[16]) == sd->cmd_seq)) {
|
711 |
15 |
wzab |
//This is a response for the right command
|
712 |
|
|
//copy the response to the slave data
|
713 |
|
|
memcpy(&sd->cmd_resp,&tmp_buf[14],12);
|
714 |
|
|
sd->cmd_ack=2;
|
715 |
|
|
//Wake up the waiting process
|
716 |
|
|
wake_up_interruptible(&usercmd_queue);
|
717 |
|
|
}
|
718 |
|
|
}
|
719 |
16 |
wzab |
packet_number = get_be_u32(&tmp_buf[6]);
|
720 |
15 |
wzab |
#ifdef FADE_DEBUG
|
721 |
|
|
printk(KERN_INFO "pkt=%d\n",(int)packet_number);
|
722 |
|
|
#endif
|
723 |
|
|
/* To know if this is a new packet, we compare the packet number
|
724 |
|
|
* in the received packet with the number of the last unconfirmed packet,
|
725 |
|
|
* calculating the difference between those two numbers: */
|
726 |
|
|
read_lock_bh(&sd->pkts_rwlock);
|
727 |
|
|
pkt_dist=(int32_t) packet_number - (int32_t) sd->last_nack_pkt;
|
728 |
18 |
wzab |
//Check if this packet was received before
|
729 |
15 |
wzab |
is_duplicate=(sd->pkts[packet_number & PKTS_IN_WINDOW_MASK] == packet_number) ? 1 : 0;
|
730 |
|
|
read_unlock_bh(&sd->pkts_rwlock);
|
731 |
18 |
wzab |
if (unlikely((pkt_dist<0) || (pkt_dist>=PKTS_IN_WINDOW))) {
|
732 |
|
|
//This is a "too old" packet, or packet "from the future", which should not be transimtted
|
733 |
|
|
//by the FPGA
|
734 |
15 |
wzab |
if (pkt_dist<0) {
|
735 |
18 |
wzab |
// This is a packet which was already confirmed, but probably ACK was lost
|
736 |
15 |
wzab |
#ifdef FADE_DEBUG
|
737 |
|
|
printk(KERN_INFO "Packet already confirmed: pkt=%d expect=%d last=%d\n",packet_number, sd->pkts[packet_number], sd->last_nack_pkt);
|
738 |
|
|
#endif
|
739 |
18 |
wzab |
ack_packet = 1;
|
740 |
|
|
goto confirm;
|
741 |
15 |
wzab |
} else {
|
742 |
|
|
/* This is a packet with too high set number (packet "from the future"
|
743 |
|
|
* it my be a symptom of serious communication problem! */
|
744 |
|
|
printk(KERN_ERR "Packet from the future! number: %d last_confirmed: %d\n", packet_number, sd->last_nack_pkt);
|
745 |
18 |
wzab |
goto error2;
|
746 |
15 |
wzab |
}
|
747 |
|
|
}
|
748 |
18 |
wzab |
//If we get there, it means, that:
|
749 |
|
|
// pkt_dist >= 0 and pkt_dist < PKTS_IN_WINDOW
|
750 |
|
|
// So this is an expected data packet.
|
751 |
|
|
if(is_duplicate) {
|
752 |
|
|
//Packet already confirmed, probably the ACK was lost, so simply generate the ACK
|
753 |
|
|
ack_packet = 1;
|
754 |
|
|
goto confirm;
|
755 |
|
|
}
|
756 |
|
|
//Packet not confirmed yet. Confirm it only after all processing is successfully completed
|
757 |
|
|
pkt_pos=(packet_number<<LOG2_USER_LEN) & MY_BUF_LEN_MASK;
|
758 |
|
|
//We must be sure, that the pointers do not change during this check
|
759 |
|
|
read_lock_bh(&sd->ptrs_lock);
|
760 |
|
|
//Calculate free space needed to copy the packet
|
761 |
|
|
needed_space = (pkt_pos+USER_LEN-1-(sd->head)) & MY_BUF_LEN_MASK;
|
762 |
|
|
//Calculate the amount of free space in the buffer
|
763 |
|
|
buf_free = (sd->tail - sd->head -1 ) & MY_BUF_LEN_MASK;
|
764 |
|
|
#ifdef FADE_DEBUG
|
765 |
|
|
printk(KERN_INFO "packet_nr: %d Free buffer: %d needed space: %d head=%d last_nack=%d\n",
|
766 |
|
|
packet_number, needed_space, buf_free, sd->head, sd->last_nack_pkt);
|
767 |
|
|
#endif
|
768 |
|
|
read_unlock_bh(&sd->ptrs_lock);
|
769 |
|
|
if (unlikely( buf_free <= needed_space )) goto error2; //No place for copying, drop the packet
|
770 |
|
|
// Check the length of the package
|
771 |
|
|
if (unlikely(skb->len != PAYL_LEN)) {
|
772 |
|
|
printk(KERN_ERR "Error! Length of data should be %d but is %d!\n",PAYL_LEN, skb->len);
|
773 |
|
|
sd->err_flag |= FADE_ERR_INCORRECT_LENGTH;
|
774 |
|
|
goto error2;
|
775 |
|
|
}
|
776 |
|
|
// We can safely copy all the packet to the buffer:
|
777 |
|
|
res = skb_copy_bits(skb,USER_HDR_LEN,&(sd->buffer[pkt_pos]),USER_LEN);
|
778 |
|
|
#ifdef FADE_DEBUG
|
779 |
|
|
printk(KERN_INFO " skb_copy_bits: %d", res);
|
780 |
|
|
#endif
|
781 |
|
|
if (res<0) goto error2; //Unsuccessfull copying
|
782 |
|
|
//Packet was copied, so note, that we should confirm it
|
783 |
|
|
ack_packet=1;
|
784 |
|
|
/* When packet is copied, we can check if this is the last "flushed" packet */
|
785 |
|
|
if (get_be_u16(&tmp_buf[2])==0xa5a6) {
|
786 |
|
|
//Flushed packet, store its number and length (should it be protected with spinlock?)
|
787 |
|
|
sd->last_pkt_num = packet_number;
|
788 |
|
|
//Copy the length, truncating it from 64 bits
|
789 |
|
|
sd->last_pkt_len = (uint32_t) * (uint64_t *) &(sd->buffer[pkt_pos+USER_LEN-sizeof(uint64_t)]);
|
790 |
|
|
//We have received the "flushed" buffer, mark that transmission is stopped
|
791 |
|
|
sd->stopped_flag = 1;
|
792 |
20 |
wzab |
//printk(KERN_INFO "set stopped flag");
|
793 |
18 |
wzab |
}
|
794 |
|
|
/* We modify the number of the copied packet in the pkts array, to avoid
|
795 |
|
|
* unnecessary copying if we receive a duplicate
|
796 |
|
|
* To modify the pkts table, we must close pkts_rwlock for writing */
|
797 |
|
|
write_lock_bh(&sd->pkts_rwlock);
|
798 |
|
|
sd->pkts[packet_number & PKTS_IN_WINDOW_MASK]= packet_number;
|
799 |
|
|
if (packet_number == sd->last_nack_pkt) {
|
800 |
|
|
/* If our packet was the last, which prevented shifting of the head pointer,
|
801 |
|
|
* we can try now to move the head pointer.
|
802 |
|
|
* We browse the pkts table, looking for the first packet with incorrect number
|
803 |
|
|
* i.e. the packet which was not received and not confimed yet
|
804 |
|
|
*/
|
805 |
|
|
uint32_t chk_packet_num = packet_number+1;
|
806 |
|
|
uint32_t count=0;
|
807 |
|
|
while (++count < PKTS_IN_WINDOW) {
|
808 |
|
|
if (sd->pkts[(sd->last_nack_pkt + count) & PKTS_IN_WINDOW_MASK] != chk_packet_num) break; //Packet not confirmed
|
809 |
|
|
chk_packet_num++;
|
810 |
|
|
}
|
811 |
|
|
sd->last_nack_pkt += count;
|
812 |
|
|
write_unlock_bh(&sd->pkts_rwlock);
|
813 |
|
|
/* Now we can move the head position */
|
814 |
|
|
if(likely((sd->stopped_flag == 0) ||
|
815 |
|
|
((uint32_t)(sd->last_nack_pkt-1) != sd->last_pkt_num))) {
|
816 |
|
|
//Normal packet, set head right after the last serviced packet
|
817 |
|
|
write_lock_bh(&sd->ptrs_lock);
|
818 |
|
|
sd->head = (sd->last_nack_pkt*USER_LEN) & MY_BUF_LEN_MASK;
|
819 |
|
|
//Now try to wake up the reading process
|
820 |
|
|
if (((sd->head - sd->tail) & MY_BUF_LEN_MASK) >= sd->rx_wakeup_thr)
|
821 |
|
|
wake_up_interruptible(&read_queue);
|
822 |
|
|
} else {
|
823 |
|
|
//Flushed packet, set head right after the end of the packet
|
824 |
|
|
write_lock_bh(&sd->ptrs_lock);
|
825 |
|
|
sd->head = ((sd->last_nack_pkt-1)*USER_LEN+sd->last_pkt_len) & MY_BUF_LEN_MASK;
|
826 |
|
|
//We have consumed the last, "flushed" buffer, so now we can set the eof flag
|
827 |
|
|
sd-> eof_flag = 1;
|
828 |
20 |
wzab |
//printk(KERN_ALERT "set eof flag!");
|
829 |
18 |
wzab |
//And we wake up the reading process
|
830 |
|
|
wake_up_interruptible(&read_queue);
|
831 |
|
|
} //if - stopped_flag
|
832 |
|
|
write_unlock_bh(&sd->ptrs_lock);
|
833 |
|
|
} else { // if - last_nack_pkt
|
834 |
|
|
write_unlock_bh(&sd->pkts_rwlock);
|
835 |
|
|
}
|
836 |
|
|
confirm:
|
837 |
|
|
//Send the confirmation if required
|
838 |
15 |
wzab |
if (likely(ack_packet)) {
|
839 |
|
|
newskb = alloc_skb(LL_RESERVED_SPACE(dev)+MY_ACK_LEN, GFP_ATOMIC);
|
840 |
|
|
skb_reserve(newskb,LL_RESERVED_SPACE(dev));
|
841 |
|
|
skb_reset_network_header(newskb);
|
842 |
|
|
newskb->dev = dev;
|
843 |
|
|
newskb->protocol = htons(MY_PROTO_ID);
|
844 |
|
|
//Build the MAC header for the new packet
|
845 |
|
|
// Tu http://lxr.linux.no/linux+*/net/ipv4/arp.c#L586 jest pokazane jak zbudować pakiet!
|
846 |
|
|
if (dev_hard_header(newskb,dev,MY_PROTO_ID,&rcv_hdr->h_source,&rcv_hdr->h_dest,MY_ACK_LEN+ETH_HLEN) < 0)
|
847 |
|
|
goto error;
|
848 |
|
|
//Put the protocol version id to the packet
|
849 |
18 |
wzab |
put_skb_u16(newskb,MY_PROTO_VER);
|
850 |
15 |
wzab |
//Put the "ACKNOWLEDGE" type
|
851 |
18 |
wzab |
put_skb_u16(newskb,FCMD_ACK);
|
852 |
15 |
wzab |
//Copy the begining of the received packet to the acknowledge packet
|
853 |
|
|
my_data = skb_put(newskb,MY_ACK_COPIED);
|
854 |
|
|
res = skb_copy_bits(skb,4,my_data,MY_ACK_COPIED);
|
855 |
|
|
my_data = skb_put(newskb,MY_ACK_LEN -MY_ACK_COPIED-4);
|
856 |
|
|
memset(my_data,0xa5,MY_ACK_LEN - MY_ACK_COPIED-4);
|
857 |
|
|
#ifdef FADE_DEBUG
|
858 |
|
|
printk(KERN_INFO " skb_nh: %x, skb_dt: %x, skb_nh2: %x, skb_t: %x\n tail: %d head: %d\n",skb_network_header(newskb),newskb->data,
|
859 |
|
|
newskb->network_header,newskb->tail, sd->tail, sd->head) ;
|
860 |
|
|
#endif
|
861 |
|
|
dev_queue_xmit(newskb);
|
862 |
|
|
}
|
863 |
|
|
kfree_skb(skb);
|
864 |
|
|
return NET_RX_SUCCESS;
|
865 |
|
|
wrong_pkt_type_error:
|
866 |
|
|
//This code should be called with sd initialized,
|
867 |
|
|
//but to avoid kernel panic, check if sd was set
|
868 |
|
|
if(sd) {
|
869 |
|
|
write_lock_bh(&sd->flags_lock);
|
870 |
|
|
sd->err_flag |= FADE_ERR_INCORRECT_PACKET_TYPE;
|
871 |
|
|
write_unlock_bh(&sd->flags_lock);
|
872 |
|
|
} else {
|
873 |
|
|
printk(KERN_ERR "FADE: wrong_pkt_type_error called with null sd");
|
874 |
|
|
}
|
875 |
|
|
error:
|
876 |
|
|
if (newskb) kfree_skb(newskb);
|
877 |
18 |
wzab |
error2:
|
878 |
15 |
wzab |
if (skb) kfree_skb(skb);
|
879 |
|
|
return NET_RX_DROP;
|
880 |
|
|
}
|
881 |
|
|
|
882 |
|
|
/*
|
883 |
|
|
Implementation of the "device open" function
|
884 |
|
|
*/
|
885 |
|
|
static int my_proto1_open(struct inode *inode,
|
886 |
|
|
struct file *file)
|
887 |
|
|
{
|
888 |
|
|
int i;
|
889 |
|
|
slave_data * sd = NULL;
|
890 |
|
|
unsigned long flags;
|
891 |
|
|
i=iminor(inode)-MINOR(my_dev);
|
892 |
|
|
if (i >= max_slaves) {
|
893 |
|
|
printk(KERN_WARNING "Trying to access %s slave with too high minor number: %d\n",
|
894 |
|
|
DEVICE_NAME, i);
|
895 |
|
|
return -ENODEV;
|
896 |
|
|
}
|
897 |
|
|
read_lock_irqsave(&slave_table_lock,flags);
|
898 |
|
|
sd = &slave_table[i];
|
899 |
|
|
//Each device may be opened only once!
|
900 |
|
|
if (sd->is_open) {
|
901 |
|
|
return -EBUSY;
|
902 |
|
|
read_unlock_irqrestore(&slave_table_lock,flags);
|
903 |
|
|
}
|
904 |
|
|
//Prepare slave_table for operation
|
905 |
|
|
read_unlock_irqrestore(&slave_table_lock,flags);
|
906 |
|
|
sd->buffer = vmalloc_user(MY_BUF_LEN);
|
907 |
|
|
if (!sd->buffer) return -ENOMEM;
|
908 |
|
|
//Set the MAC address to 0
|
909 |
|
|
memset(sd->mac,0,sizeof(sd->mac));
|
910 |
|
|
sd->head = 0;
|
911 |
|
|
sd->tail = 0;
|
912 |
18 |
wzab |
sd->eof_flag = 0;
|
913 |
|
|
sd->stopped_flag = 0;
|
914 |
15 |
wzab |
sd->err_flag = 0;
|
915 |
|
|
sd->last_nack_pkt = 0;
|
916 |
|
|
sd->rx_wakeup_thr = 1;
|
917 |
|
|
sd->active = 0;
|
918 |
|
|
sd->cmd_seq = 0;
|
919 |
|
|
sd->is_open = 1;
|
920 |
|
|
file->private_data=sd;
|
921 |
|
|
return SUCCESS;
|
922 |
|
|
}
|
923 |
|
|
|
924 |
|
|
|
925 |
|
|
static int my_proto1_release(struct inode *inode,
|
926 |
|
|
struct file *file)
|
927 |
|
|
{
|
928 |
|
|
slave_data * sd = file->private_data;
|
929 |
18 |
wzab |
//#ifdef FADE_DEBUG
|
930 |
15 |
wzab |
printk (KERN_INFO "device_release(%p,%p)\n", inode, file);
|
931 |
18 |
wzab |
//#endif
|
932 |
15 |
wzab |
//Release resources associated with servicing of the particular device
|
933 |
|
|
if (sd) {
|
934 |
|
|
if (sd->is_open) {
|
935 |
|
|
sd->is_open = 0; //It can be dangerous! Before freeing the buffer, we must be sure, that
|
936 |
|
|
//no our packet is being processed!
|
937 |
18 |
wzab |
printk (KERN_INFO "freed MAC\n");
|
938 |
|
|
free_mac(sd); //It also sets sd->active to 0!
|
939 |
15 |
wzab |
if (sd->buffer) {
|
940 |
18 |
wzab |
printk (KERN_INFO "freed buffer\n");
|
941 |
15 |
wzab |
vfree(sd->buffer);
|
942 |
|
|
sd->buffer = NULL;
|
943 |
|
|
}
|
944 |
|
|
}
|
945 |
|
|
}
|
946 |
|
|
return SUCCESS;
|
947 |
|
|
}
|
948 |
|
|
|
949 |
|
|
/* Memory mapping */
|
950 |
|
|
void my_proto1_vma_open (struct vm_area_struct * area)
|
951 |
|
|
{ }
|
952 |
|
|
|
953 |
|
|
void my_proto1_vma_close (struct vm_area_struct * area)
|
954 |
|
|
{ }
|
955 |
|
|
|
956 |
|
|
static struct vm_operations_struct my_proto1_vm_ops = {
|
957 |
|
|
my_proto1_vma_open,
|
958 |
|
|
my_proto1_vma_close,
|
959 |
|
|
};
|
960 |
|
|
|
961 |
|
|
/*
|
962 |
|
|
mmap method implementation
|
963 |
|
|
*/
|
964 |
|
|
int my_proto1_mmap(struct file *filp,
|
965 |
|
|
struct vm_area_struct *vma)
|
966 |
|
|
{
|
967 |
|
|
slave_data * sd = filp->private_data;
|
968 |
|
|
unsigned long vsize = vma->vm_end - vma->vm_start;
|
969 |
|
|
unsigned long psize = MY_BUF_LEN;
|
970 |
|
|
if (vsize>psize)
|
971 |
|
|
return -EINVAL;
|
972 |
|
|
remap_vmalloc_range(vma,sd->buffer, 0);
|
973 |
|
|
if (vma->vm_ops)
|
974 |
|
|
return -EINVAL; //It should never happen...
|
975 |
|
|
vma->vm_ops = &my_proto1_vm_ops;
|
976 |
|
|
my_proto1_vma_open(vma); //No open(vma) was called, we have called it ourselves
|
977 |
|
|
return 0;
|
978 |
|
|
}
|
979 |
|
|
|