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

    }

}