embedded/app/usbcan/can_queue.c

   1 /**************************************************************************/
   2 /* File: can_queue.c - CAN message queues                                 */
   3 /*                                                                        */
   4 /* LinCAN - (Not only) Linux CAN bus driver                               */
   5 /* Copyright (C) 2002-2009 DCE FEE CTU Prague <http://dce.felk.cvut.cz>   */
   6 /* Copyright (C) 2002-2009 Pavel Pisa <pisa@cmp.felk.cvut.cz>             */
   7 /* Funded by OCERA and FRESCOR IST projects                               */
   8 /*                                                                        */
   9 /* LinCAN is free software; you can redistribute it and/or modify it      */
  10 /* under terms of the GNU General Public License as published by the      */
  11 /* Free Software Foundation; either version 2, or (at your option) any    */
  12 /* later version.  LinCAN is distributed in the hope that it will be      */
  13 /* useful, but WITHOUT ANY WARRANTY; without even the implied warranty    */
  14 /* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU    */
  15 /* General Public License for more details. You should have received a    */
  16 /* copy of the GNU General Public License along with LinCAN; see file     */
  17 /* COPYING. If not, write to the Free Software Foundation, 675 Mass Ave,  */
  18 /* Cambridge, MA 02139, USA.                                              */
  19 /*                                                                        */
  20 /* To allow use of LinCAN in the compact embedded systems firmware        */
  21 /* and RT-executives (RTEMS for example), main authors agree with next    */
  22 /* special exception:                                                     */
  23 /*                                                                        */
  24 /* Including LinCAN header files in a file, instantiating LinCAN generics */
  25 /* or templates, or linking other files with LinCAN objects to produce    */
  26 /* an application image/executable, does not by itself cause the          */
  27 /* resulting application image/executable to be covered by                */
  28 /* the GNU General Public License.                                        */
  29 /* This exception does not however invalidate any other reasons           */
  30 /* why the executable file might be covered by the GNU Public License.    */
  31 /* Publication of enhanced or derived LinCAN files is required although.  */
  32 /**************************************************************************/
  33
  34 #include "./can/can.h"
  35 #include "./can/can_sysdep.h"
  36 #include "./can/can_queue.h"
  37
  38 /*
  39  * Modifies Tx message processing
  40  *  0 .. local message processing disabled
  41  *  1 .. local messages disabled by default but can be enabled by canque_set_filt
  42  *  2 .. local messages enabled by default, can be disabled by canque_set_filt
  43  */
  44 extern int processlocal;
  45
  46 atomic_t edge_num_cnt;
  47
  48 //#define CAN_DEBUG
  49 #undef CAN_DEBUG
  50
  51 #ifdef CAN_DEBUG
  52         #define DEBUGQUE(fmt,args...) can_printk(KERN_ERR "can_queue (debug): " fmt,\
  53         ##args)
  54
  55 #else
  56         #define DEBUGQUE(fmt,args...)
  57 #endif
  58
  59 #define CANQUE_ROUNDROB 1
  60
  61
  62 /**
  63  * canque_fifo_flush_slots - free all ready slots from the FIFO
  64  * @fifo: pointer to the FIFO structure
  65  *
  66  * The caller should be prepared to handle situations, when some
  67  * slots are held by input or output side slots processing.
  68  * These slots cannot be flushed or their processing interrupted.
  69  *
  70  * Return Value: The nonzero value indicates, that queue has not been
  71  *      empty before the function call.
  72  */
  73 int canque_fifo_flush_slots(struct canque_fifo_t *fifo)
  74 {
  75         int ret;
  76         can_spin_irqflags_t flags;
  77         struct canque_slot_t *slot;
  78         can_spin_lock_irqsave(&fifo->fifo_lock, flags);
  79         slot=fifo->head;
  80         if(slot){
  81                 *fifo->tail=fifo->flist;
  82                 fifo->flist=slot;
  83                 fifo->head=NULL;
  84                 fifo->tail=&fifo->head;
  85         }
  86         canque_fifo_clear_fl(fifo,FULL);
  87         ret=canque_fifo_test_and_set_fl(fifo,EMPTY)?0:1;
  88         can_spin_unlock_irqrestore(&fifo->fifo_lock, flags);
  89         return ret;
  90 }
  91
  92
  93 /**
  94  * canque_fifo_init_slots - initializes slot chain of one CAN FIFO
  95  * @fifo: pointer to the FIFO structure
  96  *
  97  * Return Value: The negative value indicates, that there is no memory
  98  *      to allocate space for the requested number of the slots.
  99  */
 100 int canque_fifo_init_slots(struct canque_fifo_t *fifo)
 101 {
 102         struct canque_slot_t *slot;
 103         int slotsnr=fifo->slotsnr;
 104         if(!fifo->entry || !slotsnr) return -1;
 105         slot=fifo->entry;
 106         fifo->flist=slot;
 107         while(--slotsnr){
 108                 slot->next=slot+1;
 109                 slot++;
 110         }
 111         slot->next=NULL;
 112         fifo->head=NULL;
 113         fifo->tail=&fifo->head;
 114         canque_fifo_set_fl(fifo,EMPTY);
 115         return 1;
 116 }
 117
 118 /* atomic_dec_and_test(&qedge->edge_used);
 119  void atomic_inc(&qedge->edge_used);
 120  list_add_tail(struct list_head *new, struct list_head *head)
 121  list_for_each(edge,qends->inlist);
 122  list_entry(ptr, type, member)
 123 */
 124
 125 void __canque_edge_decref(struct canque_edge_t *edge)
 126 {
 127         __canque_edge_decref_body(edge);
 128 }
 129
 130 /**
 131  * canque_get_inslot - finds one outgoing edge and allocates slot from it
 132  * @qends: ends structure belonging to calling communication object
 133  * @qedgep: place to store pointer to found edge
 134  * @slotp: place to store pointer to  allocated slot
 135  * @cmd: command type for slot
 136  *
 137  * Function looks for the first non-blocked outgoing edge in @qends structure
 138  * and tries to allocate slot from it.
 139  * Return Value: If there is no usable edge or there is no free slot in edge
 140  *      negative value is returned.
 141  */
 142 int canque_get_inslot(struct canque_ends_t *qends,
 143         struct canque_edge_t **qedgep, struct canque_slot_t **slotp, int cmd)
 144 {
 145         int ret=-2;
 146         struct canque_edge_t *edge;
 147
 148         edge=canque_first_inedge(qends);
 149         if(edge){
 150                 if(!canque_fifo_test_fl(&edge->fifo,BLOCK)){
 151                         ret=canque_fifo_get_inslot(&edge->fifo, slotp, cmd);
 152                         if(ret>0){
 153                                 *qedgep=edge;
 154                                 DEBUGQUE("canque_get_inslot cmd=%d found edge %d\n",cmd,edge->edge_num);
 155                                 return ret;
 156
 157                         }
 158                 }
 159                 canque_edge_decref(edge);
 160         }
 161         *qedgep=NULL;
 162         DEBUGQUE("canque_get_inslot cmd=%d failed\n",cmd);
 163         return ret;
 164 }
 165
 166 /**
 167  * canque_get_inslot4id - finds best outgoing edge and slot for given ID
 168  * @qends: ends structure belonging to calling communication object
 169  * @qedgep: place to store pointer to found edge
 170  * @slotp: place to store pointer to  allocated slot
 171  * @cmd: command type for slot
 172  * @id: communication ID of message to send into edge
 173  * @prio: optional priority of message
 174  *
 175  * Function looks for the non-blocked outgoing edge accepting messages
 176  * with given ID. If edge is found, slot is allocated from that edge.
 177  * The edges with non-zero mask are preferred over edges open to all messages.
 178  * If more edges with mask accepts given message ID, the edge with
 179  * highest priority below or equal to required priority is selected.
 180  * Return Value: If there is no usable edge or there is no free slot in edge
 181  *      negative value is returned.
 182  */
 183 int canque_get_inslot4id(struct canque_ends_t *qends,
 184         struct canque_edge_t **qedgep, struct canque_slot_t **slotp,
 185         int cmd, unsigned long id, int prio)
 186 {
 187         int ret=-2;
 188         struct canque_edge_t *edge, *bestedge=NULL;
 189
 190         canque_for_each_inedge(qends, edge){
 191                 if(canque_fifo_test_fl(&edge->fifo,BLOCK))
 192                         continue;
 193                 if((id^edge->filtid)&edge->filtmask)
 194                         continue;
 195                 if(bestedge){
 196                         if(bestedge->filtmask){
 197                                 if (!edge->filtmask) continue;
 198                         } else {
 199                                 if(edge->filtmask){
 200                                         canque_edge_decref(bestedge);
 201                                         bestedge=edge;
 202                                         canque_edge_incref(bestedge);
 203                                         continue;
 204                                 }
 205                         }
 206                         if(bestedge->edge_prio<edge->edge_prio){
 207                                 if(edge->edge_prio>prio) continue;
 208                         } else {
 209                                 if(bestedge->edge_prio<=prio) continue;
 210                         }
 211                         canque_edge_decref(bestedge);
 212                 }
 213                 bestedge=edge;
 214                 canque_edge_incref(bestedge);
 215         }
 216         if((edge=bestedge)!=NULL){
 217                 ret=canque_fifo_get_inslot(&edge->fifo, slotp, cmd);
 218                 if(ret>0){
 219                         *qedgep=edge;
 220                         DEBUGQUE("canque_get_inslot4id cmd=%d id=%ld prio=%d found edge %d\n",cmd,id,prio,edge->edge_num);
 221                         return ret;
 222                 }
 223                 canque_edge_decref(bestedge);
 224         }
 225         *qedgep=NULL;
 226         DEBUGQUE("canque_get_inslot4id cmd=%d id=%ld prio=%d failed\n",cmd,id,prio);
 227         return ret;
 228 }
 229
 230
 231 /**
 232  * canque_put_inslot - schedules filled slot for processing
 233  * @qends: ends structure belonging to calling communication object
 234  * @qedge: edge slot belong to
 235  * @slot: pointer to the prepared slot
 236  *
 237  * Puts slot previously acquired by canque_get_inslot() or canque_get_inslot4id()
 238  * function call into FIFO queue and activates edge processing if needed.
 239  * Return Value: Positive value informs, that activation of output end
 240  *      has been necessary
 241  */
 242 int canque_put_inslot(struct canque_ends_t *qends,
 243         struct canque_edge_t *qedge, struct canque_slot_t *slot)
 244 {
 245         int ret;
 246         ret=canque_fifo_put_inslot(&qedge->fifo,slot);
 247         if(ret) {
 248                 canque_activate_edge(qends,qedge);
 249                 canque_notify_outends(qedge,CANQUEUE_NOTIFY_PROC);
 250         }
 251         canque_edge_decref(qedge);
 252         DEBUGQUE("canque_put_inslot for edge %d returned %d\n",qedge->edge_num,ret);
 253         return ret;
 254 }
 255
 256 /**
 257  * canque_abort_inslot - aborts preparation of the message in the slot
 258  * @qends: ends structure belonging to calling communication object
 259  * @qedge: edge slot belong to
 260  * @slot: pointer to the previously allocated slot
 261  *
 262  * Frees slot previously acquired by canque_get_inslot() or canque_get_inslot4id()
 263  * function call. Used when message copying into slot fails.
 264  * Return Value: Positive value informs, that queue full state has been negated.
 265  */
 266 int canque_abort_inslot(struct canque_ends_t *qends,
 267         struct canque_edge_t *qedge, struct canque_slot_t *slot)
 268 {
 269         int ret;
 270         ret=canque_fifo_abort_inslot(&qedge->fifo,slot);
 271         if(ret) {
 272                 canque_notify_outends(qedge,CANQUEUE_NOTIFY_SPACE);
 273         }
 274         canque_edge_decref(qedge);
 275         DEBUGQUE("canque_abort_inslot for edge %d returned %d\n",qedge->edge_num,ret);
 276         return ret;
 277 }
 278
 279 /**
 280  * canque_filter_msg2edges - sends message into all edges which accept its ID
 281  * @qends: ends structure belonging to calling communication object
 282  * @msg: pointer to CAN message
 283  *
 284  * Sends message to all outgoing edges connected to the given ends, which accepts
 285  * message communication ID.
 286  * Return Value: Returns number of edges message has been send to
 287  */
 288 int canque_filter_msg2edges(struct canque_ends_t *qends, struct canmsg_t *msg)
 289 {
 290         int destnr=0;
 291         int ret;
 292         unsigned long msgid;
 293         struct canque_edge_t *edge;
 294         struct canque_slot_t *slot;
 295
 296         DEBUGQUE("canque_filter_msg2edges for msg ID 0x%08lx and flags 0x%02x\n",
 297                         msg->id, msg->flags);
 298         msgid = canque_filtid2internal(msg->id, msg->flags);
 299
 300         canque_for_each_inedge(qends, edge) {
 301                 if(canque_fifo_test_fl(&edge->fifo,BLOCK))
 302                         continue;
 303                 if((msgid^edge->filtid)&edge->filtmask)
 304                         continue;
 305                 ret=canque_fifo_get_inslot(&edge->fifo, &slot, 0);
 306                 if(ret>0){
 307                         slot->msg=*msg;
 308                         destnr++;
 309                         ret=canque_fifo_put_inslot(&edge->fifo,slot);
 310                         if(ret) {
 311                                 canque_activate_edge(qends,edge);
 312                                 canque_notify_outends(edge,CANQUEUE_NOTIFY_PROC);
 313                         }
 314
 315                 }
 316         }
 317         DEBUGQUE("canque_filter_msg2edges sent msg ID %ld to %d edges\n",msg->id,destnr);
 318         return destnr;
 319 }
 320
 321 /**
 322  * canque_test_outslot - test and retrieve ready slot for given ends
 323  * @qends: ends structure belonging to calling communication object
 324  * @qedgep: place to store pointer to found edge
 325  * @slotp: place to store pointer to received slot
 326  *
 327  * Function takes highest priority active incoming edge and retrieves
 328  * oldest ready slot from it.
 329  * Return Value: Negative value informs, that there is no ready output
 330  *      slot for given ends. Positive value is equal to the command
 331  *      slot has been allocated by the input side.
 332  */
 333 int canque_test_outslot(struct canque_ends_t *qends,
 334         struct canque_edge_t **qedgep, struct canque_slot_t **slotp)
 335 {
 336         can_spin_irqflags_t flags;
 337         int prio;
 338         struct canque_edge_t *edge;
 339         int ret;
 340
 341         can_spin_lock_irqsave(&qends->ends_lock, flags);
 342         for(prio=CANQUEUE_PRIO_NR;--prio>=0;){
 343                 while(!list_empty(&qends->active[prio])){
 344                         edge=list_entry(qends->active[prio].next,struct canque_edge_t,activepeers);
 345                         if(!canque_fifo_test_fl(&edge->fifo,DEAD)) {
 346                                 /* The first test on unlocked FIFO */
 347                                 if(canque_fifo_test_fl(&edge->fifo,EMPTY)) {
 348                                         can_spin_lock(&edge->fifo.fifo_lock);
 349                                         /* Test has to be repeated to ensure that EMPTY
 350                                            state has not been nagated when locking FIFO */
 351                                         if(canque_fifo_test_fl(&edge->fifo,EMPTY)) {
 352                                                 canque_fifo_set_fl(&edge->fifo,INACTIVE);
 353                                                 list_del(&edge->activepeers);
 354                                                 list_add(&edge->activepeers,&qends->idle);
 355                                                 can_spin_unlock(&edge->fifo.fifo_lock);
 356                                                 continue;
 357                                         }
 358                                         can_spin_unlock(&edge->fifo.fifo_lock);
 359                                 }
 360                                 canque_edge_incref(edge);
 361                                 can_spin_unlock_irqrestore(&qends->ends_lock, flags);
 362                                 *qedgep=edge;
 363                                 DEBUGQUE("canque_test_outslot found edge %d\n",edge->edge_num);
 364                                 ret=canque_fifo_test_outslot(&edge->fifo, slotp);
 365                                 if(ret>=0)
 366                                         return ret;
 367
 368                                 canque_edge_decref(edge);
 369                                 can_spin_lock_irqsave(&qends->ends_lock, flags);
 370                         } else {
 371                                 can_spin_lock(&edge->fifo.fifo_lock);
 372                                 canque_fifo_set_fl(&edge->fifo,INACTIVE);
 373                                 list_del(&edge->activepeers);
 374                                 list_add(&edge->activepeers,&qends->idle);
 375                                 can_spin_unlock(&edge->fifo.fifo_lock);
 376                         }
 377                 }
 378         }
 379         can_spin_unlock_irqrestore(&qends->ends_lock, flags);
 380         *qedgep=NULL;
 381 //      DEBUGQUE("canque_test_outslot no ready slot\n");
 382         return -1;
 383 }
 384
 385 /**
 386  * canque_free_outslot - frees processed output slot
 387  * @qends: ends structure belonging to calling communication object
 388  * @qedge: edge slot belong to
 389  * @slot: pointer to the processed slot
 390  *
 391  * Function releases processed slot previously acquired by canque_test_outslot()
 392  * function call.
 393  * Return Value: Return value informs if input side has been notified
 394  *      to know about change of edge state
 395  */
 396 int canque_free_outslot(struct canque_ends_t *qends,
 397         struct canque_edge_t *qedge, struct canque_slot_t *slot)
 398 {
 399         int ret;
 400         can_spin_irqflags_t flags;
 401         ret=canque_fifo_free_outslot(&qedge->fifo, slot);
 402         if(ret&CAN_FIFOF_EMPTY){
 403                 canque_notify_inends(qedge,CANQUEUE_NOTIFY_EMPTY);
 404         }
 405         if(ret&CAN_FIFOF_FULL)
 406                 canque_notify_inends(qedge,CANQUEUE_NOTIFY_SPACE);
 407         can_spin_lock_irqsave(&qends->ends_lock, flags);
 408         if((ret&CAN_FIFOF_EMPTY) || CANQUE_ROUNDROB ){
 409                 can_spin_lock(&qedge->fifo.fifo_lock);
 410                 if(canque_fifo_test_fl(&qedge->fifo,EMPTY)){
 411                         canque_fifo_set_fl(&qedge->fifo,INACTIVE);
 412                         list_del(&qedge->activepeers);
 413                         list_add(&qedge->activepeers,&qends->idle);
 414                 } else{
 415                         list_del(&qedge->activepeers);
 416                         list_add_tail(&qedge->activepeers,&qends->active[qedge->edge_prio]);
 417                 }
 418                 can_spin_unlock(&qedge->fifo.fifo_lock);
 419         }
 420         can_spin_unlock_irqrestore(&qends->ends_lock, flags);
 421         canque_edge_decref(qedge);
 422         DEBUGQUE("canque_free_outslot for edge %d returned %d\n",qedge->edge_num,ret);
 423         return ret;
 424 }
 425
 426 /**
 427  * canque_again_outslot - reschedule output slot to process it again later
 428  * @qends: ends structure belonging to calling communication object
 429  * @qedge: edge slot belong to
 430  * @slot: pointer to the slot for re-processing
 431  *
 432  * Function reschedules slot previously acquired by canque_test_outslot()
 433  * function call for second time processing.
 434  * Return Value: Function cannot fail.
 435  */
 436 int canque_again_outslot(struct canque_ends_t *qends,
 437         struct canque_edge_t *qedge, struct canque_slot_t *slot)
 438 {
 439         int ret;
 440         ret=canque_fifo_again_outslot(&qedge->fifo, slot);
 441         canque_edge_decref(qedge);
 442         DEBUGQUE("canque_again_outslot for edge %d returned %d\n",qedge->edge_num,ret);
 443         return ret;
 444 }
 445
 446 /**
 447  * canque_set_filt - sets filter for specified edge
 448  * @qedge: pointer to the edge
 449  * @filtid: ID to set for the edge
 450  * @filtmask: mask used for ID match check
 451  * @filtflags: required filer flags
 452  *
 453  * Return Value: Negative value is returned if edge is in the process of delete.
 454  */
 455 int canque_set_filt(struct canque_edge_t *qedge,
 456         unsigned long filtid, unsigned long filtmask, int filtflags)
 457 {
 458         int ret;
 459         can_spin_irqflags_t flags;
 460
 461         can_spin_lock_irqsave(&qedge->fifo.fifo_lock,flags);
 462
 463         if(!(filtflags&MSG_PROCESSLOCAL) && (processlocal<2))
 464                 filtflags |= MSG_LOCAL_MASK;
 465
 466         qedge->filtid=canque_filtid2internal(filtid, filtflags);
 467         qedge->filtmask=canque_filtid2internal(filtmask, filtflags>>MSG_FILT_MASK_SHIFT);
 468
 469         if(canque_fifo_test_fl(&qedge->fifo,DEAD)) ret=-1;
 470         else ret=canque_fifo_test_and_set_fl(&qedge->fifo,BLOCK)?1:0;
 471
 472         can_spin_unlock_irqrestore(&qedge->fifo.fifo_lock,flags);
 473         if(ret>=0){
 474                 canque_notify_bothends(qedge,CANQUEUE_NOTIFY_FILTCH);
 475         }
 476         can_spin_lock_irqsave(&qedge->fifo.fifo_lock,flags);
 477         if(!ret) canque_fifo_clear_fl(&qedge->fifo,BLOCK);
 478         can_spin_unlock_irqrestore(&qedge->fifo.fifo_lock,flags);
 479
 480         DEBUGQUE("canque_set_filt for edge %d, ID %ld, mask %ld, flags %d returned %d\n",
 481                   qedge->edge_num,filtid,filtmask,filtflags,ret);
 482         return ret;
 483 }
 484
 485 /**
 486  * canque_flush - fluesh all ready slots in the edge
 487  * @qedge: pointer to the edge
 488  *
 489  * Tries to flush all allocated slots from the edge, but there could
 490  * exist some slots associated to edge which are processed by input
 491  * or output side and cannot be flushed at this moment.
 492  * Return Value: The nonzero value indicates, that queue has not been
 493  *      empty before the function call.
 494  */
 495 int canque_flush(struct canque_edge_t *qedge)
 496 {
 497         int ret;
 498         can_spin_irqflags_t flags;
 499
 500         ret=canque_fifo_flush_slots(&qedge->fifo);
 501         if(ret){
 502                 canque_notify_inends(qedge,CANQUEUE_NOTIFY_EMPTY);
 503                 canque_notify_inends(qedge,CANQUEUE_NOTIFY_SPACE);
 504                 can_spin_lock_irqsave(&qedge->outends->ends_lock, flags);
 505                 can_spin_lock(&qedge->fifo.fifo_lock);
 506                 if(canque_fifo_test_fl(&qedge->fifo,EMPTY)){
 507                         list_del(&qedge->activepeers);
 508                         list_add(&qedge->activepeers,&qedge->outends->idle);
 509                 }
 510                 can_spin_unlock(&qedge->fifo.fifo_lock);
 511                 can_spin_unlock_irqrestore(&qedge->outends->ends_lock, flags);
 512         }
 513         DEBUGQUE("canque_flush for edge %d returned %d\n",qedge->edge_num,ret);
 514         return ret;
 515 }
 516
 517 /**
 518  * canqueue_ends_init_gen - subsystem independent routine to initialize ends state
 519  * @qends: pointer to the ends structure
 520  *
 521  * Return Value: Cannot fail.
 522  */
 523 int canqueue_ends_init_gen(struct canque_ends_t *qends)
 524 {
 525         int i;
 526         qends->ends_flags=0;
 527         for(i=CANQUEUE_PRIO_NR;--i>=0;){
 528                 INIT_LIST_HEAD(&qends->active[i]);
 529         }
 530         INIT_LIST_HEAD(&qends->idle);
 531         INIT_LIST_HEAD(&qends->inlist);
 532         INIT_LIST_HEAD(&qends->outlist);
 533         can_spin_lock_init(&qends->ends_lock);
 534         return 0;
 535 }
 536
 537
 538 /**
 539  * canqueue_connect_edge - connect edge between two communication entities
 540  * @qedge: pointer to edge
 541  * @inends: pointer to ends the input of the edge should be connected to
 542  * @outends: pointer to ends the output of the edge should be connected to
 543  *
 544  * Return Value: Negative value informs about failed operation.
 545  */
 546 int canqueue_connect_edge(struct canque_edge_t *qedge, struct canque_ends_t *inends, struct canque_ends_t *outends)
 547 {
 548         can_spin_irqflags_t flags;
 549         if(qedge == NULL) return -1;
 550         DEBUGQUE("canqueue_connect_edge %d\n",qedge->edge_num);
 551         canque_edge_incref(qedge);
 552         flags=canque_edge_lock_both_ends(inends, outends);
 553         can_spin_lock(&qedge->fifo.fifo_lock);
 554         qedge->inends=inends;
 555         list_add(&qedge->inpeers,&inends->inlist);
 556         qedge->outends=outends;
 557         list_add(&qedge->outpeers,&outends->outlist);
 558         list_add(&qedge->activepeers,&outends->idle);
 559         can_spin_unlock(&qedge->fifo.fifo_lock);
 560         canque_edge_unlock_both_ends(inends, outends, flags);
 561         canque_notify_bothends(qedge, CANQUEUE_NOTIFY_ATTACH);
 562
 563         if(canque_fifo_test_and_set_fl(&qedge->fifo, READY))
 564                 canque_edge_decref(qedge);
 565         return 0;
 566 }
 567
 568 /**
 569  * canqueue_disconnect_edge - disconnect edge from communicating entities
 570  * @qedge: pointer to edge
 571  *
 572  * Return Value: Negative value means, that edge is used by somebody
 573  *      other and cannot be disconnected. Operation has to be delayed.
 574  */
 575 int canqueue_disconnect_edge(struct canque_edge_t *qedge)
 576 {
 577         int ret;
 578         can_spin_irqflags_t flags;
 579         struct canque_ends_t *inends, *outends;
 580
 581         inends=qedge->inends;
 582         outends=qedge->outends;
 583
 584         if(inends && outends) {
 585                 flags=canque_edge_lock_both_ends(inends, outends);
 586         } else {
 587                 DEBUGQUE("canqueue_disconnect_edge called with not fully connected edge");
 588                 if(inends) can_spin_lock_irqsave(&inends->ends_lock,flags);
 589                 else if(outends) can_spin_lock_irqsave(&outends->ends_lock,flags);
 590                 else flags=0;
 591         }
 592
 593         can_spin_lock(&qedge->fifo.fifo_lock);
 594         if(atomic_read(&qedge->edge_used)==0) {
 595                 if(qedge->outends){
 596                         list_del(&qedge->activepeers);
 597                         mb(); /* memory barrier for list_empty use in canque_dead_func */
 598                         list_del(&qedge->outpeers);
 599                         qedge->outends=NULL;
 600                 }
 601                 if(qedge->inends){
 602                         list_del(&qedge->inpeers);
 603                         qedge->inends=NULL;
 604                 }
 605                 ret=1;
 606         } else ret=-1;
 607         can_spin_unlock(&qedge->fifo.fifo_lock);
 608
 609         if(inends && outends) {
 610                 canque_edge_unlock_both_ends(inends, outends, flags);
 611         } else {
 612                 if(inends) can_spin_unlock_irqrestore(&inends->ends_lock,flags);
 613                 else if(outends) can_spin_unlock_irqrestore(&outends->ends_lock,flags);
 614         }
 615
 616         DEBUGQUE("canqueue_disconnect_edge %d returned %d\n",qedge->edge_num,ret);
 617         return ret;
 618 }
 619
 620
 621 /**
 622  * canqueue_block_inlist - block slot allocation of all outgoing edges of specified ends
 623  * @qends: pointer to ends structure
 624  */
 625 void canqueue_block_inlist(struct canque_ends_t *qends)
 626 {
 627         struct canque_edge_t *edge;
 628
 629         canque_for_each_inedge(qends, edge) {
 630                 canque_fifo_set_fl(&edge->fifo,BLOCK);
 631         }
 632 }
 633
 634
 635 /**
 636  * canqueue_block_outlist - block slot allocation of all incoming edges of specified ends
 637  * @qends: pointer to ends structure
 638  */
 639 void canqueue_block_outlist(struct canque_ends_t *qends)
 640 {
 641         struct canque_edge_t *edge;
 642
 643         canque_for_each_outedge(qends, edge) {
 644                 canque_fifo_set_fl(&edge->fifo,BLOCK);
 645         }
 646 }
 647
 648
 649 /**
 650  * canqueue_ends_kill_inlist - sends request to die to all outgoing edges
 651  * @qends: pointer to ends structure
 652  * @send_rest: select, whether already allocated slots should be processed
 653  *      by FIFO output side
 654  *
 655  * Return Value: Non-zero value means, that not all edges could be immediately
 656  *      disconnected and that ends structure memory release has to be delayed
 657  */
 658 int canqueue_ends_kill_inlist(struct canque_ends_t *qends, int send_rest)
 659 {
 660         struct canque_edge_t *edge;
 661
 662         canque_for_each_inedge(qends, edge){
 663                 canque_notify_bothends(edge, CANQUEUE_NOTIFY_DEAD_WANTED);
 664                 if(send_rest){
 665                         canque_edge_incref(edge);
 666                         if(!canque_fifo_test_and_set_fl(&edge->fifo, FREEONEMPTY)){
 667                                 if(!canque_fifo_test_fl(&edge->fifo, EMPTY))
 668                                         continue;
 669                                 if(!canque_fifo_test_and_clear_fl(&edge->fifo, FREEONEMPTY))
 670                                         continue;
 671                         }
 672                         canque_edge_decref(edge);
 673                 }
 674         }
 675         return list_empty(&qends->inlist)?0:1;
 676 }
 677
 678
 679 /**
 680  * canqueue_ends_kill_outlist - sends request to die to all incoming edges
 681  * @qends: pointer to ends structure
 682  *
 683  * Return Value: Non-zero value means, that not all edges could be immediately
 684  *      disconnected and that ends structure memory release has to be delayed
 685  */
 686 int canqueue_ends_kill_outlist(struct canque_ends_t *qends)
 687 {
 688         struct canque_edge_t *edge;
 689
 690         canque_for_each_outedge(qends, edge){
 691                 canque_notify_bothends(edge, CANQUEUE_NOTIFY_DEAD_WANTED);
 692         }
 693         return list_empty(&qends->outlist)?0:1;
 694 }
 695
 696
 697 /**
 698  * canqueue_ends_filt_conjuction - computes conjunction of incoming edges filters filters
 699  * @qends: pointer to ends structure
 700  * @filt: pointer the filter structure filled by computed filters conjunction
 701  *
 702  * Return Value: Number of incoming edges
 703  */
 704 int canqueue_ends_filt_conjuction(struct canque_ends_t *qends, struct canfilt_t *filt)
 705 {
 706         struct canque_edge_t *edge;
 707         int cnt=0;
 708         unsigned long filtid=0;
 709         unsigned long filtmask=~0;
 710         unsigned long local_only=canque_filtid2internal(0,MSG_LOCAL);
 711
 712         canque_for_each_inedge(qends, edge){
 713                 /* skip edges processing only local messages */
 714                 if(edge->filtid & edge->filtmask & local_only)
 715                         continue;
 716
 717                 if(!cnt++)
 718                         filtid = edge->filtid;
 719                 else
 720                         filtmask &= ~(filtid ^ edge->filtid);
 721
 722                 filtmask &= edge->filtmask;
 723         }
 724
 725         filt->id = filtid & MSG_ID_MASK;
 726         filt->mask = filtmask & MSG_ID_MASK;
 727         filtid >>= 28;
 728         filtmask >>= 28;
 729         filt->flags = filtid & MSG_EXT;
 730         if(filtmask & (MSG_EXT))
 731                 filt->flags |= MSG_EXT_MASK;
 732         if(filtid & (MSG_RTR<<1))
 733                 filt->flags |= MSG_RTR<<1;
 734         if(filtmask & (MSG_RTR<<1))
 735                 filt->flags |= MSG_RTR_MASK;
 736         return cnt;
 737 }
 738
 739
 740 /**
 741  * canqueue_ends_flush_inlist - flushes all messages in incoming edges
 742  * @qends: pointer to ends structure
 743  *
 744  * Return Value: Negative value informs about unsuccessful result
 745  */
 746 int canqueue_ends_flush_inlist(struct canque_ends_t *qends)
 747 {
 748         struct canque_edge_t *edge;
 749
 750         canque_for_each_inedge(qends, edge){
 751                 canque_flush(edge);
 752         }
 753         return 0;
 754 }
 755
 756
 757 /**
 758  * canqueue_ends_flush_outlist - flushes all messages in outgoing edges
 759  * @qends: pointer to ends structure
 760  *
 761  * Return Value: Negative value informs about unsuccessful result
 762  */
 763 int canqueue_ends_flush_outlist(struct canque_ends_t *qends)
 764 {
 765         struct canque_edge_t *edge;
 766
 767         canque_for_each_outedge(qends, edge){
 768                 canque_flush(edge);
 769         }
 770         return 0;
 771 }
 772
 773
 774
 775