/* ** $Id: lvm.c,v 2.63.1.3 2007/12/28 15:32:23 roberto Exp $ ** Lua virtual machine ** See Copyright Notice in lua.h */ #include #include #include #define lvm_c #define LUA_CORE #include "lua.h" #include "ldebug.h" #include "ldo.h" #include "lfunc.h" #include "lgc.h" #include "lobject.h" #include "lopcodes.h" #include "lstate.h" #include "lstring.h" #include "ltable.h" #include "ltm.h" #include "lvm.h" /* limit for table tag-method chains (to avoid loops) */ #define MAXTAGLOOP 100 const TValue *luaV_tonumber (const TValue *obj, TValue *n) { lua_Number num; if (ttisnumber(obj)) return obj; if (ttisstring(obj) && luaO_str2d(svalue(obj), &num)) { setnvalue(n, num); return n; } else return NULL; } int luaV_tostring (lua_State *L, StkId obj) { if (!ttisnumber(obj)) return 0; else { char s[LUAI_MAXNUMBER2STR]; lua_Number n = nvalue(obj); lua_number2str(s, n); setsvalue2s(L, obj, luaS_new(L, s)); return 1; } } static void traceexec (lua_State *L, const Instruction *pc) { lu_byte mask = L->hookmask; const Instruction *oldpc = L->savedpc; L->savedpc = pc; if ((mask & LUA_MASKCOUNT) && L->hookcount == 0) { resethookcount(L); luaD_callhook(L, LUA_HOOKCOUNT, -1); } if (mask & LUA_MASKLINE) { Proto *p = ci_func(L->ci)->l.p; int npc = pcRel(pc, p); int newline = getline(p, npc); /* call linehook when enter a new function, when jump back (loop), or when enter a new line */ if (npc == 0 || pc <= oldpc || newline != getline(p, pcRel(oldpc, p))) luaD_callhook(L, LUA_HOOKLINE, newline); } } static void callTMres (lua_State *L, StkId res, const TValue *f, const TValue *p1, const TValue *p2) { ptrdiff_t result = savestack(L, res); setobj2s(L, L->top, f); /* push function */ setobj2s(L, L->top+1, p1); /* 1st argument */ setobj2s(L, L->top+2, p2); /* 2nd argument */ luaD_checkstack(L, 3); L->top += 3; luaD_call(L, L->top - 3, 1); res = restorestack(L, result); L->top--; setobjs2s(L, res, L->top); } static void callTM (lua_State *L, const TValue *f, const TValue *p1, const TValue *p2, const TValue *p3) { setobj2s(L, L->top, f); /* push function */ setobj2s(L, L->top+1, p1); /* 1st argument */ setobj2s(L, L->top+2, p2); /* 2nd argument */ setobj2s(L, L->top+3, p3); /* 3th argument */ luaD_checkstack(L, 4); L->top += 4; luaD_call(L, L->top - 4, 0); } void luaV_gettable (lua_State *L, const TValue *t, TValue *key, StkId val) { int loop; for (loop = 0; loop < MAXTAGLOOP; loop++) { const TValue *tm; if (ttistable(t)) { /* `t' is a table? */ Table *h = hvalue(t); const TValue *res = luaH_get(h, key); /* do a primitive get */ if (!ttisnil(res) || /* result is no nil? */ (tm = fasttm(L, h->metatable, TM_INDEX)) == NULL) { /* or no TM? */ setobj2s(L, val, res); return; } /* else will try the tag method */ } else if (ttisnil(tm = luaT_gettmbyobj(L, t, TM_INDEX))) luaG_typeerror(L, t, "index"); if (ttisfunction(tm)) { callTMres(L, val, tm, t, key); return; } t = tm; /* else repeat with `tm' */ } luaG_runerror(L, "loop in gettable"); } void luaV_settable (lua_State *L, const TValue *t, TValue *key, StkId val) { int loop; for (loop = 0; loop < MAXTAGLOOP; loop++) { const TValue *tm; if (ttistable(t)) { /* `t' is a table? */ Table *h = hvalue(t); TValue *oldval = luaH_set(L, h, key); /* do a primitive set */ if (!ttisnil(oldval) || /* result is no nil? */ (tm = fasttm(L, h->metatable, TM_NEWINDEX)) == NULL) { /* or no TM? */ setobj2t(L, oldval, val); luaC_barriert(L, h, val); return; } /* else will try the tag method */ } else if (ttisnil(tm = luaT_gettmbyobj(L, t, TM_NEWINDEX))) luaG_typeerror(L, t, "index"); if (ttisfunction(tm)) { callTM(L, tm, t, key, val); return; } t = tm; /* else repeat with `tm' */ } luaG_runerror(L, "loop in settable"); } static int call_binTM (lua_State *L, const TValue *p1, const TValue *p2, StkId res, TMS event) { const TValue *tm = luaT_gettmbyobj(L, p1, event); /* try first operand */ if (ttisnil(tm)) tm = luaT_gettmbyobj(L, p2, event); /* try second operand */ if (ttisnil(tm)) return 0; callTMres(L, res, tm, p1, p2); return 1; } static const TValue *get_compTM (lua_State *L, Table *mt1, Table *mt2, TMS event) { const TValue *tm1 = fasttm(L, mt1, event); const TValue *tm2; if (tm1 == NULL) return NULL; /* no metamethod */ if (mt1 == mt2) return tm1; /* same metatables => same metamethods */ tm2 = fasttm(L, mt2, event); if (tm2 == NULL) return NULL; /* no metamethod */ if (luaO_rawequalObj(tm1, tm2)) /* same metamethods? */ return tm1; return NULL; } static int call_orderTM (lua_State *L, const TValue *p1, const TValue *p2, TMS event) { const TValue *tm1 = luaT_gettmbyobj(L, p1, event); const TValue *tm2; if (ttisnil(tm1)) return -1; /* no metamethod? */ tm2 = luaT_gettmbyobj(L, p2, event); if (!luaO_rawequalObj(tm1, tm2)) /* different metamethods? */ return -1; callTMres(L, L->top, tm1, p1, p2); return !l_isfalse(L->top); } static int l_strcmp (const TString *ls, const TString *rs) { const char *l = getstr(ls); size_t ll = ls->tsv.len; const char *r = getstr(rs); size_t lr = rs->tsv.len; for (;;) { int temp = strcoll(l, r); if (temp != 0) return temp; else { /* strings are equal up to a `\0' */ size_t len = strlen(l); /* index of first `\0' in both strings */ if (len == lr) /* r is finished? */ return (len == ll) ? 0 : 1; else if (len == ll) /* l is finished? */ return -1; /* l is smaller than r (because r is not finished) */ /* both strings longer than `len'; go on comparing (after the `\0') */ len++; l += len; ll -= len; r += len; lr -= len; } } } int luaV_lessthan (lua_State *L, const TValue *l, const TValue *r) { int res; if (ttype(l) != ttype(r)) return luaG_ordererror(L, l, r); else if (ttisnumber(l)) return luai_numlt(nvalue(l), nvalue(r)); else if (ttisstring(l)) return l_strcmp(rawtsvalue(l), rawtsvalue(r)) < 0; else if ((res = call_orderTM(L, l, r, TM_LT)) != -1) return res; return luaG_ordererror(L, l, r); } static int lessequal (lua_State *L, const TValue *l, const TValue *r) { int res; if (ttype(l) != ttype(r)) return luaG_ordererror(L, l, r); else if (ttisnumber(l)) return luai_numle(nvalue(l), nvalue(r)); else if (ttisstring(l)) return l_strcmp(rawtsvalue(l), rawtsvalue(r)) <= 0; else if ((res = call_orderTM(L, l, r, TM_LE)) != -1) /* first try `le' */ return res; else if ((res = call_orderTM(L, r, l, TM_LT)) != -1) /* else try `lt' */ return !res; return luaG_ordererror(L, l, r); } int luaV_equalval (lua_State *L, const TValue *t1, const TValue *t2) { const TValue *tm; lua_assert(ttype(t1) == ttype(t2)); switch (ttype(t1)) { case LUA_TNIL: return 1; case LUA_TNUMBER: return luai_numeq(nvalue(t1), nvalue(t2)); case LUA_TBOOLEAN: return bvalue(t1) == bvalue(t2); /* true must be 1 !! */ case LUA_TLIGHTUSERDATA: return pvalue(t1) == pvalue(t2); case LUA_TUSERDATA: { if (uvalue(t1) == uvalue(t2)) return 1; tm = get_compTM(L, uvalue(t1)->metatable, uvalue(t2)->metatable, TM_EQ); break; /* will try TM */ } case LUA_TTABLE: { if (hvalue(t1) == hvalue(t2)) return 1; tm = get_compTM(L, hvalue(t1)->metatable, hvalue(t2)->metatable, TM_EQ); break; /* will try TM */ } default: return gcvalue(t1) == gcvalue(t2); } if (tm == NULL) return 0; /* no TM? */ callTMres(L, L->top, tm, t1, t2); /* call TM */ return !l_isfalse(L->top); } void luaV_concat (lua_State *L, int total, int last) { do { StkId top = L->base + last + 1; int n = 2; /* number of elements handled in this pass (at least 2) */ if (!(ttisstring(top-2) || ttisnumber(top-2)) || !tostring(L, top-1)) { if (!call_binTM(L, top-2, top-1, top-2, TM_CONCAT)) luaG_concaterror(L, top-2, top-1); } else if (tsvalue(top-1)->len == 0) /* second op is empty? */ (void)tostring(L, top - 2); /* result is first op (as string) */ else { /* at least two string values; get as many as possible */ size_t tl = tsvalue(top-1)->len; char *buffer; int i; /* collect total length */ for (n = 1; n < total && tostring(L, top-n-1); n++) { size_t l = tsvalue(top-n-1)->len; if (l >= MAX_SIZET - tl) luaG_runerror(L, "string length overflow"); tl += l; } buffer = luaZ_openspace(L, &G(L)->buff, tl); tl = 0; for (i=n; i>0; i--) { /* concat all strings */ size_t l = tsvalue(top-i)->len; memcpy(buffer+tl, svalue(top-i), l); tl += l; } setsvalue2s(L, top-n, luaS_newlstr(L, buffer, tl)); } total -= n-1; /* got `n' strings to create 1 new */ last -= n-1; } while (total > 1); /* repeat until only 1 result left */ } static void Arith (lua_State *L, StkId ra, const TValue *rb, const TValue *rc, TMS op) { TValue tempb, tempc; const TValue *b, *c; if ((b = luaV_tonumber(rb, &tempb)) != NULL && (c = luaV_tonumber(rc, &tempc)) != NULL) { lua_Number nb = nvalue(b), nc = nvalue(c); switch (op) { case TM_ADD: setnvalue(ra, luai_numadd(nb, nc)); break; case TM_SUB: setnvalue(ra, luai_numsub(nb, nc)); break; case TM_MUL: setnvalue(ra, luai_nummul(nb, nc)); break; case TM_DIV: setnvalue(ra, luai_numdiv(nb, nc)); break; case TM_MOD: setnvalue(ra, luai_nummod(nb, nc)); break; case TM_POW: setnvalue(ra, luai_numpow(nb, nc)); break; case TM_UNM: setnvalue(ra, luai_numunm(nb)); break; default: lua_assert(0); break; } } else if (!call_binTM(L, rb, rc, ra, op)) luaG_aritherror(L, rb, rc); } /* ** some macros for common tasks in `luaV_execute' */ #define runtime_check(L, c) { if (!(c)) break; } #define RA(i) (base+GETARG_A(i)) /* to be used after possible stack reallocation */ #define RB(i) check_exp(getBMode(GET_OPCODE(i)) == OpArgR, base+GETARG_B(i)) #define RC(i) check_exp(getCMode(GET_OPCODE(i)) == OpArgR, base+GETARG_C(i)) #define RKB(i) check_exp(getBMode(GET_OPCODE(i)) == OpArgK, \ ISK(GETARG_B(i)) ? k+INDEXK(GETARG_B(i)) : base+GETARG_B(i)) #define RKC(i) check_exp(getCMode(GET_OPCODE(i)) == OpArgK, \ ISK(GETARG_C(i)) ? k+INDEXK(GETARG_C(i)) : base+GETARG_C(i)) #define KBx(i) check_exp(getBMode(GET_OPCODE(i)) == OpArgK, k+GETARG_Bx(i)) #define dojump(L,pc,i) {(pc) += (i); luai_threadyield(L);} #define Protect(x) { L->savedpc = pc; {x;}; base = L->base; } #define arith_op(op,tm) { \ TValue *rb = RKB(i); \ TValue *rc = RKC(i); \ if (ttisnumber(rb) && ttisnumber(rc)) { \ lua_Number nb = nvalue(rb), nc = nvalue(rc); \ setnvalue(ra, op(nb, nc)); \ } \ else \ Protect(Arith(L, ra, rb, rc, tm)); \ } void luaV_execute (lua_State *L, int nexeccalls) { LClosure *cl; StkId base; TValue *k; const Instruction *pc; reentry: /* entry point */ lua_assert(isLua(L->ci)); pc = L->savedpc; cl = &clvalue(L->ci->func)->l; base = L->base; k = cl->p->k; /* main loop of interpreter */ for (;;) { const Instruction i = *pc++; StkId ra; if ((L->hookmask & (LUA_MASKLINE | LUA_MASKCOUNT)) && (--L->hookcount == 0 || L->hookmask & LUA_MASKLINE)) { traceexec(L, pc); if (L->status == LUA_YIELD) { /* did hook yield? */ L->savedpc = pc - 1; return; } base = L->base; } /* warning!! several calls may realloc the stack and invalidate `ra' */ ra = RA(i); lua_assert(base == L->base && L->base == L->ci->base); lua_assert(base <= L->top && L->top <= L->stack + L->stacksize); lua_assert(L->top == L->ci->top || luaG_checkopenop(i)); switch (GET_OPCODE(i)) { case OP_MOVE: { setobjs2s(L, ra, RB(i)); continue; } case OP_LOADK: { setobj2s(L, ra, KBx(i)); continue; } case OP_LOADBOOL: { setbvalue(ra, GETARG_B(i)); if (GETARG_C(i)) pc++; /* skip next instruction (if C) */ continue; } case OP_LOADNIL: { TValue *rb = RB(i); do { setnilvalue(rb--); } while (rb >= ra); continue; } case OP_GETUPVAL: { int b = GETARG_B(i); setobj2s(L, ra, cl->upvals[b]->v); continue; } case OP_GETGLOBAL: { TValue g; TValue *rb = KBx(i); sethvalue(L, &g, cl->env); lua_assert(ttisstring(rb)); Protect(luaV_gettable(L, &g, rb, ra)); continue; } case OP_GETTABLE: { Protect(luaV_gettable(L, RB(i), RKC(i), ra)); continue; } case OP_SETGLOBAL: { TValue g; sethvalue(L, &g, cl->env); lua_assert(ttisstring(KBx(i))); Protect(luaV_settable(L, &g, KBx(i), ra)); continue; } case OP_SETUPVAL: { UpVal *uv = cl->upvals[GETARG_B(i)]; setobj(L, uv->v, ra); luaC_barrier(L, uv, ra); continue; } case OP_SETTABLE: { Protect(luaV_settable(L, ra, RKB(i), RKC(i))); continue; } case OP_NEWTABLE: { int b = GETARG_B(i); int c = GETARG_C(i); sethvalue(L, ra, luaH_new(L, luaO_fb2int(b), luaO_fb2int(c))); Protect(luaC_checkGC(L)); continue; } case OP_SELF: { StkId rb = RB(i); setobjs2s(L, ra+1, rb); Protect(luaV_gettable(L, rb, RKC(i), ra)); continue; } case OP_ADD: { arith_op(luai_numadd, TM_ADD); continue; } case OP_SUB: { arith_op(luai_numsub, TM_SUB); continue; } case OP_MUL: { arith_op(luai_nummul, TM_MUL); continue; } case OP_DIV: { arith_op(luai_numdiv, TM_DIV); continue; } case OP_MOD: { arith_op(luai_nummod, TM_MOD); continue; } case OP_POW: { arith_op(luai_numpow, TM_POW); continue; } case OP_UNM: { TValue *rb = RB(i); if (ttisnumber(rb)) { lua_Number nb = nvalue(rb); setnvalue(ra, luai_numunm(nb)); } else { Protect(Arith(L, ra, rb, rb, TM_UNM)); } continue; } case OP_NOT: { int res = l_isfalse(RB(i)); /* next assignment may change this value */ setbvalue(ra, res); continue; } case OP_LEN: { const TValue *rb = RB(i); switch (ttype(rb)) { case LUA_TTABLE: { setnvalue(ra, cast_num(luaH_getn(hvalue(rb)))); break; } case LUA_TSTRING: { setnvalue(ra, cast_num(tsvalue(rb)->len)); break; } default: { /* try metamethod */ Protect( if (!call_binTM(L, rb, luaO_nilobject, ra, TM_LEN)) luaG_typeerror(L, rb, "get length of"); ) } } continue; } case OP_CONCAT: { int b = GETARG_B(i); int c = GETARG_C(i); Protect(luaV_concat(L, c-b+1, c); luaC_checkGC(L)); setobjs2s(L, RA(i), base+b); continue; } case OP_JMP: { dojump(L, pc, GETARG_sBx(i)); continue; } case OP_EQ: { TValue *rb = RKB(i); TValue *rc = RKC(i); Protect( if (equalobj(L, rb, rc) == GETARG_A(i)) dojump(L, pc, GETARG_sBx(*pc)); ) pc++; continue; } case OP_LT: { Protect( if (luaV_lessthan(L, RKB(i), RKC(i)) == GETARG_A(i)) dojump(L, pc, GETARG_sBx(*pc)); ) pc++; continue; } case OP_LE: { Protect( if (lessequal(L, RKB(i), RKC(i)) == GETARG_A(i)) dojump(L, pc, GETARG_sBx(*pc)); ) pc++; continue; } case OP_TEST: { if (l_isfalse(ra) != GETARG_C(i)) dojump(L, pc, GETARG_sBx(*pc)); pc++; continue; } case OP_TESTSET: { TValue *rb = RB(i); if (l_isfalse(rb) != GETARG_C(i)) { setobjs2s(L, ra, rb); dojump(L, pc, GETARG_sBx(*pc)); } pc++; continue; } case OP_CALL: { int b = GETARG_B(i); int nresults = GETARG_C(i) - 1; if (b != 0) L->top = ra+b; /* else previous instruction set top */ L->savedpc = pc; switch (luaD_precall(L, ra, nresults)) { case PCRLUA: { nexeccalls++; goto reentry; /* restart luaV_execute over new Lua function */ } case PCRC: { /* it was a C function (`precall' called it); adjust results */ if (nresults >= 0) L->top = L->ci->top; base = L->base; continue; } default: { return; /* yield */ } } } case OP_TAILCALL: { int b = GETARG_B(i); if (b != 0) L->top = ra+b; /* else previous instruction set top */ L->savedpc = pc; lua_assert(GETARG_C(i) - 1 == LUA_MULTRET); switch (luaD_precall(L, ra, LUA_MULTRET)) { case PCRLUA: { /* tail call: put new frame in place of previous one */ CallInfo *ci = L->ci - 1; /* previous frame */ int aux; StkId func = ci->func; StkId pfunc = (ci+1)->func; /* previous function index */ if (L->openupval) luaF_close(L, ci->base); L->base = ci->base = ci->func + ((ci+1)->base - pfunc); for (aux = 0; pfunc+aux < L->top; aux++) /* move frame down */ setobjs2s(L, func+aux, pfunc+aux); ci->top = L->top = func+aux; /* correct top */ lua_assert(L->top == L->base + clvalue(func)->l.p->maxstacksize); ci->savedpc = L->savedpc; ci->tailcalls++; /* one more call lost */ L->ci--; /* remove new frame */ goto reentry; } case PCRC: { /* it was a C function (`precall' called it) */ base = L->base; continue; } default: { return; /* yield */ } } } case OP_RETURN: { int b = GETARG_B(i); if (b != 0) L->top = ra+b-1; if (L->openupval) luaF_close(L, base); L->savedpc = pc; b = luaD_poscall(L, ra); if (--nexeccalls == 0) /* was previous function running `here'? */ return; /* no: return */ else { /* yes: continue its execution */ if (b) L->top = L->ci->top; lua_assert(isLua(L->ci)); lua_assert(GET_OPCODE(*((L->ci)->savedpc - 1)) == OP_CALL); goto reentry; } } case OP_FORLOOP: { lua_Number step = nvalue(ra+2); lua_Number idx = luai_numadd(nvalue(ra), step); /* increment index */ lua_Number limit = nvalue(ra+1); if (luai_numlt(0, step) ? luai_numle(idx, limit) : luai_numle(limit, idx)) { dojump(L, pc, GETARG_sBx(i)); /* jump back */ setnvalue(ra, idx); /* update internal index... */ setnvalue(ra+3, idx); /* ...and external index */ } continue; } case OP_FORPREP: { const TValue *init = ra; const TValue *plimit = ra+1; const TValue *pstep = ra+2; L->savedpc = pc; /* next steps may throw errors */ if (!tonumber(init, ra)) luaG_runerror(L, LUA_QL("for") " initial value must be a number"); else if (!tonumber(plimit, ra+1)) luaG_runerror(L, LUA_QL("for") " limit must be a number"); else if (!tonumber(pstep, ra+2)) luaG_runerror(L, LUA_QL("for") " step must be a number"); setnvalue(ra, luai_numsub(nvalue(ra), nvalue(pstep))); dojump(L, pc, GETARG_sBx(i)); continue; } case OP_TFORLOOP: { StkId cb = ra + 3; /* call base */ setobjs2s(L, cb+2, ra+2); setobjs2s(L, cb+1, ra+1); setobjs2s(L, cb, ra); L->top = cb+3; /* func. + 2 args (state and index) */ Protect(luaD_call(L, cb, GETARG_C(i))); L->top = L->ci->top; cb = RA(i) + 3; /* previous call may change the stack */ if (!ttisnil(cb)) { /* continue loop? */ setobjs2s(L, cb-1, cb); /* save control variable */ dojump(L, pc, GETARG_sBx(*pc)); /* jump back */ } pc++; continue; } case OP_SETLIST: { int n = GETARG_B(i); int c = GETARG_C(i); int last; Table *h; if (n == 0) { n = cast_int(L->top - ra) - 1; L->top = L->ci->top; } if (c == 0) c = cast_int(*pc++); runtime_check(L, ttistable(ra)); h = hvalue(ra); last = ((c-1)*LFIELDS_PER_FLUSH) + n; if (last > h->sizearray) /* needs more space? */ luaH_resizearray(L, h, last); /* pre-alloc it at once */ for (; n > 0; n--) { TValue *val = ra+n; setobj2t(L, luaH_setnum(L, h, last--), val); luaC_barriert(L, h, val); } continue; } case OP_CLOSE: { luaF_close(L, ra); continue; } case OP_CLOSURE: { Proto *p; Closure *ncl; int nup, j; p = cl->p->p[GETARG_Bx(i)]; nup = p->nups; ncl = luaF_newLclosure(L, nup, cl->env); ncl->l.p = p; for (j=0; jl.upvals[j] = cl->upvals[GETARG_B(*pc)]; else { lua_assert(GET_OPCODE(*pc) == OP_MOVE); ncl->l.upvals[j] = luaF_findupval(L, base + GETARG_B(*pc)); } } setclvalue(L, ra, ncl); Protect(luaC_checkGC(L)); continue; } case OP_VARARG: { int b = GETARG_B(i) - 1; int j; CallInfo *ci = L->ci; int n = cast_int(ci->base - ci->func) - cl->p->numparams - 1; if (b == LUA_MULTRET) { Protect(luaD_checkstack(L, n)); ra = RA(i); /* previous call may change the stack */ b = n; L->top = ra + n; } for (j = 0; j < b; j++) { if (j < n) { setobjs2s(L, ra + j, ci->base - n + j); } else { setnilvalue(ra + j); } } continue; } } } } /a> 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 
/*---------------------------------------------------------------------------*\
Original copyright
	FILE........: lsp.c
	AUTHOR......: David Rowe
	DATE CREATED: 24/2/93

Heavily modified by Jean-Marc Valin (c) 2002-2006 (fixed-point, 
                       optimizations, additional functions, ...)

   This file contains functions for converting Linear Prediction
   Coefficients (LPC) to Line Spectral Pair (LSP) and back. Note that the
   LSP coefficients are not in radians format but in the x domain of the
   unit circle.

   Speex License:

   Redistribution and use in source and binary forms, with or without
   modification, are permitted provided that the following conditions
   are met:
   
   - Redistributions of source code must retain the above copyright
   notice, this list of conditions and the following disclaimer.
   
   - Redistributions in binary form must reproduce the above copyright
   notice, this list of conditions and the following disclaimer in the
   documentation and/or other materials provided with the distribution.
   
   - Neither the name of the Xiph.org Foundation nor the names of its
   contributors may be used to endorse or promote products derived from
   this software without specific prior written permission.
   
   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
   ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
   A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
   EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
   PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
   LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
   NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
   SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

/*---------------------------------------------------------------------------*\

  Introduction to Line Spectrum Pairs (LSPs)
  ------------------------------------------

  LSPs are used to encode the LPC filter coefficients {ak} for
  transmission over the channel.  LSPs have several properties (like
  less sensitivity to quantisation noise) that make them superior to
  direct quantisation of {ak}.

  A(z) is a polynomial of order lpcrdr with {ak} as the coefficients.

  A(z) is transformed to P(z) and Q(z) (using a substitution and some
  algebra), to obtain something like:

    A(z) = 0.5[P(z)(z+z^-1) + Q(z)(z-z^-1)]  (1)

  As you can imagine A(z) has complex zeros all over the z-plane. P(z)
  and Q(z) have the very neat property of only having zeros _on_ the
  unit circle.  So to find them we take a test point z=exp(jw) and
  evaluate P (exp(jw)) and Q(exp(jw)) using a grid of points between 0
  and pi.

  The zeros (roots) of P(z) also happen to alternate, which is why we
  swap coefficients as we find roots.  So the process of finding the
  LSP frequencies is basically finding the roots of 5th order
  polynomials.

  The root so P(z) and Q(z) occur in symmetrical pairs at +/-w, hence
  the name Line Spectrum Pairs (LSPs).

  To convert back to ak we just evaluate (1), "clocking" an impulse
  thru it lpcrdr times gives us the impulse response of A(z) which is
  {ak}.

\*---------------------------------------------------------------------------*/

#ifdef HAVE_CONFIG_H
#include "config-speex.h"
#endif

#include <math.h>
#include "lsp.h"
#include "stack_alloc.h"
#include "math_approx.h"

#ifndef M_PI
#define M_PI           3.14159265358979323846  /* pi */
#endif

#ifndef NULL
#define NULL 0
#endif

#ifdef FIXED_POINT

#define FREQ_SCALE 16384

/*#define ANGLE2X(a) (32768*cos(((a)/8192.)))*/
#define ANGLE2X(a) (SHL16(spx_cos(a),2))

/*#define X2ANGLE(x) (acos(.00006103515625*(x))*LSP_SCALING)*/
#define X2ANGLE(x) (spx_acos(x))

#ifdef BFIN_ASM
#include "lsp_bfin.h"
#endif

#else

/*#define C1 0.99940307
#define C2 -0.49558072
#define C3 0.03679168*/

#define FREQ_SCALE 1.
#define ANGLE2X(a) (spx_cos(a))
#define X2ANGLE(x) (acos(x))

#endif


/*---------------------------------------------------------------------------*\

   FUNCTION....: cheb_poly_eva()

   AUTHOR......: David Rowe
   DATE CREATED: 24/2/93

   This function evaluates a series of Chebyshev polynomials

\*---------------------------------------------------------------------------*/

#ifdef FIXED_POINT

#ifndef OVERRIDE_CHEB_POLY_EVA
static inline spx_word32_t cheb_poly_eva(
  spx_word16_t *coef, /* P or Q coefs in Q13 format               */
  spx_word16_t     x, /* cos of freq (-1.0 to 1.0) in Q14 format  */
  int              m, /* LPC order/2                              */
  char         *stack
)
{
    int i;
    spx_word16_t b0, b1;
    spx_word32_t sum;

    /*Prevents overflows*/
    if (x>16383)
       x = 16383;
    if (x<-16383)
       x = -16383;

    /* Initialise values */
    b1=16384;
    b0=x;

    /* Evaluate Chebyshev series formulation usin g iterative approach  */
    sum = ADD32(EXTEND32(coef[m]), EXTEND32(MULT16_16_P14(coef[m-1],x)));
    for(i=2;i<=m;i++)
    {
       spx_word16_t tmp=b0;
       b0 = SUB16(MULT16_16_Q13(x,b0), b1);
       b1 = tmp;
       sum = ADD32(sum, EXTEND32(MULT16_16_P14(coef[m-i],b0)));
    }
    
    return sum;
}
#endif

#else

static float cheb_poly_eva(spx_word32_t *coef, spx_word16_t x, int m, char *stack)
{
   int k;
   float b0, b1, tmp;

   /* Initial conditions */
   b0=0; /* b_(m+1) */
   b1=0; /* b_(m+2) */

   x*=2;

   /* Calculate the b_(k) */
   for(k=m;k>0;k--)
   {
      tmp=b0;                           /* tmp holds the previous value of b0 */
      b0=x*b0-b1+coef[m-k];    /* b0 holds its new value based on b0 and b1 */
      b1=tmp;                           /* b1 holds the previous value of b0 */
   }

   return(-b1+.5*x*b0+coef[m]);
}
#endif

/*---------------------------------------------------------------------------*\

    FUNCTION....: lpc_to_lsp()

    AUTHOR......: David Rowe
    DATE CREATED: 24/2/93

    This function converts LPC coefficients to LSP
    coefficients.

\*---------------------------------------------------------------------------*/

#ifdef FIXED_POINT
#define SIGN_CHANGE(a,b) (((a)&0x70000000)^((b)&0x70000000)||(b==0))
#else
#define SIGN_CHANGE(a,b) (((a)*(b))<0.0)
#endif


int lpc_to_lsp (spx_coef_t *a,int lpcrdr,spx_lsp_t *freq,int nb,spx_word16_t delta, char *stack)
/*  float *a 		     	lpc coefficients			*/
/*  int lpcrdr			order of LPC coefficients (10) 		*/
/*  float *freq 	      	LSP frequencies in the x domain       	*/
/*  int nb			number of sub-intervals (4) 		*/
/*  float delta			grid spacing interval (0.02) 		*/


{
    spx_word16_t temp_xr,xl,xr,xm=0;
    spx_word32_t psuml,psumr,psumm,temp_psumr/*,temp_qsumr*/;
    int i,j,m,flag,k;
    VARDECL(spx_word32_t *Q);                 	/* ptrs for memory allocation 		*/
    VARDECL(spx_word32_t *P);
    VARDECL(spx_word16_t *Q16);         /* ptrs for memory allocation 		*/
    VARDECL(spx_word16_t *P16);
    spx_word32_t *px;                	/* ptrs of respective P'(z) & Q'(z)	*/
    spx_word32_t *qx;
    spx_word32_t *p;
    spx_word32_t *q;
    spx_word16_t *pt;                	/* ptr used for cheb_poly_eval()
				whether P' or Q' 			*/
    int roots=0;              	/* DR 8/2/94: number of roots found 	*/
    flag = 1;                	/*  program is searching for a root when,
				1 else has found one 			*/
    m = lpcrdr/2;            	/* order of P'(z) & Q'(z) polynomials 	*/

    /* Allocate memory space for polynomials */
    ALLOC(Q, (m+1), spx_word32_t);
    ALLOC(P, (m+1), spx_word32_t);

    /* determine P'(z)'s and Q'(z)'s coefficients where
      P'(z) = P(z)/(1 + z^(-1)) and Q'(z) = Q(z)/(1-z^(-1)) */

    px = P;                      /* initialise ptrs 			*/
    qx = Q;
    p = px;
    q = qx;

#ifdef FIXED_POINT
    *px++ = LPC_SCALING;
    *qx++ = LPC_SCALING;
    for(i=0;i<m;i++){
       *px++ = SUB32(ADD32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *p++);
       *qx++ = ADD32(SUB32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *q++);
    }
    px = P;
    qx = Q;
    for(i=0;i<m;i++)
    {
       /*if (fabs(*px)>=32768)
          speex_warning_int("px", *px);
       if (fabs(*qx)>=32768)
       speex_warning_int("qx", *qx);*/
       *px = PSHR32(*px,2);
       *qx = PSHR32(*qx,2);
       px++;
       qx++;
    }
    /* The reason for this lies in the way cheb_poly_eva() is implemented for fixed-point */
    P[m] = PSHR32(P[m],3);
    Q[m] = PSHR32(Q[m],3);
#else
    *px++ = LPC_SCALING;
    *qx++ = LPC_SCALING;
    for(i=0;i<m;i++){
       *px++ = (a[i]+a[lpcrdr-1-i]) - *p++;
       *qx++ = (a[i]-a[lpcrdr-1-i]) + *q++;
    }
    px = P;
    qx = Q;
    for(i=0;i<m;i++){
       *px = 2**px;
       *qx = 2**qx;
       px++;
       qx++;
    }
#endif

    px = P;             	/* re-initialise ptrs 			*/
    qx = Q;

    /* now that we have computed P and Q convert to 16 bits to
       speed up cheb_poly_eval */

    ALLOC(P16, m+1, spx_word16_t);
    ALLOC(Q16, m+1, spx_word16_t);

    for (i=0;i<m+1;i++)
    {
       P16[i] = P[i];
       Q16[i] = Q[i];
    }

    /* Search for a zero in P'(z) polynomial first and then alternate to Q'(z).
    Keep alternating between the two polynomials as each zero is found 	*/

    xr = 0;             	/* initialise xr to zero 		*/
    xl = FREQ_SCALE;               	/* start at point xl = 1 		*/

    for(j=0;j<lpcrdr;j++){
	if(j&1)            	/* determines whether P' or Q' is eval. */
	    pt = Q16;
	else
	    pt = P16;

	psuml = cheb_poly_eva(pt,xl,m,stack);	/* evals poly. at xl 	*/
	flag = 1;
	while(flag && (xr >= -FREQ_SCALE)){
           spx_word16_t dd;
           /* Modified by JMV to provide smaller steps around x=+-1 */
#ifdef FIXED_POINT
           dd = MULT16_16_Q15(delta,SUB16(FREQ_SCALE, MULT16_16_Q14(MULT16_16_Q14(xl,xl),14000)));
           if (psuml<512 && psuml>-512)
              dd = PSHR16(dd,1);
#else
           dd=delta*(1-.9*xl*xl);
           if (fabs(psuml)<.2)
              dd *= .5;
#endif
           xr = SUB16(xl, dd);                        	/* interval spacing 	*/
	    psumr = cheb_poly_eva(pt,xr,m,stack);/* poly(xl-delta_x) 	*/
	    temp_psumr = psumr;
	    temp_xr = xr;

    /* if no sign change increment xr and re-evaluate poly(xr). Repeat til
    sign change.
    if a sign change has occurred the interval is bisected and then
    checked again for a sign change which determines in which
    interval the zero lies in.
    If there is no sign change between poly(xm) and poly(xl) set interval
    between xm and xr else set interval between xl and xr and repeat till
    root is located within the specified limits 			*/

	    if(SIGN_CHANGE(psumr,psuml))
            {
		roots++;

		psumm=psuml;
		for(k=0;k<=nb;k++){
#ifdef FIXED_POINT
		    xm = ADD16(PSHR16(xl,1),PSHR16(xr,1));        	/* bisect the interval 	*/
#else
                    xm = .5*(xl+xr);        	/* bisect the interval 	*/
#endif
		    psumm=cheb_poly_eva(pt,xm,m,stack);
		    /*if(psumm*psuml>0.)*/
		    if(!SIGN_CHANGE(psumm,psuml))
                    {
			psuml=psumm;
			xl=xm;
		    } else {
			psumr=psumm;
			xr=xm;
		    }
		}

	       /* once zero is found, reset initial interval to xr 	*/
	       freq[j] = X2ANGLE(xm);
	       xl = xm;
	       flag = 0;       		/* reset flag for next search 	*/
	    }
	    else{
		psuml=temp_psumr;
		xl=temp_xr;
	    }
	}
    }
    return(roots);
}

/*---------------------------------------------------------------------------*\

	FUNCTION....: lsp_to_lpc()

	AUTHOR......: David Rowe
	DATE CREATED: 24/2/93

        Converts LSP coefficients to LPC coefficients.

\*---------------------------------------------------------------------------*/

#ifdef FIXED_POINT

void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
/*  float *freq 	array of LSP frequencies in the x domain	*/
/*  float *ak 		array of LPC coefficients 			*/
/*  int lpcrdr  	order of LPC coefficients 			*/
{
    int i,j;
    spx_word32_t xout1,xout2,xin;
    spx_word32_t mult, a;
    VARDECL(spx_word16_t *freqn);
    VARDECL(spx_word32_t **xp);
    VARDECL(spx_word32_t *xpmem);
    VARDECL(spx_word32_t **xq);
    VARDECL(spx_word32_t *xqmem);
    int m = lpcrdr>>1;

    /* 
    
       Reconstruct P(z) and Q(z) by cascading second order polynomials
       in form 1 - 2cos(w)z(-1) + z(-2), where w is the LSP frequency.
       In the time domain this is:

       y(n) = x(n) - 2cos(w)x(n-1) + x(n-2)
    
       This is what the ALLOCS below are trying to do:

         int xp[m+1][lpcrdr+1+2]; // P matrix in QIMP
         int xq[m+1][lpcrdr+1+2]; // Q matrix in QIMP

       These matrices store the output of each stage on each row.  The
       final (m-th) row has the output of the final (m-th) cascaded
       2nd order filter.  The first row is the impulse input to the
       system (not written as it is known).

       The version below takes advantage of the fact that a lot of the
       outputs are zero or known, for example if we put an inpulse
       into the first section the "clock" it 10 times only the first 3
       outputs samples are non-zero (it's an FIR filter).
    */

    ALLOC(xp, (m+1), spx_word32_t*);
    ALLOC(xpmem, (m+1)*(lpcrdr+1+2), spx_word32_t);

    ALLOC(xq, (m+1), spx_word32_t*);
    ALLOC(xqmem, (m+1)*(lpcrdr+1+2), spx_word32_t);
    
    for(i=0; i<=m; i++) {
      xp[i] = xpmem + i*(lpcrdr+1+2);
      xq[i] = xqmem + i*(lpcrdr+1+2);
    }

    /* work out 2cos terms in Q14 */

    ALLOC(freqn, lpcrdr, spx_word16_t);
    for (i=0;i<lpcrdr;i++) 
       freqn[i] = ANGLE2X(freq[i]);

    #define QIMP  21   /* scaling for impulse */

    xin = SHL32(EXTEND32(1), (QIMP-1)); /* 0.5 in QIMP format */
   
    /* first col and last non-zero values of each row are trivial */
    
    for(i=0;i<=m;i++) {
     xp[i][1] = 0;
     xp[i][2] = xin;
     xp[i][2+2*i] = xin;
     xq[i][1] = 0;
     xq[i][2] = xin;
     xq[i][2+2*i] = xin;
    }

    /* 2nd row (first output row) is trivial */

    xp[1][3] = -MULT16_32_Q14(freqn[0],xp[0][2]);
    xq[1][3] = -MULT16_32_Q14(freqn[1],xq[0][2]);

    xout1 = xout2 = 0;

    /* now generate remaining rows */

    for(i=1;i<m;i++) {

      for(j=1;j<2*(i+1)-1;j++) {
	mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]);
	xp[i+1][j+2] = ADD32(SUB32(xp[i][j+2], mult), xp[i][j]);
	mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]);
	xq[i+1][j+2] = ADD32(SUB32(xq[i][j+2], mult), xq[i][j]);
      }

      /* for last col xp[i][j+2] = xq[i][j+2] = 0 */

      mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]);
      xp[i+1][j+2] = SUB32(xp[i][j], mult);
      mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]);
      xq[i+1][j+2] = SUB32(xq[i][j], mult);
    }

    /* process last row to extra a{k} */

    for(j=1;j<=lpcrdr;j++) {
      int shift = QIMP-13;

      /* final filter sections */
      a = PSHR32(xp[m][j+2] + xout1 + xq[m][j+2] - xout2, shift); 
      xout1 = xp[m][j+2];
      xout2 = xq[m][j+2];
      
      /* hard limit ak's to +/- 32767 */

      if (a < -32767) a = -32767;
      if (a > 32767) a = 32767;
      ak[j-1] = (short)a;
     
    }

}

#else

void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
/*  float *freq 	array of LSP frequencies in the x domain	*/
/*  float *ak 		array of LPC coefficients 			*/
/*  int lpcrdr  	order of LPC coefficients 			*/


{
    int i,j;
    float xout1,xout2,xin1,xin2;
    VARDECL(float *Wp);
    float *pw,*n1,*n2,*n3,*n4=NULL;
    VARDECL(float *x_freq);
    int m = lpcrdr>>1;

    ALLOC(Wp, 4*m+2, float);
    pw = Wp;

    /* initialise contents of array */

    for(i=0;i<=4*m+1;i++){       	/* set contents of buffer to 0 */
	*pw++ = 0.0;
    }

    /* Set pointers up */

    pw = Wp;
    xin1 = 1.0;
    xin2 = 1.0;

    ALLOC(x_freq, lpcrdr, float);
    for (i=0;i<lpcrdr;i++)
       x_freq[i] = ANGLE2X(freq[i]);

    /* reconstruct P(z) and Q(z) by  cascading second order
      polynomials in form 1 - 2xz(-1) +z(-2), where x is the
      LSP coefficient */

    for(j=0;j<=lpcrdr;j++){
       int i2=0;
	for(i=0;i<m;i++,i2+=2){
	    n1 = pw+(i*4);
	    n2 = n1 + 1;
	    n3 = n2 + 1;
	    n4 = n3 + 1;
	    xout1 = xin1 - 2.f*x_freq[i2] * *n1 + *n2;
	    xout2 = xin2 - 2.f*x_freq[i2+1] * *n3 + *n4;
	    *n2 = *n1;
	    *n4 = *n3;
	    *n1 = xin1;
	    *n3 = xin2;
	    xin1 = xout1;
	    xin2 = xout2;
	}
	xout1 = xin1 + *(n4+1);
	xout2 = xin2 - *(n4+2);
	if (j>0)
	   ak[j-1] = (xout1 + xout2)*0.5f;
	*(n4+1) = xin1;
	*(n4+2) = xin2;

	xin1 = 0.0;
	xin2 = 0.0;
    }

}
#endif


#ifdef FIXED_POINT

/*Makes sure the LSPs are stable*/
void lsp_enforce_margin(spx_lsp_t *lsp, int len, spx_word16_t margin)
{
   int i;
   spx_word16_t m = margin;
   spx_word16_t m2 = 25736-margin;
  
   if (lsp[0]<m)
      lsp[0]=m;
   if (lsp[len-1]>m2)
      lsp[len-1]=m2;
   for (i=1;i<len-1;i++)
   {
      if (lsp[i]<lsp[i-1]+m)
         lsp[i]=lsp[i-1]+m;

      if (lsp[i]>lsp[i+1]-m)
         lsp[i]= SHR16(lsp[i],1) + SHR16(lsp[i+1]-m,1);
   }
}


void lsp_interpolate(spx_lsp_t *old_lsp, spx_lsp_t *new_lsp, spx_lsp_t *interp_lsp, int len, int subframe, int nb_subframes)
{
   int i;
   spx_word16_t tmp = DIV32_16(SHL32(EXTEND32(1 + subframe),14),nb_subframes);
   spx_word16_t tmp2 = 16384-tmp;
   for (i=0;i<len;i++)
   {
      interp_lsp[i] = MULT16_16_P14(tmp2,old_lsp[i]) + MULT16_16_P14(tmp,new_lsp[i]);
   }
}

#else

/*Makes sure the LSPs are stable*/
void lsp_enforce_margin(spx_lsp_t *lsp, int len, spx_word16_t margin)
{
   int i;
   if (lsp[0]<LSP_SCALING*margin)
      lsp[0]=LSP_SCALING*margin;
   if (lsp[len-1]>LSP_SCALING*(M_PI-margin))
      lsp[len-1]=LSP_SCALING*(M_PI-margin);
   for (i=1;i<len-1;i++)
   {
      if (lsp[i]<lsp[i-1]+LSP_SCALING*margin)
         lsp[i]=lsp[i-1]+LSP_SCALING*margin;

      if (lsp[i]>lsp[i+1]-LSP_SCALING*margin)
         lsp[i]= .5f* (lsp[i] + lsp[i+1]-LSP_SCALING*margin);
   }
}


void lsp_interpolate(spx_lsp_t *old_lsp, spx_lsp_t *new_lsp, spx_lsp_t *interp_lsp, int len, int subframe, int nb_subframes)
{
   int i;
   float tmp = (1.0f + subframe)/nb_subframes;
   for (i=0;i<len;i++)
   {
      interp_lsp[i] = (1-tmp)*old_lsp[i] + tmp*new_lsp[i];
   }
}

#endif