TransferCore.cpp

00001 // TransferCore.cpp 2013/9/14
00002 #include "TransferCore.h"
00003 
00004 void transData::init(uint8_t* base, int pos)
00005 {
00006     _base = base;
00007     _pos = pos;
00008 }
00009 
00010 void transData::append(uint32_t data)
00011 {
00012     ST<uint32_t>(_base+_pos, data);
00013     _pos += sizeof(uint32_t);
00014 }
00015 
00016 uint8_t* transData::data()
00017 {
00018     return _base;
00019 }
00020 
00021 int transData::length()
00022 {
00023     return _pos;
00024 }
00025 
00026 int TransferCore::Transfer(SWD* swd, uint8_t* request, uint8_t* response) // 0x05
00027 {
00028     _swd = swd;
00029     reqData.init(request, 2);
00030     resData.init(response, 3);
00031     post_read = check_write = false;
00032     response_count = 0;
00033     uint8_t ack = 0;
00034     for(int count = reqData.get<uint8_t>(); count > 0; count--) {
00035         uint8_t cmd = reqData.get<uint8_t>();
00036         if (cmd & SWD_RnW) { // read register
00037             ack = read(cmd);
00038             check_write = false;
00039         } else { // write register
00040             ack = write(cmd);
00041         }
00042         if (ack != SWD_OK) {
00043             break;
00044         }
00045         response_count++;
00046     }
00047     if (ack == SWD_OK) {
00048         if (post_read) { // read previous data
00049             uint32_t data;
00050             ack = _swd->Transfer(DP_RDBUFF, &data);
00051             resData.append(data);
00052         } else if (check_write) { // check last write
00053             ack = _swd->Transfer(DP_RDBUFF, NULL);
00054         }
00055     }
00056     resData.data()[0] = 0x05; // transfer
00057     resData.data()[1] = response_count;
00058     resData.data()[2] = ack;
00059     return resData.length();
00060 }
00061 
00062 uint8_t TransferCore::read(uint8_t cmd)
00063 {
00064     uint8_t ack = 0;
00065     uint32_t data;
00066     if (post_read) {
00067         if ((cmd & SWD_APnDP) && !(cmd & DAP_TRANSFER_MATCH_VALUE)) {
00068             ack = _swd->Transfer(cmd, &data); // next post_read
00069         } else {
00070             ack = _swd->Transfer(DP_RDBUFF, &data);
00071             post_read = false;
00072         }
00073         if (ack != SWD_OK) {
00074             return ack;
00075         }
00076         resData.append(data);
00077     }
00078     if (cmd & DAP_TRANSFER_MATCH_VALUE) {
00079         uint32_t match_value = reqData.get<uint32_t>();
00080         if (cmd & SWD_APnDP) {
00081             ack = _swd->Transfer(cmd, NULL);
00082             if (ack != SWD_OK) {
00083                 return ack;
00084             }
00085         }
00086         for(int retry = match_retry; retry >= 0; retry--) {
00087             ack = _swd->Transfer(cmd, &data);
00088             if (ack == SWD_OK && (data&match_mask) == match_value) {
00089                 return ack;
00090             }
00091         }
00092         return ack | DAP_TRANSFER_MISMATCH; 
00093     } else {
00094         if (cmd & SWD_APnDP) {
00095             if (post_read == false) {
00096                 ack = _swd->Transfer(cmd, NULL);
00097                 post_read = true;
00098             }
00099         } else {
00100             ack = _swd->Transfer(cmd, &data);
00101             resData.append(data);
00102         }    
00103     }
00104     return ack;
00105 }
00106 
00107 uint8_t TransferCore::write(uint8_t cmd)
00108 {
00109     if (post_read) { // read previous data
00110         uint32_t data;
00111         uint8_t ack = _swd->Transfer(DP_RDBUFF, &data);
00112         if (ack != SWD_OK) {
00113             return ack;
00114         }
00115         resData.append(data);
00116         post_read = false;
00117     }    
00118     if (cmd & DAP_TRANSFER_MATCH_MASK) {
00119         match_mask = reqData.get<uint32_t>();
00120         return SWD_OK;
00121     }
00122     check_write = true;
00123     uint32_t data = reqData.get<uint32_t>();
00124     return _swd->Transfer(cmd, &data);
00125 }
00126 
00127 int TransferCore::TransferBlock(SWD* swd, uint8_t* request, uint8_t* response)
00128 {
00129     _swd = swd;
00130     reqData.init(request, 2);
00131     resData.init(response, 4);
00132     uint8_t ack = 0;
00133     response_count = 0;
00134     int count = reqData.get<uint16_t>();
00135     if (count > 0) {
00136         uint8_t cmd = reqData.get<uint8_t>();
00137         if (cmd & SWD_RnW) { // read register block
00138             ack = read_block(cmd, count);
00139         } else { // write register block
00140             ack = write_block(cmd, count);
00141         }
00142     }
00143     resData.data()[0] = 0x06; // transfer block
00144     ST<uint16_t>(resData.data()+1, response_count);
00145     resData.data()[3] = ack;    
00146     return resData.length(); 
00147 }
00148 
00149 uint8_t TransferCore::read_block(uint8_t cmd, int count)
00150 {
00151     if (cmd & SWD_APnDP) { // post AP read
00152         uint8_t ack = _swd->Transfer(cmd, NULL);
00153         if (ack != SWD_OK) {
00154             return ack;
00155         }
00156     }
00157     uint8_t ack = 0;
00158     while(count-- > 0) { // read DP/AP register
00159         if (count == 0 && (cmd & SWD_APnDP)) { // last AP read
00160             cmd = DP_RDBUFF;
00161         }
00162         uint32_t data;
00163         ack = _swd->Transfer(cmd, &data);
00164         if (ack != SWD_OK) {
00165             break;
00166         }
00167         resData.append(data);
00168         response_count++;
00169     }
00170     return ack;
00171 }
00172 
00173 uint8_t TransferCore::write_block(uint8_t cmd, int count)
00174 {
00175     while(count-- > 0) {
00176         uint32_t data = reqData.get<uint32_t>();
00177         uint8_t ack = _swd->Transfer(cmd, &data);
00178         if (ack != SWD_OK) {
00179             return ack;
00180         }
00181         response_count++;
00182     }
00183     return _swd->Transfer(DP_RDBUFF, NULL);
00184 }