/*! \fn     usbProcessIncoming(uint8_t* incomingData)
*   \brief  Process the incoming USB packet
*   \param  incomingData    Pointer to the packet (can be overwritten!)
*/
void usbProcessIncoming(uint8_t* incomingData)
{
    // Temp plugin return value
    uint8_t plugin_return_value = PLUGIN_BYTE_ERROR;

    // Use message structure
    usbMsg_t* msg = (usbMsg_t*)incomingData;

    // Get data len
    uint8_t datalen = msg->len;

    // Get data cmd
    uint8_t datacmd = msg->cmd;

#ifdef USB_FEATURE_PLUGIN_COMMS
    // Temp ret_type
    RET_TYPE temp_rettype;
#endif
    
#ifdef DEV_PLUGIN_COMMS
    char stack_str[10];
#endif
    // Debug comms
    // USBDEBUGPRINTF_P(PSTR("usb: rx cmd 0x%02x len %u\n"), datacmd, datalen);

    switch(datacmd)
    {
        // ping command
        case CMD_PING :
        {
            usbSendMessage(0, 6, msg);
            return;
        }

        // version command
        case CMD_VERSION :
        {
            msg->len = 3; // len + cmd + FLASH_CHIP
            msg->cmd = CMD_VERSION;
            msg->body.data[0] = FLASH_CHIP;
            msg->len += getVersion((char*)&msg->body.data[1], sizeof(msg->body.data) - 1);
            usbSendMessage(0, msg->len, msg);
            return;
        }

#ifdef USB_FEATURE_PLUGIN_COMMS
        // context command
        case CMD_CONTEXT :
        {
            if (checkTextField(msg->body.data, datalen, NODE_PARENT_SIZE_OF_SERVICE) == RETURN_NOK)
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("setCtx: len %d too big\n"), datalen);
            }
            else if (getSmartCardInsertedUnlocked() != TRUE)
            {
                plugin_return_value = PLUGIN_BYTE_NOCARD;
                USBPARSERDEBUGPRINTF_P(PSTR("set context: no card\n"));                
            }
            else if (setCurrentContext(msg->body.data, datalen) == RETURN_OK)
            {
                plugin_return_value = PLUGIN_BYTE_OK;
                USBPARSERDEBUGPRINTF_P(PSTR("set context: \"%s\" ok\n"), msg->body.data);
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("set context: \"%s\" failed\n"), msg->body.data);
            }
            break;
        }

        // get login
        case CMD_GET_LOGIN :
        {
            if (getLoginForContext((char*)incomingData) == RETURN_OK)
            {
                // Use the buffer to store the login...
                usbSendMessage(CMD_GET_LOGIN, strlen((char*)incomingData)+1, incomingData);
                USBPARSERDEBUGPRINTF_P(PSTR("get login: \"%s\"\n"),(char *)incomingData);
                return;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("get login: failed\n"));
            }
            break;
        }

        // get password
        case CMD_GET_PASSWORD :
        {
            if (getPasswordForContext((char*)incomingData) == RETURN_OK)
            {
                usbSendMessage(CMD_GET_PASSWORD, strlen((char*)incomingData)+1, incomingData);
                USBPARSERDEBUGPRINTF_P(PSTR("get pass: \"%s\"\n"),(char *)incomingData);
                return;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("get pass: failed\n"));
            }
            break;
        }

        // set login
        case CMD_SET_LOGIN :
        {
            if (checkTextField(msg->body.data, datalen, NODE_CHILD_SIZE_OF_LOGIN) == RETURN_NOK)
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("set login: \"%s\" checkTextField failed\n"),msg->body.data);
            }
            else if (setLoginForContext(msg->body.data, datalen) == RETURN_OK)
            {
                plugin_return_value = PLUGIN_BYTE_OK;
                USBPARSERDEBUGPRINTF_P(PSTR("set login: \"%s\" ok\n"),msg->body.data);
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("set login: \"%s\" failed\n"),msg->body.data);
            }
            break;
        }

        // set password
        case CMD_SET_PASSWORD :
        {
            if (checkTextField(msg->body.data, datalen, NODE_CHILD_SIZE_OF_PASSWORD) == RETURN_NOK)
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("set pass: len %d invalid\n"), datalen);
            }
            else if (setPasswordForContext(msg->body.data, datalen) == RETURN_OK)
            {
                plugin_return_value = PLUGIN_BYTE_OK;
                USBPARSERDEBUGPRINTF_P(PSTR("set pass: \"%s\" ok\n"),msg->body.data);
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("set pass: failed\n"));
            }
            break;
        }

        // check password
        case CMD_CHECK_PASSWORD :
        {
            if (checkTextField(msg->body.data, datalen, NODE_CHILD_SIZE_OF_PASSWORD) == RETURN_NOK)
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                break;
            }
            temp_rettype = checkPasswordForContext(msg->body.data, datalen);
            if (temp_rettype == RETURN_PASS_CHECK_NOK)
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            else if(temp_rettype == RETURN_PASS_CHECK_OK)
            {
                plugin_return_value = PLUGIN_BYTE_OK;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_NA;
            }
            break;
        }

        // set password
        case CMD_ADD_CONTEXT :
        {
            if (checkTextField(msg->body.data, datalen, NODE_PARENT_SIZE_OF_SERVICE) == RETURN_NOK)
            {
                // Check field
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("set context: len %d invalid\n"), datalen);
            }
            else if (addNewContext(msg->body.data, datalen) == RETURN_OK)
            {
                // We managed to add a new context
                plugin_return_value = PLUGIN_BYTE_OK;
                USBPARSERDEBUGPRINTF_P(PSTR("add context: \"%s\" ok\n"),msg->body.data);
            }
            else
            {
                // Couldn't add a new context
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("add context: \"%s\" failed\n"),msg->body.data);
            }
            break;
        }
#endif

#ifdef FLASH_BLOCK_IMPORT_EXPORT
        // flash export start
        case CMD_EXPORT_FLASH_START :
        {
            approveImportExportMemoryOperation(CMD_EXPORT_FLASH_START, &plugin_return_value);
            guiGetBackToCurrentScreen();
            break;
        }

        // export flash contents
        case CMD_EXPORT_FLASH :
        {
            uint8_t size = PACKET_EXPORT_SIZE;
            
            // Check that the user approved
            if (currentFlashOpUid != CMD_EXPORT_FLASH_START)
            {
                return;
            }

            //flashOpCurAddr1 is the page
            //flashOpCurAddr2 is the offset
            // Check if the export address is correct
            if (flashOpCurAddr1 >= PAGE_COUNT)
            {
                usbSendMessage(CMD_EXPORT_FLASH_END, 0, NULL);
                USBPARSERDEBUGPRINTF_P(PSTR("export: end\n"));
                currentFlashOpUid = 0;
                return;
            }

            // Check how much data we need in case we're close to the page end
            if ((BYTES_PER_PAGE - flashOpCurAddr2) < PACKET_EXPORT_SIZE)
            {
                size = (uint8_t)(BYTES_PER_PAGE - flashOpCurAddr2);
            }

            // Get a block of data and send it, increment counter
            readDataFromFlash(flashOpCurAddr1, flashOpCurAddr2, size, (void*)incomingData);
            usbSendMessage(CMD_EXPORT_FLASH, size, incomingData);
            //usbSendMessageWithRetries(CMD_EXPORT_FLASH, size, (char*)incomingData, 255);
            flashOpCurAddr2 += size;
            
            if (flashOpCurAddr2 == BYTES_PER_PAGE)
            {
                flashOpCurAddr2 = 0;
                flashOpCurAddr1++;
            }

            // Skip over the graphics address if we're in that case
            if (flashOpCurAddr1 == GRAPHIC_ZONE_PAGE_START)
            {
                flashOpCurAddr1 = GRAPHIC_ZONE_PAGE_END;
            }
            return;
        }
        
        // flash export end
        case CMD_EXPORT_FLASH_END :
        {
            currentFlashOpUid = 0;
            return;
        }

        // flash export start
        case CMD_EXPORT_EEPROM_START :
        {
            approveImportExportMemoryOperation(CMD_EXPORT_EEPROM_START, &plugin_return_value);
            guiGetBackToCurrentScreen();
            break;
        }

        // export eeprom contents
        case CMD_EXPORT_EEPROM :
        {
            uint8_t size = PACKET_EXPORT_SIZE;

            // Check that the user approved
            if (currentFlashOpUid != CMD_EXPORT_EEPROM_START)
            {
                return;
            }

            //flashOpCurAddr1 is the current eeprom address
            // Check if the export address is correct
            if (flashOpCurAddr1 >= EEPROM_SIZE)
            {
                usbSendMessage(CMD_EXPORT_EEPROM_END, 0, NULL);
                USBPARSERDEBUGPRINTF_P(PSTR("export: end\n"));
                currentFlashOpUid = 0;
                return;
            }

            // Check how much data we need
            if ((EEPROM_SIZE - flashOpCurAddr1) < PACKET_EXPORT_SIZE)
            {
                size = (uint8_t)(EEPROM_SIZE - flashOpCurAddr1);
            }

            // Get a block of data and send it, increment counter
            eeprom_read_block(incomingData, (void*)flashOpCurAddr1, size);
            usbSendMessage(CMD_EXPORT_EEPROM, size, (char*)incomingData);
            //usbSendMessageWithRetries(CMD_EXPORT_EEPROM, size, (char*)incomingData, 255);
            flashOpCurAddr1 += size;
            return;
        }
        
        // end eeprom export
        case CMD_EXPORT_EEPROM_END :
        {
            currentFlashOpUid = 0;
            return;
        }

        // import flash contents
        case CMD_IMPORT_FLASH_BEGIN :
        {
            // Check datalen for arg
            if (datalen != 1)
            {
                USBPARSERDEBUGPRINTF_P(PSTR("import: no param\n"));
                return;
            }
            
            // Ask user approval            
            approveImportExportMemoryOperation(CMD_IMPORT_FLASH_BEGIN, &plugin_return_value);

            //flashOpCurAddr1 is the page
            //flashOpCurAddr2 is the offset
            // Check what we want to write
            if (msg->body.data[0] == 0x00)
            {
                flashOpCurAddr1 = 0x0000;
                flash_import_user_space = TRUE;
            }
            else
            {
                flash_import_user_space = FALSE;
                flashOpCurAddr1 = GRAPHIC_ZONE_PAGE_START;
            }
            
            // Get back to normal screen
            guiGetBackToCurrentScreen();
            break;
        }

        // import flash contents
        case CMD_IMPORT_FLASH :
        {
            // Check if we actually approved the import, haven't gone over the flash boundaries, if we're correctly aligned page size wise
            if ((currentFlashOpUid != CMD_IMPORT_FLASH_BEGIN) || (flashOpCurAddr1 >= PAGE_COUNT) || (flashOpCurAddr2 + datalen > BYTES_PER_PAGE) || ((flash_import_user_space == FALSE) && (flashOpCurAddr1 >= GRAPHIC_ZONE_PAGE_END)))
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                currentFlashOpUid = 0;
            }
            else
            {
                flashWriteBuffer(msg->body.data, flashOpCurAddr2, datalen);
                flashOpCurAddr2+= datalen;

                // If we just filled a page, flush it to the page
                if (flashOpCurAddr2 == BYTES_PER_PAGE)
                {
                    flashWriteBufferToPage(flashOpCurAddr1);
                    flashOpCurAddr2 = 0;
                    flashOpCurAddr1++;

                    // If we are importing user contents, skip the graphics zone
                    if ((flash_import_user_space == TRUE) && (flashOpCurAddr1 == GRAPHIC_ZONE_PAGE_START))
                    {
                        flashOpCurAddr1 = GRAPHIC_ZONE_PAGE_END;
                    }
                }
                plugin_return_value = PLUGIN_BYTE_OK;
            }
            break;
        }

        // end flash import
        case CMD_IMPORT_FLASH_END :
        {
            if ((currentFlashOpUid == CMD_IMPORT_FLASH_BEGIN) && (flashOpCurAddr2 != 0))
            {
                flashWriteBufferToPage(flashOpCurAddr1);
            }
            plugin_return_value = PLUGIN_BYTE_OK;
            currentFlashOpUid = 0;
            break;
        }

        // import flash contents
        case CMD_IMPORT_EEPROM_BEGIN :
        {
            // Ask for user confirmation
            approveImportExportMemoryOperation(CMD_IMPORT_EEPROM_BEGIN, &plugin_return_value);
            guiGetBackToCurrentScreen();
            break;
        }

        // import flash contents
        case CMD_IMPORT_EEPROM :
        {
            // flashOpCurAddr1 is the current eeprom address
            if ((currentFlashOpUid != CMD_IMPORT_EEPROM_BEGIN) || ((flashOpCurAddr1 + datalen) >= EEPROM_SIZE))
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                currentFlashOpUid = 0;
            }
            else
            {
                eeprom_write_block((void*)msg->body.data, (void*)flashOpCurAddr1, datalen);
                flashOpCurAddr1+= datalen;
                plugin_return_value = PLUGIN_BYTE_OK;
            }
            break;
        }

        // end eeprom import
        case CMD_IMPORT_EEPROM_END :
        {
            plugin_return_value = PLUGIN_BYTE_OK;
            currentFlashOpUid = 0;
            break;
        }
#endif
        
        // set password bootkey
        case CMD_SET_BOOTLOADER_PWD :
        {
            if ((eeprom_read_byte((uint8_t*)EEP_BOOT_PWD_SET) != BOOTLOADER_PWDOK_KEY) && (datalen == PACKET_EXPORT_SIZE))
            {
                eeprom_write_block((void*)msg->body.data, (void*)EEP_BOOT_PWD, PACKET_EXPORT_SIZE);
                eeprom_write_byte((uint8_t*)EEP_BOOT_PWD_SET, BOOTLOADER_PWDOK_KEY);
                plugin_return_value = PLUGIN_BYTE_OK;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;
        }
        
        // Jump to bootloader
        case CMD_JUMP_TO_BOOTLOADER :
        {
            #ifndef DEV_PLUGIN_COMMS
                uint8_t temp_buffer[PACKET_EXPORT_SIZE];
            #endif
            
            // Mandatory wait for bruteforce
            userViewDelay();
            #ifdef DEV_PLUGIN_COMMS
                // Write "jump to bootloader" key in eeprom
                eeprom_write_word((uint16_t*)EEP_BOOTKEY_ADDR, BOOTLOADER_BOOTKEY);
                // Use WDT to reset the device
                cli();
                wdt_reset();
                wdt_clear_flag();
                wdt_change_enable();
                wdt_enable_2s();
                sei();
                while(1);
            #else
                if ((eeprom_read_byte((uint8_t*)EEP_BOOT_PWD_SET) == BOOTLOADER_PWDOK_KEY) && (datalen == PACKET_EXPORT_SIZE))
                {
                    eeprom_read_block((void*)temp_buffer, (void*)EEP_BOOT_PWD, PACKET_EXPORT_SIZE);
                    if (memcmp((void*)temp_buffer, (void*)msg->body.data, PACKET_EXPORT_SIZE) == 0)
                    {
                        // Write "jump to bootloader" key in eeprom
                        eeprom_write_word((uint16_t*)EEP_BOOTKEY_ADDR, BOOTLOADER_BOOTKEY);
                        // Use WDT to reset the device
                        cli();
                        wdt_reset();
                        wdt_clear_flag();
                        wdt_change_enable();
                        wdt_enable_2s();
                        sei();
                        while(1);
                    }
                }
            #endif
        }

        // Development commands
#ifdef  DEV_PLUGIN_COMMS
        // erase eeprom
        case CMD_ERASE_EEPROM :
        {
            eraseFlashUsersContents();
            firstTimeUserHandlingInit();
            plugin_return_value = PLUGIN_BYTE_OK;
            break;
        }

        // erase flash
        case CMD_ERASE_FLASH :
        {
            eraseFlashUsersContents();
            plugin_return_value = PLUGIN_BYTE_OK;
            break;
        }

        // erase eeprom
        case CMD_ERASE_SMC :
        {
            if (getSmartCardInsertedUnlocked() == TRUE)
            {
                eraseSmartCard();
                plugin_return_value = PLUGIN_BYTE_OK;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;
        }

        case CMD_DRAW_BITMAP :
        {
            usbPrintf_P(PSTR("draw bitmap file %d\n"), msg->body.data[0]);
            if (msg->body.data[3] != 0)     // clear
            {
                oledWriteActiveBuffer();
                oledClear();
                oledBitmapDrawFlash(msg->body.data[1], msg->body.data[2], msg->body.data[0], 0);
            }
            else
            {
                // don't clear, overlay active screen
                oledWriteActiveBuffer();
                oledBitmapDrawFlash(msg->body.data[1], msg->body.data[2], msg->body.data[0], 0);
            }
            return;
        }
        
        case CMD_CLONE_SMARTCARD :
        {
            if (cloneSmartCardProcess(SMARTCARD_DEFAULT_PIN) == RETURN_OK)
            {
                plugin_return_value = PLUGIN_BYTE_OK;
            } 
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;
        }

        case CMD_SET_FONT :
        {
            usbPrintf_P(PSTR("set font file %d\n"), msg->body.data[0]);
            oledSetFont(msg->body.data[0]);

            if (datalen > 1) {
                usbPrintf_P(PSTR("testing string \"%s\"\n"), (char *)&msg->body.data[1]);
                oledFlipBuffers(0,0);
                oledWriteActiveBuffer();
                oledClear();
                oledPutstr((char *)&msg->body.data[1]);
            }

            return;
        }
        case CMD_STACK_FREE:
            
            usbPutstr("Stack Free ");
            int_to_string(stackFree(),stack_str);
            usbPutstr(stack_str);
            usbPutstr(" bytes\n");
        return;
#endif

        default :   return;
    }
    usbSendMessage(datacmd, 1, &plugin_return_value);
}
Beispiel #2
0
/*! \fn     main(void)
*   \brief  Main function
*/
int main(void)
{
    uint16_t current_bootkey_val = eeprom_read_word((uint16_t*)EEP_BOOTKEY_ADDR);
    RET_TYPE flash_init_result;
    RET_TYPE touch_init_result;
    RET_TYPE card_detect_ret;
    uint8_t fuse_ok = TRUE;
    
    // Disable JTAG to gain access to pins, set prescaler to 1 (fuses not set)
    #ifndef PRODUCTION_KICKSTARTER_SETUP
        disableJTAG();
        CPU_PRESCALE(0);
    #endif
        
    // Check fuse settings: boot reset vector, 2k words, SPIEN, BOD 2.6V, programming & ver disabled
    if ((boot_lock_fuse_bits_get(GET_LOW_FUSE_BITS) != 0xFF) || (boot_lock_fuse_bits_get(GET_HIGH_FUSE_BITS) != 0xD8) || (boot_lock_fuse_bits_get(GET_EXTENDED_FUSE_BITS) != 0xFB) || (boot_lock_fuse_bits_get(GET_LOCK_BITS) != 0xFC))
    {
        fuse_ok = FALSE;
    }
    
    // Check if PB5 is low to start electrical test
    DDRB &= ~(1 << 5); PORTB |= (1 << 5);
    smallForLoopBasedDelay();
    if (!(PINB & (1 << 5)))
    {
        // Test result, true by default
        uint8_t test_result = TRUE;
        // Leave flash nS off
        DDR_FLASH_nS |= (1 << PORTID_FLASH_nS);
        PORT_FLASH_nS |= (1 << PORTID_FLASH_nS);
        // Set PORTD as output, leave PORTID_OLED_SS high
        DDRD |= 0xFF; PORTD |= 0xFF;
        // All other pins are input by default, run our test
        for (uint8_t i = 0; i < 4; i++)
        {
            PORTD |= 0xFF;
            smallForLoopBasedDelay();
            if (!(PINF & (0xC3)) || !(PINC & (1 << 6)) || !(PINE & (1 << 6)) || !(PINB & (1 << 4)))
            {
                test_result = FALSE;
            }
            PORTD &= (1 << PORTID_OLED_SS);
            smallForLoopBasedDelay();
            if ((PINF & (0xC3)) || (PINC & (1 << 6)) || (PINE & (1 << 6)) || (PINB & (1 << 4)))
            {
                test_result = FALSE;
            }
        }               
        // PB6 as test result output
        DDRB |= (1 << 6);
        // If test successful, light green LED
        if ((test_result == TRUE) && (fuse_ok == TRUE))
        {
            PORTB |= (1 << 6);
        } 
        else
        {
            PORTB &= ~(1 << 6);
        }
        while(1);
    }
    
    // This code will only be used for developers and beta testers
    #if !defined(PRODUCTION_SETUP) && !defined(PRODUCTION_KICKSTARTER_SETUP)
        // Check if we were reset and want to go to the bootloader
        if (current_bootkey_val == BOOTLOADER_BOOTKEY)
        {
            // Disable WDT
            wdt_reset();
            wdt_clear_flag();
            wdt_change_enable();
            wdt_stop();
            // Store correct bootkey
            eeprom_write_word((uint16_t*)EEP_BOOTKEY_ADDR, CORRECT_BOOTKEY);
            // Jump to bootloader
            start_bootloader();
        }
        // Check if there was a change in the mooltipass setting storage to reset the parameters to their correct values
        if (getMooltipassParameterInEeprom(USER_PARAM_INIT_KEY_PARAM) != USER_PARAM_CORRECT_INIT_KEY)
        {
            mooltipassParametersInit();
            setMooltipassParameterInEeprom(USER_PARAM_INIT_KEY_PARAM, USER_PARAM_CORRECT_INIT_KEY);
        }
    #endif

    // First time initializations for Eeprom (first boot at production or flash layout changes for beta testers)
    if (current_bootkey_val != CORRECT_BOOTKEY)
    {
        // Erase Mooltipass parameters
        mooltipassParametersInit();
        // Set bootloader password bool to FALSE
        eeprom_write_byte((uint8_t*)EEP_BOOT_PWD_SET, FALSE);
    }

    /* Check if a card is inserted in the Mooltipass to go to the bootloader */
    #ifdef AVR_BOOTLOADER_PROGRAMMING
        /* Disable JTAG to get access to the pins */
        disableJTAG();
        /* Init SMC port */
        initPortSMC();
        /* Delay for detection */
        smallForLoopBasedDelay();
        #if defined(HARDWARE_V1)
        if (PIN_SC_DET & (1 << PORTID_SC_DET))
        #elif defined(HARDWARE_OLIVIER_V1)
        if (!(PIN_SC_DET & (1 << PORTID_SC_DET)))
        #endif
        {
            uint16_t tempuint16;
            /* What follows is a copy from firstDetectFunctionSMC() */
            /* Enable power to the card */
            PORT_SC_POW &= ~(1 << PORTID_SC_POW);
            /* Default state: PGM to 0 and RST to 1 */
            PORT_SC_PGM &= ~(1 << PORTID_SC_PGM);
            DDR_SC_PGM |= (1 << PORTID_SC_PGM);
            PORT_SC_RST |= (1 << PORTID_SC_RST);
            DDR_SC_RST |= (1 << PORTID_SC_RST);
            /* Activate SPI port */
            PORT_SPI_NATIVE &= ~((1 << SCK_SPI_NATIVE) | (1 << MOSI_SPI_NATIVE));
            DDRB |= (1 << SCK_SPI_NATIVE) | (1 << MOSI_SPI_NATIVE);
            setSPIModeSMC();
            /* Let the card come online */
            smallForLoopBasedDelay();
            /* Check smart card FZ */
            readFabricationZone((uint8_t*)&tempuint16);
            if ((swap16(tempuint16)) != SMARTCARD_FABRICATION_ZONE)
            {
                removeFunctionSMC();
                start_bootloader();
            }
            else
            {
                removeFunctionSMC();
            }
        }
    #endif

    initPortSMC();                      // Initialize smart card port
    initPwm();                          // Initialize PWM controller
    initIRQ();                          // Initialize interrupts
    powerSettlingDelay();               // Let the power settle   
    initUsb();                          // Initialize USB controller
    powerSettlingDelay();               // Let the USB 3.3V LDO rise
    initI2cPort();                      // Initialize I2C interface
    rngInit();                          // Initialize avrentropy library
    oledInitIOs();                      // Initialize OLED input/outputs
    spiUsartBegin(SPI_RATE_8_MHZ);      // Start USART SPI at 8MHz

    // If offline mode isn't enabled, wait for device to be enumerated
    if (getMooltipassParameterInEeprom(OFFLINE_MODE_PARAM) == FALSE)
    {
        while(!isUsbConfigured());      // Wait for host to set configuration
    }    
    
    // Set correct timeout_enabled val
    mp_timeout_enabled = getMooltipassParameterInEeprom(LOCK_TIMEOUT_ENABLE_PARAM);

    // Launch the before flash initialization tests
    #ifdef TESTS_ENABLED
        beforeFlashInitTests();
    #endif
    
    // Check if we can initialize the Flash memory
    flash_init_result = initFlash();
    
    // Launch the after flash initialization tests
    #ifdef TESTS_ENABLED
        afterFlashInitTests();
    #endif
    
    // Set up OLED now that USB is receiving full 500mA.
    oledBegin(FONT_DEFAULT);
    
    // First time initializations for Flash (first time power up at production)
    if (current_bootkey_val != CORRECT_BOOTKEY)
    {
        // Erase everything non graphic in flash
        eraseFlashUsersContents();
        // Erase # of cards and # of users
        firstTimeUserHandlingInit();
    }
    
    // Check if we can initialize the touch sensing element
    touch_init_result = initTouchSensing();

    // Enable proximity detection
    #ifndef HARDWARE_V1
        activateProxDetection();
    #endif
    
    // Launch the after touch initialization tests
    #ifdef TESTS_ENABLED
        afterTouchInitTests();
    #endif
    
    // Test procedure to check that all HW is working
    #if defined(PRODUCTION_SETUP) || defined(PRODUCTION_KICKSTARTER_SETUP)
        if (current_bootkey_val != CORRECT_BOOTKEY)
        {
            RET_TYPE temp_rettype;        
            // Wait for USB host to upload bundle, which then sets USER_PARAM_INIT_KEY_PARAM
            //#ifdef PRODUCTION_KICKSTARTER_SETUP
            while(getMooltipassParameterInEeprom(USER_PARAM_INIT_KEY_PARAM) != 0xF1)
            {
                usbProcessIncoming(USB_CALLER_MAIN);
            }
            //#endif
            // Bundle uploaded, start the screen
            oledBegin(FONT_DEFAULT);
            oledWriteActiveBuffer();
            oledSetXY(0,0);
            // LEDs ON, to check
            setPwmDc(MAX_PWM_VAL);
            switchOnButtonWheelLeds();
            guiDisplayRawString(ID_STRING_TEST_LEDS_CH);
            // Check flash init
            if (flash_init_result != RETURN_OK)
            {
                 guiDisplayRawString(ID_STRING_TEST_FLASH_PB);
            }
            // Check touch init
            if (touch_init_result != RETURN_OK)
            {
                guiDisplayRawString(ID_STRING_TEST_TOUCH_PB);
            }
            // Touch instructions
            guiDisplayRawString(ID_STRING_TEST_INST_TCH);
            // Check prox
            while(!(touchDetectionRoutine(0) & RETURN_PROX_DETECTION));
            guiDisplayRawString(ID_STRING_TEST_DET);
            activateGuardKey();
            // Check left
            while(!(touchDetectionRoutine(0) & RETURN_LEFT_PRESSED));
            guiDisplayRawString(ID_STRING_TEST_LEFT);
            // Check wheel
            while(!(touchDetectionRoutine(0) & RETURN_WHEEL_PRESSED));
            guiDisplayRawString(ID_STRING_TEST_WHEEL);
            // Check right
            while(!(touchDetectionRoutine(0) & RETURN_RIGHT_PRESSED));
            guiDisplayRawString(ID_STRING_TEST_RIGHT);
            // Insert card
            guiDisplayRawString(ID_STRING_TEST_CARD_INS);
            while(isCardPlugged() != RETURN_JDETECT);
            temp_rettype = cardDetectedRoutine();
            // Check card
            if (!((temp_rettype == RETURN_MOOLTIPASS_BLANK) || (temp_rettype == RETURN_MOOLTIPASS_USER)))
            {
                guiDisplayRawString(ID_STRING_TEST_CARD_PB);
            }
            // Display result
            uint8_t script_return = RETURN_OK;
            if ((flash_init_result == RETURN_OK) && (touch_init_result == RETURN_OK) && ((temp_rettype == RETURN_MOOLTIPASS_BLANK) || (temp_rettype == RETURN_MOOLTIPASS_USER)))
            {
                // Inform script of success
                usbSendMessage(CMD_FUNCTIONAL_TEST_RES, 1, &script_return);
                // Wait for password to be set
                while(eeprom_read_byte((uint8_t*)EEP_BOOT_PWD_SET) != BOOTLOADER_PWDOK_KEY)
                {
                    usbProcessIncoming(USB_CALLER_MAIN);
                }
                // Display test result
                guiDisplayRawString(ID_STRING_TEST_OK);
                timerBasedDelayMs(3000);
            }
            else
            {
                // Set correct bool
                script_return = RETURN_NOK;
                // Display test result
                guiDisplayRawString(ID_STRING_TEST_NOK);
                // Inform script of failure
                usbSendMessage(CMD_FUNCTIONAL_TEST_RES, 1, &script_return);
                while(1)
                {
                    usbProcessIncoming(USB_CALLER_MAIN);
                }
            }
        }
    #endif
    
    // Stop the Mooltipass if we can't communicate with the flash or the touch interface
    #if defined(HARDWARE_OLIVIER_V1)
        #ifdef PRODUCTION_KICKSTARTER_SETUP
            while ((flash_init_result != RETURN_OK) || (touch_init_result != RETURN_OK) || (fuse_ok != TRUE));
        #else
            while ((flash_init_result != RETURN_OK) || (touch_init_result != RETURN_OK));
        #endif
    #endif
    
    // First time initializations done.... write correct value in eeprom
    if (current_bootkey_val != CORRECT_BOOTKEY)
    {
        // Store correct bootkey
        eeprom_write_word((uint16_t*)EEP_BOOTKEY_ADDR, CORRECT_BOOTKEY);
    }

    // Write inactive buffer & go to startup screen
    oledWriteInactiveBuffer();
    guiSetCurrentScreen(SCREEN_DEFAULT_NINSERTED);
    guiGetBackToCurrentScreen();
        
    // Launch the after HaD logo display tests
    #ifdef TESTS_ENABLED
        afterHadLogoDisplayTests();  
    #endif
    
    // Let's fade in the LEDs
    for (uint16_t i = 0; i < MAX_PWM_VAL; i++)
    {
        setPwmDc(i);
        timerBasedDelayMs(0);
    }
    activityDetectedRoutine();
    launchCalibrationCycle();
    touchClearCurrentDetections();
    
    // Inhibit touch inputs for the first 3 seconds
    activateTimer(TIMER_TOUCH_INHIBIT, 3000);
    while (1)
    {
        // Process possible incoming USB packets
        usbProcessIncoming(USB_CALLER_MAIN);
        
        // Call GUI routine once the touch input inhibit timer is finished
        if (hasTimerExpired(TIMER_TOUCH_INHIBIT, FALSE) == TIMER_EXPIRED)
        {
            guiMainLoop();
        }
        
        // Check if a card just got inserted / removed
        card_detect_ret = isCardPlugged();
        
        // Do appropriate actions on smartcard insertion / removal
        if (card_detect_ret == RETURN_JDETECT)
        {
            // Light up the Mooltipass and call the dedicated function
            activityDetectedRoutine();
            handleSmartcardInserted();
        }
        else if (card_detect_ret == RETURN_JRELEASED)
        {
            // Light up the Mooltipass and call the dedicated function
            activityDetectedRoutine();
            handleSmartcardRemoved();
            
            // Set correct screen
            guiDisplayInformationOnScreen(ID_STRING_CARD_REMOVED);
            guiSetCurrentScreen(SCREEN_DEFAULT_NINSERTED);
            userViewDelay();
            guiGetBackToCurrentScreen();
        }
        
        // Two quick caps lock presses wakes up the device        
        if ((hasTimerExpired(TIMER_CAPS, FALSE) == TIMER_EXPIRED) && (getKeyboardLeds() & HID_CAPS_MASK) && (wasCapsLockTimerArmed == FALSE))
        {
            wasCapsLockTimerArmed = TRUE;
            activateTimer(TIMER_CAPS, CAPS_LOCK_DEL);
        }
        else if ((hasTimerExpired(TIMER_CAPS, FALSE) == TIMER_RUNNING) && !(getKeyboardLeds() & HID_CAPS_MASK))
        {
            activityDetectedRoutine();
        }
        else if ((hasTimerExpired(TIMER_CAPS, FALSE) == TIMER_EXPIRED) && !(getKeyboardLeds() & HID_CAPS_MASK))
        {
            wasCapsLockTimerArmed = FALSE;            
        }
        
        // If we have a timeout lock
        if ((mp_timeout_enabled == TRUE) && (hasTimerExpired(SLOW_TIMER_LOCKOUT, TRUE) == TIMER_EXPIRED))
        {
            guiSetCurrentScreen(SCREEN_DEFAULT_INSERTED_LCK);
            guiGetBackToCurrentScreen();
            handleSmartcardRemoved();
        }
    }
}
Beispiel #3
0
/*! \fn     usbProcessIncoming(uint8_t* incomingData)
*   \brief  Process the incoming USB packet
*   \param  incomingData    Pointer to the packet (can be overwritten!)
*/
void usbProcessIncoming(uint8_t* incomingData)
{
    // Temp plugin return value, error by default
    uint8_t plugin_return_value = PLUGIN_BYTE_ERROR;

    // Use message structure
    usbMsg_t* msg = (usbMsg_t*)incomingData;

    // Get data len
    uint8_t datalen = msg->len;

    // Get data cmd
    uint8_t datacmd = msg->cmd;

#ifdef USB_FEATURE_PLUGIN_COMMS
    // Temp ret_type
    RET_TYPE temp_rettype;
#endif
    
#ifdef DEV_PLUGIN_COMMS
    char stack_str[10];
#endif
    // Debug comms
    // USBDEBUGPRINTF_P(PSTR("usb: rx cmd 0x%02x len %u\n"), datacmd, datalen);

    switch(datacmd)
    {
        // ping command
        case CMD_PING :
        {
            usbSendMessage(0, 6, msg);
            return;
        }

        // version command
        case CMD_VERSION :
        {
            msg->len = 3; // len + cmd + FLASH_CHIP
            msg->cmd = CMD_VERSION;
            msg->body.data[0] = FLASH_CHIP;
            msg->len += getVersion((char*)&msg->body.data[1], sizeof(msg->body.data) - 1);
            usbSendMessage(0, msg->len, msg);
            return;
        }

#ifdef USB_FEATURE_PLUGIN_COMMS
        // context command
        case CMD_CONTEXT :
        {
            if (checkTextField(msg->body.data, datalen, NODE_PARENT_SIZE_OF_SERVICE) == RETURN_NOK)
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("setCtx: len %d too big\n"), datalen);
            }
            else if (getSmartCardInsertedUnlocked() != TRUE)
            {
                plugin_return_value = PLUGIN_BYTE_NOCARD;
                USBPARSERDEBUGPRINTF_P(PSTR("set context: no card\n"));                
            }
            else if (setCurrentContext(msg->body.data, datalen) == RETURN_OK)
            {
                plugin_return_value = PLUGIN_BYTE_OK;
                USBPARSERDEBUGPRINTF_P(PSTR("set context: \"%s\" ok\n"), msg->body.data);
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("set context: \"%s\" failed\n"), msg->body.data);
            }
            break;
        }

        // get login
        case CMD_GET_LOGIN :
        {
            if (getLoginForContext((char*)incomingData) == RETURN_OK)
            {
                // Use the buffer to store the login...
                usbSendMessage(CMD_GET_LOGIN, strlen((char*)incomingData)+1, incomingData);
                USBPARSERDEBUGPRINTF_P(PSTR("get login: \"%s\"\n"),(char *)incomingData);
                return;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("get login: failed\n"));
            }
            break;
        }

        // get password
        case CMD_GET_PASSWORD :
        {
            if (getPasswordForContext((char*)incomingData) == RETURN_OK)
            {
                usbSendMessage(CMD_GET_PASSWORD, strlen((char*)incomingData)+1, incomingData);
                USBPARSERDEBUGPRINTF_P(PSTR("get pass: \"%s\"\n"),(char *)incomingData);
                return;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("get pass: failed\n"));
            }
            break;
        }

        // set login
        case CMD_SET_LOGIN :
        {
            if (checkTextField(msg->body.data, datalen, NODE_CHILD_SIZE_OF_LOGIN) == RETURN_NOK)
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("set login: \"%s\" checkTextField failed\n"),msg->body.data);
            }
            else if (setLoginForContext(msg->body.data, datalen) == RETURN_OK)
            {
                plugin_return_value = PLUGIN_BYTE_OK;
                USBPARSERDEBUGPRINTF_P(PSTR("set login: \"%s\" ok\n"),msg->body.data);
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("set login: \"%s\" failed\n"),msg->body.data);
            }
            break;
        }

        // set password
        case CMD_SET_PASSWORD :
        {
            if (checkTextField(msg->body.data, datalen, NODE_CHILD_SIZE_OF_PASSWORD) == RETURN_NOK)
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("set pass: len %d invalid\n"), datalen);
            }
            else if (setPasswordForContext(msg->body.data, datalen) == RETURN_OK)
            {
                plugin_return_value = PLUGIN_BYTE_OK;
                USBPARSERDEBUGPRINTF_P(PSTR("set pass: \"%s\" ok\n"),msg->body.data);
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("set pass: failed\n"));
            }
            break;
        }

        // check password
        case CMD_CHECK_PASSWORD :
        {
            if (checkTextField(msg->body.data, datalen, NODE_CHILD_SIZE_OF_PASSWORD) == RETURN_NOK)
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                break;
            }
            temp_rettype = checkPasswordForContext(msg->body.data, datalen);
            if (temp_rettype == RETURN_PASS_CHECK_NOK)
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            else if(temp_rettype == RETURN_PASS_CHECK_OK)
            {
                plugin_return_value = PLUGIN_BYTE_OK;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_NA;
            }
            break;
        }

        // set password
        case CMD_ADD_CONTEXT :
        {
            if (checkTextField(msg->body.data, datalen, NODE_PARENT_SIZE_OF_SERVICE) == RETURN_NOK)
            {
                // Check field
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("set context: len %d invalid\n"), datalen);
            }
            else if (addNewContext(msg->body.data, datalen) == RETURN_OK)
            {
                // We managed to add a new context
                plugin_return_value = PLUGIN_BYTE_OK;
                USBPARSERDEBUGPRINTF_P(PSTR("add context: \"%s\" ok\n"),msg->body.data);
            }
            else
            {
                // Couldn't add a new context
                plugin_return_value = PLUGIN_BYTE_ERROR;
                USBPARSERDEBUGPRINTF_P(PSTR("add context: \"%s\" failed\n"),msg->body.data);
            }
            break;
        }
#endif

#ifdef FLASH_BLOCK_IMPORT_EXPORT
        // flash export start
        case CMD_EXPORT_FLASH_START :
        {
            approveImportExportMemoryOperation(CMD_EXPORT_FLASH_START, &plugin_return_value);
            guiGetBackToCurrentScreen();
            break;
        }

        // export flash contents
        case CMD_EXPORT_FLASH :
        {
            uint8_t size = PACKET_EXPORT_SIZE;
            
            // Check that the user approved
            if (currentFlashOpUid != CMD_EXPORT_FLASH_START)
            {
                return;
            }

            //flashOpCurAddr1 is the page
            //flashOpCurAddr2 is the offset
            // Check if the export address is correct
            if (flashOpCurAddr1 >= PAGE_COUNT)
            {
                usbSendMessage(CMD_EXPORT_FLASH_END, 0, NULL);
                USBPARSERDEBUGPRINTF_P(PSTR("export: end\n"));
                currentFlashOpUid = 0;
                return;
            }

            // Check how much data we need in case we're close to the page end
            if ((BYTES_PER_PAGE - flashOpCurAddr2) < PACKET_EXPORT_SIZE)
            {
                size = (uint8_t)(BYTES_PER_PAGE - flashOpCurAddr2);
            }

            // Get a block of data and send it, increment counter
            readDataFromFlash(flashOpCurAddr1, flashOpCurAddr2, size, (void*)incomingData);
            usbSendMessage(CMD_EXPORT_FLASH, size, incomingData);
            //usbSendMessageWithRetries(CMD_EXPORT_FLASH, size, (char*)incomingData, 255);
            flashOpCurAddr2 += size;
            
            if (flashOpCurAddr2 == BYTES_PER_PAGE)
            {
                flashOpCurAddr2 = 0;
                flashOpCurAddr1++;
            }

            // Skip over the graphics address if we're in that case
            if (flashOpCurAddr1 == GRAPHIC_ZONE_PAGE_START)
            {
                flashOpCurAddr1 = GRAPHIC_ZONE_PAGE_END;
            }
            return;
        }
        
        // flash export end
        case CMD_EXPORT_FLASH_END :
        {
            currentFlashOpUid = 0;
            return;
        }

        // flash export start
        case CMD_EXPORT_EEPROM_START :
        {
            approveImportExportMemoryOperation(CMD_EXPORT_EEPROM_START, &plugin_return_value);
            guiGetBackToCurrentScreen();
            break;
        }

        // export eeprom contents
        case CMD_EXPORT_EEPROM :
        {
            uint8_t size = PACKET_EXPORT_SIZE;

            // Check that the user approved
            if (currentFlashOpUid != CMD_EXPORT_EEPROM_START)
            {
                return;
            }

            //flashOpCurAddr1 is the current eeprom address
            // Check if the export address is correct
            if (flashOpCurAddr1 >= EEPROM_SIZE)
            {
                usbSendMessage(CMD_EXPORT_EEPROM_END, 0, NULL);
                USBPARSERDEBUGPRINTF_P(PSTR("export: end\n"));
                currentFlashOpUid = 0;
                return;
            }

            // Check how much data we need
            if ((EEPROM_SIZE - flashOpCurAddr1) < PACKET_EXPORT_SIZE)
            {
                size = (uint8_t)(EEPROM_SIZE - flashOpCurAddr1);
            }

            // Get a block of data and send it, increment counter
            eeprom_read_block(incomingData, (void*)flashOpCurAddr1, size);
            usbSendMessage(CMD_EXPORT_EEPROM, size, (char*)incomingData);
            //usbSendMessageWithRetries(CMD_EXPORT_EEPROM, size, (char*)incomingData, 255);
            flashOpCurAddr1 += size;
            return;
        }
        
        // end eeprom export
        case CMD_EXPORT_EEPROM_END :
        {
            currentFlashOpUid = 0;
            return;
        }

        // import flash contents
        case CMD_IMPORT_FLASH_BEGIN :
        {
            // Check datalen for arg
            if (datalen != 1)
            {
                USBPARSERDEBUGPRINTF_P(PSTR("import: no param\n"));
                return;
            }
            
            // Ask user approval            
            approveImportExportMemoryOperation(CMD_IMPORT_FLASH_BEGIN, &plugin_return_value);

            //flashOpCurAddr1 is the page
            //flashOpCurAddr2 is the offset
            // Check what we want to write
            if (msg->body.data[0] == 0x00)
            {
                flashOpCurAddr1 = 0x0000;
                flash_import_user_space = TRUE;
            }
            else
            {
                flash_import_user_space = FALSE;
                flashOpCurAddr1 = GRAPHIC_ZONE_PAGE_START;
            }
            
            // Get back to normal screen
            guiGetBackToCurrentScreen();
            break;
        }

        // import flash contents
        case CMD_IMPORT_FLASH :
        {
            // Check if we actually approved the import, haven't gone over the flash boundaries, if we're correctly aligned page size wise
            if ((currentFlashOpUid != CMD_IMPORT_FLASH_BEGIN) || (flashOpCurAddr1 >= PAGE_COUNT) || (flashOpCurAddr2 + datalen > BYTES_PER_PAGE) || ((flash_import_user_space == FALSE) && (flashOpCurAddr1 >= GRAPHIC_ZONE_PAGE_END)))
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                currentFlashOpUid = 0;
            }
            else
            {
                flashWriteBuffer(msg->body.data, flashOpCurAddr2, datalen);
                flashOpCurAddr2+= datalen;

                // If we just filled a page, flush it to the page
                if (flashOpCurAddr2 == BYTES_PER_PAGE)
                {
                    flashWriteBufferToPage(flashOpCurAddr1);
                    flashOpCurAddr2 = 0;
                    flashOpCurAddr1++;

                    // If we are importing user contents, skip the graphics zone
                    if ((flash_import_user_space == TRUE) && (flashOpCurAddr1 == GRAPHIC_ZONE_PAGE_START))
                    {
                        flashOpCurAddr1 = GRAPHIC_ZONE_PAGE_END;
                    }
                }
                plugin_return_value = PLUGIN_BYTE_OK;
            }
            break;
        }

        // end flash import
        case CMD_IMPORT_FLASH_END :
        {
            if ((currentFlashOpUid == CMD_IMPORT_FLASH_BEGIN) && (flashOpCurAddr2 != 0))
            {
                flashWriteBufferToPage(flashOpCurAddr1);
            }
            plugin_return_value = PLUGIN_BYTE_OK;
            currentFlashOpUid = 0;
            break;
        }

        // import flash contents
        case CMD_IMPORT_EEPROM_BEGIN :
        {
            // Ask for user confirmation
            approveImportExportMemoryOperation(CMD_IMPORT_EEPROM_BEGIN, &plugin_return_value);
            guiGetBackToCurrentScreen();
            break;
        }

        // import flash contents
        case CMD_IMPORT_EEPROM :
        {
            // flashOpCurAddr1 is the current eeprom address
            if ((currentFlashOpUid != CMD_IMPORT_EEPROM_BEGIN) || ((flashOpCurAddr1 + datalen) >= EEPROM_SIZE))
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                currentFlashOpUid = 0;
            }
            else
            {
                eeprom_write_block((void*)msg->body.data, (void*)flashOpCurAddr1, datalen);
                flashOpCurAddr1+= datalen;
                plugin_return_value = PLUGIN_BYTE_OK;
            }
            break;
        }

        // end eeprom import
        case CMD_IMPORT_EEPROM_END :
        {
            plugin_return_value = PLUGIN_BYTE_OK;
            currentFlashOpUid = 0;
            break;
        }
#endif
#ifdef NODE_BLOCK_IMPORT_EXPORT
        // Read user profile in flash
        case CMD_START_MEMORYMGMT :
        {            
            // Check that the smartcard is unlocked
            if (getSmartCardInsertedUnlocked() == TRUE)
            {
                // If so, ask the user to approve memory management mode
                approveMemoryManagementMode(&plugin_return_value);
            }            
            break;
        }
        
        // Read starting parent
        case CMD_GET_STARTING_PARENT :
        {
            // Check that we're actually in memory management mode
            if (memoryManagementModeApproved == TRUE)
            {
                // Read starting parent
                uint16_t temp_address = getStartingParentAddress();
                
                // Send address
                usbSendMessage(CMD_GET_STARTING_PARENT, 2, (uint8_t*)&temp_address);
                
                // Return
                return;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;            
        }
        
        // Get a free node address
        case CMD_GET_FREE_SLOT_ADDR :
        {
            // Check that we're actually in memory management mode
            if (memoryManagementModeApproved == TRUE)
            {
                uint16_t temp_address;
                
                // Scan for next free node address
                scanNodeUsage();
                
                // Store next free node address
                temp_address = getFreeNodeAddress();
                
                // Send address
                usbSendMessage(CMD_GET_FREE_SLOT_ADDR, 2, (uint8_t*)&temp_address);
                return;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;
        }
        
        // End memory management mode
        case CMD_END_MEMORYMGMT :
        {
            // Check that we're actually in memory management mode
            if (memoryManagementModeApproved == TRUE)
            {
                // memoryManagementModeApproved is cleared when user removes his card
                guiSetCurrentScreen(SCREEN_DEFAULT_INSERTED_NLCK);
                plugin_return_value = PLUGIN_BYTE_OK;
                leaveMemoryManagementMode();
                guiGetBackToCurrentScreen();
                populateServicesLut();
                scanNodeUsage();
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;
        }
        
        // Read node from Flash
        case CMD_READ_FLASH_NODE :
        {
            // Check that the mode is approved & that args are supplied
            if ((memoryManagementModeApproved == TRUE) && (datalen == 2))
            {
                uint16_t* temp_uint_ptr = (uint16_t*)msg->body.data;
                uint8_t temp_buffer[NODE_SIZE];
                
                // Read node in flash & send it, ownership check is done in the function
                readNode((gNode*)temp_buffer, *temp_uint_ptr);
                usbSendMessage(CMD_READ_FLASH_NODE, NODE_SIZE, temp_buffer);
                return;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;
        }
        
        // Set favorite
        case CMD_SET_FAVORITE :
        {
            // Check that the mode is approved & that args are supplied
            if ((memoryManagementModeApproved == TRUE) && (datalen == 5))
            {
                uint16_t* temp_par_addr = (uint16_t*)&msg->body.data[1];
                uint16_t* temp_child_addr = (uint16_t*)&msg->body.data[3];
                
                setFav(msg->body.data[0], *temp_par_addr, *temp_child_addr);
                plugin_return_value = PLUGIN_BYTE_OK;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;            
        }
        
        // Get favorite
        case CMD_GET_FAVORITE :
        {
            // Check that the mode is approved & that args are supplied
            if ((memoryManagementModeApproved == TRUE) && (datalen == 1))
            {
                uint16_t data[2];
                readFav(msg->body.data[0], &data[0], &data[1]);
                usbSendMessage(CMD_GET_FAVORITE, 4, (void*)data);
                return;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;
        }
        
        // Set starting parent
        case CMD_SET_STARTINGPARENT :
        {
            // Check that the mode is approved & that args are supplied
            if ((memoryManagementModeApproved == TRUE) && (datalen == 2))
            {
                uint16_t* temp_par_addr = (uint16_t*)&msg->body.data[0];
                setStartingParent(*temp_par_addr);
                plugin_return_value = PLUGIN_BYTE_OK;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;            
        }
        
        // Set new CTR value
        case CMD_SET_CTRVALUE :
        {
            // Check that the mode is approved & that args are supplied
            if ((memoryManagementModeApproved == TRUE) && (datalen == USER_CTR_SIZE))
            {
                setProfileCtr(msg->body.data);
                plugin_return_value = PLUGIN_BYTE_OK;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;            
        }
        
        // Get CTR value
        case CMD_GET_CTRVALUE :
        {
            // Check that the mode is approved & that args are supplied
            if (memoryManagementModeApproved == TRUE)
            {
                // Temp buffer to store CTR
                uint8_t tempCtrVal[USER_CTR_SIZE];
                
                // Read CTR value
                readProfileCtr(tempCtrVal);
                
                // Send it
                usbSendMessage(CMD_GET_CTRVALUE, USER_CTR_SIZE, tempCtrVal);
                return;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;
        }
        
        // Add a known card to the MP, 8 first bytes is the CPZ, next 16 is the CTR nonce
        case CMD_ADD_CARD_CPZ_CTR :
        {
            // Check that the mode is approved & that args are supplied
            if ((memoryManagementModeApproved == TRUE) && (datalen == SMARTCARD_CPZ_LENGTH + AES256_CTR_LENGTH))
            {
                writeSmartCardCPZForUserId(msg->body.data, &msg->body.data[SMARTCARD_CPZ_LENGTH], getCurrentUserID());
                plugin_return_value = PLUGIN_BYTE_OK;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;
        }
        
        // Get all the cpz ctr values for current user
        case CMD_GET_CARD_CPZ_CTR :
        {
            // Check that the mode is approved
            if (memoryManagementModeApproved == TRUE)
            {
                outputLUTEntriesForGivenUser(getCurrentUserID());
                plugin_return_value = PLUGIN_BYTE_OK;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;            
        }
        
        // Write node in Flash
        case CMD_WRITE_FLASH_NODE : 
        {
            // First two bytes are the node address
            uint16_t* temp_node_addr_ptr = (uint16_t*)msg->body.data;
            uint16_t temp_flags;
            
            // Check that the plugin provided the address and packet #
            if ((memoryManagementModeApproved != TRUE) || (datalen != 3))
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            } 
            else
            {                
                // If it is the first packet, store the address and load the page in the internal buffer
                if (msg->body.data[2] == 0)
                {
                    // Read the flags and check we're not overwriting someone else's data
                    readDataFromFlash(pageNumberFromAddress(*temp_node_addr_ptr), NODE_SIZE * nodeNumberFromAddress(*temp_node_addr_ptr), 2, (void*)&temp_flags);
                    
                    // Either the node belongs to us or it is invalid
                    if((getCurrentUserID() == userIdFromFlags(temp_flags)) || (validBitFromFlags(temp_flags) == NODE_VBIT_INVALID))
                    {
                        currentNodeWritten = *temp_node_addr_ptr;
                        loadPageToInternalBuffer(pageNumberFromAddress(currentNodeWritten));                        
                    }
                }
                
                // Check that the address the plugin wants to write is the one stored and that we're not writing more than we're supposed to
                if ((currentNodeWritten == *temp_node_addr_ptr) && (currentNodeWritten != NODE_ADDR_NULL) && (msg->body.data[2] * (PACKET_EXPORT_SIZE-3) + datalen < NODE_SIZE))
                {
                    // If it's the first packet, set correct user ID
                    if (msg->body.data[2] == 0)
                    {
                        userIdToFlags((uint16_t*)&(msg->body.data[3]), getCurrentUserID());
                    }
                    
                    // Fill the data at the right place
                    flashWriteBuffer(msg->body.data + 3, (NODE_SIZE * nodeNumberFromAddress(currentNodeWritten)) + (msg->body.data[2] * (PACKET_EXPORT_SIZE-3)), datalen - 3);
                    
                    // If we finished writing, flush buffer
                    if (msg->body.data[2] == (NODE_SIZE/(PACKET_EXPORT_SIZE-3)))
                    {
                        flashWriteBufferToPage(pageNumberFromAddress(currentNodeWritten));
                    }
                    
                    plugin_return_value = PLUGIN_BYTE_OK;
                }
                else
                {
                    plugin_return_value = PLUGIN_BYTE_ERROR;
                }
            }
            break;
        }
#endif

        // import media flash contents
        case CMD_IMPORT_MEDIA_START :
        {
            #ifndef DEV_PLUGIN_COMMS
                uint8_t temp_buffer[PACKET_EXPORT_SIZE];
            #endif
            
            // Set default addresses
            mediaFlashImportPage = GRAPHIC_ZONE_PAGE_START;
            mediaFlashImportOffset = 0;
            
            // No check if dev comms
            #ifdef DEV_PLUGIN_COMMS
                plugin_return_value = PLUGIN_BYTE_OK;
                mediaFlashImportApproved = TRUE;
            #else            
                // Mandatory wait for bruteforce
                userViewDelay();
                
                // Compare with our password, can be 0xFF... if not initialized
                if (datalen == PACKET_EXPORT_SIZE)
                {
                    eeprom_read_block((void*)temp_buffer, (void*)EEP_BOOT_PWD, PACKET_EXPORT_SIZE);
                    if (memcmp((void*)temp_buffer, (void*)msg->body.data, PACKET_EXPORT_SIZE) == 0)
                    {
                        plugin_return_value = PLUGIN_BYTE_OK;
                        mediaFlashImportApproved = TRUE;
                    }
                }
            #endif
            
            break;
        }

        // import media flash contents
        case CMD_IMPORT_MEDIA :
        {
            // Check if we actually approved the import, haven't gone over the flash boundaries, if we're correctly aligned page size wise
            if ((mediaFlashImportApproved == FALSE) || (mediaFlashImportPage >= GRAPHIC_ZONE_PAGE_END) || (mediaFlashImportOffset + datalen > BYTES_PER_PAGE))
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
                mediaFlashImportApproved = FALSE;
            }
            else
            {
                flashWriteBuffer(msg->body.data, mediaFlashImportOffset, datalen);
                mediaFlashImportOffset+= datalen;

                // If we just filled a page, flush it to the page
                if (mediaFlashImportOffset == BYTES_PER_PAGE)
                {
                    flashWriteBufferToPage(mediaFlashImportPage);
                    mediaFlashImportOffset = 0;
                    mediaFlashImportPage++;
                }
                plugin_return_value = PLUGIN_BYTE_OK;
            }
            break;
        }

        // end media flash import
        case CMD_IMPORT_MEDIA_END :
        {
            if ((mediaFlashImportApproved == TRUE) && (mediaFlashImportOffset != 0))
            {
                flashWriteBufferToPage(mediaFlashImportPage);
            }
            plugin_return_value = PLUGIN_BYTE_OK;
            mediaFlashImportApproved = FALSE;
            break;
        }
        
        // Set Mooltipass param
        case CMD_SET_MOOLTIPASS_PARM :
        {
            // Check that args are supplied
            if (datalen == 2)
            {
                setMooltipassParameterInEeprom(msg->body.data[0], msg->body.data[1]);
                plugin_return_value = PLUGIN_BYTE_OK;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;
        }
        
        // Get Mooltipass param
        case CMD_GET_MOOLTIPASS_PARM :
        {
            plugin_return_value = getMooltipassParameterInEeprom(msg->body.data[0]);
            break;
        }
        
        // Reset smartcard
        case CMD_RESET_CARD :
        {
            uint16_t* temp_uint_pt = (uint16_t*)msg->body.data;
            // Check the args, check we're not authenticated, check that the card detection returns a user card, try unlocking the card with provided PIN            
            if ((datalen == 2) && (getCurrentScreen() == SCREEN_DEFAULT_INSERTED_UNKNOWN) && (mooltipassDetectedRoutine(swap16(*temp_uint_pt)) == RETURN_MOOLTIPASS_4_TRIES_LEFT))
            {
                eraseSmartCard();
                plugin_return_value = PLUGIN_BYTE_OK;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;                
            }
            break;
        }
        
        // Add current unknown smartcard
        case CMD_ADD_UNKNOWN_CARD :
        {
            uint16_t* temp_uint_pt = (uint16_t*)msg->body.data;
            
            // Check the args, check we're not authenticated, check that the card detection returns a user card, try unlocking the card with provided PIN
            if ((datalen == (2 + AES256_CTR_LENGTH)) && (getCurrentScreen() == SCREEN_DEFAULT_INSERTED_UNKNOWN) && (mooltipassDetectedRoutine(swap16(*temp_uint_pt)) == RETURN_MOOLTIPASS_4_TRIES_LEFT))
            {
                addNewUserForExistingCard(msg->body.data + 2);
                plugin_return_value = PLUGIN_BYTE_OK;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;
        }
        
        // Read card login
        case CMD_READ_CARD_LOGIN :
        {
            if (getSmartCardInsertedUnlocked() == TRUE)
            {
                uint8_t temp_data[SMARTCARD_MTP_LOGIN_LENGTH/8];
                readMooltipassWebsiteLogin(temp_data);
                usbSendMessage(CMD_READ_CARD_LOGIN, sizeof(temp_data), (void*)temp_data);
                return;
            } 
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;
        }
        
        // Read card stored password
        case CMD_READ_CARD_PASS :
        {
            if (getSmartCardInsertedUnlocked() == TRUE)
            {
                if (guiAskForConfirmation(1, (confirmationText_t*)readStoredStringToBuffer(ID_STRING_SEND_SMC_PASS)) == RETURN_OK)
                {
                    uint8_t temp_data[SMARTCARD_MTP_PASS_LENGTH/8];
                    readMooltipassWebsitePassword(temp_data);
                    usbSendMessage(CMD_READ_CARD_PASS, sizeof(temp_data), (void*)temp_data);
                    guiGetBackToCurrentScreen();
                    return;
                } 
                else
                {
                    guiGetBackToCurrentScreen();
                    plugin_return_value = PLUGIN_BYTE_ERROR;
                }
            } 
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;
        }
        
        // Set card login
        case CMD_SET_CARD_LOGIN :
        {
            if ((checkTextField(msg->body.data, datalen, SMARTCARD_MTP_LOGIN_LENGTH/8) == RETURN_OK) && (getSmartCardInsertedUnlocked() == TRUE))
            {
                if (guiAskForConfirmation(1, (confirmationText_t*)readStoredStringToBuffer(ID_STRING_SET_SMC_LOGIN)) == RETURN_OK)
                {
                    // Temp buffer for application zone 2
                    uint8_t temp_az2[SMARTCARD_AZ_BIT_LENGTH/8];
                    
                    // Read Application Zone 2
                    readApplicationZone2(temp_az2);
                    // Erase Application Zone 2
                    eraseApplicationZone1NZone2SMC(FALSE);
                    // Write our data in the buffer at the right spot
                    memcpy(temp_az2 + (SMARTCARD_MTP_LOGIN_OFFSET/8), msg->body.data, datalen);
                    // Write the new data in the card
                    writeApplicationZone2(temp_az2);
                    
                    // Return OK
                    plugin_return_value = PLUGIN_BYTE_OK;
                } 
                else
                {
                    plugin_return_value = PLUGIN_BYTE_ERROR;
                }
                guiGetBackToCurrentScreen();
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;
        }
        
        // Set card stored password
        case CMD_SET_CARD_PASS :
        {
            if ((checkTextField(msg->body.data, datalen, SMARTCARD_MTP_PASS_LENGTH/8) == RETURN_OK) && (getSmartCardInsertedUnlocked() == TRUE))
            {
                if (guiAskForConfirmation(1, (confirmationText_t*)readStoredStringToBuffer(ID_STRING_SET_SMC_PASS)) == RETURN_OK)
                {
                    // Temp buffer for application zone 1
                    uint8_t temp_az1[SMARTCARD_AZ_BIT_LENGTH/8];
                    
                    // Read Application Zone 1
                    readApplicationZone1(temp_az1);
                    // Erase Application Zone 1
                    eraseApplicationZone1NZone2SMC(TRUE);
                    // Write our data in buffer
                    memcpy(temp_az1 + (SMARTCARD_MTP_PASS_OFFSET/8), msg->body.data, datalen);
                    // Write the new data in the card
                    writeApplicationZone1(temp_az1);
                    
                    // Return OK
                    plugin_return_value = PLUGIN_BYTE_OK;
                } 
                else
                {
                    plugin_return_value = PLUGIN_BYTE_ERROR;
                }
                guiGetBackToCurrentScreen();
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;
        }
        
        // Get 32 random bytes
        case CMD_GET_RANDOM_NUMBER :
        {
            uint8_t randomBytes[32];
            fillArrayWithRandomBytes(randomBytes, 32);
            usbSendMessage(CMD_GET_RANDOM_NUMBER, 32, randomBytes);
            return;
        }            
        
        // set password bootkey
        case CMD_SET_BOOTLOADER_PWD :
        {
            if ((eeprom_read_byte((uint8_t*)EEP_BOOT_PWD_SET) != BOOTLOADER_PWDOK_KEY) && (datalen == PACKET_EXPORT_SIZE))
            {
                eeprom_write_block((void*)msg->body.data, (void*)EEP_BOOT_PWD, PACKET_EXPORT_SIZE);
                eeprom_write_byte((uint8_t*)EEP_BOOT_PWD_SET, BOOTLOADER_PWDOK_KEY);
                plugin_return_value = PLUGIN_BYTE_OK;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;
        }
        
        // Jump to bootloader
        case CMD_JUMP_TO_BOOTLOADER :
        {
            #ifndef DEV_PLUGIN_COMMS
                uint8_t temp_buffer[PACKET_EXPORT_SIZE];
            #endif
            
            // Mandatory wait for bruteforce
            userViewDelay();
            #ifdef DEV_PLUGIN_COMMS
                // Write "jump to bootloader" key in eeprom
                eeprom_write_word((uint16_t*)EEP_BOOTKEY_ADDR, BOOTLOADER_BOOTKEY);
                // Use WDT to reset the device
                cli();
                wdt_reset();
                wdt_clear_flag();
                wdt_change_enable();
                wdt_enable_2s();
                sei();
                while(1);
            #else
                if ((eeprom_read_byte((uint8_t*)EEP_BOOT_PWD_SET) == BOOTLOADER_PWDOK_KEY) && (datalen == PACKET_EXPORT_SIZE))
                {
                    eeprom_read_block((void*)temp_buffer, (void*)EEP_BOOT_PWD, PACKET_EXPORT_SIZE);
                    if (memcmp((void*)temp_buffer, (void*)msg->body.data, PACKET_EXPORT_SIZE) == 0)
                    {
                        // Write "jump to bootloader" key in eeprom
                        eeprom_write_word((uint16_t*)EEP_BOOTKEY_ADDR, BOOTLOADER_BOOTKEY);
                        // Set bootloader password bool to FALSE
                        eeprom_write_byte((uint8_t*)EEP_BOOT_PWD_SET, FALSE);
                        // Use WDT to reset the device
                        cli();
                        wdt_reset();
                        wdt_clear_flag();
                        wdt_change_enable();
                        wdt_enable_2s();
                        sei();
                        while(1);
                    }
                }
            #endif
        }

        // Development commands
#ifdef  DEV_PLUGIN_COMMS
        // erase eeprom
        case CMD_ERASE_EEPROM :
        {
            eraseFlashUsersContents();
            firstTimeUserHandlingInit();
            plugin_return_value = PLUGIN_BYTE_OK;
            break;
        }

        // erase flash
        case CMD_ERASE_FLASH :
        {
            eraseFlashUsersContents();
            plugin_return_value = PLUGIN_BYTE_OK;
            break;
        }

        // erase eeprom
        case CMD_ERASE_SMC :
        {
            if (getSmartCardInsertedUnlocked() == TRUE)
            {
                eraseSmartCard();
                plugin_return_value = PLUGIN_BYTE_OK;
            }
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;
        }

        case CMD_DRAW_BITMAP :
        {
            usbPrintf_P(PSTR("draw bitmap file %d\n"), msg->body.data[0]);
            if (msg->body.data[3] != 0)     // clear
            {
                oledWriteActiveBuffer();
                oledClear();
                oledBitmapDrawFlash(msg->body.data[1], msg->body.data[2], msg->body.data[0], 0);
            }
            else
            {
                // don't clear, overlay active screen
                oledWriteActiveBuffer();
                oledBitmapDrawFlash(msg->body.data[1], msg->body.data[2], msg->body.data[0], 0);
            }
            return;
        }
        
        case CMD_CLONE_SMARTCARD :
        {
            if (cloneSmartCardProcess(SMARTCARD_DEFAULT_PIN) == RETURN_OK)
            {
                plugin_return_value = PLUGIN_BYTE_OK;
            } 
            else
            {
                plugin_return_value = PLUGIN_BYTE_ERROR;
            }
            break;
        }

        case CMD_SET_FONT :
        {
            usbPrintf_P(PSTR("set font file %d\n"), msg->body.data[0]);
            oledSetFont(msg->body.data[0]);

            if (datalen > 1) {
                usbPrintf_P(PSTR("testing string \"%s\"\n"), (char *)&msg->body.data[1]);
                oledFlipBuffers(0,0);
                oledWriteActiveBuffer();
                oledClear();
                oledPutstr((char *)&msg->body.data[1]);
            }

            return;
        }
        case CMD_STACK_FREE:
            
            usbPutstr("Stack Free ");
            int_to_string(stackFree(),stack_str);
            usbPutstr(stack_str);
            usbPutstr(" bytes\n");
        return;
#endif

        default :   return;
    }
    usbSendMessage(datacmd, 1, &plugin_return_value);
}