Line data Source code
1 : #include "wibmod/WIB1/BNL_UDP.hh"
2 : #include "wibmod/WIB1/BNL_UDP_Exception.hh"
3 : #include <sys/socket.h>
4 : #include <string.h> //memset, strerror
5 : #include <errno.h>
6 : #include <string>
7 : #include <fcntl.h> //fcntl
8 :
9 : #include <sstream>
10 : #include <iomanip>
11 :
12 : #include <netdb.h>
13 :
14 : #include <errno.h>
15 : #include <algorithm> //STD::COUNT
16 :
17 : #define WIB_WR_BASE_PORT 32000
18 : #define WIB_RD_BASE_PORT 32001
19 : #define WIB_RPLY_BASE_PORT 32002
20 :
21 : #define WIB_PACKET_KEY 0xDEADBEEF
22 : //#define WIB_REQUEST_PACKET_SIZE (4+2+4+2)
23 : #define WIB_REQUEST_PACKET_TRAILER 0xFFFF
24 :
25 : #define WIB_RPLY_PACKET_SIZE 12
26 :
27 : #define TIMEOUT_SECONDS 2
28 : #define TIMEOUT_MICROSECONDS 0
29 : struct WIB_packet_t{
30 : uint32_t key;
31 : uint32_t reg_addr : 16;
32 : uint32_t data_MSW : 16;
33 : uint32_t data_LSW : 16;
34 : uint32_t trailer : 16;
35 : };
36 :
37 0 : static std::string dump_packet(uint8_t * data, size_t size){
38 : // printf("Err: %p %zu\n",data,size);
39 0 : std::stringstream ss;
40 0 : for(size_t iWord = 0; iWord < size;iWord++){
41 0 : ss << "0x" << std::hex << std::setfill('0') << std::setw(4) << iWord;
42 0 : ss << ": 0x" << std::hex << std::setfill('0') << std::setw(2) << int(data[iWord]);
43 0 : iWord++;
44 0 : if(iWord < size){
45 0 : ss << std::hex << std::setfill('0') << std::setw(2) << int(data[iWord]);
46 : }
47 0 : ss << std::endl;
48 : }
49 : // printf("%s",ss.str().c_str());
50 0 : return ss.str();
51 0 : }
52 :
53 0 : void BNL_UDP::FlushSocket(int sock){
54 : //turn on non-blocking
55 0 : fcntl(sock, F_SETFL, fcntl(sock, F_GETFL)| O_NONBLOCK);
56 0 : int ret;
57 0 : do{
58 0 : ret = recv(sock,buffer,buffer_size,0);
59 0 : }while(ret != -1);
60 0 : fcntl(sock, F_SETFL, fcntl(sock, F_GETFL) & (~O_NONBLOCK));
61 0 : }
62 :
63 0 : void BNL_UDP::Clear(){
64 : //close sockets
65 0 : if(readSocketFD != -1){
66 0 : close(readSocketFD);
67 0 : readSocketFD = -1;
68 : }
69 0 : if(writeSocketFD != -1){
70 0 : close(writeSocketFD);
71 0 : writeSocketFD = -1;
72 : }
73 : //Clear packet buffer
74 0 : if(buffer != NULL){
75 0 : delete [] buffer;
76 0 : buffer_size = 0;
77 : }
78 0 : writeAck = false;
79 0 : connected = false;
80 :
81 : //Reset send/recv addr structures
82 0 : memset(&readAddr,0,sizeof(readAddr));
83 0 : memset(&writeAddr,0,sizeof(writeAddr));
84 0 : }
85 :
86 : //static void printaddress(struct sockaddr_in const * addr){
87 : // printf("%u: %u.%u.%u.%u : %u\n",
88 : // addr->sin_family,
89 : // (addr->sin_addr.s_addr >> 0)&0xFF,
90 : // (addr->sin_addr.s_addr >> 8)&0xFF,
91 : // (addr->sin_addr.s_addr >> 16)&0xFF,
92 : // (addr->sin_addr.s_addr >> 24)&0xFF,
93 : // ntohs(addr->sin_port));
94 : //}
95 :
96 0 : void BNL_UDP::Setup(std::string const & address,uint16_t port_offset){
97 : //Reset the network structures
98 0 : Clear();
99 :
100 : //Allocate the recv buffer
101 0 : ResizeBuffer();
102 :
103 : //Check port_offset range
104 0 : if(port_offset > 128){
105 0 : BUException::BNL_UDP_PORT_OUT_OF_RANGE e;
106 0 : throw e;
107 0 : }
108 :
109 : //Set the ports for this device (FEMBs are iFEMB*0x10 above the base)
110 0 : readPort = WIB_RD_BASE_PORT + port_offset;
111 0 : writePort = WIB_WR_BASE_PORT + port_offset;
112 : // replyPort = WIB_RPLY_BASE_PORT + port_offset;
113 :
114 0 : remoteAddress = address;
115 : //Get the sockaddr for the address
116 0 : struct addrinfo * res;
117 0 : if(getaddrinfo(address.c_str(),NULL,NULL,&res)){
118 : //Check if we have just one "." character and is less than 5 characters
119 0 : if(address.size() <= 5 && 1 == std::count(address.begin(),address.end(),'.')){
120 0 : std::string strCrate = address.substr(0,address.find('.'));
121 0 : std::string strSlot = address.substr(address.find('.')+1);
122 0 : if(strCrate.size() != 0 && strSlot.size() != 0){
123 0 : uint8_t crate = strtoul(strCrate.c_str(),NULL,0);
124 0 : uint8_t slot = strtoul(strSlot.c_str(), NULL,0);
125 0 : if( (((crate > 0) && (crate < 7)) || 0xF == crate) &&
126 0 : (((slot > 0) && (slot < 7)) || 0xF == slot)){
127 0 : remoteAddress = std::string("192.168.");
128 : //Add the crate part of the address (200 + crate number)
129 0 : if(crate == 0xF){
130 0 : remoteAddress += "200.";
131 : }else{
132 : //generate the crate number which is 200 + crate number
133 0 : remoteAddress += "20";
134 0 : remoteAddress += ('0' + crate);
135 0 : remoteAddress += '.';
136 : }
137 0 : if(slot == 0xF){
138 0 : remoteAddress += "50";
139 : }else{
140 : //crate last IP octet that is slot number
141 0 : remoteAddress += ('0' + slot);
142 : }
143 : }
144 : }
145 0 : }
146 : //try a second time assumin gthis is a crate.slot address, fail if this still doesn't work
147 0 : if(getaddrinfo(address.c_str(),NULL,NULL,&res)){
148 0 : BUException::BAD_REMOTE_IP e;
149 0 : e.Append("Addr: ");
150 0 : e.Append(address.c_str());
151 0 : e.Append(" could not be resolved.\n");
152 0 : throw e;
153 0 : }
154 : }
155 : // readAddr = *((struct sockaddr_in *) res->ai_addr);
156 : // printaddress(&readAddr);
157 :
158 : //Generate the sockets for read and write
159 0 : if((readSocketFD = socket(AF_INET,SOCK_DGRAM,0)) < 0){
160 0 : BUException::BAD_SOCKET e;
161 0 : e.Append("read socket\n");
162 0 : throw e;
163 0 : }
164 0 : if((writeSocketFD = socket(AF_INET,SOCK_DGRAM,0)) < 0){
165 0 : BUException::BAD_SOCKET e;
166 0 : e.Append("write socket\n");
167 0 : throw e;
168 0 : }
169 : //Set a timeout for the recv socket so we don't hang on a reply
170 0 : struct timeval tv; tv.tv_sec=TIMEOUT_SECONDS; tv.tv_usec=TIMEOUT_MICROSECONDS;
171 0 : setsockopt(readSocketFD, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv,sizeof(struct timeval));
172 0 : setsockopt(writeSocketFD, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv,sizeof(struct timeval));
173 :
174 : //connect the read socket
175 0 : readAddr = *((struct sockaddr_in *) res->ai_addr);
176 0 : readAddr.sin_port = htons(readPort);
177 0 : if(connect(readSocketFD,(struct sockaddr *) &readAddr,sizeof(readAddr)) < 0){
178 0 : BUException::CONNECTION_FAILED e;
179 0 : e.Append("read socket connect\n");
180 0 : e.Append(strerror(errno));
181 0 : throw e;
182 0 : }
183 : //connect the write socket
184 0 : writeAddr = *((struct sockaddr_in *) res->ai_addr);
185 0 : writeAddr.sin_port = htons(writePort);
186 0 : if(connect(writeSocketFD,(struct sockaddr *) &writeAddr,sizeof(writeAddr)) < 0){
187 0 : BUException::CONNECTION_FAILED e;
188 0 : e.Append("write socket connect\n");
189 0 : e.Append(strerror(errno));
190 0 : throw e;
191 0 : }
192 :
193 : //Allocate the receive buffer to default size
194 0 : ResizeBuffer();
195 0 : }
196 :
197 0 : void BNL_UDP::WriteWithRetry(uint16_t address, uint32_t value, uint8_t retry_count){
198 0 : while(retry_count > 1){
199 0 : try{
200 : //Do the write
201 0 : Write(address,value);
202 0 : usleep(10);
203 : //if everything goes well, return
204 : return;
205 0 : }catch(BUException::BAD_REPLY &e){
206 : //eat the exception
207 0 : }
208 0 : total_retry_count++;
209 0 : retry_count--;
210 0 : usleep(10);
211 : }
212 : //Last chance we don't catch the exception and let it fall down the stack
213 : //Do the write
214 0 : Write(address,value);
215 0 : usleep(10);
216 : }
217 0 : void BNL_UDP::Write(uint16_t address, uint32_t value){
218 :
219 : //Flush this socket
220 0 : FlushSocket(writeSocketFD);
221 :
222 : //Build the packet to send
223 : //build the send packet
224 0 : WIB_packet_t packet;
225 0 : packet.key = htonl(WIB_PACKET_KEY);
226 0 : packet.reg_addr = htons(address);
227 0 : packet.data_MSW = htons(uint16_t((value >> 16) & 0xFFFF));
228 0 : packet.data_LSW = htons(uint16_t((value >> 0) & 0xFFFF));
229 0 : packet.trailer = htons(WIB_REQUEST_PACKET_TRAILER);
230 :
231 : //send the packet
232 0 : ssize_t send_size = sizeof(packet);
233 0 : ssize_t sent_size = 0;
234 0 : if( send_size != (sent_size = send(writeSocketFD,
235 : &packet,send_size,0))){
236 : //bad send
237 0 : BUException::SEND_FAILED e;
238 0 : if(sent_size == -1){
239 0 : e.Append("BNL_UDP::Write(uint16_t,uint32_t)\n");
240 0 : e.Append("Errnum: ");
241 0 : e.Append(strerror(errno));
242 : }
243 0 : throw e;
244 0 : }
245 :
246 : //If configured, capture confirmation packet
247 0 : if(writeAck ){
248 0 : ssize_t reply_size = recv(writeSocketFD,
249 0 : buffer,buffer_size,0);
250 :
251 0 : if(-1 == reply_size){
252 0 : BUException::BAD_REPLY e;
253 0 : std::stringstream ss;
254 0 : e.Append("BNL_UDP::Write(uint16_t,uint32_t)\n");
255 0 : ss << "Errnum(" << errno << "): " << strerror(errno) << "\n";
256 0 : e.Append(ss.str().c_str());
257 0 : e.Append(dump_packet((uint8_t*) &packet,send_size).c_str());
258 0 : throw e;
259 0 : }else if( reply_size < WIB_RPLY_PACKET_SIZE){
260 0 : BUException::BAD_REPLY e;
261 0 : std::stringstream ss;
262 0 : ss << "Bad Size: " << reply_size << "\n";
263 0 : e.Append("BNL_UDP::Write(uint16_t,uint32_t)\n");
264 0 : e.Append(ss.str().c_str());
265 0 : e.Append(dump_packet(buffer,reply_size).c_str());
266 0 : throw e;
267 0 : }
268 0 : uint16_t reply_address = uint16_t(buffer[0] << 8 | buffer[1]);
269 0 : if( reply_address != address){
270 0 : BUException::BAD_REPLY e;
271 0 : std::stringstream ss;
272 0 : ss << "Bad address: " << uint32_t(address) << " != " << uint32_t(reply_address) << "\n";
273 0 : e.Append("BNL_UDP::Write(uint16_t,uint32_t)\n");
274 0 : e.Append(ss.str().c_str());
275 0 : e.Append(dump_packet(buffer,reply_size).c_str());
276 0 : throw e;
277 0 : }
278 : }
279 0 : }
280 0 : void BNL_UDP::Write(uint16_t address, std::vector<uint32_t> const & values){
281 0 : Write(address,values.data(),values.size());
282 0 : }
283 0 : void BNL_UDP::Write(uint16_t address, uint32_t const * values, size_t word_count){
284 0 : for(size_t iWrite = 0; iWrite < word_count;iWrite++){
285 0 : WriteWithRetry(address,values[iWrite]);
286 0 : address++;
287 : }
288 :
289 : //////// //Flush the socket
290 : //////// FlushSocket(writeSocketFD);
291 : ////////
292 : //////// //Compute the size of this multi-write packet
293 : //////// // values.size() - 1 gets the number of address and MSW/LSW groups that aren't already in the packet
294 : //////// size_t packetSize = sizeof(WIB_packet_t) + (word_count-1)*6;
295 : //////// //resize the buffer if needed
296 : //////// ResizeBuffer(packetSize);
297 : ////////
298 : //////// //Set the packet key
299 : //////// (*((uint32_t*) buffer)) = htonl(WIB_PACKET_KEY);
300 : //////// //Create a pointer to 16bit words that points to the parts of buffer that are after the first 32bit word (WIB key)
301 : //////// uint16_t * packet = (uint16_t*) (buffer + sizeof(uint32_t));
302 : //////// for(size_t iWord = 0; iWord < word_count;iWord++){
303 : //////// //Set the word address
304 : //////// packet[0] = htons(address);
305 : //////// //Set the MS 16bit part of the 32bit word
306 : //////// packet[1] = htons(uint16_t((values[iWord] >> 16) & 0xFFFF));
307 : //////// //Set the LS 16bit part of the 32bit word
308 : //////// packet[2] = htons(uint16_t((values[iWord] >> 0) & 0xFFFF));
309 : //////// //move the write address forward 1 word
310 : //////// address++;
311 : //////// //move the packet pointer forward to the next word block
312 : //////// packet+=3;
313 : //////// }
314 : //////// //Set the packet trailer
315 : //////// (*packet) = htons(WIB_REQUEST_PACKET_TRAILER);
316 : ////////
317 : //////// //send the packet
318 : //////// ssize_t send_size = packetSize;
319 : //////// ssize_t sent_size = 0;
320 : //////// if( send_size != (sent_size = send(writeSocketFD,
321 : //////// buffer,send_size,0))){
322 : //////// //bad send
323 : //////// BUException::SEND_FAILED e;
324 : //////// if(sent_size == -1){
325 : //////// e.Append("BNL_UDP::Write(uint16_t,uint32_t*,size_t)\n");
326 : //////// e.Append("Errnum: ");
327 : //////// e.Append(strerror(errno));
328 : //////// }
329 : //////// throw e;
330 : //////// }
331 : //////// //If configured, capture confirmation packet
332 : //////// if(writeAck ){
333 : //////// ssize_t reply_size = recv(writeSocketFD,
334 : //////// buffer,buffer_size,0);
335 : ////////
336 : ////////
337 : //////// // writeAddr = readAddr;
338 : //////// // writeAddr.sin_port = htons(replyPort);
339 : ////////
340 : //////// // sockaddr_len = sizeof(writeAddr);
341 : //////// // ssize_t reply_size = recvfrom(recvSocketFD,
342 : ////////// recvfrom(recvSocketFD,
343 : ////////// buffer,buffer_size,0,
344 : ////////// (struct sockaddr*)&writeAddr,&sockaddr_len);
345 : //////// if(-1 == reply_size){
346 : //////// BUException::BAD_REPLY e;
347 : //////// e.Append("BNL_UDP::Write(uint16_t,uint32_t*,size_t)\n");
348 : //////// e.Append(strerror(errno));
349 : //////// throw e;
350 : //////// }else if( reply_size < WIB_RPLY_PACKET_SIZE){
351 : //////// BUException::BAD_REPLY e;
352 : //////// std::stringstream ss;
353 : //////// ss << "Bad Size: " << reply_size << "\n";
354 : //////// e.Append("BNL_UDP::Write(uint16_t,uint32_t*,size_t)\n");
355 : //////// e.Append(ss.str().c_str());
356 : //////// e.Append(dump_packet(buffer,reply_size).c_str());
357 : //////// throw e;
358 : //////// }
359 : ////////// uint16_t reply_address = uint16_t(buffer[0] << 8 | buffer[1]);
360 : ////////// if( reply_address != address){
361 : ////////// BUException::BAD_REPLY e;
362 : ////////// std::stringstream ss;
363 : ////////// ss << "Bad address: " << address << " != " << reply_address << "\n";
364 : ////////// e.Append(ss.str().c_str());
365 : ////////// throw e;
366 : ////////// }
367 : //////// }
368 0 : }
369 :
370 0 : uint32_t BNL_UDP::ReadWithRetry(uint16_t address,uint8_t retry_count){
371 0 : uint32_t val;
372 0 : while(retry_count > 1){
373 0 : try{
374 : //Do the write
375 0 : val = Read(address);
376 0 : usleep(10);
377 : //if everything goes well, return
378 : return val;
379 0 : }catch(BUException::BAD_REPLY &e){
380 : //eat the exception
381 0 : }
382 0 : usleep(10);
383 0 : total_retry_count++;
384 0 : retry_count--;
385 : }
386 : //Last chance we don't catch the exception and let it fall down the stack
387 0 : val = Read(address);
388 0 : usleep(10);
389 0 : return val;
390 : }
391 0 : uint32_t BNL_UDP::Read(uint16_t address){
392 : //Flush the socket
393 0 : FlushSocket(readSocketFD);
394 :
395 : //build the send packet
396 0 : WIB_packet_t packet;
397 0 : packet.key = htonl(WIB_PACKET_KEY);
398 0 : packet.reg_addr = htons(address);
399 0 : packet.data_MSW = packet.data_LSW = 0;
400 0 : packet.trailer = htons(WIB_REQUEST_PACKET_TRAILER);
401 :
402 : //send the packet
403 0 : ssize_t send_size = sizeof(packet);
404 0 : ssize_t sent_size = 0;
405 0 : if( send_size != (sent_size = send(readSocketFD,
406 : &packet,send_size,0))){
407 : //bad send
408 0 : BUException::SEND_FAILED e;
409 0 : if(sent_size == -1){
410 0 : e.Append("BNL_UDP::Read(uint16_t)\n");
411 0 : e.Append("Errnum: ");
412 0 : e.Append(strerror(errno));
413 : }
414 0 : throw e;
415 0 : }
416 :
417 : //Get the reply packet with the register data in it.
418 0 : ssize_t reply_size = recv(readSocketFD,
419 0 : buffer,buffer_size,0);
420 :
421 0 : if(ssize_t(-1) == reply_size){
422 0 : BUException::BAD_REPLY e;
423 0 : std::stringstream ss;
424 0 : e.Append("BNL_UDP::Read(uint16_t)\n");
425 0 : ss << "Errnum(" << errno << "): " << strerror(errno) << "\n";
426 0 : e.Append(ss.str().c_str());
427 0 : e.Append(dump_packet((uint8_t *)&packet,send_size).c_str());
428 0 : throw e;
429 0 : }else if( reply_size < WIB_RPLY_PACKET_SIZE){
430 0 : BUException::BAD_REPLY e;
431 0 : std::stringstream ss;
432 0 : ss << "Bad Size: " << reply_size << "\n";
433 0 : e.Append("BNL_UDP::Read(uint16_t)\n");
434 0 : e.Append(ss.str().c_str());
435 0 : e.Append(dump_packet(buffer,reply_size).c_str());
436 0 : throw e;
437 0 : }
438 0 : uint16_t reply_address = uint16_t(buffer[0] << 8 | buffer[1]);
439 0 : if( reply_address != address){
440 0 : BUException::BAD_REPLY e;
441 0 : std::stringstream ss;
442 0 : ss << "Bad address: " << uint32_t(address) << " != " << uint32_t(reply_address) << "\n";
443 0 : e.Append("BNL_UDP::Read(uint16_t)\n");
444 0 : e.Append(ss.str().c_str());
445 0 : e.Append(dump_packet(buffer,reply_size).c_str());
446 0 : throw e;
447 0 : }
448 :
449 :
450 : // }
451 0 : uint32_t ret = ( (uint32_t(buffer[2]) << 24) |
452 0 : (uint32_t(buffer[3]) << 16) |
453 0 : (uint32_t(buffer[4]) << 8) |
454 0 : (uint32_t(buffer[5]) << 0));
455 0 : return ret;
456 : }
457 :
458 :
459 0 : BNL_UDP::~BNL_UDP(){
460 0 : Clear();
461 0 : }
462 :
463 :
464 0 : void BNL_UDP::ResizeBuffer(size_t size){
465 : //CHeck if the requested size is larger than the already allocated size
466 : // printf("before %p %zu %zd\n",buffer,buffer_size,buffer_size);
467 0 : if(buffer_size < size){
468 : //We need to re-allocate
469 0 : if(buffer != NULL){
470 0 : delete [] buffer;
471 : }
472 0 : buffer = new uint8_t[size];
473 0 : buffer_size = size;
474 : }
475 : // printf("after %p %zu %zd\n",buffer,buffer_size,buffer_size);
476 0 : }
|
