summaryrefslogtreecommitdiff
path: root/n/asserv/src/dsp.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/dsp.c')
-rw-r--r--n/asserv/src/dsp.c55
1 files changed, 55 insertions, 0 deletions
diff --git a/n/asserv/src/dsp.c b/n/asserv/src/dsp.c
new file mode 100644
index 0000000..71317a5
--- /dev/null
+++ b/n/asserv/src/dsp.c
@@ -0,0 +1,55 @@
+/* dsp.c */
+/* {{{
+ *
+ * Copyright (C) 2004 Nicolas Schodet
+ *
+ * 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.
+ *
+ * Contact :
+ * Web: http://perso.efrei.fr/~schodet/
+ * Email: <contact@ni.fr.eu.org>
+ * }}} */
+#include "dsp.h"
+
+/* +AutoDec */
+/* -AutoDec */
+
+/* Add two signed words and saturate. */
+inline int16_t
+dsp_i16i16_add_sat (int16_t a, int16_t b)
+{
+ asm ("add %A0, %A1\n\t"
+ "adc %B0, %B1\n\t"
+ /* Branch if not V. */
+ "brvc 1f\n\t"
+ /* Load Max. */
+ "ldi %A0, 0xff\n\t"
+ "ldi %B0, 0x7f\n\t"
+ /* Branch if not S. */
+ "brlt 1f\n\t"
+ /* Load Min. */
+ "ldi %A0, 0x00\n\t"
+ "ldi %B0, 0x80\n\t"
+ "1f" "\n\t"
+ : "=r" a : "0" a, "r" b);
+ return a;
+}
+
+/* Multiply and saturate. */
+inline int16_t
+dsp_i16u8_mul_sat (int16_t a, uint8_t b)
+{
+ asm ("mul
+}