1/* $Id: isdn_v110.c,v 1.1.1.1 2007/08/03 18:52:36 Exp $ 2 * 3 * Linux ISDN subsystem, V.110 related functions (linklevel). 4 * 5 * Copyright by Thomas Pfeiffer (pfeiffer@pds.de) 6 * 7 * This software may be used and distributed according to the terms 8 * of the GNU General Public License, incorporated herein by reference. 9 * 10 */ 11 12#include <linux/string.h> 13#include <linux/kernel.h> 14#include <linux/slab.h> 15#include <linux/mm.h> 16#include <linux/delay.h> 17 18#include <linux/isdn.h> 19#include "isdn_v110.h" 20 21#undef ISDN_V110_DEBUG 22 23char *isdn_v110_revision = "$Revision: 1.1.1.1 $"; 24 25#define V110_38400 255 26#define V110_19200 15 27#define V110_9600 3 28 29/* 30 * The following data are precoded matrices, online and offline matrix 31 * for 9600, 19200 und 38400, respectively 32 */ 33static unsigned char V110_OnMatrix_9600[] = 34{0xfc, 0xfc, 0xfc, 0xfc, 0xff, 0xff, 0xff, 0xfd, 0xff, 0xff, 35 0xff, 0xfd, 0xff, 0xff, 0xff, 0xfd, 0xff, 0xff, 0xff, 0xfd, 36 0xfd, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfd, 0xff, 0xff, 37 0xff, 0xfd, 0xff, 0xff, 0xff, 0xfd, 0xff, 0xff, 0xff, 0xfd}; 38 39static unsigned char V110_OffMatrix_9600[] = 40{0xfc, 0xfc, 0xfc, 0xfc, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 41 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 42 0xfd, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 43 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; 44 45static unsigned char V110_OnMatrix_19200[] = 46{0xf0, 0xf0, 0xff, 0xf7, 0xff, 0xf7, 0xff, 0xf7, 0xff, 0xf7, 47 0xfd, 0xff, 0xff, 0xf7, 0xff, 0xf7, 0xff, 0xf7, 0xff, 0xf7}; 48 49static unsigned char V110_OffMatrix_19200[] = 50{0xf0, 0xf0, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 51 0xfd, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; 52 53static unsigned char V110_OnMatrix_38400[] = 54{0x00, 0x7f, 0x7f, 0x7f, 0x7f, 0xfd, 0x7f, 0x7f, 0x7f, 0x7f}; 55 56static unsigned char V110_OffMatrix_38400[] = 57{0x00, 0xff, 0xff, 0xff, 0xff, 0xfd, 0xff, 0xff, 0xff, 0xff}; 58 59/* 60 * FlipBits reorders sequences of keylen bits in one byte. 61 * E.g. source order 7654321 will be converted to 45670123 when keylen = 4, 62 * and to 67452301 when keylen = 2. This is necessary because ordering on 63 * the isdn line is the other way. 64 */ 65static __inline unsigned char 66FlipBits(unsigned char c, int keylen) 67{ 68 unsigned char b = c; 69 unsigned char bit = 128; 70 int i; 71 int j; 72 int hunks = (8 / keylen); 73 74 c = 0; 75 for (i = 0; i < hunks; i++) { 76 for (j = 0; j < keylen; j++) { 77 if (b & (bit >> j)) 78 c |= bit >> (keylen - j - 1); 79 } 80 bit >>= keylen; 81 } 82 return c; 83} 84 85 86/* isdn_v110_open allocates and initializes private V.110 data 87 * structures and returns a pointer to these. 88 */ 89static isdn_v110_stream * 90isdn_v110_open(unsigned char key, int hdrlen, int maxsize) 91{ 92 int i; 93 isdn_v110_stream *v; 94 95 if ((v = kzalloc(sizeof(isdn_v110_stream), GFP_ATOMIC)) == NULL) 96 return NULL; 97 v->key = key; 98 v->nbits = 0; 99 for (i = 0; key & (1 << i); i++) 100 v->nbits++; 101 102 v->nbytes = 8 / v->nbits; 103 v->decodelen = 0; 104 105 switch (key) { 106 case V110_38400: 107 v->OnlineFrame = V110_OnMatrix_38400; 108 v->OfflineFrame = V110_OffMatrix_38400; 109 break; 110 case V110_19200: 111 v->OnlineFrame = V110_OnMatrix_19200; 112 v->OfflineFrame = V110_OffMatrix_19200; 113 break; 114 default: 115 v->OnlineFrame = V110_OnMatrix_9600; 116 v->OfflineFrame = V110_OffMatrix_9600; 117 break; 118 } 119 v->framelen = v->nbytes * 10; 120 v->SyncInit = 5; 121 v->introducer = 0; 122 v->dbit = 1; 123 v->b = 0; 124 v->skbres = hdrlen; 125 v->maxsize = maxsize - hdrlen; 126 if ((v->encodebuf = kmalloc(maxsize, GFP_ATOMIC)) == NULL) { 127 kfree(v); 128 return NULL; 129 } 130 return v; 131} 132 133/* isdn_v110_close frees private V.110 data structures */ 134void 135isdn_v110_close(isdn_v110_stream * v) 136{ 137 if (v == NULL) 138 return; 139#ifdef ISDN_V110_DEBUG 140 printk(KERN_DEBUG "v110 close\n"); 141#endif 142 kfree(v->encodebuf); 143 kfree(v); 144} 145 146 147/* 148 * ValidHeaderBytes return the number of valid bytes in v->decodebuf 149 */ 150static int 151ValidHeaderBytes(isdn_v110_stream * v) 152{ 153 int i; 154 for (i = 0; (i < v->decodelen) && (i < v->nbytes); i++) 155 if ((v->decodebuf[i] & v->key) != 0) 156 break; 157 return i; 158} 159 160/* 161 * SyncHeader moves the decodebuf ptr to the next valid header 162 */ 163static void 164SyncHeader(isdn_v110_stream * v) 165{ 166 unsigned char *rbuf = v->decodebuf; 167 int len = v->decodelen; 168 169 if (len == 0) 170 return; 171 for (rbuf++, len--; len > 0; len--, rbuf++) /* such den SyncHeader in buf ! */ 172 if ((*rbuf & v->key) == 0) /* erstes byte gefunden ? */ 173 break; /* jupp! */ 174 if (len) 175 memcpy(v->decodebuf, rbuf, len); 176 177 v->decodelen = len; 178#ifdef ISDN_V110_DEBUG 179 printk(KERN_DEBUG "isdn_v110: Header resync\n"); 180#endif 181} 182 183/* DecodeMatrix takes n (n>=1) matrices (v110 frames, 10 bytes) where 184 len is the number of matrix-lines. len must be a multiple of 10, i.e. 185 only complete matices must be given. 186 From these, netto data is extracted and returned in buf. The return-value 187 is the bytecount of the decoded data. 188 */ 189static int 190DecodeMatrix(isdn_v110_stream * v, unsigned char *m, int len, unsigned char *buf) 191{ 192 int line = 0; 193 int buflen = 0; 194 int mbit = 64; 195 int introducer = v->introducer; 196 int dbit = v->dbit; 197 unsigned char b = v->b; 198 199 while (line < len) { /* Are we done with all lines of the matrix? */ 200 if ((line % 10) == 0) { /* the 0. line of the matrix is always 0 ! */ 201 if (m[line] != 0x00) { /* not 0 ? -> error! */ 202#ifdef ISDN_V110_DEBUG 203 printk(KERN_DEBUG "isdn_v110: DecodeMatrix, V110 Bad Header\n"); 204 /* returning now is not the right thing, though :-( */ 205#endif 206 } 207 line++; /* next line of matrix */ 208 continue; 209 } else if ((line % 10) == 5) { /* in line 5 there's only e-bits ! */ 210 if ((m[line] & 0x70) != 0x30) { /* 011 has to be at the beginning! */ 211#ifdef ISDN_V110_DEBUG 212 printk(KERN_DEBUG "isdn_v110: DecodeMatrix, V110 Bad 5th line\n"); 213 /* returning now is not the right thing, though :-( */ 214#endif 215 } 216 line++; /* next line */ 217 continue; 218 } else if (!introducer) { /* every byte starts with 10 (stopbit, startbit) */ 219 introducer = (m[line] & mbit) ? 0 : 1; /* current bit of the matrix */ 220 next_byte: 221 if (mbit > 2) { /* was it the last bit in this line ? */ 222 mbit >>= 1; /* no -> take next */ 223 continue; 224 } /* otherwise start with leftmost bit in the next line */ 225 mbit = 64; 226 line++; 227 continue; 228 } else { /* otherwise we need to set a data bit */ 229 if (m[line] & mbit) /* was that bit set in the matrix ? */ 230 b |= dbit; /* yes -> set it in the data byte */ 231 else 232 b &= dbit - 1; /* no -> clear it in the data byte */ 233 if (dbit < 128) /* is that data byte done ? */ 234 dbit <<= 1; /* no, got the next bit */ 235 else { /* data byte is done */ 236 buf[buflen++] = b; /* copy byte into the output buffer */ 237 introducer = b = 0; /* init of the intro sequence and of the data byte */ 238 dbit = 1; /* next we look for the 0th bit */ 239 } 240 goto next_byte; /* look for next bit in the matrix */ 241 } 242 } 243 v->introducer = introducer; 244 v->dbit = dbit; 245 v->b = b; 246 return buflen; /* return number of bytes in the output buffer */ 247} 248 249/* 250 * DecodeStream receives V.110 coded data from the input stream. It recovers the 251 * original frames. 252 * The input stream doesn't need to be framed 253 */ 254struct sk_buff * 255isdn_v110_decode(isdn_v110_stream * v, struct sk_buff *skb) 256{ 257 int i; 258 int j; 259 int len; 260 unsigned char *v110_buf; 261 unsigned char *rbuf; 262 263 if (!skb) { 264 printk(KERN_WARNING "isdn_v110_decode called with NULL skb!\n"); 265 return NULL; 266 } 267 rbuf = skb->data; 268 len = skb->len; 269 if (v == NULL) { 270 /* invalid handle, no chance to proceed */ 271 printk(KERN_WARNING "isdn_v110_decode called with NULL stream!\n"); 272 dev_kfree_skb(skb); 273 return NULL; 274 } 275 if (v->decodelen == 0) /* cache empty? */ 276 for (; len > 0; len--, rbuf++) /* scan for SyncHeader in buf */ 277 if ((*rbuf & v->key) == 0) 278 break; /* found first byte */ 279 if (len == 0) { 280 dev_kfree_skb(skb); 281 return NULL; 282 } 283 /* copy new data to decode-buffer */ 284 memcpy(&(v->decodebuf[v->decodelen]), rbuf, len); 285 v->decodelen += len; 286 ReSync: 287 if (v->decodelen < v->nbytes) { /* got a new header ? */ 288 dev_kfree_skb(skb); 289 return NULL; /* no, try later */ 290 } 291 if (ValidHeaderBytes(v) != v->nbytes) { /* is that a valid header? */ 292 SyncHeader(v); /* no -> look for header */ 293 goto ReSync; 294 } 295 len = (v->decodelen - (v->decodelen % (10 * v->nbytes))) / v->nbytes; 296 if ((v110_buf = kmalloc(len, GFP_ATOMIC)) == NULL) { 297 printk(KERN_WARNING "isdn_v110_decode: Couldn't allocate v110_buf\n"); 298 dev_kfree_skb(skb); 299 return NULL; 300 } 301 for (i = 0; i < len; i++) { 302 v110_buf[i] = 0; 303 for (j = 0; j < v->nbytes; j++) 304 v110_buf[i] |= (v->decodebuf[(i * v->nbytes) + j] & v->key) << (8 - ((j + 1) * v->nbits)); 305 v110_buf[i] = FlipBits(v110_buf[i], v->nbits); 306 } 307 v->decodelen = (v->decodelen % (10 * v->nbytes)); 308 memcpy(v->decodebuf, &(v->decodebuf[len * v->nbytes]), v->decodelen); 309 310 skb_trim(skb, DecodeMatrix(v, v110_buf, len, skb->data)); 311 kfree(v110_buf); 312 if (skb->len) 313 return skb; 314 else { 315 kfree_skb(skb); 316 return NULL; 317 } 318} 319 320/* EncodeMatrix takes input data in buf, len is the bytecount. 321 Data is encoded into v110 frames in m. Return value is the number of 322 matrix-lines generated. 323 */ 324static int 325EncodeMatrix(unsigned char *buf, int len, unsigned char *m, int mlen) 326{ 327 int line = 0; 328 int i = 0; 329 int mbit = 128; 330 int dbit = 1; 331 int introducer = 3; 332 int ibit[] = {0, 1, 1}; 333 334 while ((i < len) && (line < mlen)) { /* while we still have input data */ 335 switch (line % 10) { /* in which line of the matrix are we? */ 336 case 0: 337 m[line++] = 0x00; /* line 0 is always 0 */ 338 mbit = 128; /* go on with the 7th bit */ 339 break; 340 case 5: 341 m[line++] = 0xbf; /* line 5 is always 10111111 */ 342 mbit = 128; /* go on with the 7th bit */ 343 break; 344 } 345 if (line >= mlen) { 346 printk(KERN_WARNING "isdn_v110 (EncodeMatrix): buffer full!\n"); 347 return line; 348 } 349 next_bit: 350 switch (mbit) { /* leftmost or rightmost bit ? */ 351 case 1: 352 line++; /* rightmost -> go to next line */ 353 if (line >= mlen) { 354 printk(KERN_WARNING "isdn_v110 (EncodeMatrix): buffer full!\n"); 355 return line; 356 } 357 case 128: 358 m[line] = 128; /* leftmost -> set byte to 1000000 */ 359 mbit = 64; /* current bit in the matrix line */ 360 continue; 361 } 362 if (introducer) { /* set 110 sequence ? */ 363 introducer--; /* set on digit less */ 364 m[line] |= ibit[introducer] ? mbit : 0; /* set corresponding bit */ 365 mbit >>= 1; /* bit of matrix line >> 1 */ 366 goto next_bit; /* and go on there */ 367 } /* else push data bits into the matrix! */ 368 m[line] |= (buf[i] & dbit) ? mbit : 0; /* set data bit in matrix */ 369 if (dbit == 128) { /* was it the last one? */ 370 dbit = 1; /* then go on with first bit of */ 371 i++; /* next byte in input buffer */ 372 if (i < len) /* input buffer done ? */ 373 introducer = 3; /* no, write introducer 110 */ 374 else { /* input buffer done ! */ 375 m[line] |= (mbit - 1) & 0xfe; /* set remaining bits in line to 1 */ 376 break; 377 } 378 } else /* not the last data bit */ 379 dbit <<= 1; /* then go to next data bit */ 380 mbit >>= 1; /* go to next bit of matrix */ 381 goto next_bit; 382 383 } 384 /* if necessary, generate remaining lines of the matrix... */ 385 if ((line) && ((line + 10) < mlen)) 386 switch (++line % 10) { 387 case 1: 388 m[line++] = 0xfe; 389 case 2: 390 m[line++] = 0xfe; 391 case 3: 392 m[line++] = 0xfe; 393 case 4: 394 m[line++] = 0xfe; 395 case 5: 396 m[line++] = 0xbf; 397 case 6: 398 m[line++] = 0xfe; 399 case 7: 400 m[line++] = 0xfe; 401 case 8: 402 m[line++] = 0xfe; 403 case 9: 404 m[line++] = 0xfe; 405 } 406 return line; /* that's how many lines we have */ 407} 408 409/* 410 * Build a sync frame. 411 */ 412static struct sk_buff * 413isdn_v110_sync(isdn_v110_stream *v) 414{ 415 struct sk_buff *skb; 416 417 if (v == NULL) { 418 /* invalid handle, no chance to proceed */ 419 printk(KERN_WARNING "isdn_v110_sync called with NULL stream!\n"); 420 return NULL; 421 } 422 if ((skb = dev_alloc_skb(v->framelen + v->skbres))) { 423 skb_reserve(skb, v->skbres); 424 memcpy(skb_put(skb, v->framelen), v->OfflineFrame, v->framelen); 425 } 426 return skb; 427} 428 429/* 430 * Build an idle frame. 431 */ 432static struct sk_buff * 433isdn_v110_idle(isdn_v110_stream *v) 434{ 435 struct sk_buff *skb; 436 437 if (v == NULL) { 438 /* invalid handle, no chance to proceed */ 439 printk(KERN_WARNING "isdn_v110_sync called with NULL stream!\n"); 440 return NULL; 441 } 442 if ((skb = dev_alloc_skb(v->framelen + v->skbres))) { 443 skb_reserve(skb, v->skbres); 444 memcpy(skb_put(skb, v->framelen), v->OnlineFrame, v->framelen); 445 } 446 return skb; 447} 448 449struct sk_buff * 450isdn_v110_encode(isdn_v110_stream * v, struct sk_buff *skb) 451{ 452 int i; 453 int j; 454 int rlen; 455 int mlen; 456 int olen; 457 int size; 458 int sval1; 459 int sval2; 460 int nframes; 461 unsigned char *v110buf; 462 unsigned char *rbuf; 463 struct sk_buff *nskb; 464 465 if (v == NULL) { 466 /* invalid handle, no chance to proceed */ 467 printk(KERN_WARNING "isdn_v110_encode called with NULL stream!\n"); 468 return NULL; 469 } 470 if (!skb) { 471 /* invalid skb, no chance to proceed */ 472 printk(KERN_WARNING "isdn_v110_encode called with NULL skb!\n"); 473 return NULL; 474 } 475 rlen = skb->len; 476 nframes = (rlen + 3) / 4; 477 v110buf = v->encodebuf; 478 if ((nframes * 40) > v->maxsize) { 479 size = v->maxsize; 480 rlen = v->maxsize / 40; 481 } else 482 size = nframes * 40; 483 if (!(nskb = dev_alloc_skb(size + v->skbres + sizeof(int)))) { 484 printk(KERN_WARNING "isdn_v110_encode: Couldn't alloc skb\n"); 485 return NULL; 486 } 487 skb_reserve(nskb, v->skbres + sizeof(int)); 488 if (skb->len == 0) { 489 memcpy(skb_put(nskb, v->framelen), v->OnlineFrame, v->framelen); 490 *((int *)skb_push(nskb, sizeof(int))) = 0; 491 return nskb; 492 } 493 mlen = EncodeMatrix(skb->data, rlen, v110buf, size); 494 /* now distribute 2 or 4 bits each to the output stream! */ 495 rbuf = skb_put(nskb, size); 496 olen = 0; 497 sval1 = 8 - v->nbits; 498 sval2 = v->key << sval1; 499 for (i = 0; i < mlen; i++) { 500 v110buf[i] = FlipBits(v110buf[i], v->nbits); 501 for (j = 0; j < v->nbytes; j++) { 502 if (size--) 503 *rbuf++ = ~v->key | (((v110buf[i] << (j * v->nbits)) & sval2) >> sval1); 504 else { 505 printk(KERN_WARNING "isdn_v110_encode: buffers full!\n"); 506 goto buffer_full; 507 } 508 olen++; 509 } 510 } 511buffer_full: 512 skb_trim(nskb, olen); 513 *((int *)skb_push(nskb, sizeof(int))) = rlen; 514 return nskb; 515} 516 517int 518isdn_v110_stat_callback(int idx, isdn_ctrl *c) 519{ 520 isdn_v110_stream *v = NULL; 521 int i; 522 int ret = 0; 523 524 if (idx < 0) 525 return 0; 526 switch (c->command) { 527 case ISDN_STAT_BSENT: 528 /* Keep the send-queue of the driver filled 529 * with frames: 530 * If number of outstanding frames < 3, 531 * send down an Idle-Frame (or an Sync-Frame, if 532 * v->SyncInit != 0). 533 */ 534 if (!(v = dev->v110[idx])) 535 return 0; 536 atomic_inc(&dev->v110use[idx]); 537 for (i=0; i * v->framelen < c->parm.length; i++) { 538 if (v->skbidle > 0) { 539 v->skbidle--; 540 ret = 1; 541 } else { 542 if (v->skbuser > 0) 543 v->skbuser--; 544 ret = 0; 545 } 546 } 547 for (i = v->skbuser + v->skbidle; i < 2; i++) { 548 struct sk_buff *skb; 549 if (v->SyncInit > 0) 550 skb = isdn_v110_sync(v); 551 else 552 skb = isdn_v110_idle(v); 553 if (skb) { 554 if (dev->drv[c->driver]->interface->writebuf_skb(c->driver, c->arg, 1, skb) <= 0) { 555 dev_kfree_skb(skb); 556 break; 557 } else { 558 if (v->SyncInit) 559 v->SyncInit--; 560 v->skbidle++; 561 } 562 } else 563 break; 564 } 565 atomic_dec(&dev->v110use[idx]); 566 return ret; 567 case ISDN_STAT_DHUP: 568 case ISDN_STAT_BHUP: 569 while (1) { 570 atomic_inc(&dev->v110use[idx]); 571 if (atomic_dec_and_test(&dev->v110use[idx])) { 572 isdn_v110_close(dev->v110[idx]); 573 dev->v110[idx] = NULL; 574 break; 575 } 576 mdelay(1); 577 } 578 break; 579 case ISDN_STAT_BCONN: 580 if (dev->v110emu[idx] && (dev->v110[idx] == NULL)) { 581 int hdrlen = dev->drv[c->driver]->interface->hl_hdrlen; 582 int maxsize = dev->drv[c->driver]->interface->maxbufsize; 583 atomic_inc(&dev->v110use[idx]); 584 switch (dev->v110emu[idx]) { 585 case ISDN_PROTO_L2_V11096: 586 dev->v110[idx] = isdn_v110_open(V110_9600, hdrlen, maxsize); 587 break; 588 case ISDN_PROTO_L2_V11019: 589 dev->v110[idx] = isdn_v110_open(V110_19200, hdrlen, maxsize); 590 break; 591 case ISDN_PROTO_L2_V11038: 592 dev->v110[idx] = isdn_v110_open(V110_38400, hdrlen, maxsize); 593 break; 594 default:; 595 } 596 if ((v = dev->v110[idx])) { 597 while (v->SyncInit) { 598 struct sk_buff *skb = isdn_v110_sync(v); 599 if (dev->drv[c->driver]->interface->writebuf_skb(c->driver, c->arg, 1, skb) <= 0) { 600 dev_kfree_skb(skb); 601 /* Unable to send, try later */ 602 break; 603 } 604 v->SyncInit--; 605 v->skbidle++; 606 } 607 } else 608 printk(KERN_WARNING "isdn_v110: Couldn't open stream for chan %d\n", idx); 609 atomic_dec(&dev->v110use[idx]); 610 } 611 break; 612 default: 613 return 0; 614 } 615 return 0; 616} 617