Index: linux/drivers/dahdi/dahdi_dynamic.c =================================================================== --- linux/drivers/dahdi/dahdi_dynamic.c (.../https://fsn.dnsalias.com/svn/comma/dahdi/tags/dahdi-2.4.1/linux/drivers/dahdi/dahdi_dynamic.c) (revision 5261) +++ linux/drivers/dahdi/dahdi_dynamic.c (.../linux/drivers/dahdi/dahdi_dynamic.c) (working copy) @@ -34,6 +34,8 @@ #include #include +#include +#include /* * Tasklets provide better system interactive response at the cost of the @@ -44,7 +46,11 @@ */ #define ENABLE_TASKLETS +#define DAHDI_COMMA_EC 1 +#define DAHDI_COMMA_SIG 1 +#define COMMA_MAX_CHAN_PER_SPAN 32 + /* * Dynamic spans implemented using TDM over X with standard message * types. Message format is as follows: @@ -109,7 +115,15 @@ int timing; int master; unsigned char *msgbuf; - + struct kfifo *rxfifo; + unsigned int slip; + unsigned int skip; + unsigned int rxnuerr; + int locked; +#ifdef DAHDI_COMMA_EC + int comma_flags; + struct dahdi_echocan_state ec[COMMA_MAX_CHAN_PER_SPAN]; /* echocan state for each channel */ +#endif struct list_head list; }; @@ -125,6 +139,9 @@ static LIST_HEAD(driver_list); static int debug = 0; +static int rxfifo = 2; +static int tasklet = 1; +static int noflood = 1; static int hasmaster = 0; @@ -133,12 +150,17 @@ int newhasmaster=0; int best = 9999999; struct dahdi_dynamic *z, *master=NULL; + struct dahdi_dynamic *dspans = NULL; rcu_read_lock(); list_for_each_entry_rcu(z, &dspan_list, list) { + if (dspans == NULL) + dspans = z; if (z->timing) { z->master = 0; + z->span.flags &= ~DAHDI_FLAG_RUNNING; + z->span.syncsrc = 0; if (!(z->span.alarms & DAHDI_ALARM_RED) && (z->timing < best) && !z->dead) { /* If not in alarm and they're @@ -150,17 +172,29 @@ } } + rcu_read_unlock(); + + if (master && !hasmaster) + printk(KERN_DEBUG "TDMoX: New master: %s\n", master->span.name); + else if (!master && hasmaster) + printk(KERN_DEBUG "TDMoX: No master\n"); hasmaster = newhasmaster; /* Mark the new master if there is one */ - if (master) + /* Mark the new master if there is one */ + if (master) { + //Only this SPAN of dynamic SPANS can be a master for zaptel ! + master->span.flags |= DAHDI_FLAG_RUNNING; + master->span.syncsrc = master->span.spanno; master->master = 1; + // force dahdi timing to wake up! + master->span.lastalarms = DAHDI_ALARM_RED; + dahdi_alarm_notify(&(master->span)); + } else if (dspans) { + // force dahdi timing to wake up - let it select another master! + dspans->span.lastalarms = 0; + dahdi_alarm_notify(&(dspans->span)); + } - rcu_read_unlock(); - - if (master) - printk(KERN_INFO "TDMoX: New master: %s\n", master->span.name); - else - printk(KERN_INFO "TDMoX: No master.\n"); } static void ztd_sendmessage(struct dahdi_dynamic *z) @@ -223,25 +257,227 @@ } +#ifdef DAHDI_COMMA_EC + +#define ECHO_STATE_IDLE (0) +#define ECHO_STATE_ACTIVE (5) +#define DAHDI_COMMA_MAX_MSG 8 +/* this flag set indicates a comma device that can accept messages */ +#define ZTD_FLAG_COMMA (1 << 6) +/* this flag set indicates an outgoing message frame */ +#define ZTD_FLAG_MESSAGE (1 << 7) +#define COMMA_FLAG_MESSAGE (1 << 0) +#define COMMA_MESSAGE_EC 1 + +struct comma_msg { + unsigned short type; + unsigned short chan; + unsigned short eclen; + /* TODO: other EC parameters */ +}; + +static void comma_sendmessage(struct dahdi_dynamic *z, unsigned char* msg, int len) +{ + int msglen = 0; + unsigned char *buf = z->msgbuf; + + /* Byte 0: 0 */ + *buf = 0; + buf++; msglen++; + + /* Byte 1: Flags */ + *buf = ZTD_FLAG_MESSAGE; + buf++; msglen++; + + /* Bytes 2-3: 0 */ + *((unsigned short *)buf) = 0; + buf++; msglen++; + buf++; msglen++; + + /* Bytes 4-5: 0 */ + *((unsigned short *)buf) = 0; + buf++; msglen++; + buf++; msglen++; + + memcpy(buf, msg, len); + msglen += len; + + z->driver->transmit(z->pvt, z->msgbuf, msglen); +} + +void comma_echocan_nlp_toggle(struct dahdi_echocan_state *ec, unsigned int enable) { + // TODO: do it +} + +void comma_echocan_free(struct dahdi_chan *chan, struct dahdi_echocan_state *ec) { + unsigned long flags; + struct comma_msg msg; + struct dahdi_dynamic *z = chan->pvt; + msg.type = htons(COMMA_MESSAGE_EC); + msg.chan = htons(chan->chanpos); + msg.eclen = 0; + if (z->comma_flags & COMMA_FLAG_MESSAGE) { + printk(KERN_DEBUG "comma_echocan: channel=%d disabled sending...\n", chan->chanpos); + spin_lock_irqsave(&driver_lock, flags); + comma_sendmessage(chan->pvt, (unsigned char*)&msg, sizeof(msg)); + spin_unlock_irqrestore(&driver_lock, flags); + } +} + +static const struct dahdi_echocan_features comma_ec_features = { + .NLP_toggle = 1 +}; + +static const struct dahdi_echocan_ops comma_ec_ops = { + .name = "CommaEC", + .echocan_free = comma_echocan_free, + .echocan_NLP_toggle = comma_echocan_nlp_toggle +}; + +static int comma_echocan_create(struct dahdi_chan *chan, struct dahdi_echocanparams *ecp, + struct dahdi_echocanparam *p, struct dahdi_echocan_state **ec) { + unsigned long flags; + struct comma_msg msg; + struct dahdi_dynamic *z = chan->pvt; + if (ecp->param_count > 0) { + printk(KERN_WARNING "Comma echo canceller does not support parameters; failing request\n"); + return -EINVAL; + } + //spin_lock_irqsave(&chan->lock, flags); + *ec = &z->ec[chan->chanpos-1]; + (*ec)->ops = &comma_ec_ops; + (*ec)->features = comma_ec_features; + //spin_unlock_irqrestore(&chan->lock, flags); + msg.type = htons(COMMA_MESSAGE_EC); + msg.chan = htons(chan->chanpos); + msg.eclen = htons(ecp->tap_length); + if (z->comma_flags & COMMA_FLAG_MESSAGE) { + printk(KERN_DEBUG "comma_echocan: channel=%d eclen=%d sending...\n", chan->chanpos, ecp->tap_length); + spin_lock_irqsave(&driver_lock, flags); + comma_sendmessage(chan->pvt, (unsigned char*)&msg, sizeof(msg)); + spin_unlock_irqrestore(&driver_lock, flags); + } + + return 0; +} + +#if 0 +// TODO: well, officially, fax does not send phase reversals precisely because it does not *WANT* EC disabled! +/* Non phase-reversal version of EC disable, for fax */ +static inline int echo_can_disable_detector_update_fax(echo_can_disable_detector_state_t *det, + int16_t amp) +{ + int16_t notched; + + notched = biquad2 (&det->notch, amp); + /* Estimate the overall energy in the channel, and the energy in + the notch (i.e. overall channel energy - tone energy => noise). + Use abs instead of multiply for speed (is it really faster?). + Damp the overall energy a little more for a stable result. + Damp the notch energy a little less, so we don't damp out the + blip every time the phase reverses */ + det->channel_level += ((abs(amp) - det->channel_level) >> 5); + det->notch_level += ((abs(notched) - det->notch_level) >> 4); + if (det->channel_level > 280) { + /* There is adequate energy in the channel. Is it mostly at 2100Hz? */ + if (det->notch_level*6 < det->channel_level) { + if (det->tone_present) { + if (det->tone_cycle_duration >= 425*8) { + det->hit = TRUE; + } else { + det->tone_cycle_duration++; + } + } + det->tone_present = TRUE; + } else { + det->tone_present = FALSE; + det->tone_cycle_duration = 0; + } + } else { + det->tone_present = FALSE; + det->tone_cycle_duration = 0; + det->good_cycles = 0; + } + return det->hit; +} + + +static int dahdi_dynamic_faxdetect(struct dahdi_span *span) +{ + int x,y; + short putlin; + short getlin; + struct dahdi_chan* chan; + for (y=0;ychannels;y++) { + chan = span->chans[y]; + if (chan->echostate == ECHO_STATE_IDLE) + continue; + for (x=0;xchans[y]->writechunk[x], span->chans[y]); + if (echo_can_disable_detector_update_fax(&chan->txecdis, chan->getlin[x])) { + comma_echocan(chan, 0); + printk(KERN_INFO "Disabled echo canceller because of tone (tx) on channel %d\n", chan->channo); + break; + } + //putlin = DAHDI_XLAW(span->chans[y]->readchunk[x], span->chans[y]); + if (echo_can_disable_detector_update_fax(&chan->rxecdis, chan->putlin[x])) { + comma_echocan(chan, 0); + printk(KERN_INFO "Disabled echo canceller because of tone (rx) on channel %d\n", chan->channo); + break; + } + } + } + return 0; +} +#endif // 0 +#endif // DAHDI_COMMA_EC + +static void dahdi_process_rxfifo (struct dahdi_dynamic *z) { + unsigned char *msg = z->msgbuf; + int x = z->span.channels * DAHDI_CHUNKSIZE; + + if (__kfifo_get(z->rxfifo, msg, x) != x) { + ++z->slip; + return; + } + + for (x=0;xspan.channels;x++) { + memcpy(z->span.chans[x]->readchunk, msg, DAHDI_CHUNKSIZE); + msg += DAHDI_CHUNKSIZE; + } +} + static void __ztdynamic_run(void) { struct dahdi_dynamic *z; struct dahdi_dynamic_driver *drv; - int y; + //int y; rcu_read_lock(); list_for_each_entry_rcu(z, &dspan_list, list) { if (!z->dead) { + dahdi_process_rxfifo (z); +#ifndef DAHDI_COMMA_EC for (y=0;yspan.channels;y++) { /* Echo cancel double buffered data */ dahdi_ec_chunk(z->span.chans[y], z->span.chans[y]->readchunk, z->span.chans[y]->writechunk); } +#endif dahdi_receive(&z->span); dahdi_transmit(&z->span); /* Handle all transmissions now */ + if (noflood && (z->span.alarms & DAHDI_ALARM_RED)) { + /* Flood protection: just send a probe twice once a second during red alarm */ + z->txcnt++; + if ((z->txcnt % 512) == 0) { ztd_sendmessage(z); } + } else { + ztd_sendmessage(z); } + } + } list_for_each_entry_rcu(drv, &driver_list, list) { /* Flush any traffic still pending in the driver */ @@ -255,7 +491,9 @@ #ifdef ENABLE_TASKLETS static void ztdynamic_run(void) { - if (likely(!taskletpending)) { + if (!tasklet) { + __ztdynamic_run(); + } else if (likely(!taskletpending)) { taskletpending = 1; taskletsched++; tasklet_hi_schedule(&ztd_tlet); @@ -283,13 +521,16 @@ int newalarm; unsigned short rxpos, rxcnt; + if (ztd->locked) + return; + rcu_read_lock(); if (unlikely(msglen < 6)) { rcu_read_unlock(); newerr = ERR_LEN; if (newerr != ztd->err) { - printk(KERN_NOTICE "Span %s: Insufficient samples for header (only %d)\n", span->name, msglen); + printk(KERN_DEBUG "Span %s: Insufficient samples for header (only %d)\n", span->name, msglen); } ztd->err = newerr; return; @@ -300,7 +541,7 @@ rcu_read_unlock(); newerr = ERR_NSAMP | msg[0]; if (newerr != ztd->err) { - printk(KERN_NOTICE "Span %s: Expected %d samples, but receiving %d\n", span->name, DAHDI_CHUNKSIZE, msg[0]); + printk(KERN_DEBUG "Span %s: Expected %d samples, but receiving %d\n", span->name, DAHDI_CHUNKSIZE, msg[0]); } ztd->err = newerr; return; @@ -318,7 +559,7 @@ rcu_read_unlock(); newerr = ERR_NCHAN | nchans; if (newerr != ztd->err) { - printk(KERN_NOTICE "Span %s: Expected %d channels, but receiving %d\n", span->name, span->channels, nchans); + printk(KERN_DEBUG "Span %s: Expected %d channels, but receiving %d\n", span->name, span->channels, nchans); } ztd->err = newerr; return; @@ -343,7 +584,7 @@ rcu_read_unlock(); newerr = ERR_LEN | xlen; if (newerr != ztd->err) { - printk(KERN_NOTICE "Span %s: Expected message size %d, but was %d instead\n", span->name, xlen, msglen); + printk(KERN_DEBUG "Span %s: Expected message size %d, but was %d instead\n", span->name, xlen, msglen); } ztd->err = newerr; return; @@ -363,6 +604,20 @@ /* Pick the right bits */ sig = (bits >> ((x % 4) << 2)) & 0xff; +#if DAHDI_COMMA_SIG + { // for scoped definition below + /* Comma will indicate line power lost by setting only the D bit */ + struct dahdi_chan* chan = span->chans[x]; + if (sig == DAHDI_DBIT) { + sig = DAHDI_BITS_BD; /* indicate on hook, i.e. not ringing */ + if (chan->chan_alarms != DAHDI_ALARM_RED) + dahdi_alarm_channel(chan, DAHDI_ALARM_RED); + } else { + if (chan->chan_alarms != DAHDI_ALARM_NONE) + dahdi_alarm_channel(chan, DAHDI_ALARM_NONE); + } + } +#endif /* Update signalling if appropriate */ if (sig != span->chans[x]->rxsig) @@ -372,10 +627,11 @@ } /* Record data for channels */ - for (x=0;xchans[x]->readchunk, msg, DAHDI_CHUNKSIZE); - msg += DAHDI_CHUNKSIZE; - } + x = nchans*DAHDI_CHUNKSIZE; + // kfifo_alloc is aligned by ^2 bytes. Make sure we have enogth space in fifo for the whole packet. + if (ztd->rxfifo->size >= __kfifo_len (ztd->rxfifo) + x) + __kfifo_put(ztd->rxfifo, msg, x); + else ++ztd->skip; master = ztd->master; @@ -394,14 +650,28 @@ if (newalarm != span->alarms) { span->alarms = newalarm; + if (newalarm & DAHDI_ALARM_RED) { + span->flags &= ~DAHDI_FLAG_RUNNING; +#ifdef DAHDI_COMMA_EC + ztd->comma_flags &= ~COMMA_FLAG_MESSAGE; +#endif + } dahdi_alarm_notify(span); checkmaster(); } /* note if we had a missing packet */ if (unlikely(rxpos != rxcnt)) - printk(KERN_NOTICE "Span %s: Expected seq no %d, but received %d instead\n", span->name, rxcnt, rxpos); + printk(KERN_DEBUG "Span %s: Expected seq no %d, but received %d instead\n", span->name, rxcnt, rxpos); +#ifdef DAHDI_COMMA_EC + if (sflags & ZTD_FLAG_COMMA) { + ztd->comma_flags |= COMMA_FLAG_MESSAGE; + } else { + ztd->comma_flags &= ~COMMA_FLAG_MESSAGE; + } +#endif + /* If this is our master span, then run everything */ if (master) ztdynamic_run(); @@ -428,6 +698,10 @@ kfree(z->chans[x]); } + /* Free rxfifo */ + if (z->rxfifo) + kfree (z->rxfifo); + /* Free z */ kfree(z); @@ -587,6 +861,11 @@ /* Zero out -- probably not needed but why not */ memset(z->msgbuf, 0, bufsize); + // Allocate rx fifo + z->rxfifo = kfifo_alloc ((zds->numchans * DAHDI_CHUNKSIZE) * rxfifo, + GFP_KERNEL, NULL); + + /* Setup parameters properly assuming we're going to be okay. */ dahdi_copy_string(z->dname, zds->driver, sizeof(z->dname)); dahdi_copy_string(z->addr, zds->addr, sizeof(z->addr)); @@ -633,6 +912,9 @@ } /* Create the stuff */ + spin_lock_irqsave(&driver_lock, flags); + z->locked = 1; + spin_unlock_irqrestore(&driver_lock, flags); z->pvt = ztd->create(&z->span, z->addr); if (!z->pvt) { printk(KERN_NOTICE "Driver '%s' (%s) rejected address '%s'\n", ztd->name, ztd->desc, z->addr); @@ -652,6 +934,7 @@ spin_lock_irqsave(&dspan_lock, flags); list_add_rcu(&z->list, &dspan_list); + z->locked = 0; spin_unlock_irqrestore(&dspan_lock, flags); checkmaster(); @@ -772,15 +1055,100 @@ } rcu_read_unlock(); - if (alarmchanged) + if (alarmchanged || !hasmaster) checkmaster(); /* Do the next one */ mod_timer(&alarmcheck, jiffies + 1 * HZ); } +#ifdef CONFIG_PROC_FS +static struct proc_dir_entry *proc_entry; +static const char *ztd_procname="dahdi/dahdi_dynamic_stats"; +static int dahdi_dynamic_proc_read(char *page, char **start, off_t off, int count, int *eof, void *data) +{ + int len = 0; + unsigned long flags; + struct dahdi_dynamic *z; + + /* In Linux 2.6, this MUST NOT EXECEED 4096 bytes in one read! */ + + len = sprintf(page+len, "dahdi_dynamic info/statistics\n"); + +#ifdef ENABLE_TASKLETS + len += sprintf(page+len, "\ttaskletrun: %d\n", taskletrun); + len += sprintf(page+len, "\ttaskletsched: %d\n", taskletsched); + len += sprintf(page+len, "\ttaskletexec: %d\n", taskletexec); + len += sprintf(page+len, "\ttxerrors: %d\n", txerrors); + len += sprintf(page+len, "\ttaskletpending: %d\n", taskletpending); +#endif + + len += sprintf(page + len, "\n"); + spin_lock_irqsave(&driver_lock, flags); + list_for_each_entry_rcu(z, &dspan_list, list) { + struct dahdi_span *span = &(z->span); + if (span->name) + len += sprintf(page + len, "Span %d: %s ", span->spanno, span->name); + if (span->desc) + len += sprintf(page + len, "\"%s\"", span->desc); + else + len += sprintf(page + len, "\"\""); + + len += sprintf(page + len, " "); + if (z->master) + len += sprintf(page + len, "ClockSource "); + len += sprintf(page + len, "\n"); + + len += sprintf(page+len, "\tslip: %d", z->slip); + len += sprintf(page+len, ", skip: %d", z->skip); + len += sprintf(page+len, ", rxnuerr: %d", z->rxnuerr); + len += sprintf(page+len, ", rxfifo: %d", __kfifo_len(z->rxfifo)/span->channels/DAHDI_CHUNKSIZE); + len += sprintf(page + len, "\n"); + + if (len <= off) { /* If everything printed so far is before beginning of request */ + off -= len; + len = 0; + } + if (len > off+count) /* stop if we've already generated enough */ + break; + } + spin_unlock_irqrestore(&driver_lock, flags); + + if (len <= off) { /* If everything printed so far is before beginning of request */ + off -= len; + len = 0; + } + *start = page + off; + len -= off; /* un-count any remaining offset */ + if (len > count) + len = count; /* don't return bytes not asked for */ + return len; +} +#endif //CONFIG_PROC_FS + +static void proc_init (void) { +#ifdef CONFIG_PROC_FS + proc_entry = create_proc_read_entry(ztd_procname, 0444, NULL , dahdi_dynamic_proc_read, NULL); +#endif //CONFIG_PROC_FS +} +static void proc_cleanup (void) { +#ifdef CONFIG_PROC_FS + remove_proc_entry(ztd_procname, NULL); +#endif //CONFIG_PROC_FS +} + +#ifdef DAHDI_COMMA_EC +static const struct dahdi_echocan_factory comma_ec_factory = { + .name = "COMMA", + .owner = THIS_MODULE, + .echocan_create = comma_echocan_create, +}; +#endif + static int ztdynamic_init(void) { + rxfifo = max(rxfifo, 1); + proc_init (); dahdi_set_dynamic_ioctl(ztdynamic_ioctl); /* Start process to check for RED ALARM */ @@ -791,26 +1159,46 @@ /* Check once per second */ mod_timer(&alarmcheck, jiffies + 1 * HZ); #ifdef ENABLE_TASKLETS + if (tasklet) tasklet_init(&ztd_tlet, ztd_tasklet, 0); #endif - printk(KERN_INFO "DAHDI Dynamic Span support LOADED\n"); +#ifdef DAHDI_COMMA_EC + if (dahdi_register_echocan_factory(&comma_ec_factory)) { + printk(KERN_ERR "could not register Comma EC with DAHDI core\n"); + + return -EPERM; + } + + printk(KERN_INFO "Registered echo canceler '%s'\n", comma_ec_factory.name); +#endif + printk(KERN_INFO "DAHDI Dynamic Span support LOADED, rxfifo=%d tasklet=%d\n", rxfifo, tasklet); return 0; } static void ztdynamic_cleanup(void) { #ifdef ENABLE_TASKLETS - if (taskletpending) { + if (tasklet && taskletpending) { tasklet_disable(&ztd_tlet); tasklet_kill(&ztd_tlet); } #endif +#ifdef DAHDI_COMMA_EC + dahdi_unregister_echocan_factory(&comma_ec_factory); +#endif dahdi_set_dynamic_ioctl(NULL); del_timer(&alarmcheck); + proc_cleanup (); printk(KERN_INFO "DAHDI Dynamic Span support unloaded\n"); } + + module_param(debug, int, 0600); +module_param(rxfifo, int, 0600); +module_param(tasklet, int, 0600); +module_param(noflood, int, 0600); +MODULE_PARM_DESC(rxfifo, "RX fifo size (milliseconds)."); MODULE_DESCRIPTION("DAHDI Dynamic Span Support"); MODULE_AUTHOR("Mark Spencer ");