diff options
Diffstat (limited to 'drivers/isdn/mISDN')
30 files changed, 14828 insertions, 0 deletions
diff --git a/drivers/isdn/mISDN/Kconfig b/drivers/isdn/mISDN/Kconfig new file mode 100644 index 000000000000..4938355c4072 --- /dev/null +++ b/drivers/isdn/mISDN/Kconfig @@ -0,0 +1,44 @@ +# +# modularer ISDN driver +# + +menuconfig MISDN + tristate "Modular ISDN driver" + help + Enable support for the modular ISDN driver. + +if MISDN != n + +config MISDN_DSP + tristate "Digital Audio Processing of transparent data" + depends on MISDN + help + Enable support for digital audio processing capability. + This module may be used for special applications that require + cross connecting of bchannels, conferencing, dtmf decoding + echo cancelation, tone generation, and Blowfish encryption and + decryption. + It may use hardware features if available. + E.g. it is required for PBX4Linux. Go to http://isdn.eversberg.eu + and get more informations about this module and it's usage. + If unsure, say 'N'. + +config MISDN_L1OIP + tristate "ISDN over IP tunnel" + depends on MISDN + help + Enable support for ISDN over IP tunnel. + + It features: + - dynamic IP exchange, if one or both peers have dynamic IPs + - BRI (S0) and PRI (S2M) interface + - layer 1 control via network keepalive frames + - direct tunneling of physical interface via IP + + NOTE: This protocol is called 'Layer 1 over IP' and is not + compatible with ISDNoIP (Agfeo) or TDMoIP. Protocol description is + provided in the source code. + +source "drivers/isdn/hardware/mISDN/Kconfig" + +endif #MISDN diff --git a/drivers/isdn/mISDN/Makefile b/drivers/isdn/mISDN/Makefile new file mode 100644 index 000000000000..1cb5e633cf75 --- /dev/null +++ b/drivers/isdn/mISDN/Makefile @@ -0,0 +1,13 @@ +# +# Makefile for the modular ISDN driver +# + +obj-$(CONFIG_MISDN) += mISDN_core.o +obj-$(CONFIG_MISDN_DSP) += mISDN_dsp.o +obj-$(CONFIG_MISDN_L1OIP) += l1oip.o + +# multi objects + +mISDN_core-objs := core.o fsm.o socket.o hwchannel.o stack.o layer1.o layer2.o tei.o timerdev.o +mISDN_dsp-objs := dsp_core.o dsp_cmx.o dsp_tones.o dsp_dtmf.o dsp_audio.o dsp_blowfish.o dsp_pipeline.o dsp_hwec.o +l1oip-objs := l1oip_core.o l1oip_codec.o diff --git a/drivers/isdn/mISDN/core.c b/drivers/isdn/mISDN/core.c new file mode 100644 index 000000000000..33068177b7c9 --- /dev/null +++ b/drivers/isdn/mISDN/core.c @@ -0,0 +1,244 @@ +/* + * Copyright 2008 by Karsten Keil <kkeil@novell.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#include <linux/types.h> +#include <linux/stddef.h> +#include <linux/module.h> +#include <linux/spinlock.h> +#include <linux/mISDNif.h> +#include "core.h" + +static u_int debug; + +MODULE_AUTHOR("Karsten Keil"); +MODULE_LICENSE("GPL"); +module_param(debug, uint, S_IRUGO | S_IWUSR); + +static LIST_HEAD(devices); +DEFINE_RWLOCK(device_lock); +static u64 device_ids; +#define MAX_DEVICE_ID 63 + +static LIST_HEAD(Bprotocols); +DEFINE_RWLOCK(bp_lock); + +struct mISDNdevice +*get_mdevice(u_int id) +{ + struct mISDNdevice *dev; + + read_lock(&device_lock); + list_for_each_entry(dev, &devices, D.list) + if (dev->id == id) { + read_unlock(&device_lock); + return dev; + } + read_unlock(&device_lock); + return NULL; +} + +int +get_mdevice_count(void) +{ + struct mISDNdevice *dev; + int cnt = 0; + + read_lock(&device_lock); + list_for_each_entry(dev, &devices, D.list) + cnt++; + read_unlock(&device_lock); + return cnt; +} + +static int +get_free_devid(void) +{ + u_int i; + + for (i = 0; i <= MAX_DEVICE_ID; i++) + if (!test_and_set_bit(i, (u_long *)&device_ids)) + return i; + return -1; +} + +int +mISDN_register_device(struct mISDNdevice *dev, char *name) +{ + u_long flags; + int err; + + dev->id = get_free_devid(); + if (dev->id < 0) + return -EBUSY; + if (name && name[0]) + strcpy(dev->name, name); + else + sprintf(dev->name, "mISDN%d", dev->id); + if (debug & DEBUG_CORE) + printk(KERN_DEBUG "mISDN_register %s %d\n", + dev->name, dev->id); + err = create_stack(dev); + if (err) + return err; + write_lock_irqsave(&device_lock, flags); + list_add_tail(&dev->D.list, &devices); + write_unlock_irqrestore(&device_lock, flags); + return 0; +} +EXPORT_SYMBOL(mISDN_register_device); + +void +mISDN_unregister_device(struct mISDNdevice *dev) { + u_long flags; + + if (debug & DEBUG_CORE) + printk(KERN_DEBUG "mISDN_unregister %s %d\n", + dev->name, dev->id); + write_lock_irqsave(&device_lock, flags); + list_del(&dev->D.list); + write_unlock_irqrestore(&device_lock, flags); + test_and_clear_bit(dev->id, (u_long *)&device_ids); + delete_stack(dev); +} +EXPORT_SYMBOL(mISDN_unregister_device); + +u_int +get_all_Bprotocols(void) +{ + struct Bprotocol *bp; + u_int m = 0; + + read_lock(&bp_lock); + list_for_each_entry(bp, &Bprotocols, list) + m |= bp->Bprotocols; + read_unlock(&bp_lock); + return m; +} + +struct Bprotocol * +get_Bprotocol4mask(u_int m) +{ + struct Bprotocol *bp; + + read_lock(&bp_lock); + list_for_each_entry(bp, &Bprotocols, list) + if (bp->Bprotocols & m) { + read_unlock(&bp_lock); + return bp; + } + read_unlock(&bp_lock); + return NULL; +} + +struct Bprotocol * +get_Bprotocol4id(u_int id) +{ + u_int m; + + if (id < ISDN_P_B_START || id > 63) { + printk(KERN_WARNING "%s id not in range %d\n", + __func__, id); + return NULL; + } + m = 1 << (id & ISDN_P_B_MASK); + return get_Bprotocol4mask(m); +} + +int +mISDN_register_Bprotocol(struct Bprotocol *bp) +{ + u_long flags; + struct Bprotocol *old; + + if (debug & DEBUG_CORE) + printk(KERN_DEBUG "%s: %s/%x\n", __func__, + bp->name, bp->Bprotocols); + old = get_Bprotocol4mask(bp->Bprotocols); + if (old) { + printk(KERN_WARNING + "register duplicate protocol old %s/%x new %s/%x\n", + old->name, old->Bprotocols, bp->name, bp->Bprotocols); + return -EBUSY; + } + write_lock_irqsave(&bp_lock, flags); + list_add_tail(&bp->list, &Bprotocols); + write_unlock_irqrestore(&bp_lock, flags); + return 0; +} +EXPORT_SYMBOL(mISDN_register_Bprotocol); + +void +mISDN_unregister_Bprotocol(struct Bprotocol *bp) +{ + u_long flags; + + if (debug & DEBUG_CORE) + printk(KERN_DEBUG "%s: %s/%x\n", __func__, bp->name, + bp->Bprotocols); + write_lock_irqsave(&bp_lock, flags); + list_del(&bp->list); + write_unlock_irqrestore(&bp_lock, flags); +} +EXPORT_SYMBOL(mISDN_unregister_Bprotocol); + +int +mISDNInit(void) +{ + int err; + + printk(KERN_INFO "Modular ISDN core version %d.%d.%d\n", + MISDN_MAJOR_VERSION, MISDN_MINOR_VERSION, MISDN_RELEASE); + mISDN_initstack(&debug); + err = mISDN_inittimer(&debug); + if (err) + goto error; + err = l1_init(&debug); + if (err) { + mISDN_timer_cleanup(); + goto error; + } + err = Isdnl2_Init(&debug); + if (err) { + mISDN_timer_cleanup(); + l1_cleanup(); + goto error; + } + err = misdn_sock_init(&debug); + if (err) { + mISDN_timer_cleanup(); + l1_cleanup(); + Isdnl2_cleanup(); + } +error: + return err; +} + +void mISDN_cleanup(void) +{ + misdn_sock_cleanup(); + mISDN_timer_cleanup(); + l1_cleanup(); + Isdnl2_cleanup(); + + if (!list_empty(&devices)) + printk(KERN_ERR "%s devices still registered\n", __func__); + + if (!list_empty(&Bprotocols)) + printk(KERN_ERR "%s Bprotocols still registered\n", __func__); + printk(KERN_DEBUG "mISDNcore unloaded\n"); +} + +module_init(mISDNInit); +module_exit(mISDN_cleanup); + diff --git a/drivers/isdn/mISDN/core.h b/drivers/isdn/mISDN/core.h new file mode 100644 index 000000000000..7da7233b4c1a --- /dev/null +++ b/drivers/isdn/mISDN/core.h @@ -0,0 +1,77 @@ +/* + * Copyright 2008 by Karsten Keil <kkeil@novell.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#ifndef mISDN_CORE_H +#define mISDN_CORE_H + +extern struct mISDNdevice *get_mdevice(u_int); +extern int get_mdevice_count(void); + +/* stack status flag */ +#define mISDN_STACK_ACTION_MASK 0x0000ffff +#define mISDN_STACK_COMMAND_MASK 0x000f0000 +#define mISDN_STACK_STATUS_MASK 0xfff00000 +/* action bits 0-15 */ +#define mISDN_STACK_WORK 0 +#define mISDN_STACK_SETUP 1 +#define mISDN_STACK_CLEARING 2 +#define mISDN_STACK_RESTART 3 +#define mISDN_STACK_WAKEUP 4 +#define mISDN_STACK_ABORT 15 +/* command bits 16-19 */ +#define mISDN_STACK_STOPPED 16 +#define mISDN_STACK_INIT 17 +#define mISDN_STACK_THREADSTART 18 +/* status bits 20-31 */ +#define mISDN_STACK_BCHANNEL 20 +#define mISDN_STACK_ACTIVE 29 +#define mISDN_STACK_RUNNING 30 +#define mISDN_STACK_KILLED 31 + + +/* manager options */ +#define MGR_OPT_USER 24 +#define MGR_OPT_NETWORK 25 + +extern int connect_Bstack(struct mISDNdevice *, struct mISDNchannel *, + u_int, struct sockaddr_mISDN *); +extern int connect_layer1(struct mISDNdevice *, struct mISDNchannel *, + u_int, struct sockaddr_mISDN *); +extern int create_l2entity(struct mISDNdevice *, struct mISDNchannel *, + u_int, struct sockaddr_mISDN *); + +extern int create_stack(struct mISDNdevice *); +extern int create_teimanager(struct mISDNdevice *); +extern void delete_teimanager(struct mISDNchannel *); +extern void delete_channel(struct mISDNchannel *); +extern void delete_stack(struct mISDNdevice *); +extern void mISDN_initstack(u_int *); +extern int misdn_sock_init(u_int *); +extern void misdn_sock_cleanup(void); +extern void add_layer2(struct mISDNchannel *, struct mISDNstack *); +extern void __add_layer2(struct mISDNchannel *, struct mISDNstack *); + +extern u_int get_all_Bprotocols(void); +struct Bprotocol *get_Bprotocol4mask(u_int); +struct Bprotocol *get_Bprotocol4id(u_int); + +extern int mISDN_inittimer(u_int *); +extern void mISDN_timer_cleanup(void); + +extern int l1_init(u_int *); +extern void l1_cleanup(void); +extern int Isdnl2_Init(u_int *); +extern void Isdnl2_cleanup(void); + +#endif diff --git a/drivers/isdn/mISDN/dsp.h b/drivers/isdn/mISDN/dsp.h new file mode 100644 index 000000000000..6c3fed6b8d4f --- /dev/null +++ b/drivers/isdn/mISDN/dsp.h @@ -0,0 +1,263 @@ +/* + * Audio support data for ISDN4Linux. + * + * Copyright 2002/2003 by Andreas Eversberg (jolly@eversberg.eu) + * + * This software may be used and distributed according to the terms + * of the GNU General Public License, incorporated herein by reference. + * + */ + +#define DEBUG_DSP_CTRL 0x0001 +#define DEBUG_DSP_CORE 0x0002 +#define DEBUG_DSP_DTMF 0x0004 +#define DEBUG_DSP_CMX 0x0010 +#define DEBUG_DSP_TONE 0x0020 +#define DEBUG_DSP_BLOWFISH 0x0040 +#define DEBUG_DSP_DELAY 0x0100 +#define DEBUG_DSP_DTMFCOEFF 0x8000 /* heavy output */ + +/* options may be: + * + * bit 0 = use ulaw instead of alaw + * bit 1 = enable hfc hardware accelleration for all channels + * + */ +#define DSP_OPT_ULAW (1<<0) +#define DSP_OPT_NOHARDWARE (1<<1) + +#include <linux/timer.h> +#include <linux/workqueue.h> + +#include "dsp_ecdis.h" + +extern int dsp_options; +extern int dsp_debug; +extern int dsp_poll; +extern int dsp_tics; +extern spinlock_t dsp_lock; +extern struct work_struct dsp_workq; +extern u32 dsp_poll_diff; /* calculated fix-comma corrected poll value */ + +/*************** + * audio stuff * + ***************/ + +extern s32 dsp_audio_alaw_to_s32[256]; +extern s32 dsp_audio_ulaw_to_s32[256]; +extern s32 *dsp_audio_law_to_s32; +extern u8 dsp_audio_s16_to_law[65536]; +extern u8 dsp_audio_alaw_to_ulaw[256]; +extern u8 dsp_audio_mix_law[65536]; +extern u8 dsp_audio_seven2law[128]; +extern u8 dsp_audio_law2seven[256]; +extern void dsp_audio_generate_law_tables(void); +extern void dsp_audio_generate_s2law_table(void); +extern void dsp_audio_generate_seven(void); +extern void dsp_audio_generate_mix_table(void); +extern void dsp_audio_generate_ulaw_samples(void); +extern void dsp_audio_generate_volume_changes(void); +extern u8 dsp_silence; + + +/************* + * cmx stuff * + *************/ + +#define MAX_POLL 256 /* maximum number of send-chunks */ + +#define CMX_BUFF_SIZE 0x8000 /* must be 2**n (0x1000 about 1/2 second) */ +#define CMX_BUFF_HALF 0x4000 /* CMX_BUFF_SIZE / 2 */ +#define CMX_BUFF_MASK 0x7fff /* CMX_BUFF_SIZE - 1 */ + +/* how many seconds will we check the lowest delay until the jitter buffer + is reduced by that delay */ +#define MAX_SECONDS_JITTER_CHECK 5 + +extern struct timer_list dsp_spl_tl; +extern u32 dsp_spl_jiffies; + +/* the structure of conferences: + * + * each conference has a unique number, given by user space. + * the conferences are linked in a chain. + * each conference has members linked in a chain. + * each dsplayer points to a member, each member points to a dsplayer. + */ + +/* all members within a conference (this is linked 1:1 with the dsp) */ +struct dsp; +struct dsp_conf_member { + struct list_head list; + struct dsp *dsp; +}; + +/* the list of all conferences */ +struct dsp_conf { + struct list_head list; + u32 id; + /* all cmx stacks with the same ID are + connected */ + struct list_head mlist; + int software; /* conf is processed by software */ + int hardware; /* conf is processed by hardware */ + /* note: if both unset, has only one member */ +}; + + +/************** + * DTMF stuff * + **************/ + +#define DSP_DTMF_NPOINTS 102 + +#define ECHOCAN_BUFLEN (4*128) + +struct dsp_dtmf { + int treshold; /* above this is dtmf (square of) */ + int software; /* dtmf uses software decoding */ + int hardware; /* dtmf uses hardware decoding */ + int size; /* number of bytes in buffer */ + signed short buffer[DSP_DTMF_NPOINTS]; + /* buffers one full dtmf frame */ + u8 lastwhat, lastdigit; + int count; + u8 digits[16]; /* just the dtmf result */ +}; + + +/****************** + * pipeline stuff * + ******************/ +struct dsp_pipeline { + rwlock_t lock; + struct list_head list; + int inuse; +}; + +/*************** + * tones stuff * + ***************/ + +struct dsp_tone { + int software; /* tones are generated by software */ + int hardware; /* tones are generated by hardware */ + int tone; + void *pattern; + int count; + int index; + struct timer_list tl; +}; + +/***************** + * general stuff * + *****************/ + +struct dsp { + struct list_head list; + struct mISDNchannel ch; + struct mISDNchannel *up; + unsigned char name[64]; + int b_active; + int echo; /* echo is enabled */ + int rx_disabled; /* what the user wants */ + int rx_is_off; /* what the card is */ + int tx_mix; + struct dsp_tone tone; + struct dsp_dtmf dtmf; + int tx_volume, rx_volume; + + /* queue for sending frames */ + struct work_struct workq; + struct sk_buff_head sendq; + int hdlc; /* if mode is hdlc */ + int data_pending; /* currently an unconfirmed frame */ + + /* conference stuff */ + u32 conf_id; + struct dsp_conf *conf; + struct dsp_conf_member + *member; + + /* buffer stuff */ + int rx_W; /* current write pos for data without timestamp */ + int rx_R; /* current read pos for transmit clock */ + int rx_init; /* if set, pointers will be adjusted first */ + int tx_W; /* current write pos for transmit data */ + int tx_R; /* current read pos for transmit clock */ + int rx_delay[MAX_SECONDS_JITTER_CHECK]; + int tx_delay[MAX_SECONDS_JITTER_CHECK]; + u8 tx_buff[CMX_BUFF_SIZE]; + u8 rx_buff[CMX_BUFF_SIZE]; + int last_tx; /* if set, we transmitted last poll interval */ + int cmx_delay; /* initial delay of buffers, + or 0 for dynamic jitter buffer */ + int tx_dejitter; /* if set, dejitter tx buffer */ + int tx_data; /* enables tx-data of CMX to upper layer */ + + /* hardware stuff */ + struct dsp_features features; + int features_rx_off; /* set if rx_off is featured */ + int pcm_slot_rx; /* current PCM slot (or -1) */ + int pcm_bank_rx; + int pcm_slot_tx; + int pcm_bank_tx; + int hfc_conf; /* unique id of current conference (or -1) */ + + /* encryption stuff */ + int bf_enable; + u32 bf_p[18]; + u32 bf_s[1024]; + int bf_crypt_pos; + u8 bf_data_in[9]; + u8 bf_crypt_out[9]; + int bf_decrypt_in_pos; + int bf_decrypt_out_pos; + u8 bf_crypt_inring[16]; + u8 bf_data_out[9]; + int bf_sync; + + struct dsp_pipeline + pipeline; +}; + +/* functions */ + +extern void dsp_change_volume(struct sk_buff *skb, int volume); + +extern struct list_head dsp_ilist; +extern struct list_head conf_ilist; +extern void dsp_cmx_debug(struct dsp *dsp); +extern void dsp_cmx_hardware(struct dsp_conf *conf, struct dsp *dsp); +extern int dsp_cmx_conf(struct dsp *dsp, u32 conf_id); +extern void dsp_cmx_receive(struct dsp *dsp, struct sk_buff *skb); +extern void dsp_cmx_hdlc(struct dsp *dsp, struct sk_buff *skb); +extern void dsp_cmx_send(void *arg); +extern void dsp_cmx_transmit(struct dsp *dsp, struct sk_buff *skb); +extern int dsp_cmx_del_conf_member(struct dsp *dsp); +extern int dsp_cmx_del_conf(struct dsp_conf *conf); + +extern void dsp_dtmf_goertzel_init(struct dsp *dsp); +extern void dsp_dtmf_hardware(struct dsp *dsp); +extern u8 *dsp_dtmf_goertzel_decode(struct dsp *dsp, u8 *data, int len, + int fmt); + +extern int dsp_tone(struct dsp *dsp, int tone); +extern void dsp_tone_copy(struct dsp *dsp, u8 *data, int len); +extern void dsp_tone_timeout(void *arg); + +extern void dsp_bf_encrypt(struct dsp *dsp, u8 *data, int len); +extern void dsp_bf_decrypt(struct dsp *dsp, u8 *data, int len); +extern int dsp_bf_init(struct dsp *dsp, const u8 *key, unsigned int keylen); +extern void dsp_bf_cleanup(struct dsp *dsp); + +extern int dsp_pipeline_module_init(void); +extern void dsp_pipeline_module_exit(void); +extern int dsp_pipeline_init(struct dsp_pipeline *pipeline); +extern void dsp_pipeline_destroy(struct dsp_pipeline *pipeline); +extern int dsp_pipeline_build(struct dsp_pipeline *pipeline, const char *cfg); +extern void dsp_pipeline_process_tx(struct dsp_pipeline *pipeline, u8 *data, + int len); +extern void dsp_pipeline_process_rx(struct dsp_pipeline *pipeline, u8 *data, + int len); + diff --git a/drivers/isdn/mISDN/dsp_audio.c b/drivers/isdn/mISDN/dsp_audio.c new file mode 100644 index 000000000000..1c2dd5694773 --- /dev/null +++ b/drivers/isdn/mISDN/dsp_audio.c @@ -0,0 +1,434 @@ +/* + * Audio support data for mISDN_dsp. + * + * Copyright 2002/2003 by Andreas Eversberg (jolly@eversberg.eu) + * Rewritten by Peter + * + * This software may be used and distributed according to the terms + * of the GNU General Public License, incorporated herein by reference. + * + */ + +#include <linux/delay.h> +#include <linux/mISDNif.h> +#include <linux/mISDNdsp.h> +#include "core.h" +#include "dsp.h" + +/* ulaw[unsigned char] -> signed 16-bit */ +s32 dsp_audio_ulaw_to_s32[256]; +/* alaw[unsigned char] -> signed 16-bit */ +s32 dsp_audio_alaw_to_s32[256]; + +s32 *dsp_audio_law_to_s32; +EXPORT_SYMBOL(dsp_audio_law_to_s32); + +/* signed 16-bit -> law */ +u8 dsp_audio_s16_to_law[65536]; +EXPORT_SYMBOL(dsp_audio_s16_to_law); + +/* alaw -> ulaw */ +u8 dsp_audio_alaw_to_ulaw[256]; +/* ulaw -> alaw */ +u8 dsp_audio_ulaw_to_alaw[256]; +u8 dsp_silence; + + +/***************************************************** + * generate table for conversion of s16 to alaw/ulaw * + *****************************************************/ + +#define AMI_MASK 0x55 + +static inline unsigned char linear2alaw(short int linear) +{ + int mask; + int seg; + int pcm_val; + static int seg_end[8] = { + 0xFF, 0x1FF, 0x3FF, 0x7FF, 0xFFF, 0x1FFF, 0x3FFF, 0x7FFF + }; + + pcm_val = linear; + if (pcm_val >= 0) { + /* Sign (7th) bit = 1 */ + mask = AMI_MASK | 0x80; + } else { + /* Sign bit = 0 */ + mask = AMI_MASK; + pcm_val = -pcm_val; + } + + /* Convert the scaled magnitude to segment number. */ + for (seg = 0; seg < 8; seg++) { + if (pcm_val <= seg_end[seg]) + break; + } + /* Combine the sign, segment, and quantization bits. */ + return ((seg << 4) | + ((pcm_val >> ((seg) ? (seg + 3) : 4)) & 0x0F)) ^ mask; +} + + +static inline short int alaw2linear(unsigned char alaw) +{ + int i; + int seg; + + alaw ^= AMI_MASK; + i = ((alaw & 0x0F) << 4) + 8 /* rounding error */; + seg = (((int) alaw & 0x70) >> 4); + if (seg) + i = (i + 0x100) << (seg - 1); + return (short int) ((alaw & 0x80) ? i : -i); +} + +static inline short int ulaw2linear(unsigned char ulaw) +{ + short mu, e, f, y; + static short etab[] = {0, 132, 396, 924, 1980, 4092, 8316, 16764}; + + mu = 255 - ulaw; + e = (mu & 0x70) / 16; + f = mu & 0x0f; + y = f * (1 << (e + 3)); + y += etab[e]; + if (mu & 0x80) + y = -y; + return y; +} + +#define BIAS 0x84 /*!< define the add-in bias for 16 bit samples */ + +static unsigned char linear2ulaw(short sample) +{ + static int exp_lut[256] = { + 0, 0, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3, + 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, + 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, + 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, + 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, + 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, + 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, + 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, + 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, + 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, + 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, + 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, + 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, + 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, + 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, + 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7}; + int sign, exponent, mantissa; + unsigned char ulawbyte; + + /* Get the sample into sign-magnitude. */ + sign = (sample >> 8) & 0x80; /* set aside the sign */ + if (sign != 0) + sample = -sample; /* get magnitude */ + + /* Convert from 16 bit linear to ulaw. */ + sample = sample + BIAS; + exponent = exp_lut[(sample >> 7) & 0xFF]; + mantissa = (sample >> (exponent + 3)) & 0x0F; + ulawbyte = ~(sign | (exponent << 4) | mantissa); + + return ulawbyte; +} + +static int reverse_bits(int i) +{ + int z, j; + z = 0; + + for (j = 0; j < 8; j++) { + if ((i & (1 << j)) != 0) + z |= 1 << (7 - j); + } + return z; +} + + +void dsp_audio_generate_law_tables(void) +{ + int i; + for (i = 0; i < 256; i++) + dsp_audio_alaw_to_s32[i] = alaw2linear(reverse_bits(i)); + + for (i = 0; i < 256; i++) + dsp_audio_ulaw_to_s32[i] = ulaw2linear(reverse_bits(i)); + + for (i = 0; i < 256; i++) { + dsp_audio_alaw_to_ulaw[i] = + linear2ulaw(dsp_audio_alaw_to_s32[i]); + dsp_audio_ulaw_to_alaw[i] = + linear2alaw(dsp_audio_ulaw_to_s32[i]); + } +} + +void +dsp_audio_generate_s2law_table(void) +{ + int i; + + if (dsp_options & DSP_OPT_ULAW) { + /* generating ulaw-table */ + for (i = -32768; i < 32768; i++) { + dsp_audio_s16_to_law[i & 0xffff] = + reverse_bits(linear2ulaw(i)); + } + } else { + /* generating alaw-table */ + for (i = -32768; i < 32768; i++) { + dsp_audio_s16_to_law[i & 0xffff] = + reverse_bits(linear2alaw(i)); + } + } +} + + +/* + * the seven bit sample is the number of every second alaw-sample ordered by + * aplitude. 0x00 is negative, 0x7f is positive amplitude. + */ +u8 dsp_audio_seven2law[128]; +u8 dsp_audio_law2seven[256]; + +/******************************************************************** + * generate table for conversion law from/to 7-bit alaw-like sample * + ********************************************************************/ + +void +dsp_audio_generate_seven(void) +{ + int i, j, k; + u8 spl; + u8 sorted_alaw[256]; + + /* generate alaw table, sorted by the linear value */ + for (i = 0; i < 256; i++) { + j = 0; + for (k = 0; k < 256; k++) { + if (dsp_audio_alaw_to_s32[k] + < dsp_audio_alaw_to_s32[i]) { + j++; + } + } + sorted_alaw[j] = i; + } + + /* generate tabels */ + for (i = 0; i < 256; i++) { + /* spl is the source: the law-sample (converted to alaw) */ + spl = i; + if (dsp_options & DSP_OPT_ULAW) + spl = dsp_audio_ulaw_to_alaw[i]; + /* find the 7-bit-sample */ + for (j = 0; j < 256; j++) { + if (sorted_alaw[j] == spl) + break; + } + /* write 7-bit audio value */ + dsp_audio_law2seven[i] = j >> 1; + } + for (i = 0; i < 128; i++) { + spl = sorted_alaw[i << 1]; + if (dsp_options & DSP_OPT_ULAW) + spl = dsp_audio_alaw_to_ulaw[spl]; + dsp_audio_seven2law[i] = spl; + } +} + + +/* mix 2*law -> law */ +u8 dsp_audio_mix_law[65536]; + +/****************************************************** + * generate mix table to mix two law samples into one * + ******************************************************/ + +void +dsp_audio_generate_mix_table(void) +{ + int i, j; + s32 sample; + + i = 0; + while (i < 256) { + j = 0; + while (j < 256) { + sample = dsp_audio_law_to_s32[i]; + sample += dsp_audio_law_to_s32[j]; + if (sample > 32767) + sample = 32767; + if (sample < -32768) + sample = -32768; + dsp_audio_mix_law[(i<<8)|j] = + dsp_audio_s16_to_law[sample & 0xffff]; + j++; + } + i++; + } +} + + +/************************************* + * generate different volume changes * + *************************************/ + +static u8 dsp_audio_reduce8[256]; +static u8 dsp_audio_reduce7[256]; +static u8 dsp_audio_reduce6[256]; +static u8 dsp_audio_reduce5[256]; +static u8 dsp_audio_reduce4[256]; +static u8 dsp_audio_reduce3[256]; +static u8 dsp_audio_reduce2[256]; +static u8 dsp_audio_reduce1[256]; +static u8 dsp_audio_increase1[256]; +static u8 dsp_audio_increase2[256]; +static u8 dsp_audio_increase3[256]; +static u8 dsp_audio_increase4[256]; +static u8 dsp_audio_increase5[256]; +static u8 dsp_audio_increase6[256]; +static u8 dsp_audio_increase7[256]; +static u8 dsp_audio_increase8[256]; + +static u8 *dsp_audio_volume_change[16] = { + dsp_audio_reduce8, + dsp_audio_reduce7, + dsp_audio_reduce6, + dsp_audio_reduce5, + dsp_audio_reduce4, + dsp_audio_reduce3, + dsp_audio_reduce2, + dsp_audio_reduce1, + dsp_audio_increase1, + dsp_audio_increase2, + dsp_audio_increase3, + dsp_audio_increase4, + dsp_audio_increase5, + dsp_audio_increase6, + dsp_audio_increase7, + dsp_audio_increase8, +}; + +void +dsp_audio_generate_volume_changes(void) +{ + register s32 sample; + int i; + int num[] = { 110, 125, 150, 175, 200, 300, 400, 500 }; + int denum[] = { 100, 100, 100, 100, 100, 100, 100, 100 }; + + i = 0; + while (i < 256) { + dsp_audio_reduce8[i] = dsp_audio_s16_to_law[ + (dsp_audio_law_to_s32[i] * denum[7] / num[7]) & 0xffff]; + dsp_audio_reduce7[i] = dsp_audio_s16_to_law[ + (dsp_audio_law_to_s32[i] * denum[6] / num[6]) & 0xffff]; + dsp_audio_reduce6[i] = dsp_audio_s16_to_law[ + (dsp_audio_law_to_s32[i] * denum[5] / num[5]) & 0xffff]; + dsp_audio_reduce5[i] = dsp_audio_s16_to_law[ + (dsp_audio_law_to_s32[i] * denum[4] / num[4]) & 0xffff]; + dsp_audio_reduce4[i] = dsp_audio_s16_to_law[ + (dsp_audio_law_to_s32[i] * denum[3] / num[3]) & 0xffff]; + dsp_audio_reduce3[i] = dsp_audio_s16_to_law[ + (dsp_audio_law_to_s32[i] * denum[2] / num[2]) & 0xffff]; + dsp_audio_reduce2[i] = dsp_audio_s16_to_law[ + (dsp_audio_law_to_s32[i] * denum[1] / num[1]) & 0xffff]; + dsp_audio_reduce1[i] = dsp_audio_s16_to_law[ + (dsp_audio_law_to_s32[i] * denum[0] / num[0]) & 0xffff]; + sample = dsp_audio_law_to_s32[i] * num[0] / denum[0]; + if (sample < -32768) + sample = -32768; + else if (sample > 32767) + sample = 32767; + dsp_audio_increase1[i] = dsp_audio_s16_to_law[sample & 0xffff]; + sample = dsp_audio_law_to_s32[i] * num[1] / denum[1]; + if (sample < -32768) + sample = -32768; + else if (sample > 32767) + sample = 32767; + dsp_audio_increase2[i] = dsp_audio_s16_to_law[sample & 0xffff]; + sample = dsp_audio_law_to_s32[i] * num[2] / denum[2]; + if (sample < -32768) + sample = -32768; + else if (sample > 32767) + sample = 32767; + dsp_audio_increase3[i] = dsp_audio_s16_to_law[sample & 0xffff]; + sample = dsp_audio_law_to_s32[i] * num[3] / denum[3]; + if (sample < -32768) + sample = -32768; + else if (sample > 32767) + sample = 32767; + dsp_audio_increase4[i] = dsp_audio_s16_to_law[sample & 0xffff]; + sample = dsp_audio_law_to_s32[i] * num[4] / denum[4]; + if (sample < -32768) + sample = -32768; + else if (sample > 32767) + sample = 32767; + dsp_audio_increase5[i] = dsp_audio_s16_to_law[sample & 0xffff]; + sample = dsp_audio_law_to_s32[i] * num[5] / denum[5]; + if (sample < -32768) + sample = -32768; + else if (sample > 32767) + sample = 32767; + dsp_audio_increase6[i] = dsp_audio_s16_to_law[sample & 0xffff]; + sample = dsp_audio_law_to_s32[i] * num[6] / denum[6]; + if (sample < -32768) + sample = -32768; + else if (sample > 32767) + sample = 32767; + dsp_audio_increase7[i] = dsp_audio_s16_to_law[sample & 0xffff]; + sample = dsp_audio_law_to_s32[i] * num[7] / denum[7]; + if (sample < -32768) + sample = -32768; + else if (sample > 32767) + sample = 32767; + dsp_audio_increase8[i] = dsp_audio_s16_to_law[sample & 0xffff]; + + i++; + } +} + + +/************************************** + * change the volume of the given skb * + **************************************/ + +/* this is a helper function for changing volume of skb. the range may be + * -8 to 8, which is a shift to the power of 2. 0 == no volume, 3 == volume*8 + */ +void +dsp_change_volume(struct sk_buff *skb, int volume) +{ + u8 *volume_change; + int i, ii; + u8 *p; + int shift; + + if (volume == 0) + return; + + /* get correct conversion table */ + if (volume < 0) { + shift = volume + 8; + if (shift < 0) + shift = 0; + } else { + shift = volume + 7; + if (shift > 15) + shift = 15; + } + volume_change = dsp_audio_volume_change[shift]; + i = 0; + ii = skb->len; + p = skb->data; + /* change volume */ + while (i < ii) { + *p = volume_change[*p]; + p++; + i++; + } +} + diff --git a/drivers/isdn/mISDN/dsp_biquad.h b/drivers/isdn/mISDN/dsp_biquad.h new file mode 100644 index 000000000000..038191bc45f5 --- /dev/null +++ b/drivers/isdn/mISDN/dsp_biquad.h @@ -0,0 +1,65 @@ +/* + * SpanDSP - a series of DSP components for telephony + * + * biquad.h - General telephony bi-quad section routines (currently this just + * handles canonic/type 2 form) + * + * Written by Steve Underwood <steveu@coppice.org> + * + * Copyright (C) 2001 Steve Underwood + * + * All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + * + */ + +struct biquad2_state { + int32_t gain; + int32_t a1; + int32_t a2; + int32_t b1; + int32_t b2; + + int32_t z1; + int32_t z2; +}; + +static inline void biquad2_init(struct biquad2_state *bq, + int32_t gain, int32_t a1, int32_t a2, int32_t b1, int32_t b2) +{ + bq->gain = gain; + bq->a1 = a1; + bq->a2 = a2; + bq->b1 = b1; + bq->b2 = b2; + + bq->z1 = 0; + bq->z2 = 0; +} + +static inline int16_t biquad2(struct biquad2_state *bq, int16_t sample) +{ + int32_t y; + int32_t z0; + + z0 = sample*bq->gain + bq->z1*bq->a1 + bq->z2*bq->a2; + y = z0 + bq->z1*bq->b1 + bq->z2*bq->b2; + + bq->z2 = bq->z1; + bq->z1 = z0 >> 15; + y >>= 15; + return y; +} diff --git a/drivers/isdn/mISDN/dsp_blowfish.c b/drivers/isdn/mISDN/dsp_blowfish.c new file mode 100644 index 000000000000..18e411e95bba --- /dev/null +++ b/drivers/isdn/mISDN/dsp_blowfish.c @@ -0,0 +1,672 @@ +/* + * Blowfish encryption/decryption for mISDN_dsp. + * + * Copyright Andreas Eversberg (jolly@eversberg.eu) + * + * This software may be used and distributed according to the terms + * of the GNU General Public License, incorporated herein by reference. + * + */ + +#include <linux/mISDNif.h> +#include <linux/mISDNdsp.h> +#include "core.h" +#include "dsp.h" + +/* + * how to encode a sample stream to 64-bit blocks that will be encryped + * + * first of all, data is collected until a block of 9 samples are received. + * of course, a packet may have much more than 9 sample, but is may have + * not excacly the multiple of 9 samples. if there is a rest, the next + * received data will complete the block. + * + * the block is then converted to 9 uLAW samples without the least sigificant + * bit. the result is a 7-bit encoded sample. + * + * the samples will be reoganised to form 8 bytes of data: + * (5(6) means: encoded sample no. 5, bit 6) + * + * 0(6) 0(5) 0(4) 0(3) 0(2) 0(1) 0(0) 1(6) + * 1(5) 1(4) 1(3) 1(2) 1(1) 1(0) 2(6) 2(5) + * 2(4) 2(3) 2(2) 2(1) 2(0) 3(6) 3(5) 3(4) + * 3(3) 3(2) 3(1) 3(0) 4(6) 4(5) 4(4) 4(3) + * 4(2) 4(1) 4(0) 5(6) 5(5) 5(4) 5(3) 5(2) + * 5(1) 5(0) 6(6) 6(5) 6(4) 6(3) 6(2) 6(1) + * 6(0) 7(6) 7(5) 7(4) 7(3) 7(2) 7(1) 7(0) + * 8(6) 8(5) 8(4) 8(3) 8(2) 8(1) 8(0) + * + * the missing bit 0 of the last byte is filled with some + * random noise, to fill all 8 bytes. + * + * the 8 bytes will be encrypted using blowfish. + * + * the result will be converted into 9 bytes. the bit 7 is used for + * checksumme (CS) for sync (0, 1) and for the last bit: + * (5(6) means: crypted byte 5, bit 6) + * + * 1 0(7) 0(6) 0(5) 0(4) 0(3) 0(2) 0(1) + * 0 0(0) 1(7) 1(6) 1(5) 1(4) 1(3) 1(2) + * 0 1(1) 1(0) 2(7) 2(6) 2(5) 2(4) 2(3) + * 0 2(2) 2(1) 2(0) 3(7) 3(6) 3(5) 3(4) + * 0 3(3) 3(2) 3(1) 3(0) 4(7) 4(6) 4(5) + * CS 4(4) 4(3) 4(2) 4(1) 4(0) 5(7) 5(6) + * CS 5(5) 5(4) 5(3) 5(2) 5(1) 5(0) 6(7) + * CS 6(6) 6(5) 6(4) 6(3) 6(2) 6(1) 6(0) + * 7(0) 7(6) 7(5) 7(4) 7(3) 7(2) 7(1) 7(0) + * + * the checksum is used to detect transmission errors and frame drops. + * + * synchronisation of received block is done by shifting the upper bit of each + * byte (bit 7) to a shift register. if the rigister has the first five bits + * (10000), this is used to find the sync. only if sync has been found, the + * current block of 9 received bytes are decrypted. before that the check + * sum is calculated. if it is incorrect the block is dropped. + * this will avoid loud noise due to corrupt encrypted data. + * + * if the last block is corrupt, the current decoded block is repeated + * until a valid block has been received. + */ + +/* + * some blowfish parts are taken from the + * crypto-api for faster implementation + */ + +struct bf_ctx { + u32 p[18]; + u32 s[1024]; +}; + +static const u32 bf_pbox[16 + 2] = { + 0x243f6a88, 0x85a308d3, 0x13198a2e, 0x03707344, + 0xa4093822, 0x299f31d0, 0x082efa98, 0xec4e6c89, + 0x452821e6, 0x38d01377, 0xbe5466cf, 0x34e90c6c, + 0xc0ac29b7, 0xc97c50dd, 0x3f84d5b5, 0xb5470917, + 0x9216d5d9, 0x8979fb1b, +}; + +static const u32 bf_sbox[256 * 4] = { + 0xd1310ba6, 0x98dfb5ac, 0x2ffd72db, 0xd01adfb7, + 0xb8e1afed, 0x6a267e96, 0xba7c9045, 0xf12c7f99, + 0x24a19947, 0xb3916cf7, 0x0801f2e2, 0x858efc16, + 0x636920d8, 0x71574e69, 0xa458fea3, 0xf4933d7e, + 0x0d95748f, 0x728eb658, 0x718bcd58, 0x82154aee, + 0x7b54a41d, 0xc25a59b5, 0x9c30d539, 0x2af26013, + 0xc5d1b023, 0x286085f0, 0xca417918, 0xb8db38ef, + 0x8e79dcb0, 0x603a180e, 0x6c9e0e8b, 0xb01e8a3e, + 0xd71577c1, 0xbd314b27, 0x78af2fda, 0x55605c60, + 0xe65525f3, 0xaa55ab94, 0x57489862, 0x63e81440, + 0x55ca396a, 0x2aab10b6, 0xb4cc5c34, 0x1141e8ce, + 0xa15486af, 0x7c72e993, 0xb3ee1411, 0x636fbc2a, + 0x2ba9c55d, 0x741831f6, 0xce5c3e16, 0x9b87931e, + 0xafd6ba33, 0x6c24cf5c, 0x7a325381, 0x28958677, + 0x3b8f4898, 0x6b4bb9af, 0xc4bfe81b, 0x66282193, + 0x61d809cc, 0xfb21a991, 0x487cac60, 0x5dec8032, + 0xef845d5d, 0xe98575b1, 0xdc262302, 0xeb651b88, + 0x23893e81, 0xd396acc5, 0x0f6d6ff3, 0x83f44239, + 0x2e0b4482, 0xa4842004, 0x69c8f04a, 0x9e1f9b5e, + 0x21c66842, 0xf6e96c9a, 0x670c9c61, 0xabd388f0, + 0x6a51a0d2, 0xd8542f68, 0x960fa728, 0xab5133a3, + 0x6eef0b6c, 0x137a3be4, 0xba3bf050, 0x7efb2a98, + 0xa1f1651d, 0x39af0176, 0x66ca593e, 0x82430e88, + 0x8cee8619, 0x456f9fb4, 0x7d84a5c3, 0x3b8b5ebe, + 0xe06f75d8, 0x85c12073, 0x401a449f, 0x56c16aa6, + 0x4ed3aa62, 0x363f7706, 0x1bfedf72, 0x429b023d, + 0x37d0d724, 0xd00a1248, 0xdb0fead3, 0x49f1c09b, + 0x075372c9, 0x80991b7b, 0x25d479d8, 0xf6e8def7, + 0xe3fe501a, 0xb6794c3b, 0x976ce0bd, 0x04c006ba, + 0xc1a94fb6, 0x409f60c4, 0x5e5c9ec2, 0x196a2463, + 0x68fb6faf, 0x3e6c53b5, 0x1339b2eb, 0x3b52ec6f, + 0x6dfc511f, 0x9b30952c, 0xcc814544, 0xaf5ebd09, + 0xbee3d004, 0xde334afd, 0x660f2807, 0x192e4bb3, + 0xc0cba857, 0x45c8740f, 0xd20b5f39, 0xb9d3fbdb, + 0x5579c0bd, 0x1a60320a, 0xd6a100c6, 0x402c7279, + 0x679f25fe, 0xfb1fa3cc, 0x8ea5e9f8, 0xdb3222f8, + 0x3c7516df, 0xfd616b15, 0x2f501ec8, 0xad0552ab, + 0x323db5fa, 0xfd238760, 0x53317b48, 0x3e00df82, + 0x9e5c57bb, 0xca6f8ca0, 0x1a87562e, 0xdf1769db, + 0xd542a8f6, 0x287effc3, 0xac6732c6, 0x8c4f5573, + 0x695b27b0, 0xbbca58c8, 0xe1ffa35d, 0xb8f011a0, + 0x10fa3d98, 0xfd2183b8, 0x4afcb56c, 0x2dd1d35b, + 0x9a53e479, 0xb6f84565, 0xd28e49bc, 0x4bfb9790, + 0xe1ddf2da, 0xa4cb7e33, 0x62fb1341, 0xcee4c6e8, + 0xef20cada, 0x36774c01, 0xd07e9efe, 0x2bf11fb4, + 0x95dbda4d, 0xae909198, 0xeaad8e71, 0x6b93d5a0, + 0xd08ed1d0, 0xafc725e0, 0x8e3c5b2f, 0x8e7594b7, + 0x8ff6e2fb, 0xf2122b64, 0x8888b812, 0x900df01c, + 0x4fad5ea0, 0x688fc31c, 0xd1cff191, 0xb3a8c1ad, + 0x2f2f2218, 0xbe0e1777, 0xea752dfe, 0x8b021fa1, + 0xe5a0cc0f, 0xb56f74e8, 0x18acf3d6, 0xce89e299, + 0xb4a84fe0, 0xfd13e0b7, 0x7cc43b81, 0xd2ada8d9, + 0x165fa266, 0x80957705, 0x93cc7314, 0x211a1477, + 0xe6ad2065, 0x77b5fa86, 0xc75442f5, 0xfb9d35cf, + 0xebcdaf0c, 0x7b3e89a0, 0xd6411bd3, 0xae1e7e49, + 0x00250e2d, 0x2071b35e, 0x226800bb, 0x57b8e0af, + 0x2464369b, 0xf009b91e, 0x5563911d, 0x59dfa6aa, + 0x78c14389, 0xd95a537f, 0x207d5ba2, 0x02e5b9c5, + 0x83260376, 0x6295cfa9, 0x11c81968, 0x4e734a41, + 0xb3472dca, 0x7b14a94a, 0x1b510052, 0x9a532915, + 0xd60f573f, 0xbc9bc6e4, 0x2b60a476, 0x81e67400, + 0x08ba6fb5, 0x571be91f, 0xf296ec6b, 0x2a0dd915, + 0xb6636521, 0xe7b9f9b6, 0xff34052e, 0xc5855664, + 0x53b02d5d, 0xa99f8fa1, 0x08ba4799, 0x6e85076a, + 0x4b7a70e9, 0xb5b32944, 0xdb75092e, 0xc4192623, + 0xad6ea6b0, 0x49a7df7d, 0x9cee60b8, 0x8fedb266, + 0xecaa8c71, 0x699a17ff, 0x5664526c, 0xc2b19ee1, + 0x193602a5, 0x75094c29, 0xa0591340, 0xe4183a3e, + 0x3f54989a, 0x5b429d65, 0x6b8fe4d6, 0x99f73fd6, + 0xa1d29c07, 0xefe830f5, 0x4d2d38e6, 0xf0255dc1, + 0x4cdd2086, 0x8470eb26, 0x6382e9c6, 0x021ecc5e, + 0x09686b3f, 0x3ebaefc9, 0x3c971814, 0x6b6a70a1, + 0x687f3584, 0x52a0e286, 0xb79c5305, 0xaa500737, + 0x3e07841c, 0x7fdeae5c, 0x8e7d44ec, 0x5716f2b8, + 0xb03ada37, 0xf0500c0d, 0xf01c1f04, 0x0200b3ff, + 0xae0cf51a, 0x3cb574b2, 0x25837a58, 0xdc0921bd, + 0xd19113f9, 0x7ca92ff6, 0x94324773, 0x22f54701, + 0x3ae5e581, 0x37c2dadc, 0xc8b57634, 0x9af3dda7, + 0xa9446146, 0x0fd0030e, 0xecc8c73e, 0xa4751e41, + 0xe238cd99, 0x3bea0e2f, 0x3280bba1, 0x183eb331, + 0x4e548b38, 0x4f6db908, 0x6f420d03, 0xf60a04bf, + 0x2cb81290, 0x24977c79, 0x5679b072, 0xbcaf89af, + 0xde9a771f, 0xd9930810, 0xb38bae12, 0xdccf3f2e, + 0x5512721f, 0x2e6b7124, 0x501adde6, 0x9f84cd87, + 0x7a584718, 0x7408da17, 0xbc9f9abc, 0xe94b7d8c, + 0xec7aec3a, 0xdb851dfa, 0x63094366, 0xc464c3d2, + 0xef1c1847, 0x3215d908, 0xdd433b37, 0x24c2ba16, + 0x12a14d43, 0x2a65c451, 0x50940002, 0x133ae4dd, + 0x71dff89e, 0x10314e55, 0x81ac77d6, 0x5f11199b, + 0x043556f1, 0xd7a3c76b, 0x3c11183b, 0x5924a509, + 0xf28fe6ed, 0x97f1fbfa, 0x9ebabf2c, 0x1e153c6e, + 0x86e34570, 0xeae96fb1, 0x860e5e0a, 0x5a3e2ab3, + 0x771fe71c, 0x4e3d06fa, 0x2965dcb9, 0x99e71d0f, + 0x803e89d6, 0x5266c825, 0x2e4cc978, 0x9c10b36a, + 0xc6150eba, 0x94e2ea78, 0xa5fc3c53, 0x1e0a2df4, + 0xf2f74ea7, 0x361d2b3d, 0x1939260f, 0x19c27960, + 0x5223a708, 0xf71312b6, 0xebadfe6e, 0xeac31f66, + 0xe3bc4595, 0xa67bc883, 0xb17f37d1, 0x018cff28, + 0xc332ddef, 0xbe6c5aa5, 0x65582185, 0x68ab9802, + 0xeecea50f, 0xdb2f953b, 0x2aef7dad, 0x5b6e2f84, + 0x1521b628, 0x29076170, 0xecdd4775, 0x619f1510, + 0x13cca830, 0xeb61bd96, 0x0334fe1e, 0xaa0363cf, + 0xb5735c90, 0x4c70a239, 0xd59e9e0b, 0xcbaade14, + 0xeecc86bc, 0x60622ca7, 0x9cab5cab, 0xb2f3846e, + 0x648b1eaf, 0x19bdf0ca, 0xa02369b9, 0x655abb50, + 0x40685a32, 0x3c2ab4b3, 0x319ee9d5, 0xc021b8f7, + 0x9b540b19, 0x875fa099, 0x95f7997e, 0x623d7da8, + 0xf837889a, 0x97e32d77, 0x11ed935f, 0x16681281, + 0x0e358829, 0xc7e61fd6, 0x96dedfa1, 0x7858ba99, + 0x57f584a5, 0x1b227263, 0x9b83c3ff, 0x1ac24696, + 0xcdb30aeb, 0x532e3054, 0x8fd948e4, 0x6dbc3128, + 0x58ebf2ef, 0x34c6ffea, 0xfe28ed61, 0xee7c3c73, + 0x5d4a14d9, 0xe864b7e3, 0x42105d14, 0x203e13e0, + 0x45eee2b6, 0xa3aaabea, 0xdb6c4f15, 0xfacb4fd0, + 0xc742f442, 0xef6abbb5, 0x654f3b1d, 0x41cd2105, + 0xd81e799e, 0x86854dc7, 0xe44b476a, 0x3d816250, + 0xcf62a1f2, 0x5b8d2646, 0xfc8883a0, 0xc1c7b6a3, + 0x7f1524c3, 0x69cb7492, 0x47848a0b, 0x5692b285, + 0x095bbf00, 0xad19489d, 0x1462b174, 0x23820e00, + 0x58428d2a, 0x0c55f5ea, 0x1dadf43e, 0x233f7061, + 0x3372f092, 0x8d937e41, 0xd65fecf1, 0x6c223bdb, + 0x7cde3759, 0xcbee7460, 0x4085f2a7, 0xce77326e, + 0xa6078084, 0x19f8509e, 0xe8efd855, 0x61d99735, + 0xa969a7aa, 0xc50c06c2, 0x5a04abfc, 0x800bcadc, + 0x9e447a2e, 0xc3453484, 0xfdd56705, 0x0e1e9ec9, + 0xdb73dbd3, 0x105588cd, 0x675fda79, 0xe3674340, + 0xc5c43465, 0x713e38d8, 0x3d28f89e, 0xf16dff20, + 0x153e21e7, 0x8fb03d4a, 0xe6e39f2b, 0xdb83adf7, + 0xe93d5a68, 0x948140f7, 0xf64c261c, 0x94692934, + 0x411520f7, 0x7602d4f7, 0xbcf46b2e, 0xd4a20068, + 0xd4082471, 0x3320f46a, 0x43b7d4b7, 0x500061af, + 0x1e39f62e, 0x97244546, 0x14214f74, 0xbf8b8840, + 0x4d95fc1d, 0x96b591af, 0x70f4ddd3, 0x66a02f45, + 0xbfbc09ec, 0x03bd9785, 0x7fac6dd0, 0x31cb8504, + 0x96eb27b3, 0x55fd3941, 0xda2547e6, 0xabca0a9a, + 0x28507825, 0x530429f4, 0x0a2c86da, 0xe9b66dfb, + 0x68dc1462, 0xd7486900, 0x680ec0a4, 0x27a18dee, + 0x4f3ffea2, 0xe887ad8c, 0xb58ce006, 0x7af4d6b6, + 0xaace1e7c, 0xd3375fec, 0xce78a399, 0x406b2a42, + 0x20fe9e35, 0xd9f385b9, 0xee39d7ab, 0x3b124e8b, + 0x1dc9faf7, 0x4b6d1856, 0x26a36631, 0xeae397b2, + 0x3a6efa74, 0xdd5b4332, 0x6841e7f7, 0xca7820fb, + 0xfb0af54e, 0xd8feb397, 0x454056ac, 0xba489527, + 0x55533a3a, 0x20838d87, 0xfe6ba9b7, 0xd096954b, + 0x55a867bc, 0xa1159a58, 0xcca92963, 0x99e1db33, + 0xa62a4a56, 0x3f3125f9, 0x5ef47e1c, 0x9029317c, + 0xfdf8e802, 0x04272f70, 0x80bb155c, 0x05282ce3, + 0x95c11548, 0xe4c66d22, 0x48c1133f, 0xc70f86dc, + 0x07f9c9ee, 0x41041f0f, 0x404779a4, 0x5d886e17, + 0x325f51eb, 0xd59bc0d1, 0xf2bcc18f, 0x41113564, + 0x257b7834, 0x602a9c60, 0xdff8e8a3, 0x1f636c1b, + 0x0e12b4c2, 0x02e1329e, 0xaf664fd1, 0xcad18115, + 0x6b2395e0, 0x333e92e1, 0x3b240b62, 0xeebeb922, + 0x85b2a20e, 0xe6ba0d99, 0xde720c8c, 0x2da2f728, + 0xd0127845, 0x95b794fd, 0x647d0862, 0xe7ccf5f0, + 0x5449a36f, 0x877d48fa, 0xc39dfd27, 0xf33e8d1e, + 0x0a476341, 0x992eff74, 0x3a6f6eab, 0xf4f8fd37, + 0xa812dc60, 0xa1ebddf8, 0x991be14c, 0xdb6e6b0d, + 0xc67b5510, 0x6d672c37, 0x2765d43b, 0xdcd0e804, + 0xf1290dc7, 0xcc00ffa3, 0xb5390f92, 0x690fed0b, + 0x667b9ffb, 0xcedb7d9c, 0xa091cf0b, 0xd9155ea3, + 0xbb132f88, 0x515bad24, 0x7b9479bf, 0x763bd6eb, + 0x37392eb3, 0xcc115979, 0x8026e297, 0xf42e312d, + 0x6842ada7, 0xc66a2b3b, 0x12754ccc, 0x782ef11c, + 0x6a124237, 0xb79251e7, 0x06a1bbe6, 0x4bfb6350, + 0x1a6b1018, 0x11caedfa, 0x3d25bdd8, 0xe2e1c3c9, + 0x44421659, 0x0a121386, 0xd90cec6e, 0xd5abea2a, + 0x64af674e, 0xda86a85f, 0xbebfe988, 0x64e4c3fe, + 0x9dbc8057, 0xf0f7c086, 0x60787bf8, 0x6003604d, + 0xd1fd8346, 0xf6381fb0, 0x7745ae04, 0xd736fccc, + 0x83426b33, 0xf01eab71, 0xb0804187, 0x3c005e5f, + 0x77a057be, 0xbde8ae24, 0x55464299, 0xbf582e61, + 0x4e58f48f, 0xf2ddfda2, 0xf474ef38, 0x8789bdc2, + 0x5366f9c3, 0xc8b38e74, 0xb475f255, 0x46fcd9b9, + 0x7aeb2661, 0x8b1ddf84, 0x846a0e79, 0x915f95e2, + 0x466e598e, 0x20b45770, 0x8cd55591, 0xc902de4c, + 0xb90bace1, 0xbb8205d0, 0x11a86248, 0x7574a99e, + 0xb77f19b6, 0xe0a9dc09, 0x662d09a1, 0xc4324633, + 0xe85a1f02, 0x09f0be8c, 0x4a99a025, 0x1d6efe10, + 0x1ab93d1d, 0x0ba5a4df, 0xa186f20f, 0x2868f169, + 0xdcb7da83, 0x573906fe, 0xa1e2ce9b, 0x4fcd7f52, + 0x50115e01, 0xa70683fa, 0xa002b5c4, 0x0de6d027, + 0x9af88c27, 0x773f8641, 0xc3604c06, 0x61a806b5, + 0xf0177a28, 0xc0f586e0, 0x006058aa, 0x30dc7d62, + 0x11e69ed7, 0x2338ea63, 0x53c2dd94, 0xc2c21634, + 0xbbcbee56, 0x90bcb6de, 0xebfc7da1, 0xce591d76, + 0x6f05e409, 0x4b7c0188, 0x39720a3d, 0x7c927c24, + 0x86e3725f, 0x724d9db9, 0x1ac15bb4, 0xd39eb8fc, + 0xed545578, 0x08fca5b5, 0xd83d7cd3, 0x4dad0fc4, + 0x1e50ef5e, 0xb161e6f8, 0xa28514d9, 0x6c51133c, + 0x6fd5c7e7, 0x56e14ec4, 0x362abfce, 0xddc6c837, + 0xd79a3234, 0x92638212, 0x670efa8e, 0x406000e0, + 0x3a39ce37, 0xd3faf5cf, 0xabc27737, 0x5ac52d1b, + 0x5cb0679e, 0x4fa33742, 0xd3822740, 0x99bc9bbe, + 0xd5118e9d, 0xbf0f7315, 0xd62d1c7e, 0xc700c47b, + 0xb78c1b6b, 0x21a19045, 0xb26eb1be, 0x6a366eb4, + 0x5748ab2f, 0xbc946e79, 0xc6a376d2, 0x6549c2c8, + 0x530ff8ee, 0x468dde7d, 0xd5730a1d, 0x4cd04dc6, + 0x2939bbdb, 0xa9ba4650, 0xac9526e8, 0xbe5ee304, + 0xa1fad5f0, 0x6a2d519a, 0x63ef8ce2, 0x9a86ee22, + 0xc089c2b8, 0x43242ef6, 0xa51e03aa, 0x9cf2d0a4, + 0x83c061ba, 0x9be96a4d, 0x8fe51550, 0xba645bd6, + 0x2826a2f9, 0xa73a3ae1, 0x4ba99586, 0xef5562e9, + 0xc72fefd3, 0xf752f7da, 0x3f046f69, 0x77fa0a59, + 0x80e4a915, 0x87b08601, 0x9b09e6ad, 0x3b3ee593, + 0xe990fd5a, 0x9e34d797, 0x2cf0b7d9, 0x022b8b51, + 0x96d5ac3a, 0x017da67d, 0xd1cf3ed6, 0x7c7d2d28, + 0x1f9f25cf, 0xadf2b89b, 0x5ad6b472, 0x5a88f54c, + 0xe029ac71, 0xe019a5e6, 0x47b0acfd, 0xed93fa9b, + 0xe8d3c48d, 0x283b57cc, 0xf8d56629, 0x79132e28, + 0x785f0191, 0xed756055, 0xf7960e44, 0xe3d35e8c, + 0x15056dd4, 0x88f46dba, 0x03a16125, 0x0564f0bd, + 0xc3eb9e15, 0x3c9057a2, 0x97271aec, 0xa93a072a, + 0x1b3f6d9b, 0x1e6321f5, 0xf59c66fb, 0x26dcf319, + 0x7533d928, 0xb155fdf5, 0x03563482, 0x8aba3cbb, + 0x28517711, 0xc20ad9f8, 0xabcc5167, 0xccad925f, + 0x4de81751, 0x3830dc8e, 0x379d5862, 0x9320f991, + 0xea7a90c2, 0xfb3e7bce, 0x5121ce64, 0x774fbe32, + 0xa8b6e37e, 0xc3293d46, 0x48de5369, 0x6413e680, + 0xa2ae0810, 0xdd6db224, 0x69852dfd, 0x09072166, + 0xb39a460a, 0x6445c0dd, 0x586cdecf, 0x1c20c8ae, + 0x5bbef7dd, 0x1b588d40, 0xccd2017f, 0x6bb4e3bb, + 0xdda26a7e, 0x3a59ff45, 0x3e350a44, 0xbcb4cdd5, + 0x72eacea8, 0xfa6484bb, 0x8d6612ae, 0xbf3c6f47, + 0xd29be463, 0x542f5d9e, 0xaec2771b, 0xf64e6370, + 0x740e0d8d, 0xe75b1357, 0xf8721671, 0xaf537d5d, + 0x4040cb08, 0x4eb4e2cc, 0x34d2466a, 0x0115af84, + 0xe1b00428, 0x95983a1d, 0x06b89fb4, 0xce6ea048, + 0x6f3f3b82, 0x3520ab82, 0x011a1d4b, 0x277227f8, + 0x611560b1, 0xe7933fdc, 0xbb3a792b, 0x344525bd, + 0xa08839e1, 0x51ce794b, 0x2f32c9b7, 0xa01fbac9, + 0xe01cc87e, 0xbcc7d1f6, 0xcf0111c3, 0xa1e8aac7, + 0x1a908749, 0xd44fbd9a, 0xd0dadecb, 0xd50ada38, + 0x0339c32a, 0xc6913667, 0x8df9317c, 0xe0b12b4f, + 0xf79e59b7, 0x43f5bb3a, 0xf2d519ff, 0x27d9459c, + 0xbf97222c, 0x15e6fc2a, 0x0f91fc71, 0x9b941525, + 0xfae59361, 0xceb69ceb, 0xc2a86459, 0x12baa8d1, + 0xb6c1075e, 0xe3056a0c, 0x10d25065, 0xcb03a442, + 0xe0ec6e0e, 0x1698db3b, 0x4c98a0be, 0x3278e964, + 0x9f1f9532, 0xe0d392df, 0xd3a0342b, 0x8971f21e, + 0x1b0a7441, 0x4ba3348c, 0xc5be7120, 0xc37632d8, + 0xdf359f8d, 0x9b992f2e, 0xe60b6f47, 0x0fe3f11d, + 0xe54cda54, 0x1edad891, 0xce6279cf, 0xcd3e7e6f, + 0x1618b166, 0xfd2c1d05, 0x848fd2c5, 0xf6fb2299, + 0xf523f357, 0xa6327623, 0x93a83531, 0x56cccd02, + 0xacf08162, 0x5a75ebb5, 0x6e163697, 0x88d273cc, + 0xde966292, 0x81b949d0, 0x4c50901b, 0x71c65614, + 0xe6c6c7bd, 0x327a140a, 0x45e1d006, 0xc3f27b9a, + 0xc9aa53fd, 0x62a80f00, 0xbb25bfe2, 0x35bdd2f6, + 0x71126905, 0xb2040222, 0xb6cbcf7c, 0xcd769c2b, + 0x53113ec0, 0x1640e3d3, 0x38abbd60, 0x2547adf0, + 0xba38209c, 0xf746ce76, 0x77afa1c5, 0x20756060, + 0x85cbfe4e, 0x8ae88dd8, 0x7aaaf9b0, 0x4cf9aa7e, + 0x1948c25c, 0x02fb8a8c, 0x01c36ae4, 0xd6ebe1f9, + 0x90d4f869, 0xa65cdea0, 0x3f09252d, 0xc208e69f, + 0xb74e6132, 0xce77e25b, 0x578fdfe3, 0x3ac372e6, +}; + +/* + * Round loop unrolling macros, S is a pointer to a S-Box array + * organized in 4 unsigned longs at a row. + */ +#define GET32_3(x) (((x) & 0xff)) +#define GET32_2(x) (((x) >> (8)) & (0xff)) +#define GET32_1(x) (((x) >> (16)) & (0xff)) +#define GET32_0(x) (((x) >> (24)) & (0xff)) + +#define bf_F(x) (((S[GET32_0(x)] + S[256 + GET32_1(x)]) ^ \ + S[512 + GET32_2(x)]) + S[768 + GET32_3(x)]) + +#define EROUND(a, b, n) do { b ^= P[n]; a ^= bf_F(b); } while (0) +#define DROUND(a, b, n) do { a ^= bf_F(b); b ^= P[n]; } while (0) + + +/* + * encrypt isdn data frame + * every block with 9 samples is encrypted + */ +void +dsp_bf_encrypt(struct dsp *dsp, u8 *data, int len) +{ + int i = 0, j = dsp->bf_crypt_pos; + u8 *bf_data_in = dsp->bf_data_in; + u8 *bf_crypt_out = dsp->bf_crypt_out; + u32 *P = dsp->bf_p; + u32 *S = dsp->bf_s; + u32 yl, yr; + u32 cs; + u8 nibble; + + while (i < len) { + /* collect a block of 9 samples */ + if (j < 9) { + bf_data_in[j] = *data; + *data++ = bf_crypt_out[j++]; + i++; + continue; + } + j = 0; + /* transcode 9 samples xlaw to 8 bytes */ + yl = dsp_audio_law2seven[bf_data_in[0]]; + yl = (yl<<7) | dsp_audio_law2seven[bf_data_in[1]]; + yl = (yl<<7) | dsp_audio_law2seven[bf_data_in[2]]; + yl = (yl<<7) | dsp_audio_law2seven[bf_data_in[3]]; + nibble = dsp_audio_law2seven[bf_data_in[4]]; + yr = nibble; + yl = (yl<<4) | (nibble>>3); + yr = (yr<<7) | dsp_audio_law2seven[bf_data_in[5]]; + yr = (yr<<7) | dsp_audio_law2seven[bf_data_in[6]]; + yr = (yr<<7) | dsp_audio_law2seven[bf_data_in[7]]; + yr = (yr<<7) | dsp_audio_law2seven[bf_data_in[8]]; + yr = (yr<<1) | (bf_data_in[0] & 1); + + /* fill unused bit with random noise of audio input */ + /* encrypt */ + + EROUND(yr, yl, 0); + EROUND(yl, yr, 1); + EROUND(yr, yl, 2); + EROUND(yl, yr, 3); + EROUND(yr, yl, 4); + EROUND(yl, yr, 5); + EROUND(yr, yl, 6); + EROUND(yl, yr, 7); + EROUND(yr, yl, 8); + EROUND(yl, yr, 9); + EROUND(yr, yl, 10); + EROUND(yl, yr, 11); + EROUND(yr, yl, 12); + EROUND(yl, yr, 13); + EROUND(yr, yl, 14); + EROUND(yl, yr, 15); + yl ^= P[16]; + yr ^= P[17]; + + /* calculate 3-bit checksumme */ + cs = yl ^ (yl>>3) ^ (yl>>6) ^ (yl>>9) ^ (yl>>12) ^ (yl>>15) + ^ (yl>>18) ^ (yl>>21) ^ (yl>>24) ^ (yl>>27) ^ (yl>>30) + ^ (yr<<2) ^ (yr>>1) ^ (yr>>4) ^ (yr>>7) ^ (yr>>10) + ^ (yr>>13) ^ (yr>>16) ^ (yr>>19) ^ (yr>>22) ^ (yr>>25) + ^ (yr>>28) ^ (yr>>31); + + /* + * transcode 8 crypted bytes to 9 data bytes with sync + * and checksum information + */ + bf_crypt_out[0] = (yl>>25) | 0x80; + bf_crypt_out[1] = (yl>>18) & 0x7f; + bf_crypt_out[2] = (yl>>11) & 0x7f; + bf_crypt_out[3] = (yl>>4) & 0x7f; + bf_crypt_out[4] = ((yl<<3) & 0x78) | ((yr>>29) & 0x07); + bf_crypt_out[5] = ((yr>>22) & 0x7f) | ((cs<<5) & 0x80); + bf_crypt_out[6] = ((yr>>15) & 0x7f) | ((cs<<6) & 0x80); + bf_crypt_out[7] = ((yr>>8) & 0x7f) | (cs<<7); + bf_crypt_out[8] = yr; + } + + /* write current count */ + dsp->bf_crypt_pos = j; + +} + + +/* + * decrypt isdn data frame + * every block with 9 bytes is decrypted + */ +void +dsp_bf_decrypt(struct dsp *dsp, u8 *data, int len) +{ + int i = 0; + u8 j = dsp->bf_decrypt_in_pos; + u8 k = dsp->bf_decrypt_out_pos; + u8 *bf_crypt_inring = dsp->bf_crypt_inring; + u8 *bf_data_out = dsp->bf_data_out; + u16 sync = dsp->bf_sync; + u32 *P = dsp->bf_p; + u32 *S = dsp->bf_s; + u32 yl, yr; + u8 nibble; + u8 cs, cs0, cs1, cs2; + + while (i < len) { + /* + * shift upper bit and rotate data to buffer ring + * send current decrypted data + */ + sync = (sync<<1) | ((*data)>>7); + bf_crypt_inring[j++ & 15] = *data; + *data++ = bf_data_out[k++]; + i++; + if (k == 9) + k = 0; /* repeat if no sync has been found */ + /* check if not in sync */ + if ((sync&0x1f0) != 0x100) + continue; + j -= 9; + /* transcode receive data to 64 bit block of encrypted data */ + yl = bf_crypt_inring[j++ & 15]; + yl = (yl<<7) | bf_crypt_inring[j++ & 15]; /* bit7 = 0 */ + yl = (yl<<7) | bf_crypt_inring[j++ & 15]; /* bit7 = 0 */ + yl = (yl<<7) | bf_crypt_inring[j++ & 15]; /* bit7 = 0 */ + nibble = bf_crypt_inring[j++ & 15]; /* bit7 = 0 */ + yr = nibble; + yl = (yl<<4) | (nibble>>3); + cs2 = bf_crypt_inring[j++ & 15]; + yr = (yr<<7) | (cs2 & 0x7f); + cs1 = bf_crypt_inring[j++ & 15]; + yr = (yr<<7) | (cs1 & 0x7f); + cs0 = bf_crypt_inring[j++ & 15]; + yr = (yr<<7) | (cs0 & 0x7f); + yr = (yr<<8) | bf_crypt_inring[j++ & 15]; + + /* calculate 3-bit checksumme */ + cs = yl ^ (yl>>3) ^ (yl>>6) ^ (yl>>9) ^ (yl>>12) ^ (yl>>15) + ^ (yl>>18) ^ (yl>>21) ^ (yl>>24) ^ (yl>>27) ^ (yl>>30) + ^ (yr<<2) ^ (yr>>1) ^ (yr>>4) ^ (yr>>7) ^ (yr>>10) + ^ (yr>>13) ^ (yr>>16) ^ (yr>>19) ^ (yr>>22) ^ (yr>>25) + ^ (yr>>28) ^ (yr>>31); + + /* check if frame is valid */ + if ((cs&0x7) != (((cs2>>5)&4) | ((cs1>>6)&2) | (cs0 >> 7))) { + if (dsp_debug & DEBUG_DSP_BLOWFISH) + printk(KERN_DEBUG + "DSP BLOWFISH: received corrupt frame, " + "checksumme is not correct\n"); + continue; + } + + /* decrypt */ + yr ^= P[17]; + yl ^= P[16]; + DROUND(yl, yr, 15); + DROUND(yr, yl, 14); + DROUND(yl, yr, 13); + DROUND(yr, yl, 12); + DROUND(yl, yr, 11); + DROUND(yr, yl, 10); + DROUND(yl, yr, 9); + DROUND(yr, yl, 8); + DROUND(yl, yr, 7); + DROUND(yr, yl, 6); + DROUND(yl, yr, 5); + DROUND(yr, yl, 4); + DROUND(yl, yr, 3); + DROUND(yr, yl, 2); + DROUND(yl, yr, 1); + DROUND(yr, yl, 0); + + /* transcode 8 crypted bytes to 9 sample bytes */ + bf_data_out[0] = dsp_audio_seven2law[(yl>>25) & 0x7f]; + bf_data_out[1] = dsp_audio_seven2law[(yl>>18) & 0x7f]; + bf_data_out[2] = dsp_audio_seven2law[(yl>>11) & 0x7f]; + bf_data_out[3] = dsp_audio_seven2law[(yl>>4) & 0x7f]; + bf_data_out[4] = dsp_audio_seven2law[((yl<<3) & 0x78) | + ((yr>>29) & 0x07)]; + + bf_data_out[5] = dsp_audio_seven2law[(yr>>22) & 0x7f]; + bf_data_out[6] = dsp_audio_seven2law[(yr>>15) & 0x7f]; + bf_data_out[7] = dsp_audio_seven2law[(yr>>8) & 0x7f]; + bf_data_out[8] = dsp_audio_seven2law[(yr>>1) & 0x7f]; + k = 0; /* start with new decoded frame */ + } + + /* write current count and sync */ + dsp->bf_decrypt_in_pos = j; + dsp->bf_decrypt_out_pos = k; + dsp->bf_sync = sync; +} + + +/* used to encrypt S and P boxes */ +static inline void +encrypt_block(const u32 *P, const u32 *S, u32 *dst, u32 *src) +{ + u32 yl = src[0]; + u32 yr = src[1]; + + EROUND(yr, yl, 0); + EROUND(yl, yr, 1); + EROUND(yr, yl, 2); + EROUND(yl, yr, 3); + EROUND(yr, yl, 4); + EROUND(yl, yr, 5); + EROUND(yr, yl, 6); + EROUND(yl, yr, 7); + EROUND(yr, yl, 8); + EROUND(yl, yr, 9); + EROUND(yr, yl, 10); + EROUND(yl, yr, 11); + EROUND(yr, yl, 12); + EROUND(yl, yr, 13); + EROUND(yr, yl, 14); + EROUND(yl, yr, 15); + + yl ^= P[16]; + yr ^= P[17]; + + dst[0] = yr; + dst[1] = yl; +} + +/* + * initialize the dsp for encryption and decryption using the same key + * Calculates the blowfish S and P boxes for encryption and decryption. + * The margin of keylen must be 4-56 bytes. + * returns 0 if ok. + */ +int +dsp_bf_init(struct dsp *dsp, const u8 *key, uint keylen) +{ + short i, j, count; + u32 data[2], temp; + u32 *P = (u32 *)dsp->bf_p; + u32 *S = (u32 *)dsp->bf_s; + + if (keylen < 4 || keylen > 56) + return 1; + + /* Set dsp states */ + i = 0; + while (i < 9) { + dsp->bf_crypt_out[i] = 0xff; + dsp->bf_data_out[i] = dsp_silence; + i++; + } + dsp->bf_crypt_pos = 0; + dsp->bf_decrypt_in_pos = 0; + dsp->bf_decrypt_out_pos = 0; + dsp->bf_sync = 0x1ff; + dsp->bf_enable = 1; + + /* Copy the initialization s-boxes */ + for (i = 0, count = 0; i < 256; i++) + for (j = 0; j < 4; j++, count++) + S[count] = bf_sbox[count]; + + /* Set the p-boxes */ + for (i = 0; i < 16 + 2; i++) + P[i] = bf_pbox[i]; + + /* Actual subkey generation */ + for (j = 0, i = 0; i < 16 + 2; i++) { + temp = (((u32)key[j] << 24) | + ((u32)key[(j + 1) % keylen] << 16) | + ((u32)key[(j + 2) % keylen] << 8) | + ((u32)key[(j + 3) % keylen])); + + P[i] = P[i] ^ temp; + j = (j + 4) % keylen; + } + + data[0] = 0x00000000; + data[1] = 0x00000000; + + for (i = 0; i < 16 + 2; i += 2) { + encrypt_block(P, S, data, data); + + P[i] = data[0]; + P[i + 1] = data[1]; + } + + for (i = 0; i < 4; i++) { + for (j = 0, count = i * 256; j < 256; j += 2, count += 2) { + encrypt_block(P, S, data, data); + + S[count] = data[0]; + S[count + 1] = data[1]; + } + } + + return 0; +} + + +/* + * turn encryption off + */ +void +dsp_bf_cleanup(struct dsp *dsp) +{ + dsp->bf_enable = 0; +} diff --git a/drivers/isdn/mISDN/dsp_cmx.c b/drivers/isdn/mISDN/dsp_cmx.c new file mode 100644 index 000000000000..e92b1ba4b45e --- /dev/null +++ b/drivers/isdn/mISDN/dsp_cmx.c @@ -0,0 +1,1886 @@ +/* + * Audio crossconnecting/conferrencing (hardware level). + * + * Copyright 2002 by Andreas Eversberg (jolly@eversberg.eu) + * + * This software may be used and distributed according to the terms + * of the GNU General Public License, incorporated herein by reference. + * + */ + +/* + * The process of adding and removing parties to/from a conference: + * + * There is a chain of struct dsp_conf which has one or more members in a chain + * of struct dsp_conf_member. + * + * After a party is added, the conference is checked for hardware capability. + * Also if a party is removed, the conference is checked again. + * + * There are 3 different solutions: -1 = software, 0 = hardware-crossconnect + * 1-n = hardware-conference. The n will give the conference number. + * + * Depending on the change after removal or insertion of a party, hardware + * commands are given. + * + * The current solution is stored within the struct dsp_conf entry. + */ + +/* + * HOW THE CMX WORKS: + * + * There are 3 types of interaction: One member is alone, in this case only + * data flow from upper to lower layer is done. + * Two members will also exchange their data so they are crossconnected. + * Three or more members will be added in a conference and will hear each + * other but will not receive their own speech (echo) if not enabled. + * + * Features of CMX are: + * - Crossconnecting or even conference, if more than two members are together. + * - Force mixing of transmit data with other crossconnect/conference members. + * - Echo generation to benchmark the delay of audio processing. + * - Use hardware to minimize cpu load, disable FIFO load and minimize delay. + * - Dejittering and clock generation. + * + * There are 2 buffers: + * + * + * RX-Buffer + * R W + * | | + * ----------------+-------------+------------------- + * + * The rx-buffer is a ring buffer used to store the received data for each + * individual member. This is only the case if data needs to be dejittered + * or in case of a conference where different clocks require reclocking. + * The transmit-clock (R) will read the buffer. + * If the clock overruns the write-pointer, we will have a buffer underrun. + * If the write pointer always has a certain distance from the transmit- + * clock, we will have a delay. The delay will dynamically be increased and + * reduced. + * + * + * TX-Buffer + * R W + * | | + * -----------------+--------+----------------------- + * + * The tx-buffer is a ring buffer to queue the transmit data from user space + * until it will be mixed or sent. There are two pointers, R and W. If the write + * pointer W would reach or overrun R, the buffer would overrun. In this case + * (some) data is dropped so that it will not overrun. + * Additionally a dynamic dejittering can be enabled. this allows data from + * user space that have jitter and different clock source. + * + * + * Clock: + * + * A Clock is not required, if the data source has exactly one clock. In this + * case the data source is forwarded to the destination. + * + * A Clock is required, because the data source + * - has multiple clocks. + * - has no usable clock due to jitter or packet loss (VoIP). + * In this case the system's clock is used. The clock resolution depends on + * the jiffie resolution. + * + * If a member joins a conference: + * + * - If a member joins, its rx_buff is set to silence and change read pointer + * to transmit clock. + * + * The procedure of received data from card is explained in cmx_receive. + * The procedure of received data from user space is explained in cmx_transmit. + * The procedure of transmit data to card is cmx_send. + * + * + * Interaction with other features: + * + * DTMF: + * DTMF decoding is done before the data is crossconnected. + * + * Volume change: + * Changing rx-volume is done before the data is crossconnected. The tx-volume + * must be changed whenever data is transmitted to the card by the cmx. + * + * Tones: + * If a tone is enabled, it will be processed whenever data is transmitted to + * the card. It will replace the tx-data from the user space. + * If tones are generated by hardware, this conference member is removed for + * this time. + * + * Disable rx-data: + * If cmx is realized in hardware, rx data will be disabled if requested by + * the upper layer. If dtmf decoding is done by software and enabled, rx data + * will not be diabled but blocked to the upper layer. + * + * HFC conference engine: + * If it is possible to realize all features using hardware, hardware will be + * used if not forbidden by control command. Disabling rx-data provides + * absolutely traffic free audio processing. (except for the quick 1-frame + * upload of a tone loop, only once for a new tone) + * + */ + +/* delay.h is required for hw_lock.h */ + +#include <linux/delay.h> +#include <linux/mISDNif.h> +#include <linux/mISDNdsp.h> +#include "core.h" +#include "dsp.h" +/* + * debugging of multi party conference, + * by using conference even with two members + */ + +/* #define CMX_CONF_DEBUG */ + +/*#define CMX_DEBUG * massive read/write pointer output */ +/*#define CMX_TX_DEBUG * massive read/write on tx-buffer with content */ + +static inline int +count_list_member(struct list_head *head) +{ + int cnt = 0; + struct list_head *m; + + list_for_each(m, head) + cnt++; + return cnt; +} + +/* + * debug cmx memory structure + */ +void +dsp_cmx_debug(struct dsp *dsp) +{ + struct dsp_conf *conf; + struct dsp_conf_member *member; + struct dsp *odsp; + + printk(KERN_DEBUG "-----Current DSP\n"); + list_for_each_entry(odsp, &dsp_ilist, list) { + printk(KERN_DEBUG "* %s echo=%d txmix=%d", + odsp->name, odsp->echo, odsp->tx_mix); + if (odsp->conf) + printk(" (Conf %d)", odsp->conf->id); + if (dsp == odsp) + printk(" *this*"); + printk("\n"); + } + printk(KERN_DEBUG "-----Current Conf:\n"); + list_for_each_entry(conf, &conf_ilist, list) { + printk(KERN_DEBUG "* Conf %d (%p)\n", conf->id, conf); + list_for_each_entry(member, &conf->mlist, list) { + printk(KERN_DEBUG + " - member = %s (slot_tx %d, bank_tx %d, " + "slot_rx %d, bank_rx %d hfc_conf %d)%s\n", + member->dsp->name, member->dsp->pcm_slot_tx, + member->dsp->pcm_bank_tx, member->dsp->pcm_slot_rx, + member->dsp->pcm_bank_rx, member->dsp->hfc_conf, + (member->dsp == dsp) ? " *this*" : ""); + } + } + printk(KERN_DEBUG "-----end\n"); +} + +/* + * search conference + */ +static struct dsp_conf * +dsp_cmx_search_conf(u32 id) +{ + struct dsp_conf *conf; + + if (!id) { + printk(KERN_WARNING "%s: conference ID is 0.\n", __func__); + return NULL; + } + + /* search conference */ + list_for_each_entry(conf, &conf_ilist, list) + if (conf->id == id) + return conf; + + return NULL; +} + + +/* + * add member to conference + */ +static int +dsp_cmx_add_conf_member(struct dsp *dsp, struct dsp_conf *conf) +{ + struct dsp_conf_member *member; + + if (!conf || !dsp) { + printk(KERN_WARNING "%s: conf or dsp is 0.\n", __func__); + return -EINVAL; + } + if (dsp->member) { + printk(KERN_WARNING "%s: dsp is already member in a conf.\n", + __func__); + return -EINVAL; + } + + if (dsp->conf) { + printk(KERN_WARNING "%s: dsp is already in a conf.\n", + __func__); + return -EINVAL; + } + + member = kzalloc(sizeof(struct dsp_conf_member), GFP_ATOMIC); + if (!member) { + printk(KERN_ERR "kmalloc struct dsp_conf_member failed\n"); + return -ENOMEM; + } + member->dsp = dsp; + /* clear rx buffer */ + memset(dsp->rx_buff, dsp_silence, sizeof(dsp->rx_buff)); + dsp->rx_init = 1; /* rx_W and rx_R will be adjusted on first frame */ + dsp->rx_W = 0; + dsp->rx_R = 0; + + list_add_tail(&member->list, &conf->mlist); + + dsp->conf = conf; + dsp->member = member; + + return 0; +} + + +/* + * del member from conference + */ +int +dsp_cmx_del_conf_member(struct dsp *dsp) +{ + struct dsp_conf_member *member; + + if (!dsp) { + printk(KERN_WARNING "%s: dsp is 0.\n", + __func__); + return -EINVAL; + } + + if (!dsp->conf) { + printk(KERN_WARNING "%s: dsp is not in a conf.\n", + __func__); + return -EINVAL; + } + + if (list_empty(&dsp->conf->mlist)) { + printk(KERN_WARNING "%s: dsp has linked an empty conf.\n", + __func__); + return -EINVAL; + } + + /* find us in conf */ + list_for_each_entry(member, &dsp->conf->mlist, list) { + if (member->dsp == dsp) { + list_del(&member->list); + dsp->conf = NULL; + dsp->member = NULL; + kfree(member); + return 0; + } + } + printk(KERN_WARNING + "%s: dsp is not present in its own conf_meber list.\n", + __func__); + + return -EINVAL; +} + + +/* + * new conference + */ +static struct dsp_conf +*dsp_cmx_new_conf(u32 id) +{ + struct dsp_conf *conf; + + if (!id) { + printk(KERN_WARNING "%s: id is 0.\n", + __func__); + return NULL; + } + + conf = kzalloc(sizeof(struct dsp_conf), GFP_ATOMIC); + if (!conf) { + printk(KERN_ERR "kmalloc struct dsp_conf failed\n"); + return NULL; + } + INIT_LIST_HEAD(&conf->mlist); + conf->id = id; + + list_add_tail(&conf->list, &conf_ilist); + + return conf; +} + + +/* + * del conference + */ +int +dsp_cmx_del_conf(struct dsp_conf *conf) +{ + if (!conf) { + printk(KERN_WARNING "%s: conf is null.\n", + __func__); + return -EINVAL; + } + + if (!list_empty(&conf->mlist)) { + printk(KERN_WARNING "%s: conf not empty.\n", + __func__); + return -EINVAL; + } + list_del(&conf->list); + kfree(conf); + + return 0; +} + + +/* + * send HW message to hfc card + */ +static void +dsp_cmx_hw_message(struct dsp *dsp, u32 message, u32 param1, u32 param2, + u32 param3, u32 param4) +{ + struct mISDN_ctrl_req cq; + + memset(&cq, 0, sizeof(cq)); + cq.op = message; + cq.p1 = param1 | (param2 << 8); + cq.p2 = param3 | (param4 << 8); + if (dsp->ch.peer) + dsp->ch.peer->ctrl(dsp->ch.peer, CONTROL_CHANNEL, &cq); +} + + +/* + * do hardware update and set the software/hardware flag + * + * either a conference or a dsp instance can be given + * if only dsp instance is given, the instance is not associated with a conf + * and therefore removed. if a conference is given, the dsp is expected to + * be member of that conference. + */ +void +dsp_cmx_hardware(struct dsp_conf *conf, struct dsp *dsp) +{ + struct dsp_conf_member *member, *nextm; + struct dsp *finddsp; + int memb = 0, i, ii, i1, i2; + int freeunits[8]; + u_char freeslots[256]; + int same_hfc = -1, same_pcm = -1, current_conf = -1, + all_conf = 1; + + /* dsp gets updated (no conf) */ + if (!conf) { + if (!dsp) + return; + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG "%s checking dsp %s\n", + __func__, dsp->name); +one_member: + /* remove HFC conference if enabled */ + if (dsp->hfc_conf >= 0) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s removing %s from HFC conf %d " + "because dsp is split\n", __func__, + dsp->name, dsp->hfc_conf); + dsp_cmx_hw_message(dsp, MISDN_CTRL_HFC_CONF_SPLIT, + 0, 0, 0, 0); + dsp->hfc_conf = -1; + } + /* process hw echo */ + if (dsp->features.pcm_banks < 1) + return; + if (!dsp->echo) { + /* NO ECHO: remove PCM slot if assigned */ + if (dsp->pcm_slot_tx >= 0 || dsp->pcm_slot_rx >= 0) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG "%s removing %s from" + " PCM slot %d (TX) %d (RX) because" + " dsp is split (no echo)\n", + __func__, dsp->name, + dsp->pcm_slot_tx, dsp->pcm_slot_rx); + dsp_cmx_hw_message(dsp, MISDN_CTRL_HFC_PCM_DISC, + 0, 0, 0, 0); + dsp->pcm_slot_tx = -1; + dsp->pcm_bank_tx = -1; + dsp->pcm_slot_rx = -1; + dsp->pcm_bank_rx = -1; + } + return; + } + /* ECHO: already echo */ + if (dsp->pcm_slot_tx >= 0 && dsp->pcm_slot_rx < 0 && + dsp->pcm_bank_tx == 2 && dsp->pcm_bank_rx == 2) + return; + /* ECHO: if slot already assigned */ + if (dsp->pcm_slot_tx >= 0) { + dsp->pcm_slot_rx = dsp->pcm_slot_tx; + dsp->pcm_bank_tx = 2; /* 2 means loop */ + dsp->pcm_bank_rx = 2; + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s refresh %s for echo using slot %d\n", + __func__, dsp->name, + dsp->pcm_slot_tx); + dsp_cmx_hw_message(dsp, MISDN_CTRL_HFC_PCM_CONN, + dsp->pcm_slot_tx, 2, dsp->pcm_slot_rx, 2); + return; + } + /* ECHO: find slot */ + dsp->pcm_slot_tx = -1; + dsp->pcm_slot_rx = -1; + memset(freeslots, 1, sizeof(freeslots)); + list_for_each_entry(finddsp, &dsp_ilist, list) { + if (finddsp->features.pcm_id == dsp->features.pcm_id) { + if (finddsp->pcm_slot_rx >= 0 && + finddsp->pcm_slot_rx < sizeof(freeslots)) + freeslots[finddsp->pcm_slot_tx] = 0; + if (finddsp->pcm_slot_tx >= 0 && + finddsp->pcm_slot_tx < sizeof(freeslots)) + freeslots[finddsp->pcm_slot_rx] = 0; + } + } + i = 0; + ii = dsp->features.pcm_slots; + while (i < ii) { + if (freeslots[i]) + break; + i++; + } + if (i == ii) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s no slot available for echo\n", + __func__); + /* no more slots available */ + return; + } + /* assign free slot */ + dsp->pcm_slot_tx = i; + dsp->pcm_slot_rx = i; + dsp->pcm_bank_tx = 2; /* loop */ + dsp->pcm_bank_rx = 2; + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s assign echo for %s using slot %d\n", + __func__, dsp->name, dsp->pcm_slot_tx); + dsp_cmx_hw_message(dsp, MISDN_CTRL_HFC_PCM_CONN, + dsp->pcm_slot_tx, 2, dsp->pcm_slot_rx, 2); + return; + } + + /* conf gets updated (all members) */ + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG "%s checking conference %d\n", + __func__, conf->id); + + if (list_empty(&conf->mlist)) { + printk(KERN_ERR "%s: conference whithout members\n", + __func__); + return; + } + member = list_entry(conf->mlist.next, struct dsp_conf_member, list); + same_hfc = member->dsp->features.hfc_id; + same_pcm = member->dsp->features.pcm_id; + /* check all members in our conference */ + list_for_each_entry(member, &conf->mlist, list) { + /* check if member uses mixing */ + if (member->dsp->tx_mix) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s dsp %s cannot form a conf, because " + "tx_mix is turned on\n", __func__, + member->dsp->name); +conf_software: + list_for_each_entry(member, &conf->mlist, list) { + dsp = member->dsp; + /* remove HFC conference if enabled */ + if (dsp->hfc_conf >= 0) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s removing %s from HFC " + "conf %d because not " + "possible with hardware\n", + __func__, + dsp->name, + dsp->hfc_conf); + dsp_cmx_hw_message(dsp, + MISDN_CTRL_HFC_CONF_SPLIT, + 0, 0, 0, 0); + dsp->hfc_conf = -1; + } + /* remove PCM slot if assigned */ + if (dsp->pcm_slot_tx >= 0 || + dsp->pcm_slot_rx >= 0) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG "%s removing " + "%s from PCM slot %d (TX)" + " slot %d (RX) because not" + " possible with hardware\n", + __func__, + dsp->name, + dsp->pcm_slot_tx, + dsp->pcm_slot_rx); + dsp_cmx_hw_message(dsp, + MISDN_CTRL_HFC_PCM_DISC, + 0, 0, 0, 0); + dsp->pcm_slot_tx = -1; + dsp->pcm_bank_tx = -1; + dsp->pcm_slot_rx = -1; + dsp->pcm_bank_rx = -1; + } + } + conf->hardware = 0; + conf->software = 1; + return; + } + /* check if member has echo turned on */ + if (member->dsp->echo) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s dsp %s cannot form a conf, because " + "echo is turned on\n", __func__, + member->dsp->name); + goto conf_software; + } + /* check if member has tx_mix turned on */ + if (member->dsp->tx_mix) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s dsp %s cannot form a conf, because " + "tx_mix is turned on\n", + __func__, member->dsp->name); + goto conf_software; + } + /* check if member changes volume at an not suppoted level */ + if (member->dsp->tx_volume) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s dsp %s cannot form a conf, because " + "tx_volume is changed\n", + __func__, member->dsp->name); + goto conf_software; + } + if (member->dsp->rx_volume) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s dsp %s cannot form a conf, because " + "rx_volume is changed\n", + __func__, member->dsp->name); + goto conf_software; + } + /* check if tx-data turned on */ + if (member->dsp->tx_data) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s dsp %s cannot form a conf, because " + "tx_data is turned on\n", + __func__, member->dsp->name); + goto conf_software; + } + /* check if pipeline exists */ + if (member->dsp->pipeline.inuse) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s dsp %s cannot form a conf, because " + "pipeline exists\n", __func__, + member->dsp->name); + goto conf_software; + } + /* check if encryption is enabled */ + if (member->dsp->bf_enable) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG "%s dsp %s cannot form a " + "conf, because encryption is enabled\n", + __func__, member->dsp->name); + goto conf_software; + } + /* check if member is on a card with PCM support */ + if (member->dsp->features.pcm_id < 0) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s dsp %s cannot form a conf, because " + "dsp has no PCM bus\n", + __func__, member->dsp->name); + goto conf_software; + } + /* check if relations are on the same PCM bus */ + if (member->dsp->features.pcm_id != same_pcm) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s dsp %s cannot form a conf, because " + "dsp is on a different PCM bus than the " + "first dsp\n", + __func__, member->dsp->name); + goto conf_software; + } + /* determine if members are on the same hfc chip */ + if (same_hfc != member->dsp->features.hfc_id) + same_hfc = -1; + /* if there are members already in a conference */ + if (current_conf < 0 && member->dsp->hfc_conf >= 0) + current_conf = member->dsp->hfc_conf; + /* if any member is not in a conference */ + if (member->dsp->hfc_conf < 0) + all_conf = 0; + + memb++; + } + + /* if no member, this is an error */ + if (memb < 1) + return; + + /* one member */ + if (memb == 1) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s conf %d cannot form a HW conference, " + "because dsp is alone\n", __func__, conf->id); + conf->hardware = 0; + conf->software = 0; + member = list_entry(conf->mlist.next, struct dsp_conf_member, + list); + dsp = member->dsp; + goto one_member; + } + + /* + * ok, now we are sure that all members are on the same pcm. + * now we will see if we have only two members, so we can do + * crossconnections, which don't have any limitations. + */ + + /* if we have only two members */ + if (memb == 2) { + member = list_entry(conf->mlist.next, struct dsp_conf_member, + list); + nextm = list_entry(member->list.next, struct dsp_conf_member, + list); + /* remove HFC conference if enabled */ + if (member->dsp->hfc_conf >= 0) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s removing %s from HFC conf %d because " + "two parties require only a PCM slot\n", + __func__, member->dsp->name, + member->dsp->hfc_conf); + dsp_cmx_hw_message(member->dsp, + MISDN_CTRL_HFC_CONF_SPLIT, 0, 0, 0, 0); + member->dsp->hfc_conf = -1; + } + if (nextm->dsp->hfc_conf >= 0) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s removing %s from HFC conf %d because " + "two parties require only a PCM slot\n", + __func__, nextm->dsp->name, + nextm->dsp->hfc_conf); + dsp_cmx_hw_message(nextm->dsp, + MISDN_CTRL_HFC_CONF_SPLIT, 0, 0, 0, 0); + nextm->dsp->hfc_conf = -1; + } + /* if members have two banks (and not on the same chip) */ + if (member->dsp->features.pcm_banks > 1 && + nextm->dsp->features.pcm_banks > 1 && + member->dsp->features.hfc_id != + nextm->dsp->features.hfc_id) { + /* if both members have same slots with crossed banks */ + if (member->dsp->pcm_slot_tx >= 0 && + member->dsp->pcm_slot_rx >= 0 && + nextm->dsp->pcm_slot_tx >= 0 && + nextm->dsp->pcm_slot_rx >= 0 && + nextm->dsp->pcm_slot_tx == + member->dsp->pcm_slot_rx && + nextm->dsp->pcm_slot_rx == + member->dsp->pcm_slot_tx && + nextm->dsp->pcm_slot_tx == + member->dsp->pcm_slot_tx && + member->dsp->pcm_bank_tx != + member->dsp->pcm_bank_rx && + nextm->dsp->pcm_bank_tx != + nextm->dsp->pcm_bank_rx) { + /* all members have same slot */ + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s dsp %s & %s stay joined on " + "PCM slot %d bank %d (TX) bank %d " + "(RX) (on different chips)\n", + __func__, + member->dsp->name, + nextm->dsp->name, + member->dsp->pcm_slot_tx, + member->dsp->pcm_bank_tx, + member->dsp->pcm_bank_rx); + conf->hardware = 0; + conf->software = 1; + return; + } + /* find a new slot */ + memset(freeslots, 1, sizeof(freeslots)); + list_for_each_entry(dsp, &dsp_ilist, list) { + if (dsp != member->dsp && + dsp != nextm->dsp && + member->dsp->features.pcm_id == + dsp->features.pcm_id) { + if (dsp->pcm_slot_rx >= 0 && + dsp->pcm_slot_rx < + sizeof(freeslots)) + freeslots[dsp->pcm_slot_tx] = 0; + if (dsp->pcm_slot_tx >= 0 && + dsp->pcm_slot_tx < + sizeof(freeslots)) + freeslots[dsp->pcm_slot_rx] = 0; + } + } + i = 0; + ii = member->dsp->features.pcm_slots; + while (i < ii) { + if (freeslots[i]) + break; + i++; + } + if (i == ii) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s no slot available for " + "%s & %s\n", __func__, + member->dsp->name, + nextm->dsp->name); + /* no more slots available */ + goto conf_software; + } + /* assign free slot */ + member->dsp->pcm_slot_tx = i; + member->dsp->pcm_slot_rx = i; + nextm->dsp->pcm_slot_tx = i; + nextm->dsp->pcm_slot_rx = i; + member->dsp->pcm_bank_rx = 0; + member->dsp->pcm_bank_tx = 1; + nextm->dsp->pcm_bank_rx = 1; + nextm->dsp->pcm_bank_tx = 0; + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s adding %s & %s to new PCM slot %d " + "(TX and RX on different chips) because " + "both members have not same slots\n", + __func__, + member->dsp->name, + nextm->dsp->name, + member->dsp->pcm_slot_tx); + dsp_cmx_hw_message(member->dsp, MISDN_CTRL_HFC_PCM_CONN, + member->dsp->pcm_slot_tx, member->dsp->pcm_bank_tx, + member->dsp->pcm_slot_rx, member->dsp->pcm_bank_rx); + dsp_cmx_hw_message(nextm->dsp, MISDN_CTRL_HFC_PCM_CONN, + nextm->dsp->pcm_slot_tx, nextm->dsp->pcm_bank_tx, + nextm->dsp->pcm_slot_rx, nextm->dsp->pcm_bank_rx); + conf->hardware = 1; + conf->software = 0; + return; + /* if members have one bank (or on the same chip) */ + } else { + /* if both members have different crossed slots */ + if (member->dsp->pcm_slot_tx >= 0 && + member->dsp->pcm_slot_rx >= 0 && + nextm->dsp->pcm_slot_tx >= 0 && + nextm->dsp->pcm_slot_rx >= 0 && + nextm->dsp->pcm_slot_tx == + member->dsp->pcm_slot_rx && + nextm->dsp->pcm_slot_rx == + member->dsp->pcm_slot_tx && + member->dsp->pcm_slot_tx != + member->dsp->pcm_slot_rx && + member->dsp->pcm_bank_tx == 0 && + member->dsp->pcm_bank_rx == 0 && + nextm->dsp->pcm_bank_tx == 0 && + nextm->dsp->pcm_bank_rx == 0) { + /* all members have same slot */ + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s dsp %s & %s stay joined on PCM " + "slot %d (TX) %d (RX) on same chip " + "or one bank PCM)\n", __func__, + member->dsp->name, + nextm->dsp->name, + member->dsp->pcm_slot_tx, + member->dsp->pcm_slot_rx); + conf->hardware = 0; + conf->software = 1; + return; + } + /* find two new slot */ + memset(freeslots, 1, sizeof(freeslots)); + list_for_each_entry(dsp, &dsp_ilist, list) { + if (dsp != member->dsp && + dsp != nextm->dsp && + member->dsp->features.pcm_id == + dsp->features.pcm_id) { + if (dsp->pcm_slot_rx >= 0 && + dsp->pcm_slot_rx < + sizeof(freeslots)) + freeslots[dsp->pcm_slot_tx] = 0; + if (dsp->pcm_slot_tx >= 0 && + dsp->pcm_slot_tx < + sizeof(freeslots)) + freeslots[dsp->pcm_slot_rx] = 0; + } + } + i1 = 0; + ii = member->dsp->features.pcm_slots; + while (i1 < ii) { + if (freeslots[i1]) + break; + i1++; + } + if (i1 == ii) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s no slot available " + "for %s & %s\n", __func__, + member->dsp->name, + nextm->dsp->name); + /* no more slots available */ + goto conf_software; + } + i2 = i1+1; + while (i2 < ii) { + if (freeslots[i2]) + break; + i2++; + } + if (i2 == ii) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s no slot available " + "for %s & %s\n", + __func__, + member->dsp->name, + nextm->dsp->name); + /* no more slots available */ + goto conf_software; + } + /* assign free slots */ + member->dsp->pcm_slot_tx = i1; + member->dsp->pcm_slot_rx = i2; + nextm->dsp->pcm_slot_tx = i2; + nextm->dsp->pcm_slot_rx = i1; + member->dsp->pcm_bank_rx = 0; + member->dsp->pcm_bank_tx = 0; + nextm->dsp->pcm_bank_rx = 0; + nextm->dsp->pcm_bank_tx = 0; + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s adding %s & %s to new PCM slot %d " + "(TX) %d (RX) on same chip or one bank " + "PCM, because both members have not " + "crossed slots\n", __func__, + member->dsp->name, + nextm->dsp->name, + member->dsp->pcm_slot_tx, + member->dsp->pcm_slot_rx); + dsp_cmx_hw_message(member->dsp, MISDN_CTRL_HFC_PCM_CONN, + member->dsp->pcm_slot_tx, member->dsp->pcm_bank_tx, + member->dsp->pcm_slot_rx, member->dsp->pcm_bank_rx); + dsp_cmx_hw_message(nextm->dsp, MISDN_CTRL_HFC_PCM_CONN, + nextm->dsp->pcm_slot_tx, nextm->dsp->pcm_bank_tx, + nextm->dsp->pcm_slot_rx, nextm->dsp->pcm_bank_rx); + conf->hardware = 1; + conf->software = 0; + return; + } + } + + /* + * if we have more than two, we may check if we have a conference + * unit available on the chip. also all members must be on the same + */ + + /* if not the same HFC chip */ + if (same_hfc < 0) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s conference %d cannot be formed, because " + "members are on different chips or not " + "on HFC chip\n", + __func__, conf->id); + goto conf_software; + } + + /* for more than two members.. */ + + /* in case of hdlc, we change to software */ + if (dsp->hdlc) + goto conf_software; + + /* if all members already have the same conference */ + if (all_conf) + return; + + /* + * if there is an existing conference, but not all members have joined + */ + if (current_conf >= 0) { +join_members: + list_for_each_entry(member, &conf->mlist, list) { + /* join to current conference */ + if (member->dsp->hfc_conf == current_conf) + continue; + /* get a free timeslot first */ + memset(freeslots, 1, sizeof(freeslots)); + list_for_each_entry(dsp, &dsp_ilist, list) { + /* + * not checking current member, because + * slot will be overwritten. + */ + if ( + dsp != member->dsp && + /* dsp must be on the same PCM */ + member->dsp->features.pcm_id == + dsp->features.pcm_id) { + /* dsp must be on a slot */ + if (dsp->pcm_slot_tx >= 0 && + dsp->pcm_slot_tx < + sizeof(freeslots)) + freeslots[dsp->pcm_slot_tx] = 0; + if (dsp->pcm_slot_rx >= 0 && + dsp->pcm_slot_rx < + sizeof(freeslots)) + freeslots[dsp->pcm_slot_rx] = 0; + } + } + i = 0; + ii = member->dsp->features.pcm_slots; + while (i < ii) { + if (freeslots[i]) + break; + i++; + } + if (i == ii) { + /* no more slots available */ + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s conference %d cannot be formed," + " because no slot free\n", + __func__, conf->id); + goto conf_software; + } + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s changing dsp %s to HW conference " + "%d slot %d\n", __func__, + member->dsp->name, current_conf, i); + /* assign free slot & set PCM & join conf */ + member->dsp->pcm_slot_tx = i; + member->dsp->pcm_slot_rx = i; + member->dsp->pcm_bank_tx = 2; /* loop */ + member->dsp->pcm_bank_rx = 2; + member->dsp->hfc_conf = current_conf; + dsp_cmx_hw_message(member->dsp, MISDN_CTRL_HFC_PCM_CONN, + i, 2, i, 2); + dsp_cmx_hw_message(member->dsp, + MISDN_CTRL_HFC_CONF_JOIN, current_conf, 0, 0, 0); + } + return; + } + + /* + * no member is in a conference yet, so we find a free one + */ + memset(freeunits, 1, sizeof(freeunits)); + list_for_each_entry(dsp, &dsp_ilist, list) { + /* dsp must be on the same chip */ + if (dsp->features.hfc_id == same_hfc && + /* dsp must have joined a HW conference */ + dsp->hfc_conf >= 0 && + /* slot must be within range */ + dsp->hfc_conf < 8) + freeunits[dsp->hfc_conf] = 0; + } + i = 0; + ii = 8; + while (i < ii) { + if (freeunits[i]) + break; + i++; + } + if (i == ii) { + /* no more conferences available */ + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s conference %d cannot be formed, because " + "no conference number free\n", + __func__, conf->id); + goto conf_software; + } + /* join all members */ + current_conf = i; + goto join_members; +} + + +/* + * conf_id != 0: join or change conference + * conf_id == 0: split from conference if not already + */ +int +dsp_cmx_conf(struct dsp *dsp, u32 conf_id) +{ + int err; + struct dsp_conf *conf; + struct dsp_conf_member *member; + + /* if conference doesn't change */ + if (dsp->conf_id == conf_id) + return 0; + + /* first remove us from current conf */ + if (dsp->conf_id) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG "removing us from conference %d\n", + dsp->conf->id); + /* remove us from conf */ + conf = dsp->conf; + err = dsp_cmx_del_conf_member(dsp); + if (err) + return err; + dsp->conf_id = 0; + + /* update hardware */ + dsp_cmx_hardware(NULL, dsp); + + /* conf now empty? */ + if (list_empty(&conf->mlist)) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "conference is empty, so we remove it.\n"); + err = dsp_cmx_del_conf(conf); + if (err) + return err; + } else { + /* update members left on conf */ + dsp_cmx_hardware(conf, NULL); + } + } + + /* if split */ + if (!conf_id) + return 0; + + /* now add us to conf */ + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG "searching conference %d\n", + conf_id); + conf = dsp_cmx_search_conf(conf_id); + if (!conf) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "conference doesn't exist yet, creating.\n"); + /* the conference doesn't exist, so we create */ + conf = dsp_cmx_new_conf(conf_id); + if (!conf) + return -EINVAL; + } else if (!list_empty(&conf->mlist)) { + member = list_entry(conf->mlist.next, struct dsp_conf_member, + list); + if (dsp->hdlc && !member->dsp->hdlc) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "cannot join transparent conference.\n"); + return -EINVAL; + } + if (!dsp->hdlc && member->dsp->hdlc) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "cannot join hdlc conference.\n"); + return -EINVAL; + } + } + /* add conference member */ + err = dsp_cmx_add_conf_member(dsp, conf); + if (err) + return err; + dsp->conf_id = conf_id; + + /* if we are alone, we do nothing! */ + if (list_empty(&conf->mlist)) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "we are alone in this conference, so exit.\n"); + /* update hardware */ + dsp_cmx_hardware(NULL, dsp); + return 0; + } + + /* update members on conf */ + dsp_cmx_hardware(conf, NULL); + + return 0; +} + + +/* + * audio data is received from card + */ +void +dsp_cmx_receive(struct dsp *dsp, struct sk_buff *skb) +{ + u8 *d, *p; + int len = skb->len; + struct mISDNhead *hh = mISDN_HEAD_P(skb); + int w, i, ii; + + /* check if we have sompen */ + if (len < 1) + return; + + /* half of the buffer should be larger than maximum packet size */ + if (len >= CMX_BUFF_HALF) { + printk(KERN_ERR + "%s line %d: packet from card is too large (%d bytes). " + "please make card send smaller packets OR increase " + "CMX_BUFF_SIZE\n", __FILE__, __LINE__, len); + return; + } + + /* + * initialize pointers if not already - + * also add delay if requested by PH_SIGNAL + */ + if (dsp->rx_init) { + dsp->rx_init = 0; + if (dsp->features.unordered) { + dsp->rx_R = (hh->id & CMX_BUFF_MASK); + dsp->rx_W = (dsp->rx_R + dsp->cmx_delay) + & CMX_BUFF_MASK; + } else { + dsp->rx_R = 0; + dsp->rx_W = dsp->cmx_delay; + } + } + /* if frame contains time code, write directly */ + if (dsp->features.unordered) { + dsp->rx_W = (hh->id & CMX_BUFF_MASK); + /* printk(KERN_DEBUG "%s %08x\n", dsp->name, hh->id); */ + } + /* + * if we underrun (or maybe overrun), + * we set our new read pointer, and write silence to buffer + */ + if (((dsp->rx_W-dsp->rx_R) & CMX_BUFF_MASK) >= CMX_BUFF_HALF) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "cmx_receive(dsp=%lx): UNDERRUN (or overrun the " + "maximum delay), adjusting read pointer! " + "(inst %s)\n", (u_long)dsp, dsp->name); + /* flush buffer */ + if (dsp->features.unordered) { + dsp->rx_R = (hh->id & CMX_BUFF_MASK); + dsp->rx_W = (dsp->rx_R + dsp->cmx_delay) + & CMX_BUFF_MASK; + } else { + dsp->rx_R = 0; + dsp->rx_W = dsp->cmx_delay; + } + memset(dsp->rx_buff, dsp_silence, sizeof(dsp->rx_buff)); + } + /* if we have reached double delay, jump back to middle */ + if (dsp->cmx_delay) + if (((dsp->rx_W - dsp->rx_R) & CMX_BUFF_MASK) >= + (dsp->cmx_delay << 1)) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "cmx_receive(dsp=%lx): OVERRUN (because " + "twice the delay is reached), adjusting " + "read pointer! (inst %s)\n", + (u_long)dsp, dsp->name); + /* flush buffer */ + if (dsp->features.unordered) { + dsp->rx_R = (hh->id & CMX_BUFF_MASK); + dsp->rx_W = (dsp->rx_R + dsp->cmx_delay) + & CMX_BUFF_MASK; + } else { + dsp->rx_R = 0; + dsp->rx_W = dsp->cmx_delay; + } + memset(dsp->rx_buff, dsp_silence, sizeof(dsp->rx_buff)); + } + + /* show where to write */ +#ifdef CMX_DEBUG + printk(KERN_DEBUG + "cmx_receive(dsp=%lx): rx_R(dsp)=%05x rx_W(dsp)=%05x len=%d %s\n", + (u_long)dsp, dsp->rx_R, dsp->rx_W, len, dsp->name); +#endif + + /* write data into rx_buffer */ + p = skb->data; + d = dsp->rx_buff; + w = dsp->rx_W; + i = 0; + ii = len; + while (i < ii) { + d[w++ & CMX_BUFF_MASK] = *p++; + i++; + } + + /* increase write-pointer */ + dsp->rx_W = ((dsp->rx_W+len) & CMX_BUFF_MASK); +} + + +/* + * send (mixed) audio data to card and control jitter + */ +static void +dsp_cmx_send_member(struct dsp *dsp, int len, s32 *c, int members) +{ + struct dsp_conf *conf = dsp->conf; + struct dsp *member, *other; + register s32 sample; + u8 *d, *p, *q, *o_q; + struct sk_buff *nskb, *txskb; + int r, rr, t, tt, o_r, o_rr; + int preload = 0; + struct mISDNhead *hh, *thh; + + /* don't process if: */ + if (!dsp->b_active) { /* if not active */ + dsp->last_tx = 0; + return; + } + if (dsp->pcm_slot_tx >= 0 && /* connected to pcm slot */ + dsp->tx_R == dsp->tx_W && /* AND no tx-data */ + !(dsp->tone.tone && dsp->tone.software)) { /* AND not soft tones */ + dsp->last_tx = 0; + return; + } + +#ifdef CMX_DEBUG + printk(KERN_DEBUG + "SEND members=%d dsp=%s, conf=%p, rx_R=%05x rx_W=%05x\n", + members, dsp->name, conf, dsp->rx_R, dsp->rx_W); +#endif + + /* preload if we have delay set */ + if (dsp->cmx_delay && !dsp->last_tx) { + preload = len; + if (preload < 128) + preload = 128; + } + + /* PREPARE RESULT */ + nskb = mI_alloc_skb(len + preload, GFP_ATOMIC); + if (!nskb) { + printk(KERN_ERR + "FATAL ERROR in mISDN_dsp.o: cannot alloc %d bytes\n", + len + preload); + return; + } + hh = mISDN_HEAD_P(nskb); + hh->prim = PH_DATA_REQ; + hh->id = 0; + dsp->last_tx = 1; + + /* set pointers, indexes and stuff */ + member = dsp; + p = dsp->tx_buff; /* transmit data */ + q = dsp->rx_buff; /* received data */ + d = skb_put(nskb, preload + len); /* result */ + t = dsp->tx_R; /* tx-pointers */ + tt = dsp->tx_W; + r = dsp->rx_R; /* rx-pointers */ + rr = (r + len) & CMX_BUFF_MASK; + + /* preload with silence, if required */ + if (preload) { + memset(d, dsp_silence, preload); + d += preload; + } + + /* PROCESS TONES/TX-DATA ONLY */ + if (dsp->tone.tone && dsp->tone.software) { + /* -> copy tone */ + dsp_tone_copy(dsp, d, len); + dsp->tx_R = 0; /* clear tx buffer */ + dsp->tx_W = 0; + goto send_packet; + } + /* if we have tx-data but do not use mixing */ + if (!dsp->tx_mix && t != tt) { + /* -> send tx-data and continue when not enough */ +#ifdef CMX_TX_DEBUG + sprintf(debugbuf, "TX sending (%04x-%04x)%p: ", t, tt, p); +#endif + while (r != rr && t != tt) { +#ifdef CMX_TX_DEBUG + if (strlen(debugbuf) < 48) + sprintf(debugbuf+strlen(debugbuf), " %02x", p[t]); +#endif + *d++ = p[t]; /* write tx_buff */ + t = (t+1) & CMX_BUFF_MASK; + r = (r+1) & CMX_BUFF_MASK; + } + if (r == rr) { + dsp->tx_R = t; +#ifdef CMX_TX_DEBUG + printk(KERN_DEBUG "%s\n", debugbuf); +#endif + goto send_packet; + } + } +#ifdef CMX_TX_DEBUG + printk(KERN_DEBUG "%s\n", debugbuf); +#endif + + /* PROCESS DATA (one member / no conf) */ + if (!conf || members <= 1) { + /* -> if echo is NOT enabled */ + if (!dsp->echo) { + /* -> send tx-data if available or use 0-volume */ + while (r != rr && t != tt) { + *d++ = p[t]; /* write tx_buff */ + t = (t+1) & CMX_BUFF_MASK; + r = (r+1) & CMX_BUFF_MASK; + } + if (r != rr) + memset(d, dsp_silence, (rr-r)&CMX_BUFF_MASK); + /* -> if echo is enabled */ + } else { + /* + * -> mix tx-data with echo if available, + * or use echo only + */ + while (r != rr && t != tt) { + *d++ = dsp_audio_mix_law[(p[t]<<8)|q[r]]; + t = (t+1) & CMX_BUFF_MASK; + r = (r+1) & CMX_BUFF_MASK; + } + while (r != rr) { + *d++ = q[r]; /* echo */ + r = (r+1) & CMX_BUFF_MASK; + } + } + dsp->tx_R = t; + goto send_packet; + } + /* PROCESS DATA (two members) */ +#ifdef CMX_CONF_DEBUG + if (0) { +#else + if (members == 2) { +#endif + /* "other" becomes other party */ + other = (list_entry(conf->mlist.next, + struct dsp_conf_member, list))->dsp; + if (other == member) + other = (list_entry(conf->mlist.prev, + struct dsp_conf_member, list))->dsp; + o_q = other->rx_buff; /* received data */ + o_rr = (other->rx_R + len) & CMX_BUFF_MASK; + /* end of rx-pointer */ + o_r = (o_rr - rr + r) & CMX_BUFF_MASK; + /* start rx-pointer at current read position*/ + /* -> if echo is NOT enabled */ + if (!dsp->echo) { + /* + * -> copy other member's rx-data, + * if tx-data is available, mix + */ + while (o_r != o_rr && t != tt) { + *d++ = dsp_audio_mix_law[(p[t]<<8)|o_q[o_r]]; + t = (t+1) & CMX_BUFF_MASK; + o_r = (o_r+1) & CMX_BUFF_MASK; + } + while (o_r != o_rr) { + *d++ = o_q[o_r]; + o_r = (o_r+1) & CMX_BUFF_MASK; + } + /* -> if echo is enabled */ + } else { + /* + * -> mix other member's rx-data with echo, + * if tx-data is available, mix + */ + while (r != rr && t != tt) { + sample = dsp_audio_law_to_s32[p[t]] + + dsp_audio_law_to_s32[q[r]] + + dsp_audio_law_to_s32[o_q[o_r]]; + if (sample < -32768) + sample = -32768; + else if (sample > 32767) + sample = 32767; + *d++ = dsp_audio_s16_to_law[sample & 0xffff]; + /* tx-data + rx_data + echo */ + t = (t+1) & CMX_BUFF_MASK; + r = (r+1) & CMX_BUFF_MASK; + o_r = (o_r+1) & CMX_BUFF_MASK; + } + while (r != rr) { + *d++ = dsp_audio_mix_law[(q[r]<<8)|o_q[o_r]]; + r = (r+1) & CMX_BUFF_MASK; + o_r = (o_r+1) & CMX_BUFF_MASK; + } + } + dsp->tx_R = t; + goto send_packet; + } +#ifdef DSP_NEVER_DEFINED + } +#endif + /* PROCESS DATA (three or more members) */ + /* -> if echo is NOT enabled */ + if (!dsp->echo) { + /* + * -> substract rx-data from conf-data, + * if tx-data is available, mix + */ + while (r != rr && t != tt) { + sample = dsp_audio_law_to_s32[p[t]] + *c++ - + dsp_audio_law_to_s32[q[r]]; + if (sample < -32768) + sample = -32768; + else if (sample > 32767) + sample = 32767; + *d++ = dsp_audio_s16_to_law[sample & 0xffff]; + /* conf-rx+tx */ + r = (r+1) & CMX_BUFF_MASK; + t = (t+1) & CMX_BUFF_MASK; + } + while (r != rr) { + sample = *c++ - dsp_audio_law_to_s32[q[r]]; + if (sample < -32768) + sample = -32768; + else if (sample > 32767) + sample = 32767; + *d++ = dsp_audio_s16_to_law[sample & 0xffff]; + /* conf-rx */ + r = (r+1) & CMX_BUFF_MASK; + } + /* -> if echo is enabled */ + } else { + /* + * -> encode conf-data, if tx-data + * is available, mix + */ + while (r != rr && t != tt) { + sample = dsp_audio_law_to_s32[p[t]] + *c++; + if (sample < -32768) + sample = -32768; + else if (sample > 32767) + sample = 32767; + *d++ = dsp_audio_s16_to_law[sample & 0xffff]; + /* conf(echo)+tx */ + t = (t+1) & CMX_BUFF_MASK; + r = (r+1) & CMX_BUFF_MASK; + } + while (r != rr) { + sample = *c++; + if (sample < -32768) + sample = -32768; + else if (sample > 32767) + sample = 32767; + *d++ = dsp_audio_s16_to_law[sample & 0xffff]; + /* conf(echo) */ + r = (r+1) & CMX_BUFF_MASK; + } + } + dsp->tx_R = t; + goto send_packet; + +send_packet: + /* + * send tx-data if enabled - don't filter, + * becuase we want what we send, not what we filtered + */ + if (dsp->tx_data) { + /* PREPARE RESULT */ + txskb = mI_alloc_skb(len, GFP_ATOMIC); + if (!txskb) { + printk(KERN_ERR + "FATAL ERROR in mISDN_dsp.o: " + "cannot alloc %d bytes\n", len); + } else { + thh = mISDN_HEAD_P(txskb); + thh->prim = DL_DATA_REQ; + thh->id = 0; + memcpy(skb_put(txskb, len), nskb->data+preload, len); + /* queue (trigger later) */ + skb_queue_tail(&dsp->sendq, txskb); + } + } + /* adjust volume */ + if (dsp->tx_volume) + dsp_change_volume(nskb, dsp->tx_volume); + /* pipeline */ + if (dsp->pipeline.inuse) + dsp_pipeline_process_tx(&dsp->pipeline, nskb->data, nskb->len); + /* crypt */ + if (dsp->bf_enable) + dsp_bf_encrypt(dsp, nskb->data, nskb->len); + /* queue and trigger */ + skb_queue_tail(&dsp->sendq, nskb); + schedule_work(&dsp->workq); +} + +u32 samplecount; +struct timer_list dsp_spl_tl; +u32 dsp_spl_jiffies; /* calculate the next time to fire */ +u32 dsp_start_jiffies; /* jiffies at the time, the calculation begins */ +struct timeval dsp_start_tv; /* time at start of calculation */ + +void +dsp_cmx_send(void *arg) +{ + struct dsp_conf *conf; + struct dsp_conf_member *member; + struct dsp *dsp; + int mustmix, members; + s32 mixbuffer[MAX_POLL+100], *c; + u8 *p, *q; + int r, rr; + int jittercheck = 0, delay, i; + u_long flags; + struct timeval tv; + u32 elapsed; + s16 length; + + /* lock */ + spin_lock_irqsave(&dsp_lock, flags); + + if (!dsp_start_tv.tv_sec) { + do_gettimeofday(&dsp_start_tv); + length = dsp_poll; + } else { + do_gettimeofday(&tv); + elapsed = ((tv.tv_sec - dsp_start_tv.tv_sec) * 8000) + + ((s32)(tv.tv_usec / 125) - (dsp_start_tv.tv_usec / 125)); + dsp_start_tv.tv_sec = tv.tv_sec; + dsp_start_tv.tv_usec = tv.tv_usec; + length = elapsed; + } + if (length > MAX_POLL + 100) + length = MAX_POLL + 100; +/* printk(KERN_DEBUG "len=%d dsp_count=0x%x.%04x dsp_poll_diff=0x%x.%04x\n", + length, dsp_count >> 16, dsp_count & 0xffff, dsp_poll_diff >> 16, + dsp_poll_diff & 0xffff); + */ + + /* + * check if jitter needs to be checked + * (this is about every second = 8192 samples) + */ + samplecount += length; + if ((samplecount & 8191) < length) + jittercheck = 1; + + /* loop all members that do not require conference mixing */ + list_for_each_entry(dsp, &dsp_ilist, list) { + if (dsp->hdlc) + continue; + conf = dsp->conf; + mustmix = 0; + members = 0; + if (conf) { + members = count_list_member(&conf->mlist); +#ifdef CMX_CONF_DEBUG + if (conf->software && members > 1) +#else + if (conf->software && members > 2) +#endif + mustmix = 1; + } + + /* transmission required */ + if (!mustmix) { + dsp_cmx_send_member(dsp, length, mixbuffer, members); + + /* + * unused mixbuffer is given to prevent a + * potential null-pointer-bug + */ + } + } + + /* loop all members that require conference mixing */ + list_for_each_entry(conf, &conf_ilist, list) { + /* count members and check hardware */ + members = count_list_member(&conf->mlist); +#ifdef CMX_CONF_DEBUG + if (conf->software && members > 1) { +#else + if (conf->software && members > 2) { +#endif + /* check for hdlc conf */ + member = list_entry(conf->mlist.next, + struct dsp_conf_member, list); + if (member->dsp->hdlc) + continue; + /* mix all data */ + memset(mixbuffer, 0, length*sizeof(s32)); + list_for_each_entry(member, &conf->mlist, list) { + dsp = member->dsp; + /* get range of data to mix */ + c = mixbuffer; + q = dsp->rx_buff; + r = dsp->rx_R; + rr = (r + length) & CMX_BUFF_MASK; + /* add member's data */ + while (r != rr) { + *c++ += dsp_audio_law_to_s32[q[r]]; + r = (r+1) & CMX_BUFF_MASK; + } + } + + /* process each member */ + list_for_each_entry(member, &conf->mlist, list) { + /* transmission */ + dsp_cmx_send_member(member->dsp, length, + mixbuffer, members); + } + } + } + + /* delete rx-data, increment buffers, change pointers */ + list_for_each_entry(dsp, &dsp_ilist, list) { + if (dsp->hdlc) + continue; + p = dsp->rx_buff; + q = dsp->tx_buff; + r = dsp->rx_R; + /* move receive pointer when receiving */ + if (!dsp->rx_is_off) { + rr = (r + length) & CMX_BUFF_MASK; + /* delete rx-data */ + while (r != rr) { + p[r] = dsp_silence; + r = (r+1) & CMX_BUFF_MASK; + } + /* increment rx-buffer pointer */ + dsp->rx_R = r; /* write incremented read pointer */ + } + + /* check current rx_delay */ + delay = (dsp->rx_W-dsp->rx_R) & CMX_BUFF_MASK; + if (delay >= CMX_BUFF_HALF) + delay = 0; /* will be the delay before next write */ + /* check for lower delay */ + if (delay < dsp->rx_delay[0]) + dsp->rx_delay[0] = delay; + /* check current tx_delay */ + delay = (dsp->tx_W-dsp->tx_R) & CMX_BUFF_MASK; + if (delay >= CMX_BUFF_HALF) + delay = 0; /* will be the delay before next write */ + /* check for lower delay */ + if (delay < dsp->tx_delay[0]) + dsp->tx_delay[0] = delay; + if (jittercheck) { + /* find the lowest of all rx_delays */ + delay = dsp->rx_delay[0]; + i = 1; + while (i < MAX_SECONDS_JITTER_CHECK) { + if (delay > dsp->rx_delay[i]) + delay = dsp->rx_delay[i]; + i++; + } + /* + * remove rx_delay only if we have delay AND we + * have not preset cmx_delay + */ + if (delay && !dsp->cmx_delay) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s lowest rx_delay of %d bytes for" + " dsp %s are now removed.\n", + __func__, delay, + dsp->name); + r = dsp->rx_R; + rr = (r + delay) & CMX_BUFF_MASK; + /* delete rx-data */ + while (r != rr) { + p[r] = dsp_silence; + r = (r+1) & CMX_BUFF_MASK; + } + /* increment rx-buffer pointer */ + dsp->rx_R = r; + /* write incremented read pointer */ + } + /* find the lowest of all tx_delays */ + delay = dsp->tx_delay[0]; + i = 1; + while (i < MAX_SECONDS_JITTER_CHECK) { + if (delay > dsp->tx_delay[i]) + delay = dsp->tx_delay[i]; + i++; + } + /* + * remove delay only if we have delay AND we + * have enabled tx_dejitter + */ + if (delay && dsp->tx_dejitter) { + if (dsp_debug & DEBUG_DSP_CMX) + printk(KERN_DEBUG + "%s lowest tx_delay of %d bytes for" + " dsp %s are now removed.\n", + __func__, delay, + dsp->name); + r = dsp->tx_R; + rr = (r + delay) & CMX_BUFF_MASK; + /* delete tx-data */ + while (r != rr) { + q[r] = dsp_silence; + r = (r+1) & CMX_BUFF_MASK; + } + /* increment rx-buffer pointer */ + dsp->tx_R = r; + /* write incremented read pointer */ + } + /* scroll up delays */ + i = MAX_SECONDS_JITTER_CHECK - 1; + while (i) { + dsp->rx_delay[i] = dsp->rx_delay[i-1]; + dsp->tx_delay[i] = dsp->tx_delay[i-1]; + i--; + } + dsp->tx_delay[0] = CMX_BUFF_HALF; /* (infinite) delay */ + dsp->rx_delay[0] = CMX_BUFF_HALF; /* (infinite) delay */ + } + } + + /* if next event would be in the past ... */ + if ((s32)(dsp_spl_jiffies+dsp_tics-jiffies) <= 0) + dsp_spl_jiffies = jiffies + 1; + else + dsp_spl_jiffies += dsp_tics; + + dsp_spl_tl.expires = dsp_spl_jiffies; + add_timer(&dsp_spl_tl); + + /* unlock */ + spin_unlock_irqrestore(&dsp_lock, flags); +} + +/* + * audio data is transmitted from upper layer to the dsp + */ +void +dsp_cmx_transmit(struct dsp *dsp, struct sk_buff *skb) +{ + u_int w, ww; + u8 *d, *p; + int space; /* todo: , l = skb->len; */ +#ifdef CMX_TX_DEBUG + char debugbuf[256] = ""; +#endif + + /* check if there is enough space, and then copy */ + w = dsp->tx_W; + ww = dsp->tx_R; + p = dsp->tx_buff; + d = skb->data; + space = ww-w; + if (space <= 0) + space += CMX_BUFF_SIZE; + /* write-pointer should not overrun nor reach read pointer */ + if (space-1 < skb->len) + /* write to the space we have left */ + ww = (ww - 1) & CMX_BUFF_MASK; + else + /* write until all byte are copied */ + ww = (w + skb->len) & CMX_BUFF_MASK; + dsp->tx_W = ww; + + /* show current buffer */ +#ifdef CMX_DEBUG + printk(KERN_DEBUG + "cmx_transmit(dsp=%lx) %d bytes to 0x%x-0x%x. %s\n", + (u_long)dsp, (ww-w)&CMX_BUFF_MASK, w, ww, dsp->name); +#endif + + /* copy transmit data to tx-buffer */ +#ifdef CMX_TX_DEBUG + sprintf(debugbuf, "TX getting (%04x-%04x)%p: ", w, ww, p); +#endif + while (w != ww) { +#ifdef CMX_TX_DEBUG + if (strlen(debugbuf) < 48) + sprintf(debugbuf+strlen(debugbuf), " %02x", *d); +#endif + p[w] = *d++; + w = (w+1) & CMX_BUFF_MASK; + } +#ifdef CMX_TX_DEBUG + printk(KERN_DEBUG "%s\n", debugbuf); +#endif + +} + +/* + * hdlc data is received from card and sent to all members. + */ +void +dsp_cmx_hdlc(struct dsp *dsp, struct sk_buff *skb) +{ + struct sk_buff *nskb = NULL; + struct dsp_conf_member *member; + struct mISDNhead *hh; + + /* not if not active */ + if (!dsp->b_active) + return; + + /* check if we have sompen */ + if (skb->len < 1) + return; + + /* no conf */ + if (!dsp->conf) { + /* in case of hardware (echo) */ + if (dsp->pcm_slot_tx >= 0) + return; + if (dsp->echo) + nskb = skb_clone(skb, GFP_ATOMIC); + if (nskb) { + hh = mISDN_HEAD_P(nskb); + hh->prim = PH_DATA_REQ; + hh->id = 0; + skb_queue_tail(&dsp->sendq, nskb); + schedule_work(&dsp->workq); + } + return; + } + /* in case of hardware conference */ + if (dsp->conf->hardware) + return; + list_for_each_entry(member, &dsp->conf->mlist, list) { + if (dsp->echo || member->dsp != dsp) { + nskb = skb_clone(skb, GFP_ATOMIC); + if (nskb) { + hh = mISDN_HEAD_P(nskb); + hh->prim = PH_DATA_REQ; + hh->id = 0; + skb_queue_tail(&member->dsp->sendq, nskb); + schedule_work(&member->dsp->workq); + } + } + } +} + + diff --git a/drivers/isdn/mISDN/dsp_core.c b/drivers/isdn/mISDN/dsp_core.c new file mode 100644 index 000000000000..2f10ed82c0db --- /dev/null +++ b/drivers/isdn/mISDN/dsp_core.c @@ -0,0 +1,1191 @@ +/* + * Author Andreas Eversberg (jolly@eversberg.eu) + * Based on source code structure by + * Karsten Keil (keil@isdn4linux.de) + * + * This file is (c) under GNU PUBLIC LICENSE + * For changes and modifications please read + * ../../../Documentation/isdn/mISDN.cert + * + * Thanks to Karsten Keil (great drivers) + * Cologne Chip (great chips) + * + * This module does: + * Real-time tone generation + * DTMF detection + * Real-time cross-connection and conferrence + * Compensate jitter due to system load and hardware fault. + * All features are done in kernel space and will be realized + * using hardware, if available and supported by chip set. + * Blowfish encryption/decryption + */ + +/* STRUCTURE: + * + * The dsp module provides layer 2 for b-channels (64kbit). It provides + * transparent audio forwarding with special digital signal processing: + * + * - (1) generation of tones + * - (2) detection of dtmf tones + * - (3) crossconnecting and conferences (clocking) + * - (4) echo generation for delay test + * - (5) volume control + * - (6) disable receive data + * - (7) pipeline + * - (8) encryption/decryption + * + * Look: + * TX RX + * ------upper layer------ + * | ^ + * | |(6) + * v | + * +-----+-------------+-----+ + * |(3)(4) | + * | CMX | + * | | + * | +-------------+ + * | | ^ + * | | | + * |+---------+| +----+----+ + * ||(1) || |(2) | + * || || | | + * || Tones || | DTMF | + * || || | | + * || || | | + * |+----+----+| +----+----+ + * +-----+-----+ ^ + * | | + * v | + * +----+----+ +----+----+ + * |(5) | |(5) | + * | | | | + * |TX Volume| |RX Volume| + * | | | | + * | | | | + * +----+----+ +----+----+ + * | ^ + * | | + * v | + * +----+-------------+----+ + * |(7) | + * | | + * | Pipeline Processing | + * | | + * | | + * +----+-------------+----+ + * | ^ + * | | + * v | + * +----+----+ +----+----+ + * |(8) | |(8) | + * | | | | + * | Encrypt | | Decrypt | + * | | | | + * | | | | + * +----+----+ +----+----+ + * | ^ + * | | + * v | + * ------card layer------ + * TX RX + * + * Above you can see the logical data flow. If software is used to do the + * process, it is actually the real data flow. If hardware is used, data + * may not flow, but hardware commands to the card, to provide the data flow + * as shown. + * + * NOTE: The channel must be activated in order to make dsp work, even if + * no data flow to the upper layer is intended. Activation can be done + * after and before controlling the setting using PH_CONTROL requests. + * + * DTMF: Will be detected by hardware if possible. It is done before CMX + * processing. + * + * Tones: Will be generated via software if endless looped audio fifos are + * not supported by hardware. Tones will override all data from CMX. + * It is not required to join a conference to use tones at any time. + * + * CMX: Is transparent when not used. When it is used, it will do + * crossconnections and conferences via software if not possible through + * hardware. If hardware capability is available, hardware is used. + * + * Echo: Is generated by CMX and is used to check performane of hard and + * software CMX. + * + * The CMX has special functions for conferences with one, two and more + * members. It will allow different types of data flow. Receive and transmit + * data to/form upper layer may be swithed on/off individually without loosing + * features of CMX, Tones and DTMF. + * + * Echo Cancellation: Sometimes we like to cancel echo from the interface. + * Note that a VoIP call may not have echo caused by the IP phone. The echo + * is generated by the telephone line connected to it. Because the delay + * is high, it becomes an echo. RESULT: Echo Cachelation is required if + * both echo AND delay is applied to an interface. + * Remember that software CMX always generates a more or less delay. + * + * If all used features can be realized in hardware, and if transmit and/or + * receive data ist disabled, the card may not send/receive any data at all. + * Not receiving is usefull if only announcements are played. Not sending is + * usefull if an answering machine records audio. Not sending and receiving is + * usefull during most states of the call. If supported by hardware, tones + * will be played without cpu load. Small PBXs and NT-Mode applications will + * not need expensive hardware when processing calls. + * + * + * LOCKING: + * + * When data is received from upper or lower layer (card), the complete dsp + * module is locked by a global lock. This lock MUST lock irq, because it + * must lock timer events by DSP poll timer. + * When data is ready to be transmitted down, the data is queued and sent + * outside lock and timer event. + * PH_CONTROL must not change any settings, join or split conference members + * during process of data. + * + * HDLC: + * + * It works quite the same as transparent, except that HDLC data is forwarded + * to all other conference members if no hardware bridging is possible. + * Send data will be writte to sendq. Sendq will be sent if confirm is received. + * Conference cannot join, if one member is not hdlc. + * + */ + +#include <linux/delay.h> +#include <linux/mISDNif.h> +#include <linux/mISDNdsp.h> +#include <linux/module.h> +#include <linux/vmalloc.h> +#include "core.h" +#include "dsp.h" + +const char *mISDN_dsp_revision = "2.0"; + +static int debug; +static int options; +static int poll; +static int dtmfthreshold = 100; + +MODULE_AUTHOR("Andreas Eversberg"); +module_param(debug, uint, S_IRUGO | S_IWUSR); +module_param(options, uint, S_IRUGO | S_IWUSR); +module_param(poll, uint, S_IRUGO | S_IWUSR); +module_param(dtmfthreshold, uint, S_IRUGO | S_IWUSR); +MODULE_LICENSE("GPL"); + +/*int spinnest = 0;*/ + +spinlock_t dsp_lock; /* global dsp lock */ +struct list_head dsp_ilist; +struct list_head conf_ilist; +int dsp_debug; +int dsp_options; +int dsp_poll, dsp_tics; + +/* check if rx may be turned off or must be turned on */ +static void +dsp_rx_off_member(struct dsp *dsp) +{ + struct mISDN_ctrl_req cq; + int rx_off = 1; + + if (!dsp->features_rx_off) + return; + + /* not disabled */ + if (!dsp->rx_disabled) + rx_off = 0; + /* software dtmf */ + else if (dsp->dtmf.software) + rx_off = 0; + /* echo in software */ + else if (dsp->echo && dsp->pcm_slot_tx < 0) + rx_off = 0; + /* bridge in software */ + else if (dsp->conf) { + if (dsp->conf->software) + rx_off = 0; + } + + if (rx_off == dsp->rx_is_off) + return; + + if (!dsp->ch.peer) { + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: no peer, no rx_off\n", + __func__); + return; + } + cq.op = MISDN_CTRL_RX_OFF; + cq.p1 = rx_off; + if (dsp->ch.peer->ctrl(dsp->ch.peer, CONTROL_CHANNEL, &cq)) { + printk(KERN_DEBUG "%s: 2nd CONTROL_CHANNEL failed\n", + __func__); + return; + } + dsp->rx_is_off = rx_off; + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: %s set rx_off = %d\n", + __func__, dsp->name, rx_off); +} +static void +dsp_rx_off(struct dsp *dsp) +{ + struct dsp_conf_member *member; + + if (dsp_options & DSP_OPT_NOHARDWARE) + return; + + /* no conf */ + if (!dsp->conf) { + dsp_rx_off_member(dsp); + return; + } + /* check all members in conf */ + list_for_each_entry(member, &dsp->conf->mlist, list) { + dsp_rx_off_member(member->dsp); + } +} + +static int +dsp_control_req(struct dsp *dsp, struct mISDNhead *hh, struct sk_buff *skb) +{ + struct sk_buff *nskb; + int ret = 0; + int cont; + u8 *data; + int len; + + if (skb->len < sizeof(int)) + printk(KERN_ERR "%s: PH_CONTROL message too short\n", __func__); + cont = *((int *)skb->data); + len = skb->len - sizeof(int); + data = skb->data + sizeof(int); + + switch (cont) { + case DTMF_TONE_START: /* turn on DTMF */ + if (dsp->hdlc) { + ret = -EINVAL; + break; + } + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: start dtmf\n", __func__); + if (len == sizeof(int)) { + printk(KERN_NOTICE "changing DTMF Threshold " + "to %d\n", *((int *)data)); + dsp->dtmf.treshold = (*(int *)data) * 10000; + } + /* init goertzel */ + dsp_dtmf_goertzel_init(dsp); + + /* check dtmf hardware */ + dsp_dtmf_hardware(dsp); + break; + case DTMF_TONE_STOP: /* turn off DTMF */ + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: stop dtmf\n", __func__); + dsp->dtmf.hardware = 0; + dsp->dtmf.software = 0; + break; + case DSP_CONF_JOIN: /* join / update conference */ + if (len < sizeof(int)) { + ret = -EINVAL; + break; + } + if (*((u32 *)data) == 0) + goto conf_split; + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: join conference %d\n", + __func__, *((u32 *)data)); + ret = dsp_cmx_conf(dsp, *((u32 *)data)); + /* dsp_cmx_hardware will also be called here */ + dsp_rx_off(dsp); + if (dsp_debug & DEBUG_DSP_CMX) + dsp_cmx_debug(dsp); + break; + case DSP_CONF_SPLIT: /* remove from conference */ +conf_split: + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: release conference\n", __func__); + ret = dsp_cmx_conf(dsp, 0); + /* dsp_cmx_hardware will also be called here */ + if (dsp_debug & DEBUG_DSP_CMX) + dsp_cmx_debug(dsp); + dsp_rx_off(dsp); + break; + case DSP_TONE_PATT_ON: /* play tone */ + if (dsp->hdlc) { + ret = -EINVAL; + break; + } + if (len < sizeof(int)) { + ret = -EINVAL; + break; + } + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: turn tone 0x%x on\n", + __func__, *((int *)skb->data)); + ret = dsp_tone(dsp, *((int *)data)); + if (!ret) { + dsp_cmx_hardware(dsp->conf, dsp); + dsp_rx_off(dsp); + } + if (!dsp->tone.tone) + goto tone_off; + break; + case DSP_TONE_PATT_OFF: /* stop tone */ + if (dsp->hdlc) { + ret = -EINVAL; + break; + } + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: turn tone off\n", __func__); + dsp_tone(dsp, 0); + dsp_cmx_hardware(dsp->conf, dsp); + dsp_rx_off(dsp); + /* reset tx buffers (user space data) */ +tone_off: + dsp->rx_W = 0; + dsp->rx_R = 0; + break; + case DSP_VOL_CHANGE_TX: /* change volume */ + if (dsp->hdlc) { + ret = -EINVAL; + break; + } + if (len < sizeof(int)) { + ret = -EINVAL; + break; + } + dsp->tx_volume = *((int *)data); + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: change tx vol to %d\n", + __func__, dsp->tx_volume); + dsp_cmx_hardware(dsp->conf, dsp); + dsp_dtmf_hardware(dsp); + dsp_rx_off(dsp); + break; + case DSP_VOL_CHANGE_RX: /* change volume */ + if (dsp->hdlc) { + ret = -EINVAL; + break; + } + if (len < sizeof(int)) { + ret = -EINVAL; + break; + } + dsp->rx_volume = *((int *)data); + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: change rx vol to %d\n", + __func__, dsp->tx_volume); + dsp_cmx_hardware(dsp->conf, dsp); + dsp_dtmf_hardware(dsp); + dsp_rx_off(dsp); + break; + case DSP_ECHO_ON: /* enable echo */ + dsp->echo = 1; /* soft echo */ + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: enable cmx-echo\n", __func__); + dsp_cmx_hardware(dsp->conf, dsp); + dsp_rx_off(dsp); + if (dsp_debug & DEBUG_DSP_CMX) + dsp_cmx_debug(dsp); + break; + case DSP_ECHO_OFF: /* disable echo */ + dsp->echo = 0; + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: disable cmx-echo\n", __func__); + dsp_cmx_hardware(dsp->conf, dsp); + dsp_rx_off(dsp); + if (dsp_debug & DEBUG_DSP_CMX) + dsp_cmx_debug(dsp); + break; + case DSP_RECEIVE_ON: /* enable receive to user space */ + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: enable receive to user " + "space\n", __func__); + dsp->rx_disabled = 0; + dsp_rx_off(dsp); + break; + case DSP_RECEIVE_OFF: /* disable receive to user space */ + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: disable receive to " + "user space\n", __func__); + dsp->rx_disabled = 1; + dsp_rx_off(dsp); + break; + case DSP_MIX_ON: /* enable mixing of tx data */ + if (dsp->hdlc) { + ret = -EINVAL; + break; + } + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: enable mixing of " + "tx-data with conf mebers\n", __func__); + dsp->tx_mix = 1; + dsp_cmx_hardware(dsp->conf, dsp); + dsp_rx_off(dsp); + if (dsp_debug & DEBUG_DSP_CMX) + dsp_cmx_debug(dsp); + break; + case DSP_MIX_OFF: /* disable mixing of tx data */ + if (dsp->hdlc) { + ret = -EINVAL; + break; + } + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: disable mixing of " + "tx-data with conf mebers\n", __func__); + dsp->tx_mix = 0; + dsp_cmx_hardware(dsp->conf, dsp); + dsp_rx_off(dsp); + if (dsp_debug & DEBUG_DSP_CMX) + dsp_cmx_debug(dsp); + break; + case DSP_TXDATA_ON: /* enable txdata */ + dsp->tx_data = 1; + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: enable tx-data\n", __func__); + dsp_cmx_hardware(dsp->conf, dsp); + dsp_rx_off(dsp); + if (dsp_debug & DEBUG_DSP_CMX) + dsp_cmx_debug(dsp); + break; + case DSP_TXDATA_OFF: /* disable txdata */ + dsp->tx_data = 0; + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: disable tx-data\n", __func__); + dsp_cmx_hardware(dsp->conf, dsp); + dsp_rx_off(dsp); + if (dsp_debug & DEBUG_DSP_CMX) + dsp_cmx_debug(dsp); + break; + case DSP_DELAY: /* use delay algorithm instead of dynamic + jitter algorithm */ + if (dsp->hdlc) { + ret = -EINVAL; + break; + } + if (len < sizeof(int)) { + ret = -EINVAL; + break; + } + dsp->cmx_delay = (*((int *)data)) << 3; + /* miliseconds to samples */ + if (dsp->cmx_delay >= (CMX_BUFF_HALF>>1)) + /* clip to half of maximum usable buffer + (half of half buffer) */ + dsp->cmx_delay = (CMX_BUFF_HALF>>1) - 1; + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: use delay algorithm to " + "compensate jitter (%d samples)\n", + __func__, dsp->cmx_delay); + break; + case DSP_JITTER: /* use dynamic jitter algorithm instead of + delay algorithm */ + if (dsp->hdlc) { + ret = -EINVAL; + break; + } + dsp->cmx_delay = 0; + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: use jitter algorithm to " + "compensate jitter\n", __func__); + break; + case DSP_TX_DEJITTER: /* use dynamic jitter algorithm for tx-buffer */ + if (dsp->hdlc) { + ret = -EINVAL; + break; + } + dsp->tx_dejitter = 1; + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: use dejitter on TX " + "buffer\n", __func__); + break; + case DSP_TX_DEJ_OFF: /* use tx-buffer without dejittering*/ + if (dsp->hdlc) { + ret = -EINVAL; + break; + } + dsp->tx_dejitter = 0; + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: use TX buffer without " + "dejittering\n", __func__); + break; + case DSP_PIPELINE_CFG: + if (dsp->hdlc) { + ret = -EINVAL; + break; + } + if (len > 0 && ((char *)data)[len - 1]) { + printk(KERN_DEBUG "%s: pipeline config string " + "is not NULL terminated!\n", __func__); + ret = -EINVAL; + } else { + dsp->pipeline.inuse = 1; + dsp_cmx_hardware(dsp->conf, dsp); + ret = dsp_pipeline_build(&dsp->pipeline, + len > 0 ? (char *)data : NULL); + dsp_cmx_hardware(dsp->conf, dsp); + dsp_rx_off(dsp); + } + break; + case DSP_BF_ENABLE_KEY: /* turn blowfish on */ + if (dsp->hdlc) { + ret = -EINVAL; + break; + } + if (len < 4 || len > 56) { + ret = -EINVAL; + break; + } + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: turn blowfish on (key " + "not shown)\n", __func__); + ret = dsp_bf_init(dsp, (u8 *)data, len); + /* set new cont */ + if (!ret) + cont = DSP_BF_ACCEPT; + else + cont = DSP_BF_REJECT; + /* send indication if it worked to set it */ + nskb = _alloc_mISDN_skb(PH_CONTROL_IND, MISDN_ID_ANY, + sizeof(int), &cont, GFP_ATOMIC); + if (nskb) { + if (dsp->up) { + if (dsp->up->send(dsp->up, nskb)) + dev_kfree_skb(nskb); + } else + dev_kfree_skb(nskb); + } + if (!ret) { + dsp_cmx_hardware(dsp->conf, dsp); + dsp_dtmf_hardware(dsp); + dsp_rx_off(dsp); + } + break; + case DSP_BF_DISABLE: /* turn blowfish off */ + if (dsp->hdlc) { + ret = -EINVAL; + break; + } + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: turn blowfish off\n", __func__); + dsp_bf_cleanup(dsp); + dsp_cmx_hardware(dsp->conf, dsp); + dsp_dtmf_hardware(dsp); + dsp_rx_off(dsp); + break; + default: + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: ctrl req %x unhandled\n", + __func__, cont); + ret = -EINVAL; + } + return ret; +} + +static void +get_features(struct mISDNchannel *ch) +{ + struct dsp *dsp = container_of(ch, struct dsp, ch); + struct mISDN_ctrl_req cq; + + if (dsp_options & DSP_OPT_NOHARDWARE) + return; + if (!ch->peer) { + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: no peer, no features\n", + __func__); + return; + } + memset(&cq, 0, sizeof(cq)); + cq.op = MISDN_CTRL_GETOP; + if (ch->peer->ctrl(ch->peer, CONTROL_CHANNEL, &cq) < 0) { + printk(KERN_DEBUG "%s: CONTROL_CHANNEL failed\n", + __func__); + return; + } + if (cq.op & MISDN_CTRL_RX_OFF) + dsp->features_rx_off = 1; + if ((cq.op & MISDN_CTRL_HW_FEATURES_OP)) { + cq.op = MISDN_CTRL_HW_FEATURES; + *((u_long *)&cq.p1) = (u_long)&dsp->features; + if (ch->peer->ctrl(ch->peer, CONTROL_CHANNEL, &cq)) { + printk(KERN_DEBUG "%s: 2nd CONTROL_CHANNEL failed\n", + __func__); + } + } else + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: features not supported for %s\n", + __func__, dsp->name); +} + +static int +dsp_function(struct mISDNchannel *ch, struct sk_buff *skb) +{ + struct dsp *dsp = container_of(ch, struct dsp, ch); + struct mISDNhead *hh; + int ret = 0; + u8 *digits; + int cont; + struct sk_buff *nskb; + u_long flags; + + hh = mISDN_HEAD_P(skb); + switch (hh->prim) { + /* FROM DOWN */ + case (PH_DATA_CNF): + dsp->data_pending = 0; + /* trigger next hdlc frame, if any */ + if (dsp->hdlc) { + spin_lock_irqsave(&dsp_lock, flags); + if (dsp->b_active) + schedule_work(&dsp->workq); + spin_unlock_irqrestore(&dsp_lock, flags); + } + break; + case (PH_DATA_IND): + case (DL_DATA_IND): + if (skb->len < 1) { + ret = -EINVAL; + break; + } + if (dsp->rx_is_off) { + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: rx-data during rx_off" + " for %s\n", + __func__, dsp->name); + } + if (dsp->hdlc) { + /* hdlc */ + spin_lock_irqsave(&dsp_lock, flags); + dsp_cmx_hdlc(dsp, skb); + spin_unlock_irqrestore(&dsp_lock, flags); + if (dsp->rx_disabled) { + /* if receive is not allowed */ + break; + } + hh->prim = DL_DATA_IND; + if (dsp->up) + return dsp->up->send(dsp->up, skb); + break; + } + + /* decrypt if enabled */ + if (dsp->bf_enable) + dsp_bf_decrypt(dsp, skb->data, skb->len); + /* pipeline */ + if (dsp->pipeline.inuse) + dsp_pipeline_process_rx(&dsp->pipeline, skb->data, + skb->len); + /* change volume if requested */ + if (dsp->rx_volume) + dsp_change_volume(skb, dsp->rx_volume); + + /* check if dtmf soft decoding is turned on */ + if (dsp->dtmf.software) { + digits = dsp_dtmf_goertzel_decode(dsp, skb->data, + skb->len, (dsp_options&DSP_OPT_ULAW)?1:0); + while (*digits) { + if (dsp_debug & DEBUG_DSP_DTMF) + printk(KERN_DEBUG "%s: digit" + "(%c) to layer %s\n", + __func__, *digits, dsp->name); + cont = DTMF_TONE_VAL | *digits; + nskb = _alloc_mISDN_skb(PH_CONTROL_IND, + MISDN_ID_ANY, sizeof(int), &cont, + GFP_ATOMIC); + if (nskb) { + if (dsp->up) { + if (dsp->up->send( + dsp->up, nskb)) + dev_kfree_skb(nskb); + } else + dev_kfree_skb(nskb); + } + digits++; + } + } + /* we need to process receive data if software */ + spin_lock_irqsave(&dsp_lock, flags); + if (dsp->pcm_slot_tx < 0 && dsp->pcm_slot_rx < 0) { + /* process data from card at cmx */ + dsp_cmx_receive(dsp, skb); + } + spin_unlock_irqrestore(&dsp_lock, flags); + + if (dsp->rx_disabled) { + /* if receive is not allowed */ + break; + } + hh->prim = DL_DATA_IND; + if (dsp->up) + return dsp->up->send(dsp->up, skb); + break; + case (PH_CONTROL_IND): + if (dsp_debug & DEBUG_DSP_DTMFCOEFF) + printk(KERN_DEBUG "%s: PH_CONTROL INDICATION " + "received: %x (len %d) %s\n", __func__, + hh->id, skb->len, dsp->name); + switch (hh->id) { + case (DTMF_HFC_COEF): /* getting coefficients */ + if (!dsp->dtmf.hardware) { + if (dsp_debug & DEBUG_DSP_DTMFCOEFF) + printk(KERN_DEBUG "%s: ignoring DTMF " + "coefficients from HFC\n", + __func__); + break; + } + digits = dsp_dtmf_goertzel_decode(dsp, skb->data, + skb->len, 2); + while (*digits) { + int k; + struct sk_buff *nskb; + if (dsp_debug & DEBUG_DSP_DTMF) + printk(KERN_DEBUG "%s: digit" + "(%c) to layer %s\n", + __func__, *digits, dsp->name); + k = *digits | DTMF_TONE_VAL; + nskb = _alloc_mISDN_skb(PH_CONTROL_IND, + MISDN_ID_ANY, sizeof(int), &k, + GFP_ATOMIC); + if (nskb) { + if (dsp->up) { + if (dsp->up->send( + dsp->up, nskb)) + dev_kfree_skb(nskb); + } else + dev_kfree_skb(nskb); + } + digits++; + } + break; + case (HFC_VOL_CHANGE_TX): /* change volume */ + if (skb->len != sizeof(int)) { + ret = -EINVAL; + break; + } + spin_lock_irqsave(&dsp_lock, flags); + dsp->tx_volume = *((int *)skb->data); + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: change tx volume to " + "%d\n", __func__, dsp->tx_volume); + dsp_cmx_hardware(dsp->conf, dsp); + dsp_dtmf_hardware(dsp); + dsp_rx_off(dsp); + spin_unlock_irqrestore(&dsp_lock, flags); + break; + default: + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: ctrl ind %x unhandled " + "%s\n", __func__, hh->id, dsp->name); + ret = -EINVAL; + } + break; + case (PH_ACTIVATE_IND): + case (PH_ACTIVATE_CNF): + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: b_channel is now active %s\n", + __func__, dsp->name); + /* bchannel now active */ + spin_lock_irqsave(&dsp_lock, flags); + dsp->b_active = 1; + dsp->data_pending = 0; + dsp->rx_init = 1; + /* rx_W and rx_R will be adjusted on first frame */ + dsp->rx_W = 0; + dsp->rx_R = 0; + memset(dsp->rx_buff, 0, sizeof(dsp->rx_buff)); + dsp_cmx_hardware(dsp->conf, dsp); + dsp_dtmf_hardware(dsp); + dsp_rx_off(dsp); + spin_unlock_irqrestore(&dsp_lock, flags); + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: done with activation, sending " + "confirm to user space. %s\n", __func__, + dsp->name); + /* send activation to upper layer */ + hh->prim = DL_ESTABLISH_CNF; + if (dsp->up) + return dsp->up->send(dsp->up, skb); + break; + case (PH_DEACTIVATE_IND): + case (PH_DEACTIVATE_CNF): + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: b_channel is now inactive %s\n", + __func__, dsp->name); + /* bchannel now inactive */ + spin_lock_irqsave(&dsp_lock, flags); + dsp->b_active = 0; + dsp->data_pending = 0; + dsp_cmx_hardware(dsp->conf, dsp); + dsp_rx_off(dsp); + spin_unlock_irqrestore(&dsp_lock, flags); + hh->prim = DL_RELEASE_CNF; + if (dsp->up) + return dsp->up->send(dsp->up, skb); + break; + /* FROM UP */ + case (DL_DATA_REQ): + case (PH_DATA_REQ): + if (skb->len < 1) { + ret = -EINVAL; + break; + } + if (dsp->hdlc) { + /* hdlc */ + spin_lock_irqsave(&dsp_lock, flags); + if (dsp->b_active) { + skb_queue_tail(&dsp->sendq, skb); + schedule_work(&dsp->workq); + } + spin_unlock_irqrestore(&dsp_lock, flags); + return 0; + } + /* send data to tx-buffer (if no tone is played) */ + if (!dsp->tone.tone) { + spin_lock_irqsave(&dsp_lock, flags); + dsp_cmx_transmit(dsp, skb); + spin_unlock_irqrestore(&dsp_lock, flags); + } + break; + case (PH_CONTROL_REQ): + spin_lock_irqsave(&dsp_lock, flags); + ret = dsp_control_req(dsp, hh, skb); + spin_unlock_irqrestore(&dsp_lock, flags); + break; + case (DL_ESTABLISH_REQ): + case (PH_ACTIVATE_REQ): + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: activating b_channel %s\n", + __func__, dsp->name); + if (dsp->dtmf.hardware || dsp->dtmf.software) + dsp_dtmf_goertzel_init(dsp); + get_features(ch); + /* send ph_activate */ + hh->prim = PH_ACTIVATE_REQ; + if (ch->peer) + return ch->recv(ch->peer, skb); + break; + case (DL_RELEASE_REQ): + case (PH_DEACTIVATE_REQ): + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: releasing b_channel %s\n", + __func__, dsp->name); + spin_lock_irqsave(&dsp_lock, flags); + dsp->tone.tone = 0; + dsp->tone.hardware = 0; + dsp->tone.software = 0; + if (timer_pending(&dsp->tone.tl)) + del_timer(&dsp->tone.tl); + if (dsp->conf) + dsp_cmx_conf(dsp, 0); /* dsp_cmx_hardware will also be + called here */ + skb_queue_purge(&dsp->sendq); + spin_unlock_irqrestore(&dsp_lock, flags); + hh->prim = PH_DEACTIVATE_REQ; + if (ch->peer) + return ch->recv(ch->peer, skb); + break; + default: + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: msg %x unhandled %s\n", + __func__, hh->prim, dsp->name); + ret = -EINVAL; + } + if (!ret) + dev_kfree_skb(skb); + return ret; +} + +static int +dsp_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg) +{ + struct dsp *dsp = container_of(ch, struct dsp, ch); + u_long flags; + int err = 0; + + if (debug & DEBUG_DSP_CTRL) + printk(KERN_DEBUG "%s:(%x)\n", __func__, cmd); + + switch (cmd) { + case OPEN_CHANNEL: + break; + case CLOSE_CHANNEL: + if (dsp->ch.peer) + dsp->ch.peer->ctrl(dsp->ch.peer, CLOSE_CHANNEL, NULL); + + /* wait until workqueue has finished, + * must lock here, or we may hit send-process currently + * queueing. */ + spin_lock_irqsave(&dsp_lock, flags); + dsp->b_active = 0; + spin_unlock_irqrestore(&dsp_lock, flags); + /* MUST not be locked, because it waits until queue is done. */ + cancel_work_sync(&dsp->workq); + spin_lock_irqsave(&dsp_lock, flags); + if (timer_pending(&dsp->tone.tl)) + del_timer(&dsp->tone.tl); + skb_queue_purge(&dsp->sendq); + if (dsp_debug & DEBUG_DSP_CTRL) + printk(KERN_DEBUG "%s: releasing member %s\n", + __func__, dsp->name); + dsp->b_active = 0; + dsp_cmx_conf(dsp, 0); /* dsp_cmx_hardware will also be called + here */ + dsp_pipeline_destroy(&dsp->pipeline); + + if (dsp_debug & DEBUG_DSP_CTRL) + printk(KERN_DEBUG "%s: remove & destroy object %s\n", + __func__, dsp->name); + list_del(&dsp->list); + spin_unlock_irqrestore(&dsp_lock, flags); + + if (dsp_debug & DEBUG_DSP_CTRL) + printk(KERN_DEBUG "%s: dsp instance released\n", + __func__); + vfree(dsp); + module_put(THIS_MODULE); + break; + } + return err; +} + +static void +dsp_send_bh(struct work_struct *work) +{ + struct dsp *dsp = container_of(work, struct dsp, workq); + struct sk_buff *skb; + struct mISDNhead *hh; + + if (dsp->hdlc && dsp->data_pending) + return; /* wait until data has been acknowledged */ + + /* send queued data */ + while ((skb = skb_dequeue(&dsp->sendq))) { + /* in locked date, we must have still data in queue */ + if (dsp->data_pending) { + if (dsp_debug & DEBUG_DSP_CORE) + printk(KERN_DEBUG "%s: fifo full %s, this is " + "no bug!\n", __func__, dsp->name); + /* flush transparent data, if not acked */ + dev_kfree_skb(skb); + continue; + } + hh = mISDN_HEAD_P(skb); + if (hh->prim == DL_DATA_REQ) { + /* send packet up */ + if (dsp->up) { + if (dsp->up->send(dsp->up, skb)) + dev_kfree_skb(skb); + } else + dev_kfree_skb(skb); + } else { + /* send packet down */ + if (dsp->ch.peer) { + dsp->data_pending = 1; + if (dsp->ch.recv(dsp->ch.peer, skb)) { + dev_kfree_skb(skb); + dsp->data_pending = 0; + } + } else + dev_kfree_skb(skb); + } + } +} + +static int +dspcreate(struct channel_req *crq) +{ + struct dsp *ndsp; + u_long flags; + + if (crq->protocol != ISDN_P_B_L2DSP + && crq->protocol != ISDN_P_B_L2DSPHDLC) + return -EPROTONOSUPPORT; + ndsp = vmalloc(sizeof(struct dsp)); + if (!ndsp) { + printk(KERN_ERR "%s: vmalloc struct dsp failed\n", __func__); + return -ENOMEM; + } + memset(ndsp, 0, sizeof(struct dsp)); + if (dsp_debug & DEBUG_DSP_CTRL) + printk(KERN_DEBUG "%s: creating new dsp instance\n", __func__); + + /* default enabled */ + INIT_WORK(&ndsp->workq, (void *)dsp_send_bh); + skb_queue_head_init(&ndsp->sendq); + ndsp->ch.send = dsp_function; + ndsp->ch.ctrl = dsp_ctrl; + ndsp->up = crq->ch; + crq->ch = &ndsp->ch; + if (crq->protocol == ISDN_P_B_L2DSP) { + crq->protocol = ISDN_P_B_RAW; + ndsp->hdlc = 0; + } else { + crq->protocol = ISDN_P_B_HDLC; + ndsp->hdlc = 1; + } + if (!try_module_get(THIS_MODULE)) + printk(KERN_WARNING "%s:cannot get module\n", + __func__); + + sprintf(ndsp->name, "DSP_C%x(0x%p)", + ndsp->up->st->dev->id + 1, ndsp); + /* set frame size to start */ + ndsp->features.hfc_id = -1; /* current PCM id */ + ndsp->features.pcm_id = -1; /* current PCM id */ + ndsp->pcm_slot_rx = -1; /* current CPM slot */ + ndsp->pcm_slot_tx = -1; + ndsp->pcm_bank_rx = -1; + ndsp->pcm_bank_tx = -1; + ndsp->hfc_conf = -1; /* current conference number */ + /* set tone timer */ + ndsp->tone.tl.function = (void *)dsp_tone_timeout; + ndsp->tone.tl.data = (long) ndsp; + init_timer(&ndsp->tone.tl); + + if (dtmfthreshold < 20 || dtmfthreshold > 500) + dtmfthreshold = 200; + ndsp->dtmf.treshold = dtmfthreshold*10000; + + /* init pipeline append to list */ + spin_lock_irqsave(&dsp_lock, flags); + dsp_pipeline_init(&ndsp->pipeline); + list_add_tail(&ndsp->list, &dsp_ilist); + spin_unlock_irqrestore(&dsp_lock, flags); + + return 0; +} + + +static struct Bprotocol DSP = { + .Bprotocols = (1 << (ISDN_P_B_L2DSP & ISDN_P_B_MASK)) + | (1 << (ISDN_P_B_L2DSPHDLC & ISDN_P_B_MASK)), + .name = "dsp", + .create = dspcreate +}; + +static int dsp_init(void) +{ + int err; + int tics; + + printk(KERN_INFO "DSP modul %s\n", mISDN_dsp_revision); + + dsp_options = options; + dsp_debug = debug; + + /* set packet size */ + dsp_poll = poll; + if (dsp_poll) { + if (dsp_poll > MAX_POLL) { + printk(KERN_ERR "%s: Wrong poll value (%d), use %d " + "maximum.\n", __func__, poll, MAX_POLL); + err = -EINVAL; + return err; + } + if (dsp_poll < 8) { + printk(KERN_ERR "%s: Wrong poll value (%d), use 8 " + "minimum.\n", __func__, dsp_poll); + err = -EINVAL; + return err; + } + dsp_tics = poll * HZ / 8000; + if (dsp_tics * 8000 != poll * HZ) { + printk(KERN_INFO "mISDN_dsp: Cannot clock every %d " + "samples (0,125 ms). It is not a multiple of " + "%d HZ.\n", poll, HZ); + err = -EINVAL; + return err; + } + } else { + poll = 8; + while (poll <= MAX_POLL) { + tics = poll * HZ / 8000; + if (tics * 8000 == poll * HZ) { + dsp_tics = tics; + dsp_poll = poll; + if (poll >= 64) + break; + } + poll++; + } + } + if (dsp_poll == 0) { + printk(KERN_INFO "mISDN_dsp: There is no multiple of kernel " + "clock that equals exactly the duration of 8-256 " + "samples. (Choose kernel clock speed like 100, 250, " + "300, 1000)\n"); + err = -EINVAL; + return err; + } + printk(KERN_INFO "mISDN_dsp: DSP clocks every %d samples. This equals " + "%d jiffies.\n", dsp_poll, dsp_tics); + + spin_lock_init(&dsp_lock); + INIT_LIST_HEAD(&dsp_ilist); + INIT_LIST_HEAD(&conf_ilist); + + /* init conversion tables */ + dsp_audio_generate_law_tables(); + dsp_silence = (dsp_options&DSP_OPT_ULAW)?0xff:0x2a; + dsp_audio_law_to_s32 = (dsp_options&DSP_OPT_ULAW)?dsp_audio_ulaw_to_s32: + dsp_audio_alaw_to_s32; + dsp_audio_generate_s2law_table(); + dsp_audio_generate_seven(); + dsp_audio_generate_mix_table(); + if (dsp_options & DSP_OPT_ULAW) + dsp_audio_generate_ulaw_samples(); + dsp_audio_generate_volume_changes(); + + err = dsp_pipeline_module_init(); + if (err) { + printk(KERN_ERR "mISDN_dsp: Can't initialize pipeline, " + "error(%d)\n", err); + return err; + } + + err = mISDN_register_Bprotocol(&DSP); + if (err) { + printk(KERN_ERR "Can't register %s error(%d)\n", DSP.name, err); + return err; + } + + /* set sample timer */ + dsp_spl_tl.function = (void *)dsp_cmx_send; + dsp_spl_tl.data = 0; + init_timer(&dsp_spl_tl); + dsp_spl_tl.expires = jiffies + dsp_tics; + dsp_spl_jiffies = dsp_spl_tl.expires; + add_timer(&dsp_spl_tl); + + return 0; +} + + +static void dsp_cleanup(void) +{ + mISDN_unregister_Bprotocol(&DSP); + + if (timer_pending(&dsp_spl_tl)) + del_timer(&dsp_spl_tl); + + if (!list_empty(&dsp_ilist)) { + printk(KERN_ERR "mISDN_dsp: Audio DSP object inst list not " + "empty.\n"); + } + if (!list_empty(&conf_ilist)) { + printk(KERN_ERR "mISDN_dsp: Conference list not empty. Not " + "all memory freed.\n"); + } + + dsp_pipeline_module_exit(); +} + +module_init(dsp_init); +module_exit(dsp_cleanup); + diff --git a/drivers/isdn/mISDN/dsp_dtmf.c b/drivers/isdn/mISDN/dsp_dtmf.c new file mode 100644 index 000000000000..efc371c1f0dc --- /dev/null +++ b/drivers/isdn/mISDN/dsp_dtmf.c @@ -0,0 +1,303 @@ +/* + * DTMF decoder. + * + * Copyright by Andreas Eversberg (jolly@eversberg.eu) + * based on different decoders such as ISDN4Linux + * + * This software may be used and distributed according to the terms + * of the GNU General Public License, incorporated herein by reference. + * + */ + +#include <linux/mISDNif.h> +#include <linux/mISDNdsp.h> +#include "core.h" +#include "dsp.h" + +#define NCOEFF 8 /* number of frequencies to be analyzed */ + +/* For DTMF recognition: + * 2 * cos(2 * PI * k / N) precalculated for all k + */ +static u64 cos2pik[NCOEFF] = +{ + /* k << 15 (source: hfc-4s/8s documentation (www.colognechip.de)) */ + 55960, 53912, 51402, 48438, 38146, 32650, 26170, 18630 +}; + +/* digit matrix */ +static char dtmf_matrix[4][4] = +{ + {'1', '2', '3', 'A'}, + {'4', '5', '6', 'B'}, + {'7', '8', '9', 'C'}, + {'*', '0', '#', 'D'} +}; + +/* dtmf detection using goertzel algorithm + * init function + */ +void dsp_dtmf_goertzel_init(struct dsp *dsp) +{ + dsp->dtmf.size = 0; + dsp->dtmf.lastwhat = '\0'; + dsp->dtmf.lastdigit = '\0'; + dsp->dtmf.count = 0; +} + +/* check for hardware or software features + */ +void dsp_dtmf_hardware(struct dsp *dsp) +{ + int hardware = 1; + + if (!dsp->features.hfc_dtmf) + hardware = 0; + + /* check for volume change */ + if (dsp->tx_volume) { + if (dsp_debug & DEBUG_DSP_DTMF) + printk(KERN_DEBUG "%s dsp %s cannot do hardware DTMF, " + "because tx_volume is changed\n", + __func__, dsp->name); + hardware = 0; + } + if (dsp->rx_volume) { + if (dsp_debug & DEBUG_DSP_DTMF) + printk(KERN_DEBUG "%s dsp %s cannot do hardware DTMF, " + "because rx_volume is changed\n", + __func__, dsp->name); + hardware = 0; + } + /* check if encryption is enabled */ + if (dsp->bf_enable) { + if (dsp_debug & DEBUG_DSP_DTMF) + printk(KERN_DEBUG "%s dsp %s cannot do hardware DTMF, " + "because encryption is enabled\n", + __func__, dsp->name); + hardware = 0; + } + /* check if pipeline exists */ + if (dsp->pipeline.inuse) { + if (dsp_debug & DEBUG_DSP_DTMF) + printk(KERN_DEBUG "%s dsp %s cannot do hardware DTMF, " + "because pipeline exists.\n", + __func__, dsp->name); + hardware = 0; + } + + dsp->dtmf.hardware = hardware; + dsp->dtmf.software = !hardware; +} + + +/************************************************************* + * calculate the coefficients of the given sample and decode * + *************************************************************/ + +/* the given sample is decoded. if the sample is not long enough for a + * complete frame, the decoding is finished and continued with the next + * call of this function. + * + * the algorithm is very good for detection with a minimum of errors. i + * tested it allot. it even works with very short tones (40ms). the only + * disadvantage is, that it doesn't work good with different volumes of both + * tones. this will happen, if accoustically coupled dialers are used. + * it sometimes detects tones during speach, which is normal for decoders. + * use sequences to given commands during calls. + * + * dtmf - points to a structure of the current dtmf state + * spl and len - the sample + * fmt - 0 = alaw, 1 = ulaw, 2 = coefficients from HFC DTMF hw-decoder + */ + +u8 +*dsp_dtmf_goertzel_decode(struct dsp *dsp, u8 *data, int len, int fmt) +{ + u8 what; + int size; + signed short *buf; + s32 sk, sk1, sk2; + int k, n, i; + s32 *hfccoeff; + s32 result[NCOEFF], tresh, treshl; + int lowgroup, highgroup; + s64 cos2pik_; + + dsp->dtmf.digits[0] = '\0'; + + /* Note: The function will loop until the buffer has not enough samples + * left to decode a full frame. + */ +again: + /* convert samples */ + size = dsp->dtmf.size; + buf = dsp->dtmf.buffer; + switch (fmt) { + case 0: /* alaw */ + case 1: /* ulaw */ + while (size < DSP_DTMF_NPOINTS && len) { + buf[size++] = dsp_audio_law_to_s32[*data++]; + len--; + } + break; + + case 2: /* HFC coefficients */ + default: + if (len < 64) { + if (len > 0) + printk(KERN_ERR "%s: coefficients have invalid " + "size. (is=%d < must=%d)\n", + __func__, len, 64); + return dsp->dtmf.digits; + } + hfccoeff = (s32 *)data; + for (k = 0; k < NCOEFF; k++) { + sk2 = (*hfccoeff++)>>4; + sk = (*hfccoeff++)>>4; + if (sk > 32767 || sk < -32767 || sk2 > 32767 + || sk2 < -32767) + printk(KERN_WARNING + "DTMF-Detection overflow\n"); + /* compute |X(k)|**2 */ + result[k] = + (sk * sk) - + (((cos2pik[k] * sk) >> 15) * sk2) + + (sk2 * sk2); + } + data += 64; + len -= 64; + goto coefficients; + break; + } + dsp->dtmf.size = size; + + if (size < DSP_DTMF_NPOINTS) + return dsp->dtmf.digits; + + dsp->dtmf.size = 0; + + /* now we have a full buffer of signed long samples - we do goertzel */ + for (k = 0; k < NCOEFF; k++) { + sk = 0; + sk1 = 0; + sk2 = 0; + buf = dsp->dtmf.buffer; + cos2pik_ = cos2pik[k]; + for (n = 0; n < DSP_DTMF_NPOINTS; n++) { + sk = ((cos2pik_*sk1)>>15) - sk2 + (*buf++); + sk2 = sk1; + sk1 = sk; + } + sk >>= 8; + sk2 >>= 8; + if (sk > 32767 || sk < -32767 || sk2 > 32767 || sk2 < -32767) + printk(KERN_WARNING "DTMF-Detection overflow\n"); + /* compute |X(k)|**2 */ + result[k] = + (sk * sk) - + (((cos2pik[k] * sk) >> 15) * sk2) + + (sk2 * sk2); + } + + /* our (squared) coefficients have been calculated, we need to process + * them. + */ +coefficients: + tresh = 0; + for (i = 0; i < NCOEFF; i++) { + if (result[i] < 0) + result[i] = 0; + if (result[i] > dsp->dtmf.treshold) { + if (result[i] > tresh) + tresh = result[i]; + } + } + + if (tresh == 0) { + what = 0; + goto storedigit; + } + + if (dsp_debug & DEBUG_DSP_DTMFCOEFF) + printk(KERN_DEBUG "a %3d %3d %3d %3d %3d %3d %3d %3d" + " tr:%3d r %3d %3d %3d %3d %3d %3d %3d %3d\n", + result[0]/10000, result[1]/10000, result[2]/10000, + result[3]/10000, result[4]/10000, result[5]/10000, + result[6]/10000, result[7]/10000, tresh/10000, + result[0]/(tresh/100), result[1]/(tresh/100), + result[2]/(tresh/100), result[3]/(tresh/100), + result[4]/(tresh/100), result[5]/(tresh/100), + result[6]/(tresh/100), result[7]/(tresh/100)); + + /* calc digit (lowgroup/highgroup) */ + lowgroup = -1; + highgroup = -1; + treshl = tresh >> 3; /* tones which are not on, must be below 9 dB */ + tresh = tresh >> 2; /* touchtones must match within 6 dB */ + for (i = 0; i < NCOEFF; i++) { + if (result[i] < treshl) + continue; /* ignore */ + if (result[i] < tresh) { + lowgroup = -1; + highgroup = -1; + break; /* noise inbetween */ + } + /* good level found. This is allowed only one time per group */ + if (i < NCOEFF/2) { + /* lowgroup */ + if (lowgroup >= 0) { + /* Bad. Another tone found. */ + lowgroup = -1; + break; + } else + lowgroup = i; + } else { + /* higroup */ + if (highgroup >= 0) { + /* Bad. Another tone found. */ + highgroup = -1; + break; + } else + highgroup = i-(NCOEFF/2); + } + } + + /* get digit or null */ + what = 0; + if (lowgroup >= 0 && highgroup >= 0) + what = dtmf_matrix[lowgroup][highgroup]; + +storedigit: + if (what && (dsp_debug & DEBUG_DSP_DTMF)) + printk(KERN_DEBUG "DTMF what: %c\n", what); + + if (dsp->dtmf.lastwhat != what) + dsp->dtmf.count = 0; + + /* the tone (or no tone) must remain 3 times without change */ + if (dsp->dtmf.count == 2) { + if (dsp->dtmf.lastdigit != what) { + dsp->dtmf.lastdigit = what; + if (what) { + if (dsp_debug & DEBUG_DSP_DTMF) + printk(KERN_DEBUG "DTMF digit: %c\n", + what); + if ((strlen(dsp->dtmf.digits)+1) + < sizeof(dsp->dtmf.digits)) { + dsp->dtmf.digits[strlen( + dsp->dtmf.digits)+1] = '\0'; + dsp->dtmf.digits[strlen( + dsp->dtmf.digits)] = what; + } + } + } + } else + dsp->dtmf.count++; + + dsp->dtmf.lastwhat = what; + + goto again; +} + + diff --git a/drivers/isdn/mISDN/dsp_ecdis.h b/drivers/isdn/mISDN/dsp_ecdis.h new file mode 100644 index 000000000000..8a20af43308b --- /dev/null +++ b/drivers/isdn/mISDN/dsp_ecdis.h @@ -0,0 +1,110 @@ +/* + * SpanDSP - a series of DSP components for telephony + * + * ec_disable_detector.h - A detector which should eventually meet the + * G.164/G.165 requirements for detecting the + * 2100Hz echo cancellor disable tone. + * + * Written by Steve Underwood <steveu@coppice.org> + * + * Copyright (C) 2001 Steve Underwood + * + * All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + * + */ + +#include "dsp_biquad.h" + +struct ec_disable_detector_state { + struct biquad2_state notch; + int notch_level; + int channel_level; + int tone_present; + int tone_cycle_duration; + int good_cycles; + int hit; +}; + + +#define FALSE 0 +#define TRUE (!FALSE) + +static inline void +echo_can_disable_detector_init(struct ec_disable_detector_state *det) +{ + /* Elliptic notch */ + /* This is actually centred at 2095Hz, but gets the balance we want, due + to the asymmetric walls of the notch */ + biquad2_init(&det->notch, + (int32_t) (-0.7600000*32768.0), + (int32_t) (-0.1183852*32768.0), + (int32_t) (-0.5104039*32768.0), + (int32_t) (0.1567596*32768.0), + (int32_t) (1.0000000*32768.0)); + + det->channel_level = 0; + det->notch_level = 0; + det->tone_present = FALSE; + det->tone_cycle_duration = 0; + det->good_cycles = 0; + det->hit = 0; +} +/*- End of function --------------------------------------------------------*/ + +static inline int +echo_can_disable_detector_update(struct ec_disable_detector_state *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) { + /* The notch says yes, so we have the tone. */ + if (!det->tone_present) { + /* Do we get a kick every 450+-25ms? */ + if (det->tone_cycle_duration >= 425*8 + && det->tone_cycle_duration <= 475*8) { + det->good_cycles++; + if (det->good_cycles > 2) + det->hit = TRUE; + } + det->tone_cycle_duration = 0; + } + det->tone_present = TRUE; + } else + det->tone_present = FALSE; + det->tone_cycle_duration++; + } else { + det->tone_present = FALSE; + det->tone_cycle_duration = 0; + det->good_cycles = 0; + } + return det->hit; +} +/*- End of function --------------------------------------------------------*/ +/*- End of file ------------------------------------------------------------*/ diff --git a/drivers/isdn/mISDN/dsp_hwec.c b/drivers/isdn/mISDN/dsp_hwec.c new file mode 100644 index 000000000000..eb892d9dd5c6 --- /dev/null +++ b/drivers/isdn/mISDN/dsp_hwec.c @@ -0,0 +1,138 @@ +/* + * dsp_hwec.c: + * builtin mISDN dsp pipeline element for enabling the hw echocanceller + * + * Copyright (C) 2007, Nadi Sarrar + * + * Nadi Sarrar <nadi@beronet.com> + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the Free + * Software Foundation; either version 2 of the License, or (at your option) + * any later version. + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + * + * You should have received a copy of the GNU General Public License along with + * this program; if not, write to the Free Software Foundation, Inc., 59 + * Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The full GNU General Public License is included in this distribution in the + * file called LICENSE. + * + */ + +#include <linux/kernel.h> +#include <linux/string.h> +#include <linux/mISDNdsp.h> +#include <linux/mISDNif.h> +#include "core.h" +#include "dsp.h" +#include "dsp_hwec.h" + +static struct mISDN_dsp_element_arg args[] = { + { "deftaps", "128", "Set the number of taps of cancellation." }, +}; + +static struct mISDN_dsp_element dsp_hwec_p = { + .name = "hwec", + .new = NULL, + .free = NULL, + .process_tx = NULL, + .process_rx = NULL, + .num_args = sizeof(args) / sizeof(struct mISDN_dsp_element_arg), + .args = args, +}; +struct mISDN_dsp_element *dsp_hwec = &dsp_hwec_p; + +void dsp_hwec_enable(struct dsp *dsp, const char *arg) +{ + int deftaps = 128, + len; + struct mISDN_ctrl_req cq; + + if (!dsp) { + printk(KERN_ERR "%s: failed to enable hwec: dsp is NULL\n", + __func__); + return; + } + + if (!arg) + goto _do; + + len = strlen(arg); + if (!len) + goto _do; + + { + char _dup[len + 1]; + char *dup, *tok, *name, *val; + int tmp; + + strcpy(_dup, arg); + dup = _dup; + + while ((tok = strsep(&dup, ","))) { + if (!strlen(tok)) + continue; + name = strsep(&tok, "="); + val = tok; + + if (!val) + continue; + + if (!strcmp(name, "deftaps")) { + if (sscanf(val, "%d", &tmp) == 1) + deftaps = tmp; + } + } + } + +_do: + printk(KERN_DEBUG "%s: enabling hwec with deftaps=%d\n", + __func__, deftaps); + memset(&cq, 0, sizeof(cq)); + cq.op = MISDN_CTRL_HFC_ECHOCAN_ON; + cq.p1 = deftaps; + if (!dsp->ch.peer->ctrl(&dsp->ch, CONTROL_CHANNEL, &cq)) { + printk(KERN_DEBUG "%s: CONTROL_CHANNEL failed\n", + __func__); + return; + } +} + +void dsp_hwec_disable(struct dsp *dsp) +{ + struct mISDN_ctrl_req cq; + + if (!dsp) { + printk(KERN_ERR "%s: failed to disable hwec: dsp is NULL\n", + __func__); + return; + } + + printk(KERN_DEBUG "%s: disabling hwec\n", __func__); + memset(&cq, 0, sizeof(cq)); + cq.op = MISDN_CTRL_HFC_ECHOCAN_OFF; + if (!dsp->ch.peer->ctrl(&dsp->ch, CONTROL_CHANNEL, &cq)) { + printk(KERN_DEBUG "%s: CONTROL_CHANNEL failed\n", + __func__); + return; + } +} + +int dsp_hwec_init(void) +{ + mISDN_dsp_element_register(dsp_hwec); + + return 0; +} + +void dsp_hwec_exit(void) +{ + mISDN_dsp_element_unregister(dsp_hwec); +} + diff --git a/drivers/isdn/mISDN/dsp_hwec.h b/drivers/isdn/mISDN/dsp_hwec.h new file mode 100644 index 000000000000..eebe80c3f713 --- /dev/null +++ b/drivers/isdn/mISDN/dsp_hwec.h @@ -0,0 +1,10 @@ +/* + * dsp_hwec.h + */ + +extern struct mISDN_dsp_element *dsp_hwec; +extern void dsp_hwec_enable(struct dsp *dsp, const char *arg); +extern void dsp_hwec_disable(struct dsp *dsp); +extern int dsp_hwec_init(void); +extern void dsp_hwec_exit(void); + diff --git a/drivers/isdn/mISDN/dsp_pipeline.c b/drivers/isdn/mISDN/dsp_pipeline.c new file mode 100644 index 000000000000..850260ab57d0 --- /dev/null +++ b/drivers/isdn/mISDN/dsp_pipeline.c @@ -0,0 +1,348 @@ +/* + * dsp_pipeline.c: pipelined audio processing + * + * Copyright (C) 2007, Nadi Sarrar + * + * Nadi Sarrar <nadi@beronet.com> + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the Free + * Software Foundation; either version 2 of the License, or (at your option) + * any later version. + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + * + * You should have received a copy of the GNU General Public License along with + * this program; if not, write to the Free Software Foundation, Inc., 59 + * Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The full GNU General Public License is included in this distribution in the + * file called LICENSE. + * + */ + +#include <linux/kernel.h> +#include <linux/list.h> +#include <linux/string.h> +#include <linux/mISDNif.h> +#include <linux/mISDNdsp.h> +#include "dsp.h" +#include "dsp_hwec.h" + +/* uncomment for debugging */ +/*#define PIPELINE_DEBUG*/ + +struct dsp_pipeline_entry { + struct mISDN_dsp_element *elem; + void *p; + struct list_head list; +}; +struct dsp_element_entry { + struct mISDN_dsp_element *elem; + struct device dev; + struct list_head list; +}; + +static LIST_HEAD(dsp_elements); + +/* sysfs */ +static struct class *elements_class; + +static ssize_t +attr_show_args(struct device *dev, struct device_attribute *attr, char *buf) +{ + struct mISDN_dsp_element *elem = dev_get_drvdata(dev); + ssize_t len = 0; + int i = 0; + + *buf = 0; + for (; i < elem->num_args; ++i) + len = sprintf(buf, "%sName: %s\n%s%s%sDescription: %s\n" + "\n", buf, + elem->args[i].name, + elem->args[i].def ? "Default: " : "", + elem->args[i].def ? elem->args[i].def : "", + elem->args[i].def ? "\n" : "", + elem->args[i].desc); + + return len; +} + +static struct device_attribute element_attributes[] = { + __ATTR(args, 0444, attr_show_args, NULL), +}; + +int mISDN_dsp_element_register(struct mISDN_dsp_element *elem) +{ + struct dsp_element_entry *entry; + int ret, i; + + if (!elem) + return -EINVAL; + + entry = kzalloc(sizeof(struct dsp_element_entry), GFP_KERNEL); + if (!entry) + return -ENOMEM; + + entry->elem = elem; + + entry->dev.class = elements_class; + dev_set_drvdata(&entry->dev, elem); + snprintf(entry->dev.bus_id, BUS_ID_SIZE, elem->name); + ret = device_register(&entry->dev); + if (ret) { + printk(KERN_ERR "%s: failed to register %s\n", + __func__, elem->name); + goto err1; + } + + for (i = 0; i < (sizeof(element_attributes) + / sizeof(struct device_attribute)); ++i) + ret = device_create_file(&entry->dev, + &element_attributes[i]); + if (ret) { + printk(KERN_ERR "%s: failed to create device file\n", + __func__); + goto err2; + } + + list_add_tail(&entry->list, &dsp_elements); + + printk(KERN_DEBUG "%s: %s registered\n", __func__, elem->name); + + return 0; + +err2: + device_unregister(&entry->dev); +err1: + kfree(entry); + return ret; +} +EXPORT_SYMBOL(mISDN_dsp_element_register); + +void mISDN_dsp_element_unregister(struct mISDN_dsp_element *elem) +{ + struct dsp_element_entry *entry, *n; + + if (!elem) + return; + + list_for_each_entry_safe(entry, n, &dsp_elements, list) + if (entry->elem == elem) { + list_del(&entry->list); + device_unregister(&entry->dev); + kfree(entry); + printk(KERN_DEBUG "%s: %s unregistered\n", + __func__, elem->name); + return; + } + printk(KERN_ERR "%s: element %s not in list.\n", __func__, elem->name); +} +EXPORT_SYMBOL(mISDN_dsp_element_unregister); + +int dsp_pipeline_module_init(void) +{ + elements_class = class_create(THIS_MODULE, "dsp_pipeline"); + if (IS_ERR(elements_class)) + return PTR_ERR(elements_class); + +#ifdef PIPELINE_DEBUG + printk(KERN_DEBUG "%s: dsp pipeline module initialized\n", __func__); +#endif + + dsp_hwec_init(); + + return 0; +} + +void dsp_pipeline_module_exit(void) +{ + struct dsp_element_entry *entry, *n; + + dsp_hwec_exit(); + + class_destroy(elements_class); + + list_for_each_entry_safe(entry, n, &dsp_elements, list) { + list_del(&entry->list); + printk(KERN_WARNING "%s: element was still registered: %s\n", + __func__, entry->elem->name); + kfree(entry); + } + + printk(KERN_DEBUG "%s: dsp pipeline module exited\n", __func__); +} + +int dsp_pipeline_init(struct dsp_pipeline *pipeline) +{ + if (!pipeline) + return -EINVAL; + + INIT_LIST_HEAD(&pipeline->list); + +#ifdef PIPELINE_DEBUG + printk(KERN_DEBUG "%s: dsp pipeline ready\n", __func__); +#endif + + return 0; +} + +static inline void _dsp_pipeline_destroy(struct dsp_pipeline *pipeline) +{ + struct dsp_pipeline_entry *entry, *n; + + list_for_each_entry_safe(entry, n, &pipeline->list, list) { + list_del(&entry->list); + if (entry->elem == dsp_hwec) + dsp_hwec_disable(container_of(pipeline, struct dsp, + pipeline)); + else + entry->elem->free(entry->p); + kfree(entry); + } +} + +void dsp_pipeline_destroy(struct dsp_pipeline *pipeline) +{ + + if (!pipeline) + return; + + _dsp_pipeline_destroy(pipeline); + +#ifdef PIPELINE_DEBUG + printk(KERN_DEBUG "%s: dsp pipeline destroyed\n", __func__); +#endif +} + +int dsp_pipeline_build(struct dsp_pipeline *pipeline, const char *cfg) +{ + int len, incomplete = 0, found = 0; + char *dup, *tok, *name, *args; + struct dsp_element_entry *entry, *n; + struct dsp_pipeline_entry *pipeline_entry; + struct mISDN_dsp_element *elem; + + if (!pipeline) + return -EINVAL; + + if (!list_empty(&pipeline->list)) + _dsp_pipeline_destroy(pipeline); + + if (!cfg) + return 0; + + len = strlen(cfg); + if (!len) + return 0; + + dup = kmalloc(len + 1, GFP_KERNEL); + if (!dup) + return 0; + strcpy(dup, cfg); + while ((tok = strsep(&dup, "|"))) { + if (!strlen(tok)) + continue; + name = strsep(&tok, "("); + args = strsep(&tok, ")"); + if (args && !*args) + args = 0; + + list_for_each_entry_safe(entry, n, &dsp_elements, list) + if (!strcmp(entry->elem->name, name)) { + elem = entry->elem; + + pipeline_entry = kmalloc(sizeof(struct + dsp_pipeline_entry), GFP_KERNEL); + if (!pipeline_entry) { + printk(KERN_DEBUG "%s: failed to add " + "entry to pipeline: %s (out of " + "memory)\n", __func__, elem->name); + incomplete = 1; + goto _out; + } + pipeline_entry->elem = elem; + + if (elem == dsp_hwec) { + /* This is a hack to make the hwec + available as a pipeline module */ + dsp_hwec_enable(container_of(pipeline, + struct dsp, pipeline), args); + list_add_tail(&pipeline_entry->list, + &pipeline->list); + } else { + pipeline_entry->p = elem->new(args); + if (pipeline_entry->p) { + list_add_tail(&pipeline_entry-> + list, &pipeline->list); +#ifdef PIPELINE_DEBUG + printk(KERN_DEBUG "%s: created " + "instance of %s%s%s\n", + __func__, name, args ? + " with args " : "", args ? + args : ""); +#endif + } else { + printk(KERN_DEBUG "%s: failed " + "to add entry to pipeline: " + "%s (new() returned NULL)\n", + __func__, elem->name); + kfree(pipeline_entry); + incomplete = 1; + } + } + found = 1; + break; + } + + if (found) + found = 0; + else { + printk(KERN_DEBUG "%s: element not found, skipping: " + "%s\n", __func__, name); + incomplete = 1; + } + } + +_out: + if (!list_empty(&pipeline->list)) + pipeline->inuse = 1; + else + pipeline->inuse = 0; + +#ifdef PIPELINE_DEBUG + printk(KERN_DEBUG "%s: dsp pipeline built%s: %s\n", + __func__, incomplete ? " incomplete" : "", cfg); +#endif + kfree(dup); + return 0; +} + +void dsp_pipeline_process_tx(struct dsp_pipeline *pipeline, u8 *data, int len) +{ + struct dsp_pipeline_entry *entry; + + if (!pipeline) + return; + + list_for_each_entry(entry, &pipeline->list, list) + if (entry->elem->process_tx) + entry->elem->process_tx(entry->p, data, len); +} + +void dsp_pipeline_process_rx(struct dsp_pipeline *pipeline, u8 *data, int len) +{ + struct dsp_pipeline_entry *entry; + + if (!pipeline) + return; + + list_for_each_entry_reverse(entry, &pipeline->list, list) + if (entry->elem->process_rx) + entry->elem->process_rx(entry->p, data, len); +} + + diff --git a/drivers/isdn/mISDN/dsp_tones.c b/drivers/isdn/mISDN/dsp_tones.c new file mode 100644 index 000000000000..23dd0dd21524 --- /dev/null +++ b/drivers/isdn/mISDN/dsp_tones.c @@ -0,0 +1,551 @@ +/* + * Audio support data for ISDN4Linux. + * + * Copyright Andreas Eversberg (jolly@eversberg.eu) + * + * This software may be used and distributed according to the terms + * of the GNU General Public License, incorporated herein by reference. + * + */ + +#include <linux/mISDNif.h> +#include <linux/mISDNdsp.h> +#include "core.h" +#include "dsp.h" + + +#define DATA_S sample_silence +#define SIZE_S (&sizeof_silence) +#define DATA_GA sample_german_all +#define SIZE_GA (&sizeof_german_all) +#define DATA_GO sample_german_old +#define SIZE_GO (&sizeof_german_old) +#define DATA_DT sample_american_dialtone +#define SIZE_DT (&sizeof_american_dialtone) +#define DATA_RI sample_american_ringing +#define SIZE_RI (&sizeof_american_ringing) +#define DATA_BU sample_american_busy +#define SIZE_BU (&sizeof_american_busy) +#define DATA_S1 sample_special1 +#define SIZE_S1 (&sizeof_special1) +#define DATA_S2 sample_special2 +#define SIZE_S2 (&sizeof_special2) +#define DATA_S3 sample_special3 +#define SIZE_S3 (&sizeof_special3) + +/***************/ +/* tones loops */ +/***************/ + +/* all tones are alaw encoded */ +/* the last sample+1 is in phase with the first sample. the error is low */ + +static u8 sample_german_all[] = { + 0x80, 0xab, 0x81, 0x6d, 0xfd, 0xdd, 0x5d, 0x9d, + 0x4d, 0xd1, 0x89, 0x88, 0xd0, 0x4c, 0x9c, 0x5c, + 0xdc, 0xfc, 0x6c, + 0x80, 0xab, 0x81, 0x6d, 0xfd, 0xdd, 0x5d, 0x9d, + 0x4d, 0xd1, 0x89, 0x88, 0xd0, 0x4c, 0x9c, 0x5c, + 0xdc, 0xfc, 0x6c, + 0x80, 0xab, 0x81, 0x6d, 0xfd, 0xdd, 0x5d, 0x9d, + 0x4d, 0xd1, 0x89, 0x88, 0xd0, 0x4c, 0x9c, 0x5c, + 0xdc, 0xfc, 0x6c, + 0x80, 0xab, 0x81, 0x6d, 0xfd, 0xdd, 0x5d, 0x9d, + 0x4d, 0xd1, 0x89, 0x88, 0xd0, 0x4c, 0x9c, 0x5c, + 0xdc, 0xfc, 0x6c, +}; +static u32 sizeof_german_all = sizeof(sample_german_all); + +static u8 sample_german_old[] = { + 0xec, 0x68, 0xe1, 0x6d, 0x6d, 0x91, 0x51, 0xed, + 0x6d, 0x01, 0x1e, 0x10, 0x0c, 0x90, 0x60, 0x70, + 0x8c, + 0xec, 0x68, 0xe1, 0x6d, 0x6d, 0x91, 0x51, 0xed, + 0x6d, 0x01, 0x1e, 0x10, 0x0c, 0x90, 0x60, 0x70, + 0x8c, + 0xec, 0x68, 0xe1, 0x6d, 0x6d, 0x91, 0x51, 0xed, + 0x6d, 0x01, 0x1e, 0x10, 0x0c, 0x90, 0x60, 0x70, + 0x8c, + 0xec, 0x68, 0xe1, 0x6d, 0x6d, 0x91, 0x51, 0xed, + 0x6d, 0x01, 0x1e, 0x10, 0x0c, 0x90, 0x60, 0x70, + 0x8c, +}; +static u32 sizeof_german_old = sizeof(sample_german_old); + +static u8 sample_american_dialtone[] = { + 0x2a, 0x18, 0x90, 0x6c, 0x4c, 0xbc, 0x4c, 0x6c, + 0x10, 0x58, 0x32, 0xb9, 0x31, 0x2d, 0x8d, 0x0d, + 0x8d, 0x2d, 0x31, 0x99, 0x0f, 0x28, 0x60, 0xf0, + 0xd0, 0x50, 0xd0, 0x30, 0x60, 0x08, 0x8e, 0x67, + 0x09, 0x19, 0x21, 0xe1, 0xd9, 0xb9, 0x29, 0x67, + 0x83, 0x02, 0xce, 0xbe, 0xee, 0x1a, 0x1b, 0xef, + 0xbf, 0xcf, 0x03, 0x82, 0x66, 0x28, 0xb8, 0xd8, + 0xe0, 0x20, 0x18, 0x08, 0x66, 0x8f, 0x09, 0x61, + 0x31, 0xd1, 0x51, 0xd1, 0xf1, 0x61, 0x29, 0x0e, + 0x98, 0x30, 0x2c, 0x8c, 0x0c, 0x8c, 0x2c, 0x30, + 0xb8, 0x33, 0x59, 0x11, 0x6d, 0x4d, 0xbd, 0x4d, + 0x6d, 0x91, 0x19, +}; +static u32 sizeof_american_dialtone = sizeof(sample_american_dialtone); + +static u8 sample_american_ringing[] = { + 0x2a, 0xe0, 0xac, 0x0c, 0xbc, 0x4c, 0x8c, 0x90, + 0x48, 0xc7, 0xc1, 0xed, 0xcd, 0x4d, 0xcd, 0xed, + 0xc1, 0xb7, 0x08, 0x30, 0xec, 0xcc, 0xcc, 0x8c, + 0x10, 0x58, 0x1a, 0x99, 0x71, 0xed, 0x8d, 0x8d, + 0x2d, 0x41, 0x89, 0x9e, 0x20, 0x70, 0x2c, 0xec, + 0x2c, 0x70, 0x20, 0x86, 0x77, 0xe1, 0x31, 0x11, + 0xd1, 0xf1, 0x81, 0x09, 0xa3, 0x56, 0x58, 0x00, + 0x40, 0xc0, 0x60, 0x38, 0x46, 0x43, 0x57, 0x39, + 0xd9, 0x59, 0x99, 0xc9, 0x77, 0x2f, 0x2e, 0xc6, + 0xd6, 0x28, 0xd6, 0x36, 0x26, 0x2e, 0x8a, 0xa3, + 0x43, 0x63, 0x4b, 0x4a, 0x62, 0x42, 0xa2, 0x8b, + 0x2f, 0x27, 0x37, 0xd7, 0x29, 0xd7, 0xc7, 0x2f, + 0x2e, 0x76, 0xc8, 0x98, 0x58, 0xd8, 0x38, 0x56, + 0x42, 0x47, 0x39, 0x61, 0xc1, 0x41, 0x01, 0x59, + 0x57, 0xa2, 0x08, 0x80, 0xf0, 0xd0, 0x10, 0x30, + 0xe0, 0x76, 0x87, 0x21, 0x71, 0x2d, 0xed, 0x2d, + 0x71, 0x21, 0x9f, 0x88, 0x40, 0x2c, 0x8c, 0x8c, + 0xec, 0x70, 0x98, 0x1b, 0x59, 0x11, 0x8d, 0xcd, + 0xcd, 0xed, 0x31, 0x09, 0xb6, 0xc0, 0xec, 0xcc, + 0x4c, 0xcc, 0xec, 0xc0, 0xc6, 0x49, 0x91, 0x8d, + 0x4d, 0xbd, 0x0d, 0xad, 0xe1, +}; +static u32 sizeof_american_ringing = sizeof(sample_american_ringing); + +static u8 sample_american_busy[] = { + 0x2a, 0x00, 0x6c, 0x4c, 0x4c, 0x6c, 0xb0, 0x66, + 0x99, 0x11, 0x6d, 0x8d, 0x2d, 0x41, 0xd7, 0x96, + 0x60, 0xf0, 0x70, 0x40, 0x58, 0xf6, 0x53, 0x57, + 0x09, 0x89, 0xd7, 0x5f, 0xe3, 0x2a, 0xe3, 0x5f, + 0xd7, 0x89, 0x09, 0x57, 0x53, 0xf6, 0x58, 0x40, + 0x70, 0xf0, 0x60, 0x96, 0xd7, 0x41, 0x2d, 0x8d, + 0x6d, 0x11, 0x99, 0x66, 0xb0, 0x6c, 0x4c, 0x4c, + 0x6c, 0x00, 0x2a, 0x01, 0x6d, 0x4d, 0x4d, 0x6d, + 0xb1, 0x67, 0x98, 0x10, 0x6c, 0x8c, 0x2c, 0x40, + 0xd6, 0x97, 0x61, 0xf1, 0x71, 0x41, 0x59, 0xf7, + 0x52, 0x56, 0x08, 0x88, 0xd6, 0x5e, 0xe2, 0x2a, + 0xe2, 0x5e, 0xd6, 0x88, 0x08, 0x56, 0x52, 0xf7, + 0x59, 0x41, 0x71, 0xf1, 0x61, 0x97, 0xd6, 0x40, + 0x2c, 0x8c, 0x6c, 0x10, 0x98, 0x67, 0xb1, 0x6d, + 0x4d, 0x4d, 0x6d, 0x01, +}; +static u32 sizeof_american_busy = sizeof(sample_american_busy); + +static u8 sample_special1[] = { + 0x2a, 0x2c, 0xbc, 0x6c, 0xd6, 0x71, 0xbd, 0x0d, + 0xd9, 0x80, 0xcc, 0x4c, 0x40, 0x39, 0x0d, 0xbd, + 0x11, 0x86, 0xec, 0xbc, 0xec, 0x0e, 0x51, 0xbd, + 0x8d, 0x89, 0x30, 0x4c, 0xcc, 0xe0, 0xe1, 0xcd, + 0x4d, 0x31, 0x88, 0x8c, 0xbc, 0x50, 0x0f, 0xed, + 0xbd, 0xed, 0x87, 0x10, 0xbc, 0x0c, 0x38, 0x41, + 0x4d, 0xcd, 0x81, 0xd8, 0x0c, 0xbc, 0x70, 0xd7, + 0x6d, 0xbd, 0x2d, +}; +static u32 sizeof_special1 = sizeof(sample_special1); + +static u8 sample_special2[] = { + 0x2a, 0xcc, 0x8c, 0xd7, 0x4d, 0x2d, 0x18, 0xbc, + 0x10, 0xc1, 0xbd, 0xc1, 0x10, 0xbc, 0x18, 0x2d, + 0x4d, 0xd7, 0x8c, 0xcc, 0x2a, 0xcd, 0x8d, 0xd6, + 0x4c, 0x2c, 0x19, 0xbd, 0x11, 0xc0, 0xbc, 0xc0, + 0x11, 0xbd, 0x19, 0x2c, 0x4c, 0xd6, 0x8d, 0xcd, + 0x2a, 0xcc, 0x8c, 0xd7, 0x4d, 0x2d, 0x18, 0xbc, + 0x10, 0xc1, 0xbd, 0xc1, 0x10, 0xbc, 0x18, 0x2d, + 0x4d, 0xd7, 0x8c, 0xcc, 0x2a, 0xcd, 0x8d, 0xd6, + 0x4c, 0x2c, 0x19, 0xbd, 0x11, 0xc0, 0xbc, 0xc0, + 0x11, 0xbd, 0x19, 0x2c, 0x4c, 0xd6, 0x8d, 0xcd, +}; +static u32 sizeof_special2 = sizeof(sample_special2); + +static u8 sample_special3[] = { + 0x2a, 0xbc, 0x18, 0xcd, 0x11, 0x2c, 0x8c, 0xc1, + 0x4d, 0xd6, 0xbc, 0xd6, 0x4d, 0xc1, 0x8c, 0x2c, + 0x11, 0xcd, 0x18, 0xbc, 0x2a, 0xbd, 0x19, 0xcc, + 0x10, 0x2d, 0x8d, 0xc0, 0x4c, 0xd7, 0xbd, 0xd7, + 0x4c, 0xc0, 0x8d, 0x2d, 0x10, 0xcc, 0x19, 0xbd, + 0x2a, 0xbc, 0x18, 0xcd, 0x11, 0x2c, 0x8c, 0xc1, + 0x4d, 0xd6, 0xbc, 0xd6, 0x4d, 0xc1, 0x8c, 0x2c, + 0x11, 0xcd, 0x18, 0xbc, 0x2a, 0xbd, 0x19, 0xcc, + 0x10, 0x2d, 0x8d, 0xc0, 0x4c, 0xd7, 0xbd, 0xd7, + 0x4c, 0xc0, 0x8d, 0x2d, 0x10, 0xcc, 0x19, 0xbd, +}; +static u32 sizeof_special3 = sizeof(sample_special3); + +static u8 sample_silence[] = { + 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, + 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, + 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, + 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, + 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, + 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, + 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, + 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, + 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, + 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, + 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, + 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a, +}; +static u32 sizeof_silence = sizeof(sample_silence); + +struct tones_samples { + u32 *len; + u8 *data; +}; +static struct +tones_samples samples[] = { + {&sizeof_german_all, sample_german_all}, + {&sizeof_german_old, sample_german_old}, + {&sizeof_american_dialtone, sample_american_dialtone}, + {&sizeof_american_ringing, sample_american_ringing}, + {&sizeof_american_busy, sample_american_busy}, + {&sizeof_special1, sample_special1}, + {&sizeof_special2, sample_special2}, + {&sizeof_special3, sample_special3}, + {NULL, NULL}, +}; + +/*********************************** + * generate ulaw from alaw samples * + ***********************************/ + +void +dsp_audio_generate_ulaw_samples(void) +{ + int i, j; + + i = 0; + while (samples[i].len) { + j = 0; + while (j < (*samples[i].len)) { + samples[i].data[j] = + dsp_audio_alaw_to_ulaw[samples[i].data[j]]; + j++; + } + i++; + } +} + + +/**************************** + * tone sequence definition * + ****************************/ + +struct pattern { + int tone; + u8 *data[10]; + u32 *siz[10]; + u32 seq[10]; +} pattern[] = { + {TONE_GERMAN_DIALTONE, + {DATA_GA, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {SIZE_GA, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {1900, 0, 0, 0, 0, 0, 0, 0, 0, 0} }, + + {TONE_GERMAN_OLDDIALTONE, + {DATA_GO, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {SIZE_GO, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {1998, 0, 0, 0, 0, 0, 0, 0, 0, 0} }, + + {TONE_AMERICAN_DIALTONE, + {DATA_DT, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {SIZE_DT, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {8000, 0, 0, 0, 0, 0, 0, 0, 0, 0} }, + + {TONE_GERMAN_DIALPBX, + {DATA_GA, DATA_S, DATA_GA, DATA_S, DATA_GA, DATA_S, 0, 0, 0, 0}, + {SIZE_GA, SIZE_S, SIZE_GA, SIZE_S, SIZE_GA, SIZE_S, 0, 0, 0, 0}, + {2000, 2000, 2000, 2000, 2000, 12000, 0, 0, 0, 0} }, + + {TONE_GERMAN_OLDDIALPBX, + {DATA_GO, DATA_S, DATA_GO, DATA_S, DATA_GO, DATA_S, 0, 0, 0, 0}, + {SIZE_GO, SIZE_S, SIZE_GO, SIZE_S, SIZE_GO, SIZE_S, 0, 0, 0, 0}, + {2000, 2000, 2000, 2000, 2000, 12000, 0, 0, 0, 0} }, + + {TONE_AMERICAN_DIALPBX, + {DATA_DT, DATA_S, DATA_DT, DATA_S, DATA_DT, DATA_S, 0, 0, 0, 0}, + {SIZE_DT, SIZE_S, SIZE_DT, SIZE_S, SIZE_DT, SIZE_S, 0, 0, 0, 0}, + {2000, 2000, 2000, 2000, 2000, 12000, 0, 0, 0, 0} }, + + {TONE_GERMAN_RINGING, + {DATA_GA, DATA_S, 0, 0, 0, 0, 0, 0, 0, 0}, + {SIZE_GA, SIZE_S, 0, 0, 0, 0, 0, 0, 0, 0}, + {8000, 32000, 0, 0, 0, 0, 0, 0, 0, 0} }, + + {TONE_GERMAN_OLDRINGING, + {DATA_GO, DATA_S, 0, 0, 0, 0, 0, 0, 0, 0}, + {SIZE_GO, SIZE_S, 0, 0, 0, 0, 0, 0, 0, 0}, + {8000, 40000, 0, 0, 0, 0, 0, 0, 0, 0} }, + + {TONE_AMERICAN_RINGING, + {DATA_RI, DATA_S, 0, 0, 0, 0, 0, 0, 0, 0}, + {SIZE_RI, SIZE_S, 0, 0, 0, 0, 0, 0, 0, 0}, + {8000, 32000, 0, 0, 0, 0, 0, 0, 0, 0} }, + + {TONE_GERMAN_RINGPBX, + {DATA_GA, DATA_S, DATA_GA, DATA_S, 0, 0, 0, 0, 0, 0}, + {SIZE_GA, SIZE_S, SIZE_GA, SIZE_S, 0, 0, 0, 0, 0, 0}, + {4000, 4000, 4000, 28000, 0, 0, 0, 0, 0, 0} }, + + {TONE_GERMAN_OLDRINGPBX, + {DATA_GO, DATA_S, DATA_GO, DATA_S, 0, 0, 0, 0, 0, 0}, + {SIZE_GO, SIZE_S, SIZE_GO, SIZE_S, 0, 0, 0, 0, 0, 0}, + {4000, 4000, 4000, 28000, 0, 0, 0, 0, 0, 0} }, + + {TONE_AMERICAN_RINGPBX, + {DATA_RI, DATA_S, DATA_RI, DATA_S, 0, 0, 0, 0, 0, 0}, + {SIZE_RI, SIZE_S, SIZE_RI, SIZE_S, 0, 0, 0, 0, 0, 0}, + {4000, 4000, 4000, 28000, 0, 0, 0, 0, 0, 0} }, + + {TONE_GERMAN_BUSY, + {DATA_GA, DATA_S, 0, 0, 0, 0, 0, 0, 0, 0}, + {SIZE_GA, SIZE_S, 0, 0, 0, 0, 0, 0, 0, 0}, + {4000, 4000, 0, 0, 0, 0, 0, 0, 0, 0} }, + + {TONE_GERMAN_OLDBUSY, + {DATA_GO, DATA_S, 0, 0, 0, 0, 0, 0, 0, 0}, + {SIZE_GO, SIZE_S, 0, 0, 0, 0, 0, 0, 0, 0}, + {1000, 5000, 0, 0, 0, 0, 0, 0, 0, 0} }, + + {TONE_AMERICAN_BUSY, + {DATA_BU, DATA_S, 0, 0, 0, 0, 0, 0, 0, 0}, + {SIZE_BU, SIZE_S, 0, 0, 0, 0, 0, 0, 0, 0}, + {4000, 4000, 0, 0, 0, 0, 0, 0, 0, 0} }, + + {TONE_GERMAN_HANGUP, + {DATA_GA, DATA_S, 0, 0, 0, 0, 0, 0, 0, 0}, + {SIZE_GA, SIZE_S, 0, 0, 0, 0, 0, 0, 0, 0}, + {4000, 4000, 0, 0, 0, 0, 0, 0, 0, 0} }, + + {TONE_GERMAN_OLDHANGUP, + {DATA_GO, DATA_S, 0, 0, 0, 0, 0, 0, 0, 0}, + {SIZE_GO, SIZE_S, 0, 0, 0, 0, 0, 0, 0, 0}, + {1000, 5000, 0, 0, 0, 0, 0, 0, 0, 0} }, + + {TONE_AMERICAN_HANGUP, + {DATA_DT, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {SIZE_DT, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {8000, 0, 0, 0, 0, 0, 0, 0, 0, 0} }, + + {TONE_SPECIAL_INFO, + {DATA_S1, DATA_S2, DATA_S3, DATA_S, 0, 0, 0, 0, 0, 0}, + {SIZE_S1, SIZE_S2, SIZE_S3, SIZE_S, 0, 0, 0, 0, 0, 0}, + {2666, 2666, 2666, 8002, 0, 0, 0, 0, 0, 0} }, + + {TONE_GERMAN_GASSENBESETZT, + {DATA_GA, DATA_S, 0, 0, 0, 0, 0, 0, 0, 0}, + {SIZE_GA, SIZE_S, 0, 0, 0, 0, 0, 0, 0, 0}, + {2000, 2000, 0, 0, 0, 0, 0, 0, 0, 0} }, + + {TONE_GERMAN_AUFSCHALTTON, + {DATA_GO, DATA_S, DATA_GO, DATA_S, 0, 0, 0, 0, 0, 0}, + {SIZE_GO, SIZE_S, SIZE_GO, SIZE_S, 0, 0, 0, 0, 0, 0}, + {1000, 5000, 1000, 17000, 0, 0, 0, 0, 0, 0} }, + + {0, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0} }, +}; + +/****************** + * copy tone data * + ******************/ + +/* an sk_buff is generated from the number of samples needed. + * the count will be changed and may begin from 0 each pattern period. + * the clue is to precalculate the pointers and legths to use only one + * memcpy per function call, or two memcpy if the tone sequence changes. + * + * pattern - the type of the pattern + * count - the sample from the beginning of the pattern (phase) + * len - the number of bytes + * + * return - the sk_buff with the sample + * + * if tones has finished (e.g. knocking tone), dsp->tones is turned off + */ +void dsp_tone_copy(struct dsp *dsp, u8 *data, int len) +{ + int index, count, start, num; + struct pattern *pat; + struct dsp_tone *tone = &dsp->tone; + + /* if we have no tone, we copy silence */ + if (!tone->tone) { + memset(data, dsp_silence, len); + return; + } + + /* process pattern */ + pat = (struct pattern *)tone->pattern; + /* points to the current pattern */ + index = tone->index; /* gives current sequence index */ + count = tone->count; /* gives current sample */ + + /* copy sample */ + while (len) { + /* find sample to start with */ + while (42) { + /* warp arround */ + if (!pat->seq[index]) { + count = 0; + index = 0; + } + /* check if we are currently playing this tone */ + if (count < pat->seq[index]) + break; + if (dsp_debug & DEBUG_DSP_TONE) + printk(KERN_DEBUG "%s: reaching next sequence " + "(index=%d)\n", __func__, index); + count -= pat->seq[index]; + index++; + } + /* calculate start and number of samples */ + start = count % (*(pat->siz[index])); + num = len; + if (num+count > pat->seq[index]) + num = pat->seq[index] - count; + if (num+start > (*(pat->siz[index]))) + num = (*(pat->siz[index])) - start; + /* copy memory */ + memcpy(data, pat->data[index]+start, num); + /* reduce length */ + data += num; + count += num; + len -= num; + } + tone->index = index; + tone->count = count; + + /* return sk_buff */ + return; +} + + +/******************************* + * send HW message to hfc card * + *******************************/ + +static void +dsp_tone_hw_message(struct dsp *dsp, u8 *sample, int len) +{ + struct sk_buff *nskb; + + /* unlocking is not required, because we don't expect a response */ + nskb = _alloc_mISDN_skb(PH_CONTROL_REQ, + (len)?HFC_SPL_LOOP_ON:HFC_SPL_LOOP_OFF, len, sample, + GFP_ATOMIC); + if (nskb) { + if (dsp->ch.peer) { + if (dsp->ch.recv(dsp->ch.peer, nskb)) + dev_kfree_skb(nskb); + } else + dev_kfree_skb(nskb); + } +} + + +/***************** + * timer expires * + *****************/ +void +dsp_tone_timeout(void *arg) +{ + struct dsp *dsp = arg; + struct dsp_tone *tone = &dsp->tone; + struct pattern *pat = (struct pattern *)tone->pattern; + int index = tone->index; + + if (!tone->tone) + return; + + index++; + if (!pat->seq[index]) + index = 0; + tone->index = index; + + /* set next tone */ + if (pat->data[index] == DATA_S) + dsp_tone_hw_message(dsp, 0, 0); + else + dsp_tone_hw_message(dsp, pat->data[index], *(pat->siz[index])); + /* set timer */ + init_timer(&tone->tl); + tone->tl.expires = jiffies + (pat->seq[index] * HZ) / 8000; + add_timer(&tone->tl); +} + + +/******************** + * set/release tone * + ********************/ + +/* + * tones are relaized by streaming or by special loop commands if supported + * by hardware. when hardware is used, the patterns will be controlled by + * timers. + */ +int +dsp_tone(struct dsp *dsp, int tone) +{ + struct pattern *pat; + int i; + struct dsp_tone *tonet = &dsp->tone; + + tonet->software = 0; + tonet->hardware = 0; + + /* we turn off the tone */ + if (!tone) { + if (dsp->features.hfc_loops) + if (timer_pending(&tonet->tl)) + del_timer(&tonet->tl); + if (dsp->features.hfc_loops) + dsp_tone_hw_message(dsp, NULL, 0); + tonet->tone = 0; + return 0; + } + + pat = NULL; + i = 0; + while (pattern[i].tone) { + if (pattern[i].tone == tone) { + pat = &pattern[i]; + break; + } + i++; + } + if (!pat) { + printk(KERN_WARNING "dsp: given tone 0x%x is invalid\n", tone); + return -EINVAL; + } + if (dsp_debug & DEBUG_DSP_TONE) + printk(KERN_DEBUG "%s: now starting tone %d (index=%d)\n", + __func__, tone, 0); + tonet->tone = tone; + tonet->pattern = pat; + tonet->index = 0; + tonet->count = 0; + + if (dsp->features.hfc_loops) { + tonet->hardware = 1; + /* set first tone */ + dsp_tone_hw_message(dsp, pat->data[0], *(pat->siz[0])); + /* set timer */ + if (timer_pending(&tonet->tl)) + del_timer(&tonet->tl); + init_timer(&tonet->tl); + tonet->tl.expires = jiffies + (pat->seq[0] * HZ) / 8000; + add_timer(&tonet->tl); + } else { + tonet->software = 1; + } + + return 0; +} + + + + + diff --git a/drivers/isdn/mISDN/fsm.c b/drivers/isdn/mISDN/fsm.c new file mode 100644 index 000000000000..b5d6553f2dc8 --- /dev/null +++ b/drivers/isdn/mISDN/fsm.c @@ -0,0 +1,183 @@ +/* + * finite state machine implementation + * + * Author Karsten Keil <kkeil@novell.com> + * + * Thanks to Jan den Ouden + * Fritz Elfert + * Copyright 2008 by Karsten Keil <kkeil@novell.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#include <linux/kernel.h> +#include <linux/slab.h> +#include <linux/module.h> +#include <linux/string.h> +#include "fsm.h" + +#define FSM_TIMER_DEBUG 0 + +void +mISDN_FsmNew(struct Fsm *fsm, + struct FsmNode *fnlist, int fncount) +{ + int i; + + fsm->jumpmatrix = kzalloc(sizeof(FSMFNPTR) * fsm->state_count * + fsm->event_count, GFP_KERNEL); + + for (i = 0; i < fncount; i++) + if ((fnlist[i].state >= fsm->state_count) || + (fnlist[i].event >= fsm->event_count)) { + printk(KERN_ERR + "mISDN_FsmNew Error: %d st(%ld/%ld) ev(%ld/%ld)\n", + i, (long)fnlist[i].state, (long)fsm->state_count, + (long)fnlist[i].event, (long)fsm->event_count); + } else + fsm->jumpmatrix[fsm->state_count * fnlist[i].event + + fnlist[i].state] = (FSMFNPTR) fnlist[i].routine; +} +EXPORT_SYMBOL(mISDN_FsmNew); + +void +mISDN_FsmFree(struct Fsm *fsm) +{ + kfree((void *) fsm->jumpmatrix); +} +EXPORT_SYMBOL(mISDN_FsmFree); + +int +mISDN_FsmEvent(struct FsmInst *fi, int event, void *arg) +{ + FSMFNPTR r; + + if ((fi->state >= fi->fsm->state_count) || + (event >= fi->fsm->event_count)) { + printk(KERN_ERR + "mISDN_FsmEvent Error st(%ld/%ld) ev(%d/%ld)\n", + (long)fi->state, (long)fi->fsm->state_count, event, + (long)fi->fsm->event_count); + return 1; + } + r = fi->fsm->jumpmatrix[fi->fsm->state_count * event + fi->state]; + if (r) { + if (fi->debug) + fi->printdebug(fi, "State %s Event %s", + fi->fsm->strState[fi->state], + fi->fsm->strEvent[event]); + r(fi, event, arg); + return 0; + } else { + if (fi->debug) + fi->printdebug(fi, "State %s Event %s no action", + fi->fsm->strState[fi->state], + fi->fsm->strEvent[event]); + return 1; + } +} +EXPORT_SYMBOL(mISDN_FsmEvent); + +void +mISDN_FsmChangeState(struct FsmInst *fi, int newstate) +{ + fi->state = newstate; + if (fi->debug) + fi->printdebug(fi, "ChangeState %s", + fi->fsm->strState[newstate]); +} +EXPORT_SYMBOL(mISDN_FsmChangeState); + +static void +FsmExpireTimer(struct FsmTimer *ft) +{ +#if FSM_TIMER_DEBUG + if (ft->fi->debug) + ft->fi->printdebug(ft->fi, "FsmExpireTimer %lx", (long) ft); +#endif + mISDN_FsmEvent(ft->fi, ft->event, ft->arg); +} + +void +mISDN_FsmInitTimer(struct FsmInst *fi, struct FsmTimer *ft) +{ + ft->fi = fi; + ft->tl.function = (void *) FsmExpireTimer; + ft->tl.data = (long) ft; +#if FSM_TIMER_DEBUG + if (ft->fi->debug) + ft->fi->printdebug(ft->fi, "mISDN_FsmInitTimer %lx", (long) ft); +#endif + init_timer(&ft->tl); +} +EXPORT_SYMBOL(mISDN_FsmInitTimer); + +void +mISDN_FsmDelTimer(struct FsmTimer *ft, int where) +{ +#if FSM_TIMER_DEBUG + if (ft->fi->debug) + ft->fi->printdebug(ft->fi, "mISDN_FsmDelTimer %lx %d", + (long) ft, where); +#endif + del_timer(&ft->tl); +} +EXPORT_SYMBOL(mISDN_FsmDelTimer); + +int +mISDN_FsmAddTimer(struct FsmTimer *ft, + int millisec, int event, void *arg, int where) +{ + +#if FSM_TIMER_DEBUG + if (ft->fi->debug) + ft->fi->printdebug(ft->fi, "mISDN_FsmAddTimer %lx %d %d", + (long) ft, millisec, where); +#endif + + if (timer_pending(&ft->tl)) { + if (ft->fi->debug) { + printk(KERN_WARNING + "mISDN_FsmAddTimer: timer already active!\n"); + ft->fi->printdebug(ft->fi, + "mISDN_FsmAddTimer already active!"); + } + return -1; + } + init_timer(&ft->tl); + ft->event = event; + ft->arg = arg; + ft->tl.expires = jiffies + (millisec * HZ) / 1000; + add_timer(&ft->tl); + return 0; +} +EXPORT_SYMBOL(mISDN_FsmAddTimer); + +void +mISDN_FsmRestartTimer(struct FsmTimer *ft, + int millisec, int event, void *arg, int where) +{ + +#if FSM_TIMER_DEBUG + if (ft->fi->debug) + ft->fi->printdebug(ft->fi, "mISDN_FsmRestartTimer %lx %d %d", + (long) ft, millisec, where); +#endif + + if (timer_pending(&ft->tl)) + del_timer(&ft->tl); + init_timer(&ft->tl); + ft->event = event; + ft->arg = arg; + ft->tl.expires = jiffies + (millisec * HZ) / 1000; + add_timer(&ft->tl); +} +EXPORT_SYMBOL(mISDN_FsmRestartTimer); diff --git a/drivers/isdn/mISDN/fsm.h b/drivers/isdn/mISDN/fsm.h new file mode 100644 index 000000000000..928f5be192c1 --- /dev/null +++ b/drivers/isdn/mISDN/fsm.h @@ -0,0 +1,67 @@ +/* + * + * Author Karsten Keil <kkeil@novell.com> + * + * Thanks to Jan den Ouden + * Fritz Elfert + * Copyright 2008 by Karsten Keil <kkeil@novell.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#ifndef _MISDN_FSM_H +#define _MISDN_FSM_H + +#include <linux/timer.h> + +/* Statemachine */ + +struct FsmInst; + +typedef void (*FSMFNPTR)(struct FsmInst *, int, void *); + +struct Fsm { + FSMFNPTR *jumpmatrix; + int state_count, event_count; + char **strEvent, **strState; +}; + +struct FsmInst { + struct Fsm *fsm; + int state; + int debug; + void *userdata; + int userint; + void (*printdebug) (struct FsmInst *, char *, ...); +}; + +struct FsmNode { + int state, event; + void (*routine) (struct FsmInst *, int, void *); +}; + +struct FsmTimer { + struct FsmInst *fi; + struct timer_list tl; + int event; + void *arg; +}; + +extern void mISDN_FsmNew(struct Fsm *, struct FsmNode *, int); +extern void mISDN_FsmFree(struct Fsm *); +extern int mISDN_FsmEvent(struct FsmInst *, int , void *); +extern void mISDN_FsmChangeState(struct FsmInst *, int); +extern void mISDN_FsmInitTimer(struct FsmInst *, struct FsmTimer *); +extern int mISDN_FsmAddTimer(struct FsmTimer *, int, int, void *, int); +extern void mISDN_FsmRestartTimer(struct FsmTimer *, int, int, void *, int); +extern void mISDN_FsmDelTimer(struct FsmTimer *, int); + +#endif diff --git a/drivers/isdn/mISDN/hwchannel.c b/drivers/isdn/mISDN/hwchannel.c new file mode 100644 index 000000000000..2596fba4e614 --- /dev/null +++ b/drivers/isdn/mISDN/hwchannel.c @@ -0,0 +1,365 @@ +/* + * + * Author Karsten Keil <kkeil@novell.com> + * + * Copyright 2008 by Karsten Keil <kkeil@novell.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#include <linux/module.h> +#include <linux/mISDNhw.h> + +static void +dchannel_bh(struct work_struct *ws) +{ + struct dchannel *dch = container_of(ws, struct dchannel, workq); + struct sk_buff *skb; + int err; + + if (test_and_clear_bit(FLG_RECVQUEUE, &dch->Flags)) { + while ((skb = skb_dequeue(&dch->rqueue))) { + if (likely(dch->dev.D.peer)) { + err = dch->dev.D.recv(dch->dev.D.peer, skb); + if (err) + dev_kfree_skb(skb); + } else + dev_kfree_skb(skb); + } + } + if (test_and_clear_bit(FLG_PHCHANGE, &dch->Flags)) { + if (dch->phfunc) + dch->phfunc(dch); + } +} + +static void +bchannel_bh(struct work_struct *ws) +{ + struct bchannel *bch = container_of(ws, struct bchannel, workq); + struct sk_buff *skb; + int err; + + if (test_and_clear_bit(FLG_RECVQUEUE, &bch->Flags)) { + while ((skb = skb_dequeue(&bch->rqueue))) { + if (bch->rcount >= 64) + printk(KERN_WARNING "B-channel %p receive " + "queue if full, but empties...\n", bch); + bch->rcount--; + if (likely(bch->ch.peer)) { + err = bch->ch.recv(bch->ch.peer, skb); + if (err) + dev_kfree_skb(skb); + } else + dev_kfree_skb(skb); + } + } +} + +int +mISDN_initdchannel(struct dchannel *ch, int maxlen, void *phf) +{ + test_and_set_bit(FLG_HDLC, &ch->Flags); + ch->maxlen = maxlen; + ch->hw = NULL; + ch->rx_skb = NULL; + ch->tx_skb = NULL; + ch->tx_idx = 0; + ch->phfunc = phf; + skb_queue_head_init(&ch->squeue); + skb_queue_head_init(&ch->rqueue); + INIT_LIST_HEAD(&ch->dev.bchannels); + INIT_WORK(&ch->workq, dchannel_bh); + return 0; +} +EXPORT_SYMBOL(mISDN_initdchannel); + +int +mISDN_initbchannel(struct bchannel *ch, int maxlen) +{ + ch->Flags = 0; + ch->maxlen = maxlen; + ch->hw = NULL; + ch->rx_skb = NULL; + ch->tx_skb = NULL; + ch->tx_idx = 0; + skb_queue_head_init(&ch->rqueue); + ch->rcount = 0; + ch->next_skb = NULL; + INIT_WORK(&ch->workq, bchannel_bh); + return 0; +} +EXPORT_SYMBOL(mISDN_initbchannel); + +int +mISDN_freedchannel(struct dchannel *ch) +{ + if (ch->tx_skb) { + dev_kfree_skb(ch->tx_skb); + ch->tx_skb = NULL; + } + if (ch->rx_skb) { + dev_kfree_skb(ch->rx_skb); + ch->rx_skb = NULL; + } + skb_queue_purge(&ch->squeue); + skb_queue_purge(&ch->rqueue); + flush_scheduled_work(); + return 0; +} +EXPORT_SYMBOL(mISDN_freedchannel); + +int +mISDN_freebchannel(struct bchannel *ch) +{ + if (ch->tx_skb) { + dev_kfree_skb(ch->tx_skb); + ch->tx_skb = NULL; + } + if (ch->rx_skb) { + dev_kfree_skb(ch->rx_skb); + ch->rx_skb = NULL; + } + if (ch->next_skb) { + dev_kfree_skb(ch->next_skb); + ch->next_skb = NULL; + } + skb_queue_purge(&ch->rqueue); + ch->rcount = 0; + flush_scheduled_work(); + return 0; +} +EXPORT_SYMBOL(mISDN_freebchannel); + +static inline u_int +get_sapi_tei(u_char *p) +{ + u_int sapi, tei; + + sapi = *p >> 2; + tei = p[1] >> 1; + return sapi | (tei << 8); +} + +void +recv_Dchannel(struct dchannel *dch) +{ + struct mISDNhead *hh; + + if (dch->rx_skb->len < 2) { /* at least 2 for sapi / tei */ + dev_kfree_skb(dch->rx_skb); + dch->rx_skb = NULL; + return; + } + hh = mISDN_HEAD_P(dch->rx_skb); + hh->prim = PH_DATA_IND; + hh->id = get_sapi_tei(dch->rx_skb->data); + skb_queue_tail(&dch->rqueue, dch->rx_skb); + dch->rx_skb = NULL; + schedule_event(dch, FLG_RECVQUEUE); +} +EXPORT_SYMBOL(recv_Dchannel); + +void +recv_Bchannel(struct bchannel *bch) +{ + struct mISDNhead *hh; + + hh = mISDN_HEAD_P(bch->rx_skb); + hh->prim = PH_DATA_IND; + hh->id = MISDN_ID_ANY; + if (bch->rcount >= 64) { + dev_kfree_skb(bch->rx_skb); + bch->rx_skb = NULL; + return; + } + bch->rcount++; + skb_queue_tail(&bch->rqueue, bch->rx_skb); + bch->rx_skb = NULL; + schedule_event(bch, FLG_RECVQUEUE); +} +EXPORT_SYMBOL(recv_Bchannel); + +void +recv_Dchannel_skb(struct dchannel *dch, struct sk_buff *skb) +{ + skb_queue_tail(&dch->rqueue, skb); + schedule_event(dch, FLG_RECVQUEUE); +} +EXPORT_SYMBOL(recv_Dchannel_skb); + +void +recv_Bchannel_skb(struct bchannel *bch, struct sk_buff *skb) +{ + if (bch->rcount >= 64) { + dev_kfree_skb(skb); + return; + } + bch->rcount++; + skb_queue_tail(&bch->rqueue, skb); + schedule_event(bch, FLG_RECVQUEUE); +} +EXPORT_SYMBOL(recv_Bchannel_skb); + +static void +confirm_Dsend(struct dchannel *dch) +{ + struct sk_buff *skb; + + skb = _alloc_mISDN_skb(PH_DATA_CNF, mISDN_HEAD_ID(dch->tx_skb), + 0, NULL, GFP_ATOMIC); + if (!skb) { + printk(KERN_ERR "%s: no skb id %x\n", __func__, + mISDN_HEAD_ID(dch->tx_skb)); + return; + } + skb_queue_tail(&dch->rqueue, skb); + schedule_event(dch, FLG_RECVQUEUE); +} + +int +get_next_dframe(struct dchannel *dch) +{ + dch->tx_idx = 0; + dch->tx_skb = skb_dequeue(&dch->squeue); + if (dch->tx_skb) { + confirm_Dsend(dch); + return 1; + } + dch->tx_skb = NULL; + test_and_clear_bit(FLG_TX_BUSY, &dch->Flags); + return 0; +} +EXPORT_SYMBOL(get_next_dframe); + +void +confirm_Bsend(struct bchannel *bch) +{ + struct sk_buff *skb; + + if (bch->rcount >= 64) + return; + skb = _alloc_mISDN_skb(PH_DATA_CNF, mISDN_HEAD_ID(bch->tx_skb), + 0, NULL, GFP_ATOMIC); + if (!skb) { + printk(KERN_ERR "%s: no skb id %x\n", __func__, + mISDN_HEAD_ID(bch->tx_skb)); + return; + } + bch->rcount++; + skb_queue_tail(&bch->rqueue, skb); + schedule_event(bch, FLG_RECVQUEUE); +} +EXPORT_SYMBOL(confirm_Bsend); + +int +get_next_bframe(struct bchannel *bch) +{ + bch->tx_idx = 0; + if (test_bit(FLG_TX_NEXT, &bch->Flags)) { + bch->tx_skb = bch->next_skb; + if (bch->tx_skb) { + bch->next_skb = NULL; + test_and_clear_bit(FLG_TX_NEXT, &bch->Flags); + if (!test_bit(FLG_TRANSPARENT, &bch->Flags)) + confirm_Bsend(bch); /* not for transparent */ + return 1; + } else { + test_and_clear_bit(FLG_TX_NEXT, &bch->Flags); + printk(KERN_WARNING "B TX_NEXT without skb\n"); + } + } + bch->tx_skb = NULL; + test_and_clear_bit(FLG_TX_BUSY, &bch->Flags); + return 0; +} +EXPORT_SYMBOL(get_next_bframe); + +void +queue_ch_frame(struct mISDNchannel *ch, u_int pr, int id, struct sk_buff *skb) +{ + struct mISDNhead *hh; + + if (!skb) { + _queue_data(ch, pr, id, 0, NULL, GFP_ATOMIC); + } else { + if (ch->peer) { + hh = mISDN_HEAD_P(skb); + hh->prim = pr; + hh->id = id; + if (!ch->recv(ch->peer, skb)) + return; + } + dev_kfree_skb(skb); + } +} +EXPORT_SYMBOL(queue_ch_frame); + +int +dchannel_senddata(struct dchannel *ch, struct sk_buff *skb) +{ + /* check oversize */ + if (skb->len <= 0) { + printk(KERN_WARNING "%s: skb too small\n", __func__); + return -EINVAL; + } + if (skb->len > ch->maxlen) { + printk(KERN_WARNING "%s: skb too large(%d/%d)\n", + __func__, skb->len, ch->maxlen); + return -EINVAL; + } + /* HW lock must be obtained */ + if (test_and_set_bit(FLG_TX_BUSY, &ch->Flags)) { + skb_queue_tail(&ch->squeue, skb); + return 0; + } else { + /* write to fifo */ + ch->tx_skb = skb; + ch->tx_idx = 0; + return 1; + } +} +EXPORT_SYMBOL(dchannel_senddata); + +int +bchannel_senddata(struct bchannel *ch, struct sk_buff *skb) +{ + + /* check oversize */ + if (skb->len <= 0) { + printk(KERN_WARNING "%s: skb too small\n", __func__); + return -EINVAL; + } + if (skb->len > ch->maxlen) { + printk(KERN_WARNING "%s: skb too large(%d/%d)\n", + __func__, skb->len, ch->maxlen); + return -EINVAL; + } + /* HW lock must be obtained */ + /* check for pending next_skb */ + if (ch->next_skb) { + printk(KERN_WARNING + "%s: next_skb exist ERROR (skb->len=%d next_skb->len=%d)\n", + __func__, skb->len, ch->next_skb->len); + return -EBUSY; + } + if (test_and_set_bit(FLG_TX_BUSY, &ch->Flags)) { + test_and_set_bit(FLG_TX_NEXT, &ch->Flags); + ch->next_skb = skb; + return 0; + } else { + /* write to fifo */ + ch->tx_skb = skb; + ch->tx_idx = 0; + return 1; + } +} +EXPORT_SYMBOL(bchannel_senddata); diff --git a/drivers/isdn/mISDN/l1oip.h b/drivers/isdn/mISDN/l1oip.h new file mode 100644 index 000000000000..a23d575449f6 --- /dev/null +++ b/drivers/isdn/mISDN/l1oip.h @@ -0,0 +1,91 @@ +/* + * see notice in l1oip.c + */ + +/* debugging */ +#define DEBUG_L1OIP_INIT 0x00010000 +#define DEBUG_L1OIP_SOCKET 0x00020000 +#define DEBUG_L1OIP_MGR 0x00040000 +#define DEBUG_L1OIP_MSG 0x00080000 + +/* enable to disorder received bchannels by sequence 2143658798... */ +/* +#define REORDER_DEBUG +*/ + +/* frames */ +#define L1OIP_MAX_LEN 2048 /* max packet size form l2 */ +#define L1OIP_MAX_PERFRAME 1400 /* max data size in one frame */ + + +/* timers */ +#define L1OIP_KEEPALIVE 15 +#define L1OIP_TIMEOUT 65 + + +/* socket */ +#define L1OIP_DEFAULTPORT 931 + + +/* channel structure */ +struct l1oip_chan { + struct dchannel *dch; + struct bchannel *bch; + u32 tx_counter; /* counts xmit bytes/packets */ + u32 rx_counter; /* counts recv bytes/packets */ + u32 codecstate; /* used by codec to save data */ +#ifdef REORDER_DEBUG + int disorder_flag; + struct sk_buff *disorder_skb; + u32 disorder_cnt; +#endif +}; + + +/* card structure */ +struct l1oip { + struct list_head list; + + /* card */ + int registered; /* if registered with mISDN */ + char name[MISDN_MAX_IDLEN]; + int idx; /* card index */ + int pri; /* 1=pri, 0=bri */ + int d_idx; /* current dchannel number */ + int b_num; /* number of bchannels */ + u32 id; /* id of connection */ + int ondemand; /* if transmis. is on demand */ + int bundle; /* bundle channels in one frm */ + int codec; /* codec to use for transmis. */ + int limit; /* limit number of bchannels */ + + /* timer */ + struct timer_list keep_tl; + struct timer_list timeout_tl; + int timeout_on; + struct work_struct workq; + + /* socket */ + struct socket *socket; /* if set, socket is created */ + struct completion socket_complete;/* completion of sock thread */ + struct task_struct *socket_thread; + spinlock_t socket_lock; /* access sock outside thread */ + u32 remoteip; /* if all set, ip is assigned */ + u16 localport; /* must always be set */ + u16 remoteport; /* must always be set */ + struct sockaddr_in sin_local; /* local socket name */ + struct sockaddr_in sin_remote; /* remote socket name */ + struct msghdr sendmsg; /* ip message to send */ + struct iovec sendiov; /* iov for message */ + + /* frame */ + struct l1oip_chan chan[128]; /* channel instances */ +}; + +extern int l1oip_law_to_4bit(u8 *data, int len, u8 *result, u32 *state); +extern int l1oip_4bit_to_law(u8 *data, int len, u8 *result); +extern int l1oip_alaw_to_ulaw(u8 *data, int len, u8 *result); +extern int l1oip_ulaw_to_alaw(u8 *data, int len, u8 *result); +extern void l1oip_4bit_free(void); +extern int l1oip_4bit_alloc(int ulaw); + diff --git a/drivers/isdn/mISDN/l1oip_codec.c b/drivers/isdn/mISDN/l1oip_codec.c new file mode 100644 index 000000000000..a2dc4570ef43 --- /dev/null +++ b/drivers/isdn/mISDN/l1oip_codec.c @@ -0,0 +1,374 @@ +/* + + * l1oip_codec.c generic codec using lookup table + * -> conversion from a-Law to u-Law + * -> conversion from u-Law to a-Law + * -> compression by reducing the number of sample resolution to 4 + * + * NOTE: It is not compatible with any standard codec like ADPCM. + * + * Author Andreas Eversberg (jolly@eversberg.eu) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + */ + +/* + +How the codec works: +-------------------- + +The volume is increased to increase the dynamic range of the audio signal. +Each sample is converted to a-LAW with only 16 steps of level resolution. +A pair of two samples are stored in one byte. + +The first byte is stored in the upper bits, the second byte is stored in the +lower bits. + +To speed up compression and decompression, two lookup tables are formed: + +- 16 bits index for two samples (law encoded) with 8 bit compressed result. +- 8 bits index for one compressed data with 16 bits decompressed result. + +NOTE: The bytes are handled as they are law-encoded. + +*/ + +#include <linux/vmalloc.h> +#include <linux/mISDNif.h> +#include "core.h" + +/* definitions of codec. don't use calculations, code may run slower. */ + +static u8 *table_com; +static u16 *table_dec; + + +/* alaw -> ulaw */ +static u8 alaw_to_ulaw[256] = +{ + 0xab, 0x2b, 0xe3, 0x63, 0x8b, 0x0b, 0xc9, 0x49, + 0xba, 0x3a, 0xf6, 0x76, 0x9b, 0x1b, 0xd7, 0x57, + 0xa3, 0x23, 0xdd, 0x5d, 0x83, 0x03, 0xc1, 0x41, + 0xb2, 0x32, 0xeb, 0x6b, 0x93, 0x13, 0xcf, 0x4f, + 0xaf, 0x2f, 0xe7, 0x67, 0x8f, 0x0f, 0xcd, 0x4d, + 0xbe, 0x3e, 0xfe, 0x7e, 0x9f, 0x1f, 0xdb, 0x5b, + 0xa7, 0x27, 0xdf, 0x5f, 0x87, 0x07, 0xc5, 0x45, + 0xb6, 0x36, 0xef, 0x6f, 0x97, 0x17, 0xd3, 0x53, + 0xa9, 0x29, 0xe1, 0x61, 0x89, 0x09, 0xc7, 0x47, + 0xb8, 0x38, 0xf2, 0x72, 0x99, 0x19, 0xd5, 0x55, + 0xa1, 0x21, 0xdc, 0x5c, 0x81, 0x01, 0xbf, 0x3f, + 0xb0, 0x30, 0xe9, 0x69, 0x91, 0x11, 0xce, 0x4e, + 0xad, 0x2d, 0xe5, 0x65, 0x8d, 0x0d, 0xcb, 0x4b, + 0xbc, 0x3c, 0xfa, 0x7a, 0x9d, 0x1d, 0xd9, 0x59, + 0xa5, 0x25, 0xde, 0x5e, 0x85, 0x05, 0xc3, 0x43, + 0xb4, 0x34, 0xed, 0x6d, 0x95, 0x15, 0xd1, 0x51, + 0xac, 0x2c, 0xe4, 0x64, 0x8c, 0x0c, 0xca, 0x4a, + 0xbb, 0x3b, 0xf8, 0x78, 0x9c, 0x1c, 0xd8, 0x58, + 0xa4, 0x24, 0xde, 0x5e, 0x84, 0x04, 0xc2, 0x42, + 0xb3, 0x33, 0xec, 0x6c, 0x94, 0x14, 0xd0, 0x50, + 0xb0, 0x30, 0xe8, 0x68, 0x90, 0x10, 0xce, 0x4e, + 0xbf, 0x3f, 0xfe, 0x7e, 0xa0, 0x20, 0xdc, 0x5c, + 0xa8, 0x28, 0xe0, 0x60, 0x88, 0x08, 0xc6, 0x46, + 0xb7, 0x37, 0xf0, 0x70, 0x98, 0x18, 0xd4, 0x54, + 0xaa, 0x2a, 0xe2, 0x62, 0x8a, 0x0a, 0xc8, 0x48, + 0xb9, 0x39, 0xf4, 0x74, 0x9a, 0x1a, 0xd6, 0x56, + 0xa2, 0x22, 0xdd, 0x5d, 0x82, 0x02, 0xc0, 0x40, + 0xb1, 0x31, 0xea, 0x6a, 0x92, 0x12, 0xcf, 0x4f, + 0xae, 0x2e, 0xe6, 0x66, 0x8e, 0x0e, 0xcc, 0x4c, + 0xbd, 0x3d, 0xfc, 0x7c, 0x9e, 0x1e, 0xda, 0x5a, + 0xa6, 0x26, 0xdf, 0x5f, 0x86, 0x06, 0xc4, 0x44, + 0xb5, 0x35, 0xee, 0x6e, 0x96, 0x16, 0xd2, 0x52 +}; + +/* ulaw -> alaw */ +static u8 ulaw_to_alaw[256] = +{ + 0xab, 0x55, 0xd5, 0x15, 0x95, 0x75, 0xf5, 0x35, + 0xb5, 0x45, 0xc5, 0x05, 0x85, 0x65, 0xe5, 0x25, + 0xa5, 0x5d, 0xdd, 0x1d, 0x9d, 0x7d, 0xfd, 0x3d, + 0xbd, 0x4d, 0xcd, 0x0d, 0x8d, 0x6d, 0xed, 0x2d, + 0xad, 0x51, 0xd1, 0x11, 0x91, 0x71, 0xf1, 0x31, + 0xb1, 0x41, 0xc1, 0x01, 0x81, 0x61, 0xe1, 0x21, + 0x59, 0xd9, 0x19, 0x99, 0x79, 0xf9, 0x39, 0xb9, + 0x49, 0xc9, 0x09, 0x89, 0x69, 0xe9, 0x29, 0xa9, + 0xd7, 0x17, 0x97, 0x77, 0xf7, 0x37, 0xb7, 0x47, + 0xc7, 0x07, 0x87, 0x67, 0xe7, 0x27, 0xa7, 0xdf, + 0x9f, 0x7f, 0xff, 0x3f, 0xbf, 0x4f, 0xcf, 0x0f, + 0x8f, 0x6f, 0xef, 0x2f, 0x53, 0x13, 0x73, 0x33, + 0xb3, 0x43, 0xc3, 0x03, 0x83, 0x63, 0xe3, 0x23, + 0xa3, 0x5b, 0xdb, 0x1b, 0x9b, 0x7b, 0xfb, 0x3b, + 0xbb, 0xbb, 0x4b, 0x4b, 0xcb, 0xcb, 0x0b, 0x0b, + 0x8b, 0x8b, 0x6b, 0x6b, 0xeb, 0xeb, 0x2b, 0x2b, + 0xab, 0x54, 0xd4, 0x14, 0x94, 0x74, 0xf4, 0x34, + 0xb4, 0x44, 0xc4, 0x04, 0x84, 0x64, 0xe4, 0x24, + 0xa4, 0x5c, 0xdc, 0x1c, 0x9c, 0x7c, 0xfc, 0x3c, + 0xbc, 0x4c, 0xcc, 0x0c, 0x8c, 0x6c, 0xec, 0x2c, + 0xac, 0x50, 0xd0, 0x10, 0x90, 0x70, 0xf0, 0x30, + 0xb0, 0x40, 0xc0, 0x00, 0x80, 0x60, 0xe0, 0x20, + 0x58, 0xd8, 0x18, 0x98, 0x78, 0xf8, 0x38, 0xb8, + 0x48, 0xc8, 0x08, 0x88, 0x68, 0xe8, 0x28, 0xa8, + 0xd6, 0x16, 0x96, 0x76, 0xf6, 0x36, 0xb6, 0x46, + 0xc6, 0x06, 0x86, 0x66, 0xe6, 0x26, 0xa6, 0xde, + 0x9e, 0x7e, 0xfe, 0x3e, 0xbe, 0x4e, 0xce, 0x0e, + 0x8e, 0x6e, 0xee, 0x2e, 0x52, 0x12, 0x72, 0x32, + 0xb2, 0x42, 0xc2, 0x02, 0x82, 0x62, 0xe2, 0x22, + 0xa2, 0x5a, 0xda, 0x1a, 0x9a, 0x7a, 0xfa, 0x3a, + 0xba, 0xba, 0x4a, 0x4a, 0xca, 0xca, 0x0a, 0x0a, + 0x8a, 0x8a, 0x6a, 0x6a, 0xea, 0xea, 0x2a, 0x2a +}; + +/* alaw -> 4bit compression */ +static u8 alaw_to_4bit[256] = { + 0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03, + 0x0d, 0x02, 0x08, 0x07, 0x0f, 0x00, 0x0b, 0x04, + 0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03, + 0x0d, 0x02, 0x09, 0x06, 0x0f, 0x00, 0x0b, 0x04, + 0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03, + 0x0d, 0x02, 0x08, 0x07, 0x0f, 0x00, 0x0b, 0x04, + 0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03, + 0x0d, 0x02, 0x09, 0x06, 0x0f, 0x00, 0x0b, 0x04, + 0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03, + 0x0d, 0x02, 0x08, 0x07, 0x0f, 0x00, 0x0b, 0x04, + 0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0d, 0x02, + 0x0e, 0x02, 0x09, 0x06, 0x0f, 0x00, 0x0b, 0x04, + 0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03, + 0x0d, 0x02, 0x08, 0x07, 0x0f, 0x00, 0x0b, 0x04, + 0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03, + 0x0d, 0x02, 0x09, 0x06, 0x0f, 0x00, 0x0b, 0x04, + 0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03, + 0x0d, 0x02, 0x08, 0x07, 0x0f, 0x00, 0x0b, 0x04, + 0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03, + 0x0d, 0x02, 0x09, 0x06, 0x0f, 0x00, 0x0b, 0x04, + 0x0e, 0x02, 0x09, 0x06, 0x0f, 0x00, 0x0b, 0x04, + 0x0d, 0x02, 0x08, 0x07, 0x0f, 0x01, 0x0a, 0x05, + 0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03, + 0x0d, 0x02, 0x09, 0x07, 0x0f, 0x00, 0x0b, 0x04, + 0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03, + 0x0d, 0x02, 0x08, 0x07, 0x0f, 0x00, 0x0b, 0x04, + 0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03, + 0x0d, 0x02, 0x09, 0x06, 0x0f, 0x00, 0x0b, 0x04, + 0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03, + 0x0d, 0x02, 0x08, 0x07, 0x0f, 0x00, 0x0b, 0x04, + 0x0e, 0x01, 0x0a, 0x05, 0x0f, 0x00, 0x0c, 0x03, + 0x0d, 0x02, 0x09, 0x06, 0x0f, 0x00, 0x0b, 0x04, +}; + +/* 4bit -> alaw decompression */ +static u8 _4bit_to_alaw[16] = { + 0x5d, 0x51, 0xd9, 0xd7, 0x5f, 0x53, 0xa3, 0x4b, + 0x2a, 0x3a, 0x22, 0x2e, 0x26, 0x56, 0x20, 0x2c, +}; + +/* ulaw -> 4bit compression */ +static u8 ulaw_to_4bit[256] = { + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, + 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, + 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, + 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x04, 0x04, + 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, + 0x04, 0x04, 0x04, 0x04, 0x05, 0x05, 0x05, 0x05, + 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, 0x05, + 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, + 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, + 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x08, + 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, + 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, + 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, + 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, + 0x0f, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, + 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, 0x0e, + 0x0e, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, + 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, + 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, + 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0b, 0x0b, + 0x0b, 0x0b, 0x0b, 0x0b, 0x0b, 0x0b, 0x0b, 0x0b, + 0x0b, 0x0b, 0x0b, 0x0b, 0x0a, 0x0a, 0x0a, 0x0a, + 0x0a, 0x0a, 0x0a, 0x0a, 0x0a, 0x0a, 0x0a, 0x0a, + 0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x09, + 0x09, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, + 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, +}; + +/* 4bit -> ulaw decompression */ +static u8 _4bit_to_ulaw[16] = { + 0x11, 0x21, 0x31, 0x40, 0x4e, 0x5c, 0x68, 0x71, + 0xfe, 0xef, 0xe7, 0xdb, 0xcd, 0xbf, 0xaf, 0x9f, +}; + + +/* + * Compresses data to the result buffer + * The result size must be at least half of the input buffer. + * The number of samples also must be even! + */ +int +l1oip_law_to_4bit(u8 *data, int len, u8 *result, u32 *state) +{ + int ii, i = 0, o = 0; + + if (!len) + return 0; + + /* send saved byte and first input byte */ + if (*state) { + *result++ = table_com[(((*state)<<8)&0xff00) | (*data++)]; + len--; + o++; + } + + ii = len >> 1; + + while (i < ii) { + *result++ = table_com[(data[0]<<8) | (data[1])]; + data += 2; + i++; + o++; + } + + /* if len has an odd number, we save byte for next call */ + if (len & 1) + *state = 0x100 + *data; + else + *state = 0; + + return o; +} + +/* Decompress data to the result buffer + * The result size must be the number of sample in packet. (2 * input data) + * The number of samples in the result are even! + */ +int +l1oip_4bit_to_law(u8 *data, int len, u8 *result) +{ + int i = 0; + u16 r; + + while (i < len) { + r = table_dec[*data++]; + *result++ = r>>8; + *result++ = r; + i++; + } + + return len << 1; +} + + +/* + * law conversion + */ +int +l1oip_alaw_to_ulaw(u8 *data, int len, u8 *result) +{ + int i = 0; + + while (i < len) { + *result++ = alaw_to_ulaw[*data++]; + i++; + } + + return len; +} + +int +l1oip_ulaw_to_alaw(u8 *data, int len, u8 *result) +{ + int i = 0; + + while (i < len) { + *result++ = ulaw_to_alaw[*data++]; + i++; + } + + return len; +} + + +/* + * generate/free compression and decompression table + */ +void +l1oip_4bit_free(void) +{ + if (table_dec) + vfree(table_dec); + if (table_com) + vfree(table_com); + table_com = NULL; + table_dec = NULL; +} + +int +l1oip_4bit_alloc(int ulaw) +{ + int i1, i2, c, sample; + + /* in case, it is called again */ + if (table_dec) + return 0; + + /* alloc conversion tables */ + table_com = vmalloc(65536); + table_dec = vmalloc(512); + if (!table_com | !table_dec) { + l1oip_4bit_free(); + return -ENOMEM; + } + memset(table_com, 0, 65536); + memset(table_dec, 0, 512); + /* generate compression table */ + i1 = 0; + while (i1 < 256) { + if (ulaw) + c = ulaw_to_4bit[i1]; + else + c = alaw_to_4bit[i1]; + i2 = 0; + while (i2 < 256) { + table_com[(i1<<8) | i2] |= (c<<4); + table_com[(i2<<8) | i1] |= c; + i2++; + } + i1++; + } + + /* generate decompression table */ + i1 = 0; + while (i1 < 16) { + if (ulaw) + sample = _4bit_to_ulaw[i1]; + else + sample = _4bit_to_alaw[i1]; + i2 = 0; + while (i2 < 16) { + table_dec[(i1<<4) | i2] |= (sample<<8); + table_dec[(i2<<4) | i1] |= sample; + i2++; + } + i1++; + } + + return 0; +} + + diff --git a/drivers/isdn/mISDN/l1oip_core.c b/drivers/isdn/mISDN/l1oip_core.c new file mode 100644 index 000000000000..155b99780c4f --- /dev/null +++ b/drivers/isdn/mISDN/l1oip_core.c @@ -0,0 +1,1518 @@ +/* + + * l1oip.c low level driver for tunneling layer 1 over IP + * + * NOTE: It is not compatible with TDMoIP nor "ISDN over IP". + * + * Author Andreas Eversberg (jolly@eversberg.eu) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + * + */ + +/* module parameters: + * type: + Value 1 = BRI + Value 2 = PRI + Value 3 = BRI (multi channel frame, not supported yet) + Value 4 = PRI (multi channel frame, not supported yet) + A multi channel frame reduces overhead to a single frame for all + b-channels, but increases delay. + (NOTE: Multi channel frames are not implemented yet.) + + * codec: + Value 0 = transparent (default) + Value 1 = transfer ALAW + Value 2 = transfer ULAW + Value 3 = transfer generic 4 bit compression. + + * ulaw: + 0 = we use a-Law (default) + 1 = we use u-Law + + * limit: + limitation of B-channels to control bandwidth (1...126) + BRI: 1 or 2 + PRI: 1-30, 31-126 (126, because dchannel ist not counted here) + Also limited ressources are used for stack, resulting in less channels. + It is possible to have more channels than 30 in PRI mode, this must + be supported by the application. + + * ip: + byte representation of remote ip address (127.0.0.1 -> 127,0,0,1) + If not given or four 0, no remote address is set. + For multiple interfaces, concat ip addresses. (127,0,0,1,127,0,0,1) + + * port: + port number (local interface) + If not given or 0, port 931 is used for fist instance, 932 for next... + For multiple interfaces, different ports must be given. + + * remoteport: + port number (remote interface) + If not given or 0, remote port equals local port + For multiple interfaces on equal sites, different ports must be given. + + * ondemand: + 0 = fixed (always transmit packets, even when remote side timed out) + 1 = on demand (only transmit packets, when remote side is detected) + the default is 0 + NOTE: ID must also be set for on demand. + + * id: + optional value to identify frames. This value must be equal on both + peers and should be random. If omitted or 0, no ID is transmitted. + + * debug: + NOTE: only one debug value must be given for all cards + enable debugging (see l1oip.h for debug options) + + +Special mISDN controls: + + op = MISDN_CTRL_SETPEER* + p1 = bytes 0-3 : remote IP address in network order (left element first) + p2 = bytes 1-2 : remote port in network order (high byte first) + optional: + p2 = bytes 3-4 : local port in network order (high byte first) + + op = MISDN_CTRL_UNSETPEER* + + * Use l1oipctrl for comfortable setting or removing ip address. + (Layer 1 Over IP CTRL) + + +L1oIP-Protocol +-------------- + +Frame Header: + + 7 6 5 4 3 2 1 0 ++---------------+ +|Ver|T|I|Coding | ++---------------+ +| ID byte 3 * | ++---------------+ +| ID byte 2 * | ++---------------+ +| ID byte 1 * | ++---------------+ +| ID byte 0 * | ++---------------+ +|M| Channel | ++---------------+ +| Length * | ++---------------+ +| Time Base MSB | ++---------------+ +| Time Base LSB | ++---------------+ +| Data.... | + +... + +| | ++---------------+ +|M| Channel | ++---------------+ +| Length * | ++---------------+ +| Time Base MSB | ++---------------+ +| Time Base LSB | ++---------------+ +| Data.... | + +... + + +* Only included in some cases. + +- Ver = Version +If version is missmatch, the frame must be ignored. + +- T = Type of interface +Must be 0 for S0 or 1 for E1. + +- I = Id present +If bit is set, four ID bytes are included in frame. + +- ID = Connection ID +Additional ID to prevent Denial of Service attacs. Also it prevents hijacking +connections with dynamic IP. The ID should be random and must not be 0. + +- Coding = Type of codec +Must be 0 for no transcoding. Also for D-channel and other HDLC frames. + 1 and 2 are reserved for explicitly use of a-LAW or u-LAW codec. + 3 is used for generic table compressor. + +- M = More channels to come. If this flag is 1, the following byte contains +the length of the channel data. After the data block, the next channel will +be defined. The flag for the last channel block (or if only one channel is +transmitted), must be 0 and no length is given. + +- Channel = Channel number +0 reserved +1-3 channel data for S0 (3 is D-channel) +1-31 channel data for E1 (16 is D-channel) +32-127 channel data for extended E1 (16 is D-channel) + +- The length is used if the M-flag is 1. It is used to find the next channel +inside frame. +NOTE: A value of 0 equals 256 bytes of data. + -> For larger data blocks, a single frame must be used. + -> For larger streams, a single frame or multiple blocks with same channel ID + must be used. + +- Time Base = Timestamp of first sample in frame +The "Time Base" is used to rearange packets and to detect packet loss. +The 16 bits are sent in network order (MSB first) and count 1/8000 th of a +second. This causes a wrap arround each 8,192 seconds. There is no requirement +for the initial "Time Base", but 0 should be used for the first packet. +In case of HDLC data, this timestamp counts the packet or byte number. + + +Two Timers: + +After initialisation, a timer of 15 seconds is started. Whenever a packet is +transmitted, the timer is reset to 15 seconds again. If the timer expires, an +empty packet is transmitted. This keep the connection alive. + +When a valid packet is received, a timer 65 seconds is started. The interface +become ACTIVE. If the timer expires, the interface becomes INACTIVE. + + +Dynamic IP handling: + +To allow dynamic IP, the ID must be non 0. In this case, any packet with the +correct port number and ID will be accepted. If the remote side changes its IP +the new IP is used for all transmitted packets until it changes again. + + +On Demand: + +If the ondemand parameter is given, the remote IP is set to 0 on timeout. +This will stop keepalive traffic to remote. If the remote is online again, +traffic will continue to the remote address. This is usefull for road warriors. +This feature only works with ID set, otherwhise it is highly unsecure. + + +Socket and Thread +----------------- + +The complete socket opening and closing is done by a thread. +When the thread opened a socket, the hc->socket descriptor is set. Whenever a +packet shall be sent to the socket, the hc->socket must be checked wheter not +NULL. To prevent change in socket descriptor, the hc->socket_lock must be used. +To change the socket, a recall of l1oip_socket_open() will safely kill the +socket process and create a new one. + +*/ + +#define L1OIP_VERSION 0 /* 0...3 */ + +#include <linux/module.h> +#include <linux/delay.h> +#include <linux/mISDNif.h> +#include <linux/mISDNhw.h> +#include <linux/mISDNdsp.h> +#include <linux/init.h> +#include <linux/in.h> +#include <linux/inet.h> +#include <linux/workqueue.h> +#include <linux/kthread.h> +#include <net/sock.h> +#include "core.h" +#include "l1oip.h" + +static const char *l1oip_revision = "2.00"; + +static int l1oip_cnt; +static spinlock_t l1oip_lock; +static struct list_head l1oip_ilist; + +#define MAX_CARDS 16 +static u_int type[MAX_CARDS]; +static u_int codec[MAX_CARDS]; +static u_int ip[MAX_CARDS*4]; +static u_int port[MAX_CARDS]; +static u_int remoteport[MAX_CARDS]; +static u_int ondemand[MAX_CARDS]; +static u_int limit[MAX_CARDS]; +static u_int id[MAX_CARDS]; +static int debug; +static int ulaw; + +MODULE_AUTHOR("Andreas Eversberg"); +MODULE_LICENSE("GPL"); +module_param_array(type, uint, NULL, S_IRUGO | S_IWUSR); +module_param_array(codec, uint, NULL, S_IRUGO | S_IWUSR); +module_param_array(ip, uint, NULL, S_IRUGO | S_IWUSR); +module_param_array(port, uint, NULL, S_IRUGO | S_IWUSR); +module_param_array(remoteport, uint, NULL, S_IRUGO | S_IWUSR); +module_param_array(ondemand, uint, NULL, S_IRUGO | S_IWUSR); +module_param_array(limit, uint, NULL, S_IRUGO | S_IWUSR); +module_param_array(id, uint, NULL, S_IRUGO | S_IWUSR); +module_param(ulaw, uint, S_IRUGO | S_IWUSR); +module_param(debug, uint, S_IRUGO | S_IWUSR); + +/* + * send a frame via socket, if open and restart timer + */ +static int +l1oip_socket_send(struct l1oip *hc, u8 localcodec, u8 channel, u32 chanmask, + u16 timebase, u8 *buf, int len) +{ + u8 *p; + int multi = 0; + u8 frame[len+32]; + struct socket *socket = NULL; + mm_segment_t oldfs; + + if (debug & DEBUG_L1OIP_MSG) + printk(KERN_DEBUG "%s: sending data to socket (len = %d)\n", + __func__, len); + + p = frame; + + /* restart timer */ + if ((int)(hc->keep_tl.expires-jiffies) < 5*HZ) { + del_timer(&hc->keep_tl); + hc->keep_tl.expires = jiffies + L1OIP_KEEPALIVE*HZ; + add_timer(&hc->keep_tl); + } else + hc->keep_tl.expires = jiffies + L1OIP_KEEPALIVE*HZ; + + if (debug & DEBUG_L1OIP_MSG) + printk(KERN_DEBUG "%s: resetting timer\n", __func__); + + /* drop if we have no remote ip or port */ + if (!hc->sin_remote.sin_addr.s_addr || !hc->sin_remote.sin_port) { + if (debug & DEBUG_L1OIP_MSG) + printk(KERN_DEBUG "%s: dropping frame, because remote " + "IP is not set.\n", __func__); + return len; + } + + /* assemble frame */ + *p++ = (L1OIP_VERSION<<6) /* version and coding */ + | (hc->pri?0x20:0x00) /* type */ + | (hc->id?0x10:0x00) /* id */ + | localcodec; + if (hc->id) { + *p++ = hc->id>>24; /* id */ + *p++ = hc->id>>16; + *p++ = hc->id>>8; + *p++ = hc->id; + } + *p++ = (multi == 1)?0x80:0x00 + channel; /* m-flag, channel */ + if (multi == 1) + *p++ = len; /* length */ + *p++ = timebase>>8; /* time base */ + *p++ = timebase; + + if (buf && len) { /* add data to frame */ + if (localcodec == 1 && ulaw) + l1oip_ulaw_to_alaw(buf, len, p); + else if (localcodec == 2 && !ulaw) + l1oip_alaw_to_ulaw(buf, len, p); + else if (localcodec == 3) + len = l1oip_law_to_4bit(buf, len, p, + &hc->chan[channel].codecstate); + else + memcpy(p, buf, len); + } + len += p - frame; + + /* check for socket in safe condition */ + spin_lock(&hc->socket_lock); + if (!hc->socket) { + spin_unlock(&hc->socket_lock); + return 0; + } + /* seize socket */ + socket = hc->socket; + hc->socket = NULL; + spin_unlock(&hc->socket_lock); + /* send packet */ + if (debug & DEBUG_L1OIP_MSG) + printk(KERN_DEBUG "%s: sending packet to socket (len " + "= %d)\n", __func__, len); + hc->sendiov.iov_base = frame; + hc->sendiov.iov_len = len; + oldfs = get_fs(); + set_fs(KERNEL_DS); + len = sock_sendmsg(socket, &hc->sendmsg, len); + set_fs(oldfs); + /* give socket back */ + hc->socket = socket; /* no locking required */ + + return len; +} + + +/* + * receive channel data from socket + */ +static void +l1oip_socket_recv(struct l1oip *hc, u8 remotecodec, u8 channel, u16 timebase, + u8 *buf, int len) +{ + struct sk_buff *nskb; + struct bchannel *bch; + struct dchannel *dch; + u8 *p; + u32 rx_counter; + + if (len == 0) { + if (debug & DEBUG_L1OIP_MSG) + printk(KERN_DEBUG "%s: received empty keepalive data, " + "ignoring\n", __func__); + return; + } + + if (debug & DEBUG_L1OIP_MSG) + printk(KERN_DEBUG "%s: received data, sending to mISDN (%d)\n", + __func__, len); + + if (channel < 1 || channel > 127) { + printk(KERN_WARNING "%s: packet error - channel %d out of " + "range\n", __func__, channel); + return; + } + dch = hc->chan[channel].dch; + bch = hc->chan[channel].bch; + if (!dch && !bch) { + printk(KERN_WARNING "%s: packet error - channel %d not in " + "stack\n", __func__, channel); + return; + } + + /* prepare message */ + nskb = mI_alloc_skb((remotecodec == 3)?(len<<1):len, GFP_ATOMIC); + if (!nskb) { + printk(KERN_ERR "%s: No mem for skb.\n", __func__); + return; + } + p = skb_put(nskb, (remotecodec == 3)?(len<<1):len); + + if (remotecodec == 1 && ulaw) + l1oip_alaw_to_ulaw(buf, len, p); + else if (remotecodec == 2 && !ulaw) + l1oip_ulaw_to_alaw(buf, len, p); + else if (remotecodec == 3) + len = l1oip_4bit_to_law(buf, len, p); + else + memcpy(p, buf, len); + + /* send message up */ + if (dch && len >= 2) { + dch->rx_skb = nskb; + recv_Dchannel(dch); + } + if (bch) { + /* expand 16 bit sequence number to 32 bit sequence number */ + rx_counter = hc->chan[channel].rx_counter; + if (((s16)(timebase - rx_counter)) >= 0) { + /* time has changed forward */ + if (timebase >= (rx_counter & 0xffff)) + rx_counter = + (rx_counter & 0xffff0000) | timebase; + else + rx_counter = ((rx_counter & 0xffff0000)+0x10000) + | timebase; + } else { + /* time has changed backwards */ + if (timebase < (rx_counter & 0xffff)) + rx_counter = + (rx_counter & 0xffff0000) | timebase; + else + rx_counter = ((rx_counter & 0xffff0000)-0x10000) + | timebase; + } + hc->chan[channel].rx_counter = rx_counter; + +#ifdef REORDER_DEBUG + if (hc->chan[channel].disorder_flag) { + struct sk_buff *skb; + int cnt; + skb = hc->chan[channel].disorder_skb; + hc->chan[channel].disorder_skb = nskb; + nskb = skb; + cnt = hc->chan[channel].disorder_cnt; + hc->chan[channel].disorder_cnt = rx_counter; + rx_counter = cnt; + } + hc->chan[channel].disorder_flag ^= 1; + if (nskb) +#endif + queue_ch_frame(&bch->ch, PH_DATA_IND, rx_counter, nskb); + } +} + + +/* + * parse frame and extract channel data + */ +static void +l1oip_socket_parse(struct l1oip *hc, struct sockaddr_in *sin, u8 *buf, int len) +{ + u32 id; + u8 channel; + u8 remotecodec; + u16 timebase; + int m, mlen; + int len_start = len; /* initial frame length */ + struct dchannel *dch = hc->chan[hc->d_idx].dch; + + if (debug & DEBUG_L1OIP_MSG) + printk(KERN_DEBUG "%s: received frame, parsing... (%d)\n", + __func__, len); + + /* check lenght */ + if (len < 1+1+2) { + printk(KERN_WARNING "%s: packet error - length %d below " + "4 bytes\n", __func__, len); + return; + } + + /* check version */ + if (((*buf)>>6) != L1OIP_VERSION) { + printk(KERN_WARNING "%s: packet error - unknown version %d\n", + __func__, buf[0]>>6); + return; + } + + /* check type */ + if (((*buf)&0x20) && !hc->pri) { + printk(KERN_WARNING "%s: packet error - received E1 packet " + "on S0 interface\n", __func__); + return; + } + if (!((*buf)&0x20) && hc->pri) { + printk(KERN_WARNING "%s: packet error - received S0 packet " + "on E1 interface\n", __func__); + return; + } + + /* get id flag */ + id = (*buf>>4)&1; + + /* check coding */ + remotecodec = (*buf) & 0x0f; + if (remotecodec > 3) { + printk(KERN_WARNING "%s: packet error - remotecodec %d " + "unsupported\n", __func__, remotecodec); + return; + } + buf++; + len--; + + /* check id */ + if (id) { + if (!hc->id) { + printk(KERN_WARNING "%s: packet error - packet has id " + "0x%x, but we have not\n", __func__, id); + return; + } + if (len < 4) { + printk(KERN_WARNING "%s: packet error - packet too " + "short for ID value\n", __func__); + return; + } + id = (*buf++) << 24; + id += (*buf++) << 16; + id += (*buf++) << 8; + id += (*buf++); + len -= 4; + + if (id != hc->id) { + printk(KERN_WARNING "%s: packet error - ID mismatch, " + "got 0x%x, we 0x%x\n", + __func__, id, hc->id); + return; + } + } else { + if (hc->id) { + printk(KERN_WARNING "%s: packet error - packet has no " + "ID, but we have\n", __func__); + return; + } + } + +multiframe: + if (len < 1) { + printk(KERN_WARNING "%s: packet error - packet too short, " + "channel expected at position %d.\n", + __func__, len-len_start+1); + return; + } + + /* get channel and multiframe flag */ + channel = *buf&0x7f; + m = *buf >> 7; + buf++; + len--; + + /* check length on multiframe */ + if (m) { + if (len < 1) { + printk(KERN_WARNING "%s: packet error - packet too " + "short, length expected at position %d.\n", + __func__, len_start-len-1); + return; + } + + mlen = *buf++; + len--; + if (mlen == 0) + mlen = 256; + if (len < mlen+3) { + printk(KERN_WARNING "%s: packet error - length %d at " + "position %d exceeds total length %d.\n", + __func__, mlen, len_start-len-1, len_start); + return; + } + if (len == mlen+3) { + printk(KERN_WARNING "%s: packet error - length %d at " + "position %d will not allow additional " + "packet.\n", + __func__, mlen, len_start-len+1); + return; + } + } else + mlen = len-2; /* single frame, substract timebase */ + + if (len < 2) { + printk(KERN_WARNING "%s: packet error - packet too short, time " + "base expected at position %d.\n", + __func__, len-len_start+1); + return; + } + + /* get time base */ + timebase = (*buf++) << 8; + timebase |= (*buf++); + len -= 2; + + /* if inactive, we send up a PH_ACTIVATE and activate */ + if (!test_bit(FLG_ACTIVE, &dch->Flags)) { + if (debug & (DEBUG_L1OIP_MSG|DEBUG_L1OIP_SOCKET)) + printk(KERN_DEBUG "%s: interface become active due to " + "received packet\n", __func__); + test_and_set_bit(FLG_ACTIVE, &dch->Flags); + _queue_data(&dch->dev.D, PH_ACTIVATE_IND, MISDN_ID_ANY, 0, + NULL, GFP_ATOMIC); + } + + /* distribute packet */ + l1oip_socket_recv(hc, remotecodec, channel, timebase, buf, mlen); + buf += mlen; + len -= mlen; + + /* multiframe */ + if (m) + goto multiframe; + + /* restart timer */ + if ((int)(hc->timeout_tl.expires-jiffies) < 5*HZ || !hc->timeout_on) { + hc->timeout_on = 1; + del_timer(&hc->timeout_tl); + hc->timeout_tl.expires = jiffies + L1OIP_TIMEOUT*HZ; + add_timer(&hc->timeout_tl); + } else /* only adjust timer */ + hc->timeout_tl.expires = jiffies + L1OIP_TIMEOUT*HZ; + + /* if ip or source port changes */ + if ((hc->sin_remote.sin_addr.s_addr != sin->sin_addr.s_addr) + || (hc->sin_remote.sin_port != sin->sin_port)) { + if (debug & DEBUG_L1OIP_SOCKET) + printk(KERN_DEBUG "%s: remote address changes from " + "0x%08x to 0x%08x (port %d to %d)\n", __func__, + ntohl(hc->sin_remote.sin_addr.s_addr), + ntohl(sin->sin_addr.s_addr), + ntohs(hc->sin_remote.sin_port), + ntohs(sin->sin_port)); + hc->sin_remote.sin_addr.s_addr = sin->sin_addr.s_addr; + hc->sin_remote.sin_port = sin->sin_port; + } +} + + +/* + * socket stuff + */ +static int +l1oip_socket_thread(void *data) +{ + struct l1oip *hc = (struct l1oip *)data; + int ret = 0; + struct msghdr msg; + struct iovec iov; + mm_segment_t oldfs; + struct sockaddr_in sin_rx; + unsigned char recvbuf[1500]; + int recvlen; + struct socket *socket = NULL; + DECLARE_COMPLETION(wait); + + /* make daemon */ + allow_signal(SIGTERM); + + /* create socket */ + if (sock_create(PF_INET, SOCK_DGRAM, IPPROTO_UDP, &socket)) { + printk(KERN_ERR "%s: Failed to create socket.\n", __func__); + return -EIO; + } + + /* set incoming address */ + hc->sin_local.sin_family = AF_INET; + hc->sin_local.sin_addr.s_addr = INADDR_ANY; + hc->sin_local.sin_port = htons((unsigned short)hc->localport); + + /* set outgoing address */ + hc->sin_remote.sin_family = AF_INET; + hc->sin_remote.sin_addr.s_addr = htonl(hc->remoteip); + hc->sin_remote.sin_port = htons((unsigned short)hc->remoteport); + + /* bind to incomming port */ + if (socket->ops->bind(socket, (struct sockaddr *)&hc->sin_local, + sizeof(hc->sin_local))) { + printk(KERN_ERR "%s: Failed to bind socket to port %d.\n", + __func__, hc->localport); + ret = -EINVAL; + goto fail; + } + + /* check sk */ + if (socket->sk == NULL) { + printk(KERN_ERR "%s: socket->sk == NULL\n", __func__); + ret = -EIO; + goto fail; + } + + /* build receive message */ + msg.msg_name = &sin_rx; + msg.msg_namelen = sizeof(sin_rx); + msg.msg_control = NULL; + msg.msg_controllen = 0; + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + + /* build send message */ + hc->sendmsg.msg_name = &hc->sin_remote; + hc->sendmsg.msg_namelen = sizeof(hc->sin_remote); + hc->sendmsg.msg_control = NULL; + hc->sendmsg.msg_controllen = 0; + hc->sendmsg.msg_iov = &hc->sendiov; + hc->sendmsg.msg_iovlen = 1; + + /* give away socket */ + spin_lock(&hc->socket_lock); + hc->socket = socket; + spin_unlock(&hc->socket_lock); + + /* read loop */ + if (debug & DEBUG_L1OIP_SOCKET) + printk(KERN_DEBUG "%s: socket created and open\n", + __func__); + while (!signal_pending(current)) { + iov.iov_base = recvbuf; + iov.iov_len = sizeof(recvbuf); + oldfs = get_fs(); + set_fs(KERNEL_DS); + recvlen = sock_recvmsg(socket, &msg, sizeof(recvbuf), 0); + set_fs(oldfs); + if (recvlen > 0) { + l1oip_socket_parse(hc, &sin_rx, recvbuf, recvlen); + } else { + if (debug & DEBUG_L1OIP_SOCKET) + printk(KERN_WARNING "%s: broken pipe on socket\n", + __func__); + } + } + + /* get socket back, check first if in use, maybe by send function */ + spin_lock(&hc->socket_lock); + /* if hc->socket is NULL, it is in use until it is given back */ + while (!hc->socket) { + spin_unlock(&hc->socket_lock); + schedule_timeout(HZ/10); + spin_lock(&hc->socket_lock); + } + hc->socket = NULL; + spin_unlock(&hc->socket_lock); + + if (debug & DEBUG_L1OIP_SOCKET) + printk(KERN_DEBUG "%s: socket thread terminating\n", + __func__); + +fail: + /* close socket */ + if (socket) + sock_release(socket); + + /* if we got killed, signal completion */ + complete(&hc->socket_complete); + hc->socket_thread = NULL; /* show termination of thread */ + + if (debug & DEBUG_L1OIP_SOCKET) + printk(KERN_DEBUG "%s: socket thread terminated\n", + __func__); + return ret; +} + +static void +l1oip_socket_close(struct l1oip *hc) +{ + /* kill thread */ + if (hc->socket_thread) { + if (debug & DEBUG_L1OIP_SOCKET) + printk(KERN_DEBUG "%s: socket thread exists, " + "killing...\n", __func__); + send_sig(SIGTERM, hc->socket_thread, 0); + wait_for_completion(&hc->socket_complete); + } +} + +static int +l1oip_socket_open(struct l1oip *hc) +{ + /* in case of reopen, we need to close first */ + l1oip_socket_close(hc); + + init_completion(&hc->socket_complete); + + /* create receive process */ + hc->socket_thread = kthread_run(l1oip_socket_thread, hc, "l1oip_%s", + hc->name); + if (IS_ERR(hc->socket_thread)) { + int err = PTR_ERR(hc->socket_thread); + printk(KERN_ERR "%s: Failed (%d) to create socket process.\n", + __func__, err); + hc->socket_thread = NULL; + sock_release(hc->socket); + return err; + } + if (debug & DEBUG_L1OIP_SOCKET) + printk(KERN_DEBUG "%s: socket thread created\n", __func__); + + return 0; +} + + +static void +l1oip_send_bh(struct work_struct *work) +{ + struct l1oip *hc = container_of(work, struct l1oip, workq); + + if (debug & (DEBUG_L1OIP_MSG|DEBUG_L1OIP_SOCKET)) + printk(KERN_DEBUG "%s: keepalive timer expired, sending empty " + "frame on dchannel\n", __func__); + + /* send an empty l1oip frame at D-channel */ + l1oip_socket_send(hc, 0, hc->d_idx, 0, 0, NULL, 0); +} + + +/* + * timer stuff + */ +static void +l1oip_keepalive(void *data) +{ + struct l1oip *hc = (struct l1oip *)data; + + schedule_work(&hc->workq); +} + +static void +l1oip_timeout(void *data) +{ + struct l1oip *hc = (struct l1oip *)data; + struct dchannel *dch = hc->chan[hc->d_idx].dch; + + if (debug & DEBUG_L1OIP_MSG) + printk(KERN_DEBUG "%s: timeout timer expired, turn layer one " + "down.\n", __func__); + + hc->timeout_on = 0; /* state that timer must be initialized next time */ + + /* if timeout, we send up a PH_DEACTIVATE and deactivate */ + if (test_bit(FLG_ACTIVE, &dch->Flags)) { + if (debug & (DEBUG_L1OIP_MSG|DEBUG_L1OIP_SOCKET)) + printk(KERN_DEBUG "%s: interface become deactivated " + "due to timeout\n", __func__); + test_and_clear_bit(FLG_ACTIVE, &dch->Flags); + _queue_data(&dch->dev.D, PH_DEACTIVATE_IND, MISDN_ID_ANY, 0, + NULL, GFP_ATOMIC); + } + + /* if we have ondemand set, we remove ip address */ + if (hc->ondemand) { + if (debug & DEBUG_L1OIP_MSG) + printk(KERN_DEBUG "%s: on demand causes ip address to " + "be removed\n", __func__); + hc->sin_remote.sin_addr.s_addr = 0; + } +} + + +/* + * message handling + */ +static int +handle_dmsg(struct mISDNchannel *ch, struct sk_buff *skb) +{ + struct mISDNdevice *dev = container_of(ch, struct mISDNdevice, D); + struct dchannel *dch = container_of(dev, struct dchannel, dev); + struct l1oip *hc = dch->hw; + struct mISDNhead *hh = mISDN_HEAD_P(skb); + int ret = -EINVAL; + int l, ll; + unsigned char *p; + + switch (hh->prim) { + case PH_DATA_REQ: + if (skb->len < 1) { + printk(KERN_WARNING "%s: skb too small\n", + __func__); + break; + } + if (skb->len > MAX_DFRAME_LEN_L1 || skb->len > L1OIP_MAX_LEN) { + printk(KERN_WARNING "%s: skb too large\n", + __func__); + break; + } + /* send frame */ + p = skb->data; + l = skb->len; + while (l) { + ll = (l < L1OIP_MAX_PERFRAME)?l:L1OIP_MAX_PERFRAME; + l1oip_socket_send(hc, 0, dch->slot, 0, + hc->chan[dch->slot].tx_counter++, p, ll); + p += ll; + l -= ll; + } + skb_trim(skb, 0); + queue_ch_frame(ch, PH_DATA_CNF, hh->id, skb); + return 0; + case PH_ACTIVATE_REQ: + if (debug & (DEBUG_L1OIP_MSG|DEBUG_L1OIP_SOCKET)) + printk(KERN_DEBUG "%s: PH_ACTIVATE channel %d (1..%d)\n" + , __func__, dch->slot, hc->b_num+1); + skb_trim(skb, 0); + if (test_bit(FLG_ACTIVE, &dch->Flags)) + queue_ch_frame(ch, PH_ACTIVATE_IND, hh->id, skb); + else + queue_ch_frame(ch, PH_DEACTIVATE_IND, hh->id, skb); + return 0; + case PH_DEACTIVATE_REQ: + if (debug & (DEBUG_L1OIP_MSG|DEBUG_L1OIP_SOCKET)) + printk(KERN_DEBUG "%s: PH_DEACTIVATE channel %d " + "(1..%d)\n", __func__, dch->slot, + hc->b_num+1); + skb_trim(skb, 0); + if (test_bit(FLG_ACTIVE, &dch->Flags)) + queue_ch_frame(ch, PH_ACTIVATE_IND, hh->id, skb); + else + queue_ch_frame(ch, PH_DEACTIVATE_IND, hh->id, skb); + return 0; + } + if (!ret) + dev_kfree_skb(skb); + return ret; +} + +static int +channel_dctrl(struct dchannel *dch, struct mISDN_ctrl_req *cq) +{ + int ret = 0; + struct l1oip *hc = dch->hw; + + switch (cq->op) { + case MISDN_CTRL_GETOP: + cq->op = MISDN_CTRL_SETPEER | MISDN_CTRL_UNSETPEER; + break; + case MISDN_CTRL_SETPEER: + hc->remoteip = (u32)cq->p1; + hc->remoteport = cq->p2 & 0xffff; + hc->localport = cq->p2 >> 16; + if (!hc->remoteport) + hc->remoteport = hc->localport; + if (debug & DEBUG_L1OIP_SOCKET) + printk(KERN_DEBUG "%s: got new ip address from user " + "space.\n", __func__); + l1oip_socket_open(hc); + break; + case MISDN_CTRL_UNSETPEER: + if (debug & DEBUG_L1OIP_SOCKET) + printk(KERN_DEBUG "%s: removing ip address.\n", + __func__); + hc->remoteip = 0; + l1oip_socket_open(hc); + break; + default: + printk(KERN_WARNING "%s: unknown Op %x\n", + __func__, cq->op); + ret = -EINVAL; + break; + } + return ret; +} + +static int +open_dchannel(struct l1oip *hc, struct dchannel *dch, struct channel_req *rq) +{ + if (debug & DEBUG_HW_OPEN) + printk(KERN_DEBUG "%s: dev(%d) open from %p\n", __func__, + dch->dev.id, __builtin_return_address(0)); + if (rq->protocol == ISDN_P_NONE) + return -EINVAL; + if ((dch->dev.D.protocol != ISDN_P_NONE) && + (dch->dev.D.protocol != rq->protocol)) { + if (debug & DEBUG_HW_OPEN) + printk(KERN_WARNING "%s: change protocol %x to %x\n", + __func__, dch->dev.D.protocol, rq->protocol); + } + if (dch->dev.D.protocol != rq->protocol) + dch->dev.D.protocol = rq->protocol; + + if (test_bit(FLG_ACTIVE, &dch->Flags)) { + _queue_data(&dch->dev.D, PH_ACTIVATE_IND, MISDN_ID_ANY, + 0, NULL, GFP_KERNEL); + } + rq->ch = &dch->dev.D; + if (!try_module_get(THIS_MODULE)) + printk(KERN_WARNING "%s:cannot get module\n", __func__); + return 0; +} + +static int +open_bchannel(struct l1oip *hc, struct dchannel *dch, struct channel_req *rq) +{ + struct bchannel *bch; + int ch; + + if (!test_bit(rq->adr.channel & 0x1f, + &dch->dev.channelmap[rq->adr.channel >> 5])) + return -EINVAL; + if (rq->protocol == ISDN_P_NONE) + return -EINVAL; + ch = rq->adr.channel; /* BRI: 1=B1 2=B2 PRI: 1..15,17.. */ + bch = hc->chan[ch].bch; + if (!bch) { + printk(KERN_ERR "%s:internal error ch %d has no bch\n", + __func__, ch); + return -EINVAL; + } + if (test_and_set_bit(FLG_OPEN, &bch->Flags)) + return -EBUSY; /* b-channel can be only open once */ + bch->ch.protocol = rq->protocol; + rq->ch = &bch->ch; + if (!try_module_get(THIS_MODULE)) + printk(KERN_WARNING "%s:cannot get module\n", __func__); + return 0; +} + +static int +l1oip_dctrl(struct mISDNchannel *ch, u_int cmd, void *arg) +{ + struct mISDNdevice *dev = container_of(ch, struct mISDNdevice, D); + struct dchannel *dch = container_of(dev, struct dchannel, dev); + struct l1oip *hc = dch->hw; + struct channel_req *rq; + int err = 0; + + if (dch->debug & DEBUG_HW) + printk(KERN_DEBUG "%s: cmd:%x %p\n", + __func__, cmd, arg); + switch (cmd) { + case OPEN_CHANNEL: + rq = arg; + switch (rq->protocol) { + case ISDN_P_TE_S0: + case ISDN_P_NT_S0: + if (hc->pri) { + err = -EINVAL; + break; + } + err = open_dchannel(hc, dch, rq); + break; + case ISDN_P_TE_E1: + case ISDN_P_NT_E1: + if (!hc->pri) { + err = -EINVAL; + break; + } + err = open_dchannel(hc, dch, rq); + break; + default: + err = open_bchannel(hc, dch, rq); + } + break; + case CLOSE_CHANNEL: + if (debug & DEBUG_HW_OPEN) + printk(KERN_DEBUG "%s: dev(%d) close from %p\n", + __func__, dch->dev.id, + __builtin_return_address(0)); + module_put(THIS_MODULE); + break; + case CONTROL_CHANNEL: + err = channel_dctrl(dch, arg); + break; + default: + if (dch->debug & DEBUG_HW) + printk(KERN_DEBUG "%s: unknown command %x\n", + __func__, cmd); + err = -EINVAL; + } + return err; +} + +static int +handle_bmsg(struct mISDNchannel *ch, struct sk_buff *skb) +{ + struct bchannel *bch = container_of(ch, struct bchannel, ch); + struct l1oip *hc = bch->hw; + int ret = -EINVAL; + struct mISDNhead *hh = mISDN_HEAD_P(skb); + int l, ll, i; + unsigned char *p; + + switch (hh->prim) { + case PH_DATA_REQ: + if (skb->len <= 0) { + printk(KERN_WARNING "%s: skb too small\n", + __func__); + break; + } + if (skb->len > MAX_DFRAME_LEN_L1 || skb->len > L1OIP_MAX_LEN) { + printk(KERN_WARNING "%s: skb too large\n", + __func__); + break; + } + /* check for AIS / ulaw-silence */ + p = skb->data; + l = skb->len; + for (i = 0; i < l; i++) { + if (*p++ != 0xff) + break; + } + if (i == l) { + if (debug & DEBUG_L1OIP_MSG) + printk(KERN_DEBUG "%s: got AIS, not sending, " + "but counting\n", __func__); + hc->chan[bch->slot].tx_counter += l; + skb_trim(skb, 0); + queue_ch_frame(ch, PH_DATA_CNF, hh->id, skb); + return 0; + } + /* check for silence */ + p = skb->data; + l = skb->len; + for (i = 0; i < l; i++) { + if (*p++ != 0x2a) + break; + } + if (i == l) { + if (debug & DEBUG_L1OIP_MSG) + printk(KERN_DEBUG "%s: got silence, not sending" + ", but counting\n", __func__); + hc->chan[bch->slot].tx_counter += l; + skb_trim(skb, 0); + queue_ch_frame(ch, PH_DATA_CNF, hh->id, skb); + return 0; + } + + /* send frame */ + p = skb->data; + l = skb->len; + while (l) { + ll = (l < L1OIP_MAX_PERFRAME)?l:L1OIP_MAX_PERFRAME; + l1oip_socket_send(hc, hc->codec, bch->slot, 0, + hc->chan[bch->slot].tx_counter, p, ll); + hc->chan[bch->slot].tx_counter += ll; + p += ll; + l -= ll; + } + skb_trim(skb, 0); + queue_ch_frame(ch, PH_DATA_CNF, hh->id, skb); + return 0; + case PH_ACTIVATE_REQ: + if (debug & (DEBUG_L1OIP_MSG|DEBUG_L1OIP_SOCKET)) + printk(KERN_DEBUG "%s: PH_ACTIVATE channel %d (1..%d)\n" + , __func__, bch->slot, hc->b_num+1); + hc->chan[bch->slot].codecstate = 0; + test_and_set_bit(FLG_ACTIVE, &bch->Flags); + skb_trim(skb, 0); + queue_ch_frame(ch, PH_ACTIVATE_IND, hh->id, skb); + return 0; + case PH_DEACTIVATE_REQ: + if (debug & (DEBUG_L1OIP_MSG|DEBUG_L1OIP_SOCKET)) + printk(KERN_DEBUG "%s: PH_DEACTIVATE channel %d " + "(1..%d)\n", __func__, bch->slot, + hc->b_num+1); + test_and_clear_bit(FLG_ACTIVE, &bch->Flags); + skb_trim(skb, 0); + queue_ch_frame(ch, PH_DEACTIVATE_IND, hh->id, skb); + return 0; + } + if (!ret) + dev_kfree_skb(skb); + return ret; +} + +static int +channel_bctrl(struct bchannel *bch, struct mISDN_ctrl_req *cq) +{ + int ret = 0; + struct dsp_features *features = + (struct dsp_features *)(*((u_long *)&cq->p1)); + + switch (cq->op) { + case MISDN_CTRL_GETOP: + cq->op = MISDN_CTRL_HW_FEATURES_OP; + break; + case MISDN_CTRL_HW_FEATURES: /* fill features structure */ + if (debug & DEBUG_L1OIP_MSG) + printk(KERN_DEBUG "%s: HW_FEATURE request\n", + __func__); + /* create confirm */ + features->unclocked = 1; + features->unordered = 1; + break; + default: + printk(KERN_WARNING "%s: unknown Op %x\n", + __func__, cq->op); + ret = -EINVAL; + break; + } + return ret; +} + +static int +l1oip_bctrl(struct mISDNchannel *ch, u_int cmd, void *arg) +{ + struct bchannel *bch = container_of(ch, struct bchannel, ch); + int err = -EINVAL; + + if (bch->debug & DEBUG_HW) + printk(KERN_DEBUG "%s: cmd:%x %p\n", + __func__, cmd, arg); + switch (cmd) { + case CLOSE_CHANNEL: + test_and_clear_bit(FLG_OPEN, &bch->Flags); + test_and_clear_bit(FLG_ACTIVE, &bch->Flags); + ch->protocol = ISDN_P_NONE; + ch->peer = NULL; + module_put(THIS_MODULE); + err = 0; + break; + case CONTROL_CHANNEL: + err = channel_bctrl(bch, arg); + break; + default: + printk(KERN_WARNING "%s: unknown prim(%x)\n", + __func__, cmd); + } + return err; +} + + +/* + * cleanup module and stack + */ +static void +release_card(struct l1oip *hc) +{ + int ch; + + if (timer_pending(&hc->keep_tl)) + del_timer(&hc->keep_tl); + + if (timer_pending(&hc->timeout_tl)) + del_timer(&hc->timeout_tl); + + if (hc->socket_thread) + l1oip_socket_close(hc); + + if (hc->registered && hc->chan[hc->d_idx].dch) + mISDN_unregister_device(&hc->chan[hc->d_idx].dch->dev); + for (ch = 0; ch < 128; ch++) { + if (hc->chan[ch].dch) { + mISDN_freedchannel(hc->chan[ch].dch); + kfree(hc->chan[ch].dch); + } + if (hc->chan[ch].bch) { + mISDN_freebchannel(hc->chan[ch].bch); + kfree(hc->chan[ch].bch); +#ifdef REORDER_DEBUG + if (hc->chan[ch].disorder_skb) + dev_kfree_skb(hc->chan[ch].disorder_skb); +#endif + } + } + + spin_lock(&l1oip_lock); + list_del(&hc->list); + spin_unlock(&l1oip_lock); + + kfree(hc); +} + +static void +l1oip_cleanup(void) +{ + struct l1oip *hc, *next; + + list_for_each_entry_safe(hc, next, &l1oip_ilist, list) + release_card(hc); + + l1oip_4bit_free(); +} + + +/* + * module and stack init + */ +static int +init_card(struct l1oip *hc, int pri, int bundle) +{ + struct dchannel *dch; + struct bchannel *bch; + int ret; + int i, ch; + + spin_lock_init(&hc->socket_lock); + hc->idx = l1oip_cnt; + hc->pri = pri; + hc->d_idx = pri?16:3; + hc->b_num = pri?30:2; + hc->bundle = bundle; + if (hc->pri) + sprintf(hc->name, "l1oip-e1.%d", l1oip_cnt + 1); + else + sprintf(hc->name, "l1oip-s0.%d", l1oip_cnt + 1); + + switch (codec[l1oip_cnt]) { + case 0: /* as is */ + case 1: /* alaw */ + case 2: /* ulaw */ + case 3: /* 4bit */ + break; + default: + printk(KERN_ERR "Codec(%d) not supported.\n", + codec[l1oip_cnt]); + return -EINVAL; + } + hc->codec = codec[l1oip_cnt]; + if (debug & DEBUG_L1OIP_INIT) + printk(KERN_DEBUG "%s: using codec %d\n", + __func__, hc->codec); + + if (id[l1oip_cnt] == 0) { + printk(KERN_WARNING "Warning: No 'id' value given or " + "0, this is highly unsecure. Please use 32 " + "bit randmom number 0x...\n"); + } + hc->id = id[l1oip_cnt]; + if (debug & DEBUG_L1OIP_INIT) + printk(KERN_DEBUG "%s: using id 0x%x\n", __func__, hc->id); + + hc->ondemand = ondemand[l1oip_cnt]; + if (hc->ondemand && !hc->id) { + printk(KERN_ERR "%s: ondemand option only allowed in " + "conjunction with non 0 ID\n", __func__); + return -EINVAL; + } + + if (limit[l1oip_cnt]) + hc->b_num = limit[l1oip_cnt]; + if (!pri && hc->b_num > 2) { + printk(KERN_ERR "Maximum limit for BRI interface is 2 " + "channels.\n"); + return -EINVAL; + } + if (pri && hc->b_num > 126) { + printk(KERN_ERR "Maximum limit for PRI interface is 126 " + "channels.\n"); + return -EINVAL; + } + if (pri && hc->b_num > 30) { + printk(KERN_WARNING "Maximum limit for BRI interface is 30 " + "channels.\n"); + printk(KERN_WARNING "Your selection of %d channels must be " + "supported by application.\n", hc->limit); + } + + hc->remoteip = ip[l1oip_cnt<<2] << 24 + | ip[(l1oip_cnt<<2)+1] << 16 + | ip[(l1oip_cnt<<2)+2] << 8 + | ip[(l1oip_cnt<<2)+3]; + hc->localport = port[l1oip_cnt]?:(L1OIP_DEFAULTPORT+l1oip_cnt); + if (remoteport[l1oip_cnt]) + hc->remoteport = remoteport[l1oip_cnt]; + else + hc->remoteport = hc->localport; + if (debug & DEBUG_L1OIP_INIT) + printk(KERN_DEBUG "%s: using local port %d remote ip " + "%d.%d.%d.%d port %d ondemand %d\n", __func__, + hc->localport, hc->remoteip >> 24, + (hc->remoteip >> 16) & 0xff, + (hc->remoteip >> 8) & 0xff, hc->remoteip & 0xff, + hc->remoteport, hc->ondemand); + + dch = kzalloc(sizeof(struct dchannel), GFP_KERNEL); + if (!dch) + return -ENOMEM; + dch->debug = debug; + mISDN_initdchannel(dch, MAX_DFRAME_LEN_L1, NULL); + dch->hw = hc; + if (pri) + dch->dev.Dprotocols = (1 << ISDN_P_TE_E1) | (1 << ISDN_P_NT_E1); + else + dch->dev.Dprotocols = (1 << ISDN_P_TE_S0) | (1 << ISDN_P_NT_S0); + dch->dev.Bprotocols = (1 << (ISDN_P_B_RAW & ISDN_P_B_MASK)) | + (1 << (ISDN_P_B_HDLC & ISDN_P_B_MASK)); + dch->dev.D.send = handle_dmsg; + dch->dev.D.ctrl = l1oip_dctrl; + dch->dev.nrbchan = hc->b_num; + dch->slot = hc->d_idx; + hc->chan[hc->d_idx].dch = dch; + i = 1; + for (ch = 0; ch < dch->dev.nrbchan; ch++) { + if (ch == 15) + i++; + bch = kzalloc(sizeof(struct bchannel), GFP_KERNEL); + if (!bch) { + printk(KERN_ERR "%s: no memory for bchannel\n", + __func__); + return -ENOMEM; + } + bch->nr = i + ch; + bch->slot = i + ch; + bch->debug = debug; + mISDN_initbchannel(bch, MAX_DATA_MEM); + bch->hw = hc; + bch->ch.send = handle_bmsg; + bch->ch.ctrl = l1oip_bctrl; + bch->ch.nr = i + ch; + list_add(&bch->ch.list, &dch->dev.bchannels); + hc->chan[i + ch].bch = bch; + test_and_set_bit(bch->nr & 0x1f, + &dch->dev.channelmap[bch->nr >> 5]); + } + ret = mISDN_register_device(&dch->dev, hc->name); + if (ret) + return ret; + hc->registered = 1; + + if (debug & DEBUG_L1OIP_INIT) + printk(KERN_DEBUG "%s: Setting up network card(%d)\n", + __func__, l1oip_cnt + 1); + ret = l1oip_socket_open(hc); + if (ret) + return ret; + + hc->keep_tl.function = (void *)l1oip_keepalive; + hc->keep_tl.data = (ulong)hc; + init_timer(&hc->keep_tl); + hc->keep_tl.expires = jiffies + 2*HZ; /* two seconds first time */ + add_timer(&hc->keep_tl); + + hc->timeout_tl.function = (void *)l1oip_timeout; + hc->timeout_tl.data = (ulong)hc; + init_timer(&hc->timeout_tl); + hc->timeout_on = 0; /* state that we have timer off */ + + return 0; +} + +static int __init +l1oip_init(void) +{ + int pri, bundle; + struct l1oip *hc; + int ret; + + printk(KERN_INFO "mISDN: Layer-1-over-IP driver Rev. %s\n", + l1oip_revision); + + INIT_LIST_HEAD(&l1oip_ilist); + spin_lock_init(&l1oip_lock); + + if (l1oip_4bit_alloc(ulaw)) + return -ENOMEM; + + l1oip_cnt = 0; + while (type[l1oip_cnt] && l1oip_cnt < MAX_CARDS) { + switch (type[l1oip_cnt] & 0xff) { + case 1: + pri = 0; + bundle = 0; + break; + case 2: + pri = 1; + bundle = 0; + break; + case 3: + pri = 0; + bundle = 1; + break; + case 4: + pri = 1; + bundle = 1; + break; + default: + printk(KERN_ERR "Card type(%d) not supported.\n", + type[l1oip_cnt] & 0xff); + l1oip_cleanup(); + return -EINVAL; + } + + if (debug & DEBUG_L1OIP_INIT) + printk(KERN_DEBUG "%s: interface %d is %s with %s.\n", + __func__, l1oip_cnt, pri?"PRI":"BRI", + bundle?"bundled IP packet for all B-channels" + :"seperate IP packets for every B-channel"); + + hc = kzalloc(sizeof(struct l1oip), GFP_ATOMIC); + if (!hc) { + printk(KERN_ERR "No kmem for L1-over-IP driver.\n"); + l1oip_cleanup(); + return -ENOMEM; + } + INIT_WORK(&hc->workq, (void *)l1oip_send_bh); + + spin_lock(&l1oip_lock); + list_add_tail(&hc->list, &l1oip_ilist); + spin_unlock(&l1oip_lock); + + ret = init_card(hc, pri, bundle); + if (ret) { + l1oip_cleanup(); + return ret; + } + + l1oip_cnt++; + } + printk(KERN_INFO "%d virtual devices registered\n", l1oip_cnt); + return 0; +} + +module_init(l1oip_init); +module_exit(l1oip_cleanup); + diff --git a/drivers/isdn/mISDN/layer1.c b/drivers/isdn/mISDN/layer1.c new file mode 100644 index 000000000000..fced1a2755f8 --- /dev/null +++ b/drivers/isdn/mISDN/layer1.c @@ -0,0 +1,403 @@ +/* + * + * Author Karsten Keil <kkeil@novell.com> + * + * Copyright 2008 by Karsten Keil <kkeil@novell.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + + +#include <linux/module.h> +#include <linux/mISDNhw.h> +#include "layer1.h" +#include "fsm.h" + +static int *debug; + +struct layer1 { + u_long Flags; + struct FsmInst l1m; + struct FsmTimer timer; + int delay; + struct dchannel *dch; + dchannel_l1callback *dcb; +}; + +#define TIMER3_VALUE 7000 + +static +struct Fsm l1fsm_s = {NULL, 0, 0, NULL, NULL}; + +enum { + ST_L1_F2, + ST_L1_F3, + ST_L1_F4, + ST_L1_F5, + ST_L1_F6, + ST_L1_F7, + ST_L1_F8, +}; + +#define L1S_STATE_COUNT (ST_L1_F8+1) + +static char *strL1SState[] = +{ + "ST_L1_F2", + "ST_L1_F3", + "ST_L1_F4", + "ST_L1_F5", + "ST_L1_F6", + "ST_L1_F7", + "ST_L1_F8", +}; + +enum { + EV_PH_ACTIVATE, + EV_PH_DEACTIVATE, + EV_RESET_IND, + EV_DEACT_CNF, + EV_DEACT_IND, + EV_POWER_UP, + EV_ANYSIG_IND, + EV_INFO2_IND, + EV_INFO4_IND, + EV_TIMER_DEACT, + EV_TIMER_ACT, + EV_TIMER3, +}; + +#define L1_EVENT_COUNT (EV_TIMER3 + 1) + +static char *strL1Event[] = +{ + "EV_PH_ACTIVATE", + "EV_PH_DEACTIVATE", + "EV_RESET_IND", + "EV_DEACT_CNF", + "EV_DEACT_IND", + "EV_POWER_UP", + "EV_ANYSIG_IND", + "EV_INFO2_IND", + "EV_INFO4_IND", + "EV_TIMER_DEACT", + "EV_TIMER_ACT", + "EV_TIMER3", +}; + +static void +l1m_debug(struct FsmInst *fi, char *fmt, ...) +{ + struct layer1 *l1 = fi->userdata; + va_list va; + + va_start(va, fmt); + printk(KERN_DEBUG "%s: ", l1->dch->dev.name); + vprintk(fmt, va); + printk("\n"); + va_end(va); +} + +static void +l1_reset(struct FsmInst *fi, int event, void *arg) +{ + mISDN_FsmChangeState(fi, ST_L1_F3); +} + +static void +l1_deact_cnf(struct FsmInst *fi, int event, void *arg) +{ + struct layer1 *l1 = fi->userdata; + + mISDN_FsmChangeState(fi, ST_L1_F3); + if (test_bit(FLG_L1_ACTIVATING, &l1->Flags)) + l1->dcb(l1->dch, HW_POWERUP_REQ); +} + +static void +l1_deact_req_s(struct FsmInst *fi, int event, void *arg) +{ + struct layer1 *l1 = fi->userdata; + + mISDN_FsmChangeState(fi, ST_L1_F3); + mISDN_FsmRestartTimer(&l1->timer, 550, EV_TIMER_DEACT, NULL, 2); + test_and_set_bit(FLG_L1_DEACTTIMER, &l1->Flags); +} + +static void +l1_power_up_s(struct FsmInst *fi, int event, void *arg) +{ + struct layer1 *l1 = fi->userdata; + + if (test_bit(FLG_L1_ACTIVATING, &l1->Flags)) { + mISDN_FsmChangeState(fi, ST_L1_F4); + l1->dcb(l1->dch, INFO3_P8); + } else + mISDN_FsmChangeState(fi, ST_L1_F3); +} + +static void +l1_go_F5(struct FsmInst *fi, int event, void *arg) +{ + mISDN_FsmChangeState(fi, ST_L1_F5); +} + +static void +l1_go_F8(struct FsmInst *fi, int event, void *arg) +{ + mISDN_FsmChangeState(fi, ST_L1_F8); +} + +static void +l1_info2_ind(struct FsmInst *fi, int event, void *arg) +{ + struct layer1 *l1 = fi->userdata; + + mISDN_FsmChangeState(fi, ST_L1_F6); + l1->dcb(l1->dch, INFO3_P8); +} + +static void +l1_info4_ind(struct FsmInst *fi, int event, void *arg) +{ + struct layer1 *l1 = fi->userdata; + + mISDN_FsmChangeState(fi, ST_L1_F7); + l1->dcb(l1->dch, INFO3_P8); + if (test_and_clear_bit(FLG_L1_DEACTTIMER, &l1->Flags)) + mISDN_FsmDelTimer(&l1->timer, 4); + if (!test_bit(FLG_L1_ACTIVATED, &l1->Flags)) { + if (test_and_clear_bit(FLG_L1_T3RUN, &l1->Flags)) + mISDN_FsmDelTimer(&l1->timer, 3); + mISDN_FsmRestartTimer(&l1->timer, 110, EV_TIMER_ACT, NULL, 2); + test_and_set_bit(FLG_L1_ACTTIMER, &l1->Flags); + } +} + +static void +l1_timer3(struct FsmInst *fi, int event, void *arg) +{ + struct layer1 *l1 = fi->userdata; + + test_and_clear_bit(FLG_L1_T3RUN, &l1->Flags); + if (test_and_clear_bit(FLG_L1_ACTIVATING, &l1->Flags)) { + if (test_and_clear_bit(FLG_L1_DBLOCKED, &l1->Flags)) + l1->dcb(l1->dch, HW_D_NOBLOCKED); + l1->dcb(l1->dch, PH_DEACTIVATE_IND); + } + if (l1->l1m.state != ST_L1_F6) { + mISDN_FsmChangeState(fi, ST_L1_F3); + l1->dcb(l1->dch, HW_POWERUP_REQ); + } +} + +static void +l1_timer_act(struct FsmInst *fi, int event, void *arg) +{ + struct layer1 *l1 = fi->userdata; + + test_and_clear_bit(FLG_L1_ACTTIMER, &l1->Flags); + test_and_set_bit(FLG_L1_ACTIVATED, &l1->Flags); + l1->dcb(l1->dch, PH_ACTIVATE_IND); +} + +static void +l1_timer_deact(struct FsmInst *fi, int event, void *arg) +{ + struct layer1 *l1 = fi->userdata; + + test_and_clear_bit(FLG_L1_DEACTTIMER, &l1->Flags); + test_and_clear_bit(FLG_L1_ACTIVATED, &l1->Flags); + if (test_and_clear_bit(FLG_L1_DBLOCKED, &l1->Flags)) + l1->dcb(l1->dch, HW_D_NOBLOCKED); + l1->dcb(l1->dch, PH_DEACTIVATE_IND); + l1->dcb(l1->dch, HW_DEACT_REQ); +} + +static void +l1_activate_s(struct FsmInst *fi, int event, void *arg) +{ + struct layer1 *l1 = fi->userdata; + + mISDN_FsmRestartTimer(&l1->timer, TIMER3_VALUE, EV_TIMER3, NULL, 2); + test_and_set_bit(FLG_L1_T3RUN, &l1->Flags); + l1->dcb(l1->dch, HW_RESET_REQ); +} + +static void +l1_activate_no(struct FsmInst *fi, int event, void *arg) +{ + struct layer1 *l1 = fi->userdata; + + if ((!test_bit(FLG_L1_DEACTTIMER, &l1->Flags)) && + (!test_bit(FLG_L1_T3RUN, &l1->Flags))) { + test_and_clear_bit(FLG_L1_ACTIVATING, &l1->Flags); + if (test_and_clear_bit(FLG_L1_DBLOCKED, &l1->Flags)) + l1->dcb(l1->dch, HW_D_NOBLOCKED); + l1->dcb(l1->dch, PH_DEACTIVATE_IND); + } +} + +static struct FsmNode L1SFnList[] = +{ + {ST_L1_F3, EV_PH_ACTIVATE, l1_activate_s}, + {ST_L1_F6, EV_PH_ACTIVATE, l1_activate_no}, + {ST_L1_F8, EV_PH_ACTIVATE, l1_activate_no}, + {ST_L1_F3, EV_RESET_IND, l1_reset}, + {ST_L1_F4, EV_RESET_IND, l1_reset}, + {ST_L1_F5, EV_RESET_IND, l1_reset}, + {ST_L1_F6, EV_RESET_IND, l1_reset}, + {ST_L1_F7, EV_RESET_IND, l1_reset}, + {ST_L1_F8, EV_RESET_IND, l1_reset}, + {ST_L1_F3, EV_DEACT_CNF, l1_deact_cnf}, + {ST_L1_F4, EV_DEACT_CNF, l1_deact_cnf}, + {ST_L1_F5, EV_DEACT_CNF, l1_deact_cnf}, + {ST_L1_F6, EV_DEACT_CNF, l1_deact_cnf}, + {ST_L1_F7, EV_DEACT_CNF, l1_deact_cnf}, + {ST_L1_F8, EV_DEACT_CNF, l1_deact_cnf}, + {ST_L1_F6, EV_DEACT_IND, l1_deact_req_s}, + {ST_L1_F7, EV_DEACT_IND, l1_deact_req_s}, + {ST_L1_F8, EV_DEACT_IND, l1_deact_req_s}, + {ST_L1_F3, EV_POWER_UP, l1_power_up_s}, + {ST_L1_F4, EV_ANYSIG_IND, l1_go_F5}, + {ST_L1_F6, EV_ANYSIG_IND, l1_go_F8}, + {ST_L1_F7, EV_ANYSIG_IND, l1_go_F8}, + {ST_L1_F3, EV_INFO2_IND, l1_info2_ind}, + {ST_L1_F4, EV_INFO2_IND, l1_info2_ind}, + {ST_L1_F5, EV_INFO2_IND, l1_info2_ind}, + {ST_L1_F7, EV_INFO2_IND, l1_info2_ind}, + {ST_L1_F8, EV_INFO2_IND, l1_info2_ind}, + {ST_L1_F3, EV_INFO4_IND, l1_info4_ind}, + {ST_L1_F4, EV_INFO4_IND, l1_info4_ind}, + {ST_L1_F5, EV_INFO4_IND, l1_info4_ind}, + {ST_L1_F6, EV_INFO4_IND, l1_info4_ind}, + {ST_L1_F8, EV_INFO4_IND, l1_info4_ind}, + {ST_L1_F3, EV_TIMER3, l1_timer3}, + {ST_L1_F4, EV_TIMER3, l1_timer3}, + {ST_L1_F5, EV_TIMER3, l1_timer3}, + {ST_L1_F6, EV_TIMER3, l1_timer3}, + {ST_L1_F8, EV_TIMER3, l1_timer3}, + {ST_L1_F7, EV_TIMER_ACT, l1_timer_act}, + {ST_L1_F3, EV_TIMER_DEACT, l1_timer_deact}, + {ST_L1_F4, EV_TIMER_DEACT, l1_timer_deact}, + {ST_L1_F5, EV_TIMER_DEACT, l1_timer_deact}, + {ST_L1_F6, EV_TIMER_DEACT, l1_timer_deact}, + {ST_L1_F7, EV_TIMER_DEACT, l1_timer_deact}, + {ST_L1_F8, EV_TIMER_DEACT, l1_timer_deact}, +}; + +static void +release_l1(struct layer1 *l1) { + mISDN_FsmDelTimer(&l1->timer, 0); + if (l1->dch) + l1->dch->l1 = NULL; + module_put(THIS_MODULE); + kfree(l1); +} + +int +l1_event(struct layer1 *l1, u_int event) +{ + int err = 0; + + if (!l1) + return -EINVAL; + switch (event) { + case HW_RESET_IND: + mISDN_FsmEvent(&l1->l1m, EV_RESET_IND, NULL); + break; + case HW_DEACT_IND: + mISDN_FsmEvent(&l1->l1m, EV_DEACT_IND, NULL); + break; + case HW_POWERUP_IND: + mISDN_FsmEvent(&l1->l1m, EV_POWER_UP, NULL); + break; + case HW_DEACT_CNF: + mISDN_FsmEvent(&l1->l1m, EV_DEACT_CNF, NULL); + break; + case ANYSIGNAL: + mISDN_FsmEvent(&l1->l1m, EV_ANYSIG_IND, NULL); + break; + case LOSTFRAMING: + mISDN_FsmEvent(&l1->l1m, EV_ANYSIG_IND, NULL); + break; + case INFO2: + mISDN_FsmEvent(&l1->l1m, EV_INFO2_IND, NULL); + break; + case INFO4_P8: + mISDN_FsmEvent(&l1->l1m, EV_INFO4_IND, NULL); + break; + case INFO4_P10: + mISDN_FsmEvent(&l1->l1m, EV_INFO4_IND, NULL); + break; + case PH_ACTIVATE_REQ: + if (test_bit(FLG_L1_ACTIVATED, &l1->Flags)) + l1->dcb(l1->dch, PH_ACTIVATE_IND); + else { + test_and_set_bit(FLG_L1_ACTIVATING, &l1->Flags); + mISDN_FsmEvent(&l1->l1m, EV_PH_ACTIVATE, NULL); + } + break; + case CLOSE_CHANNEL: + release_l1(l1); + break; + default: + if (*debug & DEBUG_L1) + printk(KERN_DEBUG "%s %x unhandled\n", + __func__, event); + err = -EINVAL; + } + return err; +} +EXPORT_SYMBOL(l1_event); + +int +create_l1(struct dchannel *dch, dchannel_l1callback *dcb) { + struct layer1 *nl1; + + nl1 = kzalloc(sizeof(struct layer1), GFP_ATOMIC); + if (!nl1) { + printk(KERN_ERR "kmalloc struct layer1 failed\n"); + return -ENOMEM; + } + nl1->l1m.fsm = &l1fsm_s; + nl1->l1m.state = ST_L1_F3; + nl1->Flags = 0; + nl1->l1m.debug = *debug & DEBUG_L1_FSM; + nl1->l1m.userdata = nl1; + nl1->l1m.userint = 0; + nl1->l1m.printdebug = l1m_debug; + nl1->dch = dch; + nl1->dcb = dcb; + mISDN_FsmInitTimer(&nl1->l1m, &nl1->timer); + __module_get(THIS_MODULE); + dch->l1 = nl1; + return 0; +} +EXPORT_SYMBOL(create_l1); + +int +l1_init(u_int *deb) +{ + debug = deb; + l1fsm_s.state_count = L1S_STATE_COUNT; + l1fsm_s.event_count = L1_EVENT_COUNT; + l1fsm_s.strEvent = strL1Event; + l1fsm_s.strState = strL1SState; + mISDN_FsmNew(&l1fsm_s, L1SFnList, ARRAY_SIZE(L1SFnList)); + return 0; +} + +void +l1_cleanup(void) +{ + mISDN_FsmFree(&l1fsm_s); +} diff --git a/drivers/isdn/mISDN/layer1.h b/drivers/isdn/mISDN/layer1.h new file mode 100644 index 000000000000..9c8125fd89af --- /dev/null +++ b/drivers/isdn/mISDN/layer1.h @@ -0,0 +1,26 @@ +/* + * + * Layer 1 defines + * + * Copyright 2008 by Karsten Keil <kkeil@novell.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#define FLG_L1_ACTIVATING 1 +#define FLG_L1_ACTIVATED 2 +#define FLG_L1_DEACTTIMER 3 +#define FLG_L1_ACTTIMER 4 +#define FLG_L1_T3RUN 5 +#define FLG_L1_PULL_REQ 6 +#define FLG_L1_UINT 7 +#define FLG_L1_DBLOCKED 8 + diff --git a/drivers/isdn/mISDN/layer2.c b/drivers/isdn/mISDN/layer2.c new file mode 100644 index 000000000000..a7915a156c04 --- /dev/null +++ b/drivers/isdn/mISDN/layer2.c @@ -0,0 +1,2216 @@ +/* + * + * Author Karsten Keil <kkeil@novell.com> + * + * Copyright 2008 by Karsten Keil <kkeil@novell.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#include "fsm.h" +#include "layer2.h" + +static int *debug; + +static +struct Fsm l2fsm = {NULL, 0, 0, NULL, NULL}; + +static char *strL2State[] = +{ + "ST_L2_1", + "ST_L2_2", + "ST_L2_3", + "ST_L2_4", + "ST_L2_5", + "ST_L2_6", + "ST_L2_7", + "ST_L2_8", +}; + +enum { + EV_L2_UI, + EV_L2_SABME, + EV_L2_DISC, + EV_L2_DM, + EV_L2_UA, + EV_L2_FRMR, + EV_L2_SUPER, + EV_L2_I, + EV_L2_DL_DATA, + EV_L2_ACK_PULL, + EV_L2_DL_UNITDATA, + EV_L2_DL_ESTABLISH_REQ, + EV_L2_DL_RELEASE_REQ, + EV_L2_MDL_ASSIGN, + EV_L2_MDL_REMOVE, + EV_L2_MDL_ERROR, + EV_L1_DEACTIVATE, + EV_L2_T200, + EV_L2_T203, + EV_L2_SET_OWN_BUSY, + EV_L2_CLEAR_OWN_BUSY, + EV_L2_FRAME_ERROR, +}; + +#define L2_EVENT_COUNT (EV_L2_FRAME_ERROR+1) + +static char *strL2Event[] = +{ + "EV_L2_UI", + "EV_L2_SABME", + "EV_L2_DISC", + "EV_L2_DM", + "EV_L2_UA", + "EV_L2_FRMR", + "EV_L2_SUPER", + "EV_L2_I", + "EV_L2_DL_DATA", + "EV_L2_ACK_PULL", + "EV_L2_DL_UNITDATA", + "EV_L2_DL_ESTABLISH_REQ", + "EV_L2_DL_RELEASE_REQ", + "EV_L2_MDL_ASSIGN", + "EV_L2_MDL_REMOVE", + "EV_L2_MDL_ERROR", + "EV_L1_DEACTIVATE", + "EV_L2_T200", + "EV_L2_T203", + "EV_L2_SET_OWN_BUSY", + "EV_L2_CLEAR_OWN_BUSY", + "EV_L2_FRAME_ERROR", +}; + +static void +l2m_debug(struct FsmInst *fi, char *fmt, ...) +{ + struct layer2 *l2 = fi->userdata; + va_list va; + + if (!(*debug & DEBUG_L2_FSM)) + return; + va_start(va, fmt); + printk(KERN_DEBUG "l2 (tei %d): ", l2->tei); + vprintk(fmt, va); + printk("\n"); + va_end(va); +} + +inline u_int +l2headersize(struct layer2 *l2, int ui) +{ + return ((test_bit(FLG_MOD128, &l2->flag) && (!ui)) ? 2 : 1) + + (test_bit(FLG_LAPD, &l2->flag) ? 2 : 1); +} + +inline u_int +l2addrsize(struct layer2 *l2) +{ + return test_bit(FLG_LAPD, &l2->flag) ? 2 : 1; +} + +static u_int +l2_newid(struct layer2 *l2) +{ + u_int id; + + id = l2->next_id++; + if (id == 0x7fff) + l2->next_id = 1; + id <<= 16; + id |= l2->tei << 8; + id |= l2->sapi; + return id; +} + +static void +l2up(struct layer2 *l2, u_int prim, struct sk_buff *skb) +{ + int err; + + if (!l2->up) + return; + mISDN_HEAD_PRIM(skb) = prim; + mISDN_HEAD_ID(skb) = (l2->ch.nr << 16) | l2->ch.addr; + err = l2->up->send(l2->up, skb); + if (err) { + printk(KERN_WARNING "%s: err=%d\n", __func__, err); + dev_kfree_skb(skb); + } +} + +static void +l2up_create(struct layer2 *l2, u_int prim, int len, void *arg) +{ + struct sk_buff *skb; + struct mISDNhead *hh; + int err; + + if (!l2->up) + return; + skb = mI_alloc_skb(len, GFP_ATOMIC); + if (!skb) + return; + hh = mISDN_HEAD_P(skb); + hh->prim = prim; + hh->id = (l2->ch.nr << 16) | l2->ch.addr; + if (len) + memcpy(skb_put(skb, len), arg, len); + err = l2->up->send(l2->up, skb); + if (err) { + printk(KERN_WARNING "%s: err=%d\n", __func__, err); + dev_kfree_skb(skb); + } +} + +static int +l2down_skb(struct layer2 *l2, struct sk_buff *skb) { + int ret; + + ret = l2->ch.recv(l2->ch.peer, skb); + if (ret && (*debug & DEBUG_L2_RECV)) + printk(KERN_DEBUG "l2down_skb: ret(%d)\n", ret); + return ret; +} + +static int +l2down_raw(struct layer2 *l2, struct sk_buff *skb) +{ + struct mISDNhead *hh = mISDN_HEAD_P(skb); + + if (hh->prim == PH_DATA_REQ) { + if (test_and_set_bit(FLG_L1_NOTREADY, &l2->flag)) { + skb_queue_tail(&l2->down_queue, skb); + return 0; + } + l2->down_id = mISDN_HEAD_ID(skb); + } + return l2down_skb(l2, skb); +} + +static int +l2down(struct layer2 *l2, u_int prim, u_int id, struct sk_buff *skb) +{ + struct mISDNhead *hh = mISDN_HEAD_P(skb); + + hh->prim = prim; + hh->id = id; + return l2down_raw(l2, skb); +} + +static int +l2down_create(struct layer2 *l2, u_int prim, u_int id, int len, void *arg) +{ + struct sk_buff *skb; + int err; + struct mISDNhead *hh; + + skb = mI_alloc_skb(len, GFP_ATOMIC); + if (!skb) + return -ENOMEM; + hh = mISDN_HEAD_P(skb); + hh->prim = prim; + hh->id = id; + if (len) + memcpy(skb_put(skb, len), arg, len); + err = l2down_raw(l2, skb); + if (err) + dev_kfree_skb(skb); + return err; +} + +static int +ph_data_confirm(struct layer2 *l2, struct mISDNhead *hh, struct sk_buff *skb) { + struct sk_buff *nskb = skb; + int ret = -EAGAIN; + + if (test_bit(FLG_L1_NOTREADY, &l2->flag)) { + if (hh->id == l2->down_id) { + nskb = skb_dequeue(&l2->down_queue); + if (nskb) { + l2->down_id = mISDN_HEAD_ID(nskb); + if (l2down_skb(l2, nskb)) { + dev_kfree_skb(nskb); + l2->down_id = MISDN_ID_NONE; + } + } else + l2->down_id = MISDN_ID_NONE; + if (ret) { + dev_kfree_skb(skb); + ret = 0; + } + if (l2->down_id == MISDN_ID_NONE) { + test_and_clear_bit(FLG_L1_NOTREADY, &l2->flag); + mISDN_FsmEvent(&l2->l2m, EV_L2_ACK_PULL, NULL); + } + } + } + if (!test_and_set_bit(FLG_L1_NOTREADY, &l2->flag)) { + nskb = skb_dequeue(&l2->down_queue); + if (nskb) { + l2->down_id = mISDN_HEAD_ID(nskb); + if (l2down_skb(l2, nskb)) { + dev_kfree_skb(nskb); + l2->down_id = MISDN_ID_NONE; + test_and_clear_bit(FLG_L1_NOTREADY, &l2->flag); + } + } else + test_and_clear_bit(FLG_L1_NOTREADY, &l2->flag); + } + return ret; +} + +static int +l2mgr(struct layer2 *l2, u_int prim, void *arg) { + long c = (long)arg; + + printk(KERN_WARNING + "l2mgr: addr:%x prim %x %c\n", l2->id, prim, (char)c); + if (test_bit(FLG_LAPD, &l2->flag) && + !test_bit(FLG_FIXED_TEI, &l2->flag)) { + switch (c) { + case 'C': + case 'D': + case 'G': + case 'H': + l2_tei(l2, prim, (u_long)arg); + break; + } + } + return 0; +} + +static void +set_peer_busy(struct layer2 *l2) { + test_and_set_bit(FLG_PEER_BUSY, &l2->flag); + if (skb_queue_len(&l2->i_queue) || skb_queue_len(&l2->ui_queue)) + test_and_set_bit(FLG_L2BLOCK, &l2->flag); +} + +static void +clear_peer_busy(struct layer2 *l2) { + if (test_and_clear_bit(FLG_PEER_BUSY, &l2->flag)) + test_and_clear_bit(FLG_L2BLOCK, &l2->flag); +} + +static void +InitWin(struct layer2 *l2) +{ + int i; + + for (i = 0; i < MAX_WINDOW; i++) + l2->windowar[i] = NULL; +} + +static int +freewin(struct layer2 *l2) +{ + int i, cnt = 0; + + for (i = 0; i < MAX_WINDOW; i++) { + if (l2->windowar[i]) { + cnt++; + dev_kfree_skb(l2->windowar[i]); + l2->windowar[i] = NULL; + } + } + return cnt; +} + +static void +ReleaseWin(struct layer2 *l2) +{ + int cnt = freewin(l2); + + if (cnt) + printk(KERN_WARNING + "isdnl2 freed %d skbuffs in release\n", cnt); +} + +inline unsigned int +cansend(struct layer2 *l2) +{ + unsigned int p1; + + if (test_bit(FLG_MOD128, &l2->flag)) + p1 = (l2->vs - l2->va) % 128; + else + p1 = (l2->vs - l2->va) % 8; + return (p1 < l2->window) && !test_bit(FLG_PEER_BUSY, &l2->flag); +} + +inline void +clear_exception(struct layer2 *l2) +{ + test_and_clear_bit(FLG_ACK_PEND, &l2->flag); + test_and_clear_bit(FLG_REJEXC, &l2->flag); + test_and_clear_bit(FLG_OWN_BUSY, &l2->flag); + clear_peer_busy(l2); +} + +static int +sethdraddr(struct layer2 *l2, u_char *header, int rsp) +{ + u_char *ptr = header; + int crbit = rsp; + + if (test_bit(FLG_LAPD, &l2->flag)) { + if (test_bit(FLG_LAPD_NET, &l2->flag)) + crbit = !crbit; + *ptr++ = (l2->sapi << 2) | (crbit ? 2 : 0); + *ptr++ = (l2->tei << 1) | 1; + return 2; + } else { + if (test_bit(FLG_ORIG, &l2->flag)) + crbit = !crbit; + if (crbit) + *ptr++ = l2->addr.B; + else + *ptr++ = l2->addr.A; + return 1; + } +} + +static inline void +enqueue_super(struct layer2 *l2, struct sk_buff *skb) +{ + if (l2down(l2, PH_DATA_REQ, l2_newid(l2), skb)) + dev_kfree_skb(skb); +} + +static inline void +enqueue_ui(struct layer2 *l2, struct sk_buff *skb) +{ + if (l2->tm) + l2_tei(l2, MDL_STATUS_UI_IND, 0); + if (l2down(l2, PH_DATA_REQ, l2_newid(l2), skb)) + dev_kfree_skb(skb); +} + +inline int +IsUI(u_char *data) +{ + return (data[0] & 0xef) == UI; +} + +inline int +IsUA(u_char *data) +{ + return (data[0] & 0xef) == UA; +} + +inline int +IsDM(u_char *data) +{ + return (data[0] & 0xef) == DM; +} + +inline int +IsDISC(u_char *data) +{ + return (data[0] & 0xef) == DISC; +} + +inline int +IsRR(u_char *data, struct layer2 *l2) +{ + if (test_bit(FLG_MOD128, &l2->flag)) + return data[0] == RR; + else + return (data[0] & 0xf) == 1; +} + +inline int +IsSFrame(u_char *data, struct layer2 *l2) +{ + register u_char d = *data; + + if (!test_bit(FLG_MOD128, &l2->flag)) + d &= 0xf; + return ((d & 0xf3) == 1) && ((d & 0x0c) != 0x0c); +} + +inline int +IsSABME(u_char *data, struct layer2 *l2) +{ + u_char d = data[0] & ~0x10; + + return test_bit(FLG_MOD128, &l2->flag) ? d == SABME : d == SABM; +} + +inline int +IsREJ(u_char *data, struct layer2 *l2) +{ + return test_bit(FLG_MOD128, &l2->flag) ? + data[0] == REJ : (data[0] & 0xf) == REJ; +} + +inline int +IsFRMR(u_char *data) +{ + return (data[0] & 0xef) == FRMR; +} + +inline int +IsRNR(u_char *data, struct layer2 *l2) +{ + return test_bit(FLG_MOD128, &l2->flag) ? + data[0] == RNR : (data[0] & 0xf) == RNR; +} + +int +iframe_error(struct layer2 *l2, struct sk_buff *skb) +{ + u_int i; + int rsp = *skb->data & 0x2; + + i = l2addrsize(l2) + (test_bit(FLG_MOD128, &l2->flag) ? 2 : 1); + if (test_bit(FLG_ORIG, &l2->flag)) + rsp = !rsp; + if (rsp) + return 'L'; + if (skb->len < i) + return 'N'; + if ((skb->len - i) > l2->maxlen) + return 'O'; + return 0; +} + +int +super_error(struct layer2 *l2, struct sk_buff *skb) +{ + if (skb->len != l2addrsize(l2) + + (test_bit(FLG_MOD128, &l2->flag) ? 2 : 1)) + return 'N'; + return 0; +} + +int +unnum_error(struct layer2 *l2, struct sk_buff *skb, int wantrsp) +{ + int rsp = (*skb->data & 0x2) >> 1; + if (test_bit(FLG_ORIG, &l2->flag)) + rsp = !rsp; + if (rsp != wantrsp) + return 'L'; + if (skb->len != l2addrsize(l2) + 1) + return 'N'; + return 0; +} + +int +UI_error(struct layer2 *l2, struct sk_buff *skb) +{ + int rsp = *skb->data & 0x2; + if (test_bit(FLG_ORIG, &l2->flag)) + rsp = !rsp; + if (rsp) + return 'L'; + if (skb->len > l2->maxlen + l2addrsize(l2) + 1) + return 'O'; + return 0; +} + +int +FRMR_error(struct layer2 *l2, struct sk_buff *skb) +{ + u_int headers = l2addrsize(l2) + 1; + u_char *datap = skb->data + headers; + int rsp = *skb->data & 0x2; + + if (test_bit(FLG_ORIG, &l2->flag)) + rsp = !rsp; + if (!rsp) + return 'L'; + if (test_bit(FLG_MOD128, &l2->flag)) { + if (skb->len < headers + 5) + return 'N'; + else if (*debug & DEBUG_L2) + l2m_debug(&l2->l2m, + "FRMR information %2x %2x %2x %2x %2x", + datap[0], datap[1], datap[2], datap[3], datap[4]); + } else { + if (skb->len < headers + 3) + return 'N'; + else if (*debug & DEBUG_L2) + l2m_debug(&l2->l2m, + "FRMR information %2x %2x %2x", + datap[0], datap[1], datap[2]); + } + return 0; +} + +static unsigned int +legalnr(struct layer2 *l2, unsigned int nr) +{ + if (test_bit(FLG_MOD128, &l2->flag)) + return ((nr - l2->va) % 128) <= ((l2->vs - l2->va) % 128); + else + return ((nr - l2->va) % 8) <= ((l2->vs - l2->va) % 8); +} + +static void +setva(struct layer2 *l2, unsigned int nr) +{ + struct sk_buff *skb; + + while (l2->va != nr) { + l2->va++; + if (test_bit(FLG_MOD128, &l2->flag)) + l2->va %= 128; + else + l2->va %= 8; + if (l2->windowar[l2->sow]) { + skb_trim(l2->windowar[l2->sow], 0); + skb_queue_tail(&l2->tmp_queue, l2->windowar[l2->sow]); + l2->windowar[l2->sow] = NULL; + } + l2->sow = (l2->sow + 1) % l2->window; + } + skb = skb_dequeue(&l2->tmp_queue); + while (skb) { + dev_kfree_skb(skb); + skb = skb_dequeue(&l2->tmp_queue); + } +} + +static void +send_uframe(struct layer2 *l2, struct sk_buff *skb, u_char cmd, u_char cr) +{ + u_char tmp[MAX_L2HEADER_LEN]; + int i; + + i = sethdraddr(l2, tmp, cr); + tmp[i++] = cmd; + if (skb) + skb_trim(skb, 0); + else { + skb = mI_alloc_skb(i, GFP_ATOMIC); + if (!skb) { + printk(KERN_WARNING "%s: can't alloc skbuff\n", + __func__); + return; + } + } + memcpy(skb_put(skb, i), tmp, i); + enqueue_super(l2, skb); +} + + +inline u_char +get_PollFlag(struct layer2 *l2, struct sk_buff *skb) +{ + return skb->data[l2addrsize(l2)] & 0x10; +} + +inline u_char +get_PollFlagFree(struct layer2 *l2, struct sk_buff *skb) +{ + u_char PF; + + PF = get_PollFlag(l2, skb); + dev_kfree_skb(skb); + return PF; +} + +inline void +start_t200(struct layer2 *l2, int i) +{ + mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, i); + test_and_set_bit(FLG_T200_RUN, &l2->flag); +} + +inline void +restart_t200(struct layer2 *l2, int i) +{ + mISDN_FsmRestartTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, i); + test_and_set_bit(FLG_T200_RUN, &l2->flag); +} + +inline void +stop_t200(struct layer2 *l2, int i) +{ + if (test_and_clear_bit(FLG_T200_RUN, &l2->flag)) + mISDN_FsmDelTimer(&l2->t200, i); +} + +inline void +st5_dl_release_l2l3(struct layer2 *l2) +{ + int pr; + + if (test_and_clear_bit(FLG_PEND_REL, &l2->flag)) + pr = DL_RELEASE_CNF; + else + pr = DL_RELEASE_IND; + l2up_create(l2, pr, 0, NULL); +} + +inline void +lapb_dl_release_l2l3(struct layer2 *l2, int f) +{ + if (test_bit(FLG_LAPB, &l2->flag)) + l2down_create(l2, PH_DEACTIVATE_REQ, l2_newid(l2), 0, NULL); + l2up_create(l2, f, 0, NULL); +} + +static void +establishlink(struct FsmInst *fi) +{ + struct layer2 *l2 = fi->userdata; + u_char cmd; + + clear_exception(l2); + l2->rc = 0; + cmd = (test_bit(FLG_MOD128, &l2->flag) ? SABME : SABM) | 0x10; + send_uframe(l2, NULL, cmd, CMD); + mISDN_FsmDelTimer(&l2->t203, 1); + restart_t200(l2, 1); + test_and_clear_bit(FLG_PEND_REL, &l2->flag); + freewin(l2); + mISDN_FsmChangeState(fi, ST_L2_5); +} + +static void +l2_mdl_error_ua(struct FsmInst *fi, int event, void *arg) +{ + struct sk_buff *skb = arg; + struct layer2 *l2 = fi->userdata; + + if (get_PollFlagFree(l2, skb)) + l2mgr(l2, MDL_ERROR_IND, (void *) 'C'); + else + l2mgr(l2, MDL_ERROR_IND, (void *) 'D'); + +} + +static void +l2_mdl_error_dm(struct FsmInst *fi, int event, void *arg) +{ + struct sk_buff *skb = arg; + struct layer2 *l2 = fi->userdata; + + if (get_PollFlagFree(l2, skb)) + l2mgr(l2, MDL_ERROR_IND, (void *) 'B'); + else { + l2mgr(l2, MDL_ERROR_IND, (void *) 'E'); + establishlink(fi); + test_and_clear_bit(FLG_L3_INIT, &l2->flag); + } +} + +static void +l2_st8_mdl_error_dm(struct FsmInst *fi, int event, void *arg) +{ + struct sk_buff *skb = arg; + struct layer2 *l2 = fi->userdata; + + if (get_PollFlagFree(l2, skb)) + l2mgr(l2, MDL_ERROR_IND, (void *) 'B'); + else + l2mgr(l2, MDL_ERROR_IND, (void *) 'E'); + establishlink(fi); + test_and_clear_bit(FLG_L3_INIT, &l2->flag); +} + +static void +l2_go_st3(struct FsmInst *fi, int event, void *arg) +{ + dev_kfree_skb((struct sk_buff *)arg); + mISDN_FsmChangeState(fi, ST_L2_3); +} + +static void +l2_mdl_assign(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + + mISDN_FsmChangeState(fi, ST_L2_3); + dev_kfree_skb((struct sk_buff *)arg); + l2_tei(l2, MDL_ASSIGN_IND, 0); +} + +static void +l2_queue_ui_assign(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + skb_queue_tail(&l2->ui_queue, skb); + mISDN_FsmChangeState(fi, ST_L2_2); + l2_tei(l2, MDL_ASSIGN_IND, 0); +} + +static void +l2_queue_ui(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + skb_queue_tail(&l2->ui_queue, skb); +} + +static void +tx_ui(struct layer2 *l2) +{ + struct sk_buff *skb; + u_char header[MAX_L2HEADER_LEN]; + int i; + + i = sethdraddr(l2, header, CMD); + if (test_bit(FLG_LAPD_NET, &l2->flag)) + header[1] = 0xff; /* tei 127 */ + header[i++] = UI; + while ((skb = skb_dequeue(&l2->ui_queue))) { + memcpy(skb_push(skb, i), header, i); + enqueue_ui(l2, skb); + } +} + +static void +l2_send_ui(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + skb_queue_tail(&l2->ui_queue, skb); + tx_ui(l2); +} + +static void +l2_got_ui(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + skb_pull(skb, l2headersize(l2, 1)); +/* + * in states 1-3 for broadcast + */ + + if (l2->tm) + l2_tei(l2, MDL_STATUS_UI_IND, 0); + l2up(l2, DL_UNITDATA_IND, skb); +} + +static void +l2_establish(struct FsmInst *fi, int event, void *arg) +{ + struct sk_buff *skb = arg; + struct layer2 *l2 = fi->userdata; + + establishlink(fi); + test_and_set_bit(FLG_L3_INIT, &l2->flag); + dev_kfree_skb(skb); +} + +static void +l2_discard_i_setl3(struct FsmInst *fi, int event, void *arg) +{ + struct sk_buff *skb = arg; + struct layer2 *l2 = fi->userdata; + + skb_queue_purge(&l2->i_queue); + test_and_set_bit(FLG_L3_INIT, &l2->flag); + test_and_clear_bit(FLG_PEND_REL, &l2->flag); + dev_kfree_skb(skb); +} + +static void +l2_l3_reestablish(struct FsmInst *fi, int event, void *arg) +{ + struct sk_buff *skb = arg; + struct layer2 *l2 = fi->userdata; + + skb_queue_purge(&l2->i_queue); + establishlink(fi); + test_and_set_bit(FLG_L3_INIT, &l2->flag); + dev_kfree_skb(skb); +} + +static void +l2_release(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + skb_trim(skb, 0); + l2up(l2, DL_RELEASE_CNF, skb); +} + +static void +l2_pend_rel(struct FsmInst *fi, int event, void *arg) +{ + struct sk_buff *skb = arg; + struct layer2 *l2 = fi->userdata; + + test_and_set_bit(FLG_PEND_REL, &l2->flag); + dev_kfree_skb(skb); +} + +static void +l2_disconnect(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + skb_queue_purge(&l2->i_queue); + freewin(l2); + mISDN_FsmChangeState(fi, ST_L2_6); + l2->rc = 0; + send_uframe(l2, NULL, DISC | 0x10, CMD); + mISDN_FsmDelTimer(&l2->t203, 1); + restart_t200(l2, 2); + if (skb) + dev_kfree_skb(skb); +} + +static void +l2_start_multi(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + l2->vs = 0; + l2->va = 0; + l2->vr = 0; + l2->sow = 0; + clear_exception(l2); + send_uframe(l2, NULL, UA | get_PollFlag(l2, skb), RSP); + mISDN_FsmChangeState(fi, ST_L2_7); + mISDN_FsmAddTimer(&l2->t203, l2->T203, EV_L2_T203, NULL, 3); + skb_trim(skb, 0); + l2up(l2, DL_ESTABLISH_IND, skb); + if (l2->tm) + l2_tei(l2, MDL_STATUS_UP_IND, 0); +} + +static void +l2_send_UA(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + send_uframe(l2, skb, UA | get_PollFlag(l2, skb), RSP); +} + +static void +l2_send_DM(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + send_uframe(l2, skb, DM | get_PollFlag(l2, skb), RSP); +} + +static void +l2_restart_multi(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + int est = 0; + + send_uframe(l2, skb, UA | get_PollFlag(l2, skb), RSP); + + l2mgr(l2, MDL_ERROR_IND, (void *) 'F'); + + if (l2->vs != l2->va) { + skb_queue_purge(&l2->i_queue); + est = 1; + } + + clear_exception(l2); + l2->vs = 0; + l2->va = 0; + l2->vr = 0; + l2->sow = 0; + mISDN_FsmChangeState(fi, ST_L2_7); + stop_t200(l2, 3); + mISDN_FsmRestartTimer(&l2->t203, l2->T203, EV_L2_T203, NULL, 3); + + if (est) + l2up_create(l2, DL_ESTABLISH_IND, 0, NULL); +/* mISDN_queue_data(&l2->inst, l2->inst.id | MSG_BROADCAST, + * MGR_SHORTSTATUS | INDICATION, SSTATUS_L2_ESTABLISHED, + * 0, NULL, 0); + */ + if (skb_queue_len(&l2->i_queue) && cansend(l2)) + mISDN_FsmEvent(fi, EV_L2_ACK_PULL, NULL); +} + +static void +l2_stop_multi(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + mISDN_FsmChangeState(fi, ST_L2_4); + mISDN_FsmDelTimer(&l2->t203, 3); + stop_t200(l2, 4); + + send_uframe(l2, skb, UA | get_PollFlag(l2, skb), RSP); + skb_queue_purge(&l2->i_queue); + freewin(l2); + lapb_dl_release_l2l3(l2, DL_RELEASE_IND); + if (l2->tm) + l2_tei(l2, MDL_STATUS_DOWN_IND, 0); +} + +static void +l2_connected(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + int pr = -1; + + if (!get_PollFlag(l2, skb)) { + l2_mdl_error_ua(fi, event, arg); + return; + } + dev_kfree_skb(skb); + if (test_and_clear_bit(FLG_PEND_REL, &l2->flag)) + l2_disconnect(fi, event, NULL); + if (test_and_clear_bit(FLG_L3_INIT, &l2->flag)) { + pr = DL_ESTABLISH_CNF; + } else if (l2->vs != l2->va) { + skb_queue_purge(&l2->i_queue); + pr = DL_ESTABLISH_IND; + } + stop_t200(l2, 5); + l2->vr = 0; + l2->vs = 0; + l2->va = 0; + l2->sow = 0; + mISDN_FsmChangeState(fi, ST_L2_7); + mISDN_FsmAddTimer(&l2->t203, l2->T203, EV_L2_T203, NULL, 4); + if (pr != -1) + l2up_create(l2, pr, 0, NULL); + + if (skb_queue_len(&l2->i_queue) && cansend(l2)) + mISDN_FsmEvent(fi, EV_L2_ACK_PULL, NULL); + + if (l2->tm) + l2_tei(l2, MDL_STATUS_UP_IND, 0); +} + +static void +l2_released(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + if (!get_PollFlag(l2, skb)) { + l2_mdl_error_ua(fi, event, arg); + return; + } + dev_kfree_skb(skb); + stop_t200(l2, 6); + lapb_dl_release_l2l3(l2, DL_RELEASE_CNF); + mISDN_FsmChangeState(fi, ST_L2_4); + if (l2->tm) + l2_tei(l2, MDL_STATUS_DOWN_IND, 0); +} + +static void +l2_reestablish(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + if (!get_PollFlagFree(l2, skb)) { + establishlink(fi); + test_and_set_bit(FLG_L3_INIT, &l2->flag); + } +} + +static void +l2_st5_dm_release(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + if (get_PollFlagFree(l2, skb)) { + stop_t200(l2, 7); + if (!test_bit(FLG_L3_INIT, &l2->flag)) + skb_queue_purge(&l2->i_queue); + if (test_bit(FLG_LAPB, &l2->flag)) + l2down_create(l2, PH_DEACTIVATE_REQ, + l2_newid(l2), 0, NULL); + st5_dl_release_l2l3(l2); + mISDN_FsmChangeState(fi, ST_L2_4); + if (l2->tm) + l2_tei(l2, MDL_STATUS_DOWN_IND, 0); + } +} + +static void +l2_st6_dm_release(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + if (get_PollFlagFree(l2, skb)) { + stop_t200(l2, 8); + lapb_dl_release_l2l3(l2, DL_RELEASE_CNF); + mISDN_FsmChangeState(fi, ST_L2_4); + if (l2->tm) + l2_tei(l2, MDL_STATUS_DOWN_IND, 0); + } +} + +void +enquiry_cr(struct layer2 *l2, u_char typ, u_char cr, u_char pf) +{ + struct sk_buff *skb; + u_char tmp[MAX_L2HEADER_LEN]; + int i; + + i = sethdraddr(l2, tmp, cr); + if (test_bit(FLG_MOD128, &l2->flag)) { + tmp[i++] = typ; + tmp[i++] = (l2->vr << 1) | (pf ? 1 : 0); + } else + tmp[i++] = (l2->vr << 5) | typ | (pf ? 0x10 : 0); + skb = mI_alloc_skb(i, GFP_ATOMIC); + if (!skb) { + printk(KERN_WARNING + "isdnl2 can't alloc sbbuff for enquiry_cr\n"); + return; + } + memcpy(skb_put(skb, i), tmp, i); + enqueue_super(l2, skb); +} + +inline void +enquiry_response(struct layer2 *l2) +{ + if (test_bit(FLG_OWN_BUSY, &l2->flag)) + enquiry_cr(l2, RNR, RSP, 1); + else + enquiry_cr(l2, RR, RSP, 1); + test_and_clear_bit(FLG_ACK_PEND, &l2->flag); +} + +inline void +transmit_enquiry(struct layer2 *l2) +{ + if (test_bit(FLG_OWN_BUSY, &l2->flag)) + enquiry_cr(l2, RNR, CMD, 1); + else + enquiry_cr(l2, RR, CMD, 1); + test_and_clear_bit(FLG_ACK_PEND, &l2->flag); + start_t200(l2, 9); +} + + +static void +nrerrorrecovery(struct FsmInst *fi) +{ + struct layer2 *l2 = fi->userdata; + + l2mgr(l2, MDL_ERROR_IND, (void *) 'J'); + establishlink(fi); + test_and_clear_bit(FLG_L3_INIT, &l2->flag); +} + +static void +invoke_retransmission(struct layer2 *l2, unsigned int nr) +{ + u_int p1; + + if (l2->vs != nr) { + while (l2->vs != nr) { + (l2->vs)--; + if (test_bit(FLG_MOD128, &l2->flag)) { + l2->vs %= 128; + p1 = (l2->vs - l2->va) % 128; + } else { + l2->vs %= 8; + p1 = (l2->vs - l2->va) % 8; + } + p1 = (p1 + l2->sow) % l2->window; + if (l2->windowar[p1]) + skb_queue_head(&l2->i_queue, l2->windowar[p1]); + else + printk(KERN_WARNING + "%s: windowar[%d] is NULL\n", + __func__, p1); + l2->windowar[p1] = NULL; + } + mISDN_FsmEvent(&l2->l2m, EV_L2_ACK_PULL, NULL); + } +} + +static void +l2_st7_got_super(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + int PollFlag, rsp, typ = RR; + unsigned int nr; + + rsp = *skb->data & 0x2; + if (test_bit(FLG_ORIG, &l2->flag)) + rsp = !rsp; + + skb_pull(skb, l2addrsize(l2)); + if (IsRNR(skb->data, l2)) { + set_peer_busy(l2); + typ = RNR; + } else + clear_peer_busy(l2); + if (IsREJ(skb->data, l2)) + typ = REJ; + + if (test_bit(FLG_MOD128, &l2->flag)) { + PollFlag = (skb->data[1] & 0x1) == 0x1; + nr = skb->data[1] >> 1; + } else { + PollFlag = (skb->data[0] & 0x10); + nr = (skb->data[0] >> 5) & 0x7; + } + dev_kfree_skb(skb); + + if (PollFlag) { + if (rsp) + l2mgr(l2, MDL_ERROR_IND, (void *) 'A'); + else + enquiry_response(l2); + } + if (legalnr(l2, nr)) { + if (typ == REJ) { + setva(l2, nr); + invoke_retransmission(l2, nr); + stop_t200(l2, 10); + if (mISDN_FsmAddTimer(&l2->t203, l2->T203, + EV_L2_T203, NULL, 6)) + l2m_debug(&l2->l2m, "Restart T203 ST7 REJ"); + } else if ((nr == l2->vs) && (typ == RR)) { + setva(l2, nr); + stop_t200(l2, 11); + mISDN_FsmRestartTimer(&l2->t203, l2->T203, + EV_L2_T203, NULL, 7); + } else if ((l2->va != nr) || (typ == RNR)) { + setva(l2, nr); + if (typ != RR) + mISDN_FsmDelTimer(&l2->t203, 9); + restart_t200(l2, 12); + } + if (skb_queue_len(&l2->i_queue) && (typ == RR)) + mISDN_FsmEvent(fi, EV_L2_ACK_PULL, NULL); + } else + nrerrorrecovery(fi); +} + +static void +l2_feed_i_if_reest(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + if (!test_bit(FLG_L3_INIT, &l2->flag)) + skb_queue_tail(&l2->i_queue, skb); + else + dev_kfree_skb(skb); +} + +static void +l2_feed_i_pull(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + skb_queue_tail(&l2->i_queue, skb); + mISDN_FsmEvent(fi, EV_L2_ACK_PULL, NULL); +} + +static void +l2_feed_iqueue(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + skb_queue_tail(&l2->i_queue, skb); +} + +static void +l2_got_iframe(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + int PollFlag, i; + u_int ns, nr; + + i = l2addrsize(l2); + if (test_bit(FLG_MOD128, &l2->flag)) { + PollFlag = ((skb->data[i + 1] & 0x1) == 0x1); + ns = skb->data[i] >> 1; + nr = (skb->data[i + 1] >> 1) & 0x7f; + } else { + PollFlag = (skb->data[i] & 0x10); + ns = (skb->data[i] >> 1) & 0x7; + nr = (skb->data[i] >> 5) & 0x7; + } + if (test_bit(FLG_OWN_BUSY, &l2->flag)) { + dev_kfree_skb(skb); + if (PollFlag) + enquiry_response(l2); + } else { + if (l2->vr == ns) { + l2->vr++; + if (test_bit(FLG_MOD128, &l2->flag)) + l2->vr %= 128; + else + l2->vr %= 8; + test_and_clear_bit(FLG_REJEXC, &l2->flag); + if (PollFlag) + enquiry_response(l2); + else + test_and_set_bit(FLG_ACK_PEND, &l2->flag); + skb_pull(skb, l2headersize(l2, 0)); + l2up(l2, DL_DATA_IND, skb); + } else { + /* n(s)!=v(r) */ + dev_kfree_skb(skb); + if (test_and_set_bit(FLG_REJEXC, &l2->flag)) { + if (PollFlag) + enquiry_response(l2); + } else { + enquiry_cr(l2, REJ, RSP, PollFlag); + test_and_clear_bit(FLG_ACK_PEND, &l2->flag); + } + } + } + if (legalnr(l2, nr)) { + if (!test_bit(FLG_PEER_BUSY, &l2->flag) && + (fi->state == ST_L2_7)) { + if (nr == l2->vs) { + stop_t200(l2, 13); + mISDN_FsmRestartTimer(&l2->t203, l2->T203, + EV_L2_T203, NULL, 7); + } else if (nr != l2->va) + restart_t200(l2, 14); + } + setva(l2, nr); + } else { + nrerrorrecovery(fi); + return; + } + if (skb_queue_len(&l2->i_queue) && (fi->state == ST_L2_7)) + mISDN_FsmEvent(fi, EV_L2_ACK_PULL, NULL); + if (test_and_clear_bit(FLG_ACK_PEND, &l2->flag)) + enquiry_cr(l2, RR, RSP, 0); +} + +static void +l2_got_tei(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + u_int info; + + l2->tei = (signed char)(long)arg; + set_channel_address(&l2->ch, l2->sapi, l2->tei); + info = DL_INFO_L2_CONNECT; + l2up_create(l2, DL_INFORMATION_IND, sizeof(info), &info); + if (fi->state == ST_L2_3) { + establishlink(fi); + test_and_set_bit(FLG_L3_INIT, &l2->flag); + } else + mISDN_FsmChangeState(fi, ST_L2_4); + if (skb_queue_len(&l2->ui_queue)) + tx_ui(l2); +} + +static void +l2_st5_tout_200(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + + if (test_bit(FLG_LAPD, &l2->flag) && + test_bit(FLG_DCHAN_BUSY, &l2->flag)) { + mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, 9); + } else if (l2->rc == l2->N200) { + mISDN_FsmChangeState(fi, ST_L2_4); + test_and_clear_bit(FLG_T200_RUN, &l2->flag); + skb_queue_purge(&l2->i_queue); + l2mgr(l2, MDL_ERROR_IND, (void *) 'G'); + if (test_bit(FLG_LAPB, &l2->flag)) + l2down_create(l2, PH_DEACTIVATE_REQ, + l2_newid(l2), 0, NULL); + st5_dl_release_l2l3(l2); + if (l2->tm) + l2_tei(l2, MDL_STATUS_DOWN_IND, 0); + } else { + l2->rc++; + mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, 9); + send_uframe(l2, NULL, (test_bit(FLG_MOD128, &l2->flag) ? + SABME : SABM) | 0x10, CMD); + } +} + +static void +l2_st6_tout_200(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + + if (test_bit(FLG_LAPD, &l2->flag) && + test_bit(FLG_DCHAN_BUSY, &l2->flag)) { + mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, 9); + } else if (l2->rc == l2->N200) { + mISDN_FsmChangeState(fi, ST_L2_4); + test_and_clear_bit(FLG_T200_RUN, &l2->flag); + l2mgr(l2, MDL_ERROR_IND, (void *) 'H'); + lapb_dl_release_l2l3(l2, DL_RELEASE_CNF); + if (l2->tm) + l2_tei(l2, MDL_STATUS_DOWN_IND, 0); + } else { + l2->rc++; + mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, + NULL, 9); + send_uframe(l2, NULL, DISC | 0x10, CMD); + } +} + +static void +l2_st7_tout_200(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + + if (test_bit(FLG_LAPD, &l2->flag) && + test_bit(FLG_DCHAN_BUSY, &l2->flag)) { + mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, 9); + return; + } + test_and_clear_bit(FLG_T200_RUN, &l2->flag); + l2->rc = 0; + mISDN_FsmChangeState(fi, ST_L2_8); + transmit_enquiry(l2); + l2->rc++; +} + +static void +l2_st8_tout_200(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + + if (test_bit(FLG_LAPD, &l2->flag) && + test_bit(FLG_DCHAN_BUSY, &l2->flag)) { + mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, 9); + return; + } + test_and_clear_bit(FLG_T200_RUN, &l2->flag); + if (l2->rc == l2->N200) { + l2mgr(l2, MDL_ERROR_IND, (void *) 'I'); + establishlink(fi); + test_and_clear_bit(FLG_L3_INIT, &l2->flag); + } else { + transmit_enquiry(l2); + l2->rc++; + } +} + +static void +l2_st7_tout_203(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + + if (test_bit(FLG_LAPD, &l2->flag) && + test_bit(FLG_DCHAN_BUSY, &l2->flag)) { + mISDN_FsmAddTimer(&l2->t203, l2->T203, EV_L2_T203, NULL, 9); + return; + } + mISDN_FsmChangeState(fi, ST_L2_8); + transmit_enquiry(l2); + l2->rc = 0; +} + +static void +l2_pull_iqueue(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb, *nskb, *oskb; + u_char header[MAX_L2HEADER_LEN]; + u_int i, p1; + + if (!cansend(l2)) + return; + + skb = skb_dequeue(&l2->i_queue); + if (!skb) + return; + + if (test_bit(FLG_MOD128, &l2->flag)) + p1 = (l2->vs - l2->va) % 128; + else + p1 = (l2->vs - l2->va) % 8; + p1 = (p1 + l2->sow) % l2->window; + if (l2->windowar[p1]) { + printk(KERN_WARNING "isdnl2 try overwrite ack queue entry %d\n", + p1); + dev_kfree_skb(l2->windowar[p1]); + } + l2->windowar[p1] = skb; + i = sethdraddr(l2, header, CMD); + if (test_bit(FLG_MOD128, &l2->flag)) { + header[i++] = l2->vs << 1; + header[i++] = l2->vr << 1; + l2->vs = (l2->vs + 1) % 128; + } else { + header[i++] = (l2->vr << 5) | (l2->vs << 1); + l2->vs = (l2->vs + 1) % 8; + } + + nskb = skb_clone(skb, GFP_ATOMIC); + p1 = skb_headroom(nskb); + if (p1 >= i) + memcpy(skb_push(nskb, i), header, i); + else { + printk(KERN_WARNING + "isdnl2 pull_iqueue skb header(%d/%d) too short\n", i, p1); + oskb = nskb; + nskb = mI_alloc_skb(oskb->len + i, GFP_ATOMIC); + if (!nskb) { + dev_kfree_skb(oskb); + printk(KERN_WARNING "%s: no skb mem\n", __func__); + return; + } + memcpy(skb_put(nskb, i), header, i); + memcpy(skb_put(nskb, oskb->len), oskb->data, oskb->len); + dev_kfree_skb(oskb); + } + l2down(l2, PH_DATA_REQ, l2_newid(l2), nskb); + test_and_clear_bit(FLG_ACK_PEND, &l2->flag); + if (!test_and_set_bit(FLG_T200_RUN, &l2->flag)) { + mISDN_FsmDelTimer(&l2->t203, 13); + mISDN_FsmAddTimer(&l2->t200, l2->T200, EV_L2_T200, NULL, 11); + } +} + +static void +l2_st8_got_super(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + int PollFlag, rsp, rnr = 0; + unsigned int nr; + + rsp = *skb->data & 0x2; + if (test_bit(FLG_ORIG, &l2->flag)) + rsp = !rsp; + + skb_pull(skb, l2addrsize(l2)); + + if (IsRNR(skb->data, l2)) { + set_peer_busy(l2); + rnr = 1; + } else + clear_peer_busy(l2); + + if (test_bit(FLG_MOD128, &l2->flag)) { + PollFlag = (skb->data[1] & 0x1) == 0x1; + nr = skb->data[1] >> 1; + } else { + PollFlag = (skb->data[0] & 0x10); + nr = (skb->data[0] >> 5) & 0x7; + } + dev_kfree_skb(skb); + if (rsp && PollFlag) { + if (legalnr(l2, nr)) { + if (rnr) { + restart_t200(l2, 15); + } else { + stop_t200(l2, 16); + mISDN_FsmAddTimer(&l2->t203, l2->T203, + EV_L2_T203, NULL, 5); + setva(l2, nr); + } + invoke_retransmission(l2, nr); + mISDN_FsmChangeState(fi, ST_L2_7); + if (skb_queue_len(&l2->i_queue) && cansend(l2)) + mISDN_FsmEvent(fi, EV_L2_ACK_PULL, NULL); + } else + nrerrorrecovery(fi); + } else { + if (!rsp && PollFlag) + enquiry_response(l2); + if (legalnr(l2, nr)) + setva(l2, nr); + else + nrerrorrecovery(fi); + } +} + +static void +l2_got_FRMR(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + skb_pull(skb, l2addrsize(l2) + 1); + + if (!(skb->data[0] & 1) || ((skb->data[0] & 3) == 1) || /* I or S */ + (IsUA(skb->data) && (fi->state == ST_L2_7))) { + l2mgr(l2, MDL_ERROR_IND, (void *) 'K'); + establishlink(fi); + test_and_clear_bit(FLG_L3_INIT, &l2->flag); + } + dev_kfree_skb(skb); +} + +static void +l2_st24_tei_remove(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + + skb_queue_purge(&l2->ui_queue); + l2->tei = GROUP_TEI; + mISDN_FsmChangeState(fi, ST_L2_1); +} + +static void +l2_st3_tei_remove(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + + skb_queue_purge(&l2->ui_queue); + l2->tei = GROUP_TEI; + l2up_create(l2, DL_RELEASE_IND, 0, NULL); + mISDN_FsmChangeState(fi, ST_L2_1); +} + +static void +l2_st5_tei_remove(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + + skb_queue_purge(&l2->i_queue); + skb_queue_purge(&l2->ui_queue); + freewin(l2); + l2->tei = GROUP_TEI; + stop_t200(l2, 17); + st5_dl_release_l2l3(l2); + mISDN_FsmChangeState(fi, ST_L2_1); +} + +static void +l2_st6_tei_remove(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + + skb_queue_purge(&l2->ui_queue); + l2->tei = GROUP_TEI; + stop_t200(l2, 18); + l2up_create(l2, DL_RELEASE_IND, 0, NULL); + mISDN_FsmChangeState(fi, ST_L2_1); +} + +static void +l2_tei_remove(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + + skb_queue_purge(&l2->i_queue); + skb_queue_purge(&l2->ui_queue); + freewin(l2); + l2->tei = GROUP_TEI; + stop_t200(l2, 17); + mISDN_FsmDelTimer(&l2->t203, 19); + l2up_create(l2, DL_RELEASE_IND, 0, NULL); +/* mISDN_queue_data(&l2->inst, l2->inst.id | MSG_BROADCAST, + * MGR_SHORTSTATUS_IND, SSTATUS_L2_RELEASED, + * 0, NULL, 0); + */ + mISDN_FsmChangeState(fi, ST_L2_1); +} + +static void +l2_st14_persistant_da(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + skb_queue_purge(&l2->i_queue); + skb_queue_purge(&l2->ui_queue); + if (test_and_clear_bit(FLG_ESTAB_PEND, &l2->flag)) + l2up(l2, DL_RELEASE_IND, skb); + else + dev_kfree_skb(skb); +} + +static void +l2_st5_persistant_da(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + skb_queue_purge(&l2->i_queue); + skb_queue_purge(&l2->ui_queue); + freewin(l2); + stop_t200(l2, 19); + st5_dl_release_l2l3(l2); + mISDN_FsmChangeState(fi, ST_L2_4); + if (l2->tm) + l2_tei(l2, MDL_STATUS_DOWN_IND, 0); + dev_kfree_skb(skb); +} + +static void +l2_st6_persistant_da(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + skb_queue_purge(&l2->ui_queue); + stop_t200(l2, 20); + l2up(l2, DL_RELEASE_CNF, skb); + mISDN_FsmChangeState(fi, ST_L2_4); + if (l2->tm) + l2_tei(l2, MDL_STATUS_DOWN_IND, 0); +} + +static void +l2_persistant_da(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + skb_queue_purge(&l2->i_queue); + skb_queue_purge(&l2->ui_queue); + freewin(l2); + stop_t200(l2, 19); + mISDN_FsmDelTimer(&l2->t203, 19); + l2up(l2, DL_RELEASE_IND, skb); + mISDN_FsmChangeState(fi, ST_L2_4); + if (l2->tm) + l2_tei(l2, MDL_STATUS_DOWN_IND, 0); +} + +static void +l2_set_own_busy(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + if (!test_and_set_bit(FLG_OWN_BUSY, &l2->flag)) { + enquiry_cr(l2, RNR, RSP, 0); + test_and_clear_bit(FLG_ACK_PEND, &l2->flag); + } + if (skb) + dev_kfree_skb(skb); +} + +static void +l2_clear_own_busy(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + struct sk_buff *skb = arg; + + if (!test_and_clear_bit(FLG_OWN_BUSY, &l2->flag)) { + enquiry_cr(l2, RR, RSP, 0); + test_and_clear_bit(FLG_ACK_PEND, &l2->flag); + } + if (skb) + dev_kfree_skb(skb); +} + +static void +l2_frame_error(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + + l2mgr(l2, MDL_ERROR_IND, arg); +} + +static void +l2_frame_error_reest(struct FsmInst *fi, int event, void *arg) +{ + struct layer2 *l2 = fi->userdata; + + l2mgr(l2, MDL_ERROR_IND, arg); + establishlink(fi); + test_and_clear_bit(FLG_L3_INIT, &l2->flag); +} + +static struct FsmNode L2FnList[] = +{ + {ST_L2_1, EV_L2_DL_ESTABLISH_REQ, l2_mdl_assign}, + {ST_L2_2, EV_L2_DL_ESTABLISH_REQ, l2_go_st3}, + {ST_L2_4, EV_L2_DL_ESTABLISH_REQ, l2_establish}, + {ST_L2_5, EV_L2_DL_ESTABLISH_REQ, l2_discard_i_setl3}, + {ST_L2_7, EV_L2_DL_ESTABLISH_REQ, l2_l3_reestablish}, + {ST_L2_8, EV_L2_DL_ESTABLISH_REQ, l2_l3_reestablish}, + {ST_L2_4, EV_L2_DL_RELEASE_REQ, l2_release}, + {ST_L2_5, EV_L2_DL_RELEASE_REQ, l2_pend_rel}, + {ST_L2_7, EV_L2_DL_RELEASE_REQ, l2_disconnect}, + {ST_L2_8, EV_L2_DL_RELEASE_REQ, l2_disconnect}, + {ST_L2_5, EV_L2_DL_DATA, l2_feed_i_if_reest}, + {ST_L2_7, EV_L2_DL_DATA, l2_feed_i_pull}, + {ST_L2_8, EV_L2_DL_DATA, l2_feed_iqueue}, + {ST_L2_1, EV_L2_DL_UNITDATA, l2_queue_ui_assign}, + {ST_L2_2, EV_L2_DL_UNITDATA, l2_queue_ui}, + {ST_L2_3, EV_L2_DL_UNITDATA, l2_queue_ui}, + {ST_L2_4, EV_L2_DL_UNITDATA, l2_send_ui}, + {ST_L2_5, EV_L2_DL_UNITDATA, l2_send_ui}, + {ST_L2_6, EV_L2_DL_UNITDATA, l2_send_ui}, + {ST_L2_7, EV_L2_DL_UNITDATA, l2_send_ui}, + {ST_L2_8, EV_L2_DL_UNITDATA, l2_send_ui}, + {ST_L2_1, EV_L2_MDL_ASSIGN, l2_got_tei}, + {ST_L2_2, EV_L2_MDL_ASSIGN, l2_got_tei}, + {ST_L2_3, EV_L2_MDL_ASSIGN, l2_got_tei}, + {ST_L2_2, EV_L2_MDL_ERROR, l2_st24_tei_remove}, + {ST_L2_3, EV_L2_MDL_ERROR, l2_st3_tei_remove}, + {ST_L2_4, EV_L2_MDL_REMOVE, l2_st24_tei_remove}, + {ST_L2_5, EV_L2_MDL_REMOVE, l2_st5_tei_remove}, + {ST_L2_6, EV_L2_MDL_REMOVE, l2_st6_tei_remove}, + {ST_L2_7, EV_L2_MDL_REMOVE, l2_tei_remove}, + {ST_L2_8, EV_L2_MDL_REMOVE, l2_tei_remove}, + {ST_L2_4, EV_L2_SABME, l2_start_multi}, + {ST_L2_5, EV_L2_SABME, l2_send_UA}, + {ST_L2_6, EV_L2_SABME, l2_send_DM}, + {ST_L2_7, EV_L2_SABME, l2_restart_multi}, + {ST_L2_8, EV_L2_SABME, l2_restart_multi}, + {ST_L2_4, EV_L2_DISC, l2_send_DM}, + {ST_L2_5, EV_L2_DISC, l2_send_DM}, + {ST_L2_6, EV_L2_DISC, l2_send_UA}, + {ST_L2_7, EV_L2_DISC, l2_stop_multi}, + {ST_L2_8, EV_L2_DISC, l2_stop_multi}, + {ST_L2_4, EV_L2_UA, l2_mdl_error_ua}, + {ST_L2_5, EV_L2_UA, l2_connected}, + {ST_L2_6, EV_L2_UA, l2_released}, + {ST_L2_7, EV_L2_UA, l2_mdl_error_ua}, + {ST_L2_8, EV_L2_UA, l2_mdl_error_ua}, + {ST_L2_4, EV_L2_DM, l2_reestablish}, + {ST_L2_5, EV_L2_DM, l2_st5_dm_release}, + {ST_L2_6, EV_L2_DM, l2_st6_dm_release}, + {ST_L2_7, EV_L2_DM, l2_mdl_error_dm}, + {ST_L2_8, EV_L2_DM, l2_st8_mdl_error_dm}, + {ST_L2_1, EV_L2_UI, l2_got_ui}, + {ST_L2_2, EV_L2_UI, l2_got_ui}, + {ST_L2_3, EV_L2_UI, l2_got_ui}, + {ST_L2_4, EV_L2_UI, l2_got_ui}, + {ST_L2_5, EV_L2_UI, l2_got_ui}, + {ST_L2_6, EV_L2_UI, l2_got_ui}, + {ST_L2_7, EV_L2_UI, l2_got_ui}, + {ST_L2_8, EV_L2_UI, l2_got_ui}, + {ST_L2_7, EV_L2_FRMR, l2_got_FRMR}, + {ST_L2_8, EV_L2_FRMR, l2_got_FRMR}, + {ST_L2_7, EV_L2_SUPER, l2_st7_got_super}, + {ST_L2_8, EV_L2_SUPER, l2_st8_got_super}, + {ST_L2_7, EV_L2_I, l2_got_iframe}, + {ST_L2_8, EV_L2_I, l2_got_iframe}, + {ST_L2_5, EV_L2_T200, l2_st5_tout_200}, + {ST_L2_6, EV_L2_T200, l2_st6_tout_200}, + {ST_L2_7, EV_L2_T200, l2_st7_tout_200}, + {ST_L2_8, EV_L2_T200, l2_st8_tout_200}, + {ST_L2_7, EV_L2_T203, l2_st7_tout_203}, + {ST_L2_7, EV_L2_ACK_PULL, l2_pull_iqueue}, + {ST_L2_7, EV_L2_SET_OWN_BUSY, l2_set_own_busy}, + {ST_L2_8, EV_L2_SET_OWN_BUSY, l2_set_own_busy}, + {ST_L2_7, EV_L2_CLEAR_OWN_BUSY, l2_clear_own_busy}, + {ST_L2_8, EV_L2_CLEAR_OWN_BUSY, l2_clear_own_busy}, + {ST_L2_4, EV_L2_FRAME_ERROR, l2_frame_error}, + {ST_L2_5, EV_L2_FRAME_ERROR, l2_frame_error}, + {ST_L2_6, EV_L2_FRAME_ERROR, l2_frame_error}, + {ST_L2_7, EV_L2_FRAME_ERROR, l2_frame_error_reest}, + {ST_L2_8, EV_L2_FRAME_ERROR, l2_frame_error_reest}, + {ST_L2_1, EV_L1_DEACTIVATE, l2_st14_persistant_da}, + {ST_L2_2, EV_L1_DEACTIVATE, l2_st24_tei_remove}, + {ST_L2_3, EV_L1_DEACTIVATE, l2_st3_tei_remove}, + {ST_L2_4, EV_L1_DEACTIVATE, l2_st14_persistant_da}, + {ST_L2_5, EV_L1_DEACTIVATE, l2_st5_persistant_da}, + {ST_L2_6, EV_L1_DEACTIVATE, l2_st6_persistant_da}, + {ST_L2_7, EV_L1_DEACTIVATE, l2_persistant_da}, + {ST_L2_8, EV_L1_DEACTIVATE, l2_persistant_da}, +}; + +#define L2_FN_COUNT (sizeof(L2FnList)/sizeof(struct FsmNode)) + +static int +ph_data_indication(struct layer2 *l2, struct mISDNhead *hh, struct sk_buff *skb) +{ + u_char *datap = skb->data; + int ret = -EINVAL; + int psapi, ptei; + u_int l; + int c = 0; + + l = l2addrsize(l2); + if (skb->len <= l) { + mISDN_FsmEvent(&l2->l2m, EV_L2_FRAME_ERROR, (void *) 'N'); + return ret; + } + if (test_bit(FLG_LAPD, &l2->flag)) { /* Maybe not needed */ + psapi = *datap++; + ptei = *datap++; + if ((psapi & 1) || !(ptei & 1)) { + printk(KERN_WARNING + "l2 D-channel frame wrong EA0/EA1\n"); + return ret; + } + psapi >>= 2; + ptei >>= 1; + if (psapi != l2->sapi) { + /* not our bussiness + * printk(KERN_DEBUG "%s: sapi %d/%d sapi mismatch\n", + * __func__, + * psapi, l2->sapi); + */ + dev_kfree_skb(skb); + return 0; + } + if ((ptei != l2->tei) && (ptei != GROUP_TEI)) { + /* not our bussiness + * printk(KERN_DEBUG "%s: tei %d/%d sapi %d mismatch\n", + * __func__, + * ptei, l2->tei, psapi); + */ + dev_kfree_skb(skb); + return 0; + } + } else + datap += l; + if (!(*datap & 1)) { /* I-Frame */ + c = iframe_error(l2, skb); + if (!c) + ret = mISDN_FsmEvent(&l2->l2m, EV_L2_I, skb); + } else if (IsSFrame(datap, l2)) { /* S-Frame */ + c = super_error(l2, skb); + if (!c) + ret = mISDN_FsmEvent(&l2->l2m, EV_L2_SUPER, skb); + } else if (IsUI(datap)) { + c = UI_error(l2, skb); + if (!c) + ret = mISDN_FsmEvent(&l2->l2m, EV_L2_UI, skb); + } else if (IsSABME(datap, l2)) { + c = unnum_error(l2, skb, CMD); + if (!c) + ret = mISDN_FsmEvent(&l2->l2m, EV_L2_SABME, skb); + } else if (IsUA(datap)) { + c = unnum_error(l2, skb, RSP); + if (!c) + ret = mISDN_FsmEvent(&l2->l2m, EV_L2_UA, skb); + } else if (IsDISC(datap)) { + c = unnum_error(l2, skb, CMD); + if (!c) + ret = mISDN_FsmEvent(&l2->l2m, EV_L2_DISC, skb); + } else if (IsDM(datap)) { + c = unnum_error(l2, skb, RSP); + if (!c) + ret = mISDN_FsmEvent(&l2->l2m, EV_L2_DM, skb); + } else if (IsFRMR(datap)) { + c = FRMR_error(l2, skb); + if (!c) + ret = mISDN_FsmEvent(&l2->l2m, EV_L2_FRMR, skb); + } else + c = 'L'; + if (c) { + printk(KERN_WARNING "l2 D-channel frame error %c\n", c); + mISDN_FsmEvent(&l2->l2m, EV_L2_FRAME_ERROR, (void *)(long)c); + } + return ret; +} + +static int +l2_send(struct mISDNchannel *ch, struct sk_buff *skb) +{ + struct layer2 *l2 = container_of(ch, struct layer2, ch); + struct mISDNhead *hh = mISDN_HEAD_P(skb); + int ret = -EINVAL; + + if (*debug & DEBUG_L2_RECV) + printk(KERN_DEBUG "%s: prim(%x) id(%x) tei(%d)\n", + __func__, hh->prim, hh->id, l2->tei); + switch (hh->prim) { + case PH_DATA_IND: + ret = ph_data_indication(l2, hh, skb); + break; + case PH_DATA_CNF: + ret = ph_data_confirm(l2, hh, skb); + break; + case PH_ACTIVATE_IND: + test_and_set_bit(FLG_L1_ACTIV, &l2->flag); + l2up_create(l2, MPH_ACTIVATE_IND, 0, NULL); + if (test_and_clear_bit(FLG_ESTAB_PEND, &l2->flag)) + ret = mISDN_FsmEvent(&l2->l2m, + EV_L2_DL_ESTABLISH_REQ, skb); + break; + case PH_DEACTIVATE_IND: + test_and_clear_bit(FLG_L1_ACTIV, &l2->flag); + l2up_create(l2, MPH_DEACTIVATE_IND, 0, NULL); + ret = mISDN_FsmEvent(&l2->l2m, EV_L1_DEACTIVATE, skb); + break; + case MPH_INFORMATION_IND: + if (!l2->up) + break; + ret = l2->up->send(l2->up, skb); + break; + case DL_DATA_REQ: + ret = mISDN_FsmEvent(&l2->l2m, EV_L2_DL_DATA, skb); + break; + case DL_UNITDATA_REQ: + ret = mISDN_FsmEvent(&l2->l2m, EV_L2_DL_UNITDATA, skb); + break; + case DL_ESTABLISH_REQ: + if (test_bit(FLG_LAPB, &l2->flag)) + test_and_set_bit(FLG_ORIG, &l2->flag); + if (test_bit(FLG_L1_ACTIV, &l2->flag)) { + if (test_bit(FLG_LAPD, &l2->flag) || + test_bit(FLG_ORIG, &l2->flag)) + ret = mISDN_FsmEvent(&l2->l2m, + EV_L2_DL_ESTABLISH_REQ, skb); + } else { + if (test_bit(FLG_LAPD, &l2->flag) || + test_bit(FLG_ORIG, &l2->flag)) { + test_and_set_bit(FLG_ESTAB_PEND, + &l2->flag); + } + ret = l2down(l2, PH_ACTIVATE_REQ, l2_newid(l2), + skb); + } + break; + case DL_RELEASE_REQ: + if (test_bit(FLG_LAPB, &l2->flag)) + l2down_create(l2, PH_DEACTIVATE_REQ, + l2_newid(l2), 0, NULL); + ret = mISDN_FsmEvent(&l2->l2m, EV_L2_DL_RELEASE_REQ, + skb); + break; + default: + if (*debug & DEBUG_L2) + l2m_debug(&l2->l2m, "l2 unknown pr %04x", + hh->prim); + } + if (ret) { + dev_kfree_skb(skb); + ret = 0; + } + return ret; +} + +int +tei_l2(struct layer2 *l2, u_int cmd, u_long arg) +{ + int ret = -EINVAL; + + if (*debug & DEBUG_L2_TEI) + printk(KERN_DEBUG "%s: cmd(%x)\n", __func__, cmd); + switch (cmd) { + case (MDL_ASSIGN_REQ): + ret = mISDN_FsmEvent(&l2->l2m, EV_L2_MDL_ASSIGN, (void *)arg); + break; + case (MDL_REMOVE_REQ): + ret = mISDN_FsmEvent(&l2->l2m, EV_L2_MDL_REMOVE, NULL); + break; + case (MDL_ERROR_IND): + ret = mISDN_FsmEvent(&l2->l2m, EV_L2_MDL_ERROR, NULL); + break; + case (MDL_ERROR_RSP): + /* ETS 300-125 5.3.2.1 Test: TC13010 */ + printk(KERN_NOTICE "MDL_ERROR|REQ (tei_l2)\n"); + ret = mISDN_FsmEvent(&l2->l2m, EV_L2_MDL_ERROR, NULL); + break; + } + return ret; +} + +static void +release_l2(struct layer2 *l2) +{ + mISDN_FsmDelTimer(&l2->t200, 21); + mISDN_FsmDelTimer(&l2->t203, 16); + skb_queue_purge(&l2->i_queue); + skb_queue_purge(&l2->ui_queue); + skb_queue_purge(&l2->down_queue); + ReleaseWin(l2); + if (test_bit(FLG_LAPD, &l2->flag)) { + TEIrelease(l2); + if (l2->ch.st) + l2->ch.st->dev->D.ctrl(&l2->ch.st->dev->D, + CLOSE_CHANNEL, NULL); + } + kfree(l2); +} + +static int +l2_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg) +{ + struct layer2 *l2 = container_of(ch, struct layer2, ch); + u_int info; + + if (*debug & DEBUG_L2_CTRL) + printk(KERN_DEBUG "%s:(%x)\n", __func__, cmd); + + switch (cmd) { + case OPEN_CHANNEL: + if (test_bit(FLG_LAPD, &l2->flag)) { + set_channel_address(&l2->ch, l2->sapi, l2->tei); + info = DL_INFO_L2_CONNECT; + l2up_create(l2, DL_INFORMATION_IND, + sizeof(info), &info); + } + break; + case CLOSE_CHANNEL: + if (l2->ch.peer) + l2->ch.peer->ctrl(l2->ch.peer, CLOSE_CHANNEL, NULL); + release_l2(l2); + break; + } + return 0; +} + +struct layer2 * +create_l2(struct mISDNchannel *ch, u_int protocol, u_long options, u_long arg) +{ + struct layer2 *l2; + struct channel_req rq; + + l2 = kzalloc(sizeof(struct layer2), GFP_KERNEL); + if (!l2) { + printk(KERN_ERR "kzalloc layer2 failed\n"); + return NULL; + } + l2->next_id = 1; + l2->down_id = MISDN_ID_NONE; + l2->up = ch; + l2->ch.st = ch->st; + l2->ch.send = l2_send; + l2->ch.ctrl = l2_ctrl; + switch (protocol) { + case ISDN_P_LAPD_NT: + test_and_set_bit(FLG_LAPD, &l2->flag); + test_and_set_bit(FLG_LAPD_NET, &l2->flag); + test_and_set_bit(FLG_MOD128, &l2->flag); + l2->sapi = 0; + l2->maxlen = MAX_DFRAME_LEN; + if (test_bit(OPTION_L2_PMX, &options)) + l2->window = 7; + else + l2->window = 1; + if (test_bit(OPTION_L2_PTP, &options)) + test_and_set_bit(FLG_PTP, &l2->flag); + if (test_bit(OPTION_L2_FIXEDTEI, &options)) + test_and_set_bit(FLG_FIXED_TEI, &l2->flag); + l2->tei = (u_int)arg; + l2->T200 = 1000; + l2->N200 = 3; + l2->T203 = 10000; + if (test_bit(OPTION_L2_PMX, &options)) + rq.protocol = ISDN_P_NT_E1; + else + rq.protocol = ISDN_P_NT_S0; + rq.adr.channel = 0; + l2->ch.st->dev->D.ctrl(&l2->ch.st->dev->D, OPEN_CHANNEL, &rq); + break; + case ISDN_P_LAPD_TE: + test_and_set_bit(FLG_LAPD, &l2->flag); + test_and_set_bit(FLG_MOD128, &l2->flag); + test_and_set_bit(FLG_ORIG, &l2->flag); + l2->sapi = 0; + l2->maxlen = MAX_DFRAME_LEN; + if (test_bit(OPTION_L2_PMX, &options)) + l2->window = 7; + else + l2->window = 1; + if (test_bit(OPTION_L2_PTP, &options)) + test_and_set_bit(FLG_PTP, &l2->flag); + if (test_bit(OPTION_L2_FIXEDTEI, &options)) + test_and_set_bit(FLG_FIXED_TEI, &l2->flag); + l2->tei = (u_int)arg; + l2->T200 = 1000; + l2->N200 = 3; + l2->T203 = 10000; + if (test_bit(OPTION_L2_PMX, &options)) + rq.protocol = ISDN_P_TE_E1; + else + rq.protocol = ISDN_P_TE_S0; + rq.adr.channel = 0; + l2->ch.st->dev->D.ctrl(&l2->ch.st->dev->D, OPEN_CHANNEL, &rq); + break; + case ISDN_P_B_X75SLP: + test_and_set_bit(FLG_LAPB, &l2->flag); + l2->window = 7; + l2->maxlen = MAX_DATA_SIZE; + l2->T200 = 1000; + l2->N200 = 4; + l2->T203 = 5000; + l2->addr.A = 3; + l2->addr.B = 1; + break; + default: + printk(KERN_ERR "layer2 create failed prt %x\n", + protocol); + kfree(l2); + return NULL; + } + skb_queue_head_init(&l2->i_queue); + skb_queue_head_init(&l2->ui_queue); + skb_queue_head_init(&l2->down_queue); + skb_queue_head_init(&l2->tmp_queue); + InitWin(l2); + l2->l2m.fsm = &l2fsm; + if (test_bit(FLG_LAPB, &l2->flag) || + test_bit(FLG_PTP, &l2->flag) || + test_bit(FLG_LAPD_NET, &l2->flag)) + l2->l2m.state = ST_L2_4; + else + l2->l2m.state = ST_L2_1; + l2->l2m.debug = *debug; + l2->l2m.userdata = l2; + l2->l2m.userint = 0; + l2->l2m.printdebug = l2m_debug; + + mISDN_FsmInitTimer(&l2->l2m, &l2->t200); + mISDN_FsmInitTimer(&l2->l2m, &l2->t203); + return l2; +} + +static int +x75create(struct channel_req *crq) +{ + struct layer2 *l2; + + if (crq->protocol != ISDN_P_B_X75SLP) + return -EPROTONOSUPPORT; + l2 = create_l2(crq->ch, crq->protocol, 0, 0); + if (!l2) + return -ENOMEM; + crq->ch = &l2->ch; + crq->protocol = ISDN_P_B_HDLC; + return 0; +} + +static struct Bprotocol X75SLP = { + .Bprotocols = (1 << (ISDN_P_B_X75SLP & ISDN_P_B_MASK)), + .name = "X75SLP", + .create = x75create +}; + +int +Isdnl2_Init(u_int *deb) +{ + debug = deb; + mISDN_register_Bprotocol(&X75SLP); + l2fsm.state_count = L2_STATE_COUNT; + l2fsm.event_count = L2_EVENT_COUNT; + l2fsm.strEvent = strL2Event; + l2fsm.strState = strL2State; + mISDN_FsmNew(&l2fsm, L2FnList, ARRAY_SIZE(L2FnList)); + TEIInit(deb); + return 0; +} + +void +Isdnl2_cleanup(void) +{ + mISDN_unregister_Bprotocol(&X75SLP); + TEIFree(); + mISDN_FsmFree(&l2fsm); +} + diff --git a/drivers/isdn/mISDN/layer2.h b/drivers/isdn/mISDN/layer2.h new file mode 100644 index 000000000000..6293f80dc2d3 --- /dev/null +++ b/drivers/isdn/mISDN/layer2.h @@ -0,0 +1,140 @@ +/* + * Layer 2 defines + * + * Copyright 2008 by Karsten Keil <kkeil@novell.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#include <linux/mISDNif.h> +#include <linux/skbuff.h> +#include "fsm.h" + +#define MAX_WINDOW 8 + +struct manager { + struct mISDNchannel ch; + struct mISDNchannel bcast; + u_long options; + struct list_head layer2; + rwlock_t lock; + struct FsmInst deact; + struct FsmTimer datimer; + struct sk_buff_head sendq; + struct mISDNchannel *up; + u_int nextid; + u_int lastid; +}; + +struct teimgr { + int ri; + int rcnt; + struct FsmInst tei_m; + struct FsmTimer timer; + int tval, nval; + struct layer2 *l2; + struct manager *mgr; +}; + +struct laddr { + u_char A; + u_char B; +}; + +struct layer2 { + struct list_head list; + struct mISDNchannel ch; + u_long flag; + int id; + struct mISDNchannel *up; + signed char sapi; + signed char tei; + struct laddr addr; + u_int maxlen; + struct teimgr *tm; + u_int vs, va, vr; + int rc; + u_int window; + u_int sow; + struct FsmInst l2m; + struct FsmTimer t200, t203; + int T200, N200, T203; + u_int next_id; + u_int down_id; + struct sk_buff *windowar[MAX_WINDOW]; + struct sk_buff_head i_queue; + struct sk_buff_head ui_queue; + struct sk_buff_head down_queue; + struct sk_buff_head tmp_queue; +}; + +enum { + ST_L2_1, + ST_L2_2, + ST_L2_3, + ST_L2_4, + ST_L2_5, + ST_L2_6, + ST_L2_7, + ST_L2_8, +}; + +#define L2_STATE_COUNT (ST_L2_8+1) + +extern struct layer2 *create_l2(struct mISDNchannel *, u_int, + u_long, u_long); +extern int tei_l2(struct layer2 *, u_int, u_long arg); + + +/* from tei.c */ +extern int l2_tei(struct layer2 *, u_int, u_long arg); +extern void TEIrelease(struct layer2 *); +extern int TEIInit(u_int *); +extern void TEIFree(void); + +#define MAX_L2HEADER_LEN 4 + +#define RR 0x01 +#define RNR 0x05 +#define REJ 0x09 +#define SABME 0x6f +#define SABM 0x2f +#define DM 0x0f +#define UI 0x03 +#define DISC 0x43 +#define UA 0x63 +#define FRMR 0x87 +#define XID 0xaf + +#define CMD 0 +#define RSP 1 + +#define LC_FLUSH_WAIT 1 + +#define FLG_LAPB 0 +#define FLG_LAPD 1 +#define FLG_ORIG 2 +#define FLG_MOD128 3 +#define FLG_PEND_REL 4 +#define FLG_L3_INIT 5 +#define FLG_T200_RUN 6 +#define FLG_ACK_PEND 7 +#define FLG_REJEXC 8 +#define FLG_OWN_BUSY 9 +#define FLG_PEER_BUSY 10 +#define FLG_DCHAN_BUSY 11 +#define FLG_L1_ACTIV 12 +#define FLG_ESTAB_PEND 13 +#define FLG_PTP 14 +#define FLG_FIXED_TEI 15 +#define FLG_L2BLOCK 16 +#define FLG_L1_NOTREADY 17 +#define FLG_LAPD_NET 18 diff --git a/drivers/isdn/mISDN/socket.c b/drivers/isdn/mISDN/socket.c new file mode 100644 index 000000000000..4ba4cc364c9e --- /dev/null +++ b/drivers/isdn/mISDN/socket.c @@ -0,0 +1,781 @@ +/* + * + * Author Karsten Keil <kkeil@novell.com> + * + * Copyright 2008 by Karsten Keil <kkeil@novell.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#include <linux/mISDNif.h> +#include "core.h" + +static int *debug; + +static struct proto mISDN_proto = { + .name = "misdn", + .owner = THIS_MODULE, + .obj_size = sizeof(struct mISDN_sock) +}; + +#define _pms(sk) ((struct mISDN_sock *)sk) + +static struct mISDN_sock_list data_sockets = { + .lock = __RW_LOCK_UNLOCKED(data_sockets.lock) +}; + +static struct mISDN_sock_list base_sockets = { + .lock = __RW_LOCK_UNLOCKED(base_sockets.lock) +}; + +#define L2_HEADER_LEN 4 + +static inline struct sk_buff * +_l2_alloc_skb(unsigned int len, gfp_t gfp_mask) +{ + struct sk_buff *skb; + + skb = alloc_skb(len + L2_HEADER_LEN, gfp_mask); + if (likely(skb)) + skb_reserve(skb, L2_HEADER_LEN); + return skb; +} + +static void +mISDN_sock_link(struct mISDN_sock_list *l, struct sock *sk) +{ + write_lock_bh(&l->lock); + sk_add_node(sk, &l->head); + write_unlock_bh(&l->lock); +} + +static void mISDN_sock_unlink(struct mISDN_sock_list *l, struct sock *sk) +{ + write_lock_bh(&l->lock); + sk_del_node_init(sk); + write_unlock_bh(&l->lock); +} + +static int +mISDN_send(struct mISDNchannel *ch, struct sk_buff *skb) +{ + struct mISDN_sock *msk; + int err; + + msk = container_of(ch, struct mISDN_sock, ch); + if (*debug & DEBUG_SOCKET) + printk(KERN_DEBUG "%s len %d %p\n", __func__, skb->len, skb); + if (msk->sk.sk_state == MISDN_CLOSED) + return -EUNATCH; + __net_timestamp(skb); + err = sock_queue_rcv_skb(&msk->sk, skb); + if (err) + printk(KERN_WARNING "%s: error %d\n", __func__, err); + return err; +} + +static int +mISDN_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg) +{ + struct mISDN_sock *msk; + + msk = container_of(ch, struct mISDN_sock, ch); + if (*debug & DEBUG_SOCKET) + printk(KERN_DEBUG "%s(%p, %x, %p)\n", __func__, ch, cmd, arg); + switch (cmd) { + case CLOSE_CHANNEL: + msk->sk.sk_state = MISDN_CLOSED; + break; + } + return 0; +} + +static inline void +mISDN_sock_cmsg(struct sock *sk, struct msghdr *msg, struct sk_buff *skb) +{ + struct timeval tv; + + if (_pms(sk)->cmask & MISDN_TIME_STAMP) { + skb_get_timestamp(skb, &tv); + put_cmsg(msg, SOL_MISDN, MISDN_TIME_STAMP, sizeof(tv), &tv); + } +} + +static int +mISDN_sock_recvmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *msg, size_t len, int flags) +{ + struct sk_buff *skb; + struct sock *sk = sock->sk; + struct sockaddr_mISDN *maddr; + + int copied, err; + + if (*debug & DEBUG_SOCKET) + printk(KERN_DEBUG "%s: len %d, flags %x ch.nr %d, proto %x\n", + __func__, (int)len, flags, _pms(sk)->ch.nr, + sk->sk_protocol); + if (flags & (MSG_OOB)) + return -EOPNOTSUPP; + + if (sk->sk_state == MISDN_CLOSED) + return 0; + + skb = skb_recv_datagram(sk, flags, flags & MSG_DONTWAIT, &err); + if (!skb) + return err; + + if (msg->msg_namelen >= sizeof(struct sockaddr_mISDN)) { + msg->msg_namelen = sizeof(struct sockaddr_mISDN); + maddr = (struct sockaddr_mISDN *)msg->msg_name; + maddr->family = AF_ISDN; + maddr->dev = _pms(sk)->dev->id; + if ((sk->sk_protocol == ISDN_P_LAPD_TE) || + (sk->sk_protocol == ISDN_P_LAPD_NT)) { + maddr->channel = (mISDN_HEAD_ID(skb) >> 16) & 0xff; + maddr->tei = (mISDN_HEAD_ID(skb) >> 8) & 0xff; + maddr->sapi = mISDN_HEAD_ID(skb) & 0xff; + } else { + maddr->channel = _pms(sk)->ch.nr; + maddr->sapi = _pms(sk)->ch.addr & 0xFF; + maddr->tei = (_pms(sk)->ch.addr >> 8) & 0xFF; + } + } else { + if (msg->msg_namelen) + printk(KERN_WARNING "%s: too small namelen %d\n", + __func__, msg->msg_namelen); + msg->msg_namelen = 0; + } + + copied = skb->len + MISDN_HEADER_LEN; + if (len < copied) { + if (flags & MSG_PEEK) + atomic_dec(&skb->users); + else + skb_queue_head(&sk->sk_receive_queue, skb); + return -ENOSPC; + } + memcpy(skb_push(skb, MISDN_HEADER_LEN), mISDN_HEAD_P(skb), + MISDN_HEADER_LEN); + + err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied); + + mISDN_sock_cmsg(sk, msg, skb); + + skb_free_datagram(sk, skb); + + return err ? : copied; +} + +static int +mISDN_sock_sendmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *msg, size_t len) +{ + struct sock *sk = sock->sk; + struct sk_buff *skb; + int err = -ENOMEM; + struct sockaddr_mISDN *maddr; + + if (*debug & DEBUG_SOCKET) + printk(KERN_DEBUG "%s: len %d flags %x ch %d proto %x\n", + __func__, (int)len, msg->msg_flags, _pms(sk)->ch.nr, + sk->sk_protocol); + + if (msg->msg_flags & MSG_OOB) + return -EOPNOTSUPP; + + if (msg->msg_flags & ~(MSG_DONTWAIT|MSG_NOSIGNAL|MSG_ERRQUEUE)) + return -EINVAL; + + if (len < MISDN_HEADER_LEN) + return -EINVAL; + + if (sk->sk_state != MISDN_BOUND) + return -EBADFD; + + lock_sock(sk); + + skb = _l2_alloc_skb(len, GFP_KERNEL); + if (!skb) + goto done; + + if (memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len)) { + err = -EFAULT; + goto drop; + } + + memcpy(mISDN_HEAD_P(skb), skb->data, MISDN_HEADER_LEN); + skb_pull(skb, MISDN_HEADER_LEN); + + if (msg->msg_namelen >= sizeof(struct sockaddr_mISDN)) { + /* if we have a address, we use it */ + maddr = (struct sockaddr_mISDN *)msg->msg_name; + mISDN_HEAD_ID(skb) = maddr->channel; + } else { /* use default for L2 messages */ + if ((sk->sk_protocol == ISDN_P_LAPD_TE) || + (sk->sk_protocol == ISDN_P_LAPD_NT)) + mISDN_HEAD_ID(skb) = _pms(sk)->ch.nr; + } + + if (*debug & DEBUG_SOCKET) + printk(KERN_DEBUG "%s: ID:%x\n", + __func__, mISDN_HEAD_ID(skb)); + + err = -ENODEV; + if (!_pms(sk)->ch.peer || + (err = _pms(sk)->ch.recv(_pms(sk)->ch.peer, skb))) + goto drop; + + err = len; + +done: + release_sock(sk); + return err; + +drop: + kfree_skb(skb); + goto done; +} + +static int +data_sock_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + + if (*debug & DEBUG_SOCKET) + printk(KERN_DEBUG "%s(%p) sk=%p\n", __func__, sock, sk); + if (!sk) + return 0; + switch (sk->sk_protocol) { + case ISDN_P_TE_S0: + case ISDN_P_NT_S0: + case ISDN_P_TE_E1: + case ISDN_P_NT_E1: + if (sk->sk_state == MISDN_BOUND) + delete_channel(&_pms(sk)->ch); + else + mISDN_sock_unlink(&data_sockets, sk); + break; + case ISDN_P_LAPD_TE: + case ISDN_P_LAPD_NT: + case ISDN_P_B_RAW: + case ISDN_P_B_HDLC: + case ISDN_P_B_X75SLP: + case ISDN_P_B_L2DTMF: + case ISDN_P_B_L2DSP: + case ISDN_P_B_L2DSPHDLC: + delete_channel(&_pms(sk)->ch); + mISDN_sock_unlink(&data_sockets, sk); + break; + } + + lock_sock(sk); + + sock_orphan(sk); + skb_queue_purge(&sk->sk_receive_queue); + + release_sock(sk); + sock_put(sk); + + return 0; +} + +static int +data_sock_ioctl_bound(struct sock *sk, unsigned int cmd, void __user *p) +{ + struct mISDN_ctrl_req cq; + int err = -EINVAL, val; + struct mISDNchannel *bchan, *next; + + lock_sock(sk); + if (!_pms(sk)->dev) { + err = -ENODEV; + goto done; + } + switch (cmd) { + case IMCTRLREQ: + if (copy_from_user(&cq, p, sizeof(cq))) { + err = -EFAULT; + break; + } + if ((sk->sk_protocol & ~ISDN_P_B_MASK) == ISDN_P_B_START) { + list_for_each_entry_safe(bchan, next, + &_pms(sk)->dev->bchannels, list) { + if (bchan->nr == cq.channel) { + err = bchan->ctrl(bchan, + CONTROL_CHANNEL, &cq); + break; + } + } + } else + err = _pms(sk)->dev->D.ctrl(&_pms(sk)->dev->D, + CONTROL_CHANNEL, &cq); + if (err) + break; + if (copy_to_user(p, &cq, sizeof(cq))) + err = -EFAULT; + break; + case IMCLEAR_L2: + if (sk->sk_protocol != ISDN_P_LAPD_NT) { + err = -EINVAL; + break; + } + if (get_user(val, (int __user *)p)) { + err = -EFAULT; + break; + } + err = _pms(sk)->dev->teimgr->ctrl(_pms(sk)->dev->teimgr, + CONTROL_CHANNEL, &val); + break; + default: + err = -EINVAL; + break; + } +done: + release_sock(sk); + return err; +} + +static int +data_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) +{ + int err = 0, id; + struct sock *sk = sock->sk; + struct mISDNdevice *dev; + struct mISDNversion ver; + + switch (cmd) { + case IMGETVERSION: + ver.major = MISDN_MAJOR_VERSION; + ver.minor = MISDN_MINOR_VERSION; + ver.release = MISDN_RELEASE; + if (copy_to_user((void __user *)arg, &ver, sizeof(ver))) + err = -EFAULT; + break; + case IMGETCOUNT: + id = get_mdevice_count(); + if (put_user(id, (int __user *)arg)) + err = -EFAULT; + break; + case IMGETDEVINFO: + if (get_user(id, (int __user *)arg)) { + err = -EFAULT; + break; + } + dev = get_mdevice(id); + if (dev) { + struct mISDN_devinfo di; + + di.id = dev->id; + di.Dprotocols = dev->Dprotocols; + di.Bprotocols = dev->Bprotocols | get_all_Bprotocols(); + di.protocol = dev->D.protocol; + memcpy(di.channelmap, dev->channelmap, + MISDN_CHMAP_SIZE * 4); + di.nrbchan = dev->nrbchan; + strcpy(di.name, dev->name); + if (copy_to_user((void __user *)arg, &di, sizeof(di))) + err = -EFAULT; + } else + err = -ENODEV; + break; + default: + if (sk->sk_state == MISDN_BOUND) + err = data_sock_ioctl_bound(sk, cmd, + (void __user *)arg); + else + err = -ENOTCONN; + } + return err; +} + +static int data_sock_setsockopt(struct socket *sock, int level, int optname, + char __user *optval, int len) +{ + struct sock *sk = sock->sk; + int err = 0, opt = 0; + + if (*debug & DEBUG_SOCKET) + printk(KERN_DEBUG "%s(%p, %d, %x, %p, %d)\n", __func__, sock, + level, optname, optval, len); + + lock_sock(sk); + + switch (optname) { + case MISDN_TIME_STAMP: + if (get_user(opt, (int __user *)optval)) { + err = -EFAULT; + break; + } + + if (opt) + _pms(sk)->cmask |= MISDN_TIME_STAMP; + else + _pms(sk)->cmask &= ~MISDN_TIME_STAMP; + break; + default: + err = -ENOPROTOOPT; + break; + } + release_sock(sk); + return err; +} + +static int data_sock_getsockopt(struct socket *sock, int level, int optname, + char __user *optval, int __user *optlen) +{ + struct sock *sk = sock->sk; + int len, opt; + + if (get_user(len, optlen)) + return -EFAULT; + + switch (optname) { + case MISDN_TIME_STAMP: + if (_pms(sk)->cmask & MISDN_TIME_STAMP) + opt = 1; + else + opt = 0; + + if (put_user(opt, optval)) + return -EFAULT; + break; + default: + return -ENOPROTOOPT; + } + + return 0; +} + +static int +data_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_len) +{ + struct sockaddr_mISDN *maddr = (struct sockaddr_mISDN *) addr; + struct sock *sk = sock->sk; + int err = 0; + + if (*debug & DEBUG_SOCKET) + printk(KERN_DEBUG "%s(%p) sk=%p\n", __func__, sock, sk); + if (addr_len != sizeof(struct sockaddr_mISDN)) + return -EINVAL; + if (!maddr || maddr->family != AF_ISDN) + return -EINVAL; + + lock_sock(sk); + + if (_pms(sk)->dev) { + err = -EALREADY; + goto done; + } + _pms(sk)->dev = get_mdevice(maddr->dev); + if (!_pms(sk)->dev) { + err = -ENODEV; + goto done; + } + _pms(sk)->ch.send = mISDN_send; + _pms(sk)->ch.ctrl = mISDN_ctrl; + + switch (sk->sk_protocol) { + case ISDN_P_TE_S0: + case ISDN_P_NT_S0: + case ISDN_P_TE_E1: + case ISDN_P_NT_E1: + mISDN_sock_unlink(&data_sockets, sk); + err = connect_layer1(_pms(sk)->dev, &_pms(sk)->ch, + sk->sk_protocol, maddr); + if (err) + mISDN_sock_link(&data_sockets, sk); + break; + case ISDN_P_LAPD_TE: + case ISDN_P_LAPD_NT: + err = create_l2entity(_pms(sk)->dev, &_pms(sk)->ch, + sk->sk_protocol, maddr); + break; + case ISDN_P_B_RAW: + case ISDN_P_B_HDLC: + case ISDN_P_B_X75SLP: + case ISDN_P_B_L2DTMF: + case ISDN_P_B_L2DSP: + case ISDN_P_B_L2DSPHDLC: + err = connect_Bstack(_pms(sk)->dev, &_pms(sk)->ch, + sk->sk_protocol, maddr); + break; + default: + err = -EPROTONOSUPPORT; + } + if (err) + goto done; + sk->sk_state = MISDN_BOUND; + _pms(sk)->ch.protocol = sk->sk_protocol; + +done: + release_sock(sk); + return err; +} + +static int +data_sock_getname(struct socket *sock, struct sockaddr *addr, + int *addr_len, int peer) +{ + struct sockaddr_mISDN *maddr = (struct sockaddr_mISDN *) addr; + struct sock *sk = sock->sk; + + if (!_pms(sk)->dev) + return -EBADFD; + + lock_sock(sk); + + *addr_len = sizeof(*maddr); + maddr->dev = _pms(sk)->dev->id; + maddr->channel = _pms(sk)->ch.nr; + maddr->sapi = _pms(sk)->ch.addr & 0xff; + maddr->tei = (_pms(sk)->ch.addr >> 8) & 0xff; + release_sock(sk); + return 0; +} + +static const struct proto_ops data_sock_ops = { + .family = PF_ISDN, + .owner = THIS_MODULE, + .release = data_sock_release, + .ioctl = data_sock_ioctl, + .bind = data_sock_bind, + .getname = data_sock_getname, + .sendmsg = mISDN_sock_sendmsg, + .recvmsg = mISDN_sock_recvmsg, + .poll = datagram_poll, + .listen = sock_no_listen, + .shutdown = sock_no_shutdown, + .setsockopt = data_sock_setsockopt, + .getsockopt = data_sock_getsockopt, + .connect = sock_no_connect, + .socketpair = sock_no_socketpair, + .accept = sock_no_accept, + .mmap = sock_no_mmap +}; + +static int +data_sock_create(struct net *net, struct socket *sock, int protocol) +{ + struct sock *sk; + + if (sock->type != SOCK_DGRAM) + return -ESOCKTNOSUPPORT; + + sk = sk_alloc(net, PF_ISDN, GFP_KERNEL, &mISDN_proto); + if (!sk) + return -ENOMEM; + + sock_init_data(sock, sk); + + sock->ops = &data_sock_ops; + sock->state = SS_UNCONNECTED; + sock_reset_flag(sk, SOCK_ZAPPED); + + sk->sk_protocol = protocol; + sk->sk_state = MISDN_OPEN; + mISDN_sock_link(&data_sockets, sk); + + return 0; +} + +static int +base_sock_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + + printk(KERN_DEBUG "%s(%p) sk=%p\n", __func__, sock, sk); + if (!sk) + return 0; + + mISDN_sock_unlink(&base_sockets, sk); + sock_orphan(sk); + sock_put(sk); + + return 0; +} + +static int +base_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) +{ + int err = 0, id; + struct mISDNdevice *dev; + struct mISDNversion ver; + + switch (cmd) { + case IMGETVERSION: + ver.major = MISDN_MAJOR_VERSION; + ver.minor = MISDN_MINOR_VERSION; + ver.release = MISDN_RELEASE; + if (copy_to_user((void __user *)arg, &ver, sizeof(ver))) + err = -EFAULT; + break; + case IMGETCOUNT: + id = get_mdevice_count(); + if (put_user(id, (int __user *)arg)) + err = -EFAULT; + break; + case IMGETDEVINFO: + if (get_user(id, (int __user *)arg)) { + err = -EFAULT; + break; + } + dev = get_mdevice(id); + if (dev) { + struct mISDN_devinfo di; + + di.id = dev->id; + di.Dprotocols = dev->Dprotocols; + di.Bprotocols = dev->Bprotocols | get_all_Bprotocols(); + di.protocol = dev->D.protocol; + memcpy(di.channelmap, dev->channelmap, + MISDN_CHMAP_SIZE * 4); + di.nrbchan = dev->nrbchan; + strcpy(di.name, dev->name); + if (copy_to_user((void __user *)arg, &di, sizeof(di))) + err = -EFAULT; + } else + err = -ENODEV; + break; + default: + err = -EINVAL; + } + return err; +} + +static int +base_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_len) +{ + struct sockaddr_mISDN *maddr = (struct sockaddr_mISDN *) addr; + struct sock *sk = sock->sk; + int err = 0; + + if (!maddr || maddr->family != AF_ISDN) + return -EINVAL; + + lock_sock(sk); + + if (_pms(sk)->dev) { + err = -EALREADY; + goto done; + } + + _pms(sk)->dev = get_mdevice(maddr->dev); + if (!_pms(sk)->dev) { + err = -ENODEV; + goto done; + } + sk->sk_state = MISDN_BOUND; + +done: + release_sock(sk); + return err; +} + +static const struct proto_ops base_sock_ops = { + .family = PF_ISDN, + .owner = THIS_MODULE, + .release = base_sock_release, + .ioctl = base_sock_ioctl, + .bind = base_sock_bind, + .getname = sock_no_getname, + .sendmsg = sock_no_sendmsg, + .recvmsg = sock_no_recvmsg, + .poll = sock_no_poll, + .listen = sock_no_listen, + .shutdown = sock_no_shutdown, + .setsockopt = sock_no_setsockopt, + .getsockopt = sock_no_getsockopt, + .connect = sock_no_connect, + .socketpair = sock_no_socketpair, + .accept = sock_no_accept, + .mmap = sock_no_mmap +}; + + +static int +base_sock_create(struct net *net, struct socket *sock, int protocol) +{ + struct sock *sk; + + if (sock->type != SOCK_RAW) + return -ESOCKTNOSUPPORT; + + sk = sk_alloc(net, PF_ISDN, GFP_KERNEL, &mISDN_proto); + if (!sk) + return -ENOMEM; + + sock_init_data(sock, sk); + sock->ops = &base_sock_ops; + sock->state = SS_UNCONNECTED; + sock_reset_flag(sk, SOCK_ZAPPED); + sk->sk_protocol = protocol; + sk->sk_state = MISDN_OPEN; + mISDN_sock_link(&base_sockets, sk); + + return 0; +} + +static int +mISDN_sock_create(struct net *net, struct socket *sock, int proto) +{ + int err = -EPROTONOSUPPORT; + + switch (proto) { + case ISDN_P_BASE: + err = base_sock_create(net, sock, proto); + break; + case ISDN_P_TE_S0: + case ISDN_P_NT_S0: + case ISDN_P_TE_E1: + case ISDN_P_NT_E1: + case ISDN_P_LAPD_TE: + case ISDN_P_LAPD_NT: + case ISDN_P_B_RAW: + case ISDN_P_B_HDLC: + case ISDN_P_B_X75SLP: + case ISDN_P_B_L2DTMF: + case ISDN_P_B_L2DSP: + case ISDN_P_B_L2DSPHDLC: + err = data_sock_create(net, sock, proto); + break; + default: + return err; + } + + return err; +} + +static struct +net_proto_family mISDN_sock_family_ops = { + .owner = THIS_MODULE, + .family = PF_ISDN, + .create = mISDN_sock_create, +}; + +int +misdn_sock_init(u_int *deb) +{ + int err; + + debug = deb; + err = sock_register(&mISDN_sock_family_ops); + if (err) + printk(KERN_ERR "%s: error(%d)\n", __func__, err); + return err; +} + +void +misdn_sock_cleanup(void) +{ + sock_unregister(PF_ISDN); +} + diff --git a/drivers/isdn/mISDN/stack.c b/drivers/isdn/mISDN/stack.c new file mode 100644 index 000000000000..54cfddcc4784 --- /dev/null +++ b/drivers/isdn/mISDN/stack.c @@ -0,0 +1,674 @@ +/* + * + * Author Karsten Keil <kkeil@novell.com> + * + * Copyright 2008 by Karsten Keil <kkeil@novell.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#include <linux/mISDNif.h> +#include <linux/kthread.h> +#include "core.h" + +static u_int *debug; + +static inline void +_queue_message(struct mISDNstack *st, struct sk_buff *skb) +{ + struct mISDNhead *hh = mISDN_HEAD_P(skb); + + if (*debug & DEBUG_QUEUE_FUNC) + printk(KERN_DEBUG "%s prim(%x) id(%x) %p\n", + __func__, hh->prim, hh->id, skb); + skb_queue_tail(&st->msgq, skb); + if (likely(!test_bit(mISDN_STACK_STOPPED, &st->status))) { + test_and_set_bit(mISDN_STACK_WORK, &st->status); + wake_up_interruptible(&st->workq); + } +} + +int +mISDN_queue_message(struct mISDNchannel *ch, struct sk_buff *skb) +{ + _queue_message(ch->st, skb); + return 0; +} + +static struct mISDNchannel * +get_channel4id(struct mISDNstack *st, u_int id) +{ + struct mISDNchannel *ch; + + mutex_lock(&st->lmutex); + list_for_each_entry(ch, &st->layer2, list) { + if (id == ch->nr) + goto unlock; + } + ch = NULL; +unlock: + mutex_unlock(&st->lmutex); + return ch; +} + +static void +send_socklist(struct mISDN_sock_list *sl, struct sk_buff *skb) +{ + struct hlist_node *node; + struct sock *sk; + struct sk_buff *cskb = NULL; + + read_lock(&sl->lock); + sk_for_each(sk, node, &sl->head) { + if (sk->sk_state != MISDN_BOUND) + continue; + if (!cskb) + cskb = skb_copy(skb, GFP_KERNEL); + if (!cskb) { + printk(KERN_WARNING "%s no skb\n", __func__); + break; + } + if (!sock_queue_rcv_skb(sk, cskb)) + cskb = NULL; + } + read_unlock(&sl->lock); + if (cskb) + dev_kfree_skb(cskb); +} + +static void +send_layer2(struct mISDNstack *st, struct sk_buff *skb) +{ + struct sk_buff *cskb; + struct mISDNhead *hh = mISDN_HEAD_P(skb); + struct mISDNchannel *ch; + int ret; + + if (!st) + return; + mutex_lock(&st->lmutex); + if ((hh->id & MISDN_ID_ADDR_MASK) == MISDN_ID_ANY) { /* L2 for all */ + list_for_each_entry(ch, &st->layer2, list) { + if (list_is_last(&ch->list, &st->layer2)) { + cskb = skb; + skb = NULL; + } else { + cskb = skb_copy(skb, GFP_KERNEL); + } + if (cskb) { + ret = ch->send(ch, cskb); + if (ret) { + if (*debug & DEBUG_SEND_ERR) + printk(KERN_DEBUG + "%s ch%d prim(%x) addr(%x)" + " err %d\n", + __func__, ch->nr, + hh->prim, ch->addr, ret); + dev_kfree_skb(cskb); + } + } else { + printk(KERN_WARNING "%s ch%d addr %x no mem\n", + __func__, ch->nr, ch->addr); + goto out; + } + } + } else { + list_for_each_entry(ch, &st->layer2, list) { + if ((hh->id & MISDN_ID_ADDR_MASK) == ch->addr) { + ret = ch->send(ch, skb); + if (!ret) + skb = NULL; + goto out; + } + } + ret = st->dev->teimgr->ctrl(st->dev->teimgr, CHECK_DATA, skb); + if (!ret) + skb = NULL; + else if (*debug & DEBUG_SEND_ERR) + printk(KERN_DEBUG + "%s ch%d mgr prim(%x) addr(%x) err %d\n", + __func__, ch->nr, hh->prim, ch->addr, ret); + } +out: + mutex_unlock(&st->lmutex); + if (skb) + dev_kfree_skb(skb); +} + +static inline int +send_msg_to_layer(struct mISDNstack *st, struct sk_buff *skb) +{ + struct mISDNhead *hh = mISDN_HEAD_P(skb); + struct mISDNchannel *ch; + int lm; + + lm = hh->prim & MISDN_LAYERMASK; + if (*debug & DEBUG_QUEUE_FUNC) + printk(KERN_DEBUG "%s prim(%x) id(%x) %p\n", + __func__, hh->prim, hh->id, skb); + if (lm == 0x1) { + if (!hlist_empty(&st->l1sock.head)) { + __net_timestamp(skb); + send_socklist(&st->l1sock, skb); + } + return st->layer1->send(st->layer1, skb); + } else if (lm == 0x2) { + if (!hlist_empty(&st->l1sock.head)) + send_socklist(&st->l1sock, skb); + send_layer2(st, skb); + return 0; + } else if (lm == 0x4) { + ch = get_channel4id(st, hh->id); + if (ch) + return ch->send(ch, skb); + else + printk(KERN_WARNING + "%s: dev(%s) prim(%x) id(%x) no channel\n", + __func__, st->dev->name, hh->prim, hh->id); + } else if (lm == 0x8) { + WARN_ON(lm == 0x8); + ch = get_channel4id(st, hh->id); + if (ch) + return ch->send(ch, skb); + else + printk(KERN_WARNING + "%s: dev(%s) prim(%x) id(%x) no channel\n", + __func__, st->dev->name, hh->prim, hh->id); + } else { + /* broadcast not handled yet */ + printk(KERN_WARNING "%s: dev(%s) prim %x not delivered\n", + __func__, st->dev->name, hh->prim); + } + return -ESRCH; +} + +static void +do_clear_stack(struct mISDNstack *st) +{ +} + +static int +mISDNStackd(void *data) +{ + struct mISDNstack *st = data; + int err = 0; + +#ifdef CONFIG_SMP + lock_kernel(); +#endif + sigfillset(¤t->blocked); +#ifdef CONFIG_SMP + unlock_kernel(); +#endif + if (*debug & DEBUG_MSG_THREAD) + printk(KERN_DEBUG "mISDNStackd %s started\n", st->dev->name); + + if (st->notify != NULL) { + complete(st->notify); + st->notify = NULL; + } + + for (;;) { + struct sk_buff *skb; + + if (unlikely(test_bit(mISDN_STACK_STOPPED, &st->status))) { + test_and_clear_bit(mISDN_STACK_WORK, &st->status); + test_and_clear_bit(mISDN_STACK_RUNNING, &st->status); + } else + test_and_set_bit(mISDN_STACK_RUNNING, &st->status); + while (test_bit(mISDN_STACK_WORK, &st->status)) { + skb = skb_dequeue(&st->msgq); + if (!skb) { + test_and_clear_bit(mISDN_STACK_WORK, + &st->status); + /* test if a race happens */ + skb = skb_dequeue(&st->msgq); + if (!skb) + continue; + test_and_set_bit(mISDN_STACK_WORK, + &st->status); + } +#ifdef MISDN_MSG_STATS + st->msg_cnt++; +#endif + err = send_msg_to_layer(st, skb); + if (unlikely(err)) { + if (*debug & DEBUG_SEND_ERR) + printk(KERN_DEBUG + "%s: %s prim(%x) id(%x) " + "send call(%d)\n", + __func__, st->dev->name, + mISDN_HEAD_PRIM(skb), + mISDN_HEAD_ID(skb), err); + dev_kfree_skb(skb); + continue; + } + if (unlikely(test_bit(mISDN_STACK_STOPPED, + &st->status))) { + test_and_clear_bit(mISDN_STACK_WORK, + &st->status); + test_and_clear_bit(mISDN_STACK_RUNNING, + &st->status); + break; + } + } + if (test_bit(mISDN_STACK_CLEARING, &st->status)) { + test_and_set_bit(mISDN_STACK_STOPPED, &st->status); + test_and_clear_bit(mISDN_STACK_RUNNING, &st->status); + do_clear_stack(st); + test_and_clear_bit(mISDN_STACK_CLEARING, &st->status); + test_and_set_bit(mISDN_STACK_RESTART, &st->status); + } + if (test_and_clear_bit(mISDN_STACK_RESTART, &st->status)) { + test_and_clear_bit(mISDN_STACK_STOPPED, &st->status); + test_and_set_bit(mISDN_STACK_RUNNING, &st->status); + if (!skb_queue_empty(&st->msgq)) + test_and_set_bit(mISDN_STACK_WORK, + &st->status); + } + if (test_bit(mISDN_STACK_ABORT, &st->status)) + break; + if (st->notify != NULL) { + complete(st->notify); + st->notify = NULL; + } +#ifdef MISDN_MSG_STATS + st->sleep_cnt++; +#endif + test_and_clear_bit(mISDN_STACK_ACTIVE, &st->status); + wait_event_interruptible(st->workq, (st->status & + mISDN_STACK_ACTION_MASK)); + if (*debug & DEBUG_MSG_THREAD) + printk(KERN_DEBUG "%s: %s wake status %08lx\n", + __func__, st->dev->name, st->status); + test_and_set_bit(mISDN_STACK_ACTIVE, &st->status); + + test_and_clear_bit(mISDN_STACK_WAKEUP, &st->status); + + if (test_bit(mISDN_STACK_STOPPED, &st->status)) { + test_and_clear_bit(mISDN_STACK_RUNNING, &st->status); +#ifdef MISDN_MSG_STATS + st->stopped_cnt++; +#endif + } + } +#ifdef MISDN_MSG_STATS + printk(KERN_DEBUG "mISDNStackd daemon for %s proceed %d " + "msg %d sleep %d stopped\n", + st->dev->name, st->msg_cnt, st->sleep_cnt, st->stopped_cnt); + printk(KERN_DEBUG + "mISDNStackd daemon for %s utime(%ld) stime(%ld)\n", + st->dev->name, st->thread->utime, st->thread->stime); + printk(KERN_DEBUG + "mISDNStackd daemon for %s nvcsw(%ld) nivcsw(%ld)\n", + st->dev->name, st->thread->nvcsw, st->thread->nivcsw); + printk(KERN_DEBUG "mISDNStackd daemon for %s killed now\n", + st->dev->name); +#endif + test_and_set_bit(mISDN_STACK_KILLED, &st->status); + test_and_clear_bit(mISDN_STACK_RUNNING, &st->status); + test_and_clear_bit(mISDN_STACK_ACTIVE, &st->status); + test_and_clear_bit(mISDN_STACK_ABORT, &st->status); + skb_queue_purge(&st->msgq); + st->thread = NULL; + if (st->notify != NULL) { + complete(st->notify); + st->notify = NULL; + } + return 0; +} + +static int +l1_receive(struct mISDNchannel *ch, struct sk_buff *skb) +{ + if (!ch->st) + return -ENODEV; + __net_timestamp(skb); + _queue_message(ch->st, skb); + return 0; +} + +void +set_channel_address(struct mISDNchannel *ch, u_int sapi, u_int tei) +{ + ch->addr = sapi | (tei << 8); +} + +void +__add_layer2(struct mISDNchannel *ch, struct mISDNstack *st) +{ + list_add_tail(&ch->list, &st->layer2); +} + +void +add_layer2(struct mISDNchannel *ch, struct mISDNstack *st) +{ + mutex_lock(&st->lmutex); + __add_layer2(ch, st); + mutex_unlock(&st->lmutex); +} + +static int +st_own_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg) +{ + if (!ch->st || ch->st->layer1) + return -EINVAL; + return ch->st->layer1->ctrl(ch->st->layer1, cmd, arg); +} + +int +create_stack(struct mISDNdevice *dev) +{ + struct mISDNstack *newst; + int err; + DECLARE_COMPLETION_ONSTACK(done); + + newst = kzalloc(sizeof(struct mISDNstack), GFP_KERNEL); + if (!newst) { + printk(KERN_ERR "kmalloc mISDN_stack failed\n"); + return -ENOMEM; + } + newst->dev = dev; + INIT_LIST_HEAD(&newst->layer2); + INIT_HLIST_HEAD(&newst->l1sock.head); + rwlock_init(&newst->l1sock.lock); + init_waitqueue_head(&newst->workq); + skb_queue_head_init(&newst->msgq); + mutex_init(&newst->lmutex); + dev->D.st = newst; + err = create_teimanager(dev); + if (err) { + printk(KERN_ERR "kmalloc teimanager failed\n"); + kfree(newst); + return err; + } + dev->teimgr->peer = &newst->own; + dev->teimgr->recv = mISDN_queue_message; + dev->teimgr->st = newst; + newst->layer1 = &dev->D; + dev->D.recv = l1_receive; + dev->D.peer = &newst->own; + newst->own.st = newst; + newst->own.ctrl = st_own_ctrl; + newst->own.send = mISDN_queue_message; + newst->own.recv = mISDN_queue_message; + if (*debug & DEBUG_CORE_FUNC) + printk(KERN_DEBUG "%s: st(%s)\n", __func__, newst->dev->name); + newst->notify = &done; + newst->thread = kthread_run(mISDNStackd, (void *)newst, "mISDN_%s", + newst->dev->name); + if (IS_ERR(newst->thread)) { + err = PTR_ERR(newst->thread); + printk(KERN_ERR + "mISDN:cannot create kernel thread for %s (%d)\n", + newst->dev->name, err); + delete_teimanager(dev->teimgr); + kfree(newst); + } else + wait_for_completion(&done); + return err; +} + +int +connect_layer1(struct mISDNdevice *dev, struct mISDNchannel *ch, + u_int protocol, struct sockaddr_mISDN *adr) +{ + struct mISDN_sock *msk = container_of(ch, struct mISDN_sock, ch); + struct channel_req rq; + int err; + + + if (*debug & DEBUG_CORE_FUNC) + printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n", + __func__, dev->name, protocol, adr->dev, adr->channel, + adr->sapi, adr->tei); + switch (protocol) { + case ISDN_P_NT_S0: + case ISDN_P_NT_E1: + case ISDN_P_TE_S0: + case ISDN_P_TE_E1: +#ifdef PROTOCOL_CHECK + /* this should be enhanced */ + if (!list_empty(&dev->D.st->layer2) + && dev->D.protocol != protocol) + return -EBUSY; + if (!hlist_empty(&dev->D.st->l1sock.head) + && dev->D.protocol != protocol) + return -EBUSY; +#endif + ch->recv = mISDN_queue_message; + ch->peer = &dev->D.st->own; + ch->st = dev->D.st; + rq.protocol = protocol; + rq.adr.channel = 0; + err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq); + printk(KERN_DEBUG "%s: ret 1 %d\n", __func__, err); + if (err) + return err; + write_lock_bh(&dev->D.st->l1sock.lock); + sk_add_node(&msk->sk, &dev->D.st->l1sock.head); + write_unlock_bh(&dev->D.st->l1sock.lock); + break; + default: + return -ENOPROTOOPT; + } + return 0; +} + +int +connect_Bstack(struct mISDNdevice *dev, struct mISDNchannel *ch, + u_int protocol, struct sockaddr_mISDN *adr) +{ + struct channel_req rq, rq2; + int pmask, err; + struct Bprotocol *bp; + + if (*debug & DEBUG_CORE_FUNC) + printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n", + __func__, dev->name, protocol, + adr->dev, adr->channel, adr->sapi, + adr->tei); + ch->st = dev->D.st; + pmask = 1 << (protocol & ISDN_P_B_MASK); + if (pmask & dev->Bprotocols) { + rq.protocol = protocol; + rq.adr = *adr; + err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq); + if (err) + return err; + ch->recv = rq.ch->send; + ch->peer = rq.ch; + rq.ch->recv = ch->send; + rq.ch->peer = ch; + rq.ch->st = dev->D.st; + } else { + bp = get_Bprotocol4mask(pmask); + if (!bp) + return -ENOPROTOOPT; + rq2.protocol = protocol; + rq2.adr = *adr; + rq2.ch = ch; + err = bp->create(&rq2); + if (err) + return err; + ch->recv = rq2.ch->send; + ch->peer = rq2.ch; + rq2.ch->st = dev->D.st; + rq.protocol = rq2.protocol; + rq.adr = *adr; + err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq); + if (err) { + rq2.ch->ctrl(rq2.ch, CLOSE_CHANNEL, NULL); + return err; + } + rq2.ch->recv = rq.ch->send; + rq2.ch->peer = rq.ch; + rq.ch->recv = rq2.ch->send; + rq.ch->peer = rq2.ch; + rq.ch->st = dev->D.st; + } + ch->protocol = protocol; + ch->nr = rq.ch->nr; + return 0; +} + +int +create_l2entity(struct mISDNdevice *dev, struct mISDNchannel *ch, + u_int protocol, struct sockaddr_mISDN *adr) +{ + struct channel_req rq; + int err; + + if (*debug & DEBUG_CORE_FUNC) + printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n", + __func__, dev->name, protocol, + adr->dev, adr->channel, adr->sapi, + adr->tei); + rq.protocol = ISDN_P_TE_S0; + if (dev->Dprotocols & (1 << ISDN_P_TE_E1)) + rq.protocol = ISDN_P_TE_E1; + switch (protocol) { + case ISDN_P_LAPD_NT: + rq.protocol = ISDN_P_NT_S0; + if (dev->Dprotocols & (1 << ISDN_P_NT_E1)) + rq.protocol = ISDN_P_NT_E1; + case ISDN_P_LAPD_TE: +#ifdef PROTOCOL_CHECK + /* this should be enhanced */ + if (!list_empty(&dev->D.st->layer2) + && dev->D.protocol != protocol) + return -EBUSY; + if (!hlist_empty(&dev->D.st->l1sock.head) + && dev->D.protocol != protocol) + return -EBUSY; +#endif + ch->recv = mISDN_queue_message; + ch->peer = &dev->D.st->own; + ch->st = dev->D.st; + rq.adr.channel = 0; + err = dev->D.ctrl(&dev->D, OPEN_CHANNEL, &rq); + printk(KERN_DEBUG "%s: ret 1 %d\n", __func__, err); + if (err) + break; + rq.protocol = protocol; + rq.adr = *adr; + rq.ch = ch; + err = dev->teimgr->ctrl(dev->teimgr, OPEN_CHANNEL, &rq); + printk(KERN_DEBUG "%s: ret 2 %d\n", __func__, err); + if (!err) { + if ((protocol == ISDN_P_LAPD_NT) && !rq.ch) + break; + add_layer2(rq.ch, dev->D.st); + rq.ch->recv = mISDN_queue_message; + rq.ch->peer = &dev->D.st->own; + rq.ch->ctrl(rq.ch, OPEN_CHANNEL, NULL); /* can't fail */ + } + break; + default: + err = -EPROTONOSUPPORT; + } + return err; +} + +void +delete_channel(struct mISDNchannel *ch) +{ + struct mISDN_sock *msk = container_of(ch, struct mISDN_sock, ch); + struct mISDNchannel *pch; + + if (!ch->st) { + printk(KERN_WARNING "%s: no stack\n", __func__); + return; + } + if (*debug & DEBUG_CORE_FUNC) + printk(KERN_DEBUG "%s: st(%s) protocol(%x)\n", __func__, + ch->st->dev->name, ch->protocol); + if (ch->protocol >= ISDN_P_B_START) { + if (ch->peer) { + ch->peer->ctrl(ch->peer, CLOSE_CHANNEL, NULL); + ch->peer = NULL; + } + return; + } + switch (ch->protocol) { + case ISDN_P_NT_S0: + case ISDN_P_TE_S0: + case ISDN_P_NT_E1: + case ISDN_P_TE_E1: + write_lock_bh(&ch->st->l1sock.lock); + sk_del_node_init(&msk->sk); + write_unlock_bh(&ch->st->l1sock.lock); + ch->st->dev->D.ctrl(&ch->st->dev->D, CLOSE_CHANNEL, NULL); + break; + case ISDN_P_LAPD_TE: + pch = get_channel4id(ch->st, ch->nr); + if (pch) { + mutex_lock(&ch->st->lmutex); + list_del(&pch->list); + mutex_unlock(&ch->st->lmutex); + pch->ctrl(pch, CLOSE_CHANNEL, NULL); + pch = ch->st->dev->teimgr; + pch->ctrl(pch, CLOSE_CHANNEL, NULL); + } else + printk(KERN_WARNING "%s: no l2 channel\n", + __func__); + break; + case ISDN_P_LAPD_NT: + pch = ch->st->dev->teimgr; + if (pch) { + pch->ctrl(pch, CLOSE_CHANNEL, NULL); + } else + printk(KERN_WARNING "%s: no l2 channel\n", + __func__); + break; + default: + break; + } + return; +} + +void +delete_stack(struct mISDNdevice *dev) +{ + struct mISDNstack *st = dev->D.st; + DECLARE_COMPLETION_ONSTACK(done); + + if (*debug & DEBUG_CORE_FUNC) + printk(KERN_DEBUG "%s: st(%s)\n", __func__, + st->dev->name); + if (dev->teimgr) + delete_teimanager(dev->teimgr); + if (st->thread) { + if (st->notify) { + printk(KERN_WARNING "%s: notifier in use\n", + __func__); + complete(st->notify); + } + st->notify = &done; + test_and_set_bit(mISDN_STACK_ABORT, &st->status); + test_and_set_bit(mISDN_STACK_WAKEUP, &st->status); + wake_up_interruptible(&st->workq); + wait_for_completion(&done); + } + if (!list_empty(&st->layer2)) + printk(KERN_WARNING "%s: layer2 list not empty\n", + __func__); + if (!hlist_empty(&st->l1sock.head)) + printk(KERN_WARNING "%s: layer1 list not empty\n", + __func__); + kfree(st); +} + +void +mISDN_initstack(u_int *dp) +{ + debug = dp; +} diff --git a/drivers/isdn/mISDN/tei.c b/drivers/isdn/mISDN/tei.c new file mode 100644 index 000000000000..6fbae42127bf --- /dev/null +++ b/drivers/isdn/mISDN/tei.c @@ -0,0 +1,1340 @@ +/* + * + * Author Karsten Keil <kkeil@novell.com> + * + * Copyright 2008 by Karsten Keil <kkeil@novell.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ +#include "layer2.h" +#include <linux/random.h> +#include "core.h" + +#define ID_REQUEST 1 +#define ID_ASSIGNED 2 +#define ID_DENIED 3 +#define ID_CHK_REQ 4 +#define ID_CHK_RES 5 +#define ID_REMOVE 6 +#define ID_VERIFY 7 + +#define TEI_ENTITY_ID 0xf + +#define MGR_PH_ACTIVE 16 +#define MGR_PH_NOTREADY 17 + +#define DATIMER_VAL 10000 + +static u_int *debug; + +static struct Fsm deactfsm = {NULL, 0, 0, NULL, NULL}; +static struct Fsm teifsmu = {NULL, 0, 0, NULL, NULL}; +static struct Fsm teifsmn = {NULL, 0, 0, NULL, NULL}; + +enum { + ST_L1_DEACT, + ST_L1_DEACT_PENDING, + ST_L1_ACTIV, +}; +#define DEACT_STATE_COUNT (ST_L1_ACTIV+1) + +static char *strDeactState[] = +{ + "ST_L1_DEACT", + "ST_L1_DEACT_PENDING", + "ST_L1_ACTIV", +}; + +enum { + EV_ACTIVATE, + EV_ACTIVATE_IND, + EV_DEACTIVATE, + EV_DEACTIVATE_IND, + EV_UI, + EV_DATIMER, +}; + +#define DEACT_EVENT_COUNT (EV_DATIMER+1) + +static char *strDeactEvent[] = +{ + "EV_ACTIVATE", + "EV_ACTIVATE_IND", + "EV_DEACTIVATE", + "EV_DEACTIVATE_IND", + "EV_UI", + "EV_DATIMER", +}; + +static void +da_debug(struct FsmInst *fi, char *fmt, ...) +{ + struct manager *mgr = fi->userdata; + va_list va; + + if (!(*debug & DEBUG_L2_TEIFSM)) + return; + va_start(va, fmt); + printk(KERN_DEBUG "mgr(%d): ", mgr->ch.st->dev->id); + vprintk(fmt, va); + printk("\n"); + va_end(va); +} + +static void +da_activate(struct FsmInst *fi, int event, void *arg) +{ + struct manager *mgr = fi->userdata; + + if (fi->state == ST_L1_DEACT_PENDING) + mISDN_FsmDelTimer(&mgr->datimer, 1); + mISDN_FsmChangeState(fi, ST_L1_ACTIV); +} + +static void +da_deactivate_ind(struct FsmInst *fi, int event, void *arg) +{ + mISDN_FsmChangeState(fi, ST_L1_DEACT); +} + +static void +da_deactivate(struct FsmInst *fi, int event, void *arg) +{ + struct manager *mgr = fi->userdata; + struct layer2 *l2; + u_long flags; + + read_lock_irqsave(&mgr->lock, flags); + list_for_each_entry(l2, &mgr->layer2, list) { + if (l2->l2m.state > ST_L2_4) { + /* have still activ TEI */ + read_unlock_irqrestore(&mgr->lock, flags); + return; + } + } + read_unlock_irqrestore(&mgr->lock, flags); + /* All TEI are inactiv */ + mISDN_FsmAddTimer(&mgr->datimer, DATIMER_VAL, EV_DATIMER, NULL, 1); + mISDN_FsmChangeState(fi, ST_L1_DEACT_PENDING); +} + +static void +da_ui(struct FsmInst *fi, int event, void *arg) +{ + struct manager *mgr = fi->userdata; + + /* restart da timer */ + mISDN_FsmDelTimer(&mgr->datimer, 2); + mISDN_FsmAddTimer(&mgr->datimer, DATIMER_VAL, EV_DATIMER, NULL, 2); + +} + +static void +da_timer(struct FsmInst *fi, int event, void *arg) +{ + struct manager *mgr = fi->userdata; + struct layer2 *l2; + u_long flags; + + /* check again */ + read_lock_irqsave(&mgr->lock, flags); + list_for_each_entry(l2, &mgr->layer2, list) { + if (l2->l2m.state > ST_L2_4) { + /* have still activ TEI */ + read_unlock_irqrestore(&mgr->lock, flags); + mISDN_FsmChangeState(fi, ST_L1_ACTIV); + return; + } + } + read_unlock_irqrestore(&mgr->lock, flags); + /* All TEI are inactiv */ + mISDN_FsmChangeState(fi, ST_L1_DEACT); + _queue_data(&mgr->ch, PH_DEACTIVATE_REQ, MISDN_ID_ANY, 0, NULL, + GFP_ATOMIC); +} + +static struct FsmNode DeactFnList[] = +{ + {ST_L1_DEACT, EV_ACTIVATE_IND, da_activate}, + {ST_L1_ACTIV, EV_DEACTIVATE_IND, da_deactivate_ind}, + {ST_L1_ACTIV, EV_DEACTIVATE, da_deactivate}, + {ST_L1_DEACT_PENDING, EV_ACTIVATE, da_activate}, + {ST_L1_DEACT_PENDING, EV_UI, da_ui}, + {ST_L1_DEACT_PENDING, EV_DATIMER, da_timer}, +}; + +enum { + ST_TEI_NOP, + ST_TEI_IDREQ, + ST_TEI_IDVERIFY, +}; + +#define TEI_STATE_COUNT (ST_TEI_IDVERIFY+1) + +static char *strTeiState[] = +{ + "ST_TEI_NOP", + "ST_TEI_IDREQ", + "ST_TEI_IDVERIFY", +}; + +enum { + EV_IDREQ, + EV_ASSIGN, + EV_ASSIGN_REQ, + EV_DENIED, + EV_CHKREQ, + EV_CHKRESP, + EV_REMOVE, + EV_VERIFY, + EV_TIMER, +}; + +#define TEI_EVENT_COUNT (EV_TIMER+1) + +static char *strTeiEvent[] = +{ + "EV_IDREQ", + "EV_ASSIGN", + "EV_ASSIGN_REQ", + "EV_DENIED", + "EV_CHKREQ", + "EV_CHKRESP", + "EV_REMOVE", + "EV_VERIFY", + "EV_TIMER", +}; + +static void +tei_debug(struct FsmInst *fi, char *fmt, ...) +{ + struct teimgr *tm = fi->userdata; + va_list va; + + if (!(*debug & DEBUG_L2_TEIFSM)) + return; + va_start(va, fmt); + printk(KERN_DEBUG "tei(%d): ", tm->l2->tei); + vprintk(fmt, va); + printk("\n"); + va_end(va); +} + + + +static int +get_free_id(struct manager *mgr) +{ + u64 ids = 0; + int i; + struct layer2 *l2; + + list_for_each_entry(l2, &mgr->layer2, list) { + if (l2->ch.nr > 63) { + printk(KERN_WARNING + "%s: more as 63 layer2 for one device\n", + __func__); + return -EBUSY; + } + test_and_set_bit(l2->ch.nr, (u_long *)&ids); + } + for (i = 1; i < 64; i++) + if (!test_bit(i, (u_long *)&ids)) + return i; + printk(KERN_WARNING "%s: more as 63 layer2 for one device\n", + __func__); + return -EBUSY; +} + +static int +get_free_tei(struct manager *mgr) +{ + u64 ids = 0; + int i; + struct layer2 *l2; + + list_for_each_entry(l2, &mgr->layer2, list) { + if (l2->ch.nr == 0) + continue; + if ((l2->ch.addr & 0xff) != 0) + continue; + i = l2->ch.addr >> 8; + if (i < 64) + continue; + i -= 64; + + test_and_set_bit(i, (u_long *)&ids); + } + for (i = 0; i < 64; i++) + if (!test_bit(i, (u_long *)&ids)) + return i + 64; + printk(KERN_WARNING "%s: more as 63 dynamic tei for one device\n", + __func__); + return -1; +} + +static void +teiup_create(struct manager *mgr, u_int prim, int len, void *arg) +{ + struct sk_buff *skb; + struct mISDNhead *hh; + int err; + + skb = mI_alloc_skb(len, GFP_ATOMIC); + if (!skb) + return; + hh = mISDN_HEAD_P(skb); + hh->prim = prim; + hh->id = (mgr->ch.nr << 16) | mgr->ch.addr; + if (len) + memcpy(skb_put(skb, len), arg, len); + err = mgr->up->send(mgr->up, skb); + if (err) { + printk(KERN_WARNING "%s: err=%d\n", __func__, err); + dev_kfree_skb(skb); + } +} + +static u_int +new_id(struct manager *mgr) +{ + u_int id; + + id = mgr->nextid++; + if (id == 0x7fff) + mgr->nextid = 1; + id <<= 16; + id |= GROUP_TEI << 8; + id |= TEI_SAPI; + return id; +} + +static void +do_send(struct manager *mgr) +{ + if (!test_bit(MGR_PH_ACTIVE, &mgr->options)) + return; + + if (!test_and_set_bit(MGR_PH_NOTREADY, &mgr->options)) { + struct sk_buff *skb = skb_dequeue(&mgr->sendq); + + if (!skb) { + test_and_clear_bit(MGR_PH_NOTREADY, &mgr->options); + return; + } + mgr->lastid = mISDN_HEAD_ID(skb); + mISDN_FsmEvent(&mgr->deact, EV_UI, NULL); + if (mgr->ch.recv(mgr->ch.peer, skb)) { + dev_kfree_skb(skb); + test_and_clear_bit(MGR_PH_NOTREADY, &mgr->options); + mgr->lastid = MISDN_ID_NONE; + } + } +} + +static void +do_ack(struct manager *mgr, u_int id) +{ + if (test_bit(MGR_PH_NOTREADY, &mgr->options)) { + if (id == mgr->lastid) { + if (test_bit(MGR_PH_ACTIVE, &mgr->options)) { + struct sk_buff *skb; + + skb = skb_dequeue(&mgr->sendq); + if (skb) { + mgr->lastid = mISDN_HEAD_ID(skb); + if (!mgr->ch.recv(mgr->ch.peer, skb)) + return; + dev_kfree_skb(skb); + } + } + mgr->lastid = MISDN_ID_NONE; + test_and_clear_bit(MGR_PH_NOTREADY, &mgr->options); + } + } +} + +static void +mgr_send_down(struct manager *mgr, struct sk_buff *skb) +{ + skb_queue_tail(&mgr->sendq, skb); + if (!test_bit(MGR_PH_ACTIVE, &mgr->options)) { + _queue_data(&mgr->ch, PH_ACTIVATE_REQ, MISDN_ID_ANY, 0, + NULL, GFP_KERNEL); + } else { + do_send(mgr); + } +} + +static int +dl_unit_data(struct manager *mgr, struct sk_buff *skb) +{ + if (!test_bit(MGR_OPT_NETWORK, &mgr->options)) /* only net send UI */ + return -EINVAL; + if (!test_bit(MGR_PH_ACTIVE, &mgr->options)) + _queue_data(&mgr->ch, PH_ACTIVATE_REQ, MISDN_ID_ANY, 0, + NULL, GFP_KERNEL); + skb_push(skb, 3); + skb->data[0] = 0x02; /* SAPI 0 C/R = 1 */ + skb->data[1] = 0xff; /* TEI 127 */ + skb->data[2] = UI; /* UI frame */ + mISDN_HEAD_PRIM(skb) = PH_DATA_REQ; + mISDN_HEAD_ID(skb) = new_id(mgr); + skb_queue_tail(&mgr->sendq, skb); + do_send(mgr); + return 0; +} + +unsigned int +random_ri(void) +{ + u16 x; + + get_random_bytes(&x, sizeof(x)); + return x; +} + +static struct layer2 * +findtei(struct manager *mgr, int tei) +{ + struct layer2 *l2; + u_long flags; + + read_lock_irqsave(&mgr->lock, flags); + list_for_each_entry(l2, &mgr->layer2, list) { + if ((l2->sapi == 0) && (l2->tei > 0) && + (l2->tei != GROUP_TEI) && (l2->tei == tei)) + goto done; + } + l2 = NULL; +done: + read_unlock_irqrestore(&mgr->lock, flags); + return l2; +} + +static void +put_tei_msg(struct manager *mgr, u_char m_id, unsigned int ri, u_char tei) +{ + struct sk_buff *skb; + u_char bp[8]; + + bp[0] = (TEI_SAPI << 2); + if (test_bit(MGR_OPT_NETWORK, &mgr->options)) + bp[0] |= 2; /* CR:=1 for net command */ + bp[1] = (GROUP_TEI << 1) | 0x1; + bp[2] = UI; + bp[3] = TEI_ENTITY_ID; + bp[4] = ri >> 8; + bp[5] = ri & 0xff; + bp[6] = m_id; + bp[7] = (tei << 1) | 1; + skb = _alloc_mISDN_skb(PH_DATA_REQ, new_id(mgr), + 8, bp, GFP_ATOMIC); + if (!skb) { + printk(KERN_WARNING "%s: no skb for tei msg\n", __func__); + return; + } + mgr_send_down(mgr, skb); +} + +static void +tei_id_request(struct FsmInst *fi, int event, void *arg) +{ + struct teimgr *tm = fi->userdata; + + if (tm->l2->tei != GROUP_TEI) { + tm->tei_m.printdebug(&tm->tei_m, + "assign request for allready assigned tei %d", + tm->l2->tei); + return; + } + tm->ri = random_ri(); + if (*debug & DEBUG_L2_TEI) + tm->tei_m.printdebug(&tm->tei_m, + "assign request ri %d", tm->ri); + put_tei_msg(tm->mgr, ID_REQUEST, tm->ri, GROUP_TEI); + mISDN_FsmChangeState(fi, ST_TEI_IDREQ); + mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 1); + tm->nval = 3; +} + +static void +tei_id_assign(struct FsmInst *fi, int event, void *arg) +{ + struct teimgr *tm = fi->userdata; + struct layer2 *l2; + u_char *dp = arg; + int ri, tei; + + ri = ((unsigned int) *dp++ << 8); + ri += *dp++; + dp++; + tei = *dp >> 1; + if (*debug & DEBUG_L2_TEI) + tm->tei_m.printdebug(fi, "identity assign ri %d tei %d", + ri, tei); + l2 = findtei(tm->mgr, tei); + if (l2) { /* same tei is in use */ + if (ri != l2->tm->ri) { + tm->tei_m.printdebug(fi, + "possible duplicate assignment tei %d", tei); + tei_l2(l2, MDL_ERROR_RSP, 0); + } + } else if (ri == tm->ri) { + mISDN_FsmDelTimer(&tm->timer, 1); + mISDN_FsmChangeState(fi, ST_TEI_NOP); + tei_l2(tm->l2, MDL_ASSIGN_REQ, tei); + } +} + +static void +tei_id_test_dup(struct FsmInst *fi, int event, void *arg) +{ + struct teimgr *tm = fi->userdata; + struct layer2 *l2; + u_char *dp = arg; + int tei, ri; + + ri = ((unsigned int) *dp++ << 8); + ri += *dp++; + dp++; + tei = *dp >> 1; + if (*debug & DEBUG_L2_TEI) + tm->tei_m.printdebug(fi, "foreign identity assign ri %d tei %d", + ri, tei); + l2 = findtei(tm->mgr, tei); + if (l2) { /* same tei is in use */ + if (ri != l2->tm->ri) { /* and it wasn't our request */ + tm->tei_m.printdebug(fi, + "possible duplicate assignment tei %d", tei); + mISDN_FsmEvent(&l2->tm->tei_m, EV_VERIFY, NULL); + } + } +} + +static void +tei_id_denied(struct FsmInst *fi, int event, void *arg) +{ + struct teimgr *tm = fi->userdata; + u_char *dp = arg; + int ri, tei; + + ri = ((unsigned int) *dp++ << 8); + ri += *dp++; + dp++; + tei = *dp >> 1; + if (*debug & DEBUG_L2_TEI) + tm->tei_m.printdebug(fi, "identity denied ri %d tei %d", + ri, tei); +} + +static void +tei_id_chk_req(struct FsmInst *fi, int event, void *arg) +{ + struct teimgr *tm = fi->userdata; + u_char *dp = arg; + int tei; + + tei = *(dp+3) >> 1; + if (*debug & DEBUG_L2_TEI) + tm->tei_m.printdebug(fi, "identity check req tei %d", tei); + if ((tm->l2->tei != GROUP_TEI) && ((tei == GROUP_TEI) || + (tei == tm->l2->tei))) { + mISDN_FsmDelTimer(&tm->timer, 4); + mISDN_FsmChangeState(&tm->tei_m, ST_TEI_NOP); + put_tei_msg(tm->mgr, ID_CHK_RES, random_ri(), tm->l2->tei); + } +} + +static void +tei_id_remove(struct FsmInst *fi, int event, void *arg) +{ + struct teimgr *tm = fi->userdata; + u_char *dp = arg; + int tei; + + tei = *(dp+3) >> 1; + if (*debug & DEBUG_L2_TEI) + tm->tei_m.printdebug(fi, "identity remove tei %d", tei); + if ((tm->l2->tei != GROUP_TEI) && + ((tei == GROUP_TEI) || (tei == tm->l2->tei))) { + mISDN_FsmDelTimer(&tm->timer, 5); + mISDN_FsmChangeState(&tm->tei_m, ST_TEI_NOP); + tei_l2(tm->l2, MDL_REMOVE_REQ, 0); + } +} + +static void +tei_id_verify(struct FsmInst *fi, int event, void *arg) +{ + struct teimgr *tm = fi->userdata; + + if (*debug & DEBUG_L2_TEI) + tm->tei_m.printdebug(fi, "id verify request for tei %d", + tm->l2->tei); + put_tei_msg(tm->mgr, ID_VERIFY, 0, tm->l2->tei); + mISDN_FsmChangeState(&tm->tei_m, ST_TEI_IDVERIFY); + mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 2); + tm->nval = 2; +} + +static void +tei_id_req_tout(struct FsmInst *fi, int event, void *arg) +{ + struct teimgr *tm = fi->userdata; + + if (--tm->nval) { + tm->ri = random_ri(); + if (*debug & DEBUG_L2_TEI) + tm->tei_m.printdebug(fi, "assign req(%d) ri %d", + 4 - tm->nval, tm->ri); + put_tei_msg(tm->mgr, ID_REQUEST, tm->ri, GROUP_TEI); + mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 3); + } else { + tm->tei_m.printdebug(fi, "assign req failed"); + tei_l2(tm->l2, MDL_ERROR_RSP, 0); + mISDN_FsmChangeState(fi, ST_TEI_NOP); + } +} + +static void +tei_id_ver_tout(struct FsmInst *fi, int event, void *arg) +{ + struct teimgr *tm = fi->userdata; + + if (--tm->nval) { + if (*debug & DEBUG_L2_TEI) + tm->tei_m.printdebug(fi, + "id verify req(%d) for tei %d", + 3 - tm->nval, tm->l2->tei); + put_tei_msg(tm->mgr, ID_VERIFY, 0, tm->l2->tei); + mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 4); + } else { + tm->tei_m.printdebug(fi, "verify req for tei %d failed", + tm->l2->tei); + tei_l2(tm->l2, MDL_REMOVE_REQ, 0); + mISDN_FsmChangeState(fi, ST_TEI_NOP); + } +} + +static struct FsmNode TeiFnListUser[] = +{ + {ST_TEI_NOP, EV_IDREQ, tei_id_request}, + {ST_TEI_NOP, EV_ASSIGN, tei_id_test_dup}, + {ST_TEI_NOP, EV_VERIFY, tei_id_verify}, + {ST_TEI_NOP, EV_REMOVE, tei_id_remove}, + {ST_TEI_NOP, EV_CHKREQ, tei_id_chk_req}, + {ST_TEI_IDREQ, EV_TIMER, tei_id_req_tout}, + {ST_TEI_IDREQ, EV_ASSIGN, tei_id_assign}, + {ST_TEI_IDREQ, EV_DENIED, tei_id_denied}, + {ST_TEI_IDVERIFY, EV_TIMER, tei_id_ver_tout}, + {ST_TEI_IDVERIFY, EV_REMOVE, tei_id_remove}, + {ST_TEI_IDVERIFY, EV_CHKREQ, tei_id_chk_req}, +}; + +static void +tei_l2remove(struct layer2 *l2) +{ + put_tei_msg(l2->tm->mgr, ID_REMOVE, 0, l2->tei); + tei_l2(l2, MDL_REMOVE_REQ, 0); + list_del(&l2->ch.list); + l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL); +} + +static void +tei_assign_req(struct FsmInst *fi, int event, void *arg) +{ + struct teimgr *tm = fi->userdata; + u_char *dp = arg; + + if (tm->l2->tei == GROUP_TEI) { + tm->tei_m.printdebug(&tm->tei_m, + "net tei assign request without tei"); + return; + } + tm->ri = ((unsigned int) *dp++ << 8); + tm->ri += *dp++; + if (*debug & DEBUG_L2_TEI) + tm->tei_m.printdebug(&tm->tei_m, + "net assign request ri %d teim %d", tm->ri, *dp); + put_tei_msg(tm->mgr, ID_ASSIGNED, tm->ri, tm->l2->tei); + mISDN_FsmChangeState(fi, ST_TEI_NOP); +} + +static void +tei_id_chk_req_net(struct FsmInst *fi, int event, void *arg) +{ + struct teimgr *tm = fi->userdata; + + if (*debug & DEBUG_L2_TEI) + tm->tei_m.printdebug(fi, "id check request for tei %d", + tm->l2->tei); + tm->rcnt = 0; + put_tei_msg(tm->mgr, ID_CHK_REQ, 0, tm->l2->tei); + mISDN_FsmChangeState(&tm->tei_m, ST_TEI_IDVERIFY); + mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 2); + tm->nval = 2; +} + +static void +tei_id_chk_resp(struct FsmInst *fi, int event, void *arg) +{ + struct teimgr *tm = fi->userdata; + u_char *dp = arg; + int tei; + + tei = dp[3] >> 1; + if (*debug & DEBUG_L2_TEI) + tm->tei_m.printdebug(fi, "identity check resp tei %d", tei); + if (tei == tm->l2->tei) + tm->rcnt++; +} + +static void +tei_id_verify_net(struct FsmInst *fi, int event, void *arg) +{ + struct teimgr *tm = fi->userdata; + u_char *dp = arg; + int tei; + + tei = dp[3] >> 1; + if (*debug & DEBUG_L2_TEI) + tm->tei_m.printdebug(fi, "identity verify req tei %d/%d", + tei, tm->l2->tei); + if (tei == tm->l2->tei) + tei_id_chk_req_net(fi, event, arg); +} + +static void +tei_id_ver_tout_net(struct FsmInst *fi, int event, void *arg) +{ + struct teimgr *tm = fi->userdata; + + if (tm->rcnt == 1) { + if (*debug & DEBUG_L2_TEI) + tm->tei_m.printdebug(fi, + "check req for tei %d sucessful\n", tm->l2->tei); + mISDN_FsmChangeState(fi, ST_TEI_NOP); + } else if (tm->rcnt > 1) { + /* duplicate assignment; remove */ + tei_l2remove(tm->l2); + } else if (--tm->nval) { + if (*debug & DEBUG_L2_TEI) + tm->tei_m.printdebug(fi, + "id check req(%d) for tei %d", + 3 - tm->nval, tm->l2->tei); + put_tei_msg(tm->mgr, ID_CHK_REQ, 0, tm->l2->tei); + mISDN_FsmAddTimer(&tm->timer, tm->tval, EV_TIMER, NULL, 4); + } else { + tm->tei_m.printdebug(fi, "check req for tei %d failed", + tm->l2->tei); + mISDN_FsmChangeState(fi, ST_TEI_NOP); + tei_l2remove(tm->l2); + } +} + +static struct FsmNode TeiFnListNet[] = +{ + {ST_TEI_NOP, EV_ASSIGN_REQ, tei_assign_req}, + {ST_TEI_NOP, EV_VERIFY, tei_id_verify_net}, + {ST_TEI_NOP, EV_CHKREQ, tei_id_chk_req_net}, + {ST_TEI_IDVERIFY, EV_TIMER, tei_id_ver_tout_net}, + {ST_TEI_IDVERIFY, EV_CHKRESP, tei_id_chk_resp}, +}; + +static void +tei_ph_data_ind(struct teimgr *tm, u_int mt, u_char *dp, int len) +{ + if (test_bit(FLG_FIXED_TEI, &tm->l2->flag)) + return; + if (*debug & DEBUG_L2_TEI) + tm->tei_m.printdebug(&tm->tei_m, "tei handler mt %x", mt); + if (mt == ID_ASSIGNED) + mISDN_FsmEvent(&tm->tei_m, EV_ASSIGN, dp); + else if (mt == ID_DENIED) + mISDN_FsmEvent(&tm->tei_m, EV_DENIED, dp); + else if (mt == ID_CHK_REQ) + mISDN_FsmEvent(&tm->tei_m, EV_CHKREQ, dp); + else if (mt == ID_REMOVE) + mISDN_FsmEvent(&tm->tei_m, EV_REMOVE, dp); + else if (mt == ID_VERIFY) + mISDN_FsmEvent(&tm->tei_m, EV_VERIFY, dp); + else if (mt == ID_CHK_RES) + mISDN_FsmEvent(&tm->tei_m, EV_CHKRESP, dp); +} + +static struct layer2 * +create_new_tei(struct manager *mgr, int tei) +{ + u_long opt = 0; + u_long flags; + int id; + struct layer2 *l2; + + if (!mgr->up) + return NULL; + if (tei < 64) + test_and_set_bit(OPTION_L2_FIXEDTEI, &opt); + if (mgr->ch.st->dev->Dprotocols + & ((1 << ISDN_P_TE_E1) | (1 << ISDN_P_NT_E1))) + test_and_set_bit(OPTION_L2_PMX, &opt); + l2 = create_l2(mgr->up, ISDN_P_LAPD_NT, (u_int)opt, (u_long)tei); + if (!l2) { + printk(KERN_WARNING "%s:no memory for layer2\n", __func__); + return NULL; + } + l2->tm = kzalloc(sizeof(struct teimgr), GFP_KERNEL); + if (!l2->tm) { + kfree(l2); + printk(KERN_WARNING "%s:no memory for teimgr\n", __func__); + return NULL; + } + l2->tm->mgr = mgr; + l2->tm->l2 = l2; + l2->tm->tei_m.debug = *debug & DEBUG_L2_TEIFSM; + l2->tm->tei_m.userdata = l2->tm; + l2->tm->tei_m.printdebug = tei_debug; + l2->tm->tei_m.fsm = &teifsmn; + l2->tm->tei_m.state = ST_TEI_NOP; + l2->tm->tval = 2000; /* T202 2 sec */ + mISDN_FsmInitTimer(&l2->tm->tei_m, &l2->tm->timer); + write_lock_irqsave(&mgr->lock, flags); + id = get_free_id(mgr); + list_add_tail(&l2->list, &mgr->layer2); + write_unlock_irqrestore(&mgr->lock, flags); + if (id < 0) { + l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL); + printk(KERN_WARNING "%s:no free id\n", __func__); + return NULL; + } else { + l2->ch.nr = id; + __add_layer2(&l2->ch, mgr->ch.st); + l2->ch.recv = mgr->ch.recv; + l2->ch.peer = mgr->ch.peer; + l2->ch.ctrl(&l2->ch, OPEN_CHANNEL, NULL); + } + return l2; +} + +static void +new_tei_req(struct manager *mgr, u_char *dp) +{ + int tei, ri; + struct layer2 *l2; + + ri = dp[0] << 8; + ri += dp[1]; + if (!mgr->up) + goto denied; + tei = get_free_tei(mgr); + if (tei < 0) { + printk(KERN_WARNING "%s:No free tei\n", __func__); + goto denied; + } + l2 = create_new_tei(mgr, tei); + if (!l2) + goto denied; + else + mISDN_FsmEvent(&l2->tm->tei_m, EV_ASSIGN_REQ, dp); + return; +denied: + put_tei_msg(mgr, ID_DENIED, ri, GROUP_TEI); +} + +static int +ph_data_ind(struct manager *mgr, struct sk_buff *skb) +{ + int ret = -EINVAL; + struct layer2 *l2; + u_long flags; + u_char mt; + + if (skb->len < 8) { + if (*debug & DEBUG_L2_TEI) + printk(KERN_DEBUG "%s: short mgr frame %d/8\n", + __func__, skb->len); + goto done; + } + if (*debug & DEBUG_L2_TEI) + + if ((skb->data[0] >> 2) != TEI_SAPI) /* not for us */ + goto done; + if (skb->data[0] & 1) /* EA0 formal error */ + goto done; + if (!(skb->data[1] & 1)) /* EA1 formal error */ + goto done; + if ((skb->data[1] >> 1) != GROUP_TEI) /* not for us */ + goto done; + if ((skb->data[2] & 0xef) != UI) /* not UI */ + goto done; + if (skb->data[3] != TEI_ENTITY_ID) /* not tei entity */ + goto done; + mt = skb->data[6]; + switch (mt) { + case ID_REQUEST: + case ID_CHK_RES: + case ID_VERIFY: + if (!test_bit(MGR_OPT_NETWORK, &mgr->options)) + goto done; + break; + case ID_ASSIGNED: + case ID_DENIED: + case ID_CHK_REQ: + case ID_REMOVE: + if (test_bit(MGR_OPT_NETWORK, &mgr->options)) + goto done; + break; + default: + goto done; + } + ret = 0; + if (mt == ID_REQUEST) { + new_tei_req(mgr, &skb->data[4]); + goto done; + } + read_lock_irqsave(&mgr->lock, flags); + list_for_each_entry(l2, &mgr->layer2, list) { + tei_ph_data_ind(l2->tm, mt, &skb->data[4], skb->len - 4); + } + read_unlock_irqrestore(&mgr->lock, flags); +done: + return ret; +} + +int +l2_tei(struct layer2 *l2, u_int cmd, u_long arg) +{ + struct teimgr *tm = l2->tm; + + if (test_bit(FLG_FIXED_TEI, &l2->flag)) + return 0; + if (*debug & DEBUG_L2_TEI) + printk(KERN_DEBUG "%s: cmd(%x)\n", __func__, cmd); + switch (cmd) { + case MDL_ASSIGN_IND: + mISDN_FsmEvent(&tm->tei_m, EV_IDREQ, NULL); + break; + case MDL_ERROR_IND: + if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options)) + mISDN_FsmEvent(&tm->tei_m, EV_CHKREQ, &l2->tei); + if (test_bit(MGR_OPT_USER, &tm->mgr->options)) + mISDN_FsmEvent(&tm->tei_m, EV_VERIFY, NULL); + break; + case MDL_STATUS_UP_IND: + if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options)) + mISDN_FsmEvent(&tm->mgr->deact, EV_ACTIVATE, NULL); + break; + case MDL_STATUS_DOWN_IND: + if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options)) + mISDN_FsmEvent(&tm->mgr->deact, EV_DEACTIVATE, NULL); + break; + case MDL_STATUS_UI_IND: + if (test_bit(MGR_OPT_NETWORK, &tm->mgr->options)) + mISDN_FsmEvent(&tm->mgr->deact, EV_UI, NULL); + break; + } + return 0; +} + +void +TEIrelease(struct layer2 *l2) +{ + struct teimgr *tm = l2->tm; + u_long flags; + + mISDN_FsmDelTimer(&tm->timer, 1); + write_lock_irqsave(&tm->mgr->lock, flags); + list_del(&l2->list); + write_unlock_irqrestore(&tm->mgr->lock, flags); + l2->tm = NULL; + kfree(tm); +} + +static int +create_teimgr(struct manager *mgr, struct channel_req *crq) +{ + struct layer2 *l2; + u_long opt = 0; + u_long flags; + int id; + + if (*debug & DEBUG_L2_TEI) + printk(KERN_DEBUG "%s: %s proto(%x) adr(%d %d %d %d)\n", + __func__, mgr->ch.st->dev->name, crq->protocol, + crq->adr.dev, crq->adr.channel, crq->adr.sapi, + crq->adr.tei); + if (crq->adr.sapi != 0) /* not supported yet */ + return -EINVAL; + if (crq->adr.tei > GROUP_TEI) + return -EINVAL; + if (crq->adr.tei < 64) + test_and_set_bit(OPTION_L2_FIXEDTEI, &opt); + if (crq->adr.tei == 0) + test_and_set_bit(OPTION_L2_PTP, &opt); + if (test_bit(MGR_OPT_NETWORK, &mgr->options)) { + if (crq->protocol == ISDN_P_LAPD_TE) + return -EPROTONOSUPPORT; + if ((crq->adr.tei != 0) && (crq->adr.tei != 127)) + return -EINVAL; + if (mgr->up) { + printk(KERN_WARNING + "%s: only one network manager is allowed\n", + __func__); + return -EBUSY; + } + } else if (test_bit(MGR_OPT_USER, &mgr->options)) { + if (crq->protocol == ISDN_P_LAPD_NT) + return -EPROTONOSUPPORT; + if ((crq->adr.tei >= 64) && (crq->adr.tei < GROUP_TEI)) + return -EINVAL; /* dyn tei */ + } else { + if (crq->protocol == ISDN_P_LAPD_NT) + test_and_set_bit(MGR_OPT_NETWORK, &mgr->options); + if (crq->protocol == ISDN_P_LAPD_TE) + test_and_set_bit(MGR_OPT_USER, &mgr->options); + } + if (mgr->ch.st->dev->Dprotocols + & ((1 << ISDN_P_TE_E1) | (1 << ISDN_P_NT_E1))) + test_and_set_bit(OPTION_L2_PMX, &opt); + if ((crq->protocol == ISDN_P_LAPD_NT) && (crq->adr.tei == 127)) { + mgr->up = crq->ch; + id = DL_INFO_L2_CONNECT; + teiup_create(mgr, DL_INFORMATION_IND, sizeof(id), &id); + crq->ch = NULL; + if (!list_empty(&mgr->layer2)) { + read_lock_irqsave(&mgr->lock, flags); + list_for_each_entry(l2, &mgr->layer2, list) { + l2->up = mgr->up; + l2->ch.ctrl(&l2->ch, OPEN_CHANNEL, NULL); + } + read_unlock_irqrestore(&mgr->lock, flags); + } + return 0; + } + l2 = create_l2(crq->ch, crq->protocol, (u_int)opt, + (u_long)crq->adr.tei); + if (!l2) + return -ENOMEM; + l2->tm = kzalloc(sizeof(struct teimgr), GFP_KERNEL); + if (!l2->tm) { + kfree(l2); + printk(KERN_ERR "kmalloc teimgr failed\n"); + return -ENOMEM; + } + l2->tm->mgr = mgr; + l2->tm->l2 = l2; + l2->tm->tei_m.debug = *debug & DEBUG_L2_TEIFSM; + l2->tm->tei_m.userdata = l2->tm; + l2->tm->tei_m.printdebug = tei_debug; + if (crq->protocol == ISDN_P_LAPD_TE) { + l2->tm->tei_m.fsm = &teifsmu; + l2->tm->tei_m.state = ST_TEI_NOP; + l2->tm->tval = 1000; /* T201 1 sec */ + } else { + l2->tm->tei_m.fsm = &teifsmn; + l2->tm->tei_m.state = ST_TEI_NOP; + l2->tm->tval = 2000; /* T202 2 sec */ + } + mISDN_FsmInitTimer(&l2->tm->tei_m, &l2->tm->timer); + write_lock_irqsave(&mgr->lock, flags); + id = get_free_id(mgr); + list_add_tail(&l2->list, &mgr->layer2); + write_unlock_irqrestore(&mgr->lock, flags); + if (id < 0) { + l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL); + } else { + l2->ch.nr = id; + l2->up->nr = id; + crq->ch = &l2->ch; + id = 0; + } + return id; +} + +static int +mgr_send(struct mISDNchannel *ch, struct sk_buff *skb) +{ + struct manager *mgr; + struct mISDNhead *hh = mISDN_HEAD_P(skb); + int ret = -EINVAL; + + mgr = container_of(ch, struct manager, ch); + if (*debug & DEBUG_L2_RECV) + printk(KERN_DEBUG "%s: prim(%x) id(%x)\n", + __func__, hh->prim, hh->id); + switch (hh->prim) { + case PH_DATA_IND: + mISDN_FsmEvent(&mgr->deact, EV_UI, NULL); + ret = ph_data_ind(mgr, skb); + break; + case PH_DATA_CNF: + do_ack(mgr, hh->id); + ret = 0; + break; + case PH_ACTIVATE_IND: + test_and_set_bit(MGR_PH_ACTIVE, &mgr->options); + mISDN_FsmEvent(&mgr->deact, EV_ACTIVATE_IND, NULL); + do_send(mgr); + ret = 0; + break; + case PH_DEACTIVATE_IND: + test_and_clear_bit(MGR_PH_ACTIVE, &mgr->options); + mISDN_FsmEvent(&mgr->deact, EV_DEACTIVATE_IND, NULL); + ret = 0; + break; + case DL_UNITDATA_REQ: + return dl_unit_data(mgr, skb); + } + if (!ret) + dev_kfree_skb(skb); + return ret; +} + +static int +free_teimanager(struct manager *mgr) +{ + struct layer2 *l2, *nl2; + + if (test_bit(MGR_OPT_NETWORK, &mgr->options)) { + /* not locked lock is taken in release tei */ + mgr->up = NULL; + if (test_bit(OPTION_L2_CLEANUP, &mgr->options)) { + list_for_each_entry_safe(l2, nl2, &mgr->layer2, list) { + put_tei_msg(mgr, ID_REMOVE, 0, l2->tei); + mutex_lock(&mgr->ch.st->lmutex); + list_del(&l2->ch.list); + mutex_unlock(&mgr->ch.st->lmutex); + l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL); + } + test_and_clear_bit(MGR_OPT_NETWORK, &mgr->options); + } else { + list_for_each_entry_safe(l2, nl2, &mgr->layer2, list) { + l2->up = NULL; + } + } + } + if (test_bit(MGR_OPT_USER, &mgr->options)) { + if (list_empty(&mgr->layer2)) + test_and_clear_bit(MGR_OPT_USER, &mgr->options); + } + mgr->ch.st->dev->D.ctrl(&mgr->ch.st->dev->D, CLOSE_CHANNEL, NULL); + return 0; +} + +static int +ctrl_teimanager(struct manager *mgr, void *arg) +{ + /* currently we only have one option */ + int clean = *((int *)arg); + + if (clean) + test_and_set_bit(OPTION_L2_CLEANUP, &mgr->options); + else + test_and_clear_bit(OPTION_L2_CLEANUP, &mgr->options); + return 0; +} + +/* This function does create a L2 for fixed TEI in NT Mode */ +static int +check_data(struct manager *mgr, struct sk_buff *skb) +{ + struct mISDNhead *hh = mISDN_HEAD_P(skb); + int ret, tei; + struct layer2 *l2; + + if (*debug & DEBUG_L2_CTRL) + printk(KERN_DEBUG "%s: prim(%x) id(%x)\n", + __func__, hh->prim, hh->id); + if (test_bit(MGR_OPT_USER, &mgr->options)) + return -ENOTCONN; + if (hh->prim != PH_DATA_IND) + return -ENOTCONN; + if (skb->len != 3) + return -ENOTCONN; + if (skb->data[0] != 0) + /* only SAPI 0 command */ + return -ENOTCONN; + if (!(skb->data[1] & 1)) /* invalid EA1 */ + return -EINVAL; + tei = skb->data[1] >> 0; + if (tei > 63) /* not a fixed tei */ + return -ENOTCONN; + if ((skb->data[2] & ~0x10) != SABME) + return -ENOTCONN; + /* We got a SABME for a fixed TEI */ + l2 = create_new_tei(mgr, tei); + if (!l2) + return -ENOMEM; + ret = l2->ch.send(&l2->ch, skb); + return ret; +} + +void +delete_teimanager(struct mISDNchannel *ch) +{ + struct manager *mgr; + struct layer2 *l2, *nl2; + + mgr = container_of(ch, struct manager, ch); + /* not locked lock is taken in release tei */ + list_for_each_entry_safe(l2, nl2, &mgr->layer2, list) { + mutex_lock(&mgr->ch.st->lmutex); + list_del(&l2->ch.list); + mutex_unlock(&mgr->ch.st->lmutex); + l2->ch.ctrl(&l2->ch, CLOSE_CHANNEL, NULL); + } + list_del(&mgr->ch.list); + list_del(&mgr->bcast.list); + skb_queue_purge(&mgr->sendq); + kfree(mgr); +} + +static int +mgr_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg) +{ + struct manager *mgr; + int ret = -EINVAL; + + mgr = container_of(ch, struct manager, ch); + if (*debug & DEBUG_L2_CTRL) + printk(KERN_DEBUG "%s(%x, %p)\n", __func__, cmd, arg); + switch (cmd) { + case OPEN_CHANNEL: + ret = create_teimgr(mgr, arg); + break; + case CLOSE_CHANNEL: + ret = free_teimanager(mgr); + break; + case CONTROL_CHANNEL: + ret = ctrl_teimanager(mgr, arg); + break; + case CHECK_DATA: + ret = check_data(mgr, arg); + break; + } + return ret; +} + +static int +mgr_bcast(struct mISDNchannel *ch, struct sk_buff *skb) +{ + struct manager *mgr = container_of(ch, struct manager, bcast); + struct mISDNhead *hh = mISDN_HEAD_P(skb); + struct sk_buff *cskb = NULL; + struct layer2 *l2; + u_long flags; + int ret; + + read_lock_irqsave(&mgr->lock, flags); + list_for_each_entry(l2, &mgr->layer2, list) { + if ((hh->id & MISDN_ID_SAPI_MASK) == + (l2->ch.addr & MISDN_ID_SAPI_MASK)) { + if (list_is_last(&l2->list, &mgr->layer2)) { + cskb = skb; + skb = NULL; + } else { + if (!cskb) + cskb = skb_copy(skb, GFP_KERNEL); + } + if (cskb) { + ret = l2->ch.send(&l2->ch, cskb); + if (ret) { + if (*debug & DEBUG_SEND_ERR) + printk(KERN_DEBUG + "%s ch%d prim(%x) addr(%x)" + " err %d\n", + __func__, l2->ch.nr, + hh->prim, l2->ch.addr, ret); + } else + cskb = NULL; + } else { + printk(KERN_WARNING "%s ch%d addr %x no mem\n", + __func__, ch->nr, ch->addr); + goto out; + } + } + } +out: + read_unlock_irqrestore(&mgr->lock, flags); + if (cskb) + dev_kfree_skb(cskb); + if (skb) + dev_kfree_skb(skb); + return 0; +} + +static int +mgr_bcast_ctrl(struct mISDNchannel *ch, u_int cmd, void *arg) +{ + + return -EINVAL; +} + +int +create_teimanager(struct mISDNdevice *dev) +{ + struct manager *mgr; + + mgr = kzalloc(sizeof(struct manager), GFP_KERNEL); + if (!mgr) + return -ENOMEM; + INIT_LIST_HEAD(&mgr->layer2); + mgr->lock = __RW_LOCK_UNLOCKED(mgr->lock); + skb_queue_head_init(&mgr->sendq); + mgr->nextid = 1; + mgr->lastid = MISDN_ID_NONE; + mgr->ch.send = mgr_send; + mgr->ch.ctrl = mgr_ctrl; + mgr->ch.st = dev->D.st; + set_channel_address(&mgr->ch, TEI_SAPI, GROUP_TEI); + add_layer2(&mgr->ch, dev->D.st); + mgr->bcast.send = mgr_bcast; + mgr->bcast.ctrl = mgr_bcast_ctrl; + mgr->bcast.st = dev->D.st; + set_channel_address(&mgr->bcast, 0, GROUP_TEI); + add_layer2(&mgr->bcast, dev->D.st); + mgr->deact.debug = *debug & DEBUG_MANAGER; + mgr->deact.userdata = mgr; + mgr->deact.printdebug = da_debug; + mgr->deact.fsm = &deactfsm; + mgr->deact.state = ST_L1_DEACT; + mISDN_FsmInitTimer(&mgr->deact, &mgr->datimer); + dev->teimgr = &mgr->ch; + return 0; +} + +int TEIInit(u_int *deb) +{ + debug = deb; + teifsmu.state_count = TEI_STATE_COUNT; + teifsmu.event_count = TEI_EVENT_COUNT; + teifsmu.strEvent = strTeiEvent; + teifsmu.strState = strTeiState; + mISDN_FsmNew(&teifsmu, TeiFnListUser, ARRAY_SIZE(TeiFnListUser)); + teifsmn.state_count = TEI_STATE_COUNT; + teifsmn.event_count = TEI_EVENT_COUNT; + teifsmn.strEvent = strTeiEvent; + teifsmn.strState = strTeiState; + mISDN_FsmNew(&teifsmn, TeiFnListNet, ARRAY_SIZE(TeiFnListNet)); + deactfsm.state_count = DEACT_STATE_COUNT; + deactfsm.event_count = DEACT_EVENT_COUNT; + deactfsm.strEvent = strDeactEvent; + deactfsm.strState = strDeactState; + mISDN_FsmNew(&deactfsm, DeactFnList, ARRAY_SIZE(DeactFnList)); + return 0; +} + +void TEIFree(void) +{ + mISDN_FsmFree(&teifsmu); + mISDN_FsmFree(&teifsmn); + mISDN_FsmFree(&deactfsm); +} diff --git a/drivers/isdn/mISDN/timerdev.c b/drivers/isdn/mISDN/timerdev.c new file mode 100644 index 000000000000..b5fabc7019d8 --- /dev/null +++ b/drivers/isdn/mISDN/timerdev.c @@ -0,0 +1,301 @@ +/* + * + * general timer device for using in ISDN stacks + * + * Author Karsten Keil <kkeil@novell.com> + * + * Copyright 2008 by Karsten Keil <kkeil@novell.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#include <linux/poll.h> +#include <linux/vmalloc.h> +#include <linux/timer.h> +#include <linux/miscdevice.h> +#include <linux/module.h> +#include <linux/mISDNif.h> + +static int *debug; + + +struct mISDNtimerdev { + int next_id; + struct list_head pending; + struct list_head expired; + wait_queue_head_t wait; + u_int work; + spinlock_t lock; /* protect lists */ +}; + +struct mISDNtimer { + struct list_head list; + struct mISDNtimerdev *dev; + struct timer_list tl; + int id; +}; + +static int +mISDN_open(struct inode *ino, struct file *filep) +{ + struct mISDNtimerdev *dev; + + if (*debug & DEBUG_TIMER) + printk(KERN_DEBUG "%s(%p,%p)\n", __func__, ino, filep); + dev = kmalloc(sizeof(struct mISDNtimerdev) , GFP_KERNEL); + if (!dev) + return -ENOMEM; + dev->next_id = 1; + INIT_LIST_HEAD(&dev->pending); + INIT_LIST_HEAD(&dev->expired); + spin_lock_init(&dev->lock); + dev->work = 0; + init_waitqueue_head(&dev->wait); + filep->private_data = dev; + __module_get(THIS_MODULE); + return 0; +} + +static int +mISDN_close(struct inode *ino, struct file *filep) +{ + struct mISDNtimerdev *dev = filep->private_data; + struct mISDNtimer *timer, *next; + + if (*debug & DEBUG_TIMER) + printk(KERN_DEBUG "%s(%p,%p)\n", __func__, ino, filep); + list_for_each_entry_safe(timer, next, &dev->pending, list) { + del_timer(&timer->tl); + kfree(timer); + } + list_for_each_entry_safe(timer, next, &dev->expired, list) { + kfree(timer); + } + kfree(dev); + module_put(THIS_MODULE); + return 0; +} + +static ssize_t +mISDN_read(struct file *filep, char *buf, size_t count, loff_t *off) +{ + struct mISDNtimerdev *dev = filep->private_data; + struct mISDNtimer *timer; + u_long flags; + int ret = 0; + + if (*debug & DEBUG_TIMER) + printk(KERN_DEBUG "%s(%p, %p, %d, %p)\n", __func__, + filep, buf, (int)count, off); + if (*off != filep->f_pos) + return -ESPIPE; + + if (list_empty(&dev->expired) && (dev->work == 0)) { + if (filep->f_flags & O_NONBLOCK) + return -EAGAIN; + wait_event_interruptible(dev->wait, (dev->work || + !list_empty(&dev->expired))); + if (signal_pending(current)) + return -ERESTARTSYS; + } + if (count < sizeof(int)) + return -ENOSPC; + if (dev->work) + dev->work = 0; + if (!list_empty(&dev->expired)) { + spin_lock_irqsave(&dev->lock, flags); + timer = (struct mISDNtimer *)dev->expired.next; + list_del(&timer->list); + spin_unlock_irqrestore(&dev->lock, flags); + if (put_user(timer->id, (int *)buf)) + ret = -EFAULT; + else + ret = sizeof(int); + kfree(timer); + } + return ret; +} + +static loff_t +mISDN_llseek(struct file *filep, loff_t offset, int orig) +{ + return -ESPIPE; +} + +static ssize_t +mISDN_write(struct file *filep, const char *buf, size_t count, loff_t *off) +{ + return -EOPNOTSUPP; +} + +static unsigned int +mISDN_poll(struct file *filep, poll_table *wait) +{ + struct mISDNtimerdev *dev = filep->private_data; + unsigned int mask = POLLERR; + + if (*debug & DEBUG_TIMER) + printk(KERN_DEBUG "%s(%p, %p)\n", __func__, filep, wait); + if (dev) { + poll_wait(filep, &dev->wait, wait); + mask = 0; + if (dev->work || !list_empty(&dev->expired)) + mask |= (POLLIN | POLLRDNORM); + if (*debug & DEBUG_TIMER) + printk(KERN_DEBUG "%s work(%d) empty(%d)\n", __func__, + dev->work, list_empty(&dev->expired)); + } + return mask; +} + +static void +dev_expire_timer(struct mISDNtimer *timer) +{ + u_long flags; + + spin_lock_irqsave(&timer->dev->lock, flags); + list_del(&timer->list); + list_add_tail(&timer->list, &timer->dev->expired); + spin_unlock_irqrestore(&timer->dev->lock, flags); + wake_up_interruptible(&timer->dev->wait); +} + +static int +misdn_add_timer(struct mISDNtimerdev *dev, int timeout) +{ + int id; + u_long flags; + struct mISDNtimer *timer; + + if (!timeout) { + dev->work = 1; + wake_up_interruptible(&dev->wait); + id = 0; + } else { + timer = kzalloc(sizeof(struct mISDNtimer), GFP_KERNEL); + if (!timer) + return -ENOMEM; + spin_lock_irqsave(&dev->lock, flags); + timer->id = dev->next_id++; + if (dev->next_id < 0) + dev->next_id = 1; + list_add_tail(&timer->list, &dev->pending); + spin_unlock_irqrestore(&dev->lock, flags); + timer->dev = dev; + timer->tl.data = (long)timer; + timer->tl.function = (void *) dev_expire_timer; + init_timer(&timer->tl); + timer->tl.expires = jiffies + ((HZ * (u_long)timeout) / 1000); + add_timer(&timer->tl); + id = timer->id; + } + return id; +} + +static int +misdn_del_timer(struct mISDNtimerdev *dev, int id) +{ + u_long flags; + struct mISDNtimer *timer; + int ret = 0; + + spin_lock_irqsave(&dev->lock, flags); + list_for_each_entry(timer, &dev->pending, list) { + if (timer->id == id) { + list_del_init(&timer->list); + del_timer(&timer->tl); + ret = timer->id; + kfree(timer); + goto unlock; + } + } +unlock: + spin_unlock_irqrestore(&dev->lock, flags); + return ret; +} + +static int +mISDN_ioctl(struct inode *inode, struct file *filep, unsigned int cmd, + unsigned long arg) +{ + struct mISDNtimerdev *dev = filep->private_data; + int id, tout, ret = 0; + + + if (*debug & DEBUG_TIMER) + printk(KERN_DEBUG "%s(%p, %x, %lx)\n", __func__, + filep, cmd, arg); + switch (cmd) { + case IMADDTIMER: + if (get_user(tout, (int __user *)arg)) { + ret = -EFAULT; + break; + } + id = misdn_add_timer(dev, tout); + if (*debug & DEBUG_TIMER) + printk(KERN_DEBUG "%s add %d id %d\n", __func__, + tout, id); + if (id < 0) { + ret = id; + break; + } + if (put_user(id, (int __user *)arg)) + ret = -EFAULT; + break; + case IMDELTIMER: + if (get_user(id, (int __user *)arg)) { + ret = -EFAULT; + break; + } + if (*debug & DEBUG_TIMER) + printk(KERN_DEBUG "%s del id %d\n", __func__, id); + id = misdn_del_timer(dev, id); + if (put_user(id, (int __user *)arg)) + ret = -EFAULT; + break; + default: + ret = -EINVAL; + } + return ret; +} + +static struct file_operations mISDN_fops = { + .llseek = mISDN_llseek, + .read = mISDN_read, + .write = mISDN_write, + .poll = mISDN_poll, + .ioctl = mISDN_ioctl, + .open = mISDN_open, + .release = mISDN_close, +}; + +static struct miscdevice mISDNtimer = { + .minor = MISC_DYNAMIC_MINOR, + .name = "mISDNtimer", + .fops = &mISDN_fops, +}; + +int +mISDN_inittimer(int *deb) +{ + int err; + + debug = deb; + err = misc_register(&mISDNtimer); + if (err) + printk(KERN_WARNING "mISDN: Could not register timer device\n"); + return err; +} + +void mISDN_timer_cleanup(void) +{ + misc_deregister(&mISDNtimer); +} |
