int main(int argc, char *argv[]) { Queue *q = queueCreate(); queuePush(q, 'a'); queuePush(q, 'b'); queuePush(q, 'c'); queuePush(q, 'd'); printf("%c", queuePop(q)); printf("%c", queuePop(q)); printf("%c", queuePop(q)); queuePush(q, 'e'); queuePush(q, '\n'); printf("%c", queuePop(q)); printf("%c", queuePop(q)); printf("%c", queuePop(q)); queuePush(q, 0); queuePush(q, 0); queuePush(q, 0); queuePush(q, 0); queueDelete(q); return 0; }
void DoSpoon(UserContext *context) { // Get an available process id. int newPid = pidCount++; PCB *child = (PCB *)malloc(sizeof(PCB)); // If for any reason we can't fork, return ERROR to calling process. if (!newPid || !child) { context->regs[0] = ERROR; return; } PCB_Init(child); child->id = newPid; child->parent = current_process; queuePush(child->parent->children, child); if (queueIsEmpty(child->parent->children)) TracePrintf(3, "DoSpoon: parent queue empty.\n"); child->status = RUNNING; // Return 0 to child and arm the child for execution. child->user_context = *context; queuePush(ready_queue, child); queuePush(process_queue, child); KernelContextSwitch(SpoonKernel, current_process, child); // Return child's pid to parent and resume execution of the parent. if (current_process->id == newPid) { *context = current_process->user_context; context->regs[0] = 0; } else { context->regs[0] = newPid; } }
bool ValidateGameField() { /*проверка поля для игры: каждая ячейка должна быть доступна минимум с 2х сторон (чтобы не было тупиков) поле должно быть связным (чтобы не было недосягаемых, огороженных со всех сторон участков) */ printf("Validate game field...\n"); int k = 0; for (int i = 0; i < GAME_ROWS; i++) { for (int j = 0; j < GAME_COLS; j++) { checked[i][j] = 0; k = 0; for (int l = 0; l < 4; l++) { if (isCellReachable(j, i, l)) { k++; } } if (k < 2) { printf("Bad field for game: dead end at (y = %d, x = %d)\n", i, j); checked[i][j] = 1; return false; } } } //проверка связности поля алгоритмом, напоминающим поиск в ширину в графе qhead = 0; qtail = 0; queuePush(0, 0); int cx, cy; int rx, ry; while (!queueEmpty()) { queueTake(&cx, &cy); checked[cy][cx] = 1; for (int i = 1; i < 5; i++) { if (!isCellReachable(cx, cy, i % 4)) { continue; } getViewingCell(cx, cy, i % 4, &rx, &ry); if (checked[ry][rx] == 1) { continue; } queuePush(rx, ry); } } printf("\n"); for (int i = 0; i < GAME_ROWS; i++) { for (int j = 0; j < GAME_COLS; j++) { if (checked[i][j] == 0) { printf("Bad field for game: unreachable cell at (y = %d, x = %d)\n", i, j); return false; } } } return true; }
int main() { Queue *queue = malloc(sizeof(Queue)); queueCreate(queue, 10); queuePush(queue, 1); queuePush(queue, 2); queuePush(queue, 3); queuePop(queue); queuePop(queue); assert(queuePeek(queue) == 3); assert(queueEmpty(queue) == 0); queueDestroy(queue); return 0; }
void LoadNextProc(UserContext *context, int block) { DelayPop(); if (!queueIsEmpty(ready_queue)) { if (current_process) { current_process->user_context = *context; if (block == NO_BLOCK) { queuePush(ready_queue, current_process); } } PCB *next = queuePop(ready_queue); TracePrintf(1, "LoadNextProc: Next Process Id: %d\n", next->id); if(next->id == 2) TracePrintf(3, "LoadNextProc: PCB has %p child\n", next->parent->children->head); WriteRegister(REG_PTBR1, (unsigned int) &(next->cow.pageTable)); WriteRegister(REG_TLB_FLUSH, TLB_FLUSH_1); KernelContextSwitch(MyKCS, current_process, next); TracePrintf(1, "LoadNextProc: Got past MyKCS\n"); *context = current_process->user_context; TracePrintf(3, "LoadNextProc: current user context pc is %p\n", context->pc); } }
int sem_wait_reply(void *request){ char send = FALSE; struct request_header reply; struct sem *find; struct req_node_info node; node.id = ((struct sem_req*)request)->req.src_node; node.seq_number = ((struct sem_req*)request)->req.src_seq_number; *(char*)(request+sizeof(struct sem_req)+((struct sem_req*)request)->name_len)=0; find = hashTableSearch(g_group.sem_table, ((struct sem_req*)request)->hash_id, (request+sizeof(struct sem_req))); reply.seq_number = node.seq_number; if(find == NULL){ reply.msg_type = MSG_SEM_FAILED; sendTo(node.id, (void*)&reply, sizeof(struct request_header)); return -1; } pthread_mutex_lock(&find->lock); if(find->value >0){ send=TRUE; find->value--; } else queuePush(find->queue, (void*)&node); pthread_mutex_unlock(&find->lock); if(send==TRUE){ reply.msg_type = MSG_SEM_OK; sendTo(node.id, (void*)&reply, sizeof(struct request_header)); } return 1; }
void DoLockInit(UserContext *context) { if (BufferCheck(context->regs[0], INT_LENGTH) == ERROR || BufferWriteCheck(context->regs[0], INT_LENGTH) == ERROR) { TracePrintf(1, "DoLockInit: Pointer given not valid. Returning Error\n"); context->regs[0] = ERROR; return; } Lock *lock = malloc(sizeof(Lock)); if (lock == NULL) { TracePrintf(2, "CreateLock, malloc error\n"); context->regs[0] = ERROR; return; } lock->waiting = queueNew(); if (lock->waiting == NULL) { TracePrintf(2, "CreateLock: malloc error\n"); context->regs[0] = ERROR; return; } lock->cvars = 0; int lock_id = lock_count * NUM_RESOURCE_TYPES + LOCK; lock_count++; lock->id = lock_id; *((int *) context->regs[0]) = lock_id; queuePush(locks, lock); context->regs[0] = SUCCESS; }
void DoWait(UserContext *context) { if (BufferCheck(context->regs[0], INT_LENGTH) == ERROR || BufferWriteCheck(context->regs[0], INT_LENGTH) == ERROR) { TracePrintf(1, "DoWait: Pointer given not valid. Returning Error\n"); context->regs[0] = ERROR; return; } if (queueIsEmpty(current_process->children) && queueIsEmpty(current_process->deadChildren)) { TracePrintf(2, "DoWait: PCB %d Has no children. Returning Error.\n", current_process->id); TracePrintf(2, "DoWait: Children queue count: %d, deadChildren count: %d\n", current_process->children->length, current_process->deadChildren->length); context->regs[0] = ERROR; } else { if (queueIsEmpty(current_process->deadChildren)) { current_process->status = WAITING; queuePush(wait_queue, current_process); LoadNextProc(context, BLOCK); } ZCB *child = queuePop(current_process->deadChildren); queueRemove(process_queue, child); *((int *)context->regs[0]) = child->status; context->regs[0] = child->id; } }
void __attribute__((interrupt, no_auto_psv)) _T2Interrupt(void) { MacPacket rx_packet; Payload rx_payload; if (!radioRxQueueEmpty()) { // Check for unprocessed packet rx_packet = radioDequeueRxPacket(); if(rx_packet == NULL) return; // Retrieve payload rx_payload = macGetPayload(rx_packet); // Switch on packet type Test* test = (Test*) malloc(sizeof(Test)); if(!test) return; test->packet = rx_packet; switch(payGetType(rx_payload)) { case RADIO_TEST: test->tf = &test_radio; queuePush(fun_queue, test); break; case GYRO_TEST: test->tf = &test_gyro; queuePush(fun_queue, test); break; case HALL_TEST: test->tf = &test_hall; queuePush(fun_queue, test); break; case ACCEL_TEST: test->tf = &test_accel; queuePush(fun_queue, test); break; case DFLASH_TEST: test->tf = &test_dflash; queuePush(fun_queue, test); break; case MOTOR_TEST: test->tf = &test_motor; queuePush(fun_queue, test); break; case SMA_TEST: test->tf = &test_sma; queuePush(fun_queue, test); break; default: // temporary to check out what is happening to packets test->tf = &test_radio; queuePush(fun_queue, test); break; } } _T2IF = 0; }
int sem_imanager_req(){ int i, j; void *buf; unsigned int offset; unsigned int queue_count; unsigned int src_node; unsigned int seq_number; struct sem *find; struct req_node_info node; char *sem_name; if(g_group.coordinator.sem_id == g_group.node_id) return 1; //max 33 sem buf = mem_malloc(SEM_ISEM_SIZE); ((struct request_header*)buf)->msg_type = MSG_ISEM_MANAGER; sendRecv(g_group.coordinator.sem_id, buf, sizeof(struct request_header), buf, SEM_ISEM_SIZE); if(g_group.sem_table == NULL){ g_group.sem_table = hashTableCreate(SEM_HASH_SIZE); } src_node = ((struct isem_reply*)buf)->req.src_node; seq_number = ((struct isem_reply*)buf)->req.src_seq_number; offset = sizeof(struct isem_reply); for(i=0; i<((struct isem_reply*)buf)->sem_num; i++){ //sem name sem_name = malloc(((struct sem_info*)(buf+offset))->name_len); memcpy(sem_name, buf+offset+sizeof(struct sem_info), ((struct sem_info*)(buf+offset))->name_len); sem_name[((struct sem_info*)(buf+offset))->name_len] = 0; //search sem find = hashTableSearch(g_group.sem_table, ((struct sem_info*)(buf+offset))->hash_id, sem_name); if(find == NULL){ //create new find = createNewSem(((struct sem_info*)(buf+offset))->hash_id, sem_name, ((struct sem_info*)(buf+offset))->value); hashTableInsert(g_group.sem_table, (struct hashheader*)find); }else{ if(find->queue->use!=0){ while(queuePop(find->queue)!=NULL); } } //copy queue info queue_count = ((struct sem_info*)(buf+offset))->queue_count; offset += sizeof(struct sem_info)+strlen(sem_name); for(j=0; j< queue_count;j++){ node.id = ((struct req_node_info*)(buf+offset))->id; node.seq_number = ((struct req_node_info*)(buf+offset))->seq_number; queuePush(find->queue, (void*)&node); offset += sizeof(struct req_node_info); } } g_group.coordinator.sem_id = g_group.node_id; ((struct request_header*)buf)->msg_type = MSG_ISEM_READY; ((struct request_header*)buf)->seq_number = seq_number; sendTo(src_node ,buf, sizeof(struct request_header)); mem_free(buf); return 1; }
void scManagerReceiveMessage(NodeManagerInterface nmi, JausMessage message) { ServiceConnection sc; char string[32] = {0}; pthread_mutex_lock(&nmi->scm->mutex); sc = nmi->scm->incomingSc; while(sc) { if(sc->commandCode == message->commandCode && jausAddressEqual(sc->address, message->source) ) { if(sc->isActive) { sc->lastSentTime = ojGetTimeSec(); if(sc->queueSize && sc->queueSize == sc->queue->size) { jausMessageDestroy(queuePop(sc->queue)); queuePush(sc->queue, (void *)message); } else { queuePush(sc->queue, (void *)message); } } else { // TODO: Error? received a message for inactive SC jausMessageDestroy(message); } pthread_mutex_unlock(&nmi->scm->mutex); return; } sc = sc->nextSc; } jausAddressToString(message->source, string); jausMessageDestroy(message); pthread_mutex_unlock(&nmi->scm->mutex); }
int main() { Queue queue; queueCreate(&queue, 5); queuePush(&queue, 1); queuePush(&queue, 2); queuePop(&queue); printf("%d\n", queuePeek(&queue)); queuePop(&queue); queuePush(&queue, 1); queuePush(&queue, 2); printf("%d\n", queuePeek(&queue)); queuePush(&queue, 3); printf("%d\n", queuePeek(&queue)); printf("%d\n", queuePeek(&queue)); queueDestroy(&queue); return 0; }
int LockAcquire(Lock *lock) { if (lock == NULL) { TracePrintf(2, "LockAcquire: Lock not found.\n"); return ERROR; } else if (lock->owner && lock->owner != current_process) { queuePush(lock->waiting, current_process); return BLOCK; } else { lock->owner = current_process; return SUCCESS; } }
void KillProc(PCB *pcb) { TracePrintf(2, "KillProc\n"); if (1 == current_process->id) { TracePrintf(1, "KillProc: init program being killed. Now calling halt.\n"); Halt(); } PCB *parent = pcb->parent; if (parent) { if (parent->status == WAITING) { queueRemove(wait_queue, parent); queuePush(ready_queue, parent); } ZCB *zombie = (ZCB *) malloc(sizeof(ZCB)); zombie->id = pcb->id; zombie->status = DEAD; zombie->exit_status = pcb->status; queuePush(parent->deadChildren, zombie); queuePush(process_queue, zombie); } for (List *child = current_process->children->head; child; child = child->next) ((PCB *) child->data)->parent = NULL; if (pcb->parent) { queueRemove(pcb->parent->children, pcb); } // Update process queue queueRemove(process_queue, pcb); FreePCB(pcb); }
int main() { Queue q; queueCreate(&q, 100); for (int i = 0; i < 10; ++i) { queuePush(&q, i); } printf("Bingo\n"); for (int i = 0; i < 10; ++i) { queuePop(&q); } return 0; }
void scManagerReceiveMessage(NodeManagerInterface nmi, JausMessage message) { ServiceConnection sc = nmi->scm->incommingSc; char string[32] = {0}; while(sc) { if(sc->commandCode == message->commandCode && sc->address->id == message->source->id ) { if(sc->isActive) { sc->lastSentTime = getTimeSeconds(); if(sc->queueSize && sc->queueSize == sc->queue->size) { jausMessageDestroy(queuePop(sc->queue)); queuePush(sc->queue, (void *)message); } else { queuePush(sc->queue, (void *)message); } // cDebug(3, "Queue Size: %d\n", sc->queue->size); } else { // TODO: Error? received a message for inactive SC jausMessageDestroy(message); } return; } sc = sc->nextSc; } jausAddressToString(message->source, string); cError("libnodeManager: scManangerReceiveMessage: No SC for: %s, from: %s\n", jausMessageCommandCodeString(message), string); jausMessageDestroy(message); }
/* ** Allocate and insert a node */ static int closureInsertNode( closure_queue *pQueue, /* Add new node to this queue */ closure_cursor *pCur, /* The cursor into which to add the node */ sqlite3_int64 id, /* The node ID */ int iGeneration /* The generation number for this node */ ){ closure_avl *pNew = sqlite3_malloc( sizeof(*pNew) ); if( pNew==0 ) return SQLITE_NOMEM; memset(pNew, 0, sizeof(*pNew)); pNew->id = id; pNew->iGeneration = iGeneration; closureAvlInsert(&pCur->pClosure, pNew); queuePush(pQueue, pNew); return SQLITE_OK; }
int LockRelease(Lock *lock) { if (lock == NULL) { TracePrintf(2, "LockRelease: Lock not found.\n"); return ERROR; } else if (lock->owner != current_process) { TracePrintf(2, "LockRelease: Current process does not hold lock it is trying to release.\n"); return ERROR; } else { lock->owner = queuePop(lock->waiting); if (lock->owner) { queuePush(ready_queue, lock->owner); } return SUCCESS; } }
/*----------------------------------------------------------------------------*/ static size_t canWrite(void *object, const void *buffer, size_t length) { assert(length % sizeof(struct CanStandardMessage) == 0); struct Can * const interface = object; if (interface->mode == MODE_LISTENER) return 0; LPC_CAN_Type * const reg = interface->base.reg; const struct CanStandardMessage *input = buffer; const size_t initialLength = length; const irqState state = irqSave(); if (queueEmpty(&interface->txQueue)) { uint32_t status = reg->SR; while (length && (status & SR_TBS_MASK)) { /* One of transmit buffers is empty */ status = sendMessage(interface, (const struct CanMessage *)input, status); length -= sizeof(*input); ++input; } } while (length && !queueFull(&interface->txQueue)) { struct CanMessage *output; queuePop(&interface->pool, &output); memcpy(output, input, sizeof(*input)); queuePush(&interface->txQueue, &output); length -= sizeof(*input); ++input; } irqRestore(state); return initialLength - length; }
int TestQueue() { Queue* Q; int i; Q = malloc(sizeof(struct Queue)); queueCreate(Q, 10); for (i = 0; i < 100; ++i) queuePush(Q, i); while (!queueEmpty(Q)) { printf("%d ", queuePeek(Q)); queuePop(Q); } printf("\n"); queueDestroy(Q); }
int main (int argc, char **argv) { int i; int val[] = {2,3,6,8,10,12,14,16,18,20,21,22,23,24,25}; int value; Queue q ; queueCreate(&q, 1024); for(i = 0; i < 15; i++) { queuePush(&q, val[i]); print_list(q.node); } for(i = 0; i < 15; i++) { value = queuePeek(&q); printf("val = %d\n", value); queuePop(&q); print_list(q.node); } queueDestroy(&q); return 0; }
void lmHandlerReceiveLargeMessage(NodeManagerInterface nmi, JausMessage message) { LargeMessageList msgList; JausMessage tempMessage; JausMessage outMessage; int i; unsigned long sequenceNumber; unsigned long index; JausUnsignedInteger newDataSize = 0; JausUnsignedInteger bufferIndex = 0; char address[128] = {0}; switch(message->dataFlag) { case JAUS_FIRST_DATA_PACKET: // Check for valid SeqNumber(0), else Error if(message->sequenceNumber) { cError("LargeMessageHandler: Received First Data Packet with invalid Sequence Number(%d)\n", message->sequenceNumber); jausMessageDestroy(message); return; } // Check if LargeMessageList exists (same CC & Source) msgList = lmHandlerGetMessageList(nmi->lmh, message); if(msgList) { // Destroy the list and all messages vectorRemove(nmi->lmh->messageLists, msgList, (void *)lmHandlerMessageListEqual); lmListDestroy(msgList); } // create LargeMessageList msgList = lmListCreate(); vectorAdd(nmi->lmh->messageLists, msgList); // add message to LargeMessageList at first position msgList->commandCode = message->commandCode; msgList->source->id = message->source->id; vectorAdd(msgList->messages, message); break; case JAUS_NORMAL_DATA_PACKET: // Check if LargeMessageList exists, error if not msgList = lmHandlerGetMessageList(nmi->lmh, message); if(msgList) { // Check if item exists in LargeMessageList with seqNumber if(vectorContains(msgList->messages, message, (void *)lmHandlerLargeMessageCheck) != -1) { cError("LargeMessageHandler: Received duplicate NORMAL_DATA_PACKET\n"); jausMessageDestroy(message); } else { // insert to Vector vectorAdd(msgList->messages, message); } } else { // Destroy Message cError("LargeMessageHandler: Received NORMAL_DATA_PACKET (0x%4X) for unknown Large Message Set (never received JAUS_FIRST_DATA_PACKET)\n", message->commandCode); jausMessageDestroy(message); } break; case JAUS_RETRANSMITTED_DATA_PACKET: // Check if LargeMessageList exists, error if not msgList = lmHandlerGetMessageList(nmi->lmh, message); if(msgList) { // Check if item exists in LargeMessageList with seqNumber if(vectorContains(msgList->messages, message, (void *)lmHandlerLargeMessageCheck) != -1) { tempMessage = (JausMessage) vectorRemove(msgList->messages, message, (void *)lmHandlerLargeMessageCheck); jausMessageDestroy(tempMessage); } // insert to Vector vectorAdd(msgList->messages, message); } else { cError("LargeMessageHandler: Received RETRANSMITTED_DATA_PACKET for unknown Large Message Set (never received JAUS_FIRST_DATA_PACKET)\n"); jausMessageDestroy(message); } break; case JAUS_LAST_DATA_PACKET: // Check if LargeMessageList exists, error if not msgList = lmHandlerGetMessageList(nmi->lmh, message); if(msgList) { // insert message to end of list vectorAdd(msgList->messages, message); // Create JausMessage object outMessage = jausMessageCreate(); // Calculate new message size newDataSize = 0; for(i = 0; i < msgList->messages->elementCount; i++) { tempMessage = (JausMessage)msgList->messages->elementData[i]; newDataSize += tempMessage->dataSize; } // Setup Header and Data Buffer outMessage->properties = tempMessage->properties; outMessage->commandCode = tempMessage->commandCode; outMessage->destination->id = tempMessage->destination->id; outMessage->source->id = tempMessage->source->id; outMessage->dataControl = tempMessage->dataControl; outMessage->sequenceNumber = tempMessage->sequenceNumber; outMessage->data = (unsigned char *) malloc(newDataSize); // Populate new message sequenceNumber = 0; bufferIndex = 0; while(sequenceNumber <= message->sequenceNumber) { index = 0; do { tempMessage = (JausMessage)msgList->messages->elementData[index]; index++; if(index > msgList->messages->elementCount) { // Invalid set of messages //TODO: Here is when you would request a retransmittal if ACK/NAK is set and ask the sending component to resend some packets // Received LAST_DATA_PACKET, but do not have proper sequence of messages // Destroy the list and all messages vectorRemove(nmi->lmh->messageLists, msgList, (void *)lmHandlerMessageListEqual); lmListDestroy(msgList); cError("LargeMessageHandler: Received LAST_DATA_PACKET, but do not have proper sequence of messages\n"); jausMessageDestroy(outMessage); return; } } while(tempMessage->sequenceNumber != sequenceNumber); // Move data from tempMessage to outMessage memcpy(outMessage->data + bufferIndex, tempMessage->data, tempMessage->dataSize); bufferIndex += tempMessage->dataSize; sequenceNumber++; // TOM: switched from i++ } // Set DataSize // Set proper header flags (dataFlag JAUS_SINGLE_DATA_PACKET) outMessage->dataSize = newDataSize; outMessage->dataFlag = JAUS_SINGLE_DATA_PACKET; if(outMessage->scFlag) { scManagerReceiveMessage(nmi, outMessage); } else { queuePush(nmi->receiveQueue, (void *)outMessage); } // Destroy LargeMessageList vectorRemove(nmi->lmh->messageLists, msgList, (void *)lmHandlerMessageListEqual); lmListDestroy(msgList); } else { cError("LargeMessageHandler: Received LAST_DATA_PACKET for unknown Large Message Set (never received JAUS_FIRST_DATA_PACKET)\n"); jausMessageDestroy(message); } break; default: jausAddressToString(message->source, address); cError("lmHandler: Received (%s) with improper dataFlag (%d) from %s\n", jausMessageCommandCodeString(message), message->dataFlag, address); jausMessageDestroy(message); break; } }
/*----------------------------------------------------------------------------*/ static void interruptHandler(void *object) { struct Can * const interface = object; LPC_CAN_Type * const reg = interface->base.reg; bool event = false; while (reg->SR & SR_RBS) { if (!queueEmpty(&interface->pool)) { const uint32_t timestamp = interface->timer ? timerGetValue(interface->timer) : 0; const uint32_t data[2] = {reg->RDA, reg->RDB}; const uint32_t information = reg->RFS; struct CanStandardMessage *message; queuePop(&interface->pool, &message); message->timestamp = timestamp; message->id = reg->RID; message->length = RFS_DLC_VALUE(information); message->flags = 0; if (information & RFS_FF) message->flags |= CAN_EXT_ID; if (information & RFS_RTR) message->flags |= CAN_RTR; memcpy(message->data, data, sizeof(data)); queuePush(&interface->rxQueue, &message); event = true; } /* Release receive buffer */ reg->CMR = CMR_RRB; } uint32_t status = reg->SR; if (status & SR_TBS_MASK) { /* Disable interrupts for completed transmit buffers */ uint32_t enabledInterrupts = reg->IER; if (status & SR_TBS(0)) enabledInterrupts &= ~IER_TIE1; if (status & SR_TBS(1)) enabledInterrupts &= ~IER_TIE2; if (status & SR_TBS(2)) enabledInterrupts &= ~IER_TIE3; reg->IER = enabledInterrupts; while (!queueEmpty(&interface->txQueue) && (status & SR_TBS_MASK)) { const struct CanMessage *message; queuePop(&interface->txQueue, &message); status = sendMessage(interface, message, status); } } if (status & SR_BS) { /* * The controller is forced into a bus-off state, RM bit should be cleared * to continue normal operation. */ reg->MOD &= ~MOD_RM; } if (interface->callback && event) interface->callback(interface->callbackArgument); }
SAT_returnState route_pkt (tc_tm_pkt *pkt) { SAT_returnState res; TC_TM_app_id id; if ( !C_ASSERT(pkt != NULL && pkt->data != NULL)) { free_pkt(pkt); return SATR_ERROR; } if (!C_ASSERT(pkt->type == TC || pkt->type == TM)) { free_pkt(pkt); return SATR_ERROR; } if (!C_ASSERT(pkt->app_id < LAST_APP_ID && pkt->dest_id < LAST_APP_ID)) { free_pkt(pkt); return SATR_ERROR; } if (pkt->type == TC) { id = pkt->app_id; } else if (pkt->type == TM) { id = pkt->dest_id; } else { return SATR_ERROR; } if (id == SYSTEM_APP_ID && pkt->ser_type == TC_HOUSEKEEPING_SERVICE && pkt->ser_subtype == TM_HK_PARAMETERS_REPORT && pkt->data[0] == WOD_REP) { /* * A new WOD arrived from the OBC. Store it and extract the information. * The transmission of each WOD is handled by the COMMS dispatcher function */ SYSVIEW_PRINT("WOD from OBC"); store_wod_obc(pkt->data + 1, pkt->len - 1); } else if (id == SYSTEM_APP_ID && pkt->ser_type == TC_HOUSEKEEPING_SERVICE && pkt->ser_subtype == TM_HK_PARAMETERS_REPORT && pkt->data[0] == EXT_WOD_REP) { /* * A new exWOD arrived from the OBC. Store it and extract the information. * The transmission of each exWOD is handled by the COMMS dispatcher function */ SYSVIEW_PRINT("exWOD from OBC"); store_ex_wod_obc(pkt->data, pkt->len); } else if (id == SYSTEM_APP_ID && pkt->ser_type == TC_HOUSEKEEPING_SERVICE) { res = hk_app (pkt); } else if (id == SYSTEM_APP_ID && pkt->ser_type == TC_FUNCTION_MANAGEMENT_SERVICE) { res = function_management_app (pkt); } else if (id == SYSTEM_APP_ID && pkt->ser_type == TC_LARGE_DATA_SERVICE) { res = large_data_app (pkt); if (res == SATR_OK) { free_pkt (pkt); return SATR_OK; } } else if (id == SYSTEM_APP_ID && pkt->ser_type == TC_TEST_SERVICE) { //C_ASSERT(pkt->ser_subtype == 1 || pkt->ser_subtype == 2 || pkt->ser_subtype == 9 || pkt->ser_subtype == 11 || pkt->ser_subtype == 12 || pkt->ser_subtype == 13) { free_pkt(pkt); return SATR_ERROR; } res = test_app (pkt); } else if (id == EPS_APP_ID) { queuePush (pkt, OBC_APP_ID); } else if (id == ADCS_APP_ID) { queuePush (pkt, OBC_APP_ID); } else if (id == OBC_APP_ID) { queuePush (pkt, OBC_APP_ID); } else if (id == IAC_APP_ID) { queuePush (pkt, OBC_APP_ID); } else if (id == GND_APP_ID) { if (pkt->len > MAX_PKT_DATA) { large_data_downlinkTx_api (pkt); } else { tx_ecss (pkt); } } else if (id == DBG_APP_ID) { queuePush (pkt, OBC_APP_ID); } else { free_pkt(pkt); } return SATR_OK; }
void Ready(PCB *proc) { queuePush(ready_queue, proc); }
/*----------------------------------------------------------------------------*/ static enum result canInit(void *object, const void *configBase) { const struct CanConfig * const config = configBase; const struct CanBaseConfig baseConfig = { .channel = config->channel, .rx = config->rx, .tx = config->tx }; struct Can * const interface = object; enum result res; /* Call base class constructor */ if ((res = CanBase->init(object, &baseConfig)) != E_OK) return res; interface->base.handler = interruptHandler; interface->callback = 0; interface->timer = config->timer; interface->mode = MODE_LISTENER; const size_t poolSize = config->rxBuffers + config->txBuffers; res = queueInit(&interface->pool, sizeof(struct CanMessage *), poolSize); if (res != E_OK) return res; res = queueInit(&interface->rxQueue, sizeof(struct CanMessage *), config->rxBuffers); if (res != E_OK) return res; res = queueInit(&interface->txQueue, sizeof(struct CanMessage *), config->txBuffers); if (res != E_OK) return res; interface->poolBuffer = malloc(sizeof(struct CanStandardMessage) * poolSize); struct CanStandardMessage *message = interface->poolBuffer; for (size_t index = 0; index < poolSize; ++index) { queuePush(&interface->pool, &message); ++message; } LPC_CAN_Type * const reg = interface->base.reg; reg->MOD = MOD_RM; /* Reset CAN */ reg->IER = 0; /* Disable Receive Interrupt */ reg->GSR = 0; /* Reset error counter */ interface->rate = config->rate; reg->BTR = calcBusTimings(interface, interface->rate); /* Disable Reset mode and activate Listen Only mode */ reg->MOD = MOD_LOM; LPC_CANAF->AFMR = AFMR_AccBP; //FIXME #ifdef CONFIG_CAN_PM if ((res = pmRegister(interface, powerStateHandler)) != E_OK) return res; #endif irqSetPriority(interface->base.irq, config->priority); /* Enable interrupts on message reception and bus error */ reg->IER = IER_RIE | IER_BEIE; return E_OK; } /*----------------------------------------------------------------------------*/ static void canDeinit(void *object) { struct Can * const interface = object; LPC_CAN_Type * const reg = interface->base.reg; /* Disable all interrupts */ reg->IER = 0; #ifdef CONFIG_CAN_PM pmUnregister(interface); #endif queueDeinit(&interface->txQueue); queueDeinit(&interface->rxQueue); CanBase->deinit(interface); } /*----------------------------------------------------------------------------*/ static enum result canCallback(void *object, void (*callback)(void *), void *argument) { struct Can * const interface = object; interface->callbackArgument = argument; interface->callback = callback; return E_OK; } /*----------------------------------------------------------------------------*/ static enum result canGet(void *object, enum ifOption option, void *data) { struct Can * const interface = object; switch (option) { case IF_AVAILABLE: *(size_t *)data = queueSize(&interface->rxQueue); return E_OK; case IF_PENDING: *(size_t *)data = queueSize(&interface->txQueue); return E_OK; case IF_RATE: *(uint32_t *)data = interface->rate; return E_OK; default: return E_INVALID; } } /*----------------------------------------------------------------------------*/ static enum result canSet(void *object, enum ifOption option, const void *data) { struct Can * const interface = object; LPC_CAN_Type * const reg = interface->base.reg; switch ((enum canOption)option) { case IF_CAN_ACTIVE: changeMode(interface, MODE_ACTIVE); return E_OK; case IF_CAN_LISTENER: changeMode(interface, MODE_LISTENER); return E_OK; case IF_CAN_LOOPBACK: changeMode(interface, MODE_LOOPBACK); return E_OK; default: break; } switch (option) { case IF_RATE: { const uint32_t rate = *(const uint32_t *)data; interface->rate = rate; reg->MOD |= MOD_RM; /* Enable Reset mode */ reg->BTR = calcBusTimings(interface, rate); reg->MOD &= ~MOD_RM; return E_OK; } default: return E_INVALID; } } /*----------------------------------------------------------------------------*/ static size_t canRead(void *object, void *buffer, size_t length) { assert(length % sizeof(struct CanStandardMessage) == 0); struct Can * const interface = object; struct CanStandardMessage *output = buffer; size_t read = 0; while (read < length && !queueEmpty(&interface->rxQueue)) { struct CanMessage *input; const irqState state = irqSave(); queuePop(&interface->rxQueue, &input); memcpy(output, input, sizeof(*output)); queuePush(&interface->pool, &input); irqRestore(state); read += sizeof(*output); ++output; } return read; }