void _time_delay_for ( /* [IN] the number of ticks to delay */ register MQX_TICK_STRUCT_PTR ticks ) { /* Body */ register KERNEL_DATA_STRUCT_PTR kernel_data; register TD_STRUCT_PTR td_ptr; _GET_KERNEL_DATA(kernel_data); _KLOGE2(KLOG_time_delay_for, ticks); #if MQX_CHECK_ERRORS if (ticks == NULL) { _task_set_error(MQX_INVALID_PARAMETER); _KLOGX2(KLOG_time_delay_for, MQX_INVALID_PARAMETER); return; } /* Endif */ #endif td_ptr = kernel_data->ACTIVE_PTR; _INT_DISABLE(); /* Calculate time to wake up the task */ PSP_ADD_TICKS(ticks, &kernel_data->TIME, &td_ptr->TIMEOUT); _time_delay_internal(td_ptr); _INT_ENABLE(); _KLOGX1(KLOG_time_delay_for); } /* Endbody */
_mqx_uint _lwsem_wait_for ( /* [IN] the semaphore address */ LWSEM_STRUCT_PTR sem_ptr, /* [IN] the number of ticks to delay */ MQX_TICK_STRUCT_PTR ticks ) { /* Body */ KERNEL_DATA_STRUCT_PTR kernel_data; TD_STRUCT_PTR td_ptr; _mqx_uint result; _GET_KERNEL_DATA(kernel_data); _KLOGE3(KLOG_lwsem_wait_for, sem_ptr, ticks); #if MQX_CHECK_ERRORS if (kernel_data->IN_ISR) { _KLOGX2(KLOG_lwsem_wait_for, MQX_CANNOT_CALL_FUNCTION_FROM_ISR); return(MQX_CANNOT_CALL_FUNCTION_FROM_ISR); } /* Endif */ #endif #if MQX_CHECK_VALIDITY if (sem_ptr->VALID != LWSEM_VALID) { _KLOGX2(KLOG_lwsem_wait_for, MQX_INVALID_LWSEM); return(MQX_INVALID_LWSEM); } /* Endif */ #endif _INT_DISABLE(); if (sem_ptr->VALUE <= 0) { td_ptr = kernel_data->ACTIVE_PTR; /* Calculate time to wake up the task */ PSP_ADD_TICKS(ticks, &kernel_data->TIME, &td_ptr->TIMEOUT); result = _lwsem_wait_timed_internal(sem_ptr, td_ptr); } else { --sem_ptr->VALUE; /* Start CR 788 */ result = MQX_OK; /* End CR 788 */ } /* Endif */ //#if MQX_COMPONENT_DESTRUCTION /* We must check for component destruction */ if (sem_ptr->VALID != LWSEM_VALID) { _int_enable(); /* The semaphore has been deleted */ _KLOGX2(KLOG_lwsem_wait_for, MQX_INVALID_LWSEM); return(MQX_INVALID_LWSEM); } /* Endif */ //#endif _INT_ENABLE(); _KLOGX2(KLOG_lwsem_wait_for, result); return(result); } /* Endbody */
MQX_TICK_STRUCT_PTR _time_add_sec_to_ticks ( /* [IN] The structure to add to */ MQX_TICK_STRUCT_PTR tick_ptr, /* [IN] The number of seconds to add */ _mqx_uint secs ) { /* Body */ MQX_TICK_STRUCT tmp; if (secs) { PSP_SECONDS_TO_TICKS(secs, &tmp); PSP_ADD_TICKS(tick_ptr, &tmp, tick_ptr); } /* Endif */ return tick_ptr; } /* Endbody */
/*! * \private * * \brief Writes data to the lightweight log. * * \param[in] log_number Log number of a previously created lightweight log. * \param[in] p1 Data to be written into the log entry. * \param[in] p2 Data to be written into the log entry. * \param[in] p3 Data to be written into the log entry. * \param[in] p4 Data to be written into the log entry. * \param[in] p5 Data to be written into the log entry. * \param[in] p6 Data to be written into the log entry. * \param[in] p7 Data to be written into the log entry. * * \return MQX_OK * \return LOG_FULL (Log is full and LOG_OVERWRITE is not set.) * \return LOG_DISABLED (Log is disabled.) * \return LOG_DOES_NOT_EXIST (Log_number was not created.) * \return MQX_INVALID_COMPONENT_HANDLE (Log component data is not valid.) * * \see _lwlog_write */ _mqx_uint _lwlog_write_internal ( _mqx_uint log_number, _mqx_max_type p1, _mqx_max_type p2, _mqx_max_type p3, _mqx_max_type p4, _mqx_max_type p5, _mqx_max_type p6, _mqx_max_type p7 ) { /* Body */ KERNEL_DATA_STRUCT_PTR kernel_data; LWLOG_COMPONENT_STRUCT_PTR log_component_ptr; LWLOG_HEADER_STRUCT_PTR log_header_ptr; _mqx_max_type *data_ptr; LWLOG_ENTRY_STRUCT_PTR log_ptr; #if MQX_LWLOG_TIME_STAMP_IN_TICKS == 0 TIME_STRUCT time; MQX_TICK_STRUCT ticks; #endif _GET_KERNEL_DATA(kernel_data); log_component_ptr = (LWLOG_COMPONENT_STRUCT_PTR) kernel_data->KERNEL_COMPONENTS[KERNEL_LWLOG]; log_header_ptr = log_component_ptr->LOGS[log_number]; #if MQX_CHECK_VALIDITY if (log_component_ptr->VALID != LWLOG_VALID) { return (MQX_INVALID_COMPONENT_HANDLE); } /* Endif */ #endif #if MQX_CHECK_ERRORS if (log_header_ptr == NULL) { return (LOG_DOES_NOT_EXIST); } /* Endif */ #endif if (!(log_header_ptr->FLAGS & LWLOG_ENABLED)) { return (LOG_DISABLED); } /* Endif */ log_ptr = log_header_ptr->WRITE_PTR->NEXT_PTR; if (log_header_ptr->CURRENT_ENTRIES >= log_header_ptr->MAX_ENTRIES) { if (log_header_ptr->FLAGS & LOG_OVERWRITE) { if (log_ptr == log_header_ptr->READ_PTR) { log_header_ptr->READ_PTR = log_ptr->NEXT_PTR; } /* Endif */ log_header_ptr->OLDEST_PTR = log_ptr->NEXT_PTR; } else { return (LOG_FULL); } /* Endif */ } else { log_header_ptr->CURRENT_ENTRIES++; } /* Endif */ #if MQX_LWLOG_TIME_STAMP_IN_TICKS == 0 log_ptr->MICROSECONDS = (uint32_t)_time_get_microseconds(); PSP_ADD_TICKS(&kernel_data->TIME, &kernel_data->TIME_OFFSET, &ticks); PSP_TICKS_TO_TIME(&ticks, &time); log_ptr->SECONDS = time.SECONDS; log_ptr->MILLISECONDS = time.MILLISECONDS; #else log_ptr->TIMESTAMP = kernel_data->TIME; log_ptr->TIMESTAMP.HW_TICKS = _time_get_hwticks(); PSP_ADD_TICKS(&log_ptr->TIMESTAMP, &kernel_data->TIME_OFFSET, &log_ptr->TIMESTAMP); #endif log_ptr->SEQUENCE_NUMBER = log_header_ptr->NUMBER++; data_ptr = &log_ptr->DATA[0]; *data_ptr++ = p1; *data_ptr++ = p2; *data_ptr++ = p3; *data_ptr++ = p4; *data_ptr++ = p5; *data_ptr++ = p6; *data_ptr = p7; log_header_ptr->WRITE_PTR = log_ptr; return (MQX_OK); } /* Endbody */
_mqx_uint _lwmsgq_receive ( /* Handle to the queue */ pointer handle, /* location of message to copy to */ _mqx_max_type_ptr message, /* flags for blocking on empty */ _mqx_uint flags, /* Timeout for receive if using ticks if 0, ignored */ _mqx_uint ticks, /* Timeout if receive timout using tick struct must have flags set */ MQX_TICK_STRUCT_PTR tick_ptr ) {/* Body */ KERNEL_DATA_STRUCT_PTR kernel_data; TD_STRUCT_PTR td_ptr; LWMSGQ_STRUCT_PTR q_ptr = (LWMSGQ_STRUCT_PTR)handle; _mqx_uint i; _mqx_max_type_ptr from_ptr; _mqx_max_type_ptr to_ptr; #if MQX_ENABLE_USER_MODE && MQX_ENABLE_USER_STDAPI if (MQX_RUN_IN_USER_MODE) { return _usr_lwmsgq_receive(handle, message, flags, ticks, tick_ptr); } #endif /* Start CR 1944 */ _GET_KERNEL_DATA(kernel_data); _KLOGE6(KLOG_lwmsgq_receive, handle, message, flags, ticks, tick_ptr); /* End CR 1944 */ _int_disable(); #if MQX_CHECK_VALIDITY if (q_ptr->VALID != LWMSGQ_VALID){ _int_enable(); /* Start CR 1944 */ _KLOGX2(KLOG_lwmsgq_send, LWMSGQ_INVALID); /* End CR 1944 */ return LWMSGQ_INVALID; } /* Endif */ #endif if (LWMSGQ_IS_EMPTY(q_ptr)) { if (flags & LWMSGQ_RECEIVE_BLOCK_ON_EMPTY) { td_ptr = kernel_data->ACTIVE_PTR; while (LWMSGQ_IS_EMPTY(q_ptr)) { td_ptr->STATE = LWMSGQ_READ_BLOCKED; td_ptr->INFO = (_mqx_uint)&q_ptr->WAITING_READERS; _QUEUE_UNLINK(td_ptr); _QUEUE_ENQUEUE(&q_ptr->WAITING_READERS, &td_ptr->AUX_QUEUE); if (ticks || (flags & (LWMSGQ_TIMEOUT_UNTIL | LWMSGQ_TIMEOUT_FOR))){ if (ticks) { PSP_ADD_TICKS_TO_TICK_STRUCT(&kernel_data->TIME, ticks, &td_ptr->TIMEOUT); } else if (flags & LWMSGQ_TIMEOUT_UNTIL){ td_ptr->TIMEOUT = *tick_ptr; } else { PSP_ADD_TICKS(tick_ptr, &kernel_data->TIME, &td_ptr->TIMEOUT); } /* Endif */ _time_delay_internal(td_ptr); if (td_ptr->INFO != 0) { _int_enable(); /* Start CR 1944 */ _KLOGX2(KLOG_lwmsgq_receive, LWMSGQ_TIMEOUT); /* End CR 1944 */ return LWMSGQ_TIMEOUT; } /* Endif */ } else { _sched_execute_scheduler_internal(); /* Let other tasks run */ } /* Endif */ } /* Endwhile */ } else { _int_enable(); /* Start CR 1944 */ _KLOGX2(KLOG_lwmsgq_receive, LWMSGQ_EMPTY); /* End CR 1944 */ return LWMSGQ_EMPTY; } /* Endif */ }/* Endif */ from_ptr = q_ptr->MSG_READ_LOC; to_ptr = message; i = q_ptr->MSG_SIZE+1; while (--i) { *to_ptr++ = *from_ptr++; } /* Endwhile */ q_ptr->MSG_READ_LOC += q_ptr->MSG_SIZE; if (q_ptr->MSG_READ_LOC >= q_ptr->MSG_END_LOC) { q_ptr->MSG_READ_LOC = q_ptr->MSG_START_LOC; } /* Endif */ q_ptr->CURRENT_SIZE--; if (! _QUEUE_IS_EMPTY(&q_ptr->WAITING_WRITERS)) { _QUEUE_DEQUEUE(&q_ptr->WAITING_WRITERS, td_ptr); _BACKUP_POINTER(td_ptr, TD_STRUCT, AUX_QUEUE); td_ptr->INFO = 0; /* Signal that post is activating the task */ _TASK_READY(td_ptr, kernel_data); _CHECK_RUN_SCHEDULER(); /* Let higher priority task run */ } /* Endif */ _int_enable(); /* Start CR 1944 */ _KLOGX2(KLOG_lwmsgq_receive, MQX_OK); /* End CR 1944 */ return MQX_OK; }/* Endbody */
/*! * \brief Writes data to the log. * * The function writes the log entry only if it returns MQX_OK. * * \param[in] log_number Log number of a previously created log. * \param[in] number_of_parameters Number of parameters to write. * * \return MQX_OK * \return MQX_COMPONENT_DOES_NOT_EXIST (Log component is not created.) * \return MQX_INVALID_COMPONENT_HANDLE (Log component data is not valid.) * \return LOG_INVALID (Log_number is out of range.) * \return LOG_DOES_NOT_EXIST (Log_number was not created.) * \return LOG_DISABLED (Log is disabled.) * \return LOG_FULL (Log is full and LOG_OVERWRITE is not set.) */ _mqx_uint _log_write ( _mqx_uint log_number, _mqx_uint number_of_parameters, ... ) { /* Body */ MQX_TICK_STRUCT ticks; TIME_STRUCT time; KERNEL_DATA_STRUCT_PTR kernel_data; LOG_COMPONENT_STRUCT_PTR log_component_ptr; LOG_HEADER_STRUCT_PTR log_header_ptr; _mqx_uint_ptr data_ptr; va_list param_ptr; _mqx_uint us; _mqx_uint i; _mqx_uint size; _mqx_uint old_size; _GET_KERNEL_DATA(kernel_data); #if MQX_CHECK_ERRORS if (kernel_data->KERNEL_COMPONENTS[KERNEL_LOG] == NULL) { return(MQX_COMPONENT_DOES_NOT_EXIST); } /* Endif */ #endif log_component_ptr = (LOG_COMPONENT_STRUCT_PTR) kernel_data->KERNEL_COMPONENTS[KERNEL_LOG]; #if MQX_CHECK_VALIDITY if (log_component_ptr->VALID != LOG_VALID) { return(MQX_INVALID_COMPONENT_HANDLE); } /* Endif */ #endif #if MQX_CHECK_ERRORS if (log_number >= LOG_MAXIMUM_NUMBER) { return(LOG_INVALID); } /* Endif */ #endif log_header_ptr = log_component_ptr->LOGS[log_number]; #if MQX_CHECK_ERRORS if (log_header_ptr == NULL) { return(LOG_DOES_NOT_EXIST); } /* Endif */ #endif if (! (log_header_ptr->FLAGS & LOG_ENABLED)) { return(LOG_DISABLED); } /* Endif */ size = sizeof(LOG_ENTRY_STRUCT)/sizeof(_mqx_uint) + number_of_parameters; #if MQX_CHECK_ERRORS if (size > log_header_ptr->MAX) { /* * The request is to log an entry larger than the * entire log. Reject this. */ return(LOG_FULL); } /* Endif */ #endif va_start(param_ptr, number_of_parameters); i = number_of_parameters + 1; _INT_DISABLE(); if ((log_header_ptr->SIZE + size) > log_header_ptr->MAX) { if (log_header_ptr->FLAGS & LOG_OVERWRITE) { /* Remove leading elements until it can fit */ while ((log_header_ptr->SIZE + size) > log_header_ptr->MAX) { data_ptr = log_header_ptr->LOG_READ; old_size = *data_ptr; log_header_ptr->SIZE -= old_size; data_ptr += old_size; if (data_ptr >= log_header_ptr->LOG_END) { data_ptr = log_header_ptr->LOG_START + (data_ptr - log_header_ptr->LOG_END); } /* Endif */ if (log_header_ptr->LOG_NEXT == log_header_ptr->LOG_READ) { /* Move up the next read pointer */ log_header_ptr->LOG_NEXT = data_ptr; } /* Endif */ log_header_ptr->LOG_READ = data_ptr; } /* Endwhile */ } else { _INT_ENABLE(); va_end(param_ptr); return(LOG_FULL); } /* Endif */ } /* Endif */ log_header_ptr->SIZE += size; data_ptr = log_header_ptr->LOG_WRITE; log_header_ptr->LAST_LOG = data_ptr; *data_ptr++ = size; if (data_ptr == log_header_ptr->LOG_END) { data_ptr = log_header_ptr->LOG_START; } /* Endif */ *data_ptr++ = log_header_ptr->NUMBER++; if (data_ptr == log_header_ptr->LOG_END) { data_ptr = log_header_ptr->LOG_START; } /* Endif */ us = (_mqx_uint)_time_get_microseconds(); PSP_ADD_TICKS(&kernel_data->TIME, &kernel_data->TIME_OFFSET, &ticks); PSP_TICKS_TO_TIME(&ticks, &time); #if MQX_INT_SIZE_IN_BITS >= 32 *data_ptr++ = (_mqx_uint)time.SECONDS; #else #if PSP_ENDIAN == MQX_BIG_ENDIAN *data_ptr++ = (_mqx_uint)((time.SECONDS >> MQX_INT_SIZE_IN_BITS) & MAX_MQX_UINT); if (data_ptr == log_header_ptr->LOG_END) { data_ptr = log_header_ptr->LOG_START; } /* Endif */ *data_ptr++ = (_mqx_uint)(time.SECONDS & MAX_MQX_UINT); #else *data_ptr++ = (_mqx_uint)(time.SECONDS & MAX_MQX_UINT); if (data_ptr == log_header_ptr->LOG_END) { data_ptr = log_header_ptr->LOG_START; } /* Endif */ *data_ptr++ = (_mqx_uint)((time.SECONDS >> MQX_INT_SIZE_IN_BITS) & MAX_MQX_UINT); #endif #endif if (data_ptr >= log_header_ptr->LOG_END) { data_ptr = log_header_ptr->LOG_START; } /* Endif */ #if PSP_MEMORY_ADDRESSING_CAPABILITY >= 32 *data_ptr++ = time.MILLISECONDS; if (data_ptr == log_header_ptr->LOG_END) { data_ptr = log_header_ptr->LOG_START; } /* Endif */ *data_ptr++ = us; #elif MQX_INT_SIZE_IN_BITS == 32 #if PSP_ENDIAN == MQX_LITTLE_ENDIAN *data_ptr++ = time.MILLISECONDS | (us << 16); #else *data_ptr++ = us | (time.MILLISECONDS << 16); #endif #else *data_ptr++ = (_mqx_uint)time.MILLISECONDS; if (data_ptr == log_header_ptr->LOG_END) { data_ptr = log_header_ptr->LOG_START; } /* Endif */ *data_ptr++ = us; #endif if (data_ptr >= log_header_ptr->LOG_END) { data_ptr = log_header_ptr->LOG_START; } /* Endif */ while (--i) { *data_ptr++ = (_mqx_uint)va_arg(param_ptr, _mqx_uint); if (data_ptr == log_header_ptr->LOG_END) { data_ptr = log_header_ptr->LOG_START; } /* Endif */ } /* Endwhile */ log_header_ptr->LOG_WRITE = data_ptr; _INT_ENABLE(); va_end(param_ptr); return(MQX_OK); } /* Endbody */
boolean _time_xdate_to_ticks ( /* [IN] pointer to date and time structure */ MQX_XDATE_STRUCT_PTR xdate_ptr, /* [OUT] pointer to structure where num of ticks is to be written */ MQX_TICK_STRUCT_PTR tick_ptr ) { /* Body */ uint_32 time; _mqx_uint tmp; MQX_TICK_STRUCT t2; KERNEL_DATA_STRUCT_PTR kernel_data; #if MQX_CHECK_ERRORS if ((xdate_ptr == NULL) || (tick_ptr == NULL)) { return FALSE; } /* Endif */ #endif if (!_time_normalize_xdate(xdate_ptr)) { return FALSE; } /* Endif */ /* ** Determine the number of days since Jan 1, 1970 at 00:00:00 */ time = _time_days_before_year_internal[(xdate_ptr->YEAR - (uint_16)XCLK_FIRST_YEAR)]; tmp = (_mqx_uint)_time_check_if_leap(xdate_ptr->YEAR); /* ** Add the number of days since 0000 hours, Jan 1, to the first ** day of month. */ time += _time_days_before_month_internal[tmp][xdate_ptr->MONTH-1]; /* ** Add the number of days since the beginning of the ** month */ time += (uint_32)(xdate_ptr->MDAY - 1); /* Convert everything so far into hours */ time = time * HOURS_IN_DAY + xdate_ptr->HOUR; /* Convert to seconds */ tick_ptr->HW_TICKS = 0; t2 = _mqx_zero_tick_struct; PSP_ADD_TICKS_TO_TICK_STRUCT(&t2, SECS_IN_HOUR, &t2); PSP_MULTIPLY_TICKS_BY_UINT_32(&t2, time, tick_ptr); /* Add in seconds and minutes from XDATE struct */ tmp = xdate_ptr->SEC + xdate_ptr->MIN * SECS_IN_MINUTE; PSP_ADD_TICKS_TO_TICK_STRUCT(tick_ptr, tmp, tick_ptr); /* Convert everything into ticks */ _GET_KERNEL_DATA(kernel_data); time = (uint_32)kernel_data->TICKS_PER_SECOND; PSP_MULTIPLY_TICKS_BY_UINT_32(tick_ptr, time, tick_ptr); if (xdate_ptr->MSEC) { PSP_MILLISECONDS_TO_TICKS(xdate_ptr->MSEC, &t2); PSP_ADD_TICKS(tick_ptr, &t2, tick_ptr); } /* Endif */ if (xdate_ptr->USEC) { PSP_MICROSECONDS_TO_TICKS(xdate_ptr->USEC, &t2); PSP_ADD_TICKS(tick_ptr, &t2, tick_ptr); } /* Endif */ if (xdate_ptr->NSEC) { PSP_NANOSECONDS_TO_TICKS(xdate_ptr->NSEC, &t2); PSP_ADD_TICKS(tick_ptr, &t2, tick_ptr); } /* Endif */ if (xdate_ptr->PSEC) { PSP_PICOSECONDS_TO_TICKS(xdate_ptr->PSEC, &t2); PSP_ADD_TICKS(tick_ptr, &t2, tick_ptr); } /* Endif */ return TRUE; } /* Endbody */