| 0,0 → 1,339 |
|---|
| #include "maindefs.h" |
| #include "interrupts.h" |
| #include "msg_queues.h" |
| #include <delays.h> |
| #include <string.h> |
| |
| // The key to making this code safe for interrupts is that |
| // each queue is filled by only one writer and read by one reader. |
| // ToMainQueueFromLow: Writer is a low priority interrupt, Reader is main() |
| // ToMainQueueFromHigh: Writer is a high priority interrupt, Reader is main() |
| // FromMainQueueToLow: Writer is main(), Reader is a low priority interrupt |
| // FromMainQueueToHigh: Writer is main(), Reader is a high priority interrupt |
| |
| //#pragma udata msgqueue1_1 |
| //static msg MQ_ToMainFromLow_1; |
| //#pragma udata msgqueue1_2 |
| //static msg MQ_ToMainFromLow_2; |
| |
| #pragma udata msgqueue2_1 |
| static msg MQ_ToMainFromHigh_1; |
| #pragma udata msgqueue2_2 |
| static msg MQ_ToMainFromHigh_2; |
| #pragma udata msgqueue2_3 |
| static msg MQ_ToMainFromHigh_3; |
| |
| //#pragma udata msgqueue3_1 |
| //static msg MQ_FromMainToLow_1; |
| //#pragma udata msgqueue3_2 |
| //static msg MQ_FromMainToLow_2; |
| |
| #pragma udata msgqueue4_1 |
| static msg MQ_FromMainToHigh_1; |
| #pragma udata msgqueue4_2 |
| static msg MQ_FromMainToHigh_2; |
| #pragma udata msgqueue4_3 |
| static msg MQ_FromMainToHigh_3; |
| |
| #pragma udata |
| //static msg_queue MQ_ToMainFromLow; |
| static msg_queue MQ_ToMainFromHigh; |
| //static msg_queue MQ_FromMainToLow; |
| static msg_queue MQ_FromMainToHigh; |
| |
| static unsigned char MQ_Main_Willing_To_Block; |
| |
| void MQ_init_queue(msg_queue *qptr) { |
| unsigned char i; |
| |
| qptr->cur_write_index = 0; |
| qptr->cur_read_index = 0; |
| for (i=0;i<MSGQUEUELEN;i++) { |
| qptr->queue[i]->full = 0; |
| } |
| } |
| |
| void MQ_init() { |
| MQ_Main_Willing_To_Block = 0; |
| |
| // MQ_ToMainFromLow.queue[0] = &MQ_ToMainFromLow_1; |
| // MQ_ToMainFromLow.queue[1] = &MQ_ToMainFromLow_2; |
| |
| MQ_ToMainFromHigh.queue[0] = &MQ_ToMainFromHigh_1; |
| MQ_ToMainFromHigh.queue[1] = &MQ_ToMainFromHigh_2; |
| MQ_ToMainFromHigh.queue[2] = &MQ_ToMainFromHigh_3; |
| |
| // MQ_FromMainToLow.queue[0] = &MQ_FromMainToLow_1; |
| // MQ_FromMainToLow.queue[1] = &MQ_FromMainToLow_2; |
| |
| MQ_FromMainToHigh.queue[0] = &MQ_FromMainToHigh_1; |
| MQ_FromMainToHigh.queue[1] = &MQ_FromMainToHigh_2; |
| MQ_FromMainToHigh.queue[2] = &MQ_FromMainToHigh_3; |
| |
| // MQ_init_queue(&MQ_ToMainFromLow); |
| MQ_init_queue(&MQ_ToMainFromHigh); |
| // MQ_init_queue(&MQ_FromMainToLow); |
| MQ_init_queue(&MQ_FromMainToHigh); |
| } |
| |
| signed char MQ_send_msg(msg_queue *qptr,unsigned char length,unsigned char msgtype, void *data) { |
| unsigned char slot; |
| msg *qmsg; |
| size_t tlength = length; |
| |
| #ifdef DEBUG |
| if (length > MSGLEN) { |
| return(MSG_BAD_LEN); |
| } else if (length < 0) { |
| return(MSG_BAD_LEN); |
| } |
| #endif |
| |
| // Get the index in the msg queue to write to |
| slot = qptr->cur_write_index; |
| |
| // Retrieve the address of the slot in the queue to write to |
| qmsg = qptr->queue[slot]; |
| |
| // If the slot isn't empty, then we should return |
| if (qmsg->full != 0) { |
| return(MSG_QUEUE_FULL); |
| } |
| |
| // Otherwise fill in the message details |
| qmsg->length = length; |
| qmsg->msgtype = msgtype; |
| |
| // Copy the message itself |
| memcpy(qmsg->data,data,tlength); |
| |
| // Increment the next location in the queue to write to |
| // Note: the index will loop back to the start (implements a FIFO queue) |
| qptr->cur_write_index = (qptr->cur_write_index + 1) % MSGQUEUELEN; |
| |
| // Mark the slot in the queue as filled and return |
| qmsg->full = 1; |
| return(MSG_SEND_OKAY); |
| } |
| |
| signed char MQ_recv_msg(msg_queue *qptr, unsigned char maxlength, unsigned char *msgtype, void *data) { |
| unsigned char slot; |
| msg *qmsg; |
| size_t tlength; |
| |
| // Get the index in the message queue to read from |
| slot = qptr->cur_read_index; |
| |
| // Retrieve the address of the message in the queue to read from |
| qmsg = qptr->queue[slot]; |
| |
| // If the retrieved message contains data.. |
| if (qmsg->full == 1) { |
| // Check if the recieving buffer can hold the message data |
| if (qmsg->length > maxlength) { |
| return(MSG_BUFFER_TOOSMALL); |
| } |
| |
| // Get the length of the message data |
| tlength = qmsg->length; |
| |
| // Copy the message data into *data |
| memcpy(data,(const void*)qmsg->data,tlength); |
| |
| // Increment the read index to the next message to be read |
| qptr->cur_read_index = (qptr->cur_read_index + 1) % MSGQUEUELEN; |
| |
| // Copy the message type to the returned message |
| (*msgtype) = qmsg->msgtype; |
| |
| // Mark the slot in the queue as empty |
| qmsg->full = 0; |
| |
| // Return the size of the message returned |
| return (tlength); |
| |
| // Slot does not contain any messages |
| } else { |
| return(MSG_QUEUE_EMPTY); |
| } |
| } |
| |
| unsigned char MQ_peek_msg(msg_queue *qptr) { |
| unsigned char slot; |
| msg *qmsg; |
| |
| // Get the index in the message queue to read from |
| slot = qptr->cur_read_index; |
| |
| // Retrieve the address of the message in the queue to read from |
| qmsg = qptr->queue[slot]; |
| if (qmsg->full == 1) { |
| |
| // Return the message type of the first message in queue |
| return qmsg->msgtype; |
| |
| } else { |
| // If there are no messages in the queue, return 0 |
| return 0; |
| } |
| } |
| |
| // Check if there is an unread message in the queue |
| unsigned char MQ_check_msg_queue_unread(msg_queue *qptr) { |
| return (qptr->queue[qptr->cur_read_index]->full); |
| } |
| |
| void MQ_enter_sleep_mode(void) { |
| OSCCONbits.IDLEN = 1; // set to idle on sleep |
| |
| _asm |
| sleep |
| _endasm |
| } |
| |
| ///* Message Queue 1 - Low Interrupt -> Main */ |
| //signed char MQ_sendmsg_ToMainFromLow(unsigned char length, unsigned char msgtype, void *data) { |
| //#ifdef DEBUG |
| // if (!interrupt_in_low_interrupt_routine()) { |
| // return (MSG_NOT_IN_LOW); |
| // } |
| //#endif |
| // return (MQ_send_msg(&MQ_ToMainFromLow, length, msgtype, data)); |
| //} |
| // |
| //signed char MQ_recvmsg_ToMainFromLow(unsigned char maxlength, unsigned char *msgtype, void *data) { |
| //#ifdef DEBUG |
| // if (!interrupt_in_main_routine()) { |
| // return (MSG_NOT_IN_MAIN); |
| // } |
| //#endif |
| // return (MQ_recv_msg(&MQ_ToMainFromLow, maxlength, msgtype, data)); |
| //} |
| // |
| //unsigned char MQ_peek_ToMainFromLow() { |
| // return (MQ_peek_msg(&MQ_ToMainFromLow)); |
| //} |
| |
| /* Message Queue 2 - High Interrupt -> Main */ |
| signed char MQ_sendmsg_ToMainFromHigh(unsigned char length, unsigned char msgtype, void *data) { |
| #ifdef DEBUG |
| if (!interrupt_in_high_interrupt_routine()) { |
| return (MSG_NOT_IN_HIGH); |
| } |
| #endif |
| return (MQ_send_msg(&MQ_ToMainFromHigh, length, msgtype, data)); |
| } |
| |
| signed char MQ_recvmsg_ToMainFromHigh(unsigned char maxlength, unsigned char *msgtype, void *data) { |
| #ifdef DEBUG |
| if (!interrupt_in_main_routine()) { |
| return (MSG_NOT_IN_MAIN); |
| } |
| #endif |
| return (MQ_recv_msg(&MQ_ToMainFromHigh, maxlength, msgtype, data)); |
| } |
| |
| unsigned char MQ_peek_ToMainFromHigh() { |
| return (MQ_peek_msg(&MQ_ToMainFromHigh)); |
| } |
| |
| ///* Message Queue 3 - Main -> Low Interrupt */ |
| //signed char MQ_sendmsg_FromMainToLow(unsigned char length, unsigned char msgtype, void *data) { |
| //#ifdef DEBUG |
| // if (!interrupt_in_main_routine()) { |
| // return (MSG_NOT_IN_MAIN); |
| // } |
| //#endif |
| // return (MQ_send_msg(&MQ_FromMainToLow, length, msgtype, data)); |
| //} |
| // |
| //signed char MQ_recvmsg_FromMainToLow(unsigned char maxlength, unsigned char *msgtype, void *data) { |
| //#ifdef DEBUG |
| // if (!interrupt_in_low_interrupt_routine()) { |
| // return (MSG_NOT_IN_LOW); |
| // } |
| //#endif |
| // return (MQ_recv_msg(&MQ_FromMainToLow, maxlength, msgtype, data)); |
| //} |
| // |
| //unsigned char MQ_peek_FromMainToLow() { |
| // return (MQ_peek_msg(&MQ_FromMainToLow)); |
| //} |
| |
| /* Message Queue 4 - Main -> High Interrupt */ |
| signed char MQ_sendmsg_FromMainToHigh(unsigned char length, unsigned char msgtype, void *data) { |
| #ifdef DEBUG |
| if (!interrupt_in_main_routine()) { |
| return (MSG_NOT_IN_MAIN); |
| } |
| #endif |
| return (MQ_send_msg(&MQ_FromMainToHigh, length, msgtype, data)); |
| } |
| |
| signed char MQ_recvmsg_FromMainToHigh(unsigned char maxlength, unsigned char *msgtype, void *data) { |
| #ifdef DEBUG |
| if (!interrupt_in_high_interrupt_routine()) { |
| return (MSG_NOT_IN_HIGH); |
| } |
| #endif |
| return (MQ_recv_msg(&MQ_FromMainToHigh, maxlength, msgtype, data)); |
| } |
| |
| unsigned char MQ_peek_FromMainToHigh() { |
| return (MQ_peek_msg(&MQ_FromMainToHigh)); |
| } |
| |
| // This should only be called from a High Priority Interrupt |
| void MQ_sleep_high_interrupt_if_okay() { |
| // Check to see if main is willing to block |
| if (MQ_Main_Willing_To_Block == 0) { |
| return; |
| } |
| // Dont sleep if currently handling low interrupt |
| if (interrupt_in_low_interrupt_routine()) { |
| return; |
| } |
| |
| // Check to make sure that we're in high interrupt |
| if (!interrupt_in_high_interrupt_routine()) { |
| return; |
| } |
| |
| // Since we are the only thing executing that could be |
| // putting something into a message queue destined for main() |
| // we can safely check the message queues now |
| |
| // Check the message queues to make sure that they're empty |
| if (MQ_check_msg_queue_unread(&MQ_ToMainFromHigh)) { |
| return; |
| } |
| // if (MQ_check_msg_queue_unread(&MQ_ToMainFromLow)) { |
| // return; |
| // } |
| |
| // If everything checks out, go to sleep |
| MQ_enter_sleep_mode(); |
| } |
| |
| /* Called from main(), blocks until message recieved in queue to main */ |
| void MQ_wait_on_incoming_msg_queues() { |
| // Only run from the main function |
| if (!interrupt_in_main_routine()) { |
| return; |
| } |
| |
| MQ_Main_Willing_To_Block = 1; |
| while (1) { |
| // If any incoming message queues are not empty, break out of wait |
| if (MQ_check_msg_queue_unread(&MQ_ToMainFromHigh)) { |
| MQ_Main_Willing_To_Block = 0; |
| return; |
| } |
| // if (MQ_check_msg_queue_unread(&MQ_ToMainFromLow)) { |
| // MQ_Main_Willing_To_Block = 0; |
| // return; |
| // } |
| // Sleep for a period of time before checking again |
| Delay1KTCYx(10); |
| } |
| } |