diff --git a/include/util.h b/include/util.h
new file mode 100644
index 0000000..ee6bbc4
--- /dev/null
+++ b/include/util.h
@@ -0,0 +1,146 @@
+/*
+ * util.h
+ *
+ * Byteswapping macros lend from EmuTOS sources
+ *
+ * This file is part of BaS_gcc.
+ *
+ * BaS_gcc 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 3 of the License, or
+ * (at your option) any later version.
+ *
+ * BaS_gcc 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 BaS_gcc.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Created on: 27.10.2013
+ *      Author: mfro
+ */
+
+#ifndef UTIL_H_
+#define UTIL_H_
+
+/*
+ * WORD swpw(WORD val);
+ *   swap endianess of val, 16 bits only.
+ */
+
+#define swpw(a)                           \
+  __extension__                           \
+  ({long _tmp;                            \
+    __asm__ __volatile__                  \
+    ("move.w  %0,%1\n\t"                  \
+     "lsl.l   #8,%0\n\t"                  \
+     "lsr.l   #8,%1\n\t"                  \
+     "move.b  %1,%0"                      \
+    : "=d"(a), "=d"(_tmp) /* outputs */   \
+    : "0"(a)     /* inputs  */            \
+    : "cc"       /* clobbered */          \
+    );                                    \
+  })
+
+
+/*
+ * WORD swpl(LONG val);
+ *   swap endianess of val, 32 bits only.
+ *   e.g. ABCD => DCBA
+ */
+
+#define swpl(a)                           \
+  __extension__                           \
+  ({long _tmp;                            \
+    __asm__ __volatile__                  \
+    ("move.b  (%1),%0\n\t"                \
+     "move.b  3(%1),(%1)\n\t"             \
+     "move.b  %0,3(%1)\n\t"               \
+     "move.b  1(%1),%0\n\t"               \
+     "move.b  2(%1),1(%1)\n\t"            \
+     "move.b  %0,2(%1)"                   \
+    : "=d"(_tmp)      /* outputs */       \
+    : "a"(&a)        /* inputs  */        \
+    : "cc", "memory" /* clobbered */      \
+    );                                    \
+  })
+
+
+/*
+ * WORD swpw2(ULONG val);
+ *   swap endianness of val, treated as two 16-bit words.
+ *   e.g. ABCD => BADC
+ */
+
+#define swpw2(a)                          \
+  __extension__                           \
+  ({unsigned long _tmp;                   \
+    __asm__ __volatile__                  \
+    ("move.b  (%1),%0\n\t"                \
+     "move.b  1(%1),(%1)\n\t"             \
+     "move.b  %0,1(%1)\n\t"               \
+     "move.b  2(%1),%0\n\t"               \
+     "move.b  3(%1),2(%1)\n\t"            \
+     "move.b  %0,3(%1)"                   \
+    : "=d"(_tmp)     /* outputs */        \
+    : "a"(&a)        /* inputs  */        \
+    : "cc", "memory" /* clobbered */      \
+    );                                    \
+  })
+
+/*
+ * WORD set_sr(WORD new);
+ *   sets sr to the new value, and return the old sr value
+ */
+
+#define set_sr(a)                         \
+__extension__                             \
+({short _r, _a = (a);                     \
+  __asm__ __volatile__                    \
+  ("move.w sr,%0\n\t"                     \
+   "move.w %1,sr"                         \
+  : "=&d"(_r)       /* outputs */        \
+  : "nd"(_a)        /* inputs  */        \
+  : "cc", "memory"   /* clobbered */      \
+  );                                      \
+  _r;                                     \
+})
+
+
+/*
+ * WORD get_sr(void);
+ *   returns the current value of sr.
+ */
+
+#define get_sr()                          \
+__extension__                             \
+({short _r;                               \
+  __asm__ volatile                        \
+  ("move.w sr,%0"                         \
+  : "=dm"(_r)        /* outputs */        \
+  :                  /* inputs  */        \
+  : "cc", "memory"   /* clobbered */      \
+  );                                      \
+  _r;                                     \
+})
+
+
+
+/*
+ * void regsafe_call(void *addr)
+ *   Saves all registers to the stack, calls the function
+ *   that addr points to, and restores the registers afterwards.
+ */
+#define regsafe_call(addr)                         \
+__extension__                                      \
+({__asm__ volatile ("lea     -60(sp),sp\n\t"       \
+                    "movem.l d0-d7/a0-a6,(sp)");   \
+  ((void (*)(void))addr)();                        \
+  __asm__ volatile ("movem.l (sp),d0-d7/a0-a6\n\t" \
+                    "lea     60(sp),sp");          \
+})
+
+
+#endif /* UTIL_H_ */
diff --git a/sources/pci.c b/sources/pci.c
index c8ae95f..94f95d7 100644
--- a/sources/pci.c
+++ b/sources/pci.c
@@ -28,6 +28,7 @@
 #include "pci.h"
 #include "stdint.h"
 #include "bas_printf.h"
+#include "util.h"
 
 uint32_t pci_read_config_longword(uint16_t slot, uint16_t function, uint16_t offset)
 {
@@ -80,11 +81,11 @@ void init_pci(void)
        + MCF_PCIARB_PACR_INTMINTEN
        + MCF_PCIARB_PACR_EXTMINTEN(0x1F);
 
-   // Setup burst parameters
+   /* Setup burst parameters */
    MCF_PCI_PCICR1 = MCF_PCI_PCICR1_CACHELINESIZE(4) + MCF_PCI_PCICR1_LATTIMER(32);
    MCF_PCI_PCICR2 = MCF_PCI_PCICR2_MINGNT(16) + MCF_PCI_PCICR2_MAXLAT(16);
 
-   // Turn on error signaling
+   /* Turn on error signaling */
    MCF_PCI_PCIICR = MCF_PCI_PCIICR_TAE + MCF_PCI_PCIICR_TAE + MCF_PCI_PCIICR_REE + 32;
    MCF_PCI_PCIGSCR |= MCF_PCI_PCIGSCR_SEE;