void VM::delFrame(State *state) { CallFrame *frame = state->m_frame; CallFrame *above = frame->m_above; stackFree(state, frame->m_fastSlots, sizeof(Object **) * frame->m_fastSlotsCount); stackFree(state, frame->m_slots, sizeof(Object *) * frame->m_count); stackFree(state, frame, sizeof *frame); state->m_frame = above; }
static void cleanup(FILE *file) { stackFree(stack); stack = NULL; XML_ParserFree(parser); parser = NULL; fclose(file); }
static void pythonbriefLexer_FreeImpl( struct pythonbriefLexer_Ctx_struct * ctx ) { vectorFree( ctx->tokens ); stackFree( ctx->identStack ); ctx->origFree( ctx ); }
void test_Vector() { struct stack* alist = (struct stack*)malloc(sizeof(struct stack)); stackInitialize(alist,1024); int i = 0; for (i = 0; i < 15; i++) { char* tmp = (char*)malloc(sizeof(char)* 512); memset(tmp, 0, sizeof(char)* 512); sprintf(tmp, "hehe%d", i); printf("%s\n", tmp); stackPush(alist, tmp); } printf("\n\n"); int len = stackSize(alist); for (i = 0; i < len; i++) { char* tmp; //tmp=stackGet(alist, i); tmp=stackPop(alist); printf("%s\n", tmp); } stackFree(alist); }
char* infixToPostfix(char const* str) { generic_stack* infixStack = stackAllocate(1); generic_stack* output = stackAllocate(1); char peek; for (; *str; ++str) { stackPeek(infixStack, &peek); if (*str == '(') { peek = '('; stackPush(infixStack, &peek); } else if (*str == ')') { while (peek != '(') { stackPop(infixStack, &peek); stackPush(output, &peek); stackPeek(infixStack, &peek); } //Discard the '(' stackPop(infixStack, &peek); } else { while (!stackEmpty(infixStack) && precidence(peek) >= precidence(*str)) { stackPop(infixStack, &peek); stackPush(output, &peek); //Update the peeked stackPeek(infixStack, &peek); } stackPush(infixStack, str); } } while (!stackEmpty(infixStack)) { stackPop(infixStack, &peek); stackPush(output, &peek); } peek = '\0'; stackPush(output, &peek); char* result = malloc(stackSize(output)); memcpy(result, output->data, output->current); stackFree(infixStack); stackFree(output); return result; }
void VM::addFrame(State *state, size_t slots, size_t fastSlots) { auto *frame = (CallFrame *)stackAllocate(state, sizeof(CallFrame)); if (!frame) return; frame->m_above = state->m_frame; frame->m_count = slots; frame->m_slots = (Object **)stackAllocate(state, sizeof(Object *) * slots); if (!frame->m_slots) { stackFree(state, frame, sizeof *frame); return; } frame->m_fastSlotsCount = fastSlots; // don't need to be initialized since fast slots are not subjected to // traditional garbage collection. frame->m_fastSlots = (Object ***)stackAllocateUninitialized(state, sizeof(Object **) * fastSlots); if (!frame->m_fastSlots) { stackFree(state, frame->m_slots, sizeof(Object *) * slots); stackFree(state, frame, sizeof *frame); return; } state->m_frame = frame; }
void deleteCDG(CDGNode * root) { if (NULL == root) return; CDGNode *node; Stack *nodeStack = stackNew(sizeof(CDGNode *)); postOrder(root, nodeStack); while (!stackIsEmpty(nodeStack)) { stackPop(nodeStack, &node); deleteNode(node); } stackFree(nodeStack); }
ERROR interpret(SYMBOL_TABLE* table) { STACK_PTR stack; ERROR err; stack = gcMalloc(sizeof(struct STACK)); stackInit(stack); /// ----- POZNAMKA ----- err = recursive_interpret(table->curr,stack); stackFree(stack); gcFree(stack); return err; }
CDGNode *updateCDG(CDGNode * root, int initialize) { int size = sizeof(CDGNode *); assert(NULL != root); Stack *nodeStack = stackNew(size); CDGNode *node; postOrder(root, nodeStack); while (!stackIsEmpty(nodeStack)) { stackPop(nodeStack, &node); updateScore(node, initialize); } stackFree(nodeStack); return root; }
char* infixInsertExplicitConcatenation(char const* str) { generic_stack* output = stackAllocate(1); char temp; bool insertPlanned = false; for (; *str; str++) { if (*str == '[') { str = infixComputeBrackets(str, output); if (!str) { stackFree(output); return 0; } insertPlanned = true; } else { stackPush(output, str); if (!isOperator(*str)) { insertPlanned = true; } else if (*str == '|') { insertPlanned = false; } } if (insertPlanned && nextChar(str) != '\0' && (nextChar(str) == '(' || !isOperator(nextChar(str)))) { stackPush(output, "&"); insertPlanned = false; } } temp = '\0'; stackPush(output, &temp); char* result = malloc(strlen(output->data) + 1); strcpy(result, output->data); stackFree(output); return result; }
int freeCustomer(Customer *c) { if(c!=NULL) { while(stackIsEmpty(c->basket)==0) stackPop(c->basket); stackFree(c->basket); free(c); return 0; } else { return -1; } }
void postOrder(CDGNode * root, Stack * s) { if (NULL == root) return; Stack *temp = stackNew(sizeof(CDGNode *));; CDGNode *node; CDGNode *listNode; pushNodeListToStack(temp, root); while (!stackIsEmpty(temp)) { stackPop(temp, &node); if (getTrueNodeSet(node)) { pushNodeListToStack(temp, getTrueNodeSet(node)); } if (getFalseNodeSet(node)) { pushNodeListToStack(temp, getFalseNodeSet(node)); } stackPush(s, &node); } stackFree(temp); }
static void freeParser (pANTLR3_TREE_PARSER parser) { if (parser->rec != NULL) { // This may have ben a delegate or delegator parser, in which case the // state may already have been freed (and set to NULL therefore) // so we ignore the state if we don't have it. // if (parser->rec->state != NULL) { if (parser->rec->state->following != NULL) { stackFree(parser->rec->state->following); parser->rec->state->following = NULL; } } parser->rec->free(parser->rec); parser->rec = NULL; } ANTLR3_FREE(parser); }
bool postfix(token *tokens, int numTokens, Stack *output) { Stack operators, intermediate; int i; bool err = false; stackInit(&operators, numTokens); stackInit(&intermediate, numTokens); for(i = 0; i < numTokens; i++) { // From Wikipedia/Shunting-yard_algorithm: switch(tokenType(tokens[i])) { case value: { // If the token is a number, then add it to the output queue. //printf("Adding number %s to output stack\n", tokens[i]); evalStackPush(output, tokens[i]); } break; case function: { while(stackSize(&operators) > 0 && (tokenType(tokens[i]) != lparen) && ((precedence(tokens[i], (char*)stackTop(&operators)) <= 0))) { //printf("Moving operator %s from operator stack to output stack\n", (char*)stackTop(&operators)); evalStackPush(output, stackPop(&operators)); stackPush(&intermediate, stackTop(output)); } // If the token is a function token, then push it onto the stack. //printf("Adding operator %s to operator stack\n", tokens[i]); stackPush(&operators, tokens[i]); } break; case argsep: { /* * If the token is a function argument separator (e.g., a comma): * Until the token at the top of the stack is a left * paren, pop operators off the stack onto the output * queue. If no left paren encountered, either separator * was misplaced or parens mismatched. */ while(stackSize(&operators) > 0 && tokenType((token)stackTop(&operators)) != lparen && stackSize(&operators) > 1) { //printf("Moving operator from operator stack to output stack\n"); evalStackPush(output, stackPop(&operators)); stackPush(&intermediate, stackTop(output)); } /*if(stackSize(&operators) > 0 && tokenType((token)stackTop(&operators)) != lparen) { err = true; raise(parenMismatch); } printf("Removing left paren from operator stack\n"); stackPop(&operators); // Discard lparen*/ } break; case addop: case multop: case expop: { /* * If the token is an operator, op1, then: * while there is an operator token, op2, at the top of the stack, and * either op1 is left-associative and its precedence is less than or equal to that of op2, * or op1 is right-associative and its precedence is less than that of op2, * pop op2 off the stack, onto the output queue * push op1 onto the stack */ while(stackSize(&operators) > 0 && (tokenType((char*)stackTop(&operators)) == addop || tokenType((char*)stackTop(&operators)) == multop || tokenType((char*)stackTop(&operators)) == expop) && ((leftAssoc(tokens[i]) && precedence(tokens[i], (char*)stackTop(&operators)) <= 0) || (!leftAssoc(tokens[i]) && precedence(tokens[i], (char*)stackTop(&operators)) < 0))) { //printf("Moving operator %s from operator stack to output stack\n", (char*)stackTop(&operators)); evalStackPush(output, stackPop(&operators)); stackPush(&intermediate, stackTop(output)); } //printf("Adding operator %s to operator stack\n", tokens[i]); stackPush(&operators, tokens[i]); } break; case lparen: { // If the token is a left paren, then push it onto the stack //printf("Adding left paren to operator stack\n"); if (tokenType(stackTop(&operators)) == function) stackPush(output, FUNCTIONSEPARATOR); stackPush(&operators, tokens[i]); } break; case rparen: { /* * If the token is a right paren: * Until the token at the top of the stack is a left paren, pop operators off the stack onto the output queue * Pop the left paren from the stack, but not onto the output queue * If the stack runs out without finding a left paren, then there are mismatched parens */ while(stackSize(&operators) > 0 && tokenType((token)stackTop(&operators)) != lparen && stackSize(&operators) > 1) { //printf("Moving operator %s from operator stack to output stack\n", (char*)stackTop(&operators)); evalStackPush(output, stackPop(&operators)); stackPush(&intermediate, stackTop(output)); } if(stackSize(&operators) > 0 && tokenType((token)stackTop(&operators)) != lparen) { err = true; raise(parenMismatch); } //printf("Removing left paren from operator stack\n"); stackPop(&operators); // Discard lparen while (stackSize(&operators) > 0 && tokenType((token)stackTop(&operators)) == function) { //printf("Removing function from operator stack to output stack\n"); evalStackPush(output, stackPop(&operators)); stackPush(&intermediate, stackTop(output)); } } break; default: break; } } /* * When there are no more tokens to read: * While there are still operator tokens on the stack: * If the operator token on the top of the stack is a paren, then there are mismatched parens * Pop the operator onto the output queue */ while(stackSize(&operators) > 0) { if(tokenType((token)stackTop(&operators)) == lparen) { raise(parenMismatch); err = true; } //printf("Moving operator from operator stack to output stack\n"); evalStackPush(output, stackPop(&operators)); stackPush(&intermediate, stackTop(output)); } // pop result from intermediate stack stackPop(&intermediate); // free remaining intermediate results while (stackSize(&intermediate) > 0) { stackPop(&intermediate); } if (err == true) { while (stackSize(&operators) > 0) { token s = stackPop(&operators); //printf("Freeing %s from operators stack\n", s); free(s); } } stackFree(&intermediate); stackFree(&operators); return err; }
token doFunc(Stack *s, token function) { if (stackSize(s) == 0) { raise(inputMissing); return "NaN"; } else if (stackSize(s) == 1 && strcmp(stackTop(s), FUNCTIONSEPARATOR) == 0) { stackPop(s); raise(inputMissing); return "NaN"; } token input = (token)stackPop(s); number num = buildNumber(input); number result = num; number counter = 0; if(strncmp(function, "abs", 3) == 0) result = fabs(num); else if(strncmp(function, "floor", 5) == 0) result = floor(num); else if(strncmp(function, "ceil", 4) == 0) result = ceil(num); else if(strncmp(function, "sin", 3) == 0) result = !prefs.mode.degrees ? sin(num) : sin(toRadians(num)); else if(strncmp(function, "cos", 3) == 0) result = !prefs.mode.degrees ? cos(num) : cos(toRadians(num)); else if(strncmp(function, "tan", 3) == 0) result = !prefs.mode.degrees ? tan(num) : tan(toRadians(num)); else if(strncmp(function, "arcsin", 6) == 0 || strncmp(function, "asin", 4) == 0) result = !prefs.mode.degrees ? asin(num) : toDegrees(asin(num)); else if(strncmp(function, "arccos", 6) == 0 || strncmp(function, "acos", 4) == 0) result = !prefs.mode.degrees ? acos(num) : toDegrees(acos(num)); else if(strncmp(function, "arctan", 6) == 0 || strncmp(function, "atan", 4) == 0) result = !prefs.mode.degrees ? atan(num) : toDegrees(atan(num)); else if(strncmp(function, "sqrt", 4) == 0) result = sqrt(num); else if(strncmp(function, "cbrt", 4) == 0) result = cbrt(num); else if(strncmp(function, "log", 3) == 0) result = log(num); else if(strncmp(function, "exp", 3) == 0) result = exp(num); else if(strncmp(function, "min", 3) == 0) { while (stackSize(s) > 0 && strcmp(stackTop(s), FUNCTIONSEPARATOR) != 0) { input = (token)stackPop(s); num = buildNumber(input); if (num < result) result = num; } } else if(strncmp(function, "max", 3) == 0) { while (stackSize(s) > 0 && strcmp(stackTop(s), FUNCTIONSEPARATOR) != 0) { input = (token)stackPop(s); num = buildNumber(input); if (num > result) result = num; } } else if(strncmp(function, "sum", 3) == 0) { while (stackSize(s) > 0 && strcmp(stackTop(s), FUNCTIONSEPARATOR) != 0) { input = (token)stackPop(s); num = buildNumber(input); result += num; } } else if(strncmp(function, "avg", 3) == 0 || strncmp(function, "mean", 4) == 0) { // Result already initialized with first number counter = 1; while (stackSize(s) > 0 && strcmp(stackTop(s), FUNCTIONSEPARATOR) != 0) { input = (token)stackPop(s); num = buildNumber(input); result += num; counter++; } result /= counter; } else if(strncmp(function, "median", 6) == 0) { // needed for sorting Stack tmp, safe; // Result already initialized with first number counter = 1; stackInit(&tmp, (stackSize(s) > 0 ? stackSize(s) : 1)); stackInit(&safe, (stackSize(s) > 0 ? stackSize(s) : 1)); // add first value to the later sorted stack stackPush(&tmp, input); while (stackSize(s) > 0 && strcmp(stackTop(s), FUNCTIONSEPARATOR) != 0) { input = (token)stackPop(s); num = buildNumber(input); // save all numbers larger as the stack value while (stackSize(&tmp) > 0 && buildNumber(stackTop(&tmp)) < num) { stackPush(&safe, stackPop(&tmp)); } // push value on the sorted stack stackPush(&tmp, input); // push all saved numbers back on the sorted stack while (stackSize(&safe) > 0) { stackPush(&tmp, stackPop(&safe)); } counter++; } stackFree(&safe); // calculate the median index counter = (number)(((int)counter+1)/2); // pop all numbers until median index while (counter > 1) { stackPop(&tmp); counter--; } result = buildNumber(stackPop(&tmp)); // pop the remaining sorted stack while (stackSize(&tmp) > 0) { stackPop(&tmp); } stackFree(&tmp); } else if(strncmp(function, "var", 3) == 0) { Stack tmp; counter = 1; // second stack to store values during calculation of mean stackInit(&tmp, (stackSize(s) > 0 ? stackSize(s) : 1)); // push first value to temporary stack stackPush(&tmp, input); number mean = result; while (stackSize(s) > 0 && strcmp(stackTop(s), FUNCTIONSEPARATOR) != 0) { input = (token)stackPop(s); // push value to temporary stack stackPush(&tmp, input); num = buildNumber(input); mean += num; counter++; } // calculate mean mean /= counter; result = 0; // calculate sum of squared differences while (stackSize(&tmp) > 0) { input = (token)stackPop(&tmp); num = buildNumber(input)-mean; result += pow(num,2); } // determine variance result /= counter; stackFree(&tmp); } if (strcmp(stackTop(s), FUNCTIONSEPARATOR) == 0) stackPop(s); stackPush(s, num2Str(result)); return 0; }
int main(int argc, char *argv[]) { char* str = NULL; token* tokens = NULL; int numTokens = 0; Stack expr; int i; int ch, rflag = 0; prefs.precision = DEFAULTPRECISION; prefs.maxtokenlength = MAXTOKENLENGTH; while ((ch = getopt(argc, argv, "rm:")) != -1) { switch (ch) { case 'r': rflag = 1; break; case 'm': prefs.maxtokenlength = atoi(optarg); } } str = ufgets(stdin); while(str != NULL && strcmp(str, "quit") != 0) { if (strlen(str) == 0) { free(str); goto get_new_string; } if(type(*str) == text) { // Do something with command if (!execCommand(str)) goto no_command; free(str); str = NULL; } else { no_command: numTokens = tokenize(str, &tokens); free(str); str = NULL; if(prefs.display.tokens) { printf("\t%d tokens:\n", numTokens); for(i = 0; i < numTokens; i++) { printf("\t\"%s\"", tokens[i]); if(tokenType(tokens[i]) == value) printf(" = %f", buildNumber(tokens[i])); printf("\n"); } } // Convert to postfix stackInit(&expr, numTokens); if(prefs.display.postfix) printf("\tPostfix stack:\n"); postfix(tokens, numTokens, &expr); //stackReverse(&expr); /*printf("\tReversed postfix stack:\n\t"); for(i = 0; i < stackSize(&expr); i++) { printf("%s ", (token)(expr.content[i])); } printf("\n");*/ if(stackSize(&expr) != 1) { printf("\tError evaluating expression\n"); } else { if (!rflag) printf("\t= "); printf("%s\n", (char*)stackTop(&expr)); for (i=0; i< numTokens; i++) { if (tokens[i] == stackTop(&expr)) tokens[i] = NULL; } free(stackPop(&expr)); } for(i = 0; i < numTokens; i++) { if (tokens[i] != NULL) free(tokens[i]); } free(tokens); tokens = NULL; numTokens = 0; stackFree(&expr); } get_new_string: str = ufgets(stdin); } free(str); str = NULL; return EXIT_SUCCESS; }
CDGPath *getTopPaths(CDGContext * ctx, CDGNode * root, int numberOfPaths) { CDGPath *pathHead = NULL; CDGNode *path; CDGPath *currPath; CDGNode *node; int branch; Stack *changedNodes = stackNew(sizeof(CDGNode *)); Stack *changedBranches = stackNew(sizeof(int)); while (numberOfPaths--) { path = getTopPath(root, changedNodes, changedBranches); if (NULL == path) break; if (NULL == pathHead) { pathHead = setPathNode(newPath(), path); currPath = pathHead; } else { setNextPath(currPath, setPathNode(newPath(), path)); currPath = getNextPath(currPath); } updateCDG(root, 0); } while (!stackIsEmpty(changedNodes) && !stackIsEmpty(changedBranches)) { stackPop(changedNodes, &node); stackPop(changedBranches, &branch); if (isLeaf(node)) { setScore(node, 1); } else { if (branch) setBranchInfo(getID(node), 0, getBranchInfo(getID(node), 0)); else setBranchInfo(getID(node), getBranchInfo(getID(node), 1), 0); } } updateCDG(root, 0); stackFree(changedNodes); stackFree(changedBranches); /* Updating context */ (*ctx).topPaths = pathHead; /* Ensuring atleast one path was found */ if (NULL == pathHead) return NULL; /* Creating list of nodes from complicated path form */ CDGPath *outPathHead = NULL; currPath = NULL; path = NULL; while (NULL != pathHead) { path = getPathNode(pathHead); if (NULL == outPathHead) { outPathHead = setPathNode(newPath(), pathToList(path)); currPath = outPathHead; } else { setNextPath(currPath, setPathNode(newPath(), pathToList(path))); currPath = getNextPath(currPath); } pathHead = getNextPath(pathHead); } return outPathHead; }
/*! \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); }
/*! \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); }