예제 #1
0
/*! \fn     displayCurrentSearchLoginTexts(char* text)
*   \brief  Display current search login text
*   \param  text            Text to be displayed
*   \param  resultsarray    Pointer to the array in which to store the addresses
*   \return Number of matching parents we displayed
*/
uint8_t displayCurrentSearchLoginTexts(char* text, uint16_t* resultsarray)
{
    uint16_t tempNodeAddr;
    pNode temp_pnode;
    uint8_t i;
    
    // Set font for search text
    oledSetFont(FONT_PROFONT_18);
    
    // Clear current text
    oledFillXY(100, 14, 50, 24, 0x00);
    
    // Display new search text
    oledPutstrXY(148, 14, OLED_RIGHT, text);
    
    // Set default font
    oledSetFont(FONT_DEFAULT);
    
    // Find the address of the first match
    tempNodeAddr = searchForServiceName((uint8_t*)text, COMPARE_MODE_COMPARE, SERVICE_CRED_TYPE);
    
    if (tempNodeAddr != last_matching_parent_addr)
    {
        last_matching_parent_addr = tempNodeAddr;
        
        for (i = 0; i < 4; i++)
        {
            oledFillXY((i&1)*175, 6+(i&2)*19, 81, 14, 0x00);
        }
        
        // Print the next 4 services
        i = 0;
        while ((tempNodeAddr != NODE_ADDR_NULL) && (i != 4))
        {
            resultsarray[i] = tempNodeAddr;
            readParentNode(&temp_pnode, tempNodeAddr);
            displayServiceAtGivenSlot(i, (const char*)temp_pnode.service);
            tempNodeAddr = temp_pnode.nextParentAddress;
            i++;
        }
        
        // Store and return number of children
        last_matching_parent_number = i;
    }
    
    return last_matching_parent_number;
}
예제 #2
0
/*! \fn     guiGetPinFromUser(volatile uint16_t* pin_code, uint8_t stringID)
*   \brief  Ask the user to enter a PIN
*   \param  pin_code    Pointer to where to store the pin code
*   \param  stringID    String ID
*   \return If the user approved the request
*/
RET_TYPE guiGetPinFromUser(volatile uint16_t* pin_code, uint8_t stringID)
{
    // If we don't need a pin code, send default one
    #if defined(NO_PIN_CODE_REQUIRED) || defined(HARDWARE_V1) || defined(V2_DEVELOPERS_BOTPCB_BOOTLOADER_SETUP)
        *pin_code = SMARTCARD_DEFAULT_PIN;
        return RETURN_OK;
    #endif
    
    RET_TYPE ret_val = RETURN_NOK;
    uint8_t selected_digit = 0;
    uint8_t finished = FALSE;
    uint8_t current_pin[4];
    RET_TYPE temp_rettype;
    int8_t temp_int8;
    
    // Set current pin to 0000
    memset((void*)current_pin, 0, 4);
    
    // Draw pin entering bitmap
    oledClear();
    oledBitmapDrawFlash(0, 0, BITMAP_YES_NO, 0);
    oledBitmapDrawFlash(83, 51, BITMAP_PIN_LINES, 0);
    oledBitmapDrawFlash(238, 23, BITMAP_RIGHT_ARROW, 0);
    oledPutstrXY(0, 0, OLED_CENTRE, readStoredStringToBuffer(stringID));
    oledDisplayOtherBuffer();
    oledSetFont(FONT_PROFONT_24);
    oledWriteActiveBuffer();
    
    // Display current pin on screen
    guiDisplayPinOnPinEnteringScreen(current_pin, selected_digit);
    
    // While the user hasn't entered his pin
    while(!finished)
    {
        // Still process the USB commands
        usbProcessIncoming(USB_CALLER_PIN);
        // Detect key touches
        temp_rettype = touchDetectionRoutine(0);
        // Send it to the touch wheel interface logic
        temp_int8 = touchWheelIntefaceLogic(temp_rettype);
        
        // Position increment / decrement
        if (temp_int8 != 0)
        {
            if ((current_pin[selected_digit] == 0x0F) && (temp_int8 == 1))
            {
                current_pin[selected_digit] = 0xFF;
            }
            else if ((current_pin[selected_digit] == 0) && (temp_int8 == -1))
            {
                current_pin[selected_digit] = 0x10;
            }
            current_pin[selected_digit] += temp_int8;
            guiDisplayPinOnPinEnteringScreen(current_pin, selected_digit);
        }
        
        if ((isSmartCardAbsent() == RETURN_OK) || (hasTimerExpired(TIMER_USERINT, TRUE) == TIMER_EXPIRED))
        {
            // Smartcard removed, no reason to continue
            ret_val = RETURN_NOK;
            finished = TRUE;
        }
        if (temp_rettype & RETURN_LEFT_PRESSED)
        {
            if (selected_digit == 1)
            {
                oledFillXY(0, 23, 18, 18, 0x00);
                oledBitmapDrawFlash(0, 24, BITMAP_CROSS, 0);
            }
            if (selected_digit > 0)
            {
                // When going back set pin digit to 0
                current_pin[selected_digit] = 0;
                current_pin[--selected_digit] = 0;
            }
            else
            {
                ret_val = RETURN_NOK;
                finished = TRUE;
            }
            guiDisplayPinOnPinEnteringScreen(current_pin, selected_digit);
            oledBitmapDrawFlash(238, 23, BITMAP_RIGHT_ARROW, 0);
        }
        else if (temp_rettype & RETURN_RIGHT_PRESSED)
        {
            if (selected_digit == 2)
            {
                oledFillXY(238, 23, 18, 18, 0x00);
                oledBitmapDrawFlash(240, 24, BITMAP_TICK, 0);
            }
            if (selected_digit < 3)
            {
                selected_digit++;
            }
            else
            {
                ret_val = RETURN_OK;
                finished = TRUE;
            }
            guiDisplayPinOnPinEnteringScreen(current_pin, selected_digit);
            oledBitmapDrawFlash(0, 23, BITMAP_LEFT_ARROW, 0);
        }
    }
    
    // Reset default font
    oledSetFont(FONT_DEFAULT);
    oledWriteInactiveBuffer();
    
    // Store the pin
    *pin_code = (uint16_t)(((uint16_t)(current_pin[0]) << 12) | (((uint16_t)current_pin[1]) << 8) | (current_pin[2] << 4) | current_pin[3]);
    
    // Set current pin to 0000
    memset((void*)current_pin, 0, 4);
    
    // Prevent touches until the user lifts his finger
    touchInhibitUntilRelease();
    
    // Return success status
    return ret_val;
}
/*! \fn     displayCurrentSearchLoginTexts(char* text)
*   \brief  Display current search login text
*   \param  text            Text to be displayed
*   \param  resultsarray    Pointer to the array in which to store the addresses
*   \param  search_index    Current search index (aka strlen(text))
*   \return Number of matching parents we displayed
*/
static inline uint8_t displayCurrentSearchLoginTexts(char* text, uint16_t* resultsarray, uint8_t search_index)
{
    uint16_t tempNodeAddr;
    pNode temp_pnode;
    uint8_t i, j;
    
    // Set font for search text
    oledSetFont(FONT_PROFONT_18);
    
    // Clear current text
    oledFillXY(100, 18, 50, 23, 0x00);
    
    // Display new search text
    oledPutstrXY(148, 17, OLED_RIGHT, text);
    
    // Set default font
    oledSetFont(FONT_DEFAULT);
    
    // Find the address of the first match
    tempNodeAddr = searchForServiceName((uint8_t*)text, COMPARE_MODE_COMPARE, SERVICE_CRED_TYPE);
    
    // Only change display if the first displayed service changed
    if (tempNodeAddr != last_matching_parent_addr)
    {
        last_matching_parent_addr = tempNodeAddr;
        
        for (i = 0; i < 4; i++)
        {
            oledFillXY((i&1)*170, 2+(i&2)*23, 84, 14, 0x00);
        }
        
        // Print the next 4 services (loop is until 5 for additional checks)
        i = 0;
        uint8_t temp_bool = TRUE;
        while ((temp_bool != FALSE) && (i != 5))
        {
            resultsarray[i] = tempNodeAddr;
            readParentNode(&temp_pnode, tempNodeAddr);
            
            // Display only first 4 services
            if (i < 4)
            {
                displayCredentialAtSlot(i, (char*)temp_pnode.service, INDEX_TRUNCATE_SERVICE_SEARCH);
            }
            // Loop around
            if (temp_pnode.nextParentAddress == NODE_ADDR_NULL)
            {
                tempNodeAddr = getStartingParentAddress();
            } 
            else
            {
                tempNodeAddr = temp_pnode.nextParentAddress;
            }
            i++;
            // Check that we haven't already displayed the next node
            for (j = 0; j < i; j++)
            {
                if (resultsarray[j] == tempNodeAddr)
                {
                    temp_bool = FALSE;
                }
            }
        }
        
        // Check if we could read 5 services after the given search text to know if we need to show the right arrow
        if (i == 5)
        {       
            // Compare our text with the last service text and see if they match
            if (strncmp(text, (char*)temp_pnode.service, search_index + 1) == 0)
            {
                // show arrow
                oledBitmapDrawFlash(176, 24, BITMAP_LOGIN_RARROW, 0);
            } 
            else
            {
                // hide arrow
                oledFillXY(176, 24, 16, 16, 0);
                i = 4;
            }
        }
        
        // Store and return number of children
        last_matching_parent_number = i;
    }
    
         
    // If the text is 4 chars long no need to display right arrow
    if ((search_index == SEARCHTEXT_MAX_LENGTH - 1) && (last_matching_parent_number > 4))
    {
        // hide arrow
        last_matching_parent_number = 4;
        oledFillXY(176, 24, 16, 16, 0);
    }
         
    return last_matching_parent_number;
}
예제 #4
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
    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);
}
예제 #5
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);
}