1 /* can_queue.c - CAN message queues
2 * Linux CAN-bus device driver.
3 * New CAN queues by Pavel Pisa - OCERA team member
4 * email:pisa@cmp.felk.cvut.cz
5 * This software is released under the GPL-License.
6 * Version lincan-0.2 9 Jul 2003
9 #include "../include/can.h"
10 #include "../include/can_sysdep.h"
11 #include "../include/can_queue.h"
14 * Modifies Tx message processing
15 * 0 .. local message processing disabled
16 * 1 .. local messages disabled by default but can be enabled by canque_set_filt
17 * 2 .. local messages enabled by default, can be disabled by canque_set_filt
19 extern int processlocal;
21 atomic_t edge_num_cnt;
26 #define DEBUGQUE(fmt,args...) printk(KERN_ERR "can_queue (debug): " fmt,\
30 #define DEBUGQUE(fmt,args...)
33 #define CANQUE_ROUNDROB 1
37 * canque_fifo_flush_slots - free all ready slots from the FIFO
38 * @fifo: pointer to the FIFO structure
40 * The caller should be prepared to handle situations, when some
41 * slots are held by input or output side slots processing.
42 * These slots cannot be flushed or their processing interrupted.
44 * Return Value: The nonzero value indicates, that queue has not been
45 * empty before the function call.
47 int canque_fifo_flush_slots(struct canque_fifo_t *fifo)
51 struct canque_slot_t *slot;
52 spin_lock_irqsave(&fifo->fifo_lock, flags);
55 *fifo->tail=fifo->flist;
58 fifo->tail=&fifo->head;
60 canque_fifo_clear_fl(fifo,FULL);
61 ret=canque_fifo_test_and_set_fl(fifo,EMPTY)?0:1;
62 spin_unlock_irqrestore(&fifo->fifo_lock, flags);
68 * canque_fifo_init_slots - initialize one CAN FIFO
69 * @fifo: pointer to the FIFO structure
70 * @slotsnr: number of requested slots
72 * Return Value: The negative value indicates, that there is no memory
73 * to allocate space for the requested number of the slots.
75 int canque_fifo_init_slots(struct canque_fifo_t *fifo, int slotsnr)
78 struct canque_slot_t *slot;
79 if(!slotsnr) slotsnr=MAX_BUF_LENGTH;
80 size=sizeof(struct canque_slot_t)*slotsnr;
81 fifo->entry=kmalloc(size,GFP_KERNEL);
82 if(!fifo->entry) return -1;
91 fifo->tail=&fifo->head;
92 canque_fifo_set_fl(fifo,EMPTY);
97 * canque_fifo_done - frees slots allocated for CAN FIFO
98 * @fifo: pointer to the FIFO structure
100 int canque_fifo_done(struct canque_fifo_t *fifo)
108 /* atomic_dec_and_test(&qedge->edge_used);
109 void atomic_inc(&qedge->edge_used);
110 list_add_tail(struct list_head *new, struct list_head *head)
111 list_for_each(edge,qends->inlist);
112 list_entry(ptr, type, member)
116 * canque_get_inslot - finds one outgoing edge and allocates slot from it
117 * @qends: ends structure belonging to calling communication object
118 * @qedgep: place to store pointer to found edge
119 * @slotp: place to store pointer to allocated slot
120 * @cmd: command type for slot
122 * Function looks for the first non-blocked outgoing edge in @qends structure
123 * and tries to allocate slot from it.
124 * Return Value: If there is no usable edge or there is no free slot in edge
125 * negative value is returned.
127 int canque_get_inslot(struct canque_ends_t *qends,
128 struct canque_edge_t **qedgep, struct canque_slot_t **slotp, int cmd)
131 struct canque_edge_t *edge;
133 edge=canque_first_inedge(qends);
135 if(!canque_fifo_test_fl(&edge->fifo,BLOCK)){
136 ret=canque_fifo_get_inslot(&edge->fifo, slotp, cmd);
139 DEBUGQUE("canque_get_inslot cmd=%d found edge %d\n",cmd,edge->edge_num);
144 canque_edge_decref(edge);
147 DEBUGQUE("canque_get_inslot cmd=%d failed\n",cmd);
152 * canque_get_inslot4id - finds best outgoing edge and slot for given ID
153 * @qends: ends structure belonging to calling communication object
154 * @qedgep: place to store pointer to found edge
155 * @slotp: place to store pointer to allocated slot
156 * @cmd: command type for slot
157 * @id: communication ID of message to send into edge
158 * @prio: optional priority of message
160 * Function looks for the non-blocked outgoing edge accepting messages
161 * with given ID. If edge is found, slot is allocated from that edge.
162 * The edges with non-zero mask are preferred over edges open to all messages.
163 * If more edges with mask accepts given message ID, the edge with
164 * highest priority below or equal to required priority is selected.
165 * Return Value: If there is no usable edge or there is no free slot in edge
166 * negative value is returned.
168 int canque_get_inslot4id(struct canque_ends_t *qends,
169 struct canque_edge_t **qedgep, struct canque_slot_t **slotp,
170 int cmd, unsigned long id, int prio)
173 struct canque_edge_t *edge, *bestedge=NULL;
175 canque_for_each_inedge(qends, edge){
176 if(canque_fifo_test_fl(&edge->fifo,BLOCK))
178 if((id^edge->filtid)&edge->filtmask)
181 if(bestedge->filtmask){
182 if (!edge->filtmask) continue;
185 canque_edge_decref(bestedge);
187 canque_edge_incref(bestedge);
191 if(bestedge->edge_prio<edge->edge_prio){
192 if(edge->edge_prio>prio) continue;
194 if(bestedge->edge_prio<=prio) continue;
196 canque_edge_decref(bestedge);
199 canque_edge_incref(bestedge);
201 if((edge=bestedge)!=NULL){
202 ret=canque_fifo_get_inslot(&edge->fifo, slotp, cmd);
205 DEBUGQUE("canque_get_inslot4id cmd=%d id=%ld prio=%d found edge %d\n",cmd,id,prio,edge->edge_num);
208 canque_edge_decref(bestedge);
211 DEBUGQUE("canque_get_inslot4id cmd=%d id=%ld prio=%d failed\n",cmd,id,prio);
217 * canque_put_inslot - schedules filled slot for processing
218 * @qends: ends structure belonging to calling communication object
219 * @qedge: edge slot belong to
220 * @slot: pointer to the prepared slot
222 * Puts slot previously acquired by canque_get_inslot() or canque_get_inslot4id()
223 * function call into FIFO queue and activates edge processing if needed.
224 * Return Value: Positive value informs, that activation of output end
227 int canque_put_inslot(struct canque_ends_t *qends,
228 struct canque_edge_t *qedge, struct canque_slot_t *slot)
231 ret=canque_fifo_put_inslot(&qedge->fifo,slot);
233 canque_activate_edge(qends,qedge);
234 canque_notify_outends(qedge,CANQUEUE_NOTIFY_PROC);
236 canque_edge_decref(qedge);
237 DEBUGQUE("canque_put_inslot for edge %d returned %d\n",qedge->edge_num,ret);
242 * canque_abort_inslot - aborts preparation of the message in the slot
243 * @qends: ends structure belonging to calling communication object
244 * @qedge: edge slot belong to
245 * @slot: pointer to the previously allocated slot
247 * Frees slot previously acquired by canque_get_inslot() or canque_get_inslot4id()
248 * function call. Used when message copying into slot fails.
249 * Return Value: Positive value informs, that queue full state has been negated.
251 int canque_abort_inslot(struct canque_ends_t *qends,
252 struct canque_edge_t *qedge, struct canque_slot_t *slot)
255 ret=canque_fifo_abort_inslot(&qedge->fifo,slot);
257 canque_notify_outends(qedge,CANQUEUE_NOTIFY_SPACE);
259 canque_edge_decref(qedge);
260 DEBUGQUE("canque_abort_inslot for edge %d returned %d\n",qedge->edge_num,ret);
265 * canque_filter_msg2edges - sends message into all edges which accept its ID
266 * @qends: ends structure belonging to calling communication object
267 * @msg: pointer to CAN message
269 * Sends message to all outgoing edges connected to the given ends, which accepts
270 * message communication ID.
271 * Return Value: Returns number of edges message has been send to
273 int canque_filter_msg2edges(struct canque_ends_t *qends, struct canmsg_t *msg)
278 struct canque_edge_t *edge;
279 struct canque_slot_t *slot;
281 DEBUGQUE("canque_filter_msg2edges for msg ID 0x%08lx and flags 0x%02x\n",
282 msg->id, msg->flags);
283 msgid = canque_filtid2internal(msg->id, msg->flags);
285 canque_for_each_inedge(qends, edge) {
286 if(canque_fifo_test_fl(&edge->fifo,BLOCK))
288 if((msgid^edge->filtid)&edge->filtmask)
290 ret=canque_fifo_get_inslot(&edge->fifo, &slot, 0);
294 ret=canque_fifo_put_inslot(&edge->fifo,slot);
296 canque_activate_edge(qends,edge);
297 canque_notify_outends(edge,CANQUEUE_NOTIFY_PROC);
302 DEBUGQUE("canque_filter_msg2edges sent msg ID %ld to %d edges\n",msg->id,destnr);
307 * canque_test_outslot - test and retrieve ready slot for given ends
308 * @qends: ends structure belonging to calling communication object
309 * @qedgep: place to store pointer to found edge
310 * @slotp: place to store pointer to received slot
312 * Function takes highest priority active incoming edge and retrieves
313 * oldest ready slot from it.
314 * Return Value: Negative value informs, that there is no ready output
315 * slot for given ends. Positive value is equal to the command
316 * slot has been allocated by the input side.
318 int canque_test_outslot(struct canque_ends_t *qends,
319 struct canque_edge_t **qedgep, struct canque_slot_t **slotp)
323 struct canque_edge_t *edge;
326 spin_lock_irqsave(&qends->ends_lock, flags);
327 for(prio=CANQUEUE_PRIO_NR;--prio>=0;){
328 while(!list_empty(&qends->active[prio])){
329 edge=list_entry(qends->active[prio].next,struct canque_edge_t,outpeers);
330 if(!canque_fifo_test_fl(&edge->fifo,DEAD)) {
331 canque_edge_incref(edge);
332 spin_unlock_irqrestore(&qends->ends_lock, flags);
334 DEBUGQUE("canque_test_outslot found edge %d\n",edge->edge_num);
335 ret=canque_fifo_test_outslot(&edge->fifo, slotp);
338 spin_lock_irqsave(&qends->ends_lock, flags);
340 spin_lock(&edge->fifo.fifo_lock);
341 if(canque_fifo_test_and_set_fl(&edge->fifo,INACTIVE)) {
342 list_del(&edge->outpeers);
343 list_add(&edge->outpeers,&qends->idle);
345 spin_unlock(&edge->fifo.fifo_lock);
348 spin_unlock_irqrestore(&qends->ends_lock, flags);
350 DEBUGQUE("canque_test_outslot no ready slot\n");
355 * canque_free_outslot - frees processed output slot
356 * @qends: ends structure belonging to calling communication object
357 * @qedge: edge slot belong to
358 * @slot: pointer to the processed slot
360 * Function releases processed slot previously acquired by canque_test_outslot()
362 * Return Value: Return value informs if input side has been notified
363 * to know about change of edge state
365 int canque_free_outslot(struct canque_ends_t *qends,
366 struct canque_edge_t *qedge, struct canque_slot_t *slot)
370 ret=canque_fifo_free_outslot(&qedge->fifo, slot);
371 if(ret&CAN_FIFOF_EMPTY){
372 canque_notify_inends(qedge,CANQUEUE_NOTIFY_EMPTY);
374 if(ret&CAN_FIFOF_FULL)
375 canque_notify_inends(qedge,CANQUEUE_NOTIFY_SPACE);
376 spin_lock_irqsave(&qends->ends_lock, flags);
377 if((ret&CAN_FIFOF_EMPTY) || CANQUE_ROUNDROB ){
378 spin_lock(&qedge->fifo.fifo_lock);
379 if(canque_fifo_test_fl(&qedge->fifo,EMPTY)){
380 canque_fifo_set_fl(&qedge->fifo,INACTIVE);
381 list_del(&qedge->outpeers);
382 list_add(&qedge->outpeers,&qends->idle);
384 list_del(&qedge->outpeers);
385 list_add_tail(&qedge->outpeers,&qends->active[qedge->edge_prio]);
387 spin_unlock(&qedge->fifo.fifo_lock);
389 spin_unlock_irqrestore(&qends->ends_lock, flags);
390 canque_edge_decref(qedge);
391 DEBUGQUE("canque_free_outslot for edge %d returned %d\n",qedge->edge_num,ret);
396 * canque_again_outslot - reschedule output slot to process it again later
397 * @qends: ends structure belonging to calling communication object
398 * @qedge: edge slot belong to
399 * @slot: pointer to the slot for re-processing
401 * Function reschedules slot previously acquired by canque_test_outslot()
402 * function call for second time processing.
403 * Return Value: Function cannot fail.
405 int canque_again_outslot(struct canque_ends_t *qends,
406 struct canque_edge_t *qedge, struct canque_slot_t *slot)
409 ret=canque_fifo_again_outslot(&qedge->fifo, slot);
410 canque_edge_decref(qedge);
411 DEBUGQUE("canque_again_outslot for edge %d returned %d\n",qedge->edge_num,ret);
416 * canque_set_filt - sets filter for specified edge
417 * @qedge: pointer to the edge
418 * @filtid: ID to set for the edge
419 * @filtmask: mask used for ID match check
420 * @filtflags: required filer flags
422 * Return Value: Negative value is returned if edge is in the process of delete.
424 int canque_set_filt(struct canque_edge_t *qedge,
425 unsigned long filtid, unsigned long filtmask, int filtflags)
430 spin_lock_irqsave(&qedge->fifo.fifo_lock,flags);
432 if(!(filtflags&MSG_PROCESSLOCAL) && (processlocal<2))
433 filtflags |= MSG_LOCAL_MASK;
435 qedge->filtid=canque_filtid2internal(filtid, filtflags);
436 qedge->filtmask=canque_filtid2internal(filtmask, filtflags>>MSG_FILT_MASK_SHIFT);
438 if(canque_fifo_test_fl(&qedge->fifo,DEAD)) ret=-1;
439 else ret=canque_fifo_test_and_set_fl(&qedge->fifo,BLOCK)?1:0;
441 spin_unlock_irqrestore(&qedge->fifo.fifo_lock,flags);
443 canque_notify_bothends(qedge,CANQUEUE_NOTIFY_FILTCH);
445 spin_lock_irqsave(&qedge->fifo.fifo_lock,flags);
446 if(!ret) canque_fifo_clear_fl(&qedge->fifo,BLOCK);
447 spin_unlock_irqrestore(&qedge->fifo.fifo_lock,flags);
449 DEBUGQUE("canque_set_filt for edge %d, ID %ld, mask %ld, flags %d returned %d\n",
450 qedge->edge_num,filtid,filtmask,filtflags,ret);
455 * canque_flush - fluesh all ready slots in the edge
456 * @qedge: pointer to the edge
458 * Tries to flush all allocated slots from the edge, but there could
459 * exist some slots associated to edge which are processed by input
460 * or output side and cannot be flushed at this moment.
461 * Return Value: The nonzero value indicates, that queue has not been
462 * empty before the function call.
464 int canque_flush(struct canque_edge_t *qedge)
469 ret=canque_fifo_flush_slots(&qedge->fifo);
471 canque_notify_inends(qedge,CANQUEUE_NOTIFY_EMPTY);
472 canque_notify_inends(qedge,CANQUEUE_NOTIFY_SPACE);
473 spin_lock_irqsave(&qedge->outends->ends_lock, flags);
474 spin_lock(&qedge->fifo.fifo_lock);
475 if(canque_fifo_test_fl(&qedge->fifo,EMPTY)){
476 list_del(&qedge->outpeers);
477 list_add(&qedge->outpeers,&qedge->outends->idle);
479 spin_unlock(&qedge->fifo.fifo_lock);
480 spin_unlock_irqrestore(&qedge->outends->ends_lock, flags);
482 DEBUGQUE("canque_flush for edge %d returned %d\n",qedge->edge_num,ret);
487 * canqueue_ends_init_gen - subsystem independent routine to initialize ends state
488 * @qends: pointer to the ends structure
490 * Return Value: Cannot fail.
492 int canqueue_ends_init_gen(struct canque_ends_t *qends)
495 for(i=CANQUEUE_PRIO_NR;--i>=0;){
496 INIT_LIST_HEAD(&qends->active[i]);
498 INIT_LIST_HEAD(&qends->idle);
499 INIT_LIST_HEAD(&qends->inlist);
500 spin_lock_init(&qends->ends_lock);
506 * canqueue_connect_edge - connect edge between two communication entities
507 * @qedge: pointer to edge
508 * @inends: pointer to ends the input of the edge should be connected to
509 * @outends: pointer to ends the output of the edge should be connected to
511 * Return Value: Negative value informs about failed operation.
513 int canqueue_connect_edge(struct canque_edge_t *qedge, struct canque_ends_t *inends, struct canque_ends_t *outends)
516 if(qedge == NULL) return -1;
517 DEBUGQUE("canqueue_connect_edge %d\n",qedge->edge_num);
518 canque_edge_incref(qedge);
519 spin_lock_irqsave(&inends->ends_lock, flags);
520 spin_lock(&outends->ends_lock);
521 spin_lock(&qedge->fifo.fifo_lock);
522 qedge->inends=inends;
523 list_add(&qedge->inpeers,&inends->inlist);
524 qedge->outends=outends;
525 list_add(&qedge->outpeers,&outends->idle);
526 spin_unlock(&qedge->fifo.fifo_lock);
527 spin_unlock(&outends->ends_lock);
528 spin_unlock_irqrestore(&inends->ends_lock, flags);
529 canque_notify_bothends(qedge, CANQUEUE_NOTIFY_ATTACH);
531 if(canque_fifo_test_and_set_fl(&qedge->fifo, READY))
532 canque_edge_decref(qedge);
537 * canqueue_disconnect_edge - disconnect edge from communicating entities
538 * @qedge: pointer to edge
540 * Return Value: Negative value means, that edge is used by somebody
541 * other and cannot be disconnected. Operation has to be delayed.
543 int canqueue_disconnect_edge(struct canque_edge_t *qedge)
547 struct canque_ends_t *inends, *outends;
549 inends=qedge->inends;
550 if(inends) spin_lock_irqsave(&inends->ends_lock,flags);
551 outends=qedge->outends;
552 if(outends) spin_lock(&outends->ends_lock);
553 spin_lock(&qedge->fifo.fifo_lock);
554 if(atomic_read(&qedge->edge_used)==0) {
556 list_del(&qedge->outpeers);
560 list_del(&qedge->inpeers);
565 spin_unlock(&qedge->fifo.fifo_lock);
566 if(outends) spin_unlock(&outends->ends_lock);
567 if(inends) spin_unlock_irqrestore(&inends->ends_lock,flags);
568 DEBUGQUE("canqueue_disconnect_edge %d returned %d\n",qedge->edge_num,ret);