Subversion Repositories Code-Repo

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