release/src-rt-6.x.4708/linux/linux-2.6.36/drivers/isdn/capi/capilib.c

   1
   2 #include <linux/slab.h>
   3 #include <linux/kernel.h>
   4 #include <linux/module.h>
   5 #include <linux/isdn/capilli.h>
   6
   7 #define DBG(format, arg...) do { \
   8 printk(KERN_DEBUG "%s: " format "\n" , __func__ , ## arg); \
   9 } while (0)
  10
  11 struct capilib_msgidqueue {
  12         struct capilib_msgidqueue *next;
  13         u16 msgid;
  14 };
  15
  16 struct capilib_ncci {
  17         struct list_head list;
  18         u16 applid;
  19         u32 ncci;
  20         u32 winsize;
  21         int   nmsg;
  22         struct capilib_msgidqueue *msgidqueue;
  23         struct capilib_msgidqueue *msgidlast;
  24         struct capilib_msgidqueue *msgidfree;
  25         struct capilib_msgidqueue msgidpool[CAPI_MAXDATAWINDOW];
  26 };
  27
  28 // ---------------------------------------------------------------------------
  29 // NCCI Handling
  30
  31 static inline void mq_init(struct capilib_ncci * np)
  32 {
  33         u_int i;
  34         np->msgidqueue = NULL;
  35         np->msgidlast = NULL;
  36         np->nmsg = 0;
  37         memset(np->msgidpool, 0, sizeof(np->msgidpool));
  38         np->msgidfree = &np->msgidpool[0];
  39         for (i = 1; i < np->winsize; i++) {
  40                 np->msgidpool[i].next = np->msgidfree;
  41                 np->msgidfree = &np->msgidpool[i];
  42         }
  43 }
  44
  45 static inline int mq_enqueue(struct capilib_ncci * np, u16 msgid)
  46 {
  47         struct capilib_msgidqueue *mq;
  48         if ((mq = np->msgidfree) == NULL)
  49                 return 0;
  50         np->msgidfree = mq->next;
  51         mq->msgid = msgid;
  52         mq->next = NULL;
  53         if (np->msgidlast)
  54                 np->msgidlast->next = mq;
  55         np->msgidlast = mq;
  56         if (!np->msgidqueue)
  57                 np->msgidqueue = mq;
  58         np->nmsg++;
  59         return 1;
  60 }
  61
  62 static inline int mq_dequeue(struct capilib_ncci * np, u16 msgid)
  63 {
  64         struct capilib_msgidqueue **pp;
  65         for (pp = &np->msgidqueue; *pp; pp = &(*pp)->next) {
  66                 if ((*pp)->msgid == msgid) {
  67                         struct capilib_msgidqueue *mq = *pp;
  68                         *pp = mq->next;
  69                         if (mq == np->msgidlast)
  70                                 np->msgidlast = NULL;
  71                         mq->next = np->msgidfree;
  72                         np->msgidfree = mq;
  73                         np->nmsg--;
  74                         return 1;
  75                 }
  76         }
  77         return 0;
  78 }
  79
  80 void capilib_new_ncci(struct list_head *head, u16 applid, u32 ncci, u32 winsize)
  81 {
  82         struct capilib_ncci *np;
  83
  84         np = kmalloc(sizeof(*np), GFP_ATOMIC);
  85         if (!np) {
  86                 printk(KERN_WARNING "capilib_new_ncci: no memory.\n");
  87                 return;
  88         }
  89         if (winsize > CAPI_MAXDATAWINDOW) {
  90                 printk(KERN_ERR "capi_new_ncci: winsize %d too big\n",
  91                        winsize);
  92                 winsize = CAPI_MAXDATAWINDOW;
  93         }
  94         np->applid = applid;
  95         np->ncci = ncci;
  96         np->winsize = winsize;
  97         mq_init(np);
  98         list_add_tail(&np->list, head);
  99         DBG("kcapi: appl %d ncci 0x%x up", applid, ncci);
 100 }
 101
 102 EXPORT_SYMBOL(capilib_new_ncci);
 103
 104 void capilib_free_ncci(struct list_head *head, u16 applid, u32 ncci)
 105 {
 106         struct list_head *l;
 107         struct capilib_ncci *np;
 108
 109         list_for_each(l, head) {
 110                 np = list_entry(l, struct capilib_ncci, list);
 111                 if (np->applid != applid)
 112                         continue;
 113                 if (np->ncci != ncci)
 114                         continue;
 115                 printk(KERN_INFO "kcapi: appl %d ncci 0x%x down\n", applid, ncci);
 116                 list_del(&np->list);
 117                 kfree(np);
 118                 return;
 119         }
 120         printk(KERN_ERR "capilib_free_ncci: ncci 0x%x not found\n", ncci);
 121 }
 122
 123 EXPORT_SYMBOL(capilib_free_ncci);
 124
 125 void capilib_release_appl(struct list_head *head, u16 applid)
 126 {
 127         struct list_head *l, *n;
 128         struct capilib_ncci *np;
 129
 130         list_for_each_safe(l, n, head) {
 131                 np = list_entry(l, struct capilib_ncci, list);
 132                 if (np->applid != applid)
 133                         continue;
 134                 printk(KERN_INFO "kcapi: appl %d ncci 0x%x forced down\n", applid, np->ncci);
 135                 list_del(&np->list);
 136                 kfree(np);
 137         }
 138 }
 139
 140 EXPORT_SYMBOL(capilib_release_appl);
 141
 142 void capilib_release(struct list_head *head)
 143 {
 144         struct list_head *l, *n;
 145         struct capilib_ncci *np;
 146
 147         list_for_each_safe(l, n, head) {
 148                 np = list_entry(l, struct capilib_ncci, list);
 149                 printk(KERN_INFO "kcapi: appl %d ncci 0x%x forced down\n", np->applid, np->ncci);
 150                 list_del(&np->list);
 151                 kfree(np);
 152         }
 153 }
 154
 155 EXPORT_SYMBOL(capilib_release);
 156
 157 u16 capilib_data_b3_req(struct list_head *head, u16 applid, u32 ncci, u16 msgid)
 158 {
 159         struct list_head *l;
 160         struct capilib_ncci *np;
 161
 162         list_for_each(l, head) {
 163                 np = list_entry(l, struct capilib_ncci, list);
 164                 if (np->applid != applid)
 165                         continue;
 166                 if (np->ncci != ncci)
 167                         continue;
 168
 169                 if (mq_enqueue(np, msgid) == 0)
 170                         return CAPI_SENDQUEUEFULL;
 171
 172                 return CAPI_NOERROR;
 173         }
 174         printk(KERN_ERR "capilib_data_b3_req: ncci 0x%x not found\n", ncci);
 175         return CAPI_NOERROR;
 176 }
 177
 178 EXPORT_SYMBOL(capilib_data_b3_req);
 179
 180 void capilib_data_b3_conf(struct list_head *head, u16 applid, u32 ncci, u16 msgid)
 181 {
 182         struct list_head *l;
 183         struct capilib_ncci *np;
 184
 185         list_for_each(l, head) {
 186                 np = list_entry(l, struct capilib_ncci, list);
 187                 if (np->applid != applid)
 188                         continue;
 189                 if (np->ncci != ncci)
 190                         continue;
 191
 192                 if (mq_dequeue(np, msgid) == 0) {
 193                         printk(KERN_ERR "kcapi: msgid %hu ncci 0x%x not on queue\n",
 194                                msgid, ncci);
 195                 }
 196                 return;
 197         }
 198         printk(KERN_ERR "capilib_data_b3_conf: ncci 0x%x not found\n", ncci);
 199 }
 200
 201 EXPORT_SYMBOL(capilib_data_b3_conf);