X-Git-Url: http://pileus.org/git/?a=blobdiff_plain;f=yue%2Fmain.cpp;h=e580188dcce225a1caf375fed709594a5b036c5b;hb=aab601d3ceac5f1a8649f1eab285744373bdd8c8;hp=d3addb1e679d7b004e67ff01d77631c11b91a32b;hpb=12ef45a56c075d0587534a79451a999e9be51f8e;p=~andy%2Fcsm213a-hw

diff --git a/yue/main.cpp b/yue/main.cpp
index d3addb1..e580188 100644
--- a/yue/main.cpp
+++ b/yue/main.cpp
@@ -1,369 +1,457 @@
+#include <stdint.h>
+#include <stdarg.h>
+
 #include "mbed.h"
 #include "TSISensor.h"          // Touch Sensor
 #include "MAG3110.h"            // Magnetic Sensor
 #include "MMA8451Q.h"           // AcceleroMeter
-//#include "DataFrame.h"
-
-
 
-#define MMA8451_I2C_ADDRESS (0x1d<<1)   // acc sensor address
-#define TIME_ACCURACY     0.1
-#define LGT_SNS_DEFAULT   2             // default collecting interval in seconds
-#define ACC_SNS_DEFAULT   2
-#define MAG_SNS_DEFAULT   2
-#define TCH_SNS_DEFAULT   2
-
-// Parameters in Data Frame
-// TODO: need to sync with the pc program
+#include "serial_dma.h"         // AcceleroMeter
 
-#define HEADER    0x02
-
-#define SNS_BITS  5
-#define LGT_SNS   0x00
-#define ACC_SNS   0x01
-#define MAG_SNS   0x02
-#define TCH_SNS   0x03
-#define ADC_SNS_1 0x04
+//#include "DataFrame.h"
 
-#define INT    0
-#define LONG   1
-#define FLOAT  2
-#define DOUBLE 3
-#define TAIL   0x0A             // '\n'
+#define CLAMP(x, min, max)     \
+        ((x) < (min) ? (min) : \
+         (x) > (max) ? (max) : (x))
 
-// Command Frame
-#define START   0
-#define STOP    1
-#define SET_INT 2
+#define MMA8451_I2C_ADDRESS (0x1d<<1)   // acc sensor address
+#define TIME_ACCURACY     0.001
+
+#define ACC_SNS_DEFAULT   0.01 // default collecting interval in seconds
+#define MAG_SNS_DEFAULT   0.1
+#define LGT_SNS_DEFAULT   0.1
+#define TCH_SNS_DEFAULT   0.1
+#define A2D_SNS_DEFAULT   0.1
+#define LED_OUT_DEFAULT   0.5
+
+#define MIN_RATE          1E-4  // 1 kHz
+#define MAX_RATE          10.0  // 0.1 Hz
+
+// Common Frame Information
+#define HEADER            0x02
+#define TAIL              0x0A  // '\n'
+
+#define MAX_FRAME_SIZE    256
+
+#define SNS_NUM           5
+#define TYP_NUM           8
+#define CMD_NUM           3
+
+typedef enum {
+    SNS_ACC,
+    SNS_MAG,
+    SNS_LGT,
+    SNS_TCH,
+    SNS_A2D,
+} sns_t;
+
+typedef enum {
+    TYP_S8,  TYP_S16, TYP_S32,
+    TYP_U8,  TYP_U16, TYP_U32,
+    TYP_F32, TYP_F64,
+} type_t;
+
+typedef enum {
+    CMD_STOP,
+    CMD_START,
+    CMD_RATE,
+} cmd_t;
+
+// Data Frame Information
+#pragma pack(1)
+typedef struct {
+    uint8_t header;
+    struct {
+        uint8_t type : 4;
+        uint8_t sns  : 4;
+    } bits;
+    uint8_t count;
+    uint8_t data[];
+} state_t;
+#pragma pack()
+
+// Command Frame Information
+#pragma pack(1)
+typedef struct {
+    uint8_t header;
+    struct {
+        uint8_t cmd : 4;
+        uint8_t sns : 4;
+    } bits;
+    float   interval;
+} control_t;
+#pragma pack()
 
+// Define Devices & Pins
+MMA8451Q   accSensor(PTE25, PTE24, MMA8451_I2C_ADDRESS);
+MAG3110    magSensor(PTE25, PTE24);
+TSISensor  tchSensor;
+AnalogIn   lgtSensor(PTE22);
+AnalogIn   a2dSensor(A0);
 
+AnalogOut  a2dOutput(PTE30);
 
-#define MAX_FRAME_LEN 255
-// Define Devices & Pins
-MMA8451Q accSensor(PTE25, PTE24, MMA8451_I2C_ADDRESS);
-TSISensor touchSensor;
-MAG3110 magSensor(PTE25, PTE24);
-Serial serial(USBTX, USBRX);
-Ticker clock1;
-AnalogIn lightSensor(PTE22);
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
 
+Serial     serial(USBTX, USBRX);
+Ticker     clock1;
 
 // Global Variables
 // Initial interval: in seconds
-float lgtTmr = LGT_SNS_DEFAULT;
 float accTmr = ACC_SNS_DEFAULT;
 float magTmr = MAG_SNS_DEFAULT;
 float tchTmr = TCH_SNS_DEFAULT;
+float lgtTmr = LGT_SNS_DEFAULT;
+float a2dTmr = A2D_SNS_DEFAULT;
+float ledTmr = LED_OUT_DEFAULT;
 
-bool lgtEnable = false;
 bool accEnable = false;
-bool magEnable = true;
+bool magEnable = false;
+bool lgtEnable = false;
 bool tchEnable = false;
+bool a2dEnable = true;
+bool ledEnable = true;
 
-void clock1_interrupt();
-void serialRx_interrupt();
-void sendLightInfo();
-void sendAccInfo();
-void sendMagInfo();
-void sendTouchInfo();
-int packToFrame(char*, char, char, int, void*);
-char* floatToByte(float);
-float byteToFloat(char*);
-int calDataSize(char);
-void printHex(char*);
-void sendFrame(char*, int);
-
-
-int main() {
+bool useStr = false;
+bool useHex = false;
+bool useBin = true;
+
+uint8_t txFrame[MAX_FRAME_SIZE];
+uint8_t rxFrame[MAX_FRAME_SIZE];
+
+// Prototypes
+void clock1_interrupt(void);
+void serialRx_interrupt(void);
+
+void runCommand(control_t *cmd);
+
+void sendAccInfo(void);
+void sendMagInfo(void);
+void sendLgtInfo(void);
+void sendTchInfo(void);
+void sendA2dInfo(void);
+
+int  calDataSize(uint8_t);
+int  packToFrame(uint8_t *frame, sns_t snsType, type_t dataType, int dataNum, void *data);
+
+void printStr(const char *str, ...);
+void printHex(uint8_t *frame, int len);
+void printBin(uint8_t *frame, int len);
+
+/********
+ * Main *
+ ********/
+
+int main(void) {
     // Initialization
     // Interruption Declarations
     clock1.attach(&clock1_interrupt, TIME_ACCURACY);    // maximun accuracy be 0.1s
     serial.attach(&serialRx_interrupt, Serial::RxIrq);  // receive interrupt for serialS
+    serial.baud(115200);
     magSensor.begin();
 
-    serial.printf("\r\n============= Start of the program ============\r\n");
+    sdma_setup(UART0, 0);
+
+    printStr("\r\n============= Start of the program ============\r\n");
+
     while(1){
         wait(3);
     }
 }
 
-
-/*---------------------------------------------------------------
-  ## Receive Interruption of the Serial ##
-  -> used to receive & process user command
-  -> and configure the board accordingly
-  ---------------------------------------------------------------*/
-/*void serialRx_interrupt_userFriendlyVersion(){
-    clock1.detach();                // close the interrupt
-    serial.printf("\r\n");
-
-    // Receive the Serial Input
-    float interval;
-    char buffer[255];
-    char temp[255];
-    char ch = serial.getc();
-    int i;
-    for (i=0 ; ch!='\n' && ch!='\r'; i++){
-        serial.putc(ch);
-        buffer[i] = ch;
-        if (ch==127){                // BackSpace
-            i--;
-            i--;
-        }
-        ch = serial.getc();
-    }
-    buffer[i] = '\0';
-    serial.printf("\r\nBUFFER: %s %d\r\n", buffer,i);
-    // TODO: buffer -> lower case
-
-    // Process the Serial Input
-    // Set-Interval Command
-    if (strstr(buffer, "set")!=NULL && strstr(buffer, "int")!= NULL){
-        sscanf(buffer, "%*[^0123456789.]%s", temp);       // find the number in buffer
-        sscanf(temp, "%f", &interval);                    // translate into float
-        if (interval<0.1 || interval>5){
-            interval  = 1;
-        }
-        if (strstr(buffer, "acc")){
-            accTmr = interval;
-        }
-        if (strstr(buffer, "mag")){
-            magTmr = interval;
-        }
-        if (strstr(buffer, "light")){
-            lgtTmr = interval;
-        }
-        if (strstr(buffer, "touch")){
-            tchTmr = interval;
-        }
-    }
-
-    // Stop Command
-    else if (strstr(buffer, "stop")!= NULL){
-        serial.printf("STOP\r\n");
-        if (strstr(buffer, "acc")){
-            accEnable = false;
-            accTmr = ACC_SNS_DEFAULT;
-        }
-        if (strstr(buffer, "mag")){
-            magEnable = false;
-            magTmr = MAG_SNS_DEFAULT;
-        }
-        if (strstr(buffer, "light")){
-            lgtEnable = false;
-            lgtTmr = LGT_SNS_DEFAULT;
-        }
-        if (strstr(buffer, "touch")){
-            tchEnable = false;
-            tchTmr = TCH_SNS_DEFAULT;
-        }
-    }
-
-    // Start Command
-    else if (strstr(buffer, "start")!=NULL){
-        if (strstr(buffer, "acc") && !accEnable){
-            accEnable = true;
-            accTmr = ACC_SNS_DEFAULT;
-        }
-        if (strstr(buffer, "mag") && !magEnable){
-            magEnable = true;
-            magTmr = MAG_SNS_DEFAULT;
-        }
-        if (strstr(buffer, "light") && !lgtEnable){
-            lgtEnable = true;
-            lgtTmr = LGT_SNS_DEFAULT;
-        }
-        if (strstr(buffer, "touch") && !tchEnable){
-            tchEnable = true;
-            tchTmr = TCH_SNS_DEFAULT;
-        }
-    }
-    clock1.attach(&clock1_interrupt,TIME_ACCURACY);
-}
-*/
-
-void serialRx_interrupt(){                          // Byte version
-    clock1.detach();                                // close the interrupt temporarily
-    int i = 0;
-    char frame[MAX_FRAME_LEN];
-    char ch = serial.getc();
-    while (ch!=HEADER){
-        if (serial.readable()){
-            ch = serial.getc();
-        }
-        else{
-            serial.printf("[ERROR] broken data!\r\n");
-            clock1.attach(&clock1_interrupt,TIME_ACCURACY);
-            return;
-        }
-    }
-    //TODO: ticker for time out
-    while (serial.readable()){
-        frame[i] = ch;
-        if (ch=='\n'){
-            break;
-        }
-        ch = serial.getc();
-        i++;
-    }
-    frame[++i] = '\0';
-    float interval=-1;
-    char snsType = frame[1]|0x1F;
-    char oper = (frame[1]|0xE0)>>SNS_BITS;              // operation type
-    if (oper==SET_INT){
-        memcpy(&interval,&frame[2],4);
-        if (interval<0.0001||interval>10){
-            interval = 1;
-        }
-    }
-    switch(snsType){
-        case ACC_SNS:
-        {
-            switch(oper){
-                case START:     accEnable = true;  break;
-                case STOP:      accEnable = false; break;
-                case SET_INT:   accTmr = interval; break;
-            }
-        }
-        case MAG_SNS:
-        {
-            switch(oper){
-                case START:     magEnable = true;  break;
-                case STOP:      magEnable = false; break;
-                case SET_INT:   magTmr = interval; break;
-            }
-        }
-        case LGT_SNS:
-        {
-            switch(oper){
-                case START:     lgtEnable = true;  break;
-                case STOP:      lgtEnable = false; break;
-                case SET_INT:   lgtTmr = interval; break;
-            }
-        }
-        case TCH_SNS:
-        {
-            switch(oper){
-                case START:     tchEnable = true;  break;
-                case STOP:      tchEnable = false; break;
-                case SET_INT:   tchTmr = interval; break;
-            }
+/**********************
+ * Interrupt handlers *
+ **********************/
+
+void serialRx_interrupt(void) {                     // Byte version
+    static int state  = 0;
+    static int index  = 0;
+    static int length = 0;
+
+    control_t *cmd = (control_t*)rxFrame;
+
+    while (serial.readable()) {
+        rxFrame[index++] = serial.getc();
+        switch (state) {
+            case 0: // Header
+                if (cmd->header == HEADER)
+                    state = 1;
+                else
+                    index = 0;
+                break;
+            case 1: // Bits
+                if (cmd->bits.sns >= SNS_NUM ||
+                    cmd->bits.cmd >= CMD_NUM) {
+                    state = index = 0;
+                } else if (cmd->bits.cmd == CMD_RATE) {
+                    length = 4;
+                    state  = 2;
+                } else {
+                    state  = 3;
+                }
+                break;
+            case 2: // Data
+                if (--length == 0)
+                    state = 3;
+                break;
+            case 3: // Tail
+                if (rxFrame[index-1] == TAIL)
+                    runCommand(cmd);
+                state = 0;
+                index = 0;
+                break;
         }
     }
-    clock1.attach(&clock1_interrupt,TIME_ACCURACY);
 }
 
-void clock1_interrupt(){
+void clock1_interrupt(void){
     static int accCnt;
     static int magCnt;
     static int lgtCnt;
     static int tchCnt;
+    static int a2dCnt;
+    static int ledCnt;
 
     accCnt++;
     magCnt++;
     lgtCnt++;
     tchCnt++;
+    a2dCnt++;
+    ledCnt++;
 
-    // TODO: send data through Serial
-    if (lgtEnable && (lgtCnt<0 || lgtCnt>=lgtTmr/TIME_ACCURACY)){
-        sendLightInfo();
-        lgtCnt = 0;
+    // Write A2D output sine wave
+    a2dOutput.write(sin(a2dCnt * TIME_ACCURACY * (2*PI) * 0.1));
+
+    // Send data through Serial
+    if (accEnable && (accCnt<0 || accCnt>=accTmr/TIME_ACCURACY)){
+        sendAccInfo();
+        accCnt = 0;
     }
     if (magEnable && (magCnt<0 || magCnt>=magTmr/TIME_ACCURACY)){
         sendMagInfo();
         magCnt = 0;
     }
+    if (lgtEnable && (lgtCnt<0 || lgtCnt>=lgtTmr/TIME_ACCURACY)){
+        sendLgtInfo();
+        lgtCnt = 0;
+    }
     if (tchEnable && (tchCnt<0 || tchCnt>=tchTmr/TIME_ACCURACY)){
-        sendTouchInfo();
+        sendTchInfo();
         tchCnt = 0;
     }
-    if (accEnable && (accCnt<0 || accCnt>=accTmr/TIME_ACCURACY)){
-        sendAccInfo();
-        accCnt = 0;
+    if (a2dEnable && (a2dCnt<0 || a2dCnt>=a2dTmr/TIME_ACCURACY)){
+        sendA2dInfo();
+        a2dCnt = 0;
     }
-}
 
-void sendLightInfo(){
-    char frame[MAX_FRAME_LEN];
-    float lightData = lightSensor.read();
-    packToFrame(frame,LGT_SNS,FLOAT,1,&lightData);
-    serial.printf("[LGT] %s\r\n", frame);
-    //printHex(frame);
-    //serial.printf("[LGT] Light Intensity=%f\r\n", lightSensor.read());
+    // Toggel LED for debugging
+    if (ledEnable && (ledCnt<0 || ledCnt>=ledTmr/TIME_ACCURACY)){
+        led1   = !led1;
+	ledCnt = 0;
+    }
+
+    sdma_flush();
 }
 
+/*******************
+ * Command handler *
+ *******************/
 
-void sendTouchInfo(){
-    char frame[MAX_FRAME_LEN];
-    float touchData[2];
-    touchData[0] = touchSensor.readPercentage();
-    touchData[1] = touchSensor.readDistance();
-    packToFrame(frame,TCH_SNS,FLOAT,2,touchData);
-    serial.printf("[TCH] %s\r\n", frame);
-    //printHex(frame);
-}
+void runCommand(control_t *cmd)
+{
+    // Validate interval
+    sns_t snsType = (sns_t)cmd->bits.sns;
+    cmd_t cmdType = (cmd_t)cmd->bits.cmd;
+    float interval = CLAMP(cmd->interval, MIN_RATE, MAX_RATE);
+
+    // Save value to global data
+    switch(snsType){
+        case SNS_ACC:
+            switch(cmdType){
+                case CMD_STOP:   accEnable = false; break;
+                case CMD_START:  accEnable = true;  break;
+                case CMD_RATE:   accTmr = interval; break;
+            }
+            break;
+
+        case SNS_MAG:
+            switch(cmdType){
+                case CMD_STOP:   magEnable = false; break;
+                case CMD_START:  magEnable = true;  break;
+                case CMD_RATE:   magTmr = interval; break;
+            }
+            break;
+
+        case SNS_LGT:
+            switch(cmdType){
+                case CMD_STOP:   lgtEnable = false; break;
+                case CMD_START:  lgtEnable = true;  break;
+                case CMD_RATE:   lgtTmr = interval; break;
+            }
+            break;
+
+        case SNS_TCH:
+            switch(cmdType){
+                case CMD_STOP:   tchEnable = false; break;
+                case CMD_START:  tchEnable = true;  break;
+                case CMD_RATE:   tchTmr = interval; break;
+            }
+            break;
 
-void sendMagInfo(){
-    char frame[MAX_FRAME_LEN];
-    int magData[3];
-    magSensor.getValues(magData, magData+1, magData+2);
-    //serial.printf("[MAG] magX=%d magY=%d magZ=%d\r\n",magData[0],magData[1],magData[2]);
-    int len = packToFrame(frame,MAG_SNS,INT,3,magData);
-    sendFrame(frame, len);
-    //memcpy(magData, &frame[3], 2);
-    //serial.printf("[MAG] %d\r\n", magData[0]);
-    //printHex(frame);
+        case SNS_A2D:
+            switch(cmdType){
+                case CMD_STOP:   a2dEnable = false; break;
+                case CMD_START:  a2dEnable = true;  break;
+                case CMD_RATE:   a2dTmr = interval; break;
+            }
+            break;
+    }
 }
 
-void sendAccInfo(){
-    // get acc data
-    char frame[MAX_FRAME_LEN];
+/*******************
+ * Sensors reading *
+ *******************/
+
+void sendAccInfo(void){
     float accData[3];
     accData[0] = accSensor.getAccX();
     accData[1] = accSensor.getAccY();
     accData[2] = accSensor.getAccZ();
-    packToFrame(frame, ACC_SNS,FLOAT,3,accData);
-    serial.printf("[ACC] %s\n\r", frame);
-    //int num = int(frame[2]);
-    //memcpy(&accZ, &frame[3], 4);
-    // send acc data
-    //printHex(frame);
+    int len = packToFrame(txFrame, SNS_ACC, TYP_F32, 3, accData);
+
+    printStr("[ACC] accX=%-2.4f accY=%-2.4f accZ=%-2.4f\r\n",
+                    accData[0], accData[1], accData[2]);
+    printHex(txFrame, len);
+    printBin(txFrame, len);
+}
+
+void sendMagInfo(void){
+    // magSensor uses the wrong types,
+    // so we have to convert it
+    int tmp[3];
+    magSensor.getValues(&tmp[0], &tmp[1], &tmp[2]);
+    uint16_t magData[3];
+    magData[0] = tmp[0];
+    magData[1] = tmp[1];
+    magData[2] = tmp[2];
+    int len = packToFrame(txFrame, SNS_MAG, TYP_S16, 3, magData);
+
+    printStr("[MAG] magX=%hd magY=%hd magZ=%hd %d\r\n",
+                    magData[0], magData[1], magData[2], sizeof(int));
+    printHex(txFrame, len);
+    printBin(txFrame, len);
+}
+
+void sendLgtInfo(void){
+    float lgtData = lgtSensor.read();
+    int len = packToFrame(txFrame, SNS_LGT, TYP_F32, 1, &lgtData);
+
+    printStr("[LGT] intensity=%f\r\n",
+                    lgtSensor.read());
+    printHex(txFrame, len);
+    printBin(txFrame, len);
 }
-int calDataSize(char dataType){
+
+void sendTchInfo(void){
+    float tchData[2];
+    tchData[0] = tchSensor.readPercentage();
+    tchData[1] = tchSensor.readDistance();
+    int len = packToFrame(txFrame, SNS_TCH, TYP_F32, 2, tchData);
+
+    printStr("[TCH] force=%0.4f distance=%2.2f\r\n",
+                    tchData[0], tchData[1]);
+    printHex(txFrame, len);
+    printBin(txFrame, len);
+}
+
+void sendA2dInfo(void){
+    float a2dData[6];
+    a2dData[0] = a2dSensor.read();
+    a2dData[1] = 0;
+    a2dData[2] = 0;
+    a2dData[3] = 0;
+    a2dData[4] = 0;
+    a2dData[5] = 0;
+    int len = packToFrame(txFrame, SNS_A2D, TYP_F32, 6, a2dData);
+
+    printStr("[A2D] data=%2.2f %2.2f %2.2f %2.2f %2.2f %2.2f\r\n",
+                a2dData[0], a2dData[1], a2dData[2],
+                a2dData[3], a2dData[4], a2dData[5]);
+    printHex(txFrame, len);
+    printBin(txFrame, len);
+}
+
+/********************
+ * Helper functions *
+ ********************/
+
+int calDataSize(type_t dataType){
     switch(dataType){
-        case INT:    return 2;
-        case LONG:   return 4;
-        case FLOAT:  return 4;
-        case DOUBLE: return 8;
+        case TYP_S8:  return 1;
+        case TYP_S16: return 2;
+        case TYP_S32: return 4;
+        case TYP_U8:  return 1;
+        case TYP_U16: return 2;
+        case TYP_U32: return 4;
+        case TYP_F32: return 4;
+        case TYP_F64: return 8;
     }
     return 4;
 }
 
-void printHex(char* frame){
-    int dataType = frame[1]&0xE0;
-    int dataSize = calDataSize(dataType);
-    int dataNum = frame[2];
-    for (int i=0; i<4+dataSize*dataNum; i++){
-        serial.printf("0x%x ",frame[i]);
-    }
-    serial.printf("\r\n");
+/*******************
+ * Frame functions *
+ *******************/
+
+int packToFrame(uint8_t *frame, sns_t snsType, type_t dataType, int dataNum, void *data){
+    //const char pattern[] = "\x80\x81\x82\x83"
+    //                       "\x84\x85\x86\x87"
+    //                       "\x88\x89\x8A\x8B"
+    //                       "\x8C\x8D\x8E\x8F";
+
+    int      size  = dataNum * calDataSize(dataType);
+    state_t *state = (state_t*)frame;
+    uint8_t *tail  = &state->data[size];
+
+    state->header    = HEADER;
+    state->bits.sns  = snsType;
+    state->bits.type = dataType;
+    state->count     = dataNum;;
+    memcpy(&state->data, data, size);
+    tail[0]          = TAIL;
+    tail[1]          = '\0';
+
+    return (3 + size + 1);
 }
 
-int packToFrame(char* frame, char snsType,char dataType, int dataNum, void* data){
-    int dataSize = calDataSize(dataType);
-    frame[0] = HEADER;
-    frame[1] = (snsType|(dataType<<SNS_BITS));
-    frame[2] = dataNum;
-    memcpy(frame+3, data, dataSize*dataNum);
-    frame[3+dataNum*dataSize]= TAIL;
-    frame[4+dataNum*dataSize]= '\0';
-    frame[3] = 0x00;
-    return (4+dataNum*dataSize);
+/*******************
+ * Print functions *
+ *******************/
+
+void printHex(uint8_t *frame, int len){
+    if (!useHex)
+        return;
+
+    sdma_printf("      ");
+    for (int i=0; i<len; i++)
+        sdma_printf("%02hx ", frame[i]);
+    sdma_printf("\r\n");
 }
 
+void printStr(const char *fmt, ...){
+    if (!useStr)
+        return;
 
-void sendFrame (char* frame, int len){
-    for (int i=0; i<len; i++){
-        serial.putc(frame[i]);
-    }
+    va_list ap;
+    va_start(ap, fmt);
+    sdma_vprintf(fmt, ap);
+    va_end(ap);
+}
+
+void printBin(uint8_t *frame, int len){
+    if (!useBin)
+        return;
+
+    sdma_write(frame, len);
 }