Перевод Arduino в ChibiOS

Question

Я пытаюсь реализовать "один провод" для ChibiOS, работающий на atmega1280 (плата Arduino). Я в основном копирую / вставляю из реализации arduino (которая отлично работает):

uint8_t OneWire::reset(void)
{
    IO_REG_TYPE mask = bitmask;
    volatile IO_REG_TYPE *reg IO_REG_ASM = baseReg;
    uint8_t r;
    uint8_t retries = 125;

    noInterrupts();
    DIRECT_MODE_INPUT(reg, mask);
    interrupts();
    // wait until the wire is high... just in case
    do {
            if (--retries == 0) return 0;
            delayMicroseconds(2);
    } while ( !DIRECT_READ(reg, mask));

    noInterrupts();
    DIRECT_WRITE_LOW(reg, mask);
    DIRECT_MODE_OUTPUT(reg, mask);  // drive output low
    interrupts();
    delayMicroseconds(480);
    noInterrupts();
    DIRECT_MODE_INPUT(reg, mask);   // allow it to float
    delayMicroseconds(70);
    r = !DIRECT_READ(reg, mask);
    interrupts();
    delayMicroseconds(410);
    return r;
}

То, что я написал для реализации ChibiOS функции "reset", таково:

uint8_t OneWire::reset(void)
{
    palSetPadMode((ioportid_t)IOPORT2, 3, PAL_MODE_INPUT);

    // Wait until the wire is high.
    uint8_t retries = 125;
    do {
        if (--retries == 0) {
            return 0;
        }
        chThdSleepMicroseconds(2);
    } while (palReadPad(IOPORT2, 3) == PAL_LOW);

    palSetPadMode((ioportid_t)IOPORT2, 3, PAL_MODE_OUTPUT_PUSHPULL);
    palWritePad(IOPORT2, 3, PAL_LOW); // drive output low
    chThdSleepMicroseconds(480);

    palSetPadMode((ioportid_t)IOPORT2, 3, PAL_MODE_INPUT);
    chThdSleepMicroseconds(70);
    uint8_t result = (palReadPad(IOPORT2, 3) == PAL_LOW);
    chThdSleepMicroseconds(410);

    return result;
}

Что я делаю неправильно?