lincan/src/can_queue.c

   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
   7  */
   8
   9 #define __NO_VERSION__
  10 #include <linux/module.h>
  11 #include <linux/version.h>
  12 #if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0))
  13 #include <linux/malloc.h>
  14 #else
  15 #include <linux/slab.h>
  16 #endif
  17 #include <linux/wait.h>
  18 #include "../include/can.h"
  19 #include "../include/can_queue.h"
  20
  21 #ifdef CAN_DEBUG
  22         #define DEBUGQUE(fmt,args...) printk(KERN_ERR "can_queue (debug): " fmt,\
  23         ##args)
  24
  25   atomic_t edge_num_cnt;
  26 #else
  27         #define DEBUGQUE(fmt,args...)
  28 #endif
  29
  30 #define CANQUE_ROUNDROB 1
  31
  32 /**
  33  * canque_fifo_flush_slots - free all ready slots from the FIFO
  34  * @fifo: pointer to the FIFO structure
  35  *
  36  * The caller should be prepared to handle situations, when some
  37  * slots are held by input or output side slots processing.
  38  * These slots cannot be flushed or their processing interrupted.
  39  *
  40  * Return Value: The nonzero value indicates, that queue has not been
  41  *      empty before the function call.
  42  */
  43 int canque_fifo_flush_slots(struct canque_fifo_t *fifo)
  44 {
  45         int ret;
  46         unsigned long flags;
  47         struct canque_slot_t *slot;
  48         spin_lock_irqsave(&fifo->fifo_lock, flags);
  49         slot=fifo->head;
  50         *fifo->tail=fifo->flist;
  51         fifo->flist=slot;
  52         fifo->head=NULL;
  53         fifo->tail=&fifo->head;
  54         ret=canque_fifo_test_and_set_fl(fifo,EMPTY);
  55         spin_unlock_irqrestore(&fifo->fifo_lock, flags);
  56         return ret;
  57 }
  58
  59
  60 /**
  61  * canque_fifo_init_slots - initialize one CAN FIFO
  62  * @fifo: pointer to the FIFO structure
  63  * @slotsnr: number of requested slots
  64  *
  65  * Return Value: The negative value indicates, that there is no memory
  66  *      to allocate space for the requested number of the slots.
  67  */
  68 int canque_fifo_init_slots(struct canque_fifo_t *fifo, int slotsnr)
  69 {
  70         int size;
  71         struct canque_slot_t *slot;
  72         if(!slotsnr) slotsnr=MAX_BUF_LENGTH;
  73         size=sizeof(struct canque_slot_t)*slotsnr;
  74         fifo->entry=kmalloc(size,GFP_KERNEL);
  75         if(!fifo->entry) return -1;
  76         slot=fifo->entry;
  77         fifo->flist=slot;
  78         while(--slotsnr){
  79                 slot->next=slot+1;
  80                 slot++;
  81         }
  82         slot->next=NULL;
  83         fifo->head=NULL;
  84         fifo->tail=&fifo->head;
  85         canque_fifo_set_fl(fifo,EMPTY);
  86         return 1;
  87 }
  88
  89 /**
  90  * canque_fifo_done - frees slots allocated for CAN FIFO
  91  * @fifo: pointer to the FIFO structure
  92  */
  93 int canque_fifo_done(struct canque_fifo_t *fifo)
  94 {
  95         if(fifo->entry)
  96                 kfree(fifo->entry);
  97         fifo->entry=NULL;
  98         return 1;
  99 }
 100
 101 /* atomic_dec_and_test(&qedge->edge_used);
 102  void atomic_inc(&qedge->edge_used);
 103  list_add_tail(struct list_head *new, struct list_head *head)
 104  list_for_each(edge,qends->inlist);
 105  list_entry(ptr, type, member)
 106 */
 107
 108 /**
 109  * canque_get_inslot - finds one outgoing edge and allocates slot from it
 110  * @qends: ends structure belonging to calling communication object
 111  * @qedgep: place to store pointer to found edge
 112  * @slotp: place to store pointer to  allocated slot
 113  * @cmd: command type for slot
 114  *
 115  * Function looks for the first non-blocked outgoing edge in @qends structure
 116  * and tries to allocate slot from it.
 117  * Return Value: If there is no usable edge or there is no free slot in edge
 118  *      negative value is returned.
 119  */
 120 int canque_get_inslot(struct canque_ends_t *qends,
 121         struct canque_edge_t **qedgep, struct canque_slot_t **slotp, int cmd)
 122 {
 123         int ret=-2;
 124         unsigned long flags;
 125         struct canque_edge_t *edge;
 126
 127         spin_lock_irqsave(&qends->ends_lock, flags);
 128         if(!list_empty(&qends->inlist)){
 129                 edge=list_entry(qends->inlist.next,struct canque_edge_t,inpeers);
 130                 if(!canque_fifo_test_fl(&edge->fifo,BLOCK)&&!canque_fifo_test_fl(&edge->fifo,DEAD)){
 131                         atomic_inc(&edge->edge_used);
 132                         spin_unlock_irqrestore(&qends->ends_lock, flags);
 133                         ret=canque_fifo_get_inslot(&edge->fifo, slotp, cmd);
 134                         if(ret>0){
 135                                 *qedgep=edge;
 136                                 DEBUGQUE("canque_get_inslot cmd=%d found edge %d\n",cmd,edge->edge_num);
 137                                 return ret;
 138
 139                         }
 140                         spin_lock_irqsave(&qends->ends_lock, flags);
 141                         if(atomic_dec_and_test(&edge->edge_used))
 142                                 canque_notify_bothends(edge,CANQUEUE_NOTIFY_NOUSR);
 143                 }
 144         }
 145         spin_unlock_irqrestore(&qends->ends_lock, flags);
 146         *qedgep=NULL;
 147         DEBUGQUE("canque_get_inslot cmd=%d failed\n",cmd);
 148         return ret;
 149 }
 150
 151 /**
 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
 159  *
 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.
 167  */
 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)
 171 {
 172         int ret=-2;
 173         unsigned long flags;
 174         struct canque_edge_t *edge, *bestedge=NULL;
 175         struct list_head *entry;
 176
 177         spin_lock_irqsave(&qends->ends_lock, flags);
 178         list_for_each(entry,&qends->inlist){
 179                 edge=list_entry(entry,struct canque_edge_t,inpeers);
 180                 if(canque_fifo_test_fl(&edge->fifo,BLOCK)||canque_fifo_test_fl(&edge->fifo,DEAD))
 181                         continue;
 182                 if((id^edge->filtid)&edge->filtmask)
 183                         continue;
 184                 if(bestedge){
 185                         if(bestedge->filtmask){
 186                                 if (!edge->filtmask) continue;
 187                         } else {
 188                                 if(edge->filtmask){
 189                                         bestedge=edge;
 190                                         continue;
 191                                 }
 192                         }
 193                         if(bestedge->edge_prio<edge->edge_prio){
 194                                 if(edge->edge_prio>prio) continue;
 195                         } else {
 196                                 if(bestedge->edge_prio<=prio) continue;
 197                         }
 198                 }
 199                 bestedge=edge;
 200         }
 201         if((edge=bestedge)!=NULL){
 202                 atomic_inc(&edge->edge_used);
 203                 spin_unlock_irqrestore(&qends->ends_lock, flags);
 204                 ret=canque_fifo_get_inslot(&edge->fifo, slotp, cmd);
 205                 if(ret>0){
 206                         *qedgep=edge;
 207                         DEBUGQUE("canque_get_inslot4id cmd=%d id=%ld prio=%d found edge %d\n",cmd,id,prio,edge->edge_num);
 208                         return ret;
 209                 }
 210                 spin_lock_irqsave(&qends->ends_lock, flags);
 211                 if(atomic_dec_and_test(&edge->edge_used))
 212                         canque_notify_bothends(edge,CANQUEUE_NOTIFY_NOUSR);
 213         }
 214         spin_unlock_irqrestore(&qends->ends_lock, flags);
 215         *qedgep=NULL;
 216         DEBUGQUE("canque_get_inslot4id cmd=%d id=%ld prio=%d failed\n",cmd,id,prio);
 217         return ret;
 218 }
 219
 220
 221 /**
 222  * canque_put_inslot - schedules filled slot for processing
 223  * @qends: ends structure belonging to calling communication object
 224  * @qedge: edge slot belong to
 225  * @slot: pointer to the prepared slot
 226  *
 227  * Puts slot previously acquired by canque_get_inslot() or canque_get_inslot4id()
 228  * function call into FIFO queue and activates edge processing if needed.
 229  * Return Value: Positive value informs, that activation of output end
 230  *      has been necessary
 231  */
 232 int canque_put_inslot(struct canque_ends_t *qends,
 233         struct canque_edge_t *qedge, struct canque_slot_t *slot)
 234 {
 235         int ret;
 236         unsigned long flags;
 237         ret=canque_fifo_put_inslot(&qedge->fifo,slot);
 238         if(ret) {
 239                 canque_activate_edge(qends,qedge);
 240                 canque_notify_outends(qedge,CANQUEUE_NOTIFY_PROC);
 241         }
 242         spin_lock_irqsave(&qends->ends_lock, flags);
 243         if(atomic_dec_and_test(&qedge->edge_used))
 244                 canque_notify_bothends(qedge,CANQUEUE_NOTIFY_NOUSR);
 245         spin_unlock_irqrestore(&qends->ends_lock, flags);
 246         DEBUGQUE("canque_put_inslot for edge %d returned %d\n",qedge->edge_num,ret);
 247         return ret;
 248 }
 249
 250 /**
 251  * canque_abort_inslot - aborts preparation of the message in the slot
 252  * @qends: ends structure belonging to calling communication object
 253  * @qedge: edge slot belong to
 254  * @slot: pointer to the previously allocated slot
 255  *
 256  * Frees slot previously acquired by canque_get_inslot() or canque_get_inslot4id()
 257  * function call. Used when message copying into slot fails.
 258  * Return Value: Positive value informs, that queue full state has been negated.
 259  */
 260 int canque_abort_inslot(struct canque_ends_t *qends,
 261         struct canque_edge_t *qedge, struct canque_slot_t *slot)
 262 {
 263         int ret;
 264         unsigned long flags;
 265         ret=canque_fifo_abort_inslot(&qedge->fifo,slot);
 266         if(ret) {
 267                 canque_notify_outends(qedge,CANQUEUE_NOTIFY_SPACE);
 268         }
 269         spin_lock_irqsave(&qends->ends_lock, flags);
 270         if(atomic_dec_and_test(&qedge->edge_used))
 271                 canque_notify_bothends(qedge,CANQUEUE_NOTIFY_NOUSR);
 272         spin_unlock_irqrestore(&qends->ends_lock, flags);
 273         DEBUGQUE("canque_abort_inslot for edge %d returned %d\n",qedge->edge_num,ret);
 274         return ret;
 275 }
 276
 277 /**
 278  * canque_filter_msg2edges - sends message into all edges which accept its ID
 279  * @qends: ends structure belonging to calling communication object
 280  * @msg: pointer to CAN message
 281  *
 282  * Sends message to all outgoing edges connected to the given ends, which accepts
 283  * message communication ID.
 284  * Return Value: Returns number of edges message has been send to
 285  */
 286 int canque_filter_msg2edges(struct canque_ends_t *qends, struct canmsg_t *msg)
 287 {
 288         int destnr=0;
 289         int ret;
 290         unsigned long flags;
 291         struct canque_edge_t *edge;
 292         struct list_head *entry;
 293         struct canque_slot_t *slot;
 294
 295         DEBUGQUE("canque_filter_msg2edges for msg ID %ld\n",msg->id);
 296         spin_lock_irqsave(&qends->ends_lock, flags);
 297         list_for_each(entry,&qends->inlist){
 298                 edge=list_entry(entry,struct canque_edge_t,inpeers);
 299                 if(canque_fifo_test_fl(&edge->fifo,BLOCK)||canque_fifo_test_fl(&edge->fifo,DEAD))
 300                         continue;
 301                 /* FIXME: the next comparison should be outside of ends lock */
 302                 if((msg->id^edge->filtid)&edge->filtmask)
 303                         continue;
 304                 atomic_inc(&edge->edge_used);
 305                 spin_unlock_irqrestore(&qends->ends_lock, flags);
 306                 ret=canque_fifo_get_inslot(&edge->fifo, &slot, 0);
 307                 if(ret>0){
 308                         slot->msg=*msg;
 309                         destnr++;
 310                         ret=canque_fifo_put_inslot(&edge->fifo,slot);
 311                         if(ret) {
 312                                 canque_activate_edge(qends,edge);
 313                                 canque_notify_outends(edge,CANQUEUE_NOTIFY_PROC);
 314                         }
 315
 316                 }
 317                 spin_lock_irqsave(&qends->ends_lock, flags);
 318                 if(atomic_dec_and_test(&edge->edge_used))
 319                         canque_notify_bothends(edge,CANQUEUE_NOTIFY_NOUSR);
 320         }
 321         spin_unlock_irqrestore(&qends->ends_lock, flags);
 322         DEBUGQUE("canque_filter_msg2edges sent msg ID %ld to %d edges\n",msg->id,destnr);
 323         return destnr;
 324 }
 325
 326 /**
 327  * canque_test_outslot - test and retrieve ready slot for given ends
 328  * @qends: ends structure belonging to calling communication object
 329  * @qedgep: place to store pointer to found edge
 330  * @slotp: place to store pointer to received slot
 331  *
 332  * Function takes highest priority active incoming edge and retrieves
 333  * oldest ready slot from it.
 334  * Return Value: Negative value informs, that there is no ready output
 335  *      slot for given ends. Positive value is equal to the command
 336  *      slot has been allocated by the input side.
 337  */
 338 int canque_test_outslot(struct canque_ends_t *qends,
 339         struct canque_edge_t **qedgep, struct canque_slot_t **slotp)
 340 {
 341         unsigned long flags;
 342         int prio;
 343         struct canque_edge_t *edge;
 344
 345         spin_lock_irqsave(&qends->ends_lock, flags);
 346         for(prio=CANQUEUE_PRIO_NR;--prio>=0;){
 347                 if(!list_empty(&qends->active[prio])){
 348                         edge=list_entry(qends->active[prio].next,struct canque_edge_t,outpeers);
 349                         atomic_inc(&edge->edge_used);
 350                         spin_unlock_irqrestore(&qends->ends_lock, flags);
 351                         *qedgep=edge;
 352                         DEBUGQUE("canque_test_outslot found edge %d\n",edge->edge_num);
 353                         return canque_fifo_test_outslot(&edge->fifo, slotp);
 354                 }
 355         }
 356         spin_unlock_irqrestore(&qends->ends_lock, flags);
 357         *qedgep=NULL;
 358         DEBUGQUE("canque_test_outslot no ready slot\n");
 359         return -1;
 360 }
 361
 362 /**
 363  * canque_free_outslot - frees processed output slot
 364  * @qends: ends structure belonging to calling communication object
 365  * @qedge: edge slot belong to
 366  * @slot: pointer to the processed slot
 367  *
 368  * Function releases processed slot previously acquired by canque_test_outslot()
 369  * function call.
 370  * Return Value: Return value informs if input side has been notified
 371  *      to know about change of edge state
 372  */
 373 int canque_free_outslot(struct canque_ends_t *qends,
 374         struct canque_edge_t *qedge, struct canque_slot_t *slot)
 375 {
 376         int ret;
 377         unsigned long flags;
 378         ret=canque_fifo_free_outslot(&qedge->fifo, slot);
 379         if(ret&CAN_FIFOF_EMPTY){
 380                 canque_notify_inends(qedge,CANQUEUE_NOTIFY_EMPTY);
 381         }
 382         if(ret&CAN_FIFOF_FULL)
 383                 canque_notify_inends(qedge,CANQUEUE_NOTIFY_SPACE);
 384         spin_lock_irqsave(&qends->ends_lock, flags);
 385         if((ret&CAN_FIFOF_EMPTY) || CANQUE_ROUNDROB){
 386                 spin_lock(&qedge->fifo.fifo_lock);
 387                 if(canque_fifo_test_fl(&qedge->fifo,EMPTY)){
 388                         list_del(&qedge->outpeers);
 389                         list_add(&qedge->outpeers,&qends->idle);
 390                 }
 391             #if CANQUE_ROUNDROB
 392                 else{
 393                         list_del(&qedge->outpeers);
 394                         list_add_tail(&qedge->outpeers,&qends->active[qedge->edge_prio]);
 395                 }
 396             #endif /*CANQUE_ROUNDROB*/
 397                 spin_unlock(&qedge->fifo.fifo_lock);
 398         }
 399         if(atomic_dec_and_test(&qedge->edge_used))
 400                 canque_notify_bothends(qedge,CANQUEUE_NOTIFY_NOUSR);
 401         spin_unlock_irqrestore(&qends->ends_lock, flags);
 402         DEBUGQUE("canque_free_outslot for edge %d returned %d\n",qedge->edge_num,ret);
 403         return ret;
 404 }
 405
 406 /**
 407  * canque_again_outslot - reschedule output slot to process it again later
 408  * @qends: ends structure belonging to calling communication object
 409  * @qedge: edge slot belong to
 410  * @slot: pointer to the slot for re-processing
 411  *
 412  * Function reschedules slot previously acquired by canque_test_outslot()
 413  * function call for second time processing.
 414  * Return Value: Function cannot fail.
 415  */
 416 int canque_again_outslot(struct canque_ends_t *qends,
 417         struct canque_edge_t *qedge, struct canque_slot_t *slot)
 418 {
 419         int ret;
 420         unsigned long flags;
 421         ret=canque_fifo_again_outslot(&qedge->fifo, slot);
 422         spin_lock_irqsave(&qends->ends_lock, flags);
 423         if(atomic_dec_and_test(&qedge->edge_used))
 424                 canque_notify_bothends(qedge,CANQUEUE_NOTIFY_NOUSR);
 425         spin_unlock_irqrestore(&qends->ends_lock, flags);
 426         DEBUGQUE("canque_again_outslot for edge %d returned %d\n",qedge->edge_num,ret);
 427         return ret;
 428 }
 429
 430 /**
 431  * canque_set_filt - sets filter for specified edge
 432  * @qedge: pointer to the edge
 433  * @filtid: ID to set for the edge
 434  * @filtmask: mask used for ID match check
 435  *
 436  * Return Value: Negative value is returned if edge is in the process of delete.
 437  */
 438 int canque_set_filt(struct canque_edge_t *qedge,
 439         unsigned long filtid, unsigned long filtmask)
 440 {
 441         int ret;
 442         unsigned long flags;
 443
 444         spin_lock_irqsave(&qedge->fifo.fifo_lock,flags);
 445         atomic_inc(&qedge->edge_used);
 446         qedge->filtid=filtid;
 447         qedge->filtmask=filtmask;
 448         if(canque_fifo_test_fl(&qedge->fifo,DEAD)) ret=-1;
 449         else ret=canque_fifo_test_and_set_fl(&qedge->fifo,BLOCK)?1:0;
 450
 451         spin_unlock_irqrestore(&qedge->fifo.fifo_lock,flags);
 452         if(ret>=0){
 453                 canque_notify_bothends(qedge,CANQUEUE_NOTIFY_FILTCH);
 454         }
 455         spin_lock_irqsave(&qedge->fifo.fifo_lock,flags);
 456         if(!ret)canque_fifo_clear_fl(&qedge->fifo,BLOCK);
 457         if(atomic_dec_and_test(&qedge->edge_used))
 458                 canque_notify_bothends(qedge,CANQUEUE_NOTIFY_NOUSR);
 459         spin_unlock_irqrestore(&qedge->fifo.fifo_lock,flags);
 460
 461         DEBUGQUE("canque_set_filt for edge %d, ID %ld and mask %ld returned %d\n",qedge->edge_num,filtid,filtmask,ret);
 462         return ret;
 463 }
 464
 465 /**
 466  * canque_flush - fluesh all ready slots in the edge
 467  * @qedge: pointer to the edge
 468  *
 469  * Tries to flush all allocated slots from the edge, but there could
 470  * exist some slots associated to edge which are processed by input
 471  * or output side and cannot be flushed at this moment.
 472  * Return Value: The nonzero value indicates, that queue has not been
 473  *      empty before the function call.
 474  */
 475 int canque_flush(struct canque_edge_t *qedge)
 476 {
 477         int ret;
 478         unsigned long flags;
 479
 480         atomic_inc(&qedge->edge_used);
 481         ret=canque_fifo_flush_slots(&qedge->fifo);
 482         if(ret){
 483                 canque_notify_inends(qedge,CANQUEUE_NOTIFY_EMPTY);
 484                 canque_notify_inends(qedge,CANQUEUE_NOTIFY_SPACE);
 485                 spin_lock_irqsave(&qedge->outends->ends_lock, flags);
 486                 spin_lock(&qedge->fifo.fifo_lock);
 487                 if(canque_fifo_test_fl(&qedge->fifo,EMPTY)){
 488                         list_del(&qedge->outpeers);
 489                         list_add(&qedge->outpeers,&qedge->outends->idle);
 490                 }
 491                 if(atomic_dec_and_test(&qedge->edge_used))
 492                         canque_notify_bothends(qedge,CANQUEUE_NOTIFY_NOUSR);
 493                 spin_unlock(&qedge->fifo.fifo_lock);
 494                 spin_unlock_irqrestore(&qedge->outends->ends_lock, flags);
 495         }
 496         DEBUGQUE("canque_flush for edge %d returned %d\n",qedge->edge_num,ret);
 497         return ret;
 498 }
 499
 500 /**
 501  * canqueue_ends_init_gen - subsystem independent routine to initialize ends state
 502  * @qends: pointer to the ends structure
 503  *
 504  * Return Value: Cannot fail.
 505  */
 506 int canqueue_ends_init_gen(struct canque_ends_t *qends)
 507 {
 508         int i;
 509         for(i=CANQUEUE_PRIO_NR;--i>=0;){
 510                 INIT_LIST_HEAD(&qends->active[i]);
 511         }
 512         INIT_LIST_HEAD(&qends->idle);
 513         INIT_LIST_HEAD(&qends->inlist);
 514         spin_lock_init(&qends->ends_lock);
 515         return 0;
 516 }
 517
 518
 519 /**
 520  * canqueue_notify_kern - notification callback handler for Linux userspace clients
 521  * @qends: pointer to the callback side ends structure
 522  * @qedge: edge which invoked notification
 523  * @what: notification type
 524  */
 525 void canqueue_notify_kern(struct canque_ends_t *qends, struct canque_edge_t *qedge, int what)
 526 {
 527         DEBUGQUE("canqueue_notify_kern for edge %d and event %d\n",qedge->edge_num,what);
 528         switch(what){
 529                 case CANQUEUE_NOTIFY_EMPTY:
 530                         wake_up_interruptible(&qends->endinfo.fileinfo.emptyq);
 531                         break;
 532                 case CANQUEUE_NOTIFY_SPACE:
 533                         wake_up_interruptible(&qends->endinfo.fileinfo.writeq);
 534                         break;
 535                 case CANQUEUE_NOTIFY_PROC:
 536                         wake_up_interruptible(&qends->endinfo.fileinfo.readq);
 537                         break;
 538                 case CANQUEUE_NOTIFY_NOUSR:
 539                         wake_up_interruptible(&qends->endinfo.fileinfo.readq);
 540                         wake_up_interruptible(&qends->endinfo.fileinfo.writeq);
 541                         wake_up_interruptible(&qends->endinfo.fileinfo.emptyq);
 542                         break;
 543                 case CANQUEUE_NOTIFY_DEAD:
 544                         if(atomic_read(&qedge->edge_used)>0)
 545                                 atomic_dec(&qedge->edge_used);
 546                         break;
 547                 case CANQUEUE_NOTIFY_ATACH:
 548                         atomic_inc(&qedge->edge_used);
 549                         break;
 550         }
 551 }
 552
 553 /**
 554  * canqueue_ends_init_kern - Linux userspace clients specific ends initialization
 555  * @qends: pointer to the callback side ends structure
 556  */
 557 int canqueue_ends_init_kern(struct canque_ends_t *qends)
 558 {
 559         canqueue_ends_init_gen(qends);
 560         qends->context=NULL;
 561         init_waitqueue_head(&qends->endinfo.fileinfo.readq);
 562         init_waitqueue_head(&qends->endinfo.fileinfo.writeq);
 563         init_waitqueue_head(&qends->endinfo.fileinfo.emptyq);
 564         qends->notify=canqueue_notify_kern;
 565         DEBUGQUE("canqueue_ends_init_kern\n");
 566         return 0;
 567 }
 568
 569
 570 /**
 571  * canque_get_inslot4id_wait_kern - find or wait for best outgoing edge and slot for given ID
 572  * @qends: ends structure belonging to calling communication object
 573  * @qedgep: place to store pointer to found edge
 574  * @slotp: place to store pointer to  allocated slot
 575  * @cmd: command type for slot
 576  * @id: communication ID of message to send into edge
 577  * @prio: optional priority of message
 578  *
 579  * Same as canque_get_inslot4id(), except, that it waits for free slot
 580  * in case, that queue is full. Function is specific for Linux userspace clients.
 581  * Return Value: If there is no usable edge negative value is returned.
 582  */
 583 int canque_get_inslot4id_wait_kern(struct canque_ends_t *qends,
 584         struct canque_edge_t **qedgep, struct canque_slot_t **slotp,
 585         int cmd, unsigned long id, int prio)
 586 {
 587         int ret=-1;
 588         DEBUGQUE("canque_get_inslot4id_wait_kern for cmd %d, id %ld, prio %d\n",cmd,id,prio);
 589         wait_event_interruptible((qends->endinfo.fileinfo.writeq),
 590                 (ret=canque_get_inslot4id(qends,qedgep,slotp,cmd,id,prio))!=-1);
 591         return ret;
 592 }
 593
 594 /**
 595  * canque_get_outslot_wait_kern - receive or wait for ready slot for given ends
 596  * @qends: ends structure belonging to calling communication object
 597  * @qedgep: place to store pointer to found edge
 598  * @slotp: place to store pointer to received slot
 599  *
 600  * The same as canque_test_outslot(), except it waits in the case, that there is
 601  * no ready slot for given ends. Function is specific for Linux userspace clients.
 602  * Return Value: Negative value informs, that there is no ready output
 603  *      slot for given ends. Positive value is equal to the command
 604  *      slot has been allocated by the input side.
 605  */
 606 int canque_get_outslot_wait_kern(struct canque_ends_t *qends,
 607         struct canque_edge_t **qedgep, struct canque_slot_t **slotp)
 608 {
 609         int ret=-1;
 610         DEBUGQUE("canque_get_outslot_wait_kern\n");
 611         wait_event_interruptible((qends->endinfo.fileinfo.readq),
 612                 (ret=canque_test_outslot(qends,qedgep,slotp))!=-1);
 613         return ret;
 614 }
 615
 616 /**
 617  * canque_sync_wait_kern - wait for all slots processing
 618  * @qends: ends structure belonging to calling communication object
 619  * @qedge: pointer to edge
 620  *
 621  * Functions waits for ends transition into empty state.
 622  * Return Value: Positive value indicates, that edge empty state has been reached.
 623  *      Negative or zero value informs about interrupted wait or other problem.
 624  */
 625 int canque_sync_wait_kern(struct canque_ends_t *qends, struct canque_edge_t *qedge)
 626 {
 627         int ret=-1;
 628         DEBUGQUE("canque_sync_wait_kern\n");
 629         wait_event_interruptible((qends->endinfo.fileinfo.emptyq),
 630                 (ret=canque_fifo_test_fl(&qedge->fifo,EMPTY)?1:0));
 631         return ret;
 632 }
 633
 634
 635 /**
 636  * canque_new_edge_kern - allocate new edge structure in the Linux kernel context
 637  * @slotsnr: required number of slots in the newly allocated edge structure
 638  *
 639  * Return Value: Returns pointer to allocated slot structure or %NULL if
 640  *      there is not enough memory to process operation.
 641  */
 642 struct canque_edge_t *canque_new_edge_kern(int slotsnr)
 643 {
 644         struct canque_edge_t *qedge;
 645         qedge = (struct canque_edge_t *)kmalloc(sizeof(struct canque_edge_t), GFP_KERNEL);
 646         if(qedge == NULL) return NULL;
 647
 648         memset(qedge,0,sizeof(struct canque_edge_t));
 649         spin_lock_init(&qedge->fifo.fifo_lock);
 650         if(canque_fifo_init_slots(&qedge->fifo, slotsnr)<0){
 651                 kfree(qedge);
 652                 DEBUGQUE("canque_new_edge_kern failed\n");
 653                 return NULL;
 654         }
 655         atomic_set(&qedge->edge_used,0);
 656         qedge->filtid = 0;
 657         qedge->filtmask = 0;
 658         qedge->edge_prio = 0;
 659     #ifdef CAN_DEBUG
 660         /* not exactly clean, but enough for debugging */
 661         atomic_inc(&edge_num_cnt);
 662         qedge->edge_num=atomic_read(&edge_num_cnt);
 663     #endif /* CAN_DEBUG */
 664         DEBUGQUE("canque_new_edge_kern %d\n",qedge->edge_num);
 665         return qedge;
 666 }
 667
 668 /**
 669  * canqueue_connect_edge - connect edge between two communication entities
 670  * @qedge: pointer to edge
 671  * @inends: pointer to ends the input of the edge should be connected to
 672  * @outends: pointer to ends the output of the edge should be connected to
 673  *
 674  * Return Value: Negative value informs about failed operation.
 675  */
 676 int canqueue_connect_edge(struct canque_edge_t *qedge, struct canque_ends_t *inends, struct canque_ends_t *outends)
 677 {
 678         unsigned long flags;
 679         if(qedge == NULL) return -1;
 680         DEBUGQUE("canqueue_connect_edge %d\n",qedge->edge_num);
 681         atomic_inc(&qedge->edge_used);
 682         spin_lock_irqsave(&inends->ends_lock, flags);
 683         spin_lock(&outends->ends_lock);
 684         spin_lock(&qedge->fifo.fifo_lock);
 685         qedge->inends=inends;
 686         list_add(&qedge->inpeers,&inends->inlist);
 687         qedge->outends=outends;
 688         list_add(&qedge->outpeers,&outends->idle);
 689         spin_unlock(&qedge->fifo.fifo_lock);
 690         spin_unlock(&outends->ends_lock);
 691         spin_unlock_irqrestore(&inends->ends_lock, flags);
 692         canque_notify_bothends(qedge, CANQUEUE_NOTIFY_ATACH);
 693
 694         spin_lock_irqsave(&qedge->fifo.fifo_lock, flags);
 695         if(atomic_dec_and_test(&qedge->edge_used))
 696                 canque_notify_bothends(qedge,CANQUEUE_NOTIFY_NOUSR);
 697         spin_unlock_irqrestore(&qedge->fifo.fifo_lock, flags);
 698         return 0;
 699 }
 700
 701 /**
 702  * canqueue_disconnect_edge - disconnect edge from communicating entities
 703  * @qedge: pointer to edge
 704  *
 705  * Return Value: Negative value means, that edge is used and cannot
 706  *      be disconnected. Operation has to be delayed.
 707  */
 708 int canqueue_disconnect_edge(struct canque_edge_t *qedge)
 709 {
 710         int ret;
 711         unsigned long flags;
 712         struct canque_ends_t *inends, *outends;
 713
 714         inends=qedge->inends;
 715         if(inends) spin_lock_irqsave(&inends->ends_lock,flags);
 716         outends=qedge->outends;
 717         if(outends) spin_lock(&outends->ends_lock);
 718         spin_lock(&qedge->fifo.fifo_lock);
 719         if(atomic_read(&qedge->edge_used)==0) {
 720                 if(qedge->outends){
 721                         list_del(&qedge->outpeers);
 722                         qedge->outends=NULL;
 723                 }
 724                 if(qedge->inends){
 725                         list_del(&qedge->inpeers);
 726                         qedge->inends=NULL;
 727                 }
 728                 ret=1;
 729         } else ret=-1;
 730         spin_unlock(&qedge->fifo.fifo_lock);
 731         if(outends) spin_unlock(&outends->ends_lock);
 732         if(inends) spin_unlock_irqrestore(&inends->ends_lock,flags);
 733         DEBUGQUE("canqueue_disconnect_edge %d returned %d\n",qedge->edge_num,ret);
 734         return ret;
 735 }
 736
 737 /**
 738  * canqueue_disconnect_edge_kern - disconnect edge from communicating entities with wait
 739  * @qends: ends structure belonging to calling communication object
 740  * @qedge: pointer to edge
 741  *
 742  * Same as canqueue_disconnect_edge(), but tries to wait for state with zero
 743  * use counter.
 744  * Return Value: Negative value means, that edge is used and cannot
 745  *      be disconnected yet. Operation has to be delayed.
 746  */
 747 int canqueue_disconnect_edge_kern(struct canque_ends_t *qends, struct canque_edge_t *qedge)
 748 {
 749         canque_fifo_set_fl(&qedge->fifo,BLOCK);
 750         DEBUGQUE("canqueue_disconnect_edge_kern %d called\n",qedge->edge_num);
 751         if(!canque_fifo_test_and_set_fl(&qedge->fifo,DEAD)){
 752                 canque_notify_bothends(qedge, CANQUEUE_NOTIFY_DEAD);
 753                 if(atomic_read(&qedge->edge_used)>0)
 754                         atomic_dec(&qedge->edge_used);
 755                 DEBUGQUE("canqueue_disconnect_edge_kern %d waiting\n",qedge->edge_num);
 756                 wait_event_interruptible((qends->endinfo.fileinfo.emptyq),
 757                         (canqueue_disconnect_edge(qedge)>=0));
 758                 return 0;
 759         } else {
 760                 DEBUGQUE("canqueue_disconnect_edge_kern failed\n");
 761                 return -1;
 762         }
 763 }
 764
 765
 766 int canqueue_disconnect_list_kern(struct canque_ends_t *qends, struct list_head *list)
 767 {
 768         struct canque_edge_t *edge;
 769         unsigned long flags;
 770         for(;;){
 771                 spin_lock_irqsave(&qends->ends_lock,flags);
 772                 if(list_empty(list)){
 773                         spin_unlock_irqrestore(&qends->ends_lock,flags);
 774                         return 0;
 775                 }
 776                 if(list == &qends->inlist)
 777                         edge=list_entry(list->next,struct canque_edge_t,inpeers);
 778                 else
 779                         edge=list_entry(list->next,struct canque_edge_t,outpeers);
 780                 atomic_inc(&edge->edge_used);
 781                 spin_unlock_irqrestore(&qends->ends_lock,flags);
 782                 if(canqueue_disconnect_edge_kern(qends, edge)>=0) {
 783                         /* Free edge memory */
 784                         canque_fifo_done(&edge->fifo);
 785                         kfree(edge);
 786                 }else{
 787                         DEBUGQUE("canqueue_disconnect_list_kern in troubles\n");
 788                         DEBUGQUE("the edge %d has usage count %d and flags %ld\n",edge->edge_num,atomic_read(&edge->edge_used),edge->fifo.fifo_flags);
 789                         return -1;
 790                 }
 791         }
 792 }
 793
 794 void canqueue_block_list(struct canque_ends_t *qends, struct list_head *list)
 795 {
 796         struct canque_edge_t *edge;
 797         struct list_head *entry;
 798
 799         /* has to be called with qends->ends_lock already locked */
 800         list_for_each(entry,&qends->inlist){
 801                 if(list == &qends->inlist)
 802                         edge=list_entry(list->next,struct canque_edge_t,inpeers);
 803                 else
 804                         edge=list_entry(list->next,struct canque_edge_t,outpeers);
 805                 canque_fifo_set_fl(&edge->fifo,BLOCK);
 806         }
 807 }
 808
 809
 810 /**
 811  * canqueue_ends_done_kern - finalizing of the ends structure for Linux kernel clients
 812  * @qends: pointer to ends structure
 813  * @sync: flag indicating, that user wants to wait for processing of all remaining
 814  *      messages
 815  *
 816  * Return Value: Function should be designed such way to not fail.
 817  */
 818 int canqueue_ends_done_kern(struct canque_ends_t *qends, int sync)
 819 {
 820         unsigned long flags;
 821         int i;
 822
 823         DEBUGQUE("canqueue_ends_done_kern\n");
 824         spin_lock_irqsave(&qends->ends_lock,flags);
 825         canqueue_block_list(qends, &qends->idle);
 826         for(i=CANQUEUE_PRIO_NR;--i>=0;){
 827                 canqueue_block_list(qends, &qends->active[i]);
 828         }
 829         canqueue_block_list(qends, &qends->idle);
 830         canqueue_block_list(qends, &qends->inlist);
 831         spin_unlock_irqrestore(&qends->ends_lock,flags);
 832
 833         for(i=CANQUEUE_PRIO_NR;--i>=0;){
 834                 canqueue_disconnect_list_kern(qends, &qends->active[i]);
 835         }
 836         canqueue_disconnect_list_kern(qends, &qends->idle);
 837         canqueue_disconnect_list_kern(qends, &qends->inlist);
 838
 839         wake_up_interruptible(&qends->endinfo.fileinfo.readq);
 840         wake_up_interruptible(&qends->endinfo.fileinfo.writeq);
 841         wake_up_interruptible(&qends->endinfo.fileinfo.emptyq);
 842
 843
 844         return 0;
 845 }
 846