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.3 17 Jun 2004
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;
27 #define DEBUGQUE(fmt,args...) can_printk(KERN_ERR "can_queue (debug): " fmt,\
31 #define DEBUGQUE(fmt,args...)
34 #define CANQUE_ROUNDROB 1
38 * canque_fifo_flush_slots - free all ready slots from the FIFO
39 * @fifo: pointer to the FIFO structure
41 * The caller should be prepared to handle situations, when some
42 * slots are held by input or output side slots processing.
43 * These slots cannot be flushed or their processing interrupted.
45 * Return Value: The nonzero value indicates, that queue has not been
46 * empty before the function call.
48 int canque_fifo_flush_slots(struct canque_fifo_t *fifo)
51 can_spin_irqflags_t flags;
52 struct canque_slot_t *slot;
53 can_spin_lock_irqsave(&fifo->fifo_lock, flags);
56 *fifo->tail=fifo->flist;
59 fifo->tail=&fifo->head;
61 canque_fifo_clear_fl(fifo,FULL);
62 ret=canque_fifo_test_and_set_fl(fifo,EMPTY)?0:1;
63 can_spin_unlock_irqrestore(&fifo->fifo_lock, flags);
69 * canque_fifo_init_slots - initializes slot chain of one CAN FIFO
70 * @fifo: pointer to the FIFO structure
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)
77 struct canque_slot_t *slot;
78 int slotsnr=fifo->slotsnr;
79 if(!fifo->entry || !slotsnr) return -1;
88 fifo->tail=&fifo->head;
89 canque_fifo_set_fl(fifo,EMPTY);
93 /* atomic_dec_and_test(&qedge->edge_used);
94 void atomic_inc(&qedge->edge_used);
95 list_add_tail(struct list_head *new, struct list_head *head)
96 list_for_each(edge,qends->inlist);
97 list_entry(ptr, type, member)
100 void __canque_edge_decref(struct canque_edge_t *edge)
102 __canque_edge_decref_body(edge);
106 * canque_get_inslot - finds one outgoing edge and allocates slot from it
107 * @qends: ends structure belonging to calling communication object
108 * @qedgep: place to store pointer to found edge
109 * @slotp: place to store pointer to allocated slot
110 * @cmd: command type for slot
112 * Function looks for the first non-blocked outgoing edge in @qends structure
113 * and tries to allocate slot from it.
114 * Return Value: If there is no usable edge or there is no free slot in edge
115 * negative value is returned.
117 int canque_get_inslot(struct canque_ends_t *qends,
118 struct canque_edge_t **qedgep, struct canque_slot_t **slotp, int cmd)
121 struct canque_edge_t *edge;
123 edge=canque_first_inedge(qends);
125 if(!canque_fifo_test_fl(&edge->fifo,BLOCK)){
126 ret=canque_fifo_get_inslot(&edge->fifo, slotp, cmd);
129 DEBUGQUE("canque_get_inslot cmd=%d found edge %d\n",cmd,edge->edge_num);
134 canque_edge_decref(edge);
137 DEBUGQUE("canque_get_inslot cmd=%d failed\n",cmd);
142 * canque_get_inslot4id - finds best outgoing edge and slot for given ID
143 * @qends: ends structure belonging to calling communication object
144 * @qedgep: place to store pointer to found edge
145 * @slotp: place to store pointer to allocated slot
146 * @cmd: command type for slot
147 * @id: communication ID of message to send into edge
148 * @prio: optional priority of message
150 * Function looks for the non-blocked outgoing edge accepting messages
151 * with given ID. If edge is found, slot is allocated from that edge.
152 * The edges with non-zero mask are preferred over edges open to all messages.
153 * If more edges with mask accepts given message ID, the edge with
154 * highest priority below or equal to required priority is selected.
155 * Return Value: If there is no usable edge or there is no free slot in edge
156 * negative value is returned.
158 int canque_get_inslot4id(struct canque_ends_t *qends,
159 struct canque_edge_t **qedgep, struct canque_slot_t **slotp,
160 int cmd, unsigned long id, int prio)
163 struct canque_edge_t *edge, *bestedge=NULL;
165 canque_for_each_inedge(qends, edge){
166 if(canque_fifo_test_fl(&edge->fifo,BLOCK))
168 if((id^edge->filtid)&edge->filtmask)
171 if(bestedge->filtmask){
172 if (!edge->filtmask) continue;
175 canque_edge_decref(bestedge);
177 canque_edge_incref(bestedge);
181 if(bestedge->edge_prio<edge->edge_prio){
182 if(edge->edge_prio>prio) continue;
184 if(bestedge->edge_prio<=prio) continue;
186 canque_edge_decref(bestedge);
189 canque_edge_incref(bestedge);
191 if((edge=bestedge)!=NULL){
192 ret=canque_fifo_get_inslot(&edge->fifo, slotp, cmd);
195 DEBUGQUE("canque_get_inslot4id cmd=%d id=%ld prio=%d found edge %d\n",cmd,id,prio,edge->edge_num);
198 canque_edge_decref(bestedge);
201 DEBUGQUE("canque_get_inslot4id cmd=%d id=%ld prio=%d failed\n",cmd,id,prio);
207 * canque_put_inslot - schedules filled slot for processing
208 * @qends: ends structure belonging to calling communication object
209 * @qedge: edge slot belong to
210 * @slot: pointer to the prepared slot
212 * Puts slot previously acquired by canque_get_inslot() or canque_get_inslot4id()
213 * function call into FIFO queue and activates edge processing if needed.
214 * Return Value: Positive value informs, that activation of output end
217 int canque_put_inslot(struct canque_ends_t *qends,
218 struct canque_edge_t *qedge, struct canque_slot_t *slot)
221 ret=canque_fifo_put_inslot(&qedge->fifo,slot);
223 canque_activate_edge(qends,qedge);
224 canque_notify_outends(qedge,CANQUEUE_NOTIFY_PROC);
226 canque_edge_decref(qedge);
227 DEBUGQUE("canque_put_inslot for edge %d returned %d\n",qedge->edge_num,ret);
232 * canque_abort_inslot - aborts preparation of the message in the slot
233 * @qends: ends structure belonging to calling communication object
234 * @qedge: edge slot belong to
235 * @slot: pointer to the previously allocated slot
237 * Frees slot previously acquired by canque_get_inslot() or canque_get_inslot4id()
238 * function call. Used when message copying into slot fails.
239 * Return Value: Positive value informs, that queue full state has been negated.
241 int canque_abort_inslot(struct canque_ends_t *qends,
242 struct canque_edge_t *qedge, struct canque_slot_t *slot)
245 ret=canque_fifo_abort_inslot(&qedge->fifo,slot);
247 canque_notify_outends(qedge,CANQUEUE_NOTIFY_SPACE);
249 canque_edge_decref(qedge);
250 DEBUGQUE("canque_abort_inslot for edge %d returned %d\n",qedge->edge_num,ret);
255 * canque_filter_msg2edges - sends message into all edges which accept its ID
256 * @qends: ends structure belonging to calling communication object
257 * @msg: pointer to CAN message
259 * Sends message to all outgoing edges connected to the given ends, which accepts
260 * message communication ID.
261 * Return Value: Returns number of edges message has been send to
263 int canque_filter_msg2edges(struct canque_ends_t *qends, struct canmsg_t *msg)
268 struct canque_edge_t *edge;
269 struct canque_slot_t *slot;
271 DEBUGQUE("canque_filter_msg2edges for msg ID 0x%08lx and flags 0x%02x\n",
272 msg->id, msg->flags);
273 msgid = canque_filtid2internal(msg->id, msg->flags);
275 canque_for_each_inedge(qends, edge) {
276 if(canque_fifo_test_fl(&edge->fifo,BLOCK))
278 if((msgid^edge->filtid)&edge->filtmask)
280 ret=canque_fifo_get_inslot(&edge->fifo, &slot, 0);
284 ret=canque_fifo_put_inslot(&edge->fifo,slot);
286 canque_activate_edge(qends,edge);
287 canque_notify_outends(edge,CANQUEUE_NOTIFY_PROC);
292 DEBUGQUE("canque_filter_msg2edges sent msg ID %ld to %d edges\n",msg->id,destnr);
297 * canque_test_outslot - test and retrieve ready slot for given ends
298 * @qends: ends structure belonging to calling communication object
299 * @qedgep: place to store pointer to found edge
300 * @slotp: place to store pointer to received slot
302 * Function takes highest priority active incoming edge and retrieves
303 * oldest ready slot from it.
304 * Return Value: Negative value informs, that there is no ready output
305 * slot for given ends. Positive value is equal to the command
306 * slot has been allocated by the input side.
308 int canque_test_outslot(struct canque_ends_t *qends,
309 struct canque_edge_t **qedgep, struct canque_slot_t **slotp)
311 can_spin_irqflags_t flags;
313 struct canque_edge_t *edge;
316 can_spin_lock_irqsave(&qends->ends_lock, flags);
317 for(prio=CANQUEUE_PRIO_NR;--prio>=0;){
318 while(!list_empty(&qends->active[prio])){
319 edge=list_entry(qends->active[prio].next,struct canque_edge_t,activepeers);
320 if(!canque_fifo_test_fl(&edge->fifo,DEAD)) {
321 /* The first test on unlocked FIFO */
322 if(canque_fifo_test_fl(&edge->fifo,EMPTY)) {
323 can_spin_lock(&edge->fifo.fifo_lock);
324 /* Test has to be repeated to ensure that EMPTY
325 state has not been nagated when locking FIFO */
326 if(canque_fifo_test_fl(&edge->fifo,EMPTY)) {
327 canque_fifo_set_fl(&edge->fifo,INACTIVE);
328 list_del(&edge->activepeers);
329 list_add(&edge->activepeers,&qends->idle);
330 can_spin_unlock(&edge->fifo.fifo_lock);
333 can_spin_unlock(&edge->fifo.fifo_lock);
335 canque_edge_incref(edge);
336 can_spin_unlock_irqrestore(&qends->ends_lock, flags);
338 DEBUGQUE("canque_test_outslot found edge %d\n",edge->edge_num);
339 ret=canque_fifo_test_outslot(&edge->fifo, slotp);
343 canque_edge_decref(edge);
344 can_spin_lock_irqsave(&qends->ends_lock, flags);
346 can_spin_lock(&edge->fifo.fifo_lock);
347 canque_fifo_set_fl(&edge->fifo,INACTIVE);
348 list_del(&edge->activepeers);
349 list_add(&edge->activepeers,&qends->idle);
350 can_spin_unlock(&edge->fifo.fifo_lock);
354 can_spin_unlock_irqrestore(&qends->ends_lock, flags);
356 DEBUGQUE("canque_test_outslot no ready slot\n");
361 * canque_free_outslot - frees processed output slot
362 * @qends: ends structure belonging to calling communication object
363 * @qedge: edge slot belong to
364 * @slot: pointer to the processed slot
366 * Function releases processed slot previously acquired by canque_test_outslot()
368 * Return Value: Return value informs if input side has been notified
369 * to know about change of edge state
371 int canque_free_outslot(struct canque_ends_t *qends,
372 struct canque_edge_t *qedge, struct canque_slot_t *slot)
375 can_spin_irqflags_t flags;
376 ret=canque_fifo_free_outslot(&qedge->fifo, slot);
377 if(ret&CAN_FIFOF_EMPTY){
378 canque_notify_inends(qedge,CANQUEUE_NOTIFY_EMPTY);
380 if(ret&CAN_FIFOF_FULL)
381 canque_notify_inends(qedge,CANQUEUE_NOTIFY_SPACE);
382 can_spin_lock_irqsave(&qends->ends_lock, flags);
383 if((ret&CAN_FIFOF_EMPTY) || CANQUE_ROUNDROB ){
384 can_spin_lock(&qedge->fifo.fifo_lock);
385 if(canque_fifo_test_fl(&qedge->fifo,EMPTY)){
386 canque_fifo_set_fl(&qedge->fifo,INACTIVE);
387 list_del(&qedge->activepeers);
388 list_add(&qedge->activepeers,&qends->idle);
390 list_del(&qedge->activepeers);
391 list_add_tail(&qedge->activepeers,&qends->active[qedge->edge_prio]);
393 can_spin_unlock(&qedge->fifo.fifo_lock);
395 can_spin_unlock_irqrestore(&qends->ends_lock, flags);
396 canque_edge_decref(qedge);
397 DEBUGQUE("canque_free_outslot for edge %d returned %d\n",qedge->edge_num,ret);
402 * canque_again_outslot - reschedule output slot to process it again later
403 * @qends: ends structure belonging to calling communication object
404 * @qedge: edge slot belong to
405 * @slot: pointer to the slot for re-processing
407 * Function reschedules slot previously acquired by canque_test_outslot()
408 * function call for second time processing.
409 * Return Value: Function cannot fail.
411 int canque_again_outslot(struct canque_ends_t *qends,
412 struct canque_edge_t *qedge, struct canque_slot_t *slot)
415 ret=canque_fifo_again_outslot(&qedge->fifo, slot);
416 canque_edge_decref(qedge);
417 DEBUGQUE("canque_again_outslot for edge %d returned %d\n",qedge->edge_num,ret);
422 * canque_set_filt - sets filter for specified edge
423 * @qedge: pointer to the edge
424 * @filtid: ID to set for the edge
425 * @filtmask: mask used for ID match check
426 * @filtflags: required filer flags
428 * Return Value: Negative value is returned if edge is in the process of delete.
430 int canque_set_filt(struct canque_edge_t *qedge,
431 unsigned long filtid, unsigned long filtmask, int filtflags)
434 can_spin_irqflags_t flags;
436 can_spin_lock_irqsave(&qedge->fifo.fifo_lock,flags);
438 if(!(filtflags&MSG_PROCESSLOCAL) && (processlocal<2))
439 filtflags |= MSG_LOCAL_MASK;
441 qedge->filtid=canque_filtid2internal(filtid, filtflags);
442 qedge->filtmask=canque_filtid2internal(filtmask, filtflags>>MSG_FILT_MASK_SHIFT);
444 if(canque_fifo_test_fl(&qedge->fifo,DEAD)) ret=-1;
445 else ret=canque_fifo_test_and_set_fl(&qedge->fifo,BLOCK)?1:0;
447 can_spin_unlock_irqrestore(&qedge->fifo.fifo_lock,flags);
449 canque_notify_bothends(qedge,CANQUEUE_NOTIFY_FILTCH);
451 can_spin_lock_irqsave(&qedge->fifo.fifo_lock,flags);
452 if(!ret) canque_fifo_clear_fl(&qedge->fifo,BLOCK);
453 can_spin_unlock_irqrestore(&qedge->fifo.fifo_lock,flags);
455 DEBUGQUE("canque_set_filt for edge %d, ID %ld, mask %ld, flags %d returned %d\n",
456 qedge->edge_num,filtid,filtmask,filtflags,ret);
461 * canque_flush - fluesh all ready slots in the edge
462 * @qedge: pointer to the edge
464 * Tries to flush all allocated slots from the edge, but there could
465 * exist some slots associated to edge which are processed by input
466 * or output side and cannot be flushed at this moment.
467 * Return Value: The nonzero value indicates, that queue has not been
468 * empty before the function call.
470 int canque_flush(struct canque_edge_t *qedge)
473 can_spin_irqflags_t flags;
475 ret=canque_fifo_flush_slots(&qedge->fifo);
477 canque_notify_inends(qedge,CANQUEUE_NOTIFY_EMPTY);
478 canque_notify_inends(qedge,CANQUEUE_NOTIFY_SPACE);
479 can_spin_lock_irqsave(&qedge->outends->ends_lock, flags);
480 can_spin_lock(&qedge->fifo.fifo_lock);
481 if(canque_fifo_test_fl(&qedge->fifo,EMPTY)){
482 list_del(&qedge->activepeers);
483 list_add(&qedge->activepeers,&qedge->outends->idle);
485 can_spin_unlock(&qedge->fifo.fifo_lock);
486 can_spin_unlock_irqrestore(&qedge->outends->ends_lock, flags);
488 DEBUGQUE("canque_flush for edge %d returned %d\n",qedge->edge_num,ret);
493 * canqueue_ends_init_gen - subsystem independent routine to initialize ends state
494 * @qends: pointer to the ends structure
496 * Return Value: Cannot fail.
498 int canqueue_ends_init_gen(struct canque_ends_t *qends)
502 for(i=CANQUEUE_PRIO_NR;--i>=0;){
503 INIT_LIST_HEAD(&qends->active[i]);
505 INIT_LIST_HEAD(&qends->idle);
506 INIT_LIST_HEAD(&qends->inlist);
507 INIT_LIST_HEAD(&qends->outlist);
508 can_spin_lock_init(&qends->ends_lock);
514 * canqueue_connect_edge - connect edge between two communication entities
515 * @qedge: pointer to edge
516 * @inends: pointer to ends the input of the edge should be connected to
517 * @outends: pointer to ends the output of the edge should be connected to
519 * Return Value: Negative value informs about failed operation.
521 int canqueue_connect_edge(struct canque_edge_t *qedge, struct canque_ends_t *inends, struct canque_ends_t *outends)
523 can_spin_irqflags_t flags;
524 if(qedge == NULL) return -1;
525 DEBUGQUE("canqueue_connect_edge %d\n",qedge->edge_num);
526 canque_edge_incref(qedge);
527 flags=canque_edge_lock_both_ends(inends, outends);
528 can_spin_lock(&qedge->fifo.fifo_lock);
529 qedge->inends=inends;
530 list_add(&qedge->inpeers,&inends->inlist);
531 qedge->outends=outends;
532 list_add(&qedge->outpeers,&outends->outlist);
533 list_add(&qedge->activepeers,&outends->idle);
534 can_spin_unlock(&qedge->fifo.fifo_lock);
535 canque_edge_unlock_both_ends(inends, outends, flags);
536 canque_notify_bothends(qedge, CANQUEUE_NOTIFY_ATTACH);
538 if(canque_fifo_test_and_set_fl(&qedge->fifo, READY))
539 canque_edge_decref(qedge);
544 * canqueue_disconnect_edge - disconnect edge from communicating entities
545 * @qedge: pointer to edge
547 * Return Value: Negative value means, that edge is used by somebody
548 * other and cannot be disconnected. Operation has to be delayed.
550 int canqueue_disconnect_edge(struct canque_edge_t *qedge)
553 can_spin_irqflags_t flags;
554 struct canque_ends_t *inends, *outends;
556 inends=qedge->inends;
557 outends=qedge->outends;
559 if(inends && outends) {
560 flags=canque_edge_lock_both_ends(inends, outends);
562 DEBUGQUE("canqueue_disconnect_edge called with not fully connected edge");
563 if(inends) can_spin_lock_irqsave(&inends->ends_lock,flags);
564 if(outends) can_spin_lock(&outends->ends_lock);
568 can_spin_lock(&qedge->fifo.fifo_lock);
569 if(atomic_read(&qedge->edge_used)==0) {
571 list_del(&qedge->activepeers);
572 mb(); /* memory barrier for list_empty use in canque_dead_func */
573 list_del(&qedge->outpeers);
577 list_del(&qedge->inpeers);
582 can_spin_unlock(&qedge->fifo.fifo_lock);
584 if(inends && outends) {
585 canque_edge_unlock_both_ends(inends, outends, flags);
587 if(outends) can_spin_unlock(&outends->ends_lock);
588 if(inends) can_spin_unlock_irqrestore(&inends->ends_lock,flags);
591 DEBUGQUE("canqueue_disconnect_edge %d returned %d\n",qedge->edge_num,ret);
597 * canqueue_block_inlist - block slot allocation of all outgoing edges of specified ends
598 * @qends: pointer to ends structure
600 void canqueue_block_inlist(struct canque_ends_t *qends)
602 struct canque_edge_t *edge;
604 canque_for_each_inedge(qends, edge) {
605 canque_fifo_set_fl(&edge->fifo,BLOCK);
611 * canqueue_block_outlist - block slot allocation of all incoming edges of specified ends
612 * @qends: pointer to ends structure
614 void canqueue_block_outlist(struct canque_ends_t *qends)
616 struct canque_edge_t *edge;
618 canque_for_each_outedge(qends, edge) {
619 canque_fifo_set_fl(&edge->fifo,BLOCK);
625 * canqueue_ends_kill_inlist - sends request to die to all outgoing edges
626 * @qends: pointer to ends structure
627 * @send_rest: select, whether already allocated slots should be processed
628 * by FIFO output side
630 * Return Value: Non-zero value means, that not all edges could be immediately
631 * disconnected and that ends structure memory release has to be delayed
633 int canqueue_ends_kill_inlist(struct canque_ends_t *qends, int send_rest)
635 struct canque_edge_t *edge;
637 canque_for_each_inedge(qends, edge){
638 canque_notify_bothends(edge, CANQUEUE_NOTIFY_DEAD_WANTED);
640 canque_edge_incref(edge);
641 if(!canque_fifo_test_and_set_fl(&edge->fifo, FREEONEMPTY)){
642 if(!canque_fifo_test_fl(&edge->fifo, EMPTY))
644 if(!canque_fifo_test_and_clear_fl(&edge->fifo, FREEONEMPTY))
647 canque_edge_decref(edge);
650 return list_empty(&qends->inlist)?0:1;
655 * canqueue_ends_kill_outlist - sends request to die to all incoming edges
656 * @qends: pointer to ends structure
658 * Return Value: Non-zero value means, that not all edges could be immediately
659 * disconnected and that ends structure memory release has to be delayed
661 int canqueue_ends_kill_outlist(struct canque_ends_t *qends)
663 struct canque_edge_t *edge;
665 canque_for_each_outedge(qends, edge){
666 canque_notify_bothends(edge, CANQUEUE_NOTIFY_DEAD_WANTED);
668 return list_empty(&qends->outlist)?0:1;
673 * canqueue_ends_filt_conjuction - computes conjunction of incoming edges filters filters
674 * @qends: pointer to ends structure
675 * @filt: pointer the filter structure filled by computed filters conjunction
677 * Return Value: Number of incoming edges
679 int canqueue_ends_filt_conjuction(struct canque_ends_t *qends, struct canfilt_t *filt)
681 struct canque_edge_t *edge;
683 unsigned long filtid=0;
684 unsigned long filtmask=~0;
685 unsigned long local_only=canque_filtid2internal(0,MSG_LOCAL);
687 canque_for_each_inedge(qends, edge){
688 /* skip edges processing only local messages */
689 if(edge->filtid & edge->filtmask & local_only)
693 filtid = edge->filtid;
695 filtmask &= ~(filtid ^ edge->filtid);
697 filtmask &= edge->filtmask;
700 filt->id = filtid & MSG_ID_MASK;
701 filt->mask = filtmask & MSG_ID_MASK;
704 filt->flags = filtid & MSG_EXT;
705 if(filtmask & (MSG_EXT))
706 filt->flags |= MSG_EXT_MASK;
707 if(filtid & (MSG_RTR<<1))
708 filt->flags |= MSG_RTR<<1;
709 if(filtmask & (MSG_RTR<<1))
710 filt->flags |= MSG_RTR_MASK;
716 * canqueue_ends_flush_inlist - flushes all messages in incoming edges
717 * @qends: pointer to ends structure
719 * Return Value: Negative value informs about unsuccessful result
721 int canqueue_ends_flush_inlist(struct canque_ends_t *qends)
723 struct canque_edge_t *edge;
725 canque_for_each_inedge(qends, edge){
733 * canqueue_ends_flush_outlist - flushes all messages in outgoing edges
734 * @qends: pointer to ends structure
736 * Return Value: Negative value informs about unsuccessful result
738 int canqueue_ends_flush_outlist(struct canque_ends_t *qends)
740 struct canque_edge_t *edge;
742 canque_for_each_outedge(qends, edge){