00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "arch/macOS/arch_macOS.h"
00036 #include "arch/macOS/net_serial.h"
00037 #include <termios.h>
00038 #include <sys/select.h>
00039
00040 namespace rp{ namespace arch{ namespace net{
00041
00042 raw_serial::raw_serial()
00043 : rp::hal::serial_rxtx()
00044 , _baudrate(0)
00045 , _flags(0)
00046 , serial_fd(-1)
00047 {
00048 _init();
00049 }
00050
00051 raw_serial::~raw_serial()
00052 {
00053 close();
00054
00055 }
00056
00057 bool raw_serial::open()
00058 {
00059 return open(_portName, _baudrate, _flags);
00060 }
00061
00062 bool raw_serial::bind(const char * portname, uint32_t baudrate, uint32_t flags)
00063 {
00064 strncpy(_portName, portname, sizeof(_portName));
00065 _baudrate = baudrate;
00066 _flags = flags;
00067 return true;
00068 }
00069
00070 bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags)
00071 {
00072 if (isOpened()) close();
00073
00074 serial_fd = ::open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
00075
00076 if (serial_fd == -1) return false;
00077
00078 struct termios options, oldopt;
00079 tcgetattr(serial_fd, &oldopt);
00080 bzero(&options,sizeof(struct termios));
00081
00082 _u32 termbaud = getTermBaudBitmap(baudrate);
00083
00084 if (termbaud == (_u32)-1) {
00085 close();
00086 return false;
00087 }
00088 cfsetispeed(&options, termbaud);
00089 cfsetospeed(&options, termbaud);
00090
00091
00092 options.c_cflag |= (CLOCAL | CREAD);
00093
00094
00095 options.c_cflag &= ~PARENB;
00096 options.c_cflag &= ~CSTOPB;
00097
00098 options.c_cflag &= ~CSIZE;
00099 options.c_cflag |= CS8;
00100
00101 #ifdef CNEW_RTSCTS
00102 options.c_cflag &= ~CNEW_RTSCTS;
00103 #endif
00104
00105 options.c_iflag &= ~(IXON | IXOFF | IXANY);
00106
00107
00108 options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
00109
00110 options.c_oflag &= ~OPOST;
00111
00112 tcflush(serial_fd,TCIFLUSH);
00113
00114
00115
00116
00117
00118
00119
00120 if (tcsetattr(serial_fd, TCSANOW, &options))
00121 {
00122 close();
00123 return false;
00124 }
00125
00126 _is_serial_opened = true;
00127
00128
00129 clearDTR();
00130
00131 return true;
00132 }
00133
00134 void raw_serial::close()
00135 {
00136 if (serial_fd != -1)
00137 ::close(serial_fd);
00138 serial_fd = -1;
00139
00140 _is_serial_opened = false;
00141 }
00142
00143 int raw_serial::senddata(const unsigned char * data, _word_size_t size)
00144 {
00145
00146 if (!isOpened()) return 0;
00147
00148 if (data == NULL || size ==0) return 0;
00149
00150 size_t tx_len = 0;
00151 required_tx_cnt = 0;
00152 do {
00153 int ans = ::write(serial_fd, data + tx_len, size-tx_len);
00154
00155 if (ans == -1) return tx_len;
00156
00157 tx_len += ans;
00158 required_tx_cnt = tx_len;
00159 }while (tx_len<size);
00160
00161
00162 return tx_len;
00163 }
00164
00165
00166
00167 int raw_serial::recvdata(unsigned char * data, _word_size_t size)
00168 {
00169 if (!isOpened()) return 0;
00170
00171 int ans = ::read(serial_fd, data, size);
00172
00173 if (ans == -1) ans=0;
00174 required_rx_cnt = ans;
00175 return ans;
00176 }
00177
00178
00179 void raw_serial::flush( _u32 flags)
00180 {
00181 tcflush(serial_fd,TCIFLUSH);
00182 }
00183
00184 int raw_serial::waitforsent(_u32 timeout, _word_size_t * returned_size)
00185 {
00186 if (returned_size) *returned_size = required_tx_cnt;
00187 return 0;
00188 }
00189
00190 int raw_serial::waitforrecv(_u32 timeout, _word_size_t * returned_size)
00191 {
00192 if (!isOpened() ) return -1;
00193
00194 if (returned_size) *returned_size = required_rx_cnt;
00195 return 0;
00196 }
00197
00198 int raw_serial::waitfordata(_word_size_t data_count, _u32 timeout, _word_size_t * returned_size)
00199 {
00200 _word_size_t length = 0;
00201 if (returned_size==NULL) returned_size=(_word_size_t *)&length;
00202 *returned_size = 0;
00203
00204 int max_fd;
00205 fd_set input_set;
00206 struct timeval timeout_val;
00207
00208
00209 FD_ZERO(&input_set);
00210 FD_SET(serial_fd, &input_set);
00211 max_fd = serial_fd + 1;
00212
00213
00214 timeout_val.tv_sec = timeout / 1000;
00215 timeout_val.tv_usec = (timeout % 1000) * 1000;
00216
00217 if ( isOpened() )
00218 {
00219 if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR;
00220 if (*returned_size >= data_count)
00221 {
00222 return 0;
00223 }
00224 }
00225
00226 while ( isOpened() )
00227 {
00228
00229 int n = ::select(max_fd, &input_set, NULL, NULL, &timeout_val);
00230
00231 if (n < 0)
00232 {
00233
00234 return ANS_DEV_ERR;
00235 }
00236 else if (n == 0)
00237 {
00238
00239 return ANS_TIMEOUT;
00240 }
00241 else
00242 {
00243
00244 assert (FD_ISSET(serial_fd, &input_set));
00245
00246
00247 if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR;
00248 if (*returned_size >= data_count)
00249 {
00250 return 0;
00251 }
00252 else
00253 {
00254 int remain_timeout = timeout_val.tv_sec*1000000 + timeout_val.tv_usec;
00255 int expect_remain_time = (data_count - *returned_size)*1000000*8/_baudrate;
00256 if (remain_timeout > expect_remain_time)
00257 usleep(expect_remain_time);
00258 }
00259 }
00260
00261 }
00262 return ANS_DEV_ERR;
00263 }
00264
00265 size_t raw_serial::rxqueue_count()
00266 {
00267 if ( !isOpened() ) return 0;
00268 size_t remaining;
00269
00270 if (::ioctl(serial_fd, FIONREAD, &remaining) == -1) return 0;
00271 return remaining;
00272 }
00273
00274 void raw_serial::setDTR()
00275 {
00276 if ( !isOpened() ) return;
00277
00278 uint32_t dtr_bit = TIOCM_DTR;
00279 ioctl(serial_fd, TIOCMBIS, &dtr_bit);
00280 }
00281
00282 void raw_serial::clearDTR()
00283 {
00284 if ( !isOpened() ) return;
00285
00286 uint32_t dtr_bit = TIOCM_DTR;
00287 ioctl(serial_fd, TIOCMBIC, &dtr_bit);
00288 }
00289
00290 void raw_serial::_init()
00291 {
00292 serial_fd = 0;
00293 _portName[0] = 0;
00294 required_tx_cnt = required_rx_cnt = 0;
00295 }
00296
00297
00298
00299 _u32 raw_serial::getTermBaudBitmap(_u32 baud)
00300 {
00301 #define BAUD_CONV(_baud_) case _baud_: return _baud_
00302 switch (baud)
00303 {
00304 BAUD_CONV(1200);
00305 BAUD_CONV(1800);
00306 BAUD_CONV(2400);
00307 BAUD_CONV(4800);
00308 BAUD_CONV(9600);
00309 BAUD_CONV(19200);
00310 BAUD_CONV(38400);
00311 BAUD_CONV(57600);
00312 BAUD_CONV(115200);
00313 BAUD_CONV(230400);
00314 BAUD_CONV(460800);
00315 BAUD_CONV(500000);
00316 BAUD_CONV(576000);
00317 BAUD_CONV(921600);
00318 BAUD_CONV(1000000);
00319 BAUD_CONV(1152000);
00320 BAUD_CONV(1500000);
00321 BAUD_CONV(2000000);
00322 BAUD_CONV(2500000);
00323 BAUD_CONV(3000000);
00324 BAUD_CONV(3500000);
00325 BAUD_CONV(4000000);
00326 }
00327 return -1;
00328 }
00329
00330 }}}
00331
00332
00333
00334
00335 namespace rp{ namespace hal{
00336
00337 serial_rxtx * serial_rxtx::CreateRxTx()
00338 {
00339 return new rp::arch::net::raw_serial();
00340 }
00341
00342 void serial_rxtx::ReleaseRxTx(serial_rxtx *rxtx)
00343 {
00344 delete rxtx;
00345 }
00346
00347 }}