diff --git a/Marlin/src/HAL/HAL_LPC1768/DebugMonitor_LPC1768.cpp b/Marlin/src/HAL/HAL_LPC1768/DebugMonitor_LPC1768.cpp
new file mode 100644
index 0000000000..6798d60cc6
--- /dev/null
+++ b/Marlin/src/HAL/HAL_LPC1768/DebugMonitor_LPC1768.cpp
@@ -0,0 +1,318 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ *
+ */
+
+#ifdef TARGET_LPC1768
+
+#include "../../core/macros.h"
+#include "../../core/serial.h"
+#include
+
+#include "../shared/backtrace/unwinder.h"
+#include "../shared/backtrace/unwmemaccess.h"
+#include "watchdog.h"
+#include
+
+
+// Debug monitor that dumps to the Programming port all status when
+// an exception or WDT timeout happens - And then resets the board
+
+// All the Monitor routines must run with interrupts disabled and
+// under an ISR execution context. That is why we cannot reuse the
+// Serial interrupt routines or any C runtime, as we don't know the
+// state we are when running them
+
+// A SW memory barrier, to ensure GCC does not overoptimize loops
+#define sw_barrier() __asm__ volatile("": : :"memory");
+
+// (re)initialize UART0 as a monitor output to 250000,n,8,1
+static void TXBegin(void) {
+}
+
+// Send character through UART with no interrupts
+static void TX(char c) {
+ _DBC(c);
+}
+
+// Send String through UART
+static void TX(const char* s) {
+ while (*s) TX(*s++);
+}
+
+static void TXDigit(uint32_t d) {
+ if (d < 10) TX((char)(d+'0'));
+ else if (d < 16) TX((char)(d+'A'-10));
+ else TX('?');
+}
+
+// Send Hex number thru UART
+static void TXHex(uint32_t v) {
+ TX("0x");
+ for (uint8_t i = 0; i < 8; i++, v <<= 4)
+ TXDigit((v >> 28) & 0xF);
+}
+
+// Send Decimal number thru UART
+static void TXDec(uint32_t v) {
+ if (!v) {
+ TX('0');
+ return;
+ }
+
+ char nbrs[14];
+ char *p = &nbrs[0];
+ while (v != 0) {
+ *p++ = '0' + (v % 10);
+ v /= 10;
+ }
+ do {
+ p--;
+ TX(*p);
+ } while (p != &nbrs[0]);
+}
+
+// Dump a backtrace entry
+static bool UnwReportOut(void* ctx, const UnwReport* bte) {
+ int* p = (int*)ctx;
+
+ (*p)++;
+ TX('#'); TXDec(*p); TX(" : ");
+ TX(bte->name?bte->name:"unknown"); TX('@'); TXHex(bte->function);
+ TX('+'); TXDec(bte->address - bte->function);
+ TX(" PC:");TXHex(bte->address); TX('\n');
+ return true;
+}
+
+#ifdef UNW_DEBUG
+ void UnwPrintf(const char* format, ...) {
+ char dest[256];
+ va_list argptr;
+ va_start(argptr, format);
+ vsprintf(dest, format, argptr);
+ va_end(argptr);
+ TX(&dest[0]);
+ }
+#endif
+
+/* Table of function pointers for passing to the unwinder */
+static const UnwindCallbacks UnwCallbacks = {
+ UnwReportOut,
+ UnwReadW,
+ UnwReadH,
+ UnwReadB
+ #if defined(UNW_DEBUG)
+ ,UnwPrintf
+ #endif
+};
+
+
+/**
+ * HardFaultHandler_C:
+ * This is called from the HardFault_HandlerAsm with a pointer the Fault stack
+ * as the parameter. We can then read the values from the stack and place them
+ * into local variables for ease of reading.
+ * We then read the various Fault Status and Address Registers to help decode
+ * cause of the fault.
+ * The function ends with a BKPT instruction to force control back into the debugger
+ */
+extern "C"
+void HardFault_HandlerC(unsigned long *sp, unsigned long lr, unsigned long cause) {
+
+ static const char* causestr[] = {
+ "NMI","Hard","Mem","Bus","Usage","Debug","WDT","RSTC"
+ };
+
+ UnwindFrame btf;
+
+ // Dump report to the Programming port (interrupts are DISABLED)
+ TXBegin();
+ TX("\n\n## Software Fault detected ##\n");
+ TX("Cause: "); TX(causestr[cause]); TX('\n');
+
+ TX("R0 : "); TXHex(((unsigned long)sp[0])); TX('\n');
+ TX("R1 : "); TXHex(((unsigned long)sp[1])); TX('\n');
+ TX("R2 : "); TXHex(((unsigned long)sp[2])); TX('\n');
+ TX("R3 : "); TXHex(((unsigned long)sp[3])); TX('\n');
+ TX("R12 : "); TXHex(((unsigned long)sp[4])); TX('\n');
+ TX("LR : "); TXHex(((unsigned long)sp[5])); TX('\n');
+ TX("PC : "); TXHex(((unsigned long)sp[6])); TX('\n');
+ TX("PSR : "); TXHex(((unsigned long)sp[7])); TX('\n');
+
+ // Configurable Fault Status Register
+ // Consists of MMSR, BFSR and UFSR
+ TX("CFSR : "); TXHex((*((volatile unsigned long *)(0xE000ED28)))); TX('\n');
+
+ // Hard Fault Status Register
+ TX("HFSR : "); TXHex((*((volatile unsigned long *)(0xE000ED2C)))); TX('\n');
+
+ // Debug Fault Status Register
+ TX("DFSR : "); TXHex((*((volatile unsigned long *)(0xE000ED30)))); TX('\n');
+
+ // Auxiliary Fault Status Register
+ TX("AFSR : "); TXHex((*((volatile unsigned long *)(0xE000ED3C)))); TX('\n');
+
+ // Read the Fault Address Registers. These may not contain valid values.
+ // Check BFARVALID/MMARVALID to see if they are valid values
+ // MemManage Fault Address Register
+ TX("MMAR : "); TXHex((*((volatile unsigned long *)(0xE000ED34)))); TX('\n');
+
+ // Bus Fault Address Register
+ TX("BFAR : "); TXHex((*((volatile unsigned long *)(0xE000ED38)))); TX('\n');
+
+ TX("ExcLR: "); TXHex(lr); TX('\n');
+ TX("ExcSP: "); TXHex((unsigned long)sp); TX('\n');
+
+ btf.sp = ((unsigned long)sp) + 8*4; // The original stack pointer
+ btf.fp = btf.sp;
+ btf.lr = ((unsigned long)sp[5]);
+ btf.pc = ((unsigned long)sp[6]) | 1; // Force Thumb, as CORTEX only support it
+
+ // Perform a backtrace
+ TX("\nBacktrace:\n\n");
+ int ctr = 0;
+ UnwindStart(&btf, &UnwCallbacks, &ctr);
+
+ // Disable all NVIC interrupts
+ NVIC->ICER[0] = 0xFFFFFFFF;
+ NVIC->ICER[1] = 0xFFFFFFFF;
+
+ // Relocate VTOR table to default position
+ SCB->VTOR = 0;
+
+ // Clear cause of reset to prevent entering smoothie bootstrap
+ HAL_clear_reset_source();
+ // Restart watchdog
+ //WDT_Restart(WDT);
+ watchdog_init();
+
+ // Reset controller
+ NVIC_SystemReset();
+
+ while(1) { watchdog_init(); }
+}
+
+extern "C" {
+__attribute__((naked)) void NMI_Handler(void) {
+ __asm__ __volatile__ (
+ ".syntax unified" "\n\t"
+ A("tst lr, #4")
+ A("ite eq")
+ A("mrseq r0, msp")
+ A("mrsne r0, psp")
+ A("mov r1,lr")
+ A("mov r2,#0")
+ A("b HardFault_HandlerC")
+ );
+}
+
+__attribute__((naked)) void HardFault_Handler(void) {
+ __asm__ __volatile__ (
+ ".syntax unified" "\n\t"
+ A("tst lr, #4")
+ A("ite eq")
+ A("mrseq r0, msp")
+ A("mrsne r0, psp")
+ A("mov r1,lr")
+ A("mov r2,#1")
+ A("b HardFault_HandlerC")
+ );
+}
+
+__attribute__((naked)) void MemManage_Handler(void) {
+ __asm__ __volatile__ (
+ ".syntax unified" "\n\t"
+ A("tst lr, #4")
+ A("ite eq")
+ A("mrseq r0, msp")
+ A("mrsne r0, psp")
+ A("mov r1,lr")
+ A("mov r2,#2")
+ A("b HardFault_HandlerC")
+ );
+}
+
+__attribute__((naked)) void BusFault_Handler(void) {
+ __asm__ __volatile__ (
+ ".syntax unified" "\n\t"
+ A("tst lr, #4")
+ A("ite eq")
+ A("mrseq r0, msp")
+ A("mrsne r0, psp")
+ A("mov r1,lr")
+ A("mov r2,#3")
+ A("b HardFault_HandlerC")
+ );
+}
+
+__attribute__((naked)) void UsageFault_Handler(void) {
+ __asm__ __volatile__ (
+ ".syntax unified" "\n\t"
+ A("tst lr, #4")
+ A("ite eq")
+ A("mrseq r0, msp")
+ A("mrsne r0, psp")
+ A("mov r1,lr")
+ A("mov r2,#4")
+ A("b HardFault_HandlerC")
+ );
+}
+
+__attribute__((naked)) void DebugMon_Handler(void) {
+ __asm__ __volatile__ (
+ ".syntax unified" "\n\t"
+ A("tst lr, #4")
+ A("ite eq")
+ A("mrseq r0, msp")
+ A("mrsne r0, psp")
+ A("mov r1,lr")
+ A("mov r2,#5")
+ A("b HardFault_HandlerC")
+ );
+}
+
+/* This is NOT an exception, it is an interrupt handler - Nevertheless, the framing is the same */
+__attribute__((naked)) void WDT_IRQHandler(void) {
+ __asm__ __volatile__ (
+ ".syntax unified" "\n\t"
+ A("tst lr, #4")
+ A("ite eq")
+ A("mrseq r0, msp")
+ A("mrsne r0, psp")
+ A("mov r1,lr")
+ A("mov r2,#6")
+ A("b HardFault_HandlerC")
+ );
+}
+
+__attribute__((naked)) void RSTC_Handler(void) {
+ __asm__ __volatile__ (
+ ".syntax unified" "\n\t"
+ A("tst lr, #4")
+ A("ite eq")
+ A("mrseq r0, msp")
+ A("mrsne r0, psp")
+ A("mov r1,lr")
+ A("mov r2,#7")
+ A("b HardFault_HandlerC")
+ );
+}
+}
+#endif // TARGET_LPC1768
diff --git a/Marlin/src/HAL/HAL_LPC1768/HAL.cpp b/Marlin/src/HAL/HAL_LPC1768/HAL.cpp
index 33cd9a9995..020a1660b3 100644
--- a/Marlin/src/HAL/HAL_LPC1768/HAL.cpp
+++ b/Marlin/src/HAL/HAL_LPC1768/HAL.cpp
@@ -22,8 +22,7 @@
#include "../../inc/MarlinConfig.h"
#include "../shared/Delay.h"
-
-HalSerial usb_serial;
+#include "../../../gcode/parser.h"
// U8glib required functions
extern "C" void u8g_xMicroDelay(uint16_t val) {
@@ -51,231 +50,11 @@ int freeMemory() {
return result;
}
-// --------------------------------------------------------------------------
-// ADC
-// --------------------------------------------------------------------------
-
-#define ADC_DONE 0x80000000
-#define ADC_OVERRUN 0x40000000
-
-void HAL_adc_init(void) {
- LPC_SC->PCONP |= (1 << 12); // Enable CLOCK for internal ADC controller
- LPC_SC->PCLKSEL0 &= ~(0x3 << 24);
- LPC_SC->PCLKSEL0 |= (0x1 << 24); // 0: 25MHz, 1: 100MHz, 2: 50MHz, 3: 12.5MHZ to ADC clock divider
- LPC_ADC->ADCR = (0 << 0) // SEL: 0 = no channels selected
- | (0xFF << 8) // select slowest clock for A/D conversion 150 - 190 uS for a complete conversion
- | (0 << 16) // BURST: 0 = software control
- | (0 << 17) // CLKS: not applicable
- | (1 << 21) // PDN: 1 = operational
- | (0 << 24) // START: 0 = no start
- | (0 << 27); // EDGE: not applicable
-}
-
-// externals need to make the call to KILL compile
-#include "../../core/language.h"
-
-extern void kill(PGM_P);
-
-void HAL_adc_enable_channel(int ch) {
- pin_t pin = analogInputToDigitalPin(ch);
-
- if (pin == -1) {
- serial_error_start();
- SERIAL_PRINTF("INVALID ANALOG PORT:%d\n", ch);
- kill(MSG_KILLED);
- }
-
- int8_t pin_port = LPC1768_PIN_PORT(pin),
- pin_port_pin = LPC1768_PIN_PIN(pin),
- pinsel_start_bit = pin_port_pin > 15 ? 2 * (pin_port_pin - 16) : 2 * pin_port_pin;
- uint8_t pin_sel_register = (pin_port == 0 && pin_port_pin <= 15) ? 0 :
- pin_port == 0 ? 1 :
- pin_port == 1 ? 3 : 10;
-
- switch (pin_sel_register) {
- case 1:
- LPC_PINCON->PINSEL1 &= ~(0x3 << pinsel_start_bit);
- LPC_PINCON->PINSEL1 |= (0x1 << pinsel_start_bit);
- break;
- case 3:
- LPC_PINCON->PINSEL3 &= ~(0x3 << pinsel_start_bit);
- LPC_PINCON->PINSEL3 |= (0x3 << pinsel_start_bit);
- break;
- case 0:
- LPC_PINCON->PINSEL0 &= ~(0x3 << pinsel_start_bit);
- LPC_PINCON->PINSEL0 |= (0x2 << pinsel_start_bit);
- break;
- };
-}
-
-void HAL_adc_start_conversion(const uint8_t ch) {
- if (analogInputToDigitalPin(ch) == -1) {
- SERIAL_PRINTF("HAL: HAL_adc_start_conversion: invalid channel %d\n", ch);
- return;
- }
-
- LPC_ADC->ADCR &= ~0xFF; // Reset
- SBI(LPC_ADC->ADCR, ch); // Select Channel
- SBI(LPC_ADC->ADCR, 24); // Start conversion
-}
-
-bool HAL_adc_finished(void) {
- return LPC_ADC->ADGDR & ADC_DONE;
-}
-
-// possible config options if something similar is extended to more platforms.
-#define ADC_USE_MEDIAN_FILTER // Filter out erroneous readings
-#define ADC_MEDIAN_FILTER_SIZE 23 // Higher values increase step delay (phase shift),
- // (ADC_MEDIAN_FILTER_SIZE + 1) / 2 sample step delay (12 samples @ 500Hz: 24ms phase shift)
- // Memory usage per ADC channel (bytes): (6 * ADC_MEDIAN_FILTER_SIZE) + 16
- // 8 * ((6 * 23) + 16 ) = 1232 Bytes for 8 channels
-
-#define ADC_USE_LOWPASS_FILTER // Filter out high frequency noise
-#define ADC_LOWPASS_K_VALUE 6 // Higher values increase rise time
- // Rise time sample delays for 100% signal convergence on full range step
- // (1 : 13, 2 : 32, 3 : 67, 4 : 139, 5 : 281, 6 : 565, 7 : 1135, 8 : 2273)
- // K = 6, 565 samples, 500Hz sample rate, 1.13s convergence on full range step
- // Memory usage per ADC channel (bytes): 4 (32 Bytes for 8 channels)
-
-
-// Sourced from https://embeddedgurus.com/stack-overflow/tag/median-filter/
-struct MedianFilter {
- #define STOPPER 0 // Smaller than any datum
- struct Pair {
- Pair *point; // Pointers forming list linked in sorted order
- uint16_t value; // Values to sort
- };
-
- Pair buffer[ADC_MEDIAN_FILTER_SIZE] = {}; // Buffer of nwidth pairs
- Pair *datpoint = buffer; // Pointer into circular buffer of data
- Pair small = {NULL, STOPPER}; // Chain stopper
- Pair big = {&small, 0}; // Pointer to head (largest) of linked list.
-
- uint16_t update(uint16_t datum) {
- Pair *successor; // Pointer to successor of replaced data item
- Pair *scan; // Pointer used to scan down the sorted list
- Pair *scanold; // Previous value of scan
- Pair *median; // Pointer to median
- uint16_t i;
-
- if (datum == STOPPER) {
- datum = STOPPER + 1; // No stoppers allowed.
- }
-
- if ( (++datpoint - buffer) >= (ADC_MEDIAN_FILTER_SIZE)) {
- datpoint = buffer; // Increment and wrap data in pointer.
- }
-
- datpoint->value = datum; // Copy in new datum
- successor = datpoint->point; // Save pointer to old value's successor
- median = &big; // Median initially to first in chain
- scanold = NULL; // Scanold initially null.
- scan = &big; // Points to pointer to first (largest) datum in chain
-
- // Handle chain-out of first item in chain as special case
- if (scan->point == datpoint) {
- scan->point = successor;
- }
- scanold = scan; // Save this pointer and
- scan = scan->point ; // step down chain
-
- // Loop through the chain, normal loop exit via break.
- for (i = 0 ; i < ADC_MEDIAN_FILTER_SIZE; ++i) {
- // Handle odd-numbered item in chain
- if (scan->point == datpoint) {
- scan->point = successor; // Chain out the old datum
- }
-
- if (scan->value < datum) { // If datum is larger than scanned value
- datpoint->point = scanold->point; // Chain it in here
- scanold->point = datpoint; // Mark it chained in
- datum = STOPPER;
- }
-
- // Step median pointer down chain after doing odd-numbered element
- median = median->point; // Step median pointer
- if (scan == &small) {
- break; // Break at end of chain
- }
- scanold = scan; // Save this pointer and
- scan = scan->point; // step down chain
-
- // Handle even-numbered item in chain.
- if (scan->point == datpoint) {
- scan->point = successor;
- }
-
- if (scan->value < datum) {
- datpoint->point = scanold->point;
- scanold->point = datpoint;
- datum = STOPPER;
- }
-
- if (scan == &small) {
- break;
- }
-
- scanold = scan;
- scan = scan->point;
- }
- return median->value;
- }
-};
-
-struct LowpassFilter {
- uint32_t data_delay = 0;
- uint16_t update(const uint16_t value) {
- data_delay -= (data_delay >> (ADC_LOWPASS_K_VALUE)) - value;
- return (uint16_t)(data_delay >> (ADC_LOWPASS_K_VALUE));
- }
-};
-
-uint16_t HAL_adc_get_result(void) {
- uint32_t adgdr = LPC_ADC->ADGDR;
- CBI(LPC_ADC->ADCR, 24); // Stop conversion
-
- if (adgdr & ADC_OVERRUN) return 0;
- uint16_t data = (adgdr >> 4) & 0xFFF; // copy the 12bit data value
- uint8_t adc_channel = (adgdr >> 24) & 0x7; // copy the 3bit used channel
-
- #ifdef ADC_USE_MEDIAN_FILTER
- static MedianFilter median_filter[NUM_ANALOG_INPUTS];
- data = median_filter[adc_channel].update(data);
- #endif
-
- #ifdef ADC_USE_LOWPASS_FILTER
- static LowpassFilter lowpass_filter[NUM_ANALOG_INPUTS];
- data = lowpass_filter[adc_channel].update(data);
- #endif
-
- return ((data >> 2) & 0x3FF); // return 10bit value as Marlin expects
-}
-
-#define SBIT_CNTEN 0
-#define SBIT_PWMEN 2
-#define SBIT_PWMMR0R 1
-
-#define PWM_1 0 //P2_00 (0-1 Bits of PINSEL4)
-#define PWM_2 2 //P2_01 (2-3 Bits of PINSEL4)
-#define PWM_3 4 //P2_02 (4-5 Bits of PINSEL4)
-#define PWM_4 6 //P2_03 (6-7 Bits of PINSEL4)
-#define PWM_5 8 //P2_04 (8-9 Bits of PINSEL4)
-#define PWM_6 10 //P2_05 (10-11 Bits of PINSEL4)
-
-void HAL_pwm_init(void) {
- LPC_PINCON->PINSEL4 = _BV(PWM_5) | _BV(PWM_6);
-
- LPC_PWM1->TCR = _BV(SBIT_CNTEN) | _BV(SBIT_PWMEN);
- LPC_PWM1->PR = 0x0; // No prescalar
- LPC_PWM1->MCR = _BV(SBIT_PWMMR0R); // Reset on PWMMR0, reset TC if it matches MR0
- LPC_PWM1->MR0 = 255; // set PWM cycle(Ton+Toff)=255)
- LPC_PWM1->MR5 = 0; // Set 50% Duty Cycle for the channels
- LPC_PWM1->MR6 = 0;
-
- // Trigger the latch Enable Bits to load the new Match Values MR0, MR5, MR6
- LPC_PWM1->LER = _BV(0) | _BV(5) | _BV(6);
- // Enable the PWM output pins for PWM_5-PWM_6(P2_04 - P2_05)
- LPC_PWM1->PCR = _BV(13) | _BV(14);
+int16_t PARSED_PIN_INDEX(const char code, const int16_t dval) {
+ const uint16_t val = (uint16_t)parser.intval(code), port = val / 100, pin = val % 100;
+ const int16_t ind = (port < (NUM_DIGITAL_PINS >> 5) && (pin < 32))
+ ? GET_PIN_MAP_INDEX(port << 5 | pin) : -2;
+ return ind > -2 ? ind : dval;
}
#endif // TARGET_LPC1768
diff --git a/Marlin/src/HAL/HAL_LPC1768/HAL.h b/Marlin/src/HAL/HAL_LPC1768/HAL.h
index 1447b45d6e..5eda2ac72b 100644
--- a/Marlin/src/HAL/HAL_LPC1768/HAL.h
+++ b/Marlin/src/HAL/HAL_LPC1768/HAL.h
@@ -29,42 +29,27 @@
#define _HAL_LPC1768_H_
#define CPU_32_BIT
+#define HAL_INIT
-// --------------------------------------------------------------------------
-// Includes
-// --------------------------------------------------------------------------
+void HAL_init();
#include
#include
-
-#undef min
-#undef max
-
#include
-void _printf (const char *format, ...);
-void _putc(uint8_t c);
-uint8_t _getc();
-
extern "C" volatile uint32_t _millis;
-//arduino: Print.h
-#define DEC 10
-#define HEX 16
-#define OCT 8
-#define BIN 2
-//arduino: binary.h (weird defines)
-#define B01 1
-#define B10 2
-
#include
#include
+#include
#include "../shared/math_32bit.h"
#include "../shared/HAL_SPI.h"
#include "fastio.h"
+#include
#include "watchdog.h"
#include "HAL_timers.h"
+#include "MarlinSerial.h"
//
// Default graphical display delays
@@ -79,32 +64,20 @@ extern "C" volatile uint32_t _millis;
#define ST7920_DELAY_3 DELAY_NS(750)
#endif
-//
-// Arduino-style serial ports
-//
-#include "serial.h"
-#include "HardwareSerial.h"
-
-extern HalSerial usb_serial;
-
#if !WITHIN(SERIAL_PORT, -1, 3)
#error "SERIAL_PORT must be from -1 to 3"
#endif
#if SERIAL_PORT == -1
- #define MYSERIAL0 usb_serial
+ #define MYSERIAL0 UsbSerial
#elif SERIAL_PORT == 0
- extern HardwareSerial Serial;
- #define MYSERIAL0 Serial
+ #define MYSERIAL0 MSerial
#elif SERIAL_PORT == 1
- extern HardwareSerial Serial1;
- #define MYSERIAL0 Serial1
+ #define MYSERIAL0 MSerial1
#elif SERIAL_PORT == 2
- extern HardwareSerial Serial2;
- #define MYSERIAL0 Serial2
+ #define MYSERIAL0 MSerial2
#elif SERIAL_PORT == 3
- #define MYSERIAL0 Serial3
- extern HardwareSerial Serial3;
+ #define MYSERIAL0 MSerial3
#endif
#ifdef SERIAL_PORT_2
@@ -115,19 +88,15 @@ extern HalSerial usb_serial;
#endif
#define NUM_SERIAL 2
#if SERIAL_PORT_2 == -1
- #define MYSERIAL1 usb_serial
+ #define MYSERIAL1 UsbSerial
#elif SERIAL_PORT_2 == 0
- extern HardwareSerial Serial;
- #define MYSERIAL1 Serial
+ #define MYSERIAL1 MSerial
#elif SERIAL_PORT_2 == 1
- extern HardwareSerial Serial1;
- #define MYSERIAL1 Serial1
+ #define MYSERIAL1 MSerial1
#elif SERIAL_PORT_2 == 2
- extern HardwareSerial Serial2;
- #define MYSERIAL1 Serial2
+ #define MYSERIAL1 MSerial2
#elif SERIAL_PORT_2 == 3
- extern HardwareSerial Serial3;
- #define MYSERIAL1 Serial3
+ #define MYSERIAL1 MSerial3
#endif
#else
#define NUM_SERIAL 1
@@ -159,17 +128,33 @@ void spiSend(uint32_t chan, const uint8_t* buf, size_t n);
uint8_t spiRec(uint32_t chan);
//
-// ADC
+// ADC API
//
-#define HAL_ANALOG_SELECT(pin) HAL_adc_enable_channel(pin)
-#define HAL_START_ADC(pin) HAL_adc_start_conversion(pin)
-#define HAL_READ_ADC() HAL_adc_get_result()
-#define HAL_ADC_READY() HAL_adc_finished()
-void HAL_adc_init(void);
-void HAL_adc_enable_channel(int pin);
-void HAL_adc_start_conversion(const uint8_t adc_pin);
-uint16_t HAL_adc_get_result(void);
-bool HAL_adc_finished(void);
+#define ADC_MEDIAN_FILTER_SIZE (23) // Higher values increase step delay (phase shift),
+ // (ADC_MEDIAN_FILTER_SIZE + 1) / 2 sample step delay (12 samples @ 500Hz: 24ms phase shift)
+ // Memory usage per ADC channel (bytes): (6 * ADC_MEDIAN_FILTER_SIZE) + 16
+ // 8 * ((6 * 23) + 16 ) = 1232 Bytes for 8 channels
+
+#define ADC_LOWPASS_K_VALUE (6) // Higher values increase rise time
+ // Rise time sample delays for 100% signal convergence on full range step
+ // (1 : 13, 2 : 32, 3 : 67, 4 : 139, 5 : 281, 6 : 565, 7 : 1135, 8 : 2273)
+ // K = 6, 565 samples, 500Hz sample rate, 1.13s convergence on full range step
+ // Memory usage per ADC channel (bytes): 4 (32 Bytes for 8 channels)
+
+using FilteredADC = LPC176x::ADC;
+#define HAL_adc_init() FilteredADC::init()
+#define HAL_ANALOG_SELECT(pin) FilteredADC::enable_channel(pin)
+#define HAL_START_ADC(pin) FilteredADC::start_conversion(pin)
+#define HAL_READ_ADC() FilteredADC::get_result()
+#define HAL_ADC_READY() FilteredADC::finished_conversion()
+
+// Parse a G-code word into a pin index
+int16_t PARSED_PIN_INDEX(const char code, const int16_t dval);
+// P0.6 thru P0.9 are for the onboard SD card
+#define HAL_SENSITIVE_PINS P0_06, P0_07, P0_08, P0_09
+
+#define HAL_IDLETASK 1
+void HAL_idletask(void);
#endif // _HAL_LPC1768_H_
diff --git a/Marlin/src/HAL/HAL_LPC1768/HAL_spi.cpp b/Marlin/src/HAL/HAL_LPC1768/HAL_spi.cpp
index c8fe029d39..2765bb553a 100644
--- a/Marlin/src/HAL/HAL_LPC1768/HAL_spi.cpp
+++ b/Marlin/src/HAL/HAL_LPC1768/HAL_spi.cpp
@@ -49,7 +49,6 @@
#ifdef TARGET_LPC1768
#include "../../inc/MarlinConfig.h"
-
// --------------------------------------------------------------------------
// Includes
// --------------------------------------------------------------------------
@@ -59,7 +58,6 @@
// --------------------------------------------------------------------------
// Public functions
// --------------------------------------------------------------------------
-
#if ENABLED(LPC_SOFTWARE_SPI)
#include "SoftwareSPI.h"
@@ -127,8 +125,25 @@
#include
#include
- void spiBegin() { // setup SCK, MOSI & MISO pins for SSP0
+ // decide which HW SPI device to use
+ #ifndef LPC_HW_SPI_DEV
+ #if (SCK_PIN == P0_07 && MISO_PIN == P0_08 && MOSI_PIN == P0_09)
+ #define LPC_HW_SPI_DEV 1
+ #else
+ #if (SCK_PIN == P0_15 && MISO_PIN == P0_17 && MOSI_PIN == P0_18)
+ #define LPC_HW_SPI_DEV 0
+ #else
+ #error "Invalid pins selected for hardware SPI"
+ #endif
+ #endif
+ #endif
+ #if (LPC_HW_SPI_DEV == 0)
+ #define LPC_SSPn LPC_SSP0
+ #else
+ #define LPC_SSPn LPC_SSP1
+ #endif
+ void spiBegin() { // setup SCK, MOSI & MISO pins for SSP0
PINSEL_CFG_Type PinCfg; // data structure to hold init values
PinCfg.Funcnum = 2;
PinCfg.OpenDrain = 0;
@@ -147,10 +162,13 @@
PinCfg.Portnum = LPC1768_PIN_PORT(MOSI_PIN);
PINSEL_ConfigPin(&PinCfg);
SET_OUTPUT(MOSI_PIN);
+ // divide PCLK by 2 for SSP0
+ CLKPWR_SetPCLKDiv(LPC_HW_SPI_DEV == 0 ? CLKPWR_PCLKSEL_SSP0 : CLKPWR_PCLKSEL_SSP1, CLKPWR_PCLKSEL_CCLK_DIV_2);
+ spiInit(0);
+ SSP_Cmd(LPC_SSPn, ENABLE); // start SSP running
}
void spiInit(uint8_t spiRate) {
- SSP_Cmd(LPC_SSP0, DISABLE); // Disable SSP0 before changing rate
// table to convert Marlin spiRates (0-5 plus default) into bit rates
uint32_t Marlin_speed[7]; // CPSR is always 2
Marlin_speed[0] = 8333333; //(SCR: 2) desired: 8,000,000 actual: 8,333,333 +4.2% SPI_FULL_SPEED
@@ -160,33 +178,32 @@
Marlin_speed[4] = 500000; //(SCR: 49) desired: 500,000 actual: 500,000 SPI_SPEED_5
Marlin_speed[5] = 250000; //(SCR: 99) desired: 250,000 actual: 250,000 SPI_SPEED_6
Marlin_speed[6] = 125000; //(SCR:199) desired: 125,000 actual: 125,000 Default from HAL.h
-
- // divide PCLK by 2 for SSP0
- CLKPWR_SetPCLKDiv(CLKPWR_PCLKSEL_SSP0, CLKPWR_PCLKSEL_CCLK_DIV_2);
-
// setup for SPI mode
SSP_CFG_Type HW_SPI_init; // data structure to hold init values
SSP_ConfigStructInit(&HW_SPI_init); // set values for SPI mode
HW_SPI_init.ClockRate = Marlin_speed[MIN(spiRate, 6)]; // put in the specified bit rate
- SSP_Init(LPC_SSP0, &HW_SPI_init); // puts the values into the proper bits in the SSP0 registers
+ HW_SPI_init.Mode |= SSP_CR1_SSP_EN;
+ SSP_Init(LPC_SSPn, &HW_SPI_init); // puts the values into the proper bits in the SSP0 registers
+ }
- SSP_Cmd(LPC_SSP0, ENABLE); // start SSP0 running
+
+ static uint8_t doio(uint8_t b) {
+ /* send and receive a single byte */
+ SSP_SendData(LPC_SSPn, b & 0x00FF);
+ while (SSP_GetStatus(LPC_SSPn, SSP_STAT_BUSY)); // wait for it to finish
+ return SSP_ReceiveData(LPC_SSPn) & 0x00FF;
}
void spiSend(uint8_t b) {
- while (!SSP_GetStatus(LPC_SSP0, SSP_STAT_TXFIFO_NOTFULL)); // wait for room in the buffer
- SSP_SendData(LPC_SSP0, b & 0x00FF);
- while (SSP_GetStatus(LPC_SSP0, SSP_STAT_BUSY)); // wait for it to finish
+ doio(b);
}
void spiSend(const uint8_t* buf, size_t n) {
if (n == 0) return;
for (uint16_t i = 0; i < n; i++) {
- while (!SSP_GetStatus(LPC_SSP0, SSP_STAT_TXFIFO_NOTFULL)); // wait for room in the buffer
- SSP_SendData(LPC_SSP0, buf[i] & 0x00FF);
+ doio(buf[i]);
}
- while (SSP_GetStatus(LPC_SSP0, SSP_STAT_BUSY)); // wait for it to finish
}
void spiSend(uint32_t chan, byte b) {
@@ -195,17 +212,9 @@
void spiSend(uint32_t chan, const uint8_t* buf, size_t n) {
}
- static uint8_t get_one_byte() {
- // send a dummy byte so can clock in receive data
- SSP_SendData(LPC_SSP0,0x00FF);
- while (SSP_GetStatus(LPC_SSP0, SSP_STAT_BUSY)); // wait for it to finish
- return SSP_ReceiveData(LPC_SSP0) & 0x00FF;
- }
-
// Read single byte from SPI
uint8_t spiRec() {
- while (SSP_GetStatus(LPC_SSP0, SSP_STAT_RXFIFO_NOTEMPTY) || SSP_GetStatus(LPC_SSP0, SSP_STAT_BUSY)) SSP_ReceiveData(LPC_SSP0); //flush the receive buffer
- return get_one_byte();
+ return doio(0xff);
}
uint8_t spiRec(uint32_t chan) {
@@ -214,22 +223,25 @@
// Read from SPI into buffer
void spiRead(uint8_t*buf, uint16_t nbyte) {
- while (SSP_GetStatus(LPC_SSP0, SSP_STAT_RXFIFO_NOTEMPTY) || SSP_GetStatus(LPC_SSP0, SSP_STAT_BUSY)) SSP_ReceiveData(LPC_SSP0); //flush the receive buffer
if (nbyte == 0) return;
for (int i = 0; i < nbyte; i++) {
- buf[i] = get_one_byte();
+ buf[i] = doio(0xff);
}
}
static uint8_t spiTransfer(uint8_t b) {
- while (SSP_GetStatus(LPC_SSP0, SSP_STAT_RXFIFO_NOTEMPTY) || SSP_GetStatus(LPC_SSP0, SSP_STAT_BUSY)) SSP_ReceiveData(LPC_SSP0); //flush the receive buffer
- SSP_SendData(LPC_SSP0, b); // send the byte
- while (SSP_GetStatus(LPC_SSP0, SSP_STAT_BUSY)); // wait for it to finish
- return SSP_ReceiveData(LPC_SSP0) & 0x00FF;
+ return doio(b);
}
// Write from buffer to SPI
void spiSendBlock(uint8_t token, const uint8_t* buf) {
+ uint8_t response;
+ response = spiTransfer(token);
+
+ for (uint16_t i = 0; i < 512; i++) {
+ response = spiTransfer(buf[i]);
+ }
+ UNUSED(response);
}
/** Begin SPI transaction, set clock, bit order, data mode */
@@ -270,4 +282,3 @@ uint16_t SPIClass::transfer16(uint16_t data) {
SPIClass SPI;
#endif // TARGET_LPC1768
-
diff --git a/Marlin/src/HAL/HAL_LPC1768/HAL_temp.h b/Marlin/src/HAL/HAL_LPC1768/HAL_temp.h
deleted file mode 100644
index 20d2cf8d05..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/HAL_temp.h
+++ /dev/null
@@ -1 +0,0 @@
-// blank file needed until I get platformio to update it's copy of U8Glib-HAL
\ No newline at end of file
diff --git a/Marlin/src/HAL/HAL_LPC1768/HAL_timers.h b/Marlin/src/HAL/HAL_LPC1768/HAL_timers.h
index 4257028fc6..d818589104 100644
--- a/Marlin/src/HAL/HAL_LPC1768/HAL_timers.h
+++ b/Marlin/src/HAL/HAL_LPC1768/HAL_timers.h
@@ -20,9 +20,8 @@
*/
/**
- * HAL for Arduino Due and compatible (SAM3X8E)
*
- * For ARDUINO_ARCH_SAM
+ * HAL For LPC1768
*/
#ifndef _HAL_TIMERS_H
diff --git a/Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.cpp b/Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.cpp
deleted file mode 100644
index b27d8e1f31..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.cpp
+++ /dev/null
@@ -1,576 +0,0 @@
-/**
- * Marlin 3D Printer Firmware
- * Copyright (C) 2017 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
- *
- * Based on Sprinter and grbl.
- * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see .
- *
- */
-
-/**
- * The class Servo uses the PWM class to implement its functions
- *
- * All PWMs use the same repetition rate - 20mS because that's the normal servo rate
- */
-
-/**
- * This is a hybrid system.
- *
- * The PWM1 module is used to directly control the Servo 0, 1 & 3 pins and D9 & D10 pins. This keeps
- * the pulse width jitter to under a microsecond.
- *
- * For all other pins a timer is used to generate interrupts. The ISR
- * routine does the actual setting/clearing of pins. The upside is that any pin can
- * have a PWM channel assigned to it. The downside is that there is more pulse width
- * jitter. The jitter depends on what else is happening in the system and what ISRs
- * pre-empt the PWM ISR.
- */
-
-/**
- * The data structures are set up to minimize the computation done by the ISR which
- * minimizes ISR execution time. Execution times are 5-14µs depending on how full the
- * ISR table is. 14uS is for a 20 element ISR table.
- *
- * Two tables are used. One table contains the data used by the ISR to update/control
- * the PWM pins. The other is used as an aid when updating the ISR table.
- *
- * See the end of this file for details on the hardware/firmware interaction
- */
-
-/**
- * Directly controlled PWM pins (
- * NA means not being used as a directly controlled PWM pin
- *
- * Re-ARM MKS Sbase
- * PWM1.1 P1_18 SERVO3_PIN NA(no connection)
- * PWM1.1 P2_00 NA(E0_STEP_PIN) NA(X stepper)
- * PWM1.2 P1_20 SERVO0_PIN NA(no connection)
- * PWM1.2 P2_01 NA(X_STEP_PIN) NA(Y stepper)
- * PWM1.3 P1_21 SERVO1_PIN NA(no connection)
- * PWM1.3 P2_02 NA(Y_STEP_PIN) NA(Z stepper)
- * PWM1.4 P1_23 NA(SDSS(SSEL0)) SERVO0_PIN
- * PWM1.4 P2_03 NA(Z_STEP_PIN) NA(E0 stepper)
- * PWM1.5 P1_24 NA(X_MIN_PIN) NA(X_MIN_pin)
- * PWM1.5 P2_04 RAMPS_D9_PIN FAN_PIN
- * PWM1.6 P1_26 NA(Y_MIN_PIN) NA(Y_MIN_pin)
- * PWM1.6 P2_05 RAMPS_D10_PIN HEATER_BED_PIN
- */
-
-#ifdef TARGET_LPC1768
-
-#include "../../inc/MarlinConfig.h"
-#include
-#include "LPC1768_PWM.h"
-#include
-
-#define NUM_ISR_PWMS 20
-
-#define HAL_PWM_TIMER LPC_TIM3
-#define HAL_PWM_TIMER_ISR extern "C" void TIMER3_IRQHandler(void)
-#define HAL_PWM_TIMER_IRQn TIMER3_IRQn
-
-#define LPC_PORT_OFFSET (0x0020)
-#define LPC_PIN(pin) (1UL << pin)
-#define LPC_GPIO(port) ((volatile LPC_GPIO_TypeDef *)(LPC_GPIO0_BASE + LPC_PORT_OFFSET * port))
-
-typedef struct { // holds all data needed to control/init one of the PWM channels
- bool active_flag; // THIS TABLE ENTRY IS ACTIVELY TOGGLING A PIN
- pin_t pin;
- volatile uint32_t* set_register;
- volatile uint32_t* clr_register;
- uint32_t write_mask; // USED BY SET/CLEAR COMMANDS
- uint32_t microseconds; // value written to MR register
- uint32_t min; // lower value limit checked by WRITE routine before writing to the MR register
- uint32_t max; // upper value limit checked by WRITE routine before writing to the MR register
- uint8_t servo_index; // 0 - MAX_SERVO -1 : servo index, 0xFF : PWM channel
-} PWM_map;
-
-PWM_map PWM1_map_A[NUM_ISR_PWMS]; // compiler will initialize to all zeros
-PWM_map PWM1_map_B[NUM_ISR_PWMS]; // compiler will initialize to all zeros
-
-PWM_map *active_table = PWM1_map_A;
-PWM_map *work_table = PWM1_map_B;
-PWM_map *temp_table;
-
-#define P1_18_PWM_channel 1 // servo 3
-#define P1_20_PWM_channel 2 // servo 0
-#define P1_21_PWM_channel 3 // servo 1
-#define P1_23_PWM_channel 4 // servo 0 for MKS Sbase
-#define P2_04_PWM_channel 5 // D9
-#define P2_05_PWM_channel 6 // D10
-
-typedef struct {
- uint32_t min;
- uint32_t max;
- bool assigned;
-} table_direct;
-
-table_direct direct_table[6]; // compiler will initialize to all zeros
-
-/**
- * Prescale register and MR0 register values
- *
- * 100MHz PCLK 50MHz PCLK 25MHz PCLK 12.5MHz PCLK
- * ----------------- ----------------- ----------------- -----------------
- * desired prescale MR0 prescale MR0 prescale MR0 prescale MR0 resolution
- * prescale register register register register register register register register in degrees
- * freq value value value value value value value value
- *
- * 8 11.5 159,999 5.25 159,999 2.13 159,999 0.5625 159,999 0.023
- * 4 24 79,999 11.5 79,999 5.25 79,999 2.125 79,999 0.045
- * 2 49 39,999 24 39,999 11.5 39,999 5.25 39,999 0.090
- * 1 99 19,999 49 19,999 24 19,999 11.5 19,999 0.180
- * 0.5 199 9,999 99 9,999 49 9,999 24 9,999 0.360
- * 0.25 399 4,999 199 4,999 99 4,999 49 4,999 0.720
- * 0.125 799 2,499 399 2,499 199 2,499 99 2,499 1.440
- *
- * The desired prescale frequency column comes from an input in the range of 544 - 2400 microseconds
- * and the desire to just shift the input left or right as needed.
- *
- * A resolution of 0.2 degrees seems reasonable so a prescale frequency output of 1MHz is being used.
- * It also means we don't need to scale the input.
- *
- * The PCLK is set to 25MHz because that's the slowest one that gives whole numbers for prescale and
- * MR0 registers.
- *
- * Final settings:
- * PCLKSEL0: 0x0
- * PWM1PR: 0x018 (24)
- * PWM1MR0: 0x04E1F (19,999)
- *
- */
-
-void LPC1768_PWM_init(void) {
-
- ///// directly controlled PWM pins (interrupts not used for these)
-
- #define SBIT_CNTEN 0 // PWM1 counter & pre-scaler enable/disable
- #define SBIT_CNTRST 1 // reset counters to known state
- #define SBIT_PWMEN 3 // 1 - PWM, 0 - timer
- #define SBIT_PWMMR0R 1
- #define PCPWM1 6
- #define PCLK_PWM1 12
-
- SBI(LPC_SC->PCONP, PCPWM1); // Enable PWM1 controller (enabled on power up)
- LPC_SC->PCLKSEL0 &= ~(0x3 << PCLK_PWM1);
- LPC_SC->PCLKSEL0 |= (LPC_PWM1_PCLKSEL0 << PCLK_PWM1);
-
- uint32_t PR = (CLKPWR_GetPCLK(CLKPWR_PCLKSEL_PWM1) / 1000000) - 1; // Prescalar to create 1 MHz output
-
- LPC_PWM1->MR0 = LPC_PWM1_MR0; // TC resets every 19,999 + 1 cycles - sets PWM cycle(Ton+Toff) to 20 mS
- // MR0 must be set before TCR enables the PWM
- LPC_PWM1->TCR = _BV(SBIT_CNTEN) | _BV(SBIT_CNTRST) | _BV(SBIT_PWMEN); // Enable counters, reset counters, set mode to PWM
- CBI(LPC_PWM1->TCR, SBIT_CNTRST); // Take counters out of reset
- LPC_PWM1->PR = PR;
- LPC_PWM1->MCR = _BV(SBIT_PWMMR0R) | _BV(0); // Reset TC if it matches MR0, disable all interrupts except for MR0
- LPC_PWM1->CTCR = 0; // Disable counter mode (enable PWM mode)
- LPC_PWM1->LER = 0x07F; // Set the latch Enable Bits to load the new Match Values for MR0 - MR6
- LPC_PWM1->PCR = 0; // Single edge mode for all channels, PWM1 control of outputs off
-
- //// interrupt controlled PWM setup
-
- LPC_SC->PCONP |= 1 << 23; // power on timer3
- HAL_PWM_TIMER->PR = PR;
- HAL_PWM_TIMER->MCR = 0x0B; // Interrupt on MR0 & MR1, reset on MR0
- HAL_PWM_TIMER->MR0 = LPC_PWM1_MR0;
- HAL_PWM_TIMER->MR1 = 0;
- HAL_PWM_TIMER->TCR = _BV(0); // enable
- NVIC_EnableIRQ(HAL_PWM_TIMER_IRQn);
- NVIC_SetPriority(HAL_PWM_TIMER_IRQn, NVIC_EncodePriority(0, 4, 0));
-}
-
-
-bool ISR_table_update = false; // flag to tell the ISR that the tables need to be updated & swapped
-uint8_t ISR_index = 0; // index used by ISR to skip already actioned entries
-#define COPY_ACTIVE_TABLE for (uint8_t i = 0; i < NUM_ISR_PWMS ; i++) work_table[i] = active_table[i]
-uint32_t first_MR1_value = LPC_PWM1_MR0 + 1;
-
-void LPC1768_PWM_sort(void) {
-
- for (uint8_t i = NUM_ISR_PWMS; --i;) { // (bubble) sort table by microseconds
- bool didSwap = false;
- PWM_map temp;
- for (uint16_t j = 0; j < i; ++j) {
- if (work_table[j].microseconds > work_table[j + 1].microseconds) {
- temp = work_table[j + 1];
- work_table[j + 1] = work_table[j];
- work_table[j] = temp;
- didSwap = true;
- }
- }
- if (!didSwap) break;
- }
-}
-
-bool LPC1768_PWM_attach_pin(pin_t pin, uint32_t min /* = 1 */, uint32_t max /* = (LPC_PWM1_MR0 - 1) */, uint8_t servo_index /* = 0xFF */) {
-
- pin = GET_PIN_MAP_PIN(GET_PIN_MAP_INDEX(pin & 0xFF)); // Sometimes the upper byte is garbled
-
-//// direct control PWM code
- switch (pin) {
- case P1_23: // MKS Sbase Servo 0, PWM1 channel 4 (J3-8 PWM1.4)
- direct_table[P1_23_PWM_channel - 1].min = min;
- direct_table[P1_23_PWM_channel - 1].max = MIN(max, LPC_PWM1_MR0 - MR0_MARGIN);
- direct_table[P1_23_PWM_channel - 1].assigned = true;
- return true;
- case P1_20: // Servo 0, PWM1 channel 2 (Pin 11 P1.20 PWM1.2)
- direct_table[P1_20_PWM_channel - 1].min = min;
- direct_table[P1_20_PWM_channel - 1].max = MIN(max, LPC_PWM1_MR0 - MR0_MARGIN);
- direct_table[P1_20_PWM_channel - 1].assigned = true;
- return true;
- case P1_21: // Servo 1, PWM1 channel 3 (Pin 6 P1.21 PWM1.3)
- direct_table[P1_21_PWM_channel - 1].min = min;
- direct_table[P1_21_PWM_channel - 1].max = MIN(max, LPC_PWM1_MR0 - MR0_MARGIN);
- direct_table[P1_21_PWM_channel - 1].assigned = true;
- return true;
- case P1_18: // Servo 3, PWM1 channel 1 (Pin 4 P1.18 PWM1.1)
- direct_table[P1_18_PWM_channel - 1].min = min;
- direct_table[P1_18_PWM_channel - 1].max = MIN(max, LPC_PWM1_MR0 - MR0_MARGIN);
- direct_table[P1_18_PWM_channel - 1].assigned = true;
- return true;
- case P2_04: // D9 FET, PWM1 channel 5 (Pin 9 P2_04 PWM1.5)
- direct_table[P2_04_PWM_channel - 1].min = min;
- direct_table[P2_04_PWM_channel - 1].max = MIN(max, LPC_PWM1_MR0 - MR0_MARGIN);
- direct_table[P2_04_PWM_channel - 1].assigned = true;
- return true;
- case P2_05: // D10 FET, PWM1 channel 6 (Pin 10 P2_05 PWM1.6)
- direct_table[P2_05_PWM_channel - 1].min = min;
- direct_table[P2_05_PWM_channel - 1].max = MIN(max, LPC_PWM1_MR0 - MR0_MARGIN);
- direct_table[P2_05_PWM_channel - 1].assigned = true;
- return true;
- }
-
-//// interrupt controlled PWM code
- NVIC_DisableIRQ(HAL_PWM_TIMER_IRQn); // make it safe to update the active table
- // OK to update the active table because the
- // ISR doesn't use any of the changed items
-
- // We NEED memory barriers to ensure Interrupts are actually disabled!
- // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
- __DSB();
- __ISB();
-
- if (ISR_table_update) //use work table if that's the newest
- temp_table = work_table;
- else
- temp_table = active_table;
-
- uint8_t slot = 0;
- for (uint8_t i = 0; i < NUM_ISR_PWMS; i++) // see if already in table
- if (temp_table[i].pin == pin) {
- NVIC_EnableIRQ(HAL_PWM_TIMER_IRQn); // re-enable PWM interrupts
- return 1;
- }
-
- for (uint8_t i = 1; (i < NUM_ISR_PWMS + 1) && !slot; i++) // find empty slot
- if ( !(temp_table[i - 1].set_register)) { slot = i; break; } // any item that can't be zero when active or just attached is OK
-
- if (!slot) {
- NVIC_EnableIRQ(HAL_PWM_TIMER_IRQn); // re-enable PWM interrupts
- return 0;
- }
-
- slot--; // turn it into array index
-
- temp_table[slot].pin = pin; // init slot
- temp_table[slot].set_register = &LPC_GPIO(LPC1768_PIN_PORT(pin))->FIOSET;
- temp_table[slot].clr_register = &LPC_GPIO(LPC1768_PIN_PORT(pin))->FIOCLR;
- temp_table[slot].write_mask = LPC_PIN(LPC1768_PIN_PIN(pin));
- temp_table[slot].min = min;
- temp_table[slot].max = max; // different max for ISR PWMs than for direct PWMs
- temp_table[slot].servo_index = servo_index;
- temp_table[slot].active_flag = false;
-
- NVIC_EnableIRQ(HAL_PWM_TIMER_IRQn); // re-enable PWM interrupts
-
- return 1;
-}
-
-
-bool LPC1768_PWM_detach_pin(pin_t pin) {
-
- pin = GET_PIN_MAP_PIN(GET_PIN_MAP_INDEX(pin & 0xFF));
-
-//// direct control PWM code
- switch (pin) {
- case P1_23: // MKS Sbase Servo 0, PWM1 channel 4 (J3-8 PWM1.4)
- if (!direct_table[P1_23_PWM_channel - 1].assigned) return false;
- CBI(LPC_PWM1->PCR, 8 + P1_23_PWM_channel); // disable PWM1 module control of this pin
- LPC_PINCON->PINSEL3 &= ~(0x3 << 14); // return pin to general purpose I/O
- direct_table[P1_23_PWM_channel - 1].assigned = false;
- return true;
- case P1_20: // Servo 0, PWM1 channel 2 (Pin 11 P1.20 PWM1.2)
- if (!direct_table[P1_20_PWM_channel - 1].assigned) return false;
- CBI(LPC_PWM1->PCR, 8 + P1_20_PWM_channel); // disable PWM1 module control of this pin
- LPC_PINCON->PINSEL3 &= ~(0x3 << 8); // return pin to general purpose I/O
- direct_table[P1_20_PWM_channel - 1].assigned = false;
- return true;
- case P1_21: // Servo 1, PWM1 channel 3 (Pin 6 P1.21 PWM1.3)
- if (!direct_table[P1_21_PWM_channel - 1].assigned) return false;
- CBI(LPC_PWM1->PCR, 8 + P1_21_PWM_channel); // disable PWM1 module control of this pin
- LPC_PINCON->PINSEL3 &= ~(0x3 << 10); // return pin to general purpose I/O
- direct_table[P1_21_PWM_channel - 1].assigned = false;
- return true;
- case P1_18: // Servo 3, PWM1 channel 1 (Pin 4 P1.18 PWM1.1)
- if (!direct_table[P1_18_PWM_channel - 1].assigned) return false;
- CBI(LPC_PWM1->PCR, 8 + P1_18_PWM_channel); // disable PWM1 module control of this pin
- LPC_PINCON->PINSEL3 &= ~(0x3 << 4); // return pin to general purpose I/O
- direct_table[P1_18_PWM_channel - 1].assigned = false;
- return true;
- case P2_04: // D9 FET, PWM1 channel 5 (Pin 9 P2_04 PWM1.5)
- if (!direct_table[P2_04_PWM_channel - 1].assigned) return false;
- CBI(LPC_PWM1->PCR, 8 + P2_04_PWM_channel); // disable PWM1 module control of this pin
- LPC_PINCON->PINSEL4 &= ~(0x3 << 10); // return pin to general purpose I/O
- direct_table[P2_04_PWM_channel - 1].assigned = false;
- return true;
- case P2_05: // D10 FET, PWM1 channel 6 (Pin 10 P2_05 PWM1.6)
- if (!direct_table[P2_05_PWM_channel - 1].assigned) return false;
- CBI(LPC_PWM1->PCR, 8 + P2_05_PWM_channel); // disable PWM1 module control of this pin
- LPC_PINCON->PINSEL4 &= ~(0x3 << 4); // return pin to general purpose I/O
- direct_table[P2_05_PWM_channel - 1].assigned = false;
- return true;
- }
-
-//// interrupt controlled PWM code
- NVIC_DisableIRQ(HAL_PWM_TIMER_IRQn);
-
- // We NEED memory barriers to ensure Interrupts are actually disabled!
- // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
- __DSB();
- __ISB();
-
- if (ISR_table_update) {
- ISR_table_update = false; // don't update yet - have another update to do
- NVIC_EnableIRQ(HAL_PWM_TIMER_IRQn); // re-enable PWM interrupts
- }
- else {
- NVIC_EnableIRQ(HAL_PWM_TIMER_IRQn); // re-enable PWM interrupts
- COPY_ACTIVE_TABLE; // copy active table into work table
- }
-
- uint8_t slot = 0xFF;
- for (uint8_t i = 0; i < NUM_ISR_PWMS; i++) { // find slot
- if (work_table[i].pin == pin) {
- slot = i;
- break;
- }
- }
- if (slot == 0xFF) // return error if pin not found
- return false;
-
- work_table[slot] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
-
- LPC1768_PWM_sort(); // sort table by microseconds
- ISR_table_update = true;
- return true;
-}
-
-// value is 0-20,000 microseconds (0% to 100% duty cycle)
-// servo routine provides values in the 544 - 2400 range
-bool LPC1768_PWM_write(pin_t pin, uint32_t value) {
-
- pin = GET_PIN_MAP_PIN(GET_PIN_MAP_INDEX(pin & 0xFF));
-
-//// direct control PWM code
- switch (pin) {
- case P1_23: // MKS Sbase Servo 0, PWM1 channel 4 (J3-8 PWM1.4)
- if (!direct_table[P1_23_PWM_channel - 1].assigned) return false;
- LPC_PWM1->PCR |= _BV(8 + P1_23_PWM_channel); // enable PWM1 module control of this pin
- LPC_PINCON->PINSEL3 = 0x2 << 14; // must set pin function AFTER setting PCR
- // load the new time value
- LPC_PWM1->MR4 = MAX(MIN(value, direct_table[P1_23_PWM_channel - 1].max), direct_table[P1_23_PWM_channel - 1].min);
- LPC_PWM1->LER = 0x1 << P1_23_PWM_channel; // Set the latch Enable Bit to load the new Match Value on the next MR0
- return true;
- case P1_20: // Servo 0, PWM1 channel 2 (Pin 11 P1.20 PWM1.2)
- if (!direct_table[P1_20_PWM_channel - 1].assigned) return false;
- LPC_PWM1->PCR |= _BV(8 + P1_20_PWM_channel); // enable PWM1 module control of this pin
- LPC_PINCON->PINSEL3 |= 0x2 << 8; // must set pin function AFTER setting PCR
- // load the new time value
- LPC_PWM1->MR2 = MAX(MIN(value, direct_table[P1_20_PWM_channel - 1].max), direct_table[P1_20_PWM_channel - 1].min);
- LPC_PWM1->LER = 0x1 << P1_20_PWM_channel; // Set the latch Enable Bit to load the new Match Value on the next MR0
- return true;
- case P1_21: // Servo 1, PWM1 channel 3 (Pin 6 P1.21 PWM1.3)
- if (!direct_table[P1_21_PWM_channel - 1].assigned) return false;
- LPC_PWM1->PCR |= _BV(8 + P1_21_PWM_channel); // enable PWM1 module control of this pin
- LPC_PINCON->PINSEL3 |= 0x2 << 10; // must set pin function AFTER setting PCR
- // load the new time value
- LPC_PWM1->MR3 = MAX(MIN(value, direct_table[P1_21_PWM_channel - 1].max), direct_table[P1_21_PWM_channel - 1].min);
- LPC_PWM1->LER = 0x1 << P1_21_PWM_channel; // Set the latch Enable Bit to load the new Match Value on the next MR0
- return true;
- case P1_18: // Servo 3, PWM1 channel 1 (Pin 4 P1.18 PWM1.1)
- if (!direct_table[P1_18_PWM_channel - 1].assigned) return false;
- LPC_PWM1->PCR |= _BV(8 + P1_18_PWM_channel); // enable PWM1 module control of this pin
- LPC_PINCON->PINSEL3 |= 0x2 << 4; // must set pin function AFTER setting PCR
- // load the new time value
- LPC_PWM1->MR1 = MAX(MIN(value, direct_table[P1_18_PWM_channel - 1].max), direct_table[P1_18_PWM_channel - 1].min);
- LPC_PWM1->LER = 0x1 << P1_18_PWM_channel; // Set the latch Enable Bit to load the new Match Value on the next MR0
- return true;
- case P2_04: // D9 FET, PWM1 channel 5 (Pin 9 P2_04 PWM1.5)
- if (!direct_table[P2_04_PWM_channel - 1].assigned) return false;
- LPC_PWM1->PCR |= _BV(8 + P2_04_PWM_channel); // enable PWM1 module control of this pin
- LPC_PINCON->PINSEL4 |= 0x1 << 8; // must set pin function AFTER setting PCR
- // load the new time value
- LPC_PWM1->MR5 = MAX(MIN(value, direct_table[P2_04_PWM_channel - 1].max), direct_table[P2_04_PWM_channel - 1].min);
- LPC_PWM1->LER = 0x1 << P2_04_PWM_channel; // Set the latch Enable Bit to load the new Match Value on the next MR0
- return true;
- case P2_05: // D10 FET, PWM1 channel 6 (Pin 10 P2_05 PWM1.6)
- if (!direct_table[P2_05_PWM_channel - 1].assigned) return false;
- LPC_PWM1->PCR |= _BV(8 + P2_05_PWM_channel); // enable PWM1 module control of this pin
- LPC_PINCON->PINSEL4 |= 0x1 << 10; // must set pin function AFTER setting PCR
- // load the new time value
- LPC_PWM1->MR6 = MAX(MIN(value, direct_table[P2_05_PWM_channel - 1].max), direct_table[P2_05_PWM_channel - 1].min);
- LPC_PWM1->LER = 0x1 << P2_05_PWM_channel; // Set the latch Enable Bit to load the new Match Value on the next MR0
- return true;
- }
-
-//// interrupt controlled PWM code
- NVIC_DisableIRQ(HAL_PWM_TIMER_IRQn);
-
- // We NEED memory barriers to ensure Interrupts are actually disabled!
- // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
- __DSB();
- __ISB();
-
- if (!ISR_table_update) // use the most up to date table
- COPY_ACTIVE_TABLE; // copy active table into work table
-
- uint8_t slot = 0xFF;
- for (uint8_t i = 0; i < NUM_ISR_PWMS; i++) // find slot
- if (work_table[i].pin == pin) { slot = i; break; }
- if (slot == 0xFF) { // return error if pin not found
- NVIC_EnableIRQ(HAL_PWM_TIMER_IRQn);
- return false;
- }
-
- work_table[slot].microseconds = MAX(MIN(value, work_table[slot].max), work_table[slot].min);;
- work_table[slot].active_flag = true;
-
- LPC1768_PWM_sort(); // sort table by microseconds
- ISR_table_update = true;
-
- NVIC_EnableIRQ(HAL_PWM_TIMER_IRQn); // re-enable PWM interrupts
- return 1;
-}
-
-
-bool useable_hardware_PWM(pin_t pin) {
-
- pin = GET_PIN_MAP_PIN(GET_PIN_MAP_INDEX(pin & 0xFF));
-
- NVIC_DisableIRQ(HAL_PWM_TIMER_IRQn);
-
- // We NEED memory barriers to ensure Interrupts are actually disabled!
- // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
- __DSB();
- __ISB();
-
- bool return_flag = false;
- for (uint8_t i = 0; i < NUM_ISR_PWMS; i++) // see if it's already setup
- if (active_table[i].pin == pin) return_flag = true;
- for (uint8_t i = 0; i < NUM_ISR_PWMS; i++) // see if there is an empty slot
- if (!active_table[i].set_register) return_flag = true;
- NVIC_EnableIRQ(HAL_PWM_TIMER_IRQn); // re-enable PWM interrupts
- return return_flag;
-}
-
-
-////////////////////////////////////////////////////////////////////////////////
-
-
-#define PWM_LPC1768_ISR_SAFETY_FACTOR 5 // amount of time needed to guarantee MR1 count will be above TC
-volatile bool in_PWM_isr = false;
-
-HAL_PWM_TIMER_ISR {
- bool first_active_entry = true;
- uint32_t next_MR1_val;
-
- if (in_PWM_isr) goto exit_PWM_ISR; // prevent re-entering this ISR
- in_PWM_isr = true;
-
- if (HAL_PWM_TIMER->IR & 0x01) { // MR0 interrupt
- next_MR1_val = first_MR1_value; // only used if have a blank ISR table
- if (ISR_table_update) { // new values have been loaded so swap tables
- temp_table = active_table;
- active_table = work_table;
- work_table = temp_table;
- ISR_table_update = false;
- }
- }
- HAL_PWM_TIMER->IR = 0x3F; // clear all interrupts
-
- for (uint8_t i = 0; i < NUM_ISR_PWMS; i++) {
- if (active_table[i].active_flag) {
- if (first_active_entry) {
- first_active_entry = false;
- next_MR1_val = active_table[i].microseconds;
- }
- if (HAL_PWM_TIMER->TC < active_table[i].microseconds) {
- *active_table[i].set_register = active_table[i].write_mask; // set pin high
- }
- else {
- *active_table[i].clr_register = active_table[i].write_mask; // set pin low
- next_MR1_val = (i == NUM_ISR_PWMS -1)
- ? LPC_PWM1_MR0 + 1 // done with table, wait for MR0
- : active_table[i + 1].microseconds; // set next MR1 interrupt?
- }
- }
- }
- if (first_active_entry) next_MR1_val = LPC_PWM1_MR0 + 1; // empty table so disable MR1 interrupt
- HAL_PWM_TIMER->MR1 = MAX(next_MR1_val, HAL_PWM_TIMER->TC + PWM_LPC1768_ISR_SAFETY_FACTOR); // set next
- in_PWM_isr = false;
-
- exit_PWM_ISR:
- return;
-}
-#endif
-
-
-/////////////////////////////////////////////////////////////////
-///////////////// HARDWARE FIRMWARE INTERACTION ////////////////
-/////////////////////////////////////////////////////////////////
-
-/**
- * There are two distinct systems used for PWMs:
- * directly controlled pins
- * ISR controlled pins.
- *
- * The two systems are independent of each other. The use the same counter frequency so there's no
- * translation needed when setting the time values. The init, attach, detach and write routines all
- * start with the direct pin code which is followed by the ISR pin code.
- *
- * The PMW1 module handles the directly controlled pins. Each directly controlled pin is associated
- * with a match register (MR1 - MR6). When the associated MR equals the module's TIMER/COUNTER (TC)
- * then the pins is set to low. The MR0 register controls the repetition rate. When the TC equals
- * MR0 then the TC is reset and ALL directly controlled pins are set high. The resulting pulse widths
- * are almost immune to system loading and ISRs. No PWM1 interrupts are used.
- *
- * The ISR controlled pins use the TIMER/COUNTER, MR0 and MR1 registers from one timer. MR0 controls
- * period of the controls the repetition rate. When the TC equals MR0 then the TC is reset and an
- * interrupt is generated. When the TC equals MR1 then an interrupt is generated.
- *
- * Each interrupt does the following:
- * 1) Swaps the tables if it's a MR0 interrupt and the swap flag is set. It then clears the swap flag.
- * 2) Scans the entire ISR table (it's been sorted low to high time)
- * a. If its the first active entry then it grabs the time as a tentative time for MR1
- * b. If active and TC is less than the time then it sets the pin high
- * c. If active and TC is more than the time it sets the pin high
- * d. On every entry that sets a pin low it grabs the NEXT entry's time for use as the next MR1.
- * This results in MR1 being set to the time in the first active entry that does NOT set a
- * pin low.
- * e. If it's setting the last entry's pin low then it sets MR1 to a value bigger than MR0
- * f. If no value has been grabbed for the next MR1 then it's an empty table and MR1 is set to a
- * value greater than MR0
- */
diff --git a/Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.h b/Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.h
deleted file mode 100644
index 6c1a2480b8..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.h
+++ /dev/null
@@ -1,79 +0,0 @@
-/**
- * Marlin 3D Printer Firmware
- * Copyright (C) 2017 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
- *
- * Based on Sprinter and grbl.
- * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see .
- *
- */
-
-/**
- * The class Servo uses the PWM class to implement its functions
- *
- * All PWMs use the same repetition rate - 20mS because that's the normal servo rate
- */
-
-/**
- * This is a hybrid system.
- *
- * The PWM1 module is used to directly control the Servo 0, 1 & 3 pins. This keeps
- * the pulse width jitter to under a microsecond.
- *
- * For all other pins the PWM1 module is used to generate interrupts. The ISR
- * routine does the actual setting/clearing of pins. The upside is that any pin can
- * have a PWM channel assigned to it. The downside is that there is more pulse width
- * jitter. The jitter depends on what else is happening in the system and what ISRs
- * prempt the PWM ISR. Writing to the SD card can add 20 microseconds to the pulse
- * width.
- */
-
-/**
- * The data structures are setup to minimize the computation done by the ISR which
- * minimizes ISR execution time. Execution times are 2.2 - 3.7 microseconds.
- *
- * Two tables are used. active_table is used by the ISR. Changes to the table are
- * are done by copying the active_table into the work_table, updating the work_table
- * and then swapping the two tables. Swapping is done by manipulating pointers.
- *
- * Immediately after the swap the ISR uses the work_table until the start of the
- * next 20mS cycle. During this transition the "work_table" is actually the table
- * that was being used before the swap. The "active_table" contains the data that
- * will start being used at the start of the next 20mS period. This keeps the pins
- * well behaved during the transition.
- *
- * The ISR's priority is set to the maximum otherwise other ISRs can cause considerable
- * jitter in the PWM high time.
- *
- * See the end of this file for details on the hardware/firmware interaction
- */
-
-#ifndef _LPC1768_PWM_H_
-#define _LPC1768_PWM_H_
-
-#include
-#include
-
-#define LPC_PWM1_MR0 19999 // base repetition rate minus one count - 20mS
-#define LPC_PWM1_PCLKSEL0 CLKPWR_PCLKSEL_CCLK_DIV_4 // select clock divider for prescaler - defaults to 4 on power up
-#define MR0_MARGIN 200 // if channel value too close to MR0 the system locks up
-
-void LPC1768_PWM_init(void);
-bool LPC1768_PWM_attach_pin(pin_t pin, uint32_t min=1, uint32_t max=(LPC_PWM1_MR0 - (MR0_MARGIN)), uint8_t servo_index=0xFF);
-bool LPC1768_PWM_write(pin_t pin, uint32_t value);
-bool LPC1768_PWM_detach_pin(pin_t pin);
-bool useable_hardware_PWM(pin_t pin);
-
-#endif // _LPC1768_PWM_H_
diff --git a/Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.cpp b/Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.cpp
deleted file mode 100644
index 4997c5e4bc..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.cpp
+++ /dev/null
@@ -1,163 +0,0 @@
-/**
- * Marlin 3D Printer Firmware
- * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
- *
- * Based on Sprinter and grbl.
- * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see .
- *
- */
-
-/**
- * Based on servo.cpp - Interrupt driven Servo library for Arduino using 16 bit
- * timers- Version 2 Copyright (c) 2009 Michael Margolis. All right reserved.
- */
-
-/**
- * A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method.
- * The servos are pulsed in the background using the value most recently written using the write() method
- *
- * Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached.
- * Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four.
- *
- * The methods are:
- *
- * Servo - Class for manipulating servo motors connected to Arduino pins.
- *
- * attach(pin) - Attach a servo motor to an i/o pin.
- * attach(pin, min, max) - Attach to a pin, setting min and max values in microseconds
- * Default min is 544, max is 2400
- *
- * write() - Set the servo angle in degrees. (Invalid angles —over MIN_PULSE_WIDTH— are treated as µs.)
- * writeMicroseconds() - Set the servo pulse width in microseconds.
- * move(pin, angle) - Sequence of attach(pin), write(angle), safe_delay(servo_delay[servoIndex]).
- * With DEACTIVATE_SERVOS_AFTER_MOVE it detaches after servo_delay[servoIndex].
- * read() - Get the last-written servo pulse width as an angle between 0 and 180.
- * readMicroseconds() - Get the last-written servo pulse width in microseconds.
- * attached() - Return true if a servo is attached.
- * detach() - Stop an attached servo from pulsing its i/o pin.
- *
- */
-
-/**
- * The only time that this library wants physical movement is when a WRITE
- * command is issued. Before that all the attach & detach activity is solely
- * within the data base.
- *
- * The PWM output is inactive until the first WRITE. After that it stays active
- * unless DEACTIVATE_SERVOS_AFTER_MOVE is enabled and a MOVE command was issued.
- */
-
-#ifdef TARGET_LPC1768
-
-#include "../../inc/MarlinConfig.h"
-
-#if HAS_SERVOS
-
- #include "LPC1768_PWM.h"
- #include "LPC1768_Servo.h"
- #include "servo_private.h"
-
- ServoInfo_t servo_info[MAX_SERVOS]; // static array of servo info structures
- uint8_t ServoCount = 0; // the total number of attached servos
-
- #define US_TO_PULSE_WIDTH(p) p
- #define PULSE_WIDTH_TO_US(p) p
- #define TRIM_DURATION 0
- #define SERVO_MIN() MIN_PULSE_WIDTH // minimum value in uS for this servo
- #define SERVO_MAX() MAX_PULSE_WIDTH // maximum value in uS for this servo
-
- Servo::Servo() {
- if (ServoCount < MAX_SERVOS) {
- this->servoIndex = ServoCount++; // assign a servo index to this instance
- servo_info[this->servoIndex].pulse_width = US_TO_PULSE_WIDTH(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009
- }
- else
- this->servoIndex = INVALID_SERVO; // too many servos
- }
-
- int8_t Servo::attach(const int pin) {
- return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
- }
-
- int8_t Servo::attach(const int pin, const int min, const int max) {
-
- if (this->servoIndex >= MAX_SERVOS) return -1;
-
- if (pin > 0) servo_info[this->servoIndex].Pin.nbr = pin; // only assign a pin value if the pin info is
- // greater than zero. This way the init routine can
- // assign the pin and the MOVE command only needs the value.
-
- this->min = MIN_PULSE_WIDTH; //resolution of min/max is 1 uS
- this->max = MAX_PULSE_WIDTH;
-
- servo_info[this->servoIndex].Pin.isActive = true;
-
- return this->servoIndex;
- }
-
- void Servo::detach() {
- servo_info[this->servoIndex].Pin.isActive = false;
- }
-
- void Servo::write(int value) {
- if (value < MIN_PULSE_WIDTH) { // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
- value = map(constrain(value, 0, 180), 0, 180, SERVO_MIN(), SERVO_MAX());
- // odd - this sets zero degrees to 544 and 180 degrees to 2400 microseconds but the literature says
- // zero degrees should be 500 microseconds and 180 should be 2500
- }
- this->writeMicroseconds(value);
- }
-
- void Servo::writeMicroseconds(int value) {
- // calculate and store the values for the given channel
- byte channel = this->servoIndex;
- if (channel < MAX_SERVOS) { // ensure channel is valid
- // ensure pulse width is valid
- value = constrain(value, SERVO_MIN(), SERVO_MAX()) - (TRIM_DURATION);
- value = US_TO_PULSE_WIDTH(value); // convert to pulse_width after compensating for interrupt overhead - 12 Aug 2009
-
- servo_info[channel].pulse_width = value;
- LPC1768_PWM_attach_pin(servo_info[this->servoIndex].Pin.nbr, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH, this->servoIndex);
- LPC1768_PWM_write(servo_info[this->servoIndex].Pin.nbr, value);
-
- }
- }
-
- // return the value as degrees
- int Servo::read() { return map(this->readMicroseconds() + 1, SERVO_MIN(), SERVO_MAX(), 0, 180); }
-
- int Servo::readMicroseconds() {
- return (this->servoIndex == INVALID_SERVO) ? 0 : PULSE_WIDTH_TO_US(servo_info[this->servoIndex].pulse_width) + TRIM_DURATION;
- }
-
- bool Servo::attached() { return servo_info[this->servoIndex].Pin.isActive; }
-
- void Servo::move(const int value) {
- constexpr uint16_t servo_delay[] = SERVO_DELAY;
- static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long.");
- if (this->attach(0) >= 0) { // notice the pin number is zero here
- this->write(value);
- safe_delay(servo_delay[this->servoIndex]);
- #if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE)
- this->detach();
- LPC1768_PWM_detach_pin(servo_info[this->servoIndex].Pin.nbr); // shut down the PWM signal
- LPC1768_PWM_attach_pin(servo_info[this->servoIndex].Pin.nbr, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH, this->servoIndex); // make sure no one else steals the slot
- #endif
- }
- }
-
-#endif // HAS_SERVOS
-#endif // TARGET_LPC1768
diff --git a/Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.h b/Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.h
deleted file mode 100644
index f0d6f048a5..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/LPC1768_Servo.h
+++ /dev/null
@@ -1,62 +0,0 @@
-/**
- * Marlin 3D Printer Firmware
- * Copyright (C) 2017 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
- *
- * Based on Sprinter and grbl.
- * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see .
- *
- */
-
-/**
- * The class Servo uses the PWM class to implement its functions
- *
- * The PWM1 module is only used to generate interrups at specified times. It
- * is NOT used to directly toggle pins. The ISR writes to the pin assigned to
- * that interrupt
- *
- * All PWMs use the same repetition rate - 20mS because that's the normal servo rate
- *
- */
-
-#ifndef LPC1768_SERVO_H
-#define LPC1768_SERVO_H
-
-#include
-
- class Servo {
- public:
- Servo();
- int8_t attach(const int pin); // attach the given pin to the next free channel, set pinMode, return channel number (-1 on fail)
- int8_t attach(const int pin, const int min, const int max); // as above but also sets min and max values for writes.
- void detach();
- void write(int value); // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds
- void writeMicroseconds(int value); // write pulse width in microseconds
- void move(const int value); // attach the servo, then move to value
- // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds
- // if DEACTIVATE_SERVOS_AFTER_MOVE wait SERVO_DELAY, then detach
- int read(); // returns current pulse width as an angle between 0 and 180 degrees
- int readMicroseconds(); // returns current pulse width in microseconds for this servo (was read_us() in first release)
- bool attached(); // return true if this servo is attached, otherwise false
-
- private:
- uint8_t servoIndex; // index into the channel data for this servo
- int min;
- int max;
- };
-
- #define HAL_SERVO_LIB Servo
-
-#endif // LPC1768_SERVO_H
diff --git a/Marlin/src/HAL/HAL_LPC1768/MarlinSerial.cpp b/Marlin/src/HAL/HAL_LPC1768/MarlinSerial.cpp
new file mode 100644
index 0000000000..248e9007f1
--- /dev/null
+++ b/Marlin/src/HAL/HAL_LPC1768/MarlinSerial.cpp
@@ -0,0 +1,56 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ *
+ */
+
+#ifdef TARGET_LPC1768
+
+#include "../../inc/MarlinConfigPre.h"
+#include "MarlinSerial.h"
+
+#if (defined(SERIAL_PORT) && SERIAL_PORT == 0) || (defined(SERIAL_PORT_2) && SERIAL_PORT_2 == 0)
+ MarlinSerial MSerial(LPC_UART0);
+ extern "C" void UART0_IRQHandler(void) {
+ MSerial.IRQHandler();
+ }
+#endif
+
+#if (defined(SERIAL_PORT) && SERIAL_PORT == 1) || (defined(SERIAL_PORT_2) && SERIAL_PORT_2 == 1)
+ MarlinSerial MSerial1((LPC_UART_TypeDef *) LPC_UART1);
+ extern "C" void UART1_IRQHandler(void) {
+ MSerial1.IRQHandler();
+ }
+#endif
+
+#if (defined(SERIAL_PORT) && SERIAL_PORT == 2) || (defined(SERIAL_PORT_2) && SERIAL_PORT_2 == 2)
+ MarlinSerial MSerial2(LPC_UART2);
+ extern "C" void UART2_IRQHandler(void) {
+ MSerial2.IRQHandler();
+ }
+#endif
+
+#if (defined(SERIAL_PORT) && SERIAL_PORT == 3) || (defined(SERIAL_PORT_2) && SERIAL_PORT_2 == 3)
+ MarlinSerial MSerial3(LPC_UART3);
+ extern "C" void UART3_IRQHandler(void) {
+ MSerial3.IRQHandler();
+ }
+#endif
+
+#endif // TARGET_LPC1768
diff --git a/Marlin/src/HAL/HAL_LPC1768/MarlinSerial.h b/Marlin/src/HAL/HAL_LPC1768/MarlinSerial.h
new file mode 100644
index 0000000000..889b278e99
--- /dev/null
+++ b/Marlin/src/HAL/HAL_LPC1768/MarlinSerial.h
@@ -0,0 +1,71 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ *
+ */
+
+#ifndef MARLINSERIAL_H
+#define MARLINSERIAL_H
+#include
+#include
+
+#include "../../inc/MarlinConfigPre.h"
+#if ENABLED(EMERGENCY_PARSER)
+ #include "../../feature/emergency_parser.h"
+#endif
+
+#ifndef SERIAL_PORT
+ #define SERIAL_PORT 0
+#endif
+
+#ifndef RX_BUFFER_SIZE
+ #define RX_BUFFER_SIZE 128
+#endif
+#ifndef TX_BUFFER_SIZE
+ #define TX_BUFFER_SIZE 32
+#endif
+
+class MarlinSerial : public HardwareSerial {
+public:
+ MarlinSerial(LPC_UART_TypeDef *UARTx) :
+ HardwareSerial(UARTx)
+ #if ENABLED(EMERGENCY_PARSER)
+ , emergency_state(EmergencyParser::State::EP_RESET)
+ #endif
+ {
+ }
+
+ #if ENABLED(EMERGENCY_PARSER)
+ bool recv_callback(const char c) override {
+ emergency_parser.update(emergency_state, c);
+ return true; // do not discard character
+ }
+ #endif
+
+ #if ENABLED(EMERGENCY_PARSER)
+ EmergencyParser::State emergency_state;
+ #endif
+};
+
+extern MarlinSerial MSerial;
+extern MarlinSerial MSerial1;
+extern MarlinSerial MSerial2;
+extern MarlinSerial MSerial3;
+
+#endif // MARLINSERIAL_H
diff --git a/Marlin/src/HAL/HAL_LPC1768/servo_private.h b/Marlin/src/HAL/HAL_LPC1768/MarlinServo.h
similarity index 69%
rename from Marlin/src/HAL/HAL_LPC1768/servo_private.h
rename to Marlin/src/HAL/HAL_LPC1768/MarlinServo.h
index 451c487d73..6a6fae1fb7 100644
--- a/Marlin/src/HAL/HAL_LPC1768/servo_private.h
+++ b/Marlin/src/HAL/HAL_LPC1768/MarlinServo.h
@@ -50,35 +50,25 @@
#ifndef SERVO_PRIVATE_H
#define SERVO_PRIVATE_H
-#include
+#include
-// Macros
-//values in microseconds
-#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo
-#define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo
-#define DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached
-#define REFRESH_INTERVAL 20000 // minimum time to refresh servos in microseconds
+class MarlinServo: public Servo {
+ public:
+ void move(const int value) {
+ constexpr uint16_t servo_delay[] = SERVO_DELAY;
+ static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long.");
-#define MAX_SERVOS 4
+ if (this->attach(servo_info[this->servoIndex].Pin.nbr) >= 0) { // try to reattach
+ this->write(value);
+ safe_delay(servo_delay[this->servoIndex]); // delay to allow servo to reach position
+ #if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE)
+ this->detach();
+ #endif
+ }
-#define INVALID_SERVO 255 // flag indicating an invalid servo index
+ }
+};
-
-// Types
-
-typedef struct {
- uint8_t nbr : 8 ; // a pin number from 0 to 254 (255 signals invalid pin)
- uint8_t isActive : 1 ; // true if this channel is enabled, pin not pulsed if false
-} ServoPin_t;
-
-typedef struct {
- ServoPin_t Pin;
- unsigned int pulse_width; // pulse width in microseconds
-} ServoInfo_t;
-
-// Global variables
-
-extern uint8_t ServoCount;
-extern ServoInfo_t servo_info[MAX_SERVOS];
+#define HAL_SERVO_LIB MarlinServo
#endif // SERVO_PRIVATE_H
diff --git a/Marlin/src/HAL/HAL_LPC1768/SoftwareSPI.cpp b/Marlin/src/HAL/HAL_LPC1768/SoftwareSPI.cpp
deleted file mode 100644
index 546a1d50f1..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/SoftwareSPI.cpp
+++ /dev/null
@@ -1,90 +0,0 @@
-/**
- * Marlin 3D Printer Firmware
- * Copyright (C) 2016, 2017 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
- *
- * Based on Sprinter and grbl.
- * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see .
- *
- */
-
-/**
- * Software SPI functions originally from Arduino Sd2Card Library
- * Copyright (C) 2009 by William Greiman
- */
-
-/**
- * For TARGET_LPC1768
- */
-
-#ifdef TARGET_LPC1768
-
-#include "../../inc/MarlinConfig.h"
-
-// --------------------------------------------------------------------------
-// Software SPI
-// --------------------------------------------------------------------------
-
-/**
- * This software SPI runs at multiple rates. The SD software provides an index
- * (spiRate) of 0-6. The mapping is:
- * 0 - about 5 MHz peak (6 MHz on LPC1769)
- * 1-2 - about 2 MHz peak
- * 3 - about 1 MHz peak
- * 4 - about 500 kHz peak
- * 5 - about 250 kHz peak
- * 6 - about 125 kHz peak
- */
-
-uint8_t swSpiTransfer(uint8_t b, const uint8_t spi_speed, const pin_t sck_pin, const pin_t miso_pin, const pin_t mosi_pin) {
- for (uint8_t i = 0; i < 8; i++) {
- if (spi_speed == 0) {
- WRITE(mosi_pin, !!(b & 0x80));
- WRITE(sck_pin, HIGH);
- b <<= 1;
- if (miso_pin >= 0 && READ(miso_pin)) b |= 1;
- WRITE(sck_pin, LOW);
- }
- else {
- const uint8_t state = (b & 0x80) ? HIGH : LOW;
- for (uint8_t j = 0; j < spi_speed; j++)
- WRITE(mosi_pin, state);
-
- for (uint8_t j = 0; j < spi_speed + (miso_pin >= 0 ? 0 : 1); j++)
- WRITE(sck_pin, HIGH);
-
- b <<= 1;
- if (miso_pin >= 0 && READ(miso_pin)) b |= 1;
-
- for (uint8_t j = 0; j < spi_speed; j++)
- WRITE(sck_pin, LOW);
- }
- }
- return b;
-}
-
-void swSpiBegin(const pin_t sck_pin, const pin_t miso_pin, const pin_t mosi_pin) {
- SET_OUTPUT(sck_pin);
- if (VALID_PIN(miso_pin)) SET_INPUT(miso_pin);
- SET_OUTPUT(mosi_pin);
-}
-
-uint8_t swSpiInit(const uint8_t spiRate, const pin_t sck_pin, const pin_t mosi_pin) {
- WRITE(mosi_pin, HIGH);
- WRITE(sck_pin, LOW);
- return (SystemCoreClock == 120000000 ? 44 : 38) / POW(2, 6 - MIN(spiRate, 6));
-}
-
-#endif // TARGET_LPC1768
diff --git a/Marlin/src/HAL/HAL_LPC1768/SoftwareSPI.h b/Marlin/src/HAL/HAL_LPC1768/SoftwareSPI.h
deleted file mode 100644
index 49136e2aa3..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/SoftwareSPI.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/**
- * Marlin 3D Printer Firmware
- * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
- *
- * Based on Sprinter and grbl.
- * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see .
- *
- */
-
-#ifndef _SOFTWARE_SPI_H_
-#define _SOFTWARE_SPI_H_
-
-#include
-
-// --------------------------------------------------------------------------
-// Software SPI
-// --------------------------------------------------------------------------
-
-/**
- * This software SPI runs at multiple rates. The SD software provides an index
- * (spiRate) of 0-6. The mapping is:
- * 0 - about 5 MHz peak (6 MHz on LPC1769)
- * 1-2 - about 2 MHz peak
- * 3 - about 1 MHz peak
- * 4 - about 500 kHz peak
- * 5 - about 250 kHz peak
- * 6 - about 125 kHz peak
- */
-
-void swSpiBegin(const pin_t sck_pin, const pin_t miso_pin, const pin_t mosi_pin);
-
-// Returns the spi_speed value to be passed to swSpiTransfer
-uint8_t swSpiInit(const uint8_t spiRate, const pin_t sck_pin, const pin_t mosi_pin);
-
-uint8_t swSpiTransfer(uint8_t b, const uint8_t spi_speed, const pin_t sck_pin, const pin_t miso_pin, const pin_t mosi_pin);
-
-#endif // _SOFTWARE_SPI_H_
diff --git a/Marlin/src/HAL/HAL_LPC1768/WInterrupts.cpp b/Marlin/src/HAL/HAL_LPC1768/WInterrupts.cpp
deleted file mode 100644
index a92a61a8fd..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/WInterrupts.cpp
+++ /dev/null
@@ -1,162 +0,0 @@
-/**
- * Copyright (c) 2011-2012 Arduino. All right reserved.
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
- *
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- * See the GNU Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- */
-
-#ifdef TARGET_LPC1768
-
-#include "../../inc/MarlinConfig.h"
-
-#define GNUM 31
-
-typedef void (*interruptCB)(void);
-
-static interruptCB callbacksP0[GNUM] = {};
-static interruptCB callbacksP2[GNUM] = {};
-
-extern "C" void GpioEnableInt(const uint32_t port, const uint32_t pin, const uint32_t mode);
-extern "C" void GpioDisableInt(const uint32_t port, const uint32_t pin);
-
-
-static void __initialize() {
- NVIC_SetPriority(EINT3_IRQn, NVIC_EncodePriority(0, 1, 0));
- NVIC_EnableIRQ(EINT3_IRQn);
-}
-
-void attachInterrupt(const pin_t pin, void (*callback)(void), uint32_t mode) {
- static int enabled = 0;
-
- if (!INTERRUPT_PIN(pin)) return;
-
- if (!enabled) {
- __initialize();
- ++enabled;
- }
-
- uint8_t myport = LPC1768_PIN_PORT(pin),
- mypin = LPC1768_PIN_PIN(pin);
-
- if (myport == 0)
- callbacksP0[mypin] = callback;
- else
- callbacksP2[mypin] = callback;
-
- // Enable interrupt
- GpioEnableInt(myport,mypin,mode);
-}
-
-void detachInterrupt(const pin_t pin) {
- if (!INTERRUPT_PIN(pin)) return;
-
- const uint8_t myport = LPC1768_PIN_PORT(pin),
- mypin = LPC1768_PIN_PIN(pin);
-
- // Disable interrupt
- GpioDisableInt(myport, mypin);
-
- // unset callback
- if (myport == 0)
- callbacksP0[mypin] = 0;
- else //if (myport == 2 )
- callbacksP2[mypin] = 0;
-}
-
-
-extern "C" void GpioEnableInt(uint32_t port, uint32_t pin, uint32_t mode) {
- //pin here is the processor pin, not logical pin
- if (port == 0) {
- LPC_GPIOINT->IO0IntClr = _BV(pin);
- if (mode == RISING) {
- SBI(LPC_GPIOINT->IO0IntEnR, pin);
- CBI(LPC_GPIOINT->IO0IntEnF, pin);
- }
- else if (mode == FALLING) {
- SBI(LPC_GPIOINT->IO0IntEnF, pin);
- CBI(LPC_GPIOINT->IO0IntEnR, pin);
- }
- else if (mode == CHANGE) {
- SBI(LPC_GPIOINT->IO0IntEnR, pin);
- SBI(LPC_GPIOINT->IO0IntEnF, pin);
- }
- }
- else {
- LPC_GPIOINT->IO2IntClr = _BV(pin);
- if (mode == RISING) {
- SBI(LPC_GPIOINT->IO2IntEnR, pin);
- CBI(LPC_GPIOINT->IO2IntEnF, pin);
- }
- else if (mode == FALLING) {
- SBI(LPC_GPIOINT->IO2IntEnF, pin);
- CBI(LPC_GPIOINT->IO2IntEnR, pin);
- }
- else if (mode == CHANGE) {
- SBI(LPC_GPIOINT->IO2IntEnR, pin);
- SBI(LPC_GPIOINT->IO2IntEnF, pin);
- }
- }
-}
-
-extern "C" void GpioDisableInt(const uint32_t port, const uint32_t pin) {
- if (port == 0) {
- CBI(LPC_GPIOINT->IO0IntEnR, pin);
- CBI(LPC_GPIOINT->IO0IntEnF, pin);
- LPC_GPIOINT->IO0IntClr = _BV(pin);
- }
- else {
- CBI(LPC_GPIOINT->IO2IntEnR, pin);
- CBI(LPC_GPIOINT->IO2IntEnF, pin);
- LPC_GPIOINT->IO2IntClr = _BV(pin);
- }
-}
-
-extern "C" void EINT3_IRQHandler(void) {
- // Read in all current interrupt registers. We do this once as the
- // GPIO interrupt registers are on the APB bus, and this is slow.
- uint32_t rise0 = LPC_GPIOINT->IO0IntStatR,
- fall0 = LPC_GPIOINT->IO0IntStatF,
- rise2 = LPC_GPIOINT->IO2IntStatR,
- fall2 = LPC_GPIOINT->IO2IntStatF;
-
- // Clear the interrupts ASAP
- LPC_GPIOINT->IO0IntClr = LPC_GPIOINT->IO2IntClr = 0xFFFFFFFF;
- NVIC_ClearPendingIRQ(EINT3_IRQn);
-
- while (rise0 > 0) { // If multiple pins changes happened continue as long as there are interrupts pending
- const uint8_t bitloc = 31 - __CLZ(rise0); // CLZ returns number of leading zeros, 31 minus that is location of first pending interrupt
- if (callbacksP0[bitloc] != NULL) callbacksP0[bitloc]();
- rise0 -= _BV(bitloc);
- }
-
- while (fall0 > 0) {
- const uint8_t bitloc = 31 - __CLZ(fall0);
- if (callbacksP0[bitloc] != NULL) callbacksP0[bitloc]();
- fall0 -= _BV(bitloc);
- }
-
- while(rise2 > 0) {
- const uint8_t bitloc = 31 - __CLZ(rise2);
- if (callbacksP2[bitloc] != NULL) callbacksP2[bitloc]();
- rise2 -= _BV(bitloc);
- }
-
- while (fall2 > 0) {
- const uint8_t bitloc = 31 - __CLZ(fall2);
- if (callbacksP2[bitloc] != NULL) callbacksP2[bitloc]();
- fall2 -= _BV(bitloc);
- }
-}
-
-#endif // TARGET_LPC1768
diff --git a/Marlin/src/HAL/HAL_LPC1768/arduino.cpp b/Marlin/src/HAL/HAL_LPC1768/arduino.cpp
deleted file mode 100644
index e7a3d5e8f4..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/arduino.cpp
+++ /dev/null
@@ -1,179 +0,0 @@
-/**
- * Marlin 3D Printer Firmware
- * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
- *
- * Based on Sprinter and grbl.
- * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see .
- *
- */
-
-#ifdef TARGET_LPC1768
-
-#include "LPC1768_PWM.h"
-#include
-
-#include "../../inc/MarlinConfig.h"
-#include "../shared/Delay.h"
-
-// Interrupts
-void cli(void) { __disable_irq(); } // Disable
-void sei(void) { __enable_irq(); } // Enable
-
-// Time functions
-void _delay_ms(const int delay_ms) {
- delay(delay_ms);
-}
-
-uint32_t millis() {
- return _millis;
-}
-
-// This is required for some Arduino libraries we are using
-void delayMicroseconds(uint32_t us) {
- DELAY_US(us);
-}
-
-extern "C" void delay(const int msec) {
- volatile millis_t end = _millis + msec;
- SysTick->VAL = SysTick->LOAD; // reset systick counter so next systick is in exactly 1ms
- // this could extend the time between systicks by upto 1ms
- while PENDING(_millis, end) __WFE();
-}
-
-// IO functions
-// As defined by Arduino INPUT(0x0), OUTPUT(0x1), INPUT_PULLUP(0x2)
-void pinMode(const pin_t pin, const uint8_t mode) {
- if (!VALID_PIN(pin)) return;
-
- PINSEL_CFG_Type config = { LPC1768_PIN_PORT(pin),
- LPC1768_PIN_PIN(pin),
- PINSEL_FUNC_0,
- PINSEL_PINMODE_TRISTATE,
- PINSEL_PINMODE_NORMAL };
- switch (mode) {
- case INPUT:
- LPC_GPIO(LPC1768_PIN_PORT(pin))->FIODIR &= ~LPC_PIN(LPC1768_PIN_PIN(pin));
- break;
- case OUTPUT:
- LPC_GPIO(LPC1768_PIN_PORT(pin))->FIODIR |= LPC_PIN(LPC1768_PIN_PIN(pin));
- break;
- case INPUT_PULLUP:
- LPC_GPIO(LPC1768_PIN_PORT(pin))->FIODIR &= ~LPC_PIN(LPC1768_PIN_PIN(pin));
- config.Pinmode = PINSEL_PINMODE_PULLUP;
- break;
- case INPUT_PULLDOWN:
- LPC_GPIO(LPC1768_PIN_PORT(pin))->FIODIR &= ~LPC_PIN(LPC1768_PIN_PIN(pin));
- config.Pinmode = PINSEL_PINMODE_PULLDOWN;
- break;
- default: return;
- }
- PINSEL_ConfigPin(&config);
-}
-
-void digitalWrite(pin_t pin, uint8_t pin_status) {
- if (!VALID_PIN(pin)) return;
-
- if (pin_status)
- LPC_GPIO(LPC1768_PIN_PORT(pin))->FIOSET = LPC_PIN(LPC1768_PIN_PIN(pin));
- else
- LPC_GPIO(LPC1768_PIN_PORT(pin))->FIOCLR = LPC_PIN(LPC1768_PIN_PIN(pin));
-
- pinMode(pin, OUTPUT); // Set pin mode on every write (Arduino version does this)
-
- /**
- * Must be done AFTER the output state is set. Doing this before will cause a
- * 2uS glitch if writing a "1".
- *
- * When the Port Direction bit is written to a "1" the output is immediately set
- * to the value of the FIOPIN bit which is "0" because of power up defaults.
- */
-}
-
-bool digitalRead(pin_t pin) {
- if (!VALID_PIN(pin)) return false;
-
- return LPC_GPIO(LPC1768_PIN_PORT(pin))->FIOPIN & LPC_PIN(LPC1768_PIN_PIN(pin)) ? 1 : 0;
-}
-
-void analogWrite(pin_t pin, int pwm_value) { // 1 - 254: pwm_value, 0: LOW, 255: HIGH
- if (!VALID_PIN(pin)) return;
-
- #define MR0_MARGIN 200 // if channel value too close to MR0 the system locks up
-
- static bool out_of_PWM_slots = false;
-
- uint value = MAX(MIN(pwm_value, 255), 0);
- if (value == 0 || value == 255) { // treat as digital pin
- LPC1768_PWM_detach_pin(pin); // turn off PWM
- digitalWrite(pin, value);
- }
- else {
- if (LPC1768_PWM_attach_pin(pin, 1, LPC_PWM1->MR0, 0xFF))
- LPC1768_PWM_write(pin, map(value, 0, 255, 1, LPC_PWM1->MR0)); // map 1-254 onto PWM range
- else { // out of PWM channels
- if (!out_of_PWM_slots) SERIAL_ECHOPGM(".\nWARNING - OUT OF PWM CHANNELS\n.\n"); //only warn once
- out_of_PWM_slots = true;
- digitalWrite(pin, value); // treat as a digital pin if out of channels
- }
- }
-}
-
-extern bool HAL_adc_finished();
-
-uint16_t analogRead(pin_t adc_pin) {
- HAL_adc_start_conversion(adc_pin);
- while (!HAL_adc_finished()); // Wait for conversion to finish
- return HAL_adc_get_result();
-}
-
-// **************************
-// Persistent Config Storage
-// **************************
-
-void eeprom_write_byte(uint8_t *pos, unsigned char value) {
-
-}
-
-uint8_t eeprom_read_byte(uint8_t * pos) { return '\0'; }
-
-void eeprom_read_block(void *__dst, const void *__src, size_t __n) { }
-
-void eeprom_update_block(const void *__src, void *__dst, size_t __n) { }
-
-char *dtostrf (double __val, signed char __width, unsigned char __prec, char *__s) {
- char format_string[20];
- snprintf(format_string, 20, "%%%d.%df", __width, __prec);
- sprintf(__s, format_string, __val);
- return __s;
-}
-
-int32_t random(int32_t max) {
- return rand() % max;
-}
-
-int32_t random(int32_t min, int32_t max) {
- return min + rand() % (max - min);
-}
-
-void randomSeed(uint32_t value) {
- srand(value);
-}
-
-int map(uint16_t x, uint16_t in_min, uint16_t in_max, uint16_t out_min, uint16_t out_max) {
- return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
-}
-
-#endif // TARGET_LPC1768
diff --git a/Marlin/src/HAL/HAL_LPC1768/debug_extra_script.py b/Marlin/src/HAL/HAL_LPC1768/debug_extra_script.py
deleted file mode 100644
index ae53adea0a..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/debug_extra_script.py
+++ /dev/null
@@ -1,21 +0,0 @@
-Import("env")
-
-env.AddPostAction(
- "$BUILD_DIR/firmware.hex",
- env.VerboseAction(" ".join([
- "sed", "-i.bak",
- "s/:10040000FFFFFFFFFFFFFFFFFFFFFFFFDEF9FFFF23/:10040000FFFFFFFFFFFFFFFFFFFFFFFFFEFFFFFFFD/",
- "$BUILD_DIR/firmware.hex"
- ]), "Fixing $BUILD_DIR/firmware.hex secure flash flags"))
-env.AddPreAction(
- "upload",
- env.VerboseAction(" ".join([
- "echo",
- "'h\\nloadfile $BUILD_DIR/firmware.hex\\nr\\nq\\n'",
- ">$BUILD_DIR/aux.jlink"
- ]), "Creating auxiliary files"))
-
-env.Replace(
- UPLOADHEXCMD=
- 'JLinkExe -device MK20DX256xxx7 -speed 4000 -if swd -autoconnect 1 -CommanderScript $BUILD_DIR/aux.jlink -SkipProgOnCRCMatch = 1 -VerifyDownload = 1'
-)
diff --git a/Marlin/src/HAL/HAL_LPC1768/fastio.h b/Marlin/src/HAL/HAL_LPC1768/fastio.h
index 5f55f73c86..1374e6aafa 100644
--- a/Marlin/src/HAL/HAL_LPC1768/fastio.h
+++ b/Marlin/src/HAL/HAL_LPC1768/fastio.h
@@ -35,27 +35,23 @@
#ifndef _FASTIO_LPC1768_H
#define _FASTIO_LPC1768_H
-#include
#include
-#include
-bool useable_hardware_PWM(pin_t pin);
#define USEABLE_HARDWARE_PWM(pin) useable_hardware_PWM(pin)
-#define LPC_PORT_OFFSET (0x0020)
-#define LPC_PIN(pin) (1UL << pin)
-#define LPC_GPIO(port) ((volatile LPC_GPIO_TypeDef *)(LPC_GPIO0_BASE + LPC_PORT_OFFSET * port))
+#define LPC_PIN(pin) gpio_pin(pin)
+#define LPC_GPIO(port) gpio_port(port)
-#define SET_DIR_INPUT(IO) (LPC_GPIO(LPC1768_PIN_PORT(IO))->FIODIR &= ~LPC_PIN(LPC1768_PIN_PIN(IO)))
-#define SET_DIR_OUTPUT(IO) (LPC_GPIO(LPC1768_PIN_PORT(IO))->FIODIR |= LPC_PIN(LPC1768_PIN_PIN(IO)))
+#define SET_DIR_INPUT(IO) gpio_set_input(IO)
+#define SET_DIR_OUTPUT(IO) gpio_set_output(IO)
-#define SET_MODE(IO, mode) (pin_mode((LPC1768_PIN_PORT(IO), LPC1768_PIN_PIN(IO)), mode))
+#define SET_MODE(IO, mode) pinMode(IO, mode)
-#define WRITE_PIN_SET(IO) (LPC_GPIO(LPC1768_PIN_PORT(IO))->FIOSET = LPC_PIN(LPC1768_PIN_PIN(IO)))
-#define WRITE_PIN_CLR(IO) (LPC_GPIO(LPC1768_PIN_PORT(IO))->FIOCLR = LPC_PIN(LPC1768_PIN_PIN(IO)))
+#define WRITE_PIN_SET(IO) gpio_set(IO)
+#define WRITE_PIN_CLR(IO) gpio_clear(IO)
-#define READ_PIN(IO) ((LPC_GPIO(LPC1768_PIN_PORT(IO))->FIOPIN & LPC_PIN(LPC1768_PIN_PIN(IO))) ? 1 : 0)
-#define WRITE_PIN(IO,V) ((V) ? WRITE_PIN_SET(IO) : WRITE_PIN_CLR(IO))
+#define READ_PIN(IO) gpio_get(IO)
+#define WRITE_PIN(IO,V) gpio_set(IO, V)
/**
* Magic I/O routines
@@ -89,10 +85,10 @@ bool useable_hardware_PWM(pin_t pin);
#define _PULLDOWN(IO,V) pinMode(IO, (V) ? INPUT_PULLDOWN : INPUT)
/// check if pin is an input
-#define _GET_INPUT(IO) (LPC_GPIO(LPC1768_PIN_PORT(IO))->FIODIR & LPC_PIN(LPC1768_PIN_PIN(IO)) != 0)
+#define _GET_INPUT(IO) (!gpio_get_dir(IO))
/// check if pin is an output
-#define _GET_OUTPUT(IO) (LPC_GPIO(LPC1768_PIN_PORT(IO))->FIODIR & LPC_PIN(LPC1768_PIN_PIN(IO)) == 0)
+#define _GET_OUTPUT(IO) (gpio_get_dir(IO))
/// check if pin is a timer
/// all gpio pins are pwm capable, either interrupt or hardware pwm controlled
diff --git a/Marlin/src/HAL/HAL_LPC1768/include/Arduino.h b/Marlin/src/HAL/HAL_LPC1768/include/Arduino.h
deleted file mode 100644
index 63502ad710..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/include/Arduino.h
+++ /dev/null
@@ -1,125 +0,0 @@
-/**
- * Marlin 3D Printer Firmware
- * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
- *
- * Based on Sprinter and grbl.
- * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see .
- *
- */
-
-#ifndef __ARDUINO_H__
-#define __ARDUINO_H__
-#include
-#include
-#include
-
-#include
-
-#define HIGH 0x01
-#define LOW 0x00
-
-#define INPUT 0x00
-#define OUTPUT 0x01
-#define INPUT_PULLUP 0x02
-#define INPUT_PULLDOWN 0x03
-
-#define LSBFIRST 0
-#define MSBFIRST 1
-
-#define CHANGE 0x02
-#define FALLING 0x03
-#define RISING 0x04
-
-typedef uint8_t byte;
-#define PROGMEM
-#define PSTR(v) (v)
-#define PGM_P const char *
-
-// Used for libraries, preprocessor, and constants
-#define min(a,b) ((a)<(b)?(a):(b))
-#define max(a,b) ((a)>(b)?(a):(b))
-#define abs(x) ((x)>0?(x):-(x))
-
-#ifndef isnan
- #define isnan std::isnan
-#endif
-#ifndef isinf
- #define isinf std::isinf
-#endif
-
-#define sq(v) ((v) * (v))
-#define square(v) sq(v)
-#define constrain(value, arg_min, arg_max) ((value) < (arg_min) ? (arg_min) :((value) > (arg_max) ? (arg_max) : (value)))
-
-//Interrupts
-void cli(void); // Disable
-void sei(void); // Enable
-void attachInterrupt(const pin_t pin, void (*callback)(void), uint32_t mode);
-void detachInterrupt(const pin_t pin);
-extern "C" void GpioEnableInt(uint32_t port, uint32_t pin, uint32_t mode);
-extern "C" void GpioDisableInt(uint32_t port, uint32_t pin);
-
-// Program Memory
-#define pgm_read_ptr(addr) (*((void**)(addr)))
-#define pgm_read_byte_near(addr) (*((uint8_t*)(addr)))
-#define pgm_read_float_near(addr) (*((float*)(addr)))
-#define pgm_read_word_near(addr) (*((uint16_t*)(addr)))
-#define pgm_read_dword_near(addr) (*((uint32_t*)(addr)))
-#define pgm_read_byte(addr) pgm_read_byte_near(addr)
-#define pgm_read_float(addr) pgm_read_float_near(addr)
-#define pgm_read_word(addr) pgm_read_word_near(addr)
-#define pgm_read_dword(addr) pgm_read_dword_near(addr)
-
-#define memcpy_P memcpy
-#define sprintf_P sprintf
-#define strstr_P strstr
-#define strncpy_P strncpy
-#define vsnprintf_P vsnprintf
-#define strcpy_P strcpy
-#define snprintf_P snprintf
-#define strlen_P strlen
-#define strchr_P strchr
-
-// Time functions
-extern "C" {
- void delay(const int milis);
-}
-void _delay_ms(const int delay);
-void delayMicroseconds(unsigned long);
-uint32_t millis();
-
-//IO functions
-void pinMode(const pin_t, const uint8_t);
-void digitalWrite(pin_t, uint8_t);
-bool digitalRead(pin_t);
-void analogWrite(pin_t, int);
-uint16_t analogRead(pin_t);
-
-// EEPROM
-void eeprom_write_byte(uint8_t *pos, unsigned char value);
-uint8_t eeprom_read_byte(uint8_t *pos);
-void eeprom_read_block (void *__dst, const void *__src, size_t __n);
-void eeprom_update_block (const void *__src, void *__dst, size_t __n);
-
-int32_t random(int32_t);
-int32_t random(int32_t, int32_t);
-void randomSeed(uint32_t);
-
-char *dtostrf (double __val, signed char __width, unsigned char __prec, char *__s);
-
-int map(uint16_t x, uint16_t in_min, uint16_t in_max, uint16_t out_min, uint16_t out_max);
-
-#endif // __ARDUINO_DEF_H__
diff --git a/Marlin/src/HAL/HAL_LPC1768/include/HardwareSerial.cpp b/Marlin/src/HAL/HAL_LPC1768/include/HardwareSerial.cpp
deleted file mode 100644
index 152b045d55..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/include/HardwareSerial.cpp
+++ /dev/null
@@ -1,335 +0,0 @@
-/**
- * Marlin 3D Printer Firmware
- * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
- *
- * Based on Sprinter and grbl.
- * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see .
- *
- */
-
-#ifdef TARGET_LPC1768
-
-#include "HardwareSerial.h"
-
-#if SERIAL_PORT == 0 || SERIAL_PORT_2 == 0
- HardwareSerial Serial = HardwareSerial(LPC_UART0);
-#elif SERIAL_PORT == 1 || SERIAL_PORT_2 == 1
- HardwareSerial Serial1 = HardwareSerial((LPC_UART_TypeDef *) LPC_UART1);
-#elif SERIAL_PORT == 2 || SERIAL_PORT_2 == 2
- HardwareSerial Serial2 = HardwareSerial(LPC_UART2);
-#elif SERIAL_PORT == 3 || SERIAL_PORT_2 == 3
- HardwareSerial Serial3 = HardwareSerial(LPC_UART3);
-#endif
-
-void HardwareSerial::begin(uint32_t baudrate) {
-
- UART_CFG_Type UARTConfigStruct;
- PINSEL_CFG_Type PinCfg;
- UART_FIFO_CFG_Type FIFOConfig;
-
- if (Baudrate == baudrate) return; // No need to re-initialize
-
- if (UARTx == LPC_UART0) {
- // Initialize UART0 pin connect
- PinCfg.Funcnum = 1;
- PinCfg.OpenDrain = 0;
- PinCfg.Pinmode = 0;
- PinCfg.Pinnum = 2;
- PinCfg.Portnum = 0;
- PINSEL_ConfigPin(&PinCfg);
- PinCfg.Pinnum = 3;
- PINSEL_ConfigPin(&PinCfg);
- } else if ((LPC_UART1_TypeDef *) UARTx == LPC_UART1) {
- // Initialize UART1 pin connect
- PinCfg.Funcnum = 1;
- PinCfg.OpenDrain = 0;
- PinCfg.Pinmode = 0;
- PinCfg.Pinnum = 15;
- PinCfg.Portnum = 0;
- PINSEL_ConfigPin(&PinCfg);
- PinCfg.Pinnum = 16;
- PINSEL_ConfigPin(&PinCfg);
- } else if (UARTx == LPC_UART2) {
- // Initialize UART2 pin connect
- PinCfg.Funcnum = 1;
- PinCfg.OpenDrain = 0;
- PinCfg.Pinmode = 0;
- PinCfg.Pinnum = 10;
- PinCfg.Portnum = 0;
- PINSEL_ConfigPin(&PinCfg);
- PinCfg.Pinnum = 11;
- PINSEL_ConfigPin(&PinCfg);
- } else if (UARTx == LPC_UART3) {
- // Initialize UART2 pin connect
- PinCfg.Funcnum = 1;
- PinCfg.OpenDrain = 0;
- PinCfg.Pinmode = 0;
- PinCfg.Pinnum = 0;
- PinCfg.Portnum = 0;
- PINSEL_ConfigPin(&PinCfg);
- PinCfg.Pinnum = 1;
- PINSEL_ConfigPin(&PinCfg);
- }
-
- /* Initialize UART Configuration parameter structure to default state:
- * Baudrate = 9600bps
- * 8 data bit
- * 1 Stop bit
- * None parity
- */
- UART_ConfigStructInit(&UARTConfigStruct);
-
- // Re-configure baudrate
- UARTConfigStruct.Baud_rate = baudrate;
-
- // Initialize eripheral with given to corresponding parameter
- UART_Init(UARTx, &UARTConfigStruct);
-
- // Enable and reset the TX and RX FIFOs
- UART_FIFOConfigStructInit(&FIFOConfig);
- UART_FIFOConfig(UARTx, &FIFOConfig);
-
- // Enable UART Transmit
- UART_TxCmd(UARTx, ENABLE);
-
- // Configure Interrupts
- UART_IntConfig(UARTx, UART_INTCFG_RBR, ENABLE);
- UART_IntConfig(UARTx, UART_INTCFG_RLS, ENABLE);
-
- // Set proper priority and enable interrupts
- if (UARTx == LPC_UART0) {
- NVIC_SetPriority(UART0_IRQn, NVIC_EncodePriority(0, 3, 0));
- NVIC_EnableIRQ(UART0_IRQn);
- }
- else if ((LPC_UART1_TypeDef *) UARTx == LPC_UART1) {
- NVIC_SetPriority(UART1_IRQn, NVIC_EncodePriority(0, 3, 0));
- NVIC_EnableIRQ(UART1_IRQn);
- }
- else if (UARTx == LPC_UART2) {
- NVIC_SetPriority(UART2_IRQn, NVIC_EncodePriority(0, 3, 0));
- NVIC_EnableIRQ(UART2_IRQn);
- }
- else if (UARTx == LPC_UART3) {
- NVIC_SetPriority(UART3_IRQn, NVIC_EncodePriority(0, 3, 0));
- NVIC_EnableIRQ(UART3_IRQn);
- }
-
- RxQueueWritePos = RxQueueReadPos = 0;
- #if TX_BUFFER_SIZE > 0
- TxQueueWritePos = TxQueueReadPos = 0;
- #endif
-
- // Save the configured baudrate
- Baudrate = baudrate;
-}
-
-int16_t HardwareSerial::peek() {
- int16_t byte = -1;
-
- // Temporarily lock out UART receive interrupts during this read so the UART receive
- // interrupt won't cause problems with the index values
- UART_IntConfig(UARTx, UART_INTCFG_RBR, DISABLE);
-
- if (RxQueueReadPos != RxQueueWritePos)
- byte = RxBuffer[RxQueueReadPos];
-
- // Re-enable UART interrupts
- UART_IntConfig(UARTx, UART_INTCFG_RBR, ENABLE);
-
- return byte;
-}
-
-int16_t HardwareSerial::read() {
- int16_t byte = -1;
-
- // Temporarily lock out UART receive interrupts during this read so the UART receive
- // interrupt won't cause problems with the index values
- UART_IntConfig(UARTx, UART_INTCFG_RBR, DISABLE);
-
- if (RxQueueReadPos != RxQueueWritePos) {
- byte = RxBuffer[RxQueueReadPos];
- RxQueueReadPos = (RxQueueReadPos + 1) % RX_BUFFER_SIZE;
- }
-
- // Re-enable UART interrupts
- UART_IntConfig(UARTx, UART_INTCFG_RBR, ENABLE);
-
- return byte;
-}
-
-size_t HardwareSerial::write(uint8_t send) {
-#if TX_BUFFER_SIZE > 0
- size_t bytes = 0;
- uint32_t fifolvl = 0;
-
- // If the Tx Buffer is full, wait for space to clear
- if ((TxQueueWritePos+1) % TX_BUFFER_SIZE == TxQueueReadPos) flushTX();
-
- // Temporarily lock out UART transmit interrupts during this read so the UART transmit interrupt won't
- // cause problems with the index values
- UART_IntConfig(UARTx, UART_INTCFG_THRE, DISABLE);
-
- // LPC17xx.h incorrectly defines FIFOLVL as a uint8_t, when it's actually a 32-bit register
- if ((LPC_UART1_TypeDef *) UARTx == LPC_UART1) {
- fifolvl = *(reinterpret_cast(&((LPC_UART1_TypeDef *) UARTx)->FIFOLVL));
- } else fifolvl = *(reinterpret_cast(&UARTx->FIFOLVL));
-
- // If the queue is empty and there's space in the FIFO, immediately send the byte
- if (TxQueueWritePos == TxQueueReadPos && fifolvl < UART_TX_FIFO_SIZE) {
- bytes = UART_Send(UARTx, &send, 1, BLOCKING);
- }
- // Otherwiise, write the byte to the transmit buffer
- else if ((TxQueueWritePos+1) % TX_BUFFER_SIZE != TxQueueReadPos) {
- TxBuffer[TxQueueWritePos] = send;
- TxQueueWritePos = (TxQueueWritePos+1) % TX_BUFFER_SIZE;
- bytes++;
- }
-
- // Re-enable the TX Interrupt
- UART_IntConfig(UARTx, UART_INTCFG_THRE, ENABLE);
-
- return bytes;
-#else
- return UART_Send(UARTx, &send, 1, BLOCKING);
-#endif
-}
-
-#if TX_BUFFER_SIZE > 0
- void HardwareSerial::flushTX() {
- // Wait for the tx buffer and FIFO to drain
- while (TxQueueWritePos != TxQueueReadPos && UART_CheckBusy(UARTx) == SET);
- }
-#endif
-
-size_t HardwareSerial::available() {
- return (RxQueueWritePos + RX_BUFFER_SIZE - RxQueueReadPos) % RX_BUFFER_SIZE;
-}
-
-void HardwareSerial::flush() {
- RxQueueWritePos = 0;
- RxQueueReadPos = 0;
-}
-
-size_t HardwareSerial::printf(const char *format, ...) {
- char RxBuffer[256];
- va_list vArgs;
- va_start(vArgs, format);
- int length = vsnprintf(RxBuffer, 256, format, vArgs);
- va_end(vArgs);
- if (length > 0 && length < 256) {
- for (size_t i = 0; i < (size_t)length; ++i)
- write(RxBuffer[i]);
- }
- return length;
-}
-
-void HardwareSerial::IRQHandler() {
- uint32_t IIRValue;
- uint8_t LSRValue, byte;
-
- IIRValue = UART_GetIntId(UARTx);
- IIRValue &= UART_IIR_INTID_MASK; // check bit 1~3, interrupt identification
-
- // Receive Line Status
- if (IIRValue == UART_IIR_INTID_RLS) {
- LSRValue = UART_GetLineStatus(UARTx);
-
- // Receive Line Status
- if (LSRValue & (UART_LSR_OE | UART_LSR_PE | UART_LSR_FE | UART_LSR_RXFE | UART_LSR_BI)) {
- // There are errors or break interrupt
- // Read LSR will clear the interrupt
- Status = LSRValue;
- byte = UART_ReceiveByte(UARTx); // Dummy read on RX to clear interrupt, then bail out
- return;
- }
- }
-
- // Receive Data Available
- if (IIRValue == UART_IIR_INTID_RDA) {
- // Clear the FIFO
- while (UART_Receive(UARTx, &byte, 1, NONE_BLOCKING)) {
- #if ENABLED(EMERGENCY_PARSER)
- emergency_parser.update(emergency_state, byte);
- #endif
- if ((RxQueueWritePos + 1) % RX_BUFFER_SIZE != RxQueueReadPos) {
- RxBuffer[RxQueueWritePos] = byte;
- RxQueueWritePos = (RxQueueWritePos + 1) % RX_BUFFER_SIZE;
- } else
- break;
- }
- // Character timeout indicator
- } else if (IIRValue == UART_IIR_INTID_CTI) {
- // Character Time-out indicator
- Status |= 0x100; // Bit 9 as the CTI error
- }
-
- #if TX_BUFFER_SIZE > 0
- if (IIRValue == UART_IIR_INTID_THRE) {
- // Disable THRE interrupt
- UART_IntConfig(UARTx, UART_INTCFG_THRE, DISABLE);
-
- // Wait for FIFO buffer empty
- while (UART_CheckBusy(UARTx) == SET);
-
- // Transfer up to UART_TX_FIFO_SIZE bytes of data
- for (int i = 0; i < UART_TX_FIFO_SIZE && TxQueueWritePos != TxQueueReadPos; i++) {
- // Move a piece of data into the transmit FIFO
- if (UART_Send(UARTx, &TxBuffer[TxQueueReadPos], 1, NONE_BLOCKING)) {
- TxQueueReadPos = (TxQueueReadPos+1) % TX_BUFFER_SIZE;
- } else break;
- }
-
- // If there is no more data to send, disable the transmit interrupt - else enable it or keep it enabled
- if (TxQueueWritePos == TxQueueReadPos) {
- UART_IntConfig(UARTx, UART_INTCFG_THRE, DISABLE);
- } else UART_IntConfig(UARTx, UART_INTCFG_THRE, ENABLE);
- }
- #endif
-}
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void UART0_IRQHandler(void) {
- #if SERIAL_PORT == 0 || SERIAL_PORT_2 == 0
- Serial.IRQHandler();
- #endif
-}
-
-void UART1_IRQHandler(void) {
- #if SERIAL_PORT == 1 || SERIAL_PORT_2 == 1
- Serial1.IRQHandler();
- #endif
-}
-
-void UART2_IRQHandler(void) {
- #if SERIAL_PORT == 2 || SERIAL_PORT_2 == 2
- Serial2.IRQHandler();
- #endif
-}
-
-void UART3_IRQHandler(void) {
- #if SERIAL_PORT == 3 || SERIAL_PORT_2 == 3
- Serial3.IRQHandler();
- #endif
-}
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // TARGET_LPC1768
diff --git a/Marlin/src/HAL/HAL_LPC1768/include/HardwareSerial.h b/Marlin/src/HAL/HAL_LPC1768/include/HardwareSerial.h
deleted file mode 100644
index f3bd2f6b42..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/include/HardwareSerial.h
+++ /dev/null
@@ -1,91 +0,0 @@
-/**
- * Marlin 3D Printer Firmware
- * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
- *
- * Based on Sprinter and grbl.
- * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see .
- *
- */
-
-#ifndef HARDWARE_SERIAL_H_
-#define HARDWARE_SERIAL_H_
-
-#include "../../../inc/MarlinConfigPre.h"
-#if ENABLED(EMERGENCY_PARSER)
- #include "../../../feature/emergency_parser.h"
-#endif
-
-#include
-#include
-#include
-
-extern "C" {
- #include
- #include "lpc17xx_pinsel.h"
-}
-
-class HardwareSerial : public Stream {
-private:
- LPC_UART_TypeDef *UARTx;
-
- uint32_t Baudrate;
- uint32_t Status;
- uint8_t RxBuffer[RX_BUFFER_SIZE];
- uint32_t RxQueueWritePos;
- uint32_t RxQueueReadPos;
- #if TX_BUFFER_SIZE > 0
- uint8_t TxBuffer[TX_BUFFER_SIZE];
- uint32_t TxQueueWritePos;
- uint32_t TxQueueReadPos;
- #endif
- #if ENABLED(EMERGENCY_PARSER)
- EmergencyParser::State emergency_state;
- #endif
-
-public:
- HardwareSerial(LPC_UART_TypeDef *UARTx)
- : UARTx(UARTx)
- , Baudrate(0)
- , RxQueueWritePos(0)
- , RxQueueReadPos(0)
- #if TX_BUFFER_SIZE > 0
- , TxQueueWritePos(0)
- , TxQueueReadPos(0)
- #endif
- #if ENABLED(EMERGENCY_PARSER)
- , emergency_state(EmergencyParser::State::EP_RESET)
- #endif
- {
- }
-
- void begin(uint32_t baudrate);
- int16_t peek();
- int16_t read();
- size_t write(uint8_t send);
- #if TX_BUFFER_SIZE > 0
- void flushTX();
- #endif
- size_t available();
- void flush();
- size_t printf(const char *format, ...);
-
- operator bool() { return true; }
-
- void IRQHandler();
-
-};
-
-#endif // MARLIN_SRC_HAL_HAL_SERIAL_H_
diff --git a/Marlin/src/HAL/HAL_LPC1768/include/SoftwareSerial.cpp b/Marlin/src/HAL/HAL_LPC1768/include/SoftwareSerial.cpp
deleted file mode 100644
index 1d469dab3a..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/include/SoftwareSerial.cpp
+++ /dev/null
@@ -1,329 +0,0 @@
-/*
- * SoftwareSerial.cpp (formerly NewSoftSerial.cpp)
- *
- * Multi-instance software serial library for Arduino/Wiring
- * -- Interrupt-driven receive and other improvements by ladyada
- * (http://ladyada.net)
- * -- Tuning, circular buffer, derivation from class Print/Stream,
- * multi-instance support, porting to 8MHz processors,
- * various optimizations, PROGMEM delay tables, inverse logic and
- * direct port writing by Mikal Hart (http://www.arduiniana.org)
- * -- Pin change interrupt macros by Paul Stoffregen (http://www.pjrc.com)
- * -- 20MHz processor support by Garrett Mace (http://www.macetech.com)
- * -- ATmega1280/2560 support by Brett Hagman (http://www.roguerobotics.com/)
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
- *
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- *
- * The latest version of this library can always be found at
- * http://arduiniana.org.
- */
-
-#ifdef TARGET_LPC1768
-
-//
-// Includes
-//
-//#include
-#include "../../../inc/MarlinConfig.h"
-#include "../../shared/Delay.h"
-#include
-#include
-#include
-#include
-#include "../fastio.h"
-#include "SoftwareSerial.h"
-
-void GpioEnableInt(uint32_t port, uint32_t pin, uint32_t mode);
-void GpioDisableInt(uint32_t port, uint32_t pin);
-//
-// Statics
-//
-SoftwareSerial *SoftwareSerial::active_object = 0;
-unsigned char SoftwareSerial::_receive_buffer[_SS_MAX_RX_BUFF];
-volatile uint8_t SoftwareSerial::_receive_buffer_tail = 0;
-volatile uint8_t SoftwareSerial::_receive_buffer_head = 0;
-
-typedef struct _DELAY_TABLE {
- long baud;
- uint16_t rx_delay_centering;
- uint16_t rx_delay_intrabit;
- uint16_t rx_delay_stopbit;
- uint16_t tx_delay;
-} DELAY_TABLE;
-
-// rough delay estimation
-static const DELAY_TABLE table[] = {
- //baud |rxcenter|rxintra |rxstop |tx { 250000, 2, 4, 4, 4, }, //Done but not good due to instruction cycle error { 115200, 4, 8, 8, 8, }, //Done but not good due to instruction cycle error
- //{ 74880, 69, 139, 62, 162, }, // estimation
- //{ 57600, 100, 185, 1, 208, }, // Done but not good due to instruction cycle error
- //{ 38400, 13, 26, 26, 26, }, // Done
- //{ 19200, 26, 52, 52, 52, }, // Done { 9600, 52, 104, 104, 104, }, // Done
- //{ 4800, 104, 208, 208, 208, },
- //{ 2400, 208, 417, 417, 417, },
- //{ 1200, 416, 833, 833, 833,},
-};
-
-//
-// Private methods
-//
-
-inline void SoftwareSerial::tunedDelay(const uint32_t count) {
- DELAY_US(count);
-}
-
-// This function sets the current object as the "listening"
-// one and returns true if it replaces another
-bool SoftwareSerial::listen() {
- if (!_rx_delay_stopbit)
- return false;
-
- if (active_object != this) {
- if (active_object)
- active_object->stopListening();
-
- _buffer_overflow = false;
- _receive_buffer_head = _receive_buffer_tail = 0;
- active_object = this;
-
- setRxIntMsk(true);
- return true;
- }
-
- return false;
-}
-
-// Stop listening. Returns true if we were actually listening.
-bool SoftwareSerial::stopListening() {
- if (active_object == this) {
- setRxIntMsk(false);
- active_object = NULL;
- return true;
- }
- return false;
-}
-
-//
-// The receive routine called by the interrupt handler
-//
-void SoftwareSerial::recv() {
- uint8_t d = 0;
-
- // If RX line is high, then we don't see any start bit
- // so interrupt is probably not for us
- if (_inverse_logic ? rx_pin_read() : !rx_pin_read()) {
- // Disable further interrupts during reception, this prevents
- // triggering another interrupt directly after we return, which can
- // cause problems at higher baudrates.
- setRxIntMsk(false);//__disable_irq();//
-
- // Wait approximately 1/2 of a bit width to "center" the sample
- tunedDelay(_rx_delay_centering);
- // Read each of the 8 bits
- for (uint8_t i=8; i > 0; --i) {
- tunedDelay(_rx_delay_intrabit);
- d >>= 1;
- if (rx_pin_read()) d |= 0x80;
- }
-
- if (_inverse_logic) d = ~d;
-
- // if buffer full, set the overflow flag and return
- uint8_t next = (_receive_buffer_tail + 1) % _SS_MAX_RX_BUFF;
- if (next != _receive_buffer_head) {
- // save new data in buffer: tail points to where byte goes
- _receive_buffer[_receive_buffer_tail] = d; // save new byte
- _receive_buffer_tail = next;
- }
- else {
- _buffer_overflow = true;
- }
- tunedDelay(_rx_delay_stopbit);
- // Re-enable interrupts when we're sure to be inside the stop bit
- setRxIntMsk(true); //__enable_irq();//
- }
-}
-
-uint32_t SoftwareSerial::rx_pin_read() {
- return digitalRead(_receivePin);
-}
-
-//
-// Interrupt handling
-//
-
-/* static */
-inline void SoftwareSerial::handle_interrupt() {
- if (active_object)
- active_object->recv();
-}
-extern "C" void intWrapper() {
- SoftwareSerial::handle_interrupt();
-}
-//
-// Constructor
-//
-SoftwareSerial::SoftwareSerial(pin_t receivePin, pin_t transmitPin, bool inverse_logic /* = false */) :
- _rx_delay_centering(0),
- _rx_delay_intrabit(0),
- _rx_delay_stopbit(0),
- _tx_delay(0),
- _buffer_overflow(false),
- _inverse_logic(inverse_logic) {
- setTX(transmitPin);
- setRX(receivePin);
-}
-
-//
-// Destructor
-//
-SoftwareSerial::~SoftwareSerial() {
- end();
-}
-
-void SoftwareSerial::setTX(pin_t tx) {
- // First write, then set output. If we do this the other way around,
- // the pin would be output low for a short while before switching to
- // output hihg. Now, it is input with pullup for a short while, which
- // is fine. With inverse logic, either order is fine.
-
- digitalWrite(tx, _inverse_logic ? LOW : HIGH);
- pinMode(tx,OUTPUT);
- _transmitPin = tx;
-}
-
-void SoftwareSerial::setRX(pin_t rx) {
- pinMode(rx, INPUT_PULLUP); // pullup for normal logic!
- //if (!_inverse_logic)
- // digitalWrite(rx, HIGH);
- _receivePin = rx;
- _receivePort = LPC1768_PIN_PORT(rx);
- _receivePortPin = LPC1768_PIN_PIN(rx);
- /* GPIO_T * rxPort = digitalPinToPort(rx);
- _receivePortRegister = portInputRegister(rxPort);
- _receiveBitMask = digitalPinToBitMask(rx);*/
-}
-
-//
-// Public methods
-//
-
-void SoftwareSerial::begin(long speed) {
- _rx_delay_centering = _rx_delay_intrabit = _rx_delay_stopbit = _tx_delay = 0;
-
- for(uint8_t i = 0; i < sizeof(table)/sizeof(table[0]); ++i) {
- long baud = table[i].baud;
- if (baud == speed) {
- _rx_delay_centering = table[i].rx_delay_centering;
- _rx_delay_intrabit = table[i].rx_delay_intrabit;
- _rx_delay_stopbit = table[i].rx_delay_stopbit;
- _tx_delay = table[i].tx_delay;
- break;
- }
- }
-
- attachInterrupt(_receivePin, intWrapper, CHANGE); //this->handle_interrupt, CHANGE);
-
- listen();
- tunedDelay(_tx_delay);
-
-}
-
-void SoftwareSerial::setRxIntMsk(bool enable) {
- if (enable)
- GpioEnableInt(_receivePort,_receivePin,CHANGE);
- else
- GpioDisableInt(_receivePort,_receivePin);
-}
-
-void SoftwareSerial::end() {
- stopListening();
-}
-
-
-// Read data from buffer
-int16_t SoftwareSerial::read() {
- if (!isListening()) return -1;
-
- // Empty buffer?
- if (_receive_buffer_head == _receive_buffer_tail) return -1;
-
- // Read from "head"
- uint8_t d = _receive_buffer[_receive_buffer_head]; // grab next byte
- _receive_buffer_head = (_receive_buffer_head + 1) % _SS_MAX_RX_BUFF;
- return d;
-}
-
-size_t SoftwareSerial::available() {
- if (!isListening()) return 0;
-
- return (_receive_buffer_tail + _SS_MAX_RX_BUFF - _receive_buffer_head) % _SS_MAX_RX_BUFF;
-}
-
-size_t SoftwareSerial::write(uint8_t b) {
- // By declaring these as local variables, the compiler will put them
- // in registers _before_ disabling interrupts and entering the
- // critical timing sections below, which makes it a lot easier to
- // verify the cycle timings
-
- bool inv = _inverse_logic;
- uint16_t delay = _tx_delay;
-
- if (inv) b = ~b;
-
- cli(); // turn off interrupts for a clean txmit
-
- // Write the start bit
- digitalWrite(_transmitPin, !!inv);
-
- tunedDelay(delay);
-
- // Write each of the 8 bits
- for (uint8_t i = 8; i > 0; --i) {
- digitalWrite(_transmitPin, b & 1); // send 1 //(GPIO_Desc[_transmitPin].P)->DOUT |= GPIO_Desc[_transmitPin].bit;
- // send 0 //(GPIO_Desc[_transmitPin].P)->DOUT &= ~GPIO_Desc[_transmitPin].bit;
- tunedDelay(delay);
- b >>= 1;
- }
-
- // restore pin to natural state
- digitalWrite(_transmitPin, !inv);
-
- sei(); // turn interrupts back on
- tunedDelay(delay);
-
- return 1;
-}
-
-void SoftwareSerial::flush() {
- if (!isListening()) return;
-
- cli();
- _receive_buffer_head = _receive_buffer_tail = 0;
- sei();
-}
-
-int16_t SoftwareSerial::peek() {
- if (!isListening())
- return -1;
-
- // Empty buffer?
- if (_receive_buffer_head == _receive_buffer_tail)
- return -1;
-
- // Read from "head"
- return _receive_buffer[_receive_buffer_head];
-}
-
-#endif // TARGET_LPC1768
diff --git a/Marlin/src/HAL/HAL_LPC1768/include/SoftwareSerial.h b/Marlin/src/HAL/HAL_LPC1768/include/SoftwareSerial.h
deleted file mode 100644
index f208e14b05..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/include/SoftwareSerial.h
+++ /dev/null
@@ -1,120 +0,0 @@
-/*
- * SoftwareSerial.h (formerly NewSoftSerial.h)
- *
- * Multi-instance software serial library for Arduino/Wiring
- * -- Interrupt-driven receive and other improvements by ladyada
- * (http://ladyada.net)
- * -- Tuning, circular buffer, derivation from class Print/Stream,
- * multi-instance support, porting to 8MHz processors,
- * various optimizations, PROGMEM delay tables, inverse logic and
- * direct port writing by Mikal Hart (http://www.arduiniana.org)
- * -- Pin change interrupt macros by Paul Stoffregen (http://www.pjrc.com)
- * -- 20MHz processor support by Garrett Mace (http://www.macetech.com)
- * -- ATmega1280/2560 support by Brett Hagman (http://www.roguerobotics.com/)
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
- *
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- *
- * The latest version of this library can always be found at
- * http://arduiniana.org.
- */
-
-#ifndef SOFTWARESERIAL_H
-#define SOFTWARESERIAL_H
-
-#include
-#include
-//#include "serial.h"
-#include
-#include
-
-/******************************************************************************
-* Definitions
-******************************************************************************/
-
-#define _SS_MAX_RX_BUFF 64 // RX buffer size
-
-class SoftwareSerial : public Stream
-{
-private:
- // per object data
- pin_t _receivePin;
- pin_t _transmitPin;
-// uint32_t _receiveBitMask; // for rx interrupts
- uint32_t _receivePort;
- uint32_t _receivePortPin;
-
-
- // Expressed as 4-cycle delays (must never be 0!)
- uint16_t _rx_delay_centering;
- uint16_t _rx_delay_intrabit;
- uint16_t _rx_delay_stopbit;
- uint16_t _tx_delay;
-
- uint16_t _buffer_overflow:1;
- uint16_t _inverse_logic:1;
-
- // static data
- static unsigned char _receive_buffer[_SS_MAX_RX_BUFF];
- static volatile uint8_t _receive_buffer_tail;
- static volatile uint8_t _receive_buffer_head;
- static SoftwareSerial *active_object;
-
- // private methods
- void recv();
- uint32_t rx_pin_read();
- void tx_pin_write(uint8_t pin_state);
- void setTX(pin_t transmitPin);
- void setRX(pin_t receivePin);
- void setRxIntMsk(bool enable);
-
- // private static method for timing
- static inline void tunedDelay(uint32_t delay);
-
-public:
- // public methods
-
- SoftwareSerial(pin_t receivePin, pin_t transmitPin, bool inverse_logic = false);
- ~SoftwareSerial();
- void begin(long speed);
- bool listen();
- void end();
- bool isListening() { return this == active_object; }
- bool stopListening();
- bool overflow() { bool ret = _buffer_overflow; if (ret) _buffer_overflow = false; return ret; }
- int16_t peek();
-
- virtual size_t write(uint8_t byte);
- virtual int16_t read();
- virtual size_t available();
- virtual void flush();
- operator bool() { return true; }
-
- using Print::write;
- //using HalSerial::write;
-
- // public only for easy access by interrupt handlers
- static inline void handle_interrupt() __attribute__((__always_inline__));
-};
-
-// Arduino 0012 workaround
-#undef int
-#undef char
-#undef long
-#undef byte
-#undef float
-#undef abs
-#undef round
-
-#endif // SOFTWARESERIAL_H
diff --git a/Marlin/src/HAL/HAL_LPC1768/include/Wire.cpp b/Marlin/src/HAL/HAL_LPC1768/include/Wire.cpp
deleted file mode 100644
index 28e1fa9b56..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/include/Wire.cpp
+++ /dev/null
@@ -1,219 +0,0 @@
-/*
- TwoWire.cpp - TWI/I2C library for Wiring & Arduino
- Copyright (c) 2006 Nicholas Zambetti. All right reserved.
-
- This library is free software; you can redistribute it and/or
- modify it under the terms of the GNU Lesser General Public
- License as published by the Free Software Foundation; either
- version 2.1 of the License, or (at your option) any later version.
-
- This library is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with this library; if not, write to the Free Software
- Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
-*/
-
-#ifdef TARGET_LPC1768
-
-extern "C" {
- #include
- #include
- #include
- #include
- #include
- #include
-}
-
-#include
-
-#define USEDI2CDEV_M 1
-
-#if (USEDI2CDEV_M == 0)
- #define I2CDEV_M LPC_I2C0
-#elif (USEDI2CDEV_M == 1)
- #define I2CDEV_M LPC_I2C1
-#elif (USEDI2CDEV_M == 2)
- #define I2CDEV_M LPC_I2C2
-#else
- #error "Master I2C device not defined!"
-#endif
-
-// Initialize Class Variables //////////////////////////////////////////////////
-
-uint8_t TwoWire::rxBuffer[BUFFER_LENGTH];
-uint8_t TwoWire::rxBufferIndex = 0;
-uint8_t TwoWire::rxBufferLength = 0;
-
-uint8_t TwoWire::txAddress = 0;
-uint8_t TwoWire::txBuffer[BUFFER_LENGTH];
-uint8_t TwoWire::txBufferIndex = 0;
-uint8_t TwoWire::txBufferLength = 0;
-
-uint8_t TwoWire::transmitting = 0;
-
-// Constructors ////////////////////////////////////////////////////////////////
-
-TwoWire::TwoWire() {
-}
-
-// Public Methods //////////////////////////////////////////////////////////////
-
-void TwoWire::begin(void) {
- rxBufferIndex = 0;
- rxBufferLength = 0;
-
- txBufferIndex = 0;
- txBufferLength = 0;
-
- /*
- * Init I2C pin connect
- */
- PINSEL_CFG_Type PinCfg;
- PinCfg.OpenDrain = 0;
- PinCfg.Pinmode = 0;
-
- #if USEDI2CDEV_M == 0
- PinCfg.Funcnum = 1;
- PinCfg.Pinnum = 27;
- PinCfg.Portnum = 0;
- PINSEL_ConfigPin(&PinCfg); // SDA0 / D57 AUX-1
- PinCfg.Pinnum = 28;
- PINSEL_ConfigPin(&PinCfg); // SCL0 / D58 AUX-1
- #endif
-
- #if USEDI2CDEV_M == 1
- PinCfg.Funcnum = 3;
- PinCfg.Pinnum = 0;
- PinCfg.Portnum = 0;
- PINSEL_ConfigPin(&PinCfg); // SDA1 / D20 SCA
- PinCfg.Pinnum = 1;
- PINSEL_ConfigPin(&PinCfg); // SCL1 / D21 SCL
- #endif
-
- #if USEDI2CDEV_M == 2
- PinCfg.Funcnum = 2;
- PinCfg.Pinnum = 10;
- PinCfg.Portnum = 0;
- PINSEL_ConfigPin(&PinCfg); // SDA2 / D38 X_ENABLE_PIN
- PinCfg.Pinnum = 11;
- PINSEL_ConfigPin(&PinCfg); // SCL2 / D55 X_DIR_PIN
- #endif
-
- // Initialize I2C peripheral
- I2C_Init(I2CDEV_M, 100000);
-
- // Enable Master I2C operation
- I2C_Cmd(I2CDEV_M, I2C_MASTER_MODE, ENABLE);
-}
-
-uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity) {
- // clamp to buffer length
- if (quantity > BUFFER_LENGTH)
- quantity = BUFFER_LENGTH;
-
- // perform blocking read into buffer
- I2C_M_SETUP_Type transferMCfg;
- transferMCfg.sl_addr7bit = address >> 1; // not sure about the right shift
- transferMCfg.tx_data = NULL;
- transferMCfg.tx_length = 0;
- transferMCfg.rx_data = rxBuffer;
- transferMCfg.rx_length = quantity;
- transferMCfg.retransmissions_max = 3;
- I2C_MasterTransferData(I2CDEV_M, &transferMCfg, I2C_TRANSFER_POLLING);
-
- // set rx buffer iterator vars
- rxBufferIndex = 0;
- rxBufferLength = transferMCfg.rx_count;
-
- return transferMCfg.rx_count;
-}
-
-uint8_t TwoWire::requestFrom(int address, int quantity) {
- return requestFrom((uint8_t)address, (uint8_t)quantity);
-}
-
-void TwoWire::beginTransmission(uint8_t address) {
- // indicate that we are transmitting
- transmitting = 1;
- // set address of targeted slave
- txAddress = address;
- // reset tx buffer iterator vars
- txBufferIndex = 0;
- txBufferLength = 0;
-}
-
-void TwoWire::beginTransmission(int address) {
- beginTransmission((uint8_t)address);
-}
-
-uint8_t TwoWire::endTransmission(void) {
- // transmit buffer (blocking)
- I2C_M_SETUP_Type transferMCfg;
- transferMCfg.sl_addr7bit = txAddress >> 1; // not sure about the right shift
- transferMCfg.tx_data = txBuffer;
- transferMCfg.tx_length = txBufferLength;
- transferMCfg.rx_data = NULL;
- transferMCfg.rx_length = 0;
- transferMCfg.retransmissions_max = 3;
- Status status = I2C_MasterTransferData(I2CDEV_M, &transferMCfg, I2C_TRANSFER_POLLING);
-
- // reset tx buffer iterator vars
- txBufferIndex = 0;
- txBufferLength = 0;
-
- // indicate that we are done transmitting
- transmitting = 0;
-
- return status == SUCCESS ? 0 : 4;
-}
-
-// must be called after beginTransmission(address)
-size_t TwoWire::write(uint8_t data) {
- if (transmitting) {
- // don't bother if buffer is full
- if (txBufferLength >= BUFFER_LENGTH) return 0;
-
- // put byte in tx buffer
- txBuffer[txBufferIndex++] = data;
-
- // update amount in buffer
- txBufferLength = txBufferIndex;
- }
-
- return 1;
-}
-
-// must be called after beginTransmission(address)
-size_t TwoWire::write(const uint8_t *data, size_t quantity) {
- size_t sent = 0;
- if (transmitting)
- for (sent = 0; sent < quantity; ++sent)
- if (!write(data[sent])) break;
-
- return sent;
-}
-
-// Must be called after requestFrom(address, numBytes)
-int TwoWire::available(void) {
- return rxBufferLength - rxBufferIndex;
-}
-
-// Must be called after requestFrom(address, numBytes)
-int TwoWire::read(void) {
- return rxBufferIndex < rxBufferLength ? rxBuffer[rxBufferIndex++] : -1;
-}
-
-// Must be called after requestFrom(address, numBytes)
-int TwoWire::peek(void) {
- return rxBufferIndex < rxBufferLength ? rxBuffer[rxBufferIndex] : -1;
-}
-
-// Preinstantiate Objects //////////////////////////////////////////////////////
-
-TwoWire Wire = TwoWire();
-
-#endif // TARGET_LPC1768
\ No newline at end of file
diff --git a/Marlin/src/HAL/HAL_LPC1768/include/Wire.h b/Marlin/src/HAL/HAL_LPC1768/include/Wire.h
deleted file mode 100644
index b833d699e4..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/include/Wire.h
+++ /dev/null
@@ -1,67 +0,0 @@
-/**
- * TwoWire.h - TWI/I2C library for Arduino & Wiring
- * Copyright (c) 2006 Nicholas Zambetti. All right reserved.
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
- *
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- *
- * Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts
- */
-
-#ifndef _TWOWIRE_H_
-#define _TWOWIRE_H_
-
-#include
-
-#define BUFFER_LENGTH 32
-
-class TwoWire {
- private:
- static uint8_t rxBuffer[];
- static uint8_t rxBufferIndex;
- static uint8_t rxBufferLength;
-
- static uint8_t txAddress;
- static uint8_t txBuffer[];
- static uint8_t txBufferIndex;
- static uint8_t txBufferLength;
-
- static uint8_t transmitting;
-
- public:
- TwoWire();
- void begin();
- void beginTransmission(uint8_t);
- void beginTransmission(int);
- uint8_t endTransmission(void);
- uint8_t endTransmission(uint8_t);
-
- uint8_t requestFrom(uint8_t, uint8_t);
- uint8_t requestFrom(int, int);
-
- virtual size_t write(uint8_t);
- virtual size_t write(const uint8_t *, size_t);
- virtual int available(void);
- virtual int read(void);
- virtual int peek(void);
-
- inline size_t write(unsigned long n) { return write((uint8_t)n); }
- inline size_t write(long n) { return write((uint8_t)n); }
- inline size_t write(unsigned int n) { return write((uint8_t)n); }
- inline size_t write(int n) { return write((uint8_t)n); }
-};
-
-extern TwoWire Wire;
-
-#endif // _TWOWIRE_H_
diff --git a/Marlin/src/HAL/HAL_LPC1768/include/pinmapping.cpp b/Marlin/src/HAL/HAL_LPC1768/include/pinmapping.cpp
deleted file mode 100644
index f05fa00ace..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/include/pinmapping.cpp
+++ /dev/null
@@ -1,74 +0,0 @@
-/**
- * Marlin 3D Printer Firmware
- * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
- *
- * Based on Sprinter and grbl.
- * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see .
- *
- */
-
-#ifdef TARGET_LPC1768
-
-#include
-
-#include "../../../gcode/parser.h"
-
-// Get the digital pin for an analog index
-pin_t analogInputToDigitalPin(const int8_t p) {
- return (WITHIN(p, 0, NUM_ANALOG_INPUTS) ? adc_pin_table[p] : P_NC);
-}
-
-// Return the index of a pin number
-// The pin number given here is in the form ppp:nnnnn
-int16_t GET_PIN_MAP_INDEX(const pin_t pin) {
- const uint16_t index = (LPC1768_PIN_PORT(pin) << 5) | LPC1768_PIN_PIN(pin);
- return (index < NUM_DIGITAL_PINS && pin_map[index] != P_NC) ? index : -1;
-}
-
-// Test whether the pin is valid
-bool VALID_PIN(const pin_t p) {
- const int16_t ind = GET_PIN_MAP_INDEX(p);
- return ind >= 0 && pin_map[ind] >= 0;
-}
-
-// Get the analog index for a digital pin
-int8_t DIGITAL_PIN_TO_ANALOG_PIN(const pin_t p) {
- return (VALID_PIN(p) ? LPC1768_PIN_ADC(p) : -1);
-}
-
-// Test whether the pin is PWM
-bool PWM_PIN(const pin_t p) {
- return VALID_PIN(p) && LPC1768_PIN_PWM(p);
-}
-
-// Test whether the pin is interruptable
-bool INTERRUPT_PIN(const pin_t p) {
- return VALID_PIN(p) && LPC1768_PIN_INTERRUPT(p);
-}
-
-// Get the pin number at the given index
-pin_t GET_PIN_MAP_PIN(const int16_t ind) {
- return WITHIN(ind, 0, NUM_DIGITAL_PINS - 1) ? pin_map[ind] : P_NC;
-}
-
-int16_t PARSED_PIN_INDEX(const char code, const int16_t dval) {
- const uint16_t val = (uint16_t)parser.intval(code), port = val / 100, pin = val % 100;
- const int16_t ind = (port < (NUM_DIGITAL_PINS >> 5) && (pin < 32))
- ? GET_PIN_MAP_INDEX(port << 5 | pin) : -2;
- return ind > -2 ? ind : dval;
-}
-
-#endif // TARGET_LPC1768
diff --git a/Marlin/src/HAL/HAL_LPC1768/include/pinmapping.h b/Marlin/src/HAL/HAL_LPC1768/include/pinmapping.h
deleted file mode 100644
index de727b8bde..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/include/pinmapping.h
+++ /dev/null
@@ -1,294 +0,0 @@
-/**
- * Marlin 3D Printer Firmware
- * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
- *
- * Based on Sprinter and grbl.
- * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see .
- *
- */
-
-#ifndef _PINMAPPING_H_
-#define _PINMAPPING_H_
-
-#include "../../../inc/MarlinConfigPre.h"
-
-#include
-
-typedef int16_t pin_t;
-
-#define PORT_0 000
-#define PORT_1 001
-#define PORT_2 010
-#define PORT_3 011
-#define PORT_4 100
-
-#define PORT_(p) PORT_##p
-#define PORT(p) PORT_(p)
-
-#define PIN_0 00000
-#define PIN_1 00001
-#define PIN_2 00010
-#define PIN_3 00011
-#define PIN_4 00100
-#define PIN_5 00101
-#define PIN_6 00110
-#define PIN_7 00111
-#define PIN_8 01000
-#define PIN_9 01001
-#define PIN_10 01010
-#define PIN_11 01011
-#define PIN_12 01100
-#define PIN_13 01101
-#define PIN_14 01110
-#define PIN_15 01111
-#define PIN_16 10000
-#define PIN_17 10001
-#define PIN_18 10010
-#define PIN_19 10011
-#define PIN_20 10100
-#define PIN_21 10101
-#define PIN_22 10110
-#define PIN_23 10111
-#define PIN_24 11000
-#define PIN_25 11001
-#define PIN_26 11010
-#define PIN_27 11011
-#define PIN_28 11100
-#define PIN_29 11101
-#define PIN_30 11110
-#define PIN_31 11111
-
-#define PIN_(p) PIN_##p
-#define PIN(p) PIN_(p)
-
-#define ADC_NONE 0000
-#define ADC_CHAN_0 0001
-#define ADC_CHAN_1 0010
-#define ADC_CHAN_2 0011
-#define ADC_CHAN_3 0100
-#define ADC_CHAN_4 0101
-#define ADC_CHAN_5 0110
-#define ADC_CHAN_6 0111
-#define ADC_CHAN_7 1000
-
-#define ADC_CHAN_(c) ADC_CHAN_##c
-#define ADC_CHAN(p) ADC_CHAN_(p)
-
-#define BOOL_0 0
-#define BOOL_1 1
-#define BOOL_(b) BOOL_##b
-
-#define INTERRUPT(b) BOOL_(b)
-#define PWM(b) BOOL_(b)
-
-// Combine elements into pin bits: 0b00AAAAWIPPPNNNNN
-#define LPC1768_PIN_(port, pin, int, pwm, adc) 0b00##adc##pwm##int##port##pin
-#define LPC1768_PIN(port, pin, int, pwm, adc) LPC1768_PIN_(port, pin, int, pwm, adc)
-
-constexpr uint8_t LPC1768_PIN_PORT(const pin_t pin) { return ((uint8_t)((pin >> 5) & 0b111)); }
-constexpr uint8_t LPC1768_PIN_PIN(const pin_t pin) { return ((uint8_t)(pin & 0b11111)); }
-constexpr bool LPC1768_PIN_INTERRUPT(const pin_t pin) { return (((pin >> 8) & 0b1) != 0); }
-constexpr bool LPC1768_PIN_PWM(const pin_t pin) { return (((pin >> 9) & 0b1) != 0); }
-constexpr int8_t LPC1768_PIN_ADC(const pin_t pin) { return (int8_t)((pin >> 10) & 0b1111) - 1; }
-
-// ******************
-// Runtime pinmapping
-// ******************
-#define P_NC -1
-
-#if SERIAL_PORT != 3 && SERIAL_PORT_2 != 3
- #define P0_00 LPC1768_PIN(PORT(0), PIN( 0), INTERRUPT(1), PWM(0), ADC_NONE)
- #define P0_01 LPC1768_PIN(PORT(0), PIN( 1), INTERRUPT(1), PWM(0), ADC_NONE)
-#endif
-#if SERIAL_PORT != 0 && SERIAL_PORT_2 != 0
- #define P0_02 LPC1768_PIN(PORT(0), PIN( 2), INTERRUPT(1), PWM(0), ADC_CHAN(7))
- #define P0_03 LPC1768_PIN(PORT(0), PIN( 3), INTERRUPT(1), PWM(0), ADC_CHAN(6))
-#endif
-#define P0_04 LPC1768_PIN(PORT(0), PIN( 4), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P0_05 LPC1768_PIN(PORT(0), PIN( 5), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P0_06 LPC1768_PIN(PORT(0), PIN( 6), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P0_07 LPC1768_PIN(PORT(0), PIN( 7), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P0_08 LPC1768_PIN(PORT(0), PIN( 8), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P0_09 LPC1768_PIN(PORT(0), PIN( 9), INTERRUPT(1), PWM(0), ADC_NONE)
-#if SERIAL_PORT != 2 && SERIAL_PORT_2 != 2
- #define P0_10 LPC1768_PIN(PORT(0), PIN(10), INTERRUPT(1), PWM(0), ADC_NONE)
- #define P0_11 LPC1768_PIN(PORT(0), PIN(11), INTERRUPT(1), PWM(0), ADC_NONE)
-#endif
-#if SERIAL_PORT != 1 && SERIAL_PORT_2 != 1
- #define P0_15 LPC1768_PIN(PORT(0), PIN(15), INTERRUPT(1), PWM(0), ADC_NONE)
- #define P0_16 LPC1768_PIN(PORT(0), PIN(16), INTERRUPT(1), PWM(0), ADC_NONE)
-#endif
-#define P0_17 LPC1768_PIN(PORT(0), PIN(17), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P0_18 LPC1768_PIN(PORT(0), PIN(18), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P0_19 LPC1768_PIN(PORT(0), PIN(19), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P0_20 LPC1768_PIN(PORT(0), PIN(20), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P0_21 LPC1768_PIN(PORT(0), PIN(21), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P0_22 LPC1768_PIN(PORT(0), PIN(22), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P0_23 LPC1768_PIN(PORT(0), PIN(23), INTERRUPT(1), PWM(0), ADC_CHAN(0))
-#define P0_24 LPC1768_PIN(PORT(0), PIN(24), INTERRUPT(1), PWM(0), ADC_CHAN(1))
-#define P0_25 LPC1768_PIN(PORT(0), PIN(25), INTERRUPT(1), PWM(0), ADC_CHAN(2))
-#define P0_26 LPC1768_PIN(PORT(0), PIN(26), INTERRUPT(1), PWM(0), ADC_CHAN(3))
-#define P0_27 LPC1768_PIN(PORT(0), PIN(27), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P0_28 LPC1768_PIN(PORT(0), PIN(28), INTERRUPT(1), PWM(0), ADC_NONE)
-#if SERIAL_PORT != -1 && SERIAL_PORT_2 != -1
- #define P0_29 LPC1768_PIN(PORT(0), PIN(29), INTERRUPT(1), PWM(0), ADC_NONE)
- #define P0_30 LPC1768_PIN(PORT(0), PIN(30), INTERRUPT(1), PWM(0), ADC_NONE)
-#endif
-#define P1_00 LPC1768_PIN(PORT(1), PIN( 0), INTERRUPT(0), PWM(0), ADC_NONE)
-#define P1_01 LPC1768_PIN(PORT(1), PIN( 1), INTERRUPT(0), PWM(0), ADC_NONE)
-#define P1_04 LPC1768_PIN(PORT(1), PIN( 4), INTERRUPT(0), PWM(0), ADC_NONE)
-#define P1_08 LPC1768_PIN(PORT(1), PIN( 8), INTERRUPT(0), PWM(0), ADC_NONE)
-#define P1_09 LPC1768_PIN(PORT(1), PIN( 9), INTERRUPT(0), PWM(0), ADC_NONE)
-#define P1_10 LPC1768_PIN(PORT(1), PIN(10), INTERRUPT(0), PWM(0), ADC_NONE)
-#define P1_14 LPC1768_PIN(PORT(1), PIN(14), INTERRUPT(0), PWM(0), ADC_NONE)
-#define P1_15 LPC1768_PIN(PORT(1), PIN(15), INTERRUPT(0), PWM(0), ADC_NONE)
-#define P1_16 LPC1768_PIN(PORT(1), PIN(16), INTERRUPT(0), PWM(0), ADC_NONE)
-#define P1_17 LPC1768_PIN(PORT(1), PIN(17), INTERRUPT(0), PWM(0), ADC_NONE)
-#define P1_18 LPC1768_PIN(PORT(1), PIN(18), INTERRUPT(0), PWM(1), ADC_NONE)
-#define P1_19 LPC1768_PIN(PORT(1), PIN(19), INTERRUPT(0), PWM(0), ADC_NONE)
-#define P1_20 LPC1768_PIN(PORT(1), PIN(20), INTERRUPT(0), PWM(1), ADC_NONE)
-#define P1_21 LPC1768_PIN(PORT(1), PIN(21), INTERRUPT(0), PWM(1), ADC_NONE)
-#define P1_22 LPC1768_PIN(PORT(1), PIN(22), INTERRUPT(0), PWM(0), ADC_NONE)
-#define P1_23 LPC1768_PIN(PORT(1), PIN(23), INTERRUPT(0), PWM(1), ADC_NONE)
-#define P1_24 LPC1768_PIN(PORT(1), PIN(24), INTERRUPT(0), PWM(1), ADC_NONE)
-#define P1_25 LPC1768_PIN(PORT(1), PIN(25), INTERRUPT(0), PWM(0), ADC_NONE)
-#define P1_26 LPC1768_PIN(PORT(1), PIN(26), INTERRUPT(0), PWM(1), ADC_NONE)
-#define P1_27 LPC1768_PIN(PORT(1), PIN(27), INTERRUPT(0), PWM(0), ADC_NONE)
-#define P1_28 LPC1768_PIN(PORT(1), PIN(28), INTERRUPT(0), PWM(0), ADC_NONE)
-#define P1_29 LPC1768_PIN(PORT(1), PIN(29), INTERRUPT(0), PWM(0), ADC_NONE)
-#define P1_30 LPC1768_PIN(PORT(1), PIN(30), INTERRUPT(0), PWM(0), ADC_CHAN(4))
-#define P1_31 LPC1768_PIN(PORT(1), PIN(31), INTERRUPT(0), PWM(0), ADC_CHAN(5))
-#define P2_00 LPC1768_PIN(PORT(2), PIN( 0), INTERRUPT(1), PWM(1), ADC_NONE)
-#define P2_01 LPC1768_PIN(PORT(2), PIN( 1), INTERRUPT(1), PWM(1), ADC_NONE)
-#define P2_02 LPC1768_PIN(PORT(2), PIN( 2), INTERRUPT(1), PWM(1), ADC_NONE)
-#define P2_03 LPC1768_PIN(PORT(2), PIN( 3), INTERRUPT(1), PWM(1), ADC_NONE)
-#define P2_04 LPC1768_PIN(PORT(2), PIN( 4), INTERRUPT(1), PWM(1), ADC_NONE)
-#define P2_05 LPC1768_PIN(PORT(2), PIN( 5), INTERRUPT(1), PWM(1), ADC_NONE)
-#define P2_06 LPC1768_PIN(PORT(2), PIN( 6), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P2_07 LPC1768_PIN(PORT(2), PIN( 7), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P2_08 LPC1768_PIN(PORT(2), PIN( 8), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P2_09 LPC1768_PIN(PORT(2), PIN( 9), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P2_10 LPC1768_PIN(PORT(2), PIN(10), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P2_11 LPC1768_PIN(PORT(2), PIN(11), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P2_12 LPC1768_PIN(PORT(2), PIN(12), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P2_13 LPC1768_PIN(PORT(2), PIN(13), INTERRUPT(1), PWM(0), ADC_NONE)
-#define P3_25 LPC1768_PIN(PORT(3), PIN(25), INTERRUPT(0), PWM(1), ADC_NONE)
-#define P3_26 LPC1768_PIN(PORT(3), PIN(26), INTERRUPT(0), PWM(1), ADC_NONE)
-#define P4_28 LPC1768_PIN(PORT(4), PIN(28), INTERRUPT(0), PWM(0), ADC_NONE)
-#define P4_29 LPC1768_PIN(PORT(4), PIN(29), INTERRUPT(0), PWM(0), ADC_NONE)
-
-// Pin index for M43 and M226
-constexpr pin_t pin_map[] = {
- #if SERIAL_PORT != 3 && SERIAL_PORT_2 != 3
- P0_00, P0_01,
- #else
- P_NC, P_NC,
- #endif
- #if SERIAL_PORT != 0 && SERIAL_PORT_2 != 0
- P0_02, P0_03,
- #else
- P_NC, P_NC,
- #endif
- P0_04, P0_05, P0_06, P0_07,
- P0_08, P0_09,
- #if SERIAL_PORT != 2 && SERIAL_PORT_2 != 2
- P0_10, P0_11,
- #else
- P_NC, P_NC,
- #endif
- P_NC, P_NC, P_NC,
- #if SERIAL_PORT != 1 && SERIAL_PORT_2 != 1
- P0_15,
- P0_16,
- #else
- P_NC,
- P_NC,
- #endif
- P0_17, P0_18, P0_19, P0_20, P0_21, P0_22, P0_23,
- P0_24, P0_25, P0_26, P0_27, P0_28,
- #if SERIAL_PORT != -1 && SERIAL_PORT_2 != -1
- P0_29, P0_30,
- #else
- P_NC, P_NC,
- #endif
- P_NC,
-
- P1_00, P1_01, P_NC, P_NC, P1_04, P_NC, P_NC, P_NC,
- P1_08, P1_09, P1_10, P_NC, P_NC, P_NC, P1_14, P1_15,
- P1_16, P1_17, P1_18, P1_19, P1_20, P1_21, P1_22, P1_23,
- P1_24, P1_25, P1_26, P1_27, P1_28, P1_29, P1_30, P1_31,
-
- P2_00, P2_01, P2_02, P2_03, P2_04, P2_05, P2_06, P2_07,
- P2_08, P2_09, P2_10, P2_11, P2_12, P2_13, P_NC, P_NC,
- P_NC, P_NC, P_NC, P_NC, P_NC, P_NC, P_NC, P_NC,
- P_NC, P_NC, P_NC, P_NC, P_NC, P_NC, P_NC, P_NC,
-
- P_NC, P_NC, P_NC, P_NC, P_NC, P_NC, P_NC, P_NC,
- P_NC, P_NC, P_NC, P_NC, P_NC, P_NC, P_NC, P_NC,
- P_NC, P_NC, P_NC, P_NC, P_NC, P_NC, P_NC, P_NC,
- P_NC, P3_25, P3_26, P_NC, P_NC, P_NC, P_NC, P_NC,
-
- P_NC, P_NC, P_NC, P_NC, P_NC, P_NC, P_NC, P_NC,
- P_NC, P_NC, P_NC, P_NC, P_NC, P_NC, P_NC, P_NC,
- P_NC, P_NC, P_NC, P_NC, P_NC, P_NC, P_NC, P_NC,
- P_NC, P_NC, P_NC, P_NC, P4_28, P4_29, P_NC, P_NC
-};
-
-constexpr uint8_t NUM_DIGITAL_PINS = COUNT(pin_map);
-
-constexpr pin_t adc_pin_table[] = {
- P0_23, P0_24, P0_25, P0_26, P1_30, P1_31,
- #if SERIAL_PORT != 0 && SERIAL_PORT_2 != 0
- P0_03, P0_02
- #endif
-};
-
-#if SERIAL_PORT != 0 && SERIAL_PORT_2 != 0
- #define NUM_ANALOG_INPUTS 8
-#else
- #define NUM_ANALOG_INPUTS 6
-#endif
-
-// P0.6 thru P0.9 are for the onboard SD card
-#define HAL_SENSITIVE_PINS P0_06, P0_07, P0_08, P0_09
-
-// Get the digital pin for an analog index
-pin_t analogInputToDigitalPin(const int8_t p);
-#define digitalPinToInterrupt(pin) (pin)
-// Return the index of a pin number
-// The pin number given here is in the form ppp:nnnnn
-int16_t GET_PIN_MAP_INDEX(const pin_t pin);
-
-// Test whether the pin is valid
-bool VALID_PIN(const pin_t p);
-
-// Get the analog index for a digital pin
-int8_t DIGITAL_PIN_TO_ANALOG_PIN(const pin_t p);
-
-// Test whether the pin is PWM
-bool PWM_PIN(const pin_t p);
-
-// Test whether the pin is interruptable
-bool INTERRUPT_PIN(const pin_t p);
-#define LPC1768_PIN_INTERRUPT_M(pin) (((pin >> 8) & 0b1) != 0)
-
-// Get the pin number at the given index
-pin_t GET_PIN_MAP_PIN(const int16_t ind);
-
-// Parse a G-code word into a pin index
-int16_t PARSED_PIN_INDEX(const char code, const int16_t dval);
-
-#endif // _PINMAPPING_H_
diff --git a/Marlin/src/HAL/HAL_LPC1768/include/serial.h b/Marlin/src/HAL/HAL_LPC1768/include/serial.h
deleted file mode 100644
index 3fe8a8cb1d..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/include/serial.h
+++ /dev/null
@@ -1,161 +0,0 @@
-/**
- * Marlin 3D Printer Firmware
- * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
- *
- * Based on Sprinter and grbl.
- * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see .
- *
- */
-
-#ifndef _HAL_SERIAL_H_
-#define _HAL_SERIAL_H_
-
-#include "../../../inc/MarlinConfigPre.h"
-#if ENABLED(EMERGENCY_PARSER)
- #include "../../../feature/emergency_parser.h"
-#endif
-
-#include
-#include
-#include
-
-/**
- * Generic RingBuffer
- * T type of the buffer array
- * S size of the buffer (must be power of 2)
- */
-
-template class RingBuffer {
-public:
- RingBuffer() {index_read = index_write = 0;}
-
- uint32_t available() {return mask(index_write - index_read);}
- uint32_t free() {return buffer_size - available();}
- bool empty() {return index_read == index_write;}
- bool full() {return next(index_write) == index_read;}
- void clear() {index_read = index_write = 0;}
-
- bool peek(T *const value) {
- if (value == nullptr || empty()) return false;
- *value = buffer[index_read];
- return true;
- }
-
- uint32_t read(T *const value) {
- if (value == nullptr || empty()) return 0;
- *value = buffer[index_read];
- index_read = next(index_read);
- return 1;
- }
-
- uint32_t write(T value) {
- uint32_t next_head = next(index_write);
- if (next_head == index_read) return 0; // buffer full
- buffer[index_write] = value;
- index_write = next_head;
- return 1;
- }
-
-private:
- inline uint32_t mask(uint32_t val) {
- return val & buffer_mask;
- }
-
- inline uint32_t next(uint32_t val) {
- return mask(val + 1);
- }
-
- static const uint32_t buffer_size = S;
- static const uint32_t buffer_mask = buffer_size - 1;
- T buffer[buffer_size];
- volatile uint32_t index_write;
- volatile uint32_t index_read;
-};
-
-/**
- * Serial Interface Class
- * Data is injected directly into, and consumed from, the fifo buffers
- */
-
-class HalSerial: public Print {
-public:
-
- #if ENABLED(EMERGENCY_PARSER)
- EmergencyParser::State emergency_state;
- #endif
-
- HalSerial() : host_connected(false) { }
- virtual ~HalSerial() { }
-
- operator bool() { return host_connected; }
-
- void begin(int32_t baud) { }
-
- int16_t peek() {
- uint8_t value;
- return receive_buffer.peek(&value) ? value : -1;
- }
-
- int16_t read() {
- uint8_t value;
- return receive_buffer.read(&value) ? value : -1;
- }
-
- size_t write(const uint8_t c) {
- if (!host_connected) return 0; // Do not fill buffer when host disconnected
- while (transmit_buffer.write(c) == 0) { // Block until there is free room in buffer
- if (!host_connected) return 0; // Break infinite loop on host disconect
- }
- return 1;
- }
-
- size_t available() {
- return (size_t)receive_buffer.available();
- }
-
- void flush() {
- receive_buffer.clear();
- }
-
- uint8_t availableForWrite(void) {
- return transmit_buffer.free() > 255 ? 255 : (uint8_t)transmit_buffer.free();
- }
-
- void flushTX(void) {
- while (transmit_buffer.available() && host_connected) { /* nada */}
- }
-
- size_t printf(const char *format, ...) {
- static char buffer[256];
- va_list vArgs;
- va_start(vArgs, format);
- int length = vsnprintf((char *) buffer, 256, (char const *) format, vArgs);
- va_end(vArgs);
- size_t i = 0;
- if (length > 0 && length < 256) {
- while (i < (size_t)length && host_connected) {
- i += transmit_buffer.write(buffer[i]);
- }
- }
- return i;
- }
-
- RingBuffer receive_buffer;
- RingBuffer transmit_buffer;
- volatile bool host_connected;
-};
-
-#endif // _HAL_SERIAL_H_
diff --git a/Marlin/src/HAL/HAL_LPC1768/lpc1768_flag_script.py b/Marlin/src/HAL/HAL_LPC1768/lpc1768_flag_script.py
deleted file mode 100644
index 44ded586fe..0000000000
--- a/Marlin/src/HAL/HAL_LPC1768/lpc1768_flag_script.py
+++ /dev/null
@@ -1,60 +0,0 @@
-from __future__ import print_function
-import sys
-
-#dynamic build flags for generic compile options
-if __name__ == "__main__":
- args = " ".join([ "-std=gnu11",
- "-std=gnu++11",
- "-Os",
- "-mcpu=cortex-m3",
- "-mthumb",
-
- "-fsigned-char",
- "-fno-move-loop-invariants",
- "-fno-strict-aliasing",
- "-fsingle-precision-constant",
-
- "--specs=nano.specs",
- "--specs=nosys.specs",
-
- # For external libraries
- "-IMarlin/src/HAL/HAL_LPC1768/include",
-
- # For MarlinFirmware/U8glib-HAL
- "-IMarlin/src/HAL/HAL_LPC1768/u8g",
- "-DU8G_HAL_LINKS",
-
- "-MMD",
- "-MP",
- "-DTARGET_LPC1768"
- ])
-
- for i in range(1, len(sys.argv)):
- args += " " + sys.argv[i]
-
- print(args)
-
-# extra script for linker options
-else:
- from SCons.Script import DefaultEnvironment
- env = DefaultEnvironment()
- env.Append(
- ARFLAGS=["rcs"],
-
- ASFLAGS=["-x", "assembler-with-cpp"],
-
- CXXFLAGS=[
- "-fabi-version=0",
- "-fno-use-cxa-atexit",
- "-fno-threadsafe-statics"
- ],
- LINKFLAGS=[
- "-Wl,-Tframeworks/CMSIS/LPC1768/system/LPC1768.ld,--gc-sections",
- "-Os",
- "-mcpu=cortex-m3",
- "-mthumb",
- "--specs=nano.specs",
- "--specs=nosys.specs",
- "-u_printf_float"
- ],
- )
diff --git a/Marlin/src/HAL/HAL_LPC1768/main.cpp b/Marlin/src/HAL/HAL_LPC1768/main.cpp
index 399120dac7..f1f9ed405b 100644
--- a/Marlin/src/HAL/HAL_LPC1768/main.cpp
+++ b/Marlin/src/HAL/HAL_LPC1768/main.cpp
@@ -1,15 +1,5 @@
#ifdef TARGET_LPC1768
-// ---------------------
-// Userspace entry point
-// ---------------------
-extern void setup();
-extern void loop();
-
-extern "C" {
- #include
-}
-
#include
#include
#include
@@ -17,84 +7,67 @@ extern "C" {
#include
#include
#include
+#include
+#include
extern "C" {
#include
- #include
- #include
}
+#include "../../sd/cardreader.h"
#include "../../inc/MarlinConfig.h"
#include "HAL.h"
-#include "fastio.h"
#include "HAL_timers.h"
-#include
-#include
-#include
-#include "serial.h"
-#include "LPC1768_PWM.h"
-
-static __INLINE uint32_t SysTick_Config(uint32_t ticks) {
- if (ticks > SysTick_LOAD_RELOAD_Msk) return 1;
-
- SysTick->LOAD = (ticks & SysTick_LOAD_RELOAD_Msk) - 1; // Set reload register
- SysTick->VAL = 0; // Load the SysTick Counter Value
- SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk |
- SysTick_CTRL_TICKINT_Msk |
- SysTick_CTRL_ENABLE_Msk; // Enable SysTick IRQ and SysTick Timer
-
- NVIC_SetPriority(SysTick_IRQn, NVIC_EncodePriority(0, 0, 0)); // Set Priority for Cortex-M3 System Interrupts
- return 0;
-}
-
-extern "C" {
- extern int isLPC1769();
- extern void disk_timerproc(void);
- volatile uint32_t _millis;
-
- void SysTick_Handler(void) {
- ++_millis;
- disk_timerproc();
- }
-
- // Runs after clock init and before global static constructors
- void SystemPostInit() {
- _millis = 0; // Initialize the millisecond counter value
- SysTick_Config(SystemCoreClock / 1000); // Start millisecond global counter
-
- // Runs before setup() to configure LED_PIN and used to indicate successful bootloader execution
- #if PIN_EXISTS(LED)
- SET_DIR_OUTPUT(LED_PIN);
- WRITE_PIN_CLR(LED_PIN);
-
- // MKS_SBASE has 3 other LEDs the bootloader uses during flashing. Clear them.
- SET_DIR_OUTPUT(P1_19);
- WRITE_PIN_CLR(P1_19);
- SET_DIR_OUTPUT(P1_20);
- WRITE_PIN_CLR(P1_20);
- SET_DIR_OUTPUT(P1_21);
- WRITE_PIN_CLR(P1_21);
-
- for (uint8_t i = 6; i--;) {
- TOGGLE(LED_PIN);
- delay(100);
- }
- #endif
- }
-}
extern uint32_t MSC_SD_Init(uint8_t pdrv);
+extern "C" int isLPC1769();
+extern "C" void disk_timerproc(void);
-int main(void) {
+void SysTick_Callback() {
+ disk_timerproc();
+}
- (void)MSC_SD_Init(0);
+void HAL_init() {
+ #if PIN_EXISTS(LED)
+ SET_DIR_OUTPUT(LED_PIN);
+ WRITE_PIN_CLR(LED_PIN);
+ // MKS_SBASE has 3 other LEDs the bootloader uses during flashing. Clear them.
+ SET_DIR_OUTPUT(P1_19);
+ WRITE_PIN_CLR(P1_19);
+ SET_DIR_OUTPUT(P1_20);
+ WRITE_PIN_CLR(P1_20);
+ SET_DIR_OUTPUT(P1_21);
+ WRITE_PIN_CLR(P1_21);
+
+ // Flash status LED 3 times to indicate Marlin has started booting
+ for (uint8_t i = 0; i < 6; ++i) {
+ TOGGLE(LED_PIN);
+ delay(100);
+ }
+ #endif
+ //debug_frmwrk_init();
+ //_DBG("\n\nDebug running\n");
+ // Initialise the SD card chip select pins as soon as possible
+ #ifdef SS_PIN
+ digitalWrite(SS_PIN, HIGH);
+ pinMode(SS_PIN, OUTPUT);
+ #endif
+ #ifdef ONBOARD_SD_CS
+ digitalWrite(ONBOARD_SD_CS, HIGH);
+ pinMode(ONBOARD_SD_CS, OUTPUT);
+ #endif
USB_Init(); // USB Initialization
- USB_Connect(TRUE); // USB Connect
-
+ USB_Connect(FALSE); // USB clear connection
+ delay(1000); // Give OS time to notice
+ USB_Connect(TRUE);
+ #ifndef USB_SD_DISABLED
+ MSC_SD_Init(0); // Enable USB SD card access
+ #endif
const uint32_t usb_timeout = millis() + 2000;
while (!USB_Configuration && PENDING(millis(), usb_timeout)) {
delay(50);
+ HAL_idletask();
#if PIN_EXISTS(LED)
TOGGLE(LED_PIN); // Flash quickly during USB initialization
#endif
@@ -110,11 +83,25 @@ int main(void) {
#endif
HAL_timer_init();
+}
- LPC1768_PWM_init();
-
- setup();
- for (;;) loop();
+// HAL idle task
+void HAL_idletask(void) {
+ #if ENABLED(SDSUPPORT) && defined(SHARED_SD_CARD)
+ // If Marlin is using the SD card we need to lock it to prevent access from
+ // a PC via USB.
+ // Other HALs use IS_SD_PRINTING and IS_SD_FILE_OPEN to check for access but
+ // this will not reliably detect delete operations. To be safe we will lock
+ // the disk if Marlin has it mounted. Unfortuately there is currently no way
+ // to unmount the disk from the LCD menu.
+ // if (IS_SD_PRINTING || IS_SD_FILE_OPEN)
+ if (card.cardOK)
+ MSC_Aquire_Lock();
+ else
+ MSC_Release_Lock();
+ #endif
+ // Perform USB stack housekeeping
+ MSC_RunDeferredCommands();
}
#endif // TARGET_LPC1768
diff --git a/Marlin/src/HAL/HAL_LPC1768/persistent_store_api.h b/Marlin/src/HAL/HAL_LPC1768/persistent_store_api.h
index 9f6d626491..fb659b396c 100644
--- a/Marlin/src/HAL/HAL_LPC1768/persistent_store_api.h
+++ b/Marlin/src/HAL/HAL_LPC1768/persistent_store_api.h
@@ -21,4 +21,4 @@
*/
#include "../shared/persistent_store_api.h"
-//#define FLASH_EEPROM
+#define FLASH_EEPROM
diff --git a/Marlin/src/HAL/HAL_LPC1768/persistent_store_flash.cpp b/Marlin/src/HAL/HAL_LPC1768/persistent_store_flash.cpp
index daffddf079..ce6ab7de65 100644
--- a/Marlin/src/HAL/HAL_LPC1768/persistent_store_flash.cpp
+++ b/Marlin/src/HAL/HAL_LPC1768/persistent_store_flash.cpp
@@ -69,20 +69,15 @@ bool PersistentStore::access_start() {
__disable_irq();
status = BlankCheckSector(EEPROM_SECTOR, EEPROM_SECTOR, &first_nblank_loc, &first_nblank_val);
__enable_irq();
- SERIAL_PROTOCOLLNPAIR("Blank check status: ", status);
+
if (status == CMD_SUCCESS) {
// sector is blank so nothing stored yet
- SERIAL_PROTOCOLLNPGM("FLASH empty");
for (int i = 0; i < EEPROM_SIZE; i++) ram_eeprom[i] = EEPROM_ERASE;
current_slot = EEPROM_SLOTS;
- }
- else {
+ } else {
// current slot is the first non blank one
current_slot = first_nblank_loc / EEPROM_SIZE;
- SERIAL_PROTOCOLLNPAIR("Flash slot: ", current_slot);
uint8_t *eeprom_data = SLOT_ADDRESS(EEPROM_SECTOR, current_slot);
- SERIAL_PROTOCOLLNPAIR("Address: ", (int)eeprom_data);
-
// load current settings
for (int i = 0; i < EEPROM_SIZE; i++) ram_eeprom[i] = eeprom_data[i];
}
@@ -100,15 +95,15 @@ bool PersistentStore::access_finish() {
PrepareSector(EEPROM_SECTOR, EEPROM_SECTOR);
status = EraseSector(EEPROM_SECTOR, EEPROM_SECTOR);
__enable_irq();
- SERIAL_PROTOCOLLNPAIR("Erase status: ", status);
+
current_slot = EEPROM_SLOTS - 1;
}
- SERIAL_PROTOCOLLNPAIR("Writing data to: ", current_slot);
+
__disable_irq();
PrepareSector(EEPROM_SECTOR, EEPROM_SECTOR);
status = CopyRAM2Flash(SLOT_ADDRESS(EEPROM_SECTOR, current_slot), ram_eeprom, IAP_WRITE_4096);
__enable_irq();
- SERIAL_PROTOCOLLNPAIR("CopyRAM2Flash status: ", status);
+
if (status != CMD_SUCCESS) return false;
eeprom_dirty = false;
}
@@ -116,7 +111,7 @@ bool PersistentStore::access_finish() {
}
bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) {
- for (int i = 0; i < size; i++) ram_eeprom[pos + i] = value[i];
+ for (size_t i = 0; i < size; i++) ram_eeprom[pos + i] = value[i];
eeprom_dirty = true;
crc16(crc, value, size);
pos += size;
@@ -125,7 +120,7 @@ bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, ui
bool PersistentStore::read_data(int &pos, uint8_t* value, size_t size, uint16_t *crc, const bool writing/*=true*/) {
const uint8_t * const buff = writing ? &value[0] : &ram_eeprom[pos];
- if (writing) for (int i = 0; i < size; i++) value[i] = ram_eeprom[pos + i];
+ if (writing) for (size_t i = 0; i < size; i++) value[i] = ram_eeprom[pos + i];
crc16(crc, buff, size);
pos += size;
return false; // return true for any error
diff --git a/Marlin/src/HAL/HAL_LPC1768/spi_pins.h b/Marlin/src/HAL/HAL_LPC1768/spi_pins.h
index 841a3f845f..76c19c1176 100644
--- a/Marlin/src/HAL/HAL_LPC1768/spi_pins.h
+++ b/Marlin/src/HAL/HAL_LPC1768/spi_pins.h
@@ -50,7 +50,8 @@
#ifndef SS_PIN
#define SS_PIN P1_23
#endif
-#ifndef SDSS
+#if !defined(SDSS) || SDSS == P_NC // gets defaulted in pins.h
+ #undef SDSS
#define SDSS SS_PIN
#endif
diff --git a/Marlin/src/HAL/HAL_LPC1768/u8g_com_HAL_LPC1768_hw_spi.cpp b/Marlin/src/HAL/HAL_LPC1768/u8g/u8g_com_HAL_LPC1768_hw_spi.cpp
similarity index 99%
rename from Marlin/src/HAL/HAL_LPC1768/u8g_com_HAL_LPC1768_hw_spi.cpp
rename to Marlin/src/HAL/HAL_LPC1768/u8g/u8g_com_HAL_LPC1768_hw_spi.cpp
index 9cd913f743..9ac0ae460c 100644
--- a/Marlin/src/HAL/HAL_LPC1768/u8g_com_HAL_LPC1768_hw_spi.cpp
+++ b/Marlin/src/HAL/HAL_LPC1768/u8g/u8g_com_HAL_LPC1768_hw_spi.cpp
@@ -55,7 +55,7 @@
#ifdef TARGET_LPC1768
-#include "../../inc/MarlinConfigPre.h"
+#include "../../../inc/MarlinConfigPre.h"
#if ENABLED(DOGLCD)
diff --git a/Marlin/src/HAL/HAL_LPC1768/u8g_com_HAL_LPC1768_ssd_hw_i2c.cpp b/Marlin/src/HAL/HAL_LPC1768/u8g/u8g_com_HAL_LPC1768_ssd_hw_i2c.cpp
similarity index 99%
rename from Marlin/src/HAL/HAL_LPC1768/u8g_com_HAL_LPC1768_ssd_hw_i2c.cpp
rename to Marlin/src/HAL/HAL_LPC1768/u8g/u8g_com_HAL_LPC1768_ssd_hw_i2c.cpp
index 06399d3947..110d9149c4 100644
--- a/Marlin/src/HAL/HAL_LPC1768/u8g_com_HAL_LPC1768_ssd_hw_i2c.cpp
+++ b/Marlin/src/HAL/HAL_LPC1768/u8g/u8g_com_HAL_LPC1768_ssd_hw_i2c.cpp
@@ -77,7 +77,7 @@
#ifdef TARGET_LPC1768
-#include "../../inc/MarlinConfigPre.h"
+#include "../../../inc/MarlinConfigPre.h"
#if ENABLED(DOGLCD)
diff --git a/Marlin/src/HAL/HAL_LPC1768/u8g_com_HAL_LPC1768_ssd_sw_i2c.cpp under construction b/Marlin/src/HAL/HAL_LPC1768/u8g/u8g_com_HAL_LPC1768_ssd_sw_i2c.cpp under construction
similarity index 100%
rename from Marlin/src/HAL/HAL_LPC1768/u8g_com_HAL_LPC1768_ssd_sw_i2c.cpp under construction
rename to Marlin/src/HAL/HAL_LPC1768/u8g/u8g_com_HAL_LPC1768_ssd_sw_i2c.cpp under construction
diff --git a/Marlin/src/HAL/HAL_LPC1768/u8g_com_HAL_LPC1768_st7920_hw_spi.cpp b/Marlin/src/HAL/HAL_LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_hw_spi.cpp
similarity index 98%
rename from Marlin/src/HAL/HAL_LPC1768/u8g_com_HAL_LPC1768_st7920_hw_spi.cpp
rename to Marlin/src/HAL/HAL_LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_hw_spi.cpp
index 6b036ba5ea..827ae5abde 100644
--- a/Marlin/src/HAL/HAL_LPC1768/u8g_com_HAL_LPC1768_st7920_hw_spi.cpp
+++ b/Marlin/src/HAL/HAL_LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_hw_spi.cpp
@@ -55,13 +55,13 @@
#ifdef TARGET_LPC1768
-#include "../../inc/MarlinConfigPre.h"
+#include "../../../inc/MarlinConfigPre.h"
#if ENABLED(DOGLCD)
//#include
#include
-#include "../shared/Delay.h"
+#include "../../shared/Delay.h"
#define SPI_FULL_SPEED 0
#define SPI_HALF_SPEED 1
diff --git a/Marlin/src/HAL/HAL_LPC1768/u8g_com_HAL_LPC1768_st7920_sw_spi.cpp b/Marlin/src/HAL/HAL_LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_sw_spi.cpp
similarity index 98%
rename from Marlin/src/HAL/HAL_LPC1768/u8g_com_HAL_LPC1768_st7920_sw_spi.cpp
rename to Marlin/src/HAL/HAL_LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_sw_spi.cpp
index a189b9126d..9e9b76439a 100644
--- a/Marlin/src/HAL/HAL_LPC1768/u8g_com_HAL_LPC1768_st7920_sw_spi.cpp
+++ b/Marlin/src/HAL/HAL_LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_sw_spi.cpp
@@ -55,14 +55,15 @@
#ifdef TARGET_LPC1768
-#include "../../inc/MarlinConfigPre.h"
+#include "../../../inc/MarlinConfigPre.h"
#if ENABLED(DOGLCD)
#include
#include "SoftwareSPI.h"
-#include "../shared/Delay.h"
+#include "../../shared/Delay.h"
+#undef SPI_SPEED
#define SPI_SPEED 3 // About 1 MHz
static pin_t SCK_pin_ST7920_HAL, MOSI_pin_ST7920_HAL_HAL;
diff --git a/Marlin/src/HAL/HAL_LPC1768/u8g_com_HAL_LPC1768_sw_spi.cpp b/Marlin/src/HAL/HAL_LPC1768/u8g/u8g_com_HAL_LPC1768_sw_spi.cpp
similarity index 98%
rename from Marlin/src/HAL/HAL_LPC1768/u8g_com_HAL_LPC1768_sw_spi.cpp
rename to Marlin/src/HAL/HAL_LPC1768/u8g/u8g_com_HAL_LPC1768_sw_spi.cpp
index 533ebda37c..43b46d5ddf 100644
--- a/Marlin/src/HAL/HAL_LPC1768/u8g_com_HAL_LPC1768_sw_spi.cpp
+++ b/Marlin/src/HAL/HAL_LPC1768/u8g/u8g_com_HAL_LPC1768_sw_spi.cpp
@@ -55,13 +55,14 @@
#ifdef TARGET_LPC1768
-#include "../../inc/MarlinConfigPre.h"
+#include "../../../inc/MarlinConfigPre.h"
#if ENABLED(DOGLCD)
#include
#include "SoftwareSPI.h"
+#undef SPI_SPEED
#define SPI_SPEED 2 // About 2 MHz
static uint8_t SPI_speed = 0;
diff --git a/Marlin/src/HAL/HAL_LPC1768/usb_serial.cpp b/Marlin/src/HAL/HAL_LPC1768/usb_serial.cpp
new file mode 100644
index 0000000000..d143bae709
--- /dev/null
+++ b/Marlin/src/HAL/HAL_LPC1768/usb_serial.cpp
@@ -0,0 +1,13 @@
+#ifdef TARGET_LPC1768
+#include "../../inc/MarlinConfigPre.h"
+
+#if ENABLED(EMERGENCY_PARSER)
+ #include "../../feature/emergency_parser.h"
+ EmergencyParser::State emergency_state;
+ bool CDC_RecvCallback(const char buffer) {
+ emergency_parser.update(emergency_state, buffer);
+ return true;
+ }
+#endif // ENABLED(EMERGENCY_PARSER)
+
+#endif // TARGET_LPC1768
diff --git a/Marlin/src/HAL/HAL_LPC1768/watchdog.cpp b/Marlin/src/HAL/HAL_LPC1768/watchdog.cpp
index 589e05ebd6..9138c04ac2 100644
--- a/Marlin/src/HAL/HAL_LPC1768/watchdog.cpp
+++ b/Marlin/src/HAL/HAL_LPC1768/watchdog.cpp
@@ -30,7 +30,29 @@
#include "watchdog.h"
void watchdog_init(void) {
- WDT_Init(WDT_CLKSRC_IRC, WDT_MODE_RESET);
+ #if ENABLED(WATCHDOG_RESET_MANUAL)
+ // We enable the watchdog timer, but only for the interrupt.
+
+ // Configure WDT to only trigger an interrupt
+ // Disable WDT interrupt (just in case, to avoid triggering it!)
+ NVIC_DisableIRQ(WDT_IRQn);
+
+ // We NEED memory barriers to ensure Interrupts are actually disabled!
+ // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
+ __DSB();
+ __ISB();
+
+ // Configure WDT to only trigger an interrupt
+ // Initialize WDT with the given parameters
+ WDT_Init(WDT_CLKSRC_IRC, WDT_MODE_INT_ONLY);
+
+ // Configure and enable WDT interrupt.
+ NVIC_ClearPendingIRQ(WDT_IRQn);
+ NVIC_SetPriority(WDT_IRQn, 0); // Use highest priority, so we detect all kinds of lockups
+ NVIC_EnableIRQ(WDT_IRQn);
+ #else
+ WDT_Init(WDT_CLKSRC_IRC, WDT_MODE_RESET);
+ #endif
WDT_Start(WDT_TIMEOUT);
}
diff --git a/frameworks/CMSIS/LPC1768/lib/usb/Re-ARM_usb_driver.inf b/Marlin/src/HAL/HAL_LPC1768/win_usb_driver/lpc176x_usb_driver.inf
similarity index 100%
rename from frameworks/CMSIS/LPC1768/lib/usb/Re-ARM_usb_driver.inf
rename to Marlin/src/HAL/HAL_LPC1768/win_usb_driver/lpc176x_usb_driver.inf
diff --git a/Marlin/src/HAL/shared/servo.h b/Marlin/src/HAL/shared/servo.h
index 270923b546..93a10aaf67 100644
--- a/Marlin/src/HAL/shared/servo.h
+++ b/Marlin/src/HAL/shared/servo.h
@@ -73,7 +73,7 @@
#elif IS_TEENSY35 || IS_TEENSY36
#include "../HAL_TEENSY35_36/HAL_Servo_Teensy.h"
#elif defined(TARGET_LPC1768)
- #include "../HAL_LPC1768/LPC1768_Servo.h"
+ #include "../HAL_LPC1768/MarlinServo.h"
#elif defined(__STM32F1__) || defined(TARGET_STM32F1)
#include "../HAL_STM32F1/HAL_Servo_STM32F1.h"
#elif defined(STM32GENERIC) && defined(STM32F4)
diff --git a/Marlin/src/module/servo.h b/Marlin/src/module/servo.h
index bd7e2acf5c..04582431dd 100644
--- a/Marlin/src/module/servo.h
+++ b/Marlin/src/module/servo.h
@@ -27,6 +27,7 @@
#ifndef _SERVO_H_
#define _SERVO_H_
+#include "../inc/MarlinConfig.h"
#include "../HAL/shared/servo.h"
extern HAL_SERVO_LIB servo[NUM_SERVOS];
@@ -35,8 +36,6 @@ extern void servo_init();
#define MOVE_SERVO(I, P) servo[I].move(P)
-#include "../inc/MarlinConfig.h"
-
#if HAS_Z_SERVO_PROBE
#define DEPLOY_Z_SERVO() MOVE_SERVO(Z_PROBE_SERVO_NR, servo_angles[Z_PROBE_SERVO_NR][0])
#define STOW_Z_SERVO() MOVE_SERVO(Z_PROBE_SERVO_NR, servo_angles[Z_PROBE_SERVO_NR][1])
diff --git a/Marlin/src/pins/pins.h b/Marlin/src/pins/pins.h
index e8d9f4a7c8..48e8ac0ba2 100644
--- a/Marlin/src/pins/pins.h
+++ b/Marlin/src/pins/pins.h
@@ -299,19 +299,19 @@
#elif MB(AZSMZ_MINI)
#include "pins_AZSMZ_MINI.h" // LPC1768 env:LPC1768
#elif MB(AZTEEG_X5_GT)
- #include "pins_AZTEEG_X5_GT.h" // LPC1769 env:LPC1768
+ #include "pins_AZTEEG_X5_GT.h" // LPC1769 env:LPC1769
#elif MB(AZTEEG_X5_MINI_WIFI)
- #include "pins_AZTEEG_X5_MINI_WIFI.h" // LPC1769 env:LPC1768
+ #include "pins_AZTEEG_X5_MINI_WIFI.h" // LPC1769 env:LPC1769
#elif MB(BIQU_BQ111_A4)
#include "pins_BIQU_BQ111_A4.h" // LPC1768 env:LPC1768
#elif MB(SELENA_COMPACT)
#include "pins_SELENA_COMPACT.h" // LPC1768 env:LPC1768
#elif MB(COHESION3D_REMIX)
- #include "pins_COHESION3D_REMIX.h" // LPC1769 env:LPC1768
+ #include "pins_COHESION3D_REMIX.h" // LPC1769 env:LPC1769
#elif MB(COHESION3D_MINI)
- #include "pins_COHESION3D_MINI.h" // LPC1769 env:LPC1768
+ #include "pins_COHESION3D_MINI.h" // LPC1769 env:LPC1769
#elif MB(SMOOTHIEBOARD)
- #include "pins_SMOOTHIEBOARD.h" // LPC1769 env:LPC1768
+ #include "pins_SMOOTHIEBOARD.h" // LPC1769 env:LPC1769
//
// Other 32-bit Boards
diff --git a/Marlin/src/pins/pins_MKS_SBASE.h b/Marlin/src/pins/pins_MKS_SBASE.h
index 7a58198342..a901573c0f 100644
--- a/Marlin/src/pins/pins_MKS_SBASE.h
+++ b/Marlin/src/pins/pins_MKS_SBASE.h
@@ -141,8 +141,6 @@
// Misc. Functions
//
#define PS_ON_PIN P0_25 //TH3 Connector
-#define LPC_SOFTWARE_SPI // MKS_SBASE needs a software SPI because the
- // selected pins are not on a hardware SPI controller
/**
* Smart LCD adapter
@@ -185,14 +183,76 @@
#define ENET_TXD0 P1_00 // J12-11
#define ENET_TXD1 P1_01 // J12-12
-// A custom cable is needed. See the README file in the
-// Marlin\src\config\examples\Mks\Sbase directory
-#define SCK_PIN P1_22 // J8-2 (moved from EXP2 P0.7)
-#define MISO_PIN P1_23 // J8-3 (moved from EXP2 P0.8)
-#define MOSI_PIN P2_12 // J8-4 (moved from EXP2 P0.5)
-#define SS_PIN P0_28
-#define SDSS P0_06
+/*
+ * The SBase can share the on-board SD card with a PC via USB the following
+ * definitions control this feature:
+ */
+//#define USB_SD_DISABLED
+#define USB_SD_ONBOARD // Provide the onboard SD card to the host as a USB mass storage device
+
+/*
+ * There are a number of configurations available for the SBase SD card reader.
+ * A custom cable can be used to allow access to the LCD based SD card.
+ * A standard cable can be used for access to the LCD SD card (but no SD detect).
+ * The onboard SD card can be used and optionally shared with a PC via USB.
+ */
+
+//#define LPC_SD_CUSTOM_CABLE // Use a custom cable to access the SD
+//#define LPC_SD_LCD // Marlin uses the SD drive attached to the LCD
+#define LPC_SD_ONBOARD // Marlin uses the SD drive attached to the control board
+
+#ifdef LPC_SD_CUSTOM_CABLE
+ /**
+ * A custom cable is needed. See the README file in the
+ * Marlin\src\config\examples\Mks\Sbase directory
+ * P0.27 is on EXP2 and the on-board SD card's socket. That means it can't be
+ * used as the SD_DETECT for the LCD's SD card.
+ *
+ * The best solution is to use the custom cable to connect the LCD's SD_DETECT
+ * to a pin NOT on EXP2.
+ *
+ * If you can't find a pin to use for the LCD's SD_DETECT then comment out
+ * SD_DETECT_PIN entirely and remove that wire from the the custom cable.
+ */
+ #define SD_DETECT_PIN P2_11 // J8-5 (moved from EXP2 P0.27)
+ #define SCK_PIN P1_22 // J8-2 (moved from EXP2 P0.7)
+ #define MISO_PIN P1_23 // J8-3 (moved from EXP2 P0.8)
+ #define MOSI_PIN P2_12 // J8-4 (moved from EXP2 P0.9)
+ #define SS_PIN P0_28 // Chip select for SD card used by Marlin
+ #define ONBOARD_SD_CS P0_06 // Chip select for "System" SD card
+ #define LPC_SOFTWARE_SPI // With a custom cable we need software SPI because the
+ // selected pins are not on a hardware SPI controller
+#endif
+
+#ifdef LPC_SD_LCD
+ // use standard cable and header, SPI and SD detect sre shared with on-board SD card
+ // hardware SPI is used for both SD cards. The detect pin is shred between the
+ // LCD and onboard SD readers so we disable it.
+ #undef SD_DETECT_PIN
+ #define SCK_PIN P0_07
+ #define MISO_PIN P0_08
+ #define MOSI_PIN P0_09
+ #define SS_PIN P0_28 // Chip select for SD card used by Marlin
+ #define ONBOARD_SD_CS P0_06 // Chip select for "System" SD card
+#endif
+
+#ifdef LPC_SD_ONBOARD
+ // The external SD card is not used. Hardware SPI is used to access the card.
+ #ifdef USB_SD_ONBOARD
+ // When sharing the SD card with a PC we want the menu options to
+ // mount/unmount the card and refresh it. So we disable card detect.
+ #define SHARED_SD_CARD
+ #undef SD_DETECT_PIN
+ #else
+ #define SD_DETECT_PIN P0_27
+ #endif
+ #define SCK_PIN P0_07
+ #define MISO_PIN P0_08
+ #define MOSI_PIN P0_09
+ #define SS_PIN P0_06 // Chip select for SD card used by Marlin
+ #define ONBOARD_SD_CS P0_06 // Chip select for "System" SD card
+#endif
/**
* Example for trinamic drivers using the J8 connector on MKs Sbase.
@@ -237,18 +297,6 @@
#define E0_SERIAL_RX_PIN P0_26 // TH4
#endif
-/**
- * P0.27 is on EXP2 and the on-board SD card's socket. That means it can't be
- * used as the SD_DETECT for the LCD's SD card.
- *
- * The best solution is to use the custom cable to connect the LCD's SD_DETECT
- * to a pin NOT on EXP2.
- *
- * If you can't find a pin to use for the LCD's SD_DETECT then comment out
- * SD_DETECT_PIN entirely and remove that wire from the the custom cable.
- */
-#define SD_DETECT_PIN P2_11 // J8-5 (moved from EXP2 P0.27)
-
/**
* PWMs
*
diff --git a/Marlin/src/pins/pins_RAMPS_RE_ARM.h b/Marlin/src/pins/pins_RAMPS_RE_ARM.h
index 6f0c604ffb..534144c298 100644
--- a/Marlin/src/pins/pins_RAMPS_RE_ARM.h
+++ b/Marlin/src/pins/pins_RAMPS_RE_ARM.h
@@ -198,7 +198,6 @@
// Misc. Functions
//
#define LED_PIN P4_28 // (13)
-#define SDSS P1_23 // (53)
// define digital pin 4 for the filament runout sensor. Use the RAMPS 1.4 digital input 4 on the servos connector
#ifndef FIL_RUNOUT_PIN
@@ -364,6 +363,35 @@
#define ENET_TXD0 P1_00 // (78) J12-11
#define ENET_TXD1 P1_01 // (79) J12-12
+
+//#define USB_SD_DISABLED
+#define USB_SD_ONBOARD // Provide the onboard SD card to the host as a USB mass storage device
+
+//#define LPC_SD_LCD // Marlin uses the SD drive attached to the LCD
+#define LPC_SD_ONBOARD // Marlin uses the SD drive on the control board
+
+#ifdef LPC_SD_LCD
+ #define SCK_PIN P0_15
+ #define MISO_PIN P0_17
+ #define MOSI_PIN P0_18
+ #define SS_PIN P1_23 // Chip select for SD card used by Marlin
+ #define ONBOARD_SD_CS P0_06 // Chip select for "System" SD card
+#endif
+
+#ifdef LPC_SD_ONBOARD
+ #ifdef USB_SD_ONBOARD
+ // When sharing the SD card with a PC we want the menu options to
+ // mount/unmount the card and refresh it. So we disable card detect.
+ #define SHARED_SD_CARD
+ #undef SD_DETECT_PIN // there is also no detect pin for the onboard card
+ #endif
+ #define SCK_PIN P0_07
+ #define MISO_PIN P0_08
+ #define MOSI_PIN P0_09
+ #define SS_PIN P0_06 // Chip select for SD card used by Marlin
+ #define ONBOARD_SD_CS P0_06 // Chip select for "System" SD card
+#endif
+
/**
* Fast PWMS
*
diff --git a/frameworks/CMSIS/LPC1768/driver/debug_frmwrk.c b/frameworks/CMSIS/LPC1768/driver/debug_frmwrk.c
deleted file mode 100644
index 62e9b00016..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/debug_frmwrk.c
+++ /dev/null
@@ -1,306 +0,0 @@
-/**********************************************************************
-* $Id$ debug_frmwrk.c 2010-05-21
-*
-* @file debug_frmwrk.c
-* @brief Contains some utilities that used for debugging through UART
-* @version 2.0
-* @date 21. May. 2010
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2010, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-#include "debug_frmwrk.h"
-#include "lpc17xx_pinsel.h"
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
- #include "lpc17xx_libcfg.h"
-#else
- #include "lpc17xx_libcfg_default.h"
-#endif
-
-#ifdef _DBGFWK
-
-/* Debug framework */
-static Bool debug_frmwrk_initialized = FALSE;
-
-void (*_db_msg)(LPC_UART_TypeDef *UARTx, const void *s) = UARTPuts;
-void (*_db_msg_)(LPC_UART_TypeDef *UARTx, const void *s) = UARTPuts_;
-void (*_db_char)(LPC_UART_TypeDef *UARTx, uint8_t ch) = UARTPutChar;
-void (*_db_dec)(LPC_UART_TypeDef *UARTx, uint8_t decn) = UARTPutHex;
-void (*_db_dec_16)(LPC_UART_TypeDef *UARTx, uint16_t decn) = UARTPutHex16;
-void (*_db_dec_32)(LPC_UART_TypeDef *UARTx, uint32_t decn) = UARTPutHex32;
-void (*_db_hex)(LPC_UART_TypeDef *UARTx, uint8_t hexn) = UARTPutDec;
-void (*_db_hex_16)(LPC_UART_TypeDef *UARTx, uint16_t hexn) = UARTPutDec16;
-void (*_db_hex_32)(LPC_UART_TypeDef *UARTx, uint32_t hexn) = UARTPutDec32;
-uint8_t (*_db_get_char)(LPC_UART_TypeDef *UARTx) = UARTGetChar;
-
-
-/*********************************************************************//**
- * @brief Puts a character to UART port
- * @param[in] UARTx Pointer to UART peripheral
- * @param[in] ch Character to put
- * @return None
- **********************************************************************/
-void UARTPutChar(LPC_UART_TypeDef *UARTx, uint8_t ch) {
- if (debug_frmwrk_initialized)
- UART_Send(UARTx, &ch, 1, BLOCKING);
-}
-
-/*********************************************************************//**
- * @brief Get a character to UART port
- * @param[in] UARTx Pointer to UART peripheral
- * @return character value that returned
- **********************************************************************/
-uint8_t UARTGetChar(LPC_UART_TypeDef *UARTx) {
- uint8_t tmp = 0;
-
- if (debug_frmwrk_initialized)
- UART_Receive(UARTx, &tmp, 1, BLOCKING);
-
- return(tmp);
-}
-
-/*********************************************************************//**
- * @brief Puts a string to UART port
- * @param[in] UARTx Pointer to UART peripheral
- * @param[in] str string to put
- * @return None
- **********************************************************************/
-void UARTPuts(LPC_UART_TypeDef *UARTx, const void *str) {
- if (!debug_frmwrk_initialized) return;
-
- uint8_t *s = (uint8_t*)str;
- while (*s) UARTPutChar(UARTx, *s++);
-}
-
-/*********************************************************************//**
- * @brief Puts a string to UART port and print new line
- * @param[in] UARTx Pointer to UART peripheral
- * @param[in] str String to put
- * @return None
- **********************************************************************/
-void UARTPuts_(LPC_UART_TypeDef *UARTx, const void *str) {
- if (!debug_frmwrk_initialized) return;
-
- UARTPuts (UARTx, str);
- UARTPuts (UARTx, "\n\r");
-}
-
-/*********************************************************************//**
- * @brief Puts a decimal number to UART port
- * @param[in] UARTx Pointer to UART peripheral
- * @param[in] decnum Decimal number (8-bit long)
- * @return None
- **********************************************************************/
-void UARTPutDec(LPC_UART_TypeDef *UARTx, uint8_t decnum) {
- if (!debug_frmwrk_initialized) return;
-
- uint8_t c1 = decnum%10;
- uint8_t c2 = (decnum / 10) % 10;
- uint8_t c3 = (decnum / 100) % 10;
- UARTPutChar(UARTx, '0'+c3);
- UARTPutChar(UARTx, '0'+c2);
- UARTPutChar(UARTx, '0'+c1);
-}
-
-/*********************************************************************//**
- * @brief Puts a decimal number to UART port
- * @param[in] UARTx Pointer to UART peripheral
- * @param[in] decnum Decimal number (8-bit long)
- * @return None
- **********************************************************************/
-void UARTPutDec16(LPC_UART_TypeDef *UARTx, uint16_t decnum) {
- if (!debug_frmwrk_initialized) return;
-
- uint8_t c1 = decnum%10;
- uint8_t c2 = (decnum / 10) % 10;
- uint8_t c3 = (decnum / 100) % 10;
- uint8_t c4 = (decnum / 1000) % 10;
- uint8_t c5 = (decnum / 10000) % 10;
- UARTPutChar(UARTx, '0'+c5);
- UARTPutChar(UARTx, '0'+c4);
- UARTPutChar(UARTx, '0'+c3);
- UARTPutChar(UARTx, '0'+c2);
- UARTPutChar(UARTx, '0'+c1);
-}
-
-/*********************************************************************//**
- * @brief Puts a decimal number to UART port
- * @param[in] UARTx Pointer to UART peripheral
- * @param[in] decnum Decimal number (8-bit long)
- * @return None
- **********************************************************************/
-void UARTPutDec32(LPC_UART_TypeDef *UARTx, uint32_t decnum) {
- if (!debug_frmwrk_initialized) return;
-
- const uint8_t c1 = decnum % 10,
- c2 = (decnum / 10) % 10,
- c3 = (decnum / 100) % 10,
- c4 = (decnum / 1000) % 10,
- c5 = (decnum / 10000) % 10,
- c6 = (decnum / 100000) % 10,
- c7 = (decnum / 1000000) % 10,
- c8 = (decnum / 10000000) % 10,
- c9 = (decnum / 100000000) % 10,
- c10 = (decnum / 1000000000) % 10;
- UARTPutChar(UARTx, '0' + c10);
- UARTPutChar(UARTx, '0' + c9);
- UARTPutChar(UARTx, '0' + c8);
- UARTPutChar(UARTx, '0' + c7);
- UARTPutChar(UARTx, '0' + c6);
- UARTPutChar(UARTx, '0' + c5);
- UARTPutChar(UARTx, '0' + c4);
- UARTPutChar(UARTx, '0' + c3);
- UARTPutChar(UARTx, '0' + c2);
- UARTPutChar(UARTx, '0' + c1);
-}
-
-/*********************************************************************//**
- * @brief Puts a hex number to UART port
- * @param[in] UARTx Pointer to UART peripheral
- * @param[in] hexnum Hex number (8-bit long)
- * @return None
- **********************************************************************/
-void UARTPutHex(LPC_UART_TypeDef *UARTx, uint8_t hexnum) {
- if (!debug_frmwrk_initialized) return;
-
- UARTPuts(UARTx, "0x");
- uint8_t nibble, i = 1;
- do {
- nibble = (hexnum >> (4 * i)) & 0x0F;
- UARTPutChar(UARTx, (nibble > 9) ? ('A' + nibble - 10) : ('0' + nibble));
- } while (i--);
-}
-
-/*********************************************************************//**
- * @brief Puts a hex number to UART port
- * @param[in] UARTx Pointer to UART peripheral
- * @param[in] hexnum Hex number (16-bit long)
- * @return None
- **********************************************************************/
-void UARTPutHex16(LPC_UART_TypeDef *UARTx, uint16_t hexnum) {
- if (!debug_frmwrk_initialized) return;
-
- UARTPuts(UARTx, "0x");
- uint8_t nibble, i = 3;
- do {
- nibble = (hexnum >> (4 * i)) & 0x0F;
- UARTPutChar(UARTx, (nibble > 9) ? ('A' + nibble - 10) : ('0' + nibble));
- } while (i--);
-}
-
-/*********************************************************************//**
- * @brief Puts a hex number to UART port
- * @param[in] UARTx Pointer to UART peripheral
- * @param[in] hexnum Hex number (32-bit long)
- * @return None
- **********************************************************************/
-void UARTPutHex32(LPC_UART_TypeDef *UARTx, uint32_t hexnum) {
- if (!debug_frmwrk_initialized) return;
-
- UARTPuts(UARTx, "0x");
- uint8_t nibble, i = 7;
- do {
- nibble = (hexnum >> (4 * i)) & 0x0F;
- UARTPutChar(UARTx, (nibble > 9) ? ('A' + nibble - 10) : ('0' + nibble));
- } while (i--);
-}
-
-/*********************************************************************//**
- * @brief print function that supports format as same as printf()
- * function of library
- * @param[in] None
- * @return None
- **********************************************************************/
-//void _printf (const char *format, ...) {
-// static char buffer[512 + 1];
-// va_list vArgs;
-// char *tmp;
-// va_start(vArgs, format);
-// vsprintf((char *)buffer, (char const *)format, vArgs);
-// va_end(vArgs);
-//
-// _DBG(buffer);
-//}
-
-/*********************************************************************//**
- * @brief Initialize Debug frame work through initializing UART port
- * @param[in] None
- * @return None
- **********************************************************************/
-void debug_frmwrk_init(void) {
- UART_CFG_Type UARTConfigStruct;
- PINSEL_CFG_Type PinCfg;
-
- #if (USED_UART_DEBUG_PORT==0)
- /*
- * Initialize UART0 pin connect
- */
- PinCfg.Funcnum = 1;
- PinCfg.OpenDrain = 0;
- PinCfg.Pinmode = 0;
- PinCfg.Pinnum = 2;
- PinCfg.Portnum = 0;
- PINSEL_ConfigPin(&PinCfg);
- PinCfg.Pinnum = 3;
- PINSEL_ConfigPin(&PinCfg);
-
- #elif (USED_UART_DEBUG_PORT==1)
- /*
- * Initialize UART1 pin connect
- */
- PinCfg.Funcnum = 1;
- PinCfg.OpenDrain = 0;
- PinCfg.Pinmode = 0;
- PinCfg.Pinnum = 15;
- PinCfg.Portnum = 0;
- PINSEL_ConfigPin(&PinCfg);
- PinCfg.Pinnum = 16;
- PINSEL_ConfigPin(&PinCfg);
- #endif
-
- /* Initialize UART Configuration parameter structure to default state:
- * Baudrate = 9600bps
- * 8 data bit
- * 1 Stop bit
- * None parity
- */
- UART_ConfigStructInit(&UARTConfigStruct);
-
- // Re-configure baudrate to 115200bps
- UARTConfigStruct.Baud_rate = 115200;
-
- // Initialize DEBUG_UART_PORT peripheral with given to corresponding parameter
- UART_Init((LPC_UART_TypeDef *)DEBUG_UART_PORT, &UARTConfigStruct);
-
- // Enable UART Transmit
- UART_TxCmd((LPC_UART_TypeDef *)DEBUG_UART_PORT, ENABLE);
-
- debug_frmwrk_initialized = TRUE;
-}
-
-#endif // _DBGFWK
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_adc.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_adc.c
deleted file mode 100644
index 2b6574c39a..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_adc.c
+++ /dev/null
@@ -1,358 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_adc.c 2010-06-18
-*//**
-* @file lpc17xx_adc.c
-* @brief Contains all functions support for ADC firmware library on LPC17xx
-* @version 3.1
-* @date 26. July. 2011
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2011, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup ADC
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_adc.h"
-#include "lpc17xx_clkpwr.h"
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-
-#ifdef _ADC
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup ADC_Public_Functions
- * @{
- */
-
-/*********************************************************************//**
- * @brief Initial for ADC
- * + Set bit PCADC
- * + Set clock for ADC
- * + Set Clock Frequency
- * @param[in] ADCx pointer to LPC_ADC_TypeDef, should be: LPC_ADC
- * @param[in] rate ADC conversion rate, should be <=200KHz
- * @return None
- **********************************************************************/
-void ADC_Init(LPC_ADC_TypeDef *ADCx, uint32_t rate)
-{
- uint32_t ADCPClk, temp, tmp;
-
- CHECK_PARAM(PARAM_ADCx(ADCx));
- CHECK_PARAM(PARAM_ADC_RATE(rate));
-
- // Turn on power and clock
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCAD, ENABLE);
-
- ADCx->ADCR = 0;
-
- //Enable PDN bit
- tmp = ADC_CR_PDN;
- // Set clock frequency
- ADCPClk = CLKPWR_GetPCLK(CLKPWR_PCLKSEL_ADC);
- /* The APB clock (PCLK_ADC0) is divided by (CLKDIV+1) to produce the clock for
- * A/D converter, which should be less than or equal to 13MHz.
- * A fully conversion requires 65 of these clocks.
- * ADC clock = PCLK_ADC0 / (CLKDIV + 1);
- * ADC rate = ADC clock / 65;
- */
- temp = rate * 65;
- temp = (ADCPClk * 2 + temp)/(2 * temp) - 1; //get the round value by fomular: (2*A + B)/(2*B)
- tmp |= ADC_CR_CLKDIV(temp);
-
- ADCx->ADCR = tmp;
-}
-
-
-/*********************************************************************//**
-* @brief Close ADC
-* @param[in] ADCx pointer to LPC_ADC_TypeDef, should be: LPC_ADC
-* @return None
-**********************************************************************/
-void ADC_DeInit(LPC_ADC_TypeDef *ADCx)
-{
- CHECK_PARAM(PARAM_ADCx(ADCx));
- if (ADCx->ADCR & ADC_CR_START_MASK) //need to stop START bits before DeInit
- ADCx->ADCR &= ~ADC_CR_START_MASK;
- // Clear SEL bits
- ADCx->ADCR &= ~0xFF;
- // Clear PDN bit
- ADCx->ADCR &= ~ADC_CR_PDN;
- // Turn on power and clock
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCAD, DISABLE);
-}
-
-
-/*********************************************************************//**
-* @brief Get Result conversion from A/D data register
-* @param[in] channel number which want to read back the result
-* @return Result of conversion
-*********************************************************************/
-uint32_t ADC_GetData(uint32_t channel)
-{
- uint32_t adc_value;
-
- CHECK_PARAM(PARAM_ADC_CHANNEL_SELECTION(channel));
-
- adc_value = *(uint32_t *)((&LPC_ADC->ADDR0) + channel);
- return ADC_GDR_RESULT(adc_value);
-}
-
-/*********************************************************************//**
-* @brief Set start mode for ADC
-* @param[in] ADCx pointer to LPC_ADC_TypeDef, should be: LPC_ADC
-* @param[in] start_mode Start mode choose one of modes in
-* 'ADC_START_OPT' enumeration type definition, should be:
-* - ADC_START_CONTINUOUS
-* - ADC_START_NOW
-* - ADC_START_ON_EINT0
-* - ADC_START_ON_CAP01
-* - ADC_START_ON_MAT01
-* - ADC_START_ON_MAT03
-* - ADC_START_ON_MAT10
-* - ADC_START_ON_MAT11
-* @return None
-*********************************************************************/
-void ADC_StartCmd(LPC_ADC_TypeDef *ADCx, uint8_t start_mode)
-{
- CHECK_PARAM(PARAM_ADCx(ADCx));
- CHECK_PARAM(PARAM_ADC_START_OPT(start_mode));
-
- ADCx->ADCR &= ~ADC_CR_START_MASK;
- ADCx->ADCR |=ADC_CR_START_MODE_SEL((uint32_t)start_mode);
-}
-
-
-/*********************************************************************//**
-* @brief ADC Burst mode setting
-* @param[in] ADCx pointer to LPC_ADC_TypeDef, should be: LPC_ADC
-* @param[in] NewState
-* - 1: Set Burst mode
-* - 0: reset Burst mode
-* @return None
-**********************************************************************/
-void ADC_BurstCmd(LPC_ADC_TypeDef *ADCx, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_ADCx(ADCx));
-
- ADCx->ADCR &= ~ADC_CR_BURST;
- if (NewState){
- ADCx->ADCR |= ADC_CR_BURST;
- }
-}
-
-/*********************************************************************//**
-* @brief Set AD conversion in power mode
-* @param[in] ADCx pointer to LPC_ADC_TypeDef, should be: LPC_ADC
-* @param[in] NewState
-* - 1: AD converter is optional
-* - 0: AD Converter is in power down mode
-* @return None
-**********************************************************************/
-void ADC_PowerdownCmd(LPC_ADC_TypeDef *ADCx, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_ADCx(ADCx));
-
- ADCx->ADCR &= ~ADC_CR_PDN;
- if (NewState){
- ADCx->ADCR |= ADC_CR_PDN;
- }
-}
-
-/*********************************************************************//**
-* @brief Set Edge start configuration
-* @param[in] ADCx pointer to LPC_ADC_TypeDef, should be: LPC_ADC
-* @param[in] EdgeOption is ADC_START_ON_RISING and ADC_START_ON_FALLING
-* 0:ADC_START_ON_RISING
-* 1:ADC_START_ON_FALLING
-* @return None
-**********************************************************************/
-void ADC_EdgeStartConfig(LPC_ADC_TypeDef *ADCx, uint8_t EdgeOption)
-{
- CHECK_PARAM(PARAM_ADCx(ADCx));
- CHECK_PARAM(PARAM_ADC_START_ON_EDGE_OPT(EdgeOption));
-
- ADCx->ADCR &= ~ADC_CR_EDGE;
- if (EdgeOption){
- ADCx->ADCR |= ADC_CR_EDGE;
- }
-}
-
-/*********************************************************************//**
-* @brief ADC interrupt configuration
-* @param[in] ADCx pointer to LPC_ADC_TypeDef, should be: LPC_ADC
-* @param[in] IntType: type of interrupt, should be:
-* - ADC_ADINTEN0: Interrupt channel 0
-* - ADC_ADINTEN1: Interrupt channel 1
-* ...
-* - ADC_ADINTEN7: Interrupt channel 7
-* - ADC_ADGINTEN: Individual channel/global flag done generate an interrupt
-* @param[in] NewState:
-* - SET : enable ADC interrupt
-* - RESET: disable ADC interrupt
-* @return None
-**********************************************************************/
-void ADC_IntConfig (LPC_ADC_TypeDef *ADCx, ADC_TYPE_INT_OPT IntType, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_ADCx(ADCx));
- CHECK_PARAM(PARAM_ADC_TYPE_INT_OPT(IntType));
-
- ADCx->ADINTEN &= ~ADC_INTEN_CH(IntType);
- if (NewState){
- ADCx->ADINTEN |= ADC_INTEN_CH(IntType);
- }
-}
-
-/*********************************************************************//**
-* @brief Enable/Disable ADC channel number
-* @param[in] ADCx pointer to LPC_ADC_TypeDef, should be: LPC_ADC
-* @param[in] Channel channel number
-* @param[in] NewState Enable or Disable
-*
-* @return None
-**********************************************************************/
-void ADC_ChannelCmd (LPC_ADC_TypeDef *ADCx, uint8_t Channel, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_ADCx(ADCx));
- CHECK_PARAM(PARAM_ADC_CHANNEL_SELECTION(Channel));
-
- if (NewState == ENABLE) {
- ADCx->ADCR |= ADC_CR_CH_SEL(Channel);
- } else {
- if (ADCx->ADCR & ADC_CR_START_MASK) //need to stop START bits before disable channel
- ADCx->ADCR &= ~ADC_CR_START_MASK;
- ADCx->ADCR &= ~ADC_CR_CH_SEL(Channel);
- }
-}
-
-/*********************************************************************//**
-* @brief Get ADC result
-* @param[in] ADCx pointer to LPC_ADC_TypeDef, should be: LPC_ADC
-* @param[in] channel: channel number, should be 0...7
-* @return Data conversion
-**********************************************************************/
-uint16_t ADC_ChannelGetData(LPC_ADC_TypeDef *ADCx, uint8_t channel)
-{
- uint32_t adc_value;
-
- CHECK_PARAM(PARAM_ADCx(ADCx));
- CHECK_PARAM(PARAM_ADC_CHANNEL_SELECTION(channel));
-
- adc_value = *(uint32_t *) ((&ADCx->ADDR0) + channel);
- return ADC_DR_RESULT(adc_value);
-}
-
-/*********************************************************************//**
-* @brief Get ADC Chanel status from ADC data register
-* @param[in] ADCx pointer to LPC_ADC_TypeDef, should be: LPC_ADC
-* @param[in] channel: channel number, should be 0..7
-* @param[in] StatusType
-* 0:Burst status
-* 1:Done status
-* @return SET / RESET
-**********************************************************************/
-FlagStatus ADC_ChannelGetStatus(LPC_ADC_TypeDef *ADCx, uint8_t channel, uint32_t StatusType)
-{
- uint32_t temp;
-
- CHECK_PARAM(PARAM_ADCx(ADCx));
- CHECK_PARAM(PARAM_ADC_CHANNEL_SELECTION(channel));
- CHECK_PARAM(PARAM_ADC_DATA_STATUS(StatusType));
-
- temp = *(uint32_t *) ((&ADCx->ADDR0) + channel);
- if (StatusType) {
- temp &= ADC_DR_DONE_FLAG;
- }else{
- temp &= ADC_DR_OVERRUN_FLAG;
- }
- if (temp) {
- return SET;
- } else {
- return RESET;
- }
-
-}
-
-/*********************************************************************//**
-* @brief Get ADC Data from AD Global register
-* @param[in] ADCx pointer to LPC_ADC_TypeDef, should be: LPC_ADC
-* @return Result of conversion
-**********************************************************************/
-uint32_t ADC_GlobalGetData(LPC_ADC_TypeDef *ADCx)
-{
- CHECK_PARAM(PARAM_ADCx(ADCx));
-
- return ((uint32_t)(ADCx->ADGDR));
-}
-
-/*********************************************************************//**
-* @brief Get ADC Chanel status from AD global data register
-* @param[in] ADCx pointer to LPC_ADC_TypeDef, should be: LPC_ADC
-* @param[in] StatusType
-* 0:Burst status
-* 1:Done status
-* @return SET / RESET
-**********************************************************************/
-FlagStatus ADC_GlobalGetStatus(LPC_ADC_TypeDef *ADCx, uint32_t StatusType)
-{
- uint32_t temp;
-
- CHECK_PARAM(PARAM_ADCx(ADCx));
- CHECK_PARAM(PARAM_ADC_DATA_STATUS(StatusType));
-
- temp = ADCx->ADGDR;
- if (StatusType){
- temp &= ADC_DR_DONE_FLAG;
- }else{
- temp &= ADC_DR_OVERRUN_FLAG;
- }
- if (temp){
- return SET;
- }else{
- return RESET;
- }
-}
-
-/**
- * @}
- */
-
-#endif /* _ADC */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
-
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_can.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_can.c
deleted file mode 100644
index be8b767cfd..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_can.c
+++ /dev/null
@@ -1,1936 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_can.c 2011-03-09
-*//**
-* @file lpc17xx_can.c
-* @brief Contains all functions support for CAN firmware library on LPC17xx
-* @version 3.3
-* @date 09. March. 2011
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2011, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup CAN
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_can.h"
-#include "lpc17xx_clkpwr.h"
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-
-#ifdef _CAN
-
-/* Private Variables ---------------------------------------------------------- */
-/** @defgroup CAN_Private_Variables CAN Private Variables
- * @{
- */
-
-FunctionalState FULLCAN_ENABLE;
-
-
-/* Counts number of filters (CAN message objects) used */
-uint16_t CANAF_FullCAN_cnt = 0;
-uint16_t CANAF_std_cnt = 0;
-uint16_t CANAF_gstd_cnt = 0;
-uint16_t CANAF_ext_cnt = 0;
-uint16_t CANAF_gext_cnt = 0;
-
-/* End of Private Variables ----------------------------------------------------*/
-/**
- * @}
- */
-
-/* Private Variables ---------------------------------------------------------- */
-static void can_SetBaudrate (LPC_CAN_TypeDef *CANx, uint32_t baudrate);
-
-/*********************************************************************//**
- * @brief Setting CAN baud rate (bps)
- * @param[in] CANx point to LPC_CAN_TypeDef object, should be:
- * - LPC_CAN1: CAN1 peripheral
- * - LPC_CAN2: CAN2 peripheral
- * @param[in] baudrate: is the baud rate value will be set
- * @return None
- ***********************************************************************/
-static void can_SetBaudrate (LPC_CAN_TypeDef *CANx, uint32_t baudrate)
-{
- uint32_t result = 0;
- uint8_t NT, TSEG1, TSEG2, BRFail;
- uint32_t CANPclk = 0;
- uint32_t BRP;
- CHECK_PARAM(PARAM_CANx(CANx));
-
- if (CANx == LPC_CAN1)
- {
- CANPclk = CLKPWR_GetPCLK (CLKPWR_PCLKSEL_CAN1);
- }
- else
- {
- CANPclk = CLKPWR_GetPCLK (CLKPWR_PCLKSEL_CAN2);
- }
- result = CANPclk / baudrate;
- /* Calculate suitable nominal time value
- * NT (nominal time) = (TSEG1 + TSEG2 + 3)
- * NT <= 24
- * TSEG1 >= 2*TSEG2
- */
- BRFail = 1;
- for(NT=24;NT>0;NT=NT-2)
- {
- if ((result%NT)==0)
- {
- BRP = result / NT - 1;
- NT--;
- TSEG2 = (NT/3) - 1;
- TSEG1 = NT -(NT/3) - 1;
- BRFail = 0;
- break;
- }
- }
- if(BRFail)
- while(1); // Failed to calculate exact CAN baud rate
- /* Enter reset mode */
- CANx->MOD = 0x01;
- /* Set bit timing
- * Default: SAM = 0x00;
- * SJW = 0x03;
- */
- CANx->BTR = (TSEG2<<20)|(TSEG1<<16)|(3<<14)|BRP;
- /* Return to normal operating */
- CANx->MOD = 0;
-}
-/* End of Private Functions ----------------------------------------------------*/
-
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup CAN_Public_Functions
- * @{
- */
-
-/********************************************************************//**
- * @brief Initialize CAN peripheral with given baudrate
- * @param[in] CANx pointer to LPC_CAN_TypeDef, should be:
- * - LPC_CAN1: CAN1 peripheral
- * - LPC_CAN2: CAN2 peripheral
- * @param[in] baudrate: the value of CAN baudrate will be set (bps)
- * @return None
- *********************************************************************/
-void CAN_Init(LPC_CAN_TypeDef *CANx, uint32_t baudrate)
-{
- uint16_t i;
- CHECK_PARAM(PARAM_CANx(CANx));
-
- if(CANx == LPC_CAN1)
- {
- /* Turn on power and clock for CAN1 */
- CLKPWR_ConfigPPWR(CLKPWR_PCONP_PCAN1, ENABLE);
- /* Set clock divide for CAN1 */
- }
- else
- {
- /* Turn on power and clock for CAN1 */
- CLKPWR_ConfigPPWR(CLKPWR_PCONP_PCAN2, ENABLE);
- /* Set clock divide for CAN2 */
- }
- CLKPWR_SetPCLKDiv (CLKPWR_PCLKSEL_CAN1, CLKPWR_PCLKSEL_CCLK_DIV_2);
- CLKPWR_SetPCLKDiv (CLKPWR_PCLKSEL_CAN2, CLKPWR_PCLKSEL_CCLK_DIV_2);
- CLKPWR_SetPCLKDiv (CLKPWR_PCLKSEL_ACF, CLKPWR_PCLKSEL_CCLK_DIV_2);
-
- CANx->MOD = 1; // Enter Reset Mode
- CANx->IER = 0; // Disable All CAN Interrupts
- CANx->GSR = 0;
- /* Request command to release Rx, Tx buffer and clear data overrun */
- //CANx->CMR = CAN_CMR_AT | CAN_CMR_RRB | CAN_CMR_CDO;
- CANx->CMR = (1<<1)|(1<<2)|(1<<3);
- /* Read to clear interrupt pending in interrupt capture register */
- i = CANx->ICR;
- CANx->MOD = 0;// Return Normal operating
-
- //Reset CANAF value
- LPC_CANAF->AFMR = 0x01;
-
- //clear ALUT RAM
- for (i = 0; i < 512; i++) {
- LPC_CANAF_RAM->mask[i] = 0x00;
- }
-
- LPC_CANAF->SFF_sa = 0x00;
- LPC_CANAF->SFF_GRP_sa = 0x00;
- LPC_CANAF->EFF_sa = 0x00;
- LPC_CANAF->EFF_GRP_sa = 0x00;
- LPC_CANAF->ENDofTable = 0x00;
-
- LPC_CANAF->AFMR = 0x00;
- /* Set baudrate */
- can_SetBaudrate (CANx, baudrate);
-}
-
-/********************************************************************//**
- * @brief CAN deInit
- * @param[in] CANx pointer to LPC_CAN_TypeDef, should be:
- * - LPC_CAN1: CAN1 peripheral
- * - LPC_CAN2: CAN2 peripheral
- * @return None
- *********************************************************************/
-void CAN_DeInit(LPC_CAN_TypeDef *CANx)
-{
- CHECK_PARAM(PARAM_CANx(CANx));
-
- if(CANx == LPC_CAN1)
- {
- /* Turn on power and clock for CAN1 */
- CLKPWR_ConfigPPWR(CLKPWR_PCONP_PCAN1, DISABLE);
- }
- else
- {
- /* Turn on power and clock for CAN1 */
- CLKPWR_ConfigPPWR(CLKPWR_PCONP_PCAN2, DISABLE);
- }
-}
-
-/********************************************************************//**
- * @brief Setup Acceptance Filter Look-Up Table
- * @param[in] CANAFx pointer to LPC_CANAF_TypeDef
- * Should be: LPC_CANAF
- * @param[in] AFSection the pointer to AF_SectionDef structure
- * It contain information about 5 sections will be install in AFLUT
- * @return CAN Error could be:
- * - CAN_OBJECTS_FULL_ERROR: No more rx or tx objects available
- * - CAN_AF_ENTRY_ERROR: table error-violation of ascending numerical order
- * - CAN_OK: ID is added into table successfully
- *********************************************************************/
-CAN_ERROR CAN_SetupAFLUT(LPC_CANAF_TypeDef* CANAFx, AF_SectionDef* AFSection)
-{
- uint8_t ctrl1,ctrl2;
- uint8_t dis1, dis2;
- uint16_t SID, ID_temp,i, count = 0;
- uint32_t EID, entry, buf;
- uint16_t lowerSID, upperSID;
- uint32_t lowerEID, upperEID;
-
- CHECK_PARAM(PARAM_CANAFx(CANAFx));
- CANAFx->AFMR = 0x01;
-
-/***** setup FullCAN Table *****/
- if(AFSection->FullCAN_Sec == NULL)
- {
- FULLCAN_ENABLE = DISABLE;
- }
- else
- {
- FULLCAN_ENABLE = ENABLE;
- for(i=0;i<(AFSection->FC_NumEntry);i++)
- {
- if(count + 1 > 64)
- {
- return CAN_OBJECTS_FULL_ERROR;
- }
- ctrl1 = AFSection->FullCAN_Sec->controller;
- SID = AFSection->FullCAN_Sec->id_11;
- dis1 = AFSection->FullCAN_Sec->disable;
-
- CHECK_PARAM(PARAM_CTRL(ctrl1));
- CHECK_PARAM(PARAM_ID_11(SID));
- CHECK_PARAM(PARAM_MSG_DISABLE(dis1));
- entry = 0x00; //reset entry value
- if((CANAF_FullCAN_cnt & 0x00000001)==0)
- {
- if(count!=0x00)
- {
- buf = LPC_CANAF_RAM->mask[count-1];
- ID_temp = (buf & 0xE7FF); //mask controller & identifier bits
- if(ID_temp > ((ctrl1<<13)|SID))
- {
- return CAN_AF_ENTRY_ERROR;
- }
- }
- entry = (ctrl1<<29)|(dis1<<28)|(SID<<16)|(1<<27);
- LPC_CANAF_RAM->mask[count] &= 0x0000FFFF;
- LPC_CANAF_RAM->mask[count] |= entry;
- CANAF_FullCAN_cnt++;
- if(CANAF_FullCAN_cnt == AFSection->FC_NumEntry) //this is the lastest FullCAN entry
- count++;
- }
- else
- {
- buf = LPC_CANAF_RAM->mask[count];
- ID_temp = (buf >>16) & 0xE7FF;
- if(ID_temp > ((ctrl1<<13)|SID))
- {
- return CAN_AF_ENTRY_ERROR;
- }
- entry = (ctrl1<<13)|(dis1<<12)|(SID<<0)|(1<<11);
- LPC_CANAF_RAM->mask[count] &= 0xFFFF0000;
- LPC_CANAF_RAM->mask[count]|= entry;
- count++;
- CANAF_FullCAN_cnt++;
- }
- AFSection->FullCAN_Sec = (FullCAN_Entry *)((uint32_t)(AFSection->FullCAN_Sec)+ sizeof(FullCAN_Entry));
- }
- }
-
-/***** Setup Explicit Standard Frame Format Section *****/
- if(AFSection->SFF_Sec != NULL)
- {
- for(i=0;i<(AFSection->SFF_NumEntry);i++)
- {
- if(count + 1 > 512)
- {
- return CAN_OBJECTS_FULL_ERROR;
- }
- ctrl1 = AFSection->SFF_Sec->controller;
- SID = AFSection->SFF_Sec->id_11;
- dis1 = AFSection->SFF_Sec->disable;
-
- //check parameter
- CHECK_PARAM(PARAM_CTRL(ctrl1));
- CHECK_PARAM(PARAM_ID_11(SID));
- CHECK_PARAM(PARAM_MSG_DISABLE(dis1));
-
- entry = 0x00; //reset entry value
- if((CANAF_std_cnt & 0x00000001)==0)
- {
- if(CANAF_std_cnt !=0 )
- {
- buf = LPC_CANAF_RAM->mask[count-1];
- ID_temp = (buf & 0xE7FF); //mask controller & identifier bits
- if(ID_temp > ((ctrl1<<13)|SID))
- {
- return CAN_AF_ENTRY_ERROR;
- }
- }
- entry = (ctrl1<<29)|(dis1<<28)|(SID<<16);
- LPC_CANAF_RAM->mask[count] &= 0x0000FFFF;
- LPC_CANAF_RAM->mask[count] |= entry;
- CANAF_std_cnt++;
- if(CANAF_std_cnt == AFSection->SFF_NumEntry)//if this is the last SFF entry
- count++;
- }
- else
- {
- buf = LPC_CANAF_RAM->mask[count];
- ID_temp = (buf >>16) & 0xE7FF;
- if(ID_temp > ((ctrl1<<13)|SID))
- {
- return CAN_AF_ENTRY_ERROR;
- }
- entry = (ctrl1<<13)|(dis1<<12)|(SID<<0);
- LPC_CANAF_RAM->mask[count] &= 0xFFFF0000;
- LPC_CANAF_RAM->mask[count] |= entry;
- count++;
- CANAF_std_cnt++;
- }
- AFSection->SFF_Sec = (SFF_Entry *)((uint32_t)(AFSection->SFF_Sec)+ sizeof(SFF_Entry));
- }
- }
-
-/***** Setup Group of Standard Frame Format Identifier Section *****/
- if(AFSection->SFF_GPR_Sec != NULL)
- {
- for(i=0;i<(AFSection->SFF_GPR_NumEntry);i++)
- {
- if(count + 1 > 512)
- {
- return CAN_OBJECTS_FULL_ERROR;
- }
- ctrl1 = AFSection->SFF_GPR_Sec->controller1;
- ctrl2 = AFSection->SFF_GPR_Sec->controller2;
- dis1 = AFSection->SFF_GPR_Sec->disable1;
- dis2 = AFSection->SFF_GPR_Sec->disable2;
- lowerSID = AFSection->SFF_GPR_Sec->lowerID;
- upperSID = AFSection->SFF_GPR_Sec->upperID;
-
- /* check parameter */
- CHECK_PARAM(PARAM_CTRL(ctrl1));
- CHECK_PARAM(PARAM_CTRL(ctrl2));
- CHECK_PARAM(PARAM_MSG_DISABLE(dis1));
- CHECK_PARAM(PARAM_MSG_DISABLE(dis2));
- CHECK_PARAM(PARAM_ID_11(lowerSID));
- CHECK_PARAM(PARAM_ID_11(upperSID));
-
- entry = 0x00;
- if(CANAF_gstd_cnt!=0)
- {
- buf = LPC_CANAF_RAM->mask[count-1];
- ID_temp = buf & 0xE7FF;
- if((ctrl1 != ctrl2)||(lowerSID > upperSID)||(ID_temp > ((ctrl1<<13)|lowerSID)))
- {
- return CAN_AF_ENTRY_ERROR;
- }
- }
- entry = (ctrl1 << 29)|(dis1 << 28)|(lowerSID << 16)| \
- (ctrl2 << 13)|(dis2 << 12)|(upperSID << 0);
- LPC_CANAF_RAM->mask[count] = entry;
- CANAF_gstd_cnt++;
- count++;
- AFSection->SFF_GPR_Sec = (SFF_GPR_Entry *)((uint32_t)(AFSection->SFF_GPR_Sec)+ sizeof(SFF_GPR_Entry));
- }
- }
-
-/***** Setup Explicit Extend Frame Format Identifier Section *****/
- if(AFSection->EFF_Sec != NULL)
- {
- for(i=0;i<(AFSection->EFF_NumEntry);i++)
- {
- if(count + 1 > 512)
- {
- return CAN_OBJECTS_FULL_ERROR;
- }
- EID = AFSection->EFF_Sec->ID_29;
- ctrl1 = AFSection->EFF_Sec->controller;
-
- // check parameter
- CHECK_PARAM(PARAM_ID_29(EID));
- CHECK_PARAM(PARAM_CTRL(ctrl1));
-
- entry = (ctrl1 << 29)|(EID << 0);
- if(CANAF_ext_cnt != 0)
- {
- buf = LPC_CANAF_RAM->mask[count-1];
-// EID_temp = buf & 0x0FFFFFFF;
- if(buf > entry)
- {
- return CAN_AF_ENTRY_ERROR;
- }
- }
- LPC_CANAF_RAM->mask[count] = entry;
- CANAF_ext_cnt ++;
- count++;
- AFSection->EFF_Sec = (EFF_Entry *)((uint32_t)(AFSection->EFF_Sec)+ sizeof(EFF_Entry));
- }
- }
-
-/***** Setup Group of Extended Frame Format Identifier Section *****/
- if(AFSection->EFF_GPR_Sec != NULL)
- {
- for(i=0;i<(AFSection->EFF_GPR_NumEntry);i++)
- {
- if(count + 2 > 512)
- {
- return CAN_OBJECTS_FULL_ERROR;
- }
- ctrl1 = AFSection->EFF_GPR_Sec->controller1;
- ctrl2 = AFSection->EFF_GPR_Sec->controller2;
- lowerEID = AFSection->EFF_GPR_Sec->lowerEID;
- upperEID = AFSection->EFF_GPR_Sec->upperEID;
-
- //check parameter
- CHECK_PARAM(PARAM_CTRL(ctrl1));
- CHECK_PARAM(PARAM_CTRL(ctrl2));
- CHECK_PARAM(PARAM_ID_29(lowerEID));
- CHECK_PARAM(PARAM_ID_29(upperEID));
-
- entry = 0x00;
- if(CANAF_gext_cnt != 0)
- {
- buf = LPC_CANAF_RAM->mask[count-1];
-// EID_temp = buf & 0x0FFFFFFF;
- if((ctrl1 != ctrl2) || (lowerEID > upperEID) || (buf > ((ctrl1 << 29)|(lowerEID << 0))))
- {
- return CAN_AF_ENTRY_ERROR;
- }
- }
- entry = (ctrl1 << 29)|(lowerEID << 0);
- LPC_CANAF_RAM->mask[count++] = entry;
- entry = (ctrl2 << 29)|(upperEID << 0);
- LPC_CANAF_RAM->mask[count++] = entry;
- CANAF_gext_cnt++;
- AFSection->EFF_GPR_Sec = (EFF_GPR_Entry *)((uint32_t)(AFSection->EFF_GPR_Sec)+ sizeof(EFF_GPR_Entry));
- }
- }
- //update address values
- LPC_CANAF->SFF_sa = ((CANAF_FullCAN_cnt + 1)>>1)<<2;
- LPC_CANAF->SFF_GRP_sa = LPC_CANAF->SFF_sa + (((CANAF_std_cnt+1)>>1)<< 2);
- LPC_CANAF->EFF_sa = LPC_CANAF->SFF_GRP_sa + (CANAF_gstd_cnt << 2);
- LPC_CANAF->EFF_GRP_sa = LPC_CANAF->EFF_sa + (CANAF_ext_cnt << 2);
- LPC_CANAF->ENDofTable = LPC_CANAF->EFF_GRP_sa + (CANAF_gext_cnt << 3);
-
- if(FULLCAN_ENABLE == DISABLE)
- {
- LPC_CANAF->AFMR = 0x00; // Normal mode
- }
- else
- {
- LPC_CANAF->AFMR = 0x04;
- }
- return CAN_OK;
-}
-/********************************************************************//**
- * @brief Add Explicit ID into AF Look-Up Table dynamically.
- * @param[in] CANx pointer to LPC_CAN_TypeDef, should be:
- * - LPC_CAN1: CAN1 peripheral
- * - LPC_CAN2: CAN2 peripheral
- * @param[in] id: The ID of entry will be added
- * @param[in] format: is the type of ID Frame Format, should be:
- * - STD_ID_FORMAT: 11-bit ID value
- * - EXT_ID_FORMAT: 29-bit ID value
- * @return CAN Error, could be:
- * - CAN_OBJECTS_FULL_ERROR: No more rx or tx objects available
- * - CAN_ID_EXIT_ERROR: ID exited in table
- * - CAN_OK: ID is added into table successfully
- *********************************************************************/
-CAN_ERROR CAN_LoadExplicitEntry(LPC_CAN_TypeDef* CANx, uint32_t id, CAN_ID_FORMAT_Type format)
-{
- uint32_t tmp0 = 0;
- uint32_t buf0=0, buf1=0;
- int16_t cnt1=0, cnt2=0, bound1=0, total=0;
-
-
- CHECK_PARAM(PARAM_CANx(CANx));
- CHECK_PARAM(PARAM_ID_FORMAT(format));
-
- if (CANx == LPC_CAN1)
- {
- tmp0 = 0;
- }
- else if (CANx == LPC_CAN2)
- {
- tmp0 = 1;
- }
-
- /* Acceptance Filter Memory full - return */
- total =((CANAF_FullCAN_cnt+1)>>1)+ CANAF_FullCAN_cnt*3 +((CANAF_std_cnt + 1) >> 1)+ \
- CANAF_gstd_cnt + CANAF_ext_cnt + (CANAF_gext_cnt<<1);
- if (total >= 512){ //don't have enough space
- return CAN_OBJECTS_FULL_ERROR;
- }
-
- /* Setup Acceptance Filter Configuration
- Acceptance Filter Mode Register = Off */
- LPC_CANAF->AFMR = 0x00000001;
-
-/*********** Add Explicit Standard Identifier Frame Format entry *********/
- if(format == STD_ID_FORMAT)
- {
- id &= 0x07FF;
- id |= (tmp0 << 13); /* Add controller number */
- /* Move all remaining sections one place up
- if new entry will increase FullCAN list */
- if ((CANAF_std_cnt & 0x0001) == 0)
- {
- cnt1 = ((CANAF_FullCAN_cnt+1)>>1)+((CANAF_std_cnt+1)>>1);
- bound1 = total - cnt1;
- buf0 = LPC_CANAF_RAM->mask[cnt1];
- while(bound1--)
- {
- cnt1++;
- buf1 = LPC_CANAF_RAM->mask[cnt1];
- LPC_CANAF_RAM->mask[cnt1] = buf0;
- buf0 = buf1;
- }
- }
- if (CANAF_std_cnt == 0)
- {
- cnt2 = (CANAF_FullCAN_cnt + 1)>>1;
- /* For entering first ID */
- LPC_CANAF_RAM->mask[cnt2] = 0x0000FFFF | (id << 16);
- }
- else if (CANAF_std_cnt == 1)
- {
- cnt2 = (CANAF_FullCAN_cnt + 1)>>1;
- /* For entering second ID */
- if (((LPC_CANAF_RAM->mask[cnt2] >> 16)& 0xE7FF) > id)
- {
- LPC_CANAF_RAM->mask[cnt2] = (LPC_CANAF_RAM->mask[cnt2] >> 16) | (id << 16);
- }
- else
- {
- LPC_CANAF_RAM->mask[cnt2] = (LPC_CANAF_RAM->mask[cnt2] & 0xFFFF0000) | id;
- }
- }
- else
- {
- /* Find where to insert new ID */
- cnt1 = (CANAF_FullCAN_cnt+1)>>1;
- cnt2 = CANAF_std_cnt;
- bound1 = ((CANAF_FullCAN_cnt+1)>>1)+((CANAF_std_cnt+1)>>1);
- while (cnt1 < bound1)
- {
- /* Loop through standard existing IDs */
- if (((LPC_CANAF_RAM->mask[cnt1] >> 16) & 0xE7FF) > id)
- {
- cnt2 = cnt1 * 2;
- break;
- }
-
- if ((LPC_CANAF_RAM->mask[cnt1] & 0x0000E7FF) > id)
- {
- cnt2 = cnt1 * 2 + 1;
- break;
- }
-
- cnt1++;
- }
- /* cnt1 = U32 where to insert new ID */
- /* cnt2 = U16 where to insert new ID */
-
- if (cnt1 == bound1)
- {
- /* Adding ID as last entry */
- /* Even number of IDs exists */
- if ((CANAF_std_cnt & 0x0001) == 0)
- {
- LPC_CANAF_RAM->mask[cnt1] = 0x0000FFFF | (id << 16);
- }
- /* Odd number of IDs exists */
- else
- {
- LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | id;
- }
- }
- else
- {
- buf0 = LPC_CANAF_RAM->mask[cnt1]; /* Remember current entry */
- if ((cnt2 & 0x0001) == 0)
- {
- /* Insert new mask to even address*/
- buf1 = (id << 16) | (buf0 >> 16);
- }
- else
- {
- /* Insert new mask to odd address */
- buf1 = (buf0 & 0xFFFF0000) | id;
- }
- LPC_CANAF_RAM->mask[cnt1] = buf1;/* Insert mask */
- bound1 = ((CANAF_FullCAN_cnt+1)>>1)+((CANAF_std_cnt+1)>>1)-1;
- /* Move all remaining standard mask entries one place up */
- while (cnt1 < bound1)
- {
- cnt1++;
- buf1 = LPC_CANAF_RAM->mask[cnt1];
- LPC_CANAF_RAM->mask[cnt1] = (buf1 >> 16) | (buf0 << 16);
- buf0 = buf1;
- }
-
- if ((CANAF_std_cnt & 0x0001) == 0)
- {
- /* Even number of IDs exists */
- LPC_CANAF_RAM->mask[cnt1+1] = (buf0 <<16) |(0x0000FFFF);
- }
- }
- }
- CANAF_std_cnt++;
- //update address values
- LPC_CANAF->SFF_GRP_sa +=0x04 ;
- LPC_CANAF->EFF_sa +=0x04 ;
- LPC_CANAF->EFF_GRP_sa +=0x04;
- LPC_CANAF->ENDofTable +=0x04;
- }
-
-/*********** Add Explicit Extended Identifier Frame Format entry *********/
- else
- {
- /* Add controller number */
- id |= (tmp0) << 29;
-
- cnt1 = ((CANAF_FullCAN_cnt+1)>>1)+(((CANAF_std_cnt + 1) >> 1) + CANAF_gstd_cnt);
- cnt2 = 0;
- while (cnt2 < CANAF_ext_cnt)
- {
- /* Loop through extended existing masks*/
- if (LPC_CANAF_RAM->mask[cnt1] > id)
- {
- break;
- }
- cnt1++;/* cnt1 = U32 where to insert new mask */
- cnt2++;
- }
-
- buf0 = LPC_CANAF_RAM->mask[cnt1]; /* Remember current entry */
- LPC_CANAF_RAM->mask[cnt1] = id; /* Insert mask */
-
- CANAF_ext_cnt++;
-
- bound1 = total;
- /* Move all remaining extended mask entries one place up*/
- while (cnt2 < bound1)
- {
- cnt1++;
- cnt2++;
- buf1 = LPC_CANAF_RAM->mask[cnt1];
- LPC_CANAF_RAM->mask[cnt1] = buf0;
- buf0 = buf1;
- }
- /* update address values */
- LPC_CANAF->EFF_GRP_sa += 4;
- LPC_CANAF->ENDofTable += 4;
- }
- if(CANAF_FullCAN_cnt == 0) //not use FullCAN mode
- {
- LPC_CANAF->AFMR = 0x00;//not use FullCAN mode
- }
- else
- {
- LPC_CANAF->AFMR = 0x04;
- }
-
- return CAN_OK;
-}
-
-/********************************************************************//**
- * @brief Load FullCAN entry into AFLUT
- * @param[in] CANx: CAN peripheral selected, should be:
- * - LPC_CAN1: CAN1 peripheral
- * - LPC_CAN2: CAN2 peripheral
- * @param[in] id: identifier of entry that will be added
- * @return CAN_ERROR, could be:
- * - CAN_OK: loading is successful
- * - CAN_ID_EXIT_ERROR: ID exited in FullCAN Section
- * - CAN_OBJECTS_FULL_ERROR: no more space available
- *********************************************************************/
-CAN_ERROR CAN_LoadFullCANEntry (LPC_CAN_TypeDef* CANx, uint16_t id)
-{
- uint32_t ctrl0 = 0;
- uint32_t buf0=0, buf1=0, buf2=0;
- uint32_t tmp0=0, tmp1=0, tmp2=0;
- int16_t cnt1=0, cnt2=0, bound1=0, total=0;
-
- CHECK_PARAM(PARAM_CANx(CANx));
-
- if (CANx == LPC_CAN1)
- {
- ctrl0 = 0;
- }
- else if (CANx == LPC_CAN2)
- {
- ctrl0 = 1;
- }
-
- /* Acceptance Filter Memory full - return */
- total =((CANAF_FullCAN_cnt+1)>>1)+ CANAF_FullCAN_cnt*3 +((CANAF_std_cnt + 1) >> 1)+ \
- CANAF_gstd_cnt + CANAF_ext_cnt + (CANAF_gext_cnt<<1);
- //don't have enough space for this fullCAN Entry and its Object(3*32 bytes)
- if ((total >=508)||(CANAF_FullCAN_cnt>=64)){
- return CAN_OBJECTS_FULL_ERROR;
- }
- /* Setup Acceptance Filter Configuration
- Acceptance Filter Mode Register = Off */
- LPC_CANAF->AFMR = 0x00000001;
-
- /* Add mask for standard identifiers */
- id &= 0x07FF;
- id |= (ctrl0 << 13) | (1 << 11); /* Add controller number */
-// total = ((CANAF_std_cnt + 1) >> 1)+ CANAF_gstd_cnt + CANAF_ext_cnt + (CANAF_gext_cnt<<1);
- /* Move all remaining sections one place up
- if new entry will increase FullCAN list */
- if (((CANAF_FullCAN_cnt & 0x0001) == 0)&&(total!=0))
- {
- //then remove remaining section
- cnt1 = (CANAF_FullCAN_cnt >> 1);
- bound1 = total;
- buf0 = LPC_CANAF_RAM->mask[cnt1];
-
- while (bound1--)
- {
- cnt1++;
- buf1 = LPC_CANAF_RAM->mask[cnt1];
- LPC_CANAF_RAM->mask[cnt1] = buf0;
- buf0 = buf1;
- }
- }
- if (CANAF_FullCAN_cnt == 0)
- {
- /* For entering first ID */
- LPC_CANAF_RAM->mask[0] = 0x0000FFFF | (id << 16);
- }
- else if (CANAF_FullCAN_cnt == 1)
- {
- /* For entering second ID */
- if (((LPC_CANAF_RAM->mask[0] >> 16)& 0xE7FF) > id)
- {
- LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] >> 16) | (id << 16);
- }
- else
- {
- LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] & 0xFFFF0000) | id;
- }
- }
- else
- {
- /* Find where to insert new ID */
- cnt1 = 0;
- cnt2 = CANAF_FullCAN_cnt;
- bound1 = (CANAF_FullCAN_cnt - 1) >> 1;
- while (cnt1 <= bound1)
- {
- /* Loop through standard existing IDs */
- if (((LPC_CANAF_RAM->mask[cnt1] >> 16) & 0xE7FF) > (id & 0xE7FF))
- {
- cnt2 = cnt1 * 2;
- break;
- }
-
- if ((LPC_CANAF_RAM->mask[cnt1] & 0x0000E7FF) > (id & 0xE7FF))
- {
- cnt2 = cnt1 * 2 + 1;
- break;
- }
-
- cnt1++;
- }
- /* cnt1 = U32 where to insert new ID */
- /* cnt2 = U16 where to insert new ID */
-
- if (cnt1 > bound1)
- {
- /* Adding ID as last entry */
- /* Even number of IDs exists */
- if ((CANAF_FullCAN_cnt & 0x0001) == 0)
- {
- LPC_CANAF_RAM->mask[cnt1] = 0x0000FFFF | (id << 16);
- }
- /* Odd number of IDs exists */
- else
- {
- LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | id;
- }
- }
- else
- {
- buf0 = LPC_CANAF_RAM->mask[cnt1]; /* Remember current entry */
- if ((cnt2 & 0x0001) == 0)
- {
- /* Insert new mask to even address*/
- buf1 = (id << 16) | (buf0 >> 16);
- }
- else
- {
- /* Insert new mask to odd address */
- buf1 = (buf0 & 0xFFFF0000) | id;
- }
- LPC_CANAF_RAM->mask[cnt1] = buf1;/* Insert mask */
- bound1 = CANAF_FullCAN_cnt >> 1;
- /* Move all remaining standard mask entries one place up */
- while (cnt1 < bound1)
- {
- cnt1++;
- buf1 = LPC_CANAF_RAM->mask[cnt1];
- LPC_CANAF_RAM->mask[cnt1] = (buf1 >> 16) | (buf0 << 16);
- buf0 = buf1;
- }
-
- if ((CANAF_FullCAN_cnt & 0x0001) == 0)
- {
- /* Even number of IDs exists */
- LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000)
- | (0x0000FFFF);
- }
- }
- }
- //restruct FulCAN Object Section
- bound1 = CANAF_FullCAN_cnt - cnt2;
- cnt1 = total - (CANAF_FullCAN_cnt)*3 + cnt2*3 + 1;
- buf0 = LPC_CANAF_RAM->mask[cnt1];
- buf1 = LPC_CANAF_RAM->mask[cnt1+1];
- buf2 = LPC_CANAF_RAM->mask[cnt1+2];
- LPC_CANAF_RAM->mask[cnt1]=LPC_CANAF_RAM->mask[cnt1+1]= LPC_CANAF_RAM->mask[cnt1+2]=0x00;
- cnt1+=3;
- while(bound1--)
- {
- tmp0 = LPC_CANAF_RAM->mask[cnt1];
- tmp1 = LPC_CANAF_RAM->mask[cnt1+1];
- tmp2 = LPC_CANAF_RAM->mask[cnt1+2];
- LPC_CANAF_RAM->mask[cnt1]= buf0;
- LPC_CANAF_RAM->mask[cnt1+1]= buf1;
- LPC_CANAF_RAM->mask[cnt1+2]= buf2;
- buf0 = tmp0;
- buf1 = tmp1;
- buf2 = tmp2;
- cnt1+=3;
- }
- CANAF_FullCAN_cnt++;
- //update address values
- LPC_CANAF->SFF_sa +=0x04;
- LPC_CANAF->SFF_GRP_sa +=0x04 ;
- LPC_CANAF->EFF_sa +=0x04 ;
- LPC_CANAF->EFF_GRP_sa +=0x04;
- LPC_CANAF->ENDofTable +=0x04;
-
- LPC_CANAF->AFMR = 0x04;
- return CAN_OK;
-}
-
-/********************************************************************//**
- * @brief Load Group entry into AFLUT
- * @param[in] CANx: CAN peripheral selected, should be:
- * - LPC_CAN1: CAN1 peripheral
- * - LPC_CAN2: CAN2 peripheral
- * @param[in] lowerID, upperID: lower and upper identifier of entry
- * @param[in] format: type of ID format, should be:
- * - STD_ID_FORMAT: Standard ID format (11-bit value)
- * - EXT_ID_FORMAT: Extended ID format (29-bit value)
- * @return CAN_ERROR, could be:
- * - CAN_OK: loading is successful
- * - CAN_CONFLICT_ID_ERROR: Conflict ID occurs
- * - CAN_OBJECTS_FULL_ERROR: no more space available
- *********************************************************************/
-CAN_ERROR CAN_LoadGroupEntry(LPC_CAN_TypeDef* CANx, uint32_t lowerID, \
- uint32_t upperID, CAN_ID_FORMAT_Type format)
-{
- uint16_t tmp = 0;
- uint32_t buf0, buf1, entry1, entry2, LID,UID;
- int16_t cnt1, bound1, total;
- //LPC_CANAF_RAM_TypeDef *AFLUTTest = LPC_CANAF_RAM;
-
- CHECK_PARAM(PARAM_CANx(CANx));
- CHECK_PARAM(PARAM_ID_FORMAT(format));
-
- if(lowerID > upperID) return CAN_CONFLICT_ID_ERROR;
- if(CANx == LPC_CAN1)
- {
- tmp = 0;
- }
- else
- {
- tmp = 1;
- }
-
- total =((CANAF_FullCAN_cnt+1)>>1)+ CANAF_FullCAN_cnt*3 +((CANAF_std_cnt + 1) >> 1)+ \
- CANAF_gstd_cnt + CANAF_ext_cnt + (CANAF_gext_cnt<<1);
-
- /* Setup Acceptance Filter Configuration
- Acceptance Filter Mode Register = Off */
- LPC_CANAF->AFMR = 0x00000001;
-
-/*********Add Group of Standard Identifier Frame Format************/
- if(format == STD_ID_FORMAT)
- {
- if ((total >= 512)){//don't have enough space
- return CAN_OBJECTS_FULL_ERROR;
- }
- lowerID &=0x7FF; //mask ID
- upperID &=0x7FF;
- entry1 = (tmp << 29)|(lowerID << 16)|(tmp << 13)|(upperID << 0);
- cnt1 = ((CANAF_FullCAN_cnt+1)>>1) + ((CANAF_std_cnt + 1) >> 1);
-
- //if this is the first Group standard ID entry
- if(CANAF_gstd_cnt == 0)
- {
- LPC_CANAF_RAM->mask[cnt1] = entry1;
- }
- else
- {
- //find the position to add new Group entry
- bound1 = ((CANAF_FullCAN_cnt+1)>>1) + ((CANAF_std_cnt + 1) >> 1) + CANAF_gstd_cnt;
- while(cnt1 < bound1)
- {
- //compare controller first
- while((LPC_CANAF_RAM->mask[cnt1] >> 29)< (entry1 >> 29))//increase until meet greater or equal controller
- cnt1++;
- buf0 = LPC_CANAF_RAM->mask[cnt1];
- if((LPC_CANAF_RAM->mask[cnt1] >> 29)> (entry1 >> 29)) //meet greater controller
- {
- //add at this position
- LPC_CANAF_RAM->mask[cnt1] = entry1;
- break;
- }
- else //meet equal controller
- {
- LID = (buf0 >> 16)&0x7FF;
- UID = buf0 & 0x7FF;
- if (upperID <= LID)
- {
- //add new entry before this entry
- LPC_CANAF_RAM->mask[cnt1] = entry1;
- break;
- }
- else if (lowerID >= UID)
- {
- //load next entry to compare
- cnt1 ++;
- }
- else
- return CAN_CONFLICT_ID_ERROR;
- }
- }
- if(cnt1 >= bound1)
- {
- //add new entry at the last position in this list
- buf0 = LPC_CANAF_RAM->mask[cnt1];
- LPC_CANAF_RAM->mask[cnt1] = entry1;
- }
-
- //remove all remaining entry of this section one place up
- bound1 = total - cnt1;
- while(bound1--)
- {
- cnt1++;
- buf1 = LPC_CANAF_RAM->mask[cnt1];
- LPC_CANAF_RAM->mask[cnt1] = buf0;
- buf0 = buf1;
- }
- }
- CANAF_gstd_cnt++;
- //update address values
- LPC_CANAF->EFF_sa +=0x04 ;
- LPC_CANAF->EFF_GRP_sa +=0x04;
- LPC_CANAF->ENDofTable +=0x04;
- }
-
-
-/*********Add Group of Extended Identifier Frame Format************/
- else
- {
- if ((total >= 511)){//don't have enough space
- return CAN_OBJECTS_FULL_ERROR;
- }
- lowerID &= 0x1FFFFFFF; //mask ID
- upperID &= 0x1FFFFFFF;
- entry1 = (tmp << 29)|(lowerID << 0);
- entry2 = (tmp << 29)|(upperID << 0);
-
- cnt1 = ((CANAF_FullCAN_cnt+1)>>1) + ((CANAF_std_cnt + 1) >> 1) + CANAF_gstd_cnt + CANAF_ext_cnt;
- //if this is the first Group standard ID entry
- if(CANAF_gext_cnt == 0)
- {
- LPC_CANAF_RAM->mask[cnt1] = entry1;
- LPC_CANAF_RAM->mask[cnt1+1] = entry2;
- }
- else
- {
- //find the position to add new Group entry
- bound1 = ((CANAF_FullCAN_cnt+1)>>1) + ((CANAF_std_cnt + 1) >> 1) + CANAF_gstd_cnt \
- + CANAF_ext_cnt + (CANAF_gext_cnt<<1);
- while(cnt1 < bound1)
- {
- while((LPC_CANAF_RAM->mask[cnt1] >>29)< tmp) //increase until meet greater or equal controller
- cnt1++;
- buf0 = LPC_CANAF_RAM->mask[cnt1];
- buf1 = LPC_CANAF_RAM->mask[cnt1+1];
- if((LPC_CANAF_RAM->mask[cnt1] >> 29)> (entry1 >> 29)) //meet greater controller
- {
- //add at this position
- LPC_CANAF_RAM->mask[cnt1] = entry1;
- LPC_CANAF_RAM->mask[++cnt1] = entry2;
- break;
- }
- else //meet equal controller
- {
- LID = buf0 & 0x1FFFFFFF; //mask ID
- UID = buf1 & 0x1FFFFFFF;
- if (upperID <= LID)
- {
- //add new entry before this entry
- LPC_CANAF_RAM->mask[cnt1] = entry1;
- LPC_CANAF_RAM->mask[++cnt1] = entry2;
- break;
- }
- else if (lowerID >= UID)
- {
- //load next entry to compare
- cnt1 +=2;
- }
- else
- return CAN_CONFLICT_ID_ERROR;
- }
- }
- if(cnt1 >= bound1)
- {
- //add new entry at the last position in this list
- buf0 = LPC_CANAF_RAM->mask[cnt1];
- buf1 = LPC_CANAF_RAM->mask[cnt1+1];
- LPC_CANAF_RAM->mask[cnt1] = entry1;
- LPC_CANAF_RAM->mask[++cnt1] = entry2;
- }
- //remove all remaining entry of this section two place up
- bound1 = total - cnt1 + 1;
- cnt1++;
- while(bound1>0)
- {
- entry1 = LPC_CANAF_RAM->mask[cnt1];
- entry2 = LPC_CANAF_RAM->mask[cnt1+1];
- LPC_CANAF_RAM->mask[cnt1] = buf0;
- LPC_CANAF_RAM->mask[cnt1+1] = buf1;
- buf0 = entry1;
- buf1 = entry2;
- cnt1 +=2;
- bound1 -=2;
- }
- }
- CANAF_gext_cnt++;
- //update address values
- LPC_CANAF->ENDofTable +=0x08;
- }
- LPC_CANAF->AFMR = 0x04;
- return CAN_OK;
-}
-
-/********************************************************************//**
- * @brief Remove AFLUT entry (FullCAN entry and Explicit Standard entry)
- * @param[in] EntryType: the type of entry that want to remove, should be:
- * - FULLCAN_ENTRY
- * - EXPLICIT_STANDARD_ENTRY
- * - GROUP_STANDARD_ENTRY
- * - EXPLICIT_EXTEND_ENTRY
- * - GROUP_EXTEND_ENTRY
- * @param[in] position: the position of this entry in its section
- * Note: the first position is 0
- * @return CAN_ERROR, could be:
- * - CAN_OK: removing is successful
- * - CAN_ENTRY_NOT_EXIT_ERROR: entry want to remove is not exit
- *********************************************************************/
-CAN_ERROR CAN_RemoveEntry(AFLUT_ENTRY_Type EntryType, uint16_t position)
-{
- uint16_t cnt, bound, total;
- uint32_t buf0, buf1;
- CHECK_PARAM(PARAM_AFLUT_ENTRY_TYPE(EntryType));
- CHECK_PARAM(PARAM_POSITION(position));
-
- /* Setup Acceptance Filter Configuration
- Acceptance Filter Mode Register = Off */
- LPC_CANAF->AFMR = 0x00000001;
- total = ((CANAF_FullCAN_cnt+1)>>1)+((CANAF_std_cnt + 1) >> 1) + \
- CANAF_gstd_cnt + CANAF_ext_cnt + (CANAF_gext_cnt<<1);
-
-
-/************** Remove FullCAN Entry *************/
- if(EntryType == FULLCAN_ENTRY)
- {
- if((CANAF_FullCAN_cnt==0)||(position >= CANAF_FullCAN_cnt))
- {
- return CAN_ENTRY_NOT_EXIT_ERROR;
- }
- else
- {
- cnt = position >> 1;
- buf0 = LPC_CANAF_RAM->mask[cnt];
- bound = (CANAF_FullCAN_cnt - position -1)>>1;
- if((position & 0x0001) == 0) //event position
- {
- while(bound--)
- {
- //remove all remaining FullCAN entry one place down
- buf1 = LPC_CANAF_RAM->mask[cnt+1];
- LPC_CANAF_RAM->mask[cnt] = (buf1 >> 16) | (buf0 << 16);
- buf0 = buf1;
- cnt++;
- }
- }
- else //odd position
- {
- while(bound--)
- {
- //remove all remaining FullCAN entry one place down
- buf1 = LPC_CANAF_RAM->mask[cnt+1];
- LPC_CANAF_RAM->mask[cnt] = (buf0 & 0xFFFF0000)|(buf1 >> 16);
- LPC_CANAF_RAM->mask[cnt+1] = LPC_CANAF_RAM->mask[cnt+1] << 16;
- buf0 = buf1<<16;
- cnt++;
- }
- }
- if((CANAF_FullCAN_cnt & 0x0001) == 0)
- {
- if((position & 0x0001)==0)
- LPC_CANAF_RAM->mask[cnt] = (buf0 << 16) | (0x0000FFFF);
- else
- LPC_CANAF_RAM->mask[cnt] = buf0 | 0x0000FFFF;
- }
- else
- {
- //remove all remaining section one place down
- cnt = (CANAF_FullCAN_cnt + 1)>>1;
- bound = total + CANAF_FullCAN_cnt * 3;
- while(bound>cnt)
- {
- LPC_CANAF_RAM->mask[cnt-1] = LPC_CANAF_RAM->mask[cnt];
- cnt++;
- }
- LPC_CANAF_RAM->mask[cnt-1]=0x00;
- //update address values
- LPC_CANAF->SFF_sa -=0x04;
- LPC_CANAF->SFF_GRP_sa -=0x04 ;
- LPC_CANAF->EFF_sa -=0x04 ;
- LPC_CANAF->EFF_GRP_sa -=0x04;
- LPC_CANAF->ENDofTable -=0x04;
- }
- CANAF_FullCAN_cnt--;
-
- //delete its FullCAN Object in the FullCAN Object section
- //remove all remaining FullCAN Object three place down
- cnt = total + position * 3;
- bound = (CANAF_FullCAN_cnt - position + 1) * 3;
-
- while(bound)
- {
- LPC_CANAF_RAM->mask[cnt]=LPC_CANAF_RAM->mask[cnt+3];;
- LPC_CANAF_RAM->mask[cnt+1]=LPC_CANAF_RAM->mask[cnt+4];
- LPC_CANAF_RAM->mask[cnt+2]=LPC_CANAF_RAM->mask[cnt+5];
- bound -=3;
- cnt +=3;
- }
- }
- }
-
-/************** Remove Explicit Standard ID Entry *************/
- else if(EntryType == EXPLICIT_STANDARD_ENTRY)
- {
- if((CANAF_std_cnt==0)||(position >= CANAF_std_cnt))
- {
- return CAN_ENTRY_NOT_EXIT_ERROR;
- }
- else
- {
- cnt = ((CANAF_FullCAN_cnt+1)>>1)+ (position >> 1);
- buf0 = LPC_CANAF_RAM->mask[cnt];
- bound = (CANAF_std_cnt - position - 1)>>1;
- if((position & 0x0001) == 0) //event position
- {
- while(bound--)
- {
- //remove all remaining FullCAN entry one place down
- buf1 = LPC_CANAF_RAM->mask[cnt+1];
- LPC_CANAF_RAM->mask[cnt] = (buf1 >> 16) | (buf0 << 16);
- buf0 = buf1;
- cnt++;
- }
- }
- else //odd position
- {
- while(bound--)
- {
- //remove all remaining FullCAN entry one place down
- buf1 = LPC_CANAF_RAM->mask[cnt+1];
- LPC_CANAF_RAM->mask[cnt] = (buf0 & 0xFFFF0000)|(buf1 >> 16);
- LPC_CANAF_RAM->mask[cnt+1] = LPC_CANAF_RAM->mask[cnt+1] << 16;
- buf0 = buf1<<16;
- cnt++;
- }
- }
- if((CANAF_std_cnt & 0x0001) == 0)
- {
- if((position & 0x0001)==0)
- LPC_CANAF_RAM->mask[cnt] = (buf0 << 16) | (0x0000FFFF);
- else
- LPC_CANAF_RAM->mask[cnt] = buf0 | 0x0000FFFF;
- }
- else
- {
- //remove all remaining section one place down
- cnt = ((CANAF_FullCAN_cnt + 1)>>1) + ((CANAF_std_cnt + 1) >> 1);
- bound = total + CANAF_FullCAN_cnt * 3;
- while(bound>cnt)
- {
- LPC_CANAF_RAM->mask[cnt-1] = LPC_CANAF_RAM->mask[cnt];
- cnt++;
- }
- LPC_CANAF_RAM->mask[cnt-1]=0x00;
- //update address value
- LPC_CANAF->SFF_GRP_sa -=0x04 ;
- LPC_CANAF->EFF_sa -=0x04 ;
- LPC_CANAF->EFF_GRP_sa -=0x04;
- LPC_CANAF->ENDofTable -=0x04;
- }
- CANAF_std_cnt--;
- }
- }
-
-/************** Remove Group of Standard ID Entry *************/
- else if(EntryType == GROUP_STANDARD_ENTRY)
- {
- if((CANAF_gstd_cnt==0)||(position >= CANAF_gstd_cnt))
- {
- return CAN_ENTRY_NOT_EXIT_ERROR;
- }
- else
- {
- cnt = ((CANAF_FullCAN_cnt + 1)>>1) + ((CANAF_std_cnt + 1) >> 1)+ position + 1;
- bound = total + CANAF_FullCAN_cnt * 3;
- while (cntmask[cnt-1] = LPC_CANAF_RAM->mask[cnt];
- cnt++;
- }
- LPC_CANAF_RAM->mask[cnt-1]=0x00;
- }
- CANAF_gstd_cnt--;
- //update address value
- LPC_CANAF->EFF_sa -=0x04;
- LPC_CANAF->EFF_GRP_sa -=0x04;
- LPC_CANAF->ENDofTable -=0x04;
- }
-
-/************** Remove Explicit Extended ID Entry *************/
- else if(EntryType == EXPLICIT_EXTEND_ENTRY)
- {
- if((CANAF_ext_cnt==0)||(position >= CANAF_ext_cnt))
- {
- return CAN_ENTRY_NOT_EXIT_ERROR;
- }
- else
- {
- cnt = ((CANAF_FullCAN_cnt + 1)>>1) + ((CANAF_std_cnt + 1) >> 1)+ CANAF_gstd_cnt + position + 1;
- bound = total + CANAF_FullCAN_cnt * 3;
- while (cntmask[cnt-1] = LPC_CANAF_RAM->mask[cnt];
- cnt++;
- }
- LPC_CANAF_RAM->mask[cnt-1]=0x00;
- }
- CANAF_ext_cnt--;
- LPC_CANAF->EFF_GRP_sa -=0x04;
- LPC_CANAF->ENDofTable -=0x04;
- }
-
-/************** Remove Group of Extended ID Entry *************/
- else
- {
- if((CANAF_gext_cnt==0)||(position >= CANAF_gext_cnt))
- {
- return CAN_ENTRY_NOT_EXIT_ERROR;
- }
- else
- {
- cnt = total - (CANAF_gext_cnt<<1) + (position<<1);
- bound = total + CANAF_FullCAN_cnt * 3;
- while (cntmask[cnt] = LPC_CANAF_RAM->mask[cnt+2];
- LPC_CANAF_RAM->mask[cnt+1] = LPC_CANAF_RAM->mask[cnt+3];
- cnt+=2;
- }
- }
- CANAF_gext_cnt--;
- LPC_CANAF->ENDofTable -=0x08;
- }
- LPC_CANAF->AFMR = 0x04;
- return CAN_OK;
-}
-
-/********************************************************************//**
- * @brief Send message data
- * @param[in] CANx pointer to LPC_CAN_TypeDef, should be:
- * - LPC_CAN1: CAN1 peripheral
- * - LPC_CAN2: CAN2 peripheral
- * @param[in] CAN_Msg point to the CAN_MSG_Type Structure, it contains message
- * information such as: ID, DLC, RTR, ID Format
- * @return Status:
- * - SUCCESS: send message successfully
- * - ERROR: send message unsuccessfully
- *********************************************************************/
-Status CAN_SendMsg (LPC_CAN_TypeDef *CANx, CAN_MSG_Type *CAN_Msg)
-{
- uint32_t data;
- CHECK_PARAM(PARAM_CANx(CANx));
- CHECK_PARAM(PARAM_ID_FORMAT(CAN_Msg->format));
- if(CAN_Msg->format==STD_ID_FORMAT)
- {
- CHECK_PARAM(PARAM_ID_11(CAN_Msg->id));
- }
- else
- {
- CHECK_PARAM(PARAM_ID_29(CAN_Msg->id));
- }
- CHECK_PARAM(PARAM_DLC(CAN_Msg->len));
- CHECK_PARAM(PARAM_FRAME_TYPE(CAN_Msg->type));
-
- //Check status of Transmit Buffer 1
- if (CANx->SR & (1<<2))
- {
- /* Transmit Channel 1 is available */
- /* Write frame informations and frame data into its CANxTFI1,
- * CANxTID1, CANxTDA1, CANxTDB1 register */
- CANx->TFI1 &= ~0x000F0000;
- CANx->TFI1 |= (CAN_Msg->len)<<16;
- if(CAN_Msg->type == REMOTE_FRAME)
- {
- CANx->TFI1 |= (1<<30); //set bit RTR
- }
- else
- {
- CANx->TFI1 &= ~(1<<30);
- }
- if(CAN_Msg->format == EXT_ID_FORMAT)
- {
- CANx->TFI1 |= (0x80000000); //set bit FF
- }
- else
- {
- CANx->TFI1 &= ~(0x80000000);
- }
-
- /* Write CAN ID*/
- CANx->TID1 = CAN_Msg->id;
-
- /*Write first 4 data bytes*/
- data = (CAN_Msg->dataA[0])|(((CAN_Msg->dataA[1]))<<8)|((CAN_Msg->dataA[2])<<16)|((CAN_Msg->dataA[3])<<24);
- CANx->TDA1 = data;
-
- /*Write second 4 data bytes*/
- data = (CAN_Msg->dataB[0])|(((CAN_Msg->dataB[1]))<<8)|((CAN_Msg->dataB[2])<<16)|((CAN_Msg->dataB[3])<<24);
- CANx->TDB1 = data;
-
- /*Write transmission request*/
- CANx->CMR = 0x21;
- return SUCCESS;
- }
- //check status of Transmit Buffer 2
- else if(CANx->SR & (1<<10))
- {
- /* Transmit Channel 2 is available */
- /* Write frame informations and frame data into its CANxTFI2,
- * CANxTID2, CANxTDA2, CANxTDB2 register */
- CANx->TFI2 &= ~0x000F0000;
- CANx->TFI2 |= (CAN_Msg->len)<<16;
- if(CAN_Msg->type == REMOTE_FRAME)
- {
- CANx->TFI2 |= (1<<30); //set bit RTR
- }
- else
- {
- CANx->TFI2 &= ~(1<<30);
- }
- if(CAN_Msg->format == EXT_ID_FORMAT)
- {
- CANx->TFI2 |= (0x80000000); //set bit FF
- }
- else
- {
- CANx->TFI2 &= ~(0x80000000);
- }
-
- /* Write CAN ID*/
- CANx->TID2 = CAN_Msg->id;
-
- /*Write first 4 data bytes*/
- data = (CAN_Msg->dataA[0])|(((CAN_Msg->dataA[1]))<<8)|((CAN_Msg->dataA[2])<<16)|((CAN_Msg->dataA[3])<<24);
- CANx->TDA2 = data;
-
- /*Write second 4 data bytes*/
- data = (CAN_Msg->dataB[0])|(((CAN_Msg->dataB[1]))<<8)|((CAN_Msg->dataB[2])<<16)|((CAN_Msg->dataB[3])<<24);
- CANx->TDB2 = data;
-
- /*Write transmission request*/
- CANx->CMR = 0x41;
- return SUCCESS;
- }
- //check status of Transmit Buffer 3
- else if (CANx->SR & (1<<18))
- {
- /* Transmit Channel 3 is available */
- /* Write frame informations and frame data into its CANxTFI3,
- * CANxTID3, CANxTDA3, CANxTDB3 register */
- CANx->TFI3 &= ~0x000F0000;
- CANx->TFI3 |= (CAN_Msg->len)<<16;
- if(CAN_Msg->type == REMOTE_FRAME)
- {
- CANx->TFI3 |= (1<<30); //set bit RTR
- }
- else
- {
- CANx->TFI3 &= ~(1<<30);
- }
- if(CAN_Msg->format == EXT_ID_FORMAT)
- {
- CANx->TFI3 |= (0x80000000); //set bit FF
- }
- else
- {
- CANx->TFI3 &= ~(0x80000000);
- }
-
- /* Write CAN ID*/
- CANx->TID3 = CAN_Msg->id;
-
- /*Write first 4 data bytes*/
- data = (CAN_Msg->dataA[0])|(((CAN_Msg->dataA[1]))<<8)|((CAN_Msg->dataA[2])<<16)|((CAN_Msg->dataA[3])<<24);
- CANx->TDA3 = data;
-
- /*Write second 4 data bytes*/
- data = (CAN_Msg->dataB[0])|(((CAN_Msg->dataB[1]))<<8)|((CAN_Msg->dataB[2])<<16)|((CAN_Msg->dataB[3])<<24);
- CANx->TDB3 = data;
-
- /*Write transmission request*/
- CANx->CMR = 0x81;
- return SUCCESS;
- }
- else
- {
- return ERROR;
- }
-}
-
-/********************************************************************//**
- * @brief Receive message data
- * @param[in] CANx pointer to LPC_CAN_TypeDef, should be:
- * - LPC_CAN1: CAN1 peripheral
- * - LPC_CAN2: CAN2 peripheral
- * @param[in] CAN_Msg point to the CAN_MSG_Type Struct, it will contain received
- * message information such as: ID, DLC, RTR, ID Format
- * @return Status:
- * - SUCCESS: receive message successfully
- * - ERROR: receive message unsuccessfully
- *********************************************************************/
-Status CAN_ReceiveMsg (LPC_CAN_TypeDef *CANx, CAN_MSG_Type *CAN_Msg)
-{
- uint32_t data;
-
- CHECK_PARAM(PARAM_CANx(CANx));
-
- //check status of Receive Buffer
- if((CANx->SR &0x00000001))
- {
- /* Receive message is available */
- /* Read frame informations */
- CAN_Msg->format = (uint8_t)(((CANx->RFS) & 0x80000000)>>31);
- CAN_Msg->type = (uint8_t)(((CANx->RFS) & 0x40000000)>>30);
- CAN_Msg->len = (uint8_t)(((CANx->RFS) & 0x000F0000)>>16);
-
-
- /* Read CAN message identifier */
- CAN_Msg->id = CANx->RID;
-
- /* Read the data if received message was DATA FRAME */
- if (CAN_Msg->type == DATA_FRAME)
- {
- /* Read first 4 data bytes */
- data = CANx->RDA;
- *((uint8_t *) &CAN_Msg->dataA[0])= data & 0x000000FF;
- *((uint8_t *) &CAN_Msg->dataA[1])= (data & 0x0000FF00)>>8;;
- *((uint8_t *) &CAN_Msg->dataA[2])= (data & 0x00FF0000)>>16;
- *((uint8_t *) &CAN_Msg->dataA[3])= (data & 0xFF000000)>>24;
-
- /* Read second 4 data bytes */
- data = CANx->RDB;
- *((uint8_t *) &CAN_Msg->dataB[0])= data & 0x000000FF;
- *((uint8_t *) &CAN_Msg->dataB[1])= (data & 0x0000FF00)>>8;
- *((uint8_t *) &CAN_Msg->dataB[2])= (data & 0x00FF0000)>>16;
- *((uint8_t *) &CAN_Msg->dataB[3])= (data & 0xFF000000)>>24;
-
- /*release receive buffer*/
- CANx->CMR = 0x04;
- }
- else
- {
- /* Received Frame is a Remote Frame, not have data, we just receive
- * message information only */
- CANx->CMR = 0x04; /*release receive buffer*/
- return SUCCESS;
- }
- }
- else
- {
- // no receive message available
- return ERROR;
- }
- return SUCCESS;
-}
-
-/********************************************************************//**
- * @brief Receive FullCAN Object
- * @param[in] CANAFx: CAN Acceptance Filter register, should be: LPC_CANAF
- * @param[in] CAN_Msg point to the CAN_MSG_Type Struct, it will contain received
- * message information such as: ID, DLC, RTR, ID Format
- * @return CAN_ERROR, could be:
- * - CAN_FULL_OBJ_NOT_RCV: FullCAN Object is not be received
- * - CAN_OK: Received FullCAN Object successful
- *
- *********************************************************************/
-CAN_ERROR FCAN_ReadObj (LPC_CANAF_TypeDef* CANAFx, CAN_MSG_Type *CAN_Msg)
-{
- uint32_t *pSrc, data;
- uint32_t interrut_word, msg_idx, test_bit, head_idx, tail_idx;
-
- CHECK_PARAM(PARAM_CANAFx(CANAFx));
-
- interrut_word = 0;
-
- if (LPC_CANAF->FCANIC0 != 0)
- {
- interrut_word = LPC_CANAF->FCANIC0;
- head_idx = 0;
- tail_idx = 31;
- }
- else if (LPC_CANAF->FCANIC1 != 0)
- {
- interrut_word = LPC_CANAF->FCANIC1;
- head_idx = 32;
- tail_idx = 63;
- }
-
- if (interrut_word != 0)
- {
- /* Detect for interrupt pending */
- msg_idx = 0;
- for (msg_idx = head_idx; msg_idx <= tail_idx; msg_idx++)
- {
- test_bit = interrut_word & 0x1;
- interrut_word = interrut_word >> 1;
-
- if (test_bit)
- {
- pSrc = (uint32_t *) (LPC_CANAF->ENDofTable + LPC_CANAF_RAM_BASE + msg_idx * 12);
-
- /* Has been finished updating the content */
- if ((*pSrc & 0x03000000L) == 0x03000000L)
- {
- /*clear semaphore*/
- *pSrc &= 0xFCFFFFFF;
-
- /*Set to DatA*/
- pSrc++;
- /* Copy to dest buf */
- data = *pSrc;
- *((uint8_t *) &CAN_Msg->dataA[0])= data & 0x000000FF;
- *((uint8_t *) &CAN_Msg->dataA[1])= (data & 0x0000FF00)>>8;
- *((uint8_t *) &CAN_Msg->dataA[2])= (data & 0x00FF0000)>>16;
- *((uint8_t *) &CAN_Msg->dataA[3])= (data & 0xFF000000)>>24;
-
- /*Set to DatB*/
- pSrc++;
- /* Copy to dest buf */
- data = *pSrc;
- *((uint8_t *) &CAN_Msg->dataB[0])= data & 0x000000FF;
- *((uint8_t *) &CAN_Msg->dataB[1])= (data & 0x0000FF00)>>8;
- *((uint8_t *) &CAN_Msg->dataB[2])= (data & 0x00FF0000)>>16;
- *((uint8_t *) &CAN_Msg->dataB[3])= (data & 0xFF000000)>>24;
- /*Back to Dat1*/
- pSrc -= 2;
-
- CAN_Msg->id = *pSrc & 0x7FF;
- CAN_Msg->len = (uint8_t) (*pSrc >> 16) & 0x0F;
- CAN_Msg->format = 0; //FullCAN Object ID always is 11-bit value
- CAN_Msg->type = (uint8_t)(*pSrc >> 30) &0x01;
- /*Re-read semaphore*/
- if ((*pSrc & 0x03000000L) == 0)
- {
- return CAN_OK;
- }
- }
- }
- }
- }
- return CAN_FULL_OBJ_NOT_RCV;
-}
-/********************************************************************//**
- * @brief Get CAN Control Status
- * @param[in] CANx pointer to LPC_CAN_TypeDef, should be:
- * - LPC_CAN1: CAN1 peripheral
- * - LPC_CAN2: CAN2 peripheral
- * @param[in] arg: type of CAN status to get from CAN status register
- * Should be:
- * - CANCTRL_GLOBAL_STS: CAN Global Status
- * - CANCTRL_INT_CAP: CAN Interrupt and Capture
- * - CANCTRL_ERR_WRN: CAN Error Warning Limit
- * - CANCTRL_STS: CAN Control Status
- * @return Current Control Status that you want to get value
- *********************************************************************/
-uint32_t CAN_GetCTRLStatus (LPC_CAN_TypeDef* CANx, CAN_CTRL_STS_Type arg)
-{
- CHECK_PARAM(PARAM_CANx(CANx));
- CHECK_PARAM(PARAM_CTRL_STS_TYPE(arg));
-
- switch (arg)
- {
- case CANCTRL_GLOBAL_STS:
- return CANx->GSR;
-
- case CANCTRL_INT_CAP:
- return CANx->ICR;
-
- case CANCTRL_ERR_WRN:
- return CANx->EWL;
-
- default: // CANCTRL_STS
- return CANx->SR;
- }
-}
-/********************************************************************//**
- * @brief Get CAN Central Status
- * @param[in] CANCRx point to LPC_CANCR_TypeDef, should be: LPC_CANCR
- * @param[in] arg: type of CAN status to get from CAN Central status register
- * Should be:
- * - CANCR_TX_STS: Central CAN Tx Status
- * - CANCR_RX_STS: Central CAN Rx Status
- * - CANCR_MS: Central CAN Miscellaneous Status
- * @return Current Central Status that you want to get value
- *********************************************************************/
-uint32_t CAN_GetCRStatus (LPC_CANCR_TypeDef* CANCRx, CAN_CR_STS_Type arg)
-{
- CHECK_PARAM(PARAM_CANCRx(CANCRx));
- CHECK_PARAM(PARAM_CR_STS_TYPE(arg));
-
- switch (arg)
- {
- case CANCR_TX_STS:
- return CANCRx->CANTxSR;
-
- case CANCR_RX_STS:
- return CANCRx->CANRxSR;
-
- default: // CANCR_MS
- return CANCRx->CANMSR;
- }
-}
-/********************************************************************//**
- * @brief Enable/Disable CAN Interrupt
- * @param[in] CANx pointer to LPC_CAN_TypeDef, should be:
- * - LPC_CAN1: CAN1 peripheral
- * - LPC_CAN2: CAN2 peripheral
- * @param[in] arg: type of CAN interrupt that you want to enable/disable
- * Should be:
- * - CANINT_RIE: CAN Receiver Interrupt Enable
- * - CANINT_TIE1: CAN Transmit Interrupt Enable
- * - CANINT_EIE: CAN Error Warning Interrupt Enable
- * - CANINT_DOIE: CAN Data Overrun Interrupt Enable
- * - CANINT_WUIE: CAN Wake-Up Interrupt Enable
- * - CANINT_EPIE: CAN Error Passive Interrupt Enable
- * - CANINT_ALIE: CAN Arbitration Lost Interrupt Enable
- * - CANINT_BEIE: CAN Bus Error Interrupt Enable
- * - CANINT_IDIE: CAN ID Ready Interrupt Enable
- * - CANINT_TIE2: CAN Transmit Interrupt Enable for Buffer2
- * - CANINT_TIE3: CAN Transmit Interrupt Enable for Buffer3
- * - CANINT_FCE: FullCAN Interrupt Enable
- * @param[in] NewState: New state of this function, should be:
- * - ENABLE
- * - DISABLE
- * @return none
- *********************************************************************/
-void CAN_IRQCmd (LPC_CAN_TypeDef* CANx, CAN_INT_EN_Type arg, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_CANx(CANx));
- CHECK_PARAM(PARAM_INT_EN_TYPE(arg));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- if(NewState == ENABLE)
- {
- if(arg==CANINT_FCE)
- {
- LPC_CANAF->AFMR = 0x01;
- LPC_CANAF->FCANIE = 0x01;
- LPC_CANAF->AFMR = 0x04;
- }
- else
- CANx->IER |= (1 << arg);
- }
- else
- {
- if(arg==CANINT_FCE){
- LPC_CANAF->AFMR = 0x01;
- LPC_CANAF->FCANIE = 0x01;
- LPC_CANAF->AFMR = 0x00;
- }
- else
- CANx->IER &= ~(1 << arg);
- }
-}
-
-/********************************************************************//**
- * @brief Setting Acceptance Filter mode
- * @param[in] CANAFx point to LPC_CANAF_TypeDef object, should be: LPC_CANAF
- * @param[in] AFMode: type of AF mode that you want to set, should be:
- * - CAN_Normal: Normal mode
- * - CAN_AccOff: Acceptance Filter Off Mode
- * - CAN_AccBP: Acceptance Fileter Bypass Mode
- * - CAN_eFCAN: FullCAN Mode Enhancement
- * @return none
- *********************************************************************/
-void CAN_SetAFMode (LPC_CANAF_TypeDef* CANAFx, CAN_AFMODE_Type AFMode)
-{
- CHECK_PARAM(PARAM_CANAFx(CANAFx));
- CHECK_PARAM(PARAM_AFMODE_TYPE(AFMode));
-
- switch(AFMode)
- {
- case CAN_Normal:
- CANAFx->AFMR = 0x00;
- break;
- case CAN_AccOff:
- CANAFx->AFMR = 0x01;
- break;
- case CAN_AccBP:
- CANAFx->AFMR = 0x02;
- break;
- case CAN_eFCAN:
- CANAFx->AFMR = 0x04;
- break;
- }
-}
-
-/********************************************************************//**
- * @brief Enable/Disable CAN Mode
- * @param[in] CANx pointer to LPC_CAN_TypeDef, should be:
- * - LPC_CAN1: CAN1 peripheral
- * - LPC_CAN2: CAN2 peripheral
- * @param[in] mode: type of CAN mode that you want to enable/disable, should be:
- * - CAN_OPERATING_MODE: Normal Operating Mode
- * - CAN_RESET_MODE: Reset Mode
- * - CAN_LISTENONLY_MODE: Listen Only Mode
- * - CAN_SELFTEST_MODE: Self Test Mode
- * - CAN_TXPRIORITY_MODE: Transmit Priority Mode
- * - CAN_SLEEP_MODE: Sleep Mode
- * - CAN_RXPOLARITY_MODE: Receive Polarity Mode
- * - CAN_TEST_MODE: Test Mode
- * @param[in] NewState: New State of this function, should be:
- * - ENABLE
- * - DISABLE
- * @return none
- *********************************************************************/
-void CAN_ModeConfig(LPC_CAN_TypeDef* CANx, CAN_MODE_Type mode, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_CANx(CANx));
- CHECK_PARAM(PARAM_MODE_TYPE(mode));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- switch(mode)
- {
- case CAN_OPERATING_MODE:
- CANx->MOD = 0x00;
- break;
- case CAN_RESET_MODE:
- if(NewState == ENABLE)
- CANx->MOD |=CAN_MOD_RM;
- else
- CANx->MOD &= ~CAN_MOD_RM;
- break;
- case CAN_LISTENONLY_MODE:
- CANx->MOD |=CAN_MOD_RM;//Enter Reset mode
- if(NewState == ENABLE)
- CANx->MOD |=CAN_MOD_LOM;
- else
- CANx->MOD &=~CAN_MOD_LOM;
- CANx->MOD &=~CAN_MOD_RM;//Release Reset mode
- break;
- case CAN_SELFTEST_MODE:
- CANx->MOD |=CAN_MOD_RM;//Enter Reset mode
- if(NewState == ENABLE)
- CANx->MOD |=CAN_MOD_STM;
- else
- CANx->MOD &=~CAN_MOD_STM;
- CANx->MOD &=~CAN_MOD_RM;//Release Reset mode
- break;
- case CAN_TXPRIORITY_MODE:
- if(NewState == ENABLE)
- CANx->MOD |=CAN_MOD_TPM;
- else
- CANx->MOD &=~CAN_MOD_TPM;
- break;
- case CAN_SLEEP_MODE:
- if(NewState == ENABLE)
- CANx->MOD |=CAN_MOD_SM;
- else
- CANx->MOD &=~CAN_MOD_SM;
- break;
- case CAN_RXPOLARITY_MODE:
- if(NewState == ENABLE)
- CANx->MOD |=CAN_MOD_RPM;
- else
- CANx->MOD &=~CAN_MOD_RPM;
- break;
- case CAN_TEST_MODE:
- if(NewState == ENABLE)
- CANx->MOD |=CAN_MOD_TM;
- else
- CANx->MOD &=~CAN_MOD_TM;
- break;
- }
-}
-/*********************************************************************//**
- * @brief Set CAN command request
- * @param[in] CANx point to CAN peripheral selected, should be:
- * - LPC_CAN1: CAN1 peripheral
- * - LPC_CAN2: CAN2 peripheral
- * @param[in] CMRType command request type, should be:
- * - CAN_CMR_TR: Transmission request
- * - CAN_CMR_AT: Abort Transmission request
- * - CAN_CMR_RRB: Release Receive Buffer request
- * - CAN_CMR_CDO: Clear Data Overrun request
- * - CAN_CMR_SRR: Self Reception request
- * - CAN_CMR_STB1: Select Tx Buffer 1 request
- * - CAN_CMR_STB2: Select Tx Buffer 2 request
- * - CAN_CMR_STB3: Select Tx Buffer 3 request
- * @return CANICR (CAN interrupt and Capture register) value
- **********************************************************************/
-void CAN_SetCommand(LPC_CAN_TypeDef* CANx, uint32_t CMRType)
-{
- CHECK_PARAM(PARAM_CANx(CANx));
- CANx->CMR |= CMRType;
-}
-
-/*********************************************************************//**
- * @brief Get CAN interrupt status
- * @param[in] CANx point to CAN peripheral selected, should be:
- * - LPC_CAN1: CAN1 peripheral
- * - LPC_CAN2: CAN2 peripheral
- * @return CANICR (CAN interrupt and Capture register) value
- **********************************************************************/
-uint32_t CAN_IntGetStatus(LPC_CAN_TypeDef* CANx)
-{
- CHECK_PARAM(PARAM_CANx(CANx));
- return CANx->ICR;
-}
-
-/*********************************************************************//**
- * @brief Check if FullCAN interrupt enable or not
- * @param[in] CANAFx point to LPC_CANAF_TypeDef object, should be: LPC_CANAF
- * @return IntStatus, could be:
- * - SET: if FullCAN interrupt is enable
- * - RESET: if FullCAN interrupt is disable
- **********************************************************************/
-IntStatus CAN_FullCANIntGetStatus (LPC_CANAF_TypeDef* CANAFx)
-{
- CHECK_PARAM( PARAM_CANAFx(CANAFx));
- if (CANAFx->FCANIE)
- return SET;
- return RESET;
-}
-
-/*********************************************************************//**
- * @brief Get value of FullCAN interrupt and capture register
- * @param[in] CANAFx point to LPC_CANAF_TypeDef object, should be: LPC_CANAF
- * @param[in] type: FullCAN IC type, should be:
- * - FULLCAN_IC0: FullCAN Interrupt Capture 0
- * - FULLCAN_IC1: FullCAN Interrupt Capture 1
- * @return FCANIC0 or FCANIC1 (FullCAN interrupt and Capture register) value
- **********************************************************************/
-uint32_t CAN_FullCANPendGetStatus(LPC_CANAF_TypeDef* CANAFx, FullCAN_IC_Type type)
-{
- CHECK_PARAM(PARAM_CANAFx(CANAFx));
- CHECK_PARAM( PARAM_FULLCAN_IC(type));
- if (type == FULLCAN_IC0)
- return CANAFx->FCANIC0;
- return CANAFx->FCANIC1;
-}
-/* End of Public Variables ---------------------------------------------------------- */
-/**
- * @}
- */
-
-#endif /* _CAN */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_clkpwr.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_clkpwr.c
deleted file mode 100644
index 4f518e15a8..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_clkpwr.c
+++ /dev/null
@@ -1,350 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_clkpwr.c 2010-06-18
-*//**
-* @file lpc17xx_clkpwr.c
-* @brief Contains all functions support for Clock and Power Control
-* firmware library on LPC17xx
-* @version 3.0
-* @date 18. June. 2010
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2010, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup CLKPWR
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_clkpwr.h"
-
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup CLKPWR_Public_Functions
- * @{
- */
-
-/*********************************************************************//**
- * @brief Set value of each Peripheral Clock Selection
- * @param[in] ClkType Peripheral Clock Selection of each type,
- * should be one of the following:
- * - CLKPWR_PCLKSEL_WDT : WDT
- - CLKPWR_PCLKSEL_TIMER0 : Timer 0
- - CLKPWR_PCLKSEL_TIMER1 : Timer 1
- - CLKPWR_PCLKSEL_UART0 : UART 0
- - CLKPWR_PCLKSEL_UART1 : UART 1
- - CLKPWR_PCLKSEL_PWM1 : PWM 1
- - CLKPWR_PCLKSEL_I2C0 : I2C 0
- - CLKPWR_PCLKSEL_SPI : SPI
- - CLKPWR_PCLKSEL_SSP1 : SSP 1
- - CLKPWR_PCLKSEL_DAC : DAC
- - CLKPWR_PCLKSEL_ADC : ADC
- - CLKPWR_PCLKSEL_CAN1 : CAN 1
- - CLKPWR_PCLKSEL_CAN2 : CAN 2
- - CLKPWR_PCLKSEL_ACF : ACF
- - CLKPWR_PCLKSEL_QEI : QEI
- - CLKPWR_PCLKSEL_PCB : PCB
- - CLKPWR_PCLKSEL_I2C1 : I2C 1
- - CLKPWR_PCLKSEL_SSP0 : SSP 0
- - CLKPWR_PCLKSEL_TIMER2 : Timer 2
- - CLKPWR_PCLKSEL_TIMER3 : Timer 3
- - CLKPWR_PCLKSEL_UART2 : UART 2
- - CLKPWR_PCLKSEL_UART3 : UART 3
- - CLKPWR_PCLKSEL_I2C2 : I2C 2
- - CLKPWR_PCLKSEL_I2S : I2S
- - CLKPWR_PCLKSEL_RIT : RIT
- - CLKPWR_PCLKSEL_SYSCON : SYSCON
- - CLKPWR_PCLKSEL_MC : MC
-
- * @param[in] DivVal Value of divider, should be:
- * - CLKPWR_PCLKSEL_CCLK_DIV_4 : PCLK_peripheral = CCLK/4
- * - CLKPWR_PCLKSEL_CCLK_DIV_1 : PCLK_peripheral = CCLK/1
- * - CLKPWR_PCLKSEL_CCLK_DIV_2 : PCLK_peripheral = CCLK/2
- *
- * @return none
- **********************************************************************/
-void CLKPWR_SetPCLKDiv (uint32_t ClkType, uint32_t DivVal)
-{
- uint32_t bitpos;
-
- bitpos = (ClkType < 32) ? (ClkType) : (ClkType - 32);
-
- /* PCLKSEL0 selected */
- if (ClkType < 32)
- {
- /* Clear two bit at bit position */
- LPC_SC->PCLKSEL0 &= (~(CLKPWR_PCLKSEL_BITMASK(bitpos)));
-
- /* Set two selected bit */
- LPC_SC->PCLKSEL0 |= (CLKPWR_PCLKSEL_SET(bitpos, DivVal));
- }
- /* PCLKSEL1 selected */
- else
- {
- /* Clear two bit at bit position */
- LPC_SC->PCLKSEL1 &= ~(CLKPWR_PCLKSEL_BITMASK(bitpos));
-
- /* Set two selected bit */
- LPC_SC->PCLKSEL1 |= (CLKPWR_PCLKSEL_SET(bitpos, DivVal));
- }
-}
-
-
-/*********************************************************************//**
- * @brief Get current value of each Peripheral Clock Selection
- * @param[in] ClkType Peripheral Clock Selection of each type,
- * should be one of the following:
- * - CLKPWR_PCLKSEL_WDT : WDT
- - CLKPWR_PCLKSEL_TIMER0 : Timer 0
- - CLKPWR_PCLKSEL_TIMER1 : Timer 1
- - CLKPWR_PCLKSEL_UART0 : UART 0
- - CLKPWR_PCLKSEL_UART1 : UART 1
- - CLKPWR_PCLKSEL_PWM1 : PWM 1
- - CLKPWR_PCLKSEL_I2C0 : I2C 0
- - CLKPWR_PCLKSEL_SPI : SPI
- - CLKPWR_PCLKSEL_SSP1 : SSP 1
- - CLKPWR_PCLKSEL_DAC : DAC
- - CLKPWR_PCLKSEL_ADC : ADC
- - CLKPWR_PCLKSEL_CAN1 : CAN 1
- - CLKPWR_PCLKSEL_CAN2 : CAN 2
- - CLKPWR_PCLKSEL_ACF : ACF
- - CLKPWR_PCLKSEL_QEI : QEI
- - CLKPWR_PCLKSEL_PCB : PCB
- - CLKPWR_PCLKSEL_I2C1 : I2C 1
- - CLKPWR_PCLKSEL_SSP0 : SSP 0
- - CLKPWR_PCLKSEL_TIMER2 : Timer 2
- - CLKPWR_PCLKSEL_TIMER3 : Timer 3
- - CLKPWR_PCLKSEL_UART2 : UART 2
- - CLKPWR_PCLKSEL_UART3 : UART 3
- - CLKPWR_PCLKSEL_I2C2 : I2C 2
- - CLKPWR_PCLKSEL_I2S : I2S
- - CLKPWR_PCLKSEL_RIT : RIT
- - CLKPWR_PCLKSEL_SYSCON : SYSCON
- - CLKPWR_PCLKSEL_MC : MC
-
- * @return Value of Selected Peripheral Clock Selection
- **********************************************************************/
-uint32_t CLKPWR_GetPCLKSEL (uint32_t ClkType)
-{
- uint32_t bitpos, retval;
-
- if (ClkType < 32)
- {
- bitpos = ClkType;
- retval = LPC_SC->PCLKSEL0;
- }
- else
- {
- bitpos = ClkType - 32;
- retval = LPC_SC->PCLKSEL1;
- }
-
- retval = CLKPWR_PCLKSEL_GET(bitpos, retval);
- return retval;
-}
-
-
-
-/*********************************************************************//**
- * @brief Get current value of each Peripheral Clock
- * @param[in] ClkType Peripheral Clock Selection of each type,
- * should be one of the following:
- * - CLKPWR_PCLKSEL_WDT : WDT
- - CLKPWR_PCLKSEL_TIMER0 : Timer 0
- - CLKPWR_PCLKSEL_TIMER1 : Timer 1
- - CLKPWR_PCLKSEL_UART0 : UART 0
- - CLKPWR_PCLKSEL_UART1 : UART 1
- - CLKPWR_PCLKSEL_PWM1 : PWM 1
- - CLKPWR_PCLKSEL_I2C0 : I2C 0
- - CLKPWR_PCLKSEL_SPI : SPI
- - CLKPWR_PCLKSEL_SSP1 : SSP 1
- - CLKPWR_PCLKSEL_DAC : DAC
- - CLKPWR_PCLKSEL_ADC : ADC
- - CLKPWR_PCLKSEL_CAN1 : CAN 1
- - CLKPWR_PCLKSEL_CAN2 : CAN 2
- - CLKPWR_PCLKSEL_ACF : ACF
- - CLKPWR_PCLKSEL_QEI : QEI
- - CLKPWR_PCLKSEL_PCB : PCB
- - CLKPWR_PCLKSEL_I2C1 : I2C 1
- - CLKPWR_PCLKSEL_SSP0 : SSP 0
- - CLKPWR_PCLKSEL_TIMER2 : Timer 2
- - CLKPWR_PCLKSEL_TIMER3 : Timer 3
- - CLKPWR_PCLKSEL_UART2 : UART 2
- - CLKPWR_PCLKSEL_UART3 : UART 3
- - CLKPWR_PCLKSEL_I2C2 : I2C 2
- - CLKPWR_PCLKSEL_I2S : I2S
- - CLKPWR_PCLKSEL_RIT : RIT
- - CLKPWR_PCLKSEL_SYSCON : SYSCON
- - CLKPWR_PCLKSEL_MC : MC
-
- * @return Value of Selected Peripheral Clock
- **********************************************************************/
-uint32_t CLKPWR_GetPCLK (uint32_t ClkType)
-{
- uint32_t retval, div;
-
- retval = SystemCoreClock;
- div = CLKPWR_GetPCLKSEL(ClkType);
-
- switch (div)
- {
- case 0:
- div = 4;
- break;
-
- case 1:
- div = 1;
- break;
-
- case 2:
- div = 2;
- break;
-
- case 3:
- div = 8;
- break;
- }
- retval /= div;
-
- return retval;
-}
-
-
-
-/*********************************************************************//**
- * @brief Configure power supply for each peripheral according to NewState
- * @param[in] PPType Type of peripheral used to enable power,
- * should be one of the following:
- * - CLKPWR_PCONP_PCTIM0 : Timer 0
- - CLKPWR_PCONP_PCTIM1 : Timer 1
- - CLKPWR_PCONP_PCUART0 : UART 0
- - CLKPWR_PCONP_PCUART1 : UART 1
- - CLKPWR_PCONP_PCPWM1 : PWM 1
- - CLKPWR_PCONP_PCI2C0 : I2C 0
- - CLKPWR_PCONP_PCSPI : SPI
- - CLKPWR_PCONP_PCRTC : RTC
- - CLKPWR_PCONP_PCSSP1 : SSP 1
- - CLKPWR_PCONP_PCAD : ADC
- - CLKPWR_PCONP_PCAN1 : CAN 1
- - CLKPWR_PCONP_PCAN2 : CAN 2
- - CLKPWR_PCONP_PCGPIO : GPIO
- - CLKPWR_PCONP_PCRIT : RIT
- - CLKPWR_PCONP_PCMC : MC
- - CLKPWR_PCONP_PCQEI : QEI
- - CLKPWR_PCONP_PCI2C1 : I2C 1
- - CLKPWR_PCONP_PCSSP0 : SSP 0
- - CLKPWR_PCONP_PCTIM2 : Timer 2
- - CLKPWR_PCONP_PCTIM3 : Timer 3
- - CLKPWR_PCONP_PCUART2 : UART 2
- - CLKPWR_PCONP_PCUART3 : UART 3
- - CLKPWR_PCONP_PCI2C2 : I2C 2
- - CLKPWR_PCONP_PCI2S : I2S
- - CLKPWR_PCONP_PCGPDMA : GPDMA
- - CLKPWR_PCONP_PCENET : Ethernet
- - CLKPWR_PCONP_PCUSB : USB
- *
- * @param[in] NewState New state of Peripheral Power, should be:
- * - ENABLE : Enable power for this peripheral
- * - DISABLE : Disable power for this peripheral
- *
- * @return none
- **********************************************************************/
-void CLKPWR_ConfigPPWR (uint32_t PPType, FunctionalState NewState)
-{
- if (NewState == ENABLE)
- {
- LPC_SC->PCONP |= PPType & CLKPWR_PCONP_BITMASK;
- }
- else if (NewState == DISABLE)
- {
- LPC_SC->PCONP &= (~PPType) & CLKPWR_PCONP_BITMASK;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Enter Sleep mode with co-operated instruction by the Cortex-M3.
- * @param[in] None
- * @return None
- **********************************************************************/
-void CLKPWR_Sleep(void)
-{
- LPC_SC->PCON = 0x00;
- /* Sleep Mode*/
- __WFI();
-}
-
-
-/*********************************************************************//**
- * @brief Enter Deep Sleep mode with co-operated instruction by the Cortex-M3.
- * @param[in] None
- * @return None
- **********************************************************************/
-void CLKPWR_DeepSleep(void)
-{
- /* Deep-Sleep Mode, set SLEEPDEEP bit */
- SCB->SCR = 0x4;
- LPC_SC->PCON = 0x00;
- /* Deep Sleep Mode*/
- __WFI();
-}
-
-
-/*********************************************************************//**
- * @brief Enter Power Down mode with co-operated instruction by the Cortex-M3.
- * @param[in] None
- * @return None
- **********************************************************************/
-void CLKPWR_PowerDown(void)
-{
- /* Deep-Sleep Mode, set SLEEPDEEP bit */
- SCB->SCR = 0x4;
- LPC_SC->PCON = 0x01;
- /* Power Down Mode*/
- __WFI();
-}
-
-
-/*********************************************************************//**
- * @brief Enter Deep Power Down mode with co-operated instruction by the Cortex-M3.
- * @param[in] None
- * @return None
- **********************************************************************/
-void CLKPWR_DeepPowerDown(void)
-{
- /* Deep-Sleep Mode, set SLEEPDEEP bit */
- SCB->SCR = 0x4;
- LPC_SC->PCON = 0x03;
- /* Deep Power Down Mode*/
- __WFI();
-}
-
-/**
- * @}
- */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_dac.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_dac.c
deleted file mode 100644
index f13bf3b6a9..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_dac.c
+++ /dev/null
@@ -1,151 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_dac.c 2010-05-21
-*//**
-* @file lpc17xx_dac.c
-* @brief Contains all functions support for DAC firmware library on LPC17xx
-* @version 2.0
-* @date 21. May. 2010
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2010, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup DAC
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_dac.h"
-#include "lpc17xx_clkpwr.h"
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-
-#ifdef _DAC
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup DAC_Public_Functions
- * @{
- */
-
-/*********************************************************************//**
- * @brief Initial ADC configuration
- * - Maximum current is 700 uA
- * - Value to AOUT is 0
- * @param[in] DACx pointer to LPC_DAC_TypeDef, should be: LPC_DAC
- * @return None
- ***********************************************************************/
-void DAC_Init(LPC_DAC_TypeDef *DACx)
-{
- CHECK_PARAM(PARAM_DACx(DACx));
- /* Set default clock divider for DAC */
- // CLKPWR_SetPCLKDiv (CLKPWR_PCLKSEL_DAC, CLKPWR_PCLKSEL_CCLK_DIV_4);
- //Set maximum current output
- DAC_SetBias(LPC_DAC,DAC_MAX_CURRENT_700uA);
-}
-
-/*********************************************************************//**
- * @brief Update value to DAC
- * @param[in] DACx pointer to LPC_DAC_TypeDef, should be: LPC_DAC
- * @param[in] dac_value : value 10 bit to be converted to output
- * @return None
- ***********************************************************************/
-void DAC_UpdateValue (LPC_DAC_TypeDef *DACx,uint32_t dac_value)
-{
- uint32_t tmp;
- CHECK_PARAM(PARAM_DACx(DACx));
- tmp = DACx->DACR & DAC_BIAS_EN;
- tmp |= DAC_VALUE(dac_value);
- // Update value
- DACx->DACR = tmp;
-}
-
-/*********************************************************************//**
- * @brief Set Maximum current for DAC
- * @param[in] DACx pointer to LPC_DAC_TypeDef, should be: LPC_DAC
- * @param[in] bias : 0 is 700 uA
- * 1 350 uA
- * @return None
- ***********************************************************************/
-void DAC_SetBias (LPC_DAC_TypeDef *DACx,uint32_t bias)
-{
- CHECK_PARAM(PARAM_DAC_CURRENT_OPT(bias));
- DACx->DACR &=~DAC_BIAS_EN;
- if (bias == DAC_MAX_CURRENT_350uA)
- {
- DACx->DACR |= DAC_BIAS_EN;
- }
-}
-
-/*********************************************************************//**
- * @brief To enable the DMA operation and control DMA timer
- * @param[in] DACx pointer to LPC_DAC_TypeDef, should be: LPC_DAC
- * @param[in] DAC_ConverterConfigStruct pointer to DAC_CONVERTER_CFG_Type
- * - DBLBUF_ENA : enable/disable DACR double buffering feature
- * - CNT_ENA : enable/disable timer out counter
- * - DMA_ENA : enable/disable DMA access
- * @return None
- ***********************************************************************/
-void DAC_ConfigDAConverterControl (LPC_DAC_TypeDef *DACx,DAC_CONVERTER_CFG_Type *DAC_ConverterConfigStruct)
-{
- CHECK_PARAM(PARAM_DACx(DACx));
- DACx->DACCTRL &= ~DAC_DACCTRL_MASK;
- if (DAC_ConverterConfigStruct->DBLBUF_ENA)
- DACx->DACCTRL |= DAC_DBLBUF_ENA;
- if (DAC_ConverterConfigStruct->CNT_ENA)
- DACx->DACCTRL |= DAC_CNT_ENA;
- if (DAC_ConverterConfigStruct->DMA_ENA)
- DACx->DACCTRL |= DAC_DMA_ENA;
-}
-
-/*********************************************************************//**
- * @brief Set reload value for interrupt/DMA counter
- * @param[in] DACx pointer to LPC_DAC_TypeDef, should be: LPC_DAC
- * @param[in] time_out time out to reload for interrupt/DMA counter
- * @return None
- ***********************************************************************/
-void DAC_SetDMATimeOut(LPC_DAC_TypeDef *DACx, uint32_t time_out)
-{
- CHECK_PARAM(PARAM_DACx(DACx));
- DACx->DACCNTVAL = DAC_CCNT_VALUE(time_out);
-}
-
-/**
- * @}
- */
-
-#endif /* _DAC */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_emac.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_emac.c
deleted file mode 100644
index f0e4e4627e..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_emac.c
+++ /dev/null
@@ -1,963 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_emac.c 2010-05-21
-*//**
-* @file lpc17xx_emac.c
-* @brief Contains all functions support for Ethernet MAC firmware
-* library on LPC17xx
-* @version 2.0
-* @date 21. May. 2010
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2010, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup EMAC
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_emac.h"
-#include "lpc17xx_clkpwr.h"
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-
-#ifdef _EMAC
-
-/* Private Variables ---------------------------------------------------------- */
-/** @defgroup EMAC_Private_Variables EMAC Private Variables
- * @{
- */
-
-/* MII Mgmt Configuration register - Clock divider setting */
-const uint8_t EMAC_clkdiv[] = { 4, 6, 8, 10, 14, 20, 28, 36, 40, 44, 48, 52, 56, 60, 64};
-
-/* EMAC local DMA Descriptors */
-
-/** Rx Descriptor data array */
-static RX_Desc Rx_Desc[EMAC_NUM_RX_FRAG];
-
-/** Rx Status data array - Must be 8-Byte aligned */
-#if defined ( __CC_ARM )
-static __align(8) RX_Stat Rx_Stat[EMAC_NUM_RX_FRAG];
-#elif defined ( __ICCARM__ )
-#pragma data_alignment=8
-static RX_Stat Rx_Stat[EMAC_NUM_RX_FRAG];
-#elif defined ( __GNUC__ )
-static __attribute__ ((aligned (8))) RX_Stat Rx_Stat[EMAC_NUM_RX_FRAG];
-#endif
-
-/** Tx Descriptor data array */
-static TX_Desc Tx_Desc[EMAC_NUM_TX_FRAG];
-/** Tx Status data array */
-static TX_Stat Tx_Stat[EMAC_NUM_TX_FRAG];
-
-/* EMAC local DMA buffers */
-/** Rx buffer data */
-static uint32_t rx_buf[EMAC_NUM_RX_FRAG][EMAC_ETH_MAX_FLEN>>2];
-/** Tx buffer data */
-static uint32_t tx_buf[EMAC_NUM_TX_FRAG][EMAC_ETH_MAX_FLEN>>2];
-
-/**
- * @}
- */
-
-/* Private Functions ---------------------------------------------------------- */
-static void rx_descr_init (void);
-static void tx_descr_init (void);
-static int32_t write_PHY (uint32_t PhyReg, uint16_t Value);
-static int32_t read_PHY (uint32_t PhyReg);
-
-static void setEmacAddr(uint8_t abStationAddr[]);
-static int32_t emac_CRCCalc(uint8_t frame_no_fcs[], int32_t frame_len);
-
-
-/*--------------------------- rx_descr_init ---------------------------------*/
-/*********************************************************************//**
- * @brief Initializes RX Descriptor
- * @param[in] None
- * @return None
- ***********************************************************************/
-static void rx_descr_init (void)
-{
- /* Initialize Receive Descriptor and Status array. */
- uint32_t i;
-
- for (i = 0; i < EMAC_NUM_RX_FRAG; i++) {
- Rx_Desc[i].Packet = (uint32_t)&rx_buf[i];
- Rx_Desc[i].Ctrl = EMAC_RCTRL_INT | (EMAC_ETH_MAX_FLEN - 1);
- Rx_Stat[i].Info = 0;
- Rx_Stat[i].HashCRC = 0;
- }
-
- /* Set EMAC Receive Descriptor Registers. */
- LPC_EMAC->RxDescriptor = (uint32_t)&Rx_Desc[0];
- LPC_EMAC->RxStatus = (uint32_t)&Rx_Stat[0];
- LPC_EMAC->RxDescriptorNumber = EMAC_NUM_RX_FRAG - 1;
-
- /* Rx Descriptors Point to 0 */
- LPC_EMAC->RxConsumeIndex = 0;
-}
-
-
-/*--------------------------- tx_descr_init ---- ----------------------------*/
-/*********************************************************************//**
- * @brief Initializes TX Descriptor
- * @param[in] None
- * @return None
- ***********************************************************************/
-static void tx_descr_init (void) {
- /* Initialize Transmit Descriptor and Status array. */
- uint32_t i;
-
- for (i = 0; i < EMAC_NUM_TX_FRAG; i++) {
- Tx_Desc[i].Packet = (uint32_t)&tx_buf[i];
- Tx_Desc[i].Ctrl = 0;
- Tx_Stat[i].Info = 0;
- }
-
- /* Set EMAC Transmit Descriptor Registers. */
- LPC_EMAC->TxDescriptor = (uint32_t)&Tx_Desc[0];
- LPC_EMAC->TxStatus = (uint32_t)&Tx_Stat[0];
- LPC_EMAC->TxDescriptorNumber = EMAC_NUM_TX_FRAG - 1;
-
- /* Tx Descriptors Point to 0 */
- LPC_EMAC->TxProduceIndex = 0;
-}
-
-
-/*--------------------------- write_PHY -------------------------------------*/
-/*********************************************************************//**
- * @brief Write value to PHY device
- * @param[in] PhyReg: PHY Register address
- * @param[in] Value: Value to write
- * @return 0 - if success
- * 1 - if fail
- ***********************************************************************/
-static int32_t write_PHY (uint32_t PhyReg, uint16_t Value)
-{
- /* Write a data 'Value' to PHY register 'PhyReg'. */
- uint32_t tout;
-
- LPC_EMAC->MADR = EMAC_DEF_ADR | PhyReg;
- LPC_EMAC->MWTD = Value;
-
- /* Wait until operation completed */
- tout = 0;
- for (tout = 0; tout < EMAC_MII_WR_TOUT; tout++) {
- if ((LPC_EMAC->MIND & EMAC_MIND_BUSY) == 0) {
- return (0);
- }
- }
- // Time out!
- return (-1);
-}
-
-
-/*--------------------------- read_PHY --------------------------------------*/
-/*********************************************************************//**
- * @brief Read value from PHY device
- * @param[in] PhyReg: PHY Register address
- * @return 0 - if success
- * 1 - if fail
- ***********************************************************************/
-static int32_t read_PHY (uint32_t PhyReg)
-{
- /* Read a PHY register 'PhyReg'. */
- uint32_t tout;
-
- LPC_EMAC->MADR = EMAC_DEF_ADR | PhyReg;
- LPC_EMAC->MCMD = EMAC_MCMD_READ;
-
- /* Wait until operation completed */
- tout = 0;
- for (tout = 0; tout < EMAC_MII_RD_TOUT; tout++) {
- if ((LPC_EMAC->MIND & EMAC_MIND_BUSY) == 0) {
- LPC_EMAC->MCMD = 0;
- return (LPC_EMAC->MRDD);
- }
- }
- // Time out!
- return (-1);
-}
-
-/*********************************************************************//**
- * @brief Set Station MAC address for EMAC module
- * @param[in] abStationAddr Pointer to Station address that contains 6-bytes
- * of MAC address (should be in order from MAC Address 1 to MAC Address 6)
- * @return None
- **********************************************************************/
-static void setEmacAddr(uint8_t abStationAddr[])
-{
- /* Set the Ethernet MAC Address registers */
- LPC_EMAC->SA0 = ((uint32_t)abStationAddr[5] << 8) | (uint32_t)abStationAddr[4];
- LPC_EMAC->SA1 = ((uint32_t)abStationAddr[3] << 8) | (uint32_t)abStationAddr[2];
- LPC_EMAC->SA2 = ((uint32_t)abStationAddr[1] << 8) | (uint32_t)abStationAddr[0];
-}
-
-
-/*********************************************************************//**
- * @brief Calculates CRC code for number of bytes in the frame
- * @param[in] frame_no_fcs Pointer to the first byte of the frame
- * @param[in] frame_len length of the frame without the FCS
- * @return the CRC as a 32 bit integer
- **********************************************************************/
-static int32_t emac_CRCCalc(uint8_t frame_no_fcs[], int32_t frame_len)
-{
- int i; // iterator
- int j; // another iterator
- char byte; // current byte
- int crc; // CRC result
- int q0, q1, q2, q3; // temporary variables
- crc = 0xFFFFFFFF;
- for (i = 0; i < frame_len; i++) {
- byte = *frame_no_fcs++;
- for (j = 0; j < 2; j++) {
- if (((crc >> 28) ^ (byte >> 3)) & 0x00000001) {
- q3 = 0x04C11DB7;
- } else {
- q3 = 0x00000000;
- }
- if (((crc >> 29) ^ (byte >> 2)) & 0x00000001) {
- q2 = 0x09823B6E;
- } else {
- q2 = 0x00000000;
- }
- if (((crc >> 30) ^ (byte >> 1)) & 0x00000001) {
- q1 = 0x130476DC;
- } else {
- q1 = 0x00000000;
- }
- if (((crc >> 31) ^ (byte >> 0)) & 0x00000001) {
- q0 = 0x2608EDB8;
- } else {
- q0 = 0x00000000;
- }
- crc = (crc << 4) ^ q3 ^ q2 ^ q1 ^ q0;
- byte >>= 4;
- }
- }
- return crc;
-}
-/* End of Private Functions --------------------------------------------------- */
-
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup EMAC_Public_Functions
- * @{
- */
-
-
-/*********************************************************************//**
- * @brief Initializes the EMAC peripheral according to the specified
-* parameters in the EMAC_ConfigStruct.
- * @param[in] EMAC_ConfigStruct Pointer to a EMAC_CFG_Type structure
-* that contains the configuration information for the
-* specified EMAC peripheral.
- * @return None
- *
- * Note: This function will initialize EMAC module according to procedure below:
- * - Remove the soft reset condition from the MAC
- * - Configure the PHY via the MIIM interface of the MAC
- * - Select RMII mode
- * - Configure the transmit and receive DMA engines, including the descriptor arrays
- * - Configure the host registers (MAC1,MAC2 etc.) in the MAC
- * - Enable the receive and transmit data paths
- * In default state after initializing, only Rx Done and Tx Done interrupt are enabled,
- * all remain interrupts are disabled
- * (Ref. from LPC17xx UM)
- **********************************************************************/
-Status EMAC_Init(EMAC_CFG_Type *EMAC_ConfigStruct)
-{
- /* Initialize the EMAC Ethernet controller. */
- int32_t regv,tout, tmp;
-
- /* Set up clock and power for Ethernet module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCENET, ENABLE);
-
- /* Reset all EMAC internal modules */
- LPC_EMAC->MAC1 = EMAC_MAC1_RES_TX | EMAC_MAC1_RES_MCS_TX | EMAC_MAC1_RES_RX |
- EMAC_MAC1_RES_MCS_RX | EMAC_MAC1_SIM_RES | EMAC_MAC1_SOFT_RES;
-
- LPC_EMAC->Command = EMAC_CR_REG_RES | EMAC_CR_TX_RES | EMAC_CR_RX_RES | EMAC_CR_PASS_RUNT_FRM;
-
- /* A short delay after reset. */
- for (tout = 100; tout; tout--);
-
- /* Initialize MAC control registers. */
- LPC_EMAC->MAC1 = EMAC_MAC1_PASS_ALL;
- LPC_EMAC->MAC2 = EMAC_MAC2_CRC_EN | EMAC_MAC2_PAD_EN;
- LPC_EMAC->MAXF = EMAC_ETH_MAX_FLEN;
- /*
- * Find the clock that close to desired target clock
- */
- tmp = SystemCoreClock / EMAC_MCFG_MII_MAXCLK;
- for (tout = 0; tout < sizeof (EMAC_clkdiv); tout++){
- if (EMAC_clkdiv[tout] >= tmp) break;
- }
- tout++;
- // Write to MAC configuration register and reset
- LPC_EMAC->MCFG = EMAC_MCFG_CLK_SEL(tout) | EMAC_MCFG_RES_MII;
- // release reset
- LPC_EMAC->MCFG &= ~(EMAC_MCFG_RES_MII);
- LPC_EMAC->CLRT = EMAC_CLRT_DEF;
- LPC_EMAC->IPGR = EMAC_IPGR_P2_DEF;
-
- /* Enable Reduced MII interface. */
- LPC_EMAC->Command = EMAC_CR_RMII | EMAC_CR_PASS_RUNT_FRM;
-
- /* Reset Reduced MII Logic. */
-// LPC_EMAC->SUPP = EMAC_SUPP_RES_RMII;
-
- for (tout = 100; tout; tout--);
- LPC_EMAC->SUPP = 0;
-
- /* Put the DP83848C in reset mode */
- write_PHY (EMAC_PHY_REG_BMCR, EMAC_PHY_BMCR_RESET);
-
- /* Wait for hardware reset to end. */
- for (tout = EMAC_PHY_RESP_TOUT; tout>=0; tout--) {
- regv = read_PHY (EMAC_PHY_REG_BMCR);
- if (!(regv & (EMAC_PHY_BMCR_RESET | EMAC_PHY_BMCR_POWERDOWN))) {
- /* Reset complete, device not Power Down. */
- break;
- }
- if (tout == 0){
- // Time out, return ERROR
- return (ERROR);
- }
- }
-
- // Set PHY mode
- if (EMAC_SetPHYMode(EMAC_ConfigStruct->Mode) < 0){
- return (ERROR);
- }
-
- // Set EMAC address
- setEmacAddr(EMAC_ConfigStruct->pbEMAC_Addr);
-
- /* Initialize Tx and Rx DMA Descriptors */
- rx_descr_init ();
- tx_descr_init ();
-
- // Set Receive Filter register: enable broadcast and multicast
- LPC_EMAC->RxFilterCtrl = EMAC_RFC_MCAST_EN | EMAC_RFC_BCAST_EN | EMAC_RFC_PERFECT_EN;
-
- /* Enable Rx Done and Tx Done interrupt for EMAC */
- LPC_EMAC->IntEnable = EMAC_INT_RX_DONE | EMAC_INT_TX_DONE;
-
- /* Reset all interrupts */
- LPC_EMAC->IntClear = 0xFFFF;
-
- /* Enable receive and transmit mode of MAC Ethernet core */
- LPC_EMAC->Command |= (EMAC_CR_RX_EN | EMAC_CR_TX_EN);
- LPC_EMAC->MAC1 |= EMAC_MAC1_REC_EN;
-
- return SUCCESS;
-}
-
-
-/*********************************************************************//**
- * @brief De-initializes the EMAC peripheral registers to their
-* default reset values.
- * @param[in] None
- * @return None
- **********************************************************************/
-void EMAC_DeInit(void)
-{
- // Disable all interrupt
- LPC_EMAC->IntEnable = 0x00;
- // Clear all pending interrupt
- LPC_EMAC->IntClear = (0xFF) | (EMAC_INT_SOFT_INT | EMAC_INT_WAKEUP);
-
- /* TurnOff clock and power for Ethernet module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCENET, DISABLE);
-}
-
-
-/*********************************************************************//**
- * @brief Check specified PHY status in EMAC peripheral
- * @param[in] ulPHYState Specified PHY Status Type, should be:
- * - EMAC_PHY_STAT_LINK: Link Status
- * - EMAC_PHY_STAT_SPEED: Speed Status
- * - EMAC_PHY_STAT_DUP: Duplex Status
- * @return Status of specified PHY status (0 or 1).
- * (-1) if error.
- *
- * Note:
- * For EMAC_PHY_STAT_LINK, return value:
- * - 0: Link Down
- * - 1: Link Up
- * For EMAC_PHY_STAT_SPEED, return value:
- * - 0: 10Mbps
- * - 1: 100Mbps
- * For EMAC_PHY_STAT_DUP, return value:
- * - 0: Half-Duplex
- * - 1: Full-Duplex
- **********************************************************************/
-int32_t EMAC_CheckPHYStatus(uint32_t ulPHYState)
-{
- int32_t regv, tmp;
-#ifdef MCB_LPC_1768
- regv = read_PHY (EMAC_PHY_REG_STS);
- switch(ulPHYState){
- case EMAC_PHY_STAT_LINK:
- tmp = (regv & EMAC_PHY_SR_LINK) ? 1 : 0;
- break;
- case EMAC_PHY_STAT_SPEED:
- tmp = (regv & EMAC_PHY_SR_SPEED) ? 0 : 1;
- break;
- case EMAC_PHY_STAT_DUP:
- tmp = (regv & EMAC_PHY_SR_FULL_DUP) ? 1 : 0;
- break;
-#elif defined(IAR_LPC_1768)
- /* Use IAR_LPC_1768 board:
- * FSZ8721BL doesn't have Status Register
- * so we read Basic Mode Status Register (0x01h) instead
- */
- regv = read_PHY (EMAC_PHY_REG_BMSR);
- switch(ulPHYState){
- case EMAC_PHY_STAT_LINK:
- tmp = (regv & EMAC_PHY_BMSR_LINK_STATUS) ? 1 : 0;
- break;
- case EMAC_PHY_STAT_SPEED:
- tmp = (regv & EMAC_PHY_SR_100_SPEED) ? 1 : 0;
- break;
- case EMAC_PHY_STAT_DUP:
- tmp = (regv & EMAC_PHY_SR_FULL_DUP) ? 1 : 0;
- break;
-#endif
- default:
- tmp = -1;
- break;
- }
- return (tmp);
-}
-
-
-/*********************************************************************//**
- * @brief Set specified PHY mode in EMAC peripheral
- * @param[in] ulPHYMode Specified PHY mode, should be:
- * - EMAC_MODE_AUTO
- * - EMAC_MODE_10M_FULL
- * - EMAC_MODE_10M_HALF
- * - EMAC_MODE_100M_FULL
- * - EMAC_MODE_100M_HALF
- * @return Return (0) if no error, otherwise return (-1)
- **********************************************************************/
-int32_t EMAC_SetPHYMode(uint32_t ulPHYMode)
-{
- int32_t id1, id2, tout;
-
- /* Check if this is a DP83848C PHY. */
- id1 = read_PHY (EMAC_PHY_REG_IDR1);
- id2 = read_PHY (EMAC_PHY_REG_IDR2);
-
-#ifdef MCB_LPC_1768
- if (((id1 << 16) | (id2 & 0xFFF0)) == EMAC_DP83848C_ID) {
- switch(ulPHYMode){
- case EMAC_MODE_AUTO:
- write_PHY (EMAC_PHY_REG_BMCR, EMAC_PHY_AUTO_NEG);
-#elif defined(IAR_LPC_1768) /* Use IAR LPC1768 KickStart board */
- if (((id1 << 16) | id2) == EMAC_KSZ8721BL_ID) {
- /* Configure the PHY device */
- switch(ulPHYMode){
- case EMAC_MODE_AUTO:
- /* Use auto-negotiation about the link speed. */
- write_PHY (EMAC_PHY_REG_BMCR, EMAC_PHY_AUTO_NEG);
-// write_PHY (EMAC_PHY_REG_BMCR, EMAC_PHY_BMCR_AN);
-#endif
- /* Wait to complete Auto_Negotiation */
- for (tout = EMAC_PHY_RESP_TOUT; tout>=0; tout--) {
-
- }
- break;
- case EMAC_MODE_10M_FULL:
- /* Connect at 10MBit full-duplex */
- write_PHY (EMAC_PHY_REG_BMCR, EMAC_PHY_FULLD_10M);
- break;
- case EMAC_MODE_10M_HALF:
- /* Connect at 10MBit half-duplex */
- write_PHY (EMAC_PHY_REG_BMCR, EMAC_PHY_HALFD_10M);
- break;
- case EMAC_MODE_100M_FULL:
- /* Connect at 100MBit full-duplex */
- write_PHY (EMAC_PHY_REG_BMCR, EMAC_PHY_FULLD_100M);
- break;
- case EMAC_MODE_100M_HALF:
- /* Connect at 100MBit half-duplex */
- write_PHY (EMAC_PHY_REG_BMCR, EMAC_PHY_HALFD_100M);
- break;
- default:
- // un-supported
- return (-1);
- }
- }
- // It's not correct module ID
- else {
- return (-1);
- }
-
- // Update EMAC configuration with current PHY status
- if (EMAC_UpdatePHYStatus() < 0){
- return (-1);
- }
-
- // Complete
- return (0);
-}
-
-
-/*********************************************************************//**
- * @brief Auto-Configures value for the EMAC configuration register to
- * match with current PHY mode
- * @param[in] None
- * @return Return (0) if no error, otherwise return (-1)
- *
- * Note: The EMAC configuration will be auto-configured:
- * - Speed mode.
- * - Half/Full duplex mode
- **********************************************************************/
-int32_t EMAC_UpdatePHYStatus(void)
-{
- int32_t regv, tout;
-
- /* Check the link status. */
-#ifdef MCB_LPC_1768
- for (tout = EMAC_PHY_RESP_TOUT; tout>=0; tout--) {
- regv = read_PHY (EMAC_PHY_REG_STS);
- if (regv & EMAC_PHY_SR_LINK) {
- /* Link is on. */
- break;
- }
- if (tout == 0){
- // time out
- return (-1);
- }
- }
- /* Configure Full/Half Duplex mode. */
- if (regv & EMAC_PHY_SR_DUP) {
- /* Full duplex is enabled. */
- LPC_EMAC->MAC2 |= EMAC_MAC2_FULL_DUP;
- LPC_EMAC->Command |= EMAC_CR_FULL_DUP;
- LPC_EMAC->IPGT = EMAC_IPGT_FULL_DUP;
- } else {
- /* Half duplex mode. */
- LPC_EMAC->IPGT = EMAC_IPGT_HALF_DUP;
- }
- if (regv & EMAC_PHY_SR_SPEED) {
- /* 10MBit mode. */
- LPC_EMAC->SUPP = 0;
- } else {
- /* 100MBit mode. */
- LPC_EMAC->SUPP = EMAC_SUPP_SPEED;
- }
-#elif defined(IAR_LPC_1768)
- for (tout = EMAC_PHY_RESP_TOUT; tout>=0; tout--) {
- regv = read_PHY (EMAC_PHY_REG_BMSR);
- if (regv & EMAC_PHY_BMSR_LINK_STATUS) {
- /* Link is on. */
- break;
- }
- if (tout == 0){
- // time out
- return (-1);
- }
- }
-
- /* Configure Full/Half Duplex mode. */
- if (regv & EMAC_PHY_SR_FULL_DUP) {
- /* Full duplex is enabled. */
- LPC_EMAC->MAC2 |= EMAC_MAC2_FULL_DUP;
- LPC_EMAC->Command |= EMAC_CR_FULL_DUP;
- LPC_EMAC->IPGT = EMAC_IPGT_FULL_DUP;
- } else {
- /* Half duplex mode. */
- LPC_EMAC->IPGT = EMAC_IPGT_HALF_DUP;
- }
-
- /* Configure 100MBit/10MBit mode. */
- if (!(regv & EMAC_PHY_SR_100_SPEED)) {
- /* 10MBit mode. */
- LPC_EMAC->SUPP = 0;
- } else {
- /* 100MBit mode. */
- LPC_EMAC->SUPP = EMAC_SUPP_SPEED;
- }
-#endif
- // Complete
- return (0);
-}
-
-
-/*********************************************************************//**
- * @brief Enable/Disable hash filter functionality for specified destination
- * MAC address in EMAC module
- * @param[in] dstMAC_addr Pointer to the first MAC destination address, should
- * be 6-bytes length, in order LSB to the MSB
- * @param[in] NewState New State of this command, should be:
- * - ENABLE.
- * - DISABLE.
- * @return None
- *
- * Note:
- * The standard Ethernet cyclic redundancy check (CRC) function is calculated from
- * the 6 byte destination address in the Ethernet frame (this CRC is calculated
- * anyway as part of calculating the CRC of the whole frame), then bits [28:23] out of
- * the 32 bits CRC result are taken to form the hash. The 6 bit hash is used to access
- * the hash table: it is used as an index in the 64 bit HashFilter register that has been
- * programmed with accept values. If the selected accept value is 1, the frame is
- * accepted.
- **********************************************************************/
-void EMAC_SetHashFilter(uint8_t dstMAC_addr[], FunctionalState NewState)
-{
- uint32_t *pReg;
- uint32_t tmp;
- int32_t crc;
-
- // Calculate the CRC from the destination MAC address
- crc = emac_CRCCalc(dstMAC_addr, 6);
- // Extract the value from CRC to get index value for hash filter table
- crc = (crc >> 23) & 0x3F;
-
- pReg = (crc > 31) ? ((uint32_t *)&LPC_EMAC->HashFilterH) \
- : ((uint32_t *)&LPC_EMAC->HashFilterL);
- tmp = (crc > 31) ? (crc - 32) : crc;
- if (NewState == ENABLE) {
- (*pReg) |= (1UL << tmp);
- } else {
- (*pReg) &= ~(1UL << tmp);
- }
- // Enable Rx Filter
- LPC_EMAC->Command &= ~EMAC_CR_PASS_RX_FILT;
-}
-
-/*********************************************************************//**
- * @brief Enable/Disable Filter mode for each specified type EMAC peripheral
- * @param[in] ulFilterMode Filter mode, should be:
- * - EMAC_RFC_UCAST_EN: all frames of unicast types
- * will be accepted
- * - EMAC_RFC_BCAST_EN: broadcast frame will be
- * accepted
- * - EMAC_RFC_MCAST_EN: all frames of multicast
- * types will be accepted
- * - EMAC_RFC_UCAST_HASH_EN: The imperfect hash
- * filter will be applied to unicast addresses
- * - EMAC_RFC_MCAST_HASH_EN: The imperfect hash
- * filter will be applied to multicast addresses
- * - EMAC_RFC_PERFECT_EN: the destination address
- * will be compared with the 6 byte station address
- * programmed in the station address by the filter
- * - EMAC_RFC_MAGP_WOL_EN: the result of the magic
- * packet filter will generate a WoL interrupt when
- * there is a match
- * - EMAC_RFC_PFILT_WOL_EN: the result of the perfect address
- * matching filter and the imperfect hash filter will
- * generate a WoL interrupt when there is a match
- * @param[in] NewState New State of this command, should be:
- * - ENABLE
- * - DISABLE
- * @return None
- **********************************************************************/
-void EMAC_SetFilterMode(uint32_t ulFilterMode, FunctionalState NewState)
-{
- if (NewState == ENABLE){
- LPC_EMAC->RxFilterCtrl |= ulFilterMode;
- } else {
- LPC_EMAC->RxFilterCtrl &= ~ulFilterMode;
- }
-}
-
-/*********************************************************************//**
- * @brief Get status of Wake On LAN Filter for each specified
- * type in EMAC peripheral, clear this status if it is set
- * @param[in] ulWoLMode WoL Filter mode, should be:
- * - EMAC_WOL_UCAST: unicast frames caused WoL
- * - EMAC_WOL_UCAST: broadcast frame caused WoL
- * - EMAC_WOL_MCAST: multicast frame caused WoL
- * - EMAC_WOL_UCAST_HASH: unicast frame that passes the
- * imperfect hash filter caused WoL
- * - EMAC_WOL_MCAST_HASH: multicast frame that passes the
- * imperfect hash filter caused WoL
- * - EMAC_WOL_PERFECT:perfect address matching filter
- * caused WoL
- * - EMAC_WOL_RX_FILTER: the receive filter caused WoL
- * - EMAC_WOL_MAG_PACKET: the magic packet filter caused WoL
- * @return SET/RESET
- **********************************************************************/
-FlagStatus EMAC_GetWoLStatus(uint32_t ulWoLMode)
-{
- if (LPC_EMAC->RxFilterWoLStatus & ulWoLMode) {
- LPC_EMAC->RxFilterWoLClear = ulWoLMode;
- return SET;
- } else {
- return RESET;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Write data to Tx packet data buffer at current index due to
- * TxProduceIndex
- * @param[in] pDataStruct Pointer to a EMAC_PACKETBUF_Type structure
- * data that contain specified information about
- * Packet data buffer.
- * @return None
- **********************************************************************/
-void EMAC_WritePacketBuffer(EMAC_PACKETBUF_Type *pDataStruct)
-{
- uint32_t idx,len;
- uint32_t *sp,*dp;
-
- idx = LPC_EMAC->TxProduceIndex;
- sp = (uint32_t *)pDataStruct->pbDataBuf;
- dp = (uint32_t *)Tx_Desc[idx].Packet;
- /* Copy frame data to EMAC packet buffers. */
- for (len = (pDataStruct->ulDataLen + 3) >> 2; len; len--) {
- *dp++ = *sp++;
- }
- Tx_Desc[idx].Ctrl = (pDataStruct->ulDataLen - 1) | (EMAC_TCTRL_INT | EMAC_TCTRL_LAST);
-}
-
-/*********************************************************************//**
- * @brief Read data from Rx packet data buffer at current index due
- * to RxConsumeIndex
- * @param[in] pDataStruct Pointer to a EMAC_PACKETBUF_Type structure
- * data that contain specified information about
- * Packet data buffer.
- * @return None
- **********************************************************************/
-void EMAC_ReadPacketBuffer(EMAC_PACKETBUF_Type *pDataStruct)
-{
- uint32_t idx, len;
- uint32_t *dp, *sp;
-
- idx = LPC_EMAC->RxConsumeIndex;
- dp = (uint32_t *)pDataStruct->pbDataBuf;
- sp = (uint32_t *)Rx_Desc[idx].Packet;
-
- if (pDataStruct->pbDataBuf != NULL) {
- for (len = (pDataStruct->ulDataLen + 3) >> 2; len; len--) {
- *dp++ = *sp++;
- }
- }
-}
-
-/*********************************************************************//**
- * @brief Enable/Disable interrupt for each type in EMAC
- * @param[in] ulIntType Interrupt Type, should be:
- * - EMAC_INT_RX_OVERRUN: Receive Overrun
- * - EMAC_INT_RX_ERR: Receive Error
- * - EMAC_INT_RX_FIN: Receive Descriptor Finish
- * - EMAC_INT_RX_DONE: Receive Done
- * - EMAC_INT_TX_UNDERRUN: Transmit Under-run
- * - EMAC_INT_TX_ERR: Transmit Error
- * - EMAC_INT_TX_FIN: Transmit descriptor finish
- * - EMAC_INT_TX_DONE: Transmit Done
- * - EMAC_INT_SOFT_INT: Software interrupt
- * - EMAC_INT_WAKEUP: Wakeup interrupt
- * @param[in] NewState New State of this function, should be:
- * - ENABLE.
- * - DISABLE.
- * @return None
- **********************************************************************/
-void EMAC_IntCmd(uint32_t ulIntType, FunctionalState NewState)
-{
- if (NewState == ENABLE) {
- LPC_EMAC->IntEnable |= ulIntType;
- } else {
- LPC_EMAC->IntEnable &= ~(ulIntType);
- }
-}
-
-/*********************************************************************//**
- * @brief Check whether if specified interrupt flag is set or not
- * for each interrupt type in EMAC and clear interrupt pending
- * if it is set.
- * @param[in] ulIntType Interrupt Type, should be:
- * - EMAC_INT_RX_OVERRUN: Receive Overrun
- * - EMAC_INT_RX_ERR: Receive Error
- * - EMAC_INT_RX_FIN: Receive Descriptor Finish
- * - EMAC_INT_RX_DONE: Receive Done
- * - EMAC_INT_TX_UNDERRUN: Transmit Under-run
- * - EMAC_INT_TX_ERR: Transmit Error
- * - EMAC_INT_TX_FIN: Transmit descriptor finish
- * - EMAC_INT_TX_DONE: Transmit Done
- * - EMAC_INT_SOFT_INT: Software interrupt
- * - EMAC_INT_WAKEUP: Wakeup interrupt
- * @return New state of specified interrupt (SET or RESET)
- **********************************************************************/
-IntStatus EMAC_IntGetStatus(uint32_t ulIntType)
-{
- if (LPC_EMAC->IntStatus & ulIntType) {
- LPC_EMAC->IntClear = ulIntType;
- return SET;
- } else {
- return RESET;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Check whether if the current RxConsumeIndex is not equal to the
- * current RxProduceIndex.
- * @param[in] None
- * @return TRUE if they're not equal, otherwise return FALSE
- *
- * Note: In case the RxConsumeIndex is not equal to the RxProduceIndex,
- * it means there're available data has been received. They should be read
- * out and released the Receive Data Buffer by updating the RxConsumeIndex value.
- **********************************************************************/
-Bool EMAC_CheckReceiveIndex(void)
-{
- if (LPC_EMAC->RxConsumeIndex != LPC_EMAC->RxProduceIndex) {
- return TRUE;
- } else {
- return FALSE;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Check whether if the current TxProduceIndex is not equal to the
- * current RxProduceIndex - 1.
- * @param[in] None
- * @return TRUE if they're not equal, otherwise return FALSE
- *
- * Note: In case the RxConsumeIndex is equal to the RxProduceIndex - 1,
- * it means the transmit buffer is available and data can be written to transmit
- * buffer to be sent.
- **********************************************************************/
-Bool EMAC_CheckTransmitIndex(void)
-{
- uint32_t tmp = LPC_EMAC->TxConsumeIndex;
- if (LPC_EMAC->TxProduceIndex == ( tmp - 1 ))
- {
- return FALSE;
- }
- else if( ( tmp == 0 ) && ( LPC_EMAC->TxProduceIndex == ( EMAC_NUM_TX_FRAG - 1 ) ) )
- {
- return FALSE;
- }
- else
- {
- return TRUE;
- }
-}
-
-
-
-/*********************************************************************//**
- * @brief Get current status value of receive data (due to RxConsumeIndex)
- * @param[in] ulRxStatType Received Status type, should be one of following:
- * - EMAC_RINFO_CTRL_FRAME: Control Frame
- * - EMAC_RINFO_VLAN: VLAN Frame
- * - EMAC_RINFO_FAIL_FILT: RX Filter Failed
- * - EMAC_RINFO_MCAST: Multicast Frame
- * - EMAC_RINFO_BCAST: Broadcast Frame
- * - EMAC_RINFO_CRC_ERR: CRC Error in Frame
- * - EMAC_RINFO_SYM_ERR: Symbol Error from PHY
- * - EMAC_RINFO_LEN_ERR: Length Error
- * - EMAC_RINFO_RANGE_ERR: Range error(exceeded max size)
- * - EMAC_RINFO_ALIGN_ERR: Alignment error
- * - EMAC_RINFO_OVERRUN: Receive overrun
- * - EMAC_RINFO_NO_DESCR: No new Descriptor available
- * - EMAC_RINFO_LAST_FLAG: last Fragment in Frame
- * - EMAC_RINFO_ERR: Error Occurred (OR of all error)
- * @return Current value of receive data (due to RxConsumeIndex)
- **********************************************************************/
-FlagStatus EMAC_CheckReceiveDataStatus(uint32_t ulRxStatType)
-{
- uint32_t idx;
- idx = LPC_EMAC->RxConsumeIndex;
- return (((Rx_Stat[idx].Info) & ulRxStatType) ? SET : RESET);
-}
-
-
-/*********************************************************************//**
- * @brief Get size of current Received data in received buffer (due to
- * RxConsumeIndex)
- * @param[in] None
- * @return Size of received data
- **********************************************************************/
-uint32_t EMAC_GetReceiveDataSize(void)
-{
- uint32_t idx;
- idx =LPC_EMAC->RxConsumeIndex;
- return ((Rx_Stat[idx].Info) & EMAC_RINFO_SIZE);
-}
-
-/*********************************************************************//**
- * @brief Increase the RxConsumeIndex (after reading the Receive buffer
- * to release the Receive buffer) and wrap-around the index if
- * it reaches the maximum Receive Number
- * @param[in] None
- * @return None
- **********************************************************************/
-void EMAC_UpdateRxConsumeIndex(void)
-{
- // Get current Rx consume index
- uint32_t idx = LPC_EMAC->RxConsumeIndex;
-
- /* Release frame from EMAC buffer */
- if (++idx == EMAC_NUM_RX_FRAG) idx = 0;
- LPC_EMAC->RxConsumeIndex = idx;
-}
-
-/*********************************************************************//**
- * @brief Increase the TxProduceIndex (after writting to the Transmit buffer
- * to enable the Transmit buffer) and wrap-around the index if
- * it reaches the maximum Transmit Number
- * @param[in] None
- * @return None
- **********************************************************************/
-void EMAC_UpdateTxProduceIndex(void)
-{
- // Get current Tx produce index
- uint32_t idx = LPC_EMAC->TxProduceIndex;
-
- /* Start frame transmission */
- if (++idx == EMAC_NUM_TX_FRAG) idx = 0;
- LPC_EMAC->TxProduceIndex = idx;
-}
-
-
-/**
- * @}
- */
-
-#endif /* _EMAC */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_exti.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_exti.c
deleted file mode 100644
index 805bee4b09..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_exti.c
+++ /dev/null
@@ -1,171 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_exti.c 2010-06-18
-*//**
-* @file lpc17xx_exti.c
-* @brief Contains all functions support for External interrupt firmware
-* library on LPC17xx
-* @version 3.0
-* @date 18. June. 2010
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2010, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup EXTI
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_exti.h"
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-
-#ifdef _EXTI
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup EXTI_Public_Functions
- * @{
- */
-
-/*********************************************************************//**
- * @brief Initial for EXT
- * - Set EXTINT, EXTMODE, EXTPOLAR registers to default value
- * @param[in] None
- * @return None
- **********************************************************************/
-void EXTI_Init(void)
-{
- LPC_SC->EXTINT = 0xF;
- LPC_SC->EXTMODE = 0x0;
- LPC_SC->EXTPOLAR = 0x0;
-}
-
-
-/*********************************************************************//**
-* @brief Close EXT
-* @param[in] None
-* @return None
-**********************************************************************/
-void EXTI_DeInit(void)
-{
- ;
-}
-
-/*********************************************************************//**
- * @brief Configuration for EXT
- * - Set EXTINT, EXTMODE, EXTPOLAR register
- * @param[in] EXTICfg Pointer to a EXTI_InitTypeDef structure
- * that contains the configuration information for the
- * specified external interrupt
- * @return None
- **********************************************************************/
-void EXTI_Config(EXTI_InitTypeDef *EXTICfg)
-{
- LPC_SC->EXTINT = 0x0;
- EXTI_SetMode(EXTICfg->EXTI_Line, EXTICfg->EXTI_Mode);
- EXTI_SetPolarity(EXTICfg->EXTI_Line, EXTICfg->EXTI_polarity);
-}
-
-/*********************************************************************//**
-* @brief Set mode for EXTI pin
-* @param[in] EXTILine external interrupt line, should be:
-* - EXTI_EINT0: external interrupt line 0
-* - EXTI_EINT1: external interrupt line 1
-* - EXTI_EINT2: external interrupt line 2
-* - EXTI_EINT3: external interrupt line 3
-* @param[in] mode external mode, should be:
-* - EXTI_MODE_LEVEL_SENSITIVE
-* - EXTI_MODE_EDGE_SENSITIVE
-* @return None
-*********************************************************************/
-void EXTI_SetMode(EXTI_LINE_ENUM EXTILine, EXTI_MODE_ENUM mode)
-{
- if(mode == EXTI_MODE_EDGE_SENSITIVE)
- {
- LPC_SC->EXTMODE |= (1 << EXTILine);
- }
- else if(mode == EXTI_MODE_LEVEL_SENSITIVE)
- {
- LPC_SC->EXTMODE &= ~(1 << EXTILine);
- }
-}
-
-/*********************************************************************//**
-* @brief Set polarity for EXTI pin
-* @param[in] EXTILine external interrupt line, should be:
-* - EXTI_EINT0: external interrupt line 0
-* - EXTI_EINT1: external interrupt line 1
-* - EXTI_EINT2: external interrupt line 2
-* - EXTI_EINT3: external interrupt line 3
-* @param[in] polarity external polarity value, should be:
-* - EXTI_POLARITY_LOW_ACTIVE_OR_FALLING_EDGE
-* - EXTI_POLARITY_LOW_ACTIVE_OR_FALLING_EDGE
-* @return None
-*********************************************************************/
-void EXTI_SetPolarity(EXTI_LINE_ENUM EXTILine, EXTI_POLARITY_ENUM polarity)
-{
- if(polarity == EXTI_POLARITY_HIGH_ACTIVE_OR_RISING_EDGE)
- {
- LPC_SC->EXTPOLAR |= (1 << EXTILine);
- }
- else if(polarity == EXTI_POLARITY_LOW_ACTIVE_OR_FALLING_EDGE)
- {
- LPC_SC->EXTPOLAR &= ~(1 << EXTILine);
- }
-}
-
-/*********************************************************************//**
-* @brief Clear External interrupt flag
-* @param[in] EXTILine external interrupt line, should be:
-* - EXTI_EINT0: external interrupt line 0
-* - EXTI_EINT1: external interrupt line 1
-* - EXTI_EINT2: external interrupt line 2
-* - EXTI_EINT3: external interrupt line 3
-* @return None
-*********************************************************************/
-void EXTI_ClearEXTIFlag(EXTI_LINE_ENUM EXTILine)
-{
- LPC_SC->EXTINT = (1 << EXTILine);
-}
-
-/**
- * @}
- */
-
-#endif /* _EXTI */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
-
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_gpdma.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_gpdma.c
deleted file mode 100644
index 0267de3d17..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_gpdma.c
+++ /dev/null
@@ -1,463 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_gpdma.c 2010-03-21
-*//**
-* @file lpc17xx_gpdma.c
-* @brief Contains all functions support for GPDMA firmware
-* library on LPC17xx
-* @version 2.1
-* @date 25. July. 2011
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2010, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup GPDMA
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_gpdma.h"
-#include "lpc17xx_clkpwr.h"
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-#ifdef _GPDMA
-
-
-/* Private Variables ---------------------------------------------------------- */
-/** @defgroup GPDMA_Private_Variables GPDMA Private Variables
- * @{
- */
-
-/**
- * @brief Lookup Table of Connection Type matched with
- * Peripheral Data (FIFO) register base address
- */
-//#ifdef __IAR_SYSTEMS_ICC__
-volatile const void *GPDMA_LUTPerAddr[] = {
- (&LPC_SSP0->DR), // SSP0 Tx
- (&LPC_SSP0->DR), // SSP0 Rx
- (&LPC_SSP1->DR), // SSP1 Tx
- (&LPC_SSP1->DR), // SSP1 Rx
- (&LPC_ADC->ADGDR), // ADC
- (&LPC_I2S->I2STXFIFO), // I2S Tx
- (&LPC_I2S->I2SRXFIFO), // I2S Rx
- (&LPC_DAC->DACR), // DAC
- (&LPC_UART0->/*RBTHDLR.*/THR), // UART0 Tx
- (&LPC_UART0->/*RBTHDLR.*/RBR), // UART0 Rx
- (&LPC_UART1->/*RBTHDLR.*/THR), // UART1 Tx
- (&LPC_UART1->/*RBTHDLR.*/RBR), // UART1 Rx
- (&LPC_UART2->/*RBTHDLR.*/THR), // UART2 Tx
- (&LPC_UART2->/*RBTHDLR.*/RBR), // UART2 Rx
- (&LPC_UART3->/*RBTHDLR.*/THR), // UART3 Tx
- (&LPC_UART3->/*RBTHDLR.*/RBR), // UART3 Rx
- (&LPC_TIM0->MR0), // MAT0.0
- (&LPC_TIM0->MR1), // MAT0.1
- (&LPC_TIM1->MR0), // MAT1.0
- (&LPC_TIM1->MR1), // MAT1.1
- (&LPC_TIM2->MR0), // MAT2.0
- (&LPC_TIM2->MR1), // MAT2.1
- (&LPC_TIM3->MR0), // MAT3.0
- (&LPC_TIM3->MR1) // MAT3.1
-};
-//#else
-//const uint32_t GPDMA_LUTPerAddr[] = {
-// ((uint32_t)&LPC_SSP0->DR), // SSP0 Tx
-// ((uint32_t)&LPC_SSP0->DR), // SSP0 Rx
-// ((uint32_t)&LPC_SSP1->DR), // SSP1 Tx
-// ((uint32_t)&LPC_SSP1->DR), // SSP1 Rx
-// ((uint32_t)&LPC_ADC->ADGDR), // ADC
-// ((uint32_t)&LPC_I2S->I2STXFIFO), // I2S Tx
-// ((uint32_t)&LPC_I2S->I2SRXFIFO), // I2S Rx
-// ((uint32_t)&LPC_DAC->DACR), // DAC
-// ((uint32_t)&LPC_UART0->/*RBTHDLR.*/THR), // UART0 Tx
-// ((uint32_t)&LPC_UART0->/*RBTHDLR.*/RBR), // UART0 Rx
-// ((uint32_t)&LPC_UART1->/*RBTHDLR.*/THR), // UART1 Tx
-// ((uint32_t)&LPC_UART1->/*RBTHDLR.*/RBR), // UART1 Rx
-// ((uint32_t)&LPC_UART2->/*RBTHDLR.*/THR), // UART2 Tx
-// ((uint32_t)&LPC_UART2->/*RBTHDLR.*/RBR), // UART2 Rx
-// ((uint32_t)&LPC_UART3->/*RBTHDLR.*/THR), // UART3 Tx
-// ((uint32_t)&LPC_UART3->/*RBTHDLR.*/RBR), // UART3 Rx
-// ((uint32_t)&LPC_TIM0->MR0), // MAT0.0
-// ((uint32_t)&LPC_TIM0->MR1), // MAT0.1
-// ((uint32_t)&LPC_TIM1->MR0), // MAT1.0
-// ((uint32_t)&LPC_TIM1->MR1), // MAT1.1
-// ((uint32_t)&LPC_TIM2->MR0), // MAT2.0
-// ((uint32_t)&LPC_TIM2->MR1), // MAT2.1
-// ((uint32_t)&LPC_TIM3->MR0), // MAT3.0
-// ((uint32_t)&LPC_TIM3->MR1) // MAT3.1
-//};
-//#endif
-/**
- * @brief Lookup Table of GPDMA Channel Number matched with
- * GPDMA channel pointer
- */
-const LPC_GPDMACH_TypeDef *pGPDMACh[8] = {
- LPC_GPDMACH0, // GPDMA Channel 0
- LPC_GPDMACH1, // GPDMA Channel 1
- LPC_GPDMACH2, // GPDMA Channel 2
- LPC_GPDMACH3, // GPDMA Channel 3
- LPC_GPDMACH4, // GPDMA Channel 4
- LPC_GPDMACH5, // GPDMA Channel 5
- LPC_GPDMACH6, // GPDMA Channel 6
- LPC_GPDMACH7 // GPDMA Channel 7
-};
-/**
- * @brief Optimized Peripheral Source and Destination burst size
- */
-const uint8_t GPDMA_LUTPerBurst[] = {
- GPDMA_BSIZE_4, // SSP0 Tx
- GPDMA_BSIZE_4, // SSP0 Rx
- GPDMA_BSIZE_4, // SSP1 Tx
- GPDMA_BSIZE_4, // SSP1 Rx
- GPDMA_BSIZE_1, // ADC
- GPDMA_BSIZE_32, // I2S channel 0
- GPDMA_BSIZE_32, // I2S channel 1
- GPDMA_BSIZE_1, // DAC
- GPDMA_BSIZE_1, // UART0 Tx
- GPDMA_BSIZE_1, // UART0 Rx
- GPDMA_BSIZE_1, // UART1 Tx
- GPDMA_BSIZE_1, // UART1 Rx
- GPDMA_BSIZE_1, // UART2 Tx
- GPDMA_BSIZE_1, // UART2 Rx
- GPDMA_BSIZE_1, // UART3 Tx
- GPDMA_BSIZE_1, // UART3 Rx
- GPDMA_BSIZE_1, // MAT0.0
- GPDMA_BSIZE_1, // MAT0.1
- GPDMA_BSIZE_1, // MAT1.0
- GPDMA_BSIZE_1, // MAT1.1
- GPDMA_BSIZE_1, // MAT2.0
- GPDMA_BSIZE_1, // MAT2.1
- GPDMA_BSIZE_1, // MAT3.0
- GPDMA_BSIZE_1 // MAT3.1
-};
-/**
- * @brief Optimized Peripheral Source and Destination transfer width
- */
-const uint8_t GPDMA_LUTPerWid[] = {
- GPDMA_WIDTH_BYTE, // SSP0 Tx
- GPDMA_WIDTH_BYTE, // SSP0 Rx
- GPDMA_WIDTH_BYTE, // SSP1 Tx
- GPDMA_WIDTH_BYTE, // SSP1 Rx
- GPDMA_WIDTH_WORD, // ADC
- GPDMA_WIDTH_WORD, // I2S channel 0
- GPDMA_WIDTH_WORD, // I2S channel 1
- GPDMA_WIDTH_BYTE, // DAC
- GPDMA_WIDTH_BYTE, // UART0 Tx
- GPDMA_WIDTH_BYTE, // UART0 Rx
- GPDMA_WIDTH_BYTE, // UART1 Tx
- GPDMA_WIDTH_BYTE, // UART1 Rx
- GPDMA_WIDTH_BYTE, // UART2 Tx
- GPDMA_WIDTH_BYTE, // UART2 Rx
- GPDMA_WIDTH_BYTE, // UART3 Tx
- GPDMA_WIDTH_BYTE, // UART3 Rx
- GPDMA_WIDTH_WORD, // MAT0.0
- GPDMA_WIDTH_WORD, // MAT0.1
- GPDMA_WIDTH_WORD, // MAT1.0
- GPDMA_WIDTH_WORD, // MAT1.1
- GPDMA_WIDTH_WORD, // MAT2.0
- GPDMA_WIDTH_WORD, // MAT2.1
- GPDMA_WIDTH_WORD, // MAT3.0
- GPDMA_WIDTH_WORD // MAT3.1
-};
-
-/**
- * @}
- */
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup GPDMA_Public_Functions
- * @{
- */
-
-/********************************************************************//**
- * @brief Initialize GPDMA controller
- * @param None
- * @return None
- *********************************************************************/
-void GPDMA_Init(void)
-{
- /* Enable GPDMA clock */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCGPDMA, ENABLE);
-
- // Reset all channel configuration register
- LPC_GPDMACH0->DMACCConfig = 0;
- LPC_GPDMACH1->DMACCConfig = 0;
- LPC_GPDMACH2->DMACCConfig = 0;
- LPC_GPDMACH3->DMACCConfig = 0;
- LPC_GPDMACH4->DMACCConfig = 0;
- LPC_GPDMACH5->DMACCConfig = 0;
- LPC_GPDMACH6->DMACCConfig = 0;
- LPC_GPDMACH7->DMACCConfig = 0;
-
- /* Clear all DMA interrupt and error flag */
- LPC_GPDMA->DMACIntTCClear = 0xFF;
- LPC_GPDMA->DMACIntErrClr = 0xFF;
-}
-
-/********************************************************************//**
- * @brief Setup GPDMA channel peripheral according to the specified
- * parameters in the GPDMAChannelConfig.
- * @param[in] GPDMAChannelConfig Pointer to a GPDMA_CH_CFG_Type
- * structure that contains the configuration
- * information for the specified GPDMA channel peripheral.
- * @return ERROR if selected channel is enabled before
- * or SUCCESS if channel is configured successfully
- *********************************************************************/
-Status GPDMA_Setup(GPDMA_Channel_CFG_Type *GPDMAChannelConfig)
-{
- LPC_GPDMACH_TypeDef *pDMAch;
- uint32_t tmp1, tmp2;
-
- if (LPC_GPDMA->DMACEnbldChns & (GPDMA_DMACEnbldChns_Ch(GPDMAChannelConfig->ChannelNum))) {
- // This channel is enabled, return ERROR, need to release this channel first
- return ERROR;
- }
-
- // Get Channel pointer
- pDMAch = (LPC_GPDMACH_TypeDef *) pGPDMACh[GPDMAChannelConfig->ChannelNum];
-
- // Reset the Interrupt status
- LPC_GPDMA->DMACIntTCClear = GPDMA_DMACIntTCClear_Ch(GPDMAChannelConfig->ChannelNum);
- LPC_GPDMA->DMACIntErrClr = GPDMA_DMACIntErrClr_Ch(GPDMAChannelConfig->ChannelNum);
-
- // Clear DMA configure
- pDMAch->DMACCControl = 0x00;
- pDMAch->DMACCConfig = 0x00;
-
- /* Assign Linker List Item value */
- pDMAch->DMACCLLI = GPDMAChannelConfig->DMALLI;
-
- /* Set value to Channel Control Registers */
- switch (GPDMAChannelConfig->TransferType)
- {
- // Memory to memory
- case GPDMA_TRANSFERTYPE_M2M:
- // Assign physical source and destination address
- pDMAch->DMACCSrcAddr = GPDMAChannelConfig->SrcMemAddr;
- pDMAch->DMACCDestAddr = GPDMAChannelConfig->DstMemAddr;
- pDMAch->DMACCControl
- = GPDMA_DMACCxControl_TransferSize(GPDMAChannelConfig->TransferSize) \
- | GPDMA_DMACCxControl_SBSize(GPDMA_BSIZE_32) \
- | GPDMA_DMACCxControl_DBSize(GPDMA_BSIZE_32) \
- | GPDMA_DMACCxControl_SWidth(GPDMAChannelConfig->TransferWidth) \
- | GPDMA_DMACCxControl_DWidth(GPDMAChannelConfig->TransferWidth) \
- | GPDMA_DMACCxControl_SI \
- | GPDMA_DMACCxControl_DI \
- | GPDMA_DMACCxControl_I;
- break;
- // Memory to peripheral
- case GPDMA_TRANSFERTYPE_M2P:
- // Assign physical source
- pDMAch->DMACCSrcAddr = GPDMAChannelConfig->SrcMemAddr;
- // Assign peripheral destination address
- pDMAch->DMACCDestAddr = (uint32_t)GPDMA_LUTPerAddr[GPDMAChannelConfig->DstConn];
- pDMAch->DMACCControl
- = GPDMA_DMACCxControl_TransferSize((uint32_t)GPDMAChannelConfig->TransferSize) \
- | GPDMA_DMACCxControl_SBSize((uint32_t)GPDMA_LUTPerBurst[GPDMAChannelConfig->DstConn]) \
- | GPDMA_DMACCxControl_DBSize((uint32_t)GPDMA_LUTPerBurst[GPDMAChannelConfig->DstConn]) \
- | GPDMA_DMACCxControl_SWidth((uint32_t)GPDMA_LUTPerWid[GPDMAChannelConfig->DstConn]) \
- | GPDMA_DMACCxControl_DWidth((uint32_t)GPDMA_LUTPerWid[GPDMAChannelConfig->DstConn]) \
- | GPDMA_DMACCxControl_SI \
- | GPDMA_DMACCxControl_I;
- break;
- // Peripheral to memory
- case GPDMA_TRANSFERTYPE_P2M:
- // Assign peripheral source address
- pDMAch->DMACCSrcAddr = (uint32_t)GPDMA_LUTPerAddr[GPDMAChannelConfig->SrcConn];
- // Assign memory destination address
- pDMAch->DMACCDestAddr = GPDMAChannelConfig->DstMemAddr;
- pDMAch->DMACCControl
- = GPDMA_DMACCxControl_TransferSize((uint32_t)GPDMAChannelConfig->TransferSize) \
- | GPDMA_DMACCxControl_SBSize((uint32_t)GPDMA_LUTPerBurst[GPDMAChannelConfig->SrcConn]) \
- | GPDMA_DMACCxControl_DBSize((uint32_t)GPDMA_LUTPerBurst[GPDMAChannelConfig->SrcConn]) \
- | GPDMA_DMACCxControl_SWidth((uint32_t)GPDMA_LUTPerWid[GPDMAChannelConfig->SrcConn]) \
- | GPDMA_DMACCxControl_DWidth((uint32_t)GPDMA_LUTPerWid[GPDMAChannelConfig->SrcConn]) \
- | GPDMA_DMACCxControl_DI \
- | GPDMA_DMACCxControl_I;
- break;
- // Peripheral to peripheral
- case GPDMA_TRANSFERTYPE_P2P:
- // Assign peripheral source address
- pDMAch->DMACCSrcAddr = (uint32_t)GPDMA_LUTPerAddr[GPDMAChannelConfig->SrcConn];
- // Assign peripheral destination address
- pDMAch->DMACCDestAddr = (uint32_t)GPDMA_LUTPerAddr[GPDMAChannelConfig->DstConn];
- pDMAch->DMACCControl
- = GPDMA_DMACCxControl_TransferSize((uint32_t)GPDMAChannelConfig->TransferSize) \
- | GPDMA_DMACCxControl_SBSize((uint32_t)GPDMA_LUTPerBurst[GPDMAChannelConfig->SrcConn]) \
- | GPDMA_DMACCxControl_DBSize((uint32_t)GPDMA_LUTPerBurst[GPDMAChannelConfig->DstConn]) \
- | GPDMA_DMACCxControl_SWidth((uint32_t)GPDMA_LUTPerWid[GPDMAChannelConfig->SrcConn]) \
- | GPDMA_DMACCxControl_DWidth((uint32_t)GPDMA_LUTPerWid[GPDMAChannelConfig->DstConn]) \
- | GPDMA_DMACCxControl_I;
- break;
- // Do not support any more transfer type, return ERROR
- default:
- return ERROR;
- }
-
- /* Re-Configure DMA Request Select for source peripheral */
- if (GPDMAChannelConfig->SrcConn > 15)
- {
- LPC_SC->DMAREQSEL |= (1<<(GPDMAChannelConfig->SrcConn - 16));
- } else {
- LPC_SC->DMAREQSEL &= ~(1<<(GPDMAChannelConfig->SrcConn - 8));
- }
-
- /* Re-Configure DMA Request Select for Destination peripheral */
- if (GPDMAChannelConfig->DstConn > 15)
- {
- LPC_SC->DMAREQSEL |= (1<<(GPDMAChannelConfig->DstConn - 16));
- } else {
- LPC_SC->DMAREQSEL &= ~(1<<(GPDMAChannelConfig->DstConn - 8));
- }
-
- /* Enable DMA channels, little endian */
- LPC_GPDMA->DMACConfig = GPDMA_DMACConfig_E;
- while (!(LPC_GPDMA->DMACConfig & GPDMA_DMACConfig_E));
-
- // Calculate absolute value for Connection number
- tmp1 = GPDMAChannelConfig->SrcConn;
- tmp1 = ((tmp1 > 15) ? (tmp1 - 8) : tmp1);
- tmp2 = GPDMAChannelConfig->DstConn;
- tmp2 = ((tmp2 > 15) ? (tmp2 - 8) : tmp2);
-
- // Configure DMA Channel, enable Error Counter and Terminate counter
- pDMAch->DMACCConfig = GPDMA_DMACCxConfig_IE | GPDMA_DMACCxConfig_ITC /*| GPDMA_DMACCxConfig_E*/ \
- | GPDMA_DMACCxConfig_TransferType((uint32_t)GPDMAChannelConfig->TransferType) \
- | GPDMA_DMACCxConfig_SrcPeripheral(tmp1) \
- | GPDMA_DMACCxConfig_DestPeripheral(tmp2);
-
- return SUCCESS;
-}
-
-
-/*********************************************************************//**
- * @brief Enable/Disable DMA channel
- * @param[in] channelNum GPDMA channel, should be in range from 0 to 7
- * @param[in] NewState New State of this command, should be:
- * - ENABLE.
- * - DISABLE.
- * @return None
- **********************************************************************/
-void GPDMA_ChannelCmd(uint8_t channelNum, FunctionalState NewState)
-{
- LPC_GPDMACH_TypeDef *pDMAch;
-
- // Get Channel pointer
- pDMAch = (LPC_GPDMACH_TypeDef *) pGPDMACh[channelNum];
-
- if (NewState == ENABLE) {
- pDMAch->DMACCConfig |= GPDMA_DMACCxConfig_E;
- } else {
- pDMAch->DMACCConfig &= ~GPDMA_DMACCxConfig_E;
- }
-}
-/*********************************************************************//**
- * @brief Check if corresponding channel does have an active interrupt
- * request or not
- * @param[in] type type of status, should be:
- * - GPDMA_STAT_INT: GPDMA Interrupt Status
- * - GPDMA_STAT_INTTC: GPDMA Interrupt Terminal Count Request Status
- * - GPDMA_STAT_INTERR: GPDMA Interrupt Error Status
- * - GPDMA_STAT_RAWINTTC: GPDMA Raw Interrupt Terminal Count Status
- * - GPDMA_STAT_RAWINTERR: GPDMA Raw Error Interrupt Status
- * - GPDMA_STAT_ENABLED_CH:GPDMA Enabled Channel Status
- * @param[in] channel GPDMA channel, should be in range from 0 to 7
- * @return IntStatus status of DMA channel interrupt after masking
- * Should be:
- * - SET: the corresponding channel has no active interrupt request
- * - RESET: the corresponding channel does have an active interrupt request
- **********************************************************************/
-IntStatus GPDMA_IntGetStatus(GPDMA_Status_Type type, uint8_t channel)
-{
- CHECK_PARAM(PARAM_GPDMA_STAT(type));
- CHECK_PARAM(PARAM_GPDMA_CHANNEL(channel));
-
- switch (type)
- {
- case GPDMA_STAT_INT: //check status of DMA channel interrupts
- if (LPC_GPDMA->DMACIntStat & (GPDMA_DMACIntStat_Ch(channel)))
- return SET;
- return RESET;
- case GPDMA_STAT_INTTC: // check terminal count interrupt request status for DMA
- if (LPC_GPDMA->DMACIntTCStat & GPDMA_DMACIntTCStat_Ch(channel))
- return SET;
- return RESET;
- case GPDMA_STAT_INTERR: //check interrupt status for DMA channels
- if (LPC_GPDMA->DMACIntErrStat & GPDMA_DMACIntTCClear_Ch(channel))
- return SET;
- return RESET;
- case GPDMA_STAT_RAWINTTC: //check status of the terminal count interrupt for DMA channels
- if (LPC_GPDMA->DMACRawIntErrStat & GPDMA_DMACRawIntTCStat_Ch(channel))
- return SET;
- return RESET;
- case GPDMA_STAT_RAWINTERR: //check status of the error interrupt for DMA channels
- if (LPC_GPDMA->DMACRawIntTCStat & GPDMA_DMACRawIntErrStat_Ch(channel))
- return SET;
- return RESET;
- default: //check enable status for DMA channels
- if (LPC_GPDMA->DMACEnbldChns & GPDMA_DMACEnbldChns_Ch(channel))
- return SET;
- return RESET;
- }
-}
-
-/*********************************************************************//**
- * @brief Clear one or more interrupt requests on DMA channels
- * @param[in] type type of interrupt request, should be:
- * - GPDMA_STATCLR_INTTC: GPDMA Interrupt Terminal Count Request Clear
- * - GPDMA_STATCLR_INTERR: GPDMA Interrupt Error Clear
- * @param[in] channel GPDMA channel, should be in range from 0 to 7
- * @return None
- **********************************************************************/
-void GPDMA_ClearIntPending(GPDMA_StateClear_Type type, uint8_t channel)
-{
- CHECK_PARAM(PARAM_GPDMA_STATCLR(type));
- CHECK_PARAM(PARAM_GPDMA_CHANNEL(channel));
-
- if (type == GPDMA_STATCLR_INTTC) // clears the terminal count interrupt request on DMA channel
- LPC_GPDMA->DMACIntTCClear = GPDMA_DMACIntTCClear_Ch(channel);
- else // clear the error interrupt request
- LPC_GPDMA->DMACIntErrClr = GPDMA_DMACIntErrClr_Ch(channel);
-}
-
-/**
- * @}
- */
-
-#endif /* _GPDMA */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
-
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_gpio.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_gpio.c
deleted file mode 100644
index 8f0cd3953a..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_gpio.c
+++ /dev/null
@@ -1,762 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_gpio.c 2010-05-21
-*//**
-* @file lpc17xx_gpio.c
-* @brief Contains all functions support for GPIO firmware
-* library on LPC17xx
-* @version 2.0
-* @date 21. May. 2010
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2010, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup GPIO
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_gpio.h"
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-
-#ifdef _GPIO
-
-/* Private Functions ---------------------------------------------------------- */
-
-static LPC_GPIO_TypeDef *GPIO_GetPointer(uint8_t portNum);
-static GPIO_HalfWord_TypeDef *FIO_HalfWordGetPointer(uint8_t portNum);
-static GPIO_Byte_TypeDef *FIO_ByteGetPointer(uint8_t portNum);
-
-/*********************************************************************//**
- * @brief Get pointer to GPIO peripheral due to GPIO port
- * @param[in] portNum Port Number value, should be in range from 0 to 4.
- * @return Pointer to GPIO peripheral
- **********************************************************************/
-static LPC_GPIO_TypeDef *GPIO_GetPointer(uint8_t portNum)
-{
- LPC_GPIO_TypeDef *pGPIO = NULL;
-
- switch (portNum) {
- case 0:
- pGPIO = LPC_GPIO0;
- break;
- case 1:
- pGPIO = LPC_GPIO1;
- break;
- case 2:
- pGPIO = LPC_GPIO2;
- break;
- case 3:
- pGPIO = LPC_GPIO3;
- break;
- case 4:
- pGPIO = LPC_GPIO4;
- break;
- default:
- break;
- }
-
- return pGPIO;
-}
-
-/*********************************************************************//**
- * @brief Get pointer to FIO peripheral in halfword accessible style
- * due to FIO port
- * @param[in] portNum Port Number value, should be in range from 0 to 4.
- * @return Pointer to FIO peripheral
- **********************************************************************/
-static GPIO_HalfWord_TypeDef *FIO_HalfWordGetPointer(uint8_t portNum)
-{
- GPIO_HalfWord_TypeDef *pFIO = NULL;
-
- switch (portNum) {
- case 0:
- pFIO = GPIO0_HalfWord;
- break;
- case 1:
- pFIO = GPIO1_HalfWord;
- break;
- case 2:
- pFIO = GPIO2_HalfWord;
- break;
- case 3:
- pFIO = GPIO3_HalfWord;
- break;
- case 4:
- pFIO = GPIO4_HalfWord;
- break;
- default:
- break;
- }
-
- return pFIO;
-}
-
-/*********************************************************************//**
- * @brief Get pointer to FIO peripheral in byte accessible style
- * due to FIO port
- * @param[in] portNum Port Number value, should be in range from 0 to 4.
- * @return Pointer to FIO peripheral
- **********************************************************************/
-static GPIO_Byte_TypeDef *FIO_ByteGetPointer(uint8_t portNum)
-{
- GPIO_Byte_TypeDef *pFIO = NULL;
-
- switch (portNum) {
- case 0:
- pFIO = GPIO0_Byte;
- break;
- case 1:
- pFIO = GPIO1_Byte;
- break;
- case 2:
- pFIO = GPIO2_Byte;
- break;
- case 3:
- pFIO = GPIO3_Byte;
- break;
- case 4:
- pFIO = GPIO4_Byte;
- break;
- default:
- break;
- }
-
- return pFIO;
-}
-
-/* End of Private Functions --------------------------------------------------- */
-
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup GPIO_Public_Functions
- * @{
- */
-
-
-/* GPIO ------------------------------------------------------------------------------ */
-
-/*********************************************************************//**
- * @brief Set Direction for GPIO port.
- * @param[in] portNum Port Number value, should be in range from 0 to 4
- * @param[in] bitValue Value that contains all bits to set direction,
- * in range from 0 to 0xFFFFFFFF.
- * example: value 0x5 to set direction for bit 0 and bit 1.
- * @param[in] dir Direction value, should be:
- * - 0: Input.
- * - 1: Output.
- * @return None
- *
- * Note: All remaining bits that are not activated in bitValue (value '0')
- * will not be effected by this function.
- **********************************************************************/
-void GPIO_SetDir(uint8_t portNum, uint32_t bitValue, uint8_t dir)
-{
- LPC_GPIO_TypeDef *pGPIO = GPIO_GetPointer(portNum);
-
- if (pGPIO != NULL) {
- // Enable Output
- if (dir) {
- pGPIO->FIODIR |= bitValue;
- }
- // Enable Input
- else {
- pGPIO->FIODIR &= ~bitValue;
- }
- }
-}
-
-
-/*********************************************************************//**
- * @brief Set Value for bits that have output direction on GPIO port.
- * @param[in] portNum Port number value, should be in range from 0 to 4
- * @param[in] bitValue Value that contains all bits on GPIO to set,
- * in range from 0 to 0xFFFFFFFF.
- * example: value 0x5 to set bit 0 and bit 1.
- * @return None
- *
- * Note:
- * - For all bits that has been set as input direction, this function will
- * not effect.
- * - For all remaining bits that are not activated in bitValue (value '0')
- * will not be effected by this function.
- **********************************************************************/
-void GPIO_SetValue(uint8_t portNum, uint32_t bitValue)
-{
- LPC_GPIO_TypeDef *pGPIO = GPIO_GetPointer(portNum);
-
- if (pGPIO != NULL) {
- pGPIO->FIOSET = bitValue;
- }
-}
-
-/*********************************************************************//**
- * @brief Clear Value for bits that have output direction on GPIO port.
- * @param[in] portNum Port number value, should be in range from 0 to 4
- * @param[in] bitValue Value that contains all bits on GPIO to clear,
- * in range from 0 to 0xFFFFFFFF.
- * example: value 0x5 to clear bit 0 and bit 1.
- * @return None
- *
- * Note:
- * - For all bits that has been set as input direction, this function will
- * not effect.
- * - For all remaining bits that are not activated in bitValue (value '0')
- * will not be effected by this function.
- **********************************************************************/
-void GPIO_ClearValue(uint8_t portNum, uint32_t bitValue)
-{
- LPC_GPIO_TypeDef *pGPIO = GPIO_GetPointer(portNum);
-
- if (pGPIO != NULL) {
- pGPIO->FIOCLR = bitValue;
- }
-}
-
-/*********************************************************************//**
- * @brief Read Current state on port pin that have input direction of GPIO
- * @param[in] portNum Port number to read value, in range from 0 to 4
- * @return Current value of GPIO port.
- *
- * Note: Return value contain state of each port pin (bit) on that GPIO regardless
- * its direction is input or output.
- **********************************************************************/
-uint32_t GPIO_ReadValue(uint8_t portNum)
-{
- LPC_GPIO_TypeDef *pGPIO = GPIO_GetPointer(portNum);
-
- if (pGPIO != NULL) {
- return pGPIO->FIOPIN;
- }
-
- return (0);
-}
-
-/*********************************************************************//**
- * @brief Enable GPIO interrupt (just used for P0.0-P0.30, P2.0-P2.13)
- * @param[in] portNum Port number to read value, should be: 0 or 2
- * @param[in] bitValue Value that contains all bits on GPIO to enable,
- * in range from 0 to 0xFFFFFFFF.
- * @param[in] edgeState state of edge, should be:
- * - 0: Rising edge
- * - 1: Falling edge
- * @return None
- **********************************************************************/
-void GPIO_IntCmd(uint8_t portNum, uint32_t bitValue, uint8_t edgeState)
-{
- if((portNum == 0)&&(edgeState == 0))
- LPC_GPIOINT->IO0IntEnR = bitValue;
- else if ((portNum == 2)&&(edgeState == 0))
- LPC_GPIOINT->IO2IntEnR = bitValue;
- else if ((portNum == 0)&&(edgeState == 1))
- LPC_GPIOINT->IO0IntEnF = bitValue;
- else if ((portNum == 2)&&(edgeState == 1))
- LPC_GPIOINT->IO2IntEnF = bitValue;
- else
- //Error
- while(1);
-}
-
-/*********************************************************************//**
- * @brief Get GPIO Interrupt Status (just used for P0.0-P0.30, P2.0-P2.13)
- * @param[in] portNum Port number to read value, should be: 0 or 2
- * @param[in] pinNum Pin number, should be: 0..30(with port 0) and 0..13
- * (with port 2)
- * @param[in] edgeState state of edge, should be:
- * - 0: Rising edge
- * - 1: Falling edge
- * @return Bool could be:
- * - ENABLE: Interrupt has been generated due to a rising
- * edge on P0.0
- * - DISABLE: A rising edge has not been detected on P0.0
- **********************************************************************/
-FunctionalState GPIO_GetIntStatus(uint8_t portNum, uint32_t pinNum, uint8_t edgeState)
-{
- if((portNum == 0) && (edgeState == 0))//Rising Edge
- return ((FunctionalState)(((LPC_GPIOINT->IO0IntStatR)>>pinNum)& 0x1));
- else if ((portNum == 2) && (edgeState == 0))
- return ((FunctionalState)(((LPC_GPIOINT->IO2IntStatR)>>pinNum)& 0x1));
- else if ((portNum == 0) && (edgeState == 1))//Falling Edge
- return ((FunctionalState)(((LPC_GPIOINT->IO0IntStatF)>>pinNum)& 0x1));
- else if ((portNum == 2) && (edgeState == 1))
- return ((FunctionalState)(((LPC_GPIOINT->IO2IntStatF)>>pinNum)& 0x1));
- else
- //Error
- while(1);
-}
-/*********************************************************************//**
- * @brief Clear GPIO interrupt (just used for P0.0-P0.30, P2.0-P2.13)
- * @param[in] portNum Port number to read value, should be: 0 or 2
- * @param[in] bitValue Value that contains all bits on GPIO to enable,
- * in range from 0 to 0xFFFFFFFF.
- * @return None
- **********************************************************************/
-void GPIO_ClearInt(uint8_t portNum, uint32_t bitValue)
-{
- if(portNum == 0)
- LPC_GPIOINT->IO0IntClr = bitValue;
- else if (portNum == 2)
- LPC_GPIOINT->IO2IntClr = bitValue;
- else
- //Invalid portNum
- while(1);
-}
-
-/* FIO word accessible ----------------------------------------------------------------- */
-/* Stub function for FIO (word-accessible) style */
-
-/**
- * @brief The same with GPIO_SetDir()
- */
-void FIO_SetDir(uint8_t portNum, uint32_t bitValue, uint8_t dir)
-{
- GPIO_SetDir(portNum, bitValue, dir);
-}
-
-/**
- * @brief The same with GPIO_SetValue()
- */
-void FIO_SetValue(uint8_t portNum, uint32_t bitValue)
-{
- GPIO_SetValue(portNum, bitValue);
-}
-
-/**
- * @brief The same with GPIO_ClearValue()
- */
-void FIO_ClearValue(uint8_t portNum, uint32_t bitValue)
-{
- GPIO_ClearValue(portNum, bitValue);
-}
-
-/**
- * @brief The same with GPIO_ReadValue()
- */
-uint32_t FIO_ReadValue(uint8_t portNum)
-{
- return (GPIO_ReadValue(portNum));
-}
-
-/**
- * @brief The same with GPIO_IntCmd()
- */
-void FIO_IntCmd(uint8_t portNum, uint32_t bitValue, uint8_t edgeState)
-{
- GPIO_IntCmd(portNum, bitValue, edgeState);
-}
-
-/**
- * @brief The same with GPIO_GetIntStatus()
- */
-FunctionalState FIO_GetIntStatus(uint8_t portNum, uint32_t pinNum, uint8_t edgeState)
-{
- return (GPIO_GetIntStatus(portNum, pinNum, edgeState));
-}
-
-/**
- * @brief The same with GPIO_ClearInt()
- */
-void FIO_ClearInt(uint8_t portNum, uint32_t bitValue)
-{
- GPIO_ClearInt(portNum, bitValue);
-}
-/*********************************************************************//**
- * @brief Set mask value for bits in FIO port
- * @param[in] portNum Port number, in range from 0 to 4
- * @param[in] bitValue Value that contains all bits in to set,
- * in range from 0 to 0xFFFFFFFF.
- * @param[in] maskValue Mask value contains state value for each bit:
- * - 0: not mask.
- * - 1: mask.
- * @return None
- *
- * Note:
- * - All remaining bits that are not activated in bitValue (value '0')
- * will not be effected by this function.
- * - After executing this function, in mask register, value '0' on each bit
- * enables an access to the corresponding physical pin via a read or write access,
- * while value '1' on bit (masked) that corresponding pin will not be changed
- * with write access and if read, will not be reflected in the updated pin.
- **********************************************************************/
-void FIO_SetMask(uint8_t portNum, uint32_t bitValue, uint8_t maskValue)
-{
- LPC_GPIO_TypeDef *pFIO = GPIO_GetPointer(portNum);
- if(pFIO != NULL) {
- // Mask
- if (maskValue){
- pFIO->FIOMASK |= bitValue;
- }
- // Un-mask
- else {
- pFIO->FIOMASK &= ~bitValue;
- }
- }
-}
-
-
-/* FIO halfword accessible ------------------------------------------------------------- */
-
-/*********************************************************************//**
- * @brief Set direction for FIO port in halfword accessible style
- * @param[in] portNum Port number, in range from 0 to 4
- * @param[in] halfwordNum HalfWord part number, should be 0 (lower) or 1(upper)
- * @param[in] bitValue Value that contains all bits in to set direction,
- * in range from 0 to 0xFFFF.
- * @param[in] dir Direction value, should be:
- * - 0: Input.
- * - 1: Output.
- * @return None
- *
- * Note: All remaining bits that are not activated in bitValue (value '0')
- * will not be effected by this function.
- **********************************************************************/
-void FIO_HalfWordSetDir(uint8_t portNum, uint8_t halfwordNum, uint16_t bitValue, uint8_t dir)
-{
- GPIO_HalfWord_TypeDef *pFIO = FIO_HalfWordGetPointer(portNum);
- if(pFIO != NULL) {
- // Output direction
- if (dir) {
- // Upper
- if(halfwordNum) {
- pFIO->FIODIRU |= bitValue;
- }
- // lower
- else {
- pFIO->FIODIRL |= bitValue;
- }
- }
- // Input direction
- else {
- // Upper
- if(halfwordNum) {
- pFIO->FIODIRU &= ~bitValue;
- }
- // lower
- else {
- pFIO->FIODIRL &= ~bitValue;
- }
- }
- }
-}
-
-
-/*********************************************************************//**
- * @brief Set mask value for bits in FIO port in halfword accessible style
- * @param[in] portNum Port number, in range from 0 to 4
- * @param[in] halfwordNum HalfWord part number, should be 0 (lower) or 1(upper)
- * @param[in] bitValue Value that contains all bits in to set,
- * in range from 0 to 0xFFFF.
- * @param[in] maskValue Mask value contains state value for each bit:
- * - 0: not mask.
- * - 1: mask.
- * @return None
- *
- * Note:
- * - All remaining bits that are not activated in bitValue (value '0')
- * will not be effected by this function.
- * - After executing this function, in mask register, value '0' on each bit
- * enables an access to the corresponding physical pin via a read or write access,
- * while value '1' on bit (masked) that corresponding pin will not be changed
- * with write access and if read, will not be reflected in the updated pin.
- **********************************************************************/
-void FIO_HalfWordSetMask(uint8_t portNum, uint8_t halfwordNum, uint16_t bitValue, uint8_t maskValue)
-{
- GPIO_HalfWord_TypeDef *pFIO = FIO_HalfWordGetPointer(portNum);
- if(pFIO != NULL) {
- // Mask
- if (maskValue){
- // Upper
- if(halfwordNum) {
- pFIO->FIOMASKU |= bitValue;
- }
- // lower
- else {
- pFIO->FIOMASKL |= bitValue;
- }
- }
- // Un-mask
- else {
- // Upper
- if(halfwordNum) {
- pFIO->FIOMASKU &= ~bitValue;
- }
- // lower
- else {
- pFIO->FIOMASKL &= ~bitValue;
- }
- }
- }
-}
-
-
-/*********************************************************************//**
- * @brief Set bits for FIO port in halfword accessible style
- * @param[in] portNum Port number, in range from 0 to 4
- * @param[in] halfwordNum HalfWord part number, should be 0 (lower) or 1(upper)
- * @param[in] bitValue Value that contains all bits in to set,
- * in range from 0 to 0xFFFF.
- * @return None
- *
- * Note:
- * - For all bits that has been set as input direction, this function will
- * not effect.
- * - For all remaining bits that are not activated in bitValue (value '0')
- * will not be effected by this function.
- **********************************************************************/
-void FIO_HalfWordSetValue(uint8_t portNum, uint8_t halfwordNum, uint16_t bitValue)
-{
- GPIO_HalfWord_TypeDef *pFIO = FIO_HalfWordGetPointer(portNum);
- if(pFIO != NULL) {
- // Upper
- if(halfwordNum) {
- pFIO->FIOSETU = bitValue;
- }
- // lower
- else {
- pFIO->FIOSETL = bitValue;
- }
- }
-}
-
-
-/*********************************************************************//**
- * @brief Clear bits for FIO port in halfword accessible style
- * @param[in] portNum Port number, in range from 0 to 4
- * @param[in] halfwordNum HalfWord part number, should be 0 (lower) or 1(upper)
- * @param[in] bitValue Value that contains all bits in to clear,
- * in range from 0 to 0xFFFF.
- * @return None
- *
- * Note:
- * - For all bits that has been set as input direction, this function will
- * not effect.
- * - For all remaining bits that are not activated in bitValue (value '0')
- * will not be effected by this function.
- **********************************************************************/
-void FIO_HalfWordClearValue(uint8_t portNum, uint8_t halfwordNum, uint16_t bitValue)
-{
- GPIO_HalfWord_TypeDef *pFIO = FIO_HalfWordGetPointer(portNum);
- if(pFIO != NULL) {
- // Upper
- if(halfwordNum) {
- pFIO->FIOCLRU = bitValue;
- }
- // lower
- else {
- pFIO->FIOCLRL = bitValue;
- }
- }
-}
-
-
-/*********************************************************************//**
- * @brief Read Current state on port pin that have input direction of GPIO
- * in halfword accessible style.
- * @param[in] portNum Port number, in range from 0 to 4
- * @param[in] halfwordNum HalfWord part number, should be 0 (lower) or 1(upper)
- * @return Current value of FIO port pin of specified halfword.
- * Note: Return value contain state of each port pin (bit) on that FIO regardless
- * its direction is input or output.
- **********************************************************************/
-uint16_t FIO_HalfWordReadValue(uint8_t portNum, uint8_t halfwordNum)
-{
- GPIO_HalfWord_TypeDef *pFIO = FIO_HalfWordGetPointer(portNum);
- if(pFIO != NULL) {
- // Upper
- if(halfwordNum) {
- return (pFIO->FIOPINU);
- }
- // lower
- else {
- return (pFIO->FIOPINL);
- }
- }
- return (0);
-}
-
-
-/* FIO Byte accessible ------------------------------------------------------------ */
-
-/*********************************************************************//**
- * @brief Set direction for FIO port in byte accessible style
- * @param[in] portNum Port number, in range from 0 to 4
- * @param[in] byteNum Byte part number, should be in range from 0 to 3
- * @param[in] bitValue Value that contains all bits in to set direction,
- * in range from 0 to 0xFF.
- * @param[in] dir Direction value, should be:
- * - 0: Input.
- * - 1: Output.
- * @return None
- *
- * Note: All remaining bits that are not activated in bitValue (value '0')
- * will not be effected by this function.
- **********************************************************************/
-void FIO_ByteSetDir(uint8_t portNum, uint8_t byteNum, uint8_t bitValue, uint8_t dir)
-{
- GPIO_Byte_TypeDef *pFIO = FIO_ByteGetPointer(portNum);
- if(pFIO != NULL) {
- // Output direction
- if (dir) {
- if (byteNum <= 3) {
- pFIO->FIODIR[byteNum] |= bitValue;
- }
- }
- // Input direction
- else {
- if (byteNum <= 3) {
- pFIO->FIODIR[byteNum] &= ~bitValue;
- }
- }
- }
-}
-
-/*********************************************************************//**
- * @brief Set mask value for bits in FIO port in byte accessible style
- * @param[in] portNum Port number, in range from 0 to 4
- * @param[in] byteNum Byte part number, should be in range from 0 to 3
- * @param[in] bitValue Value that contains all bits in to set mask,
- * in range from 0 to 0xFF.
- * @param[in] maskValue Mask value contains state value for each bit:
- * - 0: not mask.
- * - 1: mask.
- * @return None
- *
- * Note:
- * - All remaining bits that are not activated in bitValue (value '0')
- * will not be effected by this function.
- * - After executing this function, in mask register, value '0' on each bit
- * enables an access to the corresponding physical pin via a read or write access,
- * while value '1' on bit (masked) that corresponding pin will not be changed
- * with write access and if read, will not be reflected in the updated pin.
- **********************************************************************/
-void FIO_ByteSetMask(uint8_t portNum, uint8_t byteNum, uint8_t bitValue, uint8_t maskValue)
-{
- GPIO_Byte_TypeDef *pFIO = FIO_ByteGetPointer(portNum);
- if(pFIO != NULL) {
- // Mask
- if (maskValue) {
- if (byteNum <= 3) {
- pFIO->FIOMASK[byteNum] |= bitValue;
- }
- }
- // Un-mask
- else {
- if (byteNum <= 3) {
- pFIO->FIOMASK[byteNum] &= ~bitValue;
- }
- }
- }
-}
-
-
-/*********************************************************************//**
- * @brief Set bits for FIO port in byte accessible style
- * @param[in] portNum Port number, in range from 0 to 4
- * @param[in] byteNum Byte part number, should be in range from 0 to 3
- * @param[in] bitValue Value that contains all bits in to set,
- * in range from 0 to 0xFF.
- * @return None
- *
- * Note:
- * - For all bits that has been set as input direction, this function will
- * not effect.
- * - For all remaining bits that are not activated in bitValue (value '0')
- * will not be effected by this function.
- **********************************************************************/
-void FIO_ByteSetValue(uint8_t portNum, uint8_t byteNum, uint8_t bitValue)
-{
- GPIO_Byte_TypeDef *pFIO = FIO_ByteGetPointer(portNum);
- if (pFIO != NULL) {
- if (byteNum <= 3){
- pFIO->FIOSET[byteNum] = bitValue;
- }
- }
-}
-
-
-/*********************************************************************//**
- * @brief Clear bits for FIO port in byte accessible style
- * @param[in] portNum Port number, in range from 0 to 4
- * @param[in] byteNum Byte part number, should be in range from 0 to 3
- * @param[in] bitValue Value that contains all bits in to clear,
- * in range from 0 to 0xFF.
- * @return None
- *
- * Note:
- * - For all bits that has been set as input direction, this function will
- * not effect.
- * - For all remaining bits that are not activated in bitValue (value '0')
- * will not be effected by this function.
- **********************************************************************/
-void FIO_ByteClearValue(uint8_t portNum, uint8_t byteNum, uint8_t bitValue)
-{
- GPIO_Byte_TypeDef *pFIO = FIO_ByteGetPointer(portNum);
- if (pFIO != NULL) {
- if (byteNum <= 3){
- pFIO->FIOCLR[byteNum] = bitValue;
- }
- }
-}
-
-
-/*********************************************************************//**
- * @brief Read Current state on port pin that have input direction of GPIO
- * in byte accessible style.
- * @param[in] portNum Port number, in range from 0 to 4
- * @param[in] byteNum Byte part number, should be in range from 0 to 3
- * @return Current value of FIO port pin of specified byte part.
- * Note: Return value contain state of each port pin (bit) on that FIO regardless
- * its direction is input or output.
- **********************************************************************/
-uint8_t FIO_ByteReadValue(uint8_t portNum, uint8_t byteNum)
-{
- GPIO_Byte_TypeDef *pFIO = FIO_ByteGetPointer(portNum);
- if (pFIO != NULL) {
- if (byteNum <= 3){
- return (pFIO->FIOPIN[byteNum]);
- }
- }
- return (0);
-}
-
-/**
- * @}
- */
-
-#endif /* _GPIO */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_i2c.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_i2c.c
deleted file mode 100644
index 99b29e017c..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_i2c.c
+++ /dev/null
@@ -1,1344 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_i2c.c 2011-03-31
-*//**
-* @file lpc17xx_i2c.c
-* @brief Contains all functions support for I2C firmware
-* library on LPC17xx
-* @version 2.1
-* @date 31. Mar. 2011
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2010, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup I2C
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_i2c.h"
-#include "lpc17xx_clkpwr.h"
-#include "lpc17xx_pinsel.h"
-
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-
-#ifdef _I2C
-
-
-/* Private Types -------------------------------------------------------------- */
-/** @defgroup I2C_Private_Types I2C Private Types
- * @{
- */
-
-/**
- * @brief I2C device configuration structure type
- */
-typedef struct
-{
- uint32_t txrx_setup; /* Transmission setup */
- int32_t dir; /* Current direction phase, 0 - write, 1 - read */
-} I2C_CFG_T;
-
-/**
- * @}
- */
-
-/* Private Variables ---------------------------------------------------------- */
-/**
- * @brief II2C driver data for I2C0, I2C1 and I2C2
- */
-static I2C_CFG_T i2cdat[3];
-
-static uint32_t I2C_MasterComplete[3];
-static uint32_t I2C_SlaveComplete[3];
-
-static uint32_t I2C_MonitorBufferIndex;
-
-/* Private Functions ---------------------------------------------------------- */
-
-/* Get I2C number */
-static int32_t I2C_getNum(LPC_I2C_TypeDef *I2Cx);
-
-/* Generate a start condition on I2C bus (in master mode only) */
-static uint32_t I2C_Start (LPC_I2C_TypeDef *I2Cx);
-
-/* Generate a stop condition on I2C bus (in master mode only) */
-static void I2C_Stop (LPC_I2C_TypeDef *I2Cx);
-
-/* I2C send byte subroutine */
-static uint32_t I2C_SendByte (LPC_I2C_TypeDef *I2Cx, uint8_t databyte);
-
-/* I2C get byte subroutine */
-static uint32_t I2C_GetByte (LPC_I2C_TypeDef *I2Cx, uint8_t *retdat, Bool ack);
-
-/* I2C set clock (hz) */
-static void I2C_SetClock (LPC_I2C_TypeDef *I2Cx, uint32_t target_clock);
-
-/*--------------------------------------------------------------------------------*/
-/********************************************************************//**
- * @brief Convert from I2C peripheral to number
- * @param[in] I2Cx: I2C peripheral selected, should be:
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @return I2C number, could be: 0..2
- *********************************************************************/
-static int32_t I2C_getNum(LPC_I2C_TypeDef *I2Cx){
- if (I2Cx == LPC_I2C0) {
- return (0);
- } else if (I2Cx == LPC_I2C1) {
- return (1);
- } else if (I2Cx == LPC_I2C2) {
- return (2);
- }
- return (-1);
-}
-
-/********************************************************************//**
- * @brief Generate a start condition on I2C bus (in master mode only)
- * @param[in] I2Cx: I2C peripheral selected, should be:
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @return value of I2C status register after generate a start condition
- *********************************************************************/
-static uint32_t I2C_Start (LPC_I2C_TypeDef *I2Cx)
-{
- // Reset STA, STO, SI
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC|I2C_I2CONCLR_STOC|I2C_I2CONCLR_STAC;
-
- // Enter to Master Transmitter mode
- I2Cx->I2CONSET = I2C_I2CONSET_STA;
-
- // Wait for complete
- while (!(I2Cx->I2CONSET & I2C_I2CONSET_SI));
- I2Cx->I2CONCLR = I2C_I2CONCLR_STAC;
- return (I2Cx->I2STAT & I2C_STAT_CODE_BITMASK);
-}
-
-/********************************************************************//**
- * @brief Generate a stop condition on I2C bus (in master mode only)
- * @param[in] I2Cx: I2C peripheral selected, should be:
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @return None
- *********************************************************************/
-static void I2C_Stop (LPC_I2C_TypeDef *I2Cx)
-{
-
- /* Make sure start bit is not active */
- if (I2Cx->I2CONSET & I2C_I2CONSET_STA)
- {
- I2Cx->I2CONCLR = I2C_I2CONCLR_STAC;
- }
-
- I2Cx->I2CONSET = I2C_I2CONSET_STO|I2C_I2CONSET_AA;
-
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC;
-}
-
-/********************************************************************//**
- * @brief Send a byte
- * @param[in] I2Cx: I2C peripheral selected, should be:
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @param[in] databyte: number of byte
- * @return value of I2C status register after sending
- *********************************************************************/
-static uint32_t I2C_SendByte (LPC_I2C_TypeDef *I2Cx, uint8_t databyte)
-{
- uint32_t CodeStatus = I2Cx->I2STAT & I2C_STAT_CODE_BITMASK;
-
- if((CodeStatus != I2C_I2STAT_M_TX_START) &&
- (CodeStatus != I2C_I2STAT_M_TX_RESTART) &&
- (CodeStatus != I2C_I2STAT_M_TX_SLAW_ACK) &&
- (CodeStatus != I2C_I2STAT_M_TX_DAT_ACK) )
- {
- return CodeStatus;
- }
-
- /* Make sure start bit is not active */
- if (I2Cx->I2CONSET & I2C_I2CONSET_STA)
- {
- I2Cx->I2CONCLR = I2C_I2CONCLR_STAC;
- }
- I2Cx->I2DAT = databyte & I2C_I2DAT_BITMASK;
-
- I2Cx->I2CONSET = I2C_I2CONSET_AA;
-
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC;
-
- return (I2Cx->I2STAT & I2C_STAT_CODE_BITMASK);
-}
-
-/********************************************************************//**
- * @brief Get a byte
- * @param[in] I2Cx: I2C peripheral selected, should be:
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @param[out] retdat pointer to return data
- * @param[in] ack assert acknowledge or not, should be: TRUE/FALSE
- * @return value of I2C status register after sending
- *********************************************************************/
-static uint32_t I2C_GetByte (LPC_I2C_TypeDef *I2Cx, uint8_t *retdat, Bool ack)
-{
- *retdat = (uint8_t) (I2Cx->I2DAT & I2C_I2DAT_BITMASK);
-
- if (ack == TRUE)
- {
- I2Cx->I2CONSET = I2C_I2CONSET_AA;
- }
- else
- {
- I2Cx->I2CONCLR = I2C_I2CONCLR_AAC;
- }
-
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC;
-
- return (I2Cx->I2STAT & I2C_STAT_CODE_BITMASK);
-}
-
-/*********************************************************************//**
- * @brief Setup clock rate for I2C peripheral
- * @param[in] I2Cx I2C peripheral selected, should be:
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @param[in] target_clock : clock of SSP (Hz)
- * @return None
- ***********************************************************************/
-static void I2C_SetClock (LPC_I2C_TypeDef *I2Cx, uint32_t target_clock)
-{
- uint32_t temp;
-
- CHECK_PARAM(PARAM_I2Cx(I2Cx));
-
- // Get PCLK of I2C controller
- if (I2Cx == LPC_I2C0)
- {
- temp = CLKPWR_GetPCLK (CLKPWR_PCLKSEL_I2C0) / target_clock;
- }
- else if (I2Cx == LPC_I2C1)
- {
- temp = CLKPWR_GetPCLK (CLKPWR_PCLKSEL_I2C1) / target_clock;
- }
- else if (I2Cx == LPC_I2C2)
- {
- temp = CLKPWR_GetPCLK (CLKPWR_PCLKSEL_I2C2) / target_clock;
- }
-
- /* Set the I2C clock value to register */
- I2Cx->I2SCLH = (uint32_t)(temp / 2);
- I2Cx->I2SCLL = (uint32_t)(temp - I2Cx->I2SCLH);
-}
-/* End of Private Functions --------------------------------------------------- */
-
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup I2C_Public_Functions
- * @{
- */
-
-/********************************************************************//**
- * @brief Initializes the I2Cx peripheral with specified parameter.
- * @param[in] I2Cx I2C peripheral selected, should be
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @param[in] clockrate Target clock rate value to initialized I2C
- * peripheral (Hz)
- * @return None
- *********************************************************************/
-void I2C_Init(LPC_I2C_TypeDef *I2Cx, uint32_t clockrate)
-{
- CHECK_PARAM(PARAM_I2Cx(I2Cx));
-
- if (I2Cx==LPC_I2C0)
- {
- /* Set up clock and power for I2C0 module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCI2C0, ENABLE);
- /* As default, peripheral clock for I2C0 module
- * is set to FCCLK / 2 */
- CLKPWR_SetPCLKDiv(CLKPWR_PCLKSEL_I2C0, CLKPWR_PCLKSEL_CCLK_DIV_2);
- }
- else if (I2Cx==LPC_I2C1)
- {
- /* Set up clock and power for I2C1 module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCI2C1, ENABLE);
- /* As default, peripheral clock for I2C1 module
- * is set to FCCLK / 2 */
- CLKPWR_SetPCLKDiv(CLKPWR_PCLKSEL_I2C1, CLKPWR_PCLKSEL_CCLK_DIV_2);
- }
- else if (I2Cx==LPC_I2C2)
- {
- /* Set up clock and power for I2C2 module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCI2C2, ENABLE);
- /* As default, peripheral clock for I2C2 module
- * is set to FCCLK / 2 */
- CLKPWR_SetPCLKDiv(CLKPWR_PCLKSEL_I2C2, CLKPWR_PCLKSEL_CCLK_DIV_2);
- }
- else {
- // Up-Support this device
- return;
- }
-
- /* Set clock rate */
- I2C_SetClock(I2Cx, clockrate);
- /* Set I2C operation to default */
- I2Cx->I2CONCLR = (I2C_I2CONCLR_AAC | I2C_I2CONCLR_STAC | I2C_I2CONCLR_I2ENC);
-}
-
-/*********************************************************************//**
- * @brief De-initializes the I2C peripheral registers to their
- * default reset values.
- * @param[in] I2Cx I2C peripheral selected, should be
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @return None
- **********************************************************************/
-void I2C_DeInit(LPC_I2C_TypeDef* I2Cx)
-{
- CHECK_PARAM(PARAM_I2Cx(I2Cx));
-
- /* Disable I2C control */
- I2Cx->I2CONCLR = I2C_I2CONCLR_I2ENC;
-
- if (I2Cx==LPC_I2C0)
- {
- /* Disable power for I2C0 module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCI2C0, DISABLE);
- }
- else if (I2Cx==LPC_I2C1)
- {
- /* Disable power for I2C1 module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCI2C1, DISABLE);
- }
- else if (I2Cx==LPC_I2C2)
- {
- /* Disable power for I2C2 module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCI2C2, DISABLE);
- }
-}
-
-/*********************************************************************//**
- * @brief Enable or disable I2C peripheral's operation
- * @param[in] I2Cx I2C peripheral selected, should be
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @param[in] NewState New State of I2Cx peripheral's operation
- * @return none
- **********************************************************************/
-void I2C_Cmd(LPC_I2C_TypeDef* I2Cx, en_I2C_Mode Mode, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
- CHECK_PARAM(PARAM_I2Cx(I2Cx));
-
- if (NewState == ENABLE)
- {
- if(Mode != I2C_SLAVE_MODE)
- I2Cx->I2CONSET = I2C_I2CONSET_I2EN;
- else
- I2Cx->I2CONSET = I2C_I2CONSET_I2EN | I2C_I2CONSET_AA;
- }
- else
- {
- I2Cx->I2CONCLR = I2C_I2CONCLR_I2ENC;
- }
-}
-
-/*********************************************************************//**
- * @brief Enable/Disable interrupt for I2C peripheral
- * @param[in] I2Cx I2C peripheral selected, should be:
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @param[in] NewState New State of I2C peripheral interrupt in NVIC core
- * should be:
- * - ENABLE: enable interrupt for this I2C peripheral
- * - DISABLE: disable interrupt for this I2C peripheral
- * @return None
- **********************************************************************/
-void I2C_IntCmd (LPC_I2C_TypeDef *I2Cx, Bool NewState)
-{
- if (NewState)
- {
- if(I2Cx == LPC_I2C0)
- {
- NVIC_EnableIRQ(I2C0_IRQn);
- }
- else if (I2Cx == LPC_I2C1)
- {
- NVIC_EnableIRQ(I2C1_IRQn);
- }
- else if (I2Cx == LPC_I2C2)
- {
- NVIC_EnableIRQ(I2C2_IRQn);
- }
- }
- else
- {
- if(I2Cx == LPC_I2C0)
- {
- NVIC_DisableIRQ(I2C0_IRQn);
- }
- else if (I2Cx == LPC_I2C1)
- {
- NVIC_DisableIRQ(I2C1_IRQn);
- }
- else if (I2Cx == LPC_I2C2)
- {
- NVIC_DisableIRQ(I2C2_IRQn);
- }
- }
- return;
-}
-
-
-/*********************************************************************//**
- * @brief Handle I2C Master states.
- * @param[in] I2Cx I2C peripheral selected, should be:
- * - LPC_I2C
- * - LPC_I2C1
- * - LPC_I2C2
- * @param[in] CodeStatus I2C state
- * @param[in] TransferCfg Pointer to a I2C_S_SETUP_Type structure that
- * contains specified information about the
- * configuration for master transfer.
- * @return It can be
- * - I2C_OK
- * -I2C_BYTE_RECV
- * -I2C_BYTE_SENT
- * -I2C_SEND_END
- * -I2C_RECV_END
- * - I2C_ERR
- * - I2C_NAK_RECV
- **********************************************************************/
-int32_t I2C_MasterHanleStates(LPC_I2C_TypeDef *I2Cx, uint32_t CodeStatus, I2C_M_SETUP_Type *TransferCfg)
-{
- uint8_t *txdat;
- uint8_t *rxdat;
- uint8_t tmp;
- int32_t Ret = I2C_OK;
-
- //get buffer to send/receive
- txdat = (uint8_t *) &TransferCfg->tx_data[TransferCfg->tx_count];
- rxdat = (uint8_t *) &TransferCfg->rx_data[TransferCfg->rx_count];
-
- switch(CodeStatus)
- {
- case I2C_I2STAT_M_TX_START:
- case I2C_I2STAT_M_TX_RESTART:
- //case I2C_I2STAT_M_RX_START:
- //case I2C_I2STAT_M_RX_RESTART
- // Send data first
- if(TransferCfg->tx_count < TransferCfg->tx_length)
- {
- /* Send slave address + WR direction bit = 0 ----------------------------------- */
- I2C_SendByte(I2Cx, (TransferCfg->sl_addr7bit << 1));
- Ret = I2C_BYTE_SENT;
- }
- else if (TransferCfg->rx_count < TransferCfg->rx_length)
- {
- /* Send slave address + RD direction bit = 1 ----------------------------------- */
- I2C_SendByte(I2Cx, ((TransferCfg->sl_addr7bit << 1) | 0x01));
- Ret = I2C_BYTE_SENT;
- }
- break;
- case I2C_I2STAT_M_TX_SLAW_ACK:
- case I2C_I2STAT_M_TX_DAT_ACK:
-
- if(TransferCfg->tx_count < TransferCfg->tx_length)
- {
- I2C_SendByte(I2Cx, *txdat);
-
- txdat++;
-
- TransferCfg->tx_count++;
-
- Ret = I2C_BYTE_SENT;
- }
- else
- {
- I2C_Stop(I2Cx);
-
- Ret = I2C_SEND_END;
-
- }
- break;
- case I2C_I2STAT_M_TX_DAT_NACK:
- I2C_Stop(I2Cx);
- Ret = I2C_SEND_END;
- break;
- case I2C_I2STAT_M_RX_ARB_LOST:
- //case I2C_I2STAT_M_TX_ARB_LOST:
- I2Cx->I2CONSET = I2C_I2CONSET_STA|I2C_I2CONSET_AA;
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC;
- break;
- case I2C_I2STAT_M_RX_SLAR_ACK:
- I2Cx->I2CONSET = I2C_I2CONSET_AA;
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC;
-
- Ret = I2C_BYTE_RECV;
- break;
- case I2C_I2STAT_M_RX_DAT_ACK:
- if (TransferCfg->rx_count rx_length)
- {
- if (TransferCfg->rx_count < (TransferCfg->rx_length - 2))
- {
- I2C_GetByte(I2Cx, &tmp, TRUE);
-
- Ret = I2C_BYTE_RECV;
- }
- else // the next byte is the last byte, send NACK instead.
- {
- I2C_GetByte(I2Cx, &tmp, FALSE);
- Ret = I2C_BYTE_RECV;
- }
- *rxdat++ = tmp;
-
- TransferCfg->rx_count++;
- }
- else
- {
- Ret = I2C_RECV_END;
- }
-
- break;
- case I2C_I2STAT_M_RX_DAT_NACK:
- I2C_GetByte(I2Cx, &tmp, FALSE);
- *rxdat++ = tmp;
- TransferCfg->rx_count++;
- I2C_Stop(I2Cx);
- Ret = I2C_RECV_END;
- break;
- case I2C_I2STAT_M_RX_SLAR_NACK:
- case I2C_I2STAT_M_TX_SLAW_NACK:
- case I2C_I2STAT_BUS_ERROR:
- // Send STOP condition
- I2C_Stop(I2Cx);
- Ret = I2C_ERR;
- break;
- /* No status information */
- case I2C_I2STAT_NO_INF:
- default:
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC;
- break;
- }
-
- return Ret;
-}
-
-/*********************************************************************//**
- * @brief Handle I2C Slave states.
- * @param[in] I2Cx I2C peripheral selected, should be:
- * - LPC_I2C
- * - LPC_I2C1
- * - LPC_I2C2
- * @param[in] CodeStatus I2C state
- * @param[in] TransferCfg Pointer to a I2C_S_SETUP_Type structure that
- * contains specified information about the
- * configuration for master transfer.
- * @return It can be
- * - I2C_OK
- * -I2C_BYTE_RECV
- * -I2C_BYTE_SENT
- * -I2C_SEND_END
- * -I2C_RECV_END
- * - I2C_ERR
- * - I2C_NAK_RECV
- **********************************************************************/
-int32_t I2C_SlaveHanleStates(LPC_I2C_TypeDef *I2Cx, uint32_t CodeStatus, I2C_S_SETUP_Type *TransferCfg)
-{
-
- int32_t Ret = I2C_OK;
- uint8_t *txdat;
- uint8_t *rxdat;
-
- //get buffer to send/receive
- txdat = (uint8_t *) &TransferCfg->tx_data[TransferCfg->tx_count];
- rxdat = (uint8_t *) &TransferCfg->rx_data[TransferCfg->rx_count];
-
- switch (CodeStatus)
- {
- /* Reading phase -------------------------------------------------------- */
- /* Own SLA+R has been received, ACK has been returned */
- case I2C_I2STAT_S_RX_SLAW_ACK:
-
- /* General call address has been received, ACK has been returned */
- case I2C_I2STAT_S_RX_GENCALL_ACK:
- I2Cx->I2CONSET = I2C_I2CONSET_AA;
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC;
- break;
- /* Arbitration has been lost in Slave Address + R/W bit as bus Master. General Call has
- been received and ACK has been returned.*/
- case I2C_I2STAT_S_RX_ARB_LOST_M_GENCALL:
- I2Cx->I2CONSET = I2C_I2CONSET_AA|I2C_I2CONSET_STA;
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC;
- break;
- /* Previously addressed with own SLA;
- * DATA byte has been received;
- * ACK has been returned */
- case I2C_I2STAT_S_RX_ARB_LOST_M_SLA:
- case I2C_I2STAT_S_RX_PRE_SLA_DAT_ACK:
-
- /*
- * All data bytes that over-flow the specified receive
- * data length, just ignore them.
- */
- if ((TransferCfg->rx_count < TransferCfg->rx_length) && (TransferCfg->rx_data != NULL))
- {
- *rxdat++ = (uint8_t)I2Cx->I2DAT;
-
- TransferCfg->rx_count++;
-
- Ret = I2C_BYTE_RECV;
- }
- if(TransferCfg->rx_count == (TransferCfg->rx_length) ) {
- I2Cx->I2CONCLR = I2C_I2CONCLR_AAC|I2C_I2CONCLR_SIC;
- Ret = I2C_BYTE_RECV;
- }
- else {
- I2Cx->I2CONSET = I2C_I2CONSET_AA;
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC;
- }
-
- break;
- /* DATA has been received, Only the first data byte will be received with ACK. Additional
- data will be received with NOT ACK. */
- case I2C_I2STAT_S_RX_PRE_GENCALL_DAT_ACK:
- if ((TransferCfg->rx_count < TransferCfg->rx_length) && (TransferCfg->rx_data != NULL))
- {
- *rxdat++ = (uint8_t)I2Cx->I2DAT;
-
- TransferCfg->rx_count++;
-
- Ret = I2C_BYTE_RECV;
- }
- I2Cx->I2CONCLR = I2C_I2CONCLR_AAC|I2C_I2CONCLR_SIC;
- break;
-
- /* Writing phase -------------------------------------------------------- */
- /* Own SLA+R has been received, ACK has been returned */
- case I2C_I2STAT_S_TX_SLAR_ACK:
-
- /* Data has been transmitted, ACK has been received */
- case I2C_I2STAT_S_TX_DAT_ACK:
- /*
- * All data bytes that over-flow the specified receive
- * data length, just ignore them.
- */
- if ((TransferCfg->tx_count < TransferCfg->tx_length) && (TransferCfg->tx_data != NULL))
- {
- I2Cx->I2DAT = *txdat++;
-
- TransferCfg->tx_count++;
-
- Ret = I2C_BYTE_SENT;
- }
-
- I2Cx->I2CONSET = I2C_I2CONSET_AA;
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC;
- break;
- /* Arbitration lost in Slave Address and R/W bit as bus Master. Own Slave Address + Read
- has been received, ACK has been returned. */
- case I2C_I2STAT_S_TX_ARB_LOST_M_SLA:
- if ((TransferCfg->tx_count < TransferCfg->tx_length) && (TransferCfg->tx_data != NULL))
- {
- I2Cx->I2DAT = *txdat++;
-
- TransferCfg->tx_count++;
-
- Ret = I2C_BYTE_SENT;
- }
- I2Cx->I2CONSET = I2C_I2CONSET_AA|I2C_I2CONSET_STA;
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC;
- break;
-
- case I2C_I2STAT_S_TX_LAST_DAT_ACK:
- /* Data has been transmitted, NACK has been received,
- * that means there's no more data to send, exit now */
- /*
- * Note: Don't wait for stop event since in slave transmit mode,
- * since there no proof lets us know when a stop signal has been received
- * on slave side.
- */
- case I2C_I2STAT_S_TX_DAT_NACK:
- I2Cx->I2CONSET = I2C_I2CONSET_AA;
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC;
- Ret = I2C_SEND_END;
- break;
-
- /* Previously addressed with own SLA;
- * DATA byte has been received;
- * NOT ACK has been returned */
- case I2C_I2STAT_S_RX_PRE_SLA_DAT_NACK:
-
- /* DATA has been received, NOT ACK has been returned */
- case I2C_I2STAT_S_RX_PRE_GENCALL_DAT_NACK:
- I2Cx->I2CONSET = I2C_I2CONSET_AA;
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC;
- Ret = I2C_RECV_END;
- break;
-
- /*
- * Note that: Return code only let us know a stop condition mixed
- * with a repeat start condition in the same code value.
- * So we should provide a time-out. In case this is really a stop
- * condition, this will return back after time out condition. Otherwise,
- * next session that is slave receive data will be completed.
- */
-
- /* A Stop or a repeat start condition */
- case I2C_I2STAT_S_RX_STA_STO_SLVREC_SLVTRX:
- I2Cx->I2CONSET = I2C_I2CONSET_AA;
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC;
- Ret = I2C_STA_STO_RECV;
- break;
-
- /* No status information */
- case I2C_I2STAT_NO_INF:
- /* Other status must be captured */
- default:
- I2Cx->I2CONSET = I2C_I2CONSET_AA;
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC;
- break;
-
- }
-
- return Ret;
-}
-/*********************************************************************//**
- * @brief General Master Interrupt handler for I2C peripheral
- * @param[in] I2Cx I2C peripheral selected, should be:
- * - LPC_I2C
- * - LPC_I2C1
- * - LPC_I2C2
- * @return None
- **********************************************************************/
-void I2C_MasterHandler(LPC_I2C_TypeDef *I2Cx)
-{
- uint32_t i2cId = I2C_getNum(I2Cx);
- uint8_t returnCode;
- I2C_M_SETUP_Type *txrx_setup;
- int32_t Ret = I2C_OK;
-
- txrx_setup = (I2C_M_SETUP_Type *) i2cdat[i2cId].txrx_setup;
-
- returnCode = (I2Cx->I2STAT & I2C_STAT_CODE_BITMASK);
-
- // Save current status
- txrx_setup->status = returnCode;
-
- Ret = I2C_MasterHanleStates(I2Cx, returnCode, txrx_setup);
-
- if(I2C_CheckError(Ret))
- {
- if(txrx_setup->retransmissions_count < txrx_setup->retransmissions_max)
- {
- // Retry
- txrx_setup->retransmissions_count ++;
- txrx_setup->tx_count = 0;
- txrx_setup->rx_count = 0;
- // Reset STA, STO, SI
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC|I2C_I2CONCLR_STOC|I2C_I2CONCLR_STAC;
- I2Cx->I2CONSET = I2C_I2CONSET_STA;
- return;
- }
- else
- {
- goto s_int_end;
- }
- }
- else if (Ret & I2C_SEND_END)
- {
- // If no need to wait for data from Slave
- if(txrx_setup->rx_count >= (txrx_setup->rx_length))
- {
- goto s_int_end;
- }
- else // Start to wait for data from Slave
- {
- // Reset STA, STO, SI
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC|I2C_I2CONCLR_STOC|I2C_I2CONCLR_STAC;
- I2Cx->I2CONSET = I2C_I2CONSET_STA;
- return;
- }
- }
- else if (Ret & I2C_RECV_END)
- {
- goto s_int_end;
- }
- else
- {
- return;
- }
-
-s_int_end:
- // Disable interrupt
- I2C_IntCmd(I2Cx, FALSE);
-
- I2Cx->I2CONCLR = I2C_I2CONCLR_AAC | I2C_I2CONCLR_SIC | I2C_I2CONCLR_STAC;
-
- I2C_MasterComplete[i2cId] = TRUE;
-
-}
-
-
-/*********************************************************************//**
- * @brief General Slave Interrupt handler for I2C peripheral
- * @param[in] I2Cx I2C peripheral selected, should be:
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @return None
- **********************************************************************/
-void I2C_SlaveHandler (LPC_I2C_TypeDef *I2Cx)
-{
- uint32_t i2cId = I2C_getNum(I2Cx);
- uint8_t returnCode;
- I2C_S_SETUP_Type *txrx_setup;
- uint32_t timeout;
- int32_t Ret = I2C_OK;
-
- txrx_setup = (I2C_S_SETUP_Type *) i2cdat[i2cId].txrx_setup;
-
-handle_state:
-
- returnCode = (I2Cx->I2STAT & I2C_STAT_CODE_BITMASK);
- // Save current status
- txrx_setup->status = returnCode;
-
-
- Ret = I2C_SlaveHanleStates(I2Cx, returnCode, txrx_setup);
-
- if(I2C_CheckError(Ret))
- {
- goto s_int_end;
- }
- else if (Ret & I2C_STA_STO_RECV)
- {
- // Temporally lock the interrupt for timeout condition
- I2C_IntCmd(I2Cx, FALSE);
- // enable time out
- timeout = I2C_SLAVE_TIME_OUT;
- while(1)
- {
- if (I2Cx->I2CONSET & I2C_I2CONSET_SI)
- {
- // re-Enable interrupt
- I2C_IntCmd(I2Cx, TRUE);
- goto handle_state;
- }
- else
- {
- timeout--;
- if (timeout == 0)
- {
- // timeout occur, it's really a stop condition
- txrx_setup->status |= I2C_SETUP_STATUS_DONE;
- goto s_int_end;
- }
- }
- }
- }
- else if(Ret &I2C_SEND_END)
- {
- goto s_int_end;
- }
- else
- {
- return;
- }
-
-s_int_end:
- // Disable interrupt
- I2C_IntCmd(I2Cx, FALSE);
- I2Cx->I2CONCLR = I2C_I2CONCLR_AAC | I2C_I2CONCLR_SIC | I2C_I2CONCLR_STAC;
-
- I2C_SlaveComplete[i2cId] = TRUE;
-}
-
-/*********************************************************************//**
- * @brief Transmit and Receive data in master mode
- * @param[in] I2Cx I2C peripheral selected, should be:
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @param[in] TransferCfg Pointer to a I2C_M_SETUP_Type structure that
- * contains specified information about the
- * configuration for master transfer.
- * @param[in] Opt a I2C_TRANSFER_OPT_Type type that selected for
- * interrupt or polling mode.
- * @return SUCCESS or ERROR
- *
- * Note:
- * - In case of using I2C to transmit data only, either transmit length set to 0
- * or transmit data pointer set to NULL.
- * - In case of using I2C to receive data only, either receive length set to 0
- * or receive data pointer set to NULL.
- * - In case of using I2C to transmit followed by receive data, transmit length,
- * transmit data pointer, receive length and receive data pointer should be set
- * corresponding.
- **********************************************************************/
-Status I2C_MasterTransferData(LPC_I2C_TypeDef *I2Cx, I2C_M_SETUP_Type *TransferCfg, \
- I2C_TRANSFER_OPT_Type Opt)
-{
- uint32_t i2cId = I2C_getNum(I2Cx); uint32_t CodeStatus;
- int32_t Ret = I2C_OK;
-
- // Reset I2C setup value to default state
- TransferCfg->tx_count = 0;
- TransferCfg->rx_count = 0;
- TransferCfg->status = 0;
-
- if (Opt == I2C_TRANSFER_POLLING)
- {
- /* First Start condition -------------------------------------------------------------- */
- TransferCfg->retransmissions_count = 0;
-retry:
- // Reset I2C setup value to default state
- TransferCfg->tx_count = 0;
- TransferCfg->rx_count = 0;
-
- // Start command
- CodeStatus = I2C_Start(I2Cx);
-
- while(1) // send data first and then receive data from Slave.
- {
- Ret = I2C_MasterHanleStates(I2Cx, CodeStatus, TransferCfg);
- if(I2C_CheckError(Ret))
- {
- TransferCfg->retransmissions_count++;
- if (TransferCfg->retransmissions_count > TransferCfg->retransmissions_max){
- // save status
- TransferCfg->status = CodeStatus | I2C_SETUP_STATUS_NOACKF;
- goto error;
- } else {
- goto retry;
- }
- }
- else if( (Ret & I2C_BYTE_SENT) ||
- (Ret & I2C_BYTE_RECV))
- {
- // Wait for sending ends
- while (!(I2Cx->I2CONSET & I2C_I2CONSET_SI));
- }
- else if (Ret & I2C_SEND_END) // already send all data
- {
- // If no need to wait for data from Slave
- if(TransferCfg->rx_count >= (TransferCfg->rx_length))
- {
- break;
- }
- else
- {
- I2C_Start(I2Cx);
- }
- }
- else if (Ret & I2C_RECV_END) // already receive all data
- {
- break;
- }
- CodeStatus = I2Cx->I2STAT & I2C_STAT_CODE_BITMASK;
- }
- return SUCCESS;
-error:
- return ERROR;
- }
-
- else if (Opt == I2C_TRANSFER_INTERRUPT)
- {
- // Setup tx_rx data, callback and interrupt handler
- i2cdat[i2cId].txrx_setup = (uint32_t) TransferCfg;
-
- // Set direction phase, write first
- i2cdat[i2cId].dir = 0;
-
- /* First Start condition -------------------------------------------------------------- */
- // Reset STA, STO, SI
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC|I2C_I2CONCLR_STOC|I2C_I2CONCLR_STAC;
- I2Cx->I2CONSET = I2C_I2CONSET_STA;
-
- I2C_IntCmd(I2Cx, TRUE);
-
- return (SUCCESS);
- }
-
- return ERROR;
-}
-
-/*********************************************************************//**
- * @brief Receive and Transmit data in slave mode
- * @param[in] I2Cx I2C peripheral selected, should be
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @param[in] TransferCfg Pointer to a I2C_S_SETUP_Type structure that
- * contains specified information about the
- * configuration for master transfer.
- * @param[in] Opt I2C_TRANSFER_OPT_Type type that selected for
- * interrupt or polling mode.
- * @return SUCCESS or ERROR
- *
- * Note:
- * The mode of slave's operation depends on the command sent from master on
- * the I2C bus. If the master send a SLA+W command, this sub-routine will
- * use receive data length and receive data pointer. If the master send a SLA+R
- * command, this sub-routine will use transmit data length and transmit data
- * pointer.
- * If the master issue an repeat start command or a stop command, the slave will
- * enable an time out condition, during time out condition, if there's no activity
- * on I2C bus, the slave will exit, otherwise (i.e. the master send a SLA+R/W),
- * the slave then switch to relevant operation mode. The time out should be used
- * because the return status code can not show difference from stop and repeat
- * start command in slave operation.
- * In case of the expected data length from master is greater than data length
- * that slave can support:
- * - In case of reading operation (from master): slave will return I2C_I2DAT_IDLE_CHAR
- * value.
- * - In case of writing operation (from master): slave will ignore remain data from master.
- **********************************************************************/
-Status I2C_SlaveTransferData(LPC_I2C_TypeDef *I2Cx, I2C_S_SETUP_Type *TransferCfg, \
- I2C_TRANSFER_OPT_Type Opt)
-{
- int32_t Ret = I2C_OK;
-
- uint32_t CodeStatus;
- uint32_t timeout;
- int32_t time_en;
- uint32_t i2cId = I2C_getNum(I2Cx);
- // Reset I2C setup value to default state
- TransferCfg->tx_count = 0;
- TransferCfg->rx_count = 0;
- TransferCfg->status = 0;
-
- // Polling option
- if (Opt == I2C_TRANSFER_POLLING)
- {
- /* Set AA bit to ACK command on I2C bus */
- I2Cx->I2CONSET = I2C_I2CONSET_AA;
-
- /* Clear SI bit to be ready ... */
- I2Cx->I2CONCLR = (I2C_I2CONCLR_SIC | I2C_I2CONCLR_STAC|I2C_I2CONCLR_STOC);
-
- time_en = 0;
- timeout = 0;
-
- while (1)
- {
- /* Check SI flag ready */
- if (I2Cx->I2CONSET & I2C_I2CONSET_SI)
- {
- time_en = 0;
-
- CodeStatus = (I2Cx->I2STAT & I2C_STAT_CODE_BITMASK);
-
- Ret = I2C_SlaveHanleStates(I2Cx, CodeStatus, TransferCfg);
- if(I2C_CheckError(Ret))
- {
- goto s_error;
- }
- else if(Ret & I2C_STA_STO_RECV)
- {
- time_en = 1;
- timeout = 0;
- }
- else if (Ret & I2C_SEND_END)
- {
- goto s_end_stage;
- }
- }
- else if (time_en)
- {
- if (timeout++ > I2C_SLAVE_TIME_OUT)
- {
- // it's really a stop condition, goto end stage
- goto s_end_stage;
- }
- }
- }
-
-s_end_stage:
- /* Clear AA bit to disable ACK on I2C bus */
- I2Cx->I2CONCLR = I2C_I2CONCLR_AAC;
-
- // Check if there's no error during operation
- // Update status
- TransferCfg->status = CodeStatus | I2C_SETUP_STATUS_DONE;
- return SUCCESS;
-
-s_error:
- /* Clear AA bit to disable ACK on I2C bus */
- I2Cx->I2CONCLR = I2C_I2CONCLR_AAC;
-
- // Update status
- TransferCfg->status = CodeStatus;
- return ERROR;
- }
-
- else if (Opt == I2C_TRANSFER_INTERRUPT)
- {
- // Setup tx_rx data, callback and interrupt handler
- i2cdat[i2cId].txrx_setup = (uint32_t) TransferCfg;
-
- // Set direction phase, read first
- i2cdat[i2cId].dir = 1;
-
- // Enable AA
- I2Cx->I2CONSET = I2C_I2CONSET_AA;
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC | I2C_I2CONCLR_STAC;
- I2C_IntCmd(I2Cx, TRUE);
-
- return (SUCCESS);
- }
-
- return ERROR;
-}
-
-/*********************************************************************//**
- * @brief Set Own slave address in I2C peripheral corresponding to
- * parameter specified in OwnSlaveAddrConfigStruct.
- * @param[in] I2Cx I2C peripheral selected, should be
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @param[in] OwnSlaveAddrConfigStruct Pointer to a I2C_OWNSLAVEADDR_CFG_Type
- * structure that contains the configuration information for the
-* specified I2C slave address.
- * @return None
- **********************************************************************/
-void I2C_SetOwnSlaveAddr(LPC_I2C_TypeDef *I2Cx, I2C_OWNSLAVEADDR_CFG_Type *OwnSlaveAddrConfigStruct)
-{
- uint32_t tmp;
- CHECK_PARAM(PARAM_I2Cx(I2Cx));
- CHECK_PARAM(PARAM_I2C_SLAVEADDR_CH(OwnSlaveAddrConfigStruct->SlaveAddrChannel));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(OwnSlaveAddrConfigStruct->GeneralCallState));
-
- tmp = (((uint32_t)(OwnSlaveAddrConfigStruct->SlaveAddr_7bit << 1)) \
- | ((OwnSlaveAddrConfigStruct->GeneralCallState == ENABLE) ? 0x01 : 0x00))& I2C_I2ADR_BITMASK;
- switch (OwnSlaveAddrConfigStruct->SlaveAddrChannel)
- {
- case 0:
- I2Cx->I2ADR0 = tmp;
- I2Cx->I2MASK0 = I2C_I2MASK_MASK((uint32_t) \
- (OwnSlaveAddrConfigStruct->SlaveAddrMaskValue));
- break;
- case 1:
- I2Cx->I2ADR1 = tmp;
- I2Cx->I2MASK1 = I2C_I2MASK_MASK((uint32_t) \
- (OwnSlaveAddrConfigStruct->SlaveAddrMaskValue));
- break;
- case 2:
- I2Cx->I2ADR2 = tmp;
- I2Cx->I2MASK2 = I2C_I2MASK_MASK((uint32_t) \
- (OwnSlaveAddrConfigStruct->SlaveAddrMaskValue));
- break;
- case 3:
- I2Cx->I2ADR3 = tmp;
- I2Cx->I2MASK3 = I2C_I2MASK_MASK((uint32_t) \
- (OwnSlaveAddrConfigStruct->SlaveAddrMaskValue));
- break;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Configures functionality in I2C monitor mode
- * @param[in] I2Cx I2C peripheral selected, should be
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @param[in] MonitorCfgType Monitor Configuration type, should be:
- * - I2C_MONITOR_CFG_SCL_OUTPUT: I2C module can 'stretch'
- * the clock line (hold it low) until it has had time to
- * respond to an I2C interrupt.
- * - I2C_MONITOR_CFG_MATCHALL: When this bit is set to '1'
- * and the I2C is in monitor mode, an interrupt will be
- * generated on ANY address received.
- * @param[in] NewState New State of this function, should be:
- * - ENABLE: Enable this function.
- * - DISABLE: Disable this function.
- * @return None
- **********************************************************************/
-void I2C_MonitorModeConfig(LPC_I2C_TypeDef *I2Cx, uint32_t MonitorCfgType, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_I2Cx(I2Cx));
- CHECK_PARAM(PARAM_I2C_MONITOR_CFG(MonitorCfgType));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- if (NewState == ENABLE)
- {
- I2Cx->MMCTRL |= MonitorCfgType;
- }
- else
- {
- I2Cx->MMCTRL &= (~MonitorCfgType) & I2C_I2MMCTRL_BITMASK;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Enable/Disable I2C monitor mode
- * @param[in] I2Cx I2C peripheral selected, should be
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @param[in] NewState New State of this function, should be:
- * - ENABLE: Enable monitor mode.
- * - DISABLE: Disable monitor mode.
- * @return None
- **********************************************************************/
-void I2C_MonitorModeCmd(LPC_I2C_TypeDef *I2Cx, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_I2Cx(I2Cx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- if (NewState == ENABLE)
- {
- I2Cx->MMCTRL |= I2C_I2MMCTRL_MM_ENA;
- I2Cx->I2CONSET = I2C_I2CONSET_AA;
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC | I2C_I2CONCLR_STAC;
- }
- else
- {
- I2Cx->MMCTRL &= (~I2C_I2MMCTRL_MM_ENA) & I2C_I2MMCTRL_BITMASK;
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC | I2C_I2CONCLR_STAC | I2C_I2CONCLR_AAC;
- }
- I2C_MonitorBufferIndex = 0;
-}
-
-
-/*********************************************************************//**
- * @brief Get data from I2C data buffer in monitor mode.
- * @param[in] I2Cx I2C peripheral selected, should be
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @return None
- * Note: In monitor mode, the I2C module may lose the ability to stretch
- * the clock (stall the bus) if the ENA_SCL bit is not set. This means that
- * the processor will have a limited amount of time to read the contents of
- * the data received on the bus. If the processor reads the I2DAT shift
- * register, as it ordinarily would, it could have only one bit-time to
- * respond to the interrupt before the received data is overwritten by
- * new data.
- **********************************************************************/
-uint8_t I2C_MonitorGetDatabuffer(LPC_I2C_TypeDef *I2Cx)
-{
- CHECK_PARAM(PARAM_I2Cx(I2Cx));
- return ((uint8_t)(I2Cx->I2DATA_BUFFER));
-}
-
-/*********************************************************************//**
- * @brief Get data from I2C data buffer in monitor mode.
- * @param[in] I2Cx I2C peripheral selected, should be
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @return None
- * Note: In monitor mode, the I2C module may lose the ability to stretch
- * the clock (stall the bus) if the ENA_SCL bit is not set. This means that
- * the processor will have a limited amount of time to read the contents of
- * the data received on the bus. If the processor reads the I2DAT shift
- * register, as it ordinarily would, it could have only one bit-time to
- * respond to the interrupt before the received data is overwritten by
- * new data.
- **********************************************************************/
-BOOL_8 I2C_MonitorHandler(LPC_I2C_TypeDef *I2Cx, uint8_t *buffer, uint32_t size)
-{
- BOOL_8 ret=FALSE;
-
- I2Cx->I2CONCLR = I2C_I2CONCLR_SIC;
-
- buffer[I2C_MonitorBufferIndex] = (uint8_t)(I2Cx->I2DATA_BUFFER);
- I2C_MonitorBufferIndex++;
- if(I2C_MonitorBufferIndex >= size)
- {
- ret = TRUE;
- }
- return ret;
-}
-/*********************************************************************//**
- * @brief Get status of Master Transfer
- * @param[in] I2Cx I2C peripheral selected, should be:
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @return Master transfer status, could be:
- * - TRUE master transfer completed
- * - FALSE master transfer have not completed yet
- **********************************************************************/
-uint32_t I2C_MasterTransferComplete(LPC_I2C_TypeDef *I2Cx)
-{
- uint32_t retval, tmp;
- tmp = I2C_getNum(I2Cx);
- retval = I2C_MasterComplete[tmp];
- I2C_MasterComplete[tmp] = FALSE;
- return retval;
-}
-
-/*********************************************************************//**
- * @brief Get status of Slave Transfer
- * @param[in] I2Cx I2C peripheral selected, should be:
- * - LPC_I2C0
- * - LPC_I2C1
- * - LPC_I2C2
- * @return Complete status, could be: TRUE/FALSE
- **********************************************************************/
-uint32_t I2C_SlaveTransferComplete(LPC_I2C_TypeDef *I2Cx)
-{
- uint32_t retval, tmp;
- tmp = I2C_getNum(I2Cx);
- retval = I2C_SlaveComplete[tmp];
- I2C_SlaveComplete[tmp] = FALSE;
- return retval;
-}
-
-
-
-/**
- * @}
- */
-
-#endif /* _I2C */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_i2s.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_i2s.c
deleted file mode 100644
index f57fd2f1be..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_i2s.c
+++ /dev/null
@@ -1,663 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_i2s.c 2010-09-23
-*//**
-* @file lpc17xx_i2s.c
-* @brief Contains all functions support for I2S firmware
-* library on LPC17xx
-* @version 3.1
-* @date 23. Sep. 2010
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2010, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup I2S
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_i2s.h"
-#include "lpc17xx_clkpwr.h"
-
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-
-#ifdef _I2S
-
-/* Private Functions ---------------------------------------------------------- */
-
-static uint8_t i2s_GetWordWidth(LPC_I2S_TypeDef *I2Sx, uint8_t TRMode);
-static uint8_t i2s_GetChannel(LPC_I2S_TypeDef *I2Sx, uint8_t TRMode);
-
-/********************************************************************//**
- * @brief Get I2S wordwidth value
- * @param[in] I2Sx I2S peripheral selected, should be: LPC_I2S
- * @param[in] TRMode is the I2S mode, should be:
- * - I2S_TX_MODE = 0: transmit mode
- * - I2S_RX_MODE = 1: receive mode
- * @return The wordwidth value, should be: 8,16 or 32
- *********************************************************************/
-static uint8_t i2s_GetWordWidth(LPC_I2S_TypeDef *I2Sx, uint8_t TRMode) {
- uint8_t value;
-
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
- CHECK_PARAM(PARAM_I2S_TRX(TRMode));
-
- if (TRMode == I2S_TX_MODE) {
- value = (I2Sx->I2SDAO) & 0x03; /* get wordwidth bit */
- } else {
- value = (I2Sx->I2SDAI) & 0x03; /* get wordwidth bit */
- }
- switch (value) {
- case I2S_WORDWIDTH_8:
- return 8;
- case I2S_WORDWIDTH_16:
- return 16;
- default:
- return 32;
- }
-}
-
-/********************************************************************//**
- * @brief Get I2S channel value
- * @param[in] I2Sx I2S peripheral selected, should be: LPC_I2S
- * @param[in] TRMode is the I2S mode, should be:
- * - I2S_TX_MODE = 0: transmit mode
- * - I2S_RX_MODE = 1: receive mode
- * @return The channel value, should be: 1(mono) or 2(stereo)
- *********************************************************************/
-static uint8_t i2s_GetChannel(LPC_I2S_TypeDef *I2Sx, uint8_t TRMode) {
- uint8_t value;
-
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
- CHECK_PARAM(PARAM_I2S_TRX(TRMode));
-
- if (TRMode == I2S_TX_MODE) {
- value = ((I2Sx->I2SDAO) & 0x04)>>2; /* get bit[2] */
- } else {
- value = ((I2Sx->I2SDAI) & 0x04)>>2; /* get bit[2] */
- }
- if(value == I2S_MONO) return 1;
- return 2;
-}
-
-/* End of Private Functions --------------------------------------------------- */
-
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup I2S_Public_Functions
- * @{
- */
-
-/********************************************************************//**
- * @brief Initialize I2S
- * - Turn on power and clock
- * @param[in] I2Sx I2S peripheral selected, should be: LPC_I2S
- * @return none
- *********************************************************************/
-void I2S_Init(LPC_I2S_TypeDef *I2Sx) {
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
-
- // Turn on power and clock
- CLKPWR_ConfigPPWR(CLKPWR_PCONP_PCI2S, ENABLE);
- LPC_I2S->I2SDAI = LPC_I2S->I2SDAO = 0x00;
-}
-
-/********************************************************************//**
- * @brief Configuration I2S, setting:
- * - master/slave mode
- * - wordwidth value
- * - channel mode
- * @param[in] I2Sx I2S peripheral selected, should be: LPC_I2S
- * @param[in] TRMode transmit/receive mode, should be:
- * - I2S_TX_MODE = 0: transmit mode
- * - I2S_RX_MODE = 1: receive mode
- * @param[in] ConfigStruct pointer to I2S_CFG_Type structure
- * which will be initialized.
- * @return none
- *********************************************************************/
-void I2S_Config(LPC_I2S_TypeDef *I2Sx, uint8_t TRMode, I2S_CFG_Type* ConfigStruct)
-{
- uint32_t bps, config;
-
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
-
- CHECK_PARAM(PARAM_I2S_WORDWIDTH(ConfigStruct->wordwidth));
- CHECK_PARAM(PARAM_I2S_CHANNEL(ConfigStruct->mono));
- CHECK_PARAM(PARAM_I2S_STOP(ConfigStruct->stop));
- CHECK_PARAM(PARAM_I2S_RESET(ConfigStruct->reset));
- CHECK_PARAM(PARAM_I2S_WS_SEL(ConfigStruct->ws_sel));
- CHECK_PARAM(PARAM_I2S_MUTE(ConfigStruct->mute));
-
- /* Setup clock */
- bps = (ConfigStruct->wordwidth +1)*8;
-
- /* Calculate audio config */
- config = (bps - 1)<<6 | (ConfigStruct->ws_sel)<<5 | (ConfigStruct->reset)<<4 |
- (ConfigStruct->stop)<<3 | (ConfigStruct->mono)<<2 | (ConfigStruct->wordwidth);
-
- if(TRMode == I2S_RX_MODE){
- LPC_I2S->I2SDAI = config;
- }else{
- LPC_I2S->I2SDAO = config;
- }
-}
-
-/********************************************************************//**
- * @brief DeInitial both I2S transmit or receive
- * @param[in] I2Sx I2S peripheral selected, should be: LPC_I2S
- * @return none
- *********************************************************************/
-void I2S_DeInit(LPC_I2S_TypeDef *I2Sx) {
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
-
- // Turn off power and clock
- CLKPWR_ConfigPPWR(CLKPWR_PCONP_PCI2S, DISABLE);
-}
-
-/********************************************************************//**
- * @brief Get I2S Buffer Level
- * @param[in] I2Sx I2S peripheral selected, should be: LPC_I2S
- * @param[in] TRMode Transmit/receive mode, should be:
- * - I2S_TX_MODE = 0: transmit mode
- * - I2S_RX_MODE = 1: receive mode
- * @return current level of Transmit/Receive Buffer
- *********************************************************************/
-uint8_t I2S_GetLevel(LPC_I2S_TypeDef *I2Sx, uint8_t TRMode)
-{
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
- CHECK_PARAM(PARAM_I2S_TRX(TRMode));
-
- if(TRMode == I2S_TX_MODE)
- {
- return ((I2Sx->I2SSTATE >> 16) & 0xFF);
- }
- else
- {
- return ((I2Sx->I2SSTATE >> 8) & 0xFF);
- }
-}
-
-/********************************************************************//**
- * @brief I2S Start: clear all STOP,RESET and MUTE bit, ready to operate
- * @param[in] I2Sx I2S peripheral selected, should be: LPC_I2S
- * @return none
- *********************************************************************/
-void I2S_Start(LPC_I2S_TypeDef *I2Sx)
-{
- //Clear STOP,RESET and MUTE bit
- I2Sx->I2SDAO &= ~I2S_DAI_RESET;
- I2Sx->I2SDAI &= ~I2S_DAI_RESET;
- I2Sx->I2SDAO &= ~I2S_DAI_STOP;
- I2Sx->I2SDAI &= ~I2S_DAI_STOP;
- I2Sx->I2SDAO &= ~I2S_DAI_MUTE;
-}
-
-/********************************************************************//**
- * @brief I2S Send data
- * @param[in] I2Sx I2S peripheral selected, should be: LPC_I2S
- * @param[in] BufferData pointer to uint32_t is the data will be send
- * @return none
- *********************************************************************/
-void I2S_Send(LPC_I2S_TypeDef *I2Sx, uint32_t BufferData) {
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
-
- I2Sx->I2STXFIFO = BufferData;
-}
-
-/********************************************************************//**
- * @brief I2S Receive Data
- * @param[in] I2Sx pointer to LPC_I2S_TypeDef
- * @return received value
- *********************************************************************/
-uint32_t I2S_Receive(LPC_I2S_TypeDef* I2Sx) {
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
-
- return (I2Sx->I2SRXFIFO);
-
-}
-
-/********************************************************************//**
- * @brief I2S Pause
- * @param[in] I2Sx I2S peripheral selected, should be: LPC_I2S
- * @param[in] TRMode is transmit/receive mode, should be:
- * - I2S_TX_MODE = 0: transmit mode
- * - I2S_RX_MODE = 1: receive mode
- * @return none
- *********************************************************************/
-void I2S_Pause(LPC_I2S_TypeDef *I2Sx, uint8_t TRMode) {
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
- CHECK_PARAM(PARAM_I2S_TRX(TRMode));
-
- if (TRMode == I2S_TX_MODE) //Transmit mode
- {
- I2Sx->I2SDAO |= I2S_DAO_STOP;
- } else //Receive mode
- {
- I2Sx->I2SDAI |= I2S_DAI_STOP;
- }
-}
-
-/********************************************************************//**
- * @brief I2S Mute
- * @param[in] I2Sx I2S peripheral selected, should be: LPC_I2S
- * @param[in] TRMode is transmit/receive mode, should be:
- * - I2S_TX_MODE = 0: transmit mode
- * - I2S_RX_MODE = 1: receive mode
- * @return none
- *********************************************************************/
-void I2S_Mute(LPC_I2S_TypeDef *I2Sx, uint8_t TRMode) {
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
- CHECK_PARAM(PARAM_I2S_TRX(TRMode));
-
- if (TRMode == I2S_TX_MODE) //Transmit mode
- {
- I2Sx->I2SDAO |= I2S_DAO_MUTE;
- } else //Receive mode
- {
- I2Sx->I2SDAI |= I2S_DAI_MUTE;
- }
-}
-
-/********************************************************************//**
- * @brief I2S Stop
- * @param[in] I2Sx I2S peripheral selected, should be: LPC_I2S
- * @param[in] TRMode is transmit/receive mode, should be:
- * - I2S_TX_MODE = 0: transmit mode
- * - I2S_RX_MODE = 1: receive mode
- * @return none
- *********************************************************************/
-void I2S_Stop(LPC_I2S_TypeDef *I2Sx, uint8_t TRMode) {
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
- CHECK_PARAM(PARAM_I2S_TRX(TRMode));
-
- if (TRMode == I2S_TX_MODE) //Transmit mode
- {
- I2Sx->I2SDAO &= ~I2S_DAO_MUTE;
- I2Sx->I2SDAO |= I2S_DAO_STOP;
- I2Sx->I2SDAO |= I2S_DAO_RESET;
- } else //Receive mode
- {
- I2Sx->I2SDAI |= I2S_DAI_STOP;
- I2Sx->I2SDAI |= I2S_DAI_RESET;
- }
-}
-
-/********************************************************************//**
- * @brief Set frequency for I2S
- * @param[in] I2Sx I2S peripheral selected, should be: LPC_I2S
- * @param[in] Freq is the frequency for I2S will be set. It can range
- * from 16-96 kHz(16, 22.05, 32, 44.1, 48, 96kHz)
- * @param[in] TRMode is transmit/receive mode, should be:
- * - I2S_TX_MODE = 0: transmit mode
- * - I2S_RX_MODE = 1: receive mode
- * @return Status: ERROR or SUCCESS
- *********************************************************************/
-Status I2S_FreqConfig(LPC_I2S_TypeDef *I2Sx, uint32_t Freq, uint8_t TRMode) {
-
- uint32_t i2s_clk;
- uint8_t channel, wordwidth;
- uint32_t x, y;
- uint64_t divider;
- uint16_t dif;
- uint16_t x_divide, y_divide;
- uint16_t err, ErrorOptimal = 0xFFFF;
-
- uint32_t N;
-
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
- CHECK_PARAM(PRAM_I2S_FREQ(Freq));
- CHECK_PARAM(PARAM_I2S_TRX(TRMode));
-
- //Get the frequency of PCLK_I2S
- i2s_clk = CLKPWR_GetPCLK(CLKPWR_PCLKSEL_I2S);
-
- if(TRMode == I2S_TX_MODE)
- {
- channel = i2s_GetChannel(I2Sx,I2S_TX_MODE);
- wordwidth = i2s_GetWordWidth(I2Sx,I2S_TX_MODE);
- }
- else
- {
- channel = i2s_GetChannel(I2Sx,I2S_RX_MODE);
- wordwidth = i2s_GetWordWidth(I2Sx,I2S_RX_MODE);
- }
-
- /* Calculate X and Y divider
- * The MCLK rate for the I2S transmitter is determined by the value
- * in the I2STXRATE/I2SRXRATE register. The required I2STXRATE/I2SRXRATE
- * setting depends on the desired audio sample rate desired, the format
- * (stereo/mono) used, and the data size.
- * The formula is:
- * I2S_MCLK = PCLK_I2S * (X/Y) / 2
- * In that, Y must be greater than or equal to X. X should divides evenly
- * into Y.
- * We have:
- * I2S_MCLK = Freq * channel*wordwidth * (I2STXBITRATE+1);
- * So: (X/Y) = (Freq * channel*wordwidth * (I2STXBITRATE+1))*2/PCLK_I2S
- * We use a loop function to chose the most suitable X,Y value
- */
-
- /* divider is a fixed point number with 16 fractional bits */
- divider = (((uint64_t)Freq *channel*wordwidth * 2)<<16) / i2s_clk;
-
- /* find N that make x/y <= 1 -> divider <= 2^16 */
- for(N=64;N>0;N--){
- if((divider*N) < (1<<16)) break;
- }
-
- if(N == 0) return ERROR;
-
- divider *= N;
-
- for (y = 255; y > 0; y--) {
- x = y * divider;
- if(x & (0xFF000000)) continue;
- dif = x & 0xFFFF;
- if(dif>0x8000) err = 0x10000-dif;
- else err = dif;
- if (err == 0)
- {
- y_divide = y;
- break;
- }
- else if (err < ErrorOptimal)
- {
- ErrorOptimal = err;
- y_divide = y;
- }
- }
- x_divide = ((uint64_t)y_divide * Freq *(channel*wordwidth)* N * 2)/i2s_clk;
- if(x_divide >= 256) x_divide = 0xFF;
- if(x_divide == 0) x_divide = 1;
-
- if (TRMode == I2S_TX_MODE)// Transmitter
- {
- I2Sx->I2STXBITRATE = N-1;
- I2Sx->I2STXRATE = y_divide | (x_divide << 8);
- } else //Receiver
- {
- I2Sx->I2SRXBITRATE = N-1;
- I2Sx->I2STXRATE = y_divide | (x_divide << 8);
- }
- return SUCCESS;
-}
-
-/********************************************************************//**
- * @brief I2S set bitrate
- * @param[in] I2Sx I2S peripheral selected, should be: LPC_I2S
- * @param[in] bitrate value will be set
- * bitrate value should be in range: 0 .. 63
- * @param[in] TRMode is transmit/receive mode, should be:
- * - I2S_TX_MODE = 0: transmit mode
- * - I2S_RX_MODE = 1: receive mode
- * @return none
- *********************************************************************/
-void I2S_SetBitRate(LPC_I2S_TypeDef *I2Sx, uint8_t bitrate, uint8_t TRMode)
-{
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
- CHECK_PARAM(PARAM_I2S_BITRATE(bitrate));
- CHECK_PARAM(PARAM_I2S_TRX(TRMode));
-
- if(TRMode == I2S_TX_MODE)
- {
- I2Sx->I2STXBITRATE = bitrate;
- }
- else
- {
- I2Sx->I2SRXBITRATE = bitrate;
- }
-}
-
-/********************************************************************//**
- * @brief Configuration operating mode for I2S
- * @param[in] I2Sx I2S peripheral selected, should be: LPC_I2S
- * @param[in] ModeConfig pointer to I2S_MODEConf_Type will be used to
- * configure
- * @param[in] TRMode is transmit/receive mode, should be:
- * - I2S_TX_MODE = 0: transmit mode
- * - I2S_RX_MODE = 1: receive mode
- * @return none
- *********************************************************************/
-void I2S_ModeConfig(LPC_I2S_TypeDef *I2Sx, I2S_MODEConf_Type* ModeConfig,
- uint8_t TRMode)
-{
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
- CHECK_PARAM(PARAM_I2S_CLKSEL(ModeConfig->clksel));
- CHECK_PARAM(PARAM_I2S_4PIN(ModeConfig->fpin));
- CHECK_PARAM(PARAM_I2S_MCLK(ModeConfig->mcena));
- CHECK_PARAM(PARAM_I2S_TRX(TRMode));
-
- if (TRMode == I2S_TX_MODE) {
- I2Sx->I2STXMODE &= ~0x0F; //clear bit 3:0 in I2STXMODE register
- if (ModeConfig->clksel == I2S_CLKSEL_MCLK) {
- I2Sx->I2STXMODE |= 0x02;
- }
- if (ModeConfig->fpin == I2S_4PIN_ENABLE) {
- I2Sx->I2STXMODE |= (1 << 2);
- }
- if (ModeConfig->mcena == I2S_MCLK_ENABLE) {
- I2Sx->I2STXMODE |= (1 << 3);
- }
- } else {
- I2Sx->I2SRXMODE &= ~0x0F; //clear bit 3:0 in I2STXMODE register
- if (ModeConfig->clksel == I2S_CLKSEL_MCLK) {
- I2Sx->I2SRXMODE |= 0x02;
- }
- if (ModeConfig->fpin == I2S_4PIN_ENABLE) {
- I2Sx->I2SRXMODE |= (1 << 2);
- }
- if (ModeConfig->mcena == I2S_MCLK_ENABLE) {
- I2Sx->I2SRXMODE |= (1 << 3);
- }
- }
-}
-
-/********************************************************************//**
- * @brief Configure DMA operation for I2S
- * @param[in] I2Sx I2S peripheral selected, should be: LPC_I2S
- * @param[in] DMAConfig pointer to I2S_DMAConf_Type will be used to configure
- * @param[in] TRMode is transmit/receive mode, should be:
- * - I2S_TX_MODE = 0: transmit mode
- * - I2S_RX_MODE = 1: receive mode
- * @return none
- *********************************************************************/
-void I2S_DMAConfig(LPC_I2S_TypeDef *I2Sx, I2S_DMAConf_Type* DMAConfig,
- uint8_t TRMode)
-{
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
- CHECK_PARAM(PARAM_I2S_DMA(DMAConfig->DMAIndex));
- CHECK_PARAM(PARAM_I2S_DMA_DEPTH(DMAConfig->depth));
- CHECK_PARAM(PARAM_I2S_TRX(TRMode));
-
- if (TRMode == I2S_RX_MODE) {
- if (DMAConfig->DMAIndex == I2S_DMA_1) {
- LPC_I2S->I2SDMA1 = (DMAConfig->depth) << 8;
- } else {
- LPC_I2S->I2SDMA2 = (DMAConfig->depth) << 8;
- }
- } else {
- if (DMAConfig->DMAIndex == I2S_DMA_1) {
- LPC_I2S->I2SDMA1 = (DMAConfig->depth) << 16;
- } else {
- LPC_I2S->I2SDMA2 = (DMAConfig->depth) << 16;
- }
- }
-}
-
-/********************************************************************//**
- * @brief Enable/Disable DMA operation for I2S
- * @param[in] I2Sx: I2S peripheral selected, should be: LPC_I2S
- * @param[in] DMAIndex chose what DMA is used, should be:
- * - I2S_DMA_1 = 0: DMA1
- * - I2S_DMA_2 = 1: DMA2
- * @param[in] TRMode is transmit/receive mode, should be:
- * - I2S_TX_MODE = 0: transmit mode
- * - I2S_RX_MODE = 1: receive mode
- * @param[in] NewState is new state of DMA operation, should be:
- * - ENABLE
- * - DISABLE
- * @return none
- *********************************************************************/
-void I2S_DMACmd(LPC_I2S_TypeDef *I2Sx, uint8_t DMAIndex, uint8_t TRMode,
- FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
- CHECK_PARAM(PARAM_I2S_DMA(DMAIndex));
- CHECK_PARAM(PARAM_I2S_TRX(TRMode));
-
- if (TRMode == I2S_RX_MODE) {
- if (DMAIndex == I2S_DMA_1) {
- if (NewState == ENABLE)
- I2Sx->I2SDMA1 |= 0x01;
- else
- I2Sx->I2SDMA1 &= ~0x01;
- } else {
- if (NewState == ENABLE)
- I2Sx->I2SDMA2 |= 0x01;
- else
- I2Sx->I2SDMA2 &= ~0x01;
- }
- } else {
- if (DMAIndex == I2S_DMA_1) {
- if (NewState == ENABLE)
- I2Sx->I2SDMA1 |= 0x02;
- else
- I2Sx->I2SDMA1 &= ~0x02;
- } else {
- if (NewState == ENABLE)
- I2Sx->I2SDMA2 |= 0x02;
- else
- I2Sx->I2SDMA2 &= ~0x02;
- }
- }
-}
-
-/********************************************************************//**
- * @brief Configure IRQ for I2S
- * @param[in] I2Sx I2S peripheral selected, should be: LPC_I2S
- * @param[in] TRMode is transmit/receive mode, should be:
- * - I2S_TX_MODE = 0: transmit mode
- * - I2S_RX_MODE = 1: receive mode
- * @param[in] level is the FIFO level that triggers IRQ request
- * @return none
- *********************************************************************/
-void I2S_IRQConfig(LPC_I2S_TypeDef *I2Sx, uint8_t TRMode, uint8_t level) {
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
- CHECK_PARAM(PARAM_I2S_TRX(TRMode));
- CHECK_PARAM(PARAM_I2S_IRQ_LEVEL(level));
-
- if (TRMode == I2S_RX_MODE) {
- I2Sx->I2SIRQ |= (level << 8);
- } else {
- I2Sx->I2SIRQ |= (level << 16);
- }
-}
-
-/********************************************************************//**
- * @brief Enable/Disable IRQ for I2S
- * @param[in] I2Sx I2S peripheral selected, should be: LPC_I2S
- * @param[in] TRMode is transmit/receive mode, should be:
- * - I2S_TX_MODE = 0: transmit mode
- * - I2S_RX_MODE = 1: receive mode
- * @param[in] NewState is new state of DMA operation, should be:
- * - ENABLE
- * - DISABLE
- * @return none
- *********************************************************************/
-void I2S_IRQCmd(LPC_I2S_TypeDef *I2Sx, uint8_t TRMode, FunctionalState NewState) {
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- if (TRMode == I2S_RX_MODE) {
- if (NewState == ENABLE)
- I2Sx->I2SIRQ |= 0x01;
- else
- I2Sx->I2SIRQ &= ~0x01;
- //Enable DMA
-
- } else {
- if (NewState == ENABLE)
- I2Sx->I2SIRQ |= 0x02;
- else
- I2Sx->I2SIRQ &= ~0x02;
- }
-}
-
-/********************************************************************//**
- * @brief Get I2S interrupt status
- * @param[in] I2Sx I2S peripheral selected, should be: LPC_I2S
- * @param[in] TRMode is transmit/receive mode, should be:
- * - I2S_TX_MODE = 0: transmit mode
- * - I2S_RX_MODE = 1: receive mode
- * @return FunctionState should be:
- * - ENABLE: interrupt is enable
- * - DISABLE: interrupt is disable
- *********************************************************************/
-FunctionalState I2S_GetIRQStatus(LPC_I2S_TypeDef *I2Sx,uint8_t TRMode)
-{
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
- if(TRMode == I2S_TX_MODE)
- return ((FunctionalState)((I2Sx->I2SIRQ >> 1)&0x01));
- else
- return ((FunctionalState)((I2Sx->I2SIRQ)&0x01));
-}
-
-/********************************************************************//**
- * @brief Get I2S interrupt depth
- * @param[in] I2Sx I2S peripheral selected, should be: LPC_I2S
- * @param[in] TRMode is transmit/receive mode, should be:
- * - I2S_TX_MODE = 0: transmit mode
- * - I2S_RX_MODE = 1: receive mode
- * @return depth of FIFO level on which to create an irq request
- *********************************************************************/
-uint8_t I2S_GetIRQDepth(LPC_I2S_TypeDef *I2Sx,uint8_t TRMode)
-{
- CHECK_PARAM(PARAM_I2Sx(I2Sx));
- if(TRMode == I2S_TX_MODE)
- return (((I2Sx->I2SIRQ)>>16)&0xFF);
- else
- return (((I2Sx->I2SIRQ)>>8)&0xFF);
-}
-/**
- * @}
- */
-
-#endif /* _I2S */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
-
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_iap.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_iap.c
deleted file mode 100644
index f915f0c35e..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_iap.c
+++ /dev/null
@@ -1,308 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_iap.c 2012-04-18
-*//**
-* @file lpc17xx_iap.c
- * @brief Contains all functions support for IAP on lpc17xx
-* @version 1.0
-* @date 18. April. 2012
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2011, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-#include "lpc17xx_iap.h"
-#include "system_LPC17xx.h"
-
-// IAP Command
-typedef void (*IAP)(uint32_t *cmd,uint32_t *result);
-IAP iap_entry = (IAP) IAP_LOCATION;
-#define IAP_Call iap_entry
-
-/** @addtogroup IAP_Public_Functions IAP Public Function
- * @ingroup IAP
- * @{
- */
-
-
-/*********************************************************************//**
- * @brief Get Sector Number
- *
- * @param[in] adr Sector Address
- *
- * @return Sector Number.
- *
- **********************************************************************/
- uint32_t GetSecNum (uint32_t adr)
-{
- uint32_t n;
-
- n = adr >> 12; // 4kB Sector
- if (n >= 0x10) {
- n = 0x0E + (n >> 3); // 32kB Sector
- }
-
- return (n); // Sector Number
-}
-
-/*********************************************************************//**
- * @brief Prepare sector(s) for write operation
- *
- * @param[in] start_sec The number of start sector
- * @param[in] end_sec The number of end sector
- *
- * @return CMD_SUCCESS/BUSY/INVALID_SECTOR.
- *
- **********************************************************************/
-IAP_STATUS_CODE PrepareSector(uint32_t start_sec, uint32_t end_sec)
-{
- IAP_COMMAND_Type command;
- command.cmd = IAP_PREPARE; // Prepare Sector for Write
- command.param[0] = start_sec; // Start Sector
- command.param[1] = end_sec; // End Sector
- IAP_Call (&command.cmd, &command.status); // Call IAP Command
- return (IAP_STATUS_CODE)command.status;
-}
-
-/*********************************************************************//**
- * @brief Copy RAM to Flash
- *
- * @param[in] dest destination buffer (in Flash memory).
- * @param[in] source source buffer (in RAM).
- * @param[in] size the write size.
- *
- * @return CMD_SUCCESS.
- * SRC_ADDR_ERROR/DST_ADDR_ERROR
- * SRC_ADDR_NOT_MAPPED/DST_ADDR_NOT_MAPPED
- * COUNT_ERROR/SECTOR_NOT_PREPARED_FOR_WRITE_OPERATION
- * BUSY
- *
- **********************************************************************/
-IAP_STATUS_CODE CopyRAM2Flash(uint8_t * dest, uint8_t* source, IAP_WRITE_SIZE size)
-{
- uint32_t sec;
- IAP_STATUS_CODE status;
- IAP_COMMAND_Type command;
-
- // Prepare sectors
- sec = GetSecNum((uint32_t)dest);
- status = PrepareSector(sec, sec);
- if(status != CMD_SUCCESS)
- return status;
-
- // write
- command.cmd = IAP_COPY_RAM2FLASH; // Copy RAM to Flash
- command.param[0] = (uint32_t)dest; // Destination Flash Address
- command.param[1] = (uint32_t)source; // Source RAM Address
- command.param[2] = size; // Number of bytes
- command.param[3] = SystemCoreClock / 1000; // CCLK in kHz
- IAP_Call (&command.cmd, &command.status); // Call IAP Command
-
- return (IAP_STATUS_CODE)command.status; // Finished without Errors
-}
-
-/*********************************************************************//**
- * @brief Erase sector(s)
- *
- * @param[in] start_sec The number of start sector
- * @param[in] end_sec The number of end sector
- *
- * @return CMD_SUCCESS.
- * INVALID_SECTOR
- * SECTOR_NOT_PREPARED_FOR_WRITE_OPERATION
- * BUSY
- *
- **********************************************************************/
-IAP_STATUS_CODE EraseSector(uint32_t start_sec, uint32_t end_sec)
-{
- IAP_COMMAND_Type command;
- IAP_STATUS_CODE status;
-
- // Prepare sectors
- status = PrepareSector(start_sec, end_sec);
- if(status != CMD_SUCCESS)
- return status;
-
- // Erase sectors
- command.cmd = IAP_ERASE; // Prepare Sector for Write
- command.param[0] = start_sec; // Start Sector
- command.param[1] = end_sec; // End Sector
- command.param[2] = SystemCoreClock / 1000; // CCLK in kHz
- IAP_Call (&command.cmd, &command.status); // Call IAP Command
- return (IAP_STATUS_CODE)command.status;
-}
-
-/*********************************************************************//**
- * @brief Blank check sector(s)
- *
- * @param[in] start_sec The number of start sector
- * @param[in] end_sec The number of end sector
- * @param[out] first_nblank_loc The offset of the first non-blank word
- * @param[out] first_nblank_val The value of the first non-blank word
- *
- * @return CMD_SUCCESS.
- * INVALID_SECTOR
- * SECTOR_NOT_BLANK
- * BUSY
- *
- **********************************************************************/
-IAP_STATUS_CODE BlankCheckSector(uint32_t start_sec, uint32_t end_sec,
- uint32_t *first_nblank_loc,
- uint32_t *first_nblank_val)
-{
- IAP_COMMAND_Type command;
-
- command.cmd = IAP_BLANK_CHECK; // Prepare Sector for Write
- command.param[0] = start_sec; // Start Sector
- command.param[1] = end_sec; // End Sector
- IAP_Call (&command.cmd, &command.status); // Call IAP Command
-
- if(command.status == SECTOR_NOT_BLANK)
- {
- // Update out value
- if(first_nblank_loc != NULL)
- *first_nblank_loc = command.result[0];
- if(first_nblank_val != NULL)
- *first_nblank_val = command.result[1];
- }
-
- return (IAP_STATUS_CODE)command.status;
-}
-
-/*********************************************************************//**
- * @brief Read part identification number
- *
- * @param[out] partID Part ID
- *
- * @return CMD_SUCCESS
- *
- **********************************************************************/
-IAP_STATUS_CODE ReadPartID(uint32_t *partID)
-{
- IAP_COMMAND_Type command;
- command.cmd = IAP_READ_PART_ID;
- IAP_Call (&command.cmd, &command.status); // Call IAP Command
-
- if(command.status == CMD_SUCCESS)
- {
- if(partID != NULL)
- *partID = command.result[0];
- }
-
- return (IAP_STATUS_CODE)command.status;
-}
-
-/*********************************************************************//**
- * @brief Read boot code version. The version is interpreted as ..
- *
- * @param[out] major The major
- * @param[out] minor The minor
- *
- * @return CMD_SUCCESS
- *
- **********************************************************************/
-IAP_STATUS_CODE ReadBootCodeVer(uint8_t *major, uint8_t* minor)
-{
- IAP_COMMAND_Type command;
- command.cmd = IAP_READ_BOOT_VER;
- IAP_Call (&command.cmd, &command.status); // Call IAP Command
-
- if(command.status == CMD_SUCCESS)
- {
- if(major != NULL)
- *major = (command.result[0] >> 8) & 0xFF;
- if(minor != NULL)
- *minor = (command.result[0]) & 0xFF;
- }
-
- return (IAP_STATUS_CODE)command.status;
-}
-
-/*********************************************************************//**
- * @brief Read Device serial number.
- *
- * @param[out] uid Serial number.
- *
- * @return CMD_SUCCESS
- *
- **********************************************************************/
-IAP_STATUS_CODE ReadDeviceSerialNum(uint32_t *uid)
-{
- IAP_COMMAND_Type command;
- command.cmd = IAP_READ_SERIAL_NUMBER;
- IAP_Call (&command.cmd, &command.status); // Call IAP Command
-
- if(command.status == CMD_SUCCESS)
- {
- if(uid != NULL)
- {
- uint32_t i = 0;
- for(i = 0; i < 4; i++)
- uid[i] = command.result[i];
- }
- }
-
- return (IAP_STATUS_CODE)command.status;
-}
-
-/*********************************************************************//**
- * @brief compare the memory contents at two locations.
- *
- * @param[in] addr1 The address of the 1st buffer (in RAM/Flash).
- * @param[in] addr2 The address of the 2nd buffer (in RAM/Flash).
- * @param[in] size Number of bytes to be compared; should be a multiple of 4.
- *
- * @return CMD_SUCCESS
- * COMPARE_ERROR
- * COUNT_ERROR (Byte count is not a multiple of 4)
- * ADDR_ERROR
- * ADDR_NOT_MAPPED
- *
- **********************************************************************/
-IAP_STATUS_CODE Compare(uint8_t *addr1, uint8_t *addr2, uint32_t size)
-{
- IAP_COMMAND_Type command;
- command.cmd = IAP_COMPARE;
- command.param[0] = (uint32_t)addr1;
- command.param[1] = (uint32_t)addr2;
- command.param[2] = size;
- IAP_Call (&command.cmd, &command.status); // Call IAP Command
-
- return (IAP_STATUS_CODE)command.status;
-}
-
-/*********************************************************************//**
- * @brief Re-invoke ISP.
- *
- * @param[in] None.
- *
- * @return None.
- *
- **********************************************************************/
-void InvokeISP(void)
-{
- IAP_COMMAND_Type command;
- command.cmd = IAP_REINVOKE_ISP;
- IAP_Call (&command.cmd, &command.status); // Call IAP Command
-}
-
-/**
- * @}
- */
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_libcfg_default.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_libcfg_default.c
deleted file mode 100644
index 4f883e3077..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_libcfg_default.c
+++ /dev/null
@@ -1,76 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_libcfg_default.c 2010-05-21
-*//**
-* @file lpc17xx_libcfg_default.c
-* @brief Library configuration source file (default), used to build
-* library without examples
-* @version 2.0
-* @date 21. May. 2010
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2010, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Library group ----------------------------------------------------------- */
-/** @addtogroup LIBCFG_DEFAULT
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_libcfg_default.h"
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup LIBCFG_DEFAULT_Public_Functions
- * @{
- */
-
-#ifndef __BUILD_WITH_EXAMPLE__
-
-#ifdef DEBUG
-/*******************************************************************************
-* @brief Reports the name of the source file and the source line number
-* where the CHECK_PARAM error has occurred.
-* @param[in] file Pointer to the source file name
-* @param[in] line assert_param error line source number
-* @return None
-*******************************************************************************/
-void check_failed(uint8_t *file, uint32_t line)
-{
- /* User can add his own implementation to report the file name and line number,
- ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
-
- /* Infinite loop */
- while(1);
-}
-#endif /* DEBUG */
-
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-/**
- * @}
- */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_mcpwm.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_mcpwm.c
deleted file mode 100644
index cd9318b4fe..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_mcpwm.c
+++ /dev/null
@@ -1,509 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_mcpwm.c 2010-05-21
-*//**
-* @file lpc17xx_mcpwm.c
-* @brief Contains all functions support for Motor Control PWM firmware
-* library on LPC17xx
-* @version 2.0
-* @date 21. May. 2010
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2010, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup MCPWM
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_mcpwm.h"
-#include "lpc17xx_clkpwr.h"
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-
-#ifdef _MCPWM
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup MCPWM_Public_Functions
- * @{
- */
-
-/*********************************************************************//**
- * @brief Initializes the MCPWM peripheral
- * @param[in] MCPWMx Motor Control PWM peripheral selected,
- * Should be: LPC_MCPWM
- * @return None
- **********************************************************************/
-void MCPWM_Init(LPC_MCPWM_TypeDef *MCPWMx)
-{
-
- /* Turn On MCPWM PCLK */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCMC, ENABLE);
- /* As default, peripheral clock for MCPWM module
- * is set to FCCLK / 2 */
- // CLKPWR_SetPCLKDiv(CLKPWR_PCLKSEL_MC, CLKPWR_PCLKSEL_CCLK_DIV_2);
-
- MCPWMx->MCCAP_CLR = MCPWM_CAPCLR_CAP(0) | MCPWM_CAPCLR_CAP(1) | MCPWM_CAPCLR_CAP(2);
- MCPWMx->MCINTFLAG_CLR = MCPWM_INT_ILIM(0) | MCPWM_INT_ILIM(1) | MCPWM_INT_ILIM(2) \
- | MCPWM_INT_IMAT(0) | MCPWM_INT_IMAT(1) | MCPWM_INT_IMAT(2) \
- | MCPWM_INT_ICAP(0) | MCPWM_INT_ICAP(1) | MCPWM_INT_ICAP(2);
- MCPWMx->MCINTEN_CLR = MCPWM_INT_ILIM(0) | MCPWM_INT_ILIM(1) | MCPWM_INT_ILIM(2) \
- | MCPWM_INT_IMAT(0) | MCPWM_INT_IMAT(1) | MCPWM_INT_IMAT(2) \
- | MCPWM_INT_ICAP(0) | MCPWM_INT_ICAP(1) | MCPWM_INT_ICAP(2);
-}
-
-
-/*********************************************************************//**
- * @brief Configures each channel in MCPWM peripheral according to the
- * specified parameters in the MCPWM_CHANNEL_CFG_Type.
- * @param[in] MCPWMx Motor Control PWM peripheral selected
- * should be: LPC_MCPWM
- * @param[in] channelNum Channel number, should be: 0..2.
- * @param[in] channelSetup Pointer to a MCPWM_CHANNEL_CFG_Type structure
-* that contains the configuration information for the
-* specified MCPWM channel.
- * @return None
- **********************************************************************/
-void MCPWM_ConfigChannel(LPC_MCPWM_TypeDef *MCPWMx, uint32_t channelNum,
- MCPWM_CHANNEL_CFG_Type * channelSetup)
-{
- if (channelNum <= 2) {
- if (channelNum == 0) {
- MCPWMx->MCTIM0 = channelSetup->channelTimercounterValue;
- MCPWMx->MCPER0 = channelSetup->channelPeriodValue;
- MCPWMx->MCPW0 = channelSetup->channelPulsewidthValue;
- } else if (channelNum == 1) {
- MCPWMx->MCTIM1 = channelSetup->channelTimercounterValue;
- MCPWMx->MCPER1 = channelSetup->channelPeriodValue;
- MCPWMx->MCPW1 = channelSetup->channelPulsewidthValue;
- } else if (channelNum == 2) {
- MCPWMx->MCTIM2 = channelSetup->channelTimercounterValue;
- MCPWMx->MCPER2 = channelSetup->channelPeriodValue;
- MCPWMx->MCPW2 = channelSetup->channelPulsewidthValue;
- } else {
- return;
- }
-
- if (channelSetup->channelType /* == MCPWM_CHANNEL_CENTER_MODE */){
- MCPWMx->MCCON_SET = MCPWM_CON_CENTER(channelNum);
- } else {
- MCPWMx->MCCON_CLR = MCPWM_CON_CENTER(channelNum);
- }
-
- if (channelSetup->channelPolarity /* == MCPWM_CHANNEL_PASSIVE_HI */){
- MCPWMx->MCCON_SET = MCPWM_CON_POLAR(channelNum);
- } else {
- MCPWMx->MCCON_CLR = MCPWM_CON_POLAR(channelNum);
- }
-
- if (channelSetup->channelDeadtimeEnable /* == ENABLE */){
- MCPWMx->MCCON_SET = MCPWM_CON_DTE(channelNum);
- MCPWMx->MCDEADTIME &= ~(MCPWM_DT(channelNum, 0x3FF));
- MCPWMx->MCDEADTIME |= MCPWM_DT(channelNum, channelSetup->channelDeadtimeValue);
- } else {
- MCPWMx->MCCON_CLR = MCPWM_CON_DTE(channelNum);
- }
-
- if (channelSetup->channelUpdateEnable /* == ENABLE */){
- MCPWMx->MCCON_CLR = MCPWM_CON_DISUP(channelNum);
- } else {
- MCPWMx->MCCON_SET = MCPWM_CON_DISUP(channelNum);
- }
- }
-}
-
-
-/*********************************************************************//**
- * @brief Write to MCPWM shadow registers - Update the value for period
- * and pulse width in MCPWM peripheral.
- * @param[in] MCPWMx Motor Control PWM peripheral selected
- * Should be: LPC_MCPWM
- * @param[in] channelNum Channel Number, should be: 0..2.
- * @param[in] channelSetup Pointer to a MCPWM_CHANNEL_CFG_Type structure
-* that contains the configuration information for the
-* specified MCPWM channel.
- * @return None
- **********************************************************************/
-void MCPWM_WriteToShadow(LPC_MCPWM_TypeDef *MCPWMx, uint32_t channelNum,
- MCPWM_CHANNEL_CFG_Type *channelSetup)
-{
- if (channelNum == 0){
- MCPWMx->MCPER0 = channelSetup->channelPeriodValue;
- MCPWMx->MCPW0 = channelSetup->channelPulsewidthValue;
- } else if (channelNum == 1) {
- MCPWMx->MCPER1 = channelSetup->channelPeriodValue;
- MCPWMx->MCPW1 = channelSetup->channelPulsewidthValue;
- } else if (channelNum == 2) {
- MCPWMx->MCPER2 = channelSetup->channelPeriodValue;
- MCPWMx->MCPW2 = channelSetup->channelPulsewidthValue;
- }
-}
-
-
-
-/*********************************************************************//**
- * @brief Configures capture function in MCPWM peripheral
- * @param[in] MCPWMx Motor Control PWM peripheral selected
- * Should be: LPC_MCPWM
- * @param[in] channelNum MCI (Motor Control Input pin) number
- * Should be: 0..2
- * @param[in] captureConfig Pointer to a MCPWM_CAPTURE_CFG_Type structure
-* that contains the configuration information for the
-* specified MCPWM capture.
- * @return
- **********************************************************************/
-void MCPWM_ConfigCapture(LPC_MCPWM_TypeDef *MCPWMx, uint32_t channelNum,
- MCPWM_CAPTURE_CFG_Type *captureConfig)
-{
- if (channelNum <= 2) {
-
- if (captureConfig->captureFalling /* == ENABLE */) {
- MCPWMx->MCCAPCON_SET = MCPWM_CAPCON_CAPMCI_FE(captureConfig->captureChannel, channelNum);
- } else {
- MCPWMx->MCCAPCON_CLR = MCPWM_CAPCON_CAPMCI_FE(captureConfig->captureChannel, channelNum);
- }
-
- if (captureConfig->captureRising /* == ENABLE */) {
- MCPWMx->MCCAPCON_SET = MCPWM_CAPCON_CAPMCI_RE(captureConfig->captureChannel, channelNum);
- } else {
- MCPWMx->MCCAPCON_CLR = MCPWM_CAPCON_CAPMCI_RE(captureConfig->captureChannel, channelNum);
- }
-
- if (captureConfig->timerReset /* == ENABLE */){
- MCPWMx->MCCAPCON_SET = MCPWM_CAPCON_RT(captureConfig->captureChannel);
- } else {
- MCPWMx->MCCAPCON_CLR = MCPWM_CAPCON_RT(captureConfig->captureChannel);
- }
-
- if (captureConfig->hnfEnable /* == ENABLE */){
- MCPWMx->MCCAPCON_SET = MCPWM_CAPCON_HNFCAP(channelNum);
- } else {
- MCPWMx->MCCAPCON_CLR = MCPWM_CAPCON_HNFCAP(channelNum);
- }
- }
-}
-
-
-/*********************************************************************//**
- * @brief Clears current captured value in specified capture channel
- * @param[in] MCPWMx Motor Control PWM peripheral selected
- * Should be: LPC_MCPWM
- * @param[in] captureChannel Capture channel number, should be: 0..2
- * @return None
- **********************************************************************/
-void MCPWM_ClearCapture(LPC_MCPWM_TypeDef *MCPWMx, uint32_t captureChannel)
-{
- MCPWMx->MCCAP_CLR = MCPWM_CAPCLR_CAP(captureChannel);
-}
-
-/*********************************************************************//**
- * @brief Get current captured value in specified capture channel
- * @param[in] MCPWMx Motor Control PWM peripheral selected,
- * Should be: LPC_MCPWM
- * @param[in] captureChannel Capture channel number, should be: 0..2
- * @return None
- **********************************************************************/
-uint32_t MCPWM_GetCapture(LPC_MCPWM_TypeDef *MCPWMx, uint32_t captureChannel)
-{
- if (captureChannel == 0){
- return (MCPWMx->MCCR0);
- } else if (captureChannel == 1) {
- return (MCPWMx->MCCR1);
- } else if (captureChannel == 2) {
- return (MCPWMx->MCCR2);
- }
- return (0);
-}
-
-
-/*********************************************************************//**
- * @brief Configures Count control in MCPWM peripheral
- * @param[in] MCPWMx Motor Control PWM peripheral selected
- * Should be: LPC_MCPWM
- * @param[in] channelNum Channel number, should be: 0..2
- * @param[in] countMode Count mode, should be:
- * - ENABLE: Enables count mode.
- * - DISABLE: Disable count mode, the channel is in timer mode.
- * @param[in] countConfig Pointer to a MCPWM_COUNT_CFG_Type structure
-* that contains the configuration information for the
-* specified MCPWM count control.
- * @return None
- **********************************************************************/
-void MCPWM_CountConfig(LPC_MCPWM_TypeDef *MCPWMx, uint32_t channelNum,
- uint32_t countMode, MCPWM_COUNT_CFG_Type *countConfig)
-{
- if (channelNum <= 2) {
- if (countMode /* == ENABLE */){
- MCPWMx->MCCNTCON_SET = MCPWM_CNTCON_CNTR(channelNum);
- if (countConfig->countFalling /* == ENABLE */) {
- MCPWMx->MCCNTCON_SET = MCPWM_CNTCON_TCMCI_FE(countConfig->counterChannel,channelNum);
- } else {
- MCPWMx->MCCNTCON_CLR = MCPWM_CNTCON_TCMCI_FE(countConfig->counterChannel,channelNum);
- }
- if (countConfig->countRising /* == ENABLE */) {
- MCPWMx->MCCNTCON_SET = MCPWM_CNTCON_TCMCI_RE(countConfig->counterChannel,channelNum);
- } else {
- MCPWMx->MCCNTCON_CLR = MCPWM_CNTCON_TCMCI_RE(countConfig->counterChannel,channelNum);
- }
- } else {
- MCPWMx->MCCNTCON_CLR = MCPWM_CNTCON_CNTR(channelNum);
- }
- }
-}
-
-
-/*********************************************************************//**
- * @brief Start MCPWM activity for each MCPWM channel
- * @param[in] MCPWMx Motor Control PWM peripheral selected
- * Should be: LPC_MCPWM
- * @param[in] channel0 State of this command on channel 0:
- * - ENABLE: 'Start' command will effect on channel 0
- * - DISABLE: 'Start' command will not effect on channel 0
- * @param[in] channel1 State of this command on channel 1:
- * - ENABLE: 'Start' command will effect on channel 1
- * - DISABLE: 'Start' command will not effect on channel 1
- * @param[in] channel2 State of this command on channel 2:
- * - ENABLE: 'Start' command will effect on channel 2
- * - DISABLE: 'Start' command will not effect on channel 2
- * @return None
- **********************************************************************/
-void MCPWM_Start(LPC_MCPWM_TypeDef *MCPWMx, uint32_t channel0,
- uint32_t channel1, uint32_t channel2)
-{
- uint32_t regVal = 0;
- regVal = (channel0 ? MCPWM_CON_RUN(0) : 0) | (channel1 ? MCPWM_CON_RUN(1) : 0) \
- | (channel2 ? MCPWM_CON_RUN(2) : 0);
- MCPWMx->MCCON_SET = regVal;
-}
-
-
-/*********************************************************************//**
- * @brief Stop MCPWM activity for each MCPWM channel
- * @param[in] MCPWMx Motor Control PWM peripheral selected
- * Should be: LPC_MCPWM
- * @param[in] channel0 State of this command on channel 0:
- * - ENABLE: 'Stop' command will effect on channel 0
- * - DISABLE: 'Stop' command will not effect on channel 0
- * @param[in] channel1 State of this command on channel 1:
- * - ENABLE: 'Stop' command will effect on channel 1
- * - DISABLE: 'Stop' command will not effect on channel 1
- * @param[in] channel2 State of this command on channel 2:
- * - ENABLE: 'Stop' command will effect on channel 2
- * - DISABLE: 'Stop' command will not effect on channel 2
- * @return None
- **********************************************************************/
-void MCPWM_Stop(LPC_MCPWM_TypeDef *MCPWMx, uint32_t channel0,
- uint32_t channel1, uint32_t channel2)
-{
- uint32_t regVal = 0;
- regVal = (channel0 ? MCPWM_CON_RUN(0) : 0) | (channel1 ? MCPWM_CON_RUN(1) : 0) \
- | (channel2 ? MCPWM_CON_RUN(2) : 0);
- MCPWMx->MCCON_CLR = regVal;
-}
-
-
-/*********************************************************************//**
- * @brief Enables/Disables 3-phase AC motor mode on MCPWM peripheral
- * @param[in] MCPWMx Motor Control PWM peripheral selected
- * Should be: LPC_MCPWM
- * @param[in] acMode State of this command, should be:
- * - ENABLE.
- * - DISABLE.
- * @return None
- **********************************************************************/
-void MCPWM_ACMode(LPC_MCPWM_TypeDef *MCPWMx, uint32_t acMode)
-{
- if (acMode){
- MCPWMx->MCCON_SET = MCPWM_CON_ACMODE;
- } else {
- MCPWMx->MCCON_CLR = MCPWM_CON_ACMODE;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Enables/Disables 3-phase DC motor mode on MCPWM peripheral
- * @param[in] MCPWMx Motor Control PWM peripheral selected
- * Should be: LPC_MCPWM
- * @param[in] dcMode State of this command, should be:
- * - ENABLE.
- * - DISABLE.
- * @param[in] outputInvered Polarity of the MCOB outputs for all 3 channels,
- * should be:
- * - ENABLE: The MCOB outputs have opposite polarity
- * from the MCOA outputs.
- * - DISABLE: The MCOB outputs have the same basic
- * polarity as the MCOA outputs.
- * @param[in] outputPattern A value contains bits that enables/disables the specified
- * output pins route to the internal MCOA0 signal, should be:
- - MCPWM_PATENT_A0: MCOA0 tracks internal MCOA0
- - MCPWM_PATENT_B0: MCOB0 tracks internal MCOA0
- - MCPWM_PATENT_A1: MCOA1 tracks internal MCOA0
- - MCPWM_PATENT_B1: MCOB1 tracks internal MCOA0
- - MCPWM_PATENT_A2: MCOA2 tracks internal MCOA0
- - MCPWM_PATENT_B2: MCOB2 tracks internal MCOA0
- * @return None
- *
- * Note: all these outputPatent values above can be ORed together for using as input parameter.
- **********************************************************************/
-void MCPWM_DCMode(LPC_MCPWM_TypeDef *MCPWMx, uint32_t dcMode,
- uint32_t outputInvered, uint32_t outputPattern)
-{
- if (dcMode){
- MCPWMx->MCCON_SET = MCPWM_CON_DCMODE;
- } else {
- MCPWMx->MCCON_CLR = MCPWM_CON_DCMODE;
- }
-
- if (outputInvered) {
- MCPWMx->MCCON_SET = MCPWM_CON_INVBDC;
- } else {
- MCPWMx->MCCON_CLR = MCPWM_CON_INVBDC;
- }
-
- MCPWMx->MCCCP = outputPattern;
-}
-
-
-/*********************************************************************//**
- * @brief Configures the specified interrupt in MCPWM peripheral
- * @param[in] MCPWMx Motor Control PWM peripheral selected
- * Should be: LPC_MCPWM
- * @param[in] ulIntType Interrupt type, should be:
- * - MCPWM_INTFLAG_LIM0: Limit interrupt for channel (0)
- * - MCPWM_INTFLAG_MAT0: Match interrupt for channel (0)
- * - MCPWM_INTFLAG_CAP0: Capture interrupt for channel (0)
- * - MCPWM_INTFLAG_LIM1: Limit interrupt for channel (1)
- * - MCPWM_INTFLAG_MAT1: Match interrupt for channel (1)
- * - MCPWM_INTFLAG_CAP1: Capture interrupt for channel (1)
- * - MCPWM_INTFLAG_LIM2: Limit interrupt for channel (2)
- * - MCPWM_INTFLAG_MAT2: Match interrupt for channel (2)
- * - MCPWM_INTFLAG_CAP2: Capture interrupt for channel (2)
- * - MCPWM_INTFLAG_ABORT: Fast abort interrupt
- * @param[in] NewState New State of this command, should be:
- * - ENABLE.
- * - DISABLE.
- * @return None
- *
- * Note: all these ulIntType values above can be ORed together for using as input parameter.
- **********************************************************************/
-void MCPWM_IntConfig(LPC_MCPWM_TypeDef *MCPWMx, uint32_t ulIntType, FunctionalState NewState)
-{
- if (NewState) {
- MCPWMx->MCINTEN_SET = ulIntType;
- } else {
- MCPWMx->MCINTEN_CLR = ulIntType;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Sets/Forces the specified interrupt for MCPWM peripheral
- * @param[in] MCPWMx Motor Control PWM peripheral selected
- * Should be LPC_MCPWM
- * @param[in] ulIntType Interrupt type, should be:
- * - MCPWM_INTFLAG_LIM0: Limit interrupt for channel (0)
- * - MCPWM_INTFLAG_MAT0: Match interrupt for channel (0)
- * - MCPWM_INTFLAG_CAP0: Capture interrupt for channel (0)
- * - MCPWM_INTFLAG_LIM1: Limit interrupt for channel (1)
- * - MCPWM_INTFLAG_MAT1: Match interrupt for channel (1)
- * - MCPWM_INTFLAG_CAP1: Capture interrupt for channel (1)
- * - MCPWM_INTFLAG_LIM2: Limit interrupt for channel (2)
- * - MCPWM_INTFLAG_MAT2: Match interrupt for channel (2)
- * - MCPWM_INTFLAG_CAP2: Capture interrupt for channel (2)
- * - MCPWM_INTFLAG_ABORT: Fast abort interrupt
- * @return None
- * Note: all these ulIntType values above can be ORed together for using as input parameter.
- **********************************************************************/
-void MCPWM_IntSet(LPC_MCPWM_TypeDef *MCPWMx, uint32_t ulIntType)
-{
- MCPWMx->MCINTFLAG_SET = ulIntType;
-}
-
-
-/*********************************************************************//**
- * @brief Clear the specified interrupt pending for MCPWM peripheral
- * @param[in] MCPWMx Motor Control PWM peripheral selected,
- * should be: LPC_MCPWM
- * @param[in] ulIntType Interrupt type, should be:
- * - MCPWM_INTFLAG_LIM0: Limit interrupt for channel (0)
- * - MCPWM_INTFLAG_MAT0: Match interrupt for channel (0)
- * - MCPWM_INTFLAG_CAP0: Capture interrupt for channel (0)
- * - MCPWM_INTFLAG_LIM1: Limit interrupt for channel (1)
- * - MCPWM_INTFLAG_MAT1: Match interrupt for channel (1)
- * - MCPWM_INTFLAG_CAP1: Capture interrupt for channel (1)
- * - MCPWM_INTFLAG_LIM2: Limit interrupt for channel (2)
- * - MCPWM_INTFLAG_MAT2: Match interrupt for channel (2)
- * - MCPWM_INTFLAG_CAP2: Capture interrupt for channel (2)
- * - MCPWM_INTFLAG_ABORT: Fast abort interrupt
- * @return None
- * Note: all these ulIntType values above can be ORed together for using as input parameter.
- **********************************************************************/
-void MCPWM_IntClear(LPC_MCPWM_TypeDef *MCPWMx, uint32_t ulIntType)
-{
- MCPWMx->MCINTFLAG_CLR = ulIntType;
-}
-
-
-/*********************************************************************//**
- * @brief Check whether if the specified interrupt in MCPWM is set or not
- * @param[in] MCPWMx Motor Control PWM peripheral selected,
- * should be: LPC_MCPWM
- * @param[in] ulIntType Interrupt type, should be:
- * - MCPWM_INTFLAG_LIM0: Limit interrupt for channel (0)
- * - MCPWM_INTFLAG_MAT0: Match interrupt for channel (0)
- * - MCPWM_INTFLAG_CAP0: Capture interrupt for channel (0)
- * - MCPWM_INTFLAG_LIM1: Limit interrupt for channel (1)
- * - MCPWM_INTFLAG_MAT1: Match interrupt for channel (1)
- * - MCPWM_INTFLAG_CAP1: Capture interrupt for channel (1)
- * - MCPWM_INTFLAG_LIM2: Limit interrupt for channel (2)
- * - MCPWM_INTFLAG_MAT2: Match interrupt for channel (2)
- * - MCPWM_INTFLAG_CAP2: Capture interrupt for channel (2)
- * - MCPWM_INTFLAG_ABORT: Fast abort interrupt
- * @return None
- **********************************************************************/
-FlagStatus MCPWM_GetIntStatus(LPC_MCPWM_TypeDef *MCPWMx, uint32_t ulIntType)
-{
- return ((MCPWMx->MCINTFLAG & ulIntType) ? SET : RESET);
-}
-
-/**
- * @}
- */
-
-#endif /* _MCPWM */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_nvic.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_nvic.c
deleted file mode 100644
index 3e01e47902..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_nvic.c
+++ /dev/null
@@ -1,148 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_nvic.c 2010-05-21
-*//**
-* @file lpc17xx_nvic.c
-* @brief Contains all expansion functions support for
-* NVIC firmware library on LPC17xx. The main
-* NVIC functions are defined in core_cm3.h
-* @version 2.0
-* @date 21. May. 2010
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2010, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup NVIC
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_nvic.h"
-
-
-/* Private Macros ------------------------------------------------------------- */
-/** @addtogroup NVIC_Private_Macros
- * @{
- */
-
-/* Vector table offset bit mask */
-#define NVIC_VTOR_MASK 0x3FFFFF80
-
-/**
- * @}
- */
-
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup NVIC_Public_Functions
- * @{
- */
-
-
-/*****************************************************************************//**
- * @brief De-initializes the NVIC peripheral registers to their default
- * reset values.
- * @param None
- * @return None
- *
- * These following NVIC peripheral registers will be de-initialized:
- * - Disable Interrupt (32 IRQ interrupt sources that matched with LPC17xx)
- * - Clear all Pending Interrupts (32 IRQ interrupt source that matched with LPC17xx)
- * - Clear all Interrupt Priorities (32 IRQ interrupt source that matched with LPC17xx)
- *******************************************************************************/
-void NVIC_DeInit(void)
-{
- uint8_t tmp;
-
- /* Disable all interrupts */
- NVIC->ICER[0] = 0xFFFFFFFF;
- NVIC->ICER[1] = 0x00000001;
- /* Clear all pending interrupts */
- NVIC->ICPR[0] = 0xFFFFFFFF;
- NVIC->ICPR[1] = 0x00000001;
-
- /* Clear all interrupt priority */
- for (tmp = 0; tmp < 32; tmp++) {
- NVIC->IP[tmp] = 0x00;
- }
-}
-
-/*****************************************************************************//**
- * @brief De-initializes the SCB peripheral registers to their default
- * reset values.
- * @param none
- * @return none
- *
- * These following SCB NVIC peripheral registers will be de-initialized:
- * - Interrupt Control State register
- * - Interrupt Vector Table Offset register
- * - Application Interrupt/Reset Control register
- * - System Control register
- * - Configuration Control register
- * - System Handlers Priority Registers
- * - System Handler Control and State Register
- * - Configurable Fault Status Register
- * - Hard Fault Status Register
- * - Debug Fault Status Register
- *******************************************************************************/
-void NVIC_SCBDeInit(void)
-{
- uint8_t tmp;
-
- SCB->ICSR = 0x0A000000;
- SCB->VTOR = 0x00000000;
- SCB->AIRCR = 0x05FA0000;
- SCB->SCR = 0x00000000;
- SCB->CCR = 0x00000000;
-
- for (tmp = 0; tmp < (sizeof(SCB->SHP) / sizeof(SCB->SHP[0])); tmp++) {
- SCB->SHP[tmp] = 0x00;
- }
-
- SCB->SHCSR = 0x00000000;
- SCB->CFSR = 0xFFFFFFFF;
- SCB->HFSR = 0xFFFFFFFF;
- SCB->DFSR = 0xFFFFFFFF;
-}
-
-
-/*****************************************************************************//**
- * @brief Set Vector Table Offset value
- * @param offset Offset value
- * @return None
- *******************************************************************************/
-void NVIC_SetVTOR(uint32_t offset)
-{
-// SCB->VTOR = (offset & NVIC_VTOR_MASK);
- SCB->VTOR = offset;
-}
-
-/**
- * @}
- */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_pinsel.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_pinsel.c
deleted file mode 100644
index 740c1eae99..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_pinsel.c
+++ /dev/null
@@ -1,318 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_pinsel.c 2010-05-21
-*//**
-* @file lpc17xx_pinsel.c
-* @brief Contains all functions support for Pin connect block firmware
-* library on LPC17xx
-* @version 2.0
-* @date 21. May. 2010
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2010, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup PINSEL
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_pinsel.h"
-
-/* Public Functions ----------------------------------------------------------- */
-
-static void set_PinFunc ( uint8_t portnum, uint8_t pinnum, uint8_t funcnum);
-static void set_ResistorMode ( uint8_t portnum, uint8_t pinnum, uint8_t modenum);
-static void set_OpenDrainMode( uint8_t portnum, uint8_t pinnum, uint8_t modenum);
-
-/*********************************************************************//**
- * @brief Setup the pin selection function
- * @param[in] portnum PORT number,
- * should be one of the following:
- * - PINSEL_PORT_0 : Port 0
- * - PINSEL_PORT_1 : Port 1
- * - PINSEL_PORT_2 : Port 2
- * - PINSEL_PORT_3 : Port 3
- *
- * @param[in] pinnum Pin number,
- * should be one of the following:
- - PINSEL_PIN_0 : Pin 0
- - PINSEL_PIN_1 : Pin 1
- - PINSEL_PIN_2 : Pin 2
- - PINSEL_PIN_3 : Pin 3
- - PINSEL_PIN_4 : Pin 4
- - PINSEL_PIN_5 : Pin 5
- - PINSEL_PIN_6 : Pin 6
- - PINSEL_PIN_7 : Pin 7
- - PINSEL_PIN_8 : Pin 8
- - PINSEL_PIN_9 : Pin 9
- - PINSEL_PIN_10 : Pin 10
- - PINSEL_PIN_11 : Pin 11
- - PINSEL_PIN_12 : Pin 12
- - PINSEL_PIN_13 : Pin 13
- - PINSEL_PIN_14 : Pin 14
- - PINSEL_PIN_15 : Pin 15
- - PINSEL_PIN_16 : Pin 16
- - PINSEL_PIN_17 : Pin 17
- - PINSEL_PIN_18 : Pin 18
- - PINSEL_PIN_19 : Pin 19
- - PINSEL_PIN_20 : Pin 20
- - PINSEL_PIN_21 : Pin 21
- - PINSEL_PIN_22 : Pin 22
- - PINSEL_PIN_23 : Pin 23
- - PINSEL_PIN_24 : Pin 24
- - PINSEL_PIN_25 : Pin 25
- - PINSEL_PIN_26 : Pin 26
- - PINSEL_PIN_27 : Pin 27
- - PINSEL_PIN_28 : Pin 28
- - PINSEL_PIN_29 : Pin 29
- - PINSEL_PIN_30 : Pin 30
- - PINSEL_PIN_31 : Pin 31
-
- * @param[in] funcnum Function number,
- * should be one of the following:
- * - PINSEL_FUNC_0 : default function
- * - PINSEL_FUNC_1 : first alternate function
- * - PINSEL_FUNC_2 : second alternate function
- * - PINSEL_FUNC_3 : third alternate function
- *
- * @return None
- **********************************************************************/
-static void set_PinFunc ( uint8_t portnum, uint8_t pinnum, uint8_t funcnum)
-{
- uint32_t pinnum_t = pinnum;
- uint32_t pinselreg_idx = 2 * portnum;
- uint32_t *pPinCon = (uint32_t *)&LPC_PINCON->PINSEL0;
-
- if (pinnum_t >= 16) {
- pinnum_t -= 16;
- pinselreg_idx++;
- }
- *(uint32_t *)(pPinCon + pinselreg_idx) &= ~(0x03UL << (pinnum_t * 2));
- *(uint32_t *)(pPinCon + pinselreg_idx) |= ((uint32_t)funcnum) << (pinnum_t * 2);
-}
-
-/*********************************************************************//**
- * @brief Setup resistor mode for each pin
- * @param[in] portnum PORT number,
- * should be one of the following:
- * - PINSEL_PORT_0 : Port 0
- * - PINSEL_PORT_1 : Port 1
- * - PINSEL_PORT_2 : Port 2
- * - PINSEL_PORT_3 : Port 3
- * @param[in] pinnum Pin number,
- * should be one of the following:
- - PINSEL_PIN_0 : Pin 0
- - PINSEL_PIN_1 : Pin 1
- - PINSEL_PIN_2 : Pin 2
- - PINSEL_PIN_3 : Pin 3
- - PINSEL_PIN_4 : Pin 4
- - PINSEL_PIN_5 : Pin 5
- - PINSEL_PIN_6 : Pin 6
- - PINSEL_PIN_7 : Pin 7
- - PINSEL_PIN_8 : Pin 8
- - PINSEL_PIN_9 : Pin 9
- - PINSEL_PIN_10 : Pin 10
- - PINSEL_PIN_11 : Pin 11
- - PINSEL_PIN_12 : Pin 12
- - PINSEL_PIN_13 : Pin 13
- - PINSEL_PIN_14 : Pin 14
- - PINSEL_PIN_15 : Pin 15
- - PINSEL_PIN_16 : Pin 16
- - PINSEL_PIN_17 : Pin 17
- - PINSEL_PIN_18 : Pin 18
- - PINSEL_PIN_19 : Pin 19
- - PINSEL_PIN_20 : Pin 20
- - PINSEL_PIN_21 : Pin 21
- - PINSEL_PIN_22 : Pin 22
- - PINSEL_PIN_23 : Pin 23
- - PINSEL_PIN_24 : Pin 24
- - PINSEL_PIN_25 : Pin 25
- - PINSEL_PIN_26 : Pin 26
- - PINSEL_PIN_27 : Pin 27
- - PINSEL_PIN_28 : Pin 28
- - PINSEL_PIN_29 : Pin 29
- - PINSEL_PIN_30 : Pin 30
- - PINSEL_PIN_31 : Pin 31
-
- * @param[in] modenum: Mode number,
- * should be one of the following:
- - PINSEL_PINMODE_PULLUP : Internal pull-up resistor
- - PINSEL_PINMODE_TRISTATE : Tri-state
- - PINSEL_PINMODE_PULLDOWN : Internal pull-down resistor
-
- * @return None
- **********************************************************************/
-void set_ResistorMode ( uint8_t portnum, uint8_t pinnum, uint8_t modenum)
-{
- uint32_t pinnum_t = pinnum;
- uint32_t pinmodereg_idx = 2 * portnum;
- uint32_t *pPinCon = (uint32_t *)&LPC_PINCON->PINMODE0;
-
- if (pinnum_t >= 16) {
- pinnum_t -= 16;
- pinmodereg_idx++ ;
- }
-
- *(uint32_t *)(pPinCon + pinmodereg_idx) &= ~(0x03UL << (pinnum_t * 2));
- *(uint32_t *)(pPinCon + pinmodereg_idx) |= ((uint32_t)modenum) << (pinnum_t * 2);
-}
-
-/*********************************************************************//**
- * @brief Setup Open drain mode for each pin
- * @param[in] portnum PORT number,
- * should be one of the following:
- * - PINSEL_PORT_0 : Port 0
- * - PINSEL_PORT_1 : Port 1
- * - PINSEL_PORT_2 : Port 2
- * - PINSEL_PORT_3 : Port 3
- *
- * @param[in] pinnum Pin number,
- * should be one of the following:
- - PINSEL_PIN_0 : Pin 0
- - PINSEL_PIN_1 : Pin 1
- - PINSEL_PIN_2 : Pin 2
- - PINSEL_PIN_3 : Pin 3
- - PINSEL_PIN_4 : Pin 4
- - PINSEL_PIN_5 : Pin 5
- - PINSEL_PIN_6 : Pin 6
- - PINSEL_PIN_7 : Pin 7
- - PINSEL_PIN_8 : Pin 8
- - PINSEL_PIN_9 : Pin 9
- - PINSEL_PIN_10 : Pin 10
- - PINSEL_PIN_11 : Pin 11
- - PINSEL_PIN_12 : Pin 12
- - PINSEL_PIN_13 : Pin 13
- - PINSEL_PIN_14 : Pin 14
- - PINSEL_PIN_15 : Pin 15
- - PINSEL_PIN_16 : Pin 16
- - PINSEL_PIN_17 : Pin 17
- - PINSEL_PIN_18 : Pin 18
- - PINSEL_PIN_19 : Pin 19
- - PINSEL_PIN_20 : Pin 20
- - PINSEL_PIN_21 : Pin 21
- - PINSEL_PIN_22 : Pin 22
- - PINSEL_PIN_23 : Pin 23
- - PINSEL_PIN_24 : Pin 24
- - PINSEL_PIN_25 : Pin 25
- - PINSEL_PIN_26 : Pin 26
- - PINSEL_PIN_27 : Pin 27
- - PINSEL_PIN_28 : Pin 28
- - PINSEL_PIN_29 : Pin 29
- - PINSEL_PIN_30 : Pin 30
- - PINSEL_PIN_31 : Pin 31
-
- * @param[in] modenum Open drain mode number,
- * should be one of the following:
- * - PINSEL_PINMODE_NORMAL : Pin is in the normal (not open drain) mode
- * - PINSEL_PINMODE_OPENDRAIN : Pin is in the open drain mode
- *
- * @return None
- **********************************************************************/
-void set_OpenDrainMode( uint8_t portnum, uint8_t pinnum, uint8_t modenum)
-{
- uint32_t *pPinCon = (uint32_t *)&LPC_PINCON->PINMODE_OD0;
-
- if (modenum == PINSEL_PINMODE_OPENDRAIN){
- *(uint32_t *)(pPinCon + portnum) |= (0x01UL << pinnum);
- } else {
- *(uint32_t *)(pPinCon + portnum) &= ~(0x01UL << pinnum);
- }
-}
-
-/* End of Public Functions ---------------------------------------------------- */
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup PINSEL_Public_Functions
- * @{
- */
-/*********************************************************************//**
- * @brief Configure trace function
- * @param[in] NewState State of the Trace function configuration,
- * should be one of the following:
- * - ENABLE : Enable Trace Function
- * - DISABLE : Disable Trace Function
- *
- * @return None
- **********************************************************************/
-void PINSEL_ConfigTraceFunc(FunctionalState NewState)
-{
- if (NewState == ENABLE) {
- LPC_PINCON->PINSEL10 |= (0x01UL << 3);
- } else if (NewState == DISABLE) {
- LPC_PINCON->PINSEL10 &= ~(0x01UL << 3);
- }
-}
-
-/*********************************************************************//**
- * @brief Setup I2C0 pins
- * @param[in] i2cPinMode I2C pin mode,
- * should be one of the following:
- * - PINSEL_I2C_Normal_Mode : The standard drive mode
- * - PINSEL_I2C_Fast_Mode : Fast Mode Plus drive mode
- *
- * @param[in] filterSlewRateEnable should be:
- * - ENABLE: Enable filter and slew rate.
- * - DISABLE: Disable filter and slew rate.
- *
- * @return None
- **********************************************************************/
-void PINSEL_SetI2C0Pins(uint8_t i2cPinMode, FunctionalState filterSlewRateEnable)
-{
- uint32_t regVal;
-
- if (i2cPinMode == PINSEL_I2C_Fast_Mode){
- regVal = PINSEL_I2CPADCFG_SCLDRV0 | PINSEL_I2CPADCFG_SDADRV0;
- }
-
- if (filterSlewRateEnable == DISABLE){
- regVal = PINSEL_I2CPADCFG_SCLI2C0 | PINSEL_I2CPADCFG_SDAI2C0;
- }
- LPC_PINCON->I2CPADCFG = regVal;
-}
-
-
-/*********************************************************************//**
- * @brief Configure Pin corresponding to specified parameters passed
- * in the PinCfg
- * @param[in] PinCfg Pointer to a PINSEL_CFG_Type structure
- * that contains the configuration information for the
- * specified pin.
- * @return None
- **********************************************************************/
-void PINSEL_ConfigPin(PINSEL_CFG_Type *PinCfg)
-{
- set_PinFunc(PinCfg->Portnum, PinCfg->Pinnum, PinCfg->Funcnum);
- set_ResistorMode(PinCfg->Portnum, PinCfg->Pinnum, PinCfg->Pinmode);
- set_OpenDrainMode(PinCfg->Portnum, PinCfg->Pinnum, PinCfg->OpenDrain);
-}
-
-
-/**
- * @}
- */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_pwm.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_pwm.c
deleted file mode 100644
index f52aa5ce85..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_pwm.c
+++ /dev/null
@@ -1,588 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_pwm.c 2011-03-31
-*//**
-* @file lpc17xx_pwm.c
-* @brief Contains all functions support for PWM firmware library on LPC17xx
-* @version 2.1
-* @date 31. Mar. 2011
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2011, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup PWM
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_pwm.h"
-#include "lpc17xx_clkpwr.h"
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-
-#ifdef _PWM
-
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup PWM_Public_Functions
- * @{
- */
-
-
-/*********************************************************************//**
- * @brief Check whether specified interrupt flag in PWM is set or not
- * @param[in] PWMx: PWM peripheral, should be LPC_PWM1
- * @param[in] IntFlag: PWM interrupt flag, should be:
- * - PWM_INTSTAT_MR0: Interrupt flag for PWM match channel 0
- * - PWM_INTSTAT_MR1: Interrupt flag for PWM match channel 1
- * - PWM_INTSTAT_MR2: Interrupt flag for PWM match channel 2
- * - PWM_INTSTAT_MR3: Interrupt flag for PWM match channel 3
- * - PWM_INTSTAT_MR4: Interrupt flag for PWM match channel 4
- * - PWM_INTSTAT_MR5: Interrupt flag for PWM match channel 5
- * - PWM_INTSTAT_MR6: Interrupt flag for PWM match channel 6
- * - PWM_INTSTAT_CAP0: Interrupt flag for capture input 0
- * - PWM_INTSTAT_CAP1: Interrupt flag for capture input 1
- * @return New State of PWM interrupt flag (SET or RESET)
- **********************************************************************/
-IntStatus PWM_GetIntStatus(LPC_PWM_TypeDef *PWMx, uint32_t IntFlag)
-{
- CHECK_PARAM(PARAM_PWMx(PWMx));
- CHECK_PARAM(PARAM_PWM_INTSTAT(IntFlag));
-
- return ((PWMx->IR & IntFlag) ? SET : RESET);
-}
-
-
-
-/*********************************************************************//**
- * @brief Clear specified PWM Interrupt pending
- * @param[in] PWMx: PWM peripheral, should be LPC_PWM1
- * @param[in] IntFlag: PWM interrupt flag, should be:
- * - PWM_INTSTAT_MR0: Interrupt flag for PWM match channel 0
- * - PWM_INTSTAT_MR1: Interrupt flag for PWM match channel 1
- * - PWM_INTSTAT_MR2: Interrupt flag for PWM match channel 2
- * - PWM_INTSTAT_MR3: Interrupt flag for PWM match channel 3
- * - PWM_INTSTAT_MR4: Interrupt flag for PWM match channel 4
- * - PWM_INTSTAT_MR5: Interrupt flag for PWM match channel 5
- * - PWM_INTSTAT_MR6: Interrupt flag for PWM match channel 6
- * - PWM_INTSTAT_CAP0: Interrupt flag for capture input 0
- * - PWM_INTSTAT_CAP1: Interrupt flag for capture input 1
- * @return None
- **********************************************************************/
-void PWM_ClearIntPending(LPC_PWM_TypeDef *PWMx, uint32_t IntFlag)
-{
- CHECK_PARAM(PARAM_PWMx(PWMx));
- CHECK_PARAM(PARAM_PWM_INTSTAT(IntFlag));
- PWMx->IR = IntFlag;
-}
-
-
-
-/*****************************************************************************//**
-* @brief Fills each PWM_InitStruct member with its default value:
-* - If PWMCounterMode = PWM_MODE_TIMER:
-* + PrescaleOption = PWM_TIMER_PRESCALE_USVAL
-* + PrescaleValue = 1
-* - If PWMCounterMode = PWM_MODE_COUNTER:
-* + CountInputSelect = PWM_COUNTER_PCAP1_0
-* + CounterOption = PWM_COUNTER_RISING
-* @param[in] PWMTimerCounterMode Timer or Counter mode, should be:
-* - PWM_MODE_TIMER: Counter of PWM peripheral is in Timer mode
-* - PWM_MODE_COUNTER: Counter of PWM peripheral is in Counter mode
-* @param[in] PWM_InitStruct Pointer to structure (PWM_TIMERCFG_Type or
-* PWM_COUNTERCFG_Type) which will be initialized.
-* @return None
-* Note: PWM_InitStruct pointer will be assigned to corresponding structure
-* (PWM_TIMERCFG_Type or PWM_COUNTERCFG_Type) due to PWMTimerCounterMode.
-*******************************************************************************/
-void PWM_ConfigStructInit(uint8_t PWMTimerCounterMode, void *PWM_InitStruct)
-{
- PWM_TIMERCFG_Type *pTimeCfg;
- PWM_COUNTERCFG_Type *pCounterCfg;
- CHECK_PARAM(PARAM_PWM_TC_MODE(PWMTimerCounterMode));
-
- pTimeCfg = (PWM_TIMERCFG_Type *) PWM_InitStruct;
- pCounterCfg = (PWM_COUNTERCFG_Type *) PWM_InitStruct;
-
- if (PWMTimerCounterMode == PWM_MODE_TIMER )
- {
- pTimeCfg->PrescaleOption = PWM_TIMER_PRESCALE_USVAL;
- pTimeCfg->PrescaleValue = 1;
- }
- else if (PWMTimerCounterMode == PWM_MODE_COUNTER)
- {
- pCounterCfg->CountInputSelect = PWM_COUNTER_PCAP1_0;
- pCounterCfg->CounterOption = PWM_COUNTER_RISING;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Initializes the PWMx peripheral corresponding to the specified
- * parameters in the PWM_ConfigStruct.
- * @param[in] PWMx PWM peripheral, should be LPC_PWM1
- * @param[in] PWMTimerCounterMode Timer or Counter mode, should be:
- * - PWM_MODE_TIMER: Counter of PWM peripheral is in Timer mode
- * - PWM_MODE_COUNTER: Counter of PWM peripheral is in Counter mode
- * @param[in] PWM_ConfigStruct Pointer to structure (PWM_TIMERCFG_Type or
- * PWM_COUNTERCFG_Type) which will be initialized.
- * @return None
- * Note: PWM_ConfigStruct pointer will be assigned to corresponding structure
- * (PWM_TIMERCFG_Type or PWM_COUNTERCFG_Type) due to PWMTimerCounterMode.
- **********************************************************************/
-void PWM_Init(LPC_PWM_TypeDef *PWMx, uint32_t PWMTimerCounterMode, void *PWM_ConfigStruct)
-{
- PWM_TIMERCFG_Type *pTimeCfg;
- PWM_COUNTERCFG_Type *pCounterCfg;
- uint64_t clkdlycnt;
-
- CHECK_PARAM(PARAM_PWMx(PWMx));
- CHECK_PARAM(PARAM_PWM_TC_MODE(PWMTimerCounterMode));
-
- pTimeCfg = (PWM_TIMERCFG_Type *)PWM_ConfigStruct;
- pCounterCfg = (PWM_COUNTERCFG_Type *)PWM_ConfigStruct;
-
-
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCPWM1, ENABLE);
- CLKPWR_SetPCLKDiv (CLKPWR_PCLKSEL_PWM1, CLKPWR_PCLKSEL_CCLK_DIV_4);
- // Get peripheral clock of PWM1
- clkdlycnt = (uint64_t) CLKPWR_GetPCLK (CLKPWR_PCLKSEL_PWM1);
-
-
- // Clear all interrupts pending
- PWMx->IR = 0xFF & PWM_IR_BITMASK;
- PWMx->TCR = 0x00;
- PWMx->CTCR = 0x00;
- PWMx->MCR = 0x00;
- PWMx->CCR = 0x00;
- PWMx->PCR = 0x00;
- PWMx->LER = 0x00;
-
- if (PWMTimerCounterMode == PWM_MODE_TIMER)
- {
- CHECK_PARAM(PARAM_PWM_TIMER_PRESCALE(pTimeCfg->PrescaleOption));
-
- /* Absolute prescale value */
- if (pTimeCfg->PrescaleOption == PWM_TIMER_PRESCALE_TICKVAL)
- {
- PWMx->PR = pTimeCfg->PrescaleValue - 1;
- }
- /* uSecond prescale value */
- else
- {
- clkdlycnt = (clkdlycnt * pTimeCfg->PrescaleValue) / 1000000;
- PWMx->PR = ((uint32_t) clkdlycnt) - 1;
- }
-
- }
- else if (PWMTimerCounterMode == PWM_MODE_COUNTER)
- {
- CHECK_PARAM(PARAM_PWM_COUNTER_INPUTSEL(pCounterCfg->CountInputSelect));
- CHECK_PARAM(PARAM_PWM_COUNTER_EDGE(pCounterCfg->CounterOption));
-
- PWMx->CTCR |= (PWM_CTCR_MODE((uint32_t)pCounterCfg->CounterOption)) \
- | (PWM_CTCR_SELECT_INPUT((uint32_t)pCounterCfg->CountInputSelect));
- }
-}
-
-/*********************************************************************//**
- * @brief De-initializes the PWM peripheral registers to their
-* default reset values.
- * @param[in] PWMx PWM peripheral selected, should be LPC_PWM1
- * @return None
- **********************************************************************/
-void PWM_DeInit (LPC_PWM_TypeDef *PWMx)
-{
- CHECK_PARAM(PARAM_PWMx(PWMx));
-
- // Disable PWM control (timer, counter and PWM)
- PWMx->TCR = 0x00;
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCPWM1, DISABLE);
-
-}
-
-
-/*********************************************************************//**
- * @brief Enable/Disable PWM peripheral
- * @param[in] PWMx PWM peripheral selected, should be LPC_PWM1
- * @param[in] NewState New State of this function, should be:
- * - ENABLE: Enable PWM peripheral
- * - DISABLE: Disable PWM peripheral
- * @return None
- **********************************************************************/
-void PWM_Cmd(LPC_PWM_TypeDef *PWMx, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_PWMx(PWMx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- if (NewState == ENABLE)
- {
- PWMx->TCR |= PWM_TCR_PWM_ENABLE;
- }
- else
- {
- PWMx->TCR &= (~PWM_TCR_PWM_ENABLE) & PWM_TCR_BITMASK;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Enable/Disable Counter in PWM peripheral
- * @param[in] PWMx PWM peripheral selected, should be LPC_PWM1
- * @param[in] NewState New State of this function, should be:
- * - ENABLE: Enable Counter in PWM peripheral
- * - DISABLE: Disable Counter in PWM peripheral
- * @return None
- **********************************************************************/
-void PWM_CounterCmd(LPC_PWM_TypeDef *PWMx, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_PWMx(PWMx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
- if (NewState == ENABLE)
- {
- PWMx->TCR |= PWM_TCR_COUNTER_ENABLE;
- }
- else
- {
- PWMx->TCR &= (~PWM_TCR_COUNTER_ENABLE) & PWM_TCR_BITMASK;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Reset Counter in PWM peripheral
- * @param[in] PWMx PWM peripheral selected, should be LPC_PWM1
- * @return None
- **********************************************************************/
-void PWM_ResetCounter(LPC_PWM_TypeDef *PWMx)
-{
- CHECK_PARAM(PARAM_PWMx(PWMx));
- PWMx->TCR |= PWM_TCR_COUNTER_RESET;
- PWMx->TCR &= (~PWM_TCR_COUNTER_RESET) & PWM_TCR_BITMASK;
-}
-
-
-/*********************************************************************//**
- * @brief Configures match for PWM peripheral
- * @param[in] PWMx PWM peripheral selected, should be LPC_PWM1
- * @param[in] PWM_MatchConfigStruct Pointer to a PWM_MATCHCFG_Type structure
-* that contains the configuration information for the
-* specified PWM match function.
- * @return None
- **********************************************************************/
-void PWM_ConfigMatch(LPC_PWM_TypeDef *PWMx, PWM_MATCHCFG_Type *PWM_MatchConfigStruct)
-{
- CHECK_PARAM(PARAM_PWMx(PWMx));
- CHECK_PARAM(PARAM_PWM1_MATCH_CHANNEL(PWM_MatchConfigStruct->MatchChannel));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(PWM_MatchConfigStruct->IntOnMatch));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(PWM_MatchConfigStruct->ResetOnMatch));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(PWM_MatchConfigStruct->StopOnMatch));
-
- //interrupt on MRn
- if (PWM_MatchConfigStruct->IntOnMatch == ENABLE)
- {
- PWMx->MCR |= PWM_MCR_INT_ON_MATCH(PWM_MatchConfigStruct->MatchChannel);
- }
- else
- {
- PWMx->MCR &= (~PWM_MCR_INT_ON_MATCH(PWM_MatchConfigStruct->MatchChannel)) \
- & PWM_MCR_BITMASK;
- }
-
- //reset on MRn
- if (PWM_MatchConfigStruct->ResetOnMatch == ENABLE)
- {
- PWMx->MCR |= PWM_MCR_RESET_ON_MATCH(PWM_MatchConfigStruct->MatchChannel);
- }
- else
- {
- PWMx->MCR &= (~PWM_MCR_RESET_ON_MATCH(PWM_MatchConfigStruct->MatchChannel)) \
- & PWM_MCR_BITMASK;
- }
-
- //stop on MRn
- if (PWM_MatchConfigStruct->StopOnMatch == ENABLE)
- {
- PWMx->MCR |= PWM_MCR_STOP_ON_MATCH(PWM_MatchConfigStruct->MatchChannel);
- }
- else
- {
- PWMx->MCR &= (~PWM_MCR_STOP_ON_MATCH(PWM_MatchConfigStruct->MatchChannel)) \
- & PWM_MCR_BITMASK;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Configures capture input for PWM peripheral
- * @param[in] PWMx PWM peripheral selected, should be LPC_PWM1
- * @param[in] PWM_CaptureConfigStruct Pointer to a PWM_CAPTURECFG_Type structure
-* that contains the configuration information for the
-* specified PWM capture input function.
- * @return None
- **********************************************************************/
-void PWM_ConfigCapture(LPC_PWM_TypeDef *PWMx, PWM_CAPTURECFG_Type *PWM_CaptureConfigStruct)
-{
- CHECK_PARAM(PARAM_PWMx(PWMx));
- CHECK_PARAM(PARAM_PWM1_CAPTURE_CHANNEL(PWM_CaptureConfigStruct->CaptureChannel));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(PWM_CaptureConfigStruct->FallingEdge));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(PWM_CaptureConfigStruct->IntOnCaption));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(PWM_CaptureConfigStruct->RisingEdge));
-
- if (PWM_CaptureConfigStruct->RisingEdge == ENABLE)
- {
- PWMx->CCR |= PWM_CCR_CAP_RISING(PWM_CaptureConfigStruct->CaptureChannel);
- }
- else
- {
- PWMx->CCR &= (~PWM_CCR_CAP_RISING(PWM_CaptureConfigStruct->CaptureChannel)) \
- & PWM_CCR_BITMASK;
- }
-
- if (PWM_CaptureConfigStruct->FallingEdge == ENABLE)
- {
- PWMx->CCR |= PWM_CCR_CAP_FALLING(PWM_CaptureConfigStruct->CaptureChannel);
- }
- else
- {
- PWMx->CCR &= (~PWM_CCR_CAP_FALLING(PWM_CaptureConfigStruct->CaptureChannel)) \
- & PWM_CCR_BITMASK;
- }
-
- if (PWM_CaptureConfigStruct->IntOnCaption == ENABLE)
- {
- PWMx->CCR |= PWM_CCR_INT_ON_CAP(PWM_CaptureConfigStruct->CaptureChannel);
- }
- else
- {
- PWMx->CCR &= (~PWM_CCR_INT_ON_CAP(PWM_CaptureConfigStruct->CaptureChannel)) \
- & PWM_CCR_BITMASK;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Read value of capture register PWM peripheral
- * @param[in] PWMx PWM peripheral selected, should be LPC_PWM1
- * @param[in] CaptureChannel: capture channel number, should be in
- * range 0 to 1
- * @return Value of capture register
- **********************************************************************/
-uint32_t PWM_GetCaptureValue(LPC_PWM_TypeDef *PWMx, uint8_t CaptureChannel)
-{
- CHECK_PARAM(PARAM_PWMx(PWMx));
- CHECK_PARAM(PARAM_PWM1_CAPTURE_CHANNEL(CaptureChannel));
-
- switch (CaptureChannel)
- {
- case 0:
- return PWMx->CR0;
-
- case 1:
- return PWMx->CR1;
-
- default:
- return (0);
- }
-}
-
-
-/********************************************************************//**
- * @brief Update value for each PWM channel with update type option
- * @param[in] PWMx PWM peripheral selected, should be LPC_PWM1
- * @param[in] MatchChannel Match channel
- * @param[in] MatchValue Match value
- * @param[in] UpdateType Type of Update, should be:
- * - PWM_MATCH_UPDATE_NOW: The update value will be updated for
- * this channel immediately
- * - PWM_MATCH_UPDATE_NEXT_RST: The update value will be updated for
- * this channel on next reset by a PWM Match event.
- * @return None
- *********************************************************************/
-void PWM_MatchUpdate(LPC_PWM_TypeDef *PWMx, uint8_t MatchChannel, \
- uint32_t MatchValue, uint8_t UpdateType)
-{
- CHECK_PARAM(PARAM_PWMx(PWMx));
- CHECK_PARAM(PARAM_PWM1_MATCH_CHANNEL(MatchChannel));
- CHECK_PARAM(PARAM_PWM_MATCH_UPDATE(UpdateType));
-
- switch (MatchChannel)
- {
- case 0:
- PWMx->MR0 = MatchValue;
- break;
-
- case 1:
- PWMx->MR1 = MatchValue;
- break;
-
- case 2:
- PWMx->MR2 = MatchValue;
- break;
-
- case 3:
- PWMx->MR3 = MatchValue;
- break;
-
- case 4:
- PWMx->MR4 = MatchValue;
- break;
-
- case 5:
- PWMx->MR5 = MatchValue;
- break;
-
- case 6:
- PWMx->MR6 = MatchValue;
- break;
- }
-
- // Write Latch register
- PWMx->LER |= PWM_LER_EN_MATCHn_LATCH(MatchChannel);
-
- // In case of update now
- if (UpdateType == PWM_MATCH_UPDATE_NOW)
- {
- PWMx->TCR |= PWM_TCR_COUNTER_RESET;
- PWMx->TCR &= (~PWM_TCR_COUNTER_RESET) & PWM_TCR_BITMASK;
- }
-}
-
-/********************************************************************//**
- * @brief Update value for multi PWM channel with update type option
- * at the same time
- * @param[in] PWMx PWM peripheral selected, should be LPC_PWM1
- * @param[in] MatchStruct Structure that contents match value of 7 pwm channels
- * @param[in] UpdateType Type of Update, should be:
- * - PWM_MATCH_UPDATE_NOW: The update value will be updated for
- * this channel immediately
- * - PWM_MATCH_UPDATE_NEXT_RST: The update value will be updated for
- * this channel on next reset by a PWM Match event.
- * @return None
- *********************************************************************/
-void PWM_MultiMatchUpdate(LPC_PWM_TypeDef *PWMx, PWM_Match_T *MatchStruct , uint8_t UpdateType)
-{
- uint8_t LatchValue = 0;
- uint8_t i;
-
- CHECK_PARAM(PARAM_PWMx(PWMx));
- CHECK_PARAM(PARAM_PWM_MATCH_UPDATE(UpdateType));
-
- //Update match value
- for(i=0;i<7;i++)
- {
- if(MatchStruct[i].Status == SET)
- {
- if(i<4)
- *((volatile unsigned int *)(&(PWMx->MR0) + i)) = MatchStruct[i].Matchvalue;
- else
- {
- *((volatile unsigned int *)(&(PWMx->MR4) + (i-4))) = MatchStruct[i].Matchvalue;
- }
- LatchValue |=(1<LER = LatchValue;
-
- // In case of update now
- if (UpdateType == PWM_MATCH_UPDATE_NOW)
- {
- PWMx->TCR |= PWM_TCR_COUNTER_RESET;
- PWMx->TCR &= (~PWM_TCR_COUNTER_RESET) & PWM_TCR_BITMASK;
- }
-}
-/********************************************************************//**
- * @brief Configure Edge mode for each PWM channel
- * @param[in] PWMx PWM peripheral selected, should be LPC_PWM1
- * @param[in] PWMChannel PWM channel, should be in range from 2 to 6
- * @param[in] ModeOption PWM mode option, should be:
- * - PWM_CHANNEL_SINGLE_EDGE: Single Edge mode
- * - PWM_CHANNEL_DUAL_EDGE: Dual Edge mode
- * @return None
- * Note: PWM Channel 1 can not be selected for mode option
- *********************************************************************/
-void PWM_ChannelConfig(LPC_PWM_TypeDef *PWMx, uint8_t PWMChannel, uint8_t ModeOption)
-{
- CHECK_PARAM(PARAM_PWMx(PWMx));
- CHECK_PARAM(PARAM_PWM1_EDGE_MODE_CHANNEL(PWMChannel));
- CHECK_PARAM(PARAM_PWM_CHANNEL_EDGE(ModeOption));
-
- // Single edge mode
- if (ModeOption == PWM_CHANNEL_SINGLE_EDGE)
- {
- PWMx->PCR &= (~PWM_PCR_PWMSELn(PWMChannel)) & PWM_PCR_BITMASK;
- }
- // Double edge mode
- else if (PWM_CHANNEL_DUAL_EDGE)
- {
- PWMx->PCR |= PWM_PCR_PWMSELn(PWMChannel);
- }
-}
-
-
-
-/********************************************************************//**
- * @brief Enable/Disable PWM channel output
- * @param[in] PWMx PWM peripheral selected, should be LPC_PWM1
- * @param[in] PWMChannel PWM channel, should be in range from 1 to 6
- * @param[in] NewState New State of this function, should be:
- * - ENABLE: Enable this PWM channel output
- * - DISABLE: Disable this PWM channel output
- * @return None
- *********************************************************************/
-void PWM_ChannelCmd(LPC_PWM_TypeDef *PWMx, uint8_t PWMChannel, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_PWMx(PWMx));
- CHECK_PARAM(PARAM_PWM1_CHANNEL(PWMChannel));
-
- if (NewState == ENABLE)
- {
- PWMx->PCR |= PWM_PCR_PWMENAn(PWMChannel);
- }
- else
- {
- PWMx->PCR &= (~PWM_PCR_PWMENAn(PWMChannel)) & PWM_PCR_BITMASK;
- }
-}
-
-/**
- * @}
- */
-
-#endif /* _PWM */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_qei.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_qei.c
deleted file mode 100644
index 01259ff686..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_qei.c
+++ /dev/null
@@ -1,514 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_qei.c 2010-05-21
-*//**
-* @file lpc17xx_qei.c
-* @brief Contains all functions support for QEI firmware library on LPC17xx
-* @version 2.0
-* @date 21. May. 2010
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2010, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup QEI
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_qei.h"
-#include "lpc17xx_clkpwr.h"
-
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-
-#ifdef _QEI
-
-/* Private Types -------------------------------------------------------------- */
-/** @defgroup QEI_Private_Types QEI Private Types
- * @{
- */
-
-/**
- * @brief QEI configuration union type definition
- */
-typedef union {
- QEI_CFG_Type bmQEIConfig;
- uint32_t ulQEIConfig;
-} QEI_CFGOPT_Type;
-
-/**
- * @}
- */
-
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup QEI_Public_Functions
- * @{
- */
-
-/*********************************************************************//**
- * @brief Resets value for each type of QEI value, such as velocity,
- * counter, position, etc..
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @param[in] ulResetType QEI Reset Type, should be one of the following:
- * - QEI_RESET_POS: Reset Position Counter
- * - QEI_RESET_POSOnIDX: Reset Position Counter on Index signal
- * - QEI_RESET_VEL: Reset Velocity
- * - QEI_RESET_IDX: Reset Index Counter
- * @return None
- **********************************************************************/
-void QEI_Reset(LPC_QEI_TypeDef *QEIx, uint32_t ulResetType)
-{
- CHECK_PARAM(PARAM_QEIx(QEIx));
- CHECK_PARAM(PARAM_QEI_RESET(ulResetType));
-
- QEIx->QEICON = ulResetType;
-}
-
-/*********************************************************************//**
- * @brief Initializes the QEI peripheral according to the specified
-* parameters in the QEI_ConfigStruct.
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @param[in] QEI_ConfigStruct Pointer to a QEI_CFG_Type structure
-* that contains the configuration information for the
-* specified QEI peripheral
- * @return None
- **********************************************************************/
-void QEI_Init(LPC_QEI_TypeDef *QEIx, QEI_CFG_Type *QEI_ConfigStruct)
-{
-
- CHECK_PARAM(PARAM_QEIx(QEIx));
- CHECK_PARAM(PARAM_QEI_DIRINV(QEI_ConfigStruct->DirectionInvert));
- CHECK_PARAM(PARAM_QEI_SIGNALMODE(QEI_ConfigStruct->SignalMode));
- CHECK_PARAM(PARAM_QEI_CAPMODE(QEI_ConfigStruct->CaptureMode));
- CHECK_PARAM(PARAM_QEI_INVINX(QEI_ConfigStruct->InvertIndex));
-
- /* Set up clock and power for QEI module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCQEI, ENABLE);
-
- /* As default, peripheral clock for QEI module
- * is set to FCCLK / 2 */
- CLKPWR_SetPCLKDiv(CLKPWR_PCLKSEL_QEI, CLKPWR_PCLKSEL_CCLK_DIV_1);
-
- // Reset all remaining value in QEI peripheral
- QEIx->QEICON = QEI_CON_RESP | QEI_CON_RESV | QEI_CON_RESI;
- QEIx->QEIMAXPOS = 0x00;
- QEIx->CMPOS0 = 0x00;
- QEIx->CMPOS1 = 0x00;
- QEIx->CMPOS2 = 0x00;
- QEIx->INXCMP = 0x00;
- QEIx->QEILOAD = 0x00;
- QEIx->VELCOMP = 0x00;
- QEIx->FILTER = 0x00;
- // Disable all Interrupt
- QEIx->QEIIEC = QEI_IECLR_BITMASK;
- // Clear all Interrupt pending
- QEIx->QEICLR = QEI_INTCLR_BITMASK;
- // Set QEI configuration value corresponding to its setting up value
- QEIx->QEICONF = ((QEI_CFGOPT_Type *)QEI_ConfigStruct)->ulQEIConfig;
-}
-
-
-/*********************************************************************//**
- * @brief De-initializes the QEI peripheral registers to their
-* default reset values.
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @return None
- **********************************************************************/
-void QEI_DeInit(LPC_QEI_TypeDef *QEIx)
-{
- CHECK_PARAM(PARAM_QEIx(QEIx));
-
- /* Turn off clock and power for QEI module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCQEI, DISABLE);
-}
-
-
-/*****************************************************************************//**
-* @brief Fills each QIE_InitStruct member with its default value:
-* - DirectionInvert = QEI_DIRINV_NONE
-* - SignalMode = QEI_SIGNALMODE_QUAD
-* - CaptureMode = QEI_CAPMODE_4X
-* - InvertIndex = QEI_INVINX_NONE
-* @param[in] QIE_InitStruct Pointer to a QEI_CFG_Type structure
-* which will be initialized.
-* @return None
-*******************************************************************************/
-void QEI_ConfigStructInit(QEI_CFG_Type *QIE_InitStruct)
-{
- QIE_InitStruct->CaptureMode = QEI_CAPMODE_4X;
- QIE_InitStruct->DirectionInvert = QEI_DIRINV_NONE;
- QIE_InitStruct->InvertIndex = QEI_INVINX_NONE;
- QIE_InitStruct->SignalMode = QEI_SIGNALMODE_QUAD;
-}
-
-
-/*********************************************************************//**
- * @brief Check whether if specified flag status is set or not
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @param[in] ulFlagType Status Flag Type, should be one of the following:
- * - QEI_STATUS_DIR: Direction Status
- * @return New Status of this status flag (SET or RESET)
- **********************************************************************/
-FlagStatus QEI_GetStatus(LPC_QEI_TypeDef *QEIx, uint32_t ulFlagType)
-{
- CHECK_PARAM(PARAM_QEIx(QEIx));
- CHECK_PARAM(PARAM_QEI_STATUS(ulFlagType));
- return ((QEIx->QEISTAT & ulFlagType) ? SET : RESET);
-}
-
-/*********************************************************************//**
- * @brief Get current position value in QEI peripheral
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @return Current position value of QEI peripheral
- **********************************************************************/
-uint32_t QEI_GetPosition(LPC_QEI_TypeDef *QEIx)
-{
- CHECK_PARAM(PARAM_QEIx(QEIx));
- return (QEIx->QEIPOS);
-}
-
-/*********************************************************************//**
- * @brief Set max position value for QEI peripheral
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @param[in] ulMaxPos Max position value to set
- * @return None
- **********************************************************************/
-void QEI_SetMaxPosition(LPC_QEI_TypeDef *QEIx, uint32_t ulMaxPos)
-{
- CHECK_PARAM(PARAM_QEIx(QEIx));
- QEIx->QEIMAXPOS = ulMaxPos;
-}
-
-/*********************************************************************//**
- * @brief Set position compare value for QEI peripheral
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @param[in] bPosCompCh Compare Position channel, should be:
- * - QEI_COMPPOS_CH_0: QEI compare position channel 0
- * - QEI_COMPPOS_CH_1: QEI compare position channel 1
- * - QEI_COMPPOS_CH_2: QEI compare position channel 2
- * @param[in] ulPosComp Compare Position value to set
- * @return None
- **********************************************************************/
-void QEI_SetPositionComp(LPC_QEI_TypeDef *QEIx, uint8_t bPosCompCh, uint32_t ulPosComp)
-{
- uint32_t *tmp;
-
- CHECK_PARAM(PARAM_QEIx(QEIx));
- CHECK_PARAM(PARAM_QEI_COMPPOS_CH(bPosCompCh));
- tmp = (uint32_t *) (&(QEIx->CMPOS0) + bPosCompCh * 4);
- *tmp = ulPosComp;
-
-}
-
-/*********************************************************************//**
- * @brief Get current index counter of QEI peripheral
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @return Current value of QEI index counter
- **********************************************************************/
-uint32_t QEI_GetIndex(LPC_QEI_TypeDef *QEIx)
-{
- CHECK_PARAM(PARAM_QEIx(QEIx));
- return (QEIx->INXCNT);
-}
-
-/*********************************************************************//**
- * @brief Set value for index compare in QEI peripheral
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @param[in] ulIndexComp Compare Index Value to set
- * @return None
- **********************************************************************/
-void QEI_SetIndexComp(LPC_QEI_TypeDef *QEIx, uint32_t ulIndexComp)
-{
- CHECK_PARAM(PARAM_QEIx(QEIx));
- QEIx->INXCMP = ulIndexComp;
-}
-
-/*********************************************************************//**
- * @brief Set timer reload value for QEI peripheral. When the velocity timer is
- * over-flow, the value that set for Timer Reload register will be loaded
- * into the velocity timer for next period. The calculated velocity in RPM
- * therefore will be affect by this value.
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @param[in] QEIReloadStruct QEI reload structure
- * @return None
- **********************************************************************/
-void QEI_SetTimerReload(LPC_QEI_TypeDef *QEIx, QEI_RELOADCFG_Type *QEIReloadStruct)
-{
- uint64_t pclk;
-
- CHECK_PARAM(PARAM_QEIx(QEIx));
- CHECK_PARAM(PARAM_QEI_TIMERRELOAD(QEIReloadStruct->ReloadOption));
-
- if (QEIReloadStruct->ReloadOption == QEI_TIMERRELOAD_TICKVAL) {
- QEIx->QEILOAD = QEIReloadStruct->ReloadValue - 1;
- } else {
- pclk = (uint64_t)CLKPWR_GetPCLK(CLKPWR_PCLKSEL_QEI);
- pclk = (pclk /(1000000/QEIReloadStruct->ReloadValue)) - 1;
- QEIx->QEILOAD = (uint32_t)pclk;
- }
-}
-
-/*********************************************************************//**
- * @brief Get current timer counter in QEI peripheral
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @return Current timer counter in QEI peripheral
- **********************************************************************/
-uint32_t QEI_GetTimer(LPC_QEI_TypeDef *QEIx)
-{
- CHECK_PARAM(PARAM_QEIx(QEIx));
- return (QEIx->QEITIME);
-}
-
-/*********************************************************************//**
- * @brief Get current velocity pulse counter in current time period
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @return Current velocity pulse counter value
- **********************************************************************/
-uint32_t QEI_GetVelocity(LPC_QEI_TypeDef *QEIx)
-{
- CHECK_PARAM(PARAM_QEIx(QEIx));
- return (QEIx->QEIVEL);
-}
-
-/*********************************************************************//**
- * @brief Get the most recently measured velocity of the QEI. When
- * the Velocity timer in QEI is over-flow, the current velocity
- * value will be loaded into Velocity Capture register.
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @return The most recently measured velocity value
- **********************************************************************/
-uint32_t QEI_GetVelocityCap(LPC_QEI_TypeDef *QEIx)
-{
- CHECK_PARAM(PARAM_QEIx(QEIx));
- return (QEIx->QEICAP);
-}
-
-/*********************************************************************//**
- * @brief Set Velocity Compare value for QEI peripheral
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @param[in] ulVelComp Compare Velocity value to set
- * @return None
- **********************************************************************/
-void QEI_SetVelocityComp(LPC_QEI_TypeDef *QEIx, uint32_t ulVelComp)
-{
- CHECK_PARAM(PARAM_QEIx(QEIx));
- QEIx->VELCOMP = ulVelComp;
-}
-
-/*********************************************************************//**
- * @brief Set value of sampling count for the digital filter in
- * QEI peripheral
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @param[in] ulSamplingPulse Value of sampling count to set
- * @return None
- **********************************************************************/
-void QEI_SetDigiFilter(LPC_QEI_TypeDef *QEIx, uint32_t ulSamplingPulse)
-{
- CHECK_PARAM(PARAM_QEIx(QEIx));
- QEIx->FILTER = ulSamplingPulse;
-}
-
-/*********************************************************************//**
- * @brief Check whether if specified interrupt flag status in QEI
- * peripheral is set or not
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @param[in] ulIntType Interrupt Flag Status type, should be:
- - QEI_INTFLAG_INX_Int: index pulse was detected interrupt
- - QEI_INTFLAG_TIM_Int: Velocity timer over flow interrupt
- - QEI_INTFLAG_VELC_Int: Capture velocity is less than compare interrupt
- - QEI_INTFLAG_DIR_Int: Change of direction interrupt
- - QEI_INTFLAG_ERR_Int: An encoder phase error interrupt
- - QEI_INTFLAG_ENCLK_Int: An encoder clock pulse was detected interrupt
- - QEI_INTFLAG_POS0_Int: position 0 compare value is equal to the
- current position interrupt
- - QEI_INTFLAG_POS1_Int: position 1 compare value is equal to the
- current position interrupt
- - QEI_INTFLAG_POS2_Int: position 2 compare value is equal to the
- current position interrupt
- - QEI_INTFLAG_REV_Int: Index compare value is equal to the current
- index count interrupt
- - QEI_INTFLAG_POS0REV_Int: Combined position 0 and revolution count interrupt
- - QEI_INTFLAG_POS1REV_Int: Combined position 1 and revolution count interrupt
- - QEI_INTFLAG_POS2REV_Int: Combined position 2 and revolution count interrupt
- * @return New State of specified interrupt flag status (SET or RESET)
- **********************************************************************/
-FlagStatus QEI_GetIntStatus(LPC_QEI_TypeDef *QEIx, uint32_t ulIntType)
-{
- CHECK_PARAM(PARAM_QEIx(QEIx));
- CHECK_PARAM(PARAM_QEI_INTFLAG(ulIntType));
-
- return((QEIx->QEIINTSTAT & ulIntType) ? SET : RESET);
-}
-
-/*********************************************************************//**
- * @brief Enable/Disable specified interrupt in QEI peripheral
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @param[in] ulIntType Interrupt Flag Status type, should be:
- * - QEI_INTFLAG_INX_Int: index pulse was detected interrupt
- * - QEI_INTFLAG_TIM_Int: Velocity timer over flow interrupt
- * - QEI_INTFLAG_VELC_Int: Capture velocity is less than compare interrupt
- * - QEI_INTFLAG_DIR_Int: Change of direction interrupt
- * - QEI_INTFLAG_ERR_Int: An encoder phase error interrupt
- * - QEI_INTFLAG_ENCLK_Int: An encoder clock pulse was detected interrupt
- * - QEI_INTFLAG_POS0_Int: position 0 compare value is equal to the
- * current position interrupt
- * - QEI_INTFLAG_POS1_Int: position 1 compare value is equal to the
- * current position interrupt
- * - QEI_INTFLAG_POS2_Int: position 2 compare value is equal to the
- * current position interrupt
- * - QEI_INTFLAG_REV_Int: Index compare value is equal to the current
- * index count interrupt
- * - QEI_INTFLAG_POS0REV_Int: Combined position 0 and revolution count interrupt
- * - QEI_INTFLAG_POS1REV_Int: Combined position 1 and revolution count interrupt
- * - QEI_INTFLAG_POS2REV_Int: Combined position 2 and revolution count interrupt
- * @param[in] NewState New function state, should be:
- * - DISABLE
- * - ENABLE
- * @return None
- **********************************************************************/
-void QEI_IntCmd(LPC_QEI_TypeDef *QEIx, uint32_t ulIntType, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_QEIx(QEIx));
- CHECK_PARAM(PARAM_QEI_INTFLAG(ulIntType));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- if (NewState == ENABLE) {
- QEIx->QEIIES = ulIntType;
- } else {
- QEIx->QEIIEC = ulIntType;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Sets (forces) specified interrupt in QEI peripheral
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @param[in] ulIntType Interrupt Flag Status type, should be:
- - QEI_INTFLAG_INX_Int: index pulse was detected interrupt
- - QEI_INTFLAG_TIM_Int: Velocity timer over flow interrupt
- - QEI_INTFLAG_VELC_Int: Capture velocity is less than compare interrupt
- - QEI_INTFLAG_DIR_Int: Change of direction interrupt
- - QEI_INTFLAG_ERR_Int: An encoder phase error interrupt
- - QEI_INTFLAG_ENCLK_Int: An encoder clock pulse was detected interrupt
- - QEI_INTFLAG_POS0_Int: position 0 compare value is equal to the
- current position interrupt
- - QEI_INTFLAG_POS1_Int: position 1 compare value is equal to the
- current position interrupt
- - QEI_INTFLAG_POS2_Int: position 2 compare value is equal to the
- current position interrupt
- - QEI_INTFLAG_REV_Int: Index compare value is equal to the current
- index count interrupt
- - QEI_INTFLAG_POS0REV_Int: Combined position 0 and revolution count interrupt
- - QEI_INTFLAG_POS1REV_Int: Combined position 1 and revolution count interrupt
- - QEI_INTFLAG_POS2REV_Int: Combined position 2 and revolution count interrupt
- * @return None
- **********************************************************************/
-void QEI_IntSet(LPC_QEI_TypeDef *QEIx, uint32_t ulIntType)
-{
- CHECK_PARAM(PARAM_QEIx(QEIx));
- CHECK_PARAM(PARAM_QEI_INTFLAG(ulIntType));
-
- QEIx->QEISET = ulIntType;
-}
-
-/*********************************************************************//**
- * @brief Clear (force) specified interrupt (pending) in QEI peripheral
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @param[in] ulIntType Interrupt Flag Status type, should be:
- - QEI_INTFLAG_INX_Int: index pulse was detected interrupt
- - QEI_INTFLAG_TIM_Int: Velocity timer over flow interrupt
- - QEI_INTFLAG_VELC_Int: Capture velocity is less than compare interrupt
- - QEI_INTFLAG_DIR_Int: Change of direction interrupt
- - QEI_INTFLAG_ERR_Int: An encoder phase error interrupt
- - QEI_INTFLAG_ENCLK_Int: An encoder clock pulse was detected interrupt
- - QEI_INTFLAG_POS0_Int: position 0 compare value is equal to the
- current position interrupt
- - QEI_INTFLAG_POS1_Int: position 1 compare value is equal to the
- current position interrupt
- - QEI_INTFLAG_POS2_Int: position 2 compare value is equal to the
- current position interrupt
- - QEI_INTFLAG_REV_Int: Index compare value is equal to the current
- index count interrupt
- - QEI_INTFLAG_POS0REV_Int: Combined position 0 and revolution count interrupt
- - QEI_INTFLAG_POS1REV_Int: Combined position 1 and revolution count interrupt
- - QEI_INTFLAG_POS2REV_Int: Combined position 2 and revolution count interrupt
- * @return None
- **********************************************************************/
-void QEI_IntClear(LPC_QEI_TypeDef *QEIx, uint32_t ulIntType)
-{
- CHECK_PARAM(PARAM_QEIx(QEIx));
- CHECK_PARAM(PARAM_QEI_INTFLAG(ulIntType));
-
- QEIx->QEICLR = ulIntType;
-}
-
-
-/*********************************************************************//**
- * @brief Calculates the actual velocity in RPM passed via velocity
- * capture value and Pulse Per Round (of the encoder) value
- * parameter input.
- * @param[in] QEIx QEI peripheral, should be LPC_QEI
- * @param[in] ulVelCapValue Velocity capture input value that can
- * be got from QEI_GetVelocityCap() function
- * @param[in] ulPPR Pulse per round of encoder
- * @return The actual value of velocity in RPM (Round per minute)
- **********************************************************************/
-uint32_t QEI_CalculateRPM(LPC_QEI_TypeDef *QEIx, uint32_t ulVelCapValue, uint32_t ulPPR)
-{
- uint64_t rpm, clock, Load, edges;
-
- // Get current Clock rate for timer input
- clock = (uint64_t)CLKPWR_GetPCLK(CLKPWR_PCLKSEL_QEI);
- // Get Timer load value (velocity capture period)
- Load = (uint64_t)(QEIx->QEILOAD + 1);
- // Get Edge
- edges = (uint64_t)((QEIx->QEICONF & QEI_CONF_CAPMODE) ? 4 : 2);
- // Calculate RPM
- rpm = ((clock * ulVelCapValue * 60) / (Load * ulPPR * edges));
-
- return (uint32_t)(rpm);
-}
-
-
-/**
- * @}
- */
-
-#endif /* _QEI */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
-
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_rit.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_rit.c
deleted file mode 100644
index ab89ed3be3..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_rit.c
+++ /dev/null
@@ -1,199 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_rit.c 2010-05-21
-*//**
-* @file lpc17xx_rit.c
-* @brief Contains all functions support for RIT firmware library on LPC17xx
-* @version 2.0
-* @date 21. May. 2010
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2010, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup RIT
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_rit.h"
-#include "lpc17xx_clkpwr.h"
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-#ifdef _RIT
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup RIT_Public_Functions
- * @{
- */
-
-/******************************************************************************//*
- * @brief Initial for RIT
- * - Turn on power and clock
- * - Setup default register values
- * @param[in] RITx is RIT peripheral selected, should be: LPC_RIT
- * @return None
- *******************************************************************************/
-void RIT_Init(LPC_RIT_TypeDef *RITx)
-{
- CHECK_PARAM(PARAM_RITx(RITx));
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCRIT, ENABLE);
- //Set up default register values
- RITx->RICOMPVAL = 0xFFFFFFFF;
- RITx->RIMASK = 0x00000000;
- RITx->RICTRL = 0x0C;
- RITx->RICOUNTER = 0x00000000;
- // Turn on power and clock
-
-}
-/******************************************************************************//*
- * @brief DeInitial for RIT
- * - Turn off power and clock
- * - ReSetup default register values
- * @param[in] RITx is RIT peripheral selected, should be: LPC_RIT
- * @return None
- *******************************************************************************/
-void RIT_DeInit(LPC_RIT_TypeDef *RITx)
-{
- CHECK_PARAM(PARAM_RITx(RITx));
-
- // Turn off power and clock
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCRIT, DISABLE);
- //ReSetup default register values
- RITx->RICOMPVAL = 0xFFFFFFFF;
- RITx->RIMASK = 0x00000000;
- RITx->RICTRL = 0x0C;
- RITx->RICOUNTER = 0x00000000;
-}
-
-/******************************************************************************//*
- * @brief Set compare value, mask value and time counter value
- * @param[in] RITx is RIT peripheral selected, should be: LPC_RIT
- * @param[in] time_interval: timer interval value (ms)
- * @return None
- *******************************************************************************/
-void RIT_TimerConfig(LPC_RIT_TypeDef *RITx, uint32_t time_interval)
-{
- uint32_t clock_rate, cmp_value;
- CHECK_PARAM(PARAM_RITx(RITx));
-
- // Get PCLK value of RIT
- clock_rate = CLKPWR_GetPCLK(CLKPWR_PCLKSEL_RIT);
-
- /* calculate compare value for RIT to generate interrupt at
- * specified time interval
- * COMPVAL = (RIT_PCLK * time_interval)/1000
- * (with time_interval unit is millisecond)
- */
- cmp_value = (clock_rate /1000) * time_interval;
- RITx->RICOMPVAL = cmp_value;
-
- /* Set timer enable clear bit to clear timer to 0 whenever
- * counter value equals the contents of RICOMPVAL
- */
- RITx->RICTRL |= (1<<1);
-}
-
-
-/******************************************************************************//*
- * @brief Enable/Disable Timer
- * @param[in] RITx is RIT peripheral selected, should be: LPC_RIT
- * @param[in] NewState New State of this function
- * -ENABLE: Enable Timer
- * -DISABLE: Disable Timer
- * @return None
- *******************************************************************************/
-void RIT_Cmd(LPC_RIT_TypeDef *RITx, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_RITx(RITx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- //Enable or Disable Timer
- if(NewState==ENABLE)
- {
- RITx->RICTRL |= RIT_CTRL_TEN;
- }
- else
- {
- RITx->RICTRL &= ~RIT_CTRL_TEN;
- }
-}
-
-/******************************************************************************//*
- * @brief Timer Enable/Disable on debug
- * @param[in] RITx is RIT peripheral selected, should be: LPC_RIT
- * @param[in] NewState New State of this function
- * -ENABLE: The timer is halted whenever a hardware break condition occurs
- * -DISABLE: Hardware break has no effect on the timer operation
- * @return None
- *******************************************************************************/
-void RIT_TimerDebugCmd(LPC_RIT_TypeDef *RITx, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_RITx(RITx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- //Timer Enable/Disable on break
- if(NewState==ENABLE)
- {
- RITx->RICTRL |= RIT_CTRL_ENBR;
- }
- else
- {
- RITx->RICTRL &= ~RIT_CTRL_ENBR;
- }
-}
-/******************************************************************************//*
- * @brief Check whether interrupt flag is set or not
- * @param[in] RITx is RIT peripheral selected, should be: LPC_RIT
- * @return Current interrupt status, could be: SET/RESET
- *******************************************************************************/
-IntStatus RIT_GetIntStatus(LPC_RIT_TypeDef *RITx)
-{
- IntStatus result;
- CHECK_PARAM(PARAM_RITx(RITx));
- if((RITx->RICTRL&RIT_CTRL_INTEN)==1) result= SET;
- else return RESET;
- //clear interrupt flag
- RITx->RICTRL |= RIT_CTRL_INTEN;
- return result;
-}
-
-/**
- * @}
- */
-
-#endif /* _RIT */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_rtc.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_rtc.c
deleted file mode 100644
index c47f938365..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_rtc.c
+++ /dev/null
@@ -1,783 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_rtc.c 2011-06-06
-*//**
-* @file lpc17xx_rtc.c
-* @brief Contains all functions support for RTC firmware library on LPC17xx
-* @version 3.1
-* @date 6. June. 2011
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2011, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup RTC
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_rtc.h"
-#include "lpc17xx_clkpwr.h"
-
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-
-#ifdef _RTC
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup RTC_Public_Functions
- * @{
- */
-
-/********************************************************************//**
- * @brief Initializes the RTC peripheral.
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @return None
- *********************************************************************/
-void RTC_Init (LPC_RTC_TypeDef *RTCx)
-{
- CHECK_PARAM(PARAM_RTCx(RTCx));
-
- /* Set up clock and power for RTC module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCRTC, ENABLE);
-
- // Clear all register to be default
- RTCx->ILR = 0x00;
- RTCx->CCR = 0x00;
- RTCx->CIIR = 0x00;
- RTCx->AMR = 0xFF;
- RTCx->CALIBRATION = 0x00;
-}
-
-
-/*********************************************************************//**
- * @brief De-initializes the RTC peripheral registers to their
-* default reset values.
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @return None
- **********************************************************************/
-void RTC_DeInit(LPC_RTC_TypeDef *RTCx)
-{
- CHECK_PARAM(PARAM_RTCx(RTCx));
-
- RTCx->CCR = 0x00;
- // Disable power and clock for RTC module
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCRTC, DISABLE);
-}
-
-/*********************************************************************//**
- * @brief Reset clock tick counter in RTC peripheral
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @return None
- **********************************************************************/
-void RTC_ResetClockTickCounter(LPC_RTC_TypeDef *RTCx)
-{
- CHECK_PARAM(PARAM_RTCx(RTCx));
-
- RTCx->CCR |= RTC_CCR_CTCRST;
- RTCx->CCR &= (~RTC_CCR_CTCRST) & RTC_CCR_BITMASK;
-}
-
-/*********************************************************************//**
- * @brief Start/Stop RTC peripheral
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @param[in] NewState New State of this function, should be:
- * - ENABLE: The time counters are enabled
- * - DISABLE: The time counters are disabled
- * @return None
- **********************************************************************/
-void RTC_Cmd (LPC_RTC_TypeDef *RTCx, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_RTCx(RTCx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- if (NewState == ENABLE)
- {
- RTCx->CCR |= RTC_CCR_CLKEN;
- }
- else
- {
- RTCx->CCR &= (~RTC_CCR_CLKEN) & RTC_CCR_BITMASK;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Enable/Disable Counter increment interrupt for each time type
- * in RTC peripheral
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @param[in] CntIncrIntType: Counter Increment Interrupt type,
- * an increment of this type value below will generates
- * an interrupt, should be:
- * - RTC_TIMETYPE_SECOND
- * - RTC_TIMETYPE_MINUTE
- * - RTC_TIMETYPE_HOUR
- * - RTC_TIMETYPE_DAYOFWEEK
- * - RTC_TIMETYPE_DAYOFMONTH
- * - RTC_TIMETYPE_DAYOFYEAR
- * - RTC_TIMETYPE_MONTH
- * - RTC_TIMETYPE_YEAR
- * @param[in] NewState New State of this function, should be:
- * - ENABLE: Counter Increment interrupt for this
- * time type are enabled
- * - DISABLE: Counter Increment interrupt for this
- * time type are disabled
- * @return None
- **********************************************************************/
-void RTC_CntIncrIntConfig (LPC_RTC_TypeDef *RTCx, uint32_t CntIncrIntType, \
- FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_RTCx(RTCx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
- CHECK_PARAM(PARAM_RTC_TIMETYPE(CntIncrIntType));
-
- if (NewState == ENABLE)
- {
- switch (CntIncrIntType)
- {
- case RTC_TIMETYPE_SECOND:
- RTCx->CIIR |= RTC_CIIR_IMSEC;
- break;
- case RTC_TIMETYPE_MINUTE:
- RTCx->CIIR |= RTC_CIIR_IMMIN;
- break;
- case RTC_TIMETYPE_HOUR:
- RTCx->CIIR |= RTC_CIIR_IMHOUR;
- break;
- case RTC_TIMETYPE_DAYOFWEEK:
- RTCx->CIIR |= RTC_CIIR_IMDOW;
- break;
- case RTC_TIMETYPE_DAYOFMONTH:
- RTCx->CIIR |= RTC_CIIR_IMDOM;
- break;
- case RTC_TIMETYPE_DAYOFYEAR:
- RTCx->CIIR |= RTC_CIIR_IMDOY;
- break;
- case RTC_TIMETYPE_MONTH:
- RTCx->CIIR |= RTC_CIIR_IMMON;
- break;
- case RTC_TIMETYPE_YEAR:
- RTCx->CIIR |= RTC_CIIR_IMYEAR;
- break;
- }
- }
- else
- {
- switch (CntIncrIntType)
- {
- case RTC_TIMETYPE_SECOND:
- RTCx->CIIR &= (~RTC_CIIR_IMSEC) & RTC_CIIR_BITMASK;
- break;
- case RTC_TIMETYPE_MINUTE:
- RTCx->CIIR &= (~RTC_CIIR_IMMIN) & RTC_CIIR_BITMASK;
- break;
- case RTC_TIMETYPE_HOUR:
- RTCx->CIIR &= (~RTC_CIIR_IMHOUR) & RTC_CIIR_BITMASK;
- break;
- case RTC_TIMETYPE_DAYOFWEEK:
- RTCx->CIIR &= (~RTC_CIIR_IMDOW) & RTC_CIIR_BITMASK;
- break;
- case RTC_TIMETYPE_DAYOFMONTH:
- RTCx->CIIR &= (~RTC_CIIR_IMDOM) & RTC_CIIR_BITMASK;
- break;
- case RTC_TIMETYPE_DAYOFYEAR:
- RTCx->CIIR &= (~RTC_CIIR_IMDOY) & RTC_CIIR_BITMASK;
- break;
- case RTC_TIMETYPE_MONTH:
- RTCx->CIIR &= (~RTC_CIIR_IMMON) & RTC_CIIR_BITMASK;
- break;
- case RTC_TIMETYPE_YEAR:
- RTCx->CIIR &= (~RTC_CIIR_IMYEAR) & RTC_CIIR_BITMASK;
- break;
- }
- }
-}
-
-
-/*********************************************************************//**
- * @brief Enable/Disable Alarm interrupt for each time type
- * in RTC peripheral
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @param[in] AlarmTimeType: Alarm Time Interrupt type,
- * an matching of this type value below with current time
- * in RTC will generates an interrupt, should be:
- * - RTC_TIMETYPE_SECOND
- * - RTC_TIMETYPE_MINUTE
- * - RTC_TIMETYPE_HOUR
- * - RTC_TIMETYPE_DAYOFWEEK
- * - RTC_TIMETYPE_DAYOFMONTH
- * - RTC_TIMETYPE_DAYOFYEAR
- * - RTC_TIMETYPE_MONTH
- * - RTC_TIMETYPE_YEAR
- * @param[in] NewState New State of this function, should be:
- * - ENABLE: Alarm interrupt for this
- * time type are enabled
- * - DISABLE: Alarm interrupt for this
- * time type are disabled
- * @return None
- **********************************************************************/
-void RTC_AlarmIntConfig (LPC_RTC_TypeDef *RTCx, uint32_t AlarmTimeType, \
- FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_RTCx(RTCx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
- CHECK_PARAM(PARAM_RTC_TIMETYPE(AlarmTimeType));
-
- if (NewState == ENABLE)
- {
- switch (AlarmTimeType)
- {
- case RTC_TIMETYPE_SECOND:
- RTCx->AMR &= (~RTC_AMR_AMRSEC) & RTC_AMR_BITMASK;
- break;
- case RTC_TIMETYPE_MINUTE:
- RTCx->AMR &= (~RTC_AMR_AMRMIN) & RTC_AMR_BITMASK;
- break;
- case RTC_TIMETYPE_HOUR:
- RTCx->AMR &= (~RTC_AMR_AMRHOUR) & RTC_AMR_BITMASK;
- break;
- case RTC_TIMETYPE_DAYOFWEEK:
- RTCx->AMR &= (~RTC_AMR_AMRDOW) & RTC_AMR_BITMASK;
- break;
- case RTC_TIMETYPE_DAYOFMONTH:
- RTCx->AMR &= (~RTC_AMR_AMRDOM) & RTC_AMR_BITMASK;
- break;
- case RTC_TIMETYPE_DAYOFYEAR:
- RTCx->AMR &= (~RTC_AMR_AMRDOY) & RTC_AMR_BITMASK;
- break;
- case RTC_TIMETYPE_MONTH:
- RTCx->AMR &= (~RTC_AMR_AMRMON) & RTC_AMR_BITMASK;
- break;
- case RTC_TIMETYPE_YEAR:
- RTCx->AMR &= (~RTC_AMR_AMRYEAR) & RTC_AMR_BITMASK;
- break;
- }
- }
- else
- {
- switch (AlarmTimeType)
- {
- case RTC_TIMETYPE_SECOND:
- RTCx->AMR |= (RTC_AMR_AMRSEC);
- break;
- case RTC_TIMETYPE_MINUTE:
- RTCx->AMR |= (RTC_AMR_AMRMIN);
- break;
- case RTC_TIMETYPE_HOUR:
- RTCx->AMR |= (RTC_AMR_AMRHOUR);
- break;
- case RTC_TIMETYPE_DAYOFWEEK:
- RTCx->AMR |= (RTC_AMR_AMRDOW);
- break;
- case RTC_TIMETYPE_DAYOFMONTH:
- RTCx->AMR |= (RTC_AMR_AMRDOM);
- break;
- case RTC_TIMETYPE_DAYOFYEAR:
- RTCx->AMR |= (RTC_AMR_AMRDOY);
- break;
- case RTC_TIMETYPE_MONTH:
- RTCx->AMR |= (RTC_AMR_AMRMON);
- break;
- case RTC_TIMETYPE_YEAR:
- RTCx->AMR |= (RTC_AMR_AMRYEAR);
- break;
- }
- }
-}
-
-
-/*********************************************************************//**
- * @brief Set current time value for each time type in RTC peripheral
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @param[in] Timetype: Time Type, should be:
- * - RTC_TIMETYPE_SECOND
- * - RTC_TIMETYPE_MINUTE
- * - RTC_TIMETYPE_HOUR
- * - RTC_TIMETYPE_DAYOFWEEK
- * - RTC_TIMETYPE_DAYOFMONTH
- * - RTC_TIMETYPE_DAYOFYEAR
- * - RTC_TIMETYPE_MONTH
- * - RTC_TIMETYPE_YEAR
- * @param[in] TimeValue Time value to set
- * @return None
- **********************************************************************/
-void RTC_SetTime (LPC_RTC_TypeDef *RTCx, uint32_t Timetype, uint32_t TimeValue)
-{
- CHECK_PARAM(PARAM_RTCx(RTCx));
- CHECK_PARAM(PARAM_RTC_TIMETYPE(Timetype));
-
- switch ( Timetype)
- {
- case RTC_TIMETYPE_SECOND:
- CHECK_PARAM(TimeValue <= RTC_SECOND_MAX);
-
- RTCx->SEC = TimeValue & RTC_SEC_MASK;
- break;
-
- case RTC_TIMETYPE_MINUTE:
- CHECK_PARAM(TimeValue <= RTC_MINUTE_MAX);
-
- RTCx->MIN = TimeValue & RTC_MIN_MASK;
- break;
-
- case RTC_TIMETYPE_HOUR:
- CHECK_PARAM(TimeValue <= RTC_HOUR_MAX);
-
- RTCx->HOUR = TimeValue & RTC_HOUR_MASK;
- break;
-
- case RTC_TIMETYPE_DAYOFWEEK:
- CHECK_PARAM(TimeValue <= RTC_DAYOFWEEK_MAX);
-
- RTCx->DOW = TimeValue & RTC_DOW_MASK;
- break;
-
- case RTC_TIMETYPE_DAYOFMONTH:
- CHECK_PARAM((TimeValue <= RTC_DAYOFMONTH_MAX) \
- && (TimeValue >= RTC_DAYOFMONTH_MIN));
-
- RTCx->DOM = TimeValue & RTC_DOM_MASK;
- break;
-
- case RTC_TIMETYPE_DAYOFYEAR:
- CHECK_PARAM((TimeValue >= RTC_DAYOFYEAR_MIN) \
- && (TimeValue <= RTC_DAYOFYEAR_MAX));
-
- RTCx->DOY = TimeValue & RTC_DOY_MASK;
- break;
-
- case RTC_TIMETYPE_MONTH:
- CHECK_PARAM((TimeValue >= RTC_MONTH_MIN) \
- && (TimeValue <= RTC_MONTH_MAX));
-
- RTCx->MONTH = TimeValue & RTC_MONTH_MASK;
- break;
-
- case RTC_TIMETYPE_YEAR:
- CHECK_PARAM(TimeValue <= RTC_YEAR_MAX);
-
- RTCx->YEAR = TimeValue & RTC_YEAR_MASK;
- break;
- }
-}
-
-/*********************************************************************//**
- * @brief Get current time value for each type time type
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @param[in] Timetype: Time Type, should be:
- * - RTC_TIMETYPE_SECOND
- * - RTC_TIMETYPE_MINUTE
- * - RTC_TIMETYPE_HOUR
- * - RTC_TIMETYPE_DAYOFWEEK
- * - RTC_TIMETYPE_DAYOFMONTH
- * - RTC_TIMETYPE_DAYOFYEAR
- * - RTC_TIMETYPE_MONTH
- * - RTC_TIMETYPE_YEAR
- * @return Value of time according to specified time type
- **********************************************************************/
-uint32_t RTC_GetTime(LPC_RTC_TypeDef *RTCx, uint32_t Timetype)
-{
- CHECK_PARAM(PARAM_RTCx(RTCx));
- CHECK_PARAM(PARAM_RTC_TIMETYPE(Timetype));
-
- switch (Timetype)
- {
- case RTC_TIMETYPE_SECOND:
- return (RTCx->SEC & RTC_SEC_MASK);
- case RTC_TIMETYPE_MINUTE:
- return (RTCx->MIN & RTC_MIN_MASK);
- case RTC_TIMETYPE_HOUR:
- return (RTCx->HOUR & RTC_HOUR_MASK);
- case RTC_TIMETYPE_DAYOFWEEK:
- return (RTCx->DOW & RTC_DOW_MASK);
- case RTC_TIMETYPE_DAYOFMONTH:
- return (RTCx->DOM & RTC_DOM_MASK);
- case RTC_TIMETYPE_DAYOFYEAR:
- return (RTCx->DOY & RTC_DOY_MASK);
- case RTC_TIMETYPE_MONTH:
- return (RTCx->MONTH & RTC_MONTH_MASK);
- case RTC_TIMETYPE_YEAR:
- return (RTCx->YEAR & RTC_YEAR_MASK);
- default:
- return (0);
- }
-}
-
-
-/*********************************************************************//**
- * @brief Set full of time in RTC peripheral
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @param[in] pFullTime Pointer to a RTC_TIME_Type structure that
- * contains time value in full.
- * @return None
- **********************************************************************/
-void RTC_SetFullTime (LPC_RTC_TypeDef *RTCx, RTC_TIME_Type *pFullTime)
-{
- CHECK_PARAM(PARAM_RTCx(RTCx));
-
- RTCx->DOM = pFullTime->DOM & RTC_DOM_MASK;
- RTCx->DOW = pFullTime->DOW & RTC_DOW_MASK;
- RTCx->DOY = pFullTime->DOY & RTC_DOY_MASK;
- RTCx->HOUR = pFullTime->HOUR & RTC_HOUR_MASK;
- RTCx->MIN = pFullTime->MIN & RTC_MIN_MASK;
- RTCx->SEC = pFullTime->SEC & RTC_SEC_MASK;
- RTCx->MONTH = pFullTime->MONTH & RTC_MONTH_MASK;
- RTCx->YEAR = pFullTime->YEAR & RTC_YEAR_MASK;
-}
-
-
-/*********************************************************************//**
- * @brief Get full of time in RTC peripheral
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @param[in] pFullTime Pointer to a RTC_TIME_Type structure that
- * will be stored time in full.
- * @return None
- **********************************************************************/
-void RTC_GetFullTime (LPC_RTC_TypeDef *RTCx, RTC_TIME_Type *pFullTime)
-{
- CHECK_PARAM(PARAM_RTCx(RTCx));
-
- pFullTime->DOM = RTCx->DOM & RTC_DOM_MASK;
- pFullTime->DOW = RTCx->DOW & RTC_DOW_MASK;
- pFullTime->DOY = RTCx->DOY & RTC_DOY_MASK;
- pFullTime->HOUR = RTCx->HOUR & RTC_HOUR_MASK;
- pFullTime->MIN = RTCx->MIN & RTC_MIN_MASK;
- pFullTime->SEC = RTCx->SEC & RTC_SEC_MASK;
- pFullTime->MONTH = RTCx->MONTH & RTC_MONTH_MASK;
- pFullTime->YEAR = RTCx->YEAR & RTC_YEAR_MASK;
-}
-
-
-/*********************************************************************//**
- * @brief Set alarm time value for each time type
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @param[in] Timetype: Time Type, should be:
- * - RTC_TIMETYPE_SECOND
- * - RTC_TIMETYPE_MINUTE
- * - RTC_TIMETYPE_HOUR
- * - RTC_TIMETYPE_DAYOFWEEK
- * - RTC_TIMETYPE_DAYOFMONTH
- * - RTC_TIMETYPE_DAYOFYEAR
- * - RTC_TIMETYPE_MONTH
- * - RTC_TIMETYPE_YEAR
- * @param[in] ALValue Alarm time value to set
- * @return None
- **********************************************************************/
-void RTC_SetAlarmTime (LPC_RTC_TypeDef *RTCx, uint32_t Timetype, uint32_t ALValue)
-{
- CHECK_PARAM(PARAM_RTCx(RTCx));
-
- switch (Timetype)
- {
- case RTC_TIMETYPE_SECOND:
- CHECK_PARAM(ALValue <= RTC_SECOND_MAX);
-
- RTCx->ALSEC = ALValue & RTC_SEC_MASK;
- break;
-
- case RTC_TIMETYPE_MINUTE:
- CHECK_PARAM(ALValue <= RTC_MINUTE_MAX);
-
- RTCx->ALMIN = ALValue & RTC_MIN_MASK;
- break;
-
- case RTC_TIMETYPE_HOUR:
- CHECK_PARAM(ALValue <= RTC_HOUR_MAX);
-
- RTCx->ALHOUR = ALValue & RTC_HOUR_MASK;
- break;
-
- case RTC_TIMETYPE_DAYOFWEEK:
- CHECK_PARAM(ALValue <= RTC_DAYOFWEEK_MAX);
-
- RTCx->ALDOW = ALValue & RTC_DOW_MASK;
- break;
-
- case RTC_TIMETYPE_DAYOFMONTH:
- CHECK_PARAM((ALValue <= RTC_DAYOFMONTH_MAX) \
- && (ALValue >= RTC_DAYOFMONTH_MIN));
-
- RTCx->ALDOM = ALValue & RTC_DOM_MASK;
- break;
-
- case RTC_TIMETYPE_DAYOFYEAR:
- CHECK_PARAM((ALValue >= RTC_DAYOFYEAR_MIN) \
- && (ALValue <= RTC_DAYOFYEAR_MAX));
-
- RTCx->ALDOY = ALValue & RTC_DOY_MASK;
- break;
-
- case RTC_TIMETYPE_MONTH:
- CHECK_PARAM((ALValue >= RTC_MONTH_MIN) \
- && (ALValue <= RTC_MONTH_MAX));
-
- RTCx->ALMON = ALValue & RTC_MONTH_MASK;
- break;
-
- case RTC_TIMETYPE_YEAR:
- CHECK_PARAM(ALValue <= RTC_YEAR_MAX);
-
- RTCx->ALYEAR = ALValue & RTC_YEAR_MASK;
- break;
- }
-}
-
-
-
-/*********************************************************************//**
- * @brief Get alarm time value for each time type
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @param[in] Timetype: Time Type, should be:
- * - RTC_TIMETYPE_SECOND
- * - RTC_TIMETYPE_MINUTE
- * - RTC_TIMETYPE_HOUR
- * - RTC_TIMETYPE_DAYOFWEEK
- * - RTC_TIMETYPE_DAYOFMONTH
- * - RTC_TIMETYPE_DAYOFYEAR
- * - RTC_TIMETYPE_MONTH
- * - RTC_TIMETYPE_YEAR
- * @return Value of Alarm time according to specified time type
- **********************************************************************/
-uint32_t RTC_GetAlarmTime (LPC_RTC_TypeDef *RTCx, uint32_t Timetype)
-{
- switch (Timetype)
- {
- case RTC_TIMETYPE_SECOND:
- return (RTCx->ALSEC & RTC_SEC_MASK);
- case RTC_TIMETYPE_MINUTE:
- return (RTCx->ALMIN & RTC_MIN_MASK);
- case RTC_TIMETYPE_HOUR:
- return (RTCx->ALHOUR & RTC_HOUR_MASK);
- case RTC_TIMETYPE_DAYOFWEEK:
- return (RTCx->ALDOW & RTC_DOW_MASK);
- case RTC_TIMETYPE_DAYOFMONTH:
- return (RTCx->ALDOM & RTC_DOM_MASK);
- case RTC_TIMETYPE_DAYOFYEAR:
- return (RTCx->ALDOY & RTC_DOY_MASK);
- case RTC_TIMETYPE_MONTH:
- return (RTCx->ALMON & RTC_MONTH_MASK);
- case RTC_TIMETYPE_YEAR:
- return (RTCx->ALYEAR & RTC_YEAR_MASK);
- default:
- return (0);
- }
-}
-
-
-/*********************************************************************//**
- * @brief Set full of alarm time in RTC peripheral
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @param[in] pFullTime Pointer to a RTC_TIME_Type structure that
- * contains alarm time value in full.
- * @return None
- **********************************************************************/
-void RTC_SetFullAlarmTime (LPC_RTC_TypeDef *RTCx, RTC_TIME_Type *pFullTime)
-{
- CHECK_PARAM(PARAM_RTCx(RTCx));
-
- RTCx->ALDOM = pFullTime->DOM & RTC_DOM_MASK;
- RTCx->ALDOW = pFullTime->DOW & RTC_DOW_MASK;
- RTCx->ALDOY = pFullTime->DOY & RTC_DOY_MASK;
- RTCx->ALHOUR = pFullTime->HOUR & RTC_HOUR_MASK;
- RTCx->ALMIN = pFullTime->MIN & RTC_MIN_MASK;
- RTCx->ALSEC = pFullTime->SEC & RTC_SEC_MASK;
- RTCx->ALMON = pFullTime->MONTH & RTC_MONTH_MASK;
- RTCx->ALYEAR = pFullTime->YEAR & RTC_YEAR_MASK;
-}
-
-
-/*********************************************************************//**
- * @brief Get full of alarm time in RTC peripheral
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @param[in] pFullTime Pointer to a RTC_TIME_Type structure that
- * will be stored alarm time in full.
- * @return None
- **********************************************************************/
-void RTC_GetFullAlarmTime (LPC_RTC_TypeDef *RTCx, RTC_TIME_Type *pFullTime)
-{
- CHECK_PARAM(PARAM_RTCx(RTCx));
-
- pFullTime->DOM = RTCx->ALDOM & RTC_DOM_MASK;
- pFullTime->DOW = RTCx->ALDOW & RTC_DOW_MASK;
- pFullTime->DOY = RTCx->ALDOY & RTC_DOY_MASK;
- pFullTime->HOUR = RTCx->ALHOUR & RTC_HOUR_MASK;
- pFullTime->MIN = RTCx->ALMIN & RTC_MIN_MASK;
- pFullTime->SEC = RTCx->ALSEC & RTC_SEC_MASK;
- pFullTime->MONTH = RTCx->ALMON & RTC_MONTH_MASK;
- pFullTime->YEAR = RTCx->ALYEAR & RTC_YEAR_MASK;
-}
-
-
-/*********************************************************************//**
- * @brief Check whether if specified Location interrupt in
- * RTC peripheral is set or not
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @param[in] IntType Interrupt location type, should be:
- * - RTC_INT_COUNTER_INCREASE: Counter Increment Interrupt
- * block generated an interrupt.
- * - RTC_INT_ALARM: Alarm generated an
- * interrupt.
- * @return New state of specified Location interrupt in RTC peripheral
- * (SET or RESET)
- **********************************************************************/
-IntStatus RTC_GetIntPending (LPC_RTC_TypeDef *RTCx, uint32_t IntType)
-{
- CHECK_PARAM(PARAM_RTCx(RTCx));
- CHECK_PARAM(PARAM_RTC_INT(IntType));
-
- return ((RTCx->ILR & IntType) ? SET : RESET);
-}
-
-
-/*********************************************************************//**
- * @brief Clear specified Location interrupt pending in
- * RTC peripheral
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @param[in] IntType Interrupt location type, should be:
- * - RTC_INT_COUNTER_INCREASE: Clear Counter Increment
- * Interrupt pending.
- * - RTC_INT_ALARM: Clear alarm interrupt pending
- * @return None
- **********************************************************************/
-void RTC_ClearIntPending (LPC_RTC_TypeDef *RTCx, uint32_t IntType)
-{
- CHECK_PARAM(PARAM_RTCx(RTCx));
- CHECK_PARAM(PARAM_RTC_INT(IntType));
-
- RTCx->ILR |= IntType;
-}
-
-/*********************************************************************//**
- * @brief Enable/Disable calibration counter in RTC peripheral
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @param[in] NewState New State of this function, should be:
- * - ENABLE: The calibration counter is enabled and counting
- * - DISABLE: The calibration counter is disabled and reset to zero
- * @return None
- **********************************************************************/
-void RTC_CalibCounterCmd(LPC_RTC_TypeDef *RTCx, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_RTCx(RTCx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- if (NewState == ENABLE)
- {
- RTCx->CCR &= (~RTC_CCR_CCALEN) & RTC_CCR_BITMASK;
- }
- else
- {
- RTCx->CCR |= RTC_CCR_CCALEN;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Configures Calibration in RTC peripheral
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @param[in] CalibValue Calibration value, should be in range from
- * 0 to 131,072
- * @param[in] CalibDir Calibration Direction, should be:
- * - RTC_CALIB_DIR_FORWARD: Forward calibration
- * - RTC_CALIB_DIR_BACKWARD: Backward calibration
- * @return None
- **********************************************************************/
-void RTC_CalibConfig(LPC_RTC_TypeDef *RTCx, uint32_t CalibValue, uint8_t CalibDir)
-{
- CHECK_PARAM(PARAM_RTCx(RTCx));
- CHECK_PARAM(PARAM_RTC_CALIB_DIR(CalibDir));
- CHECK_PARAM(CalibValue < RTC_CALIBRATION_MAX);
-
- RTCx->CALIBRATION = ((CalibValue) & RTC_CALIBRATION_CALVAL_MASK) \
- | ((CalibDir == RTC_CALIB_DIR_BACKWARD) ? RTC_CALIBRATION_LIBDIR : 0);
-}
-
-
-/*********************************************************************//**
- * @brief Write value to General purpose registers
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @param[in] Channel General purpose registers Channel number,
- * should be in range from 0 to 4.
- * @param[in] Value Value to write
- * @return None
- * Note: These General purpose registers can be used to store important
- * information when the main power supply is off. The value in these
- * registers is not affected by chip reset.
- **********************************************************************/
-void RTC_WriteGPREG (LPC_RTC_TypeDef *RTCx, uint8_t Channel, uint32_t Value)
-{
- uint32_t *preg;
-
- CHECK_PARAM(PARAM_RTCx(RTCx));
- CHECK_PARAM(PARAM_RTC_GPREG_CH(Channel));
-
- preg = (uint32_t *)&RTCx->GPREG0;
- preg += Channel;
- *preg = Value;
-}
-
-
-/*********************************************************************//**
- * @brief Read value from General purpose registers
- * @param[in] RTCx RTC peripheral selected, should be LPC_RTC
- * @param[in] Channel General purpose registers Channel number,
- * should be in range from 0 to 4.
- * @return Read Value
- * Note: These General purpose registers can be used to store important
- * information when the main power supply is off. The value in these
- * registers is not affected by chip reset.
- **********************************************************************/
-uint32_t RTC_ReadGPREG (LPC_RTC_TypeDef *RTCx, uint8_t Channel)
-{
- uint32_t *preg;
- uint32_t value;
-
- CHECK_PARAM(PARAM_RTCx(RTCx));
- CHECK_PARAM(PARAM_RTC_GPREG_CH(Channel));
-
- preg = (uint32_t *)&RTCx->GPREG0;
- preg += Channel;
- value = *preg;
- return (value);
-}
-
-/**
- * @}
- */
-
-#endif /* _RTC */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
-
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_spi.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_spi.c
deleted file mode 100644
index a690b39fc9..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_spi.c
+++ /dev/null
@@ -1,443 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_spi.c 2010-05-21
-*//**
-* @file lpc17xx_spi.c
-* @brief Contains all functions support for SPI firmware library on LPC17xx
-* @version 2.0
-* @date 21. May. 2010
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2010, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup SPI
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_spi.h"
-#include "lpc17xx_clkpwr.h"
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-#ifdef _SPI
-
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup SPI_Public_Functions
- * @{
- */
-
-/*********************************************************************//**
- * @brief Setup clock rate for SPI device
- * @param[in] SPIx SPI peripheral definition, should be LPC_SPI
- * @param[in] target_clock : clock of SPI (Hz)
- * @return None
- ***********************************************************************/
-void SPI_SetClock (LPC_SPI_TypeDef *SPIx, uint32_t target_clock)
-{
- uint32_t spi_pclk;
- uint32_t prescale, temp;
-
- CHECK_PARAM(PARAM_SPIx(SPIx));
-
- if (SPIx == LPC_SPI){
- spi_pclk = CLKPWR_GetPCLK (CLKPWR_PCLKSEL_SPI);
- } else {
- return;
- }
-
- prescale = 8;
- // Find closest clock to target clock
- while (1){
- temp = target_clock * prescale;
- if (temp >= spi_pclk){
- break;
- }
- prescale += 2;
- if(prescale >= 254){
- break;
- }
- }
-
- // Write to register
- SPIx->SPCCR = SPI_SPCCR_COUNTER(prescale);
-}
-
-
-/*********************************************************************//**
- * @brief De-initializes the SPIx peripheral registers to their
-* default reset values.
- * @param[in] SPIx SPI peripheral selected, should be LPC_SPI
- * @return None
- **********************************************************************/
-void SPI_DeInit(LPC_SPI_TypeDef *SPIx)
-{
- CHECK_PARAM(PARAM_SPIx(SPIx));
-
- if (SPIx == LPC_SPI){
- /* Set up clock and power for SPI module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCSPI, DISABLE);
- }
-}
-
-/*********************************************************************//**
- * @brief Get data bit size per transfer
- * @param[in] SPIx SPI peripheral selected, should be LPC_SPI
- * @return number of bit per transfer, could be 8-16
- **********************************************************************/
-uint8_t SPI_GetDataSize (LPC_SPI_TypeDef *SPIx)
-{
- CHECK_PARAM(PARAM_SPIx(SPIx));
- return ((SPIx->SPCR)>>8 & 0xF);
-}
-
-/********************************************************************//**
- * @brief Initializes the SPIx peripheral according to the specified
-* parameters in the UART_ConfigStruct.
- * @param[in] SPIx SPI peripheral selected, should be LPC_SPI
- * @param[in] SPI_ConfigStruct Pointer to a SPI_CFG_Type structure
-* that contains the configuration information for the
-* specified SPI peripheral.
- * @return None
- *********************************************************************/
-void SPI_Init(LPC_SPI_TypeDef *SPIx, SPI_CFG_Type *SPI_ConfigStruct)
-{
- uint32_t tmp;
-
- CHECK_PARAM(PARAM_SPIx(SPIx));
-
- if(SPIx == LPC_SPI){
- /* Set up clock and power for UART module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCSPI, ENABLE);
- } else {
- return;
- }
-
- // Configure SPI, interrupt is disable as default
- tmp = ((SPI_ConfigStruct->CPHA) | (SPI_ConfigStruct->CPOL) \
- | (SPI_ConfigStruct->DataOrder) | (SPI_ConfigStruct->Databit) \
- | (SPI_ConfigStruct->Mode) | SPI_SPCR_BIT_EN) & SPI_SPCR_BITMASK;
- // write back to SPI control register
- SPIx->SPCR = tmp;
-
- // Set clock rate for SPI peripheral
- SPI_SetClock(SPIx, SPI_ConfigStruct->ClockRate);
-
- // If interrupt flag is set, Write '1' to Clear interrupt flag
- if (SPIx->SPINT & SPI_SPINT_INTFLAG){
- SPIx->SPINT = SPI_SPINT_INTFLAG;
- }
-}
-
-
-
-/*****************************************************************************//**
-* @brief Fills each SPI_InitStruct member with its default value:
-* - CPHA = SPI_CPHA_FIRST
-* - CPOL = SPI_CPOL_HI
-* - ClockRate = 1000000
-* - DataOrder = SPI_DATA_MSB_FIRST
-* - Databit = SPI_DATABIT_8
-* - Mode = SPI_MASTER_MODE
-* @param[in] SPI_InitStruct Pointer to a SPI_CFG_Type structure
-* which will be initialized.
-* @return None
-*******************************************************************************/
-void SPI_ConfigStructInit(SPI_CFG_Type *SPI_InitStruct)
-{
- SPI_InitStruct->CPHA = SPI_CPHA_FIRST;
- SPI_InitStruct->CPOL = SPI_CPOL_HI;
- SPI_InitStruct->ClockRate = 1000000;
- SPI_InitStruct->DataOrder = SPI_DATA_MSB_FIRST;
- SPI_InitStruct->Databit = SPI_DATABIT_8;
- SPI_InitStruct->Mode = SPI_MASTER_MODE;
-}
-
-/*********************************************************************//**
- * @brief Transmit a single data through SPIx peripheral
- * @param[in] SPIx SPI peripheral selected, should be LPC_SPI
- * @param[in] Data Data to transmit (must be 16 or 8-bit long,
- * this depend on SPI data bit number configured)
- * @return none
- **********************************************************************/
-void SPI_SendData(LPC_SPI_TypeDef* SPIx, uint16_t Data)
-{
- CHECK_PARAM(PARAM_SPIx(SPIx));
-
- SPIx->SPDR = Data & SPI_SPDR_BITMASK;
-}
-
-
-
-/*********************************************************************//**
- * @brief Receive a single data from SPIx peripheral
- * @param[in] SPIx SPI peripheral selected, should be LPC_SPI
- * @return Data received (16-bit long)
- **********************************************************************/
-uint16_t SPI_ReceiveData(LPC_SPI_TypeDef* SPIx)
-{
- CHECK_PARAM(PARAM_SPIx(SPIx));
-
- return ((uint16_t) (SPIx->SPDR & SPI_SPDR_BITMASK));
-}
-
-/*********************************************************************//**
- * @brief SPI Read write data function
- * @param[in] SPIx Pointer to SPI peripheral, should be LPC_SPI
- * @param[in] dataCfg Pointer to a SPI_DATA_SETUP_Type structure that
- * contains specified information about transmit
- * data configuration.
- * @param[in] xfType Transfer type, should be:
- * - SPI_TRANSFER_POLLING: Polling mode
- * - SPI_TRANSFER_INTERRUPT: Interrupt mode
- * @return Actual Data length has been transferred in polling mode.
- * In interrupt mode, always return (0)
- * Return (-1) if error.
- * Note: This function can be used in both master and slave mode.
- ***********************************************************************/
-int32_t SPI_ReadWrite (LPC_SPI_TypeDef *SPIx, SPI_DATA_SETUP_Type *dataCfg, \
- SPI_TRANSFER_Type xfType)
-{
- uint8_t *rdata8;
- uint8_t *wdata8;
- uint16_t *rdata16;
- uint16_t *wdata16;
- uint32_t stat;
- uint32_t temp;
- uint8_t dataword;
-
- //read for empty buffer
- temp = SPIx->SPDR;
- //dummy to clear status
- temp = SPIx->SPSR;
- dataCfg->counter = 0;
- dataCfg->status = 0;
-
- if(SPI_GetDataSize (SPIx) == 8)
- dataword = 0;
- else dataword = 1;
- if (xfType == SPI_TRANSFER_POLLING){
-
- if (dataword == 0){
- rdata8 = (uint8_t *)dataCfg->rx_data;
- wdata8 = (uint8_t *)dataCfg->tx_data;
- } else {
- rdata16 = (uint16_t *)dataCfg->rx_data;
- wdata16 = (uint16_t *)dataCfg->tx_data;
- }
-
- while(dataCfg->counter < dataCfg->length)
- {
- // Write data to buffer
- if(dataCfg->tx_data == NULL){
- if (dataword == 0){
- SPI_SendData(SPIx, 0xFF);
- } else {
- SPI_SendData(SPIx, 0xFFFF);
- }
- } else {
- if (dataword == 0){
- SPI_SendData(SPIx, *wdata8);
- wdata8++;
- } else {
- SPI_SendData(SPIx, *wdata16);
- wdata16++;
- }
- }
- // Wait for transfer complete
- while (!((stat = SPIx->SPSR) & SPI_SPSR_SPIF));
- // Check for error
- if (stat & (SPI_SPSR_ABRT | SPI_SPSR_MODF | SPI_SPSR_ROVR | SPI_SPSR_WCOL)){
- // save status
- dataCfg->status = stat | SPI_STAT_ERROR;
- return (dataCfg->counter);
- }
- // Read data from SPI dat
- temp = (uint32_t) SPI_ReceiveData(SPIx);
-
- // Store data to destination
- if (dataCfg->rx_data != NULL)
- {
- if (dataword == 0){
- *(rdata8) = (uint8_t) temp;
- rdata8++;
- } else {
- *(rdata16) = (uint16_t) temp;
- rdata16++;
- }
- }
- // Increase counter
- if (dataword == 0){
- dataCfg->counter++;
- } else {
- dataCfg->counter += 2;
- }
- }
-
- // Return length of actual data transferred
- // save status
- dataCfg->status = stat | SPI_STAT_DONE;
- return (dataCfg->counter);
- }
- // Interrupt mode
- else {
-
- // Check if interrupt flag is already set
- if(SPIx->SPINT & SPI_SPINT_INTFLAG){
- SPIx->SPINT = SPI_SPINT_INTFLAG;
- }
- if (dataCfg->counter < dataCfg->length){
- // Write data to buffer
- if(dataCfg->tx_data == NULL){
- if (dataword == 0){
- SPI_SendData(SPIx, 0xFF);
- } else {
- SPI_SendData(SPIx, 0xFFFF);
- }
- } else {
- if (dataword == 0){
- SPI_SendData(SPIx, (*(uint8_t *)dataCfg->tx_data));
- } else {
- SPI_SendData(SPIx, (*(uint16_t *)dataCfg->tx_data));
- }
- }
- SPI_IntCmd(SPIx, ENABLE);
- } else {
- // Save status
- dataCfg->status = SPI_STAT_DONE;
- }
- return (0);
- }
-}
-
-
-/********************************************************************//**
- * @brief Enable or disable SPIx interrupt.
- * @param[in] SPIx SPI peripheral selected, should be LPC_SPI
- * @param[in] NewState New state of specified UART interrupt type,
- * should be:
- * - ENALBE: Enable this SPI interrupt.
-* - DISALBE: Disable this SPI interrupt.
- * @return None
- *********************************************************************/
-void SPI_IntCmd(LPC_SPI_TypeDef *SPIx, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_SPIx(SPIx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- if (NewState == ENABLE)
- {
- SPIx->SPCR |= SPI_SPCR_SPIE;
- }
- else
- {
- SPIx->SPCR &= (~SPI_SPCR_SPIE) & SPI_SPCR_BITMASK;
- }
-}
-
-
-/********************************************************************//**
- * @brief Checks whether the SPI interrupt flag is set or not.
- * @param[in] SPIx SPI peripheral selected, should be LPC_SPI
- * @return The new state of SPI Interrupt Flag (SET or RESET)
- *********************************************************************/
-IntStatus SPI_GetIntStatus (LPC_SPI_TypeDef *SPIx)
-{
- CHECK_PARAM(PARAM_SPIx(SPIx));
-
- return ((SPIx->SPINT & SPI_SPINT_INTFLAG) ? SET : RESET);
-}
-
-/********************************************************************//**
- * @brief Clear SPI interrupt flag.
- * @param[in] SPIx SPI peripheral selected, should be LPC_SPI
- * @return None
- *********************************************************************/
-void SPI_ClearIntPending(LPC_SPI_TypeDef *SPIx)
-{
- CHECK_PARAM(PARAM_SPIx(SPIx));
-
- SPIx->SPINT = SPI_SPINT_INTFLAG;
-}
-
-/********************************************************************//**
- * @brief Get current value of SPI Status register in SPIx peripheral.
- * @param[in] SPIx SPI peripheral selected, should be LPC_SPI
- * @return Current value of SPI Status register in SPI peripheral.
- * Note: The return value of this function must be used with
- * SPI_CheckStatus() to determine current flag status
- * corresponding to each SPI status type. Because some flags in
- * SPI Status register will be cleared after reading, the next reading
- * SPI Status register could not be correct. So this function used to
- * read SPI status register in one time only, then the return value
- * used to check all flags.
- *********************************************************************/
-uint32_t SPI_GetStatus(LPC_SPI_TypeDef* SPIx)
-{
- CHECK_PARAM(PARAM_SPIx(SPIx));
-
- return (SPIx->SPSR & SPI_SPSR_BITMASK);
-}
-
-/********************************************************************//**
- * @brief Checks whether the specified SPI Status flag is set or not
- * via inputSPIStatus parameter.
- * @param[in] inputSPIStatus Value to check status of each flag type.
- * This value is the return value from SPI_GetStatus().
- * @param[in] SPIStatus Specifies the SPI status flag to check,
- * should be one of the following:
- - SPI_STAT_ABRT: Slave abort.
- - SPI_STAT_MODF: Mode fault.
- - SPI_STAT_ROVR: Read overrun.
- - SPI_STAT_WCOL: Write collision.
- - SPI_STAT_SPIF: SPI transfer complete.
- * @return The new state of SPIStatus (SET or RESET)
- *********************************************************************/
-FlagStatus SPI_CheckStatus (uint32_t inputSPIStatus, uint8_t SPIStatus)
-{
- CHECK_PARAM(PARAM_SPI_STAT(SPIStatus));
-
- return ((inputSPIStatus & SPIStatus) ? SET : RESET);
-}
-
-
-/**
- * @}
- */
-
-#endif /* _SPI */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_ssp.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_ssp.c
deleted file mode 100644
index a5f0c645bd..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_ssp.c
+++ /dev/null
@@ -1,694 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_ssp.c 2010-06-18
-*//**
-* @file lpc17xx_ssp.c
-* @brief Contains all functions support for SSP firmware library on LPC17xx
-* @version 3.0
-* @date 18. June. 2010
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2010, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup SSP
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_ssp.h"
-#include "lpc17xx_clkpwr.h"
-
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-
-#ifdef _SSP
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup SSP_Public_Functions
- * @{
- */
-static void setSSPclock (LPC_SSP_TypeDef *SSPx, uint32_t target_clock);
-
-/*********************************************************************//**
- * @brief Setup clock rate for SSP device
- * @param[in] SSPx SSP peripheral definition, should be:
- * - LPC_SSP0: SSP0 peripheral
- * - LPC_SSP1: SSP1 peripheral
- * @param[in] target_clock : clock of SSP (Hz)
- * @return None
- ***********************************************************************/
-static void setSSPclock (LPC_SSP_TypeDef *SSPx, uint32_t target_clock)
-{
- uint32_t prescale, cr0_div, cmp_clk, ssp_clk;
-
- CHECK_PARAM(PARAM_SSPx(SSPx));
-
- /* The SSP clock is derived from the (main system oscillator / 2),
- so compute the best divider from that clock */
- if (SSPx == LPC_SSP0){
- ssp_clk = CLKPWR_GetPCLK (CLKPWR_PCLKSEL_SSP0);
- } else if (SSPx == LPC_SSP1) {
- ssp_clk = CLKPWR_GetPCLK (CLKPWR_PCLKSEL_SSP1);
- } else {
- return;
- }
-
- /* Find closest divider to get at or under the target frequency.
- Use smallest prescale possible and rely on the divider to get
- the closest target frequency */
- cr0_div = 0;
- cmp_clk = 0xFFFFFFFF;
- prescale = 2;
- while (cmp_clk > target_clock)
- {
- cmp_clk = ssp_clk / ((cr0_div + 1) * prescale);
- if (cmp_clk > target_clock)
- {
- cr0_div++;
- if (cr0_div > 0xFF)
- {
- cr0_div = 0;
- prescale += 2;
- }
- }
- }
-
- /* Write computed prescaler and divider back to register */
- SSPx->CR0 &= (~SSP_CR0_SCR(0xFF)) & SSP_CR0_BITMASK;
- SSPx->CR0 |= (SSP_CR0_SCR(cr0_div)) & SSP_CR0_BITMASK;
- SSPx->CPSR = prescale & SSP_CPSR_BITMASK;
-}
-
-/**
- * @}
- */
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup SSP_Public_Functions
- * @{
- */
-
-/********************************************************************//**
- * @brief Initializes the SSPx peripheral according to the specified
-* parameters in the SSP_ConfigStruct.
- * @param[in] SSPx SSP peripheral selected, should be:
- * - LPC_SSP0: SSP0 peripheral
- * - LPC_SSP1: SSP1 peripheral
- * @param[in] SSP_ConfigStruct Pointer to a SSP_CFG_Type structure
-* that contains the configuration information for the
-* specified SSP peripheral.
- * @return None
- *********************************************************************/
-void SSP_Init(LPC_SSP_TypeDef *SSPx, SSP_CFG_Type *SSP_ConfigStruct)
-{
- uint32_t tmp;
-
- CHECK_PARAM(PARAM_SSPx(SSPx));
-
- if(SSPx == LPC_SSP0) {
- /* Set up clock and power for SSP0 module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCSSP0, ENABLE);
- } else if(SSPx == LPC_SSP1) {
- /* Set up clock and power for SSP1 module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCSSP1, ENABLE);
- } else {
- return;
- }
-
- /* Configure SSP, interrupt is disable, LoopBack mode is disable,
- * SSP is disable, Slave output is disable as default
- */
- tmp = ((SSP_ConfigStruct->CPHA) | (SSP_ConfigStruct->CPOL) \
- | (SSP_ConfigStruct->FrameFormat) | (SSP_ConfigStruct->Databit))
- & SSP_CR0_BITMASK;
- // write back to SSP control register
- SSPx->CR0 = tmp;
-
- tmp = SSP_ConfigStruct->Mode & SSP_CR1_BITMASK;
- // Write back to CR1
- SSPx->CR1 = tmp;
-
- // Set clock rate for SSP peripheral
- setSSPclock(SSPx, SSP_ConfigStruct->ClockRate);
-}
-
-/*********************************************************************//**
- * @brief De-initializes the SSPx peripheral registers to their
-* default reset values.
- * @param[in] SSPx SSP peripheral selected, should be:
- * - LPC_SSP0: SSP0 peripheral
- * - LPC_SSP1: SSP1 peripheral
- * @return None
- **********************************************************************/
-void SSP_DeInit(LPC_SSP_TypeDef* SSPx)
-{
- CHECK_PARAM(PARAM_SSPx(SSPx));
-
- if (SSPx == LPC_SSP0){
- /* Set up clock and power for SSP0 module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCSSP0, DISABLE);
- } else if (SSPx == LPC_SSP1) {
- /* Set up clock and power for SSP1 module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCSSP1, DISABLE);
- }
-}
-
-/*****************************************************************************//**
-* @brief Get data size bit selected
-* @param[in] SSPx pointer to LPC_SSP_TypeDef structure, should be:
-* - LPC_SSP0: SSP0 peripheral
-* - LPC_SSP1: SSP1 peripheral
-* @return Data size, could be:
-* - SSP_DATABIT_4: 4 bit transfer
-* - SSP_DATABIT_5: 5 bit transfer
-* ...
-* - SSP_DATABIT_16: 16 bit transfer
-*******************************************************************************/
-uint8_t SSP_GetDataSize(LPC_SSP_TypeDef* SSPx)
-{
- CHECK_PARAM(PARAM_SSPx(SSPx));
- return (SSPx->CR0 & (0xF));
-}
-
-/*****************************************************************************//**
-* @brief Fills each SSP_InitStruct member with its default value:
-* - CPHA = SSP_CPHA_FIRST
-* - CPOL = SSP_CPOL_HI
-* - ClockRate = 1000000
-* - Databit = SSP_DATABIT_8
-* - Mode = SSP_MASTER_MODE
-* - FrameFormat = SSP_FRAME_SSP
-* @param[in] SSP_InitStruct Pointer to a SSP_CFG_Type structure
-* which will be initialized.
-* @return None
-*******************************************************************************/
-void SSP_ConfigStructInit(SSP_CFG_Type *SSP_InitStruct)
-{
- SSP_InitStruct->CPHA = SSP_CPHA_FIRST;
- SSP_InitStruct->CPOL = SSP_CPOL_HI;
- SSP_InitStruct->ClockRate = 1000000;
- SSP_InitStruct->Databit = SSP_DATABIT_8;
- SSP_InitStruct->Mode = SSP_MASTER_MODE;
- SSP_InitStruct->FrameFormat = SSP_FRAME_SPI;
-}
-
-
-/*********************************************************************//**
- * @brief Enable or disable SSP peripheral's operation
- * @param[in] SSPx SSP peripheral, should be:
- * - LPC_SSP0: SSP0 peripheral
- * - LPC_SSP1: SSP1 peripheral
- * @param[in] NewState New State of SSPx peripheral's operation
- * @return none
- **********************************************************************/
-void SSP_Cmd(LPC_SSP_TypeDef* SSPx, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_SSPx(SSPx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- if (NewState == ENABLE)
- {
- SSPx->CR1 |= SSP_CR1_SSP_EN;
- }
- else
- {
- SSPx->CR1 &= (~SSP_CR1_SSP_EN) & SSP_CR1_BITMASK;
- }
-}
-
-/*********************************************************************//**
- * @brief Enable or disable Loop Back mode function in SSP peripheral
- * @param[in] SSPx SSP peripheral selected, should be:
- * - LPC_SSP0: SSP0 peripheral
- * - LPC_SSP1: SSP1 peripheral
- * @param[in] NewState New State of Loop Back mode, should be:
- * - ENABLE: Enable this function
- * - DISABLE: Disable this function
- * @return None
- **********************************************************************/
-void SSP_LoopBackCmd(LPC_SSP_TypeDef* SSPx, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_SSPx(SSPx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- if (NewState == ENABLE)
- {
- SSPx->CR1 |= SSP_CR1_LBM_EN;
- }
- else
- {
- SSPx->CR1 &= (~SSP_CR1_LBM_EN) & SSP_CR1_BITMASK;
- }
-}
-
-/*********************************************************************//**
- * @brief Enable or disable Slave Output function in SSP peripheral
- * @param[in] SSPx SSP peripheral selected, should be:
- * - LPC_SSP0: SSP0 peripheral
- * - LPC_SSP1: SSP1 peripheral
- * @param[in] NewState New State of Slave Output function, should be:
- * - ENABLE: Slave Output in normal operation
- * - DISABLE: Slave Output is disabled. This blocks
- * SSP controller from driving the transmit data
- * line (MISO)
- * Note: This function is available when SSP peripheral in Slave mode
- * @return None
- **********************************************************************/
-void SSP_SlaveOutputCmd(LPC_SSP_TypeDef* SSPx, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_SSPx(SSPx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- if (NewState == ENABLE)
- {
- SSPx->CR1 &= (~SSP_CR1_SO_DISABLE) & SSP_CR1_BITMASK;
- }
- else
- {
- SSPx->CR1 |= SSP_CR1_SO_DISABLE;
- }
-}
-
-
-
-/*********************************************************************//**
- * @brief Transmit a single data through SSPx peripheral
- * @param[in] SSPx SSP peripheral selected, should be:
- * - LPC_SSP0: SSP0 peripheral
- * - LPC_SSP1: SSP1 peripheral
- * @param[in] Data Data to transmit (must be 16 or 8-bit long,
- * this depend on SSP data bit number configured)
- * @return none
- **********************************************************************/
-void SSP_SendData(LPC_SSP_TypeDef* SSPx, uint16_t Data)
-{
- CHECK_PARAM(PARAM_SSPx(SSPx));
-
- SSPx->DR = SSP_DR_BITMASK(Data);
-}
-
-
-
-/*********************************************************************//**
- * @brief Receive a single data from SSPx peripheral
- * @param[in] SSPx SSP peripheral selected, should be
- * - LPC_SSP0: SSP0 peripheral
- * - LPC_SSP1: SSP1 peripheral
- * @return Data received (16-bit long)
- **********************************************************************/
-uint16_t SSP_ReceiveData(LPC_SSP_TypeDef* SSPx)
-{
- CHECK_PARAM(PARAM_SSPx(SSPx));
-
- return ((uint16_t) (SSP_DR_BITMASK(SSPx->DR)));
-}
-
-/*********************************************************************//**
- * @brief SSP Read write data function
- * @param[in] SSPx Pointer to SSP peripheral, should be
- * - LPC_SSP0: SSP0 peripheral
- * - LPC_SSP1: SSP1 peripheral
- * @param[in] dataCfg Pointer to a SSP_DATA_SETUP_Type structure that
- * contains specified information about transmit
- * data configuration.
- * @param[in] xfType Transfer type, should be:
- * - SSP_TRANSFER_POLLING: Polling mode
- * - SSP_TRANSFER_INTERRUPT: Interrupt mode
- * @return Actual Data length has been transferred in polling mode.
- * In interrupt mode, always return (0)
- * Return (-1) if error.
- * Note: This function can be used in both master and slave mode.
- ***********************************************************************/
-int32_t SSP_ReadWrite (LPC_SSP_TypeDef *SSPx, SSP_DATA_SETUP_Type *dataCfg, \
- SSP_TRANSFER_Type xfType)
-{
- uint8_t *rdata8;
- uint8_t *wdata8;
- uint16_t *rdata16;
- uint16_t *wdata16;
- uint32_t stat;
- uint32_t tmp;
- int32_t dataword;
-
- dataCfg->rx_cnt = 0;
- dataCfg->tx_cnt = 0;
- dataCfg->status = 0;
-
-
- /* Clear all remaining data in RX FIFO */
- while (SSPx->SR & SSP_SR_RNE){
- tmp = (uint32_t) SSP_ReceiveData(SSPx);
- }
-
- // Clear status
- SSPx->ICR = SSP_ICR_BITMASK;
- if(SSP_GetDataSize(SSPx)>SSP_DATABIT_8)
- dataword = 1;
- else dataword = 0;
-
- // Polling mode ----------------------------------------------------------------------
- if (xfType == SSP_TRANSFER_POLLING){
- if (dataword == 0){
- rdata8 = (uint8_t *)dataCfg->rx_data;
- wdata8 = (uint8_t *)dataCfg->tx_data;
- } else {
- rdata16 = (uint16_t *)dataCfg->rx_data;
- wdata16 = (uint16_t *)dataCfg->tx_data;
- }
- while ((dataCfg->tx_cnt < dataCfg->length) || (dataCfg->rx_cnt < dataCfg->length)){
- if ((SSPx->SR & SSP_SR_TNF) && (dataCfg->tx_cnt < dataCfg->length)){
- // Write data to buffer
- if(dataCfg->tx_data == NULL){
- if (dataword == 0){
- SSP_SendData(SSPx, 0xFF);
- dataCfg->tx_cnt++;
- } else {
- SSP_SendData(SSPx, 0xFFFF);
- dataCfg->tx_cnt += 2;
- }
- } else {
- if (dataword == 0){
- SSP_SendData(SSPx, *wdata8);
- wdata8++;
- dataCfg->tx_cnt++;
- } else {
- SSP_SendData(SSPx, *wdata16);
- wdata16++;
- dataCfg->tx_cnt += 2;
- }
- }
- }
-
- // Check overrun error
- if ((stat = SSPx->RIS) & SSP_RIS_ROR){
- // save status and return
- dataCfg->status = stat | SSP_STAT_ERROR;
- return (-1);
- }
-
- // Check for any data available in RX FIFO
- while ((SSPx->SR & SSP_SR_RNE) && (dataCfg->rx_cnt < dataCfg->length)){
- // Read data from SSP data
- tmp = SSP_ReceiveData(SSPx);
-
- // Store data to destination
- if (dataCfg->rx_data != NULL)
- {
- if (dataword == 0){
- *(rdata8) = (uint8_t) tmp;
- rdata8++;
- } else {
- *(rdata16) = (uint16_t) tmp;
- rdata16++;
- }
- }
- // Increase counter
- if (dataword == 0){
- dataCfg->rx_cnt++;
- } else {
- dataCfg->rx_cnt += 2;
- }
- }
- }
-
- // save status
- dataCfg->status = SSP_STAT_DONE;
-
- if (dataCfg->tx_data != NULL){
- return dataCfg->tx_cnt;
- } else if (dataCfg->rx_data != NULL){
- return dataCfg->rx_cnt;
- } else {
- return (0);
- }
- }
-
- // Interrupt mode ----------------------------------------------------------------------
- else if (xfType == SSP_TRANSFER_INTERRUPT){
-
- while ((SSPx->SR & SSP_SR_TNF) && (dataCfg->tx_cnt < dataCfg->length)){
- // Write data to buffer
- if(dataCfg->tx_data == NULL){
- if (dataword == 0){
- SSP_SendData(SSPx, 0xFF);
- dataCfg->tx_cnt++;
- } else {
- SSP_SendData(SSPx, 0xFFFF);
- dataCfg->tx_cnt += 2;
- }
- } else {
- if (dataword == 0){
- SSP_SendData(SSPx, (*(uint8_t *)((uint32_t)dataCfg->tx_data + dataCfg->tx_cnt)));
- dataCfg->tx_cnt++;
- } else {
- SSP_SendData(SSPx, (*(uint16_t *)((uint32_t)dataCfg->tx_data + dataCfg->tx_cnt)));
- dataCfg->tx_cnt += 2;
- }
- }
-
- // Check error
- if ((stat = SSPx->RIS) & SSP_RIS_ROR){
- // save status and return
- dataCfg->status = stat | SSP_STAT_ERROR;
- return (-1);
- }
-
- // Check for any data available in RX FIFO
- while ((SSPx->SR & SSP_SR_RNE) && (dataCfg->rx_cnt < dataCfg->length)){
- // Read data from SSP data
- tmp = SSP_ReceiveData(SSPx);
-
- // Store data to destination
- if (dataCfg->rx_data != NULL)
- {
- if (dataword == 0){
- *(uint8_t *)((uint32_t)dataCfg->rx_data + dataCfg->rx_cnt) = (uint8_t) tmp;
- } else {
- *(uint16_t *)((uint32_t)dataCfg->rx_data + dataCfg->rx_cnt) = (uint16_t) tmp;
- }
- }
- // Increase counter
- if (dataword == 0){
- dataCfg->rx_cnt++;
- } else {
- dataCfg->rx_cnt += 2;
- }
- }
- }
-
- // If there more data to sent or receive
- if ((dataCfg->rx_cnt < dataCfg->length) || (dataCfg->tx_cnt < dataCfg->length)){
- // Enable all interrupt
- SSPx->IMSC = SSP_IMSC_BITMASK;
- } else {
- // Save status
- dataCfg->status = SSP_STAT_DONE;
- }
- return (0);
- }
-
- return (-1);
-}
-
-/*********************************************************************//**
- * @brief Checks whether the specified SSP status flag is set or not
- * @param[in] SSPx SSP peripheral selected, should be:
- * - LPC_SSP0: SSP0 peripheral
- * - LPC_SSP1: SSP1 peripheral
- * @param[in] FlagType Type of flag to check status, should be one
- * of following:
- * - SSP_STAT_TXFIFO_EMPTY: TX FIFO is empty
- * - SSP_STAT_TXFIFO_NOTFULL: TX FIFO is not full
- * - SSP_STAT_RXFIFO_NOTEMPTY: RX FIFO is not empty
- * - SSP_STAT_RXFIFO_FULL: RX FIFO is full
- * - SSP_STAT_BUSY: SSP peripheral is busy
- * @return New State of specified SSP status flag
- **********************************************************************/
-FlagStatus SSP_GetStatus(LPC_SSP_TypeDef* SSPx, uint32_t FlagType)
-{
- CHECK_PARAM(PARAM_SSPx(SSPx));
- CHECK_PARAM(PARAM_SSP_STAT(FlagType));
-
- return ((SSPx->SR & FlagType) ? SET : RESET);
-}
-
-/*********************************************************************//**
- * @brief Enable or disable specified interrupt type in SSP peripheral
- * @param[in] SSPx SSP peripheral selected, should be:
- * - LPC_SSP0: SSP0 peripheral
- * - LPC_SSP1: SSP1 peripheral
- * @param[in] IntType Interrupt type in SSP peripheral, should be:
- * - SSP_INTCFG_ROR: Receive Overrun interrupt
- * - SSP_INTCFG_RT: Receive Time out interrupt
- * - SSP_INTCFG_RX: RX FIFO is at least half full interrupt
- * - SSP_INTCFG_TX: TX FIFO is at least half empty interrupt
- * @param[in] NewState New State of specified interrupt type, should be:
- * - ENABLE: Enable this interrupt type
- * - DISABLE: Disable this interrupt type
- * @return None
- * Note: We can enable/disable multi-interrupt type by OR multi value
- **********************************************************************/
-void SSP_IntConfig(LPC_SSP_TypeDef *SSPx, uint32_t IntType, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_SSPx(SSPx));
-
- if (NewState == ENABLE)
- {
- SSPx->IMSC |= IntType;
- }
- else
- {
- SSPx->IMSC &= (~IntType) & SSP_IMSC_BITMASK;
- }
-}
-
-/*********************************************************************//**
- * @brief Check whether the specified Raw interrupt status flag is
- * set or not
- * @param[in] SSPx SSP peripheral selected, should be:
- * - LPC_SSP0: SSP0 peripheral
- * - LPC_SSP1: SSP1 peripheral
- * @param[in] RawIntType Raw Interrupt Type, should be:
- * - SSP_INTSTAT_RAW_ROR: Receive Overrun interrupt
- * - SSP_INTSTAT_RAW_RT: Receive Time out interrupt
- * - SSP_INTSTAT_RAW_RX: RX FIFO is at least half full interrupt
- * - SSP_INTSTAT_RAW_TX: TX FIFO is at least half empty interrupt
- * @return New State of specified Raw interrupt status flag in SSP peripheral
- * Note: Enabling/Disabling specified interrupt in SSP peripheral does not
- * effect to Raw Interrupt Status flag.
- **********************************************************************/
-IntStatus SSP_GetRawIntStatus(LPC_SSP_TypeDef *SSPx, uint32_t RawIntType)
-{
- CHECK_PARAM(PARAM_SSPx(SSPx));
- CHECK_PARAM(PARAM_SSP_INTSTAT_RAW(RawIntType));
-
- return ((SSPx->RIS & RawIntType) ? SET : RESET);
-}
-
-/*********************************************************************//**
- * @brief Get Raw Interrupt Status register
- * @param[in] SSPx SSP peripheral selected, should be:
- * - LPC_SSP0: SSP0 peripheral
- * - LPC_SSP1: SSP1 peripheral
- * @return Raw Interrupt Status (RIS) register value
- **********************************************************************/
-uint32_t SSP_GetRawIntStatusReg(LPC_SSP_TypeDef *SSPx)
-{
- CHECK_PARAM(PARAM_SSPx(SSPx));
- return (SSPx->RIS);
-}
-
-/*********************************************************************//**
- * @brief Check whether the specified interrupt status flag is
- * set or not
- * @param[in] SSPx SSP peripheral selected, should be:
- * - LPC_SSP0: SSP0 peripheral
- * - LPC_SSP1: SSP1 peripheral
- * @param[in] IntType Raw Interrupt Type, should be:
- * - SSP_INTSTAT_ROR: Receive Overrun interrupt
- * - SSP_INTSTAT_RT: Receive Time out interrupt
- * - SSP_INTSTAT_RX: RX FIFO is at least half full interrupt
- * - SSP_INTSTAT_TX: TX FIFO is at least half empty interrupt
- * @return New State of specified interrupt status flag in SSP peripheral
- * Note: Enabling/Disabling specified interrupt in SSP peripheral effects
- * to Interrupt Status flag.
- **********************************************************************/
-IntStatus SSP_GetIntStatus (LPC_SSP_TypeDef *SSPx, uint32_t IntType)
-{
- CHECK_PARAM(PARAM_SSPx(SSPx));
- CHECK_PARAM(PARAM_SSP_INTSTAT(IntType));
-
- return ((SSPx->MIS & IntType) ? SET :RESET);
-}
-
-/*********************************************************************//**
- * @brief Clear specified interrupt pending in SSP peripheral
- * @param[in] SSPx SSP peripheral selected, should be:
- * - LPC_SSP0: SSP0 peripheral
- * - LPC_SSP1: SSP1 peripheral
- * @param[in] IntType Interrupt pending to clear, should be:
- * - SSP_INTCLR_ROR: clears the "frame was received when
- * RxFIFO was full" interrupt.
- * - SSP_INTCLR_RT: clears the "Rx FIFO was not empty and
- * has not been read for a timeout period" interrupt.
- * @return None
- **********************************************************************/
-void SSP_ClearIntPending(LPC_SSP_TypeDef *SSPx, uint32_t IntType)
-{
- CHECK_PARAM(PARAM_SSPx(SSPx));
- CHECK_PARAM(PARAM_SSP_INTCLR(IntType));
-
- SSPx->ICR = IntType;
-}
-
-/*********************************************************************//**
- * @brief Enable/Disable DMA function for SSP peripheral
- * @param[in] SSPx SSP peripheral selected, should be:
- * - LPC_SSP0: SSP0 peripheral
- * - LPC_SSP1: SSP1 peripheral
- * @param[in] DMAMode Type of DMA, should be:
- * - SSP_DMA_TX: DMA for the transmit FIFO
- * - SSP_DMA_RX: DMA for the Receive FIFO
- * @param[in] NewState New State of DMA function on SSP peripheral,
- * should be:
- * - ENALBE: Enable this function
- * - DISABLE: Disable this function
- * @return None
- **********************************************************************/
-void SSP_DMACmd(LPC_SSP_TypeDef *SSPx, uint32_t DMAMode, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_SSPx(SSPx));
- CHECK_PARAM(PARAM_SSP_DMA(DMAMode));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- if (NewState == ENABLE)
- {
- SSPx->DMACR |= DMAMode;
- }
- else
- {
- SSPx->DMACR &= (~DMAMode) & SSP_DMA_BITMASK;
- }
-}
-
-/**
- * @}
- */
-
-#endif /* _SSP */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
-
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_systick.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_systick.c
deleted file mode 100644
index bc10c1c6d5..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_systick.c
+++ /dev/null
@@ -1,193 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_systick.c 2010-05-21
-*//**
-* @file lpc17xx_systick.c
-* @brief Contains all functions support for SYSTICK firmware library
-* on LPC17xx
-* @version 2.0
-* @date 21. May. 2010
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2010, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup SYSTICK
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_systick.h"
-#include "lpc17xx_clkpwr.h"
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-
-#ifdef _SYSTICK
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup SYSTICK_Public_Functions
- * @{
- */
-/*********************************************************************//**
- * @brief Initial System Tick with using internal CPU clock source
- * @param[in] time time interval(ms)
- * @return None
- **********************************************************************/
-void SYSTICK_InternalInit(uint32_t time)
-{
- uint32_t cclk;
- float maxtime;
-
- cclk = SystemCoreClock;
- /* With internal CPU clock frequency for LPC17xx is 'SystemCoreClock'
- * And limit 24 bit for RELOAD value
- * So the maximum time can be set:
- * 1/SystemCoreClock * (2^24) * 1000 (ms)
- */
- //check time value is available or not
- maxtime = (1<<24)/(SystemCoreClock / 1000);
- if(time > maxtime)
- //Error loop
- while(1);
- else
- {
- //Select CPU clock is System Tick clock source
- SysTick->CTRL |= ST_CTRL_CLKSOURCE;
- /* Set RELOAD value
- * RELOAD = (SystemCoreClock/1000) * time - 1
- * with time base is millisecond
- */
- SysTick->LOAD = (cclk/1000)*time - 1;
- }
-}
-
-/*********************************************************************//**
- * @brief Initial System Tick with using external clock source
- * @param[in] freq external clock frequency(Hz)
- * @param[in] time time interval(ms)
- * @return None
- **********************************************************************/
-void SYSTICK_ExternalInit(uint32_t freq, uint32_t time)
-{
- float maxtime;
-
- /* With external clock frequency for LPC17xx is 'freq'
- * And limit 24 bit for RELOAD value
- * So the maximum time can be set:
- * 1/freq * (2^24) * 1000 (ms)
- */
- //check time value is available or not
- maxtime = (1<<24)/(freq / 1000);
- if (time>maxtime)
- //Error Loop
- while(1);
- else
- {
- //Select external clock is System Tick clock source
- SysTick->CTRL &= ~ ST_CTRL_CLKSOURCE;
- /* Set RELOAD value
- * RELOAD = (freq/1000) * time - 1
- * with time base is millisecond
- */
- maxtime = (freq/1000)*time - 1;
- SysTick->LOAD = (freq/1000)*time - 1;
- }
-}
-
-/*********************************************************************//**
- * @brief Enable/disable System Tick counter
- * @param[in] NewState System Tick counter status, should be:
- * - ENABLE
- * - DISABLE
- * @return None
- **********************************************************************/
-void SYSTICK_Cmd(FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- if(NewState == ENABLE)
- //Enable System Tick counter
- SysTick->CTRL |= ST_CTRL_ENABLE;
- else
- //Disable System Tick counter
- SysTick->CTRL &= ~ST_CTRL_ENABLE;
-}
-
-/*********************************************************************//**
- * @brief Enable/disable System Tick interrupt
- * @param[in] NewState System Tick interrupt status, should be:
- * - ENABLE
- * - DISABLE
- * @return None
- **********************************************************************/
-void SYSTICK_IntCmd(FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- if(NewState == ENABLE)
- //Enable System Tick counter
- SysTick->CTRL |= ST_CTRL_TICKINT;
- else
- //Disable System Tick counter
- SysTick->CTRL &= ~ST_CTRL_TICKINT;
-}
-
-/*********************************************************************//**
- * @brief Get current value of System Tick counter
- * @param[in] None
- * @return current value of System Tick counter
- **********************************************************************/
-uint32_t SYSTICK_GetCurrentValue(void)
-{
- return (SysTick->VAL);
-}
-
-/*********************************************************************//**
- * @brief Clear Counter flag
- * @param[in] None
- * @return None
- **********************************************************************/
-void SYSTICK_ClearCounterFlag(void)
-{
- SysTick->CTRL &= ~ST_CTRL_COUNTFLAG;
-}
-/**
- * @}
- */
-
-#endif /* _SYSTICK */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
-
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_timer.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_timer.c
deleted file mode 100644
index 0e697dc3e5..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_timer.c
+++ /dev/null
@@ -1,609 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_timer.c 2011-03-10
-*//**
-* @file lpc17xx_timer.c
-* @brief Contains all functions support for Timer firmware library
-* on LPC17xx
-* @version 3.1
-* @date 10. March. 2011
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2011, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup TIM
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_timer.h"
-#include "lpc17xx_clkpwr.h"
-#include "lpc17xx_pinsel.h"
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-#ifdef _TIM
-
-/* Private Functions ---------------------------------------------------------- */
-
-static uint32_t getPClock (uint32_t timernum);
-static uint32_t converUSecToVal (uint32_t timernum, uint32_t usec);
-static uint32_t converPtrToTimeNum (LPC_TIM_TypeDef *TIMx);
-
-
-/*********************************************************************//**
- * @brief Get peripheral clock of each timer controller
- * @param[in] timernum Timer number
- * @return Peripheral clock of timer
- **********************************************************************/
-static uint32_t getPClock (uint32_t timernum)
-{
- uint32_t clkdlycnt;
- switch (timernum)
- {
- case 0:
- clkdlycnt = CLKPWR_GetPCLK (CLKPWR_PCLKSEL_TIMER0);
- break;
-
- case 1:
- clkdlycnt = CLKPWR_GetPCLK (CLKPWR_PCLKSEL_TIMER1);
- break;
-
- case 2:
- clkdlycnt = CLKPWR_GetPCLK (CLKPWR_PCLKSEL_TIMER2);
- break;
-
- case 3:
- clkdlycnt = CLKPWR_GetPCLK (CLKPWR_PCLKSEL_TIMER3);
- break;
- }
- return clkdlycnt;
-}
-
-
-/*********************************************************************//**
- * @brief Convert a time to a timer count value
- * @param[in] timernum Timer number
- * @param[in] usec Time in microseconds
- * @return The number of required clock ticks to give the time delay
- **********************************************************************/
-uint32_t converUSecToVal (uint32_t timernum, uint32_t usec)
-{
- uint64_t clkdlycnt;
-
- // Get Pclock of timer
- clkdlycnt = (uint64_t) getPClock(timernum);
-
- clkdlycnt = (clkdlycnt * usec) / 1000000;
- return (uint32_t) clkdlycnt;
-}
-
-
-/*********************************************************************//**
- * @brief Convert a timer register pointer to a timer number
- * @param[in] TIMx Pointer to LPC_TIM_TypeDef, should be:
- * - LPC_TIM0: TIMER0 peripheral
- * - LPC_TIM1: TIMER1 peripheral
- * - LPC_TIM2: TIMER2 peripheral
- * - LPC_TIM3: TIMER3 peripheral
- * @return The timer number (0 to 3) or 0xFFFF FFFF if register pointer is bad
- **********************************************************************/
-uint32_t converPtrToTimeNum (LPC_TIM_TypeDef *TIMx)
-{
- uint32_t tnum = 0xFFFFFFFF;
-
- if (TIMx == LPC_TIM0)
- {
- tnum = 0;
- }
- else if (TIMx == LPC_TIM1)
- {
- tnum = 1;
- }
- else if (TIMx == LPC_TIM2)
- {
- tnum = 2;
- }
- else if (TIMx == LPC_TIM3)
- {
- tnum = 3;
- }
-
- return tnum;
-}
-
-/* End of Private Functions ---------------------------------------------------- */
-
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup TIM_Public_Functions
- * @{
- */
-
-/*********************************************************************//**
- * @brief Get Interrupt Status
- * @param[in] TIMx Timer selection, should be:
- * - LPC_TIM0: TIMER0 peripheral
- * - LPC_TIM1: TIMER1 peripheral
- * - LPC_TIM2: TIMER2 peripheral
- * - LPC_TIM3: TIMER3 peripheral
- * @param[in] IntFlag: interrupt type, should be:
- * - TIM_MR0_INT: Interrupt for Match channel 0
- * - TIM_MR1_INT: Interrupt for Match channel 1
- * - TIM_MR2_INT: Interrupt for Match channel 2
- * - TIM_MR3_INT: Interrupt for Match channel 3
- * - TIM_CR0_INT: Interrupt for Capture channel 0
- * - TIM_CR1_INT: Interrupt for Capture channel 1
- * @return FlagStatus
- * - SET : interrupt
- * - RESET : no interrupt
- **********************************************************************/
-FlagStatus TIM_GetIntStatus(LPC_TIM_TypeDef *TIMx, TIM_INT_TYPE IntFlag)
-{
- uint8_t temp;
- CHECK_PARAM(PARAM_TIMx(TIMx));
- CHECK_PARAM(PARAM_TIM_INT_TYPE(IntFlag));
- temp = (TIMx->IR)& TIM_IR_CLR(IntFlag);
- if (temp)
- return SET;
-
- return RESET;
-
-}
-/*********************************************************************//**
- * @brief Get Capture Interrupt Status
- * @param[in] TIMx Timer selection, should be:
- * - LPC_TIM0: TIMER0 peripheral
- * - LPC_TIM1: TIMER1 peripheral
- * - LPC_TIM2: TIMER2 peripheral
- * - LPC_TIM3: TIMER3 peripheral
- * @param[in] IntFlag: interrupt type, should be:
- * - TIM_MR0_INT: Interrupt for Match channel 0
- * - TIM_MR1_INT: Interrupt for Match channel 1
- * - TIM_MR2_INT: Interrupt for Match channel 2
- * - TIM_MR3_INT: Interrupt for Match channel 3
- * - TIM_CR0_INT: Interrupt for Capture channel 0
- * - TIM_CR1_INT: Interrupt for Capture channel 1
- * @return FlagStatus
- * - SET : interrupt
- * - RESET : no interrupt
- **********************************************************************/
-FlagStatus TIM_GetIntCaptureStatus(LPC_TIM_TypeDef *TIMx, TIM_INT_TYPE IntFlag)
-{
- uint8_t temp;
- CHECK_PARAM(PARAM_TIMx(TIMx));
- CHECK_PARAM(PARAM_TIM_INT_TYPE(IntFlag));
- temp = (TIMx->IR) & (1<<(4+IntFlag));
- if(temp)
- return SET;
- return RESET;
-}
-/*********************************************************************//**
- * @brief Clear Interrupt pending
- * @param[in] TIMx Timer selection, should be:
- * - LPC_TIM0: TIMER0 peripheral
- * - LPC_TIM1: TIMER1 peripheral
- * - LPC_TIM2: TIMER2 peripheral
- * - LPC_TIM3: TIMER3 peripheral
- * @param[in] IntFlag: interrupt type, should be:
- * - TIM_MR0_INT: Interrupt for Match channel 0
- * - TIM_MR1_INT: Interrupt for Match channel 1
- * - TIM_MR2_INT: Interrupt for Match channel 2
- * - TIM_MR3_INT: Interrupt for Match channel 3
- * - TIM_CR0_INT: Interrupt for Capture channel 0
- * - TIM_CR1_INT: Interrupt for Capture channel 1
- * @return None
- **********************************************************************/
-void TIM_ClearIntPending(LPC_TIM_TypeDef *TIMx, TIM_INT_TYPE IntFlag)
-{
- CHECK_PARAM(PARAM_TIMx(TIMx));
- CHECK_PARAM(PARAM_TIM_INT_TYPE(IntFlag));
- TIMx->IR = TIM_IR_CLR(IntFlag);
-}
-
-/*********************************************************************//**
- * @brief Clear Capture Interrupt pending
- * @param[in] TIMx Timer selection, should be
- * - LPC_TIM0: TIMER0 peripheral
- * - LPC_TIM1: TIMER1 peripheral
- * - LPC_TIM2: TIMER2 peripheral
- * - LPC_TIM3: TIMER3 peripheral
- * @param[in] IntFlag interrupt type, should be:
- * - TIM_MR0_INT: Interrupt for Match channel 0
- * - TIM_MR1_INT: Interrupt for Match channel 1
- * - TIM_MR2_INT: Interrupt for Match channel 2
- * - TIM_MR3_INT: Interrupt for Match channel 3
- * - TIM_CR0_INT: Interrupt for Capture channel 0
- * - TIM_CR1_INT: Interrupt for Capture channel 1
- * @return None
- **********************************************************************/
-void TIM_ClearIntCapturePending(LPC_TIM_TypeDef *TIMx, TIM_INT_TYPE IntFlag)
-{
- CHECK_PARAM(PARAM_TIMx(TIMx));
- CHECK_PARAM(PARAM_TIM_INT_TYPE(IntFlag));
- TIMx->IR = (1<<(4+IntFlag));
-}
-
-/*********************************************************************//**
- * @brief Configuration for Timer at initial time
- * @param[in] TimerCounterMode timer counter mode, should be:
- * - TIM_TIMER_MODE: Timer mode
- * - TIM_COUNTER_RISING_MODE: Counter rising mode
- * - TIM_COUNTER_FALLING_MODE: Counter falling mode
- * - TIM_COUNTER_ANY_MODE:Counter on both edges
- * @param[in] TIM_ConfigStruct pointer to TIM_TIMERCFG_Type or
- * TIM_COUNTERCFG_Type
- * @return None
- **********************************************************************/
-void TIM_ConfigStructInit(TIM_MODE_OPT TimerCounterMode, void *TIM_ConfigStruct)
-{
- if (TimerCounterMode == TIM_TIMER_MODE )
- {
- TIM_TIMERCFG_Type * pTimeCfg = (TIM_TIMERCFG_Type *)TIM_ConfigStruct;
- pTimeCfg->PrescaleOption = TIM_PRESCALE_USVAL;
- pTimeCfg->PrescaleValue = 1;
- }
- else
- {
- TIM_COUNTERCFG_Type * pCounterCfg = (TIM_COUNTERCFG_Type *)TIM_ConfigStruct;
- pCounterCfg->CountInputSelect = TIM_COUNTER_INCAP0;
- }
-}
-
-/*********************************************************************//**
- * @brief Initial Timer/Counter device
- * Set Clock frequency for Timer
- * Set initial configuration for Timer
- * @param[in] TIMx Timer selection, should be:
- * - LPC_TIM0: TIMER0 peripheral
- * - LPC_TIM1: TIMER1 peripheral
- * - LPC_TIM2: TIMER2 peripheral
- * - LPC_TIM3: TIMER3 peripheral
- * @param[in] TimerCounterMode Timer counter mode, should be:
- * - TIM_TIMER_MODE: Timer mode
- * - TIM_COUNTER_RISING_MODE: Counter rising mode
- * - TIM_COUNTER_FALLING_MODE: Counter falling mode
- * - TIM_COUNTER_ANY_MODE:Counter on both edges
- * @param[in] TIM_ConfigStruct pointer to TIM_TIMERCFG_Type
- * that contains the configuration information for the
- * specified Timer peripheral.
- * @return None
- **********************************************************************/
-void TIM_Init(LPC_TIM_TypeDef *TIMx, TIM_MODE_OPT TimerCounterMode, void *TIM_ConfigStruct)
-{
- TIM_TIMERCFG_Type *pTimeCfg;
- TIM_COUNTERCFG_Type *pCounterCfg;
-
- CHECK_PARAM(PARAM_TIMx(TIMx));
- CHECK_PARAM(PARAM_TIM_MODE_OPT(TimerCounterMode));
-
- //set power
-
- if (TIMx== LPC_TIM0)
- {
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCTIM0, ENABLE);
- //PCLK_Timer0 = CCLK/4
- CLKPWR_SetPCLKDiv (CLKPWR_PCLKSEL_TIMER0, CLKPWR_PCLKSEL_CCLK_DIV_4);
- }
- else if (TIMx== LPC_TIM1)
- {
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCTIM1, ENABLE);
- //PCLK_Timer1 = CCLK/4
- CLKPWR_SetPCLKDiv (CLKPWR_PCLKSEL_TIMER1, CLKPWR_PCLKSEL_CCLK_DIV_4);
-
- }
-
- else if (TIMx== LPC_TIM2)
- {
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCTIM2, ENABLE);
- //PCLK_Timer2= CCLK/4
- CLKPWR_SetPCLKDiv (CLKPWR_PCLKSEL_TIMER2, CLKPWR_PCLKSEL_CCLK_DIV_4);
- }
- else if (TIMx== LPC_TIM3)
- {
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCTIM3, ENABLE);
- //PCLK_Timer3= CCLK/4
- CLKPWR_SetPCLKDiv (CLKPWR_PCLKSEL_TIMER3, CLKPWR_PCLKSEL_CCLK_DIV_4);
-
- }
-
- TIMx->CCR &= ~TIM_CTCR_MODE_MASK;
- TIMx->CCR |= TIM_TIMER_MODE;
-
- TIMx->TC =0;
- TIMx->PC =0;
- TIMx->PR =0;
- TIMx->TCR |= (1<<1); //Reset Counter
- TIMx->TCR &= ~(1<<1); //release reset
- if (TimerCounterMode == TIM_TIMER_MODE )
- {
- pTimeCfg = (TIM_TIMERCFG_Type *)TIM_ConfigStruct;
- if (pTimeCfg->PrescaleOption == TIM_PRESCALE_TICKVAL)
- {
- TIMx->PR = pTimeCfg->PrescaleValue -1 ;
- }
- else
- {
- TIMx->PR = converUSecToVal (converPtrToTimeNum(TIMx),pTimeCfg->PrescaleValue)-1;
- }
- }
- else
- {
-
- pCounterCfg = (TIM_COUNTERCFG_Type *)TIM_ConfigStruct;
- TIMx->CCR &= ~TIM_CTCR_INPUT_MASK;
- if (pCounterCfg->CountInputSelect == TIM_COUNTER_INCAP1)
- TIMx->CCR |= _BIT(2);
- }
-
- // Clear interrupt pending
- TIMx->IR = 0xFFFFFFFF;
-
-}
-
-/*********************************************************************//**
- * @brief Close Timer/Counter device
- * @param[in] TIMx Pointer to timer device, should be:
- * - LPC_TIM0: TIMER0 peripheral
- * - LPC_TIM1: TIMER1 peripheral
- * - LPC_TIM2: TIMER2 peripheral
- * - LPC_TIM3: TIMER3 peripheral
- * @return None
- **********************************************************************/
-void TIM_DeInit (LPC_TIM_TypeDef *TIMx)
-{
- CHECK_PARAM(PARAM_TIMx(TIMx));
- // Disable timer/counter
- TIMx->TCR = 0x00;
-
- // Disable power
- if (TIMx== LPC_TIM0)
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCTIM0, DISABLE);
-
- else if (TIMx== LPC_TIM1)
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCTIM1, DISABLE);
-
- else if (TIMx== LPC_TIM2)
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCTIM2, DISABLE);
-
- else if (TIMx== LPC_TIM3)
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCTIM2, DISABLE);
-
-}
-
-/*********************************************************************//**
- * @brief Start/Stop Timer/Counter device
- * @param[in] TIMx Pointer to timer device, should be:
- * - LPC_TIM0: TIMER0 peripheral
- * - LPC_TIM1: TIMER1 peripheral
- * - LPC_TIM2: TIMER2 peripheral
- * - LPC_TIM3: TIMER3 peripheral
- * @param[in] NewState
- * - ENABLE : set timer enable
- * - DISABLE : disable timer
- * @return None
- **********************************************************************/
-void TIM_Cmd(LPC_TIM_TypeDef *TIMx, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_TIMx(TIMx));
- if (NewState == ENABLE)
- {
- TIMx->TCR |= TIM_ENABLE;
- }
- else
- {
- TIMx->TCR &= ~TIM_ENABLE;
- }
-}
-
-/*********************************************************************//**
- * @brief Reset Timer/Counter device,
- * Make TC and PC are synchronously reset on the next
- * positive edge of PCLK
- * @param[in] TIMx Pointer to timer device, should be:
- * - LPC_TIM0: TIMER0 peripheral
- * - LPC_TIM1: TIMER1 peripheral
- * - LPC_TIM2: TIMER2 peripheral
- * - LPC_TIM3: TIMER3 peripheral
- * @return None
- **********************************************************************/
-void TIM_ResetCounter(LPC_TIM_TypeDef *TIMx)
-{
- CHECK_PARAM(PARAM_TIMx(TIMx));
- TIMx->TCR |= TIM_RESET;
- TIMx->TCR &= ~TIM_RESET;
-}
-
-/*********************************************************************//**
- * @brief Configuration for Match register
- * @param[in] TIMx Pointer to timer device, should be:
- * - LPC_TIM0: TIMER0 peripheral
- * - LPC_TIM1: TIMER1 peripheral
- * - LPC_TIM2: TIMER2 peripheral
- * - LPC_TIM3: TIMER3 peripheral
- * @param[in] TIM_MatchConfigStruct Pointer to TIM_MATCHCFG_Type
- * - MatchChannel : choose channel 0 or 1
- * - IntOnMatch : if SET, interrupt will be generated when MRxx match
- * the value in TC
- * - StopOnMatch : if SET, TC and PC will be stopped whenM Rxx match
- * the value in TC
- * - ResetOnMatch : if SET, Reset on MR0 when MRxx match
- * the value in TC
- * -ExtMatchOutputType: Select output for external match
- * + 0: Do nothing for external output pin if match
- * + 1: Force external output pin to low if match
- * + 2: Force external output pin to high if match
- * + 3: Toggle external output pin if match
- * MatchValue: Set the value to be compared with TC value
- * @return None
- **********************************************************************/
-void TIM_ConfigMatch(LPC_TIM_TypeDef *TIMx, TIM_MATCHCFG_Type *TIM_MatchConfigStruct)
-{
-
- CHECK_PARAM(PARAM_TIMx(TIMx));
- CHECK_PARAM(PARAM_TIM_EXTMATCH_OPT(TIM_MatchConfigStruct->ExtMatchOutputType));
-
- switch(TIM_MatchConfigStruct->MatchChannel)
- {
- case 0:
- TIMx->MR0 = TIM_MatchConfigStruct->MatchValue;
- break;
- case 1:
- TIMx->MR1 = TIM_MatchConfigStruct->MatchValue;
- break;
- case 2:
- TIMx->MR2 = TIM_MatchConfigStruct->MatchValue;
- break;
- case 3:
- TIMx->MR3 = TIM_MatchConfigStruct->MatchValue;
- break;
- default:
- //Error match value
- //Error loop
- while(1);
- }
- //interrupt on MRn
- TIMx->MCR &=~TIM_MCR_CHANNEL_MASKBIT(TIM_MatchConfigStruct->MatchChannel);
-
- if (TIM_MatchConfigStruct->IntOnMatch)
- TIMx->MCR |= TIM_INT_ON_MATCH(TIM_MatchConfigStruct->MatchChannel);
-
- //reset on MRn
- if (TIM_MatchConfigStruct->ResetOnMatch)
- TIMx->MCR |= TIM_RESET_ON_MATCH(TIM_MatchConfigStruct->MatchChannel);
-
- //stop on MRn
- if (TIM_MatchConfigStruct->StopOnMatch)
- TIMx->MCR |= TIM_STOP_ON_MATCH(TIM_MatchConfigStruct->MatchChannel);
-
- // match output type
-
- TIMx->EMR &= ~TIM_EM_MASK(TIM_MatchConfigStruct->MatchChannel);
- TIMx->EMR |= TIM_EM_SET(TIM_MatchConfigStruct->MatchChannel,TIM_MatchConfigStruct->ExtMatchOutputType);
-}
-/*********************************************************************//**
- * @brief Update Match value
- * @param[in] TIMx Pointer to timer device, should be:
- * - LPC_TIM0: TIMER0 peripheral
- * - LPC_TIM1: TIMER1 peripheral
- * - LPC_TIM2: TIMER2 peripheral
- * - LPC_TIM3: TIMER3 peripheral
- * @param[in] MatchChannel Match channel, should be: 0..3
- * @param[in] MatchValue updated match value
- * @return None
- **********************************************************************/
-void TIM_UpdateMatchValue(LPC_TIM_TypeDef *TIMx,uint8_t MatchChannel, uint32_t MatchValue)
-{
- CHECK_PARAM(PARAM_TIMx(TIMx));
- switch(MatchChannel)
- {
- case 0:
- TIMx->MR0 = MatchValue;
- break;
- case 1:
- TIMx->MR1 = MatchValue;
- break;
- case 2:
- TIMx->MR2 = MatchValue;
- break;
- case 3:
- TIMx->MR3 = MatchValue;
- break;
- default:
- //Error Loop
- while(1);
- }
-
-}
-/*********************************************************************//**
- * @brief Configuration for Capture register
- * @param[in] TIMx Pointer to timer device, should be:
- * - LPC_TIM0: TIMER0 peripheral
- * - LPC_TIM1: TIMER1 peripheral
- * - LPC_TIM2: TIMER2 peripheral
- * - LPC_TIM3: TIMER3 peripheral
- * - CaptureChannel: set the channel to capture data
- * - RisingEdge : if SET, Capture at rising edge
- * - FallingEdge : if SET, Capture at falling edge
- * - IntOnCaption : if SET, Capture generate interrupt
- * @param[in] TIM_CaptureConfigStruct Pointer to TIM_CAPTURECFG_Type
- * @return None
- **********************************************************************/
-void TIM_ConfigCapture(LPC_TIM_TypeDef *TIMx, TIM_CAPTURECFG_Type *TIM_CaptureConfigStruct)
-{
-
- CHECK_PARAM(PARAM_TIMx(TIMx));
- TIMx->CCR &= ~TIM_CCR_CHANNEL_MASKBIT(TIM_CaptureConfigStruct->CaptureChannel);
-
- if (TIM_CaptureConfigStruct->RisingEdge)
- TIMx->CCR |= TIM_CAP_RISING(TIM_CaptureConfigStruct->CaptureChannel);
-
- if (TIM_CaptureConfigStruct->FallingEdge)
- TIMx->CCR |= TIM_CAP_FALLING(TIM_CaptureConfigStruct->CaptureChannel);
-
- if (TIM_CaptureConfigStruct->IntOnCaption)
- TIMx->CCR |= TIM_INT_ON_CAP(TIM_CaptureConfigStruct->CaptureChannel);
-}
-
-/*********************************************************************//**
- * @brief Read value of capture register in timer/counter device
- * @param[in] TIMx Pointer to timer/counter device, should be:
- * - LPC_TIM0: TIMER0 peripheral
- * - LPC_TIM1: TIMER1 peripheral
- * - LPC_TIM2: TIMER2 peripheral
- * - LPC_TIM3: TIMER3 peripheral
- * @param[in] CaptureChannel: capture channel number, should be:
- * - TIM_COUNTER_INCAP0: CAPn.0 input pin for TIMERn
- * - TIM_COUNTER_INCAP1: CAPn.1 input pin for TIMERn
- * @return Value of capture register
- **********************************************************************/
-uint32_t TIM_GetCaptureValue(LPC_TIM_TypeDef *TIMx, TIM_COUNTER_INPUT_OPT CaptureChannel)
-{
- CHECK_PARAM(PARAM_TIMx(TIMx));
- CHECK_PARAM(PARAM_TIM_COUNTER_INPUT_OPT(CaptureChannel));
-
- if(CaptureChannel==0)
- return TIMx->CR0;
- else
- return TIMx->CR1;
-}
-
-/**
- * @}
- */
-
-#endif /* _TIMER */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_uart.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_uart.c
deleted file mode 100644
index c7da45399a..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_uart.c
+++ /dev/null
@@ -1,1382 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_uart.c 2011-06-06
-*//**
-* @file lpc17xx_uart.c
-* @brief Contains all functions support for UART firmware library
-* on LPC17xx
-* @version 3.2
-* @date 25. July. 2011
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2011, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup UART
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_uart.h"
-#include "lpc17xx_clkpwr.h"
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-
-#ifdef _UART
-
-/* Private Functions ---------------------------------------------------------- */
-
-static Status uart_set_divisors(LPC_UART_TypeDef *UARTx, uint32_t baudrate);
-
-
-/*********************************************************************//**
- * @brief Determines best dividers to get a target clock rate
- * @param[in] UARTx Pointer to selected UART peripheral, should be:
- * - LPC_UART0: UART0 peripheral
- * - LPC_UART1: UART1 peripheral
- * - LPC_UART2: UART2 peripheral
- * - LPC_UART3: UART3 peripheral
- * @param[in] baudrate Desired UART baud rate.
- * @return Error status, could be:
- * - SUCCESS
- * - ERROR
- **********************************************************************/
-static Status uart_set_divisors(LPC_UART_TypeDef *UARTx, uint32_t baudrate)
-{
- Status errorStatus = ERROR;
-
- uint32_t uClk;
- uint32_t d, m, bestd, bestm, tmp;
- uint64_t best_divisor, divisor;
- uint32_t current_error, best_error;
- uint32_t recalcbaud;
-
- /* get UART block clock */
- if (UARTx == (LPC_UART_TypeDef *)LPC_UART0)
- {
- uClk = CLKPWR_GetPCLK (CLKPWR_PCLKSEL_UART0);
- }
- else if (UARTx == (LPC_UART_TypeDef *)LPC_UART1)
- {
- uClk = CLKPWR_GetPCLK (CLKPWR_PCLKSEL_UART1);
- }
- else if (UARTx == LPC_UART2)
- {
- uClk = CLKPWR_GetPCLK (CLKPWR_PCLKSEL_UART2);
- }
- else if (UARTx == LPC_UART3)
- {
- uClk = CLKPWR_GetPCLK (CLKPWR_PCLKSEL_UART3);
- }
-
-
- /* In the Uart IP block, baud rate is calculated using FDR and DLL-DLM registers
- * The formula is :
- * BaudRate= uClk * (mulFracDiv/(mulFracDiv+dividerAddFracDiv) / (16 * (DLL)
- * It involves floating point calculations. That's the reason the formulae are adjusted with
- * Multiply and divide method.*/
- /* The value of mulFracDiv and dividerAddFracDiv should comply to the following expressions:
- * 0 < mulFracDiv <= 15, 0 <= dividerAddFracDiv <= 15 */
- best_error = 0xFFFFFFFF; /* Worst case */
- bestd = 0;
- bestm = 0;
- best_divisor = 0;
- for (m = 1 ; m <= 15 ;m++)
- {
- for (d = 0 ; d < m ; d++)
- {
- divisor = ((uint64_t)uClk<<28)*m/(baudrate*(m+d));
- current_error = divisor & 0xFFFFFFFF;
-
- tmp = divisor>>32;
-
- /* Adjust error */
- if(current_error > ((uint32_t)1<<31)){
- current_error = -current_error;
- tmp++;
- }
-
- if(tmp<1 || tmp>65536) /* Out of range */
- continue;
-
- if( current_error < best_error){
- best_error = current_error;
- best_divisor = tmp;
- bestd = d;
- bestm = m;
- if(best_error == 0) break;
- }
- } /* end of inner for loop */
-
- if (best_error == 0)
- break;
- } /* end of outer for loop */
-
- if(best_divisor == 0) return ERROR; /* can not find best match */
-
- recalcbaud = (uClk>>4) * bestm/(best_divisor * (bestm + bestd));
-
- /* reuse best_error to evaluate baud error*/
- if(baudrate>recalcbaud) best_error = baudrate - recalcbaud;
- else best_error = recalcbaud -baudrate;
-
- best_error = best_error * 100 / baudrate;
-
- if (best_error < UART_ACCEPTED_BAUDRATE_ERROR)
- {
- if (((LPC_UART1_TypeDef *)UARTx) == LPC_UART1)
- {
- ((LPC_UART1_TypeDef *)UARTx)->LCR |= UART_LCR_DLAB_EN;
- ((LPC_UART1_TypeDef *)UARTx)->/*DLIER.*/DLM = UART_LOAD_DLM(best_divisor);
- ((LPC_UART1_TypeDef *)UARTx)->/*RBTHDLR.*/DLL = UART_LOAD_DLL(best_divisor);
- /* Then reset DLAB bit */
- ((LPC_UART1_TypeDef *)UARTx)->LCR &= (~UART_LCR_DLAB_EN) & UART_LCR_BITMASK;
- ((LPC_UART1_TypeDef *)UARTx)->FDR = (UART_FDR_MULVAL(bestm) \
- | UART_FDR_DIVADDVAL(bestd)) & UART_FDR_BITMASK;
- }
- else
- {
- UARTx->LCR |= UART_LCR_DLAB_EN;
- UARTx->/*DLIER.*/DLM = UART_LOAD_DLM(best_divisor);
- UARTx->/*RBTHDLR.*/DLL = UART_LOAD_DLL(best_divisor);
- /* Then reset DLAB bit */
- UARTx->LCR &= (~UART_LCR_DLAB_EN) & UART_LCR_BITMASK;
- UARTx->FDR = (UART_FDR_MULVAL(bestm) \
- | UART_FDR_DIVADDVAL(bestd)) & UART_FDR_BITMASK;
- }
- errorStatus = SUCCESS;
- }
-
- return errorStatus;
-}
-
-/* End of Private Functions ---------------------------------------------------- */
-
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup UART_Public_Functions
- * @{
- */
-/* UART Init/DeInit functions -------------------------------------------------*/
-/********************************************************************//**
- * @brief Initializes the UARTx peripheral according to the specified
- * parameters in the UART_ConfigStruct.
- * @param[in] UARTx UART peripheral selected, should be:
- * - LPC_UART0: UART0 peripheral
- * - LPC_UART1: UART1 peripheral
- * - LPC_UART2: UART2 peripheral
- * - LPC_UART3: UART3 peripheral
- * @param[in] UART_ConfigStruct Pointer to a UART_CFG_Type structure
-* that contains the configuration information for the
-* specified UART peripheral.
- * @return None
- *********************************************************************/
-void UART_Init(LPC_UART_TypeDef *UARTx, UART_CFG_Type *UART_ConfigStruct)
-{
- uint32_t tmp;
-
- // For debug mode
- CHECK_PARAM(PARAM_UARTx(UARTx));
- CHECK_PARAM(PARAM_UART_DATABIT(UART_ConfigStruct->Databits));
- CHECK_PARAM(PARAM_UART_STOPBIT(UART_ConfigStruct->Stopbits));
- CHECK_PARAM(PARAM_UART_PARITY(UART_ConfigStruct->Parity));
-
-#ifdef _UART0
- if(UARTx == (LPC_UART_TypeDef *)LPC_UART0)
- {
- /* Set up clock and power for UART module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCUART0, ENABLE);
- }
-#endif
-
-#ifdef _UART1
- if(((LPC_UART1_TypeDef *)UARTx) == LPC_UART1)
- {
- /* Set up clock and power for UART module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCUART1, ENABLE);
- }
-#endif
-
-#ifdef _UART2
- if(UARTx == LPC_UART2)
- {
- /* Set up clock and power for UART module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCUART2, ENABLE);
- }
-#endif
-
-#ifdef _UART3
- if(UARTx == LPC_UART3)
- {
- /* Set up clock and power for UART module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCUART3, ENABLE);
- }
-#endif
-
- if (((LPC_UART1_TypeDef *)UARTx) == LPC_UART1)
- {
- /* FIFOs are empty */
- ((LPC_UART1_TypeDef *)UARTx)->/*IIFCR.*/FCR = ( UART_FCR_FIFO_EN \
- | UART_FCR_RX_RS | UART_FCR_TX_RS);
- // Disable FIFO
- ((LPC_UART1_TypeDef *)UARTx)->/*IIFCR.*/FCR = 0;
-
- // Dummy reading
- while (((LPC_UART1_TypeDef *)UARTx)->LSR & UART_LSR_RDR)
- {
- tmp = ((LPC_UART1_TypeDef *)UARTx)->/*RBTHDLR.*/RBR;
- }
-
- ((LPC_UART1_TypeDef *)UARTx)->TER = UART_TER_TXEN;
- // Wait for current transmit complete
- while (!(((LPC_UART1_TypeDef *)UARTx)->LSR & UART_LSR_THRE));
- // Disable Tx
- ((LPC_UART1_TypeDef *)UARTx)->TER = 0;
-
- // Disable interrupt
- ((LPC_UART1_TypeDef *)UARTx)->/*DLIER.*/IER = 0;
- // Set LCR to default state
- ((LPC_UART1_TypeDef *)UARTx)->LCR = 0;
- // Set ACR to default state
- ((LPC_UART1_TypeDef *)UARTx)->ACR = 0;
- // Set Modem Control to default state
- ((LPC_UART1_TypeDef *)UARTx)->MCR = 0;
- // Set RS485 control to default state
- ((LPC_UART1_TypeDef *)UARTx)->RS485CTRL = 0;
- // Set RS485 delay timer to default state
- ((LPC_UART1_TypeDef *)UARTx)->RS485DLY = 0;
- // Set RS485 addr match to default state
- ((LPC_UART1_TypeDef *)UARTx)->ADRMATCH = 0;
- //Dummy Reading to Clear Status
- tmp = ((LPC_UART1_TypeDef *)UARTx)->MSR;
- tmp = ((LPC_UART1_TypeDef *)UARTx)->LSR;
- }
- else
- {
- /* FIFOs are empty */
- UARTx->/*IIFCR.*/FCR = ( UART_FCR_FIFO_EN | UART_FCR_RX_RS | UART_FCR_TX_RS);
- // Disable FIFO
- UARTx->/*IIFCR.*/FCR = 0;
-
- // Dummy reading
- while (UARTx->LSR & UART_LSR_RDR)
- {
- tmp = UARTx->/*RBTHDLR.*/RBR;
- }
-
- UARTx->TER = UART_TER_TXEN;
- // Wait for current transmit complete
- while (!(UARTx->LSR & UART_LSR_THRE));
- // Disable Tx
- UARTx->TER = 0;
-
- // Disable interrupt
- UARTx->/*DLIER.*/IER = 0;
- // Set LCR to default state
- UARTx->LCR = 0;
- // Set ACR to default state
- UARTx->ACR = 0;
- // Dummy reading
- tmp = UARTx->LSR;
- }
-
- if (UARTx == LPC_UART3)
- {
- // Set IrDA to default state
- UARTx->ICR = 0;
- }
-
- // Set Line Control register ----------------------------
-
- uart_set_divisors(UARTx, (UART_ConfigStruct->Baud_rate));
-
- if (((LPC_UART1_TypeDef *)UARTx) == LPC_UART1)
- {
- tmp = (((LPC_UART1_TypeDef *)UARTx)->LCR & (UART_LCR_DLAB_EN | UART_LCR_BREAK_EN)) \
- & UART_LCR_BITMASK;
- }
- else
- {
- tmp = (UARTx->LCR & (UART_LCR_DLAB_EN | UART_LCR_BREAK_EN)) & UART_LCR_BITMASK;
- }
-
- switch (UART_ConfigStruct->Databits){
- case UART_DATABIT_5:
- tmp |= UART_LCR_WLEN5;
- break;
- case UART_DATABIT_6:
- tmp |= UART_LCR_WLEN6;
- break;
- case UART_DATABIT_7:
- tmp |= UART_LCR_WLEN7;
- break;
- case UART_DATABIT_8:
- default:
- tmp |= UART_LCR_WLEN8;
- break;
- }
-
- if (UART_ConfigStruct->Parity == UART_PARITY_NONE)
- {
- // Do nothing...
- }
- else
- {
- tmp |= UART_LCR_PARITY_EN;
- switch (UART_ConfigStruct->Parity)
- {
- case UART_PARITY_ODD:
- tmp |= UART_LCR_PARITY_ODD;
- break;
-
- case UART_PARITY_EVEN:
- tmp |= UART_LCR_PARITY_EVEN;
- break;
-
- case UART_PARITY_SP_1:
- tmp |= UART_LCR_PARITY_F_1;
- break;
-
- case UART_PARITY_SP_0:
- tmp |= UART_LCR_PARITY_F_0;
- break;
- default:
- break;
- }
- }
-
- switch (UART_ConfigStruct->Stopbits){
- case UART_STOPBIT_2:
- tmp |= UART_LCR_STOPBIT_SEL;
- break;
- case UART_STOPBIT_1:
- default:
- // Do no thing
- break;
- }
-
-
- // Write back to LCR, configure FIFO and Disable Tx
- if (((LPC_UART1_TypeDef *)UARTx) == LPC_UART1)
- {
- ((LPC_UART1_TypeDef *)UARTx)->LCR = (uint8_t)(tmp & UART_LCR_BITMASK);
- }
- else
- {
- UARTx->LCR = (uint8_t)(tmp & UART_LCR_BITMASK);
- }
-}
-
-/*********************************************************************//**
- * @brief De-initializes the UARTx peripheral registers to their
- * default reset values.
- * @param[in] UARTx UART peripheral selected, should be:
- * - LPC_UART0: UART0 peripheral
- * - LPC_UART1: UART1 peripheral
- * - LPC_UART2: UART2 peripheral
- * - LPC_UART3: UART3 peripheral
- * @return None
- **********************************************************************/
-void UART_DeInit(LPC_UART_TypeDef* UARTx)
-{
- // For debug mode
- CHECK_PARAM(PARAM_UARTx(UARTx));
-
- UART_TxCmd(UARTx, DISABLE);
-
-#ifdef _UART0
- if (UARTx == (LPC_UART_TypeDef *)LPC_UART0)
- {
- /* Set up clock and power for UART module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCUART0, DISABLE);
- }
-#endif
-
-#ifdef _UART1
- if (((LPC_UART1_TypeDef *)UARTx) == LPC_UART1)
- {
- /* Set up clock and power for UART module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCUART1, DISABLE);
- }
-#endif
-
-#ifdef _UART2
- if (UARTx == LPC_UART2)
- {
- /* Set up clock and power for UART module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCUART2, DISABLE);
- }
-#endif
-
-#ifdef _UART3
- if (UARTx == LPC_UART3)
- {
- /* Set up clock and power for UART module */
- CLKPWR_ConfigPPWR (CLKPWR_PCONP_PCUART3, DISABLE);
- }
-#endif
-}
-
-/*****************************************************************************//**
-* @brief Fills each UART_InitStruct member with its default value:
-* - 9600 bps
-* - 8-bit data
-* - 1 Stopbit
-* - None Parity
-* @param[in] UART_InitStruct Pointer to a UART_CFG_Type structure
-* which will be initialized.
-* @return None
-*******************************************************************************/
-void UART_ConfigStructInit(UART_CFG_Type *UART_InitStruct)
-{
- UART_InitStruct->Baud_rate = 9600;
- UART_InitStruct->Databits = UART_DATABIT_8;
- UART_InitStruct->Parity = UART_PARITY_NONE;
- UART_InitStruct->Stopbits = UART_STOPBIT_1;
-}
-
-/* UART Send/Recieve functions -------------------------------------------------*/
-/*********************************************************************//**
- * @brief Transmit a single data through UART peripheral
- * @param[in] UARTx UART peripheral selected, should be:
- * - LPC_UART0: UART0 peripheral
- * - LPC_UART1: UART1 peripheral
- * - LPC_UART2: UART2 peripheral
- * - LPC_UART3: UART3 peripheral
- * @param[in] Data Data to transmit (must be 8-bit long)
- * @return None
- **********************************************************************/
-void UART_SendByte(LPC_UART_TypeDef* UARTx, uint8_t Data)
-{
- CHECK_PARAM(PARAM_UARTx(UARTx));
-
- if (((LPC_UART1_TypeDef *)UARTx) == LPC_UART1)
- {
- ((LPC_UART1_TypeDef *)UARTx)->/*RBTHDLR.*/THR = Data & UART_THR_MASKBIT;
- }
- else
- {
- UARTx->/*RBTHDLR.*/THR = Data & UART_THR_MASKBIT;
- }
-
-}
-
-
-/*********************************************************************//**
- * @brief Receive a single data from UART peripheral
- * @param[in] UARTx UART peripheral selected, should be:
- * - LPC_UART0: UART0 peripheral
- * - LPC_UART1: UART1 peripheral
- * - LPC_UART2: UART2 peripheral
- * - LPC_UART3: UART3 peripheral
- * @return Data received
- **********************************************************************/
-uint8_t UART_ReceiveByte(LPC_UART_TypeDef* UARTx)
-{
- CHECK_PARAM(PARAM_UARTx(UARTx));
-
- if (((LPC_UART1_TypeDef *)UARTx) == LPC_UART1)
- {
- return (((LPC_UART1_TypeDef *)UARTx)->/*RBTHDLR.*/RBR & UART_RBR_MASKBIT);
- }
- else
- {
- return (UARTx->/*RBTHDLR.*/RBR & UART_RBR_MASKBIT);
- }
-}
-
-/*********************************************************************//**
- * @brief Send a block of data via UART peripheral
- * @param[in] UARTx Selected UART peripheral used to send data, should be:
- * - LPC_UART0: UART0 peripheral
- * - LPC_UART1: UART1 peripheral
- * - LPC_UART2: UART2 peripheral
- * - LPC_UART3: UART3 peripheral
- * @param[in] txbuf Pointer to Transmit buffer
- * @param[in] buflen Length of Transmit buffer
- * @param[in] flag Flag used in UART transfer, should be
- * NONE_BLOCKING or BLOCKING
- * @return Number of bytes sent.
- *
- * Note: when using UART in BLOCKING mode, a time-out condition is used
- * via defined symbol UART_BLOCKING_TIMEOUT.
- **********************************************************************/
-uint32_t UART_Send(LPC_UART_TypeDef *UARTx, uint8_t *txbuf,
- uint32_t buflen, TRANSFER_BLOCK_Type flag)
-{
- uint32_t bToSend, bSent, timeOut, fifo_cnt;
- uint8_t *pChar = txbuf;
-
- bToSend = buflen;
-
- // blocking mode
- if (flag == BLOCKING) {
- bSent = 0;
- while (bToSend){
- timeOut = UART_BLOCKING_TIMEOUT;
- // Wait for THR empty with timeout
- while (!(UARTx->LSR & UART_LSR_THRE)) {
- if (timeOut == 0) break;
- timeOut--;
- }
- // Time out!
- if(timeOut == 0) break;
- fifo_cnt = UART_TX_FIFO_SIZE;
- while (fifo_cnt && bToSend){
- UART_SendByte(UARTx, (*pChar++));
- fifo_cnt--;
- bToSend--;
- bSent++;
- }
- }
- }
- // None blocking mode
- else {
- bSent = 0;
- while (bToSend) {
- if (!(UARTx->LSR & UART_LSR_THRE)){
- break;
- }
- fifo_cnt = UART_TX_FIFO_SIZE;
- while (fifo_cnt && bToSend) {
- UART_SendByte(UARTx, (*pChar++));
- bToSend--;
- fifo_cnt--;
- bSent++;
- }
- }
- }
- return bSent;
-}
-
-/*********************************************************************//**
- * @brief Receive a block of data via UART peripheral
- * @param[in] UARTx Selected UART peripheral used to send data,
- * should be:
- * - LPC_UART0: UART0 peripheral
- * - LPC_UART1: UART1 peripheral
- * - LPC_UART2: UART2 peripheral
- * - LPC_UART3: UART3 peripheral
- * @param[out] rxbuf Pointer to Received buffer
- * @param[in] buflen Length of Received buffer
- * @param[in] flag Flag mode, should be NONE_BLOCKING or BLOCKING
-
- * @return Number of bytes received
- *
- * Note: when using UART in BLOCKING mode, a time-out condition is used
- * via defined symbol UART_BLOCKING_TIMEOUT.
- **********************************************************************/
-uint32_t UART_Receive(LPC_UART_TypeDef *UARTx, uint8_t *rxbuf, \
- uint32_t buflen, TRANSFER_BLOCK_Type flag)
-{
- uint32_t bToRecv, bRecv, timeOut;
- uint8_t *pChar = rxbuf;
-
- bToRecv = buflen;
-
- // Blocking mode
- if (flag == BLOCKING) {
- bRecv = 0;
- while (bToRecv){
- timeOut = UART_BLOCKING_TIMEOUT;
- while (!(UARTx->LSR & UART_LSR_RDR)){
- if (timeOut == 0) break;
- timeOut--;
- }
- // Time out!
- if(timeOut == 0) break;
- // Get data from the buffer
- (*pChar++) = UART_ReceiveByte(UARTx);
- bToRecv--;
- bRecv++;
- }
- }
- // None blocking mode
- else {
- bRecv = 0;
- while (bToRecv) {
- if (!(UARTx->LSR & UART_LSR_RDR)) {
- break;
- } else {
- (*pChar++) = UART_ReceiveByte(UARTx);
- bRecv++;
- bToRecv--;
- }
- }
- }
- return bRecv;
-}
-
-/*********************************************************************//**
- * @brief Force BREAK character on UART line, output pin UARTx TXD is
- forced to logic 0.
- * @param[in] UARTx UART peripheral selected, should be:
- * - LPC_UART0: UART0 peripheral
- * - LPC_UART1: UART1 peripheral
- * - LPC_UART2: UART2 peripheral
- * - LPC_UART3: UART3 peripheral
- * @return None
- **********************************************************************/
-void UART_ForceBreak(LPC_UART_TypeDef* UARTx)
-{
- CHECK_PARAM(PARAM_UARTx(UARTx));
-
- if (((LPC_UART1_TypeDef *)UARTx) == LPC_UART1)
- {
- ((LPC_UART1_TypeDef *)UARTx)->LCR |= UART_LCR_BREAK_EN;
- }
- else
- {
- UARTx->LCR |= UART_LCR_BREAK_EN;
- }
-}
-
-
-/********************************************************************//**
- * @brief Enable or disable specified UART interrupt.
- * @param[in] UARTx UART peripheral selected, should be
- * - LPC_UART0: UART0 peripheral
- * - LPC_UART1: UART1 peripheral
- * - LPC_UART2: UART2 peripheral
- * - LPC_UART3: UART3 peripheral
- * @param[in] UARTIntCfg Specifies the interrupt flag,
- * should be one of the following:
- - UART_INTCFG_RBR : RBR Interrupt enable
- - UART_INTCFG_THRE : THR Interrupt enable
- - UART_INTCFG_RLS : RX line status interrupt enable
- - UART1_INTCFG_MS : Modem status interrupt enable (UART1 only)
- - UART1_INTCFG_CTS : CTS1 signal transition interrupt enable (UART1 only)
- - UART_INTCFG_ABEO : Enables the end of auto-baud interrupt
- - UART_INTCFG_ABTO : Enables the auto-baud time-out interrupt
- * @param[in] NewState New state of specified UART interrupt type,
- * should be:
- * - ENALBE: Enable this UART interrupt type.
-* - DISALBE: Disable this UART interrupt type.
- * @return None
- *********************************************************************/
-void UART_IntConfig(LPC_UART_TypeDef *UARTx, UART_INT_Type UARTIntCfg, FunctionalState NewState)
-{
- uint32_t tmp;
-
- CHECK_PARAM(PARAM_UARTx(UARTx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- switch(UARTIntCfg){
- case UART_INTCFG_RBR:
- tmp = UART_IER_RBRINT_EN;
- break;
- case UART_INTCFG_THRE:
- tmp = UART_IER_THREINT_EN;
- break;
- case UART_INTCFG_RLS:
- tmp = UART_IER_RLSINT_EN;
- break;
- case UART1_INTCFG_MS:
- tmp = UART1_IER_MSINT_EN;
- break;
- case UART1_INTCFG_CTS:
- tmp = UART1_IER_CTSINT_EN;
- break;
- case UART_INTCFG_ABEO:
- tmp = UART_IER_ABEOINT_EN;
- break;
- case UART_INTCFG_ABTO:
- tmp = UART_IER_ABTOINT_EN;
- break;
- }
-
- if ((LPC_UART1_TypeDef *) UARTx == LPC_UART1)
- {
- CHECK_PARAM((PARAM_UART_INTCFG(UARTIntCfg)) || (PARAM_UART1_INTCFG(UARTIntCfg)));
- }
- else
- {
- CHECK_PARAM(PARAM_UART_INTCFG(UARTIntCfg));
- }
-
- if (NewState == ENABLE)
- {
- if ((LPC_UART1_TypeDef *) UARTx == LPC_UART1)
- {
- ((LPC_UART1_TypeDef *)UARTx)->/*DLIER.*/IER |= tmp;
- }
- else
- {
- UARTx->/*DLIER.*/IER |= tmp;
- }
- }
- else
- {
- if ((LPC_UART1_TypeDef *) UARTx == LPC_UART1)
- {
- ((LPC_UART1_TypeDef *)UARTx)->/*DLIER.*/IER &= (~tmp) & UART1_IER_BITMASK;
- }
- else
- {
- UARTx->/*DLIER.*/IER &= (~tmp) & UART_IER_BITMASK;
- }
- }
-}
-
-
-/********************************************************************//**
- * @brief Get current value of Line Status register in UART peripheral.
- * @param[in] UARTx UART peripheral selected, should be:
- * - LPC_UART0: UART0 peripheral
- * - LPC_UART1: UART1 peripheral
- * - LPC_UART2: UART2 peripheral
- * - LPC_UART3: UART3 peripheral
- * @return Current value of Line Status register in UART peripheral.
- * Note: The return value of this function must be ANDed with each member in
- * UART_LS_Type enumeration to determine current flag status
- * corresponding to each Line status type. Because some flags in
- * Line Status register will be cleared after reading, the next reading
- * Line Status register could not be correct. So this function used to
- * read Line status register in one time only, then the return value
- * used to check all flags.
- *********************************************************************/
-uint8_t UART_GetLineStatus(LPC_UART_TypeDef* UARTx)
-{
- CHECK_PARAM(PARAM_UARTx(UARTx));
-
- if (((LPC_UART1_TypeDef *)UARTx) == LPC_UART1)
- {
- return ((((LPC_UART1_TypeDef *)LPC_UART1)->LSR) & UART_LSR_BITMASK);
- }
- else
- {
- return ((UARTx->LSR) & UART_LSR_BITMASK);
- }
-}
-
-/********************************************************************//**
- * @brief Get Interrupt Identification value
- * @param[in] UARTx UART peripheral selected, should be:
- * - LPC_UART0: UART0 peripheral
- * - LPC_UART1: UART1 peripheral
- * - LPC_UART2: UART2 peripheral
- * - LPC_UART3: UART3 peripheral
- * @return Current value of UART UIIR register in UART peripheral.
- *********************************************************************/
-uint32_t UART_GetIntId(LPC_UART_TypeDef* UARTx)
-{
- CHECK_PARAM(PARAM_UARTx(UARTx));
- return (UARTx->IIR & 0x03CF);
-}
-
-/*********************************************************************//**
- * @brief Check whether if UART is busy or not
- * @param[in] UARTx UART peripheral selected, should be:
- * - LPC_UART0: UART0 peripheral
- * - LPC_UART1: UART1 peripheral
- * - LPC_UART2: UART2 peripheral
- * - LPC_UART3: UART3 peripheral
- * @return RESET if UART is not busy, otherwise return SET.
- **********************************************************************/
-FlagStatus UART_CheckBusy(LPC_UART_TypeDef *UARTx)
-{
- if (UARTx->LSR & UART_LSR_TEMT){
- return RESET;
- } else {
- return SET;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Configure FIFO function on selected UART peripheral
- * @param[in] UARTx UART peripheral selected, should be:
- * - LPC_UART0: UART0 peripheral
- * - LPC_UART1: UART1 peripheral
- * - LPC_UART2: UART2 peripheral
- * - LPC_UART3: UART3 peripheral
- * @param[in] FIFOCfg Pointer to a UART_FIFO_CFG_Type Structure that
- * contains specified information about FIFO configuration
- * @return none
- **********************************************************************/
-void UART_FIFOConfig(LPC_UART_TypeDef *UARTx, UART_FIFO_CFG_Type *FIFOCfg)
-{
- uint8_t tmp = 0;
-
- CHECK_PARAM(PARAM_UARTx(UARTx));
- CHECK_PARAM(PARAM_UART_FIFO_LEVEL(FIFOCfg->FIFO_Level));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(FIFOCfg->FIFO_DMAMode));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(FIFOCfg->FIFO_ResetRxBuf));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(FIFOCfg->FIFO_ResetTxBuf));
-
- tmp |= UART_FCR_FIFO_EN;
- switch (FIFOCfg->FIFO_Level){
- case UART_FIFO_TRGLEV0:
- tmp |= UART_FCR_TRG_LEV0;
- break;
- case UART_FIFO_TRGLEV1:
- tmp |= UART_FCR_TRG_LEV1;
- break;
- case UART_FIFO_TRGLEV2:
- tmp |= UART_FCR_TRG_LEV2;
- break;
- case UART_FIFO_TRGLEV3:
- default:
- tmp |= UART_FCR_TRG_LEV3;
- break;
- }
-
- if (FIFOCfg->FIFO_ResetTxBuf == ENABLE)
- {
- tmp |= UART_FCR_TX_RS;
- }
- if (FIFOCfg->FIFO_ResetRxBuf == ENABLE)
- {
- tmp |= UART_FCR_RX_RS;
- }
- if (FIFOCfg->FIFO_DMAMode == ENABLE)
- {
- tmp |= UART_FCR_DMAMODE_SEL;
- }
-
-
- //write to FIFO control register
- if (((LPC_UART1_TypeDef *)UARTx) == LPC_UART1)
- {
- ((LPC_UART1_TypeDef *)UARTx)->/*IIFCR.*/FCR = tmp & UART_FCR_BITMASK;
- }
- else
- {
- UARTx->/*IIFCR.*/FCR = tmp & UART_FCR_BITMASK;
- }
-}
-
-/*****************************************************************************//**
-* @brief Fills each UART_FIFOInitStruct member with its default value:
-* - FIFO_DMAMode = DISABLE
-* - FIFO_Level = UART_FIFO_TRGLEV0
-* - FIFO_ResetRxBuf = ENABLE
-* - FIFO_ResetTxBuf = ENABLE
-* - FIFO_State = ENABLE
-
-* @param[in] UART_FIFOInitStruct Pointer to a UART_FIFO_CFG_Type structure
-* which will be initialized.
-* @return None
-*******************************************************************************/
-void UART_FIFOConfigStructInit(UART_FIFO_CFG_Type *UART_FIFOInitStruct)
-{
- UART_FIFOInitStruct->FIFO_DMAMode = DISABLE;
- UART_FIFOInitStruct->FIFO_Level = UART_FIFO_TRGLEV0;
- UART_FIFOInitStruct->FIFO_ResetRxBuf = ENABLE;
- UART_FIFOInitStruct->FIFO_ResetTxBuf = ENABLE;
-}
-
-
-/*********************************************************************//**
- * @brief Start/Stop Auto Baudrate activity
- * @param[in] UARTx UART peripheral selected, should be
- * - LPC_UART0: UART0 peripheral
- * - LPC_UART1: UART1 peripheral
- * - LPC_UART2: UART2 peripheral
- * - LPC_UART3: UART3 peripheral
- * @param[in] ABConfigStruct A pointer to UART_AB_CFG_Type structure that
- * contains specified information about UART
- * auto baudrate configuration
- * @param[in] NewState New State of Auto baudrate activity, should be:
- * - ENABLE: Start this activity
- * - DISABLE: Stop this activity
- * Note: Auto-baudrate mode enable bit will be cleared once this mode
- * completed.
- * @return none
- **********************************************************************/
-void UART_ABCmd(LPC_UART_TypeDef *UARTx, UART_AB_CFG_Type *ABConfigStruct, \
- FunctionalState NewState)
-{
- uint32_t tmp;
-
- CHECK_PARAM(PARAM_UARTx(UARTx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- tmp = 0;
- if (NewState == ENABLE) {
- if (ABConfigStruct->ABMode == UART_AUTOBAUD_MODE1){
- tmp |= UART_ACR_MODE;
- }
- if (ABConfigStruct->AutoRestart == ENABLE){
- tmp |= UART_ACR_AUTO_RESTART;
- }
- }
-
- if (((LPC_UART1_TypeDef *)UARTx) == LPC_UART1)
- {
- if (NewState == ENABLE)
- {
- // Clear DLL and DLM value
- ((LPC_UART1_TypeDef *)UARTx)->LCR |= UART_LCR_DLAB_EN;
- ((LPC_UART1_TypeDef *)UARTx)->DLL = 0;
- ((LPC_UART1_TypeDef *)UARTx)->DLM = 0;
- ((LPC_UART1_TypeDef *)UARTx)->LCR &= ~UART_LCR_DLAB_EN;
- // FDR value must be reset to default value
- ((LPC_UART1_TypeDef *)UARTx)->FDR = 0x10;
- ((LPC_UART1_TypeDef *)UARTx)->ACR = UART_ACR_START | tmp;
- }
- else
- {
- ((LPC_UART1_TypeDef *)UARTx)->ACR = 0;
- }
- }
- else
- {
- if (NewState == ENABLE)
- {
- // Clear DLL and DLM value
- UARTx->LCR |= UART_LCR_DLAB_EN;
- UARTx->DLL = 0;
- UARTx->DLM = 0;
- UARTx->LCR &= ~UART_LCR_DLAB_EN;
- // FDR value must be reset to default value
- UARTx->FDR = 0x10;
- UARTx->ACR = UART_ACR_START | tmp;
- }
- else
- {
- UARTx->ACR = 0;
- }
- }
-}
-
-/*********************************************************************//**
- * @brief Clear Autobaud Interrupt Pending
- * @param[in] UARTx UART peripheral selected, should be
- * - LPC_UART0: UART0 peripheral
- * - LPC_UART1: UART1 peripheral
- * - LPC_UART2: UART2 peripheral
- * - LPC_UART3: UART3 peripheral
- * @param[in] ABIntType type of auto-baud interrupt, should be:
- * - UART_AUTOBAUD_INTSTAT_ABEO: End of Auto-baud interrupt
- * - UART_AUTOBAUD_INTSTAT_ABTO: Auto-baud time out interrupt
- * @return none
- **********************************************************************/
-void UART_ABClearIntPending(LPC_UART_TypeDef *UARTx, UART_ABEO_Type ABIntType)
-{
- CHECK_PARAM(PARAM_UARTx(UARTx));
- if (((LPC_UART1_TypeDef *)UARTx) == LPC_UART1)
- {
- UARTx->ACR |= ABIntType;
- }
- else
- UARTx->ACR |= ABIntType;
-}
-
-/*********************************************************************//**
- * @brief Enable/Disable transmission on UART TxD pin
- * @param[in] UARTx UART peripheral selected, should be:
- * - LPC_UART0: UART0 peripheral
- * - LPC_UART1: UART1 peripheral
- * - LPC_UART2: UART2 peripheral
- * - LPC_UART3: UART3 peripheral
- * @param[in] NewState New State of Tx transmission function, should be:
- * - ENABLE: Enable this function
- - DISABLE: Disable this function
- * @return none
- **********************************************************************/
-void UART_TxCmd(LPC_UART_TypeDef *UARTx, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_UARTx(UARTx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- if (NewState == ENABLE)
- {
- if (((LPC_UART1_TypeDef *)UARTx) == LPC_UART1)
- {
- ((LPC_UART1_TypeDef *)UARTx)->TER |= UART_TER_TXEN;
- }
- else
- {
- UARTx->TER |= UART_TER_TXEN;
- }
- }
- else
- {
- if (((LPC_UART1_TypeDef *)UARTx) == LPC_UART1)
- {
- ((LPC_UART1_TypeDef *)UARTx)->TER &= (~UART_TER_TXEN) & UART_TER_BITMASK;
- }
- else
- {
- UARTx->TER &= (~UART_TER_TXEN) & UART_TER_BITMASK;
- }
- }
-}
-
-/* UART IrDA functions ---------------------------------------------------*/
-
-#ifdef _UART3
-
-/*********************************************************************//**
- * @brief Enable or disable inverting serial input function of IrDA
- * on UART peripheral.
- * @param[in] UARTx UART peripheral selected, should be LPC_UART3 (only)
- * @param[in] NewState New state of inverting serial input, should be:
- * - ENABLE: Enable this function.
- * - DISABLE: Disable this function.
- * @return none
- **********************************************************************/
-void UART_IrDAInvtInputCmd(LPC_UART_TypeDef* UARTx, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_UART_IrDA(UARTx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- if (NewState == ENABLE)
- {
- UARTx->ICR |= UART_ICR_IRDAINV;
- }
- else if (NewState == DISABLE)
- {
- UARTx->ICR &= (~UART_ICR_IRDAINV) & UART_ICR_BITMASK;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Enable or disable IrDA function on UART peripheral.
- * @param[in] UARTx UART peripheral selected, should be LPC_UART3 (only)
- * @param[in] NewState New state of IrDA function, should be:
- * - ENABLE: Enable this function.
- * - DISABLE: Disable this function.
- * @return none
- **********************************************************************/
-void UART_IrDACmd(LPC_UART_TypeDef* UARTx, FunctionalState NewState)
-{
- CHECK_PARAM(PARAM_UART_IrDA(UARTx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- if (NewState == ENABLE)
- {
- UARTx->ICR |= UART_ICR_IRDAEN;
- }
- else
- {
- UARTx->ICR &= (~UART_ICR_IRDAEN) & UART_ICR_BITMASK;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Configure Pulse divider for IrDA function on UART peripheral.
- * @param[in] UARTx UART peripheral selected, should be LPC_UART3 (only)
- * @param[in] PulseDiv Pulse Divider value from Peripheral clock,
- * should be one of the following:
- - UART_IrDA_PULSEDIV2 : Pulse width = 2 * Tpclk
- - UART_IrDA_PULSEDIV4 : Pulse width = 4 * Tpclk
- - UART_IrDA_PULSEDIV8 : Pulse width = 8 * Tpclk
- - UART_IrDA_PULSEDIV16 : Pulse width = 16 * Tpclk
- - UART_IrDA_PULSEDIV32 : Pulse width = 32 * Tpclk
- - UART_IrDA_PULSEDIV64 : Pulse width = 64 * Tpclk
- - UART_IrDA_PULSEDIV128 : Pulse width = 128 * Tpclk
- - UART_IrDA_PULSEDIV256 : Pulse width = 256 * Tpclk
-
- * @return none
- **********************************************************************/
-void UART_IrDAPulseDivConfig(LPC_UART_TypeDef *UARTx, UART_IrDA_PULSE_Type PulseDiv)
-{
- uint32_t tmp, tmp1;
- CHECK_PARAM(PARAM_UART_IrDA(UARTx));
- CHECK_PARAM(PARAM_UART_IrDA_PULSEDIV(PulseDiv));
-
- tmp1 = UART_ICR_PULSEDIV(PulseDiv);
- tmp = UARTx->ICR & (~UART_ICR_PULSEDIV(7));
- tmp |= tmp1 | UART_ICR_FIXPULSE_EN;
- UARTx->ICR = tmp & UART_ICR_BITMASK;
-}
-
-#endif
-
-
-/* UART1 FullModem function ---------------------------------------------*/
-
-#ifdef _UART1
-
-/*********************************************************************//**
- * @brief Force pin DTR/RTS corresponding to given state (Full modem mode)
- * @param[in] UARTx LPC_UART1 (only)
- * @param[in] Pin Pin that NewState will be applied to, should be:
- * - UART1_MODEM_PIN_DTR: DTR pin.
- * - UART1_MODEM_PIN_RTS: RTS pin.
- * @param[in] NewState New State of DTR/RTS pin, should be:
- * - INACTIVE: Force the pin to inactive signal.
- - ACTIVE: Force the pin to active signal.
- * @return none
- **********************************************************************/
-void UART_FullModemForcePinState(LPC_UART1_TypeDef *UARTx, UART_MODEM_PIN_Type Pin, \
- UART1_SignalState NewState)
-{
- uint8_t tmp = 0;
-
- CHECK_PARAM(PARAM_UART1_MODEM(UARTx));
- CHECK_PARAM(PARAM_UART1_MODEM_PIN(Pin));
- CHECK_PARAM(PARAM_UART1_SIGNALSTATE(NewState));
-
- switch (Pin){
- case UART1_MODEM_PIN_DTR:
- tmp = UART1_MCR_DTR_CTRL;
- break;
- case UART1_MODEM_PIN_RTS:
- tmp = UART1_MCR_RTS_CTRL;
- break;
- default:
- break;
- }
-
- if (NewState == ACTIVE){
- UARTx->MCR |= tmp;
- } else {
- UARTx->MCR &= (~tmp) & UART1_MCR_BITMASK;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Configure Full Modem mode for UART peripheral
- * @param[in] UARTx LPC_UART1 (only)
- * @param[in] Mode Full Modem mode, should be:
- * - UART1_MODEM_MODE_LOOPBACK: Loop back mode.
- * - UART1_MODEM_MODE_AUTO_RTS: Auto-RTS mode.
- * - UART1_MODEM_MODE_AUTO_CTS: Auto-CTS mode.
- * @param[in] NewState New State of this mode, should be:
- * - ENABLE: Enable this mode.
- - DISABLE: Disable this mode.
- * @return none
- **********************************************************************/
-void UART_FullModemConfigMode(LPC_UART1_TypeDef *UARTx, UART_MODEM_MODE_Type Mode, \
- FunctionalState NewState)
-{
- uint8_t tmp;
-
- CHECK_PARAM(PARAM_UART1_MODEM(UARTx));
- CHECK_PARAM(PARAM_UART1_MODEM_MODE(Mode));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(NewState));
-
- switch(Mode){
- case UART1_MODEM_MODE_LOOPBACK:
- tmp = UART1_MCR_LOOPB_EN;
- break;
- case UART1_MODEM_MODE_AUTO_RTS:
- tmp = UART1_MCR_AUTO_RTS_EN;
- break;
- case UART1_MODEM_MODE_AUTO_CTS:
- tmp = UART1_MCR_AUTO_CTS_EN;
- break;
- default:
- break;
- }
-
- if (NewState == ENABLE)
- {
- UARTx->MCR |= tmp;
- }
- else
- {
- UARTx->MCR &= (~tmp) & UART1_MCR_BITMASK;
- }
-}
-
-
-/*********************************************************************//**
- * @brief Get current status of modem status register
- * @param[in] UARTx LPC_UART1 (only)
- * @return Current value of modem status register
- * Note: The return value of this function must be ANDed with each member
- * UART_MODEM_STAT_type enumeration to determine current flag status
- * corresponding to each modem flag status. Because some flags in
- * modem status register will be cleared after reading, the next reading
- * modem register could not be correct. So this function used to
- * read modem status register in one time only, then the return value
- * used to check all flags.
- **********************************************************************/
-uint8_t UART_FullModemGetStatus(LPC_UART1_TypeDef *UARTx)
-{
- CHECK_PARAM(PARAM_UART1_MODEM(UARTx));
- return ((UARTx->MSR) & UART1_MSR_BITMASK);
-}
-
-
-/* UART RS485 functions --------------------------------------------------------------*/
-
-/*********************************************************************//**
- * @brief Configure UART peripheral in RS485 mode according to the specified
-* parameters in the RS485ConfigStruct.
- * @param[in] UARTx LPC_UART1 (only)
- * @param[in] RS485ConfigStruct Pointer to a UART1_RS485_CTRLCFG_Type structure
-* that contains the configuration information for specified UART
-* in RS485 mode.
- * @return None
- **********************************************************************/
-void UART_RS485Config(LPC_UART1_TypeDef *UARTx, UART1_RS485_CTRLCFG_Type *RS485ConfigStruct)
-{
- uint32_t tmp;
-
- CHECK_PARAM(PARAM_UART1_MODEM(UARTx));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(RS485ConfigStruct->AutoAddrDetect_State));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(RS485ConfigStruct->AutoDirCtrl_State));
- CHECK_PARAM(PARAM_UART1_RS485_CFG_DELAYVALUE(RS485ConfigStruct->DelayValue));
- CHECK_PARAM(PARAM_SETSTATE(RS485ConfigStruct->DirCtrlPol_Level));
- CHECK_PARAM(PARAM_UART_RS485_DIRCTRL_PIN(RS485ConfigStruct->DirCtrlPin));
- CHECK_PARAM(PARAM_UART1_RS485_CFG_MATCHADDRVALUE(RS485ConfigStruct->MatchAddrValue));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(RS485ConfigStruct->NormalMultiDropMode_State));
- CHECK_PARAM(PARAM_FUNCTIONALSTATE(RS485ConfigStruct->Rx_State));
-
- tmp = 0;
- // If Auto Direction Control is enabled - This function is used in Master mode
- if (RS485ConfigStruct->AutoDirCtrl_State == ENABLE)
- {
- tmp |= UART1_RS485CTRL_DCTRL_EN;
-
- // Set polar
- if (RS485ConfigStruct->DirCtrlPol_Level == SET)
- {
- tmp |= UART1_RS485CTRL_OINV_1;
- }
-
- // Set pin according to
- if (RS485ConfigStruct->DirCtrlPin == UART1_RS485_DIRCTRL_DTR)
- {
- tmp |= UART1_RS485CTRL_SEL_DTR;
- }
-
- // Fill delay time
- UARTx->RS485DLY = RS485ConfigStruct->DelayValue & UART1_RS485DLY_BITMASK;
- }
-
- // MultiDrop mode is enable
- if (RS485ConfigStruct->NormalMultiDropMode_State == ENABLE)
- {
- tmp |= UART1_RS485CTRL_NMM_EN;
- }
-
- // Auto Address Detect function
- if (RS485ConfigStruct->AutoAddrDetect_State == ENABLE)
- {
- tmp |= UART1_RS485CTRL_AADEN;
- // Fill Match Address
- UARTx->ADRMATCH = RS485ConfigStruct->MatchAddrValue & UART1_RS485ADRMATCH_BITMASK;
- }
-
-
- // Receiver is disable
- if (RS485ConfigStruct->Rx_State == DISABLE)
- {
- tmp |= UART1_RS485CTRL_RX_DIS;
- }
-
- // write back to RS485 control register
- UARTx->RS485CTRL = tmp & UART1_RS485CTRL_BITMASK;
-
- // Enable Parity function and leave parity in stick '0' parity as default
- UARTx->LCR |= (UART_LCR_PARITY_F_0 | UART_LCR_PARITY_EN);
-}
-
-/*********************************************************************//**
- * @brief Enable/Disable receiver in RS485 module in UART1
- * @param[in] UARTx LPC_UART1 (only)
- * @param[in] NewState New State of command, should be:
- * - ENABLE: Enable this function.
- * - DISABLE: Disable this function.
- * @return None
- **********************************************************************/
-void UART_RS485ReceiverCmd(LPC_UART1_TypeDef *UARTx, FunctionalState NewState)
-{
- if (NewState == ENABLE){
- UARTx->RS485CTRL &= ~UART1_RS485CTRL_RX_DIS;
- } else {
- UARTx->RS485CTRL |= UART1_RS485CTRL_RX_DIS;
- }
-}
-
-/*********************************************************************//**
- * @brief Send data on RS485 bus with specified parity stick value (9-bit mode).
- * @param[in] UARTx LPC_UART1 (only)
- * @param[in] pDatFrm Pointer to data frame.
- * @param[in] size Size of data.
- * @param[in] ParityStick Parity Stick value, should be 0 or 1.
- * @return None
- **********************************************************************/
-uint32_t UART_RS485Send(LPC_UART1_TypeDef *UARTx, uint8_t *pDatFrm, \
- uint32_t size, uint8_t ParityStick)
-{
- uint8_t tmp, save;
- uint32_t cnt;
-
- if (ParityStick){
- save = tmp = UARTx->LCR & UART_LCR_BITMASK;
- tmp &= ~(UART_LCR_PARITY_EVEN);
- UARTx->LCR = tmp;
- cnt = UART_Send((LPC_UART_TypeDef *)UARTx, pDatFrm, size, BLOCKING);
- while (!(UARTx->LSR & UART_LSR_TEMT));
- UARTx->LCR = save;
- } else {
- cnt = UART_Send((LPC_UART_TypeDef *)UARTx, pDatFrm, size, BLOCKING);
- while (!(UARTx->LSR & UART_LSR_TEMT));
- }
- return cnt;
-}
-
-/*********************************************************************//**
- * @brief Send Slave address frames on RS485 bus.
- * @param[in] UARTx LPC_UART1 (only)
- * @param[in] SlvAddr Slave Address.
- * @return None
- **********************************************************************/
-void UART_RS485SendSlvAddr(LPC_UART1_TypeDef *UARTx, uint8_t SlvAddr)
-{
- UART_RS485Send(UARTx, &SlvAddr, 1, 1);
-}
-
-/*********************************************************************//**
- * @brief Send Data frames on RS485 bus.
- * @param[in] UARTx LPC_UART1 (only)
- * @param[in] pData Pointer to data to be sent.
- * @param[in] size Size of data frame to be sent.
- * @return None
- **********************************************************************/
-uint32_t UART_RS485SendData(LPC_UART1_TypeDef *UARTx, uint8_t *pData, uint32_t size)
-{
- return (UART_RS485Send(UARTx, pData, size, 0));
-}
-
-#endif /* _UART1 */
-
-#endif /* _UART */
-
-/**
- * @}
- */
-
-/**
- * @}
- */
-/* --------------------------------- End Of File ------------------------------ */
-
diff --git a/frameworks/CMSIS/LPC1768/driver/lpc17xx_wdt.c b/frameworks/CMSIS/LPC1768/driver/lpc17xx_wdt.c
deleted file mode 100644
index 39e0f9e682..0000000000
--- a/frameworks/CMSIS/LPC1768/driver/lpc17xx_wdt.c
+++ /dev/null
@@ -1,274 +0,0 @@
-/**********************************************************************
-* $Id$ lpc17xx_wdt.c 2010-05-21
-*//**
-* @file lpc17xx_wdt.c
-* @brief Contains all functions support for WDT firmware library
-* on LPC17xx
-* @version 2.0
-* @date 21. May. 2010
-* @author NXP MCU SW Application Team
-*
-* Copyright(C) 2010, NXP Semiconductor
-* All rights reserved.
-*
-***********************************************************************
-* Software that is described herein is for illustrative purposes only
-* which provides customers with programming information regarding the
-* products. This software is supplied "AS IS" without any warranties.
-* NXP Semiconductors assumes no responsibility or liability for the
-* use of the software, conveys no license or title under any patent,
-* copyright, or mask work right to the product. NXP Semiconductors
-* reserves the right to make changes in the software without
-* notification. NXP Semiconductors also make no representation or
-* warranty that such application will be suitable for the specified
-* use without further testing or modification.
-* Permission to use, copy, modify, and distribute this software and its
-* documentation is hereby granted, under NXP Semiconductors'
-* relevant copyright in the software, without fee, provided that it
-* is used in conjunction with NXP Semiconductors microcontrollers. This
-* copyright, permission, and disclaimer notice must appear in all copies of
-* this code.
-**********************************************************************/
-
-/* Peripheral group ----------------------------------------------------------- */
-/** @addtogroup WDT
- * @{
- */
-
-/* Includes ------------------------------------------------------------------- */
-#include "lpc17xx_wdt.h"
-#include "lpc17xx_clkpwr.h"
-#include "lpc17xx_pinsel.h"
-
-
-/* If this source file built with example, the LPC17xx FW library configuration
- * file in each example directory ("lpc17xx_libcfg.h") must be included,
- * otherwise the default FW library configuration file must be included instead
- */
-#ifdef __BUILD_WITH_EXAMPLE__
-#include "lpc17xx_libcfg.h"
-#else
-#include "lpc17xx_libcfg_default.h"
-#endif /* __BUILD_WITH_EXAMPLE__ */
-
-
-#ifdef _WDT
-
-/* Private Functions ---------------------------------------------------------- */
-
-static uint8_t WDT_SetTimeOut (uint8_t clk_source, uint32_t timeout);
-
-/********************************************************************//**
- * @brief Set WDT time out value and WDT mode
- * @param[in] clk_source select Clock source for WDT device
- * @param[in] timeout value of time-out for WDT (us)
- * @return None
- *********************************************************************/
-static uint8_t WDT_SetTimeOut (uint8_t clk_source, uint32_t timeout)
-{
-
- uint32_t pclk_wdt = 0;
- uint32_t tempval = 0;
-
- switch ((WDT_CLK_OPT) clk_source)
- {
- case WDT_CLKSRC_IRC:
- pclk_wdt = 4000000;
- // Calculate TC in WDT
- tempval = ((((uint64_t)pclk_wdt * (uint64_t)timeout / 4) / (uint64_t)WDT_US_INDEX));
- // Check if it valid
- if (tempval >= WDT_TIMEOUT_MIN)
- {
- LPC_WDT->WDTC = tempval;
- return SUCCESS;
- }
-
- break;
-
- case WDT_CLKSRC_PCLK:
-
- // Get WDT clock with CCLK divider = 4
- pclk_wdt = SystemCoreClock / 4;
- // Calculate TC in WDT
- tempval = ((((uint64_t)pclk_wdt * (uint64_t)timeout / 4) / (uint64_t)WDT_US_INDEX));
-
- if (tempval >= WDT_TIMEOUT_MIN)
- {
- CLKPWR_SetPCLKDiv (CLKPWR_PCLKSEL_WDT, CLKPWR_PCLKSEL_CCLK_DIV_4);
- LPC_WDT->WDTC = (uint32_t) tempval;
- return SUCCESS;
- }
-
- // Get WDT clock with CCLK divider = 2
- pclk_wdt = SystemCoreClock / 2;
- // Calculate TC in WDT
- tempval = ((((uint64_t)pclk_wdt * (uint64_t)timeout / 4) / (uint64_t)WDT_US_INDEX));
-
- if (tempval >= WDT_TIMEOUT_MIN)
- {
- CLKPWR_SetPCLKDiv (CLKPWR_PCLKSEL_WDT, CLKPWR_PCLKSEL_CCLK_DIV_2);
- LPC_WDT->WDTC = (uint32_t) tempval;
- return SUCCESS;
- }
-
- // Get WDT clock with CCLK divider = 1
- pclk_wdt = SystemCoreClock;
- // Calculate TC in WDT
- tempval = ((((uint64_t)pclk_wdt * (uint64_t)timeout / 4) / (uint64_t)WDT_US_INDEX));
-
- if (tempval >= WDT_TIMEOUT_MIN)
- {
- CLKPWR_SetPCLKDiv (CLKPWR_PCLKSEL_WDT, CLKPWR_PCLKSEL_CCLK_DIV_1);
- LPC_WDT->WDTC = (uint32_t) tempval;
- return SUCCESS;
- }
- break ;
-
-
- case WDT_CLKSRC_RTC:
- pclk_wdt = 32768;
- // Calculate TC in WDT
- tempval = ((((uint64_t)pclk_wdt * (uint64_t)timeout / 4) / (uint64_t)WDT_US_INDEX));
- // Check if it valid
- if (tempval >= WDT_TIMEOUT_MIN)
- {
- LPC_WDT->WDTC = (uint32_t) tempval;
- return SUCCESS;
- }
-
- break;
-
-// Error parameter
- default:
- break;
-}
-
- return ERROR;
-}
-
-/* End of Private Functions --------------------------------------------------- */
-
-
-/* Public Functions ----------------------------------------------------------- */
-/** @addtogroup WDT_Public_Functions
- * @{
- */
-
-
-/*********************************************************************//**
-* @brief Initial for Watchdog function
-* Clock source = RTC ,
-* @param[in] ClkSrc Select clock source, should be:
-* - WDT_CLKSRC_IRC: Clock source from Internal RC oscillator
-* - WDT_CLKSRC_PCLK: Selects the APB peripheral clock (PCLK)
-* - WDT_CLKSRC_RTC: Selects the RTC oscillator
-* @param[in] WDTMode WDT mode, should be:
-* - WDT_MODE_INT_ONLY: Use WDT to generate interrupt only
-* - WDT_MODE_RESET: Use WDT to generate interrupt and reset MCU
-* @return None
- **********************************************************************/
-void WDT_Init (WDT_CLK_OPT ClkSrc, WDT_MODE_OPT WDTMode)
-{
- CHECK_PARAM(PARAM_WDT_CLK_OPT(ClkSrc));
- CHECK_PARAM(PARAM_WDT_MODE_OPT(WDTMode));
- CLKPWR_SetPCLKDiv (CLKPWR_PCLKSEL_WDT, CLKPWR_PCLKSEL_CCLK_DIV_4);
-
- //Set clock source
- LPC_WDT->WDCLKSEL &= ~WDT_WDCLKSEL_MASK;
- LPC_WDT->WDCLKSEL |= ClkSrc;
- //Set WDT mode
- if (WDTMode == WDT_MODE_RESET){
- LPC_WDT->WDMOD |= WDT_WDMOD(WDTMode);
- }
-}
-
-/*********************************************************************//**
-* @brief Start WDT activity with given timeout value
-* @param[in] TimeOut WDT reset after timeout if it is not feed
-* @return None
- **********************************************************************/
-void WDT_Start(uint32_t TimeOut)
-{
- uint32_t ClkSrc;
-
- ClkSrc = LPC_WDT->WDCLKSEL;
- ClkSrc &=WDT_WDCLKSEL_MASK;
- WDT_SetTimeOut(ClkSrc,TimeOut);
- //enable watchdog
- LPC_WDT->WDMOD |= WDT_WDMOD_WDEN;
- WDT_Feed();
-}
-
-/********************************************************************//**
- * @brief Read WDT Time out flag
- * @param[in] None
- * @return Time out flag status of WDT
- *********************************************************************/
-FlagStatus WDT_ReadTimeOutFlag (void)
-{
- return ((FlagStatus)((LPC_WDT->WDMOD & WDT_WDMOD_WDTOF) >>2));
-}
-
-/********************************************************************//**
- * @brief Clear WDT Time out flag
- * @param[in] None
- * @return None
- *********************************************************************/
-void WDT_ClrTimeOutFlag (void)
-{
- LPC_WDT->WDMOD &=~WDT_WDMOD_WDTOF;
-}
-
-/********************************************************************//**
- * @brief Update WDT timeout value and feed
- * @param[in] TimeOut TimeOut value to be updated
- * @return None
- *********************************************************************/
-void WDT_UpdateTimeOut ( uint32_t TimeOut)
-{
- uint32_t ClkSrc;
- ClkSrc = LPC_WDT->WDCLKSEL;
- ClkSrc &=WDT_WDCLKSEL_MASK;
- WDT_SetTimeOut(ClkSrc,TimeOut);
- WDT_Feed();
-}
-
-
-/********************************************************************//**
- * @brief After set WDTEN, call this function to start Watchdog
- * or reload the Watchdog timer
- * @param[in] None
- *
- * @return None
- *********************************************************************/
-void WDT_Feed (void)
-{
- // Disable irq interrupt
- __disable_irq();
- LPC_WDT->WDFEED = 0xAA;
- LPC_WDT->WDFEED = 0x55;
- // Then enable irq interrupt
- __enable_irq();
-}
-
-/********************************************************************//**
- * @brief Get the current value of WDT
- * @param[in] None
- * @return current value of WDT
- *********************************************************************/
-uint32_t WDT_GetCurrentCount(void)
-{
- return LPC_WDT->WDTV;
-}
-
-/**
- * @}
- */
-
-#endif /* _WDT */
-
-/**
- * @}
- */
-
-/* --------------------------------- End Of File ------------------------------ */
diff --git a/frameworks/CMSIS/LPC1768/include/LPC17xx.h b/frameworks/CMSIS/LPC1768/include/LPC17xx.h
deleted file mode 100644
index 2276260195..0000000000
--- a/frameworks/CMSIS/LPC1768/include/LPC17xx.h
+++ /dev/null
@@ -1,1078 +0,0 @@
-/**************************************************************************//**
- * @file LPC17xx.h
- * @brief CMSIS Cortex-M3 Core Peripheral Access Layer Header File for
- * NXP LPC17xx Device Series
- * @version: V1.09
- * @date: 25. July. 2011
- *
- * @note
- * Copyright (C) 2009 ARM Limited. All rights reserved.
- *
- * @par
- * ARM Limited (ARM) is supplying this software for use with Cortex-M
- * processor based microcontrollers. This file can be freely distributed
- * within development tools that are supporting such ARM based processors.
- *
- * @par
- * THIS SOFTWARE IS PROVIDED "AS IS". NO WARRANTIES, WHETHER EXPRESS, IMPLIED
- * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
- * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR
- * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
- *
- ******************************************************************************/
-
-
-#ifndef __LPC17xx_H__
-#define __LPC17xx_H__
-
-/*
- * ==========================================================================
- * ---------- Interrupt Number Definition -----------------------------------
- * ==========================================================================
- */
-
-/** @addtogroup LPC17xx_System
- * @{
- */
-
-/** @brief IRQ interrupt source definition */
-typedef enum IRQn
-{
-/****** Cortex-M3 Processor Exceptions Numbers ***************************************************/
- NonMaskableInt_IRQn = -14, /*!< 2 Non Maskable Interrupt */
- MemoryManagement_IRQn = -12, /*!< 4 Cortex-M3 Memory Management Interrupt */
- BusFault_IRQn = -11, /*!< 5 Cortex-M3 Bus Fault Interrupt */
- UsageFault_IRQn = -10, /*!< 6 Cortex-M3 Usage Fault Interrupt */
- SVCall_IRQn = -5, /*!< 11 Cortex-M3 SV Call Interrupt */
- DebugMonitor_IRQn = -4, /*!< 12 Cortex-M3 Debug Monitor Interrupt */
- PendSV_IRQn = -2, /*!< 14 Cortex-M3 Pend SV Interrupt */
- SysTick_IRQn = -1, /*!< 15 Cortex-M3 System Tick Interrupt */
-
-/****** LPC17xx Specific Interrupt Numbers *******************************************************/
- WDT_IRQn = 0, /*!< Watchdog Timer Interrupt */
- TIMER0_IRQn = 1, /*!< Timer0 Interrupt */
- TIMER1_IRQn = 2, /*!< Timer1 Interrupt */
- TIMER2_IRQn = 3, /*!< Timer2 Interrupt */
- TIMER3_IRQn = 4, /*!< Timer3 Interrupt */
- UART0_IRQn = 5, /*!< UART0 Interrupt */
- UART1_IRQn = 6, /*!< UART1 Interrupt */
- UART2_IRQn = 7, /*!< UART2 Interrupt */
- UART3_IRQn = 8, /*!< UART3 Interrupt */
- PWM1_IRQn = 9, /*!< PWM1 Interrupt */
- I2C0_IRQn = 10, /*!< I2C0 Interrupt */
- I2C1_IRQn = 11, /*!< I2C1 Interrupt */
- I2C2_IRQn = 12, /*!< I2C2 Interrupt */
- SPI_IRQn = 13, /*!< SPI Interrupt */
- SSP0_IRQn = 14, /*!< SSP0 Interrupt */
- SSP1_IRQn = 15, /*!< SSP1 Interrupt */
- PLL0_IRQn = 16, /*!< PLL0 Lock (Main PLL) Interrupt */
- RTC_IRQn = 17, /*!< Real Time Clock Interrupt */
- EINT0_IRQn = 18, /*!< External Interrupt 0 Interrupt */
- EINT1_IRQn = 19, /*!< External Interrupt 1 Interrupt */
- EINT2_IRQn = 20, /*!< External Interrupt 2 Interrupt */
- EINT3_IRQn = 21, /*!< External Interrupt 3 Interrupt */
- ADC_IRQn = 22, /*!< A/D Converter Interrupt */
- BOD_IRQn = 23, /*!< Brown-Out Detect Interrupt */
- USB_IRQn = 24, /*!< USB Interrupt */
- CAN_IRQn = 25, /*!< CAN Interrupt */
- DMA_IRQn = 26, /*!< General Purpose DMA Interrupt */
- I2S_IRQn = 27, /*!< I2S Interrupt */
- ENET_IRQn = 28, /*!< Ethernet Interrupt */
- RIT_IRQn = 29, /*!< Repetitive Interrupt Timer Interrupt */
- MCPWM_IRQn = 30, /*!< Motor Control PWM Interrupt */
- QEI_IRQn = 31, /*!< Quadrature Encoder Interface Interrupt */
- PLL1_IRQn = 32, /*!< PLL1 Lock (USB PLL) Interrupt */
- USBActivity_IRQn = 33, /*!< USB Activity Interrupt */
- CANActivity_IRQn = 34 /*!< CAN Activity Interrupt */
-} IRQn_Type;
-
-
-/*
- * ==========================================================================
- * ----------- Processor and Core Peripheral Section ------------------------
- * ==========================================================================
- */
-
-/* Configuration of the Cortex-M3 Processor and Core Peripherals */
-#define __MPU_PRESENT 1 /*!< MPU present or not */
-#define __NVIC_PRIO_BITS 5 /*!< Number of Bits used for Priority Levels */
-#define __Vendor_SysTickConfig 1 /*!< Set to 1 if different SysTick Config is used */
-
-
-#include "core_cm3.h" /* Cortex-M3 processor and core peripherals */
-#include "system_LPC17xx.h" /* System Header */
-
-
-/******************************************************************************/
-/* Device Specific Peripheral registers structures */
-/******************************************************************************/
-
-#if defined ( __CC_ARM )
-#pragma anon_unions
-#endif
-
-/*------------- System Control (SC) ------------------------------------------*/
-/** @brief System Control (SC) register structure definition */
-typedef struct
-{
- __IO uint32_t FLASHCFG; /* Flash Accelerator Module */
- uint32_t RESERVED0[31];
- __IO uint32_t PLL0CON; /* Clocking and Power Control */
- __IO uint32_t PLL0CFG;
- __I uint32_t PLL0STAT;
- __O uint32_t PLL0FEED;
- uint32_t RESERVED1[4];
- __IO uint32_t PLL1CON;
- __IO uint32_t PLL1CFG;
- __I uint32_t PLL1STAT;
- __O uint32_t PLL1FEED;
- uint32_t RESERVED2[4];
- __IO uint32_t PCON;
- __IO uint32_t PCONP;
- uint32_t RESERVED3[15];
- __IO uint32_t CCLKCFG;
- __IO uint32_t USBCLKCFG;
- __IO uint32_t CLKSRCSEL;
- __IO uint32_t CANSLEEPCLR;
- __IO uint32_t CANWAKEFLAGS;
- uint32_t RESERVED4[10];
- __IO uint32_t EXTINT; /* External Interrupts */
- uint32_t RESERVED5;
- __IO uint32_t EXTMODE;
- __IO uint32_t EXTPOLAR;
- uint32_t RESERVED6[12];
- __IO uint32_t RSID; /* Reset */
- uint32_t RESERVED7[7];
- __IO uint32_t SCS; /* Syscon Miscellaneous Registers */
- __IO uint32_t IRCTRIM; /* Clock Dividers */
- __IO uint32_t PCLKSEL0;
- __IO uint32_t PCLKSEL1;
- uint32_t RESERVED8[4];
- __IO uint32_t USBIntSt; /* USB Device/OTG Interrupt Register */
- __IO uint32_t DMAREQSEL;
- __IO uint32_t CLKOUTCFG; /* Clock Output Configuration */
- } LPC_SC_TypeDef;
-
-/*------------- Pin Connect Block (PINCON) -----------------------------------*/
-/** @brief Pin Connect Block (PINCON) register structure definition */
-typedef struct
-{
- __IO uint32_t PINSEL0;
- __IO uint32_t PINSEL1;
- __IO uint32_t PINSEL2;
- __IO uint32_t PINSEL3;
- __IO uint32_t PINSEL4;
- __IO uint32_t PINSEL5;
- __IO uint32_t PINSEL6;
- __IO uint32_t PINSEL7;
- __IO uint32_t PINSEL8;
- __IO uint32_t PINSEL9;
- __IO uint32_t PINSEL10;
- uint32_t RESERVED0[5];
- __IO uint32_t PINMODE0;
- __IO uint32_t PINMODE1;
- __IO uint32_t PINMODE2;
- __IO uint32_t PINMODE3;
- __IO uint32_t PINMODE4;
- __IO uint32_t PINMODE5;
- __IO uint32_t PINMODE6;
- __IO uint32_t PINMODE7;
- __IO uint32_t PINMODE8;
- __IO uint32_t PINMODE9;
- __IO uint32_t PINMODE_OD0;
- __IO uint32_t PINMODE_OD1;
- __IO uint32_t PINMODE_OD2;
- __IO uint32_t PINMODE_OD3;
- __IO uint32_t PINMODE_OD4;
- __IO uint32_t I2CPADCFG;
-} LPC_PINCON_TypeDef;
-
-/*------------- General Purpose Input/Output (GPIO) --------------------------*/
-/** @brief General Purpose Input/Output (GPIO) register structure definition */
-typedef struct
-{
- union {
- __IO uint32_t FIODIR;
- struct {
- __IO uint16_t FIODIRL;
- __IO uint16_t FIODIRH;
- };
- struct {
- __IO uint8_t FIODIR0;
- __IO uint8_t FIODIR1;
- __IO uint8_t FIODIR2;
- __IO uint8_t FIODIR3;
- };
- };
- uint32_t RESERVED0[3];
- union {
- __IO uint32_t FIOMASK;
- struct {
- __IO uint16_t FIOMASKL;
- __IO uint16_t FIOMASKH;
- };
- struct {
- __IO uint8_t FIOMASK0;
- __IO uint8_t FIOMASK1;
- __IO uint8_t FIOMASK2;
- __IO uint8_t FIOMASK3;
- };
- };
- union {
- __IO uint32_t FIOPIN;
- struct {
- __IO uint16_t FIOPINL;
- __IO uint16_t FIOPINH;
- };
- struct {
- __IO uint8_t FIOPIN0;
- __IO uint8_t FIOPIN1;
- __IO uint8_t FIOPIN2;
- __IO uint8_t FIOPIN3;
- };
- };
- union {
- __IO uint32_t FIOSET;
- struct {
- __IO uint16_t FIOSETL;
- __IO uint16_t FIOSETH;
- };
- struct {
- __IO uint8_t FIOSET0;
- __IO uint8_t FIOSET1;
- __IO uint8_t FIOSET2;
- __IO uint8_t FIOSET3;
- };
- };
- union {
- __O uint32_t FIOCLR;
- struct {
- __O uint16_t FIOCLRL;
- __O uint16_t FIOCLRH;
- };
- struct {
- __O uint8_t FIOCLR0;
- __O uint8_t FIOCLR1;
- __O uint8_t FIOCLR2;
- __O uint8_t FIOCLR3;
- };
- };
-} LPC_GPIO_TypeDef;
-
-/** @brief General Purpose Input/Output interrupt (GPIOINT) register structure definition */
-typedef struct
-{
- __I uint32_t IntStatus;
- __I uint32_t IO0IntStatR;
- __I uint32_t IO0IntStatF;
- __O uint32_t IO0IntClr;
- __IO uint32_t IO0IntEnR;
- __IO uint32_t IO0IntEnF;
- uint32_t RESERVED0[3];
- __I uint32_t IO2IntStatR;
- __I uint32_t IO2IntStatF;
- __O uint32_t IO2IntClr;
- __IO uint32_t IO2IntEnR;
- __IO uint32_t IO2IntEnF;
-} LPC_GPIOINT_TypeDef;
-
-/*------------- Timer (TIM) --------------------------------------------------*/
-/** @brief Timer (TIM) register structure definition */
-typedef struct
-{
- __IO uint32_t IR;
- __IO uint32_t TCR;
- __IO uint32_t TC;
- __IO uint32_t PR;
- __IO uint32_t PC;
- __IO uint32_t MCR;
- __IO uint32_t MR0;
- __IO uint32_t MR1;
- __IO uint32_t MR2;
- __IO uint32_t MR3;
- __IO uint32_t CCR;
- __I uint32_t CR0;
- __I uint32_t CR1;
- uint32_t RESERVED0[2];
- __IO uint32_t EMR;
- uint32_t RESERVED1[12];
- __IO uint32_t CTCR;
-} LPC_TIM_TypeDef;
-
-/*------------- Pulse-Width Modulation (PWM) ---------------------------------*/
-/** @brief Pulse-Width Modulation (PWM) register structure definition */
-typedef struct
-{
- __IO uint32_t IR;
- __IO uint32_t TCR;
- __IO uint32_t TC;
- __IO uint32_t PR;
- __IO uint32_t PC;
- __IO uint32_t MCR;
- __IO uint32_t MR0;
- __IO uint32_t MR1;
- __IO uint32_t MR2;
- __IO uint32_t MR3;
- __IO uint32_t CCR;
- __I uint32_t CR0;
- __I uint32_t CR1;
- __I uint32_t CR2;
- __I uint32_t CR3;
- uint32_t RESERVED0;
- __IO uint32_t MR4;
- __IO uint32_t MR5;
- __IO uint32_t MR6;
- __IO uint32_t PCR;
- __IO uint32_t LER;
- uint32_t RESERVED1[7];
- __IO uint32_t CTCR;
-} LPC_PWM_TypeDef;
-
-/*------------- Universal Asynchronous Receiver Transmitter (UART) -----------*/
-/** @brief Universal Asynchronous Receiver Transmitter (UART) register structure definition */
-typedef struct
-{
- union {
- __I uint8_t RBR;
- __O uint8_t THR;
- __IO uint8_t DLL;
- uint32_t RESERVED0;
- };
- union {
- __IO uint8_t DLM;
- __IO uint32_t IER;
- };
- union {
- __I uint32_t IIR;
- __O uint8_t FCR;
- };
- __IO uint8_t LCR;
- uint8_t RESERVED1[7];
- __I uint8_t LSR;
- uint8_t RESERVED2[7];
- __IO uint8_t SCR;
- uint8_t RESERVED3[3];
- __IO uint32_t ACR;
- __IO uint8_t ICR;
- uint8_t RESERVED4[3];
- __IO uint8_t FDR;
- uint8_t RESERVED5[7];
- __IO uint8_t TER;
- uint8_t RESERVED6[39];
- __I uint8_t FIFOLVL;
-} LPC_UART_TypeDef;
-
-/** @brief Universal Asynchronous Receiver Transmitter 0 (UART0) register structure definition */
-typedef struct
-{
- union {
- __I uint8_t RBR;
- __O uint8_t THR;
- __IO uint8_t DLL;
- uint32_t RESERVED0;
- };
- union {
- __IO uint8_t DLM;
- __IO uint32_t IER;
- };
- union {
- __I uint32_t IIR;
- __O uint8_t FCR;
- };
- __IO uint8_t LCR;
- uint8_t RESERVED1[7];
- __I uint8_t LSR;
- uint8_t RESERVED2[7];
- __IO uint8_t SCR;
- uint8_t RESERVED3[3];
- __IO uint32_t ACR;
- __IO uint8_t ICR;
- uint8_t RESERVED4[3];
- __IO uint8_t FDR;
- uint8_t RESERVED5[7];
- __IO uint8_t TER;
- uint8_t RESERVED6[39];
- __I uint8_t FIFOLVL;
-} LPC_UART0_TypeDef;
-
-/** @brief Universal Asynchronous Receiver Transmitter 1 (UART1) register structure definition */
-typedef struct
-{
- union {
- __I uint8_t RBR;
- __O uint8_t THR;
- __IO uint8_t DLL;
- uint32_t RESERVED0;
- };
- union {
- __IO uint8_t DLM;
- __IO uint32_t IER;
- };
- union {
- __I uint32_t IIR;
- __O uint8_t FCR;
- };
- __IO uint8_t LCR;
- uint8_t RESERVED1[3];
- __IO uint8_t MCR;
- uint8_t RESERVED2[3];
- __I uint8_t LSR;
- uint8_t RESERVED3[3];
- __I uint8_t MSR;
- uint8_t RESERVED4[3];
- __IO uint8_t SCR;
- uint8_t RESERVED5[3];
- __IO uint32_t ACR;
- uint32_t RESERVED6;
- __IO uint32_t FDR;
- uint32_t RESERVED7;
- __IO uint8_t TER;
- uint8_t RESERVED8[27];
- __IO uint8_t RS485CTRL;
- uint8_t RESERVED9[3];
- __IO uint8_t ADRMATCH;
- uint8_t RESERVED10[3];
- __IO uint8_t RS485DLY;
- uint8_t RESERVED11[3];
- __I uint8_t FIFOLVL;
-} LPC_UART1_TypeDef;
-
-/*------------- Serial Peripheral Interface (SPI) ----------------------------*/
-/** @brief Serial Peripheral Interface (SPI) register structure definition */
-typedef struct
-{
- __IO uint32_t SPCR;
- __I uint32_t SPSR;
- __IO uint32_t SPDR;
- __IO uint32_t SPCCR;
- uint32_t RESERVED0[3];
- __IO uint32_t SPINT;
-} LPC_SPI_TypeDef;
-
-/*------------- Synchronous Serial Communication (SSP) -----------------------*/
-/** @brief Synchronous Serial Communication (SSP) register structure definition */
-typedef struct
-{
- __IO uint32_t CR0;
- __IO uint32_t CR1;
- __IO uint32_t DR;
- __I uint32_t SR;
- __IO uint32_t CPSR;
- __IO uint32_t IMSC;
- __IO uint32_t RIS;
- __IO uint32_t MIS;
- __IO uint32_t ICR;
- __IO uint32_t DMACR;
-} LPC_SSP_TypeDef;
-
-/*------------- Inter-Integrated Circuit (I2C) -------------------------------*/
-/** @brief Inter-Integrated Circuit (I2C) register structure definition */
-typedef struct
-{
- __IO uint32_t I2CONSET;
- __I uint32_t I2STAT;
- __IO uint32_t I2DAT;
- __IO uint32_t I2ADR0;
- __IO uint32_t I2SCLH;
- __IO uint32_t I2SCLL;
- __O uint32_t I2CONCLR;
- __IO uint32_t MMCTRL;
- __IO uint32_t I2ADR1;
- __IO uint32_t I2ADR2;
- __IO uint32_t I2ADR3;
- __I uint32_t I2DATA_BUFFER;
- __IO uint32_t I2MASK0;
- __IO uint32_t I2MASK1;
- __IO uint32_t I2MASK2;
- __IO uint32_t I2MASK3;
-} LPC_I2C_TypeDef;
-
-/*------------- Inter IC Sound (I2S) -----------------------------------------*/
-/** @brief Inter IC Sound (I2S) register structure definition */
-typedef struct
-{
- __IO uint32_t I2SDAO;
- __IO uint32_t I2SDAI;
- __O uint32_t I2STXFIFO;
- __I uint32_t I2SRXFIFO;
- __I uint32_t I2SSTATE;
- __IO uint32_t I2SDMA1;
- __IO uint32_t I2SDMA2;
- __IO uint32_t I2SIRQ;
- __IO uint32_t I2STXRATE;
- __IO uint32_t I2SRXRATE;
- __IO uint32_t I2STXBITRATE;
- __IO uint32_t I2SRXBITRATE;
- __IO uint32_t I2STXMODE;
- __IO uint32_t I2SRXMODE;
-} LPC_I2S_TypeDef;
-
-/*------------- Repetitive Interrupt Timer (RIT) -----------------------------*/
-/** @brief Repetitive Interrupt Timer (RIT) register structure definition */
-typedef struct
-{
- __IO uint32_t RICOMPVAL;
- __IO uint32_t RIMASK;
- __IO uint8_t RICTRL;
- uint8_t RESERVED0[3];
- __IO uint32_t RICOUNTER;
-} LPC_RIT_TypeDef;
-
-/*------------- Real-Time Clock (RTC) ----------------------------------------*/
-/** @brief Real-Time Clock (RTC) register structure definition */
-typedef struct
-{
- __IO uint8_t ILR;
- uint8_t RESERVED0[7];
- __IO uint8_t CCR;
- uint8_t RESERVED1[3];
- __IO uint8_t CIIR;
- uint8_t RESERVED2[3];
- __IO uint8_t AMR;
- uint8_t RESERVED3[3];
- __I uint32_t CTIME0;
- __I uint32_t CTIME1;
- __I uint32_t CTIME2;
- __IO uint8_t SEC;
- uint8_t RESERVED4[3];
- __IO uint8_t MIN;
- uint8_t RESERVED5[3];
- __IO uint8_t HOUR;
- uint8_t RESERVED6[3];
- __IO uint8_t DOM;
- uint8_t RESERVED7[3];
- __IO uint8_t DOW;
- uint8_t RESERVED8[3];
- __IO uint16_t DOY;
- uint16_t RESERVED9;
- __IO uint8_t MONTH;
- uint8_t RESERVED10[3];
- __IO uint16_t YEAR;
- uint16_t RESERVED11;
- __IO uint32_t CALIBRATION;
- __IO uint32_t GPREG0;
- __IO uint32_t GPREG1;
- __IO uint32_t GPREG2;
- __IO uint32_t GPREG3;
- __IO uint32_t GPREG4;
- __IO uint8_t RTC_AUXEN;
- uint8_t RESERVED12[3];
- __IO uint8_t RTC_AUX;
- uint8_t RESERVED13[3];
- __IO uint8_t ALSEC;
- uint8_t RESERVED14[3];
- __IO uint8_t ALMIN;
- uint8_t RESERVED15[3];
- __IO uint8_t ALHOUR;
- uint8_t RESERVED16[3];
- __IO uint8_t ALDOM;
- uint8_t RESERVED17[3];
- __IO uint8_t ALDOW;
- uint8_t RESERVED18[3];
- __IO uint16_t ALDOY;
- uint16_t RESERVED19;
- __IO uint8_t ALMON;
- uint8_t RESERVED20[3];
- __IO uint16_t ALYEAR;
- uint16_t RESERVED21;
-} LPC_RTC_TypeDef;
-
-/*------------- Watchdog Timer (WDT) -----------------------------------------*/
-/** @brief Watchdog Timer (WDT) register structure definition */
-typedef struct
-{
- __IO uint8_t WDMOD;
- uint8_t RESERVED0[3];
- __IO uint32_t WDTC;
- __O uint8_t WDFEED;
- uint8_t RESERVED1[3];
- __I uint32_t WDTV;
- __IO uint32_t WDCLKSEL;
-} LPC_WDT_TypeDef;
-
-/*------------- Analog-to-Digital Converter (ADC) ----------------------------*/
-/** @brief Analog-to-Digital Converter (ADC) register structure definition */
-typedef struct
-{
- __IO uint32_t ADCR;
- __IO uint32_t ADGDR;
- uint32_t RESERVED0;
- __IO uint32_t ADINTEN;
- __I uint32_t ADDR0;
- __I uint32_t ADDR1;
- __I uint32_t ADDR2;
- __I uint32_t ADDR3;
- __I uint32_t ADDR4;
- __I uint32_t ADDR5;
- __I uint32_t ADDR6;
- __I uint32_t ADDR7;
- __I uint32_t ADSTAT;
- __IO uint32_t ADTRM;
-} LPC_ADC_TypeDef;
-
-/*------------- Digital-to-Analog Converter (DAC) ----------------------------*/
-/** @brief Digital-to-Analog Converter (DAC) register structure definition */
-typedef struct
-{
- __IO uint32_t DACR;
- __IO uint32_t DACCTRL;
- __IO uint16_t DACCNTVAL;
-} LPC_DAC_TypeDef;
-
-/*------------- Motor Control Pulse-Width Modulation (MCPWM) -----------------*/
-/** @brief Motor Control Pulse-Width Modulation (MCPWM) register structure definition */
-typedef struct
-{
- __I uint32_t MCCON;
- __O uint32_t MCCON_SET;
- __O uint32_t MCCON_CLR;
- __I uint32_t MCCAPCON;
- __O uint32_t MCCAPCON_SET;
- __O uint32_t MCCAPCON_CLR;
- __IO uint32_t MCTIM0;
- __IO uint32_t MCTIM1;
- __IO uint32_t MCTIM2;
- __IO uint32_t MCPER0;
- __IO uint32_t MCPER1;
- __IO uint32_t MCPER2;
- __IO uint32_t MCPW0;
- __IO uint32_t MCPW1;
- __IO uint32_t MCPW2;
- __IO uint32_t MCDEADTIME;
- __IO uint32_t MCCCP;
- __IO uint32_t MCCR0;
- __IO uint32_t MCCR1;
- __IO uint32_t MCCR2;
- __I uint32_t MCINTEN;
- __O uint32_t MCINTEN_SET;
- __O uint32_t MCINTEN_CLR;
- __I uint32_t MCCNTCON;
- __O uint32_t MCCNTCON_SET;
- __O uint32_t MCCNTCON_CLR;
- __I uint32_t MCINTFLAG;
- __O uint32_t MCINTFLAG_SET;
- __O uint32_t MCINTFLAG_CLR;
- __O uint32_t MCCAP_CLR;
-} LPC_MCPWM_TypeDef;
-
-/*------------- Quadrature Encoder Interface (QEI) ---------------------------*/
-/** @brief Quadrature Encoder Interface (QEI) register structure definition */
-typedef struct
-{
- __O uint32_t QEICON;
- __I uint32_t QEISTAT;
- __IO uint32_t QEICONF;
- __I uint32_t QEIPOS;
- __IO uint32_t QEIMAXPOS;
- __IO uint32_t CMPOS0;
- __IO uint32_t CMPOS1;
- __IO uint32_t CMPOS2;
- __I uint32_t INXCNT;
- __IO uint32_t INXCMP;
- __IO uint32_t QEILOAD;
- __I uint32_t QEITIME;
- __I uint32_t QEIVEL;
- __I uint32_t QEICAP;
- __IO uint32_t VELCOMP;
- __IO uint32_t FILTER;
- uint32_t RESERVED0[998];
- __O uint32_t QEIIEC;
- __O uint32_t QEIIES;
- __I uint32_t QEIINTSTAT;
- __I uint32_t QEIIE;
- __O uint32_t QEICLR;
- __O uint32_t QEISET;
-} LPC_QEI_TypeDef;
-
-/*------------- Controller Area Network (CAN) --------------------------------*/
-/** @brief Controller Area Network Acceptance Filter RAM (CANAF_RAM)structure definition */
-typedef struct
-{
- __IO uint32_t mask[512]; /* ID Masks */
-} LPC_CANAF_RAM_TypeDef;
-
-/** @brief Controller Area Network Acceptance Filter(CANAF) register structure definition */
-typedef struct /* Acceptance Filter Registers */
-{
- __IO uint32_t AFMR;
- __IO uint32_t SFF_sa;
- __IO uint32_t SFF_GRP_sa;
- __IO uint32_t EFF_sa;
- __IO uint32_t EFF_GRP_sa;
- __IO uint32_t ENDofTable;
- __I uint32_t LUTerrAd;
- __I uint32_t LUTerr;
- __IO uint32_t FCANIE;
- __IO uint32_t FCANIC0;
- __IO uint32_t FCANIC1;
-} LPC_CANAF_TypeDef;
-
-/** @brief Controller Area Network Central (CANCR) register structure definition */
-typedef struct /* Central Registers */
-{
- __I uint32_t CANTxSR;
- __I uint32_t CANRxSR;
- __I uint32_t CANMSR;
-} LPC_CANCR_TypeDef;
-
-/** @brief Controller Area Network Controller (CAN) register structure definition */
-typedef struct /* Controller Registers */
-{
- __IO uint32_t MOD;
- __O uint32_t CMR;
- __IO uint32_t GSR;
- __I uint32_t ICR;
- __IO uint32_t IER;
- __IO uint32_t BTR;
- __IO uint32_t EWL;
- __I uint32_t SR;
- __IO uint32_t RFS;
- __IO uint32_t RID;
- __IO uint32_t RDA;
- __IO uint32_t RDB;
- __IO uint32_t TFI1;
- __IO uint32_t TID1;
- __IO uint32_t TDA1;
- __IO uint32_t TDB1;
- __IO uint32_t TFI2;
- __IO uint32_t TID2;
- __IO uint32_t TDA2;
- __IO uint32_t TDB2;
- __IO uint32_t TFI3;
- __IO uint32_t TID3;
- __IO uint32_t TDA3;
- __IO uint32_t TDB3;
-} LPC_CAN_TypeDef;
-
-/*------------- General Purpose Direct Memory Access (GPDMA) -----------------*/
-/** @brief General Purpose Direct Memory Access (GPDMA) register structure definition */
-typedef struct /* Common Registers */
-{
- __I uint32_t DMACIntStat;
- __I uint32_t DMACIntTCStat;
- __O uint32_t DMACIntTCClear;
- __I uint32_t DMACIntErrStat;
- __O uint32_t DMACIntErrClr;
- __I uint32_t DMACRawIntTCStat;
- __I uint32_t DMACRawIntErrStat;
- __I uint32_t DMACEnbldChns;
- __IO uint32_t DMACSoftBReq;
- __IO uint32_t DMACSoftSReq;
- __IO uint32_t DMACSoftLBReq;
- __IO uint32_t DMACSoftLSReq;
- __IO uint32_t DMACConfig;
- __IO uint32_t DMACSync;
-} LPC_GPDMA_TypeDef;
-
-/** @brief General Purpose Direct Memory Access Channel (GPDMACH) register structure definition */
-typedef struct /* Channel Registers */
-{
- __IO uint32_t DMACCSrcAddr;
- __IO uint32_t DMACCDestAddr;
- __IO uint32_t DMACCLLI;
- __IO uint32_t DMACCControl;
- __IO uint32_t DMACCConfig;
-} LPC_GPDMACH_TypeDef;
-
-/*------------- Universal Serial Bus (USB) -----------------------------------*/
-/** @brief Universal Serial Bus (USB) register structure definition */
-typedef struct
-{
- __I uint32_t HcRevision; /* USB Host Registers */
- __IO uint32_t HcControl;
- __IO uint32_t HcCommandStatus;
- __IO uint32_t HcInterruptStatus;
- __IO uint32_t HcInterruptEnable;
- __IO uint32_t HcInterruptDisable;
- __IO uint32_t HcHCCA;
- __I uint32_t HcPeriodCurrentED;
- __IO uint32_t HcControlHeadED;
- __IO uint32_t HcControlCurrentED;
- __IO uint32_t HcBulkHeadED;
- __IO uint32_t HcBulkCurrentED;
- __I uint32_t HcDoneHead;
- __IO uint32_t HcFmInterval;
- __I uint32_t HcFmRemaining;
- __I uint32_t HcFmNumber;
- __IO uint32_t HcPeriodicStart;
- __IO uint32_t HcLSTreshold;
- __IO uint32_t HcRhDescriptorA;
- __IO uint32_t HcRhDescriptorB;
- __IO uint32_t HcRhStatus;
- __IO uint32_t HcRhPortStatus1;
- __IO uint32_t HcRhPortStatus2;
- uint32_t RESERVED0[40];
- __I uint32_t Module_ID;
-
- __I uint32_t OTGIntSt; /* USB On-The-Go Registers */
- __IO uint32_t OTGIntEn;
- __O uint32_t OTGIntSet;
- __O uint32_t OTGIntClr;
- __IO uint32_t OTGStCtrl;
- __IO uint32_t OTGTmr;
- uint32_t RESERVED1[58];
-
- __I uint32_t USBDevIntSt; /* USB Device Interrupt Registers */
- __IO uint32_t USBDevIntEn;
- __O uint32_t USBDevIntClr;
- __O uint32_t USBDevIntSet;
-
- __O uint32_t USBCmdCode; /* USB Device SIE Command Registers */
- __I uint32_t USBCmdData;
-
- __I uint32_t USBRxData; /* USB Device Transfer Registers */
- __O uint32_t USBTxData;
- __I uint32_t USBRxPLen;
- __O uint32_t USBTxPLen;
- __IO uint32_t USBCtrl;
- __O uint32_t USBDevIntPri;
-
- __I uint32_t USBEpIntSt; /* USB Device Endpoint Interrupt Regs */
- __IO uint32_t USBEpIntEn;
- __O uint32_t USBEpIntClr;
- __O uint32_t USBEpIntSet;
- __O uint32_t USBEpIntPri;
-
- __IO uint32_t USBReEp; /* USB Device Endpoint Realization Reg*/
- __O uint32_t USBEpInd;
- __IO uint32_t USBMaxPSize;
-
- __I uint32_t USBDMARSt; /* USB Device DMA Registers */
- __O uint32_t USBDMARClr;
- __O uint32_t USBDMARSet;
- uint32_t RESERVED2[9];
- __IO uint32_t USBUDCAH;
- __I uint32_t USBEpDMASt;
- __O uint32_t USBEpDMAEn;
- __O uint32_t USBEpDMADis;
- __I uint32_t USBDMAIntSt;
- __IO uint32_t USBDMAIntEn;
- uint32_t RESERVED3[2];
- __I uint32_t USBEoTIntSt;
- __O uint32_t USBEoTIntClr;
- __O uint32_t USBEoTIntSet;
- __I uint32_t USBNDDRIntSt;
- __O uint32_t USBNDDRIntClr;
- __O uint32_t USBNDDRIntSet;
- __I uint32_t USBSysErrIntSt;
- __O uint32_t USBSysErrIntClr;
- __O uint32_t USBSysErrIntSet;
- uint32_t RESERVED4[15];
-
- union {
- __I uint32_t I2C_RX; /* USB OTG I2C Registers */
- __O uint32_t I2C_TX;
- };
- __I uint32_t I2C_STS;
- __IO uint32_t I2C_CTL;
- __IO uint32_t I2C_CLKHI;
- __O uint32_t I2C_CLKLO;
- uint32_t RESERVED5[824];
-
- union {
- __IO uint32_t USBClkCtrl; /* USB Clock Control Registers */
- __IO uint32_t OTGClkCtrl;
- };
- union {
- __I uint32_t USBClkSt;
- __I uint32_t OTGClkSt;
- };
-} LPC_USB_TypeDef;
-
-/*------------- Ethernet Media Access Controller (EMAC) ----------------------*/
-/** @brief Ethernet Media Access Controller (EMAC) register structure definition */
-typedef struct
-{
- __IO uint32_t MAC1; /* MAC Registers */
- __IO uint32_t MAC2;
- __IO uint32_t IPGT;
- __IO uint32_t IPGR;
- __IO uint32_t CLRT;
- __IO uint32_t MAXF;
- __IO uint32_t SUPP;
- __IO uint32_t TEST;
- __IO uint32_t MCFG;
- __IO uint32_t MCMD;
- __IO uint32_t MADR;
- __O uint32_t MWTD;
- __I uint32_t MRDD;
- __I uint32_t MIND;
- uint32_t RESERVED0[2];
- __IO uint32_t SA0;
- __IO uint32_t SA1;
- __IO uint32_t SA2;
- uint32_t RESERVED1[45];
- __IO uint32_t Command; /* Control Registers */
- __I uint32_t Status;
- __IO uint32_t RxDescriptor;
- __IO uint32_t RxStatus;
- __IO uint32_t RxDescriptorNumber;
- __I uint32_t RxProduceIndex;
- __IO uint32_t RxConsumeIndex;
- __IO uint32_t TxDescriptor;
- __IO uint32_t TxStatus;
- __IO uint32_t TxDescriptorNumber;
- __IO uint32_t TxProduceIndex;
- __I uint32_t TxConsumeIndex;
- uint32_t RESERVED2[10];
- __I uint32_t TSV0;
- __I uint32_t TSV1;
- __I uint32_t RSV;
- uint32_t RESERVED3[3];
- __IO uint32_t FlowControlCounter;
- __I uint32_t FlowControlStatus;
- uint32_t RESERVED4[34];
- __IO uint32_t RxFilterCtrl; /* Rx Filter Registers */
- __IO uint32_t RxFilterWoLStatus;
- __IO uint32_t RxFilterWoLClear;
- uint32_t RESERVED5;
- __IO uint32_t HashFilterL;
- __IO uint32_t HashFilterH;
- uint32_t RESERVED6[882];
- __I uint32_t IntStatus; /* Module Control Registers */
- __IO uint32_t IntEnable;
- __O uint32_t IntClear;
- __O uint32_t IntSet;
- uint32_t RESERVED7;
- __IO uint32_t PowerDown;
- uint32_t RESERVED8;
- __IO uint32_t Module_ID;
-} LPC_EMAC_TypeDef;
-
-
-#if defined ( __CC_ARM )
-#pragma no_anon_unions
-#endif
-
-
-/******************************************************************************/
-/* Peripheral memory map */
-/******************************************************************************/
-/* Base addresses */
-#define LPC_FLASH_BASE (0x00000000UL)
-#define LPC_RAM_BASE (0x10000000UL)
-#ifdef __LPC17XX_REV00
-#define LPC_AHBRAM0_BASE (0x20000000UL)
-#define LPC_AHBRAM1_BASE (0x20004000UL)
-#else
-#define LPC_AHBRAM0_BASE (0x2007C000UL)
-#define LPC_AHBRAM1_BASE (0x20080000UL)
-#endif
-#define LPC_GPIO_BASE (0x2009C000UL)
-#define LPC_APB0_BASE (0x40000000UL)
-#define LPC_APB1_BASE (0x40080000UL)
-#define LPC_AHB_BASE (0x50000000UL)
-#define LPC_CM3_BASE (0xE0000000UL)
-
-/* APB0 peripherals */
-#define LPC_WDT_BASE (LPC_APB0_BASE + 0x00000)
-#define LPC_TIM0_BASE (LPC_APB0_BASE + 0x04000)
-#define LPC_TIM1_BASE (LPC_APB0_BASE + 0x08000)
-#define LPC_UART0_BASE (LPC_APB0_BASE + 0x0C000)
-#define LPC_UART1_BASE (LPC_APB0_BASE + 0x10000)
-#define LPC_PWM1_BASE (LPC_APB0_BASE + 0x18000)
-#define LPC_I2C0_BASE (LPC_APB0_BASE + 0x1C000)
-#define LPC_SPI_BASE (LPC_APB0_BASE + 0x20000)
-#define LPC_RTC_BASE (LPC_APB0_BASE + 0x24000)
-#define LPC_GPIOINT_BASE (LPC_APB0_BASE + 0x28080)
-#define LPC_PINCON_BASE (LPC_APB0_BASE + 0x2C000)
-#define LPC_SSP1_BASE (LPC_APB0_BASE + 0x30000)
-#define LPC_ADC_BASE (LPC_APB0_BASE + 0x34000)
-#define LPC_CANAF_RAM_BASE (LPC_APB0_BASE + 0x38000)
-#define LPC_CANAF_BASE (LPC_APB0_BASE + 0x3C000)
-#define LPC_CANCR_BASE (LPC_APB0_BASE + 0x40000)
-#define LPC_CAN1_BASE (LPC_APB0_BASE + 0x44000)
-#define LPC_CAN2_BASE (LPC_APB0_BASE + 0x48000)
-#define LPC_I2C1_BASE (LPC_APB0_BASE + 0x5C000)
-
-/* APB1 peripherals */
-#define LPC_SSP0_BASE (LPC_APB1_BASE + 0x08000)
-#define LPC_DAC_BASE (LPC_APB1_BASE + 0x0C000)
-#define LPC_TIM2_BASE (LPC_APB1_BASE + 0x10000)
-#define LPC_TIM3_BASE (LPC_APB1_BASE + 0x14000)
-#define LPC_UART2_BASE (LPC_APB1_BASE + 0x18000)
-#define LPC_UART3_BASE (LPC_APB1_BASE + 0x1C000)
-#define LPC_I2C2_BASE (LPC_APB1_BASE + 0x20000)
-#define LPC_I2S_BASE (LPC_APB1_BASE + 0x28000)
-#define LPC_RIT_BASE (LPC_APB1_BASE + 0x30000)
-#define LPC_MCPWM_BASE (LPC_APB1_BASE + 0x38000)
-#define LPC_QEI_BASE (LPC_APB1_BASE + 0x3C000)
-#define LPC_SC_BASE (LPC_APB1_BASE + 0x7C000)
-
-/* AHB peripherals */
-#define LPC_EMAC_BASE (LPC_AHB_BASE + 0x00000)
-#define LPC_GPDMA_BASE (LPC_AHB_BASE + 0x04000)
-#define LPC_GPDMACH0_BASE (LPC_AHB_BASE + 0x04100)
-#define LPC_GPDMACH1_BASE (LPC_AHB_BASE + 0x04120)
-#define LPC_GPDMACH2_BASE (LPC_AHB_BASE + 0x04140)
-#define LPC_GPDMACH3_BASE (LPC_AHB_BASE + 0x04160)
-#define LPC_GPDMACH4_BASE (LPC_AHB_BASE + 0x04180)
-#define LPC_GPDMACH5_BASE (LPC_AHB_BASE + 0x041A0)
-#define LPC_GPDMACH6_BASE (LPC_AHB_BASE + 0x041C0)
-#define LPC_GPDMACH7_BASE (LPC_AHB_BASE + 0x041E0)
-#define LPC_USB_BASE (LPC_AHB_BASE + 0x0C000)
-
-/* GPIOs */
-#define LPC_GPIO0_BASE (LPC_GPIO_BASE + 0x00000)
-#define LPC_GPIO1_BASE (LPC_GPIO_BASE + 0x00020)
-#define LPC_GPIO2_BASE (LPC_GPIO_BASE + 0x00040)
-#define LPC_GPIO3_BASE (LPC_GPIO_BASE + 0x00060)
-#define LPC_GPIO4_BASE (LPC_GPIO_BASE + 0x00080)
-
-/******************************************************************************/
-/* Peripheral declaration */
-/******************************************************************************/
-#define LPC_SC ((LPC_SC_TypeDef *) LPC_SC_BASE )
-#define LPC_GPIO0 ((LPC_GPIO_TypeDef *) LPC_GPIO0_BASE )
-#define LPC_GPIO1 ((LPC_GPIO_TypeDef *) LPC_GPIO1_BASE )
-#define LPC_GPIO2 ((LPC_GPIO_TypeDef *) LPC_GPIO2_BASE )
-#define LPC_GPIO3 ((LPC_GPIO_TypeDef *) LPC_GPIO3_BASE )
-#define LPC_GPIO4 ((LPC_GPIO_TypeDef *) LPC_GPIO4_BASE )
-#define LPC_WDT ((LPC_WDT_TypeDef *) LPC_WDT_BASE )
-#define LPC_TIM0 ((LPC_TIM_TypeDef *) LPC_TIM0_BASE )
-#define LPC_TIM1 ((LPC_TIM_TypeDef *) LPC_TIM1_BASE )
-#define LPC_TIM2 ((LPC_TIM_TypeDef *) LPC_TIM2_BASE )
-#define LPC_TIM3 ((LPC_TIM_TypeDef *) LPC_TIM3_BASE )
-#define LPC_RIT ((LPC_RIT_TypeDef *) LPC_RIT_BASE )
-#define LPC_UART0 ((LPC_UART_TypeDef *) LPC_UART0_BASE )
-#define LPC_UART1 ((LPC_UART1_TypeDef *) LPC_UART1_BASE )
-#define LPC_UART2 ((LPC_UART_TypeDef *) LPC_UART2_BASE )
-#define LPC_UART3 ((LPC_UART_TypeDef *) LPC_UART3_BASE )
-#define LPC_PWM1 ((LPC_PWM_TypeDef *) LPC_PWM1_BASE )
-#define LPC_I2C0 ((LPC_I2C_TypeDef *) LPC_I2C0_BASE )
-#define LPC_I2C1 ((LPC_I2C_TypeDef *) LPC_I2C1_BASE )
-#define LPC_I2C2 ((LPC_I2C_TypeDef *) LPC_I2C2_BASE )
-#define LPC_I2S ((LPC_I2S_TypeDef *) LPC_I2S_BASE )
-#define LPC_SPI ((LPC_SPI_TypeDef *) LPC_SPI_BASE )
-#define LPC_RTC ((LPC_RTC_TypeDef *) LPC_RTC_BASE )
-#define LPC_GPIOINT ((LPC_GPIOINT_TypeDef *) LPC_GPIOINT_BASE )
-#define LPC_PINCON ((LPC_PINCON_TypeDef *) LPC_PINCON_BASE )
-#define LPC_SSP0 ((LPC_SSP_TypeDef *) LPC_SSP0_BASE )
-#define LPC_SSP1 ((LPC_SSP_TypeDef *) LPC_SSP1_BASE )
-#define LPC_ADC ((LPC_ADC_TypeDef *) LPC_ADC_BASE )
-#define LPC_DAC ((LPC_DAC_TypeDef *) LPC_DAC_BASE )
-#define LPC_CANAF_RAM ((LPC_CANAF_RAM_TypeDef *) LPC_CANAF_RAM_BASE)
-#define LPC_CANAF ((LPC_CANAF_TypeDef *) LPC_CANAF_BASE )
-#define LPC_CANCR ((LPC_CANCR_TypeDef *) LPC_CANCR_BASE )
-#define LPC_CAN1 ((LPC_CAN_TypeDef *) LPC_CAN1_BASE )
-#define LPC_CAN2 ((LPC_CAN_TypeDef *) LPC_CAN2_BASE )
-#define LPC_MCPWM ((LPC_MCPWM_TypeDef *) LPC_MCPWM_BASE )
-#define LPC_QEI ((LPC_QEI_TypeDef *) LPC_QEI_BASE )
-#define LPC_EMAC ((LPC_EMAC_TypeDef *) LPC_EMAC_BASE )
-#define LPC_GPDMA ((LPC_GPDMA_TypeDef *) LPC_GPDMA_BASE )
-#define LPC_GPDMACH0 ((LPC_GPDMACH_TypeDef *) LPC_GPDMACH0_BASE )
-#define LPC_GPDMACH1 ((LPC_GPDMACH_TypeDef *) LPC_GPDMACH1_BASE )
-#define LPC_GPDMACH2 ((LPC_GPDMACH_TypeDef *) LPC_GPDMACH2_BASE )
-#define LPC_GPDMACH3 ((LPC_GPDMACH_TypeDef *) LPC_GPDMACH3_BASE )
-#define LPC_GPDMACH4 ((LPC_GPDMACH_TypeDef *) LPC_GPDMACH4_BASE )
-#define LPC_GPDMACH5 ((LPC_GPDMACH_TypeDef *) LPC_GPDMACH5_BASE )
-#define LPC_GPDMACH6 ((LPC_GPDMACH_TypeDef *) LPC_GPDMACH6_BASE )
-#define LPC_GPDMACH7 ((LPC_GPDMACH_TypeDef *) LPC_GPDMACH7_BASE )
-#define LPC_USB ((LPC_USB_TypeDef *) LPC_USB_BASE )
-
-/**
- * @}
- */
-
-#endif // __LPC17xx_H__
diff --git a/frameworks/CMSIS/LPC1768/include/arm_common_tables.h b/frameworks/CMSIS/LPC1768/include/arm_common_tables.h
deleted file mode 100644
index d55c46349f..0000000000
--- a/frameworks/CMSIS/LPC1768/include/arm_common_tables.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/* ----------------------------------------------------------------------
-* Copyright (C) 2010 ARM Limited. All rights reserved.
-*
-* $Date: 11. November 2010
-* $Revision: V1.0.2
-*
-* Project: CMSIS DSP Library
-* Title: arm_common_tables.h
-*
-* Description: This file has extern declaration for common tables like Bitreverse, reciprocal etc which are used across different functions
-*
-* Target Processor: Cortex-M4/Cortex-M3
-*
-* Version 1.0.2 2010/11/11
-* Documentation updated.
-*
-* Version 1.0.1 2010/10/05
-* Production release and review comments incorporated.
-*
-* Version 1.0.0 2010/09/20
-* Production release and review comments incorporated.
-* -------------------------------------------------------------------- */
-
-#ifndef _ARM_COMMON_TABLES_H
-#define _ARM_COMMON_TABLES_H
-
-#include "arm_math.h"
-
-extern uint16_t armBitRevTable[256];
-extern q15_t armRecipTableQ15[64];
-extern q31_t armRecipTableQ31[64];
-extern const q31_t realCoefAQ31[1024];
-extern const q31_t realCoefBQ31[1024];
-
-#endif /* ARM_COMMON_TABLES_H */
diff --git a/frameworks/CMSIS/LPC1768/include/arm_math.h b/frameworks/CMSIS/LPC1768/include/arm_math.h
deleted file mode 100644
index dc9acf670b..0000000000
--- a/frameworks/CMSIS/LPC1768/include/arm_math.h
+++ /dev/null
@@ -1,7064 +0,0 @@
-/* ----------------------------------------------------------------------
- * Copyright (C) 2010 ARM Limited. All rights reserved.
- *
- * $Date: 15. July 2011
- * $Revision: V1.0.10
- *
- * Project: CMSIS DSP Library
- * Title: arm_math.h
- *
- * Description: Public header file for CMSIS DSP Library
- *
- * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
- *
- * Version 1.0.10 2011/7/15
- * Big Endian support added and Merged M0 and M3/M4 Source code.
- *
- * Version 1.0.3 2010/11/29
- * Re-organized the CMSIS folders and updated documentation.
- *
- * Version 1.0.2 2010/11/11
- * Documentation updated.
- *
- * Version 1.0.1 2010/10/05
- * Production release and review comments incorporated.
- *
- * Version 1.0.0 2010/09/20
- * Production release and review comments incorporated.
- * -------------------------------------------------------------------- */
-
-/**
- \mainpage CMSIS DSP Software Library
- *
- * Introduction
- *
- * This user manual describes the CMSIS DSP software library,
- * a suite of common signal processing functions for use on Cortex-M processor based devices.
- *
- * The library is divided into a number of modules each covering a specific category:
- * - Basic math functions
- * - Fast math functions
- * - Complex math functions
- * - Filters
- * - Matrix functions
- * - Transforms
- * - Motor control functions
- * - Statistical functions
- * - Support functions
- * - Interpolation functions
- *
- * The library has separate functions for operating on 8-bit integers, 16-bit integers,
- * 32-bit integer and 32-bit floating-point values.
- *
- * Processor Support
- *
- * The library is completely written in C and is fully CMSIS compliant.
- * High performance is achieved through maximum use of Cortex-M4 intrinsics.
- *
- * The supplied library source code also builds and runs on the Cortex-M3 and Cortex-M0 processor,
- * with the DSP intrinsics being emulated through software.
- *
- *
- * Toolchain Support
- *
- * The library has been developed and tested with MDK-ARM version 4.21.
- * The library is being tested in GCC and IAR toolchains and updates on this activity will be made available shortly.
- *
- * Using the Library
- *
- * The library installer contains prebuilt versions of the libraries in the Lib folder.
- * - arm_cortexM4lf_math.lib (Little endian and Floating Point Unit on Cortex-M4)
- * - arm_cortexM4bf_math.lib (Big endian and Floating Point Unit on Cortex-M4)
- * - arm_cortexM4l_math.lib (Little endian on Cortex-M4)
- * - arm_cortexM4b_math.lib (Big endian on Cortex-M4)
- * - arm_cortexM3l_math.lib (Little endian on Cortex-M3)
- * - arm_cortexM3b_math.lib (Big endian on Cortex-M3)
- * - arm_cortexM0l_math.lib (Little endian on Cortex-M0)
- * - arm_cortexM0b_math.lib (Big endian on Cortex-M3)
- *
- * The library functions are declared in the public file arm_math.h which is placed in the Include folder.
- * Simply include this file and link the appropriate library in the application and begin calling the library functions. The Library supports single
- * public header file arm_math.h for Cortex-M4/M3/M0 with little endian and big endian. Same header file will be used for floating point unit(FPU) variants.
- * Define the appropriate pre processor MACRO ARM_MATH_CM4 or ARM_MATH_CM3 or
- * ARM_MATH_CM0 depending on the target processor in the application.
- *
- * Examples
- *
- * The library ships with a number of examples which demonstrate how to use the library functions.
- *
- * Building the Library
- *
- * The library installer contains project files to re build libraries on MDK Tool chain in the CMSIS\DSP_Lib\Source\ARM folder.
- * - arm_cortexM0b_math.uvproj
- * - arm_cortexM0l_math.uvproj
- * - arm_cortexM3b_math.uvproj
- * - arm_cortexM3l_math.uvproj
- * - arm_cortexM4b_math.uvproj
- * - arm_cortexM4l_math.uvproj
- * - arm_cortexM4bf_math.uvproj
- * - arm_cortexM4lf_math.uvproj
- *
- * Each library project have differant pre-processor macros.
- *
- * ARM_MATH_CMx:
- * Define macro ARM_MATH_CM4 for building the library on Cortex-M4 target, ARM_MATH_CM3 for building library on Cortex-M3 target
- * and ARM_MATH_CM0 for building library on cortex-M0 target.
- *
- * ARM_MATH_BIG_ENDIAN:
- * Define macro ARM_MATH_BIG_ENDIAN to build the library for big endian targets. By default library builds for little endian targets.
- *
- * ARM_MATH_MATRIX_CHECK:
- * Define macro for checking on the input and output sizes of matrices
- *
- * ARM_MATH_ROUNDING:
- * Define macro for rounding on support functions
- *
- * __FPU_PRESENT:
- * Initialize macro __FPU_PRESENT = 1 when building on FPU supported Targets. Enable this macro for M4bf and M4lf libraries
- *
- *
- * The project can be built by opening the appropriate project in MDK-ARM 4.21 chain and defining the optional pre processor MACROs detailed above.
- *
- * Copyright Notice
- *
- * Copyright (C) 2010 ARM Limited. All rights reserved.
- */
-
-
-/**
- * @ingroup DSP_Functions
- * @defgroup groupMath Basic Math Functions
- */
-
-/**
- * @ingroup DSP_Functions
- * @defgroup groupFastMath Fast Math Functions
- * This set of functions provides a fast approximation to sine, cosine, and square root.
- * As compared to most of the other functions in the CMSIS math library, the fast math functions
- * operate on individual values and not arrays.
- * There are separate functions for Q15, Q31, and floating-point data.
- *
- */
-
-/**
- * @ingroup DSP_Functions
- * @defgroup groupCmplxMath Complex Math Functions
- * This set of functions operates on complex data vectors.
- * The data in the complex arrays is stored in an interleaved fashion
- * (real, imag, real, imag, ...).
- * In the API functions, the number of samples in a complex array refers
- * to the number of complex values; the array contains twice this number of
- * real values.
- */
-
-
-/**
- * @ingroup DSP_Functions
- * @defgroup groupFilters Filtering Functions
- */
-
-/**
- * @ingroup DSP_Functions
- * @defgroup groupMatrix Matrix Functions
- *
- * This set of functions provides basic matrix math operations.
- * The functions operate on matrix data structures. For example,
- * the type
- * definition for the floating-point matrix structure is shown
- * below:
- *
- * typedef struct
- * {
- * uint16_t numRows; // number of rows of the matrix.
- * uint16_t numCols; // number of columns of the matrix.
- * float32_t *pData; // points to the data of the matrix.
- * } arm_matrix_instance_f32;
- *
- * There are similar definitions for Q15 and Q31 data types.
- *
- * The structure specifies the size of the matrix and then points to
- * an array of data. The array is of size numRows X numCols
- * and the values are arranged in row order. That is, the
- * matrix element (i, j) is stored at:
- *
- * pData[i*numCols + j]
- *
- *
- * \par Init Functions
- * There is an associated initialization function for each type of matrix
- * data structure.
- * The initialization function sets the values of the internal structure fields.
- * Refer to the function arm_mat_init_f32(), arm_mat_init_q31()
- * and arm_mat_init_q15() for floating-point, Q31 and Q15 types, respectively.
- *
- * \par
- * Use of the initialization function is optional. However, if initialization function is used
- * then the instance structure cannot be placed into a const data section.
- * To place the instance structure in a const data
- * section, manually initialize the data structure. For example:
- *
- * arm_matrix_instance_f32 S = {nRows, nColumns, pData};
- * arm_matrix_instance_q31 S = {nRows, nColumns, pData};
- * arm_matrix_instance_q15 S = {nRows, nColumns, pData};
- *
- * where nRows specifies the number of rows, nColumns
- * specifies the number of columns, and pData points to the
- * data array.
- *
- * \par Size Checking
- * By default all of the matrix functions perform size checking on the input and
- * output matrices. For example, the matrix addition function verifies that the
- * two input matrices and the output matrix all have the same number of rows and
- * columns. If the size check fails the functions return:
- *
- * ARM_MATH_SIZE_MISMATCH
- *
- * Otherwise the functions return
- *
- * ARM_MATH_SUCCESS
- *
- * There is some overhead associated with this matrix size checking.
- * The matrix size checking is enabled via the #define
- *
- * ARM_MATH_MATRIX_CHECK
- *
- * within the library project settings. By default this macro is defined
- * and size checking is enabled. By changing the project settings and
- * undefining this macro size checking is eliminated and the functions
- * run a bit faster. With size checking disabled the functions always
- * return ARM_MATH_SUCCESS.
- */
-
-/**
- * @ingroup DSP_Functions
- * @defgroup groupTransforms Transform Functions
- */
-
-/**
- * @ingroup DSP_Functions
- * @defgroup groupController Controller Functions
- */
-
-/**
- * @ingroup DSP_Functions
- * @defgroup groupStats Statistics Functions
- */
-
-/**
- * @ingroup DSP_Functions
- * @defgroup groupSupport Support Functions
- */
-
-/**
- * @ingroup DSP_Functions
- * @defgroup groupInterpolation Interpolation Functions
- * These functions perform 1- and 2-dimensional interpolation of data.
- * Linear interpolation is used for 1-dimensional data and
- * bilinear interpolation is used for 2-dimensional data.
- */
-
-/**
- * @ingroup DSP_Lib
- * @defgroup groupExamples Examples
- */
-#ifndef _ARM_MATH_H
-#define _ARM_MATH_H
-
-#define __CMSIS_GENERIC /* disable NVIC and Systick functions */
-
-#if defined (ARM_MATH_CM4)
- #include "core_cm4.h"
-#elif defined (ARM_MATH_CM3)
- #include "core_cm3.h"
-#elif defined (ARM_MATH_CM0)
- #include "core_cm0.h"
-#else
-#include "ARMCM4.h"
-#warning "Define either ARM_MATH_CM4 OR ARM_MATH_CM3...By Default building on ARM_MATH_CM4....."
-#endif
-
-#undef __CMSIS_GENERIC /* enable NVIC and Systick functions */
-#include "string.h"
- #include "math.h"
-#ifdef __cplusplus
-extern "C"
-{
-#endif
-
-
- /**
- * @brief Macros required for reciprocal calculation in Normalized LMS
- */
-
-#define DELTA_Q31 (0x100)
-#define DELTA_Q15 0x5
-#define INDEX_MASK 0x0000003F
-#define PI 3.14159265358979f
-
- /**
- * @brief Macros required for SINE and COSINE Fast math approximations
- */
-
-#define TABLE_SIZE 256
-#define TABLE_SPACING_Q31 0x800000
-#define TABLE_SPACING_Q15 0x80
-
- /**
- * @brief Macros required for SINE and COSINE Controller functions
- */
- /* 1.31(q31) Fixed value of 2/360 */
- /* -1 to +1 is divided into 360 values so total spacing is (2/360) */
-#define INPUT_SPACING 0xB60B61
-
-
- /**
- * @brief Error status returned by some functions in the library.
- */
-
- typedef enum
- {
- ARM_MATH_SUCCESS = 0, /**< No error */
- ARM_MATH_ARGUMENT_ERROR = -1, /**< One or more arguments are incorrect */
- ARM_MATH_LENGTH_ERROR = -2, /**< Length of data buffer is incorrect */
- ARM_MATH_SIZE_MISMATCH = -3, /**< Size of matrices is not compatible with the operation. */
- ARM_MATH_NANINF = -4, /**< Not-a-number (NaN) or infinity is generated */
- ARM_MATH_SINGULAR = -5, /**< Generated by matrix inversion if the input matrix is singular and cannot be inverted. */
- ARM_MATH_TEST_FAILURE = -6 /**< Test Failed */
- } arm_status;
-
- /**
- * @brief 8-bit fractional data type in 1.7 format.
- */
- typedef int8_t q7_t;
-
- /**
- * @brief 16-bit fractional data type in 1.15 format.
- */
- typedef int16_t q15_t;
-
- /**
- * @brief 32-bit fractional data type in 1.31 format.
- */
- typedef int32_t q31_t;
-
- /**
- * @brief 64-bit fractional data type in 1.63 format.
- */
- typedef int64_t q63_t;
-
- /**
- * @brief 32-bit floating-point type definition.
- */
- typedef float float32_t;
-
- /**
- * @brief 64-bit floating-point type definition.
- */
- typedef double float64_t;
-
- /**
- * @brief definition to read/write two 16 bit values.
- */
-#define __SIMD32(addr) (*(int32_t **) & (addr))
-
-#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0)
- /**
- * @brief definition to pack two 16 bit values.
- */
-#define __PKHBT(ARG1, ARG2, ARG3) ( (((int32_t)(ARG1) << 0) & (int32_t)0x0000FFFF) | \
- (((int32_t)(ARG2) << ARG3) & (int32_t)0xFFFF0000) )
-
-#endif
-
-
- /**
- * @brief definition to pack four 8 bit values.
- */
-#ifndef ARM_MATH_BIG_ENDIAN
-
-#define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v0) << 0) & (int32_t)0x000000FF) | \
- (((int32_t)(v1) << 8) & (int32_t)0x0000FF00) | \
- (((int32_t)(v2) << 16) & (int32_t)0x00FF0000) | \
- (((int32_t)(v3) << 24) & (int32_t)0xFF000000) )
-#else
-
-#define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v3) << 0) & (int32_t)0x000000FF) | \
- (((int32_t)(v2) << 8) & (int32_t)0x0000FF00) | \
- (((int32_t)(v1) << 16) & (int32_t)0x00FF0000) | \
- (((int32_t)(v0) << 24) & (int32_t)0xFF000000) )
-
-#endif
-
-
- /**
- * @brief Clips Q63 to Q31 values.
- */
- static __INLINE q31_t clip_q63_to_q31(
- q63_t x)
- {
- return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ?
- ((0x7FFFFFFF ^ ((q31_t) (x >> 63)))) : (q31_t) x;
- }
-
- /**
- * @brief Clips Q63 to Q15 values.
- */
- static __INLINE q15_t clip_q63_to_q15(
- q63_t x)
- {
- return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ?
- ((0x7FFF ^ ((q15_t) (x >> 63)))) : (q15_t) (x >> 15);
- }
-
- /**
- * @brief Clips Q31 to Q7 values.
- */
- static __INLINE q7_t clip_q31_to_q7(
- q31_t x)
- {
- return ((q31_t) (x >> 24) != ((q31_t) x >> 23)) ?
- ((0x7F ^ ((q7_t) (x >> 31)))) : (q7_t) x;
- }
-
- /**
- * @brief Clips Q31 to Q15 values.
- */
- static __INLINE q15_t clip_q31_to_q15(
- q31_t x)
- {
- return ((q31_t) (x >> 16) != ((q31_t) x >> 15)) ?
- ((0x7FFF ^ ((q15_t) (x >> 31)))) : (q15_t) x;
- }
-
- /**
- * @brief Multiplies 32 X 64 and returns 32 bit result in 2.30 format.
- */
-
- static __INLINE q63_t mult32x64(
- q63_t x,
- q31_t y)
- {
- return ((((q63_t) (x & 0x00000000FFFFFFFF) * y) >> 32) +
- (((q63_t) (x >> 32) * y)));
- }
-
-
-#if defined (ARM_MATH_CM0) && defined ( __CC_ARM )
-#define __CLZ __clz
-#endif
-
-#if defined (ARM_MATH_CM0) && ((defined (__ICCARM__)) ||(defined (__GNUC__)) || defined (__TASKING__) )
-
- static __INLINE uint32_t __CLZ(q31_t data);
-
-
- static __INLINE uint32_t __CLZ(q31_t data)
- {
- uint32_t count = 0;
- uint32_t mask = 0x80000000;
-
- while((data & mask) == 0)
- {
- count += 1u;
- mask = mask >> 1u;
- }
-
- return(count);
-
- }
-
-#endif
-
- /**
- * @brief Function to Calculates 1/in(reciprocal) value of Q31 Data type.
- */
-
- static __INLINE uint32_t arm_recip_q31(
- q31_t in,
- q31_t * dst,
- q31_t * pRecipTable)
- {
-
- uint32_t out, tempVal;
- uint32_t index, i;
- uint32_t signBits;
-
- if(in > 0)
- {
- signBits = __CLZ(in) - 1;
- }
- else
- {
- signBits = __CLZ(-in) - 1;
- }
-
- /* Convert input sample to 1.31 format */
- in = in << signBits;
-
- /* calculation of index for initial approximated Val */
- index = (uint32_t) (in >> 24u);
- index = (index & INDEX_MASK);
-
- /* 1.31 with exp 1 */
- out = pRecipTable[index];
-
- /* calculation of reciprocal value */
- /* running approximation for two iterations */
- for (i = 0u; i < 2u; i++)
- {
- tempVal = (q31_t) (((q63_t) in * out) >> 31u);
- tempVal = 0x7FFFFFFF - tempVal;
- /* 1.31 with exp 1 */
- //out = (q31_t) (((q63_t) out * tempVal) >> 30u);
- out = (q31_t) clip_q63_to_q31(((q63_t) out * tempVal) >> 30u);
- }
-
- /* write output */
- *dst = out;
-
- /* return num of signbits of out = 1/in value */
- return (signBits + 1u);
-
- }
-
- /**
- * @brief Function to Calculates 1/in(reciprocal) value of Q15 Data type.
- */
- static __INLINE uint32_t arm_recip_q15(
- q15_t in,
- q15_t * dst,
- q15_t * pRecipTable)
- {
-
- uint32_t out = 0, tempVal = 0;
- uint32_t index = 0, i = 0;
- uint32_t signBits = 0;
-
- if(in > 0)
- {
- signBits = __CLZ(in) - 17;
- }
- else
- {
- signBits = __CLZ(-in) - 17;
- }
-
- /* Convert input sample to 1.15 format */
- in = in << signBits;
-
- /* calculation of index for initial approximated Val */
- index = in >> 8;
- index = (index & INDEX_MASK);
-
- /* 1.15 with exp 1 */
- out = pRecipTable[index];
-
- /* calculation of reciprocal value */
- /* running approximation for two iterations */
- for (i = 0; i < 2; i++)
- {
- tempVal = (q15_t) (((q31_t) in * out) >> 15);
- tempVal = 0x7FFF - tempVal;
- /* 1.15 with exp 1 */
- out = (q15_t) (((q31_t) out * tempVal) >> 14);
- }
-
- /* write output */
- *dst = out;
-
- /* return num of signbits of out = 1/in value */
- return (signBits + 1);
-
- }
-
-
- /*
- * @brief C custom defined intrinisic function for only M0 processors
- */
-#if defined(ARM_MATH_CM0)
-
- static __INLINE q31_t __SSAT(
- q31_t x,
- uint32_t y)
- {
- int32_t posMax, negMin;
- uint32_t i;
-
- posMax = 1;
- for (i = 0; i < (y - 1); i++)
- {
- posMax = posMax * 2;
- }
-
- if(x > 0)
- {
- posMax = (posMax - 1);
-
- if(x > posMax)
- {
- x = posMax;
- }
- }
- else
- {
- negMin = -posMax;
-
- if(x < negMin)
- {
- x = negMin;
- }
- }
- return (x);
-
-
- }
-
-#endif /* end of ARM_MATH_CM0 */
-
-
-
- /*
- * @brief C custom defined intrinsic function for M3 and M0 processors
- */
-#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0)
-
- /*
- * @brief C custom defined QADD8 for M3 and M0 processors
- */
- static __INLINE q31_t __QADD8(
- q31_t x,
- q31_t y)
- {
-
- q31_t sum;
- q7_t r, s, t, u;
-
- r = (char) x;
- s = (char) y;
-
- r = __SSAT((q31_t) (r + s), 8);
- s = __SSAT(((q31_t) (((x << 16) >> 24) + ((y << 16) >> 24))), 8);
- t = __SSAT(((q31_t) (((x << 8) >> 24) + ((y << 8) >> 24))), 8);
- u = __SSAT(((q31_t) ((x >> 24) + (y >> 24))), 8);
-
- sum = (((q31_t) u << 24) & 0xFF000000) | (((q31_t) t << 16) & 0x00FF0000) |
- (((q31_t) s << 8) & 0x0000FF00) | (r & 0x000000FF);
-
- return sum;
-
- }
-
- /*
- * @brief C custom defined QSUB8 for M3 and M0 processors
- */
- static __INLINE q31_t __QSUB8(
- q31_t x,
- q31_t y)
- {
-
- q31_t sum;
- q31_t r, s, t, u;
-
- r = (char) x;
- s = (char) y;
-
- r = __SSAT((r - s), 8);
- s = __SSAT(((q31_t) (((x << 16) >> 24) - ((y << 16) >> 24))), 8) << 8;
- t = __SSAT(((q31_t) (((x << 8) >> 24) - ((y << 8) >> 24))), 8) << 16;
- u = __SSAT(((q31_t) ((x >> 24) - (y >> 24))), 8) << 24;
-
- sum =
- (u & 0xFF000000) | (t & 0x00FF0000) | (s & 0x0000FF00) | (r & 0x000000FF);
-
- return sum;
- }
-
- /*
- * @brief C custom defined QADD16 for M3 and M0 processors
- */
-
- /*
- * @brief C custom defined QADD16 for M3 and M0 processors
- */
- static __INLINE q31_t __QADD16(
- q31_t x,
- q31_t y)
- {
-
- q31_t sum;
- q31_t r, s;
-
- r = (short) x;
- s = (short) y;
-
- r = __SSAT(r + s, 16);
- s = __SSAT(((q31_t) ((x >> 16) + (y >> 16))), 16) << 16;
-
- sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
-
- return sum;
-
- }
-
- /*
- * @brief C custom defined SHADD16 for M3 and M0 processors
- */
- static __INLINE q31_t __SHADD16(
- q31_t x,
- q31_t y)
- {
-
- q31_t sum;
- q31_t r, s;
-
- r = (short) x;
- s = (short) y;
-
- r = ((r >> 1) + (s >> 1));
- s = ((q31_t) ((x >> 17) + (y >> 17))) << 16;
-
- sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
-
- return sum;
-
- }
-
- /*
- * @brief C custom defined QSUB16 for M3 and M0 processors
- */
- static __INLINE q31_t __QSUB16(
- q31_t x,
- q31_t y)
- {
-
- q31_t sum;
- q31_t r, s;
-
- r = (short) x;
- s = (short) y;
-
- r = __SSAT(r - s, 16);
- s = __SSAT(((q31_t) ((x >> 16) - (y >> 16))), 16) << 16;
-
- sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
-
- return sum;
- }
-
- /*
- * @brief C custom defined SHSUB16 for M3 and M0 processors
- */
- static __INLINE q31_t __SHSUB16(
- q31_t x,
- q31_t y)
- {
-
- q31_t diff;
- q31_t r, s;
-
- r = (short) x;
- s = (short) y;
-
- r = ((r >> 1) - (s >> 1));
- s = (((x >> 17) - (y >> 17)) << 16);
-
- diff = (s & 0xFFFF0000) | (r & 0x0000FFFF);
-
- return diff;
- }
-
- /*
- * @brief C custom defined QASX for M3 and M0 processors
- */
- static __INLINE q31_t __QASX(
- q31_t x,
- q31_t y)
- {
-
- q31_t sum = 0;
-
- sum = ((sum + clip_q31_to_q15((q31_t) ((short) (x >> 16) + (short) y))) << 16) +
- clip_q31_to_q15((q31_t) ((short) x - (short) (y >> 16)));
-
- return sum;
- }
-
- /*
- * @brief C custom defined SHASX for M3 and M0 processors
- */
- static __INLINE q31_t __SHASX(
- q31_t x,
- q31_t y)
- {
-
- q31_t sum;
- q31_t r, s;
-
- r = (short) x;
- s = (short) y;
-
- r = ((r >> 1) - (y >> 17));
- s = (((x >> 17) + (s >> 1)) << 16);
-
- sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
-
- return sum;
- }
-
-
- /*
- * @brief C custom defined QSAX for M3 and M0 processors
- */
- static __INLINE q31_t __QSAX(
- q31_t x,
- q31_t y)
- {
-
- q31_t sum = 0;
-
- sum = ((sum + clip_q31_to_q15((q31_t) ((short) (x >> 16) - (short) y))) << 16) +
- clip_q31_to_q15((q31_t) ((short) x + (short) (y >> 16)));
-
- return sum;
- }
-
- /*
- * @brief C custom defined SHSAX for M3 and M0 processors
- */
- static __INLINE q31_t __SHSAX(
- q31_t x,
- q31_t y)
- {
-
- q31_t sum;
- q31_t r, s;
-
- r = (short) x;
- s = (short) y;
-
- r = ((r >> 1) + (y >> 17));
- s = (((x >> 17) - (s >> 1)) << 16);
-
- sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
-
- return sum;
- }
-
- /*
- * @brief C custom defined SMUSDX for M3 and M0 processors
- */
- static __INLINE q31_t __SMUSDX(
- q31_t x,
- q31_t y)
- {
-
- return ((q31_t)(((short) x * (short) (y >> 16)) -
- ((short) (x >> 16) * (short) y)));
- }
-
- /*
- * @brief C custom defined SMUADX for M3 and M0 processors
- */
- static __INLINE q31_t __SMUADX(
- q31_t x,
- q31_t y)
- {
-
- return ((q31_t)(((short) x * (short) (y >> 16)) +
- ((short) (x >> 16) * (short) y)));
- }
-
- /*
- * @brief C custom defined QADD for M3 and M0 processors
- */
- static __INLINE q31_t __QADD(
- q31_t x,
- q31_t y)
- {
- return clip_q63_to_q31((q63_t) x + y);
- }
-
- /*
- * @brief C custom defined QSUB for M3 and M0 processors
- */
- static __INLINE q31_t __QSUB(
- q31_t x,
- q31_t y)
- {
- return clip_q63_to_q31((q63_t) x - y);
- }
-
- /*
- * @brief C custom defined SMLAD for M3 and M0 processors
- */
- static __INLINE q31_t __SMLAD(
- q31_t x,
- q31_t y,
- q31_t sum)
- {
-
- return (sum + ((short) (x >> 16) * (short) (y >> 16)) +
- ((short) x * (short) y));
- }
-
- /*
- * @brief C custom defined SMLADX for M3 and M0 processors
- */
- static __INLINE q31_t __SMLADX(
- q31_t x,
- q31_t y,
- q31_t sum)
- {
-
- return (sum + ((short) (x >> 16) * (short) (y)) +
- ((short) x * (short) (y >> 16)));
- }
-
- /*
- * @brief C custom defined SMLSDX for M3 and M0 processors
- */
- static __INLINE q31_t __SMLSDX(
- q31_t x,
- q31_t y,
- q31_t sum)
- {
-
- return (sum - ((short) (x >> 16) * (short) (y)) +
- ((short) x * (short) (y >> 16)));
- }
-
- /*
- * @brief C custom defined SMLALD for M3 and M0 processors
- */
- static __INLINE q63_t __SMLALD(
- q31_t x,
- q31_t y,
- q63_t sum)
- {
-
- return (sum + ((short) (x >> 16) * (short) (y >> 16)) +
- ((short) x * (short) y));
- }
-
- /*
- * @brief C custom defined SMLALDX for M3 and M0 processors
- */
- static __INLINE q63_t __SMLALDX(
- q31_t x,
- q31_t y,
- q63_t sum)
- {
-
- return (sum + ((short) (x >> 16) * (short) y)) +
- ((short) x * (short) (y >> 16));
- }
-
- /*
- * @brief C custom defined SMUAD for M3 and M0 processors
- */
- static __INLINE q31_t __SMUAD(
- q31_t x,
- q31_t y)
- {
-
- return (((x >> 16) * (y >> 16)) +
- (((x << 16) >> 16) * ((y << 16) >> 16)));
- }
-
- /*
- * @brief C custom defined SMUSD for M3 and M0 processors
- */
- static __INLINE q31_t __SMUSD(
- q31_t x,
- q31_t y)
- {
-
- return (-((x >> 16) * (y >> 16)) +
- (((x << 16) >> 16) * ((y << 16) >> 16)));
- }
-
-
-
-
-#endif /* (ARM_MATH_CM3) || defined (ARM_MATH_CM0) */
-
-
- /**
- * @brief Instance structure for the Q7 FIR filter.
- */
- typedef struct
- {
- uint16_t numTaps; /**< number of filter coefficients in the filter. */
- q7_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- q7_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
- } arm_fir_instance_q7;
-
- /**
- * @brief Instance structure for the Q15 FIR filter.
- */
- typedef struct
- {
- uint16_t numTaps; /**< number of filter coefficients in the filter. */
- q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
- } arm_fir_instance_q15;
-
- /**
- * @brief Instance structure for the Q31 FIR filter.
- */
- typedef struct
- {
- uint16_t numTaps; /**< number of filter coefficients in the filter. */
- q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
- } arm_fir_instance_q31;
-
- /**
- * @brief Instance structure for the floating-point FIR filter.
- */
- typedef struct
- {
- uint16_t numTaps; /**< number of filter coefficients in the filter. */
- float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
- } arm_fir_instance_f32;
-
-
- /**
- * @brief Processing function for the Q7 FIR filter.
- * @param[in] *S points to an instance of the Q7 FIR filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
- void arm_fir_q7(
- const arm_fir_instance_q7 * S,
- q7_t * pSrc,
- q7_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @brief Initialization function for the Q7 FIR filter.
- * @param[in,out] *S points to an instance of the Q7 FIR structure.
- * @param[in] numTaps Number of filter coefficients in the filter.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of samples that are processed.
- * @return none
- */
- void arm_fir_init_q7(
- arm_fir_instance_q7 * S,
- uint16_t numTaps,
- q7_t * pCoeffs,
- q7_t * pState,
- uint32_t blockSize);
-
-
- /**
- * @brief Processing function for the Q15 FIR filter.
- * @param[in] *S points to an instance of the Q15 FIR structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
- void arm_fir_q15(
- const arm_fir_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the fast Q15 FIR filter for Cortex-M3 and Cortex-M4.
- * @param[in] *S points to an instance of the Q15 FIR filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
- void arm_fir_fast_q15(
- const arm_fir_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the Q15 FIR filter.
- * @param[in,out] *S points to an instance of the Q15 FIR filter structure.
- * @param[in] numTaps Number of filter coefficients in the filter. Must be even and greater than or equal to 4.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of samples that are processed at a time.
- * @return The function returns ARM_MATH_SUCCESS if initialization was successful or ARM_MATH_ARGUMENT_ERROR if
- * numTaps is not a supported value.
- */
-
- arm_status arm_fir_init_q15(
- arm_fir_instance_q15 * S,
- uint16_t numTaps,
- q15_t * pCoeffs,
- q15_t * pState,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the Q31 FIR filter.
- * @param[in] *S points to an instance of the Q31 FIR filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
- void arm_fir_q31(
- const arm_fir_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the fast Q31 FIR filter for Cortex-M3 and Cortex-M4.
- * @param[in] *S points to an instance of the Q31 FIR structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
- void arm_fir_fast_q31(
- const arm_fir_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the Q31 FIR filter.
- * @param[in,out] *S points to an instance of the Q31 FIR structure.
- * @param[in] numTaps Number of filter coefficients in the filter.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of samples that are processed at a time.
- * @return none.
- */
- void arm_fir_init_q31(
- arm_fir_instance_q31 * S,
- uint16_t numTaps,
- q31_t * pCoeffs,
- q31_t * pState,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the floating-point FIR filter.
- * @param[in] *S points to an instance of the floating-point FIR structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
- void arm_fir_f32(
- const arm_fir_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the floating-point FIR filter.
- * @param[in,out] *S points to an instance of the floating-point FIR filter structure.
- * @param[in] numTaps Number of filter coefficients in the filter.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of samples that are processed at a time.
- * @return none.
- */
- void arm_fir_init_f32(
- arm_fir_instance_f32 * S,
- uint16_t numTaps,
- float32_t * pCoeffs,
- float32_t * pState,
- uint32_t blockSize);
-
-
- /**
- * @brief Instance structure for the Q15 Biquad cascade filter.
- */
- typedef struct
- {
- int8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */
- q15_t *pState; /**< Points to the array of state coefficients. The array is of length 4*numStages. */
- q15_t *pCoeffs; /**< Points to the array of coefficients. The array is of length 5*numStages. */
- int8_t postShift; /**< Additional shift, in bits, applied to each output sample. */
-
- } arm_biquad_casd_df1_inst_q15;
-
-
- /**
- * @brief Instance structure for the Q31 Biquad cascade filter.
- */
- typedef struct
- {
- uint32_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */
- q31_t *pState; /**< Points to the array of state coefficients. The array is of length 4*numStages. */
- q31_t *pCoeffs; /**< Points to the array of coefficients. The array is of length 5*numStages. */
- uint8_t postShift; /**< Additional shift, in bits, applied to each output sample. */
-
- } arm_biquad_casd_df1_inst_q31;
-
- /**
- * @brief Instance structure for the floating-point Biquad cascade filter.
- */
- typedef struct
- {
- uint32_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */
- float32_t *pState; /**< Points to the array of state coefficients. The array is of length 4*numStages. */
- float32_t *pCoeffs; /**< Points to the array of coefficients. The array is of length 5*numStages. */
-
-
- } arm_biquad_casd_df1_inst_f32;
-
-
-
- /**
- * @brief Processing function for the Q15 Biquad cascade filter.
- * @param[in] *S points to an instance of the Q15 Biquad cascade structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_biquad_cascade_df1_q15(
- const arm_biquad_casd_df1_inst_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the Q15 Biquad cascade filter.
- * @param[in,out] *S points to an instance of the Q15 Biquad cascade structure.
- * @param[in] numStages number of 2nd order stages in the filter.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] postShift Shift to be applied to the output. Varies according to the coefficients format
- * @return none
- */
-
- void arm_biquad_cascade_df1_init_q15(
- arm_biquad_casd_df1_inst_q15 * S,
- uint8_t numStages,
- q15_t * pCoeffs,
- q15_t * pState,
- int8_t postShift);
-
-
- /**
- * @brief Fast but less precise processing function for the Q15 Biquad cascade filter for Cortex-M3 and Cortex-M4.
- * @param[in] *S points to an instance of the Q15 Biquad cascade structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_biquad_cascade_df1_fast_q15(
- const arm_biquad_casd_df1_inst_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @brief Processing function for the Q31 Biquad cascade filter
- * @param[in] *S points to an instance of the Q31 Biquad cascade structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_biquad_cascade_df1_q31(
- const arm_biquad_casd_df1_inst_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Fast but less precise processing function for the Q31 Biquad cascade filter for Cortex-M3 and Cortex-M4.
- * @param[in] *S points to an instance of the Q31 Biquad cascade structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_biquad_cascade_df1_fast_q31(
- const arm_biquad_casd_df1_inst_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the Q31 Biquad cascade filter.
- * @param[in,out] *S points to an instance of the Q31 Biquad cascade structure.
- * @param[in] numStages number of 2nd order stages in the filter.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] postShift Shift to be applied to the output. Varies according to the coefficients format
- * @return none
- */
-
- void arm_biquad_cascade_df1_init_q31(
- arm_biquad_casd_df1_inst_q31 * S,
- uint8_t numStages,
- q31_t * pCoeffs,
- q31_t * pState,
- int8_t postShift);
-
- /**
- * @brief Processing function for the floating-point Biquad cascade filter.
- * @param[in] *S points to an instance of the floating-point Biquad cascade structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_biquad_cascade_df1_f32(
- const arm_biquad_casd_df1_inst_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the floating-point Biquad cascade filter.
- * @param[in,out] *S points to an instance of the floating-point Biquad cascade structure.
- * @param[in] numStages number of 2nd order stages in the filter.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @return none
- */
-
- void arm_biquad_cascade_df1_init_f32(
- arm_biquad_casd_df1_inst_f32 * S,
- uint8_t numStages,
- float32_t * pCoeffs,
- float32_t * pState);
-
-
- /**
- * @brief Instance structure for the floating-point matrix structure.
- */
-
- typedef struct
- {
- uint16_t numRows; /**< number of rows of the matrix. */
- uint16_t numCols; /**< number of columns of the matrix. */
- float32_t *pData; /**< points to the data of the matrix. */
- } arm_matrix_instance_f32;
-
- /**
- * @brief Instance structure for the Q15 matrix structure.
- */
-
- typedef struct
- {
- uint16_t numRows; /**< number of rows of the matrix. */
- uint16_t numCols; /**< number of columns of the matrix. */
- q15_t *pData; /**< points to the data of the matrix. */
-
- } arm_matrix_instance_q15;
-
- /**
- * @brief Instance structure for the Q31 matrix structure.
- */
-
- typedef struct
- {
- uint16_t numRows; /**< number of rows of the matrix. */
- uint16_t numCols; /**< number of columns of the matrix. */
- q31_t *pData; /**< points to the data of the matrix. */
-
- } arm_matrix_instance_q31;
-
-
-
- /**
- * @brief Floating-point matrix addition.
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking.
- */
-
- arm_status arm_mat_add_f32(
- const arm_matrix_instance_f32 * pSrcA,
- const arm_matrix_instance_f32 * pSrcB,
- arm_matrix_instance_f32 * pDst);
-
- /**
- * @brief Q15 matrix addition.
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking.
- */
-
- arm_status arm_mat_add_q15(
- const arm_matrix_instance_q15 * pSrcA,
- const arm_matrix_instance_q15 * pSrcB,
- arm_matrix_instance_q15 * pDst);
-
- /**
- * @brief Q31 matrix addition.
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking.
- */
-
- arm_status arm_mat_add_q31(
- const arm_matrix_instance_q31 * pSrcA,
- const arm_matrix_instance_q31 * pSrcB,
- arm_matrix_instance_q31 * pDst);
-
-
- /**
- * @brief Floating-point matrix transpose.
- * @param[in] *pSrc points to the input matrix
- * @param[out] *pDst points to the output matrix
- * @return The function returns either ARM_MATH_SIZE_MISMATCH
- * or ARM_MATH_SUCCESS based on the outcome of size checking.
- */
-
- arm_status arm_mat_trans_f32(
- const arm_matrix_instance_f32 * pSrc,
- arm_matrix_instance_f32 * pDst);
-
-
- /**
- * @brief Q15 matrix transpose.
- * @param[in] *pSrc points to the input matrix
- * @param[out] *pDst points to the output matrix
- * @return The function returns either ARM_MATH_SIZE_MISMATCH
- * or ARM_MATH_SUCCESS based on the outcome of size checking.
- */
-
- arm_status arm_mat_trans_q15(
- const arm_matrix_instance_q15 * pSrc,
- arm_matrix_instance_q15 * pDst);
-
- /**
- * @brief Q31 matrix transpose.
- * @param[in] *pSrc points to the input matrix
- * @param[out] *pDst points to the output matrix
- * @return The function returns either ARM_MATH_SIZE_MISMATCH
- * or ARM_MATH_SUCCESS based on the outcome of size checking.
- */
-
- arm_status arm_mat_trans_q31(
- const arm_matrix_instance_q31 * pSrc,
- arm_matrix_instance_q31 * pDst);
-
-
- /**
- * @brief Floating-point matrix multiplication
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking.
- */
-
- arm_status arm_mat_mult_f32(
- const arm_matrix_instance_f32 * pSrcA,
- const arm_matrix_instance_f32 * pSrcB,
- arm_matrix_instance_f32 * pDst);
-
- /**
- * @brief Q15 matrix multiplication
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking.
- */
-
- arm_status arm_mat_mult_q15(
- const arm_matrix_instance_q15 * pSrcA,
- const arm_matrix_instance_q15 * pSrcB,
- arm_matrix_instance_q15 * pDst,
- q15_t * pState);
-
- /**
- * @brief Q15 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @param[in] *pState points to the array for storing intermediate results
- * @return The function returns either
- * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking.
- */
-
- arm_status arm_mat_mult_fast_q15(
- const arm_matrix_instance_q15 * pSrcA,
- const arm_matrix_instance_q15 * pSrcB,
- arm_matrix_instance_q15 * pDst,
- q15_t * pState);
-
- /**
- * @brief Q31 matrix multiplication
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking.
- */
-
- arm_status arm_mat_mult_q31(
- const arm_matrix_instance_q31 * pSrcA,
- const arm_matrix_instance_q31 * pSrcB,
- arm_matrix_instance_q31 * pDst);
-
- /**
- * @brief Q31 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking.
- */
-
- arm_status arm_mat_mult_fast_q31(
- const arm_matrix_instance_q31 * pSrcA,
- const arm_matrix_instance_q31 * pSrcB,
- arm_matrix_instance_q31 * pDst);
-
-
- /**
- * @brief Floating-point matrix subtraction
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking.
- */
-
- arm_status arm_mat_sub_f32(
- const arm_matrix_instance_f32 * pSrcA,
- const arm_matrix_instance_f32 * pSrcB,
- arm_matrix_instance_f32 * pDst);
-
- /**
- * @brief Q15 matrix subtraction
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking.
- */
-
- arm_status arm_mat_sub_q15(
- const arm_matrix_instance_q15 * pSrcA,
- const arm_matrix_instance_q15 * pSrcB,
- arm_matrix_instance_q15 * pDst);
-
- /**
- * @brief Q31 matrix subtraction
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking.
- */
-
- arm_status arm_mat_sub_q31(
- const arm_matrix_instance_q31 * pSrcA,
- const arm_matrix_instance_q31 * pSrcB,
- arm_matrix_instance_q31 * pDst);
-
- /**
- * @brief Floating-point matrix scaling.
- * @param[in] *pSrc points to the input matrix
- * @param[in] scale scale factor
- * @param[out] *pDst points to the output matrix
- * @return The function returns either
- * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking.
- */
-
- arm_status arm_mat_scale_f32(
- const arm_matrix_instance_f32 * pSrc,
- float32_t scale,
- arm_matrix_instance_f32 * pDst);
-
- /**
- * @brief Q15 matrix scaling.
- * @param[in] *pSrc points to input matrix
- * @param[in] scaleFract fractional portion of the scale factor
- * @param[in] shift number of bits to shift the result by
- * @param[out] *pDst points to output matrix
- * @return The function returns either
- * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking.
- */
-
- arm_status arm_mat_scale_q15(
- const arm_matrix_instance_q15 * pSrc,
- q15_t scaleFract,
- int32_t shift,
- arm_matrix_instance_q15 * pDst);
-
- /**
- * @brief Q31 matrix scaling.
- * @param[in] *pSrc points to input matrix
- * @param[in] scaleFract fractional portion of the scale factor
- * @param[in] shift number of bits to shift the result by
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking.
- */
-
- arm_status arm_mat_scale_q31(
- const arm_matrix_instance_q31 * pSrc,
- q31_t scaleFract,
- int32_t shift,
- arm_matrix_instance_q31 * pDst);
-
-
- /**
- * @brief Q31 matrix initialization.
- * @param[in,out] *S points to an instance of the floating-point matrix structure.
- * @param[in] nRows number of rows in the matrix.
- * @param[in] nColumns number of columns in the matrix.
- * @param[in] *pData points to the matrix data array.
- * @return none
- */
-
- void arm_mat_init_q31(
- arm_matrix_instance_q31 * S,
- uint16_t nRows,
- uint16_t nColumns,
- q31_t *pData);
-
- /**
- * @brief Q15 matrix initialization.
- * @param[in,out] *S points to an instance of the floating-point matrix structure.
- * @param[in] nRows number of rows in the matrix.
- * @param[in] nColumns number of columns in the matrix.
- * @param[in] *pData points to the matrix data array.
- * @return none
- */
-
- void arm_mat_init_q15(
- arm_matrix_instance_q15 * S,
- uint16_t nRows,
- uint16_t nColumns,
- q15_t *pData);
-
- /**
- * @brief Floating-point matrix initialization.
- * @param[in,out] *S points to an instance of the floating-point matrix structure.
- * @param[in] nRows number of rows in the matrix.
- * @param[in] nColumns number of columns in the matrix.
- * @param[in] *pData points to the matrix data array.
- * @return none
- */
-
- void arm_mat_init_f32(
- arm_matrix_instance_f32 * S,
- uint16_t nRows,
- uint16_t nColumns,
- float32_t *pData);
-
-
-
- /**
- * @brief Instance structure for the Q15 PID Control.
- */
- typedef struct
- {
- q15_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */
- #ifdef ARM_MATH_CM0
- q15_t A1;
- q15_t A2;
- #else
- q31_t A1; /**< The derived gain A1 = -Kp - 2Kd | Kd.*/
- #endif
- q15_t state[3]; /**< The state array of length 3. */
- q15_t Kp; /**< The proportional gain. */
- q15_t Ki; /**< The integral gain. */
- q15_t Kd; /**< The derivative gain. */
- } arm_pid_instance_q15;
-
- /**
- * @brief Instance structure for the Q31 PID Control.
- */
- typedef struct
- {
- q31_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */
- q31_t A1; /**< The derived gain, A1 = -Kp - 2Kd. */
- q31_t A2; /**< The derived gain, A2 = Kd . */
- q31_t state[3]; /**< The state array of length 3. */
- q31_t Kp; /**< The proportional gain. */
- q31_t Ki; /**< The integral gain. */
- q31_t Kd; /**< The derivative gain. */
-
- } arm_pid_instance_q31;
-
- /**
- * @brief Instance structure for the floating-point PID Control.
- */
- typedef struct
- {
- float32_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */
- float32_t A1; /**< The derived gain, A1 = -Kp - 2Kd. */
- float32_t A2; /**< The derived gain, A2 = Kd . */
- float32_t state[3]; /**< The state array of length 3. */
- float32_t Kp; /**< The proportional gain. */
- float32_t Ki; /**< The integral gain. */
- float32_t Kd; /**< The derivative gain. */
- } arm_pid_instance_f32;
-
-
-
- /**
- * @brief Initialization function for the floating-point PID Control.
- * @param[in,out] *S points to an instance of the PID structure.
- * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state.
- * @return none.
- */
- void arm_pid_init_f32(
- arm_pid_instance_f32 * S,
- int32_t resetStateFlag);
-
- /**
- * @brief Reset function for the floating-point PID Control.
- * @param[in,out] *S is an instance of the floating-point PID Control structure
- * @return none
- */
- void arm_pid_reset_f32(
- arm_pid_instance_f32 * S);
-
-
- /**
- * @brief Initialization function for the Q31 PID Control.
- * @param[in,out] *S points to an instance of the Q15 PID structure.
- * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state.
- * @return none.
- */
- void arm_pid_init_q31(
- arm_pid_instance_q31 * S,
- int32_t resetStateFlag);
-
-
- /**
- * @brief Reset function for the Q31 PID Control.
- * @param[in,out] *S points to an instance of the Q31 PID Control structure
- * @return none
- */
-
- void arm_pid_reset_q31(
- arm_pid_instance_q31 * S);
-
- /**
- * @brief Initialization function for the Q15 PID Control.
- * @param[in,out] *S points to an instance of the Q15 PID structure.
- * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state.
- * @return none.
- */
- void arm_pid_init_q15(
- arm_pid_instance_q15 * S,
- int32_t resetStateFlag);
-
- /**
- * @brief Reset function for the Q15 PID Control.
- * @param[in,out] *S points to an instance of the q15 PID Control structure
- * @return none
- */
- void arm_pid_reset_q15(
- arm_pid_instance_q15 * S);
-
-
- /**
- * @brief Instance structure for the floating-point Linear Interpolate function.
- */
- typedef struct
- {
- uint32_t nValues;
- float32_t x1;
- float32_t xSpacing;
- float32_t *pYData; /**< pointer to the table of Y values */
- } arm_linear_interp_instance_f32;
-
- /**
- * @brief Instance structure for the floating-point bilinear interpolation function.
- */
-
- typedef struct
- {
- uint16_t numRows; /**< number of rows in the data table. */
- uint16_t numCols; /**< number of columns in the data table. */
- float32_t *pData; /**< points to the data table. */
- } arm_bilinear_interp_instance_f32;
-
- /**
- * @brief Instance structure for the Q31 bilinear interpolation function.
- */
-
- typedef struct
- {
- uint16_t numRows; /**< number of rows in the data table. */
- uint16_t numCols; /**< number of columns in the data table. */
- q31_t *pData; /**< points to the data table. */
- } arm_bilinear_interp_instance_q31;
-
- /**
- * @brief Instance structure for the Q15 bilinear interpolation function.
- */
-
- typedef struct
- {
- uint16_t numRows; /**< number of rows in the data table. */
- uint16_t numCols; /**< number of columns in the data table. */
- q15_t *pData; /**< points to the data table. */
- } arm_bilinear_interp_instance_q15;
-
- /**
- * @brief Instance structure for the Q15 bilinear interpolation function.
- */
-
- typedef struct
- {
- uint16_t numRows; /**< number of rows in the data table. */
- uint16_t numCols; /**< number of columns in the data table. */
- q7_t *pData; /**< points to the data table. */
- } arm_bilinear_interp_instance_q7;
-
-
- /**
- * @brief Q7 vector multiplication.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_mult_q7(
- q7_t * pSrcA,
- q7_t * pSrcB,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q15 vector multiplication.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_mult_q15(
- q15_t * pSrcA,
- q15_t * pSrcB,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q31 vector multiplication.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_mult_q31(
- q31_t * pSrcA,
- q31_t * pSrcB,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Floating-point vector multiplication.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_mult_f32(
- float32_t * pSrcA,
- float32_t * pSrcB,
- float32_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @brief Instance structure for the Q15 CFFT/CIFFT function.
- */
-
- typedef struct
- {
- uint16_t fftLen; /**< length of the FFT. */
- uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
- uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
- q15_t *pTwiddle; /**< points to the twiddle factor table. */
- uint16_t *pBitRevTable; /**< points to the bit reversal table. */
- uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
- uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
- } arm_cfft_radix4_instance_q15;
-
- /**
- * @brief Instance structure for the Q31 CFFT/CIFFT function.
- */
-
- typedef struct
- {
- uint16_t fftLen; /**< length of the FFT. */
- uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
- uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
- q31_t *pTwiddle; /**< points to the twiddle factor table. */
- uint16_t *pBitRevTable; /**< points to the bit reversal table. */
- uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
- uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
- } arm_cfft_radix4_instance_q31;
-
- /**
- * @brief Instance structure for the floating-point CFFT/CIFFT function.
- */
-
- typedef struct
- {
- uint16_t fftLen; /**< length of the FFT. */
- uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
- uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
- float32_t *pTwiddle; /**< points to the twiddle factor table. */
- uint16_t *pBitRevTable; /**< points to the bit reversal table. */
- uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
- uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
- float32_t onebyfftLen; /**< value of 1/fftLen. */
- } arm_cfft_radix4_instance_f32;
-
- /**
- * @brief Processing function for the Q15 CFFT/CIFFT.
- * @param[in] *S points to an instance of the Q15 CFFT/CIFFT structure.
- * @param[in, out] *pSrc points to the complex data buffer. Processing occurs in-place.
- * @return none.
- */
-
- void arm_cfft_radix4_q15(
- const arm_cfft_radix4_instance_q15 * S,
- q15_t * pSrc);
-
- /**
- * @brief Initialization function for the Q15 CFFT/CIFFT.
- * @param[in,out] *S points to an instance of the Q15 CFFT/CIFFT structure.
- * @param[in] fftLen length of the FFT.
- * @param[in] ifftFlag flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform.
- * @param[in] bitReverseFlag flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output.
- * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if fftLen is not a supported value.
- */
-
- arm_status arm_cfft_radix4_init_q15(
- arm_cfft_radix4_instance_q15 * S,
- uint16_t fftLen,
- uint8_t ifftFlag,
- uint8_t bitReverseFlag);
-
- /**
- * @brief Processing function for the Q31 CFFT/CIFFT.
- * @param[in] *S points to an instance of the Q31 CFFT/CIFFT structure.
- * @param[in, out] *pSrc points to the complex data buffer. Processing occurs in-place.
- * @return none.
- */
-
- void arm_cfft_radix4_q31(
- const arm_cfft_radix4_instance_q31 * S,
- q31_t * pSrc);
-
- /**
- * @brief Initialization function for the Q31 CFFT/CIFFT.
- * @param[in,out] *S points to an instance of the Q31 CFFT/CIFFT structure.
- * @param[in] fftLen length of the FFT.
- * @param[in] ifftFlag flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform.
- * @param[in] bitReverseFlag flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output.
- * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if fftLen is not a supported value.
- */
-
- arm_status arm_cfft_radix4_init_q31(
- arm_cfft_radix4_instance_q31 * S,
- uint16_t fftLen,
- uint8_t ifftFlag,
- uint8_t bitReverseFlag);
-
- /**
- * @brief Processing function for the floating-point CFFT/CIFFT.
- * @param[in] *S points to an instance of the floating-point CFFT/CIFFT structure.
- * @param[in, out] *pSrc points to the complex data buffer. Processing occurs in-place.
- * @return none.
- */
-
- void arm_cfft_radix4_f32(
- const arm_cfft_radix4_instance_f32 * S,
- float32_t * pSrc);
-
- /**
- * @brief Initialization function for the floating-point CFFT/CIFFT.
- * @param[in,out] *S points to an instance of the floating-point CFFT/CIFFT structure.
- * @param[in] fftLen length of the FFT.
- * @param[in] ifftFlag flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform.
- * @param[in] bitReverseFlag flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output.
- * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if fftLen is not a supported value.
- */
-
- arm_status arm_cfft_radix4_init_f32(
- arm_cfft_radix4_instance_f32 * S,
- uint16_t fftLen,
- uint8_t ifftFlag,
- uint8_t bitReverseFlag);
-
-
-
- /*----------------------------------------------------------------------
- * Internal functions prototypes FFT function
- ----------------------------------------------------------------------*/
-
- /**
- * @brief Core function for the floating-point CFFT butterfly process.
- * @param[in, out] *pSrc points to the in-place buffer of floating-point data type.
- * @param[in] fftLen length of the FFT.
- * @param[in] *pCoef points to the twiddle coefficient buffer.
- * @param[in] twidCoefModifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
- * @return none.
- */
-
- void arm_radix4_butterfly_f32(
- float32_t * pSrc,
- uint16_t fftLen,
- float32_t * pCoef,
- uint16_t twidCoefModifier);
-
- /**
- * @brief Core function for the floating-point CIFFT butterfly process.
- * @param[in, out] *pSrc points to the in-place buffer of floating-point data type.
- * @param[in] fftLen length of the FFT.
- * @param[in] *pCoef points to twiddle coefficient buffer.
- * @param[in] twidCoefModifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
- * @param[in] onebyfftLen value of 1/fftLen.
- * @return none.
- */
-
- void arm_radix4_butterfly_inverse_f32(
- float32_t * pSrc,
- uint16_t fftLen,
- float32_t * pCoef,
- uint16_t twidCoefModifier,
- float32_t onebyfftLen);
-
- /**
- * @brief In-place bit reversal function.
- * @param[in, out] *pSrc points to the in-place buffer of floating-point data type.
- * @param[in] fftSize length of the FFT.
- * @param[in] bitRevFactor bit reversal modifier that supports different size FFTs with the same bit reversal table.
- * @param[in] *pBitRevTab points to the bit reversal table.
- * @return none.
- */
-
- void arm_bitreversal_f32(
- float32_t *pSrc,
- uint16_t fftSize,
- uint16_t bitRevFactor,
- uint16_t *pBitRevTab);
-
- /**
- * @brief Core function for the Q31 CFFT butterfly process.
- * @param[in, out] *pSrc points to the in-place buffer of Q31 data type.
- * @param[in] fftLen length of the FFT.
- * @param[in] *pCoef points to twiddle coefficient buffer.
- * @param[in] twidCoefModifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
- * @return none.
- */
-
- void arm_radix4_butterfly_q31(
- q31_t *pSrc,
- uint32_t fftLen,
- q31_t *pCoef,
- uint32_t twidCoefModifier);
-
- /**
- * @brief Core function for the Q31 CIFFT butterfly process.
- * @param[in, out] *pSrc points to the in-place buffer of Q31 data type.
- * @param[in] fftLen length of the FFT.
- * @param[in] *pCoef points to twiddle coefficient buffer.
- * @param[in] twidCoefModifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
- * @return none.
- */
-
- void arm_radix4_butterfly_inverse_q31(
- q31_t * pSrc,
- uint32_t fftLen,
- q31_t * pCoef,
- uint32_t twidCoefModifier);
-
- /**
- * @brief In-place bit reversal function.
- * @param[in, out] *pSrc points to the in-place buffer of Q31 data type.
- * @param[in] fftLen length of the FFT.
- * @param[in] bitRevFactor bit reversal modifier that supports different size FFTs with the same bit reversal table
- * @param[in] *pBitRevTab points to bit reversal table.
- * @return none.
- */
-
- void arm_bitreversal_q31(
- q31_t * pSrc,
- uint32_t fftLen,
- uint16_t bitRevFactor,
- uint16_t *pBitRevTab);
-
- /**
- * @brief Core function for the Q15 CFFT butterfly process.
- * @param[in, out] *pSrc16 points to the in-place buffer of Q15 data type.
- * @param[in] fftLen length of the FFT.
- * @param[in] *pCoef16 points to twiddle coefficient buffer.
- * @param[in] twidCoefModifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
- * @return none.
- */
-
- void arm_radix4_butterfly_q15(
- q15_t *pSrc16,
- uint32_t fftLen,
- q15_t *pCoef16,
- uint32_t twidCoefModifier);
-
- /**
- * @brief Core function for the Q15 CIFFT butterfly process.
- * @param[in, out] *pSrc16 points to the in-place buffer of Q15 data type.
- * @param[in] fftLen length of the FFT.
- * @param[in] *pCoef16 points to twiddle coefficient buffer.
- * @param[in] twidCoefModifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
- * @return none.
- */
-
- void arm_radix4_butterfly_inverse_q15(
- q15_t *pSrc16,
- uint32_t fftLen,
- q15_t *pCoef16,
- uint32_t twidCoefModifier);
-
- /**
- * @brief In-place bit reversal function.
- * @param[in, out] *pSrc points to the in-place buffer of Q15 data type.
- * @param[in] fftLen length of the FFT.
- * @param[in] bitRevFactor bit reversal modifier that supports different size FFTs with the same bit reversal table
- * @param[in] *pBitRevTab points to bit reversal table.
- * @return none.
- */
-
- void arm_bitreversal_q15(
- q15_t * pSrc,
- uint32_t fftLen,
- uint16_t bitRevFactor,
- uint16_t *pBitRevTab);
-
- /**
- * @brief Instance structure for the Q15 RFFT/RIFFT function.
- */
-
- typedef struct
- {
- uint32_t fftLenReal; /**< length of the real FFT. */
- uint32_t fftLenBy2; /**< length of the complex FFT. */
- uint8_t ifftFlagR; /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */
- uint8_t bitReverseFlagR; /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */
- uint32_t twidCoefRModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
- q15_t *pTwiddleAReal; /**< points to the real twiddle factor table. */
- q15_t *pTwiddleBReal; /**< points to the imag twiddle factor table. */
- arm_cfft_radix4_instance_q15 *pCfft; /**< points to the complex FFT instance. */
- } arm_rfft_instance_q15;
-
- /**
- * @brief Instance structure for the Q31 RFFT/RIFFT function.
- */
-
- typedef struct
- {
- uint32_t fftLenReal; /**< length of the real FFT. */
- uint32_t fftLenBy2; /**< length of the complex FFT. */
- uint8_t ifftFlagR; /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */
- uint8_t bitReverseFlagR; /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */
- uint32_t twidCoefRModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
- q31_t *pTwiddleAReal; /**< points to the real twiddle factor table. */
- q31_t *pTwiddleBReal; /**< points to the imag twiddle factor table. */
- arm_cfft_radix4_instance_q31 *pCfft; /**< points to the complex FFT instance. */
- } arm_rfft_instance_q31;
-
- /**
- * @brief Instance structure for the floating-point RFFT/RIFFT function.
- */
-
- typedef struct
- {
- uint32_t fftLenReal; /**< length of the real FFT. */
- uint16_t fftLenBy2; /**< length of the complex FFT. */
- uint8_t ifftFlagR; /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */
- uint8_t bitReverseFlagR; /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */
- uint32_t twidCoefRModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
- float32_t *pTwiddleAReal; /**< points to the real twiddle factor table. */
- float32_t *pTwiddleBReal; /**< points to the imag twiddle factor table. */
- arm_cfft_radix4_instance_f32 *pCfft; /**< points to the complex FFT instance. */
- } arm_rfft_instance_f32;
-
- /**
- * @brief Processing function for the Q15 RFFT/RIFFT.
- * @param[in] *S points to an instance of the Q15 RFFT/RIFFT structure.
- * @param[in] *pSrc points to the input buffer.
- * @param[out] *pDst points to the output buffer.
- * @return none.
- */
-
- void arm_rfft_q15(
- const arm_rfft_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst);
-
- /**
- * @brief Initialization function for the Q15 RFFT/RIFFT.
- * @param[in, out] *S points to an instance of the Q15 RFFT/RIFFT structure.
- * @param[in] *S_CFFT points to an instance of the Q15 CFFT/CIFFT structure.
- * @param[in] fftLenReal length of the FFT.
- * @param[in] ifftFlagR flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform.
- * @param[in] bitReverseFlag flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output.
- * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if fftLenReal is not a supported value.
- */
-
- arm_status arm_rfft_init_q15(
- arm_rfft_instance_q15 * S,
- arm_cfft_radix4_instance_q15 * S_CFFT,
- uint32_t fftLenReal,
- uint32_t ifftFlagR,
- uint32_t bitReverseFlag);
-
- /**
- * @brief Processing function for the Q31 RFFT/RIFFT.
- * @param[in] *S points to an instance of the Q31 RFFT/RIFFT structure.
- * @param[in] *pSrc points to the input buffer.
- * @param[out] *pDst points to the output buffer.
- * @return none.
- */
-
- void arm_rfft_q31(
- const arm_rfft_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst);
-
- /**
- * @brief Initialization function for the Q31 RFFT/RIFFT.
- * @param[in, out] *S points to an instance of the Q31 RFFT/RIFFT structure.
- * @param[in, out] *S_CFFT points to an instance of the Q31 CFFT/CIFFT structure.
- * @param[in] fftLenReal length of the FFT.
- * @param[in] ifftFlagR flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform.
- * @param[in] bitReverseFlag flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output.
- * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if fftLenReal is not a supported value.
- */
-
- arm_status arm_rfft_init_q31(
- arm_rfft_instance_q31 * S,
- arm_cfft_radix4_instance_q31 * S_CFFT,
- uint32_t fftLenReal,
- uint32_t ifftFlagR,
- uint32_t bitReverseFlag);
-
- /**
- * @brief Initialization function for the floating-point RFFT/RIFFT.
- * @param[in,out] *S points to an instance of the floating-point RFFT/RIFFT structure.
- * @param[in,out] *S_CFFT points to an instance of the floating-point CFFT/CIFFT structure.
- * @param[in] fftLenReal length of the FFT.
- * @param[in] ifftFlagR flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform.
- * @param[in] bitReverseFlag flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output.
- * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if fftLenReal is not a supported value.
- */
-
- arm_status arm_rfft_init_f32(
- arm_rfft_instance_f32 * S,
- arm_cfft_radix4_instance_f32 * S_CFFT,
- uint32_t fftLenReal,
- uint32_t ifftFlagR,
- uint32_t bitReverseFlag);
-
- /**
- * @brief Processing function for the floating-point RFFT/RIFFT.
- * @param[in] *S points to an instance of the floating-point RFFT/RIFFT structure.
- * @param[in] *pSrc points to the input buffer.
- * @param[out] *pDst points to the output buffer.
- * @return none.
- */
-
- void arm_rfft_f32(
- const arm_rfft_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst);
-
- /**
- * @brief Instance structure for the floating-point DCT4/IDCT4 function.
- */
-
- typedef struct
- {
- uint16_t N; /**< length of the DCT4. */
- uint16_t Nby2; /**< half of the length of the DCT4. */
- float32_t normalize; /**< normalizing factor. */
- float32_t *pTwiddle; /**< points to the twiddle factor table. */
- float32_t *pCosFactor; /**< points to the cosFactor table. */
- arm_rfft_instance_f32 *pRfft; /**< points to the real FFT instance. */
- arm_cfft_radix4_instance_f32 *pCfft; /**< points to the complex FFT instance. */
- } arm_dct4_instance_f32;
-
- /**
- * @brief Initialization function for the floating-point DCT4/IDCT4.
- * @param[in,out] *S points to an instance of floating-point DCT4/IDCT4 structure.
- * @param[in] *S_RFFT points to an instance of floating-point RFFT/RIFFT structure.
- * @param[in] *S_CFFT points to an instance of floating-point CFFT/CIFFT structure.
- * @param[in] N length of the DCT4.
- * @param[in] Nby2 half of the length of the DCT4.
- * @param[in] normalize normalizing factor.
- * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if fftLenReal is not a supported transform length.
- */
-
- arm_status arm_dct4_init_f32(
- arm_dct4_instance_f32 * S,
- arm_rfft_instance_f32 * S_RFFT,
- arm_cfft_radix4_instance_f32 * S_CFFT,
- uint16_t N,
- uint16_t Nby2,
- float32_t normalize);
-
- /**
- * @brief Processing function for the floating-point DCT4/IDCT4.
- * @param[in] *S points to an instance of the floating-point DCT4/IDCT4 structure.
- * @param[in] *pState points to state buffer.
- * @param[in,out] *pInlineBuffer points to the in-place input and output buffer.
- * @return none.
- */
-
- void arm_dct4_f32(
- const arm_dct4_instance_f32 * S,
- float32_t * pState,
- float32_t * pInlineBuffer);
-
- /**
- * @brief Instance structure for the Q31 DCT4/IDCT4 function.
- */
-
- typedef struct
- {
- uint16_t N; /**< length of the DCT4. */
- uint16_t Nby2; /**< half of the length of the DCT4. */
- q31_t normalize; /**< normalizing factor. */
- q31_t *pTwiddle; /**< points to the twiddle factor table. */
- q31_t *pCosFactor; /**< points to the cosFactor table. */
- arm_rfft_instance_q31 *pRfft; /**< points to the real FFT instance. */
- arm_cfft_radix4_instance_q31 *pCfft; /**< points to the complex FFT instance. */
- } arm_dct4_instance_q31;
-
- /**
- * @brief Initialization function for the Q31 DCT4/IDCT4.
- * @param[in,out] *S points to an instance of Q31 DCT4/IDCT4 structure.
- * @param[in] *S_RFFT points to an instance of Q31 RFFT/RIFFT structure
- * @param[in] *S_CFFT points to an instance of Q31 CFFT/CIFFT structure
- * @param[in] N length of the DCT4.
- * @param[in] Nby2 half of the length of the DCT4.
- * @param[in] normalize normalizing factor.
- * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if N is not a supported transform length.
- */
-
- arm_status arm_dct4_init_q31(
- arm_dct4_instance_q31 * S,
- arm_rfft_instance_q31 * S_RFFT,
- arm_cfft_radix4_instance_q31 * S_CFFT,
- uint16_t N,
- uint16_t Nby2,
- q31_t normalize);
-
- /**
- * @brief Processing function for the Q31 DCT4/IDCT4.
- * @param[in] *S points to an instance of the Q31 DCT4 structure.
- * @param[in] *pState points to state buffer.
- * @param[in,out] *pInlineBuffer points to the in-place input and output buffer.
- * @return none.
- */
-
- void arm_dct4_q31(
- const arm_dct4_instance_q31 * S,
- q31_t * pState,
- q31_t * pInlineBuffer);
-
- /**
- * @brief Instance structure for the Q15 DCT4/IDCT4 function.
- */
-
- typedef struct
- {
- uint16_t N; /**< length of the DCT4. */
- uint16_t Nby2; /**< half of the length of the DCT4. */
- q15_t normalize; /**< normalizing factor. */
- q15_t *pTwiddle; /**< points to the twiddle factor table. */
- q15_t *pCosFactor; /**< points to the cosFactor table. */
- arm_rfft_instance_q15 *pRfft; /**< points to the real FFT instance. */
- arm_cfft_radix4_instance_q15 *pCfft; /**< points to the complex FFT instance. */
- } arm_dct4_instance_q15;
-
- /**
- * @brief Initialization function for the Q15 DCT4/IDCT4.
- * @param[in,out] *S points to an instance of Q15 DCT4/IDCT4 structure.
- * @param[in] *S_RFFT points to an instance of Q15 RFFT/RIFFT structure.
- * @param[in] *S_CFFT points to an instance of Q15 CFFT/CIFFT structure.
- * @param[in] N length of the DCT4.
- * @param[in] Nby2 half of the length of the DCT4.
- * @param[in] normalize normalizing factor.
- * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if N is not a supported transform length.
- */
-
- arm_status arm_dct4_init_q15(
- arm_dct4_instance_q15 * S,
- arm_rfft_instance_q15 * S_RFFT,
- arm_cfft_radix4_instance_q15 * S_CFFT,
- uint16_t N,
- uint16_t Nby2,
- q15_t normalize);
-
- /**
- * @brief Processing function for the Q15 DCT4/IDCT4.
- * @param[in] *S points to an instance of the Q15 DCT4 structure.
- * @param[in] *pState points to state buffer.
- * @param[in,out] *pInlineBuffer points to the in-place input and output buffer.
- * @return none.
- */
-
- void arm_dct4_q15(
- const arm_dct4_instance_q15 * S,
- q15_t * pState,
- q15_t * pInlineBuffer);
-
- /**
- * @brief Floating-point vector addition.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_add_f32(
- float32_t * pSrcA,
- float32_t * pSrcB,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q7 vector addition.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_add_q7(
- q7_t * pSrcA,
- q7_t * pSrcB,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q15 vector addition.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_add_q15(
- q15_t * pSrcA,
- q15_t * pSrcB,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q31 vector addition.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_add_q31(
- q31_t * pSrcA,
- q31_t * pSrcB,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Floating-point vector subtraction.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_sub_f32(
- float32_t * pSrcA,
- float32_t * pSrcB,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q7 vector subtraction.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_sub_q7(
- q7_t * pSrcA,
- q7_t * pSrcB,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q15 vector subtraction.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_sub_q15(
- q15_t * pSrcA,
- q15_t * pSrcB,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q31 vector subtraction.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_sub_q31(
- q31_t * pSrcA,
- q31_t * pSrcB,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Multiplies a floating-point vector by a scalar.
- * @param[in] *pSrc points to the input vector
- * @param[in] scale scale factor to be applied
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_scale_f32(
- float32_t * pSrc,
- float32_t scale,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Multiplies a Q7 vector by a scalar.
- * @param[in] *pSrc points to the input vector
- * @param[in] scaleFract fractional portion of the scale value
- * @param[in] shift number of bits to shift the result by
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_scale_q7(
- q7_t * pSrc,
- q7_t scaleFract,
- int8_t shift,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Multiplies a Q15 vector by a scalar.
- * @param[in] *pSrc points to the input vector
- * @param[in] scaleFract fractional portion of the scale value
- * @param[in] shift number of bits to shift the result by
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_scale_q15(
- q15_t * pSrc,
- q15_t scaleFract,
- int8_t shift,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Multiplies a Q31 vector by a scalar.
- * @param[in] *pSrc points to the input vector
- * @param[in] scaleFract fractional portion of the scale value
- * @param[in] shift number of bits to shift the result by
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_scale_q31(
- q31_t * pSrc,
- q31_t scaleFract,
- int8_t shift,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q7 vector absolute value.
- * @param[in] *pSrc points to the input buffer
- * @param[out] *pDst points to the output buffer
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_abs_q7(
- q7_t * pSrc,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Floating-point vector absolute value.
- * @param[in] *pSrc points to the input buffer
- * @param[out] *pDst points to the output buffer
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_abs_f32(
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q15 vector absolute value.
- * @param[in] *pSrc points to the input buffer
- * @param[out] *pDst points to the output buffer
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_abs_q15(
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q31 vector absolute value.
- * @param[in] *pSrc points to the input buffer
- * @param[out] *pDst points to the output buffer
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_abs_q31(
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Dot product of floating-point vectors.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[in] blockSize number of samples in each vector
- * @param[out] *result output result returned here
- * @return none.
- */
-
- void arm_dot_prod_f32(
- float32_t * pSrcA,
- float32_t * pSrcB,
- uint32_t blockSize,
- float32_t * result);
-
- /**
- * @brief Dot product of Q7 vectors.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[in] blockSize number of samples in each vector
- * @param[out] *result output result returned here
- * @return none.
- */
-
- void arm_dot_prod_q7(
- q7_t * pSrcA,
- q7_t * pSrcB,
- uint32_t blockSize,
- q31_t * result);
-
- /**
- * @brief Dot product of Q15 vectors.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[in] blockSize number of samples in each vector
- * @param[out] *result output result returned here
- * @return none.
- */
-
- void arm_dot_prod_q15(
- q15_t * pSrcA,
- q15_t * pSrcB,
- uint32_t blockSize,
- q63_t * result);
-
- /**
- * @brief Dot product of Q31 vectors.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[in] blockSize number of samples in each vector
- * @param[out] *result output result returned here
- * @return none.
- */
-
- void arm_dot_prod_q31(
- q31_t * pSrcA,
- q31_t * pSrcB,
- uint32_t blockSize,
- q63_t * result);
-
- /**
- * @brief Shifts the elements of a Q7 vector a specified number of bits.
- * @param[in] *pSrc points to the input vector
- * @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right.
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_shift_q7(
- q7_t * pSrc,
- int8_t shiftBits,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Shifts the elements of a Q15 vector a specified number of bits.
- * @param[in] *pSrc points to the input vector
- * @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right.
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_shift_q15(
- q15_t * pSrc,
- int8_t shiftBits,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Shifts the elements of a Q31 vector a specified number of bits.
- * @param[in] *pSrc points to the input vector
- * @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right.
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_shift_q31(
- q31_t * pSrc,
- int8_t shiftBits,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Adds a constant offset to a floating-point vector.
- * @param[in] *pSrc points to the input vector
- * @param[in] offset is the offset to be added
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_offset_f32(
- float32_t * pSrc,
- float32_t offset,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Adds a constant offset to a Q7 vector.
- * @param[in] *pSrc points to the input vector
- * @param[in] offset is the offset to be added
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_offset_q7(
- q7_t * pSrc,
- q7_t offset,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Adds a constant offset to a Q15 vector.
- * @param[in] *pSrc points to the input vector
- * @param[in] offset is the offset to be added
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_offset_q15(
- q15_t * pSrc,
- q15_t offset,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Adds a constant offset to a Q31 vector.
- * @param[in] *pSrc points to the input vector
- * @param[in] offset is the offset to be added
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_offset_q31(
- q31_t * pSrc,
- q31_t offset,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Negates the elements of a floating-point vector.
- * @param[in] *pSrc points to the input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_negate_f32(
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Negates the elements of a Q7 vector.
- * @param[in] *pSrc points to the input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_negate_q7(
- q7_t * pSrc,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Negates the elements of a Q15 vector.
- * @param[in] *pSrc points to the input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_negate_q15(
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Negates the elements of a Q31 vector.
- * @param[in] *pSrc points to the input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_negate_q31(
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
- /**
- * @brief Copies the elements of a floating-point vector.
- * @param[in] *pSrc input pointer
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_copy_f32(
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Copies the elements of a Q7 vector.
- * @param[in] *pSrc input pointer
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_copy_q7(
- q7_t * pSrc,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Copies the elements of a Q15 vector.
- * @param[in] *pSrc input pointer
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_copy_q15(
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Copies the elements of a Q31 vector.
- * @param[in] *pSrc input pointer
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_copy_q31(
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
- /**
- * @brief Fills a constant value into a floating-point vector.
- * @param[in] value input value to be filled
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_fill_f32(
- float32_t value,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Fills a constant value into a Q7 vector.
- * @param[in] value input value to be filled
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_fill_q7(
- q7_t value,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Fills a constant value into a Q15 vector.
- * @param[in] value input value to be filled
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_fill_q15(
- q15_t value,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Fills a constant value into a Q31 vector.
- * @param[in] value input value to be filled
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_fill_q31(
- q31_t value,
- q31_t * pDst,
- uint32_t blockSize);
-
-/**
- * @brief Convolution of floating-point sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the location where the output result is written. Length srcALen+srcBLen-1.
- * @return none.
- */
-
- void arm_conv_f32(
- float32_t * pSrcA,
- uint32_t srcALen,
- float32_t * pSrcB,
- uint32_t srcBLen,
- float32_t * pDst);
-
-/**
- * @brief Convolution of Q15 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the location where the output result is written. Length srcALen+srcBLen-1.
- * @return none.
- */
-
- void arm_conv_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst);
-
- /**
- * @brief Convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1.
- * @return none.
- */
-
- void arm_conv_fast_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst);
-
- /**
- * @brief Convolution of Q31 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1.
- * @return none.
- */
-
- void arm_conv_q31(
- q31_t * pSrcA,
- uint32_t srcALen,
- q31_t * pSrcB,
- uint32_t srcBLen,
- q31_t * pDst);
-
- /**
- * @brief Convolution of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1.
- * @return none.
- */
-
- void arm_conv_fast_q31(
- q31_t * pSrcA,
- uint32_t srcALen,
- q31_t * pSrcB,
- uint32_t srcBLen,
- q31_t * pDst);
-
- /**
- * @brief Convolution of Q7 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1.
- * @return none.
- */
-
- void arm_conv_q7(
- q7_t * pSrcA,
- uint32_t srcALen,
- q7_t * pSrcB,
- uint32_t srcBLen,
- q7_t * pDst);
-
- /**
- * @brief Partial convolution of floating-point sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data
- * @param[in] firstIndex is the first output sample to start with.
- * @param[in] numPoints is the number of output points to be computed.
- * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
- */
-
- arm_status arm_conv_partial_f32(
- float32_t * pSrcA,
- uint32_t srcALen,
- float32_t * pSrcB,
- uint32_t srcBLen,
- float32_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints);
-
- /**
- * @brief Partial convolution of Q15 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data
- * @param[in] firstIndex is the first output sample to start with.
- * @param[in] numPoints is the number of output points to be computed.
- * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
- */
-
- arm_status arm_conv_partial_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints);
-
- /**
- * @brief Partial convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data
- * @param[in] firstIndex is the first output sample to start with.
- * @param[in] numPoints is the number of output points to be computed.
- * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
- */
-
- arm_status arm_conv_partial_fast_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints);
-
- /**
- * @brief Partial convolution of Q31 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data
- * @param[in] firstIndex is the first output sample to start with.
- * @param[in] numPoints is the number of output points to be computed.
- * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
- */
-
- arm_status arm_conv_partial_q31(
- q31_t * pSrcA,
- uint32_t srcALen,
- q31_t * pSrcB,
- uint32_t srcBLen,
- q31_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints);
-
-
- /**
- * @brief Partial convolution of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data
- * @param[in] firstIndex is the first output sample to start with.
- * @param[in] numPoints is the number of output points to be computed.
- * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
- */
-
- arm_status arm_conv_partial_fast_q31(
- q31_t * pSrcA,
- uint32_t srcALen,
- q31_t * pSrcB,
- uint32_t srcBLen,
- q31_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints);
-
- /**
- * @brief Partial convolution of Q7 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data
- * @param[in] firstIndex is the first output sample to start with.
- * @param[in] numPoints is the number of output points to be computed.
- * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
- */
-
- arm_status arm_conv_partial_q7(
- q7_t * pSrcA,
- uint32_t srcALen,
- q7_t * pSrcB,
- uint32_t srcBLen,
- q7_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints);
-
-
- /**
- * @brief Instance structure for the Q15 FIR decimator.
- */
-
- typedef struct
- {
- uint8_t M; /**< decimation factor. */
- uint16_t numTaps; /**< number of coefficients in the filter. */
- q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
- q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- } arm_fir_decimate_instance_q15;
-
- /**
- * @brief Instance structure for the Q31 FIR decimator.
- */
-
- typedef struct
- {
- uint8_t M; /**< decimation factor. */
- uint16_t numTaps; /**< number of coefficients in the filter. */
- q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
- q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
-
- } arm_fir_decimate_instance_q31;
-
- /**
- * @brief Instance structure for the floating-point FIR decimator.
- */
-
- typedef struct
- {
- uint8_t M; /**< decimation factor. */
- uint16_t numTaps; /**< number of coefficients in the filter. */
- float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
- float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
-
- } arm_fir_decimate_instance_f32;
-
-
-
- /**
- * @brief Processing function for the floating-point FIR decimator.
- * @param[in] *S points to an instance of the floating-point FIR decimator structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] blockSize number of input samples to process per call.
- * @return none
- */
-
- void arm_fir_decimate_f32(
- const arm_fir_decimate_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @brief Initialization function for the floating-point FIR decimator.
- * @param[in,out] *S points to an instance of the floating-point FIR decimator structure.
- * @param[in] numTaps number of coefficients in the filter.
- * @param[in] M decimation factor.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of input samples to process per call.
- * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
- * blockSize is not a multiple of M.
- */
-
- arm_status arm_fir_decimate_init_f32(
- arm_fir_decimate_instance_f32 * S,
- uint16_t numTaps,
- uint8_t M,
- float32_t * pCoeffs,
- float32_t * pState,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the Q15 FIR decimator.
- * @param[in] *S points to an instance of the Q15 FIR decimator structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] blockSize number of input samples to process per call.
- * @return none
- */
-
- void arm_fir_decimate_q15(
- const arm_fir_decimate_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the Q15 FIR decimator (fast variant) for Cortex-M3 and Cortex-M4.
- * @param[in] *S points to an instance of the Q15 FIR decimator structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] blockSize number of input samples to process per call.
- * @return none
- */
-
- void arm_fir_decimate_fast_q15(
- const arm_fir_decimate_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
-
-
- /**
- * @brief Initialization function for the Q15 FIR decimator.
- * @param[in,out] *S points to an instance of the Q15 FIR decimator structure.
- * @param[in] numTaps number of coefficients in the filter.
- * @param[in] M decimation factor.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of input samples to process per call.
- * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
- * blockSize is not a multiple of M.
- */
-
- arm_status arm_fir_decimate_init_q15(
- arm_fir_decimate_instance_q15 * S,
- uint16_t numTaps,
- uint8_t M,
- q15_t * pCoeffs,
- q15_t * pState,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the Q31 FIR decimator.
- * @param[in] *S points to an instance of the Q31 FIR decimator structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] blockSize number of input samples to process per call.
- * @return none
- */
-
- void arm_fir_decimate_q31(
- const arm_fir_decimate_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the Q31 FIR decimator (fast variant) for Cortex-M3 and Cortex-M4.
- * @param[in] *S points to an instance of the Q31 FIR decimator structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] blockSize number of input samples to process per call.
- * @return none
- */
-
- void arm_fir_decimate_fast_q31(
- arm_fir_decimate_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @brief Initialization function for the Q31 FIR decimator.
- * @param[in,out] *S points to an instance of the Q31 FIR decimator structure.
- * @param[in] numTaps number of coefficients in the filter.
- * @param[in] M decimation factor.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of input samples to process per call.
- * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
- * blockSize is not a multiple of M.
- */
-
- arm_status arm_fir_decimate_init_q31(
- arm_fir_decimate_instance_q31 * S,
- uint16_t numTaps,
- uint8_t M,
- q31_t * pCoeffs,
- q31_t * pState,
- uint32_t blockSize);
-
-
-
- /**
- * @brief Instance structure for the Q15 FIR interpolator.
- */
-
- typedef struct
- {
- uint8_t L; /**< upsample factor. */
- uint16_t phaseLength; /**< length of each polyphase filter component. */
- q15_t *pCoeffs; /**< points to the coefficient array. The array is of length L*phaseLength. */
- q15_t *pState; /**< points to the state variable array. The array is of length blockSize+phaseLength-1. */
- } arm_fir_interpolate_instance_q15;
-
- /**
- * @brief Instance structure for the Q31 FIR interpolator.
- */
-
- typedef struct
- {
- uint8_t L; /**< upsample factor. */
- uint16_t phaseLength; /**< length of each polyphase filter component. */
- q31_t *pCoeffs; /**< points to the coefficient array. The array is of length L*phaseLength. */
- q31_t *pState; /**< points to the state variable array. The array is of length blockSize+phaseLength-1. */
- } arm_fir_interpolate_instance_q31;
-
- /**
- * @brief Instance structure for the floating-point FIR interpolator.
- */
-
- typedef struct
- {
- uint8_t L; /**< upsample factor. */
- uint16_t phaseLength; /**< length of each polyphase filter component. */
- float32_t *pCoeffs; /**< points to the coefficient array. The array is of length L*phaseLength. */
- float32_t *pState; /**< points to the state variable array. The array is of length phaseLength+numTaps-1. */
- } arm_fir_interpolate_instance_f32;
-
-
- /**
- * @brief Processing function for the Q15 FIR interpolator.
- * @param[in] *S points to an instance of the Q15 FIR interpolator structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of input samples to process per call.
- * @return none.
- */
-
- void arm_fir_interpolate_q15(
- const arm_fir_interpolate_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @brief Initialization function for the Q15 FIR interpolator.
- * @param[in,out] *S points to an instance of the Q15 FIR interpolator structure.
- * @param[in] L upsample factor.
- * @param[in] numTaps number of filter coefficients in the filter.
- * @param[in] *pCoeffs points to the filter coefficient buffer.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of input samples to process per call.
- * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
- * the filter length numTaps is not a multiple of the interpolation factor L.
- */
-
- arm_status arm_fir_interpolate_init_q15(
- arm_fir_interpolate_instance_q15 * S,
- uint8_t L,
- uint16_t numTaps,
- q15_t * pCoeffs,
- q15_t * pState,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the Q31 FIR interpolator.
- * @param[in] *S points to an instance of the Q15 FIR interpolator structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of input samples to process per call.
- * @return none.
- */
-
- void arm_fir_interpolate_q31(
- const arm_fir_interpolate_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the Q31 FIR interpolator.
- * @param[in,out] *S points to an instance of the Q31 FIR interpolator structure.
- * @param[in] L upsample factor.
- * @param[in] numTaps number of filter coefficients in the filter.
- * @param[in] *pCoeffs points to the filter coefficient buffer.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of input samples to process per call.
- * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
- * the filter length numTaps is not a multiple of the interpolation factor L.
- */
-
- arm_status arm_fir_interpolate_init_q31(
- arm_fir_interpolate_instance_q31 * S,
- uint8_t L,
- uint16_t numTaps,
- q31_t * pCoeffs,
- q31_t * pState,
- uint32_t blockSize);
-
-
- /**
- * @brief Processing function for the floating-point FIR interpolator.
- * @param[in] *S points to an instance of the floating-point FIR interpolator structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of input samples to process per call.
- * @return none.
- */
-
- void arm_fir_interpolate_f32(
- const arm_fir_interpolate_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the floating-point FIR interpolator.
- * @param[in,out] *S points to an instance of the floating-point FIR interpolator structure.
- * @param[in] L upsample factor.
- * @param[in] numTaps number of filter coefficients in the filter.
- * @param[in] *pCoeffs points to the filter coefficient buffer.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of input samples to process per call.
- * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
- * the filter length numTaps is not a multiple of the interpolation factor L.
- */
-
- arm_status arm_fir_interpolate_init_f32(
- arm_fir_interpolate_instance_f32 * S,
- uint8_t L,
- uint16_t numTaps,
- float32_t * pCoeffs,
- float32_t * pState,
- uint32_t blockSize);
-
- /**
- * @brief Instance structure for the high precision Q31 Biquad cascade filter.
- */
-
- typedef struct
- {
- uint8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */
- q63_t *pState; /**< points to the array of state coefficients. The array is of length 4*numStages. */
- q31_t *pCoeffs; /**< points to the array of coefficients. The array is of length 5*numStages. */
- uint8_t postShift; /**< additional shift, in bits, applied to each output sample. */
-
- } arm_biquad_cas_df1_32x64_ins_q31;
-
-
- /**
- * @param[in] *S points to an instance of the high precision Q31 Biquad cascade filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_biquad_cas_df1_32x64_q31(
- const arm_biquad_cas_df1_32x64_ins_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @param[in,out] *S points to an instance of the high precision Q31 Biquad cascade filter structure.
- * @param[in] numStages number of 2nd order stages in the filter.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] postShift shift to be applied to the output. Varies according to the coefficients format
- * @return none
- */
-
- void arm_biquad_cas_df1_32x64_init_q31(
- arm_biquad_cas_df1_32x64_ins_q31 * S,
- uint8_t numStages,
- q31_t * pCoeffs,
- q63_t * pState,
- uint8_t postShift);
-
-
-
- /**
- * @brief Instance structure for the floating-point transposed direct form II Biquad cascade filter.
- */
-
- typedef struct
- {
- uint8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */
- float32_t *pState; /**< points to the array of state coefficients. The array is of length 2*numStages. */
- float32_t *pCoeffs; /**< points to the array of coefficients. The array is of length 5*numStages. */
- } arm_biquad_cascade_df2T_instance_f32;
-
-
- /**
- * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter.
- * @param[in] *S points to an instance of the filter data structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_biquad_cascade_df2T_f32(
- const arm_biquad_cascade_df2T_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @brief Initialization function for the floating-point transposed direct form II Biquad cascade filter.
- * @param[in,out] *S points to an instance of the filter data structure.
- * @param[in] numStages number of 2nd order stages in the filter.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @return none
- */
-
- void arm_biquad_cascade_df2T_init_f32(
- arm_biquad_cascade_df2T_instance_f32 * S,
- uint8_t numStages,
- float32_t * pCoeffs,
- float32_t * pState);
-
-
-
- /**
- * @brief Instance structure for the Q15 FIR lattice filter.
- */
-
- typedef struct
- {
- uint16_t numStages; /**< number of filter stages. */
- q15_t *pState; /**< points to the state variable array. The array is of length numStages. */
- q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numStages. */
- } arm_fir_lattice_instance_q15;
-
- /**
- * @brief Instance structure for the Q31 FIR lattice filter.
- */
-
- typedef struct
- {
- uint16_t numStages; /**< number of filter stages. */
- q31_t *pState; /**< points to the state variable array. The array is of length numStages. */
- q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numStages. */
- } arm_fir_lattice_instance_q31;
-
- /**
- * @brief Instance structure for the floating-point FIR lattice filter.
- */
-
- typedef struct
- {
- uint16_t numStages; /**< number of filter stages. */
- float32_t *pState; /**< points to the state variable array. The array is of length numStages. */
- float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numStages. */
- } arm_fir_lattice_instance_f32;
-
- /**
- * @brief Initialization function for the Q15 FIR lattice filter.
- * @param[in] *S points to an instance of the Q15 FIR lattice structure.
- * @param[in] numStages number of filter stages.
- * @param[in] *pCoeffs points to the coefficient buffer. The array is of length numStages.
- * @param[in] *pState points to the state buffer. The array is of length numStages.
- * @return none.
- */
-
- void arm_fir_lattice_init_q15(
- arm_fir_lattice_instance_q15 * S,
- uint16_t numStages,
- q15_t * pCoeffs,
- q15_t * pState);
-
-
- /**
- * @brief Processing function for the Q15 FIR lattice filter.
- * @param[in] *S points to an instance of the Q15 FIR lattice structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
- void arm_fir_lattice_q15(
- const arm_fir_lattice_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the Q31 FIR lattice filter.
- * @param[in] *S points to an instance of the Q31 FIR lattice structure.
- * @param[in] numStages number of filter stages.
- * @param[in] *pCoeffs points to the coefficient buffer. The array is of length numStages.
- * @param[in] *pState points to the state buffer. The array is of length numStages.
- * @return none.
- */
-
- void arm_fir_lattice_init_q31(
- arm_fir_lattice_instance_q31 * S,
- uint16_t numStages,
- q31_t * pCoeffs,
- q31_t * pState);
-
-
- /**
- * @brief Processing function for the Q31 FIR lattice filter.
- * @param[in] *S points to an instance of the Q31 FIR lattice structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_fir_lattice_q31(
- const arm_fir_lattice_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
-/**
- * @brief Initialization function for the floating-point FIR lattice filter.
- * @param[in] *S points to an instance of the floating-point FIR lattice structure.
- * @param[in] numStages number of filter stages.
- * @param[in] *pCoeffs points to the coefficient buffer. The array is of length numStages.
- * @param[in] *pState points to the state buffer. The array is of length numStages.
- * @return none.
- */
-
- void arm_fir_lattice_init_f32(
- arm_fir_lattice_instance_f32 * S,
- uint16_t numStages,
- float32_t * pCoeffs,
- float32_t * pState);
-
- /**
- * @brief Processing function for the floating-point FIR lattice filter.
- * @param[in] *S points to an instance of the floating-point FIR lattice structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_fir_lattice_f32(
- const arm_fir_lattice_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Instance structure for the Q15 IIR lattice filter.
- */
- typedef struct
- {
- uint16_t numStages; /**< number of stages in the filter. */
- q15_t *pState; /**< points to the state variable array. The array is of length numStages+blockSize. */
- q15_t *pkCoeffs; /**< points to the reflection coefficient array. The array is of length numStages. */
- q15_t *pvCoeffs; /**< points to the ladder coefficient array. The array is of length numStages+1. */
- } arm_iir_lattice_instance_q15;
-
- /**
- * @brief Instance structure for the Q31 IIR lattice filter.
- */
- typedef struct
- {
- uint16_t numStages; /**< number of stages in the filter. */
- q31_t *pState; /**< points to the state variable array. The array is of length numStages+blockSize. */
- q31_t *pkCoeffs; /**< points to the reflection coefficient array. The array is of length numStages. */
- q31_t *pvCoeffs; /**< points to the ladder coefficient array. The array is of length numStages+1. */
- } arm_iir_lattice_instance_q31;
-
- /**
- * @brief Instance structure for the floating-point IIR lattice filter.
- */
- typedef struct
- {
- uint16_t numStages; /**< number of stages in the filter. */
- float32_t *pState; /**< points to the state variable array. The array is of length numStages+blockSize. */
- float32_t *pkCoeffs; /**< points to the reflection coefficient array. The array is of length numStages. */
- float32_t *pvCoeffs; /**< points to the ladder coefficient array. The array is of length numStages+1. */
- } arm_iir_lattice_instance_f32;
-
- /**
- * @brief Processing function for the floating-point IIR lattice filter.
- * @param[in] *S points to an instance of the floating-point IIR lattice structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_iir_lattice_f32(
- const arm_iir_lattice_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the floating-point IIR lattice filter.
- * @param[in] *S points to an instance of the floating-point IIR lattice structure.
- * @param[in] numStages number of stages in the filter.
- * @param[in] *pkCoeffs points to the reflection coefficient buffer. The array is of length numStages.
- * @param[in] *pvCoeffs points to the ladder coefficient buffer. The array is of length numStages+1.
- * @param[in] *pState points to the state buffer. The array is of length numStages+blockSize-1.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_iir_lattice_init_f32(
- arm_iir_lattice_instance_f32 * S,
- uint16_t numStages,
- float32_t *pkCoeffs,
- float32_t *pvCoeffs,
- float32_t *pState,
- uint32_t blockSize);
-
-
- /**
- * @brief Processing function for the Q31 IIR lattice filter.
- * @param[in] *S points to an instance of the Q31 IIR lattice structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_iir_lattice_q31(
- const arm_iir_lattice_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @brief Initialization function for the Q31 IIR lattice filter.
- * @param[in] *S points to an instance of the Q31 IIR lattice structure.
- * @param[in] numStages number of stages in the filter.
- * @param[in] *pkCoeffs points to the reflection coefficient buffer. The array is of length numStages.
- * @param[in] *pvCoeffs points to the ladder coefficient buffer. The array is of length numStages+1.
- * @param[in] *pState points to the state buffer. The array is of length numStages+blockSize.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_iir_lattice_init_q31(
- arm_iir_lattice_instance_q31 * S,
- uint16_t numStages,
- q31_t *pkCoeffs,
- q31_t *pvCoeffs,
- q31_t *pState,
- uint32_t blockSize);
-
-
- /**
- * @brief Processing function for the Q15 IIR lattice filter.
- * @param[in] *S points to an instance of the Q15 IIR lattice structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_iir_lattice_q15(
- const arm_iir_lattice_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
-
-/**
- * @brief Initialization function for the Q15 IIR lattice filter.
- * @param[in] *S points to an instance of the fixed-point Q15 IIR lattice structure.
- * @param[in] numStages number of stages in the filter.
- * @param[in] *pkCoeffs points to reflection coefficient buffer. The array is of length numStages.
- * @param[in] *pvCoeffs points to ladder coefficient buffer. The array is of length numStages+1.
- * @param[in] *pState points to state buffer. The array is of length numStages+blockSize.
- * @param[in] blockSize number of samples to process per call.
- * @return none.
- */
-
- void arm_iir_lattice_init_q15(
- arm_iir_lattice_instance_q15 * S,
- uint16_t numStages,
- q15_t *pkCoeffs,
- q15_t *pvCoeffs,
- q15_t *pState,
- uint32_t blockSize);
-
- /**
- * @brief Instance structure for the floating-point LMS filter.
- */
-
- typedef struct
- {
- uint16_t numTaps; /**< number of coefficients in the filter. */
- float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
- float32_t mu; /**< step size that controls filter coefficient updates. */
- } arm_lms_instance_f32;
-
- /**
- * @brief Processing function for floating-point LMS filter.
- * @param[in] *S points to an instance of the floating-point LMS filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[in] *pRef points to the block of reference data.
- * @param[out] *pOut points to the block of output data.
- * @param[out] *pErr points to the block of error data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_lms_f32(
- const arm_lms_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pRef,
- float32_t * pOut,
- float32_t * pErr,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for floating-point LMS filter.
- * @param[in] *S points to an instance of the floating-point LMS filter structure.
- * @param[in] numTaps number of filter coefficients.
- * @param[in] *pCoeffs points to the coefficient buffer.
- * @param[in] *pState points to state buffer.
- * @param[in] mu step size that controls filter coefficient updates.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_lms_init_f32(
- arm_lms_instance_f32 * S,
- uint16_t numTaps,
- float32_t * pCoeffs,
- float32_t * pState,
- float32_t mu,
- uint32_t blockSize);
-
- /**
- * @brief Instance structure for the Q15 LMS filter.
- */
-
- typedef struct
- {
- uint16_t numTaps; /**< number of coefficients in the filter. */
- q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
- q15_t mu; /**< step size that controls filter coefficient updates. */
- uint32_t postShift; /**< bit shift applied to coefficients. */
- } arm_lms_instance_q15;
-
-
- /**
- * @brief Initialization function for the Q15 LMS filter.
- * @param[in] *S points to an instance of the Q15 LMS filter structure.
- * @param[in] numTaps number of filter coefficients.
- * @param[in] *pCoeffs points to the coefficient buffer.
- * @param[in] *pState points to the state buffer.
- * @param[in] mu step size that controls filter coefficient updates.
- * @param[in] blockSize number of samples to process.
- * @param[in] postShift bit shift applied to coefficients.
- * @return none.
- */
-
- void arm_lms_init_q15(
- arm_lms_instance_q15 * S,
- uint16_t numTaps,
- q15_t * pCoeffs,
- q15_t * pState,
- q15_t mu,
- uint32_t blockSize,
- uint32_t postShift);
-
- /**
- * @brief Processing function for Q15 LMS filter.
- * @param[in] *S points to an instance of the Q15 LMS filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[in] *pRef points to the block of reference data.
- * @param[out] *pOut points to the block of output data.
- * @param[out] *pErr points to the block of error data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_lms_q15(
- const arm_lms_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pRef,
- q15_t * pOut,
- q15_t * pErr,
- uint32_t blockSize);
-
-
- /**
- * @brief Instance structure for the Q31 LMS filter.
- */
-
- typedef struct
- {
- uint16_t numTaps; /**< number of coefficients in the filter. */
- q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
- q31_t mu; /**< step size that controls filter coefficient updates. */
- uint32_t postShift; /**< bit shift applied to coefficients. */
-
- } arm_lms_instance_q31;
-
- /**
- * @brief Processing function for Q31 LMS filter.
- * @param[in] *S points to an instance of the Q15 LMS filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[in] *pRef points to the block of reference data.
- * @param[out] *pOut points to the block of output data.
- * @param[out] *pErr points to the block of error data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_lms_q31(
- const arm_lms_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pRef,
- q31_t * pOut,
- q31_t * pErr,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for Q31 LMS filter.
- * @param[in] *S points to an instance of the Q31 LMS filter structure.
- * @param[in] numTaps number of filter coefficients.
- * @param[in] *pCoeffs points to coefficient buffer.
- * @param[in] *pState points to state buffer.
- * @param[in] mu step size that controls filter coefficient updates.
- * @param[in] blockSize number of samples to process.
- * @param[in] postShift bit shift applied to coefficients.
- * @return none.
- */
-
- void arm_lms_init_q31(
- arm_lms_instance_q31 * S,
- uint16_t numTaps,
- q31_t *pCoeffs,
- q31_t *pState,
- q31_t mu,
- uint32_t blockSize,
- uint32_t postShift);
-
- /**
- * @brief Instance structure for the floating-point normalized LMS filter.
- */
-
- typedef struct
- {
- uint16_t numTaps; /**< number of coefficients in the filter. */
- float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
- float32_t mu; /**< step size that control filter coefficient updates. */
- float32_t energy; /**< saves previous frame energy. */
- float32_t x0; /**< saves previous input sample. */
- } arm_lms_norm_instance_f32;
-
- /**
- * @brief Processing function for floating-point normalized LMS filter.
- * @param[in] *S points to an instance of the floating-point normalized LMS filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[in] *pRef points to the block of reference data.
- * @param[out] *pOut points to the block of output data.
- * @param[out] *pErr points to the block of error data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_lms_norm_f32(
- arm_lms_norm_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pRef,
- float32_t * pOut,
- float32_t * pErr,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for floating-point normalized LMS filter.
- * @param[in] *S points to an instance of the floating-point LMS filter structure.
- * @param[in] numTaps number of filter coefficients.
- * @param[in] *pCoeffs points to coefficient buffer.
- * @param[in] *pState points to state buffer.
- * @param[in] mu step size that controls filter coefficient updates.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_lms_norm_init_f32(
- arm_lms_norm_instance_f32 * S,
- uint16_t numTaps,
- float32_t * pCoeffs,
- float32_t * pState,
- float32_t mu,
- uint32_t blockSize);
-
-
- /**
- * @brief Instance structure for the Q31 normalized LMS filter.
- */
- typedef struct
- {
- uint16_t numTaps; /**< number of coefficients in the filter. */
- q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
- q31_t mu; /**< step size that controls filter coefficient updates. */
- uint8_t postShift; /**< bit shift applied to coefficients. */
- q31_t *recipTable; /**< points to the reciprocal initial value table. */
- q31_t energy; /**< saves previous frame energy. */
- q31_t x0; /**< saves previous input sample. */
- } arm_lms_norm_instance_q31;
-
- /**
- * @brief Processing function for Q31 normalized LMS filter.
- * @param[in] *S points to an instance of the Q31 normalized LMS filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[in] *pRef points to the block of reference data.
- * @param[out] *pOut points to the block of output data.
- * @param[out] *pErr points to the block of error data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_lms_norm_q31(
- arm_lms_norm_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pRef,
- q31_t * pOut,
- q31_t * pErr,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for Q31 normalized LMS filter.
- * @param[in] *S points to an instance of the Q31 normalized LMS filter structure.
- * @param[in] numTaps number of filter coefficients.
- * @param[in] *pCoeffs points to coefficient buffer.
- * @param[in] *pState points to state buffer.
- * @param[in] mu step size that controls filter coefficient updates.
- * @param[in] blockSize number of samples to process.
- * @param[in] postShift bit shift applied to coefficients.
- * @return none.
- */
-
- void arm_lms_norm_init_q31(
- arm_lms_norm_instance_q31 * S,
- uint16_t numTaps,
- q31_t * pCoeffs,
- q31_t * pState,
- q31_t mu,
- uint32_t blockSize,
- uint8_t postShift);
-
- /**
- * @brief Instance structure for the Q15 normalized LMS filter.
- */
-
- typedef struct
- {
- uint16_t numTaps; /**< Number of coefficients in the filter. */
- q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
- q15_t mu; /**< step size that controls filter coefficient updates. */
- uint8_t postShift; /**< bit shift applied to coefficients. */
- q15_t *recipTable; /**< Points to the reciprocal initial value table. */
- q15_t energy; /**< saves previous frame energy. */
- q15_t x0; /**< saves previous input sample. */
- } arm_lms_norm_instance_q15;
-
- /**
- * @brief Processing function for Q15 normalized LMS filter.
- * @param[in] *S points to an instance of the Q15 normalized LMS filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[in] *pRef points to the block of reference data.
- * @param[out] *pOut points to the block of output data.
- * @param[out] *pErr points to the block of error data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_lms_norm_q15(
- arm_lms_norm_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pRef,
- q15_t * pOut,
- q15_t * pErr,
- uint32_t blockSize);
-
-
- /**
- * @brief Initialization function for Q15 normalized LMS filter.
- * @param[in] *S points to an instance of the Q15 normalized LMS filter structure.
- * @param[in] numTaps number of filter coefficients.
- * @param[in] *pCoeffs points to coefficient buffer.
- * @param[in] *pState points to state buffer.
- * @param[in] mu step size that controls filter coefficient updates.
- * @param[in] blockSize number of samples to process.
- * @param[in] postShift bit shift applied to coefficients.
- * @return none.
- */
-
- void arm_lms_norm_init_q15(
- arm_lms_norm_instance_q15 * S,
- uint16_t numTaps,
- q15_t * pCoeffs,
- q15_t * pState,
- q15_t mu,
- uint32_t blockSize,
- uint8_t postShift);
-
- /**
- * @brief Correlation of floating-point sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
- * @return none.
- */
-
- void arm_correlate_f32(
- float32_t * pSrcA,
- uint32_t srcALen,
- float32_t * pSrcB,
- uint32_t srcBLen,
- float32_t * pDst);
-
- /**
- * @brief Correlation of Q15 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
- * @return none.
- */
-
- void arm_correlate_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst);
-
- /**
- * @brief Correlation of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
- * @return none.
- */
-
- void arm_correlate_fast_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst);
-
- /**
- * @brief Correlation of Q31 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
- * @return none.
- */
-
- void arm_correlate_q31(
- q31_t * pSrcA,
- uint32_t srcALen,
- q31_t * pSrcB,
- uint32_t srcBLen,
- q31_t * pDst);
-
- /**
- * @brief Correlation of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
- * @return none.
- */
-
- void arm_correlate_fast_q31(
- q31_t * pSrcA,
- uint32_t srcALen,
- q31_t * pSrcB,
- uint32_t srcBLen,
- q31_t * pDst);
-
- /**
- * @brief Correlation of Q7 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
- * @return none.
- */
-
- void arm_correlate_q7(
- q7_t * pSrcA,
- uint32_t srcALen,
- q7_t * pSrcB,
- uint32_t srcBLen,
- q7_t * pDst);
-
- /**
- * @brief Instance structure for the floating-point sparse FIR filter.
- */
- typedef struct
- {
- uint16_t numTaps; /**< number of coefficients in the filter. */
- uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */
- float32_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
- float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
- uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */
- int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */
- } arm_fir_sparse_instance_f32;
-
- /**
- * @brief Instance structure for the Q31 sparse FIR filter.
- */
-
- typedef struct
- {
- uint16_t numTaps; /**< number of coefficients in the filter. */
- uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */
- q31_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
- q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
- uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */
- int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */
- } arm_fir_sparse_instance_q31;
-
- /**
- * @brief Instance structure for the Q15 sparse FIR filter.
- */
-
- typedef struct
- {
- uint16_t numTaps; /**< number of coefficients in the filter. */
- uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */
- q15_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
- q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
- uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */
- int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */
- } arm_fir_sparse_instance_q15;
-
- /**
- * @brief Instance structure for the Q7 sparse FIR filter.
- */
-
- typedef struct
- {
- uint16_t numTaps; /**< number of coefficients in the filter. */
- uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */
- q7_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
- q7_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
- uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */
- int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */
- } arm_fir_sparse_instance_q7;
-
- /**
- * @brief Processing function for the floating-point sparse FIR filter.
- * @param[in] *S points to an instance of the floating-point sparse FIR structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] *pScratchIn points to a temporary buffer of size blockSize.
- * @param[in] blockSize number of input samples to process per call.
- * @return none.
- */
-
- void arm_fir_sparse_f32(
- arm_fir_sparse_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- float32_t * pScratchIn,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the floating-point sparse FIR filter.
- * @param[in,out] *S points to an instance of the floating-point sparse FIR structure.
- * @param[in] numTaps number of nonzero coefficients in the filter.
- * @param[in] *pCoeffs points to the array of filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] *pTapDelay points to the array of offset times.
- * @param[in] maxDelay maximum offset time supported.
- * @param[in] blockSize number of samples that will be processed per block.
- * @return none
- */
-
- void arm_fir_sparse_init_f32(
- arm_fir_sparse_instance_f32 * S,
- uint16_t numTaps,
- float32_t * pCoeffs,
- float32_t * pState,
- int32_t * pTapDelay,
- uint16_t maxDelay,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the Q31 sparse FIR filter.
- * @param[in] *S points to an instance of the Q31 sparse FIR structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] *pScratchIn points to a temporary buffer of size blockSize.
- * @param[in] blockSize number of input samples to process per call.
- * @return none.
- */
-
- void arm_fir_sparse_q31(
- arm_fir_sparse_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- q31_t * pScratchIn,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the Q31 sparse FIR filter.
- * @param[in,out] *S points to an instance of the Q31 sparse FIR structure.
- * @param[in] numTaps number of nonzero coefficients in the filter.
- * @param[in] *pCoeffs points to the array of filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] *pTapDelay points to the array of offset times.
- * @param[in] maxDelay maximum offset time supported.
- * @param[in] blockSize number of samples that will be processed per block.
- * @return none
- */
-
- void arm_fir_sparse_init_q31(
- arm_fir_sparse_instance_q31 * S,
- uint16_t numTaps,
- q31_t * pCoeffs,
- q31_t * pState,
- int32_t * pTapDelay,
- uint16_t maxDelay,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the Q15 sparse FIR filter.
- * @param[in] *S points to an instance of the Q15 sparse FIR structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] *pScratchIn points to a temporary buffer of size blockSize.
- * @param[in] *pScratchOut points to a temporary buffer of size blockSize.
- * @param[in] blockSize number of input samples to process per call.
- * @return none.
- */
-
- void arm_fir_sparse_q15(
- arm_fir_sparse_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- q15_t * pScratchIn,
- q31_t * pScratchOut,
- uint32_t blockSize);
-
-
- /**
- * @brief Initialization function for the Q15 sparse FIR filter.
- * @param[in,out] *S points to an instance of the Q15 sparse FIR structure.
- * @param[in] numTaps number of nonzero coefficients in the filter.
- * @param[in] *pCoeffs points to the array of filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] *pTapDelay points to the array of offset times.
- * @param[in] maxDelay maximum offset time supported.
- * @param[in] blockSize number of samples that will be processed per block.
- * @return none
- */
-
- void arm_fir_sparse_init_q15(
- arm_fir_sparse_instance_q15 * S,
- uint16_t numTaps,
- q15_t * pCoeffs,
- q15_t * pState,
- int32_t * pTapDelay,
- uint16_t maxDelay,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the Q7 sparse FIR filter.
- * @param[in] *S points to an instance of the Q7 sparse FIR structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] *pScratchIn points to a temporary buffer of size blockSize.
- * @param[in] *pScratchOut points to a temporary buffer of size blockSize.
- * @param[in] blockSize number of input samples to process per call.
- * @return none.
- */
-
- void arm_fir_sparse_q7(
- arm_fir_sparse_instance_q7 * S,
- q7_t * pSrc,
- q7_t * pDst,
- q7_t * pScratchIn,
- q31_t * pScratchOut,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the Q7 sparse FIR filter.
- * @param[in,out] *S points to an instance of the Q7 sparse FIR structure.
- * @param[in] numTaps number of nonzero coefficients in the filter.
- * @param[in] *pCoeffs points to the array of filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] *pTapDelay points to the array of offset times.
- * @param[in] maxDelay maximum offset time supported.
- * @param[in] blockSize number of samples that will be processed per block.
- * @return none
- */
-
- void arm_fir_sparse_init_q7(
- arm_fir_sparse_instance_q7 * S,
- uint16_t numTaps,
- q7_t * pCoeffs,
- q7_t * pState,
- int32_t *pTapDelay,
- uint16_t maxDelay,
- uint32_t blockSize);
-
-
- /*
- * @brief Floating-point sin_cos function.
- * @param[in] theta input value in degrees
- * @param[out] *pSinVal points to the processed sine output.
- * @param[out] *pCosVal points to the processed cos output.
- * @return none.
- */
-
- void arm_sin_cos_f32(
- float32_t theta,
- float32_t *pSinVal,
- float32_t *pCcosVal);
-
- /*
- * @brief Q31 sin_cos function.
- * @param[in] theta scaled input value in degrees
- * @param[out] *pSinVal points to the processed sine output.
- * @param[out] *pCosVal points to the processed cosine output.
- * @return none.
- */
-
- void arm_sin_cos_q31(
- q31_t theta,
- q31_t *pSinVal,
- q31_t *pCosVal);
-
-
- /**
- * @brief Floating-point complex conjugate.
- * @param[in] *pSrc points to the input vector
- * @param[out] *pDst points to the output vector
- * @param[in] numSamples number of complex samples in each vector
- * @return none.
- */
-
- void arm_cmplx_conj_f32(
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t numSamples);
-
- /**
- * @brief Q31 complex conjugate.
- * @param[in] *pSrc points to the input vector
- * @param[out] *pDst points to the output vector
- * @param[in] numSamples number of complex samples in each vector
- * @return none.
- */
-
- void arm_cmplx_conj_q31(
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t numSamples);
-
- /**
- * @brief Q15 complex conjugate.
- * @param[in] *pSrc points to the input vector
- * @param[out] *pDst points to the output vector
- * @param[in] numSamples number of complex samples in each vector
- * @return none.
- */
-
- void arm_cmplx_conj_q15(
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t numSamples);
-
-
-
- /**
- * @brief Floating-point complex magnitude squared
- * @param[in] *pSrc points to the complex input vector
- * @param[out] *pDst points to the real output vector
- * @param[in] numSamples number of complex samples in the input vector
- * @return none.
- */
-
- void arm_cmplx_mag_squared_f32(
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t numSamples);
-
- /**
- * @brief Q31 complex magnitude squared
- * @param[in] *pSrc points to the complex input vector
- * @param[out] *pDst points to the real output vector
- * @param[in] numSamples number of complex samples in the input vector
- * @return none.
- */
-
- void arm_cmplx_mag_squared_q31(
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t numSamples);
-
- /**
- * @brief Q15 complex magnitude squared
- * @param[in] *pSrc points to the complex input vector
- * @param[out] *pDst points to the real output vector
- * @param[in] numSamples number of complex samples in the input vector
- * @return none.
- */
-
- void arm_cmplx_mag_squared_q15(
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t numSamples);
-
-
- /**
- * @ingroup groupController
- */
-
- /**
- * @defgroup PID PID Motor Control
- *
- * A Proportional Integral Derivative (PID) controller is a generic feedback control
- * loop mechanism widely used in industrial control systems.
- * A PID controller is the most commonly used type of feedback controller.
- *
- * This set of functions implements (PID) controllers
- * for Q15, Q31, and floating-point data types. The functions operate on a single sample
- * of data and each call to the function returns a single processed value.
- * S points to an instance of the PID control data structure. in
- * is the input sample value. The functions return the output value.
- *
- * \par Algorithm:
- *
- *
- * \par
- * where \c Kp is proportional constant, \c Ki is Integral constant and \c Kd is Derivative constant
- *
- * \par
- * \image html PID.gif "Proportional Integral Derivative Controller"
- *
- * \par
- * The PID controller calculates an "error" value as the difference between
- * the measured output and the reference input.
- * The controller attempts to minimize the error by adjusting the process control inputs.
- * The proportional value determines the reaction to the current error,
- * the integral value determines the reaction based on the sum of recent errors,
- * and the derivative value determines the reaction based on the rate at which the error has been changing.
- *
- * \par Instance Structure
- * The Gains A0, A1, A2 and state variables for a PID controller are stored together in an instance data structure.
- * A separate instance structure must be defined for each PID Controller.
- * There are separate instance structure declarations for each of the 3 supported data types.
- *
- * \par Reset Functions
- * There is also an associated reset function for each data type which clears the state array.
- *
- * \par Initialization Functions
- * There is also an associated initialization function for each data type.
- * The initialization function performs the following operations:
- * - Initializes the Gains A0, A1, A2 from Kp,Ki, Kd gains.
- * - Zeros out the values in the state buffer.
- *
- * \par
- * Instance structure cannot be placed into a const data section and it is recommended to use the initialization function.
- *
- * \par Fixed-Point Behavior
- * Care must be taken when using the fixed-point versions of the PID Controller functions.
- * In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
- * Refer to the function specific documentation below for usage guidelines.
- */
-
- /**
- * @addtogroup PID
- * @{
- */
-
- /**
- * @brief Process function for the floating-point PID Control.
- * @param[in,out] *S is an instance of the floating-point PID Control structure
- * @param[in] in input sample to process
- * @return out processed output sample.
- */
-
-
- static __INLINE float32_t arm_pid_f32(
- arm_pid_instance_f32 * S,
- float32_t in)
- {
- float32_t out;
-
- /* y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2] */
- out = (S->A0 * in) +
- (S->A1 * S->state[0]) + (S->A2 * S->state[1]) + (S->state[2]);
-
- /* Update state */
- S->state[1] = S->state[0];
- S->state[0] = in;
- S->state[2] = out;
-
- /* return to application */
- return (out);
-
- }
-
- /**
- * @brief Process function for the Q31 PID Control.
- * @param[in,out] *S points to an instance of the Q31 PID Control structure
- * @param[in] in input sample to process
- * @return out processed output sample.
- *
- * Scaling and Overflow Behavior:
- * \par
- * The function is implemented using an internal 64-bit accumulator.
- * The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
- * Thus, if the accumulator result overflows it wraps around rather than clip.
- * In order to avoid overflows completely the input signal must be scaled down by 2 bits as there are four additions.
- * After all multiply-accumulates are performed, the 2.62 accumulator is truncated to 1.32 format and then saturated to 1.31 format.
- */
-
- static __INLINE q31_t arm_pid_q31(
- arm_pid_instance_q31 * S,
- q31_t in)
- {
- q63_t acc;
- q31_t out;
-
- /* acc = A0 * x[n] */
- acc = (q63_t) S->A0 * in;
-
- /* acc += A1 * x[n-1] */
- acc += (q63_t) S->A1 * S->state[0];
-
- /* acc += A2 * x[n-2] */
- acc += (q63_t) S->A2 * S->state[1];
-
- /* convert output to 1.31 format to add y[n-1] */
- out = (q31_t) (acc >> 31u);
-
- /* out += y[n-1] */
- out += S->state[2];
-
- /* Update state */
- S->state[1] = S->state[0];
- S->state[0] = in;
- S->state[2] = out;
-
- /* return to application */
- return (out);
-
- }
-
- /**
- * @brief Process function for the Q15 PID Control.
- * @param[in,out] *S points to an instance of the Q15 PID Control structure
- * @param[in] in input sample to process
- * @return out processed output sample.
- *
- * Scaling and Overflow Behavior:
- * \par
- * The function is implemented using a 64-bit internal accumulator.
- * Both Gains and state variables are represented in 1.15 format and multiplications yield a 2.30 result.
- * The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
- * There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
- * After all additions have been performed, the accumulator is truncated to 34.15 format by discarding low 15 bits.
- * Lastly, the accumulator is saturated to yield a result in 1.15 format.
- */
-
- static __INLINE q15_t arm_pid_q15(
- arm_pid_instance_q15 * S,
- q15_t in)
- {
- q63_t acc;
- q15_t out;
-
- /* Implementation of PID controller */
-
- #ifdef ARM_MATH_CM0
-
- /* acc = A0 * x[n] */
- acc = ((q31_t) S->A0 )* in ;
-
- #else
-
- /* acc = A0 * x[n] */
- acc = (q31_t) __SMUAD(S->A0, in);
-
- #endif
-
- #ifdef ARM_MATH_CM0
-
- /* acc += A1 * x[n-1] + A2 * x[n-2] */
- acc += (q31_t) S->A1 * S->state[0] ;
- acc += (q31_t) S->A2 * S->state[1] ;
-
- #else
-
- /* acc += A1 * x[n-1] + A2 * x[n-2] */
- acc = __SMLALD(S->A1, (q31_t)__SIMD32(S->state), acc);
-
- #endif
-
- /* acc += y[n-1] */
- acc += (q31_t) S->state[2] << 15;
-
- /* saturate the output */
- out = (q15_t) (__SSAT((acc >> 15), 16));
-
- /* Update state */
- S->state[1] = S->state[0];
- S->state[0] = in;
- S->state[2] = out;
-
- /* return to application */
- return (out);
-
- }
-
- /**
- * @} end of PID group
- */
-
-
- /**
- * @brief Floating-point matrix inverse.
- * @param[in] *src points to the instance of the input floating-point matrix structure.
- * @param[out] *dst points to the instance of the output floating-point matrix structure.
- * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match.
- * If the input matrix is singular (does not have an inverse), then the algorithm terminates and returns error status ARM_MATH_SINGULAR.
- */
-
- arm_status arm_mat_inverse_f32(
- const arm_matrix_instance_f32 * src,
- arm_matrix_instance_f32 * dst);
-
-
-
- /**
- * @ingroup groupController
- */
-
-
- /**
- * @defgroup clarke Vector Clarke Transform
- * Forward Clarke transform converts the instantaneous stator phases into a two-coordinate time invariant vector.
- * Generally the Clarke transform uses three-phase currents Ia, Ib and Ic to calculate currents
- * in the two-phase orthogonal stator axis Ialpha and Ibeta.
- * When Ialpha is superposed with Ia as shown in the figure below
- * \image html clarke.gif Stator current space vector and its components in (a,b).
- * and Ia + Ib + Ic = 0, in this condition Ialpha and Ibeta
- * can be calculated using only Ia and Ib.
- *
- * The function operates on a single sample of data and each call to the function returns the processed output.
- * The library provides separate functions for Q31 and floating-point data types.
- * \par Algorithm
- * \image html clarkeFormula.gif
- * where Ia and Ib are the instantaneous stator phases and
- * pIalpha and pIbeta are the two coordinates of time invariant vector.
- * \par Fixed-Point Behavior
- * Care must be taken when using the Q31 version of the Clarke transform.
- * In particular, the overflow and saturation behavior of the accumulator used must be considered.
- * Refer to the function specific documentation below for usage guidelines.
- */
-
- /**
- * @addtogroup clarke
- * @{
- */
-
- /**
- *
- * @brief Floating-point Clarke transform
- * @param[in] Ia input three-phase coordinate a
- * @param[in] Ib input three-phase coordinate b
- * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha
- * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta
- * @return none.
- */
-
- static __INLINE void arm_clarke_f32(
- float32_t Ia,
- float32_t Ib,
- float32_t * pIalpha,
- float32_t * pIbeta)
- {
- /* Calculate pIalpha using the equation, pIalpha = Ia */
- *pIalpha = Ia;
-
- /* Calculate pIbeta using the equation, pIbeta = (1/sqrt(3)) * Ia + (2/sqrt(3)) * Ib */
- *pIbeta = ((float32_t) 0.57735026919 * Ia + (float32_t) 1.15470053838 * Ib);
-
- }
-
- /**
- * @brief Clarke transform for Q31 version
- * @param[in] Ia input three-phase coordinate a
- * @param[in] Ib input three-phase coordinate b
- * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha
- * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta
- * @return none.
- *
- * Scaling and Overflow Behavior:
- * \par
- * The function is implemented using an internal 32-bit accumulator.
- * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
- * There is saturation on the addition, hence there is no risk of overflow.
- */
-
- static __INLINE void arm_clarke_q31(
- q31_t Ia,
- q31_t Ib,
- q31_t * pIalpha,
- q31_t * pIbeta)
- {
- q31_t product1, product2; /* Temporary variables used to store intermediate results */
-
- /* Calculating pIalpha from Ia by equation pIalpha = Ia */
- *pIalpha = Ia;
-
- /* Intermediate product is calculated by (1/(sqrt(3)) * Ia) */
- product1 = (q31_t) (((q63_t) Ia * 0x24F34E8B) >> 30);
-
- /* Intermediate product is calculated by (2/sqrt(3) * Ib) */
- product2 = (q31_t) (((q63_t) Ib * 0x49E69D16) >> 30);
-
- /* pIbeta is calculated by adding the intermediate products */
- *pIbeta = __QADD(product1, product2);
- }
-
- /**
- * @} end of clarke group
- */
-
- /**
- * @brief Converts the elements of the Q7 vector to Q31 vector.
- * @param[in] *pSrc input pointer
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_q7_to_q31(
- q7_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
-
-
-
- /**
- * @ingroup groupController
- */
-
- /**
- * @defgroup inv_clarke Vector Inverse Clarke Transform
- * Inverse Clarke transform converts the two-coordinate time invariant vector into instantaneous stator phases.
- *
- * The function operates on a single sample of data and each call to the function returns the processed output.
- * The library provides separate functions for Q31 and floating-point data types.
- * \par Algorithm
- * \image html clarkeInvFormula.gif
- * where pIa and pIb are the instantaneous stator phases and
- * Ialpha and Ibeta are the two coordinates of time invariant vector.
- * \par Fixed-Point Behavior
- * Care must be taken when using the Q31 version of the Clarke transform.
- * In particular, the overflow and saturation behavior of the accumulator used must be considered.
- * Refer to the function specific documentation below for usage guidelines.
- */
-
- /**
- * @addtogroup inv_clarke
- * @{
- */
-
- /**
- * @brief Floating-point Inverse Clarke transform
- * @param[in] Ialpha input two-phase orthogonal vector axis alpha
- * @param[in] Ibeta input two-phase orthogonal vector axis beta
- * @param[out] *pIa points to output three-phase coordinate a
- * @param[out] *pIb points to output three-phase coordinate b
- * @return none.
- */
-
-
- static __INLINE void arm_inv_clarke_f32(
- float32_t Ialpha,
- float32_t Ibeta,
- float32_t * pIa,
- float32_t * pIb)
- {
- /* Calculating pIa from Ialpha by equation pIa = Ialpha */
- *pIa = Ialpha;
-
- /* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */
- *pIb = -0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;
-
- }
-
- /**
- * @brief Inverse Clarke transform for Q31 version
- * @param[in] Ialpha input two-phase orthogonal vector axis alpha
- * @param[in] Ibeta input two-phase orthogonal vector axis beta
- * @param[out] *pIa points to output three-phase coordinate a
- * @param[out] *pIb points to output three-phase coordinate b
- * @return none.
- *
- * Scaling and Overflow Behavior:
- * \par
- * The function is implemented using an internal 32-bit accumulator.
- * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
- * There is saturation on the subtraction, hence there is no risk of overflow.
- */
-
- static __INLINE void arm_inv_clarke_q31(
- q31_t Ialpha,
- q31_t Ibeta,
- q31_t * pIa,
- q31_t * pIb)
- {
- q31_t product1, product2; /* Temporary variables used to store intermediate results */
-
- /* Calculating pIa from Ialpha by equation pIa = Ialpha */
- *pIa = Ialpha;
-
- /* Intermediate product is calculated by (1/(2*sqrt(3)) * Ia) */
- product1 = (q31_t) (((q63_t) (Ialpha) * (0x40000000)) >> 31);
-
- /* Intermediate product is calculated by (1/sqrt(3) * pIb) */
- product2 = (q31_t) (((q63_t) (Ibeta) * (0x6ED9EBA1)) >> 31);
-
- /* pIb is calculated by subtracting the products */
- *pIb = __QSUB(product2, product1);
-
- }
-
- /**
- * @} end of inv_clarke group
- */
-
- /**
- * @brief Converts the elements of the Q7 vector to Q15 vector.
- * @param[in] *pSrc input pointer
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_q7_to_q15(
- q7_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
-
-
- /**
- * @ingroup groupController
- */
-
- /**
- * @defgroup park Vector Park Transform
- *
- * Forward Park transform converts the input two-coordinate vector to flux and torque components.
- * The Park transform can be used to realize the transformation of the Ialpha and the Ibeta currents
- * from the stationary to the moving reference frame and control the spatial relationship between
- * the stator vector current and rotor flux vector.
- * If we consider the d axis aligned with the rotor flux, the diagram below shows the
- * current vector and the relationship from the two reference frames:
- * \image html park.gif "Stator current space vector and its component in (a,b) and in the d,q rotating reference frame"
- *
- * The function operates on a single sample of data and each call to the function returns the processed output.
- * The library provides separate functions for Q31 and floating-point data types.
- * \par Algorithm
- * \image html parkFormula.gif
- * where Ialpha and Ibeta are the stator vector components,
- * pId and pIq are rotor vector components and cosVal and sinVal are the
- * cosine and sine values of theta (rotor flux position).
- * \par Fixed-Point Behavior
- * Care must be taken when using the Q31 version of the Park transform.
- * In particular, the overflow and saturation behavior of the accumulator used must be considered.
- * Refer to the function specific documentation below for usage guidelines.
- */
-
- /**
- * @addtogroup park
- * @{
- */
-
- /**
- * @brief Floating-point Park transform
- * @param[in] Ialpha input two-phase vector coordinate alpha
- * @param[in] Ibeta input two-phase vector coordinate beta
- * @param[out] *pId points to output rotor reference frame d
- * @param[out] *pIq points to output rotor reference frame q
- * @param[in] sinVal sine value of rotation angle theta
- * @param[in] cosVal cosine value of rotation angle theta
- * @return none.
- *
- * The function implements the forward Park transform.
- *
- */
-
- static __INLINE void arm_park_f32(
- float32_t Ialpha,
- float32_t Ibeta,
- float32_t * pId,
- float32_t * pIq,
- float32_t sinVal,
- float32_t cosVal)
- {
- /* Calculate pId using the equation, pId = Ialpha * cosVal + Ibeta * sinVal */
- *pId = Ialpha * cosVal + Ibeta * sinVal;
-
- /* Calculate pIq using the equation, pIq = - Ialpha * sinVal + Ibeta * cosVal */
- *pIq = -Ialpha * sinVal + Ibeta * cosVal;
-
- }
-
- /**
- * @brief Park transform for Q31 version
- * @param[in] Ialpha input two-phase vector coordinate alpha
- * @param[in] Ibeta input two-phase vector coordinate beta
- * @param[out] *pId points to output rotor reference frame d
- * @param[out] *pIq points to output rotor reference frame q
- * @param[in] sinVal sine value of rotation angle theta
- * @param[in] cosVal cosine value of rotation angle theta
- * @return none.
- *
- * Scaling and Overflow Behavior:
- * \par
- * The function is implemented using an internal 32-bit accumulator.
- * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
- * There is saturation on the addition and subtraction, hence there is no risk of overflow.
- */
-
-
- static __INLINE void arm_park_q31(
- q31_t Ialpha,
- q31_t Ibeta,
- q31_t * pId,
- q31_t * pIq,
- q31_t sinVal,
- q31_t cosVal)
- {
- q31_t product1, product2; /* Temporary variables used to store intermediate results */
- q31_t product3, product4; /* Temporary variables used to store intermediate results */
-
- /* Intermediate product is calculated by (Ialpha * cosVal) */
- product1 = (q31_t) (((q63_t) (Ialpha) * (cosVal)) >> 31);
-
- /* Intermediate product is calculated by (Ibeta * sinVal) */
- product2 = (q31_t) (((q63_t) (Ibeta) * (sinVal)) >> 31);
-
-
- /* Intermediate product is calculated by (Ialpha * sinVal) */
- product3 = (q31_t) (((q63_t) (Ialpha) * (sinVal)) >> 31);
-
- /* Intermediate product is calculated by (Ibeta * cosVal) */
- product4 = (q31_t) (((q63_t) (Ibeta) * (cosVal)) >> 31);
-
- /* Calculate pId by adding the two intermediate products 1 and 2 */
- *pId = __QADD(product1, product2);
-
- /* Calculate pIq by subtracting the two intermediate products 3 from 4 */
- *pIq = __QSUB(product4, product3);
- }
-
- /**
- * @} end of park group
- */
-
- /**
- * @brief Converts the elements of the Q7 vector to floating-point vector.
- * @param[in] *pSrc is input pointer
- * @param[out] *pDst is output pointer
- * @param[in] blockSize is the number of samples to process
- * @return none.
- */
- void arm_q7_to_float(
- q7_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @ingroup groupController
- */
-
- /**
- * @defgroup inv_park Vector Inverse Park transform
- * Inverse Park transform converts the input flux and torque components to two-coordinate vector.
- *
- * The function operates on a single sample of data and each call to the function returns the processed output.
- * The library provides separate functions for Q31 and floating-point data types.
- * \par Algorithm
- * \image html parkInvFormula.gif
- * where pIalpha and pIbeta are the stator vector components,
- * Id and Iq are rotor vector components and cosVal and sinVal are the
- * cosine and sine values of theta (rotor flux position).
- * \par Fixed-Point Behavior
- * Care must be taken when using the Q31 version of the Park transform.
- * In particular, the overflow and saturation behavior of the accumulator used must be considered.
- * Refer to the function specific documentation below for usage guidelines.
- */
-
- /**
- * @addtogroup inv_park
- * @{
- */
-
- /**
- * @brief Floating-point Inverse Park transform
- * @param[in] Id input coordinate of rotor reference frame d
- * @param[in] Iq input coordinate of rotor reference frame q
- * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha
- * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta
- * @param[in] sinVal sine value of rotation angle theta
- * @param[in] cosVal cosine value of rotation angle theta
- * @return none.
- */
-
- static __INLINE void arm_inv_park_f32(
- float32_t Id,
- float32_t Iq,
- float32_t * pIalpha,
- float32_t * pIbeta,
- float32_t sinVal,
- float32_t cosVal)
- {
- /* Calculate pIalpha using the equation, pIalpha = Id * cosVal - Iq * sinVal */
- *pIalpha = Id * cosVal - Iq * sinVal;
-
- /* Calculate pIbeta using the equation, pIbeta = Id * sinVal + Iq * cosVal */
- *pIbeta = Id * sinVal + Iq * cosVal;
-
- }
-
-
- /**
- * @brief Inverse Park transform for Q31 version
- * @param[in] Id input coordinate of rotor reference frame d
- * @param[in] Iq input coordinate of rotor reference frame q
- * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha
- * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta
- * @param[in] sinVal sine value of rotation angle theta
- * @param[in] cosVal cosine value of rotation angle theta
- * @return none.
- *
- * Scaling and Overflow Behavior:
- * \par
- * The function is implemented using an internal 32-bit accumulator.
- * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
- * There is saturation on the addition, hence there is no risk of overflow.
- */
-
-
- static __INLINE void arm_inv_park_q31(
- q31_t Id,
- q31_t Iq,
- q31_t * pIalpha,
- q31_t * pIbeta,
- q31_t sinVal,
- q31_t cosVal)
- {
- q31_t product1, product2; /* Temporary variables used to store intermediate results */
- q31_t product3, product4; /* Temporary variables used to store intermediate results */
-
- /* Intermediate product is calculated by (Id * cosVal) */
- product1 = (q31_t) (((q63_t) (Id) * (cosVal)) >> 31);
-
- /* Intermediate product is calculated by (Iq * sinVal) */
- product2 = (q31_t) (((q63_t) (Iq) * (sinVal)) >> 31);
-
-
- /* Intermediate product is calculated by (Id * sinVal) */
- product3 = (q31_t) (((q63_t) (Id) * (sinVal)) >> 31);
-
- /* Intermediate product is calculated by (Iq * cosVal) */
- product4 = (q31_t) (((q63_t) (Iq) * (cosVal)) >> 31);
-
- /* Calculate pIalpha by using the two intermediate products 1 and 2 */
- *pIalpha = __QSUB(product1, product2);
-
- /* Calculate pIbeta by using the two intermediate products 3 and 4 */
- *pIbeta = __QADD(product4, product3);
-
- }
-
- /**
- * @} end of Inverse park group
- */
-
-
- /**
- * @brief Converts the elements of the Q31 vector to floating-point vector.
- * @param[in] *pSrc is input pointer
- * @param[out] *pDst is output pointer
- * @param[in] blockSize is the number of samples to process
- * @return none.
- */
- void arm_q31_to_float(
- q31_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @ingroup groupInterpolation
- */
-
- /**
- * @defgroup LinearInterpolate Linear Interpolation
- *
- * Linear interpolation is a method of curve fitting using linear polynomials.
- * Linear interpolation works by effectively drawing a straight line between two neighboring samples and returning the appropriate point along that line
- *
- * \par
- * \image html LinearInterp.gif "Linear interpolation"
- *
- * \par
- * A Linear Interpolate function calculates an output value(y), for the input(x)
- * using linear interpolation of the input values x0, x1( nearest input values) and the output values y0 and y1(nearest output values)
- *
- * \par Algorithm:
- *
- * y = y0 + (x - x0) * ((y1 - y0)/(x1-x0))
- * where x0, x1 are nearest values of input x
- * y0, y1 are nearest values to output y
- *
- *
- * \par
- * This set of functions implements Linear interpolation process
- * for Q7, Q15, Q31, and floating-point data types. The functions operate on a single
- * sample of data and each call to the function returns a single processed value.
- * S points to an instance of the Linear Interpolate function data structure.
- * x is the input sample value. The functions returns the output value.
- *
- * \par
- * if x is outside of the table boundary, Linear interpolation returns first value of the table
- * if x is below input range and returns last value of table if x is above range.
- */
-
- /**
- * @addtogroup LinearInterpolate
- * @{
- */
-
- /**
- * @brief Process function for the floating-point Linear Interpolation Function.
- * @param[in,out] *S is an instance of the floating-point Linear Interpolation structure
- * @param[in] x input sample to process
- * @return y processed output sample.
- *
- */
-
- static __INLINE float32_t arm_linear_interp_f32(
- arm_linear_interp_instance_f32 * S,
- float32_t x)
- {
-
- float32_t y;
- float32_t x0, x1; /* Nearest input values */
- float32_t y0, y1; /* Nearest output values */
- float32_t xSpacing = S->xSpacing; /* spacing between input values */
- int32_t i; /* Index variable */
- float32_t *pYData = S->pYData; /* pointer to output table */
-
- /* Calculation of index */
- i = (x - S->x1) / xSpacing;
-
- if(i < 0)
- {
- /* Iniatilize output for below specified range as least output value of table */
- y = pYData[0];
- }
- else if(i >= S->nValues)
- {
- /* Iniatilize output for above specified range as last output value of table */
- y = pYData[S->nValues-1];
- }
- else
- {
- /* Calculation of nearest input values */
- x0 = S->x1 + i * xSpacing;
- x1 = S->x1 + (i +1) * xSpacing;
-
- /* Read of nearest output values */
- y0 = pYData[i];
- y1 = pYData[i + 1];
-
- /* Calculation of output */
- y = y0 + (x - x0) * ((y1 - y0)/(x1-x0));
-
- }
-
- /* returns output value */
- return (y);
- }
-
- /**
- *
- * @brief Process function for the Q31 Linear Interpolation Function.
- * @param[in] *pYData pointer to Q31 Linear Interpolation table
- * @param[in] x input sample to process
- * @param[in] nValues number of table values
- * @return y processed output sample.
- *
- * \par
- * Input sample x is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
- * This function can support maximum of table size 2^12.
- *
- */
-
-
- static __INLINE q31_t arm_linear_interp_q31(q31_t *pYData,
- q31_t x, uint32_t nValues)
- {
- q31_t y; /* output */
- q31_t y0, y1; /* Nearest output values */
- q31_t fract; /* fractional part */
- int32_t index; /* Index to read nearest output values */
-
- /* Input is in 12.20 format */
- /* 12 bits for the table index */
- /* Index value calculation */
- index = ((x & 0xFFF00000) >> 20);
-
- if(index >= (nValues - 1))
- {
- return(pYData[nValues - 1]);
- }
- else if(index < 0)
- {
- return(pYData[0]);
- }
- else
- {
-
- /* 20 bits for the fractional part */
- /* shift left by 11 to keep fract in 1.31 format */
- fract = (x & 0x000FFFFF) << 11;
-
- /* Read two nearest output values from the index in 1.31(q31) format */
- y0 = pYData[index];
- y1 = pYData[index + 1u];
-
- /* Calculation of y0 * (1-fract) and y is in 2.30 format */
- y = ((q31_t) ((q63_t) y0 * (0x7FFFFFFF - fract) >> 32));
-
- /* Calculation of y0 * (1-fract) + y1 *fract and y is in 2.30 format */
- y += ((q31_t) (((q63_t) y1 * fract) >> 32));
-
- /* Convert y to 1.31 format */
- return (y << 1u);
-
- }
-
- }
-
- /**
- *
- * @brief Process function for the Q15 Linear Interpolation Function.
- * @param[in] *pYData pointer to Q15 Linear Interpolation table
- * @param[in] x input sample to process
- * @param[in] nValues number of table values
- * @return y processed output sample.
- *
- * \par
- * Input sample x is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
- * This function can support maximum of table size 2^12.
- *
- */
-
-
- static __INLINE q15_t arm_linear_interp_q15(q15_t *pYData, q31_t x, uint32_t nValues)
- {
- q63_t y; /* output */
- q15_t y0, y1; /* Nearest output values */
- q31_t fract; /* fractional part */
- int32_t index; /* Index to read nearest output values */
-
- /* Input is in 12.20 format */
- /* 12 bits for the table index */
- /* Index value calculation */
- index = ((x & 0xFFF00000) >> 20u);
-
- if(index >= (nValues - 1))
- {
- return(pYData[nValues - 1]);
- }
- else if(index < 0)
- {
- return(pYData[0]);
- }
- else
- {
- /* 20 bits for the fractional part */
- /* fract is in 12.20 format */
- fract = (x & 0x000FFFFF);
-
- /* Read two nearest output values from the index */
- y0 = pYData[index];
- y1 = pYData[index + 1u];
-
- /* Calculation of y0 * (1-fract) and y is in 13.35 format */
- y = ((q63_t) y0 * (0xFFFFF - fract));
-
- /* Calculation of (y0 * (1-fract) + y1 * fract) and y is in 13.35 format */
- y += ((q63_t) y1 * (fract));
-
- /* convert y to 1.15 format */
- return (y >> 20);
- }
-
-
- }
-
- /**
- *
- * @brief Process function for the Q7 Linear Interpolation Function.
- * @param[in] *pYData pointer to Q7 Linear Interpolation table
- * @param[in] x input sample to process
- * @param[in] nValues number of table values
- * @return y processed output sample.
- *
- * \par
- * Input sample x is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
- * This function can support maximum of table size 2^12.
- */
-
-
- static __INLINE q7_t arm_linear_interp_q7(q7_t *pYData, q31_t x, uint32_t nValues)
- {
- q31_t y; /* output */
- q7_t y0, y1; /* Nearest output values */
- q31_t fract; /* fractional part */
- int32_t index; /* Index to read nearest output values */
-
- /* Input is in 12.20 format */
- /* 12 bits for the table index */
- /* Index value calculation */
- index = ((x & 0xFFF00000) >> 20u);
-
-
- if(index >= (nValues - 1))
- {
- return(pYData[nValues - 1]);
- }
- else if(index < 0)
- {
- return(pYData[0]);
- }
- else
- {
-
- /* 20 bits for the fractional part */
- /* fract is in 12.20 format */
- fract = (x & 0x000FFFFF);
-
- /* Read two nearest output values from the index and are in 1.7(q7) format */
- y0 = pYData[index];
- y1 = pYData[index + 1u];
-
- /* Calculation of y0 * (1-fract ) and y is in 13.27(q27) format */
- y = ((y0 * (0xFFFFF - fract)));
-
- /* Calculation of y1 * fract + y0 * (1-fract) and y is in 13.27(q27) format */
- y += (y1 * fract);
-
- /* convert y to 1.7(q7) format */
- return (y >> 20u);
-
- }
-
- }
- /**
- * @} end of LinearInterpolate group
- */
-
- /**
- * @brief Fast approximation to the trigonometric sine function for floating-point data.
- * @param[in] x input value in radians.
- * @return sin(x).
- */
-
- float32_t arm_sin_f32(
- float32_t x);
-
- /**
- * @brief Fast approximation to the trigonometric sine function for Q31 data.
- * @param[in] x Scaled input value in radians.
- * @return sin(x).
- */
-
- q31_t arm_sin_q31(
- q31_t x);
-
- /**
- * @brief Fast approximation to the trigonometric sine function for Q15 data.
- * @param[in] x Scaled input value in radians.
- * @return sin(x).
- */
-
- q15_t arm_sin_q15(
- q15_t x);
-
- /**
- * @brief Fast approximation to the trigonometric cosine function for floating-point data.
- * @param[in] x input value in radians.
- * @return cos(x).
- */
-
- float32_t arm_cos_f32(
- float32_t x);
-
- /**
- * @brief Fast approximation to the trigonometric cosine function for Q31 data.
- * @param[in] x Scaled input value in radians.
- * @return cos(x).
- */
-
- q31_t arm_cos_q31(
- q31_t x);
-
- /**
- * @brief Fast approximation to the trigonometric cosine function for Q15 data.
- * @param[in] x Scaled input value in radians.
- * @return cos(x).
- */
-
- q15_t arm_cos_q15(
- q15_t x);
-
-
- /**
- * @ingroup groupFastMath
- */
-
-
- /**
- * @defgroup SQRT Square Root
- *
- * Computes the square root of a number.
- * There are separate functions for Q15, Q31, and floating-point data types.
- * The square root function is computed using the Newton-Raphson algorithm.
- * This is an iterative algorithm of the form:
- *
- * x1 = x0 - f(x0)/f'(x0)
- *
- * where x1 is the current estimate,
- * x0 is the previous estimate and
- * f'(x0) is the derivative of f() evaluated at x0.
- * For the square root function, the algorithm reduces to:
- *
- *
- * \par
- * where numRows specifies the number of rows in the table;
- * numCols specifies the number of columns in the table;
- * and pData points to an array of size numRows*numCols values.
- * The data table pTable is organized in row order and the supplied data values fall on integer indexes.
- * That is, table element (x,y) is located at pTable[x + y*numCols] where x and y are integers.
- *
- * \par
- * Let (x, y) specify the desired interpolation point. Then define:
- *
- * XF = floor(x)
- * YF = floor(y)
- *
- * \par
- * The interpolated output point is computed as:
- *