0,0 → 1,333 |
#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 |
unsigned char MQ_ToMainFromLow_Data_1[MSGLEN]; |
#pragma udata msgqueue1_2 |
unsigned char MQ_ToMainFromLow_Data_2[MSGLEN]; |
|
#pragma udata msgqueue2_1 |
unsigned char MQ_ToMainFromHigh_Data_1[MSGLEN]; |
#pragma udata msgqueue2_2 |
unsigned char MQ_ToMainFromHigh_Data_2[MSGLEN]; |
|
#pragma udata msgqueue3_1 |
unsigned char MQ_FromMainToLow_Data_1[MSGLEN]; |
#pragma udata msgqueue3_2 |
unsigned char MQ_FromMainToLow_Data_2[MSGLEN]; |
|
#pragma udata msgqueue4_1 |
unsigned char MQ_FromMainToHigh_Data_1[MSGLEN]; |
#pragma udata msgqueue4_2 |
unsigned char MQ_FromMainToHigh_Data_2[MSGLEN]; |
|
#pragma udata |
msg_queue MQ_ToMainFromLow; |
msg_queue MQ_ToMainFromHigh; |
msg_queue MQ_FromMainToLow; |
msg_queue MQ_FromMainToHigh; |
|
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].data = MQ_ToMainFromLow_Data_1; |
MQ_ToMainFromLow.queue[1].data = MQ_ToMainFromLow_Data_2; |
|
MQ_ToMainFromHigh.queue[0].data = MQ_ToMainFromHigh_Data_1; |
MQ_ToMainFromHigh.queue[1].data = MQ_ToMainFromHigh_Data_2; |
|
MQ_FromMainToLow.queue[0].data = MQ_FromMainToLow_Data_1; |
MQ_FromMainToLow.queue[1].data = MQ_FromMainToLow_Data_2; |
|
MQ_FromMainToHigh.queue[0].data = MQ_FromMainToHigh_Data_1; |
MQ_FromMainToHigh.queue[1].data = MQ_FromMainToHigh_Data_2; |
|
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); |
} |
} |