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