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;
}
