导航: 
楼 主:
218x定点运算实现g.729语音编解码218x定点运算实现g.729语音编解码
最初我学的是飞行器的设计，可是到了毕业的时候觉得人际关系复杂，改看语音编解码了。试着去研究g.723.1,离职回家之后，在家看论文，实现g.723.1,当时不知道网上还有现成的程序。后来到了深圳，从事mp3方案的开发，最初以为很深奥，其实就是一些简单外观编写，对于底层的核心软件全部控制在台湾，别人打包封装后才发给大陆。后来，我越来越知道理论设计与工程实现之后的距离。离开了公司，自己出来设计产品.我很坚定一点，把事情做到最好，不能糊弄事，否则事也会糊弄你的。几翻周折，多次设计，开壳，终于把一款学习设计好到生产，生计不是问题。才想到一个长期的项目，选择了无线的语音传输。当时想得很简单，从ali的录音里截出mp3的数据流，然后发射给接收，用比较便宜的m5661p进行解码就可以了。用的rf模块是华普的rf01,rf02.确实是实现了，因为ali的7101Q的mp3有比特率是24kbps,80kbps,64kbps,128kbps,320kbps.我们选择了24kbps,原因是当时选的rf 最大的波特率为128kbps.而且波特率最大，误码率在，灵敏度越小。我们lay实现了，发现了一个实际的问题。16K采样率，1150个数据，(我记不是很清楚)，还有ali的数据包是512 byte,最后到接收端延时有0.6秒。客户绝对是不会接收的，只好采用64kbps。延时有0.4s,而且rf频带加宽，客户要求有64个频道。无法实现。我们改换了cc1100,用fifo中断的形式实现了128kbps 的mp3,延时0.25秒.当时我们很兴奋，可是一装机，拿到实现环境地走一圈，有四十米，24kbps有70米，于是我们打朋友帮忙调天线，调外围的元器件。只可惜那朋友的老婆很厉害，我们不敢多打搅。好在得到消息，在接收端加了一颗安吉仑的放大。效果真的不一样，这次是100米，很稳定的100米。可是又遇到问题了，cc1100有近距离饱和的问题，三米以内无法收到任何数据。到了这个时候，我们没有做任何工程，是放弃还是重新开始。智者齐工，提到拿一颗218x dsp自己写程序，把语音进行压缩，这样一切都在掌握之后。很简单的，如果一帧数据出现错误你应该怎样处理。我看了很我的语音编解码，当时选用2.4kbps,结果播放音乐，及背景噪音特别大的情况下，无法接收。同学付工，在设计数字对讲机，距离有3公里，知道他们用的是melp.他说效果也可以，我也在网上下载了源程序，定点运算。国内的硕士对其研究，太复杂了。我没有信心去做它。结果在网站找到
gsm 13kbps.管主已经优化定点运算，我确实用汇编做了。而且也实现了。主频跑到12M就可实现，rf的波特率为21kbps,之所以这么大，主要是有前导码，并且rf从idle转化到rx(tx)需要几个ms的时间。如果你发射之后不去idle状态，频率会发生偏移，所以要定期进行校准。我们把板lay成了小模块。Mic 加了放大，但是一直很模糊，总觉得听不清楚，拿给销售的一听，别人还说音质很好。智者齐工一听，就只摇头，特别是吹口哨，金属声特别严重。当时我们在等一颗rf 芯片si4432,20dbm.要在2008年底出来，当时的时候是2008的八月份。我这时有点后悔，当时在选择语音编码的时候为什么没有试听对比了。确实它是不是自己想要的。我当时认为gsm 13kbps就是现在的手机算法，手机的算法是8kbps,而我用的是13kbps,效果应该更好才对。于是又才拿出手机，对比其音质，真的差得很远。网上查资料才知道手机的算法是g.729
网站,告诉我到什么地方找源代码。我从itu网站下载了g.729a,g.729ab,g.729b,g.729I。我想后来的版本一定是最好的，所以直接用一最后的一个版本。进行移植分为几个步骤：对下载的定点运算，把工程分为三个文件.encode.c,decode.c.,common.c. ； 把所有的函数局部变量改为全局变量（主要是为了好调试，同时进行汇编程序编写，它的stack没有多大，）;对其数学函数如add,sub,等，在Analog Devices\VisualDSP 3.5 16-Bit\218x\lib\src\libetsi_src(自己装的仿真软件)找到了其汇编程序。对其进行检验，因为g.729有很多的加减乘饱和判断，当时218x设计的时候已经考虑到这些问题，你只些用ena ar_sat,或者if mv sat mr.就可以轻松实现。同时修改这些函数，之前的是stack进行参数传递，我们得把它改为寄存器传递参数。其实最后得出比较重要且省指令的为Lshr;L_shl输入为mr1,mr0,ar. 对于L_msu,L_mac没有必要有。因为输入的长整型可以放在MR,你可以用指令。输入参数，ay1,ay0,mx0.my0:
Mr1 = ay1;
Mr0 = ay0;
Sr = ashift mr1 by 0(lo);
Mr2 = sr1; (符号位);
Mr = mr +mx0*my0(ss);
If mv sat mt;
(其默认为dis m_mode);这比库里的程序经简单得多。
还有就是在循环体内多动脑筋，尽量用寄存器，比如在四层循环时多一条指令，就是多出64*8条指令。还有一些技巧，如:
Cntr = x;
Do outloop until ce;
I1 = I2;
MODIFY(I3,M1);
MR = 0.MX0 = DM(I1,M1);
MY0 = DM(I3,M1);
CNTR = Y;
DO INNERLOOP UNTIL CE;
MR = MR+MX0*MY0(SS),MX0=DM(I1,M1);
INNERLOOP : MY0 = DM(I3,M1);
IF MV SAT MR;
Outloop:
DM(I6,M5) = MR1;
上面的程序看似很简单，其实可以简化，即把i1的DM变量copy到PM空间，用程序这样实现:
!copy (i1,i5,z);
Cntr = x;
Do outloop until ce;
I7 = I5;
MODIFY(I3,M1);
MR = 0.MX0 = DM(I1,M1),MY0=PM(I7,M5);
CNTR = Y;
DO INNERLOOP UNTIL CE;
INNERLOOP :
MR = MR+MX0*MY0(SS),MX0=DM(I1,M1),MY0=PM(I7,M5);
IF MV SAT MR;
Outloop: DM(I6,M5) = MR1;
这点技巧对于滤波计算及相关函数很有效。
我用的是visual dsp version 2.3.把让C语言运行，然后写汇编程序，main调用汇编程序（把控制权交给汇编），汇编程序调用C语言函数，这里把局部变量改为全局变量的必要性就出来了。对于一个C函数的编写，你可以写一半汇编，放心调用另一半C函数，而不必但心寄存器的初值。同时你还得有一份VC的程序。当你出错调试时，你可以对比VC的变量与你的全局变量。很快就找出问题。对于encode我选用了50帧的80word 做为输入，50 个10 bytes做为输出，很次改好一个C函数，运行一次，看运行的结果与50个10bytes进行比较，看是否一致.就这样改改停停，中途我孩子又出生了，加上还有学习的软件。到了一月份也就是在过年那几天才把编码程序写完，算了算也有二到三个月的时候。我一直担心的是运算量的问题，当时我用的是217x仿真去做，我心里越来越没有底，因为就50帧的数据，计算机仿真也在五分钟。我又看了国内的论文，有人提到用了89M可以实现。我就有了一点点底，别人也提到了一些简化的方法（论文我会以附件的形式给出）基音收索步长为2，大量测试减少饱和判断等等。当时我真是担心死了，好在我把仿真对象改到了2181，一运行，才得到了我写的汇编程序大约需要38M，这时我又到网上下载g729A版本，才发现里的已经简化了，正如我们国内硕士所说的方法，其实ITU早就做了这些工作。我忘了在什么忘网站上公布了其复杂度。其实g.729A编码为16M,g.729B为32M,我做的结果是38M。而且还没有优化，谢天谢地。可以也是无数祈祷的结果，荣耀归于神。
解码的程序就比较顺利，二周就写好了。运算的复杂度为8M.之后我移植到了硬件了，也就是几天的时候。结果不对，我知道也就是几条指令的原因：
Ay0 = 0;
Cntr = x;
Do searchmax UNTIL CE;
Ay0 = dm(i1,m1);
Af = ar – ay0
Searchmax:
If l t ar = pass ay0;
其实在这是错的，在软件仿真没有问题，移值后出了问题，及用cntr最后一条指令不能为if l t ar = pass ay0;
改为
If it ar = pass ay0;
Searchmax: nop;
还有一个注意的地方就是设
AR = DM(0x3FF3); /*SPORT0 WHEN MR0 = 0x8000,MR1++*/
AR = SETBIT 12 OF AR;
DM(0x3FF3) = AR;
在进行四舍五入的时候，mr0 = 0x8000为mr ++.
我把最后的结果写在下面
运算量 DM(ram) PM
Encode 36M 0x10b2 0xbe2
Decode 8M 0x1931 0x15f5
其实encode,decode公用程序为0x600.对于表格我是放在nor flash,启动之后拷到dm,大小为0xa8e,在上述dm内。
上面是软件仿真。拿到软件平台上实际测试：
Encode 60M用了 7.8ms, 32M 用了14.8M
Decode 32M 用了 3ms.
把rf 芯片加上去系统全部为60M,主要是rf占用了很多的时候，同时对于错误帧 的处理，还有自动静噪的处理等等。
教训，这次是幸运的，我应该在移植之前，充分确定其复杂度，占用系统资源大小。音质的确定，传音乐，各种情况下解码的输出是否可以接收。
基本的数学函数如下，对于最后的语音音质及所收集的论文资料，及定点程序我以附件的程序存放于
有兴趣的朋友可以去下载
因果: chenshiyangyi@163.com
.ENTRY L_mac_I1_I2;
L_mac_I1_I2:
M1 = 1;
MR = 0,MX0 = DM(I1,M1);
MY0= DM(I2,M1);
DO L_mac_I1_I2_Loop UNTIL CE;
MR = MR + MX0*MY0(SS),MX0 = DM(I1,M1);
L_mac_I1_I2_Loop:
MY0 = DM(I2,M1);
IF MV SAT MR;
RTS;
.ENTRY L_mac_I1_I5;
L_mac_I1_I5:
M1 = 1;
M5 = 1;
MR = 0,MX0 = DM(I1,M1),MY0 = PM(I5,M5);
DO L_mac_I1_I5_Loop UNTIL CE;
L_mac_I1_I5_Loop:
MR = MR + MX0*MY0(SS),MX0 = DM(I1,M1),MY0 = PM(I5,M5);
IF MV SAT MR;
RTS;
.EXTERNAL Overflow_;
/*****************************************************************/
/* Word16 div_s(Word16 var1, Word16 var2) */
/* */
/* var1: AY1 */
/* var2: AR */
/* result var_out: var1/var2 --AX1 */
/* */
/*****************************************************************/
.ENTRY div_s1_;
div_s1_:
AF = AR - AY1;
AX1 = 0x7fff;
IF EQ RTS;
IF LT JUMP div_err;
AF = PASS AR;
IF EQ JUMP div_zero;
IF LT JUMP div_err;
AF = PASS AY1;
AX1 = 0x0;
IF EQ RTS;
IF LT JUMP div_err;
AY0 = 0;
DIVS AY1, AR; /* perform fractional division*/
DIVQ AR;DIVQ AR;DIVQ AR;
DIVQ AR;DIVQ AR;DIVQ AR;
DIVQ AR;DIVQ AR;DIVQ AR;
DIVQ AR;DIVQ AR;DIVQ AR;
DIVQ AR;
DIVQ AR;
DIVQ AR;
AX1 = AY0;
RTS;
div_zero:
div_err:
AX1 = 0;
RTS;
.EXTERNAL DIV_L_num_;
.EXTERNAL DIV_denom_hi_;
.EXTERNAL DIV_denom_lo_;
.EXTERNAL DIV_approx_;
.EXTERNAL DIV_hi_;
.EXTERNAL DIV_lo_;
/*****************************************************************/
/* Word32 Div_32(Word32 L_num, Word16 denom_hi, Word16 denom_lo) */
/* L_num: MR1,MR0 */
/* denom_hi: AY1 */
/* denom_lo: AY0 */
/* result L_32: SR1,SR0 */
/* */
/*****************************************************************/
.ENTRY Div_321_;
Div_321_:
/* First approximation: 1/L_denom = 1/denom_hi */
DM(DIV_L_num_+0) = MR1;
DM(DIV_L_num_+1) = MR0;
DM(DIV_denom_hi_)= AY1;
DM(DIV_denom_lo_)= AY0;
AR = DM(DIV_denom_hi_); /* load denom_hi*/
AY1 = 0x3fff; /* div_s (0x3fff, denom_hi)*/
CALL div_s1_;
DM(DIV_approx_) = AX1;
/* 1/L_denom = approx * (2.0 - L_denom * approx) */
MX1 = DM(DIV_denom_hi_);
MX0 = DM(DIV_denom_lo_);
MY0 = AX1;
CALL Mpy_32_161_;
AX1 = 0x7fff;
AX0 = 0xffff;
AY1 = MR1;
AY0 = MR0;
CALL L_sub1_; /* 0x7fffffff-result*/
MR1 = AY1;
MR0 = AY0;
CALL L_Extract1_; /* L_Extract (result, &h, &l)*/
MX1 = MR1;
MX0 = MR0;
MY0 = DM(DIV_approx_);
CALL Mpy_32_161_;
CALL L_Extract1_;
DM(DIV_hi_) = MR1;
DM(DIV_lo_) = MR0;
MR1 = DM(DIV_L_num_+0);
MR0 = DM(DIV_L_num_+1);
CALL L_Extract1_;
MX1 = MR1;
MX0 = MR0;
MY1 = DM(DIV_hi_);
MY0 = DM(DIV_lo_);
CALL Mpy_321_;
AR = 2;
CALL L_shl1_;
RTS;
/************************************************************/
/* Word16 abs_s(Word16 var1) */
/* */
/* var1: */
/* AR */
/* */
/* return 16bit AR */
/************************************************************/
.ENTRY abs_s1_;
abs_s1_:
AR = ABS AR;
AY1 = 0x7FFF;
IF AV AR = PASS AY1;
RTS;
/************************************************************/
/* Word32 L_abs(Word32 L_var1) */
/* */
/* L_var1: */
/* SR1,Hi 16 bit */
/* SR0,Lo 16bit */
/* */
/* return 32bit:SR1,SR0 */
/************************************************************/
.ENTRY L_abs1_;
L_abs1_:
AR = PASS SR1;
IF GE RTS;
AR = -SR0;
SR0 = AR;
AY1 = 0;
AR = AY1-SR1+C-1;
SR1 = AR;
IF NOT AV RTS;
SR1 = 0x7fff;
SR0 = 0xffff;
RTS;
/************************************************************/
/* Word16 negate(Word16 var1) */
/* */
/* var1: */
/* AR */
/* */
/* return 16bit:AR */
/************************************************************/
.ENTRY negate1_;
negate1_:
AR = -AR;
AY1 = 0x7FFF;
IF AV AR = PASS AY1;
RTS;
/************************************************************/
/* Word32 L_negate(Word32 L_var1) */
/* */
/* L_var1: */
/* SR1,Hi 16 bit */
/* SR0,Lo 16bit */
/* */
/* return 32bit:SR1,SR0 */
/************************************************************/
.ENTRY L_negate1_;
L_negate1_:
AR = SR0; /* read input*/
AR = -AR;
SR0 = AR;
AY1 = 0;
AR = AY1 - SR1 + C -1;
SR1 = AR;
IF NOT AV RTS;
SR1 = 0x7fff;
SR0 = 0xffff;
RTS;
/************************************************************/
/* Word32 L_Comp(Word16 hi, Word16 lo) */
/* L_32 = hi < <16 + lo < <1 */
/* hi : MY1 */
/* lo : MY0 */
/* return 32bit : MR1,MR0 */
/************************************************************/
.ENTRY L_Comp1_;
L_Comp1_:
MR = 0;
MR1 = MY1;
AR = 1;
MR = MR + AR * MY0 (SS); /* perform MAC*/
IF MV SAT MR; /* saturate if overflow*/
RTS;
/************************************************************/
/* Word32 L_mult(Word16 var1,Word16 var2) */
/* */
/* MX0: input lo */
/* MY0: input n */
/* MR1,MR0 : return 32bit number Hi,Lo */
/************************************************************/
.ENTRY L_mult1_;
L_mult1_:
MR = MX0 * MY0 (SS); /* perform multiplication*/
IF MV SAT MR; /* saturate if overflow*/
RTS;
/************************************************************/
/* Word16 mult_r(Word16 var1,Word16 var2) */
/* */
/* MX0: input lo */
/* MY0: input n */
/* AX1 : return 32bit number Hi,Lo */
/************************************************************/
.ENTRY mult_r1_;
mult_r1_:
MR = MR1 * MY1 (RND); /* perform multiply*/
IF MV SAT MR; /* saturate if overflow*/
AX1 = MR1;
RTS;
/************************************************************/
/* Word16 mult(Word16 var1,Word16 var2) */
/* */
/* MX0: input lo */
/* MY0: input n */
/* MR1 : return 16bit number Hi,Lo */
/************************************************************/
.ENTRY mult1_;
mult1_:
MR = MX0 * MY0 (SS); /* perform multiply*/
IF MV SAT MR; /* saturate if overflow*/
RTS;
/************************************************************/
/*L_32 = L_mult(hi, n); */
/*L_32 = L_mac(L_32, mult(lo, n) , 1); */
/* */
/* MX1: input hi */
/* MX0: input lo */
/* MY0: input n */
/* MR1,MR0 : return 32bit number Hi,Lo */
/************************************************************/
.ENTRY Mpy_32_161_;
Mpy_32_161_:
MR = MX1 * MY0 (SS); /* L_mult (hi, n)*/
IF MV SAT MR; /* saturate if overflow*/
SR1 = MR1; /* save result of L_mult in SR */
SR0 = MR0;
MR = MX0 * MY0 (SS); /* mult (lo, n)*/
IF MV SAT MR; /* saturate if overflow*/
MX0 = MR1;
MR1 = SR1; MR0 = SR0;
MY0 = 1;
MR = MR + MX0 * MY0 (SS); /* do MAC*/
IF MV SAT MR; /* saturate if overflow*/
RTS;
/***************************************************************/
/*Word32 Mpy_32(Word16 hi1, Word16 lo1, Word16 hi2, Word16 lo2)*/
/* */
/* hi1: MX1 */
/* lo1: MX0 */
/* hi2: MY1 */
/* lo2: MY0 */
/* return 32bit : MR1,MR0 */
/***************************************************************/
.ENTRY Mpy_321_;
Mpy_321_:
/* L_mult (hi1, hi2), read lo2*/
MR = MX1 * MY1 (SS);
IF MV SAT MR;
SR1 = MR1;
SR0 = MR0;
MR = MX1 * MY0 (SS);
IF MV SAT MR;
AR = MR1;
MR = MX0 * MY1 (SS); /* mult (lo1, hi2)*/
MX0 = AR;
IF MV SAT MR;
MX1 = MR1;
MY0 = 1; /* perform MACs*/
MR1 = SR1; MR0 = SR0;
MR = MR + MX0 * MY0 (SS);
IF MV SAT MR;
MR = MR + MX1 * MY0 (SS);
IF MV SAT MR;
RTS; /* return*/
/************************************************************/
/*Word32 L_add(Word32 L_var1, Word32 L_var2) */
/* input parameter: */
/* AX1: L_var2 Hi 16bit */
/* AX0: L_var2 Lo 16bit */
/* AY1: L_var1 Hi 16bit */
/* AY0: L_var1 Lo 16bit */
/*return value: AY1, AY0 */
/************************************************************/
.ENTRY L_add1_;
L_add1_:
#ifdef __NO_ETSI_FLAGS
SI = MSTAT; /* save current MSTAT*/
AF = AX0 + AY0; /* add lsw's of input*/
ENA AR_SAT; /* enable ALU staturation*/
AR = AX1 + AY1 + C; /* add msw's of input*/
MSTAT = SI; /* restore MSTAT*/
IF AV AF = TGLBIT 15 OF AR;
AR = PASS AF, AY1 = AR;
AY0 = AR;
MSTAT = SI;
RTS;
#else
AR = AX0 + AY0; /* add lsw's of input*/
AY0 = AR;
AR = AX1 + AY1 + C; /* add msw's of input*/
AY1 = AR; /* if neither, return*/
IF NOT AV RTS; /* test for overflow or underflow*/
/* can only overflow or underflow if*/
/* inputs are the same sign.*/
/* positive inputs == overflow*/
/* negative inputs == underflow*/
AY1 = 0x8000;
!!AF = TSTBIT 15 OF AX1;
AF = AX1 AND AY1;
AY1 = 0x7fff;
AY0 = 0xffff;
IF GE RTS; /* positive inputs, overflow*/
AY1 = 0x8000;
AY0 = 0x0000;
RTS; /* negative inputs, underflow*/
#endif
/************************************************************/
/*Word32 L_sub(Word32 L_var1, Word32 L_var2) */
/* input parameter: */
/* AX1: L_var2 Hi 16bit */
/* AX0: L_var2 Lo 16bit */
/* AY1: L_var1 Hi 16bit */
/* AY0: L_var1 Lo 16bit */
/*return value: AY1, AY0 */
/************************************************************/
.ENTRY L_sub1_;
L_sub1_:
AR = AX0 - AY0; /* sub lsw's of input*/
AY0 = AR;
AR = AX1 - AY1 + C -1; /* sub msw's of input*/
AY1 = AR;
IF NOT AV RTS; /* test for overflow or underflow*/
AY1 = 0x8000;
!!AF = TSTBIT 15 OF AX1;
AF = AX1 AND AY1;
/* can only overflow or underflow if*/
/* inputs are different signs.*/
/* +- inputs == overflow*/
/* -+ inputs == underflow*/
AY1 = 0x7fff;
AY0 = 0xffff;
IF GE RTS; /* overflow*/
AY1 = 0x8000;
AY0 = 0x0000;
RTS; /* underflow*/
/**************************************************************/
/* */
/* Word16 round(Word32 L_var1) */
/* L_var1 32bit: */
/* AY1: Hi 16bit. */
/* AY0: Lo 16bit. */
/* */
/* return value:AX1 */
/**************************************************************/
.ENTRY round1_;
round1_:
SI = MSTAT;
ENA AR_SAT;
AF = PASS AY0;
/* if top bit of lower word set, round*/
AR = AY1;
IF LT AR = AY1 + 1;
AX1 = AR;
MSTAT = SI;
RTS;
/*************************************************************/
/* */
/* Word32 L_shr_r(Word32 L_var1, Word16 var2) */
/* */
/* L_var1 32 bits: */
/* MR1: Hi 16bit. */
/* MR0: Lo 16bit. */
/* var2 16bits: AR */
/* return 32bits regester: SR1 SR0 */
/*************************************************************/
.ENTRY L_shr_r1_;
L_shr_r1_:
AY1 = 32;
AF = AR - AY1;
SR = LSHIFT AR BY -33 (HI); /* zero SR*/
IF GE RTS;
AR = PASS AR;
IF LE JUMP L_shr1_; /* no rounding required, don't return*/
AY0 = AR;
CALL L_shr1_;
AR = AY0 - 1;
SE = AR;
AX1 = SR1;
AX0 = SR0;
AR = 1;
SR = LSHIFT AR (LO); /* shift 1 by shift value minus 1*/
AY0 = SR0;
AF = MR0 AND AY0;
AY0 = SR1;
IF EQ AF = MR1 AND AY0;
SR1 = AX1;
SR0 = AX0;
IF EQ RTS;
AY1 = 1;
AR = AX0 + AY1; /* add one to round*/
SR0 = AR;
AY1 = 0;
AR = AX1 + AY1 + C;
SR1 = AR;
#ifdef __NO_ETSI_FLAGS
RTS;
#else
IF NOT AV RTS; /* test for overflow*/
SR1 = 0x7fff;
SR0 = 0xffff;
RTS; /* overflow*/
/* cannot underflow*/
#endif
/*************************************************************/
/* */
/* Word32 L_shr(Word32 L_var1, Word16 var2) */
/* */
/* L_var1 32 bits: */
/* SR1: Hi 16bit. */
/* SR0: Lo 16bit. */
/* var2 16bits: AR */
/* return 32bits regester: SR1 SR0 */
/*************************************************************/
.ENTRY L_shr1_;
L_shr1_:
SE = EXP MR1 (HI); /* read L_var1 LSW*/
SE = EXP MR0 (LO); /* read shift value*/
AR = -AR;
IF EQ JUMP l_shift_common;
IF LT JUMP do_shiftR;
AY1 = 32;
AF = AR - AY1;
IF ac AR = PASS AY1; /* if shift amount < 32, make it 32*/
JUMP L_shl2_;
do_shiftR:
SE = AR;
SR = LSHIFT MR0 (LO), SI = MR1; /* shift in LSW*/
SR = SR OR ASHIFT SI (HI); /* shift in MSW*/
RTS;
/*************************************************************/
/* */
/* Word32 L_shl(Word32 L_var1, Word16 var2) */
/* */
/* L_var1 32 bits: */
/* SR1: Hi 16bit. */
/* SR0: Lo 16bit. */
/* var2 16bits: AR */
/* return 32bits regester: SR1 SR0 */
/*************************************************************/
.ENTRY L_shl1_;
L_shl1_:
AR = PASS AR;
IF EQ JUMP l_shift_common;
SE = EXP MR1 (HI); /* calculate number of sign bits*/
SE = EXP MR0 (LO); /* read var2*/
L_shl2_: /* SR1:0==var1,*/
/* SE==derived exponent of var1*/
/* AR==var2*/
AY1 = SE;
AF = AR + AY1, AX1 = MR1; /* will shift overflow?*/
IF GT JUMP set_oflow4; /* yes, then go handle*/
SE = AR; /* otherwise perform shift*/
SR = LSHIFT MR0 (LO), AR = MR1; /* shift in LSW*/
SR = SR OR ASHIFT AR (HI); /* shift in MSW*/
RTS;
set_oflow4:
AR = 0x7FFF;
AF = PASS AX1;
AY1 = 0x8000;
IF LT AR = PASS AY1;
SR1 = AR;
AY1 = 0x8000;
AR = AR XOR AY1;
!! AR = TGLBIT 15 OF AR;
SR0 = AR;
RTS;
.ENTRY l_shift_common;
l_shift_common:
SR1 = MR1;
SR0 = MR0;
RTS;
/************************************************************/
/* Word16 shr(Word16 var1,Word16 var2) */
/* */
/* 16bit var1: AX1 */
/* 16bit var2: AR */
/* return 16bit: AX1 */
/* */
/************************************************************/
.ENTRY shr1_;
shr1_: /* var1 in AX1, var2 in AR*/
AR = -AR, SI = AX1;
IF EQ RTS;
SE = EXP SI (HI), AX0 = AR;
IF GT JUMP SHL_FUNC;
SE = AR; /* perform shift*/
SR = ASHIFT SI (HI);
AX1 = SR1;
RTS;
/************************************************************/
/* Word16 shl(Word16 var1,Word16 var2) */
/* */
/* 16bit var1: AX1 */
/* 16bit var2: AR */
/* return 16bit: AX1 */
/* */
/************************************************************/
.ENTRY shl1_;
shl1_:
SI = AX1;
SE = EXP SI (HI), AX0 = AR; /* calculate number of sign bits*/
/* read var2*/
AF = PASS AX0, AX1 = SI;
IF EQ RTS; /* if var2==0, return var1*/
SHL_FUNC: /* SI==var1, */
/* SE==derived exponent of var1*/
/* AX0==var2*/
AY1 = SE;
AR = AX0 + AY1, AX1 = SI; /* will shift overflow?*/
IF GT JUMP set_oflow3; /* yes, then go handle*/
SE = AX0; /* otherwise perform shift*/
IF LE SR = ASHIFT SI (LO);
AX1 = SR0;
RTS;
set_oflow3:
AR = 0x7FFF;
AF = PASS AX1;
AY1 = 0x8000;
IF LT AR = PASS AY1;
AX1 = AR;
RTS;
/************************************************************/
/* void L_Extract(Word32 L_32, Word16 *hi, Word16 *lo) */
/* L_32: */
/* MR1: Hi 16 bit. */
/* MR0: Lo 16 bit. */
/* */
/* return two 16 bit regester:MR1,MR0 */
/* */
/************************************************************/
.ENTRY L_Extract1_;
L_Extract1_:
MX0 = MR1;
/* shift L_32, assign to hi*/
SR = LSHIFT MR0 BY -1(LO);
SR = SR OR ASHIFT MR1 BY -1(HI); /* shift in MSW*/
MR1 = SR1;
MR0 = SR0;
MY0 = 0x4000;
MR = MR - MX0 * MY0 (SS);
IF MV SAT MR;
MR1 = MX0;
RTS;
/************************************************************/
/* */
/* Word16 add(Word16 var1,Word16 var2) */
/* L_somme = (Word32) var1 + var2; */
/* var_out = sature(L_somme); */
/* */
/* 16bit var1: AX1 */
/* 16bit var2: AY1 */
/* */
/* return 16bit regester: AX1 */
/* */
/************************************************************/
.ENTRY add1_;
add1_:
SI = MSTAT; /* save current MSTAT */
ENA AR_SAT; /* enable ALU staturation*/
AR = AX1 + AY1; /* perform addition */
AX1 = AR;
MSTAT = SI; /* restore MSTAT*/
RTS;
/************************************************************/
/* */
/* Word16 sub(Word16 var1,Word16 var2) */
/* L_diff = (Word32) var1 - var2; */
/* var_out = sature(L_diff); */
/* */
/* 16bit var1: AX1 */
/* 16bit var2: AY1 */
/* */
/* return 16bit regester: AX1 */
/* */
/************************************************************/
.ENTRY sub1_;
sub1_:
SI = MSTAT; /* save current MSTAT*/
ENA AR_SAT; /* enable ALU staturation*/
AR = AX1 - AY1; /* perform subtraction*/
AX1 = AR;
MSTAT = SI; /* restore MSTAT*/
RTS;
/************************************************************/
/* */
/* Word16 norm_s(Word16 var1) */
/* 16 bit var1: */
/* AR */
/* return regester: AR */
/* */
/* */
/************************************************************/
.ENTRY norm_s1_;
norm_s1_:
AF = PASS AR;
IF NE SE = EXP AR (HI);
AR = SE;
IF EQ AR = PASS AF;
AR = -AR;
RTS;
/************************************************************/
/* */
/* Word16 norm_l(Word32 L_var1) */
/* */
/* L_var1: */
/* MR1 Hi 16bit,MR0 Lo 16bit */
/* */
/* return regester: AR */
/************************************************************/
.ENTRY norm_l1_;
norm_l1_:
AY1 = MR1; /* read input LSW*/
AF = MR0 OR AY1;
IF NE SE = EXP MR1 (HI);
IF NE SE = EXP MR0 (LO);
AR = SE;
IF EQ AR = PASS 0; /* if input is zero, return zero*/
AR = -AR;
RTS;