/**
* Tells the robot to start driving at the specified speed, with the specified
* driving radius to the left. Note that MAX_INT16 and MIN_INT16 have it
* drive as straight as possible, but it's still not entirely straight.
*/
void cm_drive(const int16_t &speed, const int16_t &radius)
{
sendByte(CmdDrive);
padCommand();
sendWord(speed);
padCommand();
sendWord(radius);
endCommand();
}
/**
* Tells the robot to start driving by specifying the driving speed of each wheel.
*/
void cm_direct_drive(const int16_t &right_speed, const int16_t &left_speed)
{
sendByte(CmdDriveWheels);
padCommand();
sendWord(right_speed);
padCommand();
sendWord(left_speed);
endCommand();
}
static void sendASIDPool(unsigned long address)
{
unsigned long i;
pde_t **start = (pde_t **)address;
for (i = 0; i < ASID_POOL_READ_SIZE; i++) {
pde_t *pde = start[i];
if (pde != 0) {
sendWord(i);
sendWord((unsigned long)pde);
}
}
}
static void sendPT(unsigned long address)
{
unsigned long i;
pte_t *start = (pte_t *)address;
for (i = 0; i < PT_READ_SIZE; i++) {
pte_t pte = start[i];
if (pte_get_page_base_address(pte) != 0 && pte_get_present(pte) && pte_get_super_user(pte)) {
sendWord(i);
sendWord(pte.words[0]);
}
}
}
static void sendEPQueue(unsigned long epptr)
{
tcb_t *current = (tcb_t *)endpoint_ptr_get_epQueue_head((endpoint_t *)epptr);
tcb_t *tail = (tcb_t *)endpoint_ptr_get_epQueue_tail((endpoint_t *)epptr);
if (current == 0) {
return;
}
while (current != tail) {
sendWord((unsigned long)current);
current = current->tcbEPNext;
}
sendWord((unsigned long)current);
}
void mveaudio_uncompress(short *buffer, unsigned char *data, int length)
{
int nCurOffsets[2];
int swath;
data += 4;
swath = getWord(&data) / 2;
nCurOffsets[0] = getWord(&data);
nCurOffsets[1] = getWord(&data);
sendWord(&buffer, nCurOffsets[0]);
sendWord(&buffer, nCurOffsets[1]);
processSwath(buffer, data, swath, nCurOffsets);
}
void LVP_enter(void)
{
LVP_Init(); // configure I/Os
LVP_M_LAT = SLAVE_RESET; // MCLR = Vil (GND)
delay(10);
sendWord(0x4850); // "MCHP" Lsb first
sendWord(0x4D43);
LVP_C_LAT = 1;
__delay_us(1);
LVP_C_LAT = 0;
__delay_us(1);
delay(5);
lvp_address = 0;
}
static void sendRunqueues(void)
{
unsigned int i;
sendWord((unsigned int)ksCurThread);
for (i = 0; i < NUM_READY_QUEUES; i++) {
tcb_t *current = ksReadyQueues[i].head;
if (current != 0) {
while (current != ksReadyQueues[i].end) {
sendWord((unsigned int)current);
current = current -> tcbSchedNext;
}
sendWord((unsigned int)current);
}
}
}
static void sendRunqueues(void)
{
word_t i;
sendWord((unsigned long)NODE_STATE(ksCurThread));
for (i = 0; i < NUM_READY_QUEUES; i++) {
tcb_t *current = NODE_STATE(ksReadyQueues[i]).head;
if (current != 0) {
while (current != NODE_STATE(ksReadyQueues[i]).end) {
sendWord((unsigned long)current);
current = current -> tcbSchedNext;
}
sendWord((unsigned long)current);
}
}
}
static void sendCNode(unsigned long address, unsigned int sizebits)
{
unsigned long i;
cte_t *start = (cte_t *)address;
for (i = 0; i < (1 << sizebits); i++) {
cap_t cap = start[i].cap;
if (cap_get_capType(cap) != cap_null_cap) {
cte_t *parent = getMDBParent(&start[i]);
sendWord(i);
sendWord(cap.words[0]);
sendWord(cap.words[1]);
sendWord((unsigned long)parent);
}
}
}
static void processSwath(short *fout, const unsigned char *data, int swath, int *offsets)
{
for (int i=0; i<swath; i++)
{
offsets[i&1] += audio_exp_table[data[i]];
sendWord(&fout, offsets[i&1]);
}
}
static void sendIOPT(unsigned int address, unsigned int level)
{
unsigned int i;
vtd_pte_t *start = (vtd_pte_t *)address;
for (i = 0; i < IO_PT_READ_SIZE; i++) {
vtd_pte_t vtd_pte = start[i];
if (vtd_pte_get_addr(vtd_pte) != 0) {
sendWord(i);
sendWord(vtd_pte.words[0]);
sendWord(vtd_pte.words[1]);
if (level == ia32KSnumIOPTLevels) {
sendWord(1);
} else {
sendWord(0);
}
}
}
}
static void sendPD(unsigned int address)
{
unsigned int i, exists;
pde_t *start = (pde_t *)address;
for (i = 0; i < PD_READ_SIZE; i++) {
pde_t pde = start[i];
exists = 0;
if (pde_get_pdeType(pde) == pde_pde_coarse && pde_pde_coarse_get_address(pde) != 0) {
exists = 1;
} else if (pde_get_pdeType(pde) == pde_pde_section && (pde_pde_section_get_address(pde) != 0 ||
pde_pde_section_get_AP(pde))) {
exists = 1;
}
if (exists != 0 && i < kernelBase >> pageBitsForSize(ARMSection)) {
sendWord(i);
sendWord(pde.words[0]);
}
}
}
static void sendIOPT(unsigned long address, unsigned int level)
{
unsigned long i;
vtd_pte_t *start = (vtd_pte_t *)address;
for (i = 0; i < IO_PT_READ_SIZE; i++) {
vtd_pte_t vtd_pte = start[i];
if (vtd_pte_get_addr(vtd_pte) != 0) {
sendWord(i);
sendWord(vtd_pte.words[0]);
#ifdef CONFIG_ARCH_IA32
sendWord(vtd_pte.words[1]);
#endif
if (level == x86KSnumIOPTLevels) {
sendWord(1);
} else {
sendWord(0);
}
}
}
}
static void sendPT(unsigned int address)
{
unsigned int i, exists;
pte_t *start = (pte_t *)address;
for (i = 0; i < PT_READ_SIZE; i++) {
pte_t pte = start[i];
exists = 0;
if (pte_get_pteType(pte) == pte_pte_large && (pte_pte_large_get_address(pte) != 0 ||
pte_pte_large_get_AP(pte))) {
exists = 1;
} else if (pte_get_pteType(pte) == pte_pte_small && (pte_pte_small_get_address(pte) != 0 ||
pte_pte_small_get_AP(pte))) {
exists = 1;
}
if (exists != 0) {
sendWord(i);
sendWord(pte.words[0]);
}
}
}
static void sendIOSpace(uint32_t pci_request_id)
{
uint32_t vtd_root_index;
uint32_t vtd_context_index;
vtd_rte_t* vtd_root_slot;
vtd_cte_t* vtd_context;
vtd_cte_t* vtd_context_slot;
vtd_root_index = get_pci_bus(pci_request_id);
vtd_root_slot = x86KSvtdRootTable + vtd_root_index;
vtd_context = (vtd_cte_t*)paddr_to_pptr(vtd_rte_ptr_get_ctp(vtd_root_slot));
vtd_context_index = (get_pci_dev(pci_request_id) << 3) | get_pci_fun(pci_request_id);
vtd_context_slot = &vtd_context[vtd_context_index];
if (vtd_cte_ptr_get_present(vtd_context_slot)) {
sendWord(vtd_cte_ptr_get_asr(vtd_context_slot));
} else {
sendWord(0);
}
}
static void sendPD(unsigned long address)
{
unsigned long i;
unsigned int exists;
pde_t *start = (pde_t *)address;
for (i = 0; i < PD_READ_SIZE; i++) {
pde_t pde = start[i];
exists = 1;
if (pde_get_page_size(pde) == pde_pde_small && (pde_pde_small_get_pt_base_address(pde) == 0 ||
!pde_pde_small_get_present(pde) || !pde_pde_small_get_super_user(pde))) {
exists = 0;
} else if (pde_get_page_size(pde) == pde_pde_large && (pde_pde_large_get_page_base_address(pde) == 0 ||
!pde_pde_large_get_present(pde) || !pde_pde_large_get_super_user(pde))) {
exists = 0;
}
if (exists != 0 && i < PPTR_BASE >> pageBitsForSize(X86_LargePage)) {
sendWord(i);
sendWord(pde.words[0]);
}
}
}
static void sendVersion(void)
{
sendWord(ARCH);
sendWord(CAPDL_VERSION);
}
void sendData(uint16_t w)
{
w = (w <<1) & 0x7ffe; // add start and stop bits
sendWord(w);
}
